joycon-driver 0.0.0

use std::f64::consts::PI;

use nalgebra as na;
use nom::{combinator::eof, sequence::tuple};

use crate::{
    error::{JoyConDriverError, JoyConDriverResult},
    joycon::{JoyConDevice, SUB_COMMAND_READ_HEADER_BYTES, USER_CALIBRATION_DATA_MAGIC},
    parser::parse_i16_vec3,
};

const G: f64 = 9.80665;

#[derive(Debug, Clone)]
pub struct IMUDataRaw {
    pub accel: na::Vector3<i16>,
    pub gyro: na::Vector3<i16>,
}

impl IMUDataRaw {
    pub fn to_data(&self) -> IMUData {
        IMUData::from_raw(self)
    }

    pub fn to_data_with_calib(&self, calib: &IMUCalibration) -> IMUData {
        IMUData::from_raw_with_calib(self, calib)
    }
}

impl TryFrom<&[u8]> for IMUDataRaw {
    type Error = JoyConDriverError;

    fn try_from(buf: &[u8]) -> JoyConDriverResult<Self> {
        let (_, (accel, gyro, _)) = tuple((parse_i16_vec3, parse_i16_vec3, eof))(buf)
            .map_err(|_| JoyConDriverError::InvalidResultBuffer)?;

        Ok(Self { accel, gyro })
    }
}

type F64Vec3 = na::Vector3<f64>;

#[derive(Debug, Clone)]
pub struct IMUData {
    pub accel: F64Vec3,
    pub gyro: F64Vec3,
}

impl IMUData {
    pub fn from_raw(raw: &IMUDataRaw) -> Self {
        let mut accel_raw_f64: F64Vec3 = na::convert(raw.accel);
        accel_raw_f64[0] -= 350.;
        let gyro_raw_f64: F64Vec3 = na::convert(raw.gyro);

        Self {
            accel: accel_raw_f64 * (16. / 65535. * G),
            gyro: gyro_raw_f64 * (4588. / 65535. * PI / 180.),
        }
    }

    pub fn from_raw_with_calib(raw: &IMUDataRaw, calib: &IMUCalibration) -> Self {
        let accel_raw_f64: F64Vec3 = na::convert(raw.accel);
        let acell_coeff_inv: F64Vec3 = na::convert(calib.accel_sensitivity - calib.accel_origin);

        let gyro_minus_offset: F64Vec3 = na::convert(raw.gyro - calib.gyro_origin);
        let gyro_coeff_inv: F64Vec3 = na::convert(calib.gyro_sensitivity - calib.gyro_origin);

        Self {
            accel: (accel_raw_f64 * 4.0).component_div(&acell_coeff_inv) * G,
            gyro: (gyro_minus_offset * 936.).component_div(&gyro_coeff_inv) * (PI / 180.),
        }
    }
}

// https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering/blob/master/spi_flash_notes.md#6-axis-sensor-factory-and-user-calibration
#[derive(Debug, Clone)]
pub struct IMUCalibration {
    pub accel_origin: na::Vector3<i16>,
    pub accel_sensitivity: na::Vector3<i16>,
    pub gyro_origin: na::Vector3<i16>,
    pub gyro_sensitivity: na::Vector3<i16>,
}

impl IMUCalibration {
    const DATA_BYTES: usize = 24;

    pub fn read_factory_data(device: &JoyConDevice) -> JoyConDriverResult<Self> {
        let mut buf = [0u8; SUB_COMMAND_READ_HEADER_BYTES + Self::DATA_BYTES];
        device.read(&mut buf, 0x6020)?;
        Self::try_from(&buf[SUB_COMMAND_READ_HEADER_BYTES..])
    }

    pub fn read_user_data(device: &JoyConDevice) -> JoyConDriverResult<Option<Self>> {
        let mut buf = [0u8; SUB_COMMAND_READ_HEADER_BYTES
            + USER_CALIBRATION_DATA_MAGIC.len()
            + Self::DATA_BYTES];
        device.read(&mut buf, 0x8026)?;
        const DATA_START: usize = SUB_COMMAND_READ_HEADER_BYTES + USER_CALIBRATION_DATA_MAGIC.len();
        if buf[SUB_COMMAND_READ_HEADER_BYTES..DATA_START] == USER_CALIBRATION_DATA_MAGIC {
            Self::try_from(&buf[DATA_START..]).map(Some)
        } else {
            Ok(None)
        }
    }
}

impl TryFrom<&[u8]> for IMUCalibration {
    type Error = JoyConDriverError;

    fn try_from(buf: &[u8]) -> JoyConDriverResult<Self> {
        let (_, (accel_bias, accel_sensitivity, gyro_bias, gyro_sensitivity, _)) = tuple((
            parse_i16_vec3,
            parse_i16_vec3,
            parse_i16_vec3,
            parse_i16_vec3,
            eof,
        ))(buf)
        .map_err(|_| JoyConDriverError::InvalidResultBuffer)?;

        Ok(Self {
            accel_origin: accel_bias,
            accel_sensitivity,
            gyro_origin: gyro_bias,
            gyro_sensitivity,
        })
    }
}

#[derive(Debug, Clone)]
pub struct IMUOffset(pub na::Vector3<i16>);

impl IMUOffset {
    const DATA_BYTES: usize = 6;

    pub fn read_data(device: &JoyConDevice) -> JoyConDriverResult<Self> {
        let mut buf = [0u8; SUB_COMMAND_READ_HEADER_BYTES + Self::DATA_BYTES];
        device.read(&mut buf, 0x6080)?;
        Self::try_from(&buf[SUB_COMMAND_READ_HEADER_BYTES..])
    }
}

impl TryFrom<&[u8]> for IMUOffset {
    type Error = JoyConDriverError;

    fn try_from(buf: &[u8]) -> JoyConDriverResult<Self> {
        let (_, (offset, _)) = tuple((parse_i16_vec3, eof))(buf)
            .map_err(|_| JoyConDriverError::InvalidResultBuffer)?;

        Ok(IMUOffset(offset))
    }
}