From f4903abd333b2945548c3d77995c534c8d284b24 Mon Sep 17 00:00:00 2001
From: Quaternions <krakow20@gmail.com>
Date: Wed, 18 Sep 2024 15:21:54 -0700
Subject: [PATCH] angle sin cos

---
 src/integer.rs | 33 +++++++++++++++++++++++----------
 1 file changed, 23 insertions(+), 10 deletions(-)

diff --git a/src/integer.rs b/src/integer.rs
index 758bdaf..d25e4ce 100644
--- a/src/integer.rs
+++ b/src/integer.rs
@@ -413,18 +413,31 @@ impl Angle32{
 			.wrapping_add(midpoint)
 		)
 	}
-	/*
 	#[inline]
-	pub fn cos(&self)->Unit32{
-		//TODO: fix this rounding towards 0
-		Unit32(unsafe{((self.0 as f64*ANGLE32_TO_FLOAT64_RADIANS).cos()*UNIT32_ONE_FLOAT64).to_int_unchecked()})
+	pub fn cos_sin(&self)->(Planar64,Planar64){
+		/*
+		//cordic
+		let a=self.0 as u32;
+		//initialize based on the quadrant
+		let (mut x,mut y)=match (a&(1<<31)!=0,a&(1<<30)!=0){
+			(false,false)=>( 1i64<<32, 0i64    ),//TR
+			(false,true )=>( 0i64    , 1i64<<32),//TL
+			(true ,false)=>(-1i64<<32, 0i64    ),//BL
+			(true ,true )=>( 0i64    ,-1i64<<32),//BR
+		};
+		println!("x={} y={}",Planar64::raw(x),Planar64::raw(y));
+		for i in 0..30{
+			if a&(1<<(29-i))!=0{
+				(x,y)=(x-(y>>i),y+(x>>i));
+			}
+			println!("i={i} t={} x={} y={}",(a&(1<<(29-i))!=0) as u8,Planar64::raw(x),Planar64::raw(y));
+		}
+		//don't forget the gain
+		(Planar64::raw(x),Planar64::raw(y))
+		*/
+		let (s,c)=(self.0 as f64*Self::ANGLE32_TO_FLOAT64_RADIANS).sin_cos();
+		(Planar64::raw((c*((1u64<<32) as f64)) as i64),Planar64::raw((s*((1u64<<32) as f64)) as i64))
 	}
-	#[inline]
-	pub fn sin(&self)->Unit32{
-		//TODO: fix this rounding towards 0
-		Unit32(unsafe{((self.0 as f64*ANGLE32_TO_FLOAT64_RADIANS).sin()*UNIT32_ONE_FLOAT64).to_int_unchecked()})
-	}
-	*/
 }
 impl Into<f32> for Angle32{
 	#[inline]