#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);
int cacah = 0;
uint16_t switchData[8];
uint16_t ledData[8];
uint16_t aiData[4];
uint16_t aoData[2];
uint16_t out_an1 = 0;
uint16_t out_an2 = 0;
uint16_t grafData[6] = {23205, 512, 1, 0, 257, 0};
unsigned long skr = 0;
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-Master-DWIN-Slave siap!");
  mb.master();
}
void loop() {
  if (millis() - skr > 50) {
    skr = millis();
    cacah++;
    if (cacah > 4) cacah = 0;
    if (!mb.slave()) {
      switch (cacah) {
        case 0:
          mb.readHreg(2, 20480, ledData, 8);//0x5000=20480
          break;
        case 1:
          mb.writeHreg(2, 20488, switchData, 8);//0x5008=20488
          break;
        case 2:
          mb.writeHreg(2, 20496, aiData, 4);//0x5010=20496
          break;
        case 3:
          mb.readHreg(2, 20500, aoData, 2);//0x5014=20500
          break;
        case 4:
          mb.writeHreg(2, 784, grafData, 6);//0x0310=784
          break;
      }
      while (mb.slave()) {
        mb.task();
      }
      for (int i = 0; i < 8; i++) {
        digitalWrite(pinLed[i], ledData[i] ? HIGH : LOW);
      }
      out_an1 = aoData[0];
      servo1.write(out_an1);
      out_an2 = aoData[1];
      servo2.write(out_an2);
    }
  }
  uint16_t a = analogRead(34) > 0 ? 0 : 1;
  switchData[0] = a;
  uint16_t b = analogRead(35) > 0 ? 0 : 1;
  switchData[1] = b;
  static uint8_t lastStates[6] = {0};
  uint8_t currentState;
  for (int i = 0; i < 6; i++) {
    currentState = !digitalRead(pinSwitch[i]);
    if (currentState != lastStates[i]) {
      switchData[i + 2] = currentState;
      lastStates[i] = currentState;
    }
  }
  uint16_t pot1 = analogRead(36);
  uint16_t grafik1 = map(pot1, 0, 4095, 0, 1000);
  grafData[3] = grafik1;
  aiData[0] = pot1;
  aiData[2] = 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);
  grafData[5] = grafik2;
  aiData[1] = pot2;
  aiData[3] = grafik2 / 10;
  lcd.setCursor(4, 1);
  lcd.print("    ");
  lcd.setCursor(4, 1);
  lcd.print(pot2);
  lcd.setCursor(13, 0);
  lcd.print("   ");
  lcd.setCursor(13, 0);
  lcd.print(out_an1);
  lcd.setCursor(13, 1);
  lcd.print("   ");
  lcd.setCursor(13, 1);
  lcd.print(out_an2);
}