use super::{Float, Mat3, Mat4, Vec3, Vec4};
use std::ops::{Mul, MulAssign};

#[derive(PartialEq, Debug)]
pub struct Affine3<T: Float> {
    matrix: Mat4<T>,
}

impl<T: Float> Affine3<T> {
    pub fn translation(delta: Vec3<T>) -> Self {
        #[rustfmt::skip]
        let matrix = Mat4::new(T::one(),  T::zero(), T::zero(), delta.x(),
                               T::zero(), T::one() , T::zero(), delta.y(),
                               T::zero(), T::zero(), T::one(),  delta.z(),
                               T::zero(), T::zero(), T::zero(), T::one());
        Self { matrix }
    }

    pub fn rotation(axis: Vec3<T>, angle: T) -> Self {
        let x = axis.x();
        let y = axis.y();
        let z = axis.z();
        let cos = angle.cos();
        let ncos = T::one() - cos;
        let sin = angle.sin();
        #[rustfmt::skip]
        let matrix = Mat4::new(x*x*ncos+cos,   y*x*ncos-z*sin, z*x*ncos+y*sin, T::zero(),
                               x*y*ncos+z*sin, y*y*ncos+cos,   z*y*ncos-x*sin, T::zero(),
                               x*z*ncos-y*sin, y*z*ncos+x*sin, z*z*ncos+cos,   T::zero(),
                               T::zero(),      T::zero(),      T::zero(),      T::one());
        Self { matrix }
    }

    pub fn scale(s: T) -> Self {
        #[rustfmt::skip]
        let matrix = Mat4::new(s,         T::zero(), T::zero(), T::zero(),
                               T::zero(), s        , T::zero(), T::zero(),
                               T::zero(), T::zero(), s,         T::zero(),
                               T::zero(), T::zero(), T::zero(), T::one());
        Self { matrix }
    }

    pub fn get_element(&self, row: usize, column: usize) -> T {
        self.matrix.get_element(row, column)
    }

    pub fn get_row(&self, row: usize) -> Vec4<T> {
        self.matrix.get_row(row)
    }

    pub fn get_column(&self, column: usize) -> Vec4<T> {
        self.matrix.get_column(column)
    }

    pub fn linear_map(&self) -> Mat3<T> {
        Mat3::new(
            self.matrix.get_element(0, 0),
            self.matrix.get_element(0, 1),
            self.matrix.get_element(0, 2),
            self.matrix.get_element(1, 0),
            self.matrix.get_element(1, 1),
            self.matrix.get_element(1, 2),
            self.matrix.get_element(2, 0),
            self.matrix.get_element(2, 1),
            self.matrix.get_element(2, 2),
        )
    }

    pub fn inverse(&self) -> Affine3<T> {
        // linear map should always be invertable.
        let inner = self.linear_map().try_inverse().unwrap();
        let translation = inner * self.matrix.get_column(3).xyz();
        #[rustfmt::skip]
        let matrix = Mat4::new(
            inner.get_element(0,0), inner.get_element(0,1), inner.get_element(0,2), translation.x(),
            inner.get_element(1,0), inner.get_element(1,1), inner.get_element(1,2), translation.y(),
            inner.get_element(2,0), inner.get_element(2,1), inner.get_element(2,2), translation.z(),
            T::zero(),              T::zero(),              T::zero(),              T::one());
        Self { matrix }
    }
}

impl<T: Float> Mul<Affine3<T>> for Affine3<T> {
    type Output = Self;

    fn mul(self, rhs: Affine3<T>) -> Affine3<T> {
        let matrix = self.matrix * rhs.matrix;
        Affine3 { matrix }
    }
}

impl<T: Float> Mul<Mat4<T>> for Affine3<T> {
    type Output = Mat4<T>;

    fn mul(self, rhs: Mat4<T>) -> Mat4<T> {
        self.matrix * rhs
    }
}

impl<T: Float> Mul<Affine3<T>> for Mat4<T> {
    type Output = Mat4<T>;

    fn mul(self, rhs: Affine3<T>) -> Mat4<T> {
        self * rhs.matrix
    }
}

impl<T: Float> MulAssign<Affine3<T>> for Affine3<T> {
    fn mul_assign(&mut self, rhs: Affine3<T>) {
        self.matrix *= rhs.matrix
    }
}

impl<T: Float> Mul<Vec4<T>> for Affine3<T> {
    type Output = Vec4<T>;

    fn mul(self, rhs: Vec4<T>) -> Vec4<T> {
        self.matrix * rhs
    }
}

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

    #[test]
    fn translate_translates_vector() {
        let p = Vec4::new(1.0, 2.0, 3.0, 1.0);
        let v = Vec3::new(4.0, 5.0, 6.0);
        let target = Affine3::translation(v);
        let diff = (target * p).xyz() - (p.xyz() + v);
        assert!(diff.norm() < 0.0000000001);
    }

    #[test]
    fn rotate_rotates_vector() {
        let x = Vec4::new(1.0, 0.0, 0.0, 1.0);
        let y = Vec4::new(0.0, 1.0, 0.0, 1.0);
        let z = Vec4::new(0.0, 0.0, 1.0, 1.0);
        let target = Affine3::rotation(z.xyz(), std::f64::consts::PI/2.0) * y;
        let diff = -x.xyz() - target.xyz();
        assert!(diff.norm() < 0.0000000001);
    }

    #[test]
    fn linear_map_is_inner_matrix() {
        #[rustfmt::skip]
        let target = Affine3{
            matrix: Mat4::new(1.0,  2.0,  3.0,  4.0,
                              5.0,  6.0,  7.0,  8.0,
                              9.0, 10.0, 11.0, 12.0,
                              0.0,  0.0,  0.0,  1.0)};
        let linear_map = target.linear_map();
        for i in 0..2 {
            for j in 0..2 {
                assert!(linear_map.get_element(i, j) == target.get_element(i, j));
            }
        }
    }
}