#include "stdafx.h"
#include "Actor.h"

Actor::Actor(b2World* pWorld, const b2Vec2& startPosition)
{
	//Define body
	b2BodyDef bd;
	bd.position.Set(startPosition.x, startPosition.y);
	bd.type = b2_dynamicBody;
	bd.allowSleep = false;
	bd.linearDamping = 0.01f;
	bd.angularDamping = 0.1f;
	m_pBody = pWorld->CreateBody(&bd); //Create body
	//Define shape
	b2CircleShape shape;
	shape.m_radius = 1.0f;
	//Create fixture
	m_pBody->CreateFixture(&shape, 0.0f);

	//Variables
	m_Target = startPosition;
}

Actor::~Actor()
{
}

void Actor::Update()
{
	KinematicSeekAndArrive();
	float angle = Orientation(m_pBody->GetAngle(), m_pBody->GetLinearVelocity());
	m_pBody->SetTransform(m_pBody->GetPosition(), angle);
}

void Actor::Render(Renderer* pRenderer)
{
	b2Vec2 pos = m_pBody->GetPosition();
	b2Vec2 vel = m_pBody->GetLinearVelocity();
	b2Vec2 dir = vel;
	dir.Normalize();
	dir *= 1.5f;

	pRenderer->DrawSegment(pos, pos + dir, b2Color(0, 1, 0));
	pRenderer->DrawString(pos + b2Vec2(1.0f, -1.0f), "x: %.2f, y: %.2f", vel.x, vel.y);
}

void Actor::KinematicSeekAndArrive()
{
	//Variables
	float radius = 1.0f;
	float maximumSpeed = 100.0f;
	float timeToTarget = 0.25f;

	//Calculate direction
	b2Vec2 dir = m_Target - m_pBody->GetPosition();
	if(dir.Length() < radius)
	{
		m_pBody->SetLinearVelocity(b2Vec2_zero);
		return;
	}
	b2Vec2 vel = b2Vec2(dir.x / timeToTarget, dir.y / timeToTarget);
	if(vel.Length() > maximumSpeed)
	{
		vel.Normalize();
		vel *= maximumSpeed;
	}
	m_pBody->SetLinearVelocity(vel);
	m_pBody->SetAngularVelocity(0.0f);
}

float Actor::Orientation(float currOr, const b2Vec2& vel)
{
	if(vel.Length() > 0)
	{
		b2Vec2 dir = vel;
		dir.Normalize();
		return atan2(-dir.x, dir.x);
	}
	return currOr;
}