/*
 * This source file is part of KEOS (Free 3D Engine)
 * For the latest info, see http://www.keosengine.org/
 * E-mails : thierry.vouriot@keosengine.org, yeri@keosengine.org
 * 
 * This program is free software; you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the Free Software
 * Foundation; either version 2 of the License, or (at your option) any later
 * version.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 * FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License along with
 * this program; if not, write to the Free Software Foundation, Inc., 59 Temple
 * Place - Suite 330, Boston, MA 02111-1307, USA, or go to
 * http://www.gnu.org/copyleft/lesser.txt.
 *
 */

#ifndef KEOS_QUATERNION_H
#define KEOS_QUATERNION_H

#include "KeosPrerequisites.h"
#include "KeosMatrix4.h"

namespace Keos
{
    //-----------------------------------------------------------------------
    /** Quaternions class.
     *
     */
    class CQuaternion
    {
    public :

        /** Default constructor.
         */
        CQuaternion(float X = 0.0f, float Y = 0.0f, float Z = 0.0f, float W = 1.0f);

        /** Constructor from one matrix.
         *  @param Matrix Source matrix.
         */
        CQuaternion(const CMatrix4& Matrix);

        /** Constructor from one axis and one angle.
         *  @param Axis Rotation axis.
         *  @param Angle Rotation angle.
         */
        CQuaternion(const TVector3F& Axis, float Angle);

        /** Identity quaternion.
         */
        void Identity();

        /** Normalize the quaternion.
         */
        inline void Normalize();

        /** Return the conjugate.
         *  @return Quaternion conjugate.
         */
        CQuaternion Conjugate() const;

        /** Build a rotation matrix from quaternion.
         *  @return Built matrix.
         */
        CMatrix4 ToMatrix() const;

        /** Build quaternion from one matrice.
         *  @param Matrix Source matrix.
         */
        void FromMatrix(const CMatrix4& Matrix);

        /** Build quaternion from one axis and one angle.
         *  @param Axis Rotation axis.
         *  @param Angle Rotation angle.
         */
        void FromAxisAngle(const TVector3F& Axis, float Angle);

        /** Transform quaternion into an axis and a rotation angle.
         *  @param Axis Rotation axis.
         *  @param Angle Rotation angle.
         */
        void ToAxisAngle(TVector3F& Axis, float& Angle) const;

        /** Build quaternion from three Euler angles.
         *  @param X X-axis rotation.
         *  @param Y Y-axis rotation.
         *  @param Z Z-axis rotation.
         */
        void FromEulerAngles(float X, float Y, float Z);

        // Operators
        CQuaternion operator *(const CQuaternion& Quaternion) const;
        const CQuaternion& operator *=(const CQuaternion& Quaternion);

        float x; 
        float y; 
        float z; 
        float w; 
    };

    // Globals functions
    std::istream& operator >>(std::istream& Stream, CQuaternion& Quaternion);
    std::ostream& operator <<(std::ostream& Stream, const CQuaternion& Quaternion);

    //=======================================================================
    // CQuaternion inline functions
    //=======================================================================
    
    //-----------------------------------------------------------------------
    inline CQuaternion::CQuaternion(float X, float Y, float Z, float W) :
    x(X),
    y(Y),
    z(Z),
    w(W)
    {

    }

    //-----------------------------------------------------------------------
    inline CQuaternion::CQuaternion(const CMatrix4& Matrix)
    {
        FromMatrix(Matrix);
    }

    //-----------------------------------------------------------------------
    inline CQuaternion::CQuaternion(const TVector3F& Axis, float Angle)
    {
        FromAxisAngle(Axis, Angle);
    }

    //-----------------------------------------------------------------------
    inline void CQuaternion::Identity()
    {
        x = y = z = 0.0f;
        w = 1.0f;
    }

    //-----------------------------------------------------------------------
    inline void CQuaternion::Normalize()
    {
        float Norm = x * x + y * y + z * z + w * w;

        if (std::fabs(Norm) > std::numeric_limits<float>::epsilon())
        {
            x /= Norm;
            y /= Norm;
            z /= Norm;
            w /= Norm;
        }
    }

    //-----------------------------------------------------------------------
    inline CQuaternion CQuaternion::Conjugate() const
    {
        return CQuaternion(-x, -y, -z, w);
    }

    //-----------------------------------------------------------------------
    inline CMatrix4 CQuaternion::ToMatrix() const
    {
        float xx = x * x;
        float xy = x * y;
        float xz = x * z;
        float xw = x * w;
        float yy = y * y;
        float yz = y * z;
        float yw = y * w;
        float zz = z * z;
        float zw = z * w;

        return CMatrix4(1 - 2 * (yy + zz),     2 * (xy - zw),     2 * (xy + yw), 0,
                            2 * (xy + zw), 1 - 2 * (xx + zz),     2 * (yz - xw), 0,
                            2 * (xz - yw),     2 * (yz + xw), 1 - 2 * (xx + yy), 0,
                                        0,                 0,                 0, 1);
    }

    //-----------------------------------------------------------------------
    inline void CQuaternion::FromMatrix(const CMatrix4& Matrix)
    {
        float Trace = Matrix(0, 0) + Matrix(1, 1) + Matrix(2, 2) + 1;

        if (Trace > 0)
        {
            float s = 0.5f / std::sqrtf(Trace);
            x = (Matrix(2, 1) - Matrix(1, 2)) * s;
            y = (Matrix(0, 2) - Matrix(2, 0)) * s;
            z = (Matrix(1, 0) - Matrix(0, 1)) * s;
            w = 0.25f / s;
        }
        else
        {
            if ((Matrix(0, 0) > Matrix(1, 1) && (Matrix(0, 0) > Matrix(2, 2)))
            {
                float s = std::sqrt(1 + Matrix(0, 0) - Matrix(1, 1) - Matrix(2, 2)) * 2;
                x = 0.5f / s;
                y = (Matrix(0, 1) + Matrix(1, 0)) / s;
                z = (Matrix(0, 2) + Matrix(2, 0)) / s;
                w = (Matrix(1, 2) + Matrix(2, 1)) / s;
            }
            else if (Matrix(1, 1) > Matrix(2, 2))
            {
                float s = std::sqrt(1 - Matrix(0, 0) + Matrix(1, 1) - Matrix(2, 2)) * 2;
                x = (Matrix(0, 1) + Matrix(1, 0)) / s;
                y = 0.5f / s;
                z = (Matrix(1, 2) + Matrix(2, 1)) / s;
                w = (Matrix(0, 2) + Matrix(2, 0)) / s;
            }
            else
            {
                float s = std::sqrt(1 - Matrix(0, 0) - Matrix(1, 1) + Matrix(2, 2)) * 2;
                x = (Matrix(0, 2) + Matrix(2, 0)) / s;
                y = (Matrix(1, 2) + Matrix(2, 1)) / s;
                z = 0.5f / s;
                w = (Matrix(0, 1) + Matrix(1, 0)) / s;
            }
        }
    }

    //-----------------------------------------------------------------------
    inline void CQuaternion::FromAxisAngle(const TVector3F& Axis, float Angle)
    {
        float Cos = std::cos(Angle / 2);
        float Sin = std::sin(Angle / 2);

        x = Axis.x * Sin;
        y = Axis.y * Sin;
        z = Axis.z * Sin;
        w = Cos;

        Normalize();
    }

    //-----------------------------------------------------------------------
    inline void CQuaternion::ToAxisAngle(TVector3F& Axis, float& Angle) const
    {
        // Calcul de l'angle
        Angle = std::acosf(w) * 2;

        // Calcul de l'axe
        float Norm = std::sqrtf(x * x + y * y + z * z);
        if (std::fabs(Norm) > std::numeric_limits<float>::epsilon())
        {
            Axis.x = x / Norm;
            Axis.y = y / Norm;
            Axis.z = z / Norm;
        }
        else
        {
            Axis.x = 0.0f;
            Axis.y = 1.0f;
            Axis.z = 0.0f;
        }
    }

    //-----------------------------------------------------------------------
    inline void CQuaternion::FromEulerAngles(float X, float Y, float Z)
    {
        CQuaternion Qx(TVector3F(1, 0, 0), X);
        CQuaternion Qy(TVector3F(0, 1, 0), Y);
        CQuaternion Qz(TVector3F(0, 0, 1), Z);

        *this = Qx * Qy * Qz;
    }

    //-----------------------------------------------------------------------
    inline CQuaternion CQuaternion::operator *(const CQuaternion& Quaternion) const
    {
        return CQuaternion(w * Quaternion.x + x * Quaternion.w + y * Quaternion.z - z * Quaternion.y,
                          w * Quaternion.y + y * Quaternion.w + z * Quaternion.x - x * Quaternion.z,
                          w * Quaternion.z + z * Quaternion.w + x * Quaternion.y - y * Quaternion.x,
                          w * Quaternion.w - x * Quaternion.x - y * Quaternion.y - z * Quaternion.z);
    }

    //-----------------------------------------------------------------------
    inline const CQuaternion& CQuaternion::operator *=(const CQuaternion& Quaternion)
    {
        *this = *this * Quaternion;

        return *this;
    }

    //-----------------------------------------------------------------------
    std::istream& operator >>(std::istream& Stream, CQuaternion& Quaternion)
    {
        return Stream >> Quaternion.x >> Quaternion.y >> Quaternion.z >> Quaternion.w;
    }

    //-----------------------------------------------------------------------
    std::ostream& operator <<(std::ostream& Stream, const CQuaternion& Quaternion)
    {
        return Stream << Quaternion.x << " " << Quaternion.y << " " << Quaternion.z << " " << Quaternion.w;
    }

} // namespace Keos


#endif // KEOS_QUATERNION_H
