chore: refactor and finish stick cal

src/config.rs

@@ -0,0 +1,204 @@
+/**
+ *  Storage for controller configuration, including helper functions & types, as well as sane defaults.
+ */
+use core::f32::consts::PI;
+
+use defmt::{info, warn, Format};
+use embassy_rp::{
+    flash::{Async, Flash, ERASE_SIZE},
+    peripherals::FLASH,
+};
+use packed_struct::{derive::PackedStruct, PackedStruct};
+
+use crate::{
+    packed_float::{PackedFloat, ToPackedFloatArray},
+    stick::{NotchStatus, NO_OF_CALIBRATION_POINTS, NO_OF_NOTCHES},
+    ADDR_OFFSET, FLASH_SIZE,
+};
+
+/// This needs to be incremented for ANY change to ControllerConfig
+/// else we risk loading uninitialized memory.
+pub const CONTROLLER_CONFIG_REVISION: u8 = 2;
+
+pub const DEFAULT_NOTCH_STATUS: [NotchStatus; NO_OF_NOTCHES] = [
+    NotchStatus::Cardinal,
+    NotchStatus::TertActive,
+    NotchStatus::Secondary,
+    NotchStatus::TertActive,
+    NotchStatus::Cardinal,
+    NotchStatus::TertActive,
+    NotchStatus::Secondary,
+    NotchStatus::TertActive,
+    NotchStatus::Cardinal,
+    NotchStatus::TertActive,
+    NotchStatus::Secondary,
+    NotchStatus::TertActive,
+    NotchStatus::Cardinal,
+    NotchStatus::TertActive,
+    NotchStatus::Secondary,
+    NotchStatus::TertActive,
+];
+
+#[rustfmt::skip]
+const DEFAULT_CAL_POINTS_X: [f32; NO_OF_CALIBRATION_POINTS] = [
+    0.3010610568,0.3603937084,// right
+	0.3010903951,0.3000194135,
+	0.3005567843,0.3471911134,// up right
+	0.3006904343,0.3009976295,
+	0.3000800899,0.300985051,// up
+	0.3001020858,0.300852804,
+	0.3008746305,0.2548450139,// up left
+	0.3001434092,0.3012600593,
+	0.3011594091,0.2400535218,// left
+	0.3014621077,0.3011248469,
+	0.3010860944,0.2552106305,// down left
+	0.3002197989,0.3001679513,
+	0.3004438517,0.300486505,// down
+	0.3002766984,0.3012828579,
+	0.3014959877,0.346512936,// down right
+	0.3013398149,0.3007809916
+];
+
+#[rustfmt::skip]
+const DEFAULT_CAL_POINTS_Y: [f32; NO_OF_CALIBRATION_POINTS] = [
+    0.300092277, 0.3003803475,// right
+	0.3002205792,0.301004752,
+	0.3001241394,0.3464200104,// up right
+	0.3001331245,0.3011881186,
+	0.3010685972,0.3606900641,// up
+	0.3001520488,0.3010662947,
+	0.3008837105,0.3461478452,// up left
+	0.3011732026,0.3007367683,
+	0.3011345742,0.3000566197,// left
+	0.3006843288,0.3009673425,
+	0.3011228978,0.2547579852,// down left
+	0.3011177285,0.301264851,
+	0.3002376991,0.2403885431,// down
+	0.3006540818,0.3010588401,
+	0.3011093054,0.2555000655,// down right
+	0.3000802760,0.3008482317
+];
+
+const DEFAULT_ANGLES: [f32; NO_OF_NOTCHES] = [
+    0.,
+    PI / 8.0,
+    PI * 2. / 8.,
+    PI * 3. / 8.,
+    PI * 4. / 8.,
+    PI * 5. / 8.,
+    PI * 6. / 8.,
+    PI * 7. / 8.,
+    PI * 8. / 8.,
+    PI * 9. / 8.,
+    PI * 10. / 8.,
+    PI * 11. / 8.,
+    PI * 12. / 8.,
+    PI * 13. / 8.,
+    PI * 14. / 8.,
+    PI * 15. / 8.,
+];
+
+#[derive(Debug, Clone, Format, PackedStruct)]
+#[packed_struct(endian = "msb")]
+pub struct ControllerConfig {
+    #[packed_field(size_bits = "8")]
+    pub config_revision: u8,
+    #[packed_field(size_bits = "8")]
+    pub config_version: u8,
+    #[packed_field(size_bits = "8")]
+    pub ax_waveshaping: u8,
+    #[packed_field(size_bits = "8")]
+    pub ay_waveshaping: u8,
+    #[packed_field(size_bits = "8")]
+    pub cx_waveshaping: u8,
+    #[packed_field(size_bits = "8")]
+    pub cy_waveshaping: u8,
+    #[packed_field(size_bits = "8")]
+    pub astick_analog_scaler: u8,
+    #[packed_field(size_bits = "8")]
+    pub cstick_analog_scaler: u8,
+    #[packed_field(size_bits = "8")]
+    pub x_snapback: i8,
+    #[packed_field(size_bits = "8")]
+    pub y_snapback: i8,
+    #[packed_field(size_bits = "8")]
+    pub x_smoothing: u8,
+    #[packed_field(size_bits = "8")]
+    pub y_smoothing: u8,
+    #[packed_field(size_bits = "8")]
+    pub c_xsmoothing: u8,
+    #[packed_field(size_bits = "8")]
+    pub c_ysmoothing: u8,
+    #[packed_field(element_size_bytes = "4")]
+    pub temp_cal_points_ax: [PackedFloat; 32],
+    #[packed_field(element_size_bytes = "4")]
+    pub temp_cal_points_ay: [PackedFloat; 32],
+    #[packed_field(element_size_bytes = "4")]
+    pub temp_cal_points_cx: [PackedFloat; 32],
+    #[packed_field(element_size_bytes = "4")]
+    pub temp_cal_points_cy: [PackedFloat; 32],
+    #[packed_field(element_size_bytes = "4")]
+    pub a_angles: [PackedFloat; 16],
+    #[packed_field(element_size_bytes = "4")]
+    pub c_angles: [PackedFloat; 16],
+}
+
+impl Default for ControllerConfig {
+    fn default() -> Self {
+        Self {
+            config_revision: CONTROLLER_CONFIG_REVISION,
+            config_version: 0,
+            ax_waveshaping: 0,
+            ay_waveshaping: 0,
+            cx_waveshaping: 0,
+            cy_waveshaping: 0,
+            astick_analog_scaler: 0,
+            cstick_analog_scaler: 0,
+            x_snapback: 0,
+            y_snapback: 0,
+            x_smoothing: 0,
+            y_smoothing: 0,
+            c_xsmoothing: 0,
+            c_ysmoothing: 0,
+            temp_cal_points_ax: *DEFAULT_CAL_POINTS_X.to_packed_float_array(),
+            temp_cal_points_ay: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(),
+            temp_cal_points_cx: *DEFAULT_CAL_POINTS_X.to_packed_float_array(),
+            temp_cal_points_cy: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(),
+            a_angles: *DEFAULT_ANGLES.to_packed_float_array(),
+            c_angles: *DEFAULT_ANGLES.to_packed_float_array(),
+        }
+    }
+}
+
+impl ControllerConfig {
+    pub fn from_flash_memory(
+        mut flash: &mut Flash<'static, FLASH, Async, FLASH_SIZE>,
+    ) -> Result<Self, embassy_rp::flash::Error> {
+        let mut controller_config_packed: <ControllerConfig as packed_struct::PackedStruct>::ByteArray = [0u8; 654]; // ControllerConfig byte size
+        flash.blocking_read(ADDR_OFFSET, &mut controller_config_packed)?;
+
+        match ControllerConfig::unpack(&controller_config_packed).unwrap() {
+            a if a.config_revision == CONTROLLER_CONFIG_REVISION => {
+                info!("Controller config loaded from flash: {}", a);
+                Ok(a)
+            }
+            a => {
+                warn!("Outdated controller config detected ({:02X}), or controller config was never present, using default.", a.config_revision);
+                let cfg = ControllerConfig::default();
+                info!("Going to save default controller config.");
+                cfg.write_to_flash(&mut flash)?;
+                Ok(cfg)
+            }
+        }
+    }
+
+    pub fn write_to_flash(
+        &self,
+        flash: &mut Flash<'static, FLASH, Async, FLASH_SIZE>,
+    ) -> Result<(), embassy_rp::flash::Error> {
+        info!("Writing controller config to flash.");
+        flash.blocking_erase(ADDR_OFFSET, ADDR_OFFSET + ERASE_SIZE as u32)?;
+        flash.blocking_write(ADDR_OFFSET, &self.pack().unwrap())?;
+        Ok(())
+    }
+}

src/filter.rs

@@ -1,7 +1,7 @@
 use defmt::Format;
 use libm::{fminf, powf};
 
-use crate::input::{ControllerConfig, Stick};
+use crate::config::ControllerConfig;
 
 /// Filter gains for 800Hz, the ones for 1000Hz are provided by `get_norm_gains`
 pub const FILTER_GAINS: FilterGains = FilterGains {
@@ -85,7 +85,7 @@ pub struct FilterGains {
 
 impl FilterGains {
     /// Returns filter gains for 1000Hz polling rate
-    pub fn normalize_gains(&self, controller_config: &ControllerConfig) -> Self {
+    pub fn get_normalized_gains(&self, controller_config: &ControllerConfig) -> Self {
         let mut gains = self.clone();
 
         gains.x_vel_damp = vel_damp_from_snapback(controller_config.x_snapback);

src/gcc_hid.rs

@@ -1,21 +1,20 @@
+/**
+ * Communication with the console / PC over USB HID.
+ * Includes the HID report descriptor, and the GcReport struct.
+ */
 use core::default::Default;
 
-use defmt::{debug, error, info, trace, unwrap, warn, Debug2Format, Format};
+use defmt::{debug, info, trace, warn, Format};
-use embassy_futures::{
+use embassy_futures::join::join;
-    join::join,
-    select::{self, select, Either},
-};
 use embassy_rp::{peripherals::USB, usb::Driver};
-use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+
-use embassy_sync::signal::Signal;
+use embassy_time::Instant;
-use embassy_time::{Duration, Instant};
 use embassy_usb::{
     class::hid::{HidReaderWriter, ReportId, RequestHandler, State},
     control::OutResponse,
     Builder, Handler,
 };
 use packed_struct::{derive::PackedStruct, PackedStruct};
-use portable_atomic::Ordering;
 
 use crate::input::GCC_SIGNAL;
 
@@ -238,27 +237,23 @@ impl Handler for MyDeviceHandler {
 pub async fn usb_transfer_loop(driver: Driver<'static, USB>, raw_serial: [u8; 8]) {
     let mut serial_buffer = [0u8; 64];
 
-    let serial = {
+    let serial = format_no_std::show(
-        let s = format_no_std::show(
+        &mut serial_buffer,
-            &mut serial_buffer,
+        format_args!(
-            format_args!(
+            "{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}",
-                "{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}{:02X}",
+            raw_serial[0],
-                raw_serial[0],
+            raw_serial[1],
-                raw_serial[1],
+            raw_serial[2],
-                raw_serial[2],
+            raw_serial[3],
-                raw_serial[3],
+            raw_serial[4],
-                raw_serial[4],
+            raw_serial[5],
-                raw_serial[5],
+            raw_serial[6],
-                raw_serial[6],
+            raw_serial[7]
-                raw_serial[7]
+        ),
-            ),
+    )
-        )
+    .unwrap();
-        .unwrap();
 
-        info!("Detected flash with unique serial number {}", s);
+    info!("Detected flash with unique serial number {}", serial);
-
-        s
-    };
 
     trace!("Start of config");
     let mut usb_config = embassy_usb::Config::new(0x057e, 0x0337);

src/input.rs

@@ -1,9 +1,9 @@
 use core::task::Poll;
 
-use defmt::{debug, info, warn, Format};
+use defmt::info;
 use embassy_futures::{join::join, yield_now};
 use embassy_rp::{
-    flash::{Async, Flash, ERASE_SIZE},
+    flash::{Async, Flash},
     gpio::{Input, Output, Pin},
     peripherals::{
         FLASH, PIN_10, PIN_11, PIN_16, PIN_17, PIN_18, PIN_19, PIN_20, PIN_21, PIN_22, PIN_23,
@@ -14,18 +14,14 @@ use embassy_rp::{
 };
 use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
 use embassy_time::{Instant, Timer};
-use libm::{fmaxf, fmin, fminf};
+use libm::{fmaxf, fminf};
-use packed_struct::{derive::PackedStruct, PackedStruct};
 
 use crate::{
+    config::ControllerConfig,
     filter::{run_kalman, run_waveshaping, FilterGains, WaveshapingValues, FILTER_GAINS},
     gcc_hid::GcReport,
-    packed_float::{PackedFloat, ToPackedFloatArray},
+    stick::{linearize, notch_remap, StickParams},
-    stick::{
+    FLASH_SIZE,
-        linearize, notch_remap, StickParams, DEFAULT_ANGLES, DEFAULT_CAL_POINTS_X,
-        DEFAULT_CAL_POINTS_Y, NO_OF_NOTCHES,
-    },
-    ADDR_OFFSET, FLASH_SIZE,
 };
 
 pub static GCC_SIGNAL: Signal<CriticalSectionRawMutex, GcReport> = Signal::new();
@@ -34,113 +30,6 @@ static STICK_SIGNAL: Signal<CriticalSectionRawMutex, StickState> = Signal::new()
 const STICK_HYST_VAL: f32 = 0.3;
 const FLOAT_ORIGIN: f32 = 127.5;
 
-pub const CONTROLLER_CONFIG_REVISION: u8 = 2;
-
-#[derive(Debug, Clone, Format, PackedStruct)]
-#[packed_struct(endian = "msb")]
-pub struct ControllerConfig {
-    #[packed_field(size_bits = "8")]
-    pub config_revision: u8,
-    #[packed_field(size_bits = "8")]
-    pub config_version: u8,
-    #[packed_field(size_bits = "8")]
-    pub ax_waveshaping: u8,
-    #[packed_field(size_bits = "8")]
-    pub ay_waveshaping: u8,
-    #[packed_field(size_bits = "8")]
-    pub cx_waveshaping: u8,
-    #[packed_field(size_bits = "8")]
-    pub cy_waveshaping: u8,
-    #[packed_field(size_bits = "8")]
-    pub astick_analog_scaler: u8,
-    #[packed_field(size_bits = "8")]
-    pub cstick_analog_scaler: u8,
-    #[packed_field(size_bits = "8")]
-    pub x_snapback: i8,
-    #[packed_field(size_bits = "8")]
-    pub y_snapback: i8,
-    #[packed_field(size_bits = "8")]
-    pub x_smoothing: u8,
-    #[packed_field(size_bits = "8")]
-    pub y_smoothing: u8,
-    #[packed_field(size_bits = "8")]
-    pub c_xsmoothing: u8,
-    #[packed_field(size_bits = "8")]
-    pub c_ysmoothing: u8,
-    #[packed_field(element_size_bytes = "4")]
-    pub temp_cal_points_ax: [PackedFloat; 32],
-    #[packed_field(element_size_bytes = "4")]
-    pub temp_cal_points_ay: [PackedFloat; 32],
-    #[packed_field(element_size_bytes = "4")]
-    pub temp_cal_points_cx: [PackedFloat; 32],
-    #[packed_field(element_size_bytes = "4")]
-    pub temp_cal_points_cy: [PackedFloat; 32],
-    #[packed_field(element_size_bytes = "4")]
-    pub a_angles: [PackedFloat; 16],
-    #[packed_field(element_size_bytes = "4")]
-    pub c_angles: [PackedFloat; 16],
-}
-
-impl Default for ControllerConfig {
-    fn default() -> Self {
-        Self {
-            config_revision: CONTROLLER_CONFIG_REVISION,
-            config_version: 0,
-            ax_waveshaping: 0,
-            ay_waveshaping: 0,
-            cx_waveshaping: 0,
-            cy_waveshaping: 0,
-            astick_analog_scaler: 0,
-            cstick_analog_scaler: 0,
-            x_snapback: 0,
-            y_snapback: 0,
-            x_smoothing: 0,
-            y_smoothing: 0,
-            c_xsmoothing: 0,
-            c_ysmoothing: 0,
-            temp_cal_points_ax: *DEFAULT_CAL_POINTS_X.to_packed_float_array(),
-            temp_cal_points_ay: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(),
-            temp_cal_points_cx: *DEFAULT_CAL_POINTS_X.to_packed_float_array(),
-            temp_cal_points_cy: *DEFAULT_CAL_POINTS_Y.to_packed_float_array(),
-            a_angles: *DEFAULT_ANGLES.to_packed_float_array(),
-            c_angles: *DEFAULT_ANGLES.to_packed_float_array(),
-        }
-    }
-}
-
-impl ControllerConfig {
-    pub fn from_flash_memory(
-        mut flash: &mut Flash<'static, FLASH, Async, FLASH_SIZE>,
-    ) -> Result<Self, embassy_rp::flash::Error> {
-        let mut controller_config_packed: <ControllerConfig as packed_struct::PackedStruct>::ByteArray = [0u8; 654]; // ControllerConfig byte size
-        flash.blocking_read(ADDR_OFFSET, &mut controller_config_packed)?;
-
-        match ControllerConfig::unpack(&controller_config_packed).unwrap() {
-            a if a.config_revision == CONTROLLER_CONFIG_REVISION => {
-                info!("Controller config loaded from flash: {}", a);
-                Ok(a)
-            }
-            a => {
-                warn!("Outdated controller config detected ({:02X}), or controller config was never present, using default.", a.config_revision);
-                let cfg = ControllerConfig::default();
-                info!("Going to save default controller config.");
-                cfg.write_to_flash(&mut flash)?;
-                Ok(cfg)
-            }
-        }
-    }
-
-    pub fn write_to_flash(
-        &self,
-        flash: &mut Flash<'static, FLASH, Async, FLASH_SIZE>,
-    ) -> Result<(), embassy_rp::flash::Error> {
-        info!("Writing controller config to flash.");
-        flash.blocking_erase(ADDR_OFFSET, ADDR_OFFSET + ERASE_SIZE as u32)?;
-        flash.blocking_write(ADDR_OFFSET, &self.pack().unwrap())?;
-        Ok(())
-    }
-}
-
 #[derive(Clone, Debug, Default)]
 struct StickState {
     ax: u8,
@@ -493,7 +382,7 @@ pub async fn input_loop(
     let (controlstick_params, cstick_params) =
         StickParams::from_controller_config(&controller_config);
 
-    let filter_gains = FILTER_GAINS.normalize_gains(&controller_config);
+    let filter_gains = FILTER_GAINS.get_normalized_gains(&controller_config);
 
     let stick_state_fut = async {
         let mut current_stick_state = StickState {

src/main.rs

@@ -4,6 +4,7 @@
 
 #![no_std]
 #![no_main]
+mod config;
 mod filter;
 mod gcc_hid;
 mod input;

src/stick.rs

@@ -6,7 +6,8 @@ use defmt::{debug, Format};
 use libm::{atan2f, cosf, fabs, roundf, sinf, sqrtf};
 
 use crate::{
-    input::{ControllerConfig, Stick},
+    config::{ControllerConfig, DEFAULT_NOTCH_STATUS},
+    input::Stick,
     packed_float::ToRegularArray,
 };
 
@@ -21,84 +22,6 @@ const MAX_ORDER: usize = 20;
 /// 28 degrees; this is the max angular deflection of the stick.
 const MAX_STICK_ANGLE: f32 = 0.4886921906;
 
-const DEFAULT_NOTCH_STATUS: [NotchStatus; NO_OF_NOTCHES] = [
-    NotchStatus::Cardinal,
-    NotchStatus::TertActive,
-    NotchStatus::Secondary,
-    NotchStatus::TertActive,
-    NotchStatus::Cardinal,
-    NotchStatus::TertActive,
-    NotchStatus::Secondary,
-    NotchStatus::TertActive,
-    NotchStatus::Cardinal,
-    NotchStatus::TertActive,
-    NotchStatus::Secondary,
-    NotchStatus::TertActive,
-    NotchStatus::Cardinal,
-    NotchStatus::TertActive,
-    NotchStatus::Secondary,
-    NotchStatus::TertActive,
-];
-
-#[rustfmt::skip]
-pub const DEFAULT_CAL_POINTS_X: [f32; NO_OF_CALIBRATION_POINTS] = [
-    0.3010610568,0.3603937084,// right
-	0.3010903951,0.3000194135,
-	0.3005567843,0.3471911134,// up right
-	0.3006904343,0.3009976295,
-	0.3000800899,0.300985051,// up
-	0.3001020858,0.300852804,
-	0.3008746305,0.2548450139,// up left
-	0.3001434092,0.3012600593,
-	0.3011594091,0.2400535218,// left
-	0.3014621077,0.3011248469,
-	0.3010860944,0.2552106305,// down left
-	0.3002197989,0.3001679513,
-	0.3004438517,0.300486505,// down
-	0.3002766984,0.3012828579,
-	0.3014959877,0.346512936,// down right
-	0.3013398149,0.3007809916
-];
-
-#[rustfmt::skip]
-pub const DEFAULT_CAL_POINTS_Y: [f32; NO_OF_CALIBRATION_POINTS] = [
-    0.300092277, 0.3003803475,// right
-	0.3002205792,0.301004752,
-	0.3001241394,0.3464200104,// up right
-	0.3001331245,0.3011881186,
-	0.3010685972,0.3606900641,// up
-	0.3001520488,0.3010662947,
-	0.3008837105,0.3461478452,// up left
-	0.3011732026,0.3007367683,
-	0.3011345742,0.3000566197,// left
-	0.3006843288,0.3009673425,
-	0.3011228978,0.2547579852,// down left
-	0.3011177285,0.301264851,
-	0.3002376991,0.2403885431,// down
-	0.3006540818,0.3010588401,
-	0.3011093054,0.2555000655,// down right
-	0.3000802760,0.3008482317
-];
-
-pub const DEFAULT_ANGLES: [f32; NO_OF_NOTCHES] = [
-    0.,
-    PI / 8.0,
-    PI * 2. / 8.,
-    PI * 3. / 8.,
-    PI * 4. / 8.,
-    PI * 5. / 8.,
-    PI * 6. / 8.,
-    PI * 7. / 8.,
-    PI * 8. / 8.,
-    PI * 9. / 8.,
-    PI * 10. / 8.,
-    PI * 11. / 8.,
-    PI * 12. / 8.,
-    PI * 13. / 8.,
-    PI * 14. / 8.,
-    PI * 15. / 8.,
-];
-
 #[rustfmt::skip]
 //                                                             right        notch 1      up right     notch 2      up           notch 3      up left      notch 4      left         notch 5      down left    notch 6      down         notch 7      down right   notch 8
 //                                                             0            1            2            3            4            5            6            7            8            9            10           11           12           13           14           15
@@ -134,12 +57,40 @@ impl StickParams {
             &controller_config.c_angles.to_regular_array(),
         );
 
-        todo!()
+        let linearized_cal_astick =
+            LinearizedCalibration::from_calibration_points(&cleaned_cal_points_astick);
+        let linearized_cal_cstick =
+            LinearizedCalibration::from_calibration_points(&cleaned_cal_points_cstick);
+
+        let notch_cal_astick = NotchCalibration::from_cleaned_and_linearized_calibration(
+            &cleaned_cal_points_astick,
+            &linearized_cal_astick,
+        );
+        let notch_cal_cstick = NotchCalibration::from_cleaned_and_linearized_calibration(
+            &cleaned_cal_points_cstick,
+            &linearized_cal_cstick,
+        );
+
+        let stick_params_astick = Self {
+            fit_coeffs_x: linearized_cal_astick.fit_coeffs_x.map(|e| e as f32),
+            fit_coeffs_y: linearized_cal_astick.fit_coeffs_y.map(|e| e as f32),
+            affine_coeffs: notch_cal_astick.affine_coeffs,
+            boundary_angles: notch_cal_astick.boundary_angles,
+        };
+
+        let stick_params_cstick = Self {
+            fit_coeffs_x: linearized_cal_cstick.fit_coeffs_x.map(|e| e as f32),
+            fit_coeffs_y: linearized_cal_cstick.fit_coeffs_y.map(|e| e as f32),
+            affine_coeffs: notch_cal_cstick.affine_coeffs,
+            boundary_angles: notch_cal_cstick.boundary_angles,
+        };
+
+        (stick_params_astick, stick_params_cstick)
     }
 }
 
 #[derive(Clone, Debug, Format, Copy)]
-enum NotchStatus {
+pub enum NotchStatus {
     TertInactive,
     TertActive,
     Secondary,
@@ -293,8 +244,8 @@ struct LinearizedCalibration {
     pub fit_coeffs_x: [f64; NUM_COEFFS],
     pub fit_coeffs_y: [f64; NUM_COEFFS],
 
-    pub out_x: [f32; NO_OF_NOTCHES],
+    pub linearized_points_x: [f32; NO_OF_NOTCHES + 1],
-    pub out_y: [f32; NO_OF_NOTCHES],
+    pub linearized_points_y: [f32; NO_OF_NOTCHES + 1],
 }
 
 impl LinearizedCalibration {
@@ -306,10 +257,17 @@ impl LinearizedCalibration {
     ///
     ///	Outputs:
     ///		linearization fit coefficients for X and Y
-    pub fn from_calibration_points(in_x: &[f64; 17], in_y: &[f64; 17]) -> Self {
+    pub fn from_calibration_points(cleaned_calibration_points: &CleanedCalibrationPoints) -> Self {
         let mut fit_points_x = [0f64; 5];
         let mut fit_points_y = [0f64; 5];
 
+        let in_x = cleaned_calibration_points
+            .cleaned_points_x
+            .map(|e| e as f64);
+        let in_y = cleaned_calibration_points
+            .cleaned_points_y
+            .map(|e| e as f64);
+
         fit_points_x[0] = in_x[8 + 1];
         fit_points_x[1] = (in_x[6 + 1] + in_x[10 + 1]) / 2.0f64;
         fit_points_x[2] = in_x[0];
@@ -336,23 +294,148 @@ impl LinearizedCalibration {
         fit_coeffs_x[3] = fit_coeffs_x[3] - x_zero_error as f64;
         fit_coeffs_y[3] = fit_coeffs_y[3] - y_zero_error as f64;
 
-        let mut out_x = [0f32; NO_OF_NOTCHES];
+        let mut linearized_points_x = [0f32; NO_OF_NOTCHES + 1];
-        let mut out_y = [0f32; NO_OF_NOTCHES];
+        let mut linearized_points_y = [0f32; NO_OF_NOTCHES + 1];
 
         for i in 0..=NO_OF_NOTCHES {
-            out_x[i] = linearize(in_x[i] as f32, &fit_coeffs_x.map(|e| e as f32));
+            linearized_points_x[i] = linearize(in_x[i] as f32, &fit_coeffs_x.map(|e| e as f32));
-            out_y[i] = linearize(in_y[i] as f32, &fit_coeffs_y.map(|e| e as f32));
+            linearized_points_y[i] = linearize(in_y[i] as f32, &fit_coeffs_y.map(|e| e as f32));
         }
 
         Self {
             fit_coeffs_x,
             fit_coeffs_y,
-            out_x,
+            linearized_points_x,
-            out_y,
+            linearized_points_y,
         }
     }
 }
 
+#[derive(Clone, Debug, Default)]
+struct NotchCalibration {
+    affine_coeffs: [[f32; 16]; 4],
+    boundary_angles: [f32; 4],
+}
+
+impl NotchCalibration {
+    fn from_cleaned_and_linearized_calibration(
+        cleaned_calibration_points: &CleanedCalibrationPoints,
+        linearized_calibration: &LinearizedCalibration,
+    ) -> Self {
+        let mut out = Self::default();
+
+        for i in 1..=NO_OF_NOTCHES {
+            let mut points_in = [[0f32; 3]; 3];
+            let mut points_out = [[0f32; 3]; 3];
+
+            if i == NO_OF_NOTCHES {
+                points_in[0][0] = linearized_calibration.linearized_points_x[0];
+                points_in[0][1] = linearized_calibration.linearized_points_x[i];
+                points_in[0][2] = linearized_calibration.linearized_points_x[1];
+                points_in[1][0] = linearized_calibration.linearized_points_y[0];
+                points_in[1][1] = linearized_calibration.linearized_points_y[i];
+                points_in[1][2] = linearized_calibration.linearized_points_y[1];
+                points_in[2][0] = 1.;
+                points_in[2][1] = 1.;
+                points_in[2][2] = 1.;
+                points_out[0][0] = cleaned_calibration_points.notch_points_x[0];
+                points_out[0][1] = cleaned_calibration_points.notch_points_x[i];
+                points_out[0][2] = cleaned_calibration_points.notch_points_x[1];
+                points_out[1][0] = cleaned_calibration_points.notch_points_y[0];
+                points_out[1][1] = cleaned_calibration_points.notch_points_y[i];
+                points_out[1][2] = cleaned_calibration_points.notch_points_y[1];
+                points_out[2][0] = 1.;
+                points_out[2][1] = 1.;
+                points_out[2][2] = 1.;
+            } else {
+                points_in[0][0] = linearized_calibration.linearized_points_x[0];
+                points_in[0][1] = linearized_calibration.linearized_points_x[i];
+                points_in[0][2] = linearized_calibration.linearized_points_x[i + 1];
+                points_in[1][0] = linearized_calibration.linearized_points_y[0];
+                points_in[1][1] = linearized_calibration.linearized_points_y[i];
+                points_in[1][2] = linearized_calibration.linearized_points_y[i + 1];
+                points_in[2][0] = 1.;
+                points_in[2][1] = 1.;
+                points_in[2][2] = 1.;
+                points_out[0][0] = cleaned_calibration_points.notch_points_x[0];
+                points_out[0][1] = cleaned_calibration_points.notch_points_x[i];
+                points_out[0][2] = cleaned_calibration_points.notch_points_x[i + 1];
+                points_out[1][0] = cleaned_calibration_points.notch_points_y[0];
+                points_out[1][1] = cleaned_calibration_points.notch_points_y[i];
+                points_out[1][2] = cleaned_calibration_points.notch_points_y[i + 1];
+                points_out[2][0] = 1.;
+                points_out[2][1] = 1.;
+                points_out[2][2] = 1.;
+            }
+            debug!("In points: {:?}", points_in);
+            debug!("Out points: {:?}", points_out);
+
+            let temp = inverse(&points_in);
+
+            let a = matrix_mult(&points_out, &temp);
+
+            debug!("The transform matrix is: {:?}", a);
+
+            for j in 0..2 {
+                for k in 0..2 {
+                    out.affine_coeffs[i - 1][j * 2 + k] = a[j][k];
+                }
+            }
+
+            debug!(
+                "Transform coefficients for this region are: {:?}",
+                out.affine_coeffs[i - 1]
+            );
+
+            out.boundary_angles[i - 1] = match atan2f(
+                cleaned_calibration_points.cleaned_points_y[i]
+                    - cleaned_calibration_points.cleaned_points_y[0],
+                cleaned_calibration_points.cleaned_points_x[i]
+                    - cleaned_calibration_points.cleaned_points_x[0],
+            ) {
+                a if a < out.boundary_angles[0] => a + 2. * PI,
+                a => a,
+            };
+        }
+
+        out
+    }
+}
+
+fn inverse(in_mat: &[[f32; 3]; 3]) -> [[f32; 3]; 3] {
+    let mut out_mat = [[0f32; 3]; 3];
+
+    let det = in_mat[0][0] * (in_mat[1][1] * in_mat[2][2] - in_mat[1][2] * in_mat[2][1])
+        - in_mat[0][1] * (in_mat[1][0] * in_mat[2][2] - in_mat[1][2] * in_mat[2][0])
+        + in_mat[0][2] * (in_mat[1][0] * in_mat[2][1] - in_mat[1][1] * in_mat[2][0]);
+
+    out_mat[0][0] = (in_mat[1][1] * in_mat[2][2] - in_mat[1][2] * in_mat[2][1]) / det;
+    out_mat[0][1] = (in_mat[0][2] * in_mat[2][1] - in_mat[0][1] * in_mat[2][2]) / det;
+    out_mat[0][2] = (in_mat[0][1] * in_mat[1][2] - in_mat[0][2] * in_mat[1][1]) / det;
+    out_mat[1][0] = (in_mat[1][2] * in_mat[2][0] - in_mat[1][0] * in_mat[2][2]) / det;
+    out_mat[1][1] = (in_mat[0][0] * in_mat[2][2] - in_mat[0][2] * in_mat[2][0]) / det;
+    out_mat[1][2] = (in_mat[0][2] * in_mat[1][0] - in_mat[0][0] * in_mat[1][2]) / det;
+    out_mat[2][0] = (in_mat[1][0] * in_mat[2][1] - in_mat[1][1] * in_mat[2][0]) / det;
+    out_mat[2][1] = (in_mat[0][1] * in_mat[2][0] - in_mat[0][0] * in_mat[2][1]) / det;
+    out_mat[2][2] = (in_mat[0][0] * in_mat[1][1] - in_mat[0][1] * in_mat[1][0]) / det;
+
+    out_mat
+}
+
+fn matrix_mult(a: &[[f32; 3]; 3], b: &[[f32; 3]; 3]) -> [[f32; 3]; 3] {
+    let mut out = [[0f32; 3]; 3];
+
+    for i in 0..3 {
+        for j in 0..3 {
+            for k in 0..3 {
+                out[i][j] += a[i][k] * b[k][j];
+            }
+        }
+    }
+
+    out
+}
+
 /// Calculate the power of a number
 fn curve_fit_power(base: f64, exponent: u32) -> f64 {
     if exponent == 0 {