// Version 0.2

#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 gearsTab[GEAR_COUNT] = {0};


// BEGIN - YOU CAN MODIFY THIS SECTION //

// Show voltage flag
//#define SHOW_V ON

// Manual Gears definition flag
//#define MANUAL_DEF ON

// Gears definitions
#define GEAR_1 1.39
#define GEAR_2 1.83
#define GEAR_3 2.52
#define GEAR_4 3.26
#define GEAR_5 4.12
#define GEAR_6 4.57

// END - YOU CAN MODIFY THIS SECTION //


void setup()
{
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  // Setup oled i2c address
  pinMode(A0, INPUT);  // Set A0 like analog input - ADC
 
#ifndef MANUAL_DEF
  float val = 0;
 
  val = getVoltage();
  if ( val > HIGH_GEAR )
  {
    lernGears();
  }
  else
  {
    readEEPROM();
  }
 
#else 
  hardcodeGears();
#endif
}

void loop()
{   
  static char lastGear = 0, currGear = 0;
  float val = 0.0;
  val = getVoltage();

#ifndef SHOW_V
  // Match voltage to gear
  currGear = checkGear(val);
             
  // If gear has changed -> refresh display
  if( currGear != lastGear )
  {
    reset();
    lastGear = currGear;
   
    drawGear(currGear);           
    delay(10);
  }
#else
  reset();
  drawFloat(val,1,1);
  delay(100);
#endif
}

char checkGear(float value)
{
  char gear = 0;
  if (value <= gearsTab[0]+((gearsTab[1]-gearsTab[0])/2)) 
  {
    //if('2' != lastGear) 
    gear = '1';
  }
    else if (value <= gearsTab[1]+((gearsTab[2]-gearsTab[1])/2)) 
    {
      //if('1' != lastGear) 
      gear = '2';
    }
      else if (value <= gearsTab[2]+((gearsTab[3]-gearsTab[2])/2)) 
      {
        gear = '3';
      }
        else if (value <= gearsTab[3]+((gearsTab[4]-gearsTab[3])/2)) 
        {
          gear = '4';
        }
          else if (value <= gearsTab[4]+((gearsTab[5]-gearsTab[4])/2)) 
          {
            gear = '5';
          }
            else if (value <= gearsTab[5]+0.30) 
            {
              gear = '6';
            }
              else 
              {
                gear = 'N';
              }
  return gear;
}

void readEEPROM()
{
  float check = 0;
  EEPROM.readBlock(0, gearsTab);
  for(int i=0;i<GEAR_COUNT;i++)
  {
    check += gearsTab[i];
  }
  if( 0 == check ) lernGears();
}

void hardcodeGears()
{
  gearsTab[0] = GEAR_1;
  gearsTab[1] = GEAR_2;
  gearsTab[2] = GEAR_3;
  gearsTab[3] = GEAR_4;
  gearsTab[4] = GEAR_5;
  gearsTab[5] = GEAR_6;
}

void lernGears()
{
  int gCount = 0;
  int check = 0;
  float val = 0.0;
 
  while( gCount < GEAR_COUNT )
  {
    val = getVoltage();
    if(val < 5.0)
    {
      check = 0;
      for(int 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();
    }
    else
    {
      delay(500);
    }
  }
  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()
{
  float sumVal = 0;
  int minVal = 9999, raw = 0;
  for(int i=0; i< 100; i++)
  {
    for (int 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();
}