ga3 0.3.4

use crate::*;

use std::{
    fmt::{self, Display, Formatter},
    ops::{Div, DivAssign, Mul, MulAssign},
    str::FromStr,
};

use derive_more::{Add, AddAssign, Neg, Sub, SubAssign};
use inner_space::{DotProduct, InnerSpace, VectorSpace};
use scalars::{Inv, One, Real, Zero};
use vector_space::Transform;

/// The 3D rotor type.
#[derive(Copy, Clone, Debug, PartialEq, Eq, Add, AddAssign, Sub, SubAssign, Neg)]
pub struct Rotor<T> {
    /// The scalar component.
    pub scalar: T,
    /// The component representing the xy plane.
    pub xy: T,
    /// The component representing the xz plane.
    pub xz: T,
    /// The component representing the yz plane.
    pub yz: T,
}

impl<T> Rotor<T> {
    /// Creates a new rotor from its components.
    pub fn new(scalar: T, xy: T, xz: T, yz: T) -> Self {
        Self { scalar, xy, xz, yz }
    }
}

impl<T: Real> Rotor<T> {
    /// Rotates a vector by the rotor.
    pub fn rotate(self, vector: Vector<T>) -> Vector<T> {
        self.normalize().rotate_normalized(vector)
    }

    /// Rotates a vector by the rotor using the sandwich product R v R†.
    /// The rotor has to be normalized already.
    pub fn rotate_normalized(self, v: Vector<T>) -> Vector<T> {
        let s = self.scalar;
        let a = self.xy;
        let b = self.xz;
        let c = self.yz;

        let two = T::one() + T::one();

        let s2 = s * s;
        let a2 = a * a;
        let b2 = b * b;
        let c2 = c * c;

        Vector {
            x: (s2 + c2 - a2 - b2) * v.x
                + two * (s * a - b * c) * v.y
                + two * (s * b + a * c) * v.z,
            y: -two * (s * a + b * c) * v.x
                + (s2 + b2 - a2 - c2) * v.y
                + two * (s * c - a * b) * v.z,
            z: two * (a * c - s * b) * v.x - two * (a * b + s * c) * v.y
                + (s2 + a2 - b2 - c2) * v.z,
        }
    }

    /// Rotates a bivector by the rotor.
    pub fn rotate_bivector(self, bivector: Bivector<T>) -> Bivector<T> {
        self.normalize().rotate_bivector_normalized(bivector)
    }

    /// Rotates a bivector by the rotor using the sandwich product R B R†.
    /// The rotor has to be normalized already.
    pub fn rotate_bivector_normalized(self, b: Bivector<T>) -> Bivector<T> {
        let s = self.scalar;
        let a = self.xy;
        let br = self.xz;
        let c = self.yz;

        let two = T::one() + T::one();

        let s2 = s * s;
        let a2 = a * a;
        let b2 = br * br;
        let c2 = c * c;

        Bivector {
            xy: two * (a * c - s * br) * b.yz
                + two * (a * br + s * c) * b.xz
                + (s2 + a2 - b2 - c2) * b.xy,
            xz: two * (s * a + br * c) * b.yz + (s2 + b2 - a2 - c2) * b.xz
                - two * (s * c - a * br) * b.xy,
            yz: (s2 + c2 - a2 - b2) * b.yz - two * (s * a - br * c) * b.xz
                + two * (s * br + a * c) * b.xy,
        }
    }

    /// The reverse of the rotor.
    pub fn reverse(self) -> Self {
        Self {
            scalar: self.scalar,
            xy: -self.xy,
            xz: -self.xz,
            yz: -self.yz,
        }
    }
}

impl<T: Zero> Rotor<T> {
    /// Creates a new scalar rotor.
    pub fn scalar(scalar: T) -> Self {
        Self {
            scalar,
            xy: T::zero(),
            xz: T::zero(),
            yz: T::zero(),
        }
    }

    /// Creates a new rotor along the xy plane.
    pub fn xy(xy: T) -> Self {
        Self {
            scalar: T::zero(),
            xy,
            xz: T::zero(),
            yz: T::zero(),
        }
    }

    /// Creates a new rotor along the xz plane.
    pub fn xz(xz: T) -> Self {
        Self {
            scalar: T::zero(),
            xy: T::zero(),
            xz,
            yz: T::zero(),
        }
    }

    /// Creates a new rotor along the yz plane.
    pub fn yz(yz: T) -> Self {
        Self {
            scalar: T::zero(),
            xy: T::zero(),
            xz: T::zero(),
            yz,
        }
    }
}

impl<T: Zero> Zero for Rotor<T> {
    fn zero() -> Self {
        Self {
            scalar: T::zero(),
            xy: T::zero(),
            xz: T::zero(),
            yz: T::zero(),
        }
    }

    fn is_zero(&self) -> bool {
        self.scalar.is_zero() && self.xy.is_zero() && self.xz.is_zero() && self.yz.is_zero()
    }
}

impl<T: Real> One for Rotor<T> {
    fn one() -> Self {
        Self {
            scalar: T::one(),
            xy: T::zero(),
            xz: T::zero(),
            yz: T::zero(),
        }
    }

    fn is_one(&self) -> bool {
        self.scalar.is_one() && self.xy.is_zero() && self.xz.is_zero() && self.yz.is_zero()
    }
}

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

    fn inv(self) -> Self {
        self.reverse() / self.magnitude2()
    }
}

impl<T: Real> From<T> for Rotor<T> {
    fn from(scalar: T) -> Self {
        Self::scalar(scalar)
    }
}

impl<T> Mul<T> for Rotor<T>
where
    T: Mul<Output = T> + Copy,
{
    type Output = Self;
    fn mul(self, other: T) -> Self {
        Self {
            scalar: self.scalar * other,
            xy: self.xy * other,
            xz: self.xz * other,
            yz: self.yz * other,
        }
    }
}

impl<T> MulAssign<T> for Rotor<T>
where
    T: MulAssign + Copy,
{
    fn mul_assign(&mut self, other: T) {
        self.scalar *= other;
        self.xy *= other;
        self.xz *= other;
        self.yz *= other;
    }
}

impl<T> Div<T> for Rotor<T>
where
    T: Div<Output = T> + Copy,
{
    type Output = Self;
    fn div(self, other: T) -> Self {
        Self {
            scalar: self.scalar / other,
            xy: self.xy / other,
            xz: self.xz / other,
            yz: self.yz / other,
        }
    }
}

impl<T> DivAssign<T> for Rotor<T>
where
    T: DivAssign + Copy,
{
    fn div_assign(&mut self, other: T) {
        self.scalar /= other;
        self.xy /= other;
        self.xz /= other;
        self.yz /= other;
    }
}

impl<T: Real> VectorSpace for Rotor<T> {
    type Scalar = T;
}

impl<T: Real> DotProduct for Rotor<T> {
    type Output = Self::Scalar;

    fn dot(&self, other: &Self) -> T {
        self.scalar * other.scalar - self.xy * other.xy - self.xz * other.xz - self.yz * other.yz
    }

    fn scalar(&self, other: &Self) -> T {
        self.scalar * other.scalar + self.xy * other.xy + self.xz * other.xz + self.yz * other.yz
    }
}

impl<T: Real> DotProduct<Vector<T>> for Rotor<T> {
    type Output = Vector<T>;
    #[inline]
    fn dot(&self, other: &Vector<T>) -> Vector<T> {
        other.dot(self)
    }
}

impl<T: Real> DotProduct<Bivector<T>> for Rotor<T> {
    type Output = Self;
    #[inline]
    fn dot(&self, other: &Bivector<T>) -> Self {
        other.dot(self)
    }
}

impl<T: Real> Mul for Rotor<T> {
    type Output = Self;
    fn mul(self, other: Self) -> Self {
        Self {
            scalar: self.scalar * other.scalar
                - self.xy * other.xy
                - self.xz * other.xz
                - self.yz * other.yz,
            xy: self.scalar * other.xy + other.scalar * self.xy + self.xz * other.yz
                - self.yz * other.xz,
            xz: self.scalar * other.xz + other.scalar * self.xz - self.xy * other.yz
                + self.yz * other.xy,
            yz: self.scalar * other.yz + other.scalar * self.yz + self.xy * other.xz
                - self.xz * other.xy,
        }
    }
}

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

impl<T: Real> Div for Rotor<T> {
    type Output = Self;
    fn div(self, other: Self) -> Self {
        self * other.inv()
    }
}

impl<T: Real> DivAssign for Rotor<T> {
    fn div_assign(&mut self, other: Self) {
        *self = *self / other;
    }
}

impl<T: Real> Transform<Vector<T>> for Rotor<T> {
    fn apply_point(&self, point: Vector<T>) -> Vector<T> {
        self.rotate(point)
    }
}

impl<T: Real> Transform<Bivector<T>> for Rotor<T> {
    fn apply_point(&self, bivector: Bivector<T>) -> Bivector<T> {
        self.rotate_bivector(bivector)
    }
}

impl<T: Display> Display for Rotor<T> {
    fn fmt(&self, formatter: &mut Formatter<'_>) -> fmt::Result {
        write!(
            formatter,
            "({}, {}, {}, {})",
            self.scalar, self.xy, self.xz, self.yz
        )
    }
}

impl<T: FromStr> FromStr for Rotor<T> {
    type Err = ParseRotorError;

    fn from_str(source: &str) -> Result<Self, Self::Err> {
        let trimmed = source.trim();
        let inner = trimmed
            .strip_prefix('(')
            .and_then(|s| s.strip_suffix(')'))
            .ok_or(ParseRotorError)?;
        let parts: Vec<&str> = inner.split(',').collect();
        if parts.len() != 4 {
            return Err(ParseRotorError);
        }
        let scalar = parts[0].trim().parse().map_err(|_| ParseRotorError)?;
        let xy = parts[1].trim().parse().map_err(|_| ParseRotorError)?;
        let xz = parts[2].trim().parse().map_err(|_| ParseRotorError)?;
        let yz = parts[3].trim().parse().map_err(|_| ParseRotorError)?;
        Ok(Self { scalar, xy, xz, yz })
    }
}

/// Error returned when parsing a rotor from a string fails.
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct ParseRotorError;

impl Display for ParseRotorError {
    fn fmt(&self, formatter: &mut Formatter<'_>) -> fmt::Result {
        write!(formatter, "invalid rotor format")
    }
}

impl std::error::Error for ParseRotorError {}