#include "PajaTypes.h"
#include "Vector2C.h"
#include <list>
#include "SimpleVehicleC.h"
#include "RandomMT.h"

using namespace PajaTypes;

#define	VEHICLE_RADIUS			30.0f
#define	AVOID_RADIUS				100.0f
#define	INFLUENCE_RADIUS		350.0f
#define	OBS_AVOID_RADIUS		50.0f
#define	BOUND_RADIUS_START	600.0f
#define	BOUND_RADIUS_RANGE	200.0f

#define	CHAT_TIME						(30 * 2)
#define	CHAT_IDLE_TIME			(30 * 6)
#define	GROW_TIME						(30 * 2)
#define	GROW_TIME_MIN				(30 * 1)
#define	GROW_TIME_RANGE			(30 * 80)
#define	BOUST_TIME					(30 / 2)
#define	STARE_TIME					(30 * 3)
#define	LOOKAT_TIME					(30 * 1)
#define	AGE_ANIM_TIME				(30)


inline
int32
rand_range( int32 i32Max )
{
	return gen_rand() % i32Max;
}

inline
Vector2C
truncate( const Vector2C& Vec, float32 f32Max )
{
	float32	f32Len = Vec.length();
	if( f32Len > f32Max )
		return Vec * f32Max / f32Len;
	else
		return Vec;
}


SimpleVehicleC::SimpleVehicleC() :
	m_pInstance( 0 )
{
}

SimpleVehicleC::~SimpleVehicleC()
{
	if( m_pInstance )
	{
		m_pInstance->Release();
		m_pInstance = 0;
	}
}

void
SimpleVehicleC::init( const Vector2C& Pos, PajaTypes::int32 i32ID, PajaTypes::int32 i32ModelID, PajaTypes::float32 f32Radius )
{
	m_State.m_Pos = Pos;
	m_State.m_OldPos = Pos;
	m_State.m_Vel = Vector2C( 0, 0 );
	m_State.m_TopPos = Pos;
	m_State.m_TopOldPos = Pos;
	m_State.m_TopVel = Vector2C( 0, 0 );
	m_State.m_Acc = Vector2C( 0, 0 );
	m_State.m_Dir = Vector2C( 0, 0 );
	m_State.m_f32MaxForce = 5000.0f;
	m_State.m_f32MaxSpeed = 70.0f;
	m_State.m_f32Mass = 1.0f;
	m_State.m_f32Heading = (float32)float_gen_rand() * (float32)M_PI * 2.0f;
	m_State.m_f32OldHeading = m_State.m_f32Heading;
	m_State.m_f32HeadingVel = 0.0f;
	m_State.m_f32WanderSteering = 0.0f;
	m_State.m_SteeringForce = Vector2C( 0, 0 );
	m_State.m_LookAtDir = Vector2C( 0, 0 );
	m_State.m_i32ChatCounter = CHAT_IDLE_TIME + rand_range( CHAT_IDLE_TIME );
	m_State.m_i32Task = VEC_TASK_WANDER;
	m_State.m_i32Age = 0;
	m_State.m_i32BoustCounter = 0;
	m_State.m_i32TaskCounter = 0;
	m_State.m_i32AgeAnimCounter = 0;
	m_State.m_f32AgeAnim = 0;
	m_State.m_f32OldAgeAnim = 0;
	m_State.m_f32Height = 0;
	m_State.m_f32OldHeight = 0;
	m_State.m_f32HeightVel = 0;
	m_State.m_f32HeightForce = 0;

	m_i32ModelID = i32ModelID;
	m_i32ID = i32ID;
	m_f32Radius = f32Radius;

	update_grid_pos();
}

void
SimpleVehicleC::handle_collision( SimpleVehicleC* pVec )
{
	// Handle collisions.
	Vector2C	Diff;

	Diff = m_State.m_Pos - pVec->m_State.m_Pos;
	float32	f32Len = Diff.length();
	if( f32Len < AVOID_RADIUS )
	{
		float32	f32Amount = ((f32Len - AVOID_RADIUS * 2) / (AVOID_RADIUS * 2 - m_f32Radius));
		if( f32Amount < 0.0f )
			f32Amount = 0.0f;
		f32Amount = 1.0f - f32Amount;
		float32	f32Force = f32Amount * 500.0f / f32Len;
		m_State.m_AvoidForce += Diff * f32Force + (m_State.m_Left * f32Amount * 20.0f);
		pVec->m_State.m_AvoidForce += -Diff * f32Force;
	}

	if( f32Len < INFLUENCE_RADIUS )
	{
		SimpleVehicleC* pThis = 0;
		SimpleVehicleC* pOther = 0;

		for( uint32 i = 0; i < 2; i++ )
		{
			if( i == 0 )
			{
				pThis = this;
				pOther = pVec;
			}
			else
			{
				pThis = pVec;
				pOther = this;
				Diff = -Diff;
			}

			if( pThis->m_State.m_i32Task == VEC_TASK_CHAT )
			{
				if( pOther->is_bousting() && pOther->get_age() > pThis->m_State.m_i32Age )
				{
					pThis->m_State.m_i32Task = VEC_TASK_STARE;
					pThis->m_State.m_i32TaskCounter = STARE_TIME;
					pThis->m_State.m_i32StareId = pOther->get_id();
				}
				else
				{
					if( pOther->get_task() == VEC_TASK_CHAT )
						pThis->m_State.m_LookAtDir += -Diff;
					pThis->m_State.m_SteeringForce += -(Diff / f32Len) * 50.0f;
				}
			}
			else if( pThis->m_State.m_i32Task == VEC_TASK_WANDER )
			{
				if( pOther->is_bousting() && pOther->get_age() > pThis->m_State.m_i32Age )
				{
					pThis->m_State.m_i32Task = VEC_TASK_STARE;
					pThis->m_State.m_i32TaskCounter = STARE_TIME;
					pThis->m_State.m_i32StareId = pOther->get_id();
				}
				else if( pThis->can_chat() && pOther->can_chat() )
				{
					pOther->set_chat();
					pThis->set_chat();
				}
			}
			else if( pThis->m_State.m_i32Task == VEC_TASK_STARE )
			{
				if( pOther->get_id() == pThis->m_State.m_i32StareId )
				{
					pThis->m_State.m_LookAtDir += -Diff;
					if( pOther->is_bousting() )
					{
						float32	f32Amount = 1.0f - (f32Len / INFLUENCE_RADIUS);
						pThis->m_State.m_SteeringForce += (Diff / f32Len) * f32Amount * 500.0f;
					}
					else
						pThis->m_State.m_SteeringForce += -(Diff / f32Len) * 100.0f;
				}
			}
			else if( pThis->m_State.m_i32Task == VEC_TASK_GROW )
			{
			}
		}
	}

}

void
SimpleVehicleC::handle_collision( std::vector<ObstacleS>& Obstacles )
{
	for( std::vector<ObstacleS>::iterator ObIt = Obstacles.begin(); ObIt != Obstacles.end(); ++ObIt )
	{
		ObstacleS&	Ob = (*ObIt);

		Vector2C	Diff;

		Diff = m_State.m_Pos - Ob.m_Pos;
		float32	f32Len = Diff.length();

		if( (f32Len - Ob.m_f32Radius) < OBS_AVOID_RADIUS )
		{
			float32	f32Amount = (f32Len - Ob.m_f32Radius - m_f32Radius) / (OBS_AVOID_RADIUS - m_f32Radius);
			if( f32Amount < 0.0f )
				f32Amount = 0.0f;
			f32Amount = 1.0f - f32Amount;
			Diff *= f32Amount * 500.0f / f32Len;
			m_State.m_AvoidForce += Diff;
		}
	}
}

void
SimpleVehicleC::update( float32 f32DT )
{
	// Store old pos.
	m_State.m_OldPos = m_State.m_Pos;
	m_State.m_TopOldPos = m_State.m_TopPos;
	m_State.m_f32OldHeading = m_State.m_f32Heading;
	m_State.m_f32OldAgeAnim = m_State.m_f32AgeAnim;

	Vector2C	LookAtDir( 0, 0 );

	// Update
	if( m_State.m_i32Task == VEC_TASK_WANDER )
	{
		m_State.m_SteeringForce += wander();
	}
	else if( m_State.m_i32Task == VEC_TASK_CHAT )
	{
		if( m_State.m_i32TaskCounter == 0 )
		{
			m_State.m_i32Task = VEC_TASK_WANDER;
			m_State.m_i32ChatCounter = CHAT_IDLE_TIME + rand_range( CHAT_IDLE_TIME );
		}
	}
	else if( m_State.m_i32Task == VEC_TASK_STARE )
	{
		if( m_State.m_i32TaskCounter == 0 )
		{
			m_State.m_i32Task = VEC_TASK_WANDER;
		}
	}
	else if( m_State.m_i32Task == VEC_TASK_LOOKAT )
	{
		Vector2C	Diff = m_State.m_Pos - m_State.m_LookAtPos;
		float32		f32Len = Diff.length();
		m_State.m_LookAtDir += -Diff;
		m_State.m_SteeringForce += -(Diff / f32Len) * 300.0f;

		if( m_State.m_i32TaskCounter == 0 )
		{
			m_State.m_i32Task = VEC_TASK_WANDER;
		}
	}
	else if( m_State.m_i32Task == VEC_TASK_GROW )
	{
		if( m_State.m_i32TaskCounter == 0 )
		{
			m_State.m_i32Task = VEC_TASK_WANDER;
		}
	}


	// Keep inside the bounding circle.
	float32	f32Len = m_State.m_Pos.length();
	if( f32Len >= BOUND_RADIUS_START )
	{
		Vector2C	Dir = m_State.m_Pos / f32Len;
		float32	f32Amount = (f32Len - BOUND_RADIUS_START) / BOUND_RADIUS_RANGE;
		f32Amount *= f32Amount;
		m_State.m_AvoidForce += -Dir * f32Amount * 10000.0f;
	}
	
	// Integrate
	m_State.m_SteeringForce = truncate( m_State.m_SteeringForce, m_State.m_f32MaxForce );
	m_State.m_AvoidForce = truncate( m_State.m_AvoidForce, m_State.m_f32MaxForce );

	m_State.m_LookAtDir += m_State.m_Vel * 0.3f;

	float32	f32Scale = m_State.m_Dir.dot( m_State.m_SteeringForce.normalize() );
	m_State.m_SteeringForce *= f32Scale;

	m_State.m_SteeringForce += m_State.m_AvoidForce;

	m_State.m_Acc = (m_State.m_SteeringForce / m_State.m_f32Mass);
	m_State.m_Vel = truncate( m_State.m_Vel + m_State.m_Acc * f32DT, m_State.m_f32MaxSpeed );
	m_State.m_Pos += m_State.m_Vel * f32DT;

//	f32Scale = m_State.m_Dir.dot( m_State.m_Vel.normalize() );

	m_State.m_Vel *= 0.6f; // * (f32Scale * 0.3f);

//	if( m_State.m_Vel.length() < 0.1f )
//		m_State.m_Vel = Vector2C( 0, 0 );

	update_grid_pos();

	// top
	// Integrate
	Vector2C	Acc = ((m_State.m_Pos - m_State.m_TopPos) * 500.0f) / m_State.m_f32Mass;
	m_State.m_TopVel = truncate( m_State.m_TopVel + Acc * f32DT, m_State.m_f32MaxSpeed );
	m_State.m_TopPos += m_State.m_TopVel * f32DT;
	m_State.m_TopVel *= 0.7f;

	// Height
	m_State.m_f32OldHeight = m_State.m_f32Height;
	m_State.m_f32HeightVel += ( 0 - m_State.m_f32Height) * 1700.0f * f32DT;
	m_State.m_f32HeightVel += m_State.m_f32HeightForce * 10.0f * f32DT;
	m_State.m_f32Height += m_State.m_f32HeightVel * f32DT;
	m_State.m_f32HeightVel *= 0.6f;

	//
	// Orientation
	//

	if( m_State.m_LookAtDir.length() > 0.1f )
	{
		float	f32Angle = (float)atan2( m_State.m_LookAtDir[1], m_State.m_LookAtDir[0] );
		float	f32Diff = f32Angle - m_State.m_f32Heading;

//		m_State.m_f32Heading = f32Angle;


		// Find shortest change.
		while( f32Diff > M_PI )
			f32Diff -= (float)M_PI * 2.0f;
		while( f32Diff < -M_PI )
			f32Diff += (float)M_PI * 2.0f;

		// Clamp maximum speed to PI/4
		if( fabs( f32Diff ) > M_PI / 4 )
			f32Diff *= ((float)M_PI / 4) / (float)fabs( f32Diff );

		m_State.m_f32HeadingVel += f32Diff * 300.0f * f32DT;
		m_State.m_f32HeadingVel *= 0.7f;

		m_State.m_f32Heading += m_State.m_f32HeadingVel * f32DT;

		// Keep heading on range -2PI..2PI
		while( m_State.m_f32Heading > M_PI )
			m_State.m_f32Heading -= (float)M_PI * 2.0f;
		while( m_State.m_f32Heading < -M_PI )
			m_State.m_f32Heading += (float)M_PI * 2.0f;
	}

	m_State.m_Dir = Vector2C( (float32)cos( m_State.m_f32Heading ), (float32)sin( m_State.m_f32Heading ) );
	m_State.m_Left = Vector2C( (float32)cos( m_State.m_f32Heading + M_PI/2 ), (float32)sin( m_State.m_f32Heading + M_PI/2 ) );

	m_State.m_SteeringForce = Vector2C( 0, 0 );
	m_State.m_LookAtDir = Vector2C( 0, 0 );

	if( m_State.m_i32ChatCounter )
		m_State.m_i32ChatCounter--;

	if( m_State.m_i32BoustCounter )
		m_State.m_i32BoustCounter--;

	if( m_State.m_i32TaskCounter )
		m_State.m_i32TaskCounter--;

	int32	i32AgeAnimTarget = m_State.m_i32Age * AGE_ANIM_TIME;
	if( m_State.m_i32AgeAnimCounter < i32AgeAnimTarget )
		m_State.m_i32AgeAnimCounter++;
	else if( m_State.m_i32AgeAnimCounter > i32AgeAnimTarget )
		m_State.m_i32AgeAnimCounter--;

	m_State.m_f32AgeAnim = (float32)m_State.m_i32AgeAnimCounter / (float32)AGE_ANIM_TIME;

	m_State.m_f32HeightForce = 0;
}

Vector2C
SimpleVehicleC::wander()
{
	// Based in Creig Raynolds' steering behaviours.
	const float32	f32WanderSteeringRanging = 0.6f;
	const float32	f32WanderSteeringRate = 0.3f;

	m_State.m_f32WanderSteering += ((0.5f - (float32)float_gen_rand()) * (float32)M_PI / 2.0f) * f32WanderSteeringRanging;
	Vector2C	SteerDir( (float32)cos( m_State.m_f32WanderSteering ), (float32)sin( m_State.m_f32WanderSteering ) );
	return (m_State.m_Dir * (float32)sqrt( 2.0 ) + SteerDir) * f32WanderSteeringRate * 400.0f;
}

void
SimpleVehicleC::update_grid_pos()
{
	m_State.m_i32GridX = (int32)floor( m_State.m_Pos[0] / (INFLUENCE_RADIUS * 1.2f) );
	m_State.m_i32GridY = (int32)floor( m_State.m_Pos[1] / (INFLUENCE_RADIUS * 1.2f) );
}

bool
SimpleVehicleC::can_chat() const
{
	return m_State.m_i32ChatCounter == 0 || m_State.m_i32Task == VEC_TASK_CHAT;
}

void
SimpleVehicleC::set_chat()
{
	if( m_State.m_i32Task != VEC_TASK_CHAT && m_State.m_i32Task != VEC_TASK_GROW )
	{
		m_State.m_i32Task = VEC_TASK_CHAT;
		m_State.m_i32TaskCounter = CHAT_TIME + rand_range( CHAT_TIME );
	}
}

void
SimpleVehicleC::set_stare( int32 i32ID, int32 i32CallerAge )
{
	if( m_State.m_i32Task != VEC_TASK_STARE && m_State.m_i32Task != VEC_TASK_GROW )
	{
		if( i32CallerAge > m_State.m_i32Age )
		{
			m_State.m_i32Task = VEC_TASK_STARE;
			m_State.m_i32TaskCounter = STARE_TIME;
			m_State.m_i32StareId = i32ID;
		}
	}
}

int32
SimpleVehicleC::get_task() const
{
	return m_State.m_i32Task;
}

int32
SimpleVehicleC::get_age() const
{
	return m_State.m_i32Age;
}

bool
SimpleVehicleC::is_bousting() const
{
	return m_State.m_i32BoustCounter > 0;
}

int32
SimpleVehicleC::get_id() const
{
	return m_i32ID;
}

void
SimpleVehicleC::set_look_at( const PajaTypes::Vector2C& LookAt )
{
	m_State.m_LookAtPos = LookAt;
	m_State.m_i32Task = VEC_TASK_LOOKAT;
	m_State.m_i32TaskCounter = LOOKAT_TIME;
}

void
SimpleVehicleC::set_age( PajaTypes::int32 i32Age )
{
	if( i32Age > m_State.m_i32Age )
	{
		m_State.m_i32Age = i32Age;
		m_State.m_i32Task = VEC_TASK_GROW;
		m_State.m_i32TaskCounter = GROW_TIME;
		m_State.m_i32BoustCounter = BOUST_TIME;
	}
}

int32
SimpleVehicleC::get_model_id() const
{
	return m_i32ModelID;
}

void
SimpleVehicleC::set_model( CD3DModelInstance* pModelInst )
{
	if( m_pInstance )
	{
		m_pInstance->Release();
		m_pInstance = 0;
	}
	m_pInstance = pModelInst;
}

CD3DModelInstance*
SimpleVehicleC::get_model()
{
	return m_pInstance;
}

void
SimpleVehicleC::set_height_force( float32 f32Force )
{
	m_State.m_f32HeightForce += f32Force * 100.0f;
}

float32
SimpleVehicleC::get_height() const
{
	return m_State.m_f32Height;
}

float32
SimpleVehicleC::get_old_height() const
{
	return m_State.m_f32OldHeight;
}
