#include <HardwareSerial.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>
#include <ModbusRTU.h>
ModbusRTU mb;
Servo servo1, servo2;
LiquidCrystal_I2C lcd(0x27, 16, 2);
const int pinSwitch[] = {25, 26, 27, 14, 12, 13};
const int pinLed[] = {23, 19, 18, 5, 4, 0, 2, 15};
HardwareSerial SerialDWIN(2);
void setup() {
  Serial.begin(115200);
  SerialDWIN.begin(115200, SERIAL_8N1, 16, 17);
  mb.begin(&SerialDWIN);
  servo1.attach(32);
  servo2.attach(33);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("In1:   0 Ou1:  0");
  lcd.setCursor(0, 1);
  lcd.print("In2:   0 Ou2:  0");
  for (int i = 0; i < 6; i++) {
    pinMode(pinSwitch[i], INPUT_PULLUP);
  }
  for (int i = 0; i < 8; i++) {
    pinMode(pinLed[i], OUTPUT);
    digitalWrite(pinLed[i], LOW);
  }
  Serial.println("ESP32-Slave-DWIN-Master siap!");
  mb.slave(1);//ID Slave
  for (int i = 0; i < 28; i++) {
    mb.addHreg(i);//data Modbus di Holding Register
  }
}
void loop() {
  mb.task();
  for (int i = 0; i < 8; i++) {
    digitalWrite(pinLed[i], mb.Hreg(i));
  }
  static uint8_t lastStates[6] = {0};
  uint8_t currentState;
  for (int i = 0; i < 6; i++) {
    currentState = !digitalRead(pinSwitch[i]);
    if (currentState != lastStates[i]) {
      mb.Hreg(i + 10, currentState);
      lastStates[i] = currentState;
    }
  }
  uint16_t a = analogRead(34) > 0 ? 0 : 1;
  mb.Hreg(8, a);
  uint16_t b = analogRead(35) > 0 ? 0 : 1;
  mb.Hreg(9, b);
  uint16_t pot1 = analogRead(36);
  uint16_t grafik1 = map(pot1, 0, 4095, 0, 1000);
  mb.Hreg(16, pot1);
  mb.Hreg(18, grafik1 / 10);
  lcd.setCursor(4, 0);
  lcd.print("    ");
  lcd.setCursor(4, 0);
  lcd.print(pot1);
  uint16_t pot2 = analogRead(39);
  uint16_t grafik2 = map(pot2, 0, 4095, 0, 1000);
  mb.Hreg(17, pot2);
  mb.Hreg(19, grafik2 / 10);
  lcd.setCursor(4, 1);
  lcd.print("    ");
  lcd.setCursor(4, 1);
  lcd.print(pot2);
  lcd.setCursor(13, 0);
  lcd.print("   ");
  uint16_t out_an1 = mb.Hreg(20);
  lcd.setCursor(13, 0);
  lcd.print(out_an1);
  servo1.write(out_an1);
  lcd.setCursor(13, 1);
  lcd.print("   ");
  lcd.setCursor(13, 1);
  uint16_t out_an2 = mb.Hreg(21);
  lcd.print(out_an2);
  servo2.write(out_an2);
  mb.Hreg(22, 23205); //0x5AA5
  mb.Hreg(23, 512); //0x0200
  mb.Hreg(24, 1); //0x0001
  mb.Hreg(25, grafik1); //grafik1
  mb.Hreg(26, 257); //0x0101
  mb.Hreg(27, grafik2); //grafik2
  delay(10);
}