#include "nanolib_helper.hpp"

#include <exception>
#include <iostream>
#include <nano_lib_hw_strings.hpp>
#include <sstream>
#include <string>
#include <vector>

void objectDictionaryAccessExamples(NanoLibHelper const &nanolibHelper,
									nlc::DeviceHandle const &deviceHandle);

int main() {
	NanoLibHelper nanolibHelper;

	try {
		// create access to the nanolib
		nanolibHelper.setup();

		// its possible to set the logging level to a different level
		nanolibHelper.setLoggingLevel(nlc::LogLevel::Off);

		// list all hardware available, decide for the first one
		std::vector<nlc::BusHardwareId> busHardwareIds = nanolibHelper.getBusHardware();

		if (busHardwareIds.empty()) {
			std::cerr << "No bus hardware found." << std::endl;
			return -1;
		}

		std::cout << std::endl << "Available bus hardware:" << std::endl << std::endl;

		unsigned lineNum = 0;
		// just for better overview: print out available hardware
		for (nlc::BusHardwareId const &busHwId : busHardwareIds) {
			std::cout << lineNum << ". " << busHwId.getName()
					  << " with protocol: " << busHwId.getProtocol() << std::endl;
			lineNum++;
		}

		std::cout << std::endl
				  << "Please select (enter) bus hardware number(0"
				  << "-" << (lineNum - 1) << ") and press [ENTER]:";

		std::cin >> lineNum;

		std::cout << std::endl;

		if (lineNum >= busHardwareIds.size()) {
			std::cout << "Invalid selection!" << std::endl;
			return -1;
		}

		// Use the selected bus hardware
		nlc::BusHardwareId busHwId = busHardwareIds[lineNum];

		// create bus hardware options for opening the hardware
		nlc::BusHardwareOptions busHwOptions = nanolibHelper.createBusHardwareOptions(busHwId);

		std::cout << "Opening bus hardware: " << busHwId.getName() << std::endl;

		// now able to open the hardware itself
		nanolibHelper.openBusHardware(busHwId, busHwOptions);

		nanolibHelper.setLoggingLevel(nlc::LogLevel::Off);

		std::cout << "Scanning bus for devices..." << std::endl;

		// either scan the whole bus for devices (in case the bus supports scanning)
		std::vector<nlc::DeviceId> deviceIds = nanolibHelper.scanBus(busHwId);

		nanolibHelper.setLoggingLevel(nlc::LogLevel::Off);

		for (nlc::DeviceId const &id : deviceIds) {
			std::cout << "Found device: " << id.toString();
			std::cout << std::endl;
		}

		if (deviceIds.size() == 0) {
			std::cout << "No devices found. Exiting..." << std::endl;
			return -1;
		}

		std::cout << std::endl << "Available devices:" << std::endl << std::endl;
		lineNum = 0;
		// print out available devices
		for (nlc::DeviceId const &id : deviceIds) {
			std::cout << lineNum << ". " << id.getDescription()
					  << " [device id: " << id.getDeviceId()
					  << ", hardware: " << id.getBusHardwareId().getName() << "]" << std::endl;
			lineNum++;
		}

		std::cout << std::endl
				  << "Please select (enter) device number(0"
				  << "-" << (lineNum - 1) << ") and press [ENTER]:";

		std::cin >> lineNum;

		std::cout << std::endl;

		if (lineNum >= deviceIds.size()) {
			std::cout << "Invalid selection!" << std::endl;
			return -1;
		}

		// We can create the device id manually
		// nlc::DeviceId deviceId = nlc::DeviceId(busHwId, 1, "");
		// or use the first found device
		nlc::DeviceId deviceId = deviceIds[lineNum];

		nlc::DeviceHandle deviceHandle = nanolibHelper.createDevice(deviceId);

		// now connect to the device
		nanolibHelper.connectDevice(deviceHandle);

		// now ready to work with the controller, here are some examples on how to access the
		// object dictionary:
		objectDictionaryAccessExamples(nanolibHelper, deviceHandle);

		// cleanup and close everything

		nanolibHelper.disconnectDevice(deviceHandle);

		nanolibHelper.closeBusHardware(busHwId);

		std::cout << std::endl;
		std::cout << "Closing everything successfully";
		std::cout << std::endl;
	} catch (nanolib_exception &e) {
		std::cerr << "Error occurred: " << e.what() << std::endl;
	}
}

void objectDictionaryAccessExamples(NanoLibHelper const &nanolibHelper,
									nlc::DeviceHandle const &deviceHandle) {
	std::cout << std::endl << "OD Example" << std::endl;
	std::cout << std::endl << "Reading subindex 0 of index 0x6040" << std::endl;

	std::uint16_t statusWord
		= static_cast<uint16_t>(nanolibHelper.readInteger(deviceHandle, nlc::OdIndex(0x6040, 00)));
	std::cout << "Result: " << std::to_string(statusWord) << std::endl;

	std::cout << std::endl << "Motor Stop (0x6040-0)" << std::endl;

	nanolibHelper.writeInteger(deviceHandle, 6, nlc::OdIndex(0x6040, 00), 16);

	std::cout << std::endl << "Read Nanotec home page string" << std::endl;
	std::cout << "Homepage of Nanotec Electronic GmbH & Co. KG is: "
			  << nanolibHelper.readString(deviceHandle, nlc::OdIndex(0x6505, 0x00)) << std::endl;

	std::cout << std::endl << "Read device error stack:" << std::endl;
	std::vector<std::int64_t> errorStack
		= nanolibHelper.readArray(deviceHandle, nlc::OdIndex(0x1003, 0x00));
	std::cout << "The error stack has " << std::to_string(errorStack.at(0)) << " element"
			  << std::endl;
}