import BLE from './ble.js';
import IK from './kinematics.js';
import UI from './ui.js';
import Settings from './settings.js';
import { MotionEffects } from './motion-effects.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)
  WAYPOINT_TIMEOUT_MS: 8000 // Max time to wait for a single waypoint to complete before declaring a stall
};

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

function forceUnhome(reason) {
  localStorage.setItem('wiji_homed', 'false');
  UI.log(reason, 'warn');
  document.dispatchEvent(new CustomEvent('wiji:unhomed'));
  const warningEl = document.getElementById('home-warning');
  if (warningEl) warningEl.style.display = 'block';
}

// 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') {
        forceUnhome('Steppers are not holding. Rehoming required.');
      }
    }
  }
});

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

function stopCurrentMove() {
  motionQueue = [];
  isMoving = false;
  if (queueCheckInterval) {
    clearInterval(queueCheckInterval);
    queueCheckInterval = null;
  }
  if (currentResolve) {
    currentResolve(false); // resolve false because aborted
    currentResolve = null;
  }
}

async function executeQueue() {
  if (motionQueue.length === 0) {
    isMoving = false;
    if (queueCheckInterval) {
      clearInterval(queueCheckInterval);
      queueCheckInterval = null;
    }
    
    // Restore speed if it was altered
    await MotionEffects.onSequenceEnd(currentMotionMode);
    
    if (currentResolve) currentResolve(true);
    currentResolve = null;
    document.dispatchEvent(new CustomEvent('wiji:motion-end', { detail: { isIdleMove: currentIsIdleMove } }));
    return;
  }

  isMoving = true;
  let target = motionQueue.shift();
  
  const { delay: delayBeforeSend, hasHesitated: newHasHesitated } = await MotionEffects.onWaypointSend(target, hasHesitated);
  hasHesitated = newHasHesitated;

  const doSend = async () => {
    const cmd = `X:${target.s1},${target.s2}`;
    if (BLE.isConnected()) {
      try {
        await BLE.write(cmd);
      } catch (e) {
        UI.log(`BLE error: ${e.message}`, 'error');
        stopCurrentMove();
        document.dispatchEvent(new CustomEvent('wiji:motion-end', { detail: { isIdleMove: currentIsIdleMove } }));
        return;
      }
      
      // Setup interval to monitor progress
      if (queueCheckInterval) clearInterval(queueCheckInterval);
      const waypointStartTime = Date.now();
      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);

        // Check for timeout stall
        if (Date.now() - waypointStartTime > CONFIG.WAYPOINT_TIMEOUT_MS) {
          stopCurrentMove();
          forceUnhome('Motion timeout! Arms may be stuck. Rehoming required.');
          document.dispatchEvent(new CustomEvent('wiji:motion-end', { detail: { isIdleMove: currentIsIdleMove } }));
          return;
        }

        if (motionQueue.length > 0) {
          const nextTarget = motionQueue[0];
          const queueAction = MotionEffects.getQueueAction(target.mode, nextTarget.mode, maxDist, CONFIG.BLEND_THRESHOLD_STEPS);
          if (queueAction.action === 'wait') {
            clearInterval(queueCheckInterval);
            setTimeout(executeQueue, queueAction.delay);
          } else if (queueAction.action === 'blend') {
            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) {
  const modDest = MotionEffects.modifyDestination(startX, startY, endX, endY, mode);
  const actualEndX = modDest.endX;
  const actualEndY = modDest.endY;

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

  let numSegments = MotionEffects.getSegmentCount(distance, CONFIG.SEGMENT_SIZE_MM, mode);
  
  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;

    const perturbed = MotionEffects.applyPerturbation(px, py, nx, ny, fraction, distance, mode);
    px = perturbed.px;
    py = perturbed.py;

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

  const settlingSegs = MotionEffects.getSettlingWaypoints(endX, endY, mode);
  segs = segs.concat(settlingSegs);

  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', isIdleMove = false) {
  if (isMoving) {
    // If the currently executing move is an idle move, and this is a user move, we can interrupt it.
    if (currentIsIdleMove && !isIdleMove) {
      stopCurrentMove(); // Abort the idle move
    } else {
      UI.log('Arm is currently moving! Wait for it to finish.', 'warn');
      return false;
    }
  }

  currentIsIdleMove = isIdleMove;
  mode = MotionEffects.resolveMode(mode);
  
  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;

  await MotionEffects.onSequenceStart(mode);

  document.dispatchEvent(new CustomEvent('wiji:motion-start', { detail: { isIdleMove: currentIsIdleMove } }));

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

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