import { connectToHost } from '@pollen-robotics/reachy-mini-sdk/host/embed';
import { safelyReturnToPose } from '@pollen-robotics/reachy-mini-sdk/animation';
import { Gate, Motion, idle, readPad, type Input } from './control';
import './style.css';

async function main() {
  const handle = await connectToHost();
  const { reachy } = handle;
  // External configs are deliberately unused: they cannot override motion limits.
  document.documentElement.dataset.theme = handle.theme;
  const offTheme = handle.onThemeChange(theme => { document.documentElement.dataset.theme = theme; });
  document.querySelector<HTMLDivElement>('#root')!.innerHTML = `
    <main>
      <header><div class="eyebrow">REACHY MINI</div><h1>Joystick control</h1>
        <p>1. Choose your controller. 2. Check the input indicators. 3. Center the sticks and start.</p></header>
      <section class="session" aria-label="Session controls">
        <div class="status"><span id="signal" class="dot"></span><span id="phase" role="status">Waiting for telemetry</span></div>
        <div class="session-actions"><button id="start" class="primary">Start controls</button><button id="stop">Stop & center</button></div>
        <div class="options"><label>Input<select id="source"><option value="gamepad">Gamepad</option><option value="touch">Touch / mouse</option></select></label>
          <label class="mode"><input id="turbo" type="checkbox"> Turbo <small>2× speed</small></label></div>
        <p id="pad-name" class="hint">Pair your controller with this device, then press any controller button.</p>
      </section>
      <section class="controller-check" aria-label="Controller input test">
        <label>Controller<select id="controller"><option value="">Press a controller button to detect it</option></select></label>
        <p id="input-status" role="status">Move a stick to test detection. Testing while paused does not move the robot.</p>
        <div class="input-values"><span>Left stick <output id="left-input">0.00 / 0.00</output></span><span>Right stick <output id="right-input">0.00 / 0.00</output></span><span>Triggers L / R <output id="trigger-input">0% / 0%</output></span></div>
      </section>
      <div class="controls-grid">
        <section class="card"><div class="section-title"><span class="index">01</span><h2>Look</h2><span class="chip">LEFT STICK</span></div>
          <p>Head yaw & pitch, relative to the body.</p><div class="stick" id="look" role="group" aria-label="Head touch joystick"><span class="cross horizontal"></span><span class="cross vertical"></span><span class="puck"></span></div><div class="stick-labels"><span>← Look →</span><span>↑ Tilt ↓</span></div>
        </section>
        <section class="card"><div class="section-title"><span class="index">02</span><h2>Turn & stretch</h2><span class="chip">RIGHT STICK</span></div>
          <p>Body rotation & head height.</p><div class="stick" id="body" role="group" aria-label="Body and height touch joystick"><span class="cross horizontal"></span><span class="cross vertical"></span><span class="puck"></span></div><div class="stick-labels"><span>← Turn →</span><span>↑ Raise ↓</span></div>
        </section>
        <section class="card antennas"><div class="section-title"><span class="index">03</span><h2>Antennas</h2><span class="chip">TRIGGERS / SHOULDERS</span></div>
          <p>Hold to move. Release to keep the expression.</p>
          <div class="antenna-grid"><div><span class="antenna-label">LEFT</span><button data-hold="left-out">↖ Outward <small>LT / L2</small></button><button data-hold="left-in">↘ Inward <small>LB / L1</small></button></div>
          <div><span class="antenna-label">RIGHT</span><button data-hold="right-out">Outward ↗ <small>RT / R2</small></button><button data-hold="right-in">Inward ↙ <small>RB / R1</small></button></div></div>
          <div class="readouts"><div><small>HEIGHT</small><strong id="height">—</strong></div><div><small>LEFT ANTENNA</small><strong id="left-angle">—</strong></div><div><small>RIGHT ANTENNA</small><strong id="right-angle">—</strong></div></div>
        </section>
      </div>
      <footer><p><strong>Make a little room.</strong> Start with centered sticks and released triggers. D-pad ↑ / ↓ switches Turbo / Precision.</p>
      <p>Stop centers the robot; use the host’s Leave button to end the session. Changing tabs or losing the controller pauses controls. No camera or microphone is requested.</p><p class="hint">Readouts show commanded positions. Software limits are not collision sensors.</p></footer>
    </main>`;
  const $ = <T extends HTMLElement = HTMLElement>(id: string) => document.getElementById(id) as T;
  const source = $<HTMLSelectElement>('source'), turbo = $<HTMLInputElement>('turbo'), start = $<HTMLButtonElement>('start');
  const controllerSelect = $<HTMLSelectElement>('controller');
  if (matchMedia('(pointer: coarse)').matches) {
    source.value = 'touch';
    $('pad-name').textContent = 'Drag the circles; hold antenna buttons. Controls stay on this device.';
  }
  const gate = new Gate();
  let motion: Motion | null = null, lastState = -Infinity, lastTick = performance.now(), restUntil = 0;
  let leaving = false, selectedPad = -1, worker: Worker | null = null;
  let sourceChosen = false, padSignature = '', inputMessage = '', preview = idle();
  let touch = idle();
  const held = new Set<string>();
  const pointerIds = new Map<string, number>();
  const abort = new AbortController();
  const signal = abort.signal;
  function clearInput() {
    touch = idle(); held.clear(); pointerIds.clear();
    document.querySelectorAll<HTMLElement>('.puck').forEach(p => { p.style.transform = 'translate(0,0)'; });
    document.querySelectorAll('[data-hold]').forEach(b => b.classList.remove('held'));
  }
  const fresh = () => performance.now() - lastState < 2500;
  function stop(reason: string, returnHome = true, forceHome = false) {
    const wasActive = gate.active;
    gate.pause(reason); clearInput(); motion = null;
    if (returnHome && (wasActive || forceHome) && performance.now() >= restUntil && reachy.state === 'streaming' && fresh()) {
      try {
        const plan = safelyReturnToPose(reachy);
        restUntil = performance.now() + plan.duration * 1000 + 400;
      } catch (error) { gate.reason = `Paused; centering failed: ${String(error)}`; }
    }
  }
  function pads() {
    try { return Array.from(navigator.getGamepads?.() ?? []).filter((pad): pad is Gamepad => Boolean(pad?.connected)); }
    catch { throw new Error('Gamepad access is blocked here. Open the direct Space URL or choose Touch.'); }
  }
  function scanControllers() {
    const all = pads();
    const signature = all.map(p => `${p.index}:${p.id}:${p.mapping}`).join('|');
    if (signature !== padSignature) {
      padSignature = signature;
      controllerSelect.replaceChildren();
      const placeholder = new Option(all.length ? 'Choose your controller' : 'Press a controller button to detect it', '');
      controllerSelect.add(placeholder);
      for (const pad of all) {
        const option = new Option(`${pad.id}${pad.mapping === 'standard' ? '' : ' (unsupported mapping)'}`, String(pad.index));
        option.disabled = pad.mapping !== 'standard'; controllerSelect.add(option);
      }
      const supported = all.filter(p => p.mapping === 'standard');
      if (!gate.active && !all.some(p => p.index === selectedPad)) selectedPad = supported.length === 1 ? supported[0].index : -1;
      controllerSelect.value = selectedPad < 0 ? '' : String(selectedPad);
      // A paired controller takes priority over a touch-capable screen until the
      // user explicitly chooses an input. Otherwise a touchscreen laptop can
      // silently ignore a working physical gamepad in Touch mode.
      if (!sourceChosen && supported.length) source.value = 'gamepad';
    }
  }
  function getInput(): Input {
    if (source.value === 'gamepad') {
      const pad = pads().find(p => p.index === selectedPad);
      if (!pad) throw new Error(gate.active ? 'Controller disconnected — reconnect and press Start.' : 'No mapped gamepad. Press a controller button or choose Touch / mouse.');
      $('pad-name').textContent = pad.id;
      return readPad(pad);
    }
    return { ...touch, triggers: [Number(held.has('left-out')), Number(held.has('right-out'))],
      shoulders: [held.has('left-in'), held.has('right-in')] };
  }
  start.addEventListener('click', () => {
    try {
      if (!worker) throw new Error('Control clock unavailable — reload the app in a browser that supports Web Workers.');
      if (leaving || document.hidden || performance.now() < restUntil || reachy.robotState.is_move_running) {
        throw new Error('Wait for centering to finish, then press Start.');
      }
      const input = getInput();
      const next = new Motion(reachy.robotState);
      gate.arm(input, performance.now(), lastState, reachy.state === 'streaming');
      if (!reachy.setMotorMode('enabled')) throw new Error('Robot connection is unavailable.');
      motion = next; lastTick = performance.now();
    } catch (error) { gate.pause(String(error instanceof Error ? error.message : error)); }
  }, {signal});
  $('stop').addEventListener('click', () => stop('Paused — press Start after centering to resume.', true, true), {signal});
  source.addEventListener('change', () => {
    sourceChosen = true;
    stop('Input changed — press Start to resume.');
    $('pad-name').textContent = source.value === 'touch' ? 'Drag the circles; hold antenna buttons. Controls stay on this device.' : 'Press a gamepad button, then Start.';
  }, {signal});
  controllerSelect.addEventListener('change', () => {
    stop('Controller changed — test the sticks, then press Start.');
    selectedPad = controllerSelect.value === '' ? -1 : Number(controllerSelect.value);
    sourceChosen = true; source.value = 'gamepad';
  }, {signal});
  function makeStick(id: string, offset: number) {
    const el = $(id), puck = el.querySelector<HTMLElement>('.puck')!;
    const reset = () => { touch.axes[offset] = touch.axes[offset+1] = 0; puck.style.transform = 'translate(0,0)'; };
    const move = (event: PointerEvent) => {
      if (pointerIds.get(id) !== event.pointerId) return;
      const box = el.getBoundingClientRect(), radius = box.width * .34;
      let x = (event.clientX-box.left-box.width/2)/radius, y = (event.clientY-box.top-box.height/2)/radius;
      const magnitude = Math.max(1, Math.hypot(x,y)); x /= magnitude; y /= magnitude;
      touch.axes[offset] = x; touch.axes[offset+1] = y;
      puck.style.transform = `translate(${x*radius}px,${y*radius}px)`;
    };
    el.addEventListener('pointerdown', event => {
      if (source.value !== 'touch' || !gate.active || pointerIds.has(id)) return;
      event.preventDefault(); pointerIds.set(id, event.pointerId); el.setPointerCapture(event.pointerId); move(event);
    }, {signal});
    el.addEventListener('pointermove', move, {signal});
    el.addEventListener('pointerup', event => { if (pointerIds.get(id) === event.pointerId) { pointerIds.delete(id); reset(); } }, {signal});
    for (const name of ['pointercancel', 'lostpointercapture'] as const) el.addEventListener(name, () => {
      if (pointerIds.has(id)) { reset(); stop('Touch interrupted — press Start to resume.'); }
    }, {signal});
  }
  makeStick('look', 0); makeStick('body', 2);
  document.querySelectorAll<HTMLButtonElement>('[data-hold]').forEach(button => {
    const name = button.dataset.hold!;
    button.addEventListener('pointerdown', event => {
      if (source.value !== 'touch' || !gate.active) return;
      event.preventDefault(); held.add(name); button.classList.add('held'); button.setPointerCapture(event.pointerId);
    }, {signal});
    const release = () => { held.delete(name); button.classList.remove('held'); };
    button.addEventListener('pointerup', release, {signal});
    button.addEventListener('pointercancel', () => { release(); stop('Touch interrupted — press Start to resume.'); }, {signal});
    button.addEventListener('lostpointercapture', release, {signal});
    button.addEventListener('keydown', event => {
      if ((event.key === ' ' || event.key === 'Enter') && source.value === 'touch' && gate.active) {
        event.preventDefault(); held.add(name); button.classList.add('held');
      }
    }, {signal});
    button.addEventListener('keyup', release, {signal});
    button.addEventListener('blur', release, {signal});
  });
  const onState = () => { lastState = performance.now(); };
  reachy.addEventListener('state', onState);
  reachy.requestState();
  const lost = () => stop('Robot session ended — reconnect through the host.', false);
  for (const event of ['disconnected','sessionStopped','error'] as const) reachy.addEventListener(event, lost);
  document.addEventListener('visibilitychange', () => {
    if (document.hidden) stop('Tab hidden — press Start to resume.');
    else reachy.requestState();
  }, {signal});
  window.addEventListener('blur', () => stop('Window lost focus — press Start to resume.'), {signal});
  window.addEventListener('gamepaddisconnected', event => {
    if (source.value === 'gamepad' && event.gamepad.index === selectedPad) stop('Controller disconnected — reconnect and press Start.');
  }, {signal});
  try { worker = new Worker(new URL('./heartbeat.worker.ts', import.meta.url), {type: 'module'}); }
  catch { gate.pause('Control clock unavailable — this browser cannot start Web Workers.'); }
  if (worker) worker.onmessage = () => {
    const now = performance.now(), dt = (now-lastTick)/1000; lastTick = now;
    try {
      scanControllers();
      preview = getInput(); inputMessage = '';
      if (!gate.active || !motion) return;
      if (document.hidden || !fresh() || reachy.state !== 'streaming') { stop('Connection stale or tab hidden — press Start after reconnecting.', false); return; }
      if (dt > .25) { stop('Controls stalled — press Start to resume.'); return; }
      const input = preview;
      if (input.turbo !== undefined) turbo.checked = input.turbo;
      if (!reachy.setTarget(motion.step(input, dt, turbo.checked))) stop('Robot connection interrupted.', false);
    } catch (error) {
      preview = idle(); inputMessage = String(error instanceof Error ? error.message : error);
      if (gate.active) stop(inputMessage);
    }
  };
  if (worker) worker.onerror = () => {
    stop('Control clock failed — reload this app.'); worker?.terminate(); worker = null;
  };
  let frame = 0;
  function render() {
    $('phase').textContent = gate.reason;
    $('signal').classList.toggle('live', gate.active);
    start.disabled = gate.active || leaving || !worker || performance.now() < restUntil;
    start.textContent = performance.now() < restUntil ? 'Centering…' : 'Start controls';
    document.body.dataset.input = source.value;
    controllerSelect.disabled = gate.active;
    $('left-input').textContent = `${preview.axes[0].toFixed(2)} / ${preview.axes[1].toFixed(2)}`;
    $('right-input').textContent = `${preview.axes[2].toFixed(2)} / ${preview.axes[3].toFixed(2)}`;
    $('trigger-input').textContent = `${Math.round(preview.triggers[0]*100)}% / ${Math.round(preview.triggers[1]*100)}%`;
    const moving = preview.axes.some(n => Math.abs(n) > .15) || preview.triggers.some(n => n > .15) || preview.shoulders.some(Boolean);
    $('input-status').textContent = inputMessage || (source.value === 'touch'
      ? 'Touch mode: the physical gamepad is ignored. Choose Gamepad to use your controller.'
      : moving ? (gate.active ? 'Controller input received — sending movement commands.' : 'Controller input received. Release controls, then press Start.')
        : 'Controller connected. Move a stick or trigger to check the indicators below.');
    if (source.value === 'gamepad') {
      for (const [id, offset] of [['look',0],['body',2]] as const) {
        const el = $(id), puck = el.querySelector<HTMLElement>('.puck')!;
        const radius = el.clientWidth * .34;
        puck.style.transform = `translate(${preview.axes[offset]*radius}px,${preview.axes[offset+1]*radius}px)`;
      }
      for (const [name, active] of [['left-out',preview.triggers[0]>.15],['right-out',preview.triggers[1]>.15],['left-in',preview.shoulders[0]],['right-in',preview.shoulders[1]]] as const) {
        document.querySelector(`[data-hold="${name}"]`)?.classList.toggle('held',active);
      }
    }
    const values = motion?.current;
    $('height').textContent = values ? `${(values[2]*1000).toFixed(0)} mm` : '—';
    $('left-angle').textContent = values ? `${(values[5]*180/Math.PI).toFixed(0)}°` : '—';
    $('right-angle').textContent = values ? `${(values[4]*180/Math.PI).toFixed(0)}°` : '—';
    frame = requestAnimationFrame(render);
  }
  frame = requestAnimationFrame(render);
  handle.onLeave(() => {
    leaving = true; gate.pause('Leaving'); clearInput();
    worker?.terminate(); worker = null; cancelAnimationFrame(frame); abort.abort(); offTheme();
    reachy.removeEventListener('state', onState);
    for (const event of ['disconnected','sessionStopped','error'] as const) reachy.removeEventListener(event, lost);
    safelyReturnToPose(reachy);
  });
}

void main().catch(error => {
  console.error('[teleop] startup failed', error);
  const root = document.querySelector('#root');
  if (root) root.textContent = `Unable to open Teleop: ${String(error)}`;
  window.parent.postMessage({source:'reachy-mini',type:'embed:error',version:1,message:String(error),fatal:true}, location.origin);
});