#include <util/delay.h>
#include <Wire.h> //Library for I2C communication, between VL53L1X and AT Tiny 1614
#include <VL53L1X.h> //Pololu library for ToF sensor VL53L1X
VL53L1X sensor;
#define onDelay() _delay_us(60) //PWM on time
#define offDelay() _delay_us(40) //PWM off time
#define pwmCount 50 // number of PWM cycles

//set constraints - attiny 1614 pinout
const int blue = 0;
const int red = 1;
const int green = 2;
const int black = 3;
const int button = 9;
const int blueLed = 10;

//set variables
int blueLedState = 0; // blue Led starts off
int reading; //read button state
int lastRead = 1; //sets previous state of the button to 
int stepsPerRead = 50; //n steps per ToF reading
int numReads = 10;  //number of readings performed
const float mmPerStep = 0.8; //mm advanced per step cycle sequence (4 steps)
float total; //total sum of readings, for avg calc
float avgReading; //average of 10 readings to send data
float forwardX; //distance travelled by device
// The following variables are longs because the time is measured in milliseconds, and will be a huge number
long time = 0; // last time the output pin was changed
long debounce = 200; // debounce time


void setup() {
  // put your setup code here, to run once:
  pinMode(blue, OUTPUT);
  // set pin modes
  pinMode(red, OUTPUT);
  pinMode(green, OUTPUT);
  pinMode(black, OUTPUT);
  pinMode(blueLed, OUTPUT);
  pinMode(button, INPUT);
  Serial.begin(115200); // start serial communication to send read data
  Wire.begin(); // start I2C protocol, VL53L1X to AT Tiny 1614
  Wire.setClock(400000); //Use 400 kHz for I2C

   // set sensor timeout and warning message
  sensor.setTimeout(500);
  if (!sensor.init()) {
    Serial.println("Failed to detect and initialize sensor!");
    while (1);
  }
  // Establish short distance mode, and establish a 140 ms time budget (minimum is 20 ms for short distance mode, refer to datasheet)
  sensor.setDistanceMode(VL53L1X::Short);
  sensor.setMeasurementTimingBudget(140000);
  // Start continous readings at a rate of one measurement every 140ms.
  sensor.startContinuous(140);
}

void ApBp() {
  digitalWrite(red, LOW);
  digitalWrite(black,LOW);
  for (int count = 0; count < pwmCount; ++count){
    digitalWrite(blue, HIGH);
    digitalWrite(green, HIGH);
    onDelay();
    digitalWrite(blue, LOW);
    digitalWrite(green, LOW);
    offDelay();
  }
}

void AmBp() {
  digitalWrite(blue, LOW);
  digitalWrite(black, LOW);
  for (int count = 0; count < pwmCount; ++count){
    digitalWrite(red, HIGH);
    digitalWrite(green, HIGH);
    onDelay();
    digitalWrite(red, LOW);
    digitalWrite(green, LOW);
    offDelay();
  }
}

void AmBm() {
  digitalWrite(blue, LOW);
  digitalWrite(green, LOW);
  for (int count = 0; count < pwmCount; ++count){
    digitalWrite(red, HIGH);
    digitalWrite(black, HIGH);
    onDelay();
    digitalWrite(red, LOW);
    digitalWrite(black, LOW);
    offDelay();
  }
}

void ApBm() {
  digitalWrite(red, LOW);
  digitalWrite(green, LOW);
  for (int count = 0; count < pwmCount; ++count){
    digitalWrite(blue, HIGH);
    digitalWrite(black, HIGH);
    onDelay();
    digitalWrite(blue, LOW);
    digitalWrite(black, LOW);
    offDelay();
  }
}
void fabAc() {
  while(1) {
    triage();
    demandSideTimeMng();
    spiralDev();
    bottomUpDebug();
    modularity();
    docAsYouGo();
  }
}

void stepperOff(){
  digitalWrite(blue, LOW);
  digitalWrite(red, LOW);
  digitalWrite(green, LOW);
  digitalWrite(black, LOW);
}

void switchButton() {
  reading = digitalRead(button);
  if (reading == HIGH && lastRead == LOW && millis()-time > debounce){ //reading==HIGH --> button pressed; millis90-time --> avoids false reads
      if (blueLedState == HIGH) { // if the blue LED was ON...
          blueLedState = LOW;   //... turn it OFF
          forwardX = 0; // reset X counter
      }
      else {// if it was not on, it was OFF, so...
          blueLedState = HIGH;    //...turn it ON
      }
      time = millis(); //set time to actual time in milliseconds
  }
  digitalWrite(blueLed, blueLedState); // set LED state, ON or OFF
  lastRead = reading;//once the button is released, lastRead goes back to LOW
}

void loop() {
  // put your main code here, to run repeatedly:  
    switchButton();
    if (blueLedState){ //if the LED is turned on
      total = 0;
      for(int i=0; i<numReads || blueLedState == LOW; i++){
          total = total + sensor.read();
          switchButton();
        }
      avgReading = total/numReads;
      forwardX = forwardX+mmPerStep * stepsPerRead;
      Serial.print(forwardX);
      Serial.print(",");
      Serial.print(avgReading);
      if (sensor.timeoutOccurred()){
        Serial.print("Achtung!!! Timeout");
        }
      Serial.println();
      for (int a = 0; a<stepsPerRead || blueLedState == LOW; a++){
          ApBp();
          AmBp();
          AmBm();
          ApBm();
          switchButton();
          }        
    }
    else{
      stepperOff();
      switchButton();
      }
    switchButton();  
    _delay_ms(100);
}