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

using namespace openjaus;
#define APP_VERSION "4.1"

void printMenu();
void printGlobalPose(mobility::ReportGlobalPose &report);

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

class GposComponent : public openjaus::mobility::GlobalPoseSensor
{
public:
	GposComponent() :
		GlobalPoseSensor(),
		reportGlobalPose(),
		reportGeomagneticProperty()
	{
		name = "GposDemo";
		this->reportGeomagneticProperty.setMagneticVariation_rad(1.0);

		this->reportGlobalPose.setLatitude_deg(34.5);
		this->reportGlobalPose.setLongitude_deg(-80.04);
		this->reportGlobalPose.setAltitude_m(100);
		this->reportGlobalPose.setRoll_rad(0.5);
		this->reportGlobalPose.setPitch_rad(-0.1);
		this->reportGlobalPose.setYaw_rad(0.1);

		publish(mobility::ReportGlobalPose::ID);
		publish(mobility::ReportGeomagneticProperty::ID);
	}

	// Client component has queried the current global position.
	// Here is where we would query the actual platform for is current
	// position and convert it to a global position.
    mobility::ReportGlobalPose getReportGlobalPose(mobility::QueryGlobalPose *queryGlobalPose)
    {
    	LOG("Processing Query Global Pose");

		// For this demo, we have no platform to query so we return the
		// current value of the Report Global Pose message. The value
		// can be changed using the Set Global Pose message.

    	// Set the presence vector based on what the client requests.
    	reportGlobalPose.setPresenceVector(queryGlobalPose->getQueryPresenceVector());
    	return reportGlobalPose;
    }

	mobility::ReportGeomagneticProperty getReportGeomagneticProperty(mobility::QueryGeomagneticProperty *queryGeomagneticProperty)
	{
    	LOG("Processing Query Geomagnetic Property");
    	return reportGeomagneticProperty;
	}

	// Client component has requested a change to the global position.
	// Here is where we would update the global position of the actual
	// platform based on the requested global pose. The OpenJAUS SDK
	// automatically handles ensuring that the message is coming from a
	// controlling component so we don't have to worry about it.
	bool updateGlobalPose(mobility::SetGlobalPose *setGlobalPose)
	{
		changeGlobalPose(setGlobalPose);
		return true;
	}

	bool updateGeomagneticProperty(mobility::SetGeomagneticProperty *setGeomagneticProperty)
	{
		this->reportGeomagneticProperty.setMagneticVariation_rad(setGeomagneticProperty->getMagneticVariation_rad());
		return true;
	}

	// An accessor the private ReportGlobalPose message so we can print the value.
	mobility::ReportGlobalPose getReportGlobalPose()
	{
		return reportGlobalPose;
	}

private: // methods
	// Helper method to update the global pose of the platform.
	// For this demo, we are always going to report the last requested
	// global pose back to the client.
	void changeGlobalPose(mobility::SetGlobalPose* setGlobalPose)
	{
		// We need to check that the field is enabled (based on the presence
		// vector) in the Set Global Pose message before we use it.
		if (setGlobalPose->isLatitudeEnabled())
		{
			this->reportGlobalPose.setLatitude_deg(setGlobalPose->getLatitude_deg());
		}

		if (setGlobalPose->isLongitudeEnabled())
		{
			this->reportGlobalPose.setLongitude_deg(setGlobalPose->getLongitude_deg());
		}
		if (setGlobalPose->isAltitudeEnabled())
		{
			this->reportGlobalPose.setAltitude_m(setGlobalPose->getAltitude_m());
		}

		if (setGlobalPose->isRollEnabled())
		{
			this->reportGlobalPose.setRoll_rad(setGlobalPose->getRoll_rad());
		}

		if (setGlobalPose->isPitchEnabled())
		{
			this->reportGlobalPose.setPitch_rad(setGlobalPose->getPitch_rad());
		}

		if (setGlobalPose->isYawEnabled())
		{
			this->reportGlobalPose.setYaw_rad(setGlobalPose->getYaw_rad());
		}
	}

private: // variables
	mobility::ReportGlobalPose reportGlobalPose;
	mobility::ReportGeomagneticProperty reportGeomagneticProperty;
};

int main(void)
{
	try
	{
		// Create our components
		GposComponent component;

		system::Application::setTerminalMode();
		std::cout << "Thank you for using the OpenJAUS SDK v" << OPENJAUS_SDK_VERSION << std::endl;
		std::cout << "GposDemo 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 '?':
					printMenu();
					break;

				case 't':
					std::cout << component.getSystemTree();
					break;

				case 'p':
				{
					mobility::ReportGlobalPose report = component.getReportGlobalPose();
					printGlobalPose(report);
					break;
				}

				case 'd':
					// Dismiss the controlling client.
					// The controlling client can be dismissed internally if necessary.
					std::cout << "Dismissing controller" << std::endl;
					component.dismissController();
					break;

				case '1':
					// Notify the Event service that the ReportGlobalPose message has
					// changed. This will cause a ReportGlobalPose message to be sent
					// to all the on-change event subscribers.
					std::cout << "Trigger On-Changed Event for Report Global Pose" << std::endl;
					component.notifyChanged(mobility::ReportGlobalPose::ID);
					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 << "d - Dismiss Controller\n";
	std::cout << "1 - Trigger On-Change Event: Report Global Pose\n";
	std::cout << "2 - Print Current Wrench Effort\n";
	std::cout << "ESC - Exit Component\n";
}

void printGlobalPose(mobility::ReportGlobalPose &report)
{
	std::cout << "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;
}