// anim_probe — bisect the dance_1 "head/neck collapse".
//
//   anim_probe <model.gr2> <anim.gr2> [--bone <substr>] [t0 t1 ...]
//
// libgr2's world pose per bone is well-formed (det, shear, finiteness) — the probe
// confirms that. The real question is the mtgodot bridge: metin2_anim.cpp feeds
//   local = conv(world[parent])⁻¹ · conv(world[i])
// to Skeleton3D::set_bone_pose(), which in Godot 4 DECOMPOSES to position +
// Quaternion + per-axis scale. That representation cannot hold shear. So for every
// bone we also compute the parent-relative local, its shear, and the error of a
// T·R(quat)·S round-trip (= what Godot actually keeps). High round-trip error =
// Godot will visibly break that bone once the anim leaves bind pose.
#include "gr2/gr2.h"
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <string>
#include <vector>

using gr2::Mat4;

static double det3(const Mat4& m){
    return m[0]*(m[5]*m[10]-m[6]*m[9]) - m[4]*(m[1]*m[10]-m[2]*m[9]) + m[8]*(m[1]*m[6]-m[2]*m[5]);
}
static void axes_len(const Mat4& m,double&a,double&b,double&c){
    a=std::sqrt(m[0]*m[0]+m[1]*m[1]+m[2]*m[2]);
    b=std::sqrt(m[4]*m[4]+m[5]*m[5]+m[6]*m[6]);
    c=std::sqrt(m[8]*m[8]+m[9]*m[9]+m[10]*m[10]);
}
static double shear(const Mat4& m){
    double r0[3]={m[0],m[1],m[2]}, r1[3]={m[4],m[5],m[6]}, r2[3]={m[8],m[9],m[10]};
    double a,b,c; axes_len(m,a,b,c);
    if(a<1e-9||b<1e-9||c<1e-9) return 9.99;
    auto D=[&](double*x,double*y){return x[0]*y[0]+x[1]*y[1]+x[2]*y[2];};
    return std::fmax(std::fabs(D(r0,r1))/(a*b),
           std::fmax(std::fabs(D(r0,r2))/(a*c), std::fabs(D(r1,r2))/(b*c)));
}
static bool finite16(const Mat4& m){ for(float v:m) if(!std::isfinite(v)) return false; return true; }

static Mat4 mul(const Mat4& A,const Mat4& B){   // row-major, v'=v*M
    Mat4 R{};
    for(int r=0;r<4;++r)for(int c=0;c<4;++c){ double s=0; for(int k=0;k<4;++k) s+=(double)A[r*4+k]*B[k*4+c]; R[r*4+c]=(float)s; }
    return R;
}
static Mat4 inv_affine(const Mat4& m){
    double a=m[0],b=m[1],c=m[2],d=m[4],e=m[5],f=m[6],g=m[8],h=m[9],i=m[10];
    double D=a*(e*i-f*h)-b*(d*i-f*g)+c*(d*h-e*g);
    Mat4 r{}; if(std::fabs(D)<1e-12){ for(int k=0;k<16;++k) r[k]=std::nanf(""); return r; }
    double id=1.0/D;
    double m00=(e*i-f*h)*id,m01=(c*h-b*i)*id,m02=(b*f-c*e)*id;
    double m10=(f*g-d*i)*id,m11=(a*i-c*g)*id,m12=(c*d-a*f)*id;
    double m20=(d*h-e*g)*id,m21=(b*g-a*h)*id,m22=(a*e-b*d)*id;
    r[0]=m00;r[1]=m01;r[2]=m02; r[4]=m10;r[5]=m11;r[6]=m12; r[8]=m20;r[9]=m21;r[10]=m22; r[15]=1;
    double tx=m[12],ty=m[13],tz=m[14];
    r[12]=-(tx*m00+ty*m10+tz*m20); r[13]=-(tx*m01+ty*m11+tz*m21); r[14]=-(tx*m02+ty*m12+tz*m22);
    return r;
}
// Godot set_bone_pose keeps  T · R(orthonormal, from unit-quat) · diag(S).
// R comes from Gram-Schmidt-style orthonormalization of the local basis (matching
// Basis::get_rotation_quaternion / orthonormalized), so shear IS dropped.
// Reconstruct  R·diag(S)  and return max |element - original local 3x3|.
static double trs_roundtrip_err(const Mat4& L){
    double x[3]={L[0],L[1],L[2]}, y[3]={L[4],L[5],L[6]}, z[3]={L[8],L[9],L[10]};
    auto nrm=[&](double*v){ double l=std::sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]); if(l<1e-12) return 0.0; v[0]/=l;v[1]/=l;v[2]/=l; return l; };
    auto dot=[&](double*a,double*b){return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];};
    // Gram-Schmidt: x normalized; y -= (y·x)x then normalized; z = x×y (keep handedness)
    double sx=nrm(x); if(sx==0) return 9.99;
    double d=dot(y,x); double yo[3]={y[0]-d*x[0],y[1]-d*x[1],y[2]-d*x[2]};
    double sy=std::sqrt(yo[0]*yo[0]+yo[1]*yo[1]+yo[2]*yo[2]); if(sy<1e-12) return 9.99;
    yo[0]/=sy;yo[1]/=sy;yo[2]/=sy;
    double zo[3]={ x[1]*yo[2]-x[2]*yo[1], x[2]*yo[0]-x[0]*yo[2], x[0]*yo[1]-x[1]*yo[0] };
    double sz=dot(z,zo);   // signed
    // recompose rows: sx*x, sy*yo, sz*zo   (Godot's scale is per-axis on the orthonormal frame)
    double M[9]={ sx*x[0],sx*x[1],sx*x[2],  sy*yo[0],sy*yo[1],sy*yo[2],  sz*zo[0],sz*zo[1],sz*zo[2] };
    const double orig[9]={L[0],L[1],L[2],L[4],L[5],L[6],L[8],L[9],L[10]};
    double e=0; for(int k=0;k<9;++k) e=std::fmax(e,std::fabs(M[k]-orig[k]));
    return e;
}

int main(int argc,char**argv){
    if(argc<3){ std::fprintf(stderr,"usage: anim_probe <model.gr2> <anim.gr2> [--bone <substr>] [t...]\n"); return 2; }
    std::string bonefilter;
    std::vector<float> ts;
    for(int i=3;i<argc;++i){
        if(!std::strcmp(argv[i],"--bone") && i+1<argc){ bonefilter=argv[++i]; }
        else ts.push_back((float)atof(argv[i]));
    }
    gr2::LoadError e;
    auto model=gr2::File::load_path(argv[1],&e);
    if(!model){ std::fprintf(stderr,"model: %s %s\n",e.stage.c_str(),e.message.c_str()); return 1; }
    auto anim=gr2::File::load_path(argv[2],&e);
    if(!anim){ std::fprintf(stderr,"anim: %s %s\n",e.stage.c_str(),e.message.c_str()); return 1; }
    const auto& sk=model->file_info().skeletons.at(0);
    const auto& an=anim->file_info().animations.at(0);
    std::printf("model=%s anim=%s dur=%.3f bones=%zu tracks=%zu\n\n",argv[1],an.name.c_str(),an.duration,sk.bones.size(),an.tracks.size());
    if(ts.empty()) for(int k=0;k<=16;++k) ts.push_back(an.duration*k/16.0f);

    // per-bone maxima across a fine sweep
    std::vector<double> mxLocShear(sk.bones.size(),0), mxTRS(sk.bones.size(),0), mxWShear(sk.bones.size(),0), mnDet(sk.bones.size(),1e9);
    std::vector<Mat4> W,S;
    for(int k=0;k<=240;++k){
        gr2::sample_pose(sk,an,an.duration*k/240.0f,W,S);
        for(size_t i=0;i<W.size()&&i<sk.bones.size();++i){
            if(!finite16(W[i])) continue;
            mxWShear[i]=std::fmax(mxWShear[i],shear(W[i]));
            mnDet[i]=std::fmin(mnDet[i],std::fabs(det3(W[i])));
            int p=sk.bones[i].parent;
            if(p>=0&&p<(int)W.size()&&finite16(W[p])){
                Mat4 L=mul(inv_affine(W[p]),W[i]);
                if(finite16(L)){ mxLocShear[i]=std::fmax(mxLocShear[i],shear(L)); mxTRS[i]=std::fmax(mxTRS[i],trs_roundtrip_err(L)); }
            }
        }
    }
    std::printf("== bones Godot's set_bone_pose can't represent (TRS round-trip err) ==\n");
    std::printf("%-4s %-26s %10s %10s %10s %8s  parent\n","idx","name","locShear","TRSerr","wShear","minDet");
    bool any=false;
    for(size_t i=0;i<sk.bones.size();++i){
        if(mxTRS[i]>0.01 || mxLocShear[i]>0.02){
            any=true;
            std::printf("b%-3zu %-26s %10.4f %10.4f %10.4f %8.4f  %d\n",
                i,sk.bones[i].name.c_str(),mxLocShear[i],mxTRS[i],mxWShear[i],mnDet[i],sk.bones[i].parent);
        }
    }
    if(!any) std::printf("  (none — every parent-relative local is T·R·S representable)\n");

    if(!bonefilter.empty()){
        std::printf("\n== per-frame detail for bones matching \"%s\" ==\n",bonefilter.c_str());
        for(float t: ts){
            gr2::sample_pose(sk,an,t,W,S);
            std::printf("t=%7.3f\n",t);
            for(size_t i=0;i<W.size()&&i<sk.bones.size();++i){
                if(sk.bones[i].name.find(bonefilter)==std::string::npos) continue;
                double a,b,c; axes_len(W[i],a,b,c);
                int p=sk.bones[i].parent;
                double locsh=0,trs=0;
                if(p>=0&&p<(int)W.size()){ Mat4 L=mul(inv_affine(W[p]),W[i]); locsh=shear(L); trs=trs_roundtrip_err(L); }
                std::printf("   b%-3zu %-24s  worldAxes=(%.3f %.3f %.3f) wDet=%.4f  locShear=%.4f  TRSerr=%.4f\n",
                    i,sk.bones[i].name.c_str(),a,b,c,det3(W[i]),locsh,trs);
            }
        }
    }
    return 0;
}