using AutoRig.Analyze;
using AutoRig.Mesh;
using AutoRig.Rig;

namespace AutoRig.Solve.Organic;

// s&box compat: the engine defines Vector2/Vector3 in the GLOBAL namespace, which
// shadows using-directive imports - alias explicitly to System.Numerics.
using Vector3 = System.Numerics.Vector3;

/// <summary>
/// Snaps a humanoid-shaped curve skeleton to a citizen-compatible joint set: pelvis,
/// spine chain, neck/head, clavicle/arm/hand and leg/ankle per side, placed at
/// anatomical fractions along the traced limb centerlines and symmetrized.
/// Returns null when the limbs cannot be identified.
/// </summary>
public static class HumanoidTemplate
{
    public static RigSkeleton? Build( SkeletonGraph graph, SymmetryPlane symmetry, Aabb3 bounds )
    {
        ArgumentNullException.ThrowIfNull( graph );
        if ( graph.Tips.Count < 5 )
            return null;

        var up = TemplateUtil.UpAxis( bounds.Size );
        var upExtent = MathF.Max( 1e-6f, Vector3.Dot( bounds.Size, new Vector3(
            MathF.Abs( up.X ), MathF.Abs( up.Y ), MathF.Abs( up.Z ) ) ) );
        var bottom = Vector3.Dot( bounds.Min, up );
        float HeightOf( Vector3 p ) => (Vector3.Dot( p, up ) - bottom) / upExtent;

        var tipPositions = graph.Tips.Select( t => graph.Nodes[t].Position ).ToList();
        var (pairs, unpaired) = CategoryDetector.PairTips(
            tipPositions, symmetry, bounds.Size.Length() * 0.15f );
        if ( pairs.Count < 2 || unpaired.Count == 0 )
            return null;

        float PairHeight( (int A, int B) p )
            => (HeightOf( tipPositions[p.A] ) + HeightOf( tipPositions[p.B] )) * 0.5f;

        var byHeight = pairs.OrderBy( PairHeight ).ToList();
        var legsPair = byHeight[0];
        var armsPair = byHeight[^1];
        if ( PairHeight( legsPair ) > 0.30f || PairHeight( armsPair ) < 0.50f )
            return null;

        var headTipIndex = unpaired
            .OrderByDescending( u => HeightOf( tipPositions[u] ) )
            .First();
        if ( HeightOf( tipPositions[headTipIndex] ) < 0.75f )
            return null;

        // Branch polylines run tip → junction; reverse to junction → tip so fractions
        // measure from the attachment (shoulder/hip) outward.
        var armL = BranchPolyline( graph, graph.Tips[SideOf( tipPositions[armsPair.A], symmetry ) > 0 ? armsPair.A : armsPair.B] );
        var armR = BranchPolyline( graph, graph.Tips[SideOf( tipPositions[armsPair.A], symmetry ) > 0 ? armsPair.B : armsPair.A] );
        var legL = BranchPolyline( graph, graph.Tips[SideOf( tipPositions[legsPair.A], symmetry ) > 0 ? legsPair.A : legsPair.B] );
        var legR = BranchPolyline( graph, graph.Tips[SideOf( tipPositions[legsPair.A], symmetry ) > 0 ? legsPair.B : legsPair.A] );
        var headBranch = BranchPolyline( graph, graph.Tips[headTipIndex] );
        if ( armL.Count < 2 || armR.Count < 2 || legL.Count < 2 || legR.Count < 2 || headBranch.Count < 2 )
            return null;

        // Anchors — projected onto the symmetry plane: limb attachments sit slightly
        // asymmetric on real meshes, and averaging them alone leaves the whole spine
        // leaning ("crooked spine").
        Vector3 OnPlane( Vector3 p ) => (p + CategoryDetector.Mirror( p, symmetry )) * 0.5f;
        var pelvis = OnPlane( (legL[0] + legR[0]) * 0.5f );
        var neckBase = OnPlane( (armL[0] + armR[0]) * 0.5f );
        if ( HeightOf( neckBase ) <= HeightOf( pelvis ) )
            return null;

        Vector3 SpineAt( float t ) => Vector3.Lerp( pelvis, neckBase, t );

        // Mirror-symmetrize a left position with its right twin for cleanliness.
        Vector3 Sym( Vector3 left, Vector3 right )
            => (left + CategoryDetector.Mirror( right, symmetry )) * 0.5f;

        var skeleton = new RigSkeleton();
        int Add( string name, int parent, Vector3 position )
        {
            skeleton.Joints.Add( new RigJoint { Name = name, Parent = parent, Position = position } );
            return skeleton.Joints.Count - 1;
        }

        var pelvisJ = Add( "pelvis", -1, pelvis );
        var spine0 = Add( "spine_0", pelvisJ, SpineAt( 0.25f ) );
        var spine1 = Add( "spine_1", spine0, SpineAt( 0.5f ) );
        var spine2 = Add( "spine_2", spine1, SpineAt( 0.75f ) );
        var neck = Add( "neck_0", spine2, OnPlane( Along( headBranch, 0.15f ) ) );
        Add( "head", neck, OnPlane( Along( headBranch, 0.6f ) ) );

        // Arms: clavicle 30% from spine toward the shoulder, then shoulder/elbow/hand.
        var shoulderL = Sym( Along( armL, 0f ), Along( armR, 0f ) );
        var shoulderR = CategoryDetector.Mirror( shoulderL, symmetry );
        var clavL = Add( "clavicle_L", spine2, Vector3.Lerp( SpineAt( 0.85f ), shoulderL, 0.3f ) );
        var armUpperL = Add( "arm_upper_L", clavL, shoulderL );
        var armLowerL = Add( "arm_lower_L", armUpperL, Sym( Along( armL, 0.5f ), Along( armR, 0.5f ) ) );
        Add( "hand_L", armLowerL, Sym( Along( armL, 0.95f ), Along( armR, 0.95f ) ) );

        var clavR = Add( "clavicle_R", spine2, CategoryDetector.Mirror(
            skeleton.Joints[clavL].Position, symmetry ) );
        var armUpperR = Add( "arm_upper_R", clavR, shoulderR );
        var armLowerR = Add( "arm_lower_R", armUpperR, CategoryDetector.Mirror(
            skeleton.Joints[armLowerL].Position, symmetry ) );
        Add( "hand_R", armLowerR, CategoryDetector.Mirror(
            skeleton.Joints[skeleton.IndexOf( "hand_L" )].Position, symmetry ) );

        // Legs: hip/knee/ankle at 0 / 0.5 / 0.9.
        var hipL = Sym( Along( legL, 0f ), Along( legR, 0f ) );
        var legUpperL = Add( "leg_upper_L", pelvisJ, hipL );
        var legLowerL = Add( "leg_lower_L", legUpperL, Sym( Along( legL, 0.5f ), Along( legR, 0.5f ) ) );
        Add( "ankle_L", legLowerL, Sym( Along( legL, 0.9f ), Along( legR, 0.9f ) ) );

        var legUpperR = Add( "leg_upper_R", pelvisJ, CategoryDetector.Mirror( hipL, symmetry ) );
        var legLowerR = Add( "leg_lower_R", legUpperR, CategoryDetector.Mirror(
            skeleton.Joints[legLowerL].Position, symmetry ) );
        Add( "ankle_R", legLowerR, CategoryDetector.Mirror(
            skeleton.Joints[skeleton.IndexOf( "ankle_L" )].Position, symmetry ) );

        skeleton.Validate();
        return skeleton;
    }

    static float SideOf( Vector3 p, SymmetryPlane plane ) => TemplateUtil.SideOf( p, plane );

    static List<Vector3> BranchPolyline( SkeletonGraph graph, int tip )
        => TemplateUtil.BranchPolyline( graph, tip );

    static Vector3 Along( List<Vector3> polyline, float t ) => TemplateUtil.Along( polyline, t );
}