#include "precompile.h"

#include <math.h>
#include <memory.h>

#include <glm/gtx/intersect.hpp>

#include "DetourCommon.h"

#include "mapservice.h"
#include "entity.h"
#include "roomobstacle.h"
#include "room.h"
#include "mapcollider.h"

MapService::MapService()
{

}

MapService::~MapService()
{
    if (map_cells_) {
        free(map_cells_);
        map_cells_ = nullptr;
    }
}

void MapService::Init(int width, int height, int cell_width)
{
    if (width < 1 || height < 1 || cell_width < 1) {
        A8_ABORT();
    }
    map_width_ = width;
    map_height_ = height;
    cell_width_ = cell_width;
    max_grid_id_ = map_width_ * map_height_;

    grid_offset_arr_[0] = - (map_width_ + 1);
    grid_offset_arr_[1] = - (map_width_ + 0);
    grid_offset_arr_[2] = - (map_width_ + -1);

    grid_offset_arr_[3] = - 1;
    grid_offset_arr_[4] = 0;
    grid_offset_arr_[5] = 1;

    grid_offset_arr_[6] = map_width_ - 1;
    grid_offset_arr_[7] = map_width_;
    grid_offset_arr_[8] = map_width_ + 1;

    map_cells_ = (list_head*)malloc(sizeof(list_head) * width * height);
    memset(map_cells_, 0, sizeof(list_head) * width * height);
    for (int i = 0; i < max_grid_id_; ++i) {
        INIT_LIST_HEAD(&map_cells_[i]);
    }
}

void MapService::UnInit()
{

}

bool MapService::CanAdd(const glm::vec3& pos, int rad)
{
    //上
    if (pos.z + rad + 10 > map_height_ * cell_width_) {
        return false;
    }
    //下
    if (rad + 10 > pos.z) {
        return false;
    }
    //左
    if (rad + 10 > pos.x) {
        return false;
    }
    //右
    if (pos.x + rad + 10 > map_width_ * cell_width_) {
        return false;
    }
    return true;
}

void MapService::AddTriangle(mc::Triangle* tri)
{
    glm::vec3* verts[3] = {&tri->vert0, &tri->vert1, &tri->vert2};
    float min_x = verts[0]->x;
    float max_x = verts[0]->x;
    float min_y = verts[0]->z;
    float max_y = verts[0]->z;
    {
        for (int i = 1; i < 3; ++i) {
            if (verts[i]->x < min_x) {
                min_x = verts[i]->x;
            }
            if (verts[i]->x > max_x) {
                max_x = verts[i]->x;
            }
            if (verts[i]->z < min_y) {
                min_y = verts[i]->z;
            }
            if (verts[i]->z > max_y) {
                max_y = verts[i]->z;
            }
        }
        min_x -= 1.0f;
        max_x += 1.0f;
        min_y -= 1.0f;
        max_y += 1.0f;
    }

    {
        int min_grid_x = floor(min_x / cell_width_);
        int min_grid_y = floor(min_y / cell_width_);
        int max_grid_x = ceil(max_x / cell_width_);
        int max_grid_y = ceil(max_y / cell_width_);
        if (min_grid_x < 0) {
            min_grid_x = 0;
        }
        if (min_grid_x < 0) {
            A8_ABORT();
        }
        if (max_grid_x >= map_width_) {
            max_grid_x = map_width_ - 1;
        }
        if (min_grid_y < 0) {
            min_grid_y = 0;
        }
        if (min_grid_y < 0) {
            A8_ABORT();
        }
        if (max_grid_y >= map_height_) {
            max_grid_y = map_height_ - 1;
        }
        float tri_verts[3 * 3];
        float aabb_verts[3 * 4];
        {
            GlmHelper::FillVert(tri->vert0, tri_verts + 3 * 0);
            GlmHelper::FillVert(tri->vert1, tri_verts + 3 * 1);
            GlmHelper::FillVert(tri->vert2, tri_verts + 3 * 2);
        }
        bool found = false;
        for (int x = min_grid_x; x <= max_grid_x; ++x) {
            for (int y = min_grid_y; y <= max_grid_y; ++y) {
                float world_x = x * cell_width_;
                float world_y = y * cell_width_;
                {
                    aabb_verts[0] = world_x;
                    aabb_verts[1] = 0;
                    aabb_verts[2] = world_y;

                    aabb_verts[3] = world_x + cell_width_;
                    aabb_verts[4] = 0;
                    aabb_verts[5] = world_y;

                    aabb_verts[6] = world_x + cell_width_;
                    aabb_verts[7] = 0;
                    aabb_verts[8] = world_y + cell_width_;

                    aabb_verts[9] = world_x;
                    aabb_verts[10] = 0;
                    aabb_verts[11] = world_y + cell_width_;
                }
                {
                    glm::vec3 t1[3] = {tri->vert0, tri->vert1, tri->vert2};

                    glm::vec3 tmp_verts[4];
                    for (int i = 0; i < 4; ++i) {
                        tmp_verts[i] = glm::vec3(aabb_verts[i * 3 + 0],
                                                 aabb_verts[i * 3 + 1],
                                                 aabb_verts[i * 3 + 2]);
                    }

                    glm::vec3 t2[3] = {
                        tmp_verts[0],
                            tmp_verts[1],
                            tmp_verts[2]};

                    glm::vec3 t3[3] = {
                        tmp_verts[0],
                            tmp_verts[2],
                            tmp_verts[3]};

                    if (TriIntersect(t1, t2) || TriIntersect(t1, t3)) {
                        int grid_id = x  + y * map_width_;
                        list_head* head = &map_cells_[grid_id];
                        CellNode* node = new CellNode();
                        node->tri = tri;
                        list_add_tail(&node->entry, head);
                        found = true;
                    }
                }
#if 0
                if (dtOverlapPolyPoly2D(tri_verts, 3, aabb_verts, 4)) {
                    int grid_id = x  + y * map_width_;
                    list_head* head = &map_cells_[grid_id];
                    CellNode* node = new CellNode();
                    node->tri = tri;
                    list_add_tail(&node->entry, head);
                    found = true;
                }
#endif
            }
        }
        #if 0
        if (!found) {
            abort();
        }
        #endif
    }
}

int MapService::GetGridId(float world_x, float world_y)
{
    int grid_id = (int)(world_x/cell_width_) + (int)(world_y/cell_width_) * map_width_;
    return grid_id;
}

void MapService::GetGridList(int grid_id, list_head* grid_list[9], int& grid_count)
{
    grid_count = 0;
    if (grid_id <= 0 || grid_id >= max_grid_id_) {
        return;
    }
    for (int i = 0; i < 9; ++i) {
        int idx = grid_id + grid_offset_arr_[i];
        if (idx >= 0 && idx < max_grid_id_) {
            grid_list[grid_count++] = &map_cells_[idx];
        }
    }
}

bool MapService::TriIntersect(glm::vec3 t1[3], glm::vec3 t2[3])
{
    return true;
    for (int i = 0; i < 3; ++i) {
        t1[i].y = 0.0f;
        t2[i].y = 0.0f;
    }
    glm::vec2 bary_position;
    float distance;
    for (int v = 0; v < 3; ++v) {
        glm::vec3 v1 = t1[v];
        glm::vec3 v2 = t1[(v + 1) % 3];
        glm::vec3 orig = v1;
        glm::vec3 dir = v2 - v1;
        GlmHelper::Normalize(dir);
        float max_distance = GlmHelper::Norm(v2 - v1);
        for (int i = 0; i < 2; ++i){
            bool hit = glm::intersectRayTriangle
                (orig,
                 dir,
                 t2[0],
                 t2[1],
                 t2[2],
                 bary_position,
                 distance
                 );
            if (hit && distance > 0.00001f) {
                if (distance < max_distance + 1) {
                    return true;
                }
            }
        }
    }
    return false;
}