#include <ESP32Servo.h>
#include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lox = Adafruit_VL53L0X();

static const int servoPin = 13;

Servo servo1;
const int SDL = 22;
const int SDA2 = 21; // useless in the code

const int minHeight = 5;
const int maxHeight = 600;

int minServo = 0;
int maxServo = 90;

float curHeight = 0; 

void setup() {
  Serial.begin(115200); // Starts the serial communication
  while (! Serial) {
    delay(1);
  }
  servo1.attach(servoPin);

  Serial.println("Adafruit VL53L0X test");
  if (!lox.begin()) {
    Serial.println(F("Failed to boot VL53L0X"));
    while(1);
  }
  Serial.println(F("VL53L0X API Simple Ranging example\n\n")); 
}



auto mesureHeight(){
  VL53L0X_RangingMeasurementData_t measure;
  lox.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
  if (measure.RangeStatus != 4) {  // phase failures have incorrect data
    float height = measure.RangeMilliMeter;
    if (height < 1000){
      curHeight = height;
      return height;
    } else {
      Serial.println("Incoherent measurement. Returning previous one.");
      return curHeight;
    }
  }
  else {
    Serial.println(" out of range ");
    return curHeight;
  }
}

auto heightRatio(float height){
  return min(1.0f, (height - minHeight) / (maxHeight-minHeight));
}

int servoPosition(float ratio){
  return (ratio * maxServo) + ((1-ratio) * minServo);
}

void loop() {
  
   
  float h = mesureHeight();
  float ratio = heightRatio(h);
  int servoRot = servoPosition(ratio);
  
  /*
  Serial.print("h: ");
  Serial.print(h);
  Serial.print(" ratio: ");
  Serial.print(ratio);
  Serial.print("rotation: ");
  Serial.println(servoRot);
*/
  servo1.write(servoRot);
  delay(1);
}