livoxGen1: Implement StimBuff add/del from StimProducers

There seems to be a bug where two or more stimProducers
or stimBuffs get initialized at once but we can deal with that
tomorrow.

include/user/stimulusProducer.h

@@ -72,6 +72,10 @@ public:
 	std::shared_ptr<StimulusBuffer> getAttachedStimulusBuffer(
 		const std::shared_ptr<device::DeviceAttachmentSpec>& spec) const;
 
+	virtual std::shared_ptr<StimulusBuffer> getOrCreateAttachedStimulusBuffer(
+		const std::shared_ptr<device::DeviceAttachmentSpec>& deviceAttachmentSpec,
+		int histbuffMs) = 0;
+
 protected:
 	SpinLock frameAssemblyRateLimiter;
 

stimBuffApis/livoxGen1/livoxGen1.cpp

@@ -30,6 +30,33 @@ static SmoThreadingModelDesc smoThreadingModelDesc;
 // Local collection of stimulus producers
 static std::vector<std::shared_ptr<StimulusProducer>> attachedStimulusProducers;
 
+// Check if a StimulusProducer matches the requested stim feature
+static bool isProducerForStimFeature(
+	const std::shared_ptr<StimulusProducer>& stimProducer,
+	const std::string& qualeIfaceApi)
+{
+	// Check if the qualeIfaceApi requires a PcloudStimulusProducer
+	if (qualeIfaceApi == "mesh" || qualeIfaceApi == "pcloudIntensity" ||
+		qualeIfaceApi == "pcloudAmbience")
+	{
+		// Attempt to upcast to PcloudStimulusProducer
+		auto pcloudProducer = std::dynamic_pointer_cast<PcloudStimulusProducer>(
+			stimProducer);
+
+		return pcloudProducer != nullptr;
+	}
+	else if (qualeIfaceApi == "gyro" || qualeIfaceApi == "accel")
+	{
+		/**	TODO:
+		 * Add upcast mappings for gyro and accel later when we implement
+		 * ImuStimulusProducer.
+		 */
+		return false;
+	}
+
+	return false;
+}
+
 // Get stimulus producer by device attachment spec
 static std::shared_ptr<StimulusProducer>
 getStimulusProducer(
@@ -41,7 +68,8 @@ getStimulusProducer(
 		// Compare device selectors to find matching buffer
 		if (livoxProto1::comms::deviceIdentifiersEqual(
 			stimProducer->deviceAttachmentSpec->deviceSelector,
-			spec->deviceSelector))
+			spec->deviceSelector)
+			&& isProducerForStimFeature(stimProducer, spec->qualeIfaceApi))
 		{
 			return stimProducer;
 		}
@@ -50,6 +78,37 @@ getStimulusProducer(
 	return nullptr;
 }
 
+// Helper function to parse histbuffMs from device attachment spec
+static int parseHistbuffMs(
+	const std::shared_ptr<smo::device::DeviceAttachmentSpec>& spec)
+{
+	int histbuffMs = 30000;  // Default: 30000ms (30 seconds)
+	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(
+					spec->qualeIfaceApiParams, paramName);
+			break; // Found and parsed successfully
+		} catch (const std::exception&) {
+			// Parameter not found or parse error, continue to next synonym
+			continue;
+		}
+	}
+
+	return histbuffMs;
+}
+
 // LivoxProto1DllState constructor implementation
 LivoxProto1DllState::LivoxProto1DllState()
 : dlopenHandle(nullptr, DlCloser),
@@ -94,6 +153,34 @@ public:
 private:
 	std::unique_ptr<boost::asio::deadline_timer> delayTimer;
 
+	// Helper method to ensure StimBuffer is attached
+	// Returns true if successful, false on error
+	bool ensureStimBufferAttached(std::shared_ptr<AttachDeviceReq> context)
+	{
+		if (!context->stimProducer)
+		{
+			std::cerr << __func__ << ": stimProducer is null" << std::endl;
+			return false;
+		}
+
+		// Parse histbuffMs
+		int histbuffMs = parseHistbuffMs(context->spec);
+
+		// Call getOrCreateAttachedStimulusBuffer (may throw, catch and return failure)
+		try {
+			context->stimProducer->getOrCreateAttachedStimulusBuffer(
+				context->spec, histbuffMs);
+		} catch (const std::exception& e) {
+			std::cerr << __func__ << ": Failed to create StimBuffer: "
+				<< e.what() << ". Producer is committed, DeviceReattacher will retry."
+				<< std::endl;
+			// Return false so DeviceReattacher can retry later
+			return false;
+		}
+
+		return true;
+	}
+
 public:
 	void attachDeviceReq1(
 		std::shared_ptr<AttachDeviceReq> context,
@@ -153,12 +240,12 @@ public:
 		(*livoxProto1.livoxProto1_device_getReturnModeReq)(
 			context->deviceTmp,
 			{context, std::bind(
-				&AttachDeviceReq::attachDeviceReq3,
+				&AttachDeviceReq::attachDeviceReq3_doCreateStimProducer,
 				context.get(), context,
 				std::placeholders::_1, std::placeholders::_2)});
 	}
 
-	void attachDeviceReq3(
+	void attachDeviceReq3_doCreateStimProducer(
 		std::shared_ptr<AttachDeviceReq> context,
 		bool success, uint8_t mode)
 	{
@@ -171,33 +258,19 @@ public:
 			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)
+		/* Check if PcloudStimulusProducer already exists
+		 * (race condition or double-add)
+		 */
+		auto existingProducer = getStimulusProducer(context->spec);
+		if (existingProducer)
 		{
-			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;
-			}
+			throw std::runtime_error(
+				std::string(__func__) + ": PcloudStimulusProducer already "
+				"exists for device " + context->spec->deviceSelector + " "
+				"(race condition or double-add)");
 		}
 
-		// Create and add PcloudStimulusProducer to collection now that device is ready
+		// Create & add PcloudStimulusProducer to collection since dev now ready
 		PcloudStimulusProducer::PcloudFormatDesc formatDesc;
 		formatDesc.format = PcloudStimulusProducer::PcloudFormatDesc::Format
 			::XYZI;
@@ -218,46 +291,51 @@ public:
 				<< std::endl;
 		}
 
-		context->delayedEnablePcloudData(context);
+		// Ensure StimBuffer is attached
+		attachDeviceReq4_doCreateStimBuff_maybeDirectlyCalled(context);
 	}
 
-	// Helper method to delay and then call enablePcloudDataReq
-	void delayedEnablePcloudData(
-		std::shared_ptr<AttachDeviceReq> context)
+	// Ensure StimBuffer is attached
+	void attachDeviceReq4_doCreateStimBuff_maybeDirectlyCalled(
+		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));
+		// Ensure StimBuffer is attached
+		if (!ensureStimBufferAttached(context))
+		{
+			context->callOriginalCb(false, context->spec);
+			return;
 		}
 
-	void attachDeviceReq4(
-		std::shared_ptr<AttachDeviceReq> context,
-		const boost::system::error_code& error)
+		// Continue to enable pcloud data if needed
+		attachDeviceReq5_doEnablePcloudData_maybeDirectlyCalled(context);
+	}
+
+	// Enable pcloud data if needed
+	void attachDeviceReq5_doEnablePcloudData_maybeDirectlyCalled(
+		std::shared_ptr<AttachDeviceReq> context
+		)
 	{
-		if (error)
+		if (!context->stimProducer || !context->stimProducer->device)
 		{
-			std::cerr << __func__ << ": Timer error: " << error.message()
+			std::cerr << __func__ << ": stimProducer or device is null"
 				<< std::endl;
 			context->callOriginalCb(false, context->spec);
 			return;
 		}
 
+		/* Enable pcloud data. Don't need delay since no commands were
+		 * sent to device prior to us reaching here (or delay already handled).
+		 */
 		(*livoxProto1.livoxProto1_device_enablePcloudDataReq)(
 			context->stimProducer->device,
 			{context, std::bind(
-			&AttachDeviceReq::attachDeviceReq5,
+				&AttachDeviceReq::attachDeviceReq6,
 				context.get(), context,
 				std::placeholders::_1)});
 	}
 
-	void attachDeviceReq5(
+	void attachDeviceReq6(
 		std::shared_ptr<AttachDeviceReq> context,
 		bool success)
 	{
@@ -286,16 +364,16 @@ class DetachDeviceReq
 public:
 	DetachDeviceReq(
 		const std::shared_ptr<smo::device::DeviceAttachmentSpec>& spec,
-		const std::shared_ptr<PcloudStimulusProducer>& stimProducer,
+		const std::shared_ptr<StimulusBuffer>& stimBuffer,
 		smo::Callback<sal_mlo_detachDeviceReqCbFn> cb)
 	:	smo::NonPostedAsynchronousContinuation<sal_mlo_detachDeviceReqCbFn>(
 		std::move(cb)),
-	spec(spec), stimProducer(stimProducer)
+	spec(spec), stimBuffer(stimBuffer)
 	{}
 
 public:
 	const std::shared_ptr<smo::device::DeviceAttachmentSpec> spec;
-	std::shared_ptr<PcloudStimulusProducer> stimProducer;
+	std::shared_ptr<StimulusBuffer> stimBuffer;
 private:
 	std::unique_ptr<boost::asio::deadline_timer> delayTimer;
 
@@ -319,9 +397,9 @@ public:
 	void delayedDestroyDevice(
 		std::shared_ptr<DetachDeviceReq> context)
 	{
-		// Initialize timer with device's component thread
+		// Initialize timer with LivoxGen1 metadata io_service
 		delayTimer = std::make_unique<boost::asio::deadline_timer>(
-			context->stimProducer->device->componentThread->getIoService());
+			smoThreadingModelDesc.componentThread->getIoService());
 
 		delayTimer->expires_from_now(boost::posix_time::milliseconds(5));
 		delayTimer->async_wait(
@@ -343,17 +421,65 @@ public:
 			// Fallthrough.
 		}
 
-		context->stimProducer->stop();
-		// Remove stimulus producer from collection before destroying device
-		context->stimProducer->device->nAttachedStimulusProducers--;
+		// Remove StimBuffer from collection if it exists
+		if (!context->stimBuffer)
+		{
+			throw std::runtime_error(std::string(__func__)
+				+ ": stimBuffer (API: " + context->spec->stimBuffApi + ") "
+				+ "is missing in detachDeviceReq1_delayed "
+				+ "for device " + context->spec->deviceSelector);
+		}
+
+		// Get the producer from the buffer's parent
+		auto& stimProducer = dynamic_cast<PcloudStimulusProducer&>(
+			context->stimBuffer->parent);
+
 		auto it = std::find(
+			stimProducer.attachedStimulusBuffers.begin(),
+			stimProducer.attachedStimulusBuffers.end(),
+			context->stimBuffer);
+		if (it != stimProducer.attachedStimulusBuffers.end())
+		{
+			stimProducer.attachedStimulusBuffers.erase(it);
+		}
+
+		// Clear specialized buffer members if they match
+		if (stimProducer.xyzStimulusBuffer == context->stimBuffer)
+			{ stimProducer.xyzStimulusBuffer.reset(); }
+		if (stimProducer.iStimulusBuffer == context->stimBuffer)
+			{ stimProducer.iStimulusBuffer.reset(); }
+		if (stimProducer.ambienceStimulusBuffer == context->stimBuffer)
+			{ stimProducer.ambienceStimulusBuffer.reset(); }
+
+		// Check if StimProducer has other buffers
+		if (!stimProducer.attachedStimulusBuffers.empty())
+		{
+			// Other buffers exist - just remove this buffer, done
+			context->callOriginalCb(true, context->spec);
+			return;
+		}
+
+		// No other buffers - stop and remove StimProducer
+		stimProducer.stop();
+		// Remove stimulus producer from collection before destroying device
+		stimProducer.device->nAttachedStimulusProducers--;
+		// Find and remove the producer from the collection by comparing device
+		auto it2 = std::find_if(
 			attachedStimulusProducers.begin(), attachedStimulusProducers.end(),
-			context->stimProducer);
-		if (it != attachedStimulusProducers.end())
-			{ attachedStimulusProducers.erase(it); }
+			[&stimProducer](const std::shared_ptr<StimulusProducer>& p)
+			{
+				/**	FIXME:
+				 * When we implement the ImuStimulusProducer, we need to make
+				 * sure we handle that properly here.
+				 */
+				auto pcloudProd = std::dynamic_pointer_cast<PcloudStimulusProducer>(p);
+				return pcloudProd && pcloudProd->device == stimProducer.device;
+			});
+		if (it2 != attachedStimulusProducers.end())
+			{ attachedStimulusProducers.erase(it2); }
 
 		(*livoxProto1.livoxProto1_destroyDeviceReq)(
-			context->stimProducer->device,
+			stimProducer.device,
 			{context, std::bind(
 				&DetachDeviceReq::detachDeviceReq2,
 				context.get(), context,
@@ -399,8 +525,9 @@ extern "C" sal_mlo_detachDeviceReqFn livoxGen1_detachDeviceReq;
 static const StimBuffApiDesc livoxGen1ApiDesc = {
 	.name = "livoxGen1",
 	.exportedQualeIfaceApis = {
-		{.name = "pcloud"},
+		{.name = "mesh"},
 		{.name = "pcloudIntensity"},
+		{.name = "pcloudAmbience"},
 		{.name = "gyro"},
 		{.name = "accel"}
 	},
@@ -523,33 +650,44 @@ extern "C" void livoxGen1_attachDeviceReq(
 
 	auto request = std::make_shared<AttachDeviceReq>(desc, cb);
 
-	// Check if stimulus producer already exists in the collection
-	auto pcloudDataProducer = std::static_pointer_cast<PcloudStimulusProducer>(
-		getStimulusProducer(desc));
-
-	if (pcloudDataProducer)
+	// Case 1: Check if StimBuffer already exists
+	auto stimProducerBase = getStimulusProducer(desc);
+	if (stimProducerBase)
 	{
-		request->stimProducer = pcloudDataProducer;
+		auto stimProducer = std::static_pointer_cast<PcloudStimulusProducer>(
+			stimProducerBase);
 
-		// Check if device's point cloud data is already active
-		if (pcloudDataProducer->device && pcloudDataProducer->device->pcloudDataActive)
+		auto existingBuffer = stimProducer->getAttachedStimulusBuffer(desc);
+		if (existingBuffer)
 		{
-			// Point cloud data is already active, call success callback
+			// StimBuffer exists, check if pcloud data is active
+			if (stimProducer->device && stimProducer->device->pcloudDataActive)
+			{
+				// Both StimBuffer and pcloud data are active, early return with success
 				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)});
+			// StimBuffer exists but pcloud data is not active, enable it
+			request->stimProducer = stimProducer;
+			request->attachDeviceReq5_doEnablePcloudData_maybeDirectlyCalled(
+				request);
+
 			return;
 		}
+		else
+		{
+			// StimProducer exists, StimBuffer doesn't
+			request->stimProducer = stimProducer;
+			// Ensure StimBuffer is attached and enable pcloud data if needed
+			request->attachDeviceReq4_doCreateStimBuff_maybeDirectlyCalled(
+				request);
+
+			return;
+		}
+	}
+
+	// StimProducer doesn't exist - need to create device first
 
 	// Parse integer parameters from provider params with defaults
 	/**	EXPLANATION:
@@ -654,18 +792,37 @@ extern "C" void livoxGen1_detachDeviceReq(
 	Callback<smo::stim_buff::sal_mlo_detachDeviceReqCbFn> cb
 	)
 {
-	// Check if stimulus producer exists in the collection
-	auto stimProducer = std::static_pointer_cast<PcloudStimulusProducer>(
-		getStimulusProducer(desc));
-
-	if (!stimProducer)
+	// Case 1: Check if StimBuffer doesn't exist (early return)
+	auto stimProducerBase = getStimulusProducer(desc);
+	if (!stimProducerBase)
 	{
-		cb.callbackFn(false, desc);
+		// StimProducer doesn't exist, nothing to detach - success
+		cb.callbackFn(true, desc);
 		return;
 	}
 
+	auto stimProducer = std::dynamic_pointer_cast<PcloudStimulusProducer>(
+		stimProducerBase);
+
+	if (!stimProducer)
+	{
+		throw std::runtime_error(std::string(__func__) +
+			": Failed to cast StimulusProducer to PcloudStimulusProducer "
+			"for device " + desc->deviceSelector);
+	}
+
+	// Check if StimBuffer exists
+	auto stimBuffer = stimProducer->getAttachedStimulusBuffer(desc);
+	if (!stimBuffer)
+	{
+		// StimBuffer doesn't exist, nothing to detach - success
+		cb.callbackFn(true, desc);
+		return;
+	}
+
+	// Case 2: StimBuffer exists - proceed with detach
 	auto request = std::make_shared<DetachDeviceReq>(
-		desc, stimProducer, cb);
+		desc, stimBuffer, cb);
 
 	// Disable point cloud data first
 	(*livoxProto1.livoxProto1_device_disablePcloudDataReq)(

stimBuffApis/livoxGen1/pcloudStimulusProducer.cpp

@@ -100,6 +100,28 @@ void produceStimFrameAck(void)
 {
 }
 
+std::shared_ptr<StimulusBuffer>
+PcloudStimulusProducer::getOrCreateAttachedStimulusBuffer(
+	const std::shared_ptr<device::DeviceAttachmentSpec>& deviceAttachmentSpec,
+	int histbuffMs
+	)
+{
+	// Check if buffer already exists (idempotent)
+	auto existingBuffer = getAttachedStimulusBuffer(deviceAttachmentSpec);
+	if (existingBuffer)
+		{ return existingBuffer; }
+
+	// Create new PcloudXyzStimulusBuffer (for now, always use XYZ type)
+	auto buffer = std::make_shared<PcloudXyzStimulusBuffer>(
+		*this, deviceAttachmentSpec, histbuffMs, openClInputConstraints);
+
+	// Add to collection
+	attachedStimulusBuffers.push_back(buffer);
+	// Update specialized member
+	xyzStimulusBuffer = buffer;
+	return buffer;
+}
+
 void PcloudStimulusProducer::stimFrameProductionTimesliceInd()
 {
 	produceFrameReq({nullptr, nullptr});

stimBuffApis/livoxGen1/pcloudStimulusProducer.h

@@ -62,6 +62,11 @@ public:
 	void start() override;
 	void stop() override;
 
+	std::shared_ptr<StimulusBuffer> getOrCreateAttachedStimulusBuffer(
+		const std::shared_ptr<device::DeviceAttachmentSpec>
+			&deviceAttachmentSpec,
+		int histbuffMs) override;
+
 protected:
 	void stimFrameProductionTimesliceInd() override;