#include <MovingAverage.h>

#include <Wire.h>
#include <VL53L0X.h>
#include <AccelStepper.h>
#include <MultiStepper.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 2100

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

#define LEDWW 14
#define LEDCW 10

#define REFERENCE 18

VL53L0X sensor;

AccelStepper stepper = AccelStepper(AccelStepper::DRIVER, 6, 7);

MovingAverage filter(10);

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

float phase = 0;

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

  delay(1000);
  
  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(){
  pinMode(ENABLE, OUTPUT);

  stepper.setEnablePin(1);
  stepper.setPinsInverted(true, false, true);

  stepper.setMaxSpeed(200);
  stepper.setAcceleration(50);
}

int counter = 0;

void loop() {
  distance = 10000; //filter.addSample(sensor.readRangeSingleMillimeters());

  if(referenced && distance < 2000 && !triggered && !cooldown) {
    triggered = true;
    counter = 0;
  }

  if(referenced) {
    analogWrite(LEDWW, sinf(phase) * 75 + 85);
    phase += 0.01;
  } else {
    analogWrite(LEDWW, sinf(phase) * 10 + 10);
    phase += 0.25;
  }

  Serial.print(distance);
  Serial.print("\t");
  Serial.println(stepper.currentPosition());
  
  delay(5);
}

uint32_t distance_to_go = 0;

void loop1(){
  if(setup_finished){
    if(!referenced){
      if(digitalRead(REFERENCE) == HIGH) {
        stepper.enableOutputs();
        stepper.move(-100000);
      } else {
        referenced = true;
        stepper.stop();
        stepper.setCurrentPosition(0);
        newPosition = random(0, maxSteps);
        stepper.moveTo(newPosition);
      }
    } else {
      if(triggered && !cooldown){
      	stepper.setMaxSpeed(6000);
      	stepper.setAcceleration(5000);
        stepper.moveTo(100);
        distance_to_go = stepper.distanceToGo();
        cooldown = true;
        Serial.println("TRIGGERED");
      }
      if(abs(stepper.distanceToGo()) < 2){
        Serial.println("DISTANCE < 2");
        if(!triggered){
          Serial.println("NEW POS");
          newPosition = random(0, maxSteps);
          stepper.setMaxSpeed((rand() % 750) + 200);
          stepper.setAcceleration((rand() % 500) + 100);
          stepper.moveTo(newPosition);
        }
        if(triggered) {
          Serial.println("RESET COOLDOWN");
          cooldown = false;
          triggered = false;
        }
      }
    }
    if(triggered) {
      analogWrite(LEDCW, map(stepper.distanceToGo(), distance_to_go, 0, 150, 0));
    }

    stepper.run();
  }
}