// algebra.h
// Declaration and implementation of vector and matrix functionality.
// By David Norgren (dnn13002), April 2015 for DVA304 Computer Graphics

#pragma once

#include <iostream>
#include <math.h>

#define PI  3.1415926535897932f

template <int size>

// A vector with 2 to 4 elements.
class vec {
public:
    vec() {
        for (int i = 0; i < size; i++)
            e[i] = 0;
    }
    vec(float u, float v) {
        e[0] = u; e[1] = v;
    }
    vec(float x, float y, float z) {
        e[0] = x; e[1] = y; e[2] = z;
    }
    vec(float r, float g, float b, float a) {
        e[0] = r; e[1] = g; e[2] = b; e[3] = a;
    }

    // Get
    float u() { return e[0]; }
    float v() { return e[1]; }
    float x() { return e[0]; }
    float y() { return e[1]; }
    float z() { return e[2]; }
    float w() { return e[3]; }
    float r() { return e[0]; }
    float g() { return e[1]; }
    float b() { return e[2]; }
    float a() { return e[3]; }

    // Returns the length of the vector.
    float length() {
        float a = 0;
        for (int i = 0; i < size; i++)
            a += e[i] * e[i];
        return sqrt(a);
    }

    // Returns the new vector with length 1.
    vec<size> normalize() {
        vec<size> normal;
        float len = length();
        for (int i = 0; i < size; i++)
            normal[i] = e[i] / len;
        return normal;
    }

    // Elements
    float e[size];

    // Operators
    float& operator [] (int index) {
        return e[index];
    }
    bool operator == (vec<size>& right) {
        for (int i = 0; i < size; i++)
            if (e[i] != right[i])
                return false;
        return true;
    }
    bool operator != (vec<size>& right) {
        for (int i = 0; i < size; i++)
            if (e[i] != right[i])
                return true;
        return false;
    }
    void operator += (vec<size>& right) {
        for (int i = 0; i < size; i++)
            e[i] += right[i];
    }
    void operator -= (vec<size>& right) {
        for (int i = 0; i < size; i++)
            e[i] -= right[i];
    }
    void operator *= (float& s) {
        for (int i = 0; i < size; i++)
            e[i] *= s;
    }
    void operator /= (float& s) {
        for (int i = 0; i < size; i++)
            e[i] /= s;
    }
    vec<size> operator + (vec<size>& right) {
        vec<size> sum;
        for (int i = 0; i < size; i++)
            sum.e[i] = e[i] + right[i];
        return sum;
    }
    vec<size> operator - () {
        vec<size> neg;
        for (int i = 0; i < size; i++)
            neg.e[i] = -e[i];
        return neg;
    }
    vec<size> operator - (vec<size>& right) {
        vec<size> dif;
        for (int i = 0; i < size; i++)
            dif.e[i] = e[i] - right[i];
        return dif;
    }
    vec<size> operator * (float s) {
        vec<size> scaled;
        for (int i = 0; i < size; i++)
            scaled.e[i] = e[i] * s;
        return scaled;
    }
    vec<size> operator / (float s) {
        vec<size> scaled;
        for (int i = 0; i < size; i++)
            scaled.e[i] = e[i] / s;
        return scaled;
    }
};

// Additional operators
template <int size> vec<size> operator * (float s, vec<size>& right) {
    vec<size> scaled;
    for (int i = 0; i < size; i++)
        scaled.e[i] = s * right[i];
    return scaled;
}
template <int size> std::ostream& operator << (std::ostream& os, vec<size>& print) {
    for (int i = 0; i < size; i++)
        os << print[i] << (i < size - 1 ? "," : "");
    return os;
}

// User friendly names
typedef vec<2> vec2;
typedef vec<3> vec3;
typedef vec<4> vec4;

// Returns the dot product of two vectors.
template <int size> inline float dot(vec<size>& left, vec<size>& right) {
    float p = 0;
    for (int i = 0; i < size; i++)
        p += left[i] * right[i];
    return p;
}

// Returns the cross product of two vectors.
inline vec3 cross(vec3& left, vec3& right) {
    return vec3(
        left.y() * right.z() - left.z() * right.y(),
        left.z() * right.x() - left.x() * right.z(),
        left.x() * right.y() - left.y() * right.x()
    );
}

template <int size>

// A matrix with 2x2 to 4x4 elements
class mat {
public:
    mat() {
        for (int i = 0; i < size * size; i++)
            e[i] = 0;
    }
    mat(float x1, float y1,
        float x2, float y2) {
        e[0] = x1; e[1] = x2;
        e[2] = y1; e[3] = y2;
    }
    mat(float x1, float y1, float z1,
        float x2, float y2, float z2,
        float x3, float y3, float z3) {
        e[0] = x1; e[1] = x2; e[1] = x3;
        e[2] = y1; e[3] = y2; e[4] = y3;
        e[5] = z1; e[6] = z2; e[7] = z3;
    }
    mat(float x1, float y1, float z1, float w1,
        float x2, float y2, float z2, float w2,
        float x3, float y3, float z3, float w3,
        float x4, float y4, float z4, float w4) {
        e[0] = x1; e[1] = x2; e[2] = x3; e[3] = x4;
        e[4] = y1; e[5] = y2; e[6] = y3; e[7] = y4;
        e[8] = z1; e[9] = z2; e[10] = z3; e[11] = z4;
        e[12] = w1; e[13] = w2; e[14] = w3; e[15] = w4;
    }

    // Elements
    float e[size * size];

    // Operators
    float* operator[] (int x) {
        static float col[size];
        for (int i = 0; i < size; i++)
            col[i] = e[i * size + x];
        return col;
    }
    bool operator == (mat<size>& right) {
        for (int i = 0; i < size * size; i++)
            if (e[i] != right.e[i])
                return false;
        return true;
    }
    bool operator != (mat<size>& right) {
        for (int i = 0; i < size * size; i++)
            if (e[i] != right.e[i])
                return true;
        return false;
    }
    void operator += (mat<size>& right) {
        for (int i = 0; i < size * size; i++)
            e[i] += right.e[i];
    }
    void operator -= (mat<size>& right) {
        for (int i = 0; i < size * size; i++)
            e[i] -= right.e[i];
    }
    void operator *= (mat<size>& right) {
        mat<size> product;
        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++) {
                product.e[j * 4 + i] = 0;
                for (int k = 0; k < 4; k++)
                    product.e[j * 4 + i] += e[k * 4 + i] * right.e[j * 4 + k];
            }
        }
        for (int i = 0; i < size; i++)
            e[i] = product.e[i];
    }
    void operator *= (float& s) {
        for (int i = 0; i < size * size; i++)
            e[i] *= s;
    }
    void operator /= (float& s) {
        for (int i = 0; i < size * size; i++)
            e[i] /= s;
    }
    mat<size> operator + (mat<size>& right) {
        mat<size> sum;
        for (int i = 0; i < size * size; i++)
            sum.e[i] = e[i] + right.e[i];
        return sum;
    }
    mat<size> operator - () {
        mat<size> neg;
        for (int i = 0; i < size * size; i++)
            neg.e[i] = -e[i];
        return neg;
    }
    mat<size> operator - (mat<size>& right) {
        mat<size> dif;
        for (int i = 0; i < size * size; i++)
            dif.e[i] = e[i] - right.e[i];
        return dif;
    }
    mat<size> operator * (mat<size>& right) {
        mat<size> product;
        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++) {
                product.e[j * 4 + i] = 0;
                for (int k = 0; k < 4; k++)
                    product.e[j * 4 + i] += e[k * 4 + i] * right.e[j * 4 + k];
            }
        }
        return product;
    }
    mat<size> operator * (float s) {
        mat<size> scaled;
        for (int i = 0; i < size * size; i++)
            scaled.e[i] = e[i] * s;
        return scaled;
    }
    mat<size> operator / (float s) {
        mat<size> scaled;
        for (int i = 0; i < size * size; i++)
            scaled.e[i] = e[i] / s;
        return scaled;
    }
};

// User friendly names
typedef mat<2> mat2;
typedef mat<3> mat3;
typedef mat<4> mat4;

// Additional operators
template <int size> mat<size> operator * (float s, mat<size>& right) {
    mat<size> scaled;
    for (int i = 0; i < size * size; i++)
        scaled.e[i] = s * right.e[i];
    return scaled;
}
template <int size> std::ostream& operator << (std::ostream& os, mat<size>& print) {
    for (int i = 0; i < size; i++) {
        for (int j = 0; j < size; j++)
            os << print[i][j] << (j < size - 1 ? "," : "");
        os << endl;
    }
    return os;
}

// Build a matrix for translation.
inline mat4 buildTranslation(vec3& translate) {
    return mat4(
        1, 0, 0, translate[0],
        0, 1, 0, translate[1],
        0, 0, 1, translate[2],
        0, 0, 0, 1
    );
}

// Build a matrix for X rotation.
inline mat4 buildRotationX(float angle) {
    return mat4(
        1, 0, 0, 0,
        0, cos(angle), -sin(angle), 0,
        0, sin(angle), cos(angle), 0,
        0, 0, 0, 1
    );
}

// Build a matrix for Y rotation.
inline mat4 buildRotationY(float angle) {
    return mat4(
        cos(angle), 0, sin(angle), 0,
        0, 1, 0, 0,
        -sin(angle), 0, cos(angle), 0,
        0, 0, 0, 1
    );
}

// Build a matrix for Z rotation.
inline mat4 buildRotationZ(float angle) {
    return mat4(
        cos(angle), -sin(angle), 0, 0,
        sin(angle), cos(angle), 0, 0,
        0, 0, 1, 0,
        0, 0, 0, 1
    );
}

// Build a matrix for scaling.
inline mat4 buildScale(vec3& scale) {
    return mat4(
        scale.x(), 0, 0, 0,
        0, scale.y(), 0, 0,
        0, 0, scale.z(), 0,
        0, 0, 0, 1
    );
}