#ifndef FASTMATH_H
#define FASTMATH_H

#include <iostream>
#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <vector>
#define SHIFT_AMOUNT 16
#define HALF_SHIFT (SHIFT_AMOUNT / 2)

#define SHIFT_MASK ((1 << SHIFT_AMOUNT) - 1)
#define TO_FLOAT(x)                                                            \
    (((float)(x >> SHIFT_AMOUNT)) +                                            \
     ((double)(x & SHIFT_MASK) / (1 << SHIFT_AMOUNT)))
#define TO_INT(x) ((int32_t)(x * (1 << SHIFT_AMOUNT)))
#define MUL_F(a, b) (((a) >> HALF_SHIFT) * ((b) >> HALF_SHIFT))
#define DIV_F(a, b) ((((a) << HALF_SHIFT) / (b)) << HALF_SHIFT)

struct decimal {

    int32_t i;
    constexpr decimal() = default;
    // constexpr decimal() : i(0) {}
    constexpr decimal(float i) : i(TO_INT(i)) {}
    constexpr decimal(double i) : i(TO_INT(i)) {}
    constexpr decimal(int32_t i) : i(i) {}

    inline friend std::ostream &operator<<(std::ostream &os, const decimal &d) {
        return (os << TO_FLOAT(d.i));
    }

    inline friend decimal operator+(const decimal &d1, const decimal &d2) {
        return {d1.i + d2.i};
    }
    inline decimal &operator+=(const decimal &d) { return (*this) = {i + d.i}; }

    inline friend decimal operator-(const decimal &d1, const decimal &d2) {
        return {d1.i - d2.i};
    }
    inline friend decimal operator-(const decimal &d) { return {-d.i}; }

    inline friend decimal operator*(const decimal &d1, const decimal &d2) {
        return {MUL_F(d1.i, d2.i)};
    }

    inline decimal &operator*=(const decimal &d) {
        return (*this) = {MUL_F(i, d.i)};
    }

    inline friend decimal operator/(const decimal &d1, const decimal &d2) {
        return {DIV_F(d1.i, d2.i)};
    }

    inline friend bool operator<(const decimal &d1, const decimal &d2) {
        return d1.i < d2.i;
    }

    inline friend bool operator>(const decimal &d1, const decimal &d2) {
        return d1.i > d2.i;
    }
    inline friend bool operator<=(const decimal &d1, const decimal &d2) {
        return d1.i <= d2.i;
    }
    inline friend bool operator>=(const decimal &d1, const decimal &d2) {
        return d1.i >= d2.i;
    }

    inline friend bool operator==(const decimal &d1, const decimal &d2) {
        return d1.i == d2.i;
    }
    inline friend bool operator!=(const decimal &d1, const decimal &d2) {
        return d1.i != d2.i;
    }

    inline decimal sqrt() { return {((int32_t)sqrtf(i)) << HALF_SHIFT}; }

    inline float to_float() { return TO_FLOAT(i); }
    inline bool isSmall() { return (abs(i) < (1 << (HALF_SHIFT - 1))); }
};

template <int n, class Dev> struct vec {

    decimal v[n];

    constexpr vec() noexcept = default;

    template <class... Args>
    constexpr vec(Args... args) noexcept : v{static_cast<decimal>(args)...} {
        static_assert(sizeof...(Args) == n, "Wrong number of elements for vec");
    }

    friend Dev operator+(const vec<n, Dev> &v1, const vec<n, Dev> &v2) {
        Dev newV = {};

        for (int i = 0; i < n; i++) {
            newV.v[i] = v1.v[i] + v2.v[i];
        }
        return newV;
    }
    friend Dev operator+=(const vec<n, Dev> &v1, const vec<n, Dev> &v2) {
        Dev newV = {};

        for (int i = 0; i < n; i++) {
            newV.v[i] = v1.v[i] + v2.v[i];
        }
        return newV;
    }

    friend Dev operator-(const vec<n, Dev> &v1, const vec<n, Dev> &v2) {
        Dev newV = {};

        for (int i = 0; i < n; i++) {
            newV.v[i] = v1.v[i] - v2.v[i];
        }
        return newV;
    }

    friend std::ostream &operator<<(std::ostream &os, const vec<n, Dev> &v) {
        os << "(" << v.v[0];
        for (int i = 1; i < n; i++) {
            os << ", " << v.v[i];
        }
        return (os << ")" << std::endl);
    }

    Dev operator-() {
        Dev newV = {};
        for (int i = 0; i < n; i++) {
            newV.v[i] = -v[i];
        }
        return newV;
    }

    friend Dev operator*(const vec<n, Dev> &v, const decimal &d) {
        int32_t f = d.i >> HALF_SHIFT;

        Dev newV = {};
        for (int i = 0; i < n; i++) {
            newV.v[i] = (v.v[i].i >> HALF_SHIFT) * f;
        }
        return newV;
    }
    static Dev max(const vec<n, Dev> &v1, const vec<n, Dev> &v2) {
        Dev newV = {};
        for (int i = 0; i < n; i++) {
            newV.v[i] = std::max(v1.v[i], v2.v[i]);
        }
        return newV;
    }
    static Dev min(const vec<n, Dev> &v1, const vec<n, Dev> &v2) {
        Dev newV = {};
        for (int i = 0; i < n; i++) {
            newV.v[i] = std::min(v1.v[i], v2.v[i]);
        }
        return newV;
    }

    friend Dev operator*(const decimal &d, const vec<n, Dev> &v) {
        return v * d;
    }

    friend decimal operator*(const vec<n, Dev> &v1, const vec<n, Dev> &v2) {
        decimal res = decimal(0.0f);
        for (int i = 0; i < n; i++) {
            res += v1.v[i] * v2.v[i];
        }
        return res;
    }

    friend bool operator==(const vec<n, Dev> &v1, const vec<n, Dev> &v2) {
        bool res = true;
        for (int i = 0; i < n; i++) {
            res &= v1.v[i] == v2.v[i];
        }
        return res;
    }
    bool isSmall() {
        for (int i = 0; i < n; i++) {
            if (!v[i].isSmall())
                return false;
        }
        return true;
    }
    decimal &operator[](const int &i) { return v[i]; }

    decimal len_sq() { return *this * *this; }

    decimal len() { return this->len_sq().sqrt(); }

    Dev normalize() {
        decimal f = decimal(1.0) / this->len();
        return (*this * f);
    }

    constexpr static Dev zero() {
        Dev newV = {};
        for (int i = 0; i < n; i++) {
            newV[i] = decimal(0);
        }
        return newV;
    }
};
struct vec2 : public vec<2, vec2> {

    vec2(float x, float y) : vec<2, vec2>(decimal(x), decimal(y)) {}

    vec2(double x, double y) : vec<2, vec2>(decimal(x), decimal(y)) {}

    vec2(int32_t x, int32_t y) : vec<2, vec2>(decimal(x), decimal(y)) {}

    vec2(decimal x, decimal y) : vec<2, vec2>(x, y) {}

    decimal &x() { return v[0]; }
    decimal &y() { return v[1]; }
};
struct vec3 : public vec<3, vec3> {

    constexpr vec3() : vec<3, vec3>() {}

    constexpr vec3(float x, float y, float z)
        : vec<3, vec3>(decimal(x), decimal(y), decimal(z)) {}

    constexpr vec3(double x, double y, double z)
        : vec<3, vec3>(decimal(x), decimal(y), decimal(z)) {}

    constexpr vec3(int32_t x, int32_t y, int32_t z)
        : vec<3, vec3>(decimal(x), decimal(y), decimal(z)) {}

    constexpr vec3(decimal x, decimal y, decimal z) : vec<3, vec3>(x, y, z) {}

    inline decimal &x() { return v[0]; }
    inline decimal &y() { return v[1]; }
    inline decimal &z() { return v[2]; }

    inline vec3 cross(vec3 &v) {
        return vec3((y() * v.z()) - (z() * v.y()),
                    (z() * v.x()) - (x() * v.z()),
                    (x() * v.y()) - (y() * v.x()));
    }
};
struct vec4 : public vec<4, vec4> {

    constexpr vec4() : vec<4, vec4>() {}

    vec4(float x, float y, float z, float w)
        : vec<4, vec4>(decimal(x), decimal(y), decimal(z), decimal(w)) {}

    vec4(double x, double y, double z, double w)
        : vec<4, vec4>(decimal(x), decimal(y), decimal(z), decimal(w)) {}

    vec4(int32_t x, int32_t y, int32_t z, int32_t w)
        : vec<4, vec4>(decimal(x), decimal(y), decimal(z), decimal(w)) {}

    vec4(vec3 v, decimal w) : vec<4, vec4>(v.x(), v.y(), v.z(), w) {}

    decimal &x() { return v[0]; }
    decimal &y() { return v[1]; }
    decimal &z() { return v[2]; }
    decimal &w() { return v[3]; }
};

template <int n, class Dev> struct mat {
    decimal m[n * n];

    static const int size = n;
    friend Dev operator+(const mat<n, Dev> &m1, const mat<n, Dev> &m2) {
        Dev newM = {};

        for (int i = 0; i < n * n; i++) {
            newM.v[i] = m1.m[i] + m2.m[i];
        }
        return newM;
    }
    friend Dev operator+=(const mat<n, Dev> &m1, const mat<n, Dev> &m2) {
        Dev newM = {};

        for (int i = 0; i < n * n; i++) {
            newM.m[i] = m1.m[i] + m2.m[i];
        }
        return newM;
    }

    friend Dev operator-(const mat<n, Dev> &m1, const mat<n, Dev> &m2) {
        Dev newM = {};

        for (int i = 0; i < n * n; i++) {
            newM.m[i] = m1.m[i] - m2.m[i];
        }
        return newM;
    }

    friend std::ostream &operator<<(std::ostream &os, const mat<n, Dev> &m) {
        for (int i = 0; i < n; i++) {
            os << "|" << m.m[i * n];
            for (int j = 1; j < n; j++) {
                os << ", " << m.m[i * n + j];
            }
            os << "|" << "\n";
        }
        return (os << std::endl);
    }

    template <class Dev1>
    friend Dev1 operator*(const mat<n, Dev> &mat, const vec<n, Dev1> &v) {
        Dev1 newV = vec<n, Dev1>::zero();
        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++) {
                newV[i] += mat.m[i * n + j] * v.v[j];
            }
        }
        return newV;
    }

    decimal &operator[](const int &i) { return m[i]; }

    friend Dev operator*(const mat<n, Dev> &m1, const mat<n, Dev> &m2) {
        Dev newM = mat<n, Dev>::zero();
        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++) {
                for (int k = 0; k < n; k++) {
                    newM[i * n + j] += m1.m[i * n + k] * m2.m[k * n + j];
                }
            }
        }
        return newM;
    }

    constexpr static Dev identity() {
        Dev newM = {};
        for (int i = 0; i < n; i++) {
            for (int j = 0; j < n; j++) {
                if (i == j)
                    newM.m[i * n + i] = decimal(1.0f);
                else
                    newM.m[i * n + j] = decimal(0.0f);
            }
        }
        return newM;
    }
    constexpr static Dev zero() {
        Dev newM = {};
        for (int i = 0; i < n * n; i++) {
            newM[i] = decimal(0);
        }
        return newM;
    }

    inline void set(int x, int y, decimal v) { m[y * n + x] = v; }
    inline decimal get(int x, int y) { return m[y * n + x]; }

    friend bool operator==(const mat<n, Dev> &m1, const mat<n, Dev> &m2) {
        bool res = true;
        for (int i = 0; i < n * n; i++) {
            res &= m1.m[i] == m2.m[i];
        }
        return res;
    }
    bool isSmall() {
        for (int i = 0; i < n; i++) {
            if (!m[i].isSmall())
                return false;
        }
        return true;
    }

    template <class Dev1> Dev1 cutTo() const {
        static_assert(Dev1::size < n, "Can only convert to smaller matrix");
        Dev1 newM = mat<Dev1::size, Dev1>::zero();
        for (int i = 0; i < Dev1::size; i++) {
            for (int j = 0; j < Dev1::size; j++) {
                newM.m[Dev1::size * i + j] = m[n * i + j];
            }
        }
        return newM;
    }
};

template <int n> struct matN : public mat<n, matN<n>> {};

struct mat3 : public mat<3, mat3> {};

struct mat4 : public mat<4, mat4> {
    static mat4 translation(const vec3 &v) {
        mat4 newM = mat4::identity();
        for (int i = 0; i < 3; i++) {
            newM[4 * i + 3] = v.v[i];
        }
        return newM;
    }
    static mat4 rotateOnX(float a) {
        mat4 newM = mat4::identity();
        newM.m[1 * 4 + 1] = cos(a), newM.m[2 * 4 + 2] = cos(a);
        newM.m[1 * 4 + 2] = -sin(a), newM.m[2 * 4 + 1] = sin(a);
        return newM;
    }
    static mat4 rotateOnY(float a) {
        mat4 newM = mat4::identity();
        newM.m[0 * 4 + 0] = cos(a), newM.m[2 * 4 + 2] = cos(a);
        newM.m[0 * 4 + 2] = sin(a), newM.m[2 * 4 + 0] = -sin(a);
        return newM;
    }
    static mat4 rotateOnZ(float a) {
        mat4 newM = mat4::identity();
        newM.m[0 * 4 + 0] = cos(a), newM.m[1 * 4 + 1] = cos(a);
        newM.m[1 * 4 + 0] = sin(a), newM.m[0 * 4 + 1] = -sin(a);
        return newM;
    }
};

#endif