use na::*;
use angle::*;
use util::ValueCache;
use std::ops::Mul;
use alga::general::{Identity, Multiplicative};
use std::fmt::{self, Debug};
//TODO: implement
//use alga::linear::{Similarity, Isometry, Scaling, Translation, Rotation, AffineTransformation, EuclideanSpace, ProjectiveTransformation};

bitflags!{
    #[cfg_attr(feature = "serialize", derive(Serialize, Deserialize))]
    pub struct Flags: u16{
        const NO_ROTATION_FROM_PARENT    = 1 << 0;
        const NO_SCALE_FROM_PARENT       = 1 << 1;
        const NO_TRANSLATION_FROM_PARENT = 1 << 2;
    }
}

/// Position + Orientation + Scale of an object
///
/// Represents a model matrix decomposed into it's components
/// and caches this values so the matrix only is recalculated
/// when any of the original values changes
///
/// Also calculates a global matrix from an optional parent
///
/// The update of the matrices happens whenever any of the update_*
/// functions is called so don't forget to call one of them before using
/// the matrices after a change
///
/// A node that has just been created contains the correct updated matrices
#[derive(Clone, Copy)]
#[cfg_attr(feature = "serialize", derive(Serialize, Deserialize))]
pub struct Node{
    position: ValueCache<Pnt3>,
    orientation: ValueCache<UnitQuat>,
    scale: ValueCache<Vec3>,
    local_transformation: ValueCache<Mat4>,
    global_transformation: ValueCache<Mat4>,
    global_scale: ValueCache<Vec3>,
    parent_inv: Option<Mat4>,
}

fn trafo_from_parts(pos: &Pnt3, orientation: &UnitQuat, scale: &Vec3) -> Mat4{
    let scale = Mat3::from_diagonal(scale);
    let rot = orientation.to_rotation_matrix();
    let mat = rot * scale;
    Mat4::from_columns(&[
        vec4!(mat.column(0), 0.),
        vec4!(mat.column(1), 0.),
        vec4!(mat.column(2), 0.),
        vec4!(pos.to_vec(), 1.),
    ])
}

impl Debug for Node{
    fn fmt(&self, fmt: &mut fmt::Formatter) -> fmt::Result{
        fmt.debug_struct("Node")
            .field("position", &*self.position)
            .field("orientation", &*self.orientation)
            .field("scale", &*self.scale)
            .field("parent_inv", &self.parent_inv.is_some())
            .finish()
    }
}

impl Node{
    pub fn new(pos: Pnt3, orientation: UnitQuat, scale: Vec3) -> Node{
        let local_trafo = trafo_from_parts(&pos, &orientation, &scale);
        Node{
            position: ValueCache::new(pos),
            orientation: ValueCache::new(orientation),
            scale: ValueCache::new(scale),
            local_transformation: ValueCache::new(local_trafo),
            global_transformation: ValueCache::new(local_trafo),
            global_scale: ValueCache::new(scale),
            parent_inv: None,
        }
    }

    pub fn with_parent(parent: &Node, pos: Pnt3, orientation: UnitQuat, scale: Vec3) -> Node{
        let mut node = Node::new(pos, orientation, scale);
        node.update_with_parent(Some(parent));
        node
    }

    /// A node that applies no transformation
    pub fn identity() -> Node{
        Node{
            position: ValueCache::new(origin()),
            orientation: ValueCache::new(one()),
            scale: ValueCache::new(vec3!(1.)),
            local_transformation: ValueCache::new(one()),
            global_transformation: ValueCache::new(one()),
            global_scale: ValueCache::new(vec3!(1.)),
            parent_inv: None,
        }
    }

    /// A node that applies no transformation but the global transformation
    /// contains the parent's
    pub fn identity_with_parent(parent: &Node) -> Node{
        let mut node = Node::identity();
        *node.global_scale = parent.global_scale();
        *node.global_transformation = parent.global_transformation();
        node
    }

    /// A node that orients an object to look at a certain target
    ///
    /// - eye: position of the object
    /// - at: where the object will be looking at
    /// - up: up vector
    pub fn new_look_at(eye: Pnt3, at: Pnt3, up: Vec3) -> Node{
        let rh_dir = eye - at;
        let orientation = UnitQuat::new_observer_frame(&rh_dir, &up);
        let local_transformation = Isometry3::from_parts(
                Translation::from(eye.to_vec()),
                orientation
            ).to_homogeneous();
        Node{
            position: ValueCache::new(eye),
            orientation: ValueCache::new(orientation),
            scale: ValueCache::new(vec3!(1.)),
            local_transformation: ValueCache::new(local_transformation),
            global_transformation: ValueCache::new(local_transformation),
            global_scale: ValueCache::new(vec3!(1.)),
            parent_inv: None,
        }
    }

    /// New node that stores the inverse of the current parent
    ///
    /// When parenting a node at a certain position, orientation and scale allows for those
    /// to be absolute instead of relative to the parent.
    ///
    /// This version of the method takes a Mat4 as preparent inverse global transformation
    pub fn with_preparent(parent_inv: Mat4, pos: Pnt3, orientation: UnitQuat, scale: Vec3) -> Node{
        let mut node = Node::new(pos, orientation, scale);
        node.parent_inv = Some(parent_inv);
        *node.global_transformation = parent_inv * *node.global_transformation;
        node
    }

    /// New node that stores the inverse of the current parent
    ///
    /// When parenting a node at a certain position, orientation and scale allows for those
    /// to be absolute instead of relative to the parent.
    ///
    /// This version of the method takes another node as preparent global transformation
    pub fn with_preparent_node(preparent: &Node, pos: Pnt3, orientation: UnitQuat, scale: Vec3) -> Node{
        Node::with_preparent(preparent.inv_global_transformation(), pos, orientation, scale)
    }

    /// New node that stores the inverse of the current parent and applies no transformation itself
    ///
    /// When parenting a node at a certain position, orientation and scale allows for those
    /// to be absolute instead of relative to the parent.
    ///
    /// This version of the method takes another node as preparent global transformation
    pub fn identity_with_preparent(parent_inv: Mat4) -> Node{
        let mut node = Node::identity();
        node.parent_inv = Some(parent_inv);
        *node.global_transformation = parent_inv;
        node
    }

    /// New node that stores the inverse of the current parent and applies no transformation itself
    ///
    /// When parenting a node at a certain position, orientation and scale allows for those
    /// to be absolute instead of relative to the parent.
    ///
    /// This version of the method takes a Mat4 as preparent inverse global transformation
    pub fn identity_with_preparent_node(preparent: &Node) -> Node{
        Node::identity_with_preparent(preparent.inv_global_transformation())
    }

    /// Clones this node but applies a preparent transformation
    ///
    /// Allows to parent this node with it's current position, orientation and scale as absolute
    /// instead of relative to the parent.
    pub fn clone_with_preparent(&self, preparent: &Node) -> Node{
        let mut node = self.clone();
        node.parent_inv = Some(preparent.inv_global_transformation());
        node
    }

    /// Sets the local position of this node
    pub fn set_position(&mut self, pos: Pnt3){
        *self.position = pos;
    }

    /// Returns the local position of this node
    pub fn position(&self) -> Pnt3{
        *self.position
    }

    /// Returns the initial parent inverse transformation if there's one
    pub fn preparent(&self) -> Option<Mat4>{
        self.parent_inv
    }

    /// Sets the orientation for this node from an angle and axis of rotation
    pub fn set_angle_axis(&mut self, angle: Rad<f32>, axis: &Unit<Vec3>){
        self.set_orientation(UnitQuat::from_axis_angle(axis, angle.value()));
    }

    /// Sets the orientation for this node from a quaternion
    pub fn set_orientation(&mut self, q: UnitQuat){
        *self.orientation = q;
    }

    /// Returns the orientation of this node as a quaternion which is how it's stored
    /// internally
    pub fn orientation(&self) -> UnitQuat{
        *self.orientation
    }

    /// Returns the orientation of this node as a Rotation3.
    ///
    /// This method converts the internal quaternion into a Rotation3
    pub fn rotation(&self) -> Rotation3<f32>{
        self.orientation.to_rotation_matrix()
    }

    /// Changes the orientation of the node to look at the passed position using the up vector
    pub fn look_at(&mut self, at: &Pnt3, up: &Vec3){
        let rh_dir = self.position() - at;
        let orientation = UnitQuat::new_observer_frame(&rh_dir, up);
        self.set_orientation(orientation);
    }


    /// Changes the orientation of the node to look at the passed node global position
    /// using the up vector
    pub fn look_at_node<N: NodeRef>(&mut self, node: &N, up: &Vec3){
        let at = node.global_position();
        self.look_at(&at, up);
    }

    /// Rotate this note a certain angle around the axis of rotation
    pub fn rotate(&mut self, angle: Rad<f32>, axis: &Unit<Vec3>){
        let angle = angle.to_rad().value();
        let orientation = UnitQuat::from_axis_angle(axis, angle) * *self.orientation;
        self.set_orientation(orientation);
    }

    /// Append the passed quaternion to the current local orientation
    pub fn append_orientation(&mut self, rot: &UnitQuat){
        let orientation = *rot * *self.orientation;
        self.set_orientation(orientation);
    }

    /// Append a translation to the current local position
    pub fn translate(&mut self, t: &Vec3){
        let position = *self.position + *t;
        self.set_position(position);
    }

    /// Returns the current local scale
    pub fn scale(&self) -> Vec3{
        *self.scale
    }

    /// Sets the local scale
    pub fn set_scale(&mut self, s: Vec3){
        *self.scale = s;
    }

    /// The node orientation x axis
    pub fn x_axis(&self) -> Unit<Vec3>{
        Unit::new_normalize(self.global_transformation().column(0).xyz())
    }

    /// The node orientation y axis
    pub fn y_axis(&self) -> Unit<Vec3>{
        Unit::new_normalize(self.global_transformation().column(1).xyz())
    }

    /// The node orientation z axis
    pub fn z_axis(&self) -> Unit<Vec3>{
        Unit::new_normalize(self.global_transformation().column(2).xyz())
    }

    /// rotate this node around it's local x axis
    pub fn tilt(&mut self, angle: Rad<f32>){
        let x_axis = self.x_axis();
        self.rotate(angle, &x_axis);
    }

    /// rotate this node around it's local y axis
    pub fn pan(&mut self, angle: Rad<f32>){
        let y_axis = self.y_axis();
        self.rotate(angle, &y_axis);
    }

    /// rotate this node around it's local z axis
    pub fn roll(&mut self, angle: Rad<f32>){
        let z_axis = self.z_axis();
        self.rotate(angle, &z_axis);
    }

    /// Local transformation matrix
    pub fn local_transformation(&self) -> Mat4{
        *self.local_transformation
    }

    /// Global transformation matrix
    pub fn global_transformation(&self) -> Mat4{
        *self.global_transformation
    }

    /// Global position of this node
    pub fn global_position(&self) -> Pnt3{
        self.global_transformation().column(3).xyz().to_pnt()
    }

    /// Global orientation of this node
    pub fn global_orientation(&self) -> UnitQuat{
        let rot = Mat3::from_columns(&[
            normalize(&self.global_transformation().column(0).xyz()),
            normalize(&self.global_transformation().column(1).xyz()),
            normalize(&self.global_transformation().column(2).xyz()),
        ]);
        UnitQuat::from_rotation_matrix(&Rotation3::from_matrix_unchecked(rot))
    }

    /// Global scale of this node
    pub fn global_scale(&self) -> Vec3{
        *self.global_scale
    }

    /// Inverse local transformation of this node
    ///
    /// This is cacheed internally and uses the fastest version posible to calculate
    /// the inverse so it's usually faster than calling `local_transformation().try_inverse()`
    pub fn inv_local_transformation(&self) -> Mat4{
        if *self.scale == vec3!(1.){
            return self.local_transformation.fast_orthonormal_inverse();
        }

        let gs = self.global_scale();
        if gs.x == 0. || gs.y == 0. || gs.z == 0.{
            // Hack to make this work with scale zero
            // in which case the transformation os not affine and it'll fail
            // to unwrap, since the matrix will make the object dissapear anyway
            // we just return the transformation without any change
            *self.local_transformation
        }else{
            self.local_transformation.fast_affine_inverse().unwrap()
        }
    }

    /// Inverse global transformation of this node
    ///
    /// This is cacheed internally and uses the fastest version posible to calculate
    /// the inverse so it's usually faster than calling `global_transformation().try_inverse()`
    pub fn inv_global_transformation(&self) -> Mat4{
        let gs = self.global_scale();
        if gs.x == 0. || gs.y == 0. || gs.z == 0.{
            // Same hack as local inverse
            self.global_transformation()
        }else if let Some(_parent_inv) = self.parent_inv{
            // Do we need to check global scale * parent_inv?
            self.global_transformation().fast_affine_inverse().unwrap()
        }else{
            if *self.global_scale == vec3!(1.){
                self.global_transformation().fast_orthonormal_inverse()
            }else{
                self.global_transformation().fast_affine_inverse().unwrap()
            }
        }
    }

    /// Updates the internal matrices including the glonal matrices
    /// using the optional parent passed as argument
    pub fn update_with_parent(&mut self, parent: Option<&Node>) -> bool{
        self.update_with_parent_flags(parent, Flags::empty())
    }

    /// Updates the internal matrices including the glonal matrices
    /// using the optional parent passed as argument and flags
    pub fn update_with_parent_flags(&mut self, parent: Option<&Node>, flags: Flags) -> bool{
        let changed = self.position.has_changed() ||
                      self.orientation.has_changed() ||
                      self.scale.has_changed();

        if changed {
            *self.local_transformation = trafo_from_parts(&self.position(),
                                                          &self.orientation(),
                                                          &self.scale());
        }

        self.global_scale.update();
        self.global_transformation.update();
        let global_changed = self.local_transformation.has_changed() ||
                             parent.map(|parent| parent.global_transformation.has_changed())
                                .unwrap_or(false);
        if global_changed {
            if let Some(parent) = parent{
                let parent_trafo = if flags.is_empty() {
                    parent.global_transformation()
                }else{
                    let parent_scale = if flags.contains(Flags::NO_SCALE_FROM_PARENT) {
                        let parent_scale = parent.scale();
                        let parent_inv_scale = vec3(1. / parent_scale.x, 1./ parent_scale.y, 1. / parent.scale.z);
                        parent.global_scale().component_mul(&parent_inv_scale)
                    } else {
                        parent.global_scale()
                    };
                    let parent_orientation = if flags.contains(Flags::NO_ROTATION_FROM_PARENT) {
                        parent.global_orientation() * inverse(&parent.orientation())
                    } else {
                        parent.global_orientation()
                    };
                    let parent_position = if flags.contains(Flags::NO_TRANSLATION_FROM_PARENT) {
                        (parent.global_position() - parent.position()).to_pnt()
                    } else {
                        parent.global_position()
                    };
                    trafo_from_parts(&parent_position, &parent_orientation, &parent_scale)
                };
                let parent_trafo = if let Some(parent_inv) = self.parent_inv{
                    parent_trafo
                        .fast_mul(&parent_inv)
                }else{
                    parent_trafo
                };
                *self.global_transformation = parent_trafo
                        .fast_mul(&*self.local_transformation);
                let p = parent.global_scale();
                let s = *self.scale;
                *self.global_scale = vec3(p.x * s.x, p.y * s.y, p.z * s.z);
            }else{
                *self.global_transformation = self.local_transformation();
                *self.global_scale = *self.scale;
            }
        }

        self.position.update();
        self.orientation.update();
        self.scale.update();
        self.local_transformation.update();

        changed || global_changed
    }

}

/// Trait to implement by objects that conatain a node and want to expose it's same api
///
/// `NodeRef` only exposes the non mutable api for a node
pub trait NodeRef{
    /// Returns this object's Node
    fn node(&self) -> &Node;

    /// Returns the local position of this node
    fn position(&self) -> Pnt3{
        self.node().position()
    }

    /// Returns the orientation of this node as a quaternion which is how it's stored
    /// internally
    fn orientation(&self) -> UnitQuat{
        self.node().orientation()
    }

    /// Returns the orientation of this node as a Rotation3.
    ///
    /// This method converts the internal quaternion into a Rotation3
    fn rotation(&self) -> Rotation3<f32>{
        self.node().rotation()
    }

    /// Returns the current local scale
    fn scale(&self) -> Vec3{
        self.node().scale()
    }

    /// The node orientation x axis
    fn x_axis(&self) -> Unit<Vec3>{
        self.node().x_axis()
    }

    /// The node orientation y axis
    fn y_axis(&self) -> Unit<Vec3>{
        self.node().y_axis()
    }

    /// The node orientation z axis
    fn z_axis(&self) -> Unit<Vec3>{
        self.node().z_axis()
    }

    /// Local transformation matrix
    fn local_transformation(&self) -> Mat4{
        self.node().local_transformation()
    }

    /// Global transformation matrix
    fn global_transformation(&self) -> Mat4{
        self.node().global_transformation()
    }

    /// Global position of this node
    fn global_position(&self) -> Pnt3{
        self.node().global_position()
    }

    /// Global orientation of this node
    fn global_orientation(&self) -> UnitQuat{
        self.node().global_orientation()
    }

    /// Global scale of this node
    fn global_scale(&self) -> Vec3{
        self.node().global_scale()
    }

    /// Inverse local transformation of this node
    ///
    /// This is cacheed internally and uses the fastest version posible to calculate
    /// the inverse so it's usually faster than calling `local_transformation().try_inverse()`
    fn inv_local_transformation(&self) -> Mat4{
        self.node().inv_local_transformation()
    }

    /// Inverse global transformation of this node
    ///
    /// This is cacheed internally and uses the fastest version posible to calculate
    /// the inverse so it's usually faster than calling `global_transformation().try_inverse()`
    fn inv_global_transformation(&self) -> Mat4{
        self.node().inv_global_transformation()
    }
}

/// Trait to implement by objects that conatain a node and want to expose it's same api
///
/// `NodeMut` only exposes the mutable api for a node
pub trait NodeMut{
    fn node_mut(&mut self) -> &mut Node;

    /// Updates the internal matrices including the glonal matrices
    /// using the optional parent passed as argument and flags
    fn update_with_parent_flags(&mut self, parent: Option<&Node>, flags: Flags) -> bool;

    /// Updates the internal matrices including the glonal matrices
    /// using the optional parent passed as argument
    fn update_with_parent(&mut self, parent: Option<&Node>) -> bool{
        self.update_with_parent_flags(parent, Flags::empty())
    }

    /// Changes the orientation of the node to look at the passed position using the up vector
    fn look_at(&mut self, at: &Pnt3, up: &Vec3){
        self.node_mut().look_at(at, up);
    }

    /// Rotate this note a certain angle around the axis of rotation
    fn rotate(&mut self, angle: Rad<f32>, axis: &Unit<Vec3>){
        self.node_mut().rotate(angle, axis);
    }

    /// Append the passed quaternion to the current local orientation
    fn append_orientation(&mut self, rot: &UnitQuat){
        self.node_mut().append_orientation(rot);
    }

    /// rotate this node around it's local x axis
    fn tilt(&mut self, angle: Rad<f32>){
        self.node_mut().tilt(angle);
    }

    /// rotate this node around it's local y axis
    fn pan(&mut self, angle: Rad<f32>){
        self.node_mut().pan(angle);
    }

    /// rotate this node around it's local z axis
    fn roll(&mut self, angle: Rad<f32>){
        self.node_mut().roll(angle);
    }

    /// Sets the local scale
    fn set_scale(&mut self, s: Vec3){
        self.node_mut().set_scale(s);
    }

    /// Append a translation to the current local position
    fn translate(&mut self, t: &Vec3){
        self.node_mut().translate(t);
    }

    /// Sets the local position of this node
    fn set_position(&mut self, pos: Pnt3){
        self.node_mut().set_position(pos);
    }

    /// Sets the orientation for this node from an angle and axis of rotation
    fn set_angle_axis(&mut self, angle: Rad<f32>, axis: &Unit<Vec3>){
        self.node_mut().set_angle_axis(angle, axis);
    }

    /// Sets the orientation for this node from a quaternion
    fn set_orientation(&mut self, q: UnitQuat){
        self.node_mut().set_orientation(q);
    }
}

impl NodeRef for Node{
    fn node(&self) -> &Node{
        self
    }
}

impl NodeMut for Node{
    fn node_mut(&mut self) -> &mut Node{
        self
    }

    fn update_with_parent(&mut self, parent: Option<&Node>) -> bool{
        self.update_with_parent(parent)
    }

    fn update_with_parent_flags(&mut self, parent: Option<&Node>, flags: Flags) -> bool{
        self.update_with_parent_flags(parent, flags)
    }
}

impl Mul<Node> for Node{
    type Output = Node;
    fn mul(self, right: Node) -> Node{
        let mut ret = right.clone();
        ret.update_with_parent(Some(&self));
        ret
    }
}

impl Identity<Multiplicative> for Node{
    fn identity() -> Node{
        Node::identity()
    }
}

impl From<Pnt3> for Node{
    fn from(pos: Pnt3) -> Node{
        Node::new(pos, one(), vec3!(1.))
    }
}

impl From<Translation3<f32>> for Node{
    fn from(t: Translation3<f32>) -> Node{
        Node::new(t.vector.to_pnt(), one(), vec3!(1.))
    }
}

impl From<Rotation3<f32>> for Node{
    fn from(r: Rotation3<f32>) -> Node{
        Node::new(origin(), UnitQuat::from_rotation_matrix(&r), vec3!(1.))
    }
}

impl From<Isometry3<f32>> for Node{
    fn from(i: Isometry3<f32>) -> Node{
        Node::new(i.translation.vector.to_pnt(), i.rotation, vec3!(1.))
    }
}

impl From<UnitQuaternion<f32>> for Node{
    fn from(q: UnitQuaternion<f32>) -> Node{
        Node::new(origin(), q, vec3!(1.))
    }
}