//OLED implementieren
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

//Encoder implementieren
#include <Encoder.h>
Encoder enco(2,3);

#include <Servo.h>
Servo servotilt;
Servo servorot;

byte menu_position = 0;
byte menu_position_neu = 0;
int pos_poti_rot;
int pos_poti_tilt;

void setup() {
  Serial.begin(9600);
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Old Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  } 
  pinMode(4, INPUT_PULLUP);
  enco.write(0); //Encoder auf Null setzen
  display.clearDisplay();
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(26,20);
  display.println("MOTIONCAMSTER");
  display.setCursor(26,35);
  display.println("by C.K.  V1.0");
  display.drawLine(24, 30, 104, 30, SSD1306_WHITE);
  display.display();
  delay(50);
  display.clearDisplay();
  display.display();
  display_head();
  display_mainmenu();
  servorot.attach(6);
  servotilt.attach(5);
}

void loop() {
  pos_poti_rot = lese_poti_rot();
  pos_poti_tilt = lese_poti_tilt();
  servo_rot(pos_poti_rot);
  servo_tilt(pos_poti_tilt);

  menu_auswahl(3);
  if (digitalRead(4) == LOW){
    if (menu_position==1){
      display_menu_manuell();
      delay(100);
    }
    if (menu_position>=2){
      display_menu_einstellungen();
      menu_auswahl(4);
      delay(100);
    }
    if (menu_position==0){
      display_menu_sequenz();
      menu_auswahl(4);
      delay(100);
    }
    if (menu_position>=255){
      display.drawRoundRect(5,24,118,10,3,0);
      display.drawRoundRect(5,34,118,10,3,0);
      display.drawRoundRect(5,44,118,10,3,0);
      display.drawRoundRect(5,54,118,10,3,0);
      display.display();
    }
  }  
}

int encoder_status(int encoder_zeilen){
  menu_position_neu = enco.read()/4;
    if (menu_position_neu != menu_position) {
    menu_position = menu_position_neu;
    Serial.println(menu_position_neu);
  }
  if (menu_position >= 250){
    enco.write(0);
    menu_position = 0;
  }
  if (menu_position == encoder_zeilen) {
    enco.write((encoder_zeilen*4) -4);
    menu_position = (encoder_zeilen-1);
  }
 // Serial.println(menu_position);
  return menu_position;
}

void menu_auswahl(int zeilen) {
  switch (encoder_status(zeilen)) {
  case 0:
    display.drawRoundRect(5,34,118,10,3,0);
    display.drawRoundRect(5,44,118,10,3,0);
    display.drawRoundRect(5,54,118,10,3,0);
    display.drawRoundRect(5,24,118,10,3,SSD1306_WHITE);
    display.display();
    break;
  case 1:
    display.drawRoundRect(5,24,118,10,3,0);
    display.drawRoundRect(5,44,118,10,3,0);
    display.drawRoundRect(5,54,118,10,3,0);
    display.drawRoundRect(5,34,118,10,3,SSD1306_WHITE);
    display.display();
    break;
  case 2:
    display.drawRoundRect(5,24,118,10,3,0);
    display.drawRoundRect(5,34,118,10,3,0);
    display.drawRoundRect(5,54,118,10,3,0);
    display.drawRoundRect(5,44,118,10,3,SSD1306_WHITE);
    display.display();
    break;
  case 3:
    display.drawRoundRect(5,24,118,10,3,0);
    display.drawRoundRect(5,34,118,10,3,0);
    display.drawRoundRect(5,44,118,10,3,0);
    display.drawRoundRect(5,54,118,10,3,SSD1306_WHITE);
    display.display();
    break;
  }
}

void display_head() {
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.println("Seq: <empty>");
  display.setCursor(0,10);
  display.print("Time:");
  display.display();
}

void display_mainmenu(){
  display.fillRect(0,18,128,49,0);
  display.display();
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10,25);
  display.println("Sequenz");
  display.setCursor(10,35);
  display.print("Manuell");
  display.setCursor(10,45);
  display.print("Einstellungen");
  display.display();
}

void display_menu_sequenz(){
  display.fillRect(0,18,128,49,0);
  display.display();
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10,25);
  display.println("Laden/Starten");
  display.setCursor(10,35);
  display.print("Speichern");
  display.setCursor(10,45);
  display.print("Erstellen");
  display.setCursor(10,55);
  display.print("CLR/Neu");
  display.display();
}

void display_menu_einstellungen(){
  display.fillRect(0,18,128,49,0);
  display.display();
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10,25);
  display.println("Datum");
  display.setCursor(10,35);
  display.print("Uhrzeit");
  display.setCursor(10,45);
  display.print("Servo Endlagen");
  display.setCursor(10,55);
  display.print("Stepper Endlagen");
  display.display();
}

void display_menu_manuell() {
  display.fillRect(0,18,128,49,0);
  display.display();
  display.setCursor(0,25); 
  display.print("Rot Angle: \xF7 ");
  display.setCursor(80,25); 
  display.print(pos_poti_rot);
  display.setCursor(0,35); 
  display.print("Tilt Angle:\xF7 ");
  display.setCursor(80,35); 
  display.print(pos_poti_tilt);
  display.setCursor(0,45); 
  display.print("Stp Dist: %");
  display.display();
}

int lese_poti_rot(){
  return map(analogRead(A7), 0, 1023, 0, 180);
}

int lese_poti_tilt(){
  return map(analogRead(A6), 0, 1023, 0, 180); 
}

void servo_rot(int pos){
  servorot.write(pos);
}

void servo_tilt(int pos){
  servotilt.write(pos);
}