using System;
using System.Collections.Generic;
using NVector=System.Numerics.Vector3;
using NQuaternion=System.Numerics.Quaternion;
using NMatrix=System.Numerics.Matrix4x4;

namespace Socer;

public static partial class ReplayFigure
{
    // Artist controls in DEGREES. These are conservative animation envelopes,
    // not claims about universal clinical range of motion. Thorax, humeral twist,
    // elbow hinge, forearm pronation and wrist motion are distinct controls.
    public readonly record struct ArmAngles(float Flexion,float Abduction,float HumeralTwist,
        float ElbowFlexion,float Pronation=0,float WristFlexion=0,float WristDeviation=0);
    public readonly record struct ArmMeasurements(float ShoulderFlexion,float ShoulderAbduction,
        float HumeralTwist,float ElbowFlexion,float WristBend,float UpperLength,float LowerLength,
        float TorsoClearance,float ShoulderRootExcursion,float HingeForward,bool Finite);
    readonly record struct ArmFrame(NVector Chest,NVector Up,NVector Side,NVector Forward);
    readonly record struct AnatomicalArm(NVector Shoulder,NVector Upper,NVector Bend,float Elbow,
        NVector Fingers,NVector Palm);
    const float ArmDegrees=MathF.PI/180;
    const float UpperArmLength=.27f,ForearmLength=.25f;

    static NVector AnatomicalPoint(Point p,float z)=>new(p.X,-p.Y,z);
    static Point AnatomicalGround(NVector p)=>new(p.X,-p.Y);
    static NVector AnatomicalA(Part p)=>AnatomicalPoint(p.A,p.ZA);
    static NVector AnatomicalB(Part p)=>AnatomicalPoint(p.B,p.ZB);
    static NVector UnitArm(NVector p,NVector fallback)
        =>p.LengthSquared()>.0000001f?NVector.Normalize(p):fallback;
    static NVector RotateArm(NVector v,NVector axis,float radians)
        =>NVector.Transform(v,NQuaternion.CreateFromAxisAngle(UnitArm(axis,NVector.UnitZ),radians));
    static float ArmAngle(NVector a,NVector b)
        =>MathF.Acos(Math.Clamp(NVector.Dot(UnitArm(a,NVector.UnitX),UnitArm(b,NVector.UnitX)),-1,1))/ArmDegrees;
    static float SignedArmAngle(NVector from,NVector to,NVector axis)
        =>MathF.Atan2(NVector.Dot(NVector.Cross(from,to),axis),Math.Clamp(NVector.Dot(from,to),-1,1))/ArmDegrees;
    static NVector ArmPlane(NVector v,NVector normal)=>v-normal*NVector.Dot(v,normal);
    static ArmFrame AnatomicalFrame(IReadOnlyList<Part> parts)
    {
        var chest=AnatomicalB(parts[6]);
        var up=UnitArm(chest-AnatomicalA(parts[6]),NVector.UnitZ);
        var side=UnitArm(ArmPlane(AnatomicalA(parts[10])-AnatomicalA(parts[7]),up),-NVector.UnitY);
        var forward=UnitArm(NVector.Cross(up,side),NVector.UnitX);
        side=UnitArm(NVector.Cross(forward,up),side);
        return new(chest,up,side,forward);
    }
    static ArmAngles BoundArmAngles(ArmAngles p)=>new(
        Math.Clamp(p.Flexion,-48,175),Math.Clamp(p.Abduction,-12,155),Math.Clamp(p.HumeralTwist,-85,85),
        Math.Clamp(p.ElbowFlexion,6,145),Math.Clamp(p.Pronation,-85,85),
        Math.Clamp(p.WristFlexion,-30,38),Math.Clamp(p.WristDeviation,-18,18));
    public static ArmAngles BlendArmAngles(ArmAngles a,ArmAngles b,float weight)
    {
        weight=Math.Clamp(weight,0,1);
        float Blend(float x,float y)=>x+(y-x)*weight;
        return new(Blend(a.Flexion,b.Flexion),Blend(a.Abduction,b.Abduction),
            Blend(a.HumeralTwist,b.HumeralTwist),Blend(a.ElbowFlexion,b.ElbowFlexion),
            Blend(a.Pronation,b.Pronation),Blend(a.WristFlexion,b.WristFlexion),Blend(a.WristDeviation,b.WristDeviation));
    }
    static AnatomicalArm ForwardArm(ArmFrame frame,int sign,ArmAngles angles)
    {
        var p=BoundArmAngles(angles);
        float flex=p.Flexion*ArmDegrees,abduct=p.Abduction*ArmDegrees;
        var outward=frame.Side*sign;
        var upper=frame.Forward*(MathF.Sin(flex)*MathF.Cos(abduct))+outward*MathF.Sin(abduct)-
            frame.Up*(MathF.Cos(flex)*MathF.Cos(abduct));
        var bend=frame.Forward*MathF.Cos(flex)+frame.Up*MathF.Sin(flex);
        upper=UnitArm(upper,-frame.Up);
        bend=UnitArm(RotateArm(bend,upper,p.HumeralTwist*sign*ArmDegrees),frame.Forward);
        float elbow=p.ElbowFlexion*ArmDegrees;
        var forearm=upper*MathF.Cos(elbow)+bend*MathF.Sin(elbow);
        var hinge=UnitArm(NVector.Cross(upper,bend),frame.Side);
        // Neutral palms face the thighs. Pronation rotates about the forearm;
        // wrist flexion/deviation are applied afterwards in that hand frame.
        var palm=RotateArm(hinge*(-sign),forearm,-p.Pronation*sign*ArmDegrees);
        var fingers=forearm;
        var wristAxis=UnitArm(NVector.Cross(forearm,palm),hinge);
        fingers=RotateArm(fingers,wristAxis,p.WristFlexion*ArmDegrees);
        palm=RotateArm(palm,wristAxis,p.WristFlexion*ArmDegrees);
        fingers=RotateArm(fingers,palm,p.WristDeviation*sign*ArmDegrees);
        float elevation=ArmAngle(upper,-frame.Up);
        float girdle=Smooth((elevation-35)/110);
        // Virtual clavicular/scapular support is deliberately small for this
        // rigid stylised shirt: it lifts the shoulder as the humerus rises.
        var shoulder=frame.Chest+outward*(.215f+.010f*girdle)-frame.Up*(.06f-.020f*girdle);
        return new(shoulder,upper,bend,elbow,UnitArm(fingers,forearm),UnitArm(palm,-outward));
    }
    static NQuaternion ArmRotation(NVector upper,NVector bend)
    {
        var z=-UnitArm(upper,-NVector.UnitZ);
        var x=UnitArm(ArmPlane(bend,z),NVector.UnitX);
        var y=UnitArm(NVector.Cross(z,x),NVector.UnitY);
        x=UnitArm(NVector.Cross(y,z),x);
        return NQuaternion.Normalize(NQuaternion.CreateFromRotationMatrix(new NMatrix(
            x.X,x.Y,x.Z,0,y.X,y.Y,y.Z,0,z.X,z.Y,z.Z,0,0,0,0,1)));
    }
    static NQuaternion ArmHandRotation(NVector fingers,NVector palm)
    {
        var x=UnitArm(fingers,-NVector.UnitZ);
        var z=ArmPlane(palm,x);
        if(z.LengthSquared()<.00001f)z=ArmPlane(Math.Abs(x.Z)<.8f?NVector.UnitZ:NVector.UnitX,x);
        z=UnitArm(z,NVector.UnitY);
        var y=UnitArm(NVector.Cross(z,x),NVector.UnitY);z=UnitArm(NVector.Cross(x,y),z);
        return NQuaternion.Normalize(NQuaternion.CreateFromRotationMatrix(new NMatrix(
            x.X,x.Y,x.Z,0,y.X,y.Y,y.Z,0,z.X,z.Y,z.Z,0,0,0,0,1)));
    }
    static NVector PalmReference(ArmFrame frame,NVector fingers)
    {
        var normal=ArmPlane(frame.Forward,fingers);
        if(normal.LengthSquared()<.00001f)normal=ArmPlane(frame.Up,fingers);
        if(normal.LengthSquared()<.00001f)normal=ArmPlane(frame.Side,fingers);
        return UnitArm(normal,frame.Up);
    }
    public static (Point direction,float z) AnatomicalPalmNormal(IReadOnlyList<Part> parts,int arm)
    {
        var frame=AnatomicalFrame(parts);var hand=parts[arm+2];
        var fingers=UnitArm(AnatomicalB(hand)-AnatomicalA(hand),-frame.Up);
        var normal=RotateArm(PalmReference(frame,fingers),fingers,hand.PalmRoll*ArmDegrees);
        return (AnatomicalGround(normal),normal.Z);
    }
    static void WriteAnatomicalArm(List<Part> parts,int arm,ArmFrame frame,AnatomicalArm target,float weight)
    {
        if(weight<=0)return;weight=Math.Clamp(weight,0,1);
        var upperPart=parts[arm];var lowerPart=parts[arm+1];var handPart=parts[arm+2];
        var shoulder=target.Shoulder;var upper=target.Upper;var bend=target.Bend;float elbow=target.Elbow;
        var fingers=target.Fingers;var palm=target.Palm;
        if(weight<.99999f)
        {
            var oldUpper=UnitArm(AnatomicalB(upperPart)-AnatomicalA(upperPart),-frame.Up);
            var oldLower=UnitArm(AnatomicalB(lowerPart)-AnatomicalA(lowerPart),oldUpper);
            var oldBend=UnitArm(ArmPlane(oldLower,oldUpper),ArmPlane(frame.Forward,oldUpper));
            var oldPalm=AnatomicalPalmNormal(parts,arm);
            var oldFingers=UnitArm(AnatomicalB(handPart)-AnatomicalA(handPart),oldLower);
            var oldHand=ArmHandRotation(oldFingers,AnatomicalPoint(oldPalm.direction,oldPalm.z));
            var oldForearm=ArmHandRotation(oldLower,NVector.Cross(oldUpper,oldBend));
            var wantedLower=upper*MathF.Cos(elbow)+bend*MathF.Sin(elbow);
            var wantedForearm=ArmHandRotation(wantedLower,NVector.Cross(upper,bend));
            var wantedHand=ArmHandRotation(fingers,palm);
            var oldWrist=NQuaternion.Concatenate(oldHand,NQuaternion.Inverse(oldForearm));
            var wantedWrist=NQuaternion.Concatenate(wantedHand,NQuaternion.Inverse(wantedForearm));
            var wristRotation=NQuaternion.Slerp(oldWrist,wantedWrist,weight);
            var rotation=NQuaternion.Slerp(ArmRotation(oldUpper,oldBend),ArmRotation(upper,bend),weight);
            upper=NVector.Transform(-NVector.UnitZ,rotation);bend=NVector.Transform(NVector.UnitX,rotation);
            elbow=(Math.Clamp(ArmAngle(oldUpper,oldLower),6,145)*ArmDegrees)*(1-weight)+elbow*weight;
            shoulder=NVector.Lerp(AnatomicalA(upperPart),shoulder,weight);
            var blendedLower=upper*MathF.Cos(elbow)+bend*MathF.Sin(elbow);
            var forearmRotation=ArmHandRotation(blendedLower,NVector.Cross(upper,bend));
            var handRotation=NQuaternion.Concatenate(wristRotation,forearmRotation);
            fingers=NVector.Transform(NVector.UnitX,handRotation);palm=NVector.Transform(NVector.UnitZ,handRotation);
        }
        var elbowAt=shoulder+upper*UpperArmLength;
        var lower=UnitArm(upper*MathF.Cos(elbow)+bend*MathF.Sin(elbow),upper);
        var wrist=elbowAt+lower*ForearmLength;
        palm=UnitArm(ArmPlane(palm,fingers),PalmReference(frame,fingers));
        float roll=SignedArmAngle(PalmReference(frame,fingers),palm,fingers);
        parts[arm]=upperPart with{A=AnatomicalGround(shoulder),ZA=shoulder.Z,B=AnatomicalGround(elbowAt),ZB=elbowAt.Z};
        parts[arm+1]=lowerPart with{A=AnatomicalGround(elbowAt),ZA=elbowAt.Z,B=AnatomicalGround(wrist),ZB=wrist.Z};
        var tips=wrist+fingers*.075f;
        parts[arm+2]=handPart with{A=AnatomicalGround(wrist),ZA=wrist.Z,B=AnatomicalGround(tips),ZB=tips.Z,PalmRoll=roll,ArticulatedHand=true};
    }
    public static void ApplyArmFk(List<Part> parts,int arm,ArmAngles pose,float weight=1)
    {
        if(parts.Count<13 || arm is not (7 or 10))return;
        var frame=AnatomicalFrame(parts);
        WriteAnatomicalArm(parts,arm,frame,ForwardArm(frame,arm==7?-1:1,pose),weight);
    }
    // Pole is a DIRECTION in pose space, as in the existing keeper/leg helpers.
    // Wrist is an absolute world contact position. This API is for hand contact.
    public static void ApplyArmContact(List<Part> parts,int arm,Point wrist,float wristZ,Point elbowPole,float poleZ,float weight=1)
    {
        if(parts.Count<13 || arm is not (7 or 10) || weight<=0)return;
        var frame=AnatomicalFrame(parts);var shoulder=AnatomicalA(parts[arm]);
        weight=Math.Clamp(weight,0,1);
        // Contact blending belongs in end-effector space. Interpolating two
        // complete arm orientations can reverse its half-turn branch while a
        // player rolls onto a supporting hand, throwing that hand off the turf.
        var requested=NVector.Lerp(AnatomicalB(parts[arm+1]),AnatomicalPoint(wrist,wristZ),weight);
        var delta=requested-shoulder;
        float reach=Math.Clamp(delta.Length(),.158f,.5192f);
        var axis=UnitArm(delta,-frame.Up);
        var oldPole=ArmPlane(AnatomicalB(parts[arm])-shoulder,axis);
        var wantedPole=ArmPlane(AnatomicalPoint(elbowPole,poleZ),axis);
        var pole=ArmPlane(BlendFallBoneDirection(oldPole,wantedPole,weight,1,frame.Forward),axis);
        if(pole.LengthSquared()<.00001f)pole=oldPole;
        if(pole.LengthSquared()<.00001f)pole=ArmPlane(frame.Forward+frame.Side*(arm==7?-.35f:.35f),axis);
        pole=UnitArm(pole,frame.Forward);
        float along=(UpperArmLength*UpperArmLength-ForearmLength*ForearmLength+reach*reach)/(2*reach);
        float bendDistance=MathF.Sqrt(Math.Max(0,UpperArmLength*UpperArmLength-along*along));
        var elbowAt=shoulder+axis*along+pole*bendDistance;
        var at=shoulder+axis*reach;
        var upper=UnitArm(elbowAt-shoulder,-frame.Up);var lower=UnitArm(at-elbowAt,-frame.Up);
        var bend=UnitArm(ArmPlane(lower,upper),pole);
        var oldPalm=AnatomicalPalmNormal(parts,arm);
        var palm=UnitArm(ArmPlane(AnatomicalPoint(oldPalm.direction,oldPalm.z),lower),PalmReference(frame,lower));
        WriteAnatomicalArm(parts,arm,frame,new(shoulder,upper,bend,ArmAngle(upper,lower)*ArmDegrees,lower,palm),1);
    }
    // Compatibility for the existing keeper recovery support. This preserves
    // its spherical direction blend instead of changing the tested glove pose.
    static void GoalArmDirections(List<Part> parts,int arm,Point upper,float upperZ,Point lower,float lowerZ,
        Point handDirection,float weight)
    {
        var a=parts[arm];var b=parts[arm+1];var hand=parts[arm+2];
        var u=BlendFallBoneDirection(new NVector(a.B.X-a.A.X,a.B.Y-a.A.Y,a.ZB-a.ZA),
            new NVector(upper.X,upper.Y,upperZ),weight,.27f,new NVector(handDirection.X,handDirection.Y,0));
        var elbow=a.A+new Point(u.X,u.Y);float elbowZ=a.ZA+u.Z;
        var l=BlendFallBoneDirection(new NVector(b.B.X-b.A.X,b.B.Y-b.A.Y,b.ZB-b.ZA),
            new NVector(lower.X,lower.Y,lowerZ),weight,.25f,new NVector(handDirection.X,handDirection.Y,0));
        var at=elbow+new Point(l.X,l.Y);float z=elbowZ+l.Z;
        parts[arm]=a with{B=elbow,ZB=elbowZ};
        parts[arm+1]=b with{A=elbow,ZA=elbowZ,B=at,ZB=z};
        parts[arm+2]=hand with{A=at,ZA=z,B=at+handDirection*.03f,ZB=z};
    }

    static float ArmSegmentDistance(NVector p,NVector q,NVector a,NVector b)
    {
        var d1=q-p;var d2=b-a;var r=p-a;
        float aa=NVector.Dot(d1,d1),ee=NVector.Dot(d2,d2),ff=NVector.Dot(d2,r),s,t;
        if(aa<.000001f){s=0;t=Math.Clamp(ff/Math.Max(ee,.000001f),0,1);}
        else
        {
            float cc=NVector.Dot(d1,r),bb=NVector.Dot(d1,d2),denom=aa*ee-bb*bb;
            s=denom>.000001f?Math.Clamp((bb*ff-cc*ee)/denom,0,1):0;
            t=(bb*s+ff)/Math.Max(ee,.000001f);
            if(t<0){t=0;s=Math.Clamp(-cc/aa,0,1);}
            else if(t>1){t=1;s=Math.Clamp((bb-cc)/aa,0,1);}
        }
        return NVector.Distance(p+d1*s,a+d2*t);
    }
    public static ArmMeasurements MeasureArm(IReadOnlyList<Part> parts,int arm)
    {
        var frame=AnatomicalFrame(parts);int sign=arm==7?-1:1;
        var shoulder=AnatomicalA(parts[arm]);var elbow=AnatomicalB(parts[arm]);var wrist=AnatomicalB(parts[arm+1]);
        var upper=elbow-shoulder;var lower=wrist-elbow;
        var u=UnitArm(upper,-frame.Up);var l=UnitArm(lower,u);
        float flex=MathF.Atan2(NVector.Dot(u,frame.Forward),-NVector.Dot(u,frame.Up))/ArmDegrees;
        float abduct=MathF.Asin(Math.Clamp(NVector.Dot(u,frame.Side*sign),-1,1))/ArmDegrees;
        var reference=frame.Forward*MathF.Cos(flex*ArmDegrees)+frame.Up*MathF.Sin(flex*ArmDegrees);
        var bend=UnitArm(ArmPlane(l,u),reference);
        float twist=SignedArmAngle(reference,bend,u)*sign;
        var fingers=AnatomicalB(parts[arm+2])-AnatomicalA(parts[arm+2]);
        var torsoA=AnatomicalA(parts[6])+frame.Up*.08f;var torsoB=frame.Chest-frame.Up*.08f;
        // Forearm vs a conservative clothed torso capsule. Proximal upper arm
        // intersects the shoulder attachment by design, so it is not tested.
        float clearance=ArmSegmentDistance(elbow,wrist,torsoA,torsoB)-.165f-parts[arm+1].Width*.5f;
        var root=frame.Chest+frame.Side*(sign*.215f)-frame.Up*.06f;
        bool finite=float.IsFinite(shoulder.X+shoulder.Y+shoulder.Z+elbow.X+elbow.Y+elbow.Z+wrist.X+wrist.Y+wrist.Z+fingers.X+fingers.Y+fingers.Z);
        return new(flex,abduct,twist,ArmAngle(u,l),ArmAngle(l,fingers),upper.Length(),lower.Length(),
            clearance,NVector.Distance(root,shoulder),NVector.Dot(bend,frame.Forward),finite);
    }
    public static void RepairArmAnatomy(List<Part> parts)
    {
        if(parts.Count<13)return;
        for(int arm=7;arm<=10;arm+=3)
        {
            var measure=MeasureArm(parts,arm);
            if(!measure.Finite){ApplyArmFk(parts,arm,new(8,12,0,35));continue;}
            bool invalidChain=Math.Abs(measure.UpperLength-UpperArmLength)>.003f ||
                Math.Abs(measure.LowerLength-ForearmLength)>.003f || measure.ElbowFlexion<3 || measure.ElbowFlexion>151;
            if(invalidChain)
            {
                var hand=parts[arm+1];var elbow=parts[arm];
                ApplyArmContact(parts,arm,hand.B,hand.ZB,elbow.B-elbow.A,elbow.ZB-elbow.ZA);
            }
            // Valid authored FK and genuine contact poses are not resampled.
            // Legacy wrist overrides still need their fingers to follow forearm.
            if(measure.WristBend>49)
            {
                var frame=AnatomicalFrame(parts);var fore=parts[arm+1];var hand=parts[arm+2];
                var direction=UnitArm(AnatomicalB(fore)-AnatomicalA(fore),-frame.Up);
                var at=AnatomicalA(hand);var tip=at+direction*.075f;
                var normal=UnitArm(ArmPlane(frame.Side*(arm==7?1:-1),direction),PalmReference(frame,direction));
                parts[arm+2]=hand with{B=AnatomicalGround(tip),ZB=tip.Z,PalmRoll=SignedArmAngle(PalmReference(frame,direction),normal,direction),ArticulatedHand=true};
            }
        }
    }
}