//==========================//
//Bibliotheken//
//==========================//

#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ezButton.h>

//==========================//
//Konfiguration der Pins und Variablen//
//==========================//

//Sensorik
const int LDR1_PIN = A0;  // Pin für Sensor LDR1
const int LDR2_PIN = A1;  // Pin für Sensor LDR2
const int LDR3_PIN = A2;  // Pin für Sensor LDR3
const int LDR4_PIN = A3;  // Pin für Sensor LDR4
double LDR1, LDR2, LDR3, LDR4; // Sensorwerte für LDRs

//Motoren
const int MOTOR1_PIN = 9; // Pin für Motor1
const int MOTOR2_PIN = 10; // Pin für Motor2
Servo Motor1;
Servo Motor2;
int poshorizontal = 90;        // Horizontale Position für Motor1 (90 Grad)
int posvertikal = 90;          // Vertikale Position für Motor2 (90 Grad)

//Display
const int SDA_PIN = 20;    // Pin für SDA
const int SCL_PIN = 21;    // Pin für SCL
double Kapazitaet;             // Batteriekapazität in %
double Spannung;               // Batteriespannung
LiquidCrystal_I2C lcd(0x27, 20, 4); // I2C-Adresse: 0x27, 16 Spalten, 2 Zeilen

//Analog-Stick
const int X_AXIS_PIN = A4; // Pin für X-Achse des Joysticks
const int Y_AXIS_PIN = A5; // Pin für Y-Achse des Joysticks
const int SWITCH_PIN = 2;  // Pin für Schalter
int xValue;
int yValue;
bool Switch;


bool Betrieb = true;           // Betriebsmodus: Automatisch (true) oder Manuell (false)
bool Merker = true;            // Merker für Betriebsumschaltung


void setup() {
//==========================//
//Initialisierung der Pins und Komponenten
//==========================//

  //Sensorik
  pinMode(LDR1_PIN, INPUT);
  pinMode(LDR2_PIN, INPUT);
  pinMode(LDR3_PIN, INPUT);
  pinMode(LDR4_PIN, INPUT);

  //Motoren
  pinMode(MOTOR1_PIN, OUTPUT);
  pinMode(MOTOR2_PIN, OUTPUT);
  Motor1.attach(MOTOR1_PIN);
  Motor2.attach(MOTOR2_PIN);

  //Display
  pinMode(SDA_PIN, INPUT);
  pinMode(SCL_PIN, INPUT);
  lcd.begin(16, 2);
  lcd.clear();

  //Switch
  pinMode(X_AXIS_PIN, INPUT);
  pinMode(Y_AXIS_PIN, INPUT);
  pinMode(SWITCH_PIN, INPUT);


  Serial.begin(9600); // Starte serielle Kommunikation bei 9600 Baud
}

void loop() 
{
//==========================//
//Betriebsumstellung
//==========================//
  Betriebsumstellung(); //Hier wird mit dem Switch zwischen manuellem und automatischem Betrieb geschaltet

//==========================//
//Positionsanpassung
//==========================//
  Positionsanpassung(); //Hier wird geschaut ob die Positionen im definierten Bereich sind. Wenn nicht werden sie entweder als 0 oder 180 definiert

//==========================//
//Motorenbewegung
//==========================//
  Motorenbewegung();  //Hier werden die Motoren bewegt

//==========================//
//Display
//==========================//
  Display();  //Hier werden die Werte auf dem Display ausgegeben.

  Serial.println(Switch);
}

void Betriebsumstellung()
{
  Switch = digitalRead(SWITCH_PIN); //Wert vom Switch wird gelesen => 0 oder 1

  // Wenn der Switch gedrückt wird, ändere den Betriebsmodus
  if(Switch == false) //Hier ist der Switch gedrückt
  {
  if (Switch != Merker) //Dies ist genau der Zeitpunkt beim Drücken (wenn kein delay eingebaut ist)
  {
    Betrieb = !Betrieb; //Betrieb wird umgeschaltet
  }
  }
    Merker = Switch;

  // Je nach Betriebsmodus den entsprechenden Modus ausführen
  if (Betrieb) // Wenn Betrieb true ist => Automatikbetrieb sonst Manuellbetrieb
  {
    AutomatikBetrieb();
  } 
  else 
  {
    ManuellBetrieb();
  }
}

//==========================//
//Manuelbetrieb
//==========================//
void ManuellBetrieb() 
{
  Serial.println("MANUELL");

//Einlesen der erforderlichen Stick-Werten
  xValue = analogRead(X_AXIS_PIN);
  yValue = analogRead(Y_AXIS_PIN);
  
  if (xValue <= 400) //links
  {
    posvertikal--;
  } 
  else if (xValue >= 624) //rechts
  {
    posvertikal++;
  }
  
  if (yValue <= 400) //unten
  {
    poshorizontal--;
  } 
  else if (yValue >= 624) //oben
  {
    poshorizontal++;
  }

}

//==========================//
//Automatikbetrieb
//==========================//
void AutomatikBetrieb() 
{
  Serial.println("AUTOMATISCH");

  //Einlesen der erforderlichen Sensor-Werten
  LDR1 = analogRead(LDR1_PIN) / 205.0;
  LDR2 = analogRead(LDR2_PIN) / 205.0;
  LDR3 = analogRead(LDR3_PIN) / 205.0;
  LDR4 = analogRead(LDR4_PIN) / 205.0;

  if (abs(LDR1 - LDR2) > (5*(2.0/100))) //Hier wird geschaut, ob sich die Differenz der Beiden in einem Toleranzbereich befindet
  {
    if (LDR1 > LDR2) //links 
    {
      poshorizontal++;
    } 
    else if (LDR2 > LDR1) //rechts
    {
      poshorizontal--;
    }
  }

  if (abs(LDR3 - LDR4) > (5*(2.0/100))) //Hier wird geschaut, ob sich die Differenz der Beiden in einem Toleranzbereich befindet 
  {
    if (LDR3 > LDR4) //unten
    {
      posvertikal++;
    } else if (LDR4 > LDR3) //oben
    {
      posvertikal--;
    }
  }


}

//==========================//
//Positionsanpassung
//==========================//
void Positionsanpassung() 
{
  if (poshorizontal < 0) 
  {
    poshorizontal = 0;
  } 
  else if (poshorizontal > 180) 
  {
    poshorizontal = 180;
  }

  if (posvertikal < 0) 
  {
    posvertikal = 0;
  } 
  else if (posvertikal > 180) 
  {
    posvertikal = 180;
  }
}

//==========================//
//Motorenbewegung
//==========================//
void Motorenbewegung() 
{
  Motor1.write(poshorizontal);
  Motor2.write(posvertikal);
  delay (500);
}

//==========================//
//Display
//==========================//
void Display() 
{
  // Kapazität der Batterie
  double Kapazitaet = Spannung / (abs(9.9 - 5.4)); //Hier gehört die Maximal- und Minimalspannung eingetragen

  lcd.clear();
  lcd.backlight();

  lcd.setCursor(0, 0);
  lcd.print("Kapazitaet: ");
  lcd.setCursor(15, 0);
  lcd.print(Kapazitaet);
  lcd.print("%");

  lcd.setCursor(0, 1);
  lcd.print("Servo1:  ");
  lcd.setCursor(15, 1);
  lcd.print(poshorizontal);
  lcd.print("°");

  lcd.setCursor(0, 2);
  lcd.print("Servo2:  ");
  lcd.setCursor(15, 2);
  lcd.print(posvertikal);
  lcd.print("°");
}