/*
 * VL6180X Demo
 * Ref: http://www.st.com/st-web-ui/static/active/en/resource/technical/document/application_note/DM00122600.pdf
 */
//Importation des bibliothèques
#include "TimerOne.h"  
#include <Wire.h>       // Bibliothèque pour la connection I²C


//Définition des Ports 
#define  DIGIT1  A0  // Rightmost digit (Active LOW)
#define  DIGIT2  A1
#define  DIGIT3  A2
#define  DIGIT4  A3  // Leftmost digit

#define  SEG_A   10  // Active LOW
#define  SEG_B   9
#define  SEG_C   8
#define  SEG_D   7
#define  SEG_E   6
#define  SEG_F   5
#define  SEG_G   4
#define  SEG_DP  3

#define  CHIP_EN 13  // Active HIGH

#define VL6180X_ADDRESS 0x29     //  ***Device address of VL6180X  ----> I²C*** registre 0x212???

unsigned char outPins[] = {DIGIT1, DIGIT2, DIGIT3, DIGIT4, SEG_A, SEG_B, SEG_C, SEG_D, SEG_E, SEG_F, SEG_G, SEG_DP};
unsigned char sDecode[] = {0x3F, 0x06, 0x5B, 0x4F, 0x66, 0x6D, 0x7D, 0x07, 0x7F, 0x6F};
unsigned char digits[4];
int digit = 0, dp = 0;


void VL6180X_prog(){
    if(readByte(0x0016)==0x01)      //FRESH_OUT_OF_RESET Register --> By default sets to 1 Check if the register 0x016 is equal to 1 (Watch the protocol diagram at "VL6180X basic ranging application note" Fig.3 - Initialisation Diagram).
    {       
    // Set up private registers     //Loading all the SR03 settings (these are the basic settings recommanded in the "VL6180X basic ranging application note".
    writeByte(0x0207, 0x01);       
    writeByte(0x0208, 0x01); 
    writeByte(0x0096, 0x00); 
    writeByte(0x0097, 0xfd); 
    writeByte(0x00e3, 0x01); 
    writeByte(0x00e4, 0x03); 
    writeByte(0x00e5, 0x02); 
    writeByte(0x00e6, 0x01); 
    writeByte(0x00e7, 0x03); 
    writeByte(0x00f5, 0x02); 
    writeByte(0x00d9, 0x05); 
    writeByte(0x00db, 0xce); 
    writeByte(0x00dc, 0x03); 
    writeByte(0x00dd, 0xf8); 
    writeByte(0x009f, 0x00); 
    writeByte(0x00a3, 0x3c); 
    writeByte(0x00b7, 0x00); 
    writeByte(0x00bb, 0x3c); 
    writeByte(0x00b2, 0x09); 
    writeByte(0x00ca, 0x09); 
    writeByte(0x0198, 0x01); 
    writeByte(0x01b0, 0x17); 
    writeByte(0x01ad, 0x00); 
    writeByte(0x00ff, 0x05); 
    writeByte(0x0100, 0x05); 
    writeByte(0x0199, 0x05); 
    writeByte(0x01a6, 0x1b); 
    writeByte(0x01ac, 0x3e); 
    writeByte(0x01a7, 0x1f); 
    writeByte(0x0030, 0x00);

    // Recommended register setup
    //p.55 -> Set the GPIO1 as a Interrupt Output
    writeByte(0x0011, 0x10);
    
    //p.54 -> Set the GPIO0 as Main Xshutdown input, when enabled other bits of the register are ignored **(+Signal Polarity Selection ???)**
    writeByte(0x0010, 0x10);
    
    //p.75 -> Set Averaging sample period (=Reset Value)
    writeByte(0x010a, 0x30);
    
    //p.67 -> OK(Register fixed) SYSALS Analog Gain    
    //writeByte(0x003f, 0x46);
    
    //p.64 -> Meaning at p.30 Calibration due to Temperature sensibility from the sensor User entered repeat rate of auto VHV task ??? Repeat Rate is enabled 0xFF = 255  
    writeByte(0x0031, 0xFF);
    
    //p.67 -> Recommanded Settings for SYS ALS Integration period (100ms)    
    //writeByte(0x0040, 0x63);
    
    //p.64 -> Manual trigger for VHV recalibration enabled -- Perform a single temperature calibration of the ranging sensor    
    writeByte(0x002e, 0x01);
     
    // Optional registers
    //P.63 -> Disable Early Convergence Estimate ***MODIFIED 
    writeByte(0x002d, 0x10);
    
    //p.60 -> SYSRange Intermeasurments period **IN CONTINIOUS MODE** --->Range 0-254 (0 = 10ms). Step size = 10ms   
    writeByte(0x001b, 0x04);
    
    //p.66 -> SYS ALS Intermeasurments period Same bit range as SYS Range Inter. period  
    writeByte(0x003e, 0x31);
    
    //p.57 -> Interrup mode source For ALS Reading = 4 -> New sample ready   **same for range readings 
    writeByte(0x0014, 0x04);
         
    // Change fresh out of set status to 0
    // Set the register from beginning to 0 to end the initialisation process (c.f. diagram). Needs to be set at 0 once the boot is done
    writeByte(0x0016, 0x00);  
        
    Serial.println("Configuration OK");
    }
    
  //Clear Interrupt Flag
  writeByte(0x015,0x07);
  Serial.println("Start measurements");
  //SYS RANGE START
  writeByte(0x18,0x03);             
}



void setup ()
{
  Serial.begin(9600);
  Wire.begin();                                  // join i2c bus (address optional for master)
  for (int ii = 0; ii < sizeof(outPins); ii++)   // Loop until 'ii' is bigger than the number of Outpins (defined before)
  { 
    pinMode(outPins[ii], OUTPUT);                // Defined each Outpins a Outputs
    digitalWrite(outPins[ii], HIGH);             // Set all the Outputs at high Lvl
  }
  // Attach timer interrupt
  Timer1.initialize(5000);
  Timer1.attachInterrupt(callback);
  // Init VL6180X
  pinMode(CHIP_EN, OUTPUT);
  digitalWrite(CHIP_EN, HIGH);
  delay(1000);
  VL6180X_prog();
}
//-----------------------------------------------------------------------------------

// Liaison I²C --> Reading Byte
uint8_t readByte (uint16_t subAddress) 
{
  Wire.beginTransmission(VL6180X_ADDRESS);
  Wire.write((subAddress >> 8) & 0xFF);
  Wire.write(subAddress & 0xFF);
  Wire.endTransmission(false);
  Wire.requestFrom(VL6180X_ADDRESS, 1);
  return Wire.read();
}

//-----------------------------------------------------------------------------------

// Liaison I²C --> Writing Byte
void writeByte (uint16_t subAddress, uint8_t data) 
{
  Wire.beginTransmission(VL6180X_ADDRESS);
  Wire.write((subAddress >> 8) & 0xFF);
  Wire.write(subAddress & 0xFF);
  Wire.write(data);
  Wire.endTransmission();
}

//-----------------------------------------------------------------------------------

//Fonction Initialisation 7-Seg. Display
void setSegments (unsigned char digit, unsigned char num) {
  unsigned char pattern = sDecode[num];
  for (int ii = 0; ii < 4; ii++) {
    digitalWrite(outPins[ii], digit == ii ? LOW : HIGH);
  }
  for (int jj = 4; jj < sizeof(outPins) - 1; jj++) {
    unsigned char mask = 1 << (jj - 4);
    digitalWrite(outPins[jj], (pattern & mask) != 0 ? LOW : HIGH);
  }
  digitalWrite(SEG_DP, digit == dp ? LOW : HIGH);
}

//-----------------------------------------------------------------------------------

//Fonction D'affichage 
// Scans 7 Segment display
void callback () {
  setSegments(digit, digits[digit]);
  digit = (digit + 1) % 4;
}

void writeNumber (int num) {
  for (int ii = 0; ii < sizeof(digits); ii++) {
    digits[ii] = num % 10;
    num /= 10;
  }
}

//-----------------------------------------------------------------------------------

//Range Measurements Continuous mode 
int Range_Continuous ()
{
  //Set the Timeout Value (in ms) for the timeout function -- Set to 0 for disable it
  unsigned long TimeOut = 400;
  
  //Recover the Start Time from the While loop 
  unsigned long Start_Timer = millis(); 
  // Check if the flag is set to 0
  //Serial.println(Start_Timer);
  while((readByte(0x04f) & 0x07) == 0)
  {
    //If the Timeout is different from 0 AND IF the Delta between The Start Time and the millis() function is bigger than TimeOut Value then, the function return -1. 
    if ((TimeOut > 0) && ((millis() - Start_Timer) > TimeOut))
    {
      return -1;
    }
  }
   
   //Clear Interrupt Flag
   writeByte(0x015,0x07);
   return readByte(0x062);
}

//-----------------------------------------------------------------------------------

void Range_error()
{
  int Error_Status= readByte(0x04D);
  Serial.println(Error_Status,HEX);
}

//-----------------------------------------------------------------------------------


void loop () {
  int Result = Range_Continuous();
  //If Range Continuous function returns -1 it means that the Timeout was over, the function needs to be re-started 
  if (Result<0)
  {
    Serial.println("TIMEDOUT");
    Serial.println(readByte(0x04D));
    //Reeboot the sensor registers
    writeByte(0x0016,0x00);
    VL6180X_prog();
    writeByte(0x0016,0x01);
  }  
  else {
   
    //Serial.println(Result);
     writeNumber(Result);
     Serial.println(Result);
     
  }  
}
