#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128 //OLED display width in pixles
#define SCREEN_HEIGHT 64 //OLED display height in pixles
#define OLED_RESET 4 //Reset pin# (or -1 if sharing Arduino reset pins)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
float press0v = 0;
float press1v = 0;
float press0 = 0;
float press1 = 0;
float pressure0 = 0;
float pressure1 = 0;
float constant = 200;
int delaytime = 150;

void setup() {
  Serial.begin(115200);
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  
  display.clearDisplay(); // Clear the display
  display.setTextColor(WHITE);   //Set the color - always use white despite actual display color
  display.setTextSize(2); //Set the font size
  display.setCursor(0,0); //Set the cursor coordinates
}

void loop() {
  
  // read normal Arduino value

  int in0 = analogRead(A0);
  float val0 = in0 * 5.0 / 1024.0;
  int in1 = analogRead(A1);
  float val1 = in1 * 5.0 / 1024.0;

  // read correct supply voltage

  float supply = readVcc() / 1000.0;
  float VCCcorrected0 = supply / 5 * val0;
  float VCCcorrected1 = supply / 5 * val1;

  press0v = (VCCcorrected0);
  press0 = (press0v*0.2);
pressure0 = (press0*constant);
  press1v = (VCCcorrected1);
  press1 = (press1v*0.2);
pressure1 = (press1*constant);

  Serial.print("p0 ");
  Serial.println(pressure0);
  Serial.print("p1 ");
  Serial.println(pressure1);

  display.clearDisplay();
  display.setCursor(6,2);
  display.print(press0v,02);
  display.print(" -P0v"); 
  display.setCursor(0,48);
  display.print(press1v,02);
  display.print(" -P1v");
  display.display();

  delay(delaytime);
}

long readVcc() {
  long result;
  // Read 1.1V reference against AVcc
#if defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
  ADMUX = _BV(REFS0) | _BV(MUX4) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
#elif defined (__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__)
  ADMUX = _BV(MUX5) | _BV(MUX0);
#elif defined (__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__)
  ADMUX = _BV(MUX3) | _BV(MUX2);
#else
  ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
#endif
  delay(2); // Wait for Vref to settle
  ADCSRA |= _BV(ADSC); // Convert
  while (bit_is_set(ADCSRA, ADSC));
  result = ADCL;
  result |= ADCH << 8;
  result = 1126400L / result; // Calculate Vcc (in mV); 1126400 = 1.1*1024*1000
  return result;
}