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 <livoxProto1/protocol.h>
#include <asynchronousContinuation.h>
#include <boost/asio/deadline_timer.hpp>


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),
	  livoxProto1_device_enablePcloudDataReq(nullptr),
	  livoxProto1_device_disablePcloudDataReq(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;
	livoxProto1_device_enablePcloudDataReqFn
		*livoxProto1_device_enablePcloudDataReq;
	livoxProto1_device_disablePcloudDataReqFn
		*livoxProto1_device_disablePcloudDataReq;
};

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

// Utility function to find a device in g_attachedDevices by identifier
static std::shared_ptr<livoxProto1::Device> getDevice(
	const std::string& deviceIdentifier
	)
{
	auto it = std::find_if(g_attachedDevices.begin(), g_attachedDevices.end(),
		[&deviceIdentifier](const std::shared_ptr<livoxProto1::Device>& dev) {
			return livoxProto1::comms::deviceIdentifiersEqual(
				dev->discoveredDevice.deviceIdentifier, deviceIdentifier);
		});

	return (it != g_attachedDevices.end()) ? *it : nullptr;
}

// 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;
	std::shared_ptr<livoxProto1::Device> device;
private:
	std::unique_ptr<boost::asio::deadline_timer> delayTimer;

public:
	void attachDeviceReq1(
		std::shared_ptr<AttachDeviceReq> context,
		bool success, std::shared_ptr<livoxProto1::Device> dev)
	{
		if (!success || !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";
		}

		// Set the device in the context and enable point cloud data after 5ms delay
		context->device = dev;
		context->delayedEnablePcloudData(context);
	}

	void attachDeviceReq2(
		std::shared_ptr<AttachDeviceReq> context,
		bool success)
	{
		if (!success)
		{
			std::cerr << __func__ << ": Failed to enable pcloud data for dev "
				<< context->spec->deviceSelector << std::endl;
			context->callOriginalCb(false, context->spec);
			return;
		}

		if (1 || OptionParser::getOptions().verbose)
		{
			std::cout << __func__ << ": Enabled pcloud data for device: "
				<< context->spec->deviceSelector << std::endl;
		}

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

	// Helper method to delay and then call enablePcloudDataReq
	void delayedEnablePcloudData(
		std::shared_ptr<AttachDeviceReq> context)
	{
		// Initialize timer with device's component thread
		delayTimer = std::make_unique<boost::asio::deadline_timer>(
			device->componentThread->getIoService());

		delayTimer->expires_from_now(boost::posix_time::milliseconds(5));
		delayTimer->async_wait(
			std::bind(
				&AttachDeviceReq::delayedEnablePcloudDataCallback,
				context.get(), context,
				std::placeholders::_1));
	}

	// Callback for the delayed enablePcloudDataReq
	void delayedEnablePcloudDataCallback(
		std::shared_ptr<AttachDeviceReq> context,
		const boost::system::error_code& error)
	{
		if (error)
		{
			std::cerr << __func__ << ": Timer error: " << error.message()
				<< std::endl;
			context->callOriginalCb(false, context->spec);
			return;
		}

		(*livoxProto1.livoxProto1_device_enablePcloudDataReq)(
			device,
			{context, std::bind(
			&AttachDeviceReq::attachDeviceReq2,
			context.get(), context,
			std::placeholders::_1)});
	}
};

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

public:
	const std::shared_ptr<smo::device::DeviceAttachmentSpec> spec;
	std::shared_ptr<livoxProto1::Device> device;
private:
	std::unique_ptr<boost::asio::deadline_timer> delayTimer;

public:
	void detachDeviceReq1(
		std::shared_ptr<DetachDeviceReq> context,
		bool success)
	{
		if (!success)
		{
			std::cerr << __func__ << ": Failed to disable pcloud data for dev "
				<< context->spec->deviceSelector << std::endl;
			// Fallthrough.
		}

		// Add 5ms delay before destroying device
		context->delayedDestroyDevice(context);
	}

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

			/**	NOTE:
			 * There's a decent argument for falling through here and still
			 * removing the device from g_attachedDevices.
			 */
			context->callOriginalCb(false, context->spec);
			return;
		}

		// Find the device in g_attachedDevices and remove it.
		auto deviceToRemove = getDevice(context->spec->deviceSelector);
		if (!deviceToRemove)
		{
			std::cerr << __func__ << ": Race condition: device not found "
				"in g_attachedDevices for detachment: "
				<< context->spec->deviceIdentifier << "\n";
			context->callOriginalCb(false, context->spec);
			return;
		}

		// Remove the device from the collection
		g_attachedDevices.erase(
			std::remove(
				g_attachedDevices.begin(), g_attachedDevices.end(),
				deviceToRemove),
			g_attachedDevices.end());

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

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

	// Helper method to delay and then call destroyDeviceReq
	void delayedDestroyDevice(
		std::shared_ptr<DetachDeviceReq> context)
	{
		// Initialize timer with device's component thread
		delayTimer = std::make_unique<boost::asio::deadline_timer>(
			device->componentThread->getIoService());

		delayTimer->expires_from_now(boost::posix_time::milliseconds(5));
		delayTimer->async_wait(
			std::bind(
				&DetachDeviceReq::delayedDestroyDeviceCallback,
				context.get(), context,
				std::placeholders::_1));
	}

	// Callback for the delayed destroyDeviceReq
	void delayedDestroyDeviceCallback(
		std::shared_ptr<DetachDeviceReq> context,
		const boost::system::error_code& error)
	{
		if (error)
		{
			std::cerr << __func__ << ": Timer error: " << error.message()
				<< std::endl;
			// Fallthrough.
		}

		(*livoxProto1.livoxProto1_destroyDeviceReq)(
			context->device,
			{context, std::bind(
				&DetachDeviceReq::detachDeviceReq2,
				context.get(), context,
				std::placeholders::_1)});
	}
};

// 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"));
	livoxProto1.livoxProto1_device_enablePcloudDataReq = reinterpret_cast<
		livoxProto1_device_enablePcloudDataReqFn *>(
			dlsym(
				livoxProto1.dlopenHandle.get(),
				"livoxProto1_device_enablePcloudDataReq"));
	livoxProto1.livoxProto1_device_disablePcloudDataReq = reinterpret_cast<
		livoxProto1_device_disablePcloudDataReqFn *>(
			dlsym(
				livoxProto1.dlopenHandle.get(),
				"livoxProto1_device_disablePcloudDataReq"));

	if (!livoxProto1.livoxProto1_main || !livoxProto1.livoxProto1_exit
		|| !livoxProto1.livoxProto1_getOrCreateDeviceReq
		|| !livoxProto1.livoxProto1_destroyDeviceReq
		|| !livoxProto1.livoxProto1_device_enablePcloudDataReq
		|| !livoxProto1.livoxProto1_device_disablePcloudDataReq)
	{
		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");
	}

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

	// Check if device already exists
	auto existingDevice = getDevice(desc->deviceSelector);
	if (existingDevice)
	{
		// Device already exists, set device and enable point cloud data
		std::cout << __func__ << ": Dev "
			<< existingDevice->discoveredDevice.deviceIdentifier
			<< " already attached for DASpec: " << desc->deviceSelector
			<< ".\n";
		request->device = existingDevice;
		request->delayedEnablePcloudData(request);
		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,
		{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 the device in our collection
	auto device = getDevice(desc->deviceSelector);
	if (!device)
	{
		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, device, cb);

	// Disable point cloud data first
	(*livoxProto1.livoxProto1_device_disablePcloudDataReq)(
		device,
		{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