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

#include "dusktest.h"
#include "util/memory.h"
#include "physics/physicstest.h"

static void test_aabbVsAabbOverlapAndSeparation(void **state) {
  vec3 normal; float_t depth;

  // Overlapping on X (smallest penetration axis).
  vec3 ac = { 0.6f, 0.0f, 0.0f }, ah = { 0.5f, 0.5f, 0.5f };
  vec3 bc = { 0.0f, 0.0f, 0.0f }, bh = { 0.5f, 0.5f, 0.5f };
  assert_true(physicsTestAabbVsAabb(ac, ah, bc, bh, normal, &depth));
  assert_float_equal(depth, 0.4f, 0.0001f);
  assert_float_equal(normal[0], 1.0f, 0.0001f);
  assert_float_equal(normal[1], 0.0f, 0.0001f);
  assert_float_equal(normal[2], 0.0f, 0.0001f);

  // Far apart: no overlap.
  vec3 cc = { 10.0f, 0.0f, 0.0f };
  assert_false(physicsTestAabbVsAabb(cc, ah, bc, bh, normal, &depth));

  assert_int_equal(memoryGetAllocatedCount(), 0);
}

static void test_sphereVsSphere(void **state) {
  vec3 normal; float_t depth;

  vec3 ac = { 1.0f, 0.0f, 0.0f };
  vec3 bc = { 0.0f, 0.0f, 0.0f };
  assert_true(physicsTestSphereVsSphere(ac, 0.75f, bc, 0.75f, normal, &depth));
  assert_float_equal(depth, 0.5f, 0.0001f);
  assert_float_equal(normal[0], 1.0f, 0.0001f);

  assert_false(physicsTestSphereVsSphere(ac, 0.25f, bc, 0.25f, normal, &depth));

  assert_int_equal(memoryGetAllocatedCount(), 0);
}

static void test_sphereVsAabbOutsideAndInside(void **state) {
  vec3 normal; float_t depth;

  // Sphere just outside the box corner, overlapping.
  vec3 sc = { 1.2f, 0.0f, 0.0f };
  vec3 ac = { 0.0f, 0.0f, 0.0f }, ah = { 0.5f, 0.5f, 0.5f };
  assert_true(physicsTestSphereVsAabb(sc, 0.75f, ac, ah, normal, &depth));
  assert_float_equal(normal[0], 1.0f, 0.0001f);
  assert_float_equal(depth, 0.75f - 0.7f, 0.0001f);

  // Sphere center inside the box: pushes out the nearest face.
  vec3 scInside = { 0.4f, 0.0f, 0.0f };
  assert_true(
    physicsTestSphereVsAabb(scInside, 0.1f, ac, ah, normal, &depth)
  );
  assert_float_equal(normal[0], 1.0f, 0.0001f);

  assert_int_equal(memoryGetAllocatedCount(), 0);
}

static void test_aabbVsPlane(void **state) {
  vec3 normal; float_t depth;

  vec3 ac = { 0.0f, 0.4f, 0.0f }, ah = { 0.5f, 0.5f, 0.5f };
  vec3 pn = { 0.0f, 1.0f, 0.0f };
  assert_true(physicsTestAabbVsPlane(ac, ah, pn, 0.0f, normal, &depth));
  assert_float_equal(depth, 0.1f, 0.0001f);
  assert_float_equal(normal[1], 1.0f, 0.0001f);

  vec3 acAbove = { 0.0f, 10.0f, 0.0f };
  assert_false(physicsTestAabbVsPlane(acAbove, ah, pn, 0.0f, normal, &depth));

  assert_int_equal(memoryGetAllocatedCount(), 0);
}

static void test_dispatchSymmetryAndPlaneRouting(void **state) {
  physicsshape_t cubeA = {
    .type = PHYSICS_SHAPE_CUBE,
    .data.cube.halfExtents = { 0.5f, 0.5f, 0.5f }
  };
  physicsshape_t sphereB = {
    .type = PHYSICS_SHAPE_SPHERE,
    .data.sphere.radius = 0.75f
  };
  vec3 aPos = { 0.6f, 0.0f, 0.0f };
  vec3 bPos = { 0.0f, 0.0f, 0.0f };

  vec3 normalAB; float_t depthAB;
  assert_true(physicsTestShapeVsShape(
    aPos, &cubeA, bPos, &sphereB, normalAB, &depthAB
  ));

  vec3 normalBA; float_t depthBA;
  assert_true(physicsTestShapeVsShape(
    bPos, &sphereB, aPos, &cubeA, normalBA, &depthBA
  ));

  // Swapping A/B should give the same depth and a negated normal.
  assert_float_equal(depthAB, depthBA, 0.0001f);
  assert_float_equal(normalAB[0], -normalBA[0], 0.0001f);

  // Cube vs plane routes to physicsTestAabbVsPlane.
  physicsshape_t plane = {
    .type = PHYSICS_SHAPE_PLANE,
    .data.plane = { .normal = { 0.0f, 1.0f, 0.0f }, .distance = 0.0f }
  };
  vec3 cubePos = { 0.0f, 0.4f, 0.0f };
  vec3 normal; float_t depth;
  assert_true(physicsTestShapeVsShape(
    cubePos, &cubeA, bPos, &plane, normal, &depth
  ));
  assert_float_equal(depth, 0.1f, 0.0001f);

  assert_int_equal(memoryGetAllocatedCount(), 0);
}

static void test_capsuleVsSphere(void **state) {
  vec3 normal; float_t depth;

  // Capsule standing on the Y axis, sphere touching its side.
  vec3 cc = { 0.0f, 0.0f, 0.0f };
  vec3 sc = { 0.9f, 0.0f, 0.0f };
  assert_true(physicsTestCapsuleVsSphere(
    cc, 0.5f, 1.0f, sc, 0.5f, normal, &depth
  ));
  assert_float_equal(normal[1], 0.0f, 0.0001f);
  assert_true(depth > 0.0f);

  vec3 scFar = { 5.0f, 0.0f, 0.0f };
  assert_false(physicsTestCapsuleVsSphere(
    cc, 0.5f, 1.0f, scFar, 0.5f, normal, &depth
  ));

  assert_int_equal(memoryGetAllocatedCount(), 0);
}

// Example custom shape callback: an infinite flat landscape at a fixed
// height, described by a single float_t (userData). Mirrors how a real
// terrain/heightfield callback would delegate to a primitive test.
static bool_t flatLandscapeTest(
  const vec3 selfPos, const void *selfData,
  const vec3 otherPos, const physicsshape_t *otherShape,
  vec3 outNormal, float_t *outDepth
) {
  const float_t *groundHeight = (const float_t *)selfData;
  vec3 planeNormal = { 0.0f, 1.0f, 0.0f };
  if(otherShape->type != PHYSICS_SHAPE_SPHERE) return false;
  return physicsTestSphereVsPlane(
    otherPos, otherShape->data.sphere.radius,
    planeNormal, selfPos[1] + *groundHeight, outNormal, outDepth
  );
}

static void test_customShapeFlatLandscape(void **state) {
  float_t groundHeight = 0.0f;
  physicsshape_t landscape = {
    .type = PHYSICS_SHAPE_CUSTOM,
    .data.custom = { .userData = &groundHeight, .test = flatLandscapeTest }
  };
  physicsshape_t sphere = {
    .type = PHYSICS_SHAPE_SPHERE,
    .data.sphere.radius = 0.5f
  };
  vec3 landscapePos = { 0.0f, 0.0f, 0.0f };
  // Sphere center at y=0.3, radius 0.5: overlaps the ground by 0.2.
  vec3 spherePos = { 0.0f, 0.3f, 0.0f };

  // Landscape as B: the ordering physicsWorldStep actually uses (dynamic
  // body as A, static/custom body as B).
  vec3 normal; float_t depth;
  assert_true(physicsTestShapeVsShape(
    spherePos, &sphere, landscapePos, &landscape, normal, &depth
  ));
  assert_float_equal(depth, 0.2f, 0.0001f);
  assert_float_equal(normal[0], 0.0f, 0.0001f);
  assert_float_equal(normal[1], 1.0f, 0.0001f); // pushes the sphere UP
  assert_float_equal(normal[2], 0.0f, 0.0001f);

  // Landscape as A: same depth, negated normal.
  vec3 normalSwapped; float_t depthSwapped;
  assert_true(physicsTestShapeVsShape(
    landscapePos, &landscape, spherePos, &sphere, normalSwapped, &depthSwapped
  ));
  assert_float_equal(depthSwapped, depth, 0.0001f);
  assert_float_equal(normalSwapped[1], -normal[1], 0.0001f);

  // Sphere far above the ground: no overlap.
  vec3 sphereFar = { 0.0f, 10.0f, 0.0f };
  assert_false(physicsTestShapeVsShape(
    sphereFar, &sphere, landscapePos, &landscape, normal, &depth
  ));

  assert_int_equal(memoryGetAllocatedCount(), 0);
}

static void test_customVsCustomAsserts(void **state) {
  float_t groundHeight = 0.0f;
  physicsshape_t landscapeA = {
    .type = PHYSICS_SHAPE_CUSTOM,
    .data.custom = { .userData = &groundHeight, .test = flatLandscapeTest }
  };
  physicsshape_t landscapeB = landscapeA;
  vec3 pos = { 0.0f, 0.0f, 0.0f };
  vec3 normal; float_t depth;

  expect_assert_failure(physicsTestShapeVsShape(
    pos, &landscapeA, pos, &landscapeB, normal, &depth
  ));
}

int main(int argc, char **argv) {
  const struct CMUnitTest tests[] = {
    cmocka_unit_test(test_aabbVsAabbOverlapAndSeparation),
    cmocka_unit_test(test_sphereVsSphere),
    cmocka_unit_test(test_sphereVsAabbOutsideAndInside),
    cmocka_unit_test(test_aabbVsPlane),
    cmocka_unit_test(test_dispatchSymmetryAndPlaneRouting),
    cmocka_unit_test(test_capsuleVsSphere),
    cmocka_unit_test(test_customShapeFlatLandscape),
    cmocka_unit_test(test_customVsCustomAsserts),
  };

  return cmocka_run_group_tests(tests, NULL, NULL);
}