/* -*-c++-*- Producer - Copyright (C) 2001-2004  Don Burns
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * This library 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
 * OpenSceneGraph Public License for more details.
 */

#ifndef PRODUCER_MATH_H
#define PRODUCER_MATH_H 1

#include <Producer/Export>
#include <Producer/Types>


#include <math.h>
#include <iostream>
#include <algorithm>


#ifndef M_PI
# define M_PI           3.14159265358979323846  /* pi */
#endif
#ifndef M_PIF
# define M_PIF           3.14159265358979323846f  /* pi */
#endif

namespace Producer {  

template <typename T>
inline T deg2rad(const T x)
{ return T(x*(M_PI/180.0)); }

template <typename T>
inline T rad2deg(const T x)
{ return T(x*(180.0/M_PI)); }

template <typename T>
inline T sqr(const T x)
{ return x*x; }

class PR_EXPORT Vec3
{
    public:
        Vec3() { _v[0] = _v[1] = _v[2] = 0.0f; }

        Vec3(float x, float y, float z) { _v[0] = x; _v[1] = y; _v[2] = z; }

        inline void set( float x, float y, float z)
        {
            _v[0]=x; _v[1]=y; _v[2]=z;
        }


        inline float &x() { return _v[0]; }
        inline float &y() { return _v[1]; }
        inline float &z() { return _v[2]; }

        inline float x() const { return _v[0]; }
        inline float y() const { return _v[1]; }
        inline float z() const { return _v[2]; }

        inline float& operator [] (int i) { return _v[i]; }
        inline float operator [] (int i) const { return _v[i]; }

        /// cross product
        inline const Vec3 operator ^ (const Vec3& rhs) const
        {
            return Vec3(_v[1]*rhs._v[2]-_v[2]*rhs._v[1],
                _v[2]*rhs._v[0]-_v[0]*rhs._v[2] ,
                _v[0]*rhs._v[1]-_v[1]*rhs._v[0]);
        }

        /// binary vector subtract
        inline const Vec3 operator - (const Vec3& rhs) const
        {
            return Vec3(_v[0]-rhs._v[0], _v[1]-rhs._v[1], _v[2]-rhs._v[2]);
        }

        inline const Vec3 operator - () const
        {
            return Vec3 (-_v[0], -_v[1], -_v[2]);
        }

        /// Length of the vector = sqrt( vec . vec )
        inline float length() const
        {
            return sqrt( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] );
        }

        /** normalize the vector so that it has length unity
            returns the previous length of the vector*/
        inline float normalize()
        {
            float norm = Vec3::length();
            if (norm>0.0f)
            {
                _v[0] /= norm;
                _v[1] /= norm;
                _v[2] /= norm;
            }
            return( norm );
        }

        /// multiply by scalar
        inline const Vec3 operator * (float rhs) const
        {
            return Vec3(_v[0]*rhs, _v[1]*rhs, _v[2]*rhs);
        }

        /// unary multiply by scalar
        inline Vec3& operator *= (float rhs)
        {
            _v[0]*=rhs;
            _v[1]*=rhs;
            _v[2]*=rhs;
            return *this;
        }

    float _v[3];
};

#define SET_ROW(row, v1, v2, v3, v4 )    \
    _mat[(row)][0] = (v1); \
    _mat[(row)][1] = (v2); \
    _mat[(row)][2] = (v3); \
    _mat[(row)][3] = (v4);

#define INNER_PRODUCT(a,b,r,c) \
     ((a)._mat[r][0] * (b)._mat[0][c]) \
    +((a)._mat[r][1] * (b)._mat[1][c]) \
    +((a)._mat[r][2] * (b)._mat[2][c]) \
    +((a)._mat[r][3] * (b)._mat[3][c])



class PR_EXPORT Matrix {
    public:
   
        typedef double value_type;

        Matrix() {}
        Matrix(float const * const ptr ) { set(ptr); }
        Matrix(double const * const ptr )  { set(ptr); }

        Matrix( value_type a00, value_type a01, value_type a02, value_type a03,
                value_type a10, value_type a11, value_type a12, value_type a13,
                value_type a20, value_type a21, value_type a22, value_type a23,
                value_type a30, value_type a31, value_type a32, value_type a33)
        {
                SET_ROW(0, a00, a01, a02, a03 )
                SET_ROW(1, a10, a11, a12, a13 )
                SET_ROW(2, a20, a21, a22, a23 )
                SET_ROW(3, a30, a31, a32, a33 )
        }

        void makeIdentity()
        {
                SET_ROW(0,    1, 0, 0, 0 )
                SET_ROW(1,    0, 1, 0, 0 )
                SET_ROW(2,    0, 0, 1, 0 )
                SET_ROW(3,    0, 0, 0, 1 )
        }

       void set( value_type a00, value_type a01, value_type a02, value_type a03,
                 value_type a10, value_type a11, value_type a12, value_type a13,
                 value_type a20, value_type a21, value_type a22, value_type a23,
                 value_type a30, value_type a31, value_type a32, value_type a33)
        {
                SET_ROW(0, a00, a01, a02, a03 )
                SET_ROW(1, a10, a11, a12, a13 )
                SET_ROW(2, a20, a21, a22, a23 )
                SET_ROW(3, a30, a31, a32, a33 )
        }

        void set(float const * const ptr )
        {
            value_type* local_ptr = (value_type*)_mat;
            for(int i=0;i<16;++i) local_ptr[i]=(value_type)ptr[i];
        }
        void set(double const * const ptr )
        {
            value_type* local_ptr = (value_type*)_mat;
            for(int i=0;i<16;++i) local_ptr[i]=(value_type)ptr[i];
        }

        value_type * ptr() { return (value_type*)_mat; }
        const value_type * ptr() const { return (const value_type *)_mat; }

        void preMult( const Matrix& other )
        {
            value_type t[4];
            for(int col=0; col<4; ++col) {
                t[0] = INNER_PRODUCT( other, *this, 0, col );
                t[1] = INNER_PRODUCT( other, *this, 1, col );
                t[2] = INNER_PRODUCT( other, *this, 2, col );
                t[3] = INNER_PRODUCT( other, *this, 3, col );
                _mat[0][col] = t[0];
                _mat[1][col] = t[1];
                _mat[2][col] = t[2];
                _mat[3][col] = t[3];
            }
        }   

        void postMult( const Matrix &other )
        {
            value_type t[4];
            for(int row=0; row<4; ++row)
            {
                t[0] = INNER_PRODUCT( *this, other, row, 0 );
                t[1] = INNER_PRODUCT( *this, other, row, 1 );
                t[2] = INNER_PRODUCT( *this, other, row, 2 );
                t[3] = INNER_PRODUCT( *this, other, row, 3 );
                SET_ROW(row, t[0], t[1], t[2], t[3] )
            }
        }

        inline Vec3 preMult( const Vec3& v ) const;

        void makeLookAt( const Vec3& eye,
                         const Vec3& center,
                         const Vec3& up )
        {
            Vec3 F = center - eye;
            Vec3 UP = up;
            F.normalize();
            UP.normalize();

            Vec3 s = F ^ UP;
            s.normalize();
            Vec3 u = s ^ F;
            u.normalize();

            set( s[0], u[0], -F[0], 0,
                 s[1], u[1], -F[1], 0,
                 s[2], u[2], -F[2], 0,
                 0, 0, 0, 1
            );
            preMult(Matrix::translate(-eye ));
        }

        void makeTranslate( value_type x, value_type y, value_type z )
        {
                SET_ROW(0,    1, 0, 0, 0 )
                SET_ROW(1,    0, 1, 0, 0 )
                SET_ROW(2,    0, 0, 1, 0 )
                SET_ROW(3,    x, y, z, 1 )
        }


        void makeRotate( value_type angle, value_type x, value_type y, value_type z )
        {
            value_type inversenorm  = 1.0/sqrt( x*x + y*y + z*z );
            value_type coshalfangle = cos( 0.5*angle );
            value_type sinhalfangle = sin( 0.5*angle );

            value_type fv[4];
            fv[0] = x * sinhalfangle * inversenorm;
            fv[1] = y * sinhalfangle * inversenorm;
            fv[2] = z * sinhalfangle * inversenorm;
            fv[3] = coshalfangle;

            // Source: Gamasutra, Rotating Objects Using Quaternions
            //
            //http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
#define QX  fv[0]
#define QY  fv[1]
#define QZ  fv[2]
#define QW  fv[3]

            value_type wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

            // calculate coefficients
            x2 = QX + QX;
            y2 = QY + QY;
            z2 = QZ + QZ;

            xx = QX * x2;
            xy = QX * y2;
            xz = QX * z2;

            yy = QY * y2;
            yz = QY * z2;
            zz = QZ * z2;

            wx = QW * x2;
            wy = QW * y2;
            wz = QW * z2;

            // Note.  Gamasutra gets the matrix assignments inverted, resulting
            // in left-handed rotations, which is contrary to OpenGL and OSG's 
            // methodology.  The matrix assignment has been altered in the next
            // few lines of code to do the right thing.
            // Don Burns - Oct 13, 2001
            _mat[0][0] = 1.0f - (yy + zz);
            _mat[1][0] = xy - wz;
            _mat[2][0] = xz + wy;
            _mat[3][0] = 0.0f;

            _mat[0][1] = xy + wz;
            _mat[1][1] = 1.0f - (xx + zz);
            _mat[2][1] = yz - wx;
            _mat[3][1] = 0.0f;

            _mat[0][2] = xz - wy;
            _mat[1][2] = yz + wx;
            _mat[2][2] = 1.0f - (xx + yy);
            _mat[3][2] = 0.0f;

            _mat[0][3] = 0;
            _mat[1][3] = 0;
            _mat[2][3] = 0;
            _mat[3][3] = 1;

    #undef QX 
    #undef QY 
    #undef QZ 
    #undef QW 
        }

        void makeScale( value_type x, value_type y, value_type z )
        {
                SET_ROW(0,    x, 0, 0, 0 )
                SET_ROW(1,    0, y, 0, 0 )
                SET_ROW(2,    0, 0, z, 0 )
                SET_ROW(3,    0, 0, 0, 1 )
        }



        void mult( const Matrix &lhs, const Matrix &rhs )
        {               
            if (&lhs==this)
            {
                postMult(rhs);
                return;
            }
            if (&rhs==this)
            {
                preMult(lhs);
                return;
            }

        // PRECONDITION: We assume neither &lhs nor &rhs == this
        // if it did, use preMult or postMult instead
            _mat[0][0] = INNER_PRODUCT(lhs, rhs, 0, 0);
            _mat[0][1] = INNER_PRODUCT(lhs, rhs, 0, 1);
            _mat[0][2] = INNER_PRODUCT(lhs, rhs, 0, 2);
            _mat[0][3] = INNER_PRODUCT(lhs, rhs, 0, 3);
            _mat[1][0] = INNER_PRODUCT(lhs, rhs, 1, 0);
            _mat[1][1] = INNER_PRODUCT(lhs, rhs, 1, 1);
            _mat[1][2] = INNER_PRODUCT(lhs, rhs, 1, 2);
            _mat[1][3] = INNER_PRODUCT(lhs, rhs, 1, 3);
            _mat[2][0] = INNER_PRODUCT(lhs, rhs, 2, 0);
            _mat[2][1] = INNER_PRODUCT(lhs, rhs, 2, 1);
            _mat[2][2] = INNER_PRODUCT(lhs, rhs, 2, 2);
            _mat[2][3] = INNER_PRODUCT(lhs, rhs, 2, 3);
            _mat[3][0] = INNER_PRODUCT(lhs, rhs, 3, 0);
            _mat[3][1] = INNER_PRODUCT(lhs, rhs, 3, 1);
            _mat[3][2] = INNER_PRODUCT(lhs, rhs, 3, 2);
            _mat[3][3] = INNER_PRODUCT(lhs, rhs, 3, 3);
        }



        inline Matrix operator * ( const Matrix &m ) const
        {
            Matrix r;
            r.mult(*this,m);
            return  r;
        }

        inline void operator *= ( const Matrix& other )
        {    
            if( this == &other ) 
            {
                Matrix temp(other);
                postMult( temp );
            }
            else postMult( other );
        }

    #ifndef SGL_SWAP
    #define SGL_SWAP(a,b,temp) ((temp)=(a),(a)=(b),(b)=(temp))
    #endif

        bool invert( const Matrix& mat )
        {
            if (&mat==this) {
               Matrix tm(mat);
               return invert(tm);
            }

            unsigned int indxc[4], indxr[4], ipiv[4];
            unsigned int i,j,k,l,ll;
            unsigned int icol = 0;
            unsigned int irow = 0;
            value_type temp, pivinv, dum, big;

            // copy in place this may be unnecessary
            *this = mat;

            for (j=0; j<4; j++) ipiv[j]=0;

            for(i=0;i<4;i++)
            {
               big=(value_type)0.0;
               for (j=0; j<4; j++)
                  if (ipiv[j] != 1)
                     for (k=0; k<4; k++)
                     {
                        if (ipiv[k] == 0)
                        {
                           if (SGL_ABS(operator()(j,k)) >= big)
                           {
                              big = SGL_ABS(operator()(j,k));
                              irow=j;
                              icol=k;
                           }
                        }
                        else if (ipiv[k] > 1)
                           return false;
                     }
               ++(ipiv[icol]);
               if (irow != icol)
                  for (l=0; l<4; l++) SGL_SWAP(operator()(irow,l),
                                               operator()(icol,l),
                                               temp);

               indxr[i]=irow;
               indxc[i]=icol;
               if (operator()(icol,icol) == 0)
                  return false;

               pivinv = 1.0/operator()(icol,icol);
               operator()(icol,icol) = 1;
               for (l=0; l<4; l++) operator()(icol,l) *= pivinv;
               for (ll=0; ll<4; ll++)
                  if (ll != icol)
                  {
                     dum=operator()(ll,icol);
                     operator()(ll,icol) = 0;
                     for (l=0; l<4; l++) operator()(ll,l) -= operator()(icol,l)*dum;
                  }
            }
            for (int lx=4; lx>0; --lx)
            {
               if (indxr[lx-1] != indxc[lx-1])
                  for (k=0; k<4; k++) SGL_SWAP(operator()(k,indxr[lx-1]),
                                               operator()(k,indxc[lx-1]),temp);
            }

            return true;
        }

        inline static Matrix translate( const Vec3 &);
        inline static Matrix translate( value_type x, value_type y, value_type z );
        inline static Matrix rotate( value_type angle, value_type x, value_type y, value_type z);
        inline static Matrix rotate(value_type angle, const Vec3& axis );
        inline static Matrix scale( value_type sx, value_type sy, value_type sz);

        inline value_type& operator()(int row, int col) { return _mat[row][col]; }
        inline value_type operator()(int row, int col) const { return _mat[row][col]; }

        /** call glLoadMatixf with this matrix.*/
        void glLoadMatrix() const { glLoadMatrix(ptr()); }
        
        /** call glMultMatixf with this matrix.*/
        void glMultMatrix() const { glMultMatrix(ptr()); }

        static inline void glLoadMatrix(const float* ptr) { glLoadMatrixf(ptr); }

        static inline void glLoadMatrix(const double* ptr) { glLoadMatrixd(ptr); }

        static inline void glMultMatrix(const float* ptr) { glMultMatrixf(ptr); }

        static inline void glMultMatrix(const double* ptr) { glMultMatrixd(ptr); }

    protected:
    
        value_type _mat[4][4];

        inline value_type SGL_ABS(value_type a)
        {
               return (a >= 0.0f ? a : -a);
        }
};

inline Matrix Matrix::translate(const Vec3& v )
{
    return translate(v.x(), v.y(), v.z() );
}


inline Matrix Matrix::translate( value_type tx, value_type ty, value_type tz)
{
    Matrix m;
    m.makeTranslate(tx,ty,tz);
    return m;
}

inline Matrix Matrix::rotate(value_type angle, value_type x, value_type y, value_type z )
{               
    Matrix m;
    m.makeRotate(angle,x,y,z);
    return m;
}

inline Matrix Matrix::rotate(value_type angle, const Vec3& axis )
{
    return rotate(angle,axis.x(),axis.y(),axis.z());
}

inline Matrix Matrix::scale(value_type sx, value_type sy, value_type sz)
{       
    Matrix m;
    m.makeScale(sx,sy,sz);
    return m;
}



inline Vec3 operator* (const Vec3& v, const Matrix& m )
{
    return m.preMult(v);
}

inline Vec3 Matrix::preMult( const Vec3& v ) const
{
    value_type d = 1.0f/(_mat[0][3]*v.x()+_mat[1][3]*v.y()+_mat[2][3]*v.z()+_mat[3][3]) ;
    return Vec3( (_mat[0][0]*v.x() + _mat[1][0]*v.y() + _mat[2][0]*v.z() + _mat[3][0])*d,
        (_mat[0][1]*v.x() + _mat[1][1]*v.y() + _mat[2][1]*v.z() + _mat[3][1])*d,
        (_mat[0][2]*v.x() + _mat[1][2]*v.y() + _mat[2][2]*v.z() + _mat[3][2])*d);
}



inline std::ostream& operator << (std::ostream& output, const Vec3& vec)
{
    output << vec._v[0] << " "
           << vec._v[1] << " "
           << vec._v[2];
    return output;      // to enable cascading
}



inline std::ostream& operator<< (std::ostream& os, const Matrix& m )
{
    os << "{"<<std::endl;
    for(int row=0; row<4; ++row) {
        os << "\t";
        for(int col=0; col<4; ++col)
            os << m(row,col) << " ";
        os << std::endl;
    }
    os << "}" << std::endl;
    return os;
}


}

#endif
