strafe-project/fixed_wide/src/fixed.rs

use bnum::{BInt,cast::As};
use typenum::Unsigned;
use std::marker::PhantomData;

#[derive(Clone,Copy,Debug,Hash)]
pub struct Fixed<const CHUNKS:usize,Frac>{
	pub(crate)bits:BInt<{CHUNKS}>,
	pub(crate)frac:PhantomData<Frac>,
}

impl<const CHUNKS:usize,Frac:Unsigned> Fixed<CHUNKS,Frac>{
	pub const MAX:Self=Self{bits:BInt::<CHUNKS>::MAX,frac:PhantomData};
	pub const MIN:Self=Self{bits:BInt::<CHUNKS>::MIN,frac:PhantomData};
	pub const ZERO:Self=Self{bits:BInt::<CHUNKS>::ZERO,frac:PhantomData};
	pub const ONE:Self=Self{bits:BInt::<CHUNKS>::ONE.shl(Frac::U32),frac:PhantomData};
	pub const TWO:Self=Self{bits:BInt::<CHUNKS>::TWO.shl(Frac::U32),frac:PhantomData};
	pub const HALF:Self=Self{bits:BInt::<CHUNKS>::ONE.shl(Frac::U32-1),frac:PhantomData};
	pub const NEG_ONE:Self=Self{bits:BInt::<CHUNKS>::NEG_ONE.shl(Frac::U32),frac:PhantomData};
	pub const NEG_TWO:Self=Self{bits:BInt::<CHUNKS>::NEG_TWO.shl(Frac::U32),frac:PhantomData};
	pub const NEG_HALF:Self=Self{bits:BInt::<CHUNKS>::NEG_ONE.shl(Frac::U32-1),frac:PhantomData};
	pub const fn from_bits(bits:BInt::<CHUNKS>)->Self{
		Self{
			bits,
			frac:PhantomData,
		}
	}
	pub const fn to_bits(self)->BInt<CHUNKS>{
		self.bits
	}
}

impl<const CHUNKS:usize,Frac:Unsigned,T> From<T> for Fixed<CHUNKS,Frac>
	where
		BInt<CHUNKS>:From<T>
{
	fn from(value:T)->Self{
		Self{
			bits:BInt::<{CHUNKS}>::from(value)<<Frac::U32,
			frac:PhantomData,
		}
	}
}

impl<const CHUNKS:usize,Frac> PartialEq for Fixed<CHUNKS,Frac>{
	fn eq(&self,other:&Self)->bool{
		self.bits.eq(&other.bits)
	}
}
impl<const CHUNKS:usize,Frac> Eq for Fixed<CHUNKS,Frac>{}

impl<const CHUNKS:usize,Frac> PartialOrd for Fixed<CHUNKS,Frac>{
	fn partial_cmp(&self,other:&Self)->Option<std::cmp::Ordering>{
		self.bits.partial_cmp(&other.bits)
	}
}
impl<const CHUNKS:usize,Frac> Ord for Fixed<CHUNKS,Frac>{
	fn cmp(&self,other:&Self)->std::cmp::Ordering{
		self.bits.cmp(&other.bits)
	}
}

impl<const CHUNKS:usize,Frac> std::ops::Neg for Fixed<CHUNKS,Frac>{
	type Output=Self;
	fn neg(self)->Self{
		Self{
			bits:self.bits.neg(),
			frac:PhantomData,
		}
	}
}

macro_rules! impl_additive_operator {
	( $struct: ident, $trait: ident, $method: ident, $output: ty ) => {
		impl<const CHUNKS:usize,Frac> core::ops::$trait for $struct<CHUNKS,Frac>{
			type Output = $output;

			fn $method(self, other: Self) -> Self::Output {
				Self {
					bits:self.bits.$method(other.bits),
					frac:PhantomData,
				}
			}
		}
		impl<const CHUNKS:usize,Frac:Unsigned,U> core::ops::$trait<U> for $struct<CHUNKS,Frac>
			where
				BInt::<CHUNKS>:From<U>,
		{
			type Output = $output;

			fn $method(self, other: U) -> Self::Output {
				Self {
					bits:self.bits.$method(BInt::<CHUNKS>::from(other)<<Frac::U32),
					frac:PhantomData,
				}
			}
		}
	};
}
macro_rules! impl_additive_assign_operator {
	( $struct: ident, $trait: ident, $method: ident ) => {
		impl<const CHUNKS:usize,Frac> core::ops::$trait for $struct<CHUNKS,Frac>{
			fn $method(&mut self, other: Self) {
				self.bits.$method(other.bits);
			}
		}
		impl<const CHUNKS:usize,Frac:Unsigned,U> core::ops::$trait<U> for $struct<CHUNKS,Frac>
		where
				BInt::<CHUNKS>:From<U>,
		{
			fn $method(&mut self, other: U) {
				self.bits.$method(BInt::<CHUNKS>::from(other)<<Frac::U32);
			}
		}
	};
}

// Impl arithmetic pperators
impl_additive_assign_operator!( Fixed, AddAssign, add_assign );
impl_additive_operator!( Fixed, Add, add, Self );
impl_additive_assign_operator!( Fixed, SubAssign, sub_assign );
impl_additive_operator!( Fixed, Sub, sub, Self );
impl_additive_assign_operator!( Fixed, RemAssign, rem_assign );
impl_additive_operator!( Fixed, Rem, rem, Self );

// Impl bitwise operators
impl_additive_assign_operator!( Fixed, BitAndAssign, bitand_assign );
impl_additive_operator!( Fixed, BitAnd, bitand, Self );
impl_additive_assign_operator!( Fixed, BitOrAssign, bitor_assign );
impl_additive_operator!( Fixed, BitOr, bitor, Self );
impl_additive_assign_operator!( Fixed, BitXorAssign, bitxor_assign );
impl_additive_operator!( Fixed, BitXor, bitxor, Self );

macro_rules! impl_multiply_operator_const {
	( $width:expr, $struct: ident, $trait: ident, $method: ident, $output: ty ) => {
		impl<Frac:Unsigned> core::ops::$trait for $struct<$width,Frac>{
			type Output = $output;

			fn $method(self, other: Self) -> Self::Output {
				//this can be done better but that is a job for later
				let lhs=self.bits.as_::<BInt::<{$width*2}>>();
				let rhs=other.bits.as_::<BInt::<{$width*2}>>();
				Self {
					bits:lhs.mul(rhs).shr(Frac::U32).as_(),
					frac:PhantomData,
				}
			}
		}
	};
}
macro_rules! impl_multiply_assign_operator_const {
	( $width:expr, $struct: ident, $trait: ident, $method: ident ) => {
		impl<Frac> core::ops::$trait for $struct<$width,Frac>{
			fn $method(&mut self, other: Self) {
				self.bits.$method(other.bits);
			}
		}
	};
}

macro_rules! impl_divide_operator_const {
	( $width:expr, $struct: ident, $trait: ident, $method: ident, $output: ty ) => {
		impl<Frac:Unsigned> core::ops::$trait for $struct<$width,Frac>{
			type Output = $output;

			fn $method(self, other: Self) -> Self::Output {
				//this can be done better but that is a job for later
				//this only needs to be $width+Frac::U32/64+1 but MUH CONST GENERICS!!!!!
				let lhs=self.bits.as_::<BInt::<{$width*2}>>().shl(Frac::U32);
				let rhs=other.bits.as_::<BInt::<{$width*2}>>();
				Self {
					bits:lhs.div(rhs).as_(),
					frac:PhantomData,
				}
			}
		}
	};
}
macro_rules! impl_divide_assign_operator_const {
	( $width:expr, $struct: ident, $trait: ident, $method: ident ) => {
		impl<Frac> core::ops::$trait for $struct<$width,Frac>{
			fn $method(&mut self, other: Self) {
				self.bits.$method(other.bits);
			}
		}
	};
}

macro_rules! impl_multiplicatave_operator {
	( $struct: ident, $trait: ident, $method: ident, $output: ty ) => {
		impl<const CHUNKS:usize,Frac,U> core::ops::$trait<U> for $struct<CHUNKS,Frac>
			where
				BInt::<CHUNKS>:From<U>+core::ops::$trait,
		{
			type Output = $output;

			fn $method(self, other: U) -> Self::Output {
				Self {
					bits:self.bits.$method(BInt::<CHUNKS>::from(other)),
					frac:PhantomData,
				}
			}
		}
	};
}
macro_rules! impl_multiplicatave_assign_operator {
	( $struct: ident, $trait: ident, $method: ident ) => {
		impl<const CHUNKS:usize,Frac,U> core::ops::$trait<U> for $struct<CHUNKS,Frac>
			where
				BInt::<CHUNKS>:From<U>+core::ops::$trait,
		{
			fn $method(&mut self, other: U) {
				self.bits.$method(BInt::<CHUNKS>::from(other));
			}
		}
	};
}

macro_rules! impl_operator_16 {
	( $macro: ident, $struct: ident, $trait: ident, $method: ident, $output: ty ) => {
		$macro!(1,$struct,$trait,$method,$output);
		$macro!(2,$struct,$trait,$method,$output);
		$macro!(3,$struct,$trait,$method,$output);
		$macro!(4,$struct,$trait,$method,$output);
		$macro!(5,$struct,$trait,$method,$output);
		$macro!(6,$struct,$trait,$method,$output);
		$macro!(7,$struct,$trait,$method,$output);
		$macro!(8,$struct,$trait,$method,$output);
		$macro!(9,$struct,$trait,$method,$output);
		$macro!(10,$struct,$trait,$method,$output);
		$macro!(11,$struct,$trait,$method,$output);
		$macro!(12,$struct,$trait,$method,$output);
		$macro!(13,$struct,$trait,$method,$output);
		$macro!(14,$struct,$trait,$method,$output);
		$macro!(15,$struct,$trait,$method,$output);
		$macro!(16,$struct,$trait,$method,$output);
	}
}
macro_rules! impl_assign_operator_16 {
	( $macro: ident, $struct: ident, $trait: ident, $method: ident ) => {
		$macro!(1,$struct,$trait,$method);
		$macro!(2,$struct,$trait,$method);
		$macro!(3,$struct,$trait,$method);
		$macro!(4,$struct,$trait,$method);
		$macro!(5,$struct,$trait,$method);
		$macro!(6,$struct,$trait,$method);
		$macro!(7,$struct,$trait,$method);
		$macro!(8,$struct,$trait,$method);
		$macro!(9,$struct,$trait,$method);
		$macro!(10,$struct,$trait,$method);
		$macro!(11,$struct,$trait,$method);
		$macro!(12,$struct,$trait,$method);
		$macro!(13,$struct,$trait,$method);
		$macro!(14,$struct,$trait,$method);
		$macro!(15,$struct,$trait,$method);
		$macro!(16,$struct,$trait,$method);
	}
}

impl_assign_operator_16!( impl_multiply_assign_operator_const, Fixed, MulAssign, mul_assign );
impl_operator_16!( impl_multiply_operator_const, Fixed, Mul, mul, Self );
impl_assign_operator_16!( impl_divide_assign_operator_const, Fixed, DivAssign, div_assign );
impl_operator_16!( impl_divide_operator_const, Fixed, Div, div, Self );
impl_multiplicatave_assign_operator!( Fixed, MulAssign, mul_assign );
impl_multiplicatave_operator!( Fixed, Mul, mul, Self );
impl_multiplicatave_assign_operator!( Fixed, DivAssign, div_assign );
impl_multiplicatave_operator!( Fixed, Div, div, Self );

macro_rules! impl_shift_operator {
	( $struct: ident, $trait: ident, $method: ident, $output: ty ) => {
		impl<const CHUNKS:usize,Frac> core::ops::$trait<u32> for $struct<CHUNKS,Frac>{
			type Output = $output;

			fn $method(self, other: u32) -> Self::Output {
				Self {
					bits:self.bits.$method(other),
					frac:PhantomData,
				}
			}
		}
	};
}
macro_rules! impl_shift_assign_operator {
	( $struct: ident, $trait: ident, $method: ident ) => {
		impl<const CHUNKS:usize,Frac> core::ops::$trait<u32> for $struct<CHUNKS,Frac>{
			fn $method(&mut self, other: u32) {
				self.bits.$method(other);
			}
		}
	};
}
impl_shift_assign_operator!( Fixed, ShlAssign, shl_assign );
impl_shift_operator!( Fixed, Shl, shl, Self );
impl_shift_assign_operator!( Fixed, ShrAssign, shr_assign );
impl_shift_operator!( Fixed, Shr, shr, Self );

impl<const CHUNKS:usize,Frac:Unsigned> Fixed<CHUNKS,Frac>
	where
		Fixed::<CHUNKS,Frac>:std::ops::Mul<Fixed<CHUNKS,Frac>,Output=Fixed<CHUNKS,Frac>>,
{
	pub fn sqrt_unchecked(self)->Self{
		//pow2 must be the minimum power of two which when squared is greater than self
		//the algorithm:
		//1. count "used" bits to the left of the decimal
		//2. add one
		//This is the power of two which is greater than self.
		//3. divide by 2 via >>1
		//4. add on fractional offset
		//Voila
		//0001.0000 Fixed<u8,4>
		//sqrt
		//0110.0000
		//pow2 = 0100.0000
		let mut pow2=Self{
			bits:BInt::<CHUNKS>::ONE.shl((((CHUNKS as i32*64-Frac::I32-(self.bits.leading_zeros() as i32)+1)>>1)+Frac::I32) as u32),
			frac:PhantomData,
		};
		let mut result=pow2>>1;

		loop{
			pow2>>=1;
			if pow2==Self::ZERO{
				break result;
			}
			//TODO: flip a single bit instead of adding a power of 2
			let new_result=result+pow2;
			//note that the implicit truncation in the multiply
			//means that the algorithm can return a result which squares to a number greater than the input.
			match self.cmp(&(new_result*new_result)){
				core::cmp::Ordering::Less=>continue,
				core::cmp::Ordering::Equal=>break new_result,
				core::cmp::Ordering::Greater=>result=new_result,
			}
		}
	}
	pub fn sqrt(self)->Self{
		if self<Self::ZERO{
			panic!("Square root less than zero")
		}else{
			self.sqrt_unchecked()
		}
	}
	pub fn sqrt_checked(self)->Option<Self>{
		if self<Self::ZERO{
			None
		}else{
			Some(self.sqrt_unchecked())
		}
	}
}