/*
 * file: M5Stack_VL53L0X
 * Written by Ahmad Shamshiri on April 03, 2021 in Ontario, Canada
 * www.Robojax.com YouTube http://youtube.com/robojaxTV
 
 * using Pololu VL53L0X library https://github.com/pololu/vl53l0x-arduino
 * Watch video instruction: https://youtu.be/6Js7CPe7Dwo
 * 
 * Must watch two videos
 * Introduction to M5Stack Core 2: https://youtu.be/zoDjT2_0M5g
 * Introduction to VL53L0X: https://youtu.be/S2jaAQEv3Yo
 * 

 * 
  * 
 * This code is "AS IS" without warranty or liability. Free to be used as long as you keep this note intact.
 * This code has been downloaded from Robojax.com
    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <https://www.gnu.org/licenses/>.
  
   URL: https://github.com/RobTillaart/HT16K33
  
*/
#include <M5Core2.h>
#include <Wire.h>
#include <VL53L0X.h>

VL53L0X sensor;
const uint8_t maximumDistance = 800;///mm in mm , 800mm=80cm
const uint8_t type = 1;// 1=mm , 2= cm; 3=inch (1 mm = 0.03937 inch)
char *unit[]={"mm","cm","in"};// variable for unit, mm, cm or in

float distanceCm, distanceIn;

uint8_t distance;

void printOnSerial();
void setup() {
  // put your setup code here, to run once:
  //---osmar
  M5.begin();
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextColor(YELLOW);

  M5.Lcd.setTextSize(5);
  
  Wire.begin();        // join i2c bus (address optional for master)
  Serial.println("VLX53LOX test started.");

  Serial.begin(9600);


  sensor.init();
  sensor.setTimeout(500);

  // Start continuous back-to-back mode (take readings as
  // fast as possible).  To use continuous timed mode
  // instead, provide a desired inter-measurement period in
  // ms (e.g. sensor.startContinuous(100)).
  sensor.startContinuous();


delay(1000);


}

void loop() {
  M5.Lcd.fillScreen(BLACK);    
  M5.Lcd.setCursor(0,0);  
  M5.Lcd.print("Distance");    
 distance =sensor.readRangeContinuousMillimeters();

  if(type==3){
    M5.Lcd.setCursor(0, 60);        
    distanceIn =(float) (distance*0.03937);//convert distance to inch
    M5.Lcd.printf("%.4f", distanceIn);
    M5.Lcd.print(unit[type-1]);    
  }else if(type==2){
    M5.Lcd.setCursor(0, 60);        
    distanceCm =(float) (distance/10.0);//convert distance to cm
    M5.Lcd.printf("%.1f", distanceCm);
    M5.Lcd.print(unit[type-1]);    
  }else{
    M5.Lcd.setCursor(0, 60);    
    M5.Lcd.print(distance);//simply print mm
    M5.Lcd.print(unit[type-1]);
  }
  
  
  delay(500);
  printOnSerial();
}

/*
Written by Ahmad Shamshiri for Robojax, Robojax.com
*/
void printOnSerial()
{
      Serial.print("Distance: ");  
        if(type==3){ 
           Serial.print(distanceIn);
        }else if(type==2){
           Serial.print(distanceCm);          
        }else{

           Serial.print(distance);         
        }
      Serial.print(unit[type-1]);      
      Serial.println();  
}