//
// Created by Professional on 7/31/2023.
//
#include "stdint-gcc.h"
#include "vl6180_def.h"
#include "stm32g0xx_hal.h"
#include "vl6180_api.h"
#include "main.h"
#include "i2c.h"

#define max_scale 1
uint8_t changeScale = 0;
#define ALLOW_DISABLE_WAF_FROM_BLUE_BUTTON 1
#define theVL6180Dev   0x52    // what we use as "API device
#define def_i2c_time_out 100
#if VL6180_HAVE_DMAX_RANGING
#define DMaxDispTime     0 /* Set to 1000 to display Dmax during 1 sec when no target is detected */
#else
#define DMaxDispTime     0
#endif
#define OutORangeDispfTime  800

#define g_TickCnt uwTick

enum runmode_t{
    RunRangePoll=0,
    RunAlsPoll,
    InitErr,
    ScaleSwap,
    WaitForReset,
    AlrmStart,
    AlrmRun,
    FromSwitch,
};

char buffer[10];

struct state_t {
    int OutofRAnge:1;
    int AutoScale:1;
    int FilterEn:1;
    uint8_t mode;
    int8_t ScaleSwapCnt;
    uint8_t InitScale;

    uint8_t CurAlrm;
    uint8_t AlrmFired; /* just used to keep display at least min time */
}State;

uint32_t TimeStarted;       /* various display and mode delay starting time */
VL6180_RangeData_t Range;  /* Range measurmeent  */

int alpha =(int)(0.85*(1<<16));    /* range distance running average cofs */
uint16_t range;             /* range average distance */

#define AutoThreshHigh  80  /*auto scale high thresh => AutoThreshHigh *  max_raneg => scale ++  */
#define AutoThreshLow   33  /*auto scale low thresh  => AutoThreshHigh *  max_raneg => scale ++  */
#define ErrRangeDispTime    0 /* Set to 800 ms to display error code when no target os detected */
#if ErrRangeDispTime == 0
/*   supress Warning[Pe186]: pointless comparison of unsigned integer with zero */
#   ifdef __ARMCC_VERSION /* ARM/KEIL */
#   pragma diag_suppress 186
#   endif  /* _ARMCC_VERSION */
#   ifdef __ICCARM__ /* IAR */
#       pragma diag_suppress=Pe186
#   endif  /* _ARMCC_VERSION */
#endif

#define AlrmDispTime        800

#define AlarmKeepDispTime   250  /*  alarm message retain time after it fires */
volatile int IntrFired=0;

extern volatile uint32_t SensorsEnabled;



void AbortErr( const char * msg ){
    State.mode=  WaitForReset;
}

void DoScalingSwap(int scaling){

    State.mode = ScaleSwap;
    TimeStarted=g_TickCnt;
}
uint8_t allowIT = 0;
void vl6180_init (void)
{
    allowIT = 0;
    HAL_GPIO_WritePin(CS_PROXIMITY_GPIO_Port, CS_PROXIMITY_Pin, GPIO_PIN_RESET);
    HAL_Delay(200);
    HAL_GPIO_WritePin(CS_PROXIMITY_GPIO_Port, CS_PROXIMITY_Pin, GPIO_PIN_SET);
    HAL_Delay(200);
    VL6180_WaitDeviceBooted(theVL6180Dev);
    VL6180_InitData(theVL6180Dev);
    State.InitScale=VL6180_UpscaleGetScaling(theVL6180Dev);
    State.FilterEn=VL6180_FilterGetState(theVL6180Dev);
    VL6180_Prepare(theVL6180Dev);
    VL6180_RangeSetMaxConvergenceTime(theVL6180Dev, 20);
    VL6180_RangeSetInterMeasPeriod(theVL6180Dev, 25);
    //State.InitScale=VL6180_UpscaleGetScaling(theVL6180Dev);
    /* Enable Dmax calculation only if value is displayed (to save computation power) */
    //VL6180_DMaxSetState(theVL6180Dev, DMaxDispTime>0);
    VL6180_ClearAllInterrupt(theVL6180Dev);
    VL6180_RangeStartContinuousMode(theVL6180Dev);
    allowIT = 1;
}


int VL6180_I2CRead(VL6180Dev_t addr, uint8_t  *buff, uint8_t len){
    int status;
    status = HAL_I2C_Master_Receive(&hi2c2,  addr, buff, len , def_i2c_time_out);
    if( status ){
        MX_I2C2_Init();
    }

    return status;
}

int VL6180_I2CWrite(VL6180Dev_t addr, uint8_t  *buff, uint8_t len){
    int status;
    status = HAL_I2C_Master_Transmit(&hi2c2,  addr, buff, len , def_i2c_time_out);
    if( status ){
        MX_I2C2_Init();
    }
    return status;
}


void RangeState(void) {
    int status;
    uint16_t hlimit;
    uint8_t scaling;
    VL6180_RangeGetMeasurement(theVL6180Dev, &Range );
    scaling = VL6180_UpscaleGetScaling(theVL6180Dev);
    if (Range.errorStatus) {
        AbortErr("Er r");
        return;
    }

    hlimit = VL6180_GetUpperLimit(theVL6180Dev);
    if (Range.range_mm >= (hlimit * AutoThreshHigh) / 100 && scaling < 3 && State.AutoScale) {
        VL6180_UpscaleSetScaling(theVL6180Dev, scaling + 1);
    }
    if (Range.range_mm < (hlimit * AutoThreshLow) / 100 && scaling > 1 && State.AutoScale) {
        VL6180_UpscaleSetScaling(theVL6180Dev, scaling - 1);
    }

    if (Range.errorStatus) {
        /* no valid ranging*/
        if (State.OutofRAnge) {
#if VL6180_HAVE_DMAX_RANGING
            if (g_TickCnt - TimeStarted >= ErrRangeDispTime &&  g_TickCnt - TimeStarted <  ErrRangeDispTime + DMaxDispTime ){
            }
            else

#endif
            if(g_TickCnt - TimeStarted < ErrRangeDispTime  )
            {

            }
            else{
                State.OutofRAnge=0; /* back to out of range display */
                TimeStarted=g_TickCnt;
            }
        }
        else {
            int FilterEn;
#if VL6180_WRAP_AROUND_FILTER_SUPPORT
            FilterEn = VL6180_FilterGetState(theVL6180Dev);
#else
#endif
            if( g_TickCnt - TimeStarted > OutORangeDispfTime ) {
                State.OutofRAnge = 1;
                TimeStarted = g_TickCnt;
            }
        }
    }
    else {
        State.OutofRAnge = 0;
        TimeStarted = g_TickCnt;
        range = (range * alpha + Range.range_mm * ((1 << 16) - alpha)) >> 16;
        if (State.AutoScale) {
            if (scaling == 1) {
                buffer[0] = '_';
            }
            else
            if (scaling == 2)
                buffer[0] = '=';
            else
                buffer[0] = '~';
        }

    }

    if (changeScale == 1) {
        changeScale = 0;
        TimeStarted = g_TickCnt;

        State.ScaleSwapCnt++;
        if (State.ScaleSwapCnt % (max_scale + 1) == max_scale) {
            State.AutoScale = 1;
            scaling = max_scale;
        }
        else {
#if ALLOW_DISABLE_WAF_FROM_BLUE_BUTTON
            /* togle filtering every time we roll over all scaling(pass by autoscale) */
            if (State.AutoScale)
                State.FilterEn = !State.FilterEn;
#endif
            State.AutoScale = 0;
            scaling = State.InitScale + (State.ScaleSwapCnt % max_scale);
            if (scaling > max_scale)
                scaling = scaling - (max_scale);
        }

        status = VL6180_UpscaleSetScaling(theVL6180Dev, scaling);
        if (status<0) {
            AbortErr("ErUp");
            State.mode = InitErr;
        }
        else {
            /* do not check status may fail when filter support not active */
            VL6180_FilterSetState(theVL6180Dev, State.FilterEn);
            DoScalingSwap(scaling);
        }
    }
}
uint32_t resetPressedMs = 0;
uint8_t motionDetected = 0;
__weak void proximityMessureCompleted (void)
{

}

uint8_t getRange (VL6180_RangeData_t* range)
{
    if (Range.errorStatus == 0)
    {
        *range = Range;
        return 0;
    }
    else
    {
        range->range_mm = -1;
        return 1;
    }
}
void HAL_GPIO_EXTI_Rising_Callback(uint16_t GPIO_Pin)
{
    if (GPIO_Pin == GPIO_PIN_0 && allowIT == 1)
    {
        RangeState();
        proximityMessureCompleted();
        VL6180_ClearAllInterrupt(theVL6180Dev);
    }
    else if (GPIO_Pin == GPIO_PIN_1)
    {
        uint32_t res = uwTick - resetPressedMs;
        if (res > 2000 && resetPressedMs > 0 )
            HAL_NVIC_SystemReset();
        resetPressedMs = 0;
    }
}

void HAL_GPIO_EXTI_Falling_Callback(uint16_t GPIO_Pin)
{

    if (GPIO_Pin == GPIO_PIN_1)
    {
        if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_1) == GPIO_PIN_RESET)
        {
            resetPressedMs = uwTick;
        }
    }
}
/* USER CODE END 4 */