use std::ops::{Add, Sub, Mul, Div, AddAssign, SubAssign, MulAssign, DivAssign};
use nalgebra::{
    SimdRealField, UnitQuaternion, Vector3, Quaternion, convert, RealField, zero, one, Point3,
    Matrix4, Matrix3, Rotation3,
};
use num_traits::{Zero, One};
#[cfg(feature="serde_derive")]
use serde_derive::{Serialize, Deserialize};

#[derive(Clone, Copy, Debug)]
#[cfg_attr(feature = "serde_derive", derive(Serialize, Deserialize))]
pub struct Dual<T> {
    pub r: T,
    pub d: T,
}

impl<T> Add for Dual<T>
where T: Add<Output = T>
{
    type Output = Dual<T>;
    fn add(self, rhs: Dual<T>) -> Dual<T> {
        Dual { r: self.r + rhs.r, d: self.d + rhs.d}
    }
}

impl<T> AddAssign for Dual<T>
where T: Add<Output = T> + Copy
{
    fn add_assign(&mut self, rhs: Dual<T>) {
        *self = *self + rhs
    }
}

impl<T> Sub for Dual<T>
where T: Sub<Output = T>
{
    type Output = Self;
    fn sub(self, other: Self) -> Self {
        Dual{ r: self.r - other.r, d: self.d - other.d }
    }
}

impl<T> SubAssign for Dual<T>
where T: Sub<Output = T> + Copy
{
    fn sub_assign(&mut self, rhs: Dual<T>) {
        *self = *self - rhs
    }
}

impl<T> Mul for Dual<T>
where T: Add<Output = T> + Mul<Output = T> + Copy
{
    type Output = Dual<T>;
    fn mul(self, rhs: Dual<T>) -> Dual<T> {
        Dual {
            r: self.r * rhs.r,
            d: self.r * rhs.d + rhs.r * self.d
        }
    }
}

impl<T> MulAssign for Dual<T>
where T: Add<Output = T> + Mul<Output = T> + Copy
{
    fn mul_assign(&mut self, rhs: Dual<T>) {
        *self = *self * rhs
    }
}

impl<T> Mul<T> for Dual<T>
where T: Mul<Output = T> + Copy
{
    type Output = Dual<T>;
    fn mul(self, rhs: T) -> Dual<T> {
        Dual {
            r: self.r * rhs,
            d: rhs * self.d
        }
    }
}

impl<T> MulAssign<T> for Dual<T>
where T: Mul<Output = T> + Copy
{
    fn mul_assign(&mut self, rhs: T) {
        *self = *self * rhs
    }
}

impl<T> Div for Dual<T>
where T: Sub<Output = T> + Mul<Output = T> + Div<Output = T> + Copy
{
    type Output = Dual<T>;
    fn div(self, rhs: Dual<T>) -> Dual<T>
    {
        Dual {
            r: self.r / rhs.r,
            d: (rhs.r * self.d - self.r * rhs.d) / (rhs.r * rhs.r)
        }
    }
}

impl<T> DivAssign for Dual<T>
where T: Sub<Output = T> + Mul<Output = T> + Div<Output = T> + Copy
{
    fn div_assign(&mut self, rhs: Dual<T>) {
        *self = *self / rhs
    }
}

impl<T> Div<T> for Dual<T>
where T: Mul<Output = T> + Div<Output = T> + Copy
{
    type Output = Dual<T>;
    fn div(self, rhs: T) -> Dual<T> {
        Dual {
            r: self.r / rhs,
            d: (rhs * self.d) / (rhs * rhs)
        }
    }
}

impl<T> DivAssign<T> for Dual<T>
where T: Mul<Output = T> + Div<Output = T> + Copy
{
    fn div_assign(&mut self, rhs: T) {
        *self = *self / rhs
    }
}

impl<T> Dual<T> {
    pub fn new(r: T, d: T) -> Dual<T> {
        Dual { r, d }
    }
}

impl<T> Dual<T>
where T: RealField
{
    pub fn sqrt(&self) -> Dual<T> {
        let r = self.r.sqrt();
        Dual {
            r,
            d: self.d * convert(0.5) / r,
        }
    }
}

pub type DualQuaternion<T> = Dual<Quaternion<T>>;

impl<T> DualQuaternion<T>
where
    T: SimdRealField + Mul<Output = T> + Div<Output = T> + Copy,
    T::Element: SimdRealField
{
    #[inline]
    pub fn rigid_transformation(q: UnitQuaternion<T>, t: Vector3<T>) -> DualQuaternion<T> {
        Dual { r: *q, d: Quaternion::from_imag(t).half() * *q }
    }

    #[inline]
    pub fn from_real(v: T) -> DualQuaternion<T> {
        Dual{ r: Quaternion::from_real(v), d: zero() }
    }

    #[inline]
    pub fn conjugate(&self) -> DualQuaternion<T> {
        Dual{ r: self.r.conjugate(), d: self.d.conjugate() }
    }

    #[inline]
    pub fn norm_squared(&self) -> Dual<T> {
        Dual { r: self.r.norm_squared(), d: convert::<_,T>(2.) * self.r.dot(&self.d) }
    }

    #[inline]
    pub fn norm(&self) -> Dual<T> {
        Dual { r: self.r.norm(), d: (convert::<_,T>(2.) * self.r.dot(&self.d)).simd_sqrt() }
    }

    #[inline]
    pub fn magnitude(&self) -> Dual<T> {
        self.norm()
    }

    // #[inline]
    // pub fn dot(&self, other: &Self) -> T {
    //     self.r.dot(&other.r)
    // }

    #[inline]
    pub fn normalize(&self) -> DualQuaternion<T> {
        let norm = self.r.norm();
        Dual { r: self.r / norm, d: self.d / norm }
    }

    #[inline]
    pub fn squared(&self) -> DualQuaternion<T> {
        *self * *self
    }

    // #[inline]
    // pub fn scale(self) -> T {
    //     T::one() / self.r.norm_squared()
    // }

    // #[inline]
    // pub fn ln(self) -> DualQuaternion<T> {
    //     let d = (self.r.conjugate() * self.d) * self.scale();
    //     Self::new(self.r.ln(), d)
    // }

    // #[inline]
    // pub fn log(self, base: Self) -> Self {
    //     self.ln() / base.ln()
    // }

    // #[inline]
    // pub fn exp(&self) -> DualQuaternion<T> {
    //     let r = self.r.exp();
    //     Self::new(r, r * self.d)
    // }

    // #[inline]
    // pub fn slerp(&self, other: &DualQuaternion<T>, t: T) -> DualQuaternion<T> {
    //     (*other * self.conjugate()).pow(t) * *self
    // }

    // pub fn inverse(&self) -> Option<DualQuaternion<T>> {
    //     let n = self.r.norm_squared();
    //     if n != convert(0.) {
    //         Some(self.conjugate() * (convert::<_,T>(1.) / n))
    //     }else{
    //         None
    //     }
    // }

    #[inline]
    pub fn rotation(&self) -> UnitQuaternion<T> {
        UnitQuaternion::new_normalize(self.r)
    }

    #[inline]
    pub fn translation(self) -> Vector3<T> {
        let two = convert::<_,T>(2.);
        let normalized = self.normalize();
        let vr = normalized.r.vector();
        let vd = normalized.d.vector();
        (vd * normalized.r.w - vr * normalized.d.w + vr.cross(&vd)) * two
    }

    #[inline]
    pub fn transform(&self, v: &Point3<T>) -> Point3<T> {
        let two = convert::<_,T>(2.);
        let normalized = self.normalize();

        let vr = normalized.r.vector();
        let vd = normalized.d.vector();
        let trans = (vd * normalized.r.w - vr * normalized.d.w + vr.cross(&vd)) * two;
        UnitQuaternion::new_unchecked(normalized.r) * v + trans
    }

    #[inline]
    pub fn transform_normal(&self, n: &Vector3<T>) -> Vector3<T>{
        UnitQuaternion::from_quaternion(self.r) * n
    }

    pub fn transform_position_and_normal(&self, p: &Point3<T>, n: &Vector3<T>) -> (Point3<T>, Vector3<T>)
    {
        let two = convert::<_,T>(2.);
        let normalized = self.normalize();

        let vr = normalized.r.vector();
        let vd = normalized.d.vector();
        let trans = (vd * normalized.r.w - vr * normalized.d.w + vr.cross(&vd)) * two;
        let rot = UnitQuaternion::new_unchecked(normalized.r);
        let pp = rot * p + trans;
        let np = rot * n;
        (pp, np)
    }
}

impl<N: SimdRealField> Zero for DualQuaternion<N>
where N::Element: SimdRealField
{
    #[inline]
    fn zero() -> Self {
        Self::new(Quaternion::zero(), Quaternion::zero())
    }

    #[inline]
    fn is_zero(&self) -> bool {
        self.r.is_zero() && self.d.is_zero()
    }
}

impl<N: SimdRealField> One for DualQuaternion<N>
where N::Element: SimdRealField
{
    #[inline]
    fn one() -> Self {
        Self::new(one(), zero())
    }
}

impl<N: SimdRealField> Mul<N> for DualQuaternion<N>
where N::Element: SimdRealField
{
    type Output = Self;

    #[inline]
    fn mul(self, other: N) -> Self {
        Dual{ r: self.r * other, d: self.d * other }
    }
}

impl<N: SimdRealField> MulAssign<N> for DualQuaternion<N>
where N::Element: SimdRealField
{
    #[inline]
    fn mul_assign(&mut self, other: N) {
        *self = *self * other
    }
}

// impl<T: SimdRealField> Div for DualQuaternion<T> {
//     type Output = DualQuaternion<T>;
//     fn div(self, rhs: DualQuaternion<T>) -> DualQuaternion<T>
//     {
//         Dual {
//             r: self.r.right_div(&rhs.r).unwrap(),
//             d: (rhs.r * self.d - self.r * rhs.d) * (rhs.r * rhs.r).try_inverse().unwrap(),
//         }
//     }
// }

impl<N: SimdRealField> Div<N> for DualQuaternion<N>
where N::Element: SimdRealField
{
    type Output = Self;

    #[inline]
    fn div(self, other: N) -> Self {
        Dual{ r: self.r / other, d: self.d / other }
    }
}

impl<N: SimdRealField> DivAssign<N> for DualQuaternion<N>
where N::Element: SimdRealField
{
    #[inline]
    fn div_assign(&mut self, other: N) {
        *self = *self / other
    }
}

impl<N: RealField> From<Matrix4<N>> for DualQuaternion<N> {
    fn from(matrix: Matrix4<N>) -> Self {
        let t = matrix.column(3).xyz();
        let rot = Matrix3::from_columns(&[
            matrix.column(0).xyz(),
            matrix.column(1).xyz(),
            matrix.column(2).xyz(),
        ]);
        let rot = Rotation3::from_matrix(&rot);
        let q = UnitQuaternion::from_rotation_matrix(&rot);
        DualQuaternion::rigid_transformation(q, t)
    }
}

#[test]
fn test() {
    use nalgebra::{Translation3, Isometry3};

    let v = Point3::new(1., 0., 0.);
    let q = DualQuaternion::rigid_transformation(
        UnitQuaternion::identity(),
        Vector3::new(2., 0., 0.)
    );
    let t = q.transform(&v);
    assert_eq!(t, Point3::new(3., 0., 0.));


    let v = Point3::new(1., 0., 0.);
    let q = DualQuaternion::rigid_transformation(
        UnitQuaternion::from_axis_angle(&Vector3::y_axis(), 90f64.to_radians()),
        Vector3::new(0., 0., 0.)
    );
    let t1 = q.transform(&v);
    let t2 = UnitQuaternion::from_axis_angle(&Vector3::y_axis(), 90f64.to_radians()) * v;
    assert_eq!(t1, t2);


    let v = Point3::new(1., 0., 0.);
    let q = DualQuaternion::rigid_transformation(
        UnitQuaternion::from_axis_angle(&Vector3::y_axis(), 90f64.to_radians()),
        Vector3::new(2., 0., 0.)
    );
    let t1 = q.transform(&v);
    let r = Isometry3::from_parts(
        Translation3::new(2., 0., 0.),
        UnitQuaternion::from_axis_angle(&Vector3::y_axis(), 90f64.to_radians())
    );
    let t2 = r.transform_point(&v);
    assert_eq!(t1, t2);
}