// RobotLabmate.cpp: implementation of the CRobotLabmate class.
//
//////////////////////////////////////////////////////////////////////

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

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


#define MAX_ARM_DISP 35


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
IMPLEMENT_SERIAL(CRobotLabmate, CRobotNonHolonomic, 1 )

// onstructor
CRobotLabmate::CRobotLabmate(double x, double y, double theta, short status, short id, CString name) : CRobotNonHolonomic(x, y, theta, status, id, name)
{
	m_sizex = 70;
	m_sizey = 30;
	m_beginLead = 0;
		
	for (short i=0; i<3; i++){
		m_force[i] = 0;
		m_force_w[i] = 0;
	}

	// transformations between World (W), Robot (R) amd Arm (A) coordinates
	m_tR2A[0] = -m_sizex/2;
	m_tR2A[1] = 0;
	m_tR2A[2] = 0;

	m_tA2R[0] = +m_sizex/2;
	m_tA2R[1] = 0;
	m_tA2R[2] = 0;

	// Position of the ARM in the three coordinates
	m_armA[0] = MAX_ARM_DISP;
	m_armA[1] = 0;
	m_armA[2] = 0;

	m_armR[0] = m_armA[0] * cos(m_tA2R[2]) - m_armA[1] * sin(m_tA2R[2]) + m_tA2R[0];
	m_armR[1] = m_armA[0] * sin(m_tA2R[2]) + m_armA[1] * cos(m_tA2R[2]) + m_tA2R[1];
	m_armR[2] = m_armA[2] + m_tA2R[2];

	m_armW[0] = m_armR[0] * cos(m_theta) - m_armR[1] * sin(m_theta) + m_x;
	m_armW[1] = m_armR[0] * sin(m_theta) + m_armR[1] * cos(m_theta) + m_y;
	m_armW[2] = m_armR[2] + m_theta;

	ComputeArmPos();

	double a = m_sizex/2, b = m_sizey; // robot dimensions
	m_polygon.AddPoint( CPoint( round( m_x - a*cos(m_theta) + b*sin(m_theta) ), round( m_y - a*sin(m_theta) - b*cos(m_theta) ) ) );
	m_polygon.AddPoint( CPoint( round( m_x + a*cos(m_theta) + b*sin(m_theta) ), round( m_y - b*cos(m_theta) ) ) );
	m_polygon.AddPoint( CPoint( round( m_x + a*cos(m_theta) - b*sin(m_theta) ), round( m_y + b*cos(m_theta) ) ) );
	m_polygon.AddPoint( CPoint( round( m_x - a*cos(m_theta) - b*sin(m_theta) ), round( m_y - a*sin(m_theta) + b*cos(m_theta) ) ) );
	m_polygon.ComputeCenter();
	m_polygon.SetPointsCenterFrame();
	m_rect = m_polygon.m_rect;
}

// Empty constructor
CRobotLabmate::CRobotLabmate()
{	
	m_sizex = 70;
	m_sizey = 30;
}

CRobotLabmate::~CRobotLabmate()
{

}

// Update robot's position
void CRobotLabmate::Update(CArray<CRobot*, CRobot*> *robots, double simTime, double dt, CBox* box, CMapPath *globalMap)
{
	CDataMsg *dataMsg;
	CControlMsg *controlMsg;

	// calling the sensor method (from CRobot)
	Sensor(globalMap,box);

	// If there is no box, Put the robot in the LEAD mode
	if (!box->m_exist)
		m_status = LEAD;

	// Docking
	if (m_status == DOCK) {
		m_v = 80;
		m_x += m_v * cos(m_theta) * dt;

		// If not in ontact, test if in contact
		if (!m_inContactBox) {
			DetectBox(box);
			
			// if in contact, Initialize arm position related to the box and send message DOCKOK
			if (m_inContactBox) {
				InitializeArmBox(CPoint(round(m_armW[0]), round(m_armW[1])), box, m_theta);

				m_v = 0;
				m_status = WAIT;
				controlMsg = new CControlMsg;
				controlMsg->m_code = DOCKOK;
				controlMsg->m_from = m_id;
				controlMsg->m_tstamp = simTime;
				SendControlMsg(controlMsg);
				return;
			}
		}
	}

	// Pushing the box. When the arm is in position, send message PUSHOK 
	if (m_status == PUSH) {
		if (m_armA[0] >= MAX_ARM_DISP - 10){
			m_v = 10;
			m_x += m_v * cos(m_theta) * dt;
		}
		else {
			m_status = WAIT;
			controlMsg = new CControlMsg;
			controlMsg->m_code = PUSHOK;
			controlMsg->m_tstamp = simTime;
			controlMsg->m_from = m_id;
			controlMsg->m_x = m_x;
			controlMsg->m_y = m_y;
			SendControlMsg(controlMsg);
		}
	}
	
	// If status is leading, the robot follow the planned path. If backing up, it has
	// a special planner. In both cases it broadcasts a data mesage to the other robots.
	if ( (m_status == LEAD) || (m_status == BACKUP) ){

		m_x += m_v * cos(m_theta) * dt;
		m_y += m_v * sin(m_theta) * dt;
		m_theta += m_w * dt;

		if (m_status == LEAD)
			FollowPath(simTime, m_localMap);
		else
			BackupManeuver(simTime, dt);

		// send data message each 0.01 "seconds"
		m_timer += dt;
		if ( m_timer > 0.01 ) {
			m_timer = 0;
			dataMsg = new CDataMsg;
			dataMsg->m_v = m_v;
			dataMsg->m_w = m_w;
			dataMsg->m_theta = m_theta;
			dataMsg->m_tstamp = simTime;
			SendDataMsg(dataMsg);
		}

	}
	// for the follow case, the velocity is set when the robot receives a data msg.
	else if (m_status == FOLLOW) {
	
		m_x += m_v * cos(m_theta) * dt;
		m_y += m_v * sin(m_theta) * dt;
		m_theta += m_w * dt;

	}

	// compute forces applied by the compliant arm
	ComputeForces(box);

	// recompute robot shape (polygon)
	m_polygon.Recompute(m_x, m_y, m_theta);
}


// compute forces applied by the compliant arm
void CRobotLabmate::ComputeForces(CBox* box)
{
	double k, c;
	double delta_p[3];	
	double delta_v[3];	
	double delta_va[3];
	double vbox_r[3];
	
	if ( (m_inContactBox) ) { 

		// computing arm position in the world frame, using the position of the box.
		m_armW[0] = m_armB[0] * cos(box->m_theta) - m_armB[1] * sin(box->m_theta) + box->m_x;
		m_armW[1] = m_armB[0] * sin(box->m_theta) + m_armB[1] * cos(box->m_theta) + box->m_y;
		m_armW[2] = m_armB[2] + box->m_theta;
		
		// Converting arm position from world coordinates to robot coordinates
		m_armR[0] = cos(m_theta) * m_armW[0] + sin(m_theta) * m_armW[1] - m_x * cos(m_theta) - m_y * sin(m_theta);
		m_armR[1] = -sin(m_theta) * m_armW[0] + cos(m_theta) * m_armW[1] - m_y * cos(m_theta) + m_x * sin(m_theta);
		m_armR[2] = m_armW[2] - m_theta;
		
		// Converting arm position from robot coordinates to arm coordinates
		m_armA[0] = m_armR[0] * cos(m_tR2A[2]) - m_armR[1] * sin(m_tR2A[2]) + m_tR2A[0];
		m_armA[1] = m_armR[0] * sin(m_tR2A[2]) + m_armR[1] * cos(m_tR2A[2]) + m_tR2A[1];
		m_armA[2] = m_armR[2] + m_tR2A[2];
		
		//Computing the arm displacements relative to the arm frame
		delta_p[0] =  MAX_ARM_DISP - m_armA[0];
		delta_p[1] =  0 - m_armA[1];
		delta_p[2] =  0 - m_armA[2];

		// Rotating box velocity from world coordinates to robot coordinates
		vbox_r[0] = cos(m_theta) * box->m_v[0] + sin(m_theta) * box->m_v[1];
		vbox_r[1] = -sin(m_theta) * box->m_v[0] + cos(m_theta) * box->m_v[1];
		vbox_r[2] = box->m_v[2];
		
		//Computing velocities relative to the robot
		delta_v[0] =  m_v - vbox_r[0];
		delta_v[1] =  -vbox_r[1];
		delta_v[2] =  -vbox_r[2];

		//Rotating resultant velocities to arm coordinates
		delta_va[0] = cos(m_tR2A[2]) * delta_v[0] - sin(m_tR2A[2]) * delta_v[1];
		delta_va[1] = sin(m_tR2A[2]) * delta_v[0] + cos(m_tR2A[2]) * delta_v[1];
		delta_va[2] = delta_v[2];

		k = 50;
		c = 1;
		
		m_force[0] = k * delta_p[0] + c * delta_va[0]; 
		m_force[1] = k * delta_p[1] + c * delta_va[1]; 
		m_force[2] = k * delta_p[2] + c * delta_va[2]; 
		
		if ( m_force[0] < 0 )	// the robots onlt apply positive forces in x
			m_force[0] = 0;
		
		// Putting the forces in world coordinates to compute the resultant forces.
		m_force_w[0] = m_force[0] * cos(m_theta - m_tR2A[2]) - m_force[1] * sin(m_theta - m_tR2A[2]);  //-m_tR2A[2] = +m_tA2R[2]
		m_force_w[1] = m_force[0] * sin(m_theta - m_tR2A[2]) + m_force[1] * cos(m_theta - m_tR2A[2]);
		m_force_w[2] = m_force[2];
	}
	else
		ComputeArmPos();
}

// Compute th arm position when the robots are not in contact with the box
void CRobotLabmate::ComputeArmPos()
{
	m_force_w[0] = 0;
	m_force_w[1] = 0;
	m_force_w[2] = 0;

	m_armA[0] = MAX_ARM_DISP;
	m_armA[1] = 0;
	m_armA[2] = 0;

	m_armR[0] = m_armA[0] * cos(m_tA2R[2]) - m_armA[1] * sin(m_tA2R[2]) + m_tA2R[0];
	m_armR[1] = m_armA[0] * sin(m_tA2R[2]) + m_armA[1] * cos(m_tA2R[2]) + m_tA2R[1];
	m_armR[2] = m_armA[2] + m_tA2R[2];

	m_armW[0] = m_armR[0] * cos(m_theta) - m_armR[1] * sin(m_theta) + m_x;
	m_armW[1] = m_armR[0] * sin(m_theta) + m_armR[1] * cos(m_theta) + m_y;
	m_armW[2] = m_armR[2] + m_theta;
}

// Draw the robot and the compliant arm
void CRobotLabmate::Draw(CDC *pDC)
{
	CPoint arm[5];

	if (m_tA2R[2] == 0) {
		arm[0] = m_polygon.GetPoint(1);
		arm[1] = CPoint( round( m_armW[0] + m_sizey * sin(m_armW[2]) ),
			             round( m_armW[1] - m_sizey * cos(m_armW[2]) ) );
		arm[2] = CPoint( round( m_armW[0] - m_sizey * sin(m_armW[2]) ),
			             round( m_armW[1] + m_sizey * cos(m_armW[2]) ) );
		arm[3] = m_polygon.GetPoint(2);
		arm[4] = arm[2];

	}
	else {
		double x_r = m_tA2R[0];
		double y_r = m_tA2R[1];

		double x_w = x_r * cos(m_theta) - y_r * sin(m_theta) + m_x;
		double y_w = x_r * sin(m_theta) + y_r * cos(m_theta) + m_y;

		arm[0] = CPoint( round( x_w ),
			             round( y_w ) );
		arm[1] = CPoint( round( m_armW[0] + m_sizey * sin(m_armW[2]) ),
			             round( m_armW[1] - m_sizey * cos(m_armW[2]) ) );
		arm[2] = CPoint( round( m_armW[0] - m_sizey * sin(m_armW[2]) ),
			             round( m_armW[1] + m_sizey * cos(m_armW[2]) ) );
		arm[3] = arm[0];