#include <ESP32Servo.h>

// SerialPrint als Debug
#define DEBUG 1

#if DEBUG == 1
#define debug(x) Serial.print(x)
#define debugg(x, y) Serial.print(x, y)
#define debugln(x) Serial.println(x)
#define debuggln(x, y) Serial.println(x, y)
#else
#define debug(x)
#define debugg(x, y)
#define debugln(x)
#define debugg(x, y)
#endif

// Schalter
#define BTN_Pin 4

// Poti
#define POT_Pin 0

// Joystick
#define JOY_V 32
#define JOY_H 33
#define JOY_SEL 19

// LEDs
#define LED_links 18
#define LED_rechts 5
#define LED_oben 16
#define LED_unten 17

// Servos
#define ServoVert_Pin 25
Servo ServoVert;
#define ServoHorz_Pin 26
Servo ServoHorz;

// Variablen
bool btn;
int vert = 90, horz = 90;
int ServoMin = 0, ServoMax = 180;
bool sel;
byte BrightnessMin = 10, BrightnessMax = 255, Brightness = BrightnessMax;
byte FadeRichtung = 0; // 0 = dunkler faden, 1 = heller faden

// Timer
unsigned long CurrMillis, PrevMillisServo, PrevMillisLED;
long LEDInterval = 10; // Dimmgeschwindigkeit LED; je kleiner, desto schneller
long ServoInterval = 15; // Servogeschwindigkeit

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);

  pinMode(BTN_Pin, OUTPUT);
  pinMode(POT_Pin, INPUT);

  pinMode(JOY_V, INPUT);
  pinMode(JOY_H, INPUT);
  pinMode(JOY_SEL, INPUT_PULLUP);

  pinMode(LED_links, OUTPUT);
  digitalWrite(LED_links, LOW);
  pinMode(LED_rechts, OUTPUT);
  digitalWrite(LED_rechts, LOW);
  pinMode(LED_oben, OUTPUT);
  digitalWrite(LED_oben, LOW);
  pinMode(LED_unten, OUTPUT);
  digitalWrite(LED_unten, LOW);

  ServoVert.attach(ServoVert_Pin, 500, 2500);
  ServoVert.write(vert);
  ServoHorz.attach(ServoHorz_Pin, 500, 2500);
  ServoHorz.write(horz);
  debugln("Starte Servos!");
  Datenausgabe();
}
/*
  void BlinkVert(byte pin, long freq) {
  if (CurrMillis - PrevMillis >= freq) {
    digitalWrite(pin, HIGH);
    PrevMillis = CurrMillis;
  } else {
    digitalWrite(pin, LOW);
  }
  }

  void BlinkHorz(byte pin, long freq) {
  if (CurrMillis - PrevMillis >= freq) {
    digitalWrite(pin, HIGH);
    PrevMillis = CurrMillis;
  } else {
    digitalWrite(pin, LOW);
  }
  }
*/

void Datenausgabe() {
  debug("Vert = ");
  debug(vert);
  debug(", ServoVertPos = ");
  debugln(ServoVert.read());
  debug("Horz = ");
  debug(horz);
  debug(", ServoHorzPos: ");
  debugln(ServoHorz.read());
  debug("Brightness: ");
  debugln(Brightness);
}

void loop() {
  CurrMillis = millis();

  // Schalter abfragen
  btn = digitalRead(BTN_Pin) == HIGH;

  if (btn == true) {

    ////////////////////////// LED anschalten
    digitalWrite(LED_oben, Brightness);

    ////////////////////////// LED-Helligkeit mit Joystick-Knopf dimmen
    sel = digitalRead(JOY_SEL) == LOW;
    if (CurrMillis - PrevMillisLED >= LEDInterval && sel == true) {
      switch (FadeRichtung) {
        case 0: // dunkler faden
          Brightness--;
          if (Brightness < BrightnessMin) {
            Brightness = BrightnessMin;
            FadeRichtung = 1;
          }
          break;

        case 1: // heller faden
          Brightness++;
          if (Brightness > BrightnessMax) {
            Brightness = BrightnessMax;
            FadeRichtung = 0;
          }
          break;
      }
      PrevMillisLED = CurrMillis;
      Datenausgabe();
    }

    ////////////////////////// LED-Helligkeit mit Poti dimmen
    //Brightness = map(analogRead(POT_Pin), 0, 1023, 0, 255);

    ////////////////////////// Servos rumfahren
    if (CurrMillis - PrevMillisServo >= ServoInterval) {
      ////////////////////////// ServoVert rumfahren
      if (map(analogRead(JOY_V), 0, 4095, ServoMin, ServoMax) < 90) {
        vert --;
        if (vert < ServoMin) {
          vert = ServoMin;
        }
        Datenausgabe();
      } else if (map(analogRead(JOY_V), 0, 4095, ServoMin, ServoMax) > 90) {
        vert ++;
        if (vert > ServoMax) {
          vert = ServoMax;
        }
        Datenausgabe();
      }
      ServoVert.write(vert);

      ////////////////////////// ServoHorz rumfahren
      if (map(analogRead(JOY_H), 0, 4095, ServoMin, ServoMax) < 90) {
        horz ++;
        if (horz > ServoMax) {
          horz = ServoMax;
        }
        Datenausgabe();
      } else if (map(analogRead(JOY_H), 0, 4095, ServoMin, ServoMax) > 90) {
        horz --;
        if (horz < ServoMin) {
          horz = ServoMin;
        }
        Datenausgabe();
      }
      ServoHorz.write(horz);

      PrevMillisServo = CurrMillis;
    }
  }

  ////////////////////////// Servos in Startposition fahren & LED ausschalten, wenn Schalter auf LOW ist
  if (btn == false) {
    while (vert != 90 || horz != 90) {
      if (vert < 90) {
        vert ++;
        ServoVert.write(vert);
        delay(15);
        Datenausgabe();
      } else if (vert > 90) {
        vert --;
        ServoVert.write(vert);
        delay(15);
        Datenausgabe();
      }

      if (horz < 90) {
        horz ++;
        ServoHorz.write(horz);
        delay(15);
        Datenausgabe();
      } else if (horz > 90) {
        horz --;
        ServoHorz.write(horz);
        delay(15);
        Datenausgabe();
      }
    }
    ////////////////////////// LED ausschalten
    digitalWrite(LED_oben, LOW);
  }

  /*
      ////////////////////////// LEDS blinken lassen

    vert = map(analogRead(JOY_V), 0, 4095, -100, 100); // unten = -, oben = +
    horz = map(analogRead(JOY_H), 0, 4095, -100, 100); // rechts = -, links = +
    sel = digitalRead(JOY_SEL) == LOW;

      debug("Vertikal: ");
      debugln(vert);

      switch (vert) {
        case -100 ... -51:
          BlinkVert(LED_unten, 250);
          break;
        case -50 ... -1:
          BlinkVert(LED_unten, 500);
          break;
        case 0:
          // nix tun
          break;
        case 1 ... 50:
          BlinkVert(LED_oben, 500);
          break;
        case 51 ... 100:
          BlinkVert(LED_oben, 250);
          break;
      }

      debug("Horizontal: ");
      debugln(horz);

      switch (horz) {
        case -100 ... -51:
          BlinkHorz(LED_rechts, 250);
          break;
        case -50 ... -1:
          BlinkHorz(LED_rechts, 500);
          break;
        case 0:
          // nix tun
          break;
        case 1 ... 50:
          BlinkHorz(LED_links, 500);
          break;
        case 51 ... 100:
          BlinkHorz(LED_links, 250);
          break;
      }

    if (sel == true) {
      debugln("Taster AN");
    } else {
      debugln("Taster AUS");
    }
  */
}