// Copyright (c) 2023 Dominic Masters
// 
// This software is released under the MIT License.
// https://opensource.org/licenses/MIT

#include "AABB3D.hpp"

using namespace Dawn;

bool_t Dawn::aabb3dIntersect(struct AABB3D cube1, struct AABB3D cube2) {
  return (
    (cube1.min.x <= cube2.max.x && cube1.max.x >= cube2.min.x) &&
    (cube1.min.y <= cube2.max.y && cube1.max.y >= cube2.min.y) &&
    (cube1.min.z <= cube2.max.z && cube1.max.z >= cube2.min.z)
  );
}

bool_t aabb3dSweep(
  struct AABB3D cube1, glm::vec3 velocity1,
  struct AABB3D cube2, glm::vec3 velocity2,
  glm::vec3 *normal
) {
  glm::vec3 relVel = vel1 - vel2;
  glm::vec3 relPos = pos1 - pos2;

  // If the relative velocity is zero, the cubes are already intersecting
  if(glm::length2(relVel) == 0.0f) {
    fraction = 0.0f;
    return true;
  }

  // Expand the size of the cubes by the magnitude of the relative velocity
  glm::vec3 size1exp = size1 + glm::abs(relVel);
  glm::vec3 size2exp = size2 + glm::abs(relVel);

  // Compute the times at which the cubes first and last overlap on each axis
  float_t tminx = (size1exp.x + size2exp.x - glm::abs(relPos.x)) / glm::abs(relVel.x);
  float_t tmaxx = (size1.x + size2.x - glm::abs(relPos.x)) / glm::abs(relVel.x);
  float_t tminy = (size1exp.y + size2exp.y - glm::abs(relPos.y)) / glm::abs(relVel.y);
  float_t tmaxy = (size1.y + size2.y - glm::abs(relPos.y)) / glm::abs(relVel.y);
  float_t tminz = (size1exp.z + size2exp.z - glm::abs(relPos.z)) / glm::abs(relVel.z);
  float_t tmaxz = (size1.z + size2.z - glm::abs(relPos.z)) / glm::abs(relVel.z);

  // Find the earliest and latest times of overlap
  float_t tmin = glm::max(glm::max(glm::min(tminx, tmaxx), glm::min(tminy, tmaxy)), glm::min(tminz, tmaxz));
  float_t tmax = glm::min(glm::min(glm::max(tminx, tmaxx), glm::max(tminy, tmaxy)), glm::max(tminz, tmaxz));

  // If the earliest time of overlap is greater than the length of the timestep, the cubes
  // will not collide during the timestep
  if(tmin > 1.0f) {
    fraction = 1.0f;
    return false;
  }

  // If the latest time of overlap is less than or equal to zero, the cubes are already
  // colliding and will collide throughout the timestep
  if(tmax <= 0.0f) {
    fraction = 0.0f;
    return true;
  }

  // Compute the fraction of the timestep at which the collision occurs
  fraction = glm::clamp(tmin, 0.0f, 1.0f);

  // Check if the two AABB cubes are intersecting at the time of collision
  glm::vec3 min1 = pos1 + vel1 * fraction - size1 / 2.0f;
  glm::vec3 max1 = pos1 + vel1 * fraction + size1 / 2.0f;
  glm::vec3 min2 = pos2 + vel2 * fraction - size2 / 2.0f;
  glm::vec3 max2 = pos2 + vel2 * fraction + size2 / 2.0f;
  return aabbIntersect(min1, max1, min2, max2);
}