#include <iostream>
#include <memory>
#include <vector>
#include <string>
#include <map>
#include <functional>
#include <algorithm>
#include <dlfcn.h>
#include <opts.h>
#include <user/senseApiDesc.h>
#include <user/deviceAttachmentSpec.h>
#include <livoxProto1/livoxProto1.h>
#include <livoxProto1/device.h>


namespace smo {
namespace sense_api {

// Salmanoff hooks, obtained from SMO_GET_SENSE_API_DESC_FN_NAME().
static const SmoCallbacks* smoHooksPtr = nullptr;
static SmoThreadingModelDesc smoThreadingModelDesc;

// LivoxProto1 library state
struct LivoxProto1DllState
{
	LivoxProto1DllState()
	: dlopenHandle(nullptr, DlCloser),
	  livoxProto1_main(nullptr),
	  livoxProto1_exit(nullptr),
	  livoxProto1_getOrCreateDeviceReq(nullptr),
	  livoxProto1_destroyDeviceReq(nullptr)
	{}

	static void DlCloser(void* handle)
	{
		if (handle) {
			dlclose(handle);
		}
	}

	std::unique_ptr<void, void(*)(void*)> dlopenHandle;
	livoxProto1_mainFn *livoxProto1_main;
	livoxProto1_exitFn *livoxProto1_exit;
	livoxProto1_getOrCreateDeviceReqFn *livoxProto1_getOrCreateDeviceReq;
	livoxProto1_destroyDeviceReqFn *livoxProto1_destroyDeviceReq;
};

static LivoxProto1DllState livoxProto1;

// Attached Livox devices
static std::vector<std::shared_ptr<livoxProto1::Device>> g_attachedDevices;

// Callback function declarations
extern "C" sal_mlo_initializeIndFn livoxGen1_initializeInd;
extern "C" sal_mlo_finalizeIndFn livoxGen1_finalizeInd;
extern "C" sal_mlo_attachDeviceReqFn livoxGen1_attachDeviceReq;
extern "C" sal_mlo_detachDeviceReqFn livoxGen1_detachDeviceReq;

// Sense API descriptor
static const SenseApiDesc livoxGen1ApiDesc = {
	.name = "livoxGen1",
	.exportedImplexorApis = {
		{.name = "pointCloudCoords"},
		{.name = "pointCloudIntensity"},
		{.name = "gyro"},
		{.name = "accel"}
	},
	.sal_mgmt_libOps = {
		.initializeInd = livoxGen1_initializeInd,
		.finalizeInd = livoxGen1_finalizeInd,
		.attachDeviceReq = livoxGen1_attachDeviceReq,
		.detachDeviceReq = livoxGen1_detachDeviceReq
	}
};

// Callback function implementations
extern "C" int livoxGen1_initializeInd(void)
{
	if (!smoHooksPtr)
	{
		throw std::runtime_error(std::string(__func__) + ": SMO hooks "
			"pointers not filled in.");
	}

	// Load LivoxProto1 library
	auto libPath = smoHooksPtr->searchForLibInSmoSearchPaths(
		"liblivoxProto1.so");

	livoxProto1.dlopenHandle.reset(dlopen(
		libPath.value_or("liblivoxProto1.so").c_str(), RTLD_LAZY));

	if (!livoxProto1.dlopenHandle)
	{
		throw std::runtime_error(
			std::string(__func__) +
			": Failed to load LivoxProto1 library: " +
			(dlerror() ? dlerror() : "unknown error"));
	}

	// Get LivoxProto1 library functions
	livoxProto1.livoxProto1_main = reinterpret_cast<livoxProto1_mainFn *>(
		dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_main"));
	livoxProto1.livoxProto1_exit = reinterpret_cast<livoxProto1_exitFn *>(
		dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_exit"));
	livoxProto1.livoxProto1_getOrCreateDeviceReq = reinterpret_cast<
		livoxProto1_getOrCreateDeviceReqFn *>(
			dlsym(
				livoxProto1.dlopenHandle.get(),
				"livoxProto1_getOrCreateDeviceReq"));
	livoxProto1.livoxProto1_destroyDeviceReq = reinterpret_cast<
		livoxProto1_destroyDeviceReqFn *>(
			dlsym(
				livoxProto1.dlopenHandle.get(),
				"livoxProto1_destroyDeviceReq"));

	if (!livoxProto1.livoxProto1_main || !livoxProto1.livoxProto1_exit
		|| !livoxProto1.livoxProto1_getOrCreateDeviceReq
		|| !livoxProto1.livoxProto1_destroyDeviceReq)
	{
		throw std::runtime_error(
			std::string(__func__) +
			": Failed to get LivoxProto1 library functions");
	}

	// Call LivoxProto1 library main function
	(*livoxProto1.livoxProto1_main)(
		smoThreadingModelDesc.componentThread, *smoHooksPtr);

	return 0; // Success
}

extern "C" int livoxGen1_finalizeInd(void)
{
	// Clear all attached devices
	g_attachedDevices.clear();

	// Call LivoxProto1 library exit function
	if (livoxProto1.livoxProto1_exit) {
		(*livoxProto1.livoxProto1_exit)();
	}

	if (livoxProto1.dlopenHandle)
	{
		dlclose(livoxProto1.dlopenHandle.get());
		livoxProto1.dlopenHandle.reset();
	}

	livoxProto1 = LivoxProto1DllState();
	return 0; // Success
}

extern "C" void livoxGen1_attachDeviceReq(
	const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc,
	const std::shared_ptr<smo::ComponentThread>& componentThread,
	smo::sense_api::sal_mlo_attachDeviceReqCbFn cb
	)
{
	if (!livoxProto1.livoxProto1_getOrCreateDeviceReq)
	{
		throw std::runtime_error(
			std::string(__func__) + ": LivoxProto1 getOrCreateDevice function "
			"not available");
	}

	/**		FIXME:
	 * We should acquire a spinlock here to ensure that the device isn't added
	 * in the interim while the async op executes.
	 */

	for (const auto& dev : g_attachedDevices)
	{
		if (dev->discoveredDevice.deviceIdentifier == desc->deviceIdentifier)
			{ return; }
	}

	// Parse integer parameters from provider params with defaults
	/* The Livox Avia will generally respond to a handshake request within
	 * 50ms. So we set the handshake timeout to 300ms to be safe.
	 */
	int handshakeTimeoutMs = 300;	// Default: 50ms
	/* Based on testing on a Livox Avia, the device will generally resume
	 * sending broadcast advertisement dgrams after about 5 seconds at most.
	 * Generally, it will resume sending them within 1-2 seconds.
	 */
	int retryDelayMs = 5250;			// Default: 500ms
	uint8_t smoSubnetNbits = 24;	// Default: /24 subnet
	uint16_t dataPort = 56000;		// Default data port
	uint16_t cmdPort = 56001;		// Default command port
	uint16_t imuPort = 56002;		// Default IMU port
	// Default: empty string (will trigger IP auto-detection)
	std::string smoIp = "";

	// Parse optional integer parameters from provider params
	for (const auto& param : desc->providerParams)
	{
		if (param.first == "handshake-timeout-ms")
		{
			handshakeTimeoutMs = smo::device::DeviceAttachmentSpec
				::parseRequiredParamAsInt(*desc, "handshake-timeout-ms");
		} else if (param.first == "retry-delay-ms")
		{
			retryDelayMs = smo::device::DeviceAttachmentSpec
				::parseRequiredParamAsInt(*desc, "retry-delay-ms");
		} else if (param.first == "smo-subnet-nbits")
		{
			smoSubnetNbits = static_cast<uint8_t>(
				smo::device::DeviceAttachmentSpec
				::parseRequiredParamAsInt(*desc, "smo-subnet-nbits"));
		} else if (param.first == "data-port")
		{
			dataPort = static_cast<uint16_t>(
				smo::device::DeviceAttachmentSpec
				::parseRequiredParamAsInt(*desc, "data-port"));
		} else if (param.first == "cmd-port")
		{
			cmdPort = static_cast<uint16_t>(
				smo::device::DeviceAttachmentSpec
				::parseRequiredParamAsInt(*desc, "cmd-port"));
		} else if (param.first == "imu-port")
		{
			imuPort = static_cast<uint16_t>(
				smo::device::DeviceAttachmentSpec
				::parseRequiredParamAsInt(*desc, "imu-port"));
		} else if (param.first == "smo-ip")
		{
			if (param.second.empty())
			{
				throw std::runtime_error(
					std::string(__func__) + ": smo-ip parameter is empty");
			}
			if (param.second.find('.') == std::string::npos ||
			    std::count(param.second.begin(), param.second.end(), '.') != 3)
			{
				throw std::runtime_error(
					std::string(__func__) + ": smo-ip parameter is not an "
					"IPv4 address");
			}
			smoIp = param.second;
		}
		else
		{
			throw std::runtime_error(
				std::string(__func__) + ": Unknown provider parameter: "
				+ param.first);
		}
	}

	(*livoxProto1.livoxProto1_getOrCreateDeviceReq)(
		desc->deviceSelector,  // deviceIdentifier (broadcast code)
		componentThread,
		handshakeTimeoutMs, retryDelayMs,
		smoIp, smoSubnetNbits,
		dataPort, cmdPort, imuPort,
		[desc, cb](
			bool success, std::shared_ptr<livoxProto1::Device> dev) -> void
		{
			if (!dev)
			{
				std::cerr << __func__ << ": Failed to create Livox device: "
					<< desc->deviceSelector << std::endl;
				cb(false, desc);
				return;
			}

			g_attachedDevices.push_back(dev);
			if (1 || OptionParser::getOptions().verbose)
			{
				std::cout << __func__ << ": Successfully attached Livox "
					"device: " << desc->deviceSelector << " (ID: "
					<< desc->deviceIdentifier << ")\n";
			}

			cb(success, desc);
		}
	);
}

extern "C" void livoxGen1_detachDeviceReq(
	const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc,
	smo::sense_api::sal_mlo_detachDeviceReqCbFn cb
	)
{
	/**		FIXME:
	 * We should acquire a spinlock here to ensure that iterator doesn't become
	 * invalid in the interim while the async op executes. In the meantime,
	 * we'll repeat the search in the callback.
	 */

	// Find and remove the device from our collection
	auto it = std::find_if(g_attachedDevices.begin(), g_attachedDevices.end(),
		[&desc](const std::shared_ptr<livoxProto1::Device>& dev) {
			/**		EXPLANATION:
			 * Compare the first 14 characters of the deviceIdentifier with
			 * the first 14 characters of the deviceSelector
			 */
			const std::string& devId = dev->discoveredDevice.deviceIdentifier;
			std::string devIdPrefix = devId.substr(
				0, std::min<size_t>(14, devId.size()));

			return devIdPrefix == desc->deviceSelector.substr(
				0, std::min<size_t>(14, desc->deviceSelector.size()));
		}
	);

	if (it == g_attachedDevices.end())
	{
		std::cerr << std::string(__func__)
		          << ": Device not found for detachment: "
		          << desc->deviceIdentifier << std::endl;
		cb(false, desc);
		return;
	}

	(*livoxProto1.livoxProto1_destroyDeviceReq)(
		*it,
		[cb, desc](bool success)
		{
			if (!success)
			{
				std::cerr << __func__ << ": Failed to destroy Livox device: "
					<< desc->deviceIdentifier << "\n";
				cb(false, desc);
				return;
			}

			// Find the device in g_attachedDevices and remove it.
			auto eraseIt = std::find_if(
				g_attachedDevices.begin(), g_attachedDevices.end(),
				[desc](const std::shared_ptr<livoxProto1::Device>& dev)
				{
					const std::string& devId = dev->discoveredDevice.deviceIdentifier;
					std::string devIdPrefix = devId.substr(
						0, std::min<size_t>(14, devId.size()));
					return devIdPrefix == desc->deviceSelector.substr(
						0, std::min<size_t>(14, desc->deviceSelector.size()));
				}
			);

			if (eraseIt == g_attachedDevices.end())
			{
				std::cerr << __func__ << ": Race condition: device not found "
					"in g_attachedDevices for detachment: "
					<< desc->deviceIdentifier << "\n";
				cb(false, desc);
				return;
			}

			g_attachedDevices.erase(eraseIt);
			std::cout << __func__ << ": Successfully detached Livox device: "
				<< desc->deviceIdentifier << "\n";

			cb(success, desc);
		}
	);
}

// Exported function
extern "C" smo::sense_api::SMO_GET_SENSE_API_DESC_FN_TYPEDEF
	SMO_GET_SENSE_API_DESC_FN_NAME;

const smo::sense_api::SenseApiDesc& SMO_GET_SENSE_API_DESC_FN_NAME(
	const smo::sense_api::SmoCallbacks& callbacks,
	const smo::sense_api::SmoThreadingModelDesc& threadingModel)
{
	smoHooksPtr = &callbacks;
	smoThreadingModelDesc = threadingModel;

	return livoxGen1ApiDesc;
}

} // namespace sense_api
} // namespace smo