#ifndef _MAT2_H
#define _MAT2_H

// This attempts to follow the GLSL data type standards
// Start with floats only then put in templates later

#include <cmath>
#include <iostream>
#include <limits>
#include "Vec2.h"

// Changed precision from 10e-7 to 10e-6 since it would be too small otherwise
const float Eps = 10 * numeric_limits<float>::epsilon();

class mat2
{
public:
    /* Constructors and Deconstructors */
    // No arguments gives an Identity Matrix
    mat2() {
        Identity(1.0f);
    }
    mat2(const float c) {
        Identity(c);
    }

    mat2(const float c0, const float c1,
         const float c2, const float c3) {
             m[0] = c0;
             m[1] = c1;
             m[2] = c2;
             m[3] = c3;
    }
    mat2(const mat2 &n) {
        for(int i = 0; i < mat2lim; ++i) {
            m[i] = n.m[i];
        }
    }
    mat2(const float c[]) {
        for(int i = 0; i < mat2lim; ++i) {
            m[i] = c[i];
        }
    }
    ~mat2() {}

    /* Array operator */
    float &operator[] (int i);
    const float &operator[] (int i) const;

    float operator() (unsigned int i, unsigned int j) const;
    float &operator() (unsigned int i, unsigned int j);

    /* Reset floats in Matrix to true zero or one */
    mat2 Zero(mat2 &n);

    /* Arithmetic operators */
    mat2 &operator=(const mat2 &n);
    mat2 operator+=(const mat2 &n);
    mat2 operator-=(const mat2 &n);
    mat2 operator*=(const float &c);
    mat2 operator*(const mat2 &n);
    mat2 operator*=(const mat2 &n);

    /* Equivalence operators */
    bool operator==(const mat2 &n);
    bool operator!=(const mat2 &n);

    /* Other mathematical functions */
    float Determinant() const;
    // Prefer to call these as mat2 A.Transpose() rather than A.Transpose(A)
    mat2 Transpose() const;
    mat2 Invert() const;

    /* Output functions */

    void Report();

private:
    // This is arranged in rows format
    // Each row contains one 2D Vector
    enum {
        mat2lim = 4, row2lim = 2, col2lim = 2
    };
    float m[mat2lim];
    void Identity(const float c) {
        m[0] = c; m[1] = 0;
        m[2] = 0; m[3] = c;
    }
};

inline float &mat2::operator[] (int i) {
    return m[i];
}

inline const float &mat2::operator[] (int i) const {
    return m[i];
}

/* Parenthesis operator */
inline float mat2::operator() (unsigned int i, unsigned int j) const {
    return m[(i * row2lim) + j];
}

inline float &mat2::operator() (unsigned int i, unsigned int j) {
    // Row based matrix index
    return m[(i * row2lim) + j];
}

/* Ones or Zeros */
inline mat2 mat2::Zero(mat2 &n) {
    for(int i = 0; i < mat2lim; ++i) {
        if((n[i] <= Eps) && (n[i] >= -Eps))
            n[i] = 0.0f;
    }
    return (n);
}

/* Arithmetic operators */
inline mat2 &mat2::operator=(const mat2 &n) {
    for(int i = 0; i < mat2lim; ++i) {
        m[i] = n[i];
    }
    return (*this);
}

inline mat2 mat2::operator+=(const mat2 &n) {
    return Zero(mat2(m[0] += n[0], m[1] += n[0], m[2] += n[2], m[3] += n[3]));
}

inline mat2 mat2::operator-=(const mat2 &n) {
    return Zero(mat2(m[0] -= n[0], m[1] -= n[1], m[2] -= n[2], m[3] -= n[3]));
}

/* Non-member overloaded operators */
inline mat2 operator+(const mat2 &a, const mat2 &b) {
    return mat2(a[0] + b[0], a[1] + b[1], a[2] + b[2], a[3] + b[3]);
}

inline mat2 operator-(const mat2 &a, const mat2 &b) {
    return mat2(a[0] - b[0], a[1] - b[1], a[2] - b[2], a[3] - b[3]);
}

inline mat2 operator*(const mat2 &b, const float &c) {
    return mat2(b[0] * c, b[1] * c, b[2] * c, b[3] * c);
}

inline mat2 mat2::operator*=(const float &c) {
    return Zero(mat2(m[0] *= c, m[1] *= c, m[2] *= c, m[3] *= c));
}

inline mat2 mat2::operator*(const mat2 &n) {
    mat2 r;

    r(0, 0) = (*this)(0, 0) * n(0, 0) + (*this)(0, 1) * n(1, 0); // r[0][0]
    r(0, 1) = (*this)(0, 0) * n(0, 1) + (*this)(0, 1) * n(1, 1); // r[0][1]
    r(1, 0) = (*this)(1, 0) * n(0, 0) + (*this)(1, 1) * n(1, 0); // r[1][0]
    r(1, 1) = (*this)(1, 0) * n(0, 1) + (*this)(1, 1) * n(1, 1); // r[1][1]

    return Zero(r);
}

inline mat2 mat2::operator*=(const mat2 &n) {
    *this = *this * n;
    return Zero(*this);
}

/* Equivelence operators */

inline bool mat2::operator==(const mat2 &n) {
    for(int i = 0; i < mat2lim; ++i)
    {
        if(m[i] >= fabs(n[i] + Eps) && m[i] <= fabs(n[i] - Eps)) {
            ; // Keep checking for next index
        }
        else
            return false; // Break if condition not met
    }
    return true;
}

inline bool mat2::operator!=(const mat2 &n) {
    for(int i = 0; i < mat2lim; ++i)
    {
        if(m[i] >= fabs(n[i] + Eps) && m[i] <= fabs(n[i] - Eps)) {
            ; // Keep checking for next index
        }
        else
            return true; // Break if condition not met
    }
    return false;
}

/* The other mathematical functions */
inline float mat2::Determinant() const {
    return((*this)(0, 0) * (*this)(1, 1) - (*this)(0, 1) * (*this)(1, 0));
}

inline mat2 mat2::Transpose() const {
    mat2 r;

    for(int i = 0; i < mat2lim; ++i) {
        for(int j = 0; j < mat2lim; ++j) {
            r(i, j) = (*this)(i, j);
        }
    }
    return r;
}

inline mat2 mat2::Invert() const {
    mat2 r;
    float Det = (*this).Determinant();

    if(fabs(Det) > Eps) {
        // Swap the diagonal
        r(0, 0) = (*this)(1, 1);
        r(1, 1) = (*this)(0, 0);
        // Negate the rest
        r(0, 1) = -(*this)(0, 1);
        r(1, 0) = -(*this)(1, 0);
        // Multiply against 1 over Determinant
        r *= (1.0f / Det);
        return (r);
    }
    else
        cout << "Inverse does not exist" << endl;
        return (*this);
}

/* Output functions */

void mat2::Report() {
    cout << "[ " << (*this)(0, 0) << " " << (*this)(0, 1) << " ]" << endl;
    cout << "[ " << (*this)(1, 0) << " " << (*this)(1, 1) << " ]" << endl << endl;
}

#endif /* _MAT2_H */