#include "ingame.h"
#include "scene.h"

#include "twn_game_api.h"
#include "twn_vec.h"

#define STB_PERLIN_IMPLEMENTATION
#include <stb_perlin.h>
#include <SDL2/SDL.h>

#include <stdlib.h>


#define TERRAIN_FREQUENCY 0.15f
#define TERRAIN_RADIUS 128
#define GRASS_RADIUS 16
#define TERRAIN_DISTANCE (TERRAIN_RADIUS * 2)
#define HALF_TERRAIN_DISTANCE ((float)TERRAIN_DISTANCE / 2)
#define PLAYER_HEIGHT 0.6f
#define TREE_DENSITY 0.03f

#define G_CONST 0.1f

/* TODO: pregenerate grid of levels of detail */
static float heightmap[TERRAIN_DISTANCE][TERRAIN_DISTANCE];


/* vehicle sim ! */
/* https://www.youtube.com/watch?v=pwbwFdWBkU0 */
/* representation is a "jelly" box with spring configuration trying to make it coherent */

/* == springs == */
/* damped spring: F = -kx - cv */
/* x = length(p1-p0) - at_rest_length */
/* v = dot(v1 - v0, unit(p1-p0)) */
/* F = (-kx - cv) * unit(p1-p0) */
/* v += (F/m)*t */
/* one points gains positive F, other negative F, to come together */

/* == ground interaction == */
/* if point is under terrain, then apply this: */
/* x = y difference on point and ground */
/* -x(n) = x * normal */
/* -v(n) = dot(v, normal) */
/* -F = (-kx(n)-cv(n)) * normal */

/* == friction == */
/* v(o)/F(o) are perpendicular to slope (x - x(n)) */
/* if v(o) == 0, then */
/*     -- at rest, static friction overcomes */
/*     if F(o) <= f(s)*x(n), F(o) = 0 */
/*     else, F(o) -= f(k) * x(n) */
/* else if length(v(o) + (F(o)/m)*t) <= (f(k)*x(n)*t), v(o) = 0 */
/*     else, F = -unit(v(o)*f(k)*x(n)) */

#define VEHICLE_MASS 200.0f
#define VEHICLE_LENGTH 3.0f
#define VEHICLE_WIDTH 1.7f
#define VEHICLE_HEIGHT 1.3f
/* spring constant */
#define VEHICLE_SPRING_K 400.0f
/* damping constant */
#define VEHICLE_SPRING_C 20.0f
#define VEHICLE_FRICTION_S 1000000.0f
#define VEHICLE_FRICTION_K 1000000.0f

/* initial, ideal corner positions */
static const Vec3 vbpi[8] = {
    [0] = { 0, 0, 0 },
    [1] = { VEHICLE_LENGTH, 0, 0 },
    [2] = { VEHICLE_LENGTH, 0, VEHICLE_WIDTH },
    [3] = { 0, 0, VEHICLE_WIDTH },
    [4] = { 0, VEHICLE_HEIGHT, 0 },
    [5] = { VEHICLE_LENGTH, VEHICLE_HEIGHT, 0 },
    [6] = { VEHICLE_LENGTH, VEHICLE_HEIGHT, VEHICLE_WIDTH },
    [7] = { 0, VEHICLE_HEIGHT, VEHICLE_WIDTH },
};
/* corner positions in simulation */
static Vec3 vbp[8] = { vbpi[0], vbpi[1], vbpi[2], vbpi[3],
                       vbpi[4], vbpi[5], vbpi[6], vbpi[7], };
/* corner velocities */
static Vec3 vbv[8];
/* springs */
static uint8_t vbs[28][2] = {
    {0, 1}, {4, 5}, {0, 4},
    {1, 2}, {5, 6}, {1, 5},
    {2, 3}, {6, 7}, {2, 6},
    {3, 0}, {7, 4}, {3, 7},

    {0, 2}, {0, 5}, {0, 7},
    {1, 3}, {1, 6}, {1, 4},
    {4, 6}, {2, 7}, {2, 5},
    {5, 7}, {3, 4}, {3, 6},

    {0, 6}, {1, 7}, {2, 4}, {3, 5},
};


static float height_at(SceneIngame *scn, Vec2 position);
static Vec3 normal_at(SceneIngame *scn, Vec2 position);


static void draw_vehicle(SceneIngame *scn) {
    for (size_t i = 0; i < 12; ++i)
        draw_line_3d(vbp[vbs[i][0]], vbp[vbs[i][1]], 1, (Color){255, 255, 255, 255});
    for (size_t i = 12; i < 24; ++i)
        draw_line_3d(vbp[vbs[i][0]], vbp[vbs[i][1]], 1, (Color){200, 200, 200, 255});
    for (size_t i = 24; i < 28; ++i)
        draw_line_3d(vbp[vbs[i][0]], vbp[vbs[i][1]], 1, (Color){255, 125, 125, 255});
}


static void process_vehicle(SceneIngame *scn) {
    /* apply gravity */
    Vec3 Facc[8] = {0};

    Vec3 const Fg = { .y = -VEHICLE_MASS * G_CONST };
    for (size_t i = 0; i < 8; ++i)
        Facc[i] = vec3_add(Facc[i], Fg);

    /* apply springs */
    for (size_t i = 0; i < 24; ++i) {
        Vec3 const p0 = vbp[vbs[i][0]];
        Vec3 const p1 = vbp[vbs[i][1]];
        Vec3 const v0 = vbv[vbs[i][0]];
        Vec3 const v1 = vbv[vbs[i][1]];
        Vec3 const pd = vec3_sub(p1, p0);
        Vec3 const pn = vec3_norm(pd);
        /* TODO: length at rest could be precalculated */
        float const lar = vec3_length(vec3_sub(vbpi[vbs[i][1]], vbpi[vbs[i][0]]));
        float const x = vec3_length(pd) - lar;
        float const v = vec3_dot(vec3_sub(v1, v0), pn);
        Vec3 const Fs = vec3_scale(pn, -VEHICLE_SPRING_K * x - VEHICLE_SPRING_C * v);
        Facc[vbs[i][0]] = vec3_sub(Facc[vbs[i][0]], Fs);
        Facc[vbs[i][1]] = vec3_add(Facc[vbs[i][1]], Fs);
    }

    /* spring and friction against the ground */
    for (size_t i = 0; i < 8; ++i) {
        Vec3 const p = vbp[i];
        Vec3 const v = vbv[i];
        float const h = height_at(scn, (Vec2){ p.x, p.z });
        if (h > p.y) {
            /* displacement force */
            Vec3 const n = normal_at(scn, (Vec2){ p.x, p.z });
            float const xn = (h - p.y) * n.y;
            float const vn = vec3_dot(v, n);
            Vec3 const Fd = vec3_scale(n, -VEHICLE_SPRING_K * xn - VEHICLE_SPRING_C * vn);
            Facc[i] = vec3_sub(Facc[i], Fd);

            /* friction force, perpendicular to displacement */
            /* portions aligned to surface normal */
            Vec3 const vov = vec3_sub(v, vec3_scale(n, vn));
            float const vo = vec3_length(vov);
            Vec3 const Fov = vec3_sub(Facc[i], vec3_scale(n, vec3_dot(Facc[i], n)));
            float const Fo = vec3_length(Fov);
            float const fkxn = VEHICLE_FRICTION_K * xn;
            if (fabsf(0.0f - vo) <= 0.0001f) {
                /* cannot overcome static friction, zero force along the surface */
                if (Fo <= VEHICLE_FRICTION_S * xn) {
                    Facc[i] = vec3_sub(Facc[i], Fov);
                }
                /* apply kinematic friction */
                else {
                    Facc[i] = vec3_sub(Facc[i], vec3_scale(vec3_norm(Fov), fkxn));
                }
            /* velocity with gain along the surface will not overcome */
            } else if (vo + (Fo / VEHICLE_MASS) * ctx.frame_duration <= fkxn * ctx.frame_duration) {
                vbv[i] = vec3_sub(vbv[i], vov);
            } else {
                Facc[i] = vec3_sub(Facc[i], vec3_scale(vec3_norm(vov), fkxn));
            }
        }
    }

    /* integrate forces and velocity */
    for (size_t i = 0; i < 8; ++i) {
        Vec3 const vd = vec3_scale(Facc[i], (1.0f / VEHICLE_MASS) * ctx.frame_duration);
        vbv[i] = vec3_add(vbv[i], vd);
        vbp[i] = vec3_add(vbp[i], vbv[i]);
        vbv[i] = vec3_scale(vbv[i], 0.99f);
    }
}


static void process_fly_mode(State *state) {
    SceneIngame *scn = (SceneIngame *)state->scene;

    DrawCameraFromPrincipalAxesResult dir_and_up =
        draw_camera_from_principal_axes(scn->pos, scn->roll, scn->pitch, scn->yaw, (float)M_PI_2 * 0.8f, 1, TERRAIN_RADIUS * sqrtf(3));

    scn->looking_direction = dir_and_up.direction;

    const Vec3 right = m_vec_norm(m_vec_cross(dir_and_up.direction, dir_and_up.up));
    const float speed = 0.1f; /* TODO: put this in a better place */
    if (input_action_pressed("player_left"))
        scn->pos = vec3_sub(scn->pos, m_vec_scale(right, speed));

    if (input_action_pressed("player_right"))
        scn->pos = vec3_add(scn->pos, m_vec_scale(right, speed));

    if (input_action_pressed("player_forward"))
        scn->pos = vec3_add(scn->pos, m_vec_scale(dir_and_up.direction, speed));

    if (input_action_pressed("player_backward"))
        scn->pos = vec3_sub(scn->pos, m_vec_scale(dir_and_up.direction, speed));

    if (input_action_pressed("player_jump"))
        scn->pos.y += speed;

    if (input_action_pressed("player_run"))
        scn->pos.y -= speed;
}

/* TODO: could be baked in map format */
static Vec3 normal_at(SceneIngame *scn, Vec2 position) {
    int const x = (int)(roundf(HALF_TERRAIN_DISTANCE + (position.x - scn->pos.x)));
    int const y = (int)(roundf(HALF_TERRAIN_DISTANCE + (position.y - scn->pos.z)));

    float const height0 = heightmap[x][y];
    float const height1 = heightmap[x + 1][y];
    float const height2 = heightmap[x][y + 1];

    Vec3 const a = { .x = 1, .y = height0 - height1, .z = 0 };
    Vec3 const b = { .x = 0, .y = height0 - height2, .z = -1 };

    return vec3_norm(vec3_cross(a, b));
}

/* TODO: don't operate on triangles, instead interpolate on quads */
static float height_at(SceneIngame *scn, Vec2 position) {
    int const x = (int)(roundf(HALF_TERRAIN_DISTANCE + (position.x - scn->pos.x)));
    int const y = (int)(roundf(HALF_TERRAIN_DISTANCE + (position.y - scn->pos.z)));

    float const height0 = heightmap[x][y];
    float const height1 = heightmap[x + 1][y];
    float const height2 = heightmap[x][y + 1];
    float const height3 = heightmap[x + 1][y + 1];

    Vec2 incell = { position.x - floorf(position.x), position.y - floorf(position.y) };

    float const weight0 = (1 - incell.x) * (1 - incell.y);
    float const weight1 = ( incell.x) * (1 - incell.y);
    float const weight2 = (1 - incell.x) * ( incell.y);
    float const weight3 = ( incell.x) * ( incell.y);

    return (height0 * weight0 + height1 * weight1 + height2 * weight2 + height3 * weight3) / (weight0 + weight1 + weight2 + weight3);
}


static void process_ground_mode(State *state) {
    SceneIngame *scn = (SceneIngame *)state->scene;

    DrawCameraFromPrincipalAxesResult dir_and_up =
        draw_camera_from_principal_axes(scn->pos, scn->roll, scn->pitch, scn->yaw, (float)M_PI_2 * 0.8f, 1, TERRAIN_RADIUS * sqrtf(3));

    scn->looking_direction = dir_and_up.direction;

    dir_and_up.direction.y = 0;
    dir_and_up.direction = vec3_norm(dir_and_up.direction);

    const Vec3 right = m_vec_norm(m_vec_cross(dir_and_up.direction, dir_and_up.up));
    const float speed = 0.20f; /* TODO: put this in a better place */

    Vec3 target = scn->pos;

    /* gravity */
    {
        float const height = height_at(scn, (Vec2){scn->pos.x, scn->pos.z});

        if (target.y > height + PLAYER_HEIGHT)
            target.y = target.y - 0.6f;

        if (target.y < height + PLAYER_HEIGHT)
            target.y = height + PLAYER_HEIGHT;
    }

    /* movement */
    {
        Vec3 direction = {0, 0, 0};

        if (input_action_pressed("player_left"))
           direction = m_vec_sub(direction, m_vec_scale(right, speed));

        if (input_action_pressed("player_right"))
           direction = m_vec_add(direction, m_vec_scale(right, speed));

        if (input_action_pressed("player_forward"))
            direction = m_vec_add(direction, m_vec_scale(dir_and_up.direction, speed));

        if (input_action_pressed("player_backward"))
            direction = m_vec_sub(direction, m_vec_scale(dir_and_up.direction, speed));

        target = m_vec_add(target, direction);
    }

    /* interpolate */
    scn->pos.x = (target.x - scn->pos.x) * (0.13f / 0.9f) + scn->pos.x;
    scn->pos.y = (target.y - scn->pos.y) * (0.13f / 0.9f) + scn->pos.y;
    scn->pos.z = (target.z - scn->pos.z) * (0.13f / 0.9f) + scn->pos.z;
}


static void generate_terrain(SceneIngame *scn) {
    for (int ly = 0; ly < TERRAIN_DISTANCE; ly++) {
        for (int lx = 0; lx < TERRAIN_DISTANCE; lx++) {
            float x = floorf(scn->pos.x - HALF_TERRAIN_DISTANCE + (float)lx);
            float y = floorf(scn->pos.z - HALF_TERRAIN_DISTANCE + (float)ly);

            float height = stb_perlin_noise3((float)x * TERRAIN_FREQUENCY, (float)y * TERRAIN_FREQUENCY, 0, 0, 0, 0) * 3 - 1;
            height += stb_perlin_noise3((float)x * TERRAIN_FREQUENCY / 10, (float)y * TERRAIN_FREQUENCY / 10, 0, 0, 0, 0) * 20 - 1;

            heightmap[lx][ly] = height;
        }
    }
}


static int32_t ceil_sqrt(int32_t const n) {
    int32_t res = 1;
    while(res * res < n)
        res++;
    return res;
}


static uint32_t adler32(const void *buf, size_t buflength) {
     const uint8_t *buffer = (const uint8_t*)buf;

     uint32_t s1 = 1;
     uint32_t s2 = 0;

     for (size_t n = 0; n < buflength; n++) {
        s1 = (s1 + buffer[n]) % 65521;
        s2 = (s2 + s1) % 65521;
     }
     return (s2 << 16) | s1;
}


static void draw_terrain(SceneIngame *scn) {
    /* used to cull invisible tiles over field of view (to horizon) */
    Vec2 const d = vec2_norm((Vec2){ .x = scn->looking_direction.x, .y = scn->looking_direction.z });
    float const c = cosf((float)M_PI_2 * 0.8f * 0.8f);

    /* draw terrain in circle */
    int32_t const rsi = (int32_t)TERRAIN_RADIUS * (int32_t)TERRAIN_RADIUS;
    for (int32_t iy = -(int32_t)TERRAIN_RADIUS; iy <= (int32_t)TERRAIN_RADIUS - 1; ++iy) {
        int32_t const dx = ceil_sqrt(rsi - (iy + (iy <= 0)) * (iy + (iy <= 0)));
        for (int32_t ix = -dx; ix < dx - 1; ++ix) {
            int32_t lx = ix + TERRAIN_RADIUS;
            int32_t ly = iy + TERRAIN_RADIUS;

            float x = (float)(floorf(scn->pos.x - HALF_TERRAIN_DISTANCE + (float)lx));
            float y = (float)(floorf(scn->pos.z - HALF_TERRAIN_DISTANCE + (float)ly));

            /* cull tiles outside of vision */
            if (vec2_dot(vec2_norm((Vec2){x - scn->pos.x + d.x * 2, y - scn->pos.z + d.y * 2}), d) < c)
                continue;

            float d0 = heightmap[lx][ly];
            float d1 = heightmap[lx + 1][ly];
            float d2 = heightmap[lx + 1][ly - 1];
            float d3 = heightmap[lx][ly - 1];

            draw_quad("/assets/grass2.png",
                            (Vec3){ (float)x,     d0, (float)y },
                            (Vec3){ (float)x + 1, d1, (float)y },
                            (Vec3){ (float)x + 1, d2, (float)y - 1 },
                            (Vec3){ (float)x,     d3, (float)y - 1 },
                            (Rect){ .w = 128, .h = 128 },
                            (Color){255, 255, 255, 255});

            if (((float)(adler32(&((Vec2){x, y}), sizeof (Vec2)) % 100) / 100) <= TREE_DENSITY)
                draw_billboard("/assets/trreez.png",
                              (Vec3){ (float)x, d0 + 1.95f, (float)y },
                              (Vec2){2.f, 2.f},
                              (Rect){0},
                              (Color){255, 255, 255, 255}, true);
        }
    }

    int32_t const rsi_g = (int32_t)GRASS_RADIUS * (int32_t)GRASS_RADIUS;
    for (int32_t iy = -(int32_t)GRASS_RADIUS; iy <= (int32_t)GRASS_RADIUS - 1; ++iy) {
        int32_t const dx = ceil_sqrt(rsi_g - (iy + (iy <= 0)) * (iy + (iy <= 0)));
        for (int32_t ix = -dx; ix < dx; ++ix) {
            int32_t lx = ix + TERRAIN_RADIUS;
            int32_t ly = iy + TERRAIN_RADIUS;

            float x = (float)(floorf(scn->pos.x - HALF_TERRAIN_DISTANCE + (float)lx));
            float y = (float)(floorf(scn->pos.z - HALF_TERRAIN_DISTANCE + (float)ly));

            float d = heightmap[lx][ly];

            draw_billboard("/assets/grasses/25.png",
                          (Vec3){
                              (float)x + (float)((adler32(&((Vec2){x, y}), sizeof (Vec2))) % 32) / 64.0f,
                              d + 0.2f,
                              (float)y + (float)((adler32(&((Vec2){y, x}), sizeof (Vec2))) % 32) / 64.0f
                          },
                          (Vec2){0.4f, 0.4f},
                          (Rect){0},
                          (Color){255, 255, 255, 255}, true);
        }
    }
}


static void ingame_tick(State *state) {
    SceneIngame *scn = (SceneIngame *)state->scene;

    input_action("player_left", "A");
    input_action("player_right", "D");
    input_action("player_forward", "W");
    input_action("player_backward", "S");
    input_action("player_jump", "SPACE");
    input_action("player_run", "LSHIFT");
    input_action("mouse_capture_toggle", "ESCAPE");
    input_action("toggle_camera_mode", "C");

    // draw_model("models/test.obj", (Vec3){0}, (Vec3){0,0,1}, (Vec3){1.f / 64,1.f / 64,1.f / 64});
    // draw_model("models/test2.obj", (Vec3){0}, (Vec3){0,0,1}, (Vec3){1.f / 64,1.f / 64,1.f / 64});
    // draw_model("models/bunny.obj", (Vec3){0}, (Vec3){0,0,1}, (Vec3){4.,4.,4.});

    if (scn->mouse_captured) {
        const float sensitivity = 0.4f * (float)DEG2RAD; /* TODO: put this in a better place */
        scn->yaw += (float)ctx.mouse_movement.x * sensitivity;
        scn->pitch -= (float)ctx.mouse_movement.y * sensitivity;
        scn->pitch = clampf(scn->pitch, (float)-M_PI * 0.49f, (float)M_PI * 0.49f);
    }

    if (input_action_just_pressed("toggle_camera_mode"))
        scn->flying_camera = !scn->flying_camera;

    if (scn->flying_camera) {
        process_fly_mode(state);
    } else {
        process_ground_mode(state);
    }

    /* toggle mouse capture with end key */
    if (input_action_just_pressed("mouse_capture_toggle"))
        scn->mouse_captured = !scn->mouse_captured;

    process_vehicle(scn);

    ctx.mouse_capture = scn->mouse_captured;

    generate_terrain(scn);
    draw_terrain(scn);
    draw_vehicle(scn);

    draw_skybox("/assets/miramar/miramar_*.tga");

    ctx.fog_color = (Color){ 140, 147, 160, 255 };
    ctx.fog_density = 0.015f;
}


static void ingame_end(State *state) {
    free(state->scene);
}


Scene *ingame_scene(State *state) {
    (void)state;

    SceneIngame *new_scene = calloc(1, sizeof *new_scene);
    new_scene->base.tick = ingame_tick;
    new_scene->base.end = ingame_end;

    new_scene->mouse_captured = true;

    m_audio(m_set(path, "music/woah.ogg"),
            m_opt(channel, "soundtrack"),
            m_opt(repeat, true));

    new_scene->pos = (Vec3){ 0.1f, 0.0, 0.1f };

    return (Scene *)new_scene;
}