#include <cstdlib>
#include <assert.h>
#include "SimpleAgent.h"

SimpleAgent::SimpleAgent(vector<Sensor *> sensorList, vector<Actuator *> actuatorList, vector<CommChannel *> channelList)
: Agent(sensorList, actuatorList, channelList),
m_sensorLeft(NULL),
m_sensorRight(NULL),
m_sensorUp(NULL),
m_sensorDown(NULL),
m_actuatorLeft(NULL),
m_actuatorRight(NULL),
m_actuatorUp(NULL),
m_actuatorDown(NULL),
m_channelLeft(NULL),
m_channelRight(NULL),
m_channelUp(NULL),
m_channelDown(NULL),
m_timeStep(0),
m_timeWave(0)
{
	int i;

	for (i = 0; i < (int) sensorList.size(); i++)
	{
		Sensor *sensor = sensorList[i];
		switch (sensor->getData()->orient)
		{
		case LEFT:
			m_sensorLeft = sensor;
			break;
		case RIGHT:
			m_sensorRight = sensor;
			break;
		case UP:
			m_sensorUp = sensor;
			break;
		case DOWN:
			m_sensorDown = sensor;
			break;
		}
	}

	for (i = 0; i < (int) actuatorList.size(); i++)
	{
		Actuator *actuator = actuatorList[i];
		switch (actuator->getData()->orient)
		{
		case LEFT:
			m_actuatorLeft = actuator;
			break;
		case RIGHT:
			m_actuatorRight = actuator;
			break;
		case UP:
			m_actuatorUp = actuator;
			break;
		case DOWN:
			m_actuatorDown = actuator;
			break;
		}
	}

	for (i = 0; i < (int) channelList.size(); i++)
	{
		CommChannel *channel = channelList[i];
		switch (channel->getOrient())
		{
		case LEFT:
			m_channelLeft = channel;
			break;
		case RIGHT:
			m_channelRight = channel;
			break;
		case UP:
			m_channelUp = channel;
			break;
		case DOWN:
			m_channelDown = channel;
			break;
		}
	}
}

SimpleAgent::~SimpleAgent()
{
}

void SimpleAgent::step()
{
	int rank;

	MPI_Comm_rank(MPI_COMM_WORLD, &rank);

	if (m_actuatorLeft) {
		float p = (float) (PI * sin(PI / 4 * 2 * local - m_timeStep * PI / 100));
		m_actuatorLeft->setTargetAngle(p);
	}
	if (m_actuatorRight) {
		float p = (float) (PI * sin(PI / 4 * (2 * local + 1) - m_timeStep * PI / 100));
		m_actuatorRight->setTargetAngle(p);
	}
	if (m_actuatorUp) {
		m_actuatorUp->setTargetAngle(0);
	}
	if (m_actuatorDown) {
		m_actuatorDown->setTargetAngle(0);
	}

	int x = 0;

	if(!m_channelLeft) {
		if(!m_timeStep)	x = 1;
	} else if(m_channelLeft->getChannel()->available()) {
		m_channelLeft->getChannel()->read(&x, MPI_INT, 1);
//		printf("[%d] received %d\n", rank, x);
		m_timeWave = m_timeStep;
		local = x;
	}
	if(m_channelRight && x) {
//		printf("[%d] sent %d\n", rank, x);
		x++;
		m_channelRight->getChannel()->write(&x, MPI_INT, 1);
	}

	m_timeStep++;
}