#include <stdio.h>
#include "ad5940.h"
#include "ad5941_port.h"
#include "eis.h"
#include "ble.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "nvs_flash.h"

#define AD5941_EXPECTED_ADIID   0x4144

static EISConfig cfg;
static EISPoint results[EIS_MAX_POINTS];
static bool afe_inited;

static void do_sweep(void)
{
    if (!afe_inited) {
        eis_init(&cfg);
        afe_inited = true;
    }

    uint32_t n = eis_calc_num_points(&cfg);
    int got = eis_sweep(results, n);
    printf("Sweep complete: %d points\n", got);

    ble_send_sweep_start(got, cfg.freq_start_hz, cfg.freq_stop_hz);
    for (int i = 0; i < got; i++) {
        ble_send_eis_point(i, &results[i]);
        vTaskDelay(pdMS_TO_TICKS(10));
    }
    ble_send_sweep_end();
}

void app_main(void)
{
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        nvs_flash_erase();
        nvs_flash_init();
    }

    printf("EIS4: AD5941 bring-up\n");

    AD5940_MCUResourceInit(NULL);
    vTaskDelay(pdMS_TO_TICKS(50));
    AD5940_HWReset();
    vTaskDelay(pdMS_TO_TICKS(200));
    AD5940_Initialize();

    uint32_t adiid = AD5940_ReadReg(REG_AFECON_ADIID);
    printf("ADIID: 0x%04lX %s\n", adiid,
           adiid == AD5941_EXPECTED_ADIID ? "(OK)" : "(FAIL)");
    if (adiid != AD5941_EXPECTED_ADIID) return;

    eis_default_config(&cfg);

    ble_init();
    printf("Waiting for BLE connection...\n");
    ble_wait_for_connection();
    ble_send_config(&cfg);

    BleCommand cmd;
    for (;;) {
        if (ble_recv_command(&cmd, UINT32_MAX) != 0)
            continue;

        switch (cmd.type) {
        case CMD_SET_SWEEP:
            cfg.freq_start_hz    = cmd.sweep.freq_start;
            cfg.freq_stop_hz     = cmd.sweep.freq_stop;
            cfg.points_per_decade = cmd.sweep.ppd;
            eis_reconfigure(&cfg);
            printf("Sweep: %.0f-%.0f Hz, %u ppd\n",
                   cfg.freq_start_hz, cfg.freq_stop_hz, cfg.points_per_decade);
            ble_send_config(&cfg);
            break;

        case CMD_SET_RTIA:
            if (cmd.rtia < RTIA_COUNT) {
                cfg.rtia = cmd.rtia;
                eis_reconfigure(&cfg);
                printf("RTIA: %u\n", cfg.rtia);
            }
            ble_send_config(&cfg);
            break;

        case CMD_SET_RCAL:
            if (cmd.rcal < RCAL_COUNT) {
                cfg.rcal = cmd.rcal;
                eis_reconfigure(&cfg);
                printf("RCAL: %u\n", cfg.rcal);
            }
            ble_send_config(&cfg);
            break;

        case CMD_START_SWEEP:
            do_sweep();
            break;

        case CMD_GET_CONFIG:
            ble_send_config(&cfg);
            break;
        }
    }
}