///|
/// A candidate produced by comparing one predicted track with one measurement.
pub struct AssociationCandidate {
  track_id : Int
  measurement_id : Int
  distance : Double
  likelihood : Double
  gated : Bool
  metric : String
} derive(Debug)

///|
/// Construct a candidate and normalize invalid scores.
pub fn AssociationCandidate::new(
  track_id : Int,
  measurement_id : Int,
  distance : Double,
  likelihood : Double,
  gated : Bool,
  metric : String,
) -> AssociationCandidate {
  let safe_distance = if distance.is_nan() ||
    distance.is_inf() ||
    distance < 0.0 {
    1.0e300
  } else {
    distance
  }
  let safe_likelihood = if likelihood.is_nan() || likelihood.is_inf() {
    0.0
  } else if likelihood < 0.0 {
    0.0
  } else if likelihood > 1.0 {
    1.0
  } else {
    likelihood
  }
  {
    track_id,
    measurement_id,
    distance: safe_distance,
    likelihood: safe_likelihood,
    gated,
    metric,
  }
}

///|
pub fn AssociationCandidate::track_id(self : AssociationCandidate) -> Int {
  self.track_id
}

///|
pub fn AssociationCandidate::measurement_id(self : AssociationCandidate) -> Int {
  self.measurement_id
}

///|
pub fn AssociationCandidate::distance(self : AssociationCandidate) -> Double {
  self.distance
}

///|
pub fn AssociationCandidate::likelihood(self : AssociationCandidate) -> Double {
  self.likelihood
}

///|
pub fn AssociationCandidate::gated(self : AssociationCandidate) -> Bool {
  self.gated
}

///|
pub fn AssociationCandidate::metric(self : AssociationCandidate) -> String {
  self.metric
}

///|
/// A decision for a single track after global assignment.
pub struct AssociationDecision {
  track_id : Int
  measurement_id : Int?
  distance : Double
  confidence : Double
  accepted : Bool
  reason : String
} derive(Debug)

///|
pub fn AssociationDecision::accepted(
  track_id : Int,
  measurement_id : Int,
  distance : Double,
  confidence : Double,
  reason : String,
) -> AssociationDecision {
  {
    track_id,
    measurement_id: Some(measurement_id),
    distance: distance.max(0.0),
    confidence: confidence.clamp(min=0.0, max=1.0),
    accepted: true,
    reason,
  }
}

///|
pub fn AssociationDecision::unmatched(
  track_id : Int,
  reason : String,
) -> AssociationDecision {
  {
    track_id,
    measurement_id: None,
    distance: 1.0e300,
    confidence: 0.0,
    accepted: false,
    reason,
  }
}

///|
pub fn AssociationDecision::track_id(self : AssociationDecision) -> Int {
  self.track_id
}

///|
pub fn AssociationDecision::measurement_id(self : AssociationDecision) -> Int? {
  self.measurement_id
}

///|
pub fn AssociationDecision::distance(self : AssociationDecision) -> Double {
  self.distance
}

///|
pub fn AssociationDecision::confidence(self : AssociationDecision) -> Double {
  self.confidence
}

///|
pub fn AssociationDecision::is_accepted(self : AssociationDecision) -> Bool {
  self.accepted
}

///|
pub fn AssociationDecision::reason(self : AssociationDecision) -> String {
  self.reason
}

///|
/// Gating and assignment policy for a sensor update.
pub struct AssociationConfig {
  gate_threshold : Double
  minimum_likelihood : Double
  miss_cost : Double
  allow_reuse : Bool
  prefer_likelihood : Bool
} derive(Debug)

///|
pub fn AssociationConfig::new(
  gate_threshold : Double,
  minimum_likelihood : Double,
  miss_cost : Double,
) -> AssociationConfig {
  {
    gate_threshold: if gate_threshold <= 0.0 || gate_threshold.is_nan() {
      1.0
    } else {
      gate_threshold
    },
    minimum_likelihood: minimum_likelihood.clamp(min=0.0, max=1.0),
    miss_cost: if miss_cost.is_nan() || miss_cost < 0.0 {
      1.0
    } else {
      miss_cost
    },
    allow_reuse: false,
    prefer_likelihood: false,
  }
}

///|
pub fn AssociationConfig::gate_threshold(self : AssociationConfig) -> Double {
  self.gate_threshold
}

///|
pub fn AssociationConfig::minimum_likelihood(
  self : AssociationConfig,
) -> Double {
  self.minimum_likelihood
}

///|
pub fn AssociationConfig::miss_cost(self : AssociationConfig) -> Double {
  self.miss_cost
}

///|
pub fn AssociationConfig::allow_reuse(self : AssociationConfig) -> Bool {
  self.allow_reuse
}

///|
pub fn AssociationConfig::prefer_likelihood(self : AssociationConfig) -> Bool {
  self.prefer_likelihood
}

///|
pub fn AssociationConfig::with_reuse(
  self : AssociationConfig,
  enabled : Bool,
) -> AssociationConfig {
  {
    gate_threshold: self.gate_threshold,
    minimum_likelihood: self.minimum_likelihood,
    miss_cost: self.miss_cost,
    allow_reuse: enabled,
    prefer_likelihood: self.prefer_likelihood,
  }
}

///|
pub fn AssociationConfig::with_likelihood_priority(
  self : AssociationConfig,
  enabled : Bool,
) -> AssociationConfig {
  {
    gate_threshold: self.gate_threshold,
    minimum_likelihood: self.minimum_likelihood,
    miss_cost: self.miss_cost,
    allow_reuse: self.allow_reuse,
    prefer_likelihood: enabled,
  }
}

///|
/// The result of one batch assignment.
pub struct AssociationBatch {
  decisions : Array[AssociationDecision]
  candidates : Array[AssociationCandidate]
  unmatched_measurements : Array[Int]
  total_cost : Double
  accepted_count : Int
} derive(Debug)

///|
pub fn AssociationBatch::new(
  decisions : Array[AssociationDecision],
  candidates : Array[AssociationCandidate],
  unmatched_measurements : Array[Int],
) -> AssociationBatch {
  let mut accepted = 0
  let mut cost = 0.0
  for decision in decisions {
    if decision.is_accepted() {
      accepted = accepted + 1
      cost = cost + decision.distance()
    }
  }
  {
    decisions: decisions.copy(),
    candidates: candidates.copy(),
    unmatched_measurements: unmatched_measurements.copy(),
    total_cost: cost,
    accepted_count: accepted,
  }
}

///|
pub fn AssociationBatch::decisions(
  self : AssociationBatch,
) -> Array[AssociationDecision] {
  self.decisions.copy()
}

///|
pub fn AssociationBatch::candidates(
  self : AssociationBatch,
) -> Array[AssociationCandidate] {
  self.candidates.copy()
}

///|
pub fn AssociationBatch::unmatched_measurements(
  self : AssociationBatch,
) -> Array[Int] {
  self.unmatched_measurements.copy()
}

///|
pub fn AssociationBatch::total_cost(self : AssociationBatch) -> Double {
  self.total_cost
}

///|
pub fn AssociationBatch::accepted_count(self : AssociationBatch) -> Int {
  self.accepted_count
}

///|
pub fn AssociationBatch::acceptance_rate(self : AssociationBatch) -> Double {
  if self.decisions.length() == 0 {
    0.0
  } else {
    self.accepted_count.to_double() / self.decisions.length().to_double()
  }
}

///|
/// Calculate a bounded Euclidean distance.
pub fn association_euclidean_distance(
  expected : Array[Double],
  observed : Array[Double],
) -> Double {
  if expected.length() != observed.length() || expected.length() == 0 {
    1.0e300
  } else {
    let mut sum = 0.0
    for i in 0.. Double {
  if expected.length() != observed.length() ||
    expected.length() != variances.length() ||
    expected.length() == 0 {
    return 1.0e300
  }
  let mut sum = 0.0
  for i in 0.. Double {
  if distance.is_nan() || distance.is_inf() || distance < 0.0 {
    0.0
  } else {
    let safe_scale = scale.max(1.0e-12)
    1.0 / (1.0 + distance / safe_scale * (distance / safe_scale))
  }
}

///|
pub fn association_candidate_from_vectors(
  track_id : Int,
  measurement_id : Int,
  expected : Array[Double],
  observed : Array[Double],
  variances : Array[Double],
  config : AssociationConfig,
) -> AssociationCandidate {
  let distance = if variances.length() == expected.length() {
    association_mahalanobis_distance(expected, observed, variances)
  } else {
    association_euclidean_distance(expected, observed)
  }
  let likelihood = association_likelihood(distance, config.gate_threshold())
  let gated = distance <= config.gate_threshold() &&
    likelihood >= config.minimum_likelihood()
  AssociationCandidate::new(
    track_id,
    measurement_id,
    distance,
    likelihood,
    gated,
    if variances.length() == expected.length() {
      "mahalanobis"
    } else {
      "euclidean"
    },
  )
}

///|
fn association_candidate_better(
  left : AssociationCandidate,
  right : AssociationCandidate,
  prefer_likelihood : Bool,
) -> Bool {
  if prefer_likelihood {
    if left.likelihood() == right.likelihood() {
      left.distance() < right.distance()
    } else {
      left.likelihood() > right.likelihood()
    }
  } else if left.distance() == right.distance() {
    left.likelihood() > right.likelihood()
  } else {
    left.distance() < right.distance()
  }
}

///|
fn association_find_measurement(used : Array[Int], id : Int) -> Bool {
  for value in used {
    if value == id {
      return true
    }
  }
  false
}

///|
/// Greedy global assignment with deterministic tie breaking.
pub fn associate_candidates(
  track_ids : Array[Int],
  measurement_ids : Array[Int],
  candidates : Array[AssociationCandidate],
  config : AssociationConfig,
) -> AssociationBatch {
  let decisions = Array::make(
    track_ids.length(),
    AssociationDecision::unmatched(0, "missing"),
  )
  let used_measurements : Array[Int] = []
  for i, track_id in track_ids {
    let mut found : AssociationCandidate? = None
    for candidate in candidates {
      if candidate.track_id() == track_id && candidate.gated() {
        if config.allow_reuse() ||
          !association_find_measurement(
            used_measurements,
            candidate.measurement_id(),
          ) {
          match found {
            None => found = Some(candidate)
            Some(previous) =>
              if association_candidate_better(
                  candidate,
                  previous,
                  config.prefer_likelihood(),
                ) {
                found = Some(candidate)
              }
          }
        }
      }
    }
    decisions[i] = match found {
      None => AssociationDecision::unmatched(track_id, "no-gated-candidate")
      Some(candidate) => {
        if !config.allow_reuse() {
          used_measurements.push(candidate.measurement_id())
        }
        AssociationDecision::accepted(
          track_id,
          candidate.measurement_id(),
          candidate.distance(),
          candidate.likelihood(),
          "gated",
        )
      }
    }
  }
  let unmatched : Array[Int] = []
  for measurement_id in measurement_ids {
    if !association_find_measurement(used_measurements, measurement_id) {
      unmatched.push(measurement_id)
    }
  }
  AssociationBatch::new(decisions, candidates, unmatched)
}

///|
/// A stable track identity with lifecycle counters and quality history.
pub struct TrackLedgerEntry {
  id : Int
  mut hits : Int
  mut misses : Int
  mut age : Int
  mut score : Double
  mut last_timestamp : Int?
  mut last_measurement : Int?
  mut active : Bool
} derive(Debug)

///|
pub fn TrackLedgerEntry::new(id : Int) -> TrackLedgerEntry {
  {
    id,
    hits: 0,
    misses: 0,
    age: 0,
    score: 0.0,
    last_timestamp: None,
    last_measurement: None,
    active: true,
  }
}

///|
pub fn TrackLedgerEntry::id(self : TrackLedgerEntry) -> Int {
  self.id
}

///|
pub fn TrackLedgerEntry::hits(self : TrackLedgerEntry) -> Int {
  self.hits
}

///|
pub fn TrackLedgerEntry::misses(self : TrackLedgerEntry) -> Int {
  self.misses
}

///|
pub fn TrackLedgerEntry::age(self : TrackLedgerEntry) -> Int {
  self.age
}

///|
pub fn TrackLedgerEntry::score(self : TrackLedgerEntry) -> Double {
  self.score
}

///|
pub fn TrackLedgerEntry::last_timestamp(self : TrackLedgerEntry) -> Int? {
  self.last_timestamp
}

///|
pub fn TrackLedgerEntry::last_measurement(self : TrackLedgerEntry) -> Int? {
  self.last_measurement
}

///|
pub fn TrackLedgerEntry::active(self : TrackLedgerEntry) -> Bool {
  self.active
}

///|
pub fn TrackLedgerEntry::observe(
  self : TrackLedgerEntry,
  timestamp : Int,
  measurement_id : Int?,
  confidence : Double,
  miss_limit : Int,
) -> Unit {
  self.age = self.age + 1
  let safe_confidence = confidence.clamp(min=0.0, max=1.0)
  match measurement_id {
    Some(id) => {
      self.hits = self.hits + 1
      self.misses = 0
      self.score = 0.8 * self.score + 0.2 * safe_confidence
      self.last_timestamp = Some(timestamp)
      self.last_measurement = Some(id)
      self.active = true
    }
    None => {
      self.misses = self.misses + 1
      self.score = 0.95 * self.score
      if self.misses >= miss_limit.max(1) {
        self.active = false
      }
    }
  }
}

///|
pub fn TrackLedgerEntry::reactivate(self : TrackLedgerEntry) -> Unit {
  self.active = true
  self.misses = 0
}

///|
/// A bounded in-memory registry for assignment results.
pub struct TrackLedger {
  mut entries : Array[TrackLedgerEntry]
  capacity : Int
  miss_limit : Int
  mut evictions : Int
} derive(Debug)

///|
pub fn TrackLedger::new(capacity : Int, miss_limit : Int) -> TrackLedger {
  {
    entries: [],
    capacity: capacity.max(1),
    miss_limit: miss_limit.max(1),
    evictions: 0,
  }
}

///|
pub fn TrackLedger::length(self : TrackLedger) -> Int {
  self.entries.length()
}

///|
pub fn TrackLedger::capacity(self : TrackLedger) -> Int {
  self.capacity
}

///|
pub fn TrackLedger::evictions(self : TrackLedger) -> Int {
  self.evictions
}

///|
pub fn TrackLedger::entries(self : TrackLedger) -> Array[TrackLedgerEntry] {
  self.entries.copy()
}

///|
fn TrackLedger::track_ledger_find(self : TrackLedger, id : Int) -> Int {
  for i, entry in self.entries {
    if entry.id() == id {
      return i
    }
  }
  -1
}

///|
pub fn TrackLedger::ensure(self : TrackLedger, id : Int) -> TrackLedgerEntry {
  let index = self.track_ledger_find(id)
  if index >= 0 {
    self.entries[index]
  } else {
    let entry = TrackLedgerEntry::new(id)
    if self.entries.length() >= self.capacity {
      self.entries.remove(0) |> ignore
      self.evictions = self.evictions + 1
    }
    self.entries.push(entry)
    entry
  }
}

///|
pub fn TrackLedger::observe(
  self : TrackLedger,
  timestamp : Int,
  decisions : Array[AssociationDecision],
) -> Unit {
  for decision in decisions {
    let entry = self.ensure(decision.track_id())
    entry.observe(
      timestamp,
      decision.measurement_id(),
      decision.confidence(),
      self.miss_limit,
    )
  }
}

///|
pub fn TrackLedger::active_entries(
  self : TrackLedger,
) -> Array[TrackLedgerEntry] {
  let result : Array[TrackLedgerEntry] = []
  for entry in self.entries {
    if entry.active() {
      result.push(entry)
    }
  }
  result
}

///|
pub fn TrackLedger::inactive_entries(
  self : TrackLedger,
) -> Array[TrackLedgerEntry] {
  let result : Array[TrackLedgerEntry] = []
  for entry in self.entries {
    if !entry.active() {
      result.push(entry)
    }
  }
  result
}

///|
pub fn TrackLedger::find(self : TrackLedger, id : Int) -> TrackLedgerEntry? {
  let index = self.track_ledger_find(id)
  if index < 0 {
    None
  } else {
    Some(self.entries[index])
  }
}

///|
pub fn TrackLedger::remove_inactive(self : TrackLedger) -> Int {
  let kept : Array[TrackLedgerEntry] = []
  let mut removed = 0
  for entry in self.entries {
    if entry.active() {
      kept.push(entry)
    } else {
      removed = removed + 1
    }
  }
  self.entries = kept
  removed
}

///|
/// Return a normalized quality score for a batch.
pub fn association_batch_quality(batch : AssociationBatch) -> Double {
  let acceptance = batch.acceptance_rate()
  let unmatched_penalty = batch.unmatched_measurements().length().to_double()
  let cost_penalty = if batch.accepted_count() == 0 {
    0.0
  } else {
    (batch.total_cost() / batch.accepted_count().to_double()).min(20.0) / 20.0
  }
  (acceptance * (1.0 - 0.5 * cost_penalty) - 0.05 * unmatched_penalty).clamp(
    min=0.0,
    max=1.0,
  )
}

///|
pub fn association_confidence_margin(
  best : AssociationCandidate,
  second : AssociationCandidate?,
) -> Double {
  match second {
    None => best.likelihood()
    Some(other) =>
      (best.distance() - other.distance()).abs() / best.distance().max(1.0e-12)
  }
}

///|
pub fn association_gate_probability(
  distance : Double,
  threshold : Double,
) -> Double {
  if distance < 0.0 || threshold <= 0.0 {
    0.0
  } else {
    (1.0 - distance / threshold).clamp(min=0.0, max=1.0)
  }
}

///|
pub fn association_candidate_summary(
  candidate : AssociationCandidate,
) -> String {
  "track=" +
  candidate.track_id().to_string() +
  ",measurement=" +
  candidate.measurement_id().to_string() +
  ",distance=" +
  candidate.distance().to_string() +
  ",likelihood=" +
  candidate.likelihood().to_string()
}

///|
pub fn association_batch_summary(batch : AssociationBatch) -> String {
  "accepted=" +
  batch.accepted_count().to_string() +
  ",unmatched=" +
  batch.unmatched_measurements().length().to_string() +
  ",quality=" +
  association_batch_quality(batch).to_string()
}

///|
pub fn association_validate_ids(
  track_ids : Array[Int],
  measurement_ids : Array[Int],
) -> Bool {
  for i in 0.. Array[AssociationCandidate] {
  let result : Array[AssociationCandidate] = []
  for i, track_id in track_ids {
    if i < predictions.length() {
      for j, measurement_id in measurement_ids {
        if j < observations.length() {
          result.push(
            association_candidate_from_vectors(
              track_id,
              measurement_id,
              predictions[i],
              observations[j],
              variances,
              config,
            ),
          )
        }
      }
    }
  }
  result
}

///|
pub fn associate_vectors(
  track_ids : Array[Int],
  measurement_ids : Array[Int],
  predictions : Array[Array[Double]],
  observations : Array[Array[Double]],
  variances : Array[Double],
  config : AssociationConfig,
) -> AssociationBatch {
  let candidates = association_pairwise_candidates(
    track_ids, measurement_ids, predictions, observations, variances, config,
  )
  associate_candidates(track_ids, measurement_ids, candidates, config)
}