/* Digital twin - the real X2 model, posed and shaded by live telemetry.

   The robot already reports all 31 revolute joint positions and efforts at
   100 Hz, and the dashboard already streams them. This tab draws them, which
   turns a table of numbers into something you can check against the machine in
   front of you: an arm folded in the picture but straight in the room means the
   encoder, the model, or your idea of which robot you are connected to is wrong.

   ON LOAD, AND WHY IT IS NOT TEMPERATURE
   --------------------------------------
   This robot does not publish joint temperature. aimdk_msgs/JointState carries
   name, position, velocity, effort and error_code and nothing else; the
   GetAllJointState service returns the same type; /diagnostics is the Orbbec
   camera's own internals; and across all 53 aimdk_msgs types the only
   temperature fields are pmu_temperature and battery_temperature. The
   norealtime DCU joint topics may carry more, but their message type is not
   installed anywhere on the robot, so nothing here can deserialise them.

   So this shades by *effort*, which is honest and nearly as useful: motor
   heating is I squared R, and current tracks torque, so the joint pulling
   hardest is the joint getting hottest. Every number on screen is measured.

   Load is shown as a percentage of each joint's RATED torque, taken from the
   URDF's <limit effort="..."> and baked into model.json. That matters: rated
   torque here runs from 0.6 Nm to 120 Nm, so an absolute Nm ramp would paint
   every wrist permanently cold and every leg permanently hot regardless of what
   the robot was doing.

   Geometry is baked offline by build_model.py - 112 MB of vendor STL down to
   1.4 MB. Nothing here parses a URDF at runtime.
*/

import { store, num } from '../core.js';
import { el, card, pageHead, button, badge, note, toggle, segmented, stat, emptyState } from '../ui.js';
import { RobotModel } from '../model3d.js';

const POSES = {
  zero: {},
  stand: {
    left_hip_pitch_joint: -0.30, right_hip_pitch_joint: -0.30,
    left_knee_joint: 0.62, right_knee_joint: 0.62,
    left_ankle_pitch_joint: -0.32, right_ankle_pitch_joint: -0.32,
    left_shoulder_roll_joint: -0.10, right_shoulder_roll_joint: 0.10,
    left_elbow_joint: -0.30, right_elbow_joint: -0.30,
  },
  crouch: {
    left_hip_pitch_joint: -1.20, right_hip_pitch_joint: -1.20,
    left_knee_joint: 2.00, right_knee_joint: 2.00,
    left_ankle_pitch_joint: -0.85, right_ankle_pitch_joint: -0.85,
    waist_pitch_joint: 0.25,
    left_elbow_joint: -0.60, right_elbow_joint: -0.60,
  },
  tpose: { left_shoulder_roll_joint: -1.57, right_shoulder_roll_joint: 1.57 },
  wave: {
    right_shoulder_pitch_joint: -0.4, right_shoulder_roll_joint: 1.30,
    right_elbow_joint: -1.30, right_wrist_pitch_joint: 0.3,
    left_shoulder_roll_joint: -0.10,
  },
};

// Cool grey through blue, green, amber, red. Grey rather than deep blue at the
// bottom so an idle joint reads as "nothing happening" instead of "measured
// zero", which matters when half the robot is parked.
const LOAD_RAMP = [
  [0.42, 0.46, 0.52],
  [0.23, 0.51, 0.87],
  [0.29, 0.76, 0.55],
  [0.95, 0.75, 0.16],
  [0.90, 0.28, 0.20],
];

function rampColour(t) {
  const clamped = Math.max(0, Math.min(1, t));
  const scaled = clamped * (LOAD_RAMP.length - 1);
  const index = Math.min(LOAD_RAMP.length - 2, Math.floor(scaled));
  const frac = scaled - index;
  const a = LOAD_RAMP[index], b = LOAD_RAMP[index + 1];
  return [a[0] + (b[0] - a[0]) * frac,
          a[1] + (b[1] - a[1]) * frac,
          a[2] + (b[2] - a[2]) * frac];
}

function css(colour) {
  return `rgb(${colour.map((c) => Math.round(c * 255)).join(',')})`;
}

export default {
  id: 'model',
  label: 'Twin',
  icon: 'motion',

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

    const statusBadge = badge('loading…');
    const canvas = el('canvas', {
      style: { width: '100%', height: '520px', display: 'block', cursor: 'grab',
               borderRadius: 'var(--radius-sm)', touchAction: 'none' },
    });

    const camera = { yaw: -0.9, pitch: 0.18, distance: 3.0, target: [0, 0, 0.75], fov: 0.75 };

    let robot = null;
    let source = 'live';
    let shading = 'load';        // 'load' | 'plain'
    let spinning = false;
    let frame = null;
    let disposed = false;
    const manual = {};

    // joint name -> {link, rated}. Built once the model is loaded: shading
    // paints links, but telemetry is per joint, and the link a joint drives is
    // its child in the URDF tree.
    const jointMeta = new Map();

    root.appendChild(pageHead(
      'Digital twin',
      'The real X2 model, posed by live joint telemetry and shaded by how hard each '
      + 'joint is working. Drag to orbit, scroll to zoom.',
      [
        segmented(
          [{ value: 'load', label: 'Load' }, { value: 'plain', label: 'Plain' }],
          shading,
          (value) => { shading = value; legend.style.display = value === 'load' ? '' : 'none'; },
        ),
        segmented(
          [{ value: 'live', label: 'Follow robot' }, { value: 'pose', label: 'Poses' }],
          source,
          (value) => {
            source = value;
            poseRow.style.display = value === 'pose' ? '' : 'none';
            statusBadge.textContent = value === 'live' ? 'following robot' : 'manual pose';
            statusBadge.dataset.tone = value === 'live' ? 'good' : 'accent';
          },
        ),
        statusBadge,
      ],
    ));

    /* -- Load tiles -------------------------------------------------------- */

    const tiles = el('div.grid.cols-4');
    const refs = {};
    for (const [key, label] of [
      ['peak', 'Highest load'],
      ['total', 'Total torque'],
      ['busy', 'Joints under load'],
      ['faults', 'Joints reporting error'],
    ]) {
      // sub: ' ' rather than '' so the .stat-sub element always exists and
      // setTile can write into it. Same pattern as the Navigation tab.
      const node = stat(label, '—', { sub: ' ' });
      refs[key] = node;
      tiles.appendChild(node);
    }
    root.appendChild(tiles);

    function setTile(key, value, sub, tone = 'default', unit = '') {
      const node = refs[key];
      if (!node) return;
      node.dataset.tone = tone;
      const valueNode = node.querySelector('.stat-value');
      valueNode.textContent = String(value);
      if (unit) valueNode.appendChild(el('span.unit', { text: unit }));
      node.querySelector('.stat-sub').textContent = sub || '';
    }

    /* -- Legend and hardest-working list ----------------------------------- */

    const legend = el('div.row', {
      style: { gap: '10px', alignItems: 'center', flexWrap: 'wrap' },
    },
      el('span', { text: 'Load', style: { fontSize: '11.5px', color: 'var(--text-3)' } }),
      (() => {
        const bar = el('div', {
          style: {
            height: '10px', width: '190px', borderRadius: '5px',
            border: '1px solid var(--border)',
            background: `linear-gradient(90deg, ${LOAD_RAMP.map(css).join(',')})`,
          },
        });
        return bar;
      })(),
      el('span', { text: '0 % → 100 % of rated torque',
                   style: { fontSize: '11.5px', color: 'var(--text-3)' } }),
    );

    const poseRow = el('div.row', { style: { gap: '8px', flexWrap: 'wrap', display: 'none' } },
      ...Object.keys(POSES).map((name) => button(
        name === 'tpose' ? 'T-pose' : name[0].toUpperCase() + name.slice(1),
        () => applyPose(name),
        { size: 'sm' },
      )),
    );

    const hardest = el('div.stack', { style: { gap: '5px' } });

    root.appendChild(card('X2 Ultra', {
      sub: '41 links · 31 revolute joints · baked from x2_ultra.urdf',
      actions: [
        toggle('Spin', false, (on) => { spinning = on; }),
        button('Reset view', () => {
          camera.yaw = -0.9; camera.pitch = 0.18; camera.distance = 3.0;
          camera.target = [0, 0, 0.75];
        }, { size: 'sm', style: 'ghost' }),
      ],
      flush: true,
      foot: el('div.stack', { style: { gap: '10px' } }, legend, poseRow),
    }, canvas));

    root.appendChild(card('Working hardest', {
      sub: 'Measured torque against each joint’s rated limit, highest first',
    }, hardest));

    root.appendChild(note(
      'This robot does not report joint temperature — aimdk_msgs/JointState carries only '
      + 'position, velocity, effort and an error code, and no service or topic on the unit '
      + 'exposes motor temperature. Colour here is measured torque as a share of each joint’s '
      + 'rated limit, which is the closest honest indicator of which joints are heating: motor '
      + 'heating rises with current, and current tracks torque.',
      'info',
    ));

    /* -- Boot -------------------------------------------------------------- */

    try {
      robot = new RobotModel(canvas);
    } catch (err) {
      root.replaceChildren(pageHead('Digital twin', 'The 3D view could not start.'),
                           emptyState('WebGL unavailable', err.message));
      return { node: root };
    }

    try {
      const [modelRes, geometryRes] = await Promise.all([
        fetch('/model/model.json', { cache: 'force-cache' }),
        fetch('/model/model.bin', { cache: 'force-cache' }),
      ]);
      if (!modelRes.ok || !geometryRes.ok) {
        throw new Error('The baked model is not on the server (web/model/).');
      }
      const model = await modelRes.json();
      const geometry = await geometryRes.arrayBuffer();
      robot.load(model, geometry);

      for (const joint of model.joints) {
        if (joint.type !== 'revolute') continue;
        jointMeta.set(joint.name, {
          link: joint.child,
          // Fall back to a mid-range rating rather than 0 - dividing by zero
          // would paint an unrated joint permanently red.
          rated: joint.effort && joint.effort > 0 ? joint.effort : 40,
        });
      }

      statusBadge.textContent = 'following robot';
      statusBadge.dataset.tone = 'good';
    } catch (err) {
      robot.dispose();
      root.replaceChildren(
        pageHead('Digital twin', 'The 3D model could not be loaded.'),
        emptyState('Model missing', err.message),
      );
      return { node: root };
    }

    /* -- Interaction -------------------------------------------------------- */

    let dragging = null;

    canvas.addEventListener('pointerdown', (event) => {
      dragging = { x: event.clientX, y: event.clientY };
      canvas.setPointerCapture(event.pointerId);
      canvas.style.cursor = 'grabbing';
    });

    canvas.addEventListener('pointermove', (event) => {
      if (!dragging) return;
      camera.yaw -= (event.clientX - dragging.x) * 0.008;
      camera.pitch = Math.max(-1.35, Math.min(1.35,
        camera.pitch + (event.clientY - dragging.y) * 0.008));
      dragging = { x: event.clientX, y: event.clientY };
    });

    for (const type of ['pointerup', 'pointercancel']) {
      canvas.addEventListener(type, () => { dragging = null; canvas.style.cursor = 'grab'; });
    }

    canvas.addEventListener('wheel', (event) => {
      event.preventDefault();
      camera.distance = Math.max(0.8, Math.min(12,
        camera.distance * (1 + Math.sign(event.deltaY) * 0.1)));
    }, { passive: false });

    function applyPose(name) {
      source = 'pose';
      poseRow.style.display = '';
      statusBadge.textContent = 'manual pose';
      statusBadge.dataset.tone = 'accent';
      for (const key of robot.jointAngles.keys()) manual[key] = 0;
      Object.assign(manual, POSES[name] || {});
    }

    /* -- Live telemetry ------------------------------------------------------ */

    function readJoints() {
      const groups = store.state?.joints || {};
      const rows = [];
      for (const list of Object.values(groups)) {
        if (!Array.isArray(list)) continue;
        for (const joint of list) {
          if (joint && typeof joint.position === 'number') rows.push(joint);
        }
      }
      return rows;
    }

    /* -- Frame loop ---------------------------------------------------------- */

    const styles = getComputedStyle(document.documentElement);

    function background() {
      const raw = (styles.getPropertyValue('--surface-2') || '').trim();
      const match = /^#?([0-9a-f]{6})$/i.exec(raw);
      if (!match) return [0.09, 0.10, 0.12];
      const value = parseInt(match[1], 16);
      return [((value >> 16) & 255) / 255, ((value >> 8) & 255) / 255, (value & 255) / 255];
    }

    let lastPanel = 0;

    function loop() {
      if (disposed) return;
      frame = requestAnimationFrame(loop);

      if (spinning && !dragging) camera.yaw += 0.004;

      const rows = source === 'live' ? readJoints() : [];
      const angles = {};
      if (source === 'live') {
        for (const joint of rows) angles[joint.name] = joint.position;
      } else {
        Object.assign(angles, manual);
      }
      robot.setJoints(angles);

      // Shading. In pose mode there is no live torque to show, so the model
      // stays neutral rather than freezing the last real reading onto a pose
      // the robot is not actually holding.
      robot.linkColours.clear();
      let loads = [];
      if (shading === 'load' && source === 'live') {
        for (const joint of rows) {
          const meta = jointMeta.get(joint.name);
          if (!meta) continue;
          const effort = Math.abs(joint.effort || 0);
          const share = effort / meta.rated;
          robot.linkColours.set(meta.link, rampColour(share));
          loads.push({ name: joint.name, effort, share, rated: meta.rated,
                       error: joint.error || 0 });
        }
      }

      robot.render(camera, background());

      const now = Date.now();
      if (now - lastPanel > 300) {
        lastPanel = now;
        paintPanels(loads);
      }
    }

    function paintPanels(loads) {
      if (!loads.length) {
        const why = source === 'live' ? 'Waiting for joint telemetry…'
                                      : 'Manual pose — no live torque to show.';
        hardest.replaceChildren(el('div', {
          text: why, style: { fontSize: '12.5px', color: 'var(--text-3)' },
        }));
        for (const key of ['peak', 'total', 'busy', 'faults']) setTile(key, '—', '');
        return;
      }

      loads.sort((a, b) => b.share - a.share);
      const peak = loads[0];
      const total = loads.reduce((sum, l) => sum + l.effort, 0);
      const busy = loads.filter((l) => l.share >= 0.25).length;
      const faults = loads.filter((l) => l.error).length;

      setTile('peak', num(peak.share * 100, 0),
              `${peak.name.replace(/_joint$/, '').replace(/_/g, ' ')} · ${num(peak.effort, 1)} Nm`,
              peak.share > 0.8 ? 'critical' : peak.share > 0.5 ? 'warning' : 'good', ' %');
      setTile('total', num(total, 0), 'sum across 31 joints', 'default', ' Nm');
      setTile('busy', busy, 'above 25 % of rated',
              busy > 6 ? 'warning' : 'default');
      setTile('faults', faults, faults ? 'check the Motion tab' : 'all clear',
              faults ? 'critical' : 'good');

      hardest.replaceChildren(...loads.slice(0, 6).map((l) => el('div.row', {
        style: { gap: '10px', alignItems: 'center' },
      },
        el('span', {
          text: l.name.replace(/_joint$/, '').replace(/_/g, ' '),
          style: { fontSize: '12px', width: '170px', flexShrink: '0' },
        }),
        el('div', {
          style: {
            flex: '1', height: '8px', borderRadius: '4px',
            background: 'var(--surface-2)', overflow: 'hidden', minWidth: '60px',
          },
        },
          el('div', {
            style: {
              width: `${Math.max(2, Math.min(100, l.share * 100))}%`, height: '100%',
              background: css(rampColour(l.share)), transition: 'width .25s',
            },
          }),
        ),
        el('span', {
          text: `${num(l.effort, 1)} / ${num(l.rated, 0)} Nm`,
          style: { fontSize: '11px', fontFamily: 'var(--mono)', color: 'var(--text-3)',
                   width: '110px', textAlign: 'right', flexShrink: '0' },
        }),
        el('span', {
          text: `${num(l.share * 100, 0)}%`,
          style: { fontSize: '11.5px', fontWeight: '600', width: '42px',
                   textAlign: 'right', flexShrink: '0', color: css(rampColour(l.share)) },
        }),
      )));
    }

    loop();

    return {
      node: root,
      dispose: () => {
        disposed = true;
        if (frame) cancelAnimationFrame(frame);
        robot.dispose();
      },
    };
  },
};