/**
 * WijiBoard – Kinematics  (exact port of PositionControl.cpp)
 * web/js/kinematics.js
 *
 * ── Mechanism: symmetric 5-bar parallel linkage ─────────────────
 *
 *                  END EFFECTOR (x, y)
 *                 /              \
 *           L2 (110)          L2 (110)
 *               /                  \
 *         ELBOW1                  ELBOW2
 *              \                  /
 *           L1 (85)           L1 (85)
 *                \              /
 *         MOTOR1 (-d2,0)   MOTOR2 (+d2,0)
 *                   |        |
 *                  [===BASE===]  (d=25.8 mm wide)
 *                   d2=12.9 mm
 *
 * Both motors are mounted in the centre mechanism box.
 * Motor 1 is at (-d2, 0), Motor 2 is at (+d2, 0).
 * Each motor drives a proximal arm (l1). The distal arms (l2)
 * connect the elbows to the shared end-effector.
 *
 * IK: given target (x, y), solve θ1 and θ2 independently:
 *   Motor 1 sees the target at (x - d2, y) from its pivot.
 *   Motor 2 sees the target at (x + d2, y) from its pivot.
 *   Each uses the standard 2-link IK (law of cosines).
 *
 * Source: lib/Position/PositionControl.cpp  (nerd-sniped/WijiBoard)
 */

// ── Exact constants from PositionControl.cpp ─────────────────────
const ARM = {
  d: 25.8,    // full motor separation (mm)
  d2: 12.9,    // half motor separation — motor 1 at (-d2,0), motor 2 at (+d2,0)
  l1: 85.0,    // proximal link length (mm)
  l2: 110.0,   // distal  link length (mm)
  STEPS_PER_REV: 2048,
  STEP_ANGLE_DEG: 360 / 2048,  // ≈ 0.17578125°
};

// ── Workspace limits ──────────────────────────────────────────────
const LIMITS = {
  // Outer bounding box (the board surface)
  X_MIN: -150,
  X_MAX: 150,
  Y_MIN: -30,    // numbers sit slightly below Y=0
  Y_MAX: 145,

  // Centre exclusion zone (the motor/mechanism box)
  // Motors are at ±12.9 mm; box is a bit larger to account for the housing
  BOX_HALF_W: 22,   // ±22 mm in X  (tune to real hardware)
  BOX_HALF_H: 22,   // 0..22 mm in Y (box sits above the base line)
  BOX_Y_MIN: -5,
  BOX_Y_MAX: 22,
};

// ── Letter / number position lookup table ────────────────────────
// Directly ported from PositionControl.cpp
const LOOKUP_TABLE = {
  'Q': { x: -66.5, y: 91.6 },
  'W': { x: 70.8, y: 92.0 },
  'E': { x: -41.4, y: 125.5 },
  'R': { x: -44.0, y: 96.0 },
  'T': { x: 1.3, y: 97.8 },
  'Y': { x: 117.8, y: 91.7 },
  'U': { x: 22.5, y: 98.3 },
  'I': { x: 53.5, y: 124.5 },
  'O': { x: -112.0, y: 82.0 },
  'P': { x: -90.0, y: 89.0 },
  'A': { x: -143.0, y: 99.0 },
  'S': { x: -19.8, y: 98.0 },
  'D': { x: -68.5, y: 121.7 },
  'F': { x: -19.5, y: 127.0 },
  'G': { x: 4.0, y: 128.4 },
  'H': { x: 31.3, y: 127.3 },
  'J': { x: 72.5, y: 121.0 },
  'K': { x: 93.0, y: 116.8 },
  'L': { x: 114.3, y: 110.2 },
  'Z': { x: 130.0, y: 75.0 },
  'X': { x: 95.9, y: 87.3 },
  'C': { x: -94.0, y: 116.0 },
  'V': { x: 44.5, y: 97.0 },
  'B': { x: -119.0, y: 109.0 },
  'N': { x: -135.4, y: 75.1 },
  'M': { x: 132.0, y: 100.5 },

  '+': { x: -110.0, y: 1.0 },
  '-': { x: 110.0, y: 1.0 },
  '*': { x: 110.0, y: -24.0 },
  ',': { x: -110.0, y: -24.0 },

  '0': { x: -130.4, y: 44.2 },
  '1': { x: -101.1, y: 53.3 },
  '2': { x: -71.5, y: 60.4 },
  '3': { x: -41.7, y: 65.2 },
  '4': { x: -13.0, y: 66.5 },
  '5': { x: 16.3, y: 68.0 },
  '6': { x: 45.8, y: 64.0 },
  '7': { x: 75.7, y: 61.1 },
  '8': { x: 103.6, y: 53.8 },
  '9': { x: 132.1, y: 45.0 },
};

// ── IK solver — exact port of calculateInverseKinematics() ───────
/**
 * Compute motor angles for a given target end-effector position.
 * Exactly mirrors the C++ implementation in PositionControl.cpp.
 *
 * @param {number} x  Target X in mm (origin = midpoint between motors)
 * @param {number} y  Target Y in mm
 * @returns {{ theta1: number, theta2: number, reachable: boolean }}
 *   theta1 / theta2 in RADIANS (motor 1 = left, motor 2 = right)
 */
function solve(x, y) {
  const { d2, l1, l2 } = ARM;

  // ── Motor 1 (left pivot at -d2, 0) ───────────────────────────
  const xmd = x - d2;                             // target X relative to left motor
  const s = Math.sqrt(xmd * xmd + y * y);       // distance: left motor → target
  const cosW1 = (l2 * l2 - s * s - l1 * l1) / (-2 * l1 * s);
  if (cosW1 < -1 || cosW1 > 1) return { theta1: 0, theta2: 0, reachable: false };
  const q = Math.atan2(y, xmd);
  const w1 = Math.acos(cosW1);
  const theta1 = q - w1;

  // ── Motor 2 (right pivot at +d2, 0) ──────────────────────────
  const xpd = x + d2;                             // target X relative to right motor
  const t = Math.sqrt(xpd * xpd + y * y);       // distance: right motor → target
  const cosW2 = (l2 * l2 - t * t - l1 * l1) / (-2 * l1 * t);
  if (cosW2 < -1 || cosW2 > 1) return { theta1: 0, theta2: 0, reachable: false };
  const r = Math.atan2(y, xpd);
  const w2 = Math.acos(cosW2);
  const theta2 = r + w2;

  return { theta1, theta2, reachable: true };
}

// ── Forward kinematics ───────────────────────────────────────────
/**
 * Given motor angles, compute elbow and end-effector positions.
 * The end-effector is found as the intersection of the two distal
 * link circles — this is the FK complement to the 5-bar IK above.
 *
 * In practice for visualisation we just re-derive the elbow
 * positions from each motor.
 *
 * @param {number} theta1  Motor 1 angle (radians)
 * @param {number} theta2  Motor 2 angle (radians)
 * @returns {{
 *   elbow1: {x,y}, elbow2: {x,y},
 *   endX: number, endY: number
 * }}
 */
function forward(theta1, theta2) {
  const { d2, l1, l2 } = ARM;

  // Elbow 1 (tip of motor 1's proximal link)
  const e1x = -d2 + l1 * Math.cos(theta1);
  const e1y = l1 * Math.sin(theta1);

  // Elbow 2 (tip of motor 2's proximal link)
  const e2x = d2 + l1 * Math.cos(theta2);
  const e2y = l1 * Math.sin(theta2);

  // End-effector: intersection of circle(elbow1, l2) and circle(elbow2, l2)
  // Use the same approach as the IK: each distal link points from its elbow to the EE.
  // For visualisation accuracy, reconstruct EE by reversing the IK:
  //   From IK: theta1 = q - w1  →  q = atan2(y, x - d2)
  //   We know theta1 and the elbow position, so EE is at l2 along some direction.
  // Simplest: use circle-circle intersection of the two elbow-radius-l2 circles.
  const dx = e2x - e1x;
  const dy = e2y - e1y;
  const dist = Math.sqrt(dx * dx + dy * dy);

  if (dist < 1e-6 || dist > 2 * l2) {
    // Degenerate / unreachable — just average the two elbows
    return {
      elbow1: { x: e1x, y: e1y },
      elbow2: { x: e2x, y: e2y },
      endX: (e1x + e2x) / 2,
      endY: (e1y + e2y) / 2,
    };
  }

  const a = dist / 2;
  const h = Math.sqrt(l2 * l2 - a * a);
  const mx = (e1x + e2x) / 2;
  const my = (e1y + e2y) / 2;

  // Two intersection candidates — pick the one with higher Y (the "up" configuration)
  const px1 = mx + h * (dy / dist);
  const py1 = my - h * (dx / dist);
  const px2 = mx - h * (dy / dist);
  const py2 = my + h * (dx / dist);

  const { endX, endY } = py1 > py2
    ? { endX: px1, endY: py1 }
    : { endX: px2, endY: py2 };

  return {
    elbow1: { x: e1x, y: e1y },
    elbow2: { x: e2x, y: e2y },
    endX, endY,
  };
}

// ── Workspace check ───────────────────────────────────────────────
/**
 * Check whether a point is inside the valid workspace.
 * @param {number} x
 * @param {number} y
 * @returns {{ ok: boolean, reason: string }}
 */
function checkWorkspace(x, y) {
  // Outer bounding box
  if (x < LIMITS.X_MIN || x > LIMITS.X_MAX)
    return { ok: false, reason: `X=${x.toFixed(1)} outside board limits [${LIMITS.X_MIN}, ${LIMITS.X_MAX}]` };
  if (y < LIMITS.Y_MIN || y > LIMITS.Y_MAX)
    return { ok: false, reason: `Y=${y.toFixed(1)} outside board limits [${LIMITS.Y_MIN}, ${LIMITS.Y_MAX}]` };

  // Centre exclusion zone (mechanism box)
  if (
    x > -LIMITS.BOX_HALF_W && x < LIMITS.BOX_HALF_W &&
    y > LIMITS.BOX_Y_MIN && y < LIMITS.BOX_Y_MAX
  ) {
    return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) is inside the mechanism exclusion zone` };
  }

  // IK reachability
  const { reachable } = solve(x, y);
  if (!reachable) return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) is outside arm reach` };

  return { ok: true, reason: '' };
}

// ── Lookup ────────────────────────────────────────────────────────
function lookup(char) {
  return LOOKUP_TABLE[char.toUpperCase()] ?? null;
}

// ── Unit converters ───────────────────────────────────────────────
function stepsToRad(steps) {
  return (steps / ARM.STEPS_PER_REV) * 2 * Math.PI;
}
function radToSteps(rad) {
  return Math.round((rad / (2 * Math.PI)) * ARM.STEPS_PER_REV);
}
function stepsToDeg(steps) {
  return steps * ARM.STEP_ANGLE_DEG;
}
function radToDeg(rad) {
  return rad * 180 / Math.PI;
}

export default {
  ARM, LIMITS, LOOKUP_TABLE,
  solve, forward, checkWorkspace, lookup,
  stepsToRad, radToSteps, stepsToDeg, radToDeg,
};