int red_r=7;                      //Right Red Light Pin
int amber_r=6;                    //Right Amber Light Pin
int green_r=5;                    //Right Green Light Pin
int red_l=4;                      //Left Red Light Pin
int amber_l=3;                    //Left Amber Light Pin
int green_l=2;                    //Left Green Light Pin
int reset_button=16;              //Reset Button Pin A2
int begin_reading_button=17;      //begin Button Pin A3
int set_up_button=18;             //Set Up Button Pin A4   
int calibration_button=19;        //Calibration Button Pin A5

int pot_right=1;                  //Left Potentiometer Pin A1
int pot_left=0;                   //Right Potentiometer Pin A0

const int upper_dis = 3000;       const int lower_dis = 0;
float diff1_low = 5;              float diff1_med = 7.5;
float unb_low = 32.5;             float unb_med = 42.5;
float low_3 = 5;                  float med_3 = 7.5;
float diff1_0 = 0.0;              float diff1_1 = 0.0;
float diff2_0 = 0.0;              float diff2_1 = 0.0;
float adjust_3 = 0.0;             float adjust_4 = 0.0;
float pos1_0 = 0.0;               float pos1_1 = 0.0;
float pos2_0 = 0.0;               float pos2_1 = 0.0;
float pos3_0 = 0.0;               float pos3_1 = 0.0;
float pos4_0 = 0.0;               float pos4_1 = 0.0;

float unbalance = 0.0;
float known_length = 50;          //mm
float small = 0.0;                //determine which rotor is further away
float flash_time = 0.0;

bool shim_left;                   bool shim_right;
bool calibration_start = false;   
bool set_up_start = false;        bool set_up_restart = true;
bool set_up_complete = false;     bool begin_program = false;
bool move_back1; bool move_back2; //declare bools to determine which rotor needs to be moved back: left (true) or right (false)


const int trigPin_l = 13; // Trigger Pin of left Ultrasonic Sensor 
const int echoPin_l = 12; // Echo Pin of left Ultrasonic Sensor
const int trigPin_r = 9; // Trigger Pin of right Ultrasonic Sensor
const int echoPin_r = 8; // Echo Pin of right Ultrasonic Sensor
unsigned long time;
int s; int s0; int s1; int ms; int m; int m0; int m1;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600); 
  pinMode(red_l, OUTPUT);
  pinMode(amber_l, OUTPUT);
  pinMode(green_l, OUTPUT);
  pinMode(red_r, OUTPUT);
  pinMode(amber_r, OUTPUT);
  pinMode(green_r, OUTPUT); 

  pinMode(calibration_button, INPUT_PULLUP);      //reset button
  pinMode(set_up_button, INPUT_PULLUP);           //set up button
  pinMode(reset_button, INPUT_PULLUP);            //calibration button
  pinMode(begin_reading_button, INPUT_PULLUP);    //begin reading button
  calibration();
  set_up_seq();

  Serial.print("Time");           Serial.print("\t");
  Serial.print("Position 1");     Serial.print("\t");
  Serial.print("Postition 2");    Serial.print("\t");
  Serial.print("Difference 1");   Serial.print("\t");
  Serial.print("Displacement 1"); Serial.print("\t");
  Serial.print("Displacement 2"); Serial.print("\t");
  Serial.println("Difference 2"); 
}

void loop() {
  // put your main code here, to run repeatedly:
  delay(100);
  // ensure all lights are turned off - might change to have it during loops
  digitalWrite(red_l,LOW);
  digitalWrite(amber_l,LOW);
  digitalWrite(green_l,LOW);
  digitalWrite(red_r,LOW);
  digitalWrite(amber_r,LOW);
  digitalWrite(green_r,LOW);

  time = millis()/100;
  millisecondsToMSMS();
  Serial.print("\t");
  

  //need to read sensors 1 & ""
  pos1_1 = analogRead(pot_left);             // reads the value of the potentiometer (value between 0 and 1023)
  pos1_1 = map(pos1_1, 0, 1023, 0, 8000);     // maps the valuse of the potentiometer between the values of 0 to 80mm
  pos2_1 = analogRead(pot_right);            // reads the value of the potentiometer (value between 0 and 1023)
  pos2_1 = map(pos2_1, 0, 1023, 0, 8000);     // maps the valuse of the potentiometer between the values of 0 to 80mm
  
  if(pos1_1>=upper_dis && pos2_1>=upper_dis){
    diff1_1 = abs(pos1_1 - pos2_1);
    if (pos1_0!=pos1_1 || pos2_0!=pos2_1){ // need to change for calculated distances
      Serial.print(pos1_1); Serial.print("\t\t"); Serial.print(pos2_1); Serial.print("\t\t"); Serial.print(diff1_1); Serial.println("\t\t");
      pos1_0 = pos1_1;
      pos2_0 = pos2_1;
    }
    else {
      Serial.println("\t\t\t\t\t\t");
    }
    diff1_0 = diff1_1;
    if (pos1_1 <= pos2_1){
      move_back1 = true; //left rig moves back
    }
    else{
      move_back1 = false; //right rig moves back
    }
    if (diff1_1 <= diff1_low){
      digitalWrite(red_l,LOW); digitalWrite(amber_l,LOW); digitalWrite(green_l,HIGH);
      digitalWrite(red_r,LOW); digitalWrite(amber_r,LOW); digitalWrite(green_r,HIGH);
    }
    else if (diff1_1 <= diff1_med){
      if (move_back1 == true){
        digitalWrite(red_l,LOW); digitalWrite(amber_l,HIGH); digitalWrite(green_l,LOW); //amber left on
        digitalWrite(red_r,LOW); digitalWrite(amber_r,LOW); digitalWrite(green_r,HIGH); //green right on
      }
      else{
        digitalWrite(red_l,LOW); digitalWrite(amber_l,LOW); digitalWrite(green_l,HIGH); //green left on
        digitalWrite(red_r,LOW); digitalWrite(amber_r,HIGH); digitalWrite(green_r,LOW); //amber right on
      }
    }
    else{
      if(move_back1 == true){
        digitalWrite(red_l,HIGH); digitalWrite(amber_l,LOW); digitalWrite(green_l,LOW); //red left on
        digitalWrite(red_r,LOW); digitalWrite(amber_r,LOW); digitalWrite(green_r,HIGH); //green right on
      }
      else{
        digitalWrite(red_l,LOW); digitalWrite(amber_l,LOW); digitalWrite(green_l,HIGH); //green left on
        digitalWrite(red_r,HIGH); digitalWrite(amber_r,LOW); digitalWrite(green_r,LOW); //red right on
      }
    }
  }
  else{
    Serial.print("\t\t\t\t\t\t");
    pos3_1= (0.0344/2 * readUltrasonicDistance1(trigPin_l, echoPin_l)); //reading is in cm
    pos3_1 = map(pos3_1, 2, 400, 0, 3980);                              // converts reading to mm
    pos3_1 = pos3_1 - adjust_3;
    pos4_1= (0.0344/2 * readUltrasonicDistance2(trigPin_r, echoPin_r)); //reading is in cm
    pos4_1 = map(pos4_1, 2, 400, 0, 3980);                              // converts reading to mm
    pos4_1 = pos4_1 - adjust_4;

    if (pos3_1 <= pos4_1){
      small = pos3_1;
    }
    else{
      small = pos4_1;
    }
    if (small >= unb_med){
      unbalance = 0.5; //mm
    }
    else if (unb_low <= small <= unb_med){
      unbalance = 0.2; //mm
    }
    else if (small <= unb_low){
      unbalance = 0.05; //mm
    }
    else{
      Serial.println("************************************");
      Serial.println("ERROR - CONFLICT: reading is larger than allowed for US");
      Serial.println(pos3_1);
      Serial.println(pos4_1);
      Serial.println("************************************");
      flashing_red_light();
    }
    diff2_1 = abs(pos3_1 - pos4_1);
    if (pos3_0!=pos3_1 || pos4_0!=pos4_1){ // need to change for calculated distances
      Serial.print(pos3_1); Serial.print("\t\t"); Serial.print(pos4_1); Serial.print("\t\t"); Serial.print(diff2_1); Serial.println("\t\t");
      pos3_0 = pos3_1;
      pos4_0 = pos4_1;
    }
    else{
      Serial.println("\t\t\t\t\t\t");
    }
    diff2_0 = diff2_1;
    if (pos3_1 < lower_dis || pos4_1 < lower_dis){
      Serial.println("************************************");
      Serial.println("ERROR - TOO CLOSE: reading is closer than possible");
      Serial.println(pos3_1);
      Serial.println(pos4_1);
      Serial.println("************************************");
      flashing_red_light();
    }
    if (pos3_1 <= pos4_1){
      move_back2 = true; //left rig moves back
    }
    else{
      move_back2 = false; //right rig moves back
    }
    if (diff2_1 <= unbalance){
      digitalWrite(red_l,LOW); digitalWrite(amber_l,LOW); digitalWrite(green_l,HIGH);
      digitalWrite(red_r,LOW); digitalWrite(amber_r,LOW); digitalWrite(green_r,HIGH);
    }
    else if (diff2_1 <= 1.5*unbalance){
      if (move_back2 == true){
        digitalWrite(red_l,LOW); digitalWrite(amber_l,HIGH); digitalWrite(green_l,LOW); //amber left on
        digitalWrite(red_r,LOW); digitalWrite(amber_r,LOW); digitalWrite(green_r,HIGH); //green right on
      }
      else{
        digitalWrite(red_l,LOW); digitalWrite(amber_l,LOW); digitalWrite(green_l,HIGH); //green left on
        digitalWrite(red_r,LOW); digitalWrite(amber_r,HIGH); digitalWrite(green_r,LOW); //amber right on
      }
    }
    else{
      if(move_back2 == true){
        digitalWrite(red_l,HIGH); digitalWrite(amber_l,LOW); digitalWrite(green_l,LOW); //red left on
        digitalWrite(red_r,LOW); digitalWrite(amber_r,LOW); digitalWrite(green_r,HIGH); //green right on
      }
      else{
        digitalWrite(red_l,LOW); digitalWrite(amber_l,LOW); digitalWrite(green_l,HIGH); //green left on
        digitalWrite(red_r,HIGH); digitalWrite(amber_r,LOW); digitalWrite(green_r,LOW); //red right on
      }
    }
  }
}




long readUltrasonicDistance1(int trigPin_l, int echoPin_l)
{
	pinMode(trigPin_l, OUTPUT);  // Clear the trigger
	digitalWrite(trigPin_l, LOW);
	delay(2);
	// Sets the trigger pin to HIGH state for 10 microseconds
	digitalWrite(trigPin_l, HIGH);
	delay(10);
	digitalWrite(trigPin_l, LOW);
	pinMode(echoPin_l, INPUT);
	// Reads the echo pin, and returns
	// the sound wave travel time in microseconds
	return pulseIn(echoPin_l, HIGH);
}

long readUltrasonicDistance2(int trigPin_r, int echoPin_r)
{
	pinMode(trigPin_r, OUTPUT);  // Clear the trigger
	digitalWrite(trigPin_r, LOW);
	delay(2);
	// Sets the trigger pin to HIGH state for 10 microseconds
	digitalWrite(trigPin_r, HIGH);
	delay(10);
	digitalWrite(trigPin_r, LOW);
	pinMode(echoPin_r, INPUT);
	// Reads the echo pin, and returns
	// the sound wave travel time in microseconds
	return pulseIn(echoPin_r, HIGH);
}

void millisecondsToMSMS(){
  uint32_t t = time;

    ms = t % 10;
    t=(t-ms)/10;

    s = t % 60;
    s0 = s % 10;
    s1 = s/10;

    t = (t - s)/60;
    m = t % 60;
    m0 = m % 10;
    m1 = m/10;
    Serial.print(m1);Serial.print(m0);Serial.print(":");Serial.print(s1);Serial.print(s0);Serial.print(".");Serial.print(ms);
    return;
}

//create Calibration function
void calibration() {
  Serial.println("CALIBRATING - Insert sensor guides, press BLUE button when complete");
  while (calibration_start == false){
    if (digitalRead(calibration_button) == LOW){
      calibration_start = true;
    }
  }
  delay(300);

  //*********************************************************************************************
  pos3_0 = (0.0344/2 * readUltrasonicDistance1(trigPin_l, echoPin_l)); //reading is in cm
  pos3_0 = map(pos3_0, 2, 400, 0, 3980); // converts reading to mm
  pos4_0 = (0.0344/2 * readUltrasonicDistance2(trigPin_r, echoPin_r));
  pos4_0 = map(pos4_0, 2, 400, 0, 3980);
  if (pos3_0 != known_length){
    adjust_3 = pos3_0 - known_length; // mm
    Serial.print("adjust_3 \t");
    Serial.println(adjust_3);
  }
  if (pos4_0 != known_length){
    adjust_4 = pos4_0 - known_length;
    Serial.print("adjust_4 \t");
    Serial.println(adjust_4);
  }
  flash_time = 0;
  while (flash_time <= 2 && digitalRead(calibration_button) == HIGH){
    digitalWrite(red_l,0); digitalWrite(amber_l,0); digitalWrite(green_l,255);
    digitalWrite(red_r,0); digitalWrite(amber_r,0); digitalWrite(green_r,255);
    delay(500);
    digitalWrite(green_l, 0);
    digitalWrite(green_r, 0);
    delay(500);
    flash_time = flash_time+1;
  }
  Serial.println("Remove block guide pieces");
  delay(1000);
}

void set_up_seq(){
  Serial.println("SETTING UP - Insert stator guide, close the rig until it stops, press the ORANGE button to start reading");
  while (set_up_restart == true){
    while (set_up_start == false){
      if (digitalRead(set_up_button) == LOW){
        set_up_start = true;
      }
    }
    while (set_up_complete == false && set_up_start == true){
      pos3_0= (0.0344/2 * readUltrasonicDistance1(trigPin_l, echoPin_l)); //reading is in cm
      pos3_0 = map(pos3_0, 2, 400, 0, 3980);                             // converts reading to mm
      pos3_1 = pos3_0 - adjust_3;
      pos3_0 = pos3_1;
      pos4_0= (0.0344/2 * readUltrasonicDistance2(trigPin_r, echoPin_r)); //reading is in cm
      pos4_0 = map(pos4_0, 2, 400, 0, 3980);                             // converts reading to mm
      pos4_1 = pos4_0 - adjust_4;
      pos4_0 = pos4_1;

      if (pos3_1 == 0){
        shim_left = false;
      }
      else if (pos3_1 > 0){
        shim_left = true;
      }
      else{
        Serial.println("************************************");
        Serial.println(pos3_1);
        Serial.println(pos4_1);
        Serial.println("ERROR IN CALIBRATION (A) - pos 3 too small"); // pos3_1 must be less than 0 - not possible
        Serial.println("************************************");
        flashing_red_light();
        calibration();
      }
      if (pos4_1 == 0){
        shim_right = false;
      }
      else if (pos4_1 > 0){
        shim_right = true;
      }
      else{
        Serial.println("************************************");
        Serial.println("ERROR IN CALIBRATION (B) - pos 4 too small");
        Serial.println(pos3_1);
        Serial.println(pos4_1);
        Serial.println("************************************");
        flashing_red_light();
        calibration();
      }
      if (shim_left == true){
        if (shim_right == true){
          Serial.println("************************************");
          Serial.println("ERROR IN CALIBRATION (C) - both side need shims - should only be one");
          Serial.println(pos3_1);
          Serial.println(pos4_1);
          Serial.println("************************************");
          flashing_red_light();
          calibration();
        }
        else{
          digitalWrite(red_l,LOW); digitalWrite(amber_l,HIGH); digitalWrite(green_l,LOW);
          digitalWrite(red_r,LOW); digitalWrite(amber_r,LOW); digitalWrite(green_r,HIGH);
          Serial.print("Add left shim of width \t");
          Serial.print(pos3_1);
          Serial.println("mm");
          delay(500);
          set_up_start = false;
          Serial.println("Close the rig until it stops, press the ORANGE button when complete");
          
            //delay?
        }
      }
      else{
        if (shim_right == true){
          digitalWrite(red_l,LOW); digitalWrite(amber_l,LOW); digitalWrite(green_l,HIGH);
          digitalWrite(red_r,LOW); digitalWrite(amber_r,HIGH); digitalWrite(green_r,LOW);
          Serial.print("Add right shim of width \t");
          Serial.print(pos4_1);
          Serial.println("mm");
          delay(500);
          set_up_start = false;
          Serial.println("Close the rig until it stops, press the ORANGE button when complete");
          
        }
        else{
          digitalWrite(red_l,LOW); digitalWrite(amber_l,LOW); digitalWrite(green_l,HIGH);
          digitalWrite(red_r,LOW); digitalWrite(amber_r,LOW); digitalWrite(green_r,HIGH);
          delay(5000);
          digitalWrite(green_l, LOW);
          digitalWrite(green_r, LOW);
          set_up_complete = true;
          set_up_restart = false;
        }
      }
    }
  }
  Serial.println("SET UP COMPLETE- Insert stator, press the GREEN button to start reading");
  while (begin_program == false){
    if (digitalRead(begin_reading_button) == LOW){
      begin_program = true;
    }
    delay(200);
  }
}

void flashing_red_light(){
  while (digitalRead(reset_button) == HIGH){
    digitalWrite(red_l,HIGH); digitalWrite(amber_l,LOW); digitalWrite(green_l,LOW);
    digitalWrite(red_r,HIGH); digitalWrite(amber_r,LOW); digitalWrite(green_r,LOW);
    delay(500);
    digitalWrite(red_l,LOW);
    digitalWrite(red_r,LOW);
    delay(500);
  }
}