#include "cute_c2.h"
#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 > 1) return;

    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
        );
    }
}

void check_collision_and_move(struct Shape* shape, const struct TOIInfo* toi) {
    if (toi->toi < 1)
    {
        Vector2 normal = {-toi->normal.x, -toi->normal.y};
        Vector2 nV =
            Vector2Scale(
                normal,
                fabs(
                Vector2DotProduct(
                    normal,
                    Vector2Scale(shape->vel, 1.0-toi->toi)
                ) - 0.01)
            );
        shape->vel = Vector2Add(
            shape->vel, nV
        );
    }
}

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


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

    Shape B = {
        .pos = {200,300},
        .center = {200,300},
        .type = C2_TYPE_AABB,
        .shape.boxDim = {256, 64},
        .colour = RED,
    };

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

    Shape D = {
        .pos = {460,243},
        .center = {460,243},
        .type = C2_TYPE_AABB,
        .shape.boxDim = {64, 50},
        .colour = RED,
    };

    Shape E = {
        .pos = {427,219},
        .center = {349,217.666},
        .type = C2_TYPE_POLY,
        .shape.poly = {
            .count = 3,
            .verts = {
                {0,0},
                {100, -50},
                {100,0},
            },
        },
        .colour = RED,
    };
    c2MakePoly(&E.shape.poly);
    for (int i = 0; i < E.shape.poly.count; ++i) {
        E.shape.poly.verts[i].x += E.pos.x;
        E.shape.poly.verts[i].y += E.pos.y;
    }

    while (!WindowShouldClose())
    {
        float frame_time = 1.0/60;
        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);
        A.vel = Vector2Scale(move_dir, 400);
        A.vel.y += 15000 * frame_time;

        c2AABB Abox= {
            {A.center.x - A.shape.boxDim.x / 2, A.center.y - A.shape.boxDim.y / 2},
            {A.center.x + A.shape.boxDim.x / 2, A.center.y + A.shape.boxDim.y / 2},
        };
        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 Dbox= {
            {D.center.x - D.shape.boxDim.x / 2, D.center.y - D.shape.boxDim.y / 2},
            {D.center.x + D.shape.boxDim.x / 2, D.center.y + D.shape.boxDim.y / 2},
        };

        // Actually need to order via TOI and resolve
        // For each resolve, recheck TOI
        struct TOIInfo toi4 = {0};
        toi4.toi = AABBToPolyTOI(
            Abox, (c2v){A.vel.x *frame_time, A.vel.y * frame_time},
            &E.shape.poly, NULL, (c2v){0,0},
            &toi4.normal, &toi4.contact
        );
        check_collision_and_move(&A, &toi4);
        struct TOIInfo toi1 = {0};
        toi1.toi = AABBToPolyTOI(
            Abox, (c2v){A.vel.x *frame_time, A.vel.y * frame_time},
            &C.shape.poly, NULL, (c2v){0,0},
            &toi1.normal, &toi1.contact
        );
        check_collision_and_move(&A, &toi1);

        struct TOIInfo toi2 = {0};
        toi2.toi = AABBToAABBTOI(
            Abox, (c2v){A.vel.x *frame_time, A.vel.y * frame_time},
            Dbox, (c2v){0,0},
            &toi2.normal, &toi2.contact
        );
        //check_collision_and_move(&A, &toi2);

        struct TOIInfo toi0 = {0};
        toi0.toi = AABBToAABBTOI(
            Abox, (c2v){A.vel.x *frame_time, A.vel.y * frame_time},
            Bbox, (c2v){0,0},
            &toi0.normal, &toi0.contact
        );

        check_collision_and_move(&A, &toi0);


        A.pos = Vector2Add(
            A.pos , Vector2Scale(A.vel, frame_time)
        );
        A.center = A.pos;

        char buf[32];
        BeginDrawing();
            ClearBackground(RAYWHITE);
            DrawShape(&B);
            DrawShape(&C);
            //DrawShape(&D);
            DrawShape(&E);
            draw_toi_info(&toi0);
            draw_toi_info(&toi1);
            draw_toi_info(&toi2);
            sprintf(buf, "TOI: %.3f, %.3f, %.3f", toi0.toi, toi1.toi, toi2.toi);
            DrawText(buf, 16, 16, 12, BLACK);
            sprintf(buf, "Velocity: %.3f, %.3f", A.vel.x, A.vel.y);
            DrawText(buf, 16, 32, 12, BLACK);


            DrawShape(&A);
        EndDrawing();
    }
    return 0;
}