#include <HardwareSerial.h>
#include <ModbusMaster.h>

HardwareSerial RS485Serial(1);
ModbusMaster node;

// Pin mapping for XIAO RS485 breakout
const int RS485_RX = 6;   // UART1 RX
const int RS485_TX = 5;   // UART1 TX
const int RS485_EN = D2;   // DE/RE control (single pin)

// Sensor/Modbus parameters (from datasheet)
const uint8_t  SLAVE_ADDR = 0x01;        // default address
const uint16_t REG_CALC_DISTANCE = 0x0100;
//const uint16_t REG_REAL_DISTANCE = 0x0101;
const uint16_t REG_TEMPERATURE   = 0x0102;
const uint16_t REG_SLAVE_ADDR    = 0x0200;

// Forward declarations for ModbusMaster callbacks
void preTransmission();
void postTransmission();

void setup() {
  Serial.begin(9600);
  delay(5000);
  Serial.println("ModbusMaster: Seeed SKU101991042 Starting");

 // Wait for the hardware serial to be ready
  while (!Serial)
    ;
  Serial.println("Serial done");

  // RS485 transceiver enable pin
  pinMode(RS485_EN, OUTPUT);
  digitalWrite(RS485_EN, LOW);           // start in RX mode

  // Datasheet: 9600 baud, 8N1
  RS485Serial.begin(9600, SERIAL_8N1, RS485_RX, RS485_TX);
  while (!RS485Serial)
    ;
  Serial.println("RS485 done");

  // Tie ModbusMaster to the UART we just configured
  node.begin(SLAVE_ADDR, RS485Serial);

  // Register callbacks for half-duplex direction control
  node.preTransmission(preTransmission);
  node.postTransmission(postTransmission);

  Serial.println("ModbusMaster: Seeed 101991042 distance & temperature reader ready.");
}

// Toggle DE/RE around TX per ModbusMaster design
void preTransmission() {
  digitalWrite(RS485_EN, HIGH);          // enable driver (TX)
  delayMicroseconds(250);                // transceiver turn-around margin
}

void postTransmission() {
  delayMicroseconds(250);                // ensure last bit left the wire
  digitalWrite(RS485_EN, LOW);           // back to receive
}

void loop() {
  static uint32_t last = 0;
  //if (millis() - last >= 1000) {         // poll at 1 Hz
  if (millis() - last >= 360000) {         // poll at 0.2 Hz
    last = millis();

    // --- 0x0100 Calculated distance (mm) ---
    uint8_t result = node.readHoldingRegisters(REG_CALC_DISTANCE, 1);
    if (result == node.ku8MBSuccess) {
      uint16_t dist_calc_mm = node.getResponseBuffer(0); // big-endian per Modbus
      float dist_calc_cm = dist_calc_mm / 10.0f;
      Serial.printf("Calculated distance: %u mm (%.1f cm)\n", dist_calc_mm, dist_calc_cm);
    } else {
      Serial.printf("Read 0x0100 failed (err=%u)\n", result);
    }
    delay(1000);
/*
    // --- 0x0101 Real-time distance (mm) ---
    result = node.readHoldingRegisters(REG_REAL_DISTANCE, 1);
    if (result == node.ku8MBSuccess) {
      uint16_t dist_real_mm = node.getResponseBuffer(0);
      float dist_real_cm = dist_real_mm / 10.0f;
      Serial.printf("Real-time distance:  %u mm (%.1f cm)\n", dist_real_mm, dist_real_cm);
    } else {
      Serial.printf("Read 0x0101 failed (err=%u)\n", result);
    }
*/
    // --- 0x0102 Temperature (INT16, 0.1°C) ---
    result = node.readHoldingRegisters(REG_TEMPERATURE, 1);
    if (result == node.ku8MBSuccess) {
      uint16_t raw = node.getResponseBuffer(0);
      int16_t temp_i16 = (int16_t)raw;
      float temp_c = temp_i16 / 10.0f;
      Serial.printf("Temperature: %.1f °C\n", temp_c);
    } else {
      Serial.printf("Read 0x0102 failed (err=%u)\n", result);
    }
    delay(1000);
  }
}