#include <ESP32Servo.h>

#define LDRCE 27
#define LDRCD 12
#define LDRBE 26
#define LDRBD 14
#define servo1 18
#define servo2 5

Servo horizontal;
int servoh = 90; // variável para armazenar a posição do Servo horizontal
int servohLimitHigh = 180; // limite máximo que o servo horizontal rotaciona
int servohLimitLow = 65; // limite mínimo que o servo horizontal rotaciona

Servo vertical; // Servo da vertical
int servov = 90; // variável para armazenar a posição do Servo vertical
int servovLimitHigh = 120; // limite máximo que o servo vertical rotaciona
int servovLimitLow = 15; // limite mínimo que o servo vertical rotaciona

int pos1 = 0;
int pos2 = 0;

void setup() {
  Serial.begin(115200);
  pinMode(LDRCE, INPUT);
  pinMode(LDRCD, INPUT);
  pinMode(LDRBE, INPUT);
  pinMode(LDRBD, INPUT);

  horizontal.attach(servo1);
  vertical.attach(servo2);
  horizontal.write(180); // move o eixo do Servo horizontal
  vertical.write(45); // move o eixo do Servo vertical
  delay(3000);
}

void loop() {
  int CE = analogRead(LDRCE); // LDR em cima a esquerda
  int CD = analogRead(LDRCD); // LDR em cima a direita
  int BE = analogRead(LDRBE); // LDR em baixo a esquerda
  int BD = analogRead(LDRBD); // LDR em baixo a direita

  int dtime = 10; // valor do delay entre as rotações do Servo
  int tol = 20; // valor estabelecido de tolerância

  int MC = (CE + CD) / 2; // média das leituras dos LDRs de cima
  int MB = (BE + BD) / 2; // média das leituras dos LDRs de baixo
  int ME = (BE + CE) / 2; // média das leituras dos LDRs à esquerda
  int MD = (CD + BD) / 2; // média das leituras dos LDRs à direita
  int dvert = MC - MB; // checa a diferença entre as médias de cima e baixo
  int dhoriz = ME - MD; // checa a diferença entre as médias da esquerda e direita

  // print dos valores no Monitor Serial
  Serial.print(MC);
  Serial.print(" ");
  Serial.print(MB);
  Serial.print(" ");
  Serial.print(ME);
  Serial.print(" ");
  Serial.print(MD);
  Serial.print(" ");
  Serial.print(dtime);
  Serial.print(" ");
  Serial.print(tol);
  Serial.println(" ");
  delay(50); // pequeno delay para que possamos realmente ver os números

  if (-tol > dvert || dvert > tol) { // verifica se a diferença está fora da tolerância, se sim, muda o ângulo do Servo vertical
    if (MC > MB) {
      servov++;
      if (servov > servovLimitHigh) {
        servov = servovLimitHigh;
      }
    }
    else if (MC < MB) {
      servov--;
      if (servov < servovLimitLow) {
        servov = servovLimitLow;
      }
    }
    vertical.write(servov);
  }

  if (-tol > dhoriz || dhoriz > tol) { // verifica se a diferença está fora da tolerância, se sim, muda o ângulo do Servo horizontal
    if (ME > MD) {
      servoh--;
      if (servoh < servohLimitLow) {
        servoh = servohLimitLow;
      }
    }
    else if (ME < MD) {
      servoh++;
      if (servoh > servohLimitHigh) {
        servoh = servohLimitHigh;
      }
    }
    else if (ME == MD) { // checa se a média dos LDRs à esquerda é igual à média dos LDRs à direita
      // nenhum comando
    }
    horizontal.write(servoh);
  }

  delay(dtime); // aguarda o tempo especificado para o próximo loop
}