#include "vulkan_engine/vulkan/physics.h"

#include <iostream>
#include <limits>

namespace veng {
void Physics::invokeOnColisionEvent(
    gsl::not_null<utils::ThreadPool*> thread_pool,
    std::unordered_map<std::string, std::shared_ptr<Model>>& models) {
  constexpr std::float_t EPSILON = std::numeric_limits<std::float_t>::epsilon();

  for (std::unordered_map<std::string, std::shared_ptr<Model>>::iterator
           iter_A = models.begin();
       iter_A != models.end(); iter_A++) {
    for (auto iter_B = std::next(iter_A); iter_B != models.end(); iter_B++) {
      auto model_A = iter_A->second;
      std::lock_guard<std::mutex> Alock(model_A->modding);
      auto model_B = iter_B->second;
      std::lock_guard<std::mutex> Block(model_B->modding);
      if (!model_A->colision || !model_B->colision) continue;
      std::float_t distance =
          glm::distance(model_A->position, model_B->position);
      std::float_t modelA_radius = model_A->radius * model_A->scale.x;
      std::float_t modelB_radius = model_B->radius * model_B->scale.x;
      if (distance <= modelA_radius + modelB_radius) {
        model_A = iter_A->second;
        model_B = iter_B->second;
        if (model_A->OnColision)
          thread_pool->enqueueJob(model_A->OnColision, model_A, model_B);
        model_A = iter_A->second;
        model_B = iter_B->second;
        if (model_B->OnColision)
          thread_pool->enqueueJob(model_B->OnColision, model_A, model_B);
      }
    }
  }
}

void Physics::invokeOnColisionEvent(
    gsl::not_null<utils::ThreadPool*> thread_pool,
    std::unordered_map<utils::Snowflake, std::shared_ptr<Model>>& models) {
  constexpr std::float_t EPSILON = std::numeric_limits<std::float_t>::epsilon();

  for (std::unordered_map<utils::Snowflake, std::shared_ptr<Model>>::iterator
           iter_A = models.begin();
       iter_A != models.end(); iter_A++) {
    for (auto iter_B = std::next(iter_A); iter_B != models.end(); iter_B++) {
      auto model_A = iter_A->second;
      std::lock_guard<std::mutex> Alock(model_A->modding);
      auto model_B = iter_B->second;
      std::lock_guard<std::mutex> Block(model_B->modding);
      if (!model_A->colision || !model_B->colision) continue;
      std::float_t distance =
          glm::distance(model_A->position, model_B->position);
      std::float_t modelA_radius = model_A->radius * model_A->scale.x;
      std::float_t modelB_radius = model_B->radius * model_B->scale.x;
      if (distance <= modelA_radius + modelB_radius) {
        model_A = iter_A->second;
        model_B = iter_B->second;
        if (model_A->OnColision)
          thread_pool->enqueueJob(model_A->OnColision, model_A, model_B);
        model_A = iter_A->second;
        model_B = iter_B->second;
        if (model_B->OnColision)
          thread_pool->enqueueJob(model_B->OnColision, model_A, model_B);
      }
    }
  }
}

bool Physics::RayTrace(const glm::vec3& rayOrigin, const glm::vec3& rayDir,
                       const glm::vec3& v0, const glm::vec3& v1,
                       const glm::vec3& v2, std::float_t& outDistance) {
  constexpr std::float_t EPSILON = std::numeric_limits<std::float_t>::epsilon();

  // 삼각형 엣지와 노멀 계산
  glm::vec3 edge1 = v1 - v0;
  glm::vec3 edge2 = v2 - v0;
  glm::vec3 normal = glm::cross(edge1, edge2);

  // 평행 여부 판단
  glm::vec3 h = glm::cross(rayDir, edge2);
  std::float_t a = glm::dot(edge1, h);

  if (fabs(a) < EPSILON) {
    // 광선 방향과 삼각형 평면이 거의 평행 → coplanar 검사
    // 시작점이 평면 위에 있는지
    std::float_t distToPlane = glm::dot(normal, rayOrigin - v0);
    if (fabs(distToPlane) < EPSILON) {
      // 평면 위에 있다면, 점이 삼각형 내부에 있는지 검사
      auto pointInTri = [&](const glm::vec3& P) {
        // 엣지마다 P가 같은 반대 방향 노멀 쪽에 있는지
        glm::vec3 c0 = glm::cross(v1 - v0, P - v0);
        glm::vec3 c1 = glm::cross(v2 - v1, P - v1);
        glm::vec3 c2 = glm::cross(v0 - v2, P - v2);

        std::float_t f0 = glm::dot(normal, c0);
        std::float_t f1 = glm::dot(normal, c1);
        std::float_t f2 = glm::dot(normal, c2);

        return (f0 >= EPSILON && f1 >= EPSILON && f2 >= EPSILON);
      };
      if (pointInTri(rayOrigin)) {
        outDistance = 0.0f;
        return true;
      }
    }
    return false;
  }

  // 기존 Möller–Trumbore 알고리즘
  std::float_t f = 1.0f / a;
  glm::vec3 s = rayOrigin - v0;
  std::float_t u = f * glm::dot(s, h);
  if (u < 0.0f || u > 1.0f) return false;

  glm::vec3 q = glm::cross(s, edge1);
  std::float_t v = f * glm::dot(rayDir, q);
  if (v < 0.0f || u + v > 1.0f) return false;

  std::float_t t = f * glm::dot(edge2, q);
  if (t > EPSILON) {
    outDistance = t;
    return true;
  }

  return false;
}

bool Physics::IsPointInsideMesh_(const glm::vec3& point,
                                 const std::vector<veng::Vertex>& vertices,
                                 const std::vector<std::uint32_t>& indices) {
  glm::vec3 rayDir = glm::vec3(1.0f, 0.0f, 0.0f);  // X+ 방향 광선
  int intersectionCount = 0;

  for (size_t i = 0; i < indices.size(); i += 3) {
    const glm::vec3& v0 = vertices[indices[i + 0]].position;
    const glm::vec3& v1 = vertices[indices[i + 1]].position;
    const glm::vec3& v2 = vertices[indices[i + 2]].position;

    std::float_t t;
    if (RayTrace(point, rayDir, v0, v1, v2, t)) {
      intersectionCount++;
    }
  }

  return (intersectionCount % 2 == 1);  // 홀수면 내부}
}

}  // namespace veng