#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Fonts/FreeSans9pt7b.h>
#include <Wire.h>
#include <ezButton.h>

Adafruit_SSD1306 myDisplay(128, 64, &Wire);

ezButton button1(D0);  // create Button object that attach to pin 0;
ezButton button2(D1);  // create Button object that attach to pin 1;
ezButton button3(D2);  // create Button object that attach to pin 2;

const int button1Pin = D0;
const int button2Pin = D1;
const int button3Pin = D2;
const int valve1Pin = D3;
const int motor2Pin = D6;
const int motor1Pin = D7;
const int motor3Pin = D8;
const int motor4Pin = D9;
const int valve2Pin = D10;

int button1State = LOW;
int button2State = LOW;
int button3State = LOW;

void setup() {
  Serial.begin(9600);
  myDisplay.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  myDisplay.setFont(&FreeSans9pt7b);
  button1.setDebounceTime(100); // set debounce time to 50 milliseconds
  button2.setDebounceTime(100); // set debounce time to 50 milliseconds
  button3.setDebounceTime(100); // set debounce time to 50 milliseconds
  button1.setCountMode(COUNT_FALLING);
  button2.setCountMode(COUNT_FALLING);
  button3.setCountMode(COUNT_FALLING);
  pinMode(button1Pin, INPUT);
  pinMode(button2Pin, INPUT);
  pinMode(button3Pin, INPUT);
  pinMode(motor1Pin, OUTPUT);
  pinMode(valve1Pin, OUTPUT);
  pinMode(motor2Pin, OUTPUT);
  pinMode(motor3Pin, OUTPUT);
  pinMode(valve2Pin, OUTPUT);
  pinMode(motor4Pin, OUTPUT);
}

void loop() {
  myDisplay.clearDisplay();
  myDisplay.setTextSize(1);
  myDisplay.setTextColor(WHITE);
  myDisplay.setCursor(0,15);

  button1.loop(); // MUST call the loop() function first
  button2.loop(); // MUST call the loop() function first
  button3.loop(); // MUST call the loop() function first

  digitalWrite(motor1Pin, LOW);
  digitalWrite(valve1Pin, LOW);
  digitalWrite(motor2Pin, LOW);
  digitalWrite(motor3Pin, LOW);
  digitalWrite(valve2Pin, LOW);
  digitalWrite(motor4Pin, LOW);

  button1State = digitalRead(button1Pin);
  button2State = digitalRead(button2Pin);
  button3State = digitalRead(button3Pin);

  if (button1State == HIGH) {
    if (button2State == HIGH) {
      if (button3State == HIGH) {
        Serial.println("Program A is running");

        myDisplay.clearDisplay();
        myDisplay.println("Peristalsis");
        myDisplay.println("RUNNING");
        myDisplay.display();

        for(int i = 0; i < 2; i ++) {

          digitalWrite(motor1Pin, HIGH);
          digitalWrite(valve1Pin, LOW);
          digitalWrite(motor2Pin, LOW);

          digitalWrite(motor3Pin, LOW);
          digitalWrite(valve2Pin, LOW);
          digitalWrite(motor4Pin, LOW);

          delay(1000);

          digitalWrite(motor1Pin, LOW);
          digitalWrite(valve1Pin, HIGH);
          digitalWrite(motor2Pin, HIGH);

          digitalWrite(motor3Pin, LOW);
          digitalWrite(valve2Pin, LOW);
          digitalWrite(motor4Pin, LOW);

          delay(1000);

          digitalWrite(motor1Pin, LOW);
          digitalWrite(valve1Pin, LOW);
          digitalWrite(motor2Pin, LOW);

          digitalWrite(motor3Pin, HIGH);
          digitalWrite(valve2Pin, LOW);
          digitalWrite(motor4Pin, LOW);

          delay(1000);

          digitalWrite(motor1Pin, LOW);
          digitalWrite(valve1Pin, LOW);
          digitalWrite(motor2Pin, LOW);

          digitalWrite(motor3Pin, LOW);
          digitalWrite(valve2Pin, HIGH);
          digitalWrite(motor4Pin, HIGH);

          delay(1000);
        }
        myDisplay.clearDisplay();
        myDisplay.setCursor(0,15);
        myDisplay.println("Peristalsis");
        myDisplay.println("DONE");
        myDisplay.display();
      }
    }
  }

  if(button1.isReleased() && button2.isReleased() ) {

    Serial.println("Program B is running");

    myDisplay.clearDisplay();
    myDisplay.println("Segmentation");
    myDisplay.println("RUNNING");
    myDisplay.display();

    for(int i = 0; i < 1; i ++) {

      digitalWrite(motor1Pin, HIGH);
      digitalWrite(valve1Pin, LOW);
      digitalWrite(motor2Pin, LOW);

      digitalWrite(motor3Pin, HIGH);
      digitalWrite(valve2Pin, LOW);
      digitalWrite(motor4Pin, LOW);

      delay(1000);

      digitalWrite(motor1Pin, LOW);
      digitalWrite(valve1Pin, LOW);
      digitalWrite(motor2Pin, LOW);

      digitalWrite(motor3Pin, LOW);
      digitalWrite(valve2Pin, LOW);
      digitalWrite(motor4Pin, LOW);

      delay(1000);

      digitalWrite(motor1Pin, LOW);
      digitalWrite(valve1Pin, HIGH);
      digitalWrite(motor2Pin, HIGH);

      digitalWrite(motor3Pin, LOW);
      digitalWrite(valve2Pin, HIGH);
      digitalWrite(motor4Pin, HIGH);

      delay(1000);

      digitalWrite(motor1Pin, LOW);
      digitalWrite(valve1Pin, LOW);
      digitalWrite(motor2Pin, LOW);

      digitalWrite(motor3Pin, LOW);
      digitalWrite(valve2Pin, LOW);
      digitalWrite(motor4Pin, LOW);

      delay(1000);
    }
    myDisplay.clearDisplay();
    myDisplay.setCursor(0,15);
    myDisplay.println("Segmentation");
    myDisplay.println("DONE");
    myDisplay.display();
  }

  if(button1.isReleased() && button3.isReleased() ) {

    Serial.println("Program C is running");

    myDisplay.clearDisplay();
    myDisplay.println("Contractions");
    myDisplay.println("RUNNING");
    myDisplay.display();

    for(int i = 0; i < 1; i ++) {

      digitalWrite(motor1Pin, HIGH);
      digitalWrite(valve1Pin, LOW);
      digitalWrite(motor2Pin, LOW);

      digitalWrite(motor3Pin, LOW);
      digitalWrite(valve2Pin, LOW);
      digitalWrite(motor4Pin, LOW);

      delay(1000);

      digitalWrite(motor1Pin, LOW);
      digitalWrite(valve1Pin, LOW);
      digitalWrite(motor2Pin, LOW);

      digitalWrite(motor3Pin, HIGH);
      digitalWrite(valve2Pin, LOW);
      digitalWrite(motor4Pin, LOW);

      delay(1000);

      digitalWrite(motor1Pin, LOW);
      digitalWrite(valve1Pin, HIGH);
      digitalWrite(motor2Pin, HIGH);

      digitalWrite(motor3Pin, LOW);
      digitalWrite(valve2Pin, LOW);
      digitalWrite(motor4Pin, LOW);

      delay(1000);

      digitalWrite(motor1Pin, LOW);
      digitalWrite(valve1Pin, LOW);
      digitalWrite(motor2Pin, LOW);

      digitalWrite(motor3Pin, LOW);
      digitalWrite(valve2Pin, HIGH);
      digitalWrite(motor4Pin, HIGH);

      delay(1000);
    }
    myDisplay.clearDisplay();
    myDisplay.setCursor(0,15);
    myDisplay.println("Contractions");
    myDisplay.println("DONE");
    myDisplay.display();
  }

}