#include "pch.h"
#include "Physics.h"

#include "audio/AudioMixer.h"
#include "Camera.h"
#include "EditorNew.h"
#include "Engine.h"
#include "WEntity.h"

bool ObjectLayerPairFilterImpl::ShouldCollide(JPH::ObjectLayer inObject1, JPH::ObjectLayer inObject2) const {
	switch (inObject1) {
		case Layers::NON_MOVING:
			return inObject2 == Layers::MOVING; // Non moving only collides with moving
		case Layers::MOVING:
			return true; // Moving collides with everything
		default:
			// JPH_ASSERT(false);
			return false;
	}
}

BPLayerInterfaceImpl::BPLayerInterfaceImpl() {
	// Create a mapping table from object to broad phase layer
	mObjectToBroadPhase[Layers::NON_MOVING] = BroadPhaseLayers::NON_MOVING;
	mObjectToBroadPhase[Layers::MOVING] = BroadPhaseLayers::MOVING;
}

JPH::uint BPLayerInterfaceImpl::GetNumBroadPhaseLayers() const {
	return BroadPhaseLayers::NUM_LAYERS;
}

JPH::BroadPhaseLayer BPLayerInterfaceImpl::GetBroadPhaseLayer(JPH::ObjectLayer inLayer) const {
	// JPH_ASSERT(inLayer < Layers::NUM_LAYERS);
	return mObjectToBroadPhase[inLayer];
}

#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED)
const char* BPLayerInterfaceImpl::GetBroadPhaseLayerName(JPH::BroadPhaseLayer inLayer) const {
	switch ((JPH::BroadPhaseLayer::Type)inLayer) {
		case (JPH::BroadPhaseLayer::Type)BroadPhaseLayers::NON_MOVING:
			return "NON_MOVING";
		case (JPH::BroadPhaseLayer::Type)BroadPhaseLayers::MOVING:
			return "MOVING";
		default:
			// JPH_ASSERT(false);
			return "INVALID";
	}
}
#endif

bool ObjectVsBroadPhaseLayerFilterImpl::ShouldCollide(JPH::ObjectLayer inLayer1, JPH::BroadPhaseLayer inLayer2) const {
	switch (inLayer1) {
		case Layers::NON_MOVING:
			return inLayer2 == BroadPhaseLayers::MOVING;
		case Layers::MOVING:
			return true;
		default:
			// JPH_ASSERT(false);
			return false;
	}
}

JPH::ValidateResult MyContactListener::OnContactValidate(const JPH::Body& inBody1, const JPH::Body& inBody2,
	JPH::RVec3Arg inBaseOffset, const JPH::CollideShapeResult& inCollisionResult) {
	// std::cout << "Contact validate callback" << std::endl;

	// Allows you to ignore a contact before it is created (using layers to not make objects collide is cheaper!)
	return JPH::ValidateResult::AcceptAllContactsForThisBodyPair;
}

void MyContactListener::OnContactAdded(const JPH::Body& inBody1, const JPH::Body& inBody2,
	const JPH::ContactManifold& inManifold, JPH::ContactSettings& ioSettings) {

	WEntity* me = WEngine->body_to_game_object[inBody1.GetID()];
	WEntity* other = WEngine->body_to_game_object[inBody2.GetID()];


	JPH::CollisionEstimationResult result;
	EstimateCollisionResponse(inBody1, inBody2, inManifold, result, ioSettings.mCombinedFriction, ioSettings.mCombinedRestitution);
	// std::cout <<
	// if(ioSettings.mCombinedRestitution > 0.01f) {
	// 	WEngine->editor->send_notification_message( " ------------------ AMOGUS" );
	// 	WEngine->editor->send_notification_message( std::to_string(ioSettings.mCombinedRestitution) );
	// }

	glm::vec3 contact_pos = glm::from_jph(inManifold.GetWorldSpaceContactPointOn1(0));


	if (me != nullptr && other != nullptr) {
		// TODO: delete this if()?
		if (WEngine->body_to_game_object.contains(inBody1.GetID()) && WEngine->body_to_game_object.contains(inBody2.GetID())){
			WCollision col_a = WCollision{me, other, result, contact_pos};
			WCollision col_b = WCollision{other, me, result, contact_pos};

			WEngine->collisions_mutex.lock();
			WEngine->collisions.push_back(col_a);
			// me->temp_collisions.add(&WEngine->collisions.back());
			WEngine->collisions.push_back(col_b);
			// other->temp_collisions.add(&WEngine->collisions.back());
			WEngine->collisions_mutex.unlock();
		}
	}
	// 	me->on_collision(*me,other);
	// }
}

void MyContactListener::OnContactPersisted(const JPH::Body& inBody1, const JPH::Body& inBody2,
	const JPH::ContactManifold& inManifold, JPH::ContactSettings& ioSettings) {
}

void MyContactListener::OnContactRemoved(const JPH::SubShapeIDPair& inSubShapePair) {
	// std::cout << "A contact was removed" << std::endl;
}

void MyBodyActivationListener::OnBodyActivated(const JPH::BodyID& inBodyID, JPH::uint64 inBodyUserData) {
	// std::cout << "A body got activated" << std::endl;
}

void MyBodyActivationListener::OnBodyDeactivated(const JPH::BodyID& inBodyID, JPH::uint64 inBodyUserData) {
	// std::cout << "A body went to sleep" << std::endl;
}

void Physics::set_up_ray_cast(JPH::RRayCast& r_ray_cast, JPH::RayCastSettings& ray_cast_settings, glm::vec3 origin,
	glm::vec3 dir) {
	// r_ray_cast.mDirection = glm::vec3(dir.GetX(), dir.GetY(), dir.GetZ());
	r_ray_cast.mDirection = JPH::Vec3(dir.x, dir.y, dir.z);
	r_ray_cast.mOrigin = JPH::Vec3(origin.x, origin.y, origin.z);

	ray_cast_settings.mBackFaceMode = JPH::EBackFaceMode::CollideWithBackFaces;
	ray_cast_settings.mTreatConvexAsSolid = true;
}

void Physics::tick() {
	// ------------- Physics state ------------ //
	// Next step
	++step;
	// If you take larger steps than 1 / 60th of a second you need to do multiple collision steps in order to keep the simulation stable. Do 1 collision step per 1 / 60th of a second (round up).
	const int cCollisionSteps = 1;

	{
		ZoneScopedN("Physics update");
		// Step the world
		WEngine->physics.physics_system->Update(
			WEngine->delta_time < 1.0/60.0 ? WEngine->delta_time : 1.0/60.0, cCollisionSteps, this->temp_allocator, this->job_system);
	}

	{
		ZoneScopedN("Collisions callback update");
		int col_idx = 0;

		static smallvec<WEntity*, 1000> col_entitties;
		col_entitties.resize(0);

		static std::array<float, 1000> forces;
		static std::array<glm::vec3, 1000> hit_positions;

		auto contains = [](auto arr, auto elem) -> int {
			int i = 0;
			for(auto& other : arr){
				if(other == elem) {
					return i;
				}
				i++;
			}
			return -1;
		};

		for (WCollision collision : WEngine->collisions) {
			// collision.me->on_collision(*collision.me, collision.other, collision.collision_res);

			int arr_idx = contains(col_entitties, collision.me);

			if(arr_idx < 0) {
				col_entitties.push_back(collision.me);
				arr_idx = col_entitties.size() - 1;
				forces[arr_idx] = 0.0f;
				hit_positions[arr_idx] = glm::vec3(0,0,0);
			}

			hit_positions[arr_idx] = collision.pos;

			for(JPH::CollisionEstimationResult::Impulse impulse : collision.collision_res.mImpulses) {
				float delta_vel = impulse.mContactImpulse/collision.me->collision_shape->GetMassProperties().mMass;
				forces[arr_idx] +=delta_vel;
			}

		}
		int i = 0;
		for (WEntity* entitty : col_entitties) {
			entitty->col_force = forces[i];

			if(entitty->col_sound.has_value()) {
				AudioFile* col_sound = entitty->col_sound.value();

				float len_to_player = glm::length(WEngine->camera->pos - entitty->get_pos());

				forces[i] *= 1000.0f;
				forces[i] = glm::abs(forces[i]);
				SIMDString<> msg;
				msg = "Force - ";
				msg += std::to_string(forces[i]);


				// float volume = forces[i]*1.0f;
				float volume = forces[i]*1.0f;

				if(forces[i]> 0.2f) {
					volume = glm::min(volume, 1.0f);

					WEngine->editor->gui_dbg_print_msg(msg.c_str());

					float att = 1.0f - 1.0f/(1.0f + len_to_player*2.5f);
					att *= 1.0f - 1.0f/(1.+volume);

					volume *= att;

					// col_sound->start({-1, volume, entitty->get_pos()});
					col_sound->start({-1, volume, hit_positions[i]});

				}

			}
			i++;
		}

		for (const WCollision& collision : WEngine->collisions) {
			// collision.me->on_collision(*collision.me, collision.other, collision.collision_res);
			if(collision.me->on_collision) {
				collision.me->on_collision(collision);
			}
		}
	}
	WEngine->collisions.resize(0);
}

RayCastResClosest Physics::shape_cast_closest(JPH::Ref<JPH::Shape> shape, glm::vec3 origin,
	JPH::BodyFilter& inBodyFilter, JPH::BroadPhaseLayerFilter inBroadPhaseLayerFilter,
	JPH::ObjectLayerFilter inObjectLayerFilter, JPH::ShapeFilter inShapeFilter) {

	// RayCastResClosest res;

	// const JPH::Vec3 cast_motion = JPH::Vec3(0, -15, 0);
	const JPH::Vec3 cast_motion = JPH::Vec3(0, 0, 0);
	JPH::RShapeCast r_shape_cast = JPH::RShapeCast{
		shape,
		JPH::Vec3::sReplicate(1),
		// JPH::RMat44::sTranslation(glm::to_jph(origin)),
		JPH::RMat44::sIdentity(),
		cast_motion
	};

	JPH::ShapeCastSettings settings = {
		.mBackFaceModeTriangles = JPH::EBackFaceMode::CollideWithBackFaces,
	};

	JPH::ClosestHitCollisionCollector<JPH::CastShapeCollector> collector;

	physics_system->GetNarrowPhaseQuery().CastShape(
		r_shape_cast, settings,
		// JPH::Vec3(0,0,0),
		glm::to_jph(origin),
		collector,
		{}, {}, inBodyFilter
	);
	RayCastResClosest res;
	if(collector.HadHit()) {
		JPH::ShapeCastResult hit = collector.mHit;

		// r_shape_cast.
		JPH::RVec3 hit_pos = r_shape_cast.GetPointOnRay(collector.mHit.mFraction);
		glm::vec3 hit_p_1 = glm::from_jph(hit.mContactPointOn1);
		glm::vec3 hit_p_2 = glm::from_jph(hit.mContactPointOn1);



		res.hit = true;
		res.body_id = hit.mBodyID2;
		res.pos = glm::from_jph(hit_pos);
		// res.dist = glm::length(dir) * collector.mHit.mFraction;
		res.dist = collector.mHit.mFraction;


	}
	return res;
}

RayCastResClosest Physics::RayCastClosest(glm::vec3 origin, glm::vec3 dir, JPH::BodyFilter& inBodyFilter,
	JPH::BroadPhaseLayerFilter inBroadPhaseLayerFilter, JPH::ObjectLayerFilter inObjectLayerFilter,
	JPH::ShapeFilter inShapeFilter) {
	JPH::RRayCast r_ray_cast;
	JPH::RayCastSettings ray_cast_settings;

	JPH::ClosestHitCollisionCollector<JPH::CastRayCollector> collector;

	set_up_ray_cast(r_ray_cast, ray_cast_settings, origin, dir);

	this->physics_system->GetNarrowPhaseQuery().CastRay(
		r_ray_cast, ray_cast_settings, collector,
		inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter
	);
	RayCastResClosest res;

	if (collector.HadHit()) {
		JPH::RayCastResult hit = collector.mHit;
		JPH::RVec3 hit_pos = r_ray_cast.GetPointOnRay(collector.mHit.mFraction);

		res.hit = true;
		res.body_id = hit.mBodyID;
		res.pos = glm::vec3(hit_pos.GetX(), hit_pos.GetY(), hit_pos.GetZ());
		res.dist = glm::length(dir) * collector.mHit.mFraction;

		JPH::BodyLockRead lock(this->physics_system->GetBodyLockInterface(), hit.mBodyID);
		if (lock.Succeeded()) {
			const JPH::Body& hit_body = lock.GetBody();
			JPH::Vec3 normal = hit_body.GetWorldSpaceSurfaceNormal(hit.mSubShapeID2, hit_pos);
			res.norm = glm::vec3(normal.GetX(), normal.GetY(), normal.GetZ());
		}
	}
	return res;
}

// extern JPH::AllocateFunction Allocate;
// extern JPH::FreeFunction Free;
// extern JPH::AlignedAllocateFunction AlignedAllocate;
// extern JPH::AlignedFreeFunction AlignedFree;

Physics::Physics() {
	// std::cout << Allocate;
	// std::cout << '\n';
	// std::cout << Free;
	// std::cout << '\n';
	// std::cout << AlignedAllocate;
	// std::cout << '\n';
	// std::cout << AlignedFree;
	// std::cout << '\n';

	// Install callbacks
	// Trace = TraceImpl;
	// JPH_IF_ENABLE_ASSERTS(AssertFailed = AssertFailedImpl;)

	// Create a factory
	JPH::Factory::sInstance = new JPH::Factory();

	// Register all Jolt physics types
	JPH::RegisterTypes();

	// B.t.w. 10 MB is way too much for this example but it is a typical value you can use.
	temp_allocator = new JPH::TempAllocatorImpl(10 * 1024 * 1024);

	// We need a job system that will execute physics jobs on multiple threads. Typically you would implement the JobSystem interface yourself.
	job_system = new JPH::JobSystemThreadPool(JPH::cMaxPhysicsJobs, JPH::cMaxPhysicsBarriers,
		std::thread::hardware_concurrency() - 1);

	const JPH::uint cMaxBodies = 65536;

	// 0 for the default settings.
	const JPH::uint cNumBodyMutexes = 0;

	// For a real project use something in the order of 65536.
	const JPH::uint cMaxBodyPairs = 65536;

	// For a real project use something in the order of 10240.
	const JPH::uint cMaxContactConstraints = 10240;

	// Create mapping table from object layer to broadphase layer
	// Note: As this is an interface, PhysicsSystem will take a reference to this so this instance needs to stay alive!
	// broad_phase_layer_interface = BPLayerInterfaceImpl();

	// Create class that filters object vs broadphase layers
	// Note: As this is an interface, PhysicsSystem will take a reference to this so this instance needs to stay alive!
	// object_vs_broadphase_layer_filter = ObjectVsBroadPhaseLayerFilterImpl();

	// Create class that filters object vs object layers
	// Note: As this is an interface, PhysicsSystem will take a reference to this so this instance needs to stay alive!
	// object_vs_object_layer_filter = ObjectLayerPairFilterImpl();

	// Now we can create the actual physics system.
	physics_system = new JPH::PhysicsSystem();
	physics_system->Init(cMaxBodies, cNumBodyMutexes, cMaxBodyPairs, cMaxContactConstraints, broad_phase_layer_interface,
		object_vs_broadphase_layer_filter, object_vs_object_layer_filter);


	// A body activation listener gets notified when bodies activate and go to sleep
	// Note that this is called from a job so whatever you do here needs to be thread safe.
	// Registering one is entirely optional.
	body_activation_listener = new MyBodyActivationListener();
	physics_system->SetBodyActivationListener(body_activation_listener);

	// A contact listener gets notified when bodies (are about to) collide, and when they separate again.
	// Note that this is called from a job so whatever you do here needs to be thread safe.
	// Registering one is entirely optional.
	contact_listener = new MyContactListener();
	physics_system->SetContactListener(contact_listener);

	// The main way to interact with the bodies in the physics system is through the body interface. There is a locking and a non-locking
	// variant of this. We're going to use the locking version (even though we're not planning to access bodies from multiple threads)
	// BodyInterface &body_interface = physics_system.GetBodyInterface();
	body_interface = &physics_system->GetBodyInterface();
}
