#include "terrain.h"
#include "math/math_util.h"
#include "math/vector3f.h"
#include "util/util.h"

#include <SDL2/SDL.h>
#include <SDL2/SDL_image.h>
#include <math.h>

#define PLANE_SIZE 128
#define PLANE_MAX_HEIGHT 10

#define NUM_ARRAY_ELEMENTS(a) sizeof(a) / sizeof(*a)

static float GetHeight(int x, int y, SDL_Surface* surface)
{
    if(x < 0 || x >= surface->w || y < 0 || y >= surface->h)
        return 0.0f;

    Uint32 pixel = ( (Uint32*)surface->pixels )[y * surface->w + x];
    Uint8 r, g, b;
    SDL_GetRGB(pixel, surface->format, &r, &g, &b);

    float height = (float)r / 255.0f;

    return height * 40.0f;
}

static vec3_t GenerateNomal(int x, int y, SDL_Surface* surface)
{
    float hLeft  = GetHeight(x-1, y, surface);
    float hRight = GetHeight(x+1, y, surface);
    float hUp    = GetHeight(x, y+1, surface);
    float hDown  = GetHeight(x, y-1, surface);

    vec3_t normal = { hLeft - hRight, 2.0f, hDown - hUp};
    return vec3_normalize(&normal);
}

GLfloat Terrain_GetHeightOfTerrain(terrain_t* terrain, GLfloat x, GLfloat z)
{
    GLfloat terrainX = x - terrain->position.x;
    GLfloat terrainZ = z - terrain->position.z;

    GLfloat gridSquareSize = (float)terrain->l / ( (float)PLANE_SIZE - 1 );

    GLint gridX = (GLint) floor(terrainX / gridSquareSize);
    GLint gridZ = (GLint) floor(terrainZ / gridSquareSize);

    if(gridX >= PLANE_SIZE - 1 || gridX < 0 || gridZ >= PLANE_SIZE - 1 || gridZ < 0)
    {
        printf("called\n");
        return 0;
    }

    GLfloat xCoord = fmod(terrainX, gridSquareSize) / gridSquareSize;
    GLfloat zCoord = fmod(terrainZ, gridSquareSize) / gridSquareSize;
    GLfloat answer;

    /* Determine in which triangle of the square are we. "Bary Centric Interpolation"*/
    if(xCoord <= (1 - zCoord)){
        /* 0, heights[gridX][gridZ], 0) */
        vec3_t p1 = { 0, terrain->height[ gridX * PLANE_SIZE + gridZ ], 0 };
        /* 1, heights[gridX + 1][gridZ], 0) */
        vec3_t p2 = { 1, terrain->height[ (gridX + 1) * PLANE_SIZE + gridZ ], 0};
        /* 0, heights[gridX][gridZ + 1], 1) */
        vec3_t p3 = { 0, terrain->height[ gridX * PLANE_SIZE + (gridZ + 1) ], 1};

        vec2_t pos = {xCoord, zCoord};

        answer = baryCentric(&p1, &p2, &p3, &pos);
    } else {
        /* (1, heights[gridX + 1][gridZ], 0) */
        vec3_t p1 = { 1, terrain->height[ (gridX + 1) * PLANE_SIZE + gridZ ], 0 };
        /* (1, heights[gridX + 1][gridZ + 1], 1) */
        vec3_t p2 = { 1, terrain->height[ (gridX + 1) * PLANE_SIZE + (gridZ + 1) ], 1};
        /* (0, heights[gridX][gridZ + 1], 1) */
        vec3_t p3 = { 0, terrain->height[ gridX * PLANE_SIZE + (gridZ + 1) ], 1};
        vec2_t pos = {xCoord, zCoord};

        answer = baryCentric(&p1, &p2, &p3, &pos);
    }

    return answer;
}

terrain_t *Terrain_Create( int w, int l, const char* heightmap_path, texture_t *blendmap, TerrainTexturePack *textures )
{
    terrain_t *terrain = (terrain_t*) malloc( sizeof(terrain_t) );
    terrain->height = (GLfloat*) malloc( sizeof(GLfloat) * PLANE_SIZE * PLANE_SIZE);

    terrain->blendmap = blendmap;
    terrain->w = w; terrain->l = l;
    terrain->textures = *textures;

    SDL_Surface* surface = IMG_Load(heightmap_path);
    if(surface == NULL)
        Util_FatalError("Heightmap file could not be loaded\n");

    vertex_t data[PLANE_SIZE * PLANE_SIZE];
    int x, y;
    for(x = 0; x < PLANE_SIZE; x++)
    {
        for(y = 0; y < PLANE_SIZE; y++)
        {
            vertex_t* v = &data[y + x * PLANE_SIZE];
            v->position = vec3_create( (float)x / (float)PLANE_SIZE, 0.0f, (float)y / (float)PLANE_SIZE);
            /* Heightmap cordinates */
            int image_x = v->position.x * surface->w, image_y = v->position.z * surface->h;

            v->texCoord = vec2_create( v->position.x, v->position.z);
            GLfloat height = GetHeight(image_x, image_y, surface);
            terrain->height[y + x * PLANE_SIZE] = height;
            v->position.y = height;
            v->position.x *= w;
            v->position.z *= l;

            v->normal = GenerateNomal(image_x, image_y, surface);
            color_t temp = {+1.0f, +0.0f, +0.0f, +1.0f};
            v->color = temp;
        }
    }

    int runner = 0;
    GLushort indices[ (PLANE_SIZE-1) * (PLANE_SIZE-1) * 6 ];
    for(x = 0; x < PLANE_SIZE-1; x++)
    {
        for(y = 0; y < PLANE_SIZE-1; y++)
        {
            indices[runner++] = PLANE_SIZE * x + y;
            indices[runner++] = PLANE_SIZE * x + y + 1;
            indices[runner++] = PLANE_SIZE * x + y + PLANE_SIZE;

            indices[runner++] = PLANE_SIZE * x + y + 1;
            indices[runner++] = PLANE_SIZE * x + y + PLANE_SIZE + 1;
            indices[runner++] = PLANE_SIZE * x + y + PLANE_SIZE;
        }
    }

    GLsizeiptr vertexBufferSize = NUM_ARRAY_ELEMENTS(data) * sizeof(vertex_t);
    GLsizeiptr indexBufferSize = NUM_ARRAY_ELEMENTS(indices) * sizeof(GLushort);

    SDL_FreeSurface(surface);

    terrain->shape = Shape_CreateFromRawData(data, vertexBufferSize, indices, indexBufferSize);
    return terrain;
}

void Terrain_Destroy( terrain_t* terrain )
{
    if(terrain->height)
        free(terrain->height);

    Shape_Free(terrain->shape);

    int i;
    for(i = 0; i < 4; i++)
        Texture_Destroy(terrain->textures.texture[i]);

    Texture_Destroy(terrain->blendmap);
    free(terrain);
}