salmanoff/stimBuffApis/livoxGen1/livoxGen1.cpp

#include <boostAsioLinkageFix.h>
#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>
#include "pcloudDataProducer.h"
#include "livoxGen1.h"


namespace smo {
namespace stim_buff {

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

// Local collection of stimulus producers
static std::vector<std::shared_ptr<StimulusProducer>> attachedStimulusProducers;

// Get stimulus producer by device attachment spec
static std::shared_ptr<StimulusProducer>
getStimulusProducer(
	const std::shared_ptr<smo::device::DeviceAttachmentSpec>& spec
	)
{
	for (const auto& stimProducer : attachedStimulusProducers)
	{
		// Compare device selectors to find matching buffer
		if (livoxProto1::comms::deviceIdentifiersEqual(
			stimProducer->deviceAttachmentSpec->deviceSelector,
			spec->deviceSelector))
		{
			return stimProducer;
		}
	}

	return nullptr;
}

// LivoxProto1DllState constructor implementation
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),
  livoxProto1_device_getReturnModeReq(nullptr),
  livoxProto1_getPcloudDataFdDesc(nullptr)
{}

// LivoxProto1DllState DlCloser implementation
void LivoxProto1DllState::DlCloser(void* handle)
{
	if (handle) {
		dlclose(handle);
	}
}

LivoxProto1DllState livoxProto1;

// 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<PcloudDataProducer> stimProducer;
	std::shared_ptr<livoxProto1::Device> deviceTmp;

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/find Livox device: "
				<< context->spec->deviceSelector << std::endl;
			context->callOriginalCb(false, context->spec);
			return;
		}

		// Stash device pointer until after getReturnMode succeeds
		context->deviceTmp = dev;

		if (1 || smoHooksPtr->OptionParser_getOptions().verbose)
		{
			std::cout << __func__ << ": Successfully attached/found Livox "
				"device: " << context->spec->deviceSelector << " (ID: "
				<< context->spec->deviceIdentifier << ")\n";
		}

		/* Delay here because getOrCreate just sent HandshakeReq, so device
		 * may not yet be ready for another command.
		 */
		context->delayedGetReturnMode(context);
	}

	void delayedGetReturnMode(
		std::shared_ptr<AttachDeviceReq> context)
	{
		// Initialize timer with device's component thread
		delayTimer = std::make_unique<boost::asio::deadline_timer>(
			context->deviceTmp->componentThread->getIoService());

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

	void attachDeviceReq2(
		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_getReturnModeReq)(
			context->deviceTmp,
			{context, std::bind(
				&AttachDeviceReq::attachDeviceReq3,
				context.get(), context,
				std::placeholders::_1, std::placeholders::_2)});
	}

	void attachDeviceReq3(
		std::shared_ptr<AttachDeviceReq> context,
		bool success, uint8_t mode)
	{
		if (!success)
		{
			std::cerr << __func__ << ": Failed to get return mode for dev "
				<< context->spec->deviceSelector << std::endl;

			context->callOriginalCb(false, context->spec);
			return;
		}

		// Parse history buffer duration from quale-iface-api-params
		int histbuffMs = 30000;  // Default: 30000ms (30 seconds)
		(void)histbuffMs;
		const std::vector<std::string> histbuffParamNames = {
			"history-buffer-duration-ms",
			"hist-buff-duration-ms",
			"histbuff-duration-ms",
			"histbuff-ms"
		};

		// Loop through synonyms in reverse order; lattermost synonym wins.
		for (auto synIt = histbuffParamNames.rbegin();
			synIt != histbuffParamNames.rend(); ++synIt)
		{
			const auto& paramName = *synIt;
			try {
				histbuffMs = smo::device::DeviceAttachmentSpec
					::parseRequiredParamAsInt(
						context->spec->qualeIfaceApiParams, paramName);
				break; // Found and parsed successfully
			} catch (const std::exception&) {
				// Parameter not found or parse error, continue to next synonym
				continue;
			}
		}

		// Create and add PcloudDataProducer to collection now that device is ready
		PcloudDataProducer::PcloudFormatDesc formatDesc;
		formatDesc.format = PcloudDataProducer::PcloudFormatDesc::Format::XYZI;
		auto pcloudDataProducer = std::make_shared<PcloudDataProducer>(
			context->spec, context->deviceTmp, formatDesc, 30);

		context->stimProducer = pcloudDataProducer;
		context->deviceTmp->nAttachedStimBuffs++;
		attachedStimulusProducers.push_back(pcloudDataProducer);

		pcloudDataProducer->start();

		if (1 || smoHooksPtr->OptionParser_getOptions().verbose)
		{
			std::cout << __func__ << ": Got return mode (" << (int)mode
				<< ") for device: " << context->spec->deviceSelector
				<< std::endl;
		}

		context->delayedEnablePcloudData(context);
	}

	// 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>(
			context->stimProducer->device->componentThread->getIoService());

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

	void attachDeviceReq4(
		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)(
			context->stimProducer->device,
			{context, std::bind(
			&AttachDeviceReq::attachDeviceReq5,
			context.get(), context,
			std::placeholders::_1)});
	}

	void attachDeviceReq5(
		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 || smoHooksPtr->OptionParser_getOptions().verbose)
		{
			std::cout << __func__ << ": Enabled pcloud data for device: "
				<< context->spec->deviceSelector << std::endl;
		}

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

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

public:
	const std::shared_ptr<smo::device::DeviceAttachmentSpec> spec;
	std::shared_ptr<PcloudDataProducer> stimProducer;
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 "
				"stim producer " << context->spec->deviceSelector << std::endl;
			// Fallthrough.
		}

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

	// 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>(
			context->stimProducer->device->componentThread->getIoService());

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

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

		context->stimProducer->stop();
		// Remove stimulus producer from collection before destroying device
		context->stimProducer->device->nAttachedStimBuffs--;
		auto it = std::find(
			attachedStimulusProducers.begin(), attachedStimulusProducers.end(),
			context->stimProducer);
		if (it != attachedStimulusProducers.end())
			{ attachedStimulusProducers.erase(it); }

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

	void detachDeviceReq2(
		std::shared_ptr<DetachDeviceReq> context,
		bool success)
	{
		if (!success)
		{
			std::cerr << __func__ << ": Failed to destroy dev "
				"device " << context->spec->deviceSelector << " for stim "
				"producer.\n";

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

		if (1 || smoHooksPtr->OptionParser_getOptions().verbose)
		{
			std::cout << __func__ << ": Successfully detached pcloud stim "
				"producer for device " << context->spec->deviceSelector
				<< " and possibly also destroyed device.\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"));
	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"));
	livoxProto1.livoxProto1_device_getReturnModeReq = reinterpret_cast<
		livoxProto1_device_getReturnModeReqFn *>(
			dlsym(
				livoxProto1.dlopenHandle.get(),
				"livoxProto1_device_getReturnModeReq"));
	livoxProto1.livoxProto1_getPcloudDataFdDesc = reinterpret_cast<
		livoxProto1_getPcloudDataFdDescFn *>(
			dlsym(
				livoxProto1.dlopenHandle.get(),
				"livoxProto1_getPcloudDataFdDesc"));

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

	attachedStimulusProducers.clear();

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

	livoxProto1.dlopenHandle.reset(nullptr);

	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 stimulus producer already exists in the collection
	auto pcloudDataProducer = std::static_pointer_cast<PcloudDataProducer>(
		getStimulusProducer(desc));

	if (pcloudDataProducer)
	{
		request->stimProducer = pcloudDataProducer;

		// Check if device's point cloud data is already active
		if (pcloudDataProducer->device && pcloudDataProducer->device->pcloudDataActive)
		{
			// Point cloud data is already active, call success callback
			request->callOriginalCb(true, request->spec);
			return;
		}

		/* Enable pcloud data first. Don't need delay since no commands were
		 * sent to device prior to us reaching here.
		 */
		(*livoxProto1.livoxProto1_device_enablePcloudDataReq)(
			pcloudDataProducer->device,
			{request, std::bind(
				&AttachDeviceReq::attachDeviceReq5,
				request.get(), request,
				std::placeholders::_1)});
		return;
	}

	// Parse integer parameters from provider params with defaults
	/**	EXPLANATION:
	 * We may want to add a new param here called "command-delay-ms" to control
	 * the delay we insert between commands sent to the device. 5ms has been
	 * shown to be sufficient for the Livox Avia.
	 */

	/* The Livox Avia will generally respond to a handshake request within
	 * 5ms.
	 */
	int commandTimeoutMs = 5;		// Default: 5ms
	/* 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: 5250ms
	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 == "cmd-timeout-ms")
		{
			commandTimeoutMs = smo::device::DeviceAttachmentSpec
				::parseRequiredParamAsInt(
					desc->providerParams, "cmd-timeout-ms");
		} else if (param.first == "command-timeout-ms")
		{
			commandTimeoutMs = smo::device::DeviceAttachmentSpec
				::parseRequiredParamAsInt(
					desc->providerParams, "command-timeout-ms");
		} else if (param.first == "retry-delay-ms")
		{
			retryDelayMs = smo::device::DeviceAttachmentSpec
				::parseRequiredParamAsInt(
					desc->providerParams, "retry-delay-ms");
		} else if (param.first == "smo-subnet-nbits")
		{
			smoSubnetNbits = static_cast<uint8_t>(
				smo::device::DeviceAttachmentSpec
				::parseRequiredParamAsInt(
					desc->providerParams, "smo-subnet-nbits"));
		} else if (param.first == "data-port")
		{
			dataPort = static_cast<uint16_t>(
				smo::device::DeviceAttachmentSpec
				::parseRequiredParamAsInt(desc->providerParams, "data-port"));
		} else if (param.first == "cmd-port")
		{
			cmdPort = static_cast<uint16_t>(
				smo::device::DeviceAttachmentSpec
				::parseRequiredParamAsInt(desc->providerParams, "cmd-port"));
		} else if (param.first == "imu-port")
		{
			imuPort = static_cast<uint16_t>(
				smo::device::DeviceAttachmentSpec
				::parseRequiredParamAsInt(desc->providerParams, "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,
		commandTimeoutMs, 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
	)
{
	// Check if stimulus producer exists in the collection
	auto stimProducer = std::static_pointer_cast<PcloudDataProducer>(
		getStimulusProducer(desc));

	if (!stimProducer)
	{
		cb.callbackFn(false, desc);
		return;
	}

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

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