#include <SDI12.h>

#define SDI12_DATA_PIN 7

// Initialize SDI12 instance
SDI12 mySDI12(SDI12_DATA_PIN);

// Sensor simulation state variables
bool measurementRequested[2] = {false, false};

void setup() {
  Serial.begin(115200);
  while (!Serial); // Wait for Serial Monitor on USB

  mySDI12.begin();
  
  // Explicitly set the library to slave mode (listening)
  mySDI12.forceListen();

  Serial.println(F("--- SDI-12 Dual Sensor Simulator Ready ---"));
  Serial.println(F("Listening for queries on Pin 7..."));
}

String rxBuffer = "";

void loop() {
  // Read incoming characters into a non-destructive buffer
  while (mySDI12.available() > 0) {
    char c = mySDI12.read();

    // Ignore Break Signal / NULL / line control bytes
    if (c == 0x00 || c == '\r' || c == '\n') continue;

    rxBuffer += c;

    // Command complete upon receiving standard SDI-12 terminator '!'
    if (c == '!') {
      processCommand(rxBuffer);
      rxBuffer = ""; // Reset buffer for next command
    }
  }
}

void processCommand(String command) {
  Serial.print(F("\n>> Rx Command: "));
  Serial.println(command);

  if (command.length() < 2) return;

  char addr = command.charAt(0);
  char cmdType = command.charAt(1);

  int sensorIndex = -1;
  if (addr == '1') sensorIndex = 0;
  if (addr == '2') sensorIndex = 1;

  if (sensorIndex == -1) return; // Command was not addressed to sensor '1' or '2'

  // Turnaround delay required by SDI-12 specification (~15ms)
  delay(15); 

  // -----------------------------------------------------------------
  // 1) IDENTIFICATION COMMAND (aI!)
  // -----------------------------------------------------------------
  if (cmdType == 'I') {
    // TEROS 22 model string structure: [address]13METER  TEROS22011
    String idResponse = String(addr) + "13METER  TEROS22011\r\n";
    
    // sendResponse handles output state, transmission, and returning to listening mode
    mySDI12.sendResponse(idResponse);
    
    Serial.print(F("<< Sent ID: "));
    Serial.print(idResponse);
  }
  
  // -----------------------------------------------------------------
  // 2) MEASUREMENT COMMAND (aM!)
  // -----------------------------------------------------------------
  else if (cmdType == 'M') {
    measurementRequested[sensorIndex] = true;

    // TEROS 22 format: [address][ttt][n] -> ttt = wait time in sec (001), n = num values (2)
    String mResponse = String(addr) + "0212\r\n";

    mySDI12.sendResponse(mResponse);
    
    Serial.print(F("<< Sent M Response: "));
    Serial.print(mResponse);
  }

  // -----------------------------------------------------------------
  // 3) DATA COLLECTION COMMAND (aD0!)
  // -----------------------------------------------------------------
  else if (cmdType == 'D' && command.length() >= 3 && command.charAt(2) == '0') {
    String dataResponse = "";

    if (measurementRequested[sensorIndex]) {
      float waterPotential = (sensorIndex == 0) ? -99800.0 : -100000.0; // kPa
      float temperature    = (sensorIndex == 0) ? 26.5  : 26.3;  // °C

      // SDI-12 data string format: a+val1+val2
      // dataResponse = String(addr) + "+" + String(waterPotential, 1) + "+" + String(temperature, 1) + "\r\n";
      dataResponse = String(addr) + String(waterPotential, 1) + "+" + String(temperature, 1) + "\r\n";
      measurementRequested[sensorIndex] = false;
    } else {
      dataResponse = String(addr) + "+0.0+0.0\r\n";
    }

    mySDI12.sendResponse(dataResponse);
    
    Serial.print(F("<< Sent Data: "));
    Serial.print(dataResponse);
  }
}