///|
/// Lucas-Kanade Pyramidal Optical Flow Tracker for high-speed subpixel corner tracking.
/// Allows real-time 60+ FPS marker tracking across consecutive video frames without re-executing full contour search.
pub(all) struct OpticalFlowTracker {
  win_size : Int
  max_level : Int
  max_iterations : Int
  epsilon : Double
  min_eigen_threshold : Double
} derive(Debug, Eq)

///|
/// Creates a new Lucas-Kanade optical flow tracker.
pub fn OpticalFlowTracker::new(
  win_size? : Int = 11,
  max_level? : Int = 3,
  max_iterations? : Int = 20,
  epsilon? : Double = 0.03,
  min_eigen_threshold? : Double = 1.0e-4,
) -> OpticalFlowTracker {
  { win_size, max_level, max_iterations, epsilon, min_eigen_threshold }
}

///|
/// Builds an image pyramid with Gaussian downsampling for multi-scale optical flow.
pub fn build_image_pyramid(img : Image, max_level : Int) -> Array[Image] {
  let pyramid : Array[Image] = [img]
  let mut curr = img

  for _l in 1..<=max_level {
    let next_w = curr.width / 2
    let next_h = curr.height / 2
    if next_w < 8 || next_h < 8 {
      break
    }
    let down = Image::new(next_w, next_h)
    for y in 0.. (Array[Point2D], Array[Bool], Array[Double]) {
  let n = prev_points.length()
  let next_points = prev_points.copy()
  let status = Array::make(n, true)
  let errors = Array::make(n, 0.0)

  let prev_pyr = build_image_pyramid(prev_img, self.max_level)
  let next_pyr = build_image_pyramid(next_img, self.max_level)
  let actual_levels = prev_pyr.length() - 1

  let r = self.win_size / 2

  for i in 0..= 0 {
      let pyr_prev = prev_pyr[level]
      let pyr_next = next_pyr[level]
      let lev_scale = (1 << level).to_double()

      let p_x = pt_prev.x / lev_scale
      let p_y = pt_prev.y / lev_scale

      // Optical flow iteration at current pyramid level
      let mut u_x = g_x
      let mut u_y = g_y

      // Compute spatial gradient matrix G = sum [Ix^2  Ix*Iy; Ix*Iy  Iy^2] over window
      let mut gxx = 0.0
      let mut gyy = 0.0
      let mut gxy = 0.0

      for dy in -r..<=r {
        for dx in -r..<=r {
          let wx = p_x + dx.to_double()
          let wy = p_y + dy.to_double()

          let ix = (
              pyr_prev.sample_bilinear(Point2D::new(wx + 1.0, wy)) -
              pyr_prev.sample_bilinear(Point2D::new(wx - 1.0, wy))
            ) *
            0.5
          let iy = (
              pyr_prev.sample_bilinear(Point2D::new(wx, wy + 1.0)) -
              pyr_prev.sample_bilinear(Point2D::new(wx, wy - 1.0))
            ) *
            0.5

          gxx += ix * ix
          gyy += iy * iy
          gxy += ix * iy
        }
      }

      let det = gxx * gyy - gxy * gxy
      let min_eigen = 0.5 *
        (gxx + gyy - ((gxx - gyy) * (gxx - gyy) + 4.0 * gxy * gxy).sqrt())

      if min_eigen < self.min_eigen_threshold || det.abs() < 1.0e-12 {
        pt_valid = false
        break
      }

      // Iterative LK refinement
      let mut iter = 0
      while iter < self.max_iterations {
        iter += 1
        let mut bx = 0.0
        let mut by = 0.0

        for dy in -r..<=r {
          for dx in -r..<=r {
            let wx = p_x + dx.to_double()
            let wy = p_y + dy.to_double()
            let nx = wx + u_x
            let ny = wy + u_y

            let it = pyr_prev.sample_bilinear(Point2D::new(wx, wy)) -
              pyr_next.sample_bilinear(Point2D::new(nx, ny))

            let ix = (
                pyr_prev.sample_bilinear(Point2D::new(wx + 1.0, wy)) -
                pyr_prev.sample_bilinear(Point2D::new(wx - 1.0, wy))
              ) *
              0.5
            let iy = (
                pyr_prev.sample_bilinear(Point2D::new(wx, wy + 1.0)) -
                pyr_prev.sample_bilinear(Point2D::new(wx, wy - 1.0))
              ) *
              0.5

            bx += it * ix
            by += it * iy
          }
        }

        // Solve G * eta = b
        let eta_x = (gyy * bx - gxy * by) / det
        let eta_y = (gxx * by - gxy * bx) / det

        u_x += eta_x
        u_y += eta_y

        if eta_x * eta_x + eta_y * eta_y < self.epsilon * self.epsilon {
          break
        }
      }

      if level > 0 {
        g_x = u_x * 2.0
        g_y = u_y * 2.0
      } else {
        g_x = u_x
        g_y = u_y
      }

      level -= 1
    }

    if pt_valid {
      next_points[i] = Point2D::new(pt_prev.x + g_x, pt_prev.y + g_y)
      status[i] = true
      errors[i] = (g_x * g_x + g_y * g_y).sqrt()
    } else {
      status[i] = false
      errors[i] = 1.0e6
    }
  }

  (next_points, status, errors)
}