#include "Matrix.h"
#include <math.h>       /* fabs() */
#include <iostream>
#include <iomanip>      /* setprecision() */

#define FP_EQ(x,y) (fabs((x)-(y)) < 0.00001)

Matrix::Matrix()
{
    /* set matrix to the identity matrix */
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 4; j++)
        {
            m_matrix[i][j] = i == j ? 1.0 : 0.0;
        }
    }
    m_inverse_calculated = false;
}

Matrix Matrix::identity()
{
    Matrix res;
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 4; j++)
        {
            res[i][j] = i == j ? 1.0 : 0.0;
        }
    }
    return res;
}

/* Formulas from http://www.cvl.iis.u-tokyo.ac.jp/~miyazaki/tech/teche23.html */
double Matrix::determinant()
{
    return m_matrix[0][0] * m_matrix[1][1] * m_matrix[2][2] * m_matrix[3][3]
         + m_matrix[0][0] * m_matrix[1][2] * m_matrix[2][3] * m_matrix[3][1]
         + m_matrix[0][0] * m_matrix[1][3] * m_matrix[2][1] * m_matrix[3][2]
         + m_matrix[0][1] * m_matrix[1][0] * m_matrix[2][3] * m_matrix[3][2]

         + m_matrix[0][1] * m_matrix[1][2] * m_matrix[2][0] * m_matrix[3][3]
         + m_matrix[0][1] * m_matrix[1][3] * m_matrix[2][2] * m_matrix[3][0]
         + m_matrix[0][2] * m_matrix[1][0] * m_matrix[2][1] * m_matrix[3][3]
         + m_matrix[0][2] * m_matrix[1][1] * m_matrix[2][3] * m_matrix[3][0]

         + m_matrix[0][2] * m_matrix[1][3] * m_matrix[2][0] * m_matrix[3][1]
         + m_matrix[0][3] * m_matrix[1][0] * m_matrix[2][2] * m_matrix[3][1]
         + m_matrix[0][3] * m_matrix[1][1] * m_matrix[2][0] * m_matrix[3][2]
         + m_matrix[0][3] * m_matrix[1][2] * m_matrix[2][1] * m_matrix[3][0]

         - m_matrix[0][0] * m_matrix[1][1] * m_matrix[2][3] * m_matrix[3][2]
         - m_matrix[0][0] * m_matrix[1][2] * m_matrix[2][1] * m_matrix[3][3]
         - m_matrix[0][0] * m_matrix[1][3] * m_matrix[2][2] * m_matrix[3][1]
         - m_matrix[0][1] * m_matrix[1][0] * m_matrix[2][2] * m_matrix[3][3]

         - m_matrix[0][1] * m_matrix[1][2] * m_matrix[2][3] * m_matrix[3][0]
         - m_matrix[0][1] * m_matrix[1][3] * m_matrix[2][0] * m_matrix[3][2]
         - m_matrix[0][2] * m_matrix[1][0] * m_matrix[2][3] * m_matrix[3][1]
         - m_matrix[0][2] * m_matrix[1][1] * m_matrix[2][0] * m_matrix[3][3]

         - m_matrix[0][2] * m_matrix[1][3] * m_matrix[2][1] * m_matrix[3][0]
         - m_matrix[0][3] * m_matrix[1][0] * m_matrix[2][1] * m_matrix[3][2]
         - m_matrix[0][3] * m_matrix[1][1] * m_matrix[2][2] * m_matrix[3][0]
         - m_matrix[0][3] * m_matrix[1][2] * m_matrix[2][0] * m_matrix[3][1];
}

/* Formulas from http://www.cvl.iis.u-tokyo.ac.jp/~miyazaki/tech/teche23.html */
void Matrix::calculateInverse()
{
    if (m_inverse_calculated)
        return;
    m_inverse_calculated = true;
    m_inverse_valid = false;
    double det = determinant();
    if (det == 0.0)
        return;
    m_inverse[0][0] = m_matrix[1][1] * m_matrix[2][2] * m_matrix[3][3]
                    + m_matrix[1][2] * m_matrix[2][3] * m_matrix[3][1]
                    + m_matrix[1][3] * m_matrix[2][1] * m_matrix[3][2]
                    - m_matrix[1][1] * m_matrix[2][3] * m_matrix[3][2]
                    - m_matrix[1][2] * m_matrix[2][1] * m_matrix[3][3]
                    - m_matrix[1][3] * m_matrix[2][2] * m_matrix[3][1];
    m_inverse[0][1] = m_matrix[0][1] * m_matrix[2][3] * m_matrix[3][2]
                    + m_matrix[0][2] * m_matrix[2][1] * m_matrix[3][3]
                    + m_matrix[0][3] * m_matrix[2][2] * m_matrix[3][1]
                    - m_matrix[0][1] * m_matrix[2][2] * m_matrix[3][3]
                    - m_matrix[0][2] * m_matrix[2][3] * m_matrix[3][1]
                    - m_matrix[0][3] * m_matrix[2][1] * m_matrix[3][2];
    m_inverse[0][2] = m_matrix[0][1] * m_matrix[1][2] * m_matrix[3][3]
                    + m_matrix[0][2] * m_matrix[1][3] * m_matrix[3][1]
                    + m_matrix[0][3] * m_matrix[1][1] * m_matrix[3][2]
                    - m_matrix[0][1] * m_matrix[1][3] * m_matrix[3][2]
                    - m_matrix[0][2] * m_matrix[1][1] * m_matrix[3][3]
                    - m_matrix[0][3] * m_matrix[1][2] * m_matrix[3][1];
    m_inverse[0][3] = m_matrix[0][1] * m_matrix[1][3] * m_matrix[2][2]
                    + m_matrix[0][2] * m_matrix[1][1] * m_matrix[2][3]
                    + m_matrix[0][3] * m_matrix[1][2] * m_matrix[2][1]
                    - m_matrix[0][1] * m_matrix[1][2] * m_matrix[2][3]
                    - m_matrix[0][2] * m_matrix[1][3] * m_matrix[2][1]
                    - m_matrix[0][3] * m_matrix[1][1] * m_matrix[2][2];
    m_inverse[1][0] = m_matrix[1][0] * m_matrix[2][3] * m_matrix[3][2]
                    + m_matrix[1][2] * m_matrix[2][0] * m_matrix[3][3]
                    + m_matrix[1][3] * m_matrix[2][2] * m_matrix[3][0]
                    - m_matrix[1][0] * m_matrix[2][2] * m_matrix[3][3]
                    - m_matrix[1][2] * m_matrix[2][3] * m_matrix[3][0]
                    - m_matrix[1][3] * m_matrix[2][0] * m_matrix[3][2];
    m_inverse[1][1] = m_matrix[0][0] * m_matrix[2][2] * m_matrix[3][3]
                    + m_matrix[0][2] * m_matrix[2][3] * m_matrix[3][0]
                    + m_matrix[0][3] * m_matrix[2][0] * m_matrix[3][2]
                    - m_matrix[0][0] * m_matrix[2][3] * m_matrix[3][2]
                    - m_matrix[0][2] * m_matrix[2][0] * m_matrix[3][3]
                    - m_matrix[0][3] * m_matrix[2][2] * m_matrix[3][0];
    m_inverse[1][2] = m_matrix[0][0] * m_matrix[1][3] * m_matrix[3][2]
                    + m_matrix[0][2] * m_matrix[1][0] * m_matrix[3][3]
                    + m_matrix[0][3] * m_matrix[1][2] * m_matrix[3][0]
                    - m_matrix[0][0] * m_matrix[1][2] * m_matrix[3][3]
                    - m_matrix[0][2] * m_matrix[1][3] * m_matrix[3][0]
                    - m_matrix[0][3] * m_matrix[1][0] * m_matrix[3][2];
    m_inverse[1][3] = m_matrix[0][0] * m_matrix[1][2] * m_matrix[2][3]
                    + m_matrix[0][2] * m_matrix[1][3] * m_matrix[2][0]
                    + m_matrix[0][3] * m_matrix[1][0] * m_matrix[2][2]
                    - m_matrix[0][0] * m_matrix[1][3] * m_matrix[2][2]
                    - m_matrix[0][2] * m_matrix[1][0] * m_matrix[2][3]
                    - m_matrix[0][3] * m_matrix[1][2] * m_matrix[2][0];
    m_inverse[2][0] = m_matrix[1][0] * m_matrix[2][1] * m_matrix[3][3]
                    + m_matrix[1][1] * m_matrix[2][3] * m_matrix[3][0]
                    + m_matrix[1][3] * m_matrix[2][0] * m_matrix[3][1]
                    - m_matrix[1][0] * m_matrix[2][3] * m_matrix[3][1]
                    - m_matrix[1][1] * m_matrix[2][0] * m_matrix[3][3]
                    - m_matrix[1][3] * m_matrix[2][1] * m_matrix[3][0];
    m_inverse[2][1] = m_matrix[0][0] * m_matrix[2][3] * m_matrix[3][1]
                    + m_matrix[0][1] * m_matrix[2][0] * m_matrix[3][3]
                    + m_matrix[0][3] * m_matrix[2][1] * m_matrix[3][0]
                    - m_matrix[0][0] * m_matrix[2][1] * m_matrix[3][3]
                    - m_matrix[0][1] * m_matrix[2][3] * m_matrix[3][0]
                    - m_matrix[0][3] * m_matrix[2][0] * m_matrix[3][1];
    m_inverse[2][2] = m_matrix[0][0] * m_matrix[1][1] * m_matrix[3][3]
                    + m_matrix[0][1] * m_matrix[1][3] * m_matrix[3][0]
                    + m_matrix[0][3] * m_matrix[1][0] * m_matrix[3][1]
                    - m_matrix[0][0] * m_matrix[1][3] * m_matrix[3][1]
                    - m_matrix[0][1] * m_matrix[1][0] * m_matrix[3][3]
                    - m_matrix[0][3] * m_matrix[1][1] * m_matrix[3][0];
    m_inverse[2][3] = m_matrix[0][0] * m_matrix[1][3] * m_matrix[2][1]
                    + m_matrix[0][1] * m_matrix[1][0] * m_matrix[2][3]
                    + m_matrix[0][3] * m_matrix[1][1] * m_matrix[2][0]
                    - m_matrix[0][0] * m_matrix[1][1] * m_matrix[2][3]
                    - m_matrix[0][1] * m_matrix[1][3] * m_matrix[2][0]
                    - m_matrix[0][3] * m_matrix[1][0] * m_matrix[2][1];
    m_inverse[3][0] = m_matrix[1][0] * m_matrix[2][2] * m_matrix[3][1]
                    + m_matrix[1][1] * m_matrix[2][0] * m_matrix[3][2]
                    + m_matrix[1][2] * m_matrix[2][1] * m_matrix[3][0]
                    - m_matrix[1][0] * m_matrix[2][1] * m_matrix[3][2]
                    - m_matrix[1][1] * m_matrix[2][2] * m_matrix[3][0]
                    - m_matrix[1][2] * m_matrix[2][0] * m_matrix[3][1];
    m_inverse[3][1] = m_matrix[0][0] * m_matrix[2][1] * m_matrix[3][2]
                    + m_matrix[0][1] * m_matrix[2][2] * m_matrix[3][0]
                    + m_matrix[0][2] * m_matrix[2][0] * m_matrix[3][1]
                    - m_matrix[0][0] * m_matrix[2][2] * m_matrix[3][1]
                    - m_matrix[0][1] * m_matrix[2][0] * m_matrix[3][2]
                    - m_matrix[0][2] * m_matrix[2][1] * m_matrix[3][0];
    m_inverse[3][2] = m_matrix[0][0] * m_matrix[1][2] * m_matrix[3][1]
                    + m_matrix[0][1] * m_matrix[1][0] * m_matrix[3][2]
                    + m_matrix[0][2] * m_matrix[1][1] * m_matrix[3][0]
                    - m_matrix[0][0] * m_matrix[1][1] * m_matrix[3][2]
                    - m_matrix[0][1] * m_matrix[1][2] * m_matrix[3][0]
                    - m_matrix[0][2] * m_matrix[1][0] * m_matrix[3][1];
    m_inverse[3][3] = m_matrix[0][0] * m_matrix[1][1] * m_matrix[2][2]
                    + m_matrix[0][1] * m_matrix[1][2] * m_matrix[2][0]
                    + m_matrix[0][2] * m_matrix[1][0] * m_matrix[2][1]
                    - m_matrix[0][0] * m_matrix[1][2] * m_matrix[2][1]
                    - m_matrix[0][1] * m_matrix[1][0] * m_matrix[2][2]
                    - m_matrix[0][2] * m_matrix[1][1] * m_matrix[2][0];
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 4; j++)
        {
            m_inverse[i][j] /= det;
        }
    }
    m_inverse_valid = true;
}

Matrix Matrix::getInverse()
{
    calculateInverse();
    Matrix m;
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 4; j++)
        {
            m.m_matrix[i][j] = m_inverse[i][j];
            m.m_inverse[i][j] = m_matrix[i][j];
        }
    }
    m.m_inverse_calculated = true;
    m.m_inverse_valid = m_inverse_valid;
    return m;
}

Matrix & Matrix::operator*=(const Matrix & other)
{
    Matrix temp = (*this) * other;
    (*this) = temp;
    return (*this);
}

bool operator==(const Matrix & m1, const Matrix & m2)
{
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 4; j++)
        {
            if (! FP_EQ(m1[i][j], m2[i][j]))
                return false;
        }
    }
    return true;
}

std::ostream & operator<<(std::ostream & out, const Matrix & m)
{
    out << std::setprecision(3);
    for (int i = 0; i < 4; i++)
    {
        out << (i == 0 ? "[ " : "  ");
        out << "[ ";
        for (int j = 0; j < 4; j++)
        {
            out << m[i][j];
            if (j < 3)
                out << ", ";
        }
        out << " ]";
        if (i == 3)
            out << " ]";
        out << std::endl;
    }
    return out;
}