/*************************************************************************
*                       MAIN CONTROL .CPP                                *
*************************************************************************/

#define _USE_MATH_DEFINES
#define DEFAULT_PORT	8101

// Algorithm definitions
#define ROBOT_NUM		4
#define STEPS_NUM		20
#define TURN_RATE		M_PI / 4
#define TARG_DIST		50
#define TARG_BEAR		0

// Preprocessor definitions and includes
#include "ctdbsdk.hpp"  // c-tree headers

// Other includes
#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
#include <math.h>
#include <time.h>
#include "opencv2/core/core.hpp"

// Struct to pass arguments in thread routines
struct data {
	int id;
} pid[ROBOT_NUM];

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

void *pid_init(void *arg);

// Function declarations
void Init(void);
void ManageTables(void);
void Update_Coords(CTTable *table, int i, double x, double y, double theta);
void Display_Coords(CTTable *table);
void Save_Control(CTTable *table, int row, int col, double val);
void Display_Control(CTTable *table);
void Save_Inputs(CTTable *table, int row, int col, double val);
void Display_Inputs(CTTable *table);
void Delete_Records(CTTable *table);
void Cleanup(void);
void Check_Table_Mode(CTTable *);
void Handle_Exception(CTException);

int main (NINT argc, pTEXT argv[]) {
	int i, j;
	float e[ROBOT_NUM];
	double startX = 0.0, startY = 0.0, startTh = 0.0;
	pthread_t p_id[ROBOT_NUM];

	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());
	}
/***********************************************************************************************/
	// ALGORITHM PREPARATIONS
	srand (time(NULL));

	// Starting state  -> p_init: 2 * ROBOT_NUM x 1
	cv::Mat p_init(2 * ROBOT_NUM, 1, CV_64F);
	float partition = 10000 / ROBOT_NUM;
	for (i = 0, j = 1; i < 2 * ROBOT_NUM && j <= ROBOT_NUM; i = i + 2, j++) {
		// y = [0, 5000]
		float random = ((float) rand()) / (float) RAND_MAX;
		startX = i * partition;
		startY = 10000 * random;
		startTh = 180 * random;

		p_init.at<double>(i, 0) = startX;
		p_init.at<double>(i + 1, 0) = startY;

		// Update database
		Update_Coords(&TCoord, j, startX, startY, startTh);
	}

	// Final state -> p_final: 2 * ROBOT_NUM x 1
	cv::Mat p_final(2 * ROBOT_NUM, 1, CV_64F);
	for (i = 0; i < 2 * ROBOT_NUM; i = i + 2) {
		p_final.at<double>(i, 0) = 5000;
		p_final.at<double>(i + 1, 0) = 5000;
	}

	// Unique parameters e[i] = [0.5, 1.5]
	float part = 1.0 / (ROBOT_NUM - 1);
	for (i = 0; i < ROBOT_NUM; i++) {
		e[i] = (i * part) + 0.5;
		//printf("e%d = %f\n", i, e[i]);
	}
	double min = ROBOT_NUM, max = 0;
	for (i = 0; i < ROBOT_NUM; i++) {
		for (j = i + 1; j < ROBOT_NUM; j++) {
			if (fabs(e[i] - e[j]) < min) {
				min = fabs(e[i] - e[j]);
			}
		}
		if (e[i] > max) {
			max = e[i];
		}
	}
	if (STEPS_NUM < ((2 * M_PI) / min)) {
		printf("Error! Number of steps too small!\n");
		printf("Min steps are %d\n", (int) ceil(((2 * M_PI) / min)));
		Cleanup();
		exit(0);
	}
	if (TURN_RATE > (M_PI / max)) {
		printf("Error! Turn rate too big!\n");
		printf("max is %f\n", (M_PI / max));
		Cleanup();
		exit(0);
	}

	// Desired Final Position -> p_des: 2 * ROBOT_NUM x 1
	cv::Mat p_des(2 * ROBOT_NUM, 1, CV_64F);
	for (int j = 0, i = 0; i < 2 * ROBOT_NUM && j < ROBOT_NUM; i = i + 2, j++) {
		p_des.at<double>(i, 0) = p_final.at<double>(i, 0) - (cos(startTh + (e[j] * STEPS_NUM * TURN_RATE) + TARG_BEAR) * TARG_DIST);
		p_des.at<double>(i + 1, 0) = p_final.at<double>(i + 1, 0) - (sin(startTh + (e[j] * STEPS_NUM * TURN_RATE) + TARG_BEAR) * TARG_DIST);
	}

	// Controllability Matrix C: 2n x k
	cv::Mat C;
	C.create(2 * ROBOT_NUM, STEPS_NUM, CV_64F);
	for (int k = 0; k < STEPS_NUM; k++) {
		for (int j = 0, i = 0; j < 2 * ROBOT_NUM && i < ROBOT_NUM; j = j + 2, i++) {
			C.at<double>(j, k) = cos(startTh + (e[i] * k * TURN_RATE));
			C.at<double>(j + 1, k) = sin(startTh + (e[i] * k * TURN_RATE));
			// Save Controllability Matrix to Database
			Save_Control(&TControl, j + 1, k, C.at<double>(j, k));
			Save_Control(&TControl, j + 2, k, C.at<double>(j + 1, k));
		}
	}

	cv::Mat u(STEPS_NUM, 1, CV_64F), sub(2 * ROBOT_NUM, 1, CV_64F), C_inv(STEPS_NUM, 2 * ROBOT_NUM, CV_64F);
	cv::subtract(p_des, p_init, sub, cv::noArray(), -1);
	cv::invert(C, C_inv, cv::DECOMP_SVD);

	// Control Inputs u: k x 1
	u = C_inv * sub;
	for (i = 0; i < STEPS_NUM; i++) {
		// Save Control Inputs to Database
		Save_Inputs(&TControlIn, i + 1, 0, u.at<double>(i, 0));
	}
/***********************************************************************************************/
	//system("gnome-terminal -x sh -c '/usr/local/MobileSim/MobileSim -R p3dx --nomap ' &");


	// Create threads
	for (i = 0; i < ROBOT_NUM; i++) {
		pid[i].id = i;

		if ((pthread_create(&p_id[i], NULL, pid_init, &pid[i])) != 0) {
			printf("Error! Can't create thread!\n");
			Cleanup();
			exit(1);
		}
	}

	// Wait for threads
	for (i = 0; i < ROBOT_NUM; i++) {
		if (pthread_join(p_id[i], NULL)) {
			printf("Error! Thread can't join!\n");
			Cleanup();
			exit(1);
		}
	}

	printf("\nPress <ENTER> key to exit . . .\n");
	getchar();

	Cleanup();

	return(0);
}

void Init(void) {
	CTBOOL create_session = NO, create_database = NO;
	printf("\nINITIALIZING..\n");

	try {
		ctdbStartDatabaseEngine();

		// connect to server
		MySession.Logon("FAIRCOMS", "ADMIN", "ADMIN");
		printf("Login to server DONE..\n");
	}
	catch (const CTException &E) {
		if (E.GetErrorCode() == FNOP_ERR) {
			create_session = YES;
		}
		else {
			Handle_Exception(E);
		}
	}

	if (create_session) {
		try {
			// create new session
			MySession.Create("FAIRCOMS", "ADMIN", "ADMIN");
			printf("New Session created..\n");
			// 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) {
		if (E.GetErrorCode() == INOT_ERR) {
			create_database = YES;
		}
		else {
			Handle_Exception(E);
		}
	}

	if (create_database) {
		try {
			// create a new database MyDatabase
			MySession.CreateDatabase("MyDatabase", "");
			printf("New Database created..\n");
			// connect to the database
			MyDatabase.Connect("MyDatabase");
			printf("Database connected..\n");
		}
		catch (const CTException &E) {
			Handle_Exception(E);
		}
	}
}

void ManageTables(void) {
	char str[32];
	CTBOOL create_table = NO;
	CTRecord MyRecord1(TCoord);
	CTRecord MyRecord2(TControl);
	CTRecord MyRecord3(TControlIn);

	printf("\nMANAGING TABLES..\n");

	try {
		// add TCoord
		MyDatabase.AddTable("TCoord", "");
		printf("TCoord added..\n");
		MyRecord1.SetDefaultIndex(CTDB_ROWID_IDXNO);
		TCoord.Open("TCoord", CTOPEN_NORMAL);
		printf("TCoord opened..\n");
		Delete_Records(&TCoord);
	}
	catch(...) {
		create_table = YES;
	}

	if (create_table) {
		// create TCoord
		try {
			TCoord.AddField("ID", CT_INTEGER, 1);
			TCoord.AddField("X", CT_DOUBLE, 8);
			TCoord.AddField("Y", CT_DOUBLE, 8);
			TCoord.AddField("Th", CT_DOUBLE, 8);
			TCoord.Create("TCoord", CTCREATE_NORMAL);
			printf("TCoord created..\n");
			MyRecord1.SetDefaultIndex(CTDB_ROWID_IDXNO);
			TCoord.Open("TCoord", CTOPEN_NORMAL);
			printf("TCoord opened..\n");
		}
		catch (const CTException &E) {
			Handle_Exception(E);
		}
		create_table = NO;
	}
	else {
		Check_Table_Mode(&TCoord);
	}

	// initialize TCoord with data
	try {
		for(int i = 0; i < ROBOT_NUM; i++) {
			MyRecord1.Clear();

			MyRecord1.SetFieldAsNumber(0, i + 1);
			MyRecord1.SetFieldAsFloat(1, -1);
			MyRecord1.SetFieldAsFloat(2, -1);
			MyRecord1.SetFieldAsFloat(3, -1);

			// add record
			MyRecord1.Write();
		}
	}
	catch(CTException &E) {
		Handle_Exception(E);
	}

	try {
		// add TControl
		MyDatabase.AddTable("TControl", "");
		printf("TControl added..\n");
		MyRecord2.SetDefaultIndex(CTDB_ROWID_IDXNO);
		TControl.Open("TControl", CTOPEN_NORMAL);
		printf("TControl opened..\n");
		Delete_Records(&TControl);
	}
	catch(...) {
		create_table = YES;
	}

	if (create_table) {
		// create TControl
		try {
			for (int i = 0; i < STEPS_NUM; i++) {
				sprintf(str, "Field%d", i);
				TControl.AddField(str, CT_DOUBLE, 8);
			}
			TControl.Create("TControl", CTCREATE_NORMAL);
			printf("TControl created..\n");
			MyRecord2.SetDefaultIndex(CTDB_ROWID_IDXNO);
			TControl.Open("TControl", CTOPEN_NORMAL);
			printf("TControl opened..\n");
		}
		catch (const CTException &E) {
			Handle_Exception(E);
		}
		create_table = NO;
	}
	else {
		Check_Table_Mode(&TControl);
	}

	// initialize TControl with data
	try {
		for(int i = 0; i < 2 * ROBOT_NUM; i++) {
			MyRecord2.Clear();
			for (int j = 0; j < STEPS_NUM; j++) {
				MyRecord2.SetFieldAsFloat(j, -1);
			}
			// add record
			MyRecord2.Write();
		}
	}
	catch(CTException &E) {
		Handle_Exception(E);
	}

	try {
		// add TControlIn
		MyDatabase.AddTable("TControlIn", "");
		printf("TControlIn added..\n");
		MyRecord3.SetDefaultIndex(CTDB_ROWID_IDXNO);
		TControlIn.Open("TControlIn", CTOPEN_NORMAL);
		printf("TControlIn opened..\n");
		Delete_Records(&TControlIn);
	}
	catch(...) {
		create_table = YES;
	}

	if (create_table) {
		// create TControlIn
		try {
			for (int i = 0; i < ROBOT_NUM; i++) {
				sprintf(str, "Field%d", i);
				TControlIn.AddField(str, CT_DOUBLE, 8);
			}
			TControlIn.Create("TControlIn", CTCREATE_NORMAL);
			printf("TControlIn created..\n");
			MyRecord3.SetDefaultIndex(CTDB_ROWID_IDXNO);
			TControlIn.Open("TControlIn", CTOPEN_NORMAL);
			printf("TControlIn opened..\n");
		}
		catch (const CTException &E) {
			Handle_Exception(E);
		}
		create_table = NO;
	}
	else {
		Check_Table_Mode(&TControlIn);
	}

	// initialize TControlIn with data
	try {
		for(int i = 0; i < STEPS_NUM; i++) {
			MyRecord3.Clear();
			for (int j = 0; j < ROBOT_NUM; j++) {
				MyRecord3.SetFieldAsFloat(j, -1);
			}
			// add record
			MyRecord3.Write();
		}
	}
	catch(CTException &E) {
		Handle_Exception(E);
	}
}

void Update_Coords(CTTable *table, int i, double x, double y, double theta) {
	try {
		CTRecord MyRecord(table);
		MyRecord.Clear();
		MyRecord.Lock(CTLOCK_WRITE_BLOCK);

		//printf("\nData updating!\n");
		if (MyRecord.FindRowid(i, CTFIND_EQ)) {
			MyRecord.SetFieldAsFloat(1, x);
			MyRecord.SetFieldAsFloat(2, y);
			MyRecord.SetFieldAsFloat(3, theta);

			MyRecord.Write();
			//printf("\nData updated!\n");
		}
		else {
			printf("\nNo Record found with id %d!\n", i);
		}
		MyRecord.Unlock();
	}
	catch(const CTException &E) {
		Handle_Exception(E);
	}
}

void Display_Coords(CTTable *table) {
   CTBOOL found;
   int id;
   double x, y, theta;

   try {
	   CTRecord	MyRecord(table);
	   // read first record
	   found = MyRecord.First();
	   while (found) {
		   id = MyRecord.GetFieldAsShort(0);
		   x = MyRecord.GetFieldAsFloat(1);
		   y = MyRecord.GetFieldAsFloat(2);
		   theta = MyRecord.GetFieldAsFloat(3);

		   printf("ID: %d -> X:	%f, Y:	%f, Theta:	%f\n", id, x, y, theta);

		   // read next record
		   found = MyRecord.Next();
	   }
   }
   catch(CTException &E) {
	   Handle_Exception(E);
   }
}

void Save_Control(CTTable *table, int row, int col, double val) {
	try {
		CTRecord MyRecord(table);
		MyRecord.Clear();
		MyRecord.Lock(CTLOCK_WRITE_BLOCK);

		//printf("\nData updating!\n");
		if (MyRecord.FindRowid(row, CTFIND_EQ)) {
			MyRecord.SetFieldAsFloat(col, val);

			MyRecord.Write();
		}
		else {
			printf("\nNo Record found with in [%d, %d]!\n", row, col);
		}
		MyRecord.Unlock();
	}
	catch (const CTException &E) {
		Handle_Exception(E);
	}
}

void Display_Control(CTTable *table) {
	CTBOOL found;
	double val;

	try {
		CTRecord MyRecord(table);
		// read first record
		found = MyRecord.First();
		printf("Ck\n");
		while (found) {
			printf("Row ID %d: ", (int) MyRecord.GetRowid());
			for (int i = 0; i < STEPS_NUM; i++){
				val = MyRecord.GetFieldAsFloat(i);
				printf(" %f ", val);
			}
			// read next record
			found = MyRecord.Next();
			printf("\n");
		}
	}
	catch(CTException &E) {
		Handle_Exception(E);
	}
}

void Save_Inputs(CTTable *table, int row, int col, double val){
	try {
		CTRecord MyRecord(table);
		MyRecord.Clear();
		MyRecord.Lock(CTLOCK_WRITE_BLOCK);

		//printf("\nData updating!\n");
		if (MyRecord.FindRowid(row, CTFIND_EQ)) {
			MyRecord.SetFieldAsFloat(col, val);

			MyRecord.Write();
		}
		else {
			printf("\nNo Record found with in [%d, %d]!\n", row, col);
		}
		MyRecord.Unlock();
	}
	catch (const CTException &E) {
		Handle_Exception(E);
	}
}

void Display_Inputs(CTTable *table) {
	CTBOOL found;
	double val;

	try {
		CTRecord MyRecord(table);
		// read first record
		found = MyRecord.First();
		printf("u\n");
		while (found) {
			printf("Row ID %d: ", (int) MyRecord.GetRowid());
			val = MyRecord.GetFieldAsFloat(0);
			printf("%f\n", val);

			// read next record
			found = MyRecord.Next();
		}
	}
	catch(CTException &E) {
		Handle_Exception(E);
	}
}

void Delete_Records(CTTable *table) {
	CTBOOL found;

	try {
		CTRecord MyRecord(table);
		// read first record
		found = MyRecord.First();

		while (found) {
			// delete record
			MyRecord.Delete();
			// read next record
			found = MyRecord.Next();
		}
	}
	catch(CTException &E) {
		Handle_Exception(E);
	}
}

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

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

		MyDatabase.DeleteTable("TCoord", "");
		MyDatabase.DeleteTable("TControl", "");
		MyDatabase.DeleteTable("TControlIn", "");
		printf("Tables 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);
}

void *pid_init(void *arg) {
	int ret;
	char str[128];
	int id;
	struct data *my_data;

	// Pass the arguments
	my_data = (struct data *) arg;
	id = my_data->id;

	sprintf(str, "gnome-terminal -x sh -c '/usr/local/MobileSim-new/MobileSim -p %d --nomap ' &", DEFAULT_PORT + id);
	ret = system(str);
	if (ret != 0) {
		printf("\nError! MobileSim didn't launch..!\n");
	}

	usleep(2000000);

	sprintf(str, "gnome-terminal -x sh -c '../../MobileRob/Release/MobileRob -rrtp %d %d'", DEFAULT_PORT + id, DEFAULT_PORT + id);
	ret = system(str);
	if (ret != 0) {
		printf("\nError! Robot didn't launch..!\n");
	}

	pthread_exit(NULL);
	return 0;
}

