#include "stm32l1xx_hal.h"
#include "app/thermo/tsensor_ll.h"
#include "app/thermo/tsensor.h"
#include "core/csect.h"
#include "core/config.h"

// THERMO_SENSOR_KEEP_INITIALIZED
// Do not turn off (=1) the sensor periferal driver btw measures
#define THERMO_SENSOR_KEEP_INITIALIZED  1

// THERMO_SENSOR_AVG_WINDOW
// Digital exp-filter window size
#define THERMO_SENSOR_AVG_WINDOW        24   

// THERMO_SENSOR_INTERVAL
// Temperature measuring interval, ms
#define THERMO_SENSOR_INTERVAL          CONFIG_TSENSOR_INTERVAL   

static tTSensorTemp vThermoSensor_AVGTemp = THERMO_SENSOR_INVALID_TEMP_VALUE;
static size_t vThermoSensor_AVGPoints = 0;
static uint32_t vThermoSensor_LastTick = 0;
static uint32_t vThermoSensor_Interval = THERMO_SENSOR_INTERVAL;

#if THERMO_SENSOR_KEEP_INITIALIZED
static bool vThermoSensor_bPowered = false;
#endif

static void ThermoSensor_UpdateAVGTemp( tTSensorTemp new_value ) ;   
static void ThermoSensor_ResetAvgTemp();
static bool ThermoSensor_Serve();
static bool ThermoSensor_GetReady();
static tTSensorTemp ThermoSensor_GetAvgTemp();
static void ThermoSensor_SetInterval(size_t ms);

const sThermoSensorHandle_t ThermoSensor = {
    .GetReady = ThermoSensor_GetReady,
    .ResetTempAVG = ThermoSensor_ResetAvgTemp,
    .ServeSensor = ThermoSensor_Serve,
    .GetTempAVG = ThermoSensor_GetAvgTemp,
    .SetSensorInterval = ThermoSensor_SetInterval
};

static bool ThermoSensor_GetReady()
{
   bool bReady = false;

   __DI__
         bReady =  ( vThermoSensor_AVGPoints > 0 ) 
                && ( vThermoSensor_AVGTemp != THERMO_SENSOR_INVALID_TEMP_VALUE );
   __EI__

  return bReady;
}

static void ThermoSensor_SetInterval(uint32_t ms)
{
    __DI__ vThermoSensor_Interval = ms; __EI__ 
}

static void ThermoSensor_ResetAvgTemp()
{
    DI();
    vThermoSensor_AVGTemp = THERMO_SENSOR_INVALID_TEMP_VALUE;
    vThermoSensor_AVGPoints = 0;
    EI();
}

static tTSensorTemp ThermoSensor_GetAvgTemp()
{
    tTSensorTemp value;
    __DI__ value = vThermoSensor_AVGTemp; __EI__
    return value;
}

static bool ThermoSensor_Serve()
{
    bool bServe = false;

    __DI__ bServe = (HAL_GetTick() - vThermoSensor_LastTick > vThermoSensor_Interval); __EI__

    if( bServe )
    {
        #if THERMO_SENSOR_KEEP_INITIALIZED
        if( !vThermoSensor_bPowered )
        {
            vThermoSensor_bPowered = I2CTempSensor.Init();
        }

        if( vThermoSensor_bPowered )
        {
            tTSensorTemp temp_value;
            if( I2CTempSensor.GetTemp( &temp_value ) )
            {            
                ThermoSensor_UpdateAVGTemp( temp_value );
            }
            else
            {
                ThermoSensor_ResetAvgTemp();
            }
        }
        #else
        if( I2CTempSensor.Init() )
        {
            tTSensorTemp temp_value;
            if( I2CTempSensor.GetTemp( &temp_value ) )
            {            
                ThermoSensor_UpdateAVGTemp( temp_value );
            }
            else
            {
                ThermoSensor_ResetAvgTemp();
            }
            
            I2CTempSensor.DeInit();         
        }
        else
        {
            ThermoSensor_ResetAvgTemp();
        }
        #endif

        __DI__ vThermoSensor_LastTick = HAL_GetTick(); __EI__
    }

    return bServe;
}

// ThermoSensor_UpdateAVGTemp
// Performs averaging with exponential digital filter
static void ThermoSensor_UpdateAVGTemp( tTSensorTemp new_value ) 
{
    DI();
    if( (THERMO_SENSOR_INVALID_TEMP_VALUE) == vThermoSensor_AVGTemp )
    {
        vThermoSensor_AVGTemp = new_value;
        vThermoSensor_AVGPoints = 1;
    }
    else
    {
        vThermoSensor_AVGTemp = ( new_value + ( vThermoSensor_AVGTemp * vThermoSensor_AVGPoints ) ) / ( vThermoSensor_AVGPoints + 1 );

        if( vThermoSensor_AVGPoints < (THERMO_SENSOR_AVG_WINDOW) )
            vThermoSensor_AVGPoints++;    
        
        // Now @tmp is 16-bit value: [BBBBBBBBBB000000]
        // So, lets perform averaging: 
        //           n * AVG[i-1] + tmp
        //  AVG[i] = -------------------  
        //              n+1
        // where
        //  @AVG[i-1] is averaged temperature before operation,
        //  @AVG[i] is averaged temperature after operation,
        //  @n is the filter window size (=24),
        //  @tmp is momentary measured temperature, [BBBBBBBBBB000000].
        //
        // So after averaging, bits 5..0 will be filled up.
        // Since the sensor has a 0.25*C accuracy, the @tmp should be divided
        // by 4 to get the value in degrees. But after averaging the whole accuracy
        // is changed due to the bits 5..0 are filled up. So, we need to divide
        // the value not 4, but 256 times to get value in degrees:
        //  /4  - sensor accuracy
        //  /64 - averaging accuracy (6 bits, 2^6=64)
        // So, 4*64 = /256 of degree.
    }
    EI();
    
}