#include <Arduino.h>
#include <U8g2lib.h>
#include <SPI.h>
#include <Wire.h>

// connexion hardware spi:
U8G2_SH1106_128X64_NONAME_1_HW_I2C u8g2(U8G2_R0);
// U8G2_R0 ou U8G2_R2: mode paysage, U8G2_R1 ou U8G2_R3: mode portrait

int boostPressure;
int boostMax = 0;
int boostMin = 0;

unsigned long startMillis;
unsigned long currentMillis;
const unsigned long period = 50;

const int sensorHistoryLength = 128;
int sensorHistory[sensorHistoryLength];
int sensorHistoryPos = sensorHistoryLength - 1;

void setup(void) {
  u8g2.begin();
  startMillis = millis();
  pinMode(13, OUTPUT);
 
 //Splash screen
u8g2.firstPage();  
  do {
u8g2.setFont(u8g2_font_tenfatguys_tf);
u8g2.drawStr(1, 15, "R E A D Y");
u8g2.drawStr(1, 38, "T O    ) )");
u8g2.drawStr(1, 60, "R A C E");
} while( u8g2.nextPage() );
  
  // show splash screen for a 2 seconds
  delay(2500);
}


void loop(void) {

  
  // Only read from the sensors every 50 ms
  currentMillis = millis();
  if (currentMillis - startMillis >= period) {
    readSensorData();
    startMillis = currentMillis;
  }
  /* if (boostPressure > 2); {
  digitalWrite(13, HIGH);
  delay(10);
  digitalWrite(13, LOW);
  delay(10);
 	 }*/

  u8g2.firstPage();
  do {
    // Draw current pressure
    u8g2.setFont(u8g2_font_maniac_tr);
    char cstr[6];
    dtostrf((float)boostPressure / 100, 1, 2, cstr);
    u8g2.drawStr(10, 36, cstr);

    // Draw max pressure
    u8g2.setFont(u8g2_font_smart_patrol_nbp_tn);
    dtostrf((float)boostMax / 100, 1, 2, cstr);
    int yPos = u8g2.getStrWidth(cstr);
    u8g2.drawStr(124 - yPos, 11, cstr);

    drawBarGraph(0, 51, 124, 12);
    //drawGraph(0, 32, 128, 31);

    //Draw orthonormal
     u8g2.drawFrame(0,50,124,14);
     u8g2.drawVLine(48,50,13);
     u8g2.drawVLine(93,50,13);

    //Draw unit
    u8g2.setFont(u8g2_font_smart_patrol_nbp_tr);
    u8g2.drawStr(88, 36, "bars");


  
  }
  
   while ( u8g2.nextPage() );
}


float normaliseSensorData(int m) {
  /*
    Scale the sensor reading into range
    m = measurement to be scaled
    rmin = minimum of the range of the measurement
    rmax = maximum of the range of the measurement
    tmin = minimum of the range of the desired target scaling
    tmax = maximum of the range of the desired target scaling
    normalisedValue = ((m − rmin) / (rmax − rmin)) * (tmax − tmin) + tmin
    https://stats.stackexchange.com/a/281164
  */

  /*
    Bosch MAP Sensor voltage ranges from 0.5v to 4.5v, converted to analogRead values (0 min, 1023 max) that's 102 to 921
    rmin = 102
    rmax = 921
    Sensor reads from 0 to 50 psi
    tmin = 0
    tmax = 5000
    normalisedValue = ((m − 102) / (921 − 102)) * (5000 − 0) + 0
    normalisedValue = ((m − 102) / 871) * 5000
    normalisedValue = (m − 102) / 0.1638
  */
  
  return ((m - 102) / 0.1638);
}


void readSensorData(void) {
  float absolutePressure = normaliseSensorData(analogRead(A0));
  
  // Subtract 14.7 psi == pressure at sea level
  // Additional 2.57psi subtracted as boost was showing 2.57 with engine off
  boostPressure = (absolutePressure - 1727)/14.504;

  // Update max and min
  if (boostPressure > boostMax) boostMax = boostPressure;
  if (boostPressure < boostMin) boostMin = boostPressure;

  // Log the history
  addSensorHistory(boostPressure);
}


void addSensorHistory(int val) {
  sensorHistory[sensorHistoryPos] = val;
  sensorHistoryPos--;
  if (sensorHistoryPos < 0) sensorHistoryPos = sensorHistoryLength - 1;
}


int getSensorHistory(int index) {
  index += sensorHistoryPos;
  if (index >= sensorHistoryLength) index = index - sensorHistoryLength;
  return sensorHistory[index];
}


void drawBarGraph(int x, int y, int len, int height) {
  if (boostPressure > 0) {
    // Draw the pressure bar behind the graph
    int barLength = ((float)boostPressure / boostMax) * len;
    u8g2.setDrawColor(2);
    u8g2.drawBox(x, y, barLength, height);
    u8g2.setDrawColor(1);
  }
}