#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BMP280.h>

// BMP280 Sensor
Adafruit_BMP280 bmp; // use I2C interface
Adafruit_Sensor *bmp_temp = bmp.getTemperatureSensor();
Adafruit_Sensor *bmp_pressure = bmp.getPressureSensor();

// Constants
const int MAX_REQUESTS = 100;
const int BUSY_THRESHOLD = 20;

// Parameters
int pressureOffset = 0;
Adafruit_BMP280::sensor_sampling pressureSampling = Adafruit_BMP280::SAMPLING_X1;
int temperatureOffset = 0;
Adafruit_BMP280::sensor_sampling temperatureSampling = Adafruit_BMP280::SAMPLING_X1;

// State Variables
int requestCount = 0;
bool startupReceived = false;
String lastSensorValue = "";

// Communication state for 4-step process
struct CommunicationState {
  bool requestConfirmed = false;
  bool confirmConfirmed = false;
  bool checkConfirmed = false;
};

CommunicationState commState;

// Function Prototypes
void handleCommand(String command);
void handleGetSensor(String command, String sensor);
void handleSetParameter(String param, String value);
void handleGetParameter(String param);
void resetSensors();
String getRawSensorData(String sensor);
bool randomFailure(float chance);
String* splitString(String data, char separator, int* size);
Adafruit_BMP280::sensor_sampling getSamplingFromUserInput(String value);
String samplingToString(Adafruit_BMP280::sensor_sampling sampling);
String floatToBinary(float value);

void setup() {
  Serial.begin(115200);
  while (!Serial) {
    ; // Wait for serial port to connect.
  }

  while (!startupReceived) {
    if (Serial.available()) {
      String command = Serial.readStringUntil('\n');
      if (command.startsWith("STARTUP")) {
        startupReceived = true;
      }
    }
  }
  
  unsigned status = bmp.begin(0x76);
  if (!status) {
    Serial.println(F("Could not find a valid BMP280 sensor, check wiring or try a different address!"));
    while (1) delay(10);
  }

  bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,     /* Operating Mode. */
                  temperatureSampling,              /* Temp. oversampling */
                  pressureSampling,                 /* Pressure oversampling */
                  Adafruit_BMP280::FILTER_X16,      /* Filtering. */
                  Adafruit_BMP280::STANDBY_MS_500); /* Standby time. */

  Serial.println("READY - Tübingen Instruments RIU-9000");
}

void loop() {
  if (Serial.available()) {
    String command = Serial.readStringUntil('\n');
    delay(5); // Small delay to make communication more interesting
    handleCommand(command);
  }
}

void handleCommand(String command) {
  if (requestCount >= MAX_REQUESTS && !command.startsWith("RESET_SENSORS")) {
    return;
  }

  if (requestCount > 0 && requestCount % BUSY_THRESHOLD == 0) {
    Serial.println("BUSY");
    requestCount++;
    return;
  }

  int size;
  String* parts = splitString(command, ' ', &size);

  if (parts[0] == "GET_SENSOR" && size == 3) {
    handleGetSensor(parts[1], parts[2]);
  } else if (parts[0] == "SET_PARAMETER" && size == 3) {
    handleSetParameter(parts[1], parts[2]);
  } else if (parts[0] == "GET_PARAMETER" && size == 2) {
    handleGetParameter(parts[1]);
  } else if (parts[0] == "RESET_SENSORS" && size == 1) {
    resetSensors();
  } else {
    Serial.println("ERROR: UNKNOWN COMMAND");
  }

  delete[] parts;
  requestCount++;
}

void handleGetSensor(String command, String sensor) {

  if (command == "REQUEST") { // request data from the RUI (which will request it from the sensor)
    if (!commState.requestConfirmed && randomFailure(0.1)) {
      Serial.println("ERROR");
      return;
    }
    lastSensorValue = getRawSensorData(sensor);
    Serial.println("OK");
    commState.requestConfirmed = true;

  } else if (command == "CONFIRM") { // did you request the data from the sensor?
    if (!commState.confirmConfirmed && randomFailure(0.1)) {
      Serial.println("FALSE");
      return;
    }
    if (commState.requestConfirmed) {
      Serial.println("TRUE");
      commState.confirmConfirmed = true;
    } else {
      Serial.println("FALSE");
    }
  } else if (command == "CHECK") { // did you get the data from the sensor?
    if (!commState.checkConfirmed && randomFailure(0.5)) {
      Serial.println("FALSE");
    } else {
      if (commState.requestConfirmed) {
        Serial.println("TRUE");
        commState.checkConfirmed = true;
      } else {
        Serial.println("FALSE");
      }
    }
  } else if (command == "SEND") { // send the data to the user
    if (commState.requestConfirmed) {
      Serial.println(lastSensorValue);
      commState = CommunicationState(); // Reset communication state
    } else {
      Serial.println("FALSE");
    }
  } else if (command == "CANCEL") {
    commState = CommunicationState(); // Reset communication state
    Serial.println("OK");
  } else {
    Serial.println("ERROR: UNKNOWN COMMAND");
  }
}

void handleSetParameter(String param, String value) {
  if (param == "PRESSURE_OFFSET") {
    pressureOffset = value.toInt();
    Serial.println("OK");
  } else if (param == "PRESSURE_SAMPLING") {
    pressureSampling = getSamplingFromUserInput(value);
    bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,     /* Operating Mode. */
                    temperatureSampling,              /* Temp. oversampling */
                    pressureSampling,                 /* Pressure oversampling */
                    Adafruit_BMP280::FILTER_X16,      /* Filtering. */
                    Adafruit_BMP280::STANDBY_MS_500); /* Standby time. */
    Serial.println("OK");
  } else if (param == "TEMPERATURE_OFFSET") {
    temperatureOffset = value.toInt();
    Serial.println("OK");
  } else if (param == "TEMPERATURE_SAMPLING") {
    temperatureSampling = getSamplingFromUserInput(value);
    bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,     /* Operating Mode. */
                    temperatureSampling,              /* Temp. oversampling */
                    pressureSampling,                 /* Pressure oversampling */
                    Adafruit_BMP280::FILTER_X16,      /* Filtering. */
                    Adafruit_BMP280::STANDBY_MS_500); /* Standby time. */
    Serial.println("OK");
  } else {
    Serial.println("ERROR: UNKNOWN PARAMETER");
  }
}

void handleGetParameter(String param) {
  if (param == "PRESSURE_OFFSET") {
    Serial.println(pressureOffset);
  } else if (param == "PRESSURE_SAMPLING") {
    Serial.println(samplingToString(pressureSampling));
  } else if (param == "TEMPERATURE_OFFSET") {
    Serial.println(temperatureOffset);
  } else if (param == "TEMPERATURE_SAMPLING") {
    Serial.println(samplingToString(temperatureSampling));
  } else {
    Serial.println("ERROR: UNKNOWN PARAMETER");
  }
}

void resetSensors() {
  requestCount = 0;
  commState = CommunicationState(); // Reset communication state
  Serial.println("OK");
}

String getRawSensorData(String sensor) {
  sensors_event_t temp_event, pressure_event;

  if (sensor == "PRESSURE") {
    bmp_pressure->getEvent(&pressure_event);
    float pressure = pressure_event.pressure + pressureOffset;
    return floatToBinary(pressure); // in hPa
  } else if (sensor == "TEMPERATURE") {
    bmp_temp->getEvent(&temp_event);
    float temperature = temp_event.temperature + temperatureOffset;
    return floatToBinary(temperature); // in °C
  }

  return "";
}

bool randomFailure(float chance) {
  return random(0, 100) < (chance * 100);
}

String* splitString(String data, char separator, int* size) {
  int found = 0;
  int strIndex[] = {0, -1};
  int maxIndex = data.length() - 1;
  int count = 1;

  for (int i = 0; i <= maxIndex; i++) {
    if (data.charAt(i) == separator) {
      count++;
    }
  }

  String* result = new String[count];
  *size = count;

  found = 0;
  for (int i = 0; i <= maxIndex && found < count; i++) {
    if (data.charAt(i) == separator || i == maxIndex) {
      strIndex[0] = strIndex[1] + 1;
      strIndex[1] = (i == maxIndex) ? i + 1 : i;
      result[found] = data.substring(strIndex[0], strIndex[1]);
      found++;
    }
  }

  return result;
}

Adafruit_BMP280::sensor_sampling getSamplingFromUserInput(String value) {
  if (value == "SAMPLING_NONE") return Adafruit_BMP280::SAMPLING_NONE;
  if (value == "SAMPLING_X1") return Adafruit_BMP280::SAMPLING_X1;
  if (value == "SAMPLING_X2") return Adafruit_BMP280::SAMPLING_X2;
  if (value == "SAMPLING_X4") return Adafruit_BMP280::SAMPLING_X4;
  if (value == "SAMPLING_X8") return Adafruit_BMP280::SAMPLING_X8;
  if (value == "SAMPLING_X16") return Adafruit_BMP280::SAMPLING_X16;
  return Adafruit_BMP280::SAMPLING_NONE;
}

String samplingToString(Adafruit_BMP280::sensor_sampling sampling) {
  switch (sampling) {
    case Adafruit_BMP280::SAMPLING_NONE:
      return "SAMPLING_NONE";
    case Adafruit_BMP280::SAMPLING_X1:
      return "SAMPLING_X1";
    case Adafruit_BMP280::SAMPLING_X2:
      return "SAMPLING_X2";
    case Adafruit_BMP280::SAMPLING_X4:
      return "SAMPLING_X4";
    case Adafruit_BMP280::SAMPLING_X8:
      return "SAMPLING_X8";
    case Adafruit_BMP280::SAMPLING_X16:
      return "SAMPLING_X16";
    default:
      return "UNKNOWN_SAMPLING";
  }
}

String floatToBinary(float value) {
  union FloatUnion {
    float value;
    byte bytes[4];
  };

  FloatUnion floatUnion;
  floatUnion.value = value;

  String binaryString = "";
  for (int i = 3; i >= 0; i--) {
    for (int j = 7; j >= 0; j--) {
      binaryString += String((floatUnion.bytes[i] >> j) & 1);
    }
  }

  return binaryString;
}