using System;

namespace CharacterEditor;

/// <summary>One pose owner composes growth and optional ground-contact IK over native animation.</summary>
public sealed class CharacterFootPlacement : IDisposable
{
 readonly CharacterGrowthPose growth = new();
 readonly int[] supportBones = new int[8];
 readonly Transform[] bind = new Transform[8];
 readonly Vector3[] heightAxes = new Vector3[8];
 readonly float[] heightOffsets = new float[8];
 Vector3[,] contactLocal;
 NativeSoleContacts.Point[] contacts;
 Model[] shoes = Array.Empty<Model>();
 float shoeCheck;
 readonly int[] hips = new int[2], knees = new int[2], ankles = new int[2];
 int[] parents, legBones;
 bool[] legBoneMask;
 Transform[] original, solved;
 SkinnedModelRenderer target;
 Model model;
 bool ownsOverrides, footRigValid;
 float size = -1, male = -1;
 static readonly string[] ManualFeet = { "ik.foot_left.enabled", "ik.foot_right.enabled" };

 public void Prepare(SkinnedModelRenderer body)
 {
  if (ownsOverrides && target.IsValid() && target.SceneModel is not null) target.SceneModel.ClearBoneOverrides();
  ownsOverrides = false;
 }

 /// <summary>
 /// Growth applies to native graph playback and frozen/sequence previews, even
 /// while airborne or with ground contact disabled. Call Prepare before native
 /// pose evaluation so the previous submitted pose never feeds back into itself.
 /// </summary>
 public void Update(SkinnedModelRenderer body, CharacterAppearanceData appearance, bool groundFeet = true, bool allowEditor = false)
 {
  if (!body.IsValid() || appearance is null || (body.Scene.IsEditor && !allowEditor) || !body.Enabled || body.SceneModel is null) return;
  var native = body.SceneModel;
  // Another pose owner takes precedence. Growth and contact submit together,
  // rather than each mistaking the other's overrides for an external owner.
  if (native.HasBoneOverrides()) return;
  growth.Configure(body, appearance);
  bool growing = growth.IsActive;
  bool resolveFeet = groundFeet && !body.Scene.IsEditor && body.UseAnimGraph && body.Parameters.GetBool("b_grounded")
   && body.Parameters.GetInt("sit") == 0 && !body.Parameters.GetBool("b_sit")
   && !body.Parameters.GetBool("b_swim") && !body.Parameters.GetBool("b_noclip") && body.Parameters.GetInt("special_movement_states") == 0;
  if (!growing && !resolveFeet) return;
  if (target != body || model != body.Model || original is null) CacheRig(body);
  resolveFeet &= footRigValid;
  if (!growing && !resolveFeet) return;

  if (growing)
  {
   for (int i = 0; i < model.BoneCount; i++) original[i] = native.GetBoneWorldTransform(i);
   growth.ApplyTo(original, solved);
   // The contact solve starts from the grown anatomy, retaining the native
   // rotations and shortened limb lengths during each candidate solve.
   Array.Copy(solved, original, original.Length);
  }
  else
   foreach (int i in legBones) solved[i] = original[i] = native.GetBoneWorldTransform(i);

  if (resolveFeet)
  {
   shoeCheck -= Time.Delta;
   if (shoeCheck <= 0 || contacts is null)
   {
    shoeCheck = .5f;
    var current = body.GameObject.GetComponentsInChildren<SkinnedModelRenderer>(true,true)
     .Where(r=>r!=body && r.Enabled && r.GameObject.Enabled && AutomaticWardrobeFit.FootContacts(r.Model) is not null).Select(r=>r.Model).ToArray();
    if (!current.SequenceEqual(shoes) || contacts is null) { shoes=current; size=-1; contacts=null; }
   }
   UpdateContacts(appearance);
  }

  float scale=MathF.Max(.01f,body.WorldScale.z);bool changed=growing;
  for(int side=0;resolveFeet && side<2;side++)
  {
   if(body.Parameters.GetBool(ManualFeet[side]))continue;
   var foot=original[ankles[side]].Position;
   var ground=body.Scene.Trace.Ray(foot+Vector3.Up*12*scale,foot-Vector3.Up*20*scale).IgnoreGameObjectHierarchy(body.GameObject.Root).Run();
   if(!ground.Hit)continue;
   float floor=ground.HitPosition.z+.04f*scale;
   float lift=floor-Height(side);
   if(lift<=.005f*scale||lift>8*scale)continue;
   for(int iteration=0;iteration<10;iteration++)
   {
    Solve(side,foot+Vector3.Up*lift);
    float residual=floor-Height(side);
    if(residual<=.002f*scale)break;
    lift+=MathF.Min(residual,scale);
   }
   changed=true;
  }
  if(!changed)return;
  // Submitting only the corrected legs allows the update below to reevaluate
  // unprotected arms without the native grip IK result. Preserve the complete
  // evaluated pose whenever contact changes it; defer this capture until a
  // correction exists so unchanged adult frames still read only the legs.
  if (!growing)
   for (int i = 0; i < solved.Length; i++)
    if (!legBoneMask[i]) solved[i] = native.GetBoneWorldTransform(i);
  for (int i = 0; i < solved.Length; i++) native.SetBoneOverride(i,body.WorldTransform.ToLocal(solved[i]));
  ownsOverrides=true;
  float rate=native.PlaybackRate;
  try { native.PlaybackRate=0;native.Update(0); }
  finally { native.PlaybackRate=rate; }
#pragma warning disable CS0612, CS0618
  body.PostAnimationUpdate(); // Native bone merging keeps every garment on the final shared pose.
#pragma warning restore CS0612, CS0618
 }

 void CacheRig(SkinnedModelRenderer body)
 {
  target = body; model = body.Model; size = male = -1; shoeCheck = 0;
  contacts = null; contactLocal = null; shoes = Array.Empty<Model>(); footRigValid = false;
  int count = model?.BoneCount ?? 0;
  original = new Transform[count]; solved = new Transform[count];
  parents = new int[count]; legBones = Array.Empty<int>();
  legBoneMask = new bool[count];
  var names = new string[count];
  for (int i = 0; i < count; i++) { names[i] = model.GetBoneName(i); parents[i] = model.GetBoneParent(i); }
  int Find(string name) => Array.IndexOf(names, name);
  for (int i = 0; i < supportBones.Length; i++)
  {
   supportBones[i] = Find(NativeSoleContacts.Bones[i]);
   if (supportBones[i] < 0) return;
   bind[i] = model.GetBoneTransform(supportBones[i]);
  }
  hips[0]=Find("leg_upper_L");hips[1]=Find("leg_upper_R");knees[0]=Find("leg_lower_L");knees[1]=Find("leg_lower_R");ankles[0]=Find("ankle_L");ankles[1]=Find("ankle_R");
  if (hips.Any(i=>i<0) || knees.Any(i=>i<0) || ankles.Any(i=>i<0)) return;
  int Depth(int i)
  {
   int depth = 0;
   while (i >= 0 && depth <= count) { if (i >= count) return count + 1; depth++; i = parents[i]; }
   return depth;
  }
  var depths = new int[count];
  for (int i = 0; i < count; i++) { depths[i] = Depth(i); if (depths[i] > count) return; }
  bool InLeg(int i) { while(i>=0) { if(i==hips[0]||i==hips[1])return true; i=parents[i]; } return false; }
  legBones=Enumerable.Range(0,count).Where(InLeg).OrderBy(i=>depths[i]).ToArray();
  foreach (int i in legBones) legBoneMask[i] = true;
  footRigValid = true;
 }

 void UpdateContacts(CharacterAppearanceData appearance)
 {
  float feetMale=BodyRegions.MaleAmount(appearance,"feet");
  if(size!=appearance.FootSize||male!=feetMale||contacts is null)
  {
   size=appearance.FootSize;male=feetMale;
   var all=NativeSoleContacts.Points.Concat(shoes.SelectMany(m=>AutomaticWardrobeFit.FootContacts(m)??Array.Empty<AutomaticWardrobeFit.SolePoint>()).Select(s=>s.Male with { Position=Vector3.Lerp(s.Female,s.Male.Position,male) }))
    .Select(s=>s with { Position=Shape(s.Position,size,male,appearance.BaseShape is not null) }).ToArray();
   contacts=FootContactHull.Reduce(all);
   contactLocal = new Vector3[contacts.Length,4];
   for(int i=0;i<contacts.Length;i++)
   {
    var sample=contacts[i];var p=sample.Position;
    for(int k=0;k<4;k++){int b=Bone(sample,k);contactLocal[i,k]=bind[b].Rotation.Inverse*(p-bind[b].Position);}
   }
  }
 }

 void Solve(int side,Vector3 targetFoot)
 {
  int h=hips[side],k=knees[side],a=ankles[side];
  var hip=original[h].Position;var knee=original[k].Position;var foot=original[a].Position;
  float upper=(knee-hip).Length,lower=(foot-knee).Length;
  var axis=(targetFoot-hip).Normal;
  float distance=Math.Clamp((targetFoot-hip).Length,.001f,upper+lower-.0001f);
  float along=(upper*upper-lower*lower+distance*distance)/(2*distance);
  var bend=(knee-hip)-axis*Vector3.Dot(knee-hip,axis);
  if(bend.LengthSquared<.000001f)bend=original[h].Rotation.Forward;
  var newKnee=hip+axis*along+bend.Normal*MathF.Sqrt(MathF.Max(0,upper*upper-along*along));
  solved[h]=new Transform(hip,Rotation.FromToRotation(knee-hip,newKnee-hip)*original[h].Rotation,original[h].Scale);
  solved[k]=new Transform(newKnee,Rotation.FromToRotation(foot-knee,targetFoot-newKnee)*original[k].Rotation,original[k].Scale);
  solved[a]=new Transform(targetFoot,original[a].Rotation,original[a].Scale);
  foreach(int i in legBones)
  {
   if(i==h||i==k||i==a)continue;
   int ancestor=i;while(ancestor>=0&&ancestor!=h)ancestor=parents[ancestor];
   if(ancestor!=h)continue;
   int p=parents[i];
   solved[i]=solved[p].ToWorld(original[p].ToLocal(original[i]));
  }
 }
 float Height(int side)
 {
  // Plane clearance needs only world Z, not four complete rotated positions
  // per vertex. Cache the eight bone rows once for this candidate pose.
  for(int b=0;b<supportBones.Length;b++)
  {
   var t=solved[supportBones[b]];
   heightAxes[b]=new Vector3((t.Rotation*Vector3.Forward).z,(t.Rotation*Vector3.Left).z,(t.Rotation*Vector3.Up).z)*t.Scale;
   heightOffsets[b]=t.Position.z;
  }
  float minimum=float.MaxValue;
  for(int i=0;i<contacts.Length;i++)
  {
   var sample=contacts[i];if((sample.Position.y>0?0:1)!=side)continue;
   float z=0;
   for(int k=0;k<4;k++){float w=Weight(sample,k);if(w==0)continue;int b=Bone(sample,k);z+=(heightOffsets[b]+Vector3.Dot(heightAxes[b],contactLocal[i,k]))*w;}
   minimum=MathF.Min(minimum,z);
  }
  return minimum;
 }
 static int Bone(NativeSoleContacts.Point s,int k)=>k switch{0=>s.B0,1=>s.B1,2=>s.B2,_=>s.B3};
 static float Weight(NativeSoleContacts.Point s,int k)=>k switch{0=>s.W0,1=>s.W1,2=>s.W2,_=>s.W3};
 static Vector3 Shape(Vector3 point,float size,float male,bool unified)
 {
  var gender=unified?Vector3.Lerp(new(.8f,.84f,1),Vector3.One,male):Vector3.One;
  var footSize=Vector3.One+new Vector3(.22f,.18f,0)*((size-.5f)*2);
  var center=new Vector3(0,point.y>0?7.8f:-7.8f,1.8f);var mask=new Vector3(9,4,4.2f);
  var p=(point-center)/mask;float t=Math.Clamp((1-p.Length)/.65f,0,1);float influence=t*t*(3-2*t);
  var shift=new Vector3(-2.8f/9*(1-gender.x)*footSize.x,0,0);
  var candidate=Vector3.Lerp(p,p*gender*footSize+shift,influence);if(candidate.Length>1)candidate=candidate.Normal;
  return point+(candidate-p)*mask;
 }
 public void Dispose(){Prepare(target);target=null;model=null;contacts=null;contactLocal=null;original=solved=null;parents=legBones=null;legBoneMask=null;footRigValid=false;shoes=Array.Empty<Model>();growth.Configure(null,1,Transform.Zero);}
}

/// <summary>Clear owned poses before native playback, then compose growth and contact before rendering.</summary>
public sealed class CharacterFootPlacementSystem:GameObjectSystem<CharacterFootPlacementSystem>
{
 public CharacterFootPlacementSystem(Scene scene):base(scene)
 {
  Listen(Stage.UpdateBones,-100,PrepareFeet,"Prepare foot IK");
  Listen(Stage.UpdateBones,100,UpdateFeet,"Resolve foot contact");
 }
 void PrepareFeet(){foreach(var a in Scene.GetAllComponents<CharacterAvatar>())a.PrepareFeet();}
 void UpdateFeet(){foreach(var a in Scene.GetAllComponents<CharacterAvatar>())a.UpdateFeet();}
}