use std::ops::{Add, Div, Mul, Neg, Sub};
use super::wide_acc::{narrow_triple_ceil, narrow_triple_floor, quadratic_form_exact};
use super::{FixedMatrix, FixedPoint, FixedVector};
use crate::fixed_point::core_types::errors::OverflowDetected;
use crate::fixed_point::universal::fasc::stack_evaluator::compute::{
compute_checked_add, downscale_to_storage_ceil, downscale_to_storage_floor,
};
use crate::fixed_point::universal::fasc::stack_evaluator::{
upscale_to_compute, BinaryStorage, ComputeStorage,
};
#[cfg(table_format = "q16_16")]
use crate::fixed_point::frac_config;
#[cfg(table_format = "q64_64")]
use crate::fixed_point::i256::mul_i128_to_i256;
#[cfg(table_format = "q64_64")]
use crate::fixed_point::I256;
#[cfg(table_format = "q128_128")]
use crate::fixed_point::I512;
#[cfg(table_format = "q256_256")]
use crate::fixed_point::{I1024, I512};
#[inline]
fn st_add(a: BinaryStorage, b: BinaryStorage) -> Result<BinaryStorage, OverflowDetected> {
a.checked_add(b).ok_or(OverflowDetected::TierOverflow)
}
#[inline]
fn st_sub(a: BinaryStorage, b: BinaryStorage) -> Result<BinaryStorage, OverflowDetected> {
a.checked_sub(b).ok_or(OverflowDetected::TierOverflow)
}
#[inline]
fn st_neg(a: BinaryStorage) -> Result<BinaryStorage, OverflowDetected> {
a.checked_neg().ok_or(OverflowDetected::TierOverflow)
}
#[inline]
fn compute_zero() -> ComputeStorage {
upscale_to_compute(FixedPoint::ZERO.raw())
}
#[inline]
pub(crate) fn exact_product(a: BinaryStorage, b: BinaryStorage) -> ComputeStorage {
#[cfg(table_format = "q16_16")]
{
(a as i64) * (b as i64)
}
#[cfg(table_format = "q32_32")]
{
(a as i128) * (b as i128)
}
#[cfg(table_format = "q64_64")]
{
mul_i128_to_i256(a, b)
}
#[cfg(table_format = "q128_128")]
{
let a_neg = a.is_negative();
let b_neg = b.is_negative();
let abs_a = if a_neg { -a } else { a };
let abs_b = if b_neg { -b } else { b };
let product = abs_a.mul_to_i512(abs_b);
if a_neg != b_neg { -product } else { product }
}
#[cfg(table_format = "q256_256")]
{
let a_neg = a.is_negative();
let b_neg = b.is_negative();
let abs_a = if a_neg { -a } else { a };
let abs_b = if b_neg { -b } else { b };
let product = abs_a.mul_to_i1024(abs_b);
if a_neg != b_neg { -product } else { product }
}
}
#[inline]
fn directed_divide(a: BinaryStorage, b: BinaryStorage) -> Result<(BinaryStorage, BinaryStorage), OverflowDetected> {
#[cfg(table_format = "q16_16")]
{
if b == 0 { return Err(OverflowDetected::DivisionByZero); }
let num = (a as i64) << frac_config::FRAC_BITS;
let den = b as i64;
let q = num / den;
let rem = num - q * den;
let (f, c) = if rem == 0 {
(q, q)
} else if (num < 0) == (den < 0) {
(q, q + 1)
} else {
(q - 1, q)
};
let fits = |v: i64| v >= i32::MIN as i64 && v <= i32::MAX as i64;
if !fits(f) || !fits(c) { return Err(OverflowDetected::TierOverflow); }
Ok((f as i32, c as i32))
}
#[cfg(table_format = "q32_32")]
{
if b == 0 { return Err(OverflowDetected::DivisionByZero); }
let num = (a as i128) << 32;
let den = b as i128;
let q = num / den;
let rem = num - q * den;
let (f, c) = if rem == 0 {
(q, q)
} else if (num < 0) == (den < 0) {
(q, q + 1)
} else {
(q - 1, q)
};
let fits = |v: i128| v >= i64::MIN as i128 && v <= i64::MAX as i128;
if !fits(f) || !fits(c) { return Err(OverflowDetected::TierOverflow); }
Ok((f as i64, c as i64))
}
#[cfg(table_format = "q64_64")]
{
if b == 0 { return Err(OverflowDetected::DivisionByZero); }
let num = I256::from_i128(a) << 64usize;
let den = I256::from_i128(b);
let q = num / den;
let rem = num - q * den;
let one = I256::from_i128(1);
let (f, c) = if rem.is_zero() {
(q, q)
} else if num.is_negative() == den.is_negative() {
(q, q + one)
} else {
(q - one, q)
};
if !f.fits_in_i128() || !c.fits_in_i128() { return Err(OverflowDetected::TierOverflow); }
Ok((f.as_i128(), c.as_i128()))
}
#[cfg(table_format = "q128_128")]
{
if b.is_zero() { return Err(OverflowDetected::DivisionByZero); }
let num = I512::from_i256(a) << 128usize;
let den = I512::from_i256(b);
let q = num / den;
let rem = num - q * den;
let one = I512::from_i128(1);
let (f, c) = if rem == I512::zero() {
(q, q)
} else if num.is_negative() == den.is_negative() {
(q, q + one)
} else {
(q - one, q)
};
if !f.fits_in_i256() || !c.fits_in_i256() { return Err(OverflowDetected::TierOverflow); }
Ok((f.as_i256(), c.as_i256()))
}
#[cfg(table_format = "q256_256")]
{
if b == I512::zero() { return Err(OverflowDetected::DivisionByZero); }
let num = I1024::from_i512(a) << 256usize;
let den = I1024::from_i512(b);
let q = num / den;
let rem = num - q * den;
let one = I1024::from_i128(1);
let zero = I1024::zero();
let (f, c) = if rem == zero {
(q, q)
} else if (num < zero) == (den < zero) {
(q, q + one)
} else {
(q - one, q)
};
if !f.fits_in_i512() || !c.fits_in_i512() { return Err(OverflowDetected::TierOverflow); }
Ok((f.as_i512(), c.as_i512()))
}
}
#[inline]
fn certified_sqrt(x: BinaryStorage) -> Result<(BinaryStorage, BinaryStorage), OverflowDetected> {
let n = upscale_to_compute(x);
let mut k = compute_narrow_integer(isqrt_compute(n))?;
let unit = unit_raw();
let mut steps = 0u32;
while exact_product(k, k) > n {
k = st_sub(k, unit)?;
steps += 1;
assert!(steps <= 2, "certified_sqrt: candidate failed the certificate downward");
}
loop {
let next = st_add(k, unit)?;
if exact_product(next, next) <= n {
k = next;
steps += 1;
assert!(steps <= 2, "certified_sqrt: candidate failed the certificate upward");
} else {
break;
}
}
let floor = k;
let ceil = if exact_product(k, k) == n { k } else { st_add(k, unit)? };
Ok((floor, ceil))
}
#[inline]
fn isqrt_compute(n: ComputeStorage) -> ComputeStorage {
#[cfg(table_format = "q16_16")]
{
debug_assert!(n >= 0);
(n as u64).isqrt() as i64
}
#[cfg(table_format = "q32_32")]
{
debug_assert!(n >= 0);
(n as u128).isqrt() as i128
}
#[cfg(any(table_format = "q64_64", table_format = "q128_128", table_format = "q256_256"))]
{
if n == ComputeStorage::zero() {
return n;
}
let bits = compute_bit_length(n);
let mut k = ComputeStorage::from_i128(1) << (((bits + 1) / 2) as usize);
loop {
let next = (k + n / k) >> 1;
if next >= k {
return k;
}
k = next;
}
}
}
#[cfg(any(table_format = "q64_64", table_format = "q128_128", table_format = "q256_256"))]
#[inline]
fn compute_bit_length(n: ComputeStorage) -> u32 {
let words = &n.words;
for i in (0..words.len()).rev() {
if words[i] != 0 {
return i as u32 * 64 + (64 - words[i].leading_zeros());
}
}
0
}
#[inline]
fn compute_narrow_integer(v: ComputeStorage) -> Result<BinaryStorage, OverflowDetected> {
#[cfg(table_format = "q16_16")]
{
i32::try_from(v).map_err(|_| OverflowDetected::TierOverflow)
}
#[cfg(table_format = "q32_32")]
{
i64::try_from(v).map_err(|_| OverflowDetected::TierOverflow)
}
#[cfg(table_format = "q64_64")]
{
if v.fits_in_i128() { Ok(v.as_i128()) } else { Err(OverflowDetected::TierOverflow) }
}
#[cfg(table_format = "q128_128")]
{
if v.fits_in_i256() { Ok(v.as_i256()) } else { Err(OverflowDetected::TierOverflow) }
}
#[cfg(table_format = "q256_256")]
{
if v.fits_in_i512() { Ok(v.as_i512()) } else { Err(OverflowDetected::TierOverflow) }
}
}
#[inline]
fn unit_raw() -> BinaryStorage {
#[cfg(any(table_format = "q16_16", table_format = "q32_32", table_format = "q64_64"))]
{ 1 }
#[cfg(table_format = "q128_128")]
{ crate::fixed_point::I256::from_i128(1) }
#[cfg(table_format = "q256_256")]
{ I512::from_i128(1) }
}
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub struct Interval {
lo: FixedPoint,
hi: FixedPoint,
}
impl Interval {
#[inline]
pub fn point(x: FixedPoint) -> Self {
Interval { lo: x, hi: x }
}
#[inline]
pub fn new(lo: FixedPoint, hi: FixedPoint) -> Self {
Self::try_new(lo, hi).expect("Interval::new: lo > hi")
}
#[inline]
pub fn try_new(lo: FixedPoint, hi: FixedPoint) -> Result<Self, OverflowDetected> {
if lo > hi {
return Err(OverflowDetected::InvalidInput);
}
Ok(Interval { lo, hi })
}
#[inline]
fn from_raw(lo: BinaryStorage, hi: BinaryStorage) -> Self {
let iv = Interval { lo: FixedPoint::from_raw(lo), hi: FixedPoint::from_raw(hi) };
debug_assert!(iv.lo <= iv.hi, "Interval invariant violated: lo > hi");
iv
}
#[inline]
pub fn lo(self) -> FixedPoint { self.lo }
#[inline]
pub fn hi(self) -> FixedPoint { self.hi }
#[inline]
pub fn width(self) -> FixedPoint {
FixedPoint::from_raw(st_sub(self.hi.raw(), self.lo.raw()).expect("Interval::width: overflow"))
}
#[inline]
pub fn is_point(self) -> bool { self.lo == self.hi }
#[inline]
pub fn contains(self, x: FixedPoint) -> bool { self.lo <= x && x <= self.hi }
#[inline]
pub fn contains_zero(self) -> bool { self.contains(FixedPoint::ZERO) }
#[inline]
pub fn is_certainly_positive(self) -> bool { self.lo > FixedPoint::ZERO }
#[inline]
pub fn is_certainly_negative(self) -> bool { self.hi < FixedPoint::ZERO }
pub fn try_add(self, rhs: Self) -> Result<Self, OverflowDetected> {
Ok(Self::from_raw(st_add(self.lo.raw(), rhs.lo.raw())?, st_add(self.hi.raw(), rhs.hi.raw())?))
}
pub fn try_sub(self, rhs: Self) -> Result<Self, OverflowDetected> {
Ok(Self::from_raw(st_sub(self.lo.raw(), rhs.hi.raw())?, st_sub(self.hi.raw(), rhs.lo.raw())?))
}
pub fn try_neg(self) -> Result<Self, OverflowDetected> {
Ok(Self::from_raw(st_neg(self.hi.raw())?, st_neg(self.lo.raw())?))
}
pub fn try_mul(self, rhs: Self) -> Result<Self, OverflowDetected> {
let p = [
exact_product(self.lo.raw(), rhs.lo.raw()),
exact_product(self.lo.raw(), rhs.hi.raw()),
exact_product(self.hi.raw(), rhs.lo.raw()),
exact_product(self.hi.raw(), rhs.hi.raw()),
];
let mut mn = p[0];
let mut mx = p[0];
for q in &p[1..] {
if *q < mn { mn = *q; }
if *q > mx { mx = *q; }
}
Ok(Self::from_raw(downscale_to_storage_floor(mn)?, downscale_to_storage_ceil(mx)?))
}
pub fn try_div(self, rhs: Self) -> Result<Self, OverflowDetected> {
if rhs.contains_zero() {
return Err(OverflowDetected::DivisionByZero);
}
let corners = [
directed_divide(self.lo.raw(), rhs.lo.raw())?,
directed_divide(self.lo.raw(), rhs.hi.raw())?,
directed_divide(self.hi.raw(), rhs.lo.raw())?,
directed_divide(self.hi.raw(), rhs.hi.raw())?,
];
let mut lo = corners[0].0;
let mut hi = corners[0].1;
for (f, c) in &corners[1..] {
if *f < lo { lo = *f; }
if *c > hi { hi = *c; }
}
Ok(Self::from_raw(lo, hi))
}
pub fn try_sqrt(self) -> Result<Self, OverflowDetected> {
if self.lo < FixedPoint::ZERO {
return Err(OverflowDetected::DomainError);
}
let (lo, _) = certified_sqrt(self.lo.raw())?;
let (_, hi) = certified_sqrt(self.hi.raw())?;
Ok(Self::from_raw(lo, hi))
}
pub fn try_dot(a: &[FixedPoint], b: &[FixedPoint]) -> Result<Self, OverflowDetected> {
assert_eq!(a.len(), b.len(), "Interval::dot: length mismatch");
let mut acc = compute_zero();
for i in 0..a.len() {
acc = compute_checked_add(acc, exact_product(a[i].raw(), b[i].raw()))?;
}
Ok(Self::from_raw(downscale_to_storage_floor(acc)?, downscale_to_storage_ceil(acc)?))
}
pub fn try_dot_intervals(a: &[Interval], b: &[Interval]) -> Result<Self, OverflowDetected> {
assert_eq!(a.len(), b.len(), "Interval::dot_intervals: length mismatch");
let mut lo_acc = compute_zero();
let mut hi_acc = compute_zero();
for i in 0..a.len() {
let p = [
exact_product(a[i].lo.raw(), b[i].lo.raw()),
exact_product(a[i].lo.raw(), b[i].hi.raw()),
exact_product(a[i].hi.raw(), b[i].lo.raw()),
exact_product(a[i].hi.raw(), b[i].hi.raw()),
];
let mut mn = p[0];
let mut mx = p[0];
for q in &p[1..] {
if *q < mn { mn = *q; }
if *q > mx { mx = *q; }
}
lo_acc = compute_checked_add(lo_acc, mn)?;
hi_acc = compute_checked_add(hi_acc, mx)?;
}
Ok(Self::from_raw(downscale_to_storage_floor(lo_acc)?, downscale_to_storage_ceil(hi_acc)?))
}
pub fn try_quadratic_form(v: &FixedVector, m: &FixedMatrix) -> Result<Self, OverflowDetected> {
let acc = quadratic_form_exact(v, m)?;
Ok(Self::from_raw(narrow_triple_floor(acc)?, narrow_triple_ceil(acc)?))
}
pub fn sqrt(self) -> Self {
self.try_sqrt().expect("Interval::sqrt: domain error or overflow")
}
pub fn dot(a: &[FixedPoint], b: &[FixedPoint]) -> Self {
Self::try_dot(a, b).expect("Interval::dot: overflow")
}
pub fn quadratic_form(v: &FixedVector, m: &FixedMatrix) -> Self {
Self::try_quadratic_form(v, m).expect("Interval::quadratic_form: overflow")
}
pub fn dot_intervals(a: &[Interval], b: &[Interval]) -> Self {
Self::try_dot_intervals(a, b).expect("Interval::dot_intervals: overflow")
}
}
impl Add for Interval {
type Output = Self;
#[inline]
fn add(self, rhs: Self) -> Self { self.try_add(rhs).expect("Interval: addition overflow") }
}
impl Sub for Interval {
type Output = Self;
#[inline]
fn sub(self, rhs: Self) -> Self { self.try_sub(rhs).expect("Interval: subtraction overflow") }
}
impl Mul for Interval {
type Output = Self;
#[inline]
fn mul(self, rhs: Self) -> Self { self.try_mul(rhs).expect("Interval: multiplication overflow") }
}
impl Div for Interval {
type Output = Self;
#[inline]
fn div(self, rhs: Self) -> Self { self.try_div(rhs).expect("Interval: division by an interval containing zero, or overflow") }
}
impl Neg for Interval {
type Output = Self;
#[inline]
fn neg(self) -> Self { self.try_neg().expect("Interval: negation overflow") }
}
impl From<FixedPoint> for Interval {
#[inline]
fn from(x: FixedPoint) -> Self { Interval::point(x) }
}