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

U8G2_SH1106_128X64_NONAME_1_HW_I2C u8g2(U8G2_R0);

unsigned long startMillis;
unsigned long currentMillis;
unsigned long time_passed;
unsigned long lastReset;

const unsigned long period = 25;

const int sensorHistoryLength = 128;
int sensorHistory[sensorHistoryLength];
int sensorHistoryPos = sensorHistoryLength - 1;
float sensorData;
int boostMax = 1600; 
int BoostMaxTop = 0;
int boostMin = 0;
float inbar;
float maxinbar;

const char* type="Analog Reads";
const char* unit = "Volts";

void setup(void) {
  u8g2.begin();
  startMillis = millis();
  time_passed = millis();
}


void loop(void) {
  // Only read from the sensors every 50 ms
  currentMillis = millis();
  if (currentMillis - startMillis >= period) {
    readSensorData();
    startMillis = currentMillis;
    // Set Max Boost Value
    if (sensorData >= BoostMaxTop)
    {
      BoostMaxTop = sensorData;
    }
    else if (sensorData <  BoostMaxTop)
    {
      if (millis() - lastReset > 5000) {
        lastReset += 5000;
        time_passed = 0;
        BoostMaxTop = sensorData;
      }
    }
  }

  u8g2.firstPage();
  do {
    // Draw current data
    u8g2.setFont(u8g2_font_fub20_tf);

 
    if(analogRead(A0) == 0)
    {
      sensorData = 0;
      BoostMaxTop = 0;
    }
    
    // Change analog to BAR
    inbar = sensorData*1.0;
    char cstd[6];
    dtostrf((float)inbar, 1, 1, cstd);
    u8g2.drawStr(0, 20, cstd);
		u8g2.setFont(u8g2_font_helvR08_tf);
    u8g2.drawStr(10, 42, type);
    // Draw max pressure
    u8g2.setFont(u8g2_font_fub11_tf);
    // Change Analog to Vol
    maxinbar = (BoostMaxTop * 5.0)/1023.0;
    char csta[6];
    dtostrf((float)maxinbar, 1, 1, csta);

    int yPos = u8g2.getStrWidth(csta);
    u8g2.drawStr(128 - yPos, 11, csta);

    drawBarGraph(0, 22, 125, 8);
    drawGraph(0, 32, 125, 31);
		u8g2.setFont(u8g2_font_helvR08_tf);
    u8g2.drawStr(100, 23, unit);

  } while ( u8g2.nextPage() );
}


void readSensorData(void) {
  sensorData = analogRead(A0);
  addSensorHistory(sensorData);
}

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];
}

// Display functions
void drawGraph(int x, int y, int len, int height) {
  // Draw the lines
  drawHorizontalDottedLine(x, y, len);
  drawHorizontalDottedLine(x, y + height, len);

  int absMin = abs(boostMin);
  int range = absMin + boostMax;

  // Draw 0 line
  int zeroYPos = mapValueToYPos(absMin, range, y, height);
  drawHorizontalDottedLine(x, zeroYPos, len);

  // Draw the graph line
  for (int i = 0; i < 128; i++) {
    // Scale the values so that the min is always 0
    int valueY = getSensorHistory(i) + absMin;

    // Calculate the coordinants
    int yPos = mapValueToYPos(valueY, range, y, height);
    int xPos = len - i;
    if (yPos < zeroYPos) {
      // Point is above zero line, fill in space under graph
      u8g2.drawVLine(xPos, yPos, zeroYPos + 1 - yPos);
    } else {
      // Point is below zero line, draw graph line without filling in
      u8g2.drawPixel(xPos, yPos);  
    }
  }
}

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

// Maps a value to a y height
int mapValueToYPos(int val, int range, int y, int height) {
  float valueY = ((float)val / range) * height;
  return y + height - (int)valueY;
}

void drawHorizontalDottedLine(int x, int y, int len) {
  for (int i = 0; i < len; i++) {
    if (!(i % 4)) u8g2.drawPixel(x + i, y);
  }
}