salmanoff/commonLibs/livoxProto1/core.cpp

#include <algorithm>
#include <iostream>
#include <functional>
#include <optional>
#include <opts.h>
#include <asynchronousContinuation.h>
#include <callback.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)
{
	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;
}

// GetOrCreateDeviceReq nested class implementation
class DeviceManager::GetOrCreateDeviceReq
:	public smo::NonPostedAsynchronousContinuation<
		livoxProto1_getOrCreateDeviceReqCbFn>
{
public:
	DeviceManager& deviceManager;
	// The device we're trying to connect (holds all connection parameters)
	std::shared_ptr<Device> pendingDevice;

public:
	GetOrCreateDeviceReq(
		DeviceManager& mgr,
		std::shared_ptr<Device> device,
		smo::Callback<livoxProto1_getOrCreateDeviceReqCbFn> cb)
	:	smo::NonPostedAsynchronousContinuation<
			livoxProto1_getOrCreateDeviceReqCbFn>(std::move(cb)),
	deviceManager(mgr), pendingDevice(device)
	{}

	// Public accessor for the original callback
	void callOriginalCallback(bool success, std::shared_ptr<Device> device)
		{ callOriginalCb(success, device); }

	void callOriginalCallbackWithFailure()
		{ callOriginalCallback(false, nullptr); }

	void getOrCreateDeviceReq1(
		std::shared_ptr<GetOrCreateDeviceReq> context, bool connectSuccess
		)
	{
		if (!connectSuccess)
		{
			std::cerr << __func__ << ": Connection failed for device "
				<< context->pendingDevice->discoveredDevice.deviceIdentifier
				<< std::endl;
			context->callOriginalCallbackWithFailure();
			return;
		}

		// Connection successful, add device to collection
		context->deviceManager.devices.push_back(context->pendingDevice);
		if (OptionParser::getOptions().verbose)
		{
			std::cout << __func__ << ": Successfully connected and added device "
				<< context->pendingDevice->discoveredDevice.deviceIdentifier
				<< std::endl;
		}

		// Return success with the connected device
		context->callOriginalCallback(true, context->pendingDevice);
	}
};

void DeviceManager::getOrCreateDeviceReq(
	const std::string &deviceIdentifier,
	const std::shared_ptr<smo::ComponentThread>& componentThread,
	int handshakeTimeoutMs, int retryDelayMs,
	const std::string& smoIp, uint8_t smoSubnetNbits,
	uint16_t dataPort, uint16_t cmdPort, uint16_t imuPort,
	smo::Callback<livoxProto1_getOrCreateDeviceReqCbFn> callback)
{
	// 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)
	{
		// Device already exists and is connected, return it
		callback.callbackFn(true, existingDevice.value());
		return;
	}

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

	// Create the continuation request object to hold state and callbacks
	auto request = std::make_shared<GetOrCreateDeviceReq>(
		*this, newDevice, std::move(callback));

	// Start the connection process - only add to collection on success
	request->pendingDevice->connectReq(
		{request, std::bind(
			&DeviceManager::GetOrCreateDeviceReq::getOrCreateDeviceReq1,
			request.get(), request, std::placeholders::_1)});
}

class DeviceManager::DestroyDeviceReq
:	public smo::NonPostedAsynchronousContinuation<
		livoxProto1_destroyDeviceReqCbFn>
{
public:
	DeviceManager& deviceManager;
	std::shared_ptr<Device> pendingDevice;

public:
	DestroyDeviceReq(
		DeviceManager& mgr,
		std::shared_ptr<Device> device,
		smo::Callback<livoxProto1_destroyDeviceReqCbFn> cb)
	:	smo::NonPostedAsynchronousContinuation<
			livoxProto1_destroyDeviceReqCbFn>(std::move(cb)),
	deviceManager(mgr), pendingDevice(device)
	{}

	// Public accessor for the original callback
	void callOriginalCallback(bool success)
		{ callOriginalCb(success); }

	void callOriginalCallbackWithFailure()
		{ callOriginalCallback(false); }

	void destroyDeviceReq1(
		std::shared_ptr<DestroyDeviceReq> context, bool success
		)
	{
		context->deviceManager.devices.erase(
			std::remove(
				context->deviceManager.devices.begin(),
				context->deviceManager.devices.end(),
				context->pendingDevice),
			context->deviceManager.devices.end());

		context->callOriginalCallback(success);
	}
};

void DeviceManager::destroyDeviceReq(
	std::shared_ptr<Device> dev,
	smo::Callback<livoxProto1_destroyDeviceReqCbFn> callback
)
{
	/**		EXPLANATION:
	 * Check to see if the device is in our collection. If so, call
	 * disconnectReq and then remove it.
	 */
	std::shared_ptr<Device> device = getDevice(dev->discoveredDevice).
		value_or(nullptr);

	if (!device)
	{
		callback.callbackFn(false);
		return;
	}

	auto request = std::make_shared<DestroyDeviceReq>(
		*this, device, std::move(callback));

	device->disconnectReq(
		{request, std::bind(
			&DeviceManager::DestroyDeviceReq::destroyDeviceReq1,
			request.get(), request, std::placeholders::_1)});
}

void main(const std::shared_ptr<smo::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();
}

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

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

} // namespace livoxProto1