#include "raylib.h"
#include <cmath>

void handleRightClick(Camera3D camera, Vector3* target_position) {
    Ray ray = GetMouseRay(GetMousePosition(), camera);
    if (ray.direction.y > 0)
        return;

    // Calculate t (distance along ray to ground plane at y=0)
    float t = 0 - ray.position.y / ray.direction.y;

    // Calculate where ray hits the ground
    target_position->x = ray.position.x + (ray.direction.x * t);
    target_position->z = ray.position.z + (ray.direction.z * t);
}

int main() {
    InitWindow(900, 600, "My First 3D Cube");

    Camera3D camera = {0};
    camera.position = (Vector3){10.0f, 10.0f, 10.0f};
    camera.target = (Vector3){0.0f, 0.0f, 0.0f};
    camera.up = (Vector3){0.0f, 2.0f, 0.0f};
    camera.fovy = 90.0f;
    camera.projection = CAMERA_PERSPECTIVE;

    Vector3 cube_position = {0, 1.0, 0};
    Vector3 target_position = {0, 0, 0};
    float move_speed = 0.001f;

    while (!WindowShouldClose()) {

        if (IsMouseButtonPressed(MOUSE_BUTTON_RIGHT)) {
            handleRightClick(camera, &target_position);
        }

        // Movement to target
        if(cube_position.x != target_position.x || cube_position.z != target_position.z) {
            float xdiff = target_position.x - cube_position.x;
            float zdiff = target_position.z - cube_position.z;
            float distance = sqrtf(pow(xdiff, 2) + pow(zdiff, 2));

            // snap to target
            if (distance < move_speed) {
                cube_position.x = target_position.x;
                cube_position.z = target_position.z;
                continue;
            }

            cube_position.x += (xdiff/distance) * move_speed;
            cube_position.z += (zdiff/distance) * move_speed;
        }

        BeginDrawing();
        ClearBackground((Color){0, 0, 0, 0});

        BeginMode3D(camera);
        DrawPlane((Vector3){0, 0, 0}, (Vector2) {500, 500}, BLUE);
        DrawCube(cube_position, 2.0f, 2.0f, 2.0f, RED);
        //DrawGrid(100, 1.0f);
        EndMode3D();

        EndDrawing();
    }

    CloseWindow();
    return 0;
}