use crate::CalError;
use std::collections::BTreeMap;
const BISECTION_STEPS: usize = 128;
#[derive(Debug, Clone, PartialEq)]
pub enum CompuMethod {
Identity,
Linear {
slope: f64,
intercept: f64,
},
RatFunc {
coeffs: [f64; 6],
},
Table {
tab_ref: Option<String>,
points: Vec<(f64, f64)>,
interpolate: bool,
},
}
impl CompuMethod {
#[must_use]
pub const fn identity() -> Self {
Self::Identity
}
#[must_use]
pub const fn linear(slope: f64, intercept: f64) -> Self {
Self::Linear { slope, intercept }
}
#[must_use]
pub const fn rat_func(coeffs: [f64; 6]) -> Self {
Self::RatFunc { coeffs }
}
#[must_use]
pub fn tab_intp(points: Vec<(f64, f64)>) -> Self {
Self::Table {
tab_ref: None,
points,
interpolate: true,
}
}
#[must_use]
pub fn tab_no_intp(points: Vec<(f64, f64)>) -> Self {
Self::Table {
tab_ref: None,
points,
interpolate: false,
}
}
#[must_use]
pub fn from_a2l(method: &a2l_parse::CompuMethod) -> Self {
match method.conversion_type {
a2l_parse::ConversionType::Identity => Self::Identity,
a2l_parse::ConversionType::Linear => match method.coeffs {
a2l_parse::Coeffs::Linear([slope, intercept]) => Self::Linear { slope, intercept },
_ => Self::Identity,
},
a2l_parse::ConversionType::RatFunc => match method.coeffs {
a2l_parse::Coeffs::RatFunc(coeffs) => Self::RatFunc { coeffs },
_ => Self::Identity,
},
a2l_parse::ConversionType::Table => Self::Table {
tab_ref: method.tab_ref.clone(),
points: Vec::new(),
interpolate: true,
},
}
}
#[must_use]
pub const fn keyword(&self) -> &'static str {
match self {
Self::Identity => "IDENTITY",
Self::Linear { .. } => "LINEAR",
Self::RatFunc { .. } => "RAT_FUNC",
Self::Table { .. } => "TABLE",
}
}
#[must_use]
pub fn points(&self) -> &[(f64, f64)] {
match self {
Self::Table { points, .. } => points,
_ => &[],
}
}
#[must_use]
pub fn tab_ref(&self) -> Option<&str> {
match self {
Self::Table { tab_ref, .. } => tab_ref.as_deref(),
_ => None,
}
}
#[must_use]
pub const fn is_identity(&self) -> bool {
matches!(self, Self::Identity)
}
pub fn set_points(&mut self, points: Vec<(f64, f64)>) {
if let Self::Table { points: slot, .. } = self {
*slot = points;
}
}
pub fn resolved(&self, tables: &BTreeMap<String, Vec<(f64, f64)>>) -> Result<Self, CalError> {
let Self::Table {
tab_ref,
points,
interpolate,
} = self
else {
return Ok(self.clone());
};
if !points.is_empty() {
return Ok(self.clone());
}
let Some(name) = tab_ref else {
return Err(CalError::Unsupported {
subject: "TABLE conversion".to_string(),
reason: "no points and no COMPU_TAB_REF to resolve them from".to_string(),
});
};
let Some(found) = tables.get(name.as_str()) else {
return Err(CalError::Unsupported {
subject: format!("COMPU_TAB_REF `{name}`"),
reason: "no table registered under that name".to_string(),
});
};
if found.is_empty() {
return Err(CalError::Unsupported {
subject: format!("COMPU_TAB_REF `{name}`"),
reason: "the registered table has no points".to_string(),
});
}
Ok(Self::Table {
tab_ref: tab_ref.clone(),
points: found.clone(),
interpolate: *interpolate,
})
}
#[must_use]
pub fn apply(&self, raw: f64) -> f64 {
match self {
Self::Identity => raw,
Self::Linear { slope, intercept } => slope * raw + intercept,
Self::RatFunc { coeffs } => {
let [a, b, c, d, e, f] = *coeffs;
let x = raw;
(a * x * x + b * x + c) / (d * x * x + e * x + f)
}
Self::Table {
points,
interpolate,
..
} => table_apply(points, *interpolate, raw),
}
}
pub fn invert(
&self,
name: &str,
physical: f64,
value_min: f64,
value_max: f64,
) -> Result<f64, CalError> {
match self {
Self::Identity => in_range(name, physical, value_min, value_max),
Self::Linear { slope, intercept } => {
if *slope == 0.0 || !slope.is_finite() {
return Err(CalError::Unsupported {
subject: format!("`{name}`"),
reason: format!("linear conversion with slope {slope} is not invertible"),
});
}
in_range(name, (physical - intercept) / slope, value_min, value_max)
}
Self::RatFunc { .. } => bisect(self, name, physical, value_min, value_max),
Self::Table {
points,
interpolate,
..
} => table_invert(points, *interpolate, name, physical, value_min, value_max),
}
}
}
fn in_range(name: &str, raw: f64, value_min: f64, value_max: f64) -> Result<f64, CalError> {
if !raw.is_finite() || raw < value_min || raw > value_max {
return Err(CalError::OutOfBounds {
name: name.to_string(),
value: raw,
lower: value_min,
upper: value_max,
});
}
Ok(raw)
}
fn bisect(
method: &CompuMethod,
name: &str,
physical: f64,
value_min: f64,
value_max: f64,
) -> Result<f64, CalError> {
let mut lo = value_min;
let mut hi = value_max;
let y_lo = method.apply(lo);
let y_hi = method.apply(hi);
if !y_lo.is_finite() || !y_hi.is_finite() {
return Err(CalError::Unsupported {
subject: format!("`{name}`"),
reason: "rational conversion is not finite at the deposit bounds".to_string(),
});
}
let descending = y_lo > y_hi;
let low_physical = if descending { y_hi } else { y_lo };
let high_physical = if descending { y_lo } else { y_hi };
if physical < low_physical || physical > high_physical {
return Err(CalError::OutOfBounds {
name: name.to_string(),
value: physical,
lower: low_physical,
upper: high_physical,
});
}
for _ in 0..BISECTION_STEPS {
let mid = lo.midpoint(hi);
if hi - lo <= 1e-9 * mid.abs().max(1.0) {
break;
}
let below = if descending {
method.apply(mid) > physical
} else {
method.apply(mid) < physical
};
if below {
lo = mid;
} else {
hi = mid;
}
}
Ok(lo.midpoint(hi))
}
fn table_apply(points: &[(f64, f64)], interpolate: bool, raw: f64) -> f64 {
let Some(&(first_x, first_y)) = points.first() else {
return raw;
};
if raw <= first_x {
return first_y;
}
let Some(&(last_x, last_y)) = points.last() else {
return raw;
};
if raw >= last_x {
return last_y;
}
for pair in points.windows(2) {
let [first, second] = pair else { continue };
let (x0, y0) = *first;
let (x1, y1) = *second;
if raw >= x0 && raw < x1 {
if !interpolate {
return y0;
}
let span = x1 - x0;
if span == 0.0 {
return y0;
}
return y0 + (y1 - y0) * (raw - x0) / span;
}
}
first_y
}
fn table_invert(
points: &[(f64, f64)],
interpolate: bool,
name: &str,
physical: f64,
value_min: f64,
value_max: f64,
) -> Result<f64, CalError> {
if points.is_empty() {
return Err(CalError::Unsupported {
subject: format!("`{name}`"),
reason: "TABLE conversion has no points to invert".to_string(),
});
}
let lowest = points
.iter()
.map(|&(_, phys)| phys)
.fold(f64::INFINITY, f64::min);
let highest = points
.iter()
.map(|&(_, phys)| phys)
.fold(f64::NEG_INFINITY, f64::max);
if !physical.is_finite() || physical < lowest || physical > highest {
return Err(CalError::OutOfBounds {
name: name.to_string(),
value: physical,
lower: lowest,
upper: highest,
});
}
if !interpolate {
let Some(&first) = points.first() else {
return Err(CalError::Unsupported {
subject: format!("`{name}`"),
reason: "TABLE conversion has no points to invert".to_string(),
});
};
let mut best = first;
for &point in points {
if point.1 <= physical {
best = point;
}
}
if physical < best.1 {
best = first;
}
return in_range(name, best.0, value_min, value_max);
}
let mut best_raw: Option<f64> = None;
let mut best_error = f64::INFINITY;
for pair in points.windows(2) {
let [first, second] = pair else { continue };
let (x0, y0) = *first;
let (x1, y1) = *second;
let (low_y, high_y) = if y0 <= y1 { (y0, y1) } else { (y1, y0) };
if physical < low_y || physical > high_y {
continue;
}
let span = y1 - y0;
let raw = if span == 0.0 {
x0
} else {
x0 + (physical - y0) * (x1 - x0) / span
};
let error = (table_apply(points, interpolate, raw) - physical).abs();
if error < best_error {
best_error = error;
best_raw = Some(raw);
}
}
let Some(raw) = best_raw else {
return Err(CalError::OutOfBounds {
name: name.to_string(),
value: physical,
lower: lowest,
upper: highest,
});
};
in_range(name, raw, value_min, value_max)
}