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

#include "stdafx.h"
#include "simulator.h"
#include "BoxCircle.h"

#include "const.h"
#include <math.h>

#include "robotHolonomic.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

#include "myglobals.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
IMPLEMENT_SERIAL(CBoxCircle, CBox, 1 )

CBoxCircle::CBoxCircle()
{
}

CBoxCircle::~CBoxCircle()
{
}

CBoxCircle::CBoxCircle(CRect rect)
{
	m_exist = TRUE;
	m_rect = rect;
	m_x = rect.CenterPoint().x; 
	m_y = rect.CenterPoint().y; 
	m_radius = rect.Size().cx / 2;
}

void CBoxCircle::Draw(CDC *pDC)
{
	m_rect = CRect(round(m_x-m_radius), round(m_y-m_radius), round(m_x+m_radius), round(m_y+m_radius));

	pDC->SelectStockObject(BLACK_PEN);
	pDC->SelectObject(&YELLOW);
	pDC->Ellipse(m_rect);
}


void CBoxCircle::Update(CArray<CRobot*, CRobot*> *robots, double dt, CMapPath *map)
{
	int i;

	double fx = 0;
	double fy = 0;
	double ftheta = 0;

	CBox::Update(robots, dt, map);
	m_mass = BoxMass;

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

	// Contact forces from robots that are transporting
	for(i=0; i<robots->GetSize(); i++) 
		(robots->GetAt(i))->ForceOnBoxes(this, fx, fy, ftheta );

	// Damping
	fx += -Cb * m_mass * m_v[0];
	fy += -Cb * m_mass * m_v[1];

	// Computing Velocities (vx, vy)
	m_v[0] += fx * dt / m_mass;
	m_v[1] += fy * dt / m_mass;

	// Saturation
	double factor;
	if ( (m_v[0] > 300) || (m_v[1] > 300) )
		if (m_v[0] > m_v[1]) {
			factor = m_v[0] / 300;
			m_v[0] = m_v[0] / factor;
			m_v[1] = m_v[1] / factor;
		}
		else{
			factor = m_v[1] / 300;
			m_v[1] = m_v[1] / factor;
			m_v[0] = m_v[0] / factor;
		}
	if ( (m_v[0] < -300) || (m_v[1] < -300) )
		if (m_v[0] < m_v[1]) {
			factor = m_v[0] / -300;
			m_v[0] = m_v[0] / factor;
			m_v[1] = m_v[1] / factor;
		}
		else{
			factor = m_v[1] / -300;
			m_v[1] = m_v[1] / factor;
			m_v[0] = m_v[0] / factor;
		}

	// Integrating Position
	m_x += m_v[0] * dt;
	m_y += m_v[1] * dt;
}


// Compute the force that are been applied in the box by the contact with obstacles
// For now, we are considering only circle obstacles
void CBoxCircle::ForceFromObstacles(CObstacle *obst, double &fx, double &fy)
{

	double dist;
	double angle;
	double deltaN;
	double deltaNDot;
	double lambdaN;

	if (!obst->m_erased) {

		// distance from circle obstacles
		DistanceCircleCircle(obst->m_center.x, obst->m_center.y, m_x, m_y, obst->m_radius, m_radius, dist, angle);
		// the force must be in the normal direction
		angle += PI; 

		// if dist < 0 (collision), compute the contact forces in the normal direction
		// using a spring-damper contact model.
		if (dist < 0) {
			deltaN = -dist;
			deltaNDot = m_v[0] * cos(angle) + m_v[1] * sin(angle); // velocity in the normal direction
			lambdaN = Kc_bo * deltaN - Cc_bo * deltaNDot;

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

void CBoxCircle::Serialize(CArchive& ar)
{
	CBox::Serialize(ar);
}