// Nikolai found library to smooth out and average values from sensor
// Include library
#include "Adafruit_VL53L0X.h"

// make an instance of the library:
Adafruit_VL53L0X sensor = Adafruit_VL53L0X();

const int maxDistance = 2000;

// Do i need pinMode?

void setup() { // Setup code here, runs once
  // initialize serial, wait 3 seconds for Serial Monitor to open:
  Serial.begin(9600);
  if (!Serial) delay(3000);

  // Initialize sensor, stop if it fails:
  if (!sensor.begin()) {
    Serial.println("Sensor not responding. Check wiring.");
    while (true);
  }
  /* config can be:
    VL53L0X_SENSE_DEFAULT: about 500mm range
    VL53L0X_SENSE_LONG_RANGE: about 2000mm range
    VL53L0X_SENSE_HIGH_SPEED: about 500mm range
    VL53L0X_SENSE_HIGH_ACCURACY: about 400mm range, 1mm accuracy
  */
  sensor.configSensor(Adafruit_VL53L0X::VL53L0X_SENSE_LONG_RANGE); // TODO is the long range choice affecting accuracy?

  // Set sensor to range continuously:
  sensor.startRangeContinuous();
}

void loop() { // Runs repeatedly
  // If reading is done:
  if (sensor.isRangeComplete()) {
    // Read the result:
    int result = sensor.readRangeResult(); // readRangeResult() from Wire Library
    
    // If within maximum distance...
    if (result < maxDistance) {
      // print result (distance in mm):
      Serial.println(result);
    }
  }
}

// Serial Monitor prints at erradic velocities - Sensor from shop seems to be reading a larger range of values.
// I moved hand laterally and sensor did 200~ / 1050~ / 200~

#include "Adafruit_VL53L0X.h"
#include <Servo.h>

// SENSOR
Adafruit_VL53L0X sensor = Adafruit_VL53L0X();
const int maxDistance = 2000;

// SERVO
Servo servoMotor; // Create instance of the servo
int servoPin = 9; // Control pin for servo motor
long lastMoveTime = 0; // Time when the servo was last updated, in ms

void setup() {
  Serial.begin(9600); // initialize serial communications
  if (!Serial) delay(3000);

  // pinMode(A4, INPUT);
  // pinMode(servoPin, OUTPUT);

  // Initialize SENSOR, stop if it fails:
  if (!sensor.begin()) {
    Serial.println("Sensor not responding. Check wiring.");
    while (true);
  }
  sensor.configSensor(Adafruit_VL53L0X::VL53L0X_SENSE_LONG_RANGE); // TODO is the long range choice affecting accuracy?
  sensor.startRangeContinuous();

  // SERVO
  servoMotor.attach(servoPin); // attaches the servo on pin 9 to the servo object
}

void loop() {
  // SENSOR
  // If reading is done:
  if (sensor.isRangeComplete()) {
    // Read the result:
    int result = sensor.readRangeResult(); // readRangeResult() from Wire Library
    
    // If within maximum distance...
    if (result < maxDistance) {
      // print result (distance in mm):
      Serial.println(result);
    }

    // SERVO Output:
    // int analogServoValue = analogRead(A4); // Read data line from distance sensor
    //Serial.println(analogServoValue); // Printing it

    int servoAngle = map(result, 0, 500, 0, 179); // map(value, fromLow, fromHigh, toLow, toHigh)
    // //   // TODO: check actual maximum for servo reading

    if (millis() - lastMoveTime > 20) {
      servoMotor.write(servoAngle);
      lastMoveTime = millis();
    }
    // Is there a way to reset the servoMotor angle to 0º?
  }
}

/*************************************************** 
  This is an example for our Adafruit 16-channel PWM & Servo driver
  Servo test - this will drive 8 servos, one after the other on the
  first 8 pins of the PCA9685

  Pick one up today in the adafruit shop!
  ------> http://www.adafruit.com/products/815
  
  These drivers use I2C to communicate, 2 pins are required to  
  interface.

  Adafruit invests time and resources providing this open source code, 
  please support Adafruit and open-source hardware by purchasing 
  products from Adafruit!

  Written by Limor Fried/Ladyada for Adafruit Industries.  
  BSD license, all text above must be included in any redistribution
 ****************************************************/

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// you can also call it with a different address you want
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);
// you can also call it with a different address and I2C interface
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40, Wire);

// Depending on your servo make, the pulse width min and max may vary, you 
// want these to be as small/large as possible without hitting the hard stop
// for max range. You'll have to tweak them as necessary to match the servos you
// have!
#define SERVOMIN  150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  250 // This is the 'maximum' pulse length count (out of 4096)
#define USMIN  600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX  2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates

// our servo # counter
uint8_t servonum = 0; // 0 = there is 1 servo

void setup() {
  Serial.begin(9600);
  Serial.println("8 channel Servo test!");

  pwm.begin();
  /*
   * In theory the internal oscillator (clock) is 25MHz but it really isn't
   * that precise. You can 'calibrate' this by tweaking this number until
   * you get the PWM update frequency you're expecting!
   * The int.osc. for the PCA9685 chip is a range between about 23-27MHz and
   * is used for calculating things like writeMicroseconds()
   * Analog servos run at ~50 Hz updates, It is importaint to use an
   * oscilloscope in setting the int.osc frequency for the I2C PCA9685 chip.
   * 1) Attach the oscilloscope to one of the PWM signal pins and ground on
   *    the I2C PCA9685 chip you are setting the value for.
   * 2) Adjust setOscillatorFrequency() until the PWM update frequency is the
   *    expected value (50Hz for most ESCs)
   * Setting the value here is specific to each individual I2C PCA9685 chip and
   * affects the calculations for the PWM update frequency. 
   * Failure to correctly set the int.osc value will cause unexpected PWM results
   */
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates

  delay(10);
}

// You can use this function if you'd like to set the pulse length in seconds
// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. It's not precise!
void setServoPulse(uint8_t n, double pulse) {
  double pulselength;
  
  pulselength = 1000000;   // 1,000,000 us per second
  pulselength /= SERVO_FREQ;   // Analog servos run at ~60 Hz updates
  Serial.print(pulselength); Serial.println(" us per period"); 
  pulselength /= 4096;  // 12 bits of resolution
  Serial.print(pulselength); Serial.println(" us per bit"); 
  pulse *= 1000000;  // convert input seconds to us
  pulse /= pulselength;
  Serial.println(pulse);
  pwm.setPWM(n, 0, pulse);
}

void loop() {
  // Drive each servo one at a time using setPWM()
  Serial.println(servonum);
  for (uint16_t pulselen = SERVOMIN; pulselen < SERVOMAX; pulselen++) {
    pwm.setPWM(servonum, 0, pulselen);
  }

  delay(500);
  for (uint16_t pulselen = SERVOMAX; pulselen > SERVOMIN; pulselen--) {
    pwm.setPWM(servonum, 0, pulselen);
  }

  delay(500);

  // Drive each servo one at a time using writeMicroseconds(), it's not precise due to calculation rounding!
  // The writeMicroseconds() function is used to mimic the Arduino Servo library writeMicroseconds() behavior. 
  for (uint16_t microsec = USMIN; microsec < USMAX; microsec++) {
    pwm.writeMicroseconds(servonum, microsec);
  }

  delay(500);
  for (uint16_t microsec = USMAX; microsec > USMIN; microsec--) {
    pwm.writeMicroseconds(servonum, microsec);
  }

  delay(500);

  servonum++;
  if (servonum > 4) servonum = 0; // Testing the first 5 servo channels
}


// Board 0: I2C Address = 0x40

#include "Adafruit_VL53L0X.h"
const byte VL53LOX_InterruptPin = 6;
const byte VL53LOX_ShutdownPin = 9;
volatile byte VL53LOX_State = LOW;
Adafruit_VL53L0X lox = Adafruit_VL53L0X();

void setup() {
  Serial.begin(115200);

  // wait until serial port opens for native USB devices
  while (!Serial) {
    delay(1);
  }
  Serial.println(F("VL53L0X API Interrupt Ranging example\n\n"));

  pinMode(VL53LOX_ShutdownPin, INPUT_PULLUP);
  pinMode(VL53LOX_InterruptPin, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(VL53LOX_InterruptPin), VL53LOXISR,
                  CHANGE);
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);

  // if lox.begin failes its becasue it was a warm boot and the VL53LOX is in
  // continues mesurement mode we can use an IO pin to reset the device in case
  // we get stuck in this mode
  while (!lox.begin()) {
    Serial.println(F("Failed to boot VL53L0X"));
    Serial.println("Adafruit VL53L0X XShut set Low to Force HW Reset");
    digitalWrite(VL53LOX_ShutdownPin, LOW);
    delay(100);
    digitalWrite(VL53LOX_ShutdownPin, HIGH);
    Serial.println("Adafruit VL53L0X XShut set high to Allow Boot");
    delay(100);
  }

  // Second Parameter options are VL53L0X_GPIOFUNCTIONALITY_OFF,
  // VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW,
  // VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH,
  // VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT,
  // VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY

  Serial.println("Set GPIO Config so if range is lower the LowThreshold "
                 "trigger Gpio Pin ");
  lox.setGpioConfig(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
                    VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW,
                    VL53L0X_INTERRUPTPOLARITY_LOW);

  // Set Interrupt Treashholds
  // Low reading set to 50mm  High Set to 100mm
  FixPoint1616_t LowThreashHold = (50 * 65536.0);
  FixPoint1616_t HighThreashHold = (100 * 65536.0);
  Serial.println("Set Interrupt Threasholds... ");
  lox.setInterruptThresholds(LowThreashHold, HighThreashHold, true);

  // Enable Continous Measurement Mode
  Serial.println("Set Mode VL53L0X_DEVICEMODE_CONTINUOUS_RANGING... ");
  lox.setDeviceMode(VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, false);

  Serial.println("StartMeasurement... ");
  lox.startMeasurement();
}

void VL53LOXISR() {
  // Read if we are high or low
  VL53LOX_State = digitalRead(VL53LOX_InterruptPin);
  // set the built in LED to reflect in range on for Out of range off for in
  // range
  digitalWrite(LED_BUILTIN, VL53LOX_State);
}

void loop() {
  if (VL53LOX_State == LOW) {
    VL53L0X_RangingMeasurementData_t measure;
    Serial.print("Reading a measurement... ");
    lox.getRangingMeasurement(
        &measure, false); // pass in 'true' to get debug data printout!

    if (measure.RangeStatus != 4) { // phase failures have incorrect data
      Serial.print("Distance (mm): ");
      Serial.println(measure.RangeMilliMeter);
    } else {
      Serial.println(" out of range ");
    }
    // you have to clear the interrupt to get triggered again
    lox.clearInterruptMask(false);

  } else {
    delay(10);
  }
}

#include "Adafruit_VL53L0X.h"
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h> 

// Servo Driver
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); // uses the default address 0x40

// address we will assign if dual sensor is present
#define LOX1_ADDRESS 0x30
#define LOX2_ADDRESS 0x31

// set the pins to shutdown
#define SHT_LOX1 7
#define SHT_LOX2 6

// objects for the vl53l0x
Adafruit_VL53L0X lox1 = Adafruit_VL53L0X();
Adafruit_VL53L0X lox2 = Adafruit_VL53L0X();

const int maxDistance = 2000;
long lastMoveTime = 0; // Time when the servos were last updated, in ms

// this holds the measurement
VL53L0X_RangingMeasurementData_t measure1;
VL53L0X_RangingMeasurementData_t measure2;

#define SERVOMIN  150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  350 // This is the 'maximum' pulse length count (out of 4096)
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates

// our servo # counter
uint8_t servonum = 2; // 0 = there is 1 servo

/*
    Reset all sensors by setting all of their XSHUT pins low for delay(10), then set all XSHUT high to bring out of reset
    Keep sensor #1 awake by keeping XSHUT pin high
    Put all other sensors into shutdown by pulling XSHUT pins low
    Initialize sensor #1 with lox.begin(new_i2c_address) Pick any number but 0x29 and it must be under 0x7F. Going with 0x30 to 0x3F is probably OK.
    Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.
    Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever you set the first sensor to
 */
void setID() {
  // all reset
  digitalWrite(SHT_LOX1, LOW);    
  digitalWrite(SHT_LOX2, LOW);
  delay(10);
  // all unreset
  digitalWrite(SHT_LOX1, HIGH);
  digitalWrite(SHT_LOX2, HIGH);
  delay(10);

  // activating LOX1 and resetting LOX2
  digitalWrite(SHT_LOX1, HIGH);
  digitalWrite(SHT_LOX2, LOW);

  // initing LOX1
  if(!lox1.begin(LOX1_ADDRESS)) {
    Serial.println(F("Failed to boot first VL53L0X"));
    while(1);
  }
  delay(10);

  // activating LOX2
  digitalWrite(SHT_LOX2, HIGH);
  delay(10);

  //initing LOX2
  if(!lox2.begin(LOX2_ADDRESS)) {
    Serial.println(F("Failed to boot second VL53L0X"));
    while(1);
  }
}

void read_dual_sensors() {
  
  lox1.rangingTest(&measure1, false); // pass in 'true' to get debug data printout!
  lox2.rangingTest(&measure2, false); // pass in 'true' to get debug data printout!

  // print sensor one reading
  Serial.print(F("1: "));
  if(measure1.RangeStatus != 4) {     // if not out of range
    Serial.print(measure1.RangeMilliMeter);
  } else {
    Serial.print(F("Out of range"));
  }
  
  Serial.print(F(" "));

  // print sensor two reading
  Serial.print(F("2: "));
  if(measure2.RangeStatus != 4) {
    Serial.print(measure2.RangeMilliMeter);
  } else {
    Serial.print(F("Out of range"));
  }
  
  Serial.println();
}

void setup() {
  Serial.begin(115200);

  // wait until serial port opens for native USB devices
  while (! Serial) { delay(1); }

  pinMode(SHT_LOX1, OUTPUT);
  pinMode(SHT_LOX2, OUTPUT);

  Serial.println(F("Shutdown pins inited..."));

  digitalWrite(SHT_LOX1, LOW);
  digitalWrite(SHT_LOX2, LOW);

  Serial.println(F("Both in r3eset mode...(pins are low)"));
  
  
  Serial.println(F("Starting..."));
  setID();

  // Servo start
  pwm.begin();
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates
  pwm.setPWM(0, 0, SERVOMIN);
}

void loop() {
  read_dual_sensors(); // Probably can be deleted
  delay(20);

  if (lox1.isRangeComplete()) {
    int result = lox1.readRangeResult(); // from wire library

    int servoAngle = map(result, 0, 500, 0, 4095); // map(value, fromLow, fromHigh, toLow, toHigh)

    if (millis() - lastMoveTime > 20) {
      pwm.setPWM(0, 0, servoAngle);
      lastMoveTime = millis();
    }
  }
}