// Pose maths in radians and metres (the daemon's flat matrix translation units).
export const RAD = Math.PI / 180;
export const clamp = (n: number, a: number, b: number) => Math.max(a, Math.min(b, n));
export const deadzone = (n: number) => Math.abs(n) <= .15 ? 0 : Math.sign(n) * (Math.abs(n) - .15) / .85;
export interface Input { axes: number[]; triggers: number[]; shoulders: boolean[]; turbo?: boolean }
export const idle = (): Input => ({ axes: [0, 0, 0, 0], triggers: [0, 0], shoulders: [false, false] });
export const neutral = (input: Input) => input.axes.every(n => Math.abs(n) <= .15) &&
  input.triggers.every(n => n <= .15) && !input.shoulders.some(Boolean);
export interface Snapshot { head?: number[]; antennas?: number[]; antennas_joint_positions?: number[]; body_yaw?: number; head_joint_positions?: number[] }
export interface Target { head: number[]; antennas: number[]; body_yaw: number }

export function matrix(roll: number, pitch: number, yaw: number, x: number, y: number, z: number): number[] {
  const cr = Math.cos(roll), sr = Math.sin(roll), cp = Math.cos(pitch), sp = Math.sin(pitch), cy = Math.cos(yaw), sy = Math.sin(yaw);
  return [cy*cp, cy*sp*sr-sy*cr, cy*sp*cr+sy*sr, x,
    sy*cp, sy*sp*sr+cy*cr, sy*sp*cr-cy*sr, y, -sp, cp*sr, cp*cr, z, 0, 0, 0, 1];
}

export class Motion {
  target: number[];
  current: number[];
  roll: number;
  x: number;
  y: number;
  constructor(state: Snapshot) {
    const h = state.head, ant = state.antennas_joint_positions ?? state.antennas;
    const body = state.body_yaw ?? state.head_joint_positions?.[0];
    if (!h || h.length !== 16 || !h.every(Number.isFinite) || !ant || ant.length !== 2 || !ant.every(Number.isFinite) || !Number.isFinite(body)) {
      throw new Error('Waiting for complete head, body and antenna telemetry.');
    }
    this.roll = Math.atan2(h[9], h[10]); this.x = h[3]; this.y = h[7];
    this.current = [Math.atan2(h[4], h[0]) - body!, Math.asin(clamp(-h[8], -1, 1)), h[11], body!, ...ant];
    this.target = [...this.current];
  }
  step(input: Input, seconds: number, turbo: boolean): Target {
    if (input.axes.length !== 4 || input.triggers.length !== 2 || input.shoulders.length !== 2 ||
      ![...input.axes, ...input.triggers, seconds].every(Number.isFinite)) throw new Error('Invalid control input');
    const dt = clamp(seconds, 0, .05), multiplier = turbo ? 2 : 1;
    let yaw = deadzone(input.axes[0]), pitch = deadzone(input.axes[1]);
    if (Math.abs(yaw) > Math.abs(pitch) * 2) pitch = 0;
    else if (Math.abs(pitch) > Math.abs(yaw) * 2) yaw = 0;
    const rates = [-yaw*75*RAD, pitch*60*RAD, -deadzone(input.axes[3])*.025,
      -deadzone(input.axes[2])*60*RAD,
      (Number(input.shoulders[1])-deadzone(input.triggers[1]))*120*RAD,
      (deadzone(input.triggers[0])-Number(input.shoulders[0]))*120*RAD];
    this.target = this.target.map((v, i) => v + rates[i]*dt*multiplier);
    this.target[2] = clamp(this.target[2], -.020, .030);
    const factor = 1 - .7*Math.abs(this.target[2])/(this.target[2] > 0 ? .030 : .020);
    const limits = [45*RAD*factor, 20*RAD*factor, 0, 90*RAD, 60*RAD, 60*RAD];
    for (const i of [0, 1, 3, 4, 5]) this.target[i] = clamp(this.target[i], -limits[i], limits[i]);
    const alpha = 1-Math.exp(-dt/(turbo ? .045 : .08));
    const maxRates = [90*RAD, 60*RAD, .025, 60*RAD, 120*RAD, 120*RAD];
    this.current = this.current.map((v, i) => v + clamp((this.target[i]-v)*alpha, -maxRates[i]*dt*multiplier, maxRates[i]*dt*multiplier));
    const [neck, p, z, body, right, left] = this.current;
    return {head: matrix(this.roll, p, body+neck, this.x, this.y, z), antennas: [right, left], body_yaw: body};
  }
}

export function readPad(pad: Pick<Gamepad, 'mapping' | 'axes' | 'buttons' | 'connected'>): Input {
  if (!pad.connected || pad.mapping !== 'standard' || pad.axes.length < 4 || pad.buttons.length < 14) {
    throw new Error('Use a standard-mapped Xbox/PlayStation gamepad, or choose Touch controls.');
  }
  const axis = (i: number) => clamp(pad.axes[i], -1, 1);
  const pressed = (i: number) => pad.buttons[i].pressed;
  return { axes: [0,1,2,3].map(axis), triggers: [6,7].map(i => clamp(pad.buttons[i].value, 0, 1)),
    shoulders: [pressed(4), pressed(5)], turbo: pressed(12) ? true : pressed(13) ? false : undefined };
}

export class Gate {
  active = false;
  reason = 'Paused — choose controls, then press Start.';
  arm(input: Input, now: number, lastState: number, connected: boolean) {
    if (!connected || now - lastState > 2500) throw new Error('Waiting for a fresh robot connection.');
    if (!neutral(input)) throw new Error('Center both sticks and release triggers/shoulders before starting.');
    this.active = true; this.reason = 'Live';
  }
  pause(reason: string) { this.active = false; this.reason = reason; }
}