/*
 * Copyright Staffan Gimåker 2006-2008, Anders Boberg 2006-2008.
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef __PEEKABOT_MATRIX_HH
#define __PEEKABOT_MATRIX_HH

/**
 * \file 4x4 and 3x3 matrix implementations.
 */

#include <cmath>
#include <cstring>
#include <algorithm>
#include "Types.hh"
#include "Vector.hh"


namespace peekabot
{


    template<class S = real_unit>
    class Matrix4
    {
    public:
        typedef S UnitType;


        // Matrix stored in column-major order, as OpenGL expects them.
        Vector4<S> m_cols[4];

        Matrix4() throw()
        {
        }


        template<class U>
        Matrix4(const Matrix4<U> &mat) throw()
        {
            for( int i = 0; i < 4; ++i )
                m_cols[i] = mat.m_cols[i];
        }

        
        Matrix4(const S *a) throw()
        {
            memcpy(m_cols, a, 4*4*sizeof(S));
        }

        
        template<class U>
        Matrix4(const Vector4<U> &col1,
                const Vector4<U> &col2,
                const Vector4<U> &col3,
                const Vector4<U> &col4) throw()
        {
            m_cols[0] = col1;
            m_cols[1] = col2;
            m_cols[2] = col3;
            m_cols[3] = col4;
        }


        template<class U>
        Matrix4(const Vector4<U> cols[]) throw()
        {
            m_cols[0] = cols[0];
            m_cols[1] = cols[1];
            m_cols[2] = cols[2];
            m_cols[3] = cols[3];
        }


        Vector4<S> &col(int col) throw()
        {
            return m_cols[col];
        }


        const Vector4<S> &col(int col) const throw()
        {
            return m_cols[col];
        }


        Vector4<S> &operator()(int col) throw()
        {
            return m_cols[col];
        }


        const Vector4<S> &operator()(int col) const throw()
        {
            return m_cols[col];
        }


        S &operator()(int row, int col) throw()
        {
            return m_cols[col](row);
        }


        const S &operator()(int row, int col) const throw()
        {
            return m_cols[col](row);
        }


        Matrix4 &transpose() throw()
        {
            for( int r = 0; r < 4; ++r )
                for( int c = 0; c < 4; ++c )
                    if( c > r )
                        std::swap((*this)(r,c), (*this)(c,r));
            return *this;
        }


        Matrix4 T() throw()
        {
            Matrix4 m;
            for( int r = 0; r < 4; ++r )
                for( int c = 0; c < 4; ++c )
                    m(r,c) = (*this)(c,r);
            return m;
        }


        static const Matrix4 &identity() throw()
        {
            static Matrix4 eye = Matrix4(Vector4<S>(1,0,0,0),
                                         Vector4<S>(0,1,0,0),
                                         Vector4<S>(0,0,1,0),
                                         Vector4<S>(0,0,0,1));
            return eye;
        }


        Matrix4 &load_identity() throw()
        {
            *this = identity();
            return *this;
        }


        Matrix4 &operator*=(S scalar) throw()
        {
            for( int c = 0; c < 4; ++c )
                m_cols[c] *= scalar;
        }


        Matrix4 &operator/=(S scalar) throw()
        {
            for( int c = 0; c < 4; ++c )
                m_cols[c] /= scalar;
        }


        Matrix4 operator*(S scalar) const throw()
        {
            return Matrix4(m_cols[0]*scalar,
                           m_cols[1]*scalar,
                           m_cols[2]*scalar,
                           m_cols[3]*scalar);
        }


        Matrix4 operator/(S scalar) const throw()
        {
            return Matrix4(m_cols[0]/scalar,
                           m_cols[1]/scalar,
                           m_cols[2]/scalar,
                           m_cols[3]/scalar);
        }


        template<class U>
        Matrix4 operator*(const Matrix4<U> &m) const throw()
        {
            Matrix4 ret;

            for( int i = 0; i < 4; ++i ) // row
            {
                for( int j = 0; j < 4; ++j ) // col
                {
                    // ret(i,j) = this->row(i) . m->col(j)
                    ret(i,j) = 0;
                    for( int k = 0; k < 4; ++k )
                        ret(i,j) += m_cols[k](i) * m(k,j);
                }
            }
            
            return ret;
        }


        template<class U>
        Matrix4 &operator*=(const Matrix4<U> &m) throw()
        {
            return (*this = m* (*this));
        }


        Matrix4 inverse() const throw()
        {
            // Could be optimized to one messy (!) return Matrix4r(...) call.

            Matrix4 rinv;

            for( int r = 0; r < 3; ++r )
                for( int c = 0; c < 3; ++c )
                    rinv(r,c) = (*this)(c,r);
            for( int c = 0; c < 3; ++c )
                rinv(3,c) = 0;
            rinv(3) = Vector4<S>(0,0,0,1);

            return rinv*Matrix4<S>(Vector4<S>(1,0,0,0),
                                   Vector4<S>(0,1,0,0),
                                   Vector4<S>(0,0,1,0),
                                   Vector4<S>(-(*this)(0,3),
                                              -(*this)(1,3),
                                              -(*this)(2,3),1));
        }


        /*Matrix4 &invert() throw()
        {
            // TODO

            return *this;
        }*/


        template<class U>
        Vector4<U> operator*(const Vector4<U> &v) const throw()
        {
            return Vector4<U>(m_cols[0](0) * v(0) + m_cols[1](0) * v(1) + m_cols[2](0) * v(2) + m_cols[3](0) * v(3),
                              m_cols[0](1) * v(0) + m_cols[1](1) * v(1) + m_cols[2](1) * v(2) + m_cols[3](1) * v(3),
                              m_cols[0](2) * v(0) + m_cols[1](2) * v(1) + m_cols[2](2) * v(2) + m_cols[3](2) * v(3),
                              m_cols[0](3) * v(0) + m_cols[1](3) * v(1) + m_cols[2](3) * v(2) + m_cols[3](3) * v(3));
        }


        bool operator!=(const Matrix4<S> &m) const throw()
        {
            for( int c = 0; c < 4; ++c )
                for( int r = 0; r < 4; ++r )
                    if( m(r,c) != m_cols[c](r) )
                        return true;
            return false;
        }

        bool operator==(const Matrix4<S> &m) const throw()
        {
            return !(*this != m);
        }


        bool equals(const Matrix4<S> &m, S limit) const throw()
        {
            for( int c = 0; c < 4; ++c )
                for( int r = 0; r < 4; ++r )
                    if( (S)fabs( m(r,c) - m_cols[c](r) ) > limit )
                        return false;
            return true;
        }

        




        static Matrix4<S> rotate_x(S rad) throw()
        {
            return Matrix4<S>(
                Vector4<S>(1,         0,        0, 0),
                Vector4<S>(0,  cos(rad), sin(rad), 0),
                Vector4<S>(0, -sin(rad), cos(rad), 0),
                Vector4<S>(0,         0,        0, 1));
        }

        static Matrix4<S> rotate_y(S rad) throw()
        {
            return Matrix4<S>(
                Vector4<S>(cos(rad), 0, -sin(rad), 0),
                Vector4<S>(       0, 1,         0, 0),
                Vector4<S>(sin(rad), 0,  cos(rad), 0),
                Vector4<S>(       0, 0,         0, 1));
        }

        static Matrix4<S> rotate_z(S rad)
        {
            return Matrix4<S>(
                Vector4<S>( cos(rad), sin(rad), 0, 0),
                Vector4<S>(-sin(rad), cos(rad), 0, 0),
                Vector4<S>(        0,        0, 1, 0),
                Vector4<S>(        0,        0, 0, 1));
        }

        /**
         * \pre x,y,z must form a unit length vector in R3.
         */
        template<class U>
        static Matrix4<S> rotate(U x, U y, U z, S rad)
        {
            S c = cos(rad);
            S s = sin(rad);
            return Matrix4<S>(
                Vector4<S>(  x*x*(1-c)+c, x*y*(1-c)+z*s, x*z*(1-c)-y*s, 0),
                Vector4<S>(x*y*(1-c)-z*s,   y*y*(1-c)+c, y*z*(1-c)+x*s, 0),
                Vector4<S>(x*z*(1-c)+y*s, y*z*(1-c)-x*s,   z*z*(1-c)+c, 0),
                Vector4<S>(            0,             0,             0, 1));
        }

        template<class U>
        static Matrix4<S> rotate(const Vector3<U> &axis, S rad)
        {
            U axis_norm = axis.norm();
            return rotate(
                axis(0)/axis_norm,
                axis(1)/axis_norm,
                axis(2)/axis_norm, rad);
        }

        template<class U>
        static Matrix4<S> rotate(const Vector4<U> &axis, S rad)
        {
            Vector3<U> axis_ = axis;
            axis_.normalize();
            return rotate(axis_, rad);
        }


        template<class U>
        static Matrix4<S> translate(const Vector4<U> &p)
        {
            return Matrix4<S>(
                Vector4<S>(1, 0, 0, 0),
                Vector4<S>(0, 1, 0, 0),
                Vector4<S>(0, 0, 1, 0),
                Vector4<S>(p(0), p(1), p(2), 1));
        }

        template<class U>
        static Matrix4<S> translate(const Vector3<U> &p)
        {
            return Matrix4<S>(
                Vector4<S>(1, 0, 0, 0),
                Vector4<S>(0, 1, 0, 0),
                Vector4<S>(0, 0, 1, 0),
                Vector4<S>(p, 1));
        }

        static Matrix4<S> translate(S x, S y, S z)
        {
            return Matrix4<S>(
                Vector4<S>(1, 0, 0, 0),
                Vector4<S>(0, 1, 0, 0),
                Vector4<S>(0, 0, 1, 0),
                Vector4<S>(x, y, z, 1));
        }

        static Matrix4<S> scale(S x, S y, S z)
        {
            return Matrix4<S>(
                Vector4<S>(x, 0, 0, 0),
                Vector4<S>(0, y, 0, 0),
                Vector4<S>(0, 0, z, 0),
                Vector4<S>(0, 0, 0, 1));
        }
    };




    template<class T>
    std::ostream &operator<<(std::ostream &os, const Matrix4<T> &m)
    {
        os << "[" 
           << m(0,0) << " " 
           << m(0,1) << " " 
           << m(0,2) << " " 
           << m(0,3) << "\n";
        
        os << " " 
           << m(1,0) << " " 
           << m(1,1) << " " 
           << m(1,2) << " " 
           << m(1,3) << "\n";
        
        os << " " 
           << m(2,0) << " " 
           << m(2,1) << " " 
           << m(2,2) << " " 
           << m(2,3) << "\n";
        
        os << " " 
           << m(3,0) << " " 
           << m(3,1) << " " 
           << m(3,2) << " " 
           << m(3,3) << "]";

        return os;
    }



    typedef Matrix4<real_unit> Matrix4ru;

    typedef Matrix4<real32> Matrix4f;

    typedef Matrix4<real64> Matrix4d;

    /**
     * \brief The Matrix type used internally in the render engine. (deprecated)
     */
    typedef Matrix4ru Matrix4r;


}


#endif // __PEEKABOT_MATRIX_HH
