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

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

int spark_plug = 7; //rele collegato alla candela
int led = 3;

int sw = 5;
int sw_flag = 0;

int shift = 6;
int shift_flag = 0;

unsigned long wait = 0 , cnt = 0;


//display variables
int x = 0;
int y = 23;
int width = 127;
int height = 9;


void setup()
{
  Serial.begin(9600);
  pinMode(spark_plug, OUTPUT);
  pinMode(led, OUTPUT);
  pinMode(sw, INPUT_PULLUP);
  pinMode(shift, INPUT_PULLUP);
  
  if(!display.begin()) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

}

//map function: returns the waiting time 
unsigned long selected_map(int cnt){
  switch(cnt){
    case 1:
    	return 10;
    case 2:
    	return 20;
    case 3:
    	return 30;
    case 4:
    	return 40;
    case 5:
    	return 50;
    case 6:
    	return 60;
  }

}

void drawBar(){
  
  int step = 21;
  display.drawRect(x, y, width, height, SSD1306_WHITE);

  for(int i = 0; i < 5; i++){
    display.fillRect(x+step, y, 1, 8, SSD1306_WHITE);
    step += 21;
  }
}

void drawProgress() {

  int step = 0;

  for(int i = 0; i < cnt; i++){
    display.fillRect(x+step+2, y+2, 18, 5, SSD1306_WHITE);
    step+=21;
  }  
}

void loop()
{

  /////////display output///////////
  display.clearDisplay();
  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.setCursor(0, 5);
  display.print("CutTime: ");
  display.setCursor(65, 0);
  display.setTextSize(2);
  

  ////////////temporized task//////////
  if(cnt){
    
    display.println(wait);
    display.setCursor(105, 0);
    display.println("ms");
      ////////////read input for activate quickshifter////////////
    if (digitalRead(shift) == 0 && shift_flag == 0){
      shift_flag = 1;
      digitalWrite(spark_plug, HIGH);
      digitalWrite(led, HIGH);
      delay(wait);
      digitalWrite(spark_plug, LOW);
      digitalWrite(led, LOW);
      //Serial.println("map: " + String(cnt) + " selected_time: " + String(selected_time) + " wait_time: " + String(wait));
    }
    if(digitalRead(shift) == 1 && shift_flag == 1) shift_flag = 0;
  }
  else{
    display.println("OFF");
  }
  
  
  ////////////read input for changing map////////////
  if (digitalRead(sw) == 0 && sw_flag == 0){
    sw_flag = 1;
    cnt++;
    wait = selected_map(cnt);
    if(cnt > 6) cnt = 0;
    //Serial.println("map: " + String(cnt) + " selected_time: " + String(selected_time) + " wait_time: " + String(wait));
  }
  if(digitalRead(sw) == 1 && sw_flag == 1) sw_flag = 0;
  

  drawBar();
  drawProgress();
  display.display();  
}