use rin_math::{
    Pnt3, Mat4, vec4, Vec3, Mat3, UnitQuat, vec3, one, Unit,
    Isometry3, Translation, Rotation3, Translation3, Rad, ToVec, ToPnt, Angle, FastInverse,
    One, IsParallel
};
use rin_util::{ValueCache, Error, Result};
use std::ops::Mul;
use std::fmt::{self, Debug};
#[cfg(any(feature = "serialize", feature="ecs"))]
use serde_derive::{Serialize, Deserialize};
#[cfg(feature="ecs")]
use rinecs::{system, WriteAndParent, Has, Tag, Not};

//TODO: implement
//use alga::linear::{Similarity, Isometry, Scaling, Translation, Rotation, AffineTransformation, EuclideanSpace, ProjectiveTransformation};

/// 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(any(feature = "serialize", feature="ecs"), derive(Serialize, Deserialize))]
#[cfg_attr(feature="ecs", derive(rinecs::HierarchicalComponent))]
#[cfg_attr(feature="ecs", changes)]
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>,
    #[cfg(feature="ecs")]
    has_changed: bool,
}

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 Default for Node{
    fn default() -> Node{
        Node::identity()
    }
}

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,
            #[cfg(feature="ecs")]
            has_changed: false,
        }
    }

    pub fn from_position(pos: Pnt3) -> Node{
        Node::new(pos, one(), vec3!(1.))
    }

    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(Pnt3::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,
            #[cfg(feature="ecs")]
            has_changed: false,
        }
    }

    #[cfg(feature="ecs")]
    pub fn has_changed(&self) -> bool{
        self.has_changed
    }

    #[cfg(feature="ecs")]
    pub fn reset_changed(&mut self){
        self.has_changed = false;
    }

    /// 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
    ///
    /// Returns an error if the look at direction and the up vector are parallel
    pub fn new_look_at(eye: Pnt3, at: Pnt3, up: Vec3) -> Result<Node>{
        let rh_dir = eye - at;
        if rh_dir.is_parallel(&up) {
            return Err(Error::new("Look at direction and up vector can't be parallel"))
        }

        let orientation = UnitQuat::face_towards(&rh_dir, &up);
        let local_transformation = Isometry3::from_parts(
            Translation::from(eye.to_vec()),
            orientation
        ).to_homogeneous();

        Ok(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,
            #[cfg(feature="ecs")]
            has_changed: false,
        })
    }

    /// 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
    ///
    /// Returns an error if the look at direction and the up vector are parallel
    pub fn look_at(&mut self, at: &Pnt3, up: &Vec3) -> Result<()> {
        let rh_dir = self.position() - at;
        if rh_dir.is_parallel(up) {
            return Err(Error::new("Look at direction and up vector can't be parallel"))
        }
        let orientation = UnitQuat::face_towards(&rh_dir, up);
        self.set_orientation(orientation);
        Ok(())
    }

    /// Changes the orientation of the node to look at the passed direction using the up vector
    ///
    /// Returns an error if the look at direction and the up vector are parallel
    pub fn face_towards(&mut self, dir: &Vec3, up: &Vec3) -> Result<()> {
        if dir.is_parallel(up) {
            return Err(Error::new("Look at direction and up vector can't be parallel"))
        }
        self.set_orientation(UnitQuat::face_towards(dir, up));
        Ok(())
    }


    /// Changes the orientation of the node to look at the passed node global position
    /// using the up vector
    ///
    /// Returns an error if the look at direction and the up vector are parallel
    pub fn look_at_node<N: NodeRef>(&mut self, node: &N, up: &Unit<Vec3>) -> Result<()>{
        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 local_x_axis(&self) -> Unit<Vec3>{
        Unit::new_normalize(self.local_transformation().column(0).xyz())
    }

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

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

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

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

    /// The node orientation z axis
    pub fn global_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.local_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.local_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.local_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(&[
            self.global_transformation().column(0).xyz().normalize(),
            self.global_transformation().column(1).xyz().normalize(),
            self.global_transformation().column(2).xyz().normalize(),
        ]);
        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.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{
        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 = parent.global_transformation();
                let parent_trafo = if let Some(parent_inv) = self.parent_inv{
                    parent_trafo * parent_inv
                }else{
                    parent_trafo
                };
                *self.global_transformation = parent_trafo * *self.local_transformation;

                let s = &self.scale;
                let p = parent.global_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();

        #[cfg(feature="ecs")]
        {
            self.has_changed |= changed || global_changed;
        }

        changed || global_changed
    }

    /// Updates the internal matrices including the glonal matrices
    /// using the optional parent passed as argument and flags
    pub fn update_with_parent_parts(
        &mut self,
        parent_loc: Option<&Node>,
        parent_orientation: Option<&Node>,
        parent_scale: Option<&Node>) -> 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_loc.map(|parent| parent.global_transformation.has_changed())
                .unwrap_or(false)
            || parent_orientation.map(|parent| parent.global_transformation.has_changed())
                .unwrap_or(false)
            || parent_scale.map(|parent| parent.global_scale.has_changed())
                .unwrap_or(false);
        if global_changed {
            if parent_loc.is_some() || parent_orientation.is_some() || parent_scale.is_some() {
                let parent_translation = parent_loc
                    .map(|p| p.global_position())
                    .unwrap_or(Pnt3::origin());
                let parent_orientation = parent_orientation
                    .map(|p| p.global_orientation())
                    .unwrap_or(UnitQuat::identity());
                let parent_scale = parent_scale
                    .map(|p| p.global_scale())
                    .unwrap_or(vec3!(1.));

                let parent_trafo = trafo_from_parts(
                    &parent_translation,
                    &parent_orientation,
                    &parent_scale
                );
                let parent_trafo = if let Some(parent_inv) = self.parent_inv{
                    parent_trafo * parent_inv
                }else{
                    parent_trafo
                };
                *self.global_transformation = parent_trafo * *self.local_transformation;

                let s = &self.scale;
                let p = parent_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();

        #[cfg(feature="ecs")]
        {
            self.has_changed |= changed || global_changed;
        }

        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 local_x_axis(&self) -> Unit<Vec3>{
        self.node().local_x_axis()
    }

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

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

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

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

    /// The node orientation z axis
    fn global_z_axis(&self) -> Unit<Vec3>{
        self.node().global_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_parts(
        &mut self,
        parent_loc: Option<&Node>,
        parent_orientation: Option<&Node>,
        parent_scale: Option<&Node>) -> 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_parts(parent, parent, parent)
    }

    /// 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) -> Result<()>{
        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_parts(
        &mut self,
        parent_loc: Option<&Node>,
        parent_orientation: Option<&Node>,
        parent_scale: Option<&Node>) -> bool
    {
        self.update_with_parent_parts(parent_loc, parent_orientation, parent_scale)
    }
}

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 One for Node{
    fn one() -> 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(Pnt3::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<UnitQuat> for Node{
    fn from(q: UnitQuat) -> Node{
        Node::new(Pnt3::origin(), q, vec3!(1.))
    }
}

#[cfg(feature="ecs")]
pub struct NodeParts;

#[cfg(feature="ecs")]
pub struct DynamicTransformation;

#[cfg(feature="ecs")]
#[system(name = "transfo. dyn. update")]
#[needs("NodeParts")]
#[updates("Node")]
pub fn update_dynamic(mut entities: rinecs::Entities, _: rinecs::Resources){
    entities.changes_ordered_iter_with::<(
        WriteAndParent<Node>,
        Has<DynamicTransformation>),_>(|((trafo, parent), _)|{
            trafo.has_changed = false;
            trafo.update_with_parent(parent)
        });
}

#[cfg(feature="ecs")]
#[system(name = "transfo. static update")]
#[needs("NodeParts")]
#[updates("Node")]
pub fn update_static(mut entities: rinecs::Entities, _: rinecs::Resources){
    entities.changes_ordered_iter_with::<(
        WriteAndParent<Node>,
        Not<DynamicTransformation>),_>(|((trafo, parent), _)|{
            trafo.has_changed = false;
            trafo.update_with_parent(parent)
        });
}

#[cfg(feature="ecs")]
#[system(name = "transfo. all update")]
#[needs("NodeParts")]
#[updates("Node")]
pub fn update_all(mut entities: rinecs::Entities, _: rinecs::Resources){
    entities.changes_ordered_iter_with::<WriteAndParent<Node>,_>(|(trafo, parent)|{
            trafo.has_changed = false;
            trafo.update_with_parent(parent)
        });
}