diff --git a/.gitignore b/.gitignore
index daef46b..5eb13e1 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,6 +2,8 @@ Firmware/.vscode
 Firmware/RP2040/build
 Firmware/RP2040/.ignore
 Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/tud_xid/tud_xid_xremote_rom.h
+Firmware/RP2040/src/BLEServer/att_delayed_response.h
+Firmware/ESP32/main/BLEServer/att_delayed_response.h
 Firmware/ESP32/.ignore
 Firmware/ESP32/build
 Firmware/ESP32/components/btstack
diff --git a/Firmware/ESP32/CMakeLists.txt b/Firmware/ESP32/CMakeLists.txt
index 710289f..f4aa783 100644
--- a/Firmware/ESP32/CMakeLists.txt
+++ b/Firmware/ESP32/CMakeLists.txt
@@ -1,16 +1,26 @@
 cmake_minimum_required(VERSION 3.5)
+
 set(CMAKE_C_STANDARD 11)
 set(CMAKE_CXX_STANDARD 17)
 
 set(EXTERNAL_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../external)
+set(EXTERNAL_CMAKE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../cmake)
+set(BTSTACK_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/components/btstack)
 
-include(${EXTERNAL_DIR}/init_submodules.cmake)
-include(${EXTERNAL_DIR}/patch_libs.cmake)
+include(${CMAKE_CURRENT_SOURCE_DIR}/../FWDefines.cmake)
+include(${EXTERNAL_CMAKE_DIR}/init_submodules.cmake)
+include(${EXTERNAL_CMAKE_DIR}/patch_libs.cmake)
+include(${EXTERNAL_CMAKE_DIR}/generate_gatt_header.cmake)
 include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/integrate_btstack.cmake)
 
 init_git_submodules(${EXTERNAL_DIR})
 apply_lib_patches(${EXTERNAL_DIR})
 integrate_btstack(${EXTERNAL_DIR})
+generate_gatt_header(
+    ${BTSTACK_ROOT} 
+    ${CMAKE_CURRENT_SOURCE_DIR}/../RP2040/src/BLEServer/att_delayed_response.gatt
+    ${CMAKE_CURRENT_SOURCE_DIR}/main/BLEServer/att_delayed_response.h
+    )
 
 set(BLUEPAD32_ROOT ${EXTERNAL_DIR}/bluepad32/src/components/bluepad32)
 
@@ -24,4 +34,4 @@ set(EXTRA_COMPONENT_DIRS
 )
 
 include($ENV{IDF_PATH}/tools/cmake/project.cmake)
-project(OGX-Mini-ESP32)
\ No newline at end of file
+project("${FW_NAME}-${FW_VERSION}-ESP32")
\ No newline at end of file
diff --git a/Firmware/ESP32/main/1btstack_config.h b/Firmware/ESP32/main/1btstack_config.h
new file mode 100644
index 0000000..be4f0c4
--- /dev/null
+++ b/Firmware/ESP32/main/1btstack_config.h
@@ -0,0 +1,82 @@
+#ifndef _ESP_BTSTACK_CONFIG_H
+#define _ESP_BTSTACK_CONFIG_H
+
+// BTstack features that can be enabled
+// #define ENABLE_LE_PERIPHERAL
+// #define ENABLE_LE_CENTRAL
+// #define ENABLE_L2CAP_LE_CREDIT_BASED_FLOW_CONTROL_MODE
+// #define ENABLE_LOG_INFO
+// #define ENABLE_LOG_ERROR
+// #define ENABLE_PRINTF_HEXDUMP
+#define ENABLE_SCO_OVER_HCI
+
+// BTstack configuration. buffers, sizes, ...
+#define HCI_OUTGOING_PRE_BUFFER_SIZE 4
+#define HCI_ACL_PAYLOAD_SIZE (1691 + 4)
+#define HCI_ACL_CHUNK_SIZE_ALIGNMENT 4
+// #define MAX_NR_AVDTP_CONNECTIONS 1
+// #define MAX_NR_AVDTP_STREAM_ENDPOINTS 1
+// #define MAX_NR_AVRCP_CONNECTIONS 2
+// #define MAX_NR_BNEP_CHANNELS 1
+// #define MAX_NR_BNEP_SERVICES 1
+// #define MAX_NR_BTSTACK_LINK_KEY_DB_MEMORY_ENTRIES 2
+// #define MAX_NR_GATT_CLIENTS 4
+// #define MAX_NR_HCI_CONNECTIONS 4
+// #define MAX_NR_HID_HOST_CONNECTIONS 4
+// #define MAX_NR_HIDS_CLIENTS 4
+// #define MAX_NR_HFP_CONNECTIONS 1
+// #define MAX_NR_L2CAP_CHANNELS 6
+// #define MAX_NR_L2CAP_SERVICES 5
+// #define MAX_NR_RFCOMM_CHANNELS 1
+// #define MAX_NR_RFCOMM_MULTIPLEXERS 1
+// #define MAX_NR_RFCOMM_SERVICES 1
+// #define MAX_NR_SERVICE_RECORD_ITEMS 4
+// #define MAX_NR_SM_LOOKUP_ENTRIES 3
+// #define MAX_NR_WHITELIST_ENTRIES 16
+// #define MAX_NR_LE_DEVICE_DB_ENTRIES 16
+
+// Limit number of ACL/SCO Buffer to use by stack to avoid cyw43 shared bus overrun
+#define MAX_NR_CONTROLLER_ACL_BUFFERS 6
+#define MAX_NR_CONTROLLER_SCO_PACKETS 6
+
+// Enable and configure HCI Controller to Host Flow Control to avoid cyw43 shared bus overrun
+#define ENABLE_HCI_CONTROLLER_TO_HOST_FLOW_CONTROL
+#define HCI_HOST_ACL_PACKET_LEN 1024
+#define HCI_HOST_ACL_PACKET_NUM 6
+#define HCI_HOST_SCO_PACKET_LEN 120
+#define HCI_HOST_SCO_PACKET_NUM 6
+
+#ifndef HCI_INCOMING_PRE_BUFFER_SIZE
+#define HCI_INCOMING_PRE_BUFFER_SIZE 6
+#endif
+
+// Link Key DB and LE Device DB using TLV on top of Flash Sector interface
+// #define NVM_NUM_DEVICE_DB_ENTRIES 16
+// #define NVM_NUM_LINK_KEYS 16
+
+// We don't give btstack a malloc, so use a fixed-size ATT DB.
+// #define MAX_ATT_DB_SIZE 512
+#define HAVE_MALLOC
+
+// BTstack HAL configuration
+// #define HAVE_EMBEDDED_TIME_MS
+
+// // map btstack_assert onto Pico SDK assert()
+// #define HAVE_ASSERT
+
+// // Some USB dongles take longer to respond to HCI reset (e.g. BCM20702A).
+#define HCI_RESET_RESEND_TIMEOUT_MS 1000
+
+// #define ENABLE_SOFTWARE_AES128
+// #define ENABLE_MICRO_ECC_FOR_LE_SECURE_CONNECTIONS
+
+// #define HAVE_BTSTACK_STDIN
+
+// To get the audio demos working even with HCI dump at 115200, this truncates long ACL packetws
+#define HCI_DUMP_STDOUT_MAX_SIZE_ACL 100
+
+#ifdef ENABLE_CLASSIC
+#define ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE
+#endif
+
+#endif  // _ESP_BTSTACK_CONFIG_H
\ No newline at end of file
diff --git a/Firmware/ESP32/main/BLEServer/BLEServer.cpp b/Firmware/ESP32/main/BLEServer/BLEServer.cpp
new file mode 100644
index 0000000..4d8b64d
--- /dev/null
+++ b/Firmware/ESP32/main/BLEServer/BLEServer.cpp
@@ -0,0 +1,264 @@
+#include <cstring>
+#include <string>
+#include <esp_ota_ops.h>
+#include <esp_system.h>
+
+#include "att_server.h"
+#include "btstack.h"
+
+#include "Gamepad.h"
+#include "BLEServer/BLEServer.h"
+#include "BLEServer/att_delayed_response.h"
+#include "UserSettings/UserProfile.h"
+#include "UserSettings/UserSettings.h"
+
+namespace BLEServer {
+
+static constexpr uint16_t PACKET_LEN_MAX = 18;
+
+#pragma pack(push, 1)
+struct SetupPacket
+{
+    uint8_t max_gamepads{1};
+    uint8_t index{0};
+    uint8_t device_type{0};
+    uint8_t profile_id{1};
+};
+static_assert(sizeof(SetupPacket) == 4, "BLEServer::SetupPacket struct size mismatch");
+#pragma pack(pop)
+
+SetupPacket setup_packet_;
+
+namespace Handle
+{
+    static constexpr uint16_t FW_VERSION    = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789020_01_VALUE_HANDLE;
+    static constexpr uint16_t FW_NAME       = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789021_01_VALUE_HANDLE;
+
+    static constexpr uint16_t START_UPDATE  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789030_01_VALUE_HANDLE;
+    static constexpr uint16_t COMMIT_UPDATE = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789031_01_VALUE_HANDLE;
+
+    static constexpr uint16_t SETUP_PACKET  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789040_01_VALUE_HANDLE;
+    static constexpr uint16_t PROFILE_PT1   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789041_01_VALUE_HANDLE;
+    static constexpr uint16_t PROFILE_PT2   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789042_01_VALUE_HANDLE;
+    static constexpr uint16_t PROFILE_PT3   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789043_01_VALUE_HANDLE;
+}
+
+namespace ADV
+{
+    // Flags general discoverable, BR/EDR not supported
+    static const uint8_t FLAGS[]    = { 0x02, 0x01, 0x06 };
+    static const uint8_t NAME_TYPE  = 0x09;
+
+    #pragma pack(push, 1)
+    struct Data
+    {
+        uint8_t flags[sizeof(FLAGS)];
+        uint8_t name_len;
+        uint8_t name_type;
+        uint8_t name[sizeof(FIRMWARE_NAME) - 1];
+
+        Data()
+        {
+            std::memcpy(flags, FLAGS, sizeof(flags));
+            name_len = sizeof(FIRMWARE_NAME);
+            name_type = NAME_TYPE;
+            std::memcpy(name, FIRMWARE_NAME, sizeof(name));
+        }
+    };
+    static_assert(sizeof(Data) == 5 + sizeof(FIRMWARE_NAME) - 1, "BLEServer::ADV::Data struct size mismatch");
+    #pragma pack(pop)
+}
+
+static int verify_write(const uint16_t buffer_size, const uint16_t expected_size, bool pending_write = false, bool expected_pending_write = false)
+{
+    if (buffer_size != expected_size)
+    {
+        return ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LENGTH;
+    }
+    if (pending_write != expected_pending_write)
+    {
+        return ATT_ERROR_WRITE_NOT_PERMITTED;
+    }
+    return 0;
+}
+
+static uint16_t att_read_callback(  hci_con_handle_t connection_handle,
+                                    uint16_t att_handle,
+                                    uint16_t offset,
+                                    uint8_t *buffer,
+                                    uint16_t buffer_size)
+{
+    static UserProfile profile;
+    SetupPacket setup_packet_resp;
+    std::string fw_version;
+    std::string fw_name;
+
+    switch (att_handle)
+    {
+        case Handle::FW_VERSION:
+            fw_version = FIRMWARE_VERSION;
+            if (buffer)
+            {
+                std::memcpy(buffer, reinterpret_cast<const uint8_t*>(fw_version.c_str()), fw_version.size());
+            }
+            return static_cast<uint16_t>(fw_version.size());
+
+        case Handle::FW_NAME:
+            fw_name = FIRMWARE_NAME;
+            if (buffer)
+            {
+                std::memcpy(buffer, reinterpret_cast<const uint8_t*>(fw_name.c_str()), fw_name.size());
+            }
+            return static_cast<uint16_t>(fw_name.size());
+
+        case Handle::SETUP_PACKET:
+            if (buffer)
+            {
+                //App has already written a setup packet with the index
+                setup_packet_resp.max_gamepads = static_cast<uint8_t>(MAX_GAMEPADS);
+                setup_packet_resp.index = setup_packet_.index;
+                setup_packet_resp.device_type = static_cast<uint8_t>(UserSettings::get_instance().get_current_driver());
+                setup_packet_resp.profile_id = UserSettings::get_instance().get_active_profile_id(setup_packet_.index);
+
+                std::memcpy(buffer, &setup_packet_resp, sizeof(setup_packet_resp));
+            }
+            return sizeof(setup_packet_);
+
+        case Handle::PROFILE_PT1:
+            if (buffer)
+            {
+                //App has already written the profile id it wants to the setup packet
+                profile = UserSettings::get_instance().get_profile_by_id(setup_packet_.profile_id);
+                std::memcpy(buffer, &profile, PACKET_LEN_MAX);
+            }
+            return PACKET_LEN_MAX;
+
+        case Handle::PROFILE_PT2:
+            if (buffer)
+            {
+                std::memcpy(buffer, reinterpret_cast<uint8_t*>(&profile) + PACKET_LEN_MAX, PACKET_LEN_MAX);
+            }
+            return PACKET_LEN_MAX;
+
+        case Handle::PROFILE_PT3:
+            if (buffer)
+            {
+                std::memcpy(buffer, reinterpret_cast<uint8_t*>(&profile) + PACKET_LEN_MAX * 2, sizeof(UserProfile) - PACKET_LEN_MAX * 2);
+            }
+            return sizeof(UserProfile) - PACKET_LEN_MAX * 2;
+
+        default:
+            break;
+    }
+    return 0;
+}
+
+static int att_write_callback(hci_con_handle_t connection_handle,
+                              uint16_t att_handle,
+                              uint16_t transaction_mode,
+                              uint16_t offset,
+                              uint8_t *buffer,
+                              uint16_t buffer_size)
+{
+    static UserProfile temp_profile;
+    static bool pending_write = false;
+
+    int ret = 0;
+
+    switch (att_handle)
+    {
+        case Handle::START_UPDATE:
+            pending_write = true;
+            break;
+
+        case Handle::SETUP_PACKET:
+            if ((ret = verify_write(buffer_size, sizeof(SetupPacket))) != 0)
+            {
+                break;
+            }
+
+            std::memcpy(&setup_packet_, buffer, buffer_size);
+            if (setup_packet_.index >= MAX_GAMEPADS)
+            {
+                setup_packet_.index = 0;
+                ret = ATT_ERROR_OUT_OF_RANGE;
+            }
+            if (setup_packet_.profile_id > UserSettings::MAX_PROFILES)
+            {
+                setup_packet_.profile_id = 1;
+                ret = ATT_ERROR_OUT_OF_RANGE;
+            }
+            if (ret)
+            {
+                break;
+            }
+
+            if (pending_write)
+            {
+                //App wants to store a new device driver type
+                UserSettings::get_instance().store_driver_type(static_cast<DeviceDriverType>(setup_packet_.device_type));
+            }
+            break;
+
+        case Handle::PROFILE_PT1:
+            if ((ret = verify_write(buffer_size, PACKET_LEN_MAX, pending_write, true)) != 0)
+            {
+                break;
+            }
+            std::memcpy(&temp_profile, buffer, buffer_size);
+            break;
+
+        case Handle::PROFILE_PT2:
+            if ((ret = verify_write(buffer_size, PACKET_LEN_MAX, pending_write, true)) != 0)
+            {
+                break;
+            }
+            std::memcpy(reinterpret_cast<uint8_t*>(&temp_profile) + PACKET_LEN_MAX, buffer, buffer_size);
+            break;
+
+        case Handle::PROFILE_PT3:
+            if ((ret = verify_write(buffer_size, sizeof(UserProfile) - PACKET_LEN_MAX * 2, pending_write, true)) != 0)
+            {
+                break;
+            }
+            std::memcpy(reinterpret_cast<uint8_t*>(&temp_profile) + PACKET_LEN_MAX * 2, buffer, buffer_size);
+            break;
+
+        case Handle::COMMIT_UPDATE:
+            if ((ret = verify_write(0, 0, pending_write, true)) != 0)
+            {
+                break;
+            }
+            UserSettings::get_instance().store_profile(setup_packet_.index, temp_profile);
+            pending_write = false;
+            break;
+
+        default:
+            break;
+    }
+    return ret;
+}
+
+void init_server()
+{
+    UserSettings::get_instance().initialize_flash();
+
+    // setup ATT server
+    att_server_init(profile_data, att_read_callback, att_write_callback);
+
+    // setup advertisements
+    uint16_t adv_int_min = 0x0030;
+    uint16_t adv_int_max = 0x0030;
+    uint8_t adv_type = 0;
+
+    bd_addr_t null_addr;
+    std::memset(null_addr, 0, sizeof(null_addr));
+
+    static ADV::Data adv_data = ADV::Data();
+
+    gap_advertisements_set_params(adv_int_min, adv_int_max, adv_type, 0, null_addr, 0x07, 0x00);
+    gap_advertisements_set_data(static_cast<uint8_t>(sizeof(adv_data)), reinterpret_cast<uint8_t*>(&adv_data));
+    gap_advertisements_enable(1);
+}
+
+} // namespace BLEServer
\ No newline at end of file
diff --git a/Firmware/ESP32/main/BLEServer/BLEServer.h b/Firmware/ESP32/main/BLEServer/BLEServer.h
new file mode 100644
index 0000000..d72962e
--- /dev/null
+++ b/Firmware/ESP32/main/BLEServer/BLEServer.h
@@ -0,0 +1,11 @@
+#ifndef BLE_SERVER_H
+#define BLE_SERVER_H
+
+#include <cstdint>
+
+namespace BLEServer 
+{
+    void init_server();
+}
+
+#endif // BLE_SERVER_H
\ No newline at end of file
diff --git a/Firmware/ESP32/main/BTManager/BTManager.cpp b/Firmware/ESP32/main/BTManager/BTManager.cpp
new file mode 100644
index 0000000..ad0b366
--- /dev/null
+++ b/Firmware/ESP32/main/BTManager/BTManager.cpp
@@ -0,0 +1,245 @@
+#include "btstack_port_esp32.h"
+#include "btstack_run_loop.h"
+#include "btstack_stdio_esp32.h"
+#include "uni.h"
+
+#include "sdkconfig.h"
+#include "Board/ogxm_log.h"
+#include "Board/board_api.h"
+#include "BTManager/BTManager.h"
+
+void BTManager::run_task()
+{
+    board_api::init_board(); 
+    UserSettings::get_instance().initialize_flash();
+
+    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
+    {
+        UserProfile profile = UserSettings::get_instance().get_profile_by_index(i);
+        devices_[i].mapper.set_profile(profile);
+    }
+
+    i2c_driver_.initialize_i2c(
+        static_cast<i2c_port_t>(CONFIG_I2C_PORT), 
+        static_cast<gpio_num_t>(CONFIG_I2C_SDA_PIN), 
+        static_cast<gpio_num_t>(CONFIG_I2C_SCL_PIN),  
+        CONFIG_I2C_BAUDRATE);
+
+    xTaskCreatePinnedToCore(
+        [](void* parameter)
+        { 
+            get_instance().i2c_driver_.run_tasks(); 
+        },
+        "i2c",
+        2048 * 2,
+        nullptr,
+        configMAX_PRIORITIES-8,
+        nullptr,
+        1 );
+
+    btstack_init();
+
+    uni_platform_set_custom(get_bp32_driver());
+    uni_init(0, nullptr);
+
+    btstack_timer_source_t led_timer;
+    led_timer.process = check_led_cb;
+    led_timer.context = nullptr;
+    btstack_run_loop_set_timer(&led_timer, LED_TIME_MS);
+    btstack_run_loop_add_timer(&led_timer);
+
+    btstack_timer_source_t driver_update_timer;
+    driver_update_timer.process = driver_update_timer_cb;
+    driver_update_timer.context = nullptr;
+    btstack_run_loop_set_timer(&driver_update_timer, UserSettings::GP_CHECK_DELAY_MS);
+    btstack_run_loop_add_timer(&driver_update_timer);
+
+    //Doesn't return
+    btstack_run_loop_execute();
+}
+
+bool BTManager::is_connected(uint8_t index)
+{
+    if (index >= devices_.size())
+    {
+        return false;
+    }
+    return devices_[index].connected.load();
+}
+
+bool BTManager::any_connected()
+{
+    for (auto& device : devices_)
+    {
+        if (device.connected.load())
+        {
+            return true;
+        }
+    }
+    return false;
+}
+
+void BTManager::check_led_cb(btstack_timer_source *ts)
+{
+    static bool led_state = false;
+    led_state = !led_state;
+
+    if constexpr (board_api::NUM_LEDS == 1)
+    {
+        board_api::set_led(get_instance().any_connected() ? true : (led_state ? true : false));
+    }
+    else
+    {
+        for (uint8_t i = 0; i < board_api::NUM_LEDS; ++i)
+        {
+            board_api::set_led(i, get_instance().is_connected(i) ? true : (led_state ? true : false));
+        }
+    }
+
+    btstack_run_loop_set_timer(ts, LED_TIME_MS);
+    btstack_run_loop_add_timer(ts);
+}
+
+void BTManager::send_driver_type(DeviceDriverType driver_type)
+{
+    OGXM_LOG("BP32: Sending driver type: %s\n", DRIVER_NAME(driver_type).c_str());
+
+    if constexpr (I2CDriver::MULTI_SLAVE)
+    {
+        for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
+        {
+            I2CDriver::PacketIn packet_in = devices_[i].packet_in;
+            packet_in.packet_id = I2CDriver::PacketID::SET_DRIVER;
+            packet_in.index = i;
+            packet_in.device_driver = driver_type;
+            i2c_driver_.write_packet(i + 1, packet_in);
+        }
+    }
+    else
+    {
+        I2CDriver::PacketIn packet_in = devices_[0].packet_in;
+        packet_in.packet_id = I2CDriver::PacketID::SET_DRIVER;
+        packet_in.index = 0;
+        packet_in.device_driver = driver_type;
+        i2c_driver_.write_packet(0x01, packet_in);
+    }
+}
+
+uni_hid_device_t* BTManager::get_connected_bp32_device(uint8_t index)
+{
+    uni_hid_device_t* bp_device = nullptr;
+    if (!(bp_device = uni_hid_device_get_instance_for_idx(index)) || 
+        !uni_bt_conn_is_connected(&bp_device->conn) ||
+        !bp_device->report_parser.play_dual_rumble)
+    {
+        return nullptr;
+    }
+    return bp_device;
+}
+
+void BTManager::driver_update_timer_cb(btstack_timer_source *ts)
+{
+    BTManager& bt_manager = get_instance();
+    I2CDriver::PacketIn& packet_in = bt_manager.devices_.front().packet_in;
+
+    if (get_connected_bp32_device(0) &&
+        UserSettings::get_instance().check_for_driver_change(packet_in))
+    {
+        OGXM_LOG("BP32: Driver change detected\n");
+        UserSettings::get_instance().store_driver_type(UserSettings::get_instance().get_current_driver());
+    }
+
+    //Notify pico of current driver type regardless of change
+    bt_manager.send_driver_type(UserSettings::get_instance().get_current_driver());
+
+    btstack_run_loop_set_timer(ts, UserSettings::GP_CHECK_DELAY_MS);
+    btstack_run_loop_add_timer(ts);
+}
+
+void BTManager::send_feedback_cb(void* context)
+{
+    FBContext* fb_context = reinterpret_cast<FBContext*>(context);
+    uni_hid_device_t* bp_device = nullptr;
+
+    if (!(bp_device = get_connected_bp32_device(fb_context->index)))
+    {
+        return;
+    }
+
+    I2CDriver::PacketOut packet_out = fb_context->packet_out->load();
+
+    if (packet_out.rumble_l || packet_out.rumble_r)
+    {
+        bp_device->report_parser.play_dual_rumble(
+            bp_device, 
+            0, 
+            FEEDBACK_TIME_MS, 
+            packet_out.rumble_l, 
+            packet_out.rumble_r
+            );
+    }
+}
+
+//This will have to be changed once full support for multiple devices is added
+void BTManager::feedback_timer_cb(btstack_timer_source *ts)
+{
+    static FBContext fb_contexts[MAX_GAMEPADS];
+    BTManager& bt_manager = get_instance();
+
+    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
+    {
+        if (!get_connected_bp32_device(i))
+        {
+            continue;
+        }
+
+        FBContext& fb_context = fb_contexts[i];
+        fb_context.index = i;
+        fb_context.packet_out = &bt_manager.devices_[i].packet_out;
+        fb_context.cb_reg.callback = send_feedback_cb;
+        fb_context.cb_reg.context = reinterpret_cast<void*>(&fb_context);
+
+        //Register a read on i2c thread, with callback to send feedback on btstack thread
+        bt_manager.i2c_driver_.read_packet(I2CDriver::MULTI_SLAVE ? i + 1 : 0x01,
+            [&fb_context](const I2CDriver::PacketOut& packet_out)
+            {
+                fb_context.packet_out->store(packet_out);
+                btstack_run_loop_execute_on_main_thread(&fb_context.cb_reg);
+            });
+    }
+
+    btstack_run_loop_set_timer(ts, FEEDBACK_TIME_MS);
+    btstack_run_loop_add_timer(ts);
+}
+
+void BTManager::manage_connection(uint8_t index, bool connected)
+{
+    devices_[index].connected.store(connected);
+    if (connected)
+    {
+        if (!fb_timer_running_)
+        {
+            fb_timer_running_ = true;
+            fb_timer_.process = feedback_timer_cb;
+            fb_timer_.context = nullptr;
+            btstack_run_loop_set_timer(&fb_timer_, FEEDBACK_TIME_MS);
+            btstack_run_loop_add_timer(&fb_timer_);
+        }
+    }
+    else
+    {
+        if (!any_connected())
+        {
+            if (fb_timer_running_)
+            {
+                fb_timer_running_ = false;
+                btstack_run_loop_remove_timer(&fb_timer_);
+            }
+        }
+
+        I2CDriver::PacketIn packet_in = I2CDriver::PacketIn();
+        packet_in.packet_id = I2CDriver::PacketID::SET_PAD;
+        packet_in.index = index;
+        i2c_driver_.write_packet(I2CDriver::MULTI_SLAVE ? packet_in.index + 1 : 0x01, packet_in);
+    }
+}
\ No newline at end of file
diff --git a/Firmware/ESP32/main/BTManager/BTManager.h b/Firmware/ESP32/main/BTManager/BTManager.h
new file mode 100644
index 0000000..c5f9ce4
--- /dev/null
+++ b/Firmware/ESP32/main/BTManager/BTManager.h
@@ -0,0 +1,82 @@
+#ifndef _BT_MANAGER_H_ 
+#define _BT_MANAGER_H_
+
+#include <cstdint>
+#include <array>
+#include <atomic>
+#include <string>
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+
+#include "I2CDriver/I2CDriver.h"
+#include "Gamepad.h"
+
+class BTManager
+{
+public:
+    static BTManager& get_instance()
+    {
+        static BTManager instance;
+        return instance;
+    }
+
+    void run_task();
+    bool any_connected();
+    bool is_connected(uint8_t index);
+
+private:
+    BTManager() = default;
+    ~BTManager() = default;
+    BTManager(const BTManager&) = delete;
+    BTManager& operator=(const BTManager&) = delete;
+
+    static constexpr uint32_t FEEDBACK_TIME_MS = 200;
+    static constexpr uint32_t LED_TIME_MS = 500;
+
+    struct Device
+    {
+        std::atomic<bool> connected{false};
+        GamepadMapper mapper;
+        I2CDriver::PacketIn packet_in;
+        std::atomic<I2CDriver::PacketOut> packet_out; //Can be updated from i2c thread
+    };
+
+    struct FBContext
+    {
+        uint8_t index;
+        std::atomic<I2CDriver::PacketOut>* packet_out;
+        btstack_context_callback_registration_t cb_reg;
+    };
+
+    std::array<Device, MAX_GAMEPADS> devices_;
+    I2CDriver i2c_driver_;
+
+    btstack_timer_source_t fb_timer_;
+    bool fb_timer_running_ = false;
+
+    void send_driver_type(DeviceDriverType driver_type);
+    void manage_connection(uint8_t index, bool connected);
+    
+    static uni_hid_device_t* get_connected_bp32_device(uint8_t index);
+    static void check_led_cb(btstack_timer_source *ts);
+    static void send_feedback_cb(void* context);
+    static void feedback_timer_cb(btstack_timer_source *ts);
+    static void driver_update_timer_cb(btstack_timer_source *ts);
+
+    //Bluepad32 driver
+
+    void init(int argc, const char** arg_V);
+    void init_complete_cb(void);
+    uni_error_t device_discovered_cb(bd_addr_t addr, const char* name, uint16_t cod, uint8_t rssi);
+    void device_connected_cb(uni_hid_device_t* device);
+    void device_disconnected_cb(uni_hid_device_t* device);
+    uni_error_t device_ready_cb(uni_hid_device_t* device);
+    void controller_data_cb(uni_hid_device_t* bp_device, uni_controller_t* controller);
+    const uni_property_t* get_property_cb(uni_property_idx_t idx);
+    void oob_event_cb(uni_platform_oob_event_t event, void* data);
+
+    static uni_platform* get_bp32_driver();
+
+}; // class BTManager
+
+#endif // _BT_MANAGER_H_
\ No newline at end of file
diff --git a/Firmware/ESP32/main/BTManager/BTManager_BP32.cpp b/Firmware/ESP32/main/BTManager/BTManager_BP32.cpp
new file mode 100644
index 0000000..c35990f
--- /dev/null
+++ b/Firmware/ESP32/main/BTManager/BTManager_BP32.cpp
@@ -0,0 +1,187 @@
+#include "btstack_port_esp32.h"
+#include "btstack_run_loop.h"
+#include "btstack_stdio_esp32.h"
+#include "uni.h"
+
+#include "Board/ogxm_log.h"
+#include "BTManager/BTManager.h"
+
+void BTManager::init(int argc, const char** arg_V) 
+{
+    OGXM_LOG("BP32: Initializing Bluepad32\n");
+}
+
+void BTManager::init_complete_cb(void) 
+{
+    uni_bt_enable_new_connections_unsafe(true);
+    // uni_bt_del_keys_unsafe();
+    uni_property_dump_all();
+}
+
+uni_error_t BTManager::device_discovered_cb(bd_addr_t addr, const char* name, uint16_t cod, uint8_t rssi) 
+{
+    if (!((cod & UNI_BT_COD_MINOR_MASK) & UNI_BT_COD_MINOR_GAMEPAD))
+    {
+        return UNI_ERROR_IGNORE_DEVICE;
+    }
+    return UNI_ERROR_SUCCESS;
+}
+
+void BTManager::device_connected_cb(uni_hid_device_t* device) 
+{
+    OGXM_LOG("BP32: Device connected, addr:  %p, index: %i\n", device, uni_hid_device_get_idx_for_instance(device));
+}
+
+void BTManager::device_disconnected_cb(uni_hid_device_t* device) 
+{
+    int idx = uni_hid_device_get_idx_for_instance(device);
+    if (idx >= MAX_GAMEPADS || idx < 0)
+    {
+        return;
+    }
+    manage_connection(static_cast<uint8_t>(idx), false);
+}
+
+uni_error_t BTManager::device_ready_cb(uni_hid_device_t* device) 
+{
+    int idx = uni_hid_device_get_idx_for_instance(device);
+    if (idx >= MAX_GAMEPADS || idx < 0)
+    {
+        return UNI_ERROR_IGNORE_DEVICE;
+    }
+    manage_connection(static_cast<uint8_t>(idx), true);
+    return UNI_ERROR_SUCCESS;
+}
+
+void BTManager::controller_data_cb(uni_hid_device_t* bp_device, uni_controller_t* controller) 
+{
+    static uni_gamepad_t prev_uni_gps[MAX_GAMEPADS] = {};
+
+    if (controller->klass != UNI_CONTROLLER_CLASS_GAMEPAD)
+    {
+        return;
+    }
+
+    int idx = uni_hid_device_get_idx_for_instance(bp_device);
+    uni_gamepad_t *uni_gp = &controller->gamepad;
+
+    if (idx < 0 || idx >= MAX_GAMEPADS || std::memcmp(uni_gp, &prev_uni_gps[idx], sizeof(uni_gamepad_t)) == 0)
+    {
+        return;
+    }
+
+    I2CDriver::PacketIn& packet_in =  devices_[idx].packet_in;
+    GamepadMapper& mapper =  devices_[idx].mapper;
+
+    packet_in = I2CDriver::PacketIn();
+    packet_in.packet_id = I2CDriver::PacketID::SET_PAD;
+    packet_in.index = static_cast<uint8_t>(idx);
+
+    switch (uni_gp->dpad) 
+    {
+        case DPAD_UP:
+            packet_in.dpad = mapper.DPAD_UP;
+            break;
+        case DPAD_DOWN:
+            packet_in.dpad = mapper.DPAD_DOWN;
+            break;
+        case DPAD_LEFT:
+            packet_in.dpad = mapper.DPAD_LEFT;
+            break;
+        case DPAD_RIGHT:
+            packet_in.dpad = mapper.DPAD_RIGHT;
+            break;
+        case (DPAD_UP | DPAD_RIGHT):
+            packet_in.dpad = mapper.DPAD_UP_RIGHT;
+            break;
+        case (DPAD_DOWN | DPAD_RIGHT):
+            packet_in.dpad = mapper.DPAD_DOWN_RIGHT;
+            break;
+        case (DPAD_DOWN | DPAD_LEFT):
+            packet_in.dpad = mapper.DPAD_DOWN_LEFT;
+            break;
+        case (DPAD_UP | DPAD_LEFT):
+            packet_in.dpad = mapper.DPAD_UP_LEFT;
+            break;
+        default:
+            break;
+    }
+
+    if (uni_gp->buttons & BUTTON_A) packet_in.buttons |= mapper.BUTTON_A;
+    if (uni_gp->buttons & BUTTON_B) packet_in.buttons |= mapper.BUTTON_B;
+    if (uni_gp->buttons & BUTTON_X) packet_in.buttons |= mapper.BUTTON_X;
+    if (uni_gp->buttons & BUTTON_Y) packet_in.buttons |= mapper.BUTTON_Y;
+    if (uni_gp->buttons & BUTTON_SHOULDER_L) packet_in.buttons |= mapper.BUTTON_LB;
+    if (uni_gp->buttons & BUTTON_SHOULDER_R) packet_in.buttons |= mapper.BUTTON_RB;
+    if (uni_gp->buttons & BUTTON_THUMB_L)    packet_in.buttons |= mapper.BUTTON_L3;
+    if (uni_gp->buttons & BUTTON_THUMB_R)    packet_in.buttons |= mapper.BUTTON_R3;
+    if (uni_gp->misc_buttons & MISC_BUTTON_BACK)    packet_in.buttons |= mapper.BUTTON_BACK;
+    if (uni_gp->misc_buttons & MISC_BUTTON_START)   packet_in.buttons |= mapper.BUTTON_START;
+    if (uni_gp->misc_buttons & MISC_BUTTON_SYSTEM)  packet_in.buttons |= mapper.BUTTON_SYS;
+    if (uni_gp->misc_buttons & MISC_BUTTON_CAPTURE) packet_in.buttons |= mapper.BUTTON_MISC;
+
+    packet_in.trigger_l = mapper.trigger_l(uni_gp->brake);
+    packet_in.trigger_r = mapper.trigger_r(uni_gp->throttle);
+
+    packet_in.joystick_lx = mapper.joystick_lx(uni_gp->axis_x);
+    packet_in.joystick_ly = mapper.joystick_ly(uni_gp->axis_y);
+    packet_in.joystick_rx = mapper.joystick_rx(uni_gp->axis_rx);
+    packet_in.joystick_ry = mapper.joystick_ry(uni_gp->axis_ry);
+
+    i2c_driver_.write_packet(I2CDriver::MULTI_SLAVE ? packet_in.index + 1 : 0x01, packet_in);
+
+    std::memcpy(&prev_uni_gps[idx], uni_gp, sizeof(uni_gamepad_t));
+}
+
+const uni_property_t* BTManager::get_property_cb(uni_property_idx_t idx) 
+{
+    return nullptr;
+}
+
+void BTManager::oob_event_cb(uni_platform_oob_event_t event, void* data) 
+{
+    return;
+}
+
+uni_platform* BTManager::get_bp32_driver() 
+{
+    static uni_platform driver = {
+        .name = "OGXMiniW",
+        .init = 
+            [](int argc, const char** argv)
+            { get_instance().init(argc, argv); },
+
+        .on_init_complete = 
+            [](void)
+            { get_instance().init_complete_cb(); },
+
+        .on_device_discovered = 
+            [](bd_addr_t addr, const char* name, uint16_t cod, uint8_t rssi) 
+            { return get_instance().device_discovered_cb(addr, name, cod, rssi); },
+
+        .on_device_connected = 
+            [](uni_hid_device_t* device) 
+            { get_instance().device_connected_cb(device); },
+
+        .on_device_disconnected = 
+            [](uni_hid_device_t* device) 
+            { get_instance().device_disconnected_cb(device); },
+
+        .on_device_ready = 
+            [](uni_hid_device_t* device) 
+            { return get_instance().device_ready_cb(device); }, 
+
+        .on_controller_data = 
+            [](uni_hid_device_t* device, uni_controller_t* controller) 
+            { get_instance().controller_data_cb(device, controller); },
+
+        .get_property = 
+            [](uni_property_idx_t idx) 
+            {  return get_instance().get_property_cb(idx); },
+
+        .on_oob_event = 
+            [](uni_platform_oob_event_t event, void* data) 
+            { get_instance().oob_event_cb(event, data); },
+    };
+    return &driver;
+}
\ No newline at end of file
diff --git a/Firmware/ESP32/main/Bluepad32/Bluepad32.cpp b/Firmware/ESP32/main/Bluepad32/Bluepad32.cpp
deleted file mode 100644
index dcb6d68..0000000
--- a/Firmware/ESP32/main/Bluepad32/Bluepad32.cpp
+++ /dev/null
@@ -1,338 +0,0 @@
-#include <functional>
-
-#include "btstack_port_esp32.h"
-#include "btstack_run_loop.h"
-#include "btstack_stdio_esp32.h"
-#include "uni.h"
-
-#include "sdkconfig.h"
-#include "I2CDriver/I2CDriver.h"
-#include "Board/board_api.h"
-#include "Bluepad32/Gamepad.h"
-#include "Bluepad32/Bluepad32.h"
-
-namespace BP32 {
-
-static constexpr uint8_t  MAX_DEVICES = CONFIG_BLUEPAD32_MAX_DEVICES;
-static constexpr uint32_t FEEDBACK_TIME_MS = 200;
-static constexpr uint32_t LED_TIME_MS = 500;
-
-I2CDriver i2c_driver_;
-btstack_timer_source_t feedback_timer_;
-std::atomic<bool> devs_conn_[MAX_DEVICES]{false};
-
-static inline void send_feedback_cb(void* context)
-{
-    I2CDriver::PacketOut packet_out = reinterpret_cast<std::atomic<I2CDriver::PacketOut>*>(context)->load();
-    uni_hid_device_t* bp_device = nullptr;
-
-    if (!(bp_device = uni_hid_device_get_instance_for_idx(packet_out.index)) || 
-        !uni_bt_conn_is_connected(&bp_device->conn) ||
-        !bp_device->report_parser.play_dual_rumble)
-    {
-        return;
-    }
-
-    if (packet_out.rumble_l || packet_out.rumble_r)
-    {
-        bp_device->report_parser.play_dual_rumble(
-            bp_device, 
-            0, 
-            FEEDBACK_TIME_MS, 
-            packet_out.rumble_l, 
-            packet_out.rumble_r
-            );
-    }
-}
-
-//This will have to be changed once full support for multiple devices is added
-static inline void feedback_timer_cb(btstack_timer_source *ts)
-{
-    static btstack_context_callback_registration_t cb_registration[MAX_DEVICES];
-    static std::atomic<I2CDriver::PacketOut> packets_out[MAX_DEVICES];
-
-    uni_hid_device_t* bp_device = nullptr;
-
-    for (uint8_t i = 0; i < MAX_DEVICES; ++i)
-    {
-        if (!(bp_device = uni_hid_device_get_instance_for_idx(i)) || 
-            !uni_bt_conn_is_connected(&bp_device->conn))
-        {
-            continue;
-        }
-
-        cb_registration[i].callback = send_feedback_cb;
-        cb_registration[i].context = reinterpret_cast<void*>(&packets_out[i]);
-        
-        //Register a read on i2c thread, with callback to send feedback on btstack thread
-        i2c_driver_.i2c_read_blocking_safe( I2CDriver::MULTI_SLAVE ? i + 1 : 0x01,
-                                            [i](const I2CDriver::PacketOut& packet_out)
-                                            {
-                                                packets_out[i].store(packet_out);
-                                                btstack_run_loop_execute_on_main_thread(&cb_registration[i]);
-                                            });
-    }
-
-    btstack_run_loop_set_timer(ts, FEEDBACK_TIME_MS);
-    btstack_run_loop_add_timer(ts);
-}
-
-inline void check_led_cb(btstack_timer_source *ts)
-{
-    static bool led_state = false;
-    led_state = !led_state;
-
-    if constexpr (board_api::NUM_LEDS == 1)
-    {
-        board_api::set_led(any_connected() ? 1 : (led_state ? 1 : 0));
-    }
-    else
-    {
-        for (uint8_t i = 0; i < board_api::NUM_LEDS; ++i)
-        {
-            board_api::set_led(i, devs_conn_[i].load() ? 1 : (led_state ? 1 : 0));
-        }
-    }
-
-    btstack_run_loop_set_timer(ts, LED_TIME_MS);
-    btstack_run_loop_add_timer(ts);
-}
-
-//BT Driver
-
-static void init(int argc, const char** arg_V) {}
-
-static void init_complete_cb(void) 
-{
-    uni_bt_enable_new_connections_unsafe(true);
-
-    // Based on runtime condition, you can delete or list the stored BT keys.
-    if (1)
-    {
-        uni_bt_del_keys_unsafe();
-    }
-    else
-    {
-        uni_bt_list_keys_unsafe();
-    }
-
-    uni_property_dump_all();
-}
-
-static uni_error_t device_discovered_cb(bd_addr_t addr, const char* name, uint16_t cod, uint8_t rssi) 
-{
-    if (!((cod & UNI_BT_COD_MINOR_MASK) & UNI_BT_COD_MINOR_GAMEPAD))
-    {
-        return UNI_ERROR_IGNORE_DEVICE;
-    }
-    return UNI_ERROR_SUCCESS;
-}
-
-static void device_connected_cb(uni_hid_device_t* device) 
-{
-#ifdef CONFIG_BLUEPAD32_USB_CONSOLE_ENABLE
-    logd("BP32", "Device connected, addr:  %p, index: %i\n", device, uni_hid_device_get_idx_for_instance(device));
-#endif
-}
-
-void device_disconnected_cb(uni_hid_device_t* device) 
-{
-#ifdef CONFIG_BLUEPAD32_USB_CONSOLE_ENABLE
-    logd("BP32", "Device disconnected, addr:  %p, index: %i\n", device, uni_hid_device_get_idx_for_instance(device));
-#endif
-
-    int idx = uni_hid_device_get_idx_for_instance(device);
-    if (idx >= MAX_DEVICES || idx < 0)
-    {
-        return;
-    }
-
-    devs_conn_[idx].store(false);
-    if (!any_connected())
-    {
-        btstack_run_loop_remove_timer(&feedback_timer_);
-    }
-
-    I2CDriver::PacketIn packet_in = I2CDriver::PacketIn();
-    packet_in.index = static_cast<uint8_t>(idx);
-    
-    i2c_driver_.i2c_write_blocking_safe(I2CDriver::MULTI_SLAVE ? packet_in.index + 1 : 0x01, packet_in);
-}
-
-static uni_error_t device_ready_cb(uni_hid_device_t* device) 
-{
-    int idx = uni_hid_device_get_idx_for_instance(device);
-    if (idx >= MAX_DEVICES || idx < 0)
-    {
-        return UNI_ERROR_IGNORE_DEVICE;
-    }
-
-    devs_conn_[idx].store(true);
-
-    feedback_timer_.process = feedback_timer_cb;
-    feedback_timer_.context = nullptr;
-
-    btstack_run_loop_set_timer(&feedback_timer_, FEEDBACK_TIME_MS);
-    btstack_run_loop_add_timer(&feedback_timer_);
-
-    return UNI_ERROR_SUCCESS;
-}
-
-static inline void controller_data_cb(uni_hid_device_t* device, uni_controller_t* controller) 
-{
-    static uni_gamepad_t prev_uni_gps[MAX_DEVICES]{0};
-
-    if (controller->klass != UNI_CONTROLLER_CLASS_GAMEPAD)
-    {
-        return;
-    }
-
-    uni_gamepad_t *uni_gp = &controller->gamepad;
-    int idx = uni_hid_device_get_idx_for_instance(device);
-
-    if (idx < 0 || std::memcmp(uni_gp, &prev_uni_gps[idx], sizeof(uni_gamepad_t)) == 0)
-    {
-        return;
-    }
-
-    I2CDriver::PacketIn packet_in;
-    packet_in.index = static_cast<uint8_t>(idx);
-
-    switch (uni_gp->dpad) 
-    {
-        case DPAD_UP:
-            packet_in.dpad = Gamepad::DPad::UP;
-            break;
-        case DPAD_DOWN:
-            packet_in.dpad = Gamepad::DPad::DOWN;
-            break;
-        case DPAD_LEFT:
-            packet_in.dpad = Gamepad::DPad::LEFT;
-            break;
-        case DPAD_RIGHT:
-            packet_in.dpad = Gamepad::DPad::RIGHT;
-            break;
-        case (DPAD_UP | DPAD_RIGHT):
-            packet_in.dpad = Gamepad::DPad::UP_RIGHT;
-            break;
-        case (DPAD_DOWN | DPAD_RIGHT):
-            packet_in.dpad = Gamepad::DPad::DOWN_RIGHT;
-            break;
-        case (DPAD_DOWN | DPAD_LEFT):
-            packet_in.dpad = Gamepad::DPad::DOWN_LEFT;
-            break;
-        case (DPAD_UP | DPAD_LEFT):
-            packet_in.dpad = Gamepad::DPad::UP_LEFT;
-            break;
-        default:
-            break;
-    }
-
-    if (uni_gp->buttons & BUTTON_A) packet_in.buttons |= Gamepad::Button::A;
-    if (uni_gp->buttons & BUTTON_B) packet_in.buttons |= Gamepad::Button::B;
-    if (uni_gp->buttons & BUTTON_X) packet_in.buttons |= Gamepad::Button::X;
-    if (uni_gp->buttons & BUTTON_Y) packet_in.buttons |= Gamepad::Button::Y;
-    if (uni_gp->buttons & BUTTON_SHOULDER_L) packet_in.buttons |= Gamepad::Button::LB;
-    if (uni_gp->buttons & BUTTON_SHOULDER_R) packet_in.buttons |= Gamepad::Button::RB;
-    if (uni_gp->buttons & BUTTON_THUMB_L)    packet_in.buttons |= Gamepad::Button::L3;
-    if (uni_gp->buttons & BUTTON_THUMB_R)    packet_in.buttons |= Gamepad::Button::R3;
-    if (uni_gp->misc_buttons & MISC_BUTTON_BACK)    packet_in.buttons |= Gamepad::Button::BACK;
-    if (uni_gp->misc_buttons & MISC_BUTTON_START)   packet_in.buttons |= Gamepad::Button::START;
-    if (uni_gp->misc_buttons & MISC_BUTTON_SYSTEM)  packet_in.buttons |= Gamepad::Button::SYS;
-    if (uni_gp->misc_buttons & MISC_BUTTON_CAPTURE) packet_in.buttons |= Gamepad::Button::MISC;
-
-    packet_in.trigger_l = Scale::uint10_to_uint8(uni_gp->brake);
-    packet_in.trigger_r = Scale::uint10_to_uint8(uni_gp->throttle);
-
-    packet_in.joystick_lx = Scale::int10_to_int16(uni_gp->axis_x);
-    packet_in.joystick_ly = Scale::int10_to_int16(uni_gp->axis_y);
-    packet_in.joystick_rx = Scale::int10_to_int16(uni_gp->axis_rx);
-    packet_in.joystick_ry = Scale::int10_to_int16(uni_gp->axis_ry);
-
-    i2c_driver_.i2c_write_blocking_safe(I2CDriver::MULTI_SLAVE ? packet_in.index + 1 : 0x01, packet_in);
-
-    std::memcpy(&prev_uni_gps[idx], uni_gp, sizeof(uni_gamepad_t));
-}
-
-static const uni_property_t* get_property_cb(uni_property_idx_t idx) 
-{
-    return nullptr;
-}
-
-static void oob_event_cb(uni_platform_oob_event_t event, void* data) 
-{
-    return;
-}
-
-uni_platform* get_driver() 
-{
-    static uni_platform driver = 
-    {
-        .name = "OGXMiniW",
-        .init = init,
-        .on_init_complete = init_complete_cb,
-        .on_device_discovered = device_discovered_cb,
-        .on_device_connected = device_connected_cb,
-        .on_device_disconnected = device_disconnected_cb,
-        .on_device_ready = device_ready_cb,
-        .on_controller_data = controller_data_cb,
-        .get_property = get_property_cb,
-        .on_oob_event = oob_event_cb,
-    };
-    return &driver;
-}
-
-void run_i2c_task(void* parameter)
-{
-    i2c_driver_.initialize_i2c();
-    i2c_driver_.run_tasks();
-}
-
-//Public
-
-bool any_connected()
-{
-    for (auto& connected : devs_conn_)
-    {
-        if (connected.load())
-        {
-            return true;
-        }
-    }
-    return false;
-}
-
-bool connected(uint8_t index)
-{
-    return devs_conn_[index].load();
-}
-
-void run_task()
-{
-    board_api::init_pins(); 
-
-    xTaskCreatePinnedToCore(
-        run_i2c_task,
-        "i2c",
-        2048 * 2,
-        nullptr,
-        configMAX_PRIORITIES-8,
-        nullptr,
-        1 );
-    
-    btstack_init();
-
-    uni_platform_set_custom(get_driver());
-    uni_init(0, nullptr);
-
-    btstack_timer_source_t led_timer;
-    led_timer.process = check_led_cb;
-    led_timer.context = nullptr;
-
-    btstack_run_loop_set_timer(&led_timer, LED_TIME_MS);
-    btstack_run_loop_add_timer(&led_timer);
-
-    btstack_run_loop_execute();
-}
-
-} // namespace BP32
\ No newline at end of file
diff --git a/Firmware/ESP32/main/Bluepad32/Bluepad32.h b/Firmware/ESP32/main/Bluepad32/Bluepad32.h
deleted file mode 100644
index 3c7d2cc..0000000
--- a/Firmware/ESP32/main/Bluepad32/Bluepad32.h
+++ /dev/null
@@ -1,13 +0,0 @@
-#ifndef _BLUEPAD32_DRIVER_H_
-#define _BLUEPAD32_DRIVER_H_
-
-#include <cstdint>
-
-namespace BP32
-{
-    void run_task();
-    bool any_connected();
-    bool connected(uint8_t index);
-}
-
-#endif // _BLUEPAD32_DRIVER_H_
\ No newline at end of file
diff --git a/Firmware/ESP32/main/Bluepad32/Gamepad.h b/Firmware/ESP32/main/Bluepad32/Gamepad.h
deleted file mode 100644
index 6bcb7f4..0000000
--- a/Firmware/ESP32/main/Bluepad32/Gamepad.h
+++ /dev/null
@@ -1,100 +0,0 @@
-#ifndef GAMEPAD_H
-#define GAMEPAD_H
-
-#include <cstdint>
-
-namespace Gamepad 
-{
-    namespace DPad
-    {
-        static constexpr uint8_t UP    = 0x01;
-        static constexpr uint8_t DOWN  = 0x02;
-        static constexpr uint8_t LEFT  = 0x04;
-        static constexpr uint8_t RIGHT = 0x08;
-        static constexpr uint8_t UP_LEFT    = UP | LEFT;
-        static constexpr uint8_t UP_RIGHT   = UP | RIGHT;
-        static constexpr uint8_t DOWN_LEFT  = DOWN | LEFT;
-        static constexpr uint8_t DOWN_RIGHT = DOWN | RIGHT;
-        static constexpr uint8_t NONE = 0x00;
-    }
-    namespace Button 
-    {
-        static constexpr uint16_t A     = 0x0001;
-        static constexpr uint16_t B     = 0x0002;
-        static constexpr uint16_t X     = 0x0004;
-        static constexpr uint16_t Y     = 0x0008;
-        static constexpr uint16_t L3    = 0x0010;
-        static constexpr uint16_t R3    = 0x0020;
-        static constexpr uint16_t BACK  = 0x0040;
-        static constexpr uint16_t START = 0x0080;
-        static constexpr uint16_t LB    = 0x0100;
-        static constexpr uint16_t RB    = 0x0200;
-        static constexpr uint16_t SYS   = 0x0400;
-        static constexpr uint16_t MISC  = 0x0800;
-    }
-
-} // namespace Gamepad
-
-namespace Scale 
-{
-    namespace INT_16
-    {
-        static constexpr int16_t MIN = INT16_MIN;
-        static constexpr int16_t MID = 0;
-        static constexpr int16_t MAX = INT16_MAX;
-    }
-    namespace UINT_16
-    {
-        static constexpr uint16_t MIN = 0;
-        static constexpr uint16_t MID = 0x8000;
-        static constexpr uint16_t MAX = 0xFFFF;
-    }
-    namespace UINT_8
-    {
-        static constexpr uint8_t  MAX  = 0xFF;
-        static constexpr uint8_t  MID  = 0x80;
-        static constexpr uint8_t  MIN  = 0x00;
-    }
-    namespace INT_10
-    {
-        static constexpr int32_t  MIN  = -512;
-        static constexpr int32_t  MAX  = 511;
-    }
-    namespace UINT_10
-    {
-        static constexpr int32_t MAX = 1023;
-    }
-
-    static inline int16_t int10_to_int16(int32_t value)
-    {
-        constexpr int32_t scale_factor = INT_16::MAX - INT_16::MIN;
-        constexpr int32_t range = INT_10::MAX - INT_10::MIN;
-
-        if (value >= INT_10::MAX)
-        {
-            return INT_16::MAX;
-        }
-        else if (value <= INT_10::MIN)
-        {
-            return INT_16::MIN;
-        }
-
-        int32_t scaled_value = (value - INT_10::MIN) * scale_factor;
-        return static_cast<int16_t>(scaled_value / range + INT_16::MIN);
-    }
-    static inline uint8_t uint10_to_uint8(int32_t value)
-    {
-        if (value > UINT_10::MAX) 
-        {
-            value = UINT_10::MAX;
-        }
-        else if (value < 0) 
-        {
-            value = 0;
-        }
-        return static_cast<uint8_t>(value >> 2);
-    }
-    
-} // namespace Scale
-
-#endif // GAMEPAD_H
\ No newline at end of file
diff --git a/Firmware/ESP32/main/Board/board_api.cpp b/Firmware/ESP32/main/Board/board_api.cpp
index 7534e3e..2d55014 100644
--- a/Firmware/ESP32/main/Board/board_api.cpp
+++ b/Firmware/ESP32/main/Board/board_api.cpp
@@ -9,18 +9,18 @@
 namespace board_api {
 
 SemaphoreHandle_t leds_mutex_ = nullptr;
-SemaphoreHandle_t reset_mutex_ = nullptr;
+// SemaphoreHandle_t reset_mutex_ = nullptr;
 
-void init_pins()
+void init_board()
 {
     if (leds_mutex_ == nullptr)
     {
         leds_mutex_ = xSemaphoreCreateMutex();
     }
-    if (reset_mutex_ == nullptr)
-    {
-        reset_mutex_ = xSemaphoreCreateMutex();
-    }
+    // if (reset_mutex_ == nullptr)
+    // {
+    //     reset_mutex_ = xSemaphoreCreateMutex();
+    // }
 
     if (xSemaphoreTake(leds_mutex_, portMAX_DELAY))
     {
diff --git a/Firmware/ESP32/main/Board/board_api.h b/Firmware/ESP32/main/Board/board_api.h
index 37989a3..08d55f5 100644
--- a/Firmware/ESP32/main/Board/board_api.h
+++ b/Firmware/ESP32/main/Board/board_api.h
@@ -12,7 +12,7 @@ namespace board_api
 {
     static constexpr gpio_num_t RESET_PIN = static_cast<gpio_num_t>(CONFIG_RESET_PIN);
     static_assert(RESET_PIN < MAX_GPIO_NUM, "Invalid RESET_PIN");
-    
+
     static constexpr gpio_num_t LED_PINS[] =
     {
     #if defined(CONFIG_ENABLE_LED_1) && (CONFIG_LED_PIN_1 < MAX_GPIO_NUM) && (CONFIG_BLUEPAD32_MAX_DEVICES > 0)
@@ -35,7 +35,7 @@ namespace board_api
 
     static constexpr uint8_t NUM_LEDS = sizeof(LED_PINS) / sizeof(LED_PINS[0]);
 
-    void init_pins();
+    void init_board();
     void set_led(uint8_t index, bool state);
     void set_led(bool state);
 }
diff --git a/Firmware/ESP32/main/Board/ogxm_log.h b/Firmware/ESP32/main/Board/ogxm_log.h
new file mode 100644
index 0000000..d67985d
--- /dev/null
+++ b/Firmware/ESP32/main/Board/ogxm_log.h
@@ -0,0 +1,56 @@
+#ifndef _OGXM_LOG_H_
+#define _OGXM_LOG_H_
+
+#include <esp_log.h>
+#if ESP_LOG_LEVEL >= ESP_LOG_INFO
+
+#include <cstdint>
+#include <string>
+#include <sstream>
+#include <iomanip>
+#include <stdarg.h>
+
+#include "uni.h"
+
+namespace OGXM 
+{
+    static inline void log(const std::string& message)
+    {
+        logi(message.c_str());
+    }
+
+    static inline void log(const char* fmt, ...)
+    {
+        va_list args;
+        va_start(args, fmt);
+        uni_logv(fmt, args);
+        va_end(args);
+    }
+
+    static inline void log_hex(const std::string& message, const uint8_t* data, const size_t len)
+    {
+        log(message);
+
+        std::ostringstream hex_stream;
+        hex_stream << std::hex << std::setfill('0');
+        for (uint16_t i = 0; i < len; ++i)
+        {
+            hex_stream << std::setw(2) << static_cast<int>(data[i]) << " ";
+        }
+
+        log(hex_stream.str());
+    }
+
+} // namespace OGXM
+
+#define OGXM_LOG OGXM::log
+#define OGXM_LOG_HEX OGXM::log_hex
+
+#else // ESP_LOG_LEVEL >= ESP_LOG_INFO
+
+#define OGXM_LOG(...)
+#define OGXM_LOG_HEX(...)
+
+#endif // ESP_LOG_LEVEL >= ESP_LOG_INFO
+
+#endif // _OGXM_LOG_H_
\ No newline at end of file
diff --git a/Firmware/ESP32/main/CMakeLists.txt b/Firmware/ESP32/main/CMakeLists.txt
index ff93d79..bee4fda 100644
--- a/Firmware/ESP32/main/CMakeLists.txt
+++ b/Firmware/ESP32/main/CMakeLists.txt
@@ -1,10 +1,16 @@
+include(${CMAKE_CURRENT_SOURCE_DIR}/../../FWDefines.cmake)
+
 idf_component_register( 
     SRCS    
         "main.c" 
-        "c_wrapper.cpp"
+        "main.cpp"
         "Board/board_api.cpp"
-        "Bluepad32/Bluepad32.cpp"
+        "BTManager/BTManager.cpp"
+        "BTManager/BTManager_BP32.cpp"
+        "BLEServer/BLEServer.cpp"
         "I2CDriver/I2CDriver.cpp"
+        "UserSettings/UserSettings.cpp"
+        "UserSettings/UserProfile.cpp"
     INCLUDE_DIRS 
         "."
     REQUIRES 
@@ -12,4 +18,9 @@ idf_component_register(
         btstack 
         driver 
         nvs_flash
+)
+
+target_compile_definitions(${COMPONENT_LIB} PRIVATE
+        FIRMWARE_NAME=\"${FW_NAME}\"
+        FIRMWARE_VERSION=\"${FW_VERSION}\"
 )
\ No newline at end of file
diff --git a/Firmware/ESP32/main/Gamepad.h b/Firmware/ESP32/main/Gamepad.h
new file mode 100644
index 0000000..7666174
--- /dev/null
+++ b/Firmware/ESP32/main/Gamepad.h
@@ -0,0 +1,246 @@
+#ifndef GAMEPAD_H
+#define GAMEPAD_H
+
+#include <cstdint>
+
+#include "sdkconfig.h"
+#include "UserSettings/UserProfile.h"
+#include "UserSettings/UserSettings.h"
+#include "Board/ogxm_log.h"
+
+#define MAX_GAMEPADS CONFIG_BLUEPAD32_MAX_DEVICES
+static_assert(  MAX_GAMEPADS > 0 && 
+                MAX_GAMEPADS <= 4, 
+                "MAX_GAMEPADS must be between 1 and 4");
+
+namespace INT_16
+{
+    static constexpr int16_t MIN = INT16_MIN;
+    static constexpr int16_t MID = 0;
+    static constexpr int16_t MAX = INT16_MAX;
+}
+namespace UINT_16
+{
+    static constexpr uint16_t MIN = 0;
+    static constexpr uint16_t MID = 0x8000;
+    static constexpr uint16_t MAX = 0xFFFF;
+}
+namespace UINT_8
+{
+    static constexpr uint8_t  MAX  = 0xFF;
+    static constexpr uint8_t  MID  = 0x80;
+    static constexpr uint8_t  MIN  = 0x00;
+}
+namespace INT_10
+{
+    static constexpr int32_t  MIN  = -512;
+    static constexpr int32_t  MAX  = 511;
+}
+namespace UINT_10
+{
+    static constexpr int32_t MAX = 1023;
+}
+
+namespace Gamepad
+{
+    static constexpr uint8_t DPAD_UP         = 0x01;
+    static constexpr uint8_t DPAD_DOWN       = 0x02;
+    static constexpr uint8_t DPAD_LEFT       = 0x04;
+    static constexpr uint8_t DPAD_RIGHT      = 0x08;
+    static constexpr uint8_t DPAD_UP_LEFT    = DPAD_UP | DPAD_LEFT;
+    static constexpr uint8_t DPAD_UP_RIGHT   = DPAD_UP | DPAD_RIGHT;
+    static constexpr uint8_t DPAD_DOWN_LEFT  = DPAD_DOWN | DPAD_LEFT;
+    static constexpr uint8_t DPAD_DOWN_RIGHT = DPAD_DOWN | DPAD_RIGHT;
+    static constexpr uint8_t DPAD_NONE       = 0x00;
+
+    static constexpr uint16_t BUTTON_A     = 0x0001;
+    static constexpr uint16_t BUTTON_B     = 0x0002;
+    static constexpr uint16_t BUTTON_X     = 0x0004;
+    static constexpr uint16_t BUTTON_Y     = 0x0008;
+    static constexpr uint16_t BUTTON_L3    = 0x0010;
+    static constexpr uint16_t BUTTON_R3    = 0x0020;
+    static constexpr uint16_t BUTTON_BACK  = 0x0040;
+    static constexpr uint16_t BUTTON_START = 0x0080;
+    static constexpr uint16_t BUTTON_LB    = 0x0100;
+    static constexpr uint16_t BUTTON_RB    = 0x0200;
+    static constexpr uint16_t BUTTON_SYS   = 0x0400;
+    static constexpr uint16_t BUTTON_MISC  = 0x0800;
+}
+
+class GamepadMapper
+{
+public:
+    uint8_t DPAD_UP         = Gamepad::DPAD_UP        ;
+    uint8_t DPAD_DOWN       = Gamepad::DPAD_DOWN      ;
+    uint8_t DPAD_LEFT       = Gamepad::DPAD_LEFT      ;
+    uint8_t DPAD_RIGHT      = Gamepad::DPAD_RIGHT     ;
+    uint8_t DPAD_UP_LEFT    = Gamepad::DPAD_UP_LEFT   ;
+    uint8_t DPAD_UP_RIGHT   = Gamepad::DPAD_UP_RIGHT  ;
+    uint8_t DPAD_DOWN_LEFT  = Gamepad::DPAD_DOWN_LEFT ;
+    uint8_t DPAD_DOWN_RIGHT = Gamepad::DPAD_DOWN_RIGHT;
+    uint8_t DPAD_NONE       = Gamepad::DPAD_NONE      ;
+
+    uint16_t BUTTON_A     = Gamepad::BUTTON_A    ;
+    uint16_t BUTTON_B     = Gamepad::BUTTON_B    ;
+    uint16_t BUTTON_X     = Gamepad::BUTTON_X    ;
+    uint16_t BUTTON_Y     = Gamepad::BUTTON_Y    ;
+    uint16_t BUTTON_L3    = Gamepad::BUTTON_L3   ;
+    uint16_t BUTTON_R3    = Gamepad::BUTTON_R3   ;
+    uint16_t BUTTON_BACK  = Gamepad::BUTTON_BACK ;
+    uint16_t BUTTON_START = Gamepad::BUTTON_START;
+    uint16_t BUTTON_LB    = Gamepad::BUTTON_LB   ;
+    uint16_t BUTTON_RB    = Gamepad::BUTTON_RB   ;
+    uint16_t BUTTON_SYS   = Gamepad::BUTTON_SYS  ;
+    uint16_t BUTTON_MISC  = Gamepad::BUTTON_MISC ;
+
+    GamepadMapper() = default;
+    ~GamepadMapper() = default;
+
+    void set_profile(const UserProfile& profile)
+    {
+        set_profile_options(profile);
+        set_profile_mappings(profile);
+        set_profile_deadzones(profile);
+    }
+
+    inline int16_t joystick_ly(int32_t value_10bit) 
+    {
+        int16_t value = int10_to_int16(value_10bit);
+        value = profile_invert_ly_ ? invert_joy(value) : value;
+        return (value <= dz_.joystick_l_neg || value >= dz_.joystick_l_pos) ? value : INT_16::MID;
+    };
+
+    inline int16_t joystick_lx(int32_t value_10bit) 
+    {
+        int16_t value = int10_to_int16(value_10bit);
+        return (value <= dz_.joystick_l_neg || value >= dz_.joystick_l_pos) ? value : INT_16::MID;
+    };
+
+    inline int16_t joystick_ry(int32_t value_10bit) 
+    {
+        int16_t value = int10_to_int16(value_10bit);
+        value = profile_invert_ry_ ? invert_joy(value) : value;
+        return (value <= dz_.joystick_r_neg || value >= dz_.joystick_r_pos) ? value : INT_16::MID;
+    };
+
+    inline int16_t joystick_rx(int32_t value_10bit) 
+    {
+        int16_t value = int10_to_int16(value_10bit);
+        return (value <= dz_.joystick_r_neg || value >= dz_.joystick_r_pos) ? value : INT_16::MID;
+    };
+
+    inline uint8_t trigger_l(int32_t value_10bit) 
+    {
+        uint8_t value = uint10_to_uint8(value_10bit);
+        return (value >= dz_.trigger_l) ? value : UINT_8::MIN;
+    };
+
+    inline uint8_t trigger_r(int32_t value_10bit) 
+    {
+        uint8_t value = uint10_to_uint8(value_10bit);
+        return (value >= dz_.trigger_r) ? value : UINT_8::MIN;
+    };
+
+private:
+    bool profile_invert_ly_{false};
+    bool profile_invert_ry_{false};
+
+    struct Deadzones
+    {
+        uint8_t trigger_l{0};
+        uint8_t trigger_r{0};
+        int16_t joystick_l_neg{0};
+        int16_t joystick_l_pos{0};
+        int16_t joystick_r_neg{0};
+        int16_t joystick_r_pos{0};
+    } dz_;
+
+    void set_profile_options(const UserProfile& profile)
+    {
+        profile_invert_ly_ = profile.invert_ly ? true : false;
+        profile_invert_ry_ = profile.invert_ry ? true : false;
+    }
+
+    void set_profile_mappings(const UserProfile& profile)
+    {
+        DPAD_UP         = profile.dpad_up;
+        DPAD_DOWN       = profile.dpad_down;
+        DPAD_LEFT       = profile.dpad_left;
+        DPAD_RIGHT      = profile.dpad_right;
+        DPAD_UP_LEFT    = profile.dpad_up | profile.dpad_left;
+        DPAD_UP_RIGHT   = profile.dpad_up | profile.dpad_right;
+        DPAD_DOWN_LEFT  = profile.dpad_down | profile.dpad_left;
+        DPAD_DOWN_RIGHT = profile.dpad_down | profile.dpad_right;
+        DPAD_NONE       = 0;
+
+        BUTTON_A     = profile.button_a;
+        BUTTON_B     = profile.button_b;
+        BUTTON_X     = profile.button_x;
+        BUTTON_Y     = profile.button_y;
+        BUTTON_L3    = profile.button_l3;
+        BUTTON_R3    = profile.button_r3;
+        BUTTON_BACK  = profile.button_back;
+        BUTTON_START = profile.button_start;
+        BUTTON_LB    = profile.button_lb;
+        BUTTON_RB    = profile.button_rb;
+        BUTTON_SYS   = profile.button_sys;
+        BUTTON_MISC  = profile.button_misc;
+
+        OGXM_LOG("Mappings: A: %i B: %i X: %i Y: %i L3: %i R3: %i BACK: %i START: %i LB: %i RB: %i SYS: %i MISC: %i\n", 
+            BUTTON_A, BUTTON_B, BUTTON_X, BUTTON_Y, BUTTON_L3, BUTTON_R3, BUTTON_BACK, BUTTON_START, BUTTON_LB, BUTTON_RB, BUTTON_SYS, BUTTON_MISC);
+    }
+
+    void set_profile_deadzones(const UserProfile& profile) //Deadzones in the profile are 0-255 (0-100%)
+    {
+        dz_.trigger_l = profile.dz_trigger_l;
+        dz_.trigger_r = profile.dz_trigger_r;
+
+        dz_.joystick_l_pos = uint8_to_int16(profile.dz_joystick_l / 2);
+        dz_.joystick_l_neg = invert_joy(dz_.joystick_l_pos);
+        dz_.joystick_r_pos = uint8_to_int16(profile.dz_joystick_r / 2);
+        dz_.joystick_r_neg = invert_joy(dz_.joystick_r_pos);
+
+        OGXM_LOG("Deadzones: TL: %i TR: %i JL: %i JR: %i\n", dz_.trigger_l, dz_.trigger_r, dz_.joystick_l_pos, dz_.joystick_r_pos);
+    }
+
+    static inline int16_t int10_to_int16(int32_t value)
+    {
+        constexpr int32_t scale_factor = INT_16::MAX - INT_16::MIN;
+        constexpr int32_t range = INT_10::MAX - INT_10::MIN;
+
+        if (value >= INT_10::MAX)
+        {
+            return INT_16::MAX;
+        }
+        else if (value <= INT_10::MIN)
+        {
+            return INT_16::MIN;
+        }
+
+        int32_t scaled_value = (value - INT_10::MIN) * scale_factor;
+        return static_cast<int16_t>(scaled_value / range + INT_16::MIN);
+    }
+    static inline uint8_t uint10_to_uint8(int32_t value)
+    {
+        if (value > UINT_10::MAX) 
+        {
+            value = UINT_10::MAX;
+        }
+        else if (value < 0) 
+        {
+            value = 0;
+        }
+        return static_cast<uint8_t>(value >> 2);
+    }
+    static inline int16_t uint8_to_int16(uint8_t value)
+    {
+        return static_cast<int16_t>((static_cast<int32_t>(value) << 8) - UINT_16::MID);
+    }
+    static inline int16_t invert_joy(int16_t value) 
+    {    
+        return (value == INT_16::MIN) ? INT_16::MAX : -value; 
+    }
+
+}; // class GamepadMapper
+
+#endif // GAMEPAD_H
\ No newline at end of file
diff --git a/Firmware/ESP32/main/I2CDriver/I2CDriver.cpp b/Firmware/ESP32/main/I2CDriver/I2CDriver.cpp
index d66d109..493e29a 100644
--- a/Firmware/ESP32/main/I2CDriver/I2CDriver.cpp
+++ b/Firmware/ESP32/main/I2CDriver/I2CDriver.cpp
@@ -5,27 +5,35 @@
 #include <esp_log.h>
 
 #include "I2CDriver/I2CDriver.h"
-#include "Bluepad32/Bluepad32.h"
 
 I2CDriver::~I2CDriver()
 {
-    i2c_driver_delete(I2C_NUM_0);
+    i2c_driver_delete(i2c_port_);
 }
 
-void I2CDriver::initialize_i2c()
+void I2CDriver::initialize_i2c(i2c_port_t i2c_port, gpio_num_t sda, gpio_num_t scl, uint32_t clk_speed)
 {
+    if (initialized_)
+    {
+        i2c_driver_delete(i2c_port_);
+    }
+
+    i2c_port_ = i2c_port;
+
     i2c_config_t conf;
     std::memset(&conf, 0, sizeof(i2c_config_t));
 
     conf.mode = I2C_MODE_MASTER;
-    conf.sda_io_num = GPIO_NUM_21;
-    conf.scl_io_num = GPIO_NUM_22;
+    conf.sda_io_num = sda;
+    conf.scl_io_num = scl;
     conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
     conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
-    conf.master.clk_speed = 1000 * 1000;
+    conf.master.clk_speed = clk_speed;
 
-    i2c_param_config(I2C_NUM_0, &conf);
-    i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0);
+    i2c_param_config(i2c_port_, &conf);
+    i2c_driver_install(i2c_port_, conf.mode, 0, 0, 0);
+
+    initialized_ = true;
 }
 
 void I2CDriver::run_tasks()
@@ -41,4 +49,24 @@ void I2CDriver::run_tasks()
 
         vTaskDelay(1);
     }
+}
+
+void I2CDriver::write_packet(uint8_t address, const PacketIn& data_in) 
+{
+    task_queue_.push([this, address, data_in]() 
+    {
+        i2c_write_blocking(address, reinterpret_cast<const uint8_t*>(&data_in), sizeof(PacketIn));
+    });
+}
+
+void I2CDriver::read_packet(uint8_t address, std::function<void(const PacketOut&)> callback) 
+{
+    task_queue_.push([this, address, callback]() 
+    {
+        PacketOut data_out;
+        if (i2c_read_blocking(address, reinterpret_cast<uint8_t*>(&data_out), sizeof(PacketOut)) == ESP_OK)
+        {
+            callback(data_out);
+        }
+    });
 }
\ No newline at end of file
diff --git a/Firmware/ESP32/main/I2CDriver/I2CDriver.h b/Firmware/ESP32/main/I2CDriver/I2CDriver.h
index 82d2b7f..05b88e2 100644
--- a/Firmware/ESP32/main/I2CDriver/I2CDriver.h
+++ b/Firmware/ESP32/main/I2CDriver/I2CDriver.h
@@ -8,6 +8,7 @@
 
 #include "sdkconfig.h"
 #include "RingBuffer.h"
+#include "UserSettings/DeviceDriverTypes.h"
 
 class I2CDriver 
 {
@@ -19,47 +20,36 @@ public:
         true;
 #endif
 
-    enum class PacketID : uint8_t { UNKNOWN = 0, SET_PAD, GET_PAD };
+    enum class PacketID : uint8_t { UNKNOWN = 0, SET_PAD, GET_PAD, SET_DRIVER };
+    enum class PacketResp : uint8_t { OK = 1, ERROR };
 
     #pragma pack(push, 1)
     struct PacketIn
     {
-        uint8_t packet_len;
-        uint8_t packet_id;
-        uint8_t index;
-        uint8_t dpad;
-        uint16_t buttons;
-        uint8_t trigger_l;
-        uint8_t trigger_r;
-        int16_t joystick_lx;
-        int16_t joystick_ly;
-        int16_t joystick_rx;
-        int16_t joystick_ry;
-
-        PacketIn()
-        {
-            std::memset(this, 0, sizeof(PacketIn));
-            packet_len = sizeof(PacketIn);
-            packet_id = static_cast<uint8_t>(PacketID::SET_PAD);
-        }
+        uint8_t packet_len{sizeof(PacketIn)};
+        PacketID packet_id{static_cast<uint8_t>(PacketID::SET_PAD)};
+        uint8_t index{0};
+        DeviceDriverType device_driver{DeviceDriverType::NONE};
+        uint8_t dpad{0};
+        uint16_t buttons{0};
+        uint8_t trigger_l{0};
+        uint8_t trigger_r{0};
+        int16_t joystick_lx{0};
+        int16_t joystick_ly{0};
+        int16_t joystick_rx{0};
+        int16_t joystick_ry{0};
+        std::array<uint8_t, 15> reserved1{0};
     };
-    static_assert(sizeof(PacketIn) == 16, "PacketIn is misaligned");
+    static_assert(sizeof(PacketIn) == 32, "PacketIn is misaligned");
 
     struct PacketOut
     {
-        uint8_t packet_len;
-        uint8_t packet_id;
-        uint8_t index;
-        uint8_t rumble_l;
-        uint8_t rumble_r;
-        uint8_t reserved[3];
-
-        PacketOut()
-        {
-            std::memset(this, 0, sizeof(PacketOut));
-            packet_len = sizeof(PacketOut);
-            packet_id = static_cast<uint8_t>(PacketID::GET_PAD);
-        }
+        uint8_t packet_len{0};
+        PacketID packet_id{0};
+        uint8_t index{0};
+        uint8_t rumble_l{0};
+        uint8_t rumble_r{0};
+        std::array<uint8_t, 3> reserved{0};
     };
     static_assert(sizeof(PacketOut) == 8, "PacketOut is misaligned");
     #pragma pack(pop)
@@ -67,36 +57,22 @@ public:
     I2CDriver() = default;
     ~I2CDriver();
 
-    void initialize_i2c();
+    void initialize_i2c(i2c_port_t i2c_port, gpio_num_t sda, gpio_num_t scl, uint32_t clk_speed);
 
     //Does not return
     void run_tasks();
 
-    inline void i2c_write_blocking_safe(uint8_t address, const PacketIn& packet_in) 
-    {
-        task_queue_.push([this, address, packet_in]() 
-        {
-            i2c_write_blocking(address, reinterpret_cast<const uint8_t*>(&packet_in), sizeof(PacketIn));
-        });
-    }
-
-    inline void i2c_read_blocking_safe(uint8_t address, std::function<void(const PacketOut&)> callback) 
-    {
-        task_queue_.push([this, address, callback]() 
-        {
-            PacketOut packet_out;
-            if (i2c_read_blocking(address, reinterpret_cast<uint8_t*>(&packet_out), sizeof(PacketOut)) == ESP_OK)
-            {
-                callback(packet_out);
-            }
-        });
-    }
+    void write_packet(uint8_t address, const PacketIn& data_in);
+    void read_packet(uint8_t address, std::function<void(const PacketOut&)> callback);
 
 private:
-    using TaskQueue = RingBuffer<std::function<void()>, 6>;
+    using TaskQueue = RingBuffer<std::function<void()>, CONFIG_I2C_RING_BUFFER_SIZE>;
+    
     TaskQueue task_queue_;
+    i2c_port_t i2c_port_ = I2C_NUM_0;
+    bool initialized_ = false;
 
-    inline esp_err_t i2c_write_blocking(uint8_t address, const uint8_t* buffer, size_t len) 
+    static inline esp_err_t i2c_write_blocking(uint8_t address, const uint8_t* buffer, size_t len) 
     {
         i2c_cmd_handle_t cmd = i2c_cmd_link_create();
         i2c_master_start(cmd);
@@ -109,7 +85,7 @@ private:
         return ret;
     }
 
-    inline esp_err_t i2c_read_blocking(uint8_t address, uint8_t* buffer, size_t len) 
+    static inline esp_err_t i2c_read_blocking(uint8_t address, uint8_t* buffer, size_t len) 
     {
         i2c_cmd_handle_t cmd = i2c_cmd_link_create();
         i2c_master_start(cmd);
@@ -127,6 +103,6 @@ private:
         i2c_cmd_link_delete(cmd);
         return ret;
     }
-};
+}; // class I2CDriver
 
 #endif // _I2C_DRIVER_H_
\ No newline at end of file
diff --git a/Firmware/ESP32/main/Kconfig b/Firmware/ESP32/main/Kconfig
index 76774b6..3701397 100644
--- a/Firmware/ESP32/main/Kconfig
+++ b/Firmware/ESP32/main/Kconfig
@@ -1,5 +1,25 @@
 menu "OGXMini Options"
 
+    config I2C_RING_BUFFER_SIZE
+        int "Set I2C ring buffer size"
+        default 6
+
+    config I2C_PORT
+        int "Set I2C port"
+        default 0
+
+    config I2C_SDA_PIN
+        int "Set I2C SDA pin"
+        default 21
+
+    config I2C_SCL_PIN
+        int "Set I2C SCL pin"
+        default 22
+
+    config I2C_BAUDRATE
+        int "Set I2C baudrate"
+        default 1000000
+
     config RESET_PIN
         int "Set reset pin"
         default 9
diff --git a/Firmware/ESP32/main/RingBuffer.h b/Firmware/ESP32/main/RingBuffer.h
index 61cde39..23e7659 100644
--- a/Firmware/ESP32/main/RingBuffer.h
+++ b/Firmware/ESP32/main/RingBuffer.h
@@ -5,7 +5,7 @@
 #include <atomic>
 #include <array>
 
-template<typename Type, size_t Size>
+template<typename Type, size_t SIZE>
 class RingBuffer
 {
 public:
@@ -47,9 +47,7 @@ public:
     }
 
 private:
-    const size_t SIZE = Size;
-
-    std::array<Type, Size> buffer_;
+    std::array<Type, SIZE> buffer_;
     std::atomic<size_t> head_;
     std::atomic<size_t> tail_;
 };
diff --git a/Firmware/ESP32/main/UserSettings/DeviceDriverTypes.h b/Firmware/ESP32/main/UserSettings/DeviceDriverTypes.h
new file mode 100644
index 0000000..b560dac
--- /dev/null
+++ b/Firmware/ESP32/main/UserSettings/DeviceDriverTypes.h
@@ -0,0 +1,39 @@
+#ifndef _DEVICE_DRIVER_TYPES_H_
+#define _DEVICE_DRIVER_TYPES_H_
+
+#include <cstdint>
+
+enum class DeviceDriverType : uint8_t
+{
+    NONE = 0,
+    XBOXOG,
+    XBOXOG_SB,
+    XBOXOG_XR,
+    XINPUT,
+    PS3,
+    DINPUT,
+    PSCLASSIC,
+    SWITCH,
+    WEBAPP = 100,
+    UART_BRIDGE
+};
+
+static inline std::string DRIVER_NAME(DeviceDriverType driver)
+{
+    switch (driver)
+    {
+        case DeviceDriverType::XBOXOG: return "XBOX OG";
+        case DeviceDriverType::XBOXOG_SB: return "XBOX OG SB";
+        case DeviceDriverType::XBOXOG_XR: return "XBOX OG XR";
+        case DeviceDriverType::XINPUT: return "XINPUT";
+        case DeviceDriverType::PS3: return "PS3";
+        case DeviceDriverType::DINPUT: return "DINPUT";
+        case DeviceDriverType::PSCLASSIC: return "PS CLASSIC";
+        case DeviceDriverType::SWITCH: return "SWITCH";
+        case DeviceDriverType::WEBAPP: return "WEBAPP";
+        case DeviceDriverType::UART_BRIDGE: return "UART BRIDGE";
+        default: return "UNKNOWN";
+    }
+}
+
+#endif // _DEVICE_DRIVER_TYPES_H_
\ No newline at end of file
diff --git a/Firmware/ESP32/main/UserSettings/NVSHelper.h b/Firmware/ESP32/main/UserSettings/NVSHelper.h
new file mode 100644
index 0000000..c28ec49
--- /dev/null
+++ b/Firmware/ESP32/main/UserSettings/NVSHelper.h
@@ -0,0 +1,113 @@
+#ifndef _NVS_HELPER_H_
+#define _NVS_HELPER_H_
+
+#include <cstdint>
+#include <array>
+#include <string>
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
+#include <nvs_flash.h>
+#include <nvs.h>
+#include <esp_err.h>
+
+class NVSHelper
+{
+public:
+    static NVSHelper& get_instance()
+    {
+        static NVSHelper instance;
+        return instance;
+    }
+
+    esp_err_t write(const std::string& key, const void* value, size_t len)
+    {
+        esp_err_t err;
+        nvs_handle_t handle;
+
+        xSemaphoreTake(nvs_mutex_, portMAX_DELAY);
+
+        if ((err = nvs_open(NVS_NAMESPACE, NVS_READWRITE, &handle)) != ESP_OK) 
+        {
+            xSemaphoreGive(nvs_mutex_);
+            return err;
+        }
+
+        if ((err = nvs_set_blob(handle, key.c_str(), value, len)) != ESP_OK) 
+        {
+            nvs_close(handle);
+            xSemaphoreGive(nvs_mutex_);
+            return err;
+        } 
+
+        err = nvs_commit(handle);
+        nvs_close(handle);
+
+        xSemaphoreGive(nvs_mutex_);
+        return err;
+    }
+
+    esp_err_t read(const std::string& key, void* value, size_t len)
+    {
+        esp_err_t err;
+        nvs_handle_t handle;
+
+        xSemaphoreTake(nvs_mutex_, portMAX_DELAY);
+
+        if ((err = nvs_open(NVS_NAMESPACE, NVS_READWRITE, &handle)) != ESP_OK) 
+        {
+            xSemaphoreGive(nvs_mutex_);
+            return err;
+        }
+
+        err = nvs_get_blob(handle, key.c_str(), value, &len);
+        nvs_close(handle);
+
+        xSemaphoreGive(nvs_mutex_);
+        return err;
+    }
+
+    esp_err_t erase_all()
+    {
+        esp_err_t err;
+        nvs_handle_t handle;
+
+        xSemaphoreTake(nvs_mutex_, portMAX_DELAY);
+
+        if ((err = nvs_open(NVS_NAMESPACE, NVS_READWRITE, &handle)) != ESP_OK) 
+        {
+            xSemaphoreGive(nvs_mutex_);
+            return err;
+        }
+
+        err = nvs_erase_all(handle);
+        nvs_close(handle);
+
+        xSemaphoreGive(nvs_mutex_);
+        return err;
+    }
+
+private:
+    NVSHelper()
+    {
+        nvs_mutex_ = xSemaphoreCreateMutex();
+        xSemaphoreTake(nvs_mutex_, portMAX_DELAY);
+
+        if (nvs_flash_init() != ESP_OK) 
+        {
+            ESP_ERROR_CHECK(nvs_flash_erase());
+            ESP_ERROR_CHECK(nvs_flash_init());
+        }
+
+        xSemaphoreGive(nvs_mutex_);
+    }
+    ~NVSHelper() = default;
+    NVSHelper(const NVSHelper&) = delete;
+    NVSHelper& operator=(const NVSHelper&) = delete; 
+
+    SemaphoreHandle_t nvs_mutex_;
+
+    static constexpr const char NVS_NAMESPACE[] = "user_data";
+    
+}; // class NVSHelper
+
+#endif // _NVS_HELPER_H_
\ No newline at end of file
diff --git a/Firmware/ESP32/main/UserSettings/UserProfile.cpp b/Firmware/ESP32/main/UserSettings/UserProfile.cpp
new file mode 100644
index 0000000..1f77b59
--- /dev/null
+++ b/Firmware/ESP32/main/UserSettings/UserProfile.cpp
@@ -0,0 +1,48 @@
+#include <cstring>
+
+#include "Gamepad.h"
+#include "UserSettings/UserProfile.h"
+
+UserProfile::UserProfile()
+{
+    id = 1;
+
+    dz_trigger_l = 0;
+    dz_trigger_r = 0;
+    dz_joystick_l = 0;
+    dz_joystick_r = 0;
+
+    invert_ly = 0;
+    invert_ry = 0;
+
+    dpad_up = Gamepad::DPAD_UP;
+    dpad_down = Gamepad::DPAD_DOWN;
+    dpad_left = Gamepad::DPAD_LEFT;
+    dpad_right = Gamepad::DPAD_RIGHT;
+
+    button_a = Gamepad::BUTTON_A;
+    button_b = Gamepad::BUTTON_B;
+    button_x = Gamepad::BUTTON_X;
+    button_y = Gamepad::BUTTON_Y;
+    button_l3 = Gamepad::BUTTON_L3;
+    button_r3 = Gamepad::BUTTON_R3;
+    button_back = Gamepad::BUTTON_BACK;
+    button_start = Gamepad::BUTTON_START;
+    button_lb = Gamepad::BUTTON_LB;
+    button_rb = Gamepad::BUTTON_RB;
+    button_sys = Gamepad::BUTTON_SYS;
+    button_misc = Gamepad::BUTTON_MISC;
+
+    analog_enabled = 1;
+
+    analog_off_up = 0;
+    analog_off_down = 1;
+    analog_off_left = 2;
+    analog_off_right = 3;
+    analog_off_a = 4;
+    analog_off_b = 5;
+    analog_off_x = 6;
+    analog_off_y = 7;
+    analog_off_lb = 8;
+    analog_off_rb = 9;
+}
\ No newline at end of file
diff --git a/Firmware/ESP32/main/UserSettings/UserProfile.h b/Firmware/ESP32/main/UserSettings/UserProfile.h
new file mode 100644
index 0000000..2efa406
--- /dev/null
+++ b/Firmware/ESP32/main/UserSettings/UserProfile.h
@@ -0,0 +1,56 @@
+#ifndef _USER_PROFILE_H_
+#define _USER_PROFILE_H_
+
+#include <cstdint>
+
+#pragma pack(push, 1)
+struct UserProfile
+{
+    uint8_t id;
+
+    uint8_t dz_trigger_l;
+    uint8_t dz_trigger_r;
+
+    uint8_t dz_joystick_l;
+    uint8_t dz_joystick_r;
+
+    uint8_t invert_ly;
+    uint8_t invert_ry;
+
+    uint8_t dpad_up;
+    uint8_t dpad_down;
+    uint8_t dpad_left;
+    uint8_t dpad_right;
+
+    uint16_t button_a;
+    uint16_t button_b;
+    uint16_t button_x;
+    uint16_t button_y;
+    uint16_t button_l3;
+    uint16_t button_r3;
+    uint16_t button_back;
+    uint16_t button_start;
+    uint16_t button_lb;
+    uint16_t button_rb;
+    uint16_t button_sys;
+    uint16_t button_misc;
+
+    uint8_t analog_enabled;
+
+    uint8_t analog_off_up;
+    uint8_t analog_off_down;
+    uint8_t analog_off_left;
+    uint8_t analog_off_right;
+    uint8_t analog_off_a;
+    uint8_t analog_off_b;
+    uint8_t analog_off_x;
+    uint8_t analog_off_y;
+    uint8_t analog_off_lb;
+    uint8_t analog_off_rb;
+
+    UserProfile();
+};
+static_assert(sizeof(UserProfile) == 46, "UserProfile struct size mismatch");
+#pragma pack(pop)
+
+#endif // _USER_PROFILE_H_
\ No newline at end of file
diff --git a/Firmware/ESP32/main/UserSettings/UserSettings.cpp b/Firmware/ESP32/main/UserSettings/UserSettings.cpp
new file mode 100644
index 0000000..00e9852
--- /dev/null
+++ b/Firmware/ESP32/main/UserSettings/UserSettings.cpp
@@ -0,0 +1,267 @@
+#include <cstring>
+#include <array>
+#include <string>
+#include <nvs_flash.h>
+#include <nvs.h>
+#include <esp_err.h>
+#include <esp_log.h>
+
+#include "Gamepad.h"
+#include "UserSettings/NVSHelper.h"
+
+static constexpr uint32_t BUTTON_COMBO(const uint16_t& buttons, const uint8_t& dpad = 0)
+{
+    return (static_cast<uint32_t>(buttons) << 16) | static_cast<uint32_t>(dpad);
+}
+
+namespace ButtonCombo 
+{
+    static constexpr uint32_t PS3       = BUTTON_COMBO(Gamepad::BUTTON_START, Gamepad::DPAD_LEFT);
+    static constexpr uint32_t DINPUT    = BUTTON_COMBO(Gamepad::BUTTON_START | Gamepad::BUTTON_RB, Gamepad::DPAD_LEFT);
+    static constexpr uint32_t XINPUT    = BUTTON_COMBO(Gamepad::BUTTON_START, Gamepad::DPAD_UP);
+    static constexpr uint32_t SWITCH    = BUTTON_COMBO(Gamepad::BUTTON_START, Gamepad::DPAD_DOWN);
+    static constexpr uint32_t XBOXOG    = BUTTON_COMBO(Gamepad::BUTTON_START, Gamepad::DPAD_RIGHT);
+    static constexpr uint32_t XBOXOG_SB = BUTTON_COMBO(Gamepad::BUTTON_START | Gamepad::BUTTON_RB, Gamepad::DPAD_RIGHT);
+    static constexpr uint32_t XBOXOG_XR = BUTTON_COMBO(Gamepad::BUTTON_START | Gamepad::BUTTON_LB, Gamepad::DPAD_RIGHT);
+    static constexpr uint32_t PSCLASSIC = BUTTON_COMBO(Gamepad::BUTTON_START | Gamepad::BUTTON_A);
+    static constexpr uint32_t WEBAPP    = BUTTON_COMBO(Gamepad::BUTTON_START | Gamepad::BUTTON_LB | Gamepad::BUTTON_RB);
+};
+
+static constexpr DeviceDriverType VALID_DRIVER_TYPES[] =
+{
+#if MAX_GAMEPADS > 1
+    DeviceDriverType::DINPUT, 
+    DeviceDriverType::SWITCH, 
+
+#else // MAX_GAMEPADS == 1
+    DeviceDriverType::XBOXOG, 
+    DeviceDriverType::XBOXOG_SB, 
+    DeviceDriverType::XBOXOG_XR,
+    DeviceDriverType::DINPUT, 
+    DeviceDriverType::SWITCH, 
+    DeviceDriverType::PS3,
+    DeviceDriverType::PSCLASSIC, 
+    DeviceDriverType::XINPUT,
+
+#endif
+};
+
+struct ComboMap { uint32_t combo; DeviceDriverType driver; };
+static constexpr std::array<ComboMap, 9> BUTTON_COMBO_MAP = 
+{{
+    { ButtonCombo::XBOXOG,    DeviceDriverType::XBOXOG    },
+    { ButtonCombo::XBOXOG_SB, DeviceDriverType::XBOXOG_SB },
+    { ButtonCombo::XBOXOG_XR, DeviceDriverType::XBOXOG_XR },
+    { ButtonCombo::WEBAPP,    DeviceDriverType::WEBAPP    },
+    { ButtonCombo::DINPUT,    DeviceDriverType::DINPUT    },
+    { ButtonCombo::SWITCH,    DeviceDriverType::SWITCH    },
+    { ButtonCombo::XINPUT,    DeviceDriverType::XINPUT    },
+    { ButtonCombo::PS3,       DeviceDriverType::PS3       },
+    { ButtonCombo::PSCLASSIC, DeviceDriverType::PSCLASSIC }
+}};
+
+const std::string UserSettings::INIT_FLAG_KEY()
+{
+    return std::string("init_flag");
+}
+
+const std::string UserSettings::PROFILE_KEY(const uint8_t profile_id)
+{
+    return std::string("profile_") + std::to_string(profile_id);
+}
+
+const std::string UserSettings::ACTIVE_PROFILE_KEY(const uint8_t index)
+{
+    return std::string("active_id_") + std::to_string(index);
+}
+
+const std::string UserSettings::DRIVER_TYPE_KEY()
+{
+    return std::string("driver_type");
+}
+
+const std::string UserSettings::FIRMWARE_VER_KEY()
+{
+    return std::string("firmware_ver");
+}
+
+//Checks for first boot and initializes user profiles, call before tusb is inited.
+void UserSettings::initialize_flash()
+{
+    ESP_LOGD("UserSettings", "Checking for UserSettings init flag");
+
+    uint8_t init_flag = 0;
+
+    ESP_ERROR_CHECK(nvs_helper_.read(INIT_FLAG_KEY(), &init_flag, sizeof(init_flag)));
+
+    if (init_flag == INIT_FLAG)
+    {
+        ESP_LOGD("UserSettings", "UserSettings already initialized");
+        return;
+    }
+
+    current_driver_ = DEFAULT_DRIVER();
+    uint8_t driver_type = static_cast<uint8_t>(current_driver_);
+    ESP_ERROR_CHECK(nvs_helper_.write(DRIVER_TYPE_KEY(), &driver_type, sizeof(driver_type)));
+
+    uint8_t active_id = 1;
+    for (uint8_t i = 0; i < MAX_GAMEPADS; i++)
+    {
+        ESP_ERROR_CHECK(nvs_helper_.write(ACTIVE_PROFILE_KEY(i), &active_id, sizeof(active_id)));
+    }
+
+    UserProfile profile = UserProfile();
+    for (uint8_t i = 0; i < MAX_PROFILES; i++)
+    {
+        profile.id = i + 1;
+        ESP_ERROR_CHECK(nvs_helper_.write(PROFILE_KEY(profile.id), &profile, sizeof(UserProfile)));
+    }
+
+    init_flag = INIT_FLAG;
+    ESP_ERROR_CHECK(nvs_helper_.write(INIT_FLAG_KEY(), &init_flag, sizeof(init_flag)));
+}
+
+DeviceDriverType UserSettings::get_current_driver()
+{
+    if (current_driver_ != DeviceDriverType::NONE)
+    {
+        return current_driver_;
+    }
+
+    uint8_t stored_type = 0;
+    nvs_helper_.read(DRIVER_TYPE_KEY(), &stored_type, sizeof(stored_type));
+
+    if (is_valid_driver(static_cast<DeviceDriverType>(stored_type)))
+    {
+        current_driver_ = static_cast<DeviceDriverType>(stored_type);
+        return current_driver_;
+    }
+
+    current_driver_ = DEFAULT_DRIVER();
+    return current_driver_;
+}
+
+//Checks if button combo has been held for 3 seconds, returns true if mode has been changed
+bool UserSettings::check_for_driver_change(const I2CDriver::PacketIn& packet_in)
+{
+    static uint32_t last_button_combo = BUTTON_COMBO(packet_in.buttons, packet_in.dpad);
+    static uint8_t call_count = 0;
+
+    uint32_t current_button_combo = BUTTON_COMBO(packet_in.buttons, packet_in.dpad);
+
+    if (!(current_button_combo & (static_cast<uint32_t>(Gamepad::BUTTON_START) << 16)) || 
+        last_button_combo != current_button_combo)
+    {
+        last_button_combo = current_button_combo;
+        call_count = 0;
+        return false;
+    }
+
+    ++call_count;
+
+    if (call_count < GP_CHECK_COUNT)
+    {
+        return false;
+    }
+
+    call_count = 0;
+
+    DeviceDriverType new_driver = DeviceDriverType::NONE;
+
+    for (const auto& combo : BUTTON_COMBO_MAP)
+    {
+        if (combo.combo == current_button_combo &&
+            is_valid_driver(combo.driver))
+        {
+            new_driver = combo.driver;
+            break;
+        }
+    }
+
+    if (new_driver == DeviceDriverType::NONE || new_driver == current_driver_)
+    {
+        return false;
+    }
+
+    current_driver_ = new_driver;
+    return true;
+}
+
+void UserSettings::store_driver_type(DeviceDriverType new_driver_type)
+{
+    if (!is_valid_driver(new_driver_type))
+    {
+        return;
+    }
+
+    uint8_t new_driver = static_cast<uint8_t>(new_driver_type);
+    nvs_helper_.write(DRIVER_TYPE_KEY(), &new_driver, sizeof(new_driver));
+}
+
+void UserSettings::store_profile(const uint8_t index, const UserProfile& profile)
+{
+    if (index > MAX_GAMEPADS || profile.id < 1 || profile.id > MAX_PROFILES)
+    {
+        return;
+    }
+
+    if (nvs_helper_.write(PROFILE_KEY(profile.id), &profile, sizeof(UserProfile)) == ESP_OK)
+    {
+        nvs_helper_.write(ACTIVE_PROFILE_KEY(index), &profile.id, sizeof(profile.id));
+    }
+}
+
+void UserSettings::store_profile_and_driver_type(DeviceDriverType new_driver_type, const uint8_t index, const UserProfile& profile)
+{
+    store_driver_type(new_driver_type);
+    store_profile(index, profile);
+}
+
+uint8_t UserSettings::get_active_profile_id(const uint8_t index)
+{
+    uint8_t read_profile_id = 0;
+
+    if (nvs_helper_.read(ACTIVE_PROFILE_KEY(index), &read_profile_id, sizeof(read_profile_id)) != ESP_OK ||
+        read_profile_id < 1 || 
+        read_profile_id > MAX_PROFILES)
+    {
+        return 0x01;
+    }
+    return read_profile_id;
+}
+
+UserProfile UserSettings::get_profile_by_index(const uint8_t index)
+{
+    return get_profile_by_id(get_active_profile_id(index));
+}
+
+UserProfile UserSettings::get_profile_by_id(const uint8_t profile_id)
+{
+    UserProfile profile;
+
+    if (profile_id < 1 || 
+        profile_id > MAX_PROFILES ||
+        nvs_helper_.read(PROFILE_KEY(profile_id), &profile, sizeof(UserProfile)) != ESP_OK)
+    {
+        return profile;
+    }
+
+    return UserProfile();
+}
+
+DeviceDriverType UserSettings::DEFAULT_DRIVER()
+{
+    return VALID_DRIVER_TYPES[0];
+}
+
+bool UserSettings::is_valid_driver(DeviceDriverType mode)
+{
+    for (const auto& driver : VALID_DRIVER_TYPES)
+    {
+        if (mode == driver)
+        {
+            return true;
+        }
+    }
+    return false;
+}
\ No newline at end of file
diff --git a/Firmware/ESP32/main/UserSettings/UserSettings.h b/Firmware/ESP32/main/UserSettings/UserSettings.h
new file mode 100644
index 0000000..16f016a
--- /dev/null
+++ b/Firmware/ESP32/main/UserSettings/UserSettings.h
@@ -0,0 +1,60 @@
+#ifndef _USER_SETTINGS_H_
+#define _USER_SETTINGS_H_
+
+#include <cstdint>
+#include <atomic>
+
+#include "sdkconfig.h"
+#include "I2CDriver/I2CDriver.h"
+#include "UserSettings/UserProfile.h"
+#include "UserSettings/DeviceDriverTypes.h"
+#include "UserSettings/NVSHelper.h"
+
+class UserSettings
+{
+public:
+    static constexpr uint8_t MAX_PROFILES = 8;
+    static constexpr uint32_t GP_CHECK_DELAY_MS = 1000;
+
+    static UserSettings& get_instance()
+    {
+        static UserSettings instance;
+        return instance;
+    }
+
+    void initialize_flash();
+
+    DeviceDriverType get_current_driver();
+    bool check_for_driver_change(const I2CDriver::PacketIn& packet_in);
+    
+    UserProfile get_profile_by_index(const uint8_t index);
+    UserProfile get_profile_by_id(const uint8_t profile_id);
+    uint8_t get_active_profile_id(const uint8_t index);
+
+    void store_driver_type(DeviceDriverType new_driver_type);
+    void store_profile(uint8_t index, const UserProfile& profile);
+    void store_profile_and_driver_type(DeviceDriverType new_driver_type, uint8_t index, const UserProfile& profile);
+
+private:
+    UserSettings() = default;
+    ~UserSettings() = default;
+    UserSettings(const UserSettings&) = delete;
+    UserSettings& operator=(const UserSettings&) = delete;
+
+    static constexpr uint8_t GP_CHECK_COUNT = 3000 / GP_CHECK_DELAY_MS;
+    static constexpr uint8_t INIT_FLAG = 0x82;
+
+    NVSHelper& nvs_helper_{NVSHelper::get_instance()};
+    DeviceDriverType current_driver_{DeviceDriverType::NONE};
+
+    bool is_valid_driver(DeviceDriverType mode);
+
+    DeviceDriverType DEFAULT_DRIVER();
+    const std::string INIT_FLAG_KEY();
+    const std::string PROFILE_KEY(const uint8_t profile_id);
+    const std::string ACTIVE_PROFILE_KEY(const uint8_t index);
+    const std::string DRIVER_TYPE_KEY();
+    const std::string FIRMWARE_VER_KEY();
+};
+
+#endif // _USER_SETTINGS_H_
\ No newline at end of file
diff --git a/Firmware/ESP32/main/main.c b/Firmware/ESP32/main/main.c
index 09f9591..ca1710d 100644
--- a/Firmware/ESP32/main/main.c
+++ b/Firmware/ESP32/main/main.c
@@ -1,7 +1,7 @@
 #include <stdlib.h>
 #include <stdint.h>
 
-#include "c_wrapper.h"
+#include "main.h"
 
 void app_main(void) 
 {
diff --git a/Firmware/ESP32/main/c_wrapper.cpp b/Firmware/ESP32/main/main.cpp
similarity index 63%
rename from Firmware/ESP32/main/c_wrapper.cpp
rename to Firmware/ESP32/main/main.cpp
index 8ea03df..c4f057c 100644
--- a/Firmware/ESP32/main/c_wrapper.cpp
+++ b/Firmware/ESP32/main/main.cpp
@@ -2,19 +2,17 @@
 #include <freertos/FreeRTOS.h>
 #include <freertos/task.h>
 
-#include "c_wrapper.h"
+#include "main.h"
 #include "sdkconfig.h"
-#include "Bluepad32/Bluepad32.h"
-
-void bp32_run_task(void* parameter)
-{
-    BP32::run_task();
-}
+#include "BTManager/BTManager.h"
 
 void cpp_main()
 {
     xTaskCreatePinnedToCore(
-        bp32_run_task,
+        [](void* parameter)
+        { 
+            BTManager::get_instance().run_task(); 
+        },
         "bp32",
         2048 * 4,
         NULL,
diff --git a/Firmware/ESP32/main/c_wrapper.h b/Firmware/ESP32/main/main.h
similarity index 100%
rename from Firmware/ESP32/main/c_wrapper.h
rename to Firmware/ESP32/main/main.h
diff --git a/Firmware/ESP32/sdkconfig b/Firmware/ESP32/sdkconfig
index 35766dc..014fb2f 100644
--- a/Firmware/ESP32/sdkconfig
+++ b/Firmware/ESP32/sdkconfig
@@ -1381,20 +1381,17 @@ CONFIG_HEAP_TRACING_OFF=y
 #
 # Log output
 #
-CONFIG_LOG_DEFAULT_LEVEL_NONE=y
+# CONFIG_LOG_DEFAULT_LEVEL_NONE is not set
 # CONFIG_LOG_DEFAULT_LEVEL_ERROR is not set
 # CONFIG_LOG_DEFAULT_LEVEL_WARN is not set
-# CONFIG_LOG_DEFAULT_LEVEL_INFO is not set
+CONFIG_LOG_DEFAULT_LEVEL_INFO=y
 # CONFIG_LOG_DEFAULT_LEVEL_DEBUG is not set
 # CONFIG_LOG_DEFAULT_LEVEL_VERBOSE is not set
-CONFIG_LOG_DEFAULT_LEVEL=0
+CONFIG_LOG_DEFAULT_LEVEL=3
 CONFIG_LOG_MAXIMUM_EQUALS_DEFAULT=y
-# CONFIG_LOG_MAXIMUM_LEVEL_ERROR is not set
-# CONFIG_LOG_MAXIMUM_LEVEL_WARN is not set
-# CONFIG_LOG_MAXIMUM_LEVEL_INFO is not set
 # CONFIG_LOG_MAXIMUM_LEVEL_DEBUG is not set
 # CONFIG_LOG_MAXIMUM_LEVEL_VERBOSE is not set
-CONFIG_LOG_MAXIMUM_LEVEL=0
+CONFIG_LOG_MAXIMUM_LEVEL=3
 CONFIG_LOG_COLORS=y
 CONFIG_LOG_TIMESTAMP_SOURCE_RTOS=y
 # CONFIG_LOG_TIMESTAMP_SOURCE_SYSTEM is not set
@@ -1961,6 +1958,11 @@ CONFIG_WIFI_PROV_STA_ALL_CHANNEL_SCAN=y
 #
 # OGXMini Options
 #
+CONFIG_I2C_RING_BUFFER_SIZE=6
+CONFIG_I2C_PORT=0
+CONFIG_I2C_SDA_PIN=21
+CONFIG_I2C_SCL_PIN=22
+CONFIG_I2C_BAUDRATE=1000000
 CONFIG_RESET_PIN=9
 # CONFIG_MULTI_SLAVE_MODE is not set
 CONFIG_ENABLE_LED_1=y
@@ -1978,11 +1980,11 @@ CONFIG_BLUEPAD32_PLATFORM_CUSTOM=y
 # CONFIG_BLUEPAD32_PLATFORM_MAKEFILE is not set
 CONFIG_BLUEPAD32_MAX_DEVICES=1
 CONFIG_BLUEPAD32_GAP_SECURITY=y
-CONFIG_BLUEPAD32_LOG_LEVEL_NONE=y
+# CONFIG_BLUEPAD32_LOG_LEVEL_NONE is not set
 # CONFIG_BLUEPAD32_LOG_LEVEL_ERROR is not set
-# CONFIG_BLUEPAD32_LOG_LEVEL_INFO is not set
+CONFIG_BLUEPAD32_LOG_LEVEL_INFO=y
 # CONFIG_BLUEPAD32_LOG_LEVEL_DEBUG is not set
-CONFIG_BLUEPAD32_LOG_LEVEL=0
+CONFIG_BLUEPAD32_LOG_LEVEL=2
 # CONFIG_BLUEPAD32_USB_CONSOLE_ENABLE is not set
 CONFIG_BLUEPAD32_ENABLE_BLE_BY_DEFAULT=y
 CONFIG_BLUEPAD32_MAX_ALLOWLIST=4
diff --git a/Firmware/FWDefines.cmake b/Firmware/FWDefines.cmake
new file mode 100644
index 0000000..dfc6cda
--- /dev/null
+++ b/Firmware/FWDefines.cmake
@@ -0,0 +1,2 @@
+set(FW_NAME "OGX-Mini")
+set(FW_VERSION "v1.0.0a3")
\ No newline at end of file
diff --git a/Firmware/RP2040/.vscode/settings.json b/Firmware/RP2040/.vscode/settings.json
index 166f44d..26c31ef 100644
--- a/Firmware/RP2040/.vscode/settings.json
+++ b/Firmware/RP2040/.vscode/settings.json
@@ -1,15 +1,11 @@
 {
     "C_Cpp.default.configurationProvider": "ms-vscode.cmake-tools",
-    "cmake.buildEnvironment": {
-        "PICO_SDK_PATH": "C:/Programming/pico-sdk"
-    },
-    "cmake.environment": {
-        "PICO_SDK_PATH": "C:/Programming/pico-sdk"
-    },
+
     "cmake.configureArgs": [
-        "-DOGXM_BOARD=PI_PICO2",
+        "-DOGXM_BOARD=PI_PICOW",
         "-DMAX_GAMEPADS=1"
     ],
+    
     "files.associations": {
         "array": "cpp",
         "atomic": "cpp",
diff --git a/Firmware/RP2040/CMakeLists.txt b/Firmware/RP2040/CMakeLists.txt
index 807b34c..43f46d9 100644
--- a/Firmware/RP2040/CMakeLists.txt
+++ b/Firmware/RP2040/CMakeLists.txt
@@ -1,25 +1,26 @@
 cmake_minimum_required(VERSION 3.13)
 
-set(FW_NAME "OGX-Mini")
-set(FW_VERSION "v1.0.0a2")
+include(${CMAKE_CURRENT_LIST_DIR}/../FWDefines.cmake)
 
 set(CMAKE_C_COMPILER arm-none-eabi-gcc)
 set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
 
 set(CMAKE_C_STANDARD 11)
-set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD 20)
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
 set(SRC ${CMAKE_CURRENT_LIST_DIR}/src)
 set(EXTERNAL_DIR ${CMAKE_CURRENT_LIST_DIR}/../external)
+set(PICOSDK_VERSION_TAG "2.1.0")
 
-include(${EXTERNAL_DIR}/init_submodules.cmake)
-include(${EXTERNAL_DIR}/patch_libs.cmake)
+include(${CMAKE_CURRENT_LIST_DIR}/../cmake/init_submodules.cmake)
+include(${CMAKE_CURRENT_LIST_DIR}/../cmake/patch_libs.cmake)
+include(${CMAKE_CURRENT_LIST_DIR}/../cmake/generate_gatt_header.cmake)
 include(${CMAKE_CURRENT_LIST_DIR}/cmake/get_pico_sdk.cmake)
 
+get_pico_sdk(${EXTERNAL_DIR} ${PICOSDK_VERSION_TAG})
 init_git_submodules(${EXTERNAL_DIR})
 apply_lib_patches(${EXTERNAL_DIR})
-get_pico_sdk(${EXTERNAL_DIR})
 
 set(PICO_PIO_USB_PATH ${EXTERNAL_DIR}/Pico-PIO-USB)
 set(PICO_TINYUSB_PATH ${EXTERNAL_DIR}/tinyusb)
@@ -37,8 +38,14 @@ set(SOURCES_BOARD
 
     ${SRC}/TaskQueue/TaskQueue.cpp
 
+    ${SRC}/Board/ogxm_log.cpp
     ${SRC}/Board/board_api.cpp
-
+    ${SRC}/Board/board_api_private/board_api_led.cpp
+    ${SRC}/Board/board_api_private/board_api_rgb.cpp
+    ${SRC}/Board/board_api_private/board_api_bt.cpp
+    ${SRC}/Board/board_api_private/board_api_esp32.cpp
+    ${SRC}/Board/board_api_private/board_api_usbh.cpp
+    
     ${SRC}/UserSettings/UserSettings.cpp
     ${SRC}/UserSettings/UserProfile.cpp
 
@@ -97,6 +104,16 @@ elseif (OGXM_BOARD STREQUAL "PI_PICO2")
     set(PICO_PLATFORM rp2350)
     set(FLASH_SIZE_MB 4)
 
+elseif(OGXM_BOARD STREQUAL "PI_PICOW")
+    set(EN_BLUETOOTH TRUE)
+    set(PICO_BOARD pico_w)
+
+elseif(OGXM_BOARD STREQUAL "PI_PICO2W")
+    set(EN_BLUETOOTH TRUE)
+    set(PICO_BOARD pico2_w)
+    set(PICO_PLATFORM rp2350)
+    set(FLASH_SIZE_MB 4)
+
 elseif(OGXM_BOARD STREQUAL "ADA_FEATHER")
     set(EN_USB_HOST TRUE)
     set(EN_RGB TRUE)
@@ -115,11 +132,7 @@ elseif(OGXM_BOARD STREQUAL "EXTERNAL_4CH")
     set(EN_4CH TRUE)
     set(EN_RGB TRUE)
 
-elseif(OGXM_BOARD STREQUAL "PI_PICOW")
-    set(EN_BLUETOOTH TRUE)
-    set(PICO_BOARD pico_w)
-
-elseif(OGXM_BOARD STREQUAL "W_ESP32")
+elseif(OGXM_BOARD STREQUAL "PICO_ESP32")
     set(EN_ESP32 TRUE)
     set(EN_UART_BRIDGE TRUE)
 
@@ -168,8 +181,12 @@ endif()
 if(EN_BLUETOOTH)
     add_compile_definitions(CONFIG_EN_BLUETOOTH=1)
     message(STATUS "Bluetooth enabled.")
+
+    generate_gatt_header(${BTSTACK_ROOT} ${SRC}/BLEServer/att_delayed_response.gatt ${SRC}/BLEServer/att_delayed_response.h)
+    
     list(APPEND SOURCES_BOARD
         ${SRC}/Bluepad32/Bluepad32.cpp
+        ${SRC}/BLEServer/BLEServer.cpp
     )
     list(APPEND INC_DIRS_BOARD
         ${SRC}
@@ -229,8 +246,10 @@ if(EN_UART_BRIDGE)
     )
 endif()
 
-add_compile_definitions(PICO_FLASH_SIZE_BYTES=${FLASH_SIZE_MB}*1024*1024)
+add_compile_definitions(FIRMWARE_NAME="${FW_NAME}")
 add_compile_definitions(FIRMWARE_VERSION="${FW_VERSION}")
+add_compile_definitions(PICO_FLASH_SIZE_BYTES=${FLASH_SIZE_MB}*1024*1024)
+add_compile_definitions(NVS_SECTORS=4)
 
 # Check for DVD dongle firmware
 if(EXISTS ${SRC}/USBDevice/DeviceDriver/XboxOG/tud_xid/tud_xid_xremote_rom.h)
@@ -246,8 +265,11 @@ endif()
 include(${PICO_SDK_PATH}/pico_sdk_init.cmake)
 
 message("PICO_SDK_VERSION_STRING: ${PICO_SDK_VERSION_STRING}")
-if(PICO_SDK_VERSION_STRING VERSION_LESS "2.1.0")
-    message(FATAL_ERROR "Raspberry Pi Pico SDK version 2.1.0 (or later) required. Your version is ${PICO_SDK_VERSION_STRING}")
+if(PICO_SDK_VERSION_STRING VERSION_LESS "${PICOSDK_VERSION_TAG}")
+    message(FATAL_ERROR 
+        "Raspberry Pi Pico SDK version ${PICOSDK_VERSION_TAG} (or later) required. 
+        Your version is ${PICO_SDK_VERSION_STRING}"
+    )
 endif()
 
 project(${FW_NAME} C CXX ASM)
@@ -259,16 +281,32 @@ add_executable(${FW_NAME} ${SOURCES_BOARD})
 set(BUILD_STR "")
 
 if(CMAKE_BUILD_TYPE STREQUAL "Debug")
+    if(OGXM_BOARD STREQUAL "PI_PICOW")
+        message(FATAL_ERROR "Debug build will not work with the Pico W currently")
+    endif()
+
+    message(STATUS "Debug build enabled.")
     set(BUILD_STR "-Debug")
+
+    set(TX_PIN 0)
+    set(RX_PIN 1)
+    set(UART_PORT)
+
+    include(${CMAKE_CURRENT_LIST_DIR}/cmake/get_pico_uart_port.cmake)
+    get_pico_uart_port(${TX_PIN} UART_PORT)
+
+    message(STATUS "UART port: ${UART_PORT}, TX: ${TX_PIN}, RX: ${RX_PIN}")
+
     pico_enable_stdio_uart(${FW_NAME} 1)
     target_compile_definitions(${FW_NAME} PRIVATE
-        PICO_DEFAULT_UART=1
-        PICO_DEFAULT_UART_TX_PIN=4
-        PICO_DEFAULT_UART_RX_PIN=5
+        PICO_DEFAULT_UART=${UART_PORT}
+        PICO_DEFAULT_UART_TX_PIN=${TX_PIN}
+        PICO_DEFAULT_UART_RX_PIN=${RX_PIN}
     )
-    add_compile_definitions(LOG=1)
-    add_compile_definitions(CFG_TUSB_DEBUG=1)
+ 
+    add_compile_definitions(CFG_TUSB_DEBUG=2)
     add_compile_definitions(OGXM_DEBUG=1)
+
     target_compile_options(${FW_NAME} PRIVATE
         -Wall                # Enable most warnings
         -Wextra              # Enable extra warnings
@@ -277,12 +315,13 @@ if(CMAKE_BUILD_TYPE STREQUAL "Debug")
         -Wno-unused-parameter # Disable warnings for unused parameters
         # -Werror              # Treat warnings as errors
     )
+
 elseif(CMAKE_BUILD_TYPE STREQUAL "Release")
     pico_enable_stdio_uart(${FW_NAME} 0)
     add_compile_definitions(CFG_TUSB_DEBUG=0)
     add_compile_options(
         -O3                     # Optimize for speed
-        -mcpu=cortex-m0plus     # Target ARM Cortex-M0+
+        # -mcpu=cortex-m0plus     # Target ARM Cortex-M0+
         -mthumb                 # Use Thumb instruction set
         -ffunction-sections     # Place each function in its own section
         -fdata-sections         # Place each data item in its own section
diff --git a/Firmware/RP2040/cmake/get_pico_sdk.cmake b/Firmware/RP2040/cmake/get_pico_sdk.cmake
index 0141c70..33d6c82 100644
--- a/Firmware/RP2040/cmake/get_pico_sdk.cmake
+++ b/Firmware/RP2040/cmake/get_pico_sdk.cmake
@@ -1,16 +1,36 @@
-function(get_pico_sdk EXTERNAL_DIR)
+function(get_pico_sdk EXTERNAL_DIR VERSION_TAG)
     if(NOT DEFINED ENV{PICO_SDK_PATH})
-        message("PICO_SDK_PATH not set, downloading Pico SDK...")
+        message("PICO_SDK_PATH not set")
         set(PICO_SDK_PATH ${EXTERNAL_DIR}/pico-sdk PARENT_SCOPE)
+
         if(NOT EXISTS ${PICO_SDK_PATH})
+            message("Cloning pico-sdk to ${PICO_SDK_PATH}")
             execute_process(
                 COMMAND git clone --recursive https://github.com/raspberrypi/pico-sdk.git
                 WORKING_DIRECTORY ${EXTERNAL_DIR}
             )
+
         endif()
+
+        execute_process(
+            COMMAND git fetch --tags
+            WORKING_DIRECTORY ENV{PICO_SDK_PATH}
+            )
+    
+        execute_process(
+            COMMAND git checkout tags/${VERSION_TAG}
+            WORKING_DIRECTORY ENV{PICO_SDK_PATH}
+            )
+    
+        execute_process(
+            COMMAND git submodule update --init --recursive 
+            WORKING_DIRECTORY ENV{PICO_SDK_PATH}
+            )
+
     else()
         message("Using PICO_SDK_PATH from environment: $ENV{PICO_SDK_PATH}")
-        set(PICO_SDK_PATH $ENV{PICO_SDK_PATH} PARENT_SCOPE)
+        set(PICO_SDK_PATH ENV{PICO_SDK_PATH} PARENT_SCOPE)
+
     endif()
 
     set(PICOTOOL_FETCH_FROM_GIT_PATH ${EXTERNAL_DIR}/picotool PARENT_SCOPE)
diff --git a/Firmware/RP2040/cmake/get_pico_uart_port.cmake b/Firmware/RP2040/cmake/get_pico_uart_port.cmake
new file mode 100644
index 0000000..ed4adf6
--- /dev/null
+++ b/Firmware/RP2040/cmake/get_pico_uart_port.cmake
@@ -0,0 +1,9 @@
+function(get_pico_uart_port TX_PIN_IN PICO_UART_PORT_OUT)
+    if(TX_PIN_IN EQUAL 0 OR TX_PIN_IN EQUAL 12 OR TX_PIN_IN EQUAL 16)
+        set(${PICO_UART_PORT_OUT} 0 PARENT_SCOPE)
+    elseif(TX_PIN_IN EQUAL 4 OR TX_PIN_IN EQUAL 8)
+        set(${PICO_UART_PORT_OUT} 1 PARENT_SCOPE)
+    else()
+        message(FATAL_ERROR "Invalid TX pin for Pico UART")
+    endif()
+endfunction()
\ No newline at end of file
diff --git a/Firmware/RP2040/src/BLEServer/BLEServer.cpp b/Firmware/RP2040/src/BLEServer/BLEServer.cpp
new file mode 100644
index 0000000..ab85c65
--- /dev/null
+++ b/Firmware/RP2040/src/BLEServer/BLEServer.cpp
@@ -0,0 +1,304 @@
+#include <cstring>
+#include <string>
+#include <algorithm>
+
+#include "att_delayed_response.h"
+#include "btstack.h"
+
+#include "BLEServer/BLEServer.h"
+#include "UserSettings/UserProfile.h"
+#include "UserSettings/UserSettings.h"
+#include "TaskQueue/TaskQueue.h"
+
+namespace BLEServer {
+
+static constexpr uint16_t PACKET_LEN_MAX = 18;
+
+namespace Handle
+{
+    static constexpr uint16_t FW_VERSION    = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789020_01_VALUE_HANDLE;
+    static constexpr uint16_t FW_NAME       = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789021_01_VALUE_HANDLE;
+
+    static constexpr uint16_t START_UPDATE  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789030_01_VALUE_HANDLE;
+    static constexpr uint16_t COMMIT_UPDATE = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789031_01_VALUE_HANDLE;
+
+    static constexpr uint16_t SETUP_PACKET  = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789040_01_VALUE_HANDLE;
+    static constexpr uint16_t PROFILE_PT1   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789041_01_VALUE_HANDLE;
+    static constexpr uint16_t PROFILE_PT2   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789042_01_VALUE_HANDLE;
+    static constexpr uint16_t PROFILE_PT3   = ATT_CHARACTERISTIC_12345678_1234_1234_1234_123456789043_01_VALUE_HANDLE;
+}
+
+namespace ADV
+{
+    // Flags general discoverable, BR/EDR not supported
+    static const uint8_t FLAGS[]    = { 0x02, 0x01, 0x06 };
+    static const uint8_t NAME_TYPE  = 0x09;
+
+    #pragma pack(push, 1)
+    struct Data
+    {
+        uint8_t flags[sizeof(FLAGS)];
+        uint8_t name_len;
+        uint8_t name_type;
+        uint8_t name[sizeof(FIRMWARE_NAME) - 1];
+
+        Data()
+        {
+            std::memcpy(flags, FLAGS, sizeof(flags));
+            name_len = sizeof(FIRMWARE_NAME);
+            name_type = NAME_TYPE;
+            std::string fw_name = FIRMWARE_NAME;
+            std::memcpy(name, fw_name.c_str(), std::min(sizeof(name), fw_name.size()));
+        }
+    };
+    static_assert(sizeof(Data) == 5 + sizeof(FIRMWARE_NAME) - 1, "BLEServer::ADV::Data struct size mismatch");
+    #pragma pack(pop)
+}
+
+#pragma pack(push, 1)
+struct SetupPacket
+{
+    uint8_t max_gamepads{1};
+    uint8_t index{0};
+    uint8_t device_type{0};
+    uint8_t profile_id{1};
+};
+static_assert(sizeof(SetupPacket) == 4, "BLEServer::SetupPacket struct size mismatch");
+#pragma pack(pop)
+
+SetupPacket setup_packet_;
+
+static int verify_write(const uint16_t buffer_size, const uint16_t expected_size, bool pending_write = false, bool expected_pending_write = false)
+{
+    if (buffer_size != expected_size)
+    {
+        return ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LENGTH;
+    }
+    if (pending_write != expected_pending_write)
+    {
+        return ATT_ERROR_WRITE_NOT_PERMITTED;
+    }
+    return 0;
+}
+
+static void disconnect_client_cb(btstack_timer_source_t *ts) 
+{
+    hci_con_handle_t connection_handle = *static_cast<hci_con_handle_t*>(ts->context);
+    hci_send_cmd(&hci_disconnect, connection_handle);
+    delete static_cast<hci_con_handle_t*>(ts->context);
+}
+
+static void queue_disconnect(hci_con_handle_t connection_handle, uint32_t dealy_ms)
+{
+    static btstack_timer_source_t disconnect_timer;
+
+    hci_con_handle_t* connection_handle_ptr = new hci_con_handle_t(connection_handle);
+
+    disconnect_timer.process = disconnect_client_cb;
+    disconnect_timer.context = connection_handle_ptr;
+
+    btstack_run_loop_set_timer(&disconnect_timer, dealy_ms);
+    btstack_run_loop_add_timer(&disconnect_timer);
+}
+
+static uint16_t att_read_callback(  hci_con_handle_t connection_handle,
+                                    uint16_t att_handle,
+                                    uint16_t offset,
+                                    uint8_t *buffer,
+                                    uint16_t buffer_size)
+{
+    static UserProfile profile;
+    SetupPacket setup_packet_resp;
+    std::string fw_version;
+    std::string fw_name;
+
+    switch (att_handle)
+    {
+        case Handle::FW_VERSION:
+            fw_version = FIRMWARE_VERSION;
+            if (buffer)
+            {
+                std::memcpy(buffer, reinterpret_cast<const uint8_t*>(fw_version.c_str()), fw_version.size());
+            }
+            return static_cast<uint16_t>(fw_version.size());
+
+        case Handle::FW_NAME:
+            fw_name = FIRMWARE_NAME;
+            if (buffer)
+            {
+                std::memcpy(buffer, reinterpret_cast<const uint8_t*>(fw_name.c_str()), fw_name.size());;
+            }
+            return static_cast<uint16_t>(fw_name.size());
+
+        case Handle::SETUP_PACKET:
+            if (buffer)
+            {
+                setup_packet_resp.max_gamepads = static_cast<uint8_t>(MAX_GAMEPADS);
+                setup_packet_resp.index = setup_packet_.index;
+                setup_packet_resp.device_type = static_cast<uint8_t>(UserSettings::get_instance().get_current_driver());
+                //App has already written a setup packet with the player index
+                setup_packet_resp.profile_id = UserSettings::get_instance().get_active_profile_id(setup_packet_.index);
+
+                std::memcpy(buffer, &setup_packet_resp, sizeof(setup_packet_resp));
+            }
+            return sizeof(setup_packet_);
+
+        case Handle::PROFILE_PT1:
+            if (buffer)
+            {
+                //App has already written the profile id it wants to the setup packet
+                profile = UserSettings::get_instance().get_profile_by_id(setup_packet_.profile_id);
+                std::memcpy(buffer, &profile, PACKET_LEN_MAX);
+            }
+            return PACKET_LEN_MAX;
+
+        case Handle::PROFILE_PT2:
+            if (buffer)
+            {
+                std::memcpy(buffer, reinterpret_cast<uint8_t*>(&profile) + PACKET_LEN_MAX, PACKET_LEN_MAX);
+            }
+            return PACKET_LEN_MAX;
+
+        case Handle::PROFILE_PT3:
+            if (buffer)
+            {
+                std::memcpy(buffer, reinterpret_cast<uint8_t*>(&profile) + PACKET_LEN_MAX * 2, sizeof(UserProfile) - PACKET_LEN_MAX * 2);
+            }
+            return sizeof(UserProfile) - PACKET_LEN_MAX * 2;
+
+        default:
+            break;
+    }
+    return 0;
+}
+
+static int att_write_callback(  hci_con_handle_t connection_handle,
+                                uint16_t att_handle,
+                                uint16_t transaction_mode,
+                                uint16_t offset,
+                                uint8_t *buffer,
+                                uint16_t buffer_size)
+{
+    static UserProfile temp_profile;
+    static DeviceDriverType temp_driver_type = DeviceDriverType::NONE;
+    static bool pending_write = false;
+
+    int ret = 0;
+
+    switch (att_handle)
+    {
+        case Handle::START_UPDATE:
+            pending_write = true;
+            break;
+
+        case Handle::SETUP_PACKET:
+            if ((ret = verify_write(buffer_size, sizeof(SetupPacket))) != 0)
+            {
+                break;
+            }
+
+            std::memcpy(&setup_packet_, buffer, buffer_size);
+            if (setup_packet_.index >= MAX_GAMEPADS)
+            {
+                setup_packet_.index = 0;
+                ret = ATT_ERROR_OUT_OF_RANGE;
+            }
+            if (setup_packet_.profile_id > UserSettings::MAX_PROFILES)
+            {
+                setup_packet_.profile_id = 1;
+                ret = ATT_ERROR_OUT_OF_RANGE;
+            }
+            if (ret)
+            {
+                break;
+            }
+
+            if (pending_write)
+            {
+                //App wants to store a new device driver type
+                temp_driver_type = static_cast<DeviceDriverType>(setup_packet_.device_type);
+            }
+            break;
+
+        case Handle::PROFILE_PT1:
+            if ((ret = verify_write(buffer_size, PACKET_LEN_MAX, pending_write, true)) != 0)
+            {
+                break;
+            }
+            std::memcpy(&temp_profile, buffer, buffer_size);
+            break;
+
+        case Handle::PROFILE_PT2:
+            if ((ret = verify_write(buffer_size, PACKET_LEN_MAX, pending_write, true)) != 0)
+            {
+                break;
+            }
+            std::memcpy(reinterpret_cast<uint8_t*>(&temp_profile) + PACKET_LEN_MAX, buffer, buffer_size);
+            break;
+
+        case Handle::PROFILE_PT3:
+            if ((ret = verify_write(buffer_size, sizeof(UserProfile) - PACKET_LEN_MAX * 2, pending_write, true)) != 0)
+            {
+                break;
+            }
+            std::memcpy(reinterpret_cast<uint8_t*>(&temp_profile) + PACKET_LEN_MAX * 2, buffer, buffer_size);
+            break;
+
+        case Handle::COMMIT_UPDATE:
+            if ((ret = verify_write(0, 0, pending_write, true)) != 0)
+            {
+                break;
+            }
+
+            //Delay until after we've returned
+            queue_disconnect(connection_handle, 500);
+
+            //We don't want to write to the flash from here, it'll reset core1 before writing, queue the task on core0
+            if (temp_driver_type != DeviceDriverType::NONE && temp_driver_type != UserSettings::get_instance().get_current_driver())
+            {
+                TaskQueue::Core0::queue_delayed_task(TaskQueue::Core0::get_new_task_id(), 1000, false,
+                []{
+                    UserSettings::get_instance().store_profile_and_driver_type(temp_driver_type, setup_packet_.index, temp_profile);
+                });
+            }
+            else
+            {
+                TaskQueue::Core0::queue_delayed_task(TaskQueue::Core0::get_new_task_id(), 1000, false,
+                []{
+                    UserSettings::get_instance().store_profile(setup_packet_.index, temp_profile);
+                });
+            }
+            
+            temp_driver_type = DeviceDriverType::NONE;
+            pending_write = false;
+            break;
+
+        default:
+            break;
+    }
+    return ret;
+}
+
+void init_server()
+{
+    UserSettings::get_instance().initialize_flash();
+
+    // setup ATT server
+    att_server_init(profile_data, att_read_callback, att_write_callback);
+
+    // setup advertisements
+    uint16_t adv_int_min = 0x0030;
+    uint16_t adv_int_max = 0x0030;
+    uint8_t adv_type = 0;
+
+    bd_addr_t null_addr;
+    std::memset(null_addr, 0, sizeof(null_addr));
+
+    static ADV::Data adv_data = ADV::Data();
+
+    gap_advertisements_set_params(adv_int_min, adv_int_max, adv_type, 0, null_addr, 0x07, 0x00);
+    gap_advertisements_set_data(static_cast<uint8_t>(sizeof(adv_data)), reinterpret_cast<uint8_t*>(&adv_data));
+    gap_advertisements_enable(1);
+}
+
+} // namespace BLEServer
\ No newline at end of file
diff --git a/Firmware/RP2040/src/BLEServer/BLEServer.h b/Firmware/RP2040/src/BLEServer/BLEServer.h
new file mode 100644
index 0000000..d72962e
--- /dev/null
+++ b/Firmware/RP2040/src/BLEServer/BLEServer.h
@@ -0,0 +1,11 @@
+#ifndef BLE_SERVER_H
+#define BLE_SERVER_H
+
+#include <cstdint>
+
+namespace BLEServer 
+{
+    void init_server();
+}
+
+#endif // BLE_SERVER_H
\ No newline at end of file
diff --git a/Firmware/RP2040/src/BLEServer/att_delayed_response.gatt b/Firmware/RP2040/src/BLEServer/att_delayed_response.gatt
new file mode 100644
index 0000000..4f059d3
--- /dev/null
+++ b/Firmware/RP2040/src/BLEServer/att_delayed_response.gatt
@@ -0,0 +1,32 @@
+PRIMARY_SERVICE, GAP_SERVICE
+CHARACTERISTIC, GAP_DEVICE_NAME, READ, "XPad"
+
+PRIMARY_SERVICE, GATT_SERVICE
+CHARACTERISTIC, GATT_DATABASE_HASH, READ,
+
+// Handle::PRIMARY_SERVICE
+PRIMARY_SERVICE, 12345678-1234-1234-1234-123456789012
+
+// Handle::FW_VERSION
+CHARACTERISTIC,  12345678-1234-1234-1234-123456789020, READ | DYNAMIC,
+
+// Handle::FW_NAME
+CHARACTERISTIC,  12345678-1234-1234-1234-123456789021, READ | DYNAMIC,
+
+// Handle::UPDATE_START
+CHARACTERISTIC,  12345678-1234-1234-1234-123456789030, WRITE | DYNAMIC,
+
+// Handle::UPDATE_COMMIT
+CHARACTERISTIC,  12345678-1234-1234-1234-123456789031, WRITE | DYNAMIC,
+
+// Handle::SETUP_PACKET
+CHARACTERISTIC,  12345678-1234-1234-1234-123456789040, READ | WRITE | DYNAMIC,
+
+// Handle::PROFILE_PT1
+CHARACTERISTIC,  12345678-1234-1234-1234-123456789041, READ | WRITE | DYNAMIC,
+
+// Handle::PROFILE_PT2
+CHARACTERISTIC,  12345678-1234-1234-1234-123456789042, READ | WRITE | DYNAMIC,
+
+// Handle::PROFILE_PT3
+CHARACTERISTIC,  12345678-1234-1234-1234-123456789043, READ | WRITE | DYNAMIC,
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Bluepad32/Bluepad32.cpp b/Firmware/RP2040/src/Bluepad32/Bluepad32.cpp
index 1745afb..d0df285 100644
--- a/Firmware/RP2040/src/Bluepad32/Bluepad32.cpp
+++ b/Firmware/RP2040/src/Bluepad32/Bluepad32.cpp
@@ -9,7 +9,9 @@
 
 #include "sdkconfig.h"
 #include "Bluepad32/Bluepad32.h"
+#include "BLEServer/BLEServer.h"
 #include "Board/board_api.h"
+#include "Board/ogxm_log.h"
 
 #ifndef CONFIG_BLUEPAD32_PLATFORM_CUSTOM
     #error "Pico W must use BLUEPAD32_PLATFORM_CUSTOM"
@@ -124,14 +126,7 @@ static void check_led_cb(btstack_timer_source *ts)
 
 static void init(int argc, const char** arg_V)
 {
-    if (!led_timer_set_)
-    {
-        led_timer_set_ = true;
-        led_timer_.process = check_led_cb;
-        led_timer_.context = nullptr;
-        btstack_run_loop_set_timer(&led_timer_, LED_CHECK_TIME_MS);
-        btstack_run_loop_add_timer(&led_timer_);
-    }
+
 }
 
 static void init_complete_cb(void) 
@@ -166,8 +161,9 @@ static void device_disconnected_cb(uni_hid_device_t* device)
     }
 
     bt_devices_[idx].connected = false;
+    bt_devices_[idx].gamepad->reset_pad_in();
 
-    if (!led_timer_set_)
+    if (!led_timer_set_ && !any_connected())
     {
         led_timer_set_ = true;
         led_timer_.process = check_led_cb;
@@ -271,13 +267,13 @@ static void controller_data_cb(uni_hid_device_t* device, uni_controller_t* contr
     if (uni_gp->misc_buttons & MISC_BUTTON_START)   gp_in.buttons |= gamepad->MAP_BUTTON_START;
     if (uni_gp->misc_buttons & MISC_BUTTON_SYSTEM)  gp_in.buttons |= gamepad->MAP_BUTTON_SYS;
 
-    gp_in.trigger_l = Scale::uint10_to_uint8(uni_gp->brake);
-    gp_in.trigger_r = Scale::uint10_to_uint8(uni_gp->throttle);
+    gp_in.trigger_l = gamepad->scale_trigger_l<10>(static_cast<uint16_t>(uni_gp->brake));
+    gp_in.trigger_r = gamepad->scale_trigger_r<10>(static_cast<uint16_t>(uni_gp->throttle));
 
-    gp_in.joystick_lx = Scale::int10_to_int16(uni_gp->axis_x);
-    gp_in.joystick_ly = Scale::int10_to_int16(uni_gp->axis_y);
-    gp_in.joystick_rx = Scale::int10_to_int16(uni_gp->axis_rx);
-    gp_in.joystick_ry = Scale::int10_to_int16(uni_gp->axis_ry);
+    gp_in.joystick_lx = gamepad->scale_joystick_lx<10>(uni_gp->axis_x);
+    gp_in.joystick_ly = gamepad->scale_joystick_ly<10>(uni_gp->axis_y);
+    gp_in.joystick_rx = gamepad->scale_joystick_rx<10>(uni_gp->axis_rx);
+    gp_in.joystick_ry = gamepad->scale_joystick_ry<10>(uni_gp->axis_ry);
 
     gamepad->set_pad_in(gp_in);
 }
@@ -314,9 +310,17 @@ void run_task(Gamepad (&gamepads)[MAX_GAMEPADS])
         bt_devices_[i].gamepad = &gamepads[i];
     }
 
+    BLEServer::init_server();
+
     uni_platform_set_custom(get_driver());
     uni_init(0, nullptr);
 
+    led_timer_set_ = true;
+    led_timer_.process = check_led_cb;
+    led_timer_.context = nullptr;
+    btstack_run_loop_set_timer(&led_timer_, LED_CHECK_TIME_MS);
+    btstack_run_loop_add_timer(&led_timer_);
+
     btstack_run_loop_execute();
 }
 
diff --git a/Firmware/RP2040/src/Board/board_api.cpp b/Firmware/RP2040/src/Board/board_api.cpp
index 0810249..04f90bf 100644
--- a/Firmware/RP2040/src/Board/board_api.cpp
+++ b/Firmware/RP2040/src/Board/board_api.cpp
@@ -1,185 +1,99 @@
 #include <pico/stdlib.h>
 #include <pico/mutex.h>
+#include <pico/multicore.h>
 #include <hardware/clocks.h>
-#include <hardware/gpio.h>
-#include <hardware/watchdog.h>
 
-#include "Board/board_api.h"
-#include "OGXMini/Debug.h"
+#include "tusb.h"
+
 #include "board_config.h"
-
-#if defined(CONFIG_EN_BLUETOOTH)
-#include <pico/cyw43_arch.h>
-#endif // defined(CONFIG_EN_BLUETOOTH)
-
-#if defined(CONFIG_EN_RGB)
-#include "WS2812.pio.h"
-#include "Board/Pico_WS2812/WS2812.hpp"
-#endif // defined(CONFIG_EN_RGB)
+#include "Board/board_api.h"
+#include "Board/ogxm_log.h"
+#include "Board/board_api_private/board_api_private.h"
 
 namespace board_api {
 
-bool inited_ = false;
 mutex_t gpio_mutex_;
 
-void init_vcc_en_pin_unsafe()
+bool esp32::uart_bridge_mode()
 {
-#if defined(VCC_EN_PIN)
-    gpio_init(VCC_EN_PIN);
-    gpio_set_dir(VCC_EN_PIN, GPIO_OUT);
-    gpio_put(VCC_EN_PIN, 1);
-#endif 
+    bool ret = false;
+    if (board_api_esp32::uart_bridge_mode)
+    {
+        mutex_enter_blocking(&gpio_mutex_);
+        ret = board_api_esp32::uart_bridge_mode();
+        mutex_exit(&gpio_mutex_);
+    }
+    return ret;
 }
 
-void init_rgb_unsafe()
+void esp32::enter_programming_mode()
 {
-#if defined(CONFIG_EN_RGB) && defined(RGB_PWR_PIN)
-        gpio_init(RGB_PWR_PIN);
-        gpio_set_dir(RGB_PWR_PIN, GPIO_OUT);
-        gpio_put(RGB_PWR_PIN, 1);
-#endif // defined(CONFIG_EN_RGB) && defined(RGB_PWR_PIN)
+    if (board_api_esp32::enter_programming_mode)
+    {
+        mutex_enter_blocking(&gpio_mutex_);
+        board_api_esp32::enter_programming_mode();
+        mutex_exit(&gpio_mutex_);
+    }
 }
 
-void init_led_indicator_unsafe()
+void esp32::reset()
 {
-#if defined(LED_INDICATOR_PIN) && !defined(CONFIG_EN_BLUETOOTH)   
-    gpio_init(LED_INDICATOR_PIN);
-    gpio_set_dir(LED_INDICATOR_PIN, GPIO_OUT);
-    gpio_put(LED_INDICATOR_PIN, 0);
-#endif // defined(LED_INDICATOR_PIN)
+    if (board_api_esp32::reset)
+    {
+        mutex_enter_blocking(&gpio_mutex_);
+        board_api_esp32::reset();
+        mutex_exit(&gpio_mutex_);
+    }
 }
 
-void init_esp32_io_unsafe()
+bool usb::host_connected()
 {
-#if defined(CONFIG_EN_ESP32)
-    gpio_init(ESP_PROG_PIN);
-    gpio_set_dir(ESP_PROG_PIN, GPIO_OUT);
-    gpio_put(ESP_PROG_PIN, 1);
-
-    gpio_init(ESP_RST_PIN);
-    gpio_set_dir(ESP_RST_PIN, GPIO_OUT);
-    gpio_put(ESP_RST_PIN, 1);
-
-#endif //defined(CONFIG_EN_ESP32)
+    if (board_api_usbh::host_connected)
+    {
+        return board_api_usbh::host_connected();
+    }
+    return false;
 }
 
-void init_uart_bridge_io_unsafe()
+//Only call this from core0
+void usb::disconnect_all()
 {
-#if defined(CONFIG_EN_UART_BRIDGE)
-    gpio_init(MODE_SEL_PIN);
-    gpio_set_dir(MODE_SEL_PIN, GPIO_IN);
-    gpio_pull_up(MODE_SEL_PIN);
-
-#endif // defined(CONFIG_EN_UART_BRIDGE)
-}
-
-void init_uart_debug_unsafe()
-{
-#if defined(OGXM_DEBUG)
-    uart_init(DEBUG_UART_PORT, PICO_DEFAULT_UART_BAUD_RATE);
-    gpio_set_function(PICO_DEFAULT_UART_TX_PIN, GPIO_FUNC_UART);
-    gpio_set_function(PICO_DEFAULT_UART_RX_PIN, GPIO_FUNC_UART);
-#endif // defined(OGXM_DEBUG)
+    OGXM_LOG("Disconnecting USB and resetting Core1\n");
+    tud_disconnect();
+    sleep_ms(300);
+    multicore_reset_core1();
+    sleep_ms(300);
 }
 
+// If using PicoW, only use this method from the core running btstack and after you've called init_bluetooth
 void set_led(bool state)
 {
     mutex_enter_blocking(&gpio_mutex_);
 
-    if (!inited_)
+    if (board_api_led::set_led)
     {
-        mutex_exit(&gpio_mutex_);
-        return;
+        board_api_led::set_led(state);
+    }
+    if (board_api_bt::set_led)
+    {
+        board_api_bt::set_led(state);
+    }
+    if (board_api_rgb::set_led)
+    {
+        board_api_rgb::set_led(state ? 0x00 : 0xFF, state ? 0xFF : 0x00, 0x00);
     }
-
-#if defined(CONFIG_EN_RGB)
-    static WS2812 ws2812 = WS2812(RGB_PXL_PIN, 1, pio1, 0, WS2812::FORMAT_GRB);
-
-    ws2812.setPixelColor(0, state ? WS2812::RGB(0x00, 0xFF, 0x00) : WS2812::RGB(0xFF, 0, 0));
-    ws2812.show();
-
-#endif // defined(CONFIG_EN_RGB)
-
-#if defined(CONFIG_EN_BLUETOOTH)
-    cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, state ? 1 : 0);
-
-#elif defined(LED_INDICATOR_PIN)  
-    gpio_put(LED_INDICATOR_PIN, state ? 1 : 0);
-
-#endif //defined(CONFIG_EN_RGB)
 
     mutex_exit(&gpio_mutex_);
 }
 
-#if defined(CONFIG_EN_UART_BRIDGE)
-bool uart_bridge_mode()
-{
-    if (!inited_)
-    {
-        return false;
-    }
-
-    bool mode = false;
-    mutex_enter_blocking(&gpio_mutex_);
-
-    gpio_pull_up(MODE_SEL_PIN);
-    if (gpio_get(MODE_SEL_PIN) == 0) 
-    {
-        mode = true;
-    } 
-
-    mutex_exit(&gpio_mutex_);
-    return mode;
-}  
-#endif // defined(CONFIG_EN_UART_BRIDGE)
-
-#if defined(CONFIG_EN_ESP32)
-void reset_esp32_unsafe() 
-{
-    gpio_put(ESP_RST_PIN, 0);
-    sleep_ms(500);
-    gpio_put(ESP_RST_PIN, 1);
-    sleep_ms(250);
-}
-
-void reset_esp32() 
-{
-    if (!inited_)
-    {
-        return;
-    }
-    mutex_enter_blocking(&gpio_mutex_);
-    reset_esp32_unsafe();
-    mutex_exit(&gpio_mutex_);
-}
-
-void enter_esp32_prog_mode() 
-{
-    if (!inited_)
-    {
-        return;
-    }
-    mutex_enter_blocking(&gpio_mutex_);
-
-    gpio_put(ESP_PROG_PIN, 1);
-    sleep_ms(250);
-    gpio_put(ESP_PROG_PIN, 0);
-	sleep_ms(250);
-
-    reset_esp32_unsafe();
-
-	gpio_put(ESP_PROG_PIN, 1);
-
-    mutex_exit(&gpio_mutex_);
-}
-#endif // defined(CONFIG_EN_ESP32)
-
 void reboot()
 {
     #define AIRCR_REG (*((volatile uint32_t *)(0xE000ED0C)))
     #define AIRCR_SYSRESETREQ (1 << 2)
     #define AIRCR_VECTKEY (0x5FA << 16)
 
+    OGXM_LOG("Rebooting\n");
+
     AIRCR_REG = AIRCR_VECTKEY | AIRCR_SYSRESETREQ;
     while(1);
 }
@@ -189,13 +103,18 @@ uint32_t ms_since_boot()
     return to_ms_since_boot(get_absolute_time());
 }
 
+//Call after board is initialized
+void init_bluetooth()
+{
+    if (board_api_bt::init)
+    {
+        board_api_bt::init();
+    }
+}
+
+//Call on core0 before any other method
 void init_board()
 {
-    if (inited_)
-    {
-        return;
-    }
-
     if (!set_sys_clock_khz(SYSCLOCK_KHZ, true))
     {
         if (!set_sys_clock_khz((SYSCLOCK_KHZ / 2), true))
@@ -209,24 +128,31 @@ void init_board()
     if (!mutex_is_initialized(&gpio_mutex_))
     {
         mutex_init(&gpio_mutex_); 
+        mutex_enter_blocking(&gpio_mutex_);
+
+        if (ogxm_log::init)
+        {
+            ogxm_log::init();
+        }
+        if (board_api_led::init)
+        {
+            board_api_led::init();
+        }
+        if (board_api_rgb::init)
+        {
+            board_api_rgb::init();
+        }
+        if (board_api_esp32::init)
+        {
+            board_api_esp32::init();
+        }
+        if (board_api_usbh::init)
+        {
+            board_api_usbh::init();
+        }
+
+        mutex_exit(&gpio_mutex_);
     }
-    
-    mutex_enter_blocking(&gpio_mutex_);
-
-    init_uart_debug_unsafe();
-    init_vcc_en_pin_unsafe();
-    init_rgb_unsafe();
-    init_led_indicator_unsafe();
-    init_esp32_io_unsafe();
-    init_uart_bridge_io_unsafe();
-
-    inited_ = true;
-
-    mutex_exit(&gpio_mutex_);
-
-#if (OGXM_BOARD != PI_PICOW) // cyw43_arch needs to be inited from core1 first
-    set_led(false);
-#endif
 
     OGXM_LOG("Board initialized\n");
 }
diff --git a/Firmware/RP2040/src/Board/board_api.h b/Firmware/RP2040/src/Board/board_api.h
index 22e3d5e..4115dfa 100644
--- a/Firmware/RP2040/src/Board/board_api.h
+++ b/Firmware/RP2040/src/Board/board_api.h
@@ -7,14 +7,23 @@
 namespace board_api
 {
     void init_board();
-    void set_led(bool state);
+    void init_bluetooth();
     void reboot();
-
-    bool uart_bridge_mode();
-    void reset_esp32();
-    void enter_esp32_prog_mode();
-
+    void set_led(bool state);
     uint32_t ms_since_boot();
+
+    namespace usb
+    {
+        bool host_connected();
+        void disconnect_all();
+    }
+
+    namespace esp32
+    {
+        bool uart_bridge_mode();
+        void reset();
+        void enter_programming_mode();
+    }
 }
 
 #endif // _OGXM_BOARD_API_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Board/board_api_private/board_api_bt.cpp b/Firmware/RP2040/src/Board/board_api_private/board_api_bt.cpp
new file mode 100644
index 0000000..358d76c
--- /dev/null
+++ b/Firmware/RP2040/src/Board/board_api_private/board_api_bt.cpp
@@ -0,0 +1,29 @@
+#include "board_config.h"
+#if defined(CONFIG_EN_BLUETOOTH)
+
+#include <pico/cyw43_arch.h>
+
+#include "Board/board_api_private/board_api_private.h"
+
+#if defined(CYW43_WL_GPIO_LED_PIN) && defined(LED_INDICATOR_PIN)
+static_assert(CYW43_WL_GPIO_LED_PIN != LED_INDICATOR_PIN, "CYW43_WL_GPIO_LED_PIN cannot be the same as LED_INDICATOR_PIN");
+#endif
+
+namespace board_api_bt {
+
+void init()
+{
+    if (cyw43_arch_init() != 0)
+    {  
+        panic("CYW43 init failed");
+    }  
+}
+
+void set_led(bool state)
+{
+    cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, state ? 1 : 0);
+}
+
+} // namespace board_api_bt
+
+#endif // defined(CONFIG_EN_BLUETOOTH)
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Board/board_api_private/board_api_esp32.cpp b/Firmware/RP2040/src/Board/board_api_private/board_api_esp32.cpp
new file mode 100644
index 0000000..b4c1193
--- /dev/null
+++ b/Firmware/RP2040/src/Board/board_api_private/board_api_esp32.cpp
@@ -0,0 +1,54 @@
+#include "board_config.h"
+#if defined(CONFIG_EN_ESP32)
+
+#include <pico/stdlib.h>
+#include <hardware/gpio.h>
+
+#include "Board/board_api_private/board_api_private.h"
+
+namespace board_api_esp32 {
+
+bool uart_bridge_mode()
+{
+    gpio_pull_up(MODE_SEL_PIN);
+    return (gpio_get(MODE_SEL_PIN) == 0);
+}  
+
+void reset() 
+{
+    gpio_put(ESP_RST_PIN, 0);
+    sleep_ms(500);
+    gpio_put(ESP_RST_PIN, 1);
+    sleep_ms(250);
+}
+
+void enter_programming_mode() 
+{
+    gpio_put(ESP_PROG_PIN, 1);
+    sleep_ms(250);
+    gpio_put(ESP_PROG_PIN, 0);
+	sleep_ms(250);
+
+    reset();
+
+	gpio_put(ESP_PROG_PIN, 1);
+}
+
+void init()
+{
+    gpio_init(ESP_PROG_PIN);
+    gpio_set_dir(ESP_PROG_PIN, GPIO_OUT);
+    gpio_put(ESP_PROG_PIN, 1);
+
+    gpio_init(ESP_RST_PIN);
+    gpio_set_dir(ESP_RST_PIN, GPIO_OUT);
+    gpio_put(ESP_RST_PIN, 1);
+
+    gpio_init(MODE_SEL_PIN);
+    gpio_set_dir(MODE_SEL_PIN, GPIO_IN);
+    gpio_pull_up(MODE_SEL_PIN);
+}
+
+} // namespace board_api_esp32
+
+#endif // defined(CONFIG_EN_ESP32)
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Board/board_api_private/board_api_led.cpp b/Firmware/RP2040/src/Board/board_api_private/board_api_led.cpp
new file mode 100644
index 0000000..2cb9018
--- /dev/null
+++ b/Firmware/RP2040/src/Board/board_api_private/board_api_led.cpp
@@ -0,0 +1,22 @@
+#include "board_config.h"
+#if defined(LED_INDICATOR_PIN)
+
+#include <hardware/gpio.h>
+
+namespace board_api_led {
+    
+void init()
+{ 
+    gpio_init(LED_INDICATOR_PIN);
+    gpio_set_dir(LED_INDICATOR_PIN, GPIO_OUT);
+    gpio_put(LED_INDICATOR_PIN, 0);
+}
+
+void set_led(bool state)
+{
+    gpio_put(LED_INDICATOR_PIN, state ? 1 : 0);
+}
+
+} // namespace board_api
+
+#endif // LED_INDICATOR_PIN
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Board/board_api_private/board_api_private.h b/Firmware/RP2040/src/Board/board_api_private/board_api_private.h
new file mode 100644
index 0000000..65b8b1b
--- /dev/null
+++ b/Firmware/RP2040/src/Board/board_api_private/board_api_private.h
@@ -0,0 +1,41 @@
+#ifndef BOARD_API_PRIVATE_H
+#define BOARD_API_PRIVATE_H
+
+#include <cstdint>
+#include <string>
+#include <sstream>
+#include <iostream>
+
+namespace board_api_bt
+{
+    void init() __attribute__((weak));
+    void set_led(bool state) __attribute__((weak));
+}
+
+namespace board_api_led
+{
+    void init() __attribute__((weak));
+    void set_led(bool state) __attribute__((weak));
+}
+
+namespace board_api_rgb
+{
+    void init() __attribute__((weak));
+    void set_led(uint8_t r, uint8_t g, uint8_t b) __attribute__((weak));
+}
+
+namespace board_api_esp32
+{
+    void init() __attribute__((weak));
+    bool uart_bridge_mode() __attribute__((weak));
+    void reset() __attribute__((weak));
+    void enter_programming_mode() __attribute__((weak));
+}
+
+namespace board_api_usbh
+{
+    void init() __attribute__((weak));
+    bool host_connected() __attribute__((weak));
+}
+
+#endif // BOARD_API_PRIVATE_H
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Board/board_api_private/board_api_rgb.cpp b/Firmware/RP2040/src/Board/board_api_private/board_api_rgb.cpp
new file mode 100644
index 0000000..abbc363
--- /dev/null
+++ b/Firmware/RP2040/src/Board/board_api_private/board_api_rgb.cpp
@@ -0,0 +1,36 @@
+#include "board_config.h"
+#if defined(CONFIG_EN_RGB)
+
+#include <hardware/gpio.h>
+
+#include "Board/Pico_WS2812/WS2812.hpp"
+#include "Board/board_api_private/board_api_private.h"
+
+namespace board_api_rgb {
+
+WS2812& get_ws2812()
+{
+    static WS2812 ws2812 = WS2812(RGB_PXL_PIN, 1, pio1, 0, WS2812::FORMAT_GRB);
+    return ws2812;
+}
+
+void init()
+{
+#if defined(RGB_PWR_PIN)
+    gpio_init(RGB_PWR_PIN);
+    gpio_set_dir(RGB_PWR_PIN, GPIO_OUT);
+    gpio_put(RGB_PWR_PIN, 1);
+#endif
+
+    set_led(0xFF, 0, 0);
+}
+
+void set_led(uint8_t r, uint8_t g, uint8_t b)
+{
+    get_ws2812().setPixelColor(0, WS2812::RGB(r, g, b));
+    get_ws2812().show();
+}
+
+} // namespace board_api_rgb
+
+#endif // defined(CONFIG_EN_RGB)
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Board/board_api_private/board_api_usbh.cpp b/Firmware/RP2040/src/Board/board_api_private/board_api_usbh.cpp
new file mode 100644
index 0000000..633ed81
--- /dev/null
+++ b/Firmware/RP2040/src/Board/board_api_private/board_api_usbh.cpp
@@ -0,0 +1,70 @@
+#include "board_config.h"
+#if defined(CONFIG_EN_USB_HOST)
+
+#include <atomic>
+#include <hardware/gpio.h>
+
+#include "Board/board_api_private/board_api_private.h"
+
+namespace board_api_usbh {
+
+std::atomic<bool> host_connected_ = false;
+
+void host_pin_isr(uint gpio, uint32_t events) 
+{
+    gpio_set_irq_enabled(PIO_USB_DP_PIN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, false);
+    gpio_set_irq_enabled(PIO_USB_DP_PIN + 1, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, false);
+
+    if (gpio == PIO_USB_DP_PIN || gpio == PIO_USB_DP_PIN + 1) 
+    {
+        uint32_t dp_state = gpio_get(PIO_USB_DP_PIN);
+        uint32_t dm_state = gpio_get(PIO_USB_DP_PIN + 1);
+
+        if (dp_state || dm_state) 
+        {
+            host_connected_.store(true);
+        } 
+        else 
+        {
+            host_connected_.store(false);
+            gpio_set_irq_enabled(PIO_USB_DP_PIN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true);
+            gpio_set_irq_enabled(PIO_USB_DP_PIN + 1, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true);
+        }
+    }
+}
+
+bool host_connected()
+{
+    return host_connected_.load();
+}
+
+void init()
+{
+#if defined(VCC_EN_PIN)
+    gpio_init(VCC_EN_PIN);
+    gpio_set_dir(VCC_EN_PIN, GPIO_OUT);
+    gpio_put(VCC_EN_PIN, 1);
+#endif 
+
+    gpio_init(PIO_USB_DP_PIN);
+    gpio_set_dir(PIO_USB_DP_PIN, GPIO_IN);
+    gpio_pull_down(PIO_USB_DP_PIN);
+
+    gpio_init(PIO_USB_DP_PIN + 1);
+    gpio_set_dir(PIO_USB_DP_PIN + 1, GPIO_IN);
+    gpio_pull_down(PIO_USB_DP_PIN + 1);
+
+    if (gpio_get(PIO_USB_DP_PIN) || gpio_get(PIO_USB_DP_PIN + 1)) 
+    {
+        host_connected_.store(true);
+    } 
+    else
+    {
+        gpio_set_irq_enabled_with_callback(PIO_USB_DP_PIN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, &host_pin_isr);
+        gpio_set_irq_enabled_with_callback(PIO_USB_DP_PIN + 1, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, &host_pin_isr);
+    }
+}
+
+} // namespace board_api_usbh
+
+#endif // defined(CONFIG_EN_USB_HOST)
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Board/ogxm_log.cpp b/Firmware/RP2040/src/Board/ogxm_log.cpp
new file mode 100644
index 0000000..62868cd
--- /dev/null
+++ b/Firmware/RP2040/src/Board/ogxm_log.cpp
@@ -0,0 +1,91 @@
+#include "board_config.h"
+#if defined(OGXM_DEBUG)
+
+#include <cstdint>
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <iomanip>
+#include <pico/mutex.h>
+#include <hardware/uart.h>
+#include <hardware/gpio.h>
+
+#include "USBDevice/DeviceDriver/DeviceDriverTypes.h"
+#include "Board/ogxm_log.h"
+
+std::ostream& operator<<(std::ostream& os, DeviceDriverType type) 
+{
+    switch (type) 
+    {
+        case DeviceDriverType::NONE:          os << "NONE"; break;
+        case DeviceDriverType::XBOXOG:        os << "XBOXOG"; break;
+        case DeviceDriverType::XBOXOG_SB:     os << "XBOXOG_SB"; break;
+        case DeviceDriverType::XBOXOG_XR:     os << "XBOXOG_XR"; break;
+        case DeviceDriverType::XINPUT:        os << "XINPUT"; break;
+        case DeviceDriverType::PS3:           os << "PS3"; break;
+        case DeviceDriverType::DINPUT:        os << "DINPUT"; break;
+        case DeviceDriverType::PSCLASSIC:     os << "PSCLASSIC"; break;
+        case DeviceDriverType::SWITCH:        os << "SWITCH"; break;
+        case DeviceDriverType::WEBAPP:        os << "WEBAPP"; break;
+        case DeviceDriverType::UART_BRIDGE:   os << "UART_BRIDGE"; break;
+        default:                              os << "UNKNOWN"; break;
+    }
+    return os;
+}
+
+namespace ogxm_log {
+
+void init()
+{
+    uart_init(DEBUG_UART_PORT, PICO_DEFAULT_UART_BAUD_RATE);
+    gpio_set_function(PICO_DEFAULT_UART_TX_PIN, GPIO_FUNC_UART);
+    gpio_set_function(PICO_DEFAULT_UART_RX_PIN, GPIO_FUNC_UART);
+}
+
+void log(const std::string& message)
+{
+    static mutex_t log_mutex;
+
+    if (!mutex_is_initialized(&log_mutex))
+    {
+        mutex_init(&log_mutex);
+    }
+
+    mutex_enter_blocking(&log_mutex);
+
+    std::string formatted_msg = "OGXM: " + message;
+
+    uart_puts(DEBUG_UART_PORT, formatted_msg.c_str());
+
+    mutex_exit(&log_mutex);
+}
+
+void log(const char* fmt, ...)
+{
+    va_list args;
+    va_start(args, fmt);
+
+    char buffer[256];
+    vsnprintf(buffer, sizeof(buffer), fmt, args);
+
+    std::string formatted_msg = std::string(buffer);
+
+    log(formatted_msg);
+
+    va_end(args);
+}
+
+void log_hex(const uint8_t* data, size_t size)
+{
+    std::ostringstream hex_stream;
+    hex_stream << std::hex << std::setfill('0');
+    for (uint16_t i = 0; i < size; ++i)
+    {
+        hex_stream << std::setw(2) << static_cast<int>(data[i]) << " ";
+    }
+    log(hex_stream.str());
+}
+
+} // namespace ogxm_log
+
+#endif // defined(OGXM_DEBUG)
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Board/ogxm_log.h b/Firmware/RP2040/src/Board/ogxm_log.h
new file mode 100644
index 0000000..a0e64e8
--- /dev/null
+++ b/Firmware/RP2040/src/Board/ogxm_log.h
@@ -0,0 +1,61 @@
+#ifndef BOARD_API_LOG_H
+#define BOARD_API_LOG_H
+
+#include <cstdint>
+
+#include "board_config.h"
+#if defined(OGXM_DEBUG)
+
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <stdarg.h>
+
+#include "USBDevice/DeviceDriver/DeviceDriverTypes.h"
+
+std::ostream& operator<<(std::ostream& os, DeviceDriverType type);
+
+namespace ogxm_log
+{
+    void init() __attribute__((weak));
+
+    //Don't use this directly, use the OGXM_LOG macro
+    void log(const std::string& message);
+
+    //Don't use this directly, use the OGXM_LOG macro
+    void log(const char* fmt, ...);
+
+    //Don't use this directly, use the OGXM_LOG_HEX macro
+    void log_hex(const uint8_t* data, size_t size);
+
+    template <typename T>
+    std::string to_string(const T& value)
+    {
+        std::ostringstream stream;
+        stream << value;
+        return stream.str();
+    }
+}
+
+#define OGXM_LOG ogxm_log::log
+#define OGXM_LOG_HEX ogxm_log::log_hex
+#define OGXM_ASSERT(x) if (!(x)) { OGXM_LOG("Assertion failed: " #x); while(1); }
+#define OGXM_ASSERT_MSG(x, msg) if (!(x)) { OGXM_LOG("Assertion failed: " #x " " msg); while(1); }
+#define OGXM_TO_STRING ogxm_log::to_string
+
+#else // OGXM_DEBUG
+
+namespace ogxm_log
+{
+    void init() __attribute__((weak));
+}
+
+#define OGXM_LOG(...)
+#define OGXM_LOG_HEX(...)
+#define OGXM_ASSERT(x)
+#define OGXM_ASSERT_MSG(x, msg)
+#define OGXM_TO_STRING(x)
+    
+#endif // OGXM_DEBUG
+
+#endif // BOARD_API_LOG_H
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Descriptors/PS3.h b/Firmware/RP2040/src/Descriptors/PS3.h
index 35c9f12..72516d6 100644
--- a/Firmware/RP2040/src/Descriptors/PS3.h
+++ b/Firmware/RP2040/src/Descriptors/PS3.h
@@ -5,8 +5,6 @@
 #include <cstring>
 #include <random>
 
-#include "tusb.h"
-
 namespace PS3 
 {
 	static constexpr uint8_t MAGIC_BYTES[8] = { 0x21, 0x26, 0x01, 0x07, 0x00, 0x00, 0x00, 0x00 };
@@ -190,6 +188,7 @@ namespace PS3
 		0xFF, 0xFF,
 		0x00, 0x20, 0x40, 0xCE, 0x00, 0x00, 0x00,
 		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00
 	};
 
 	struct BTInfo
@@ -201,19 +200,18 @@ namespace PS3
 
 		BTInfo()
 		{
-			std::memset(this, 0, sizeof(BTInfo));
 			std::memcpy(device_address, DEFAULT_BT_INFO_HEADER, sizeof(DEFAULT_BT_INFO_HEADER));
 
             std::mt19937 gen(12345);
             std::uniform_int_distribution<uint8_t> dist(0, 0xFF);
 
-			for (uint8_t addr = 0; addr < 3; addr++) 
+			for (uint8_t i = 4; i < sizeof(device_address); i++) 
 			{
-				device_address[4 + addr] = dist(gen);
+				device_address[i] = dist(gen);
 			}
-			for (uint8_t addr = 0; addr < 6; addr++) 
+			for (uint8_t i = 1; i < sizeof(host_address); i++) 
 			{
-				host_address[1 + addr] = dist(gen);
+				host_address[i] = dist(gen);
 			}
 		}
 	};
diff --git a/Firmware/RP2040/src/Gamepad.h b/Firmware/RP2040/src/Gamepad.h
index bac7692..4ea0e66 100644
--- a/Firmware/RP2040/src/Gamepad.h
+++ b/Firmware/RP2040/src/Gamepad.h
@@ -8,8 +8,9 @@
 #include <array>
 #include <pico/mutex.h>
 
-#include "Scale.h"
+#include "Range.h"
 #include "UserSettings/UserProfile.h"
+#include "Board/ogxm_log.h"
 
 class Gamepad 
 {
@@ -198,15 +199,6 @@ public:
 
     inline void set_pad_in(PadIn pad_in)
     {
-        pad_in.trigger_l = (pad_in.trigger_l > dz_.trigger_l) ? pad_in.trigger_l : UINT_8::MIN;
-        pad_in.trigger_r = (pad_in.trigger_r > dz_.trigger_r) ? pad_in.trigger_r : UINT_8::MIN;
-        pad_in.joystick_lx = (pad_in.joystick_lx < dz_.joystick_l_neg || pad_in.joystick_lx > dz_.joystick_l_pos) ? pad_in.joystick_lx : INT_16::MID;
-        pad_in.joystick_ly = (pad_in.joystick_ly < dz_.joystick_l_neg || pad_in.joystick_ly > dz_.joystick_l_pos) ? pad_in.joystick_ly : INT_16::MID;
-        pad_in.joystick_rx = (pad_in.joystick_rx < dz_.joystick_r_neg || pad_in.joystick_rx > dz_.joystick_r_pos) ? pad_in.joystick_rx : INT_16::MID;
-        pad_in.joystick_ry = (pad_in.joystick_ry < dz_.joystick_r_neg || pad_in.joystick_ry > dz_.joystick_r_pos) ? pad_in.joystick_ry : INT_16::MID;
-        pad_in.joystick_ly = profile_invert_ly_ ? Scale::invert_joy(pad_in.joystick_ly) : pad_in.joystick_ly;
-        pad_in.joystick_ry = profile_invert_ry_ ? Scale::invert_joy(pad_in.joystick_ry) : pad_in.joystick_ry;
-
         mutex_enter_blocking(&pad_in_mutex_);
         pad_in_ = pad_in;
         new_pad_in_.store(true);
@@ -251,6 +243,226 @@ public:
         mutex_exit(&chatpad_in_mutex_);
     }
 
+    /*  Get joy value adjusted for deadzones, scaling, and inversion settings. 
+        <bits> param is optional, used for scaling specific bit values as opposed 
+        to full range values. */
+    template <uint8_t bits = 0, typename T>
+    inline int16_t scale_joystick_rx(T value) const
+    {
+        int16_t joy_value = 0;
+        if constexpr (bits > 0)
+        {
+            joy_value = Range::scale_from_bits<int16_t, bits>(value);
+        }
+        else if constexpr (!std::is_same_v<T, int16_t>)
+        {
+            joy_value = Range::scale<int16_t>(value);
+        }
+        else
+        {
+            joy_value = value;
+        }
+        if (joy_value > dz_.joystick_r_pos)
+        {
+            joy_value = Range::scale(
+                            joy_value, 
+                            dz_.joystick_r_pos, 
+                            Range::MAX<int16_t>, 
+                            Range::MID<int16_t>, 
+                            Range::MAX<int16_t>);
+        }
+        else if (joy_value < dz_.joystick_r_neg)
+        {
+            joy_value = Range::scale(
+                            joy_value, 
+                            Range::MIN<int16_t>, 
+                            dz_.joystick_r_neg, 
+                            Range::MIN<int16_t>, 
+                            Range::MID<int16_t>);
+        }
+        else
+        {
+            joy_value = 0;
+        }
+        return joy_value;
+    }
+
+    /*  Get joy value adjusted for deadzones, scaling, and inversion settings. 
+        <bits> param is optional, used for scaling specific bit values as opposed 
+        to full range values. */
+    template <uint8_t bits = 0, typename T>
+    inline int16_t scale_joystick_ry(T value, bool invert = false) const
+    {
+        int16_t joy_value = 0;
+        if constexpr (bits > 0)
+        {
+            joy_value = Range::scale_from_bits<int16_t, bits>(value);
+        }
+        else if constexpr (!std::is_same_v<T, int16_t>)
+        {
+            joy_value = Range::scale<int16_t>(value);
+        }
+        else
+        {
+            joy_value = value;
+        }
+        if (joy_value > dz_.joystick_r_pos)
+        {
+            joy_value = Range::scale(
+                            joy_value, 
+                            dz_.joystick_r_pos, 
+                            Range::MAX<int16_t>, 
+                            Range::MID<int16_t>, 
+                            Range::MAX<int16_t>);
+        }
+        else if (joy_value < dz_.joystick_r_neg)
+        {
+            joy_value = Range::scale(
+                            joy_value, 
+                            Range::MIN<int16_t>, 
+                            dz_.joystick_r_neg, 
+                            Range::MIN<int16_t>, 
+                            Range::MID<int16_t>);
+        }
+        else
+        {
+            joy_value = 0;
+        }
+        return profile_invert_ry_ ? (invert ? joy_value : Range::invert(joy_value)) : (invert ? Range::invert(joy_value) : joy_value);
+    }
+
+    /*  Get joy value adjusted for deadzones, scaling, and inversion settings. 
+        <bits> param is optional, used for scaling specific bit values as opposed 
+        to full range values. */
+    template <uint8_t bits = 0, typename T>
+    inline int16_t scale_joystick_lx(T value) const
+    {
+        int16_t joy_value = 0;
+        if constexpr (bits > 0)
+        {
+            joy_value = Range::scale_from_bits<int16_t, bits>(value);
+        }
+        else if constexpr (!std::is_same_v<T, int16_t>)
+        {
+            joy_value = Range::scale<int16_t>(value);
+        }
+        else
+        {
+            joy_value = value;
+        }
+        if (joy_value > dz_.joystick_l_pos)
+        {
+            joy_value = Range::scale(
+                            joy_value, 
+                            dz_.joystick_l_pos, 
+                            Range::MAX<int16_t>, 
+                            Range::MID<int16_t>, 
+                            Range::MAX<int16_t>);
+        }
+        else if (joy_value < dz_.joystick_l_neg)
+        {
+            joy_value = Range::scale(
+                            joy_value, 
+                            Range::MIN<int16_t>, 
+                            dz_.joystick_l_neg, 
+                            Range::MIN<int16_t>, 
+                            Range::MID<int16_t>);
+        }
+        else
+        {
+            joy_value = 0;
+        }
+        return joy_value;
+    }
+
+    /*  Get joy value adjusted for deadzones, scaling, and inversion settings. 
+        <bits> param is optional, used for scaling specific bit values as opposed 
+        to full range values. */
+    template <uint8_t bits = 0, typename T>
+    inline int16_t scale_joystick_ly(T value, bool invert = false) const
+    {
+        int16_t joy_value = 0;
+        if constexpr (bits > 0)
+        {
+            joy_value = Range::scale_from_bits<int16_t, bits>(value);
+        }
+        else if constexpr (!std::is_same_v<T, int16_t>)
+        {
+            joy_value = Range::scale<int16_t>(value);
+        }
+        else
+        {
+            joy_value = value;
+        }
+        if (joy_value > dz_.joystick_l_pos)
+        {
+            joy_value = Range::scale(
+                            joy_value, 
+                            dz_.joystick_l_pos, 
+                            Range::MAX<int16_t>, 
+                            Range::MID<int16_t>, 
+                            Range::MAX<int16_t>);
+        }
+        else if (joy_value < dz_.joystick_l_neg)
+        {
+            joy_value = Range::scale(
+                            joy_value, 
+                            Range::MIN<int16_t>, 
+                            dz_.joystick_l_neg, 
+                            Range::MIN<int16_t>, 
+                            Range::MID<int16_t>);
+        }
+        else
+        {
+            joy_value = 0;
+        }
+        return profile_invert_ly_ ? (invert ? joy_value : Range::invert(joy_value)) : (invert ? Range::invert(joy_value) : joy_value);
+    }
+
+    /*  Get trigger value adjusted for deadzones, scaling, and inversion. 
+        <bits> param is optional, used for scaling speicifc bit values 
+        as opposed to full range values */
+    template <uint8_t bits = 0, typename T>
+    inline uint8_t scale_trigger_l(T value) const
+    {
+        uint8_t trigger_value = 0;
+        if constexpr (bits > 0)
+        {
+            trigger_value = Range::scale_from_bits<uint8_t, bits>(value);
+        }
+        else if constexpr (!std::is_same_v<T, uint8_t>)
+        {
+            trigger_value = Range::scale<uint8_t>(value);
+        }
+        else
+        {
+            trigger_value = value;
+        }
+        return trigger_value > dz_.trigger_l ? Range::scale(trigger_value, dz_.trigger_l, Range::MAX<uint8_t>) : 0;
+    }
+
+    /*  Get trigger value adjusted for deadzones, scaling, and inversion. 
+        <bits> param is optional, used for scaling speicifc bit values 
+        as opposed to full range values */
+    template <uint8_t bits = 0, typename T>
+    inline uint8_t scale_trigger_r(T value) const
+    {
+        uint8_t trigger_value = 0;
+        if constexpr (bits > 0)
+        {
+            trigger_value = Range::scale_from_bits<uint8_t, bits>(value);
+        }
+        else if constexpr (!std::is_same_v<T, uint8_t>)
+        {
+            trigger_value = Range::scale<uint8_t>(value);
+        }
+        else
+        {
+            trigger_value = value;
+        }
+        return trigger_value > dz_.trigger_l ? Range::scale(trigger_value, dz_.trigger_l, Range::MAX<uint8_t>) : 0;
+    }
+
 private:
     mutex_t pad_in_mutex_;
     mutex_t pad_out_mutex_;
@@ -330,10 +542,22 @@ private:
         dz_.trigger_l = profile.dz_trigger_l;
         dz_.trigger_r = profile.dz_trigger_r;
 
-        dz_.joystick_l_pos = Scale::uint8_to_int16(profile.dz_joystick_l / 2);
-        dz_.joystick_l_neg = Scale::invert_joy(dz_.joystick_l_pos);
-        dz_.joystick_r_pos = Scale::uint8_to_int16(profile.dz_joystick_r / 2);
-        dz_.joystick_r_neg = Scale::invert_joy(dz_.joystick_r_pos);
+        OGXM_LOG("dz_.trigger_l: %d\n", dz_.trigger_l);
+        OGXM_LOG("dz_.trigger_r: %d\n", dz_.trigger_r);
+        OGXM_LOG("profile.dz_joystick_l: %d\n", profile.dz_joystick_l);
+        OGXM_LOG("profile.dz_joystick_r: %d\n", profile.dz_joystick_r);
+
+        dz_.joystick_l_pos = profile.dz_joystick_l ? Range::scale<int16_t>(static_cast<int8_t>(profile.dz_joystick_l / 2)) : 0;
+        dz_.joystick_l_neg = Range::invert(dz_.joystick_l_pos);
+        dz_.joystick_r_pos = profile.dz_joystick_r ? Range::scale<int16_t>(static_cast<int8_t>(profile.dz_joystick_r / 2)) : 0;
+        dz_.joystick_r_neg = Range::invert(dz_.joystick_r_pos);
+
+        OGXM_LOG("dz_.trigger_l: %d\n", dz_.trigger_l);
+        OGXM_LOG("dz_.trigger_r: %d\n", dz_.trigger_r);
+        OGXM_LOG("dz_.joystick_l_pos: %d\n", dz_.joystick_l_pos);
+        OGXM_LOG("dz_.joystick_l_neg: %d\n", dz_.joystick_l_neg);
+        OGXM_LOG("dz_.joystick_r_pos: %d\n", dz_.joystick_r_pos);
+        OGXM_LOG("dz_.joystick_r_neg: %d\n", dz_.joystick_r_neg);
     }
 };
 
diff --git a/Firmware/RP2040/src/I2CDriver/4Channel/I2CDriver.h b/Firmware/RP2040/src/I2CDriver/4Channel/I2CDriver.h
index 51669db..e97ea21 100644
--- a/Firmware/RP2040/src/I2CDriver/4Channel/I2CDriver.h
+++ b/Firmware/RP2040/src/I2CDriver/4Channel/I2CDriver.h
@@ -6,72 +6,56 @@
 
 #include "board_config.h"
 #include "Gamepad.h"
+#include "USBHost/HostDriver/HostDriverTypes.h"
 
-#include "USBHost/HostDriver/HostDriver.h"
-
+//Run on core0
 class I2CDriver
 {
 public:
     virtual ~I2CDriver() {};
     virtual void initialize(uint8_t address) = 0;
     virtual void process(Gamepad (&gamepads)[MAX_GAMEPADS]) = 0;
-    virtual void notify_tuh_mounted(HostDriver::Type host_type = HostDriver::Type::UNKNOWN) = 0;
-    virtual void notify_tuh_unmounted(HostDriver::Type host_type = HostDriver::Type::UNKNOWN) = 0;
-    virtual void notify_xbox360w_connected(uint8_t idx) {};
-    virtual void notify_xbox360w_disconnected(uint8_t idx) {};
+    virtual void notify_tuh(bool mounted, HostDriverType host_type = HostDriverType::UNKNOWN) = 0;
+    virtual void notify_xbox360w(bool connected, uint8_t idx) {};
 
 protected:
-    enum class PacketID : uint8_t { UNKNOWN = 0, PAD, STATUS, ENABLE, DISABLE, REBOOT };
-    enum class SlaveStatus : uint8_t { NC = 0, NOT_READY, READY, RESP_OK };
+    enum class PacketID : uint8_t { UNKNOWN = 0, PAD, COMMAND };
+    enum class Command  : uint8_t { UNKNOWN = 0, STATUS, DISABLE };
+    enum class Status   : uint8_t { UNKNOWN = 0, NC, ERROR, OK, READY, NOT_READY };
 
 #pragma pack(push, 1)
     struct PacketIn
     {
-        uint8_t packet_len;
-        uint8_t packet_id;
-        Gamepad::PadIn pad_in;
-
-        PacketIn()
-        {
-            std::memset(this, 0, sizeof(PacketIn));
-            packet_len = sizeof(PacketIn);
-            packet_id = static_cast<uint8_t>(PacketID::PAD);
-        }
+        uint8_t packet_len{sizeof(PacketIn)};
+        PacketID packet_id{PacketID::PAD};
+        Gamepad::PadIn pad_in{Gamepad::PadIn()};
+        Gamepad::ChatpadIn chatpad_in{0};
+        std::array<uint8_t, 4> reserved{0};
     };
-    static_assert(sizeof(PacketIn) == 28, "I2CDriver::PacketIn is misaligned");
+    static_assert(sizeof(PacketIn) == 32, "I2CDriver::PacketIn is misaligned");
+    static_assert((sizeof(PacketIn) % 8) == 0, "I2CDriver::PacketIn is not a multiple of 8");
 
     struct PacketOut
     {
-        uint8_t packet_len;
-        uint8_t packet_id;
-        Gamepad::PadOut pad_out;    
-
-        PacketOut()
-        {
-            std::memset(this, 0, sizeof(PacketOut));
-            packet_len = sizeof(PacketOut);
-            packet_id = static_cast<uint8_t>(PacketID::PAD);
-        }
+        uint8_t packet_len{sizeof(PacketOut)};
+        PacketID packet_id{PacketID::PAD};
+        Gamepad::PadOut pad_out{Gamepad::PadOut()};
+        std::array<uint8_t, 4> reserved{0};
     };
-    static_assert(sizeof(PacketOut) == 4, "I2CDriver::PacketOut is misaligned");
+    static_assert(sizeof(PacketOut) == 8, "I2CDriver::PacketOut is misaligned");
 
-    struct PacketStatus
+    struct PacketCMD
     {
-        uint8_t packet_len;
-        uint8_t packet_id;
-        uint8_t status;
-
-        PacketStatus()
-        {
-            packet_len = sizeof(PacketStatus);
-            packet_id = static_cast<uint8_t>(PacketID::STATUS);
-            status = static_cast<uint8_t>(SlaveStatus::NC);
-        }
+        uint8_t packet_len{sizeof(PacketCMD)};
+        PacketID packet_id{PacketID::COMMAND};
+        Command command{Command::UNKNOWN};
+        Status status{Status::UNKNOWN};
+        std::array<uint8_t, 4> reserved{0};
     };
-    static_assert(sizeof(PacketStatus) == 3, "I2CDriver::PacketStatus is misaligned");
+    static_assert(sizeof(PacketCMD) == 8, "I2CDriver::PacketCMD is misaligned");
 #pragma pack(pop)
 
-    static constexpr size_t MAX_PACKET_SIZE = std::max(sizeof(PacketStatus), std::max(sizeof(PacketIn), sizeof(PacketOut)));
+    static constexpr size_t MAX_PACKET_SIZE = sizeof(PacketIn);
 };
 
 #endif // I2C_DRIVER_4CH_H
\ No newline at end of file
diff --git a/Firmware/RP2040/src/I2CDriver/4Channel/I2CManager.h b/Firmware/RP2040/src/I2CDriver/4Channel/I2CManager.h
index 76244f3..e31780a 100644
--- a/Firmware/RP2040/src/I2CDriver/4Channel/I2CManager.h
+++ b/Firmware/RP2040/src/I2CDriver/4Channel/I2CManager.h
@@ -4,6 +4,7 @@
 #include <cstdint>
 #include <memory>
 #include <atomic>   
+#include <hardware/gpio.h>
 
 #include "board_config.h"
 #include "Gamepad.h"
diff --git a/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.cpp b/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.cpp
index 9f11d0d..226328a 100644
--- a/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.cpp
+++ b/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.cpp
@@ -1,14 +1,9 @@
 #include <cstring>
+#include <hardware/gpio.h>
 
 #include "TaskQueue/TaskQueue.h"
-#include "OGXMini/OGXMini.h"    
 #include "I2CDriver/4Channel/I2CMaster.h"
 
-I2CMaster::~I2CMaster()
-{
-    TaskQueue::Core0::cancel_delayed_task(tid_update_slave_);
-}
-
 void I2CMaster::initialize(uint8_t address) 
 {
     i2c_init(I2C_PORT, I2C_BAUDRATE);
@@ -25,17 +20,6 @@ void I2CMaster::initialize(uint8_t address)
     {
         slaves_[i].address = address + i + 1;
     }
-
-    tid_update_slave_ = TaskQueue::Core0::get_new_task_id();
-
-    TaskQueue::Core0::queue_delayed_task(tid_update_slave_, 2000, true, [this]
-    {
-        for (auto& slave : slaves_)
-        {
-            check_slave_status(slave);
-            sleep_us(10);
-        }
-    });
 }
 
 void I2CMaster::process(Gamepad (&gamepads)[MAX_GAMEPADS])
@@ -44,69 +28,86 @@ void I2CMaster::process(Gamepad (&gamepads)[MAX_GAMEPADS])
     {
         Slave& slave = slaves_[i];
 
-        if (slave.status != SlaveStatus::READY || !slave_detected(slave.address))
+        if (!slave.enabled.load() || !slave_detected(slave.address))
         {
             continue;
         }
-        if (send_packet_in(slave, gamepads[i + 1]))
+
+        PacketCMD packet_cmd;
+        packet_cmd.packet_id = PacketID::COMMAND;
+        packet_cmd.command = Command::STATUS;
+
+        if (!write_blocking(slave.address, &packet_cmd, sizeof(PacketCMD)) ||
+            !read_blocking(slave.address, &packet_cmd, sizeof(PacketCMD)))
         {
-            get_packet_out(slave, gamepads[i + 1]);
+            continue;
+        }
+
+        if (packet_cmd.status == Status::READY)
+        {
+            Gamepad& gamepad = gamepads[i + 1];
+            PacketIn packet_in;
+            packet_in.pad_in = gamepad.get_pad_in();
+            packet_in.chatpad_in = gamepad.get_chatpad_in();
+
+            if (write_blocking(slave.address, &packet_in, sizeof(PacketIn)))
+            {
+                PacketOut packet_out;
+                if (read_blocking(slave.address, &packet_out, sizeof(PacketOut)))
+                {
+                    gamepad.set_pad_out(packet_out.pad_out);
+                }
+            }
         }
 
         sleep_us(100);
     }
 }
 
-void I2CMaster::notify_tuh_mounted(HostDriver::Type host_type)
+void I2CMaster::notify_tuh(bool mounted, HostDriverType host_type)
 {
-    if (host_type == HostDriver::Type::XBOX360W)
+    if (host_type != HostDriverType::XBOX360W)
     {
-        i2c_enabled_.store(true);
+        return;
     }
-}
 
-void I2CMaster::notify_tuh_unmounted(HostDriver::Type host_type)
-{
-    if (host_type == HostDriver::Type::XBOX360W)
+    i2c_enabled_.store(mounted);
+
+    if (!mounted)
     {
-        i2c_enabled_.store(false);
-
-        TaskQueue::Core0::queue_task([this]()
-        {
-            for (auto& slave : slaves_)
+        //Called from core1 so queue on core0
+        TaskQueue::Core0::queue_task(
+            [this]()
             {
-                notify_disable(slave);
-            }
-        });
+                for (auto& slave : slaves_)
+                {
+                    notify_disable(slave.address);
+                }
+            });
     }
 }
 
-void I2CMaster::notify_xbox360w_connected(uint8_t idx)
+void I2CMaster::notify_xbox360w(bool connected, uint8_t idx)
 {
     if (idx < 1 || idx >= MAX_GAMEPADS)
     {
         return;
     }
-    slaves_[idx - 1].enabled.store(true);
 
-    TaskQueue::Core0::queue_task([this, idx]()
-    {
-        notify_enable(slaves_[idx - 1].address);
-    });
-}
+    slaves_[idx - 1].enabled.store(connected);
 
-void I2CMaster::notify_xbox360w_disconnected(uint8_t idx)
-{
-    if (idx < 1 || idx >= MAX_GAMEPADS)
+    if (!connected)
     {
-        return;
+        //Called from core1 so queue on core0
+        TaskQueue::Core0::queue_task(
+            [this]()
+            {
+                for (auto& slave : slaves_)
+                {
+                    notify_disable(slave.address);
+                }
+            });
     }
-    slaves_[idx - 1].enabled.store(false);
-
-    TaskQueue::Core0::queue_task([this, idx]()
-    {
-        notify_disable(slaves_[idx - 1]);
-    });
 }
 
 bool I2CMaster::slave_detected(uint8_t address)
@@ -116,95 +117,30 @@ bool I2CMaster::slave_detected(uint8_t address)
     return (result >= 0);
 }
 
-void I2CMaster::check_slave_status(Slave& slave)
+void I2CMaster::notify_disable(uint8_t address)
 {
-    if (!slave_detected(slave.address))
+    if (!slave_detected(address))
     {
-        slave.status = SlaveStatus::NC;
         return;
     }
-    if (slave.enabled.load())
+
+    int retries = 10;
+
+    while (retries--)
     {
-        slave.status = get_slave_status(slave.address);
-    }
-    slave.status = SlaveStatus::NOT_READY;
-}
-
-I2CMaster::SlaveStatus I2CMaster::get_slave_status(uint8_t address)
-{
-    PacketStatus status_packet;
-    status_packet.packet_id = static_cast<uint8_t>(PacketID::STATUS);
-
-    int count = i2c_write_blocking(I2C_PORT, address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
-    if (count == sizeof(PacketStatus))
-    {
-        count = i2c_read_blocking(I2C_PORT, address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
-        return static_cast<SlaveStatus>(status_packet.status);
-    }
-    return SlaveStatus::NC;
-}
-
-I2CMaster::SlaveStatus I2CMaster::notify_enable(uint8_t address)
-{
-    PacketStatus status_packet;
-    status_packet.packet_id = static_cast<uint8_t>(PacketID::ENABLE);
-
-    int count = i2c_write_blocking(I2C_PORT, address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
-    if (count == sizeof(PacketStatus))
-    {
-        count = i2c_read_blocking(I2C_PORT, address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
-        return static_cast<SlaveStatus>(status_packet.status);
-    }
-    return SlaveStatus::NC;
-}
-
-bool I2CMaster::notify_disable(Slave& slave)
-{
-    if (slave_detected(slave.address) && slave.enabled.load())
-    {
-        int retries = 6;
-        bool success = false;
-
-        while (!success && retries--)
+        PacketCMD packet_cmd;
+        packet_cmd.packet_id = PacketID::COMMAND;
+        packet_cmd.command = Command::DISABLE;
+        if (write_blocking(address, &packet_cmd, sizeof(PacketCMD)))
         {
-            PacketStatus status_packet;
-            status_packet.packet_id = static_cast<uint8_t>(PacketID::DISABLE);
-
-            int count = i2c_write_blocking(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
-            if (count == sizeof(PacketStatus))
+            if (read_blocking(address, &packet_cmd, sizeof(PacketCMD)))
             {
-                count = i2c_read_blocking(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&status_packet), sizeof(PacketStatus), false);
-                success = (static_cast<SlaveStatus>(status_packet.status) == SlaveStatus::RESP_OK);
+                if (packet_cmd.status == Status::OK)
+                {
+                    break;
+                }
             }
-
-            sleep_ms(1);
         }
-        return success;
+        sleep_ms(1);
     }
-    return false;
-}
-
-bool I2CMaster::send_packet_in(Slave& slave, Gamepad& gamepad)
-{
-    static PacketIn packet_in = PacketIn();
-
-    Gamepad::PadIn pad_in = gamepad.get_pad_in();
-    packet_in.pad_in = pad_in;
-
-    int count = i2c_write_blocking(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&packet_in), sizeof(packet_in), false);
-    return (count == sizeof(PacketIn));
-}
-
-bool I2CMaster::get_packet_out(Slave& slave, Gamepad& gamepad)
-{
-    static PacketOut packet_out = PacketOut();
-
-    int count = i2c_read_blocking(I2C_PORT, slave.address, reinterpret_cast<uint8_t*>(&packet_out), sizeof(PacketOut), false);
-    if (count != sizeof(PacketOut))
-    {
-        return false;
-    }
-
-    gamepad.set_pad_out(packet_out.pad_out);
-    return true;
 }
\ No newline at end of file
diff --git a/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.h b/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.h
index 6331fd6..922fa7a 100644
--- a/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.h
+++ b/Firmware/RP2040/src/I2CDriver/4Channel/I2CMaster.h
@@ -4,8 +4,6 @@
 #include <cstdint>
 #include <atomic>
 #include <array>
-
-#include <hardware/gpio.h>
 #include <hardware/i2c.h>
 
 #include "board_config.h"
@@ -15,42 +13,38 @@
 class I2CMaster : public I2CDriver
 {
 public:
-    ~I2CMaster() override;
+    ~I2CMaster() = default;
     void initialize(uint8_t address) override;
     void process(Gamepad (&gamepads)[MAX_GAMEPADS]) override;
-    void notify_tuh_mounted(HostDriver::Type host_type) override;
-    void notify_tuh_unmounted(HostDriver::Type host_type) override;
-    void notify_xbox360w_connected(uint8_t idx) override;
-    void notify_xbox360w_disconnected(uint8_t idx) override;
+    void notify_tuh(bool mounted, HostDriverType host_type) override;
+    void notify_xbox360w(bool connected, uint8_t idx) override;
 
 private:
     struct Slave
     {
         uint8_t address{0xFF};
-        SlaveStatus status{SlaveStatus::NC};
+        Status status{Status::NC};
         std::atomic<bool> enabled{false};
     };
 
     static constexpr size_t NUM_SLAVES = MAX_GAMEPADS - 1;
     static_assert(NUM_SLAVES > 0, "I2CMaster::NUM_SLAVES must be greater than 0 to use I2C");
 
-    uint32_t tid_update_slave_;
-    bool update_slave_status_{false};
-
     std::atomic<bool> i2c_enabled_{false};
-    // std::atomic<bool> notify_deinit_{false};
-    std::array<Slave, NUM_SLAVES> slaves_;  
+    std::array<Slave, NUM_SLAVES> slaves_; 
+
+    void notify_disable(uint8_t address);
 
     static bool slave_detected(uint8_t address);
-
-    void check_slave_status(Slave& slave);
-    bool notify_disable(Slave& slave);
-    SlaveStatus notify_enable(uint8_t address);
-    // bool send_packet_status(uint8_t address, PacketID packet_id);
-    SlaveStatus get_slave_status(uint8_t address);
-
-    bool send_packet_in(Slave& slave, Gamepad& gamepad);
-    bool get_packet_out(Slave& slave, Gamepad& gamepad);
+    
+    static inline bool read_blocking(uint8_t address, void* buffer, size_t len)
+    {
+        return (i2c_read_blocking(I2C_PORT, address, reinterpret_cast<uint8_t*>(buffer), len, false) == static_cast<int>(len));
+    }
+    static inline bool write_blocking(uint8_t address, void* buffer, size_t len)
+    {
+        return (i2c_write_blocking(I2C_PORT, address, reinterpret_cast<uint8_t*>(buffer), len, false) == static_cast<int>(len));
+    }
 };
 
 #endif // I2C_MASTER_4CH_H
\ No newline at end of file
diff --git a/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.cpp b/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.cpp
index ea7f40c..0924cb2 100644
--- a/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.cpp
+++ b/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.cpp
@@ -1,15 +1,16 @@
 #include <cstring>
 #include <array>
+#include <hardware/gpio.h>
 
-#include "USBHost/HostManager.h"
 #include "OGXMini/OGXMini.h"
 #include "I2CDriver/4Channel/I2CSlave.h"
+#include "TaskQueue/TaskQueue.h"
 
-I2CSlave* I2CSlave::this_instance_ = nullptr;
+I2CSlave* I2CSlave::instance_ = nullptr;
 
 void I2CSlave::initialize(uint8_t address) 
 {
-    this_instance_ = this;
+    instance_ = this;
 
     i2c_init(I2C_PORT, I2C_BAUDRATE);
 
@@ -24,36 +25,29 @@ void I2CSlave::initialize(uint8_t address)
     i2c_slave_init(I2C_PORT, address, &slave_handler);
 }
 
-void I2CSlave::notify_tuh_mounted(HostDriver::Type host_type)
+void I2CSlave::notify_tuh(bool mounted, HostDriverType host_type)
 {
-    i2c_disabled_.store(true);
-}
-
-void I2CSlave::notify_tuh_unmounted(HostDriver::Type host_type)
-{
-    i2c_disabled_.store(false);
+    tuh_mounted_.store(mounted);
 }
 
 void I2CSlave::process(Gamepad (&gamepads)[MAX_GAMEPADS])
 {
-    if (i2c_disabled_.load())
+    if (tuh_mounted_.load())
     {
         return;
     }
     //Don't want to hang up the i2c bus by doing this in the slave handler
-    if (packet_in_.packet_id == static_cast<uint8_t>(PacketID::PAD))
+    if (new_pad_in_.load())
     {
-        if (new_packet_in_)
-        {
-            new_packet_in_ = false;
-            gamepads[0].set_pad_in(packet_in_.pad_in);
-        }
-        
-        if (gamepads[0].new_pad_out())
-        {
-            packet_out_.pad_out = gamepads[0].get_pad_out();
-        } 
+        new_pad_in_.store(false);
+        gamepads[0].set_pad_in(packet_in_.pad_in);
+        gamepads[0].set_chatpad_in(packet_in_.chatpad_in);
     }
+    
+    if (gamepads[0].new_pad_out())
+    {
+        packet_out_.pad_out = gamepads[0].get_pad_out();
+    } 
 }
 
 I2CSlave::PacketID I2CSlave::get_packet_id(uint8_t* buffer_in)
@@ -66,24 +60,14 @@ I2CSlave::PacketID I2CSlave::get_packet_id(uint8_t* buffer_in)
                 return PacketID::PAD;
             }
             break;
-        case PacketID::DISABLE:
-            if (buffer_in[0] == sizeof(PacketStatus))
+
+        case PacketID::COMMAND:
+            if (buffer_in[0] == sizeof(PacketCMD))
             {
-                return PacketID::DISABLE;
-            }
-            break;
-        case PacketID::ENABLE:
-            if (buffer_in[0] == sizeof(PacketStatus))
-            {
-                return PacketID::ENABLE;
-            }
-            break;
-        case PacketID::STATUS:
-            if (buffer_in[0] == sizeof(PacketStatus))
-            {
-                return PacketID::STATUS;
+                return PacketID::COMMAND;
             }
             break;
+
         default:
             break;
     }
@@ -92,74 +76,74 @@ I2CSlave::PacketID I2CSlave::get_packet_id(uint8_t* buffer_in)
 
 void I2CSlave::slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
 {
-    static int count = 0;
-    static std::array<uint8_t, MAX_PACKET_SIZE> buffer_in{0};
-    static std::array<uint8_t, MAX_PACKET_SIZE> buffer_out{0};
+    static size_t count = 0;
+    static bool enabled = false;
+    static uint8_t buffer_in[MAX_PACKET_SIZE];
+    static uint8_t buffer_out[MAX_PACKET_SIZE];
+
+    PacketIn  *packet_in_p = reinterpret_cast<PacketIn*>(buffer_in);
+    PacketOut *packet_out_p = reinterpret_cast<PacketOut*>(buffer_out);
+    PacketCMD *packet_cmd_in_p = reinterpret_cast<PacketCMD*>(buffer_in);
+    PacketCMD *packet_cmd_out_p = reinterpret_cast<PacketCMD*>(buffer_out);
 
     switch (event) 
     {
         case I2C_SLAVE_RECEIVE: // master has written
             if (count < MAX_PACKET_SIZE)
             {
-                buffer_in.data()[count] = i2c_read_byte_raw(i2c);
+                buffer_in[count] = i2c_read_byte_raw(i2c);
                 ++count;
             }
-            // else // Something's wrong, reset
-            // {
-            //     count = 0;
-            //     buffer_in.fill(0);
-            //     buffer_out.fill(0);
-            // }
             break;
 
-        case I2C_SLAVE_FINISH: // master signalled Stop / Restart
+        case I2C_SLAVE_FINISH:
             // Each master write has an ID indicating the type of data to send back on the next read request
             // Every write has an associated read
-            switch (get_packet_id(buffer_in.data())) 
+            switch (get_packet_id(buffer_in)) 
             {
                 case PacketID::PAD:
-                    this_instance_->packet_in_ = *reinterpret_cast<PacketIn*>(buffer_in.data());
-                    this_instance_->new_packet_in_ = true;
-                    *reinterpret_cast<PacketOut*>(buffer_out.data()) = this_instance_->packet_out_;
+                    instance_->packet_in_ = *packet_in_p;
+                    *packet_out_p = instance_->packet_out_;
+                    instance_->new_pad_in_.store(true);
                     break;
 
-                case PacketID::STATUS:
-                    buffer_out.data()[0] = sizeof(PacketStatus);
-                    buffer_out.data()[1] = static_cast<uint8_t>(PacketID::STATUS);
-                    // if something is mounted by tuh, signal that we're not talking to the master
-                    buffer_out.data()[2] = 
-                        this_instance_->i2c_disabled_.load() ? 
-                        static_cast<uint8_t>(SlaveStatus::NOT_READY) : 
-                        static_cast<uint8_t>(SlaveStatus::READY);
-                    break;
-
-                case PacketID::ENABLE:
-                    buffer_out.data()[0] = sizeof(PacketStatus);
-                    buffer_out.data()[1] = static_cast<uint8_t>(PacketID::STATUS);
-                    buffer_out.data()[2] = static_cast<uint8_t>(SlaveStatus::RESP_OK);
-
-                    OGXMini::update_tud_status(true);
-                    break;
-
-                case PacketID::DISABLE:
-                    buffer_out.data()[0] = sizeof(PacketStatus);
-                    buffer_out.data()[1] = static_cast<uint8_t>(PacketID::STATUS);
-                    buffer_out.data()[2] = static_cast<uint8_t>(SlaveStatus::RESP_OK);
-                    if (!this_instance_->i2c_disabled_.load())
+                case PacketID::COMMAND:
+                    switch (packet_cmd_in_p->command)
                     {
-                        OGXMini::update_tud_status(false);
-                    }
-                    break;
+                        case Command::DISABLE:
+                            packet_cmd_out_p->packet_len = sizeof(PacketCMD);
+                            packet_cmd_out_p->packet_id = PacketID::COMMAND;
+                            packet_cmd_out_p->command = Command::DISABLE;
+                            packet_cmd_out_p->status = Status::OK;
+                            if (!instance_->tuh_mounted_.load())
+                            {
+                                OGXMini::host_mounted(false);
+                            }
+                            break;
 
-                default:
+                        case Command::STATUS:
+                            packet_cmd_out_p->packet_len = sizeof(PacketCMD);
+                            packet_cmd_out_p->packet_id = PacketID::COMMAND;
+                            packet_cmd_out_p->command = Command::STATUS;
+                            packet_cmd_out_p->status = instance_->tuh_mounted_.load() ? Status::NOT_READY : Status::READY;
+                            if (!instance_->tuh_mounted_.load() && !enabled)
+                            {
+                                enabled = true;
+                                OGXMini::host_mounted(true);
+                            }
+                            break;
+
+                        default:
+                            break;
+                    }
                     break;
             }
             count = 0;
+            std::memset(buffer_in, 0, sizeof(buffer_in));
             break;
 
-        case I2C_SLAVE_REQUEST: // master requesting data
-            i2c_write_raw_blocking(i2c, buffer_out.data(), buffer_out.data()[0]);
-            buffer_in.fill(0);
+        case I2C_SLAVE_REQUEST:
+            i2c_write_raw_blocking(i2c, buffer_out, buffer_out[0]);
             break;
 
         default:
diff --git a/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.h b/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.h
index 78c5ddd..396ba46 100644
--- a/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.h
+++ b/Firmware/RP2040/src/I2CDriver/4Channel/I2CSlave.h
@@ -4,7 +4,6 @@
 #include <cstdint>
 #include <atomic>
 #include <cstring>
-#include <hardware/gpio.h>
 #include <hardware/i2c.h>
 #include <pico/i2c_slave.h>
 
@@ -18,16 +17,15 @@ public:
     ~I2CSlave() = default;
     void initialize(uint8_t address) override;
     void process(Gamepad (&gamepads)[MAX_GAMEPADS]) override;
-    void notify_tuh_mounted(HostDriver::Type host_type) override;
-    void notify_tuh_unmounted(HostDriver::Type host_type) override;
+    void notify_tuh(bool mounted, HostDriverType host_type) override;
 
 private:
-    static I2CSlave* this_instance_;
+    static I2CSlave* instance_;
 
     PacketIn packet_in_;
     PacketOut packet_out_;
-    bool new_packet_in_{false};
-    std::atomic<bool> i2c_disabled_{false};
+    std::atomic<bool> new_pad_in_{false};
+    std::atomic<bool> tuh_mounted_{false};
 
     static PacketID get_packet_id(uint8_t* buffer_in);
     static void slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event);
diff --git a/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.cpp b/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.cpp
index 4dbcb5a..a4d75c8 100644
--- a/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.cpp
+++ b/Firmware/RP2040/src/I2CDriver/ESP32/I2CDriver.cpp
@@ -8,44 +8,33 @@
 #include "board_config.h"
 #include "Board/board_api.h"
 #include "I2CDriver/ESP32/I2CDriver.h"
+#include "UserSettings/UserSettings.h"
 #include "TaskQueue/TaskQueue.h"
+#include "Board/ogxm_log.h"
 
 namespace I2CDriver {
 
-//May expand commands in the future
-enum class PacketID : uint8_t { UNKNOWN = 0, SET_PAD, GET_PAD };
+enum class PacketID : uint8_t { UNKNOWN = 0, SET_PAD, GET_PAD, SET_DRIVER };
 
 #pragma pack(push, 1)
 struct PacketIn
 {
-    uint8_t packet_len;
-    uint8_t packet_id;
-    uint8_t index;
-    uint8_t gp_data[sizeof(Gamepad::PadIn) - sizeof(Gamepad::PadIn::analog)];
-
-    PacketIn()
-    {
-        std::memset(this, 0, sizeof(PacketIn));
-        packet_len = sizeof(PacketIn);
-        packet_id = static_cast<uint8_t>(PacketID::SET_PAD);
-    }
+    uint8_t packet_len{sizeof(PacketIn)};
+    PacketID packet_id{PacketID::SET_PAD};
+    uint8_t index{0};
+    DeviceDriverType device_type{DeviceDriverType::NONE};
+    Gamepad::PadIn pad_in{Gamepad::PadIn()};
+    std::array<uint8_t, 5> reserved{0};
 };
-static_assert(sizeof(PacketIn) == 16, "i2c_driver_esp::PacketIn size mismatch");
+static_assert(sizeof(PacketIn) == 32, "i2c_driver_esp::PacketIn size mismatch");
 
 struct PacketOut
 {
-    uint8_t packet_len;
-    uint8_t packet_id;
-    uint8_t index;
-    Gamepad::PadOut pad_out;
-    uint8_t reserved[3];
-
-    PacketOut()
-    {
-        std::memset(this, 0, sizeof(PacketOut));
-        packet_len = sizeof(PacketOut);
-        packet_id = static_cast<uint8_t>(PacketID::GET_PAD);
-    }
+    uint8_t packet_len{sizeof(PacketOut)};
+    PacketID packet_id{PacketID::GET_PAD};
+    uint8_t index{0};
+    Gamepad::PadOut pad_out{Gamepad::PadOut()};
+    std::array<uint8_t, 3> reserved{0};
 };
 static_assert(sizeof(PacketOut) == 8, "i2c_driver_esp::PacketOut size mismatch");
 #pragma pack(pop)
@@ -55,108 +44,79 @@ static constexpr uint8_t I2C_ADDR = 0x01;
 
 Gamepad* gamepads_[MAX_GAMEPADS];
 
-inline void process_in_packet(PacketIn* packet_in)
-{
-    Gamepad::PadIn gp_in;
-    std::memcpy(&gp_in, packet_in->gp_data, sizeof(packet_in->gp_data));
-
-    //This is a bandaid since the ESP32 doesn't have access to user profiles atm
-    //Will update this once I write a BLE server for interfacing with the webapp
-
-    Gamepad* gamepad = gamepads_[packet_in->index];
-    Gamepad::PadIn mapped_gp_in;
-
-    if (gp_in.dpad & Gamepad::DPAD_UP)    mapped_gp_in.dpad |= gamepad->MAP_DPAD_UP;
-    if (gp_in.dpad & Gamepad::DPAD_DOWN)  mapped_gp_in.dpad |= gamepad->MAP_DPAD_DOWN;
-    if (gp_in.dpad & Gamepad::DPAD_LEFT)  mapped_gp_in.dpad |= gamepad->MAP_DPAD_LEFT;
-    if (gp_in.dpad & Gamepad::DPAD_RIGHT) mapped_gp_in.dpad |= gamepad->MAP_DPAD_RIGHT;
-
-    if (gp_in.buttons & Gamepad::BUTTON_START)  mapped_gp_in.buttons |= gamepad->MAP_BUTTON_START;
-    if (gp_in.buttons & Gamepad::BUTTON_BACK)   mapped_gp_in.buttons |= gamepad->MAP_BUTTON_BACK;
-    if (gp_in.buttons & Gamepad::BUTTON_L3)     mapped_gp_in.buttons |= gamepad->MAP_BUTTON_L3;
-    if (gp_in.buttons & Gamepad::BUTTON_R3)     mapped_gp_in.buttons |= gamepad->MAP_BUTTON_R3;
-    if (gp_in.buttons & Gamepad::BUTTON_LB)     mapped_gp_in.buttons |= gamepad->MAP_BUTTON_LB;
-    if (gp_in.buttons & Gamepad::BUTTON_RB)     mapped_gp_in.buttons |= gamepad->MAP_BUTTON_RB;
-    if (gp_in.buttons & Gamepad::BUTTON_SYS)    mapped_gp_in.buttons |= gamepad->MAP_BUTTON_SYS;
-    if (gp_in.buttons & Gamepad::BUTTON_A)      mapped_gp_in.buttons |= gamepad->MAP_BUTTON_A;
-    if (gp_in.buttons & Gamepad::BUTTON_B)      mapped_gp_in.buttons |= gamepad->MAP_BUTTON_B;
-    if (gp_in.buttons & Gamepad::BUTTON_X)      mapped_gp_in.buttons |= gamepad->MAP_BUTTON_X;
-    if (gp_in.buttons & Gamepad::BUTTON_Y)      mapped_gp_in.buttons |= gamepad->MAP_BUTTON_Y;
-
-    mapped_gp_in.trigger_l = gp_in.trigger_l;
-    mapped_gp_in.trigger_r = gp_in.trigger_r;
-    mapped_gp_in.joystick_lx = gp_in.joystick_lx;
-    mapped_gp_in.joystick_ly = gp_in.joystick_ly;
-    mapped_gp_in.joystick_rx = gp_in.joystick_rx;
-    mapped_gp_in.joystick_ry = gp_in.joystick_ry;
-
-    gamepad->set_pad_in(mapped_gp_in);
-}
-
-inline void fill_out_report(uint8_t index, PacketOut* report_out)
-{
-    if (index >= MAX_GAMEPADS)
-    {
-        return;
-    }
-
-    report_out->packet_len = sizeof(PacketOut);
-    report_out->packet_id = static_cast<uint8_t>(PacketID::GET_PAD);
-    report_out->index = index;
-    report_out->pad_out = gamepads_[index]->get_pad_out();
-}
-
-PacketID get_packet_id(uint8_t* buffer_in)
-{
-    switch (static_cast<PacketID>(buffer_in[1]))
-    {
-        case PacketID::SET_PAD:
-            if (buffer_in[0] == sizeof(PacketIn))
-            {
-                return PacketID::SET_PAD;
-            }
-            break;
-        //Unused ATM
-        // case PacketID::GET_PAD:
-        //     if (buffer_in[0] == sizeof(PacketOut))
-        //     {
-        //         return PacketID::GET_PAD;
-        //     }
-        //     break;
-        default:
-            break;
-    }
-    return PacketID::UNKNOWN;
-}
-
 static inline void slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
 {
-    static int count = 0;
-    static std::array<uint8_t, MAX_BUFFER_SIZE> buffer_in{0};
-    static std::array<uint8_t, MAX_BUFFER_SIZE> buffer_out{0};
+    static size_t count = 0;
+    static PacketIn packet_in;
+    static PacketOut packet_out;
+    static DeviceDriverType current_device_type = UserSettings::get_instance().get_current_driver();
 
     switch (event) 
     {
-        case I2C_SLAVE_RECEIVE: // master has written
+        case I2C_SLAVE_RECEIVE:
             if (count < sizeof(PacketIn))
             {
-                buffer_in.data()[count] = i2c_read_byte_raw(i2c);
+                reinterpret_cast<uint8_t*>(&packet_in)[count] = i2c_read_byte_raw(i2c);
                 ++count;
             }
             break;
 
-        case I2C_SLAVE_FINISH: // master signalled Stop / Restart
-            if (get_packet_id(buffer_in.data()) == PacketID::SET_PAD)
+        case I2C_SLAVE_FINISH:
+            if (count == 0)
             {
-                process_in_packet(reinterpret_cast<PacketIn*>(buffer_in.data()));
+                break;
+            }
+            switch (packet_in.packet_id)
+            {
+                case PacketID::SET_PAD:
+                    if (packet_in.index < MAX_GAMEPADS)
+                    {
+                        gamepads_[packet_in.index]->set_pad_in(packet_in.pad_in);
+                    }
+                    break;
+
+                case PacketID::SET_DRIVER:
+                    OGXM_LOG("I2C: Received SET_DRIVER packet: " + OGXM_TO_STRING(packet_in.device_type) + "\n");
+
+                    if (packet_in.device_type != DeviceDriverType::NONE &&
+                        packet_in.device_type != current_device_type)
+                    {
+                        OGXM_LOG("I2C: Driver change detected.\n");
+
+                        //Any writes to flash should be done on Core0
+                        TaskQueue::Core0::queue_delayed_task(TaskQueue::Core0::get_new_task_id(), 1000, false, 
+                        [new_device_type = packet_in.device_type]
+                        { 
+                            UserSettings::get_instance().store_driver_type(new_device_type);
+                        });
+                    }
+                    break;
+                    
+                default:
+                    break;
             }
             count = 0;
             break;
 
-        case I2C_SLAVE_REQUEST: // master requesting data
-            fill_out_report(reinterpret_cast<PacketIn*>(buffer_in.data())->index, reinterpret_cast<PacketOut*>(buffer_out.data()));
-            i2c_write_raw_blocking(i2c, buffer_out.data(), buffer_out.data()[0]);
-            buffer_in.fill(0);
+        case I2C_SLAVE_REQUEST:
+            if (packet_in.index < MAX_GAMEPADS)
+            {
+                packet_out.index = packet_in.index;
+                packet_out.pad_out = gamepads_[packet_in.index]->get_pad_out();
+            }
+            // switch (packet_in.packet_id)
+            // {
+            //     case PacketID::SET_PAD:
+                    
+                    i2c_write_raw_blocking(i2c, reinterpret_cast<const uint8_t*>(&packet_out), packet_out.packet_len);
+            //         break;
+
+            //     default:
+                    // i2c_write_raw_blocking(i2c, reinterpret_cast<const uint8_t*>(&packet_out), 1);
+            //         break;
+            // }
+            
+            // packet_in = PacketIn();
             break;
 
         default:
@@ -182,6 +142,8 @@ void initialize(Gamepad (&gamepads)[MAX_GAMEPADS])
     gpio_pull_up(I2C_SCL_PIN);
 
     i2c_slave_init(I2C_PORT, I2C_ADDR, &slave_handler);
+
+    OGXM_LOG("I2C Driver initialized\n");
 }
 
 } // namespace i2c_driver_esp32
\ No newline at end of file
diff --git a/Firmware/RP2040/src/OGXMini/Debug.h b/Firmware/RP2040/src/OGXMini/Debug.h
deleted file mode 100644
index 7f9c2b4..0000000
--- a/Firmware/RP2040/src/OGXMini/Debug.h
+++ /dev/null
@@ -1,55 +0,0 @@
-#ifndef _OGXM_DEBUG_H_
-#define _OGXM_DEBUG_H_
-
-#include "board_config.h"
-#if defined(OGXM_DEBUG)
-
-#include <cstdint>
-#include <string>
-#include <sstream>
-#include <iomanip>
-#include <pico/mutex.h>
-#include <hardware/uart.h>
-
-namespace OGXMini {
-
-    //Don't use this directly, use the OGXM_LOG macro
-    static inline void log(const std::string& message)
-    {
-        static mutex_t log_mutex;
-
-        if (!mutex_is_initialized(&log_mutex))
-        {
-            mutex_init(&log_mutex);
-        }
-
-        mutex_enter_blocking(&log_mutex);
-        uart_puts(DEBUG_UART_PORT, message.c_str());
-        mutex_exit(&log_mutex);
-    }
-
-    //Don't use this directly, use the OGXM_LOG macro
-    static inline void log_hex(const uint8_t* data, size_t size)
-    {
-        std::ostringstream hex_stream;
-        hex_stream << std::hex << std::setfill('0');
-        for (uint16_t i = 0; i < size; ++i)
-        {
-            hex_stream << std::setw(2) << static_cast<int>(data[i]) << " ";
-        }
-        log(hex_stream.str());
-    }
-
-} // namespace OGXMini
-
-#endif // defined(OGXM_DEBUG)
-
-#if defined(OGXM_DEBUG)
-    #define OGXM_LOG OGXMini::log
-    #define OGXM_LOG_HEX OGXMini::log_hex
-#else
-    #define OGXM_LOG(...)
-    #define OGXM_LOG_HEX(...)
-#endif // OGXM_DEBUG
-
-#endif // _OGXM_DEBUG_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/OGXMini/OGXMini.h b/Firmware/RP2040/src/OGXMini/OGXMini.h
index 24a6f43..bb136fe 100644
--- a/Firmware/RP2040/src/OGXMini/OGXMini.h
+++ b/Firmware/RP2040/src/OGXMini/OGXMini.h
@@ -11,7 +11,7 @@ namespace OGXMini
     static constexpr int32_t TUD_CHECK_DELAY_MS = 500;
 
     void run_program();
-    void update_tud_status(bool host_mounted);
+    void host_mounted(bool mounted);
 
 } // namespace OGXMini
 
diff --git a/Firmware/RP2040/src/OGXMini/OGXMini_4Channel.cpp b/Firmware/RP2040/src/OGXMini/OGXMini_4Channel.cpp
index 6512839..7ec3e7c 100644
--- a/Firmware/RP2040/src/OGXMini/OGXMini_4Channel.cpp
+++ b/Firmware/RP2040/src/OGXMini/OGXMini_4Channel.cpp
@@ -10,6 +10,7 @@
 #include "USBDevice/DeviceManager.h"
 #include "USBHost/HostManager.h"
 #include "Board/board_api.h"
+#include "Board/ogxm_log.h"
 #include "OGXMini/OGXMini.h"
 #include "I2CDriver/4Channel/I2CManager.h"
 #include "Gamepad.h"
@@ -18,13 +19,45 @@
 namespace OGXMini {
 
 Gamepad gamepads_[MAX_GAMEPADS];
-std::atomic<bool> tud_inited_{false};
+
+//Called by tusb host or i2c driver so we know to connect or disconnect usb
+void host_mounted(bool host_mounted)
+{
+    static std::atomic<bool> tud_is_inited = false;
+
+    board_api::set_led(host_mounted);
+
+    if (!host_mounted && tud_is_inited.load())
+    {
+        TaskQueue::Core0::queue_task([]()
+        {
+            OGXM_LOG("Disconnecting USB and rebooting.\n");
+            board_api::usb::disconnect_all();
+            board_api::reboot();
+        });
+    }
+    else if (!tud_is_inited.load() && host_mounted)
+    {
+        TaskQueue::Core0::queue_task([]() 
+        { 
+            OGXM_LOG("Initializing USB stack.\n");
+            tud_init(BOARD_TUD_RHPORT); 
+            tud_is_inited.store(true);
+        });
+    }
+}
 
 void core1_task()
 {
     HostManager& host_manager = HostManager::get_instance();
     host_manager.initialize(gamepads_);
 
+    //Pico-PIO-USB will not reliably detect a hot plug on some boards, so monitor and init host stack after connection
+    while(!board_api::usb::host_connected())
+    {
+        sleep_ms(100);
+    }
+
     pio_usb_configuration_t pio_cfg = PIO_USB_CONFIG;
     tuh_configure(BOARD_TUH_RHPORT, TUH_CFGID_RPI_PIO_USB_CONFIGURATION, &pio_cfg);
 
@@ -43,47 +76,23 @@ void core1_task()
     }
 }
 
-//Called by tusb host or i2c driver so we know to connect or disconnect usb
-void update_tud_status(bool host_mounted)
-{
-    board_api::set_led(host_mounted);
-
-    if (!host_mounted && tud_inited_.load())
-    {
-        TaskQueue::Core0::queue_task([]()
-        {
-            tud_disconnect();
-            sleep_ms(300);
-            multicore_reset_core1();
-            sleep_ms(300);
-            board_api::reboot();
-        });
-    }
-    else if (!tud_inited_.load())
-    {
-        TaskQueue::Core0::queue_task([]()
-        {
-            tud_init(BOARD_TUD_RHPORT);
-        });
-    }
-}
-
 void set_gp_check_timer(uint32_t task_id, UserSettings& user_settings)
 {
-    TaskQueue::Core0::queue_delayed_task(task_id, UserSettings::GP_CHECK_DELAY_MS, true, [&user_settings]
-    {
-        //Check gamepad inputs for button combo to change usb device driver
-        if (user_settings.check_for_driver_change(gamepads_[0]))
+    TaskQueue::Core0::queue_delayed_task(task_id, UserSettings::GP_CHECK_DELAY_MS, true, 
+        [&user_settings]
         {
-            //This will store the new mode and reboot the pico
-            user_settings.store_driver_type_safe(user_settings.get_current_driver());
-        }
-    });
+            //Check gamepad inputs for button combo to change usb device driver
+            if (user_settings.check_for_driver_change(gamepads_[0]))
+            {
+                //This will store the new mode and reboot the pico
+                user_settings.store_driver_type(user_settings.get_current_driver());
+            }
+        });
 }
 
 void run_program()
 {
-    UserSettings user_settings;
+    UserSettings& user_settings = UserSettings::get_instance();
     user_settings.initialize_flash();
 
     board_api::init_board();
@@ -93,18 +102,15 @@ void run_program()
         gamepads_[i].set_profile(user_settings.get_profile_by_index(i));
     }
 
-    DeviceManager::get_instance().initialize_driver(user_settings.get_current_driver(), gamepads_);
-    I2CManager::get_instance().initialize_driver();
+    DeviceManager& device_manager = DeviceManager::get_instance();
+    device_manager.initialize_driver(user_settings.get_current_driver(), gamepads_);
+    
+    I2CManager& i2c_manager = I2CManager::get_instance();
+    i2c_manager.initialize_driver();
     
     multicore_reset_core1();
     multicore_launch_core1(core1_task);
 
-    uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
-    set_gp_check_timer(tid_gp_check, user_settings);
-
-    DeviceDriver* device_driver = DeviceManager::get_instance().get_driver();
-    I2CDriver* i2c_driver = I2CManager::get_instance().get_driver();
-
     //Wait for something to call tud_init
     while (!tud_inited())
     {
@@ -112,7 +118,11 @@ void run_program()
         sleep_ms(10);
     }
 
-    tud_inited_.store(true);
+    uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
+    set_gp_check_timer(tid_gp_check, user_settings);
+
+    DeviceDriver* device_driver = device_manager.get_driver();
+    I2CDriver* i2c_driver = i2c_manager.get_driver();
 
     while (true)
     {
diff --git a/Firmware/RP2040/src/OGXMini/OGXMini_ESP32.cpp b/Firmware/RP2040/src/OGXMini/OGXMini_ESP32.cpp
index db8efa2..c1671c8 100644
--- a/Firmware/RP2040/src/OGXMini/OGXMini_ESP32.cpp
+++ b/Firmware/RP2040/src/OGXMini/OGXMini_ESP32.cpp
@@ -1,5 +1,5 @@
 #include "board_config.h"
-#if OGXM_BOARD == W_ESP32
+#if (OGXM_BOARD == PICO_ESP32)
 
 #include <pico/multicore.h>
 
@@ -18,7 +18,7 @@ namespace OGXMini {
 static Gamepad gamepads_[MAX_GAMEPADS];
 
 //Not using this for ESP32 currently
-void update_tud_status(bool host_mounted) { }
+void host_mounted(bool host_mounted) { }
 
 void core1_task()
 {
@@ -30,58 +30,39 @@ void core1_task()
     }
 }
 
-void run_esp32_uart_bridge()
+void run_uart_bridge()
 {
     DeviceManager& device_manager = DeviceManager::get_instance();
-    device_manager.initialize_driver(DeviceDriver::Type::UART_BRIDGE, gamepads_);
+    device_manager.initialize_driver(DeviceDriverType::UART_BRIDGE, gamepads_);
 
-    board_api::enter_esp32_prog_mode();
+    board_api::esp32::enter_programming_mode();
     
     device_manager.get_driver()->process(0, gamepads_[0]); //Runs UART Bridge task, doesn't return
 }
 
-void set_gp_check_timer(uint32_t task_id, UserSettings& user_settings)
-{
-    TaskQueue::Core0::queue_delayed_task(task_id, UserSettings::GP_CHECK_DELAY_MS, true, [&user_settings]
-    {
-        //Check gamepad inputs for button combo to change usb device driver
-        if (user_settings.check_for_driver_change(gamepads_[0]))
-        {
-            //This will store the new mode and reboot the pico
-            user_settings.store_driver_type_safe(user_settings.get_current_driver());
-        }
-    });
-}
-
 void run_program()
 {
-    UserSettings user_settings;
-    user_settings.initialize_flash();
-
     board_api::init_board();
 
-    if (board_api::uart_bridge_mode())
+    UserSettings& user_settings = UserSettings::get_instance();
+    user_settings.initialize_flash();
+
+    //MODE_SEL_PIN is used to determine if UART bridge should be run
+    if (board_api::esp32::uart_bridge_mode())
     {
-        run_esp32_uart_bridge();
+        run_uart_bridge();
         return;
     }
 
-    for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
-    {
-        gamepads_[i].set_profile(user_settings.get_profile_by_index(i));
-    }
-
-    DeviceManager::get_instance().initialize_driver(user_settings.get_current_driver(), gamepads_);
+    DeviceManager& device_manager = DeviceManager::get_instance();
+    device_manager.initialize_driver(user_settings.get_current_driver(), gamepads_);
 
     multicore_reset_core1();
     multicore_launch_core1(core1_task);
 
-    board_api::reset_esp32();
+    board_api::esp32::reset();
 
-    uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
-    set_gp_check_timer(tid_gp_check, user_settings);
-
-    DeviceDriver* device_driver = DeviceManager::get_instance().get_driver();
+    DeviceDriver* device_driver = device_manager.get_driver();
 
     tud_init(BOARD_TUD_RHPORT);
 
diff --git a/Firmware/RP2040/src/OGXMini/OGXMini_PicoW.cpp b/Firmware/RP2040/src/OGXMini/OGXMini_PicoW.cpp
index b66e110..dd87118 100644
--- a/Firmware/RP2040/src/OGXMini/OGXMini_PicoW.cpp
+++ b/Firmware/RP2040/src/OGXMini/OGXMini_PicoW.cpp
@@ -1,8 +1,8 @@
 #include "board_config.h"
-#if (OGXM_BOARD == PI_PICOW)
+#if (OGXM_BOARD == PI_PICOW) || (OGXM_BOARD == PI_PICO2W)
 
+#include <hardware/clocks.h>
 #include <pico/multicore.h>
-#include <pico/cyw43_arch.h>
 
 #include "tusb.h"
 #include "bsp/board_api.h"
@@ -19,16 +19,12 @@ namespace OGXMini {
 Gamepad gamepads_[MAX_GAMEPADS];
 
 //Not using this for Pico W currently
-void update_tud_status(bool host_mounted) { }
+void host_mounted(bool host_mounted) { }
 
 void core1_task()
 {
-    if (cyw43_arch_init() != 0)
-    {  
-        panic("CYW43 init failed");
-    }
-
-    //Doesn't return, don't do anything with core1 unless it's executing within the BTStack loop
+    board_api::init_bluetooth();
+    board_api::set_led(true);
     bluepad32::run_task(gamepads_);
 }
 
@@ -40,24 +36,25 @@ void set_gp_check_timer(uint32_t task_id, UserSettings& user_settings)
         if (user_settings.check_for_driver_change(gamepads_[0]))
         {
             //This will store the new mode and reboot the pico
-            user_settings.store_driver_type_safe(user_settings.get_current_driver());
+            user_settings.store_driver_type(user_settings.get_current_driver());
         }
     });
 }
 
 void run_program()
 {
-    UserSettings user_settings;
-    user_settings.initialize_flash();
-
     board_api::init_board();
 
+    UserSettings& user_settings = UserSettings::get_instance();
+    user_settings.initialize_flash();
+
     for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
     {
         gamepads_[i].set_profile(user_settings.get_profile_by_index(i));
     }
 
-    DeviceManager::get_instance().initialize_driver(user_settings.get_current_driver(), gamepads_);
+    DeviceManager& device_manager = DeviceManager::get_instance();
+    device_manager.initialize_driver(user_settings.get_current_driver(), gamepads_);
 
     multicore_reset_core1();
     multicore_launch_core1(core1_task);
@@ -65,7 +62,7 @@ void run_program()
     uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
     set_gp_check_timer(tid_gp_check, user_settings);
 
-    DeviceDriver* device_driver = DeviceManager::get_instance().get_driver();
+    DeviceDriver* device_driver = device_manager.get_driver();
 
     tud_init(BOARD_TUD_RHPORT);
 
diff --git a/Firmware/RP2040/src/OGXMini/OGXMini_Standard.cpp b/Firmware/RP2040/src/OGXMini/OGXMini_Standard.cpp
index 32d65e8..b1db7f0 100644
--- a/Firmware/RP2040/src/OGXMini/OGXMini_Standard.cpp
+++ b/Firmware/RP2040/src/OGXMini/OGXMini_Standard.cpp
@@ -13,16 +13,49 @@
 #include "TaskQueue/TaskQueue.h"
 #include "Gamepad.h"
 #include "Board/board_api.h"
+#include "Board/ogxm_log.h"
 
 namespace OGXMini {
 
 Gamepad gamepads_[MAX_GAMEPADS];
 
+//Called by tusb host so we know to connect or disconnect usb
+void host_mounted(bool host_mounted)
+{
+    static std::atomic<bool> tud_is_inited = false;
+
+    board_api::set_led(host_mounted);
+
+    if (!host_mounted && tud_is_inited.load())
+    {
+        TaskQueue::Core0::queue_task([]()
+        {
+            OGXM_LOG("USB disconnected, rebooting.\n");
+            board_api::usb::disconnect_all();
+            board_api::reboot();
+        });
+    }
+    else if (!tud_is_inited.load())
+    {
+        TaskQueue::Core0::queue_task([]() 
+        { 
+            tud_init(BOARD_TUD_RHPORT); 
+            tud_is_inited.store(true);
+        });
+    }
+}
+
 void core1_task()
 {
     HostManager& host_manager = HostManager::get_instance();
     host_manager.initialize(gamepads_);
 
+    //Pico-PIO-USB will not reliably detect a hot plug on some boards, monitor and init host stack after connection
+    while(!board_api::usb::host_connected())
+    {
+        sleep_ms(100);
+    }
+
     pio_usb_configuration_t pio_cfg = PIO_USB_CONFIG;
     tuh_configure(BOARD_TUH_RHPORT, TUH_CFGID_RPI_PIO_USB_CONFIGURATION, &pio_cfg);
 
@@ -41,53 +74,28 @@ void core1_task()
     }
 }
 
-//Called by tusb host or i2c driver so we know to connect or disconnect usb
-void update_tud_status(bool host_mounted)
-{
-    static bool tud_is_inited = false;
-
-    board_api::set_led(host_mounted);
-
-    if (!host_mounted && tud_is_inited)
-    {
-        TaskQueue::Core0::queue_task([]()
-        {
-            tud_disconnect();
-            sleep_ms(300);
-            multicore_reset_core1();
-            sleep_ms(300);
-            board_api::reboot();
-        });
-    }
-    else if (!tud_is_inited)
-    {
-        if (TaskQueue::Core0::queue_task([]() { tud_init(BOARD_TUD_RHPORT); }))
-        {
-            tud_is_inited = true;
-        }
-    }
-}
-
 void set_gp_check_timer(uint32_t task_id, UserSettings& user_settings)
 {
     TaskQueue::Core0::queue_delayed_task(task_id, UserSettings::GP_CHECK_DELAY_MS, true, [&user_settings]
     {
+        OGXM_LOG("Checking for driver change.\n");
         //Check gamepad inputs for button combo to change usb device driver
         if (user_settings.check_for_driver_change(gamepads_[0]))
         {
+            OGXM_LOG("Driver change detected, storing new driver.\n");
             //This will store the new mode and reboot the pico
-            user_settings.store_driver_type_safe(user_settings.get_current_driver());
+            user_settings.store_driver_type(user_settings.get_current_driver());
         }
     });
 }
 
 void run_program()
 {
-    UserSettings user_settings;
-    user_settings.initialize_flash();
-
     board_api::init_board();
 
+    UserSettings& user_settings = UserSettings::get_instance();
+    user_settings.initialize_flash();
+
     for (uint8_t i = 0; i < MAX_GAMEPADS; ++i)
     {
         gamepads_[i].set_profile(user_settings.get_profile_by_index(i));
@@ -99,18 +107,18 @@ void run_program()
     multicore_reset_core1();
     multicore_launch_core1(core1_task);
 
-    uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
-    set_gp_check_timer(tid_gp_check, user_settings);
-
-    DeviceDriver* device_driver = device_manager.get_driver();
-
-    // Wait for something to call update_tud_status()
+    // Wait for something to call host_mounted()
     while (!tud_inited())
     {
         TaskQueue::Core0::process_tasks();
         sleep_ms(100);
     }
 
+    uint32_t tid_gp_check = TaskQueue::Core0::get_new_task_id();
+    set_gp_check_timer(tid_gp_check, user_settings);
+
+    DeviceDriver* device_driver = device_manager.get_driver();
+
     while (true)
     {
         TaskQueue::Core0::process_tasks();
diff --git a/Firmware/RP2040/src/Range.h b/Firmware/RP2040/src/Range.h
new file mode 100644
index 0000000..7934ea9
--- /dev/null
+++ b/Firmware/RP2040/src/Range.h
@@ -0,0 +1,307 @@
+#ifndef _RANGE_H_
+#define _RANGE_H_
+
+#include <cstdint>
+#include <limits>
+#include <type_traits>
+#include "Board/ogxm_log.h"
+
+namespace Range {
+
+    template <typename T>
+    requires std::is_integral_v<T>
+    constexpr T MAX = std::numeric_limits<T>::max();
+
+    template <typename T>
+    requires std::is_integral_v<T>
+    constexpr T MIN = std::numeric_limits<T>::min();
+
+    template <typename T>
+    requires std::is_integral_v<T>
+    constexpr T MID = 
+        [] {
+            if constexpr (std::is_unsigned_v<T>) 
+            {
+                return MAX<T> / 2 + 1;
+            } 
+            return 0;
+        }();
+    static_assert(MID<uint8_t> == 128, "MID<uint8_t> != 128");
+    static_assert(MID<int8_t> == 0, "MID<int8_t> != 0");
+
+    template <typename T>
+    requires std::is_integral_v<T>
+    constexpr uint16_t NUM_BITS = sizeof(T) * 8;
+
+    //Maximum value for a given number of bits, result will be signed/unsigned depending on type
+    template<typename T, uint8_t bits>
+    requires std::is_integral_v<T>
+    constexpr T BITS_MAX() 
+    {
+        static_assert(bits <= NUM_BITS<T>, "BITS_MAX: Return type cannot represet maximum bit value");
+        static_assert(bits <= 64, "BITS_MAX: Bits exceed 64");
+        if constexpr (std::is_unsigned_v<T>) 
+        {
+            return (1ULL << bits) - 1;
+        }
+        return (1LL << (bits - 1)) - 1;
+    }
+    static_assert(BITS_MAX<int16_t, 10>() == 511, "BITS_MAX<int16_t>(10) != 511");
+    static_assert(BITS_MAX<uint16_t, 10>() == 1023, "BITS_MAX<uint16_t>(10) != 1023");
+
+    //Minimum value for a given number of bits, result will be signed/unsigned depending on type
+    template<typename T, uint8_t bits>
+    requires std::is_integral_v<T>
+    constexpr T BITS_MIN() 
+    {
+        static_assert(bits <= NUM_BITS<T>, "BITS_MIN: Return type cannot represet minimum bit value");
+        static_assert(bits <= 64, "BITS_MIN: Bits exceed 64");
+        if constexpr (std::is_unsigned_v<T>) 
+        {
+            return 0;
+        }
+        return static_cast<T>(-(1LL << (bits - 1)));
+    }
+    static_assert(BITS_MIN<int16_t, 10>() == -512, "BITS_MIN<int16_t>(10) != -512");
+    static_assert(BITS_MIN<uint16_t, 10>() == 0, "BITS_MIN<uint16_t>(10) != 0");
+
+    template <typename T>
+    static inline T invert(T value) 
+    {   
+        if constexpr (std::is_unsigned_v<T>) 
+        {
+            return Range::MAX<T> - value;
+        } 
+        return (value == Range::MIN<T>) ? Range::MAX<T> : -value; 
+    }
+
+    template <typename To, typename From>
+    requires std::is_integral_v<To> && std::is_integral_v<From>
+    static inline To clamp(From value) 
+    {
+        if constexpr (std::is_signed_v<From> != std::is_signed_v<To>)
+        {
+            using CommonType = std::common_type_t<To, From>;
+            return static_cast<To>((static_cast<CommonType>(value) < static_cast<CommonType>(Range::MIN<To>)) 
+                                    ? Range::MIN<To> 
+                                    : (static_cast<CommonType>(value) > static_cast<CommonType>(Range::MAX<To>)) 
+                                        ? Range::MAX<To> 
+                                        : static_cast<CommonType>(value));
+        }
+        else
+        {
+            return static_cast<To>((value < Range::MIN<To>) 
+                                    ? Range::MIN<To> 
+                                    : (value > Range::MAX<To>) 
+                                        ? Range::MAX<To> 
+                                        : value);
+        }
+    }
+
+    template <typename T>
+    requires std::is_integral_v<T>
+    static inline T clamp(T value, T min, T max) 
+    {
+        return (value < min) ? min : (value > max) ? max : value;
+    }
+
+    // //Scale value from one range to another, will clamp value to input range
+    // template <typename To, typename From>
+    // requires std::is_integral_v<To> && std::is_integral_v<From>
+    // static inline To scale(From value, From min_from, From max_from, To min_to, To max_to) 
+    // {
+    //     constexpr bool is_signed_from = std::is_signed_v<From>;
+    //     constexpr bool is_signed_to = std::is_signed_v<To>;
+
+    //     constexpr auto shift_from = is_signed_from ? static_cast<uint64_t>(0 - Range::MIN<From>) : 0;
+    //     constexpr auto shift_to = is_signed_to ? static_cast<uint64_t>(0 - Range::MIN<To>) : 0;
+
+    //     const uint64_t u_value = static_cast<uint64_t>(value) + shift_from;
+    //     const uint64_t u_min_from = static_cast<uint64_t>(min_from) + shift_from;
+    //     const uint64_t u_max_from = static_cast<uint64_t>(max_from) + shift_from;
+    //     const uint64_t u_min_to = static_cast<uint64_t>(min_to) + shift_to;
+    //     const uint64_t u_max_to = static_cast<uint64_t>(max_to) + shift_to;
+
+    //     const uint64_t scaled = ((u_value - u_min_from) * (u_max_to - u_min_to)) / (u_max_from - u_min_from) + u_min_to;
+
+    //     return static_cast<To>(scaled - (is_signed_to ? shift_to : 0));
+    // }
+
+    template <typename To, typename From>
+    requires std::is_integral_v<To> && std::is_integral_v<From>
+    static inline To scale(From value, From min_from, From max_from, To min_to, To max_to) 
+    {
+        if constexpr (std::is_unsigned_v<From> && std::is_unsigned_v<To>) 
+        {
+            // Both unsigned
+            uint64_t scaled = static_cast<uint64_t>(value - min_from) * 
+                            (max_to - min_to) / 
+                            (max_from - min_from) + min_to;
+            return static_cast<To>(scaled);
+        } 
+        else if constexpr (std::is_signed_v<From> && std::is_unsigned_v<To>) 
+        {
+            // From signed, To unsigned
+            uint64_t shift_from = static_cast<uint64_t>(-min_from);
+            uint64_t u_value = static_cast<uint64_t>(value) + shift_from;
+            uint64_t u_min_from = static_cast<uint64_t>(min_from) + shift_from;
+            uint64_t u_max_from = static_cast<uint64_t>(max_from) + shift_from;
+
+            uint64_t scaled = (u_value - u_min_from) * 
+                            (max_to - min_to) / 
+                            (u_max_from - u_min_from) + min_to;
+            return static_cast<To>(scaled);
+        } 
+        else if constexpr (std::is_unsigned_v<From> && std::is_signed_v<To>) 
+        {
+            // From unsigned, To signed
+            uint64_t shift_to = static_cast<uint64_t>(-min_to);
+            uint64_t scaled = static_cast<uint64_t>(value - min_from) * 
+                            (static_cast<uint64_t>(max_to) + shift_to - static_cast<uint64_t>(min_to) - shift_to) / 
+                            (max_from - min_from) + static_cast<uint64_t>(min_to) + shift_to;
+            return static_cast<To>(scaled - shift_to);
+        } 
+        else 
+        {
+            // Both signed
+            int64_t shift_from = -min_from;
+            int64_t shift_to = -min_to;
+
+            int64_t scaled = (static_cast<int64_t>(value) + shift_from - (min_from + shift_from)) * 
+                            (max_to + shift_to - (min_to + shift_to)) / 
+                            (max_from - min_from) + (min_to + shift_to);
+            return static_cast<To>(scaled - shift_to);
+        }
+    }
+
+    template <typename To, typename From>
+    static inline To scale(From value) 
+    {
+        return scale<To>(value, Range::MIN<From>, Range::MAX<From>, Range::MIN<To>, Range::MAX<To>);
+    }
+
+    template <typename To, typename From>
+    static inline To scale(From value, To min_to, To max_to) 
+    {
+        return scale<To>(value, Range::MIN<From>, Range::MAX<From>, min_to, max_to);
+    }
+
+    //Cast value to signed/unsigned for accurate scaling
+    template <typename To, uint8_t bits, typename From>
+    static inline To scale_from_bits(From value) 
+    {
+        return scale<To>(
+            value, 
+            BITS_MIN<From, bits>(), 
+            BITS_MAX<From, bits>(), 
+            Range::MIN<To>, 
+            Range::MAX<To>);
+    }
+
+    //Cast value to signed/unsigned for accurate scaling
+    template <typename To, uint8_t bits, typename From>
+    static inline To scale_to_bits(From value) 
+    {
+        return scale<To>(
+            value, 
+            Range::MIN<From>, 
+            Range::MAX<From>, 
+            BITS_MIN<To, bits>(), 
+            BITS_MAX<To, bits>());
+    }
+
+} // namespace Range
+
+namespace Scale //Scale and invert values
+{
+    static inline uint8_t int16_to_uint8(int16_t value)
+    {
+        uint16_t shifted_value = static_cast<uint16_t>(value + Range::MID<uint16_t>);
+        return static_cast<uint8_t>(shifted_value >> 8);
+    }
+    static inline uint16_t int16_to_uint16(int16_t value)
+    {
+        return static_cast<uint16_t>(value + Range::MID<uint16_t>);
+    }
+    static inline int8_t int16_to_int8(int16_t value)
+    {
+        return static_cast<int8_t>((value + Range::MID<uint16_t>) >> 8);
+    }
+
+    static inline uint8_t uint16_to_uint8(uint16_t value)
+    {
+        return static_cast<uint8_t>(value >> 8);
+    }
+    static inline int16_t uint16_to_int16(uint16_t value)
+    {
+        return static_cast<int16_t>(value - Range::MID<uint16_t>);
+    }
+    static inline int8_t uint16_to_int8(uint16_t value)
+    {
+        return static_cast<int8_t>((value >> 8) - Range::MID<uint8_t>);
+    }
+
+    static inline int16_t uint8_to_int16(uint8_t value)
+    {
+        return static_cast<int16_t>((static_cast<int32_t>(value) << 8) - Range::MID<uint16_t>);
+    }
+    static inline uint16_t uint8_to_uint16(uint8_t value)
+    {
+        return static_cast<uint16_t>(value) << 8;
+    }
+    static inline int8_t uint8_to_int8(uint8_t value)
+    {
+        return static_cast<int8_t>(value - Range::MID<uint8_t>);
+    }
+
+    static inline int16_t int8_to_int16(int8_t value)
+    {
+        return static_cast<int16_t>(value) << 8;
+    }
+    static inline uint16_t int8_to_uint16(int8_t value)
+    {
+        return static_cast<uint16_t>((value + Range::MID<uint8_t>) << 8);
+    }
+    static inline uint8_t int8_to_uint8(int8_t value)
+    {
+        return static_cast<uint8_t>(value + Range::MID<uint8_t>);
+    }
+
+    // static inline uint8_t int10_to_uint8(int32_t value)
+    // {
+    //     return Range::scale<uint8_t>(value, 10);
+    //     // return static_cast<uint8_t>(value >> 2);
+    // }
+    // static inline int16_t int10_to_int16(int32_t value)
+    // {
+    //     return Range::scale<int16_t>(value, 10);
+    //     // constexpr int32_t scale_factor = Range::MAX<int16_t> - Range::MIN<int16_t>;
+    //     // constexpr int32_t range = INT_10::MAX - INT_10::MIN;
+
+    //     // if (value >= INT_10::MAX)
+    //     // {
+    //     //     return Range::MAX<int16_t>;
+    //     // }
+    //     // else if (value <= INT_10::MIN)
+    //     // {
+    //     //     return Range::MIN<int16_t>;
+    //     // }
+
+    //     // int32_t scaled_value = (value - INT_10::MIN) * scale_factor;
+    //     // return static_cast<int16_t>(scaled_value / range + Range::MIN<int16_t>);
+    // }
+    // static inline uint8_t uint10_to_uint8(int32_t value)
+    // {
+    //     if (value > UINT_10::MAX) 
+    //     {
+    //         value = UINT_10::MAX;
+    //     }
+    //     else if (value < 0) 
+    //     {
+    //         value = 0;
+    //     }
+    //     return static_cast<uint8_t>(value >> 2);
+    // }
+} // namespace Scale
+
+#endif // _RANGE_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/Scale.h b/Firmware/RP2040/src/Scale.h
deleted file mode 100644
index 28627f3..0000000
--- a/Firmware/RP2040/src/Scale.h
+++ /dev/null
@@ -1,151 +0,0 @@
-#ifndef _SCALE_H_
-#define _SCALE_H_
-
-#include <cstdint>
-
-namespace INT_16
-{
-    static constexpr int16_t MIN = INT16_MIN;
-    static constexpr int16_t MID = 0;
-    static constexpr int16_t MAX = INT16_MAX;
-}
-namespace UINT_16
-{
-    static constexpr uint16_t MIN = 0;
-    static constexpr uint16_t MID = 0x8000;
-    static constexpr uint16_t MAX = 0xFFFF;
-}
-namespace UINT_8
-{
-    static constexpr uint8_t  MAX  = 0xFF;
-    static constexpr uint8_t  MID  = 0x80;
-    static constexpr uint8_t  MIN  = 0x00;
-}
-namespace INT_10
-{
-    static constexpr int32_t  MIN  = -512;
-    static constexpr int32_t  MAX  = 511;
-}
-namespace UINT_10
-{
-    static constexpr int32_t MAX = 1023;
-}
-
-namespace Scale //Scale and invert values
-{
-    static inline uint8_t invert_joy(uint8_t value) 
-    {    
-        return static_cast<uint8_t>(UINT_8::MAX - value); 
-    }
-    static inline int8_t invert_joy(int8_t value) 
-    {    
-        return (value == std::numeric_limits<int8_t>::min()) ? std::numeric_limits<int8_t>::max() : -value; 
-    }
-    static inline uint16_t invert_joy(uint16_t value) 
-    {    
-        return static_cast<uint16_t>(std::numeric_limits<uint16_t>::max() - value); 
-    }
-    static inline int16_t invert_joy(int16_t value) 
-    {    
-        return (value == std::numeric_limits<int16_t>::min()) ? std::numeric_limits<int16_t>::max() : -value; 
-    }
-
-    static inline uint8_t int16_to_uint8(int16_t value)
-    {
-        uint16_t shifted_value = static_cast<uint16_t>(value + UINT_16::MID);
-        return static_cast<uint8_t>(shifted_value >> 8);
-    }
-    static inline uint16_t int16_to_uint16(int16_t value)
-    {
-        return static_cast<uint16_t>(value + UINT_16::MID);
-    }
-    static inline int8_t int16_to_int8(int16_t value)
-    {
-        return static_cast<int8_t>((value + UINT_16::MID) >> 8);
-    }
-
-    static inline uint8_t uint16_to_uint8(uint16_t value)
-    {
-        return static_cast<uint8_t>(value >> 8);
-    }
-    static inline int16_t uint16_to_int16(uint16_t value)
-    {
-        return static_cast<int16_t>(value - UINT_16::MID);
-    }
-    static inline int8_t uint16_to_int8(uint16_t value)
-    {
-        return static_cast<int8_t>((value >> 8) - UINT_8::MID);
-    }
-
-    static inline int16_t uint8_to_int16(uint8_t value)
-    {
-        return static_cast<int16_t>((static_cast<int32_t>(value) << 8) - UINT_16::MID);
-    }
-    static inline uint16_t uint8_to_uint16(uint8_t value)
-    {
-        return static_cast<uint16_t>(value) << 8;
-    }
-    static inline int8_t uint8_to_int8(uint8_t value)
-    {
-        return static_cast<int8_t>(value - UINT_8::MID);
-    }
-
-    static inline int16_t int8_to_int16(int8_t value)
-    {
-        return static_cast<int16_t>(value) << 8;
-    }
-    static inline uint16_t int8_to_uint16(int8_t value)
-    {
-        return static_cast<uint16_t>((value + UINT_8::MID) << 8);
-    }
-    static inline uint8_t int8_to_uint8(int8_t value)
-    {
-        return static_cast<uint8_t>(value + UINT_8::MID);
-    }
-
-    static inline uint8_t int10_to_uint8(int32_t value)
-    {
-        value = value - INT_10::MIN;
-
-        if (value >= UINT_10::MAX) 
-        {
-            return UINT_8::MAX;
-        }
-        else if (value <= 0) 
-        {
-            return 0;
-        }
-        return static_cast<uint8_t>(value >> 2);
-    }
-    static inline int16_t int10_to_int16(int32_t value)
-    {
-        constexpr int32_t scale_factor = INT_16::MAX - INT_16::MIN;
-        constexpr int32_t range = INT_10::MAX - INT_10::MIN;
-
-        if (value >= INT_10::MAX)
-        {
-            return INT_16::MAX;
-        }
-        else if (value <= INT_10::MIN)
-        {
-            return INT_16::MIN;
-        }
-
-        int32_t scaled_value = (value - INT_10::MIN) * scale_factor;
-        return static_cast<int16_t>(scaled_value / range + INT_16::MIN);
-    }
-    static inline uint8_t uint10_to_uint8(int32_t value)
-    {
-        if (value > UINT_10::MAX) 
-        {
-            value = UINT_10::MAX;
-        }
-        else if (value < 0) 
-        {
-            value = 0;
-        }
-        return static_cast<uint8_t>(value >> 2);
-    }
-} // namespace Scale
-
-#endif // _SCALE_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/TaskQueue/TaskQueue.cpp b/Firmware/RP2040/src/TaskQueue/TaskQueue.cpp
index 674a10d..51c7257 100644
--- a/Firmware/RP2040/src/TaskQueue/TaskQueue.cpp
+++ b/Firmware/RP2040/src/TaskQueue/TaskQueue.cpp
@@ -1,351 +1,189 @@
-#include <array>
-#include <algorithm>
-#include <atomic>
-#include <pico/stdlib.h>
-#include <hardware/timer.h>
-#include <hardware/irq.h>
-#include <hardware/sync.h>
-
-#include "Board/board_api.h"
-#include "board_config.h"
 #include "TaskQueue/TaskQueue.h"
 
-namespace TaskQueue {
+TaskQueue::TaskQueue(CoreNum core_num) 
+{   
+    alarm_num_ = (core_num == CoreNum::Core0) ? 0 : 1;
+    alarm_num_ += ((OGXM_BOARD == PI_PICOW) || (OGXM_BOARD == PI_PICO2W)) ? 1 : 0; //BTStack uses alarm 0
 
-class CoreQueue
+    hw_set_bits(&timer_hw->inte, 1u << alarm_num_);
+
+    irq_set_exclusive_handler(
+        TIMER_IRQ(alarm_num_), 
+        (core_num == CoreNum::Core0) ? timer_irq_wrapper_c0 : timer_irq_wrapper_c1);
+
+    irq_set_enabled(TIMER_IRQ(alarm_num_), true);
+}
+
+uint32_t TaskQueue::get_new_task_id()
 {
-public:
-    enum class CoreNum : uint8_t
+    return new_task_id_++;
+}
+
+bool TaskQueue::queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function)
+{
+    uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
+    for (const auto& task : task_queue_delayed_) 
     {
-        Core0 = 0,
-        Core1,
-    };
-
-    CoreQueue(CoreNum core_num) 
-        :   core_num_(core_num)
-    {   
-        uint32_t idx = (core_num_ == CoreNum::Core0) ? 0 : 1;
-#if (OGXM_BOARD != PI_PICOW)
-        alarm_num_ = idx;
-#else
-        alarm_num_ = idx + 1; //BTStack uses alarm 0
-#endif
-        instances_[idx] = this;
-
-        hw_set_bits(&timer_hw->inte, 1u << alarm_num_);
-
-        irq_set_exclusive_handler(
-            TIMER_IRQ(alarm_num_), 
-            (core_num_ == CoreNum::Core0) ? timer_irq_wrapper_c0 : timer_irq_wrapper_c1);
-
-        irq_set_enabled(TIMER_IRQ(alarm_num_), true);
+        if (task.task_id == task_id)
+        {
+            spin_unlock(spinlock_delayed_, irq_state);
+            return false;
+        }
     }
 
-    ~CoreQueue() = default;
+    hw_set_bits(&timer_hw->inte, 1u << alarm_num_);
 
-    uint32_t get_new_task_id()
+    //Might come back to this, there is overflow potential
+    uint64_t target_time = timer_hw->timerawl + static_cast<uint64_t>(delay_ms) * 1000;
+
+    for (auto& task : task_queue_delayed_) 
     {
-        return new_task_id_++;
+        if (!task.function) 
+        {
+            task.target_time = target_time;
+            task.interval_ms = repeating ? delay_ms : 0;
+            task.function = function;
+            task.task_id = task_id;
+
+            auto it = std::min_element(task_queue_delayed_.begin(), task_queue_delayed_.end(), [](const DelayedTask& a, const DelayedTask& b) 
+            {
+                return a.function && (!b.function || a.target_time < b.target_time);
+            });
+
+            if (it != task_queue_delayed_.end() && it->function) 
+            {
+                timer_hw->alarm[alarm_num_] = static_cast<uint32_t>(it->target_time);
+            }
+
+            spin_unlock(spinlock_delayed_, irq_state);
+            return true;
+        }
     }
 
-    bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function)
+    spin_unlock(spinlock_delayed_, irq_state);
+    return false;
+}
+
+void TaskQueue::cancel_delayed_task(uint32_t task_id)
+{
+    uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
+    bool found = false;
+
+    for (auto& task : task_queue_delayed_) 
     {
-        uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
-        for (const auto& task : task_queue_delayed_) 
+        if (task.task_id == task_id) 
         {
-            if (task.task_id == task_id)
-            {
-                spin_unlock(spinlock_delayed_, irq_state);
-                return false;
-            }
-        }
-
-        hw_set_bits(&timer_hw->inte, 1u << alarm_num_);
-
-        //Might come back to this, there is overflow potential
-        uint64_t target_time = timer_hw->timerawl + static_cast<uint64_t>(delay_ms) * 1000;
-
-        for (auto& task : task_queue_delayed_) 
-        {
-            if (!task.function) 
-            {
-                task.target_time = target_time;
-                task.interval_ms = repeating ? delay_ms : 0;
-                task.function = function;
-                task.task_id = task_id;
-
-                auto it = std::min_element(task_queue_delayed_.begin(), task_queue_delayed_.end(), [](const DelayedTask& a, const DelayedTask& b) 
-                {
-                    return a.function && (!b.function || a.target_time < b.target_time);
-                });
-
-                if (it != task_queue_delayed_.end() && it->function) 
-                {
-                    timer_hw->alarm[alarm_num_] = static_cast<uint32_t>(it->target_time);
-                }
-
-                spin_unlock(spinlock_delayed_, irq_state);
-                return true;
-            }
+            task.function = nullptr;
+            task.task_id = 0;
+            task.interval_ms = 0;
+            found = true;
         }
+    }
 
+    if (!found)
+    {
         spin_unlock(spinlock_delayed_, irq_state);
-        return false;
+        return;
     }
 
-    void cancel_delayed_task(uint32_t task_id)
-    {
-        uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
-        bool found = false;
+    hw_set_bits(&timer_hw->inte, 1u << alarm_num_);
 
-        for (auto& task : task_queue_delayed_) 
+    int64_t next_target_time = get_next_target_time_unsafe(task_queue_delayed_);
+    if (next_target_time >= 0) 
+    {
+        timer_hw->alarm[alarm_num_] = static_cast<uint32_t>(next_target_time);
+    }
+    
+    spin_unlock(spinlock_delayed_, irq_state);
+}
+
+bool TaskQueue::queue_task(const std::function<void()>& function)
+{
+    uint32_t irq_state = spin_lock_blocking(spinlock_queue_);
+    for (auto& task : task_queue_)
+    {
+        if (!task.function)
         {
-            if (task.task_id == task_id) 
+            task.function = function;
+            spin_unlock(spinlock_queue_, irq_state);
+            return true;
+        }
+    }
+    spin_unlock(spinlock_queue_, irq_state);
+    return false;
+}
+
+void TaskQueue::process_tasks()
+{
+    uint32_t irq_state = spin_lock_blocking(spinlock_queue_);
+    for (auto& task : task_queue_)
+    {
+        if (task.function)
+        {
+            auto function = task.function;
+            task.function = nullptr;
+            spin_unlock(spinlock_queue_, irq_state);
+
+            function();
+
+            irq_state = spin_lock_blocking(spinlock_queue_);
+        }
+        else
+        {
+            break; //No more tasks
+        }
+    }
+    spin_unlock(spinlock_queue_, irq_state);
+}
+
+uint64_t TaskQueue::get_time_64_us()
+{
+    static spin_lock_t* spinlock_time_ = nullptr;
+    if (!spinlock_time_)
+    {
+        int spinlock_time_num = spin_lock_claim_unused(true);
+        spinlock_time_ = spin_lock_instance(static_cast<uint>(spinlock_time_num));
+    }
+    uint32_t irq_state = spin_lock_blocking(spinlock_time_);
+    uint32_t lo = timer_hw->timelr;
+    uint32_t hi = timer_hw->timehr;
+    spin_unlock(spinlock_time_, irq_state);
+    return ((uint64_t) hi << 32u) | lo;;
+}
+
+void TaskQueue::timer_irq_handler()
+{
+    hw_clear_bits(&timer_hw->intr, 1u << alarm_num_);
+
+    uint64_t now = get_time_64_us();
+    uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
+
+    for (auto& task : task_queue_delayed_) 
+    {
+        if (task.function && task.target_time <= now) 
+        {
+            auto function = task.function;
+            if (task.interval_ms) 
+            {
+                task.target_time += (task.interval_ms * 1000);
+            } 
+            else 
             {
                 task.function = nullptr;
                 task.task_id = 0;
-                task.interval_ms = 0;
-                found = true;
             }
-        }
 
-        if (!found)
-        {
             spin_unlock(spinlock_delayed_, irq_state);
-            return;
+            queue_task(function);
+            irq_state = spin_lock_blocking(spinlock_delayed_);
         }
-
-        hw_set_bits(&timer_hw->inte, 1u << alarm_num_);
-
-        int64_t next_target_time = get_next_target_time_unsafe(task_queue_delayed_);
-        if (next_target_time >= 0) 
-        {
-            timer_hw->alarm[alarm_num_] = static_cast<uint32_t>(next_target_time);
-        }
-        
-        spin_unlock(spinlock_delayed_, irq_state);
     }
 
-    bool queue_task(const std::function<void()>& function)
+    int64_t next_target_time = get_next_target_time_unsafe(task_queue_delayed_);
+    if (next_target_time >= 0) 
     {
-        uint32_t irq_state = spin_lock_blocking(spinlock_queue_);
-        for (auto& task : task_queue_)
-        {
-            if (!task.function)
-            {
-                task.function = function;
-                spin_unlock(spinlock_queue_, irq_state);
-                return true;
-            }
-        }
-        spin_unlock(spinlock_queue_, irq_state);
-        return false;
+        timer_hw->alarm[alarm_num_] = static_cast<uint32_t>(next_target_time);
     }
 
-    void process_tasks()
-    {
-        uint32_t irq_state = spin_lock_blocking(spinlock_queue_);
-        for (auto& task : task_queue_)
-        {
-            if (task.function)
-            {
-                auto function = task.function;
-                task.function = nullptr;
-                spin_unlock(spinlock_queue_, irq_state);
-
-                function();
-
-                irq_state = spin_lock_blocking(spinlock_queue_);
-            }
-            else
-            {
-                break; //No more tasks
-            }
-        }
-        spin_unlock(spinlock_queue_, irq_state);
-    }
-
-private:
-    CoreQueue& operator=(const CoreQueue&) = delete;
-    CoreQueue(CoreQueue&&) = delete;
-    CoreQueue& operator=(CoreQueue&&) = delete;
-
-    static CoreQueue* instances_[static_cast<uint8_t>(CoreNum::Core1) + 1];
-
-    struct Task
-    {
-        // uint32_t task_id = 0;
-        std::function<void()> function = nullptr;
-    };
-
-    struct DelayedTask
-    {
-        uint32_t task_id = 0;
-        uint32_t interval_ms = 0;
-        uint64_t target_time = 0;
-        std::function<void()> function = nullptr;
-    };
-
-    static constexpr uint8_t MAX_TASKS = 8;
-    static constexpr uint8_t MAX_DELAYED_TASKS = MAX_TASKS * 2;
-
-    CoreNum core_num_;
-    uint32_t alarm_num_;
-    uint32_t new_task_id_ = 1;
-
-    int spinlock_queue_num_ = spin_lock_claim_unused(true);
-    int spinlock_delayed_num_ = spin_lock_claim_unused(true);
-    spin_lock_t* spinlock_queue_ = spin_lock_instance(static_cast<uint>(spinlock_queue_num_));
-    spin_lock_t* spinlock_delayed_ = spin_lock_instance(static_cast<uint>(spinlock_delayed_num_));
-
-    std::array<Task, MAX_TASKS> task_queue_;
-    std::array<DelayedTask, MAX_DELAYED_TASKS> task_queue_delayed_;
-
-    static void timer_irq_wrapper_c0()
-    {
-        instances_[static_cast<uint8_t>(CoreNum::Core0)]->timer_irq_handler();
-    }
-
-    static void timer_irq_wrapper_c1()
-    {
-        instances_[static_cast<uint8_t>(CoreNum::Core1)]->timer_irq_handler();
-    }
-    
-    static inline uint32_t TIMER_IRQ(uint32_t alarm_num)
-    {
-        return timer_hardware_alarm_get_irq_num(timer_hw, alarm_num);
-    }
-
-    static inline uint64_t get_time_64_us()
-    {
-        static spin_lock_t* spinlock_time_ = nullptr;
-        if (!spinlock_time_)
-        {
-            int spinlock_time_num = spin_lock_claim_unused(true);
-            spinlock_time_ = spin_lock_instance(static_cast<uint>(spinlock_time_num));
-        }
-        uint32_t irq_state = spin_lock_blocking(spinlock_time_);
-        uint32_t lo = timer_hw->timelr;
-        uint32_t hi = timer_hw->timehr;
-        spin_unlock(spinlock_time_, irq_state);
-        return ((uint64_t) hi << 32u) | lo;;
-    }
-
-    void timer_irq_handler()
-    {
-        hw_clear_bits(&timer_hw->intr, 1u << alarm_num_);
-
-        uint64_t now = get_time_64_us();
-        uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
-
-        for (auto& task : task_queue_delayed_) 
-        {
-            if (task.function && task.target_time <= now) 
-            {
-                auto function = task.function;
-                if (task.interval_ms) 
-                {
-                    task.target_time += (task.interval_ms * 1000);
-                } 
-                else 
-                {
-                    task.function = nullptr;
-                    task.task_id = 0;
-                }
-
-                spin_unlock(spinlock_delayed_, irq_state);
-                queue_task(function);
-                irq_state = spin_lock_blocking(spinlock_delayed_);
-            }
-        }
-
-        int64_t next_target_time = get_next_target_time_unsafe(task_queue_delayed_);
-        if (next_target_time >= 0) 
-        {
-            timer_hw->alarm[alarm_num_] = static_cast<uint32_t>(next_target_time);
-        }
-
-        spin_unlock(spinlock_delayed_, irq_state);
-    }
-
-    static inline int64_t get_next_target_time_unsafe(std::array<DelayedTask, MAX_DELAYED_TASKS>& task_queue_delayed)
-    {
-        auto it = std::min_element(task_queue_delayed.begin(), task_queue_delayed.end(), [](const DelayedTask& a, const DelayedTask& b) 
-        {
-            //Get task with the earliest target time
-            return a.function && (!b.function || a.target_time < b.target_time);
-        });
-
-        if (it != task_queue_delayed.end() && it->function) 
-        {
-            return static_cast<int64_t>(it->target_time);
-        }
-        return -1;
-    }
-}; // class CoreQueue
-
-CoreQueue* CoreQueue::instances_[static_cast<uint8_t>(CoreQueue::CoreNum::Core1) + 1]{nullptr};
-
-namespace Core0
-{
-    static inline CoreQueue& get_core_0()
-    {
-        static CoreQueue core(CoreQueue::CoreNum::Core0);
-        return core;
-    }
-    uint32_t get_new_task_id()
-    {
-        return get_core_0().get_new_task_id();
-    }
-    void cancel_delayed_task(uint32_t task_id)
-    {
-        get_core_0().cancel_delayed_task(task_id);
-    }
-    bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function)
-    {
-        return get_core_0().queue_delayed_task(task_id, delay_ms, repeating, function);
-    }
-    bool queue_task(const std::function<void()>& function)
-    {
-        return get_core_0().queue_task(function);
-    }
-    void process_tasks()
-    {
-        get_core_0().process_tasks();
-    }
-}
-
-#if OGXM_BOARD != PI_PICOW
-
-namespace Core1
-{
-    static inline CoreQueue& get_core_1()
-    {
-        static CoreQueue core(CoreQueue::CoreNum::Core1);
-        return core;
-    }
-    uint32_t get_new_task_id()
-    {
-        return get_core_1().get_new_task_id();
-    }
-    void cancel_delayed_task(uint32_t task_id)
-    {
-        get_core_1().cancel_delayed_task(task_id);
-    }
-    bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function)
-    {
-        return get_core_1().queue_delayed_task(task_id, delay_ms, repeating, function);
-    }
-    bool queue_task(const std::function<void()>& function)
-    {
-        return get_core_1().queue_task(function);
-    }
-    void process_tasks()
-    {
-        get_core_1().process_tasks();
-    }
-}
-
-#endif // OGXM_BOARD != PI_PICOW
-
-} // namespace TaskQueue
\ No newline at end of file
+    spin_unlock(spinlock_delayed_, irq_state);
+}
\ No newline at end of file
diff --git a/Firmware/RP2040/src/TaskQueue/TaskQueue.h b/Firmware/RP2040/src/TaskQueue/TaskQueue.h
index 15b2b10..920111e 100644
--- a/Firmware/RP2040/src/TaskQueue/TaskQueue.h
+++ b/Firmware/RP2040/src/TaskQueue/TaskQueue.h
@@ -3,33 +3,157 @@
 
 #include <cstdint>
 #include <functional>
+#include <memory>
+#include <pico/stdlib.h>
+#include <hardware/timer.h>
+#include <hardware/irq.h>
+#include <hardware/sync.h>
 
 #include "board_config.h"
 
-/*  Queue tasks to be executed with process_tasks() in the main loop on either core.
-    Don't use this on the core running BTStack, that is not safe. */
-namespace TaskQueue 
+class TaskQueue
 {
-    namespace Core0 
+public:
+    struct Core0
     {
-        uint32_t get_new_task_id();
-        void cancel_delayed_task(uint32_t task_id);
-        bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function);
-        bool queue_task(const std::function<void()>& function);
-        void process_tasks();
-    }
+        static inline uint32_t get_new_task_id()
+        {
+            return get_core0().get_new_task_id();
+        }
+        static inline void cancel_delayed_task(uint32_t task_id)
+        {
+            get_core0().cancel_delayed_task(task_id);
+        }
+        static inline bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function)
+        {
+            return get_core0().queue_delayed_task(task_id, delay_ms, repeating, function);
+        }
+        static inline bool queue_task(const std::function<void()>& function)
+        {
+            return get_core0().queue_task(function);
+        }
+        static inline void process_tasks()
+        {
+            get_core0().process_tasks();
+        }
+    };
 
-#if OGXM_BOARD != PI_PICOW
-    namespace Core1 
+#if (OGXM_BOARD != PI_PICOW) && (OGXM_BOARD != PI_PICO2W) //BTstack uses core1
+    struct Core1
     {
-        uint32_t get_new_task_id();
-        void cancel_delayed_task(uint32_t task_id);
-        bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function);
-        bool queue_task(const std::function<void()>& function);
-        void process_tasks();
-    }
+        static inline uint32_t get_new_task_id()
+        {
+            return get_core1().get_new_task_id();
+        }
+        static inline void cancel_delayed_task(uint32_t task_id)
+        {
+            get_core1().cancel_delayed_task(task_id);
+        }
+        static inline bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function)
+        {
+            return get_core1().queue_delayed_task(task_id, delay_ms, repeating, function);
+        }
+        static inline bool queue_task(const std::function<void()>& function)
+        {
+            return get_core1().queue_task(function);
+        }
+        static inline void process_tasks()
+        {
+            get_core1().process_tasks();
+        }
+    }; // Core1
 #endif // OGXM_BOARD != PI_PICOW
 
-} // namespace TaskQueue
+private:
+    enum class CoreNum : uint8_t
+    {
+        Core0 = 0,
+        Core1 = 1
+    };
+
+    TaskQueue& operator=(const TaskQueue&) = delete;
+    TaskQueue(TaskQueue&&) = delete;
+    TaskQueue& operator=(TaskQueue&&) = delete;
+
+    TaskQueue(CoreNum core_num);
+    ~TaskQueue() = default;
+
+    struct Task
+    {
+        // uint32_t task_id = 0;
+        std::function<void()> function = nullptr;
+    };
+
+    struct DelayedTask
+    {
+        uint32_t task_id = 0;
+        uint32_t interval_ms = 0;
+        uint64_t target_time = 0;
+        std::function<void()> function = nullptr;
+    };
+
+    static constexpr uint8_t MAX_TASKS = 8;
+    static constexpr uint8_t MAX_DELAYED_TASKS = MAX_TASKS * 2;
+
+    // CoreNum core_num_;
+    uint32_t alarm_num_;
+    uint32_t new_task_id_ = 1;
+
+    int spinlock_queue_num_ = spin_lock_claim_unused(true);
+    int spinlock_delayed_num_ = spin_lock_claim_unused(true);
+    spin_lock_t* spinlock_queue_ = spin_lock_instance(static_cast<uint>(spinlock_queue_num_));
+    spin_lock_t* spinlock_delayed_ = spin_lock_instance(static_cast<uint>(spinlock_delayed_num_));
+
+    std::array<Task, MAX_TASKS> task_queue_;
+    std::array<DelayedTask, MAX_DELAYED_TASKS> task_queue_delayed_;
+
+    static TaskQueue& get_core0()
+    {
+        static TaskQueue core(CoreNum::Core0);
+        return core;
+    }
+    static TaskQueue& get_core1()
+    {
+        static TaskQueue core(CoreNum::Core1);
+        return core;
+    }
+
+    uint32_t get_new_task_id();
+    bool queue_delayed_task(uint32_t task_id, uint32_t delay_ms, bool repeating, const std::function<void()>& function);
+    void cancel_delayed_task(uint32_t task_id);
+    bool queue_task(const std::function<void()>& function);
+    void process_tasks();
+
+    void timer_irq_handler();
+    static uint64_t get_time_64_us();
+
+    static inline void timer_irq_wrapper_c0()
+    {
+        get_core0().timer_irq_handler();
+    }
+    static inline void timer_irq_wrapper_c1()
+    {
+        get_core1().timer_irq_handler();
+    }
+    static inline uint32_t TIMER_IRQ(uint32_t alarm_num)
+    {
+        return timer_hardware_alarm_get_irq_num(timer_hw, alarm_num);
+    }
+    static inline int64_t get_next_target_time_unsafe(std::array<DelayedTask, MAX_DELAYED_TASKS>& task_queue_delayed)
+    {
+        auto it = std::min_element(task_queue_delayed.begin(), task_queue_delayed.end(), [](const DelayedTask& a, const DelayedTask& b) 
+        {
+            //Get task with the earliest target time
+            return a.function && (!b.function || a.target_time < b.target_time);
+        });
+
+        if (it != task_queue_delayed.end() && it->function) 
+        {
+            return static_cast<int64_t>(it->target_time);
+        }
+        return -1;
+    }
+
+}; // class TaskQueue
 
 #endif // TASK_QUEUE_H
\ No newline at end of file
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.h b/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.h
index 654d66a..ed6a749 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.h
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriver.h
@@ -18,21 +18,6 @@
 class DeviceDriver
 {
 public:
-    enum class Type : uint8_t
-    {
-        NONE = 0,
-        PS3 = 1,
-        DINPUT = 2,
-        SWITCH = 4,
-        PSCLASSIC = 5,
-        XINPUT = 6,
-        XBOXOG = 7,
-        XBOXOG_SB = 8,
-        XBOXOG_XR = 9,
-        WEBAPP = 100,
-        UART_BRIDGE = 101
-    };
-
     virtual void initialize() = 0;
     virtual void process(const uint8_t idx, Gamepad& gamepad) = 0;
     virtual uint16_t get_report_cb(uint8_t itf, uint8_t report_id, hid_report_type_t report_type, uint8_t* buffer, uint16_t req_len) = 0;
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriverTypes.h b/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriverTypes.h
new file mode 100644
index 0000000..1fae44e
--- /dev/null
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/DeviceDriverTypes.h
@@ -0,0 +1,21 @@
+#ifndef _DEVICE_DRIVER_TYPES_H_
+#define _DEVICE_DRIVER_TYPES_H_
+
+#include <cstdint>
+
+enum class DeviceDriverType : uint8_t
+{
+    NONE = 0,
+    XBOXOG,
+    XBOXOG_SB,
+    XBOXOG_XR,
+    XINPUT,
+    PS3,
+    DINPUT,
+    PSCLASSIC,
+    SWITCH,
+    WEBAPP = 100,
+    UART_BRIDGE
+};
+
+#endif // _DEVICE_DRIVER_TYPES_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/PS3/PS3.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/PS3/PS3.cpp
index 61c9e6c..c070b5d 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/PS3/PS3.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/PS3/PS3.cpp
@@ -123,7 +123,7 @@ void PS3Device::process(const uint8_t idx, Gamepad& gamepad)
     {
         Gamepad::PadOut gp_out;
         gp_out.rumble_l = report_out_.rumble.left_motor_force;
-        gp_out.rumble_r = report_out_.rumble.right_motor_on ? UINT_8::MAX : 0;
+        gp_out.rumble_r = report_out_.rumble.right_motor_on ? Range::MAX<uint8_t> : 0;
         gamepad.set_pad_out(gp_out);
         new_report_out_ = false;
     }
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/PSClassic/PSClassic.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/PSClassic/PSClassic.cpp
index db5fdd5..0720673 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/PSClassic/PSClassic.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/PSClassic/PSClassic.cpp
@@ -54,9 +54,9 @@ void PSClassicDevice::process(const uint8_t idx, Gamepad& gamepad)
         }
 
         int16_t joy_lx = gp_in.joystick_lx;
-        int16_t joy_ly = Scale::invert_joy(gp_in.joystick_ly);
+        int16_t joy_ly = Range::invert(gp_in.joystick_ly);
         int16_t joy_rx = gp_in.joystick_rx;
-        int16_t joy_ry = Scale::invert_joy(gp_in.joystick_ry);
+        int16_t joy_ry = Range::invert(gp_in.joystick_ry);
 
         if (meets_pos_threshold(joy_lx, joy_rx))
         {
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.cpp
index 526d6b3..9346cc6 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.cpp
@@ -57,13 +57,13 @@ void WebAppDevice::process(const uint8_t idx, Gamepad& gamepad)
             break;
 
         case ReportID::WRITE_PROFILE:
-            if (user_settings_.valid_mode(static_cast<DeviceDriver::Type>(in_report_.input_mode)))
+            if (user_settings_.is_valid_driver(static_cast<DeviceDriverType>(in_report_.input_mode)))
             {
-                success = user_settings_.store_profile_and_driver_type_safe(static_cast<DeviceDriver::Type>(in_report_.input_mode), in_report_.player_idx, in_report_.profile);
+                success = user_settings_.store_profile_and_driver_type(static_cast<DeviceDriverType>(in_report_.input_mode), in_report_.player_idx, in_report_.profile);
             }
             else
             {
-                success = user_settings_.store_profile_safe(in_report_.player_idx, in_report_.profile);
+                success = user_settings_.store_profile(in_report_.player_idx, in_report_.profile);
             }
             if (!success)
             {
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.h b/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.h
index 175f78d..9477a21 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.h
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/WebApp/WebApp.h
@@ -41,9 +41,9 @@ private:
     static_assert(sizeof(Report) == 50, "WebApp report size mismatch");
     #pragma pack(pop)
 
-    UserSettings user_settings_{UserSettings()};
+    UserSettings& user_settings_{UserSettings::get_instance()};
     Report in_report_{Report()};
-    DeviceDriver::Type driver_type_{DeviceDriver::Type::WEBAPP};
+    DeviceDriverType driver_type_{DeviceDriverType::WEBAPP};
 };
 
 #endif // _WEBAAPP_DEVICE_H_
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/XInput/XInput.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/XInput/XInput.cpp
index f4b4661..43d1465 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/XInput/XInput.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/XInput/XInput.cpp
@@ -64,9 +64,9 @@ void XInputDevice::process(const uint8_t idx, Gamepad& gamepad)
         in_report_.trigger_r = gp_in.trigger_r;
 
         in_report_.joystick_lx = gp_in.joystick_lx;
-        in_report_.joystick_ly = Scale::invert_joy(gp_in.joystick_ly);
+        in_report_.joystick_ly = Range::invert(gp_in.joystick_ly);
         in_report_.joystick_rx = gp_in.joystick_rx;
-        in_report_.joystick_ry = Scale::invert_joy(gp_in.joystick_ry);
+        in_report_.joystick_ry = Range::invert(gp_in.joystick_ry);
 
         if (tud_suspended())
         {
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_GP.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_GP.cpp
index b127096..eeda9c0 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_GP.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_GP.cpp
@@ -78,9 +78,9 @@ void XboxOGDevice::process(const uint8_t idx, Gamepad& gamepad)
         in_report_.trigger_r = gp_in.trigger_r;
 
         in_report_.joystick_lx = gp_in.joystick_lx;
-        in_report_.joystick_ly = Scale::invert_joy(gp_in.joystick_ly);
+        in_report_.joystick_ly = Range::invert(gp_in.joystick_ly);
         in_report_.joystick_rx = gp_in.joystick_rx;
-        in_report_.joystick_ry = Scale::invert_joy(gp_in.joystick_ry);
+        in_report_.joystick_ry = Range::invert(gp_in.joystick_ry);
 
         if (tud_suspended())
         {
diff --git a/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.cpp b/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.cpp
index b433bc6..aee8901 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceDriver/XboxOG/XboxOG_SB.cpp
@@ -5,7 +5,6 @@
 #include "Descriptors/XInput.h"
 #include "USBDevice/DeviceDriver/XboxOG/tud_xid/tud_xid.h"
 #include "USBDevice/DeviceDriver/XboxOG/XboxOG_SB.h"
-#include "OGXMini/Debug.h"
 
 static constexpr std::array<XboxOGSBDevice::ButtonMap, 9> GP_MAP = 
 {{
@@ -232,10 +231,11 @@ void XboxOGSBDevice::process(const uint8_t idx, Gamepad& gamepad)
     in_report_.leftPedal    = Scale::uint8_to_uint16(gp_in.trigger_l);
     in_report_.rightPedal   = Scale::uint8_to_uint16(gp_in.trigger_r);
     in_report_.middlePedal  = chatpad_pressed(  gp_in_chatpad, XInput::Chatpad::CODE_BACK) ? 0xFF00 : 0x0000;
-    in_report_.rotationLever= chatpad_pressed(  gp_in_chatpad, XInput::Chatpad::CODE_MESSENGER) ? 0 :
-                                                (gp_in.buttons & Gamepad::BUTTON_BACK) ? 0 :
-                                                (gp_in.dpad & Gamepad::DPAD_LEFT) ? INT_16::MIN :
-                                                (gp_in.dpad & Gamepad::DPAD_RIGHT) ? INT_16::MAX : 0;
+    in_report_.rotationLever= chatpad_pressed(  gp_in_chatpad, XInput::Chatpad::CODE_MESSENGER) 
+                                                    ? 0 : (gp_in.buttons & Gamepad::BUTTON_BACK) 
+                                                        ? 0 : (gp_in.dpad & Gamepad::DPAD_LEFT)  
+                                                            ? Range::MIN<int16_t> : (gp_in.dpad & Gamepad::DPAD_RIGHT) 
+                                                                ? Range::MAX<int16_t> : 0;
 
     in_report_.sightChangeX = gp_in.joystick_lx;
     in_report_.sightChangeY = gp_in.joystick_ly;
@@ -246,15 +246,15 @@ void XboxOGSBDevice::process(const uint8_t idx, Gamepad& gamepad)
         vmouse_x_ += axis_value_x / sensitivity_;
     }
 
-    int32_t axis_value_y = static_cast<int32_t>(Scale::invert_joy(gp_in.joystick_ry));
+    int32_t axis_value_y = static_cast<int32_t>(Range::invert(gp_in.joystick_ry));
     if (std::abs(axis_value_y) > DEFAULT_DEADZONE)
     {
         vmouse_y_ -= axis_value_y / sensitivity_;
     }
 
     if (vmouse_x_ < 0) vmouse_x_ = 0;
-    if (vmouse_x_ > UINT_16::MAX) vmouse_x_ = UINT_16::MAX;
-    if (vmouse_y_ > UINT_16::MAX) vmouse_y_ = UINT_16::MAX;
+    if (vmouse_x_ > Range::MAX<uint16_t>) vmouse_x_ = Range::MAX<uint16_t>;
+    if (vmouse_y_ > Range::MAX<uint16_t>) vmouse_y_ = Range::MAX<uint16_t>;
     if (vmouse_y_ < 0) vmouse_y_ = 0;
 
     if (gp_in.buttons & Gamepad::BUTTON_L3)
diff --git a/Firmware/RP2040/src/USBDevice/DeviceManager.cpp b/Firmware/RP2040/src/USBDevice/DeviceManager.cpp
index 0f9fa30..96ae298 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceManager.cpp
+++ b/Firmware/RP2040/src/USBDevice/DeviceManager.cpp
@@ -16,43 +16,43 @@
 #include "USBDevice/DeviceDriver/UARTBridge/UARTBridge.h"
 #endif // defined(CONFIG_EN_UART_BRIDGE)
 
-void DeviceManager::initialize_driver(DeviceDriver::Type driver_type, Gamepad(&gamepads)[MAX_GAMEPADS])
+void DeviceManager::initialize_driver(DeviceDriverType driver_type, Gamepad(&gamepads)[MAX_GAMEPADS])
 {
     bool has_analog = false; //TODO: Put gamepad setup in the drivers themselves
     switch (driver_type)
     {
-        case DeviceDriver::Type::DINPUT:
+        case DeviceDriverType::DINPUT:
             has_analog = true;
             device_driver_ = std::make_unique<DInputDevice>();
             break;
-        case DeviceDriver::Type::PS3:
+        case DeviceDriverType::PS3:
             has_analog = true;
             device_driver_ = std::make_unique<PS3Device>();
             break;
-        case DeviceDriver::Type::PSCLASSIC:
+        case DeviceDriverType::PSCLASSIC:
             device_driver_ = std::make_unique<PSClassicDevice>();
             break;
-        case DeviceDriver::Type::SWITCH:
+        case DeviceDriverType::SWITCH:
             device_driver_ = std::make_unique<SwitchDevice>();
             break;
-        case DeviceDriver::Type::XINPUT:
+        case DeviceDriverType::XINPUT:
             device_driver_ = std::make_unique<XInputDevice>();
             break;
-        case DeviceDriver::Type::XBOXOG:
+        case DeviceDriverType::XBOXOG:
             has_analog = true;
             device_driver_ = std::make_unique<XboxOGDevice>();
             break;
-        case DeviceDriver::Type::XBOXOG_SB:
+        case DeviceDriverType::XBOXOG_SB:
             device_driver_ = std::make_unique<XboxOGSBDevice>();
             break;
-        case DeviceDriver::Type::XBOXOG_XR:
+        case DeviceDriverType::XBOXOG_XR:
             device_driver_ = std::make_unique<XboxOGXRDevice>();
             break;
-        case DeviceDriver::Type::WEBAPP:
+        case DeviceDriverType::WEBAPP:
             device_driver_ = std::make_unique<WebAppDevice>();
             break;
 #if defined(CONFIG_EN_UART_BRIDGE)
-        case DeviceDriver::Type::UART_BRIDGE:
+        case DeviceDriverType::UART_BRIDGE:
             device_driver_ = std::make_unique<UARTBridgeDevice>();
             break;
 #endif //defined(CONFIG_EN_UART_BRIDGE)
diff --git a/Firmware/RP2040/src/USBDevice/DeviceManager.h b/Firmware/RP2040/src/USBDevice/DeviceManager.h
index 227fcf1..515f688 100644
--- a/Firmware/RP2040/src/USBDevice/DeviceManager.h
+++ b/Firmware/RP2040/src/USBDevice/DeviceManager.h
@@ -5,6 +5,7 @@
 #include <memory>
 
 #include "Gamepad.h"
+#include "USBDevice/DeviceDriver/DeviceDriverTypes.h"
 #include "USBDevice/DeviceDriver/DeviceDriver.h"
 
 class DeviceManager 
@@ -20,7 +21,7 @@ public:
 	}
 
 	//Must be called before any other method
-	void initialize_driver(DeviceDriver::Type driver_type, Gamepad(&gamepads)[MAX_GAMEPADS]);
+	void initialize_driver(DeviceDriverType driver_type, Gamepad(&gamepads)[MAX_GAMEPADS]);
 	
 	DeviceDriver* get_driver() { return device_driver_.get(); }
 	
diff --git a/Firmware/RP2040/src/USBHost/HardwareIDs.h b/Firmware/RP2040/src/USBHost/HardwareIDs.h
index f9cf292..b02b794 100644
--- a/Firmware/RP2040/src/USBHost/HardwareIDs.h
+++ b/Firmware/RP2040/src/USBHost/HardwareIDs.h
@@ -71,19 +71,19 @@ struct HostTypeMap
 {
     const HardwareID* ids;
     size_t num_ids;
-    HostDriver::Type type;
+    HostDriverType type;
 };
 
 static const HostTypeMap HOST_TYPE_MAP[] = 
 {
-    { DINPUT_IDS, sizeof(DINPUT_IDS) / sizeof(HardwareID), HostDriver::Type::DINPUT },
-    { PS4_IDS, sizeof(PS4_IDS) / sizeof(HardwareID), HostDriver::Type::PS4 },
-    { PS5_IDS, sizeof(PS5_IDS) / sizeof(HardwareID), HostDriver::Type::PS5 },
-    { PS3_IDS, sizeof(PS3_IDS) / sizeof(HardwareID), HostDriver::Type::PS3 },
-    { SWITCH_WIRED_IDS, sizeof(SWITCH_WIRED_IDS) / sizeof(HardwareID), HostDriver::Type::SWITCH },
-    { SWITCH_PRO_IDS, sizeof(SWITCH_PRO_IDS) / sizeof(HardwareID), HostDriver::Type::SWITCH_PRO },
-    { PSCLASSIC_IDS, sizeof(PSCLASSIC_IDS) / sizeof(HardwareID), HostDriver::Type::PSCLASSIC },
-    { N64_IDS, sizeof(N64_IDS) / sizeof(HardwareID), HostDriver::Type::N64 },
+    { DINPUT_IDS, sizeof(DINPUT_IDS) / sizeof(HardwareID), HostDriverType::DINPUT },
+    { PS4_IDS, sizeof(PS4_IDS) / sizeof(HardwareID), HostDriverType::PS4 },
+    { PS5_IDS, sizeof(PS5_IDS) / sizeof(HardwareID), HostDriverType::PS5 },
+    { PS3_IDS, sizeof(PS3_IDS) / sizeof(HardwareID), HostDriverType::PS3 },
+    { SWITCH_WIRED_IDS, sizeof(SWITCH_WIRED_IDS) / sizeof(HardwareID), HostDriverType::SWITCH },
+    { SWITCH_PRO_IDS, sizeof(SWITCH_PRO_IDS) / sizeof(HardwareID), HostDriverType::SWITCH_PRO },
+    { PSCLASSIC_IDS, sizeof(PSCLASSIC_IDS) / sizeof(HardwareID), HostDriverType::PSCLASSIC },
+    { N64_IDS, sizeof(N64_IDS) / sizeof(HardwareID), HostDriverType::N64 },
 };
 
 #endif // _HW_ID_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/DInput/DInput.cpp b/Firmware/RP2040/src/USBHost/HostDriver/DInput/DInput.cpp
index b3a11f8..267ebba 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/DInput/DInput.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/DInput/DInput.cpp
@@ -81,25 +81,25 @@ void DInputHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t insta
 
     if (in_report->l2_axis > 0)
     {
-        gp_in.trigger_l = in_report->l2_axis;
+        gp_in.trigger_l = gamepad.scale_trigger_l(in_report->l2_axis);
     }
     else
     {
-        gp_in.trigger_l = (in_report->buttons[0] & DInput::Buttons0::L2) ? UINT_8::MAX : UINT_8::MIN;
+        gp_in.trigger_l = (in_report->buttons[0] & DInput::Buttons0::L2) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
     }
     if (in_report->r2_axis > 0)
     {
-        gp_in.trigger_r = in_report->r2_axis;
+        gp_in.trigger_r = gamepad.scale_trigger_r(in_report->r2_axis);
     }
     else
     {
-        gp_in.trigger_r = (in_report->buttons[0] & DInput::Buttons0::R2) ? UINT_8::MAX : UINT_8::MIN;
+        gp_in.trigger_r = (in_report->buttons[0] & DInput::Buttons0::R2) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
     }
 
-    gp_in.joystick_lx = Scale::uint8_to_int16(in_report->joystick_lx);
-    gp_in.joystick_ly = Scale::uint8_to_int16(in_report->joystick_ly);
-    gp_in.joystick_rx = Scale::uint8_to_int16(in_report->joystick_rx);
-    gp_in.joystick_ry = Scale::uint8_to_int16(in_report->joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/HIDGeneric/HIDGeneric.cpp b/Firmware/RP2040/src/USBHost/HostDriver/HIDGeneric/HIDGeneric.cpp
index e3dc78f..7fa1b16 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/HIDGeneric/HIDGeneric.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/HIDGeneric/HIDGeneric.cpp
@@ -1,5 +1,9 @@
+#include <cstring>
 #include <memory>
 
+#include "host/usbh.h"
+#include "class/hid/hid_host.h"
+
 #include "USBHost/HIDParser/HIDReportDescriptor.h"
 #include "USBHost/HostDriver/HIDGeneric/HIDGeneric.h"
 
@@ -64,10 +68,10 @@ void HIDHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
             break;
     }
 
-    gp_in.joystick_lx = hid_joystick_data_.X;
-    gp_in.joystick_ly = hid_joystick_data_.Y;
-    gp_in.joystick_rx = hid_joystick_data_.Z;
-    gp_in.joystick_ry = hid_joystick_data_.Rz;
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(hid_joystick_data_.X);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(hid_joystick_data_.Y);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(hid_joystick_data_.Z);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(hid_joystick_data_.Rz);
 
     if (hid_joystick_data_.buttons[1])  gp_in.buttons |= gamepad.MAP_BUTTON_X;
     if (hid_joystick_data_.buttons[2])  gp_in.buttons |= gamepad.MAP_BUTTON_A;
@@ -75,8 +79,8 @@ void HIDHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
     if (hid_joystick_data_.buttons[4])  gp_in.buttons |= gamepad.MAP_BUTTON_Y;
     if (hid_joystick_data_.buttons[5])  gp_in.buttons |= gamepad.MAP_BUTTON_LB;
     if (hid_joystick_data_.buttons[6])  gp_in.buttons |= gamepad.MAP_BUTTON_RB;
-    if (hid_joystick_data_.buttons[7])  gp_in.trigger_l = UINT_8::MAX;
-    if (hid_joystick_data_.buttons[8])  gp_in.trigger_r = UINT_8::MAX;
+    if (hid_joystick_data_.buttons[7])  gp_in.trigger_l = Range::MAX<uint8_t>;
+    if (hid_joystick_data_.buttons[8])  gp_in.trigger_r = Range::MAX<uint8_t>;
     if (hid_joystick_data_.buttons[9])  gp_in.buttons |= gamepad.MAP_BUTTON_BACK;
     if (hid_joystick_data_.buttons[10]) gp_in.buttons |= gamepad.MAP_BUTTON_START;
     if (hid_joystick_data_.buttons[11]) gp_in.buttons |= gamepad.MAP_BUTTON_L3;
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/HostDriver.h b/Firmware/RP2040/src/USBHost/HostDriver/HostDriver.h
index 265b0ae..d213e20 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/HostDriver.h
+++ b/Firmware/RP2040/src/USBHost/HostDriver/HostDriver.h
@@ -6,30 +6,12 @@
 #include "UserSettings/UserProfile.h"
 #include "UserSettings/UserSettings.h"
 #include "Gamepad.h"
+#include "USBHost/HostDriver/HostDriverTypes.h"
 
 //Use HostManager, don't use this directly
 class HostDriver
 {
 public:
-    enum class Type
-    {
-        UNKNOWN = 0,
-        SWITCH_PRO,
-        SWITCH,
-        PSCLASSIC,
-        DINPUT,
-        PS3,
-        PS4,
-        PS5,
-        N64,
-        XBOXOG,
-        XBOXONE,
-        XBOX360W,
-        XBOX360,
-        XBOX360_CHATPAD,
-        HID_GENERIC
-    };
-
     HostDriver(uint8_t idx)
         : idx_(idx) {}
 
@@ -49,12 +31,12 @@ protected:
     {
         Gamepad::PadOut gp_out = gamepad.get_pad_out();
         bool reset = false;
-        if (gp_out.rumble_l != UINT_8::MAX)
+        if (gp_out.rumble_l != Range::MAX<uint8_t>)
         {
             gp_out.rumble_l = 0;
             reset = true;
         }
-        if (gp_out.rumble_r != UINT_8::MAX)
+        if (gp_out.rumble_r != Range::MAX<uint8_t>)
         {
             gp_out.rumble_r = 0;
             reset = true;
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/HostDriverTypes.h b/Firmware/RP2040/src/USBHost/HostDriver/HostDriverTypes.h
new file mode 100644
index 0000000..9099d19
--- /dev/null
+++ b/Firmware/RP2040/src/USBHost/HostDriver/HostDriverTypes.h
@@ -0,0 +1,25 @@
+#ifndef HOST_DRIVER_TYPES_H
+#define HOST_DRIVER_TYPES_H
+
+#include <cstdint>
+
+enum class HostDriverType
+{
+    UNKNOWN = 0,
+    SWITCH_PRO,
+    SWITCH,
+    PSCLASSIC,
+    DINPUT,
+    PS3,
+    PS4,
+    PS5,
+    N64,
+    XBOXOG,
+    XBOXONE,
+    XBOX360W,
+    XBOX360,
+    XBOX360_CHATPAD,
+    HID_GENERIC
+};
+
+#endif // HOST_DRIVER_TYPES_H
\ No newline at end of file
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/N64/N64.cpp b/Firmware/RP2040/src/USBHost/HostDriver/N64/N64.cpp
index 327dae6..ffef075 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/N64/N64.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/N64/N64.cpp
@@ -1,5 +1,8 @@
 #include <cstring>
 
+#include "host/usbh.h"
+#include "class/hid/hid_host.h"
+
 #include "USBHost/HostDriver/N64/N64.h"
 
 void N64Host::initialize(Gamepad& gamepad, uint8_t address, uint8_t instance, const uint8_t* report_desc, uint16_t desc_len)
@@ -91,13 +94,13 @@ void N64Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
             break;
     }
 
-    gp_in.joystick_ry = Scale::uint8_to_int16(in_report->joystick_y);
-    gp_in.joystick_rx = Scale::uint8_to_int16(in_report->joystick_x);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_y);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_x);
 
-    gp_in.trigger_l = (in_report->buttons & N64::Buttons::L) ? UINT_8::MAX : UINT_8::MIN;
+    gp_in.trigger_l = (in_report->buttons & N64::Buttons::L) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
 
-    gp_in.joystick_ly = Scale::uint8_to_int16(joy_ry);
-    gp_in.joystick_lx = Scale::uint8_to_int16(joy_rx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(joy_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(joy_rx);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/PS3/PS3.cpp b/Firmware/RP2040/src/USBHost/HostDriver/PS3/PS3.cpp
index fb3755c..cd34fbb 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/PS3/PS3.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/PS3/PS3.cpp
@@ -111,17 +111,17 @@ void PS3Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
     if (in_report->buttons[0] & PS3::Buttons0::DPAD_LEFT)  gp_in.dpad |= gamepad.MAP_DPAD_LEFT;
     if (in_report->buttons[0] & PS3::Buttons0::DPAD_RIGHT) gp_in.dpad |= gamepad.MAP_DPAD_RIGHT;
 
-    if (in_report->buttons[0] & PS3::Buttons0::SELECT)      gp_in.buttons |= gamepad.MAP_BUTTON_BACK;
-    if (in_report->buttons[0] & PS3::Buttons0::START)       gp_in.buttons |= gamepad.MAP_BUTTON_START;
-    if (in_report->buttons[0] & PS3::Buttons0::L3)          gp_in.buttons |= gamepad.MAP_BUTTON_L3;
-    if (in_report->buttons[0] & PS3::Buttons0::R3)          gp_in.buttons |= gamepad.MAP_BUTTON_R3;
-    if (in_report->buttons[1] & PS3::Buttons1::L1)          gp_in.buttons |= gamepad.MAP_BUTTON_LB;
-    if (in_report->buttons[1] & PS3::Buttons1::R1)          gp_in.buttons |= gamepad.MAP_BUTTON_RB;
-    if (in_report->buttons[1] & PS3::Buttons1::TRIANGLE)    gp_in.buttons |= gamepad.MAP_BUTTON_Y;
-    if (in_report->buttons[1] & PS3::Buttons1::CIRCLE)      gp_in.buttons |= gamepad.MAP_BUTTON_B;
-    if (in_report->buttons[1] & PS3::Buttons1::CROSS)       gp_in.buttons |= gamepad.MAP_BUTTON_A;
-    if (in_report->buttons[1] & PS3::Buttons1::SQUARE)      gp_in.buttons |= gamepad.MAP_BUTTON_X;
-    if (in_report->buttons[2] & PS3::Buttons2::SYS)         gp_in.buttons |= gamepad.MAP_BUTTON_SYS;
+    if (in_report->buttons[0] & PS3::Buttons0::SELECT)   gp_in.buttons |= gamepad.MAP_BUTTON_BACK;
+    if (in_report->buttons[0] & PS3::Buttons0::START)    gp_in.buttons |= gamepad.MAP_BUTTON_START;
+    if (in_report->buttons[0] & PS3::Buttons0::L3)       gp_in.buttons |= gamepad.MAP_BUTTON_L3;
+    if (in_report->buttons[0] & PS3::Buttons0::R3)       gp_in.buttons |= gamepad.MAP_BUTTON_R3;
+    if (in_report->buttons[1] & PS3::Buttons1::L1)       gp_in.buttons |= gamepad.MAP_BUTTON_LB;
+    if (in_report->buttons[1] & PS3::Buttons1::R1)       gp_in.buttons |= gamepad.MAP_BUTTON_RB;
+    if (in_report->buttons[1] & PS3::Buttons1::TRIANGLE) gp_in.buttons |= gamepad.MAP_BUTTON_Y;
+    if (in_report->buttons[1] & PS3::Buttons1::CIRCLE)   gp_in.buttons |= gamepad.MAP_BUTTON_B;
+    if (in_report->buttons[1] & PS3::Buttons1::CROSS)    gp_in.buttons |= gamepad.MAP_BUTTON_A;
+    if (in_report->buttons[1] & PS3::Buttons1::SQUARE)   gp_in.buttons |= gamepad.MAP_BUTTON_X;
+    if (in_report->buttons[2] & PS3::Buttons2::SYS)      gp_in.buttons |= gamepad.MAP_BUTTON_SYS;
 
     if (gamepad.analog_enabled())
     {
@@ -137,13 +137,13 @@ void PS3Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
         gp_in.analog[gamepad.MAP_ANALOG_OFF_RB] = in_report->r1_axis;
     }
 
-    gp_in.trigger_l = in_report->l2_axis;
-    gp_in.trigger_r = in_report->r2_axis;
+    gp_in.trigger_l = gamepad.scale_trigger_l(in_report->l2_axis);
+    gp_in.trigger_r = gamepad.scale_trigger_r(in_report->r2_axis);
 
-    gp_in.joystick_lx = Scale::uint8_to_int16(in_report->joystick_lx);
-    gp_in.joystick_ly = Scale::uint8_to_int16(in_report->joystick_ly);
-    gp_in.joystick_rx = Scale::uint8_to_int16(in_report->joystick_rx);
-    gp_in.joystick_ry = Scale::uint8_to_int16(in_report->joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ly(in_report->joystick_ry);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/PS4/PS4.cpp b/Firmware/RP2040/src/USBHost/HostDriver/PS4/PS4.cpp
index 0acb2c8..3ac3ebf 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/PS4/PS4.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/PS4/PS4.cpp
@@ -69,13 +69,13 @@ void PS4Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
     if (in_report_.buttons[2] & PS4::Buttons2::PS)       gp_in.buttons |= gamepad.MAP_BUTTON_SYS;
     if (in_report_.buttons[2] & PS4::Buttons2::TP)       gp_in.buttons |= gamepad.MAP_BUTTON_MISC;
 
-    gp_in.trigger_l = in_report_.trigger_l;
-    gp_in.trigger_r = in_report_.trigger_r;
+    gp_in.trigger_l = gamepad.scale_trigger_l(in_report_.trigger_l);
+    gp_in.trigger_r = gamepad.scale_trigger_r(in_report_.trigger_r);
 
-    gp_in.joystick_lx = Scale::uint8_to_int16(in_report_.joystick_lx);
-    gp_in.joystick_ly = Scale::uint8_to_int16(in_report_.joystick_ly);
-    gp_in.joystick_rx = Scale::uint8_to_int16(in_report_.joystick_rx);
-    gp_in.joystick_ry = Scale::uint8_to_int16(in_report_.joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report_.joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report_.joystick_ly);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report_.joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report_.joystick_ry);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.cpp b/Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.cpp
index 510ae3a..c710182 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/PS5/PS5.cpp
@@ -86,13 +86,13 @@ void PS5Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance
     if (in_report->buttons[2] & PS5::Buttons2::PS)       gp_in.buttons |= gamepad.MAP_BUTTON_SYS;
     if (in_report->buttons[2] & PS5::Buttons2::MUTE)     gp_in.buttons |= gamepad.MAP_BUTTON_MISC;
 
-    gp_in.trigger_l = in_report->trigger_l;
-    gp_in.trigger_r = in_report->trigger_r;
+    gp_in.trigger_l = gamepad.scale_trigger_l(in_report->trigger_l);
+    gp_in.trigger_r = gamepad.scale_trigger_r(in_report->trigger_r);
 
-    gp_in.joystick_lx = Scale::uint8_to_int16(in_report->joystick_lx);
-    gp_in.joystick_ly = Scale::uint8_to_int16(in_report->joystick_ly);
-    gp_in.joystick_rx = Scale::uint8_to_int16(in_report->joystick_rx);
-    gp_in.joystick_ry = Scale::uint8_to_int16(in_report->joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry);
     
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/PSClassic/PSClassic.cpp b/Firmware/RP2040/src/USBHost/HostDriver/PSClassic/PSClassic.cpp
index 4d5c30a..e0d3d5f 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/PSClassic/PSClassic.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/PSClassic/PSClassic.cpp
@@ -60,8 +60,8 @@ void PSClassicHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t in
     if (in_report->buttons & PSClassic::Buttons::SELECT)   gp_in.buttons |= gamepad.MAP_BUTTON_BACK;
     if (in_report->buttons & PSClassic::Buttons::START)    gp_in.buttons |= gamepad.MAP_BUTTON_START;
 
-    gp_in.trigger_l = (in_report->buttons & PSClassic::Buttons::L2) ? UINT_8::MAX : UINT_8::MIN;
-    gp_in.trigger_r = (in_report->buttons & PSClassic::Buttons::R2) ? UINT_8::MAX : UINT_8::MIN;
+    gp_in.trigger_l = (in_report->buttons & PSClassic::Buttons::L2) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
+    gp_in.trigger_r = (in_report->buttons & PSClassic::Buttons::R2) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/SwitchPro/SwitchPro.cpp b/Firmware/RP2040/src/USBHost/HostDriver/SwitchPro/SwitchPro.cpp
index fd9eed5..e5d7caf 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/SwitchPro/SwitchPro.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/SwitchPro/SwitchPro.cpp
@@ -12,18 +12,6 @@ void SwitchProHost::initialize(Gamepad& gamepad, uint8_t address, uint8_t instan
     init_switch_host(gamepad, address, instance);
 }
 
-// bool SwitchProHost::send_handshake(uint8_t address, uint8_t instance)
-// {
-//     std::array<uint8_t, 2> handshake = { SwitchPro::CMD::HID, SwitchPro::CMD::HANDSHAKE };
-//     return tuh_hid_send_report(address, instance, 0, handshake.data(), handshake.size());
-// }
-
-// bool SwitchProHost::disable_timeout(uint8_t address, uint8_t instance)
-// {
-//     std::array<uint8_t, 2> timeout = { SwitchPro::CMD::HID, SwitchPro::CMD::DISABLE_TIMEOUT };
-//     return tuh_hid_send_report(address, instance, 0, timeout.data(), timeout.size());
-// }
-
 uint8_t SwitchProHost::get_output_sequence_counter()
 {
     uint8_t counter = sequence_counter_;
@@ -127,13 +115,14 @@ void SwitchProHost::init_switch_host(Gamepad& gamepad, uint8_t address, uint8_t
             if (tuh_hid_send_report(address, instance, 0, &out_report_, report_size))
             {
                 init_state_ = InitState::DONE;
+                tuh_hid_receive_report(address, instance);
             }
             break;
         default:
             break;
     }
 
-    tuh_hid_receive_report(address, instance);
+    // tuh_hid_receive_report(address, instance);
 }
 
 void SwitchProHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t instance, const uint8_t* report, uint16_t len)
@@ -171,13 +160,18 @@ void SwitchProHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t in
     if (in_report->buttons[2] & SwitchPro::Buttons2::DPAD_LEFT)  gp_in.dpad |= gamepad.MAP_DPAD_LEFT;
     if (in_report->buttons[2] & SwitchPro::Buttons2::DPAD_RIGHT) gp_in.dpad |= gamepad.MAP_DPAD_RIGHT;
 
-    gp_in.trigger_l = in_report->buttons[2] & SwitchPro::Buttons2::ZL ? UINT_8::MAX : UINT_8::MIN;
-    gp_in.trigger_r = in_report->buttons[0] & SwitchPro::Buttons0::ZR ? UINT_8::MAX : UINT_8::MIN;
+    gp_in.trigger_l = in_report->buttons[2] & SwitchPro::Buttons2::ZL ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
+    gp_in.trigger_r = in_report->buttons[0] & SwitchPro::Buttons0::ZR ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
 
-    gp_in.joystick_lx = normalize_axis(in_report->joysticks[0] | ((in_report->joysticks[1] & 0xF) << 8));
-    gp_in.joystick_ly = Scale::invert_joy(normalize_axis((in_report->joysticks[1] >> 4) | (in_report->joysticks[2] << 4)));
-    gp_in.joystick_rx = normalize_axis(in_report->joysticks[3] | ((in_report->joysticks[4] & 0xF) << 8));
-    gp_in.joystick_ry = Scale::invert_joy(normalize_axis((in_report->joysticks[4] >> 4) | (in_report->joysticks[5] << 4)));
+    uint16_t joy_lx = in_report->joysticks[0] | ((in_report->joysticks[1] & 0xF) << 8);
+    uint16_t joy_ly = (in_report->joysticks[1] >> 4) | (in_report->joysticks[2] << 4);
+    uint16_t joy_rx = in_report->joysticks[3] | ((in_report->joysticks[4] & 0xF) << 8);
+    uint16_t joy_ry = (in_report->joysticks[4] >> 4) | (in_report->joysticks[5] << 4);
+
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(normalize_axis(joy_lx));
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(normalize_axis(joy_ly), true);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(normalize_axis(joy_rx));
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(normalize_axis(joy_ry), true);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/SwitchPro/SwitchPro.h b/Firmware/RP2040/src/USBHost/HostDriver/SwitchPro/SwitchPro.h
index efaffd6..a599b72 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/SwitchPro/SwitchPro.h
+++ b/Firmware/RP2040/src/USBHost/HostDriver/SwitchPro/SwitchPro.h
@@ -5,6 +5,7 @@
 
 #include "Descriptors/SwitchPro.h"
 #include "USBHost/HostDriver/HostDriver.h"
+#include "Board/ogxm_log.h"
 
 class SwitchProHost : public HostDriver
 {
@@ -35,28 +36,18 @@ private:
     SwitchPro::OutReport out_report_{};
 
     void init_switch_host(Gamepad& gamepad, uint8_t address, uint8_t instance);
-    // bool send_handshake(uint8_t address, uint8_t instance);
-    // bool disable_timeout(uint8_t address, uint8_t instance);
     uint8_t get_output_sequence_counter();
 
     static inline int16_t normalize_axis(uint16_t value)
     {
-        /*  12bit value from the controller doesnt cover the full 12bit range seemingly
-            isn't completely centered at 2047 either so I may be missing something here
-            tried to get as close as possible with the multiplier */
-
+        /*  12bit value from the controller doesnt cover the full 12bit range seemingly,
+            isn't completely centered at 2047 either so I may be missing something here.
+            Tried to get as close as possible with the multiplier */
+            
+        OGXM_LOG("Value: %d\n", value);
         int32_t normalized_value = (value - 2047) * 22;
-
-        if (normalized_value < INT16_MIN) 
-        {
-            return INT16_MIN;
-        } 
-        else if (normalized_value > INT16_MAX) 
-        {
-            return INT16_MAX;
-        }
-
-        return static_cast<int16_t>(normalized_value); 
+        OGXM_LOG("Normalized value: %d\n", normalized_value);
+        return Range::clamp<int16_t>(normalized_value);
     }
 };
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/SwitchWired/SwitchWired.cpp b/Firmware/RP2040/src/USBHost/HostDriver/SwitchWired/SwitchWired.cpp
index 2d35274..8c8c7c7 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/SwitchWired/SwitchWired.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/SwitchWired/SwitchWired.cpp
@@ -64,13 +64,13 @@ void SwitchWiredHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t
     if (in_report->buttons & SwitchWired::Buttons::L3)      gp_in.buttons |= gamepad.MAP_BUTTON_L3;
     if (in_report->buttons & SwitchWired::Buttons::R3)      gp_in.buttons |= gamepad.MAP_BUTTON_R3;
 
-    gp_in.trigger_l = (in_report->buttons & SwitchWired::Buttons::ZL) ? UINT_8::MAX : UINT_8::MIN;
-    gp_in.trigger_r = (in_report->buttons & SwitchWired::Buttons::ZR) ? UINT_8::MAX : UINT_8::MIN;
+    gp_in.trigger_l = (in_report->buttons & SwitchWired::Buttons::ZL) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
+    gp_in.trigger_r = (in_report->buttons & SwitchWired::Buttons::ZR) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
 
-    gp_in.joystick_lx = Scale::uint8_to_int16(in_report->joystick_lx);
-    gp_in.joystick_ly = Scale::uint8_to_int16(in_report->joystick_ly);
-    gp_in.joystick_rx = Scale::uint8_to_int16(in_report->joystick_rx);
-    gp_in.joystick_ry = Scale::uint8_to_int16(in_report->joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360.cpp b/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360.cpp
index 8aa3470..ae3776e 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360.cpp
@@ -7,7 +7,6 @@
 
 void Xbox360Host::initialize(Gamepad& gamepad, uint8_t address, uint8_t instance, const uint8_t* report_desc, uint16_t desc_len)
 {
-    std::memset(&prev_in_report_, 0, sizeof(XInput::InReport));
     tuh_xinput::set_led(address, instance, idx_ + 1, true);
     tuh_xinput::receive_report(address, instance);
 }
@@ -40,13 +39,13 @@ void Xbox360Host::process_report(Gamepad& gamepad, uint8_t address, uint8_t inst
     if (in_report_->buttons[1] & XInput::Buttons1::X)      gp_in.buttons |= gamepad.MAP_BUTTON_X;
     if (in_report_->buttons[1] & XInput::Buttons1::Y)      gp_in.buttons |= gamepad.MAP_BUTTON_Y;
 
-    gp_in.trigger_l = in_report_->trigger_l;    
-    gp_in.trigger_r = in_report_->trigger_r;
+    gp_in.trigger_l = gamepad.scale_trigger_l(in_report_->trigger_l);
+    gp_in.trigger_r = gamepad.scale_trigger_r(in_report_->trigger_r);
 
-    gp_in.joystick_lx = in_report_->joystick_lx;
-    gp_in.joystick_ly = Scale::invert_joy(in_report_->joystick_ly);
-    gp_in.joystick_rx = in_report_->joystick_rx;
-    gp_in.joystick_ry = Scale::invert_joy(in_report_->joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report_->joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report_->joystick_ly, true);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report_->joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report_->joystick_ry, true);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.cpp b/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.cpp
index 4d4e500..1e1dbc3 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/XInput/Xbox360W.cpp
@@ -8,7 +8,7 @@
 #include "TaskQueue/TaskQueue.h"
 #include "USBHost/HostDriver/XInput/tuh_xinput/tuh_xinput.h"
 #include "USBHost/HostDriver/XInput/Xbox360W.h"
-#include "OGXMini/Debug.h"
+#include "Board/ogxm_log.h"
 
 Xbox360WHost::~Xbox360WHost()
 {
@@ -71,13 +71,13 @@ void Xbox360WHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t ins
     if (in_report->buttons[1] & XInput::Buttons1::X)      gp_in.buttons |= gamepad.MAP_BUTTON_X;
     if (in_report->buttons[1] & XInput::Buttons1::Y)      gp_in.buttons |= gamepad.MAP_BUTTON_Y;
 
-    gp_in.trigger_l = in_report->trigger_l;
-    gp_in.trigger_r = in_report->trigger_r;
+    gp_in.trigger_l = gamepad.scale_trigger_l(in_report->trigger_l);
+    gp_in.trigger_r = gamepad.scale_trigger_r(in_report->trigger_r);
 
-    gp_in.joystick_lx = in_report->joystick_lx;
-    gp_in.joystick_ly = Scale::invert_joy(in_report->joystick_ly);
-    gp_in.joystick_rx = in_report->joystick_rx;
-    gp_in.joystick_ry = Scale::invert_joy(in_report->joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly, true);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry, true);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOG.cpp b/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOG.cpp
index 37085d2..858b073 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOG.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOG.cpp
@@ -48,19 +48,19 @@ void XboxOGHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t insta
         gp_in.analog[gamepad.MAP_ANALOG_OFF_Y]  = in_report->y;
         gp_in.analog[gamepad.MAP_ANALOG_OFF_LB] = in_report->black;
         gp_in.analog[gamepad.MAP_ANALOG_OFF_RB] = in_report->white;
-        gp_in.analog[gamepad.MAP_ANALOG_OFF_UP]    = (in_report->buttons & XboxOG::GP::Buttons::DPAD_UP) ? UINT_8::MAX : UINT_8::MIN;
-        gp_in.analog[gamepad.MAP_ANALOG_OFF_DOWN]  = (in_report->buttons & XboxOG::GP::Buttons::DPAD_DOWN) ? UINT_8::MAX : UINT_8::MIN;
-        gp_in.analog[gamepad.MAP_ANALOG_OFF_LEFT]  = (in_report->buttons & XboxOG::GP::Buttons::DPAD_LEFT) ? UINT_8::MAX : UINT_8::MIN;
-        gp_in.analog[gamepad.MAP_ANALOG_OFF_RIGHT] = (in_report->buttons & XboxOG::GP::Buttons::DPAD_RIGHT) ? UINT_8::MAX : UINT_8::MIN;
+        gp_in.analog[gamepad.MAP_ANALOG_OFF_UP]    = (in_report->buttons & XboxOG::GP::Buttons::DPAD_UP) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
+        gp_in.analog[gamepad.MAP_ANALOG_OFF_DOWN]  = (in_report->buttons & XboxOG::GP::Buttons::DPAD_DOWN) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
+        gp_in.analog[gamepad.MAP_ANALOG_OFF_LEFT]  = (in_report->buttons & XboxOG::GP::Buttons::DPAD_LEFT) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
+        gp_in.analog[gamepad.MAP_ANALOG_OFF_RIGHT] = (in_report->buttons & XboxOG::GP::Buttons::DPAD_RIGHT) ? Range::MAX<uint8_t> : Range::MIN<uint8_t>;
     }
 
-    gp_in.trigger_l = in_report->trigger_l;
-    gp_in.trigger_r = in_report->trigger_r;
+    gp_in.trigger_l = gamepad.scale_trigger_l(in_report->trigger_l);
+    gp_in.trigger_r = gamepad.scale_trigger_r(in_report->trigger_r);
 
-    gp_in.joystick_lx = in_report->joystick_lx;
-    gp_in.joystick_ly = Scale::invert_joy(in_report->joystick_ly);
-    gp_in.joystick_rx = in_report->joystick_rx;
-    gp_in.joystick_ry = Scale::invert_joy(in_report->joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly, true);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry, true);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOne.cpp b/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOne.cpp
index be2a6b4..bf3667c 100644
--- a/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOne.cpp
+++ b/Firmware/RP2040/src/USBHost/HostDriver/XInput/XboxOne.cpp
@@ -39,13 +39,13 @@ void XboxOneHost::process_report(Gamepad& gamepad, uint8_t address, uint8_t inst
     if (in_report->buttons[0] & XboxOne::Buttons0::X)     gp_in.buttons |= gamepad.MAP_BUTTON_X;
     if (in_report->buttons[0] & XboxOne::Buttons0::Y)     gp_in.buttons |= gamepad.MAP_BUTTON_Y;
 
-    gp_in.trigger_l = static_cast<uint8_t>(in_report->trigger_l >> 2);
-    gp_in.trigger_r = static_cast<uint8_t>(in_report->trigger_r >> 2);
+    gp_in.trigger_l = gamepad.scale_trigger_l(static_cast<uint8_t>(in_report->trigger_l >> 2));
+    gp_in.trigger_r = gamepad.scale_trigger_r(static_cast<uint8_t>(in_report->trigger_r >> 2));
 
-    gp_in.joystick_lx = in_report->joystick_lx;
-    gp_in.joystick_ly = Scale::invert_joy(in_report->joystick_ly);
-    gp_in.joystick_rx = in_report->joystick_rx;
-    gp_in.joystick_ry = Scale::invert_joy(in_report->joystick_ry);
+    gp_in.joystick_lx = gamepad.scale_joystick_lx(in_report->joystick_lx);
+    gp_in.joystick_ly = gamepad.scale_joystick_ly(in_report->joystick_ly, true);
+    gp_in.joystick_rx = gamepad.scale_joystick_rx(in_report->joystick_rx);
+    gp_in.joystick_ry = gamepad.scale_joystick_ry(in_report->joystick_ry, true);
 
     gamepad.set_pad_in(gp_in);
 
diff --git a/Firmware/RP2040/src/USBHost/HostManager.h b/Firmware/RP2040/src/USBHost/HostManager.h
index e287a9a..0826204 100644
--- a/Firmware/RP2040/src/USBHost/HostManager.h
+++ b/Firmware/RP2040/src/USBHost/HostManager.h
@@ -53,7 +53,7 @@ public:
 	}
 
 	//XInput doesn't need report_desc or desc_len
-	inline bool setup_driver(const HostDriver::Type driver_type, const uint8_t address, const uint8_t instance, uint8_t const* report_desc = nullptr, uint16_t desc_len = 0)
+	inline bool setup_driver(const HostDriverType driver_type, const uint8_t address, const uint8_t instance, uint8_t const* report_desc = nullptr, uint16_t desc_len = 0)
 	{
 		uint8_t gp_idx = find_free_gamepad();
 		if (gp_idx == INVALID_IDX || instance >= MAX_INTERFACES)
@@ -74,40 +74,40 @@ public:
 
 		switch (driver_type)
 		{
-			case HostDriver::Type::PS5:
+			case HostDriverType::PS5:
 				interface.driver = std::make_unique<PS5Host>(gp_idx);
 				break;
-			case HostDriver::Type::PS4:
+			case HostDriverType::PS4:
 				interface.driver = std::make_unique<PS4Host>(gp_idx);
 				break;
-			case HostDriver::Type::PS3:
+			case HostDriverType::PS3:
 				interface.driver = std::make_unique<PS3Host>(gp_idx);
 				break;
-			case HostDriver::Type::DINPUT:
+			case HostDriverType::DINPUT:
 				interface.driver = std::make_unique<DInputHost>(gp_idx);
 				break;
-			case HostDriver::Type::SWITCH:
+			case HostDriverType::SWITCH:
 				interface.driver = std::make_unique<SwitchWiredHost>(gp_idx);
 				break;
-			case HostDriver::Type::SWITCH_PRO:
+			case HostDriverType::SWITCH_PRO:
 				interface.driver = std::make_unique<SwitchProHost>(gp_idx);
 				break;
-			case HostDriver::Type::N64:
+			case HostDriverType::N64:
 				interface.driver = std::make_unique<N64Host>(gp_idx);
 				break;
-			case HostDriver::Type::PSCLASSIC:
+			case HostDriverType::PSCLASSIC:
 				interface.driver = std::make_unique<PSClassicHost>(gp_idx);
 				break;
-			case HostDriver::Type::XBOXOG:
+			case HostDriverType::XBOXOG:
 				interface.driver = std::make_unique<XboxOGHost>(gp_idx);
 				break;
-			case HostDriver::Type::XBOXONE:
+			case HostDriverType::XBOXONE:
 				interface.driver = std::make_unique<XboxOneHost>(gp_idx);
 				break;
-			case HostDriver::Type::XBOX360:
+			case HostDriverType::XBOX360:
 				interface.driver = std::make_unique<Xbox360Host>(gp_idx);
 				break;
-			case HostDriver::Type::XBOX360W: //Composite device, takes up all 4 gamepads when mounted
+			case HostDriverType::XBOX360W: //Composite device, takes up all 4 gamepads when mounted
 				interface.driver = std::make_unique<Xbox360WHost>(gp_idx);
 				break;
 			default:
@@ -130,7 +130,7 @@ public:
 		return true;
 	}
 
-	inline void process_report(DriverClass driver_class, uint8_t address, uint8_t instance, const uint8_t* report, uint16_t len)
+	inline void process_report(uint8_t address, uint8_t instance, const uint8_t* report, uint16_t len)
 	{
 		for (auto& device_slot : device_slots_)
 		{
@@ -143,7 +143,7 @@ public:
 		}
 	}
 
-	inline void connect_cb(DriverClass driver_class, uint8_t address, uint8_t instance)
+	inline void connect_cb(uint8_t address, uint8_t instance)
 	{
 		for (auto& device_slot : device_slots_)
 		{
@@ -156,7 +156,7 @@ public:
 		}
 	}
 
-	inline void disconnect_cb(DriverClass driver_class, uint8_t address, uint8_t instance)
+	inline void disconnect_cb(uint8_t address, uint8_t instance)
 	{
 		for (auto& device_slot : device_slots_)
 		{
@@ -180,7 +180,7 @@ public:
 			}
 			for (uint8_t i = 0; i < MAX_INTERFACES; ++i)
 			{
-				if (device_slot.interfaces[i].driver != nullptr && device_slot.interfaces[i].gamepad->new_pad_out())
+				if (device_slot.interfaces[i].driver && device_slot.interfaces[i].gamepad->new_pad_out())
 				{
 					device_slot.interfaces[i].driver->send_feedback(*device_slot.interfaces[i].gamepad, device_slot.address, i);
 					tuh_task();
@@ -200,7 +200,7 @@ public:
 		}
 	}
 
-	static inline HostDriver::Type get_type(const HardwareID& ids)
+	static inline HostDriverType get_type(const HardwareID& ids)
 	{
 		for (const auto& map : HOST_TYPE_MAP)
 		{
@@ -212,25 +212,25 @@ public:
 				}
 			}
 		}
-		return HostDriver::Type::UNKNOWN;
+		return HostDriverType::UNKNOWN;
 	}
 
-	static inline HostDriver::Type get_type(const tuh_xinput::DevType xinput_type)
+	static inline HostDriverType get_type(const tuh_xinput::DevType xinput_type)
 	{
 		switch (xinput_type)
 		{
 			case tuh_xinput::DevType::XBOXONE:
-				return HostDriver::Type::XBOXONE;
+				return HostDriverType::XBOXONE;
 			case tuh_xinput::DevType::XBOX360W:
-				return HostDriver::Type::XBOX360W;
+				return HostDriverType::XBOX360W;
 			case tuh_xinput::DevType::XBOX360:
-				return HostDriver::Type::XBOX360;
+				return HostDriverType::XBOX360;
 			// case tuh_xinput::DevType::XBOX360_CHATPAD:
-			// 	return HostDriver::Type::XBOX360_CHATPAD;
+			// 	return HostDriverType::XBOX360_CHATPAD;
 			case tuh_xinput::DevType::XBOXOG:
-				return HostDriver::Type::XBOXOG;
+				return HostDriverType::XBOXOG;
 			default:
-				return HostDriver::Type::UNKNOWN;
+				return HostDriverType::UNKNOWN;
 		}
 	}
 
diff --git a/Firmware/RP2040/src/USBHost/tuh_callbacks.cpp b/Firmware/RP2040/src/USBHost/tuh_callbacks.cpp
index 2c42ff1..f0af110 100644
--- a/Firmware/RP2040/src/USBHost/tuh_callbacks.cpp
+++ b/Firmware/RP2040/src/USBHost/tuh_callbacks.cpp
@@ -12,11 +12,11 @@
 
 #if defined(CONFIG_EN_4CH)
 #include "I2CDriver/4Channel/I2CManager.h"
-#endif //defined(CONFIG_EN_4CH)
+#endif 
 
 usbh_class_driver_t const* usbh_app_driver_get_cb(uint8_t* driver_count)
 {
-    *driver_count += 1;
+    *driver_count = 1;
     return tuh_xinput::class_driver();
 }
 
@@ -32,10 +32,10 @@ void tuh_hid_mount_cb(uint8_t dev_addr, uint8_t instance, uint8_t const* desc_re
     if (host_manager.setup_driver(HostManager::get_type({ vid, pid }), dev_addr, instance, desc_report, desc_len))
     {
 #if defined(CONFIG_EN_4CH)
-        I2CManager::get_instance().get_driver()->notify_tuh_mounted();
-#endif //defined(CONFIG_EN_4CH)
+        I2CManager::get_instance().get_driver()->notify_tuh(true);
+#endif
 
-        OGXMini::update_tud_status(true);
+        OGXMini::host_mounted(true);
     }
 }
 
@@ -47,16 +47,16 @@ void tuh_hid_umount_cb(uint8_t dev_addr, uint8_t instance)
     if (!host_manager.any_mounted())
     {
 #if defined(CONFIG_EN_4CH)
-        I2CManager::get_instance().get_driver()->notify_tuh_unmounted();
-#endif //defined(CONFIG_EN_4CH)
+        I2CManager::get_instance().get_driver()->notify_tuh(false);
+#endif
 
-        OGXMini::update_tud_status(false);
+        OGXMini::host_mounted(false);
     }
 }
 
 void tuh_hid_report_received_cb(uint8_t dev_addr, uint8_t instance, uint8_t const* report, uint16_t len)
 {
-    HostManager::get_instance().process_report(HostManager::DriverClass::HID, dev_addr, instance, report, len);
+    HostManager::get_instance().process_report(dev_addr, instance, report, len);
 }
 
 //XINPUT
@@ -64,15 +64,15 @@ void tuh_hid_report_received_cb(uint8_t dev_addr, uint8_t instance, uint8_t cons
 void tuh_xinput::mount_cb(uint8_t dev_addr, uint8_t instance, const tuh_xinput::Interface* interface)
 {
     HostManager& host_manager = HostManager::get_instance();
-    HostDriver::Type host_type = HostManager::get_type(interface->dev_type);
+    HostDriverType host_type = HostManager::get_type(interface->dev_type);
 
     if (host_manager.setup_driver(host_type, dev_addr, instance))
     {
 #if defined(CONFIG_EN_4CH)
-        I2CManager::get_instance().get_driver()->notify_tuh_mounted(host_type);
-#endif //defined(CONFIG_EN_4CH)
+        I2CManager::get_instance().get_driver()->notify_tuh(true, host_type);
+#endif
 
-        OGXMini::update_tud_status(true);
+        OGXMini::host_mounted(true);
     }
 }
 
@@ -84,34 +84,34 @@ void tuh_xinput::unmount_cb(uint8_t dev_addr, uint8_t instance, const tuh_xinput
     if (!host_manager.any_mounted())
     {
 #if defined(CONFIG_EN_4CH)
-        I2CManager::get_instance().get_driver()->notify_tuh_mounted(host_manager.get_type(interface->dev_type));
-#endif //defined(CONFIG_EN_4CH)
+        I2CManager::get_instance().get_driver()->notify_tuh(false, host_manager.get_type(interface->dev_type));
+#endif
 
-        OGXMini::update_tud_status(false);
+        OGXMini::host_mounted(false);
     }
 }
 
 void tuh_xinput::report_received_cb(uint8_t dev_addr, uint8_t instance, const uint8_t* report, uint16_t len)
 {
-    HostManager::get_instance().process_report(HostManager::DriverClass::XINPUT, dev_addr, instance, report, len);
+    HostManager::get_instance().process_report(dev_addr, instance, report, len);
 }
 
 void tuh_xinput::xbox360w_connect_cb(uint8_t dev_addr, uint8_t instance)
 {
 #if defined(CONFIG_EN_4CH)
     uint8_t idx = HostManager::get_instance().get_gamepad_idx(HostManager::DriverClass::XINPUT, dev_addr, instance);
-    I2CManager::get_instance().get_driver()->notify_xbox360w_connected(idx);
-#endif //defined(CONFIG_EN_4CH)
+    I2CManager::get_instance().get_driver()->notify_xbox360w(true, idx);
+#endif
 
-    HostManager::get_instance().connect_cb(HostManager::DriverClass::XINPUT, dev_addr, instance);
+    HostManager::get_instance().connect_cb(dev_addr, instance);
 }
 
 void tuh_xinput::xbox360w_disconnect_cb(uint8_t dev_addr, uint8_t instance)
 {
 #if defined(CONFIG_EN_4CH)
     uint8_t idx = HostManager::get_instance().get_gamepad_idx(HostManager::DriverClass::XINPUT, dev_addr, instance);
-    I2CManager::get_instance().get_driver()->notify_xbox360w_disconnected(idx);
-#endif //defined(CONFIG_EN_4CH)
+    I2CManager::get_instance().get_driver()->notify_xbox360w(false, idx);
+#endif
 
-    HostManager::get_instance().disconnect_cb(HostManager::DriverClass::XINPUT, dev_addr, instance);
+    HostManager::get_instance().disconnect_cb(dev_addr, instance);
 }
\ No newline at end of file
diff --git a/Firmware/RP2040/src/UserSettings/NVSTool.h b/Firmware/RP2040/src/UserSettings/NVSTool.h
new file mode 100644
index 0000000..740b8ce
--- /dev/null
+++ b/Firmware/RP2040/src/UserSettings/NVSTool.h
@@ -0,0 +1,175 @@
+#ifndef _NVS_TOOL_H_
+#define _NVS_TOOL_H_
+
+#include <cstdint>
+#include <string>
+#include <array>
+#include <cstring>
+#include <hardware/flash.h>
+#include <pico/mutex.h>
+
+/* Define NVS_SECTORS (number of sectors to allocate to storage) either here or with CMake */
+
+class NVSTool
+{
+public:
+    static constexpr size_t KEY_LEN_MAX = 16; //Including null terminator
+    static constexpr size_t VALUE_LEN_MAX = FLASH_PAGE_SIZE - KEY_LEN_MAX;
+    static constexpr uint32_t MAX_ENTRIES = ((NVS_SECTORS * FLASH_SECTOR_SIZE) / FLASH_PAGE_SIZE) - 1;
+
+    static NVSTool& get_instance()
+    {
+        static NVSTool instance;
+        return instance;
+    }
+
+    bool write(const std::string& key, const void* value, size_t len)
+    {
+        if (!valid_args(key, len))
+        {
+            return false;
+        }
+
+        mutex_enter_blocking(&nvs_mutex_);
+
+        for (uint32_t i = 1; i < MAX_ENTRIES; ++i) 
+        {
+            Entry* read_entry = get_entry(i);
+
+            if (!is_valid_entry(read_entry) || std::strcmp(read_entry->key, key.c_str()) == 0)
+            {
+                update_entry(i, key, value, len);
+
+                mutex_exit(&nvs_mutex_);
+                return true;
+            }
+        }
+
+        mutex_exit(&nvs_mutex_);
+        return false; // No space for new entry
+    }
+
+    bool read(const std::string& key, void* value, size_t len)
+    {
+        if (!valid_args(key, len))
+        {
+            return false;
+        }
+
+        mutex_enter_blocking(&nvs_mutex_);
+
+        for (uint32_t i = 1; i < MAX_ENTRIES; ++i) 
+        {
+            Entry* read_entry = get_entry(i);
+
+            if (std::strcmp(read_entry->key, key.c_str()) == 0)
+            {
+                std::memcpy(value, read_entry->value, len);
+
+                mutex_exit(&nvs_mutex_);
+                return true;
+            }
+            else if (!is_valid_entry(read_entry))
+            {
+                // Key not found
+                break;
+            }
+        }
+
+        mutex_exit(&nvs_mutex_);
+        return false;
+    }
+
+private:
+    NVSTool()
+    {
+        mutex_init(&nvs_mutex_);
+
+        Entry* initial_entry = get_entry(0);
+
+        if (std::strcmp(initial_entry->key, INVALID_KEY) != 0) 
+        {
+            mutex_enter_blocking(&nvs_mutex_);
+
+            for (uint32_t i = 0; i < NVS_SECTORS; ++i) 
+            {
+                flash_range_erase(NVS_START_OFFSET + i * FLASH_SECTOR_SIZE, FLASH_SECTOR_SIZE);
+            }
+
+            Entry entry;
+
+            for (uint32_t i = 0; i < MAX_ENTRIES; ++i) 
+            {
+                flash_range_program(NVS_START_OFFSET + i * sizeof(Entry), 
+                                    reinterpret_cast<const uint8_t*>(&entry), 
+                                    sizeof(Entry));
+            }
+
+            mutex_exit(&nvs_mutex_);
+        }
+    }
+
+    ~NVSTool() = default;
+    NVSTool(const NVSTool&) = delete;
+    NVSTool& operator=(const NVSTool&) = delete;
+
+    struct Entry 
+    {
+        char key[KEY_LEN_MAX];
+        uint8_t value[VALUE_LEN_MAX];
+
+        Entry()
+        {
+            std::fill(std::begin(key), std::end(key), '\0');
+            std::strncpy(key, INVALID_KEY, sizeof(key) - 1);
+            std::fill(std::begin(value), std::end(value), 0xFF);
+        }
+    };
+    static_assert(sizeof(Entry) == FLASH_PAGE_SIZE, "NVSTool::Entry size mismatch");
+
+    static constexpr const char INVALID_KEY[KEY_LEN_MAX] = "INVALID";
+    static constexpr uint32_t NVS_START_OFFSET = PICO_FLASH_SIZE_BYTES - FLASH_SECTOR_SIZE * NVS_SECTORS;
+
+    mutex_t nvs_mutex_;
+
+    inline Entry* get_entry(const uint32_t index) 
+    {
+        return reinterpret_cast<Entry*>(XIP_BASE + NVS_START_OFFSET + index * sizeof(Entry));
+    }
+
+    inline bool is_valid_entry(const Entry* entry) 
+    {
+        return std::strcmp(entry->key, INVALID_KEY) != 0;
+    }
+
+    inline bool valid_args(const std::string& key, size_t len)
+    {
+        return (key.size() < KEY_LEN_MAX - 1 && len <= sizeof(Entry::value) && std::strcmp(key.c_str(), INVALID_KEY) != 0);
+    }
+
+    void update_entry(uint32_t index, const std::string& key, const void* buffer, size_t len)
+    {
+        uint32_t entry_offset = index * sizeof(Entry);
+        uint32_t sector_offset = ((NVS_START_OFFSET + entry_offset) / FLASH_SECTOR_SIZE) * FLASH_SECTOR_SIZE;
+
+        std::array<uint8_t, FLASH_SECTOR_SIZE> sector_buffer;
+        std::memcpy(sector_buffer.data(), reinterpret_cast<const uint8_t*>(XIP_BASE + sector_offset), FLASH_SECTOR_SIZE);
+
+        flash_range_erase(sector_offset, FLASH_SECTOR_SIZE);
+
+        Entry* entry_to_write = reinterpret_cast<Entry*>(sector_buffer.data() + entry_offset);
+
+        *entry_to_write = Entry();
+        std::strncpy(entry_to_write->key, key.c_str(), key.size());
+        entry_to_write->key[key.size()] = '\0';
+        std::memcpy(entry_to_write->value, buffer, len);
+
+        for (uint32_t i = 0; i < FLASH_SECTOR_SIZE / FLASH_PAGE_SIZE; ++i) 
+        {
+            flash_range_program(sector_offset + i * FLASH_PAGE_SIZE, sector_buffer.data() + i * FLASH_PAGE_SIZE, FLASH_PAGE_SIZE);
+        }
+    }
+
+}; // class NVSTool
+
+#endif // _NVS_TOOL_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/UserSettings/UserSettings.cpp b/Firmware/RP2040/src/UserSettings/UserSettings.cpp
index e5caaaf..3820675 100644
--- a/Firmware/RP2040/src/UserSettings/UserSettings.cpp
+++ b/Firmware/RP2040/src/UserSettings/UserSettings.cpp
@@ -1,241 +1,117 @@
 #include <cstring>
 #include <array>
-#include <pico/stdlib.h>
 #include <pico/multicore.h>
-#include <hardware/flash.h>
-#include <hardware/regs/addressmap.h>
 
 #include "tusb.h"
 
+#include "Board/ogxm_log.h"
 #include "Board/board_api.h"
 #include "UserSettings/UserSettings.h"
 
-static constexpr uint32_t button_combo(const uint16_t& buttons, const uint8_t& dpad = 0)
+static constexpr uint32_t BUTTON_COMBO(const uint16_t& buttons, const uint8_t& dpad = 0)
 {
     return (static_cast<uint32_t>(buttons) << 16) | static_cast<uint32_t>(dpad);
 }
 
 namespace ButtonCombo 
 {
-    static constexpr uint32_t PS3       = button_combo(Gamepad::BUTTON_START, Gamepad::DPAD_LEFT);
-    static constexpr uint32_t DINPUT    = button_combo(Gamepad::BUTTON_START | Gamepad::BUTTON_RB, Gamepad::DPAD_LEFT);
-    static constexpr uint32_t XINPUT    = button_combo(Gamepad::BUTTON_START, Gamepad::DPAD_UP);
-    static constexpr uint32_t SWITCH    = button_combo(Gamepad::BUTTON_START, Gamepad::DPAD_DOWN);
-    static constexpr uint32_t XBOXOG    = button_combo(Gamepad::BUTTON_START, Gamepad::DPAD_RIGHT);
-    static constexpr uint32_t XBOXOG_SB = button_combo(Gamepad::BUTTON_START | Gamepad::BUTTON_RB, Gamepad::DPAD_RIGHT);
-    static constexpr uint32_t XBOXOG_XR = button_combo(Gamepad::BUTTON_START | Gamepad::BUTTON_LB, Gamepad::DPAD_RIGHT);
-    static constexpr uint32_t PSCLASSIC = button_combo(Gamepad::BUTTON_START | Gamepad::BUTTON_A);
-    static constexpr uint32_t WEBAPP    = button_combo(Gamepad::BUTTON_START | Gamepad::BUTTON_LB | Gamepad::BUTTON_RB);
+    static constexpr uint32_t PS3       = BUTTON_COMBO(Gamepad::BUTTON_START, Gamepad::DPAD_LEFT);
+    static constexpr uint32_t DINPUT    = BUTTON_COMBO(Gamepad::BUTTON_START | Gamepad::BUTTON_RB, Gamepad::DPAD_LEFT);
+    static constexpr uint32_t XINPUT    = BUTTON_COMBO(Gamepad::BUTTON_START, Gamepad::DPAD_UP);
+    static constexpr uint32_t SWITCH    = BUTTON_COMBO(Gamepad::BUTTON_START, Gamepad::DPAD_DOWN);
+    static constexpr uint32_t XBOXOG    = BUTTON_COMBO(Gamepad::BUTTON_START, Gamepad::DPAD_RIGHT);
+    static constexpr uint32_t XBOXOG_SB = BUTTON_COMBO(Gamepad::BUTTON_START | Gamepad::BUTTON_RB, Gamepad::DPAD_RIGHT);
+    static constexpr uint32_t XBOXOG_XR = BUTTON_COMBO(Gamepad::BUTTON_START | Gamepad::BUTTON_LB, Gamepad::DPAD_RIGHT);
+    static constexpr uint32_t PSCLASSIC = BUTTON_COMBO(Gamepad::BUTTON_START | Gamepad::BUTTON_A);
+    static constexpr uint32_t WEBAPP    = BUTTON_COMBO(Gamepad::BUTTON_START | Gamepad::BUTTON_LB | Gamepad::BUTTON_RB);
 };
 
-static constexpr DeviceDriver::Type VALID_DRIVER_TYPES[] =
+static constexpr DeviceDriverType VALID_DRIVER_TYPES[] =
 {
 #if defined(CONFIG_EN_4CH)
-    DeviceDriver::Type::XBOXOG, 
-    DeviceDriver::Type::XBOXOG_SB, 
-    DeviceDriver::Type::XINPUT,
-    DeviceDriver::Type::PS3,
-    DeviceDriver::Type::PSCLASSIC, 
-    DeviceDriver::Type::WEBAPP,
+    DeviceDriverType::XBOXOG, 
+    DeviceDriverType::XBOXOG_SB, 
+    DeviceDriverType::XINPUT,
+    DeviceDriverType::PS3,
+    DeviceDriverType::PSCLASSIC, 
+    DeviceDriverType::WEBAPP,
     #if defined(XREMOTE_ROM_AVAILABLE)
-        DeviceDriver::Type::XBOXOG_XR,
+    DeviceDriverType::XBOXOG_XR,
     #endif
 
 #elif MAX_GAMEPADS > 1
-    DeviceDriver::Type::DINPUT, 
-    DeviceDriver::Type::SWITCH, 
-    DeviceDriver::Type::WEBAPP,
+    DeviceDriverType::DINPUT, 
+    DeviceDriverType::SWITCH, 
+    DeviceDriverType::WEBAPP,
 
 #else // MAX_GAMEPADS == 1
-    DeviceDriver::Type::XBOXOG, 
-    DeviceDriver::Type::XBOXOG_SB, 
-    DeviceDriver::Type::DINPUT, 
-    DeviceDriver::Type::SWITCH, 
-    DeviceDriver::Type::WEBAPP,
-    DeviceDriver::Type::PS3,
-    DeviceDriver::Type::PSCLASSIC, 
-    DeviceDriver::Type::XINPUT,
+    DeviceDriverType::XBOXOG, 
+    DeviceDriverType::XBOXOG_SB, 
+    DeviceDriverType::DINPUT, 
+    DeviceDriverType::SWITCH, 
+    DeviceDriverType::WEBAPP,
+    DeviceDriverType::PS3,
+    DeviceDriverType::PSCLASSIC, 
+    DeviceDriverType::XINPUT,
     #if defined(XREMOTE_ROM_AVAILABLE)
-        DeviceDriver::Type::XBOXOG_XR,
+    DeviceDriverType::XBOXOG_XR,
     #endif
 
 #endif
 };
 
-struct ComboMap { uint32_t combo; DeviceDriver::Type driver; };
+struct ComboMap { uint32_t combo; DeviceDriverType driver; };
 static constexpr std::array<ComboMap, 9> BUTTON_COMBO_MAP = 
 {{
-    { ButtonCombo::XBOXOG,    DeviceDriver::Type::XBOXOG    },
-    { ButtonCombo::XBOXOG_SB, DeviceDriver::Type::XBOXOG_SB },
-    { ButtonCombo::XBOXOG_XR, DeviceDriver::Type::XBOXOG_XR },
-    { ButtonCombo::WEBAPP,    DeviceDriver::Type::WEBAPP    },
-    { ButtonCombo::DINPUT,    DeviceDriver::Type::DINPUT    },
-    { ButtonCombo::SWITCH,    DeviceDriver::Type::SWITCH    },
-    { ButtonCombo::XINPUT,    DeviceDriver::Type::XINPUT    },
-    { ButtonCombo::PS3,       DeviceDriver::Type::PS3       },
-    { ButtonCombo::PSCLASSIC, DeviceDriver::Type::PSCLASSIC }
+    { ButtonCombo::XBOXOG,    DeviceDriverType::XBOXOG    },
+    { ButtonCombo::XBOXOG_SB, DeviceDriverType::XBOXOG_SB },
+    { ButtonCombo::XBOXOG_XR, DeviceDriverType::XBOXOG_XR },
+    { ButtonCombo::WEBAPP,    DeviceDriverType::WEBAPP    },
+    { ButtonCombo::DINPUT,    DeviceDriverType::DINPUT    },
+    { ButtonCombo::SWITCH,    DeviceDriverType::SWITCH    },
+    { ButtonCombo::XINPUT,    DeviceDriverType::XINPUT    },
+    { ButtonCombo::PS3,       DeviceDriverType::PS3       },
+    { ButtonCombo::PSCLASSIC, DeviceDriverType::PSCLASSIC }
 }};
 
-mutex_t UserSettings::flash_mutex_;
-DeviceDriver::Type UserSettings::current_driver_ = DeviceDriver::Type::NONE;
-
-//Checks for first boot and initializes user profiles, call before tusb is inited.
-void UserSettings::initialize_flash()
+const std::string UserSettings::INIT_FLAG_KEY()
 {
-    if (!mutex_is_initialized(&flash_mutex_))
-    {
-        mutex_init(&flash_mutex_);
-    }
-
-    mutex_enter_blocking(&flash_mutex_);
-
-    const uint8_t* read_init_flag = reinterpret_cast<const uint8_t*>(XIP_BASE + flash_offset(INIT_FLAG_SECTOR));
-
-    if (*read_init_flag == INIT_FLAG)
-    {
-        mutex_exit(&flash_mutex_);
-        return;
-    }
-
-    uint32_t saved_interrupts = save_and_disable_interrupts();
-
-    // {
-    //     std::array<uint8_t, FLASH_PAGE_SIZE> device_mode_buffer;
-    //     device_mode_buffer.fill(0xFF);
-
-    //     // device_mode_buffer[0] = static_cast<uint8_t>(DeviceDriver::Type::XBOXOG);
-
-    //     flash_range_erase(flash_offset(DEVICE_MODE_SECTOR), FLASH_SECTOR_SIZE);
-    //     flash_range_program(flash_offset(DEVICE_MODE_SECTOR), device_mode_buffer.data(), FLASH_PAGE_SIZE);
-    // }
-    
-    {
-        std::array<uint8_t, FLASH_PAGE_SIZE> profile_ids;
-        profile_ids.fill(0xFF);
-        std::memset(profile_ids.data(), 0x01, 4);
-
-        flash_range_erase(flash_offset(ACTIVE_PROFILES_SECTOR), FLASH_SECTOR_SIZE);
-        flash_range_program(flash_offset(ACTIVE_PROFILES_SECTOR), profile_ids.data(), FLASH_PAGE_SIZE);
-    }
-
-    {    
-        std::array<uint8_t, FLASH_PAGE_SIZE> profile_buffer;
-        profile_buffer.fill(0);
-
-        flash_range_erase(flash_offset(PROFILES_START_SECTOR), FLASH_SECTOR_SIZE);
-
-        for (uint8_t i = 0; i < MAX_PROFILES; i++)
-        {
-            UserProfile profile = UserProfile();
-            profile.id = i + 1;
-            std::memcpy(profile_buffer.data(), reinterpret_cast<const uint8_t*>(&profile), sizeof(UserProfile));
-            flash_range_program(flash_offset(PROFILES_START_SECTOR) + profile_offset(profile.id), profile_buffer.data(), FLASH_PAGE_SIZE);
-        }
-    }
-
-    {
-        std::array<uint8_t, FLASH_PAGE_SIZE> init_flag_buffer;
-        init_flag_buffer.fill(INIT_FLAG);
-
-        flash_range_erase(flash_offset(INIT_FLAG_SECTOR), FLASH_SECTOR_SIZE);
-        flash_range_program(flash_offset(INIT_FLAG_SECTOR), init_flag_buffer.data(), FLASH_PAGE_SIZE);
-    }
-
-    restore_interrupts(saved_interrupts);
-    mutex_exit(&flash_mutex_);
+    return std::string("init_flag");
 }
 
-bool UserSettings::verify_firmware_version()
+const std::string UserSettings::PROFILE_KEY(const uint8_t profile_id)
 {
-    mutex_enter_blocking(&flash_mutex_);
-
-    const char* flash_firmware_version = reinterpret_cast<const char*>(XIP_BASE + flash_offset(FIRMWARE_VER_FLAG_SECTOR));
-    bool match = std::memcmp(flash_firmware_version, FIRMWARE_VERSION, sizeof(FIRMWARE_VERSION)) == 0;
-
-    mutex_exit(&flash_mutex_);
-    return match;
+    return std::string("profile_") + std::to_string(profile_id);
 }
 
-bool UserSettings::write_firmware_version_safe()
+const std::string UserSettings::ACTIVE_PROFILE_KEY(const uint8_t index)
 {
-    mutex_enter_blocking(&flash_mutex_);
-    uint32_t saved_interrupts = save_and_disable_interrupts();
-
-    flash_range_erase(flash_offset(FIRMWARE_VER_FLAG_SECTOR), FLASH_SECTOR_SIZE);
-    flash_range_program(flash_offset(FIRMWARE_VER_FLAG_SECTOR), reinterpret_cast<const uint8_t*>(FIRMWARE_VERSION), sizeof(FIRMWARE_VERSION));
-
-    restore_interrupts(saved_interrupts);
-    mutex_exit(&flash_mutex_);
-
-    return verify_firmware_version();
+    return std::string("active_id_") + std::to_string(index);
 }
 
-DeviceDriver::Type UserSettings::get_current_driver()
+const std::string UserSettings::DRIVER_TYPE_KEY()
 {
-    if (current_driver_ != DeviceDriver::Type::NONE)
-    {
-        return current_driver_;
-    }
-
-    mutex_enter_blocking(&flash_mutex_);
-
-    int stored_value = *reinterpret_cast<const int*>(XIP_BASE + flash_offset(DRIVER_TYPE_SECTOR));
-
-    mutex_exit(&flash_mutex_);
-
-    for (const auto& driver : VALID_DRIVER_TYPES)
-    {
-        if (stored_value == static_cast<int>(driver))
-        {
-            current_driver_ = driver;
-            return current_driver_;
-        }
-    }
-
-    current_driver_ = get_default_driver();
-
-    return current_driver_;
+    return std::string("driver_type");
 }
 
-void UserSettings::store_driver_type_unsafe(const DeviceDriver::Type new_mode)
+const std::string UserSettings::FIRMWARE_VER_KEY()
 {
-    std::array<int, FLASH_PAGE_SIZE / sizeof(int)> mode_buffer;
-    mode_buffer.fill(static_cast<int>(new_mode));
-
-    mutex_enter_blocking(&flash_mutex_);
-    uint32_t saved_interrupts = save_and_disable_interrupts();
-    
-    flash_range_erase(flash_offset(DRIVER_TYPE_SECTOR), FLASH_SECTOR_SIZE);
-    flash_range_program(flash_offset(DRIVER_TYPE_SECTOR), reinterpret_cast<uint8_t*>(mode_buffer.data()), FLASH_PAGE_SIZE);
-
-    restore_interrupts(saved_interrupts); 
-    mutex_exit(&flash_mutex_);   
+    return std::string("firmware_ver");
 }
 
-//Disconnects usb and resets pico, thread safe
-void UserSettings::store_driver_type_safe(DeviceDriver::Type new_mode) 
+DeviceDriverType UserSettings::DEFAULT_DRIVER()
 {
-    if (tud_connected())
-    {
-        tud_disconnect();
-        sleep_ms(300);
-    }
-
-    multicore_reset_core1();
-
-    store_driver_type_unsafe(new_mode);
-
-    board_api::reboot();
+    return VALID_DRIVER_TYPES[0];
 }
 
 //Checks if button combo has been held for 3 seconds, returns true if mode has been changed
 bool UserSettings::check_for_driver_change(Gamepad& gamepad)
 {
     Gamepad::PadIn gp_in = gamepad.get_pad_in();
-    static uint32_t last_button_combo = button_combo(gp_in.buttons, gp_in.dpad);
+    static uint32_t last_button_combo = BUTTON_COMBO(gp_in.buttons, gp_in.dpad);
     static uint8_t call_count = 0;
 
-    uint32_t current_button_combo = button_combo(gp_in.buttons, gp_in.dpad);
+    uint32_t current_button_combo = BUTTON_COMBO(gp_in.buttons, gp_in.dpad);
 
     if (!(current_button_combo & (static_cast<uint32_t>(Gamepad::BUTTON_START) << 16)) || 
         last_button_combo != current_button_combo)
@@ -254,24 +130,18 @@ bool UserSettings::check_for_driver_change(Gamepad& gamepad)
 
     call_count = 0;
 
-    DeviceDriver::Type new_driver = DeviceDriver::Type::NONE;
+    DeviceDriverType new_driver = DeviceDriverType::NONE;
 
-    for (const auto& combo : BUTTON_COMBO_MAP)
+    for (const auto& combo_map : BUTTON_COMBO_MAP)
     {
-        if (combo.combo == current_button_combo)
+        if (combo_map.combo == current_button_combo && is_valid_driver(combo_map.driver))
         {
-            for (const auto& driver : VALID_DRIVER_TYPES)
-            {
-                if (combo.driver == driver)
-                {
-                    new_driver = combo.driver;
-                    break;
-                }
-            }
+            new_driver = combo_map.driver;
+            break;
         }
     }
 
-    if (new_driver == DeviceDriver::Type::NONE || new_driver == current_driver_)
+    if (new_driver == DeviceDriverType::NONE || new_driver == current_driver_)
     {
         return false;
     }
@@ -281,50 +151,8 @@ bool UserSettings::check_for_driver_change(Gamepad& gamepad)
     return true;
 }
 
-void UserSettings::store_profile_unsafe(const UserProfile& profile)
-{
-    mutex_enter_blocking(&flash_mutex_);
-
-    uint32_t saved_interrupts = save_and_disable_interrupts();
-    
-    std::array<uint8_t, FLASH_PAGE_SIZE * MAX_PROFILES> profiles_buffer;
-
-    std::memcpy(profiles_buffer.data(), reinterpret_cast<const uint8_t*>(XIP_BASE + flash_offset(PROFILES_START_SECTOR)), FLASH_PAGE_SIZE * MAX_PROFILES);
-    std::memcpy(profiles_buffer.data() + profile_offset(profile.id), reinterpret_cast<const uint8_t*>(&profile), sizeof(UserProfile));
-
-    flash_range_erase(flash_offset(PROFILES_START_SECTOR), FLASH_SECTOR_SIZE);
-
-    for (uint8_t i = 0; i < MAX_PROFILES; i++)
-    {
-        flash_range_program(flash_offset(PROFILES_START_SECTOR) + (i * FLASH_PAGE_SIZE), profiles_buffer.data() + (i * FLASH_PAGE_SIZE), FLASH_PAGE_SIZE);
-    }
-
-    restore_interrupts(saved_interrupts);
-    mutex_exit(&flash_mutex_);
-}
-
-void UserSettings::store_active_profile_id_unsafe(const uint8_t index, const uint8_t profile_id)
-{
-    mutex_enter_blocking(&flash_mutex_);
-
-    std::array<uint8_t, FLASH_PAGE_SIZE> read_profile_ids;
-    read_profile_ids.fill(0xFF);
-
-    std::memcpy(read_profile_ids.data(), reinterpret_cast<const uint8_t*>(XIP_BASE + flash_offset(ACTIVE_PROFILES_SECTOR)), 4);
-
-    read_profile_ids[index] = profile_id;
-
-    uint32_t saved_interrupts = save_and_disable_interrupts();
-    
-    flash_range_erase(flash_offset(ACTIVE_PROFILES_SECTOR), FLASH_SECTOR_SIZE);
-    flash_range_program(flash_offset(ACTIVE_PROFILES_SECTOR), read_profile_ids.data(), FLASH_PAGE_SIZE);
-
-    restore_interrupts(saved_interrupts);
-    mutex_exit(&flash_mutex_);
-}
-
-//Disconnects usb and resets pico, thread safe
-bool UserSettings::store_profile_safe(uint8_t index, const UserProfile& profile)
+//Disconnects usb and resets pico, call from core0
+bool UserSettings::store_profile(uint8_t index, const UserProfile& profile)
 {
     if (profile.id < 1 || profile.id > MAX_PROFILES)
     {
@@ -335,23 +163,18 @@ bool UserSettings::store_profile_safe(uint8_t index, const UserProfile& profile)
         index = 0;
     }
 
-    if (tud_connected())
-    {
-        tud_disconnect();
-        sleep_ms(300);
-    }
+    board_api::usb::disconnect_all();
 
-    multicore_reset_core1();
-
-    store_active_profile_id_unsafe(index, profile.id);
-    store_profile_unsafe(profile);
+    nvs_tool_.write(ACTIVE_PROFILE_KEY(index), &profile.id, sizeof(uint8_t));
+    nvs_tool_.write(PROFILE_KEY(profile.id), &profile, sizeof(UserProfile));
 
     board_api::reboot();
+
     return true;
 }
 
-//Disconnects usb and resets pico, thread safe
-bool UserSettings::store_profile_and_driver_type_safe(DeviceDriver::Type new_driver_type, uint8_t index, const UserProfile& profile)
+//Disconnects usb and resets pico, call from core0
+bool UserSettings::store_profile_and_driver_type(DeviceDriverType new_driver_type, uint8_t index, const UserProfile& profile)
 {
     if (profile.id < 1 || profile.id > MAX_PROFILES)
     {
@@ -362,45 +185,58 @@ bool UserSettings::store_profile_and_driver_type_safe(DeviceDriver::Type new_dri
         index = 0;
     }
 
-    bool found = false;
+    bool valid_driver = false;
     for (const auto& driver : VALID_DRIVER_TYPES)
     {
         if (new_driver_type == driver)
         {
-            found = true;
+            valid_driver = true;
             break;
         }
     }
-    if (!found)
+    if (!valid_driver)
     {
-        new_driver_type = get_default_driver();
+        new_driver_type = DEFAULT_DRIVER();
     }
 
-    if (tud_connected())
-    {
-        tud_disconnect();
-        sleep_ms(300);
-    }
+    board_api::usb::disconnect_all();
 
-    multicore_reset_core1();
-
-    store_driver_type_unsafe(new_driver_type);
-    store_active_profile_id_unsafe(index, profile.id);
-    store_profile_unsafe(profile);
+    nvs_tool_.write(ACTIVE_PROFILE_KEY(index), &profile.id, sizeof(uint8_t));
+    nvs_tool_.write(PROFILE_KEY(profile.id), &profile, sizeof(UserProfile));
+    nvs_tool_.write(DRIVER_TYPE_KEY(), &new_driver_type, sizeof(uint8_t));
 
     board_api::reboot();
     
     return true;
 }
 
+//Disconnects usb and resets pico if it's a new & valid mode, call from core0
+void UserSettings::store_driver_type(DeviceDriverType new_driver) 
+{
+    if (!is_valid_driver(new_driver))
+    {
+        OGXM_LOG("Invalid driver type detected during store: " + OGXM_TO_STRING(new_driver) + "\n");
+        return;
+    }
+
+    OGXM_LOG("Storing new driver type: " + OGXM_TO_STRING(new_driver) + "\n");
+
+    board_api::usb::disconnect_all();
+
+    nvs_tool_.write(DRIVER_TYPE_KEY(), &new_driver, sizeof(uint8_t));
+
+    board_api::reboot();
+}
+
 uint8_t UserSettings::get_active_profile_id(const uint8_t index)
 {
-    mutex_enter_blocking(&flash_mutex_);
+    if (index > MAX_GAMEPADS - 1)
+    {
+        return 0x01;
+    }
 
-    const uint8_t* base_address = (const uint8_t*)(XIP_BASE + flash_offset(ACTIVE_PROFILES_SECTOR));
-    uint8_t read_profile_id = base_address[index];
-
-    mutex_exit(&flash_mutex_);
+    uint8_t read_profile_id = 0;
+    nvs_tool_.read(ACTIVE_PROFILE_KEY(index), &read_profile_id, sizeof(uint8_t));
 
     if (read_profile_id < 1 || read_profile_id > MAX_PROFILES)
     {
@@ -416,14 +252,8 @@ UserProfile UserSettings::get_profile_by_index(const uint8_t index)
 
 UserProfile UserSettings::get_profile_by_id(const uint8_t profile_id)
 {
-    mutex_enter_blocking(&flash_mutex_);
-
-    const uint8_t* base_address = reinterpret_cast<const uint8_t*>(XIP_BASE + flash_offset(PROFILES_START_SECTOR) + profile_offset(profile_id));
-
     UserProfile profile;
-    std::memcpy(&profile, base_address, sizeof(UserProfile));
-
-    mutex_exit(&flash_mutex_);
+    nvs_tool_.read(PROFILE_KEY(profile_id), &profile, sizeof(UserProfile));
 
     if (profile.id != profile_id)
     {
@@ -432,19 +262,110 @@ UserProfile UserSettings::get_profile_by_id(const uint8_t profile_id)
     return profile;
 }
 
-DeviceDriver::Type UserSettings::get_default_driver()
+bool UserSettings::is_valid_driver(DeviceDriverType driver)
 {
-    return VALID_DRIVER_TYPES[0];
-}
-
-bool UserSettings::valid_mode(DeviceDriver::Type mode)
-{
-    for (const auto& driver : VALID_DRIVER_TYPES)
+    for (const auto& valid_driver : VALID_DRIVER_TYPES)
     {
-        if (mode == driver)
+        if (driver == valid_driver)
         {
             return true;
         }
     }
     return false;
+}
+
+DeviceDriverType UserSettings::get_current_driver()
+{
+    if (current_driver_ != DeviceDriverType::NONE)
+    {
+        return current_driver_;
+    }
+
+    uint8_t stored_value = 0;
+    nvs_tool_.read(DRIVER_TYPE_KEY(), &stored_value, sizeof(uint8_t));
+
+    if (is_valid_driver(static_cast<DeviceDriverType>(stored_value)))
+    {
+        OGXM_LOG("Driver type read from flash: " + OGXM_TO_STRING(static_cast<DeviceDriverType>(stored_value)) + "\n");
+
+        current_driver_ = static_cast<DeviceDriverType>(stored_value);
+        return current_driver_;
+    }
+
+    OGXM_LOG("Invalid driver type read from flash, setting default driver\n");
+
+    current_driver_ = DEFAULT_DRIVER();
+    return current_driver_;
+}
+
+bool UserSettings::verify_firmware_version()
+{
+    std::string fw_version = FIRMWARE_VERSION;
+    char read_fw_version[fw_version.size()];
+    std::fill(read_fw_version, read_fw_version + fw_version.size(), '\0');
+
+    nvs_tool_.read(FIRMWARE_VER_KEY(), read_fw_version, fw_version.size());
+
+    return (std::memcmp(read_fw_version, fw_version.c_str(), fw_version.size()) == 0);
+}
+
+bool UserSettings::write_firmware_version()
+{
+    std::string fw_version = FIRMWARE_VERSION;
+    nvs_tool_.write(FIRMWARE_VER_KEY(), fw_version.c_str(), fw_version.size());
+    return verify_firmware_version();
+}
+
+//Checks for first boot and initializes user profiles, call before tusb is inited.
+void UserSettings::initialize_flash()
+{
+    OGXM_LOG("Initializing flash\n");
+
+    uint8_t read_init_flag = 0;
+    nvs_tool_.read(INIT_FLAG_KEY(), &read_init_flag, sizeof(uint8_t));
+
+    if (read_init_flag == FLASH_INIT_FLAG)
+    {
+        OGXM_LOG("Flash already initialized\n");
+
+        if (!verify_firmware_version())
+        {
+            OGXM_LOG("Firmware version mismatch, writing new version\n");
+            write_firmware_version();
+        }
+        return;
+    }
+
+    OGXM_LOG("Writing default driver\n");
+
+    uint8_t device_mode_buffer = static_cast<uint8_t>(DEFAULT_DRIVER());
+    nvs_tool_.write(DRIVER_TYPE_KEY(), &device_mode_buffer, sizeof(uint8_t));
+
+    OGXM_LOG("Writing default profile ids\n");
+
+    for (uint8_t i = 0; i < MAX_GAMEPADS; i++)
+    {
+        uint8_t profile_id = i + 1;
+        nvs_tool_.write(ACTIVE_PROFILE_KEY(i), &profile_id, sizeof(uint8_t));
+    }
+
+    OGXM_LOG("Writing default profiles\n");
+
+    {
+        UserProfile profile = UserProfile();
+        for (uint8_t i = 0; i < MAX_PROFILES; i++)
+        {
+            profile.id = i + 1;
+            nvs_tool_.write(PROFILE_KEY(profile.id), &profile, sizeof(UserProfile));
+        }
+    }
+
+    OGXM_LOG("Writing init flag\n");    
+
+    uint8_t init_flag_buffer = FLASH_INIT_FLAG;
+    nvs_tool_.write(INIT_FLAG_KEY(), &init_flag_buffer, sizeof(uint8_t));
+
+    write_firmware_version();
+
+    OGXM_LOG("Flash initialized\n");
 }
\ No newline at end of file
diff --git a/Firmware/RP2040/src/UserSettings/UserSettings.h b/Firmware/RP2040/src/UserSettings/UserSettings.h
index f54794a..8d6e83a 100644
--- a/Firmware/RP2040/src/UserSettings/UserSettings.h
+++ b/Firmware/RP2040/src/UserSettings/UserSettings.h
@@ -2,73 +2,63 @@
 #define _USER_SETTINGS_H_
 
 #include <cstdint>
-#include <pico/stdlib.h>
-#include <pico/mutex.h>
-#include <hardware/flash.h>
-#include <hardware/sync.h>
+#include <string>
 
 #include "board_config.h"
-#include "USBDevice/DeviceDriver/DeviceDriver.h"
+#include "USBDevice/DeviceDriver/DeviceDriverTypes.h"
 #include "UserSettings/UserProfile.h"
+#include "UserSettings/NVSTool.h"
 #include "Gamepad.h"
 
+/* Only write/store flash from Core0 */
 class UserSettings
 {
 public:
     static constexpr uint8_t MAX_PROFILES = 8;
     static constexpr int32_t GP_CHECK_DELAY_MS = 600;
 
-    UserSettings() = default;
-    ~UserSettings() = default;
+    static UserSettings& get_instance()
+    {
+        static UserSettings instance;
+        return instance;
+    }
 
     void initialize_flash();
 
     bool verify_firmware_version();
-    bool write_firmware_version_safe();
+    bool write_firmware_version();
 
-    bool valid_mode(DeviceDriver::Type mode);
+    bool is_valid_driver(DeviceDriverType driver);
 
-    DeviceDriver::Type get_current_driver();
+    DeviceDriverType get_current_driver();
     bool check_for_driver_change(Gamepad& gamepad);
     
     UserProfile get_profile_by_index(const uint8_t index);
     UserProfile get_profile_by_id(const uint8_t profile_id);
     uint8_t get_active_profile_id(const uint8_t index);
 
-    void store_driver_type_safe(DeviceDriver::Type new_driver_type);
-    bool store_profile_safe(uint8_t index, const UserProfile& profile);
-    bool store_profile_and_driver_type_safe(DeviceDriver::Type new_driver_type, uint8_t index, const UserProfile& profile);
+    void store_driver_type(DeviceDriverType new_driver_type);
+    bool store_profile(uint8_t index, const UserProfile& profile);
+    bool store_profile_and_driver_type(DeviceDriverType new_driver_type, uint8_t index, const UserProfile& profile);
 
 private:
+    UserSettings() = default;
+    ~UserSettings() = default;
+    UserSettings(const UserSettings&) = delete;
+    UserSettings& operator=(const UserSettings&) = delete;
+
     static constexpr uint8_t GP_CHECK_COUNT = 3000 / GP_CHECK_DELAY_MS;
-
-    //Sectors are counted backwards from the end of the flash, indexed from 1
-    static constexpr uint32_t FLASH_SIZE_BYTES = PICO_FLASH_SIZE_BYTES;
-    static constexpr uint8_t INIT_FLAG_SECTOR = 1;
-    static constexpr uint8_t FIRMWARE_VER_FLAG_SECTOR = 2;
-    static constexpr uint8_t DRIVER_TYPE_SECTOR = 3;
-    static constexpr uint8_t ACTIVE_PROFILES_SECTOR = 4;
-    static constexpr uint8_t PROFILES_START_SECTOR = 6;
-    static constexpr uint8_t INIT_FLAG = 0x88;
-
-    static mutex_t flash_mutex_; 
-    static DeviceDriver::Type current_driver_;
+    static constexpr uint8_t FLASH_INIT_FLAG = 0x21;
     
-    void store_active_profile_id_unsafe(const uint8_t index, const uint8_t profile_id);
-    void store_profile_unsafe(const UserProfile& profile);
-    void store_driver_type_unsafe(const DeviceDriver::Type new_mode);
-    DeviceDriver::Type get_default_driver();
-
-    static inline uint32_t flash_offset(const uint8_t sector)
-    {
-        return FLASH_SIZE_BYTES - static_cast<uint32_t>(FLASH_SECTOR_SIZE * sector);
-    }
-
-    //Offset relative to sector start, 1 profile per page
-    static inline uint32_t profile_offset(const uint8_t profile_id)
-    {
-        return (static_cast<uint32_t>(profile_id - 1) * FLASH_PAGE_SIZE);
-    }
+    NVSTool& nvs_tool_{NVSTool::get_instance()};
+    DeviceDriverType current_driver_{DeviceDriverType::NONE};
+    
+    DeviceDriverType DEFAULT_DRIVER();
+    const std::string INIT_FLAG_KEY();
+    const std::string PROFILE_KEY(const uint8_t profile_id);
+    const std::string ACTIVE_PROFILE_KEY(const uint8_t index);
+    const std::string DRIVER_TYPE_KEY();
+    const std::string FIRMWARE_VER_KEY();
 };
 
 #endif // _USER_SETTINGS_H_
\ No newline at end of file
diff --git a/Firmware/RP2040/src/board_config.h b/Firmware/RP2040/src/board_config.h
index 341336e..bb48951 100644
--- a/Firmware/RP2040/src/board_config.h
+++ b/Firmware/RP2040/src/board_config.h
@@ -9,15 +9,19 @@
 #define PI_PICO 1
 #define PI_PICO2 2
 #define PI_PICOW 3
-#define PI_PICOW2 4
+#define PI_PICO2W 4
 #define RP_ZERO 5
 #define ADA_FEATHER 6
 #define INTERNAL_4CH 7
 #define EXTERNAL_4CH 8
-#define W_ESP32 9
+#define PICO_ESP32 9
 
 #define SYSCLOCK_KHZ 240000
 
+#ifdef PICO_BOARD
+    #define O_BOARD PICO_BOARD
+#endif
+
 #ifndef MAX_GAMEPADS
     #define MAX_GAMEPADS 1
 #endif
@@ -30,7 +34,10 @@
     #define PIO_USB_DP_PIN      0 // DM = 1
     #define LED_INDICATOR_PIN   25
 
-#elif OGXM_BOARD == PI_PICOW || OGXM_BOARD == PI_PICOW2
+#elif OGXM_BOARD == PI_PICOW
+
+#elif OGXM_BOARD == PI_PICO2W
+    // #define LED_INDICATOR_PIN 0
 
 #elif OGXM_BOARD == RP_ZERO
     #define RGB_PXL_PIN 16 
@@ -62,7 +69,7 @@
     #define SLAVE_ADDR_PIN_1  13
     #define SLAVE_ADDR_PIN_2  14
 
-#elif OGXM_BOARD == W_ESP32
+#elif OGXM_BOARD == PICO_ESP32
     #define I2C_SDA_PIN     18 // SCL = 19
     #define UART0_TX_PIN    16 // RX = 17
     #define UART0_RX_PIN    (UART0_TX_PIN + 1)
@@ -78,7 +85,7 @@
 #endif // OGXM_BOARD
 
 #if defined(I2C_SDA_PIN)
-    #define I2C_BAUDRATE 400 * 1000
+    #define I2C_BAUDRATE 1000 * 1000
     #define I2C_SCL_PIN (I2C_SDA_PIN + 1)
     #define I2C_PORT    ((I2C_SDA_PIN == 2 ) || \
                          (I2C_SDA_PIN == 6 ) || \
@@ -113,6 +120,7 @@
 #endif // PIO_USB_DP_PIN
 
 #if defined(OGXM_DEBUG)
+    //Pins and port are defined in CMakeLists.txt
     #define DEBUG_UART_PORT __CONCAT(uart,PICO_DEFAULT_UART)
 #endif // defined(OGXM_DEBUG)
 
diff --git a/Firmware/RP2040/src/btstack_config.h b/Firmware/RP2040/src/btstack_config.h
index ff9c9a0..c4f404e 100644
--- a/Firmware/RP2040/src/btstack_config.h
+++ b/Firmware/RP2040/src/btstack_config.h
@@ -5,8 +5,12 @@
 #define ENABLE_LE_PERIPHERAL
 #define ENABLE_LE_CENTRAL
 #define ENABLE_L2CAP_LE_CREDIT_BASED_FLOW_CONTROL_MODE
+
+#ifndef OGXM_DEBUG
 #define ENABLE_LOG_INFO
 #define ENABLE_LOG_ERROR
+#endif
+
 #define ENABLE_PRINTF_HEXDUMP
 #define ENABLE_SCO_OVER_HCI
 
@@ -36,23 +40,27 @@
 #define MAX_NR_LE_DEVICE_DB_ENTRIES 16
 
 // Limit number of ACL/SCO Buffer to use by stack to avoid cyw43 shared bus overrun
-#define MAX_NR_CONTROLLER_ACL_BUFFERS 3
-#define MAX_NR_CONTROLLER_SCO_PACKETS 3
+#define MAX_NR_CONTROLLER_ACL_BUFFERS 6
+#define MAX_NR_CONTROLLER_SCO_PACKETS 6
 
 // Enable and configure HCI Controller to Host Flow Control to avoid cyw43 shared bus overrun
 #define ENABLE_HCI_CONTROLLER_TO_HOST_FLOW_CONTROL
 #define HCI_HOST_ACL_PACKET_LEN 1024
-#define HCI_HOST_ACL_PACKET_NUM 3
+#define HCI_HOST_ACL_PACKET_NUM 6
 #define HCI_HOST_SCO_PACKET_LEN 120
-#define HCI_HOST_SCO_PACKET_NUM 3
+#define HCI_HOST_SCO_PACKET_NUM 6
+
+#ifndef HCI_INCOMING_PRE_BUFFER_SIZE
+#define HCI_INCOMING_PRE_BUFFER_SIZE 6
+#endif
 
 // Link Key DB and LE Device DB using TLV on top of Flash Sector interface
 #define NVM_NUM_DEVICE_DB_ENTRIES 16
 #define NVM_NUM_LINK_KEYS 16
 
 // We don't give btstack a malloc, so use a fixed-size ATT DB.
-// #define MAX_ATT_DB_SIZE 512
-#define HAVE_MALLOC
+#define MAX_ATT_DB_SIZE 512
+// #define HAVE_MALLOC
 
 // BTstack HAL configuration
 #define HAVE_EMBEDDED_TIME_MS
@@ -75,4 +83,4 @@
 #define ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE
 #endif
 
-#endif  // MICROPY_INCLUDED_EXTMOD_BTSTACK_BTSTACK_CONFIG_H
\ No newline at end of file
+#endif  // _PICO_BTSTACK_BTSTACK_CONFIG_H
\ No newline at end of file
diff --git a/Firmware/cmake/generate_gatt_header.cmake b/Firmware/cmake/generate_gatt_header.cmake
new file mode 100644
index 0000000..8fc76d1
--- /dev/null
+++ b/Firmware/cmake/generate_gatt_header.cmake
@@ -0,0 +1,36 @@
+function(generate_gatt_header BTSTACK_ROOT GATT_INPUT_PATH GATT_HEADER_OUTPUT_PATH)
+    set(SCRIPT_PATH ${BTSTACK_ROOT}/tool/compile_gatt.py)
+
+    if(NOT EXISTS ${SCRIPT_PATH})
+        message(FATAL_ERROR "GATT compilation script not found: ${SCRIPT_PATH}")
+    endif()
+
+    if(EXISTS ${GATT_HEADER_OUTPUT_PATH})
+        message(STATUS "GATT header file already exists: ${GATT_HEADER_OUTPUT_PATH}\nTo regenerate a new GATT header, delete the file and compile again.")
+        return()
+    endif()
+
+    find_package(Python3 COMPONENTS Interpreter REQUIRED)
+
+    if(Python3_Interpreter_FOUND)
+        message(STATUS "Python3 found: ${Python3_EXECUTABLE}")
+        message(STATUS "Python3 version: ${Python3_VERSION}")
+    else()
+        message(FATAL_ERROR "Python3 not found! Python is required to integrate BTStack with ESP-IDF")
+    endif()
+
+    message(STATUS "Generating GATT header file: ${GATT_HEADER_OUTPUT_PATH}")
+    execute_process(
+        COMMAND ${Python3_EXECUTABLE} ${SCRIPT_PATH} ${GATT_INPUT_PATH} ${GATT_HEADER_OUTPUT_PATH}
+        WORKING_DIRECTORY ${BTSTACK_ROOT}
+        RESULT_VARIABLE PYTHON_RESULT
+        OUTPUT_VARIABLE PYTHON_OUTPUT
+        ERROR_VARIABLE PYTHON_ERROR
+    )
+
+    if(PYTHON_RESULT EQUAL 0)
+        message(STATUS "GATT header file generated successfully:\n${PYTHON_OUTPUT}")
+    else()
+        message(FATAL_ERROR "GATT header file generation failed:\n${PYTHON_ERROR}")
+    endif()
+endfunction()
\ No newline at end of file
diff --git a/Firmware/external/init_submodules.cmake b/Firmware/cmake/init_submodules.cmake
similarity index 100%
rename from Firmware/external/init_submodules.cmake
rename to Firmware/cmake/init_submodules.cmake
diff --git a/Firmware/external/patch_libs.cmake b/Firmware/cmake/patch_libs.cmake
similarity index 100%
rename from Firmware/external/patch_libs.cmake
rename to Firmware/cmake/patch_libs.cmake
diff --git a/README.md b/README.md
index 1982783..20bdb21 100644
--- a/README.md
+++ b/README.md
@@ -3,13 +3,13 @@
 
 Firmware for the RP2040, capable of emulating gamepads for several game consoles. The firmware comes in many flavors, supported on the [Adafruit Feather USB Host board](https://www.adafruit.com/product/5723), Pi Pico, Pi Pico 2, Waveshare RP2040-Zero, Pi Pico W, RP2040/ESP32 hybrid, and a 4-Channel RP2040-Zero setup.
 
-Visit the web app here: [https://wiredopposite.github.io/OGX-Mini-WebApp/](https://wiredopposite.github.io/OGX-Mini-WebApp/) to change your mappings and deadzone settings. To pair the OGX-Mini with the web app, plug your controller in, then connect it to your PC, hold **Start + Left Bumper + Right Bumper** to enter web app mode. Click "Connect" in the web app and select the OGX-Mini.
+Visit the web app here: [**https://wiredopposite.github.io/OGX-Mini-WebApp/**](https://wiredopposite.github.io/OGX-Mini-WebApp/) to change your mappings and deadzone settings. To pair the OGX-Mini with the web app, plug your controller in, then connect it to your PC, hold **Start + Left Bumper + Right Bumper** to enter web app mode. Click "Connect" in the web app and select the OGX-Mini.
 
 ## Supported platforms
 - Original Xbox
 - Playstation 3
 - Nintendo Switch (docked)
-- XInput (use [UsbdSecPatch](https://github.com/InvoxiPlayGames/UsbdSecPatch) for Xbox 360, or select the patch in J-Runner while flashing your NAND)
+- XInput (use [**UsbdSecPatch**](https://github.com/InvoxiPlayGames/UsbdSecPatch) for Xbox 360, or select the patch in J-Runner while flashing your NAND)
 - Playstation Classic
 - DInput
 
@@ -68,11 +68,12 @@ Note: Bluetooth functionality is in early testing, some may have quirks.
 - Steam
 - Stadia
 - And more
-Please visit [this page](https://bluepad32.readthedocs.io/en/latest/supported_gamepads/) for a more comprehensive list of supported controllers and Bluetooth pairing instructions.
+
+Please visit [**this page**](https://bluepad32.readthedocs.io/en/latest/supported_gamepads/) for a more comprehensive list of supported controllers and Bluetooth pairing instructions.
 
 ## Features new to v1.0.0
 - Bluetooth functionality for the Pico W and Pico+ESP32.
-- Web application for configuring deadzones and buttons mappings, supports up to 8 saved profiles.
+- Web application (connectable via USB or Bluetooth) for configuring deadzones and buttons mappings, supports up to 8 saved profiles.
 - Pi Pico 2 (RP2350) support.
 - Reduced latency by about 3-4 ms, graphs showing comparisons are coming
 - 4 channel functionality, connect 4 Picos and use one Xbox 360 wireless adapter to control all 4.
@@ -85,8 +86,6 @@ Please visit [this page](https://bluepad32.readthedocs.io/en/latest/supported_ga
 - RGB LED support for RP2040-Zero and Adafruit Feather boards.
 
 ## Planned additions
-- Bluetooth web application
-- Deadzone scaling
 - Anti-deadzone settings
 - More accurate report parser for unknown HID controllers
 - Hardware design for internal OG Xbox install
@@ -111,11 +110,21 @@ If your third party controller isn't working, but the original version is listed
 
 ## Build
 ### RP2040
-You can compile this for different boards with the CMake argument ```OGXM_BOARD``` while configuring the project. The options are:
-```PI_PICO``` ```RP_ZERO``` ```ADA_FEATHER``` ```PI_PICOW``` ```W_ESP32``` ```EXTERNAL_4CH```
+You can compile this for different boards with the CMake argument ```OGXM_BOARD``` while configuring the project. 
+
+The options are:
+- ```PI_PICO``` 
+- ```PI_PICO2``` 
+- ```PI_PICOW``` 
+- ```PI_PICO2W``` 
+- ```RP_ZERO``` 
+- ```ADA_FEATHER``` 
+- ```PICO_ESP32``` 
+- ```EXTERNAL_4CH```
+
 You can also set ```MAX_GAMEPADS``` which, if greater than one, will only support DInput (PS3) and Switch.
 
-You'll need git, python3, CMake, Ninja and the GCC ARM toolchain installed. CMake scripts will patch some files in Bluepad32 and BTStack, and also init the git submodules to make sure everything's there. Here's an example on Windows:
+You'll need git, python3, CMake, Ninja and the GCC ARM toolchain installed. CMake scripts will patch some files in Bluepad32 and BTStack and also make sure all git submodules (plus their submodules and dependencies) are downloaded. Here's an example on Windows:
 ```
 git clone --recursive https://github.com/wiredopposite/OGX-Mini.git
 cd OGX-Mini/Firmware/RP2040
@@ -125,8 +134,8 @@ cmake --build build
 Or just install the GCC ARM toolchain and use the CMake Tools extension in VSCode.
 
 ### ESP32
-Please see the Hardware directory for a schematic showing how to hookup the ESP32 to your RP2040.
+Please see the Hardware directory for a diagram showing how to hookup the ESP32 to your RP2040.
 
-You will need ESP-IDF v5.1, esptool, python3, and git installed. If you use VSCode you can install the ESP-IDF extension and configure the project for v5.1, it'll download everything for you and then you just click the build button at the bottom of the window.
+You will need ESP-IDF v5.1, esptool, python3, and git installed. If you use VSCode, you can install the ESP-IDF extension and configure the project for ESP-IDF v5.1, it'll download everything for you and then you just click the build button at the bottom of the window.
 
-When you build with ESP-IDF, Cmake will run a python script that copies the necessary BTStack files into the components directory, this is needed since BTStack isn't configured as an ESP-IDF component when you download it. 
\ No newline at end of file
+When you build with ESP-IDF, Cmake will run a python script that copies the necessary BTStack files into the components directory, this is needed since BTStack isn't configured as an ESP-IDF component when you download it with git. 
\ No newline at end of file
diff --git a/WebApp b/WebApp
index 49958c4..0e572ef 160000
--- a/WebApp
+++ b/WebApp
@@ -1 +1 @@
-Subproject commit 49958c4dffe2776d5aa581d4f44610450348ac9c
+Subproject commit 0e572eff08b25950c06f77fb9c8eac71d9f79a48