using Sandbox;
using System;
namespace PaintballBaddies;

/// <summary>Native motion rotations on the sausage's own rest lengths.</summary>
internal sealed class GrumpySausagePose
{
    private sealed class Joint
    {
        public string Name,Parent;
        public Transform Rest,Local,SourceRest,SourceLocal;
    }
    private readonly List<Joint> joints=new();
    private readonly Dictionary<string,Transform> sourcePose=new();
    internal readonly Dictionary<string,Transform> Poses=new();
    private readonly Dictionary<string,List<string>> subtree=new();
    private readonly Dictionary<string,Rotation> palmFrames=new();
    internal GrumpySausagePose(Model target,Model source)
    {
        var pending=target.Bones.AllBones.ToList();
        while(pending.Count>0)
        {
            var bone=pending.FirstOrDefault(b=>b.Parent is null || joints.Any(j=>j.Name==b.Parent.Name));
            if(bone is null)break;
            var rest=target.GetBoneTransform(bone.Name);var sourceRest=source.GetBoneTransform(bone.Name);
            string parent=bone.Parent?.Name;
            joints.Add(new Joint{Name=bone.Name,Parent=parent,Rest=rest,SourceRest=sourceRest,
                Local=parent is null ? rest : target.GetBoneTransform(parent).ToLocal(rest),
                SourceLocal=parent is null ? sourceRest : source.GetBoneTransform(parent).ToLocal(sourceRest)});
            pending.Remove(bone);
        }
        // Every bone's subtree (itself plus all descendants), for arm overrides.
        foreach(var j in joints)
        {
            var list=new List<string>{j.Name};
            for(int i=0;i<list.Count;i++)
                foreach(var c in joints)if(c.Parent==list[i])list.Add(c.Name);
            subtree[j.Name]=list;
        }
        // This fitted rig has wrist bones but no finger bones. Derive each
        // wrist's palm axes from the native donor's bind pose once, instead of
        // looking for fingers that do not exist in the rendered sausage.
        foreach(string side in new[]{"L","R"})
        {
            var wrist=source.GetBoneTransform("hand_"+side);
            var middle=source.GetBoneTransform("finger_middle_0_"+side).Position;
            var across=source.GetBoneTransform("finger_index_0_"+side).Position-source.GetBoneTransform("finger_ring_0_"+side).Position;
            var along=(middle-wrist.Position).Normal;
            var palm=Vector3.Cross(along,across).Normal*(side=="L" ? 1 : -1);
            palmFrames[side]=Rotation.LookAt(wrist.Rotation.Inverse*along,wrist.Rotation.Inverse*palm);
        }
    }
    /// <summary>Carrying pose for one arm, like a waiter: upper arm hanging
    /// close to the side, elbow bent about 90 degrees, forearm pointing forward
    /// with the palm under the load. Model space: +x forward, +y left, +z up.</summary>
    internal void Carry(string side,float weight)
    {
        if(weight<=0)return;
        float s=side=="L" ? 1 : -1;
        AimBone("arm_upper_"+side,"arm_lower_"+side,new Vector3(.22f,.2f*s,-1).Normal,weight);
        AimBone("arm_lower_"+side,"hand_"+side,new Vector3(1,.12f*s,.08f).Normal,weight);
        SetPalm(side,Vector3.Up,weight);
    }
    /// <summary>Turns the hand palm-up and lays the fingers flat, like a waiter
    /// balancing a tray.</summary>
    private void SetPalm(string side,Vector3 normal,float weight)
    {
        string hand="hand_"+side;
        if(!Poses.TryGetValue(hand,out var h) || !Poses.TryGetValue("arm_lower_"+side,out var elbow)
            || !subtree.TryGetValue(hand,out var list) || !palmFrames.TryGetValue(side,out var frame))return;
        var along=(h.Position-elbow.Position).Normal;
        var palm=normal-along*Vector3.Dot(normal,along);
        if(palm.Length<.05f)palm=Vector3.Up-along*Vector3.Dot(Vector3.Up,along);
        if(palm.Length<.05f)return;
        // Spread forearm roll across the elbow-to-wrist segment. Twisting only
        // the hand collapsed the narrow wrist where its skin blends into it.
        var currentPalm=(h.Rotation*frame).Up;
        currentPalm=(currentPalm-along*Vector3.Dot(currentPalm,along)).Normal;
        float roll=MathF.Atan2(Vector3.Dot(Vector3.Cross(currentPalm,palm.Normal),along),Vector3.Dot(currentPalm,palm.Normal))*180/MathF.PI;
        var forearmRoll=Rotation.FromAxis(along,roll*.6f*weight.Clamp(0,1));
        foreach(var name in subtree["arm_lower_"+side])
        {
            var p=Poses[name];p.Position=elbow.Position+forearmRoll*(p.Position-elbow.Position);
            p.Rotation=forearmRoll*p.Rotation;Poses[name]=p;
        }
        h=Poses[hand];
        // Align the wrist with the forearm as well as turning the palm upward.
        var wanted=Rotation.LookAt(along,palm.Normal)*frame.Inverse;
        var q=Rotation.Slerp(h.Rotation,wanted,weight.Clamp(0,1))*h.Rotation.Inverse;
        var pivot=h.Position;
        foreach(var name in list)
        {
            if(!Poses.TryGetValue(name,out var p))continue;
            p.Position=pivot+q*(p.Position-pivot);p.Rotation=q*p.Rotation;Poses[name]=p;
        }
    }
    internal Rotation PalmFrame(string side)=>Poses["hand_"+side].Rotation*palmFrames[side];
    internal Vector3 PalmCenter(string side)=>Poses["hand_"+side].Position+PalmFrame(side).Forward*2.1f+PalmFrame(side).Up*2.0f;
    /// <summary>Throwing arm. Overhand for eggs and knives (hand cocked behind
    /// the head, whip forward, follow through across the body); backhand
    /// frisbee for plates (arm across the chest, fling out flat to the side).
    /// phase: 0..1 wind-up (1 = release), 1..2 follow-through.</summary>
    internal void Throw(string side,float phase,bool frisbee,float weight)
    {
        if(weight<=0)return;
        float s=side=="L" ? 1 : -1;
        var rest=(new Vector3(.2f,.25f*s,-1),new Vector3(.9f,.1f*s,.1f));
        // Reach across to the load in the other hand and take one.
        var reach=(new Vector3(.6f,-.35f*s,-.7f),new Vector3(.45f,-.85f*s,.15f));
        (Vector3 U,Vector3 F)[] keys=frisbee
            ? new[]{rest,reach,
                    (new Vector3(.5f,-.5f*s,-.25f),new Vector3(-.5f,-.8f*s,.05f)), // cocked across the chest
                    (new Vector3(.65f,.72f*s,.05f),new Vector3(.85f,.5f*s,.08f)),  // release, flat
                    (new Vector3(.25f,.95f*s,-.1f),new Vector3(.3f,.95f*s,-.12f))} // follow-through out wide
            : new[]{rest,reach,
                    (new Vector3(-.3f,.45f*s,.6f),new Vector3(-.25f,.1f*s,1)),      // cocked behind the head
                    (new Vector3(.85f,.25f*s,.4f),new Vector3(1,.05f*s,-.05f)),     // release
                    (new Vector3(.55f,-.15f*s,-.65f),new Vector3(.65f,-.35f*s,-.6f))}; // follow-through across
        // 0-.25 reach, hold to .35 (take it), .35-.65 raise and cock, hold to .78,
        // whip to release at 1, follow through to 1.5.
        int a,b;float t;
        if(phase<.25f){a=0;b=1;t=phase/.25f;}
        else if(phase<.35f){a=1;b=1;t=0;}
        else if(phase<.65f){a=1;b=2;t=(phase-.35f)/.3f;}
        else if(phase<.78f){a=2;b=2;t=0;}
        else if(phase<1){a=2;b=3;t=(phase-.78f)/.22f;}
        else{a=3;b=4;t=((phase-1)/.5f).Clamp(0,1);}
        t=t*t*(3-2*t);
        var up=Vector3.Lerp(keys[a].U.Normal,keys[b].U.Normal,t).Normal;
        var fore=Vector3.Lerp(keys[a].F.Normal,keys[b].F.Normal,t).Normal;
        AimBone("arm_upper_"+side,"arm_lower_"+side,up,weight);
        AimBone("arm_lower_"+side,"hand_"+side,fore,weight);
        var palm=Vector3.Up;
        if(!frisbee && phase>=.35f)
        {
            float roll=phase<.78f ? ((phase-.35f)/.43f).Clamp(0,1) : ((phase-.78f)/.22f).Clamp(0,1);
            roll=roll*roll*(3-2*roll);
            palm=phase<.78f ? Vector3.Lerp(Vector3.Up,Vector3.Forward,roll).Normal : Vector3.Lerp(Vector3.Forward,Vector3.Down,roll).Normal;
        }
        SetPalm(side,palm,weight);
    }
    private void AimBone(string bone,string child,Vector3 dir,float weight)
    {
        if(!Poses.TryGetValue(bone,out var b) || !Poses.TryGetValue(child,out var c) || !subtree.TryGetValue(bone,out var list))return;
        var pivot=b.Position;var current=(c.Position-pivot).Normal;
        var axis=Vector3.Cross(current,dir);float sin=axis.Length;
        if(sin<.0001f)
        {
            if(Vector3.Dot(current,dir)>0)return;
            axis=Vector3.Cross(current,MathF.Abs(current.z)<.9f ? Vector3.Up : Vector3.Left).Normal;sin=0;
        }
        float angle=MathF.Atan2(sin,Vector3.Dot(current,dir))*180/MathF.PI*weight.Clamp(0,1);
        var q=Rotation.FromAxis(sin>0 ? axis/sin : axis,angle);
        foreach(var name in list)
        {
            if(!Poses.TryGetValue(name,out var p))continue;
            p.Position=pivot+q*(p.Position-pivot);p.Rotation=q*p.Rotation;Poses[name]=p;
        }
    }
    internal void Update(SkinnedModelRenderer driver,SkinnedModelRenderer slip,float slipBlend,Angles recoil,int missingParts=0)
    {
        sourcePose.Clear();Poses.Clear();
        foreach(var j in joints)
        {
            var p=driver.TryGetBoneTransform(j.Name,out var world) ? driver.WorldTransform.ToLocal(world) : j.SourceRest;
            if(slipBlend>0 && slip.IsValid() && slip.TryGetBoneTransform(j.Name,out var other))
            {
                var s=slip.WorldTransform.ToLocal(other);
                p.Position=Vector3.Lerp(p.Position,s.Position,slipBlend);
                p.Rotation=Rotation.Slerp(p.Rotation,s.Rotation,slipBlend);
            }
            sourcePose[j.Name]=p;
        }
        foreach(var j in joints)
        {
            var animated=sourcePose[j.Name];
            var local=j.Parent is null ? animated : sourcePose[j.Parent].ToLocal(animated);
            var result=j.Local;
            result.Rotation=j.Local.Rotation*j.SourceLocal.Rotation.Inverse*local.Rotation;
            // Do not copy the human skeleton's animated bone positions/scales.
            // This preserves arm, leg and casing lengths even during a full fall.
            if(j.Parent is null)result.Position=j.Rest.Position+(animated.Position-j.SourceRest.Position)*.40f;
            if(j.Name=="spine_1" || j.Name=="spine_2")result.Rotation*=Rotation.From(recoil*.5f);
            Poses[j.Name]=j.Parent is null ? result : Poses[j.Parent].ToWorld(result);
        }
        // The native gait supplies the timing; our larger boots plant on the floor.
        float bottom=float.PositiveInfinity;
        foreach(string side in new[]{"L","R"})
        {
            if((missingParts&(side=="L" ? 8 : 16))!=0)continue;
            if(Poses.TryGetValue("ankle_"+side,out var foot))bottom=MathF.Min(bottom,foot.Position.z-4.7f);
            if(Poses.TryGetValue("ball_"+side,out var toe))bottom=MathF.Min(bottom,toe.Position.z-1.55f);
        }
        if(slipBlend>.1f)
        {
            foreach(var item in new[]{("pelvis",5f),("spine_0",7f),("spine_1",8f),("spine_2",7f),("head",7f)})
                if(Poses.TryGetValue(item.Item1,out var p))bottom=MathF.Min(bottom,p.Position.z-item.Item2*slipBlend);
        }
        if(float.IsFinite(bottom))
        {
            float lift=-bottom;
            foreach(var j in joints){var p=Poses[j.Name];p.Position+=Vector3.Up*lift;Poses[j.Name]=p;}
        }
    }
}