#include <iostream>
#include <iomanip>
#include <thread>
#include <chrono>

#include "icsneo/icsneocpp.h"
#include "icsneo/communication/message/livedatamessage.h"
#include "icsneo/communication/livedata.h"

int main() {
	// Print version
	std::cout << "Running libicsneo " << icsneo::GetVersion() << std::endl;
	std::cout << "\nFinding devices... " << std::flush;
	auto devices = icsneo::FindAllDevices(); // This is type std::vector<std::shared_ptr<icsneo::Device>>
	std::cout << "OK, " << devices.size() << " device" << (devices.size() == 1 ? "" : "s") << " found" << std::endl;

	// List off the devices
	for(auto& device : devices)
		std::cout << '\t' << device->describe() << " @ Handle " << device->getNeoDevice().handle << std::endl;
	std::cout << std::endl;

	for(auto& device : devices) {
		std::cout << "Connecting to " << device->describe() << "... ";
		bool ret = device->open();
		if(!ret) { // Failed to open
			std::cout << "FAIL" << std::endl;
			std::cout << icsneo::GetLastError() << std::endl << std::endl;
			continue;
		}
		std::cout << "OK" << std::endl;

		// Create a subscription message for the GPS signals
		std::cout << "\tSending a live data subscribe command... ";
		auto msg = std::make_shared<icsneo::LiveDataCommandMessage>();
		msg->appendSignalArg(icsneo::LiveDataValueType::GPS_LATITUDE);
		msg->appendSignalArg(icsneo::LiveDataValueType::GPS_LONGITUDE);
		msg->appendSignalArg(icsneo::LiveDataValueType::GPS_ACCURACY);
		msg->cmd = icsneo::LiveDataCommand::SUBSCRIBE;
		msg->handle = icsneo::LiveDataUtil::getNewHandle();
		msg->updatePeriod = std::chrono::milliseconds(100);
		msg->expirationTime = std::chrono::milliseconds(0);
		// Transmit the subscription message
		ret = device->subscribeLiveData(msg);
		std::cout << (ret ? "OK" : "FAIL") << std::endl;

		// Register a handler that uses the data after it arrives every ~100ms
		std::cout << "\tStreaming messages for 3 seconds... " << std::endl << std::endl;
		auto filter = std::make_shared<icsneo::MessageFilter>(icsneo::Message::Type::LiveData);
		auto handler = device->addMessageCallback(std::make_shared<icsneo::MessageCallback>(filter, [&msg](std::shared_ptr<icsneo::Message> message) {
			auto ldMsg = std::dynamic_pointer_cast<icsneo::LiveDataMessage>(message);
			switch(ldMsg->cmd) {
				case icsneo::LiveDataCommand::STATUS: {
					auto msg2 = std::dynamic_pointer_cast<icsneo::LiveDataStatusMessage>(message);
					std::cout << "[Handle] " << ldMsg->handle << std::endl;
					std::cout << "[Requested Command] " << msg2->requestedCommand << std::endl;
					std::cout << "[Status] " << msg2->status << std::endl << std::endl;
					break;
				}
				case icsneo::LiveDataCommand::RESPONSE: {
					auto valueMsg = std::dynamic_pointer_cast<icsneo::LiveDataValueMessage>(message);
					if((valueMsg->handle == msg->handle) && (valueMsg->values.size() == msg->args.size()))
					{
						std::cout << "[Handle] " << msg->handle << std::endl;
						std::cout << "[Values] " << valueMsg->numArgs << std::endl;
						for(uint32_t i = 0; i < valueMsg->numArgs; ++i) {
							std::cout << "[" << msg->args[i]->valueType << "] ";
							auto scaledValue = icsneo::LiveDataUtil::liveDataValueToDouble(*valueMsg->values[i]);
							std::cout << scaledValue << std::endl;
						}
						std::cout << std::endl;
					}
					break;
				}
				default: // Ignoring other commands
					break;
			}
		}));
		// Run handler for three seconds to observe the signal data
		std::this_thread::sleep_for(std::chrono::seconds(3));
		// Unsubscribe from the GPS signals and run handler for one more second
		// Unsubscription only requires a valid in-use handle, in this case from our previous subscription
		ret = device->unsubscribeLiveData(msg->handle);
		// The handler should no longer print values
		std::this_thread::sleep_for(std::chrono::seconds(1));
		device->removeMessageCallback(handler);
		std::cout << "\tDisconnecting... ";
		ret = device->close();
		std::cout << (ret ? "OK\n" : "FAIL\n") << std::endl;
	}
	return 0;
}