#include "baseentity.h"
#include "baseplayer.h"
#include "cglm/mat4.h"
#include "cglm/util.h"
#include "cglm/vec2.h"
#include "cglm/vec3.h"
#include "console.h"
#include "engine.h"
#include "physics.h"
#include "physics_gen.h"

class CMOBAPlayer: public CBasePlayer
{
public:
	virtual void Precache() override;
	virtual void Spawn( void ) override;
	virtual void Destroy( void ) override;
	virtual void ReadParameter( const char *szName, const char *szValue ) override;
	virtual void Think( float fDelta ) override;

	void AirAccelerate();
	void Accelerate( float fDelta, vec2 wishDir, float fWishSpeed, float fAcceleration );
	void Move( float fDelta );


	bool bIsForward = 0;
	bool bIsBack = 0;
	bool bIsLeft = 0;
	bool bIsRight = 0;

	bool bIsJumping = 0;
	bool bIsCrouching = 0;
	bool bIsProning = 0;
	bool bIsFiring = 0;
	bool bIsFiring2 = 0;

	float fPitch = 0;
	float fYaw = 0;
	
	vec3 m_velocity;

	CPxBoxMesh mesh;
	CPxRigidBody rigidbody;
};

void CMOBAPlayer::Precache()
{

}

void CMOBAPlayer::Spawn()
{
	mesh.m_fRadius[0] = 0.3;
	mesh.m_fRadius[1] = 0.3;
	mesh.m_fRadius[2] = 0.8;
	mesh.Spawn();
	px_matrix m = {};
	m.m[0] = 1;
	m.m[5] = 1;
	m.m[10] = 1;
	m.m[15] = 1;
	m.m[12] = m_position[0];
	m.m[13] = m_position[1];
	m.m[14] = m_position[2];
	rigidbody.Spawn(&mesh, m, {
			.gravity_scale = 1,
			.continous = true,
			.lockrotx = 1,
			.lockroty = 1,
			.lockrotz = 1,
			.dominance = 127,
			});
};


void CMOBAPlayer::ReadParameter( const char *szName, const char *szValue )
{
	CBaseEntity::ReadParameter(szName, szValue);
}

void CMOBAPlayer::Destroy()
{

}

void CMOBAPlayer::AirAccelerate()
{

}

void CMOBAPlayer::Accelerate( float fDelta, vec2 wishDir, float fWishSpeed, float fAcceleration )
{
	float fCurrentSpeed = glm_vec2_dot(wishDir, m_velocity);
	float fAddSpeed = fWishSpeed - fCurrentSpeed;
	if ( fAddSpeed < 0 )
		return;
	float fAccelSpeed = fAcceleration*fDelta*fWishSpeed;
	if ( fAccelSpeed > fAddSpeed )
		fAccelSpeed = fAddSpeed;
	glm_vec2_scale(wishDir, fAccelSpeed, m_velocity);
}

void CMOBAPlayer::Move( float fDelta )
{
	vec2 forward = {(float)bIsForward-(float)bIsBack,0};
	vec2 right = {0,(float)bIsLeft-(float)bIsRight};
	vec2 wishDir;
	float fWishSpeed;

	glm_vec2_rotate(forward, fYaw, forward);
	glm_vec2_rotate(forward, fYaw, forward);
	glm_vec2_add(forward, right, wishDir);
	fWishSpeed = glm_vec2_distance(wishDir, (vec3){0,0,0});

	if (fWishSpeed != 0)
	{
		glm_vec2_divs(wishDir, fWishSpeed, wishDir);
		fWishSpeed = 10;
	} else {
		glm_vec2_zero(wishDir);
	}

	Accelerate(fDelta, wishDir, fWishSpeed, 5.5);
}


void CMOBAPlayer::Think( float fDelta )
{
	m_position[0] = rigidbody.GetPosition().m[0];
	m_position[1] = rigidbody.GetPosition().m[1];
	m_position[2] = rigidbody.GetPosition().m[2];
	px_vec3 v = rigidbody.GetVelocity();
	px_vec3 p = rigidbody.GetPosition();

	for (int i = 0; i < 3; i++)
		m_velocity[i] = v.m[i];

	px_cast_result r = px_box_cast(px, 0.3,0.3,0.01, {p.m[0],p.m[1],p.m[2]-0.79f}, {0,0,0}, {0,0,-1}, 1);
	if (r.hit)
	{
		Move(fDelta);
	}

	for (int i = 0; i < 3; i++)
		v.m[i] = m_velocity[i];

	rigidbody.SetVelocity(v);
};

void PlayerForward(int argc, char **argv) {if (g_localClient.pBasePlayer) ((CMOBAPlayer*)g_localClient.pBasePlayer)->bIsForward = true;};
void PlayerForwardUp(int argc, char **argv) {if (g_localClient.pBasePlayer) ((CMOBAPlayer*)g_localClient.pBasePlayer)->bIsForward = false;};
void PlayerBack(int argc, char **argv) {if (g_localClient.pBasePlayer) ((CMOBAPlayer*)g_localClient.pBasePlayer)->bIsBack = true;};
void PlayerBackUp(int argc, char **argv) {if (g_localClient.pBasePlayer) ((CMOBAPlayer*)g_localClient.pBasePlayer)->bIsBack = false;};
void PlayerLeft(int argc, char **argv) {if (g_localClient.pBasePlayer) ((CMOBAPlayer*)g_localClient.pBasePlayer)->bIsLeft = true;};
void PlayerLeftUp(int argc, char **argv) {if (g_localClient.pBasePlayer) ((CMOBAPlayer*)g_localClient.pBasePlayer)->bIsLeft = false;};
void PlayerRight(int argc, char **argv) {if (g_localClient.pBasePlayer) ((CMOBAPlayer*)g_localClient.pBasePlayer)->bIsRight = true;};
void PlayerRightUp(int argc, char **argv) {if (g_localClient.pBasePlayer) ((CMOBAPlayer*)g_localClient.pBasePlayer)->bIsRight = false;};

ConCommand moveForwardCmd("+forward", PlayerForward, 0);
ConCommand moveForwardUpCmd("-forward", PlayerForwardUp, 0);
ConCommand moveBackwardCmd("+back", PlayerBack, 0);
ConCommand moveBackwardUpCmd("-back", PlayerBackUp, 0);
ConCommand moveLeftCmd("+left", PlayerLeft, 0);
ConCommand moveLeftUpCmd("-left", PlayerLeftUp, 0);
ConCommand moveRightCmd("+right", PlayerRight, 0);
ConCommand moveRightUpCmd("-right", PlayerRightUp, 0);

DECLARE_ENTITY(player, CMOBAPlayer);