/* Motion - joint-level control and end effectors. */

import {
  store, command, num, RAD2DEG, DEG2RAD,
} from '../core.js';
import {
  el, card, pageHead, button, note, segmented, range, table, badge, emptyState,
} from '../ui.js';
import { bars } from '../charts.js';

export default {
  id: 'motion',
  label: 'Motion',
  icon: 'motion',

  async render() {
    const spec = store.spec;
    const root = el('div.stack');

    root.appendChild(pageHead(
      'Motion',
      'Drive joints and end effectors directly. Limits shown are the published X2 '
      + 'Ultra ranges; the server clamps anything outside them before publishing.',
    ));

    /* ==================================================================
       Joint control
       ================================================================== */

    const groups = spec?.joint_groups || [];
    let activeGroup = groups[0]?.key || 'arm';
    let controlMode = 'position';

    const sliders = new Map();     // joint name -> range control
    const targets = new Map();     // joint name -> radians
    const editorHost = el('div');
    const stateHost = el('div');

    const stiffness = range({ min: 0, max: 200, step: 1, value: 60, precision: 0, unit: ' N·m/rad' });
    const damping = range({ min: 0, max: 20, step: 0.5, value: 3, precision: 1, unit: ' N·m·s/rad' });

    function buildEditor() {
      const group = groups.find((g) => g.key === activeGroup);
      if (!group) { editorHost.replaceChildren(emptyState('Unknown joint group')); return; }

      sliders.clear();
      const rows = el('div.stack', { style: { gap: '10px' } });

      for (const joint of group.joints) {
        const fixed = joint.min_deg === joint.max_deg;
        const current = targets.get(joint.name) ?? 0;

        const control = range({
          min: joint.min_deg, max: joint.max_deg,
          step: fixed ? 1 : 0.5,
          value: current * RAD2DEG,
          precision: 1, unit: '°',
          onInput: (deg) => targets.set(joint.name, deg * DEG2RAD),
        });
        if (fixed) {
          control.querySelector('input').disabled = true;
          control.style.opacity = '.5';
        }
        sliders.set(joint.name, control);

        rows.appendChild(el('div', {},
          el('div.row.between', { style: { marginBottom: '3px' } },
            el('span', { text: joint.label, style: { fontSize: '12px', fontWeight: '550' } }),
            el('span', {
              text: fixed ? 'fixed' : `${joint.min_deg}° … ${joint.max_deg}°`,
              style: { fontSize: '10.5px', color: 'var(--text-3)', fontFamily: 'var(--mono)' },
            }),
          ),
          control,
        ));
      }

      editorHost.replaceChildren(rows);
    }

    async function sendTargets() {
      const group = groups.find((g) => g.key === activeGroup);
      if (!group) return;
      const payload = {};
      for (const joint of group.joints) {
        if (joint.min_deg === joint.max_deg) continue;
        payload[joint.name] = targets.get(joint.name) ?? 0;
      }
      await command('/api/joints', {
        group: activeGroup,
        mode: controlMode,
        targets: payload,
        stiffness: stiffness.getValue(),
        damping: damping.getValue(),
      }, { successTitle: `${group.label} command sent` });
    }

    function zeroTargets() {
      const group = groups.find((g) => g.key === activeGroup);
      for (const joint of group?.joints || []) {
        targets.set(joint.name, 0);
        sliders.get(joint.name)?.setValue(0);
      }
    }

    function syncFromRobot() {
      const live = store.state?.joints?.[activeGroup] || [];
      for (const joint of live) {
        targets.set(joint.name, joint.position);
        sliders.get(joint.name)?.setValue(joint.position * RAD2DEG);
      }
    }

    const groupTabs = segmented(
      groups.map((g) => ({ value: g.key, label: g.label })),
      activeGroup,
      (value) => { activeGroup = value; buildEditor(); paintState(); },
    );

    const modeTabs = segmented(
      (spec?.joint_modes || []).map((m) => ({ value: m.id, label: m.label })),
      controlMode,
      (value) => { controlMode = value; },
    );

    buildEditor();

    root.appendChild(el('div.grid.pair', {},
      card('Joint targets', { sub: 'JointCommandArray', actions: [groupTabs] },
        el('div.stack', {},
          el('div.row.between', {},
            el('span', { text: 'Control mode', style: { fontSize: '12px', color: 'var(--text-2)' } }),
            modeTabs,
          ),
          editorHost,
          el('div.grid.cols-2', { style: { gap: '10px' } },
            el('div', {},
              el('div', { text: 'Stiffness', style: { fontSize: '12px', color: 'var(--text-2)', marginBottom: '4px' } }),
              stiffness,
            ),
            el('div', {},
              el('div', { text: 'Damping', style: { fontSize: '12px', color: 'var(--text-2)', marginBottom: '4px' } }),
              damping,
            ),
          ),
          el('div.row', {},
            button('Send targets', sendTargets, { style: 'primary' }),
            button('Sync from robot', syncFromRobot),
            button('Zero all', zeroTargets, { style: 'ghost' }),
          ),
          note('Head pitch has a published range of 0° on the X2 Ultra — the axis exists in the '
             + 'message but is not articulated, so its slider is disabled.', 'info'),
        ),
      ),
      card('Measured joint state', { sub: 'JointStateArray' }, stateHost),
    ));

    /* ==================================================================
       End effectors
       ================================================================== */

    let handSide = 'right';
    const handSliders = [];
    const handHost = el('div');
    const handStateHost = el('div');

    function buildHand() {
      const joints = spec?.dexhand_joints || [];
      handSliders.length = 0;
      const rows = el('div.stack', { style: { gap: '8px' } });

      joints.forEach((name, index) => {
        const control = range({ min: 0, max: 1.6, step: 0.02, value: 0, precision: 2, unit: ' rad' });
        handSliders.push(control);
        rows.appendChild(el('div', {},
          el('div', { text: name, style: { fontSize: '11.5px', color: 'var(--text-2)', marginBottom: '2px' } }),
          control,
        ));
      });

      handHost.replaceChildren(rows);
    }

    async function sendHand() {
      await command('/api/hand', {
        side: handSide,
        positions: handSliders.map((s) => s.getValue()),
      }, { successTitle: `${handSide} hand` });
    }

    async function applyHandPreset(preset) {
      preset.positions.forEach((value, index) => handSliders[index]?.setValue(value));
      await command('/api/hand', { side: handSide, preset: preset.key },
                    { successTitle: `${preset.label} · ${handSide}` });
    }

    buildHand();

    const handSendBtn = button('Send hand pose', sendHand, { style: 'primary' });

    const presetRow = el('div.row.tight', {},
      ...(spec?.hand_presets || []).map((preset) =>
        button(preset.label, () => applyHandPreset(preset), { size: 'sm' })),
    );

    const sideTabs = segmented(
      [{ value: 'left', label: 'Left' }, { value: 'right', label: 'Right' }],
      handSide,
      (value) => { handSide = value; paintState(); },
    );

    root.appendChild(el('div.grid.pair', {},
      card('Dexterous hand', { sub: 'HandCommandArray · OmniHand', actions: [sideTabs] },
        el('div.stack', {},
          presetRow,
          handHost,
          handSendBtn,
          note('Disable native motor control first — run "aima em stop-app mc" on the robot — or the '
             + 'built-in controller and these commands will fight each other.', 'warning', '⚠'),
        ),
      ),
      card('Hand state', { sub: 'HandStateArray' }, handStateHost),
    ));

    /* ==================================================================
       Live state
       ================================================================== */

    function paintState() {
      const s = store.state;
      if (!s) return;

      const group = groups.find((g) => g.key === activeGroup);
      const live = s.joints?.[activeGroup] || [];
      const limits = Object.fromEntries((group?.joints || []).map((j) => [j.name, j]));

      if (!live.length) {
        stateHost.replaceChildren(emptyState(
          'No joint state yet',
          `Nothing has been received on ${group?.state_topic || 'the joint state topic'}.`,
        ));
      } else {
        const rows = live.map((joint) => {
          const limit = limits[joint.name] || { min_deg: -180, max_deg: 180 };
          const deg = joint.position * RAD2DEG;
          const range_ = (limit.max_deg - limit.min_deg) || 1;
          const headroom = Math.min(deg - limit.min_deg, limit.max_deg - deg) / range_;
          return {
            label: joint.label || joint.name,
            value: deg,
            min: limit.min_deg,
            max: limit.max_deg,
            colorIndex: 0,
            tone: joint.error ? 'critical' : headroom < 0.03 ? 'warning' : 'default',
          };
        });

        stateHost.replaceChildren(
          bars(rows, { precision: 1, unit: '°' }),
          el('div', { style: { marginTop: '14px' } },
            table([
              { key: 'name', label: 'Joint', get: (r) => r.label || r.name },
              { key: 'pos', label: 'Position', align: 'right', get: (r) => `${num(r.position * RAD2DEG, 1)}°` },
              { key: 'vel', label: 'Velocity', align: 'right', get: (r) => num(r.velocity, 3) },
              { key: 'eff', label: 'Effort', align: 'right', get: (r) => num(r.effort, 2) },
              {
                key: 'err', label: 'Fault',
                get: (r) => (r.error ? badge(`code ${r.error}`, 'critical') : badge('ok', 'good')),
              },
            ], live),
          ),
        );
      }

      const hand = s.hand_state?.[handSide] || [];
      const attached = (s.hand_type || 'None') !== 'None';

      handStateHost.replaceChildren(
        el('div.row', { style: { marginBottom: '10px' } },
          badge(s.hand_type || 'unknown', attached ? 'accent' : 'warning'),
          badge(`${hand.length} joints reported`),
        ),
        hand.length
          ? bars(hand.map((joint) => ({
              label: joint.name, value: joint.position, min: 0, max: 1.6, colorIndex: 2,
            })), { precision: 2, unit: ' rad', diverging: false })
          : emptyState(
              attached ? 'No hand state yet' : 'No hand hardware attached',
              attached
                ? 'Nothing received on /aima/hal/joint/hand/state.'
                : 'GetHandType reports NONE for both sides on this robot, and the state message '
                  + 'carries no joints. Attach an OmniHand or OmniPicker to use this panel.',
            ),
      );

      // Sending hand commands with no hand attached would silently do nothing.
      for (const control of handSliders) {
        control.querySelector('input').disabled = !attached;
      }
      handSendBtn.disabled = !attached;
      for (const btn of presetRow.children) btn.disabled = !attached;
    }

    paintState();

    let last = 0;
    const unsubscribe = store.on('state', () => {
      const now = Date.now();
      if (now - last < 600) return;
      last = now;
      paintState();
    });

    return {
      node: root,
      dispose: () => {
        unsubscribe();
      },
    };
  },
};