// -*- 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
//

use super::{
    super::{
        base::{AccelMode, PhyType},
        world::WorldContext,
    },
    RotMode, Rotation, RotationLim,
};

#[derive(Clone, Debug)]
pub struct RotMove {
    rot: RotationLim,
    rot_acc: RotationLim,
    rot_speed: RotationLim,
    acc_mode: AccelMode,
}

impl RotMove {
    pub fn new(rot_mode: RotMode) -> RotMove {
        RotMove {
            rot: RotationLim::new(Rotation::new_zero(rot_mode)),
            rot_acc: RotationLim::new(Rotation::new_zero_rel()),
            rot_speed: RotationLim::new(Rotation::new_zero_rel()),
            acc_mode: AccelMode::Decelerate,
        }
    }

    pub fn new_rel() -> RotMove {
        RotMove::new(RotMode::Relative)
    }

    pub fn new_abs() -> RotMove {
        RotMove::new(RotMode::Absolute)
    }

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

    pub fn set_rot(&mut self, rot: &Rotation) {
        self.rot.get_mut().assign(rot);
    }

    pub fn set_rot_lim(&mut self, neg_lim: &Rotation, pos_lim: &Rotation) {
        debug_assert_eq!(neg_lim.mode(), RotMode::Relative);
        debug_assert_eq!(pos_lim.mode(), RotMode::Relative);
        self.rot.set_limits(neg_lim, pos_lim);
    }

    pub fn get_rot_acc(&self) -> Rotation {
        self.rot_acc.get().clone()
    }

    pub fn get_rot_acc_mode(&self) -> AccelMode {
        self.acc_mode
    }

    pub fn set_rot_acc(&mut self, acc: &Rotation, mode: AccelMode) {
        self.rot_acc.get_mut().assign(acc);
        self.acc_mode = mode;
    }

    pub fn set_rot_acc_lim(&mut self, neg_lim: &Rotation, pos_lim: &Rotation) {
        debug_assert_eq!(neg_lim.mode(), RotMode::Relative);
        debug_assert_eq!(pos_lim.mode(), RotMode::Relative);
        self.rot_acc.set_limits(neg_lim, pos_lim);
    }

    pub fn set_rot_speed_lim(&mut self, neg_lim: &Rotation, pos_lim: &Rotation) {
        debug_assert_eq!(neg_lim.mode(), RotMode::Relative);
        debug_assert_eq!(pos_lim.mode(), RotMode::Relative);
        self.rot_speed.set_limits(neg_lim, pos_lim);
    }

    pub fn get_rot_speed(&self) -> Rotation {
        self.rot_speed.get().clone()
    }

    /// Accelerate towards the specified speed with a specified acceleration.
    pub fn acc_to_speed(&mut self, speed: &Rotation, acc: &Rotation) {
        let mut acc = acc.clone();
        let mut speed_lim_neg = Rotation::new_rel(
            -PhyType::UNLIMITED,
            -PhyType::UNLIMITED,
            -PhyType::UNLIMITED,
        );
        let mut speed_lim_pos =
            Rotation::new_rel(PhyType::UNLIMITED, PhyType::UNLIMITED, PhyType::UNLIMITED);
        for axis in &Rotation::all_rot_axes() {
            let cur_speed = self.rot_speed.get().get(*axis);
            let new_speed = speed.get(*axis);
            let new_acc = acc.get(*axis);

            if cur_speed < new_speed {
                if new_acc < 0.into() {
                    acc.set(*axis, -new_acc);
                }
                speed_lim_pos.set(*axis, new_speed);
            } else {
                if new_acc > 0.into() {
                    acc.set(*axis, -new_acc);
                }
                speed_lim_neg.set(*axis, new_speed);
            }
        }
        self.set_rot_acc(&acc, AccelMode::Accelerate);
        self.set_rot_speed_lim(&speed_lim_neg, &speed_lim_pos);
    }

    fn do_calc(&mut self, period_sec: PhyType) {
        match self.acc_mode {
            AccelMode::Accelerate => {
                // Accelerate rotation.
                let rot_acc = self.get_rot_acc();
                *self.rot_speed.get_mut() += rot_acc * period_sec;
            }
            AccelMode::Decelerate => {
                // Decelerate towards zero.
                let rot_acc = -(self.get_rot_acc().copy_sign(self.rot_speed.get()));
                self.rot_speed
                    .get_mut()
                    .add_sat_bi_assign(&(rot_acc * period_sec), &Rotation::new_zero_abs());
            }
        }

        // Calculate the new rotational position.
        let rot_speed = self.get_rot_speed();
        *self.rot.get_mut() += rot_speed * period_sec;
    }

    pub fn calc(&mut self, world_context: &WorldContext) {
        self.do_calc(world_context.period_sec());
    }
}

impl std::fmt::Display for RotMove {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(
            f,
            "RotMove({},\nrot={},\naccel={},\nspeed={})",
            self.acc_mode, self.rot, self.rot_acc, self.rot_speed
        )
    }
}

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

    #[test]
    fn test_base() {
        // Zero
        let a = RotMove::new_abs();
        assert_eq!(a.get_rot().alpha(), 0.into());
        assert_eq!(a.get_rot().beta(), 0.into());
        assert_eq!(a.get_rot().gamma(), 0.into());
        assert_eq!(a.get_rot().mode(), RotMode::Absolute);
        assert_eq!(a.get_rot_acc().alpha(), 0.into());
        assert_eq!(a.get_rot_acc().beta(), 0.into());
        assert_eq!(a.get_rot_acc().gamma(), 0.into());
        assert_eq!(a.get_rot_acc_mode(), AccelMode::Decelerate);
        assert_eq!(a.get_rot_speed().alpha(), 0.into());
        assert_eq!(a.get_rot_speed().beta(), 0.into());
        assert_eq!(a.get_rot_speed().gamma(), 0.into());
        let a = RotMove::new_rel();
        assert_eq!(a.get_rot().mode(), RotMode::Relative);

        // Set rot/rot_acc
        let mut a = RotMove::new_rel();
        a.set_rot(&Rotation::new_abs(1.into(), 2.into(), 3.into()));
        a.set_rot_acc(
            &Rotation::new_rel(4.into(), 5.into(), 6.into()),
            AccelMode::Accelerate,
        );
        assert_eq!(a.get_rot().alpha(), 1.into());
        assert_eq!(a.get_rot().beta(), 2.into());
        assert_eq!(a.get_rot().gamma(), 3.into());
        assert_eq!(a.get_rot().mode(), RotMode::Absolute);
        assert_eq!(a.get_rot_acc().alpha(), 4.into());
        assert_eq!(a.get_rot_acc().beta(), 5.into());
        assert_eq!(a.get_rot_acc().gamma(), 6.into());
        assert_eq!(a.get_rot_acc_mode(), AccelMode::Accelerate);
        assert_eq!(a.get_rot_speed().alpha(), 0.into());
        assert_eq!(a.get_rot_speed().beta(), 0.into());
        assert_eq!(a.get_rot_speed().gamma(), 0.into());
    }

    #[test]
    fn test_limits() {
        // Rot lim
        let mut a = RotMove::new_rel();
        a.set_rot_lim(
            &Rotation::new_rel((-1).into(), (-2).into(), (-3).into()),
            &Rotation::new_rel(1.into(), 2.into(), 3.into()),
        );
        a.set_rot(&Rotation::new_rel(2.into(), 3.into(), 4.into()));
        assert_eq!(a.get_rot().alpha(), 1.into());
        assert_eq!(a.get_rot().beta(), 2.into());
        assert_eq!(a.get_rot().gamma(), 3.into());
        assert_eq!(a.get_rot().mode(), RotMode::Relative);

        // Rot acc lim
        let mut a = RotMove::new_rel();
        a.set_rot_acc_lim(
            &Rotation::new_rel((-1).into(), (-2).into(), (-3).into()),
            &Rotation::new_rel(1.into(), 2.into(), 3.into()),
        );
        a.set_rot_acc(
            &Rotation::new_rel(2.into(), 3.into(), 4.into()),
            AccelMode::Accelerate,
        );
        assert_eq!(a.get_rot_acc().alpha(), 1.into());
        assert_eq!(a.get_rot_acc().beta(), 2.into());
        assert_eq!(a.get_rot_acc().gamma(), 3.into());
        assert_eq!(a.get_rot_acc().mode(), RotMode::Relative);

        // Rot speed lim
        let mut a = RotMove::new_rel();
        a.set_rot_speed_lim(
            &Rotation::new_rel(
                radians((-10).into()),
                radians((-20).into()),
                radians((-30).into()),
            ),
            &Rotation::new_rel(radians(10.into()), radians(20.into()), radians(30.into())),
        );
        a.set_rot_acc(
            &Rotation::new_rel(radians(1.into()), radians(1.into()), radians(1.into())),
            AccelMode::Accelerate,
        );
        a.do_calc(1.into());
        assert_eq!(a.get_rot_speed().alpha(), radians(1.into()));
        assert_eq!(a.get_rot_speed().beta(), radians(1.into()));
        assert_eq!(a.get_rot_speed().gamma(), radians(1.into()));
        a.do_calc(40.into());
        assert_eq!(a.get_rot_speed().alpha(), radians(10.into()));
        assert_eq!(a.get_rot_speed().beta(), radians(20.into()));
        assert_eq!(a.get_rot_speed().gamma(), radians(30.into()));
    }

    #[test]
    fn test_calc() {
        let mut a = RotMove::new_rel();

        // Accelerate positive
        a.set_rot_acc(
            &Rotation::new_rel(radians(2.into()), radians(3.into()), radians(4.into())),
            AccelMode::Accelerate,
        );
        a.do_calc(1.into());
        assert_eq!(a.get_rot_speed().alpha(), radians(2.into()));
        assert_eq!(a.get_rot_speed().beta(), radians(3.into()));
        assert_eq!(a.get_rot_speed().gamma(), radians(4.into()));
        a.do_calc(1.into());
        assert_eq!(a.get_rot_speed().alpha(), radians(4.into()));
        assert_eq!(a.get_rot_speed().beta(), radians(6.into()));
        assert_eq!(a.get_rot_speed().gamma(), radians(8.into()));
        a.do_calc(10.into());
        assert_eq!(a.get_rot_speed().alpha(), radians(24.into()));
        assert_eq!(a.get_rot_speed().beta(), radians(36.into()));
        assert_eq!(a.get_rot_speed().gamma(), radians(48.into()));

        // Decelerate from positive
        a.set_rot_acc(
            &Rotation::new_rel(radians(5.into()), radians(6.into()), radians(7.into())),
            AccelMode::Decelerate,
        );
        a.do_calc(1.into());
        assert_eq!(a.get_rot_speed().alpha(), radians(19.into()));
        assert_eq!(a.get_rot_speed().beta(), radians(30.into()));
        assert_eq!(a.get_rot_speed().gamma(), radians(41.into()));
        a.do_calc(1.into());
        assert_eq!(a.get_rot_speed().alpha(), radians(14.into()));
        assert_eq!(a.get_rot_speed().beta(), radians(24.into()));
        assert_eq!(a.get_rot_speed().gamma(), radians(34.into()));
        a.do_calc(20.into());
        assert_eq!(a.get_rot_speed().alpha(), radians(0.into()));
        assert_eq!(a.get_rot_speed().beta(), radians(0.into()));
        assert_eq!(a.get_rot_speed().gamma(), radians(0.into()));

        let mut a = RotMove::new_rel();

        // Accelerate negative
        a.set_rot_acc(
            &Rotation::new_rel(
                radians((-2).into()),
                radians((-3).into()),
                radians((-4).into()),
            ),
            AccelMode::Accelerate,
        );
        a.do_calc(1.into());
        assert_eq!(a.get_rot_speed().alpha(), radians((-2).into()));
        assert_eq!(a.get_rot_speed().beta(), radians((-3).into()));
        assert_eq!(a.get_rot_speed().gamma(), radians((-4).into()));
        a.do_calc(1.into());
        assert_eq!(a.get_rot_speed().alpha(), radians((-4).into()));
        assert_eq!(a.get_rot_speed().beta(), radians((-6).into()));
        assert_eq!(a.get_rot_speed().gamma(), radians((-8).into()));
        a.do_calc(10.into());
        assert_eq!(a.get_rot_speed().alpha(), radians((-24).into()));
        assert_eq!(a.get_rot_speed().beta(), radians((-36).into()));
        assert_eq!(a.get_rot_speed().gamma(), radians((-48).into()));

        // Decelerate from negative
        a.set_rot_acc(
            &Rotation::new_rel(radians(5.into()), radians(6.into()), radians(7.into())),
            AccelMode::Decelerate,
        );
        a.do_calc(1.into());
        assert_eq!(a.get_rot_speed().alpha(), radians((-19).into()));
        assert_eq!(a.get_rot_speed().beta(), radians((-30).into()));
        assert_eq!(a.get_rot_speed().gamma(), radians((-41).into()));
        a.do_calc(1.into());
        assert_eq!(a.get_rot_speed().alpha(), radians((-14).into()));
        assert_eq!(a.get_rot_speed().beta(), radians((-24).into()));
        assert_eq!(a.get_rot_speed().gamma(), radians((-34).into()));
        a.do_calc(20.into());
        assert_eq!(a.get_rot_speed().alpha(), radians(0.into()));
        assert_eq!(a.get_rot_speed().beta(), radians(0.into()));
        assert_eq!(a.get_rot_speed().gamma(), radians(0.into()));
    }

    #[test]
    fn test_acc_to_speed() {
        let mut a = RotMove::new_rel();

        let acc = Rotation::new_rel(radians(1.into()), radians(2.into()), radians(3.into()));
        let speed = Rotation::new_rel(radians(5.into()), radians(6.into()), radians(7.into()));
        a.acc_to_speed(&speed, &acc);
        a.do_calc(20.into());
        assert_eq!(a.get_rot_acc(), acc);
        assert_eq!(a.get_rot_speed(), speed);

        let acc = Rotation::new_rel(radians(1.into()), radians(2.into()), radians(3.into()));
        let speed = Rotation::new_rel(
            radians((-5).into()),
            radians((-6).into()),
            radians((-7).into()),
        );
        a.acc_to_speed(&speed, &acc);
        a.do_calc(30.into());
        assert_eq!(a.get_rot_acc(), -acc);
        assert_eq!(a.get_rot_speed(), speed);

        let acc = Rotation::new_rel(
            radians((-1).into()),
            radians((-2).into()),
            radians((-3).into()),
        );
        let speed = Rotation::new_rel(radians(5.into()), radians(6.into()), radians(7.into()));
        a.acc_to_speed(&speed, &acc);
        a.do_calc(40.into());
        assert_eq!(a.get_rot_acc(), -acc);
        assert_eq!(a.get_rot_speed(), speed);
    }
}

// vim: ts=4 sw=4 expandtab