#include <uf/utils/math/physics/impl.h>

#include <uf/utils/math/physics/common.h>
#include <uf/utils/math/physics/narrowphase.h>
#include <uf/utils/math/physics/broadphase.h>
#include <uf/utils/math/physics/integration.h>
#include <uf/utils/math/physics/solvers.h>
#include <uf/utils/math/physics/constraints.h>

#include <uf/engine/scene/scene.h>
#include <uf/engine/graph/graph.h>

#define UF_PHYSICS_TEST 0

pod::PhysicsSettings uf::physics::settings;

// Bindings
void uf::physics::initialize() {
	uf::physics::initialize( uf::scene::getCurrentScene() );
}
void uf::physics::initialize( uf::Object& object ) {
	uf::physics::initialize( object.getComponent<pod::World>() );
}
void uf::physics::initialize( pod::World& world ) {
	// to-do: any additional initialization (since it's not needed right now)
}
void uf::physics::tick() {
	uf::physics::tick( uf::scene::getCurrentScene() );
}
void uf::physics::tick( uf::Object& scene ) {
	uf::physics::time::previous = uf::physics::time::current;
	uf::physics::time::current = uf::physics::time::timer.elapsed();

	uf::physics::time::delta = uf::physics::time::current - uf::physics::time::previous;
	if ( uf::physics::time::delta > uf::physics::time::clamp ) {
		uf::physics::time::delta = uf::physics::time::clamp;
	}

	if ( uf::physics::settings.async ) {
		uf::thread::queue( "Physics", [&](){ uf::physics::tick( scene, uf::physics::time::delta ); });	
	} else {
		uf::physics::tick( scene, uf::physics::time::delta );
	}
}
void uf::physics::tick( float dt ) {
	uf::physics::tick( uf::scene::getCurrentScene(), dt );
}
void uf::physics::tick( uf::Object& object, float dt ) {
	uf::physics::tick( object.getComponent<pod::World>(), dt );
}
void uf::physics::tick( pod::World& world, float dt ) {
	if ( !uf::physics::settings.fixedStep ) {
		if ( uf::physics::settings.substeps > 0 ) uf::physics::substep( world, dt, uf::physics::settings.substeps );
		else uf::physics::step( world, dt );

		return;
	}

	static float accumulator = 0;
	accumulator += dt; 

	float timestep = uf::physics::settings.timestep;

	while ( accumulator >= timestep ) { 
		if ( uf::physics::settings.substeps > 0 ) uf::physics::substep( world, timestep, uf::physics::settings.substeps ); 
		else uf::physics::step( world, timestep ); 
		accumulator -= timestep; 
	}

	if ( uf::physics::settings.debugDraw.mask != pod::Collider::CATEGORY_NONE ) impl::draw( world, dt );
}
void uf::physics::terminate() {
	uf::physics::terminate( uf::scene::getCurrentScene() );
}
void uf::physics::terminate( uf::Object& object ) {
	uf::physics::terminate( object.getComponent<pod::World>() );
}
void uf::physics::terminate( pod::World& world ) {
	world.bodies.clear();
}

// Implementation
void uf::physics::substep( pod::World& world, float dt, int32_t substeps ) {
	float h = dt / substeps;
	for ( auto i = 0; i < substeps; ++i ) {
		uf::physics::step( world, h );
	}
}
void uf::physics::step( pod::World& world, float dt ) {
	auto& bodies = world.bodies;
	auto& constraints = world.constraints;
	auto& dynamicBvh = world.dynamicBvh;
	auto& staticBvh = world.staticBvh;
	auto& activeCollisions = world.activeCollisions;
	auto& collisionEvents = world.collisionEvents;

	STATIC_THREAD_LOCAL(pod::CollisionEvent::events_t, previousCollisionEvents);
	STATIC_THREAD_LOCAL(pod::CollisionEvent::map_t, previousCollisions);
	
	std::swap( previousCollisions, activeCollisions );
	std::swap( previousCollisionEvents, collisionEvents );

	if ( bodies.empty() ) return;

	++uf::physics::settings.frameCounter;

	// flatten all transforms into a contiguous buffer
	static thread_local uf::stl::vector<pod::Transform<>*> originalTransforms; // stores the pointer to the original transform
	static thread_local uf::stl::vector<pod::Transform<>> flattenedTransforms; // stores the flattened transforms
	static thread_local uf::stl::unordered_map<pod::Transform<>*, pod::Transform<>*> transformsMap; // maps the original transform to the flattened one (for querying)

	if ( uf::physics::settings.flattenTransforms ) {
		originalTransforms.resize( bodies.size() );
		flattenedTransforms.resize( bodies.size() );
		transformsMap.clear();

		// copy all transforms, flatten them, and map the original to the reference
		for ( auto i = 0; i < bodies.size(); ++i ) {
			auto& body = *bodies[i];
			//if ( body.inverseMass == 0.0f ) continue; // static bodies don't ever move, so the hierarchy problem doesn't affect it (right?)
			auto& transform = *body.transform;
			auto& flattenedTransform = flattenedTransforms[i];
			auto& originalTransform = originalTransforms[i];

			originalTransform = body.transform;
			flattenedTransform = uf::transform::flatten( transform );		
			transformsMap[originalTransform] = &flattenedTransform;
			body.transform = &flattenedTransform; // body now points to the flattened transform
		}
	}

	for ( auto* body : bodies ) {
		impl::integrate( *body, dt );
	}

	// rebuild static bvh if dirty
	if ( staticBvh.dirty && uf::physics::settings.useSplitBvhs ) {
		impl::buildBroadphaseBVH( staticBvh, bodies, uf::physics::settings.broadphaseBvhCapacity, uf::physics::settings.useSplitBvhs, true ); // (re)build
	}

	switch ( impl::decideBVHUpdate( dynamicBvh, bodies, uf::physics::settings.bvhUpdatePolicy, uf::physics::settings.frameCounter ) ) {
		case pod::BVH::UpdatePolicy::Decision::REBUILD: {
			impl::buildBroadphaseBVH( dynamicBvh, bodies, uf::physics::settings.broadphaseBvhCapacity, uf::physics::settings.useSplitBvhs, false ); // (re)build
		} break;
		case pod::BVH::UpdatePolicy::Decision::REFIT: {
			impl::refitBVH( dynamicBvh, bodies ); // refit
		} break;
		case pod::BVH::UpdatePolicy::Decision::NONE:
		default: {
			// no-op
		} break;
	}

	// query for overlaps
	pod::BVH::pairs_t pairs;
	impl::queryOverlaps( dynamicBvh, pairs );
	if ( uf::physics::settings.useSplitBvhs ) {
		impl::queryOverlaps( dynamicBvh, staticBvh, pairs );
	}

	// build islands from overlaps
	STATIC_THREAD_LOCAL(uf::stl::vector<pod::Island>, islands);
	impl::buildIslands( pairs, bodies, constraints, islands );

	if ( uf::physics::settings.warmupSolver ) impl::prepareManifoldCache( uf::physics::settings.manifoldsCache, islands, bodies );

	// iterate islands
	//#pragma omp parallel for schedule(dynamic)
	auto tasks = uf::thread::schedule(true);
	for ( auto& island : islands ) tasks.queue([&]{
		auto& manifolds = island.manifolds;
		auto& constraints = island.constraints;
		manifolds.clear();

		// sleeping island, skip (asleep islands shouldn't ever be in here)
		if ( !island.awake ) return;

		// pairs of bodies that shouldn't ever collide
		uf::stl::unordered_set<size_t> ignorePairs;
		// mark constraining objects as non-colliding (to-do: add a flag)
		for ( auto& constraint : constraints ) {
			ignorePairs.insert( impl::makePairKey( *constraint->a, *constraint->b ) );
		}

		// iterate overlap pairs
		for ( auto& [ ia, ib ] : island.pairs ) {
			auto& a = *bodies[ia];
			auto& b = *bodies[ib];
			auto pairKey = impl::makePairKey( a, b );

			pod::Manifold manifold;
			// marked as ignored
			if ( ignorePairs.find( pairKey ) != ignorePairs.end() ) continue;
			// did not collide
			if ( !impl::generateManifold( a, b, manifold, dt ) ) continue;

			// compute local points (for reprojection)
			impl::computeLocalManifold( manifold );

			// bodies with meshes already reorient the normal to the triangle's center
			bool shouldReorient = true;
			// do not do it for meshes because it'll reorient to the mesh's origin
			if ( a.collider.type == pod::ShapeType::MESH || b.collider.type == pod::ShapeType::MESH ) shouldReorient = false;
			// do not do it for planes
			//if ( a.collider.type == pod::ShapeType::PLANE || b.collider.type == pod::ShapeType::PLANE ) shouldReorient = false;
			if ( shouldReorient ) {
				for ( auto& c : manifold.points ) c.normal = impl::orientNormalToAB( a, b, c.normal );
			}
			// retrieve accumulated impulses
			if ( uf::physics::settings.warmupSolver ) {
				auto it = uf::physics::settings.manifoldsCache.find( pairKey );
				if ( it != uf::physics::settings.manifoldsCache.end() ) impl::retrieveManifold( manifold, it->second );
			}
			// merge similar contacts from a mesh to ensure continuity
			if ( a.collider.type == pod::ShapeType::MESH || b.collider.type == pod::ShapeType::MESH ) {
				impl::mergeManifold( manifold );
			}
			// keep four most important contacts
			impl::reduceManifold( manifold );
			// no points remained, skip
			if ( manifold.points.empty() ) continue;
			// wake up bodies
			if ( a.activity.awake && !b.activity.awake ) impl::wakeBody( b );
			if ( b.activity.awake && !a.activity.awake ) impl::wakeBody( a );
			// mark as grounded
			bool isTrigger = (a.collider.category & pod::Collider::CATEGORY_TRIGGER) || (b.collider.category & pod::Collider::CATEGORY_TRIGGER);
			if ( !isTrigger ) for ( auto& c : manifold.points ) {
				if ( std::fabs(uf::vector::dot(c.normal, pod::Vector3f{0,1,0})) > uf::physics::settings.groundedThreshold ) {
					// only mark if contact point is below body
					if ( c.point.y < impl::getPosition(a).y ) a.activity.grounded = true;
					if ( c.point.y < impl::getPosition(b).y ) b.activity.grounded = true;
				}
			}

			// store manifold
			manifolds.emplace_back( manifold );
		}
		// pass manifolds to solver
		impl::solveManifold( manifolds, dt );
		// do position correction
		impl::solvePositions( manifolds, dt );
		// solve constraints
		impl::solveConstraints( constraints, dt );
		// cache manifold positions
		if ( uf::physics::settings.warmupSolver ) impl::updateManifoldCache( manifolds, uf::physics::settings.manifoldsCache );
	});
	uf::thread::execute( tasks );

	// prune expired manifolds in the cache
	if ( uf::physics::settings.warmupSolver ) impl::pruneManifoldCache( uf::physics::settings.manifoldsCache );

	for ( auto& island : islands ) for ( auto& manifold : island.manifolds ) {
		// dispatch collision events
		impl::dispatchManifold( manifold, collisionEvents, activeCollisions, previousCollisions );
		// draw collision events
		if ( uf::physics::settings.debugDraw.contacts ) impl::drawManifold( manifold );
	}
	// dispatch exiting collisions
	for ( auto& [ key, pair ] : previousCollisions ) {
		// sustained collision
		if ( activeCollisions.count( key ) > 0 ) continue;
		// mark as exiting
		collisionEvents.emplace_back(pod::CollisionEvent{
			.state = pod::CollisionState::EXIT,
			.a = pair.first,
			.b = pair.second,
		});
	}

	// snap velocities of bodies
	for ( auto* b : bodies ) impl::snapVelocity( *b, dt );

	// unflatten all transforms
	if ( uf::physics::settings.flattenTransforms ) for ( auto i = 0; i < bodies.size(); ++i ) {
		auto& body = *bodies[i];
		//if ( body.inverseMass == 0.0f ) continue; // do not bother with static bodies
		auto& originalTransform = originalTransforms[i];
		auto& flattenedTransform = flattenedTransforms[i];
		
		// now points back to original transform
		body.transform = originalTransform;
		auto& transform = *body.transform;

		// solo transform: simply update
		if ( !transform.reference ) {
			// update position and orientation
			transform.position = flattenedTransform.position;
			transform.orientation = flattenedTransform.orientation;
		// referential transform: convert new world space into local space (to original transform)
		} else {
			pod::Transform<> parentTransform;
			// parent is not within the physics system, use original reference
			if ( transformsMap.count( originalTransform->reference ) == 0 ) {
				parentTransform = uf::transform::flatten( *originalTransform->reference );
			// parent is within the physics system, use the already-flattened transform
			} else {
				parentTransform = *transformsMap[originalTransform->reference];
			}

			transform.position = impl::applyInverse( parentTransform, flattenedTransform.position );
			transform.orientation = uf::quaternion::multiply(
				uf::quaternion::inverse( parentTransform.orientation ),
				flattenedTransform.orientation
			);
		}
	}
}

void uf::physics::setMass( pod::PhysicsBody& body, float mass ) {
	body.inverseMass = 1.0f / mass;
	uf::physics::updateInertia( body );
}

void uf::physics::setColliderCategory( pod::PhysicsBody& body, uint32_t category ) {
	body.collider.category = category;
}
void uf::physics::setColliderCategory( pod::PhysicsBody& body, const uf::stl::string& category ) {
	auto c = uf::string::uppercase( category );
	if ( c == "NONE" ) return uf::physics::setColliderCategory( body, pod::Collider::CATEGORY_NONE );
	if ( c == "STATIC" ) return uf::physics::setColliderCategory( body, pod::Collider::CATEGORY_STATIC );
	if ( c == "DYNAMIC" ) return uf::physics::setColliderCategory( body, pod::Collider::CATEGORY_DYNAMIC );
	if ( c == "PLAYER" ) return uf::physics::setColliderCategory( body, pod::Collider::CATEGORY_PLAYER );
	if ( c == "NPC" ) return uf::physics::setColliderCategory( body, pod::Collider::CATEGORY_NPC );
	if ( c == "TRIGGER" ) return uf::physics::setColliderCategory( body, pod::Collider::CATEGORY_TRIGGER );
	if ( c == "PROJECTILE" ) return uf::physics::setColliderCategory( body, pod::Collider::CATEGORY_PROJECTILE );
	if ( c == "CHARACTER" ) return uf::physics::setColliderCategory( body, pod::Collider::CATEGORY_CHARACTER );
	if ( c == "ALL" ) return uf::physics::setColliderCategory( body, pod::Collider::CATEGORY_ALL );
}
void uf::physics::setColliderMask( pod::PhysicsBody& body, uint32_t mask ) {
	body.collider.mask = mask;
}
void uf::physics::setColliderMask( pod::PhysicsBody& body, const uf::stl::string& mask ) {
	auto m = uf::string::uppercase( mask );
	if ( m == "NONE" ) return uf::physics::setColliderMask( body, pod::Collider::MASK_NONE );
	if ( m == "STATIC" ) return uf::physics::setColliderMask( body, pod::Collider::MASK_STATIC );
	if ( m == "DYNAMIC" ) return uf::physics::setColliderMask( body, pod::Collider::MASK_DYNAMIC );
	if ( m == "PLAYER" ) return uf::physics::setColliderMask( body, pod::Collider::MASK_PLAYER );
	if ( m == "NPC" ) return uf::physics::setColliderMask( body, pod::Collider::MASK_NPC );
	if ( m == "TRIGGER" ) return uf::physics::setColliderMask( body, pod::Collider::MASK_TRIGGER );
	if ( m == "PROJECTILE" ) return uf::physics::setColliderMask( body, pod::Collider::MASK_PROJECTILE );
	if ( m == "CHARACTER" ) return uf::physics::setColliderMask( body, pod::Collider::MASK_CHARACTER );
	if ( m == "ALL" ) return uf::physics::setColliderMask( body, pod::Collider::MASK_ALL );
}
void uf::physics::setGravity( pod::PhysicsBody& body, const pod::Vector3f& gravity ) {
	body.gravity = gravity;
}
pod::Vector3f uf::physics::getGravity( pod::PhysicsBody& body ) {
	return uf::vector::isValid( body.gravity ) ? body.gravity : body.world->gravity;
}

void uf::physics::updateInertia( pod::PhysicsBody& body ) {
	if ( body.inverseMass == 0.0f ) {
		body.inverseInertiaTensor = { 0.0f, 0.0f, 0.0f };
		return;
	}

	float mass = 1.0f / body.inverseMass;
	pod::Vector3f inertiaTensor = {};
	switch ( body.collider.type ) {
		case pod::ShapeType::AABB: {
			pod::Vector3f dims = (body.collider.aabb.max - body.collider.aabb.min);
			pod::Vector3f dimsSq = dims * dims;
			inertiaTensor = pod::Vector3f{ dimsSq.y + dimsSq.z, dimsSq.x + dimsSq.z, dimsSq.x + dimsSq.y } * (mass / 12.0f);
			inertiaTensor = uf::vector::max( inertiaTensor, { EPS, EPS, EPS } );
			body.inverseInertiaTensor = 1.0f / inertiaTensor;
		} break;
		case pod::ShapeType::OBB: {
			pod::Vector3f dims = body.collider.obb.extent * 2.0f;
			pod::Vector3f dimsSq = dims * dims;
			inertiaTensor = pod::Vector3f{ dimsSq.y + dimsSq.z, dimsSq.x + dimsSq.z, dimsSq.x + dimsSq.y } * (mass / 12.0f);
			inertiaTensor = uf::vector::max( inertiaTensor, { EPS, EPS, EPS } );
			body.inverseInertiaTensor = 1.0f / inertiaTensor;
		} break;
		case pod::ShapeType::SPHERE: {
			float I = 0.4f * mass * body.collider.sphere.radius * body.collider.sphere.radius;
			float invI = 1.0f / I;
			inertiaTensor = { I, I, I };
			body.inverseInertiaTensor = { invI, invI, invI };
		} break;
		case pod::ShapeType::CAPSULE: {
			float r = body.collider.capsule.radius;
			float h = uf::vector::norm( body.collider.capsule.up ) * 2.0f; // full cyl height

			float Ixx = 0.25f * mass * r * r + (1.0f/12.0f) * mass * h * h;
			float Iyy = 0.5f * mass * r * r;
			float Izz = Ixx;

			inertiaTensor = { Ixx, Iyy, Izz };
			body.inverseInertiaTensor = { 1.0f/Ixx, 1.0f/Iyy, 1.0f/Izz };
		} break;
		case pod::ShapeType::MESH:
		case pod::ShapeType::CONVEX_HULL: {
			const auto& bvh = *body.collider.mesh.bvh;

		#if 1
			pod::Vector3f dims = (body.bounds.max - body.bounds.min);
			pod::Vector3f dimsSq = dims * dims;
			inertiaTensor = pod::Vector3f{ dimsSq.y + dimsSq.z, dimsSq.x + dimsSq.z, dimsSq.x + dimsSq.y } * (mass / 12.0f);
			inertiaTensor = uf::vector::max( inertiaTensor, { EPS, EPS, EPS } );
			body.inverseInertiaTensor = 1.0f / inertiaTensor;
		#else
			pod::Matrix3f inertia = {};
			float totalVolume = 0.0f;

			// compute total volume
			for ( size_t i = 0; i < bvh.nodes.size(); ++i ) {
				if ( bvh.nodes[i].getCount() == 0 ) continue;
				const auto& box = bvh.bounds[i];

				auto extents = box.max - box.min;
				totalVolume += extents.x * extents.y * extents.z;
			}

			if ( totalVolume < EPS ) {
				inertiaTensor = { FLT_MAX, FLT_MAX, FLT_MAX };
				body.inverseInertiaTensor = { 0.0f, 0.0f, 0.0f };
			} else {
				// accumulate inertia
				for ( size_t i = 0; i < bvh.nodes.size(); ++i ) {
					if ( bvh.nodes[i].getCount() == 0 ) continue;
					const auto& box = bvh.bounds[i];

					auto extents = box.max - box.min;
					float mass = mass * extents.x * extents.y * extents.z / totalVolume;

					// inertia tensor of a box about its center
					float x2 = extents.x * extents.x;
					float y2 = extents.y * extents.y;
					float z2 = extents.z * extents.z;

					pod::Matrix3f Ibox;
					Ibox(0,0) = (1.0f/12.0f) * mass * (y2 + z2);
					Ibox(1,1) = (1.0f/12.0f) * mass * (x2 + z2);
					Ibox(2,2) = (1.0f/12.0f) * mass * (x2 + y2);

					// parallel axis theorem
					pod::Vector3f center = (box.min + box.max) * 0.5f;
					pod::Vector3f d = center; // relative to mesh COM (assume COM at origin for now)
					float dist2 = uf::vector::magnitude( d );

					pod::Matrix3f pat = uf::matrix::identityi<pod::Matrix3f>() * (mass * dist2);
					pat -= uf::matrix::outerProduct(d, d) * mass;

					inertia += Ibox + pat;
				}
				
				inertiaTensor = { inertia(0,0), inertia(1,1), inertia(2,2) };
				body.inverseInertiaTensor = 1.0f / inertiaTensor;
			}
		#endif
		} break;
		// to-do: add others
		default: {
		} break;
	}
}
void uf::physics::applyForce( pod::PhysicsBody& body, const pod::Vector3f& force ) {
	if ( body.inverseMass == 0.0f ) return;
	impl::wakeBody( body );
	body.forceAccumulator += force;
}
void uf::physics::applyForceAtPoint( pod::PhysicsBody& body, const pod::Vector3f& force, const pod::Vector3f& point ) {
	if ( body.inverseMass == 0.0f ) return;
	impl::wakeBody( body );
	// linear force
	body.forceAccumulator += force;
	// angular force
	pod::Vector3f r = point - impl::getPosition( body );
	body.torqueAccumulator += uf::vector::cross( r, force );
}
void uf::physics::applyImpulse( pod::PhysicsBody& body, const pod::Vector3f& impulse ) {
	if ( body.inverseMass == 0.0f ) return; impl::wakeBody( body );
	body.velocity += impulse * body.inverseMass;
}
void uf::physics::applyTorque( pod::PhysicsBody& body, const pod::Vector3f& torque ) {
	if ( body.inverseMass == 0.0f ) return; impl::wakeBody( body );
	body.torqueAccumulator += torque;
}
void uf::physics::setVelocity( pod::PhysicsBody& body, const pod::Vector3f& v ) {
	impl::wakeBody( body );
	if ( body.inverseMass == 0.0f ) {
		body.transform->position += v * uf::physics::time::delta;
	} else {
		body.velocity = v;
	}
}
void uf::physics::applyVelocity( pod::PhysicsBody& body, const pod::Vector3f& v ) {
	impl::wakeBody( body );
	body.velocity += v;
}
void uf::physics::setAngularVelocity( pod::PhysicsBody& body, const pod::Vector3f& v ) {
	impl::wakeBody( body );
	body.angularVelocity = v;
}
void uf::physics::setAngularVelocity( pod::PhysicsBody& body, const pod::Quaternion<>& q, float dt ) {
	if ( !dt ) dt = uf::physics::time::delta;
	float angle = 2.0f * std::acos( q.w );
	float sinHalfAngle = std::sqrt( 1.0f - q.w * q.w );

	pod::Vector3f axis{ 0, 0, 0 };
	if ( sinHalfAngle > EPS ) {
		axis.x = q.x / sinHalfAngle;
		axis.y = q.y / sinHalfAngle;
		axis.z = q.z / sinHalfAngle;
	}

	impl::wakeBody( body );
	body.angularVelocity = axis * ( angle / dt );
}
void uf::physics::applyAngularVelocity( pod::PhysicsBody& body, const pod::Vector3f& v ) {
	impl::wakeBody( body );
	body.angularVelocity += v;
}
void uf::physics::applyAngularVelocity( pod::PhysicsBody& body, const pod::Quaternion<>& q, float dt ) {
	if ( !dt ) dt = uf::physics::time::delta;
	float angle = 2.0f * std::acos( q.w );
	float sinHalfAngle = std::sqrt( 1.0f - q.w * q.w );

	pod::Vector3f axis{ 0, 0, 0 };
	if ( sinHalfAngle > EPS ) {
		axis.x = q.x / sinHalfAngle;
		axis.y = q.y / sinHalfAngle;
		axis.z = q.z / sinHalfAngle;
	}

	impl::wakeBody( body );
	body.angularVelocity += axis * ( angle / dt );
}
void uf::physics::applyRotation( pod::PhysicsBody& body, const pod::Quaternion<>& q ) {
	impl::wakeBody( body );
	uf::transform::rotate( *body.transform, q );
}
void uf::physics::applyRotation( pod::PhysicsBody& body, const pod::Vector3f& axis, float angle ) {
	uf::physics::applyRotation( body, uf::quaternion::axisAngle( axis, angle ) );
}
pod::World& uf::physics::getWorld() {
	auto& scene = uf::scene::getCurrentScene();
	return scene.getComponent<pod::World>();
}
const pod::CollisionEvent::events_t& uf::physics::getCollisionEvents( const pod::World& world ) {
	return world.collisionEvents;
}
pod::CollisionEvent::events_t uf::physics::getCollisionEvents( const pod::PhysicsBody& body ) {
	const auto& events = uf::physics::getCollisionEvents( *body.world );
	pod::CollisionEvent::events_t res;
	for ( auto& event : events ) {
		if ( event.a != &body && event.b != &body ) continue;
		res.emplace_back( event );
	}
	return res;
}
uf::stl::string uf::physics::getCollisionMaterialName( const pod::CollisionEvent& event ) {
	auto triID = MIN( event.featureA, event.featureB );
	auto* other = event.a->collider.type == pod::ShapeType::MESH ? event.a : event.b;
	return impl::getMaterialName( *other, triID );
}

// body creation
pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, float mass, const pod::Vector3f& positionOffset, const pod::Quaternion<>& orientationOffset ) {
	auto& root = object.getComponent<pod::PhysicsBody>();
	auto isRoot = !root.world;
	auto& body = isRoot ? root : *(new pod::PhysicsBody);
	// initial initialization
	body.world = &world;
	body.object = &object;
	body.transform = &object.getComponent<pod::Transform<>>();
	body.offsetPosition = positionOffset;
	body.offsetOrientation = orientationOffset;
	body.inverseMass = mass == 0.0f ? 0.0f : 1.0f / mass;
	body.collider.type = {};

	// append next in chain
	int children = 0;
	if ( !isRoot ) {
		auto* current = &root;
		while ( current->next ) {
			++children;
			current = current->next;
		}
		current->next = &body;
	}

	if ( body.inverseMass == 0.0f ) {
		uf::physics::setColliderCategory(body, "STATIC");
		uf::physics::setColliderMask(body, "STATIC");
		world.staticBvh.dirty = true; // mark as dirty
	} else {
		uf::physics::setColliderCategory(body, "DYNAMIC");
		uf::physics::setColliderMask(body, "DYNAMIC");
		world.dynamicBvh.dirty = true; // mark as dirty
	}

	// insert into world if not already inserted
	if ( std::find( world.bodies.begin(), world.bodies.end(), &body ) == world.bodies.end() ) {
		world.bodies.emplace_back( &body );
	}

	return body;
}

pod::PhysicsBody& uf::physics::create( uf::Object& object, float mass, const pod::Vector3f& positionOffset, const pod::Quaternion<>& orientationOffset ) {
	return create( getWorld(), object, mass, positionOffset, orientationOffset );
}

void uf::physics::destroy( uf::Object& object ) {
	if ( !object.hasComponent<pod::PhysicsBody>() ) return;
	
	auto& root = object.getComponent<pod::PhysicsBody>();
	auto* current = &root;
	while ( current != NULL ) {
		auto* next = current->next;
	   	uf::physics::destroy( *current );
		if ( current != &root ) delete current;
		current = next;
	}
}
void uf::physics::destroy( pod::PhysicsBody& body ) {
	auto& world = *body.world;

	uf::physics::unconstrain( body );

	// remove from world
	for ( auto it = world.bodies.begin(); it != world.bodies.end(); ++it ) {
		if ( (*it)->object != body.object ) continue;
		world.bodies.erase(it);
		break;
	}

	// remove any pointered collider data
	if ( body.collider.type == pod::ShapeType::MESH ) {
		if ( body.collider.mesh.bvh ) delete body.collider.mesh.bvh;
		body.collider.mesh.bvh = NULL;
		body.collider.mesh.mesh = NULL;
	}
	if ( body.collider.type == pod::ShapeType::CONVEX_HULL ) {
		if ( body.collider.convexHull.bvh ) delete body.collider.convexHull.bvh;
		body.collider.convexHull.bvh = NULL;
		body.collider.convexHull.mesh = NULL;
	}
}

#if UF_PHYSICS_TEST
	#include "tests.inl"
#endif