#include <ESP_FlexyStepper.h>
#include <Adafruit_GFX.h>
#include <SPI.h>
#include <Adafruit_ILI9341.h>
#include <ESP32Servo.h>
Servo Roll;
Servo Flip;

//Define Relay pins:
#define Heater 21

//Define Temp pin:
#define temp_s 35

//Define LED pins:
#define Red 32
#define Yellow 33
#define Green 25

//Define Btn pins:
#define Start_btn 26
#define Stop_btn 27
#define Emer_btn 14

//Define Ultrasonic pins:
#define trig 22
#define echo 34
float duration;
int distance;

//Define Motor pins:
const int dir_P = 16; //Plunger motor
const int dir_E = 17; //Extruder motor
const int step_P = 9;
const int step_E = 0;
const int speed = 200;
ESP_FlexyStepper plunger;
ESP_FlexyStepper extruder;

//Define TFT
#define TFT_DC 2
#define TFT_CS 15
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

void light(){
  tft.fillCircle(135, 165, 20, ILI9341_RED); 
  digitalWrite(Red, HIGH);
  digitalWrite(Yellow, LOW);
  digitalWrite(Green, LOW);
  delay(2000);
  tft.fillCircle(180, 165, 20, ILI9341_YELLOW); 
  digitalWrite(Red, LOW);
  digitalWrite(Yellow, HIGH);
  digitalWrite(Green, LOW);
  delay(2000);
  tft.fillCircle(225, 165, 20, ILI9341_GREEN); 
  digitalWrite(Red, LOW);
  digitalWrite(Yellow, LOW);
  digitalWrite(Green, HIGH);
  delay(2000);
  digitalWrite(Red, LOW);
  digitalWrite(Yellow, LOW);
  digitalWrite(Green, LOW);
}

void relay(){
  pinMode(Heater, HIGH);
  delay(3000);
  pinMode(Heater, LOW);
  delay(3000);
}

void motor(){
  digitalWrite(dir_P, HIGH); //Control motor spin direction.
  digitalWrite(dir_E, LOW); //Control motor spin direction.

  for(int x= 0; x < speed; x++)
  {
    digitalWrite(step_P, HIGH);
    delayMicroseconds(2000);
    digitalWrite(step_P, LOW);
    delayMicroseconds(2000);
    digitalWrite(step_E, HIGH);
    delayMicroseconds(2000);
    digitalWrite(step_E, LOW);
    delayMicroseconds(2000);
  } 
  delay(100);
}

void motor2_0(){
  // set the speed and acceleration rates for the stepper motor
  plunger.setSpeedInStepsPerSecond(100);
  plunger.setAccelerationInStepsPerSecondPerSecond(100);  
  extruder.setSpeedInStepsPerSecond(100);
  extruder.setAccelerationInStepsPerSecondPerSecond(100);
  // Rotate the motor in the forward direction one revolution (200 steps). 
  // This function call will not return until the motion is complete.
  
  plunger.moveRelativeInSteps(200);
  delay(1000);
  // rotate backward 1 rotation, then wait 1 second
  plunger.moveRelativeInSteps(-200);
  delay(1000);
  extruder.moveRelativeInSteps(200);
  delay(1000);
  // rotate backward 1 rotation, then wait 1 second
  extruder.moveRelativeInSteps(-200);
  delay(1000);

}

void display(){
  tft.setCursor(60, 80);
  tft.setTextColor(ILI9341_YELLOW);
  tft.setTextSize(3);
  tft.println("Donut Maker X");

  tft.setCursor(40, 140);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(2);
  tft.println("System Initializing :-)");
}

void range(){
  digitalWrite(trig, HIGH);
  delayMicroseconds(1000);
  digitalWrite(trig, LOW);
  duration = pulseIn(echo, HIGH);
  distance = (duration/2) / 29.1;
  
  Serial.print("Left: ");
  Serial.print(distance);
  Serial.println("cm");
}

void servo(){
  Roll.write(360);
  delay(3000);
  Flip.write(180);
  delay(1500);
  Roll.write(0);
  Flip.write(0);
  delay(1000);
}

void program(){
  int btn_state;

  Serial.print("Main Program Starting");
  int x = digitalRead(Start_btn);
  int y = digitalRead(Stop_btn);
  int z = digitalRead(Emer_btn);

  if(x == 1 && y == 0 && z == 0){
    btn_state = 1;
    Serial.println(btn_state);
  }
  else if(x == 0 && y == 1 && z == 0){
    btn_state = 2;
    Serial.println(btn_state);
  }
  else if(x == 0 && y == 0 && z == 1){
    btn_state = 3;
    Serial.println(btn_state);
  }
  else{
    btn_state = 0;
    Serial.println(btn_state);
  }

  if(btn_state == 1){
    tft.fillScreen(ILI9341_BLACK);
    tft.setCursor(65, 80);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(3);
    Serial.print("Light Test");
    tft.print("Running Light");
    tft.setCursor(145, 110);
    tft.print("Test!");
    light();
    btn_state = 0;
    delay(100);
    tft.fillScreen(ILI9341_BLACK);
  }
  else if(btn_state == 2){
    tft.fillScreen(ILI9341_BLACK);
    tft.setCursor(65, 80);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(3);
    Serial.print("Servo Test");
    tft.print("Running Servo");
    tft.setCursor(145, 110);
    tft.print("Test!");
    servo();
    btn_state = 0;
    delay(100);
    tft.fillScreen(ILI9341_BLACK);
  }
  else if(btn_state == 3){
    tft.fillScreen(ILI9341_BLACK);
    tft.setCursor(65, 80);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(3);
    Serial.print("Motor Test");
    tft.print("Runnning Motor");
    tft.setCursor(145, 110);
    tft.print("Test!");
    motor2_0();
    btn_state = 0;
    delay(100);
    tft.fillScreen(ILI9341_BLACK);
  }
  else{
    tft.setCursor(12, 100);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(3);
    Serial.print("No Button Pressed");
    tft.print("No Button Pressed");
    delay(100);
  }
  Serial.println();
  delay(100);
}

void btn(){
  int x = digitalRead(Start_btn);
  int y = digitalRead(Stop_btn);
  int z = digitalRead(Emer_btn);

  Serial.print("Start =");
  Serial.print(x);
  Serial.print("  Stop =");
  Serial.print(y);
  Serial.print("  Emer =");
  Serial.print(z);
  Serial.println();
}

void setup() {
  // put your setup code here, to run once:
  tft.begin();
  tft.setRotation(3);
  Serial.begin(115200);
  Serial.println("System Starting");
  display();
  Roll.attach(13);
  Flip.attach(12);

  pinMode(Heater, OUTPUT);

  pinMode(dir_P, OUTPUT);
  pinMode(dir_E, OUTPUT);
  pinMode(step_P, OUTPUT);
  pinMode(step_E, OUTPUT);
  plunger.connectToPins(step_P, dir_P);
  extruder.connectToPins(step_E, dir_E);

  pinMode(Red, OUTPUT);
  pinMode(Yellow, OUTPUT);
  pinMode(Green, OUTPUT);

  pinMode(Start_btn, INPUT);
  pinMode(Stop_btn, INPUT);
  pinMode(Emer_btn, INPUT);

  pinMode(trig ,OUTPUT);
  pinMode(echo ,INPUT);

  Roll.write(0);
  Flip.write(0);
  delay(2000);
  tft.fillScreen(ILI9341_BLACK);
}

void loop() {
  program();
  //btn();
  //motor();
  //servo();
  //motor2_0();
  //relay();
  //light();
  //range();
  //display();
  delay(100);
}