#include <Wire.h>
#include <VL53L0X.h>
// 20 teeth to 125 = 6.25 at 1/8 steps = 10000 steps per revolution = 251mm at 80mm diameter = ca. 36000 steps for 900mm

#define LIMITUP 2700
#define LIMITDOWN 1800

#define STEP 6
#define DIR 7
#define ENABLE 1

#define LEDWW 14
#define LEDCW 10

#define REFERENCE 18

VL53L0X sensor;

volatile bool referenced = false;
int maxSteps = floor((LIMITUP - LIMITDOWN) * (10000/251));
volatile int position = 0;
int newPosition = 0;
volatile int distance = 10000;
volatile bool setup_finished = false;

void step(bool dir = false, int del = 1000){
  if(dir) digitalWrite(DIR, HIGH);
  if(!dir) digitalWrite(DIR, LOW);
  
  if(!dir) position++;
  if(dir) position--;
  //Serial.println(position);
  digitalWrite(STEP, HIGH);
  delayMicroseconds(del);
  digitalWrite(STEP, LOW);
  delayMicroseconds(del);
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);

  delay(1000);

  pinMode(STEP, OUTPUT);
  pinMode(DIR, OUTPUT);
  pinMode(ENABLE, OUTPUT);
  pinMode(REFERENCE, INPUT_PULLUP);

  Wire.setSDA(16);
  Wire.setSCL(17);
  Wire.begin();

  // for(int i = 0; i < 10000; i++){ // 10000 steps = 1 revolution
  //   step(true);
  // }
  sensor.setTimeout(500);
  if (!sensor.init())
  {
    
    while (1) {
      Serial.println("Failed to detect and initialize sensor!");
      delay(100);
    }
  }

  sensor.setSignalRateLimit(0.1);
  sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
  sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);

  //analogWrite(LEDCW, 25);
  //analogWrite(LEDWW, 25);
  delay(1000);
  digitalWrite(ENABLE, LOW);
  setup_finished = true;
}

void setup1(){}

void loop() {
  distance = sensor.readRangeSingleMillimeters();
  Serial.print(distance);
  Serial.print("\t");
  Serial.println(position);
  delay(10);
}

uint32_t counter = 0;
uint32_t last = 0;
uint32_t next = 0;

void loop1(){
  if(setup_finished){
    if(!referenced){
      if(digitalRead(REFERENCE) == HIGH) {
        digitalWrite(ENABLE, LOW);
        analogWrite(LEDCW, 0);
        analogWrite(LEDWW, 25);
        step(true);
      } else {
        referenced = true;
        position = 0;
        analogWrite(LEDCW, 50);
        delay(100);
        analogWrite(LEDCW, 25);
      }
    } else {
      if(position == newPosition){
        newPosition = random(0, maxSteps);
      } else {
        if(position > newPosition) step(true, 1500);
        if(position < newPosition) step(false, 1500);
      }
    }
  }
}