// Forensketch
// https://forum.arduino.cc/
// https://forum.arduino.cc/t/servosteuerung-mit-einem-taster/1209398
// to be deleted 2024-03

#include <Streaming.h>     // https://github.com/janelia-arduino/Streaming
#include "Servo.h"

//#define DEBUG              // Wenn aktiviert, werden Zwischenwerte ausgegeben

#ifdef DEBUG
#define DBG_PRINTLN(...) Serial << __VA_ARGS__ << endl
#else
#define DBG_PRINTLN(...)
#endif

Servo servoOne;
Servo servoTwo;
constexpr byte servos {2};
Servo myServo[servos] {servoOne, servoTwo};

constexpr byte tastePin {4};


uint8_t servoAuswahl;
uint8_t servoPos[servos] = {0, 0};
bool servoRichtung = false;
bool lastRichtung = false;   // Merker

uint32_t ausloesezeit;
constexpr uint32_t debounce {40};
constexpr uint32_t tasterReaktion {500};
constexpr uint32_t servoReaktion {4000};

enum class STATE {warten, auswahl, position};
STATE state = STATE::warten;

void setup()
{
  Serial.begin(115200);
#if (defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2650)) // https://github.com/arduino/Arduino/issues/10764
  delay(300);
#endif
  Serial << (F("\r\nStart...\r\n")) << endl;
  DBG_PRINTLN(__FILE__);
  DBG_PRINTLN( __TIMESTAMP__);
  pinMode(tastePin, INPUT_PULLUP);
  myServo[0].attach(2);
  myServo[1].attach(3);
}

void  loop()
{
  switch (state)
  {
    case STATE::warten:
      if (tasteCheck())                               // Taste ausgelöst
      {
        ausloesezeit = millis();                      // zeit merken
        servoAuswahl = 0;                             // ersten Servo einstellen
        state = STATE::auswahl;                       // nächster Status
        Serial << "to auswahl\n";
      }
      break;
    case STATE::auswahl:
      if (millis() - ausloesezeit < tasterReaktion)   // Innerhalb der Reaktionszeit
      {
        if (tasteCheck())                             // Taste gedrückt
        {
          servoAuswahl++;  // nächsten Servo setzen
        }
        if (servoAuswahl >= servos)                   // Begrenzung
        {
          servoAuswahl = 0;  // von vorn anfangen
        }
      }
      else                                            // Zeit abgelaufen
      {
        if (!tasteCheck())                            // Taste losgelassen -> kann evtl. entfallen
        {
          servoRichtung = HIGH;                       // Vorbelegen
          lastRichtung = LOW;
          ausloesezeit = millis();
          state = STATE::position;                    // nächsten Status
          Serial << "to position\n";
        }
      }
      break;
    case STATE::position:
      if (!digitalRead(tastePin))  // Taste gedrückt
      {
        lastRichtung = servoRichtung;
        if (servoRichtung && servoPos[servoAuswahl] < 255)  // Aufwärts und unter oberer Grenze
        {
          servoPos[servoAuswahl]++;
        }
        else if (!servoRichtung && servoPos[servoAuswahl] > 0)
        {
          servoPos[servoAuswahl]--;
        }
      }
      else if (lastRichtung == servoRichtung)  // Taste losgelassen UND vorher gedrückt
      {
        servoRichtung = !servoRichtung;
      }
      if (millis() - ausloesezeit > servoReaktion)         // Zeit abgelaufen
      {
        state = STATE::warten;  // fange an von vorn
        Serial << "to warten\n";
      }
      break;
  }
}

bool tasteCheck()
{
  bool ret = false;
  static uint32_t lastPressed = 0;
  static bool lastState = LOW;
  if (!digitalRead(tastePin))                         // Taste gedrückt
  {
    if (!lastState)                                   // und vorher nicht
    {
      lastPressed = millis();                         // Zeit merken
      lastState = HIGH;                               // status setzen
      ret = true;                                     //
      Serial << "fire\n";
    }
  }
  else                                                // nciht gedrückt
  {
    if (lastState)                                    // vorher aber schon
    {
      if (millis() - lastPressed > debounce)          // Zeit abgelaufen
      {
        lastState = LOW;                              // merker löschen
      }
    }
  }
  return ret;
}