#include "HX711_ADC.h"

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

int load_cell_DT=11;              // Load cell
int load_cell_SCK=10;

int left_light = 0;   int left_light_1 = 0;   int left_light_2 = 0;   int left_light_3 = 0;
int right_light = 0;  int right_light_1 = 0;  int right_light_2 = 0;  int right_light_3 = 0;

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 diff3_0 = 0.0;              float diff3_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 load5_0 = 0.0;              float load5_1 = 0.0;
//float load6_0 = 0; float load6_1 = 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;
const float div_1 = 21000.0/50.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; bool move_back3; //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;

HX711_ADC LoadCell(load_cell_DT, load_cell_SCK);
LoadCell.begin();

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(pot_left, INPUT); //left potentiometer
  //pinMode(pot_right, INPUT); //right potentiometer
  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
  //HX711_ADC LoadCell(load_cell_DT, load_cell_SCK);
  //LoadCell.begin();
  calibration();
  set_up_seq();
  time = millis();
  Serial.print("Load");         Serial.print("\t");
  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.print("Difference 2");   Serial.println(); 
   
}

void loop() {
  // put your main code here, to run repeatedly:
  delay(1000);
  
  time_total = millis()/1000;
  Serial.print(time_total); Serial.print("\t");
  
  read_load_cell();
  Serial.print(load5_1);
  if (load5_0!=load5_1){ // need to change for calculated distances
    Serial.print(load5_1); Serial.print("\t\t");
    load5_0 = load5_1;
  }
  else {
    Serial.println("\t\t");
  }
  if (load5_1 < 0){
      move_back3 = true; //left rig moves back
  }
  else{
    move_back3 = false; //right rig moves back
  }
  if (abs(load5_1) <= low_3){
    left_light_3 = 1; right_light_3 = 1; //both lights are green
  }
  else if (abs(load5_1) <= med_3){
    if (move_back3 == true){
      left_light_3 = 3; right_light_3 = 1; //amber left, green right 
    }
    else{
      left_light_3 = 1; right_light_3 = 3; //green left, amber right 
    }
  }
  else{
    if(move_back3 == true){
      left_light_3 = 7; right_light_3 = 1; //red left, green right
    }
    else{
      left_light_3 = 1; right_light_3 = 7; //green left, red right 
    }
  }


  //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 (move_back1 != move_back3){
      flashing_red_light();
    }
    if (diff1_1 <= diff1_low){
      left_light_1 = 1; right_light_1 = 1; //both lights are green
    }
    else if (diff1_1 <= diff1_med){
      if (move_back1 == true){
        left_light_1 = 3; right_light_1 = 1; //amber left, green right
      }
      else{
        left_light_1 = 1; right_light_1 = 3; //green left, amber right 
      }
    }
    else{
      if(move_back1 == true){
        left_light_1 = 7; right_light_1 = 1; //red left, green right
      }
      else{
        left_light_1 = 1; right_light_1 = 7; //green left, red right 
      }
    }
    left_light = left_light_1 + left_light_3;
    if (left_light ==2){
      digitalWrite(red_l,LOW); digitalWrite(amber_l,LOW); digitalWrite(green_l,HIGH); //green right on
    }
    else if ( left_light == 4 || left_light == 6){
      digitalWrite(red_l,LOW); digitalWrite(amber_l,HIGH); digitalWrite(green_l,LOW); //amber left on
    }
    else if (left_light == 8 || left_light == 10 || left_light == 14){
      digitalWrite(red_l,HIGH); digitalWrite(amber_l,LOW); digitalWrite(green_l,LOW); //red left on
    }
    else{
      flashing_red_light();
    }
    right_light = right_light_1 + right_light_3;
    if (right_light ==2){
      digitalWrite(red_r,LOW); digitalWrite(amber_r,LOW); digitalWrite(green_r,HIGH); //green right on
    }
    else if ( right_light == 4 || right_light == 6){
      digitalWrite(red_r,LOW); digitalWrite(amber_r,HIGH); digitalWrite(green_r,LOW); //amber right on
    }
    else if (right_light == 8 || right_light == 10 || right_light == 14){
      digitalWrite(red_r,HIGH); digitalWrite(amber_r,LOW); digitalWrite(green_r,LOW); //red right on
    }
    else{
      flashing_red_light();
    }

//******************************************************AMENDED TO THIS POINT******************************************************//











  }
  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_back1 = true; //left rig moves back
    }
    else{
      move_back1 = 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_1, int echoPin_1)
{
	pinMode(trigPin_1, OUTPUT);  // Clear the trigger
	digitalWrite(trigPin_1, LOW);
	delay(2);
	// Sets the trigger pin to HIGH state for 10 microseconds
	digitalWrite(trigPin_1, HIGH);
	delay(10);
	digitalWrite(trigPin_1, LOW);
	pinMode(echoPin_1, INPUT);
	// Reads the echo pin, and returns
	// the sound wave travel time in microseconds
	return pulseIn(echoPin_1, HIGH);
}

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

void read_load_cell(){
  //LoadCell.begin();
  load5_1 = LoadCell.getData();
  load5_1 = load5_0/div_1;
  load5_1 = load5_1 - 25.0;
  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,LOW); digitalWrite(amber_l,LOW); digitalWrite(green_l,HIGH);
    digitalWrite(red_r,LOW); digitalWrite(amber_r,LOW); digitalWrite(green_r,HIGH);
    delay(500);
    digitalWrite(green_l, LOW);
    digitalWrite(green_r, LOW);
    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;

      //pos3_0 = 0.0344/2 * readUltrasonicDistance1(trigPin_l, echoPin_l);
      //pos4_0 = 0.0344/2 * readUltrasonicDistance2(trigPin_r, echoPin_r);
      //pos3_1 = pos3_0 - adjust_3;
      //pos4_1 = pos4_0 - adjust_4;

      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();
      }
      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();
      }
      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();
        }
        else{
          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 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,HIGH); digitalWrite(green_l,LOW);
          digitalWrite(red_r,LOW); digitalWrite(amber_r,LOW); digitalWrite(green_r,HIGH);
          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);
  }
}