#include "CPhysicalEntity.h"
	#include "CEngine.h"
	#include "CGame.h"
	#include "CResourcesManager.h"
	#include "CTransformable.h"
	void CPhysicalEntity::V_Init()
	{
	if(!WorldUnit)
	{
	auto opt_worldId = GetEntity()->GetInitParam<int>("phys_worldid");
	auto opt_collision = GetEntity()->GetInitParam<std::string>("phys_collision");
	auto opt_type = GetEntity()->GetInitParam<std::string>("phys_type");
	if(!opt_worldId.has_value() || !opt_collision.has_value() || !opt_type.has_value()) { return; }
	auto opt_mass = GetEntity()->GetInitParam<float>("phys_mass");
	CTransform start_transform;
	float mass = 100.0f;
	if(opt_mass.has_value()) { mass = opt_mass.value(); }
	auto opt_start_position = GetEntity()->GetInitParam<glm::vec3>("phys_position");
	auto opt_start_quat = GetEntity()->GetInitParam<glm::quat>("phys_rotation");
	auto opt_start_euler = GetEntity()->GetInitParam<CAngles>("phys_euler");
	if(opt_start_position.has_value())
	{
	start_transform.SetPosition(opt_start_position.value());
	}
	else
	{
	start_transform.SetPosition(glm::vec3(0, 0, 0));
	}
	if(opt_start_quat.has_value())
	{
	start_transform.SetRotation(opt_start_quat.value());
	}
	if(opt_start_euler.has_value())
	{
	start_transform.GetEulerRotation().SetRotation(opt_start_euler.value());
	}
	if(!opt_start_quat.has_value() && !opt_start_euler.has_value())
	{
	start_transform.SetRotation(glm::identity<glm::quat>());
	}
	start_transform.SetScale(glm::vec3(1, 1, 1));
	auto game = CEngine::GetInstance()->Components.GetComponentTyped<CGame>();
	auto resman = CEngine::GetInstance()->Components.GetComponentTyped<CResourcesManager>();
	auto world = game->PhysicsEngine->GetWorld(opt_worldId.value());
	if(!world)
	{
	Log::ErrInstance() << "No world for physical entity\n";
	return;
	}
	auto _col = resman->GetOrCreate("collision", opt_collision.value());
	if(!_col)
	{
	Log::ErrInstance() << "No collision for physical entity\n";
	return;
	}
	_col->Wait();
	auto col = std::dynamic_pointer_cast<CCollision>(_col);
	if(!_col)
	{
	Log::ErrInstance() << "No collision for physical entity (2)\n";
	return;
	}
	std::string type = opt_type.value();
	if(type == "rigid")
	{
	WorldUnit = world->CreateRigidBody(col, start_transform, mass); //TODO remove on destructor
	}
	else if(type == "kinematic")
	{
	WorldUnit = world->CreateKinematicBody(col, start_transform); //TODO remove on destructor
	}
	else if(type == "ghost")
	{
	WorldUnit = world->CreateGhostBody(col, start_transform); //TODO remove on destructor
	}
	else
	{
	Log::ErrInstance() << "Unknown physical type\n";
	}
	Log::Instance() << "Created local physical with mass of " << mass << Log::Endl;
	}
	WorldUnit->GetTransformCallback() += [this](CTransformBase::CMeasurePack oldpack, CTransformBase* _this) -> void
	{
	auto transformable = GetEntity()->Components.GetComponentTyped<CTransformable>();
	if(transformable && WorldUnit->IsUpdatingInternalTransform())
	{
	transformable->Transform.SetPRS(_this->GetPRS());
	}
	};
	auto entity = GetEntity();
	auto transformable = entity->Components.GetComponentTyped<CTransformable>();
	if(!transformable)
	{
	entity->Components.CreateComponent<CTransformable>();
	transformable = entity->Components.GetComponentTyped<CTransformable>();
	}
	transformable->Transform.OnTransformChanged += [this](CTransformBase::CMeasurePack oldpack, CTransformBase* _this) -> void
	{
	auto transformable = GetEntity()->Components.GetComponentTyped<CTransformable>();
	if(transformable && !WorldUnit->IsUpdatingInternalTransform())
	{
	WorldUnit->SetTransform(transformable->Transform);
	auto rigidBody = dynamic_cast<CRigidBody*>(WorldUnit);
	if(rigidBody)
	{
	rigidBody->Activate();
	}
	}
	};
	}
	#pragma pack(push, 1)
	struct PhysicalFlags
	{
	unsigned int Type : 2;
	unsigned int __unused : 6;
	};
	#pragma pack(pop)
	void CPhysicalEntity::FullPack(CBufferWrapper& packet)
	{
	auto colName = WorldUnit->GetCollision()->Name;
	auto rigid = dynamic_cast<CRigidBody*>(WorldUnit);
	float mass = 0.0f;
	if(rigid)
	{
	mass = rigid->GetMass();
	}
	auto trans = WorldUnit->GetTransform();
	auto rigidBody = dynamic_cast<CRigidBody*>(WorldUnit);
	auto kinematicBody = dynamic_cast<CKinematicBody*>(WorldUnit);
	auto ghostBody = dynamic_cast<CGhostBody*>(WorldUnit);
	PhysicalFlags flags;
	if(rigidBody) { flags.Type = 0; }
	else if(kinematicBody) { flags.Type = 1; }
	else if(ghostBody) { flags.Type = 2; }
	Log::Instance() << "Write Physical Flag: " << flags.Type << Log::Endl;
	packet.WriteLenString<std::uint8_t, std::string>(colName);
	packet.Write<float>(mass);
	packet.Write<PhysicalFlags>(flags);
	packet.Write<glm::vec3>(trans.GetPosition());
	packet.Write<glm::quat>(trans.GetRotation());
	}
	void CPhysicalEntity::FullUnpack(CBufferWrapper& packet)
	{
	auto colName = packet.ReadLenString<std::uint8_t, std::string>();
	auto mass = packet.Read<float>();
	auto flags = packet.Read<PhysicalFlags>();
	auto pos = packet.Read<glm::vec3>();
	auto rot = packet.Read<glm::quat>();
	auto game = CEngine::GetInstance()->Components.GetComponentTyped<CGame>();
	auto resman = CEngine::GetInstance()->Components.GetComponentTyped<CResourcesManager>();
	auto world = game->PhysicsEngine->GetWorld(0); //TODO not zero
	CTransform start_transform;
	start_transform.SetPRS
	(
	{
	pos,
	rot,
	glm::vec3(1, 1, 1)
	}
	);
	if(!world)
	{
	Log::ErrInstance() << "No world for physical entity\n";
	return;
	}
	auto _col = resman->GetOrCreate("collision", colName + ".ecol");
	if(!_col)
	{
	Log::ErrInstance() << "No collision for physical entity\n";
	return;
	}
	auto col = std::dynamic_pointer_cast<CCollision>(_col);
	//col->Wait();
	if(!col)
	{
	Log::ErrInstance() << "No collision for physical entity (2)\n";
	return;
	}
	Log::Instance() << "Read Physical Flag: " << flags.Type << Log::Endl;
	if(flags.Type == 0)
	{
	WorldUnit = world->CreateRigidBody(col, start_transform, mass); //TODO remove on destructor
	}
	else if(flags.Type == 1)
	{
	WorldUnit = world->CreateKinematicBody(col, start_transform); //TODO remove on destructor
	}
	else if(flags.Type == 2)
	{
	WorldUnit = world->CreateGhostBody(col, start_transform); //TODO remove on destructor
	}
	else
	{
	Log::ErrInstance() << "Unknown physical type\n";
	}
	//WorldUnit = world->CreateRigidBody(col, start_transform, mass); //TODO remove on destructor
	}
	LINK_SOL_USERTYPE(CPhysicalEntity);
	LINK_ENTITY_COMPONENT_TO_CLASS(CPhysicalEntity, physical);