//////////////////////////////////////////////////////////////////////
// MuRoS - Multi Robot Simulator
//
// Luiz Chaimowicz
// GRASP Lab. University of Pennsylvania
// VERLab - DCC - UFMG - Brasil
//
// RobotHolonomic.cpp: implementation of the CRobotHolonomic class.
//
// RobotXR4000.cpp: implementation of the CRobotXR4000 class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "simulator.h"
#include "RobotXR4000.h"
#include "const.h"
#include <math.h>

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif
#include "myglobals.h"


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
IMPLEMENT_SERIAL(CRobotXR4000, CRobotHolonomic, 1 )

CRobotXR4000::CRobotXR4000(double x, double y, double theta, short status, short id, CString name) : CRobotHolonomic(x, y, theta, status, id, name)
{
	m_radius = 25;
}

CRobotXR4000::CRobotXR4000()
{
}

CRobotXR4000::~CRobotXR4000()
{
}

void CRobotXR4000::Update(CArray<CRobot*, CRobot*> *robots, double simTime, double dt, CBox* box, CMapPath *globalMap)
{
	CRobot::Update(robots, simTime, dt, box, globalMap);

	Potential(robots, dt, globalMap, box, simTime);
		
	m_x += m_vx * dt;
	m_y += m_vy * dt;
}

void CRobotXR4000::Draw(CDC *pDC)
{
	CPoint line[2];

	m_rect = CRect( round(m_x - m_radius), round(m_y - m_radius), round(m_x + m_radius), round(m_y + m_radius) );

	pDC->SelectObject(&RED_PEN);

	if (m_status == TRANSPORT)
		pDC->SelectStockObject(BLACK_BRUSH);
	else if (m_status == DOCK)
		pDC->SelectStockObject(GRAY_BRUSH);
	else if (m_status == STOPPED)
		pDC->SelectStockObject(WHITE_BRUSH);
	else if (m_status == WAIT)
		pDC->SelectObject(&BLUE);

	pDC->Ellipse(m_rect);
}

void CRobotXR4000::Serialize(CArchive& ar)
{
	CRobotHolonomic::Serialize(ar);
}

void CRobotXR4000::Potential(CArray<CRobot*, CRobot*> *robots, double dt, CMapPath *map, CBox* box, double simTime)
{
	short i;

	double fx = 0;
	double fy = 0;

	double dist;
	double angle;

	// Repulsive forces from other robots
	for (i=0; i<robots->GetSize(); i++) 
		if (robots->GetAt(i)->m_id != m_id)
			ForceFromRobots(robots->GetAt(i), fx, fy);

	// Repulsive forces from obstacles
	for (i=0; i<m_localMap->m_obstacles.GetSize(); i++)
		ForceFromObstacles(m_localMap->m_obstacles[i], fx, fy);

	// Atractive force from box (Docking or waiting)
	// Repulsive if dist < 0 in all cases
	if (box->m_exist) {
		ForceFromBoxes(box, fx, fy);
	}
	else {
		m_status = TRANSPORT;
	}

	// Atractive force from goal
	if ( (m_localMap->m_goalReal.x != -1) && (m_status == TRANSPORT) ) {
		dist = sqrt( (map->m_goalReal.x - m_x) * (map->m_goalReal.x - m_x) + (map->m_goalReal.y - m_y) * (map->m_goalReal.y - m_y) );
		angle = atan2( (map->m_goalReal.y - m_y), (map->m_goalReal.x - m_x) );
		
		fx += Ka_rg * dist * cos(angle);
		fy += Ka_rg * dist * sin(angle);
	}

	// Damping
	fx += - Cr * m_vx;
	fy += - Cr * m_vy;

	// Computing velocities
	m_vx += fx * dt;
	m_vy += fy * dt;

	//Saturation (keeping scale between vx and vy)
	double factor;
	if ( (m_vx > 200) || (m_vy > 200) )
		if (m_vx > m_vy) {
		factor = m_vx / 200;
			m_vx = m_vx / factor;
			m_vy = m_vy / factor;
		}
		else{
			factor = m_vy / 200;
			m_vy = m_vy / factor;
			m_vx = m_vx / factor;
		}
	if ( (m_vx < -200) || (m_vy < -200) )
		if (m_vx < m_vy) {
			factor = m_vx / -200;
			m_vx = m_vx / factor;
			m_vy = m_vy / factor;
		}
		else{
			factor = m_vy / -200;
			m_vy = m_vy / factor;
			m_vx = m_vx / factor;
		}

	// Change to WAIT status if close to the box and the distance is small
	//dist = sqrt( (box->m_x - m_x) * (box->m_x - m_x) + (box->m_y - m_y) * (box->m_y - m_y) ) - box->m_lx - m_radius; //*****	
	if ( (m_status == DOCK) && (fabs(m_vx) < 1) && ( fabs(m_vy) < 1) && (dist < 5) ) {

		m_status = WAIT;
		m_timer = simTime;

		CControlMsg *controlMsg;
		controlMsg = new CControlMsg;
		controlMsg->m_code = BOXLOCKED;
		controlMsg->m_from = m_id;
		controlMsg->m_to = 0;
		controlMsg->m_x = m_x;
		controlMsg->m_y = m_y;
		
		SendControlMsg(controlMsg);
	}
	else if ( (m_status == WAIT) && ( (simTime - m_timer) > 10) ) {
		CControlMsg *controlMsg;
		controlMsg = new CControlMsg;
		controlMsg->m_code = TIMEOUT;
		controlMsg->m_from = m_id;
		controlMsg->m_to = 0;

		SendControlMsg(controlMsg);
	}

	if ( (box->m_exist) && (m_status == TRANSPORT) && (dist > m_sensorRange) ) {

		m_status = STOPPED;

		CControlMsg *controlMsg;
		controlMsg = new CControlMsg;
		controlMsg->m_code = LOSTBOX;
		controlMsg->m_from = m_id;
		controlMsg->m_to = 0;
		
		SendControlMsg(controlMsg);
	}

}