/* -*-c++-*- header - Open Producer - Copyright (C) 2002 Don Burns
 * Distributed under the terms of the GNU LIBRARY GENERAL PUBLIC LICENSE (LGPL)
 * as published by the Free Software Foundation.
 */
#ifndef PRODUCER_MATH_H
#define PRODUCER_MATH_H 1

#include <Producer/Export>

#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 {  

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:
        Matrix() {}
        Matrix(float const * const ptr )
        {
                std::copy(ptr,ptr+16,(float*)(_mat));
        }

        Matrix( float a00, float a01, float a02, float a03,
                float a10, float a11, float a12, float a13,
                float a20, float a21, float a22, float a23,
                float a30, float a31, float a32, float 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( float a00, float a01, float a02, float a03,
                float a10, float a11, float a12, float a13,
                float a20, float a21, float a22, float a23,
                float a30, float a31, float a32, float 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 preMult( const Matrix& other )
        {
            float 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 )
        {
            float 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;

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

        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;
            Vec3 u = s ^ F;

            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( float x, float y, float 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( float angle, float x, float y, float z )
        {
            float inversenorm  = 1.0/sqrt( x*x + y*y + z*z );
            float coshalfangle = cos( 0.5*angle );
            float sinhalfangle = sin( 0.5*angle );

            float 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]

            double 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( float x, float y, float 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;
            float 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=(float)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( float x, float y, float z );
        inline static Matrix rotate( float angle, float x, float y, float z);
        inline static Matrix rotate(float angle, const Vec3& axis );
        inline static Matrix scale( float sx, float sy, float sz);

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



    private:
        float _mat[4][4];

            inline float SGL_ABS(float 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( float tx, float ty, float tz)
{
    Matrix m;
    m.makeTranslate(tx,ty,tz);
    return m;
}

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

inline Matrix Matrix::scale(float sx, float sy, float 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
{
    float 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
