// von https://github.com/PaulStoffregen/Encoder
#include <Encoder.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

const byte knopf = 8, pinA = 2, pinB = 3, servoPin = 12;

LiquidCrystal_I2C lcd(0x27, 16, 2);
Encoder dreher(pinA, pinB);
Servo servo;

struct Settings {
  int links;
  int rechts;
  float periode;        // 1.0 bis 5.0 Sekunden
} einstellung = {0, 180, 2.5};

void printData(void) {
  char buf[17];
  sprintf(buf, "%3d  %3d  ", einstellung.links, einstellung.rechts, (int)10*einstellung.periode);
  lcd.setCursor(0, 1);
  lcd.print(buf);
  lcd.print(einstellung.periode, 1);
  lcd.print(" s");
}

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

  // Init
  lcd.init();
  lcd.backlight();

  pinMode(knopf, INPUT_PULLUP);
  servo.attach(servoPin);

  // Print Info-Zeile und  
  lcd.setCursor(0, 0);
  lcd.print("L:*  R:   1/f:  ");
  printData();
}

enum { M_LINKS, M_RECHTS, M_FREQ } modus = M_LINKS;

void knopfdruck(void)
{
  switch(modus)
  {
    case M_LINKS:
      lcd.setCursor(2, 0); lcd.print(' ');
      lcd.setCursor(7, 0); lcd.print('*');
      modus = M_RECHTS;
      break;
    case M_RECHTS:
      lcd.setCursor(7, 0);  lcd.print(' ');
      lcd.setCursor(14, 0); lcd.print('*');
      modus = M_FREQ;
      break;
    case M_FREQ:
      lcd.setCursor(14, 0);  lcd.print(' ');
      lcd.setCursor(2, 0); lcd.print('*');
      modus = M_LINKS;
      break;
  }
}
void bewegung(char delta) // +1 oder -1
{
  switch(modus)
  {
    case M_LINKS:
      einstellung.links += delta;
      einstellung.links = constrain(einstellung.links, 0, 180);
      printData();
      break;
    case M_RECHTS:
      einstellung.rechts += delta;
      einstellung.rechts = constrain(einstellung.rechts, 0, 180);
      printData();
      break;
    case M_FREQ:
      einstellung.periode += delta * 0.1;
      if(einstellung.periode > +8.0) einstellung.periode = +8.0;
      else if(einstellung.periode < +1.0) einstellung.periode = 1.0;      
      break;
  }
  printData();
}

// Sägezahn-Funktion:
void servoNachstellen(void) // alle 100ms
{
  const float dt = 0.1; // Sekunden
  float anteil = dt / einstellung.periode;

  static boolean aufsteigend = true;
  static float phase = 0.0;         // 0 bis 1

  if(aufsteigend)
  {
    phase += anteil;
    if(phase > 1.0)
    {
      phase = 1.0 - (phase - 1.0);
      aufsteigend = false;
    }
  }
  else
  {
    phase -= anteil;
    if(phase < 0.0)
    {
      phase = -phase;
      aufsteigend = true;
    }
  }
  // kein direktes Map, da float: 
  Serial.println(phase, 1);

  int winkel = (einstellung.rechts - einstellung.links) * phase + einstellung.links + 0.5;

  servo.write(winkel);

}


void loop()
{
  uint32_t jetzt = millis();

  // Alle 100ms die Servo-Steuerung aufrufen:
  static uint32_t nextCall = 0;
  if(jetzt >= nextCall)
  {
    servoNachstellen();
    nextCall += 100;    // isochron
  }

  // Knopf erkennen:
  static uint32_t knopfSperre = 0;
  if(jetzt >= knopfSperre)
  {
    static byte zuvor = HIGH;
    byte zustand = digitalRead(knopf);
    if(zustand == LOW && zuvor == HIGH)
    {
      knopfdruck();
      knopfSperre = jetzt + 5;
    }
    else if(zustand == HIGH && zuvor == LOW)
    {
      knopfSperre = jetzt + 5;
    }
    zuvor = zustand;
  }

  // Drehgeber:
  static long alt = 0;
  long neu = dreher.read();
  long delta = neu - alt;
  if (delta >= 4) {
    bewegung(-1);
    alt = neu;
  }
  else if (delta <= -4) {
    bewegung(+1);
    alt = neu;
  }
}