/****************************************************************************
*                           MOBILE ROBOT THREAD                             *
****************************************************************************/

#define _USE_MATH_DEFINES
#define DEFAULT_PORT	8101

#include <Aria.h>
//#include <cmath>
#include "ctdbsdk.hpp"  // c-tree headers

// Global declarations
CTSession   MySession(CTSESSION_CTDB);
CTDatabase 	MyDatabase(MySession);
CTTable     TCoord(MyDatabase), TControl(MyDatabase), TControlIn(MyDatabase);

// Function declarations
void Init(void);
void ManageTables(void);
void Add_Records(int i, double x, double y, double theta);
double Get_Record(CTTable *table, int row, int col);
void Cleanup(void);
void Check_Table_Mode(CTTable *);
void Handle_Exception(CTException);

int main(int argc, char **argv) {
	int port, myID, steps;
	double startX=0, startY=0, startTh=0, nextX, nextY, b0, b1, u, a;

	Init();

	ManageTables();

	//if session is active, retrieve the server name, user logon name and user password
	if (MySession.IsActive()) {
		printf("\nServer name      : %s\n", MySession.GetServerName().c_str());
	}

	//if database is active, retrieve the database name and table count
	if (MyDatabase.IsActive()) {
		printf("Database         : %s\n", MyDatabase.GetName().c_str());
		printf("Connected tables : %d\n", MyDatabase.GetTableCount());
	}

	Aria::init();
	ArArgumentParser argParser(&argc, argv);
	argParser.loadDefaultArguments();
	ArRobot robot;
	ArRobotConnector robotConnector(&argParser, &robot);

	if(!robotConnector.connectRobot()) {
		ArLog::log(ArLog::Terse, "Could not connect to the robot.");
		if(argParser.checkHelpAndWarnUnparsed()) {
			// -help not given, just exit.
			Aria::logOptions();
			Aria::exit(1);
			return 1;
		}
	}


	// Trigger argument parsing
	if (!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed()) {
		Aria::logOptions();
		Aria::exit(1);
		return 1;
	}

	ArKeyHandler keyHandler;
	Aria::setKeyHandler(&keyHandler);
	robot.attachKeyHandler(&keyHandler);

	robot.runAsync(true);

	// Find connected port
	port = atoi(argParser.getArg(1));
	myID = port - DEFAULT_PORT + 1;
	printf("\n%d -> CONNECTED TO PORT: %d\n", myID, port);

	// turn on the motors, turn off amigobot sounds
	robot.enableMotors();
	robot.comInt(ArCommands::SOUNDTOG, 0);

	// position the robot in a specific place
	startX = Get_Record(&TCoord, myID, 1);
	startY = Get_Record(&TCoord, myID, 2);
	startTh = Get_Record(&TCoord, myID, 3);

	ArRobotPacket pkt;
	pkt.setID(ArCommands::SIM_SET_POSE);
	pkt.uByteToBuf(0); // argument type: ignored.
	pkt.byte4ToBuf((ArTypes::Byte4)startX);
	pkt.byte4ToBuf((ArTypes::Byte4)startY);
	pkt.byte4ToBuf((ArTypes::Byte4)startTh);
	pkt.finalizePacket();
	robot.lock();
	robot.getDeviceConnection()->writePacket(&pkt);
	robot.unlock();

	// to move the robot to the desired pose.
	ArPose myInitialPose;
	myInitialPose.setPose(startX, startY, startTh);
	robot.moveTo(myInitialPose);

	printf("\nSTARTING STATE:	%f, %f, %f\n", robot.getX(), robot.getY(), robot.getTh());

	// Start approaching desired final state
	steps = TControl.GetFieldCount();
	float currX = startX;
	float currY = startY;
	float currTh = startTh;

	for (int i = 0; i < steps; i++) {
		b0 = Get_Record(&TControl, ((2 * myID) - 1), i);
		b1 = Get_Record(&TControl, (2 * myID), i);
		u = Get_Record(&TControlIn, i + 1, 0);
		nextX = currX + b0 * u;
		nextY = currY + b1 * u;
		printf("Step %d: nextX: %f, nextY: %f\n", i, nextX, nextY);

		// Go to next position
		a = (atan2((nextY - currY), (nextX - currX)) * 180) / M_PI;

		if (a != 0) {
			robot.lock();
			robot.setRotVel(10);
			robot.unlock();
			while (fabs(a - robot.getTh()) > 2) {
				// Do nothing
				printf("Step %d ->a: %f, Th: %f, diff: %f\n", i, a, robot.getTh(), fabs(a - robot.getTh()));
			}
		}
		robot.lock();
		robot.setRotVel(0);
		robot.setVel(100);
		robot.unlock();
		if (a != 90 && a != -90) {
			while (fabs(nextX - robot.getX()) > 250) {
				// Do nothing
				printf("Step %d ->X: %f, Y: %f, diffX: %f, diffY: %f\n", i, robot.getX(), robot.getY(), fabs(nextX - robot.getX()), fabs(nextY - robot.getY()));
			}
		}
		else {
			while (fabs(nextY - robot.getY()) > 250) {
				// Do nothing
				printf("Step %d ->X: %f, Y: %f, diffX: %f, diffY: %f\n", i, robot.getX(), robot.getY(), fabs(nextX - robot.getX()), fabs(nextY - robot.getY()));
			}
		}
		robot.lock();
		robot.setVel(0);
		robot.unlock();

		//currX = nextX;
		//currY = nextY;
		currX = robot.getX();
		currY = robot.getY();
		currTh = robot.getTh();

		Add_Records(myID, currX, currY, currTh);
		//printf("Step %d: actualX: %f, actualY: %f\n", i, robot.getX(), robot.getY());


	}
	printf("\nPOSITION REACHED ! ! !\n");

	// wait for robot task loop to end before exiting the program
	robot.waitForRunExit();

	Aria::exit(0);

	return 0;
}

void Init(void) {
	printf("\nINITIALIZING..\n");

	try {
		ctdbStartDatabaseEngine();

		// connect to server
		MySession.Logon("FAIRCOMS", "ADMIN", "ADMIN");
		printf("Login to server DONE..\n");
	}
	catch (const CTException &E) {
		Handle_Exception(E);
	}

	try {
		// connect to the database
		MyDatabase.Connect("MyDatabase");
		printf("Database connected..\n");
	}
	catch (const CTException &E) {
		Handle_Exception(E);
	}
}

void ManageTables(void) {
	printf("\nMANAGING TABLE..\n");
	CTRecord MyRecord1(TCoord);
	CTRecord MyRecord2(TControl);
	CTRecord MyRecord3(TControlIn);

	// Open TCoord
	try {
		MyRecord1.SetDefaultIndex(CTDB_ROWID_IDXNO);
		TCoord.Open("TCoord", CTOPEN_NORMAL);
		printf("TCoord opened..\n");
	}
	catch(const CTException &E) {
		Handle_Exception(E);
	}
	Check_Table_Mode(&TCoord);

	// TControl
	try {
		MyRecord2.SetDefaultIndex(CTDB_ROWID_IDXNO);
		TControl.Open("TControl", CTOPEN_NORMAL);
		printf("TControl opened..\n");
	}
	catch(const CTException &E) {
		Handle_Exception(E);
	}
	Check_Table_Mode(&TControl);

	// Open TControlIn
	try {
		MyRecord3.SetDefaultIndex(CTDB_ROWID_IDXNO);
		TControlIn.Open("TControlIn", CTOPEN_NORMAL);
		printf("TControlIn opened..\n");
	}
	catch(const CTException &E) {
		Handle_Exception(E);
	}
	Check_Table_Mode(&TControlIn);
}

void Add_Records(int i, double x, double y, double theta) {
	try {
		CTRecord MyRecord(TCoord);
		MyRecord.Clear();
		MyRecord.Lock(CTLOCK_WRITE_BLOCK);

		if (MyRecord.FindRowid(i, CTFIND_EQ)) {
			MyRecord.SetFieldAsFloat(1, x);
			MyRecord.SetFieldAsFloat(2, y);
			MyRecord.SetFieldAsFloat(3, theta);

			MyRecord.Write();
		}
		MyRecord.Unlock();
	}
	catch(const CTException &E) {
		Handle_Exception(E);
	}
}

double Get_Record(CTTable *table, int row, int col) {
	double val;
	try {
		CTRecord MyRecord(table);
		MyRecord.Clear();

		if (MyRecord.FindRowid(row, CTFIND_EQ)) {
			val = MyRecord.GetFieldAsFloat(col);
		}
		else {
			val = -1;
			printf("\nError! Element [%d, %d] missing!\n", row, col);
		}
	}
	catch(const CTException &E) {
		Handle_Exception(E);
	}

	return val;
}

void Cleanup(void) {
	printf("CLOSING DOWN..\n");
	try {
		TCoord.ResetAll();

		// close TCoord
		TCoord.Close();
		TControl.Close();
		TControlIn.Close();
		printf("Tables closed..\n");

		//MyDatabase.DeleteTable("TCoord", "");
		//printf("Table deleted..\n");

		// disconnect MyDatabase
		MyDatabase.Disconnect();
		printf("Database Disconnected..\n");

		// delete MyDatabase
		MySession.DeleteDatabase("MyDatabase");
		printf("Database Deleted..\n");

		// Session logout
		MySession.Logout();
		printf("Logout DONE..\n");
	}
	catch (const CTException &E) {
		Handle_Exception(E);
	}
}

void Check_Table_Mode(CTTable *table) {
	try {
		// get table create mode
		CTCREATE_MODE mode = table->GetCreateMode();

		// check if table is under transaction processing control
		if ((mode & CTCREATE_TRNLOG)) {
			// change file mode to disable transaction processing
			mode ^= CTCREATE_TRNLOG;
			table->UpdateCreateMode(mode);
		}
	}
	catch (const CTException &E) {
		Handle_Exception(E);
	}
}

void Handle_Exception(CTException err) {
   printf ("\nERROR: [%d] - %s\n", err.GetErrorCode(), err.GetErrorMsg());
   printf("*** Execution aborted *** \nPress <ENTER> key to exit...\n");
   getchar();
   Cleanup();

   exit(1);
}

