#include "vec.hpp"

#include <array>
#include <cmath>
#include <utility>

namespace math {
    float vec3f::dot(const vec3f &rhs) const {
        return (x * rhs.x) + (y * rhs.y) + (z * rhs.z);
    }

    double vec3f::ddot(const vec3f &rhs) const {
        return (static_cast<double>(x) * rhs.x) + (static_cast<double>(y) * rhs.y) + (static_cast<double>(z) * rhs.z);
    }

    float vec3f::dot(const vec4f &rhs) const {
        return (x * rhs.x) + (y * rhs.y) + (z * rhs.z);
    }

    double vec3f::ddot(const vec4f &rhs) const {
        return (static_cast<double>(x) * rhs.x) + (static_cast<double>(y) * rhs.y) + (static_cast<double>(z) * rhs.z);
    }

    float vec3f::operator*(const vec3f &rhs) const {
        return dot(rhs);
    }

    float vec3f::operator*(const vec4f &rhs) const {
        return dot(rhs);
    }

    vec3f vec3f::operator*(const float rhs) const {
        return {x * rhs, y * rhs, z * rhs};
    }

    vec3f vec3f::operator+(const vec3f &rhs) const {
        return {x + rhs.x, y + rhs.y, z + rhs.z};
    }

    vec3f vec3f::operator-(const vec3f &rhs) const {
        return {x - rhs.x, y - rhs.y, z - rhs.z};
    }

    bool vec3f::operator==(const vec3f &rhs) const {
        return x == rhs.x && y == rhs.y && z == rhs.z;
    }

    float &vec3f::operator[](const std::size_t i) {
        switch (i) {
            case 0:
                return x;
            case 1:
                return y;
            case 2:
                return z;
            default:
                throw std::out_of_range("vec3f::operator[]");
        }
    }

    vec3f vec3f::normalize() const {
        vec3f res{0.0f, 0.0f, 0.0f};
        if (const auto d = dot(*this); d > 0.0f) {
            res = *this * (1.0f / std::sqrt(d));
        }

        return res;
    }

    vec3f vec3f::multiply_add(const vec3f &rhs, const float scale) const {
        return {
            x + (scale * rhs.x),
            y + (scale * rhs.y),
            z + (scale * rhs.z)
        };
    }

    vec3f vec3f::cross(const vec3f &rhs) const {
        return {
            (y * rhs.z) - (z * rhs.y),
            (z * rhs.x) - (x * rhs.z),
            (x * rhs.y) - (y * rhs.x)
        };
    }

    float vec3f::magnitude() const {
        return std::sqrt(dot(*this));
    }

    vec3f vec3f::operator-() const {
        return {-x, -y, -z};
    }

    vec3f project_point_on_plane(const vec3f &point, const vec3f &normal) {
        const auto inv_denom = 1.0f / normal.dot(normal);
        const auto dist = normal.dot(point) * inv_denom;

        const auto scaled_normal = normal * inv_denom;

        return point - (scaled_normal * dist);
    }

    vec3f vec3f::perpendicular() const {
        vec3f res{0.0f, 0.0f, 0.0f};
        auto pos = 0;
        auto min = 1.0f;

        if (std::abs(x) < min) {
            pos = 0;
            min = std::abs(x);
        }
        if (std::abs(y) < min) {
            pos = 1;
            min = std::abs(y);
        }
        if (std::abs(z) < min) {
            pos = 2;
            min = std::abs(z);
        }

        switch (pos) {
            case 0:
                res.x = 1.0f;
                break;
            case 1:
                res.y = 1.0f;
                break;
            case 2:
                res.z = 1.0f;
                break;
            default:
                std::unreachable();
        }

        res = project_point_on_plane(res, *this);

        res = res.normalize();

        return res;
    }

    std::array<float, 3> vec3f::into_array() const {
        return std::array{x, y, z};
    }

    vec3f vec3f::to_angles(const vec3f forward) {
        const vec3f temp{forward.x, forward.y, 0.0f};

        auto pitch = -std::atan2(forward.z, temp.magnitude());
        auto yaw = std::atan2(forward.y, forward.x);
        auto roll = 0.0f;

        return {pitch, yaw, roll};
    }

    std::tuple<vec3f, vec3f, vec3f> vec3f::from_angles(const vec3f &angles) {
        vec3f forward{0.0f, 0.0f, 0.0f};
        vec3f right{0.0f, 0.0f, 0.0f};
        vec3f up{0.0f, 0.0f, 0.0f};

        const auto pitch = angles.x * (std::numbers::pi * 2 / 360);
        const auto pitch_sin = std::sin(pitch);
        const auto pitch_cos = std::cos(pitch);
        const auto yaw = angles.y * (std::numbers::pi * 2 / 360);
        const auto yaw_sin = std::sin(yaw);
        const auto yaw_cos = std::cos(yaw);
        const auto roll = angles.z * (std::numbers::pi * 2 / 360);
        const auto roll_sin = std::sin(roll);
        const auto roll_cos = std::cos(roll);

        forward = {
            static_cast<float>(pitch_cos * yaw_cos), static_cast<float>(pitch_cos * yaw_sin),
            static_cast<float>(-pitch_sin)
        };
        right = {
            static_cast<float>(-1 * roll_sin * pitch_sin * yaw_cos + -1 * roll_cos * -yaw_sin),
            static_cast<float>(-1 * roll_sin * pitch_sin * yaw_sin + -1 * roll_cos * yaw_cos),
            static_cast<float>(-1 * roll_sin * pitch_cos),
        };
        up = {
            static_cast<float>(roll_cos * pitch_sin * yaw_cos + -roll_sin * -yaw_sin),
            static_cast<float>(roll_cos * pitch_sin * yaw_sin + -roll_sin * yaw_cos),
            static_cast<float>(roll_cos * pitch_cos),
        };
        return {forward, right, up};
    }

    /**
     * turn forward towards side on the plane defined by forward and side
     * if angle = 90, the result will be equal to side
     * assumes side and forward are perpendicular, and normalized
     * to turn away from side, use a negative angle
     *
     * @param forward
     * @param side
     * @param angle
     * @return
     */
    vec3f vec3f::turn(const vec3f &forward, const vec3f &side, const float angle) {
        const float scale_forward = static_cast<float>(std::cos(degrees_to_radians(angle)));
        const float scale_side = static_cast<float>(std::sin(degrees_to_radians(angle)));

        return (forward * scale_forward) + (side * scale_side);
    }

    float & vec4f::operator[](std::size_t i) {
        switch (i) {
            case 0:
                return x;
            case 1:
                return y;
            case 2:
                return z;
            case 3:
                return w;
            default:
                throw std::out_of_range("vec4f::operator[]");
        }
    }
}