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

LiquidCrystal_I2C lcd(0x27, 16, 2);

// ===== Servos =====
Servo servo1, servo2, servo3;
int ang1 = 90, ang2 = 90, ang3 = 90;

// ===== Keypad =====
const byte ROWS = 4;
const byte COLS = 4;

char keys[ROWS][COLS] = {
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};

byte rowPins[ROWS] = {2,3,4,5};
byte colPins[COLS] = {6,7,8,12};

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

// ===== Função para atualizar LCD =====
void updateLCD() {
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("S1:");
  lcd.print(ang1);
  lcd.print(" S2:");
  lcd.print(ang2);
  lcd.setCursor(0,1);
  lcd.print("S3:");
  lcd.print(ang3);
}

void setup() {
  servo1.attach(9);
  servo2.attach(10);
  servo3.attach(11);

  lcd.init();
  lcd.backlight();

  updateLCD();
}

void loop() {
  char key = keypad.getKey();

  if (key) {
    switch (key) {
      case '1': ang1 = max(0, ang1 - 5); break;
      case '2': ang1 = min(180, ang1 + 5); break;

      case '4': ang2 = max(0, ang2 - 5); break;
      case '5': ang2 = min(180, ang2 + 5); break;

      case '7': ang3 = max(0, ang3 - 5); break;
      case '8': ang3 = min(180, ang3 + 5); break;

      case '*':
        ang1 = ang2 = ang3 = 90;
        break;
    }

    servo1.write(ang1);
    servo2.write(ang2);
    servo3.write(ang3);
    updateLCD();
    delay(150);
  }
}