#ifndef MATH_QUAT_HPP
#define MATH_QUAT_HPP

#include <cmath>
#include "math/vector.hpp"

namespace engine::math {

struct Quaternion {
    static constexpr Quaternion zero() {
        return {0.f, 0.f, 0.f, 0.f};
    }

    static constexpr Quaternion one() {
        return {1.f, 0.f, 0.f, 0.f};
    }

    static constexpr Quaternion euler_zxy(float rx, float ry, float rz) {
        float ca = std::cos(rx / 2.f), sa = std::sin(rx / 2.f),
              cb = std::cos(ry / 2.f), sb = std::sin(ry / 2.f),
              cc = std::cos(rz / 2.f), sc = std::sin(rz / 2.f);
        return {
            ca * cb * cc + sa * sb * sc,
            sa * cb * cc + ca * sb * sc,
            ca * sb * cc - sa * cb * sc,
            ca * cb * sc - sa * sb * cc,
        };
    }

    static constexpr Quaternion rot_y(float a) {
        return {std::cos(a / 2.f), 0.f, std::sin(a / 2.f), 0.f};
    }

    static constexpr Quaternion look_towards(const Vector3& dir, const Vector3& up) {
        // TODO: extract common code between Matrix4::look_at and this. We should have something
        // similar to a function returning a 3x3 matrix which does:
        //   e_x -> up.cross(-dir).normalize()
        //   e_y -> (-dir).cross(e_x).normalize()
        //   e_z -> (-dir).normalize()
        Vector3 new_x = up.cross(-dir).normalize();
        Vector3 new_y = (-dir).cross(new_x).normalize();
        Vector3 new_z = (-dir).normalize();
        return {
                          std::sqrt(std::max(0.f,  new_x.x + new_y.y + new_z.z + 1.f)) / 2.f,
            std::copysign(std::sqrt(std::max(0.f,  new_x.x - new_y.y - new_z.z + 1.f)) / 2.f, new_y.z - new_z.y),
            std::copysign(std::sqrt(std::max(0.f, -new_x.x + new_y.y - new_z.z + 1.f)) / 2.f, new_z.x - new_x.z),
            std::copysign(std::sqrt(std::max(0.f, -new_x.x - new_y.y + new_z.z + 1.f)) / 2.f, new_x.y - new_y.x),
        };
    }

    float w, x, y, z;

    constexpr Quaternion() {}
    constexpr Quaternion(float w, float x, float y, float z) : w{w}, x{x}, y{y}, z{z} {}

    constexpr bool operator==(const Quaternion& other) const & {
        return w == other.w && x == other.x && y == other.y && z == other.z;
    }

    constexpr bool operator!=(const Quaternion& other) const & {
        return !(*this == other);
    }

    constexpr Quaternion operator+() const & {
        return *this;
    }

    constexpr Quaternion operator-() const & {
        return { -w, -x, -y, -z };
    }

    constexpr Quaternion operator+(const Quaternion& other) const & {
        return { w + other.w, x + other.x, y + other.y, z + other.z };
    }

    constexpr Quaternion operator-(const Quaternion& other) const & {
        return *this + (-other);
    }

    constexpr Quaternion operator*(const Quaternion& other) const & {
        return {
            w * other.w - x * other.x - y * other.y - z * other.z,
            w * other.x + x * other.w + y * other.z - z * other.y,
            w * other.y + y * other.w + z * other.x - x * other.z,
            w * other.z + z * other.w + x * other.y - y * other.x,
        };
    }

    constexpr Quaternion conjugate() const & {
        return {w, -x, -y, -z};
    }

    constexpr Vector3 rot(const Vector3& v) const & {
        return {
            (2.f * (w * w + x * x) - 1.f) * v.x + (2.f * (x * y - w * z)      ) * v.y + (2.f * (x * z + w * y)      ) * v.z,
            (2.f * (x * y + w * z)      ) * v.x + (2.f * (w * w + y * y) - 1.f) * v.y + (2.f * (y * z - w * x)      ) * v.z,
            (2.f * (x * z - w * y)      ) * v.x + (2.f * (y * z + w * x)      ) * v.y + (2.f * (w * w + z * z) - 1.f) * v.z,
        };
    }
};

}

#endif // MATH_QUAT_HPP