using System;
using System.Collections.Generic;

namespace Socer;

public static partial class ReplayFigure
{
    static bool ApplyKeeperSave(Footballer p,MatchSimulation m,List<Part> parts,Point forward,Point side)
    {
        bool dive=p.DiveRemaining>0 || (m.Seconds>=p.DiveEndedAt &&
            m.Seconds-p.DiveEndedAt<HumanFigure.DiveRecoverySeconds(p,m));
        if(dive)
        {
            ApplyKeeperDive(p,m,parts,forward,side);
            return true;
        }
        float block=m.KeeperBlockPoseWeight(p);
        if(block<=0 && !(p.KeeperBlockUntil>0 && m.Seconds>=p.KeeperBlockStartedAt &&
            m.Seconds<p.KeeperBlockUntil+.22f))return false;

        // A low spread is a squat: hips lower with the shirt, and planted boots
        // make both knees flex instead of shortening or disconnecting the torso.
        var left=parts[1].B;float leftZ=parts[1].ZB;
        var right=parts[4].B;float rightZ=parts[4].ZB;
        bool low=p.KeeperBlockKind==2;
        var shift=forward*((low?.10f:.07f)*block);
        float lower=(low?.48f:.20f)*block;
        for(int i=0;i<parts.Count;i++)
        {
            var v=parts[i];parts[i]=v with{A=v.A+shift,B=v.B+shift,ZA=v.ZA-lower,ZB=v.ZB-lower};
        }
        float spread=low?.46f:.32f;
        for(int leg=0;leg<2;leg++)
        {
            int sign=leg==0?-1:1;
            var resting=leg==0?left:right;
            float restZ=leg==0?leftZ:rightZ;
            var wanted=p.Position+side*(sign*spread)+forward*(low?.16f:.09f);
            KeeperLeg(parts,leg,resting*(1-block)+wanted*block,restZ*(1-block)+.07f*block,
                forward,0,forward,0);
        }
        for(int s=0;s<2;s++)
        {
            int sign=s==0?-1:1;
            KeeperArm(parts,7+s*3,m.GlovePosition(p,sign),m.GloveHeight(p),
                new Point(),-1,forward*(1-block),block);
        }
        KeepKeeperAboveGround(parts);
        return true;
    }

    static void ApplyKeeperDive(Footballer p,MatchSimulation m,List<Part> parts,Point forward,Point side)
    {
        var reachVector=p.DiveTarget-p.DiveOrigin;
        // A central save (or a forward interception) stays square to the shot.
        // Only genuinely lateral reach rolls the body over a supporting side.
        bool vertical=reachVector.Length<.18f || Math.Abs(Point.Dot(reachVector.Unit,forward))>.92f;
        var axis=vertical?forward:reachVector.Unit;
        float blend=HumanFigure.DiveBlend(p,m);
        float recovery=p.DiveRemaining>0?0:Math.Clamp((m.Seconds-p.DiveEndedAt)/HumanFigure.DiveRecoverySeconds(p,m),0,1);
        // Roll off the landing side first, then drive upwards from a tucked
        // leg. Reversing the take-off rotation and root together made the
        // goalkeeper levitate from the turf with both legs hanging straight.
        float rollBlend=!vertical && recovery>0?1-Smooth(recovery/.60f):blend;
        float riseBlend=!vertical && recovery>0?1-Smooth((recovery-.22f)/.78f):blend;
        float high=Smooth((p.DiveHeight-.75f)/1.75f);
        float angle=vertical?0:rollBlend*(1.38f-high*.42f);
        float cos=MathF.Cos(angle),sin=MathF.Sin(angle);
        var pelvis=parts[6].A;float pelvisZ=parts[6].ZA;
        float flight=p.DiveRemaining>0?Smooth(p.DiveAge/.22f)*
            (1-Smooth((p.DiveAge-.52f)/.43f)):0;
        float load=Smooth(p.DiveAge/.06f)*(1-Smooth((p.DiveAge-.09f)/.11f));
        float landing=Smooth((p.DiveAge-.64f)/.20f);
        float rootZ=pelvisZ-(vertical?.12f*(load+landing*blend)*(1-flight):(.60f-high*.10f)*riseBlend);
        // A high save jumps before rolling sideways. Root lift carries every
        // joint; the forearms never stretch upwards to the target ball height.
        float lift=Math.Clamp(p.DiveHeight-(rootZ+.50f*cos)-.22f,0,1.5f)*flight;
        (Point,float) Joint(Point at,float z)
        {
            var local=at-pelvis;
            float along=Point.Dot(local,axis),height=z-pelvisZ;
            return (at+axis*(along*(cos-1)+height*sin),
                rootZ+height*cos-along*sin+lift);
        }
        for(int i=0;i<parts.Count;i++)
        {
            var v=parts[i];var a=Joint(v.A,v.ZA);var b=Joint(v.B,v.ZB);
            parts[i]=v with{A=a.Item1,ZA=a.Item2,B=b.Item1,ZB=b.Item2};
        }
        KeepKeeperAboveGround(parts);
        if(!vertical && recovery>0)
        {
            float kneel=.22f*MathF.Sin(recovery*MathF.PI);
            for(int i=0;i<parts.Count;i++)
            {var v=parts[i];parts[i]=v with{ZA=v.ZA-kneel,ZB=v.ZB-kneel};}
        }

        float forwardAlong=Point.Dot(forward,axis);
        var bendForward=forward+axis*(forwardAlong*(cos-1));
        float bendForwardZ=-forwardAlong*sin;
        float trail=Smooth(p.DiveAge/.26f)*(1-Smooth((p.DiveAge-.60f)/.35f))*blend;
        for(int leg=0;leg<2;leg++)
        {
            var ankle=parts[leg*3+1];
            var wanted=ankle.B-axis*((vertical?(leg==0?.09f:.12f):(leg==0?.15f:.24f))*trail);
            float wantedZ=ankle.ZB+(vertical?.10f:(leg==0?.12f:.20f))*trail;
            int sign=leg==0?-1:1;
            bool leading=sign==(Point.Dot(axis,side)>=0?1:-1);
            if(!vertical && recovery>0)
            {
                // The near boot folds under the body before the trailing foot
                // joins it. Only its reset is airborne; the rising half keeps
                // both soles fixed instead of playing a running stride.
                float reset=Smooth(recovery/(leading?.28f:.48f));
                float crouch=Smooth(recovery/.16f)*(1-Smooth((recovery-.52f)/.38f));
                var plant=p.Position+side*(sign*.19f)+forward*((leading?.18f:-.15f)*crouch);
                wanted=wanted*(1-reset)+plant*reset;
                float bootLift=leading?.07f:.11f;
                float swing=MathF.Sin(Math.Clamp(recovery/(leading?.28f:.48f),0,1)*MathF.PI);
                wantedZ=wantedZ*(1-reset)+(.065f+bootLift*swing)*reset;
            }
            else if(p.DiveRemaining>0)
            {
                // Establish the pushing foot while the inside leg releases.
                // The anchor uses the captured take-off point, never a new
                // random pose or an offset that travels with the sliding root.
                float push=.65f*Smooth(p.DiveAge/.04f)*(1-Smooth((p.DiveAge-.07f)/.07f))*(vertical?1:leading?0:1);
                var plant=p.DiveOrigin+side*(sign*.19f)+forward*.025f;
                wanted=wanted*(1-push)+plant*push;wantedZ=wantedZ*(1-push)+.065f*push;
            }
            KeeperLeg(parts,leg,wanted,wantedZ,bendForward,bendForwardZ,bendForward,bendForwardZ);
        }
        // Reach through the ball, then tuck for landing. The curve is continuous
        // when DiveRemaining becomes zero, so recovery cannot reset the arms.
        float reach=Smooth((p.DiveAge-.025f)/.17f)*
            (1-Smooth((p.DiveAge-.52f)/.30f))*blend;
        for(int s=0;s<2;s++)
        {
            int arm=7+s*3,sign=s==0?-1:1;
            var shoulder=parts[arm];
            var tucked=shoulder.A+bendForward*.19f+side*(sign*.10f);
            float tuckedZ=shoulder.ZA-.29f*cos+bendForwardZ*.19f;
            var ready=p.DiveRemaining<=0 && m.Owner==p.Id?m.GlovePosition(p,sign):
                p.Position+forward*.24f+side*(sign*.40f);
            float readyZ=p.DiveRemaining<=0 && m.Owner==p.Id?m.GloveHeight(p):1.00f;
            tucked=ready*(1-blend)+tucked*blend;
            tuckedZ=readyZ*(1-blend)+tuckedZ*blend;
            var target=m.GlovePosition(p,sign);
            var hand=tucked*(1-reach)+target*reach;
            float z=tuckedZ*(1-reach)+m.GloveHeight(p)*reach;
            // Anatomical down follows the rolled torso. A front or side pole
            // can align with the retracting wrist and invert its elbow plane;
            // torso-down stays distinct from the forward-reaching glove.
            KeeperArm(parts,arm,hand,z,axis*-sin,-cos,forward*(1-blend),blend);
        }
        if(!vertical && recovery>0 && m.Owner!=p.Id)
        {
            // A free hand braces the loaded knee while the other arm balances.
            // A keeper holding the ball keeps both gloves around it instead.
            int lead=Point.Dot(axis,side)>=0?1:0;
            int arm=7+lead*3;
            var hand=parts[arm+1];var knee=parts[lead*3];
            float support=.85f*Smooth(recovery/.34f)*(1-Smooth((recovery-.56f)/.35f));
            var shoulder=parts[arm];
            SolveKeeperLimb(shoulder.A,shoulder.ZA,knee.B+forward*.035f,knee.ZB+.035f,
                .27f,.25f,side*(lead==0?-1:1),0,
                out var elbow,out float elbowZ,out var palm,out float palmZ);
            GoalArmDirections(parts,arm,(elbow-shoulder.A)*(1/.27f),(elbowZ-shoulder.ZA)/.27f,
                (palm-elbow)*(1/.25f),(palmZ-elbowZ)/.25f,forward,support);
        }
        KeepKeeperAboveGround(parts);
    }

    static void KeeperArm(List<Part> parts,int arm,Point target,float targetZ,
        Point bend,float bendZ,Point handDirection,float handDirectionZ)
    {
        var upper=parts[arm];var lower=parts[arm+1];var glove=parts[arm+2];
        SolveKeeperLimb(upper.A,upper.ZA,target,targetZ,.27f,.25f,bend,bendZ,
            out var elbow,out float elbowZ,out var hand,out float handZ);
        parts[arm]=upper with{B=elbow,ZB=elbowZ};
        parts[arm+1]=lower with{A=elbow,ZA=elbowZ,B=hand,ZB=handZ};
        float directionLength=MathF.Sqrt(handDirection.Length*handDirection.Length+handDirectionZ*handDirectionZ);
        if(directionLength>.0001f){handDirection*=1/directionLength;handDirectionZ/=directionLength;}
        parts[arm+2]=glove with{A=hand,ZA=handZ,B=hand+handDirection*.03f,ZB=handZ+handDirectionZ*.03f};
    }

    static void KeeperLeg(List<Part> parts,int leg,Point target,float targetZ,
        Point bend,float bendZ,Point bootDirection,float bootDirectionZ)
    {
        int i=leg*3;var thigh=parts[i];var shin=parts[i+1];var boot=parts[i+2];
        SolveKeeperLimb(thigh.A,thigh.ZA,target,targetZ,.405f,.405f,bend,bendZ,
            out var knee,out float kneeZ,out var ankle,out float ankleZ);
        parts[i]=thigh with{B=knee,ZB=kneeZ};
        parts[i+1]=shin with{A=knee,ZA=kneeZ,B=ankle,ZB=ankleZ};
        parts[i+2]=boot with{A=ankle,ZA=ankleZ,B=ankle+bootDirection*.24f,ZB=ankleZ+bootDirectionZ*.24f};
    }

    static void SolveKeeperLimb(Point root,float rootZ,Point target,float targetZ,
        float upper,float lower,Point bend,float bendZ,
        out Point joint,out float jointZ,out Point end,out float endZ)
    {
        var delta=target-root;float dz=targetZ-rootZ;
        float distance=MathF.Sqrt(delta.Length*delta.Length+dz*dz);
        if(distance<.0001f){delta=new Point(.0001f,0);dz=0;distance=.0001f;}
        float reach=Math.Clamp(distance,Math.Abs(upper-lower)+.0001f,upper+lower-.0001f);
        if(upper<.30f)
        {
            // A hard fully-straight arm limit makes the elbow's bend collapse
            // with a square-root cusp. Ease the last nine centimetres of reach
            // so a high save can extend smoothly without locking the elbow.
            float start=upper+lower-.09f;
            if(reach>start)reach=start+.075f*(1-MathF.Exp(-(distance-start)/.075f));
        }
        var direction=delta*(1/distance);float directionZ=dz/distance;
        end=root+direction*reach;endZ=rootZ+directionZ*reach;
        float along=(upper*upper-lower*lower+reach*reach)/(2*reach);
        float height=MathF.Sqrt(Math.Max(0,upper*upper-along*along));
        float dot=Point.Dot(bend,direction)+bendZ*directionZ;
        var pole=bend-direction*dot;float poleZ=bendZ-directionZ*dot;
        float length=MathF.Sqrt(pole.Length*pole.Length+poleZ*poleZ);
        if(length<.001f)
        {
            var fallback=Math.Abs(directionZ)>.8f?new Point(1,0):new Point(-direction.Y,direction.X);
            float fallbackDot=Point.Dot(fallback,direction);
            pole=fallback-direction*fallbackDot;poleZ=-directionZ*fallbackDot;
            length=MathF.Sqrt(pole.Length*pole.Length+poleZ*poleZ);
        }
        joint=root+direction*along+pole*(height/Math.Max(.0001f,length));
        jointZ=rootZ+directionZ*along+poleZ*height/Math.Max(.0001f,length);
    }

    static void KeepKeeperAboveGround(List<Part> parts)
    {
        float minimum=.065f;
        for(int i=0;i<parts.Count;i++)minimum=Math.Min(minimum,Math.Min(parts[i].ZA,parts[i].ZB));
        float raise=.065f-minimum;
        if(raise<=0)return;
        for(int i=0;i<parts.Count;i++)
        {var v=parts[i];parts[i]=v with{ZA=v.ZA+raise,ZB=v.ZB+raise};}
    }
}