#include "raylib.h"
#include "raymath.h"
#include "reader.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "arrayfuncs.h"
#include "c3dtypes.h"
#include "vecfunc.h"

const int RENDERWIDTH = 1920;
const int RENDERHEIGHT = 1080;

const int HALFWIDTH = RENDERWIDTH / 2;
const int HALFHEIGHT = RENDERHEIGHT / 2;

const int SCREENWIDTH = 1280;
const int SCREENHEIGHT = 720;

const float WIDTHSCALE = (float)RENDERWIDTH / SCREENWIDTH;
const float HEIGHTSCALE = (float)RENDERHEIGHT / SCREENHEIGHT;

const float fov = 70.0 / 180.0 * 3.14159265369;
const float half_fov = fov / 2;

float proj; // HALFWIDTH / tan(half_fov) Inited later

bool printdebug;

static inline Vector3 Tri2DBaryAtPoint(Tri2D *t, Vector2 p) {
    Vector3 retvec;
    Vector2 v0 = Vector2Subtract(t->b, t->a); // cachable
    Vector2 v1 = Vector2Subtract(t->c, t->a); // cacheable
    Vector2 v2 = Vector2Subtract(p, t->a);
    float d00 = Vector2DotProduct(v0, v0); // cacheable
    float d01 = Vector2DotProduct(v0, v1); // cacheable
    float d11 = Vector2DotProduct(v1, v1); // cahceable
    float d20 = Vector2DotProduct(v2, v0);
    float d21 = Vector2DotProduct(v2, v1);
    float denom = d00 * d11 - d01 * d01; // cache
    retvec.x = (d11 * d20 - d01 * d21) / denom;
    retvec.y = (d00 * d21 - d01 * d20) / denom;
    retvec.z = 1.0 - retvec.x - retvec.y;
    return retvec;
}

Vector2 BaryAndTritoPoint(Tri2D *t, Vector3 bary) {
    Vector2 retvec;
    retvec.x = bary.x * t->a.x + bary.y * t->b.x + bary.z * t->c.x;
    retvec.y = bary.y * t->a.y + bary.y * t->b.y + bary.z * t->c.y;
    return retvec;
}

static inline float DepthAtBary(Tri2D *t, Vector3 bary) {
    float firstrepz = 1 / t->adepth;
    float secondrepz = 1 / t->bdepth;
    float thirdrepz = 1 / t->cdepth;

    float finrepz = firstrepz * bary.x + secondrepz * bary.y + thirdrepz * bary.z;

    return 1 / finrepz;
}

double Min(double a, double b) {
    if (a > b) {
        return b;
    }
    return a;
}

double Max(double a, double b) {
    if (a < b) {
        return b;
    }
    return a;
}

static inline int IndexOfZBuff(int row, int col) { return row + RENDERWIDTH * col; }

// sort triangle verts so that point A is the "highest" point (lowest y val) and
// point C is the "lowest" pont (highest y val)
void Tri2DSortByY(Tri2D *t) {
    Vector2 temp;
    if (t->a.y > t->b.y) {
        temp = t->a;
        t->a = t->b;
        t->b = temp;
    }
    if (t->b.y > t->c.y) {
        temp = t->b;
        t->b = t->c;
        t->c = temp;
    }
    if (t->a.y > t->b.y) {
        temp = t->a;
        t->a = t->b;
        t->b = temp;
    }
}

// Draws triangle with a flat top. Note A and B must be the top points with C
// being the bottom "spike"
void FillTopFlatZbuffer(Zee *zee, Tri2D *t, Tri2D *tp) {
    if (t->b.x < t->a.x) {
        Vector2 e = t->b;
        t->b = t->a;
        t->a = e;
    }
    // Becasue we are trying to get the x values in terms of the y values, we
    // need inverse slope
    float atocslopeinv =
        (t->c.x - t->a.x) / (t->c.y - t->a.y); // dif in x from start to end with a and c
    float btocslopinv =
        (t->c.x - t->b.x) / (t->c.y - t->b.y); // dif in x from start to end with b and c

    // start at bottom of triangle (point c) so that we do not need to determine
    // which vertex is on which side and increment it with its proper slope
    double curx1 = t->c.x;
    double curx2 = t->c.x;

    for (int scanline = t->c.y; scanline >= t->a.y; scanline--) {
        if (0 <= scanline && scanline < RENDERHEIGHT) {
            for (int i = Max(curx1, 0); i < Min(curx2, RENDERWIDTH); i++) {
                /* zee[IndexOfZBuff(i, scanline)].triangle = tp; */

                Vector3 baryatpoint = Tri2DBaryAtPoint(tp, (Vector2){i, scanline});
                float depth = DepthAtBary(tp, baryatpoint);
                if (depth > zee[IndexOfZBuff(i, scanline)].depth) {
                    zee[IndexOfZBuff(i, scanline)].triangle = tp;
                    zee[IndexOfZBuff(i, scanline)].depth = depth;
                    // printf("here\n");
                }
            }
        }
        curx1 -= atocslopeinv; // subtract because we are working backwards (reason
                               // why we start with point c in slope equtn)
        curx2 -= btocslopinv;
    }
}
void PrintTri2D(Tri2D t) {
    printf("{(TRI2D) A: (%f, %f), B: (%f, %f), C:(%f,%f) }\n ", t.a.x, t.a.y, t.b.x, t.b.y, t.c.x,
           t.c.y);
}

// Draws triangle with a flat bottomp. Note B and C must be the bottom points
// with A being the top "spike"
void FillBottomFlatZbuffer(Zee *zee, Tri2D *t, Tri2D *tp) {
    if (t->c.x < t->b.x) {
        Vector2 e = t->c;
        t->c = t->b;
        t->b = e;
    }
    // Becasue we are trying to get the x values in terms of the y values, we
    // need inverse slope
    float atobslopeinv =
        (t->b.x - t->a.x) / (t->b.y - t->a.y); // dif in x from start to end with a and c
    float atocslopeinv =
        (t->c.x - t->a.x) / (t->c.y - t->a.y); // dif in x from start to end with b and c

    // start at top of triangle (point c) so that we do not need to determine
    // which vertex is on which side and increment it with its proper slope
    double curx1 = t->a.x;
    double curx2 = t->a.x;

    for (int scanline = t->a.y; scanline < t->c.y;
         scanline++) { // TODO: Possibly more optimization possible here, use linear correspondance
                       // for y, not just x to get depth
        if (0 <= scanline && scanline < RENDERHEIGHT) {
            Vector3 f1baryatpoint = Tri2DBaryAtPoint(tp, (Vector2){Max(curx1, 0), scanline});
            float f1depth = DepthAtBary(tp, f1baryatpoint);

            Vector3 f1baryatpoint2 = Tri2DBaryAtPoint(tp, (Vector2){Max(curx1, 0) + 1, scanline});
            float f1depth2 = DepthAtBary(tp, f1baryatpoint2);
            float dslope = f1depth2 - f1depth;

            for (int i = Max(curx1, 0); i < Min(curx2, RENDERWIDTH); i++) {
                /* zee[IndexOfZBuff(i, scanline)].triangle = tp; */

                /* Vector3 baryatpoint = Tri2DBaryAtPoint(tp, (Vector2){i, scanline}); */
                /* float depth = DepthAtBary(tp, baryatpoint); */

                float aproxdepth = f1depth + (dslope * ((float)i - (float)(Max(curx1, 0))));

                if (aproxdepth > zee[IndexOfZBuff(i, scanline)].depth) {
                    zee[IndexOfZBuff(i, scanline)].triangle = tp;
                    zee[IndexOfZBuff(i, scanline)].depth = aproxdepth;
                    // printf("here\n");
                }
            }
        }
        curx1 += atobslopeinv;
        curx2 += atocslopeinv;
    }
}

void DrawTriZuff(Zee *zbuf, Tri2D *t) {
    Tri2DSortByY(t);

    if (t->b.y == t->c.y) { // if bottom of triangle is flat
        FillBottomFlatZbuffer(zbuf, t, t);
    } else if (t->a.y == t->b.y) { // if top of triangle is flat

        FillTopFlatZbuffer(zbuf, t, t);

    } else {        // funny split tri
        Vector2 v4; // v4 is the vertex on the line between a and c. It is used
                    // to split the triangle into a top and bottom
        v4.y = t->b.y;
        float slope =
            (float)((t->c.x) - (t->a.x)) / ((float)(t->c.y - t->a.y)); // get slope in run over rise
                                                                       // becasue we need to find x
        float changeiny = (float)(t->b.y - t->a.y);
        float officalxpos = t->a.x + (slope * changeiny);
        v4.x = officalxpos;
        Tri2D bottomflattrires;
        bottomflattrires.a = t->a;
        bottomflattrires.b = t->b;
        bottomflattrires.c = v4;
        bottomflattrires.color = t->color;
        Tri2D topflattrires;
        topflattrires.a = t->b;
        topflattrires.b = v4;
        topflattrires.c = t->c;
        topflattrires.color = t->color;

        FillBottomFlatZbuffer(zbuf, &bottomflattrires, t);
        FillTopFlatZbuffer(zbuf, &topflattrires, t);
    }
}

float Sign(Vector2 *v1, Vector2 *v2, Vector2 *v3) {
    return (v1->x - v3->x) * (v2->y - v3->y) - (v2->x - v3->x) * (v1->y - v3->y);
}

static inline bool IsInTri(Tri2D tri, Vector2 p) {
    float d1, d2, d3;
    bool has_neg, has_pos;

    d1 = Sign(&p, &tri.a, &tri.b);
    d2 = Sign(&p, &tri.b, &tri.c);
    d3 = Sign(&p, &tri.c, &tri.a);

    has_neg = (d1 < 0) || (d2 < 0) || (d3 < 0);
    has_pos = (d1 > 0) || (d2 > 0) || (d3 > 0);

    return !(has_neg && has_pos);
}

void CtrlLocalCam(LocalCam *cam, float time) {
    cam->velocity.x = 0;
    cam->velocity.y = 0;
    cam->velocity.z = 0;
    cam->angleVelocity.x = 0;
    cam->angleVelocity.y = 0;
    cam->angleVelocity.z = 0;

    if (IsKeyDown(KEY_W)) {
        Vector3 forceForward = (Vector3){0, 0, -500};
        Vector3 rotatedforce = RotateAboutY(forceForward, cam->angles.y);
        // printf("%f", cam->angles.y);
        // Vector3Print(rotatedforce);
        cam->velocity = Vector3Add(cam->velocity, rotatedforce);

        // cam->velocity.z = cam->velocity.z + 50000*time*sin(cam->angles.y);
        // cam->velocity.x = cam->velocity.x + 500*time*cos(cam->angles.y);
    }
    if (IsKeyDown(KEY_S)) {
        Vector3 forceForward = (Vector3){0, 0, 500};
        Vector3 rotatedforce = RotateAboutY(forceForward, cam->angles.y);
        cam->velocity = Vector3Add(cam->velocity, rotatedforce);

        // cam->velocity.z = cam->velocity.z + 50000*time*sin(cam->angles.y);
        // cam->velocity.x = cam->velocity.x + 500*time*cos(cam->angles.y);
    }

    if (IsKeyDown(KEY_A)) {
        Vector3 forceForward = (Vector3){-500, 0, 0};
        Vector3 rotatedforce = RotateAboutY(forceForward, cam->angles.y);
        cam->velocity = Vector3Add(cam->velocity, rotatedforce);

        // cam->velocity.z = cam->velocity.z + 50000*time*sin(cam->angles.y);
        // cam->velocity.x = cam->velocity.x + 500*time*cos(cam->angles.y);
    }
    if (IsKeyDown(KEY_D)) {
        Vector3 forceForward = (Vector3){500, 0, 0};
        Vector3 rotatedforce = RotateAboutY(forceForward, cam->angles.y);
        cam->velocity = Vector3Add(cam->velocity, rotatedforce);

        // cam->velocity.z = cam->velocity.z + 50000*time*sin(cam->angles.y);
        // cam->velocity.x = cam->velocity.x + 500*time*cos(cam->angles.y);
    }

    if (IsKeyDown(KEY_SPACE)) {
        cam->velocity.y = cam->velocity.y - 50;
    }

    if (IsKeyDown(KEY_LEFT_SHIFT)) {
        cam->velocity.y = cam->velocity.y + 50;
    }

    if (IsKeyDown(KEY_Q)) {
        cam->angleVelocity.y = cam->angleVelocity.y + time * 30;
    }
    if (IsKeyDown(KEY_E)) {
        cam->angleVelocity.y = cam->angleVelocity.y - time * 30;
    }

    if (IsKeyDown(KEY_G)) {
        printf("PosX: %f PosY: %f PosZ: %f\n", cam->position.x, cam->position.y, cam->position.z);
        printf("RotX; %f RotY: %f RotZ: %f\n", cam->angles.x, cam->angles.y, cam->angles.z);
    }
}

void LocalCamApplyVelo(LocalCam *cam, float time) {
    cam->position.x = cam->position.x + cam->velocity.x * time;
    cam->position.y = cam->position.y + cam->velocity.y * time;
    cam->position.z = cam->position.z + cam->velocity.z * time;

    cam->angles.x = cam->angles.x + cam->angleVelocity.x * time;
    cam->angles.y = cam->angles.y + cam->angleVelocity.y * time;
    cam->angles.z = cam->angles.z + cam->angleVelocity.z * time;
    // printf("%f %f %f\n",cam->angles.x, cam->angles.y, cam->angles.z);
}

Vector2 Conv3Dto2D(Vector3 v) {
    Vector2 returnvector;
    returnvector.x = proj * v.x / (-v.z);
    returnvector.y = proj * v.y / (-v.z);
    return returnvector;
}

Vector2 Conv2DCenteredToScreen(Vector2 v) {
    Vector2 returnvector;
    returnvector.x = v.x + HALFWIDTH;
    returnvector.y = v.y + HALFHEIGHT;
    return returnvector;
}

static inline Vector3 TransformWithCam(Vector3 v, LocalCam *cam) {
    Vector3 returnvector;
    returnvector.x = v.x - cam->position.x;
    returnvector.y = v.y - cam->position.y;
    returnvector.z = v.z - cam->position.z;

    returnvector = RotateAboutZ(returnvector, cam->angles.z);
    returnvector = RotateAboutY(returnvector, -cam->angles.y);
    returnvector = RotateAboutX(returnvector, -cam->angles.x);

    // printf("Before: %f %f %f, After: %f %f %f\n", v.x, v.y, v.z,
    // returnvector.x, returnvector.y, returnvector.z);
    return returnvector;
}

Tri TriTransformWithCam(Tri *t, LocalCam *cam) {
    Tri rettri;
    rettri.a = TransformWithCam(t->a, cam);
    rettri.b = TransformWithCam(t->b, cam);
    rettri.c = TransformWithCam(t->c, cam);

    rettri.color = t->color;

    return rettri;
}

Tri2D ConvertTriToTri2D(Tri *t) {
    Tri2D rettri2d;
    rettri2d.a = Conv3Dto2D(t->a);
    rettri2d.b = Conv3Dto2D(t->b);
    rettri2d.c = Conv3Dto2D(t->c);

    rettri2d.a = Conv2DCenteredToScreen(rettri2d.a);
    rettri2d.b = Conv2DCenteredToScreen(rettri2d.b);
    rettri2d.c = Conv2DCenteredToScreen(rettri2d.c);

    rettri2d.adepth = t->a.z;
    rettri2d.bdepth = t->b.z;
    rettri2d.cdepth = t->c.z;

    rettri2d.color = t->color;
    return rettri2d;
}

int main() {
    printf("%d\n", TestFunc(5));
    proj = HALFWIDTH / tan(half_fov);

    SetConfigFlags(FLAG_WINDOW_UNDECORATED);
    InitWindow(SCREENWIDTH, SCREENHEIGHT, "raylib [core] example - basic window");

    // SetWindowPosition(0,1080);
    Vector2 a = GetMonitorPosition(0);
    int mh = GetMonitorHeight(0);
    int mw = GetMonitorWidth(0);
    int w = SCREENWIDTH;
    int h = SCREENHEIGHT;
    printf("mh:%d mw:%d w:%d h:%d\n", mh, mw, w, h);
    SetWindowPosition(a.x + (0.5 * mw) - (w / 2), a.y + (0.5 * mh) - (0.5 * h));

    RenderTexture2D uiraylibtexture = LoadRenderTexture(RENDERWIDTH, RENDERHEIGHT);

    RenderTexture2D render3dtexture = LoadRenderTexture(RENDERWIDTH, RENDERHEIGHT);

    Texture2D directaccesstex;

    // Init cube model
    // TODO: Load from obj

    LocalCam camera;
    camera.position = (Vector3){0, 0, 0};
    camera.acceleration = (Vector3){0, 0, 0};
    camera.angleAcceleration = (Vector3){0, 0, 0};
    camera.angles = (Vector3){0, 0, 0};
    camera.velocity = (Vector3){0, 0, 0};
    camera.angleVelocity = (Vector3){0, 0, 0};

    Vector3 point = (Vector3){0, 0, -10};

    Tri internaltriarray[50];
    TriArray tarr;
    tarr.arr = internaltriarray;
    tarr.length = 0;
    TriArrayAppend(&tarr, (Tri){(Vector3){0, 0, -1000}, (Vector3){0, 800, -1000},
                                (Vector3){800, 800, -1000}, WHITE});

    TriArrayAppend(&tarr, (Tri){(Vector3){0, 0, -2000}, (Vector3){0, 800, -2000},
                                (Vector3){800, 800, -2000}, BLUE});

    Tri internaltransformedtriarray[50];
    TriArray TransformedTris;
    TransformedTris.arr = internaltransformedtriarray;
    TransformedTris.length = 0;

    Tri2D internaltri2darray[50];
    Tri2DArray Tri2Darr;
    Tri2Darr.length = 0;
    Tri2Darr.arr = internaltri2darray;

    // static Zee ZBuff[1920][1080] = {{(Zee){10000,NULL}}}; //FIXME: Stupid
    // static makes the file 32 Megs because pog
    Zee *ZBuff = malloc(RENDERHEIGHT * RENDERWIDTH * sizeof(Zee));
    static Color display[1920 * 1080 * 4];
    memset(display, 0, sizeof(display));

    Tri2D funners =
        (Tri2D){(Vector2){50, 50}, (Vector2){500, 50}, (Vector2){500, 500}, 0, 0, 0, GREEN};
    Tri2D funners2 =
        (Tri2D){(Vector2){600, 0}, (Vector2){600, 500}, (Vector2){1000, 500}, 0, 0, 0, RED};

    Tri2D fullscreentritop =
        (Tri2D){(Vector2){0, 0}, (Vector2){1920, 0}, (Vector2){1920, 1080}, 0, 0, 0, BLUE};
    Tri2D fullscreentribottom =
        (Tri2D){(Vector2){0, 0}, (Vector2){0, 1080}, (Vector2){1920, 1080}, 0, 0, 0, RED};

    Tri2D blank = (Tri2D){(Vector2){-10, -10}, (Vector2){-10, -10}, (Vector2){-10, -10}};

    Tri2D norm = (Tri2D){(Vector2){500, 50}, (Vector2){0, 0}, (Vector2){250, 500}, 0, 0, 0, GREEN};

    bool run3d = true;
    while (!WindowShouldClose() && run3d) {
        float frametime = GetFrameTime();
        CtrlLocalCam(&camera, frametime);
        LocalCamApplyVelo(&camera, frametime);
        // ClearBackground(BLACK);

        /* Vector3 TransVector = TransformWithCam(point,&camera); */
        /* if (TransVector.z < 0) { */
        /* Vector2 MPos = Conv3Dto2D(TransVector); */
        /* Vector2 FinPos = Conv2DCenteredToScreen(MPos); */
        /* DrawCircleV(FinPos,100,BLACK); */
        /* } */
        /* EndTextureMode(); */

        TransformedTris.length = 0;
        Tri2Darr.length = 0;

        for (int i = 0; i < tarr.length; i++) {
            TriArrayAppend(&TransformedTris, TriTransformWithCam(&tarr.arr[i], &camera));
        }

        for (int i = 0; i < TransformedTris.length; i++) {
            if ((TransformedTris.arr[i].a.z < 0) && (TransformedTris.arr[i].b.z < 0) &&
                (TransformedTris.arr[i].c.z < 0)) {
                Tri2DArrayAppend(&Tri2Darr, ConvertTriToTri2D(&TransformedTris.arr[i]));
            }
        }

        /* for (int i = 0; i < RENDERHEIGHT*RENDERWIDTH; i ++) { */
        /*     ZBuff[i] = (Zee){10000,NULL}; */
        /* } */
        memset(display, 0, sizeof(display));
        memset(ZBuff, 0, sizeof(Zee) * 1920 * 1080);
        for (int i = 0; i < RENDERHEIGHT * RENDERWIDTH; i++) {
            ZBuff[i].depth = -10000000;
        }
        /* for (int y = 0; y < RENDERHEIGHT; y++){ */
        /*     for (int x = 0; x<RENDERWIDTH; x++){ */
        /* 	ZBuff[x][y].triangle = &blank; */
        /*     } */
        /* } */

        /* for (int i = 0; i < Tri2Darr.length; i++) { */
        /*     for (int y = 0; y < RENDERHEIGHT; y++) { */
        /*         for (int x = 0; x < RENDERWIDTH; x++) { */
        /*             if (IsInTri(Tri2Darr.arr[i], (Vector2){x, y})) { */
        /*                 ZBuff[IndexOfZBuff(x, y)].triangle = &Tri2Darr.arr[i]; */
        /*             } */
        /*         } */
        /*     } */
        /* } */

        for (int i = 0; i < Tri2Darr.length; i++) {
            DrawTriZuff(ZBuff, &Tri2Darr.arr[i]);
        }

        // FillTopFlatZbuffer(ZBuff, &funners);
        // FillBottomFlatZbuffer(ZBuff,&funners2);

        // FillTopFlatZbuffer(ZBuff, &fullscreentritop);
        // FillBottomFlatZbuffer(ZBuff, &fullscreentribottom);

        // DrawTriZuff(ZBuff, &norm);
        // DrawTriZuff(ZBuff, TriTransformWithCam(&norm, &camera));

        int index = 0;
        for (int y = 0; y < RENDERHEIGHT; y++) {
            for (int x = 0; x < RENDERWIDTH; x++) {
                // int index  = (x+y*RENDERWIDTH);
                /* Color * c = &ZBuff[x][y].triangle->color;
                display[index] = c->r;
                display[index+1] = c->g;
                display[index+2] = c->b;
                display[index+3] = c->a;
                */

                if (ZBuff[IndexOfZBuff(x, y)].triangle != 0) { // memset sets this to 0
                    // DrawPixel(x,y,ZBuff[x][y].triangle->color);

                    display[index] = ZBuff[IndexOfZBuff(x, y)].triangle->color;
                    // Zee test = ZBuff[IndexOfZBuff(x,y)];
                    // display[index] = test.triangle->color;
                }
                index = index + 1;
            }
        }

        BeginTextureMode(uiraylibtexture);
        // gui stuff
        EndTextureMode();

        // Copytexture to main display :0

        BeginDrawing();
        ClearBackground(BLACK);
        UpdateTexture(render3dtexture.texture, display);

        // Copies render3dtexture to screen
        DrawTexturePro(
            render3dtexture.texture,
            (Rectangle){0, 0, render3dtexture.texture.width, render3dtexture.texture.height},
            (Rectangle){0, 0, SCREENWIDTH, SCREENHEIGHT}, (Vector2){0, 0}, 0, WHITE);

        // Copies uiraylibtexture to screen (not this is not the texture used
        // for 3d stuff
        DrawTexturePro(
            uiraylibtexture.texture,
            (Rectangle){0, 0, uiraylibtexture.texture.width, -uiraylibtexture.texture.height},
            (Rectangle){0, 0, SCREENWIDTH, SCREENHEIGHT}, (Vector2){0, 0}, 0, WHITE);

        char fpstext[40];
        sprintf(fpstext, "%d", GetFPS());
        DrawText(fpstext, 0, 0, 20, WHITE);

        EndDrawing();
    }

    CloseWindow();

    return 0;
}