#include "HX711.h"
#include <LiquidCrystal_I2C.h>
#include <SoftwareSerial.h>

SoftwareSerial Serial1(PB0, PB1);
LiquidCrystal_I2C lcd(0x27, 20, 4); 

// RX 0
// TX 1
#define pulse1 2
#define pulse2 3
#define pulse3 4
#define pulse4 5
#define ena 6
#define manual_1 7
#define manual_2 8
#define manual_3 9
#define manual_4 10
#define txEnable 11
// #define *variable* 12
const int loadcellSCK = 13;
const int Dout_1 = 14;
const int Dout_2 = 15;
const int Dout_3 = 16;
const int Dout_4 = 17;

// dosing pump variables
int uSec = 1250;
long pulPerDose_1 = 2000;
long pulPerDose_2 = 2000;
long pulPerDose_3 = 2000;
long pulPerDose_4 = 2000;
int seq;
int last;
int w;

// Serial communication
int address = 101;
int commandStart = 1200;
int commandStop = 1300;
int responseStart = 1250;
int responseStop = 1350;
int responseError = 1500;
int command;
int response;

// loadcells 
HX711 scale1;
HX711 scale2;
HX711 scale3;
HX711 scale4;
float reading1;
float reading2;
float reading3;
float reading4;
float calibration1 = 420;
float calibration2 = 420;
float calibration3 = 420;
float calibration4 = 420;

// Timer_1 variables
unsigned long currentMillis;
int trigger;

// Timer_2 variables
const int duration_2 = 10000; // milliseconds
const int duration_3 = 10000;  // milliseconds
unsigned long offset_2;
int screen;


void setup() {
  
  pinMode(ena,OUTPUT); 
  pinMode(pulse1,OUTPUT);
  pinMode(pulse2,OUTPUT);
  pinMode(pulse3,OUTPUT);
  pinMode(pulse4,OUTPUT);
  pinMode(txEnable,OUTPUT);
  pinMode(manual_1, INPUT_PULLUP);
  pinMode(manual_2, INPUT_PULLUP);
  pinMode(manual_3, INPUT_PULLUP);
  pinMode(manual_4, INPUT_PULLUP);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0,0);
  lcd.print("**initializing**");
  lcd.setCursor(0,1);
  lcd.print(">>>>>");
  delay(1000);

  Serial1.begin(9600);
  Serial1.println("Working...");
  scale1.begin(Dout_1, loadcellSCK);
  scale1.set_scale(calibration1);
  scale2.begin(Dout_2, loadcellSCK);
  scale2.set_scale(calibration2);
  scale3.begin(Dout_3, loadcellSCK);
  scale3.set_scale(calibration3);
  scale4.begin(Dout_4, loadcellSCK);
  scale4.set_scale(calibration4);
  lcd.setCursor(0,1);
  lcd.print(">>>>>>>>>>");
  delay(1000);
  
  digitalWrite(ena, HIGH);
  digitalWrite(txEnable, LOW);
  lcd.setCursor(0,1);
  lcd.print(">>>>>>>>>>>>>>>");
  delay(1000);

  seq = 1;
  last = 1;
  offset_2 = millis();
  lcd.setCursor(0,1);
  lcd.print(">>>>>>>>>>>>>>>>>>>>");
  delay(1000);
}

void loop() {
  // alarm condition
  while(seq == 0) {
    getReadings();
    updateDisplay();
    timer_2();
    if(trigger == 1) {
      digitalWrite(txEnable, HIGH);
      Serial1.println((address + responseStop));
      digitalWrite(txEnable, LOW);
      trigger = 0;
    }
    if(Serial1.available()) {
      command = Serial1.parseInt();
    }
    if((command - address) == commandStart) {
      if(last == 1) {
        seq = 11;
      }
      else {
        delay(1000);
        digitalWrite(txEnable, HIGH);
        Serial1.println((address + responseStart));
        digitalWrite(txEnable, LOW);
        seq = last;
      }
    }
    if((command - address) == commandStop) {
      seq = 0;
      trigger = 1;
    }
  }

  // normal state
  while( seq == 1) {
    last = 1;
    getReadings();
    updateDisplay();
    timer_2();
    if(digitalRead(manual_1) == LOW ||
      digitalRead(manual_2) == LOW ||
      digitalRead(manual_3) == LOW ||
      digitalRead(manual_4) == LOW) {
        seq = 20;
    }
    if(Serial1.available()) {
      command = Serial1.parseInt();
    }
    if((command - address) == commandStart) {
      if(last == 1) {
        seq = 11;
      }
      else {
        seq = last;
      }
    }
    if((command - address) == commandStop) {
      seq = 0;
      trigger = 1;
    }
  }

  // Dosing pump 1
  while(seq == 11) {
    last = 12;
    screen = 2;
    digitalWrite(txEnable, HIGH);
    Serial1.println((address + responseStart));
    digitalWrite(txEnable, LOW);
    delay(1000);
    digitalWrite(ena, LOW);
    updateDisplay();  getReadings(); 
    for(long x = 0; x < pulPerDose_1; x++) {
      digitalWrite(pulse1,HIGH); 
      delayMicroseconds(uSec);    
      digitalWrite(pulse1,LOW); 
      delayMicroseconds(uSec); 
      if(Serial1.available()) {
        getReadings();
        timer_2(); 
        updateDisplay();
        command = Serial1.parseInt();
        if((command - address) == commandStop) {
          seq = 0;
          break;
        }
      }
    }
    delay(500);
    digitalWrite(ena, HIGH);
    if(seq == 0) {
      seq = 0;
      trigger = 1;
    }
    else {
      seq = 12;
    }
  }

  // Dosing pump 2
  while(seq == 12) {
    last = 13;
    screen = 2;
    delay(1000);
    digitalWrite(ena, LOW);
    updateDisplay();  getReadings(); 
    for(long x = 0; x < pulPerDose_2; x++) {
      digitalWrite(pulse2,HIGH); 
      delayMicroseconds(uSec);    
      digitalWrite(pulse2,LOW); 
      delayMicroseconds(uSec); 
      if(Serial1.available()) {
        getReadings();
        timer_2(); 
        updateDisplay();
        command = Serial1.parseInt();
        if((command - address) == commandStop) {
          seq = 0;
          break;
        }
      }
    }
    delay(500);
    digitalWrite(ena, HIGH);
    if(seq == 0) {
      seq = 0;
      trigger = 1;
    }
    else {
      seq = 13;
    }
  }

  // Dosing pump 3
  while(seq == 13) {
    last = 14;
    screen = 2;
    delay(1000);
    digitalWrite(ena, LOW);
    updateDisplay();  getReadings(); 
    for(long x = 0; x < pulPerDose_3; x++) {
      digitalWrite(pulse3,HIGH); 
      delayMicroseconds(uSec);    
      digitalWrite(pulse3,LOW); 
      delayMicroseconds(uSec); 
      if(Serial1.available()) {
        getReadings();
        timer_2(); 
        updateDisplay();
        command = Serial1.parseInt();
        if((command - address) == commandStop) {
          seq = 0;
          break;
        }
      }
    }
    delay(500);
    digitalWrite(ena, HIGH);
    if(seq == 0) {
      seq = 0;
      trigger = 1;
    }
    else {
      seq = 14;
    }
  }

  // Dosing pump 4
  while(seq == 14) {
    last = 1;
    screen = 2;
    delay(1000);
    digitalWrite(ena, LOW);
    updateDisplay();  getReadings(); 
    for(long x = 0; x < pulPerDose_4; x++) {
      digitalWrite(pulse4,HIGH); 
      delayMicroseconds(uSec);    
      digitalWrite(pulse4,LOW); 
      delayMicroseconds(uSec); 
      if(Serial1.available()) {
        getReadings();
        timer_2(); 
        updateDisplay();
        command = Serial1.parseInt();
        if((command - address) == commandStop) {
          seq = 0;
          break;
        }
      }
    }
    delay(500);
    digitalWrite(ena, HIGH);
    if(seq == 0) {
      seq = 0;
      trigger = 1;
    }
    else {
      digitalWrite(txEnable, HIGH);
      Serial1.println((address + responseStop));
      digitalWrite(txEnable, LOW);
      seq = 1;
    }
  }

  // manual opperation
  while(seq == 20) {
    last = 1;
    if(digitalRead(manual_1) == LOW || digitalRead(manual_2) == LOW || digitalRead(manual_3) == LOW || digitalRead(manual_4) == LOW) {
        seq = 20;
        digitalWrite(ena, LOW);
    }
    else {
      seq = last;
    }
    while(digitalRead(manual_1) == LOW) {
      delay(10);
      w = 21; timer_2(); updateDisplay();  getReadings();
      for(long x = 0; x < 100; x++) {
        digitalWrite(pulse1,HIGH); 
        delayMicroseconds(uSec * 4);    
        digitalWrite(pulse1,LOW); 
        delayMicroseconds(uSec * 4); 
      }
      if(digitalRead(manual_1) == HIGH) {
        digitalWrite(ena, HIGH);
      }
    }
    while(digitalRead(manual_2) == LOW) {
      delay(10);
      w = 22; timer_2(); updateDisplay();  getReadings();
      for(long x = 0; x < 100; x++) {
        digitalWrite(pulse2,HIGH); 
        delayMicroseconds(uSec * 4);    
        digitalWrite(pulse2,LOW); 
        delayMicroseconds(uSec * 4); 
      }
      if(digitalRead(manual_2) == HIGH) {
        digitalWrite(ena, HIGH);
      }
    }
    while(digitalRead(manual_3) == LOW) {
      delay(10);
      w = 23; timer_2(); updateDisplay();  getReadings();
      for(long x = 0; x < 100; x++) {
        digitalWrite(pulse3,HIGH); 
        delayMicroseconds(uSec * 4);    
        digitalWrite(pulse3,LOW); 
        delayMicroseconds(uSec * 4); 
      }
      if(digitalRead(manual_3) == HIGH) {
        digitalWrite(ena, HIGH);
      }
    }
    while(digitalRead(manual_4) == LOW) {
      delay(10);
      w = 24; timer_2(); updateDisplay();  getReadings();
      for(long x = 0; x < 100; x++) {
        digitalWrite(pulse4,HIGH); 
        delayMicroseconds(uSec * 4);    
        digitalWrite(pulse4,LOW); 
        delayMicroseconds(uSec * 4); 
      }
      if(digitalRead(manual_4) == HIGH) {
        digitalWrite(ena, HIGH);
      }
    }

  }
}

void updateDisplay() {
  // loadcells
  if(screen == 1){
    lcd.setCursor(0,0);
    lcd.print("Reagent 1: ");
    if(reading1 < 10) {
      lcd.print("0");
      lcd.print(reading1);
      lcd.print("kg");
      lcd.print("  ");
    }
    if(reading1 >= 10) {
      lcd.print(reading1);
      lcd.print("kg");
      lcd.print("  ");
    }

    lcd.setCursor(0,1);
    lcd.print("Reagent 2: ");
    if(reading2 < 10) {
      lcd.print("0");
      lcd.print(reading2);
      lcd.print("kg");
      lcd.print("  ");
    }
    if(reading2 >= 10) {
      lcd.print(reading2);
      lcd.print("kg");
      lcd.print("  ");
    }

    lcd.setCursor(0,2);
    lcd.print("Reagent 3: ");
    if(reading3 < 10) {
      lcd.print("0");
      lcd.print(reading3);
      lcd.print("kg");
      lcd.print("  ");
    }
    if(reading3 >= 10) {
      lcd.print(reading3);
      lcd.print("kg");
      lcd.print("  ");
    }

    lcd.setCursor(0,3);
    lcd.print("Reagent 4: ");
    if(reading4 < 10) {
      lcd.print("0");
      lcd.print(reading4);
      lcd.print("kg");
      lcd.print("  ");
    }
    if(reading4 >= 10) {
      lcd.print(reading4);
      lcd.print("kg");
      lcd.print("  ");
    }
  }
  // pumps
  if(screen == 2) {
    if(seq == 1 || seq == 0) {
      lcd.setCursor(0,0);
      lcd.print("Pump 1 STOP         ");
      lcd.setCursor(0,1);
      lcd.print("Pump 2 STOP         ");
      lcd.setCursor(0,2);
      lcd.print("Pump 3 STOP         ");
      lcd.setCursor(0,3);
      lcd.print("Pump 4 STOP         ");
    }
    if(seq == 11) {
      lcd.setCursor(0,0);
      lcd.print("Pump 1 RUN          ");
      lcd.setCursor(0,1);
      lcd.print("Pump 2 STOP         ");
      lcd.setCursor(0,2);
      lcd.print("Pump 3 STOP         ");
      lcd.setCursor(0,3);
      lcd.print("Pump 4 STOP         ");
    }
    if(seq == 12) {
      lcd.setCursor(0,0);
      lcd.print("Pump 1 STOP         ");
      lcd.setCursor(0,1);
      lcd.print("Pump 2 RUN          ");
      lcd.setCursor(0,2);
      lcd.print("Pump 3 STOP         ");
      lcd.setCursor(0,3);
      lcd.print("Pump 4 STOP         ");
    }
    if(seq == 13) {
      lcd.setCursor(0,0);
      lcd.print("Pump 1 STOP         ");
      lcd.setCursor(0,1);
      lcd.print("Pump 2 STOP         ");
      lcd.setCursor(0,2);
      lcd.print("Pump 3 RUN          ");
      lcd.setCursor(0,3);
      lcd.print("Pump 4 STOP         ");
    }
    if(seq == 14) {
      lcd.setCursor(0,0);
      lcd.print("Pump 1 STOP         ");
      lcd.setCursor(0,1);
      lcd.print("Pump 2 STOP         ");
      lcd.setCursor(0,2);
      lcd.print("Pump 3 STOP         ");
      lcd.setCursor(0,3);
      lcd.print("Pump 4 RUN          ");
    }
    if(seq == 20) {
      if(w == 21) {
        lcd.setCursor(0,0);
        lcd.print("Pump 1 JOG          ");
        lcd.setCursor(0,1);
        lcd.print("Pump 2 STOP         ");
        lcd.setCursor(0,2);
        lcd.print("Pump 3 STOP         ");
        lcd.setCursor(0,3);
        lcd.print("Pump 4 STOP         ");
      }
      if(w == 22) {
        lcd.setCursor(0,0);
        lcd.print("Pump 1 STOP         ");
        lcd.setCursor(0,1);
        lcd.print("Pump 2 JOG          ");
        lcd.setCursor(0,2);
        lcd.print("Pump 3 STOP         ");
        lcd.setCursor(0,3);
        lcd.print("Pump 4 STOP         ");
      }
      if(w == 23) {
        lcd.setCursor(0,0);
        lcd.print("Pump 1 STOP         ");
        lcd.setCursor(0,1);
        lcd.print("Pump 2 STOP         ");
        lcd.setCursor(0,2);
        lcd.print("Pump 3 JOG          ");
        lcd.setCursor(0,3);
        lcd.print("Pump 4 STOP         ");
      }
      if(w == 24) {
        lcd.setCursor(0,0);
        lcd.print("Pump 1 STOP         ");
        lcd.setCursor(0,1);
        lcd.print("Pump 2 STOP         ");
        lcd.setCursor(0,2);
        lcd.print("Pump 3 STOP         ");
        lcd.setCursor(0,3);
        lcd.print("Pump 4 JOG          ");
      }
    }
  }
  // other
  if(screen == 3) {
    lcd.setCursor(0,0);
    lcd.print("3                   ");
    lcd.setCursor(0,1);
    lcd.print("3                   ");
    lcd.setCursor(0,2);
    lcd.print("3                   ");
    lcd.setCursor(0,3);
    lcd.print("3                   ");
  }
}

void getReadings() {
  reading1 = scale1.get_units();
  reading2 = scale2.get_units();
  reading3 = scale3.get_units();
  reading4 = scale4.get_units();
  
}

void timer_2() {
  currentMillis = millis();

  if(currentMillis < (offset_2 + duration_2)) {
    screen = 1;
  }
  if((offset_2 + duration_2) <= currentMillis && currentMillis < (offset_2 + (duration_2 * 2))) {
    screen = 2;
  }
  if((offset_2 + (duration_2 * 2)) <= currentMillis && currentMillis < (offset_2 + (duration_2 * 3))) {
    screen = 3;
  }
  if((offset_2 + (duration_2 * 3)) <= currentMillis) {
    offset_2 = currentMillis;
  }

  /* if(currentMillis >= (offset_2 + duration_2)) {
    if(currentMillis < (offset_2 + duration_2 + duration_3)) {
      screen = 2;
    }

    if(currentMillis >= (offset_2 + duration_2 + duration_3)) {
      offset_2 = currentMillis;
    }
  } */
}