salmanoff/commonLibs/livoxProto1/core.cpp

#include <algorithm>
#include <iostream>
#include <user/senseApiDesc.h>
#include "protocol.h"
#include "core.h"

namespace livoxProto1 {

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

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::shared_ptr<Device> DeviceManager::getOrCreateDevice(
	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
	)
{
	// 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) {
		return existingDevice;
	}

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

	// Add to our collection
	devices.push_back(newDevice);
	return newDevice;
}

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

bool DeviceManager::isDeviceKnown(const std::string& deviceIdentifier)
{
	return broadcastListener.deviceExists(deviceIdentifier);
}

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

	protoState.isInitialized = true;
	protoState.componentThread = componentThread;
	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