#include <Wire.h>                                 //I2C Driver
#include <Adafruit_GFX.h>                         //Graphics library
#include <Adafruit_SSD1306.h>                     //SSD1306 OLED Driver
#include <Servo.h>

#define OLED_RESET 4                              //Digital pin 4 set aside for OLED reset
Adafruit_SSD1306 display(OLED_RESET);             //We will call out OLED object display, but you can choose any name

Servo servo;

const float wastegateStart = 1000.0;     //Millibars
const float maxPressure = 1200.0;        //Millibars
const float minPressure = -300.0;        //Millibars

void setup() {
  servo.attach(6);
  servo.write(0);

  display.begin(SSD1306_SWITCHCAPVCC, 0x3c);
  display.clearDisplay();
}
 
void loop() {
  float pressureVal = map(analogRead(A0), 0, 1023, minPressure, maxPressure);
  float pressure = pressureVal/1000.0;

  int graph = map(analogRead(A0), 0, 1023, 0, 124);

  display.clearDisplay();                         //always start with a clear
  display.setTextSize(2);                         //set up text size
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print(pressure);
  display.print(" Bar");
  
  display.drawRect(0, 16, 128, 16, WHITE);
  display.fillRect(2, 18, graph, 12, WHITE);    //Draw a rectangle (x,y,width,height,color)
  
  display.display();

  if(pressureVal > wastegateStart){
    int open = map(analogRead(A0), 0, 1023, 0, 180);
    servo.write(open);
  }
}