/**
 * 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 "entity/entitymanager.h"
#include "entity/component/display/entityposition.h"
#include "entity/component/physics/entityphysics.h"
#include "physics/physicsworld.h"

static void test_entityPhysicsGettersAndSetters(void **state) {
  entitymanager_t mgr;
  entityManagerInit(&mgr);

  entityid_t entity = entityManagerAdd(&mgr);
  componentid_t phys = entityAddComponent(&mgr, entity, COMPONENT_TYPE_PHYSICS);

  // Defaults: dynamic 1x1x1 cube, zero velocity, not on ground.
  assert_int_equal(
    entityPhysicsGetBodyType(&mgr, entity, phys), PHYSICS_BODY_DYNAMIC
  );
  assert_false(entityPhysicsIsOnGround(&mgr, entity, phys));

  vec3 velocity;
  entityPhysicsGetVelocity(&mgr, entity, phys, velocity);
  assert_float_equal(velocity[0], 0.0f, 0.0001f);
  assert_float_equal(velocity[1], 0.0f, 0.0001f);
  assert_float_equal(velocity[2], 0.0f, 0.0001f);

  vec3 setVel = { 1.0f, 2.0f, 3.0f };
  entityPhysicsSetVelocity(&mgr, entity, phys, setVel);
  entityPhysicsGetVelocity(&mgr, entity, phys, velocity);
  assert_float_equal(velocity[0], 1.0f, 0.0001f);
  assert_float_equal(velocity[1], 2.0f, 0.0001f);
  assert_float_equal(velocity[2], 3.0f, 0.0001f);

  vec3 impulse = { 1.0f, 0.0f, 0.0f };
  entityPhysicsApplyImpulse(&mgr, entity, phys, impulse);
  entityPhysicsGetVelocity(&mgr, entity, phys, velocity);
  assert_float_equal(velocity[0], 2.0f, 0.0001f);

  // Impulses are a no-op on static bodies.
  entityPhysicsSetBodyType(&mgr, entity, phys, PHYSICS_BODY_STATIC);
  entityPhysicsApplyImpulse(&mgr, entity, phys, impulse);
  entityPhysicsGetVelocity(&mgr, entity, phys, velocity);
  assert_float_equal(velocity[0], 2.0f, 0.0001f);
  assert_int_equal(
    entityPhysicsGetBodyType(&mgr, entity, phys), PHYSICS_BODY_STATIC
  );

  physicsshape_t sphere = {
    .type = PHYSICS_SHAPE_SPHERE,
    .data.sphere.radius = 2.0f
  };
  entityPhysicsSetShape(&mgr, entity, phys, sphere);
  physicsshape_t got = entityPhysicsGetShape(&mgr, entity, phys);
  assert_int_equal(got.type, PHYSICS_SHAPE_SPHERE);
  assert_float_equal(got.data.sphere.radius, 2.0f, 0.0001f);

  entityManagerDispose(&mgr);
  assert_int_equal(memoryGetAllocatedCount(), 0);
}

static void test_physicsWorldGravityIntegratesDynamicBody(void **state) {
  entitymanager_t mgr;
  entityManagerInit(&mgr);
  physicsworld_t world;
  physicsWorldInit(&world);

  entityid_t entity = entityManagerAdd(&mgr);
  componentid_t posComp = entityAddComponent(
    &mgr, entity, COMPONENT_TYPE_POSITION
  );
  entityAddComponent(&mgr, entity, COMPONENT_TYPE_PHYSICS);

  const float_t dt = 1.0f / 60.0f;
  physicsWorldStep(&world, &mgr, dt);

  vec3 pos;
  entityPositionGetWorldPosition(&mgr, entity, posComp, pos);

  // One step of gravity: velocity.y = gravity.y * dt, position integrates
  // that same-step velocity (semi-implicit Euler).
  float_t expectedVelY = world.gravity[1] * dt;
  float_t expectedPosY = expectedVelY * dt;
  assert_float_equal(pos[1], expectedPosY, 0.0001f);
  assert_true(pos[1] < 0.0f);

  entityManagerDispose(&mgr);
  assert_int_equal(memoryGetAllocatedCount(), 0);
}

static void test_physicsWorldRestsOnStaticFloor(void **state) {
  entitymanager_t mgr;
  entityManagerInit(&mgr);
  physicsworld_t world;
  physicsWorldInit(&world);

  // Static floor: a wide, thin platform centered at y=0 (top face at 0.5).
  entityid_t floorEntity = entityManagerAdd(&mgr);
  componentid_t floorPosComp = entityAddComponent(
    &mgr, floorEntity, COMPONENT_TYPE_POSITION
  );
  componentid_t floorPhysComp = entityAddComponent(
    &mgr, floorEntity, COMPONENT_TYPE_PHYSICS
  );
  entityPhysicsSetBodyType(&mgr, floorEntity, floorPhysComp, PHYSICS_BODY_STATIC);
  physicsshape_t floorShape = {
    .type = PHYSICS_SHAPE_CUBE,
    .data.cube.halfExtents = { 5.0f, 0.5f, 5.0f }
  };
  entityPhysicsSetShape(&mgr, floorEntity, floorPhysComp, floorShape);
  vec3 floorPos = { 0.0f, 0.0f, 0.0f };
  entityPositionSetLocalPosition(&mgr, floorEntity, floorPosComp, floorPos);

  // Dynamic body (default 1x1x1 cube) starting 4 units above the floor.
  entityid_t entity = entityManagerAdd(&mgr);
  componentid_t posComp = entityAddComponent(
    &mgr, entity, COMPONENT_TYPE_POSITION
  );
  componentid_t physComp = entityAddComponent(
    &mgr, entity, COMPONENT_TYPE_PHYSICS
  );
  vec3 startPos = { 0.0f, 4.0f, 0.0f };
  entityPositionSetLocalPosition(&mgr, entity, posComp, startPos);

  const float_t dt = 1.0f / 60.0f;
  for(int32_t i = 0; i < 300; i++) {
    physicsWorldStep(&world, &mgr, dt);
  }

  vec3 pos;
  entityPositionGetWorldPosition(&mgr, entity, posComp, pos);
  // Resting height: floor top (0.5) + body half-extent (0.5).
  assert_float_equal(pos[1], 1.0f, 0.01f);
  assert_true(entityPhysicsIsOnGround(&mgr, entity, physComp));

  vec3 velocity;
  entityPhysicsGetVelocity(&mgr, entity, physComp, velocity);
  assert_float_equal(velocity[1], 0.0f, 0.01f);

  entityManagerDispose(&mgr);
  assert_int_equal(memoryGetAllocatedCount(), 0);
}

static void test_physicsWorldDynamicVsDynamicSeparates(void **state) {
  entitymanager_t mgr;
  entityManagerInit(&mgr);
  physicsworld_t world;
  physicsWorldInit(&world);

  entityid_t entityA = entityManagerAdd(&mgr);
  componentid_t posA = entityAddComponent(
    &mgr, entityA, COMPONENT_TYPE_POSITION
  );
  componentid_t physA = entityAddComponent(
    &mgr, entityA, COMPONENT_TYPE_PHYSICS
  );
  vec3 posAStart = { 0.0f, 0.0f, 0.3f };
  entityPositionSetLocalPosition(&mgr, entityA, posA, posAStart);
  entityPhysicsGet(&mgr, entityA, physA)->gravityScale = 0.0f;

  entityid_t entityB = entityManagerAdd(&mgr);
  componentid_t posB = entityAddComponent(
    &mgr, entityB, COMPONENT_TYPE_POSITION
  );
  componentid_t physB = entityAddComponent(
    &mgr, entityB, COMPONENT_TYPE_PHYSICS
  );
  vec3 posBStart = { 0.0f, 0.0f, -0.3f };
  entityPositionSetLocalPosition(&mgr, entityB, posB, posBStart);
  entityPhysicsGet(&mgr, entityB, physB)->gravityScale = 0.0f;

  // Both default to 1x1x1 cubes (half-extent 0.5), overlapping by 0.4 on Z.
  physicsWorldStep(&world, &mgr, 1.0f / 60.0f);

  vec3 finalA, finalB;
  entityPositionGetWorldPosition(&mgr, entityA, posA, finalA);
  entityPositionGetWorldPosition(&mgr, entityB, posB, finalB);

  assert_float_equal(finalA[2], 0.5f, 0.0001f);
  assert_float_equal(finalB[2], -0.5f, 0.0001f);
  // Fully separated: touching exactly, no remaining overlap.
  assert_float_equal(finalA[2] - finalB[2], 1.0f, 0.0001f);

  entityManagerDispose(&mgr);
  assert_int_equal(memoryGetAllocatedCount(), 0);
}

static void test_physicsWorldStepNoPhysicsEntitiesIsNoop(void **state) {
  entitymanager_t mgr;
  entityManagerInit(&mgr);
  physicsworld_t world;
  physicsWorldInit(&world);

  entityManagerAdd(&mgr);
  physicsWorldStep(&world, &mgr, 1.0f / 60.0f);

  entityManagerDispose(&mgr);
  assert_int_equal(memoryGetAllocatedCount(), 0);
}

int main(int argc, char **argv) {
  const struct CMUnitTest tests[] = {
    cmocka_unit_test(test_entityPhysicsGettersAndSetters),
    cmocka_unit_test(test_physicsWorldGravityIntegratesDynamicBody),
    cmocka_unit_test(test_physicsWorldRestsOnStaticFloor),
    cmocka_unit_test(test_physicsWorldDynamicVsDynamicSeparates),
    cmocka_unit_test(test_physicsWorldStepNoPhysicsEntitiesIsNoop),
  };

  return cmocka_run_group_tests(tests, NULL, NULL);
}