#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 = 50;
const int maxHeight = 455;

int minServo = 30;
int maxServo = 60;
int lastAngle = -1;


float curHeight = 50;
float targetHeight = 250;

float equilibrium = 0;

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;
  }
}

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")); 

  Serial.println("Setuping");
  servo1.write(minServo);
  delay(2000);
  float h = mesureHeight();
  if (h < maxHeight) {
    Serial.println("CHANGE FAN SPEED");
    while (1);
  }
  Serial.println("Setuping for downwards");
  int angle = minServo;
  while (h > maxHeight) {
    ++angle;
    servo1.write(angle);
    delay(100);
    h = mesureHeight();
  }
  angle -= 2;
  servo1.write(angle);
  equilibrium = angle;
  Serial.println("Done setuping have funning");
}


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

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

void loop() {
   
  float h = mesureHeight();
  Serial.print("Measure = ");
  Serial.println(h);
  if (abs(h - targetHeight) > 1.0f) {
    servo1.write(equilibrium + 5);
  }

  delay(1);
}