#include "app/vbat/vbat.h"
#include "drivers/adc/adc.h"
#include "core/gpio.h"
#include "core/main.h" // SystemClock_HSI
#include "core/config_pins.h"
#include "core/csect.h"

 static bool VBat_Init();
 static void VBat_DeInit();
 static bool VBat_Serve();
 static uint32_t VBat_GetVoltage();
 static void VBat_SetInterval(size_t ms);
 static void VBat_IC_Enable();
 static void VBat_IC_Disable();

 #define pADCHandle (&ADC1Handle)
 #define VBAT_ADC_RESOLUTION    eADCRes_12bit
                        
 static bool bVBATInitialized = false;
 static uint32_t VBatMontor_LastTick = 0;
 static uint32_t VBatMontor_Interval = VBAT_MONITOR_INTERVAL; 
 static uint32_t g_VBatVoltage = VBAT_INVALID_VALUE;
 static uint32_t g_VDDAVoltage = VBAT_INVALID_VALUE;

 #if VBAT_AVERAGING_POINTS > 0
 static uint32_t aVBatVoltage_Avg[ VBAT_AVERAGING_POINTS ] = {0};
 static size_t VBatVoltageAvgIndex = 1; // VBatVoltageAvgIndex=1 means the averaging mode is active since startup
 #if VBAT_AVERAGING_POINTS > 3
 #include "stdlib.h" //qsort
 #endif
 #endif

 const sVBat_Handle_t VBatHandle = {

   .Init = VBat_Init,
   .SetMeasureInterval = VBat_SetInterval,
   .ServeMonitor = VBat_Serve,
   .GetVoltage = VBat_GetVoltage,
   .DeInit = VBat_DeInit,
 };

 static bool VBat_Init()
 {
    bool bRet = false;

    __DI__ bRet = bVBATInitialized; __EI__

    if( !bRet )                        
    {
        if( SystemClock_HSI( true ) )
        {
            if( pADCHandle->Init( VBAT_ADC_RESOLUTION ) )
            {
                VBat_IC_Disable();

                {
                    GPIO_InitTypeDef GPIO_InitStruct = {0};

                    // Configure the pin muxing
                    GPIO_InitStruct.Pin = CONFIG_PIN__VBAT_ADC;  
                    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
                    GPIO_InitStruct.Pull = GPIO_NOPULL;
                    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;        

                    HAL_GPIO_Init(CONFIG_PORT__VBAT_ADC, &GPIO_InitStruct);
                }
                {
                    GPIO_InitTypeDef GPIO_InitStruct = {0};

                    // Configure the pin muxing
                    GPIO_InitStruct.Pin = CONFIG_PIN__VBAT_EN;  
                    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
                    GPIO_InitStruct.Pull = GPIO_NOPULL;
                    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;        

                    HAL_GPIO_Init(CONFIG_PORT__VBAT_EN, &GPIO_InitStruct);
                }

                #if VBAT_INIT_FOR_MEASURE == 0
                __DI__ 
                  g_VBatVoltage = VBAT_INVALID_VALUE; 
                  #if VBAT_AVERAGING_POINTS > 0
                  for( size_t i = 0; i < VBAT_AVERAGING_POINTS; ++i )
                  { aVBatVoltage_Avg[ i ] = 0; }
                  VBatVoltageAvgIndex = 1; // VBatVoltageAvgIndex=1 means the averaging mode is active since startup
                  #endif
                __EI__
                #endif

                bRet = true;
            }
        }
    }

    if( !bRet )
    {
        VBat_DeInit();

        SystemClock_HSI( false );
    }

    __DI__ bVBATInitialized = bRet; __EI__

    return bRet;
 }

 static void VBat_DeInit()
 {
    DI();

    if( !bVBATInitialized )
    {
        EI();
        return;
    }

    EI();

    VBat_IC_Disable();

    {
        GPIO_InitTypeDef GPIO_InitStruct = {0};

        // Configure the pin muxing
        GPIO_InitStruct.Pin = CONFIG_PIN__VBAT_ADC;  
        GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
        GPIO_InitStruct.Pull = GPIO_NOPULL;
        GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;        

        HAL_GPIO_Init(CONFIG_PORT__VBAT_ADC, &GPIO_InitStruct);
    }
    {
        GPIO_InitTypeDef GPIO_InitStruct = {0};

        // Configure the pin muxing
        GPIO_InitStruct.Pin = CONFIG_PIN__VBAT_EN;  
        GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
        GPIO_InitStruct.Pull = GPIO_PULLDOWN;
        GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;        

        HAL_GPIO_Init(CONFIG_PORT__VBAT_EN, &GPIO_InitStruct);
    }

    pADCHandle->Stop( CONFIG_ADC__VBAT_ADC );

    pADCHandle->DeInit();

    SystemClock_HSI( false );

    __DI__ bVBATInitialized = false; __EI__
 }

 static void VBat_SetInterval(uint32_t ms)
 {
     __DI__ VBatMontor_Interval = ms; __EI__ 
 }

 // VBat_IC_Enable
 // Enable external battery sensing circuit (MOSFET)
 static void VBat_IC_Enable()
 {
    HAL_GPIO_WritePin( CONFIG_PORT__VBAT_EN, CONFIG_PIN__VBAT_EN, GPIO_PIN_SET );

    // wait until transition process completes in external RC-circuit
    HAL_Wait_us( 100 );
 }

 // VBat_IC_Disable
 // Disable external battery sensing circuit (MOSFET) 
 static void VBat_IC_Disable()
 {
    HAL_GPIO_WritePin( CONFIG_PORT__VBAT_EN, CONFIG_PIN__VBAT_EN, GPIO_PIN_RESET );
 }

 #if VBAT_AVERAGING_POINTS == 3
 uint16_t middle_of_3(uint16_t a, uint16_t b, uint16_t c)
 {
    uint16_t middle;
 
    if ((a <= b) && (a <= c)){
       middle = (b <= c) ? b : c;
    }
    else{
       if ((b <= a) && (b <= c)){
          middle = (a <= c) ? a : c;
       }
       else{
          middle = (a <= b) ? a : b;
       }
    }
 
    return middle;
 }
 #endif
 #if VBAT_AVERAGING_POINTS > 3
 static int qsort_compare( const uint32_t * i, const uint32_t * j )
 {
    return ((*i > *j)?1:( (*i < *j)?-1:0 ));
 }
 #endif

 // VBat_Serve()
 // VBat: Battery Voltage sensing measurement
 // Implements the battery voltage sensing using periodical measurements.
 // Take a look on the "picture":
 //
 // |<--------- T ---------->|
 // |                        |
 // *------------------------|-|-|---------------------------|-|-|------------....--> time
 // ^ Startup                Take several measurements       Take several measurements
 // Function takes several measurements each interval T (@VBatMontor_Interval) - package of measurements
 // During performing these several measurements the function peforms median filtering.
 // Variable @VBatVoltageAvgIndex is responsible for indexing each measurement in the package.
 static bool VBat_Serve()
 {
    bool bServe = false;
    bool bUpdated = false;
    // Is averaging enabled?
    #if VBAT_AVERAGING_POINTS > 0
      // averaging enabled
      __DI__
      #if VBAT_INIT_FOR_MEASURE == 0
      if( bVBATInitialized )
      {
      #endif
          // Averaging is active if VBatVoltageAvgIndex>0 and VBatVoltageAvgIndex <= VBAT_AVERAGING_POINTS
          if( (VBatVoltageAvgIndex > 0) && (VBatVoltageAvgIndex <= VBAT_AVERAGING_POINTS) )
          {  
              // averaging interval 
              // Is @VBAT_AVERAGING_INTERVAL timeout ran out?
              bServe = (HAL_GetTick() - VBatMontor_LastTick > VBAT_AVERAGING_INTERVAL );
          }
          else
          {
              // measurement interval (control interval)
              // Is @VBatMontor_LastTick timeout ran out?
              bServe = (HAL_GetTick() - VBatMontor_LastTick > VBatMontor_Interval);
          }
      #if VBAT_INIT_FOR_MEASURE == 0
      }
      #endif
      __EI__
    #else
      // averaging disabled
      #if VBAT_INIT_FOR_MEASURE
      __DI__ bServe = (HAL_GetTick() - VBatMontor_LastTick > VBatMontor_Interval); __EI__
      #else
      __DI__ bServe = bVBATInitialized && (HAL_GetTick() - VBatMontor_LastTick > VBatMontor_Interval); __EI__
      #endif
    #endif

    if( bServe )
    {
       #if VBAT_INIT_FOR_MEASURE
       if( VBat_Init() )
       {
       #endif

           VBat_IC_Enable();

           bool bResult = false;

           uint32_t xVDDAVoltage = 0;   
           uint32_t xVBatVoltage = 0;   
           uint32_t xVDDAVoltage_ADC = VBAT_INVALID_VALUE;           
           uint32_t xVBatVoltage_ADC = VBAT_INVALID_VALUE;
           // To measure actual voltage it is required 
           // to measure actual power supply voltage using ADC_CHANNEL_VREFINT
           if( pADCHandle->Start( ADC_CHANNEL_VREFINT ) )
           {               
               if( pADCHandle->Measure( VBAT_MONITOR_MEAS_TIMEOUT, &xVDDAVoltage_ADC ) )
               {
                   if( pADCHandle->Start( CONFIG_ADC__VBAT_ADC ) )
                   {
                       if( pADCHandle->Measure( VBAT_MONITOR_MEAS_TIMEOUT, &xVBatVoltage_ADC ) )
                       {
                           // Calculate VDDA voltage first
                           xVDDAVoltage = (ADC_VREF_VOLTAGE_mV * ADC_GetFullScale( VBAT_ADC_RESOLUTION ) / xVDDAVoltage_ADC);

                           // Find out the target voltage using VDDA:
                           xVBatVoltage = xVDDAVoltage * xVBatVoltage_ADC / ADC_GetFullScale( VBAT_ADC_RESOLUTION );

                           bResult = true;
                       }
                   }
               }
           }

           __DI__           
           #if VBAT_AVERAGING_POINTS > 0
           #if VBAT_AVERAGING_POINTS < 3
           #error Invalid VBAT_AVERAGING_POINTS value: the value greater than 2 is required for meadian filter
           #endif
           #if VBAT_AVERAGING_POINTS % 2 == 0
           #error Invalid VBAT_AVERAGING_POINTS value: an odd value is required for meadian filter
           #endif
           if( bResult ) 
           {
               // The program reach this point due to the following reasons:
               //  - in interval mode: @VBatMontor_Interval timeout expires;
               //      so it is required to activate averaging mode and collect samples.
               //  - in averaging mode: @VBAT_AVERAGING_INTERVAL timeout expires;
               //      so it is required to collect current sample and wait for the next one. 
               if( VBatVoltageAvgIndex == 0 )
               {
                   // so, it is required to switch to averaging mode if it is not active
                   VBatVoltageAvgIndex = 1;
               }

               // update VDDA voltage without averaging
               g_VDDAVoltage = xVDDAVoltage;

               // collecting samples
               aVBatVoltage_Avg[ VBatVoltageAvgIndex - 1 ] = xVBatVoltage;

               // take the sample in account, shift to the next filter cell
               VBatVoltageAvgIndex++;

               // are filter cells full?
               if( VBatVoltageAvgIndex > VBAT_AVERAGING_POINTS )
               {
                   VBatVoltageAvgIndex = 0; // disable averaging, switch to the interval mode
                   bUpdated = true;
                   #if VBAT_AVERAGING_POINTS == 3
                   g_VBatVoltage = middle_of_3( aVBatVoltage_Avg[0], aVBatVoltage_Avg[1], aVBatVoltage_Avg[2] );
                   #elif VBAT_AVERAGING_POINTS > 3
                   #warning Uneffective sorting, you sould use VBAT_AVERAGING_POINTS = 3
                   qsort(   aVBatVoltage_Avg, 
                            VBAT_AVERAGING_POINTS, 
                            sizeof(aVBatVoltage_Avg[0]), 
                            (int(*) (const void *, const void *))qsort_compare  );

                   g_VBatVoltage = aVBatVoltage_Avg[ ((VBAT_AVERAGING_POINTS - 1U) / 2U) ];                   
                   #else
                   #error Invalid VBAT_AVERAGING_POINTS value
                   #endif
               }
           }
           else
           {
               g_VDDAVoltage = VBAT_INVALID_VALUE;
               g_VBatVoltage = VBAT_INVALID_VALUE;
           }
           #else
           if( bResult )
           {
                g_VDDAVoltage = xVDDAVoltage;
                g_VBatVoltage = xVBatVoltage;                 
                bUpdated = true;
           }
           else
           {  
                g_VDDAVoltage = VBAT_INVALID_VALUE;
                g_VBatVoltage = VBAT_INVALID_VALUE;
           }
           #endif           
           VBatMontor_LastTick = HAL_GetTick();
           __EI__

           VBat_IC_Disable();

       #if VBAT_INIT_FOR_MEASURE
         VBat_DeInit();
       }
       else
       {
           __DI__ g_VBatVoltage = VBAT_INVALID_VALUE; __EI__
       }
       #endif
    }
    (void)g_VDDAVoltage;
    return bUpdated;
 }

 static uint32_t VBat_GetVoltage()
 {  
    uint32_t xValue;
    #if VBAT_VOLTAGE_MULTIPLIER > 0 || VBAT_VOLTAGE_DIVIDER > 0
      #if VBAT_VOLTAGE_MULTIPLIER == 2 && VBAT_VOLTAGE_DIVIDER < 2
        __DI__ xValue = (g_VBatVoltage << 1); __EI__
      #else
        __DI__ xValue = g_VBatVoltage; __EI__
        #if VBAT_VOLTAGE_MULTIPLIER > 0
        xValue *= VBAT_VOLTAGE_MULTIPLIER;
        #endif
        #if VBAT_VOLTAGE_DIVIDER > 0
        xValue /= VBAT_VOLTAGE_DIVIDER;
        #endif
      #endif
    #else
    __DI__ xValue = g_VBatVoltage; __EI__
    #endif
    return xValue;
 }