/**
 * 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
 *
 * ── IMPORTANT: PositionControl.cpp motor convention ─────────────
 *   Motor 1 pivot is at (+d2, 0) = (+12.9, 0)  [right side!]
 *   Motor 2 pivot is at (-d2, 0) = (-12.9, 0)  [left  side!]
 *
 *   This is determined by how the C++ IK formulas use the offsets:
 *     Motor 1:  xmd = x - d2  → target is measured from x = +d2
 *     Motor 2:  xpd = x + d2  → target is measured from x = -d2
 *
 *   All FK / visualisation code MUST use this same convention or
 *   the arms will appear visually crossed even for valid positions.
 *
 * 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 separation; M1 at (+d2, 0), M2 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 °/step
  HOME_STEPS: { m1: 0, m2: -1024 }, // M1 at 0°, M2 at -180° (arms folded outward)
};

// ── WORKSPACE CONSTRAINTS ─────────────────────────────────────────
// All values are in mm.  Tune these to match real hardware.
// They are exported so the visualiser can draw the zones.
const LIMITS = {
  // ── Outer board boundary ───────────────────────────────────────
  // The EE cannot be requested outside this rectangle.
  X_MIN: -150,  // ← TUNE: left edge of board
  X_MAX: 150,  // ← TUNE: right edge of board
  Y_MIN: -30,  // ← TUNE: bottom (numbers reach ~44 mm; sign coords go lower)
  Y_MAX: 145,  // ← TUNE: top (highest letter is ~128 mm)

  // ── Centre mechanism exclusion box ────────────────────────────
  // Rectangular zone centred on (0, 0) where the motor housing sits.
  // The EE and both elbows must stay outside this area.
  BOX_HALF_W: 30,  // ← TUNE: half-width in X (motors at ±12.9, housing wider)
  BOX_Y_MIN: -10,  // ← TUNE: bottom of housing
  BOX_Y_MAX: 40,  // ← TUNE: top of housing

  // ── Elbow exclusion zone (prevents arm crossing near the box) ─
  // Each elbow has a separate rectangular exclusion box.
  // Left-side elbow (from M1 at +d2): must NOT enter this region.
  // Right-side elbow (from M2 at -d2): uses mirrored X limits.
  //   If ELBOW_BOX_X_INNER is 5, the left elbow's X must be > +5 mm
  //   (can never cross to the other side of the box mid-point).
  ELBOW_BOX_X_INNER: 5,   // ← TUNE: inner X margin from centre for each elbow
  ELBOW_BOX_Y_MAX: 50,  // ← TUNE: Y below which elbow crossing is forbidden
};

// ── 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 (pivot at +d2, 0 = +12.9 mm) ─────────────────────
  // xmd = x - d2  is the X component of (target – M1_pivot).
  const xmd = x - d2;
  const s = Math.sqrt(xmd * xmd + y * y);
  if (s < 1e-6) return { theta1: 0, theta2: 0, reachable: false };
  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 (pivot at -d2, 0 = -12.9 mm) ─────────────────────
  // xpd = x + d2  is the X component of (target – M2_pivot).
  const xpd = x + d2;
  const t = Math.sqrt(xpd * xpd + y * y);
  if (t < 1e-6) return { theta1: 0, theta2: 0, reachable: false };
  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;

  // ── IMPORTANT: match PositionControl.cpp motor convention ──────
  // Motor 1 pivot at (+d2, 0), Motor 2 pivot at (-d2, 0).
  // Using the opposite sign here is the single most common source
  // of visually-crossed arms in the SVG visualiser.

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

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

  // End-effector: intersection of the two distal-link circles
  // (radius l2, centred on each elbow). Pick the "upward" solution.
  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 — fall back to midpoint
    return {
      elbow1: { x: e1x, y: e1y },
      elbow2: { x: e2x, y: e2y },
      endX: (e1x + e2x) / 2,
      endY: (e1y + e2y) / 2,
      valid: false,
    };
  }

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

  // Two intersection candidates
  const px1 = mx + h * (dy / dist);
  const py1 = my + h * (-dx / dist);
  const px2 = mx - h * (dy / dist);
  const py2 = my - h * (-dx / dist);

  // Always pick the candidate with higher Y (EE above the elbow line)
  const useFirst = py1 >= py2;
  const endX = useFirst ? px1 : px2;
  const endY = useFirst ? py1 : py2;

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

// ── Arm-crossing check ────────────────────────────────────────────
/**
 * Returns true if the two proximal arm segments geometrically
 * cross each other near the centre mechanism box.
 *
 * Physical rule: each elbow must stay on the OUTER side of the
 * mechanism housing. If elbow1 (from M1 at +d2) has a small
 * positive X at low Y, or elbow2 (from M2 at -d2) has a small
 * negative X at low Y, the arm would collide with the housing.
 *
 * @param {number} theta1  Motor 1 angle (rad)
 * @param {number} theta2  Motor 2 angle (rad)
 * @returns {boolean}  true = arms will cross / collide
 */
function armsCrossed(theta1, theta2) {
  const { d2, l1 } = ARM;
  const { ELBOW_BOX_X_INNER: XI, ELBOW_BOX_Y_MAX: YM } = LIMITS;

  // Elbow positions (same formula as forward())
  const e1x = +d2 + l1 * Math.cos(theta1);
  const e1y = l1 * Math.sin(theta1);
  const e2x = -d2 + l1 * Math.cos(theta2);
  const e2y = l1 * Math.sin(theta2);

  // Elbow1 (from M1 on the RIGHT) must not appear far to the LEFT at low height
  // Elbow2 (from M2 on the LEFT)  must not appear far to the RIGHT at low height
  // Both conditions together catch the "arms have swapped sides" scenario.
  const e1_crossed = e1x < -XI && e1y < YM;  // M1's elbow went too far left
  const e2_crossed = e2x > XI && e2y < YM;  // M2's elbow went too far right

  return e1_crossed || e2_crossed;
}

// ── Workspace check ───────────────────────────────────────────────
/**
 * Check whether a target point is safe to move to.
 *
 * Order of checks (fail-fast):
 *   1. Board outer boundary
 *   2. Mechanism housing exclusion box
 *   3. IK geometric reachability
 *   4. Elbow-crossing guard (prevents physically impossible arm configs)
 *
 * @param {number} x
 * @param {number} y
 * @returns {{ ok: boolean, reason: string }}
 */
function checkWorkspace(x, y) {
  const L = LIMITS;

  // 1. Board boundary
  if (x < L.X_MIN || x > L.X_MAX)
    return { ok: false, reason: `X=${x.toFixed(1)} mm outside board (${L.X_MIN}…${L.X_MAX})` };
  if (y < L.Y_MIN || y > L.Y_MAX)
    return { ok: false, reason: `Y=${y.toFixed(1)} mm outside board (${L.Y_MIN}…${L.Y_MAX})` };

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

  // 3. IK geometric reachability
  const { theta1, theta2, reachable } = solve(x, y);
  if (!reachable)
    return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) is geometrically unreachable` };

  // 4. Elbow-crossing guard
  if (armsCrossed(theta1, theta2))
    return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) would cross elbows near the mechanism box` };

  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, armsCrossed, checkWorkspace, lookup,
  stepsToRad, radToSteps, stepsToDeg, radToDeg,
};