//Screen init
#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);  

//Pins
#define GoButton A0
#define FuelPump 3
#define Drain 4
#define Injector1 5
#define Injector2 6
#define Injector3 7 
#define Injector4 8

//Print Locations
#define FirstRow 0,0
#define SecondRow 0, 11
#define ThirdRow 0, 22
#define ForthRow 0, 33
#define FifthRow 0, 44
#define SixthRow 0, 55
#define CountdownLocation 85, 44

//Pump prime, Pulse on in mS, Stop screen
#define PumpDelay 1500
#define SystemLatency 35
#define PulseDelayConstant 5
#define PulseTestOn 3
#define SonicTestOn 8
#define StopDelay 1000
#define JitterTime 5

//Long press check mS, how long to hold in seconds
#define LongPressCheck 300
#define LongPressCountdown 1.60


String SystemMode[4] = {"Flow test: 5sec 100%", "Pattern: 15sec 30%", "Sonic cleaner: 10min", "Drain mode: 30sec"};
String SystemStatus[4] = {"Flow Testing...", "Pattern Testing...", "Sonic Cleaning...", "Draining..."};
int ModeNumber = 0;


bool ButtonPressed () {
  if (digitalRead(GoButton)) return false;
  delay(JitterTime);
  if (digitalRead(GoButton)) 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_s6 (String value) {
  display.setCursor(SixthRow);
  display.print(value);
}

void print_countdown (float value) {
  display.setCursor(CountdownLocation);
  display.print(value, 1);
}

void print_mode(){
  Print_s1(SystemMode[ModeNumber]);
}

void print_status (){
  Print_s1(SystemStatus[ModeNumber]);
}

void print_prompt (){
  display.clearDisplay();
  print_mode();
  Print_s5 ("Hold to Start.");
  Print_s6 ("Click for Next Page.");
  display.display();
}

void print_abort(){
  Print_s5("Click to Abort.");
}

void print_running(float Timer) {
    display.clearDisplay();
    print_status();
    Print_f2(Timer);
    print_abort();
    display.display();

}

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

void InjectorsON(){
  digitalWrite(Injector1, LOW);
  digitalWrite(Injector2, LOW);
  digitalWrite(Injector3, LOW);
  digitalWrite(Injector4, LOW);
}

void InjectorsOFF(){
  digitalWrite(Injector1, HIGH);
  digitalWrite(Injector2, HIGH);
  digitalWrite(Injector3, HIGH);
  digitalWrite(Injector4, HIGH);
}

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

void FlowTest (){
  PrimePump();
  float Timer = 5.00;
  InjectorsON();
  while (Timer > 0 && digitalRead(GoButton)) {
    print_running(Timer);
    delay(100-SystemLatency);
    Timer-= 0.1;
   }
  TestDone();
}

void PulseTest(){
  PrimePump();
  float Timer = 15.00; 
 
  while (Timer > 0 && digitalRead(GoButton)) {
    print_running(Timer);
    for (int i=0; i < PulseDelayConstant; i++) {
      InjectorsON();
      delay(PulseTestOn);
      InjectorsOFF();
      delay(80-PulseTestOn);
    }
    Timer-= 0.1;
  }
  TestDone();
}

void SonicBath(){
  delay(PumpDelay);
  float Timer = 600.00; 
  while (Timer > 0 && digitalRead(GoButton)) {
    print_running(Timer);
    for (int i=0; i < PulseDelayConstant; i++) {
      InjectorsON();
      delay(SonicTestOn);
      InjectorsOFF();
      delay(SonicTestOn);
    }
    Timer-= 0.1;
  }
  TestDone();
}
void DrainLoop(){
  ModeNumber = 3;
  delay(PumpDelay);
  float Timer = 30.00; 
  digitalWrite(Drain, LOW);
  while (Timer > 0 && digitalRead(GoButton)) {
    print_running(Timer);
    delay(100-SystemLatency);
    Timer-= 0.1;
   }
   digitalWrite(Drain, HIGH);
   TestDone();
}

bool CountDown(){
  float timer = LongPressCountdown;
  while (timer > 0) {
    if (ButtonPressed()) {
      display.clearDisplay();
      print_mode();
      Print_s5("Hold to Start:");
      print_countdown(timer);
      display.display();
      delay(SystemLatency);
      timer -= 0.1;
    }
    else return false;
  }
  return true;
}

void setup()
{

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

  pinMode (Injector1, OUTPUT);
  pinMode (Injector2, OUTPUT);
  pinMode (Injector3, OUTPUT);
  pinMode (Injector4, OUTPUT);
  pinMode (FuelPump, OUTPUT);  
  pinMode (Drain, OUTPUT);
  
  pinMode (GoButton, INPUT_PULLUP);
  
  digitalWrite(Injector1, HIGH);
  digitalWrite(Injector2, HIGH);
  digitalWrite(Injector3, HIGH);
  digitalWrite(Injector4, HIGH);
  digitalWrite(FuelPump, HIGH);
  digitalWrite(Drain, HIGH);
  
}

void loop()
{

print_prompt();

if (ButtonPressed()) {
  delay(LongPressCheck);
  if (ButtonPressed()) {
    if (CountDown()) {
      if (ModeNumber == 3) DrainLoop();
      if (ModeNumber == 0) FlowTest();
      if (ModeNumber == 1) PulseTest();
      if (ModeNumber == 2) SonicBath();
     }
  }
  else {
    ModeNumber++;
    if (ModeNumber == 4) ModeNumber = 0;
  }
 
}

}