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

// OLED varables and configs
#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);
#if (SSD1306_LCDHEIGHT != 64)
#error("Height incorrect, please fix Adafruit_SSD1306.h!");
#endif

// Defines
#define HIGH_GEAR 4.0
#define GEAR_COUNT 6

// Variables
float val = 0, sumVal = 0;
char lastGear = 0, currGear = 0;
int raw = 0, minVal = 9999;
int i = 0, j = 0;
float gearsTab[GEAR_COUNT] = {0};

void setup()
{
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  // Setup oled i2c address
  pinMode(A0, INPUT);  // Set A0 like analog input - ADC
 
  float check = 0;
  val = getVoltage();
  if ( val > HIGH_GEAR ) lernGears();
  else
  {
    EEPROM.readBlock(0, gearsTab);
    for(i=0;i<GEAR_COUNT;i++)
    {
      check += gearsTab[i];
    }
    if( 0 == check ) lernGears();
  }
}

void loop()
{   
  val = getVoltage();
 
  // Match voltage to gear
  if (val <= gearsTab[0]+((gearsTab[1]-gearsTab[0])/2)) 
  {
    if('2' != lastGear)  currGear = '1';
  }
    else if (val <= gearsTab[1]+((gearsTab[2]-gearsTab[1])/2)) 
    {
      if('1' != lastGear)  currGear = '2';
    }
      else if (val <= gearsTab[2]+((gearsTab[3]-gearsTab[2])/2)) 
      {
        currGear = '3';
      }
        else if (val <= gearsTab[3]+((gearsTab[4]-gearsTab[3])/2)) 
        {
          currGear = '4';
        }
          else if (val <= gearsTab[4]+((gearsTab[5]-gearsTab[4])/2)) 
          {
            currGear = '5';
          }
            else if (val <= gearsTab[5]+0.30) 
            {
              currGear = '6';
            }
              else 
              {
                currGear = 'N';
              }
             
  // If gear has changed -> refresh display
  if( currGear != lastGear )
  {
    reset();
    lastGear = currGear;
   
    drawGear(currGear);           
    delay(10);
  }
}

void lernGears()
{
  int gCount = 0;
  int check = 0;
 
  while( gCount < GEAR_COUNT )
  {
    val = getVoltage();
    check = 0;
    for(i=0;i<6;i++)
    {
      if( 0 != gearsTab[i] && ( val <= gearsTab[i]+0.25 && val >= gearsTab[i]-0.25 ))
      {
        check = 1;
        break;
      }
    }
    if( 0 == check )
    {
      gearsTab[gCount]=val;
      gCount++;
    }
    delay(1000);
    showGears();
  }
  sort(gearsTab,GEAR_COUNT);
  EEPROM.updateBlock(0, gearsTab);
}

void showGears()
{
  reset();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print(gearsTab[0]);   
  display.setCursor(0,10);
  display.print(gearsTab[1]);   
  display.setCursor(0,20);
  display.print(gearsTab[2]);   
  display.setCursor(68,0);
  display.print(gearsTab[3]);   
  display.setCursor(68,10);
  display.print(gearsTab[4]);   
  display.setCursor(68,20);
  display.print(gearsTab[5]);     
  display.display();
}

void sort(float a[], int size)
{
  for(int k=0; k<(size-1); k++)
  {
    for(int o=0; o<(size-(k+1)); o++)
    {
      if( a[o] > a[o+1] )
      {
        float t = a[o];
        a[o] = a[o+1];
        a[o+1] = t;
      }
    }
  }
}

float getVoltage()
{
  sumVal = 0;
  minVal = 9999;
  for(i=0; i< 100; i++)
  {
    for (j=0; j < 10; j++){
        raw = analogRead(A0);
        if(minVal>raw) minVal = raw;
    }
    sumVal += minVal * (5.0 / 1023.0);
  }
  return (sumVal / 100);
}

void reset()
{
  display.clearDisplay();
}

void drawGear(char arg)
{
  display.setTextSize(6);
  display.setTextColor(WHITE);
  display.setCursor(display.width()/2-display.width()/4+18,11);
  display.drawRoundRect((display.width()/2)-(display.width()/4), 0, (display.width()/2), display.height(), display.height()/6, WHITE);
  display.drawRoundRect((display.width()/2)-(display.width()/4)-1, 1, (display.width()/2)+2, display.height()-2, display.height()/6, WHITE);
  display.print(arg);   
  display.display();
}

void drawFloat(float arg, int x, int y)
{
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(x,y);

  display.print(arg);   
  display.display();
}