/*
  Example sketch to control a stepper motor with A4988 stepper motor driver, 
  AccelStepper library and Arduino: continuous rotation. 
  More info: https://www.makerguides.com 
*/

// Include the AccelStepper library:
#include "AccelStepper.h"

#include <Wire.h>
#include <VL53L0X.h>

// Define stepper motor connections and motor interface type. 
#define dirPin 2
#define stepPin 3
#define motorInterfaceType 1
// Motor interface type must be set to 1 when using a driver
#define ms1Pin 4
#define ms2Pin 5
#define ms3Pin 6

#define speedPin A0
double lastTime = 0;
// double lastTimesensors = 0;
double thisTime = 0;
const int SPEED_MIN = 50;
const int SPEED_MAX = 800;


// Create a new instance of the AccelStepper class:
AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);

// IR SENSORS

#define XSHUT_pin4 8 //not required for address change
#define XSHUT_pin3 9
#define XSHUT_pin2 10
#define XSHUT_pin1 11

//ADDRESS_DEFAULT 0b0101001 or 41
#define Sensor4_newAddress 41  //not required address change
#define Sensor3_newAddress 42
#define Sensor2_newAddress 43
#define Sensor1_newAddress 44

// IR sensors
VL53L0X sensor4;
VL53L0X sensor3;
VL53L0X sensor2;
VL53L0X sensor1;


// bool last_sensor1_state = 0;
// bool last_sensor2_state = 0;
// bool last_sensor3_state = 0;
// bool last_sensor4_state = 0;

void setup() {
  Serial.begin(115200);
  // Set the maximum speed in steps per second:
  stepper.setMaxSpeed(4000);

  // MS
  // 1 2 3
  // 0 0 0 full
  // 1 0 0 1/2
  // 0 1 0 1/4
  // 0 0 1 1/8
  // 1 1 1 1/16
  digitalWrite(ms1Pin, 1);
  digitalWrite(ms2Pin, 1);
  digitalWrite(ms3Pin, 1);
  
  // Set the speed in steps per second:
  stepper.setSpeed(200);


  // VL53L0X
  
  /*WARNING*/
  //Shutdown pins of VL53L0X ACTIVE-LOW-ONLY NO TOLERANT TO 5V will fry them
  pinMode(XSHUT_pin1, OUTPUT);
  pinMode(XSHUT_pin2, OUTPUT);
  pinMode(XSHUT_pin3, OUTPUT);
  pinMode(XSHUT_pin4, OUTPUT);
  
  digitalWrite(XSHUT_pin1, LOW);
  digitalWrite(XSHUT_pin2, LOW);
  digitalWrite(XSHUT_pin3, LOW);
  digitalWrite(XSHUT_pin4, LOW);

  Wire.begin();

  //Change address of sensor and power up next one

  pinMode(XSHUT_pin1, INPUT);
  sensor1.init();
  sensor1.setAddress(Sensor1_newAddress);
  delay(10);
  
  pinMode(XSHUT_pin2, INPUT);
  sensor2.init();
  sensor2.setAddress(Sensor2_newAddress);
  delay(10);
  
  pinMode(XSHUT_pin3, INPUT);
  sensor3.init();
  sensor3.setAddress(Sensor3_newAddress);
  delay(10);

  pinMode(XSHUT_pin4, INPUT);
  sensor4.init();
  sensor4.setAddress(Sensor4_newAddress);
  delay(10);


  sensor1.setTimeout(0);
  sensor2.setTimeout(0);
  sensor3.setTimeout(0);
  sensor4.setTimeout(0);

  // // if (!sensor.init())
  // // {
  // //   Serial.println("Failed to detect and initialize  sensor!");
  // //   while (1) {}
  // // }
  sensor1.startContinuous();
  sensor2.startContinuous();
  sensor3.startContinuous();
  sensor4.startContinuous();
}

void loop() {
  thisTime = millis();
  if ((thisTime - lastTime) > 1000) {
    lastTime = thisTime;
    int speed_in = analogRead(speedPin);
    Serial.print("speed_in : ");
    Serial.print(speed_in);
    int speed = map(speed_in, 0, 1023, SPEED_MIN, SPEED_MAX);
    Serial.print(" | speed : ");
    Serial.println(speed);
    stepper.setSpeed(speed);
  }

  // Step the motor with a constant speed as set by setSpeed():
  stepper.runSpeed();
  
    int state1 = sensor1.readRangeContinuousMillimeters();
    Serial.print("s1 : ");
    Serial.print(state1);
    Serial.print(" | ");
    
    int state2 = sensor2.readRangeContinuousMillimeters();
    Serial.print("s2 : ");
    Serial.print(state2);
    Serial.print(" | ");
    
    int state3 = sensor3.readRangeContinuousMillimeters();
    Serial.print("s3 : ");
    Serial.print(state3);
    Serial.print(" | ");

    int state4 = sensor4.readRangeContinuousMillimeters();
    Serial.print("s4 : ");
    Serial.println(state4);
  
  
  // if (sensor1.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
  // if (sensor2.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
  // if (sensor3.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
  // if (sensor4.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
}