#include <LFramework/USB/Host/UsbHostManager.h>
#include <LFramework/USB/Host/UsbHDevice.h>
#include <LFramework/Debug.h>
#include <iostream>
#include <iomanip>

#include <thread>
#include <chrono>
#include <iomanip>

#pragma comment(lib, "User32")
#pragma comment(lib, "Advapi32")
#pragma comment(lib, "Ole32")
#pragma comment(lib, "Winusb")
#pragma comment(lib, "SetupAPI")

std::string findDevice(){
    auto devices = UsbHostManager::enumerateDevices();

    for(auto& device : devices){
        if(device.vid == 0x0301 && device.pid == 0x1111){
            return device.path;
        }
    }
    return {};
}


enum class UsbRequestType : uint8_t {
    PollBegin,
    Poll
};

struct UsbRequest {
    UsbRequestType type;
    uint8_t data;
};

struct UsbResponse {
    uint8_t data;
};


UsbResponse poll(LFramework::USB::UsbHEndpoint* rxEp, LFramework::USB::UsbHEndpoint* txEp, UsbRequest request) {
    auto txFuture = txEp->transferAsync(&request, sizeof(request));
    txFuture->wait();

    uint8_t rxBuffer[64];

    auto rxFuture = rxEp->transferAsync(rxBuffer, sizeof(rxBuffer));
    auto rxSize = rxFuture->wait();

    if(rxSize != sizeof(UsbResponse)){
        std::cout << "Invalid RX size" << std::endl;
        return {0xff};
    }else{
        return *((UsbResponse*)(&rxBuffer[0]));
    }

}
void readControllerState(LFramework::USB::UsbHEndpoint* rxEp, LFramework::USB::UsbHEndpoint* txEp) {

    if(poll(rxEp, txEp, {UsbRequestType::PollBegin, 0x01}).data != 0xff) {
        std::cout << "Unexpected response for PollBegin request" << std::endl;
    }

    auto controllerType = poll(rxEp, txEp, {UsbRequestType::Poll, 0x42}).data;
    auto controlValue = poll(rxEp, txEp, {UsbRequestType::Poll, 0x00}).data;

    std::cout << "Controller type: 0x" << std::hex <<  (int)controllerType << std::endl;


    if(controlValue != 0x5A){
        std::cout << "Unexpected value for Poll control " << std::endl;
    }

    auto payloadDataSize = (controllerType & 0xf) * 2;

    for(size_t i = 0; i < payloadDataSize; ++i){
        auto padData = poll(rxEp, txEp, {UsbRequestType::Poll, 0x00}).data;
    }
}



int main() {

    lfDebug() << "Test";
    auto devicePath = findDevice();

    if(!devicePath.empty()){
        std::cout << "Device found!" << std::endl;

        LFramework::USB::UsbHDevice device(devicePath);
        std::cout << "Device open!" << std::endl;

        auto bulkInterface = device.getInterface(0);
        auto txEp = bulkInterface->getEndpoint(false, 0);
        auto rxEp = bulkInterface->getEndpoint(true, 0);


        //reset connection
        auto txFuture = txEp->transferAsync(nullptr,  0);

        uint8_t rxBuffer[64];

        while(true){
            auto rxFuture = rxEp->transferAsync(rxBuffer, sizeof(rxBuffer));
            auto rxSize = rxFuture->wait();
            if(rxSize == 0){
                break;
            }
        }
        txFuture->wait();




        while(true){
            readControllerState(rxEp, txEp);
            //std::this_thread::sleep_for(std::chrono::milliseconds(500));
        }

    }else{
        std::cout << "Failed to find device" << std::endl;
    }

    return 0;
}