import { assert } from "../../../core/assert.js";
import { PAIR_UINT32_MAP_ABSENT, PairUint32Map } from "../../../core/collection/PairUint32Map.js";

/**
 * Maximum contact points per manifold. Four is the universal industry choice
 * (Bullet's btPersistentManifold, Box2D, Jolt) — enough to express stable
 * face-on-face contact, small enough to fit inertia-driven solver passes.
 * @type {number}
 */
export const MAX_CONTACTS_PER_MANIFOLD = 4;

/**
 * Per-contact field stride in the data Float64Array.
 *
 * Layout per contact (all world-space):
 *   0..2  : world_a    (contact point on body A's surface)
 *   3..5  : world_b    (contact point on body B's surface)
 *   6..8  : normal_w   (from B toward A)
 *   9     : depth      (positive = penetration, negative = speculative gap)
 *   10    : j_n        (accumulated normal impulse, warm-start)
 *   11    : j_t1       (accumulated tangent impulse, axis 1)
 *   12    : j_t2       (accumulated tangent impulse, axis 2)
 *   13    : feature_id (uint32 packed as f64 — stable cross-frame ID of the
 *                       geometric feature pair that produced this contact;
 *                       0 means "no info, fall back to position matching")
 *   14    : friction   (already-combined coefficient for THIS contact's
 *                       specific source-collider pair — see combine_material)
 *   15    : restitution(already-combined coefficient for this contact)
 *
 * Solver uses `(world_a + world_b) * 0.5` as the application point; storing
 * both surface points enables the per-frame warm-start matcher in the
 * narrowphase to compare contact-point positions when feature_id is 0.
 *
 * Friction / restitution are stored **per contact** (not per body pair) so a
 * compound body whose colliders have different materials gets the right
 * coefficient at each contact: the narrowphase knows the exact source collider
 * on each side of every contact and combines there.
 *
 * @type {number}
 */
export const CONTACT_STRIDE = 16;

/**
 * Per-contact feature_id offset within {@link CONTACT_STRIDE}.
 * Exposed for the narrowphase matcher; solver code does not read this.
 * @type {number}
 */
export const CONTACT_FEATURE_ID_OFFSET = 13;

/**
 * Per-slot Float64 stride: room for {@link MAX_CONTACTS_PER_MANIFOLD} contacts.
 * @type {number}
 */
export const SLOT_DATA_STRIDE = CONTACT_STRIDE * MAX_CONTACTS_PER_MANIFOLD;

/**
 * Per-slot meta stride in the Uint32 meta buffer:
 *   0 : bodyA
 *   1 : bodyB
 *   2 : packed = count(0..3) | touched(bit 4) | prev_touched(bit 5) | grace(bits 6..13)
 * @type {number}
 */
const SLOT_META_STRIDE = 3;

const FLAG_TOUCHED = 1 << 4;
const FLAG_PREV_TOUCHED = 1 << 5;
const COUNT_MASK = 0x0F;
const GRACE_SHIFT = 6;
const GRACE_MASK = 0xFF; // 8 bits

/**
 * Number of consecutive frames a manifold may stay untouched before eviction.
 * Two frames of grace lets a one-frame separation survive (jitter friendly)
 * while still keeping the cache bounded.
 * @type {number}
 */
const DEFAULT_GRACE = 2;

/**
 * Sentinel for "no slot".
 * @type {number}
 */
export const MANIFOLD_SLOT_ABSENT = -1;

const DEFAULT_INITIAL_CAPACITY = 64;

/**
 * Persistent contact-manifold cache keyed by canonical body-id pairs.
 *
 * Owns:
 *  - the SoA `Float64Array` of contact points (per-slot stride 52),
 *  - the small meta `Uint32Array` (bodyA, bodyB, packed flags + count),
 *  - a {@link PairUint32Map} from `(idA, idB)` to slot id,
 *  - a dense `__live_slots` array of currently-allocated slot ids,
 *  - a min-heap-free-stack of released slot ids so reuse is deterministic.
 *
 * Lifecycle per simulation step:
 *  1. Broadphase calls {@link acquire} for every encountered pair, which marks
 *     each touched slot as "fresh this frame".
 *  2. Narrowphase fills in contact points via {@link set_contact} /
 *     {@link clear_contacts}.
 *  3. End-of-step calls {@link advance_frame} to roll touched → prev_touched
 *     and evict slots that have been untouched for {@link DEFAULT_GRACE}
 *     consecutive frames.
 *
 * @author Alex Goldring
 * @copyright Company Named Limited (c) 2026
 */
export class ManifoldStore {

    /**
     * @param {number} [initial_capacity] slots
     */
    constructor(initial_capacity = DEFAULT_INITIAL_CAPACITY) {
        assert.isNonNegativeInteger(initial_capacity, 'initial_capacity');
        this.__capacity = Math.max(4, initial_capacity);
        this.__high_water = 0;

        this.__data = new Float64Array(this.__capacity * SLOT_DATA_STRIDE);
        this.__meta = new Uint32Array(this.__capacity * SLOT_META_STRIDE);

        this.__pair_index = new PairUint32Map(this.__capacity * 2);

        // Dense list of currently-allocated slot ids + reverse map.
        this.__live_slots = new Uint32Array(this.__capacity);
        this.__live_pos = new Int32Array(this.__capacity);
        this.__live_pos.fill(-1);
        this.__live_count = 0;

        // Min-heap of released slot ids.
        this.__free_heap = new Uint32Array(this.__capacity);
        this.__free_count = 0;
    }

    /**
     * @returns {number} number of currently-live slots
     */
    get count() {
        return this.__live_count;
    }

    /**
     * @returns {number} total allocated slot capacity
     */
    get capacity() {
        return this.__capacity;
    }

    /**
     * Look up a slot by canonical pair. Caller is responsible for ordering
     * `(idA, idB)` such that the same pair always hashes the same way.
     *
     * @param {number} idA
     * @param {number} idB
     * @returns {number} slot id or {@link MANIFOLD_SLOT_ABSENT}
     */
    find(idA, idB) {
        const v = this.__pair_index.get(idA, idB);
        return v === PAIR_UINT32_MAP_ABSENT ? MANIFOLD_SLOT_ABSENT : v;
    }

    /**
     * Acquire a slot for the pair, creating one if absent, and mark it
     * "touched this frame". Returns the slot id. Calling `acquire` twice in
     * the same step on the same pair is a no-op for the second call.
     *
     * @param {number} idA
     * @param {number} idB
     * @returns {number} slot id
     */
    acquire(idA, idB) {
        let slot = this.find(idA, idB);
        if (slot === MANIFOLD_SLOT_ABSENT) {
            slot = this.__allocate_slot();
            const meta_off = slot * SLOT_META_STRIDE;
            this.__meta[meta_off] = idA;
            this.__meta[meta_off + 1] = idB;
            this.__meta[meta_off + 2] = 0; // count=0, no flags, grace=0
            // Zero the contact data — important so a recycled slot's warm-start
            // impulses don't leak across pair identities.
            const data_off = slot * SLOT_DATA_STRIDE;
            this.__data.fill(0, data_off, data_off + SLOT_DATA_STRIDE);

            this.__pair_index.set(idA, idB, slot);
        }
        const meta_off = slot * SLOT_META_STRIDE;
        // Set touched bit, clear grace.
        let packed = this.__meta[meta_off + 2];
        packed |= FLAG_TOUCHED;
        packed &= ~(GRACE_MASK << GRACE_SHIFT);
        this.__meta[meta_off + 2] = packed;
        return slot;
    }

    /**
     * @param {number} slot
     * @returns {number} body A id stored at this slot
     */
    bodyA(slot) {
        return this.__meta[slot * SLOT_META_STRIDE];
    }

    /**
     * @param {number} slot
     * @returns {number}
     */
    bodyB(slot) {
        return this.__meta[slot * SLOT_META_STRIDE + 1];
    }

    /**
     * @param {number} slot
     * @returns {number} 0..MAX_CONTACTS_PER_MANIFOLD
     */
    contact_count(slot) {
        return this.__meta[slot * SLOT_META_STRIDE + 2] & COUNT_MASK;
    }

    /**
     * @param {number} slot
     * @returns {boolean}
     */
    is_touched(slot) {
        return (this.__meta[slot * SLOT_META_STRIDE + 2] & FLAG_TOUCHED) !== 0;
    }

    /**
     * @param {number} slot
     * @returns {boolean}
     */
    was_touched_prev(slot) {
        return (this.__meta[slot * SLOT_META_STRIDE + 2] & FLAG_PREV_TOUCHED) !== 0;
    }

    /**
     * Iterate the dense list of currently-live slots without going through
     * the pair map. `i` ranges `[0, count)`.
     * @param {number} i
     * @returns {number}
     */
    live_at(i) {
        return this.__live_slots[i];
    }

    /**
     * Clear the contact list for a slot (counts go to zero, warm-start
     * impulses also reset). Used when narrowphase determines the pair is
     * separated this frame.
     * @param {number} slot
     */
    clear_contacts(slot) {
        const meta_off = slot * SLOT_META_STRIDE;
        this.__meta[meta_off + 2] = this.__meta[meta_off + 2] & ~COUNT_MASK;

        const data_off = slot * SLOT_DATA_STRIDE;
        this.__data.fill(0, data_off, data_off + SLOT_DATA_STRIDE);
    }

    /**
     * Reset the contact count to zero so the next `set_contact(slot, 0, …)`
     * lands at index 0, but DO NOT zero the data slab. Warm-start impulses
     * (`j_n`, `j_t1`, `j_t2`) at each contact slot survive — the subsequent
     * `set_contact` calls only overwrite the first 10 floats (position,
     * normal, depth) per contact.
     *
     * Use this when narrowphase determines the pair still has overlap and
     * is about to refill the slot with the new frame's contacts. Use
     * {@link clear_contacts} instead when the pair has genuinely separated
     * and the cached impulses should be evicted.
     *
     * @param {number} slot
     */
    begin_refill(slot) {
        const meta_off = slot * SLOT_META_STRIDE;
        this.__meta[meta_off + 2] = this.__meta[meta_off + 2] & ~COUNT_MASK;
    }

    /**
     * Write a contact point into a slot. Increments contact_count if `idx`
     * exceeds the current count.
     *
     * Writes geometry (offsets 0..9), the feature_id at offset 13, and the
     * per-contact combined friction / restitution at offsets 14 / 15.
     * Deliberately does NOT touch the warm-start impulse fields (offsets
     * 10..12) — the narrowphase match-and-merge pass uses this to refill
     * a slot while preserving the cached impulses that the solver's
     * warm-start needs.
     *
     * @param {number} slot
     * @param {number} idx contact index in `[0, MAX_CONTACTS_PER_MANIFOLD)`
     * @param {number} lax
     * @param {number} lay
     * @param {number} laz
     * @param {number} lbx
     * @param {number} lby
     * @param {number} lbz
     * @param {number} nx normal world x (B toward A)
     * @param {number} ny
     * @param {number} nz
     * @param {number} depth
     * @param {number} feature_id stable cross-frame identifier of the
     *   geometric feature pair that produced this contact; 0 means
     *   "no info, fall back to position matching"
     * @param {number} friction combined friction for this contact's specific
     *   source-collider pair
     * @param {number} restitution combined restitution for this contact
     */
    set_contact(slot, idx, lax, lay, laz, lbx, lby, lbz, nx, ny, nz, depth, feature_id = 0, friction = 0, restitution = 0) {
        const off = slot * SLOT_DATA_STRIDE + idx * CONTACT_STRIDE;
        this.__data[off] = lax;
        this.__data[off + 1] = lay;
        this.__data[off + 2] = laz;
        this.__data[off + 3] = lbx;
        this.__data[off + 4] = lby;
        this.__data[off + 5] = lbz;
        this.__data[off + 6] = nx;
        this.__data[off + 7] = ny;
        this.__data[off + 8] = nz;
        this.__data[off + 9] = depth;
        // j_n, j_t1, j_t2 (offsets 10..12) are warm-start; preserved across calls.
        this.__data[off + 13] = feature_id;
        this.__data[off + 14] = friction;
        this.__data[off + 15] = restitution;

        const meta_off = slot * SLOT_META_STRIDE;
        const count_now = this.__meta[meta_off + 2] & COUNT_MASK;
        if (idx + 1 > count_now) {
            this.__meta[meta_off + 2] = (this.__meta[meta_off + 2] & ~COUNT_MASK) | (idx + 1);
        }
    }

    /**
     * Read the per-contact combined friction stored at one contact.
     * @param {number} slot
     * @param {number} idx
     * @returns {number}
     */
    friction_of(slot, idx) {
        return this.__data[slot * SLOT_DATA_STRIDE + idx * CONTACT_STRIDE + 14];
    }

    /**
     * Read the per-contact combined restitution stored at one contact.
     * @param {number} slot
     * @param {number} idx
     * @returns {number}
     */
    restitution_of(slot, idx) {
        return this.__data[slot * SLOT_DATA_STRIDE + idx * CONTACT_STRIDE + 15];
    }

    /**
     * Zero the warm-start impulse fields (j_n, j_t1, j_t2) for one contact
     * index inside a slot. Called by the narrowphase match-and-merge pass
     * after writing a candidate that did NOT match any prior-frame contact:
     * the geometry at this index is "new" and inheriting impulses from
     * whatever happened to be cached at this index last frame would
     * destabilise the solver.
     *
     * Distinct from {@link clear_contacts} (which wipes the entire slot's
     * data slab including geometry) — `clear_impulses` only touches the
     * three impulse fields at one contact index, leaving the geometric
     * fields and feature_id intact.
     *
     * @param {number} slot
     * @param {number} idx
     */
    clear_impulses(slot, idx) {
        const off = slot * SLOT_DATA_STRIDE + idx * CONTACT_STRIDE;
        this.__data[off + 10] = 0;
        this.__data[off + 11] = 0;
        this.__data[off + 12] = 0;
    }

    /**
     * Read the feature_id stored at one contact within a slot.
     * @param {number} slot
     * @param {number} idx
     * @returns {number}
     */
    feature_id_of(slot, idx) {
        return this.__data[slot * SLOT_DATA_STRIDE + idx * CONTACT_STRIDE + 13];
    }

    /**
     * Read a contact point. Writes 10 floats: lax, lay, laz, lbx, lby, lbz, nx, ny, nz, depth.
     * @param {number} slot
     * @param {number} idx
     * @param {number[]|Float64Array} out
     * @param {number} out_offset
     */
    get_contact(slot, idx, out, out_offset) {
        const off = slot * SLOT_DATA_STRIDE + idx * CONTACT_STRIDE;
        for (let i = 0; i < 10; i++) {
            out[out_offset + i] = this.__data[off + i];
        }
    }

    /**
     * Get the contact-normal world x component for a given contact.
     * Provided for testability without exposing the raw Float64Array view.
     * @param {number} slot
     * @param {number} idx
     * @returns {number}
     */
    contact_normal_x(slot, idx) {
        return this.__data[slot * SLOT_DATA_STRIDE + idx * CONTACT_STRIDE + 6];
    }

    /** @param {number} slot @param {number} idx @returns {number} */
    contact_normal_y(slot, idx) {
        return this.__data[slot * SLOT_DATA_STRIDE + idx * CONTACT_STRIDE + 7];
    }

    /** @param {number} slot @param {number} idx @returns {number} */
    contact_normal_z(slot, idx) {
        return this.__data[slot * SLOT_DATA_STRIDE + idx * CONTACT_STRIDE + 8];
    }

    /** @param {number} slot @param {number} idx @returns {number} */
    contact_depth(slot, idx) {
        return this.__data[slot * SLOT_DATA_STRIDE + idx * CONTACT_STRIDE + 9];
    }

    /**
     * Raw contact-point buffer. Hot-path solver code dereferences this
     * directly via {@link slot_data_offset} + {@link CONTACT_STRIDE} to avoid
     * per-field accessor overhead. Buffer identity is stable until grow.
     * @returns {Float64Array}
     */
    get data_buffer() {
        return this.__data;
    }

    /**
     * Word offset into {@link data_buffer} where the first contact of `slot`
     * begins. Subsequent contacts within the slot are at
     * `offset + c * CONTACT_STRIDE`.
     * @param {number} slot
     * @returns {number}
     */
    slot_data_offset(slot) {
        return slot * SLOT_DATA_STRIDE;
    }

    /**
     * Advance touched flags by one frame and evict slots that have been
     * untouched for {@link DEFAULT_GRACE} consecutive frames.
     */
    advance_frame() {
        // Iterate live slots backwards so swap-with-last during release doesn't
        // skip indices.
        for (let i = this.__live_count - 1; i >= 0; i--) {
            const slot = this.__live_slots[i];
            const meta_off = slot * SLOT_META_STRIDE;
            let packed = this.__meta[meta_off + 2];

            const was_touched = (packed & FLAG_TOUCHED) !== 0;
            // Roll touched → prev_touched, clear touched.
            if (was_touched) {
                packed |= FLAG_PREV_TOUCHED; // single-bit mask: clear-then-set is just set
                packed &= ~FLAG_TOUCHED;
                // Reset grace.
                packed &= ~(GRACE_MASK << GRACE_SHIFT);
                this.__meta[meta_off + 2] = packed;
            } else {
                // Bump grace.
                const grace = ((packed >>> GRACE_SHIFT) & GRACE_MASK) + 1;
                packed &= ~FLAG_PREV_TOUCHED;
                packed &= ~(GRACE_MASK << GRACE_SHIFT);
                packed |= (grace & GRACE_MASK) << GRACE_SHIFT;
                this.__meta[meta_off + 2] = packed;

                if (grace >= DEFAULT_GRACE) {
                    this.__release_slot(slot);
                }
            }
        }
    }

    /**
     * @private
     * @returns {number} newly-allocated slot id
     */
    __allocate_slot() {
        let slot;
        if (this.__free_count > 0) {
            slot = this.__heap_pop();
        } else {
            if (this.__high_water === this.__capacity) {
                this.__grow();
            }
            slot = this.__high_water++;
        }
        const pos = this.__live_count++;
        this.__live_slots[pos] = slot;
        this.__live_pos[slot] = pos;
        return slot;
    }

    /**
     * @private
     * @param {number} slot
     */
    __release_slot(slot) {
        const meta_off = slot * SLOT_META_STRIDE;
        const idA = this.__meta[meta_off];
        const idB = this.__meta[meta_off + 1];

        this.__pair_index.delete(idA, idB);

        // Remove from dense live list (swap with last).
        const pos = this.__live_pos[slot];
        const last_pos = --this.__live_count;
        if (pos !== last_pos) {
            const swapped = this.__live_slots[last_pos];
            this.__live_slots[pos] = swapped;
            this.__live_pos[swapped] = pos;
        }
        this.__live_pos[slot] = -1;

        this.__heap_push(slot);
    }

    /**
     * @private
     */
    __grow() {
        const new_capacity = this.__capacity << 1;
        this.__capacity = new_capacity;

        const grow_f64 = (a, stride) => {
            const n = new Float64Array(new_capacity * stride);
            n.set(a);
            return n;
        };
        const grow_u32 = (a, stride) => {
            const n = new Uint32Array(new_capacity * stride);
            n.set(a);
            return n;
        };

        this.__data = grow_f64(this.__data, SLOT_DATA_STRIDE);
        this.__meta = grow_u32(this.__meta, SLOT_META_STRIDE);

        const next_live = new Uint32Array(new_capacity);
        next_live.set(this.__live_slots);
        this.__live_slots = next_live;

        const next_live_pos = new Int32Array(new_capacity);
        next_live_pos.set(this.__live_pos);
        next_live_pos.fill(-1, this.__live_pos.length);
        this.__live_pos = next_live_pos;

        const next_free = new Uint32Array(new_capacity);
        next_free.set(this.__free_heap);
        this.__free_heap = next_free;
    }

    /**
     * @private
     * @param {number} v
     */
    __heap_push(v) {
        const heap = this.__free_heap;
        let i = this.__free_count++;
        heap[i] = v;
        while (i > 0) {
            const parent = (i - 1) >>> 1;
            if (heap[parent] > heap[i]) {
                const tmp = heap[parent];
                heap[parent] = heap[i];
                heap[i] = tmp;
                i = parent;
            } else break;
        }
    }

    /**
     * @private
     * @returns {number}
     */
    __heap_pop() {
        const heap = this.__free_heap;
        const result = heap[0];
        const new_count = --this.__free_count;
        if (new_count > 0) {
            heap[0] = heap[new_count];
            let i = 0;
            while (true) {
                const l = (i << 1) + 1;
                const r = l + 1;
                let smallest = i;
                if (l < new_count && heap[l] < heap[smallest]) smallest = l;
                if (r < new_count && heap[r] < heap[smallest]) smallest = r;
                if (smallest === i) break;
                const tmp = heap[i];
                heap[i] = heap[smallest];
                heap[smallest] = tmp;
                i = smallest;
            }
        }
        return result;
    }
}
