#include "laser.h"
#include "i2c.h"
#include <stdint.h>

#include "vl53l0x.h"
#include "tick.h"

#define ADDR_LASER 0x29  // VL53L0X
#define IDENTIFICATION_MODEL_ID            0xC0





unsigned char Laser_ReadReg(unsigned char reg)
{
    unsigned char val;
    I2C1_Write(ADDR_LASER, reg);
    I2C1_Read(ADDR_LASER, &val, 1);
    I2C1_Stop();
    return val;
}

unsigned char Laser_WriteReg(unsigned char reg, unsigned char reg2)
{
    unsigned char val;
    I2C1_Write(ADDR_LASER, reg);
    
    I2C1_Write(ADDR_LASER, reg2);
    I2C1_Stop();
    return val;
}

VL53L0X sensor;

bool Laser_Init_v2(void){
  I2C1_Init();
  bool fl = sensor.init();
  sensor.startContinuous();
  return fl;
}

bool isRangeReady()
{
  return sensor.isRangeReady();
}


uint16_t getRange()
{
  
  return sensor.readRange();
}