#include "dl_physics.h"

#include <cstdarg>

//#define JPH_DEBUG_RENDERER
#include <Jolt/Jolt.h>

#include <Jolt/RegisterTypes.h>
#include <Jolt/Core/Factory.h>
#include <Jolt/Core/TempAllocator.h>
#include <Jolt/Core/JobSystemThreadPool.h>
#include <Jolt/Physics/PhysicsSettings.h>
#include <Jolt/Physics/PhysicsSystem.h>
#include <Jolt/Physics/Collision/Shape/BoxShape.h>
#include <Jolt/Physics/Collision/Shape/SphereShape.h>
#include <Jolt/Physics/Body/BodyCreationSettings.h>
#include <Jolt/Physics/Body/BodyActivationListener.h>
#include <Jolt/Physics/Body/Body.h>
#include <Jolt/Renderer/DebugRendererSimple.h>
#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>

#include "dl_rigid_body.h"
#include "base/dl_color.h"
#include "render/dl_graphics.h"

namespace dl
{

Physics* g_physics;

using namespace JPH;

// joltPhysic 是y向上的右手坐标系（只修改重力方向应该也没有问题）
static RVec3 to_rvec3(const Position3& pos)
{
	return RVec3(pos.x, pos.y, pos.z);
}
static Position3 to_pos3(const RVec3& pos)
{
	return { pos.GetX(), pos.GetY(), pos.GetZ() };
}

struct RigidBodyShape
{
	// 不同缩放使用不同形状
	std::unordered_map<uint64_t, JPH::ShapeRefC> _mapRef;
};


// Callback for traces, connect this to your own trace function if you have one
static void TraceImpl(const char* inFMT, ...)
{
	// Format the message
	va_list list;
	va_start(list, inFMT);
	char buffer[1024];
	vsnprintf(buffer, sizeof(buffer), inFMT, list);
	va_end(list);

	// Print to the TTY
	log_warn("JPH: {}", buffer);
}

#ifdef JPH_ENABLE_ASSERTS

// Callback for asserts, connect this to your own assert handler if you have one
static bool AssertFailedImpl(const char* inExpression, const char* inMessage, const char* inFile, uint inLine)
{
	log_err("JPH Assert: {}[{}], {}, {}", inFile, inLine, inExpression, inMessage);

	return true;
};

#endif // JPH_ENABLE_ASSERTS

// 物体所属的“层/分组”，决定它能和**谁碰撞、谁不碰撞**
// 至少需要：静态层 + 移动层
// 你也可以加更多：玩家层、敌人层、子弹层、触发器层...
namespace Layers
{
	// 静态物体：地面、墙、房子、石头（永远不动）
	static constexpr ObjectLayer NON_MOVING = 0;

	// 移动物体：角色、箱子、球、怪物（会动的）
	static constexpr ObjectLayer MOVING = 1;

	// 总层数（固定写 2）
	static constexpr ObjectLayer NUM_LAYERS = 2;
};
// 决定：两个物体层是否可以碰撞
class ObjectLayerPairFilterImpl : public ObjectLayerPairFilter
{
public:
	// 引擎问：物体1 和 物体2 能碰撞吗？返回 true=能撞，false=不撞
	virtual bool ShouldCollide(ObjectLayer inObject1, ObjectLayer inObject2) const override
	{
		switch (inObject1)
		{
			// 静态物体（地面/墙）
		case Layers::NON_MOVING:
			// 只和【移动层】碰撞 → 正确！地面只跟角色/箱子撞
			return inObject2 == Layers::MOVING;

			// 移动物体（角色/箱子）
		case Layers::MOVING:
			// 和所有层都能碰撞 → 角色碰地面、角色碰角色、箱子碰箱子
			return true;

		default:
			JPH_ASSERT(false); // 非法层，报错
			return false;
		}
	}
};
// 粗检测层：物理引擎用来加速碰撞检测
namespace BroadPhaseLayers
{
	static constexpr BroadPhaseLayer NON_MOVING(0); // 静态物体树
	static constexpr BroadPhaseLayer MOVING(1);     // 移动物体树
	static constexpr uint NUM_LAYERS(2);            // 两棵树
};

// BroadPhaseLayerInterface implementation
// This defines a mapping between object and broadphase layers.
class BPLayerInterfaceImpl final : public BroadPhaseLayerInterface
{
public:
	BPLayerInterfaceImpl()
	{
		// Create a mapping table from object to broad phase layer
		mObjectToBroadPhase[Layers::NON_MOVING] = BroadPhaseLayers::NON_MOVING;
		mObjectToBroadPhase[Layers::MOVING] = BroadPhaseLayers::MOVING;
	}

	virtual uint					GetNumBroadPhaseLayers() const override
	{
		return BroadPhaseLayers::NUM_LAYERS;
	}

	virtual BroadPhaseLayer			GetBroadPhaseLayer(ObjectLayer inLayer) const override
	{
		JPH_ASSERT(inLayer < Layers::NUM_LAYERS);
		return mObjectToBroadPhase[inLayer];
	}

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

private:
	BroadPhaseLayer					mObjectToBroadPhase[Layers::NUM_LAYERS];
};
/// 类功能：判断一个 对象层 (ObjectLayer) 是否可以与 粗检测层 (BroadPhaseLayer) 发生碰撞
class ObjectVsBroadPhaseLayerFilterImpl : public ObjectVsBroadPhaseLayerFilter
{
public:
	// 核心函数：返回 true=可以碰撞，返回 false=忽略碰撞
	virtual bool				ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override
	{
		switch (inLayer1)
		{
			// 如果物体1是【静态物体】（地面、墙壁）
		case Layers::NON_MOVING:
			// 规则：只与【移动粗检测层】发生碰撞（静态物体之间不检测）
			return inLayer2 == BroadPhaseLayers::MOVING;

			// 如果物体1是【移动物体】（角色、箱子）
		case Layers::MOVING:
			// 规则：与所有层都可以碰撞
			return true;

		default:
			// 非法层，触发断言报错
			JPH_ASSERT(false);
			return false;
		}
	}
};

// 示例：碰撞接触监听器（用于监听物体碰撞/分离事件）
class MyContactListener : public ContactListener
{
public:
	// 碰撞验证回调：在创建接触前调用，可提前拒绝碰撞
	virtual ValidateResult	OnContactValidate(const Body& inBody1, const Body& inBody2, RVec3Arg inBaseOffset, const CollideShapeResult& inCollisionResult) override
	{
		//cout << "Contact validate callback" << endl; // 碰撞验证回调

		// 允许为这对物体接受所有接触（使用层过滤来忽略碰撞更高效）
		return ValidateResult::AcceptAllContactsForThisBodyPair;
	}

	// 碰撞添加：两个物体**首次接触**时调用（只触发一次）
	virtual void			OnContactAdded(const Body& inBody1, const Body& inBody2, const ContactManifold& inManifold, ContactSettings& ioSettings) override
	{
		//cout << "A contact was added" << endl; // 新增碰撞接触
	}

	// 碰撞持续：两个物体**保持接触**时每帧调用
	virtual void			OnContactPersisted(const Body& inBody1, const Body& inBody2, const ContactManifold& inManifold, ContactSettings& ioSettings) override
	{
		//cout << "A contact was persisted" << endl; // 碰撞持续中
	}

	// 碰撞移除：两个物体**分离**时调用（只触发一次）
	virtual void			OnContactRemoved(const SubShapeIDPair& inSubShapePair) override
	{
		//cout << "A contact was removed" << endl; // 碰撞接触已移除
	}
};

// 示例：刚体激活/休眠监听器
class MyBodyActivationListener : public BodyActivationListener
{
public:
	// 物体被激活：从静止→运动状态（被撞击、受力、移动时触发）
	virtual void		OnBodyActivated(const BodyID& inBodyID, uint64 inBodyUserData) override
	{
		//cout << "A body got activated" << endl; // 刚体已激活
	}

	// 物体休眠：从运动→静止状态（完全不动一段时间后触发，节省性能）
	virtual void		OnBodyDeactivated(const BodyID& inBodyID, uint64 inBodyUserData) override
	{
		//cout << "A body went to sleep" << endl; // 刚体已休眠
	}
};

inline Color to_color(JPH::Color& color)
{
	return createColor(color.r, color.g, color.b, color.a);
}

class MyDebugRenderer : public JPH::DebugRendererSimple
{
public:
	// 绘制单条线
	virtual void DrawLine(JPH::RVec3Arg inFrom, JPH::RVec3Arg inTo, JPH::ColorArg inColor) override
	{
		auto p0 = to_pos3(inFrom);
		auto p1 = to_pos3(inTo);
		Color color = to_color(inColor);
		g_gfx->SetColor(color);
		g_gfx->Line(p0, p1);
	}

	// 绘制三角形（用于实体/凸包）
	virtual void DrawTriangle(JPH::RVec3Arg v1, JPH::RVec3Arg v2, JPH::RVec3Arg v3,
		JPH::ColorArg col, ECastShadow = ECastShadow::Off) override
	{
		auto p0 = to_pos3(v1);
		auto p1 = to_pos3(v2);
		auto p2 = to_pos3(v3);
		Color color = to_color(col);
		g_gfx->SetColor(color);
		g_gfx->Triangle(p0, p1, p2);
	}
	virtual void DrawText3D(RVec3Arg inPosition, const string_view& inString,
		ColorArg inColor = JPH::Color::sWhite, float inHeight = 0.5f)override
	{

	}
};

struct PhysicsData
{
	// 对象层 → 粗检测层 映射表
	BPLayerInterfaceImpl broad_phase_layer_interface;
	// 对象层 vs 粗检测层 过滤
	ObjectVsBroadPhaseLayerFilterImpl object_vs_broadphase_layer_filter;
	// 对象层 vs 对象层 碰撞过滤（最常用！）
	ObjectLayerPairFilterImpl object_vs_object_layer_filter;
	// 激活休眠回调
	MyBodyActivationListener body_activation_listener;
	// 碰撞回调
	MyContactListener contact_listener;
	// 调试绘图
	MyDebugRenderer debug_render;
};

Physics::Physics() :
	_t{ 0 }
{
	// 使用默认分配器
	RegisterDefaultAllocator();

	// 自定义调试器
	Trace = TraceImpl;
	JPH_IF_ENABLE_ASSERTS(AssertFailed = AssertFailedImpl;)

	// 创建工厂类
	JPH::Factory::sInstance = new JPH::Factory();

	// 注册物理类型（形状）
	RegisterTypes();

	// 内存池
	_tempMemory = new JPH::TempAllocatorImpl{ 10 * 1024 * 1024 };

	// 创建工作线程
	_jobSystem = new JPH::JobSystemThreadPool(
		cMaxPhysicsJobs, cMaxPhysicsBarriers,
		thread::hardware_concurrency() - 1
	);

	_data = new PhysicsData;
	// 调试绘图
	JPH::DebugRenderer::sInstance = &_data->debug_render;

	// 物理系统支持的最大刚体数量
	const uint cMaxBodies = 1024;

	// 保护刚体并发访问的互斥锁数量（0=使用引擎默认值）
	const uint cNumBodyMutexes = 0;

	// 最大待检测碰撞对数量（粗检测阶段的队列上限）
	const uint cMaxBodyPairs = 1024;

	// 最大接触约束数量（实际碰撞点缓存）
	// 太小会导致物体穿透/穿模
	const uint cMaxContactConstraints = 1024;

	

	// 主系统
	_system = new JPH::PhysicsSystem;

	_system->Init(cMaxBodies, cNumBodyMutexes, cMaxBodyPairs, cMaxContactConstraints,
		_data->broad_phase_layer_interface,
		_data->object_vs_broadphase_layer_filter,
		_data->object_vs_object_layer_filter);

	_system->SetGravity(JPH::Vec3(0, 0, -9.81f));
	// 激活休眠回调
	_system->SetBodyActivationListener(&_data->body_activation_listener);

	// 碰撞回调
	_system->SetContactListener(&_data->contact_listener);

	// 物体管理器
	_bodyInterface = &_system->GetBodyInterface();
}

dl::RigidBody* Physics::CreateRigidBody(JPH::ShapeRefC* ref,
	const Position3& pos, const Position3& rotate, 
	RigidBodyType type, bool active)
{
	RigidBody* ret = new RigidBody;

	BodyCreationSettings setting {
		*ref,
		to_rvec3(pos),
		Quat::sEulerAngles(to_rvec3(rotate)),
		(EMotionType)type,
		type == RigidBodyType::STATIC ? Layers::NON_MOVING : Layers::MOVING
	};
	setting.mRestitution = 0.5f; // 弹性
	ret->_body = _bodyInterface->CreateBody(setting);

	_bodyInterface->AddBody(ret->_body->GetID(), 
		active ? EActivation::Activate : EActivation::DontActivate);
	return ret;
}

dl::RigidBody* Physics::CreateRigidBodyDefault(Transform* trans)
{
	auto ref = CreateShapeBox(0, { 1,1,1 });
	return CreateRigidBody(ref, {}, {}, RigidBodyType::DYNAMIC, true);
}

inline uint64_t hash3f_64(const Position3& pos)
{
	auto ux = *(uint32_t*)(&pos.x);
	auto uy = *(uint32_t*)(&pos.y);
	auto uz = *(uint32_t*)(&pos.z);

	uint64_t h = (
		(uint64_t)ux * 0x123456789 +
		(uint64_t)uy * 0x987654321 +
		(uint64_t)uz * 0x555555555
		);

	h ^= h >> 32;
	h *= 0x9E3779B97F4A7C15; // 黄金比例常数
	h ^= h >> 32;
	return h;
}

JPH::ShapeRefC* Physics::CreateShapeBox(size_t shape_id, 
	const Position3& box,
	const Position3& scale,
	const Position3& position,
	const Position3& rotate)
{
	RigidBodyShape* shape;

	auto iter = _allShape.find(shape_id);
	if (iter == _allShape.end())
	{
		shape = new RigidBodyShape;
		_allShape[shape_id] = shape;
	}
	else
		shape = iter->second;

	uint64_t s_index = hash3f_64(scale);

	auto iter1 = shape->_mapRef.find(s_index);
	if (iter1 == shape->_mapRef.end())
	{
		Vec3 vec{
			box.x * scale.x,
			box.y * scale.y,
			box.z * scale.z };
		BoxShapeSettings setting{ vec };

		ShapeSettings::ShapeResult result = setting.Create();
		if (result.HasError())
			log_err("JPH Shape: {}", result.GetError());
		ShapeRefC ref = result.Get();


		// 偏移，旋转
		if (!Every::Zero(position)
			|| !Every::Zero(scale))
		{
			RotatedTranslatedShapeSettings setting{
				to_rvec3(position),
				JPH::Quat::sEulerAngles(to_rvec3(rotate)),
				ref
			};

			ShapeSettings::ShapeResult result = setting.Create();
			if (result.HasError())
				log_err("JPH Shape: {}", result.GetError());
			ref = result.Get();
		}
		shape->_mapRef[s_index] = ref;
		return &shape->_mapRef[s_index];
	}
	else
		return &iter1->second;
}

void Physics::Update()
{
	// 添加大量物体后调用，可优化性能 _system->OptimizeBroadPhase();

	//
	constexpr float DT_STANDARD = 1.0f / 60.0f;
	// 单帧最大处理时间（防止卡顿雪崩）
	constexpr float DT_MAX = 0.1f;
	//
	float dt = g_time->GetDelta();

	if (dt > DT_MAX)
		dt = DT_MAX;
	_t += dt;

	// 固定步长迭代物理
	int steps = 0;
	while (_t >= DT_STANDARD && steps < 4) // 最多4步防卡死
	{
		_system->Update(DT_STANDARD, 1, _tempMemory, _jobSystem);

		_t -= DT_STANDARD;
		steps++;
	}

	// 插值
	float alpha = _t / DT_STANDARD;
}

void Physics::RenderDebug()
{
	//struct DrawFilter : public JPH::BodyDrawFilter
	//{
	//	virtual bool ShouldDraw(const JPH::Body& body) const override
	//	{
	//		// 只画动态
	//		return body.GetMotionType() == JPH::EMotionType::Dynamic;
	//		// 或按层：return body.GetCollisionLayer() == LAYER_PLAYER;
	//	}
	//};

	//DrawFilter filter;

	JPH::BodyManager::DrawSettings drawSettings;
	drawSettings.mDrawShape = true;          // 碰撞形状（线框/实体）
	drawSettings.mDrawShapeWireframe = true;// 线框优先
	drawSettings.mDrawBoundingBox = true;   // AABB
	drawSettings.mDrawCenterOfMassTransform = true;  // 质心点

	// 绘制所有 Body
	_system->DrawBodies(drawSettings, &_data->debug_render);

	_system->DrawConstraints(&_data->debug_render);        // 约束本体
	_system->DrawConstraintLimits(&_data->debug_render);    // 限制范围
	_system->DrawConstraintReferenceFrame(&_data->debug_render); // 局部坐标系
}

void Physics::_delete_rigid_body(RigidBody* rigid_body)
{
	_bodyInterface->RemoveBody(rigid_body->_body->GetID());
	_bodyInterface->DestroyBody(rigid_body->_body->GetID());
}

Physics::~Physics()
{
	for (auto& iter : _allShape)
	{
		delete iter.second;
	}

	delete _system;
	delete _jobSystem;
	delete _tempMemory;

	// Unregisters all types with the factory and cleans up the default material
	UnregisterTypes();

	// Destroy the factory
	delete JPH::Factory::sInstance;
	JPH::Factory::sInstance = nullptr;

	delete _data;
}

}