#pragma once

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

using namespace ECS;

class CollisionSystem : public EntitySystem {
  public:
    // 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;
                    v2glm = transform->matrix * v2glm;
                    v3glm = transform->matrix * v3glm;

                    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 configure(World *pWorld) override { myWorld = pWorld; }

    World *myWorld;

    KDTree *kdtree;
};