#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"

using namespace ECS;

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

    // Initialize the kdtree
    void build() {
        std::vector<Triangle *> triangles;
        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;

                for (int i = 0; i < mesh->vertex_count; i += 3) {
                    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);
                    triangles.emplace_back(triangle);

                    points.emplace_back(new Point(v1, triangle));
                    points.emplace_back(new Point(v2, triangle));
                    points.emplace_back(new Point(v3, triangle));
                }
            });

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

        std::cout << "Start building kdtree with " << points.size() << " points" << std::endl;
        kdtree = new KDTree(points);
        std::cout << "Done" << std::endl;

        std::cout << kdtree->to_string() << std::endl;
    }

    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 * 5.0);

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

            if (result) {
                // Output to console
                std::cout << "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;
};