#include "audio_service.h"

#include <math.h>
#include <stddef.h>
#include <stdint.h>
#include <string.h>

#include "driver/i2s_std.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

/* CrowPanel 7.0-inch V3 onboard NS4168 connections. */
#define AUDIO_I2S_PORT              I2S_NUM_0
#define AUDIO_PIN_BCLK              42
#define AUDIO_PIN_LRCLK             18
#define AUDIO_PIN_DOUT              17

#define AUDIO_SAMPLE_RATE_HZ        16000U
#define AUDIO_DMA_BUFFER_COUNT      4
#define AUDIO_DMA_FRAMES            128U
#define AUDIO_CHANNEL_COUNT         2U

#define PING_DURATION_MS            70U
#define PING_TOTAL_FRAMES           ((AUDIO_SAMPLE_RATE_HZ * PING_DURATION_MS) / 1000U)
#define PING_ATTACK_FRAMES          ((AUDIO_SAMPLE_RATE_HZ * 4U) / 1000U)
#define PING_RELEASE_FRAMES         ((AUDIO_SAMPLE_RATE_HZ * 12U) / 1000U)
#define PING_PEAK_AMPLITUDE         7000

#define DISTANCE_MIN_M              0.3f
#define DISTANCE_MAX_M              8.0f
#define PING_FREQUENCY_NEAR_HZ      1800U
#define PING_FREQUENCY_FAR_HZ       700U
#define PING_INTERVAL_NEAR_MS       120U
#define PING_INTERVAL_FAR_MS        900U

#define SINE_TABLE_BITS             8U
#define SINE_TABLE_SIZE             (1U << SINE_TABLE_BITS)
#define AUDIO_TASK_STACK_BYTES      3072U
#define AUDIO_WRITE_TIMEOUT_MS      25U

typedef struct {
    bool enabled;
    bool has_target;
    float nearest_distance_m;
} audio_state_snapshot_t;

static const char *TAG = "audio_service";
static portMUX_TYPE s_state_lock = portMUX_INITIALIZER_UNLOCKED;
static bool s_initialized;
static bool s_initializing;
static bool s_enabled = true;
static bool s_has_target;
static float s_nearest_distance_m;
static TaskHandle_t s_audio_task;
static i2s_chan_handle_t s_tx_channel;

/* Kept out of the worker stack: 512 B each for the LUT and stereo DMA chunk. */
static int16_t s_sine_table[SINE_TABLE_SIZE];
static int16_t s_pcm_chunk[AUDIO_DMA_FRAMES * AUDIO_CHANNEL_COUNT];

static void audio_task(void *context);

static void init_sine_table(void)
{
    const float phase_scale = (2.0f * 3.14159265358979323846f) / (float)SINE_TABLE_SIZE;

    for (uint32_t i = 0; i < SINE_TABLE_SIZE; ++i) {
        s_sine_table[i] = (int16_t)(sinf((float)i * phase_scale) * 32767.0f);
    }
}

static audio_state_snapshot_t state_snapshot(void)
{
    audio_state_snapshot_t snapshot;

    taskENTER_CRITICAL(&s_state_lock);
    snapshot.enabled = s_enabled;
    snapshot.has_target = s_has_target;
    snapshot.nearest_distance_m = s_nearest_distance_m;
    taskEXIT_CRITICAL(&s_state_lock);

    return snapshot;
}

static void notify_audio_task(TaskHandle_t task)
{
    if (task != NULL) {
        xTaskNotifyGive(task);
    }
}

static uint32_t map_frequency_hz(float distance_m)
{
    const float position = (distance_m - DISTANCE_MIN_M) /
                           (DISTANCE_MAX_M - DISTANCE_MIN_M);
    const float span = (float)(PING_FREQUENCY_NEAR_HZ - PING_FREQUENCY_FAR_HZ);

    return (uint32_t)((float)PING_FREQUENCY_NEAR_HZ - (position * span) + 0.5f);
}

static uint32_t map_interval_ms(float distance_m)
{
    const float position = (distance_m - DISTANCE_MIN_M) /
                           (DISTANCE_MAX_M - DISTANCE_MIN_M);
    const float span = (float)(PING_INTERVAL_FAR_MS - PING_INTERVAL_NEAR_MS);

    return (uint32_t)((float)PING_INTERVAL_NEAR_MS + (position * span) + 0.5f);
}

static uint32_t envelope_q15(uint32_t frame)
{
    if (frame < PING_ATTACK_FRAMES) {
        return (frame * 32767U) / PING_ATTACK_FRAMES;
    }

    const uint32_t frames_after = PING_TOTAL_FRAMES - frame - 1U;
    if (frames_after < PING_RELEASE_FRAMES) {
        return (frames_after * 32767U) / PING_RELEASE_FRAMES;
    }

    return 32767U;
}

static bool write_pcm(const int16_t *samples, size_t frame_count)
{
    const uint8_t *source = (const uint8_t *)samples;
    const size_t total_bytes = frame_count * AUDIO_CHANNEL_COUNT * sizeof(int16_t);
    size_t offset = 0;

    while (offset < total_bytes) {
        size_t bytes_written = 0;
        const esp_err_t err = i2s_channel_write(s_tx_channel,
                                                source + offset,
                                                total_bytes - offset,
                                                &bytes_written,
                                                AUDIO_WRITE_TIMEOUT_MS);
        if (err != ESP_OK || bytes_written == 0U) {
            ESP_LOGW(TAG, "I2S write failed: %s (%u/%u bytes)",
                     esp_err_to_name(err),
                     (unsigned)offset,
                     (unsigned)total_bytes);
            return false;
        }
        offset += bytes_written;
    }

    return true;
}

static bool target_still_audible(void)
{
    const audio_state_snapshot_t snapshot = state_snapshot();
    return snapshot.enabled && snapshot.has_target;
}

static bool play_ping(uint32_t frequency_hz)
{
    uint32_t phase = 0;
    const uint32_t phase_step = (uint32_t)(((uint64_t)frequency_hz << 32) /
                                           AUDIO_SAMPLE_RATE_HZ);

    for (uint32_t first_frame = 0; first_frame < PING_TOTAL_FRAMES;
         first_frame += AUDIO_DMA_FRAMES) {
        if (!target_still_audible()) {
            return false;
        }

        uint32_t frame_count = PING_TOTAL_FRAMES - first_frame;
        if (frame_count > AUDIO_DMA_FRAMES) {
            frame_count = AUDIO_DMA_FRAMES;
        }

        for (uint32_t i = 0; i < frame_count; ++i) {
            const uint32_t frame = first_frame + i;
            const uint32_t envelope = envelope_q15(frame);
            const int32_t wave = s_sine_table[phase >> (32U - SINE_TABLE_BITS)];
            int32_t sample = (wave * PING_PEAK_AMPLITUDE) >> 15;
            sample = (sample * (int32_t)envelope) >> 15;
            phase += phase_step;

            s_pcm_chunk[i * AUDIO_CHANNEL_COUNT] = (int16_t)sample;
            s_pcm_chunk[(i * AUDIO_CHANNEL_COUNT) + 1U] = (int16_t)sample;
        }

        if (!write_pcm(s_pcm_chunk, frame_count)) {
            return false;
        }
    }

    return true;
}

/*
 * Queue one DMA ring of zero samples. Once these writes complete, every tone
 * buffer has reached the peripheral, so it is safe to clear and stop the DMA
 * without clipping the 70 ms envelope tail.
 */
static void drain_with_silence(void)
{
    memset(s_pcm_chunk, 0, sizeof(s_pcm_chunk));
    for (uint32_t i = 0; i < AUDIO_DMA_BUFFER_COUNT; ++i) {
        if (!write_pcm(s_pcm_chunk, AUDIO_DMA_FRAMES)) {
            break;
        }
    }
}

static void stop_output(bool *i2s_running, bool drain)
{
    if (!*i2s_running) {
        return;
    }

    if (drain) {
        drain_with_silence();
    }
    (void)i2s_channel_disable(s_tx_channel);
    *i2s_running = false;
}

static void audio_task(void *context)
{
    (void)context;
    bool i2s_running = false;
    bool active_session = false;
    TickType_t last_ping_tick = 0;

    taskENTER_CRITICAL(&s_state_lock);
    s_audio_task = xTaskGetCurrentTaskHandle();
    taskEXIT_CRITICAL(&s_state_lock);

    for (;;) {
        const audio_state_snapshot_t snapshot = state_snapshot();

        if (!snapshot.enabled || !snapshot.has_target) {
            stop_output(&i2s_running, false);
            active_session = false;
            (void)ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
            continue;
        }

        const TickType_t now = xTaskGetTickCount();
        if (active_session) {
            const TickType_t interval_ticks =
                pdMS_TO_TICKS(map_interval_ms(snapshot.nearest_distance_m));
            const TickType_t elapsed_ticks = now - last_ping_tick;
            if (elapsed_ticks < interval_ticks) {
                (void)ulTaskNotifyTake(pdTRUE, interval_ticks - elapsed_ticks);
                continue;
            }
        }

        if (!i2s_running) {
            if (i2s_channel_enable(s_tx_channel) != ESP_OK) {
                ESP_LOGE(TAG, "Could not start I2S output");
                (void)ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(250));
                continue;
            }
            i2s_running = true;
        }

        last_ping_tick = xTaskGetTickCount();
        active_session = true;
        const bool completed = play_ping(map_frequency_hz(snapshot.nearest_distance_m));
        stop_output(&i2s_running, completed);
    }
}

esp_err_t audio_service_init(void)
{
    taskENTER_CRITICAL(&s_state_lock);
    if (s_initialized) {
        taskEXIT_CRITICAL(&s_state_lock);
        return ESP_OK;
    }
    if (s_initializing) {
        taskEXIT_CRITICAL(&s_state_lock);
        return ESP_ERR_INVALID_STATE;
    }
    s_initializing = true;
    taskEXIT_CRITICAL(&s_state_lock);

    init_sine_table();

    i2s_chan_config_t channel_config =
        I2S_CHANNEL_DEFAULT_CONFIG(AUDIO_I2S_PORT, I2S_ROLE_MASTER);
    channel_config.dma_desc_num = AUDIO_DMA_BUFFER_COUNT;
    channel_config.dma_frame_num = AUDIO_DMA_FRAMES;
    channel_config.auto_clear = true;

    const i2s_std_config_t standard_config = {
        .clk_cfg = I2S_STD_CLK_DEFAULT_CONFIG(AUDIO_SAMPLE_RATE_HZ),
        .slot_cfg = I2S_STD_PHILIPS_SLOT_DEFAULT_CONFIG(
            I2S_DATA_BIT_WIDTH_16BIT, I2S_SLOT_MODE_STEREO),
        .gpio_cfg = {
            .mclk = I2S_GPIO_UNUSED,
            .bclk = AUDIO_PIN_BCLK,
            .ws = AUDIO_PIN_LRCLK,
            .dout = AUDIO_PIN_DOUT,
            .din = I2S_GPIO_UNUSED,
            .invert_flags = {
                .mclk_inv = false,
                .bclk_inv = false,
                .ws_inv = false,
            },
        },
    };

    esp_err_t err = i2s_new_channel(&channel_config, &s_tx_channel, NULL);
    if (err == ESP_OK) {
        err = i2s_channel_init_std_mode(s_tx_channel, &standard_config);
    }

    if (err != ESP_OK) {
        if (s_tx_channel != NULL) {
            (void)i2s_del_channel(s_tx_channel);
            s_tx_channel = NULL;
        }
        taskENTER_CRITICAL(&s_state_lock);
        s_initializing = false;
        taskEXIT_CRITICAL(&s_state_lock);
        ESP_LOGE(TAG, "NS4168 I2S initialisation failed: %s", esp_err_to_name(err));
        return err;
    }

    const BaseType_t task_created = xTaskCreate(audio_task,
                                                "proximity_audio",
                                                AUDIO_TASK_STACK_BYTES,
                                                NULL,
                                                tskIDLE_PRIORITY + 4,
                                                NULL);
    if (task_created != pdPASS) {
        (void)i2s_del_channel(s_tx_channel);
        s_tx_channel = NULL;
        taskENTER_CRITICAL(&s_state_lock);
        s_initializing = false;
        taskEXIT_CRITICAL(&s_state_lock);
        return ESP_ERR_NO_MEM;
    }

    taskENTER_CRITICAL(&s_state_lock);
    s_initialized = true;
    s_initializing = false;
    taskEXIT_CRITICAL(&s_state_lock);

    ESP_LOGI(TAG, "NS4168 ready: 16 kHz, stereo, BCLK=%d LRCLK=%d DOUT=%d",
             AUDIO_PIN_BCLK, AUDIO_PIN_LRCLK, AUDIO_PIN_DOUT);
    return ESP_OK;
}

void audio_service_set_enabled(bool enabled)
{
    TaskHandle_t task;

    taskENTER_CRITICAL(&s_state_lock);
    s_enabled = enabled;
    task = s_audio_task;
    taskEXIT_CRITICAL(&s_state_lock);

    notify_audio_task(task);
}

bool audio_service_is_enabled(void)
{
    bool enabled;

    taskENTER_CRITICAL(&s_state_lock);
    enabled = s_enabled;
    taskEXIT_CRITICAL(&s_state_lock);

    return enabled;
}

void audio_service_set_nearest_distance(float distance_m)
{
    if (!isfinite(distance_m) || distance_m <= 0.0f) {
        audio_service_clear_target();
        return;
    }

    if (distance_m < DISTANCE_MIN_M) {
        distance_m = DISTANCE_MIN_M;
    } else if (distance_m > DISTANCE_MAX_M) {
        distance_m = DISTANCE_MAX_M;
    }

    TaskHandle_t task;
    taskENTER_CRITICAL(&s_state_lock);
    s_nearest_distance_m = distance_m;
    s_has_target = true;
    task = s_audio_task;
    taskEXIT_CRITICAL(&s_state_lock);

    notify_audio_task(task);
}

void audio_service_clear_target(void)
{
    TaskHandle_t task;

    taskENTER_CRITICAL(&s_state_lock);
    s_has_target = false;
    s_nearest_distance_m = 0.0f;
    task = s_audio_task;
    taskEXIT_CRITICAL(&s_state_lock);

    notify_audio_task(task);
}

bool audio_service_has_target(void)
{
    bool has_target;

    taskENTER_CRITICAL(&s_state_lock);
    has_target = s_has_target;
    taskEXIT_CRITICAL(&s_state_lock);

    return has_target;
}

bool audio_service_get_nearest_distance(float *distance_m)
{
    bool has_target;
    float distance;

    taskENTER_CRITICAL(&s_state_lock);
    has_target = s_has_target;
    distance = s_nearest_distance_m;
    taskEXIT_CRITICAL(&s_state_lock);

    if (has_target && distance_m != NULL) {
        *distance_m = distance;
    }
    return has_target;
}