/*
 * This file is part of libgim.
 *
 * libgim is free software: you can redistribute it and/or modify it under the
 * terms of the GNU General Public License as published by the Free Software
 * Foundation, either version 3 of the License, or (at your option) any later
 * version.
 * 
 * libgim 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 GNU General Public License for more
 * details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with libgim.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Copyright 2010 Danny Robson <danny@blubinc.net>
 */

#include "matrix.hpp"

#include "debug.hpp"
#include "range.hpp"
#include "maths.hpp"

#include <algorithm>

using namespace util;
using namespace maths;

matrix::matrix (size_t _rows, size_t _columns):
        m_rows    (_rows),
        m_columns (_columns),
        m_data    (NULL) {
    if (m_rows <= 0 || m_columns <= 0)
        throw std::runtime_error ("rows and columns must be positive");

    m_data = new double[size ()];
}


matrix::matrix (size_t                                _rows,
                size_t                                _columns,
                const std::initializer_list <double> &_data):
    m_rows    (_rows),
    m_columns (_columns),
    m_data    (NULL)
{
    if (m_rows <= 0 || m_columns <= 0)
        throw std::runtime_error ("rows and columns must be positive");
    if (size () != _data.size ())
        throw std::runtime_error ("element and initializer size differs");
    check_hard (m_rows * m_columns == _data.size());

    m_data = new double[size ()];
    std::copy (_data.begin (), _data.end (), m_data);
}


matrix::matrix (const std::initializer_list <vector> &rhs):
    m_rows    (rhs.size    ()),
    m_columns (rhs.begin()->size ()),
    m_data    (new double[m_rows * m_columns]) 
{
    double *row_cursor = m_data;

    for (auto i = rhs.begin (); i != rhs.end (); ++i) {
        check (i->size () == m_columns);

        std::copy (i->data (), i->data () + i->size (), row_cursor);
        row_cursor += m_columns;
    }
}


matrix::matrix (const matrix &rhs):
        m_rows    (rhs.m_rows),
        m_columns (rhs.m_columns) {
    m_data = new double [m_rows * m_columns];
    std::copy (rhs.m_data, rhs.m_data + m_rows * m_columns, m_data);
}


matrix::matrix (matrix &&rhs):
        m_rows    (rhs.m_rows),
        m_columns (rhs.m_columns),
        m_data    (rhs.m_data) {
    rhs.m_data = NULL;
}


matrix::~matrix()
    { delete [] m_data; }


void
matrix::sanity (void) const {
    check (m_rows    > 0);
    check (m_columns > 0);
    check (m_data != NULL);
}


const double *
matrix::operator [] (unsigned int row) const {
    check_hard (row < m_rows);
    return m_data + row * m_columns;
}


double *
matrix::operator [] (unsigned int row) {
    check_hard (row < m_rows);
    return m_data + row * m_columns;
}


const double *
matrix::data (void) const
    { return m_data; }


matrix&
matrix::operator  =(const matrix& rhs) {
    if (size () != rhs.size ()) {
        delete [] m_data;
        m_data = new double [m_rows * m_columns];
    }

    m_rows    = rhs.m_rows;
    m_columns = rhs.m_columns;
    std::copy (rhs.m_data, rhs.m_data + m_rows * m_columns, m_data);

    return *this;
}


matrix
matrix::operator * (double scalar) const {
    matrix val (*this);

    for (unsigned int i = 0; i < m_rows; ++i)
        for (unsigned int j = 0; j < m_columns; ++j)
            val[i][j] *= scalar;

    return val;
}


matrix&
matrix::operator *=(double scalar) {
    for (unsigned int i = 0; i < m_rows; ++i)
        for (unsigned int j = 0; j < m_columns; ++j)
            (*this)[i][j] *= scalar;

    return *this;
}


matrix&
matrix::operator /= (double scalar)
    { return (*this) *= (1.0 / scalar); }


matrix
matrix::operator  + (double scalar) const {
    matrix val (*this);
    for (unsigned int i = 0; i < m_rows; ++i)
        for (unsigned int j = 0; j < m_columns; ++j)
            val[i][j] += scalar;

    return val;
}


matrix&
matrix::operator +=(double scalar) {
    for (unsigned int i = 0; i < m_rows; ++i)
        for (unsigned int j = 0; j < m_columns; ++j)
            (*this)[i][j] += scalar;

    return *this;
}


matrix
matrix::operator * (const matrix& rhs) const {
    if (m_columns != rhs.rows ())
        throw std::invalid_argument ("matrices size mismatch in multiplication");

    matrix val (matrix::zeroes (m_rows, rhs.columns ()));

    for (unsigned int i = 0; i < m_rows; ++i) 
        for (unsigned int j = 0; j < rhs.columns (); ++j)
            for (unsigned int k = 0; k < m_columns; ++k)
                val[i][j] += (*this)[i][k] * rhs[k][j];

    return val;
}


matrix&
matrix::operator *=(const matrix& rhs)
    { return *this = *this * rhs; }


bool
matrix::operator ==(const matrix& rhs) const {
    if (rhs.rows    () != rows     () ||
        rhs.columns () != columns  ())
        return false;

    return std::equal (m_data, m_data + size (), rhs.data ());
}


//matrix  transpose (void) const { ; }


size_t
matrix::rows (void) const 
    { return m_rows; }


size_t
matrix::columns (void) const
    { return m_columns; }


size_t
matrix::size (void) const 
    { return rows () * columns (); }


bool
matrix::is_square (void) const 
    { return m_rows == m_columns; }


bool
matrix::is_magic (void) const {
    if (!is_square ())
        return false;

    unsigned int expected = m_rows * (m_rows * m_rows + 1) / 2;
    range<double> numbers (1, m_rows * m_rows);

    for (unsigned int i = 0; i < m_rows; ++i) {
        unsigned int sum1 = 0, sum2 = 0;

        for (unsigned int j = 0; j < m_columns; ++j) {
            if (!numbers.contains ((*this)[i][j]) ||
                !numbers.contains ((*this)[j][i]))
                return false;

            sum1 += (*this)[i][j];
            sum2 += (*this)[j][i];
        }
        
        if (sum1 != expected || sum2 != expected)
            return false;
    }

    return true;
}


bool
matrix::is_homogeneous (void) const {
    if (m_rows != m_columns)
        return false;

    // Check the final row is all zeroes
    for (unsigned int i = 0; i < m_columns - 1; ++i) {
        if (!almost_equal ((*this)[m_rows - 1][i], 0.))
            return false;
    }

    // Except for the last element, which has to be one
    return almost_equal ((*this)[m_rows - 1][m_columns - 1], 1.);
}


double
matrix::determinant (void) const {
    if (m_rows != m_columns)
        not_implemented ();

    switch (m_rows) {
        case 2: return determinant2x2 ();
        case 3: return determinant3x3 ();
        case 4: return determinant4x4 ();
    }

    not_implemented ();
}


// With matrix A = [ a, b ]
//                 [ c, d ]
//  
// det (A) = ad - bc

double
matrix::determinant2x2 (void) const {
    check_eq (m_rows,    2);
    check_eq (m_columns, 2);

    return (*this)[0][0] * (*this)[1][1] -
           (*this)[0][1] * (*this)[1][0];
}



//                   [ a, b, c ]
// Given matrix A =  [ d, e, f ]
//                   [ g, h, i ]
//
//  det (A) = aei + bfg + cdh - afg - bdi - ceg
//  det (A) = a(ei - fg) + b(fg - di) + c(dh - eg)
double
matrix::determinant3x3 (void) const {
    check_eq (m_rows,    3);
    check_eq (m_columns, 3);

    return (*this)[0][0] * (*this)[1][1] * (*this)[2][2] + // aei
           (*this)[0][1] * (*this)[1][2] * (*this)[2][0] + // bfg
           (*this)[0][2] * (*this)[1][0] * (*this)[2][1] - // cdh
           (*this)[0][0] * (*this)[1][2] * (*this)[2][1] - // afh
           (*this)[0][1] * (*this)[1][0] * (*this)[2][2] - // bdi
           (*this)[0][2] * (*this)[1][1] * (*this)[2][0];  // ceg
}


// From libMathematics, http://www.geometrictools.com/ 
double 
matrix::determinant4x4 (void) const {
    check_eq (m_rows,    4);
    check_eq (m_columns, 4);

    double a0 = m_data[ 0] * m_data[ 5] - m_data[ 1] * m_data[ 4],
           a1 = m_data[ 0] * m_data[ 6] - m_data[ 2] * m_data[ 4],
           a2 = m_data[ 0] * m_data[ 7] - m_data[ 3] * m_data[ 4],
           a3 = m_data[ 1] * m_data[ 6] - m_data[ 2] * m_data[ 5],
           a4 = m_data[ 1] * m_data[ 7] - m_data[ 3] * m_data[ 5],
           a5 = m_data[ 2] * m_data[ 7] - m_data[ 3] * m_data[ 6],
           b0 = m_data[ 8] * m_data[13] - m_data[ 9] * m_data[12],
           b1 = m_data[ 8] * m_data[14] - m_data[10] * m_data[12],
           b2 = m_data[ 8] * m_data[15] - m_data[11] * m_data[12],
           b3 = m_data[ 9] * m_data[14] - m_data[10] * m_data[13],
           b4 = m_data[ 9] * m_data[15] - m_data[11] * m_data[13],
           b5 = m_data[10] * m_data[15] - m_data[11] * m_data[14];

    return a0 * b5 - a1 * b4 + a2 * b3 + a3 * b2 - a4 * b1 + a5 * b0;
}


matrix
matrix::inverse (void) const {
    if (m_rows != m_columns)
        not_implemented ();

    switch (m_rows) {
        case  2: return inverse2x2 ();
        case  3: return inverse3x3 ();
        case  4: return inverse4x4 ();
    }

    not_implemented ();
}


matrix
matrix::inverse2x2 (void) const {
    check (m_rows    == 2);
    check (m_columns == 2);

    double det = determinant2x2 ();
    if (almost_equal (det, 0.))
        throw not_invertible ();
    return matrix (2, 2, {  (*this)[1][1], -(*this)[0][1],
                           -(*this)[1][0],  (*this)[0][0] }) /= det;
}


//                   [ a, b, c ]
// Given matrix A =  [ d, e, f ]
//                   [ g, h, i ]
//
matrix
matrix::inverse3x3 (void) const {
    check (m_rows    == 3);
    check (m_columns == 3);

    double det = determinant3x3();
    if (almost_equal (det, 0.))
        throw not_invertible ();

    matrix val (m_rows, m_columns, {
        (*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1], // ei - fh
        (*this)[0][2] * (*this)[2][1] - (*this)[0][1] * (*this)[2][2], // ch - bi
        (*this)[0][1] * (*this)[1][2] - (*this)[0][2] * (*this)[1][1], // bf - ce
        (*this)[1][2] * (*this)[2][0] - (*this)[1][0] * (*this)[2][2], // fg - di
        (*this)[0][0] * (*this)[2][2] - (*this)[0][2] * (*this)[2][0], // ai - cg
        (*this)[0][2] * (*this)[1][0] - (*this)[0][0] * (*this)[1][2], // cd - af
        (*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0], // dh - eg
        (*this)[0][1] * (*this)[2][0] - (*this)[0][0] * (*this)[2][1], // bg - ah
        (*this)[0][0] * (*this)[1][1] - (*this)[0][1] * (*this)[1][0]  // ae - bd
    });

    return val /= det;

    //matrix val ({ vector::cross ((*this)[1], (*this)[2], 3),
    //              vector::cross ((*this)[2], (*this)[0], 3),
    //              vector::cross ((*this)[0], (*this)[1], 3) });
    //return val /= determinant3x3 ();
}


matrix
matrix::inverse4x4 (void) const {
    double a0 = m_data[ 0] * m_data[ 5] - m_data[ 1] * m_data[ 4],
           a1 = m_data[ 0] * m_data[ 6] - m_data[ 2] * m_data[ 4],
           a2 = m_data[ 0] * m_data[ 7] - m_data[ 3] * m_data[ 4],
           a3 = m_data[ 1] * m_data[ 6] - m_data[ 2] * m_data[ 5],
           a4 = m_data[ 1] * m_data[ 7] - m_data[ 3] * m_data[ 5],
           a5 = m_data[ 2] * m_data[ 7] - m_data[ 3] * m_data[ 6],
           b0 = m_data[ 8] * m_data[13] - m_data[ 9] * m_data[12],
           b1 = m_data[ 8] * m_data[14] - m_data[10] * m_data[12],
           b2 = m_data[ 8] * m_data[15] - m_data[11] * m_data[12],
           b3 = m_data[ 9] * m_data[14] - m_data[10] * m_data[13],
           b4 = m_data[ 9] * m_data[15] - m_data[11] * m_data[13],
           b5 = m_data[10] * m_data[15] - m_data[11] * m_data[14];

    double det = a0 * b5 - a1 * b4 + a2 * b3 + a3 * b2 - a4 * b1 + a5 * b0;
    if (almost_equal (det, 0.))
        throw not_invertible ();

    return matrix (4, 4, {
        + m_data[ 5] * b5 - m_data[ 6] * b4 + m_data[ 7] * b3,
        - m_data[ 1] * b5 + m_data[ 2] * b4 - m_data[ 3] * b3,
        + m_data[13] * a5 - m_data[14] * a4 + m_data[15] * a3,
        - m_data[ 9] * a5 + m_data[10] * a4 - m_data[11] * a3,
        - m_data[ 4] * b5 + m_data[ 6] * b2 - m_data[ 7] * b1,
        + m_data[ 0] * b5 - m_data[ 2] * b2 + m_data[ 3] * b1,
        - m_data[12] * a5 + m_data[14] * a2 - m_data[15] * a1,
        + m_data[ 8] * a5 - m_data[10] * a2 + m_data[11] * a1,
        + m_data[ 4] * b4 - m_data[ 5] * b2 + m_data[ 7] * b0,
        - m_data[ 0] * b4 + m_data[ 1] * b2 - m_data[ 3] * b0,
        + m_data[12] * a4 - m_data[13] * a2 + m_data[15] * a0,
        - m_data[ 8] * a4 + m_data[ 9] * a2 - m_data[11] * a0,
        - m_data[ 4] * b3 + m_data[ 5] * b1 - m_data[ 6] * b0,
        + m_data[ 0] * b3 - m_data[ 1] * b1 + m_data[ 2] * b0,
        - m_data[12] * a3 + m_data[13] * a1 - m_data[14] * a0,
        + m_data[ 8] * a3 - m_data[ 9] * a1 + m_data[10] * a0
    }) /= det;
}



matrix
matrix::zeroes (size_t diag)
        { return zeroes (diag, diag); }

        
matrix
matrix::zeroes (size_t rows, size_t columns) {
    matrix m (rows, columns);
    std::fill (m.m_data, m.m_data + m.size (), 0.0);
    return m;
}


matrix
matrix::identity (size_t diag) {
    matrix val (zeroes (diag));
    for (unsigned int i = 0; i < diag; ++i)
        val[i][i] = 1.0;
    return val;
}


matrix
matrix::magic (size_t n) {
    check_hard (n > 2);

    if (n % 2 == 1)
        return magic_odd (n);

    if (n % 4 == 0)
        return magic_even_single (n);

    return magic_even_double (n);
}


// Use the 'siamese' method. Start from the top centre, progress up-left one.
// If filled then drop down one row instead. Wrap around indexing.
matrix
matrix::magic_odd (size_t n) {
    check_hard (n > 2);
    check_hard (n % 2 == 1);

    matrix val (zeroes (n));
    for (unsigned int i = 1, x = n / 2, y = 0; i <= n * n; ++i) {
        val[y][x] = i;

        unsigned int x1 = (x +     1) % n,
                     y1 = (y + n - 1) % n;

        if (!almost_equal (val[y1][x1], 0)) {
            x1 = x;
            y1 = (y + 1) % n;
        }

        x = x1;
        y = y1;
    }

    return val;
}


matrix
matrix::magic_even_single (size_t)
    { not_implemented (); }


matrix
matrix::magic_even_double (size_t)
    { not_implemented (); }