#include <string.h>
#include <iostream>
#include "mof/Matrix3D.hpp"
#include "mof/utilities.hpp"
#include <d3dx9.h>
#include <mof/ConsoleIO.hpp>

const int DIMENSION = 3;


mof::Matrix3D::Matrix3D()
: m_pImpl(new Array)
{
    for(int i = 0 ; i <= DIMENSION ; ++i){
        for(int j = 0 ; j <= DIMENSION ; ++j){
            if(i == j)m_pImpl->elements[i][j] = 1;
            else m_pImpl->elements[i][j] = 0;
        }
    }
}

mof::Matrix3D::Matrix3D(const mof::Matrix3D::Array & arr)
: m_pImpl(new Array)
{
    *m_pImpl = arr;
}


mof::Matrix3D::~Matrix3D(){
}


mof::real mof::Matrix3D::at(int row , int column) const{
    return m_pImpl->elements[row][column];
}

mof::Matrix3D::Array mof::Matrix3D::getArray() const{
    return *m_pImpl;
}


mof::Matrix3D mof::Matrix3D::createIdentity(){
    return mof::Matrix3D();
}


mof::Matrix3D mof::Matrix3D::createTransposed(const mof::Matrix3D& matrix){
    Matrix3D transposed;
    for(int i = 0 ; i <= DIMENSION ; i++){
        for(int j = 0 ; j <= DIMENSION ; j++){
            transposed.m_pImpl->elements[j][i] = matrix.m_pImpl->elements[i][j];
        }
    }
    return transposed;
}


mof::Matrix3D mof::Matrix3D::createRotation(const mof::Vector3D& radians){
    mof::real cosx = cosf(radians.x);
    mof::real sinx = sinf(radians.x);
    mof::real cosy = cosf(radians.y);
    mof::real siny = sinf(radians.y);
    mof::real cosz = cosf(radians.z);
    mof::real sinz = sinf(radians.z);
    mof::Matrix3D::Array x = 
    {{
        { 1.f , 0.f   , 0.f   , 0.f } ,
        { 0.f , cosx  , sinx  , 0.f } ,
        { 0.f , -sinx , cosx  , 0.f } ,
        { 0.f , 0.f   , 0.f   , 1.f }  
    }};
    mof::Matrix3D::Array y = 
    {{
        { cosy , 0.f , -siny , 0.f } ,
        { 0.f  , 1.f , 0.f   , 0.f } ,
        { siny , 0.f , cosy  , 0.f } ,
        { 0.f  , 0.f , 0.f   , 1.f }
    }};
    mof::Matrix3D::Array z = 
    {{
        { cosz  , sinz , 0.f , 0.f } ,
        { -sinz , cosz , 0.f , 0.f } ,
        { 0.f   , 0.f  , 1.f , 0.f } ,
        { 0.f   , 0.f  , 0.f , 1.f }  
    }};
    return mof::Matrix3D(x) * mof::Matrix3D(y) * mof::Matrix3D(z); //TODO 積をハードコーディング
}


mof::Matrix3D mof::Matrix3D::createTranslation(const mof::Vector3D& position){
    Matrix3D matrix;
    matrix.m_pImpl->elements[DIMENSION][0] = position.x;
    matrix.m_pImpl->elements[DIMENSION][1] = position.y;
    matrix.m_pImpl->elements[DIMENSION][2] = position.z;
    return matrix;
}

mof::Matrix3D mof::Matrix3D::createScaling(const mof::Vector3D& scaling){
    Matrix3D matrix;
    matrix.m_pImpl->elements[0][0] = scaling.x;
    matrix.m_pImpl->elements[1][1] = scaling.y;
    matrix.m_pImpl->elements[2][2] = scaling.z;
    return matrix;
}



mof::Matrix3D mof::Matrix3D::createLookAtLH
(
    const mof::Vector3D& eye ,
    const mof::Vector3D& lookAt ,
    const mof::Vector3D& up
)
{
    D3DXVECTOR3 vEye(eye.x , eye.y , eye.z);
    D3DXVECTOR3 vLookAt(lookAt.x , lookAt.y , lookAt.z);
    D3DXVECTOR3 vUp(up.x , up.y , up.z);
    D3DXMATRIX   m;
    mof::Matrix3D::Array array;
    D3DXMatrixLookAtLH( &m , &vEye , &vLookAt , &vUp );
    for( int i = 0 ; i <= DIMENSION ; i++ )
    {
        for( int j = 0 ; j <= DIMENSION ; j++ )
        {
            array.elements[i][j] = m.m[i][j];
        }
    }
    return mof::Matrix3D( array );
}

mof::Vector3D mof::Matrix3D::getTranslation(const mof::Matrix3D& matrix){
    return mof::Vector3D(
        matrix.m_pImpl->elements[DIMENSION][0] ,
        matrix.m_pImpl->elements[DIMENSION][1] ,
        matrix.m_pImpl->elements[DIMENSION][2]
        );
}
        
mof::Vector3D mof::Matrix3D::getDiagonal() const{
    return mof::Vector3D(
        m_pImpl->elements[0][0] ,
        m_pImpl->elements[1][1] ,
        m_pImpl->elements[2][2]
        );
}
        


mof::Matrix3D mof::Matrix3D::operator *(const mof::Matrix3D& matrix) const{
    mof::Matrix3D multiplied;
    for(int i = 0 ; i <= DIMENSION ; i++){
        for(int j = 0 ; j <= DIMENSION ; j++){
            mof::real sum = 0;
            for(int k = 0 ; k <= DIMENSION ; k++){
                sum += m_pImpl->elements[i][k] * matrix.m_pImpl->elements[k][j];
            }
            multiplied.m_pImpl->elements[i][j] = sum;
        }
    }
    return multiplied;
}


mof::Matrix3D mof::Matrix3D::operator +(const mof::Matrix3D& matrix) const{
    mof::Matrix3D result;
    for(int i = 0 ; i <= DIMENSION ; i++){
        for(int j = 0 ; j <= DIMENSION ; j++){
            result.m_pImpl->elements[i][j] = 
                m_pImpl->elements[i][j] + matrix.m_pImpl->elements[i][j];
        }
    }
    return result;
}


mof::Matrix3D mof::Matrix3D::operator -(const mof::Matrix3D& matrix) const{
    mof::Matrix3D result;
    for(int i = 0 ; i <= DIMENSION ; i++){
        for(int j = 0 ; j <= DIMENSION ; j++){
            result.m_pImpl->elements[i][j] = 
                m_pImpl->elements[i][j] - matrix.m_pImpl->elements[i][j];
        }
    }
    return result;
}


mof::Matrix3D mof::operator *(const mof::Matrix3D& matrix , mof::real f){
    mof::Matrix3D result;
    for(int i = 0 ; i <= DIMENSION ; i++){
        for(int j = 0 ; j <= DIMENSION ; j++){
            result.m_pImpl->elements[i][j] = 
                matrix.m_pImpl->elements[i][j] * f;
        }
    }
    return result;
}
        
mof::Matrix3D mof::operator *(mof::real f , mof::Matrix3D const& matrix){
    return matrix * f;
}


mof::Vector3D mof::operator *(const mof::Vector3D& vec , const mof::Matrix3D& matrix) {
    mof::real input[4] = {vec.x , vec.y , vec.z , 1};
    mof::real output[4];
    for(int i = 0 ; i < DIMENSION ; i++){
        mof::real sum = 0;
        for(int k = 0 ; k <= DIMENSION ; k++){
            sum += input[k] * matrix.at(k , i);
        }
        output[i] = sum;
        
    }
    return mof::Vector3D(output[0] , output[1] , output[2]);
}


std::ostream& mof::operator <<(std::ostream& os , const mof::Matrix3D& mat){
    for(int i = 0 ; i <= DIMENSION ; i++){
        for(int j = 0 ; j <= DIMENSION ; j++){
            if(j != 0)os << " , ";
            os << mat.m_pImpl->elements[i][j];
        }
        os << '\n';
    }
    return os;
}
