#include <WiFi.h>
#include <ModbusIP_ESP8266.h>

const int TRIG_PIN = D27;  // Connect the TRIG pin of the ultrasonic sensor to pin D1 on the ESP8266
const int ECHO_PIN = D26;  // Connect the ECHO pin of the ultrasonic sensor to pin D2 on the ESP8266

const int REG = 0;               // Modbus Hreg Offset
IPAddress remote(192, 168, 0, 88);  // Address of Modbus Slave device
const int LOOP_COUNT = 10;

ModbusIP mb;  //ModbusIP object
uint16_t nilai = 0;
int lm, lm1, lm2;
bool status;

void setup() {
  Serial.begin(115200);
  WiFi.begin("Redmi 10", "jotunheim908");
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
  mb.client();

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  status = true;  // Since we are not using the BME280 sensor, set status to true
}

uint16_t res = 0;
uint8_t show = LOOP_COUNT;
uint16_t REG_kirim1 = 6;
uint16_t REG_kirim2 = 7;
uint16_t REG_kirim3 = 8;

void loop() {
  if (lm2 < millis()) {
    lm2 = millis() + 1;
    if (mb.isConnected(remote)) {  // Check if connection to Modbus Slave is established

      if (lm < millis()) {
        lm = millis() + 500;
        mb.readHreg(remote, REG, &res);  // Initiate Read Coil from Modbus Slave
      }

      if (lm1 < millis()) {
        lm1 = millis() + 500;
        // Read distance from ultrasonic sensor
        long duration, distance;
        digitalWrite(TRIG_PIN, LOW);
        delayMicroseconds(2);
        digitalWrite(TRIG_PIN, HIGH);
        delayMicroseconds(10);
        digitalWrite(TRIG_PIN, LOW);
        duration = pulseIn(ECHO_PIN, HIGH);
        distance = (duration / 2) / 29.1;  // Convert the duration to distance in centimeters

        // Write distance to Modbus registers
        mb.writeHreg(remote, REG_kirim1, &distance, 1, 0, show);
      }
    } else {
      mb.connect(remote);  // Try to connect if no connection
    }
    mb.task();  // Common local Modbus task
  }

  if (!show--) {  // Display Slave register value one time per second (with default settings)
    Serial.println(res);
    show = LOOP_COUNT;
  }
}