#ifndef _TMATH_HPP_
#define _TMATH_HPP_

#include <cmath>

const float pi = 3.141593f;
const float pi2 = 6.283185f;
const float sr2 = 1.414214f;
const float isr2 = 0.7071068f;

namespace tmath {

template<typename T>
struct vec2 {
    T val[2];

    T& operator [] (int32_t idx) { return val[idx]; }
    const T& operator [] (int32_t idx) const { return val[idx]; }
    vec2<T> operator + (const vec2<T>& m) const { return vec2<T>{val[0] + m.val[0], val[1] + m.val[1]}; }
    vec2<T> operator - (const vec2<T>& m) const { return vec2<T>{val[0] - m.val[0], val[1] - m.val[1]}; }
    vec2<T> operator * (T scalar) const { return vec2<T>{val[0] * scalar, val[1] * scalar}; }
    T dot(const vec2<T>& m) const { return val[0] * m.val[0] + val[1] * m.val[1]; }
};

template<typename T>
struct vec3 {
    T val[3];

    T& operator [] (int32_t idx) { return val[idx]; }
    const T& operator [] (int32_t idx) const { return val[idx]; }
    vec3<T> operator + (const vec3<T>& m) const { return vec3<T>{val[0] + m.val[0], val[1] + m.val[1], val[2] + m.val[2]}; }
    vec3<T> operator - (const vec3<T>& m) const { return vec3<T>{val[0] - m.val[0], val[1] - m.val[1], val[2] - m.val[2]}; }
    vec3<T> operator * (T scalar) const { return vec3<T>{val[0] * scalar, val[1] * scalar, val[2] * scalar}; }
    T dot(const vec3<T>& m) const { return val[0] * m.val[0] + val[1] * m.val[1] + val[2] * m.val[2]; }
    vec3<T> cross(const vec3<T>& m) const {
        return vec3<T>{
            val[1] * m.val[2] - val[2] * m.val[1],
            val[2] * m.val[0] - val[0] * m.val[2],
            val[0] * m.val[1] - val[1] * m.val[0]
        };
    }
    vec3<T> normalize() const {
        T factor = std::sqrt(val[0] * val[0] + val[1] * val[1] + val[2] * val[2]);
        return vec3<T>{val[0] / factor, val[1] / factor, val[2] / factor};
    }
};

template<typename T>
struct vec4 {
    T val[4];

    T& operator [] (int32_t idx) { return val[idx]; }
    const T& operator [] (int32_t idx) const { return val[idx]; }
};

// |  0  4  8 12  |
// |  1  5  9 13  |
// |  2  6 10 14  |
// |  3  7 11 15  |
template<typename T>
struct mat4 {
    T val[16];

    T& operator [] (int32_t idx) { return val[idx]; }
    const T& operator [] (int32_t idx) const { return val[idx]; }

    mat4<T> operator + (const mat4<T>& m) const {
        return mat4<T> {
            val[ 0] + m.val[ 0], val[ 1] + m.val[ 1], val[ 2] + m.val[ 2], val[ 3] + m.val[ 3],
            val[ 4] + m.val[ 4], val[ 5] + m.val[ 5], val[ 6] + m.val[ 6], val[ 7] + m.val[ 7],
            val[ 8] + m.val[ 8], val[ 9] + m.val[ 9], val[10] + m.val[10], val[11] + m.val[11],
            val[12] + m.val[12], val[13] + m.val[13], val[14] + m.val[14], val[15] + m.val[15]
        };
    }

    vec3<T> operator * (const vec3<T>& m) const {
        T fx = val[ 0] * m.val[0] + val[ 4] * m.val[1] + val[ 8] * m.val[2] + val[12];
        T fy = val[ 1] * m.val[0] + val[ 5] * m.val[1] + val[ 9] * m.val[2] + val[13];
        T fz = val[ 2] * m.val[0] + val[ 6] * m.val[1] + val[10] * m.val[2] + val[14];
        T fw = val[ 3] * m.val[0] + val[ 7] * m.val[1] + val[11] * m.val[2] + val[15];
        return vec3<T>{ fx / fw, fy / fw, fz / fw};
    }

    mat4<T> operator * (const mat4<T>& m) const {
        return mat4<T> {
            val[ 0] * m.val[ 0] + val[ 4] * m.val[ 1] + val[ 8] * m.val[ 2] + val[12] * m.val[ 3],
            val[ 1] * m.val[ 0] + val[ 5] * m.val[ 1] + val[ 9] * m.val[ 2] + val[13] * m.val[ 3],
            val[ 2] * m.val[ 0] + val[ 6] * m.val[ 1] + val[10] * m.val[ 2] + val[14] * m.val[ 3],
            val[ 3] * m.val[ 0] + val[ 7] * m.val[ 1] + val[11] * m.val[ 2] + val[15] * m.val[ 3],
            val[ 0] * m.val[ 4] + val[ 4] * m.val[ 5] + val[ 8] * m.val[ 6] + val[12] * m.val[ 7],
            val[ 1] * m.val[ 4] + val[ 5] * m.val[ 5] + val[ 9] * m.val[ 6] + val[13] * m.val[ 7],
            val[ 2] * m.val[ 4] + val[ 6] * m.val[ 5] + val[10] * m.val[ 6] + val[14] * m.val[ 7],
            val[ 3] * m.val[ 4] + val[ 7] * m.val[ 5] + val[11] * m.val[ 6] + val[15] * m.val[ 7],
            val[ 0] * m.val[ 8] + val[ 4] * m.val[ 9] + val[ 8] * m.val[10] + val[12] * m.val[11],
            val[ 1] * m.val[ 8] + val[ 5] * m.val[ 9] + val[ 9] * m.val[10] + val[13] * m.val[11],
            val[ 2] * m.val[ 8] + val[ 6] * m.val[ 9] + val[10] * m.val[10] + val[14] * m.val[11],
            val[ 3] * m.val[ 8] + val[ 7] * m.val[ 9] + val[11] * m.val[10] + val[15] * m.val[11],
            val[ 0] * m.val[12] + val[ 4] * m.val[13] + val[ 8] * m.val[14] + val[12] * m.val[15],
            val[ 1] * m.val[12] + val[ 5] * m.val[13] + val[ 9] * m.val[14] + val[13] * m.val[15],
            val[ 2] * m.val[12] + val[ 6] * m.val[13] + val[10] * m.val[14] + val[14] * m.val[15],
            val[ 3] * m.val[12] + val[ 7] * m.val[13] + val[11] * m.val[14] + val[15] * m.val[15]
        };
    }

    mat4<T> translate(T tx, T ty, T tz) const {
        return mat4<T>{
            val[ 0] + val[ 3] * tx, val[ 1] + val[ 3] * ty, val[ 2]  + val[ 3] * tz, val[ 3],
            val[ 4] + val[ 7] * tx, val[ 5] + val[ 7] * ty, val[ 6]  + val[ 7] * tz, val[ 7],
            val[ 8] + val[11] * tx, val[ 9] + val[11] * ty, val[10]  + val[11] * tz, val[11],
            val[12] + val[15] * tx, val[13] + val[15] * ty, val[14]  + val[15] * tz, val[15]
        };
    }

    mat4<T> scale(T sx, T sy, T sz) const {
        return mat4<T>{
            val[ 0] * sx, val[ 1] * sy, val[ 2] * sz, val[ 3],
            val[ 4] * sx, val[ 5] * sy, val[ 6] * sz, val[ 7],
            val[ 8] * sx, val[ 9] * sy, val[10] * sz, val[11],
            val[12] * sx, val[13] * sy, val[14] * sz, val[15]
        };
    }

    mat4<T> rotatex(T rx) const {
        T sv = std::sin(rx);
        T cv = std::cos(rx);
        return mat4<T>{
            val[ 0], val[ 1] * cv - val[ 2] * sv, val[ 1] * sv + val[ 2] * cv, val[ 3],
            val[ 4], val[ 5] * cv - val[ 6] * sv, val[ 5] * sv + val[ 6] * cv, val[ 7],
            val[ 8], val[ 9] * cv - val[10] * sv, val[ 9] * sv + val[10] * cv, val[11],
            val[12], val[13] * cv - val[14] * sv, val[13] * sv + val[14] * cv, val[15]
        };
    }

    mat4<T> rotatey(T ry) const {
        T sv = std::sin(ry);
        T cv = std::cos(ry);
        return mat4<T>{
            val[ 0] * cv + val[ 2] * sv, val[ 1], -val[ 0] * sv + val[ 2] * cv, val[ 3],
            val[ 4] * cv + val[ 6] * sv, val[ 5], -val[ 4] * sv + val[ 6] * cv, val[ 7],
            val[ 8] * cv + val[10] * sv, val[ 9], -val[ 8] * sv + val[10] * cv, val[11],
            val[12] * cv + val[14] * sv, val[13], -val[12] * sv + val[14] * cv, val[15]
        };
    }

    mat4<T> rotatez(T rz) const {
        T sv = std::sin(rz);
        T cv = std::cos(rz);
        return mat4<T>{
            val[ 0] * cv - val[ 1] * sv, val[ 0] * sv + val[ 1] * cv, val[ 2], val[ 3],
            val[ 4] * cv - val[ 5] * sv, val[ 4] * sv + val[ 5] * cv, val[ 6], val[ 7],
            val[ 8] * cv - val[ 9] * sv, val[ 8] * sv + val[ 9] * cv, val[10], val[11],
            val[12] * cv - val[13] * sv, val[12] * sv + val[13] * cv, val[14], val[15]
        };
    }

    mat4<T> homo_inverse() const {
        T det = val[0] * val[5] * val[10] + val[4] * val[9] * val[2] + val[8] * val[1] * val[6]
               -val[0] * val[6] * val[9] - val[1] * val[4] * val[10] - val[2] * val[5] * val[8];
        T idet = (T)1.0 / det;
        T adj0 = (val[5] * val[10] - val[6] * val[9]) * idet;
        T adj1 = (val[4] * val[10] - val[6] * val[8]) * -idet;
        T adj2 = (val[4] * val[9] - val[5] * val[8]) * idet;
        T adj3 = (val[1] * val[10] - val[2] * val[9]) * -idet;
        T adj4 = (val[0] * val[10] - val[2] * val[8]) * idet;
        T adj5 = (val[0] * val[9] - val[1] * val[8]) * -idet;
        T adj6 = (val[1] * val[6] - val[2] * val[5]) * idet;
        T adj7 = (val[0] * val[6] - val[2] * val[4]) * -idet;
        T adj8 = (val[0] * val[5] - val[1] * val[4]) * idet;
        T d1 = -adj0 * val[12] - adj1 * val[13] - adj2 * val[14];
        T d2 = -adj3 * val[12] - adj4 * val[13] - adj5 * val[14];
        T d3 = -adj6 * val[12] - adj7 * val[13] - adj8 * val[14];
        return mat4<T>{
            adj0, adj3, adj6, (T)0,
            adj1, adj4, adj7, (T)0,
            adj2, adj5, adj8, (T)0,
            d1, d2, d3, (T)1
        };
    }

    mat4<T> transpose() const {
        return mat4<T>{
            val[0], val[4], val[ 8], val[12],
            val[1], val[5], val[ 9], val[13],
            val[2], val[6], val[10], val[14],
            val[3], val[7], val[11], val[15],
        };
    }
    
    static mat4<T> identity() {
        return mat4<T>{(T)1, (T)0, (T)0, (T)0, (T)0, (T)1, (T)0, (T)0, (T)0, (T)0, (T)1, (T)0, (T)0, (T)0, (T)0, (T)1};
    }

    static mat4<T> perspective(T width, T height, T near, T far, bool lh = true) {
        T factor = lh ? (T)1 : (T)-1;
        T val = far / (far - near);        
        return mat4<T>{
            (T)2 * near / width, (T)0, (T)0, (T)0,
            (T)0, (T)2 * near / height, (T)0, (T)0,
            (T)0, (T)0, val * factor, (T)factor,
            (T)0, (T)0, -near * val, (T)0
        };
    }

    // same as glm::perspectiveNO, mapping z-axis to [-1, 1] (OpenGL clip volume definition)
    static mat4<T> perspective_fov(T fov, T aspect, T near, T far, bool lh = true) {
        T factor = lh ? (T)1 : (T)-1;
        T yscale = (T)1 / std::tan(fov * (T)0.5);
        T xscale = yscale / aspect;
        T val = far / (far - near);
        return mat4<T>{
            xscale, (T)0, (T)0, (T)0,
            (T)0, yscale, (T)0, (T)0,
            (T)0, (T)0, (far + near) / (far - near) * factor, (T)factor,
            (T)0, (T)0, -(T)2 * far * near / (far - near), (T)0
        };
    }

    static mat4<T> orthogonal(T width, T height, T near, T far, bool lh = true) {
        T factor = lh ? (T)1 : (T)-1;
        T dist = far - near;
        return mat4<T>{
            (T)2 / width, (T)0, (T)0, (T)0,
            (T)0, (T)2 / height, (T)0, (T)0,
            (T)0, (T)0, factor / dist, -near / dist,
            (T)0, (T)0, (T)0, (T)1
        }
    }

    static mat4<T> look_at(vec3<T> eye, vec3<T> at, vec3<T> up, bool lh = true) {
        T factor = lh ? (T)1 : (T)-1;
        vec3<T> zaxis = (at - eye).normalize() * factor;
        vec3<T> xaxis = up.cross(zaxis).normalize();
        vec3<T> yaxis = zaxis.cross(xaxis);
        return mat4<T>{
            xaxis[0], yaxis[0], zaxis[0], (T)0,
            xaxis[1], yaxis[1], zaxis[1], (T)0,
            xaxis[2], yaxis[2], zaxis[2], (T)0,
            -xaxis.dot(eye) * factor, -yaxis.dot(eye) * factor, -zaxis.dot(eye) * factor, (T)1
        };
    }
};

using ivec2 = vec2<int32_t>;
using ivec3 = vec3<int32_t>;
using ivec4 = vec4<int32_t>;
using fvec2 = vec2<float>;
using fvec3 = vec3<float>;
using fvec4 = vec4<float>;
using fmat4 = mat4<float>;

}

#endif