///|
/// Lifecycle state of a tracked target marker.
pub(all) enum TrackState {
  Tentative
  Confirmed
  Lost
  Deleted
} derive(Debug, Eq)

///|
/// A single marker track record containing ID, filtered pose, and trajectory history.
pub(all) struct MarkerTrack {
  track_id : Int
  marker_id : Int
  mut state : TrackState
  filter : ExtendedKalmanFilter6DOF
  mut hits : Int
  mut age : Int
  mut lost_frames : Int
  history : Array[Point3D]
} derive(Debug)

///|
/// Creates a new MarkerTrack from an initial marker detection.
pub fn MarkerTrack::new(
  track_id : Int,
  marker_id : Int,
  initial_pose : Pose3D,
) -> MarkerTrack {
  let filter = ExtendedKalmanFilter6DOF::new(initial_pose)
  let history : Array[Point3D] = [initial_pose.tvec]
  {
    track_id,
    marker_id,
    state: Tentative,
    filter,
    hits: 1,
    age: 1,
    lost_frames: 0,
    history,
  }
}

///|
/// Multi-target marker tracking engine for continuous video streams.
pub(all) struct MultiMarkerTracker {
  tracks : Array[MarkerTrack]
  mut next_track_id : Int
  max_lost_frames : Int
  min_hits_to_confirm : Int
} derive(Debug)

///|
/// Creates a new MultiMarkerTracker.
pub fn MultiMarkerTracker::new(
  max_lost_frames? : Int = 10,
  min_hits_to_confirm? : Int = 3,
) -> MultiMarkerTracker {
  { tracks: [], next_track_id: 1, max_lost_frames, min_hits_to_confirm }
}

///|
/// Updates all tracks with newly detected markers in the current frame.
pub fn MultiMarkerTracker::update(
  self : MultiMarkerTracker,
  detections : Array[MarkerDetection],
  dt? : Double = 0.033,
) -> Array[MarkerTrack] {
  // 1. Predict all existing tracks
  for t in self.tracks {
    t.filter.predict(dt)
    t.age += 1
  }

  // 2. Associate detections with tracks by marker ID and nearest 3D distance
  let matched_detections = Array::make(detections.length(), false)
  let matched_tracks = Array::make(self.tracks.length(), false)

  for i in 0.. {
            let dist = t.filter.pose().tvec.distance(p.tvec)
            if dist < min_dist {
              min_dist = dist
              best_det_idx = j
            }
          }
          None => ()
        }
      }
    }

    if best_det_idx >= 0 && min_dist < 1.0 {
      matched_tracks[i] = true
      matched_detections[best_det_idx] = true
      let det_pose = detections[best_det_idx].pose
      match det_pose {
        Some(p) => {
          t.filter.update(p)
          t.hits += 1
          t.lost_frames = 0
          if t.hits >= self.min_hits_to_confirm {
            t.state = Confirmed
          }
          t.history.push(t.filter.pose().tvec)
          if t.history.length() > 100 {
            let _ = t.history.remove(0)
          }
        }
        None => ()
      }
    } else {
      t.lost_frames += 1
      if t.lost_frames > self.max_lost_frames {
        t.state = Deleted
      } else {
        t.state = Lost
      }
    }
  }

  // 3. Create new tracks for unassociated detections
  for j in 0.. {
          let new_t = MarkerTrack::new(self.next_track_id, detections[j].id, p)
          self.next_track_id += 1
          self.tracks.push(new_t)
        }
        None => ()
      }
    }
  }

  // 4. Remove deleted tracks
  let active_tracks : Array[MarkerTrack] = []
  for t in self.tracks {
    if t.state != Deleted {
      active_tracks.push(t)
    }
  }
  self.tracks.clear()
  for t in active_tracks {
    self.tracks.push(t)
  }

  self.tracks
}