#include <SoftwareSerial.h>
#include <Wire.h>

// ----- CONFIGURAZIONE -----
const int RX_PIN = 3;  // RX Arduino <- TX FDO2
const int TX_PIN = 2;  // TX Arduino -> RX FDO2 (serve level shifter 3.3V)
#define AD5693R_ADDR 0x0C  // I2C address del DAC AD5693R
const int MAX_CONSECUTIVE_ERRORS = 6;  // Allarme dopo 6 errori
const int LOOP_DELAY_MS = 500;  // Lettura ogni 500 ms
// ---------------------------

SoftwareSerial sensorSerial(RX_PIN, TX_PIN);
int consecutiveErrors = 0;

void setup() {
  Serial.begin(9600);             // Debug (opzionale)
  sensorSerial.begin(19200);      // Baud rate FDO2
  Wire.begin();                   // Inizializza I2C
  delay(1200);                    // Warm-up FDO2
  Serial.println("FDO2 init OK. Pronto.");
}

void loop() {
  sensorSerial.print("#MOXY\r");
  String response = readLineFromSensor();

  long O, T;
  unsigned long S;

  if (parseMOXY(response, O, T, S)) {
    consecutiveErrors = 0;

    float pO2_hPa = O / 1000.0;
    float output_mV = 0.45 * pO2_hPa + 10.0;

    if (output_mV > 160.0) output_mV = 160.0;
    if (output_mV < 0.0)   output_mV = 0.0;

    uint16_t dacVal = round((output_mV / 5000.0) * 65535);
    writeDAC(dacVal);

    Serial.print("ppO2: ");
    Serial.print(pO2_hPa);
    Serial.print(" hPa  |  Vout: ");
    Serial.print(output_mV);
    Serial.print(" mV  |  DAC: ");
    Serial.println(dacVal);
  } else {
    consecutiveErrors++;
    Serial.print("Errore lettura ");
    Serial.print(consecutiveErrors);
    Serial.print(" / ");
    Serial.println(MAX_CONSECUTIVE_ERRORS);

    if (consecutiveErrors >= MAX_CONSECUTIVE_ERRORS) {
      Serial.println("Errore persistente: uscita a 0 mV");
      writeDAC(0);  // forza la cella a 0 mV → allarme
    }
  }

  delay(LOOP_DELAY_MS);
}

// ---------- FUNZIONI ----------

String readLineFromSensor() {
  String s = "";
  unsigned long t0 = millis();
  while (millis() - t0 < 200) {
    while (sensorSerial.available()) {
      char c = sensorSerial.read();
      if (c == '\r' || c == '\n') {
        if (s.length() > 0) return s;
      } else {
        s += c;
      }
    }
  }
  return s;
}

bool parseMOXY(const String& r, long &O, long &T, unsigned long &S) {
  if (!r.startsWith("#MOXY")) return false;

  String payload = r.substring(5);
  payload.trim();

  int i1 = payload.indexOf(' ');
  int i2 = payload.indexOf(' ', i1 + 1);
  if (i1 < 0 || i2 < 0) return false;

  String O_str = payload.substring(0, i1);
  String T_str = payload.substring(i1 + 1, i2);
  String S_str = payload.substring(i2 + 1);
  S_str.trim();

  O = O_str.toInt();
  T = T_str.toInt();
  S = (unsigned long) S_str.toInt();

  // Se il campo status indica errore, consideriamo fallita la lettura
  if (S > 1) return false;

  return true;
}

void writeDAC(uint16_t val) {
  Wire.beginTransmission(AD5693R_ADDR);
  Wire.write(0x30);                   // Command: Write & Update DAC Register
  Wire.write((val >> 8) & 0xFF);      // MSB
  Wire.write(val & 0xFF);             // LSB
  Wire.endTransmission();
}