#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 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->pos.x - shape->shape.boxDim.x / 2,
                shape->pos.y - shape->shape.boxDim.y / 2,
                shape->shape.boxDim.x,
                shape->shape.boxDim.y,
                shape->colour
            );
        break;
        case C2_TYPE_CIRCLE:
            DrawCircle(
                shape->pos.x,
                shape->pos.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 = {300,150},
        .type = C2_TYPE_CIRCLE,
        //.shape.box = {{0, 0}, {64, 64}},
        .shape.circle = {{0, 0}, 64},
        .colour = RED,
    };

    Shape B = {
        .pos = {43,60},
        .type = C2_TYPE_AABB,
        .shape.boxDim = {32, 32},
        .colour = BLUE,
    };

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

    Shape D = {
        .pos = {200,300},
        .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, 100);
  
        B.pos = Vector2Add(
            B.pos , Vector2Scale(B.vel, frame_time)
        );
        
        c2Circle Acirc= {
            {A.pos.x, A.pos.y},
            A.shape.circle.r
        };
        c2AABB Bbox= {
            {B.pos.x - B.shape.boxDim.x / 2, B.pos.y - B.shape.boxDim.y / 2},
            {B.pos.x + B.shape.boxDim.x / 2, B.pos.y + B.shape.boxDim.y / 2},
        };
        c2AABB Cbox= {
            {C.pos.x - C.shape.boxDim.x / 2, C.pos.y - C.shape.boxDim.y / 2},
            {C.pos.x + C.shape.boxDim.x / 2, C.pos.y + C.shape.boxDim.y / 2},
        };

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

        struct TOIInfo toi0 = {0};
        toi0.toi = AABBToAABBTOI(
            Bbox, test_velocityB,
            Cbox, static_velocity,
            &toi0.normal, &toi0.contact
        );

        test_spd = Vector2Normalize(Vector2Subtract(A.pos, B.pos));
        test_velocityB = (c2v){test_spd.x * 100, test_spd.y * 100};
        struct TOIInfo toi1 = {0};
        toi1.toi = AABBToCircleTOI(
            Bbox, test_velocityB,
            Acirc, static_velocity,
            &toi1.normal, &toi1.contact
        );

        test_spd = Vector2Normalize(Vector2Subtract(D.pos, B.pos));
        test_velocityB = (c2v){test_spd.x * 100, test_spd.y * 100};
        struct TOIInfo toi2 = {0};
        toi2.toi = AABBToPolyTOI(
            Bbox, test_velocityB,
            &D.shape.poly, NULL, static_velocity,
            &toi2.normal, &toi2.contact
        );

        //char buf[32];
        BeginDrawing();
            ClearBackground(RAYWHITE);
            DrawShape(&A);
            DrawShape(&C);
            DrawShape(&D);
            DrawShape(&B);
            DrawLineEx(B.pos, A.pos, 1, BLACK);
            DrawLineEx(C.pos, A.pos, 1, BLACK);
            DrawLineEx(D.pos, A.pos, 1, BLACK);
            //DrawText(buf, A.pos.x, A.pos.y, 12, BLACK);
            draw_toi_info(&toi0);
            draw_toi_info(&toi1);
            draw_toi_info(&toi2);
        EndDrawing();
    }
    return 0;
}