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

#define APP_VERSION "4.1"

using namespace openjaus;

// Helper functions
void printMenu();
void printWrenchEffort(mobility::ReportWrenchEffort &report);

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

// Implement the user specific behavior needed for the Primitive Driver.
class PdComponent : public mobility::PrimitiveDriver
{
public:
	PdComponent(std::string name) :
		PrimitiveDriver(),
		reportWrenchEffort()
	{
		this->name = name;

		// Initialize the ReportWrenchEffort message.
		// This message would be populated by the actual platform.
		// Setting values for all fields to show the fields being populated,
		// though they probably wouldn't make sense on a real platform.
		reportWrenchEffort.setPropulsiveLinearEffortX_percent(1.0);
		reportWrenchEffort.setPropulsiveLinearEffortY_percent(2.0);
		reportWrenchEffort.setPropulsiveLinearEffortZ_percent(3.0);
		reportWrenchEffort.setPropulsiveRotationalEffortX_percent(14.0);
		reportWrenchEffort.setPropulsiveRotationalEffortY_percent(15.0);
		reportWrenchEffort.setPropulsiveRotationalEffortZ_percent(16.0);
		reportWrenchEffort.setResistiveLinearEffortX_percent(27.0);
		reportWrenchEffort.setResistiveLinearEffortY_percent(28.0);
		reportWrenchEffort.setResistiveLinearEffortZ_percent(29.0);
		reportWrenchEffort.setResistiveRotationalEffortX_percent(30.0);
		reportWrenchEffort.setResistiveRotationalEffortY_percent(31.0);
		reportWrenchEffort.setResistiveRotationalEffortZ_percent(32.0);

		publish(mobility::ReportWrenchEffort::ID);
	}

	// Client component has queried the current wrench effort.
	// Here is where we would query the actual platform for is current
	// state and convert it to a wrench effort.
	mobility::ReportWrenchEffort getReportWrenchEffort(mobility::QueryWrenchEffort *queryWrenchEffort)
	{
		// Log that we're processing the Query message for debugging.
		LOG("Processing Query Wrench Effort");

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

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

	// Client component has requested a change to the wrench effort.
	// Here is where we would control the actual platform based on
	// the requested wrench efforts. 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 setWrenchEffort(mobility::SetWrenchEffort *setWrenchEffort)
	{
		// Log that we're processing the Query message for debugging.
		LOG("Processing Set Wrench Effort");

		// Change the wrench effort being applied to the platform.
		changeWrenchEffort(setWrenchEffort);
		return true;
	}

	// An accessor the private ReportWrenchEffort message so we can print the value.
	mobility::ReportWrenchEffort getReportWrenchEffort()
	{
		return reportWrenchEffort;
	}

private: //methods
	// Helper method to update the wrench effort being applied to the platform.
	// For this demo, we are always going to report the last requested wrench
	// effort back to the client.
	void changeWrenchEffort(mobility::SetWrenchEffort* setWrenchEffort)
	{
		// We need to check that the field is enabled (based on the presence
		// vector) in the Set Wrench Effort message before we use it.
		if (setWrenchEffort->isPropulsiveLinearEffortXEnabled())
		{
			double value = setWrenchEffort->getPropulsiveLinearEffortX_percent();
			reportWrenchEffort.setPropulsiveLinearEffortX_percent(value);
		}

		if (setWrenchEffort->isPropulsiveLinearEffortYEnabled())
		{
			double value = setWrenchEffort->getPropulsiveLinearEffortY_percent();
			reportWrenchEffort.setPropulsiveLinearEffortY_percent(value);
		}

		if (setWrenchEffort->isPropulsiveLinearEffortZEnabled())
		{
			double value = setWrenchEffort->getPropulsiveLinearEffortZ_percent();
			reportWrenchEffort.setPropulsiveLinearEffortZ_percent(value);
		}

		if (setWrenchEffort->isPropulsiveRotationalEffortXEnabled())
		{
			double value = setWrenchEffort->getPropulsiveRotationalEffortX_percent();
			reportWrenchEffort.setPropulsiveRotationalEffortX_percent(value);
		}

		if (setWrenchEffort->isPropulsiveRotationalEffortYEnabled())
		{
			double value = setWrenchEffort->getPropulsiveRotationalEffortY_percent();
			reportWrenchEffort.setPropulsiveRotationalEffortY_percent(value);
		}

		if (setWrenchEffort->isPropulsiveRotationalEffortZEnabled())
		{
			double value = setWrenchEffort->getPropulsiveRotationalEffortZ_percent();
			reportWrenchEffort.setPropulsiveRotationalEffortZ_percent(value);
		}

		if (setWrenchEffort->isResistiveLinearEffortXEnabled())
		{
			double value = setWrenchEffort->getResistiveLinearEffortX_percent();
			reportWrenchEffort.setResistiveLinearEffortX_percent(value);
		}

		if (setWrenchEffort->isResistiveLinearEffortYEnabled())
		{
			double value = setWrenchEffort->getResistiveLinearEffortY_percent();
			reportWrenchEffort.setResistiveLinearEffortY_percent(value);
		}

		if (setWrenchEffort->isResistiveLinearEffortZEnabled())
		{
			double value = setWrenchEffort->getResistiveLinearEffortZ_percent();
			reportWrenchEffort.setResistiveLinearEffortZ_percent(value);
		}

		if (setWrenchEffort->isResistiveRotationalEffortXEnabled())
		{
			double value = setWrenchEffort->getResistiveRotationalEffortX_percent();
			reportWrenchEffort.setResistiveRotationalEffortX_percent(value);
		}

		if (setWrenchEffort->isResistiveRotationalEffortYEnabled())
		{
			double value = setWrenchEffort->getResistiveRotationalEffortY_percent();
			reportWrenchEffort.setResistiveRotationalEffortY_percent(value);
		}

		if (setWrenchEffort->isResistiveRotationalEffortZEnabled())
		{
			double value = setWrenchEffort->getResistiveRotationalEffortZ_percent();
			reportWrenchEffort.setResistiveRotationalEffortZ_percent(value);
		}
	}

private: // variables
	mobility::ReportWrenchEffort reportWrenchEffort;
};


int main(void)
{
	try
	{
		// Create the Primitive Driver Component
		PdComponent component("PdDemo");

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

		std::cout << "Thank you for using the OpenJAUS SDK v" << OPENJAUS_SDK_VERSION << std::endl;
		std::cout << "PdDemo 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::ReportWrenchEffort report = component.getReportWrenchEffort();
					printWrenchEffort(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 'i':
					// Initialize the Primitive Driver.
					// Normally this would happen automatically and would do whatever
					// work is necessary to initialize the platform. For example,
					// you may need to run a calibration sequence.
					// Since we don't have an actual platform we use a manual trigger.
					std::cout << "Initializing the Primitive Driver" << std::endl;

					// After the platform has been initialized, trigger the component
					// to move out of the Init state. This is required since the Primitive
					// Driver implements the Management interface.
					component.initialized();
					break;

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

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