//! Type for representing a RealSense Motion frame.
//!
//! Motion data for any Motion frame is held as a 3-vector. This data (retrieved
//! through `motion()`) represents different things depending on the device recorded.
//!
//! See the docs for [MotionFrame::motion] for more.

use super::prelude::{CouldNotGetFrameSensorError, FrameCategory, FrameConstructionError, FrameEx};
use crate::{
    check_rs2_error,
    kind::{Rs2Extension, Rs2FrameMetadata, Rs2StreamKind, Rs2TimestampDomain},
    sensor::Sensor,
    stream_profile::StreamProfile,
};
use anyhow::Result;
use num_traits::FromPrimitive;
use realsense_sys as sys;
use std::{
    convert::{TryFrom, TryInto},
    marker::PhantomData,
    ptr::{self, NonNull},
};

/// A unit struct defining an Accel frame.
#[derive(Debug)]
pub struct Accel;
/// A unit struct defining a Gyro frame.
#[derive(Debug)]
pub struct Gyro;

/// Holds the raw data pointer and derived data from an RS2 Motion Frame.
///
/// All fields in this struct are initialized during struct creation (via `try_from`).
/// Everything called from here during runtime should be valid as long as the
/// Frame is in scope... like normal Rust.
#[derive(Debug)]
pub struct MotionFrame<Kind> {
    /// The raw data pointer from the original rs2 frame.
    frame_ptr: NonNull<sys::rs2_frame>,
    /// The timestamp of the frame.
    timestamp: f64,
    /// The RealSense time domain from which the timestamp is derived.
    timestamp_domain: Rs2TimestampDomain,
    /// The Stream Profile that created the frame.
    frame_stream_profile: StreamProfile,
    /// The motion data held in this Motion Frame. Motion data is represented as a
    /// 3-vector, with different conventions depending on the device recorded.
    /// See the [motion](MotionFrame::motion) function for more documentation.
    motion: [f32; 3],
    /// A boolean used during `Drop` calls. This allows for proper handling of the pointer
    /// during ownership transfer.
    should_drop: bool,
    /// Holds the type metadata of this frame.
    _phantom: PhantomData<Kind>,
}

/// A motion frame type holding the raw pointer and derived metadata for an RS2 Accel frame.
pub type AccelFrame = MotionFrame<Accel>;
/// A motion frame type holding the raw pointer and derived metadata for an RS2 Gyro frame.
pub type GyroFrame = MotionFrame<Gyro>;

impl FrameCategory for AccelFrame {
    fn extension() -> Rs2Extension {
        Rs2Extension::MotionFrame
    }

    fn kind() -> Rs2StreamKind {
        Rs2StreamKind::Accel
    }

    fn has_correct_kind(&self) -> bool {
        self.frame_stream_profile.kind() == Self::kind()
    }
}

impl FrameCategory for GyroFrame {
    fn extension() -> Rs2Extension {
        Rs2Extension::MotionFrame
    }

    fn kind() -> Rs2StreamKind {
        Rs2StreamKind::Gyro
    }

    fn has_correct_kind(&self) -> bool {
        self.frame_stream_profile.kind() == Self::kind()
    }
}

impl<K> Drop for MotionFrame<K> {
    /// Drop the raw pointer stored with this struct whenever it goes out of scope.
    fn drop(&mut self) {
        unsafe {
            if self.should_drop {
                sys::rs2_release_frame(self.frame_ptr.as_ptr());
            }
        }
    }
}

unsafe impl<K> Send for MotionFrame<K> {}

impl<K> TryFrom<NonNull<sys::rs2_frame>> for MotionFrame<K> {
    type Error = anyhow::Error;

    /// Attempt to create an Image frame of extension K from the raw `rs2_frame`. All
    /// members of the ImageFrame struct are validated and populated during this call.
    ///
    /// # Errors
    ///
    /// There are a number of errors that may occur if the data in the `rs2_frame` is not
    /// valid, all of type [FrameConstructionError].
    ///
    /// - [CouldNotGetTimestamp](FrameConstructionError::CouldNotGetTimestamp)
    /// - [CouldNotGetTimestampDomain](FrameConstructionError::CouldNotGetTimestampDomain)
    /// - [CouldNotGetFrameStreamProfile](FrameConstructionError::CouldNotGetFrameStreamProfile)
    /// - [CouldNotGetDataSize](FrameConstructionError::CouldNotGetDataSize)
    /// - [CouldNotGetData](FrameConstructionError::CouldNotGetData)
    ///
    /// See [FrameConstructionError] documentation for more details.
    ///
    fn try_from(frame_ptr: NonNull<sys::rs2_frame>) -> Result<Self, Self::Error> {
        unsafe {
            let mut err = ptr::null_mut::<sys::rs2_error>();

            let timestamp = sys::rs2_get_frame_timestamp(frame_ptr.as_ptr(), &mut err);
            check_rs2_error!(err, FrameConstructionError::CouldNotGetTimestamp)?;

            let timestamp_domain =
                sys::rs2_get_frame_timestamp_domain(frame_ptr.as_ptr(), &mut err);
            check_rs2_error!(err, FrameConstructionError::CouldNotGetTimestampDomain)?;

            let profile_ptr = sys::rs2_get_frame_stream_profile(frame_ptr.as_ptr(), &mut err);
            check_rs2_error!(err, FrameConstructionError::CouldNotGetFrameStreamProfile)?;

            let nonnull_profile_ptr =
                NonNull::new(profile_ptr as *mut sys::rs2_stream_profile).unwrap();
            let profile = StreamProfile::try_from(nonnull_profile_ptr)?;

            let size = sys::rs2_get_frame_data_size(frame_ptr.as_ptr(), &mut err);
            check_rs2_error!(err, FrameConstructionError::CouldNotGetDataSize)?;

            let ptr = sys::rs2_get_frame_data(frame_ptr.as_ptr(), &mut err);
            check_rs2_error!(err, FrameConstructionError::CouldNotGetData)?;

            let data_as_ptr = ptr.as_ref().unwrap() as *const std::os::raw::c_void;
            let data_size_in_f32s = (size as usize) / std::mem::size_of::<f32>();

            let motion_raw =
                std::slice::from_raw_parts(data_as_ptr.cast::<f32>(), data_size_in_f32s);

            Ok(MotionFrame {
                frame_ptr,
                timestamp,
                timestamp_domain: Rs2TimestampDomain::from_i32(timestamp_domain as i32).unwrap(),
                frame_stream_profile: profile,
                motion: [motion_raw[0], motion_raw[1], motion_raw[2]],
                should_drop: true,
                _phantom: PhantomData::<K> {},
            })
        }
    }
}

impl<K> FrameEx for MotionFrame<K> {
    fn stream_profile(&self) -> &StreamProfile {
        &self.frame_stream_profile
    }

    fn sensor(&self) -> Result<Sensor> {
        unsafe {
            let mut err = std::ptr::null_mut::<sys::rs2_error>();
            let sensor_ptr = sys::rs2_get_frame_sensor(self.frame_ptr.as_ptr(), &mut err);
            check_rs2_error!(err, CouldNotGetFrameSensorError)?;

            Ok(Sensor::try_from(NonNull::new(sensor_ptr).unwrap())?)
        }
    }
    fn timestamp(&self) -> f64 {
        self.timestamp
    }

    fn timestamp_domain(&self) -> Rs2TimestampDomain {
        self.timestamp_domain
    }

    fn metadata(&self, metadata_kind: Rs2FrameMetadata) -> Option<std::os::raw::c_longlong> {
        if !self.supports_metadata(metadata_kind) {
            return None;
        }

        unsafe {
            let mut err = std::ptr::null_mut::<sys::rs2_error>();

            let val = sys::rs2_get_frame_metadata(
                self.frame_ptr.as_ptr(),
                #[allow(clippy::useless_conversion)]
                (metadata_kind as i32).try_into().unwrap(),
                &mut err,
            );
            if err.as_ref().is_none() {
                Some(val)
            } else {
                sys::rs2_free_error(err);
                None
            }
        }
    }

    fn supports_metadata(&self, metadata_kind: Rs2FrameMetadata) -> bool {
        unsafe {
            let mut err = std::ptr::null_mut::<sys::rs2_error>();

            let supports_metadata = sys::rs2_supports_frame_metadata(
                self.frame_ptr.as_ptr(),
                #[allow(clippy::useless_conversion)]
                (metadata_kind as i32).try_into().unwrap(),
                &mut err,
            );

            if err.as_ref().is_none() {
                supports_metadata != 0
            } else {
                sys::rs2_free_error(err);
                false
            }
        }
    }

    unsafe fn get_owned_raw(mut self) -> NonNull<sys::rs2_frame> {
        self.should_drop = false;

        self.frame_ptr
    }
}

impl AccelFrame {
    /// Returns a 3-item array representing the sensor motion recorded in the Accel frame.
    ///
    /// Accelerations are reported as [x, y, z] values, and are in units of m/s^2
    ///
    /// This function will return different data conventions entirely depending on the device
    /// used to create the measurement.
    ///
    /// # Intel RealSense D435i
    ///
    /// - `motion[0]`: Positive x-axis points to the right.
    /// - `motion[1]`: Positive y-axis points down.
    /// - `motion[2]`: Positive z-axis points forward.
    ///
    /// # Intel RealSense T265
    ///
    /// - `motion[0]`: Positive X direction is towards right imager.
    /// - `motion[1]`: Positive Y direction is upwards toward the top of the device.
    /// - `motion[2]`: Positive Z direction is inwards toward the back of the device.
    ///
    /// Read more about the coordinate frames of RealSense motion in
    /// [the RealSense docs](https://www.intelrealsense.com/how-to-getting-imu-data-from-d435i-and-t265/)
    ///
    pub fn acceleration(&self) -> &[f32; 3] {
        &self.motion
    }
}

impl GyroFrame {
    /// Returns a 3-item array representing the sensor motion recorded in the Gyro frame.
    ///
    /// Gyroscope measurements are reported as [x, y, z] values, and are in units of radians/s
    ///
    /// This function will return different data conventions entirely depending on the device
    /// used to create the measurement.
    ///
    /// # Intel RealSense D435i
    ///
    /// - `motion[0]`: Positive x-axis points to the right.
    /// - `motion[1]`: Positive y-axis points down.
    /// - `motion[2]`: Positive z-axis points forward.
    ///
    /// # Intel RealSense T265
    ///
    /// - `motion[0]`: Positive X direction is towards right imager.
    /// - `motion[1]`: Positive Y direction is upwards toward the top of the device.
    /// - `motion[2]`: Positive Z direction is inwards toward the back of the device.
    ///
    /// Read more about the coordinate frames of RealSense motion in
    /// [the RealSense docs](https://www.intelrealsense.com/how-to-getting-imu-data-from-d435i-and-t265/)
    ///
    pub fn rotational_velocity(&self) -> &[f32; 3] {
        &self.motion
    }
}

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

    #[test]
    fn frame_has_correct_kind() {
        assert_eq!(AccelFrame::kind(), Rs2StreamKind::Accel);
        assert_eq!(GyroFrame::kind(), Rs2StreamKind::Gyro);
    }
}