// -*- coding: utf-8 -*-
//
// Copyright 2021 Michael Büsch <m@bues.ch>
//
// Licensed under the Apache License version 2.0
// or the MIT license, at your option.
// SPDX-License-Identifier: Apache-2.0 OR MIT
//

pub mod rotmove;

use crate::base::{degrees, radians, PhyType};
use crate::vector::Vector;
use std::cmp::{max, min};
use std::ops::{Add, AddAssign, Deref, DerefMut, Mul, MulAssign, Neg, Sub, SubAssign};

#[derive(Copy, Clone, PartialEq, Debug)]
pub enum RotAxis {
    Alpha = 0,
    Beta = 1,
    Gamma = 2,
}

pub const NR_ROTS: usize = 3;

impl std::fmt::Display for RotAxis {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(
            f,
            "{}",
            match self {
                RotAxis::Alpha => "alpha",
                RotAxis::Beta => "beta",
                RotAxis::Gamma => "gamma",
            }
        )
    }
}

#[derive(Copy, Clone, PartialEq, Debug)]
pub enum RotMode {
    Absolute = 0,
    Relative = 1,
}

impl std::fmt::Display for RotMode {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(
            f,
            "{}",
            match self {
                RotMode::Absolute => "abs",
                RotMode::Relative => "rel",
            }
        )
    }
}

#[derive(Clone, Debug, PartialEq)]
pub struct Rotation {
    mode: RotMode,
    rot: [PhyType; NR_ROTS],
}

impl Rotation {
    pub fn new(alpha: PhyType, beta: PhyType, gamma: PhyType, mode: RotMode) -> Rotation {
        let rot = match mode {
            RotMode::Absolute => [
                Rotation::modulo(alpha),
                Rotation::modulo(beta),
                Rotation::modulo(gamma),
            ],
            RotMode::Relative => [alpha, beta, gamma],
        };
        Rotation { mode, rot }
    }

    pub fn new_abs(alpha: PhyType, beta: PhyType, gamma: PhyType) -> Rotation {
        Rotation::new(alpha, beta, gamma, RotMode::Absolute)
    }

    pub fn new_rel(alpha: PhyType, beta: PhyType, gamma: PhyType) -> Rotation {
        Rotation::new(alpha, beta, gamma, RotMode::Relative)
    }

    pub fn new_zero(mode: RotMode) -> Rotation {
        Rotation::new(0.into(), 0.into(), 0.into(), mode)
    }

    pub fn new_zero_abs() -> Rotation {
        Rotation::new_zero(RotMode::Absolute)
    }

    pub fn new_zero_rel() -> Rotation {
        Rotation::new_zero(RotMode::Relative)
    }

    pub fn modulo(mut rot: PhyType) -> PhyType {
        let maxval = radians(360.into());
        while rot < 0.into() {
            let prev = rot;
            rot += maxval;
            if rot == prev {
                break;
            }
        }
        rot %= maxval;
        min(max(rot, 0.into()), maxval)
    }

    pub fn all_rot_axes() -> [RotAxis; NR_ROTS] {
        [RotAxis::Alpha, RotAxis::Beta, RotAxis::Gamma]
    }

    pub fn mode(&self) -> RotMode {
        self.mode
    }

    pub fn get(&self, axis: RotAxis) -> PhyType {
        self.rot[axis as usize]
    }

    pub fn alpha(&self) -> PhyType {
        self.get(RotAxis::Alpha)
    }

    pub fn beta(&self) -> PhyType {
        self.get(RotAxis::Beta)
    }

    pub fn gamma(&self) -> PhyType {
        self.get(RotAxis::Gamma)
    }

    pub fn set(&mut self, axis: RotAxis, value: PhyType) {
        self.rot[axis as usize] = value;
    }

    pub fn set_alpha(&mut self, value: PhyType) {
        self.set(RotAxis::Alpha, value);
    }

    pub fn set_beta(&mut self, value: PhyType) {
        self.set(RotAxis::Beta, value);
    }

    pub fn set_gamma(&mut self, value: PhyType) {
        self.set(RotAxis::Gamma, value);
    }

    pub fn absolute(&self) -> Rotation {
        match self.mode {
            RotMode::Absolute => self.clone(),
            RotMode::Relative => Rotation::new(
                Rotation::modulo(self.alpha()),
                Rotation::modulo(self.beta()),
                Rotation::modulo(self.gamma()),
                RotMode::Absolute,
            ),
        }
    }

    pub fn set_zero(&mut self) {
        for i in 0..NR_ROTS {
            self.rot[i] = 0.into();
        }
    }

    pub fn assign(&mut self, other: &Rotation) {
        self.mode = other.mode;
        for i in 0..NR_ROTS {
            self.rot[i] = other.rot[i];
        }
    }

    pub fn copy_sign(&self, sign: &Rotation) -> Rotation {
        debug_assert_eq!(self.mode(), RotMode::Relative);
        Rotation::new(
            self.alpha().copysign(*sign.alpha()).into(),
            self.beta().copysign(*sign.beta()).into(),
            self.gamma().copysign(*sign.gamma()).into(),
            RotMode::Relative,
        )
    }

    /// Add and bi-directionally saturate towards a value.
    pub fn add_sat_bi(&self, rot: &Rotation, sat: &Rotation) -> Rotation {
        let mut ret = self.clone();
        for axis in &Rotation::all_rot_axes() {
            if ret.get(*axis) < sat.get(*axis) {
                ret.set(*axis, min(ret.get(*axis) + rot.get(*axis), sat.get(*axis)));
            } else {
                ret.set(*axis, max(ret.get(*axis) + rot.get(*axis), sat.get(*axis)));
            }
        }
        ret
    }

    pub fn add_sat_bi_assign(&mut self, rot: &Rotation, sat: &Rotation) {
        self.assign(&self.add_sat_bi(rot, sat));
    }

    /// Rotate a vector based on the current rotation state.
    /// The rotation mapping is as follows:
    ///     alpha => Roll rotation in direction of the vector (Y axis).
    ///              Therefore alpha does not affect the vector.
    ///     beta => Pitch angle around the X axis.
    ///     gamma => Yaw angle around the Z axis.
    /// Directions are right-handed.
    pub fn rotate_vector(&self, vect: &Vector) -> Vector {
        let v_length = vect.length();
        if v_length < PhyType::EPSILON {
            *vect
        } else {
            // Calculate the current rotation of the vector.
            let tmp: PhyType = (vect.z() / v_length).asin().into();
            let v_beta: PhyType = tmp * (-1).into();
            let v_cos_beta: PhyType = v_beta.cos().into();
            let v_len_on_xy_plane = v_cos_beta * v_length;
            let v_gamma: PhyType = if v_len_on_xy_plane.abs() >= *PhyType::EPSILON {
                let tmp: PhyType = (vect.x() / v_len_on_xy_plane).acos().into();
                if vect.y() < 0.into() {
                    -tmp
                } else {
                    tmp
                }
            } else {
                0.into()
            };

            // Rotate the angles
            let v_beta = v_beta + self.beta();
            let v_gamma = v_gamma + self.gamma();
            assert!(v_beta.abs() <= *radians(90.into()));

            // Construct a new vector with the rotated angles.
            let v_sincos_beta = v_beta.sin_cos();
            let v_sin_beta: PhyType = v_sincos_beta.0.into();
            let v_cos_beta: PhyType = v_sincos_beta.1.into();
            let v_sincos_gamma = v_gamma.sin_cos();
            let v_sin_gamma: PhyType = v_sincos_gamma.0.into();
            let v_cos_gamma: PhyType = v_sincos_gamma.1.into();
            let v_len_on_xy_plane = v_cos_beta * v_length;
            Vector::new(
                v_cos_gamma * v_len_on_xy_plane,
                v_sin_gamma * v_len_on_xy_plane,
                v_sin_beta * v_length * (-1).into(),
            )
        }
    }
}

impl std::fmt::Display for Rotation {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(
            f,
            "Rot({}, a={}*, b={}*, g={}*)",
            self.mode(),
            degrees(self.alpha()),
            degrees(self.beta()),
            degrees(self.gamma())
        )
    }
}

impl Add for Rotation {
    type Output = Self;

    fn add(self, other: Self) -> Self::Output {
        Rotation::new(
            self.alpha() + other.alpha(),
            self.beta() + other.beta(),
            self.gamma() + other.gamma(),
            self.mode,
        )
    }
}

impl AddAssign for Rotation {
    fn add_assign(&mut self, other: Self) {
        let new = self.clone() + other;
        self.set_alpha(new.alpha());
        self.set_beta(new.beta());
        self.set_gamma(new.gamma());
    }
}

impl Sub for Rotation {
    type Output = Self;

    fn sub(self, other: Self) -> Self::Output {
        Rotation::new(
            self.alpha() - other.alpha(),
            self.beta() - other.beta(),
            self.gamma() - other.gamma(),
            self.mode,
        )
    }
}

impl SubAssign for Rotation {
    fn sub_assign(&mut self, other: Self) {
        let new = self.clone() - other;
        self.set_alpha(new.alpha());
        self.set_beta(new.beta());
        self.set_gamma(new.gamma());
    }
}

impl Neg for Rotation {
    type Output = Self;

    fn neg(self) -> Self::Output {
        Rotation::new(
            self.alpha() * (-1).into(),
            self.beta() * (-1).into(),
            self.gamma() * (-1).into(),
            RotMode::Relative,
        )
    }
}

impl Mul<PhyType> for Rotation {
    type Output = Self;

    fn mul(self, rhs: PhyType) -> Self::Output {
        Rotation::new(
            self.alpha() * rhs,
            self.beta() * rhs,
            self.gamma() * rhs,
            self.mode,
        )
    }
}

impl MulAssign<PhyType> for Rotation {
    fn mul_assign(&mut self, rhs: PhyType) {
        let new = self.clone() * rhs;
        self.set_alpha(new.alpha());
        self.set_beta(new.beta());
        self.set_gamma(new.gamma());
    }
}

#[derive(Clone, Copy, PartialEq, Debug)]
pub struct RotLimits {
    pub neg: PhyType,
    pub pos: PhyType,
}

impl RotLimits {
    pub fn new(neg: PhyType, pos: PhyType) -> RotLimits {
        RotLimits { neg, pos }
    }

    pub fn zero() -> RotLimits {
        RotLimits {
            neg: 0.into(),
            pos: 0.into(),
        }
    }

    pub fn unlimited() -> RotLimits {
        RotLimits {
            neg: -PhyType::UNLIMITED,
            pos: PhyType::UNLIMITED,
        }
    }
}

impl std::fmt::Display for RotLimits {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(f, "{:.1}*/{:.1}*", degrees(self.neg), degrees(self.pos))
    }
}

#[derive(Clone, Debug)]
pub struct RotationLim {
    rot: Rotation,
    lim: [RotLimits; NR_ROTS],
}

impl RotationLim {
    pub fn new(rot: Rotation) -> RotationLim {
        RotationLim {
            rot,
            lim: [
                RotLimits::unlimited(),
                RotLimits::unlimited(),
                RotLimits::unlimited(),
            ],
        }
    }

    pub fn get(&self) -> &Rotation {
        &self.rot
    }

    pub fn get_mut(&mut self) -> RotationLimMutWrapper {
        RotationLimMutWrapper { rot_lim: self }
    }

    pub fn set_limits_axis(&mut self, axis: RotAxis, mut limits: RotLimits) {
        debug_assert!(limits.neg <= limits.pos);
        if limits.neg == PhyType::UNLIMITED {
            limits.neg = -PhyType::UNLIMITED;
        }
        if limits.pos == -PhyType::UNLIMITED {
            limits.pos = PhyType::UNLIMITED;
        }
        self.lim[axis as usize] = limits;
        self.apply_limits();
    }

    pub fn set_limits(&mut self, neg: &Rotation, pos: &Rotation) {
        for axis in &Rotation::all_rot_axes() {
            self.set_limits_axis(
                *axis,
                RotLimits {
                    neg: neg.get(*axis),
                    pos: pos.get(*axis),
                },
            );
        }
    }

    fn apply_limits(&mut self) {
        for axis in &Rotation::all_rot_axes() {
            let limits = self.lim[*axis as usize];
            debug_assert!(limits.neg <= limits.pos);
            self.rot
                .set(*axis, self.rot.get(*axis).clamp(limits.neg, limits.pos));
        }
    }
}

impl std::fmt::Display for RotationLim {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(
            f,
            "RotationLim({}, lim={},{},{})",
            self.rot,
            self.lim[RotAxis::Alpha as usize],
            self.lim[RotAxis::Beta as usize],
            self.lim[RotAxis::Gamma as usize]
        )
    }
}

#[derive(Debug)]
pub struct RotationLimMutWrapper<'a> {
    rot_lim: &'a mut RotationLim,
}

impl<'a> Deref for RotationLimMutWrapper<'a> {
    type Target = Rotation;

    fn deref(&self) -> &Self::Target {
        &self.rot_lim.rot
    }
}

impl<'a> DerefMut for RotationLimMutWrapper<'a> {
    fn deref_mut(&mut self) -> &mut Self::Target {
        &mut self.rot_lim.rot
    }
}

impl<'a> Drop for RotationLimMutWrapper<'a> {
    fn drop(&mut self) {
        self.rot_lim.apply_limits();
    }
}

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

    #[test]
    fn test_base() {
        // Constructors
        let a = Rotation::new(1.into(), 2.into(), 3.into(), RotMode::Absolute);
        assert_eq!(a.alpha(), 1.into());
        assert_eq!(a.beta(), 2.into());
        assert_eq!(a.gamma(), 3.into());
        assert_eq!(a.mode(), RotMode::Absolute);

        let a = Rotation::new(4.into(), 5.into(), 6.into(), RotMode::Relative);
        assert_eq!(a.alpha(), 4.into());
        assert_eq!(a.beta(), 5.into());
        assert_eq!(a.gamma(), 6.into());
        assert_eq!(a.mode(), RotMode::Relative);

        let a = Rotation::new_abs(1.into(), 2.into(), 3.into());
        assert_eq!(a.alpha(), 1.into());
        assert_eq!(a.beta(), 2.into());
        assert_eq!(a.gamma(), 3.into());
        assert_eq!(a.mode(), RotMode::Absolute);

        let a = Rotation::new_rel(4.into(), 5.into(), 6.into());
        assert_eq!(a.alpha(), 4.into());
        assert_eq!(a.beta(), 5.into());
        assert_eq!(a.gamma(), 6.into());
        assert_eq!(a.mode(), RotMode::Relative);

        let a = Rotation::new_zero_abs();
        assert_eq!(a.alpha(), 0.into());
        assert_eq!(a.beta(), 0.into());
        assert_eq!(a.gamma(), 0.into());
        assert_eq!(a.mode(), RotMode::Absolute);

        let a = Rotation::new_zero_rel();
        assert_eq!(a.alpha(), 0.into());
        assert_eq!(a.beta(), 0.into());
        assert_eq!(a.gamma(), 0.into());
        assert_eq!(a.mode(), RotMode::Relative);

        // Modulo
        assert_eq!(Rotation::modulo(radians(0.into())), radians(0.into()));
        assert_eq!(Rotation::modulo(radians(1.into())), radians(1.into()));
        assert_eq!(Rotation::modulo(radians(90.into())), radians(90.into()));
        assert_eq!(Rotation::modulo(radians(180.into())), radians(180.into()));
        assert_eq!(Rotation::modulo(radians(270.into())), radians(270.into()));
        assert_eq!(Rotation::modulo(radians(359.into())), radians(359.into()));
        assert_eq!(Rotation::modulo(radians(360.into())), radians(0.into()));
        assert_eq!(Rotation::modulo(radians(361.into())), radians(1.into()));
        assert_eq!(Rotation::modulo(radians(719.into())), radians(359.into()));
        assert_eq!(Rotation::modulo(radians(720.into())), radians(0.into()));
        assert_eq!(Rotation::modulo(radians(721.into())), radians(1.into()));
        assert_eq!(Rotation::modulo(radians((-1).into())), radians(359.into()));
        assert_eq!(Rotation::modulo(radians((-90).into())), radians(270.into()));
        assert_eq!(
            Rotation::modulo(radians((-180).into())),
            radians(180.into())
        );
        assert_eq!(Rotation::modulo(radians((-270).into())), radians(90.into()));
        assert_eq!(Rotation::modulo(radians((-359).into())), radians(1.into()));
        assert_eq!(Rotation::modulo(radians((-360).into())), radians(0.into()));
        assert_eq!(
            Rotation::modulo(radians((-361).into())),
            radians(359.into())
        );
        assert_eq!(Rotation::modulo(radians((-719).into())), radians(1.into()));
        assert_eq!(Rotation::modulo(radians((-720).into())), radians(0.into()));
        assert_eq!(
            Rotation::modulo(radians((-721).into())),
            radians(359.into())
        );

        // Set
        let mut a = Rotation::new_zero_abs();
        a.set_alpha(1.into());
        a.set_beta(2.into());
        a.set_gamma(3.into());
        assert_eq!(a.alpha(), 1.into());
        assert_eq!(a.beta(), 2.into());
        assert_eq!(a.gamma(), 3.into());

        // Make absolute
        let a = Rotation::new(
            radians(1.into()),
            radians(2.into()),
            radians(3.into()),
            RotMode::Absolute,
        );
        let a = a.absolute();
        assert_eq!(a.alpha(), radians(1.into()));
        assert_eq!(a.beta(), radians(2.into()));
        assert_eq!(a.gamma(), radians(3.into()));
        assert_eq!(a.mode(), RotMode::Absolute);
        let a = Rotation::new(
            radians(1.into()),
            radians(2.into()),
            radians(3.into()),
            RotMode::Relative,
        );
        let a = a.absolute();
        assert_eq!(a.alpha(), radians(1.into()));
        assert_eq!(a.beta(), radians(2.into()));
        assert_eq!(a.gamma(), radians(3.into()));
        assert_eq!(a.mode(), RotMode::Absolute);
        let a = Rotation::new(
            radians((-1).into()),
            radians((-2).into()),
            radians((-3).into()),
            RotMode::Relative,
        );
        let a = a.absolute();
        assert_eq!(a.alpha(), radians(359.into()));
        assert_eq!(a.beta(), radians(358.into()));
        assert_eq!(a.gamma(), radians(357.into()));
        assert_eq!(a.mode(), RotMode::Absolute);

        // Set zero
        let mut a = Rotation::new(1.into(), 2.into(), 3.into(), RotMode::Absolute);
        a.set_zero();
        assert_eq!(a.alpha(), 0.into());
        assert_eq!(a.beta(), 0.into());
        assert_eq!(a.gamma(), 0.into());
        assert_eq!(a.mode(), RotMode::Absolute);
        let mut a = Rotation::new(1.into(), 2.into(), 3.into(), RotMode::Relative);
        a.set_zero();
        assert_eq!(a.alpha(), 0.into());
        assert_eq!(a.beta(), 0.into());
        assert_eq!(a.gamma(), 0.into());
        assert_eq!(a.mode(), RotMode::Relative);

        // Clone
        let a = Rotation::new(1.into(), 2.into(), 3.into(), RotMode::Absolute).clone();
        assert_eq!(a.alpha(), 1.into());
        assert_eq!(a.beta(), 2.into());
        assert_eq!(a.gamma(), 3.into());
        assert_eq!(a.mode(), RotMode::Absolute);
        let a = Rotation::new(1.into(), 2.into(), 3.into(), RotMode::Relative).clone();
        assert_eq!(a.alpha(), 1.into());
        assert_eq!(a.beta(), 2.into());
        assert_eq!(a.gamma(), 3.into());
        assert_eq!(a.mode(), RotMode::Relative);

        // Assign
        let mut a = Rotation::new_zero_rel();
        let b = Rotation::new(1.into(), 2.into(), 3.into(), RotMode::Absolute);
        a.assign(&b);
        assert_eq!(a.alpha(), 1.into());
        assert_eq!(a.beta(), 2.into());
        assert_eq!(a.gamma(), 3.into());
        assert_eq!(a.mode(), RotMode::Absolute);

        // Copy sign
        let a = Rotation::new(1.into(), 2.into(), 3.into(), RotMode::Relative);
        let b = a.copy_sign(&Rotation::new(
            (-4).into(),
            5.into(),
            6.into(),
            RotMode::Relative,
        ));
        assert_eq!(b.alpha(), (-1).into());
        assert_eq!(b.beta(), 2.into());
        assert_eq!(b.gamma(), 3.into());
        assert_eq!(b.mode(), RotMode::Relative);
        let b = a.copy_sign(&Rotation::new(
            4.into(),
            (-5).into(),
            6.into(),
            RotMode::Relative,
        ));
        assert_eq!(b.alpha(), 1.into());
        assert_eq!(b.beta(), (-2).into());
        assert_eq!(b.gamma(), 3.into());
        assert_eq!(b.mode(), RotMode::Relative);
        let b = a.copy_sign(&Rotation::new(
            4.into(),
            5.into(),
            (-6).into(),
            RotMode::Relative,
        ));
        assert_eq!(b.alpha(), 1.into());
        assert_eq!(b.beta(), 2.into());
        assert_eq!(b.gamma(), (-3).into());
        assert_eq!(b.mode(), RotMode::Relative);
    }

    #[test]
    fn test_add_sat_bi() {
        // Positive
        let sat = Rotation::new(10.into(), 20.into(), 30.into(), RotMode::Relative);
        let mut a = Rotation::new(1.into(), 2.into(), 3.into(), RotMode::Relative);
        a.add_sat_bi_assign(
            &Rotation::new(4.into(), 5.into(), 6.into(), RotMode::Relative),
            &sat,
        );
        assert_eq!(a.alpha(), 5.into());
        assert_eq!(a.beta(), 7.into());
        assert_eq!(a.gamma(), 9.into());
        assert_eq!(a.mode(), RotMode::Relative);
        a.add_sat_bi_assign(
            &Rotation::new(400.into(), 500.into(), 600.into(), RotMode::Relative),
            &sat,
        );
        assert_eq!(a.alpha(), 10.into());
        assert_eq!(a.beta(), 20.into());
        assert_eq!(a.gamma(), 30.into());
        assert_eq!(a.mode(), RotMode::Relative);

        // Negative
        let sat = Rotation::new((-10).into(), (-20).into(), (-30).into(), RotMode::Relative);
        let mut a = Rotation::new(1.into(), 2.into(), 3.into(), RotMode::Relative);
        a.add_sat_bi_assign(
            &Rotation::new((-4).into(), (-6).into(), (-8).into(), RotMode::Relative),
            &sat,
        );
        assert_eq!(a.alpha(), (-3).into());
        assert_eq!(a.beta(), (-4).into());
        assert_eq!(a.gamma(), (-5).into());
        assert_eq!(a.mode(), RotMode::Relative);
        a.add_sat_bi_assign(
            &Rotation::new(
                (-400).into(),
                (-500).into(),
                (-600).into(),
                RotMode::Relative,
            ),
            &sat,
        );
        assert_eq!(a.alpha(), (-10).into());
        assert_eq!(a.beta(), (-20).into());
        assert_eq!(a.gamma(), (-30).into());
        assert_eq!(a.mode(), RotMode::Relative);
    }

    #[test]
    fn test_limits() {
        // Positive limit
        let a = Rotation::new(
            radians(10.into()),
            radians(20.into()),
            radians(30.into()),
            RotMode::Absolute,
        );
        let mut b = RotationLim::new(a);
        b.set_limits(
            &Rotation::new(
                radians(0.into()),
                radians(0.into()),
                radians(0.into()),
                RotMode::Absolute,
            ),
            &Rotation::new(
                radians(9.into()),
                radians(19.into()),
                radians(29.into()),
                RotMode::Absolute,
            ),
        );
        assert_eq!(b.get().alpha(), radians(9.into()));
        assert_eq!(b.get().beta(), radians(19.into()));
        assert_eq!(b.get().gamma(), radians(29.into()));
        assert_eq!(b.get().mode(), RotMode::Absolute);
        {
            let mut c = b.get_mut();
            c.set_alpha(radians(10.into()));
            assert_eq!(c.alpha(), radians(10.into()));
        }
        assert_eq!(b.get().alpha(), radians(9.into()));

        // Negative limit
        let a = Rotation::new(
            radians((-10).into()),
            radians((-20).into()),
            radians((-30).into()),
            RotMode::Relative,
        );
        let mut b = RotationLim::new(a);
        b.set_limits(
            &Rotation::new(
                radians((-9).into()),
                radians((-19).into()),
                radians((-29).into()),
                RotMode::Relative,
            ),
            &Rotation::new(
                radians(0.into()),
                radians(0.into()),
                radians(0.into()),
                RotMode::Relative,
            ),
        );
        assert_eq!(b.get().alpha(), radians((-9).into()));
        assert_eq!(b.get().beta(), radians((-19).into()));
        assert_eq!(b.get().gamma(), radians((-29).into()));
        assert_eq!(b.get().mode(), RotMode::Relative);
        {
            let mut c = b.get_mut();
            c.set_alpha(radians((-10).into()));
            assert_eq!(c.alpha(), radians((-10).into()));
        }
        assert_eq!(b.get().alpha(), radians((-9).into()));
    }

    #[test]
    fn test_arithmetic() {
        // Add
        let a = Rotation::new(
            radians(1.into()),
            radians(2.into()),
            radians(3.into()),
            RotMode::Absolute,
        );
        let b = Rotation::new(
            radians(10.into()),
            radians(20.into()),
            radians(30.into()),
            RotMode::Absolute,
        );
        let c = a + b;
        assert_eq!(c.alpha(), radians(11.into()));
        assert_eq!(c.beta(), radians(22.into()));
        assert_eq!(c.gamma(), radians(33.into()));
        assert_eq!(c.mode(), RotMode::Absolute);
        let a = Rotation::new(
            radians((-1).into()),
            radians((-2).into()),
            radians((-3).into()),
            RotMode::Relative,
        );
        let b = Rotation::new(
            radians(10.into()),
            radians(20.into()),
            radians(30.into()),
            RotMode::Relative,
        );
        let c = a + b;
        assert_eq!(c.alpha(), radians(9.into()));
        assert_eq!(c.beta(), radians(18.into()));
        assert_eq!(c.gamma(), radians(27.into()));
        assert_eq!(c.mode(), RotMode::Relative);

        // AddAssign
        let mut a = Rotation::new(
            radians(1.into()),
            radians(2.into()),
            radians(3.into()),
            RotMode::Absolute,
        );
        let b = Rotation::new(
            radians(10.into()),
            radians(20.into()),
            radians(30.into()),
            RotMode::Absolute,
        );
        a += b;
        assert_eq!(a.alpha(), radians(11.into()));
        assert_eq!(a.beta(), radians(22.into()));
        assert_eq!(a.gamma(), radians(33.into()));
        assert_eq!(a.mode(), RotMode::Absolute);
        let mut a = Rotation::new(
            radians((-1).into()),
            radians((-2).into()),
            radians((-3).into()),
            RotMode::Relative,
        );
        let b = Rotation::new(
            radians(10.into()),
            radians(20.into()),
            radians(30.into()),
            RotMode::Relative,
        );
        a += b;
        assert_eq!(a.alpha(), radians(9.into()));
        assert_eq!(a.beta(), radians(18.into()));
        assert_eq!(a.gamma(), radians(27.into()));
        assert_eq!(a.mode(), RotMode::Relative);

        // Sub
        let a = Rotation::new(
            radians(1.into()),
            radians(2.into()),
            radians(3.into()),
            RotMode::Absolute,
        );
        let b = Rotation::new(
            radians(10.into()),
            radians(20.into()),
            radians(30.into()),
            RotMode::Absolute,
        );
        let c = a - b;
        assert_eq!(c.alpha(), Rotation::modulo(radians((-9).into())));
        assert_eq!(c.beta(), Rotation::modulo(radians((-18).into())));
        assert_eq!(c.gamma(), Rotation::modulo(radians((-27).into())));
        assert_eq!(c.mode(), RotMode::Absolute);
        let a = Rotation::new(
            radians((-1).into()),
            radians((-2).into()),
            radians((-3).into()),
            RotMode::Relative,
        );
        let b = Rotation::new(
            radians(10.into()),
            radians(20.into()),
            radians(30.into()),
            RotMode::Relative,
        );
        let c = a - b;
        assert_eq!(c.alpha(), radians((-11).into()));
        assert_eq!(c.beta(), radians((-22).into()));
        assert_eq!(c.gamma(), radians((-33).into()));
        assert_eq!(c.mode(), RotMode::Relative);

        // SubAssign
        let mut a = Rotation::new(
            radians(1.into()),
            radians(2.into()),
            radians(3.into()),
            RotMode::Absolute,
        );
        let b = Rotation::new(
            radians(10.into()),
            radians(20.into()),
            radians(30.into()),
            RotMode::Absolute,
        );
        a -= b;
        assert_eq!(a.alpha(), Rotation::modulo(radians((-9).into())));
        assert_eq!(a.beta(), Rotation::modulo(radians((-18).into())));
        assert_eq!(a.gamma(), Rotation::modulo(radians((-27).into())));
        assert_eq!(a.mode(), RotMode::Absolute);
        let mut a = Rotation::new(
            radians((-1).into()),
            radians((-2).into()),
            radians((-3).into()),
            RotMode::Relative,
        );
        let b = Rotation::new(
            radians(10.into()),
            radians(20.into()),
            radians(30.into()),
            RotMode::Relative,
        );
        a -= b;
        assert_eq!(a.alpha(), radians((-11).into()));
        assert_eq!(a.beta(), radians((-22).into()));
        assert_eq!(a.gamma(), radians((-33).into()));
        assert_eq!(a.mode(), RotMode::Relative);

        // Neg
        let a = Rotation::new(
            radians(1.into()),
            radians(2.into()),
            radians(3.into()),
            RotMode::Relative,
        );
        let b = -a;
        assert_eq!(b.alpha(), radians((-1).into()));
        assert_eq!(b.beta(), radians((-2).into()));
        assert_eq!(b.gamma(), radians((-3).into()));
        assert_eq!(b.mode(), RotMode::Relative);
        let a = Rotation::new(
            radians(1.into()),
            radians(2.into()),
            radians(3.into()),
            RotMode::Absolute,
        );
        let b = -a;
        assert_eq!(b.alpha(), radians((-1).into()));
        assert_eq!(b.beta(), radians((-2).into()));
        assert_eq!(b.gamma(), radians((-3).into()));
        assert_eq!(b.mode(), RotMode::Relative);

        // Mul
        let a = Rotation::new(
            radians(1.into()),
            radians(2.into()),
            radians(3.into()),
            RotMode::Absolute,
        );
        let b = a * 10.into();
        assert_eq!(b.alpha(), Rotation::modulo(radians(10.into())));
        assert_eq!(b.beta(), Rotation::modulo(radians(20.into())));
        assert_eq!(b.gamma(), Rotation::modulo(radians(30.into())));
        assert_eq!(b.mode(), RotMode::Absolute);
        let a = Rotation::new(
            radians((-1).into()),
            radians((-2).into()),
            radians((-3).into()),
            RotMode::Relative,
        );
        let b = a * 10.into();
        assert_eq!(b.alpha(), radians((-10).into()));
        assert_eq!(b.beta(), radians((-20).into()));
        assert_eq!(b.gamma(), radians((-30).into()));
        assert_eq!(b.mode(), RotMode::Relative);

        // MulAssign
        let mut a = Rotation::new(
            radians(1.into()),
            radians(2.into()),
            radians(3.into()),
            RotMode::Absolute,
        );
        a *= 10.into();
        assert_eq!(a.alpha(), Rotation::modulo(radians(10.into())));
        assert_eq!(a.beta(), Rotation::modulo(radians(20.into())));
        assert_eq!(a.gamma(), Rotation::modulo(radians(30.into())));
        assert_eq!(a.mode(), RotMode::Absolute);
        let mut a = Rotation::new(
            radians((-1).into()),
            radians((-2).into()),
            radians((-3).into()),
            RotMode::Relative,
        );
        a *= 10.into();
        assert_eq!(a.alpha(), radians((-10).into()));
        assert_eq!(a.beta(), radians((-20).into()));
        assert_eq!(a.gamma(), radians((-30).into()));
        assert_eq!(a.mode(), RotMode::Relative);
    }

    #[test]
    fn test_rotate_vector() {
        // No rotation.
        let r = Rotation::new_zero_rel();
        let v = Vector::new(10.into(), 11.into(), 12.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), 10.into());
        assert_eq!(v.y(), 11.into());
        assert_eq!(v.z(), 12.into());

        // Alpha does not affect vector rotation.
        let r = Rotation::new_rel(radians(10.into()), radians(0.into()), radians(0.into()));
        let v = Vector::new(10.into(), 11.into(), 12.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), 10.into());
        assert_eq!(v.y(), 11.into());
        assert_eq!(v.z(), 12.into());
        let r = Rotation::new_rel(radians((-10).into()), radians(0.into()), radians(0.into()));
        let v = Vector::new(10.into(), 11.into(), 12.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), 10.into());
        assert_eq!(v.y(), 11.into());
        assert_eq!(v.z(), 12.into());

        // Yaw 45 deg
        let r = Rotation::new_rel(radians(0.into()), radians(0.into()), radians(45.into()));
        let v = Vector::new(10.into(), 0.into(), 0.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), (radians(45.into()).cos() * 10.0).into());
        assert_eq!(v.y(), (radians(45.into()).sin() * 10.0).into());
        assert_eq!(v.z(), 0.into());

        // Yaw -45 deg
        let r = Rotation::new_rel(radians(0.into()), radians(0.into()), radians((-45).into()));
        let v = Vector::new(10.into(), 0.into(), 0.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), (radians((-45).into()).cos() * 10.0).into());
        assert_eq!(v.y(), (radians((-45).into()).sin() * 10.0).into());
        assert_eq!(v.z(), 0.into());

        // Yaw 90 deg
        let r = Rotation::new_rel(radians(0.into()), radians(0.into()), radians(90.into()));
        let v = Vector::new(10.into(), 11.into(), 12.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), (-11).into());
        assert_eq!(v.y(), 10.into());
        assert_eq!(v.z(), 12.into());

        // Yaw -90 deg
        let r = Rotation::new_rel(radians(0.into()), radians(0.into()), radians((-90).into()));
        let v = Vector::new(10.into(), 11.into(), 12.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), 11.into());
        assert_eq!(v.y(), (-10).into());
        assert_eq!(v.z(), 12.into());

        // Yaw 135 deg
        let r = Rotation::new_rel(radians(0.into()), radians(0.into()), radians(135.into()));
        let v = Vector::new(10.into(), 0.into(), 0.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), (radians(135.into()).cos() * 10.0).into());
        assert_eq!(v.y(), (radians(135.into()).sin() * 10.0).into());
        assert_eq!(v.z(), 0.into());

        // Yaw -135 deg
        let r = Rotation::new_rel(radians(0.into()), radians(0.into()), radians((-135).into()));
        let v = Vector::new(10.into(), 0.into(), 0.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), (radians((-135).into()).cos() * 10.0).into());
        assert_eq!(v.y(), (radians((-135).into()).sin() * 10.0).into());
        assert_eq!(v.z(), 0.into());

        // Yaw 180 deg
        let r = Rotation::new_rel(radians(0.into()), radians(0.into()), radians(180.into()));
        let v = Vector::new(10.into(), 11.into(), 12.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), (-10).into());
        assert_eq!(v.y(), (-11).into());
        assert_eq!(v.z(), 12.into());

        // Yaw -180 deg
        let r = Rotation::new_rel(radians(0.into()), radians(0.into()), radians((-180).into()));
        let v = Vector::new(10.into(), 11.into(), 12.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), (-10).into());
        assert_eq!(v.y(), (-11).into());
        assert_eq!(v.z(), 12.into());

        // Yaw 360 deg
        let r = Rotation::new_rel(radians(0.into()), radians(0.into()), radians(360.into()));
        let v = Vector::new(10.into(), 11.into(), 12.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), 10.into());
        assert_eq!(v.y(), 11.into());
        assert_eq!(v.z(), 12.into());

        // Yaw -360 deg
        let r = Rotation::new_rel(radians(0.into()), radians(0.into()), radians((-360).into()));
        let v = Vector::new(10.into(), 11.into(), 12.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), 10.into());
        assert_eq!(v.y(), 11.into());
        assert_eq!(v.z(), 12.into());

        // Pitch 90 deg
        let r = Rotation::new_rel(radians(0.into()), radians(90.into()), radians(0.into()));
        let v = Vector::new(10.into(), 0.into(), 0.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), 0.into());
        assert_eq!(v.y(), 0.into());
        assert_eq!(v.z(), (-10.0).into());

        // Pitch -90 deg
        let r = Rotation::new_rel(radians(0.into()), radians((-90).into()), radians(0.into()));
        let v = Vector::new(10.into(), 0.into(), 0.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), 0.into());
        assert_eq!(v.y(), 0.into());
        assert_eq!(v.z(), 10.0.into());

        // Pitch 45 deg
        let r = Rotation::new_rel(radians(0.into()), radians(45.into()), radians(0.into()));
        let v = Vector::new(10.into(), 0.into(), 0.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), (radians(45.into()).cos() * 10.0).into());
        assert_eq!(v.y(), 0.into());
        assert_eq!(v.z(), (radians(45.into()).sin() * -10.0).into());

        // Pitch -45 deg
        let r = Rotation::new_rel(radians(0.into()), radians((-45).into()), radians(0.into()));
        let v = Vector::new(10.into(), 0.into(), 0.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), (radians((-45).into()).cos() * 10.0).into());
        assert_eq!(v.y(), 0.into());
        assert_eq!(v.z(), (radians((-45).into()).sin() * -10.0).into());

        // Length 0 on XY plane.
        let r = Rotation::new_rel(radians(0.into()), radians(90.into()), radians(0.into()));
        let v = Vector::new(0.into(), 0.into(), 12.into());
        let v = r.rotate_vector(&v);
        assert_eq!(v.x(), 12.into());
        assert_eq!(v.y(), 0.into());
        assert_eq!(v.z(), 0.into());

        // Check if forward and backward rotation cancel out.
        for beta in -15..=164 {
            for gamma in -360..=360 {
                let r_fwd = Rotation::new_rel(
                    radians(0.into()),
                    radians(beta.into()),
                    radians(gamma.into()),
                );
                let r_bwd = Rotation::new_rel(
                    radians(0.into()),
                    radians((-beta).into()),
                    radians((-gamma).into()),
                );
                let v = Vector::new(23.into(), 15.into(), 98.into());
                let v = r_fwd.rotate_vector(&v);
                let v = r_bwd.rotate_vector(&v);
                assert_eq!(v.x(), 23.into());
                assert_eq!(v.y(), 15.into());
                assert_eq!(v.z(), 98.into());
            }
        }
    }
}

// vim: ts=4 sw=4 expandtab