sp2 0.2.0

use super::Movement;

use ga2::{Rotor, Vector};
use num_traits::{real::Real, Inv, One, Zero};
use vector_space::{interpolate, DotProduct as _};

use std::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Sub, SubAssign};

#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub struct Transform<T> {
    pub translation: Vector<T>,
    pub orientation: Rotor<T>,
}

impl<T: Real> Transform<T> {
    pub fn lerp(&self, other: &Self, ratio: T) -> Self {
        Self {
            translation: interpolate(self.translation, other.translation, ratio),
            orientation: interpolate(self.orientation, other.orientation, ratio),
        }
    }

    pub fn apply(&self, other: Vector<T>) -> Vector<T> {
        self.translation + self.orientation.dot(other)
    }
}

impl<T: Real> One for Transform<T> {
    fn one() -> Self {
        Self {
            translation: Vector::zero(),
            orientation: Rotor::one(),
        }
    }

    fn is_one(&self) -> bool {
        self.translation.is_zero() && self.orientation.is_one()
    }
}

impl<T: Real> Mul<T> for Transform<T> {
    type Output = Self;

    fn mul(self, other: T) -> Self {
        Self {
            translation: self.translation * other,
            orientation: self.orientation * other,
        }
    }
}

impl<T: Real> Div<T> for Transform<T> {
    type Output = Self;

    fn div(self, other: T) -> Self {
        Self {
            translation: self.translation / other,
            orientation: self.orientation / other,
        }
    }
}

impl<T: Real + MulAssign> MulAssign<T> for Transform<T> {
    fn mul_assign(&mut self, other: T) {
        self.translation *= other;
        self.orientation *= other;
    }
}

impl<T: Real + DivAssign> DivAssign<T> for Transform<T> {
    fn div_assign(&mut self, other: T) {
        self.translation /= other;
        self.orientation /= other;
    }
}

impl<T: Real> Mul for Transform<T> {
    type Output = Self;

    fn mul(self, other: Self) -> Self {
        Self {
            translation: self.translation + self.orientation.dot(other.translation),
            orientation: self.orientation * other.orientation,
        }
    }
}

impl<T: Real> Div for Transform<T> {
    type Output = Self;

    fn div(self, other: Self) -> Self {
        self * other.inv()
    }
}

impl<T: Real> Inv for Transform<T> {
    type Output = Self;

    fn inv(self) -> Self {
        let orientation = self.orientation.inv();
        Self {
            translation: orientation.dot(-self.translation),
            orientation,
        }
    }
}

impl<T: Real + AddAssign + MulAssign> MulAssign for Transform<T> {
    fn mul_assign(&mut self, other: Self) {
        self.translation += self.orientation.dot(other.translation);
        self.orientation *= other.orientation;
    }
}

impl<T: Real + SubAssign + DivAssign> DivAssign for Transform<T> {
    fn div_assign(&mut self, other: Self) {
        self.orientation /= other.orientation;
        self.translation -= self.orientation.dot(other.translation);
    }
}

impl<T: Real> Add<Vector<T>> for Transform<T> {
    type Output = Self;

    fn add(self, other: Vector<T>) -> Self {
        Self {
            translation: self.translation + other,
            ..self
        }
    }
}

impl<T: Real> Sub<Vector<T>> for Transform<T> {
    type Output = Self;

    fn sub(self, other: Vector<T>) -> Self {
        Self {
            translation: self.translation - other,
            ..self
        }
    }
}

impl<T: Real + AddAssign> AddAssign<Vector<T>> for Transform<T> {
    fn add_assign(&mut self, other: Vector<T>) {
        self.translation += other;
    }
}

impl<T: Real + SubAssign> SubAssign<Vector<T>> for Transform<T> {
    fn sub_assign(&mut self, other: Vector<T>) {
        self.translation -= other;
    }
}

impl<T: Real> Mul<Rotor<T>> for Transform<T> {
    type Output = Self;

    fn mul(self, other: Rotor<T>) -> Self {
        Self {
            orientation: self.orientation * other,
            ..self
        }
    }
}

impl<T: Real> Div<Rotor<T>> for Transform<T> {
    type Output = Self;

    fn div(self, other: Rotor<T>) -> Self {
        Self {
            orientation: self.orientation / other,
            ..self
        }
    }
}

impl<T: Real + MulAssign> MulAssign<Rotor<T>> for Transform<T> {
    fn mul_assign(&mut self, other: Rotor<T>) {
        self.orientation *= other;
    }
}

impl<T: Real + DivAssign> DivAssign<Rotor<T>> for Transform<T> {
    fn div_assign(&mut self, other: Rotor<T>) {
        self.orientation /= other;
    }
}

impl<T: Real> Add<Movement<T>> for Transform<T> {
    type Output = Self;

    fn add(self, other: Movement<T>) -> Self {
        Self {
            translation: self.translation + other.velocity,
            orientation: self.orientation * other.rotation.exp(),
        }
    }
}

impl<T: Real> Sub<Movement<T>> for Transform<T> {
    type Output = Self;

    fn sub(self, other: Movement<T>) -> Self {
        Self {
            translation: self.translation - other.velocity,
            orientation: self.orientation / other.rotation.exp(),
        }
    }
}

impl<T: Real + AddAssign + MulAssign> AddAssign<Movement<T>> for Transform<T> {
    fn add_assign(&mut self, other: Movement<T>) {
        self.translation += other.velocity;
        self.orientation *= other.rotation.exp();
    }
}

impl<T: Real + SubAssign + DivAssign> SubAssign<Movement<T>> for Transform<T> {
    fn sub_assign(&mut self, other: Movement<T>) {
        self.translation -= other.velocity;
        self.orientation /= other.rotation.exp();
    }
}