///|
struct TrackState {
  track_id : Int32
  segment_id : Int64
  first_timestamp_us : Int64
  last_timestamp_us : Int64
  accepted_count : Int32
  state : Array[Double]
  covariance : @kalman.Matrix
  first_filtered : @types.Point2
  last_filtered : @types.Point2
  observed_path_length_m : Double
}

///|
/// Bounded per-session state. Frame validation precedes all mutable work;
/// candidates are committed only after the complete frame succeeds.
pub struct MotionEstimator {
  session_id : String
  config : MotionConfig
  mut tracks : Map[Int32, TrackState]
  mut last_timestamp : Int64?
  mut next_segment : Int64
}

///|
pub fn MotionEstimator::new(
  session_id : String,
  config : MotionConfig,
) -> MotionEstimator raise @types.GroundError {
  if session_id.length() == 0 {
    raise @types.InvalidInput("session_id must not be empty")
  }
  {
    session_id,
    config,
    tracks: Map([]),
    last_timestamp: None,
    next_segment: 1L,
  }
}

///|
pub fn MotionEstimator::active_track_count(self : MotionEstimator) -> Int32 {
  self.tracks.length()
}

///|
pub fn MotionEstimator::last_timestamp_us(self : MotionEstimator) -> Int64? {
  self.last_timestamp
}

///|
fn validate_ids(
  ids : Array[Int32],
  seen : Map[Int32, Bool],
) -> Unit raise @types.GroundError {
  for id in ids {
    if id <= 0 || seen.contains(id) {
      raise @types.InvalidInput(
        "duplicate, conflicting, or nonpositive track identity",
      )
    }
    seen[id] = true
  }
}

///|
fn initial_track(
  observation : @types.GroundObservation,
  timestamp_us : Int64,
  segment_id : Int64,
  config : MotionConfig,
) -> TrackState {
  let r = observation.covariance
  let vv = config.initial_velocity_std * config.initial_velocity_std
  {
    track_id: observation.track_id,
    segment_id,
    first_timestamp_us: timestamp_us,
    last_timestamp_us: timestamp_us,
    accepted_count: 1,
    state: [observation.position.x, observation.position.y, 0.0, 0.0],
    covariance: @kalman.Matrix::from_rows([
      [r.xx, r.xy, 0.0, 0.0],
      [r.xy, r.yy, 0.0, 0.0],
      [0.0, 0.0, vv, 0.0],
      [0.0, 0.0, 0.0, vv],
    ]),
    first_filtered: observation.position,
    last_filtered: observation.position,
    observed_path_length_m: 0.0,
  }
}

///|
fn make_estimate(track : TrackState, status : MotionStatus) -> TrackEstimate {
  let accepted = status is Initializing ||
    status is Ready ||
    status is Uncertain
  let ready = status is Ready
  let observable = ready || status is Uncertain
  {
    track_id: track.track_id,
    segment_id: track.segment_id.to_string(),
    status,
    position: if accepted {
      Some({ x: track.state[0], y: track.state[1], })
    } else {
      None
    },
    position_covariance: if accepted {
      covariance_block(track.covariance, 0)
    } else {
      None
    },
    velocity: if observable {
      Some({ x: track.state[2], y: track.state[3], })
    } else {
      None
    },
    velocity_covariance: if observable {
      covariance_block(track.covariance, 2)
    } else {
      None
    },
    position_std_m: if accepted {
      covariance_std(track.covariance, 0)
    } else {
      None
    },
    velocity_std_mps: if observable {
      covariance_std(track.covariance, 2)
    } else {
      None
    },
    speed_mps: if ready {
      Some(norm2(track.state[2], track.state[3]))
    } else {
      None
    },
    observed_path_length_m: track.observed_path_length_m,
    net_displacement_m: norm2(
      track.last_filtered.x - track.first_filtered.x,
      track.last_filtered.y - track.first_filtered.y,
    ),
    accepted_count: track.accepted_count,
    last_accepted_timestamp_us: track.last_timestamp_us,
    filtered_state: if accepted {
      Some(track.state.copy())
    } else {
      None
    },
    filtered_covariance: if accepted {
      Some(flatten_covariance(track.covariance))
    } else {
      None
    },
  }
}

///|
fn readiness(track : TrackState, config : MotionConfig) -> MotionStatus {
  if track.accepted_count >= config.min_observations &&
    track.last_timestamp_us - track.first_timestamp_us >= config.min_span_us {
    if config.max_position_std_m is Some(limit) {
      match covariance_std(track.covariance, 0) {
        None => return Uncertain
        Some(deviation) if deviation > limit => return Uncertain
        _ => ()
      }
    }
    if config.max_velocity_std_mps is Some(limit) {
      match covariance_std(track.covariance, 2) {
        None => return Uncertain
        Some(deviation) if deviation > limit => return Uncertain
        _ => ()
      }
    }
    Ready
  } else {
    Initializing
  }
}

///|
fn advance_track(
  previous : TrackState,
  observation : @types.GroundObservation,
  timestamp_us : Int64,
  config : MotionConfig,
) -> (TrackState, MotionStatus) raise @types.GroundError {
  let dt = (timestamp_us - previous.last_timestamp_us).to_double() / 1000000.0
  let transition = @kalman.constant_velocity_transition(2, dt)
  let process = process_matrix(dt, config.acceleration_spectral_density)
  let measurement = measurement_matrix(observation.covariance)
  if !transition.is_finite() || !process.is_finite() {
    return (previous, NumericalFailure)
  }
  let candidate = @kalman.KalmanND::new(
    previous.state,
    previous.covariance.to_rows(),
    process.to_rows(),
    measurement.to_rows(),
    transition.to_rows(),
    observation_matrix().to_rows(),
  )
  candidate.set_gate_threshold(config.innovation_gate)
  candidate.predict()
  if !healthy_state(candidate.state(), candidate.covariance()) {
    return (previous, NumericalFailure)
  }
  let report = candidate.update_and_report([
    observation.position.x,
    observation.position.y,
  ])
  if !@types.is_finite(report.nis()) {
    return (previous, NumericalFailure)
  }
  match report.result() {
    @kalman.RejectedByGate => (previous, RejectedInnovation)
    @kalman.Accepted => {
      let state = candidate.state()
      let covariance = candidate.covariance()
      let step = norm2(
        state[0] - previous.last_filtered.x,
        state[1] - previous.last_filtered.y,
      )
      let path = previous.observed_path_length_m + step
      let displacement = norm2(
        state[0] - previous.first_filtered.x,
        state[1] - previous.first_filtered.y,
      )
      if !healthy_state(state, covariance) ||
        !@types.is_finite(path) ||
        !@types.is_finite(displacement) ||
        covariance_block(covariance, 0) is None ||
        covariance_block(covariance, 2) is None ||
        !@types.is_finite(norm2(state[2], state[3])) {
        return (previous, NumericalFailure)
      }
      if previous.accepted_count == 2147483647 {
        raise @types.InvalidInput("accepted observation counter exhausted")
      }
      let next = {
        ..previous,
        last_timestamp_us: timestamp_us,
        accepted_count: previous.accepted_count + 1,
        state,
        covariance,
        last_filtered: { x: state[0], y: state[1], },
        observed_path_length_m: path,
      }
      (next, readiness(next, config))
    }
    _ => (previous, NumericalFailure)
  }
}

///|
/// Apply one globally increasing frame atomically. Missing tracks produce no
/// prediction output. Lost/removed identities and accepted-observation gaps at
/// least max_gap_us close and release the segment without retaining tombstones.
pub fn MotionEstimator::update(
  self : MotionEstimator,
  timestamp_us : Int64,
  observations : Array[@types.GroundObservation],
  lost~ : Array[Int32] = [],
  removed~ : Array[Int32] = [],
) -> FrameResult raise @types.GroundError {
  if timestamp_us < 0L {
    raise @types.InvalidInput("timestamp_us must be nonnegative")
  }
  if self.last_timestamp is Some(previous) && timestamp_us <= previous {
    raise @types.InvalidInput("frame timestamps must strictly increase")
  }
  if observations.length() > self.config.max_tracks ||
    lost.length() > self.config.max_tracks ||
    removed.length() > self.config.max_tracks {
    raise @types.InvalidInput("frame identity limit exceeded")
  }
  let seen : Map[Int32, Bool] = Map([])
  let ordered = observations.copy()
  ordered.sort_by((a, b) => a.track_id.compare(b.track_id))
  for observation in ordered {
    observation.validate()
    validate_ids([observation.track_id], seen)
  }
  validate_ids(lost, seen)
  validate_ids(removed, seen)
  let closing : Map[Int32, Bool] = Map([])
  for id in lost {
    closing[id] = true
  }
  for id in removed {
    closing[id] = true
  }
  let staged : Map[Int32, TrackState] = Map([])
  let closed : Array[Int64] = []
  for id, track in self.tracks {
    if closing.contains(id) ||
      timestamp_us - track.last_timestamp_us >= self.config.max_gap_us {
      closed.push(track.segment_id)
    } else {
      staged[id] = track
    }
  }
  let results : Array[TrackEstimate] = []
  let mut next_segment = self.next_segment
  for observation in ordered {
    match staged.get(observation.track_id) {
      Some(previous) => {
        let (next, status) = advance_track(
          previous,
          observation,
          timestamp_us,
          self.config,
        )
        staged[observation.track_id] = next
        results.push(make_estimate(next, status))
      }
      None => {
        if staged.length() >= self.config.max_tracks {
          raise @types.InvalidInput("active track capacity exceeded")
        }
        if next_segment == 9223372036854775807L {
          raise @types.InvalidInput("segment identity counter exhausted")
        }
        let initial = initial_track(
          observation,
          timestamp_us,
          next_segment,
          self.config,
        )
        if !healthy_state(initial.state, initial.covariance) {
          results.push(make_estimate(initial, NumericalFailure))
        } else {
          staged[observation.track_id] = initial
          results.push(make_estimate(initial, Initializing))
        }
        next_segment = next_segment + 1L
      }
    }
  }
  results.sort_by((a, b) => a.track_id.compare(b.track_id))
  closed.sort()
  self.tracks = staged
  self.last_timestamp = Some(timestamp_us)
  self.next_segment = next_segment
  {
    session_id: self.session_id,
    timestamp_us,
    tracks: results,
    closed_segment_ids: closed.map(id => id.to_string()),
  }
}