#version 450
#extension GL_GOOGLE_include_directive : require
layout(local_size_x = 64) in;
#include "rigid_common.glsl"
layout(std430, set = 0, binding = 8) readonly buffer Tables { uint tbl[]; };   // after the noise words: per material friction and restitution of a rigid contact (6 words per material precede)

// One sub-step of the rigid-body engine, one workgroup per body (a thread per outline sample). Port of PhysicsWorld::step.
shared vec4 sP, sV;                       // p, v of the body being solved
shared int accN, accPX, accPY, accNX, accNY, accD, accWet, woke;
shared float sImpact;   // fastest approach speed into a contact during this sub-step (fragile bodies take damage from it)

void contact_static(inout vec3 u, vec2 r, vec2 n, float e, float mu, float invM, float invI) {
    vec2 vc = u.xy + u.z * vec2(-r.y, r.x);
    float vn = dot(vc, n);
    if (vn >= 0.0) return;
    float rn = r.x * n.y - r.y * n.x;
    float k = invM + rn * rn * invI;
    float rest = -vn < 0.1667 ? 0.0 : e;   // 009c1ec0: restitution only for vn < -1 unit/s (10 px/s = 0.1667 cells per frame, pixel scale 10)
    float j = -(1.0 + rest) * vn / k;
    u.xy += j * n * invM;
    u.z += invI * (r.x * (j * n.y) - r.y * (j * n.x));
    vec2 wc = u.xy + u.z * vec2(-r.y, r.x);
    vec2 t = vec2(-n.y, n.x);
    float vt = dot(wc, t);
    float rt = r.x * t.y - r.y * t.x;
    float kt = invM + rt * rt * invI;
    float jt = clamp(-vt / kt, -mu * j, mu * j);
    u.xy += jt * t * invM;
    u.z += invI * (r.x * (jt * t.y) - r.y * (jt * t.x));
}

vec2 snap_axis(vec2 g) {
    if (abs(g.x) > 2.0 * abs(g.y)) g.y = 0.0; else if (abs(g.y) > 2.0 * abs(g.x)) g.x = 0.0;
    return g;
}

vec2 terrain_normal(ivec2 c) {
    vec2 g = vec2(0.0);
    for (int dy = -2; dy <= 2; ++dy)
        for (int dx = -2; dx <= 2; ++dx)
            if ((dx != 0 || dy != 0) && solid_at(c + ivec2(dx, dy))) {
                float d = length(vec2(dx, dy));
                g -= vec2(dx, dy) / (d * d);
            }
    g = snap_axis(g);
    float l = length(g);
    return l < 1e-6 ? vec2(0.0, -1.0) : g / l;
}

// A world point inside the mask of body C? Gives the outward normal of C's surface (world, pointing into the point's owner) and the depth.
bool mask_contact(vec2 wpnt, vec4 cp, int sc, out vec2 nw, out float depth) {
    float qc = cos(cp.z), qs = sin(cp.z);
    vec2 d = wpnt - cp.xy;
    vec2 l = vec2(qc * d.x + qs * d.y, -qs * d.x + qc * d.y);
    vec2 im = l + vec2(shf(sc, 5), shf(sc, 6));
    ivec2 ic = ivec2(floor(im));
    nw = vec2(0.0, -1.0); depth = 0.5;
    if (!shape_solid(sc, ic.x, ic.y)) return false;
    vec2 g = vec2(0.0);
    for (int dy = -2; dy <= 2; ++dy)
        for (int dx = -2; dx <= 2; ++dx)
            if ((dx != 0 || dy != 0) && shape_solid(sc, ic.x + dx, ic.y + dy)) {
                float dd = length(vec2(dx, dy));
                g -= vec2(dx, dy) / (dd * dd);
            }
    g = snap_axis(g);
    float gl = length(g);
    if (gl < 1e-6) { g = l; gl = length(g); if (gl < 1e-6) { g = vec2(0.0, -1.0); gl = 1.0; } }
    g /= gl;
    for (float t = 0.5; t <= 6.0; t += 0.5) {
        depth = t;
        if (!shape_solid(sc, int(floor(im.x + g.x * t)), int(floor(im.y + g.y * t)))) break;
    }
    nw = rot2(qc, qs, g);
    return true;
}

void add_contact(vec2 rel, vec2 n, float depth) {   // rel: world point relative to the centre of mass of the body being solved
    atomicAdd(accN, 1);
    atomicAdd(accPX, int(round(rel.x * 256.0)));
    atomicAdd(accPY, int(round(rel.y * 256.0)));
    atomicAdd(accNX, int(round(n.x * 1024.0)));
    atomicAdd(accNY, int(round(n.y * 1024.0)));
    atomicMax(accD, int(depth * 256.0));
}

void main() {
    int bi = int(gl_WorkGroupID.x);
    int t = int(gl_LocalInvocationID.x);
    if (bi >= bhdr.x) return;
    Body B = bodies[bi];
    bool asleep = (B.i.z & B_ASLEEP) != 0;
    vec2 wp0 = B.p.xy - vec2(pc.view_x, pc.view_y);
    if ((B.i.z & B_REMOVED) != 0 || wp0.x < 4.0 || wp0.y < 4.0 || wp0.x > float(pc.gw - 4) || wp0.y > float(pc.gh - 4)) {   // frozen outside the window
        if (t == 0) bnext[bi] = B;
        return;
    }
    if ((B.i.z & B_NAILED) != 0 && (B.i.z & B_REMOVED) == 0) {   // hangs on the wall while there is terrain around the nail
        vec2 org = B.p.xy + rot2(cos(B.p.z), sin(B.p.z), B.q.xy - vec2(shf(B.i.x, 5), shf(B.i.x, 6)));
        ivec2 nc = ivec2(floor(org + B.n.yz));
        bool held = false;
        int nr = int(B.n.w);
        for (int dy = -nr; dy <= nr; ++dy)
            for (int dx = -nr; dx <= nr; ++dx) held = held || (dx * dx + dy * dy <= nr * nr && solid_at(nc + ivec2(dx, dy)));   // a surface near the nail point
        if (held && B.l.w > 0.0) {   // the joint breaks when its reaction force is above (mass * 160) * break_force (docs/IMPLEMENTATION_BLOCKERS.md): the nail holds the body still, so the
            // reaction is the whole acceleration it would have had: gravity plus the velocity change the impulses of this frame gave it (explosions, kicks), in cells per frame^2 (x 360 = m/s^2)
            const vec2 a = B.v.xy + vec2(0.0, 0.1667);
            const float lim = 160.0 * B.l.w / 360.0;
            if (dot(a, a) > lim * lim) held = false;
        }
        if (held) { B.v.xy = vec2(0.0); B.p.w = 0.0; if (t == 0) bnext[bi] = B; return; }   // (the impulses of the frame went into the wall)
        B.i.z &= ~(B_NAILED | B_ASLEEP);   // the wall is gone or the joint broke: it falls
        B.i.w = 0;
        asleep = false;
    }
    int s = B.i.x;
    int nS = shi(s, 4);
    float mass = shf(s, 7), inertia = shf(s, 8), radius = shf(s, 9);
    float invM = 1.0 / (mass * B.q.z), invI = 1.0 / (inertia * B.q.z);
    const float dt = 0.5;
    if (t == 0) { sP = B.p; sV = B.v; accWet = 0; woke = 0; accN = 0; accPX = 0; accPY = 0; accNX = 0; accNY = 0; accD = 0; sImpact = 0.0; }
    barrier();

    if (!asleep) {
        // buoyancy and drag in liquids
        // 00bda390 / 00bde0f0 (docs/LIQUID_PHYSICS_COMPLETE_GUIDE.md section 8): ONE probe at (int(x), int(y) + 8) decides; in a liquid (not a powder) the force is -b m (v + g gravity_scale) at the centre,
        // b = the component's buoyancy (PhysicsBody2Component constructor 00672980: 0.7). dv/dt = g (1 - b) - b v, no submerged fraction, no density, no extra rotation damping.
        float c = cos(sP.z), sn = sin(sP.z);
        if (t == 0) {   // the probe sits at the ENTITY ORIGIN (the image point B.q, not the centre of mass) plus 8 in y, float coordinates truncating toward zero
            const vec2 org = sP.xy + rot2(c, sn, B.q.xy - vec2(shf(s, 5), shf(s, 6)));
            if (liquid_at(ivec2(int(org.x), int(org.y) + 8))) accWet = 1;
        }
        barrier();
        if (t == 0) {
            const float buoy = B.d.x * float(accWet);   // the component's buoyancy
            vec4 P = sP, V = sV;
            const float h = dt / 60.0;   // 009be020: v = (v + h (g + F / m)) / (1 + h linear_damping), the damping acts on the gravity just added
            V.y += 0.1667 * dt * (1.0 - buoy);
            V.xy *= (1.0 - buoy * dt / 60.0) / (1.0 + h * B.d.y);
            P.w /= 1.0 + h * B.d.z;
            float sp = length(V.xy);
            if (sp > 2500.0 / 60.0) V.xy *= (2500.0 / 60.0) / sp;   // the initial cap: 250 physics units per second (x 10 pixels)
            P.w = clamp(P.w, -1.5707963, 1.5707963);   // 009be020: |h omega| <= pi / 2
            vec2 stepv = V.xy * dt;
            if (length(stepv) > 20.0) stepv *= 20.0 / length(stepv);   // after the constraints a translation of at most 2 physics units (20 pixels) per step
            P.xy += stepv;
            P.z += P.w * dt;
            sP = P; sV = V;
        }
        barrier();
        // terrain contacts: outline samples inside solid cells form one aggregated manifold
        c = cos(sP.z); sn = sin(sP.z);
        if (t < nS) {
            vec2 r = rot2(c, sn, sample_of(s, t));
            vec2 pnt = sP.xy + r;
            ivec2 cell = ivec2(floor(pnt));
            if (solid_at(cell)) {
                vec2 nm = terrain_normal(cell);
                float depth = 0.5;
                for (float tt = 0.5; tt <= 6.0; tt += 0.5) {
                    depth = tt;
                    if (!solid_at(ivec2(floor(pnt + nm * tt)))) break;
                }
                add_contact(r, nm, depth);
            }
        }
        barrier();
        if (t == 0 && accN > 0) {
            vec2 cp = vec2(float(accPX), float(accPY)) / (256.0 * float(accN));
            vec2 an = vec2(float(accNX), float(accNY)) / 1024.0;
            float al = length(an);
            if (al > 1e-6) {
                vec3 u = vec3(sV.xy, sP.w);
                sImpact = max(sImpact, -dot(u.xy + u.z * vec2(-cp.y, cp.x), an / al));
                // the contacted terrain material: its own solid_friction / solid_restitution (009c18c0: friction sqrt(fA fB), restitution max)
                float tf = 0.3, tr = 0.2;
                {
                    const ivec2 cc = ivec2(floor(sP.xy + cp));
                    for (int k = 0; k < 9; ++k) {
                        const ivec2 q = cc + ivec2(k % 3 - 1, k / 3 - 1) - ivec2(pc.view_x, pc.view_y);
                        if (q.x < 0 || q.y < 0 || q.x >= pc.gw || q.y >= pc.gh) continue;
                        const uint tm = cell_mat(cells[q.y * pc.gw + q.x]);
                        if (tm != 0u && (mflags(tm) & F_STATIC) != 0u) {
                            const uint tb = 1024u + 6u * mats[mats.length() - 1u].w + 2u * tm;
                            tf = uintBitsToFloat(tbl[tb]); tr = uintBitsToFloat(tbl[tb + 1u]);
                            break;
                        }
                    }
                }
                for (int it = 0; it < 2; ++it) contact_static(u, cp, an / al, max(B.v.z, tr), sqrt(B.v.w * tf), invM, invI);
                sV.xy = u.xy; sP.w = u.z;
                float push = min(float(accD) / 256.0, 3.0) * 0.7;
                sP.xy += an / al * push;
            }
        }
        barrier();
    }

    // contacts with other bodies: my outline in their mask and theirs in mine
    for (int j = 0; j < bhdr.x; ++j) {
        if (j == bi) continue;
        Body C = bodies[j];
        if ((C.i.z & B_REMOVED) != 0) continue;
        if (B.m.x > 0.5 && B.m.x == C.m.x) continue;   // parts of one corpse do not collide with each other
        bool casleep = (C.i.z & B_ASLEEP) != 0;
        if (asleep && casleep) continue;
        int sc = C.i.x;
        float rc = shf(sc, 9);
        if (distance(sP.xy, C.p.xy) > radius + rc) continue;
        if (t == 0) { accN = 0; accPX = 0; accPY = 0; accNX = 0; accNY = 0; accD = 0; }
        barrier();
        vec4 myp = vec4(sP.xy, sP.z, 0.0);
        float c = cos(sP.z), sn = sin(sP.z);
        if (t < nS) {
            vec2 w = sP.xy + rot2(c, sn, sample_of(s, t));
            vec2 nw; float depth;
            if (mask_contact(w, C.p, sc, nw, depth)) add_contact(w - sP.xy, nw, depth);
        }
        int ncs = shi(sc, 4);
        if (t < ncs) {
            float cc = cos(C.p.z), cs = sin(C.p.z);
            vec2 w = C.p.xy + rot2(cc, cs, sample_of(sc, t));
            vec2 nw; float depth;
            if (mask_contact(w, myp, s, nw, depth)) add_contact(w - sP.xy, -nw, depth);
        }
        barrier();
        if (t == 0 && accN > 0) {
            if (asleep && !casleep && length(C.v.xy) > 0.15) woke = 1;   // a moving neighbour wakes a sleeping body; resting contacts do not
            if (!asleep) {
                float invMc = 1.0 / (shf(sc, 7) * C.q.z), invIc = 1.0 / (shf(sc, 8) * C.q.z);
                float e = max(B.v.z, C.v.z), mu = sqrt(B.v.w * C.v.w);
                vec2 ra = vec2(float(accPX), float(accPY)) / (256.0 * float(accN));
                vec2 rcv = sP.xy + ra - C.p.xy;
                vec2 sn_ = vec2(float(accNX), float(accNY)) / 1024.0;
                float al = length(sn_);
                if (al > 1e-6) {
                    vec2 n = sn_ / al;
                    vec3 u = vec3(sV.xy, sP.w), uc = vec3(C.v.xy, C.p.w);   // uc follows the partner's response virtually, only my side is applied
                    for (int it = 0; it < 2; ++it) {
                        vec2 va = u.xy + u.z * vec2(-ra.y, ra.x), vc = uc.xy + uc.z * vec2(-rcv.y, rcv.x);
                        float vn = dot(va - vc, n);
                        if (vn >= 0.0) break;
                        if (it == 0) sImpact = max(sImpact, -vn);
                        float ran = ra.x * n.y - ra.y * n.x, rcn = rcv.x * n.y - rcv.y * n.x;
                        float kk = invM + invMc + ran * ran * invI + rcn * rcn * invIc;
                        float jn = -(1.0 + (-vn < 0.1667 ? 0.0 : e)) * vn / kk;
                        u.xy += jn * n * invM; u.z += invI * (ra.x * (jn * n.y) - ra.y * (jn * n.x));
                        uc.xy -= jn * n * invMc; uc.z -= invIc * (rcv.x * (jn * n.y) - rcv.y * (jn * n.x));
                        vec2 wa = u.xy + u.z * vec2(-ra.y, ra.x), wc = uc.xy + uc.z * vec2(-rcv.y, rcv.x);
                        vec2 tg = vec2(-n.y, n.x);
                        float vt = dot(wa - wc, tg);
                        float rat = ra.x * tg.y - ra.y * tg.x, rct = rcv.x * tg.y - rcv.y * tg.x;
                        float kt = invM + invMc + rat * rat * invI + rct * rct * invIc;
                        float jt = clamp(-vt / kt, -mu * jn, mu * jn);
                        u.xy += jt * tg * invM; u.z += invI * (ra.x * (jt * tg.y) - ra.y * (jt * tg.x));
                        uc.xy -= jt * tg * invMc; uc.z -= invIc * (rcv.x * (jt * tg.y) - rcv.y * (jt * tg.x));
                    }
                    sV.xy = u.xy; sP.w = u.z;
                    float push = min(float(accD) / 256.0, 3.0) * 0.6;
                    sP.xy += n * push * invM / (invM + invMc);
                }
            }
        }
        barrier();
    }

    // pin joints (ragdolls): the two anchors are pulled together; every body applies its own side of the impulse
    int brokenJoints = 0;
    if (t == 0) {
        for (int k = 0; k < 4; ++k) {
            int j = B.jn[k];
            if (j < 0) continue;
            Body C = bodies[j];
            if ((C.i.z & B_REMOVED) != 0) { brokenJoints |= 1 << k; continue; }   // the partner is gone (cut away, burnt): the joint is dropped and the body wakes
            if (asleep) { if ((C.i.z & B_ASLEEP) == 0 && length(C.v.xy) > 0.15) woke = 1; continue; }
            vec4 ja = k == 0 ? B.ja0 : (k == 1 ? B.ja1 : (k == 2 ? B.ja2 : B.ja3));
            int sc = C.i.x;
            float invMc = 1.0 / (shf(sc, 7) * C.q.z), invIc = 1.0 / (shf(sc, 8) * C.q.z);
            vec2 ra = rot2(cos(sP.z), sin(sP.z), ja.xy), rb = rot2(cos(C.p.z), sin(C.p.z), ja.zw);
            vec2 err = (C.p.xy + rb) - (sP.xy + ra);
            vec2 va = sV.xy + sP.w * vec2(-ra.y, ra.x), vb = C.v.xy + C.p.w * vec2(-rb.y, rb.x);
            vec2 r = (vb - va) + err * 0.35;   // residual relative velocity of the anchors, with a pull toward each other
            float m = invM + invMc;
            mat2 K = mat2(m + invI * ra.y * ra.y + invIc * rb.y * rb.y, -invI * ra.x * ra.y - invIc * rb.x * rb.y,
                          -invI * ra.x * ra.y - invIc * rb.x * rb.y, m + invI * ra.x * ra.x + invIc * rb.x * rb.x);
            vec2 J = inverse(K) * r;
            const float jbk = k == 0 ? B.jb.x : (k == 1 ? B.jb.y : (k == 2 ? B.jb.z : B.jb.w));
            if (jbk != 0.0) {   // force of the joint: impulse / dt, in newtons (mass 0.01 per pixel, 360 = cells/frame^2 -> m/s^2); limit = (m_a + m_b) * 160 * break_force,
                // for the ragdoll kinds 0x2b67 / 0x2b68 (stored negative) max((m_a + m_b) * 400, 200) * break_force
                const float M = (shf(s, 7) * B.q.z + shf(sc, 7) * C.q.z) * 0.01;
                const float F = length(J) * 0.01 * 360.0 / dt, lim = jbk > 0.0 ? M * 160.0 * jbk : max(M * 400.0, 200.0) * -jbk;
                if (F * F > lim * lim) brokenJoints |= 1 << k;
            }
            sV.xy += J * invM;
            sP.w += invI * (ra.x * J.y - ra.y * J.x);
            sP.xy += err * 0.15 * invM / m;   // and a little of the error is corrected directly
            woke = max(woke, 0);
        }
    }
    barrier();
    if (t == 0) {
        if (B.n.x > 0.0 && B.i.y >= 0 && sImpact > B.n.x) atomicAdd(ents[B.i.y].x.x, int((sImpact - B.n.x) * 750.0));   // fall / impact damage of fragile bodies
        Body o = B;
        o.p = sP;
        o.v.xy = sV.xy;
        for (int k = 0; k < 4; ++k) if ((brokenJoints & (1 << k)) != 0) { o.jn[k] = -1; woke = 1; }
        if (woke != 0) { o.i.z &= ~B_ASLEEP; o.i.w = 0; }
        bnext[bi] = o;
    }
}