//================= Copyright kotofyt, All rights reserved ==================//
//
// Purpose: Wrapper for rapier physics 
//
//===========================================================================//

#![allow(nonstandard_style)]
#![allow(unused)]

macro_rules! V_malloc {
    ($t:ty, $count:expr) => { 
        malloc(size_of::<$t>() * $count) as *mut $t
    };
}

use std::{default, ptr::{self, null}, sync::Arc};

use parry3d::{glamx::vec3, shape::{Shape, ShapeType, SharedShape}};
use rapier3d::{geometry::Ball, na::{UnitQuaternion, Vector4}};
use rapier3d::prelude::*;
use libc::{malloc, free};

#[repr(C)]
#[derive(Clone, Copy)]
pub struct BallShape_t
{
    m_fRadius: f32,
}

#[repr(C)]
#[derive(Clone, Copy)]
pub struct CuboidShape_t
{
    m_fExtentX: f32,
    m_fExtentY: f32,
    m_fExtentZ: f32,
}


#[derive(Clone)]
pub struct RapierShape_t
{
    m_eType: ShapeType,
    m_pShape: *mut dyn Shape,
    m_sharedShape: SharedShape,
}

#[repr(C)]
pub enum EPhysicsBodyType
{
	k_EPhysics_Static,
	k_EPhysics_Dynamic,
	k_EPhysics_KinematicPositionBased,
	k_EPhysics_KinematicVelocityBased,
}



#[derive(Clone)]
pub struct RapierCollider_t
{
    m_collider: Collider,
}



#[derive(Clone)]
pub struct RapierWorld_t
{
    m_colliders: ColliderSet,
    m_rigidBodies: RigidBodySet,
    m_islandManager: IslandManager,
    m_broadPhase: DefaultBroadPhase,
    m_narrowPhase: NarrowPhase,
    m_impulseJointSet: ImpulseJointSet,
    m_multibodyJointSet: MultibodyJointSet,
    m_ccdSolver: CCDSolver,
}

#[derive(Clone)]
pub struct RapierPhysicsBody_t
{
    m_body: RigidBody,
    m_pCollider: *mut RapierCollider_t,
    m_pWorld: *mut RapierWorld_t,

    m_hRigidBodyHandle: RigidBodyHandle,
    m_hColliderHandle: ColliderHandle,
}

#[derive(Clone, Copy)]
pub struct RapierPhysics_t
{

}

#[no_mangle]
pub unsafe extern "C" fn CRapierPhysicsBody_SetPosition( this: *mut RapierPhysicsBody_t, fX: f32, fY: f32, fZ: f32 )
{
    let world: &mut RapierWorld_t = &mut *(*this).m_pWorld;
    world.m_rigidBodies[(*this).m_hRigidBodyHandle]
        .set_translation(vec3(fX, fY, fZ), true);
}

#[no_mangle]
pub unsafe extern "C" fn CRapierPhysicsBody_SetRotation( this: *mut RapierPhysicsBody_t, fX: f32, fY: f32, fZ: f32, fW: f32 )
{
    let world: &mut RapierWorld_t = &mut *(*this).m_pWorld;
    world.m_rigidBodies[(*this).m_hRigidBodyHandle]
        .set_translation(vec3(fX, fY, fZ), true);
}

#[repr(C)]
pub struct Vector {
	x: f32,
	y: f32,
	z: f32,
}

#[no_mangle]
pub unsafe extern "C" fn CRapierPhysicsBody_GetPosition( this: *mut RapierPhysicsBody_t ) -> Vector
{
    let world: &mut RapierWorld_t = &mut *(*this).m_pWorld;
    let position = world.m_rigidBodies[(*this).m_hRigidBodyHandle]
        .translation().to_array();
    return Vector { x: position[0], y: position[1], z: position[2]}
}

#[repr(C)]
pub struct Quat {
	x: f32,
	y: f32,
	z: f32,
	w: f32,
}

#[no_mangle]
pub unsafe extern "C" fn CRapierPhysicsBody_GetRotation( this: *mut RapierPhysicsBody_t ) -> Quat 
{
    let world: &mut RapierWorld_t = &mut *(*this).m_pWorld;
    let rotationVector = world.m_rigidBodies[(*this).m_hRigidBodyHandle]
        .rotation().to_array();
    Quat{ x: rotationVector[0], y: rotationVector[1], z: rotationVector[2], w: rotationVector[3]}
}

#[no_mangle]
pub unsafe extern "C" fn CRapierPhysicsWorld_Frame( this: *mut RapierWorld_t, fDelta: f32 )
{
    let vGravity = vec3(0.0, -9.8, 0.0);
    let mut integrationParameters = IntegrationParameters::default();
    integrationParameters.dt = fDelta;
    let mut physicsPipeline = PhysicsPipeline::new();
    let physicsHooks = ();
    let eventHandler = ();
    physicsPipeline.step(
        vGravity,
        &integrationParameters,
        &mut (*this).m_islandManager,
        &mut (*this).m_broadPhase,
        &mut (*this).m_narrowPhase,
        &mut (*this).m_rigidBodies,
        &mut (*this).m_colliders,
        &mut (*this).m_impulseJointSet,
        &mut (*this).m_multibodyJointSet,
        &mut (*this).m_ccdSolver,
        &physicsHooks,
        &eventHandler,
    );
}

#[no_mangle]
pub unsafe extern "C" fn CRapierPhysicsWorld_CreateRigidBody( 
    this: *mut RapierWorld_t, 
    pCollider: *mut RapierCollider_t, 
    eType: EPhysicsBodyType ) -> *mut RapierPhysicsBody_t
{
    let eRapierBodyType: RigidBodyType;
    match eType
    {
        EPhysicsBodyType::k_EPhysics_Static => eRapierBodyType = RigidBodyType::Fixed,
        EPhysicsBodyType::k_EPhysics_Dynamic => eRapierBodyType = RigidBodyType::Dynamic,
        EPhysicsBodyType::k_EPhysics_KinematicPositionBased => eRapierBodyType = RigidBodyType::KinematicPositionBased,
        EPhysicsBodyType::k_EPhysics_KinematicVelocityBased => eRapierBodyType = RigidBodyType::KinematicVelocityBased,
    }
    let pBody = V_malloc!(RapierPhysicsBody_t, 1);
    std::ptr::write(&mut (*pBody).m_body, RigidBodyBuilder::new(eRapierBodyType).build());
    (*pBody).m_pCollider = pCollider;

    let hRigidBodyHandle = (*this).m_rigidBodies.insert((*pBody).m_body.clone());
    let hColliderHandle = (*this).m_colliders.insert_with_parent(
        (*pCollider).m_collider.clone(), hRigidBodyHandle, &mut (*this).m_rigidBodies);
    (*pBody).m_hRigidBodyHandle = hRigidBodyHandle;
    (*pBody).m_hColliderHandle = hColliderHandle;
    (*pBody).m_pWorld = this;

    pBody
}

#[no_mangle]
pub unsafe extern "C" fn CRapierPhysics_New() -> *mut RapierPhysics_t
{
    let physics = RapierPhysics_t {};
    let pPhysics = V_malloc!(RapierPhysics_t, 1);
    *pPhysics = physics;
    pPhysics
}

#[no_mangle]
pub unsafe extern "C" fn CRapierPhysics_CreateBall( this: *mut RapierPhysics_t, ball: BallShape_t ) -> *mut RapierShape_t
{
    let rapierShape = Ball::new(ball.m_fRadius);
    let pRapierShapeMemory: *mut Ball = V_malloc!(Ball, 1);
    *pRapierShapeMemory = rapierShape;

    let shape: RapierShape_t = RapierShape_t { 
        m_eType: ShapeType::Ball, 
        m_pShape: pRapierShapeMemory, 
        m_sharedShape: SharedShape::new(rapierShape) 
    };
    let pShapeMemory: *mut RapierShape_t = V_malloc!(RapierShape_t, 1);
    *pShapeMemory = shape;
    pShapeMemory
}

#[no_mangle]
pub unsafe extern "C" fn CRapierPhysics_CreateCube( this: *mut RapierPhysics_t, cuboid: CuboidShape_t ) -> *mut RapierShape_t
{
    let rapierShape = Cuboid::new(vec3(cuboid.m_fExtentX, cuboid.m_fExtentY, cuboid.m_fExtentZ));
    let pRapierShapeMemory: *mut Cuboid = V_malloc!(Cuboid, 1);
    *pRapierShapeMemory = rapierShape;

    let shape: RapierShape_t = RapierShape_t { 
        m_eType: ShapeType::Cuboid, 
        m_pShape: pRapierShapeMemory, 
        m_sharedShape: SharedShape::new(rapierShape) 
    };
    let pShapeMemory: *mut RapierShape_t = V_malloc!(RapierShape_t, 1);
    *pShapeMemory = shape;
    pShapeMemory
}

#[no_mangle]
pub unsafe extern "C" fn CRapierPhysics_CreateCollider( this: *mut RapierPhysics_t, pShape: *mut RapierShape_t ) -> *mut RapierCollider_t
{
    let pRapierShape = (*pShape).m_pShape;
    let shape: &SharedShape = &(*pShape).m_sharedShape;
    let pCollider = V_malloc!(RapierCollider_t, 1);
    std::ptr::write(&mut (*pCollider).m_collider, ColliderBuilder::new(shape.clone()).build());
    pCollider
}

#[no_mangle]
pub unsafe extern "C" fn CRapierPhysics_CreateWorld( this: *mut RapierPhysics_t ) -> *mut RapierWorld_t
{
    let pWorld: *mut RapierWorld_t = V_malloc!(RapierWorld_t, 1);
    let world: &mut RapierWorld_t = &mut *pWorld;
    std::ptr::write(&mut (*pWorld).m_colliders, ColliderSet::new());
    std::ptr::write(&mut (*pWorld).m_rigidBodies, RigidBodySet::new());
    std::ptr::write(&mut (*pWorld).m_islandManager, IslandManager::new());
    std::ptr::write(&mut (*pWorld).m_broadPhase, DefaultBroadPhase::new());
    std::ptr::write(&mut (*pWorld).m_narrowPhase, NarrowPhase::new());
    std::ptr::write(&mut (*pWorld).m_impulseJointSet, ImpulseJointSet::new());
    std::ptr::write(&mut (*pWorld).m_multibodyJointSet, MultibodyJointSet::new());
    std::ptr::write(&mut (*pWorld).m_ccdSolver, CCDSolver::new());
    pWorld
}