/*******************************************************************************
 * @file    piezo_measure.c
 * @brief   Piezo echo measurement sequencing
 ******************************************************************************/
#include <zephyr/kernel.h>
#include <errno.h>
#include <string.h>

#include "piezo_measure.h"
#include "app_nvs.h"
#include "debug_print.h"

/* burst 주파수 */
#define PIEZO_CFG_FREQ_1_8MHZ 0
#define PIEZO_CFG_FREQ_1_9MHZ 1
#define PIEZO_CFG_FREQ_2_0MHZ 2
#define PIEZO_CFG_FREQ_2_1MHZ 3
#define PIEZO_CFG_FREQ_2_2MHZ 4
#define PIEZO_CFG_FREQ_2_3MHZ 5

/* 앱에서 저장/변경 가능한 piezo 측정 기본값 */
#define PIEZO_CFG_FREQ_DEFAULT       PIEZO_CFG_FREQ_2_1MHZ
#define PIEZO_CFG_CYCLES_DEFAULT     7
#define PIEZO_CFG_DELAY_DEFAULT      10
#define PIEZO_CFG_SAMPLES_DEFAULT    100
#define PIEZO_CFG_AVG_DEFAULT        3

/* 설정값 검증 범위 */
#define PIEZO_CFG_FREQ_MAX           PIEZO_CFG_FREQ_2_3MHZ
#define PIEZO_CFG_AVG_MAX            10
#define PIEZO_CFG_SAMPLES_MIN        80
#define PIEZO_CFG_SAMPLES_MAX        117
#define PIEZO_CFG_DELAY_MAX_US       50

/* piezo_select_channel() 안에서 MUX settle 1.3 ms를 이미 기다린 뒤 analog path/ADC 입력 안정화를 위해 0.5 ms를 한 번 더 기다림 */
#define PIEZO_POST_SELECT_SETTLE_US  500

/* burst 사이의 회복 시간 */
#define PIEZO_AVG_INTER_BURST_GAP_US 650

/* real capture 전에 같은 채널에서 버스트+ADC capture를 버리는 횟수(dummy) */
#define PIEZO_DUMMY_CAPTURE_COUNT    1

/* 채널 0 시작 전 안정을 위한 추가 딜레이 */
#define PIEZO_CH0_WARMUP_TO_REAL_SETTLE_US 3000

/* 6채널 sweep 결과 (각 채널의 평균 완료 후 최종만 저장) */
static uint16_t piezo_channels[PIEZO_NUM_CHANNELS][PIEZO_MEASURE_MAX_SAMPLES];

/* 단일 capture 임시 버퍼 (dummy capture와 real capture 모두 이 버퍼 재사용) */
static uint16_t echo_capture[PIEZO_MEASURE_MAX_SAMPLES];

/* 평균 누적 버퍼 (uint16 샘플을 avg 횟수만큼 더하므로 overflow 여유를 위해 uint32) */
static uint32_t echo_accum[PIEZO_MEASURE_MAX_SAMPLES];

/* NVS에서 읽어 오거나 mcs? 명령으로 갱신되는 현재 측정 설정 */
static piezo_config_t g_piezo_config = {
    .freq = PIEZO_CFG_FREQ_DEFAULT,
    .cycles = PIEZO_CFG_CYCLES_DEFAULT,
    .avg = PIEZO_CFG_AVG_DEFAULT,
    .delay_us = PIEZO_CFG_DELAY_DEFAULT,
    .samples = PIEZO_CFG_SAMPLES_DEFAULT,
};

/*
 * 저장/측정에 사용할 piezo 설정값 검증
 */
bool piezo_config_validate(const piezo_config_t *cfg)
{
    if (cfg == NULL)
    {
        return false;
    }

    if (cfg->freq > PIEZO_CFG_FREQ_MAX)  // 주파수 : 1.8 MHz (00 00) ~ 2.3 MHz (00 05) 범위에서만 동작하도록 제한
    {
        return false;
    }

    if ((cfg->cycles < 3U) || (cfg->cycles > 7U))  // burst cycles : 3 ~ 7 범위만 허용
    {
        return false;
    }

    if ((cfg->avg == 0U) || (cfg->avg > PIEZO_CFG_AVG_MAX)) // averaging : 1 ~ 10 범위만 허용
    {
        return false;
    }

    if (cfg->delay_us > PIEZO_CFG_DELAY_MAX_US) // burst-to-capture delay : 0 ~ 50 us 범위만 허용
    {
        return false;
    }

    if ((cfg->samples < PIEZO_CFG_SAMPLES_MIN) || (cfg->samples > PIEZO_CFG_SAMPLES_MAX))   // sample count : 80 ~ 117 범위만 허용
    {
        return false;
    }

    return true;
}

const piezo_config_t *piezo_config_get(void)
{
    return &g_piezo_config;
}

/*
 * 부팅 시 NVS 설정 읽기 (없거나 유효하지 않으면 기본값 유지)
 */
int piezo_config_init(void)
{
    int err = app_nvs_init(&g_piezo_config, piezo_config_validate, NULL);
    if (err)
    {
        return err;
    }

    return 0;
}

/*
* 런타임 설정 변경: RAM 설정을 갱신한 뒤 NVS에도 저장
*/
int piezo_config_set(const piezo_config_t *cfg)
{
    if (!piezo_config_validate(cfg))
    {
        return -EINVAL;
    }

    g_piezo_config = *cfg;
    return app_nvs_save_piezo(&g_piezo_config);
}

/*
 * sweep 명령이 끝난 뒤 BLE 응답에서 채널별 결과 버퍼 참조 시 사용
 */
const uint16_t *piezo_measure_channel_buffer(uint8_t channel)
{
    if (channel >= PIEZO_NUM_CHANNELS)
    {
        return NULL;
    }

    return piezo_channels[channel];
}

/*
 * mec?/mad? 같은 단일 채널 명령의 최종 결과 버퍼 반환
 */
const uint16_t *piezo_measure_single_buffer(void)
{
    return echo_capture;
}

/*
 * piezo 측정 공통 준비 단계
 * 전원 인가, piezo GPIO 초기화, echo ADC 초기화/wake를 한 번에 수행
 */
int piezo_measure_start_session(void)
{
    int err = piezo_init();
    if (err)
    {
        DBG_PRINTF("[PIEZO] init fail err=%d\r\n", err);
        return ECHO_STATUS_PIEZO;
    }

    piezo_power_on();
    k_msleep(PIEZO_POWER_STABILIZE_MS);

    err = echo_adc_init();
    if (err)
    {
        DBG_PRINTF("[ECHO] init fail err=%d\r\n", err);
        return ECHO_STATUS_ADC_INIT;
    }

    err = echo_adc_wake();
    if (err)
    {
        DBG_PRINTF("[ECHO] wake fail err=%d\r\n", err);
        return ECHO_STATUS_ADC_INIT;
    }

    DBG_PRINTF("[SWEEP] piezo session ready\r\n");
    return ECHO_STATUS_OK;
}

/*
 * 6채널 전체 sweep
 * 각 채널마다 dummy capture를 먼저 버린 뒤, cfg->avg회 real capture를 sample index별로 평균
 */
int piezo_measure_sweep(void)
{
    const piezo_config_t *cfg = piezo_config_get();

    uint8_t freq = cfg->freq;
    uint8_t cycles = cfg->cycles;
    uint16_t capture_delay_us = cfg->delay_us;
    uint16_t samples = cfg->samples;
    uint8_t avg = cfg->avg;

    if (capture_delay_us < 0)
    {
        capture_delay_us = 0;
    }

    DBG_PRINTF("[SWEEP] freq=0x%04X cycles=%u avg=%u delay_us=%u samples=%u\r\n", cfg->freq, cfg->cycles, cfg->avg, cfg->delay_us, cfg->samples);

    for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
    {
        //DBG_PRINTF("[SWEEP] ch=%d start\r\n", ch);

        // MUX 채널 선택
        int err = piezo_select_channel(ch);
        if (err)
        {
            DBG_PRINTF("[PIEZO] mux fail ch=%d err=%d\r\n", ch, err);
            return ECHO_STATUS_MUX;
        }

        k_busy_wait(PIEZO_POST_SELECT_SETTLE_US);

        // ADC dummy read/wake 2byte (채널 전환 직후 첫 ADC 접근을 실제 데이터에 섞지 않기 위한 준비 단계)
        err = echo_adc_wake();
        if (err)
        {
            DBG_PRINTF("[ECHO] dummy read fail ch=%d err=%d\r\n", ch, err);
            return ECHO_STATUS_CAPTURE;
        }
        
        if (ch == 0U) // 채널 0 시작 전에만 dummy burst + ADC capture 수행
        {
            // Dummy burst + ADC caputre
            /*
            for (uint8_t dummy = 0; dummy < PIEZO_DUMMY_CAPTURE_COUNT; dummy++)
            {
                k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);

                // 실제 측정 신호 흔들림 방지를 위해 dummy 측정에도 IRQ 잠금 적용
                unsigned int key = irq_lock();  

                // burst + delay + capture
                piezo_burst_sw_freq(freq, cycles);  // 주파수 옵션 있는 burst 함수
                k_busy_wait(capture_delay_us);
                err = echo_adc_capture(echo_capture, samples);

                // IRQ 잠금 해제
                irq_unlock(key);

                if (err)
                {
                    DBG_PRINTF("[ECHO] dummy capture fail ch=%d dummy=%d err=%d\r\n", ch, dummy, err);
                    return ECHO_STATUS_CAPTURE;
                }
            }*/

            k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
            piezo_burst_sw_freq(freq, cycles);
            k_busy_wait(capture_delay_us);
            err = echo_adc_capture(echo_capture, samples);
            if (err)
            {
                //DBG_PRINTF("[ECHO] dummy capture fail ch=%d dummy=%d err=%d\r\n", ch, dummy, err);
                return ECHO_STATUS_CAPTURE;
            }

            // 마지막 dummy capture와 첫 real capture 사이에도 같은 회복 시간 + 추가 안정화
            k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US + PIEZO_CH0_WARMUP_TO_REAL_SETTLE_US);
        }
        
        // 여기부터 실제 평균에 들어갈 capture
        memset(echo_accum, 0, sizeof(echo_accum));

        for (uint16_t a = 0; a < avg; a++)
        {
            k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);

            // burst-to-capture 사이 타이밍 흔들림을 줄이기 위해 real capture 구간만 IRQ를 잠금
            unsigned int key = irq_lock();

            // burst + delay + capture
            piezo_burst_sw_freq(freq, cycles);  // 주파수 옵션 있는 burst 함수
            k_busy_wait(capture_delay_us);
            err = echo_adc_capture(echo_capture, samples);

            // IRQ 잠금 해제
            irq_unlock(key);

            if (err)
            {
                DBG_PRINTF("[ECHO] capture fail ch=%d avg=%d err=%d\r\n", ch, a, err);
                return ECHO_STATUS_CAPTURE;
            }

            // 같은 sample index끼리 누적
            for (uint16_t i = 0; i < samples; i++)
            {
                echo_accum[i] += echo_capture[i];
            }
        }

        // 채널별 최종 평균 파형 저장 - 이후 cmd handler가 reb: 패킷으로 전송
        for (uint16_t i = 0; i < samples; i++)
        {
            piezo_channels[ch][i] = (uint16_t)(echo_accum[i] / avg);
        }

        //DBG_PRINTF("[SWEEP] ch=%d done\r\n", ch);
    }

    //DBG_PRINTF("[SWEEP] done\r\n");
    return ECHO_STATUS_OK;
}

/*
 * 테스트용
 * 단일 채널 burst + echo capture
 * mec? 디버그/테스트 명령에서 사용하며, 결과는 echo_capture에 평균 저장
 */
int piezo_measure_single_capture(uint8_t freq, uint8_t cycles, uint16_t delay_us, uint16_t num_samples, uint16_t averaging, uint8_t channel)
{
    if (channel >= PIEZO_NUM_CHANNELS)
    {
        return ECHO_STATUS_MUX;
    }

    if ((num_samples == 0U) || (num_samples > PIEZO_MEASURE_MAX_SAMPLES))
    {
        return ECHO_STATUS_CAPTURE;
    }

    /* averaging 0은 명령 파라미터 누락/미지정으로 보고 1회 capture로 보정 */
    if (averaging == 0U)
    {
        averaging = 1U;
    }

    int err = piezo_select_channel(channel);
    if (err)
    {
        return ECHO_STATUS_MUX;
    }

    k_busy_wait(PIEZO_POST_SELECT_SETTLE_US);

    err = echo_adc_wake();
    if (err)
    {
        return ECHO_STATUS_CAPTURE;
    }

    // real capture 전에 같은 채널에서 dummy burst/capture를 수행하고 버림
    for (uint8_t dummy = 0; dummy < PIEZO_DUMMY_CAPTURE_COUNT; dummy++)
    {
        k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
        piezo_burst_sw_freq(freq, cycles);
        k_busy_wait(delay_us);

        err = echo_adc_capture(echo_capture, num_samples);
        if (err)
        {
            return ECHO_STATUS_CAPTURE;
        }
    }

    // 마지막 dummy capture와 첫 real capture 사이에도 같은 회복 시간
    k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);

    memset(echo_accum, 0, sizeof(echo_accum));

    for (uint16_t avg = 0; avg < averaging; avg++)
    {
        if (avg > 0U)
        {
            k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
        }

        unsigned int key = irq_lock();

        piezo_burst_sw_freq(freq, cycles);
        k_busy_wait(delay_us);
        err = echo_adc_capture(echo_capture, num_samples);

        irq_unlock(key);

        if (err)
        {
            return ECHO_STATUS_CAPTURE;
        }

        for (uint16_t i = 0; i < num_samples; i++)
        {
            echo_accum[i] += echo_capture[i];
        }
    }

    for (uint16_t i = 0; i < num_samples; i++)
    {
        echo_capture[i] = (uint16_t)(echo_accum[i] / averaging);
    }

    return ECHO_STATUS_OK;
}

/*
 * 테스트용
 * 단일 채널 ADC-only capture
 * piezo burst 없이 현재 echo ADC 입력만 평균해서 확인할 때 사용
 */
int piezo_measure_adc_only_capture(uint16_t num_samples, uint16_t averaging, uint8_t channel)
{
    if (channel >= PIEZO_NUM_CHANNELS)
    {
        return ECHO_STATUS_MUX;
    }

    if ((num_samples == 0U) || (num_samples > PIEZO_MEASURE_MAX_SAMPLES))
    {
        return ECHO_STATUS_CAPTURE;
    }

    if (averaging == 0U)
    {
        averaging = 1U;
    }

    int err = piezo_select_channel(channel);
    if (err)
    {
        return ECHO_STATUS_MUX;
    }

    k_busy_wait(PIEZO_POST_SELECT_SETTLE_US);

    err = echo_adc_wake();
    if (err)
    {
        return ECHO_STATUS_CAPTURE;
    }

    memset(echo_accum, 0, sizeof(echo_accum));

    for (uint16_t avg = 0; avg < averaging; avg++)
    {
        if (avg > 0U)
        {
            k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
        }

        // burst가 없으므로 ADC capture 자체의 간섭만 줄이기 위해 짧게 IRQ를 잠금
        unsigned int key = irq_lock();

        err = echo_adc_capture(echo_capture, num_samples);
        
        irq_unlock(key);

        if (err)
        {
            return ECHO_STATUS_CAPTURE;
        }

        for (uint16_t i = 0; i < num_samples; i++)
        {
            echo_accum[i] += echo_capture[i];
        }
    }

    for (uint16_t i = 0; i < num_samples; i++)
    {
        echo_capture[i] = (uint16_t)(echo_accum[i] / averaging);
    }

    return ECHO_STATUS_OK;
}