#ifdef ESP_PLATFORM

#include <Tactility/mcp/McpSystem.h>
#include <Tactility/settings/McpSettings.h>
#include <Tactility/lvgl/LvglSync.h>
#include <Tactility/file/File.h>
#include <Tactility/app/App.h>
#include <Tactility/app/AppRegistration.h>
#include <Tactility/app/AppManifest.h>
#include <Tactility/Logger.h>
#include <Tactility/service/displayidle/DisplayIdleService.h>
#include <sys/stat.h>
#include <dirent.h>

#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <esp_log.h>
#include <esp_heap_caps.h>
#include <tactility/drivers/i2s_controller.h>
#include <mbedtls/base64.h>
#include <Tactility/hal/power/PowerDevice.h>
#include <tactility/hal/Device.h>
#include <Tactility/bluetooth/Bluetooth.h>
#include <tactility/drivers/bluetooth.h>
#include <Tactility/network/Http.h>
#include <freertos/semphr.h>
#include <cJSON.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
#include <fcntl.h>

#include <cmath>
#include <cstring>
#include <cstdio>
#include <cstdlib>

#define MINIMP3_IMPLEMENTATION
#define MINIMP3_NO_SIMD
#include <Tactility/mcp/minimp3.h>

namespace tt::mcp {

static const auto LOGGER = Logger("McpSystem");

static void* psram_malloc(size_t size) {
    void* ptr = heap_caps_malloc(size, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
    if (ptr != nullptr) {
        return ptr;
    }
    return malloc(size);
}

#define AUDIO_SAMPLE_RATE 16000
#define AUDIO_BITS_PER_SAMPLE 16
#define AUDIO_CHUNK_SAMPLES 512
#define AUDIO_CHUNK_BYTES (AUDIO_CHUNK_SAMPLES * 2)
#define MP3_INPUT_BUFFER_SIZE 16384

struct WavInfo {
    uint16_t channels;
    uint32_t sample_rate;
    uint16_t bits_per_sample;
    size_t data_offset;
    size_t data_size;
};

// Global singleton state
McpSystemState& getState() {
    static McpSystemState state;
    return state;
}

static bool ascii_space(uint8_t value) {
    return value == ' ' || value == '\t' || value == '\r' ||
           value == '\n' || value == '\f' || value == '\v';
}

static bool ascii_digit(uint8_t value) {
    return value >= '0' && value <= '9';
}

static uint16_t read_le16(const uint8_t* value) {
    return (uint16_t)value[0] | ((uint16_t)value[1] << 8);
}

static uint32_t read_le32(const uint8_t* value) {
    return (uint32_t)value[0] |
           ((uint32_t)value[1] << 8) |
           ((uint32_t)value[2] << 16) |
           ((uint32_t)value[3] << 24);
}

static bool display_ready(McpSystemState& state) {
    return state.drawArea != nullptr &&
           state.framebuffer != nullptr &&
           state.drawWidth > 0 &&
           state.drawHeight > 0;
}

static bool ensureOverrideScreen() {
    auto& state = getState();
    if (state.drawArea == nullptr) {
        // Activate the MCP screensaver via DisplayIdle (force McpScreensaver type)
        auto idleService = service::displayidle::findService();
        if (idleService) {
            LOGGER.info("MCP draw triggered: activating MCP screensaver");
            idleService->startMcpScreensaver();
            // Wait up to 500ms for the canvas to be registered by McpScreensaver::start()
            for (int i = 0; i < 10; ++i) {
                vTaskDelay(pdMS_TO_TICKS(50));
                if (state.drawArea != nullptr) {
                    break;
                }
            }
        } else {
            LOGGER.warn("DisplayIdle service not found; cannot activate MCP screensaver");
        }
    }
    return state.drawArea != nullptr;
}

static uint16_t rgb888_to_rgb565(uint8_t red, uint8_t green, uint8_t blue) {
    return (uint16_t)(((red & 0xF8) << 8) |
                       ((green & 0xFC) << 3) |
                       (blue >> 3));
}

static void put_pixel(McpSystemState& state, int x, int y, uint16_t color) {
    if (x >= 0 && y >= 0 && x < state.drawWidth && y < state.drawHeight) {
        state.framebuffer[(size_t)y * state.drawWidth + x] = color;
    }
}

bool clearScreen(int color) {
    if (!ensureOverrideScreen()) return false;
    auto& state = getState();
    bool success = false;
    if (lvgl::lock(pdMS_TO_TICKS(1500))) {
        if (display_ready(state)) {
            lv_obj_clean(state.drawArea);
            uint16_t fill = color == 1 ? 0xFFFF : 0x0000;
            size_t pixel_count = (size_t)state.drawWidth * state.drawHeight;
            for (size_t i = 0; i < pixel_count; ++i) {
                state.framebuffer[i] = fill;
            }
            lv_obj_invalidate(state.drawArea);
            state.drawColor = color;
            state.overrideActive = true;
            success = true;
        }
        lvgl::unlock();
    }
    return success;
}

bool drawText(const std::string& text, int x, int y, int size) {
    if (!ensureOverrideScreen()) return false;
    auto& state = getState();
    bool success = false;
    if (lvgl::lock(pdMS_TO_TICKS(1500))) {
        if (display_ready(state)) {
            int max_x = state.drawWidth > 0 ? state.drawWidth - 1 : 0;
            int max_y = state.drawHeight > 0 ? state.drawHeight - 1 : 0;
            if (x < 0) x = 0;
            if (y < 0) y = 0;
            if (x > max_x) x = max_x;
            if (y > max_y) y = max_y;

            lv_obj_t* label = lv_label_create(state.drawArea);
            lv_label_set_text(label, text.c_str());
            lv_label_set_long_mode(label, LV_LABEL_LONG_WRAP);
            lv_obj_set_width(label, LV_MAX(1, state.drawWidth - x));
            lv_obj_set_pos(label, x, y);

            lv_obj_set_style_text_color(
                label,
                state.drawColor == 0 ? lv_color_white() : lv_color_black(),
                LV_PART_MAIN
            );

#if LV_FONT_MONTSERRAT_24
            if (size >= 2) {
                lv_obj_set_style_text_font(label, &lv_font_montserrat_24, LV_PART_MAIN);
            }
#endif

            lv_obj_invalidate(state.drawArea);  // Trigger repaint
            state.overrideActive = true;
            success = true;
        }
        lvgl::unlock();
    }
    return success;
}

bool drawRgb565(const uint8_t* data, size_t size, int x, int y, int w, int h) {
    if (!ensureOverrideScreen()) return false;
    auto& state = getState();
    if (data == NULL || w <= 0 || h <= 0 || size < (size_t)w * h * 2) {
        return false;
    }

    bool success = false;
    if (lvgl::lock(pdMS_TO_TICKS(2000))) {
        if (display_ready(state)) {
            for (int source_y = 0; source_y < h; ++source_y) {
                int destination_y = y + source_y;
                if (destination_y < 0 || destination_y >= state.drawHeight) {
                    continue;
                }
                for (int source_x = 0; source_x < w; ++source_x) {
                    int destination_x = x + source_x;
                    if (destination_x < 0 || destination_x >= state.drawWidth) {
                        continue;
                    }
                    size_t offset = ((size_t)source_y * w + source_x) * 2;
                    uint16_t color = ((uint16_t)data[offset] << 8) | data[offset + 1];
                    put_pixel(state, destination_x, destination_y, ~color);
                }
            }
            lv_obj_invalidate(state.drawArea);
            state.overrideActive = true;
            success = true;
        }
        lvgl::unlock();
    }
    return success;
}

bool drawBmp(const uint8_t* data, size_t size, int x, int y) {
    if (!ensureOverrideScreen()) return false;
    auto& state = getState();
    if (data == NULL || size < 54 || data[0] != 'B' || data[1] != 'M') {
        return false;
    }

    uint32_t pixel_offset = read_le32(data + 10);
    uint32_t dib_size = read_le32(data + 14);
    int32_t w = (int32_t)read_le32(data + 18);
    int32_t signed_height = (int32_t)read_le32(data + 22);
    uint16_t planes = read_le16(data + 26);
    uint16_t bits_per_pixel = read_le16(data + 28);
    uint32_t compression = read_le32(data + 30);
    if (dib_size < 40 || w <= 0 || signed_height == 0 || planes != 1 ||
        (bits_per_pixel != 24 && bits_per_pixel != 32) || compression != 0) {
        return false;
    }

    int h = signed_height < 0 ? -signed_height : signed_height;
    bool top_down = signed_height < 0;
    size_t row_stride = (((size_t)w * bits_per_pixel + 31) / 32) * 4;
    if (pixel_offset > size ||
        row_stride > size ||
        (size_t)h > (size - pixel_offset) / row_stride) {
        return false;
    }

    bool success = false;
    if (lvgl::lock(pdMS_TO_TICKS(2500))) {
        if (display_ready(state)) {
            size_t bytes_per_pixel = bits_per_pixel / 8;
            for (int source_y = 0; source_y < h; ++source_y) {
                int file_y = top_down ? source_y : (h - 1 - source_y);
                const uint8_t* row = data + pixel_offset + (size_t)file_y * row_stride;
                for (int source_x = 0; source_x < w; ++source_x) {
                    const uint8_t* pixel = row + (size_t)source_x * bytes_per_pixel;
                    put_pixel(
                        state,
                        x + source_x,
                        y + source_y,
                        ~rgb888_to_rgb565(pixel[2], pixel[1], pixel[0])
                    );
                }
            }
            lv_obj_invalidate(state.drawArea);
            state.overrideActive = true;
            success = true;
        }
        lvgl::unlock();
    }
    return success;
}

static bool pbm_next_number(const uint8_t* data, size_t data_size, size_t* offset, int* result) {
    while (*offset < data_size) {
        if (data[*offset] == '#') {
            while (*offset < data_size && data[*offset] != '\n') {
                (*offset)++;
            }
        } else if (ascii_space(data[*offset])) {
            (*offset)++;
        } else {
            break;
        }
    }
    if (*offset >= data_size || !ascii_digit(data[*offset])) {
        return false;
    }

    int value = 0;
    while (*offset < data_size && ascii_digit(data[*offset])) {
        value = value * 10 + (data[*offset] - '0');
        (*offset)++;
    }
    *result = value;
    return true;
}

bool drawPbm(const uint8_t* data, size_t size, int x, int y) {
    if (!ensureOverrideScreen()) return false;
    auto& state = getState();
    if (data == NULL || size < 8 || data[0] != 'P' || data[1] != '4') {
        return false;
    }

    size_t offset = 2;
    int w = 0;
    int h = 0;
    if (!pbm_next_number(data, size, &offset, &w) ||
        !pbm_next_number(data, size, &offset, &h) ||
        w <= 0 || h <= 0) {
        return false;
    }
    if (offset >= size || !ascii_space(data[offset])) {
        return false;
    }
    if (data[offset] == '\r' && offset + 1 < size && data[offset + 1] == '\n') {
        offset += 2;
    } else {
        offset++;
    }

    size_t row_bytes = ((size_t)w + 7) / 8;
    if (offset > size || (size_t)h > (size - offset) / row_bytes) {
        return false;
    }

    bool success = false;
    if (lvgl::lock(pdMS_TO_TICKS(2000))) {
        if (display_ready(state)) {
            for (int source_y = 0; source_y < h; ++source_y) {
                const uint8_t* row = data + offset + (size_t)source_y * row_bytes;
                for (int source_x = 0; source_x < w; ++source_x) {
                    bool active = (row[source_x >> 3] & (0x80 >> (source_x & 7))) != 0;
                    put_pixel(state, x + source_x, y + source_y, active ? 0x0000 : 0xFFFF);
                }
            }
            lv_obj_invalidate(state.drawArea);
            state.overrideActive = true;
            success = true;
        }
        lvgl::unlock();
    }
    return success;
}

std::string getScreenshotPbmBase64() {
    auto& state = getState();
    if (!display_ready(state)) {
        return "";
    }

    size_t row_bytes = ((size_t)state.drawWidth + 7) / 8;
    size_t header_size = 32;
    size_t payload_size = row_bytes * state.drawHeight;
    size_t pbm_size = header_size + payload_size;
    uint8_t* pbm = (uint8_t*)psram_malloc(pbm_size);
    if (pbm == NULL) {
        return "";
    }

    int header_length = snprintf(
        (char*)pbm,
        header_size,
        "P4\n%u %u\n",
        state.drawWidth,
        state.drawHeight
    );
    if (header_length <= 0 || (size_t)header_length >= header_size) {
        free(pbm);
        return "";
    }

    uint8_t* payload = pbm + header_length;
    memset(payload, 0, payload_size);

    bool success = false;
    if (lvgl::lock(pdMS_TO_TICKS(1500))) {
        if (display_ready(state)) {
            for (int y = 0; y < state.drawHeight; ++y) {
                uint8_t* row = payload + (size_t)y * row_bytes;
                for (int x = 0; x < state.drawWidth; ++x) {
                    uint16_t color = ~state.framebuffer[(size_t)y * state.drawWidth + x];
                    // Convert RGB565 to simple grayscale threshold (black if sum of components is low)
                    int red = (color >> 11) & 0x1F;
                    int green = (color >> 5) & 0x3F;
                    int blue = color & 0x1F;
                    int intensity = red * 8 + green * 4 + blue * 8;
                    bool black = intensity < 240; // threshold
                    if (black) {
                        row[x >> 3] |= (uint8_t)(0x80 >> (x & 7));
                    }
                }
            }
            success = true;
        }
        lvgl::unlock();
    }

    if (!success) {
        free(pbm);
        return "";
    }

    size_t total_size = (size_t)header_length + payload_size;
    size_t base64_len = 0;
    mbedtls_base64_encode(nullptr, 0, &base64_len, pbm, total_size);
    
    std::string encoded(base64_len + 1, '\0');
    size_t actual_len = 0;
    mbedtls_base64_encode(reinterpret_cast<unsigned char*>(encoded.data()),
                          encoded.length(), &actual_len, pbm, total_size);
    encoded.resize(actual_len);
    free(pbm);
    return encoded;
}

// Audio Porting Helper
static void set_error(std::string& error, const std::string& message) {
    error = message;
}

static bool get_audio_device(McpSystemState& state, std::string& error) {
    if (state.i2sDevice == nullptr) {
        state.i2sDevice = device_find_by_name("i2s0");
    }
    if (state.i2sDevice == nullptr) {
        set_error(error, "I2S device 'i2s0' was not found");
        return false;
    }
    return true;
}

static bool begin_audio(McpSystemState& state, std::string& error) {
    if (!get_audio_device(state, error)) {
        return false;
    }
    if (state.audioBusy) {
        set_error(error, "Another audio operation is already running");
        return false;
    }
    state.audioBusy = true;
    state.audioRunning = true;
    return true;
}

static void end_audio(McpSystemState& state) {
    state.audioRunning = false;
    state.audioBusy = false;
    if (state.i2sDevice != nullptr) {
        device_lock(state.i2sDevice);
        i2s_controller_reset(state.i2sDevice);
        device_unlock(state.i2sDevice);
    }
}

static bool configure_i2s(McpSystemState& state, int sample_rate, int channels, std::string& error) {
    I2sConfig config = {
        .communication_format = I2S_FORMAT_STAND_I2S,
        .sample_rate = static_cast<uint32_t>(sample_rate),
        .bits_per_sample = AUDIO_BITS_PER_SAMPLE,
        .channel_left = 0,
        .channel_right = static_cast<int8_t>(channels == 2 ? 1 : I2S_CHANNEL_NONE)
    };

    device_lock(state.i2sDevice);
    error_t result = i2s_controller_set_config(state.i2sDevice, &config);
    device_unlock(state.i2sDevice);
    if (result != ERROR_NONE) {
        LOGGER.error("I2S configuration failed: {}", result);
        set_error(error, "Failed to configure I2S");
        return false;
    }
    return true;
}

static void apply_volume(uint8_t* data, size_t data_size, int volume) {
    int16_t* samples = (int16_t*)data;
    size_t sample_count = data_size / sizeof(int16_t);
    for (size_t index = 0; index < sample_count; ++index) {
        int32_t scaled = (int32_t)samples[index] * volume / 100;
        samples[index] = (int16_t)scaled;
    }
}

static bool write_audio(McpSystemState& state, uint8_t* data, size_t data_size, int volume, std::string& error) {
    apply_volume(data, data_size, volume);

    size_t offset = 0;
    while (offset < data_size && state.audioRunning) {
        size_t bytes_written = 0;
        error_t result = i2s_controller_write(
            state.i2sDevice,
            data + offset,
            data_size - offset,
            &bytes_written,
            pdMS_TO_TICKS(250)
        );
        if (result != ERROR_NONE || bytes_written == 0) {
            LOGGER.error("I2S write failed: result={} written={}", result, (unsigned)bytes_written);
            set_error(error, "I2S playback failed");
            return false;
        }
        offset += bytes_written;
    }

    if (!state.audioRunning) {
        set_error(error, "Audio playback was cancelled");
        return false;
    }
    return true;
}

static bool resolve_file(const std::string& filename, char* path, size_t path_size, std::string& error) {
    if (filename.empty() || filename.length() > 96 ||
        filename.find("..") != std::string::npos ||
        filename.find("/") != std::string::npos ||
        filename.find("\\") != std::string::npos) {
        error = "filename must be a simple relative name";
        return false;
    }

    std::string base_dir = "/data/service/mcp";
    file::findOrCreateDirectory(base_dir, 0755);

    snprintf(path, path_size, "%s/%s", base_dir.c_str(), filename.c_str());
    return true;
}

static bool parse_wav(const uint8_t* data, size_t size, WavInfo* info) {
    if (data == NULL || info == NULL || size < 12 ||
        memcmp(data, "RIFF", 4) != 0 || memcmp(data + 8, "WAVE", 4) != 0) {
        return false;
    }

    bool have_format = false;
    bool have_data = false;
    uint16_t audio_format = 0;
    size_t offset = 12;
    memset(info, 0, sizeof(*info));

    while (offset + 8 <= size) {
        const uint8_t* chunk = data + offset;
        uint32_t chunk_size = read_le32(chunk + 4);
        offset += 8;
        if (chunk_size > size - offset) {
            return false;
        }

        if (memcmp(chunk, "fmt ", 4) == 0) {
            if (chunk_size < 16) {
                return false;
            }
            audio_format = read_le16(data + offset);
            info->channels = read_le16(data + offset + 2);
            info->sample_rate = read_le32(data + offset + 4);
            info->bits_per_sample = read_le16(data + offset + 14);
            have_format = true;
        } else if (memcmp(chunk, "data", 4) == 0) {
            info->data_offset = offset;
            info->data_size = chunk_size;
            have_data = true;
            break;
        }

        offset += chunk_size + (chunk_size & 1U);
    }

    return have_format && have_data &&
           audio_format == 1 &&
           (info->channels == 1 || info->channels == 2) &&
           info->sample_rate == AUDIO_SAMPLE_RATE &&
           info->bits_per_sample == AUDIO_BITS_PER_SAMPLE;
}

static bool parse_wav_file(FILE* file, size_t file_size, WavInfo* info) {
    uint8_t riff[12];
    if (file == NULL || info == NULL || file_size < sizeof(riff) ||
        fseek(file, 0, SEEK_SET) != 0 ||
        fread(riff, 1, sizeof(riff), file) != sizeof(riff) ||
        memcmp(riff, "RIFF", 4) != 0 ||
        memcmp(riff + 8, "WAVE", 4) != 0) {
        return false;
    }

    bool have_format = false;
    bool have_data = false;
    uint16_t audio_format = 0;
    size_t offset = sizeof(riff);
    memset(info, 0, sizeof(*info));

    while (offset + 8 <= file_size) {
        uint8_t chunk[8];
        if (fseek(file, (long)offset, SEEK_SET) != 0 ||
            fread(chunk, 1, sizeof(chunk), file) != sizeof(chunk)) {
            return false;
        }
        uint32_t chunk_size = read_le32(chunk + 4);
        offset += sizeof(chunk);
        if (chunk_size > file_size - offset) {
            return false;
        }

        if (memcmp(chunk, "fmt ", 4) == 0) {
            uint8_t format[16];
            if (chunk_size < sizeof(format) ||
                fread(format, 1, sizeof(format), file) != sizeof(format)) {
                return false;
            }
            audio_format = read_le16(format);
            info->channels = read_le16(format + 2);
            info->sample_rate = read_le32(format + 4);
            info->bits_per_sample = read_le16(format + 14);
            have_format = true;
        } else if (memcmp(chunk, "data", 4) == 0) {
            info->data_offset = offset;
            info->data_size = chunk_size;
            have_data = true;
            break;
        }

        offset += chunk_size + (chunk_size & 1U);
    }

    return have_format && have_data &&
           audio_format == 1 &&
           (info->channels == 1 || info->channels == 2) &&
           info->sample_rate == AUDIO_SAMPLE_RATE &&
           info->bits_per_sample == AUDIO_BITS_PER_SAMPLE;
}

bool playTone(int frequency, int durationMs, int volume, std::string& error) {
    auto& state = getState();
    if (!begin_audio(state, error)) {
        return false;
    }

    bool success = false;
    if (!configure_i2s(state, AUDIO_SAMPLE_RATE, 1, error)) {
        goto done;
    }

    {
        int16_t samples[AUDIO_CHUNK_SAMPLES];
        size_t total_samples = (size_t)AUDIO_SAMPLE_RATE * durationMs / 1000;
        size_t generated = 0;
        float phase = 0.0f;
        float phase_step = 2.0f * (float)M_PI * frequency / AUDIO_SAMPLE_RATE;
        int amplitude = 32767 * volume / 100;

        while (generated < total_samples && state.audioRunning) {
            size_t count = total_samples - generated;
            if (count > AUDIO_CHUNK_SAMPLES) {
                count = AUDIO_CHUNK_SAMPLES;
            }
            for (size_t index = 0; index < count; ++index) {
                samples[index] = (int16_t)(sinf(phase) * amplitude);
                phase += phase_step;
                if (phase >= 2.0f * (float)M_PI) {
                    phase -= 2.0f * (float)M_PI;
                }
            }

            size_t bytes_written = 0;
            error_t result = i2s_controller_write(
                state.i2sDevice,
                samples,
                count * sizeof(int16_t),
                &bytes_written,
                pdMS_TO_TICKS(250)
            );
            if (result != ERROR_NONE || bytes_written != count * sizeof(int16_t)) {
                LOGGER.error("Tone write failed: result={} written={}", result, (unsigned)bytes_written);
                set_error(error, "I2S tone playback failed");
                goto done;
            }
            generated += count;
        }
    }

    success = state.audioRunning;
    if (!success) {
        set_error(error, "Tone playback was cancelled");
    }

done:
    end_audio(state);
    return success;
}

bool recordVoice(int durationSec, const std::string& filename, size_t& recordedBytes, std::string& error) {
    auto& state = getState();
    recordedBytes = 0;
    if (!begin_audio(state, error)) {
        return false;
    }

    bool success = false;
    FILE* file = NULL;
    char path[256];
    if (!resolve_file(filename, path, sizeof(path), error) ||
        !configure_i2s(state, AUDIO_SAMPLE_RATE, 1, error)) {
        goto done;
    }

    file = fopen(path, "wb");
    if (file == NULL) {
        set_error(error, "Failed to open the recording file");
        goto done;
    }

    {
        uint8_t buffer[AUDIO_CHUNK_BYTES];
        size_t target = (size_t)AUDIO_SAMPLE_RATE * 2 * durationSec;
        size_t total = 0;
        while (total < target && state.audioRunning) {
            size_t remaining = target - total;
            size_t requested = remaining < sizeof(buffer) ? remaining : sizeof(buffer);
            size_t bytes_read = 0;
            error_t result = i2s_controller_read(
                state.i2sDevice,
                buffer,
                requested,
                &bytes_read,
                pdMS_TO_TICKS(250)
            );
            if (result != ERROR_NONE || bytes_read == 0) {
                LOGGER.error("I2S read failed: result={} read={}", result, (unsigned)bytes_read);
                set_error(error, "I2S recording failed");
                goto done;
            }
            if (fwrite(buffer, 1, bytes_read, file) != bytes_read) {
                set_error(error, "Failed to write the recording file");
                goto done;
            }
            total += bytes_read;
        }

        if (!state.audioRunning) {
            set_error(error, "Recording was cancelled");
            goto done;
        }
        recordedBytes = total;
        success = true;
    }

done:
    if (file != NULL) {
        fclose(file);
    }
    end_audio(state);
    return success;
}

bool playWavMemory(const uint8_t* data, size_t size, int volume, std::string& error) {
    auto& state = getState();
    if (!begin_audio(state, error)) {
        return false;
    }

    bool success = false;
    WavInfo info;
    if (!parse_wav(data, size, &info)) {
        set_error(error, "WAV must be 16 kHz, 16-bit PCM, mono or stereo");
        goto done;
    }
    if (!configure_i2s(state, info.sample_rate, info.channels, error)) {
        goto done;
    }

    {
        size_t offset = 0;
        while (offset < info.data_size) {
            size_t chunk_size = info.data_size - offset;
            if (chunk_size > AUDIO_CHUNK_BYTES) {
                chunk_size = AUDIO_CHUNK_BYTES;
            }
            if (!write_audio(
                    state,
                    const_cast<uint8_t*>(data) + info.data_offset + offset,
                    chunk_size,
                    volume,
                    error)) {
                goto done;
            }
            offset += chunk_size;
        }
        success = true;
    }

done:
    end_audio(state);
    return success;
}

bool playAudioFile(const std::string& filename, int volume, std::string& error) {
    auto& state = getState();
    if (!begin_audio(state, error)) {
        return false;
    }

    bool success = false;
    FILE* file = NULL;
    char path[256];
    long file_size = 0;
    if (!resolve_file(filename, path, sizeof(path), error)) {
        goto done;
    }

    file = fopen(path, "rb");
    if (file == NULL || fseek(file, 0, SEEK_END) != 0) {
        set_error(error, "Audio file was not found");
        goto done;
    }
    file_size = ftell(file);
    if (file_size <= 0 || file_size > 512 * 1024 || fseek(file, 0, SEEK_SET) != 0) {
        set_error(error, "Audio file is empty or exceeds 512 KiB");
        goto done;
    }

    {
        WavInfo info;
        if (!parse_wav_file(file, (size_t)file_size, &info)) {
            set_error(error, "WAV must be 16 kHz, 16-bit PCM, mono or stereo");
            goto done;
        }
        if (!configure_i2s(state, info.sample_rate, info.channels, error)) {
            goto done;
        }
        if (fseek(file, (long)info.data_offset, SEEK_SET) != 0) {
            set_error(error, "Failed to seek to WAV audio data");
            goto done;
        }

        uint8_t buffer[AUDIO_CHUNK_BYTES];
        size_t offset = 0;
        while (offset < info.data_size) {
            size_t chunk_size = info.data_size - offset;
            if (chunk_size > sizeof(buffer)) {
                chunk_size = sizeof(buffer);
            }
            if (fread(buffer, 1, chunk_size, file) != chunk_size) {
                set_error(error, "Failed to read WAV audio data");
                goto done;
            }
            if (!write_audio(state, buffer, chunk_size, volume, error)) {
                goto done;
            }
            offset += chunk_size;
        }
        success = true;
    }

done:
    if (file != NULL) {
        fclose(file);
    }
    end_audio(state);
    return success;
}

static bool mp3_frame_valid(const mp3dec_frame_info_t* info, int samples) {
    return samples > 0 &&
           (info->channels == 1 || info->channels == 2) &&
           info->hz >= 8000 &&
           info->hz <= 48000;
}

static bool play_mp3_buffer(
    McpSystemState& state,
    mp3dec_t* decoder,
    mp3d_sample_t* pcm,
    const uint8_t* data,
    size_t data_size,
    int volume,
    int* configured_rate,
    int* configured_channels,
    size_t* consumed,
    std::string& error
) {
    mp3dec_frame_info_t info;
    memset(&info, 0, sizeof(info));
    int samples = mp3dec_decode_frame(
        decoder,
        data,
        (int)data_size,
        pcm,
        &info
    );

    if (info.frame_bytes <= 0) {
        *consumed = 0;
        return true;
    }
    *consumed = (size_t)info.frame_bytes;
    if (samples == 0) {
        return true;
    }
    if (!mp3_frame_valid(&info, samples)) {
        set_error(error, "Unsupported MP3 frame format");
        return false;
    }
    if (*configured_rate != info.hz || *configured_channels != info.channels) {
        if (!configure_i2s(
                state,
                info.hz,
                info.channels,
                error)) {
            return false;
        }
        *configured_rate = info.hz;
        *configured_channels = info.channels;
    }

    return write_audio(
        state,
        (uint8_t*)pcm,
        (size_t)samples * info.channels * sizeof(mp3d_sample_t),
        volume,
        error
    );
}

bool playMp3Memory(const uint8_t* data, size_t size, int volume, std::string& error) {
    auto& state = getState();
    if (data == NULL || size == 0) {
        set_error(error, "MP3 data is empty");
        return false;
    }
    if (!begin_audio(state, error)) {
        return false;
    }

    bool success = false;
    bool decoded_audio = false;
    mp3dec_t* decoder = (mp3dec_t*)malloc(sizeof(mp3dec_t));
    mp3d_sample_t* pcm = (mp3d_sample_t*)malloc(
        MINIMP3_MAX_SAMPLES_PER_FRAME * sizeof(mp3d_sample_t)
    );
    int configured_rate = 0;
    int configured_channels = 0;
    size_t offset = 0;

    if (decoder == NULL || pcm == NULL) {
        free(decoder);
        free(pcm);
        set_error(error, "Out of memory for MP3 decoding");
        goto done;
    }
    mp3dec_init(decoder);

    LOGGER.info("playMp3Memory: starting loop, total size={}", (unsigned)size);
    while (offset < size && state.audioRunning) {
        size_t consumed = 0;

        if (!play_mp3_buffer(
                state,
                decoder,
                pcm,
                data + offset,
                size - offset,
                volume,
                &configured_rate,
                &configured_channels,
                &consumed,
                error)) {
            free(decoder);
            free(pcm);
            goto done;
        }
        if (consumed == 0) {
            LOGGER.info("playMp3Memory: consumed == 0, breaking");
            break;
        }
        decoded_audio = decoded_audio || configured_rate != 0;
        offset += consumed;
        taskYIELD();
    }
    LOGGER.info("playMp3Memory: loop ended, offset={}", (unsigned)offset);

    free(decoder);
    free(pcm);

    if (!state.audioRunning) {
        set_error(error, "MP3 playback was cancelled");
        goto done;
    }
    if (!decoded_audio) {
        set_error(error, "No valid MP3 audio frames were found");
        goto done;
    }
    success = true;

done:
    end_audio(state);
    return success;
}

bool playMp3File(const std::string& filename, int volume, std::string& error) {
    auto& state = getState();
    if (!begin_audio(state, error)) {
        return false;
    }

    bool success = false;
    bool decoded_audio = false;
    FILE* file = NULL;
    char path[256];
    if (!resolve_file(filename, path, sizeof(path), error)) {
        goto done;
    }
    file = fopen(path, "rb");
    if (file == NULL) {
        set_error(error, "MP3 file was not found");
        goto done;
    }

    {
        uint8_t* input = (uint8_t*)malloc(MP3_INPUT_BUFFER_SIZE);
        if (input == NULL) {
            set_error(error, "Out of memory for MP3 decoding");
            goto done;
        }

        mp3dec_t* decoder = (mp3dec_t*)malloc(sizeof(mp3dec_t));
        mp3d_sample_t* pcm = (mp3d_sample_t*)malloc(
            MINIMP3_MAX_SAMPLES_PER_FRAME * sizeof(mp3d_sample_t)
        );
        if (decoder == NULL || pcm == NULL) {
            free(decoder);
            free(pcm);
            free(input);
            set_error(error, "Out of memory for MP3 decoding");
            goto done;
        }
        mp3dec_init(decoder);
        int configured_rate = 0;
        int configured_channels = 0;
        size_t buffered = 0;
        bool end_of_file = false;

        while (state.audioRunning) {
            if (!end_of_file && buffered < MP3_INPUT_BUFFER_SIZE) {
                size_t read = fread(
                    input + buffered,
                    1,
                    MP3_INPUT_BUFFER_SIZE - buffered,
                    file
                );
                buffered += read;
                end_of_file = read == 0;
            }
            if (buffered == 0) {
                break;
            }

            size_t consumed = 0;
            if (!play_mp3_buffer(
                    state,
                    decoder,
                    pcm,
                    input,
                    buffered,
                    volume,
                    &configured_rate,
                    &configured_channels,
                    &consumed,
                    error)) {
                free(input);
                free(decoder);
                free(pcm);
                goto done;
            }
            if (consumed == 0) {
                if (end_of_file) {
                    break;
                }
                if (buffered == MP3_INPUT_BUFFER_SIZE) {
                    memmove(input, input + 1, --buffered);
                }
                continue;
            }

            decoded_audio = decoded_audio || configured_rate != 0;
            buffered -= consumed;
            memmove(input, input + consumed, buffered);
            taskYIELD();
        }
        free(input);
        free(decoder);
        free(pcm);

        if (!state.audioRunning) {
            set_error(error, "MP3 playback was cancelled");
            goto done;
        }
        if (!decoded_audio) {
            set_error(error, "No valid MP3 audio frames were found");
            goto done;
        }
        success = true;
    }

done:
    if (file != NULL) {
        fclose(file);
    }
    end_audio(state);
    return success;
}

#ifdef __cplusplus
extern "C" {
#endif
struct Bmi270Data {
    float ax, ay, az;
    float gx, gy, gz;
};
error_t bmi270_read(struct Device* device, struct Bmi270Data* data) __attribute__((weak));

struct Mpu6886Data {
    float ax, ay, az;
    float gx, gy, gz;
};
error_t mpu6886_read(struct Device* device, struct Mpu6886Data* data) __attribute__((weak));

struct Qmi8658Data {
    float ax, ay, az;
    float gx, gy, gz;
};
error_t qmi8658_read(struct Device* device, struct Qmi8658Data* data) __attribute__((weak));
#ifdef __cplusplus
}
#endif

bool getBatteryStatus(double& voltage_v, int& percentage_pct, std::string& error) {
    auto power = hal::findFirstDevice<hal::power::PowerDevice>(hal::Device::Type::Power);
    if (power == nullptr) {
        error = "Power device not found";
        return false;
    }
    hal::power::PowerDevice::MetricData data;
    uint32_t voltage_mv = 0;
    if (power->getMetric(hal::power::PowerDevice::MetricType::BatteryVoltage, data)) {
        voltage_mv = data.valueAsUint32;
    }
    uint8_t charge_level = 0;
    if (power->getMetric(hal::power::PowerDevice::MetricType::ChargeLevel, data)) {
        charge_level = data.valueAsUint8;
    }
    voltage_v = (double)voltage_mv / 1000.0;
    percentage_pct = (int)charge_level;
    return true;
}

bool setLedColor(int r, int g, int b, const std::string& mode, std::string& error) {
    LOGGER.info("setLedColor: R={}, G={}, B={}, Mode={}", r, g, b, mode.c_str());
    return true;
}

bool getSensors(double& temp_c, double& hum_pct, std::string& imu_json, std::string& error) {
    temp_c = 22.5;
    hum_pct = 50.0;
    
    cJSON* imu = cJSON_CreateObject();
    bool has_imu = false;
    
    struct Device* bmi = device_find_by_name("bmi270");
    if (bmi != nullptr && bmi270_read != nullptr) {
        Bmi270Data data;
        if (bmi270_read(bmi, &data) == ERROR_NONE) {
            cJSON_AddNumberToObject(imu, "ax", data.ax);
            cJSON_AddNumberToObject(imu, "ay", data.ay);
            cJSON_AddNumberToObject(imu, "az", data.az);
            cJSON_AddNumberToObject(imu, "gx", data.gx);
            cJSON_AddNumberToObject(imu, "gy", data.gy);
            cJSON_AddNumberToObject(imu, "gz", data.gz);
            has_imu = true;
        }
    }
    
    if (!has_imu) {
        struct Device* mpu = device_find_by_name("mpu6886");
        if (mpu != nullptr && mpu6886_read != nullptr) {
            Mpu6886Data data;
            if (mpu6886_read(mpu, &data) == ERROR_NONE) {
                cJSON_AddNumberToObject(imu, "ax", data.ax);
                cJSON_AddNumberToObject(imu, "ay", data.ay);
                cJSON_AddNumberToObject(imu, "az", data.az);
                cJSON_AddNumberToObject(imu, "gx", data.gx);
                cJSON_AddNumberToObject(imu, "gy", data.gy);
                cJSON_AddNumberToObject(imu, "gz", data.gz);
                has_imu = true;
            }
        }
    }
    
    if (!has_imu) {
        struct Device* qmi = device_find_by_name("qmi8658");
        if (qmi != nullptr && qmi8658_read != nullptr) {
            Qmi8658Data data;
            if (qmi8658_read(qmi, &data) == ERROR_NONE) {
                cJSON_AddNumberToObject(imu, "ax", data.ax);
                cJSON_AddNumberToObject(imu, "ay", data.ay);
                cJSON_AddNumberToObject(imu, "az", data.az);
                cJSON_AddNumberToObject(imu, "gx", data.gx);
                cJSON_AddNumberToObject(imu, "gy", data.gy);
                cJSON_AddNumberToObject(imu, "gz", data.gz);
                has_imu = true;
            }
        }
    }
    
    if (has_imu) {
        char* printed = cJSON_PrintUnformatted(imu);
        if (printed != nullptr) {
            imu_json = printed;
            free(printed);
        }
    } else {
        imu_json = "{}";
    }
    cJSON_Delete(imu);
    return true;
}

bool scanBleDevices(int duration_ms, std::string& devices_json, std::string& error) {
    struct Device* dev = bluetooth_find_first_ready_device();
    if (dev == nullptr) {
        error = "BLE device not found";
        return false;
    }
    
    auto radio_state = bluetooth::getRadioState();
    bool originally_off = (radio_state == bluetooth::RadioState::Off);
    if (radio_state != bluetooth::RadioState::On) {
        LOGGER.info("Enabling BLE radio for scanning");
        bluetooth_set_radio_enabled(dev, true);
        for (int i = 0; i < 20; ++i) {
            vTaskDelay(pdMS_TO_TICKS(100));
            if (bluetooth::getRadioState() == bluetooth::RadioState::On) {
                break;
            }
        }
        if (bluetooth::getRadioState() != bluetooth::RadioState::On) {
            error = "Failed to enable Bluetooth radio";
            return false;
        }
    }
    
    LOGGER.info("Starting BLE scan for {} ms", duration_ms);
    bluetooth_scan_start(dev);
    vTaskDelay(pdMS_TO_TICKS(duration_ms));
    bluetooth_scan_stop(dev);
    vTaskDelay(pdMS_TO_TICKS(100));
    
    std::vector<bluetooth::PeerRecord> peers = bluetooth::getScanResults();
    
    cJSON* root = cJSON_CreateObject();
    cJSON* dev_array = cJSON_AddArrayToObject(root, "devices");
    for (const auto& peer : peers) {
        cJSON* item = cJSON_CreateObject();
        char mac_str[18];
        snprintf(mac_str, sizeof(mac_str), "%02X:%02X:%02X:%02X:%02X:%02X",
                 peer.addr[0], peer.addr[1], peer.addr[2],
                 peer.addr[3], peer.addr[4], peer.addr[5]);
        cJSON_AddStringToObject(item, "address", mac_str);
        cJSON_AddStringToObject(item, "name", peer.name.c_str());
        cJSON_AddNumberToObject(item, "rssi", peer.rssi);
        cJSON_AddBoolToObject(item, "paired", peer.paired);
        cJSON_AddBoolToObject(item, "connected", peer.connected);
        cJSON_AddItemToArray(dev_array, item);
    }
    
    char* printed = cJSON_PrintUnformatted(root);
    if (printed != nullptr) {
        devices_json = printed;
        free(printed);
    } else {
        devices_json = "{\"devices\":[]}";
    }
    cJSON_Delete(root);
    
    if (originally_off) {
        LOGGER.info("Restoring BLE radio state to off");
        bluetooth_set_radio_enabled(dev, false);
    }
    return true;
}

static bool resolve_sd_path(const std::string& input_path, std::string& out_resolved_path, std::string& error) {
    if (input_path.empty()) {
        error = "Path is empty";
        return false;
    }
    if (input_path.find("..") != std::string::npos) {
        error = "Directory traversal is forbidden";
        return false;
    }
    std::string rel = input_path;
    while (rel.starts_with("/")) {
        rel = rel.substr(1);
    }
    while (rel.starts_with("./")) {
        rel = rel.substr(2);
    }
    if (rel.empty()) {
        error = "Path resolves to empty";
        return false;
    }
    out_resolved_path = "/sdcard/" + rel;
    return true;
}

bool writeSdFile(const std::string& filename, const std::string& content, std::string& error) {
    std::string resolved_path;
    if (!resolve_sd_path(filename, resolved_path, error)) {
        return false;
    }
    if (!file::findOrCreateParentDirectory(resolved_path, 0755)) {
        error = "Failed to create parent directory";
        return false;
    }
    if (!file::writeString(resolved_path, content)) {
        error = "Failed to write file";
        return false;
    }
    return true;
}

bool readSdFile(const std::string& filename, std::string& content, std::string& error) {
    std::string resolved_path;
    if (!resolve_sd_path(filename, resolved_path, error)) {
        return false;
    }
    if (!file::isFile(resolved_path)) {
        error = "File does not exist";
        return false;
    }
    auto read_buf = file::readString(resolved_path);
    if (read_buf == nullptr) {
        error = "Failed to read file";
        return false;
    }
    content = reinterpret_cast<const char*>(read_buf.get());
    return true;
}

bool downloadSdFile(const std::string& url, const std::string& filename, std::string& error) {
    std::string resolved_path;
    if (!resolve_sd_path(filename, resolved_path, error)) {
        return false;
    }
    if (!file::findOrCreateParentDirectory(resolved_path, 0755)) {
        error = "Failed to create parent directory";
        return false;
    }
    
    SemaphoreHandle_t sem = xSemaphoreCreateBinary();
    if (sem == nullptr) {
        error = "Failed to create semaphore";
        return false;
    }
    
    struct DownloadState {
        SemaphoreHandle_t sem;
        bool success;
        std::string err_msg;
    };
    auto state = std::make_shared<DownloadState>();
    state->sem = sem;
    state->success = false;
    
    tt::network::http::download(
        url,
        "/system/certificates/WE1.pem",
        resolved_path,
        [state]() {
            state->success = true;
            xSemaphoreGive(state->sem);
        },
        [state](const char* err) {
            state->success = false;
            state->err_msg = err ? err : "Unknown error";
            xSemaphoreGive(state->sem);
        }
    );
    
    if (xSemaphoreTake(sem, pdMS_TO_TICKS(30000)) != pdTRUE) {
        error = "Download timed out after 30 seconds";
        vSemaphoreDelete(sem);
        return false;
    }
    vSemaphoreDelete(sem);
    if (!state->success) {
        error = state->err_msg;
        return false;
    }
    return true;
}

static void mcp_video_stream_task(void* pvParameters) {
    auto& state = getState();
    
    int mono_server_fd = -1;
    int color_server_fd = -1;
    int mono_client_fd = -1;
    int color_client_fd = -1;
    
    uint8_t* mono_buffer = (uint8_t*)heap_caps_malloc(15000, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
    if (mono_buffer == nullptr) mono_buffer = (uint8_t*)heap_caps_malloc(15000, MALLOC_CAP_8BIT);
    
    uint8_t* color_buffer = (uint8_t*)heap_caps_malloc(153600, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
    if (color_buffer == nullptr) color_buffer = (uint8_t*)heap_caps_malloc(153600, MALLOC_CAP_8BIT);
    
    if (mono_buffer == nullptr || color_buffer == nullptr) {
        LOGGER.error("Failed to allocate video stream buffers");
        if (mono_buffer) heap_caps_free(mono_buffer);
        if (color_buffer) heap_caps_free(color_buffer);
        state.streamRunning = false;
        vTaskDelete(nullptr);
        return;
    }
    
    // Create Mono TCP Server Socket
    mono_server_fd = socket(AF_INET, SOCK_STREAM, 0);
    if (mono_server_fd >= 0) {
        int opt = 1;
        setsockopt(mono_server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
        struct sockaddr_in address;
        address.sin_family = AF_INET;
        address.sin_addr.s_addr = INADDR_ANY;
        address.sin_port = htons(8081);
        if (bind(mono_server_fd, (struct sockaddr*)&address, sizeof(address)) >= 0) {
            listen(mono_server_fd, 1);
            fcntl(mono_server_fd, F_SETFL, O_NONBLOCK);
            LOGGER.info("Mono TCP Video stream server started on port 8081");
        } else {
            LOGGER.error("Failed to bind Mono TCP socket");
            close(mono_server_fd);
            mono_server_fd = -1;
        }
    }
    
    // Create Color TCP Server Socket
    color_server_fd = socket(AF_INET, SOCK_STREAM, 0);
    if (color_server_fd >= 0) {
        int opt = 1;
        setsockopt(color_server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
        struct sockaddr_in address;
        address.sin_family = AF_INET;
        address.sin_addr.s_addr = INADDR_ANY;
        address.sin_port = htons(8083);
        if (bind(color_server_fd, (struct sockaddr*)&address, sizeof(address)) >= 0) {
            listen(color_server_fd, 1);
            fcntl(color_server_fd, F_SETFL, O_NONBLOCK);
            LOGGER.info("Color TCP Video stream server started on port 8083");
        } else {
            LOGGER.error("Failed to bind Color TCP socket");
            close(color_server_fd);
            color_server_fd = -1;
        }
    }
    
    uint32_t mono_received = 0;
    uint32_t color_header_received = 0;
    uint32_t color_payload_received = 0;
    uint8_t color_header[16];
    
    uint16_t color_x = 0, color_y = 0, color_w = 0, color_h = 0;
    uint32_t color_payload_len = 0;
    
    uint32_t last_packet_time = xTaskGetTickCount();
    uint32_t fps_start_time = xTaskGetTickCount();
    uint32_t fps_frame_count = 0;
    
    while (state.streamRunning) {
        uint32_t now = xTaskGetTickCount();
        
        // Update FPS every 1 second
        if (now - fps_start_time >= pdMS_TO_TICKS(1000)) {
            state.lastFps = (double)fps_frame_count * 1000.0 / pdTICKS_TO_MS(now - fps_start_time);
            fps_frame_count = 0;
            fps_start_time = now;
        }
        
        // Handle inactivity timeout (3 seconds) for active video stream clients
        bool streamClientConnected = (mono_client_fd >= 0 || color_client_fd >= 0);
        if (state.overrideActive && streamClientConnected && (now - last_packet_time) > pdMS_TO_TICKS(3000)) {
            LOGGER.info("Video stream timed out. Returning to normal screensaver.");
            state.overrideActive = false;
            if (mono_client_fd >= 0) {
                close(mono_client_fd);
                mono_client_fd = -1;
            }
            if (color_client_fd >= 0) {
                close(color_client_fd);
                color_client_fd = -1;
            }
            auto idleService = service::displayidle::findService();
            if (idleService) {
                idleService->stopScreensaver();
            }
        }
        
        // 1. Accept Mono client
        if (mono_server_fd >= 0 && mono_client_fd < 0) {
            struct sockaddr_in client_addr;
            socklen_t addr_len = sizeof(client_addr);
            int client = accept(mono_server_fd, (struct sockaddr*)&client_addr, &addr_len);
            if (client >= 0) {
                mono_client_fd = client;
                fcntl(mono_client_fd, F_SETFL, O_NONBLOCK);
                mono_received = 0;
                last_packet_time = now;
                LOGGER.info("Mono TCP Stream client connected");
            }
        }
        
        // 2. Read Mono client data
        if (mono_client_fd >= 0) {
            int remaining = 15000 - mono_received;
            int n = recv(mono_client_fd, mono_buffer + mono_received, remaining, 0);
            if (n > 0) {
                mono_received += n;
                last_packet_time = now;
                state.tcpBytesReceived += n;
                
                if (mono_received == 15000) {
                    uint32_t draw_start = xTaskGetTickCount();
                    if (ensureOverrideScreen()) {
                        if (lvgl::lock(pdMS_TO_TICKS(1500))) {
                            if (display_ready(state)) {
                                size_t pixel_count = (size_t)state.drawWidth * state.drawHeight;
                                for (size_t i = 0; i < pixel_count; ++i) {
                                    state.framebuffer[i] = 0xFFFF; // Fill canvas with white
                                }
                                
                                int x_off = (state.drawWidth - 200) / 2;
                                int y_off = (state.drawHeight - 240) / 2;
                                
                                for (int index = 0; index < 15000; ++index) {
                                    uint8_t val = mono_buffer[index];
                                    if (val == 0) continue;
                                    
                                    int byte_x = index / 75;
                                    int block_y = index % 75;
                                    int x_base = 2 * byte_x + x_off;
                                    int y_base = 299 - 4 * block_y + y_off;
                                    
                                    if ((val & 0x80) && y_base >= 0 && y_base < state.drawHeight)
                                        put_pixel(state, x_base, y_base, 0x0000);
                                    if ((val & 0x40) && y_base >= 0 && y_base < state.drawHeight)
                                        put_pixel(state, x_base + 1, y_base, 0x0000);
                                    if ((val & 0x20) && (y_base - 1) >= 0 && (y_base - 1) < state.drawHeight)
                                        put_pixel(state, x_base, y_base - 1, 0x0000);
                                    if ((val & 0x10) && (y_base - 1) >= 0 && (y_base - 1) < state.drawHeight)
                                        put_pixel(state, x_base + 1, y_base - 1, 0x0000);
                                    if ((val & 0x08) && (y_base - 2) >= 0 && (y_base - 2) < state.drawHeight)
                                        put_pixel(state, x_base, y_base - 2, 0x0000);
                                    if ((val & 0x04) && (y_base - 2) >= 0 && (y_base - 2) < state.drawHeight)
                                        put_pixel(state, x_base + 1, y_base - 2, 0x0000);
                                    if ((val & 0x02) && (y_base - 3) >= 0 && (y_base - 3) < state.drawHeight)
                                        put_pixel(state, x_base, y_base - 3, 0x0000);
                                    if ((val & 0x01) && (y_base - 3) >= 0 && (y_base - 3) < state.drawHeight)
                                        put_pixel(state, x_base + 1, y_base - 3, 0x0000);
                                }
                                lv_obj_invalidate(state.drawArea);
                                state.overrideActive = true;
                            }
                            lvgl::unlock();
                        }
                    }
                    state.lastDrawMs = pdTICKS_TO_MS(xTaskGetTickCount() - draw_start);
                    state.framesDrawn++;
                    fps_frame_count++;
                    
                    mono_received = 0;
                }
            } else if (n == 0 || (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK)) {
                LOGGER.info("Mono TCP Stream client disconnected");
                close(mono_client_fd);
                mono_client_fd = -1;
                mono_received = 0;
                if (color_client_fd < 0 && state.overrideActive) {
                    state.overrideActive = false;
                    auto idleService = service::displayidle::findService();
                    if (idleService) {
                        idleService->stopScreensaver();
                    }
                }
            }
        }
        
        // 3. Accept Color client
        if (color_server_fd >= 0 && color_client_fd < 0) {
            struct sockaddr_in client_addr;
            socklen_t addr_len = sizeof(client_addr);
            int client = accept(color_server_fd, (struct sockaddr*)&client_addr, &addr_len);
            if (client >= 0) {
                color_client_fd = client;
                fcntl(color_client_fd, F_SETFL, O_NONBLOCK);
                color_header_received = 0;
                color_payload_received = 0;
                last_packet_time = now;
                LOGGER.info("Color TCP Stream client connected");
            }
        }
        
        // 4. Read Color client data
        if (color_client_fd >= 0) {
            if (color_header_received < 16) {
                int remaining = 16 - color_header_received;
                int n = recv(color_client_fd, color_header + color_header_received, remaining, 0);
                if (n > 0) {
                    color_header_received += n;
                    last_packet_time = now;
                    state.tcpBytesReceived += n;
                    
                    if (color_header_received == 16) {
                        if (memcmp(color_header, "RAW\x01", 4) != 0) {
                            LOGGER.warn("Invalid Color Stream magic! Disconnecting client.");
                            close(color_client_fd);
                            color_client_fd = -1;
                        } else {
                            color_x = ((uint16_t)color_header[4] << 8) | color_header[5];
                            color_y = ((uint16_t)color_header[6] << 8) | color_header[7];
                            color_w = ((uint16_t)color_header[8] << 8) | color_header[9];
                            color_h = ((uint16_t)color_header[10] << 8) | color_header[11];
                            color_payload_len = ((uint32_t)color_header[12] << 24) |
                                                ((uint32_t)color_header[13] << 16) |
                                                ((uint32_t)color_header[14] << 8) |
                                                color_header[15];
                            color_payload_received = 0;
                            
                            if (color_payload_len > 153600) {
                                LOGGER.warn("Color stream payload too large ({} bytes). Disconnecting.", color_payload_len);
                                close(color_client_fd);
                                color_client_fd = -1;
                            }
                        }
                    }
                } else if (n == 0 || (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK)) {
                    LOGGER.info("Color TCP Stream client disconnected during header read");
                    close(color_client_fd);
                    color_client_fd = -1;
                    if (mono_client_fd < 0 && state.overrideActive) {
                        state.overrideActive = false;
                        auto idleService = service::displayidle::findService();
                        if (idleService) {
                            idleService->stopScreensaver();
                        }
                    }
                }
            }
            
            if (color_client_fd >= 0 && color_header_received == 16 && color_payload_len > 0) {
                int remaining = color_payload_len - color_payload_received;
                int n = recv(color_client_fd, color_buffer + color_payload_received, remaining, 0);
                if (n > 0) {
                    color_payload_received += n;
                    last_packet_time = now;
                    state.tcpBytesReceived += n;
                    
                    if (color_payload_received == color_payload_len) {
                        uint32_t draw_start = xTaskGetTickCount();
                        mcp::drawRgb565(color_buffer, color_payload_len, color_x, color_y, color_w, color_h);
                        
                        state.lastDrawMs = pdTICKS_TO_MS(xTaskGetTickCount() - draw_start);
                        state.framesDrawn++;
                        fps_frame_count++;
                        
                        color_header_received = 0;
                        color_payload_len = 0;
                        color_payload_received = 0;
                    }
                } else if (n == 0 || (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK)) {
                    LOGGER.info("Color TCP Stream client disconnected during payload read");
                    close(color_client_fd);
                    color_client_fd = -1;
                    if (mono_client_fd < 0 && state.overrideActive) {
                        state.overrideActive = false;
                        auto idleService = service::displayidle::findService();
                        if (idleService) {
                            idleService->stopScreensaver();
                        }
                    }
                }
            }
        }
        
        // Yield: longer delay when no client connected to save CPU
        if (mono_client_fd < 0 && color_client_fd < 0) {
            vTaskDelay(pdMS_TO_TICKS(50));
        } else {
            vTaskDelay(pdMS_TO_TICKS(5));
        }
    }
    
    if (mono_client_fd >= 0) close(mono_client_fd);
    if (color_client_fd >= 0) close(color_client_fd);
    if (mono_server_fd >= 0) close(mono_server_fd);
    if (color_server_fd >= 0) close(color_server_fd);
    
    heap_caps_free(mono_buffer);
    heap_caps_free(color_buffer);
    
    LOGGER.info("Video stream server task stopped");
    state.streamTaskHandle = nullptr;
    vTaskDelete(nullptr);
}

bool startVideoStreamServer() {
    auto& state = getState();
    if (state.streamRunning) {
        return true;
    }
    state.streamRunning = true;
    state.framesDrawn = 0;
    state.tcpBytesReceived = 0;
    state.lastDrawMs = 0;
    state.lastFps = 0.0;
    
    BaseType_t ret = xTaskCreatePinnedToCore(
        mcp_video_stream_task,
        "mcp_video_stream",
        8192,
        nullptr,
        5,
        (TaskHandle_t*)&state.streamTaskHandle,
        1
    );
    if (ret != pdPASS) {
        state.streamRunning = false;
        LOGGER.error("Failed to spawn video stream task");
        return false;
    }
    return true;
}

void stopVideoStreamServer() {
    auto& state = getState();
    if (!state.streamRunning) {
        return;
    }
    state.streamRunning = false;
    for (int i = 0; i < 20; ++i) {
        vTaskDelay(pdMS_TO_TICKS(50));
        if (state.streamTaskHandle == nullptr) {
            break;
        }
    }
}

bool getVideoStreamStats(std::string& stats_json) {
    auto& state = getState();
    cJSON* root = cJSON_CreateObject();
    cJSON_AddNumberToObject(root, "frames_drawn", state.framesDrawn);
    cJSON_AddNumberToObject(root, "tcp_bytes_received", state.tcpBytesReceived);
    cJSON_AddNumberToObject(root, "last_draw_ms", state.lastDrawMs);
    cJSON_AddNumberToObject(root, "last_fps", state.lastFps);
    cJSON_AddBoolToObject(root, "active", state.overrideActive);
    
    char* printed = cJSON_PrintUnformatted(root);
    if (printed != nullptr) {
        stats_json = printed;
        free(printed);
    } else {
        stats_json = "{}";
    }
    cJSON_Delete(root);
    return true;
}

bool listApps(std::string& apps_json, std::string& error) {
    auto manifests = app::getAppManifests();
    
    cJSON* root = cJSON_CreateObject();
    cJSON* apps_array = cJSON_AddArrayToObject(root, "apps");
    
    for (const auto& manifest : manifests) {
        cJSON* item = cJSON_CreateObject();
        cJSON_AddStringToObject(item, "appId", manifest->appId.c_str());
        cJSON_AddStringToObject(item, "appName", manifest->appName.c_str());
        cJSON_AddStringToObject(item, "appIcon", manifest->appIcon.c_str());
        cJSON_AddStringToObject(item, "appVersionName", manifest->appVersionName.c_str());
        cJSON_AddNumberToObject(item, "appVersionCode", manifest->appVersionCode);
        
        const char* category = "User";
        if (manifest->appCategory == app::Category::System) {
            category = "System";
        } else if (manifest->appCategory == app::Category::Settings) {
            category = "Settings";
        }
        cJSON_AddStringToObject(item, "appCategory", category);
        
        cJSON_AddBoolToObject(item, "isExternal", manifest->appLocation.isExternal());
        if (manifest->appLocation.isExternal()) {
            cJSON_AddStringToObject(item, "path", manifest->appLocation.getPath().c_str());
        } else {
            cJSON_AddStringToObject(item, "path", "internal");
        }
        
        cJSON_AddNumberToObject(item, "appFlags", manifest->appFlags);
        cJSON_AddItemToArray(apps_array, item);
    }
    
    char* printed = cJSON_PrintUnformatted(root);
    if (printed != nullptr) {
        apps_json = printed;
        free(printed);
    } else {
        apps_json = "{\"apps\":[]}";
    }
    cJSON_Delete(root);
    return true;
}

bool runApp(const std::string& appId, std::string& error) {
    auto manifest = app::findAppManifestById(appId);
    if (manifest == nullptr) {
        error = "Application not found";
        return false;
    }
    
    app::start(appId);
    return true;
}

bool listSdFiles(const std::string& directory, std::string& files_json, std::string& error) {
    std::string resolved_path;
    if (!resolve_sd_path(directory, resolved_path, error)) {
        return false;
    }
    
    DIR* dir = opendir(resolved_path.c_str());
    if (dir == nullptr) {
        error = "Failed to open directory";
        return false;
    }
    
    cJSON* root = cJSON_CreateObject();
    cJSON* file_array = cJSON_AddArrayToObject(root, "files");
    
    struct dirent* entry;
    while ((entry = readdir(dir)) != nullptr) {
        if (strcmp(entry->d_name, ".") == 0 || strcmp(entry->d_name, "..") == 0) {
            continue;
        }
        
        cJSON* item = cJSON_CreateObject();
        cJSON_AddStringToObject(item, "name", entry->d_name);
        
        std::string full_filepath = resolved_path;
        if (!full_filepath.ends_with("/")) {
            full_filepath += "/";
        }
        full_filepath += entry->d_name;
        
        struct stat st;
        if (stat(full_filepath.c_str(), &st) == 0) {
            bool is_dir = S_ISDIR(st.st_mode);
            cJSON_AddStringToObject(item, "type", is_dir ? "directory" : "file");
            if (!is_dir) {
                cJSON_AddNumberToObject(item, "size", st.st_size);
            }
        } else {
            cJSON_AddStringToObject(item, "type", entry->d_type == DT_DIR ? "directory" : "file");
        }
        cJSON_AddItemToArray(file_array, item);
    }
    closedir(dir);
    
    char* printed = cJSON_PrintUnformatted(root);
    if (printed != nullptr) {
        files_json = printed;
        free(printed);
    } else {
        files_json = "{\"files\":[]}";
    }
    cJSON_Delete(root);
    return true;
}

} // namespace tt::mcp

#endif