// Copyright (C) 2016 ChaosForge Ltd
// http://chaosforge.org/
//
// This file is part of Nova libraries. 
// For conditions of distribution and use, see copying.txt file in root folder.

#include "nv/bullet/bullet_world.hh"

#include "src/bullet/bullet_helper.hh"
#include <btBulletDynamicsCommon.h>
#include <btBulletCollisionCommon.h>

using namespace nv;

struct alignas( 16 ) motion_state : public btMotionState
{
	btTransform m_gfx_world_tr;
	btTransform	m_com_offset;
	btTransform	m_com_offset_inv;
	SIMD_FORCE_INLINE void* operator new( size_t sz ) { return btAlignedAlloc( sz, 16 ); }
	SIMD_FORCE_INLINE void  operator delete( void* ptr ) { btAlignedFree( ptr ); }

	motion_state( const btTransform& start_tr = btTransform::getIdentity(), const btTransform& com_offset = btTransform::getIdentity() )
		: m_gfx_world_tr( start_tr )
		, m_com_offset( com_offset )
		, m_com_offset_inv( m_com_offset.inverse() )
	{
	}

	motion_state( const transform& startTrans = transform(), const nv::transform& com_offset = nv::transform() )
		: m_gfx_world_tr( n2b( startTrans ) )
		, m_com_offset( n2b( com_offset ) )
		, m_com_offset_inv( m_com_offset.inverse() )
	{
	}

	nv::transform get_world_transform() const
	{
		return b2n( m_gfx_world_tr );
	}

	void set_world_transform( const nv::transform& tr )
	{
		m_gfx_world_tr = n2b( tr );
	}

	virtual void	getWorldTransform( btTransform& com_world_tr ) const
	{
		com_world_tr = m_gfx_world_tr * m_com_offset_inv;
	}

	virtual void	setWorldTransform( const btTransform& com_world_tr )
	{
		m_gfx_world_tr = com_world_tr * m_com_offset;
	}

};


#define SELF static_cast<btDiscreteDynamicsWorld*>( world )

bullet_world::bullet_world()
{
	btBroadphaseInterface*               broadphase = new btDbvtBroadphase();
	btDefaultCollisionConfiguration*     collisionConfiguration = new btDefaultCollisionConfiguration();
	btCollisionDispatcher*               dispatcher = new btCollisionDispatcher( collisionConfiguration );
	btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
	btDiscreteDynamicsWorld*             dworld = new btDiscreteDynamicsWorld( dispatcher, broadphase, solver, collisionConfiguration );
	world = dworld;
}

void bullet_world::set_gravity( const vec3& gravity )
{
	SELF->setGravity( n2b( gravity ) );
}

int nv::bullet_world::step_simulation( float dtime, int max_sub_steps /*= 1*/, float fixed_time_step /*= 1.0f / 60.0f */ )
{
	return SELF->stepSimulation( dtime, max_sub_steps, fixed_time_step );
}

collision_shape bullet_world::create_sphere( float radius )
{
	return collision_shape{ ( void* )new btSphereShape( radius ) };
}

nv::collision_shape nv::bullet_world::create_capsule( float radius, float height )
{
	return collision_shape{ ( void* )new btCapsuleShape( radius, height ) };
}

nv::collision_shape nv::bullet_world::create_cylinder( const vec3& half_extents )
{
	return collision_shape{ ( void* )new btCylinderShape( n2b( half_extents ) ) };
}

collision_shape bullet_world::create_box( const vec3& half_extens )
{
	return collision_shape{ ( void* )new btBoxShape( n2b( half_extens ) ) };
}

collision_shape bullet_world::create_static_plane( const vec3& norm, float cst )
{
	return collision_shape{ ( void* )new btStaticPlaneShape( n2b( norm ), cst ) };
}

void bullet_world::add_rigid_body( rigid_body body )
{
	SELF->addRigidBody( static_cast<btRigidBody*>( body.internal ) );
}

rigid_body bullet_world::create_rigid_body( float mass, const transform& tr, collision_shape shape, const vec3& com_offset )
{
	btCollisionShape* sh = static_cast<btCollisionShape*>( shape.internal );
	bool is_dynamic = ( mass != 0.f );

	btVector3 local_inertia( 0, 0, 0 );
	if ( is_dynamic )
		sh->calculateLocalInertia( mass, local_inertia );

	motion_state* ms = new motion_state( tr, nv::transform( com_offset ) );

	btRigidBody::btRigidBodyConstructionInfo rb_info( mass, ms, sh, local_inertia );
	btRigidBody* body = new btRigidBody( rb_info );
	return rigid_body{ body };
}

nv::collision_shape nv::bullet_world::create_mesh( array_view< vec3 > vtx, array_view< uint32 > idx )
{
	btTriangleMesh* mesh = new btTriangleMesh;
	for ( uint32 i = 0; i < idx.size() / 3; ++i )
	{
		mesh->addTriangle( n2b( vtx[idx[i * 3+0]] ), n2b(vtx[idx[i * 3+1]]), n2b( vtx[idx[i * 3+ 2]] ) );
	}

//	btTriangleIndexVertexArray* array = new btTriangleIndexVertexArray( idx.size() / 3, (int*)idx.data(), 3 * sizeof(int), vtx.size(), (float*)vtx.data(), sizeof(vtx) );
	btBvhTriangleMeshShape* shape = new btBvhTriangleMeshShape( mesh, true );
//	shape->buildOptimizedBvh();
	return collision_shape{ shape, nullptr };
}

constraint nv::bullet_world::create_hinge_constraint( rigid_body a, const transform& ta, rigid_body b, const transform& tb, const vec2& low_high, const vec3& params /*= vec3( 0.9f, 0.3f, 1.0f ) */ )
{
	btRigidBody* rba = static_cast<btRigidBody*>( a.internal );
	btRigidBody* rbb = static_cast<btRigidBody*>( b.internal );
	btHingeConstraint* hinge = new btHingeConstraint( *rba, *rbb, n2b( ta ), n2b( tb ) );
	hinge->setLimit( low_high.x, low_high.y, params.x, params.y, params.z );
	return constraint{ hinge };
}

constraint nv::bullet_world::create_cone_twist_constraint( rigid_body a, const transform& ta, rigid_body b, const transform& tb, const vec3& sst, const vec3& params /*= vec3( 1.0f, 0.3f, 1.0f ) */ )
{
	btRigidBody* rba = static_cast<btRigidBody*>( a.internal );
	btRigidBody* rbb = static_cast<btRigidBody*>( b.internal );
	btConeTwistConstraint* cone_twist = new btConeTwistConstraint( *rba, *rbb, n2b( ta ), n2b( tb ) );
	cone_twist->setLimit( sst.x, sst.y, sst.z, params.x, params.y, params.z );
	return constraint{ cone_twist };
}

void nv::bullet_world::remove_rigid_body( rigid_body body )
{
	SELF->removeRigidBody( static_cast<btRigidBody*>( body.internal ) );
}

void nv::bullet_world::add_constraint( constraint cons )
{
	SELF->addConstraint( static_cast<btTypedConstraint*>( cons.internal ), true );
}

void nv::bullet_world::set_rigid_body_damping( rigid_body body, float linear, float angular )
{
	btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
	rb->setDamping( linear, angular );
}

void nv::bullet_world::set_rigid_body_deactivation_time( rigid_body body, float time )
{
	btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
	rb->setDeactivationTime( time );
}

void nv::bullet_world::set_rigid_body_activation_state( rigid_body body, activation_state state )
{
	btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
	rb->setActivationState( state );
}

void nv::bullet_world::set_rigid_body_sleeping_thresholds( rigid_body body, float linear, float angular )
{
	btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
	rb->setSleepingThresholds( linear, angular );
}

void nv::bullet_world::set_rigid_body_linear_velocity( rigid_body body, const vec3& velocity )
{
	btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
	rb->setLinearVelocity( n2b( velocity ) );
}

void nv::bullet_world::set_rigid_body_ccd( rigid_body body, float radius, float threshold )
{
	btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
	rb->setCcdMotionThreshold( threshold );
	rb->setCcdSweptSphereRadius( radius );
}

nv::transform nv::bullet_world::get_world_transform( rigid_body body )
{
	btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
	return static_cast<motion_state*>( rb->getMotionState() )->get_world_transform();
}

void nv::bullet_world::set_world_transform( rigid_body body, const nv::transform& tr )
{
	btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
	static_cast<motion_state*>( rb->getMotionState() )->set_world_transform( tr );
}

int nv::bullet_world::get_collision_flags( rigid_body body )
{
	btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
	return rb->getCollisionFlags();
}

void nv::bullet_world::set_collision_flags( rigid_body body, int flags )
{
	btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
	return rb->setCollisionFlags( flags );
}

class ClosestStaticRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
{
public:
	ClosestStaticRayResultCallback( const btVector3& from, const btVector3&  to ) : btCollisionWorld::ClosestRayResultCallback( from, to ) {}

	virtual btScalar addSingleResult( btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace )
	{
		if ( !rayResult.m_collisionObject->isStaticObject() )
			return 1.0;

		return ClosestRayResultCallback::addSingleResult( rayResult, normalInWorldSpace );
	}
};


bool nv::bullet_world::ray_test( const vec3& from, const vec3& to, vec3& hpos, vec3& hnorm, bool static_only )
{
	btVector3 bfrom( n2b( from ) );
	btVector3 bto( n2b( to ) );
	if ( !static_only )
	{
		btCollisionWorld::ClosestRayResultCallback raycb( bfrom, bto );
		SELF->rayTest( bfrom, bto, raycb );
		bool result = false;
		if ( raycb.hasHit() )
		{
			result = true;
			hpos = b2n( raycb.m_hitPointWorld );
			hnorm = b2n( raycb.m_hitNormalWorld );
		}
		return result;
	}
	else
	{
		ClosestStaticRayResultCallback raycb( bfrom, bto );
		SELF->rayTest( bfrom, bto, raycb );
		bool result = false;
		if ( raycb.hasHit() )
		{
			result = true;
			hpos = b2n( raycb.m_hitPointWorld );
			hnorm = b2n( raycb.m_hitNormalWorld );
		}
		return result;
	}
}

void bullet_world::release( collision_shape sh )
{
	if ( !sh.internal ) return;
	if ( sh.mesh )
	{
		btStridingMeshInterface* smi = static_cast<btStridingMeshInterface*>( sh.mesh );
		delete smi;
	}

	btCollisionShape* cs = static_cast<btCollisionShape*>( sh.internal );
	delete cs;

}

void bullet_world::release( constraint c )
{
	if ( !c.internal ) return;
	btTypedConstraint* tc = static_cast<btTypedConstraint*>( c.internal );
	SELF->removeConstraint( tc );
	delete tc;
}

void bullet_world::release( rigid_body body )
{
	if ( !body.internal ) return;
	btRigidBody* rb = static_cast<btRigidBody*>( body.internal );
	SELF->removeRigidBody( rb );
	delete rb;
}