salmanoff/smocore/deviceManager/deviceManager.cpp

#include <iostream>
#include <fstream>
#include <stdexcept>
#include <string>
#include <vector>
#include <sstream>
#include <memory>
#include <opts.h>
#include <asynchronousContinuation.h>
#include <serializedAsynchronousContinuation.h>
#include <callback.h>
#include <componentThread.h>
#include <deviceManager/deviceManager.h>
#include <deviceManager/deviceReattacher.h>
#include <senseApis/senseApiManager.h>
#include <marionette/marionette.h>
#include <mind.h>

namespace smo {
namespace device {

std::vector<std::shared_ptr<DeviceAttachmentSpec>>
    DeviceManager::deviceAttachmentSpecs;
std::vector<std::shared_ptr<Device>>
    DeviceManager::devices;
std::vector<std::shared_ptr<DeviceRole>>
    DeviceManager::attachedDeviceRoles;
std::vector<DeviceAttachmentSpec>
    DeviceManager::commandLineDASpecs;

DeviceManager::~DeviceManager()
{
}

const std::string DeviceManager::stringifyDeviceSpecs(void)
{
	std::ostringstream oss;

	for (const auto& spec : DeviceManager::deviceAttachmentSpecs) {
		oss << "Device Attachment Spec: " << spec->stringify();
	}

	return oss.str();
}

class DeviceManager::NewDeviceAttachmentSpecInd
:	public PostedAsynchronousContinuation<newDeviceAttachmentSpecIndCbFn>
{
public:
	NewDeviceAttachmentSpecInd(
		const std::shared_ptr<DeviceAttachmentSpec> &s,
		const std::shared_ptr<Device> &d,
		const std::shared_ptr<ComponentThread> &caller,
		Callback<newDeviceAttachmentSpecIndCbFn> cb)
	:	PostedAsynchronousContinuation<newDeviceAttachmentSpecIndCbFn>(
			caller, cb),
	spec(s), device(d)
	{}

public:
	std::shared_ptr<DeviceAttachmentSpec> spec;
	std::shared_ptr<Device> device;

public:
	void newDeviceAttachmentSpecInd1_posted(
		[[maybe_unused]] std::shared_ptr<NewDeviceAttachmentSpecInd> context
		)
	{
		DeviceManager::getInstance().attachSenseDeviceReq(
			spec,
			{context, std::bind(
				&NewDeviceAttachmentSpecInd::newDeviceAttachmentSpecInd2,
				context.get(), context,
				std::placeholders::_1, std::placeholders::_2)});
	}

	void newDeviceAttachmentSpecInd2(
		[[maybe_unused]] std::shared_ptr<NewDeviceAttachmentSpecInd> context,
		bool success,
		std::shared_ptr<DeviceAttachmentSpec> deviceSpec
		)
	{
		if (!success)
		{
			std::cerr << __func__ << ": Attach failed for device spec "
				<< deviceSpec->stringify() << std::endl;
			callOriginalCb(false, nullptr, deviceSpec);
			return;
		}

		try {
			// Create DeviceRole and add it to both DeviceManager's and Device's collections
			auto deviceRole = std::make_shared<DeviceRole>(*device, spec);
			device->deviceRoles.push_back(deviceRole);
			attachedDeviceRoles.push_back(deviceRole);

			// Callback with success
			callOriginalCb(true, deviceRole, spec);
		} catch (const std::exception& e) {
			// Attach failed, callback with error
			callOriginalCb(false, nullptr, spec);
		}
	}
};

class DeviceManager::RemoveDeviceAttachmentSpecReq
:	public PostedAsynchronousContinuation<removeDeviceAttachmentSpecReqCbFn>
{
public:
	RemoveDeviceAttachmentSpecReq(
		const std::shared_ptr<DeviceAttachmentSpec> &s,
		const std::shared_ptr<ComponentThread> &caller,
		Callback<removeDeviceAttachmentSpecReqCbFn> cb)
	:	PostedAsynchronousContinuation<removeDeviceAttachmentSpecReqCbFn>(
			caller, cb),
	spec(s)
	{}

public:
	void removeDeviceAttachmentSpecReq1_posted(
		[[maybe_unused]] std::shared_ptr<RemoveDeviceAttachmentSpecReq> context
		)
	{
		// Call detachSenseDeviceReq first - only clean up metadata if this succeeds
		DeviceManager::getInstance().detachSenseDeviceReq(
			spec,
			{context, std::bind(
				&RemoveDeviceAttachmentSpecReq::removeDeviceAttachmentSpecReq2,
				context.get(), context,
				std::placeholders::_1, std::placeholders::_2)});
	}

	void removeDeviceAttachmentSpecReq2(
		[[maybe_unused]] std::shared_ptr<RemoveDeviceAttachmentSpecReq> context,
		bool success,
		std::shared_ptr<DeviceAttachmentSpec> deviceSpec
		)
	{
		if (!success)
		{
			// Detach failed, callback with failure (metadata remains intact)
			callOriginalCb(false, deviceSpec);
			return;
		}

		// Detach succeeded, now find and clean up metadata
		try {
			// Find the DeviceRole in attachedDeviceRoles
			auto deviceRoleIt = std::find_if(
				attachedDeviceRoles.begin(),
				attachedDeviceRoles.end(),
				[&spec = spec](const std::shared_ptr<DeviceRole> &role) {
					return *role->deviceAttachmentSpec == *spec;
				}
			);

			if (deviceRoleIt == attachedDeviceRoles.end())
			{
				// DeviceRole not found, callback with failure
				callOriginalCb(false, deviceSpec);
				return;
			}

			auto deviceRole = *deviceRoleIt;
			auto& device = deviceRole->parentDevice;

			// Remove DeviceRole from DeviceManager's collection
			attachedDeviceRoles.erase(deviceRoleIt);

			// Remove DeviceRole from Device's collection
			auto deviceRoleIt2 = std::find(
				device.deviceRoles.begin(),
				device.deviceRoles.end(),
				deviceRole);
			if (deviceRoleIt2 != device.deviceRoles.end())
			{
				device.deviceRoles.erase(deviceRoleIt2);
			}

			// Remove DeviceAttachmentSpec from deviceAttachmentSpecs collection
			auto specIt = std::find_if(
				deviceAttachmentSpecs.begin(),
				deviceAttachmentSpecs.end(),
				[&spec = spec](const std::shared_ptr<DeviceAttachmentSpec> &existingSpec) {
					return *existingSpec == *spec;
				});
			if (specIt != deviceAttachmentSpecs.end())
			{
				deviceAttachmentSpecs.erase(specIt);
			}

			// Callback with success
			callOriginalCb(true, deviceSpec);
		} catch (const std::exception& e) {
			// Cleanup failed, callback with error
			callOriginalCb(false, deviceSpec);
		}
	}

public:
	std::shared_ptr<DeviceAttachmentSpec> spec;
};

void DeviceManager::newDeviceAttachmentSpecInd(
	const DeviceAttachmentSpec &spec,
	Callback<newDeviceAttachmentSpecIndCbFn> callback)
{
	// First, add the spec to deviceAttachmentSpecs if it's not already there
	bool specExists = false;
	std::shared_ptr<DeviceAttachmentSpec> specPtr = nullptr;
	for (const auto& existingSpec : deviceAttachmentSpecs)
	{
		if (*existingSpec == spec)
		{
			specExists = true;
			specPtr = existingSpec;
			break;
		}
	}

	if (!specExists)
	{
		specPtr = std::make_shared<DeviceAttachmentSpec>(spec);
		deviceAttachmentSpecs.push_back(specPtr);
	}

	bool deviceExists = false;
	std::shared_ptr<Device> device = nullptr;
	for (const auto& existingDevice : devices)
	{
		if (existingDevice->deviceIdentifier != spec.deviceIdentifier)
			{ continue; }

		device = existingDevice;
		deviceExists = true;
		break;
	}

	// If device doesn't exist, create a new one and add it
	if (!device)
	{
		device = std::make_shared<Device>(spec.deviceIdentifier);
		devices.push_back(device);
	}

	// Check if a DeviceRole w/ this spec already exists in attachedDeviceRoles
	bool deviceRoleExists = false;
	std::shared_ptr<DeviceRole> existingDeviceRole = nullptr;
	for (const auto& role : attachedDeviceRoles)
	{
		if (*role->deviceAttachmentSpec == spec)
		{
			deviceRoleExists = true;
			existingDeviceRole = role;
			break;
		}
	}

	// If DeviceRole exists, both spec and device must also exist
	if (deviceRoleExists)
	{
		if (!specExists || !deviceExists)
		{
			throw std::runtime_error(
				"Program error: DeviceRole exists but spec or device doesn't "
				"pre-exist. specExists=" + std::to_string(specExists) +
				", deviceExists=" + std::to_string(deviceExists));
		}

		// Already attached, callback with success
		callback.callbackFn(true, existingDeviceRole, specPtr);
		return;
	}

	// Create async continuation
	const auto& caller = ComponentThread::getSelf();
	auto continuation = std::make_shared<NewDeviceAttachmentSpecInd>(
		specPtr, device, caller, callback);

	mrntt::mrntt.thread->getIoService().post(
		std::bind(
			&NewDeviceAttachmentSpecInd::newDeviceAttachmentSpecInd1_posted,
			continuation.get(), continuation));
}

void DeviceManager::removeDeviceAttachmentSpecReq(
	const DeviceAttachmentSpec &spec,
	Callback<removeDeviceAttachmentSpecReqCbFn> callback)
{
	// Find the shared_ptr to the spec in the collection
	std::shared_ptr<DeviceAttachmentSpec> specPtr = nullptr;
	for (const auto& existingSpec : deviceAttachmentSpecs)
	{
		if (*existingSpec == spec)
		{
			specPtr = existingSpec;
			break;
		}
	}

	if (!specPtr)
	{
		// Spec not found, callback with failure
		callback.callbackFn(false, nullptr);
		return;
	}

	// Create async continuation
	const auto& caller = ComponentThread::getSelf();
	auto continuation = std::make_shared<RemoveDeviceAttachmentSpecReq>(
		specPtr, caller, callback);

	mrntt::mrntt.thread->getIoService().post(
		std::bind(
			&RemoveDeviceAttachmentSpecReq::removeDeviceAttachmentSpecReq1_posted,
			continuation.get(), continuation));
}

class DeviceManager::AttachSenseDeviceReq
:	public SerializedAsynchronousContinuation<attachSenseDeviceReqCbFn>
{
public:
	AttachSenseDeviceReq(
		const std::shared_ptr<DeviceAttachmentSpec>& spec,
		const std::shared_ptr<ComponentThread> &caller,
		Callback<attachSenseDeviceReqCbFn> cb,
		std::vector<std::reference_wrapper<Qutex>> requiredLocks)
	:	SerializedAsynchronousContinuation<attachSenseDeviceReqCbFn>(
			caller, cb, requiredLocks),
		spec(spec)
	{}

public:
	void attachSenseDeviceReq1_posted(
		[[maybe_unused]] std::shared_ptr<AttachSenseDeviceReq> context
		)
	{
		if (caller->id != ComponentThread::MRNTT)
		{
			std::cerr << std::string(__func__)
				<< ": executed on non-mrntt thread: "
				<< caller->name << std::endl;
			callOriginalCb(false, spec);
			return;
		}

		/**		FIXME:
		 * We should acquire a spinlock here to ensure that the device isn't
		 * added in the interim while the async op executes.
		 */
		auto libOpt = sense_api::SenseApiManager::getInstance()
			.getSenseApiLibByApiName(spec->api);

		if (!libOpt)
		{
			std::cerr << std::string(__func__) + ": No library found for API '"
				<< spec->api << "'" << std::endl;
			callOriginalCb(false, spec);
			return;
		}

		auto& lib = *libOpt.value();
		if (!lib.senseApiDesc.sal_mgmt_libOps.attachDeviceReq)
		{
			std::cerr << std::string(__func__) + ": attachDeviceReq() is NULL "
				"for library '" << lib.libraryPath << "'" << std::endl;
			callOriginalCb(false, spec);
			return;
		}

		/**		EXPLANATION:
		 * We pass in either the body or world thread here, depending on whether
		 * the device is an introspector (idev) or extrospector (edev).
		 *
		 * Introspectors are attached to the body thread; extrospectors are
		 * attached to the world thread.
		 */
		std::shared_ptr<ComponentThread> threadForAttachment;
		if (spec->sensorType == 'e')
		{
			threadForAttachment = mind::globalMind->world.thread;
			std::cout << __func__ << ": Attaching edev "
				<< spec->deviceIdentifier << " to world thread" << "\n";
		}
		else
		{
			threadForAttachment = mind::globalMind->body.thread;
			std::cout << __func__ << ": Attaching non-edev "
				<< spec->deviceIdentifier << " to body thread" << "\n";
		}

		lib.senseApiDesc.sal_mgmt_libOps.attachDeviceReq(
			spec, threadForAttachment,
			{context, std::bind(
				&AttachSenseDeviceReq::attachSenseDeviceReq2,
				context.get(), context,
				std::placeholders::_1, std::placeholders::_2)});
	 }

	 void attachSenseDeviceReq2(
		[[maybe_unused]] std::shared_ptr<AttachSenseDeviceReq> context,
		bool success,
		std::shared_ptr<DeviceAttachmentSpec> deviceSpec
		)
	{
		callOriginalCb(success, deviceSpec);
	}

	 void detachSenseDeviceReq1_posted(
		[[maybe_unused]] std::shared_ptr<DetachSenseDeviceReq> context
		)
	{
		if (caller->id != ComponentThread::MRNTT)
		{
			std::cerr << std::string(__func__)
				<< ": executed on non-mrntt thread: "
				<< caller->name << std::endl;
			callOriginalCb(false, spec);
			return;
		}

		/**		FIXME:
		 * We should acquire a spinlock here to ensure that the device isn't
		 * removed in the interim while the async op executes.
		 */
		auto libOpt = sense_api::SenseApiManager::getInstance()
			.getSenseApiLibByApiName(spec->api);

		if (!libOpt)
		{
			std::cerr << std::string(__func__) + ": No library found for API '"
				<< spec->api << "'" << std::endl;
			callOriginalCb(false, spec);
			return;
		}
		auto& lib = *libOpt.value();
		if (!lib.senseApiDesc.sal_mgmt_libOps.detachDeviceReq)
		{
			std::cerr << std::string(__func__) + ": detachDeviceReq() is NULL "
				"for library '" << lib.libraryPath << "'" << std::endl;
			callOriginalCb(false, spec);
			return;
		}
		lib.senseApiDesc.sal_mgmt_libOps.detachDeviceReq(
			spec,
			{context, std::bind(
				&DetachSenseDeviceReq::detachSenseDeviceReq2,
				context.get(), context,
				std::placeholders::_1, std::placeholders::_2)});
	 }

	void detachSenseDeviceReq2(
		[[maybe_unused]] std::shared_ptr<DetachSenseDeviceReq> context,
		bool success,
		std::shared_ptr<DeviceAttachmentSpec> deviceSpec
		)
	{
		callOriginalCb(success, deviceSpec);
	}

public:
	std::shared_ptr<DeviceAttachmentSpec> spec;
};

void DeviceManager::attachSenseDeviceReq(
    const std::shared_ptr<DeviceAttachmentSpec>& spec,
    Callback<attachSenseDeviceReqCbFn> cb
    )
{
	const auto& caller = ComponentThread::getSelf();

	// Get the sense API lib's qutex
	auto libOpt = sense_api::SenseApiManager::getInstance()
		.getSenseApiLibByApiName(spec->api);

		if (!libOpt)
	{
		std::cerr << "attachSenseDeviceReq: No library found for API '"
			<< spec->api << "'" << std::endl;
		cb.callbackFn(false, spec);
		return;
	}

	auto& lib = *libOpt.value();

	auto request = std::make_shared<AttachSenseDeviceReq>(
		spec, caller, cb,
		LockSet<attachSenseDeviceReqCbFn>::Set{
			std::ref(lib.qutex)
		});

	AttachSenseDeviceReq::LockerAndInvoker lockvoker(
		*request, mrntt::mrntt.thread,
		std::bind(
			&AttachSenseDeviceReq::attachSenseDeviceReq1_posted,
			request.get(), request));
}

void DeviceManager::detachSenseDeviceReq(
    const std::shared_ptr<DeviceAttachmentSpec>& spec,
    Callback<detachSenseDeviceReqCbFn> cb
    )
{
	const auto& caller = ComponentThread::getSelf();

	// Get the sense API lib's qutex
	auto libOpt = sense_api::SenseApiManager::getInstance()
		.getSenseApiLibByApiName(spec->api);

		if (!libOpt)
	{
		std::cerr << "detachSenseDeviceReq: No library found for API '"
			<< spec->api << "'" << std::endl;
		cb.callbackFn(false, spec);
		return;
	}

	auto& lib = *libOpt.value();

	auto request = std::make_shared<DetachSenseDeviceReq>(
		spec, caller, cb,
		LockSet<detachSenseDeviceReqCbFn>::Set{
			std::ref(lib.qutex)
		});

	DetachSenseDeviceReq::LockerAndInvoker lockvoker(
		*request, mrntt::mrntt.thread,
		std::bind(
			&DetachSenseDeviceReq::detachSenseDeviceReq1_posted,
			request.get(), request));
}

class DeviceManager::AttachAllUnattachedDevicesFromReq
:	public PostedAsynchronousContinuation<
		attachAllUnattachedDevicesFromReqCbFn>
{
public:
	AttachAllUnattachedDevicesFromReq(
		const unsigned int totalNSpecs,
		const std::shared_ptr<std::vector<DeviceAttachmentSpec>>& specs,
		const std::shared_ptr<ComponentThread>& caller,
		Callback<attachAllUnattachedDevicesFromReqCbFn> cb)
	:	PostedAsynchronousContinuation<attachAllUnattachedDevicesFromReqCbFn>(
			caller, cb),
	loop(totalNSpecs), specs(specs)
	{}

public:
	void attachAllUnattachedDevicesFromReq1_posted(
		[[maybe_unused]] std::shared_ptr<AttachAllUnattachedDevicesFromReq>
			context
		)
	{
		for (const auto& spec : *specs)
		{
			DeviceManager::getInstance().newDeviceAttachmentSpecInd(
				spec,
				{context, std::bind(
					&AttachAllUnattachedDevicesFromReq::attachAllUnattachedDevicesFromReq2,
					context.get(), context,
					std::placeholders::_1, std::placeholders::_2,
					std::placeholders::_3)});
		}
	}

	// Callback methods for the attachment sequence
	void attachAllUnattachedDevicesFromReq2(
		std::shared_ptr<AttachAllUnattachedDevicesFromReq> context,
		bool success, [[maybe_unused]] std::shared_ptr<DeviceRole> deviceRole,
		std::shared_ptr<DeviceAttachmentSpec> spec
		)
	{
		if (!success)
		{
			std::cerr << __func__ << ": Failed to attach device: "
				<< spec->deviceIdentifier << "\n";
			// Fallthrough.
		}

		if (!context->loop.incrementSuccessOrFailureAndTestForCompletionDueTo(
			success))
		{
			return;
		}

		if (OptionParser::getOptions().verbose)
		{
			std::cout << __func__ << ": " << context->loop.nSucceeded.load()
					<< " devices attached, "
					<< context->loop.nFailed.load() << " devices failed\n";
		}

		context->callOriginalCb(loop);
	}

public:
	AsynchronousLoop loop;
	std::shared_ptr<std::vector<DeviceAttachmentSpec>> specs;
};

void DeviceManager::attachAllUnattachedDevicesFromReq(
	const std::shared_ptr<std::vector<DeviceAttachmentSpec>> &specs,
	Callback<attachAllUnattachedDevicesFromReqCbFn> cb
	)
{
	if (specs->size() == 0)
	{
		AsynchronousLoop tmp(0);
		cb.callbackFn(tmp);
		return;
	}

	const auto& caller = ComponentThread::getSelf();
	auto request = std::make_shared<AttachAllUnattachedDevicesFromReq>(
		specs->size(), specs, caller, std::move(cb));

	mrntt::mrntt.thread->getIoService().post(
		std::bind(
			&AttachAllUnattachedDevicesFromReq::attachAllUnattachedDevicesFromReq1_posted,
			request.get(), request));
}

void DeviceManager::attachAllUnattachedDevicesFromCmdlineReq(
	Callback<attachAllUnattachedDevicesFromReqCbFn> cb
	)
{
	auto specs = std::make_shared<std::vector<DeviceAttachmentSpec>>(
		commandLineDASpecs);
	attachAllUnattachedDevicesFromReq(specs, std::move(cb));
}

void DeviceManager::attachAllUnattachedDevicesFromKnownListReq(
	Callback<attachAllUnattachedDevicesFromReqCbFn> cb
	)
{
	// Create a vector to hold unattached device specs
	auto unattachedSpecs = std::make_shared<
		std::vector<DeviceAttachmentSpec>>();

	// Cycle through all DA specs in deviceAttachmentSpecs
	for (const auto& spec : deviceAttachmentSpecs)
	{
		bool isAttached = false;

		// Cross reference with attachedDeviceRoles
		for (const auto& role : attachedDeviceRoles)
		{
			if (*role->deviceAttachmentSpec == *spec)
			{
				isAttached = true;
				break;
			}
		}

		// If spec doesn't appear in attachedDeviceRoles, add it to the vector
		if (!isAttached) {
			unattachedSpecs->push_back(*spec);
		}
	}

	// Pass the vector to the existing function
	attachAllUnattachedDevicesFromReq(unattachedSpecs, std::move(cb));
}

class DeviceManager::DetachAllAttachedDeviceRoles
:	public PostedAsynchronousContinuation<
		detachAllAttachedDeviceRolesCbFn>
{
public:
	DetachAllAttachedDeviceRoles(
		const unsigned int totalNSpecs,
		const std::shared_ptr<ComponentThread>& caller,
		Callback<detachAllAttachedDeviceRolesCbFn> cb)
	:	PostedAsynchronousContinuation<detachAllAttachedDeviceRolesCbFn>(
			caller, cb),
	loop(totalNSpecs)
	{}

	void detachAllAttachedDeviceRoles1_posted(
		[[maybe_unused]] std::shared_ptr<DetachAllAttachedDeviceRoles> context
		)
	{
		for (const auto& deviceRole : DeviceManager::attachedDeviceRoles)
		{
			DeviceManager::getInstance().detachSenseDeviceReq(
				deviceRole->deviceAttachmentSpec,
				{context, std::bind(
					&DetachAllAttachedDeviceRoles::detachAllAttachedDeviceRoles2,
					context.get(), context,
					std::placeholders::_1, std::placeholders::_2)});
		}
	}

	void detachAllAttachedDeviceRoles2(
		std::shared_ptr<DetachAllAttachedDeviceRoles> context,
		bool success, std::shared_ptr<DeviceAttachmentSpec> spec
		)
	{
		if (!success)
		{
			std::cerr << __func__ << ": Failed to detach device: "
				<< spec->deviceIdentifier << "\n";
			// Fallthrough.
		}

		if (!context->loop.incrementSuccessOrFailureAndTestForCompletionDueTo(
			success))
		{
			return;
		}

		if (OptionParser::getOptions().verbose)
		{
			std::cout << __func__ << ": " << context->loop.nSucceeded.load()
				<< " devices detached, "
				<< context->loop.nFailed.load() << " devices failed\n";
		}

		context->callOriginalCb(loop);
	}

public:
	AsynchronousLoop loop;
};

void DeviceManager::detachAllAttachedDeviceRoles(
	Callback<detachAllAttachedDeviceRolesCbFn> cb
	)
{
	if (DeviceManager::getInstance().attachedDeviceRoles.size() == 0)
	{
		AsynchronousLoop tmp(0);
		cb.callbackFn(tmp);
		return;
	}

	const auto& caller = ComponentThread::getSelf();
	auto request = std::make_shared<DetachAllAttachedDeviceRoles>(
		DeviceManager::getInstance().attachedDeviceRoles.size(),
		caller, std::move(cb));

	mrntt::mrntt.thread->getIoService().post(
		std::bind(
			&DetachAllAttachedDeviceRoles::detachAllAttachedDeviceRoles1_posted,
			request.get(), request));
}

void DeviceManager::initializeDeviceReattacher()
{
	deviceReattacher = std::make_unique<DeviceReattacher>(
		*this, mrntt::mrntt.thread);

	deviceReattacher->start();
}

void DeviceManager::finalizeDeviceReattacher()
{
	if (deviceReattacher) {
		deviceReattacher->stop();
	}
}

} // namespace device
} // namespace smo