// Pinout configuration
const int grinderPin = 9, pumpPin = 8, boilerPin = 6, powderPin = 7, wheelPin = 10, invertWheelPin = 11, powderSensorPin = 2, vaporSensorPin = 5, tempSensorPin = A7, wheelStartSensorPin = 4, wheelEndSensorPin = 3, redLED = 13, greenLED = 12;

// Global string
String readString;

// Thermistor config
// WARNING! while the old coffee machine used a PTC thermistor, this was replaced with an NTC one as it was more readily available
// Code must be adjusted accordingly if a PTC resistor is used once more!
int Vo;
float R1 = 10000; // <-- change this value to the resistance of the fixed resistor (so don't change PLS!)
float logR2, R2, T, Tc, Told;
float c1 = 8.5e-04, c2 = 1.85e-04, c3 = 2e-07; // Here's where you can perform actual calibration

// Variable to store desired Max boiler Temp
int desiredTemp = 70;

// Variables to Store errors:
int unrecoverableErr = 0;
int warning = 0;
int warned = 0; // support variable needed to prevent arduino from making second coffee after refill. this is a shitty hack and resets the board.

// Variables for cleaning and other maintenance
int dry = 0;
int noCoffee = 0;

// Milliseconds to delay between each cycle
const int milliseconds = 10;
int timeratio = 1;
// pumpRatio is = seconds to pump water for and will be updated over serial
int pumpRatio = 15;
// This next variable is used to get current "time" in ms and break out of while cycles that need a time limit safeguard
unsigned long startTime;

//_____________________________________________________________________________

void(* resetFunc) (void) = 0;  // declare reset fuction at address 0

//_____________________________________________________________________________

void setup() {
  // put your setup code here, to run once:
  // first we set pins as I/O and initialize outputs LOW
  pinMode(boilerPin, OUTPUT); pinMode(pumpPin, OUTPUT);  pinMode(grinderPin, OUTPUT); pinMode(powderPin, OUTPUT); pinMode(wheelPin, OUTPUT); pinMode(invertWheelPin, OUTPUT); pinMode (redLED, OUTPUT); pinMode (greenLED, OUTPUT);
  pinMode(powderSensorPin, INPUT); pinMode(vaporSensorPin, INPUT); pinMode(tempSensorPin, INPUT); pinMode(wheelStartSensorPin, INPUT); pinMode(wheelEndSensorPin, INPUT);
  digitalWrite(grinderPin, LOW); digitalWrite(pumpPin, LOW); digitalWrite(boilerPin, LOW); digitalWrite(powderPin, LOW); digitalWrite(wheelPin, LOW); digitalWrite(invertWheelPin, LOW); digitalWrite(redLED, LOW); digitalWrite(greenLED, LOW);
  // timeratio easily allows to determine how many cycles are required to make 1s pass (ms * ratio = 1s)
  timeratio = 1000/milliseconds;
  // initialize serial:
  Serial.begin(9600);
  delay(100);
  Serial.write("Arduino running :)\n");
}

//_____________________________________________________________________________

void loop() {
  // put your main code here, to run repeatedly:
  digitalWrite(greenLED, HIGH);
  // Check if unrecoverable error has occurred:
  if (unrecoverableErr == 1) {
    startTime = millis();
    delay(100);
    Serial.write("Error has occurred.\n"); delay(100); Serial.write("Check machine and restart\n");
    digitalWrite(greenLED, LOW);
    while (true){
      digitalWrite(redLED, HIGH);
      delay(1000);
      digitalWrite(redLED, LOW);
      delay(1000);
      // reset arduino after 30s
      if (millis() - startTime == 30000) {
        break;
      }
    }
    resetFunc(); //call reset.
  }
  
  // check if data has been sent on serial from ESP or PC:
  if (Serial.available() > 0) {
  // read the incoming data: (we only care about the first few characters, I've chosen 4)
    readString = "";
        serialRead();
        String incomingData = readString.substring(0,4);

    //if (incomingData == "read") {
      //digitalWrite(greenLED, HIGH);
    //}

    if (incomingData == "rist") {
      // set parameters for ristretto:
      delay(100);
      Serial.write("Ristretto\n");
      pumpRatio = 10;
    }

    if (incomingData == "espr") {
      // set parameters for espresso:
      delay(100);
      Serial.write("Espresso\n");
      pumpRatio = 13;
    }

    if (incomingData == "long") {
      // set parameters for lungo:
      delay(100);
      Serial.write("Lungo\n");
      pumpRatio = 16;
    }
    
    // check if the incoming data is "make":
    if (incomingData == "make") {
      // run the code to make coffee:
      delay(100);
      Serial.write("Making some coffee!\n");
      makeCoffee();
    }
    
    if (incomingData == "dryr") {
      // run the code to make a dry run (no powder, no water):
      delay(100);
      Serial.write("DryRun\n");
      dry = 1;
      makeCoffee();
    }

    if (incomingData == "noco") {
      // run the code to make a dry run (no powder, nor water):
      delay(100);
      Serial.write("NoCo\n");
      pumpRatio = 12;
      noCoffee = 1;
      makeCoffee();
    }

    if (incomingData == "grin") {
      // only grind
      Grind();
    }
    
    if (incomingData == "pump") {
      // run the code to just pump water:
      pumpRatio = 15;
      Pump();
    }

    if (incomingData == "pres") {
      // only press
      Press();
    }
    
    if (incomingData == "unpr") {
      // only unpress
      unPress();
    }

    if (incomingData == "heat") {
      // only heat
      desiredTemp = 80;
      Heat();
    }

    if (incomingData == "drop") {
      // only drop
      Drop();
    }

    if (incomingData == "clea") {
      // clean machine
      delay(100);
      Serial.write("s-clean\n");
      dry = 1;
      makeCoffee();
      noCoffee = 1;
      makeCoffee();
      delay(100);
      Serial.write("S-cleaning done.\n");
    }
  }

  // making steam:
  while (digitalRead(vaporSensorPin) == HIGH) {
    desiredTemp = 90;
    Serial.write("Vapor heating\n");
    delay(500);
    Heat(); // Heat the boiler to vapor temperature
    delay(20000);
    
    if (unrecoverableErr == 1) {
      break;
    }
  }
  delay(milliseconds);
}

//_____________________________________________________________________________

// this is how we read the input
void serialRead() {
  while (Serial.available()) {
  delay(10);  
  if (Serial.available() > 0) {
    char c = Serial.read();
    readString += c;}
  }
}

//_____________________________________________________________________________

void Grind() {
  digitalWrite(greenLED, LOW);
  delay(100);
  Serial.write("grinding...\n");
  digitalWrite(grinderPin, HIGH);
  startTime = millis();
  while (true) {
    delay(milliseconds);
    if (digitalRead(powderSensorPin) == HIGH) {
      digitalWrite(grinderPin, LOW);
      delay(100);
      Serial.write("Grinding Done\n");
      break;
    }
    if (millis() - startTime > 15000) {
      Serial.write("Warning, grinding took too long!\n");
      digitalWrite(grinderPin, LOW);
      delay(100);
      Serial.write("Out of Coffee?\n");
      warning = 1;
      warned = 1;
      break;
    }
  }
  digitalWrite(greenLED, HIGH);
}

//_____________________________________________________________________________

void Drop() {
  digitalWrite(greenLED, LOW);
  delay(100);
  Serial.write("dropping...\n");
  digitalWrite(powderPin, HIGH);
  delay(1000);
  digitalWrite(powderPin, LOW);
  delay(100);
  Serial.write("Dropped\n");
  digitalWrite(greenLED, HIGH);
}

//_____________________________________________________________________________

void Heat() {
  digitalWrite(greenLED, LOW);
  digitalWrite(redLED, HIGH);
  startTime = millis();
  delay(100);
  Serial.write("Heating to ");
  delay(100);
  Serial.print(desiredTemp);
  delay(100);
  Tc = 0;
  Told = - 100;

  // monitor temperature and adjust boilerPin as needed:
  while (true) {
    
    // read temperature from tempSensorPin:
    Vo = analogRead(tempSensorPin);
    R2 = R1 * (1023.0 / (float)Vo - 1.0);
    logR2 = log(R2);
    T = (1.0 / (c1 + c2*logR2 + c3*logR2*logR2*logR2));
    Tc = (T - 273.15);
    
    if (millis() - startTime == 1000) {
      Told = Tc; // support variable to store temp at beginning, so that we can be sure it's increasing
    }

    // check if temperature is within the acceptable range and break out of the loop without error if done heating:
    if (Tc > desiredTemp) {
      // temperature is within range, so break out of the loop:
      digitalWrite(boilerPin, LOW);
      delay(100);
      Serial.write("reached desired temp\n");
      delay(100);
      Serial.print(Tc);
      delay(100);
      digitalWrite(redLED, LOW);
      break;
    }

    if (Tc < -100) {
      delay(100);
      Serial.write("u-Thermocouple: unplugged or failed\n");
      digitalWrite(boilerPin, LOW);
      unrecoverableErr = 1;
      break;
    }

    if (millis() - startTime > 20000 && Tc - Told < 1) {
      delay(100);
      Serial.write("p-Thermocouple: positioning or relay fault\n");
      digitalWrite(boilerPin, LOW);
      unrecoverableErr = 1;
      break;
    }

    if (millis() - startTime > 30000) {
      delay(100);
      Serial.write("h-taking too long, continuing...\n");
      digitalWrite(boilerPin, LOW);
      digitalWrite(redLED, LOW);
      break;
    }
    
    // We do this at the end so that the relay is not uselessly cycled
    digitalWrite(boilerPin, HIGH);
    delay(milliseconds);

  }
  digitalWrite(greenLED, HIGH);
}

//_____________________________________________________________________________

void Press() {
  digitalWrite(greenLED, LOW);
  delay(100);
  Serial.write("pressing...\n");
  
      // Reset Press before every coffee
      digitalWrite(invertWheelPin, HIGH);
      delay(1000);
      digitalWrite(invertWheelPin, LOW);
      
  digitalWrite(wheelPin, HIGH);
  startTime = millis();

  while (true) {
    delay(milliseconds);
    if (digitalRead(wheelEndSensorPin) == HIGH) {
      delay(900); // extra delay added to compensate for slightly incorrect endstop placement after repair
      digitalWrite(wheelPin, LOW);
      delay(100);
      Serial.write("Pressed\n");
      break;
    }
    if (millis() - startTime > 15000) {
      delay(100);
      Serial.write("p-end of pressing not detected\n");
      digitalWrite(wheelPin, LOW);
      unrecoverableErr = 1;
      break;
    }
  }
  digitalWrite(greenLED, HIGH);
}

//_____________________________________________________________________________

void unPress() {
  digitalWrite(greenLED, LOW);
  delay(100);
  Serial.write("unPressing...\n");

      // Reset Press before every coffee
      digitalWrite(wheelPin, HIGH);
      delay(1500);
      digitalWrite(wheelPin, LOW);
      
  digitalWrite(invertWheelPin, HIGH);
  startTime = millis();
  
  while (true) {
    delay(milliseconds);
    if (digitalRead(wheelStartSensorPin) == HIGH) {
      delay(1500);
      digitalWrite(invertWheelPin, LOW);
      delay(100);
      Serial.write("UnPressed\n");
      break;
    }
    if (millis() - startTime > 15000) {
      delay(100);
      Serial.write("u-end of unPressing not detected\n");
      digitalWrite(invertWheelPin, LOW);
      unrecoverableErr = 1;
      break;
    }
  }
  digitalWrite(greenLED, HIGH);
}

//_____________________________________________________________________________

void Pump() {
  digitalWrite(greenLED, LOW);
  delay(100);
  Serial.write("pumping Water...\n");
  digitalWrite(pumpPin, HIGH);
  delay(1000*pumpRatio);
  digitalWrite(pumpPin, LOW);
  delay(100);
  Serial.write("Pumping water done\n");
  digitalWrite(greenLED, HIGH);
}

//_____________________________________________________________________________

void makeCoffee() {
  // code to make coffee goes here...
  // It would be much smarter to break down complicated functions into subroutines: press/unpress etc...
  // Hence that's what we'll have done by the time the first release goes public ;)
  // makeCoffee() can still run all the same, but clean() can use the same press/unpress code!
  // care must be taken to place these functions earlier in the code, or the compiler will rightfully freak out

  while (true) {
  
  desiredTemp = 75;
  Heat(); // First we pre-heat the boiler
  if (unrecoverableErr == 1) {
    break;
  }

  if (dry == 0 && noCoffee == 0) {
    Grind(); // Grinding some nice roasted coffee beans!
  }

  // If grinder won't stop, coffee probably needs to be refilled.
  if (warning == 1) {
    delay(100);
    Serial.write("refill coffee and send cont command\n");
    while (true) {
      // check if data has been sent on serial from ESP or PC:
      if (Serial.available() > 0) {
      // read the incoming data: (we only care about the first few characters, I've chosen 4)
        readString = "";
        serialRead();
        String incomingData = readString.substring(0,4);

        if (incomingData == "cont") {
         // coffee refilled, making is allowed to continue
          Serial.write("Refilled\n");
          warning = 0;
          Grind(); // Grinding some nice roasted coffee beans!
          if (warning == 1) {
            unrecoverableErr = 1;
            Serial.write("g-failure: second grinding failure, check grinder!\n");
          }
          break;
        }
      }
    }
  }

  // Stop if grinding failed a second time
  if (unrecoverableErr == 1) {
    break;
  }

  delay(1000); // this delay is mandatory to prevent powder spillage caused by grinder inertia.
  unPress(); // unpressing the press to reset the machine
  
  if (unrecoverableErr == 1) {
    break;
  }
  
  Drop(); // Dropping the powder in the press
  delay(1000);
  
  Press(); // Self explainatory

  if (unrecoverableErr == 1) {
    break;
  }

  desiredTemp = 85;
  Heat(); // First we pre-heat the boiler
  if (unrecoverableErr == 1) {
    break;
  }

  if (dry == 0) {
    digitalWrite(boilerPin, HIGH);
    delay(100);
    Pump(); // Pump ratio (in seconds) will be read from serial input in final release
    digitalWrite(boilerPin, LOW);
  }
  delay(3000); // Allow pressure to wane
  
  unPress(); // unpressing the press to reset the machine

  if (unrecoverableErr == 1) {
    break;
  }
  delay(100);
  Serial.write("Complete!\n");
  delay(1000);
  dry = 0;
  noCoffee = 0;
  if (warned == 1) {
    resetFunc(); //call reset. I am ashamed of this, but it works.
  }
  break;
  }
}