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

#include "stdafx.h"
#include "simulator.h"
#include "RobotHolonomic.h"
#include "RobotNonHolonomic.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(CRobotHolonomic, CRobot, 1 )

// Empty Constructor
CRobotHolonomic::CRobotHolonomic()
{
}

// Constructor
CRobotHolonomic::CRobotHolonomic(double x, double y, double theta, short status, short id, CString name) : CRobot(x, y, theta, status, id, name)
{
	m_vx = 0;
	m_vy = 0;
	m_radius = 10;
}

CRobotHolonomic::~CRobotHolonomic()
{
}

// Update robot's position
void CRobotHolonomic::Update(CArray<CRobot*, CRobot*> *robots, double simTime, double dt, CBox* box, CMapPath *globalMap)
{
	// call the update of the upper class
	CRobot::Update(robots, simTime, dt, box, globalMap);

	// Keep trak of robots position
	CPoint p = CPoint(round(m_x), round(m_y));
	if (m_path.GetSize() > 0) {
		if (p != m_path[m_path.GetSize()-1])
			m_path.Add(p);
	}
	else
		m_path.Add(p);


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

// Draw the robot
void CRobotHolonomic::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 CRobotHolonomic::Serialize(CArchive& ar)
{
	CRobot::Serialize(ar);

	if (ar.IsStoring()) {
		ar << m_vx;
		ar << m_vy;
	}
	else {
		ar >> m_vx;
		ar >> m_vy;
	}
}

// Potential ontroller. The robot is attrated by the goal and the box depending on its state.
// There is a repulsion from other robots and obstacles, an the collisions are also treated
// Constants are defined in the file myglobals.h, initialized  in the file simulator.cpp and 
// can be modified dynamically
void CRobotHolonomic::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 and contact 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 and contact 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) ) {

		DistanceCircleCircle(map->m_goalReal.x, map->m_goalReal.y, m_x, m_y, 0, 0, dist, angle);
		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,
	// and send a message to the other robots
	if ( (m_status == DOCK) && (fabs(m_vx) < 10) && ( fabs(m_vy) < 10) && (m_distBox < 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);
	}
	// If not all robots are ready, after timeout, send a message to other robots and start transportation
	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 lose contact with the box, stop and send a message to the others
	else if ( (box->m_exist) && (m_status == TRANSPORT) && (m_distBox > (m_sensorRange - m_radius) ) ) {
		m_status = STOPPED;

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


// Compute the fores due to the obstales
// Constants are defined in the file myglobals.h, initialized  in the file 
// simulator.cpp and can be modified dynamically
void CRobotHolonomic::ForceFromObstacles(CObstacle *obst, double &fx, double &fy)
{
	double angle;
	double deltaN;
	double deltaNDot;
	double lambdaN;

	if (obst->m_inRange) {
		angle = obst->m_angle + PI;
		
		if (obst->m_inContact) {
			// contact forces
			deltaN = obst->m_dist;
			deltaNDot = m_vx * cos(angle) + m_vy * sin(angle); // velocity in the normal direction
			lambdaN = Kc_ro * deltaN - Cc_ro * deltaNDot;

			fx +=  lambdaN * cos(angle);
			fy +=  lambdaN * sin(angle);
		}
		else if (obst->m_dist < m_sensorRange) {
			// potential forces (repulsion)
			fx += -Kr_ro * cos(angle-PI) * 1/fabs(obst->m_dist);
			fy += -Kr_ro * sin(angle-PI) * 1/fabs(obst->m_dist);
		}
	}
}

// Compute the forces due to the boxes
// Constants are defined in the file myglobals.h, initialized  in the file 
// simulator.cpp and can be modified dynamically
void CRobotHolonomic::ForceFromBoxes(CBox *box, double &fx, double &fy)
{
	double deltaN;
	double deltaNDot;
	double lambdaN;

	double angle = m_angleBox + PI;

	if ( (m_status == DOCK) || (m_status == WAIT) || ( (boxAttraction) && (m_status == TRANSPORT) ) )
		if (!m_inContactBox) {

			// Attractive force
			fx += Ka_rb * m_distBox * cos(angle-PI);
			fy += Ka_rb * m_distBox * sin(angle-PI);
		}
		
	if ( (m_inContactBox) ) {
		// Contact forces
		deltaN = m_distBox;
		deltaNDot = (m_vx - box->m_v[0]) * cos(angle) + (m_vy - box->m_v[1]) * sin(angle); // velocity in the normal direction
		lambdaN = Kc_rb * deltaN - Cc_rb * deltaNDot;

		fx += lambdaN * cos(angle);
		fy += lambdaN * sin(angle);
	}
}

// Forces and torque applied on the boxes by the robot
// Constants are defined in the file myglobals.h, initialized  in the file 
// simulator.cpp and can be modified dynamically
void CRobotHolonomic::ForceOnBoxes(CBox *box, double &fx, double &fy, double &ftheta)
{
	double deltaN;
	double deltaNDot;
	double lambdaN;
	double angle;
	double dist;

	double kw = 0.0001;  // constant for angular velocity of the box

	double fx_Box;
	double fy_Box;

	if (m_status == TRANSPORT) {
		if (m_inContactBox) {
			deltaN = m_distBox;
			angle = m_angleBox; // don't need to add PI here. m_angle box is computed in the robot frame!
				
			deltaNDot = (m_vx - box->m_v[0]) * cos(angle + PI) + (m_vy - box->m_v[1]) * sin(angle + PI); // velocity in the normal direction
			lambdaN = Kc_rb * deltaN - Cc_rb * deltaNDot;
			
			// The robot can never pull the box. So the force in the inverse normal direction 
			// of the box must be always positive
			if (lambdaN < 0)
				lambdaN = 0;

			fx_Box = lambdaN * cos(angle);
			fy_Box = lambdaN * sin(angle);

			fx += fx_Box;
			fy += fy_Box;

			// Computing torque
			DistanceCircleCircle(m_contactPoint.x, m_contactPoint.y, box->m_x, box->m_y, 0, 0, dist, angle);
			double ftheta_Box = fy_Box * cos(angle) -fx_Box * sin(angle);
			ftheta += kw * ftheta_Box * dist;
		}
	}
}

// Forces due to the other robots
// Constants are defined in the file myglobals.h, initialized  in the file 
// simulator.cpp and can be modified dynamically
void CRobotHolonomic::ForceFromRobots(CRobot *robot, double &fx, double &fy)
{
	double deltaN;
	double deltaNDot;
	double lambdaN;

	double dist;
	double angle;

	DistanceCircleCircle(robot->m_x, robot->m_y, m_x, m_y, robot->m_radius, m_radius, dist, angle);
	angle += PI;

	double vx;
	double vy;
			
	if (dist < 0) {

		// contact forces
		if (robot->IsKindOf( RUNTIME_CLASS(CRobotHolonomic) ) ) {
			vx = ( (CRobotHolonomic *) robot)->m_vx;
			vy = ( (CRobotHolonomic *) robot)->m_vy;
		}
		else {
			vx = ( (CRobotNonHolonomic *) robot)->m_v * cos(robot->m_theta);
			vy = ( (CRobotNonHolonomic *) robot)->m_v * sin(robot->m_theta);
		}
		deltaN = -dist;
		deltaNDot = (m_vx - vx) * cos(angle) + (m_vy - vy) * sin(angle); // velocity in the normal direction
		lambdaN = Kc_rr * deltaN - Cc_rr * deltaNDot;

		fx +=  lambdaN * cos(angle);
		fy +=  lambdaN * sin(angle);
	}
	else if (dist < m_sensorRange ){
		// potential forces
		fx += -Kr_rr * cos(angle-PI) * 1/fabs(dist);
		fy += -Kr_rr * sin(angle-PI) * 1/fabs(dist);
	}
}