//! Mutable orthonormal coordinate frame matching the pinned grid math behavior.

use libremetaverse_types::{Error, Matrix4, Quaternion, Vector3};
use std::sync::{RwLock, RwLockReadGuard, RwLockWriteGuard};

#[derive(Clone, Copy, Debug, PartialEq)]
struct FrameState {
    origin: Vector3,
    x_axis: Vector3,
    y_axis: Vector3,
    z_axis: Vector3,
}

/// Origin plus forward, left, and up axes used by agent camera/movement code.
#[derive(Debug)]
pub struct CoordinateFrame {
    state: RwLock<FrameState>,
}

impl Clone for CoordinateFrame {
    fn clone(&self) -> Self {
        Self {
            state: RwLock::new(*read(&self.state)),
        }
    }
}

fn read<T>(value: &RwLock<T>) -> RwLockReadGuard<'_, T> {
    value
        .read()
        .unwrap_or_else(std::sync::PoisonError::into_inner)
}

fn write<T>(value: &RwLock<T>) -> RwLockWriteGuard<'_, T> {
    value
        .write()
        .unwrap_or_else(std::sync::PoisonError::into_inner)
}

fn finite(value: Vector3) -> bool {
    value.x.is_finite() && value.y.is_finite() && value.z.is_finite()
}

fn frame_finite(value: FrameState) -> bool {
    finite(value.origin) && finite(value.x_axis) && finite(value.y_axis) && finite(value.z_axis)
}

fn orthonormalize(value: &mut FrameState) -> Result<(), Error> {
    value.x_axis = Vector3::normalize(value.x_axis)?;
    let projection = Vector3::multiply_with_vector3_single(
        value.x_axis,
        Vector3::dot(value.x_axis, value.y_axis)?,
    )?;
    value.y_axis = Vector3::normalize(Vector3::subtract(value.y_axis, projection)?)?;
    value.z_axis = Vector3::cross(value.x_axis, value.y_axis)?;
    Ok(())
}

impl CoordinateFrame {
    pub fn x_axis_with_field() -> Vector3 {
        Vector3::unit_x()
    }

    pub fn y_axis_with_field() -> Vector3 {
        Vector3::unit_y()
    }

    pub fn z_axis_with_field() -> Vector3 {
        Vector3::unit_z()
    }

    pub fn new_with_vector3(origin: Vector3) -> Result<Self, Error> {
        Self::from_state(FrameState {
            origin,
            x_axis: Self::x_axis_with_field(),
            y_axis: Self::y_axis_with_field(),
            z_axis: Self::z_axis_with_field(),
        })
    }

    pub fn new_with_vector3_matrix4(origin: Vector3, rotation: Matrix4) -> Result<Self, Error> {
        Self::from_state(FrameState {
            origin,
            x_axis: rotation.at_axis(),
            y_axis: rotation.left_axis(),
            z_axis: rotation.up_axis(),
        })
    }

    pub fn new_with_vector3_quaternion(
        origin: Vector3,
        rotation: Quaternion,
    ) -> Result<Self, Error> {
        Self::new_with_vector3_matrix4(origin, Matrix4::create_from_quaternion(rotation)?)
    }

    pub fn new_with_vector3_vector3(origin: Vector3, direction: Vector3) -> Result<Self, Error> {
        let frame = Self::new_with_vector3(origin)?;
        frame.look_direction_with_vector3(direction)?;
        if !frame_finite(*read(&frame.state)) {
            return Err(Error::Argument);
        }
        Ok(frame)
    }

    pub fn new_with_vector3_vector3_vector3_vector3(
        origin: Vector3,
        x_axis: Vector3,
        y_axis: Vector3,
        z_axis: Vector3,
    ) -> Result<Self, Error> {
        Self::from_state(FrameState {
            origin,
            x_axis,
            y_axis,
            z_axis,
        })
    }

    fn from_state(state: FrameState) -> Result<Self, Error> {
        if !frame_finite(state) {
            return Err(Error::Argument);
        }
        Ok(Self {
            state: RwLock::new(state),
        })
    }

    pub fn look_at_with_vector3_vector3(
        &self,
        origin: Vector3,
        target: Vector3,
    ) -> Result<(), Error> {
        self.look_at_with_vector3_vector3_vector3(origin, target, Self::z_axis_with_field())
    }

    pub fn look_at_with_vector3_vector3_vector3(
        &self,
        origin: Vector3,
        target: Vector3,
        up_direction: Vector3,
    ) -> Result<(), Error> {
        if !finite(origin) || !finite(target) || !finite(up_direction) {
            return Err(Error::Argument);
        }
        let at = Vector3::normalize(Vector3::subtract(target, origin)?)?;
        let mut state = write(&self.state);
        state.origin = origin;
        set_look_direction(&mut state, at, up_direction)
    }

    pub fn look_direction_with_vector3(&self, at: Vector3) -> Result<(), Error> {
        self.look_direction_with_vector3_vector3(at, Self::z_axis_with_field())
    }

    pub fn look_direction_with_vector3_vector3(
        &self,
        at: Vector3,
        up_direction: Vector3,
    ) -> Result<(), Error> {
        if !finite(at) || !finite(up_direction) {
            return Err(Error::Argument);
        }
        set_look_direction(&mut write(&self.state), at, up_direction)
    }

    pub fn look_direction_with_double(&self, heading: f64) -> Result<(), Error> {
        if !heading.is_finite() {
            return Err(Error::Argument);
        }
        let mut state = write(&self.state);
        state.y_axis = Vector3 {
            x: heading.cos() as f32,
            y: heading.sin() as f32,
            z: state.y_axis.z,
        };
        state.x_axis = Vector3 {
            x: -heading.sin() as f32,
            y: heading.cos() as f32,
            z: state.x_axis.z,
        };
        Ok(())
    }

    pub fn pitch(&self, angle: f32) -> Result<(), Error> {
        let axis = read(&self.state).y_axis;
        self.rotate_with_quaternion(Quaternion::create_from_axis_angle_with_vector3_single(
            axis, angle,
        )?)
    }

    pub fn reset_axes(&self) -> Result<(), Error> {
        let mut state = write(&self.state);
        state.x_axis = Self::x_axis_with_field();
        state.y_axis = Self::y_axis_with_field();
        state.z_axis = Self::z_axis_with_field();
        Ok(())
    }

    pub fn roll(&self, angle: f32) -> Result<(), Error> {
        let axis = read(&self.state).x_axis;
        self.rotate_with_quaternion(Quaternion::create_from_axis_angle_with_vector3_single(
            axis, angle,
        )?)
    }

    pub fn rotate_with_matrix4(&self, matrix: Matrix4) -> Result<(), Error> {
        let mut state = write(&self.state);
        state.x_axis = Vector3::transform(state.x_axis, matrix)?;
        state.y_axis = Vector3::transform(state.y_axis, matrix)?;
        orthonormalize(&mut state)?;
        if !frame_finite(*state) {
            return Err(Error::InvalidOperation);
        }
        Ok(())
    }

    pub fn rotate_with_quaternion(&self, rotation: Quaternion) -> Result<(), Error> {
        self.rotate_with_matrix4(Matrix4::create_from_quaternion(rotation)?)
    }

    pub fn rotate_with_single_vector3(
        &self,
        angle: f32,
        rotation_axis: Vector3,
    ) -> Result<(), Error> {
        self.rotate_with_quaternion(Quaternion::create_from_axis_angle_with_vector3_single(
            rotation_axis,
            angle,
        )?)
    }

    pub fn yaw(&self, angle: f32) -> Result<(), Error> {
        let axis = read(&self.state).z_axis;
        self.rotate_with_quaternion(Quaternion::create_from_axis_angle_with_vector3_single(
            axis, angle,
        )?)
    }

    pub fn origin(&self) -> Vector3 {
        read(&self.state).origin
    }

    pub fn set_origin(&mut self, value: Vector3) {
        assert!(finite(value), "non-finite CoordinateFrame origin");
        write(&self.state).origin = value;
    }

    pub fn x_axis_with_property(&self) -> Vector3 {
        read(&self.state).x_axis
    }

    pub fn set_x_axis_with_property(&mut self, value: Vector3) {
        assert!(finite(value), "non-finite CoordinateFrame X axis");
        write(&self.state).x_axis = value;
    }

    pub fn y_axis_with_property(&self) -> Vector3 {
        read(&self.state).y_axis
    }

    pub fn set_y_axis_with_property(&mut self, value: Vector3) {
        assert!(finite(value), "non-finite CoordinateFrame Y axis");
        write(&self.state).y_axis = value;
    }

    pub fn z_axis_with_property(&self) -> Vector3 {
        read(&self.state).z_axis
    }

    pub fn set_z_axis_with_property(&mut self, value: Vector3) {
        assert!(finite(value), "non-finite CoordinateFrame Z axis");
        write(&self.state).z_axis = value;
    }
}

fn set_look_direction(
    state: &mut FrameState,
    mut at: Vector3,
    up_direction: Vector3,
) -> Result<(), Error> {
    let mut left = Vector3::cross(up_direction, at)?;
    if left == Vector3::zero() {
        at.x += 0.01;
        at = Vector3::normalize(at)?;
        left = Vector3::cross(up_direction, at)?;
    }
    state.x_axis = at;
    state.y_axis = Vector3::normalize(left)?;
    state.z_axis = Vector3::cross(at, state.y_axis)?;
    Ok(())
}

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

    #[test]
    fn origin_constructor_uses_grid_axes() {
        let frame = CoordinateFrame::new_with_vector3(Vector3::zero()).unwrap();
        assert_eq!(frame.x_axis_with_property(), Vector3::unit_x());
        assert_eq!(frame.y_axis_with_property(), Vector3::unit_y());
        assert_eq!(frame.z_axis_with_property(), Vector3::unit_z());
    }

    #[test]
    fn look_direction_handles_parallel_up_without_zero_left_axis() {
        let frame = CoordinateFrame::new_with_vector3(Vector3::zero()).unwrap();
        frame
            .look_direction_with_vector3_vector3(Vector3::unit_z(), Vector3::unit_z())
            .unwrap();
        assert_ne!(frame.y_axis_with_property(), Vector3::zero());
    }

    #[test]
    fn constructor_rejects_non_finite_input() {
        assert!(matches!(
            CoordinateFrame::new_with_vector3(Vector3 {
                x: f32::NAN,
                y: 0.0,
                z: 0.0,
            }),
            Err(Error::Argument)
        ));
    }

    #[test]
    fn rotations_keep_axes_orthonormal() {
        let frame = CoordinateFrame::new_with_vector3(Vector3::zero()).unwrap();
        frame.yaw(0.75).unwrap();
        let x = frame.x_axis_with_property();
        let y = frame.y_axis_with_property();
        let z = frame.z_axis_with_property();
        assert!((Vector3::dot(x, y).unwrap()).abs() < 1.0e-5);
        assert!((Vector3::dot(x, z).unwrap()).abs() < 1.0e-5);
        assert!((Vector3::dot(y, z).unwrap()).abs() < 1.0e-5);
    }
}
