/*
VCC to arduino pin 5v
GND to arduino pin GND
SCL to arduino pin A5 (or the SCL pin for your MEGA2560)
SDA to arduino pin A4 (or the SDA pin for your MEGA2560)
*/

#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET  -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

float i = -2000;

void setup() {
  Serial.begin(9600);

  pinMode(13,OUTPUT);
  digitalWrite(13,LOW);
  
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // Address 0x3D for 128x64

  // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  display.display();
  delay(2000); // Pause for 2 seconds

  // Clear the buffer
  display.clearDisplay();
  // Show the display buffer on the screen. You MUST call display() after
  // drawing commands to make them visible on screen!
  // display.display() is NOT necessary after every single drawing command,
  // unless that's what you want...rather, you can batch up a bunch of
  // drawing operations and then update the screen all at once by calling
  // display.display(). These examples demonstrate both approaches...
}


/* 
 * display.setTextSize(2);             // Draw 2X-scale text
 * display.setTextColor(WHITE);        // Draw white text
 * display.setTextColor(BLACK, WHITE); // Draw 'inverse' text
 * display.setCursor(0,0);             // Start at top-left corner
 * display.clearDisplay();             // Clear the buffer
 * display.display();
 */
float Sensor = 0;
float Pressure = 0;

void loop() {
  // Clear the buffer
  display.clearDisplay();

  display.setTextSize(2);             // Draw 2X-scale text
  display.setTextColor(WHITE);        // Draw white text
  display.setCursor(0,0);             // Start at top-left corner
  display.println("  VACUUM");
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.print("Pressure");
  display.setTextSize(2);  
  display.println(" (kPa)");
  display.setTextSize(2);
  Sensor = smooth();
  //Pressure = mapf(Sensor,140,460,9.3,-30);
  Pressure = -0.1185*Sensor + 25.954;
  
  if(Pressure <= -15) {
    digitalWrite(13,LOW);
    display.setTextColor(BLACK, WHITE);
  }
  else{
    digitalWrite(13,HIGH);    
    display.setTextColor(WHITE);
  }
  
  display.println(floatAlignRigiht(Pressure));  
  display.println(floatAlignRigiht(Sensor));  
  
  display.display();
}

String floatAlignRigiht ( float num )  //for setTextSize(4);
{
  int    space  = 0;
  String spaces = "";
 
  //how large is this number?
  //always assumes value has no digits after the decimal point.
/* 
       if ( num > -1000 && -100 > num ) space = 1;
  else if ( num > -100 && -10 > num ) space = 2;
  else if ( num > -10 && 0 > num ) space = 3;
  else if ( num > -1 && 10 > num ) space = 4;
  else if ( num > 9 && 100 > num ) space = 3;
  else if ( num > 99 && 1000 > num ) space = 2;
  else if ( num > 999 && 10000 > num ) space = 1;
*/
       if (num <= -100.0) space = 1;
  else if (num <= -10.0) space = 2;
  else if (num <  0) space = 3;
  else if (num < 10) space = 4;
  else if (num < 100) space = 3;
  else if (num < 1000) space = 2;
  else if (num < 10000) space = 1;
  
  //add the correct amount of spaces infront of our value
  for ( uint8_t s=0; s<space; s++ ) spaces += F(" ");
 
  //return value (or your code here)
  return spaces + String ( num);
}

double mapf(double val, double in_min, double in_max, double out_min, double out_max) {
    return (val - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

int smooth(){
  int i;
  int value = 0;
  int numReadings = 20;

  for (i = 0; i < numReadings; i++){
    // Read light sensor data.
    value = value + analogRead(A0);

    // 1ms pause adds more stability between reads.
    delay(10);
  }

  // Take an average of all the readings.
  value = value / numReadings;

  return value;
}