//Screen init begin
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define ScrX 128
#define ScrY 64
Adafruit_SSD1306 display = Adafruit_SSD1306(ScrX, ScrY, &Wire);  
//Screen init end

#define GoButton A0
#define ModeButton A1
#define FuelPump 3
#define Injector 4
#define FirstRow 0,0
#define SecondRow 0, 11
#define ThirdRow 0, 22
#define ForthRow 0, 33
#define FifthRow 0, 44
#define PumpDelay 1500
#define PulseTestOn 3
#define SonicTestOn 5
#define StopDelay 1000
#define JitterTime 30
#define SystemLatency 10
#define PulseDelayConstant 5



String SystemMode[3] = {"Flow test: 6sec 100%", "Pattern: 15sec 30%", "Sonic cleaner 10min"};
int ModeNumber = 0;
bool ModePressed = false;
bool GoPressed = false;

bool ButtonPressed (byte button) {
  if (digitalRead(button)) return false;
  delay(JitterTime);
  if (digitalRead(button)) return false;
  return true;
}

void Print_s1 (String value) {
  display.setCursor(FirstRow);
  display.print(value);
}
void Print_s2 (String value) {
  display.setCursor(SecondRow);
  display.print(value);
}
void Print_f2 (float value) {
  display.setCursor(SecondRow);
  display.print(value, 2);
}

void Print_s4 (String value) {
  display.setCursor(ForthRow);
  display.print(value);
}

void Print_s5 (String value) {
  display.setCursor(FifthRow);
  display.print(value);
}

void print_prompt (){
  display.clearDisplay();
  Print_s1 (SystemMode[ModeNumber]);
  Print_s5 ("Start?");
  display.display();
}

void PrimePump (){
  display.clearDisplay();
  Print_s1("Pump Priming...");
  display.display();
  digitalWrite(FuelPump, LOW);
  delay(PumpDelay);
}

void TestDone (){
  digitalWrite(Injector, HIGH);
  digitalWrite(FuelPump, HIGH);
  display.clearDisplay();
  Print_s1("Done!");
  display.display();
  delay(StopDelay);
}

void FlowTest (){
  PrimePump();
  float Timer = 6.00; 
  while (Timer > 0 && digitalRead(GoButton)) {
    display.clearDisplay();
    Print_s1("Flow Testing...");
    Print_f2(Timer);
    Print_s5("Abort?");
    display.display();
    digitalWrite(Injector, LOW);
    delay(SystemLatency);
    Timer-= 0.1;
   }
  TestDone();
}

void PulseTest(){
  PrimePump();
  float Timer = 15.00; 
 
  while (Timer > 0 && digitalRead(GoButton)) {
    display.clearDisplay();
    Print_s1("Pattern Testing...");
    Print_f2(Timer);
    Print_s5("Abort?");
    display.display();
    for (int i=0; i < PulseDelayConstant; i++) {
      digitalWrite(Injector, LOW);
      delay(PulseTestOn);
      digitalWrite(Injector, HIGH);
      delay((40-PulseTestOn-SystemLatency));
    }
    Timer-= 0.05;
  }
  TestDone();
}

void SonicBath(){
  float Timer = 600.00; 
  digitalWrite(Injector, LOW);
  while (Timer > 0 && digitalRead(GoButton)) {
    display.clearDisplay();
    Print_s1("Sonic Cleaning...");
    Print_f2(Timer);
    Print_s5("Abort?");
    display.display();
    for (int i=0; i < PulseDelayConstant; i++) {
      digitalWrite(Injector, LOW);
      delay(SonicTestOn);
      digitalWrite(Injector, HIGH);
      delay((20-PulseTestOn-SystemLatency));
    }
    Timer-= 0.05;
  }
  TestDone();
}


void setup()
{

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);  

  pinMode (Injector, OUTPUT);
  pinMode (FuelPump, OUTPUT);
  
  pinMode (GoButton, INPUT_PULLUP);
  pinMode (ModeButton, INPUT_PULLUP);
  
  digitalWrite(Injector, HIGH);
  digitalWrite(FuelPump, HIGH);
  
}

void loop()
{


if ( ButtonPressed(ModeButton) ) ModePressed = true;
if ( ModePressed && !ButtonPressed(ModeButton)) {
  ModeNumber++;
  ModePressed = false;
}
if ( ModeNumber==3 ) ModeNumber=0;

print_prompt();

if ( ButtonPressed(GoButton) ) GoPressed = true;
if ( GoPressed && !ButtonPressed(GoButton)) {
  if (ModeNumber == 0) FlowTest();
  if (ModeNumber == 1) PulseTest();
  if (ModeNumber == 2) SonicBath();
  GoPressed = false;
}

}