SkyPortal-plugin/Source/SkyPortal/Private/SkyPortalSubsystem.cpp

// A lot of this code is made because of the work of https://github.com/capull0/SkyDumper and https://github.com/silicontrip/SkyReader

#include "SkyPortalSubsystem.h"
#include "Engine/Engine.h"

#include "HAL/RunnableThread.h"
#include "Misc/ScopeLock.h"
#include "SkyPortalIO.h"


void USkyPortalSubsystem::Initialize(FSubsystemCollectionBase& Collection)
{

	Super::Initialize(Collection);
	// Start the status checker thread
	StatusChecker = new FPortalStatusChecker(this, 0.1f);  // Check every 5 seconds
	StatusCheckerThread = FRunnableThread::Create(StatusChecker, TEXT("PortalStatusCheckerThread"), 0, TPri_AboveNormal);

	UE_LOG(LogTemp, Log, TEXT("SkyPortalSubsystem Initialized"));

}

void USkyPortalSubsystem::Deinitialize()
{
	// Stop the thread when the subsystem is deinitialized
	if (StatusChecker)
	{
		StatusChecker->Stop();
		StatusCheckerThread->WaitForCompletion();

		delete StatusCheckerThread;
		delete StatusChecker;
		StatusChecker = nullptr;
		StatusCheckerThread = nullptr;
	}

	// Disconnect portal
	if (PortalDevice) {
		hid_close(PortalDevice);
	}
	hid_exit();


	UE_LOG(LogTemp, Log, TEXT("SkyPortalSubsystem Deinitialized"));
	Super::Deinitialize();
}

// Antenna up / activate
void USkyPortalSubsystem::ActivatePortal(int active)
{
	RWBlock req, res;

	memset(req.buf, 0, rw_buf_size);
	req.buf[1] = 'A';
	req.buf[2] = active;
	do { Write(&req); } while (CheckResponse(&res, 'A'));

}

// Start portal
void USkyPortalSubsystem::RestartPortal()
{
	RWBlock req, res;

	memset(req.buf, 0, rw_buf_size);
	req.buf[1] = 'R';

	do { Write(&req); } while (CheckResponse(&res, 'R'));

}


/*
Number of endpoints : 2
found an IN End Point 0 with attributes interrupt and address 0x1
found an OUT End Point 1 with attributes interrupt and address 0x1
*/
bool USkyPortalSubsystem::OpenPortalHandle() {

	//reset
	if (PortalDevice) {
		hid_close(PortalDevice);
	}

	/*
		Declare two pointers to hold information about HID devices.
		"list" will point to the head of the linked list of devices,
		"attributes" will be used to iterate through the list.
	*/
	struct hid_device_info* list, * attributes;

	list = hid_enumerate(0x0, 0x0);
	// If `list` is NULL, that means no devices were found or there was an error.
	// In this case, print an error message and terminate the program.
	if (!list) {
		UE_LOG(LogHIDApi, Error, TEXT("No devices found"));
		// Get the error message from the HIDAPI
		HidError = hid_error(NULL);
		UE_LOG(LogHIDApi, Error, TEXT("%s"), *HidError);
		return false;
	}
	attributes = list;




	// Iterate through the linked list of devices
	int vendorCount = sizeof(VendorIds) / sizeof(VendorIds[0]);
	int productCount = sizeof(ProductIds) / sizeof(ProductIds[0]);

	while (attributes) {
		// Check if the devices match any of vendor_id and product_id 
		for (int i = 0; i < vendorCount; i++) {
			for (int j = 0; j < productCount; j++) {
				if (attributes->vendor_id == VendorIds[i] && attributes->product_id == ProductIds[j]) {
					UE_LOG(LogHIDApi, Display, TEXT("Portal found"));
					UE_LOG(LogHIDApi, Log, TEXT("Vendor ID: 0x%x, Product ID: 0x%x"), attributes->vendor_id, attributes->product_id);

					PortalDevice = hid_open(attributes->vendor_id, attributes->product_id, NULL);
					if (PortalDevice) {
						UE_LOG(LogHIDApi, Display, TEXT("Successful connection to Portal."));

						// Free the device list
						hid_free_enumeration(list);
						//bPortalConnected = true;
						return true;
					}

					break;
				}
			}
		}

		// Move to the next device in the list
		attributes = attributes->next;
	}

	// Free the device list
	hid_free_enumeration(list);
	UE_LOG(LogHIDApi, Error, TEXT("No Portals found"));
	return false;
}
//{ Region Color & light functions
void USkyPortalSubsystem::ChangePortalColor(const FLinearColor& Color)
{

	unsigned char r = FMath::Clamp(Color.R * 100, 0.0f, 255.0f);
	unsigned char g = FMath::Clamp(Color.G * 100, 0.0f, 255.0f);
	unsigned char b = FMath::Clamp(Color.B * 100, 0.0f, 255.0f);

	RWBlock req;

	memset(req.buf, 0, rw_buf_size);

	req.buf[1] = 'C';
	req.buf[2] = r; // R
	req.buf[3] = g; // G
	req.buf[4] = b; // B

	// no response for this one.
	Write(&req);
}
void USkyPortalSubsystem::ChangePortalColorside(const FLinearColor& Color, const EPortalSide PortalSide, const float BlendTime)
{

	unsigned char r = FMath::Clamp(Color.R * 100, 0, 255);
	unsigned char g = FMath::Clamp(Color.G * 100, 0, 255);
	unsigned char b = FMath::Clamp(Color.B * 100, 0, 255);
	EPortalSide _portalside;
	RWBlock req, res;

	memset(req.buf, 0, rw_buf_size);

	if (PortalSide == EPortalSide::BOTH) {
		_portalside = EPortalSide::LEFT;
	}

	switch (_portalside) {
	case EPortalSide::LEFT:
		req.buf[1] = 'J';
		req.buf[2] = 0x00;
	case EPortalSide::RIGHT:
		req.buf[1] = 'J';
		req.buf[2] = 0x02;
	case EPortalSide::TRAP:
		req.buf[1] = 'L';
		req.buf[2] = 0x01;
		req.buf[3] = FMath::Max3(r, g, b); // calculate brightness
		Write(&req); //since it's a color command for trap, only 3 bytes are needed, no response.
		return;
	}

	req.buf[3] = r; // R
	req.buf[4] = g; // G
	req.buf[5] = b; // B

	//Convert the time in millisecond into two bytes
	uint16_t _time = BlendTime;
	uint8_t _time_low = _time & 0xFF;       // Get the low byte by masking the least significant 8 bits
	uint8_t _time_high = (_time >> 8) & 0xFF; // Get the high byte extracted by shifting the bits 8 positions to the right and masking the result
	req.buf[6] = _time_low;
	req.buf[7] = _time_high;

	do { Write(&req); } while (CheckResponse(&res, 'J'));

	if (PortalSide == EPortalSide::BOTH) {
		ChangePortalColorside(Color, EPortalSide::RIGHT, BlendTime); // send a second command for the right side
	}
}
//}


// write function need to be different on windows, as hid_write doesn't work with the way windows handle I/O
#if PLATFORM_WINDOWS

#include <Windows.h>

#define HID_CTL_CODE(id)    \
        CTL_CODE(FILE_DEVICE_KEYBOARD, (id), METHOD_NEITHER, FILE_ANY_ACCESS)
#define HID_IN_CTL_CODE(id)  \
        CTL_CODE(FILE_DEVICE_KEYBOARD, (id), METHOD_IN_DIRECT, FILE_ANY_ACCESS)
#define IOCTL_HID_SET_OUTPUT_REPORT             HID_IN_CTL_CODE(101)

struct hid_device_ {
	HANDLE device_handle;
	BOOL blocking;
	USHORT output_report_length;
	unsigned char* write_buf;
	size_t input_report_length;
	USHORT feature_report_length;
	unsigned char* feature_buf;
	wchar_t* last_error_str;
	BOOL read_pending;
	char* read_buf;
	OVERLAPPED ol;
	OVERLAPPED write_ol;
	struct hid_device_info* device_info;
};


void USkyPortalSubsystem::Write(RWBlock* pb)
{
	if (!ensure(PortalDevice)) {
		UE_LOG(LogSkyportalIO, Error, TEXT("No Portal found"));
		return;
	}
	BOOL res;

	OVERLAPPED ol;
	memset(&ol, 0, sizeof(ol));

	DWORD bytes_returned;

	res = DeviceIoControl(PortalDevice->device_handle,
		IOCTL_HID_SET_OUTPUT_REPORT,
		(unsigned char*)pb->buf, 0x21,
		(unsigned char*)pb->buf, 0x21,
		&bytes_returned, &ol);
	ensureMsgf(res, TEXT("Unable to write to Portal"));
}

#else
void USkyPortalSubsystem::Write(RWBlock* pb) {

	if (!ensure(PortalDevice)) {
		UE_LOG(LogSkyportalIO, Error, TEXT("No Portal found"));
		return;
	}

	pb->buf[0] = 0; // Use report 0


	ensureMsgf(hid_write(PortalDevice, pb->buf, 0x21) != -1, TEXT("Unable to write to Portal, %s"), hid_error(PortalDevice));
	UE_LOG(LogSkyportalIO, Error, TEXT("Unable to write to Portal. error:\n %s"), hid_error(PortalDevice));
}
#endif


FPortalStatusData USkyPortalSubsystem::ParsePortalStatus(const RWBlock& ResponseBlock)
{
	FPortalStatusData PortalStatusData;

	// Parse the figure status array (little-endian 32-bit integer)
	uint32 FigureStatusArray = 0;
	// Reading the 32-bit integer (character status array) from the buffer
	FigureStatusArray |= ResponseBlock.buf[1]; // 1st byte
	FigureStatusArray |= (ResponseBlock.buf[2] << 8);  // 2nd byte
	FigureStatusArray |= (ResponseBlock.buf[3] << 16); // 3rd byte
	FigureStatusArray |= (ResponseBlock.buf[4] << 24); // 4th byte

	TStaticArray<EFigureStatus, 16> tempArray;
	// For each of the 16 entries, extract the 2-bit status and map it to EFigureStatus
	for (int32 i = 0; i < 16; ++i)
	{
		uint8 StatusBits = (FigureStatusArray >> (i * 2)) & 0b11;  // Extract 2 bits
		EFigureStatus FigureStatus;

		switch (StatusBits)
		{
		case 0b00:
			FigureStatus = EFigureStatus::NOT_PRESENT;
			break;
		case 0b01:
			FigureStatus = EFigureStatus::PRESENT;
			break;
		case 0b11:
			FigureStatus = EFigureStatus::ADDED;
			break;
		case 0b10:
			FigureStatus = EFigureStatus::REMOVED;
			break;
		default:
			FigureStatus = EFigureStatus::NOT_PRESENT;  // Default case
			break;
		}

		// Add to the array of figure statuses
		//PortalStatusData.StatusArray.Insert(FigureStatus, i);
		tempArray[i] = FigureStatus;
	}
	PortalStatusData.StatusArray.SetNum(0);
	PortalStatusData.StatusArray.Append(tempArray);
	// The next byte is the response counter
	PortalStatusData.Counter = ResponseBlock.buf[5];

	// The last byte is the boolean indicating whether the portal is ready
	PortalStatusData.bIsReady = ResponseBlock.buf[6] != 0;  // 0 means not ready, non-zero means ready

	return PortalStatusData;
}

void USkyPortalSubsystem::Sleep(int sleepMs) {
	FPlatformProcess::Sleep(sleepMs * 0.0001);
}

void USkyPortalSubsystem::CheckComplexResponse() {

	if (!PortalDevice) {
		return;
	}

	RWBlock req, res;

	memset(req.buf, 0, rw_buf_size);
	req.buf[1] = 'S';
	Write(&req);

	int BuffResponse = hid_read_timeout(PortalDevice, res.buf, rw_buf_size, TIMEOUT);
	if (BuffResponse < 0) {
		UE_LOG(LogSkyportalIO, Error, TEXT("Error.\n %s"), hid_error(PortalDevice));
		return;
	}
	EPortalCommand CommandResponse = GetPortalCommandFromChar(res.buf[0]);
	switch (CommandResponse)
	{
	case A:
		break;
	case C:
		break;
	case J:
		break;
	case L:
		break;
	case M:
		break;
	case Q:
		break;
	case R:
		break;
	case S:
		CurrentStatusData = ParsePortalStatus(res);

		//Send delegate when new informations are received
		if (OldStatusData != CurrentStatusData) {

			for (int i = 0; i < CurrentStatusData.StatusArray.Num(); i++) {
				if (CurrentStatusData.StatusArray[i] != OldStatusData.StatusArray[i]) {
					if (
						//!FalsePositive() //filter conflicting infos
						true) {
						FigureDataBlock FigureData;
						switch (CurrentStatusData.StatusArray[i])
						{
						case EFigureStatus::NOT_PRESENT:
							OnSkylanderRemoved.Broadcast(00, i);
							break;
						case EFigureStatus::PRESENT:
							FigureData = ReadFigureBlocks(i);
							OnSkylanderAdded.Broadcast(GetFigureID(FigureData), i);
							break;
						case EFigureStatus::ADDED:
							FigureData = ReadFigureBlocks(i);
							OnSkylanderAdded.Broadcast(GetFigureID(FigureData), i);
						case EFigureStatus::REMOVED:
							OnSkylanderRemoved.Broadcast(00, i);
						}
					}
				}
			}


			OldStatusData = CurrentStatusData;
		}


		break;
	default:
		break;
	}

}

TArray<uint8> USkyPortalSubsystem::QueryBlock(uint8 characterIndex, uint8 block)
{
	TArray<uint8> QueryCommand;
	QueryCommand.SetNum(0x21);
	QueryCommand[1] = 'Q';
	QueryCommand[2] = characterIndex;
	QueryCommand[3] = block;

	TArray<uint8> Output;

	do {
		portalConnection->Write(QueryCommand);
		output = portalConnection->Read();
	} while (output[0] != 'Q' || (output[1] % 0x10 != characterIndex && output[1] != 0x01) || output[2] != block);

	return output;
}



bool USkyPortalSubsystem::FalsePositive() const
{
	int dif = FMath::Abs(CurrentStatusData.Counter - OldStatusData.Counter);
	return (dif <= 2 || dif >= 254);
}



/* Verify the command response, when only a character is sended by the portal.
*
*TODO: Refacto this function to handle better the response/output from the portal
*/
bool USkyPortalSubsystem::CheckResponse(RWBlock* res, char expect)
{
	if (!PortalDevice) {
		return false;
	}

	int b = hid_read_timeout(PortalDevice, res->buf, rw_buf_size, TIMEOUT);

	if (b < 0) {
		UE_LOG(LogSkyportalIO, Error, TEXT("Error.\n %s"), hid_error(PortalDevice));
		return false;
	}

	res->BytesTransferred = b;

	/* this is here to debug the different responses from the portal.
	#if DEBUG
		SkylanderIO* skio;
		skio = new SkylanderIO();
		printf("<<<\n");
		skio->fprinthex(stdout, res->buf, 0x21);
		delete skio;
	#endif
	*/

	// found wireless USB but portal is not connected
	if (res->buf[0] == 'Z')
	{
		UE_LOG(LogSkyportalIO, Error, TEXT("found wireless USB but portal is not connected"));
		return false;
	}

	// Status says no skylander on portal	
	if (res->buf[0] == 'Q' && res->buf[1] == 0) {
		UE_LOG(LogSkyportalIO, Warning, TEXT("Status says no skylander on portal"));
	}

	if (res->buf[0] == 'R' && res->buf[1] == 0) {
		UE_LOG(LogSkyportalIO, Warning, TEXT("Status says no skylander on portal"));
	}

	return   (res->buf[0] != expect);
}



bool USkyPortalSubsystem::WriteBlock(unsigned int block, unsigned char data[0x10], int skylander) {
	RWBlock req, res; //request and response buffer
	unsigned char verify[0x10]; // A 16-byte array used to verify the data that was written to the portal.

	UE_LOG(LogSkyportalIO, Verbose, TEXT("Trying to write the current block :%X\n"), block);

	// Trying to write 3 times
	for (int retries = 0; retries < 3; retries++) {
		// Write request
		// W	57 10 <block number> <0x10 bytes of data>
		memset(req.buf, 0, rw_buf_size);//Reset request buffer
		req.buf[1] = 'W';
		req.buf[2] = 0x10 + skylander;
		req.buf[3] = (unsigned char)block;
		memcpy(req.buf + 4, data, 0x10);

		do { Write(&req); } while (CheckResponse(&res, 'W'));

		Sleep(100); //Wait 0.1 seconds for write to take effect

		memset(verify, 0xCD, sizeof(verify)); // 0xCD is a placeholder value
		ReadBlock(block, verify, skylander);

		if (memcmp(data, verify, sizeof(verify))) {
			UE_LOG(LogSkyportalIO, Error, TEXT("verification of the written block failed"));
			continue; //retry
		}
		UE_LOG(LogSkyportalIO, Verbose, TEXT("block successfully written"));
		return true;
	}
	UE_LOG(LogSkyportalIO, Fatal, TEXT("failed to write block"));
	return false;
}


EPortalCommand USkyPortalSubsystem::GetPortalCommandFromChar(unsigned char Char)
{
	switch (Char)
	{
	case 'A':
		return EPortalCommand::A;
	case 'C':
		return EPortalCommand::C;
	case 'J':
		return EPortalCommand::J;
	case 'L':
		return EPortalCommand::L;
	case 'M':
		return EPortalCommand::M;
	case 'Q':
		return EPortalCommand::Q;
	case 'R':
		return EPortalCommand::R;
	case 'S':
		return EPortalCommand::S;
	default:
		// Handle the case when the character doesn't match any enum
		// Return a default or invalid value, or handle the error
		UE_LOG(LogSkyportalIO, Warning, TEXT("Invalid character for Portal Command: %c"), TCHAR(Char));
		return EPortalCommand::S;  // 'S' for Status as a default
	}

}

bool USkyPortalSubsystem::ReadBlock(unsigned int block, unsigned char data[0x10], int skylanderIndex) {
	RWBlock req, res; //request and response buffers
	unsigned char followup;

	UE_LOG(LogSkyportalIO, Verbose, TEXT("PortalIO:ReadBlock :%X"), block);


	// Checking if the block is not out of range
	if (!ensure(block < 0x40)) {
		UE_LOG(LogSkyportalIO, Error, TEXT("PortalIO:ReadBlock failed, block out of range"));
		return false;  // Early return instead of throwing an exception
	}

	// Send query request

	// Trying to read data 15x
	for (int attempt = 0; attempt < 15; attempt++)
	{
		int i = 0;

		memset(req.buf, 0, rw_buf_size); // Clear the request buffer (initialize all bytes to zero)
		req.buf[1] = 'Q';
		followup = 0x10 + skylanderIndex;
		req.buf[2] = followup;
		if (block == 0) {
			req.buf[2] = followup + 0x10; // may not be needed
		}
		req.buf[3] = (unsigned char)block;

		memset(&(res.buf), 0, rw_buf_size); // Clear the response buffer to prepare for incoming data


		do { Write(&req); } while (CheckResponse(&res, 'Q'));

		if (res.buf[0] == 'Q' && res.buf[2] == (unsigned char)block) {
			// Got our query back
			if (res.buf[1] == followup) {
				// got the query back with no error
				memcpy(data, res.buf + 3, 0x10);
				UE_LOG(LogSkyportalIO, Verbose, TEXT("PortalIO:ReadBlock success"));
				return true;
			}
		}


	} // retries

	UE_LOG(LogSkyportalIO, Fatal, TEXT("PortalIO:ReadBlock failed after retries"));
	ensureMsgf(false, TEXT("PortalIO: Failed to read block after multiple retries"));
	return false;
}

bool USkyPortalSubsystem::ConnectPortal()
{

	bPortalConnected = OpenPortalHandle();
	if (bPortalConnected) {
		RestartPortal();
		ActivatePortal(1);

		Sleep(500);
		ChangePortalColor(FLinearColor(0.5, 0.5, 0.5));

		UE_LOG(LogSkyportalIO, Log, TEXT("Portal connected: "));
	}
	return bPortalConnected;
}

bool USkyPortalSubsystem::bIsPortalReady()
{
	return false;
}

void USkyPortalSubsystem::SendPortalCommand(EPortalCommand Command)
{

}

void USkyPortalSubsystem::SendPortalSound(USoundWave* Sound)
{

}



FPortalStatusChecker::FPortalStatusChecker(USkyPortalSubsystem* InSubsystem, float InCheckInterval)
	: SkyPortalSubsystem(InSubsystem), CheckInterval(InCheckInterval), bShouldRun(true)
{
}

bool FPortalStatusChecker::Init()
{
	// Initialization logic, if necessary (e.g., logging)
	return true;
}

uint32 FPortalStatusChecker::Run()
{
	// Main loop of the thread, runs until Stop() is called
	while (bShouldRun)
	{
		// Check the portal status
		CheckPortalStatus();

		// Sleep for the specified interval
		FPlatformProcess::Sleep(CheckInterval);
	}

	return 0;  // Exit code for the thread
}

void FPortalStatusChecker::Stop()
{
	// Signal the thread to stop running
	bShouldRun = false;
}

void FPortalStatusChecker::Exit()
{
	// Cleanup after the thread has stopped
}

void FPortalStatusChecker::CheckPortalStatus()
{
	// Ensure the subsystem is valid
	if (SkyPortalSubsystem && SkyPortalSubsystem->bPortalConnected)
	{
		UE_LOG(LogSkyportalIO, Verbose, TEXT("Check portal"));

		// Call the subsystem function to get portal status

		SkyPortalSubsystem->CheckComplexResponse();

		// Do something with the status (log, notify, etc.)
		UE_LOG(LogSkyportalIO, Verbose, TEXT("Portal Status:"));
	}
}

FigureDataBlock USkyPortalSubsystem::ReadFigureBlocks(uint8 FigureIndex)
{
	FigureDataBlock FigureData;
	FigureData.error = false;  // Initialize error flag

	RWBlock req, res;

	// Loop over all 64 blocks
	for (uint8 BlockIndex = 0; BlockIndex < FIGURE_TOTAL_BLOCKS; ++BlockIndex)
	{
		// Prepare the request buffer
		memset(req.buf, 0, rw_buf_size);
		req.buf[1] = 'Q';            // Command character
		req.buf[2] = FigureIndex;     // Figure index (0x00 to 0x0F)
		req.buf[3] = BlockIndex;      // Block index (0x00 to 0x3F)

		// Send the request
		Write(&req);

		// Read the response
		int BuffResponse = hid_read_timeout(PortalDevice, res.buf, rw_buf_size, TIMEOUT);

		// Check if the response was received successfully
		if (BuffResponse < rw_buf_size)
		{
			FigureData.error = true;  // Mark error flag
			UE_LOG(LogSkyportalIO, Warning, TEXT("Error receiving data for block %d of figure %d"), BlockIndex, FigureIndex);
			break;
		}

		// Parse the response status
		uint8 StatusByte = res.buf[2];
		uint8 ReturnedBlockIndex = res.buf[3];

		// Check for errors in the status byte
		if (StatusByte == 0x01)  // Error in response
		{
			FigureData.error = true;
			UE_LOG(LogSkyportalIO, Warning, TEXT("Error reading block %d for figure %d"), BlockIndex, FigureIndex);
			break;
		}
		else if (StatusByte != (0x10 + 'Q'))  // Unexpected status byte
		{
			FigureData.error = true;
			UE_LOG(LogSkyportalIO, Warning, TEXT("Unexpected status byte 0x%02X for block %d"), StatusByte, BlockIndex);
			break;
		}

		// Verify that the block index matches the requested block
		if (ReturnedBlockIndex != BlockIndex)
		{
			FigureData.error = true;
			UE_LOG(LogSkyportalIO, Warning, TEXT("Mismatched block index. Expected %d, got %d"), BlockIndex, ReturnedBlockIndex);
			break;
		}

		// Copy block data into FigureData
		memcpy(FigureData.blockdata[BlockIndex], &res.buf[4], FIGURE_BLOCK_SIZE);
	}

	return FigureData;  // Return the complete figure data
}