import { decodeMotionPose } from './controller.js';
import { worldPositions } from './pose.js';
import { ActionStabilizer,plantToes } from './action-stabilizer.js';

const DT=1/60,ANTICIPATION=.24,EXTENSION=.50,LANDING=.55;
const clamp=(x,a,b)=>Math.max(a,Math.min(b,x));
const vector=(x,n)=>(Array.isArray(x)||ArrayBuffer.isView(x))&&x.length===n&&Array.from(x).every(Number.isFinite);

/** Stationary action pose controller. The host owns gravity and collisions.
 * Features require superhq-motion-actions-v2 weights. No custom skills or clips
 * are loaded by this runtime. MotionGameController composes these canonical
 * poses with locomotion and horizontal game physics.
 */
export class MotionActions {
  constructor(network,rig,{contract,phase=0,runtimeCorrections=true}={}) {
    if(contract!=='superhq-motion-actions-v2'||!Number.isFinite(phase))throw Error('Expected v2 action weights.');
    this.network=network;this.rig=rig;this.runtimeCorrections=runtimeCorrections;this.reset({phase});
  }
  reset({phase=0}={}) {
    if(!Number.isFinite(phase))throw Error('Invalid phase.');
    this.phase=((phase%1)+1)%1;this.mode='stance';this.elapsed=0;this.airElapsed=0;
    this.depth=0;this.depthVelocity=0;this.goal=0;this.response=.65;
    this.wasGrounded=true;this.lastVerticalVelocity=0;this.takeoffSent=false;this.pendingJump=false;
    this.stabilizer=new ActionStabilizer();this.plantWeight=1;this.previousMode=null;this.goalChanged=false;
    const x=this.features();x[34]=0;
    const pose=decodeMotionPose(this.network.forward(x),this.rig);
    const world=worldPositions(pose,this.rig);
    this.anchors=[world[22].slice(),world[27].slice()];this.jumpAnchorMean=this.anchors[0].map((v,i)=>(v+this.anchors[1][i])/2);
  }
  setGoal({crouchDepth,responseSeconds=.65}) {
    if(!Number.isFinite(crouchDepth)||crouchDepth<0||crouchDepth>1||
        !Number.isFinite(responseSeconds)||responseSeconds<.5||responseSeconds>.9)throw Error('Invalid crouch goal.');
    this.goalChanged ||= crouchDepth!==this.goal||responseSeconds!==this.response;
    this.goal=crouchDepth;this.response=responseSeconds;
  }
  requestJump({horizontalSpeed=0}={}) {
    if(!Number.isFinite(horizontalSpeed)||horizontalSpeed<0)throw Error('Invalid movement speed.');
    if(this.mode!=='stance'||!this.wasGrounded||this.depth>.05||this.goal>.05||horizontalSpeed>.05||this.pendingJump)return false;
    this.pendingJump=true;return true;
  }
  features() {
    const x=new Float32Array(48);x[3]=1;for(const j of [9,15,21])x[j]=1;
    x.set([Math.sin(this.phase*2*Math.PI),Math.cos(this.phase*2*Math.PI),.4],22);x[25]=1;x[26]=1;x[41]=1;
    return x;
  }
  step({position=[0,0,0],verticalVelocity=0,grounded=true}={}) {
    if(!vector(position,3)||!Number.isFinite(verticalVelocity)||typeof grounded!=='boolean')throw Error('Invalid host physics state.');
    if(this.pendingJump){this.mode='anticipation';this.elapsed=0;this.takeoffSent=false;this.pendingJump=false;}
    if(!grounded&&this.wasGrounded) {
      this.mode=this.mode==='anticipation'?'extension':'air';this.airElapsed=0;this.elapsed=0;
    }
    if(grounded&&!this.wasGrounded) {
      this.mode='landing';this.elapsed=0;this.landingEntry=clamp(this.airElapsed/EXTENSION,0,1);this.impactVelocity=this.lastVerticalVelocity;
    }
    if(this.mode==='extension'&&this.airElapsed>=EXTENSION)this.mode='air';
    if(this.mode==='landing'&&this.elapsed>=LANDING){this.mode='stance';this.depth=0;this.depthVelocity=0;}
    const x=this.features();let takeoffRequested=false;
    if(this.mode==='stance') {
      const omega=3.89/this.response,error=this.depth-this.goal,cross=this.depthVelocity+omega*error,decay=Math.exp(-omega*DT);
      this.depth=this.goal+(error+cross*DT)*decay;this.depthVelocity=(this.depthVelocity-omega*cross*DT)*decay;
      x[30]=this.depth;x[34]=1;x[38]=this.depthVelocity;x[39]=this.depth;x.set(this.anchors.flat(),42);
    } else {
      x.set(this.jumpAnchorMean,45);
      if(this.mode==='anticipation') {
        x[35]=1;x[38]=Math.min(this.elapsed,ANTICIPATION)/(ANTICIPATION+EXTENSION);
        if(this.elapsed>=ANTICIPATION&&!this.takeoffSent){takeoffRequested=true;this.takeoffSent=true;}
      } else if(this.mode==='extension') {
        x[35]=1;x[38]=(ANTICIPATION+this.airElapsed)/(ANTICIPATION+EXTENSION);
      } else if(this.mode==='air') {
        x[36]=1;x[38]=clamp((this.airElapsed-EXTENSION)/.25,0,1);
      } else {
        x[37]=1;x[38]=clamp(this.elapsed/LANDING,0,1);x[42]=this.landingEntry;x[43]=.38;x[44]=this.impactVelocity;
      }
    }
    x[40]=verticalVelocity;x[41]=Number(grounded);
    const output=this.network.forward(x),raw=decodeMotionPose(output,this.rig);
    let canonical=raw,correction={transition:null,feet:[]};
    if(this.runtimeCorrections){
      const changed=this.previousMode!==null&&(this.previousMode!==this.mode||this.wasGrounded!==grounded||this.goalChanged);
      let next=raw;
      if(changed){
        const preview=x.slice(),phase=this.phase+.4*DT;preview[22]=Math.sin(phase*2*Math.PI);preview[23]=Math.cos(phase*2*Math.PI);
        if(this.mode==='stance'){
          const omega=3.89/this.response,error=this.depth-this.goal,cross=this.depthVelocity+omega*error,decay=Math.exp(-omega*DT);
          preview[30]=preview[39]=this.goal+(error+cross*DT)*decay;preview[38]=(this.depthVelocity-omega*cross*DT)*decay;
        }else if(this.mode==='anticipation'||this.mode==='extension')preview[38]=Math.min(1,x[38]+DT/(ANTICIPATION+EXTENSION));
        else if(this.mode==='air')preview[38]=Math.min(1,x[38]+DT/.25);
        else if(this.mode==='landing')preview[38]=Math.min(1,x[38]+DT/LANDING);
        next=decodeMotionPose(this.network.forward(preview),this.rig);
      }
      const filtered=this.stabilizer.apply(raw,next,changed);canonical=filtered.pose;correction.transition=filtered.correction;
      this.plantWeight+=(Number(this.mode==='stance'&&grounded)-this.plantWeight)*(1-Math.exp(-DT/.08));
      if(!grounded)this.plantWeight=0;
      if(this.plantWeight>.001){const planted=plantToes(canonical,this.rig,this.anchors,this.plantWeight);canonical=planted.pose;correction.feet=planted.details;}
    }
    const pose={...canonical,rootPosition:canonical.rootPosition.map((v,i)=>v+position[i])};
    const rawPose={...raw,rootPosition:raw.rootPosition.map((v,i)=>v+position[i])};
    const result={...pose,rawPose,correction,input:x,mode:this.mode,takeoffRequested,depth:this.depth,phase:this.phase,hostPosition:Array.from(position)};
    this.phase=(this.phase+.4*DT)%1;this.elapsed+=DT;if(!grounded)this.airElapsed+=DT;
    this.previousMode=this.mode;this.goalChanged=false;this.wasGrounded=grounded;this.lastVerticalVelocity=verticalVelocity;
    return result;
  }
}

/** Example host physics only. Games should supply their own collision state. */
export class TestPhysics {
  constructor({impulse=3.5,gravity=9.81,platformHeight=0}={}) {
    if(![impulse,gravity,platformHeight].every(Number.isFinite)||impulse<=0||gravity<=0||platformHeight<0)throw Error('Invalid test physics.');
    this.impulse=impulse;this.gravity=gravity;this.initialPlatform=platformHeight;this.reset();
  }
  reset(){this.platformHeight=this.initialPlatform;this.position=[0,0,0];this.velocity=0;this.grounded=true;this.takeoffs=0;this.landings=0;}
  state(){return {position:this.position.slice(),verticalVelocity:this.velocity,grounded:this.grounded};}
  /** Stand the host on a surface at height (used after a ledge climb). */
  place(height){if(!Number.isFinite(height)||height<0)throw Error('Invalid ground height.');this.position=[0,height,0];this.velocity=0;this.grounded=true;this.platformHeight=height;}
  launch(){if(!this.grounded)return false;this.velocity=this.impulse;this.grounded=false;this.takeoffs++;return true;}
  step(){if(this.grounded)return;const previous=this.position[1];this.position[1]+=this.velocity*DT-.5*this.gravity*DT*DT;this.velocity-=this.gravity*DT;
    const floor=this.velocity<0&&previous>=this.platformHeight?this.platformHeight:0;
    if(this.position[1]<=floor&&this.velocity<0){this.position[1]=floor;this.velocity=0;this.grounded=true;this.landings++;}}
}