using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Xml.Serialization;
using System.ComponentModel;

namespace VisiMath
{
    /// <summary>
    /// Transformation state
    /// </summary>
    public struct TransformationState
    {
        public Vector3 Position;
        public Vector3 Scale;
        public Quaternion Rotation;
    }
    
    /// <summary>
    /// Generic transformation stored in transformation matrix with no other data saved
    /// Acessing rotation/scaling properties requires recalulcation (slow!)
    /// </summary>        
    public class BaseTransformation : ITransformation
    {
        /// <summary>
        /// Internaly stored transformation
        /// </summary>
        protected Matrix4 trans;

        /// <summary>
        /// True if object supports movement
        /// </summary>
        [XmlIgnore]
        public virtual bool Movable { get { return false; } }
        /// <summary>
        /// True if object supports rotation
        /// </summary>
        [XmlIgnore]
        public virtual bool Rotable { get { return false; } }
        /// <summary>
        /// True if object supports scaling
        /// </summary>
        [XmlIgnore]
        public virtual bool Scalable { get { return false; } }



        /// <summary>
        /// Callback for any transformation change
        /// </summary>
        public event TransformationCallback ChangeCallback;

        protected void OnChange()
        {
            if (ChangeCallback != null)
                ChangeCallback(this);
        }

        /// <summary>
        /// Default constructor
        /// </summary>
        public BaseTransformation()
        {
            trans = Matrix4.Identitity;
        }

        /// <summary>
        /// Constructs transformation from the already finished matrix
        /// </summary>
        /// <param name="mat">Transformation matrix</param>
        public BaseTransformation(ref Matrix4 mat)
        {
            trans = new Matrix4(ref mat);
        }
        
        /// <summary>
        /// Gets or sets transformation matrix
        /// </summary>
        [XmlIgnore]
        virtual public Matrix4 TransMatrix
        {
            get
            {
                return trans;
            }
            set
            {
                trans = value;
            }
        }

        /// <summary>
        /// Multiplies current matrix with another(trans = m*trans)
        /// </summary>
        /// <param name="m">another transformation</param>
        virtual protected void MulMatrix(ref Matrix4 m)
        {
            trans = Matrix4.Multiply(ref m, ref trans);

            OnChange();
        }

        /// <summary>
        /// Used to quickly get all of the properties with one call
        /// </summary>
        /// <param name="pos">Position vector</param>
        /// <param name="rot">RPY rotation</param>
        /// <param name="scale">Scale</param>
        /// <param name="uniscale">Uniform Scale</param>
        virtual public void GetAll(out Vector3 pos, out Vector3 rot, out Vector3 scale, out float uniscale)
        {
            Quaternion q;
            trans.Deconstruct(out uniscale, out scale, out q, out pos);
            rot = q.GetRPY();
        }

        /// <summary>
        /// Used to quickly get all of the properties with one call
        /// </summary>
        /// <param name="pos">Position vector</param>
        /// <param name="rot">quaternion rotation</param>
        /// <param name="scale">Scale</param>
        /// <param name="uniscale">Uniform Scale</param>
        virtual public void GetAll(out Vector3 pos, out Quaternion rot, out Vector3 scale, out float uniscale)
        {
            trans.Deconstruct(out uniscale, out scale, out rot, out pos);
        }

        protected Stack<TransformationState> StateStack = new Stack<TransformationState>(1);

        public TransformationState GetCurrentState()
        {
            TransformationState rval = new TransformationState();

            float uni;

            GetAll(out rval.Position, out rval.Rotation, out rval.Scale, out uni);

            return rval;
        }

        public virtual void SetCurrentState(TransformationState state)
        {
            trans = Matrix4.CreateTransformation(ref state.Scale, ref state.Rotation, ref state.Position);
            OnChange();
        }

        /// <summary>
        /// Pushes current state of transformation to internal stack
        /// </summary>
        public virtual void PushState()
        {
            StateStack.Push(GetCurrentState());            
        }
        /// <summary>
        /// Pops current state of transformation from internal state stack and sets it as current
        /// </summary>
        public virtual void PopState()
        {
            try
            {
                TransformationState state = StateStack.Pop();
                SetCurrentState(state);
            }
            catch
            {
            }
        }
        /// <summary>
        /// Pops state of transformation from internal stack
        /// </summary>
        public virtual void UndoPushState()
        {
            try
            {
                TransformationState state = StateStack.Pop();
            }
            catch
            {
            }
        }



        #region Position

        /// <summary>
        /// Position in vector - virtual so derived classes may override it
        /// </summary>        
        [TypeConverter(typeof(VectorConvertor))]
        virtual public Vector3 Position
        {
            get
            {
                return trans.Translation;
            }
            set
            {
                trans.Translation = value;
                OnChange();
            }
        }

        /// <summary>
        /// X position from the Position property
        /// </summary>
        [XmlIgnore]
        public float X
        {
            get
            {
                return trans.Translation.X;
            }
            set
            {
                Vector3 vec = trans.Translation;
                vec.X = value;
                trans.Translation = vec;
                OnChange();
            }
        }

        /// <summary>
        /// Y position from the Position property
        /// </summary>
        [XmlIgnore]
        public float Y
        {
            get
            {
                return trans.Translation.Y;
            }
            set
            {
                Vector3 vec = trans.Translation;
                vec.Y = value;
                trans.Translation = vec;
                OnChange();
            }
        }


        /// <summary>
        /// Z position from the Position property
        /// </summary>
        [XmlIgnore]
        public float Z
        {
            get
            {
                return trans.Translation.Z;
            }
            set
            {
                Vector3 vec = trans.Translation;
                vec.Z = value;
                trans.Translation = vec;
                OnChange();
            }
        }

        #endregion

        #region Rotation

        /// <summary>
        /// Rotation in quaternion, virtual, so derived classes may override it
        /// Note: getting quaternion requires deconstructing matrix. Setting quaternion requires deconstructing and new transformation construction.
        /// May not be completely accurate for more complicated matrix
        /// </summary>
        virtual public Quaternion Rotation
        {
            get
            {
                return Quaternion.CreateFromMatrix(ref trans);
            }
            set
            {
                float uni;
                Vector3 sc, tr;
                Quaternion q;

                trans.Deconstruct(out uni, out sc, out q, out tr);

                trans = Matrix4.CreateTransformation(ref sc, ref value, ref tr);
                OnChange();
            }
        }

        /// <summary>
        /// Rotation in roll-pitch-yaw format, virtual, so derived classes may override it
        /// Note: getting rotation requires deconstructing matrix. Setting quaternion requires deconstructing and new transformation construction.
        /// May not be completely accurate for more complicated matrix
        /// </summary>
        [XmlIgnore]
        virtual public Vector3 RPY
        {
            get
            {
                return Quaternion.CreateFromMatrix(ref trans).GetRPY();
            }
            set
            {
                float uni;
                Vector3 sc, tr;
                Quaternion q;

                trans.Deconstruct(out uni, out sc, out q, out tr);
                q = Quaternion.CreateFromRPY(ref value);

                trans = Matrix4.CreateTransformation(ref sc, ref q, ref tr);
                OnChange();
            }
        }


        /// <summary>
        /// Pitch rotation from RPY
        /// </summary>
        [XmlIgnore]
        public float PitchAngle
        {
            get
            {
                Vector3 rpy = RPY;
                return rpy.Y;
            }
            set
            {
                Vector3 rpy = RPY;
                rpy.Y = value;
                RPY = rpy;
                OnChange();
            }
        }

        /// <summary>
        /// Yaw rotation from RPY
        /// </summary>
        [XmlIgnore]
        public float YawAngle
        {
            get
            {
                Vector3 rpy = RPY;
                return rpy.Z;
            }
            set
            {
                Vector3 rpy = RPY;
                rpy.Z = value;
                RPY = rpy;
                OnChange();
            }
        }

        /// <summary>
        /// Roll rotation from RPY
        /// </summary>
        [XmlIgnore]
        public float RollAngle
        {
            get
            {
                Vector3 rpy = RPY;
                return rpy.X;
            }
            set
            {
                Vector3 rpy = RPY;
                rpy.X = value;
                RPY = rpy;
                OnChange();
            }
        }

        #endregion 

        #region Scale

        /// <summary>
        /// Uniform scale, returns zero if scale is not uniform.
        /// Note: getting scale requires deconstructing matrix. Setting scale requires deconstructing and new transformation construction.
        /// May not be completely accurate for more complicated matrix
        /// </summary>
        virtual public float UniformScale
        {
            get
            {
                return trans.GetUniformScale();
            }
            set
            {
                float uni;
                Vector3 sc, tr;
                Quaternion q;

                trans.Deconstruct(out uni, out sc, out q, out tr);
                sc.Assign(value, value, value);

                trans = Matrix4.CreateTransformation(ref sc, ref q, ref tr);
                OnChange();
            }
        }

        /// <summary>
        /// Scale vector, returns zero if scale is not uniform.
        /// Note: getting scale requires deconstructing matrix. Setting scale requires deconstructing and new transformation construction.
        /// May not be completely accurate for more complicated matrix
        /// </summary>
        [TypeConverter(typeof(VectorConvertor))]
        virtual public Vector3 Scale
        {
            get
            {
                Vector3 sc; float uni;
                trans.GetScale(out uni, out sc);
                return sc;
            }
            set
            {
                float uni;
                Vector3 sc, tr;
                Quaternion q;

                trans.Deconstruct(out uni, out sc, out q, out tr);

                trans = Matrix4.CreateTransformation(ref value, ref q, ref tr);
                OnChange();
            }
        }

        #endregion

        #region Movement

        /// <summary>
        /// Move transformation in world coordinates (translation)
        /// Modifies position property
        /// </summary>
        /// <param name="movement">Translation vector</param>
        public void Move(ref Vector3 movement)
        {
            Position = Position + movement;
        }

        /// <summary>
        /// Move transformation in world coordinates (translation)
        /// Modifies position property
        /// </summary>
        /// <param name="movement">Translation vector</param>
        public void Move(Vector3 movement)
        {
            Position = Position + movement;
        }


        /// <summary>
        /// Move transformation in world coordinates (translation) in specified angle and distance
        /// Modifies position property
        /// </summary>
        /// <param name="q">rotation</param>
        /// <param name="distance">distance</param>
        public void Move(ref Quaternion q, float distance)
        {
            Vector3 move = q * new Vector3(distance, 0, 0);
            Move(ref move);
        }

        /// <summary>
        /// Move transformation in world coordinates (translation) in specified angle and distance
        /// Modifies position property
        /// </summary>
        /// <param name="axis">Rotation axis</param>
        /// <param name="angle">Rotation angle</param>
        /// <param name="distance">distance</param>
        public void Move(ref Vector3 axis, float angle, float distance)
        {
            Quaternion q = Quaternion.CreateFromAxisAngle(ref axis, angle);
            Move(ref q, distance);
        }

        /// <summary>
        /// Move transformation in world coordinates (translation) in directional vector and distance
        /// Modifies position property
        /// </summary>
        /// <param name="direction">Directional vector (should be normalized)</param>
        /// <param name="distance">distance</param>
        public void Move(ref Vector3 direction, float distance)
        {
            Vector3 move = Vector3.Mul(ref direction, distance);
            Move(ref move);
        }

        #region Movement in local space
        /// <summary>
        /// Move transformation in local coordinates
        /// </summary>
        /// <param name="movement">Translation vector</param>
        virtual public void MoveLocal(ref Vector3 movement)
        {
            Vector3 transf = Rotation * movement;
            
            Move(ref transf);
        }

        /// <summary>
        /// Move transformation in local coordinates (translation) in specified angle and distance
        /// </summary>
        /// <param name="q">rotation</param>
        /// <param name="distance">distance</param>
        public void MoveLocal(ref Quaternion q, float distance)
        {
            Vector3 move = q * new Vector3(distance, 0, 0);
            MoveLocal(ref move);
        }

        /// <summary>
        /// Move transformation in local coordinates (translation) in specified angle and distance
        /// </summary>
        /// <param name="axis">rotation axis</param>
        /// <param name="angle">angle</param>
        /// <param name="distance">distance</param>
        public void MoveLocal(ref Vector3 axis, float angle, float distance)
        {
            Quaternion q = Quaternion.CreateFromAxisAngle(ref axis, angle);
            MoveLocal(ref q, distance);
        }

        /// <summary>
        /// Move transformation in local coordinates (translation) in directional vector and distance
        /// </summary>
        /// <param name="direction">Directional vector (should be normalized)</param>
        /// <param name="distance">distance</param>
        public void MoveLocal(ref Vector3 direction, float distance)
        {
            Vector3 move = Vector3.Mul(ref direction, distance);
            MoveLocal(ref move);
        }

        #endregion
        #endregion

        #region Rotation change

        /// <summary>
        /// Rotate whole transformation in world coordinates
        /// Calls MulMatrix
        /// </summary>
        /// <param name="q">rotation</param>
        public void Rotate(ref Quaternion q)
        {
            //todo!! : correct description?
            //Matrix4 m = Matrix4.CreateRotation(ref q);
            //MulMatrix(ref m);
            Rotation = Rotation * q;
        }

        /// <summary>
        /// Rotate whole transformation in world coordinates
        /// Calls MulMatrix
        /// </summary>
        /// <param name="axis">rotation axis</param>
        /// <param name="angle">angle in radians</param>
        public void Rotate(ref Vector3 axis, float angle)
        {
            //Matrix4 m = Matrix4.CreateRotationAxisAngle(axis, angle);
            //MulMatrix(ref m);
            Quaternion q = Quaternion.CreateFromAxisAngle(ref axis, angle);
            Rotate(ref q);
        }
        /// <summary>
        /// Rotate whole transformation in world coordinates
        /// Calls MulMatrix
        /// </summary>
        /// <param name="RPY">Roll-pitch-yaw rotation</param>
        public void Rotate(ref Vector3 rpy)
        {
            //Matrix4 m = Matrix4.CreateRotationRPY(ref rpy);
            //MulMatrix(ref m);
            Quaternion q = Quaternion.CreateFromRPY(ref rpy);
            Rotate(ref q);
        }

        /// <summary>
        /// Rotate whole transformation in world coordinates
        /// Calls MulMatrix
        /// </summary>
        /// <param name="RPY">Roll-pitch-yaw rotation</param>
        public void Rotate(Vector3 rpy)
        {
            Rotate(ref rpy);
        }


        #region Local rotation

        /// <summary>
        /// Rotate in local coordinates (around the zero)
        /// </summary>
        /// <param name="q">rotation</param>
        public void RotateLocal(ref Quaternion q)
        {
            Rotation = q * Rotation;
        }

        /// <summary>
        /// Rotate in local coordinates (around the local zero)
        /// Changes Rotation quaternion
        /// </summary>
        /// <param name="axis">rotation axis</param>
        /// <param name="angle">angle in radians</param>
        public void RotateLocal(ref Vector3 axis, float angle)
        {
            Quaternion q = Quaternion.CreateFromAxisAngle(ref axis, angle);
            Rotation = q * Rotation;
        }

        /// <summary>
        /// Rotate in local coordinates (around the local zero)
        /// Changes Rotation quaternion
        /// </summary>
        /// <param name="rpy">rotation in roll-pitch-yaw</param>
        public void RotateLocal(ref Vector3 rpy)
        {
            Quaternion q = Quaternion.CreateFromRPY(ref rpy);
            Rotation = q * Rotation;
        }

        /// <summary>
        /// Rotate in local coordinates (around the local zero)
        /// Changes Rotation quaternion
        /// </summary>
        /// <param name="rpy">rotation in roll-pitch-yaw</param>
        public void RotateLocal(Vector3 rpy)
        {
            RotateLocal(ref rpy);
        }

        /// <summary>
        /// Rotate in local coordinates (changes pitch)
        /// Changes RPY
        /// </summary>
        /// <param name="angle">angle in radians</param>
        public void Pitch(float angle)
        {
            Quaternion q = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, -angle);
            RotateLocal(ref q);
            /*
            Vector3 rpy = RPY;
            rpy.Z += angle;
            RPY = rpy;*/
        }

        /// <summary>
        /// Rotate in local coordinates (changes yaw)
        /// Changes RPY
        /// </summary>
        /// <param name="angle">angle in radians</param>
        public void Yaw(float angle)
        {
            Quaternion q = Quaternion.CreateFromAxisAngle(Vector3.UnitY, angle);
            RotateLocal(ref q);
            /*
            Vector3 rpy = RPY;
            rpy.Y += angle;
            RPY = rpy;*/
        }

        /// <summary>
        /// Rotate in local coordinates (changes roll)
        /// Changes RPY
        /// </summary>
        /// <param name="angle">angle in radians</param>
        public void Roll(float angle)
        {
            Quaternion q = Quaternion.CreateFromAxisAngle(Vector3.UnitX, angle);
            RotateLocal(ref q);

            /*
            Vector3 rpy = RPY;
            rpy.X += angle;
            RPY = rpy;
             */
        }

        #endregion
        #endregion

        #region Rescale

        /// <summary>
        /// Rescale transformation by uniform number
        /// Calls MulMatrix
        /// </summary>
        /// <param name="scalefactor">uniform scale</param>
        public void Rescale(float scalefactor)
        {
            Matrix4 m = Matrix4.CreateScale(scalefactor);
            MulMatrix(ref m);
        }

        /// <summary>
        /// Rescale transformation by non-uniform vecotr
        /// Calls MulMatrix
        /// </summary>
        /// <param name="scalefactor">Scales fo all three axis</param>
        public void Rescale(ref Vector3 scalefactor)
        {
            Matrix4 m = Matrix4.CreateScale(ref scalefactor);
            MulMatrix(ref m);
        }

        /// <summary>
        /// Local uniform rescale (changes basic scale of the transformation)
        /// Changes UniformScale property
        /// </summary>
        /// <param name="scalefactor">uniform scale</param>
        public void RescaleLocal(float scalefactor)
        {
            UniformScale = UniformScale * scalefactor;
        }

        /// <summary>
        /// Local non-uniform rescale (changes basic scale of the transformation)
        /// Changes Scale property
        /// </summary>
        /// <param name="scalefactor">Scales fo all three axis</param>
        public void RescaleLocal(ref Vector3 scalefactor)
        {
            Vector3 sc = Scale;
            Scale = new Vector3(sc.X * scalefactor.X, sc.Y * scalefactor.Y, sc.Z * scalefactor.Z);
        }
        #endregion

        #region utilities

        /// <summary>
        /// Returns location of point in local space to the world space
        /// </summary>
        /// <param name="point">position in local space</param>
        /// <returns>position in world space</returns>
        public Vector3 GetPointInWorldSpace(Vector3 point)
        {
            trans.Transform(ref point);
            return point;
        }


        #endregion

        #region IXmlSerializable Members

        /// <summary>
        /// Reqired by IXMLserializable
        /// </summary>
        /// <returns>Null</returns>
        public System.Xml.Schema.XmlSchema GetSchema()
        {
            return null;
        }

        /// <summary>
        /// Deserializes object from XML
        /// </summary>
        /// <param name="reader">XML reader</param>
        public virtual void ReadXml(System.Xml.XmlReader reader)
        {
            if (reader["type"] != "generic")
                return;
            XmlSerializer ser = new XmlSerializer(trans.GetType());
            trans = (Matrix4) ser.Deserialize(reader);
        }

        public virtual void WriteXml(System.Xml.XmlWriter writer)
        {
            writer.WriteAttributeString("type", "generic");
            writer.WriteStartElement("Transformation matrix");
            XmlSerializer ser = new XmlSerializer(trans.GetType());
            ser.Serialize(writer, trans);
            writer.WriteEndElement();
        }

        /// <summary>
        /// Returns copy of the transformation
        /// </summary>
        /// <returns></returns>
        public virtual ITransformation GetCopy()
        {
            BaseTransformation newtrans = new BaseTransformation();
            newtrans.trans = this.trans;
            return newtrans;
        }


        #endregion

        public override string ToString()
        {
            return "BasicTransformation";
        }
    }

    /// <summary>
    /// Transformation allowing only movement, no rotation / scaling
    /// </summary>    
    public class MoveTransformation : BaseTransformation
    {
        /// <summary>
        /// Position stored directly in the vector
        /// </summary>
        protected Vector3 _position = new Vector3(0);
        
        protected bool _changed = false;

        /// <summary>
        /// Internal value used to watch for changes in parameters for matrix recalculation
        /// </summary>
        protected bool changed
        {
            get
            {
                return _changed;
            }
            set
            {
                _changed = true;
                OnChange();
            }
        }
            
        public override bool Movable
        {
            get
            {
                return true;
            }
        }

        /// <summary>
        /// Position in vector
        /// </summary>        
        public override Vector3 Position
        {
            get
            {
                return _position;
            }
            set
            {
                _position = value;
                changed = true;
            }
        }

        /// <summary>
        /// Updates Matrix from internal values and sets changed to false
        /// </summary>
        public virtual void UpdateMatrix()
        {
            trans = Matrix4.CreateTranslation(ref _position); 
            changed = false;
        }

        /// <summary>
        /// Updates if needed and returns transformation.
        /// No direct changes to matrix allowed
        /// </summary>
        [XmlIgnore]
        public override Matrix4 TransMatrix
        {
            get
            {
                if (changed)
                    UpdateMatrix();
                return trans;
            }
            set
            {
            }
        }

        /// <summary>
        /// Rotation is always Identity
        /// </summary>
        public override Quaternion Rotation
        {
            get
            {
                return Quaternion.Identity;
            }
            set
            {
            }
        }

        /// <summary>
        /// RPY is always 0,0,0
        /// </summary>
        public override Vector3 RPY
        {
            get
            {
                return Vector3.Zero;
            }
            set
            {
            }
        }

        /// <summary>
        /// Scale is always 1,1,1
        /// </summary>
        public override Vector3 Scale
        {
            get
            {
                return Vector3.One;
            }
            set
            {
            }
        }

        /// <summary>
        /// Uniform scale is always 1
        /// </summary>
        public override float UniformScale
        {
            get
            {
                return 1;
            }
            set
            {
            }
        }

        /// <summary>
        /// No direct manipulation with the matrix allowed!
        /// </summary>
        /// <param name="m"></param>
        protected override void MulMatrix(ref Matrix4 m)
        {
        }

        public override void GetAll(out Vector3 pos, out Quaternion rot, out Vector3 scale, out float uniscale)
        {
            pos = _position;
            rot = Quaternion.Identity;
            scale = new Vector3(1);
            uniscale = 1;
        }

        public override void GetAll(out Vector3 pos, out Vector3 rot, out Vector3 scale, out float uniscale)
        {
            pos = _position;
            rot = new Vector3();
            scale = new Vector3(1);
            uniscale = 1;
        }

        public override void SetCurrentState(TransformationState state)
        {
            Position = state.Position;
        }

        public override void WriteXml(System.Xml.XmlWriter writer)
        {
            writer.WriteAttributeString("type", "MoveTrans");
            writer.WriteStartElement("Translation");
            _position.WriteXml(writer);
            writer.WriteEndElement();
        }

        /// <summary>
        /// Deserializes object from XML
        /// </summary>
        /// <param name="reader">XML reader</param>
        public override void ReadXml(System.Xml.XmlReader reader)
        {
            if (reader["type"] != "MoveTrans")
                return;
            reader.ReadStartElement("Translation");
            _position.ReadXml(reader);
            reader.ReadEndElement();
            changed = true;
        }

        public override string ToString()
        {
            return "MoveTrans to " + _position.ToString();
        }

        /// <summary>
        /// Returns copy of the transformation
        /// </summary>
        /// <returns></returns>
        public override ITransformation GetCopy()
        {
            MoveTransformation newtrans = new MoveTransformation();
            newtrans._position = this._position;
            newtrans._changed = true;
            return newtrans;
        }

    }


    /// <summary>
    /// Transformation storing information with roll-pitch-yaw angles, no scale allowed
    /// This transformation type is usefull for simulating vehicles
    /// </summary>    
    public class RPYTransformation : MoveTransformation
    {
        /// <summary>
        /// Rotation stored in roll pitch yaw format
        /// </summary>
        protected Vector3 _rpy;

        public override bool Rotable
        {
            get
            {
                return true;
            }
        }
        /// <summary>
        /// Rotation in roll-pitch-yaw format
        /// </summary>
        public override Vector3 RPY
        {
            get
            {
                return _rpy;
            }
            set
            {
                _rpy = value;
                changed = true;
            }
        }

        /// <summary>
        /// Rotation in quaternion, calculated from or recalculated to the internal RPY
        /// </summary>
        public override Quaternion Rotation
        {
            get
            {
                return Quaternion.CreateFromRPY(ref _rpy);
            }
            set
            {
                _rpy = value.GetRPY();
                changed = false;
            }
        }


        /// <summary>
        /// Updates matrix - creates new based on the rotation and translation
        /// </summary>
        public override void UpdateMatrix()
        {
            _rpy.ClampRPY();
            trans = Matrix4.CreateTransformation(ref _rpy, ref _position);
            changed = false;
        }

        public override void GetAll(out Vector3 pos, out Quaternion rot, out Vector3 scale, out float uniscale)
        {
            pos = _position;
            rot = Quaternion.CreateFromRPY(ref _rpy);
            scale = new Vector3(1);
            uniscale = 1;
        }

        public override void GetAll(out Vector3 pos, out Vector3 rot, out Vector3 scale, out float uniscale)
        {
            pos = _position;
            rot = _rpy;
            scale = new Vector3(1);
            uniscale = 1;
        }

        public override void SetCurrentState(TransformationState state)
        {
            Position = state.Position;
        }


        public override void WriteXml(System.Xml.XmlWriter writer)
        {
            writer.WriteAttributeString("type", "RPYTrans");
            writer.WriteStartElement("Translation");
            _position.WriteXml(writer);
            writer.WriteEndElement();
            writer.WriteStartElement("Rotation");
            XMLHelper.WriteFloatAttribute(writer, "Roll", _rpy.X);
            XMLHelper.WriteFloatAttribute(writer, "Pitch", _rpy.Y);
            XMLHelper.WriteFloatAttribute(writer, "Yaw", _rpy.Z);
            writer.WriteEndElement();
        }

        /// <summary>
        /// Returns copy of the transformation
        /// </summary>
        /// <returns></returns>
        public override ITransformation GetCopy()
        {
            RPYTransformation newtrans = new RPYTransformation();
            newtrans._position = this._position;
            newtrans._changed = true;
            newtrans._rpy = this._rpy;
            return newtrans;
        }


        /// <summary>
        /// Deserializes object from XML
        /// </summary>
        /// <param name="reader">XML reader</param>
        public override void ReadXml(System.Xml.XmlReader reader)
        {
            if (reader["type"] != "RPYTrans")
                return;
            changed = true;
            while (reader.Read())
            {
                if (reader.Name == "Translation")
                {
                    _position.ReadXml(reader);
                }
                else if (reader.Name == "Rotation")
                {
                    _rpy.X = XMLHelper.FloatFromString(reader["Roll"]);
                    _rpy.Y = XMLHelper.FloatFromString(reader["Pitch"]); ;
                    _rpy.Z = XMLHelper.FloatFromString(reader["Yaw"]);;

                }
                else return;
            }
        }

        public override string ToString()
        {
            return "RPYTrans to " + _position.ToString() + " and rot: " + _rpy.ToString();
        }
    }

    /// <summary>
    /// Transformation storing information in rotational quaternion and translation vector.
    /// No scale change allowed.
    /// </summary>    
    public class QTransformation : MoveTransformation
    {
        /// <summary>
        /// Rotation stored in the internal quaternion
        /// </summary>
        protected Quaternion _q = Quaternion.Identity;

        public override bool Rotable
        {
            get
            {
                return true;
            }
        }

        /// <summary>
        /// Rotation in quaternion
        /// </summary>
        public override Quaternion Rotation
        {
            get
            {
                return _q;
            }
            set
            {
                value.Normalize();
                _q = value;
                changed = true;
            }
        }

        /// <summary>
        /// Rotation in roll-pitch-yaw format, calculated from or recalculated to internal quaternion
        /// /// </summary>
        public override Vector3 RPY
        {
            get
            {
                return _q.GetRPY();
            }
            set
            {
                _q = Quaternion.CreateFromRPY(ref value);
                changed = true;
            }
        }

        /// <summary>
        /// Updates transformation from the rotational quaternion and translation vector
        /// </summary>
        public override void UpdateMatrix()
        {
            trans = Matrix4.CreateTransformation(ref _q, ref _position);
            changed = false;
        }

        public override void GetAll(out Vector3 pos, out Quaternion rot, out Vector3 scale, out float uniscale)
        {
            pos = _position;
            rot = _q;
            scale = new Vector3(1);
            uniscale = 1;
        }

        public override void GetAll(out Vector3 pos, out Vector3 rot, out Vector3 scale, out float uniscale)
        {
            pos = _position;
            rot = _q.GetRPY();
            scale = new Vector3(1);
            uniscale = 1;
        }

        public override void SetCurrentState(TransformationState state)
        {
            Position = state.Position;
            Rotation = state.Rotation;
        }


        public override void WriteXml(System.Xml.XmlWriter writer)
        {
            writer.WriteAttributeString("type", "QTrans");
            writer.WriteStartElement("Translation");
            _position.WriteXml(writer);
            writer.WriteEndElement();
            writer.WriteStartElement("Rotation");
            _q.WriteXml(writer);
            writer.WriteEndElement();
        }


        /// <summary>
        /// Deserializes object from XML
        /// </summary>
        /// <param name="reader">XML reader</param>
        public override void ReadXml(System.Xml.XmlReader reader)
        {
            if (reader["type"] != "QTrans")
                return;
            changed = true;
            while (reader.Read())
            {
                if (reader.IsStartElement())
                {
                    if (reader.Name == "Translation")
                    {
                        _position.ReadXml(reader);
                    }
                    else if (reader.Name == "Rotation")
                    {
                        _q.ReadXml(reader);
                    }
                }
                else return;
            }
        }

        public override string ToString()
        {
            return "QTrans to " + _position.ToString() + " and rot " + _q.ToString();
        }

        /// <summary>
        /// Returns copy of the transformation
        /// </summary>
        /// <returns></returns>
        public override ITransformation GetCopy()
        {
            QTransformation newtrans = new QTransformation();
            newtrans._position = this._position;
            newtrans._changed = true;
            newtrans._q = this._q;
            return newtrans;
        }

    }
    

    /// <summary>
    /// Full transformation using scale, rotational quaternion and translation vector
    /// </summary>    
    public class Transformation : QTransformation
    {
        /// <summary>
        /// Internaly stored scale vector
        /// </summary>
        protected Vector3 _scale;

        public override bool Scalable
        {
            get
            {
                return true;
            }
        }
        public Transformation()
        {
            _scale = new Vector3(1, 1, 1);
        }

        /// <summary>
        /// Updates transformation from the scale vector, quaternion and translation vector
        /// </summary>
        public override void UpdateMatrix()
        {
            trans = Matrix4.CreateTransformation(ref _scale, ref _q, ref _position);
            changed = false;
        }

        /// <summary>
        /// Scale in different axis
        /// </summary>
        public override Vector3 Scale
        {
            get
            {
                return _scale;
            }
            set
            {
                _scale = value;
                changed = true;
            }
        }

        /// <summary>
        /// Uniform scale or returns zero if scale is not uniform
        /// </summary>
        public override float UniformScale
        {
            get
            {
                if (MathTools.AlmostEqual(_scale.X, _scale.Y, 0.0001f) && MathTools.AlmostEqual(_scale.X, _scale.Z, 0.0001f))
                {
                    return _scale.X;
                }
                return 0;
            }
            set
            {
                Scale = new Vector3(value);
            }
        }

        public override void GetAll(out Vector3 pos, out Quaternion rot, out Vector3 scale, out float uniscale)
        {
            pos = _position;
            rot = _q;
            scale = _scale;
            uniscale = UniformScale;
        }

        public override void GetAll(out Vector3 pos, out Vector3 rot, out Vector3 scale, out float uniscale)
        {
            pos = _position;
            rot = _q.GetRPY();
            scale = _scale;
            uniscale = UniformScale;
        }

        public override void SetCurrentState(TransformationState state)
        {
            Position = state.Position;
            Rotation = state.Rotation;
            Scale = state.Scale;
        }


        /// <summary>
        /// Deserializes object from XML
        /// </summary>
        /// <param name="reader">XML reader</param>
        public override void ReadXml(System.Xml.XmlReader reader)
        {
            if (reader["type"] != "FullTrans")
                return;
            changed = true;
            while (reader.Read())
            {
                if (reader.IsStartElement())
                {
                    if (reader.Name == "Translation")
                    {
                        _position.ReadXml(reader);
                    }
                    else if (reader.Name == "Rotation")
                    {
                        _q.ReadXml(reader);
                    }
                    else if (reader.Name == "Scale")
                    {
                        _scale.ReadXml(reader);
                    }
                    else if (reader.Name == "UniformScale")
                    {
                        float val = XMLHelper.FloatFromString(reader["scale"]);
                        if (val != 0)
                            UniformScale = val;
                    }
                }
                else return;
            }
        }

        /// <summary>
        /// Serializes object to XML
        /// </summary>
        /// <param name="writer">XML writer to save values</param>
        public override void WriteXml(System.Xml.XmlWriter writer)
        {
            writer.WriteAttributeString("type", "FullTrans");
            writer.WriteStartElement("Translation");
            _position.WriteXml(writer);
            writer.WriteEndElement();
            writer.WriteStartElement("Rotation");
            _q.WriteXml(writer);
            writer.WriteEndElement();

            if (UniformScale != 0)            
            {
                writer.WriteStartElement("UniformScale");
                writer.WriteAttributeString("scale", _scale.X.ToString(XMLHelper.CF));
                writer.WriteEndElement();
            }
            else
            {
                writer.WriteStartElement("Scale");
                _scale.WriteXml(writer);
                writer.WriteEndElement();
            }
        }

        public override string ToString()
        {
            return "FullTrans to pos " + _position.ToString() + " rot " + _q.ToString() + " scale " + _scale.ToString();
        }

        /// <summary>
        /// Returns copy of the transformation
        /// </summary>
        /// <returns></returns>
        public override ITransformation GetCopy()
        {
            Transformation newtrans = new Transformation();
            newtrans._position = this._position;
            newtrans._changed = true;
            newtrans._q = this._q;
            newtrans._scale = this._scale;
            return newtrans;
        }


    }//transformation



    public static class TransformationFactory
    {

        public static ITransformation ReadFromXML(System.Xml.XmlReader reader)
        {
            string type = reader["type"];
            ITransformation rval;

            if (type == "FullTrans")
            {
                rval = new Transformation();
            }
            else if (type == "generic")
            {
                rval = new BaseTransformation();
            }
            else if (type == "MoveTrans")
            {
                rval = new MoveTransformation();
            }
            else if (type == "RPYTrans")
            {
                rval = new RPYTransformation();
            }
            else if (type == "QTrans")
            {
                rval = new QTransformation();
            }
            else
                return null; //todo: try to guess the type             
            rval.ReadXml(reader);
            return rval;
        }
    }


}
