import { FEATURE } from './featureTypes.js'

// Simplified Klipper-style motion planner. For every segment it computes the
// trapezoid the firmware would run given the requested feedrate, the active
// acceleration and the junction limits between consecutive moves, which yields
// the speed the toolhead actually reaches and how long each segment takes.
//
// It ignores extruder-specific limits, minimum cruise ratio smoothing and
// input shaper, so times are approximate but relative values are faithful.

const SQRT2_MINUS_1 = Math.SQRT2 - 1

export function planMotion(store, options = {}) {
  const {
    maxVelocity = 500,
    squareCornerVelocity = 5,
    defaultAccel = 3000
  } = options

  const n = store.count
  const positions = store.positions
  const feed = store.feed
  const accelField = store.accel
  const type = store.type

  const distance = new Float32Array(n)
  const deltaV2 = new Float32Array(n)
  const maxStartV2 = new Float32Array(n)
  const maxCruiseV2 = new Float32Array(n)
  const startV2 = new Float32Array(n)
  const actualSpeed = new Float32Array(n)
  const segmentTime = new Float32Array(n)

  const scv2 = squareCornerVelocity * squareCornerVelocity

  let prevValid = false
  let prevDx = 0
  let prevDy = 0
  let prevDz = 0
  let prevAccel = defaultAccel
  let prevJunctionDeviation = 0

  for (let i = 0; i < n; i++) {
    const p = i * 3
    const dx = positions[p + 3] - positions[p]
    const dy = positions[p + 4] - positions[p + 1]
    const dz = positions[p + 5] - positions[p + 2]
    const d = Math.sqrt(dx * dx + dy * dy + dz * dz)
    distance[i] = d

    if (type[i] === FEATURE.NONE || d <= 0) {
      // Position jump: no motion, acts as a full stop.
      deltaV2[i] = 0
      maxStartV2[i] = 0
      maxCruiseV2[i] = 0
      prevValid = false
      continue
    }

    let accel = accelField[i]
    if (!(accel > 0)) accel = defaultAccel
    let vReq = feed[i]
    if (!(vReq > 0)) vReq = maxVelocity
    if (vReq > maxVelocity) vReq = maxVelocity
    const cruiseV2 = vReq * vReq
    maxCruiseV2[i] = cruiseV2
    deltaV2[i] = 2 * d * accel
    const junctionDeviation = (scv2 * SQRT2_MINUS_1) / accel

    const ux = dx / d
    const uy = dy / d
    const uz = dz / d

    if (!prevValid) {
      maxStartV2[i] = 0
    } else {
      let cosTheta = -(ux * prevDx + uy * prevDy + uz * prevDz)
      if (cosTheta > 0.999999) {
        maxStartV2[i] = 0
      } else {
        if (cosTheta < -0.999999) cosTheta = -0.999999
        const sinThetaD2 = Math.sqrt(0.5 * (1 - cosTheta))
        const rJd = sinThetaD2 / (1 - sinThetaD2)
        let limit = rJd * junctionDeviation * accel
        const prevLimit = rJd * prevJunctionDeviation * prevAccel
        if (prevLimit < limit) limit = prevLimit
        if (cruiseV2 < limit) limit = cruiseV2
        const prevCruise = maxCruiseV2[i - 1]
        if (prevCruise < limit) limit = prevCruise
        const reachable = maxStartV2[i - 1] + deltaV2[i - 1]
        if (reachable < limit) limit = reachable
        maxStartV2[i] = limit
      }
    }

    prevValid = true
    prevDx = ux
    prevDy = uy
    prevDz = uz
    prevAccel = accel
    prevJunctionDeviation = junctionDeviation
  }

  // Backward pass: nothing may end faster than it can decelerate into the
  // next junction (the file ends at rest).
  let nextStartV2 = 0
  for (let i = n - 1; i >= 0; i--) {
    const reachable = nextStartV2 + deltaV2[i]
    const v2 = maxStartV2[i] < reachable ? maxStartV2[i] : reachable
    startV2[i] = v2
    nextStartV2 = v2
  }

  // Forward pass and trapezoid generation.
  let currentStartV2 = 0
  for (let i = 0; i < n; i++) {
    const d = distance[i]
    if (type[i] === FEATURE.NONE || d <= 0) {
      currentStartV2 = 0
      continue
    }
    let sV2 = startV2[i] < currentStartV2 ? startV2[i] : currentStartV2
    const nextS = i + 1 < n ? startV2[i + 1] : 0
    const reachableEnd = sV2 + deltaV2[i]
    let eV2 = nextS < reachableEnd ? nextS : reachableEnd
    if (eV2 < 0) eV2 = 0
    let cV2 = (sV2 + eV2 + deltaV2[i]) * 0.5
    if (cV2 > maxCruiseV2[i]) cV2 = maxCruiseV2[i]
    if (cV2 < sV2) cV2 = sV2
    if (cV2 < eV2) cV2 = eV2

    let accel = accelField[i]
    if (!(accel > 0)) accel = defaultAccel
    const sV = Math.sqrt(sV2)
    const eV = Math.sqrt(eV2)
    const cV = Math.sqrt(cV2)
    const halfInvAccel = 0.5 / accel
    const accelD = (cV2 - sV2) * halfInvAccel
    const decelD = (cV2 - eV2) * halfInvAccel
    let cruiseD = d - accelD - decelD
    if (cruiseD < 0) cruiseD = 0
    const t =
      (cV - sV) / accel + (cV > 0 ? cruiseD / cV : 0) + (cV - eV) / accel

    actualSpeed[i] = cV
    segmentTime[i] = t
    currentStartV2 = eV2
  }

  return { actualSpeed, segmentTime }
}

// Sums segment times per layer and expands them back to one value per
// segment so the shader can colour by layer time with a plain attribute.
export function computeLayerTimes(store, segmentTime, layers) {
  const n = store.count
  const layerTime = new Float32Array(n)
  const perLayer = new Float32Array(layers.length)
  let total = 0
  for (let l = 0; l < layers.length; l++) {
    const { start, end } = layers[l]
    let sum = 0
    for (let i = start; i < end && i < n; i++) sum += segmentTime[i]
    perLayer[l] = sum
    total += sum
    for (let i = start; i < end && i < n; i++) layerTime[i] = sum
  }
  return { layerTime, perLayer, total }
}