#include <ESP32Servo.h>
#include "Arduino.h"
#include "Trommel.h"
#include "LED.h"


//  Arduino UNO pin Signal Kabel

//  1 NeoPixel RGB LED
//  2 Endschalter von der PM
//  4 digitale signal vom TCRT5000
//  7 Signal für PM startknopf
//  6 Stop Taster (der zweite pin vom Taster kommt an GND)
//  5 Start Taster  (der zweite pin vom Taster kommt an GND)
//  8 Das Signalkabel vom Hallsensor
//  9 ServoLaden (servo der die Hülse auf die PowerMatic schiebt)
//  10  ServoStart (servo der Start drückt an der PowerMatic)
//  12  Step pin vom Schritttreiber
//  13  Dir oder direction pin vom Schritttreiber

extern float trommelpos = 0.00; 
float stepdegree;
//Trommel-------------------------------------------------------------------------
//Wieviele schritte der motor braucht für eine komplette umdrehung
//mit einem Schrittmotor der 200(1.8 Grad pro schritt) schritte braucht für eine umdrehung:
//200 ohne microstepping, 400 mit 1/2 microstepping, 800 mit 1/4 microstepping usw.
#define StepperStepsPR 800
//drehrichtung vom motor. 0 = gegen uhrzeigersinn, 1 = mit uhrzeigersinn
#define StepperDir 0
//Wie schnell der motor drehen soll
//kleinerer wert = schneller(min. 100), höher = langsamer
#define StepperSpeed 50000  //def 5000
//falls die position der Trommel nicht ganz stimmt
//Dann macht der motor soviele Schritte mehr, es sind auch negative zahlen erlaubt
//Dreht sie nicht weit genug positive zahl, dreht sie zu weit negative zahl
#define TrommelPosKorrektur -50 //def 4

//Servo Laden-------------------------------------------------------------------------
//Die Servo anfangs position und end position in Grad
//min. 0, max. 180.   7-8 Grad entsprechen etwa 1mm.
#define ServoLadenAnfangPos 2   //Std:2
#define ServoLadenEndPos 163
//Wie schnell der Servo drehen soll
//2 = min. um so höher die zahl um so langsamer, 10 ist schon ziemlich langsam
#define ServoLadenSpeed 6

//Servo Start-------------------------------------------------------------------------
//gibt an ob ein Servo für start drücken an der PM verwendet wird oder
//der PowerMatic start knopf angeklemmt wurde
//0 = ohne servo, 1 = mit servo
#define ServoStartEnable 1
//Die Servo anfangs position und end position in Grad
//min. 0, max. 180
#define ServoStartAnfangPos 2
#define ServoStartEndPos 45
//Wie schnell der Servo drehen soll
//2 = min. um so höher die zahl um so langsamer, 10 ist schon ziemlich langsam
#define ServoStartSpeed 2

//IR TCRT5000-----------------------------------------------------------------
//0 = ohne IR sensor, 1 = mit IR sensor
#define IREnable 1


//LED-------------------------------------------------------------------------
//0 = aus(min.),  255 = Hell(max.)
#define LEDHelligkeit 180


//..............Servo..............
Servo servoLaden;
Servo servoStart;
#define pinServoLaden 19
#define pinServoStart 18
//..............Schrittmotor..............
#define pinStepperDir 13
#define pinStepperStep 12
//..............Taster..............
#define pinTasterStop 27
#define pinTasterStart 26
//..............NeoPixel LED..............
#define pinNeoPixel 4
//..............PowerMatic..............
#define pinEndschalter 34
#define pinPMStart 7
//..............Trommel Schalter..............
#define pinTrommelEndschalter 32
//..............IR sensor..............
#define pinIRd 35



clsLED LED(pinNeoPixel, LEDHelligkeit);
clsTrommel Trommel(pinStepperDir, pinStepperStep, pinTrommelEndschalter, StepperStepsPR, 
                   StepperSpeed, StepperDir, TrommelPosKorrektur);

bool ServoLadenAnfangPosOK = false;  //hat nur true wenn der servo an der anfangs position ist
bool ServoStartAnfangPosOK = false;  //hat nur true wenn der servo an der anfangs position ist
int last_zustand = 99;


void setup() 
{
  Serial.begin(9600);
  pinMode(pinTasterStart, INPUT_PULLUP);
  pinMode(pinTasterStop, INPUT_PULLUP);

  pinMode(pinEndschalter, INPUT);
  if(IREnable) pinMode(pinIRd, INPUT);


  if(ServoStartEnable)
  {
    servoStart.attach(pinServoStart);
    while(!runStartServo(ServoStartAnfangPos)){} //sicher stellen das der servo in der richtigen pos. ist
  }else{
    ServoStartAnfangPosOK = true;
    pinMode(pinPMStart, OUTPUT);
  }

  servoLaden.attach(pinServoLaden);
  while(!runLadenServo(ServoLadenAnfangPos)){} //sicher stellen das der servo in der richtigen pos. ist
  stepdegree = 3600.00/StepperStepsPR;
  Serial.println(stepdegree);
  Trommel.begin();
  LED.begin();

  delay(10);

}

void loop()
{
  static enum {AUS, EIN, AUSRICHTEN, STEPPER, LADEN, STOPFEN, FEHLER} zustand = AUS;
  const char* zustand_txt[] = {"AUS", "EIN", "Ausrichten", "Trommel bewegen", "Hülse laden", "Stopfen", "Fehler"};
  static bool OneCycle = false;
  static bool HuelseDrauf = false;
  uint8_t tmpTasterPressed = 0;

  tmpTasterPressed = checkTaster(true);
  //Serial.println(digitalRead(pinTrommelEndschalter));

  switch(zustand)
  {
    case AUS:
      switch(tmpTasterPressed)
      {
        case pinTasterStop:  
          Serial.println("Stoptaste");
          Trommel.trommelPosOK(true);
          zustand = AUSRICHTEN;  
          break;
        case pinTasterStart:
          Serial.println("Starttaste");
          Serial.println(ServoLadenAnfangPosOK);
          Serial.println(ServoStartAnfangPosOK);
          if(Trommel.trommelPosOK(false) && ServoLadenAnfangPosOK && ServoStartAnfangPosOK) 
            {zustand = EIN;}
          break;
        default: 
          if(Trommel.trommelPosOK(false) && ServoLadenAnfangPosOK && ServoStartAnfangPosOK)
            {LED.setFarbe(clsLED::ROT);}
          else
            {LED.blink(250, clsLED::ROT);}
      }
      break;
    case EIN:
      zustand = STEPPER;
      break;
    case AUSRICHTEN:     
      LED.setFarbe(clsLED::BLAU);
      if(ServoStartEnable)
      { 
        if(!runStartServo(ServoStartAnfangPos)) {
          //Serial.println("StartServo nicht in Anfangsposition");
          break;
        }
      }

      if(runLadenServo(ServoLadenAnfangPos))
      {
        if(!Trommel.trommelPosOK(false))
        {
          if(Trommel.move45Grad()) zustand = AUS;
        }else{
          zustand = AUS;
          trommelpos = 0.00;
        }
      }
      break;
    case STEPPER:
      if(Trommel.move45Grad())  //Schrittmotor 45Grad drehen
      {
        if(Trommel.trommelPosOK(false))   //prüfen ob pos. erreicht wurde
        {
          zustand = LADEN;
        }else{
          zustand = FEHLER;
          Serial.println("Trommel-Endpunkt nicht erreicht (Hall-Sensor)");
        }
      }
      break;
    case LADEN:
        if(!HuelseDrauf)
        {
          if(runLadenServo(ServoLadenEndPos))   //Zigihülse ganz reinstecken
          {
            HuelseDrauf = true;
            if(!IREnable) break;

            delay(20);
            if(digitalRead(pinIRd) == HIGH) 
            {
              HuelseDrauf = false;
              zustand = FEHLER;
              Serial.println("Fehler bei Laden: Huelse von IR nicht erkannt");
            }
          }
        }else{
          if(runLadenServo(ServoLadenAnfangPos))   //servo in ausgangs pos. zurückdrehen
          {
            HuelseDrauf = false;
            zustand = STOPFEN;
          }
        }
      break;
    case STOPFEN:
      switch(StopfMaschine(false, tmpTasterPressed))
      {
        case 0:
          zustand = FEHLER;
          Serial.println("Fehler bei Stopfen: Endschalterfehler");
          break;
        case 2:
          if(OneCycle)
            {zustand = AUS;}
          else
            {zustand = STEPPER;} // Nächste zigi machen
          break;
      }
      break;
    case FEHLER:
      LED.blink(250, clsLED::ROT);
      if(tmpTasterPressed == pinTasterStop)
      {
        StopfMaschine(true, tmpTasterPressed);
        HuelseDrauf = false;

        zustand = AUSRICHTEN;
      }
      break;
  }
  if (zustand != last_zustand){
    Serial.print("Neuer Zustand: ");
    Serial.println(zustand_txt[zustand]);
    last_zustand = zustand;
  }
  //Serial.println(zustand + "\t" + last_zustand);

  if(zustand != AUS && zustand != AUSRICHTEN && zustand != FEHLER)
  {
    switch(tmpTasterPressed)
    {
      case pinTasterStop:
        if(OneCycle && zustand != STOPFEN) 
        {
          zustand = FEHLER;
        }else{ 
          OneCycle = true;
          LED.setFarbe(clsLED::ORANGE);
        }
        break;
      case pinTasterStart:
        OneCycle = false;
        LED.setFarbe(clsLED::GRUEN);
    }
  }
}


//rückgabe wert: 0 = fehler,  1 = noch nicht fertig, 2 = fertig
uint8_t StopfMaschine(bool resetZustand, uint8_t &tmpTasterPressed) 
{
  static enum { AUS, EIN, START, STOPFEN, FEHLER } zustand = AUS;
  static bool ServoEndPosOk = false;

  if(resetZustand)
  {
    zustand = AUS;
    ServoEndPosOk = false;
    return 2;
  }

  if(!ServoStartEnable)
  {
    if(zustand == START || zustand == STOPFEN)
    {
      //falls die PM die HOPPER meldung hat oder die PM pausieren will
      if(tmpTasterPressed == pinTasterStart)  
      {
        digitalWrite(pinPMStart, HIGH);   //Signal an die PM senden
        delay(20);
        digitalWrite(pinPMStart, LOW);

        tmpTasterPressed = 0; //Löschen damit in der loop nix passiert
      }
    } 
  }

  switch (zustand) 
  {
  case AUS:
    if (checkEndschalter()) 
      {zustand = EIN;}
    else 
      {zustand = FEHLER;}
    break;
  case EIN:
    if(ServoStartEnable)
    {
      if(!ServoEndPosOk)
      {
        if(runStartServo(ServoStartEndPos)) ServoEndPosOk = true;
      }else{
        if(runStartServo(ServoStartAnfangPos)) 
        {
          ServoEndPosOk = false;
          zustand = START;
        }
      }
    }else{
      digitalWrite(pinPMStart, HIGH);   //Signal an die PM senden zum Starten
      delay(20);
      digitalWrite(pinPMStart, LOW);
      zustand = START;
    }
    break;
  case START:
    if (!checkEndschalter()) zustand = STOPFEN;
    break;
  case STOPFEN:
    if (checkEndschalter()) 
    {
      zustand = AUS;
      return 2;
    }
    break;
  case FEHLER:
    zustand = AUS;
    return 0;
  }
  return 1;
}




bool runLadenServo(uint8_t endWinkel)
{
  static unsigned long prevMillis = 0;
  static uint8_t istWinkel = 0;
 
  if(millis() - prevMillis >= ServoLadenSpeed)
  {
    prevMillis = millis();
    if(istWinkel == endWinkel) 
    {
      if(istWinkel == ServoLadenAnfangPos)
        {ServoLadenAnfangPosOK = true;}
      else
        {ServoLadenAnfangPosOK = false;}

      return true;
    }else{
      if(istWinkel < endWinkel)
        {istWinkel += 1;}
      else
        {istWinkel -= 1;}
      
      ServoLadenAnfangPosOK = false;
      /*Serial.print("Winkel: ");
      Serial.print(istWinkel);
      Serial.print("\t Endwinkel: ");
      Serial.print(endWinkel);
      Serial.print("\t x=");
      Serial.println(map(istWinkel, ServoLadenAnfangPos, ServoLadenEndPos, 0, 100));*/
      servoLaden.write(istWinkel);
    }
  }
  return false;
}

bool runStartServo(uint8_t endWinkel)
{
  static unsigned long prevMillis = 0;
  static uint8_t istWinkel = 0;
 
  if(millis() - prevMillis >= ServoStartSpeed)
  {
    prevMillis = millis();
    if(istWinkel == endWinkel) 
    {
      if(istWinkel == ServoStartAnfangPos)
        {ServoStartAnfangPosOK = true;}
      else
        {ServoStartAnfangPosOK = false;}

      return true;
    }else{
      if(istWinkel < endWinkel)
        {istWinkel += 1;}
      else
        {istWinkel -= 1;}
      
      ServoStartAnfangPosOK = false;
      servoStart.write(istWinkel);
    }
  }
  return false;
}


uint8_t checkTaster(bool delayEin)
{ 
  static unsigned long prevMillis = 0;
  static bool isTasterPressed = false;

  //Tasten überprüfung alle 50 millisek.  20x die sek.
  if(millis() - prevMillis >= 50 || !delayEin)
  {
    prevMillis = millis();

    if(digitalRead(pinTasterStart) == LOW)
    {
      if(isTasterPressed && delayEin) return 0;
      isTasterPressed = true; 
      return pinTasterStart;
    }else if(digitalRead(pinTasterStop) == LOW)
    {
      if(isTasterPressed && delayEin) return 0; 
      isTasterPressed = true; 
      return pinTasterStop;
    }else{
      isTasterPressed = false;
      prevMillis += 50;
    }
  }
  return 0;
}


bool checkEndschalter()
{
  static unsigned long prevMillis = 0;
  static bool lastState = false; 
  
  if(millis() - prevMillis >= 50)
  {
    prevMillis = millis();
    if(digitalRead(pinEndschalter) == HIGH)
      {lastState = true;}
    else
      {lastState = false;}
  }
  return lastState;
}