// C++ code
//
#include <Adafruit_LiquidCrystal.h>

#include <Servo.h>

#include "DFRobot_RGBLCD1602.h"

const int colorR = 255;
const int colorG = 255;
const int colorB = 255;

DFRobot_RGBLCD1602 lcd(/*lcdCols*/16,/*lcdRows*/2);  //16 characters and 2 lines of show

int tl = 0; //beginwaarde top left
int tr = 0; //beginwaarde top right
int dl = 0; //beginwaarde down left
int dr = 0; //beginwaarde down right
int servo1pos = 90;
int servo2pos = 90;

int speed = 30;
int tol = 30;

Servo servo1; // horizontal sero

Servo servo2; // vertical servo


void setup()
{

  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);
  servo1.attach(10); //servo horizontaal 
  servo1.write(90);
  servo2.attach(9); //servo vertikaal 
  servo2.write(90);
  Serial.begin(9600);

  lcd.init();  
  lcd.setRGB(colorR, colorG, colorB);
  lcd.print("hello sun");
  delay(2000); // Wait for 2000 millisecond(s)
  lcd.clear();
  delay(1000); // Wait for 1000 millisecond(s)
 
  lcd.setCursor(0, 0);
  lcd.print("TL:");
  
  lcd.setCursor(0, 1);
  lcd.print("DL:");
  
  lcd.setCursor(8, 0);
  lcd.print("TR:");
  
  lcd.setCursor(8, 1);
  lcd.print("DR:");
  
}

void loop()
{
  int tl = analogRead(A3);
  lcd.setCursor(4, 0);
  lcd.print(tl);

  int tr = analogRead(A2);
  lcd.setCursor(12, 0);
  lcd.print(tr);

  int dl = analogRead(A1);
  lcd.setCursor(4, 1);
  lcd.print(dl);

  int dr = analogRead(A0);
  lcd.setCursor(12, 1);
  lcd.print(dr);

  delay(100); // Wait for 100 millisecond(s)

   int avt = (tl + tr)/2; // gemiddelde waarde top
   int avd = (dl + dr)/2; // gemiddelde waarde down
   int avl = (tl + dl)/2; // gemiddelde waarde links
   int avr = (tr + dr)/2; // gemiddelde waarde rechts


if (-1*tol > (avl - avr) || (avl - avr) > tol) { //ZON ZIT AAN LINKER OF RECHTER KANT > SERVO 2 DRAAIT


  if (avl > avr){ // ZON ZIT LINKS > SERVO 2 DRAAIT NAAR 60
    
    for (servo2pos >= 0; servo2pos <= 60; servo2pos += 1) {  
    servo2.write (servo2pos);
    delay (speed);
    }
    for (servo2pos <= 180; servo2pos >= 60; servo2pos -= 1) {  
    servo2.write (servo2pos);
    delay (speed);
    }

  }

  else if (avl < avr) { // ZON ZIT RECHTS > SERVO 2 DRAAIT NAAR 120

    for (servo2pos >= 0; servo2pos <= 120; servo2pos += 1) {  
    servo2.write (servo2pos);
    delay (speed);
    }
    for (servo2pos <= 180; servo2pos >= 120; servo2pos -= 1) {  
    servo2.write (servo2pos);
    delay (speed);
    }

  }

}

if (-1*tol > (avt - avd) || (avt - avd) > tol) { // ZON ZIT BOVEN OF ONDER > SERVO 1 DRAAIT


  if (avt > avd) { // ZON ZIT BOVEN > SERVO 1 DRAAIT NAAR 0

    for (servo1pos >= 0; servo1pos <= 20; servo1pos += 1) {  
    servo1.write (servo1pos);
    delay (speed);
    }
    for (servo1pos <= 180; servo1pos >= 20; servo1pos -= 1) {  
    servo1.write (servo1pos);
    delay (speed);
    }

  }

  else if (avd > avt) { // ZON ZIT ONDER > SERVO 1 DRAAIT NAAR 180

    for (servo1pos >= 0; servo1pos <= 160; servo1pos += 1) {  
    servo1.write (servo1pos);
    delay (speed);
    }
    for (servo1pos <= 180; servo1pos >= 160; servo1pos -= 1) {  
    servo1.write (servo1pos);
    delay (speed);
    }

  }

} 

} //CLOSE LOOP