#include "devices/Display.h"
#include <driver/gpio.h>

#include <tactility/device.h>
#include <tactility/drivers/i2c_controller.h>
#include <tactility/log.h>
#include <tactility/error.h>
#include <tactility/freertos/freertos.h>

#include <Tactility/hal/Configuration.h>
#include <ButtonControl.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>

using namespace tt::hal;

static constexpr auto* TAG = "WaveshareRLCD";
static constexpr uint8_t ES8311_I2C_ADDR = 0x18; // Speaker DAC
static constexpr uint8_t ES7210_I2C_ADDR = 0x40; // 4-ch mic ADC array

static ::Device* s_i2c_bus = nullptr; // kept for dynamic reclocking

// Forward decl for I2S rate hook
static error_t reconfigureEs8311ForRate(uint32_t sample_rate);

// ES8311 register helper using cached i2c bus
static error_t es8311Write(uint8_t reg, uint8_t val) {
    if (!s_i2c_bus) return ERROR_RESOURCE;
    uint8_t pair[2] = { reg, val };
    return i2c_controller_write_register_array(s_i2c_bus, ES8311_I2C_ADDR, pair, sizeof(pair), pdMS_TO_TICKS(200));
}

// Public C hook called from esp32_i2s driver when sample rate changes (weak linkage)
extern "C" void waveshare_rlcd_on_i2s_rate_change(uint32_t sample_rate) {
    reconfigureEs8311ForRate(sample_rate);
}

static error_t reconfigureEs8311ForRate(uint32_t sample_rate) {
    if (!s_i2c_bus) return ERROR_NONE;
    // Your MP3 files are 16kHz mono (faith-mysterious-garden/page001.mp3 size 31652).
    // The boot init already programs perfect 16k config (0x48/0x10/0x00/0x03/0x00/0xFF).
    // Re-writing dividers while I2S is reconfiguring causes pop + repetition glitch.
    // So: for 16k, do nothing. For other rates, only ensure unmuted.
    if (sample_rate == 16000 || sample_rate == 0) {
        return ERROR_NONE;
    }
    // For non-16k: keep clock tree stable, only ensure DAC unmuted/powered
    // (ES8311 slave will follow external BCLK/WS)
    es8311Write(0x31, 0x00);
    es8311Write(0x32, 0xBF);
    es8311Write(0x00, 0x80);
    LOG_I(TAG, "ES8311 kept for %u Hz (no divider change)", (unsigned)sample_rate);
    return ERROR_NONE;
}

/** ES8311 speaker DAC init — EXACT sequence from working MicroPython audio_util.py
 *  RLCD board: MCLK=12.288MHz (via GPIO16 PWM), I2S bclk=9 ws=45 tx=8
 *  The working MP sequence does a soft-reset then programs full clock tree.
 */
static error_t initEs8311(::Device* i2c) {
    auto wr = [&](uint8_t reg, uint8_t val) -> error_t {
        uint8_t pair[2] = { reg, val };
        return i2c_controller_write_register_array(i2c, ES8311_I2C_ADDR, pair, sizeof(pair), pdMS_TO_TICKS(200));
    };
    error_t err;
    // 1. Soft reset sequence (exactly as MP)
    if ((err = wr(0x00, 0x1F)) != ERROR_NONE) return err;
    vTaskDelay(pdMS_TO_TICKS(10));
    if ((err = wr(0x00, 0x00)) != ERROR_NONE) return err;
    vTaskDelay(pdMS_TO_TICKS(10));

    // Clock config — MP values: 16kHz SR, MCLK=12.288MHz reference
    // For RLCD MCLK=12.288MHz the MP uses pre_div=3 pre_multi=1 (0x48)
    if ((err = wr(0x01, 0x3F)) != ERROR_NONE) return err;
    if ((err = wr(0x02, 0x48)) != ERROR_NONE) return err;
    if ((err = wr(0x03, 0x10)) != ERROR_NONE) return err;
    if ((err = wr(0x04, 0x10)) != ERROR_NONE) return err;
    if ((err = wr(0x05, 0x00)) != ERROR_NONE) return err;
    if ((err = wr(0x06, 0x03)) != ERROR_NONE) return err;
    if ((err = wr(0x07, 0x00)) != ERROR_NONE) return err;
    if ((err = wr(0x08, 0xFF)) != ERROR_NONE) return err;

    // Format: 16-bit standard I2S
    if ((err = wr(0x09, 0x0C)) != ERROR_NONE) return err;
    if ((err = wr(0x0A, 0x0C)) != ERROR_NONE) return err;

    // System / DAC power
    if ((err = wr(0x0D, 0x01)) != ERROR_NONE) return err;
    if ((err = wr(0x0E, 0x02)) != ERROR_NONE) return err;
    if ((err = wr(0x12, 0x00)) != ERROR_NONE) return err;
    if ((err = wr(0x13, 0x10)) != ERROR_NONE) return err;
    if ((err = wr(0x1C, 0x6A)) != ERROR_NONE) return err;
    if ((err = wr(0x37, 0x08)) != ERROR_NONE) return err;
    if ((err = wr(0x32, 0xBF)) != ERROR_NONE) return err; // 0dB
    if ((err = wr(0x31, 0x00)) != ERROR_NONE) return err; // unmute
    if ((err = wr(0x00, 0x80)) != ERROR_NONE) return err; // power on

    return ERROR_NONE;
}

/** ES7210 4-ch mic ADC init — low-level ops via helper to avoid bulk-array
 *  complexity from ESPHome reference sequence. Ported from working MP's ES7210::init()
 *  MCLK=12.288MHz, 16kHz SR, 16-bit I2S, dual mic.
 */
static error_t initEs7210(::Device* i2c) {
    auto wr = [&](uint8_t reg, uint8_t val) -> error_t {
        uint8_t pair[2] = { reg, val };
        return i2c_controller_write_register_array(i2c, ES7210_I2C_ADDR, pair, sizeof(pair), pdMS_TO_TICKS(200));
    };
    auto rd = [&](uint8_t reg, uint8_t* out) -> error_t {
        return i2c_controller_read_register(i2c, ES7210_I2C_ADDR, reg, out, 1, pdMS_TO_TICKS(200));
    };

    error_t err;
    uint8_t v;

    // Soft reset
    if ((err = wr(0x00, 0xFF)) != ERROR_NONE) return err;
    vTaskDelay(pdMS_TO_TICKS(20));
    if ((err = wr(0x00, 0x32)) != ERROR_NONE) return err;
    vTaskDelay(pdMS_TO_TICKS(20));
    if ((err = wr(0x01, 0x3F)) != ERROR_NONE) return err; // clock off during config

    if ((err = wr(0x09, 0x30)) != ERROR_NONE) return err;
    if ((err = wr(0x0A, 0x30)) != ERROR_NONE) return err;
    if ((err = wr(0x23, 0x2A)) != ERROR_NONE) return err;
    if ((err = wr(0x22, 0x0A)) != ERROR_NONE) return err;
    if ((err = wr(0x20, 0x0A)) != ERROR_NONE) return err;
    if ((err = wr(0x21, 0x2A)) != ERROR_NONE) return err;

    if ((err = rd(0x08, &v)) != ERROR_NONE) return err;
    if ((err = wr(0x08, v & ~0x01)) != ERROR_NONE) return err;

    if ((err = wr(0x40, 0xC3)) != ERROR_NONE) return err;
    if ((err = wr(0x41, 0x70)) != ERROR_NONE) return err;
    if ((err = wr(0x42, 0x70)) != ERROR_NONE) return err;

    // I2S format: 16-bit standard I2S, TDM disabled
    if ((err = wr(0x11, 0x60)) != ERROR_NONE) return err;
    if ((err = wr(0x12, 0x00)) != ERROR_NONE) return err;

    // SR: 16kHz @ 12.288MHz MCLK — adc_div=3, dll=1, doubler=1, osr=32, lrck=768
    if ((err = wr(0x02, 0xC3)) != ERROR_NONE) return err;
    if ((err = wr(0x07, 0x20)) != ERROR_NONE) return err;
    if ((err = wr(0x04, 0x03)) != ERROR_NONE) return err;
    if ((err = wr(0x05, 0x00)) != ERROR_NONE) return err;

    for (int i = 0; i < 4; i++) {
        if ((err = rd(0x43 + i, &v)) != ERROR_NONE) return err;
        if ((err = wr(0x43 + i, v & ~0x10)) != ERROR_NONE) return err;
    }

    if ((err = wr(0x4B, 0xFF)) != ERROR_NONE) return err;
    if ((err = wr(0x4C, 0xFF)) != ERROR_NONE) return err;

    // Enable ADC12 clocks
    if ((err = rd(0x01, &v)) != ERROR_NONE) return err;
    if ((err = wr(0x01, v & ~0x0B)) != ERROR_NONE) return err;
    // Power on MIC1/2 bias, ADC, PGA
    if ((err = wr(0x4B, 0x00)) != ERROR_NONE) return err;

    if ((err = rd(0x43, &v)) != ERROR_NONE) return err;
    if ((err = wr(0x43, (v & ~0x0F) | 0x10 | 0x0A)) != ERROR_NONE) return err; // SELMIC + gain 30dB

    if ((err = rd(0x44, &v)) != ERROR_NONE) return err;
    if ((err = wr(0x44, (v & ~0x0F) | 0x10 | 0x0A)) != ERROR_NONE) return err;

    if ((err = wr(0x47, 0x08)) != ERROR_NONE) return err;
    if ((err = wr(0x48, 0x08)) != ERROR_NONE) return err;
    if ((err = wr(0x49, 0x08)) != ERROR_NONE) return err;
    if ((err = wr(0x4A, 0x08)) != ERROR_NONE) return err;

    if ((err = wr(0x06, 0x04)) != ERROR_NONE) return err; // Power down DLL

    if ((err = wr(0x00, 0x71)) != ERROR_NONE) return err;
    vTaskDelay(pdMS_TO_TICKS(20));
    if ((err = wr(0x00, 0x41)) != ERROR_NONE) return err;
    vTaskDelay(pdMS_TO_TICKS(100));

    return ERROR_NONE;
}

static bool initBoot() {
    // 1. Initialize speaker amplifier control (GPIO 46, Active HIGH for RLCD)
    gpio_config_t io_conf = {};
    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.pin_bit_mask = (1ULL << GPIO_NUM_46);
    io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
    gpio_config(&io_conf);

    // RLCD: amp enable is active HIGH. Keep LOW during early boot to avoid
    // pop / power dip during codec init, then enable after successful codec
    // init so that MCP play_tone / play_wav works immediately.
    // Screen freeze was caused by GPIO5 conflict (DTS bclk=5 == display DC=5),
    // not by amp power — that is now fixed in DTS (bclk=9).
    gpio_set_level(GPIO_NUM_46, 0);

    vTaskDelay(pdMS_TO_TICKS(100));

    // 2. Configure audio codecs — probe before init, non-fatal if absent
    auto* i2c_bus = device_find_by_name("i2c0");
    if (i2c_bus == nullptr) {
        LOG_W(TAG, "i2c0 not found, skipping audio init");
        return true;
    }
    s_i2c_bus = i2c_bus; // keep for reclocking hook

    bool audio_ok = false;

    // ES8311 DAC @ 0x18
    if (i2c_controller_has_device_at_address(i2c_bus, ES8311_I2C_ADDR, pdMS_TO_TICKS(200)) == ERROR_NONE) {
        error_t err = initEs8311(i2c_bus);
        if (err != ERROR_NONE) {
            LOG_E(TAG, "ES8311 @0x%02X init failed: %s (non-fatal)", ES8311_I2C_ADDR, error_to_string(err));
        } else {
            LOG_I(TAG, "ES8311 DAC @0x%02X initialized", ES8311_I2C_ADDR);
            audio_ok = true;
        }
    } else {
        LOG_W(TAG, "ES8311 not found @0x%02X", ES8311_I2C_ADDR);
    }

    // ES7210 mic ADC @ 0x40
    if (i2c_controller_has_device_at_address(i2c_bus, ES7210_I2C_ADDR, pdMS_TO_TICKS(200)) == ERROR_NONE) {
        error_t err = initEs7210(i2c_bus);
        if (err != ERROR_NONE) {
            LOG_E(TAG, "ES7210 @0x%02X init failed: %s (non-fatal)", ES7210_I2C_ADDR, error_to_string(err));
        } else {
            LOG_I(TAG, "ES7210 mic ADC @0x%02X initialized", ES7210_I2C_ADDR);
            audio_ok = true;
        }
    } else {
        LOG_W(TAG, "ES7210 not found @0x%02X (mic disabled)", ES7210_I2C_ADDR);
    }

    // Enable speaker amp if any codec present — matches working MP behavior
    // where amp is toggled per playback; for Tactility we keep it on after boot
    // so MCP play_tone / play_mp3 works without extra board hooks.
    if (audio_ok) {
        gpio_set_level(GPIO_NUM_46, 1);
        LOG_I(TAG, "Speaker amp GPIO46 enabled");
    }

    return true;
}

// C-visible amp control for MCP / future audio gating — weak-compatible pattern
// Called from McpSystem.cpp if linked.
extern "C" void waveshare_rlcd_speaker_amp_set(bool enable) {
    gpio_set_level(GPIO_NUM_46, enable ? 1 : 0);
}

static DeviceVector createDevices() {
    return {
        createDisplay(),
        ButtonControl::createTwoButtonControl(GPIO_NUM_18, GPIO_NUM_0) // KEY=18 (primary), BOOT=0 (secondary)
    };
}

extern const Configuration hardwareConfiguration = {
    .initBoot = initBoot,
    .createDevices = createDevices
};