//
// This code is intended to show users how to develop a JAUS
// Primitive Driver (PD) client using the OpenJAUS SDK and
// should be used in conjunction with the Primitive Driver
// tutorial found at http://www.openjaus.com/.
//
#include <signal.h>
#include <openjaus.h>
#include "openjaus/core.h"
#include "openjaus/mobility/PrimitiveDriver.h"

#define APP_VERSION "4.1"

using namespace openjaus;

// Declare the message callbacks to be used by the client component.
void processControlResponse(const model::ControlResponse& response);
bool processReportStatus(core::ReportStatus &report);
bool processReportWrenchEffort(mobility::ReportWrenchEffort &report);

void printMenu();
void sendMessage(core::Base& component, const std::vector<transport::Address>& pdList, model::Message* message);

static bool mainRunning = false;
void signalHandler(int n);

int main(void)
{
	std::vector<transport::Address> pdList;
	uint32_t periodicSubscriptionId = 0;
	uint32_t onChangeSubscriptionId = 0;

	try
	{
		// Create the component
		core::Base component;
		component.setName("PdClientDemo");

		// Add the callbacks that will execute when specific messages are received.
		component.addMessageCallback(processReportWrenchEffort);
		component.addMessageCallback(processReportStatus);

		// Enable terminal mode
		system::Application::setTerminalMode();

		std::cout << "Thank you for using the OpenJAUS SDK v" << OPENJAUS_SDK_VERSION << std::endl;
		std::cout << "PrimitiveDriverClientDemo v" << APP_VERSION << std::endl;

		printMenu();

		// Setup our signal handlers
		signal(SIGTERM, signalHandler);
		signal(SIGINT, signalHandler);

		// Call Run on component, this starts the StateMachineRunner thread to process messages
		component.run();

		mainRunning = true;
		unsigned char choice = 0;
		while(mainRunning)
		{
			choice = system::Application::getChar();
			switch(choice)
			{
				case system::Application::ASCII_ESC:
					mainRunning = false;
					break;

				case '?': // Print Menu
					printMenu();
					break;

				case 't': // Print System Tree
					std::cout << component.getSystemTree();
					break;

				case 'f': // Find Primitive Driver
					pdList = component.getSystemTree()->lookupService(mobility::PrimitiveDriverInterface::PrimitiveDriverUri());

					std::cout << "PD Services (" << pdList.size() << "):\n";
					for(size_t i = 0; i < pdList.size(); i++)
					{
						std::cout << "\t" << pdList.at(i).toString() << std::endl;
					}
					break;

				case '1': // Create Periodic Event (Report Wrench Effort)
					if (pdList.size() > 0)
					{
						mobility::QueryWrenchEffort *query = new mobility::QueryWrenchEffort();

						query->setQueryPresenceVector(mobility::ReportWrenchEffort::PV_ALL_FIELDS);

						// Magic Number [10]: 10 is the requested rate (in Hz) of the periodic event
						periodicSubscriptionId = component.subscribePeriodic(pdList.at(0), query, 10.0);

						std::cout << "Created Periodic Event: " << periodicSubscriptionId << std::endl;
					}
					break;

				case '2': // Stop Periodic Event (Report Wrench Effort)
					if(periodicSubscriptionId != 0)
					{
						std::cout << "Un-subscribing Periodic Event: " << periodicSubscriptionId << std::endl;
						if(component.unsubscribe(periodicSubscriptionId))
						{
							periodicSubscriptionId = 0;
						}
					}
					break;

				case '3': // Create On-Change Event (Report Wrench Effort)
					if(pdList.size() > 0)
					{
						mobility::QueryWrenchEffort *query = new mobility::QueryWrenchEffort();
						query->setQueryPresenceVector(mobility::ReportWrenchEffort::PV_ALL_FIELDS);
						onChangeSubscriptionId = component.subscribeOnChange(pdList.at(0), query);
						std::cout << "Created On-Change Event: " << onChangeSubscriptionId << std::endl;
					}
					break;

				case '4': // Cancel On-Change Event (Report Wrench Effort)
					if(onChangeSubscriptionId)
					{
						std::cout << "Un-subscribing OnChange Event: " << onChangeSubscriptionId << std::endl;
						if (component.unsubscribe(onChangeSubscriptionId))
						{
							onChangeSubscriptionId = 0;
						}
					}
					break;

				case '5': // Request PD Control
					if(pdList.size() > 0)
					{
						component.requestControl(pdList.at(0), processControlResponse);
						std::cout << "Requesting Control of PD at " << pdList.at(0) << std::endl;
					}
					break;

				case '6': // Release PD Control
					if(pdList.size() > 0)
					{
						try
						{
							component.releaseControl(pdList.at(0));
							std::cout << "Sent Release Control to " << pdList.at(0) << std::endl;
						}
						catch(system::Exception &e)
						{
							std::cout << e.toString() << std::endl;
						}
					}
					break;

				case 'q': // Query Status
				{
					std::cout << "Sending Query Status" << std::endl;

					core::QueryStatus* query = new core::QueryStatus();
					sendMessage(component, pdList, query);
					break;
				}

				case 'a': // Reset
				{
					std::cout << "Sending Reset" << std::endl;

					core::Reset *reset = new core::Reset();
					sendMessage(component, pdList, reset);
					break;
				}

				case 's': // Resume
				{
					std::cout << "Sending Resume" << std::endl;

					core::Resume *resume = new core::Resume();
					sendMessage(component, pdList, resume);
					break;
				}

				case 'd': // Set Emergency
				{
					std::cout << "Sending Set Emergency" << std::endl;

					core::SetEmergency *setEmergency = new core::SetEmergency();
					sendMessage(component, pdList, setEmergency);
					break;
				}

				case 'z': // Clear Emergency
				{
					std::cout << "Sending Clear Emergency" << std::endl;

					core::ClearEmergency *clearEmergency = new core::ClearEmergency();
					sendMessage(component, pdList, clearEmergency);
					break;
				}

				case 'x': // Shutdown
				{
					std::cout << "Sending Shutdown" << std::endl;

					core::Shutdown *shutdown = new core::Shutdown();
					sendMessage(component, pdList, shutdown);
					break;
				}

				case 'c': // Standby
				{
					std::cout << "Sending Standby" << std::endl;

					core::Standby *standby = new core::Standby();
					sendMessage(component, pdList, standby);
					break;
				}

				case '7': // Query Wrench Effort
				{
					std::cout << "Sending Query Wrench Effort" << std::endl;

					mobility::QueryWrenchEffort *query = new mobility::QueryWrenchEffort();
					query->setQueryPresenceVector(mobility::ReportWrenchEffort::PV_ALL_FIELDS);
					sendMessage(component, pdList, query);
					break;
				}

				case '8': // Set Wrench Effort
				{
					std::cout << "Sending Set Wrench Effort" << std::endl;

					mobility::SetWrenchEffort *setWrench = new mobility::SetWrenchEffort();
					setWrench->enablePropulsiveLinearEffortX();
					setWrench->setPropulsiveLinearEffortX_percent(50.0);

					setWrench->enablePropulsiveLinearEffortY();
					setWrench->setPropulsiveLinearEffortY_percent(60.0);

					setWrench->enablePropulsiveLinearEffortZ();
					setWrench->setPropulsiveLinearEffortZ_percent(70.0);

					sendMessage(component, pdList, setWrench);
					break;
				}
			}
		}

		std::cout << "Shutting Down...\n";

		component.stop();
	}
	catch (system::Exception &expn)
	{
		std::cout <<expn.toString() << std::endl;
		system::Logger::log(expn);
	}

	return 0;
}

void signalHandler(int n)
{
	// Got Signal, shutdown gracefully
	std::cout << "Got Signal, shutdown gracefully" << std::endl;
	mainRunning = false;
}

void printMenu()
{
	std::cout << "Menu:\n";
	std::cout << "? - Print Menu\n";
	std::cout << "t - Print System Tree\n";
	std::cout << "f - Find Primitive Driver\n";
	std::cout << "Event Service\n";
	std::cout << "1 - Create Periodic Event (Report Wrench Effort)\n";
	std::cout << "2 - Stop Periodic Event (Report Wrench Effort)\n";
	std::cout << "3 - Create On-Change Event (Report Wrench Effort)\n";
	std::cout << "4 - Cancel On-Change Event (Report Wrench Effort)\n";
	std::cout << "Access Control Service\n";
	std::cout << "5 - Request PD Control\n";
	std::cout << "6 - Release PD Control\n";
	std::cout << "Management Service\n";
	std::cout << "q - Query Status\n";
	std::cout << "a - Reset\n";
	std::cout << "s - Resume\n";
	std::cout << "d - Set Emergency\n";
	std::cout << "z - Clear Emergency\n";
	std::cout << "x - Shutdown\n";
	std::cout << "c - Standby\n";
	std::cout << "Primitive Driver Service\n";
	std::cout << "7 - Query Wrench Effort\n";
	std::cout << "8 - Set Wrench Effort\n";
	std::cout << "ESC - Exit Component\n";
}

void processControlResponse(const model::ControlResponse &response)
{
	std::cout << "Recv Control Request Response from: " << response.getAddress() << std::endl;
	std::cout << "Response code: " << response.getResponseType() << std::endl;
}

bool processReportStatus(core::ReportStatus &report)
{
	std::cout << "Recv Report Status from: " << report.getSource() << std::endl;
	std::cout << "Status: " << report.getStatusToString() << std::endl;

	return true;
}

bool processReportWrenchEffort(mobility::ReportWrenchEffort &report)
{
	std::cout << "Received Report Wrench Effort" << std::endl;
	std::cout << "-----------------------------" << std::endl;
	std::cout << "PropulsiveLinearEffortX: " << report.getPropulsiveLinearEffortX_percent() << std::endl;
	std::cout << "PropulsiveLinearEffortY: " << report.getPropulsiveLinearEffortY_percent() << std::endl;
	std::cout << "PropulsiveLinearEffortZ: " << report.getPropulsiveLinearEffortZ_percent() << std::endl;
	std::cout << "PropulsiveRotationalEffortX: " << report.getPropulsiveRotationalEffortX_percent() << std::endl;
	std::cout << "PropulsiveRotationalEffortY: " << report.getPropulsiveRotationalEffortY_percent() << std::endl;
	std::cout << "PropulsiveRotationalEffortZ: " << report.getPropulsiveRotationalEffortZ_percent() << std::endl;
	std::cout << "ResistiveLinearEffortX: " << report.getResistiveLinearEffortX_percent() << std::endl;
	std::cout << "ResistiveLinearEffortY: " << report.getResistiveLinearEffortY_percent() << std::endl;
	std::cout << "ResistiveLinearEffortZ: " << report.getResistiveLinearEffortZ_percent() << std::endl;
	std::cout << "ResistiveRotationalEffortX: " << report.getResistiveRotationalEffortX_percent() << std::endl;
	std::cout << "ResistiveRotationalEffortY: " << report.getResistiveRotationalEffortY_percent() << std::endl;
	std::cout << "ResistiveRotationalEffortZ: " << report.getResistiveRotationalEffortZ_percent() << std::endl;
	std::cout << std::endl;

	return true;
}

// NOTE: You MUST pass the component by reference if you choose this pattern
// otherwise you will run into problems.
void sendMessage(core::Base& component, const std::vector<transport::Address>& pdList, model::Message* message)
{
	if (pdList.size() > 0)
	{
		message->setDestination(pdList.at(0));
		component.sendMessage(message);
	}
	else
	{
		std::cout << "No known Primitive Drivers!. You need to find one first." << std::endl;
	}
}