salmanoff/commonLibs/livoxProto1/core.cpp

#include <boostAsioLinkageFix.h>

#include <algorithm>
#include <iostream>
#include <functional>
#include <optional>
#include <opts.h>
#include <user/senseApiDesc.h>
#include "protocol.h"
#include "core.h"
#include "device.h"
#include "broadcastListener.h"
#include "livoxProto1.h"


namespace livoxProto1 {

static ProtoState protoState =
{
	.isInitialized = false,
	.componentThread = nullptr,
	.deviceManager = nullptr,
	.smoCallbacks = {}
};

ProtoState& getProtoState()
{
	return protoState;
}

DeviceManager::DeviceManager()
: broadcastListener(protoState.componentThread),
  udpCommandDemuxer(protoState.componentThread, *this)
{
	broadcastListener.setDeviceGoneAwayCb(deviceGoneAwayInd);
}

void DeviceManager::deviceGoneAwayInd(const comms::DiscoveredDevice &device)
{
	std::cout << "Device gone away: " << device.stringify() << std::endl;

	// Check if device exists in our collection
	if (!protoState.deviceManager->getDevice(device)) {
		return;
	}

	// Find and remove the device from the collection
	auto it = std::find_if(
		protoState.deviceManager->devices.begin(),
		protoState.deviceManager->devices.end(),
		[&device](const std::shared_ptr<Device> &d) {
			return d->discoveredDevice == device;
		}
	);
	if (it != protoState.deviceManager->devices.end()) {
		protoState.deviceManager->devices.erase(it);
	}
}

std::optional<std::shared_ptr<Device>> DeviceManager::getDevice(
	const std::string &deviceIdentifier
	)
{
	for (auto& device : devices)
	{
		if (comms::deviceIdentifiersEqual(
			device->discoveredDevice.deviceIdentifier, deviceIdentifier))
		{
			return device;
		}
	}
	return std::nullopt;
}

sscl::co::ViralNonPostingInvoker<LivoxProto1GetOrCreateDeviceResult>
DeviceManager::getOrCreateDeviceCReq(
	const std::string &deviceIdentifier,
	const std::shared_ptr<sscl::ComponentThread>& componentThread,
	int commandTimeoutMs, int retryDelayMs,
	const std::string& smoIp, uint8_t smoSubnetNbits,
	uint16_t dataPort, uint16_t cmdPort, uint16_t imuPort)
{
	LivoxProto1GetOrCreateDeviceResult result;

	// Validate smoIp format using Boost.Asio IPv4 validation
	if (!smoIp.empty() && !comms::isValidIPv4(smoIp))
	{
		throw std::invalid_argument(
			std::string(__func__) +
			": Invalid IPv4 smoIp format: " + smoIp);
	}

	// Validate subnet nbits
	if (smoSubnetNbits > 32)
	{
		throw std::invalid_argument(
			std::string(__func__) +
			": smoSubnetNbits must be between 0 and 32, got: " +
			std::to_string(smoSubnetNbits));
	}

	// First try to get existing device
	auto existingDevice = getDevice(deviceIdentifier);
	if (existingDevice)
	{
		result.success = true;
		result.device = existingDevice.value();
		co_return result;
	}

	// Device doesn't exist, create a new one but don't add it to collection yet
	auto newDevice = std::make_shared<Device>(
		deviceIdentifier, componentThread,
		commandTimeoutMs, retryDelayMs,
		smoIp, smoSubnetNbits,
		dataPort, cmdPort, imuPort);

	// Start the connection process - only add to collection on success
	const bool connectSuccess = co_await newDevice->connectCReq();
	if (!connectSuccess)
	{
		std::cerr << __func__ << ": Connection failed for device "
			<< newDevice->discoveredDevice.deviceIdentifier
			<< std::endl;
		co_return result;
	}

	devices.push_back(newDevice);
	if (getProtoState().smoCallbacks.OptionParser_getOptions().verbose)
	{
		std::cout << __func__ << ": Successfully connected and added device "
			<< newDevice->discoveredDevice.deviceIdentifier
			<< std::endl;
	}

	result.success = true;
	result.device = newDevice;
	co_return result;
}

sscl::co::ViralNonPostingInvoker<bool> DeviceManager::destroyDeviceCReq(
	std::shared_ptr<Device> dev)
{
	/**		EXPLANATION:
	 * Check to see if the device is in our collection. If so, call
	 * disconnectCReq and then remove it.
	 */
	std::shared_ptr<Device> device = getDevice(dev->discoveredDevice).
		value_or(nullptr);

	if (!device || device->nAttachedStimulusProducers > 0) {
		co_return false;
	}

	const bool success = co_await device->disconnectCReq();

	devices.erase(
		std::remove(devices.begin(), devices.end(), device),
		devices.end());

	co_return success;
}

void main(const std::shared_ptr<sscl::ComponentThread> &componentThread,
	const smo::stim_buff::SmoCallbacks& smoCallbacks)
{
	if (protoState.isInitialized) {
		return;
	}

	protoState.isInitialized = true;
	protoState.componentThread = componentThread;
	protoState.smoCallbacks = smoCallbacks;
	protoState.deviceManager = std::make_unique<DeviceManager>();
	protoState.deviceManager->broadcastListener.start();
	protoState.deviceManager->udpCommandDemuxer.start();
}

void exit(void)
{
	if (!protoState.isInitialized) {
		return;
	}

	protoState.deviceManager->udpCommandDemuxer.stop();
	protoState.deviceManager->broadcastListener.stop();
	protoState.deviceManager.reset();
	protoState.componentThread.reset();
	protoState.isInitialized = false;
}

} // namespace livoxProto1