#include <chrono>
#include "ui.h"
#include "hid_handler.h"
#include <setupapi.h>
#include <regex>
#include "utils.h"

///
HidHandler hid_handler;

///
void HidHandler::command_software_version()
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x2A; // Code.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_serial_number()
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x25; // Code.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_hmd_serial_number()
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x26; // Code.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_tracking_flex_serial_number()
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x7E; // Code.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_oled_serial_number()
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x5E; // Code.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_usage_timer(unsigned char usage_type)
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x5A; // Code.
    command += usage_type; // Usage type requested.
    // 0: Total powered on time.
    // 1: Display on time.
    // 2: Longest display on time.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_mic_stereo(unsigned char stereo)
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x53; // Code.
    command += stereo; // Mono/Stereo.
    // 0: Mono.
    // 1: Stereo.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_mic_integer_gain(unsigned short integer_gain)
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x47; // Code.
    command += (unsigned char)((integer_gain >> 8) & 0xFF); // MSBs of gain.
    command += (unsigned char)(integer_gain & 0xFF); // LSBs of gain.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_mic_fractional_gain(unsigned short fractional_gain)
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x67; // Code.
    command += (unsigned char)((fractional_gain >> 8) & 0xFF); // MSBs of gain.
    command += (unsigned char)(fractional_gain & 0xFF); // LSBs of gain.
    // Note: This is in 16-bit fixed point (Q15) format.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

// Instantly activates the fan at the provided speed
void HidHandler::command_fan_speed_immediate(unsigned char fan_speed)
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x46; // Code.
    command += fan_speed; // Speed.
    // 0-100 allowed as percentage.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

// Changes the fan speed but it will not activate the fan if it is not running
void HidHandler::command_fan_speed_deferred(unsigned char fan_speed)
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x66; // Code.
    command += fan_speed; // Speed.
    // 0-100 allowed as percentage.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_periodic_data_rate(unsigned short rate)
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x52; // Code.
    command += (unsigned char)((rate >> 8) & 0xFF); // MSBs of rate.
    command += (unsigned char)(rate & 0xFF); // LSBs of rate.
    // Rate is in milliseconds.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_oled_vertical_flip(unsigned char flip)
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x50; // Code.
    command += flip; // Flip or not.
    // 0: Do not flip, 1: Flip.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_oled_brightness(unsigned short brightness)
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x49; // Code.
    command += (unsigned char)((brightness >> 8) & 0xFF); // MSBs of brightness.
    command += (unsigned char)(brightness & 0xFF); // LSBs of brightness.
    // Values 0 to 1023 allowed.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

/// Offset proximity sensor sensitivites
void HidHandler::command_proximity_offset(uint16_t offset)
{
    std::string command;
    command += (unsigned char)0x00;
    command += (unsigned char)0x74;
    command += (unsigned char)(offset & 0xFF); // low byte
    command += (unsigned char)((offset >> 8) & 0xFF); // high byte
    
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_restart_in_bootloader()
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x42; // Code.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_fpga_switch()
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x65; // Code.
    command += (unsigned char)0x42; // FPGA action.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_rgb_led_color(unsigned char red,
    unsigned char green,
    unsigned char blue)
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x4C; // Code.
    command += red;
    command += green;
    command += blue;
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_read_config(unsigned char block)
{
    waiting_on_block.store(block);
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x55; // Code.
    command += block;
    // 16 block numbers, 0-15.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_write_config(unsigned char block,
    unsigned char data[32])
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x57; // Code.
    command += block;
    for (uint32_t b = 0; b < 32; ++b)
    {
        command += data[b];
    }
    // 16 block numbers, 0-15.
    // Data is 32 bytes of raw data.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_save_config()
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x56; // Code.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_proximity_settings(unsigned char pwen,
    unsigned char prate,
    unsigned char pwlong,
    unsigned char pgain,
    unsigned char ppulse,
    unsigned char plen,
    unsigned char pldrive,
    unsigned char pwtime,
    unsigned char pdselect,
    unsigned char pmavg,
    unsigned char proxavg)
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x4D; // Code.
    command += pwen; // Wait time disabled (0) or enabled (1).
    command += prate; // 0-255: duration of proximity event.
    command += pwlong; // Normal wait time (0) or long wait time (1).
    command += pgain; // Photodiode gain: 0-3.
    command += ppulse; // Max number of laser pulses: 0-63.
    command += plen; // Pulse length: 0-7.
    command += pldrive; // Laser diode current: 0-8.
    command += pwtime; // 0-255: Wait time between pulse trains.
    command += pdselect; // Photodiode selection: 1, 2, or 3.
    command += pmavg; // Moving average: 0-3.
    command += proxavg; // Hardware averaging: 0-7.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_vxr_checksum(unsigned int start_address,
    unsigned int length)
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x4B; // Code.
    command += start_address & 0xF;
    command += (start_address >> 8) & 0xF;
    command += (start_address >> 16) & 0xF;
    command += (start_address >> 24) & 0xF;
    command += length & 0xF;
    command += (length >> 8) & 0xF;
    command += (length >> 16) & 0xF;
    command += (length >> 24) & 0xF;
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_vxr_query_tags()
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x54; // Code.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_vxr_delete_block(unsigned char bank)
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x44; // Code.
    command += bank;
    // Deletes 64k block, 0-7.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_vxr_program(unsigned char length,
    unsigned int start_location,
    unsigned char* data)
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x41; // Code.
    command += length;
    command += start_location & 0xF;
    command += (start_location >> 8) & 0xF;
    command += (start_location >> 16) & 0xF;
    command += (start_location >> 24) & 0xF;
    for (unsigned int b = 0; b < length; ++b)
    {
        command += data[b];
    }
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_vxr_reset()
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x59; // Code.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_vxr_name()
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x4E; // Code.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_hardware_test(unsigned char test)
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x4A; // Code.
    command += test;
    // 'H' = USB Hub.
    // 'L' = RGB LED.
    // 'S' = USB-C Switch.
    // 'V' = VXR7200.
    // 'P' = Proximity Sensor.
    // 'O' = OLED.
    // 'F' = Fan.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_enter_fatp()
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x40; // Code.
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

void HidHandler::command_edid_switch(unsigned char which_edid)
{
    std::string command;
    command += (unsigned char)0x00;
    command += (unsigned char)0x64; // Code for EDID switch, 'd'
    command += which_edid; // 0: both 75 and 90, 1: 90 only, 2: 75 only

    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

///
void HidHandler::command_direct_oled(unsigned char which_oled,
    unsigned char length,
    unsigned char* data)
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x6F; // Code.
    command += which_oled; // 0 = left, 1 = right.
    command += length;
    for (unsigned int b = 0; b < length; ++b)
    {
        command += data[b];
    }
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

void HidHandler::blcommand_restart()
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x42; // Code to restart into application
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

void HidHandler::blcommand_software_version()
{
    if (!bl_request_software_version.load())
    {
        std::string command;
        command += (unsigned char)0x00; // Report ID.
        command += (unsigned char)0x2A; // Code to retrieve software version
        std::lock_guard<std::mutex> command_lock(command_mutex);
        bl_request_software_version.store(true);
        pending_commands.push_back(command);
    }
}

void HidHandler::blcommand_write_data(uint32_t mem_address, unsigned char length, unsigned char* data)
{
    uint8_t command[64];
    command[0] = (unsigned char)0x44; // Code to write Flash data
    command[1] = length;
    command[2] = (unsigned char)(mem_address & 0xFF);
    command[3] = (unsigned char)((mem_address & 0xFF00) >> 8);
    command[4] = (unsigned char)((mem_address & 0xFF0000) >> 16);
    command[5] = (unsigned char)((mem_address & 0xFF000000) >> 24);
    memcpy(&(command[6]), data, length);

    /*
    * Doing this way appears to corrupt the data.
    * Unsigned / signed conversion?
    for (int i = 0; i < length; i++)
    {
        command[6 + i] += data[i];
    }
    */

    command[6 + length] = crc8_bootloader(command, 6 + length);

    std::string cmdout((char*)command, 7 + length);
    cmdout.insert(cmdout.begin(), 0);

    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(cmdout);
}

void HidHandler::blcommand_program_data(uint32_t mem_address)
{
    uint8_t command[7];
    command[0] = (unsigned char)0x50; // Code to program Flash data
    command[1] = 0;
    command[2] = (unsigned char)(mem_address & 0xFF);
    command[3] = (unsigned char)((mem_address & 0xFF00) >> 8);
    command[4] = (unsigned char)((mem_address & 0xFF0000) >> 16);
    command[5] = (unsigned char)((mem_address & 0xFF000000) >> 24);
    command[6] = crc8_bootloader(command, 6);

    std::string cmdout((char*)command, 7);
    cmdout.insert(cmdout.begin(), 0);

    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(cmdout);
}

void HidHandler::blcommand_erase_app()
{
    std::string command;
    command += (unsigned char)0x00; // Report ID.
    command += (unsigned char)0x22; // Code to erase application region in Flash
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

void HidHandler::blcommand_erase_flash(unsigned char erase_size_code, uint32_t mem_address)
{
    uint8_t command[7];
    command[0] = (unsigned char)0x65; // Code to erase Flash by block or sector
    command[1] = erase_size_code;
    command[2] = (unsigned char)(mem_address & 0xFF);
    command[3] = (unsigned char)((mem_address & 0xFF00) >> 8);
    command[4] = (unsigned char)((mem_address & 0xFF0000) >> 16);
    command[5] = (unsigned char)((mem_address & 0xFF000000) >> 24);
    command[6] = crc8_bootloader(command, 6);

    std::string cmdout((char*)command, 7);
    cmdout.insert(cmdout.begin(), 0);

    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(cmdout);
}

void HidHandler::cdcommand_get_crash_dump()
{
    switch (crashdump_status)
    {
    case Crash_Dump_State::Begin:
        cdcommand_software_version();
        break;
    case Crash_Dump_State::Got_SWVer:
        cdcommand_get_reason();
        break;
    case Crash_Dump_State::Got_Reason:
        cdcommand_get_processor_state(0);
        break;
    case Crash_Dump_State::Got_Proc1:
        cdcommand_get_processor_state(1);
        break;
    case Crash_Dump_State::Got_Proc2:
        cdcommand_get_processor_state(2);
        break;
    case Crash_Dump_State::Got_Proc3:
        cdcommand_get_processor_state(3);
        break;
    case Crash_Dump_State::Got_Proc4:
        cdcommand_get_task_name();
        break;
    case Crash_Dump_State::Got_Name:
        cdcommand_get_num_mem_regions();
        break;
    case Crash_Dump_State::Got_Num_Mem_Regions:
        // Need to get each of the regions, uses a state variable 
        // to tell us which one to request
        // The "current_mem_region_number" state variable will 
        // be auto-incremented after region info is received
        cdcommand_get_mem_region_info(current_mem_region_number.load());
        break;
    case Crash_Dump_State::Got_Mem_Region_Info:
        // Start collecting memory data
        {
            uint32_t len_request;
            uint32_t end_addr = std::get<2>(cdinfo.mem_info[current_mem_region_number.load()]);
            // Check if we need to move to the next memory region
            if (current_mem_addr.load() >= end_addr)
            {
                current_mem_region_number.store(current_mem_region_number.load() + 1);
                if (current_mem_region_number.load() >= cdinfo.num_mem_regions)
                {
                    // We have finished!
                    crashdump_status.store(Crash_Dump_State::Done);
                    break;
                }
                else
                {
                    current_mem_addr.store(std::get<1>(cdinfo.mem_info[current_mem_region_number.load()]));
                    end_addr = std::get<2>(cdinfo.mem_info[current_mem_region_number.load()]);
                }
            }

            if ((end_addr - current_mem_addr.load()) > 60)
            {
                len_request = 60;
            }
            else
            {
                len_request = end_addr - current_mem_addr.load();
            }
            cdcommand_get_memory_dump(current_mem_addr.load(), len_request, 0);
        }
        break;
    case Crash_Dump_State::Done:
        break;
    default:
        // Do nothing
        break;
    }
}

void HidHandler::cdcommand_software_version()
{
    // It's completely identical to the regular application's version
    // of this command
    command_software_version();
}

void HidHandler::cdcommand_get_reason() 
{
    std::string command;
    command += (char)0; // Report ID (always zero)
    command += (char)0x52; // 'R', CRASH_CMD_GET_REASON
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

void HidHandler::cdcommand_get_processor_state(uint32_t which_part) 
{
    std::string command;
    command += (char)0; // Report ID (always zero)
    command += (char)0x53; // 'S', CRASH_CMD_GET_PROCESSOR_STATE
    command += (char)which_part; // 0 to 3 for the groups of processor registers
    // Group 0 (15 registers): R0 through R14 
    // Group 1 (15 registers): R15, PSR, MSP, PSP, Exception PSR, and S0-S9
    // Group 2 (15 registers): S10-S24
    // Group 3 (8 registers): S25-S31, FPSCR
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

void HidHandler::cdcommand_get_task_name() 
{
    std::string command;
    command += (char)0; // Report ID (always zero)
    command += (char)0x54; // 'T', CRASH_CMD_GET_TASK_NAME
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

void HidHandler::cdcommand_get_num_mem_regions()
{
    std::string command;
    command += (char)0; // Report ID (always zero)
    command += (char)0x47; // 'G', CRASH_CMD_GET_NUM_MEMORY_REGIONS
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

void HidHandler::cdcommand_get_mem_region_info(uint32_t which_region)
{
    std::string command;
    command += (char)0; // Report ID (always zero)
    command += (char)0x49; // 'I', CRASH_CMD_GET_MEMORY_REGION_INFO
    command += (char)which_region;
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

void HidHandler::cdcommand_get_memory_dump(uint32_t startaddr, uint32_t length, uint32_t repeat_count)
{
    std::string command;
    command += (char)0; // Report ID (always zero)
    command += (char)0x4D; // 'M', CRASH_CMD_GET_MEMORY
    command += (char)length;
    command += (char)repeat_count;
    command += (char)0; // intentional blank spot
    command += (char)(startaddr & 0xFF);
    command += (char)((startaddr & 0xFF00) >> 8);
    command += (char)((startaddr & 0xFF0000) >> 16);
    command += (char)((startaddr & 0xFF000000) >> 24);
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

void HidHandler::cdcommand_restart()
{
    std::string command;
    command += (char)0; // Report ID (always zero)
    command += (char)0x43; // 'C', CRASH_CMD_RESTART
    std::lock_guard<std::mutex> command_lock(command_mutex);
    pending_commands.push_back(command);
}

// Verifies the raw_config is a valid config region:
// - all tags have valid CRC
// - ends with an 0xFF, or in the rare case config is 
//   exactly 512 bytes long, a CRC is the final byte
bool HidHandler::verify_config()
{
    uint32_t iter_b = 0;
    // Parse raw config previously read from Beyond
    for (; iter_b < CONFIG_DATA_LENGTH; iter_b++)
    {
        unsigned char tag = raw_config[iter_b];
        if (tag == Config_Tags::Invalid)
        {
            return true; // We reached the end of all valid tags, and with no errors!
        }
        unsigned char length;
        if ((iter_b + 1) < CONFIG_DATA_LENGTH)
        {
            length = raw_config[iter_b + 1];
        }
        else
        {
            return false; // Length would have been past the end of config region
        }
        unsigned char stored_crc;
        if ((iter_b + 2 + length) < CONFIG_DATA_LENGTH)
        {
            stored_crc = raw_config[iter_b + 2 + length];
        }
        else
        {
            return false; // CRC would have been past the end
        }

        unsigned char calc_crc = crc8(&(raw_config[iter_b]), 2 + length);
        if (stored_crc != calc_crc)
            return false; // CRC error
        iter_b += 2 + length; // this and the iter_b++ in the for loop will take us past the CRC byte
    }
    // Managed to reach the end of the config region
    // If we are exactly at CONFIG_DATA_LENGTH, then it is still a valid config region
    // But if iter_b went past that value, something was wrong
    return (iter_b == CONFIG_DATA_LENGTH);
}

void HidHandler::parse_config()
{
    // Start new config tag/data tuples
    config.clear();
    // Parse raw config previously read from Beyond
    for (uint32_t iter_b = 0; iter_b < CONFIG_DATA_LENGTH; iter_b++)
    {
        unsigned char tag = raw_config[iter_b];
        unsigned char length = (iter_b < 511) ? raw_config[iter_b + 1] : 0;
        if (tag == Config_Tags::Invalid)
        {
            break; // We reached end of all valid tags
        }
        std::string data;
        for (uint32_t iter_c = 0; iter_c < length; iter_c++)
        {
            if ((iter_b + iter_c + 2) < CONFIG_DATA_LENGTH)
                data += raw_config[iter_b + iter_c + 2]; // the '2' moves us past the tag and length bytes
        }
        auto entry = std::make_tuple(tag, data);
        // Eliminate any duplicates that were in the raw_config
        bool duplicate = false;
        for (uint32_t iter_t = 0; iter_t < config.size(); iter_t++)
        {
            if (std::get<0>(entry) == std::get<0>(config[iter_t]))
            {
                duplicate = true;
                break;
            }
        }
        if (!duplicate)
        {
            config.push_back(entry);
        }
        iter_b += 2 + length; // this and the iter_b++ in the for loop will take us past the CRC byte
    }
}

// Adds all new entries passed as an argument to the Beyond's config
// If any of the new_entries are already in the current config, the 
// values are overwritten with the new values.
// Requires the "raw_config" to have already been loaded by 
// using command_read_config for all 16 blocks
void HidHandler::save_config(std::vector<Config_T> new_entries)
{
    // Reads through and extracts data values from raw config bytes (must have
    // been previously read from HMD)
    parse_config();

    // Add our new entries to the set of tag/data tuples
    auto config_length = config.size();
    for (auto& entry : new_entries)
    {
        bool already_in_config = false;
        for (uint32_t iter_d = 0; iter_d < config_length; iter_d++)
        {
            if (std::get<0>(entry) == std::get<0>(config[iter_d]))
            {
                already_in_config = true;
                config.at(iter_d) = entry; // replace the old entry with this one
            }
        }
        if (!already_in_config)
        {
            config.push_back(entry);
        }
    }

    // Write out the new raw_config and save it to the Beyond
    memset(raw_config, Config_Tags::Invalid, CONFIG_DATA_LENGTH);

    uint32_t cursor = 0;
    for (auto& c : config)
    {
        unsigned char tag = std::get<0>(c);
        std::string data = std::get<1>(c);
        raw_config[cursor] = tag;
        raw_config[cursor + 1] = data.length();
        for (uint32_t iter_e = 0; iter_e < data.length(); iter_e++)
        {
            raw_config[cursor + 2 + iter_e] = data.at(iter_e);
        }
        raw_config[cursor + 2 + data.length()] = crc8(&(raw_config[cursor]), 2 + data.length());
        cursor += 3 + data.length();

        if (cursor >= CONFIG_DATA_LENGTH) [[unlikely]]
            return; // don't know how to handle too long of a config. shouldn't happen though.
    }

    for (uint32_t block = 0; block < 16; ++block)
    {
        command_write_config(block, &raw_config[block * 32]);
    }
    command_save_config();
}

static inline uint16_t buffer_to_u16(unsigned char* buf)
{
    return ((uint32_t)buf[0]) + (((uint32_t)buf[1]) << 8);
}

static inline uint32_t buffer_to_u32(unsigned char* buf)
{
    return ((uint32_t)buf[0]) + (((uint32_t)buf[1]) << 8) + (((uint32_t)buf[2]) << 16) + (((uint32_t)buf[3]) << 24);
}

///
void HidHandler::run()
{
    thread = std::thread([&]()
        {
            bl_request_software_version.store(false);

            hid_init();

            device = hid_open(0x35bd, 0x0101, NULL);
            if (!device)
            {
                device = hid_open(0x35bd, 0x4004, NULL); // attempt to open the bootloader
                if (!device)
                {
                    device = hid_open(0x35bd, 0x1001, NULL); // attempt to open the crash handler
                    if (!device)
                    {
                        awaiting_device.store(true);
                        device_state.store(Beyond_Device_State::Disconnected);
                    }
                    else
                    {
                        awaiting_device.store(false);
                        device_state.store(Beyond_Device_State::CrashHandler);
                        hid_set_nonblocking(device, true); // Don't block so thread stays loose.
                    }
                }
                else
                {
                    awaiting_device.store(false);
                    device_state.store(Beyond_Device_State::Bootloader);
                    hid_set_nonblocking(device, true); // Don't block so thread stays loose.
                }
            }
            else
            {
                awaiting_device.store(false);
                device_state.store(Beyond_Device_State::Application);
                hid_set_nonblocking(device, true); // Don't block so thread stays loose.
            }

            auto [exists, driverKey] = HidHandler::in_dfu();
            if (exists)
            {
                device_state.store(Beyond_Device_State::EyetrackingDFU);
            }
            // Let other threads know we're connected and ready for commands.
            ready.store(true);

            // This will allow us to identify the block of config memory we requested.
            waiting_on_block.store(0);

            /* RESET
            unsigned char new_config[512] = { 1,
                                              16,
                                              88,
                                              67,
                                              78,
                                              76,
                                              49,
                                              68,
                                              50,
                                              50,
                                              67,
                                              57,
                                              48,
                                              48,
                                              48,
                                              51,
                                              49,
                                              54,
                                              96,
                                              6,
                                              2,
                                              26,
                                              6,
                                              180,
                                              11,
                                              2,
                                              158,
                                              2,
                                              180,
                                              255, 255};
            for (uint32_t block = 0; block < 16; ++block) {
              command_write_config(block, &new_config[block * 32]);
            }
            command_save_config(); */

            last_usage_timer_request = 0;
            usage_timer_status = Usage_Timer_State::Done;


            unsigned char buffer[64]; // Reports are always 64 bytes long.
            while (ui.alive.load())
            {
                uint64_t now = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();

                // Handle outgoing data.
                if (device)
                {
                    std::lock_guard<std::mutex> command_lock(command_mutex);
                    for (uint32_t c = 0; c < pending_commands.size(); ++c)
                    {
                        if (Beyond_Device_State::Bootloader == device_state.load())
                        {
                            // Bootloader uses regular report, application and crash handler use feature report.
                            hid_write(device, (const unsigned char*)pending_commands[c].c_str(), pending_commands[c].size());
                        }
                        else
                        {
                            hid_send_feature_report(device, (const unsigned char*)pending_commands[c].c_str(), pending_commands[c].size());
                        }
                    }
                    pending_commands.clear();
                }

                if (awaiting_device.load() || entering_dfu.load())
                {
                    Sleep(1000); // Don't clog the core.
                    if (!device && (!ui.app_inactive.load() || now - last_device_check_request >= 6))
                    {
                        last_device_check_request = now;
                        device = hid_open(0x35bd, 0x0101, NULL);
                        if (!device)
                        {
                            device = hid_open(0x35bd, 0x4004, NULL); // attempt to open the bootloader
                            if (!device)
                            {
                                device = hid_open(0x35bd, 0x1001, NULL); // attempt to open the crash handler
                                if (!device)
                                {
                                    awaiting_device.store(true);
                                    device_state.store(Beyond_Device_State::Disconnected);
                                }
                                else
                                {
                                    awaiting_device.store(false);
                                    device_state.store(Beyond_Device_State::CrashHandler);
                                    hid_set_nonblocking(device, true); // Don't block so thread stays loose.
                                }
                            }
                            else
                            {
                                awaiting_device.store(false);
                                device_state.store(Beyond_Device_State::Bootloader);
                                hid_set_nonblocking(device, true); // Don't block so thread stays loose.
                            }
                        }
                        else
                        {
                            awaiting_device.store(false);
                            device_state.store(Beyond_Device_State::Application);
                            hid_set_nonblocking(device, true); // Don't block so thread stays loose.

                            // Device state can be in application before DFU command exits.
                            // Anytime DFU mode is leaving, "leaving_dfu" should be enabled.
                            if (!leaving_dfu.load())
                            {
                                auto [exists, driverKey] = HidHandler::in_dfu();
                                if (exists)
                                {
                                    device_state.store(Beyond_Device_State::EyetrackingDFU);
                                }
                            }
                            leaving_dfu.store(false);
                        }
                    }

                    // DFU command was sent. Keep checking for the DFU device
                    if (entering_dfu.load())
                    {
                        auto [exists, driverKey] = HidHandler::in_dfu();
                        if (exists && entering_dfu.load())
                        {
                            entering_dfu.store(false);
                            device_state.store(Beyond_Device_State::EyetrackingDFU);
                        }
                    }
                    continue;
                }

                // Perform camera uninstall and reboot from the flagged launch argument
                if (startup_camera_restart.load() && Beyond_Device_State::Application == device_state.load())
                {
                    uninstall_camera("VID_35BD", "PID_0202");

                    Sleep(2000);
                    cdcommand_restart();
                    startup_camera_restart.store(false);
                    continue;
                }

                // Periodic operations.
                // Get the usage stats
                if (now - last_usage_timer_request >= 5)
                {
                    if (Beyond_Device_State::Application == device_state.load())
                    {
                        if (usage_timer_status == Usage_Timer_State::Done && !ui.app_inactive.load())
                        {
                            usage_timer_status = Usage_Timer_State::Get_Total_Powered_Time;
                            command_usage_timer(0);
                        }
                        else
                        {
                            // Wait 5 more seconds and try again
                            usage_timer_status = Usage_Timer_State::Done;
                            // log << "USAGE TIMER response timed out\n";
                        }
                    }

                    last_usage_timer_request = now;
                }

                if (now - last_info_log >= 15)
                {
                    pending_log.store(true);
                    last_info_log = now;
                }

                // Periodically attempt to retrieve the lighthouse serial from the usb tree
                if (now - last_lhr_serial_request >= 5 && !utils.get_device_type().empty() && tundra_serial.empty())
                {
                    if (Beyond_Device_State::Application == device_state.load())
                    {
                        std::string s = get_lighthouse_serial();

                        if (s.find("LHR") != std::string::npos)
                        {
                            tundra_serial = s;
                        }
                    }

                    last_lhr_serial_request = now;
                }

                // Verify the eyetracking camera device exists
                if (now - last_camera_check_request >= 4)
                {
                    int cam_check_max_attempts = 3;

                    if (et_camera_state == Beyond_Camera_State::Unknown && et_camera_check_attempt < cam_check_max_attempts && ui.deviceType == "BS2E" && Beyond_Device_State::Application == device_state.load())
                    {
                        auto [exists, response] = get_device("VID_35BD", "PID_0202", L"Bigeye");

                        if (!exists && response.empty())
                        {
                            et_camera_check_attempt++;

                            if (et_camera_check_attempt >= cam_check_max_attempts)
                            {
                                et_camera_state = Beyond_Camera_State::NotFound;
                            }
                        }
                        
                        if (exists)
                        {
                            et_camera_state = Beyond_Camera_State::Valid;
                        }     
                    }

                    last_camera_check_request = now;
                }

                // Handle incoming data.
                int32_t bytes_read = device ? hid_read(device, buffer, 64) : 0;
                if (bytes_read < 0)
                {
                    awaiting_device.store(true);
                    firmware_version = "";
                    blfirmware_version = "";
                    hmd_serial = "";
                    tundra_serial = "";
                    device = NULL;
                    continue;
                }
                if (bytes_read > 0)
                {
                    if (Beyond_Device_State::Bootloader == device_state.load())
                    {
                        if (bl_request_software_version.load())
                        {
                            bl_request_software_version.store(false);
                            blfirmware_version = (char*)(&buffer[0]); // Buffer is null-terminated.
                        }
                        else
                        {
                            bl_got_status.store(true);
                            bl_status.store(buffer[0]);
                        }
                    }
                    else if (Beyond_Device_State::CrashHandler == device_state.load())
                    {
                        switch (buffer[0])
                        {
                        case 0x2A: // '*', software version
                            // same as the normal application
                            cdinfo.swver = (char*)(&buffer[1]); // Buffer is null-terminated.
                            crashdump_status.store(Crash_Dump_State::Got_SWVer);
                            break;
                        case 0x52: // 'R', crash reason
                            cdinfo.crash_reason = static_cast<Beyond_Crash_Reason>(buffer[1]);
                            cdinfo.crash_irq_num = buffer_to_u16(&(buffer[2]));
                            cdinfo.hfsr = buffer_to_u32(&(buffer[4]));
                            cdinfo.cfsr = buffer_to_u32(&(buffer[8]));
                            crashdump_status.store(Crash_Dump_State::Got_Reason);
                            break;
                        case 0x53: // 'S', processor state
                            if (buffer[1] == 0)
                            {
                                // Filling part 1 of the processor state
                                // Registers R0-R14
                                for (int i = 0; i <= 14; i++)
                                {
                                    cdinfo.regs[i] = buffer_to_u32(&(buffer[4 + 4 * i]));
                                }
                                crashdump_status.store(Crash_Dump_State::Got_Proc1);
                            }
                            if (buffer[1] == 1)
                            {
                                // Filling part 2 of the processor state
                                // Register R15 
                                // PSR, MSP, PSP, and exception PSR
                                // Floating point registers S0-S9
                                cdinfo.regs[15] = buffer_to_u32(&(buffer[4]));
                                cdinfo.psr = buffer_to_u32(&(buffer[8]));
                                cdinfo.msp = buffer_to_u32(&(buffer[12]));
                                cdinfo.psp = buffer_to_u32(&(buffer[16]));
                                cdinfo.exception_psr = buffer_to_u32(&(buffer[20]));

                                for (int i = 0; i <= 9 ; i++)
                                {
                                    cdinfo.floats[i] = buffer_to_u32(&(buffer[24 + 4 * i]));
                                }
                                crashdump_status.store(Crash_Dump_State::Got_Proc2);
                            }
                            if (buffer[1] == 2)
                            {
                                // Filling part 3 of the processor state
                                // Floating point registers S10-S24
                                for (int i = 0; i <= 14; i++)
                                {
                                    cdinfo.floats[i + 10] = buffer_to_u32(&(buffer[4 + 4 * i]));
                                }
                                crashdump_status.store(Crash_Dump_State::Got_Proc3);
                            }
                            if (buffer[1] == 3)
                            {
                                // Filling part 4 of the processor state
                                // Floating point registers S25-S31 and FPSCR (floating point status)
                                for (int i = 0; i <= 6; i++)
                                {
                                    cdinfo.floats[i + 25] = buffer_to_u32(&(buffer[4 + 4 * i]));
                                }
                                cdinfo.fpscr = buffer_to_u32(&(buffer[32]));
                                crashdump_status.store(Crash_Dump_State::Got_Proc4);
                            }
                            break;
                        case 0x54: // 'T', crashing task name
                            cdinfo.crashed_task_name = std::string((char*)(&buffer[2]), buffer[1]);
                            crashdump_status.store(Crash_Dump_State::Got_Name);
                            break;
                        case 0x47: // 'G', get number of memory regions
                            cdinfo.num_mem_regions = buffer[1];
                            cdinfo.mem_info.reserve(cdinfo.num_mem_regions);
                            cdinfo.mem_dump.reserve(cdinfo.num_mem_regions);
                            current_mem_region_number.store(0); // we will be requesting the 1st region info next
                            crashdump_status.store(Crash_Dump_State::Got_Num_Mem_Regions);
                            break;
                        case 0x49: // 'I', memory region info
                            // Contains start address, end address, and name of region
                            cdinfo.mem_info.push_back(
                                std::make_tuple(std::string((char*)&(buffer[12])), buffer_to_u32(&(buffer[4])), buffer_to_u32(&(buffer[8]))));
                            // Allocate space for the memory dump
                            // Use the difference between the memory addresses for the size to preallocate
                            cdinfo.mem_dump.push_back(new uint8_t[std::get<2>(cdinfo.mem_info[buffer[1]]) - std::get<1>(cdinfo.mem_info[buffer[1]])]);

                            // check if we have collected all the regions, 
                            // otherwise increment the requested region number
                            if ((current_mem_region_number.load() + 1) >= cdinfo.num_mem_regions)
                            {
                                current_mem_region_number.store(0);
                                total_mem_dumped.store(0);
                                current_mem_addr.store(std::get<1>(cdinfo.mem_info[current_mem_region_number.load()])); // set to the start of the first region
                                crashdump_status.store(Crash_Dump_State::Got_Mem_Region_Info);
                            }
                            else
                            {
                                current_mem_region_number.store(current_mem_region_number.load() + 1);
                            }
                            break;
                        case 0x4D: // 'M', get memory dump
                            // Figure out where this should be placed
                            // We had requested "current_mem_addr" in "current_mem_region_number"
                            // placement location is then current_mem_addr - region.start_addr
                            {
                                uint32_t start_addr = std::get<1>(cdinfo.mem_info[current_mem_region_number.load()]);
                                uint32_t array_addr = current_mem_addr.load() - start_addr;
                                uint8_t len = buffer[1];
                                memcpy(&(cdinfo.mem_dump[current_mem_region_number.load()][array_addr]),
                                    &(buffer[4]),
                                    len);
                                current_mem_addr.store(current_mem_addr.load() + len);
                                total_mem_dumped.store(total_mem_dumped.load() + len);
                            }

                            break;
                        }
                        hid_handler.cdcommand_get_crash_dump(); // Moves to the next item in the crash dump state
                        // if there are no more data packets to read, then it won't send any request
                    }
                    else
                    {
                        switch (buffer[0])
                        {

                            //** Periodic DATA received.
                        case 0x23:
                        {
                            if (pending_log.load())
                            {
                                unsigned char code = buffer[0];
                                unsigned char data_length = buffer[1];
                                unsigned short fan_speed = ((buffer[2] << 8) | buffer[3]);
                                unsigned short proximity_distance = ((buffer[4] << 8) | buffer[5]);
                                unsigned short cc1_pin = ((buffer[6] << 8) | buffer[7]);
                                unsigned short cc2_pin = ((buffer[8] << 8) | buffer[9]);
                                float pc_temp = *(float*)(&buffer[10]); // Kelvin
                                pc_temp = pc_temp - 273.15f; // Celsius
                                float left_temp = *(float*)(&buffer[14]); // Celsius
                                float right_temp = *(float*)(&buffer[18]); // Celsius
                                time_t now = time(0);
                                struct tm tstruct;
                                char buf[80];
                                tstruct = *localtime(&now);
                                strftime(buf, sizeof(buf), "%Y-%m-%d %X", &tstruct);
                                log << "\"SystemTime\":" << buf << ",";
                                log << "\"FanDuty\":" << fan_speed << ",";
                                log << "\"MainboardT\":" << pc_temp << ",";
                                log << "\"DisplayLeftT\":" << left_temp << ",";
                                log << "\"DisplayRightT\":" << right_temp;
                                log << "\n";
                                log.flush();

                                pending_log.store(false);
                            }

                            if (ui.finding_fan_gen.load())
                            {
                                unsigned short fan_speed = ((buffer[2] << 8) | buffer[3]);
                                log << "Collecting RPM sample: " << fan_speed << std::endl;
                                rpm_samples.push_back(fan_speed);
                            }
                            break;
                        }

                        //** SUCCESS response.
                        case 0x24:
                        {
                            //  log << "SUCCESS response.\n";
                            break;
                        }

                        //** FAIL response.
                        case 0x45:
                        {
                            //  log << "FAIL response.\n";
                            break;
                        }

                        //** Software version response.
                        case 0x2A:
                        {
                            firmware_version = (char*)(&buffer[1]); // Buffer is null-terminated.
                            break;
                        }

                        //** Serial number response.
                        case 0x25:
                        {
                            std::string version = (char*)(&buffer[1]); // Buffer is null-terminated.
                            //  log << "SERIAL NUMBER response: " << version.c_str() << "\n";
                            break;
                        }

                        //** HMD serial number response.
                        case 0x26:
                        {
                            hmd_serial = (char*)(&buffer[1]); // Buffer is null-terminated.
                            // log << "HMD SERIAL response: " << version.c_str() << "\n";
                            break;
                        }

                        //** Tracking flex serial number response.
                        case 0x7E:
                        {
                            std::string version = (char*)(&buffer[1]); // Buffer is null-terminated.
                            //  log << "TRACKING FLEX response: " << version.c_str() << "\n";
                            break;
                        }

                        //** OLED serial number response.
                        case 0x5E:
                        {
                            std::string version = (char*)(&buffer[1]); // Buffer is null-terminated.
                            //  log << "OLED SERIAL response: " << version.c_str() << "\n";
                            break;
                        }

                        //** Usage timer response.
                        case 0x5A:
                        {
                            if (!ui.app_inactive.load())
                            {
                                int64_t usage = ((int64_t)buffer[1]) + ((int64_t)buffer[2] << 8) + ((int64_t)buffer[3] << 16) + ((int64_t)buffer[4] << 24);
                                if (usage_timer_status == Usage_Timer_State::Get_Total_Powered_Time)
                                {
                                    total_power_on = usage * 10;
                                    command_usage_timer(0x01);
                                    usage_timer_status = Usage_Timer_State::Get_Display_Time;
                                }
                                else if (usage_timer_status == Usage_Timer_State::Get_Display_Time)
                                {
                                    displays_on = usage * 10;
                                    command_usage_timer(0x02);
                                    usage_timer_status = Usage_Timer_State::Get_Longest_Continuous_Time;
                                }
                                else if (usage_timer_status == Usage_Timer_State::Get_Longest_Continuous_Time)
                                {
                                    longest_displays_on = usage * 10;
                                    usage_timer_status = Usage_Timer_State::Done;
                                }
                                //log << "USAGE TIMER response: " << usage << " seconds\n";
                            }
                            break;
                        }

                        //** Signature/config data response.
                        case 0x55:
                        {
                            // Outside processes could call this hid command.
                            // Only read if we're intentionally fetching
                            if (ui.fetching_config.load())
                            {
                                unsigned char block = waiting_on_block.load();
                                log << "SIGNATURE response for block: " << block << "\n";
                                memcpy(&raw_config[block * 32], &buffer[2], 32);
                                /*
                                for (uint32_t b = 0; b < 32; ++b) {
                                  log << raw_config[block * 32 + b] << "\n";
                                }
                                log.flush();*/
                                waiting_on_block.store(0xFF);
                            }

                            break;
                        }

                        //** VXR checksum response.
                        case 0x4b:
                        {
                            unsigned int checksum = *((unsigned int*)(&buffer[2]));
                            //  log << "VXR CHECKSUM response.\n";
                            break;
                        }

                        //** VXR tags response.
                        case 0x54:
                        {
                            unsigned char tags = buffer[1];
                            //  log << "VXR TAGS response.\n";
                            break;
                        }

                        //** VXR firmware name response.
                        case 0x4E:
                        {
                            std::string name = (char*)(&buffer[1]);
                            //  log << "VXR FIRMWARE NAME response.\n";
                            break;
                        }

                        //** HW self-test response.
                        case 0x4A:
                        {
                            unsigned char passed = buffer[1]; // 0 = failed, 1 = passed.
                            //  log << "HW SELF-TEST response.\n";
                            break;
                        }
                        }
                    }
                }
                Sleep(ui.app_inactive.load() ? 1000 : 1); // Don't clog the core.
            }
            log.close();
        });
}

std::pair<bool, std::string> HidHandler::in_dfu()
{
    return HidHandler::get_device("VID_35BD", "PID_0282", L"");
}

std::wstring get_device_property(HDEVINFO info, SP_DEVINFO_DATA& dev, DWORD property)
{
    WCHAR buffer[1024];
    DWORD requiredSize = 0;
    if (SetupDiGetDeviceRegistryPropertyW(info, &dev, property, nullptr, (PBYTE)buffer, sizeof(buffer), &requiredSize))
    {
        return buffer;
    }

    DWORD err = GetLastError();
    hid_handler.log << L"Failed to get device property: " << property << L" Error: " << err << std::endl;
    return L"";
}

std::wstring trim_usb_path(const std::wstring& locPath, int levels)
{
    if (locPath.empty())
    {
        return L"";
    }

    size_t cut = locPath.size();
    for (int i = 0; i < levels; i++)
    {
        cut = locPath.find_last_of(L'#', cut - 1);
        if (cut == std::wstring::npos)
        {
            return locPath.substr(0, locPath.find_last_of(L'#'));
        }
    }

    return locPath.substr(0, cut);
}

std::string HidHandler::get_lighthouse_serial()
{
    log << L"Fetching lighthouse serial." << std::endl;
    HDEVINFO deviceInfoSet = SetupDiGetClassDevsW(nullptr, L"USB", nullptr, DIGCF_PRESENT | DIGCF_ALLCLASSES);
    if (deviceInfoSet == INVALID_HANDLE_VALUE)
    {
        log << L"Failed to get device info list. (0x1)" << std::endl;
        return "";
    }

    SP_DEVINFO_DATA devInfo = { sizeof(SP_DEVINFO_DATA) };
    std::wstring hubRootLoc;
    std::wstring hmdHardwareId = L"VID_35BD&PID_0101";
    std::wstring tundraHardwareId = L"VID_28DE&PID_2300";
    std::wstring hostDeviceClass = L"36fc9e60-c465-11cf-8056-444553540000";

    std::string deviceType = utils.get_device_type();

    // Beyond models can have variation in USB tree layout
    if (deviceType.empty())
    {
        return "";
    }

    int hubRootLevels = deviceType == "BS1" ? 1 : 2;

    // Find the first host device in USB tree with vid 35BD & pid 0101
    for (DWORD i = 0; SetupDiEnumDeviceInfo(deviceInfoSet, i, &devInfo); i++)
    {
        std::wstring hardwareId = get_device_property(deviceInfoSet, devInfo, SPDRP_HARDWAREID);
        std::wstring classGuid = get_device_property(deviceInfoSet, devInfo, SPDRP_CLASSGUID);

        // Trim the path to get the headset's root hub location
        if (!hardwareId.empty() && !classGuid.empty() && hardwareId.find(hmdHardwareId) != std::wstring::npos && classGuid.find(hostDeviceClass) != std::wstring::npos)
        {
            std::wstring locPath = get_device_property(deviceInfoSet, devInfo, SPDRP_LOCATION_PATHS);
            hubRootLoc = trim_usb_path(locPath, hubRootLevels);
            log << L"Using device's root hub: " << hubRootLoc.c_str() << " from " << hubRootLevels << " level(s)." << std::endl;
            break;
        }
    }

    if (hubRootLoc.empty())
    {
        log << L"Target device " << hmdHardwareId << " parent was not found." << std::endl;
        SetupDiDestroyDeviceInfoList(deviceInfoSet);
        return "";
    }

    // Get devices with vid 28DE & pid 2300 then find the first host instance that shares the same parent as the headset root hub
    // This works unless the same hub shares duplicate tundra devices (.e.g, a tundra device is connected to the headset's accessory port)
    for (DWORD i = 0; SetupDiEnumDeviceInfo(deviceInfoSet, i, &devInfo); i++)
    {
        std::wstring hardwareId = get_device_property(deviceInfoSet, devInfo, SPDRP_HARDWAREID);
        std::wstring classGuid = get_device_property(deviceInfoSet, devInfo, SPDRP_CLASSGUID);

        if (!hardwareId.empty() && !classGuid.empty() && hardwareId.find(tundraHardwareId) != std::wstring::npos && classGuid.find(hostDeviceClass) != std::wstring::npos)
        {
            std::wstring locPath = get_device_property(deviceInfoSet, devInfo, SPDRP_LOCATION_PATHS);

            // Check up to 4 levels to find a matching parent
            for (int p = 1; p < 4; p++)
            {
                std::wstring tundraHubParent = trim_usb_path(locPath, p);

                // Parent should match with root hub location
                if (tundraHubParent == hubRootLoc)
                {
                    log << L"Found matching tundra device with root hub: " << hubRootLoc.c_str() << std::endl;

                    WCHAR idBuffer[512];
                    if (SetupDiGetDeviceInstanceIdW(deviceInfoSet, &devInfo, idBuffer, 512, nullptr))
                    {
                        // Extract the LHR serial from the instance id
                        std::string parentId = utils.string_wide_to_narrow(idBuffer);
                        std::regex serialRegex(R"(LHR-[A-Za-z0-9]+)");
                        std::smatch match;
                        if (std::regex_search(parentId, match, serialRegex))
                        {
                            SetupDiDestroyDeviceInfoList(deviceInfoSet);
                            std::string serial = match[0];

                            log << L"Found lighthouse serial: " << serial.c_str() << std::endl;
                            return serial;
                        }
                    }

                    break;
                }
            }
        }
    }

    log << L"Device not found" << std::endl;
    SetupDiDestroyDeviceInfoList(deviceInfoSet);
    return "";
}

std::string HidHandler::get_device_rev(std::string targetVid, std::string targetPid)
{
    log << L"Fetching device version: " << targetVid.c_str() << " " << targetPid.c_str() << std::endl;
    HDEVINFO deviceInfoSet = SetupDiGetClassDevs(nullptr, L"USB", nullptr, DIGCF_PRESENT | DIGCF_ALLCLASSES);

    if (deviceInfoSet == INVALID_HANDLE_VALUE)
    {
        log << L"Failed to get device info list. (0x2)" << std::endl;
        return "";
    }

    SP_DEVINFO_DATA devInfoData;
    devInfoData.cbSize = sizeof(SP_DEVINFO_DATA);

    for (DWORD i = 0; SetupDiEnumDeviceInfo(deviceInfoSet, i, &devInfoData); i++)
    {
        std::wstring hardwareId = get_device_property(deviceInfoSet, devInfoData, SPDRP_HARDWAREID);

        if (!hardwareId.empty())
        {
            std::string idStr = utils.string_wide_to_narrow(hardwareId);
            if (idStr.find(targetVid) != std::string::npos && idStr.find(targetPid) != std::string::npos && idStr.find("REV_") != std::string::npos)
            {
                log << L"Got device id: " << hardwareId.c_str() << std::endl;
                SetupDiDestroyDeviceInfoList(deviceInfoSet);

                // Get formatted version code from hardware id
                std::regex revRegex(R"(REV_([0-9A-Fa-f]+))");
                std::smatch match;
                if (std::regex_search(idStr, match, revRegex))
                {
                    std::string rev = match[1];
                    rev.erase(0, rev.find_first_not_of('0'));
                    
                    log << L"Found device version: " << rev.c_str() << std::endl;
                    return rev;
                }

                return "";
            }
        }
    }

    log << L"Device not found" << std::endl;
    SetupDiDestroyDeviceInfoList(deviceInfoSet);
    return "";
}

std::pair<bool, std::string> HidHandler::get_device(std::string targetVid, std::string targetPid, std::wstring targetName)
{
    HDEVINFO deviceInfoSet = SetupDiGetClassDevs(nullptr, L"USB", nullptr, DIGCF_PRESENT | DIGCF_ALLCLASSES);

    if (deviceInfoSet == INVALID_HANDLE_VALUE)
    {
        log << L"Failed to get device info list. (0x3)" << std::endl;
        return std::make_pair(false, "err");
    }

    SP_DEVINFO_DATA devInfoData;
    devInfoData.cbSize = sizeof(SP_DEVINFO_DATA);

    for (DWORD i = 0; SetupDiEnumDeviceInfo(deviceInfoSet, i, &devInfoData); i++)
    {
        std::wstring hardwareId = get_device_property(deviceInfoSet, devInfoData, SPDRP_HARDWAREID);

        if (!hardwareId.empty())
        {
            std::string idStr = utils.string_wide_to_narrow(hardwareId);
            if (idStr.find(targetVid) != std::string::npos && idStr.find(targetPid) != std::string::npos)
            {
                // If a target name is defined, bail if this device isn't the same name.
                if (!targetName.empty())
                {
                    std::wstring frName = get_device_property(deviceInfoSet, devInfoData, SPDRP_FRIENDLYNAME);

                    if (frName != targetName)
                    {
                        continue;
                    }
                }

                // Check if the device driver is installed
                char driverKey[1024];
                if (SetupDiGetDeviceRegistryPropertyA(deviceInfoSet, &devInfoData, SPDRP_DRIVER, nullptr, (PBYTE)driverKey, sizeof(driverKey), nullptr))
                {
                    SetupDiDestroyDeviceInfoList(deviceInfoSet);
                    return std::make_pair(true, driverKey); // Driver is installed
                }
                else
                {
                    SetupDiDestroyDeviceInfoList(deviceInfoSet);
                    return std::make_pair(true, ""); // Device found, but no driver
                }
            }
        }
    }

    SetupDiDestroyDeviceInfoList(deviceInfoSet);
    return std::make_pair(false, "");
}

// Find the camera device class under the vid & pid
// Verify the camera name is not correct by comparing with the description
// If the "Bigeye" camera name isn't correct, the name matches description
// Requires elevated privledges
bool HidHandler::uninstall_camera(std::string targetVid, std::string targetPid)
{
    std::wstring cameraDeviceClass = L"ca3e7ab9-b4c3-4ae6-8251-579ef933890f";

    log << L"Attempting to uninstall the camera device" << std::endl;
    HDEVINFO deviceInfoSet = SetupDiGetClassDevs(nullptr, L"USB", nullptr, DIGCF_PRESENT | DIGCF_ALLCLASSES);

    if (deviceInfoSet == INVALID_HANDLE_VALUE)
    {
        log << L"Failed to get device info list. (0x3)" << std::endl;
        return false;
    }

    SP_DEVINFO_DATA devInfoData;
    devInfoData.cbSize = sizeof(SP_DEVINFO_DATA);

    for (DWORD i = 0; SetupDiEnumDeviceInfo(deviceInfoSet, i, &devInfoData); i++)
    {
        std::wstring hardwareId = get_device_property(deviceInfoSet, devInfoData, SPDRP_HARDWAREID);

        if (!hardwareId.empty())
        {
            std::wstring classGuid = get_device_property(deviceInfoSet, devInfoData, SPDRP_CLASSGUID);
            std::string idStr = utils.string_wide_to_narrow(hardwareId);

            if (idStr.find(targetVid) != std::string::npos && idStr.find(targetPid) != std::string::npos && classGuid.find(cameraDeviceClass) != std::wstring::npos)
            {
                std::wstring frName = get_device_property(deviceInfoSet, devInfoData, SPDRP_FRIENDLYNAME);
                std::wstring frDesc = get_device_property(deviceInfoSet, devInfoData, SPDRP_DEVICEDESC);

                if (frName != frDesc)
                {
                    log << L"Device description and name don't match: " + frName + L" > " + frDesc << std::endl;
                    continue;
                }

                log << L"Got camera device. Uninstalling" << std::endl;

                SP_REMOVEDEVICE_PARAMS removeParams{};
                removeParams.ClassInstallHeader.cbSize = sizeof(SP_CLASSINSTALL_HEADER);
                removeParams.ClassInstallHeader.InstallFunction = DIF_REMOVE;
                removeParams.Scope = DI_REMOVEDEVICE_GLOBAL;
                removeParams.HwProfile = 0;

                if (!SetupDiSetClassInstallParamsW(
                    deviceInfoSet,
                    &devInfoData,
                    &removeParams.ClassInstallHeader,
                    sizeof(removeParams)))
                {
                    SetupDiDestroyDeviceInfoList(deviceInfoSet);
                    return false;
                }

                if (!SetupDiCallClassInstaller(
                    DIF_REMOVE,
                    deviceInfoSet,
                    &devInfoData))
                {
                    SetupDiDestroyDeviceInfoList(deviceInfoSet);
                    return false;
                }
                
                SetupDiDestroyDeviceInfoList(deviceInfoSet);
                return true;
            }
        }
    }

    SetupDiDestroyDeviceInfoList(deviceInfoSet);
    return false;
}

///
void HidHandler::shutdown()
{
    if (thread.joinable())
    {
        thread.join();
    }
}