#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SH110X.h>

#define oled_Address 0x3C // Change this if your OLED has a different I2C address
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1

int M3200address = 0x28;   // 0x28, 0x36 or 0x46, depending on the sensor.
float maxPressure = 100;   // pressure in PSI for this sensor, 100, 250, 500, ... 10k.
byte status;

unsigned long pressureMillis = 0;
const int pressureTiming = 500; // 500ms every data acquisition

// Define the OLED display
Adafruit_SH1106G display = Adafruit_SH1106G(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void setup() {
  Serial.begin(9600);
  Serial.println("M3200 initializing");
  Wire.begin();

  // Initialize the OLED display
  if (!display.begin(oled_Address)) {
    Serial.println("SSD1306 allocation failed");
    for (;;);
  }

  display.display();  // Clear the display buffer
  delay(2000);        // Pause for 2 seconds
  display.clearDisplay(); // Clear the display buffer
}

void loop() {
  if (millis() - pressureMillis >= pressureTiming) {
    pressureMillis = millis();
    Wire.requestFrom(M3200address, 4);   // request 4 bytes
    int n = Wire.available();

    if (n == 4) {
      status = 0;
      uint16_t rawP;     // pressure data from sensor
      uint16_t rawT;     // temperature data from sensor
      rawP = (uint16_t) Wire.read();    // upper 8 bits
      rawP <<= 8;
      rawP |= (uint16_t) Wire.read();    // lower 8 bits
      rawT = (uint16_t) Wire.read();    // upper 8 bits
      rawT <<= 8;
      rawT |= (uint16_t) Wire.read();   // lower 8 bits

      status = rawP >> 14;   // The status is 0, 1, 2 or 3
      rawP &= 0x3FFF;   // keep 14 bits, remove status bits

      rawT >>= 5;     // the lowest 5 bits are not used

      float pressure = ((rawP - 1000.0) / (15000.0 - 1000.0)) * maxPressure;
      float temperature = ((rawT - 512.0) / (1075.0 - 512.0)) * 55.0;

      // Display data on OLED
      display.clearDisplay();
      display.setTextSize(1);
      display.setTextColor(SH110X_WHITE);
      display.setCursor(0, 0);
      display.print("Status: ");
      display.println(status);
      display.print("Pressure: ");
      display.print(pressure);
      display.println(" PSI");
      display.print("Temperature: ");
      display.print(temperature);
      display.println(" C");
      display.display();

      Serial.println("-------------------------------------");
    } else {
      Serial.println("M3200 Pressure Transducer not Detected");
    }
  }
}