#include "AccelStepper.h"  //Including der Servo Library
#include "PololuMaestro.h" //Including der PolouMaestro Library, für den Motor Driver
#include "Configuration.h" //Including unseres Konfigurationsfiles!

String serialBuffer = "";  //Leeren der SerialBuffer Variable!
String actions[TOTAL_ACTIONS];  //Holen des ersten wertes, aus dem Konfigurationsfile, (Max Aktionen.)

int counter = 0;   //Counter Variable zum Start auf 0 Setzen, (Zählt Schritte)
int lastIndex = 0;  //Index Variable auch auf 0 -> Index für den Standort

AccelStepper stepper(X_INTERFACE_TYPE, X_STEP_PIN, X_DIR_PIN); // Definition, der Variabeln aus dem Konfigurationsfile für dem Steppermotor! Pins für (Schritte, Interface, und Dir)
MicroMaestro maestro(maestroSerial); // Definiert einen neuen Servo Controller, anhand der Eingebundenen Librarys und des Konfigurationsfiles!

void setup() {
  Serial.begin(9600); // Festlegen des Serial port für das Debugging
  maestroSerial.begin(9600); // Festlegen des Servo controller Ports, gleich wie Debugging!
  Serial2.begin(9600); // Festlegen des Bluetooth module Port. -> Auch gleich um Meldungen zu übergeben!
  stepper.setMaxSpeed(X_MAX_SPEED); // Setzt die max. Geschwindigkeit für den Stepper, aus unserem Konfigurationsfile.
  pinMode(X_ENDSTOP_PIN, INPUT_PULLUP); // Initialisiert den Endstop Pin, als Pullup-Schalter -> also Kontaktschalter!
  homeXAxis(); // Bei jedem Start, soll der Getränkeautomat, an seine Start; Ausgangsposition zurückfahren!
}

// Die Funktion um die Startposition automatisch zu finden:
void homeXAxis() {
  int endStopState = digitalRead(X_ENDSTOP_PIN);
  
  while (endStopState == HIGH) {
    stepper.moveTo(100);
    stepper.setSpeed(X_HOME_SPEED);
    stepper.runSpeed();
    endStopState = digitalRead(X_ENDSTOP_PIN);
  }

  stepper.moveTo(stepper.currentPosition() - 50);
  while (stepper.distanceToGo() != 0) {
    stepper.setSpeed(X_PARK_SPEED * -1);
    stepper.runSpeed();
  }

  endStopState = digitalRead(X_ENDSTOP_PIN);

  while (endStopState == HIGH) {
    stepper.moveTo(100);
    stepper.setSpeed(X_PARK_SPEED);
    stepper.runSpeed();
    endStopState = digitalRead(X_ENDSTOP_PIN);
  }
  stepper.setCurrentPosition(0);
}

void loop() {
  while(Serial2.available() > 0) {
    char ch = Serial2.read();
    serialBuffer += ch;
    if (ch == '\n') {
      for(int i=0; i<serialBuffer.length(); i++) {
        if(serialBuffer.substring(i, i+1) == ",") {
          actions[counter] = serialBuffer.substring(lastIndex, i);
          lastIndex = i + 1;
          counter++;
        }

        if(i == serialBuffer.length() - 1) {
          actions[counter] = serialBuffer.substring(lastIndex, i);
        }
      }

      for(int z=0; z<TOTAL_ACTIONS; z++) {
        if(actions[z] != "0") {
          parseInput(actions[z]);
        }
      }

      Serial2.println("H");

      for(int y=0; y<TOTAL_ACTIONS; y++) {
        actions[y] = "0";
      }
      
      serialBuffer = "";
      counter = 0;
      lastIndex = 0;      
    }
  }
}

void parseInput(String input) {
  input.trim();
  byte command = input.charAt(0);

  switch(command) {
    case 'H':
      homeXAxis();
      break;
    case 'X':
      moveXTo(input);
      break;
    case 'F':
      pour(input);
      break;
  }
}

//Funktion, um zu der jeweiligen Flasche zu fahren.
void moveXTo(String input) {
  int pos = input.substring(1).toInt(); //Bestimmt aktuelle Position des Schrittmotors!
  
  Serial.print("X geht zu: ");  //Ausgabe1 auf Debugging-Monitor
  Serial.println(pos);  //Ausgabe2 der Position!
  
  Serial2.println(input);  //Ausgabe3 der Inputs, von der App (Smartphone)
  
  if(pos < 0 && pos >= X_MAX_POS) {  //Wenn die Position zwischen 0 und dem festgelegten max Wert liegt, wird fortgefahren.
    stepper.setAcceleration(X_ACCELERATION);  //Holen des X_Acceleration Wertes aus dem Konfigurationsfile
    stepper.moveTo(pos);  //Legt den wert (Wo er hin muss) für den Stepper Motor fest!
      if(pos < stepper.currentPosition()) {  //Wenn die angegebene Position kleiner ist als die Aktuelle.. 
        stepper.setSpeed(-100); //..wird die Geschwindigkeit gedrosselt!
      } else {
        stepper.setSpeed(100);  //..sonst nicht.
      }
    while(stepper.distanceToGo() != 0) { // Bis die Distanz für das Ziel nicht 0 also der Stepper Motor am Ausgangspunkt angekommen ist, wird stepper.run() weiter ausgeführt!
      stepper.run();  //Fährt den Motor zurück!
    }
  } else {
    Serial.println("Die Position, sollte zwischen -4995 und 0 sein!"); // Ein Fehler ist aufgetreten, er kann nicht automatisch die Startposition finden. (Sollte nicht mehr passieren, ist jedoch trozdem sicherer so!) ->> Lösung: Strom ausschalten & manuell zurückfahren
  }
}

// Hauptfunktion für das herauslassen der Getränke:
void pour(String input) { //Methode pour, wird durch Ionic vorgegeben und umschreibt eine Kommunikationsart. (Gleichername verwendet, einfachheitshalber)
  int count = 0; // Setzen der Count Variable, zur Zählung der Zutaten
  int times = 0; // Setzen der Times Variabel, wie viele Male, eine Zutat gewählt wird!
  int holdDuration = 0; // Setzen der Öffnungsdauer, wie lange auf eine Flasche gedrückt wird.
  int waitDuration = 0; // Setzen der Wartezeit, bis die nächste Zutat gewählt wird.
  
  for(int z=0; z<input.length(); z++) {
    byte parameter = input.substring(z, z+1).charAt(0);

    switch(parameter) {
      case 'F':
        times = getParameterValue(input, z);
        break;
      case 'H':
        holdDuration = getParameterValue(input, z);
        break;
      case 'W':
        waitDuration = getParameterValue(input, z);
        break;
    }
  }

  if(holdDuration > 0 && waitDuration > 0) {
    for(int i=0; i<times; i++) {
      maestro.setSpeed(0, SERVO_RAISE_SPEED);
      maestro.setTarget(0, SERVO_MAX_POS);
      delay(holdDuration);
      maestro.setSpeed(0, SERVO_RELEASE_SPEED);
      maestro.setTarget(0, SERVO_MIN_POS);
      if(times - 1 > count) {
        delay(waitDuration);
      } else {
        delay(DELAY_BETWEEN_INGREDIENTS);
      }
      count++;
    }
  } else {
    Serial.println("Die Öffnungsdauer und Wartezeit, kann nicht kleiner, gleich 0 sein!");
  }
}

int getParameterValue(String input, int z) {
  for(int y=z+1; y<input.length(); y++) {
    if(input.substring(y, y+1) == " ") {
      return input.substring(z+1, y).toInt();
      break;
    }
    if(y == input.length() - 1) {
      return input.substring(z+1, y+1).toInt();
    }
  }
}