PHYSX_DEFAULT_SIMULATION_SHAPE_FLAGS :: cast(u8)(PhysX.PxShapeFlags.Visualization | PhysX.PxShapeFlags.SceneQueryShape | PhysX.PxShapeFlags.SimulationShape);
PHYSX_DEFAULT_TRIGGER_SHAPE_FLAGS :: cast(u8)(PhysX.PxShapeFlags.Visualization | PhysX.PxShapeFlags.SceneQueryShape | PhysX.PxShapeFlags.TriggerShape);

PhysX_Handle :: #type, distinct u32;

PhysX_Actor_Type :: enum {
    STATIC;
    DYNAMIC;
}

PhysX_Actor :: struct {
    type : PhysX_Actor_Type;

    sync_rotation_from_physx: bool = true;

    union {
        static: *PhysX.PxRigidStatic;
        dynamic: *PhysX.PxRigidDynamic;
    }
}

PhysX_Scene :: struct {
    scene: *PhysX.PxScene;
    actors : PArray(PhysX_Actor, PhysX_Handle);
}

init_physx :: () {
    default_allocator = PhysX.get_default_allocator();
    default_error_callback = PhysX.get_default_error_callback();

    foundation := PhysX.PxCreateFoundation(PhysX.PX_PHYSICS_VERSION, *default_allocator, *default_error_callback);
    if foundation == null {
        log_error("Could not initialize PhysX\n");
        return;
    }
    
    pvd := PhysX.PxCreatePvd(foundation);
    transport := PhysX.PxDefaultPvdSocketTransportCreate("127.0.0.1", 5425, 10);
    PhysX.PxPvd_connect(pvd, transport, cast(u8)PhysX.PxPvdInstrumentationFlags.ALL);

    tolerance_scale : PhysX.PxTolerancesScale;
    tolerance_scale.length = 1;
    tolerance_scale.speed = 10;
    physics = PhysX.PxCreatePhysics(PhysX.PX_PHYSICS_VERSION, foundation, *tolerance_scale, true, pvd, null);

    dispatcher = PhysX.PxDefaultCpuDispatcherCreate(2);
    material = PhysX.PxPhysics_createMaterial(physics, 0.0, 0.0, 0.6);

    // Callbacks
    info : PhysX.SimulationEventCallbackInfo;
    info.triggerCallback = on_physx_trigger;
    event_callback = PhysX.create_simulation_event_callbacks(*info);
}

internal_on_trigger_enter :: (trigger: *Entity, other: *Entity) {
    print("% entered by %\n", trigger.type, other.type);
}

internal_on_trigger_exit :: (trigger: *Entity, other: *Entity) {
    print("% exited by %\n", trigger.type, other.type);
}

on_physx_trigger :: (_u: *void, pair: *PhysX.PxTriggerPair, count: u32) #c_call {
    push_context {
        trigger := cast(*Entity)pair.triggerActor.userData;
        other := cast(*Entity)pair.otherActor.userData;

        status := cast(PhysX.PxPairFlags)pair.status;
        if status & .NotifyTouchFound {
            internal_on_trigger_enter(trigger, other);
        } else if status & .NotifyTouchLost {
            internal_on_trigger_exit(trigger, other);
        }
    }
}

tick_physx :: (scene: *PhysX_Scene, dt: float) {
    PhysX.PxScene_simulate(scene.scene, dt, null, null, 0, true);
    PhysX.PxScene_fetchResults(scene.scene, true, null);
}

init_physx_scene :: (game_scene: *Scene) {
    tolerance_scale : PhysX.PxTolerancesScale;
    tolerance_scale.length = 1;
    tolerance_scale.speed = 10;

    scene_desc := PhysX.PxSceneDesc_new(*tolerance_scale);
    scene_desc.gravity.y = -9.81;

    scene_desc.cpuDispatcher = xx dispatcher;
    scene_desc.simulationEventCallback = event_callback;
    PhysX.set_default_filter_shader(*scene_desc);

    scene := PhysX.PxPhysics_createScene(physics, *scene_desc);
    
    // @Incomplete: If debug
    pvd_client := PhysX.PxScene_getScenePvdClient(scene);
    if pvd_client {
        PhysX.PxPvdSceneClient_setScenePvdFlag(pvd_client, xx PhysX.PxPvdSceneFlag.TRANSMIT_CONSTRAINTS, true);
        PhysX.PxPvdSceneClient_setScenePvdFlag(pvd_client, xx PhysX.PxPvdSceneFlag.TRANSMIT_CONTACTS, true);
        PhysX.PxPvdSceneClient_setScenePvdFlag(pvd_client, xx PhysX.PxPvdSceneFlag.TRANSMIT_SCENEQUERIES, true);
    }

    physx_scene : PhysX_Scene;
    physx_scene.scene = scene;
    physx_scene.actors.data.allocator = game_scene.allocator;
    physx_scene.actors.indices.allocator = game_scene.allocator;

    game_scene.physx_scene = physx_scene;
}

deinit_physx_scene :: (game_scene: *Scene) {
    PhysX.PxScene_release(game_scene.physx_scene.scene);    
}

pre_physx_sync :: (game_scene: *Scene) {
    for game_scene.entities {
        if it.flags & .PHYSICS {
            if it.physics.physx_handle != 0 {
                // @Incomplete: Update the transform!
                physx_actor := parray_get(*game_scene.physx_scene.actors, it.physics.physx_handle);
                if physx_actor.type == .DYNAMIC {
                    PhysX.PxRigidDynamic_setLinearVelocity(physx_actor.dynamic, *it.physics.velocity, true);
                }
            } else {
                create_physx_actor(it);
            }
        }
    }
}

post_physx_sync :: (game_scene: *Scene) {
    for game_scene.entities {
        if it.flags & .PHYSICS {
            if it.physics.physx_handle != 0 {
                physx_actor := parray_get(*game_scene.physx_scene.actors, it.physics.physx_handle);
                if physx_actor.type == .DYNAMIC {
                    vel := PhysX.PxRigidDynamic_getLinearVelocity(physx_actor.dynamic);
                    it.physics.velocity = vel;

                    transform := PhysX.PxRigidActor_getGlobalPose(physx_actor.dynamic);

                    if physx_actor.sync_rotation_from_physx {
                        set_position_rotation(it, transform.p, transform.q);
                    } else {
                        set_position(it, transform.p);
                    }
                }
            }
        }
    }
}

create_physx_actor :: (e: *Entity) {
    actor : *PhysX.PxRigidActor;
    transform : PhysX.PxTransform;

    if e.physics.type == .CAPSULE {
        angle := PI * 0.5;
        half_angle := angle * 0.5;

        sin_half := sin(half_angle);
        cos_half := cos(half_angle);

        rotation := Quaternion.{0, 0, sin(-PI * 0.25), cos(-PI * 0.25)};
        transform = PhysX.PxTransform_new(*e.transform.position, *rotation);
    } else {
        transform = PhysX.PxTransform_new(*e.transform.position, *e.transform.orientation);
    }

    if e.physics.dynamic {
        dynamic := PhysX.PxPhysics_createRigidDynamic(physics, *transform);
        actor = dynamic;

        if e.physics.lock & .ANGULAR_X {
            PhysX.PxRigidDynamic_setRigidDynamicLockFlag(dynamic, xx PhysX.PxRigidDynamicLockFlags.LockAngularX, true);
        }
        if e.physics.lock & .ANGULAR_Y {
            PhysX.PxRigidDynamic_setRigidDynamicLockFlag(dynamic, xx PhysX.PxRigidDynamicLockFlags.LockAngularY, true);
        }
        if e.physics.lock & .ANGULAR_Z {
            PhysX.PxRigidDynamic_setRigidDynamicLockFlag(dynamic, xx PhysX.PxRigidDynamicLockFlags.LockAngularZ, true);
        }
    } else {
        actor = PhysX.PxPhysics_createRigidStatic(physics, *transform);
    }

    material := PhysX.PxPhysics_createMaterial(physics, e.physics.static_friction, e.physics.dynamic_friction, e.physics.restitution);
    geo : *PhysX.PxGeometry;
    
    actor.userData = e;

    if e.physics.type == {
        case .SPHERE; {
            geo = PhysX.PxSphereGeometry_new(e.physics.sphere.radius);
        }
        case .BOX; {
            geo = PhysX.PxBoxGeometry_new(e.physics.box.half_extent*e.transform.scale);
        }
        case .CAPSULE; {
            geo = PhysX.PxCapsuleGeometry_new(e.physics.capsule.radius, e.physics.capsule.half_height-e.physics.capsule.radius);
        }
    }

    shape := PhysX.PxPhysics_createShape(physics, geo, material, false, ifx e.physics.trigger then PHYSX_DEFAULT_TRIGGER_SHAPE_FLAGS else PHYSX_DEFAULT_SIMULATION_SHAPE_FLAGS);

    PhysX.PxRigidActor_attachShape(actor, shape);

    PhysX.PxScene_addActor(e.scene.physx_scene.scene, actor, null);

    PhysX.PxShape_release(shape);
    PhysX.PxBase_release(material);

    physics_actor : PhysX_Actor;
    physics_actor.type = ifx e.physics.dynamic then .DYNAMIC else .STATIC;
    physics_actor.sync_rotation_from_physx = e.physics.type != .CAPSULE; // @Incomplete

    if physics_actor.type == .DYNAMIC {
        physics_actor.dynamic = xx actor;
    } else {
        physics_actor.static = xx actor;
    }

    e.physics.physx_handle = parray_add(*e.scene.physx_scene.actors, physics_actor);
    e.physics.enabled = true;
}

add_physx_sphere :: (entity: *Entity, radius: float) -> PhysX_Handle {
    geo := PhysX.PxSphereGeometry_new(radius);

    transform := PhysX.PxTransform_new(*entity.transform.position);
    actor := PhysX.PxPhysics_createRigidDynamic(physics, *transform);
    actor.userData = entity;

    material := PhysX.PxPhysics_createMaterial(physics, 0.1, 0.1, 0.5);

    //PhysX.PxMaterial_setRestitutionCombineMode(material,1); // Turn off restitution no matter the other material

    shape := PhysX.PxPhysics_createShape(physics, *geo, material, false, PHYSX_DEFAULT_SHAPE_FLAGS);
    PhysX.PxRigidActor_attachShape(actor, shape);

    PhysX.PxRigidBodyExt_updateMassAndInertia(actor, 0.1, null, false);
    PhysX.PxScene_addActor(entity.scene.physx_scene.scene, actor, null);

    PhysX.PxShape_release(shape);
    PhysX.PxBase_release(material);

    physics_actor : PhysX_Actor;
    physics_actor.type = .DYNAMIC;
    physics_actor.sync_rotation_from_physx = true;
    physics_actor.dynamic = actor;

    entity.physx_handle = parray_add(*entity.scene.physx_scene.actors, physics_actor);
    return entity.physx_handle;
}

add_physx_capsule :: (entity: *Entity, half_height: float, radius: float) -> PhysX_Handle {
    geo := PhysX.PxCapsuleGeometry_new(radius, half_height-radius);

    angle := PI * 0.5;
    half_angle := angle * 0.5;

    sin_half := sin(half_angle);
    cos_half := cos(half_angle);

    rotation := Quaternion.{0, 0, sin(-PI * 0.25), cos(-PI * 0.25)};

    transform := PhysX.PxTransform_new(*entity.transform.position, *rotation);

    actor := PhysX.PxPhysics_createRigidDynamic(physics, *transform);
    actor.userData = entity;

    PhysX.PxRigidDynamic_setRigidDynamicLockFlag(actor, xx PhysX.PxRigidDynamicLockFlags.LockAngularX, true);
    PhysX.PxRigidDynamic_setRigidDynamicLockFlag(actor, xx PhysX.PxRigidDynamicLockFlags.LockAngularY, true);
    PhysX.PxRigidDynamic_setRigidDynamicLockFlag(actor, xx PhysX.PxRigidDynamicLockFlags.LockAngularZ, true);

    material := PhysX.PxPhysics_createMaterial(physics, 0.0, 0.0, 0.0);

    PhysX.PxMaterial_setRestitutionCombineMode(material,1); // Turn off restitution no matter the other material

    shape := PhysX.PxPhysics_createShape(physics, *geo, material, false, PHYSX_DEFAULT_SHAPE_FLAGS);
    PhysX.PxRigidActor_attachShape(actor, shape);

    PhysX.PxRigidBodyExt_updateMassAndInertia(actor, 0.1, null, false);
    PhysX.PxScene_addActor(entity.scene.physx_scene.scene, actor, null);

    PhysX.PxShape_release(shape);
    PhysX.PxBase_release(material);

    physics_actor : PhysX_Actor;
    physics_actor.type = .DYNAMIC;
    physics_actor.sync_rotation_from_physx = false;
    physics_actor.dynamic = actor;

    entity.physx_handle = parray_add(*entity.scene.physx_scene.actors, physics_actor);
    return entity.physics.physx_handle;
}

add_physx_box :: (entity: *Entity, half_extent: Vector3, offset: Vector3 = .{}) -> PhysX_Handle {
    shape := PhysX.PxPhysics_createShape(physics, PhysX.PxBoxGeometry_new(half_extent), material, false, PHYSX_DEFAULT_SHAPE_FLAGS);
    pos := entity.transform.position + offset;
    t := PhysX.PxTransform_new(*pos, *entity.transform.orientation);

    body := PhysX.PxPhysics_createRigidStatic(physics, *t);
    body.userData = entity;
    PhysX.PxRigidActor_attachShape(body, shape);
    PhysX.PxScene_addActor(entity.scene.physx_scene.scene, body, null);

    PhysX.PxShape_release(shape);

    physics_actor : PhysX_Actor;
    physics_actor.type = .STATIC;
    physics_actor.static = body;

    entity.physx_handle = parray_add(*entity.scene.physx_scene.actors, physics_actor);
    return entity.physics.physx_handle;

}

create_stack :: (scene: *PhysX.PxScene, t: PhysX.PxTransform, size: u32, half_extent: float) {
	shape := PhysX.PxPhysics_createShape(physics, PhysX.PxBoxGeometry_new(half_extent, half_extent, half_extent), material, false, PHYSX_DEFAULT_SHAPE_FLAGS);
	for i: 0..size-1 {
		for j: 0..size-i-1 {
            pos := Vector3.{cast(float)(j*2) - cast(float)(size-i), cast(float)(i*2+1), 0} * half_extent;
			local_tm := PhysX.PxTransform_new(*pos);

			body := PhysX.PxPhysics_createRigidDynamic(physics, *PhysX.PxTransform_transform(*t, *local_tm));
			PhysX.PxRigidActor_attachShape(body, shape);
			PhysX.PxRigidBodyExt_updateMassAndInertia(body, 10., null, false);
			PhysX.PxScene_addActor(scene, body, null);
		}
	}
	//shape->release();
}

Hit :: struct {

}

physx_raycast :: (origin: Vector3, direction: Vector3, max_distance: float = 1000.0) -> bool, Hit {
    hit : PhysX.PxRaycastHit;
    filter_data := PhysX.PxQueryFilterData_new();
    has_hit := PhysX.PxSceneQueryExt_raycastSingle(engine.current_scene.physx_scene.scene, *origin, *direction, max_distance, 0, *hit, *filter_data, null, null);
    return has_hit, .{};
}

create_dynamic :: (scene: *PhysX.PxScene, t: PhysX.PxTransform, geometry: *PhysX.PxGeometry, velocity: Vector3 = .{}) -> *PhysX.PxRigidDynamic {
    p := Vector3.{0,0,0};
    transform := PhysX.PxTransform_new(cast(s32)PhysX.PxIDENTITY.PxIdentity);
    dynamic := PhysX.PxCreateDynamic(physics, *t, geometry, material, 10.0, *transform);
	PhysX.PxRigidBody_setAngularDamping(dynamic, 0.5);
	PhysX.PxRigidDynamic_setLinearVelocity(dynamic, *velocity, true);
	PhysX.PxScene_addActor(scene, dynamic, null);
	return dynamic;
}

PhysX :: #import "PhysX";

#scope_file

physics : *PhysX.PxPhysics;
material : *PhysX.PxMaterial;
default_allocator : PhysX.PxAllocatorCallback;
default_error_callback : PhysX.PxErrorCallback;
default_filter_shader : PhysX.SimulationFilterShader;
dispatcher : *PhysX.PxDefaultCpuDispatcher;
event_callback : *PhysX.PxSimulationEventCallback;