livoxGen1: Implement attachDevice/detachDevice

Preliminary draft implementation. Unsurprisingly it doesn't
work. The handshake doesn't get accepted by the device it seems?
And we can't receive bcast dgrams since we're blocking
synchronously -- therefore we're not processing the incoming UDP
dgrams.

senseApis/livoxGen1/livoxGen1.cpp

@@ -4,10 +4,12 @@
 #include <string>
 #include <map>
 #include <functional>
+#include <algorithm>
 #include <dlfcn.h>
 #include <user/senseApiDesc.h>
 #include <user/deviceAttachmentSpec.h>
 #include <livoxProto1/livoxProto1.h>
+#include <livoxProto1/device.h>
 
 namespace smo {
 namespace sense_api {
@@ -22,7 +24,8 @@ struct LivoxProto1DllState
 	LivoxProto1DllState()
 	: dlopenHandle(nullptr, DlCloser),
 	  livoxProto1_main(nullptr),
-	  livoxProto1_exit(nullptr)
+	  livoxProto1_exit(nullptr),
+	  livoxProto1_getOrCreateDevice(nullptr)
 	{}
 
 	static void DlCloser(void* handle)
@@ -35,10 +38,14 @@ struct LivoxProto1DllState
 	std::unique_ptr<void, void(*)(void*)> dlopenHandle;
 	livoxProto1_mainFn *livoxProto1_main;
 	livoxProto1_exitFn *livoxProto1_exit;
+	livoxProto1_getOrCreateDeviceFn *livoxProto1_getOrCreateDevice;
 };
 
 static LivoxProto1DllState livoxProto1;
 
+// Attached Livox devices
+static std::vector<std::shared_ptr<livoxProto1::Device>> g_attachedDevices;
+
 // Callback function declarations
 extern "C" int livoxGen1_initializeInd(void);
 extern "C" int livoxGen1_finalizeInd(void);
@@ -93,24 +100,34 @@ extern "C" int livoxGen1_initializeInd(void)
 		dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_main"));
 	livoxProto1.livoxProto1_exit = reinterpret_cast<livoxProto1_exitFn *>(
 		dlsym(livoxProto1.dlopenHandle.get(), "livoxProto1_exit"));
+	livoxProto1.livoxProto1_getOrCreateDevice = reinterpret_cast<
+		livoxProto1_getOrCreateDeviceFn *>(
+			dlsym(
+				livoxProto1.dlopenHandle.get(),
+				"livoxProto1_getOrCreateDevice"));
 
-	if (!livoxProto1.livoxProto1_main || !livoxProto1.livoxProto1_exit) {
+	if (!livoxProto1.livoxProto1_main || !livoxProto1.livoxProto1_exit
+		|| !livoxProto1.livoxProto1_getOrCreateDevice)
+	{
 		throw std::runtime_error(
 			std::string(__func__) +
 			": Failed to get LivoxProto1 library functions");
 	}
 
 	// Call LivoxProto1 library main function
-	livoxProto1.livoxProto1_main(smoThreadingModelDesc.componentThread);
+	(*livoxProto1.livoxProto1_main)(smoThreadingModelDesc.componentThread);
 
 	return 0; // Success
 }
 
 extern "C" int livoxGen1_finalizeInd(void)
 {
-	// TODO: Implement finalization logic
+	// Clear all attached devices
+	g_attachedDevices.clear();
+
+	// Call LivoxProto1 library exit function
 	if (livoxProto1.livoxProto1_exit) {
-		livoxProto1.livoxProto1_exit();
+		(*livoxProto1.livoxProto1_exit)();
 	}
 
 	if (livoxProto1.dlopenHandle)
@@ -127,8 +144,84 @@ extern "C" int livoxGen1_attachDeviceReq(
 	const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc
 	)
 {
-	// TODO: Implement device attachment logic
+	if (!livoxProto1.livoxProto1_getOrCreateDevice)
-	(void)desc; // Suppress unused parameter warning
+	{
+		throw std::runtime_error(
+			std::string(__func__) + ": LivoxProto1 getOrCreateDevice function "
+			"not available");
+	}
+
+	// Parse integer parameters from provider params with defaults
+	int handshakeTimeoutMs = 250;	// Default: 50ms
+	int retryDelayMs = 500;			// 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: 10.42.0.1
+	std::string smoIp = "10.42.0.1";
+
+	// 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") {
+			smoIp = param.second;
+		}
+		else
+		{
+			throw std::runtime_error(
+				std::string(__func__) + ": Unknown provider parameter: "
+				+ param.first);
+		}
+	}
+
+	// Call getOrCreateDevice with parsed parameters
+	auto device = (*livoxProto1.livoxProto1_getOrCreateDevice)(
+		desc->deviceSelector,  // deviceIdentifier (broadcast code)
+		smoThreadingModelDesc.componentThread,
+		handshakeTimeoutMs, retryDelayMs,
+		smoIp, smoSubnetNbits,
+		dataPort, cmdPort, imuPort);
+
+	if (!device)
+	{
+		throw std::runtime_error(
+			std::string(__func__) + ": Failed to create Livox device: "
+			+ desc->deviceSelector);
+	}
+
+	g_attachedDevices.push_back(device);
+	std::cout << __func__ << ": Successfully attached Livox device: "
+		<< desc->deviceSelector << " (ID: " << desc->deviceIdentifier << ")\n";
+
 	return 0; // Success
 }
 
@@ -136,8 +229,32 @@ extern "C" int livoxGen1_detachDeviceReq(
 	const std::shared_ptr<smo::device::DeviceAttachmentSpec>& desc
 	)
 {
-	// TODO: Implement device detachment logic
+	// Find and remove the device from our collection
-	(void)desc; // Suppress unused parameter warning
+	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 << __func__ << ": Device not found for detachment: "
+			<< desc->deviceIdentifier << "\n";
+		return -1; // Device not found
+	}
+
+	g_attachedDevices.erase(it);
+	std::cout << __func__ << ": Successfully detached Livox device: "
+		<< desc->deviceIdentifier << "\n";
 	return 0; // Success
 }