use crate::protocol::{*};
use crate::communication::extractor::Extractor;


// -- WheelAccel8 -----------------------------------------------------------------------------------------------
#[derive(Debug, Copy, Clone)]
pub struct WheelAccel8 {
    pub wheel: i8,
    pub accel: i8,
}


impl WheelAccel8 {
    pub fn new() -> WheelAccel8{
        WheelAccel8 {
            wheel: 0,
            accel: 0,
        }
    }


    pub const fn size() -> usize { 2 }


    pub fn parse(slice_data: &[u8]) -> Result<WheelAccel8, &'static str> {
        if slice_data.len() == WheelAccel8::size() {
            let mut ext: Extractor = Extractor::from_slice(slice_data);
            Ok(WheelAccel8{
                wheel: ext.get_i8(),
                accel: ext.get_i8(),
            })
        }
        else { Err("Wrong length") }
    }
}


impl Serializable for WheelAccel8 {
    fn to_vec(&self) -> Vec<u8> {
        let mut vec_data : Vec<u8> = Vec::new();

        vec_data.extend_from_slice(&self.wheel.to_le_bytes());
        vec_data.extend_from_slice(&self.accel.to_le_bytes());

        vec_data
    }
}


// -- WheelAccel8AndRequestData -----------------------------------------------------------------------------------------------
#[derive(Debug, Copy, Clone)]
pub struct WheelAccel8AndRequestData {
    pub wheel: i8,
    pub accel: i8,
    pub data_type: DataType,
}


impl WheelAccel8AndRequestData {
    pub fn new() -> WheelAccel8AndRequestData{
        WheelAccel8AndRequestData {
            wheel: 0,
            accel: 0,
            data_type: DataType::None,
        }
    }


    pub const fn size() -> usize { 3 }


    pub fn parse(slice_data: &[u8]) -> Result<WheelAccel8AndRequestData, &'static str> {
        if slice_data.len() == WheelAccel8AndRequestData::size() {
            let mut ext: Extractor = Extractor::from_slice(slice_data);
            Ok(WheelAccel8AndRequestData{
                wheel: ext.get_i8(),
                accel: ext.get_i8(),
                data_type: DataType::from_u8(ext.get_u8()),
            })
        }
        else { Err("Wrong length") }
    }
}


impl Serializable for WheelAccel8AndRequestData {
    fn to_vec(&self) -> Vec<u8> {
        let mut vec_data : Vec<u8> = Vec::new();

        vec_data.extend_from_slice(&self.wheel.to_le_bytes());
        vec_data.extend_from_slice(&self.accel.to_le_bytes());
        vec_data.push(self.data_type.into());

        vec_data
    }
}


// -- Quad8 -----------------------------------------------------------------------------------------------
#[derive(Debug, Copy, Clone)]
pub struct Quad8 {
    pub roll: i8,
    pub pitch: i8,
    pub yaw: i8,
    pub throttle: i8,
}


impl Quad8 {
    pub fn new() -> Quad8{
        Quad8 {
            roll: 0,
            pitch: 0,
            yaw: 0,
            throttle: 0,
        }
    }


    pub const fn size() -> usize { 4 }


    pub fn parse(slice_data: &[u8]) -> Result<Quad8, &'static str> {
        if slice_data.len() == Quad8::size() {
            let mut ext: Extractor = Extractor::from_slice(slice_data);
            Ok(Quad8{
                roll: ext.get_i8(),
                pitch: ext.get_i8(),
                yaw: ext.get_i8(),
                throttle: ext.get_i8(),
            })
        }
        else { Err("Wrong length") }
    }
}


impl Serializable for Quad8 {
    fn to_vec(&self) -> Vec<u8> {
        let mut vec_data : Vec<u8> = Vec::new();

        vec_data.extend_from_slice(&self.roll.to_le_bytes());
        vec_data.extend_from_slice(&self.pitch.to_le_bytes());
        vec_data.extend_from_slice(&self.yaw.to_le_bytes());
        vec_data.extend_from_slice(&self.throttle.to_le_bytes());

        vec_data
    }
}


// -- Quad8AndRequestData -----------------------------------------------------------------------------------------------
#[derive(Debug, Copy, Clone)]
pub struct Quad8AndRequestData {
    pub roll: i8,
    pub pitch: i8,
    pub yaw: i8,
    pub throttle: i8,
    pub data_type: DataType,
}


impl Quad8AndRequestData {
    pub fn new() -> Quad8AndRequestData{
        Quad8AndRequestData {
            roll: 0,
            pitch: 0,
            yaw: 0,
            throttle: 0,
            data_type: DataType::None,
        }
    }


    pub const fn size() -> usize { 5 }


    pub fn parse(slice_data: &[u8]) -> Result<Quad8AndRequestData, &'static str> {
        if slice_data.len() == Quad8AndRequestData::size() {
            let mut ext: Extractor = Extractor::from_slice(slice_data);
            Ok(Quad8AndRequestData{
                roll: ext.get_i8(),
                pitch: ext.get_i8(),
                yaw: ext.get_i8(),
                throttle: ext.get_i8(),
                data_type: DataType::from_u8(ext.get_u8()),
            })
        }
        else { Err("Wrong length") }
    }
}


impl Serializable for Quad8AndRequestData {
    fn to_vec(&self) -> Vec<u8> {
        let mut vec_data : Vec<u8> = Vec::new();

        vec_data.extend_from_slice(&self.roll.to_le_bytes());
        vec_data.extend_from_slice(&self.pitch.to_le_bytes());
        vec_data.extend_from_slice(&self.yaw.to_le_bytes());
        vec_data.extend_from_slice(&self.throttle.to_le_bytes());
        vec_data.push(self.data_type.into());

        vec_data
    }
}


// -- Position16 -----------------------------------------------------------------------------------------------
#[derive(Debug, Copy, Clone)]
pub struct Position16 {
    pub position_x: i16,
    pub position_y: i16,
    pub position_z: i16,
    pub velocity: i16,
    pub heading: i16,
    pub rotational_velocity: i16,
}


impl Position16 {
    pub fn new() -> Position16{
        Position16 {
            position_x: 0,
            position_y: 0,
            position_z: 0,
            velocity: 0,
            heading: 0,
            rotational_velocity: 0,
        }
    }


    pub const fn size() -> usize { 12 }


    pub fn parse(slice_data: &[u8]) -> Result<Position16, &'static str> {
        if slice_data.len() == Position16::size() {
            let mut ext: Extractor = Extractor::from_slice(slice_data);
            Ok(Position16{
                position_x: ext.get_i16(),
                position_y: ext.get_i16(),
                position_z: ext.get_i16(),
                velocity: ext.get_i16(),
                heading: ext.get_i16(),
                rotational_velocity: ext.get_i16(),
            })
        }
        else { Err("Wrong length") }
    }
}


impl Serializable for Position16 {
    fn to_vec(&self) -> Vec<u8> {
        let mut vec_data : Vec<u8> = Vec::new();

        vec_data.extend_from_slice(&self.position_x.to_le_bytes());
        vec_data.extend_from_slice(&self.position_y.to_le_bytes());
        vec_data.extend_from_slice(&self.position_z.to_le_bytes());
        vec_data.extend_from_slice(&self.velocity.to_le_bytes());
        vec_data.extend_from_slice(&self.heading.to_le_bytes());
        vec_data.extend_from_slice(&self.rotational_velocity.to_le_bytes());

        vec_data
    }
}


// -- Position -----------------------------------------------------------------------------------------------
#[derive(Debug, Copy, Clone)]
pub struct Position {
    pub x: f32,
    pub y: f32,
    pub z: f32,
    pub velocity: f32,
    pub heading: i16,
    pub rotational_velocity: i16,
}


impl Position {
    pub fn new() -> Position{
        Position {
            x: 0.0_f32,
            y: 0.0_f32,
            z: 0.0_f32,
            velocity: 0.0_f32,
            heading: 0,
            rotational_velocity: 0,
        }
    }


    pub const fn size() -> usize { 20 }


    pub fn parse(slice_data: &[u8]) -> Result<Position, &'static str> {
        if slice_data.len() == Position::size() {
            let mut ext: Extractor = Extractor::from_slice(slice_data);
            Ok(Position{
                x: ext.get_f32(),
                y: ext.get_f32(),
                z: ext.get_f32(),
                velocity: ext.get_f32(),
                heading: ext.get_i16(),
                rotational_velocity: ext.get_i16(),
            })
        }
        else { Err("Wrong length") }
    }
}


impl Serializable for Position {
    fn to_vec(&self) -> Vec<u8> {
        let mut vec_data : Vec<u8> = Vec::new();

        vec_data.extend_from_slice(&self.x.to_le_bytes());
        vec_data.extend_from_slice(&self.y.to_le_bytes());
        vec_data.extend_from_slice(&self.z.to_le_bytes());
        vec_data.extend_from_slice(&self.velocity.to_le_bytes());
        vec_data.extend_from_slice(&self.heading.to_le_bytes());
        vec_data.extend_from_slice(&self.rotational_velocity.to_le_bytes());

        vec_data
    }
}



