// This file is part of the 64k demo project.
// It implements a lightweight SDF-based physics engine.

#include "3d/physics.h"
#include "3d/bvh.h"
#include "3d/sdf_cpu.h"
#include <algorithm>

namespace {
// Helper to get world AABB (copied from bvh.cc or shared)
AABB get_world_aabb(const Object3D& obj) {
  BoundingVolume local = obj.get_local_bounds();
  mat4 model = obj.get_model_matrix();

  vec3 corners[8] = {
      {local.min.x, local.min.y, local.min.z},
      {local.max.x, local.min.y, local.min.z},
      {local.min.x, local.max.y, local.min.z},
      {local.max.x, local.max.y, local.min.z},
      {local.min.x, local.min.y, local.max.z},
      {local.max.x, local.min.y, local.max.z},
      {local.min.x, local.max.y, local.max.z},
      {local.max.x, local.max.y, local.max.z},
  };

  AABB world;
  for (int i = 0; i < 8; ++i) {
    vec4 p = model * vec4(corners[i].x, corners[i].y, corners[i].z, 1.0f);
    world.expand(p.xyz());
  }
  return world;
}
} // namespace

float PhysicsSystem::sample_sdf(const Object3D& obj, vec3 world_p) {
  mat4 inv_model = obj.get_model_matrix().inverse();
  vec4 local_p4 = inv_model * vec4(world_p.x, world_p.y, world_p.z, 1.0f);
  vec3 q = local_p4.xyz();

  float d = 1000.0f;
  if (obj.type == ObjectType::SPHERE) {
    d = q.len() - 1.0f;
  } else if (obj.type == ObjectType::BOX || obj.type == ObjectType::CUBE) {
    d = sdf::sdBox(q, vec3(1.0f, 1.0f, 1.0f));
  } else if (obj.type == ObjectType::TORUS) {
    d = sdf::sdTorus(q, vec2(1.0f, 0.4f));
  } else if (obj.type == ObjectType::PLANE) {
    d = sdf::sdPlane(q, vec3(0.0f, 1.0f, 0.0f), 0.0f);
  }

  // Extract scale from model matrix (assuming orthogonal with uniform or
  // non-uniform scale)
  mat4 model = obj.get_model_matrix();
  float sx = vec3(model.m[0], model.m[1], model.m[2]).len();
  float sy = vec3(model.m[4], model.m[5], model.m[6]).len();
  float sz = vec3(model.m[8], model.m[9], model.m[10]).len();
  float s = std::min(sx, std::min(sy, sz));

  return d * s;
}

void PhysicsSystem::resolve_collision(Object3D& a, Object3D& b) {
  if (a.is_static && b.is_static)
    return;

  // Probe points for 'a' (center and corners)
  BoundingVolume local = a.get_local_bounds();
  mat4 model_a = a.get_model_matrix();
  vec3 probes[9] = {
      {0, 0, 0}, // Center
      {local.min.x, local.min.y, local.min.z},
      {local.max.x, local.min.y, local.min.z},
      {local.min.x, local.max.y, local.min.z},
      {local.max.x, local.max.y, local.min.z},
      {local.min.x, local.min.y, local.max.z},
      {local.max.x, local.min.y, local.max.z},
      {local.min.x, local.max.y, local.max.z},
      {local.max.x, local.max.y, local.max.z},
  };

  for (int i = 0; i < 9; ++i) {
    vec3 world_probe =
        (model_a * vec4(probes[i].x, probes[i].y, probes[i].z, 1.0f)).xyz();
    float d = sample_sdf(b, world_probe);

    if (d < 0.0f) {
      // Collision detected!
      float penetration = -d;

      // Calculate normal via gradient of b's SDF
      auto b_sdf = [this, &b](vec3 p) { return sample_sdf(b, p); };
      vec3 normal = sdf::calc_normal(world_probe, b_sdf);

      // Resolution
      if (!a.is_static) {
        // Positional correction
        a.position += normal * penetration;

        // Velocity response
        float v_dot_n = vec3::dot(a.velocity, normal);
        if (v_dot_n < 0) {
          a.velocity -= normal * (1.0f + a.restitution) * v_dot_n;
        }
      }
    }
  }
}

void PhysicsSystem::update(Scene& scene, float dt) {
  if (dt <= 0)
    return;

  // 1. Integration
  for (auto& obj : scene.objects) {
    if (obj.is_static)
      continue;
    obj.velocity += gravity * dt;
    obj.position += obj.velocity * dt;
  }

  // 2. Broad Phase
  BVH bvh;
  BVHBuilder::build(bvh, scene.objects);

  // 3. Narrow Phase & Resolution
  // We do multiple iterations for better stability? Just 1 for now.
  for (int iter = 0; iter < 2; ++iter) {
    for (int i = 0; i < (int)scene.objects.size(); ++i) {
      Object3D& a = scene.objects[i];
      if (a.is_static)
        continue;

      AABB query_box = get_world_aabb(a);
      std::vector<int> candidates;
      bvh.query(query_box, candidates);

      for (int cand_idx : candidates) {
        if (cand_idx == i)
          continue;
        resolve_collision(a, scene.objects[cand_idx]);
      }
    }
  }
}