#include "CollisionSystem.h"

using namespace Seele;
using namespace Seele::Component;

CollisionSystem::CollisionSystem(entt::registry& registry)
    : registry(registry)
{
    registry.on_construct<Component::Collider>().connect<&BVH::colliderCallback>(bvh);
}

CollisionSystem::~CollisionSystem()
{
    
}

void CollisionSystem::detectCollisions(Array<Collision>& collisions)
{
    collisions.clear();
    auto view = registry.view<Collider, Transform>();
    for(auto && [entity, collider, transform] : view.each())
    {
        if(collider.type == ColliderType::DYNAMIC)
        {
            bvh.updateDynamicCollider(entity, collider.boundingbox.getTransformedBox(transform.toMatrix()));
        }
        collider.physicsMesh.transform(transform).visualize();
    }
    bvh.visualize();
    Array<Pair<entt::entity, entt::entity>> overlaps;
    bvh.findOverlaps(overlaps);
    for(auto pair : overlaps)
    {
        if(checkCollision(pair))
        {
            collisions.add(Collision {
                .a = pair.key,
                .b = pair.value,
            });
        }
    }
}

bool CollisionSystem::checkCollision(Pair<entt::entity, entt::entity> pair)
{
    const auto&[collider1, transform1] = registry.get<Collider, Transform>(pair.key);
    const auto&[collider2, transform2] = registry.get<Collider, Transform>(pair.value);
    ShapeBase shape1 = collider1.physicsMesh.transform(transform1);
    ShapeBase shape2 = collider2.physicsMesh.transform(transform2);
    Witness witness;
    if(cachedWitness.exists(pair))
    {
        witness = cachedWitness[pair];
    }
    if(witnessValid(witness, shape1, shape2))
    {
        return false;
    }
    return createWitness(witness, shape1, shape2);
}

void CollisionSystem::updateWitness(Witness& result, const glm::vec3& point, const glm::vec3& v1, const glm::vec3& v2)
{
    const glm::vec3 faceNormal = glm::normalize(glm::cross(v1, v2));

    result.n = faceNormal;
    result.p = point;
}

bool CollisionSystem::createWitness(Witness& result, const ShapeBase& source, const ShapeBase& other)
{
    for (size_t i = 0; i < source.indices.size(); i += 3)
    {
        const glm::vec3 point1 = source.vertices[source.indices[i + 0]];
        const glm::vec3 point2 = source.vertices[source.indices[i + 1]];
        const glm::vec3 point3 = source.vertices[source.indices[i + 2]];
        const glm::vec3 v1 = point2 - point1;
        const glm::vec3 v2 = point3 - point1;
        
        updateWitness(result, point1, v1, v2);
        result.point1Index = source.indices[i + 0];
        result.point2Index = source.indices[i + 1];
        result.point3Index = source.indices[i + 2];
        result.point4Index = UINT32_MAX;
        bool valid = witnessValid(result, source, other);
        // if not, it is a valid separating plane, so no collision
        if (valid)
        {
            return false;
        }
    }
    for (size_t i = 0; i < source.indices.size(); i += 3)
    {
        auto findEdgePlane = [this, &source, &other, &result](uint32_t point1Index, uint32_t point2Index)
        {
            const glm::vec3 point1 = source.vertices[point1Index];
            const glm::vec3 point2 = source.vertices[point2Index];
            result.point1Index = point1Index;
            result.point2Index = point2Index;
            const glm::vec3 d1 = point2 - point1;
            for (size_t j = 0; j < other.indices.size(); j += 3)
            {
                for (const auto& [point3Index, point4Index] : { std::pair(j+1, j), std::pair(j+2, j+1), std::pair(j, j+2) })
                {
                    result.point3Index = other.indices[point3Index];
                    result.point4Index = other.indices[point4Index];
                    const glm::vec3 point3 = other.vertices[result.point3Index];
                    const glm::vec3 point4 = other.vertices[result.point4Index];
                    const glm::vec3 d2 = point3 - point4;
                    updateWitness(result, point1, d2, d1);
                    if (witnessValid(result, source, other))
                    {
                        return true;
                    }
                }
            }
            return false;
        };
        if(findEdgePlane(source.indices[i], source.indices[i+1]))
        {
            return false;
        }
        if(findEdgePlane(source.indices[i+1], source.indices[i+2]))
        {
            return false;
        }
        if(findEdgePlane(source.indices[i+2], source.indices[i]))
        {
            return false;
        }
    }
    // no separating plane was found, collision
    return true;
}

bool CollisionSystem::witnessValid(const Witness& witness, const Component::ShapeBase& shape1, const Component::ShapeBase& shape2)
{
    const float e = 0.0001f;

    for (size_t i = 0; i < shape1.vertices.size(); i++)
    {
        if(glm::dot(witness.n, shape1.vertices[i] - witness.p) > e)
        {
            // something intersecting the separating plane
            // it is not valid anymore
            return false;
        }
    }
    for (size_t i = 0; i < shape2.vertices.size(); i++)
    {
        if(glm::dot(witness.n, shape2.vertices[i] - witness.p) < -e)
        {
            // something intersecting the separating plane
            // it is not valid anymore
            return false;
        }
    }
    return true;
}