import BLE from './ble.js';
import IK from './kinematics.js';
import UI from './ui.js';
import Settings from './settings.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 parts = msg.slice(2).split(',');
    steps1 = Number(parts[0]);
    steps2 = Number(parts[1]);
    
    // parts[2] is the holding status (1 = holding, 0 = disabled)
    if (parts.length > 2) {
      const isHolding = parts[2] === '1';
      if (!isHolding && localStorage.getItem('wiji_homed') === 'true') {
        localStorage.setItem('wiji_homed', 'false');
        UI.log('Steppers are not holding. Rehoming required.', 'warn');
        document.dispatchEvent(new CustomEvent('wiji:unhomed'));
        const warningEl = document.getElementById('home-warning');
        if (warningEl) warningEl.style.display = 'block';
      }
    }
  }
});

// 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;
let currentMotionMode = 'direct';
let hasHesitated = false;

async function executeQueue() {
  if (motionQueue.length === 0) {
    isMoving = false;
    if (queueCheckInterval) clearInterval(queueCheckInterval);
    
    // Restore speed if it was altered
    if (BLE.isConnected() && ['jumpy', 'hesitant', 'overshoot'].includes(currentMotionMode)) {
      try { await BLE.write(`SPD:${Settings.speed}`); } catch(e){}
    }
    
    if (currentResolve) currentResolve(true);
    currentResolve = null;
    return;
  }

  isMoving = true;
  let target = motionQueue.shift();
  
  let delayBeforeSend = 0;
  if (target.mode === 'hesitant' && target.fraction >= 0.45 && !hasHesitated) {
     hasHesitated = true;
     delayBeforeSend = 400;
  } else if (target.mode === 'overshoot_settle') {
     delayBeforeSend = 200; // brief pause before returning
  }

  const doSend = async () => {
    const cmd = `X:${target.s1},${target.s2}`;
    if (BLE.isConnected()) {
      if (target.mode === 'overshoot_settle') {
        try { await BLE.write(`SPD:${Math.floor(Math.max(100, Settings.speed * 0.4))}`); } catch(e){}
      }
      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 or wait depending on mode.
          if (target.mode === 'erratic') {
            if (maxDist <= 5) { // wait for full stop
               clearInterval(queueCheckInterval);
               setTimeout(executeQueue, 50 + Math.random() * 100); 
            }
          } else if (target.mode === 'jumpy') {
            if (maxDist <= 5) { // stop and go rhythm
               clearInterval(queueCheckInterval);
               setTimeout(executeQueue, 100);
            }
          } else {
             // direct, snaky, hesitant, etc 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;
      }
    }
  };

  if (delayBeforeSend > 0) {
    setTimeout(doSend, delayBeforeSend);
  } else {
    doSend();
  }
}

function buildSegments(startX, startY, endX, endY, mode) {
  let actualEndX = endX;
  let actualEndY = endY;

  if (mode === 'overshoot') {
     const totalDx = endX - startX;
     const totalDy = endY - startY;
     const totalDist = Math.sqrt(totalDx * totalDx + totalDy * totalDy);
     if (totalDist > 0) {
       actualEndX = endX + (totalDx / totalDist) * 15;
       actualEndY = endY + (totalDy / totalDist) * 15;
       
       const wsCheck = IK.checkWorkspace(actualEndX, actualEndY);
       if (!wsCheck.ok) {
           actualEndX = endX;
           actualEndY = endY;
       }
     }
  }

  const dx = actualEndX - startX;
  const dy = actualEndY - startY;
  const distance = Math.sqrt(dx * dx + dy * dy);

  let numSegments = Math.ceil(distance / CONFIG.SEGMENT_SIZE_MM);
  
  if (mode === 'snaky' || mode === 'erratic') {
    numSegments = Math.max(numSegments, Math.ceil(distance / 10)); // 10mm segments max for finer resolution
  }
  if (numSegments === 0) numSegments = 1;

  let segs = [];

  const nx = -dy / distance || 0;
  const ny = dx / distance || 0;

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

    if (mode === 'snaky') {
      const cycles = Math.max(1, Math.floor(distance / 50)); 
      const offset = Math.sin(fraction * Math.PI * 2 * cycles) * 15;
      px += nx * offset;
      py += ny * offset;
    } else if (mode === 'erratic') {
      px += (Math.random() - 0.5) * 10;
      py += (Math.random() - 0.5) * 10;
    }

    const wsCheck = IK.checkWorkspace(px, py);
    if (!wsCheck.ok) {
      px = startX + dx * fraction;
      py = startY + dy * fraction;
    }

    const res = IK.solve(px, py);
    if (!res.reachable || IK.armsCrossed(res.theta1, res.theta2)) {
       px = startX + dx * fraction;
       py = startY + dy * fraction;
       const fallbackRes = IK.solve(px, py);
       if (!fallbackRes.reachable || IK.armsCrossed(fallbackRes.theta1, fallbackRes.theta2)) {
           return null;
       }
       segs.push({
           s1: IK.radToStepsAbsolute(fallbackRes.theta1, 1),
           s2: IK.radToStepsAbsolute(fallbackRes.theta2, 2),
           mode, 
           fraction
       });
       continue;
    }

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

  if (mode === 'overshoot') {
     const res = IK.solve(endX, endY);
     if (res.reachable && !IK.armsCrossed(res.theta1, res.theta2)) {
        const s1 = IK.radToStepsAbsolute(res.theta1, 1);
        const s2 = IK.radToStepsAbsolute(res.theta2, 2);
        segs.push({ s1, s2, mode: 'overshoot_settle', fraction: 1.1 });
     }
  }

  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;
  }

  if (mode === 'random') {
    const modes = ['direct', 'snaky', 'erratic', 'jumpy', 'hesitant', 'overshoot'];
    mode = modes[Math.floor(Math.random() * modes.length)];
  }
  
  currentMotionMode = mode;
  hasHesitated = false;

  // Quick check on final destination
  const destCheck = IK.checkWorkspace(targetX, targetY);
  if (!destCheck.ok) {
    UI.log(`[ERROR] 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(`[ERROR] 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(`[ERROR] Target unreachable (path blocked by mechanism)`, 'error');
      return false;
    }
  }

  motionQueue = segments;

  if (BLE.isConnected()) {
    if (mode === 'jumpy' || mode === 'hesitant') {
      const fastSpeed = Math.min(2000, Settings.speed * 1.5);
      try { await BLE.write(`SPD:${Math.floor(fastSpeed)}`); } catch(e){}
    }
  }

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

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