use std::io::{Cursor, Error};
use bytes::{Buf, BytesMut};
use crate::nineteen::header::decode_header;
use crate::packet::ensure_packet_size;
use crate::packet::motion::{Motion, MotionPacket};
use crate::types::{CornerProperty, Property3D};
pub const PACKET_SIZE: usize = 1343;
pub fn decode_motion(cursor: &mut Cursor<&mut BytesMut>) -> Result<MotionPacket, Error> {
ensure_packet_size(PACKET_SIZE, cursor)?;
let header = decode_header(cursor)?;
let mut cars = Vec::with_capacity(20);
for _ in 0..20 {
cars.push(Motion::new(
decode_position(cursor),
decode_velocity(cursor),
decode_forward_direction(cursor),
decode_right_direction(cursor),
decode_g_force(cursor),
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
))
}
Ok(MotionPacket::new(
header,
cars,
decode_suspension_position(cursor),
decode_suspension_velocity(cursor),
decode_suspension_acceleration(cursor),
decode_wheel_speed(cursor),
decode_wheel_slip(cursor),
decode_local_velocity(cursor),
decode_angular_velocity(cursor),
decode_angular_acceleration(cursor),
cursor.get_f32_le(),
))
}
fn decode_position(cursor: &mut Cursor<&mut BytesMut>) -> Property3D<f32> {
Property3D::new(
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
)
}
fn decode_velocity(cursor: &mut Cursor<&mut BytesMut>) -> Property3D<f32> {
Property3D::new(
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
)
}
fn decode_forward_direction(cursor: &mut Cursor<&mut BytesMut>) -> Property3D<i16> {
Property3D::new(
cursor.get_i16_le(),
cursor.get_i16_le(),
cursor.get_i16_le(),
)
}
fn decode_right_direction(cursor: &mut Cursor<&mut BytesMut>) -> Property3D<i16> {
Property3D::new(
cursor.get_i16_le(),
cursor.get_i16_le(),
cursor.get_i16_le(),
)
}
fn decode_g_force(cursor: &mut Cursor<&mut BytesMut>) -> Property3D<f32> {
Property3D::new(
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
)
}
fn decode_suspension_position(cursor: &mut Cursor<&mut BytesMut>) -> CornerProperty<f32> {
CornerProperty::new(
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
)
}
fn decode_suspension_velocity(cursor: &mut Cursor<&mut BytesMut>) -> CornerProperty<f32> {
CornerProperty::new(
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
)
}
fn decode_suspension_acceleration(cursor: &mut Cursor<&mut BytesMut>) -> CornerProperty<f32> {
CornerProperty::new(
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
)
}
fn decode_wheel_speed(cursor: &mut Cursor<&mut BytesMut>) -> CornerProperty<f32> {
CornerProperty::new(
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
)
}
fn decode_wheel_slip(cursor: &mut Cursor<&mut BytesMut>) -> CornerProperty<f32> {
CornerProperty::new(
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
)
}
fn decode_local_velocity(cursor: &mut Cursor<&mut BytesMut>) -> Property3D<f32> {
Property3D::new(
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
)
}
fn decode_angular_velocity(cursor: &mut Cursor<&mut BytesMut>) -> Property3D<f32> {
Property3D::new(
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
)
}
fn decode_angular_acceleration(cursor: &mut Cursor<&mut BytesMut>) -> Property3D<f32> {
Property3D::new(
cursor.get_f32_le(),
cursor.get_f32_le(),
cursor.get_f32_le(),
)
}
#[cfg(test)]
mod tests {
use std::io::Cursor;
use assert_approx_eq::assert_approx_eq;
use bytes::{BufMut, BytesMut};
use crate::nineteen::motion::{decode_motion, PACKET_SIZE};
fn put_packet_header(mut bytes: BytesMut) -> BytesMut {
bytes.put_u16_le(2019);
bytes.put_u8(1);
bytes.put_u8(2);
bytes.put_u8(3);
bytes.put_u8(0);
bytes.put_u64_le(u64::max_value());
bytes.put_f32_le(1.0);
bytes.put_u32_le(u32::max_value());
bytes.put_u8(0);
bytes
}
#[test]
fn decode_motion_with_error() {
let mut bytes = BytesMut::with_capacity(0);
let mut cursor = Cursor::new(&mut bytes);
let packet = decode_motion(&mut cursor);
assert!(packet.is_err());
}
#[test]
fn decode_motion_with_success() {
let mut bytes = BytesMut::with_capacity(PACKET_SIZE);
bytes = put_packet_header(bytes);
bytes.put_f32_le(1.0);
bytes.put_f32_le(2.0);
bytes.put_f32_le(3.0);
bytes.put_f32_le(4.0);
bytes.put_f32_le(5.0);
bytes.put_f32_le(6.0);
bytes.put_i16_le(7);
bytes.put_i16_le(8);
bytes.put_i16_le(9);
bytes.put_i16_le(10);
bytes.put_i16_le(11);
bytes.put_i16_le(12);
bytes.put_f32_le(13.0);
bytes.put_f32_le(14.0);
bytes.put_f32_le(15.0);
bytes.put_f32_le(16.0);
bytes.put_f32_le(17.0);
bytes.put_f32_le(18.0);
let padding = vec![0u8; 1140];
bytes.put(padding.as_slice());
bytes.put_f32_le(19.0);
bytes.put_f32_le(20.0);
bytes.put_f32_le(21.0);
bytes.put_f32_le(22.0);
bytes.put_f32_le(23.0);
bytes.put_f32_le(24.0);
bytes.put_f32_le(25.0);
bytes.put_f32_le(26.0);
bytes.put_f32_le(27.0);
bytes.put_f32_le(28.0);
bytes.put_f32_le(29.0);
bytes.put_f32_le(30.0);
bytes.put_f32_le(31.0);
bytes.put_f32_le(32.0);
bytes.put_f32_le(33.0);
bytes.put_f32_le(34.0);
bytes.put_f32_le(35.0);
bytes.put_f32_le(36.0);
bytes.put_f32_le(37.0);
bytes.put_f32_le(38.0);
bytes.put_f32_le(39.0);
bytes.put_f32_le(40.0);
bytes.put_f32_le(41.0);
bytes.put_f32_le(42.0);
bytes.put_f32_le(43.0);
bytes.put_f32_le(44.0);
bytes.put_f32_le(45.0);
bytes.put_f32_le(46.0);
bytes.put_f32_le(47.0);
bytes.put_f32_le(48.0);
let mut cursor = Cursor::new(&mut bytes);
let packet = decode_motion(&mut cursor).unwrap();
let motion = packet.cars()[0];
assert_approx_eq!(1.0, motion.position().x());
assert_approx_eq!(4.0, motion.velocity().x());
assert_eq!(7, motion.forward_direction().x());
assert_eq!(10, motion.right_direction().x());
assert_approx_eq!(13.0, motion.g_force().x());
assert_approx_eq!(16.0, motion.yaw());
assert_approx_eq!(17.0, motion.pitch());
assert_approx_eq!(18.0, motion.roll());
assert_approx_eq!(19.0, packet.suspension_position().front_left());
assert_approx_eq!(23.0, packet.suspension_velocity().front_left());
assert_approx_eq!(27.0, packet.suspension_acceleration().front_left());
assert_approx_eq!(31.0, packet.wheel_speed().front_left());
assert_approx_eq!(35.0, packet.wheel_slip().front_left());
assert_approx_eq!(39.0, packet.local_velocity().x());
assert_approx_eq!(42.0, packet.angular_velocity().x());
assert_approx_eq!(45.0, packet.angular_acceleration().x());
assert_approx_eq!(48.0, packet.front_wheels_angle());
}
}