#include <sstream>
#include <thread>
#include <chrono>
#include <string>
#include <stdexcept>
#include <memory>
#include <unistd.h>
#include <ifaddrs.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <fcntl.h>
#include <errno.h>
#include <cstring>
#include <netinet/in.h>
#include <optional>
#include <boost/asio/deadline_timer.hpp>
#include <boost/asio/posix/stream_descriptor.hpp>
#include <opts.h>
#include "device.h"
#include "protocol.h"
#include "core.h"


/**		EXPLANATION:
 * This file contains the implementation of the Device class.
 *
 *		FIXME:
 * We may need to check how smo-subnet-nbits is used in here because we didn't
 * actually check to see under what conditions it's required vs optional. Hence
 * we don't currently enforce correct usage of it, and we just assume that the
 * livoxGen1's policy of supplying a default value of 24 is correct.
 */

namespace livoxProto1 {

// Static member definition for devices under construction
std::unordered_map<std::string, std::vector<Device::CommandHandler>>
	Device::devicesUnderConstruction;

namespace comms {

DiscoveredDevice::DiscoveredDevice(
	const std::string &deviceIdentifier,
	DeviceType deviceType,
	const std::string &ipAddr)
: deviceIdentifier(deviceIdentifier),
  deviceType(deviceType),
  ipAddr(ipAddr)
{
}

DiscoveredDevice::DiscoveredDevice(
	const BroadcastMessage &msg, const std::string &ipAddr
)
: DiscoveredDevice(
	reinterpret_cast<const char*>(msg.broadcast_code),
	static_cast<DeviceType>(msg.dev_type),
	ipAddr)
{
}

std::string DiscoveredDevice::stringify(void) const
{
	std::ostringstream oss;
	oss << "DiscoveredDevice{"
		<< "identifier='" << deviceIdentifier << "', "
		<< "ipAddr='" << ipAddr << "', "
		<< "deviceType=" << (int)deviceType << " (" << getDeviceTypeName() << ")"
		<< "}";
	return oss.str();
}

std::string DiscoveredDevice::getDeviceTypeName(void) const
{
	switch (deviceType)
	{
		case DeviceType::Hub: return "Hub";
		case DeviceType::Mid40: return "Mid-40";
		case DeviceType::Tele15: return "Tele-15";
		case DeviceType::Horizon: return "Horizon";
		case DeviceType::Mid70: return "Mid-70";
		case DeviceType::Avia: return "Avia";
		default: return "Unknown";
	}
}

} // namespace comms

Device::Device(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)
: discoveredDevice(
	deviceIdentifier, comms::DeviceType::Mid40,
	// Initialize empty. IP will be set upon successful connection.
	""),
nAttachedStimulusProducers(0),
componentThread(componentThread),
commandTimeoutMs(commandTimeoutMs), retryDelayMs(retryDelayMs),
smoIp(smoIp), detectedSmoListeningIp(""), smoSubnetNbits(smoSubnetNbits),
dataPort(dataPort), cmdPort(cmdPort), imuPort(imuPort),
heartbeatActive(false),
pcloudDataActive(false)
{
}

Device::~Device()
{
	stopHeartbeat();

	if (pcloudDataActive.load()) {
		pcloudDataActive.store(false);
	}

	heartbeatTimer.reset();
}

namespace {

constexpr uint8_t CMD_SET_GENERAL = 0x00;
constexpr uint8_t CMD_ID_HANDSHAKE_ACK = 0x01;
constexpr uint8_t CMD_ID_SAMPLING_RESPONSE = 0x04;
constexpr uint8_t CMD_SET_LIDAR = 0x01;
constexpr uint8_t CMD_ID_SET_RETURN_MODE_RESPONSE = 0x06;
constexpr uint8_t CMD_ID_GET_RETURN_MODE_RESPONSE = 0x07;
constexpr uint16_t LIVOX_COMMAND_PORT = 65000;

using UdpCommandResponseResult = comms::UdpCommandResponseResult;

comms::UdpCommandDemuxer *getUdpCommandDemuxer()
{
	auto &protoState = livoxProto1::getProtoState();
	if (!protoState.deviceManager) {
		return nullptr;
	}

	return &protoState.deviceManager->udpCommandDemuxer;
}

std::shared_ptr<boost::asio::posix::stream_descriptor> getCmdEndpointFdDesc()
{
	auto &protoState = livoxProto1::getProtoState();
	if (!protoState.deviceManager) {
		return nullptr;
	}

	return protoState.deviceManager->udpCommandDemuxer.getCmdEndpointFdDesc();
}

bool sendHandshakeRequest(
	Device &device,
	const std::string &deviceIP,
	int cmdSocketFd)
{
	/**		EXPLANATION:
	 * Prepare handshake request.
	 */
	comms::HandshakeRequest handshakeReq(
		device.detectedSmoListeningIp,
		device.dataPort, device.cmdPort, device.imuPort);
	handshakeReq.swapContentsToProtocolEndianness();
	handshakeReq.header.setCrc16FromRawBytes();
	handshakeReq.header.swapCrc16ToProtocolEndianness();
	handshakeReq.footer.crc_32 = handshakeReq.calculateCrc32();
	handshakeReq.footer.swapCrc32ToProtocolEndianness();

	struct sockaddr_in deviceAddr;
	memset(&deviceAddr, 0, sizeof(deviceAddr));
	deviceAddr.sin_family = AF_INET;
	deviceAddr.sin_addr.s_addr = inet_addr(deviceIP.c_str());
	deviceAddr.sin_port = htons(LIVOX_COMMAND_PORT);

	ssize_t bytesSent = sendto(
		cmdSocketFd,
		&handshakeReq, sizeof(comms::HandshakeRequest), 0,
		(struct sockaddr*)&deviceAddr, sizeof(deviceAddr));

	if (bytesSent < 0)
	{
		std::cerr << __func__ << ": Failed to send handshake request: "
			<< strerror(errno) << std::endl;
		return false;
	}

	return true;
}

bool processHandshakeResponse(
	const UdpCommandResponseResult &responseResult,
	Device &device,
	const std::string &deviceIP)
{
	if (responseResult.outcome == UdpCommandResponseResult::Outcome::Timeout)
	{
		std::cerr << __func__ << ": Command timeout with "
			<< deviceIP << "(" << device.discoveredDevice.deviceIdentifier
			<< ")" << std::endl;
		return false;
	}

	if (responseResult.outcome == UdpCommandResponseResult::Outcome::RecvError)
	{
		std::cerr << __func__ << ": Receive error during handshake with "
			<< deviceIP << std::endl;
		return false;
	}

	const ssize_t bytesReceived = responseResult.bytesReceived;
	if (bytesReceived < static_cast<ssize_t>(sizeof(comms::HandshakeResponse)))
	{
		std::cerr << __func__ << ": Response of size " << bytesReceived
			<< " too small from " << deviceIP << std::endl;
		return false;
	}

	comms::HandshakeResponse *resp = reinterpret_cast<comms::HandshakeResponse*>(
		const_cast<uint8_t*>(responseResult.buffer));

	/**		EXPLANATION:
	 * Following the clean receiving flow:
	 * 1. Swap CRC32 to host endianness first
	 * 2. Validate CRC32
	 * 3. Swap CRC16 to host endianness
	 * 4. Validate CRC16
	 * 5. Swap content to host endianness
	 */
	resp->footer.swapCrc32ToHostEndianness();
	if (!resp->validateCrc32())
	{
		std::cerr << __func__ << ": CRC32 validation failed from "
			<< deviceIP << std::endl;
		return false;
	}
	resp->header.swapCrc16ToHostEndianness();
	if (!resp->header.validateCrc16())
	{
		std::cerr << __func__ << ": CRC16 validation failed from "
			<< deviceIP << std::endl;
		return false;
	}
	resp->swapContentsToHostEndianness();
	if (!resp->sanityCheck() || resp->ret_code != 0x00)
	{
		std::cerr << __func__ << ": Invalid response from "
			<< deviceIP << std::endl;
		return false;
	}

	if (getProtoState().smoCallbacks.OptionParser_getOptions().verbose)
	{
		std::cout << __func__ << ": Handshake successful with "
				<< deviceIP << "("
				<< device.discoveredDevice.deviceIdentifier
				<< ")" << "\n";
	}

	return true;
}

bool sendEnDisablePcloudCommand(
	Device &device, uint8_t enableFlag, const char *commandName)
{
	livoxProto1::comms::StartStopSamplingMessage message;
	message.enable = enableFlag;

	message.swapContentsToProtocolEndianness();
	message.header.setCrc16FromRawBytes();
	message.header.swapCrc16ToProtocolEndianness();
	message.footer.crc_32 = message.calculateCrc32();
	message.footer.swapCrc32ToProtocolEndianness();

	struct sockaddr_in deviceAddr;
	memset(&deviceAddr, 0, sizeof(deviceAddr));
	deviceAddr.sin_family = AF_INET;
	deviceAddr.sin_addr.s_addr =
		inet_addr(device.discoveredDevice.ipAddr.c_str());
	deviceAddr.sin_port = htons(LIVOX_COMMAND_PORT);

	auto &protoState = livoxProto1::getProtoState();
	if (!protoState.deviceManager)
	{
		std::cerr << __func__ << ": No device manager available" << std::endl;
		return false;
	}

	auto cmdEndpointFdDesc = getCmdEndpointFdDesc();
	if (!cmdEndpointFdDesc)
	{
		std::cerr << __func__ << ": No command endpoint available" << std::endl;
		return false;
	}

	ssize_t bytesSent = sendto(
		cmdEndpointFdDesc->native_handle(),
		&message, sizeof(message), 0,
		(struct sockaddr*)&deviceAddr, sizeof(deviceAddr));

	if (bytesSent < 0)
	{
		std::cerr << __func__ << ": Failed to send " << commandName
			<< " command: " << strerror(errno) << std::endl;
		return false;
	}

	return true;
}

bool processSamplingResponse(
	const UdpCommandResponseResult &responseResult,
	Device &device,
	const char *failureContext)
{
	(void)failureContext;

	if (responseResult.outcome == UdpCommandResponseResult::Outcome::Timeout)
	{
		std::cerr << __func__ << ": Command timeout for device "
		    << device.discoveredDevice.deviceIdentifier
		    << std::endl;
		return false;
	}

	if (responseResult.outcome == UdpCommandResponseResult::Outcome::RecvError)
	{
		std::cerr
		    << __func__ << ": Receive error during command for device "
		    << device.discoveredDevice.deviceIdentifier
		    << std::endl;
		return false;
	}

	const ssize_t bytesReceived = responseResult.bytesReceived;
	if (bytesReceived < static_cast<ssize_t>(sizeof(comms::SamplingResponse)))
	{
		std::cerr << __func__ << ": Response of size "
		    << bytesReceived
		    << " is too small for sampling response (expected "
		    << sizeof(comms::SamplingResponse) << ")"
		    << std::endl;
		return false;
	}

	comms::SamplingResponse *response =
		reinterpret_cast<comms::SamplingResponse*>(
			const_cast<uint8_t*>(responseResult.buffer));

	response->swapContentsToHostEndianness();
	if (!response->sanityCheck())
	{
		std::cerr << __func__ << ": Invalid sampling response structure.\n";
		return false;
	}

	if (!(response->command.cmd_set == CMD_SET_GENERAL &&
	      response->command.cmd_id == CMD_ID_SAMPLING_RESPONSE &&
	      response->ret_code == 0x00))
	{
		if (getProtoState().smoCallbacks.OptionParser_getOptions().verbose)
		{
			std::cout << __func__ << ": Failed to en/disable pcloud data "
				"for device "
				"(" << device.discoveredDevice.deviceIdentifier
				<< ") @(" << device.discoveredDevice.ipAddr << "). "
				<< "cmd_set: " << (int)response->command.cmd_set
				<< ", cmd_id: " << (int)response->command.cmd_id
				<< ", ret_code: " << (int)response->ret_code << "\n";
		}
		return false;
	}

	return true;
}

bool sendSetReturnModeCommand(Device &device, uint8_t returnMode)
{
	auto cmdEndpointFdDesc = getCmdEndpointFdDesc();
	if (!cmdEndpointFdDesc)
	{
		std::cerr << __func__ << ": No command endpoint available.\n";
		return false;
	}

	comms::SetLiDARReturnMode setReturnModeMsg;
	setReturnModeMsg.mode = returnMode;
	setReturnModeMsg.swapContentsToProtocolEndianness();
	setReturnModeMsg.header.setCrc16FromRawBytes();
	setReturnModeMsg.header.swapCrc16ToProtocolEndianness();
	setReturnModeMsg.footer.crc_32 = setReturnModeMsg.calculateCrc32();
	setReturnModeMsg.footer.swapCrc32ToProtocolEndianness();

	struct sockaddr_in deviceAddr;
	deviceAddr.sin_family = AF_INET;
	deviceAddr.sin_addr.s_addr = inet_addr(
		device.discoveredDevice.ipAddr.c_str());
	deviceAddr.sin_port = htons(LIVOX_COMMAND_PORT);

	ssize_t bytesSent = sendto(
		cmdEndpointFdDesc->native_handle(),
		&setReturnModeMsg, sizeof(setReturnModeMsg), 0,
		(struct sockaddr*)&deviceAddr, sizeof(deviceAddr));

	if (bytesSent < 0)
	{
		std::cerr << __func__ << ": Failed to send set return mode message: "
			<< strerror(errno) << std::endl;
		return false;
	}

	std::cout << __func__ << ": Sent set return mode message to "
		<< device.discoveredDevice.ipAddr << ":" << LIVOX_COMMAND_PORT
		<< std::endl;
	return true;
}

bool processSetReturnModeResponse(
	const UdpCommandResponseResult &responseResult,
	Device &device,
	uint8_t returnMode)
{
	if (responseResult.outcome == UdpCommandResponseResult::Outcome::Timeout)
	{
		std::cerr << __func__ << ": Set return mode timeout with "
			<< device.discoveredDevice.ipAddr << "("
			<< device.discoveredDevice.deviceIdentifier << ")" << "\n";
		return false;
	}

	if (responseResult.outcome == UdpCommandResponseResult::Outcome::RecvError)
	{
		std::cerr << __func__ << ": Receive error during set return mode with "
			<< device.discoveredDevice.ipAddr << "\n";
		return false;
	}

	const ssize_t bytesReceived = responseResult.bytesReceived;
	if (bytesReceived
		< static_cast<ssize_t>(sizeof(comms::SetLiDARReturnModeResponse)))
	{
		std::cerr << __func__ << ": Response of size "
			<< bytesReceived << " too small from "
			<< device.discoveredDevice.ipAddr << "\n";
		return false;
	}

	comms::SetLiDARReturnModeResponse *response =
		reinterpret_cast<comms::SetLiDARReturnModeResponse*>(
			const_cast<uint8_t*>(responseResult.buffer));
	response->swapContentsToHostEndianness();

	if (response->command.cmd_set != CMD_SET_LIDAR ||
		response->command.cmd_id != CMD_ID_SET_RETURN_MODE_RESPONSE ||
		response->ret_code != 0x00)
	{
		return false;
	}

	device.currentReturnMode = Device::ReturnMode(returnMode);
	return true;
}

bool sendGetReturnModeCommand(Device &device)
{
	auto cmdEndpointFdDesc = getCmdEndpointFdDesc();
	if (!cmdEndpointFdDesc)
	{
		std::cerr << __func__ << ": No command endpoint available.\n";
		return false;
	}

	comms::GetLiDARReturnMode getReturnModeMsg;
	getReturnModeMsg.swapContentsToProtocolEndianness();
	getReturnModeMsg.header.setCrc16FromRawBytes();
	getReturnModeMsg.header.swapCrc16ToProtocolEndianness();
	getReturnModeMsg.footer.crc_32 = getReturnModeMsg.calculateCrc32();
	getReturnModeMsg.footer.swapCrc32ToProtocolEndianness();

	struct sockaddr_in deviceAddr;
	deviceAddr.sin_family = AF_INET;
	deviceAddr.sin_addr.s_addr = inet_addr(device.discoveredDevice.ipAddr.c_str());
	deviceAddr.sin_port = htons(LIVOX_COMMAND_PORT);

	ssize_t bytesSent = sendto(
		cmdEndpointFdDesc->native_handle(),
		&getReturnModeMsg, sizeof(getReturnModeMsg), 0,
		(struct sockaddr*)&deviceAddr, sizeof(deviceAddr));

	if (bytesSent < 0)
	{
		std::cerr << __func__ << ": Failed to send get return mode message: "
			<< strerror(errno) << std::endl;
		return false;
	}

	std::cout << __func__ << ": Sent get return mode message to "
		<< device.discoveredDevice.ipAddr << ":" << LIVOX_COMMAND_PORT
		<< std::endl;
	return true;
}

bool processGetReturnModeResponse(
	const UdpCommandResponseResult &responseResult,
	Device &device,
	uint8_t &outReturnMode)
{
	if (responseResult.outcome == UdpCommandResponseResult::Outcome::Timeout)
	{
		std::cerr << __func__ << ": Get return mode timeout with "
			<< device.discoveredDevice.ipAddr << "("
			<< device.discoveredDevice.deviceIdentifier
			<< ")" << "\n";
		return false;
	}

	if (responseResult.outcome == UdpCommandResponseResult::Outcome::RecvError)
	{
		std::cerr << __func__ << ": Receive error during get return mode with "
			<< device.discoveredDevice.ipAddr << std::endl;
		return false;
	}

	const ssize_t bytesReceived = responseResult.bytesReceived;
	if (bytesReceived
		< static_cast<ssize_t>(sizeof(comms::GetLiDARReturnModeResponse)))
	{
		std::cerr << __func__ << ": Response of size "
			<< bytesReceived << " too small from "
			<< device.discoveredDevice.ipAddr << std::endl;
		return false;
	}

	comms::GetLiDARReturnModeResponse *response =
		reinterpret_cast<comms::GetLiDARReturnModeResponse*>(
			const_cast<uint8_t*>(responseResult.buffer));
	response->swapContentsToHostEndianness();

	if (!(response->command.cmd_set == CMD_SET_LIDAR &&
	      response->command.cmd_id == CMD_ID_GET_RETURN_MODE_RESPONSE &&
	      response->ret_code == 0x00))
	{
		return false;
	}

	device.currentReturnMode = Device::ReturnMode(response->mode);
	outReturnMode = response->mode;
	return true;
}

} // namespace

sscl::co::ViralNonPostingInvoker<bool> Device::connectCReq()
{
	/**		FIXME:
	 * WE need to assign the ipAddr to the Device being connected up.
	 */

	if (getProtoState().smoCallbacks.OptionParser_getOptions().verbose) {
		std::cout << __func__ << ": Trying to connect to known device" << "\n";
	}

	Device::ConnectIpResult knownResult = co_await connectToKnownDeviceCReq();
	if (!knownResult.success)
	{
		if (getProtoState().smoCallbacks.OptionParser_getOptions().verbose)
		{
			std::cout << __func__ << ": Trying to connect to device by "
				<< "identifier" << "\n";
		}

		Device::ConnectIpResult idResult = co_await
			connectByDeviceIdentifierCReq();

		if (!idResult.success)
			{ co_return false; }

		discoveredDevice.ipAddr = idResult.ipAddr;
		startHeartbeat();
	}
	else
	{
		discoveredDevice.ipAddr = knownResult.ipAddr;
		startHeartbeat();
	}

	const bool success = true;
	if (!success)
	{
		std::cerr << __func__ << ": Failed to connect to device "
			"(" << discoveredDevice.deviceIdentifier
			<< ") @(" << discoveredDevice.ipAddr << ").\n";
		co_return false;
	}

	co_return success;
}

sscl::co::ViralNonPostingInvoker<Device::ConnectIpResult>
Device::connectToKnownDeviceCReq()
{
	/**		EXPLANATION:
	 * This function is used to connect to a device that is already known to the
	 * broadcastListener.
	 */
	Device::ConnectIpResult result;

	auto &protoState = livoxProto1::getProtoState();
	if (!protoState.deviceManager)
	{
		co_return result;
	}

	if (!protoState.deviceManager->broadcastListener.deviceExists(
		discoveredDevice.deviceIdentifier))
	{
		co_return result;
	}

	std::shared_ptr<livoxProto1::comms::DiscoveredDevice> deviceInfo =
		protoState.deviceManager->broadcastListener.getDevice(
			discoveredDevice.deviceIdentifier);
	if (!deviceInfo)
	{
		co_return result;
	}

	const std::string deviceIP = deviceInfo->ipAddr;

	auto smoIpResult = getSmoIp(deviceIP);
	if (!smoIpResult.has_value())
	{
		std::cerr << __func__ << ": Failed to detect SMO listening IP for "
			<< "known device ("
			<< discoveredDevice.deviceIdentifier << ")"
			<< " @(" << deviceIP << ").\n";

		co_return result;
	}

	if (getProtoState().smoCallbacks.OptionParser_getOptions().verbose)
	{
		std::cout << __func__ << ": Detected SMO listening IP for known device "
			<< discoveredDevice.deviceIdentifier
			<< " @(" << deviceIP << ") is "
			<< smoIpResult.value() << ". About to try to handshake.\n";
	}

	detectedSmoListeningIp = smoIpResult.value();

	const bool handshakeSuccess = co_await executeHandshakeCReq(deviceIP);
	result.success = handshakeSuccess;
	result.ipAddr = deviceIP;
	co_return result;
}

sscl::co::ViralNonPostingInvoker<Device::ConnectIpResult>
Device::connectByDeviceIdentifierCReq()
{
	/**		EXPLANATION:
	 * This method uses heuristic device IP construction from the serial number.
	 * This requires smoIp to be provided because:
	 * 1. We need the network prefix to generate a valid device IP address
	 * 2. Without a target device subnet, we cannot detect which interface faces the device
	 * 3. Therefore, if smoIp is omitted, heuristic construction is impossible
	 *
	 * If smoIp is not provided, the driver must rely only on broadcast advertisements
	 * from the device (handled by connectToKnownDeviceReq).
	 */
	Device::ConnectIpResult result;

	if (smoIp.empty()) { co_return result; }

	const std::string deviceIP = generateClientDeviceIpFromSerialNumber(
		discoveredDevice.deviceIdentifier);

	detectedSmoListeningIp = smoIp;

	if (getProtoState().smoCallbacks.OptionParser_getOptions().verbose)
	{
		std::cout << __func__ << ": About to try to connect to device by "
			<< "identifier (" << discoveredDevice.deviceIdentifier << ")"
			<< " at IP (" << smoIp << ").\n";
	}

	const bool handshakeSuccess = co_await executeHandshakeCReq(deviceIP);
	result.success = handshakeSuccess;
	result.ipAddr = deviceIP;
	co_return result;
}

sscl::co::ViralNonPostingInvoker<bool> Device::executeHandshakeCReq(
	const std::string &deviceIP)
{
	auto &protoState = livoxProto1::getProtoState();
	if (!protoState.deviceManager)
		{ co_return false; }

	auto cmdEndpointFdDesc = getCmdEndpointFdDesc();
	if (!cmdEndpointFdDesc)
	{
		std::cerr << __func__ << ": UdpCommandDemuxer not started or no "
			"command endpoint available." << std::endl;
		co_return false;
	}

	if (detectedSmoListeningIp.empty()) { co_return false; }

	if (!cmdEndpointFdDesc->is_open())
	{
		throw std::runtime_error(
			std::string(__func__) +
			": cmdEndpointFdDesc is null; cannot set up async callbacks "
			"for device " + deviceIP + "("
			+ discoveredDevice.deviceIdentifier + ")" + " handshake."
			"Check UdpCommandDemuxer initialization."
		);
	}

	try {
		if (!sendHandshakeRequest(
			*this, deviceIP, cmdEndpointFdDesc->native_handle()))
		{
			co_return false;
		}

		comms::UdpCommandDemuxer *demuxer = getUdpCommandDemuxer();
		if (!demuxer) { co_return false; }

		/**		EXPLANATION:
		 * Register a UDP command handler for handshake ACK
		 * (cmd_set=0x00, cmd_id=0x01).
		 * The handler will be called by the UdpCommandDemuxer when a handshake
		 * response is received.
		 */
		const UdpCommandResponseResult responseResult = co_await
			demuxer->waitForCommandResponseCReq(
				CMD_SET_GENERAL, CMD_ID_HANDSHAKE_ACK,
				deviceIP,
				commandTimeoutMs);

		co_return processHandshakeResponse(responseResult, *this, deviceIP);
	} catch (const std::exception &e) {
		std::cerr << __func__ << ": Handshake failed with " << deviceIP << ": "
			<< e.what() << std::endl;
		co_return false;
	}
}

sscl::co::ViralNonPostingInvoker<bool> Device::disconnectCReq()
{
	stopHeartbeat();

	if (discoveredDevice.ipAddr.empty())
	{
		std::cout << __func__ << ": No device IP available, skipping "
			"disconnect message" << std::endl;
		co_return true;
	}

	auto cmdEndpointFdDesc = getCmdEndpointFdDesc();
	if (!cmdEndpointFdDesc)
	{
		std::cout << __func__ << ": No command endpoint available, skipping "
			"disconnect message" << std::endl;
		co_return true;
	}

	comms::DisconnectMessage disconnectMsg;
	disconnectMsg.swapContentsToProtocolEndianness();
	disconnectMsg.header.setCrc16FromRawBytes();
	disconnectMsg.header.swapCrc16ToProtocolEndianness();
	disconnectMsg.footer.crc_32 = disconnectMsg.calculateCrc32();
	disconnectMsg.footer.swapCrc32ToProtocolEndianness();

	struct sockaddr_in deviceAddr;
	deviceAddr.sin_family = AF_INET;
	deviceAddr.sin_addr.s_addr = inet_addr(discoveredDevice.ipAddr.c_str());
	deviceAddr.sin_port = htons(LIVOX_COMMAND_PORT);

	ssize_t bytesSent = sendto(
		cmdEndpointFdDesc->native_handle(),
		&disconnectMsg, sizeof(disconnectMsg), 0,
		(struct sockaddr*)&deviceAddr, sizeof(deviceAddr));

	if (bytesSent < 0)
	{
		std::cerr << __func__ << ": Failed to send disconnect message: "
			<< strerror(errno) << std::endl;
	}

	std::cout << __func__ << ": Sent disconnect message to "
		<< discoveredDevice.ipAddr << ":" << LIVOX_COMMAND_PORT << std::endl;

	co_return true;
}

sscl::co::ViralNonPostingInvoker<bool> Device::enablePcloudDataCReq()
{
	if (discoveredDevice.ipAddr.empty())
	{
		std::cerr << __func__ << ": No device IP available for device "
			<< discoveredDevice.deviceIdentifier << std::endl;
		co_return false;
	}

	if (!sendEnDisablePcloudCommand(*this, 0x01, "enable pcloud data"))
		{ co_return false; }

	comms::UdpCommandDemuxer *demuxer = getUdpCommandDemuxer();
	if (!demuxer)
		{ co_return false; }

	const UdpCommandResponseResult responseResult = co_await
		demuxer->waitForCommandResponseCReq(
			CMD_SET_GENERAL, CMD_ID_SAMPLING_RESPONSE,
			discoveredDevice.ipAddr,
			commandTimeoutMs);

	if (!processSamplingResponse(responseResult, *this, "enable"))
		{ co_return false; }

	pcloudDataActive.store(true);
	co_return true;
}

sscl::co::ViralNonPostingInvoker<bool> Device::disablePcloudDataCReq()
{
	if (discoveredDevice.ipAddr.empty())
	{
		std::cerr << __func__ << ": No device IP available for device "
			<< discoveredDevice.deviceIdentifier << std::endl;
		co_return false;
	}

	/* Unconditionally close the pcloud data socket early since there's no good
	 * reason to only close it if the command packet succeeds and ACKs. We want
	 * to stop receiving data immediately when disable is requested.
	 */
	cleanupPcloudDataSocket();

	if (!sendEnDisablePcloudCommand(*this, 0x00, "disable pcloud data"))
		{ co_return false; }

	comms::UdpCommandDemuxer *demuxer = getUdpCommandDemuxer();
	if (!demuxer)
		{ co_return false; }

	const UdpCommandResponseResult responseResult = co_await
		demuxer->waitForCommandResponseCReq(
			CMD_SET_GENERAL,
			CMD_ID_SAMPLING_RESPONSE,
			discoveredDevice.ipAddr,
			commandTimeoutMs);

	if (!processSamplingResponse(responseResult, *this, "disable"))
		{ co_return false; }

	pcloudDataActive.store(false);
	co_return true;
}

sscl::co::ViralNonPostingInvoker<bool>
Device::setReturnModeCReq(uint8_t returnMode)
{
	if (discoveredDevice.ipAddr.empty())
	{
		std::cerr << __func__ << ": No device IP available for device "
			<< discoveredDevice.deviceIdentifier << std::endl;
		co_return false;
	}

	if (!sendSetReturnModeCommand(*this, returnMode)) { co_return false; }

	comms::UdpCommandDemuxer *demuxer = getUdpCommandDemuxer();
	if (!demuxer) { co_return false; }

	const UdpCommandResponseResult responseResult = co_await
		demuxer->waitForCommandResponseCReq(
			CMD_SET_LIDAR,
			CMD_ID_SET_RETURN_MODE_RESPONSE,
			discoveredDevice.ipAddr,
			commandTimeoutMs);

	co_return processSetReturnModeResponse(
		responseResult, *this, returnMode);
}

sscl::co::ViralNonPostingInvoker<Device::GetReturnModeResult>
Device::getReturnModeCReq()
{
	Device::GetReturnModeResult result;

	if (discoveredDevice.ipAddr.empty())
	{
		std::cerr << __func__ << ": No device IP available for device "
			<< discoveredDevice.deviceIdentifier << std::endl;
		co_return result;
	}

	if (!sendGetReturnModeCommand(*this)) { co_return result; }

	comms::UdpCommandDemuxer *demuxer = getUdpCommandDemuxer();
	if (!demuxer) { co_return result; }

	const UdpCommandResponseResult responseResult = co_await
		demuxer->waitForCommandResponseCReq(
			CMD_SET_LIDAR,
			CMD_ID_GET_RETURN_MODE_RESPONSE,
			discoveredDevice.ipAddr,
			commandTimeoutMs);

	uint8_t returnMode = 0;
	if (!processGetReturnModeResponse(responseResult, *this, returnMode))
		{ co_return result; }

	result.success = true;
	result.returnMode = returnMode;
	co_return result;
}

std::string Device::generateClientDeviceIpFromSerialNumber(
	const std::string& broadcastCode
	)
{
	/**		EXPLANATION:
	 * The input string is either a serial number (14 chars) or a broadcast code
	 * (15 chars). We need to determine which one it is and extract the serial
	 * number from the broadcast code.
	 *
	 * To generate a default IP address, we use the device's subnet: X.X.X.1XX
	 * where XX = last two digits of serial. We use the smoIp and smoSubnetNbits
	 * to determine the network prefix.
	 */
	if (broadcastCode.empty())
	{
		throw std::invalid_argument(
			std::string(__func__) + ": Broadcast code cannot be empty");
	}

	std::string serialNumber;
	if (broadcastCode.length() == 14)
	{
		// Input is a serial number
		serialNumber = broadcastCode;
	} else if (broadcastCode.length() == 15)
	{
		// Input is a broadcast code (serial + selector)
		serialNumber = broadcastCode.substr(0, 14);
	} else
	{
		// Invalid length
		throw std::invalid_argument(
			std::string(__func__) +
			": Broadcast code must be 14 or 15 characters long");
	}

	// Extract last two digits of serial number
	if (serialNumber.length() < 2)
	{
		throw std::invalid_argument(
			std::string(__func__) + ": Serial number too short");
	}

	std::string lastTwoDigits = serialNumber.substr(serialNumber.length() - 2);
	// Validate that last two characters are digits
	if (lastTwoDigits[0] < '0' || lastTwoDigits[0] > '9' ||
		lastTwoDigits[1] < '0' || lastTwoDigits[1] > '9')
	{
		throw std::invalid_argument(
			std::string(__func__) +
			": Last two characters of serial number must be digits");
	}

	/**		EXPLANATION:
	 * Use the device's subnet: X.X.X.1XX where XX = last two digits of serial.
	 * We use the smoIp and smoSubnetNbits to determine the network prefix.
	 */
	// Parse smoIp to extract network prefix
	auto smoIpOctets = comms::parseIPv4Address(smoIp);
	if (!smoIpOctets.has_value())
	{
		throw std::invalid_argument(
			std::string(__func__) + ": Invalid smoIp format: must be X.X.X.X");
	}

	// Generate subnet mask based on nbits
	uint32_t subnetMask = getSubnetMaskFor(smoSubnetNbits);

	uint32_t smoIpAddr = (std::stoi(smoIpOctets->octet1) << 24) |
		(std::stoi(smoIpOctets->octet2) << 16) |
		(std::stoi(smoIpOctets->octet3) << 8) |
		std::stoi(smoIpOctets->octet4);

	// Apply subnet mask to get network prefix
	uint32_t networkPrefix = smoIpAddr & subnetMask;

	// Extract octets from network prefix
	uint8_t octet1 = (networkPrefix >> 24) & 0xFF;
	uint8_t octet2 = (networkPrefix >> 16) & 0xFF;
	uint8_t octet3 = (networkPrefix >> 8) & 0xFF;

	// Use the first three octets and append "1" + last two digits
	return std::to_string(octet1) + "." + std::to_string(octet2) + "." +
		std::to_string(octet3) + ".1" + lastTwoDigits;
}

static void discardHeartbeatAck(
	const uint8_t* data, ssize_t bytesReceived,
	const struct sockaddr_in& senderAddr
	)
{
	(void)senderAddr;

	// Check if we have enough data for a HeartbeatACK message
	if (bytesReceived
		< static_cast<ssize_t>(sizeof(livoxProto1::comms::HeartbeatACK)))
	{
		std::cout << __func__ << ": Received heartbeat ACK with insufficient "
			"data (" << bytesReceived << " bytes, expected "
			<< sizeof(livoxProto1::comms::HeartbeatACK) << ")" << std::endl;
		return;
	}

	// Directly use a non-const reference to HeartbeatACK structure
	livoxProto1::comms::HeartbeatACK& ack =
		*reinterpret_cast<livoxProto1::comms::HeartbeatACK*>(
			const_cast<uint8_t*>(data));

	ack.swapContentsToHostEndianness();

	if (!ack.validateCrc32())
	{
		std::cerr << __func__ << ": Discarded heartbeat ACK - CRC32 validation "
			"failed" << std::endl;
		return;
	}

	if (!ack.header.validateCrc16())
	{
		std::cerr << __func__ << ": Discarded heartbeat ACK - CRC16 validation "
			"failed" << std::endl;
		return;
	}

	if (!ack.sanityCheck())
	{
		std::cerr << __func__ << ": Discarded heartbeat ACK - sanity check "
			"failed" << std::endl;
		return;
	}

	if (ack.work_state == 0x01) { return; }

	// Print work_state with human-readable description
	std::string workStateStr;
	switch (ack.work_state)
	{
		case 0x00:
			workStateStr = "Initializing";
			break;
		case 0x01:
			workStateStr = "Normal";
			break;
		case 0x02:
			workStateStr = "Power-Saving";
			break;
		case 0x03:
			workStateStr = "Standby";
			break;
		case 0x04:
			workStateStr = "Error";
			break;
		default:
			workStateStr = "Unknown";
			break;
	}

	std::cerr << __func__ << ": Lidar not ready for operation: work_state: 0x"
		<< std::hex << static_cast<int>(ack.work_state) << std::dec
		<< " (" << workStateStr << "), ack_msg: 0x"
		<< std::hex << ack.ack_msg << std::dec << std::endl;
}

void Device::startHeartbeat()
{
	if (!componentThread || discoveredDevice.ipAddr.empty())
	{
		throw std::runtime_error(
			std::string(__func__) +
			": Can't start heartbeat without component thread or IP");
	}

	// Register heartbeat ACK handler (cmd_set=0x00, cmd_id=0x03)
	sscl::SpinLock::Guard lock(heartbeatActiveLock);

	registerUdpCommandHandler(
		0x00, 0x03, discardHeartbeatAck, discoveredDevice.ipAddr);

	// Create heartbeat timer
	heartbeatTimer = std::make_unique<boost::asio::deadline_timer>(
		componentThread->getIoContext());

	heartbeatActive.store(true);

	// Send first heartbeat immediately
	sendHeartbeat();
}

void Device::stopHeartbeat()
{
	{
		sscl::SpinLock::Guard lock(heartbeatActiveLock);

		heartbeatActive.store(false);
		unregisterUdpCommandHandler(0x00, 0x03, discoveredDevice.ipAddr);
	}

	if (heartbeatTimer) {
		heartbeatTimer->cancel();
		heartbeatTimer.reset();
	}
}

void Device::sendHeartbeat()
{
	if (!heartbeatActive.load())
	{
		std::cerr << __func__ << ": Ending heartbeat loop due to "
			"heartbeatActive==false.\n";
		return;
	}

	if (discoveredDevice.ipAddr.empty())
	{
		std::cerr << __func__ << ": Ending heartbeat loop due to "
			"discoveredDevice.ipAddr.empty().\n";
		return;
	}

	// Get the command endpoint from the UdpCommandDemuxer
	auto& protoState = livoxProto1::getProtoState();
	if (!protoState.deviceManager)
	{
		std::cerr << __func__ << ": No device manager available\n";
		return;
	}

	auto cmdEndpointFdDesc = protoState.deviceManager->udpCommandDemuxer
		.getCmdEndpointFdDesc();
	if (!cmdEndpointFdDesc)
	{
		std::cerr << __func__ << ": No command endpoint available\n";
		return;
	}

	try {
		comms::HeartbeatMessage heartbeatMsg;
		heartbeatMsg.swapContentsToProtocolEndianness();
		heartbeatMsg.header.setCrc16FromRawBytes();
		heartbeatMsg.header.swapCrc16ToProtocolEndianness();
		heartbeatMsg.footer.crc_32 = heartbeatMsg.calculateCrc32();
		heartbeatMsg.footer.swapCrc32ToProtocolEndianness();

		// Set up destination address for raw socket
		struct sockaddr_in deviceAddr;
		memset(&deviceAddr, 0, sizeof(deviceAddr));
		deviceAddr.sin_family = AF_INET;
		deviceAddr.sin_addr.s_addr = inet_addr(discoveredDevice.ipAddr.c_str());
		// Heartbeats and commands go to port 65000
		deviceAddr.sin_port = htons(65000);

		ssize_t bytesSent = sendto(
			cmdEndpointFdDesc->native_handle(),
			&heartbeatMsg, sizeof(heartbeatMsg), 0,
			(struct sockaddr*)&deviceAddr, sizeof(deviceAddr));

		if (bytesSent < 0)
		{
			std::cerr << "[" << __func__ << "] Failed to send heartbeat: "
				<< strerror(errno) << std::endl;
			return;
		}

		/**		EXPLANATION:
		 * Schedule next heartbeat in 1 second, per the spec.
		 */
		heartbeatTimer->expires_from_now(boost::posix_time::seconds(1));
		heartbeatTimer->async_wait(
			[this](const boost::system::error_code& error) {
				onHeartbeatTimer(error);
			}
		);
	}
	catch (const std::exception& e)
	{
		std::cerr << __func__ << ": Heartbeat send failed for device "
			<< discoveredDevice.deviceIdentifier
			<< ": " << e.what() << std::endl;
	}
}

void Device::onHeartbeatTimer(const boost::system::error_code& error)
{
	// Timer was cancelled, heartbeat stopped
	if (error == boost::asio::error::operation_aborted) {
		return;
	}

	if (error)
	{
		std::cerr << "[" << __func__ << "] Heartbeat timer error for device "
			<< discoveredDevice.deviceIdentifier
			<< ": " << error.message() << std::endl;

		return;
	}

	// Send next heartbeat
	{
		sscl::SpinLock::Guard lock(heartbeatActiveLock);
		if (!heartbeatActive.load())
			{ return; }

		sendHeartbeat();
	}
}

uint32_t Device::getSubnetMaskFor(uint8_t nbits)
{
	if (nbits > 32) {
		throw std::invalid_argument(
			std::string(__func__) + ": nbits must be between 0 and 32");
	}

	// Generate subnet mask: set the first nbits to 1, rest to 0
	if (nbits == 0) {
		return 0x00000000;
	} else if (nbits == 32) {
		return 0xFFFFFFFF;
	} else {
		// Create mask with nbits set to 1 from the left
		return (0xFFFFFFFF << (32 - nbits));
	}
}

std::optional<std::string> Device::detectSmoIp(const std::string& deviceIP)
{
	/**		EXPLANATION:
	 * This function detects the SMO IP address of the interface that's facing
	 * the device by iterating through all network interfaces and checking for
	 * the interface that has the IP address in the same subnet as the device's
	 * IP address.
	 */
	try {
		// Parse the device IP to get the network prefix
		auto deviceIpOctets = comms::parseIPv4Address(deviceIP);
		if (!deviceIpOctets.has_value()) {
			return std::nullopt;
		}

		// Convert device IP octets to integers for bitwise operations
		uint32_t deviceIpAddr = (std::stoi(deviceIpOctets->octet1) << 24) |
			(std::stoi(deviceIpOctets->octet2) << 16) |
			(std::stoi(deviceIpOctets->octet3) << 8) |
			std::stoi(deviceIpOctets->octet4);

		// Generate subnet mask based on nbits
		uint32_t subnetMask = getSubnetMaskFor(smoSubnetNbits);

		/* Get all network interfaces using getifaddrs (Linux/Unix specific)
		 *
		 * FIXME: Add Windows support using GetAdaptersAddresses when porting
		 */
		struct ifaddrs *ifaddr;
		if (getifaddrs(&ifaddr) == -1) {
			return std::nullopt;
		}

		// Use unique_ptr for automatic cleanup (RAII) to free ifaddrs
		auto ifaddr_deleter = [](struct ifaddrs* ptr) { freeifaddrs(ptr); };
		std::unique_ptr<struct ifaddrs, decltype(ifaddr_deleter)> ifaddr_ptr(
			ifaddr, ifaddr_deleter);

		std::string found_ip;

		/**		EXPLANATION:
		 * Iterate through all network interfaces and check if the IP address is
		 * in the same subnet as the device's IP address.
		 */
		for (struct ifaddrs *ifa = ifaddr; ifa != nullptr; ifa = ifa->ifa_next)
		{
			if (ifa->ifa_addr == nullptr) continue;

			// Check if it's IPv4
			if (ifa->ifa_addr->sa_family != AF_INET) { continue; }

			// Get the IPv4 address
			struct sockaddr_in* addr_in = (struct sockaddr_in*)ifa->ifa_addr;
			char ip_str[INET_ADDRSTRLEN];
			if (inet_ntop(
				AF_INET, &addr_in->sin_addr, ip_str, INET_ADDRSTRLEN)
					== nullptr)
			{
				continue;
			}

			std::string ip = ip_str;

			// Check if this IP is in the same subnet
			auto ipOctets = comms::parseIPv4Address(ip);
			if (!ipOctets.has_value()) { continue; }

			// Convert IP octets to integer
			uint32_t ipAddr = (std::stoi(ipOctets->octet1) << 24) |
				(std::stoi(ipOctets->octet2) << 16) |
				(std::stoi(ipOctets->octet3) << 8) |
				std::stoi(ipOctets->octet4);

			/* Check if this iface's IP is in the same subnet as the device's IP
			 * using the calculated mask. Only compare the bits that are set in
			 * the subnet mask.
			 */
			if ((ipAddr & subnetMask) == (deviceIpAddr & subnetMask))
			{
				found_ip = ip;
				break;
			}
		}

		// Return the found IP (empty string if none found)
		if (!found_ip.empty()) {
			return found_ip;
		}

		return std::nullopt;
	} catch (const std::exception& e) {
		std::cerr << "Error detecting SMO IP: " << e.what() << std::endl;
		return std::nullopt;
	}
}

std::optional<std::string> Device::getSmoIp(const std::string& deviceIP)
{
	/**		EXPLANATION:
	 * This is only used when connecting to a device that's already known to
	 * the broadcastListener.
	 * It is NOT and SHOULD not be used when connecting by heuristic IP
	 * construction using the client device's serial number.
	 *
	 * Determines the SMO listening IP address for this device.
	 * If smoIp is provided, validate it against detected IP and return it.
	 * If smoIp is empty, attempt auto-detection based on device IP.
	 * Returns std::optional<std::string> - empty if detection fails.
	 */
	auto detectedIp = detectSmoIp(deviceIP);

	if (!smoIp.empty())
	{
		// smoIp was provided, validate it against detected IP
		if (detectedIp.has_value() && detectedIp.value() != smoIp)
		{
			// Print warning if provided smoIp doesn't match detected IP
			std::cerr << "Warning: Provided smo-ip (" << smoIp 
				<< ") doesn't match detected IP (" << detectedIp.value() 
				<< ") for device " << discoveredDevice.deviceIdentifier
				<< " @(" << deviceIP << ")"
				<< ". Using provided smo-ip anyway." << std::endl;
		}
		return smoIp;
	}

	if (detectedIp.has_value()) {
		return detectedIp.value();
	}

	// Auto-detection failed
	return std::nullopt;
}

void Device::cleanupPcloudDataSocket()
{
	pcloudDataActive.store(false);
}

void Device::handleUdpDgram(
	const uint8_t *data, ssize_t bytesReceived,
	const struct sockaddr_in &senderAddr
	)
{
	// Check minimum size for any valid protocol message
	if (bytesReceived < static_cast<ssize_t>(
		sizeof(comms::Header) + sizeof(comms::Command)))
	{
		// Too small for header + command
		return;
	}

	// Extract command set and command ID from the first two bytes after the header
	uint8_t cmd_set = data[sizeof(comms::Header)];
	uint8_t cmd_id = data[sizeof(comms::Header) + 1];

	// Look for a registered handler for this command
	auto key = std::make_pair(cmd_set, cmd_id);
	auto it = udpCommandHandlers.find(key);

	if (it != udpCommandHandlers.end())
	{
		// Found a registered handler, invoke it
		try
		{
			it->second(data, bytesReceived, senderAddr);
		}
		catch (const std::exception& e)
		{
			std::cerr << __func__ << ": Exception in command handler for "
			          << discoveredDevice.deviceIdentifier
			          << " cmd_set=" << (int)cmd_set
			          << " cmd_id=" << (int)cmd_id
			          << ": " << e.what() << std::endl;
		}
	}
	// Unknown command types are silently ignored
}

void Device::registerUdpCommandHandler(
	uint8_t cmd_set, uint8_t cmd_id,
	std::function<void(
		const uint8_t* data, ssize_t bytesReceived,
		const struct sockaddr_in& senderAddr)> handler,
	const std::string& deviceIP
	)
{
	/**		EXPLANATION:
	 * Register a UDP command handler for the given cmd_set and cmd_id.
	 * If the handler already exists for the given device IP, replace it.
	 * If the handler does not exist, add it to the temporary collection.
	 *
	 * Adding a handler to a cmd_set+cmd_id pair which already has a handler
	 * results in the new handler replacing the old one.
	 */
	auto key = std::make_pair(cmd_set, cmd_id);
	udpCommandHandlers[key] = handler; // Don't move, we need to copy

	/**		EXPLANATION:
	 * Add to temporary collection if deviceIP is provided (not empty)
	 */
	if (!deviceIP.empty())
	{
		auto& handlers = devicesUnderConstruction[deviceIP];
		auto it = std::find_if(handlers.begin(), handlers.end(),
			[cmd_set, cmd_id](const CommandHandler& existing) {
				return existing.cmd_set == cmd_set && existing.cmd_id == cmd_id;
			});

		// Create the new command handler
		CommandHandler cmdHandler;
		cmdHandler.cmd_set = cmd_set;
		cmdHandler.cmd_id = cmd_id;
		cmdHandler.handler = std::move(handler);

		if (it != handlers.end()) {
			// Replace existing handler
			*it = std::move(cmdHandler);
		} else {
			// Add new handler
			handlers.push_back(std::move(cmdHandler));
		}
	}
}

void Device::unregisterUdpCommandHandler(
	uint8_t cmd_set, uint8_t cmd_id, const std::string& deviceIP
	)
{
	auto key = std::make_pair(cmd_set, cmd_id);
	udpCommandHandlers.erase(key);

	/**		EXPLANATION:
	 * Remove from temporary collection if deviceIP is provided (not empty)
	 */
	if (!deviceIP.empty())
	{
		auto it = devicesUnderConstruction.find(deviceIP);
		if (it != devicesUnderConstruction.end()) {
			// Remove the specific command handler for this cmd_set/cmd_id
			auto& handlers = it->second;
			handlers.erase(
				std::remove_if(handlers.begin(), handlers.end(),
					[cmd_set, cmd_id](const CommandHandler& h) {
						return h.cmd_set == cmd_set && h.cmd_id == cmd_id;
					}),
				handlers.end());

			// If no handlers left for this IP, remove the entire entry
			if (handlers.empty()) {
				devicesUnderConstruction.erase(it);
			}
		}
	}
}

} // namespace livoxProto1