salmanoff/stimBuffApis/livoxGen1/livoxGen1.cpp

#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 <callback.h>
#include <livoxProto1/livoxProto1.h>
#include <livoxProto1/device.h>
#include <asynchronousContinuation.h>


namespace smo {
namespace stim_buff {

// Salmanoff hooks, obtained from SMO_GET_STIM_BUFF_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;

// Continuation classes for async operations
class AttachDeviceReq
:	public smo::NonPostedAsynchronousContinuation<sal_mlo_attachDeviceReqCbFn>
{
public:
	AttachDeviceReq(
		const std::shared_ptr<smo::device::DeviceAttachmentSpec>& spec,
		smo::Callback<sal_mlo_attachDeviceReqCbFn> cb)
	:	smo::NonPostedAsynchronousContinuation<sal_mlo_attachDeviceReqCbFn>(
			std::move(cb)),
	spec(spec)
	{}

public:
	const std::shared_ptr<smo::device::DeviceAttachmentSpec> spec;

public:
	void attachDeviceReq1(
		std::shared_ptr<AttachDeviceReq> context,
		bool success, std::shared_ptr<livoxProto1::Device> dev)
	{
		if (!dev)
		{
			std::cerr << __func__ << ": Failed to create Livox device: "
				<< context->spec->deviceSelector << std::endl;
			context->callOriginalCb(false, context->spec);
			return;
		}

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

		context->callOriginalCb(success, context->spec);
	}
};

class DetachDeviceReq
:	public smo::NonPostedAsynchronousContinuation<sal_mlo_detachDeviceReqCbFn>
{
public:
	DetachDeviceReq(
		const std::shared_ptr<smo::device::DeviceAttachmentSpec>& spec,
		smo::Callback<sal_mlo_detachDeviceReqCbFn> cb)
	:	smo::NonPostedAsynchronousContinuation<sal_mlo_detachDeviceReqCbFn>(
		std::move(cb)),
	spec(spec)
	{}

public:
	const std::shared_ptr<smo::device::DeviceAttachmentSpec> spec;

public:
	void detachDeviceReq1(
		std::shared_ptr<DetachDeviceReq> context,
		bool success)
	{
		if (!success)
		{
			std::cerr << __func__ << ": Failed to destroy Livox device: "
				<< context->spec->deviceIdentifier << "\n";
			context->callOriginalCb(false, context->spec);
			return;
		}

		// Find the device in g_attachedDevices and remove it.
		auto eraseIt = std::find_if(
			g_attachedDevices.begin(), g_attachedDevices.end(),
			[context](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 == context->spec->deviceSelector.substr(
					0, std::min<size_t>(14, context->spec->deviceSelector.size()));
			}
		);

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

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

		context->callOriginalCb(success, context->spec);
	}
};

// 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;

// Stim Buff API descriptor
static const StimBuffApiDesc livoxGen1ApiDesc = {
	.name = "livoxGen1",
	.exportedQualeIfaceApis = {
		{.name = "pcloud"},
		{.name = "pcloudIntensity"},
		{.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,
	Callback<smo::stim_buff::sal_mlo_attachDeviceReqCbFn> cb
	)
{
	if (!livoxProto1.livoxProto1_getOrCreateDeviceReq)
	{
		throw std::runtime_error(
			std::string(__func__) + ": LivoxProto1 getOrCreateDevice function "
			"not available");
	}

	for (const auto& dev : g_attachedDevices)
	{
		if (dev->discoveredDevice.deviceIdentifier == desc->deviceIdentifier)
		{
			cb.callbackFn(true, desc);
			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);
		}
	}

	auto request = std::make_shared<AttachDeviceReq>(desc, cb);

	(*livoxProto1.livoxProto1_getOrCreateDeviceReq)(
		desc->deviceSelector,  // deviceIdentifier (broadcast code)
		componentThread,
		handshakeTimeoutMs, retryDelayMs,
		smoIp, smoSubnetNbits,
		dataPort, cmdPort, imuPort,
		{request, std::bind(
			&AttachDeviceReq::attachDeviceReq1,
			request.get(), request,
			std::placeholders::_1, std::placeholders::_2)});
}

extern "C" void livoxGen1_detachDeviceReq(
	const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc,
	Callback<smo::stim_buff::sal_mlo_detachDeviceReqCbFn> cb
	)
{
	// 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.callbackFn(false, desc);
		return;
	}

	auto request = std::make_shared<DetachDeviceReq>(desc, cb);

	(*livoxProto1.livoxProto1_destroyDeviceReq)(
		*it,
		{request, std::bind(
			&DetachDeviceReq::detachDeviceReq1,
			request.get(), request,
			std::placeholders::_1)});
}

// Exported function
extern "C" smo::stim_buff::SMO_GET_STIM_BUFF_API_DESC_FN_TYPEDEF
	SMO_GET_STIM_BUFF_API_DESC_FN_NAME;

const smo::stim_buff::StimBuffApiDesc& SMO_GET_STIM_BUFF_API_DESC_FN_NAME(
	const smo::stim_buff::SmoCallbacks& callbacks,
	const smo::stim_buff::SmoThreadingModelDesc& threadingModel)
{
	smoHooksPtr = &callbacks;
	smoThreadingModelDesc = threadingModel;

	return livoxGen1ApiDesc;
}

} // namespace stim_buff
} // namespace smo