import * as Cesium from 'cesium'

export interface AnchorPoint {
  id: string
  position: Cesium.Cartesian3  // base position on terrain
  heightOffset: number          // meters above terrain surface
}

// ── helpers ──────────────────────────────────────────────────────────────────

function v3add(a: Cesium.Cartesian3, b: Cesium.Cartesian3): Cesium.Cartesian3 {
  return Cesium.Cartesian3.add(a, b, new Cesium.Cartesian3())
}
function v3sub(a: Cesium.Cartesian3, b: Cesium.Cartesian3): Cesium.Cartesian3 {
  return Cesium.Cartesian3.subtract(a, b, new Cesium.Cartesian3())
}
function v3scale(v: Cesium.Cartesian3, s: number): Cesium.Cartesian3 {
  return Cesium.Cartesian3.multiplyByScalar(v, s, new Cesium.Cartesian3())
}
function v3norm(v: Cesium.Cartesian3): Cesium.Cartesian3 {
  return Cesium.Cartesian3.normalize(v, new Cesium.Cartesian3())
}
function v3cross(a: Cesium.Cartesian3, b: Cesium.Cartesian3): Cesium.Cartesian3 {
  return Cesium.Cartesian3.cross(a, b, new Cesium.Cartesian3())
}

/** Return the effective 3-D position of an anchor including its height offset. */
export function effectivePosition(anchor: AnchorPoint): Cesium.Cartesian3 {
  if (anchor.heightOffset === 0) return anchor.position.clone()
  const up = v3norm(anchor.position)
  return v3add(anchor.position, v3scale(up, anchor.heightOffset))
}

// ── Catmull-Rom → cubic Bézier ───────────────────────────────────────────────

interface Segment {
  p0: Cesium.Cartesian3
  c1: Cesium.Cartesian3
  c2: Cesium.Cartesian3
  p1: Cesium.Cartesian3
}

/**
 * Convert anchor points to cubic Bézier segments via Catmull-Rom.
 * The curve passes through every anchor point with C1 continuity.
 */
export function computeSegments(anchors: AnchorPoint[]): Segment[] {
  if (anchors.length < 2) return []
  const pts = anchors.map(effectivePosition)
  const n = pts.length
  // phantom end-points so the curve starts/ends at the first/last anchor
  const ext = [pts[0], ...pts, pts[n - 1]]

  const segments: Segment[] = []
  for (let i = 0; i < n - 1; i++) {
    const pm1 = ext[i]
    const p0  = ext[i + 1]
    const p1  = ext[i + 2]
    const p2  = ext[i + 3]
    // Catmull-Rom tangent handles converted to cubic Bézier control points
    const c1 = v3add(p0, v3scale(v3sub(p1, pm1), 1 / 6))
    const c2 = v3add(p1, v3scale(v3sub(p0, p2), 1 / 6))
    segments.push({ p0, c1, c2, p1 })
  }
  return segments
}

function evalBezier(seg: Segment, t: number): Cesium.Cartesian3 {
  const { p0, c1, c2, p1 } = seg
  const mt = 1 - t
  return new Cesium.Cartesian3(
    mt ** 3 * p0.x + 3 * mt ** 2 * t * c1.x + 3 * mt * t ** 2 * c2.x + t ** 3 * p1.x,
    mt ** 3 * p0.y + 3 * mt ** 2 * t * c1.y + 3 * mt * t ** 2 * c2.y + t ** 3 * p1.y,
    mt ** 3 * p0.z + 3 * mt ** 2 * t * c1.z + 3 * mt * t ** 2 * c2.z + t ** 3 * p1.z,
  )
}

/** Sample the full spline as a polyline (all segments concatenated). */
export function samplePath(
  anchors: AnchorPoint[],
  samplesPerSegment = 40,
): Cesium.Cartesian3[] {
  const segs = computeSegments(anchors)
  if (segs.length === 0) return anchors.map(effectivePosition)

  const pts: Cesium.Cartesian3[] = []
  segs.forEach((seg, i) => {
    const from = i === 0 ? 0 : 1
    for (let s = from; s <= samplesPerSegment; s++) {
      pts.push(evalBezier(seg, s / samplesPerSegment))
    }
  })
  return pts
}

// ── Rail geometry ─────────────────────────────────────────────────────────────

export interface RailPositions {
  left: Cesium.Cartesian3[]
  right: Cesium.Cartesian3[]
}

/**
 * Given a centre-line path compute parallel left/right rail positions.
 * @param gauge  distance between rails in metres (default 2.5 for visual clarity)
 */
export function computeRails(
  path: Cesium.Cartesian3[],
  gauge = 2.5,
): RailPositions {
  const left: Cesium.Cartesian3[] = []
  const right: Cesium.Cartesian3[] = []
  const half = gauge / 2
  const n = path.length

  for (let i = 0; i < n; i++) {
    const pt = path[i]

    // Finite-difference tangent along the curve
    let tangent: Cesium.Cartesian3
    if (i === 0)         tangent = v3sub(path[1], path[0])
    else if (i === n - 1) tangent = v3sub(path[n - 1], path[n - 2])
    else                  tangent = v3sub(path[i + 1], path[i - 1])
    tangent = v3norm(tangent)

    // Local up = radially outward from Earth centre
    const up = v3norm(pt)
    // Track-right = tangent × up  (right-hand rule → points right when facing forward)
    const rightDir = v3norm(v3cross(tangent, up))

    left.push(v3add(pt, v3scale(rightDir, -half)))
    right.push(v3add(pt, v3scale(rightDir,  half)))
  }

  return { left, right }
}