using System;
using System.Collections.Generic;
using NumericsVector=System.Numerics.Vector3;
using NumericsQuaternion=System.Numerics.Quaternion;
using NumericsMatrix=System.Numerics.Matrix4x4;
namespace Socer;

public static partial class ReplayFigure
{
    // Event-driven inertial articulation: captured impact direction/speed, limb
    // lag, damped ground recoil and pain. No player rigidbodies or frame garbage.
    static void ApplyImpactFall(Footballer p,MatchSimulation m,List<Part> parts,Point forward,Point side)
    {
        var entryLeft=MeasureArm(parts,7);var entryRight=MeasureArm(parts,10);
        float clock=m.InjuryAnimationSeconds(p);
        // Medics lift the settled pose captured at the start of loading.
        // Letting the source keep writhing could reverse the shortest body
        // rotation arc while the player is already halfway onto the stretcher.
        if(m.MedicalPatientId==p.Id && m.MedicalStage==MedicalTreatmentStage.Load)clock=m.MedicalLoadAnimationClock;
        float age=Math.Max(0,clock-p.FallAt);
        float remaining=p.FallUntil-clock;
        float down=HumanFigure.FallBlend(p,m);
        if(p.InjuryCannotContinue)down=Smooth(age/(p.FallVariant==4?.58f:.44f));
        float recovery=p.InjuryCannotContinue?1:Smooth(remaining/.85f);
        float blend=Smooth(age/.16f)*recovery;
        float energy=Math.Clamp(.35f+p.FallSpeed*.09f+p.FallSeverity*.30f,.35f,1.2f);
        float impactAge=Math.Max(0,age-.40f);
        float recoil=MathF.Sin(impactAge*15)*MathF.Exp(-impactAge*5.5f)*.10f*energy*down;
        bool trip=p.FallVariant==0,back=p.FallVariant==1,roll=p.FallVariant==4;
        float handed=p.FallVariant==2?-1:1;
        var axis=p.FallDirection.Length>.01f?p.FallDirection:forward;
        var lateral=new Point(-axis.Y,axis.X);
        bool injured=p.InjurySeverity is InjurySeverity.Minor or InjurySeverity.Serious;
        float pain=injured?Smooth((age-.66f)/.45f)*recovery:0;
        float pushUp=!p.InjuryCannotContinue && remaining<.85f?
            MathF.Sin(Math.Clamp((.85f-remaining)/.85f,0,1)*MathF.PI)*.75f:0;
        float wince=pain*MathF.Pow(Math.Max(0,MathF.Sin((age-.7f)*2.9f+p.Id*.17f)),4);
        float angle=down*1.47f+recoil-wince*.045f;
        float twist=roll?MathF.Sin(Math.Clamp(age/.95f,0,1)*MathF.PI)*.85f*down:
            p.FallVariant>=2?handed*(.24f*down+recoil*.55f):recoil*.35f;
        var bodyForward=forward*MathF.Cos(twist)+side*MathF.Sin(twist);
        var bodySide=new Point(-bodyForward.Y,bodyForward.X);
        float cos=MathF.Cos(angle),sin=MathF.Sin(angle);
        (Point xy,float z) Joint(float across,float ahead,float z)
        {
            var local=bodySide*across+bodyForward*ahead;
            float x=Point.Dot(local,axis),y=Point.Dot(local,lateral),h=z-.86f;
            return(p.Position+lateral*y+axis*(x*cos+h*sin),
                .86f-.57f*down+h*cos-x*sin);
        }
        void Bone(int i,(Point xy,float z) a,(Point xy,float z) b)
        {
            var old=parts[i];parts[i]=old with{
                A=old.A*(1-blend)+a.xy*blend,ZA=old.ZA*(1-blend)+a.z*blend,
                B=old.B*(1-blend)+b.xy*blend,ZB=old.ZB*(1-blend)+b.z*blend};
        }
        float buckle=down*(.7f+p.FallSeverity*.3f);
        for(int leg=0;leg<2;leg++)
        {
            float sign=leg==0?-1:1;
            float lag=MathF.Sin(age*9+leg*1.3f)*MathF.Exp(-Math.Max(0,age-.23f)*3.1f)*
                Smooth(age/.12f)*.22f*energy*down;
            float upper=(trip?(leg==0?-.35f:.45f):back?.75f:roll?1.05f:.35f+leg*.35f)*buckle+lag;
            float lower=(trip?(leg==0?-1.45f:-.55f):back?-.6f:roll?-1.5f:-.8f-leg*.3f)*buckle-lag*.7f;
            if(pain>0 && leg==p.Id%2)
            {
                upper+=(1.10f-upper)*pain;lower+=(-1.45f-lower)*pain;
                upper+=wince*.12f;lower-=wince*.08f;
            }
            float kneeAhead=MathF.Sin(upper)*.405f,kneeZ=.86f-MathF.Cos(upper)*.405f;
            float footAhead=kneeAhead+MathF.Sin(lower)*.405f,footZ=kneeZ-MathF.Cos(lower)*.405f;
            var hip=Joint(sign*.12f,0,.86f);var knee=Joint(sign*.12f,kneeAhead,kneeZ);
            var foot=Joint(sign*.12f,footAhead,footZ);
            Bone(leg*3,hip,knee);Bone(leg*3+1,knee,foot);
            Bone(leg*3+2,foot,Joint(sign*.12f,footAhead+.24f,footZ));
        }
        float chestAhead=(trip?.09f:back?-.05f:.04f)*down+recoil*.09f+wince*.07f;
        float chestZ=.86f+MathF.Sqrt(.56f*.56f-chestAhead*chestAhead);
        Bone(6,Joint(0,0,.86f),Joint(0,chestAhead,chestZ));
        for(int arm=0;arm<2;arm++)
        {
            float sign=arm==0?-1:1;
            // Establish the moving shoulder frame first; the linked FK below
            // supplies humerus/hinge/wrist together after torso restoration.
            int first=7+arm*3;var wanted=Joint(sign*.215f,chestAhead,chestZ-.06f);
            var offset=(wanted.xy-parts[first].A)*blend;float dz=(wanted.z-parts[first].ZA)*blend;
            for(int i=first;i<first+3;i++)
            {var v=parts[i];parts[i]=v with{A=v.A+offset,B=v.B+offset,ZA=v.ZA+dz,ZB=v.ZB+dz};}
        }
        float neck=chestZ+.22f-wince*.025f;
        var head=Joint(0,chestAhead,neck);Bone(13,head,head);
        var hair=Joint(0,chestAhead-.01f,neck+.10f);Bone(14,hair,hair);
        for(int eye=0;eye<2;eye++)
        {
            var at=Joint((eye==0?-1:1)*.052f,chestAhead+.151f,neck+.026f);
            Bone(15+eye,at,at);
        }
        RestoreFallTorso(parts);
        RestoreFallBoneLengths(parts);
        if(!p.InjuryCannotContinue && remaining<.85f)
            ApplyImpactGetUp(parts,p,forward,side,Math.Clamp((.85f-remaining)/.85f,0,1));
        for(int arm=0;arm<2;arm++)
        {
            int slot=7+arm*3,sign=arm==0?-1:1;var entry=arm==0?entryLeft:entryRight;
            var rest=new ArmAngles(entry.ShoulderFlexion,entry.ShoulderAbduction,entry.HumeralTwist,
                entry.ElbowFlexion,10);
            float secondary=MathF.Sin(age*10+arm*1.7f)*MathF.Exp(-Math.Max(0,age-.20f)*3)*
                Smooth(age/.15f)*energy*down;
            // Protective reach belongs in front of the thorax even for a
            // backwards impact. Separate absolute segment angles previously
            // made a nearly straight elbow fold behind its humeral hinge.
            var protect=trip?new ArmAngles(67+secondary*8,36,-12,64+secondary*10,28):
                back?new ArmAngles(34+secondary*8,49,-15,77+secondary*10,22):
                roll?new ArmAngles(52+secondary*8,43,-18,103+secondary*8,30):
                new ArmAngles(sign==handed?57:34,sign==handed?48:34,-12,sign==handed?83:62,25);
            ApplyArmFk(parts,slot,BlendArmAngles(rest,protect,blend));
            var frame=AnatomicalFrame(parts);
            float support=pushUp*blend*Smooth(((.85f-remaining)/.85f-.30f)/.16f);
            if(support>0)
            {
                // Each hand returns to its own knee. Sending both arms toward
                // one knee dragged the far forearm through the rising shirt.
                var knee=parts[arm*3].B;float kz=parts[arm*3].ZB;
                var at=AnatomicalPoint(knee,kz)+frame.Forward*.075f+frame.Up*.045f;
                var pole=-frame.Forward+frame.Side*(sign*.30f);
                float reach=(at-AnatomicalA(parts[slot])).Length();
                support*=1-Smooth((reach-.46f)/.055f);
                ApplyArmContact(parts,slot,AnatomicalGround(at),at.Z,AnatomicalGround(pole),pole.Z,support);
            }
            if(pain>0 && arm==p.Id%2)
            {
                int leg=p.Id%2;int bone=leg*3+(p.InjuryPart==InjuryBodyPart.Ankle?1:0);
                var at=AnatomicalB(parts[bone])+frame.Forward*.07f;
                if(p.InjuryPart==InjuryBodyPart.Shoulder)
                    at=frame.Chest-frame.Side*(sign*.13f)+frame.Forward*.255f-frame.Up*.07f;
                var pole=p.InjuryPart==InjuryBodyPart.Shoulder?
                    -frame.Up+frame.Side*(sign*.40f)+frame.Forward*.60f:
                    -frame.Forward+frame.Side*(sign*.30f);
                float reach=(at-AnatomicalA(parts[slot])).Length();
                float reachable=1-Smooth((reach-.46f)/.055f);
                ApplyArmContact(parts,slot,AnatomicalGround(at),at.Z,AnatomicalGround(pole),pole.Z,pain*(1-pushUp)*blend*reachable);
            }
        }
        KeepFallAboveGround(parts);
        if(m.MedicalPatientId==p.Id && m.MedicalStage is MedicalTreatmentStage.Load or MedicalTreatmentStage.CarryOff)
            ApplyStretcherPatient(p,m,parts);
    }

    static void ApplyImpactGetUp(List<Part> parts,Footballer p,Point forward,Point side,float t)
    {
        float weight=Smooth(t/.40f),rise=Smooth((t-.12f)/.88f);
        var frame=AnatomicalFrame(parts);var root=AnatomicalA(parts[6]);
        var f=AnatomicalPoint(forward,0);var s=AnatomicalPoint(side,0);
        float pitch=1.10f*(1-Smooth(t));
        var targetUp=NumericsVector.UnitZ*MathF.Cos(pitch)+f*MathF.Sin(pitch);
        // A genuine backward fall starts face-up, so its thorax must travel a
        // larger angle before folding over the planted knee. Let that rotation
        // settle longer while the original foot/root planting stays on time.
        float orientDuration=Point.Dot(p.FallDirection,forward)<-.45f?.48f:.40f;
        float orientWeight=Smooth(t/orientDuration);
        var orientation=NumericsQuaternion.Slerp(ArmRotation(-frame.Up,frame.Forward),
            ArmRotation(-targetUp,NumericsVector.Cross(targetUp,s)),orientWeight);
        var up=NumericsVector.Transform(NumericsVector.UnitZ,orientation);
        var front=NumericsVector.Transform(NumericsVector.UnitX,orientation);
        var across=NumericsVector.Transform(NumericsVector.UnitY,orientation);
        // ArmRotation's Y is the opposite of this rig's anatomical side.
        across=-across;
        var target=AnatomicalPoint(p.Position-forward*(.06f*(1-rise)),.34f+.52f*rise);
        var pelvis=NumericsVector.Lerp(root,target,weight);
        for(int i=0;i<parts.Count;i++)
        {
            var v=parts[i];
            NumericsVector Move(NumericsVector at)
            {var d=at-root;return pelvis+up*NumericsVector.Dot(d,frame.Up)+across*NumericsVector.Dot(d,frame.Side)+front*NumericsVector.Dot(d,frame.Forward);}
            var a=Move(AnatomicalA(v));var b=Move(AnatomicalB(v));
            parts[i]=v with{A=AnatomicalGround(a),ZA=a.Z,B=AnatomicalGround(b),ZB=b.Z};
        }
        for(int leg=0;leg<2;leg++)
        {
            int i=leg*3,sign=leg==0?-1:1;var thigh=parts[i];
            var hip=pelvis+across*(sign*.12f);
            parts[i]=thigh with{A=AnatomicalGround(hip),ZA=hip.Z};
            float close=Smooth((t-(leg==0?.53f:.75f))/(leg==0?.25f:.25f));
            var planted=p.Position+side*(sign*.18f)+forward*((leg==0?.26f:-.21f)*(1-close));
            var ankle=parts[i+1];
            SolveKeeperLimb(AnatomicalGround(hip),hip.Z,planted,.06f,.405f,.405f,forward,0,
                out var targetKnee,out float targetKneeZ,out var targetAnkle,out float targetAnkleZ);
            var oldUpper=UnitArm(AnatomicalB(thigh)-AnatomicalA(thigh),-up);
            var oldLower=UnitArm(AnatomicalB(ankle)-AnatomicalA(ankle),-up);
            var wantedUpper=UnitArm(AnatomicalPoint(targetKnee,targetKneeZ)-hip,-up);
            var wantedLower=UnitArm(AnatomicalPoint(targetAnkle,targetAnkleZ)-AnatomicalPoint(targetKnee,targetKneeZ),-up);
            // Blend the hip frame and the knee's hinge angle, not an ankle
            // chord passing through the hip. That chord made the IK pole flip
            // while a coiled falling leg was being replanted on the turf.
            float oldFlex=MathF.Atan2(NumericsVector.Dot(oldUpper,front),-NumericsVector.Dot(oldUpper,up));
            float wantedFlex=MathF.Atan2(NumericsVector.Dot(wantedUpper,front),-NumericsVector.Dot(wantedUpper,up));
            // A coiled thigh can pass the +/-PI chart seam as the thorax
            // rolls upright. Keep its positive hip-flexion branch continuous.
            if(oldFlex<-MathF.PI*.5f)oldFlex+=MathF.Tau;
            if(wantedFlex<-MathF.PI*.5f)wantedFlex+=MathF.Tau;
            float flex=oldFlex*(1-weight)+wantedFlex*weight;
            float oldSpread=MathF.Asin(Math.Clamp(NumericsVector.Dot(oldUpper,across*sign),-1,1));
            float wantedSpread=MathF.Asin(Math.Clamp(NumericsVector.Dot(wantedUpper,across*sign),-1,1));
            float spread=oldSpread*(1-weight)+wantedSpread*weight;
            var upper=front*(MathF.Sin(flex)*MathF.Cos(spread))+across*(sign*MathF.Sin(spread))-
                up*(MathF.Cos(flex)*MathF.Cos(spread));
            var bend=-front*MathF.Cos(flex)-up*MathF.Sin(flex);
            var contactBend=UnitArm(ArmPlane(wantedLower,wantedUpper),bend);
            bend=UnitArm(ArmPlane(BlendFallBoneDirection(bend,contactBend,weight,1,across),upper),bend);
            float kneeAngle=(ArmAngle(oldUpper,oldLower)*(1-weight)+ArmAngle(wantedUpper,wantedLower)*weight)*ArmDegrees;
            var knee=hip+upper*.405f;
            var foot=knee+(upper*MathF.Cos(kneeAngle)+bend*MathF.Sin(kneeAngle))*.405f;
            parts[i]=thigh with{A=AnatomicalGround(hip),ZA=hip.Z,B=AnatomicalGround(knee),ZB=knee.Z};
            parts[i+1]=ankle with{A=AnatomicalGround(knee),ZA=knee.Z,B=AnatomicalGround(foot),ZB=foot.Z};
            var oldBoot=parts[i+2];
            var toe=BlendFallBoneDirection(AnatomicalB(oldBoot)-AnatomicalA(oldBoot),f,weight,1,frame.Up);
            parts[i+2]=oldBoot with{A=AnatomicalGround(foot),ZA=foot.Z,
                B=AnatomicalGround(foot+toe*.24f),ZB=foot.Z+toe.Z*.24f};
        }
    }

    static void RestoreFallTorso(List<Part> parts)
    {
        var torso=parts[6];var delta=torso.B-torso.A;float z=torso.ZB-torso.ZA;
        float length=MathF.Sqrt(delta.Length*delta.Length+z*z);
        if(length<.0001f)return;
        var chest=torso.A+delta*(.56f/length);float chestZ=torso.ZA+z*(.56f/length);
        var shift=chest-torso.B;float raise=chestZ-torso.ZB;
        parts[6]=torso with{B=chest,ZB=chestZ};
        // Shoulders, head, hair and eyes share the corrected ribcage. Stretching
        // only the torso would otherwise detach every upper-body attachment.
        for(int i=7;i<parts.Count;i++)
        {var v=parts[i];parts[i]=v with{A=v.A+shift,B=v.B+shift,ZA=v.ZA+raise,ZB=v.ZB+raise};}
        var across=parts[10].A-parts[7].A;float acrossZ=parts[10].ZA-parts[7].ZA;
        var up=delta*(1/length);float upZ=z/length;
        float projection=Point.Dot(across,up)+acrossZ*upZ;
        across-=up*projection;acrossZ-=upZ*projection;
        float width=MathF.Sqrt(across.Length*across.Length+acrossZ*acrossZ);
        if(width<.001f)return;
        across*=.215f/width;acrossZ*=.215f/width;
        var shoulderCentre=chest-up*.06f;float shoulderCentreZ=chestZ-upZ*.06f;
        for(int arm=0;arm<2;arm++)
        {
            int first=7+arm*3;float sign=arm==0?-1:1;
            var wanted=shoulderCentre+across*sign;float wantedZ=shoulderCentreZ+acrossZ*sign;
            var offset=wanted-parts[first].A;float offsetZ=wantedZ-parts[first].ZA;
            for(int i=first;i<first+3;i++)
            {var v=parts[i];parts[i]=v with{A=v.A+offset,B=v.B+offset,ZA=v.ZA+offsetZ,ZB=v.ZB+offsetZ};}
        }
    }

    static void RestoreFallBoneLengths(List<Part> parts)
    {
        void Chain(int first,float upper,float lower)
        {
            var a=parts[first];var b=parts[first+1];var end=parts[first+2];
            var upperDelta=a.B-a.A;float upperZ=a.ZB-a.ZA;
            float n=MathF.Sqrt(upperDelta.Length*upperDelta.Length+upperZ*upperZ);
            if(n<.00001f){upperDelta=new Point();upperZ=-1;n=1;}
            var joint=a.A+upperDelta*(upper/n);float jz=a.ZA+upperZ*(upper/n);
            var lowerDelta=b.B-b.A;float lowerZ=b.ZB-b.ZA;
            n=MathF.Sqrt(lowerDelta.Length*lowerDelta.Length+lowerZ*lowerZ);
            if(n<.00001f){lowerDelta=new Point();lowerZ=-1;n=1;}
            var at=joint+lowerDelta*(lower/n);float z=jz+lowerZ*(lower/n);
            parts[first]=a with{B=joint,ZB=jz};
            parts[first+1]=b with{A=joint,ZA=jz,B=at,ZB=z};
            parts[first+2]=end with{A=at,ZA=z,B=at+(end.B-end.A),ZB=z+(end.ZB-end.ZA)};
        }
        Chain(0,.405f,.405f);Chain(3,.405f,.405f);Chain(7,.27f,.25f);Chain(10,.27f,.25f);
    }

    static void KeepFallAboveGround(List<Part> parts)
    {
        float minimum=0;
        foreach(var v in parts)minimum=Math.Min(minimum,Math.Min(v.ZA,v.ZB)-Math.Min(.12f,v.Width*.5f));
        if(minimum>=0)return;
        for(int i=0;i<parts.Count;i++)
        {var v=parts[i];parts[i]=v with{ZA=v.ZA-minimum,ZB=v.ZB-minimum};}
    }

    static NumericsVector BlendFallBoneDirection(NumericsVector from,NumericsVector to,float t,
        float length,NumericsVector preferredPlane)
    {
        float a=from.Length(),b=to.Length();
        if(a<.00001f || b<.00001f)return b>.00001f?to*(length/b):from;
        from/=a;to/=b;
        float dot=Math.Clamp(NumericsVector.Dot(from,to),-1,1);
        if(dot>.9995f)return NumericsVector.Normalize(NumericsVector.Lerp(from,to,t))*length;
        var plane=to-from*dot;
        if(plane.LengthSquared()<.000001f)
        {
            // A stable anatomical plane resolves truly opposed directions.
            plane=preferredPlane-from*NumericsVector.Dot(preferredPlane,from);
            if(plane.LengthSquared()<.000001f)
            {
                var alternate=Math.Abs(from.Z)<.8f?NumericsVector.UnitZ:NumericsVector.UnitX;
                plane=alternate-from*NumericsVector.Dot(alternate,from);
            }
        }
        plane=NumericsVector.Normalize(plane);
        float angle=MathF.Acos(dot)*t;
        return (from*MathF.Cos(angle)+plane*MathF.Sin(angle))*length;
    }

    static NumericsQuaternion BlendFallBodyOrientation(NumericsQuaternion from,NumericsQuaternion to,float t)
    {
        float dot=NumericsQuaternion.Dot(from,to);
        // At a half-turn there are two equally valid lifting paths. Numerical
        // noise around zero must not reverse that path halfway through a lift.
        if(dot<-.002f){to=new(-to.X,-to.Y,-to.Z,-to.W);dot=-dot;}
        if(dot>.9995f)return NumericsQuaternion.Normalize(new(
            from.X*(1-t)+to.X*t,from.Y*(1-t)+to.Y*t,
            from.Z*(1-t)+to.Z*t,from.W*(1-t)+to.W*t));
        float angle=MathF.Acos(Math.Clamp(dot,-1,1));
        float denominator=MathF.Sin(angle);
        float a=MathF.Sin((1-t)*angle)/denominator,b=MathF.Sin(t*angle)/denominator;
        return NumericsQuaternion.Normalize(new(from.X*a+to.X*b,from.Y*a+to.Y*b,
            from.Z*a+to.Z*b,from.W*a+to.W*b));
    }

    static void ApplyStretcherPatient(Footballer p,MatchSimulation m,List<Part> parts)
    {
        float blend=m.MedicalStage==MedicalTreatmentStage.Load?Smooth(m.MedicalStageProgress):1;
        var axis=(m.MedicalExitPoint-m.MedicalIncidentPoint).Unit;
        if(axis.Length<.01f)axis=new Point(0,-1);
        // The simulation flips Y when rendered. This shoulder handedness keeps
        // the shirt and face looking upwards on the stretcher.
        var side=new Point(axis.Y,-axis.X);var pelvis=m.MedicalCarryingPosition+axis*.09f;
        const float height=.96f;
        var originalPelvis=new NumericsVector(parts[6].A.X,parts[6].A.Y,parts[6].ZA);
        var originalUp=NumericsVector.Normalize(new NumericsVector(parts[6].B.X-parts[6].A.X,
            parts[6].B.Y-parts[6].A.Y,parts[6].ZB-parts[6].ZA));
        var originalSide=new NumericsVector(parts[10].A.X-parts[7].A.X,
            parts[10].A.Y-parts[7].A.Y,parts[10].ZA-parts[7].ZA);
        originalSide=NumericsVector.Normalize(originalSide-originalUp*NumericsVector.Dot(originalSide,originalUp));
        var originalFront=NumericsVector.Cross(originalUp,originalSide);
        NumericsQuaternion Frame(NumericsVector up,NumericsVector across)
        {
            var front=NumericsVector.Cross(up,across);
            return NumericsQuaternion.CreateFromRotationMatrix(new NumericsMatrix(
                up.X,up.Y,up.Z,0,across.X,across.Y,across.Z,0,
                front.X,front.Y,front.Z,0,0,0,0,1));
        }
        var carriedUp=new NumericsVector(axis.X,axis.Y,0);
        var carriedSide=new NumericsVector(side.X,side.Y,0);
        var carriedFront=NumericsVector.Cross(carriedUp,carriedSide);
        var carriedRoot=new NumericsVector(pelvis.X,pelvis.Y,height);
        var orientation=BlendFallBodyOrientation(Frame(originalUp,originalSide),Frame(carriedUp,carriedSide),blend);
        var up=NumericsVector.Transform(NumericsVector.UnitX,orientation);
        var across=NumericsVector.Transform(NumericsVector.UnitY,orientation);
        var front=NumericsVector.Transform(NumericsVector.UnitZ,orientation);
        // Translation is blended once by Bone below. Rotating the source rig
        // first prevents opposite poses from collapsing through the pelvis.
        var root=originalPelvis;
        (Point,float) Lift(Point at,float z)
        {
            var local=new NumericsVector(at.X,at.Y,z)-originalPelvis;
            var transformed=root+up*NumericsVector.Dot(local,originalUp)+
                across*NumericsVector.Dot(local,originalSide)+front*NumericsVector.Dot(local,originalFront);
            return (new Point(transformed.X,transformed.Y),transformed.Z);
        }
        for(int i=0;i<parts.Count;i++)
        {
            var part=parts[i];var a=Lift(part.A,part.ZA);var b=Lift(part.B,part.ZB);
            parts[i]=part with{A=a.Item1,ZA=a.Item2,B=b.Item1,ZB=b.Item2};
        }
        void Bone(int i,Point a,float az,Point b,float bz)
        {
            NumericsVector Target(Point at,float z)
            {
                var local=new NumericsVector(at.X,at.Y,z)-carriedRoot;
                return carriedRoot+up*NumericsVector.Dot(local,carriedUp)+
                    across*NumericsVector.Dot(local,carriedSide)+front*NumericsVector.Dot(local,carriedFront);
            }
            // Both limb shapes interpolate in the SAME moving torso frame.
            // Mixing a partly rotated coiled leg with an already horizontal
            // world-space leg can cross a zero-length direction and flip it.
            var targetA=Target(a,az);var targetB=Target(b,bz);
            a=new Point(targetA.X,targetA.Y);az=targetA.Z;
            b=new Point(targetB.X,targetB.Y);bz=targetB.Z;
            var old=parts[i];var start=NumericsVector.Lerp(new(old.A.X,old.A.Y,old.ZA),targetA,blend);
            float length=i is 0 or 1 or 3 or 4?.405f:i is 7 or 10?.27f:i is 8 or 11?.25f:i==6?.56f:0;
            var end=length>0?start+BlendFallBoneDirection(new(old.B.X-old.A.X,old.B.Y-old.A.Y,old.ZB-old.ZA),
                targetB-targetA,blend,length,front):NumericsVector.Lerp(new(old.B.X,old.B.Y,old.ZB),targetB,blend);
            parts[i]=old with{A=new(start.X,start.Y),ZA=start.Z,B=new(end.X,end.Y),ZB=end.Z};
        }
        for(int leg=0;leg<2;leg++)
        {
            float sign=leg==0?-1:1;int i=leg*3;
            var hip=pelvis+side*(sign*.12f);var knee=hip-axis*.400f;
            var foot=knee-axis*.400f;
            Bone(i,hip,height,knee,height+.063f);Bone(i+1,knee,height+.063f,foot,height);
            Bone(i+2,foot,height,foot,height+.16f);
        }
        Bone(6,pelvis,height,pelvis+axis*.56f,height);
        for(int arm=0;arm<2;arm++)
        {
            float sign=arm==0?-1:1;int i=7+arm*3;
            var shoulder=pelvis+axis*.50f+side*(sign*.215f);
            var elbow=shoulder-axis*.24f+side*(sign*.08f);
            var hand=elbow-axis*.19f-side*(sign*.15f);
            Bone(i,shoulder,height,elbow,height+.0922f);
            Bone(i+1,elbow,height+.0922f,hand,height+.1538f);Bone(i+2,hand,height+.1538f,hand,height+.1538f);
        }
        var head=pelvis+axis*.78f;
        Bone(13,head,height,head,height);Bone(14,head+axis*.10f,height,head+axis*.10f,height);
        for(int eye=0;eye<2;eye++)
        {
            var at=head+side*((eye==0?-1:1)*.052f)+axis*.026f;
            Bone(15+eye,at,height+.151f,at,height+.151f);
        }
        RestoreFallTorso(parts);
        RestoreFallBoneLengths(parts);
        // Loading can rotate a trailing boot through the turf before the
        // body is horizontal. Lift the entire articulated rig, preserving
        // every joint and bone rather than clipping individual endpoints.
        KeepFallAboveGround(parts);
    }

    public static Point MedicalCarryAxis(MatchSimulation m)
    {
        var axis=(m.MedicalExitPoint-m.MedicalIncidentPoint).Unit;
        return axis.Length>.01f?axis:new Point(0,-1);
    }
    public static Point MedicalStretcherCentre(MatchSimulation m)=>m.MedicalStage is
        MedicalTreatmentStage.CarryOff or MedicalTreatmentStage.AwaitSubstitute?
        m.MedicalCarryingPosition:m.MedicalIncidentPoint;

    public static Point MedicalActorPosition(MatchSimulation m,int index)
    {
        var axis=MedicalCarryAxis(m);float sign=index==0?1:-1;
        var centre=MedicalStretcherCentre(m);
        if(m.MedicalStage==MedicalTreatmentStage.Approach)
            centre=m.MedicalExitPoint*(1-Smooth(m.MedicalStageProgress))+
                m.MedicalIncidentPoint*Smooth(m.MedicalStageProgress);
        return centre+axis*(sign*1.24f);
    }

    public static List<Part> PoseMedicalMedic(Footballer medic,MatchSimulation m,int index,List<Part> parts)
    {
        var axis=MedicalCarryAxis(m);float sign=index==0?1:-1;
        bool walking=m.MedicalStage is MedicalTreatmentStage.Approach or MedicalTreatmentStage.CarryOff;
        float progress=m.MedicalStageProgress;
        var travel=m.MedicalStage==MedicalTreatmentStage.Approach?axis*-1:axis;
        medic.Position=MedicalActorPosition(m,index);
        var face=m.MedicalStage==MedicalTreatmentStage.CarryOff?axis:axis*-sign;
        if(m.MedicalStage==MedicalTreatmentStage.Approach)
        {
            float turn=index==1?Smooth((progress-.68f)/.32f)*MathF.PI:0;
            face=new Point(-axis.X*MathF.Cos(turn)+axis.Y*MathF.Sin(turn),
                -axis.X*MathF.Sin(turn)-axis.Y*MathF.Cos(turn));
        }
        else if(m.MedicalStage==MedicalTreatmentStage.Load && index==0)
        {
            // The front carrier turns before walking out, while the stretcher
            // is loaded. Never flip his whole body on the CarryOff boundary.
            float turn=Smooth((progress-.24f)/.76f)*MathF.PI;
            face=new Point(-axis.X*MathF.Cos(turn)+axis.Y*MathF.Sin(turn),
                -axis.X*MathF.Sin(turn)-axis.Y*MathF.Cos(turn));
        }
        float carrying=m.MedicalStage==MedicalTreatmentStage.Load?Smooth(progress):
            m.MedicalStage==MedicalTreatmentStage.CarryOff?1:0;
        medic.Facing=MathF.Atan2(face.Y,face.X);
        float distance=(m.MedicalExitPoint-m.MedicalIncidentPoint).Length;
        // One phase is a complete pair of steps. The former one-metre cycle
        // made rushing medics snap their knees through the stance transition.
        float cycles=m.MedicalStage==MedicalTreatmentStage.Approach?distance*Smooth(progress)/1.9f:
            distance/1.9f+(m.MedicalStage==MedicalTreatmentStage.CarryOff?distance*progress/1.25f:0);
        medic.GaitPhase=(cycles+index*.5f)%1;
        float speed=walking?distance/Math.Max(.1f,m.MedicalStageDuration)*
            (m.MedicalStage==MedicalTreatmentStage.Approach?6*progress*(1-progress):1):0;
        if(m.MedicalStage==MedicalTreatmentStage.CarryOff)speed*=Smooth(progress/.05f)*Smooth((1-progress)/.05f);
        medic.VisualSpeed=Math.Min(carrying>.5f?1.7f:5.6f,speed);
        medic.VisualVelocity=travel*medic.VisualSpeed;medic.VisualAcceleration=new Point();
        medic.Number=0;medic.Team=2;medic.Id=100+index;
        Pose(medic,m,parts);
        var forward=new Point(MathF.Cos(medic.Facing),MathF.Sin(medic.Facing));
        var side=new Point(-forward.Y,forward.X);
        // Staff do not need footballers' planted-ball/contact gait. Use a
        // smooth periodic step with comfortable knee flexion at every phase;
        // an IK reach limit must never straighten then unlock a rushing knee.
        float activity=Math.Clamp(medic.VisualSpeed/1.3f,0,1);
        float phase=medic.GaitPhase*MathF.Tau;
        float hipHeight=.78f+MathF.Sin(phase*2)*.012f*activity;
        float hipShift=hipHeight-parts[6].ZA;
        for(int i=0;i<parts.Count;i++)
        {var part=parts[i];parts[i]=part with{ZA=part.ZA+hipShift,ZB=part.ZB+hipShift};}
        for(int leg=0;leg<2;leg++)
        {
            int i=leg*3;float s=leg==0?-1:1;
            float stridePhase=phase+(leg==0?MathF.PI:0);
            float lift=Math.Max(0,MathF.Sin(stridePhase));lift*=lift;
            var hip=medic.Position+side*(s*.12f);
            parts[i]=parts[i] with{A=hip,ZA=hipHeight};
            var foot=medic.Position+side*(s*.18f)+travel*(MathF.Cos(stridePhase)*.20f*activity);
            KeeperLeg(parts,leg,foot,.065f+lift*.15f*activity,forward,0,forward,0);
        }
        float kneel=m.MedicalStage==MedicalTreatmentStage.Assess?1:
            m.MedicalStage==MedicalTreatmentStage.Load?1-Smooth(progress):
            m.MedicalStage==MedicalTreatmentStage.Approach?Smooth((progress-.82f)/.18f):0;
        var left=parts[1];var right=parts[4];
        for(int i=0;i<parts.Count;i++)
        {
            var part=parts[i];parts[i]=part with{
                A=part.A+forward*(.12f*kneel),B=part.B+forward*(.12f*kneel),
                ZA=part.ZA-.29f*kneel,ZB=part.ZB-.29f*kneel,
                Color=i is 0 or 1 or 3 or 4?"#203b49":i is 6 or 7 or 10?"#d8ebe6":part.Color};
        }
        if(kneel>0)
        {
            KeeperLeg(parts,0,left.B,left.ZB,forward,0,forward,0);
            KeeperLeg(parts,1,right.B,right.ZB,forward,0,forward,0);
        }
        for(int arm=0;arm<2;arm++)
        {
            int i=7+arm*3;float armSign=arm==0?-1:1;
            var ordinary=parts[i+1];
            var handle=MedicalStretcherCentre(m)+axis*(sign*1.05f)+side*(armSign*.29f);
            var target=ordinary.B*(1-carrying)+handle*carrying;
            float z=ordinary.ZB*(1-carrying)+.90f*carrying;
            if(kneel>0)
            {
                var assess=medic.Position+forward*.32f+side*(armSign*.18f);
                target=target*(1-kneel)+assess*kneel;z=z*(1-kneel)+.67f*kneel;
            }
            KeeperArm(parts,i,target,z,side*armSign,-.3f,forward,0);
        }
        KeepFallAboveGround(parts);
        return parts;
    }
}