#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>
Servo servo1, servo2;
LiquidCrystal_I2C lcd(0x27, 16, 2);
int dig_out[8] = {23, 19, 18, 5, 4, 0, 2, 15};
int dig_in[6] = {25, 26, 27, 14, 12, 13};
int baca[6] = { 0, 0, 0, 0, 0, 0 };
int baca1[6] = { 0, 0, 0, 0, 0, 0 };
char p[][3] = { "p0", "p1", "p2", "p3", "p4", "p5", "p6", "p7" };
byte dataBuffer[5];
byte dataIndex = 0;
bool dataReady = false;
bool ambil = false;
unsigned long skr = 0;
void setup() {
  Serial.begin(115200);
  Serial2.begin(9600, SERIAL_8N1, 16, 17);
  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 < 8; i++) {
    pinMode(dig_out[i], OUTPUT);
  }
  for (int i = 0; i < 6; i++) {
    pinMode(dig_in[i], INPUT_PULLUP);
  }
}
void loop() {
  if (millis() - skr > 100) {
    skr = millis();
    int a = analogRead(34);
    int b = analogRead(35);
    int pot1 = analogRead(36);
    int pot2 = analogRead(39);
    int progbar1 = map(pot1, 0, 4095, 0, 100);
    int progbar2 = map(pot2, 0, 4095, 0, 100);
    int gauge1 = 2.6 * progbar1 - 40;
    int gauge2 = 2.6 * progbar2 - 40;
    int grafik1 = map(pot1, 0, 4095, 0, 255);
    int grafik2 = map(pot2, 0, 4095, 0, 255);
    lcd.setCursor(4, 0);
    lcd.print("    ");
    lcd.setCursor(4, 0);
    lcd.print(pot1);
    lcd.setCursor(4, 1);
    lcd.print("    ");
    lcd.setCursor(4, 1);
    lcd.print(pot2);
    sendWaveformData(0, grafik1);
    sendWaveformData(1, grafik2);
    sendNumberData("n0", pot1);
    sendNumberData("pot1", pot1/11);
    sendNumberData("n1", pot2);
    sendNumberData("pot2", pot2);
    sendNumberData("pot3", pot2/50);
    sendProgBar("j0", progbar1);
    sendProgBar("j1", progbar2);
    sendGaugeData("z0", (gauge1 >= 0) ? gauge1 : 360 + gauge1);
    sendGaugeData("z1", (gauge2 >= 0) ? gauge2 : 360 + gauge2);
    sendPictureData(p[0], a ? 3 : 4);
    sendPictureData(p[1], b ? 3 : 4);
    for (int i = 0; i < 6; i++) {
      baca[i] = !digitalRead(dig_in[i]);
      if (baca[i] != baca1[i]) sendPictureData(p[i + 2], baca[i] + 3);
      baca1[i] = baca[i];
    }
  }
  bacadata();
  if (dataReady) {
    prosesdata();
    dataReady = false;
  }
}
void bacadata() {
  while (Serial2.available() && !dataReady) {
    byte data = Serial2.read();
    if (data > 0x29 && data < 0x3A) ambil = true;
    if (ambil) {
      dataBuffer[dataIndex] = data;
      dataIndex++;
    }
    if (dataIndex >= 5) {
      dataReady = true;
      dataIndex = 0;
      ambil = false;
    }
  }
}
void prosesdata() {
  byte kode = dataBuffer[0];
  byte nilai = dataBuffer[1];
  byte byte3 = dataBuffer[2];
  byte byte4 = dataBuffer[3];
  byte byte5 = dataBuffer[4];
  if (byte3 == 0x00 && byte4 == 0x00 && byte5 == 0x00) {
    switch (kode) {
      case 0x30:
        if (nilai) digitalWrite(dig_out[0], HIGH);
        else digitalWrite(dig_out[0], LOW);
        Serial.print("ID.1=");
        Serial.println(nilai);
        break;
      case 0x31:
        if (nilai) digitalWrite(dig_out[1], HIGH);
        else digitalWrite(dig_out[1], LOW);
        Serial.print("ID.2=");
        Serial.println(nilai);
        break;
      case 0x32:
        if (nilai) digitalWrite(dig_out[2], HIGH);
        else digitalWrite(dig_out[2], LOW);
        Serial.print("ID.3=");
        Serial.println(nilai);
        break;
      case 0x33:
        if (nilai) digitalWrite(dig_out[3], HIGH);
        else digitalWrite(dig_out[3], LOW);
        Serial.print("ID.4=");
        Serial.println(nilai);
        break;
      case 0x34:
        if (nilai) digitalWrite(dig_out[4], HIGH);
        else digitalWrite(dig_out[4], LOW);
        Serial.print("ID.5=");
        Serial.println(nilai);
        break;
      case 0x35:
        if (nilai) digitalWrite(dig_out[5], HIGH);
        else digitalWrite(dig_out[5], LOW);
        Serial.print("ID.6=");
        Serial.println(nilai);
        break;
      case 0x36:
        if (nilai) digitalWrite(dig_out[6], HIGH);
        else digitalWrite(dig_out[6], LOW);
        Serial.print("ID.7=");
        Serial.println(nilai);
        break;
      case 0x37:
        if (nilai) digitalWrite(dig_out[7], HIGH);
        else digitalWrite(dig_out[7], LOW);
        Serial.print("ID.8=");
        Serial.println(nilai);
        break;
      case 0x38:
        lcd.setCursor(13, 0);
        lcd.print("   ");
        lcd.setCursor(13, 0);
        lcd.print(nilai);
        Serial.print("Servo1=");
        Serial.println(nilai);
        servo1.write(nilai);
        delay(15);
        break;
      case 0x39:
        lcd.setCursor(13, 1);
        lcd.print("   ");
        lcd.setCursor(13, 1);
        lcd.print(nilai);
        Serial.print("Servo2=");
        Serial.println(nilai);
        servo2.write(nilai);
        delay(15);
        break;
    }
  }
}
void sendWaveformData(int channel, int value) {
  Serial2.print("add 2,");
  Serial2.print(channel);
  Serial2.print(",");
  Serial2.print(value);
  sendTermination();
}
void sendPictureData(const char* componentName, int value) {
  Serial2.print(componentName);
  Serial2.print(".pic=");
  Serial2.print(value);
  sendTermination();
}
void sendNumberData(const char* componentName, int value) {
  Serial2.print(componentName);
  Serial2.print(".val=");
  Serial2.print(value);
  sendTermination();
}
void sendProgBar(const char* componentName, int value) {
  Serial2.print(componentName);
  Serial2.print(".val=");
  Serial2.print(value);
  sendTermination();
}
void sendGaugeData(const char* componentName, int value) {
  Serial2.print(componentName);
  Serial2.print(".val=");
  Serial2.print(value);
  sendTermination();
}
void sendTermination() {
  Serial2.write(0xFF);
  Serial2.write(0xFF);
  Serial2.write(0xFF);
}