/**
 * @file ads1015_driver.c
 * @brief Implementation of the ADS1015 I2C ADC driver.
 *
 * This driver provides functions to configure and read from the ADS1015
 * 12-bit, 128 SPS I2C ADC. The device supports up to four single-ended
 * inputs or two differential inputs.
 */

#include "ads1015_driver.h"

ADS1015_I2C i2c_ads1015;

ADS1015_filter_t g_ph_filter = {0};

volatile ADS1015_State_t ads1015_state = ADS1015_IDLE;

// Write the register
static void writeRegister(ADS1015_I2C *i2c, uint8_t reg, uint16_t value) {
	uint8_t pData[3] = { reg, (uint8_t) (value >> 8), (uint8_t) (value & 0xFF) };
	HAL_I2C_Master_Transmit(i2c->hi2c, i2c->m_i2cAddress, pData, 3, 10);
}

// Read the register
static uint16_t readRegister(ADS1015_I2C *i2c, uint8_t reg) {
	HAL_I2C_Master_Transmit(i2c->hi2c, i2c->m_i2cAddress, &reg, 1, 10);
	uint8_t pData[2] = { 0, 0 };
	HAL_I2C_Master_Receive(i2c->hi2c, i2c->m_i2cAddress, pData, 2, 10);
	return ((pData[0] << 8) | pData[1]);
}


/*
 * // The ADC input range (or gain) can be changed via the following
 // functions, but be careful never to exceed VDD +0.3V max, or to
 // exceed the upper and lower limits if you adjust the input range!
 // Setting these values incorrectly may destroy your ADC!
 //                                                                ADS1015  ADS1115
 //                                                                -------  -------
 // ADSsetGain(GAIN_TWOTHIRDS);  // 2/3x gain +/- 6.144V  1 bit = 3mV      0.1875mV (default)
 // ADSsetGain(GAIN_ONE);        // 1x gain   +/- 4.096V  1 bit = 2mV      0.125mV
 // ADSsetGain(GAIN_TWO);        // 2x gain   +/- 2.048V  1 bit = 1mV      0.0625mV
 // ADSsetGain(GAIN_FOUR);       // 4x gain   +/- 1.024V  1 bit = 0.5mV    0.03125mV
 // ADSsetGain(GAIN_EIGHT);      // 8x gain   +/- 0.512V  1 bit = 0.25mV   0.015625mV
 // ADSsetGain(GAIN_SIXTEEN);    // 16x gain  +/- 0.256V  1 bit = 0.125mV  0.0078125mV
 */
// Declare an ADS1015 structure
void ADS1015_Init(void)
{
	i2c_ads1015.hi2c = &hi2c1;
	i2c_ads1015.m_i2cAddress = ADS1015_ADDR_GND; //  It's Important to shift the address << 1
	i2c_ads1015.m_conversionDelay = ADS1015_CONVERSIONDELAY;
	i2c_ads1015.m_bitShift = 4;
	i2c_ads1015.m_gain = GAIN_FOUR; /* 4x gain   +/- 1.024V  1 bit = 0.5mV    0.03125mV */
}


/**
 * @brief Converte um valor float em uma escala de FLOAT_MIN a FLOAT_MAX para uint16_t em escala de UINT16_MIN a UINT16_MAX
 *
 * @param value Valor float entre FLOAT_MIN e FLOAT_MAX
 *
 * @return uint16_t Valor convertido na escala de UINT16_MIN a UINT16_MAX
 */
uint16_t ADS1015_Compress_To_IntScale(float value)
{
    // Verifica limites do valor de entrada
    if (value < ADS1015_PH_FLOAT_SCALE_MIN)
          return ADS1015_PH_OUT_SCALE_MIN;
    else if (value > ADS1015_PH_FLOAT_SCALE_MAX)
          return ADS1015_PH_OUT_SCALE_MAX;

    // Converte o valor usando a fórmula: ((value - FLOAT_MIN) / (FLOAT_MAX - FLOAT_MIN)) * (UINT16_MAX - UINT16_MIN) + UINT16_MIN
    uint16_t result = (uint16_t)(((value - ADS1015_PH_FLOAT_SCALE_MIN) / (ADS1015_PH_FLOAT_SCALE_MAX - ADS1015_PH_FLOAT_SCALE_MIN)) * ((float)ADS1015_PH_OUT_SCALE_MAX - (float)ADS1015_PH_OUT_SCALE_MIN) + (float)ADS1015_PH_OUT_SCALE_MIN);

    // Garante que o resultado não exceda o limite superior
    if (result > ADS1015_PH_OUT_SCALE_MAX)
         return (ADS1015_PH_OUT_SCALE_MAX);

    return result;
}

void ADS1015_Process_System(void)
{
    static uint32_t state_timer = 0;
    static uint32_t sample_count = 0;
    uint16_t config_reg = 0;

    switch (ads1015_state)
    {
        case ADS1015_IDLE:
            sample_count = 0;
            ads1015_state = ADS1015_START_CONVERSION;
            break;

        case ADS1015_START_CONVERSION:
            state_timer = g_ms_counter;

            /* Escreve no Config Register (P[1:0]=01b) com OS=1 para
             * iniciar a conversão single-shot. O bit OS só tem efeito
             * quando o dispositivo está em power-down (seção 7.4.2.1) */
        	// Write config register to the ADC
        	writeRegister(&i2c_ads1015, ADS1015_REG_POINTER_CONFIG, ADS1015_CONFIG_START_SINGLE);

        	ads1015_state = ADS1015_WAIT_CONVERSION;
            break;

        case ADS1015_WAIT_CONVERSION:
            /* Poll do bit OS no Config Register (P[1:0]=01b):
             * Ao LER o bit OS:
             *   0b = dispositivo ainda convertendo
             *   1b = conversão concluída, dado pronto no Conversion Register
             * (Tabela 8-4, descrição do bit OS)
             */

            config_reg = readRegister(&i2c_ads1015, ADS1015_REG_POINTER_CONFIG);

            if (((config_reg & ADS1015_REG_CONFIG_OS_SINGLE) != 0) && (g_ms_counter - state_timer)>ADS1015_TIME_PER_SAMPLE)
            {
                ads1015_state = ADS1015_READ_DATA;
            }
            // (opcional) tratar timeout comparando g_ms_counter - state_timer
            // Datasheet: conversão single-cycle, tempo = 1/DR (ex: 1600SPS ≈ 625µs)
            break;

        case ADS1015_READ_DATA:
            {
                /* Muda o Address Pointer para o Conversion Register (P[1:0]=00b)
                 * e lê os 16 bits. Dado é 12 bits em complemento de 2,
                 * left-justified em D[15:4]; D[3:0] sempre lê 0h (Tabela 8-3) */
                int16_t raw = (int16_t)readRegister(&i2c_ads1015, ADS1015_REG_POINTER_CONVERT);
                raw >>= 4;   // remove os 4 bits reservados (D[3:0]), resultado = 12 bits signed

                float value = (float)(2048 - raw); // ((float)raw * ADS1015_ADC_VREF) / ADS1015_ADC_MAX; // usa FSR configurado no PGA

                ADS1015_pH_NewSample(value);

                sample_count++;

                if (sample_count >= ADS1015_BURST_SIZE)
                {
                    ads1015_state = ADS1015_IDLE;
                }
                else
                {
                    state_timer = g_ms_counter;
                    ads1015_state = ADS1015_WAIT_NEXT_SAMPLE;
                }
            }
            break;

        case ADS1015_WAIT_NEXT_SAMPLE:
            if ((g_ms_counter - state_timer) >= ADS1015_TIME_PER_SAMPLE)
            {
                ads1015_state = ADS1015_START_CONVERSION;
            }
            break;
    }
}


/**
* Simple insertion sort - BURST_SIZE is small, so the cost is negligible
* and it avoids depending on qsort/heap allocation.
**/
void ADS1015_Sort_Array(float *v, uint8_t n)
{
    for (uint8_t i = 1; i < n; i++)
    {
        float key = v[i];
        int8_t j = (int8_t)i - 1;
        while (j >= 0 && v[j] > key)
        {
            v[j + 1] = v[j];
            j--;
        }
        v[j + 1] = key;
    }
}

/**
 * Trimmed mean: sorts the buffer IN PLACE (the caller's burst buffer is
 * about to be reset right after this call anyway, so preserving its
 * original order is not needed), discards TRIM_COUNT values from each
 * end, and averages the remainder. Sorting in place avoids a temporary
 * array and removes the need for memcpy()/string.h entirely.
 **/
float ADS1015_Trimmed_Mean(float *buffer, uint8_t n, uint8_t trim)
{
    ADS1015_Sort_Array(buffer, n);

    uint8_t start = trim;
    uint8_t end   = n - trim; /* exclusive */

    if (end <= start) /* guard against a misconfigured trim value */
    {
        start = 0;
        end   = n;
    }

    float sum = 0.0f;
    uint8_t count = 0;
    for (uint8_t i = start; i < end; i++)
    {
        sum += buffer[i];
        count++;
    }
    return sum / (float)count;
}

/*
 *  Simple average of the history buffer (stage-2 sliding window)
 */
float ADS1015_History_Average(const float *hist, uint8_t n)
{
    float sum = 0.0f;
    for (uint8_t i = 0; i < n; i++)
        sum += hist[i];
    return sum / (float)n;
}


/*
 * Common processing core: applied identically to all 3 channels.
 * Receives the current raw reading and the channel's state (global
 * arrays/struct).
 **/
void ADS1015_Process_Sample(ADS1015_filter_t *ch, float raw)
{
    /* --- Stage 1: accumulate into the current burst --- */
    ch->burst[ch->burst_index] = raw;
    ch->burst_index++;

    if (ch->burst_index < ADS1015_BURST_SIZE)
        return; /* still collecting the burst, nothing else to do */

    /* Burst complete: compute the trimmed mean */
    float burst_result = ADS1015_Trimmed_Mean(ch->burst, ADS1015_BURST_SIZE, ADS1015_TRIM_COUNT);
    ch->burst_index = 0; /* reset for the next burst */

    /* --- Stage 2a: rolling history / sliding window --- */
    ch->history[ch->history_index] = burst_result;
    ch->history_index = (uint8_t)((ch->history_index + 1) % ADS1015_HISTORY_SIZE);
    if (ch->history_count < ADS1015_HISTORY_SIZE)
        ch->history_count++;

    float window_average = ADS1015_History_Average(ch->history, ch->history_count);

    /* --- Stage 2b: first-order IIR low-pass filter on the burst result --- */
    if (!ch->iir_initialized)
    {
        ch->iir_state = burst_result;
        ch->iir_initialized = 1;
    }
    else
    {
        ch->iir_state = ADS1015_IIR_ALPHA * burst_result + (1.0f - ADS1015_IIR_ALPHA) * ch->iir_state;
    }

    /* Final output: combines the IIR state (fast response to real drift)
     * with the window average (more stable, slower response). Adjust
     * the weighting per channel if needed. */
    ch->filtered_value = 0.7f * ch->iir_state + 0.3f * window_average;
    ch->value_valid = 1;
}


/* ---------------------------------------------------------------------- */
/*                   PUBLIC FUNCTIONS - ONE PER CHANNEL                   */
/* ---------------------------------------------------------------------- */


void ADS1015_pH_NewSample(float raw)
{
	ADS1015_Process_Sample(&g_ph_filter, raw);
}







