cu-lewansoul 0.13.0

use bincode::{Decode, Encode};
use cu_linux_resources::LinuxSerialPort;
use cu29::prelude::*;
use cu29::resource::{Owned, ResourceBindingMap, ResourceBindings, ResourceManager};
use cu29::units::si::angle::degree;
use cu29::units::si::f32::Angle;
use serde::{Deserialize, Serialize};
use std::io::{self, Read, Write};

#[allow(dead_code)]
mod servo {
    // From "lx-16a LewanSoul Bus Servo Communication Protocol.pdf"
    pub const SERVO_MOVE_TIME_WRITE: u8 = 1; // 7 bytes
    pub const SERVO_MOVE_TIME_READ: u8 = 2; // 3 bytes
    pub const SERVO_MOVE_TIME_WAIT_WRITE: u8 = 7; // 7 bytes
    pub const SERVO_MOVE_TIME_WAIT_READ: u8 = 8; // 3 bytes
    pub const SERVO_MOVE_START: u8 = 11; // 3 bytes
    pub const SERVO_MOVE_STOP: u8 = 12; // 3 bytes
    pub const SERVO_ID_WRITE: u8 = 13; // 4 bytes
    pub const SERVO_ID_READ: u8 = 14; // 3 bytes
    pub const SERVO_ANGLE_OFFSET_ADJUST: u8 = 17; // 4 bytes
    pub const SERVO_ANGLE_OFFSET_WRITE: u8 = 18; // 3 bytes
    pub const SERVO_ANGLE_OFFSET_READ: u8 = 19; // 3 bytes
    pub const SERVO_ANGLE_LIMIT_WRITE: u8 = 20; // 7 bytes
    pub const SERVO_ANGLE_LIMIT_READ: u8 = 21; // 3 bytes
    pub const SERVO_VIN_LIMIT_WRITE: u8 = 22; // 7 bytes
    pub const SERVO_VIN_LIMIT_READ: u8 = 23; // 3 bytes
    pub const SERVO_TEMP_MAX_LIMIT_WRITE: u8 = 24; // 4 bytes
    pub const SERVO_TEMP_MAX_LIMIT_READ: u8 = 25; // 3 bytes
    pub const SERVO_TEMP_READ: u8 = 0x1A; // 26 -> 3 bytes
    pub const SERVO_VIN_READ: u8 = 0x1B; // 27 -> 3 bytes
    pub const SERVO_POS_READ: u8 = 28; // 3 bytes
    pub const SERVO_OR_MOTOR_MODE_WRITE: u8 = 29; // 7 bytes
    pub const SERVO_OR_MOTOR_MODE_READ: u8 = 30; // 3 bytes
    pub const SERVO_LOAD_OR_UNLOAD_WRITE: u8 = 31; // 4 bytes
    pub const SERVO_LOAD_OR_UNLOAD_READ: u8 = 32; // 3 bytes
    pub const SERVO_LED_CTRL_WRITE: u8 = 33; // 4 bytes
    pub const SERVO_LED_CTRL_READ: u8 = 34; // 3 bytes
    pub const SERVO_LED_ERROR_WRITE: u8 = 35; // 4 bytes
    pub const SERVO_LED_ERROR_READ: u8 = 36; // 3 bytes
}

const MAX_SERVOS: usize = 8; // in theory it could be higher. Revisit if needed.

/// Compute the checksum for the given data.
/// The spec is "Checksum:The calculation method is as follows:
// Checksum=~(ID+ Length+Cmd+ Prm1+...PrmN)If the numbers in the
// brackets are calculated and exceeded 255,Then take the lowest one byte, "~"
// means Negation."
#[inline]
fn compute_checksum(data: impl Iterator<Item = u8>) -> u8 {
    let mut checksum: u8 = 0;
    for byte in data {
        checksum = checksum.wrapping_add(byte);
    }
    !checksum
}

// angle in degrees, returns position in 0.24 degrees
#[inline]
#[allow(dead_code)]
fn angle_to_position(angle: Angle) -> i16 {
    let angle = angle.get::<degree>();
    (angle * 1000.0 / 240.0) as i16
}

/// This is a driver for the LewanSoul LX-16A, LX-225 etc.  Serial Bus Servos.
#[derive(Reflect)]
#[reflect(from_reflect = false)]
pub struct Lewansoul {
    #[reflect(ignore)]
    port: LinuxSerialPort,
    #[allow(dead_code)]
    ids: [u8; 8], // TODO: WIP
}

#[derive(Copy, Clone, Debug, Eq, PartialEq)]
pub enum Binding {
    Serial,
}

pub struct LewansoulResources {
    pub serial: Owned<LinuxSerialPort>,
}

impl<'r> ResourceBindings<'r> for LewansoulResources {
    type Binding = Binding;

    fn from_bindings(
        manager: &'r mut ResourceManager,
        mapping: Option<&ResourceBindingMap<Self::Binding>>,
    ) -> CuResult<Self> {
        let mapping = mapping.ok_or_else(|| {
            CuError::from("Lewansoul requires a `serial` resource mapping in copperconfig")
        })?;
        let path = mapping.get(Binding::Serial).ok_or_else(|| {
            CuError::from("Lewansoul resources must include `serial: <bundle.resource>`")
        })?;
        let serial = manager
            .take::<LinuxSerialPort>(path.typed())
            .map_err(|e| e.add_cause("Failed to fetch Lewansoul serial resource"))?;
        Ok(Self { serial })
    }
}

impl Lewansoul {
    fn send_packet(&mut self, id: u8, command: u8, data: &[u8]) -> io::Result<()> {
        let mut packet = vec![0x55, 0x55, id, data.len() as u8 + 3, command];
        packet.extend_from_slice(data);
        let checksum = compute_checksum(packet[2..].iter().copied());
        packet.push(checksum);

        // println!("Packet: {:02x?}", packet);
        self.port.write_all(&packet)?;
        Ok(())
    }

    /// This should only be use at HW setup. I leave it there for you to make a tool around it if needed.
    /// See the unit test how you can reuse independently those functions
    #[allow(dead_code)]
    fn reassign_servo_id(&mut self, id: u8, new_id: u8) -> io::Result<()> {
        self.send_packet(id, servo::SERVO_ID_WRITE, &[new_id])?;
        self.read_response()?;
        Ok(())
    }

    #[allow(dead_code)]
    fn read_current_position(&mut self, id: u8) -> io::Result<f32> {
        self.send_packet(id, servo::SERVO_POS_READ, &[])?;
        let response = self.read_response()?;
        Ok((i16::from_le_bytes([response.2[0], response.2[1]])) as f32 * 240.0 / 1000.0)
    }

    #[allow(dead_code)]
    fn read_present_voltage(&mut self, id: u8) -> io::Result<f32> {
        self.send_packet(id, servo::SERVO_VIN_READ, &[])?;
        let response = self.read_response()?;
        Ok(u16::from_le_bytes([response.2[0], response.2[1]]) as f32 / 1000.0)
    }

    #[allow(dead_code)]
    fn read_temperature(&mut self, id: u8) -> io::Result<u8> {
        self.send_packet(id, servo::SERVO_TEMP_READ, &[])?;
        let response = self.read_response()?;
        Ok(response.2[0])
    }

    #[allow(dead_code)]
    fn read_angle_limits(&mut self, id: u8) -> io::Result<(f32, f32)> {
        self.send_packet(id, servo::SERVO_ANGLE_LIMIT_READ, &[])?;
        let response = self.read_response()?;
        Ok((
            (i16::from_le_bytes([response.2[0], response.2[1]]) as f32) * 240.0 / 1000.0,
            (i16::from_le_bytes([response.2[2], response.2[3]]) as f32) * 240.0 / 1000.0,
        ))
    }

    #[allow(dead_code)]
    fn ping(&mut self, id: u8) -> CuResult<()> {
        self.send_packet(id, servo::SERVO_ID_READ, &[])
            .map_err(|e| CuError::new_with_cause("IO Error trying to write to the SBUS", e))?;
        let response = self.read_response().map_err(|e| {
            CuError::new_with_cause("IO Error trying to read the ping response from SBUS", e)
        })?;

        if response.2[0] == id {
            Ok(())
        } else {
            Err(format!(
                "The servo ID {} did not respond to ping got {} as ID instead.",
                id, response.2[0]
            )
            .into())
        }
    }

    fn read_response(&mut self) -> io::Result<(u8, u8, Vec<u8>)> {
        let mut header = [0; 5];
        self.port.read_exact(&mut header)?;
        if header[0] != 0x55 || header[1] != 0x55 {
            return Err(io::Error::other("Invalid header"));
        }
        let id = header[2];
        let length = header[3];
        let command = header[4];
        let mut remaining = vec![0; length as usize - 2]; // -2 for length itself already read + command already read
        self.port.read_exact(&mut remaining)?;
        let checksum = compute_checksum(
            header[2..]
                .iter()
                .chain(remaining[..remaining.len() - 1].iter())
                .copied(),
        );
        if checksum != *remaining.last().unwrap() {
            return Err(io::Error::other("Invalid checksum"));
        }
        Ok((id, command, remaining[..remaining.len() - 1].to_vec()))
    }
}

impl Freezable for Lewansoul {
    // This driver is stateless as the IDs are recreate at new time, we keep the default implementation.
}

#[derive(Debug, Clone, Default, Serialize, Deserialize, Encode, Decode, Reflect)]
pub struct ServoPositionsPayload {
    pub positions: [Angle; MAX_SERVOS],
}

impl CuSinkTask for Lewansoul {
    type Resources<'r> = LewansoulResources;
    type Input<'m> = input_msg!(ServoPositionsPayload);

    fn new(config: Option<&ComponentConfig>, resources: Self::Resources<'_>) -> CuResult<Self>
    where
        Self: Sized,
    {
        let ComponentConfig(kv) =
            config.ok_or("RPGpio needs a config, None was passed as ComponentConfig")?;

        let mut ids = [0u8; 8];
        for (i, id) in ids.iter_mut().enumerate() {
            match kv.get(format!("servo{i}").as_str()) {
                Some(servo) => *id = servo.clone().into(),
                None if i == 0 => {
                    return Err(
                        "You need to specify at least one servo ID to address (as \"servo0\")"
                            .into(),
                    );
                }
                None => break,
            }
        }

        let port = resources.serial.0;

        Ok(Lewansoul { port, ids })
    }

    fn process(&mut self, _clock: &RobotClock, _input: &Self::Input<'_>) -> CuResult<()> {
        todo!()
    }
}

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

    #[test]
    #[ignore]
    fn end2end_2_servos() {
        let mut config = ComponentConfig::default();
        config.0.insert("servo0".to_string(), 1.into());
        config.0.insert("servo1".to_string(), 2.into());

        let serial = cu_linux_resources::LinuxSerialPort::open("/dev/ttyACM0", 115_200, 100)
            .expect("open /dev/ttyACM0");
        let resources = LewansoulResources {
            serial: Owned(serial),
        };
        let mut lewansoul = Lewansoul::new(Some(&config), resources).unwrap();
        let _position = lewansoul.read_current_position(1).unwrap();

        let _angle_limits = lewansoul.read_angle_limits(1).unwrap();
    }
}