#include "system.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "nvs.h"
#include "esp_timer.h"
#include <string.h>

static SystemState_t _systemState;

static EventGroupHandle_t _systemEvent;
static EventManager_t _eventManager;

static QueueHandle_t _nvsRequestQueue;
static const char* NVS_NAMESPACE = "bt_devices";
static const char* NVS_KEY_COUNT = "count";
static const char* NVS_NAMESPACE_SETTINGS = "settings";
static const char* NVS_KEY_VOLUME = "volume";

static esp_err_t nvs_load_devices_internal(paired_device_t *devices, size_t *count);
static esp_err_t nvs_save_devices_internal(const paired_device_t *devices, size_t count);

void system_init(void)
{
    _systemState.zeroAngle = 0.0f;
    _systemState.primaryAxis = PRIMARY_AXIS;
    _systemState.pairedDeviceCount = 0;
    _systemState.isCharging = false;
    _systemState.swapLR = false;
    _systemState.volume = 50;  // Default volume
    _systemState.batteryVoltage_mv = 0;
    _systemState.batteryPercentage = 0;

	_systemEvent = xEventGroupCreate();

	_eventManager.count = 0;
    _eventManager.mutex = xSemaphoreCreateMutex();

    system_initNvsService();

    // Load saved volume from NVS
    system_loadVolume();
}

int system_getPrimaryAxis(void)
{
    int axis;

    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    axis = _systemState.primaryAxis;
    xSemaphoreGive(_eventManager.mutex);

    return axis;
}

float system_getAngle(void)
{
    float angle;

    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);


    angle = _systemState.imu.raw[_systemState.primaryAxis] - _systemState.zeroAngle;


	xSemaphoreGive(_eventManager.mutex);

    return angle;
}

void system_setChargeStatus(bool charging)
{
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    _systemState.isCharging = charging;
    xSemaphoreGive(_eventManager.mutex);
}

bool system_getChargeStatus(void)
{
    bool charging;
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    charging = _systemState.isCharging;
    xSemaphoreGive(_eventManager.mutex);
    return charging;
}

void system_setBatteryVoltage(int voltage_mv)
{
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    _systemState.batteryVoltage_mv = voltage_mv;
    xSemaphoreGive(_eventManager.mutex);
}

int system_getBatteryVoltage(void)
{
    int voltage;
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    voltage = _systemState.batteryVoltage_mv;
    xSemaphoreGive(_eventManager.mutex);
    return voltage;
}

void system_setBatteryPercentage(int percentage)
{
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    _systemState.batteryPercentage = percentage;
    xSemaphoreGive(_eventManager.mutex);
}

int system_getBatteryPercentage(void)
{
    int percentage;
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    percentage = _systemState.batteryPercentage;
    xSemaphoreGive(_eventManager.mutex);
    return percentage;
}

void system_setSwapLR(bool swap)
{
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    _systemState.swapLR = swap;
    xSemaphoreGive(_eventManager.mutex);
    ESP_LOGI("system", "Swap L/R: %s", swap ? "ON" : "OFF");
}

bool system_getSwapLR(void)
{
    bool swap;
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    swap = _systemState.swapLR;
    xSemaphoreGive(_eventManager.mutex);
    return swap;
}

void system_toggleSwapLR(void)
{
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    _systemState.swapLR = !_systemState.swapLR;
    ESP_LOGI("system", "Swap L/R toggled: %s", _systemState.swapLR ? "ON" : "OFF");
    xSemaphoreGive(_eventManager.mutex);
}

void system_setZeroAngle(void)
{
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);

    
	_systemState.zeroAngle = _systemState.imu.raw[_systemState.primaryAxis];

    ESP_LOGI("system", "Zero: %.1f", _systemState.zeroAngle);

	xSemaphoreGive(_eventManager.mutex);
}

void system_clearZeroAngle(void)
{
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);

	_systemState.zeroAngle = 0.0f;

    ESP_LOGI("system", "Zero: %.1f", _systemState.zeroAngle);

	xSemaphoreGive(_eventManager.mutex);
}

float system_getZeroAngle(void)
{
    float angle;

    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);

	angle = _systemState.zeroAngle;

	xSemaphoreGive(_eventManager.mutex);

    return angle;
}

void system_setImuData(ImuData_t imu)
{
	xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);

	_systemState.imu = imu;

	xSemaphoreGive(_eventManager.mutex);

    //ESP_LOGI("g", "New IMU Data: %.2f", xy);

	system_notifyAll(EM_EVENT_NEW_DATA);
}

ImuData_t system_getImuData(void)
{
	ImuData_t imu;
	xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);

    imu = _systemState.imu;

	xSemaphoreGive(_eventManager.mutex);

	return imu;
}

void system_waitForEvent(void)
{
}

SystemState_t *system_getState(void)
{
	return &_systemState;
}


BaseType_t system_subscribe(TaskHandle_t task) {

	EventManager_t *em = &_eventManager;

    ESP_LOGI("g", "Subscribe: %p", task);
    if (xSemaphoreTake(em->mutex, portMAX_DELAY) == pdFALSE) {
        return pdFAIL;
    }
    if (em->count < EM_MAX_SUBSCRIBERS) {
        // avoid duplicates?
        ESP_LOGI("g", "PASS");
        em->subscribers[em->count++] = task;
        xSemaphoreGive(em->mutex);
        return pdPASS;
    }
    xSemaphoreGive(em->mutex);
    return pdFAIL; // full
}

BaseType_t system_unsubscribe(TaskHandle_t task) {

	EventManager_t *em = &_eventManager;

    BaseType_t removed = pdFAIL;
    if (xSemaphoreTake(em->mutex, portMAX_DELAY) == pdTRUE) {
        for (size_t i = 0; i < em->count; ++i) {
            if (em->subscribers[i] == task) {
                // shift down
                for (size_t j = i; j + 1 < em->count; ++j)
                    em->subscribers[j] = em->subscribers[j+1];
                em->count--;
                removed = pdPASS;
                break;
            }
        }
        xSemaphoreGive(em->mutex);
    }
    return removed;
}

void system_notifyAll(uint32_t eventBits) {
	EventManager_t *em = &_eventManager;
    if (xSemaphoreTake(em->mutex, portMAX_DELAY) == pdTRUE) {
        for (size_t i = 0; i < em->count; ++i) {
            // set the bits in each task's notification value
            //ESP_LOGI("g", "Notify: %p", em->subscribers[i]);
            xTaskNotify(em->subscribers[i],
                        eventBits,
                        eSetBits);
        }
        xSemaphoreGive(em->mutex);
    }
}

void system_requestBtRefresh(void) {
    ESP_LOGI("system", "BT Refresh requested");
    system_notifyAll(EM_EVENT_BT_REFRESH);
}

void system_requestBtConnect(int device_index) {
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    _systemState.btDeviceIndex = device_index;
    xSemaphoreGive(_eventManager.mutex);
    
    ESP_LOGI("system", "BT Connect requested for device %d", device_index);
    system_notifyAll(EM_EVENT_BT_CONNECT);
}

int system_getBtDeviceIndex(void) {
    int index;
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    index = _systemState.btDeviceIndex;
    xSemaphoreGive(_eventManager.mutex);
    return index;
}

void system_requestVolumeUp(void) {
    ESP_LOGI("system", "Volume Up requested");
    system_notifyAll(EM_EVENT_VOLUME_UP);
}

void system_requestVolumeDown(void) {
    ESP_LOGI("system", "Volume Down requested");
    system_notifyAll(EM_EVENT_VOLUME_DOWN);
}

void system_setVolume(int volume) {
    if (volume < 0) volume = 0;
    if (volume > 100) volume = 100;

    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    _systemState.volume = volume;
    xSemaphoreGive(_eventManager.mutex);

    ESP_LOGI("system", "Volume set to %d", volume);
}

int system_getVolume(void) {
    int volume;
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    volume = _systemState.volume;
    xSemaphoreGive(_eventManager.mutex);
    return volume;
}

esp_err_t system_saveVolume(void) {
    nvs_handle_t nvs_handle;
    esp_err_t ret;

    int volume = system_getVolume();

    ret = nvs_open(NVS_NAMESPACE_SETTINGS, NVS_READWRITE, &nvs_handle);
    if (ret != ESP_OK) {
        ESP_LOGE("system", "Failed to open NVS namespace for volume write: %s", esp_err_to_name(ret));
        return ret;
    }

    ret = nvs_set_i32(nvs_handle, NVS_KEY_VOLUME, volume);
    if (ret != ESP_OK) {
        ESP_LOGE("system", "Failed to write volume to NVS: %s", esp_err_to_name(ret));
        nvs_close(nvs_handle);
        return ret;
    }

    ret = nvs_commit(nvs_handle);
    nvs_close(nvs_handle);

    if (ret == ESP_OK) {
        ESP_LOGI("system", "Volume %d saved to NVS", volume);
    } else {
        ESP_LOGE("system", "Failed to commit volume to NVS: %s", esp_err_to_name(ret));
    }

    return ret;
}

esp_err_t system_loadVolume(void) {
    nvs_handle_t nvs_handle;
    esp_err_t ret;
    int32_t volume = 50;  // Default value

    ret = nvs_open(NVS_NAMESPACE_SETTINGS, NVS_READONLY, &nvs_handle);
    if (ret != ESP_OK) {
        ESP_LOGI("system", "No saved volume found, using default: %d", volume);
        system_setVolume(volume);
        return ESP_OK;  // Not an error, just means no saved value
    }

    ret = nvs_get_i32(nvs_handle, NVS_KEY_VOLUME, &volume);
    nvs_close(nvs_handle);

    if (ret == ESP_OK) {
        ESP_LOGI("system", "Loaded volume from NVS: %d", volume);
        system_setVolume(volume);
    } else if (ret == ESP_ERR_NVS_NOT_FOUND) {
        ESP_LOGI("system", "No saved volume found, using default: %d", volume);
        system_setVolume(volume);
        ret = ESP_OK;  // Not an error
    } else {
        ESP_LOGE("system", "Failed to read volume from NVS: %s", esp_err_to_name(ret));
    }

    return ret;
}

void system_initNvsService(void) {
    _nvsRequestQueue = xQueueCreate(10, sizeof(nvs_request_t));
    if (_nvsRequestQueue == NULL) {
        ESP_LOGE("system", "Failed to create NVS request queue");
    }
    
    memset(_systemState.pairedDevices, 0, sizeof(_systemState.pairedDevices));
    _systemState.pairedDeviceCount = 0;
    
    paired_device_t devices[MAX_PAIRED_DEVICES];
    size_t count = 0;
    if (nvs_load_devices_internal(devices, &count) == ESP_OK) {
        xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
        memcpy(_systemState.pairedDevices, devices, count * sizeof(paired_device_t));
        _systemState.pairedDeviceCount = count;
        xSemaphoreGive(_eventManager.mutex);
        ESP_LOGI("system", "Loaded %d paired devices from NVS", count);
    }
}

static esp_err_t nvs_load_devices_internal(paired_device_t *devices, size_t *count) {
    nvs_handle_t nvs_handle;
    esp_err_t ret;
    
    ret = nvs_open(NVS_NAMESPACE, NVS_READONLY, &nvs_handle);
    if (ret != ESP_OK) {
        ESP_LOGE("system", "Failed to open NVS namespace: %s", esp_err_to_name(ret));
        *count = 0;
        return ret;
    }
    
    size_t device_count = 0;
    size_t required_size = sizeof(size_t);
    ret = nvs_get_blob(nvs_handle, NVS_KEY_COUNT, &device_count, &required_size);
    if (ret != ESP_OK) {
        nvs_close(nvs_handle);
        *count = 0;
        return ESP_OK;
    }
    
    device_count = (device_count > MAX_PAIRED_DEVICES) ? MAX_PAIRED_DEVICES : device_count;
    
    for (size_t i = 0; i < device_count; i++) {
        char key[16];
        snprintf(key, sizeof(key), "device_%zu", i);
        required_size = sizeof(paired_device_t);
        ret = nvs_get_blob(nvs_handle, key, &devices[i], &required_size);
        if (ret != ESP_OK) {
            ESP_LOGW("system", "Failed to load device %zu", i);
            device_count = i;
            break;
        }
    }
    
    nvs_close(nvs_handle);
    *count = device_count;
    return ESP_OK;
}

static esp_err_t nvs_save_devices_internal(const paired_device_t *devices, size_t count) {
    nvs_handle_t nvs_handle;
    esp_err_t ret;
    
    ret = nvs_open(NVS_NAMESPACE, NVS_READWRITE, &nvs_handle);
    if (ret != ESP_OK) {
        ESP_LOGE("system", "Failed to open NVS namespace for write: %s", esp_err_to_name(ret));
        return ret;
    }
    
    ret = nvs_set_blob(nvs_handle, NVS_KEY_COUNT, &count, sizeof(size_t));
    if (ret != ESP_OK) {
        nvs_close(nvs_handle);
        return ret;
    }
    
    for (size_t i = 0; i < count; i++) {
        char key[16];
        snprintf(key, sizeof(key), "device_%zu", i);
        ret = nvs_set_blob(nvs_handle, key, &devices[i], sizeof(paired_device_t));
        if (ret != ESP_OK) {
            nvs_close(nvs_handle);
            return ret;
        }
    }
    
    ret = nvs_commit(nvs_handle);
    nvs_close(nvs_handle);
    return ret;
}

void system_processNvsRequests(void) {
    nvs_request_t request;
    
    while (xQueueReceive(_nvsRequestQueue, &request, 0) == pdTRUE) {
        xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
        switch (request.operation) {
            case NVS_OP_SAVE_DEVICE:
                request.result = nvs_save_devices_internal(_systemState.pairedDevices, _systemState.pairedDeviceCount);
                break;
                
            case NVS_OP_LOAD_DEVICES:
                request.result = nvs_load_devices_internal(_systemState.pairedDevices, &_systemState.pairedDeviceCount);
                break;
                
            case NVS_OP_REMOVE_DEVICE: {
                size_t removed_index = SIZE_MAX;
                for (size_t i = 0; i < _systemState.pairedDeviceCount; i++) {
                    if (memcmp(_systemState.pairedDevices[i].bda, request.bda, ESP_BD_ADDR_LEN) == 0) {
                        removed_index = i;
                        break;
                    }
                }
                
                if (removed_index != SIZE_MAX) {
                    for (size_t i = removed_index; i < _systemState.pairedDeviceCount - 1; i++) {
                        _systemState.pairedDevices[i] = _systemState.pairedDevices[i + 1];
                    }
                    _systemState.pairedDeviceCount--;
                    request.result = nvs_save_devices_internal(_systemState.pairedDevices, _systemState.pairedDeviceCount);
                } else {
                    request.result = ESP_ERR_NOT_FOUND;
                }
                break;
            }
            
            case NVS_OP_UPDATE_TIMESTAMP: {
                for (size_t i = 0; i < _systemState.pairedDeviceCount; i++) {
                    if (memcmp(_systemState.pairedDevices[i].bda, request.bda, ESP_BD_ADDR_LEN) == 0) {
                        _systemState.pairedDevices[i].last_connected = (uint32_t)(esp_timer_get_time() / 1000000);
                        request.result = nvs_save_devices_internal(_systemState.pairedDevices, _systemState.pairedDeviceCount);
                        break;
                    }
                }
                break;
            }

            default:
                request.result = ESP_ERR_INVALID_ARG;
                break;
        }
        xSemaphoreGive(_eventManager.mutex);

        // Send the result directly as the notification value (not a pointer)
        if (request.requestor) {
            xTaskNotify(request.requestor, (uint32_t)request.result, eSetValueWithOverwrite);
        }
    }
}

esp_err_t system_savePairedDevice(const paired_device_t *device) {
    if (!device) return ESP_ERR_INVALID_ARG;
    
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    
    size_t existing_index = SIZE_MAX;
    for (size_t i = 0; i < _systemState.pairedDeviceCount; i++) {
        if (memcmp(_systemState.pairedDevices[i].bda, device->bda, ESP_BD_ADDR_LEN) == 0) {
            existing_index = i;
            break;
        }
    }
    
    if (existing_index != SIZE_MAX) {
        _systemState.pairedDevices[existing_index] = *device;
    } else if (_systemState.pairedDeviceCount < MAX_PAIRED_DEVICES) {
        _systemState.pairedDevices[_systemState.pairedDeviceCount++] = *device;
    } else {
        xSemaphoreGive(_eventManager.mutex);
        return ESP_ERR_NO_MEM;
    }
    
    xSemaphoreGive(_eventManager.mutex);
    
    nvs_request_t request = {
        .operation = NVS_OP_SAVE_DEVICE,
        .device = *device,
        .requestor = xTaskGetCurrentTaskHandle()
    };
    
    if (xQueueSend(_nvsRequestQueue, &request, pdMS_TO_TICKS(NVS_TIMEOUT_MS)) == pdTRUE) {
        uint32_t notification;
        if (xTaskNotifyWait(0, UINT32_MAX, &notification, pdMS_TO_TICKS(NVS_TIMEOUT_MS)) == pdTRUE) {
            // notification contains the result directly (not a pointer)
            return (esp_err_t)notification;
        }
    }
    return ESP_ERR_TIMEOUT;
}

esp_err_t system_loadPairedDevices(paired_device_t *devices, size_t *count) {
    if (!devices || !count) return ESP_ERR_INVALID_ARG;
    
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    size_t copy_count = (*count < _systemState.pairedDeviceCount) ? *count : _systemState.pairedDeviceCount;
    memcpy(devices, _systemState.pairedDevices, copy_count * sizeof(paired_device_t));
    *count = copy_count;
    xSemaphoreGive(_eventManager.mutex);
    
    return ESP_OK;
}

esp_err_t system_removePairedDevice(esp_bd_addr_t bda) {
    nvs_request_t request = {
        .operation = NVS_OP_REMOVE_DEVICE,
        .requestor = xTaskGetCurrentTaskHandle()
    };
    memcpy(request.bda, bda, ESP_BD_ADDR_LEN);
    
    if (xQueueSend(_nvsRequestQueue, &request, pdMS_TO_TICKS(NVS_TIMEOUT_MS)) == pdTRUE) {
        uint32_t notification;
        if (xTaskNotifyWait(0, UINT32_MAX, &notification, pdMS_TO_TICKS(NVS_TIMEOUT_MS)) == pdTRUE) {
            // notification contains the result directly (not a pointer)
            return (esp_err_t)notification;
        }
    }
    return ESP_ERR_TIMEOUT;
}

bool system_isDeviceKnown(esp_bd_addr_t bda) {
    bool known = false;
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    for (size_t i = 0; i < _systemState.pairedDeviceCount; i++) {
        if (memcmp(_systemState.pairedDevices[i].bda, bda, ESP_BD_ADDR_LEN) == 0) {
            known = true;
            break;
        }
    }
    xSemaphoreGive(_eventManager.mutex);
    return known;
}

esp_err_t system_getKnownDeviceCount(size_t *count) {
    if (!count) return ESP_ERR_INVALID_ARG;
    
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    *count = _systemState.pairedDeviceCount;
    xSemaphoreGive(_eventManager.mutex);
    return ESP_OK;
}

esp_err_t system_updateConnectionTimestamp(esp_bd_addr_t bda) {
    nvs_request_t request = {
        .operation = NVS_OP_UPDATE_TIMESTAMP,
        .requestor = xTaskGetCurrentTaskHandle()
    };
    memcpy(request.bda, bda, ESP_BD_ADDR_LEN);
    
    if (xQueueSend(_nvsRequestQueue, &request, pdMS_TO_TICKS(NVS_TIMEOUT_MS)) == pdTRUE) {
        uint32_t notification;
        if (xTaskNotifyWait(0, UINT32_MAX, &notification, pdMS_TO_TICKS(NVS_TIMEOUT_MS)) == pdTRUE) {
            // notification contains the result directly (not a pointer)
            return (esp_err_t)notification;
        }
    }
    return ESP_ERR_TIMEOUT;
}

const paired_device_t* system_getPairedDevices(size_t *count) {
    if (!count) return NULL;

    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    *count = _systemState.pairedDeviceCount;
    xSemaphoreGive(_eventManager.mutex);

    return (const paired_device_t*)_systemState.pairedDevices;
}

esp_err_t system_clearAllPairedDevices(void) {
    // Directly clear in-memory state and save to NVS
    xSemaphoreTake(_eventManager.mutex, portMAX_DELAY);
    _systemState.pairedDeviceCount = 0;
    esp_err_t ret = nvs_save_devices_internal(_systemState.pairedDevices, 0);
    xSemaphoreGive(_eventManager.mutex);

    if (ret == ESP_OK) {
        ESP_LOGI("system", "Cleared all paired devices from NVS");
    } else {
        ESP_LOGE("system", "Failed to clear paired devices: %s", esp_err_to_name(ret));
    }

    return ret;
}