// -*- 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 crate::base::{Axis, Direction, PhyType, NR_AXES, NR_DIRS};
use std::ops::{Add, AddAssign, Deref, DerefMut, Mul, MulAssign, Neg, Sub, SubAssign};

#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Vector {
    x: PhyType,
    y: PhyType,
    z: PhyType,
}

impl Vector {
    pub fn new(x: PhyType, y: PhyType, z: PhyType) -> Vector {
        Vector { x, y, z }
    }

    pub fn new_zero() -> Vector {
        Vector::new(0.into(), 0.into(), 0.into())
    }

    pub fn length(&self) -> PhyType {
        ((self.x * self.x) + (self.y * self.y) + (self.z * self.z))
            .sqrt()
            .into()
    }

    fn inv_length(&self) -> Option<PhyType> {
        let abs_len = self.length();
        debug_assert!(abs_len >= 0.0.into());
        if *abs_len < *PhyType::EPSILON {
            None
        } else {
            let one: PhyType = 1.0.into();
            Some(one / abs_len)
        }
    }

    pub fn set_length(&mut self, length: PhyType) {
        let norm = self.normalize();
        self.set_xyz(norm.x() * length, norm.y() * length, norm.z() * length);
    }

    pub fn normalize(&self) -> Vector {
        match self.inv_length() {
            Some(inv_length) => Vector::new(
                self.x() * inv_length,
                self.y() * inv_length,
                self.z() * inv_length,
            ),
            None => Vector::new_zero(),
        }
    }

    pub fn x(&self) -> PhyType {
        self.x
    }

    pub fn set_x(&mut self, x: PhyType) {
        self.x = x;
    }

    pub fn y(&self) -> PhyType {
        self.y
    }

    pub fn set_y(&mut self, y: PhyType) {
        self.y = y;
    }

    pub fn z(&self) -> PhyType {
        self.z
    }

    pub fn set_z(&mut self, z: PhyType) {
        self.z = z;
    }

    pub fn set_xyz(&mut self, x: PhyType, y: PhyType, z: PhyType) {
        self.set_x(x);
        self.set_y(y);
        self.set_z(z);
    }

    pub fn set_zero(&mut self) {
        self.set_xyz(0.into(), 0.into(), 0.into());
    }

    pub fn assign(&mut self, other: &Vector) {
        self.set_xyz(other.x(), other.y(), other.z());
    }

    pub fn get_axis(&self, axis: Axis) -> PhyType {
        match axis {
            Axis::X => self.x(),
            Axis::Y => self.y(),
            Axis::Z => self.z(),
        }
    }

    pub fn set_axis(&mut self, axis: Axis, value: PhyType) {
        match axis {
            Axis::X => self.set_x(value),
            Axis::Y => self.set_y(value),
            Axis::Z => self.set_z(value),
        }
    }
}

impl std::fmt::Display for Vector {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(f, "Vector(x={}, y={}, z={})", self.x(), self.y(), self.z())
    }
}

impl Default for Vector {
    fn default() -> Self {
        Vector::new_zero()
    }
}

impl Add for Vector {
    type Output = Self;

    fn add(self, other: Self) -> Self::Output {
        Vector::new(
            self.x() + other.x(),
            self.y() + other.y(),
            self.z() + other.z(),
        )
    }
}

impl AddAssign for Vector {
    fn add_assign(&mut self, other: Self) {
        self.set_xyz(
            self.x() + other.x(),
            self.y() + other.y(),
            self.z() + other.z(),
        );
    }
}

impl Sub for Vector {
    type Output = Self;

    fn sub(self, other: Self) -> Self::Output {
        Vector::new(
            self.x() - other.x(),
            self.y() - other.y(),
            self.z() - other.z(),
        )
    }
}

impl SubAssign for Vector {
    fn sub_assign(&mut self, other: Self) {
        self.set_xyz(
            self.x() - other.x(),
            self.y() - other.y(),
            self.z() - other.z(),
        );
    }
}

impl Neg for Vector {
    type Output = Self;

    fn neg(self) -> Self::Output {
        self * (-1).into()
    }
}

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

    fn mul(self, rhs: PhyType) -> Self::Output {
        Vector::new(self.x() * rhs, self.y() * rhs, self.z() * rhs)
    }
}

impl MulAssign<PhyType> for Vector {
    fn mul_assign(&mut self, rhs: PhyType) {
        self.set_xyz(self.x() * rhs, self.y() * rhs, self.z() * rhs);
    }
}

#[derive(Clone, Debug)]
pub struct VectorLim {
    vect: Vector,
    lim_neg: PhyType,
    lim_pos: PhyType,
    clipping: [[PhyType; NR_DIRS]; NR_AXES],
}

impl VectorLim {
    pub fn new(vect: Vector) -> VectorLim {
        VectorLim {
            vect,
            lim_neg: -PhyType::UNLIMITED,
            lim_pos: PhyType::UNLIMITED,
            clipping: [
                [-PhyType::UNLIMITED, PhyType::UNLIMITED],
                [-PhyType::UNLIMITED, PhyType::UNLIMITED],
                [-PhyType::UNLIMITED, PhyType::UNLIMITED],
            ],
        }
    }

    #[allow(unused)]
    pub fn new_lim(vect: Vector, neg: PhyType, pos: PhyType) -> VectorLim {
        let mut _self = VectorLim::new(vect);
        _self.set_limits(neg, pos);
        _self
    }

    pub fn new_zero() -> VectorLim {
        VectorLim::new(Default::default())
    }

    pub fn limited(&self) -> Vector {
        let mut vect = self.vect;

        // Apply length limits
        if self.lim_neg != -PhyType::UNLIMITED || self.lim_pos != PhyType::UNLIMITED {
            let length = vect.length();
            let lim_length: PhyType = length.clamp(self.lim_neg, self.lim_pos);
            if length != lim_length {
                vect.set_length(lim_length);
            }
        }

        // Clip cartesian coordinates
        for axis in &[Axis::X, Axis::Y, Axis::Z] {
            let neg = self.clipping[*axis as usize][Direction::Neg as usize];
            let pos = self.clipping[*axis as usize][Direction::Pos as usize];
            if neg != -PhyType::UNLIMITED || pos != PhyType::UNLIMITED {
                let axval = vect.get_axis(*axis);
                let clip_axval: PhyType = axval.clamp(neg, pos);
                if axval != clip_axval {
                    vect.set_axis(*axis, clip_axval);
                }
            }
        }

        vect
    }

    pub fn apply_limits(&mut self) {
        self.vect.assign(&self.limited());
    }

    pub fn set_limits(&mut self, mut neg: PhyType, mut pos: PhyType) {
        if neg == PhyType::UNLIMITED {
            neg = -PhyType::UNLIMITED;
        }
        if pos == -PhyType::UNLIMITED {
            pos = PhyType::UNLIMITED;
        }
        debug_assert!(neg <= pos);
        self.lim_neg = neg;
        self.lim_pos = pos;
    }

    pub fn set_clipping(&mut self, axis: Axis, mut neg: PhyType, mut pos: PhyType) {
        if neg == PhyType::UNLIMITED {
            neg = -PhyType::UNLIMITED;
        }
        if pos == -PhyType::UNLIMITED {
            pos = PhyType::UNLIMITED;
        }
        debug_assert!(neg <= pos);
        self.clipping[axis as usize][Direction::Neg as usize] = neg;
        self.clipping[axis as usize][Direction::Pos as usize] = pos;
    }
}

impl std::fmt::Display for VectorLim {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(
            f,
            "Lim({}, lim={}/{}, x_clip={}/{}, y_clip={}/{}, z_clip={}/{})",
            self.vect,
            self.lim_neg,
            self.lim_pos,
            self.clipping[Axis::X as usize][Direction::Neg as usize],
            self.clipping[Axis::X as usize][Direction::Pos as usize],
            self.clipping[Axis::Y as usize][Direction::Neg as usize],
            self.clipping[Axis::Y as usize][Direction::Pos as usize],
            self.clipping[Axis::Z as usize][Direction::Neg as usize],
            self.clipping[Axis::Z as usize][Direction::Pos as usize]
        )
    }
}

impl Default for VectorLim {
    fn default() -> Self {
        VectorLim::new_zero()
    }
}

impl Deref for VectorLim {
    type Target = Vector;

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

impl DerefMut for VectorLim {
    fn deref_mut(&mut self) -> &mut Self::Target {
        &mut self.vect
    }
}

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

    #[test]
    fn test_base() {
        let a = Vector::new(1.into(), 2.into(), 3.into());
        assert_eq!(a.x(), 1.into());
        assert_eq!(a.y(), 2.into());
        assert_eq!(a.z(), 3.into());

        let a = Vector::new((-1).into(), (-2).into(), (-3).into());
        assert_eq!(a.x(), (-1).into());
        assert_eq!(a.y(), (-2).into());
        assert_eq!(a.z(), (-3).into());

        let a = Vector::new_zero();
        assert_eq!(a.x(), 0.into());
        assert_eq!(a.y(), 0.into());
        assert_eq!(a.z(), 0.into());

        let mut a = Vector::new(1.into(), 2.into(), 3.into());
        a.set_xyz(4.into(), 5.into(), 6.into());
        assert_eq!(a.x(), 4.into());
        assert_eq!(a.y(), 5.into());
        assert_eq!(a.z(), 6.into());

        let mut a = Vector::new(1.into(), 2.into(), 3.into());
        a.set_xyz((-4).into(), (-5).into(), (-6).into());
        assert_eq!(a.x(), (-4).into());
        assert_eq!(a.y(), (-5).into());
        assert_eq!(a.z(), (-6).into());

        let mut a = Vector::new(1.into(), 2.into(), 3.into());
        a.set_zero();
        assert_eq!(a.x(), 0.into());
        assert_eq!(a.y(), 0.into());
        assert_eq!(a.z(), 0.into());

        let mut a = Vector::new(1.into(), 2.into(), 3.into());
        a.assign(&Vector::new(4.into(), 5.into(), 6.into()));
        assert_eq!(a.x(), 4.into());
        assert_eq!(a.y(), 5.into());
        assert_eq!(a.z(), 6.into());
    }

    #[test]
    fn test_length() {
        // Parallel to axis
        let a = Vector::new(42.into(), 0.into(), 0.into());
        assert_eq!(a.length(), 42.into());
        let a = Vector::new(0.into(), 42.into(), 0.into());
        assert_eq!(a.length(), 42.into());
        let a = Vector::new(0.into(), 0.into(), 42.into());
        assert_eq!(a.length(), 42.into());

        // Length of normalized vector
        let a = Vector::new(1.into(), 1.into(), 1.into());
        assert_eq!(a.length(), ((1 * 1 + 1 * 1 + 1 * 1) as f64).sqrt().into());

        // Length of vector
        let a = Vector::new(5.into(), 7.into(), 13.into());
        assert_eq!(a.length(), ((5 * 5 + 7 * 7 + 13 * 13) as f64).sqrt().into());

        // Length of negative vector
        let a = Vector::new((-5).into(), (-7).into(), (-13).into());
        assert_eq!(a.length(), ((5 * 5 + 7 * 7 + 13 * 13) as f64).sqrt().into());

        // Set positive length
        let mut a = Vector::new(5.into(), 7.into(), 13.into());
        assert_eq!(a.length(), ((5 * 5 + 7 * 7 + 13 * 13) as f64).sqrt().into());
        assert_eq!(a.x(), 5.into());
        assert_eq!(a.y(), 7.into());
        assert_eq!(a.z(), 13.into());
        a.set_length((((5 * 5 + 7 * 7 + 13 * 13) as f64).sqrt() * 2.0).into());
        assert_eq!(
            a.length(),
            (((5 * 5 + 7 * 7 + 13 * 13) as f64).sqrt() * 2.0).into()
        );
        assert_eq!(a.x(), 10.into());
        assert_eq!(a.y(), 14.into());
        assert_eq!(a.z(), 26.into());
        a.set_length(((5 * 5 + 7 * 7 + 13 * 13) as f64).sqrt().into());
        assert_eq!(a.length(), ((5 * 5 + 7 * 7 + 13 * 13) as f64).sqrt().into());
        assert_eq!(a.x(), 5.into());
        assert_eq!(a.y(), 7.into());
        assert_eq!(a.z(), 13.into());

        // Set negative length
        a.set_length((((5 * 5 + 7 * 7 + 13 * 13) as f64).sqrt() * -1.0).into());
        assert_eq!(a.length(), ((5 * 5 + 7 * 7 + 13 * 13) as f64).sqrt().into());
        assert_eq!(a.x(), (-5).into());
        assert_eq!(a.y(), (-7).into());
        assert_eq!(a.z(), (-13).into());
    }

    #[test]
    fn test_limited() {
        // Pos limit
        let mut a = VectorLim::new(Vector::new(11.into(), 12.into(), 13.into()));
        let len: PhyType = ((11 * 11 + 12 * 12 + 13 * 13) as f64).sqrt().into();
        assert!(len > 20.into() && len < 21.into());
        assert_eq!(a.length(), len);
        a.set_limits((-9).into(), 9.into());
        assert_eq!(a.length(), len);
        a.set_length(11.into());
        assert_eq!(a.length(), 11.into());
        assert_eq!(a.limited().length(), 9.into());
        assert_eq!(a.length(), 11.into());
        a.apply_limits();
        assert_eq!(a.length(), 9.into());

        // Neg limit
        let mut a = VectorLim::new(Vector::new(11.into(), 12.into(), 13.into()));
        let len: PhyType = ((11 * 11 + 12 * 12 + 13 * 13) as f64).sqrt().into();
        assert!(len > 20.into() && len < 21.into());
        assert_eq!(a.length(), len);
        a.set_limits(30.into(), 40.into());
        assert_eq!(a.length(), len);
        a.set_length(25.into());
        assert_eq!(a.length(), 25.into());
        assert_eq!(a.limited().length(), 30.into());
        assert_eq!(a.length(), 25.into());
        a.apply_limits();
        assert_eq!(a.length(), 30.into());
    }

    #[test]
    fn test_clipped() {
        // Pos clipping
        let mut a = VectorLim::new(Vector::new(10.into(), 20.into(), 30.into()));
        a.set_clipping(Axis::X, 5.into(), 9.into());
        a.set_clipping(Axis::Y, 5.into(), 19.into());
        a.set_clipping(Axis::Z, 5.into(), 29.into());
        assert_eq!(a.x(), 10.into());
        assert_eq!(a.y(), 20.into());
        assert_eq!(a.z(), 30.into());
        let b = a.limited();
        assert_eq!(b.x(), 9.into());
        assert_eq!(b.y(), 19.into());
        assert_eq!(b.z(), 29.into());
        assert_eq!(a.x(), 10.into());
        assert_eq!(a.y(), 20.into());
        assert_eq!(a.z(), 30.into());
        a.apply_limits();
        assert_eq!(b.x(), 9.into());
        assert_eq!(b.y(), 19.into());
        assert_eq!(b.z(), 29.into());

        // Neg clipping
        let mut a = VectorLim::new(Vector::new((-10).into(), (-20).into(), (-30).into()));
        a.set_clipping(Axis::X, (-9).into(), (-5).into());
        a.set_clipping(Axis::Y, (-19).into(), (-5).into());
        a.set_clipping(Axis::Z, (-29).into(), (-5).into());
        assert_eq!(a.x(), (-10).into());
        assert_eq!(a.y(), (-20).into());
        assert_eq!(a.z(), (-30).into());
        let b = a.limited();
        assert_eq!(b.x(), (-9).into());
        assert_eq!(b.y(), (-19).into());
        assert_eq!(b.z(), (-29).into());
        assert_eq!(a.x(), (-10).into());
        assert_eq!(a.y(), (-20).into());
        assert_eq!(a.z(), (-30).into());
        a.apply_limits();
        assert_eq!(b.x(), (-9).into());
        assert_eq!(b.y(), (-19).into());
        assert_eq!(b.z(), (-29).into());
    }

    #[test]
    fn test_arithmetic() {
        // Add
        let a = Vector::new(1.into(), 2.into(), 3.into());
        let b = Vector::new(10.into(), 20.into(), 30.into());
        let c = a + b;
        assert_eq!(c.x(), 11.into());
        assert_eq!(c.y(), 22.into());
        assert_eq!(c.z(), 33.into());

        // Add assign
        let mut a = Vector::new(1.into(), 2.into(), 3.into());
        let b = Vector::new(10.into(), 20.into(), 30.into());
        a += b;
        assert_eq!(a.x(), 11.into());
        assert_eq!(a.y(), 22.into());
        assert_eq!(a.z(), 33.into());

        // Sub
        let a = Vector::new(1.into(), 2.into(), 3.into());
        let b = Vector::new(10.into(), 20.into(), 30.into());
        let c = a - b;
        assert_eq!(c.x(), (-9).into());
        assert_eq!(c.y(), (-18).into());
        assert_eq!(c.z(), (-27).into());

        // Sub assign
        let mut a = Vector::new(1.into(), 2.into(), 3.into());
        let b = Vector::new(10.into(), 20.into(), 30.into());
        a -= b;
        assert_eq!(a.x(), (-9).into());
        assert_eq!(a.y(), (-18).into());
        assert_eq!(a.z(), (-27).into());

        // Neg
        let a = Vector::new(1.into(), 2.into(), 3.into());
        let b = -a;
        assert_eq!(b.x(), (-1).into());
        assert_eq!(b.y(), (-2).into());
        assert_eq!(b.z(), (-3).into());
        let a = Vector::new((-1).into(), (-2).into(), (-3).into());
        let b = -a;
        assert_eq!(b.x(), 1.into());
        assert_eq!(b.y(), 2.into());
        assert_eq!(b.z(), 3.into());

        // Mul
        let a = Vector::new(1.into(), 2.into(), 3.into());
        let b = a * 10.into();
        assert_eq!(b.x(), 10.into());
        assert_eq!(b.y(), 20.into());
        assert_eq!(b.z(), 30.into());
        let a = Vector::new(1.into(), 2.into(), 3.into());
        let b = a * (-10).into();
        assert_eq!(b.x(), (-10).into());
        assert_eq!(b.y(), (-20).into());
        assert_eq!(b.z(), (-30).into());

        // Mul assign
        let mut a = Vector::new(1.into(), 2.into(), 3.into());
        a *= 10.into();
        assert_eq!(a.x(), 10.into());
        assert_eq!(a.y(), 20.into());
        assert_eq!(a.z(), 30.into());
        let mut a = Vector::new(1.into(), 2.into(), 3.into());
        a *= (-10).into();
        assert_eq!(a.x(), (-10).into());
        assert_eq!(a.y(), (-20).into());
        assert_eq!(a.z(), (-30).into());
    }
}

// vim: ts=4 sw=4 expandtab