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

#define APP_VERSION "4.1"

using namespace openjaus;

bool processReportGlobalPose(mobility::ReportGlobalPose &report);
bool processReportGeomagneticProperty(mobility::ReportGeomagneticProperty &report);
bool processReportTimeout(core::ReportTimeout &report);
void processControlResponse(const model::ControlResponse& response);

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

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

int main(void)
{
	std::vector<transport::Address> gposList;
	uint32_t subscriptionId = 0;
	uint32_t subscriptionId2 = 0;
	uint32_t subscriptionId3 = 0;

	try
	{
		// Create our components
		core::Base component;
		component.setName("GposClientDemo");
		component.addMessageCallback(processReportGlobalPose);
		component.addMessageCallback(processReportGeomagneticProperty);
		component.addMessageCallback(processReportTimeout);

		std::cout << "Thank you for using the OpenJAUS SDK v" << OPENJAUS_SDK_VERSION << std::endl;
		system::Application::setTerminalMode();
		std::cout << "GposClientDemo v" << APP_VERSION << std::endl;
		printMenu();		

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

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

		mainRunning = true;
		unsigned char choice = 0;
		while(mainRunning) // ESC
		{
			choice = system::Application::getChar();
			switch(choice)
			{
				case system::Application::ASCII_ESC:
					mainRunning = false;
					break;
					
				case 't':
					std::cout << component.getSystemTree()->toString();
					break;

				case '?':
					printMenu();
					break;

				case '1':
				{
					gposList = component.getSystemTree()->lookupService(mobility::GlobalPoseSensor::GlobalPoseSensorUri());
					std::cout << "GPOS Services (" << gposList.size() << "):\n";
					for(size_t i = 0; i < gposList.size(); i++)
					{
						std::cout << "\t" << gposList.at(i).toString() << std::endl;
					}
					break;
				}

				case '2':
				{
					std::cout << "Sending Query Global Pose" << std::endl;
					mobility::QueryGlobalPose *query = new mobility::QueryGlobalPose();
					query->setQueryPresenceVector(mobility::ReportGlobalPose::PV_ALL_FIELDS);
					sendMessage(component, gposList, query);
					break;
				}

				case '3':
				{
					std::cout << "Sending Query Geomagnetic Property" << std::endl;
					mobility::QueryGeomagneticProperty *query = new mobility::QueryGeomagneticProperty();
					sendMessage(component, gposList, query);
					break;
				}

				case '4':
					if(gposList.size() > 0)
					{
						mobility::QueryGlobalPose *query = new mobility::QueryGlobalPose();
						query->setQueryPresenceVector(mobility::ReportGlobalPose::PV_ALL_FIELDS);

						// Magic Number [10]: 10 is the requested rate (in Hz) of the periodic event
						subscriptionId = component.subscribePeriodic(gposList.at(0), query, 10.0);
						std::cout << "Created Periodic Event: " << subscriptionId << std::endl;
					}
					break;

				case '5':
					if(subscriptionId)
					{
						std::cout << "Un-subscribing Periodic Event: " << subscriptionId << std::endl;
						if(component.unsubscribe(subscriptionId))
						{
							subscriptionId = 0;
						}
					}
					break;

				case '6':
					if(gposList.size() > 0)
					{
						component.requestControl(gposList.at(0), processControlResponse);
						std::cout << "Request Control Gpos at " << gposList.at(0) << std::endl;
					}
					break;

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

				case '8':
				{
					std::cout << "Sending Set Global Pose" << std::endl;

					mobility::SetGlobalPose *pose = new mobility::SetGlobalPose();

					pose->enableLatitude();
					pose->setLatitude_deg(50.0);

					pose->enableLongitude();
					pose->setLongitude_deg(50.0);

					sendMessage(component, gposList, pose);
					break;
				}

				case '9':
					if(gposList.size() > 0)
					{
						mobility::QueryGeomagneticProperty *query = new mobility::QueryGeomagneticProperty();
						subscriptionId2 = component.subscribePeriodic(gposList.at(0), query, 1.0);
						std::cout << "Created Periodic Event: " << subscriptionId2 << std::endl;
					}
					break;

				case '0':
					if(subscriptionId2)
					{
						std::cout << "Un-subscribing Periodic Event: " << subscriptionId2 << std::endl;
						if(component.unsubscribe(subscriptionId2))
						{
							subscriptionId2 = 0;
						}
					}
					break;

				case 'a':
				{
					std::cout << "Sending Query Timeout" << std::endl;

					core::QueryTimeout *query = new core::QueryTimeout();
					sendMessage(component, gposList, query);
					break;
				}

				case 'b':
					if(gposList.size() > 0)
					{
						mobility::QueryGlobalPose *query = new mobility::QueryGlobalPose();
						query->setQueryPresenceVector(mobility::ReportGlobalPose::PV_ALL_FIELDS);
						subscriptionId3 = component.subscribeOnChange(gposList.at(0), query);
						std::cout << "Created On-Change Event: " << subscriptionId3 << std::endl;
					}
					break;

				case 'c':
					if(subscriptionId3)
					{
						std::cout << "Un-subscribing OnChange Event: " << subscriptionId3 << std::endl;
						if(component.unsubscribe(subscriptionId3))
						{
							subscriptionId3 = 0;
						}
					}
					break;
			}
		}

		std::cout << "Shutting Down...\n";
		component.stop();
	}
	catch(openjaus::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;
}

bool processReportGlobalPose(mobility::ReportGlobalPose &report)
{
	std::cout << "Received Report Global Pose" << std::endl;
	std::cout << "---------------------------" << std::endl;
	std::cout << "Latitude: " << report.getLatitude_deg() << std::endl;
	std::cout << "Longitude: " << report.getLongitude_deg() << std::endl;
	std::cout << "Altitude: " << report.getAltitude_m() << std::endl;
	std::cout << "Roll: " << report.getRoll_rad() << std::endl;
	std::cout << "Pitch: " << report.getPitch_rad() << std::endl;
	std::cout << "Yaw: " << report.getYaw_rad() << std::endl;
	std::cout << std::endl;
	return true;
}

bool processReportGeomagneticProperty(mobility::ReportGeomagneticProperty &report)
{
	std::cout << "Received Report Geomagnetic Property\n";
	std::cout << "Magnetic Variation: " << report.getMagneticVariation_rad() << std::endl;
	return true;
}

bool processReportTimeout(core::ReportTimeout &report)
{
	std::cout << "Received Report Timeout\n";
	std::cout << "Timeout (sec): " << (int)report.getTimeout_sec() << std::endl;
	return true;
}

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

void printMenu()
{
	std::cout << "Menu:" << std::endl;
	std::cout << "t - Print System Tree" << std::endl;
	std::cout << "1 - Find Global Pose Sensor" << std::endl;
	std::cout << "2 - Query Global Pose" << std::endl;
	std::cout << "3 - Query Geomagnetic Property" << std::endl;
	std::cout << "4 - Create Periodic Event (Report GPOS)" << std::endl;
	std::cout << "5 - Stop Periodic Event (Report GPOS)" << std::endl;
	std::cout << "6 - Request GPOS Control" << std::endl;
	std::cout << "7 - Release GPOS Control" << std::endl;
	std::cout << "8 - Set Global Pose" << std::endl;
	std::cout << "9 - Create Periodic Event (Report Geomagnetic Property)" << std::endl;
	std::cout << "0 - Stop Periodic Event (Report Geomagnetic Property)" << std::endl;
	std::cout << "a - Query Access Control Timeout" << std::endl;
	std::cout << "b - Create On-Change Event (Report GPOS)" << std::endl;
	std::cout << "c - Cancel On-Change Event (Report GPOS)" << std::endl;
	std::cout << "? - Output Menu" << std::endl;
	std::cout << "ESC - Exit Component" << std::endl;
}

// 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>& gposList, model::Message* message)
{
	if (gposList.size() > 0)
	{
		message->setDestination(gposList.at(0));
		component.sendMessage(message);
	}
	else
	{
		std::cout << "No known Global Pose Sensors!. You need to find one first." << std::endl;
	}
}