// 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, Tp, Tstart, 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; // warning: there is usually ~5s dead time from pump startup to water flowing out of nozzle
int pumpStatus = 0; // support variable to enable pumping while heating, added feb 24
// 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;
unsigned long pMillis;

//_____________________________________________________________________________
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){ // lock the machine in a loop while blinking the RED LED to indicate an error
      digitalWrite(redLED, HIGH);
      delay(100);
      digitalWrite(redLED, LOW);
      delay(900);
      if (Serial.available() > 0) {
        warning = 0;
        warned = 0;
        unrecoverableErr = 0; // added on 19/11/24 to reset unrecoverable error when serial com is received
        break;
      }
      // reset arduino after 30s -> removed reset on 19/11/24, LED should stay blinking and machine locked until reboot or serialcom received
      //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; // updated from 15 to 10s after fixing restriction 19/11/24
    }

    if (incomingData == "espr") {
      // set parameters for espresso:
      delay(100);
      Serial.write("Espresso\n");
      pumpRatio = 15; // updated from 20 to 15s after fixing restriction 19/11/24
    }

    if (incomingData == "long") {
      // set parameters for lungo:
      delay(100);
      Serial.write("Lungo\n");
      pumpRatio = 20; // updated from 25 to 20s after fixing restriction 19/11/24
    }

    if (incomingData == "amer") {
      // set parameters for lungo:
      delay(100);
      Serial.write("Americano\n");
      pumpRatio = 60;
    }
    
    // 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 perform a dry run (no powder, nor water):
      delay(100);
      Serial.write("DryRun\n");
      dry = 1;
      makeCoffee();
    }

    if (incomingData == "noco") {
      // run the code to perform a wet run (water, but no powder):
      delay(100);
      Serial.write("NoCo\n");
      pumpRatio = 10;
      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");
    }
    if (incomingData == "f-un") {
      Serial.write("F-Unpress\n");
      delay(1000);
      digitalWrite(invertWheelPin, HIGH);
      delay(5000);
      digitalWrite(invertWheelPin, LOW);
    }
    if (incomingData == "f-pr") {
      Serial.write("F-Press\n");
      delay(1000);
      digitalWrite(wheelPin, HIGH);
      delay(5000);
      digitalWrite(wheelPin, LOW);
    }
  }

  // making steam:
  while (digitalRead(vaporSensorPin) == HIGH) {
    desiredTemp = 100;
    Serial.write("Vapor heating\n");
    delay(500);
    Heat(); // Heat the boiler to vapor temperature
    //delay(20000); // no idea why if had a 20s delay originally, I attempted to remove it to see how it goes on 19/11/24
    delay(200); // assuming 20000 was a type error, 200ms seems sensible
    
    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();
  pMillis = startTime; // value to store 
  delay(100);
  Serial.write("Heating to ");
  delay(100);
  Serial.print(desiredTemp);
  delay(100);
  // initialize variables at safe values
  Tc = 0; // current temperature
  Tp = -10; // temperature at previous cycle
  Tstart = - 100; // temperature at start of Heat() function

  // 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)); // compute temperature from NTC
    Tc = (T - 273.15); // convert from Kelvin to Celsius for readability
    
    if (millis() - startTime > 500 && millis() - startTime < 1500) {
      Tstart = Tc; // support variable to store temp at beginning, so that we can be sure it's increasing
      delay(1001); // make sure we only set Tstart once!
    }

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

    if (Tc < -100) { // break the loop if temperature < -100, can only happen if cable gets unplugged
      delay(100);
      Serial.write("u-Thermocouple: unplugged or failed\n");
      unrecoverableErr = 1;
      break;
    }

    if (millis() - startTime > 20000 && Tc - Tstart < 1 && pumpStatus == 0) { // break the loop if temperature is not increasing
      delay(100);
      Serial.write("p-Thermocouple: not detecting heating, positioning or relay fault\n");
      unrecoverableErr = 1;
      break;
    }

    if (millis() - startTime > 90000 && pumpStatus == 0) { // break out of the loop after 60s if the boiler is not yet hot
      delay(100);
      Serial.write("h-taking too long, continuing...\n");
      break;
    }

    // actual heater logic follows:
    // check aprox. derivative every second and shut off heater if temperature is increasing too quickly!
    // conversely, heater is turned on if temperature is not incresing quickly enough
    if (millis() > (pMillis + 1000)){

      // initially run the heater on fully. eg: if we set the number to 30 and start with
      // Tambient = 20C and desiredTemp = 90 the heater will stay fully on until 90-30 = 60C
      //if (Tc < (desiredTemp - 30)) { // old if with variable temp

      // new if with hard-coded minimum pulsing enable temperature of 70C
      if (Tc < 70 || Tc < (desiredTemp - 20)) {
        digitalWrite(boilerPin, HIGH);
      }

      // change this value for proportional behaviour: lower values will cycle the relay more often.
      // Cycling often reduces temperature undershoot, but will shorten contact lifespan
      else if ((Tc - Tp) < 0.6) {
        digitalWrite(boilerPin, HIGH);
      }

      //else if ((Tc - Tp) > 1) {
      //  digitalWrite(boilerPin, LOW);
      //}
      
      else {
        digitalWrite(boilerPin, LOW);
        // delay(1000); // extra 1s delay to keep boiler off for 2s total (delay + millis())
      }

      // this next if lets us call us for pumping while still monitoring heat
      if (pumpStatus == 1 && ((millis() - startTime)/1000) < pumpRatio) {
        digitalWrite(pumpPin, HIGH);
      }
      else if (pumpStatus == 1 && ((millis() - startTime)/1000) > pumpRatio) {
        digitalWrite(pumpPin, LOW);
        break;
      }
      else {
        //digitalWrite(pumpPin, LOW);
        //pumpStatus = 0;
      }
      
      Tp = Tc;
      pMillis = millis();
      
    }

    delay(milliseconds);

  }
  
  digitalWrite(boilerPin, LOW);
  digitalWrite(redLED, LOW);
  digitalWrite(greenLED, HIGH);
  
}

//_____________________________________________________________________________
void Press() {
  digitalWrite(greenLED, LOW);
  delay(100);
  Serial.write("pressing...\n");
  
      // Slightly move the press in the opposite direction to make sure the motor's commutator engages
      digitalWrite(invertWheelPin, HIGH);
      delay(100);
      digitalWrite(invertWheelPin, LOW);
      
  digitalWrite(wheelPin, HIGH);
  startTime = millis();

  while (true) {
    delay(milliseconds);
    if (digitalRead(wheelEndSensorPin) == HIGH) {
      delay(90); // 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 > 20000) { // changed from 15s to 20s to account for motor slowdown over the years
      delay(100);
      Serial.write("p-end of pressing not detected\n");
      digitalWrite(wheelPin, LOW);
      unrecoverableErr = 1;  //temporarily disabled because of failing sensor(s)
      break;
    }
  }
  digitalWrite(greenLED, HIGH);
}

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

      // Slightly move the press in the opposite direction to make sure the motor's commutator engages
      digitalWrite(wheelPin, HIGH);
      delay(100);
      digitalWrite(wheelPin, LOW);
      
  digitalWrite(invertWheelPin, HIGH);
  startTime = millis();
  
  while (true) {
    delay(milliseconds);
    if (digitalRead(wheelStartSensorPin) == HIGH) {
      delay(1000);
      digitalWrite(invertWheelPin, LOW);
      delay(100);
      Serial.write("UnPressed\n");
      break;
    }
    if (millis() - startTime > 20000) { // changed from 15s to 20s to account for motor slowdown over the years
      delay(100);
      Serial.write("u-end of unPressing not detected\n");
      digitalWrite(invertWheelPin, LOW);
      //unrecoverableErr = 1;  //temporarily disabled because of failing sensor(s)
      break;
    }
  }
  digitalWrite(greenLED, HIGH);
}

//_____________________________________________________________________________
void Pump() {
  digitalWrite(greenLED, LOW);
  delay(100);
  Serial.write("pumping Water...\n");
  pumpStatus = 1; // set pumpStatus variable to 1 to tell Heat() function to pump; this is so that Proportional heating control can still function while pumping, preventing overheat
  Heat(); // call Heat function with current parameters
  pumpStatus = 0; // set pumpStatus variable to 0
  
  // OLD PUMPING LOGIC BEFORE MOVING INSIDE Heat()
  //digitalWrite(pumpPin, HIGH);
  //while (pumpRatio > 1) {
  //  pumpRatio--;
  //  delay(1000);
  //}
  //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 = 80;
  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.

  // Slightly move the press in the opposite direction to make sure the press sensor detects contact
      digitalWrite(wheelPin, HIGH);
      delay(1000);
      digitalWrite(wheelPin, LOW);
  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 = 90;
  Heat(); // First we pre-heat the boiler
  if (unrecoverableErr == 1) {
    break;
  }

  if (dry == 0) {
    //digitalWrite(boilerPin, HIGH);
    delay(100);
    desiredTemp = 95; // temperature to try and maintain while making coffee
    Pump(); // Pump ratio (in seconds) will be read from serial input in final release
    digitalWrite(boilerPin, LOW);
  }

  // Slightly move the press back to create a way for vapor to escape (added 19/11/24)
  digitalWrite(invertWheelPin, HIGH);
  delay(3000);
  digitalWrite(invertWheelPin, 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;
  }
}