#include "vl53l0x.h"

#include "i2c.h"

#define CHECK_STATUS() if(status != VL53L0X_ERROR_NONE) return status

namespace Laser
{
    VL53L0X_Dev_t device;
    
    VL53L0X_Error Init()
    {
        static I2c wire(GPIOB, GPIO_Pin_10, GPIO_Pin_11);
        device.wire = (void*) &wire;
        device.I2cDevAddr = DEFAULT_ADRESS;
        device.Present = 0;
        device.Id = 0;
        
        VL53L0X_Error status = VL53L0X_ERROR_NONE;
        
        uint32_t refSpadCount = 0;
        uint8_t isApertureSpads = 0;
        uint8_t VhvSettings = 0;
        uint8_t PhaseCal = 0;
        
        status = VL53L0X_DataInit( &device); //vl53l0x initialisation and calibration
        CHECK_STATUS();
    
        status = VL53L0X_StaticInit( &device);
        CHECK_STATUS();
    
        status = VL53L0X_PerformRefSpadManagement( &device, &refSpadCount, &isApertureSpads);
        CHECK_STATUS();

        status = VL53L0X_PerformRefCalibration( &device, &VhvSettings, &PhaseCal);
        CHECK_STATUS();

        status = VL53L0X_SetReferenceSpads( &device, refSpadCount, isApertureSpads);
        CHECK_STATUS();

        status = VL53L0X_SetRefCalibration( &device, VhvSettings, PhaseCal);
        CHECK_STATUS();

        status = VL53L0X_SetDeviceMode( &device, VL53L0X_DEVICEMODE_SINGLE_RANGING);
        CHECK_STATUS();

        status = VL53L0X_SetLimitCheckValue( &device, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, (FixPoint1616_t)(0.25*65536) );
        CHECK_STATUS();

        status = VL53L0X_SetLimitCheckValue( &device, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, (FixPoint1616_t)(32*65536) );
        CHECK_STATUS();

        status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds( &device, 20000 );
        
        return status;
    }
    
    uint16_t read()
    {
        VL53L0X_RangingMeasurementData_t data;
        VL53L0X_Error status = VL53L0X_ERROR_NONE;
        
        status = VL53L0X_PerformSingleRangingMeasurement( &device, &data); //read range from vl53l0x
        if(status != VL53L0X_ERROR_NONE) return 0;
        
        status = VL53L0X_ClearInterruptMask( &device, 0);
        if(status != VL53L0X_ERROR_NONE) return 0;
        
        return data.RangeMilliMeter; //return distance in millimeters
    }
}
