#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);
const float fuelstoich=14.64; //input fuel stoich afr
float wbv=0;
float wbl=0;
float wbafr=0;
int delaytime = 150;


void setup() {
  Serial.begin(115200);
  pinMode(A3, 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(A3);
  float val0 = in0 * 5.0 / 1024.0;

  // read correct supply voltage

  float supply = readVcc() / 1000.0;
  float val3Corrected = supply / 5 * val0;

  wbv=(val3Corrected);
  wbl=(wbv*0.136)+0.68; //spartan 3 equation for lambda is (v*0.136)+0.68
  wbafr=(wbl*fuelstoich); //AFR equation is AFR=(lambda*fuelstoichAFR)

  Serial.println(val3Corrected);

  display.clearDisplay();
  display.setCursor(6,2);
  display.print(wbl,02);
  display.print(" -LAM");
  display.setCursor(6,25);
  display.print(wbv,02);
  display.print(" -VLT");  
  display.setCursor(0,48);
  display.print(wbafr,02);
  display.print(" -AFR");
  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;
}