spyder-ik 0.1.0

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

use nalgebra::Vector3;
use thiserror::Error;

use crate::{
    leg::{Leg, LegId},
    spyder::{LegParams, Spyder},
};

const DEFAULT_CW_ANG_RES: u64 = 36;
const DEFAULT_AZIMUTH: f64 = PI / 3.0;
pub const NUM_LEGS: u64 = 6;

#[derive(Error, Debug)]
pub enum SpyderBuilderError {
    #[error("Missing required parameters: {0}")]
    MissingParams(String),

    #[error("Invalid link lengths {0}")]
    InvalidGeometry(String),

    #[error("Invalid Parameter - expected: {0} got: {1}")]
    InvalidParameter(String, u64),
}

#[derive(Clone, Debug, Default)]
pub struct SpyderBuilder {
    pub leg_params: Option<LegParams>,
    pub canonical_home: Option<Vector3<f64>>,
    pub cw_ang_resolution: Option<u64>,
    pub azimuth_rad: Option<f64>,
}

impl SpyderBuilder {
    /// Leg link lengths
    pub fn leg_params(mut self, l1: f64, l2: f64, l3: f64) -> Self {
        self.leg_params = Some(LegParams { l1, l2, l3 });
        self
    }

    /// home position as distance from center joint to canonical target
    /// for a standing pose
    pub fn home(mut self, x: f64, y: f64, z: f64) -> Self {
        self.canonical_home = Some(Vector3::new(x, y, z));
        self
    }

    pub fn azimuth_angle(mut self, beta: f64) -> Self {
        self.azimuth_rad = Some(beta.to_radians());
        self
    }

    /// directions available to hexapod walk cycle 360/cw_angle_resolution
    /// measured clockwise
    pub fn cw_ang_resolution(mut self, cw_angle_resolution: u64) -> Self {
        self.cw_ang_resolution = Some(cw_angle_resolution);
        self
    }

    pub fn build(&self) -> Result<Spyder, SpyderBuilderError> {
        let Some(home) = self.canonical_home else {
            return Err(SpyderBuilderError::MissingParams("home".to_string()))?;
        };

        let Some(leg_params) = &self.leg_params else {
            return Err(SpyderBuilderError::MissingParams("leg_params".to_string()));
        };

        if !(leg_params.l1.is_normal() && leg_params.l2.is_normal() && leg_params.l3.is_normal()) {
            return Err(SpyderBuilderError::InvalidGeometry(format!(
                "{:?}",
                self.leg_params.clone()
            )));
        }

        let cw_ang_resolution = self.cw_ang_resolution.unwrap_or(DEFAULT_CW_ANG_RES);
        if cw_ang_resolution == 0 {
            return Err(SpyderBuilderError::InvalidParameter(
                "cw_ang_resolution != 0".to_string(),
                cw_ang_resolution,
            ));
        }

        // define leg and associated leg frame
        let mut legs: Vec<Leg> = vec![];
        for i in 0..NUM_LEGS {
            // Map leg id
            let id = if i.is_multiple_of(2) {
                LegId::Group1 { id: i }
            } else {
                LegId::Group2 { id: i }
            };
            // Get center yaw
            legs.push(Leg {
                heading: None,
                id,
                home,
            });
        }

        Ok(Spyder::new(
            legs,
            leg_params.clone(),
            self.cw_ang_resolution.unwrap_or(DEFAULT_CW_ANG_RES),
            self.azimuth_rad.unwrap_or(DEFAULT_AZIMUTH),
        ))
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_valid_builder() {
        let spyder = SpyderBuilder::default()
            .leg_params(42.25, 120.0, 180.0)
            .home(160.0, 0.0, -100.0)
            .azimuth_angle(60.0)
            .cw_ang_resolution(60)
            .build()
            .unwrap();

        // legs
        spyder
            .legs
            .iter()
            .for_each(|leg| assert_eq!(leg.home, Vector3::new(160.0, 0.0, -100.0)));

        // link lengths
        assert_eq!(spyder.leg_params.l1, 42.25);
        assert_eq!(spyder.leg_params.l2, 120.0);
        assert_eq!(spyder.leg_params.l3, 180.0);

        // azimuth
        assert_eq!(spyder.azimuth_rad, 60.0_f64.to_radians())
    }
}