/** Bounded runtime corrections for action poses.
 * Canonical poses only: host translation and collision state are never filtered.
 * Transition offsets use a critically damped decay; toe planting uses two-bone
 * IK with the predicted knee plane and ankle orientation preserved.
 */
export const add=(a,b)=>a.map((v,i)=>v+b[i]);
export const sub=(a,b)=>a.map((v,i)=>v-b[i]);
const scale=(a,s)=>a.map(v=>v*s),dot=(a,b)=>a.reduce((s,v,i)=>s+v*b[i],0);
const cross=(a,b)=>[a[1]*b[2]-a[2]*b[1],a[2]*b[0]-a[0]*b[2],a[0]*b[1]-a[1]*b[0]];
const length=a=>Math.hypot(...a),unit=a=>scale(a,1/Math.max(length(a),1e-12));
const bounded=(v,maximum)=>scale(v,Math.min(1,maximum/Math.max(length(v),1e-12)));
const clamp=(v,a,b)=>Math.max(a,Math.min(b,v));
export const multiply=(a,b)=>[
  a[3]*b[0]+a[0]*b[3]+a[1]*b[2]-a[2]*b[1],
  a[3]*b[1]-a[0]*b[2]+a[1]*b[3]+a[2]*b[0],
  a[3]*b[2]+a[0]*b[1]-a[1]*b[0]+a[2]*b[3],
  a[3]*b[3]-dot(a.slice(0,3),b.slice(0,3))];
const inverse=q=>[-q[0],-q[1],-q[2],q[3]];
const rotate=(v,q)=>multiply(multiply(q,[...v,0]),inverse(q)).slice(0,3);
const log=q=>{if(q[3]<0)q=q.map(v=>-v);const n=length(q.slice(0,3));return scale(q.slice(0,3),n<1e-10?2:2*Math.atan2(n,q[3])/n);};
const exp=v=>{const a=length(v);return [...scale(v,a<1e-10?.5:Math.sin(a/2)/a),Math.cos(a/2)];};
const delta=(a,b)=>log(multiply(a,inverse(b)));
const between=(a,b)=>{a=unit(a);b=unit(b);const d=clamp(dot(a,b),-1,1);if(d<-.999999){const basis=Math.abs(a[0])<.8?[1,0,0]:[0,1,0];return exp(scale(unit(cross(a,basis)),Math.PI));}const q=[...cross(a,b),1+d];return scale(q,1/Math.max(length(q),1e-12));};
const copy=p=>({...p,rootPosition:p.rootPosition.slice(),localQuaternions:p.localQuaternions.map(q=>q.slice())});
const DT=1/60;

export function forward(pose,rig) {
  const positions=[],rotations=[];
  for(let i=0;i<rig.names.length;i++){
    const p=rig.parents[i],q=pose.localQuaternions[i];
    positions.push(p<0?pose.rootPosition.slice():add(positions[p],rotate(rig.offsets[i],rotations[p])));
    rotations.push(p<0?q:multiply(rotations[p],q));
  }
  return {positions,rotations};
}

export class ActionStabilizer {
  constructor(){this.previous=null;this.velocity=null;this.offset=null;this.offsetVelocity=null;this.age=0;}
  apply(raw,next,changed) {
    const p=copy(raw),rootLimit=.08,angleLimit=Math.PI/4;
    let saturated=false;
    if(changed&&this.previous){
      const incomingRoot=scale(sub(next.rootPosition,raw.rootPosition),1/DT);
      const previousVelocity=this.velocity||{root:[0,0,0],rot:raw.localQuaternions.map(()=>[0,0,0])};
      const requestedRoot=sub(add(this.previous.rootPosition,scale(previousVelocity.root,DT)),raw.rootPosition);
      const requestedRot=raw.localQuaternions.map((q,i)=>delta(multiply(exp(scale(previousVelocity.rot[i],DT)),this.previous.localQuaternions[i]),q));
      saturated=length(requestedRoot)>rootLimit||requestedRot.some(v=>length(v)>angleLimit);
      this.offset={root:bounded(requestedRoot,rootLimit),rot:requestedRot.map(v=>bounded(v,angleLimit))};
      this.offsetVelocity={root:sub(previousVelocity.root,incomingRoot),rot:raw.localQuaternions.map((q,i)=>sub(previousVelocity.rot[i],scale(delta(next.localQuaternions[i],q),1/DT)))};
      this.age=0;
    }
    if(this.offset){
      const omega=36,t=this.age,decay=Math.exp(-omega*t);
      const evolve=(v,velocity,limit)=>{
        const requested=v.map((x,i)=>(x+(velocity[i]+omega*x)*t)*decay);
        saturated ||= length(requested)>limit+1e-10;
        return bounded(requested,limit);
      };
      const root=evolve(this.offset.root,this.offsetVelocity.root,rootLimit);
      p.rootPosition=add(raw.rootPosition,root);
      p.localQuaternions=raw.localQuaternions.map((q,i)=>multiply(exp(evolve(this.offset.rot[i],this.offsetVelocity.rot[i],angleLimit)),q));
      this.age+=DT;if(this.age>.5)this.offset=null;
    }
    if(this.previous)this.velocity={root:scale(sub(p.rootPosition,this.previous.rootPosition),1/DT),rot:p.localQuaternions.map((q,i)=>scale(delta(q,this.previous.localQuaternions[i]),1/DT))};
    this.previous=copy(p);
    const correction={root_mm:length(sub(p.rootPosition,raw.rootPosition))*1000,maximum_joint_degrees:Math.max(...p.localQuaternions.map((q,i)=>length(delta(q,raw.localQuaternions[i]))*180/Math.PI)),saturated};
    return {pose:p,correction};
  }
}

/** Stationary-only plant. Limits apply to actual displacement and each joint. */
export function plantToes(raw,rig,anchors,weight=1,{maximum=.015,degrees=5}={}) {
  const p=copy(raw),details=[];
  for(const [side,chain] of [[18,19,20,22],[23,24,25,27]].entries()){
    const [hip,knee,ankle,toe]=chain,{positions:w,rotations:g}=forward(p,rig);
    const requested=sub(anchors[side],w[toe]),offset=scale(bounded(requested,maximum),weight);
    const a=w[hip],b=w[knee],c=w[ankle],target=add(c,offset),direction=unit(sub(target,a));
    const l1=length(sub(b,a)),l2=length(sub(c,b)),distance=clamp(length(sub(target,a)),Math.abs(l1-l2)+1e-6,l1+l2-1e-6);
    let pole=sub(sub(b,a),scale(direction,dot(sub(b,a),direction)));
    if(length(pole)<1e-8)pole=cross(direction,rotate([1,0,0],g[knee]));
    if(length(pole)<1e-8)pole=cross(direction,Math.abs(direction[0])<.8?[1,0,0]:[0,1,0]);
    const along=(l1*l1+distance*distance-l2*l2)/(2*distance);
    const kneeTarget=add(add(a,scale(direction,along)),scale(unit(pole),Math.sqrt(Math.max(0,l1*l1-along*along))));
    const original=copy(p),limit=degrees*Math.PI/180;
    p.localQuaternions[hip]=multiply(inverse(g[rig.parents[hip]]),multiply(between(sub(b,a),sub(kneeTarget,a)),g[hip]));
    let f=forward(p,rig);
    p.localQuaternions[knee]=multiply(inverse(f.rotations[hip]),multiply(between(sub(f.positions[ankle],f.positions[knee]),sub(add(a,scale(direction,distance)),f.positions[knee])),f.rotations[knee]));
    f=forward(p,rig);p.localQuaternions[ankle]=multiply(inverse(f.rotations[knee]),g[ankle]);
    for(const j of [hip,knee,ankle])p.localQuaternions[j]=multiply(exp(bounded(delta(p.localQuaternions[j],original.localQuaternions[j]),limit)),original.localQuaternions[j]);
    // Scaling the rotation deltas enforces the endpoint bound even near extension.
    const candidate=copy(p);let low=0,high=1;
    if(length(sub(forward(p,rig).positions[toe],w[toe]))>maximum+1e-7){
      for(let n=0;n<14;n++){const fraction=(low+high)/2;for(const j of [hip,knee,ankle])p.localQuaternions[j]=multiply(exp(scale(delta(candidate.localQuaternions[j],original.localQuaternions[j]),fraction)),original.localQuaternions[j]);if(length(sub(forward(p,rig).positions[toe],w[toe]))<=maximum)low=fraction;else high=fraction;}
      for(const j of [hip,knee,ankle])p.localQuaternions[j]=multiply(exp(scale(delta(candidate.localQuaternions[j],original.localQuaternions[j]),low)),original.localQuaternions[j]);
    }
    const actual=forward(p,rig).positions[toe];
    details.push({requested_mm:length(requested)*1000,actual_mm:length(sub(actual,w[toe]))*1000,residual_mm:length(sub(actual,anchors[side]))*1000,maximum_joint_degrees:Math.max(...[hip,knee,ankle].map(j=>length(delta(p.localQuaternions[j],original.localQuaternions[j]))*180/Math.PI)),saturated:length(requested)>maximum});
  }
  return {pose:p,details};
}