// 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"

DEFINE_LOG_CATEGORY(LogHIDApi);
DEFINE_LOG_CATEGORY(LogSkyportalIO);

void USkyPortalSubsystem::Initialize(FSubsystemCollectionBase& Collection)
{

	Super::Initialize(Collection);
	// Custom initialization logic
	UE_LOG(LogTemp, Log, TEXT("SkyPortalSubsystem Initialized"));

	// Initialize HIDAPI
	int res = hid_init();
	if (res == 0)
	{
		UE_LOG(LogHIDApi, Log, TEXT("HIDAPI initialized successfully."));

	}
	else
	{
		UE_LOG(LogHIDApi, Error, TEXT("Failed to initialize HIDAPI."));
		HidError = hid_error(NULL);
		UE_LOG(LogHIDApi, Error, TEXT("%s"), *HidError);
	}

}

void USkyPortalSubsystem::Deinitialize()
{
	// Disconnect portal
	if (PortalDevice) {
		hid_close(PortalDevice);
	}
	hid_exit();


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



/*
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::ConnectPortal() {
	/*
		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;
}

void USkyPortalSubsystem::ChangePortalColor(const FLinearColor& Color)
{

	unsigned char r = Color.R;
	unsigned char g = Color.G;
	unsigned char b = Color.B;

	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::Write(RWBlock* pb) {
	// TODO: Need to make this function async
	if (!ensure(PortalDevice)) {
		UE_LOG(LogSkyportalIO, Error, TEXT("No Portal found"));
		return;
	}

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

	/*
	for (int attempt = 0; attempt < 10; attempt++) {
		if (hid_write(PortalDevice, pb->buf, 0x21) == -1) {
			Sleep(100);
		}
		else {
			return;
		}
	}
	*/
	ensureMsgf(hid_write(PortalDevice, pb->buf, 0x21) != -1, TEXT("Unable to write to Portal, %s"), hid_error(PortalDevice));
	UE_LOG(LogSkyportalIO, Verbose, TEXT("Writed"));
}


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


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("Unable to read Skylander from Portal.\n"));
		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"));
	}

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


bool USkyPortalSubsystem::ReadBlock(unsigned int block, unsigned char data[0x10], int skylander) {
	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 + skylander;
		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;
}