SpaghettiKart/src/engine/editor/EditorMath.cpp

#include "EditorMath.h"

#include <libultraship/libultraship.h>
#include "port/Game.h"
#include "port/Engine.h"
#include <libultra/types.h>
#include "GameObject.h"

#include <vector>
#include <limits>
#include <cmath>

extern "C" {
#include "common_structs.h"
#include "main.h"
#include "defines.h"
#include "actors.h"
#include "math_util.h"
#include "math_util_2.h"
#include "camera.h"
}

std::vector<Mtx> EditorMatrix;

bool IsInGameScreen() {
    auto wnd = GameEngine::Instance->context->GetWindow();
    Ship::Coords mouse = wnd->GetMousePos();

    // Define viewport boundaries
    int left = gfx_current_game_window_viewport.x;
    int right = left + OTRGetGameRenderWidth();
    int top = gfx_current_game_window_viewport.y;
    int bottom = top + OTRGetGameRenderHeight();

    // Check if the mouse is within the game render area
    return (mouse.x >= left && mouse.x < right) && (mouse.y >= top && mouse.y < bottom);
}

FVector ScreenRayTrace() {
    auto wnd = GameEngine::Instance->context->GetWindow();
    Camera* camera = &cameras[0];

    Ship::Coords mouse = wnd->GetMousePos();
    mouse.x -= gfx_current_game_window_viewport.x;
    mouse.y -= gfx_current_game_window_viewport.y;
    // Get screen dimensions
    uint32_t width = OTRGetGameViewportWidth();
    uint32_t height = OTRGetGameViewportHeight();

    // Convert mouse to NDS screen coordinates
    float x = (2.0f * mouse.x) / width - 1.0f;  // Normalized X: -1 to 1
    float y = 1.0f - (2.0f * mouse.y) / height; // Normalized Y: -1 to 1
    float z = 1.0f; // z is typically 1.0 for the near plane

    FVector4 rayClip = {x, y, z, 1.0f};

    Mat4 perspMtx;
    u16 perspNorm;
    guPerspectiveF(perspMtx, &perspNorm, gCameraZoom[0], OTRGetAspectRatio(), CM_GetProps()->NearPersp, CM_GetProps()->FarPersp, 1.0f);

    Mat4 inversePerspMtx;
    if (InverseMatrix((float*)&perspMtx, (float*)&inversePerspMtx) != 2) {
        FVector4 rayEye = MultiplyMatrixVector(inversePerspMtx, (float*)&rayClip.x);

        Mat4 lookAtMtx;
        guLookAtF(lookAtMtx, camera->pos[0], camera->pos[1], camera->pos[2], camera->lookAt[0], camera->lookAt[1], camera->lookAt[2], camera->up[0], camera->up[1], camera->up[2]);
        Mat4 inverseViewMtx;
        if (InverseMatrix((float*)&lookAtMtx, (float*)&inverseViewMtx[0][0]) != 2) {
            rayEye.w = 0;
            FVector4 invRayWor = MultiplyMatrixVector(inverseViewMtx, (float*)&rayEye.x);

            FVector direction;
            direction = FVector(invRayWor.x, invRayWor.y, invRayWor.z);

            return direction;
        }
    }
    return FVector(0, 0, 0);
}

bool QueryCollisionRayActor(Vec3f rayOrigin, Vec3f rayDir, Vec3f actorMin, Vec3f actorMax, float* t) {
    float tmin = -FLT_MAX, tmax = FLT_MAX;

    for (int i = 0; i < 3; i++) {
        if (fabs(rayDir[i]) > 1e-6f) { // Avoid division by zero
            float t1 = (actorMin[i] - rayOrigin[i]) / rayDir[i];
            float t2 = (actorMax[i] - rayOrigin[i]) / rayDir[i];

            if (t1 > t2) { float temp = t1; t1 = t2; t2 = temp; }

            tmin = fmax(tmin, t1);
            tmax = fmin(tmax, t2);

            if (tmax < tmin) return false; // No intersection
        } else if (rayOrigin[i] < actorMin[i] || rayOrigin[i] > actorMax[i]) {
            return false; // Ray is outside the slab
        }
    }

    *t = tmin; // Distance to first intersection
    return true;
}

FVector4 MultiplyMatrixVector(float matrix[4][4], float vector[4]) {
    FVector4 result;
    float* resultPtr = &result.x;
    for (int i = 0; i < 4; i++) {
        resultPtr[i] = 0;
        for (int j = 0; j < 4; j++) {
            resultPtr[i] += matrix[j][i] * vector[j];  // Swap [i][j] → [j][i] for column order
        }
    }
    return result;
}

// https://stackoverflow.com/questions/1148309/inverting-a-4x4-matrix
static bool InverseMatrix(const float m[16], float invOut[16]) {
    float inv[16], det;
    int i;

    inv[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] +
             m[13] * m[6] * m[11] - m[13] * m[7] * m[10];

    inv[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] -
             m[12] * m[6] * m[11] + m[12] * m[7] * m[10];

    inv[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] +
             m[12] * m[5] * m[11] - m[12] * m[7] * m[9];

    inv[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] -
              m[12] * m[5] * m[10] + m[12] * m[6] * m[9];

    inv[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] -
             m[13] * m[2] * m[11] + m[13] * m[3] * m[10];

    inv[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] +
             m[12] * m[2] * m[11] - m[12] * m[3] * m[10];

    inv[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] -
             m[12] * m[1] * m[11] + m[12] * m[3] * m[9];

    inv[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] +
              m[12] * m[1] * m[10] - m[12] * m[2] * m[9];

    inv[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] +
             m[13] * m[2] * m[7] - m[13] * m[3] * m[6];

    inv[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] -
             m[12] * m[2] * m[7] + m[12] * m[3] * m[6];

    inv[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] +
              m[12] * m[1] * m[7] - m[12] * m[3] * m[5];

    inv[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] -
              m[12] * m[1] * m[6] + m[12] * m[2] * m[5];

    inv[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] -
             m[9] * m[2] * m[7] + m[9] * m[3] * m[6];

    inv[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] +
             m[8] * m[2] * m[7] - m[8] * m[3] * m[6];

    inv[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] -
              m[8] * m[1] * m[7] + m[8] * m[3] * m[5];

    inv[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] + m[8] * m[1] * m[6] -
              m[8] * m[2] * m[5];

    det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];

    if (det == 0) {
        return false;
    }

    det = 1.0 / det;

    for (i = 0; i < 16; i++) {
        invOut[i] = inv[i] * det;
    }

    return true;
}

FVector TransformVecByMatrix(const FVector& vec, const float mtx[4][4]) {
    FVector result;
    result.x = vec.x * mtx[0][0] + vec.y * mtx[1][0] + vec.z * mtx[2][0] + mtx[3][0];
    result.y = vec.x * mtx[0][1] + vec.y * mtx[1][1] + vec.z * mtx[2][1] + mtx[3][1];
    result.z = vec.x * mtx[0][2] + vec.y * mtx[1][2] + vec.z * mtx[2][2] + mtx[3][2];
    return result;
}

FVector TransformVecDirection(const FVector& dir, const float mtx[4][4]) {
    FVector result;
    result.x = dir.x * mtx[0][0] + dir.y * mtx[1][0] + dir.z * mtx[2][0];
    result.y = dir.x * mtx[0][1] + dir.y * mtx[1][1] + dir.z * mtx[2][1];
    result.z = dir.x * mtx[0][2] + dir.y * mtx[1][2] + dir.z * mtx[2][2];
    return result;
}

Ray RayToLocalSpace(MtxF mtx, const Ray& ray) {
    MtxF inverse;

    if (InverseMatrix((float*)&mtx, (float*)&inverse) != 2) {
        FVector localRayOrigin = TransformVecByMatrix(ray.Origin, (float(*)[4])&inverse);
        FVector localRayDir = TransformVecDirection(ray.Direction, (float(*)[4])&inverse);
        return Ray{localRayOrigin, localRayDir.Normalize()};
    }
    return Ray{}; // Fail. Return empty ray
}

bool IntersectRayTriangle(const Ray& ray, const Triangle& tri, float& t) {
    constexpr float EPSILON = 1e-6f;

    // Adjust the triangle vertices by the object's position
    FVector v0 = tri.v0;
    FVector v1 = tri.v1;
    FVector v2 = tri.v2;

    FVector edge1 = v1 - v0;
    FVector edge2 = v2 - v0;
    FVector h = ray.Direction.Cross(edge2);
    float a = edge1.Dot(h);

    if (std::abs(a) < EPSILON)
        return false; // Ray is parallel to triangle

    float f = 1.0f / a;
    FVector s = ray.Origin - v0;
    float u = f * s.Dot(h);

    if (u < 0.0f || u > 1.0f)
        return false;

    FVector q = s.Cross(edge1);
    float v = f * ray.Direction.Dot(q);

    if (v < 0.0f || u + v > 1.0f)
        return false;

    t = f * edge2.Dot(q);
    return t > EPSILON;
}

bool IntersectRayTriangleAndTransform(const Ray& ray, FVector pos, const Triangle& tri, float& t) {
    constexpr float EPSILON = 1e-6f;

    // Adjust the triangle vertices by the object's position
    FVector v0 = tri.v0 + pos;
    FVector v1 = tri.v1 + pos;
    FVector v2 = tri.v2 + pos;

    FVector edge1 = v1 - v0;
    FVector edge2 = v2 - v0;
    FVector h = ray.Direction.Cross(edge2);
    float a = edge1.Dot(h);

    if (std::abs(a) < EPSILON)
        return false; // Ray is parallel to triangle

    float f = 1.0f / a;
    FVector s = ray.Origin - v0;
    float u = f * s.Dot(h);

    if (u < 0.0f || u > 1.0f)
        return false;

    FVector q = s.Cross(edge1);
    float v = f * ray.Direction.Dot(q);

    if (v < 0.0f || u + v > 1.0f)
        return false;

    t = f * edge2.Dot(q);
    return t > EPSILON;
}

std::optional<FVector> QueryHandleIntersection(MtxF mtx, Ray ray, const Triangle& tri) {
    float t;
    Ray localRay = RayToLocalSpace(mtx, ray);
    if (IntersectRayTriangle(localRay, tri, t)) {
        FVector localClickPosition = localRay.Origin + localRay.Direction * t;
        FVector worldClickPosition = TransformVecByMatrix(localClickPosition, (float(*)[4])&mtx);

        return worldClickPosition; // Stop checking objects if we selected a Gizmo handle
    }
    return std::nullopt;
}

bool IntersectRaySphere(const Ray& ray, const FVector& sphereCenter, float radius, float& t) {
    const float EPSILON = 1e-6f;

    // Vector from ray origin to sphere center
    FVector oc = ray.Origin - sphereCenter;

    // Quadratic equation coefficients
    float a = ray.Direction.Dot(ray.Direction);
    float b = 2.0f * oc.Dot(ray.Direction);
    float c = oc.Dot(oc) - (radius * radius);

    // Compute discriminant
    float discriminant = (b * b) - (4 * a * c);

    // No intersection if discriminant is negative
    if (discriminant < 0) {
        return false;
    }

    // Compute nearest intersection point
    float sqrtD = sqrtf(discriminant);
    float t0 = (-b - sqrtD) / (2.0f * a);
    float t1 = (-b + sqrtD) / (2.0f * a);

    // Select the closest valid intersection
    if (t0 > EPSILON) {
        t = t0;
        return true;
    } else if (t1 > EPSILON) {
        t = t1;
        return true;
    }

    return false; // Sphere is behind the ray origin
}

// bool FindClosestObject(const Ray& ray, const std::vector<GameObject*>& objects, GameObject* outObject, float& outDistance) {
//     float closestDist = std::numeric_limits<float>::max();
//     bool found = false;

//     for (const auto& obj : objects) {
//         for (const auto& tri : obj.Triangles) {
//             float t;
//             if (IntersectRayTriangle(ray, tri, *obj.Pos, t) && t < closestDist) {
//                 closestDist = t;
//                 outObject = obj;
//                 found = true;
//             }
//         }
//     }

//     if (found) {
//         outDistance = closestDist;
//         return true;
//     }
    
//     return false;
// }

// Transform a matrix to a matrix identity
void Editor_MatrixIdentity(Mat4 mtx) {
    register s32 i;
    register s32 k;

    for (i = 0; i < 4; i++) {
        for (k = 0; k < 4; k++) {
            mtx[i][k] = (i == k) ? 1.0f : 0.0f;
        }
    }
}

void Editor_AddMatrix(Mat4 mtx, int32_t flags) {
    EditorMatrix.emplace_back();
    guMtxF2L(mtx, &EditorMatrix.back());
    gSPMatrix(gDisplayListHead++, &EditorMatrix.back(), flags);
}

float CalculateAngle(const FVector& start, const FVector& end) {
    float dot = start.Dot(end);

    float magStart = start.Magnitude();
    float magEnd = end.Magnitude();

    float cosAngle = dot / (magStart * magEnd);
    cosAngle = std::min(1.0f, std::max(-1.0f, cosAngle));

    return acos(cosAngle);
}

void SetDirectionFromRotator(IRotator rot, s8 direction[3]) {
    float yaw = (rot.yaw) * (M_PI / 32768.0f);  // Convert from n64 binary angles 0-0xFFFF 0-360 degrees to radians
    float pitch = rot.pitch * (M_PI / 32768.0f); 

    // Compute unit direction vector
    float x = cosf(yaw) * cosf(pitch);
    float y = -sinf(pitch);
    float z = -sinf(yaw) * cosf(pitch);

    // Scale into -127 to 127 range (not 128 to avoid overflow)
    direction[0] = static_cast<s8>(x * 127.0f);
    direction[1] = static_cast<s8>(y * 127.0f);
    direction[2] = static_cast<s8>(z * 127.0f);

    //printf("Light dir %d %d %d (from rot 0x%X 0x%X 0x%X)\n", direction[0], direction[1], direction[2], rotator[0], rotator[1], rotator[2]);
}

void SetRotatorFromDirection(FVector direction, IRotator* rot) {
    // Compute pitch (inverse of -sinf(pitch))
    float pitch = -asinf(direction.y);

    // Compute yaw (inverse of cosf(yaw) * cosf(pitch))
    float yaw = atan2f(-direction.z, direction.x);

    // Convert back to N64 angles (0-0xFFFF range)
    rot->pitch = (s16)(pitch * (32768.0f / M_PI));
    rot->yaw = (s16)(yaw * (32768.0f / M_PI));
    rot->roll = 0; // Assume no roll, since it's undefined from direction alone
}

FVector GetPositionAheadOfCamera(f32 dist) {
    FVector pos = FVector(cameras[0].pos[0], cameras[0].pos[1], cameras[0].pos[2]);

    f32 pitch = (cameras[0].rot[2] / 65535.0f) * 360.0f;
    f32 yaw = (cameras[0].rot[1] / 65535.0f) * 360.0f;

    // Convert degrees to radians
    pitch = pitch * M_PI / 180.0f;
    yaw = yaw * M_PI / 180.0f;

    // Compute forward vector
    FVector forward(
        -sinf(yaw),  // X
        -sinf(pitch), // Y
        cosf(yaw)               // Z (vertical component)
    );

    // Move 1000 units forward from the camera position
    return pos + (forward * dist);
}