/*
 * Copyright (C) 2018 dimercur
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "tasks.h"
#include <stdexcept>

using namespace std;

// Déclaration des priorités des taches
constexpr int PRIORITY_TSERVER = 30;
constexpr int PRIORITY_TOPENCOMROBOT = 20;
constexpr int PRIORITY_TMOVE = 20;
constexpr int PRIORITY_TSENDTOMON = 22;
constexpr int PRIORITY_TRECEIVEFROMMON = 25;
constexpr int PRIORITY_TSTARTROBOT = 20;
constexpr int PRIORITY_TCAMERA = 21;
constexpr int PRIORITY_TBATTERY = 23;

/*
 * Some remarks:
 * 1- This program is mostly a template. It shows you how to create tasks, semaphore
 *   message queues, mutex ... and how to use them
 * 
 * 2- semDumber is, as name say, useless. Its goal is only to show you how to use semaphore
 * 
 * 3- Data flow is probably not optimal
 * 
 * 4- Take into account that ComRobot::Write will block your task when serial buffer is full,
 *   time for internal buffer to flush
 * 
 * 5- Same behavior existe for ComMonitor::Write !
 * 
 * 6- When you want to write something in terminal, use cout and terminate with endl
 * 
 * 7- Good luck !
 */

/**
 * @brief Initialisation des structures de l'application (tâches, mutex, 
 * semaphore, etc.)
 */
void Tasks::Init() {
	int status;
	int err;


	/* **************************************************************************************
	 * 	Mutex creation
	 * *************************************************************************************/
	if ((err = rt_mutex_create(&mutex_monitor, nullptr))) {
		cerr << "Error mutex create mutex_monitor: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if ((err = rt_mutex_create(&mutex_robot, nullptr))) {
		cerr << "Error mutex create mutex_robot: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if ((err = rt_mutex_create(&mutex_robotStarted, nullptr))) {
		cerr << "Error mutex create mutex_robotStarted: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if ((err = rt_mutex_create(&mutex_move, nullptr))) {
		cerr << "Error mutex create mutex_move: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if ((err = rt_mutex_create(&mutex_watchdogMode, nullptr))) {
		cerr << "Error mutex create mutex_watchdogMode: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	cout << "Mutexes created successfully" << endl;

	/* **************************************************************************************
	 * 	Semaphores creation
	 * *************************************************************************************/
	if ((err = rt_sem_create(&sem_barrier, nullptr, 0, S_FIFO))) {
		cerr << "Error semaphore create sem_barrier: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if ((err = rt_sem_create(&sem_openComRobot, nullptr, 0, S_FIFO))) {
		cerr << "Error semaphore create sem_openComRobot: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if ((err = rt_sem_create(&sem_serverOk, nullptr, 0, S_FIFO))) {
		cerr << "Error semaphore create sem_serverOk: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if ((err = rt_sem_create(&sem_startRobot, nullptr, 0, S_FIFO))) {
		cerr << "Error semaphore create sem_startRobot: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if ((err = rt_sem_create(&sem_stopRobot, nullptr, 0, S_FIFO))) {
		cerr << "Error semaphore create sem_stopRobot: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if ((err = rt_sem_create(&sem_stopComRobot, nullptr, 0, S_FIFO))) {
		cerr << "Error semaphore create sem_stopComRobot: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if ((err = rt_sem_create(&sem_stopServer, nullptr, 0, S_FIFO))) {
		cerr << "Error semaphore create sem_stopServer: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	cout << "Semaphores created successfully" << endl;

	/* *************************************************************************************
	 * Tasks creation
	 * *************************************************************************************/
	if ((err = rt_task_create(&th_server, "th_server", 0, PRIORITY_TSERVER, 0))) {
		cerr << "Error task create th_server: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if ((err = rt_task_create(&th_sendToMon, "th_sendToMon", 0, PRIORITY_TSENDTOMON, 0))) {
		cerr << "Error task create th_sendToMon: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if ((err = rt_task_create(&th_receiveFromMon, "th_receiveFromMon", 0, PRIORITY_TRECEIVEFROMMON, 0))) {
		cerr << "Error task create th_receiveFromMon: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if ((err = rt_task_create(&th_openComRobot, "th_openComRobot", 0, PRIORITY_TOPENCOMROBOT, 0))) {
		cerr << "Error task create th_openComRobot: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if ((err = rt_task_create(&th_startRobot, "th_startRobot", 0, PRIORITY_TSTARTROBOT, 0))) {
		cerr << "Error task create th_startRobot: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if ((err = rt_task_create(&th_move, "th_move", 0, PRIORITY_TMOVE, 0))) {
		cerr << "Error task create th_move: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}

	if ((err = rt_task_create(&th_sendBatteryLevel, "th_sendBatteryLevel", 0, PRIORITY_TBATTERY, 0))) {
		cerr << "Error task create th_sendBatteryLevel: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}

	cout << "Tasks created successfully" << endl;

	/* *************************************************************************************
	 * Message queues creation
	 * *************************************************************************************/
	if ((err = rt_queue_create(&q_messageToMon, "q_messageToMon", sizeof(Message *) * 50, Q_UNLIMITED, Q_FIFO)) < 0) {
		cerr << "Error msg queue create q_messageToMon: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	cout << "Queues created successfully" << endl;

}

/**
 * @brief Démarrage des tâches
 */
void Tasks::Run() {
	rt_task_set_priority(nullptr, T_LOPRIO);
	int err;

	if (err = rt_task_start(&th_server, reinterpret_cast<void (*)(void *)>(&Tasks::ServerTask), this)) {
		cerr << "Error task start ServerTask: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if (err = rt_task_start(&th_sendToMon, reinterpret_cast<void (*)(void *)>(&Tasks::SendToMonTask), this)) {
		cerr << "Error task start SendToMonTask: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if (err = rt_task_start(&th_receiveFromMon, reinterpret_cast<void (*)(void *)>(&Tasks::ReceiveFromMonTask), this)) {
		cerr << "Error task start ReceiveFromMonTask: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if (err = rt_task_start(&th_openComRobot, reinterpret_cast<void (*)(void *)>(&Tasks::OpenComRobot), this)) {
		cerr << "Error task start OpenComRobot: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if (err = rt_task_start(&th_startRobot, reinterpret_cast<void (*)(void *)>(&Tasks::StartRobotTask), this)) {
		cerr << "Error task start StartRobotTask: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if (err = rt_task_start(&th_move, reinterpret_cast<void (*)(void *)>(&Tasks::MoveTask), this)) {
		cerr << "Error task start MoveTask: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}
	if (err = rt_task_start(&th_sendBatteryLevel, reinterpret_cast<void (*)(void *)>(&Tasks::SendBatteryLevel), this)) {
		cerr << "Error task start SendBatteryLevel: " << strerror(-err) << endl;
		exit(EXIT_FAILURE);
	}

	cout << "Tasks launched" << endl;
}

/**
 * @brief Arrêt des tâches
 */
void Tasks::Stop() {
	monitor.Close();
	robot.Close();
}

/**
 */
void Tasks::Join() {
	cout << "Tasks synchronized" << endl;
	rt_sem_broadcast(&sem_barrier);
	pause();
}

/**
 * @brief Thread handling server communication with the monitor.
 */
void Tasks::ServerTask(void *arg) {
	int status;

	cout << "Start " << __PRETTY_FUNCTION__ << endl;
	// Synchronization barrier (waiting that all tasks are started)
	rt_sem_p(&sem_barrier, TM_INFINITE);

	/* **************************************************************************************
	 * The task server starts here
	 * *************************************************************************************/
	rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
	status = monitor.Open(SERVER_PORT);
	rt_mutex_release(&mutex_monitor);

	cout << "Open server on port " << (SERVER_PORT) << " (" << status << ")" << endl;

	if (status < 0)
		throw runtime_error{
				"Unable to start server on port " + to_string(SERVER_PORT)
		};
	monitor.AcceptClient(); // Wait the monitor client
	cout << "Rock'n'Roll baby, client accepted!" << endl;
	rt_sem_broadcast(&sem_serverOk);
}

/**
 * @brief Thread sending data to monitor.
 */
[[noreturn]] void Tasks::SendToMonTask(void *arg) {
	Message *msg;

	cout << "Start " << __PRETTY_FUNCTION__ << endl;
	// Synchronization barrier (waiting that all tasks are starting)
	rt_sem_p(&sem_barrier, TM_INFINITE);

	/* *************************************************************************************
	 * The task sendToMon starts here
	 * *************************************************************************************/
	rt_sem_p(&sem_serverOk, TM_INFINITE);

	while (true) {
		cout << "wait msg to send" << endl;
		msg = ReadInQueue(&q_messageToMon);
		cout << "Send msg to mon: " << msg->ToString() << endl;
		rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
		monitor.Write(msg); // The message is deleted with the Write
		rt_mutex_release(&mutex_monitor);
	}
}

/**
 * @brief Thread receiving data from monitor.
 */
[[noreturn]] void Tasks::ReceiveFromMonTask(void *arg) {
	Message *msgRcv;

	cout << "Start " << __PRETTY_FUNCTION__ << endl;
	// Synchronization barrier (waiting that all tasks are starting)
	rt_sem_p(&sem_barrier, TM_INFINITE);

	/* *************************************************************************************
	 * The task receiveFromMon starts here
	 * *************************************************************************************/
	rt_sem_p(&sem_serverOk, TM_INFINITE);
	cout << "Received message from monitor activated" << endl;

	while (true) {
		msgRcv = monitor.Read();
		cout << "Rcv <= " << msgRcv->ToString() << endl;

		if (msgRcv->CompareID(MESSAGE_MONITOR_LOST)) {
			cout << "Monitor connection lost! Stopping robot..." << endl;
//			rt_sem_v(&sem_stopCamera); // TODO
			rt_sem_v(&sem_stopRobot);
			rt_sem_v(&sem_stopComRobot);
			rt_sem_v(&sem_stopServer);
		} else if (msgRcv->CompareID(MESSAGE_ROBOT_COM_OPEN)) {
			rt_sem_v(&sem_openComRobot);
		} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITHOUT_WD)) {
			rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
			watchdogMode = WITHOUT_WATCHDOG;
			rt_mutex_release(&mutex_watchdogMode);
			rt_sem_v(&sem_startRobot);
		} else if (msgRcv->CompareID(MESSAGE_ROBOT_START_WITH_WD)) {
			// TODO: gérer la watchdog
			rt_mutex_acquire(&mutex_watchdogMode, TM_INFINITE);
			watchdogMode = WITH_WATCHDOG;
			rt_mutex_release(&mutex_watchdogMode);
			rt_sem_v(&sem_startRobot);
		} else if (msgRcv->CompareID(MESSAGE_ROBOT_GO_FORWARD) ||
		           msgRcv->CompareID(MESSAGE_ROBOT_GO_BACKWARD) ||
		           msgRcv->CompareID(MESSAGE_ROBOT_GO_LEFT) ||
		           msgRcv->CompareID(MESSAGE_ROBOT_GO_RIGHT) ||
		           msgRcv->CompareID(MESSAGE_ROBOT_STOP)) {

			rt_mutex_acquire(&mutex_move, TM_INFINITE);
			move = msgRcv->GetID();
			rt_mutex_release(&mutex_move);
		} else if (msgRcv->CompareID(MESSAGE_CAM_OPEN)) {
			// TODO
		} else if (msgRcv->CompareID(MESSAGE_CAM_CLOSE)) {
			// TODO
		} else if (msgRcv->CompareID(MESSAGE_CAM_ASK_ARENA)) {
			// TODO
		} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
			// TODO
		} else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) {
			// TODO
		} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_START)) {
			// TODO
		} else if (msgRcv->CompareID(MESSAGE_CAM_POSITION_COMPUTE_STOP)) {
			// TODO
		}
		delete (msgRcv); // mus be deleted manually, no consumer
	}
}

/**
 * @brief Thread opening communication with the robot.
 */
[[noreturn]] void Tasks::OpenComRobot(void *arg) {
	int status;
	int err;

	cout << "Start " << __PRETTY_FUNCTION__ << endl;
	// Synchronization barrier (waiting that all tasks are starting)
	rt_sem_p(&sem_barrier, TM_INFINITE);

	/* *************************************************************************************
	 * The task openComRobot starts here
	 * *************************************************************************************/
	while (true) {
		rt_sem_p(&sem_openComRobot, TM_INFINITE);
		cout << "Open serial com (";
		rt_mutex_acquire(&mutex_robot, TM_INFINITE);
		status = robot.Open();
		rt_mutex_release(&mutex_robot);
		cout << status;
		cout << ")" << endl;

		Message *msgSend;
		if (status < 0) {
			msgSend = new Message(MESSAGE_ANSWER_NACK);
		} else {
			msgSend = new Message(MESSAGE_ANSWER_ACK);
		}
		WriteInQueue(&q_messageToMon, msgSend); // msgSend will be deleted by sendToMon
	}
}

/**
 * @brief Thread starting the communication with the robot.
 */
[[noreturn]] void Tasks::StartRobotTask(void *arg) {
	cout << "Start " << __PRETTY_FUNCTION__ << endl;
	// Synchronization barrier (waiting that all tasks are starting)
	rt_sem_p(&sem_barrier, TM_INFINITE);

	/* *************************************************************************************
	 * The task startRobot starts here
	 * *************************************************************************************/
	while (true) {
		Message *msgSend;
		rt_sem_p(&sem_startRobot, TM_INFINITE);
		cout << "Start robot without watchdog (";
		rt_mutex_acquire(&mutex_robot, TM_INFINITE);
		msgSend = robot.Write(ComRobot::StartWithoutWD());
		rt_mutex_release(&mutex_robot);
		cout << msgSend->GetID();
		cout << ")" << endl;

		cout << "Movement answer: " << msgSend->ToString() << endl;
		WriteInQueue(&q_messageToMon, msgSend);  // msgSend will be deleted by sendToMon

		if (msgSend->GetID() == MESSAGE_ANSWER_ACK) {
			rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
			robotStarted = 1;
			rt_mutex_release(&mutex_robotStarted);
		}
	}
}


[[noreturn]] void Tasks::SendBatteryLevel(void *arg) {
	int rs;
	cout << "Start " << __PRETTY_FUNCTION__ << endl;
	// Synchronization barrier (waiting that all tasks are starting)
	rt_sem_p(&sem_barrier, TM_INFINITE);

	/* *************************************************************************************/
	/* The task sendBatteryLevel starts here                                               */
	/* *************************************************************************************/

	rt_task_set_periodic(nullptr, TM_NOW, 500000000); // 500 ms

	while (true) {
		rt_task_wait_period(nullptr);
		cout << "Periodic battery level update" << endl;

		rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
		rs = robotStarted;
		rt_mutex_release(&mutex_robotStarted);
		if (rs == 1) {
			rt_mutex_acquire(&mutex_robot, TM_INFINITE);
			auto msg = dynamic_cast<MessageBattery *>(robot.Write(ComRobot::GetBattery()));
			if (msg == nullptr) {
				cout << "NULLPTR" << endl;
			} else {
				WriteInQueue(&q_messageToMon, msg);
			}
			rt_mutex_release(&mutex_robot);
		}
	}

}

/**
 * @brief Thread handling control of the robot.
 */
[[noreturn]] void Tasks::MoveTask(void *arg) {
	int rs;
	int cpMove;
	int counter = 0;

	cout << "Start " << __PRETTY_FUNCTION__ << endl;
	// Synchronization barrier (waiting that all tasks are starting)
	rt_sem_p(&sem_barrier, TM_INFINITE);

	/* **************************************************************************************
	 * The task starts here
	 * *************************************************************************************/
	rt_task_set_periodic(nullptr, TM_NOW, 100000000); // 100 ms

	while (true) {
		rt_task_wait_period(nullptr);
		cout << "Periodic movement update" << endl;
		rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
		rs = robotStarted;
		rt_mutex_release(&mutex_robotStarted);
		if (rs == 1) {
			rt_mutex_acquire(&mutex_move, TM_INFINITE);
			cpMove = move;
			rt_mutex_release(&mutex_move);

			cout << "move: " << cpMove << endl;

			rt_mutex_acquire(&mutex_robot, TM_INFINITE);
			auto msgSend = robot.Write(new Message((MessageID) cpMove));
			if (msgSend->CompareID(MESSAGE_ANSWER_ACK)) {
				counter = 0;
			} else {
				counter ++;
			}
			if (counter == 3) {
				rt_sem_v(&sem_stopRobot);
				rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
				monitor.Write(new Message(MESSAGE_ANSWER_COM_ERROR));
				rt_mutex_release(&mutex_monitor);
			}
			rt_mutex_release(&mutex_robot);
		}
	}
}

/**
 * Write a message in a given queue
 * @param queue Queue identifier
 * @param msg Message to be stored
 */
void Tasks::WriteInQueue(RT_QUEUE *queue, Message *msg) {
	int err;
	if ((err = rt_queue_write(queue, (const void *) &msg, sizeof((const void *) &msg), Q_NORMAL)) < 0) {
		cerr << "Write in queue failed: " << strerror(-err) << endl;
		throw runtime_error{"Error in write in queue"};
	}
}

/**
 * Read a message from a given queue, block if empty
 * @param queue Queue identifier
 * @return Message read
 */
Message *Tasks::ReadInQueue(RT_QUEUE *queue) {
	int err;
	Message *msg;

	if ((err = rt_queue_read(queue, &msg, sizeof((void *) &msg), TM_INFINITE)) < 0) {
		cout << "Read in queue failed: " << strerror(-err) << endl;
		throw runtime_error{"Error in read in queue"};
	}/** else {
        cout << "@msg :" << msg << endl;
    } **/

	return msg;
}