import test from 'node:test';
import assert from 'node:assert/strict';
import { Gate, Motion, RAD, deadzone, idle, matrix, readPad, type Snapshot } from './control.js';
const state = (): Snapshot => ({head: matrix(0,0,0,0,0,0), antennas: [-.17,.17], body_yaw: 0});

test('first command preserves measured pose and SDK antenna order', () => {
  const snapshot = {head: matrix(.1,.1,.5,.002,.003,.008), antennas: [-.3,.4], body_yaw: .3};
  const result = new Motion(snapshot).step(idle(),0,false);
  result.head.forEach((n,i) => assert.ok(Math.abs(n-snapshot.head[i]) < 1e-10));
  assert.deepEqual(result.antennas, [-.3,.4]); assert.equal(result.body_yaw,.3);
});
test('incomplete/non-finite telemetry cannot arm a motion model', () => {
  assert.throws(() => new Motion({head: [0]}));
  assert.throws(() => new Motion({...state(), body_yaw: NaN}));
});
test('elapsed-time target integration is frame-rate independent', () => {
  const results = [30,60,120].map(hz => {
    const motion = new Motion(state()); const input = idle(); input.axes[0] = .4;
    for(let i=0;i<hz;i++) motion.step(input,1/hz,false);
    return motion.target[0];
  });
  results.forEach(n => assert.ok(Math.abs(n-results[0]) < 1e-9));
});
test('head follows body while keeping relative look yaw', () => {
  const motion = new Motion(state()), input = idle(); input.axes[2] = 1;
  const result = motion.step(input,.02,false);
  assert.ok(result.body_yaw < 0);
  assert.ok(Math.abs(Math.atan2(result.head[4],result.head[0])-result.body_yaw) < 1e-9);
});
test('pitch direction matches Pollen: stick up means negative pitch', () => {
  const motion = new Motion(state()), input = idle(); input.axes[1] = -1;
  motion.step(input,.02,false); assert.ok(motion.target[1] < 0);
});
test('left and right antennas remain independently expressive', () => {
  const motion = new Motion(state()), input = idle(); input.triggers[0] = 1;
  motion.step(input,.02,false);
  assert.equal(motion.target[4],-.17); assert.ok(motion.target[5] > .17);
  input.triggers[0] = 0; input.shoulders[1] = true;
  motion.step(input,.02,false); assert.ok(motion.target[4] > -.17);
});
test('Turbo doubles antenna integration without enlarging limits', () => {
  const a = new Motion(state()), b = new Motion(state()), input = idle(); input.triggers[0] = 1;
  a.step(input,.02,false); b.step(input,.02,true);
  assert.ok(Math.abs((b.target[5]-.17)-2*(a.target[5]-.17)) < 1e-9);
  for(let i=0;i<2000;i++) b.step(input,.02,true);
  assert.ok(b.current[5] <= 60*RAD);
});
test('stalls are capped and all channels remain bounded', () => {
  const motion = new Motion(state()), input = idle(); input.axes = [1,1,1,1]; input.triggers = [1,1];
  motion.step(input,100,false); assert.ok(Math.abs(motion.target[3]) <= 60*RAD*.05+1e-9);
  for(let i=0;i<2000;i++) motion.step(input,.02,true);
  assert.ok(Math.abs(motion.current[0]) < 65*RAD);
  assert.ok(Math.abs(motion.current[3]) <= 90*RAD);
  assert.ok(motion.current[2] >= -.02 && motion.current[2] <= .03);
});
test('deadzone is continuous and prevents trigger drift', () => {
  assert.equal(deadzone(.1),0); assert.ok(deadzone(.150001) < .00001);
  const motion = new Motion(state()), input = idle(); input.triggers = [.05,.05];
  motion.step(input,.02,false); assert.deepEqual(motion.target.slice(4),[-.17,.17]);
});
test('arm requires fresh telemetry, connection, and centered controls', () => {
  const gate = new Gate();
  assert.throws(() => gate.arm(idle(),4000,0,true));
  assert.throws(() => gate.arm(idle(),100,100,false));
  const held = idle(); held.triggers[1] = 1;
  assert.throws(() => gate.arm(held,100,100,true)); assert.equal(gate.active,false);
  gate.arm(idle(),100,100,true); assert.equal(gate.active,true);
  gate.pause('Controller lost'); assert.equal(gate.active,false);
  assert.equal(gate.reason,'Controller lost');
});
test('standard Gamepad mapping retains sticks, antennas and D-pad', () => {
  const buttons = Array.from({length: 16}, () => ({pressed:false,value:0,touched:false}));
  buttons[6] = {pressed:true,value:.75,touched:true}; buttons[12].pressed = true;
  const input = readPad({mapping:'standard',connected:true,axes:[.2,-.3,.4,-.5],buttons});
  assert.deepEqual(input.axes,[.2,-.3,.4,-.5]); assert.deepEqual(input.triggers,[.75,0]); assert.equal(input.turbo,true);
  assert.throws(() => readPad({mapping:'',connected:true,axes:[0,0,0,0],buttons}));
});
test('invalid samples cannot become robot targets', () => {
  const input = idle(); input.axes[0] = NaN;
  assert.throws(() => new Motion(state()).step(input,.02,false));
});