#pragma once

#include "../../Util/kdtree.h"
#include "../Components/LODObjMesh.h"
#include "../Components/Mesh.h"
#include "../Components/MouseLook.h"
#include "../Components/ObjMesh.h"
#include "../Components/Transform.h"
#include "../ECS.h"

#include <map>

using namespace ECS;

class CollisionSystem : public EntitySystem {
  public:
    CollisionSystem(Entity *collision_entity) : collision_entity(collision_entity) {}

    // Initialize the kdtree
    void build() {
        std::vector<Point *> points;

        // ObjMesh
        myWorld->each<ObjMesh, Transform>(
            [&](Entity *ent, ComponentHandle<ObjMesh> mesh, ComponentHandle<Transform> transform) {
                std::vector<unsigned int> indices = mesh->indices;
                std::vector<float> vertices = mesh->vertices;

                std::map<Vector, Point *> triangle_map;

                // We iterate over the index array and add the resulting triangles to a hash map of
                // vertices. That way, each vertex holds a list of all triangles it is involved in.
                for (int i = 0; i < mesh->vertex_count; i += 3) {
                    // Build vertices from this triangle
                    float v0p0 = vertices[indices[i + 0] * 14 + 0];
                    float v0p1 = vertices[indices[i + 0] * 14 + 1];
                    float v0p2 = vertices[indices[i + 0] * 14 + 2];

                    float v1p0 = vertices[indices[i + 1] * 14 + 0];
                    float v1p1 = vertices[indices[i + 1] * 14 + 1];
                    float v1p2 = vertices[indices[i + 1] * 14 + 2];

                    float v2p0 = vertices[indices[i + 2] * 14 + 0];
                    float v2p1 = vertices[indices[i + 2] * 14 + 1];
                    float v2p2 = vertices[indices[i + 2] * 14 + 2];

                    glm::vec4 v1glm(v0p0, v0p1, v0p2, 1.0);
                    glm::vec4 v2glm(v1p0, v1p1, v1p2, 1.0);
                    glm::vec4 v3glm(v2p0, v2p1, v2p2, 1.0);

                    // Transform to World Position -- these are local coordinates with
                    // individual mesh origins
                    v1glm = transform->matrix * v1glm + glm::vec4(transform->get_origin(), 0.0);
                    v2glm = transform->matrix * v2glm + glm::vec4(transform->get_origin(), 0.0);
                    v3glm = transform->matrix * v3glm + glm::vec4(transform->get_origin(), 0.0);

                    Vector v1(v1glm.x, v1glm.y, v1glm.z);
                    Vector v2(v2glm.x, v2glm.y, v2glm.z);
                    Vector v3(v3glm.x, v3glm.y, v3glm.z);

                    Triangle *triangle = new Triangle(v1, v2, v3);

                    if (triangle_map.count(v1) == 0) {
                        triangle_map[v1] = new Point(v1, std::list<Triangle *>{triangle});
                    } else {
                        triangle_map[v1]->triangles.emplace_back(triangle);
                    }

                    if (triangle_map.count(v2) == 0) {
                        triangle_map[v2] = new Point(v2, std::list<Triangle *>{triangle});
                    } else {
                        triangle_map[v2]->triangles.emplace_back(triangle);
                    }

                    if (triangle_map.count(v3) == 0) {
                        triangle_map[v3] = new Point(v3, std::list<Triangle *>{triangle});
                    } else {
                        triangle_map[v3]->triangles.emplace_back(triangle);
                    }
                }

                // Convert to list
                std::transform(
                    triangle_map.begin(), triangle_map.end(), back_inserter(points),
                    [](const std::map<Vector, Point *>::value_type &val) { return val.second; });
            });

        // LODObjMesh
        myWorld->each<LODObjMesh, Transform>([&](Entity *ent, ComponentHandle<LODObjMesh> lodMesh,
                                                 ComponentHandle<Transform> transform) {
            // TODO
        });

        kdtree = new KDTree(points);
    }

    void tick(World *pWorld, float deltaTime) override {
        pWorld->each<Transform, MouseLook>([&](Entity *ent, ComponentHandle<Transform> transform,
                                               ComponentHandle<MouseLook> mouse_look) {
            glm::vec3 origin_glm = transform->get_origin();
            Vector origin = Vector(origin_glm.x, origin_glm.y, origin_glm.z);

            glm::vec3 direction_glm = mouse_look->get_look_direction();
            Vector direction = Vector(direction_glm.x, direction_glm.y, direction_glm.z);

            Ray ray(origin, direction);

            Vector collision_position(0, 0, 0);
            const Triangle *result = kdtree->intersect_ray(ray, collision_position);

            if (result) {
                // Output to console
                std::cout << "Point: " << collision_position[0] << ", " << collision_position[1]
                          << ", " << collision_position[2] << " || Triangle: " << result->p1[0]
                          << ", " << result->p1[1] << ", " << result->p1[2] << "  |  "
                          << result->p2[0] << ", " << result->p2[1] << ", " << result->p2[2]
                          << "  |  " << result->p3[0] << ", " << result->p3[1] << ", "
                          << result->p3[2] << std::endl;

                // Ouput visually
                glm::vec3 pos =
                    glm::vec3(collision_position[0], collision_position[1], collision_position[2]);
                collision_entity->get<Transform>()->set_origin(pos);
            }
        });
    }

    void configure(World *pWorld) override { myWorld = pWorld; }

    World *myWorld;

    KDTree *kdtree;

    Entity *collision_entity;
};