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 <callback.h>
#include <componentThread.h>
#include <deviceManager/deviceManager.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;

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);
		}
	}
};

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

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

	// Find or create the Device for this spec
	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;
		}
	}

	// Consistency check: 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, spec);
		return;
	}

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

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

class DeviceManager::AttachSenseDeviceReq
:	public PostedAsynchronousContinuation<attachSenseDeviceReqCbFn>
{
public:
	AttachSenseDeviceReq(
		const std::shared_ptr<DeviceAttachmentSpec>& spec,
		const std::shared_ptr<ComponentThread> &caller,
		Callback<attachSenseDeviceReqCbFn> cb)
	:	PostedAsynchronousContinuation<attachSenseDeviceReqCbFn>(
			caller, cb),
		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();
	auto request = std::make_shared<AttachSenseDeviceReq>(
		spec, caller, cb);

	mrntt::mrntt.thread->getIoService().post(
		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();
	auto request = std::make_shared<DetachSenseDeviceReq>(
		spec, caller, cb);

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

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

public:
	void attachAllSenseDevicesFromSpecsReq1_posted(
		[[maybe_unused]] std::shared_ptr<AttachAllSenseDevicesFromSpecsReq>
			context
		)
	{
		for (const auto& spec : DeviceManager::deviceAttachmentSpecs)
		{
			DeviceManager::getInstance().newDeviceAttachmentSpecInd(
				spec,
				{context, std::bind(
					&AttachAllSenseDevicesFromSpecsReq::attachAllSenseDevicesFromSpecsReq2,
					context.get(), context,
					std::placeholders::_1, std::placeholders::_2,
					std::placeholders::_3)});
		}
	}

	// Callback methods for the attachment sequence
	void attachAllSenseDevicesFromSpecsReq2(
		std::shared_ptr<AttachAllSenseDevicesFromSpecsReq> context,
		bool success, 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;
};

void DeviceManager::attachAllSenseDevicesFromSpecsReq(
	Callback<attachAllSenseDevicesFromSpecsReqCbFn> cb
	)
{
	if (DeviceManager::getInstance().deviceAttachmentSpecs.size() == 0)
	{
		AsynchronousLoop tmp(0);
		cb.callbackFn(tmp);
		return;
	}

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

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

class DeviceManager::DetachAllSenseDevicesReq
:	public AttachAllSenseDevicesFromSpecsReq
{
public:
	using AttachAllSenseDevicesFromSpecsReq::AttachAllSenseDevicesFromSpecsReq;

	void detachAllSenseDevicesReq1_posted(
		[[maybe_unused]] std::shared_ptr<DetachAllSenseDevicesReq> context
		)
	{
		for (const auto& spec : DeviceManager::deviceAttachmentSpecs)
		{
			DeviceManager::getInstance().detachSenseDeviceReq(
				spec,
				{context, std::bind(
					&DetachAllSenseDevicesReq::detachAllSenseDevicesReq2,
					context.get(), context,
					std::placeholders::_1, std::placeholders::_2)});
		}
	}

	void detachAllSenseDevicesReq2(
		std::shared_ptr<DetachAllSenseDevicesReq> 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);
	}
};

void DeviceManager::detachAllSenseDevicesReq(
	Callback<detachAllSenseDevicesReqCbFn> cb
	)
{
	if (DeviceManager::getInstance().deviceAttachmentSpecs.size() == 0)
	{
		AsynchronousLoop tmp(0);
		cb.callbackFn(tmp);
		return;
	}

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

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

} // namespace device
} // namespace smo