// Trajectory planning (port of nextdrawcore/motion.py).

//

// Moves are returned as lists of commands for the dripfeed:

//   ['lower'] / ['raise']

//   ['T3', [time, v1, a1, j1, v2, a2, j2], [inches, xyzPosAfter]]

//   ['TD', [time, v1a, v1b, a1, j1, v2a, v2b, a2, j2], [inches, xyzPosAfter]]

//

// Native motor axes on the NextDraw are mixed:

//   motor1 = x + y, motor2 = x - y.

import { checkLimitsTol, distance, dotProductXY, isClose, moveDistT3, pyRound, rateT3 } from './calc.js';

import * as plan from './plan.js';

// Rounded-up maximum distance covered by one step (low-res / hi-res).

const MAX_STEP_DIST_LR = 0.000696;

const MAX_STEP_DIST_HR = 0.000348;

// The Python planner stores these values in array('f'), i.e. float32.

const f32 = Math.fround;

export function copyPos(pos) {

  return { ...pos };

}

/** Plan a full pen-down path: lower pen, follow vertices, raise pen. */

export function trajectory(nd, vertexList, xyzPos = null) {

  if (xyzPos == null) xyzPos = copyPos(nd.pen.phys);

  xyzPos.zUp = false;

  const middle = planTrajectory(nd, vertexList, xyzPos);

  if (middle == null) return null;

  return [['lower'], ...middle, ['raise']];

}

/** Plan motion through a vertex list with junction-speed look-ahead. */

export function planTrajectory(nd, vertexList, xyzPos = null) {

  let trajLength = vertexList.length;

  if (trajLength < 2) return null;

  if (nd.pen.phys.xpos == null) return null;

  if (xyzPos == null) xyzPos = copyPos(nd.pen.phys);

  const penUp = xyzPos.zUp;

  if (trajLength < 3) {

    const [moves] = computeSegment(nd, [vertexList[1][0], vertexList[1][1], 0, 0, false], xyzPos);

    return moves ?? [];

  }

  const speedLimit = penUp ? nd.speedPenup : nd.speedPendown;

  const trajDists = [f32(0.0)];

  const trajVels = [f32(0.0)];

  const trajVectors = [];

  const trimmedPath = [];

  const minDist = nd.params.resolution === 1 ? MAX_STEP_DIST_HR : MAX_STEP_DIST_LR;

  let lastIndex = 0;

  for (let i = 1; i < trajLength; i++) {

    // Skip segments likely to be shorter than one motor step.

    const dx = vertexList[i][0] - vertexList[lastIndex][0];

    const dy = vertexList[i][1] - vertexList[lastIndex][1];

    const d = distance(dx, dy);

    if (d >= minDist) {

      trajDists.push(f32(d));

      trajVectors.push([dx / d, dy / d]);

      trimmedPath.push([vertexList[i][0], vertexList[i][1]]);

      lastIndex = i;

    }

  }

  trajLength = trajDists.length;

  if (trajLength < 2) return null;

  if (trajLength < 3) {

    // Only one usable segment remains. (The Python driver returns the raw

    // (move_list, data_list) tuple here, which the dripfeed then silently

    // ignores, so the line is never drawn. We draw it.)

    const [moves] = computeSegment(nd, [trimmedPath[0][0], trimmedPath[0][1], 0, 0, false], xyzPos);

    return moves ?? [];

  }

  const [jerkUp, jerkDown] = plan.calcJerk(nd);

  const jerkRate = penUp ? jerkUp : jerkDown;

  // Forward pass: speed limits at each vertex from jerk and cornering.

  for (let i = 1; i < trajLength - 1; i++) {

    const segLength = trajDists[i];

    const vPrevExit = trajVels[i - 1];

    let vMax = plan.scurvePlan(vPrevExit, speedLimit, jerkRate, segLength, 0.007);

    if (vMax == null) continue;

    let vec = trajVectors[i - 1];

    let len = distance(vec[0], vec[1]);

    const v1 = [vec[0] / len, vec[1] / len];

    vec = trajVectors[i];

    len = distance(vec[0], vec[1]);

    const v2 = [vec[0] / len, vec[1] / len];

    let cosine = dotProductXY(v1, v2);

    if (cosine < 0) cosine = 0; // Corner sharper than 90 degrees

    if (vMax > speedLimit * 0.5) vMax = Math.min(vMax, vMax * cosine ** 4);

    else if (vMax > speedLimit * 0.25) vMax = Math.min(vMax, vMax * cosine);

    vMax = Math.min(vMax, speedLimit * 0.8);

    trajVels.push(f32(vMax));

  }

  trajVels.push(f32(0.0)); // Come to rest at the final vertex.

  // Reverse pass: make sure every deceleration fits in its segment.

  for (let j = 1; j < trajLength; j++) {

    const i = trajLength - j;

    const vFinal = trajVels[i];

    const vInitial = trajVels[i - 1];

    const segLength = trajDists[i];

    if (segLength <= 0 || vInitial < vFinal || isClose(vInitial, vFinal, 1e-9)) continue;

    const vInitMax = plan.scurvePlan(vFinal, speedLimit, jerkRate, segLength, 0.007);

    trajVels[i - 1] = f32(Math.min(vInitial, vInitMax));

  }

  const moveList = [];

  for (let i = 0; i < trajLength - 1; i++) {

    const [moves, data] = computeSegment(

      nd, [trimmedPath[i][0], trimmedPath[i][1], trajVels[i], trajVels[i + 1], false], xyzPos);

    if (moves != null) {

      xyzPos = data[0];

      moveList.push(...moves);

    }

  }

  return moveList;

}

/**

 * Plan one straight segment with given initial/final speeds.

 * data = [xDest, yDest, vInitial, vFinal, ignoreLimits].

 * Returns [moveList, [xyzPosAfter]] or [null, null] if there is nothing to do.

 */

export function computeSegment(nd, data, xyzPos = null) {

  let [xDest, yDest, vi, vf, ignoreLimits] = data;

  if (xyzPos == null) xyzPos = copyPos(nd.pen.phys);

  const fCurrentX = xyzPos.xpos;

  if (fCurrentX == null) return [null, null];

  const fCurrentY = xyzPos.ypos;

  const penUp = xyzPos.zUp;

  const constantVelMode = nd.params.constSpeed && !penUp;

  if (!ignoreLimits) {

    const tol = nd.params.boundsTolerance;

    let xBounded, yBounded;

    [xDest, xBounded] = checkLimitsTol(xDest, nd.bounds[0][0], nd.bounds[1][0], tol);

    [yDest, yBounded] = checkLimitsTol(yDest, nd.bounds[0][1], nd.bounds[1][1], tol);

    if (xBounded || yBounded) nd.warn?.('bounds');

  }

  const dxIn = xDest - fCurrentX;

  const dyIn = yDest - fCurrentY;

  const motorSteps1 = pyRound(nd.stepScale * (dxIn + dyIn));

  const motorSteps2 = pyRound(nd.stepScale * (dxIn - dyIn));

  const m1Rounded = motorSteps1 / (2.0 * nd.stepScale);

  const m2Rounded = motorSteps2 / (2.0 * nd.stepScale);

  const dxRounded = m1Rounded + m2Rounded;

  const dyRounded = m1Rounded - m2Rounded;

  if (Math.abs(motorSteps1) < 1 && Math.abs(motorSteps2) < 1) return [null, null];

  const distInch = distance(dxRounded, dyRounded);

  const [jerkUp, jerkDown] = plan.calcJerk(nd);

  const speedLimit = penUp ? nd.speedPenup : nd.speedPendown;

  const jerkRate = penUp ? jerkUp : jerkDown;

  if (constantVelMode) {

    vi = speedLimit;

    vf = speedLimit;

  } else {

    vi = Math.min(vi, speedLimit);

    vf = Math.min(vf, speedLimit);

  }

  // Sub-segment codes: 1 S-curve accel, 2 S-curve decel, 3 constant velocity.

  const subsegs = [];

  const dists = [];

  const vels = [];

  const jerks = [];

  let accelDist = 0;

  let decelDist = 0;

  let kase = 0;

  const constSpeedTime = isClose(vi, 0) ? 100 : distInch / vi;

  if (constantVelMode || (isClose(vi, speedLimit) && isClose(vf, speedLimit))) {

    kase = 3;

  } else if (isClose(vi, vf, 1e-2) && constSpeedTime < 0.013) {

    kase = 3; // Very short; keep it a single constant-speed command.

  } else if (isClose(vi, vf, 1e-2) && constSpeedTime < 0.030 && vi > speedLimit / 2) {

    kase = 3; // Smooth motion at higher speeds.

  } else {

    accelDist = plan.scurvePlan(vi, speedLimit, jerkRate, null);

    decelDist = plan.scurvePlan(vf, speedLimit, jerkRate, null);

    const distSvm = accelDist + decelDist;

    const distSse = vi <= vf

      ? plan.scurvePlan(vi, vf, jerkRate, null)

      : plan.scurvePlan(vf, vi, jerkRate, null);

    if (distInch >= distSvm) {

      if (isClose(vi, speedLimit)) kase = isClose(distInch, distSvm) ? 2 : 4;

      else if (isClose(vf, speedLimit)) kase = isClose(distInch, distSvm) ? 1 : 5;

      else kase = 6;

    } else if (isClose(vi, vf, 1e-3) && isClose(vi, 0, 1e-3)) {

      kase = 7; // Starts and stops at rest: always a triangle.

    } else if (distInch > distSse) {

      kase = 7;

    } else if (isClose(distInch, distSse, 1e-3)) {

      kase = vi < vf ? 1 : 2;

    }

  }

  jerks.push(f32(jerkRate));

  if (kase === 1) {

    subsegs.push(1);

  } else if (kase === 2) {

    subsegs.push(2);

  } else if (kase === 3) {

    subsegs.push(3);

  } else if (kase === 4) {

    subsegs.push(3);

    dists.push(f32(distInch - decelDist));

    vels.push(f32(speedLimit));

    subsegs.push(2);

    jerks.push(f32(jerkRate));

  } else if (kase === 5) {

    subsegs.push(1);

    dists.push(f32(accelDist));

    vels.push(f32(speedLimit));

    subsegs.push(3);

    jerks.push(f32(jerkRate));

  } else if (kase === 6) {

    subsegs.push(1);

    dists.push(f32(accelDist));

    vels.push(f32(speedLimit));

    subsegs.push(3);

    dists.push(f32(distInch - decelDist));

    vels.push(f32(speedLimit));

    subsegs.push(2);

    jerks.push(f32(jerkRate));

    jerks.push(f32(jerkRate));

  } else if (kase === 7) {

    const vMid = plan.striangle(vi, vf, speedLimit, jerkRate, distInch);

    const time1 = plan.scurveTime(vi, vMid, jerkRate);

    const time2 = plan.scurveTime(vMid, vf, jerkRate);

    let time3 = 0;

    const jTemp = plan.scurveJerk(vi, vf, distInch, jerkRate);

    if (jTemp != null) time3 = plan.scurveTime(vi, vf, jTemp);

    if (time3 !== 0 && (time1 + time2 > time3 || time1 + time2 < 0.012)) kase = 0;

    if (kase === 7) {

      subsegs.push(1);

      dists.push(f32(plan.scurvePlan(vi, vMid, jerkRate, null)));

      vels.push(f32(vMid));

      subsegs.push(2);

      jerks.push(f32(jerkRate));

    }

  }

  if (kase === 0) {

    // Reduced-jerk single S-curve for moves too short for the full plan.

    let jTemp = plan.scurveJerk(vi, vf, distInch, jerkRate);

    if (jTemp == null) jTemp = plan.scurveJerk2(vi, vf, distInch, jerkRate);

    if (jTemp == null) return [null, null];

    jerks[jerks.length - 1] = f32(jTemp);

    subsegs.push(vi < vf ? 1 : 2);

  }

  return computeSubsegmentCmds(

    [vi, xyzPos, motorSteps1, motorSteps2, nd.stepScale, vf, distInch], subsegs, dists, vels, jerks);

}

/** Turn a planned segment into T3/TD commands (motion.compute_subsegment_cmds). */

export function computeSubsegmentCmds(params, subsegs, dists, vels, jerks) {

  const [vi, xyzPos, motorSteps1, motorSteps2, stepScale, vf, segmentLength] = params;

  const velocityScale = stepScale * (2147483648 / 25000);

  const jerkScale = stepScale * 2147483648 / (25000 ** 3);

  const isrToSteps = 1.0 / 2147483648;

  const motorStepDist = distance(motorSteps1, motorSteps2);

  // Direction of travel, scaled by sqrt(2) for the motor-axis conversion.

  const vNorm1 = Math.sqrt(2) * motorSteps1 / motorStepDist;

  const vNorm2 = Math.sqrt(2) * motorSteps2 / motorStepDist;

  let prevMotor1 = 0;

  let prevMotor2 = 0;

  let prevVel1 = pyRound((vi * velocityScale) * vNorm1);

  let prevVel2 = pyRound((vi * velocityScale) * vNorm2);

  const moveList = [];

  const count = subsegs.length;

  for (let index = 0; index < count; index++) {

    let dest1, dest2, subVf;

    if (index === count - 1) {

      // Last sub-segment always ends at the exact final position and speed.

      dest1 = motorSteps1;

      dest2 = motorSteps2;

      subVf = vf;

    } else {

      subVf = vels[index];

      dest1 = pyRound(motorSteps1 * dists[index] / segmentLength);

      dest2 = pyRound(motorSteps2 * dists[index] / segmentLength);

    }

    const stepsSub1 = dest1 - prevMotor1;

    const stepsSub2 = dest2 - prevMotor2;

    const vel1 = pyRound((subVf * velocityScale) * vNorm1);

    const vel2 = pyRound((subVf * velocityScale) * vNorm2);

    const jerkRate = jerks[index];

    const jerk1 = jerkRate * jerkScale * vNorm1;

    const jerk2 = jerkRate * jerkScale * vNorm2;

    let t1 = 0;

    let t2 = 0;

    const type = subsegs[index];

    if (type === 1 || type === 2) {

      // S-curve: TD = two T3 halves, jerk then counter-jerk.

      const sign = type === 1 ? 1 : -1;

      const j1 = sign * pyRound(jerk1);

      const j2 = sign * pyRound(jerk2);

      if (j1 !== 0) t1 = pyRound(Math.sqrt(Math.abs((vel1 - prevVel1) / j1)));

      if (j2 !== 0) t2 = pyRound(Math.sqrt(Math.abs((vel2 - prevVel2) / j2)));

      const test1 = moveDistT3(t1, prevVel1, 0, j1)[0];

      const test2 = moveDistT3(t2, prevVel2, 0, j2)[0];

      const moveTime = Math.abs(test1) > Math.abs(test2) ? t1 : t2;

      if (moveTime === 0) continue;

      const a1 = pyRound(sign * jerk1 * moveTime);

      const a2 = pyRound(sign * jerk2 * moveTime);

      const r1 = rateT3(moveTime, prevVel1, 0, j1);

      const r2 = rateT3(moveTime, prevVel2, 0, j2);

      const td = [moveTime, prevVel1, r1 + a1, a1, j1, prevVel2, r2 + a2, a2, j2];

      const [s1, s2, inches, nv1, nv2] = plan.tdSegData(td, xyzPos, stepScale);

      prevVel1 = nv1;

      prevVel2 = nv2;

      moveList.push(['TD', td, [inches, copyPos(xyzPos)]]);

      prevMotor1 += s1;

      prevMotor2 += s2;

    } else if (type === 3) {

      if (vel1 !== 0) t1 = Math.ceil(Math.abs(stepsSub1 / (vel1 * isrToSteps)));

      if (vel2 !== 0) t2 = Math.ceil(Math.abs(stepsSub2 / (vel2 * isrToSteps)));

      const test1 = moveDistT3(t1, vel1, 0, 0)[0];

      const test2 = moveDistT3(t2, vel2, 0, 0)[0];

      const moveTime = Math.abs(test1) > Math.abs(test2) ? t1 : t2;

      if (moveTime === 0) continue;

      const t3 = [moveTime, vel1, 0, 0, vel2, 0, 0];

      const [s1, s2, inches, nv1, nv2] = plan.t3SegData(t3, xyzPos, stepScale);

      prevVel1 = nv1;

      prevVel2 = nv2;

      moveList.push(['T3', t3, [inches, copyPos(xyzPos)]]);

      prevMotor1 += s1;

      prevMotor2 += s2;

    }

  }

  return [moveList, [xyzPos]];

}