#include <VL53L1X.h>
#include <Wire.h>
#include <Servo.h>
#include <EEPROM.h>

#define TARGET_DISTANCE_EEPROM_ADDRESS 1

#define BUTTON_PIN 2
#define SERVO_PIN 9
#define LED_PIN 13 // built in led pin = 13

#define MIN_PULSE_WIDTH       544
#define MAX_PULSE_WIDTH      2400
#define DEFAULT_PULSE_WIDTH  1500
#define PULSE_WIDTH_REBOUND 5
#define PULSE_WIDTH_TOLERANCE 1 
#define PULSE_WIDTH_ADJUSTMENT_WEIGHT 0.1

#define MIN_MAX_ALERT_PULSE_WIDTH_TOLERANCE 10 

#define SENSOR_ANGLE_RAD 0.698131701 // sensor pointing 40 deg down

#define MAIN_LOOP_DELAY 100

#define DEFAULT_TARGET_DISTANCE 800
#define MAX_TARGET_DISTANCE 3000
#define MIN_TARGET_DISTANCE 500

#define GEAR_RATIO 0.3

#define BLINK_DELAY 50UL
#define BLINK_PAUSE_DELAY_SHORT 100UL
#define BLINK_PAUSE_DELAY_MED 500UL
#define BLINK_PAUSE_DELAY_LONG 2000UL


Servo myservo;
VL53L1X sensor;

int measuredDistance = DEFAULT_TARGET_DISTANCE;

int targetDistance = DEFAULT_TARGET_DISTANCE;
int heightCalculated = DEFAULT_TARGET_DISTANCE; // will be used for actual angle calculations when new measurement is taken

int currentPulseWidth = DEFAULT_PULSE_WIDTH;
int targetDistancePulseWidth = DEFAULT_PULSE_WIDTH;
float servoAngleToPulseWidthRatio = (MAX_PULSE_WIDTH - MIN_PULSE_WIDTH) / (GEAR_RATIO * PI); // to convert angle adjustment to pulse width adjustment

float targetAngleRad = SENSOR_ANGLE_RAD; //real sensor-to-horizon angle in default position. assume this is close to servo middle position
float currentAngleRad = SENSOR_ANGLE_RAD; // this will be calculated based on distance measurements
float smoothedAngleRad = SENSOR_ANGLE_RAD;

// status flags
bool LedBlinkState = false;
bool callibrating = false;
bool trackerEnabled = false;
bool maxPositionAlert = false;
bool sensorFailureAlert = false;

//LED timer
unsigned long ledBlinkTime = 0UL;
 
void setup() {
  pinMode(LED_PIN, OUTPUT);
  pinMode(BUTTON_PIN, INPUT);
  
  Wire.begin();
  Wire.setClock(400000); // use 400 kHz I2C
  sensor.setTimeout(500);
  if (!sensor.init()){ //sesor error
    while (1){
      toggleLed(); // blink fast if cannot initialise sensor
      delay(50);
    }
  }

  sensor.setDistanceMode(VL53L1X::Long); // Long = 4m range
  sensor.setMeasurementTimingBudget(50000);
  sensor.startContinuous(50);
  
  myservo.attach(SERVO_PIN);
  
  EEPROM.get(TARGET_DISTANCE_EEPROM_ADDRESS, targetDistance); // read saved target distance from eeprom
  
  if(targetDistance <= 0){
    targetDistance = DEFAULT_TARGET_DISTANCE;
  } else {
    targetDistance = max(min(targetDistance, MAX_TARGET_DISTANCE), MIN_TARGET_DISTANCE);
  }
  
  calculateHeight();
  
  // move servo to default position
  myservo.writeMicroseconds(currentPulseWidth);
  disableLed();
}

void loop() {
  handleButtonActions();
  handleLedActions();
  
  //measure distance and calculate servo adjustment
  if(trackerEnabled){

    int servoPulseWidthAdjustment = 0; 
    
    sensor.read();
    
    if(sensor.ranging_data.range_status == 0){ //RangeValid
      measuredDistance = sensor.ranging_data.range_mm;
      sensorFailureAlert = false;

      if(measuredDistance > heightCalculated){
        
        currentAngleRad = asin(float(heightCalculated) / float(measuredDistance)); // calculate estimated current sensor-to-horizon angle
        
        float angleDiff = currentAngleRad - targetAngleRad; // positive value means sensor needs to be moved down
        servoPulseWidthAdjustment = int(angleDiff * servoAngleToPulseWidthRatio * PULSE_WIDTH_ADJUSTMENT_WEIGHT);
  
        if(abs(servoPulseWidthAdjustment) <= PULSE_WIDTH_TOLERANCE){ //avoid small adjustments
          servoPulseWidthAdjustment = 0;
        }

        currentPulseWidth = currentPulseWidth + servoPulseWidthAdjustment; // increasing this value moves sensor up (decreasing sensor-to-horizon angle)
        
      }  else {   
        servoPulseWidthAdjustment = PULSE_WIDTH_REBOUND; // rebound up if measured distance is too low most likely bacause wheel part itself is in sensor's range
        currentPulseWidth = min(currentPulseWidth + servoPulseWidthAdjustment, DEFAULT_PULSE_WIDTH); // servo devault position is maximum here to avoid crawling all the way up if there is an obstacle close to sensor
      }
    } else {
      sensorFailureAlert = true;
    }
    
    currentPulseWidth = max(min(currentPulseWidth, MAX_PULSE_WIDTH), MIN_PULSE_WIDTH); // make sure it stays between min and max values

    if(min(MAX_PULSE_WIDTH - currentPulseWidth, currentPulseWidth - MIN_PULSE_WIDTH) <= MIN_MAX_ALERT_PULSE_WIDTH_TOLERANCE ){
      maxPositionAlert = true;
    } else {
      maxPositionAlert = false;
    }
  } else {
    // use default position if tracker disabled
    currentPulseWidth = DEFAULT_PULSE_WIDTH; 
    maxPositionAlert = false;
    sensorFailureAlert = false;
  }

  // move servo
  myservo.writeMicroseconds(currentPulseWidth);
  
  delay(MAIN_LOOP_DELAY);
}

void handleLedActions(){ // blink led
  if(maxPositionAlert or sensorFailureAlert){
    enableLed();
  } else { 
    unsigned long timeDiff = millis() - ledBlinkTime;
    unsigned long blinkDelay = BLINK_DELAY;

    if(LedBlinkState){ // if already enabled
      blinkDelay = BLINK_DELAY;
    } else if(callibrating){
      blinkDelay = BLINK_PAUSE_DELAY_SHORT;
    } else if(trackerEnabled){
      blinkDelay = BLINK_PAUSE_DELAY_MED;
    } else {
      blinkDelay = BLINK_PAUSE_DELAY_LONG;
    }
 
    if(timeDiff > blinkDelay){
      toggleLed();
      ledBlinkTime = millis(); // reset timer
    }
  } 
}

void handleButtonActions(){
  if(digitalRead(BUTTON_PIN) == HIGH){   
    unsigned long buttonPressedTime = millis();
    while(digitalRead(BUTTON_PIN) == HIGH){ // stop everything else while pressed
      enableLed();
      delay(10);
    }
    //button release 
    disableLed();
    unsigned long timeDiff = millis() - buttonPressedTime;
    if(timeDiff > 3000UL){ 
      // 3 seconds
      callibrating = !callibrating; //toggle callibrating status
      if(callibrating){ // disable tracker
        trackerEnabled = false;
      }
    } else if(timeDiff > 1000UL){  
      // 1 second
      if(callibrating){ // read and set target distance 
        callibrateDistance();
      }        
    } else if(timeDiff > 10UL){ 
      // short
      if(!callibrating){
        trackerEnabled = !trackerEnabled; // toggle tracker state
      }
    }
  }
}

void callibrateDistance(){
  sensor.read();
  targetDistance = sensor.ranging_data.range_mm;
  callibrating = false;
  calculateHeight();
  EEPROM.put(TARGET_DISTANCE_EEPROM_ADDRESS, targetDistance); // store callibration data permanently
}

void calculateHeight(){
  heightCalculated = int(float(targetDistance) * sin(targetAngleRad)); // this height will be used to calculate angle based on distance measurements
}

void enableLed(){
  if(LedBlinkState == false){
    digitalWrite(LED_PIN, HIGH);
    LedBlinkState = true;
  }
}

void disableLed(){
  if(LedBlinkState == true){
    digitalWrite(LED_PIN, LOW);
    LedBlinkState = false;
  }
}

void toggleLed(){
  if(LedBlinkState){ // if enabled
    disableLed();
  } else {
    enableLed();
  }
}