const clamp=(v,a,b)=>Math.max(a,Math.min(b,v));
const lerp=(a,b,u)=>a+(b-a)*u;
export function slerp(a,b,u) {
  let dot=a.reduce((s,v,i)=>s+v*b[i],0);
  if(dot<0){b=b.map(v=>-v);dot=-dot;}
  dot=clamp(dot,-1,1);
  const angle=Math.acos(dot),s=Math.sin(angle);
  const q=dot>.9995?a.map((v,i)=>lerp(v,b[i],u)):
    a.map((v,i)=>(Math.sin((1-u)*angle)*v+Math.sin(u*angle)*b[i])/s);
  const length=Math.hypot(...q);return q.map(v=>v/length);
}

export function blendPoses(a,b,u) {
  return {rootPosition:a.rootPosition.map((v,i)=>lerp(v,b.rootPosition[i],u)),
    localQuaternions:a.localQuaternions.map((q,i)=>slerp(q,b.localQuaternions[i],u)),
    contacts:a.contacts.map((v,i)=>lerp(v,b.contacts[i],u))};
}

/** Forward kinematics, shared by the raw experiment and its metrics. */
export function worldPositions(pose,rig) {
  const mul=(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]-a[0]*b[0]-a[1]*b[1]-a[2]*b[2]];
  const rotate=(v,q)=>mul(mul(q,[...v,0]),[-q[0],-q[1],-q[2],q[3]]).slice(0,3);
  const positions=[],rotations=[];
  for(let i=0;i<rig.names.length;i++) {
    const parent=rig.parents[i],q=pose.localQuaternions[i];
    if(parent<0){positions.push(pose.rootPosition.slice());rotations.push(q);}
    else{const o=rotate(rig.offsets[i],rotations[parent]);positions.push(o.map((v,j)=>v+positions[parent][j]));rotations.push(mul(rotations[parent],q));}
  }
  return positions;
}


/** The game checkpoints' canonical Head frame was derived from the neck-to-head
 * vector during source conversion, so a level source head appears pitched
 * 10.9 degrees forward (measured on the Quaternius A_TPose; see
 * docs/motion-one-shot-actions-and-character.md). Correct it before display.
 * Locomotion, actions and clips share the bias; poses are otherwise unchanged.
 */
export const HEAD_PITCH_BIAS_DEGREES=10.9;
export function correctHeadPitch(pose,degrees=HEAD_PITCH_BIAS_DEGREES) {
  const h=-degrees*Math.PI/360,r=[Math.sin(h),0,0,Math.cos(h)],q=pose.localQuaternions[6];
  const head=[q[3]*r[0]+q[0]*r[3]+q[1]*r[2]-q[2]*r[1],q[3]*r[1]-q[0]*r[2]+q[1]*r[3]+q[2]*r[0],
    q[3]*r[2]+q[0]*r[1]-q[1]*r[0]+q[2]*r[3],q[3]*r[3]-q[0]*r[0]-q[1]*r[1]-q[2]*r[2]];
  const localQuaternions=pose.localQuaternions.slice();localQuaternions[6]=head;
  return {...pose,localQuaternions};
}