ESP32-WijiBoard/web/js/motion.js

import BLE from './ble.js';
import IK from './kinematics.js';
import UI from './ui.js';

const CONFIG = {
  SEGMENT_SIZE_MM: 25, // Maximum distance between waypoints to prevent out-of-bounds joint-space arcs
  BLEND_THRESHOLD_STEPS: 300 // Send next waypoint when within this many steps (prevents deceleration)
};

// Global state tracking
let steps1 = 0;
let steps2 = 0;

// Listen to BLE position updates
document.addEventListener('ble:status', (e) => {
  const msg = e.detail;
  if (msg.startsWith('P:')) {
    const [s1, s2] = msg.slice(2).split(',').map(Number);
    steps1 = s1;
    steps2 = s2;
  }
});

// To allow UI sections to override/sync steps initially
function setSteps(s1, s2) {
  steps1 = s1;
  steps2 = s2;
}

function getSteps() {
  return { steps1, steps2 };
}

let motionQueue = [];
let isMoving = false;
let currentResolve = null;
let queueCheckInterval = null;

async function executeQueue() {
  if (motionQueue.length === 0) {
    isMoving = false;
    if (queueCheckInterval) clearInterval(queueCheckInterval);
    if (currentResolve) currentResolve(true);
    currentResolve = null;
    return;
  }

  isMoving = true;
  let target = motionQueue.shift();
  
  const cmd = `X:${target.s1},${target.s2}`;
  if (BLE.isConnected()) {
    try {
      await BLE.write(cmd);
    } catch (e) {
      UI.log(`BLE error: ${e.message}`, 'error');
      motionQueue = []; // clear queue on error
      isMoving = false;
      if (queueCheckInterval) clearInterval(queueCheckInterval);
      if (currentResolve) currentResolve(false);
      currentResolve = null;
      return;
    }
    
    // Setup interval to monitor progress
    if (queueCheckInterval) clearInterval(queueCheckInterval);
    queueCheckInterval = setInterval(async () => {
      // Calculate distance to target in steps
      const dist1 = Math.abs(target.s1 - steps1);
      const dist2 = Math.abs(target.s2 - steps2);
      const maxDist = Math.max(dist1, dist2);

      if (motionQueue.length > 0) {
        // We have more waypoints. Blend!
        if (target.mode === 'direct') {
          if (maxDist <= CONFIG.BLEND_THRESHOLD_STEPS) {
             clearInterval(queueCheckInterval);
             executeQueue(); // start next waypoint early
          }
        } else if (target.mode === 'erratic') {
          if (maxDist <= 5) { // wait for full stop
             clearInterval(queueCheckInterval);
             setTimeout(executeQueue, 150); // slight pause for erratic effect
          }
        } else {
           // Default to direct blending
           if (maxDist <= CONFIG.BLEND_THRESHOLD_STEPS) {
             clearInterval(queueCheckInterval);
             executeQueue();
          }
        }
      } else {
        // Last waypoint. Wait until closely reached.
        if (maxDist <= 10) {
          clearInterval(queueCheckInterval);
          motionQueue = [];
          executeQueue(); // Will trigger resolve
        }
      }
    }, 40); // check ~25 times a second
    
  } else {
    // Simulation: instantly jump to final target
    const finalTarget = motionQueue.length > 0 ? motionQueue[motionQueue.length - 1] : target;
    steps1 = finalTarget.s1;
    steps2 = finalTarget.s2;
    console.log(`[sim] Jumped to X:${steps1},${steps2} via trajectory`);
    
    // Trigger pseudo-status event for the UI to update markers
    document.dispatchEvent(new CustomEvent('ble:status', { detail: `P:${steps1},${steps2}` }));
    
    motionQueue = [];
    isMoving = false;
    if (currentResolve) {
      currentResolve(true);
      currentResolve = null;
    }
  }
}

function buildSegments(startX, startY, endX, endY, mode) {
  const dx = endX - startX;
  const dy = endY - startY;
  const distance = Math.sqrt(dx * dx + dy * dy);

  let numSegments = Math.ceil(distance / CONFIG.SEGMENT_SIZE_MM);
  if (numSegments === 0) numSegments = 1;

  let segs = [];

  for (let i = 1; i <= numSegments; i++) {
    const fraction = i / numSegments;
    const px = startX + dx * fraction;
    const py = startY + dy * fraction;

    const wsCheck = IK.checkWorkspace(px, py);
    if (!wsCheck.ok) return null;

    const res = IK.solve(px, py);
    if (!res.reachable || IK.armsCrossed(res.theta1, res.theta2)) return null;

    const s1 = IK.radToStepsAbsolute(res.theta1, 1);
    const s2 = IK.radToStepsAbsolute(res.theta2, 2);
    
    segs.push({ s1, s2, mode });
  }
  return segs;
}

/**
 * Moves the arm to Cartesian targetX, targetY.
 * Dynamically slices the path into segments to prevent out-of-bounds arcs.
 * @returns {Promise<boolean>} Resolves to true if successful, false if aborted/error.
 */
async function goto(targetX, targetY, mode = 'direct') {
  if (isMoving) {
    UI.log('Arm is currently moving! Wait for it to finish.', 'warn');
    return false;
  }

  // Quick check on final destination
  const destCheck = IK.checkWorkspace(targetX, targetY);
  if (!destCheck.ok) {
    UI.log(`⛔ Destination blocked: ${destCheck.reason}`, 'error');
    return false;
  }

  // Calculate current Cartesian position
  const t1 = IK.stepsToRad(steps1);
  const t2 = IK.stepsToRad(steps2);
  const currentPos = IK.forward(t1, t2);

  if (!currentPos.valid || !isFinite(currentPos.endX) || !isFinite(currentPos.endY)) {
    UI.log('Cannot calculate current IK position (arms disconnected?). Force Home needed.', 'error');
    return false;
  }

  let segments = buildSegments(currentPos.endX, currentPos.endY, targetX, targetY, mode);

  if (!segments) {
    // Attempt rerouting through safe overhead point
    const SAFE_X = 0;
    const SAFE_Y = 75;

    // Prevent infinite loop if already at/near safe point
    const distToSafe = Math.sqrt(Math.pow(currentPos.endX - SAFE_X, 2) + Math.pow(currentPos.endY - SAFE_Y, 2));
    if (distToSafe < 5) {
       UI.log(`⛔ Path completely blocked even via safe point`, 'error');
       return false;
    }

    const leg1 = buildSegments(currentPos.endX, currentPos.endY, SAFE_X, SAFE_Y, mode);
    if (leg1) {
      const leg2 = buildSegments(SAFE_X, SAFE_Y, targetX, targetY, mode);
      if (leg2) {
        segments = leg1.concat(leg2);
        UI.log(`Rerouting around exclusion zone via (0, 75)`, 'info');
      }
    }

    if (!segments) {
      UI.log(`⛔ Target unreachable (path blocked by mechanism)`, 'error');
      return false;
    }
  }

  motionQueue = segments;

  return new Promise((resolve) => {
    currentResolve = resolve;
    executeQueue();
  });
}

export default {
  CONFIG,
  goto,
  setSteps,
  getSteps,
  get isMoving() { return isMoving; }
};