#include "cute_c2_ext.h"
#include "raylib.h"
#include "raymath.h"

#include <stdio.h>

const int screenWidth = 800;
const int screenHeight = 450;

typedef struct Shape {
    Vector2 pos;
    Vector2 center;
    Vector2 vel;
    C2_TYPE type;
    union {
        c2v boxDim;
        c2Circle circle;
        c2Poly poly;
    } shape;
    Color colour;
} Shape;


void DrawShape(const Shape* shape) {
    switch (shape->type) 
    {
        case C2_TYPE_AABB:
            DrawRectangle(
                shape->center.x - shape->shape.boxDim.x / 2,
                shape->center.y - shape->shape.boxDim.y / 2,
                shape->shape.boxDim.x,
                shape->shape.boxDim.y,
                shape->colour
            );
        break;
        case C2_TYPE_CIRCLE:
            DrawCircle(
                shape->center.x,
                shape->center.y, shape->shape.circle.r,
                shape->colour
            );
        break;
        case C2_TYPE_POLY:
            for (int i = 0; i < shape->shape.poly.count; ++i) {
                int next = (i + 1 + shape->shape.poly.count) % shape->shape.poly.count;
                DrawLineV(
                    (Vector2){shape->shape.poly.verts[i].x, shape->shape.poly.verts[i].y},
                    (Vector2){shape->shape.poly.verts[next].x, shape->shape.poly.verts[next].y},
                    shape->colour
                );
            }
        break;
        default:
        break;
    }
}

struct TOIInfo {
    double toi;
    c2v contact;
    c2v normal;
};

void draw_toi_info(const struct TOIInfo* info)
{
    if (info->toi != INFINITY)
    {
        DrawCircle(info->contact.x, info->contact.y, 4, GREEN);
        DrawLineEx(
            (Vector2){info->contact.x, info->contact.y},
            (Vector2){
                info->contact.x + info->normal.x * 12,
                info->contact.y + info->normal.y * 12
            },
            2, GREEN
        );
    }
}

int main(void)
{
    InitWindow(screenWidth, screenHeight, "raylib");
    SetTargetFPS(60);

    Shape A = {
        .pos = {450,150},
        .center = {450,150},
        .type = C2_TYPE_CIRCLE,
        .shape.circle = {{0, 0}, 64},
        .colour = RED,
    };

    Shape B = {
        .pos = {300,150},
        .center = {300,150},
        .type = C2_TYPE_AABB,
        .shape.boxDim = {32, 32},
        .colour = BLUE,
    };

    Shape C = {
        .pos = {200,100},
        .center = {200,100},
        .type = C2_TYPE_AABB,
        .shape.boxDim = {64, 64},
        .colour = RED,
    };

    Shape D = {
        .pos = {200,250},
        .center = {250,266.666},
        .type = C2_TYPE_POLY,
        .shape.poly = {
            .count = 3,
            .verts = {
                {0,0},
                {100, 0},
                {50,50},
            },
        },
        .colour = RED,
    };
    c2MakePoly(&D.shape.poly);
    for (int i = 0; i < D.shape.poly.count; ++i) {
        D.shape.poly.verts[i].x += D.pos.x;
        D.shape.poly.verts[i].y += D.pos.y;
    }

    while (!WindowShouldClose())
    {
        float frame_time = GetFrameTime();
        Vector2 move_dir = {0};

        if (IsKeyDown(KEY_LEFT)) {
            move_dir.x += -1;
        }
        if (IsKeyDown(KEY_RIGHT)) {
            move_dir.x += 1;
        }
        if (IsKeyDown(KEY_UP)) {
            move_dir.y += -1;
        }
        if (IsKeyDown(KEY_DOWN)) {
            move_dir.y += 1;
        }

        move_dir = Vector2Normalize(move_dir);
        B.vel = Vector2Scale(move_dir, 200);
  
        B.pos = Vector2Add(
            B.pos , Vector2Scale(B.vel, frame_time)
        );
        B.center = B.pos;
        
        c2Circle Acirc= {
            {A.center.x, A.center.y},
            A.shape.circle.r
        };
        c2AABB Bbox= {
            {B.center.x - B.shape.boxDim.x / 2, B.center.y - B.shape.boxDim.y / 2},
            {B.center.x + B.shape.boxDim.x / 2, B.center.y + B.shape.boxDim.y / 2},
        };
        c2AABB Cbox= {
            {C.center.x - C.shape.boxDim.x / 2, C.center.y - C.shape.boxDim.y / 2},
            {C.center.x + C.shape.boxDim.x / 2, C.center.y + C.shape.boxDim.y / 2},
        };

        const float TEST_VELOCITY = 100;

        Vector2 test_spd = Vector2Normalize(Vector2Subtract(C.center, B.center));
        c2v test_velocityB = {test_spd.x * TEST_VELOCITY, test_spd.y * TEST_VELOCITY};
        const c2v static_velocity = {0,0};

        struct TOIInfo toi0 = {0};
        toi0.toi = AABBToAABBTOI(
            Bbox, test_velocityB,
            Cbox, static_velocity,
            &toi0.normal, &toi0.contact
        );
        struct Shape projB0 = B;
        projB0.pos = Vector2Add(
            projB0.pos ,
            (Vector2){test_velocityB.x * toi0.toi, test_velocityB.y * toi0.toi}
        );
        projB0.center = projB0.pos;
        projB0.colour = PINK;

        test_spd = Vector2Normalize(Vector2Subtract(A.center, B.center));
        test_velocityB = (c2v){test_spd.x * TEST_VELOCITY, test_spd.y * TEST_VELOCITY};
        struct TOIInfo toi1 = {0};
        toi1.toi = AABBToCircleTOI(
            Bbox, test_velocityB,
            Acirc, static_velocity,
            &toi1.normal, &toi1.contact
        );
        struct Shape projB1 = B;
        projB1.pos = Vector2Add(
            projB1.pos ,
            (Vector2){test_velocityB.x * toi1.toi, test_velocityB.y * toi1.toi}
        );
        projB1.center = projB1.pos;
        projB1.colour = PINK;

        test_spd = Vector2Normalize(Vector2Subtract(D.center, B.center));
        test_velocityB = (c2v){test_spd.x * TEST_VELOCITY, test_spd.y * TEST_VELOCITY};
        struct TOIInfo toi2 = {0};
        toi2.toi = AABBToPolyTOI(
            Bbox, test_velocityB,
            &D.shape.poly, NULL, static_velocity,
            &toi2.normal, &toi2.contact
        );
        struct Shape projB2 = B;
        projB2.pos = Vector2Add(
            projB2.pos ,
            (Vector2){test_velocityB.x * toi2.toi, test_velocityB.y * toi2.toi}
        );
        projB2.center = projB2.pos;
        projB2.colour = PINK;

        BeginDrawing();
            ClearBackground(RAYWHITE);
            DrawShape(&A);
            DrawShape(&C);
            DrawShape(&D);
            if (toi0.toi <= 1)
            {
                DrawShape(&projB0);
                draw_toi_info(&toi0);
            }
            if (toi1.toi <= 1)
            {
                DrawShape(&projB1);
                draw_toi_info(&toi1);
            }
            if (toi2.toi <= 1)
            {
                DrawShape(&projB2);
                draw_toi_info(&toi2);
            }

            DrawShape(&B);
            DrawLineEx(A.center, B.center, 1, BLACK);
            DrawLineEx(C.center, B.center, 1, BLACK);
            DrawLineEx(D.center, B.center, 1, BLACK);
            //DrawText(buf, A.pos.x, A.pos.y, 12, BLACK);
        EndDrawing();
    }
    return 0;
}