#pragma once

#include <sstream>
#include "../Common/Sizes.hpp"
#include "Rotation.hpp"
#include "Trig.hpp"

template <uint R, uint C, typename T = Real>
struct Matrix {
    Matrix() : elements{} {}

    explicit Matrix(const T scalar) {
        std::fill(elements, elements + R * C, scalar);
    }

    template<typename ...Args, std::enable_if_t<sizeof...(Args) == R * C, Int> = 0>
    Matrix(Args... args): elements{ static_cast<T>(args)... } {}

    explicit Matrix(const T values[R * C]) {
        std::copy(values, values + R * C, elements);
    }

    static Matrix<R, R, T> identity() {
        Matrix<R, R, T> result{};
        for (Int i = 0; i < R; i++) {
            result(i, i) = 1;
        }
        return result;
    }

    static Matrix<4, 4, T> transformation(Vector<3> position) {
        return {
            1.0, 0.0, 0.0, position.x(),
            0.0, 1.0, 0.0, position.y(),
            0.0, 0.0, 1.0, position.z(),
            0.0, 0.0, 0.0, 1.0
        };
    }

    static Matrix<4, 4, T> rotation(Rotation angles) {
        auto radians = angles.vector.map([](auto a) { return Math::radians(a); });

        auto c = radians.map([](auto a) { return cos(a); });
        auto s = radians.map([](auto a) { return sin(a); });

        Matrix<4, 4, T> rotation_x{
            1.0f, 0.0f,  0.0f,   0.0f,
            0.0f, c.x(), -s.x(), 0.0f,
            0.0f, s.x(), c.x(),  0.0f,
            0.0f, 0.0f,  0.0f,   1.0f,
        };

        Matrix<4, 4, T> rotation_y{
            c.y(),  0.0f, s.y(),  0.0f,
            0.0f,   1.0f, 0.0f,   0.0f,
            -s.y(), 0.0f, c.y(),  0.0f,
            0.0f,   0.0f, 0.0f,   1.0f,
        };

        Matrix<4, 4, T> rotation_z{
            c.z(), -s.z(), 0.0f,  0.0f,
            s.z(), c.z(),  0.0f,  0.0f,
            0.0f,  0.0f,   1.0f,  0.0f,
            0.0f,  0.0f,   0.0f,  1.0f,
        };

        return rotation_x * rotation_y * rotation_z;
    }

    Vector<C, T> row(USize index) const {
        return Vector<C, T>{ &elements[index * C] };
    }

    Vector<R, T> col(USize index) const {
        Vector<R, T> result{};
        for (Int i = 0; i < R; i++) {
            result[i] = this->operator()(index, i);
        }
        return result;
    }

    Matrix transpose() const {
        Matrix result{};
        for (Int y = 0; y < R; y++) {
            for (Int x = 0; x < C; x++) {
                result(x, y) = this->operator()(y, x);
            }
        }
        return result;
    }

    Matrix operator+(const Matrix other) const {
        Matrix result{};
        for (Int i = 0; i < R * C; i++) {
            result.elements[i] = elements[i] + other.elements[i];
        }
        return result;
    }

    Matrix operator*(Real scalar) {
        Matrix result{};
        for (Int i = 0; i < R * C; i++) {
            result.elements[i] = elements[i] * scalar;
        }
        return result;
    }

    template<uint N>
    Matrix<R, N, T> operator*(const Matrix<C, N, T> other) const {
        Matrix<R, N, T> result{};
        for (Int y = 0; y < R; y++) {
            for (Int x = 0; x < N; x++) {
                auto r = row(y);
                auto c = other.col(x);

                auto dot = r * c;

                result(x, y) = dot;
            }
        }
        return result;
    }

    Vector<R, T> operator*(const Vector<R, T> vector) const {
        Matrix<R, 1, T> matrix(vector.elements);
        matrix = this->operator*(matrix);
        return Vector<R, T>{ matrix.elements };
    }

    const T& operator()(const USize x, const USize y) const {
        return elements[y * C + x];
    }
    T& operator()(const USize x, const USize y) {
        return elements[y * C + x];
    }

    std::string string() {
        std::stringstream str{};
        for (Int x = 0; x < R; x++) {
            for (Int y = 0; y < C; y++) {
                str << this->operator()(x, y) << " ";
            }

            str << "\n";
        }

        return str.str();
    }

    T elements[R * C];
};