#include <AccelStepper.h>
#include <Servo.h>
#include <HX711.h>
#include "drinks.h"

// Konfiguration des DRV8825-Schrittmotortreibers
#define MOTOR_1_PIN_STEP 2  // Anschluss des Schritt-Impulses
#define MOTOR_1_PIN_DIR 3   // Anschluss der Richtungssteuerung
#define MOTOR_1_PIN_EN 4    // Anschluss der Treiberaktivierung

#define MOTOR_2_PIN_STEP 5  // Anschluss des Schritt-Impulses
#define MOTOR_2_PIN_DIR 6   // Anschluss der Richtungssteuerung
#define MOTOR_2_PIN_EN 4    // Anschluss der Treiberaktivierung

#define MOTOR_3_PIN_STEP 7  // Anschluss des Schritt-Impulses
#define MOTOR_3_PIN_DIR 8   // Anschluss der Richtungssteuerung
#define MOTOR_3_PIN_EN 4    // Anschluss der Treiberaktivierung

#define Stepper1_PIN_HOME_SWITCH A0 // Anschluss des Endschalters für die Referenzfahrt
#define Stepper2_PIN_HOME_SWITCH A1 // Anschluss des Endschalters für die Referenzfahrt
#define Stepper3_PIN_HOME_SWITCH A2 // Anschluss des Endschalters für die Referenzfahrt

// Erstellen der Stepper-Objekte
AccelStepper stepper1(AccelStepper::DRIVER, MOTOR_1_PIN_STEP, MOTOR_1_PIN_DIR);
AccelStepper stepper2(AccelStepper::DRIVER, MOTOR_2_PIN_STEP, MOTOR_2_PIN_DIR);
AccelStepper stepper3(AccelStepper::DRIVER, MOTOR_3_PIN_STEP, MOTOR_3_PIN_DIR);

// Stepper-Einstellungen
int stepper_speed = 200;
int stepper_acceleration = 50;
 
// Ein Flag, um die Referenzfahrt zu verfolgen
bool stepper1_home = false; 
bool stepper2_home = false;
bool stepper3_home = false;

// HX711 circuit wiring
const int LOADCELL_DOUT_PIN = 11;
const int LOADCELL_SCK_PIN = 12;

// HX711 Adjustment settings
const long LOADCELL_OFFSET = 0;
const long LOADCELL_DIVIDER = 0;

// create servo object to control a servo
Servo myservo1;  
Servo myservo2;

// create Scale object to control a Scale
HX711 scale;

// Einstellungen zum Abfüllen
const int slowDownThreshold = 100; // Schwelle, ab der der Servo verlangsamt wird (in Gramm)
const int slowDownAngle = 60; // Winkel, um den der Servo verlangsamt wird
bool station_filled = false; // Ein Flag, um die Befüllung zu verfolgen

// Angaben zum Errechnen der Getränke
const int slot[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}; // Anzahl der Plätze
int value_fillup[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // Temporäre Liste für die Befüllung
const int position[] = {0, 33, 66, 99, 133, 166, 0, 33, 66, 99, 133, 166}; // Position Getränke
int select_list = 1; // Neue Liste wählen


void setup() {  
 
 Serial.begin(9600);

 pinMode(Stepper1_PIN_HOME_SWITCH, INPUT_PULLUP);
 pinMode(Stepper2_PIN_HOME_SWITCH, INPUT_PULLUP);
 pinMode(Stepper3_PIN_HOME_SWITCH, INPUT_PULLUP);

 // HX711 Initialize library
 scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN);
 scale.set_scale(LOADCELL_DIVIDER);
 scale.set_offset(LOADCELL_OFFSET);

 // Initialisiere die Waage und Grundstellung anfahren
 myservo1.attach(9);
 myservo1.write(90);
 myservo2.attach(10);
 myservo2.write(90);

 //Referenzfahrten aller Stepper nacheinander
 homing_steppers();
 

}


void loop() {

update_list();
mix_drink();
delay(5000);
Serial.println("ende");
select_list++;
Serial.println(select_list);

}



//Unterprogramme:

//Getränkeliste updaten
void update_list() {
 switch (select_list) {
  case 1:
   for (int i = 0; i < sizeof(slot) / sizeof(slot[0]); i++) {
   value_fillup[i] = myList1[i]; // Überschreibe die temporäre Liste mit den neuen Werten
   }
   break;
  case 2:
   for (int i = 0; i < sizeof(slot) / sizeof(slot[0]); i++) {
   value_fillup[i] = myList2[i]; // Überschreibe die temporäre Liste mit den neuen Werten
   }
   break;
  case 3:
   for (int i = 0; i < sizeof(slot) / sizeof(slot[0]); i++) {
   value_fillup[i] = myList3[i]; // Überschreibe die temporäre Liste mit den neuen Werten
   }
   break;
  case 4:
   for (int i = 0; i < sizeof(slot) / sizeof(slot[0]); i++) {
   value_fillup[i] = myList4[i]; // Überschreibe die temporäre Liste mit den neuen Werten
   }
   break;
  case 5:
   for (int i = 0; i < sizeof(slot) / sizeof(slot[0]); i++) {
   value_fillup[i] = myList5[i]; // Überschreibe die temporäre Liste mit den neuen Werten
   }
   break;
  default:

   break;
 }
}
//Getränk mischen
void mix_drink() {
 for (int i = 0; i < sizeof(slot) / sizeof(slot[0]); i++) {
    int value1 = slot[i]; // Lese den  Wert aus der ersten Liste
    int value2 = value_fillup[i]; // Lese den  Wert aus der zweiten Liste
    
    if (value2 > 0) { //Fährt nur Plätze an, die einen Wert haben

     if (value1 <= 5) { // Plätze für Revovler 1
       station_fill1(value1, value2);
     }
     else { // Plätze für Revovler 2
       station_fill2(value1, value2);
     }
    }
  }
}
//Servo über Gewicht abfüllen lassen
void station_fill1(int value1, int value2) {
 stepper1.runToNewPosition(position[value1]);
 while (station_filled == false) {
  Serial.println(map(scale.get_units(10), 0, 2100, 0, 5000));
   long weight = map(scale.get_units(10), 0, 2100, 0, 5000); // Lies das Gewicht von der Waage (in Gramm)
   // Wenn das gewünschte Gewicht erreicht ist, halte den Servo an
   if (weight >= value2) {
     myservo1.write(90); // Stelle den Servo in eine bestimmte Position (90 Grad)
     Serial.println("Gewünschtes Gewicht erreicht.");
     station_filled = true; // Setzen des Flag um die Schleife zu unterbrechen
     scale.tare(0);
  } 
  else {
   // Ansonsten bewege den Servo weiterhin in die gewünschte Richtung
   // Überprüfe, ob das Gewicht nahe am Zielgewicht ist und verlangsame den Servo
   if (value2 - weight <= slowDownThreshold) {
     int slowDownSpeed = map(value2 - weight, 0, slowDownThreshold, 0, slowDownAngle);
     myservo1.write(90 - slowDownSpeed);
    } 
    else {
     myservo1.write(0); // Zum Beispiel, Servo in die andere Richtung (0 Grad)
    }
  }
 }
 station_filled = false; //Zurücksetzen des Flag
}
void station_fill2(int value1, int value2) {
 stepper2.runToNewPosition(position[value1]);
 while (station_filled == false) {
  Serial.println(map(scale.get_units(10), 0, 2100, 0, 5000));
   long weight = map(scale.get_units(10), 0, 2100, 0, 5000); // Lies das Gewicht von der Waage (in Gramm)
   // Wenn das gewünschte Gewicht erreicht ist, halte den Servo an
   if (weight >= value2) {
     myservo2.write(90); // Stelle den Servo in eine bestimmte Position (90 Grad)
     Serial.println("Gewünschtes Gewicht erreicht.");
     station_filled = true; // Setzen des Flag um die Schleife zu unterbrechen
     scale.tare(0);
   } 
   else {
   // Ansonsten bewege den Servo weiterhin in die gewünschte Richtung
   // Überprüfe, ob das Gewicht nahe am Zielgewicht ist und verlangsame den Servo
    if (value2 - weight <= slowDownThreshold) {
     int slowDownSpeed = map(value2 - weight, 0, slowDownThreshold, 0, slowDownAngle);
     myservo2.write(90 - slowDownSpeed);
    } 
    else {
     myservo2.write(0); // Zum Beispiel, Servo in die andere Richtung (0 Grad)
    }
  }
 }
 station_filled = false; //Zurücksetzen des Flag
}

//Referenzfahrt
void homing_steppers() {
 // Stepper1:
 stepper1.setMaxSpeed(stepper_speed / 5);
 stepper1.setAcceleration(stepper_acceleration / 5);
 while ((digitalRead(Stepper1_PIN_HOME_SWITCH) == HIGH) && (stepper1_home == false)) {    // Solange der Endschalter nicht ausgelöst wurde
   stepper1.move(1); // Schritte in die entgegengesetzte Richtung
   stepper1.run();
  }
 stepper1.stop();
 stepper1.setCurrentPosition(0);
 stepper1_home = true;
 Serial.println("Stepper 1 home");
 stepper1.setMaxSpeed(stepper_speed);
 stepper1.setAcceleration(stepper_acceleration);
 // Stepper 2:
 stepper2.setMaxSpeed(stepper_speed / 5);
 stepper2.setAcceleration(stepper_acceleration / 5);
 while ((digitalRead(Stepper2_PIN_HOME_SWITCH) == HIGH) && (stepper2_home == false)) {    // Solange der Endschalter nicht ausgelöst wurde
   stepper2.move(1); // Schritte in die entgegengesetzte Richtung
   stepper2.run();
  }
 stepper2.stop();
 stepper2.setCurrentPosition(0);
 stepper2_home = true;
 Serial.println("Stepper 2 home");
 stepper2.setMaxSpeed(stepper_speed);
 stepper2.setAcceleration(stepper_acceleration);
 // Stepper 3:
 stepper3.setMaxSpeed(stepper_speed / 5);
 stepper3.setAcceleration(stepper_acceleration / 5);
 while ((digitalRead(Stepper3_PIN_HOME_SWITCH) == HIGH) && (stepper3_home == false)) {    // Solange der Endschalter nicht ausgelöst wurde
   stepper3.move(1); // Schritte in die entgegengesetzte Richtung
   stepper3.run();
  }
 stepper3.stop();
 stepper3.setCurrentPosition(0);
 stepper3_home = true;
 Serial.println("Stepper 3 home");
 stepper3.setMaxSpeed(stepper_speed);
 stepper3.setAcceleration(stepper_acceleration);
 Serial.println("Homing abgeschlossen!");
}