#include "camera.h"

#include <cmath>

namespace {
constexpr float PI = 3.14159265358979323846f;
constexpr float DEG2RAD = PI / 180.0f;

// ---- Vec3 helpers -----------------------------------------------------------

Vec3 add(Vec3 a, Vec3 b) { return {a.x+b.x, a.y+b.y, a.z+b.z}; }
Vec3 sub(Vec3 a, Vec3 b) { return {a.x-b.x, a.y-b.y, a.z-b.z}; }
Vec3 mul(Vec3 v, float s){ return {v.x*s, v.y*s, v.z*s}; }

float dot(Vec3 a, Vec3 b) { return a.x*b.x + a.y*b.y + a.z*b.z; }

Vec3 cross(Vec3 a, Vec3 b) {
    return {a.y*b.z - a.z*b.y,
            a.z*b.x - a.x*b.z,
            a.x*b.y - a.y*b.x};
}

Vec3 normalize(Vec3 v) {
    float len = std::sqrt(dot(v, v));
    if (len <= 1e-6f) return {0.0f, 0.0f, 0.0f};
    return {v.x/len, v.y/len, v.z/len};
}

// Rodrigues' rotation: rotate v around unit axis by angle (radians).
Vec3 rotateAround(Vec3 v, Vec3 axis, float angle) {
    float c = std::cos(angle);
    float s = std::sin(angle);
    float d = dot(v, axis);
    // v*cos + cross(axis,v)*sin + axis*dot(axis,v)*(1-cos)
    Vec3 cr = cross(axis, v);
    return {v.x*c + cr.x*s + axis.x*d*(1.0f-c),
            v.y*c + cr.y*s + axis.y*d*(1.0f-c),
            v.z*c + cr.z*s + axis.z*d*(1.0f-c)};
}

} // namespace

// ---- Public -----------------------------------------------------------------

void Camera::update(GLFWwindow* window, float dt) {
    const float velocity = moveSpeed * dt;
    Vec3 f = forward();
    Vec3 r = right();
    Vec3 u = worldUp; // fly vertically along gravity up

    if (glfwGetKey(window, GLFW_KEY_W) == GLFW_PRESS) position = add(position, mul(f, velocity));
    if (glfwGetKey(window, GLFW_KEY_S) == GLFW_PRESS) position = sub(position, mul(f, velocity));
    if (glfwGetKey(window, GLFW_KEY_A) == GLFW_PRESS) position = sub(position, mul(r, velocity));
    if (glfwGetKey(window, GLFW_KEY_D) == GLFW_PRESS) position = add(position, mul(r, velocity));
    if (glfwGetKey(window, GLFW_KEY_SPACE)      == GLFW_PRESS) position = add(position, mul(u, velocity));
    if (glfwGetKey(window, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS) position = sub(position, mul(u, velocity));
}

void Camera::processMouse(double xoffset, double yoffset) {
    // Yaw: rotate around worldUp
    float yawAngle = -static_cast<float>(xoffset) * mouseSensitivity * DEG2RAD;
    Vec3 wup = normalize(worldUp);
    Vec3 newFwd = normalize(rotateAround(orientForward, wup, yawAngle));

    // Pitch: rotate around camera right
    Vec3 r = normalize(cross(newFwd, wup));
    float pitchAngle = -static_cast<float>(yoffset) * mouseSensitivity * DEG2RAD;
    Vec3 pitched = normalize(rotateAround(newFwd, r, pitchAngle));

    // Clamp: prevent looking past straight up/down (|dot| < sin(89°))
    constexpr float PITCH_LIMIT = 0.9998f;
    if (std::abs(dot(pitched, wup)) < PITCH_LIMIT) {
        newFwd = pitched;
    }

    orientForward = newFwd;
}

Vec3 Camera::forward() const {
    return orientForward;
}

Vec3 Camera::right() const {
    return normalize(cross(orientForward, worldUp));
}

Vec3 Camera::up() const {
    // Actual camera up: perpendicular to both forward and right, tilts with horizon.
    return normalize(cross(right(), orientForward));
}

void Camera::buildViewRotationMatrix(float out[16]) const {
    Vec3 f = forward();
    Vec3 r = right();
    Vec3 u = up();
    out[0]  = r.x;  out[1]  = u.x;  out[2]  = -f.x; out[3]  = 0.0f;
    out[4]  = r.y;  out[5]  = u.y;  out[6]  = -f.y; out[7]  = 0.0f;
    out[8]  = r.z;  out[9]  = u.z;  out[10] = -f.z; out[11] = 0.0f;
    out[12] = 0.0f; out[13] = 0.0f; out[14] = 0.0f; out[15] = 1.0f;
}

void Camera::buildViewMatrix(float out[16]) const {
    Vec3 f = forward();
    Vec3 r = right();
    Vec3 u = up();

    out[0]  = r.x;  out[1]  = u.x;  out[2]  = -f.x; out[3]  = 0.0f;
    out[4]  = r.y;  out[5]  = u.y;  out[6]  = -f.y; out[7]  = 0.0f;
    out[8]  = r.z;  out[9]  = u.z;  out[10] = -f.z; out[11] = 0.0f;
    out[12] = -dot(r, position);
    out[13] = -dot(u, position);
    out[14] =  dot(f, position);
    out[15] = 1.0f;
}
