px4sim 0.1.6

use crate::util::{cnoise, noise, CanAdd};
use itertools::Itertools;

#[derive(Default)]
pub struct DroneData {
    // GPS ---------------------------------------------------------------------
    /**
     * GPS (latitude, longtitude) in [°]*10⁷
     */
    pub coordinates: (i32, i32),
    /**
     * GPS altitude in millimeters [mm]
     */
    pub altitude: i32,
    /**
     * GPS (horizontal, vertical) dilution of precision [-] unknown => u16::MAX
     */
    pub position_error: (u16, u16),
    /**
     * GPS ground velocity [cm/s] unknown => u16::MAX
     */
    pub ground_velocity: u16,
    /**
     * GPS (north, east, down) velocity [cm/s]
     */
    pub gps_velocity: (i16, i16, i16),
    /**
     * Course over ground [c° (centi-degrees)] (NOT heading, but direction of movement), 0.0..359.99 degrees. unknown => u16::MAX
     */
    pub course_over_ground: u16,
    // STATE -------------------------------------------------------------------
    /**
     * Vehicle attitude expressed as normalized quaternion in w, x, y, z order
     */
    pub attitude_quaternion: [f32; 4],
    /**
     * Body frame (roll, pitch, yaw) phi angular velocity [rad/s]
     */
    pub angular_velocity: (f32, f32, f32),
    /**
     * Linear velocity (latitude, longitude, altitude) [cm/s]
     */
    pub linear_velocity: (i16, i16, i16),
    /**
     * Indicated airspeed [cm/s]
     */
    pub indicated_airspeed: u16,
    /**
     * True airspeed [cm/s]
     */
    pub true_airspeed: u16,
    /**
     * Linear acceleration (x, y, z) [mG]
     */
    pub linear_acceleration: (i16, i16, i16),
    // IMU ---------------------------------------------------------------------
    /**
     * Measured acceleration (x, y, z) [m/s²]
     */
    pub measured_acceleration: (f32, f32, f32),
    /**
     * Measured angular velocity (x, y, z) [rad/s]
     */
    pub measured_angular_velocity: (f32, f32, f32),
    /**
     * Measured magnetic field (x, y, z) [gauss]
     */
    pub measured_magnetic_field: (f32, f32, f32),
    /**
     * Absolute pressure [hPa]
     */
    pub absolute_pressure: f32,
    /**
     * Differential pressure (airspeed) [hPa]
     */
    pub differential_pressure: f32,
    /**
     * Altitude calculated from pressure [?]
     */
    pub altitude_from_pressure: f32,
    /**
     * Temperature [°C]
     */
    pub measured_temperature: f32,
}

impl DroneData {
    pub fn new() -> Self {
        Self {
            // gps
            coordinates: [47.397742, 8.545594].map(|x| (x * 1e7) as i32).into(),
            altitude: 488 * 1000,
            position_error: (30, 40),
            // state
            attitude_quaternion: [1., 0., 0., 0.],
            // IMU
            measured_acceleration: (0., 0., -9.81),
            measured_magnetic_field: (0.215, 0.01, 0.43),
            absolute_pressure: 955.98,
            altitude_from_pressure: 488.,
            ..Self::default()
        }
    }

    pub fn noise() -> Self {
        Self {
            // gps
            coordinates: (0..2).map(|_| cnoise(4.) as i32).collect_tuple().unwrap(),
            altitude: cnoise(4.) as i32,
            position_error: (0..2).map(|_| noise(2.) as u16).collect_tuple().unwrap(),
            ground_velocity: noise(2.) as u16,
            gps_velocity: (0..3).map(|_| cnoise(4.) as i16).collect_tuple().unwrap(),
            course_over_ground: noise(2.) as u16,
            // IMU
            measured_acceleration: (0..3).map(|_| cnoise(0.00001)).collect_tuple().unwrap(),
            measured_angular_velocity: (0..3).map(|_| cnoise(0.00001)).collect_tuple().unwrap(),
            measured_magnetic_field: (0..3).map(|_| cnoise(0.00001)).collect_tuple().unwrap(),
            absolute_pressure: cnoise(0.00001),
            differential_pressure: 0.00001,
            altitude_from_pressure: cnoise(0.00001),
            measured_temperature: cnoise(0.00001),
            ..Self::default()
        }
    }
}

impl std::ops::Add<Self> for DroneData {
    type Output = Self;
    fn add(self, other: Self) -> Self {
        Self {
            coordinates: self.coordinates.add(other.coordinates),
            altitude: self.altitude + other.altitude,
            position_error: self.position_error.add(other.position_error),
            ground_velocity: self.ground_velocity + other.ground_velocity,
            gps_velocity: self.gps_velocity.add(other.gps_velocity),
            course_over_ground: self.course_over_ground + other.course_over_ground,
            attitude_quaternion: self.attitude_quaternion.add(other.attitude_quaternion),
            angular_velocity: self.angular_velocity.add(other.angular_velocity),
            linear_velocity: self.linear_velocity.add(other.linear_velocity),
            indicated_airspeed: self.indicated_airspeed + other.indicated_airspeed,
            true_airspeed: self.true_airspeed + other.true_airspeed,
            linear_acceleration: self.linear_acceleration.add(other.linear_acceleration),
            measured_acceleration: self.measured_acceleration.add(other.measured_acceleration),
            measured_angular_velocity: self
                .measured_angular_velocity
                .add(other.measured_angular_velocity),
            measured_magnetic_field: self
                .measured_magnetic_field
                .add(other.measured_magnetic_field),
            absolute_pressure: self.absolute_pressure + other.absolute_pressure,
            differential_pressure: self.differential_pressure + other.differential_pressure,
            altitude_from_pressure: self.altitude_from_pressure + other.altitude_from_pressure,
            measured_temperature: self.measured_temperature + other.measured_temperature,
        }
    }
}