#include <LiquidCrystal_I2C.h>  // LiquidCrystal I2C - Version: 1.1.1
#include <Wire.h>
#include <elapsedMillis.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);

#define start 2
#define stop 3
#define up_limit 4
#define down_limit 5
#define estop 6
#define up 7
#define down 8
#define run 9
#define select 10
#define time_up 11
#define time_down 12
#define reset 13

int strt = 0;
int stp = 0;
int u_lim = 0;
int d_lim = 0;
int estp = 0;
int sel = 0;
int t_up = 0;
int t_down = 0;
int rst = 0;

int a = 0;
int b = 0;
int c = 0;
int d = 0;
int e = 0; //fault flag
int f = 0;
int g = 0;
int h = 0;
int i = 0;
int j = 0;
int ready_flag = 0;

int head_down = 0;
int head_up = 0;
int bowl_run = 0;
int spiral_run = 0;
int initialise = 0;
int cycle_running = 0;
int estp_flag = 0;

int down_timeout = 3500;
int up_timeout = 3500;
unsigned long start_time = 0;
unsigned long up_timer = 0;
unsigned long down_timer = 0;




void setup() {

  pinMode(start, INPUT_PULLUP);
  pinMode(stop, INPUT_PULLUP);
  pinMode(up_limit, INPUT_PULLUP);
  pinMode(down_limit, INPUT_PULLUP);
  pinMode(estop, INPUT_PULLUP);
  pinMode(select, INPUT_PULLUP);
  pinMode(time_up, INPUT_PULLUP);
  pinMode(time_down, INPUT_PULLUP);
  pinMode(reset, INPUT_PULLUP);
  pinMode(up, OUTPUT);
  pinMode(down, OUTPUT);
  pinMode(run, OUTPUT);


  lcd.init();                   // initialize the lcd
  lcd.backlight();              // turns on the backlight
  lcd.setCursor(0, 0);          // set cursor to first row
  lcd.print("Mixer");  // print out to LCD
  lcd.setCursor(0, 1);          // set cursor to first row
  lcd.print("Control");        // print out to LCD
  delay(1000);
  lcd.clear();

  Serial.begin(9600);
}

void loop() {

  // Mixer ready should only appaer if the head is up and all flags are 0'd
  // limit off timers should be inplace, so the mixer does not sit on the limit too long.

  strt = digitalRead(start);
  stp = digitalRead(stop);
  u_lim = digitalRead(up_limit);
  d_lim = digitalRead(down_limit);
  estp = digitalRead(estop);
  sel = digitalRead(select);
  t_up = digitalRead(time_up);
  t_down = digitalRead(time_down);
  rst = digitalRead(reset);

  if (initialise == 0) {

    if (((d_lim == LOW and u_lim == HIGH) or (d_lim == HIGH and u_lim == HIGH)) and strt == LOW and e == 0 and head_up == 0) digitalWrite(up, HIGH), head_up = 1;

    if (u_lim == LOW and d_lim == HIGH) digitalWrite(up, LOW), head_up = 0, initialise = 1;

  }

  if (initialise == 1) {


    // Overall safety status d == 1 is good d == 0 is a fault
    if (a == 0 and b == 0 and e == 0 and head_up == 0 and head_down == 0 and estp_flag == 0) {
      d = 1;
    }
    else {
      d = 0;
    }

    if (d == 1 and ready_flag == 0) ready_flag = 1, mixer_ready();

    if (u_lim == LOW and d_lim == HIGH and strt == LOW and a == 0) {
      digitalWrite(down, HIGH), down_timer = millis(), a = 1, head_down = 1, cycle_running = 1;
    }

    if (head_down == 1 and c == 0) {
      c = 1;
      lcd.clear();
      lcd.setCursor(0, 0);          // set cursor to first row
      lcd.print("Mixer head");  // print out to LCD
      lcd.setCursor(0, 1);          // set cursor to first row
      lcd.print("Going down");
    }

    //if timer is timed out, an alarm/fault should be triggered
    if (estp_flag == 1 or e == 1) {
      digitalWrite(down, LOW), head_down = 0;
    }
  }

  Serial.print("Head Down = "), Serial.print(head_down);
  Serial.print("  a = "), Serial.print(a);
  Serial.print("  b = "), Serial.print(b);
  Serial.print("  c = "), Serial.print(c);
  Serial.print("  d = "), Serial.print(d);
  Serial.print("  e = "), Serial.print(e);
  Serial.print("  f = "), Serial.print(f);
  Serial.print("  g = "), Serial.print(g);
  Serial.print("  h = "), Serial.print(h);
  Serial.print("  i = "), Serial.print(i);
  Serial.print("  j = "), Serial.println(j);





  faults();
  resetting();

}

void faults() {
  if (estp == LOW and estp_flag == 0) {
    estp_flag = 1;
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("ESTOP");  // print out to LCD
    lcd.setCursor(0, 1);          // set cursor to first row
    lcd.print("Pressed");
  }
  if (estp_flag == 1) {
    digitalWrite(up, LOW);
    digitalWrite(down, LOW);
    digitalWrite(run, LOW);
  }

  if (head_up == 1 and h == 0) up_timer = millis(), h = 1;
  if (head_up == 1 and millis() - up_timer > up_timeout and b == 0) {
    digitalWrite(up, LOW);
    b = 1;
    lcd.clear();
    delay(500);
    lcd.setCursor(6, 1);          // set cursor to first row
    lcd.print("UP LIMIT");  // print out to LCD
    lcd.setCursor(6, 2);          // set cursor to first row
    lcd.print("NOT MADE");
  }

  if (a == 1 and millis() - down_timer > down_timeout and b == 0 and d_lim == 1) {
    digitalWrite(down, LOW);
    b = 1;
    lcd.clear();
    delay(500);
    lcd.setCursor(5, 1);          // set cursor to first row
    lcd.print("DOWN LIMIT");  // print out to LCD
    lcd.setCursor(6, 2);          // set cursor to first row
    lcd.print("NOT MADE");
  }

  if (u_lim == LOW and d_lim == LOW and e == 0) {
    e = 1;
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Mixer");  // print out to LCD
    lcd.setCursor(0, 1);          // set cursor to first row
    lcd.print("Limit fault");
    lcd.setCursor(0, 2);          // set cursor to first row
    lcd.print("Both limits Active");
  }

}


void resetting() {
  if (rst == LOW and estp == HIGH) {
    a = 0, b = 0, c = 0, e = 0, h = 0, head_down = 0, head_up = 0, estp_flag = 0, lcd.clear();
    cycle_running = 0;
    ready_flag = 0;
    initialise = 0;
    lcd.setCursor(5, 1);          // set cursor to first row
    lcd.print("RESETING");
    g = 1;
    delay(2000);
    lcd.clear();
    g = 0;
    digitalWrite(up, LOW);
    digitalWrite(down, LOW);
    digitalWrite(run, LOW);
  }

}

void mixer_ready() {
  lcd.setCursor(0, 0);
  lcd.print("Mixer");  // print out to LCD
  lcd.setCursor(0, 1);          // set cursor to first row
  lcd.print("Ready");
  j = millis();
}