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 quadruped-shaped curve skeleton to a named four-legged joint set:
/// pelvis at the rear hips, a spine chain along the horizontal body toward the
/// shoulders, neck/head from the head branch, optional two-joint tail, and
/// upper/lower/ankle per leg with FL/FR/BL/BR suffixes (front = head end).
/// Returns null when the body plan cannot be identified.
/// </summary>
public static class QuadrupedTemplate
{
    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 );

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

        // The two lowest pairs are the legs.
        var legPairs = pairs.Where( p => PairHeight( p ) <= 0.35f )
            .OrderBy( PairHeight ).Take( 2 ).ToList();
        if ( legPairs.Count < 2 || unpaired.Count == 0 )
            return null;

        // Head: the highest unpaired tip.
        var headTipIndex = unpaired.OrderByDescending( u => HeightOf( tipPositions[u] ) ).First();
        if ( HeightOf( tipPositions[headTipIndex] ) < 0.4f )
            return null;
        var headPosition = tipPositions[headTipIndex];

        // Front pair = leg pair whose midpoint is nearer the head (horizontally).
        Vector3 Flatten( Vector3 p ) => p - up * Vector3.Dot( p, up );
        Vector3 PairMid( (int A, int B) p ) => (tipPositions[p.A] + tipPositions[p.B]) * 0.5f;
        legPairs = legPairs.OrderBy( p =>
            Vector3.Distance( Flatten( PairMid( p ) ), Flatten( headPosition ) ) ).ToList();
        var frontPair = legPairs[0];
        var backPair = legPairs[1];

        // Left/right per pair (same convention as the humanoid template).
        (int L, int R) Sides( (int A, int B) pair )
            => TemplateUtil.SideOf( tipPositions[pair.A], symmetry ) > 0
                ? (pair.A, pair.B)
                : (pair.B, pair.A);
        var (frontL, frontR) = Sides( frontPair );
        var (backL, backR) = Sides( backPair );

        var legFL = TemplateUtil.BranchPolyline( graph, graph.Tips[frontL] );
        var legFR = TemplateUtil.BranchPolyline( graph, graph.Tips[frontR] );
        var legBL = TemplateUtil.BranchPolyline( graph, graph.Tips[backL] );
        var legBR = TemplateUtil.BranchPolyline( graph, graph.Tips[backR] );
        var headBranch = TemplateUtil.BranchPolyline( graph, graph.Tips[headTipIndex] );
        if ( legFL.Count < 2 || legFR.Count < 2 || legBL.Count < 2 || legBR.Count < 2
            || headBranch.Count < 2 )
            return null;

        Vector3 Sym( Vector3 left, Vector3 right )
            => (left + CategoryDetector.Mirror( right, symmetry )) * 0.5f;

        // Anchors: rear hips and front shoulders, projected onto the symmetry plane
        // so the spine cannot lean off-axis.
        Vector3 OnPlane( Vector3 p ) => (p + CategoryDetector.Mirror( p, symmetry )) * 0.5f;
        var pelvis = OnPlane( Sym( legBL[0], legBR[0] ) );
        var shoulders = OnPlane( Sym( legFL[0], legFR[0] ) );
        if ( Vector3.Distance( Flatten( pelvis ), Flatten( shoulders ) ) < bounds.Size.Length() * 0.05f )
            return null;   // legs bunched together — not a quadruped body plan

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

        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.85f ) );
        var neck = Add( "neck_0", spine2, TemplateUtil.Along( headBranch, 0.2f ) );
        Add( "head", neck, TemplateUtil.Along( headBranch, 0.65f ) );

        // Optional tail: an unpaired non-head tip whose attachment is behind the pelvis.
        var backward = Vector3.Normalize( Flatten( pelvis - shoulders ) );
        foreach ( var u in unpaired )
        {
            if ( u == headTipIndex )
                continue;
            var branch = TemplateUtil.BranchPolyline( graph, graph.Tips[u] );
            if ( branch.Count < 2 )
                continue;
            if ( Vector3.Dot( Flatten( tipPositions[u] - pelvis ), backward ) <= 0f )
                continue;
            var tail0 = Add( "tail_0", pelvisJ, TemplateUtil.Along( branch, 0.35f ) );
            Add( "tail_1", tail0, TemplateUtil.Along( branch, 0.9f ) );
            break;
        }

        void AddLeg( string suffix, int parent, List<Vector3> left, List<Vector3> right, bool isLeft )
        {
            Vector3 At( float t ) => isLeft
                ? Sym( TemplateUtil.Along( left, t ), TemplateUtil.Along( right, t ) )
                : CategoryDetector.Mirror(
                    Sym( TemplateUtil.Along( left, t ), TemplateUtil.Along( right, t ) ), symmetry );
            var upper = Add( $"leg_upper_{suffix}", parent, At( 0f ) );
            var lower = Add( $"leg_lower_{suffix}", upper, At( 0.5f ) );
            Add( $"ankle_{suffix}", lower, At( 0.9f ) );
        }

        AddLeg( "FL", spine2, legFL, legFR, isLeft: true );
        AddLeg( "FR", spine2, legFL, legFR, isLeft: false );
        AddLeg( "BL", pelvisJ, legBL, legBR, isLeft: true );
        AddLeg( "BR", pelvisJ, legBL, legBR, isLeft: false );

        skeleton.Validate();
        return skeleton;
    }
}