use super::channel::{Channel, ChannelInit, EpochOutput};
use super::design::{AcquisitionDesign, Design, LockConfig};
use super::sink::EpochSink;
use super::LoopConfig;
use crate::iq::acq::{acquire, samples_needed, AcqConfig};
use crate::iq::{Cf64, IqError, IqSource, SampleSpec};
use crate::portable_math::PortableFloat;
use serde::{Deserialize, Serialize};
const CHUNK: usize = 1 << 16;
#[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize)]
#[serde(rename_all = "SCREAMING_SNAKE_CASE")]
pub enum LockState {
PullIn,
Locked,
Lost,
Reacq,
Retired,
}
impl LockState {
pub fn as_str(self) -> &'static str {
match self {
LockState::PullIn => "PULL_IN",
LockState::Locked => "LOCKED",
LockState::Lost => "LOST",
LockState::Reacq => "REACQ",
LockState::Retired => "RETIRED",
}
}
pub fn code(self) -> u8 {
self as u8
}
pub fn from_code(c: u8) -> Option<Self> {
Some(match c {
0 => LockState::PullIn,
1 => LockState::Locked,
2 => LockState::Lost,
3 => LockState::Reacq,
4 => LockState::Retired,
_ => return None,
})
}
}
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
pub struct LockEvent {
pub channel: u32,
pub epoch: u64,
pub sample_index: u64,
pub code_epoch_s: f64,
pub from: LockState,
pub to: LockState,
pub reason: String,
#[serde(default, skip_serializing_if = "Option::is_none")]
pub doppler_hz: Option<f64>,
#[serde(default, skip_serializing_if = "Option::is_none")]
pub code_phase_chips: Option<f64>,
#[serde(default, skip_serializing_if = "Option::is_none")]
pub statistic: Option<f64>,
#[serde(default, skip_serializing_if = "Option::is_none")]
pub threshold: Option<f64>,
}
#[derive(Clone, Debug)]
pub struct SessionChannel {
pub init: ChannelInit,
pub config: LoopConfig,
pub lock: LockConfig,
pub acquisition: AcquisitionDesign,
}
impl SessionChannel {
pub fn from_design(init: ChannelInit, d: &Design) -> Self {
let config = d.loop_config_for(init.code.period_s());
Self {
init,
config,
lock: d.lock_config(),
acquisition: d.acquisition().clone(),
}
}
pub fn from_config(init: ChannelInit, config: LoopConfig) -> Self {
Self {
init,
config,
lock: LockConfig::default(),
acquisition: AcquisitionDesign::default(),
}
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
enum Purpose {
FalseLock { center: f64, alias_hz: f64 },
Reacq { center: f64 },
}
#[derive(Clone, Debug)]
struct Collect {
purpose: Purpose,
start: u64,
cfg: AcqConfig,
need: usize,
buf: Vec<Cf64>,
}
struct Managed {
setup: SessionChannel,
ch: Option<Channel>,
state: LockState,
state_since_s: f64,
good_since: Option<f64>,
bad_since: Option<f64>,
updates_since_check: usize,
attempts: u32,
lost_since: Option<f64>,
next_attempt_s: f64,
reacq_interval_s: f64,
suspect: bool,
check_unavailable: bool,
reacq_center: f64,
next_epoch: u64,
last_t: f64,
loop_t: f64,
collect: Option<Collect>,
scratch: Vec<EpochOutput>,
}
pub struct TrackSession {
spec: SampleSpec,
chans: Vec<Managed>,
consumed: u64,
threads: usize,
}
impl TrackSession {
pub fn new(spec: SampleSpec, channels: Vec<SessionChannel>) -> Result<Self, String> {
let chans = channels
.into_iter()
.map(|setup| {
let ch = Channel::new(&spec, &setup.init, &setup.config)?;
Ok(Managed {
reacq_center: setup.init.doppler_hz,
setup,
ch: Some(ch),
state: LockState::PullIn,
state_since_s: 0.0,
good_since: None,
bad_since: None,
updates_since_check: 0,
attempts: 0,
lost_since: None,
next_attempt_s: 0.0,
reacq_interval_s: 0.0,
suspect: false,
check_unavailable: false,
next_epoch: 0,
last_t: 0.0,
loop_t: 0.0,
collect: None,
scratch: Vec::new(),
})
})
.collect::<Result<Vec<_>, String>>()?;
Ok(Self {
spec,
chans,
consumed: 0,
threads: 1,
})
}
pub fn with_threads(mut self, n: usize) -> Self {
self.threads = n.max(1);
self
}
pub fn process(&mut self, samples: &[Cf64], sink: &mut dyn EpochSink) -> Result<(), IqError> {
let chunk_start = self.consumed;
let chunk_end = chunk_start + samples.len() as u64;
let spec = self.spec;
#[cfg(not(target_arch = "wasm32"))]
if self.threads > 1 && self.chans.len() > 1 {
self.process_parallel(samples, chunk_start, chunk_end, sink)?;
self.consumed = chunk_end;
return Ok(());
}
for (idx, m) in self.chans.iter_mut().enumerate() {
m.advance(idx, samples, chunk_start, chunk_end, &spec, sink)?;
}
self.consumed = chunk_end;
Ok(())
}
#[cfg(not(target_arch = "wasm32"))]
fn process_parallel(
&mut self,
samples: &[Cf64],
chunk_start: u64,
chunk_end: u64,
sink: &mut dyn EpochSink,
) -> Result<(), IqError> {
let spec = self.spec;
let n = self.threads.min(self.chans.len());
let mut groups: Vec<Vec<(usize, &mut Managed)>> = (0..n).map(|_| Vec::new()).collect();
for (idx, m) in self.chans.iter_mut().enumerate() {
groups[idx % n].push((idx, m));
}
let mut done: Vec<(usize, Buffered, Result<(), IqError>)> = std::thread::scope(|sc| {
let handles: Vec<_> = groups
.into_iter()
.map(|g| {
sc.spawn(move || {
g.into_iter()
.map(|(idx, m)| {
let mut buf = Buffered::default();
let r = m.advance(
idx,
samples,
chunk_start,
chunk_end,
&spec,
&mut buf,
);
(idx, buf, r)
})
.collect::<Vec<_>>()
})
})
.collect();
handles
.into_iter()
.flat_map(|h| h.join().expect("a tracking thread panicked"))
.collect()
});
done.sort_by_key(|d| d.0);
for (_, buf, r) in done {
buf.replay(sink)?;
r?;
}
Ok(())
}
pub fn run(
&mut self,
src: &mut dyn IqSource,
max_samples: Option<u64>,
sink: &mut dyn EpochSink,
) -> Result<(), IqError> {
let mut buf = vec![Cf64::default(); CHUNK];
let mut done = 0u64;
loop {
let want = match max_samples {
Some(m) => (m.saturating_sub(done)).min(CHUNK as u64) as usize,
None => CHUNK,
};
if want == 0 {
break;
}
let n = src.read(&mut buf[..want])?;
if n == 0 {
break;
}
self.process(&buf[..n], sink)?;
done += n as u64;
}
sink.finish()
}
}
#[derive(Default)]
struct Buffered(Vec<Item>);
enum Item {
Epoch(usize, Box<EpochOutput>, LockState),
Event(Box<LockEvent>),
}
impl Buffered {
fn replay(self, sink: &mut dyn EpochSink) -> Result<(), IqError> {
for it in self.0 {
match it {
Item::Epoch(ch, e, st) => sink.epoch(ch, &e, st)?,
Item::Event(ev) => sink.event(&ev)?,
}
}
Ok(())
}
}
impl EpochSink for Buffered {
fn epoch(&mut self, ch: usize, e: &EpochOutput, state: LockState) -> Result<(), IqError> {
self.0.push(Item::Epoch(ch, Box::new(e.clone()), state));
Ok(())
}
fn event(&mut self, ev: &LockEvent) -> Result<(), IqError> {
self.0.push(Item::Event(Box::new(ev.clone())));
Ok(())
}
}
impl Managed {
fn advance(
&mut self,
idx: usize,
samples: &[Cf64],
chunk_start: u64,
chunk_end: u64,
spec: &SampleSpec,
sink: &mut dyn EpochSink,
) -> Result<(), IqError> {
let m = self;
if let Some(ch) = m.ch.as_mut() {
m.scratch.clear();
ch.process(samples, &mut m.scratch);
let epochs = std::mem::take(&mut m.scratch);
for e in &epochs {
m.on_epoch(idx, e, chunk_end, spec, sink)?;
sink.epoch(idx, e, m.state)?;
}
m.scratch = epochs;
}
if let Some(c) = m.collect.as_mut() {
if c.start >= chunk_start && c.start < chunk_end {
let from = (c.start - chunk_start) as usize;
let take = (c.need - c.buf.len()).min(samples.len() - from);
c.buf.extend_from_slice(&samples[from..from + take]);
} else if c.start < chunk_start && c.buf.len() < c.need {
let take = (c.need - c.buf.len()).min(samples.len());
c.buf.extend_from_slice(&samples[..take]);
}
}
if m.collect.as_ref().is_some_and(|c| c.buf.len() >= c.need) {
let c = m.collect.take().expect("checked above");
m.on_search(idx, c, chunk_end, spec, sink)?;
}
Ok(())
}
#[allow(clippy::too_many_arguments)]
fn transition(
&mut self,
idx: usize,
to: LockState,
reason: &str,
at_sample: u64,
extra: (Option<f64>, Option<f64>, Option<f64>, Option<f64>),
sink: &mut dyn EpochSink,
) -> Result<(), IqError> {
let from = self.state;
self.state = to;
self.state_since_s = self.last_t;
self.good_since = None;
self.bad_since = None;
if to == LockState::Locked {
self.attempts = 0;
self.lost_since = None;
}
if to == LockState::Lost && self.lost_since.is_none() {
self.lost_since = Some(self.last_t);
self.next_attempt_s = self.last_t;
self.reacq_interval_s = self.setup.lock.reacq_interval_s;
}
sink.event(&LockEvent {
channel: idx as u32,
epoch: self.next_epoch,
sample_index: at_sample,
code_epoch_s: self.last_t,
from,
to,
reason: reason.into(),
doppler_hz: extra.0,
code_phase_chips: extra.1,
statistic: extra.2,
threshold: extra.3,
})
}
fn on_epoch(
&mut self,
idx: usize,
e: &EpochOutput,
chunk_end: u64,
spec: &SampleSpec,
sink: &mut dyn EpochSink,
) -> Result<(), IqError> {
let lk = self.setup.lock.clone();
let t = e.code_epoch_s;
self.last_t = t;
self.loop_t = e.t_coh_s;
self.next_epoch = e.epoch + 1;
let both = e.phase_lock && e.code_lock;
let held = |since: &mut Option<f64>, cond: bool| -> bool {
if cond {
let s = *since.get_or_insert(t);
t - s >= lk.loss_dwell_s
} else {
*since = None;
false
}
};
match self.state {
LockState::PullIn => {
if held(&mut self.good_since, both) {
self.transition(
idx,
LockState::Locked,
"locked",
e.sample_index,
(None, None, None, None),
sink,
)?;
} else if t - self.state_since_s > lk.pull_in_max_s {
self.reacq_center = e.doppler_hz;
self.transition(
idx,
LockState::Lost,
"pull-in-timeout",
e.sample_index,
(None, None, None, None),
sink,
)?;
}
}
LockState::Locked => {
if held(&mut self.bad_since, !both) {
self.reacq_center = e.doppler_hz;
self.transition(
idx,
LockState::Lost,
"loss-of-lock",
e.sample_index,
(None, None, None, None),
sink,
)?;
}
}
LockState::Lost => {
if lk.reacquire {
if self.window_spent(t) {
self.ch = None;
return self.transition(
idx,
LockState::Retired,
"retired",
e.sample_index,
(None, None, None, None),
sink,
);
}
if self.collect.is_none() && t >= self.next_attempt_s {
let searchable = self.start(
Purpose::Reacq {
center: self.reacq_center,
},
chunk_end,
spec,
)?;
if !searchable {
self.ch = None;
return self.transition(
idx,
LockState::Retired,
"retired",
e.sample_index,
(None, None, None, None),
sink,
);
}
self.transition(
idx,
LockState::Reacq,
"reacq-start",
e.sample_index,
(Some(self.reacq_center), None, None, None),
sink,
)?;
}
} else if !self.suspect && held(&mut self.good_since, both) {
self.transition(
idx,
LockState::Locked,
"recovered",
e.sample_index,
(None, None, None, None),
sink,
)?;
}
}
LockState::Reacq | LockState::Retired => {}
}
let watching = matches!(
self.state,
LockState::PullIn | LockState::Locked | LockState::Lost
);
if lk.false_lock_check && watching && !self.check_unavailable {
self.updates_since_check += 1;
if self.updates_since_check >= self.setup.config.cn0_windows && self.collect.is_none() {
self.updates_since_check = 0;
let alias_hz = 1.0 / (2.0 * e.t_coh_s);
let searchable = self.start(
Purpose::FalseLock {
center: e.doppler_hz,
alias_hz,
},
chunk_end,
spec,
)?;
if !searchable {
self.check_unavailable = true;
}
}
}
Ok(())
}
fn window_spent(&self, t: f64) -> bool {
self.lost_since
.is_some_and(|s| t - s >= self.setup.lock.reacq_window_s)
}
fn reacq_failed(
&mut self,
idx: usize,
now: u64,
stat: Option<(f64, f64)>,
sink: &mut dyn EpochSink,
) -> Result<(), IqError> {
self.attempts += 1;
let extra = (None, None, stat.map(|s| s.0), stat.map(|s| s.1));
let lk = &self.setup.lock;
let capped = lk.max_reacq_attempts > 0 && self.attempts >= lk.max_reacq_attempts;
self.next_attempt_s = self.last_t + self.reacq_interval_s;
self.reacq_interval_s = (2.0 * self.reacq_interval_s).min(lk.reacq_max_interval_s);
if capped || self.window_spent(self.last_t) {
self.ch = None;
self.transition(idx, LockState::Retired, "retired", now, extra, sink)
} else {
self.transition(idx, LockState::Lost, "reacq-failed", now, extra, sink)
}
}
fn start(&mut self, purpose: Purpose, start: u64, spec: &SampleSpec) -> Result<bool, IqError> {
let code = self.setup.init.code.as_ref();
let period = code.period_s();
let t_track_s = self.setup.config.coherent_periods as f64 * period;
let base = self.setup.acquisition.acq_config(period, t_track_s);
let cfg = match purpose {
Purpose::Reacq { .. } => AcqConfig {
doppler_max_hz: self.setup.lock.reacq_doppler_window_hz,
..base
},
Purpose::FalseLock { alias_hz, .. } => {
let need_s = 2.0 / alias_hz;
let n = ((need_s / period - 1e-9).ceil() as usize).max(base.coherent_periods);
AcqConfig {
coherent_periods: n,
noncoherent: 1,
doppler_max_hz: alias_hz,
doppler_step_hz: alias_hz / 2.0,
pfa: base.pfa,
}
}
};
let Ok(need) = samples_needed(spec, code, &cfg) else {
return Ok(false);
};
self.collect = Some(Collect {
purpose,
start,
cfg,
need,
buf: Vec::with_capacity(need),
});
Ok(true)
}
fn on_search(
&mut self,
idx: usize,
c: Collect,
now: u64,
spec: &SampleSpec,
sink: &mut dyn EpochSink,
) -> Result<(), IqError> {
let code = self.setup.init.code.clone();
let center = match c.purpose {
Purpose::FalseLock { center, .. } | Purpose::Reacq { center } => center,
};
let shifted = SampleSpec {
if_hz: spec.if_hz + center,
..*spec
};
let grid = match acquire(&c.buf, &shifted, code.as_ref(), &c.cfg) {
Ok(g) => g,
Err(_) => {
return match c.purpose {
Purpose::FalseLock { .. } => {
self.check_unavailable = true;
Ok(())
}
Purpose::Reacq { .. } => self.reacq_failed(idx, now, None, sink),
};
}
};
match c.purpose {
Purpose::FalseLock { alias_hz, .. } => {
let row_max = |r: &Vec<f64>| r.iter().copied().fold(0.0_f64, f64::max);
let mid = grid.grid.len() / 2;
let tracked = row_max(&grid.grid[mid]);
let (alias_idx, alias) = [0, grid.grid.len() - 1]
.into_iter()
.map(|i| (i, row_max(&grid.grid[i])))
.fold((0, 0.0), |a, b| if b.1 > a.1 { b } else { a });
let ratio_db = 10.0 * (alias / tracked.max(f64::MIN_POSITIVE)).plog10();
let margin = self.setup.lock.false_lock_margin_db;
if ratio_db > margin {
let alias_doppler = center + if alias_idx == 0 { -alias_hz } else { alias_hz };
self.reacq_center = alias_doppler;
self.suspect = true;
let to = match self.state {
LockState::PullIn | LockState::Locked => LockState::Lost,
s => s,
};
self.transition(
idx,
to,
"false-lock",
now,
(Some(alias_doppler), None, Some(ratio_db), Some(margin)),
sink,
)?;
} else {
self.suspect = false;
}
}
Purpose::Reacq { .. } => {
let mut r = grid.result.clone();
if r.acquired {
r.doppler_hz += center;
let init = ChannelInit::from_acquisition(
code,
&r,
spec,
c.start,
self.setup.init.periods_per_bit,
);
let ch = Channel::new_at(spec, &init, &self.setup.config, now, self.next_epoch)
.map_err(IqError::Format)?;
self.ch = Some(ch);
self.suspect = false;
self.updates_since_check = 0;
self.transition(
idx,
LockState::PullIn,
"reacquired",
now,
(
Some(r.doppler_hz),
Some(r.code_phase_chips),
Some(r.statistic),
Some(r.threshold),
),
sink,
)?;
} else {
self.reacq_failed(idx, now, Some((r.statistic, r.threshold)), sink)?;
}
}
}
Ok(())
}
}