// Códigos e projetos de base: https://wokwi.com/projects/387130527094355969.
// e https://wokwi.com/projects/396512177128928257.

#include <Wire.h> // Importa a biblioteca para I2C.
#include <Adafruit_PWMServoDriver.h> // Importa a biblioteca para o controle do servomotor.
#include <LiquidCrystal_I2C.h> // Importa a biblioteca para o LCD I2C.

LiquidCrystal_I2C lcd(0x27, 16, 2); // Define o endereço I2C do LCD e seu tamanho.

#define PCA9685_ADDRESS  0x40 // Define o endereço I2C da placa PCA9685.
// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(PCA9685_ADDRESS);

#define SERVOMIN 150    // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 600   // This is the 'maximum' pulse length count (out of 4096)
#define SERVO_FREQ 50  // Analog servos run at ~50 Hz updates

// Adjust these angles according to Servo360_PCA_AngleFInd code that I have provided
int leftAngle = 0;     // angel for motor to move left (If the motor move in right you can change the name to rightAngle)
int rightAngle = 180;  // angel for motor to move right (If the motor move in left you can change the name to leftAngle)
int stopAngle = 90;    // Angle when motor stops

// our servo # counter
uint8_t servonum = 0;
byte totalServos = 1;  // Define how many servos we are using here

// Define the time (in milliseconds 1 second = 1000 millisecond) for each servo to move in right direction
int rightMoveTime[13] = { 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000 };  // Wait for this millisecond when motor move right
int stopWaitTime[13] = { 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000 };   // Wait for this millisecond after motor has reached it's point
int leftMoveTime[13] = { 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000 };   // Wait for this millisecond when motor move left

// Define a posição inicial do cursor do display:
int cursorX = 0;
int cursorY = 0;

int p = 0;
int m = 0;

void setup() {
  pinMode(34, INPUT);

  lcd.init();
  lcd.backlight();
  
  pwm.begin();  // Initialize PWM instance
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates

  // Stop all servos
  for (int servoNum = 0; servoNum < totalServos; servoNum++) {  // initially stop all servo motors
    pwm.setPWM(servoNum, 0, getAnglePulse(stopAngle));
  }
}

void loop() {
  
  m = 0;
  p = 0;

  // Enquanto a chave estiver em nível lógico alto o servomotor é operado.
  while(digitalRead(34) == HIGH){
    if(m == 0){
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Servomotor");
      lcd.setCursor(0, 1);
      lcd.print("ligado.");
      m = 1;
    }
    // Run one servo at a time
    for (int currentServo = 0; currentServo < totalServos; currentServo++) {
      pwm.setPWM(currentServo, 0, getAnglePulse(rightAngle));  // First move servo to right side
      delay(rightMoveTime[currentServo]);                                    // move motor in right for this time defined in start
      pwm.setPWM(currentServo, 0, getAnglePulse(stopAngle));   // Stop servo at this point
      delay(stopWaitTime[currentServo]);                                    // Keep motor stopped at this position defined in start
      pwm.setPWM(currentServo, 0, getAnglePulse(leftAngle));   // Now move servo to left side back
      delay(leftMoveTime[currentServo]);                                     // move motor in left for this time defined in start
      pwm.setPWM(currentServo, 0, getAnglePulse(stopAngle));   // Stop This servo
      delay(50);                                              // wait for 50 milliseconds before moving next servo
      }
  }
  // Enquanto a chave estiver em nível lógico baixo o servomotor é mantido desligado.
  while (digitalRead(34) == LOW) {
    if(p == 0){
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Servomotor");
      lcd.setCursor(0, 1);
      lcd.print("desligado.");
      p = 1;
    }
    } 
}
int getAnglePulse(int inpAngle) {
  int retPulse = map(inpAngle, leftAngle, rightAngle, SERVOMIN, SERVOMAX);
  return retPulse;
}