From 501230e1216f4115a9fe9f62435753788e1cb4b3 Mon Sep 17 00:00:00 2001
From: Gabe Venberg <gabevenberg@gmail.com>
Date: Fri, 27 Oct 2023 14:28:10 -0500
Subject: [PATCH] made heading_to_sector.

---
 src/heading_drawing.rs   | 29 +++++++++++++++++----------
 src/led.rs               | 43 +++++++++++++++++++++++++++++-----------
 src/tilt_compensation.rs |  6 +++---
 3 files changed, 52 insertions(+), 26 deletions(-)

diff --git a/src/heading_drawing.rs b/src/heading_drawing.rs
index 52b1dfe..85a0a4c 100644
--- a/src/heading_drawing.rs
+++ b/src/heading_drawing.rs
@@ -1,31 +1,38 @@
-use crate::line_drawing::{draw_line, FourQuadrantMatrix, Line, UPoint};
+use core::f32::consts::PI;
+
+use crate::line_drawing::{draw_line, FourQuadrantMatrix, Line, Point, UPoint};
 
 #[derive(Debug, Clone, Copy)]
 struct Sector {
-    total_sectors: u8,
-    sectors: u8,
+    total_sectors: usize,
+    sector: usize,
 }
 
 //heading starts at north, with the positive direction being clockwise.
 //Heading ranges from -pi to pi.
 //
 //sectors have 0 at north an proceed clockwise, always being positive.
-fn heading_to_sector(sectors: u8, heading: f32) -> Sector {
-    todo!()
+fn heading_to_sector(sectors: usize, heading: f32) -> Sector {
+    let half_sector = PI / sectors as f32;
+    let sector_size = 2.0 * half_sector;
+    Sector {
+        total_sectors: sectors,
+        sector: (modulo(heading + half_sector, 2.0 * PI) / (sector_size)) as usize,
+    }
 }
 
-fn sector_to_line(sector: Sector, radius: usize) -> Line {
+fn modulo(a: f32, b: f32) -> f32 {
+    ((a % b) + b) % b
+}
+
+fn heading_to_line(heading: f32, square_size: usize) -> Line {
     todo!()
 }
 
 pub fn draw_heading<const X: usize, const Y: usize>(
     heading: f32,
-    sectors: u8,
 ) -> FourQuadrantMatrix<{ X }, { Y }, u8> {
     let mut ret = FourQuadrantMatrix::new(UPoint { x: X / 2, y: Y / 2 });
-    draw_line::<X, Y>(
-        &sector_to_line(heading_to_sector(sectors, heading), X.max(Y)),
-        &mut ret,
-    );
+    draw_line::<X, Y>(&heading_to_line(heading, X.min(Y)), &mut ret);
     ret
 }
diff --git a/src/led.rs b/src/led.rs
index 0efea88..f1a7c83 100644
--- a/src/led.rs
+++ b/src/led.rs
@@ -92,23 +92,42 @@ pub fn direction_to_led(direction: Direction) -> [[u8; 5]; 5] {
 }
 
 pub fn theta_to_direction(heading: Heading) -> Direction {
+    // if heading.0 < (-7. * PI / 8.) {
+    //     Direction::North
+    // } else if heading.0 < (-5. * PI / 8.) {
+    //     Direction::NorthWest
+    // } else if heading.0 < (-3. * PI / 8.) {
+    //     Direction::West
+    // } else if heading.0 < (-PI / 8.) {
+    //     Direction::SouthWest
+    // } else if heading.0 < (PI / 8.) {
+    //     Direction::South
+    // } else if heading.0 < (3. * PI / 8.) {
+    //     Direction::SouthEast
+    // } else if heading.0 < (5. * PI / 8.) {
+    //     Direction::East
+    // } else if heading.0 < (7. * PI / 8.) {
+    //     Direction::NorthEast
+    // } else {
+    //     Direction::North
+    // }
     if heading.0 < (-7. * PI / 8.) {
-        Direction::North
-    } else if heading.0 < (-5. * PI / 8.) {
-        Direction::NorthWest
-    } else if heading.0 < (-3. * PI / 8.) {
-        Direction::West
-    } else if heading.0 < (-PI / 8.) {
-        Direction::SouthWest
-    } else if heading.0 < (PI / 8.) {
         Direction::South
-    } else if heading.0 < (3. * PI / 8.) {
+    } else if heading.0 < (-5. * PI / 8.) {
         Direction::SouthEast
-    } else if heading.0 < (5. * PI / 8.) {
+    } else if heading.0 < (-3. * PI / 8.) {
         Direction::East
-    } else if heading.0 < (7. * PI / 8.) {
+    } else if heading.0 < (-PI / 8.) {
         Direction::NorthEast
-    } else {
+    } else if heading.0 < (PI / 8.) {
         Direction::North
+    } else if heading.0 < (3. * PI / 8.) {
+        Direction::NorthWest
+    } else if heading.0 < (5. * PI / 8.) {
+        Direction::West
+    } else if heading.0 < (7. * PI / 8.) {
+        Direction::SouthWest
+    } else {
+        Direction::South
     }
 }
diff --git a/src/tilt_compensation.rs b/src/tilt_compensation.rs
index 5dce79e..7c6574b 100644
--- a/src/tilt_compensation.rs
+++ b/src/tilt_compensation.rs
@@ -18,14 +18,14 @@ pub struct NedMeasurement {
 //theta=0 at south, pi/-pi at north, pi/2 at east, and -pi/2 at west (current)
 pub struct Heading(pub f32);
 
-/// board has forward in the -y direction and right in the -x direction, and down in the -z. (SWU),  algs for tilt compensation
+/// board has forward in the y direction and right in the -x direction, and down in the -z. (ENU),  algs for tilt compensation
 /// need forward in +x and right in +y (this is known as the NED (north, east, down) cordinate
 /// system)
 /// also converts to f32
 pub fn swd_to_ned(measurement: Measurement) -> NedMeasurement {
     NedMeasurement {
-        x: measurement.y as f32,
-        y: measurement.x as f32,
+        x: -measurement.y as f32,
+        y: -measurement.x as f32,
         z: -measurement.z as f32,
     }
 }