// Import the required libraries
#include <Wire.h>
#include "SSD1306Ascii.h"
#include "SSD1306AsciiWire.h"
#include <Servo.h>

// Define the I2C address and reset pin of the OLED display
#define I2C_ADDRESS 0x3C
#define RST_PIN -1

// Create an instance of the SSD1306AsciiWire class
SSD1306AsciiWire oled;

// Define the pin numbers for the LEDs and buttons
int Led_Red = 13;
int Led_Yellow = A0;
int up = 12;
int down = 11;
int left = 9;
int right = 10;
int select = 8;
int start = 7;
int es_up = A1;
int es_down = A2;

// Define the variables for the state, length, inch, cm, mm, and pieces
int state = 0;
int length = 0;
int inch = 0;
int step_inch = 112;
int feed_speed = 700;
int step_cut = 5000;
int cut_speed = 800;
int cm = 0;
int mm = 0;
int pieces = 0;

// Define the pin numbers for the motors
const int MOT1_DIR = 4; 
const int MOT1_STP = 3; 
const int MOT2_DIR = 6;
const int MOT2_STP = 5;

//#define MOT1_DIR 4
//#define MOT1_STP 3
//#define MOT2_DIR 6
//#define MOT2_STP 5

// Create a servo object called servo1 Attached with A3
//Servo servo;

//int servoPin = A3;


// The setup function is called once when the Arduino starts up
void setup() {
  // Initialize the I2C communication
  Wire.begin();
  Wire.setClock(400000L);
  // Initialize the serial port
  //Serial.begin(9600);

  // Initialize the OLED display
  oled.begin(&Adafruit128x64, I2C_ADDRESS);
  oled.setFont(Adafruit5x7);
  oled.clear();

  // Set the pin modes for the LEDs and buttons
  pinMode(Led_Red, OUTPUT);
  pinMode(Led_Yellow, OUTPUT);
  pinMode(up, INPUT_PULLUP);
  pinMode(down, INPUT_PULLUP);
  pinMode(left, INPUT_PULLUP);
  pinMode(right, INPUT_PULLUP);
  pinMode(select, INPUT_PULLUP);
  pinMode(start, INPUT_PULLUP);

  // Set the pin modes for the motors
  pinMode(MOT1_DIR, OUTPUT);
  pinMode(MOT1_STP, OUTPUT);
  pinMode(MOT2_DIR, OUTPUT);
  pinMode(MOT2_STP, OUTPUT);

  //servo.attach(A3);
}


//=============================================================================
//---------------------------- loop() -----------------------------------------
//---- Wait for a button press and then call matching function ----------------
//=============================================================================

void loop() {
  // put your main code here, to run repeatedly:

  First_Screen(); // Display "Positive Cutter" Vir.1.1 Powered by JamyKowshik
  //delay(1000);
  
  // State = 0,1 --------------------- Mode Selection ------------------
  //--------------------------------------------------------------------

  if (state == 0 && !digitalRead(up)) 
  { // 0 >Up> 1 Auto(3)
    digitalWrite(Led_Yellow, HIGH);
    delay(20);
    Mode_Selection();
    cursorup();
    state = 1;
    digitalWrite(Led_Yellow, LOW);
  }

  if (state == 1 && !digitalRead(up)) 
  { // 1 >Up> 1 Auto(3)
    digitalWrite(Led_Yellow, HIGH);
    delay(20);
    Mode_Selection();
    cursorup();
    state = 1;
    digitalWrite(Led_Yellow, LOW);
  }

  if (state == 0 && !digitalRead(down)) 
  { // 0 >Down> 0 Manual(2)
    digitalWrite(Led_Yellow, HIGH);
    delay(20);
    Mode_Selection();
    cursordown();
    state = 0;
    digitalWrite(Led_Yellow, LOW);
  }

  if (state == 1 && !digitalRead(down)) 
  { // 1 >Down> 0 Manual(2)
    digitalWrite(Led_Yellow, HIGH);
    delay(20);
    Mode_Selection();
    cursordown();
    state = 0;
    digitalWrite(Led_Yellow, LOW);
  }

  //============================SELECT=============================

  // Move the servo to 5 degrees
  //servo.write(5);
  delay(20);

  if (state == 0 && !digitalRead(select)) 
  {
    digitalWrite(Led_Yellow, HIGH);
    delay(200);
    oled.clear();
    Manual_Mode();
    state = 2;
    digitalWrite(Led_Yellow, LOW);
  }

  if (state == 1 && !digitalRead(select)) 
  {
    digitalWrite(Led_Yellow, HIGH);
    delay(200);
    oled.clear();
    Length_Menu();
    state = 3;
    digitalWrite(Led_Yellow, LOW);
  }


// State = 2, -------------------- Menual Mode --------------------
//-----------------------------------------------------------------

if (state == 2 && !digitalRead(left))
{
  digitalWrite(Led_Yellow, HIGH);
  delay(100);
  Feed_Out_Manual();
  digitalWrite(Led_Yellow, LOW);
}

if (state == 2 && !digitalRead(right))
{
  digitalWrite(Led_Yellow, HIGH);
  delay(100);
  Feed_In_Manual();
  digitalWrite(Led_Yellow, LOW);
}

if (state == 2 && !digitalRead(up))
{
  
  digitalWrite(Led_Yellow, HIGH);
  delay(20);
  Cut_Up();
  digitalWrite(Led_Yellow, LOW);
}

if (state == 2 && !digitalRead(down))
{
  digitalWrite(Led_Yellow, HIGH);
  delay(20);
  Cut_Down();
  digitalWrite(Led_Yellow, LOW);
}

if (state == 2 && !digitalRead(start))
{
  delay(200);
  digitalWrite(Led_Red, HIGH);
  digitalWrite(Led_Yellow, HIGH);
  
  // Move the servo to 45 degrees
  //servo.write(45);
  delay(1000);
  
  Cut_Up();

// Move the servo to 10 degrees
  //servo.write(10);
  delay(1000);

  Feed_Back();
  delay(1000);

  Cut_Down();
  digitalWrite(Led_Yellow, LOW);
  digitalWrite(Led_Red, LOW);
  
  delay(1000);
  Feed_Return();
}

if (state == 2 && !digitalRead(select))
{
  digitalWrite(Led_Yellow, HIGH);
  delay(200);
  oled.clear();
  state = 0;
  digitalWrite(Led_Yellow, LOW);
}

//---------------------------------------------------------------------
// State = 3, ----------------------- Auto Mode -----------------------
//------------------------- Lenght Input and Print Value --------------

if (state == 3 && !digitalRead(right))
{
  digitalWrite(Led_Yellow, HIGH);
  delay(200);
  inch += 1;
  //delay(75);
  oled.clear();
  oled.set1X();
  oled.println();
  oled.set2X();
  oled.println();
  oled.println();
  oled.setCursor(20, 45);
  oled.print(inch, DEC);
  oled.setCursor(60, 45);
  oled.print("=INCH");
  digitalWrite(Led_Yellow, LOW);
}

if (state == 3 && !digitalRead(left))
{
  digitalWrite(Led_Yellow, HIGH);
  delay(200);
  inch -= 1;
  //delay(75);
  oled.clear();
  oled.set1X();
  oled.println();
  oled.set2X();
  oled.println();
  oled.println();
  oled.setCursor(20, 45);
  oled.print(inch, DEC);
  oled.setCursor(60, 45);
  oled.print("=INCH");
  digitalWrite(Led_Yellow, LOW);
}

if (state == 3 && !digitalRead(up))
{
  digitalWrite(Led_Yellow, HIGH);
  delay(200);
  inch += 10;
  //delay(75);
  oled.clear();
  oled.set1X();
  oled.println();
  oled.set2X();
  oled.println();
  oled.println();
  oled.setCursor(20, 45);
  oled.print(inch, DEC);
  oled.setCursor(60, 45);
  oled.print("=INCH");
  digitalWrite(Led_Yellow, LOW);
}

if (state == 3 && !digitalRead(down))
{
  digitalWrite(Led_Yellow, HIGH);
  delay(200);
  inch -= 10;
  //delay(75);
  oled.clear();
  oled.set1X();
  oled.println();
  oled.set2X();
  oled.println();
  oled.println();
  oled.setCursor(20, 45);
  oled.print(inch, DEC);
  oled.setCursor(60, 45);
  oled.print("=INCH");
  digitalWrite(Led_Yellow, LOW);
}

if (state == 3 && !digitalRead(select))
{
  digitalWrite(Led_Yellow, HIGH);
  delay(200);
  oled.clear();
  Quantity_Menu();
  state = 5;
  digitalWrite(Led_Yellow, LOW);
}

// State = 5, ---------- Quantity Input and Print Value ----------

if (state == 5 && !digitalRead(right))
{
  digitalWrite(Led_Yellow, HIGH);
  delay(200);
  pieces += 1;
  //delay(75);
  oled.clear();
  oled.set1X();
  oled.println();
  oled.set2X();
  oled.println();
  oled.println();
  oled.setCursor(20, 45);
  oled.print(pieces, DEC);
  oled.setCursor(60, 45);
  oled.print("=PIECES");
  digitalWrite(Led_Yellow, LOW);
}

if (state == 5 && !digitalRead(left))
{
  digitalWrite(Led_Yellow, HIGH);
  delay(200);
  pieces -= 1;
  //delay(75);
  oled.clear();
  oled.set1X();
  oled.println();
  oled.set2X();
  oled.println();
  oled.println();
  oled.setCursor(20, 45);
  oled.print(pieces, DEC);
  oled.setCursor(60, 45);
  oled.print("=PIECES");
  digitalWrite(Led_Yellow, LOW);
}

if (state == 5 && !digitalRead(up))
{
  digitalWrite(Led_Yellow, HIGH);
  delay(200);
  pieces += 10;
  //delay(75);
  oled.clear();
  oled.set1X();
  oled.println();
  oled.set2X();
  oled.println();
  oled.println();
  oled.setCursor(20, 45);
  oled.print(pieces, DEC);
  oled.setCursor(60, 45);
  oled.print("=PIECES");
  digitalWrite(Led_Yellow, LOW);
}

if (state == 5 && !digitalRead(down))
{
  digitalWrite(Led_Yellow, HIGH);
  delay(200);
  pieces -= 10;
  //delay(75);
  oled.clear();
  oled.set1X();
  oled.println();
  oled.set2X();
  oled.println();
  oled.println();
  oled.setCursor(20, 45);
  oled.print(pieces, DEC);
  oled.setCursor(60, 45);
  oled.print("=PIECES");
  digitalWrite(Led_Yellow, LOW);
}

//---------- Print Lenght and Quantity Value ----------

if (state == 5 && !digitalRead(select))
{
  digitalWrite(Led_Yellow, HIGH);
  delay(200);
  oled.clear();
  oled.println();
  oled.println();
  oled.println();
  oled.set2X();
  oled.setCursor(20, 45);
  oled.print(inch, DEC);
  oled.setCursor(60, 45);
  oled.println("=INCH");
  oled.setCursor(20, 45);
  oled.print(pieces, DEC);
  oled.setCursor(60, 45);
  oled.print("=PIECES");
  digitalWrite(Led_Yellow, LOW);
}

//---------- Start Auto Cutting ----------

if (state == 5 && !digitalRead(start))
{
  digitalWrite(Led_Yellow, HIGH);
  delay(500);
  digitalWrite(Led_Yellow, LOW);
  digitalWrite(Led_Red, HIGH);

  oled.clear();
  oled.set2X();
  oled.setCursor(22, 25);
  oled.println("CUTTING");
  oled.println(">>> ======");
  oled.setCursor(55, 65);
  oled.println(pieces, DEC);
  oled.setCursor(5, 65);
  oled.println("====== <<<");

  for(int j=0; j< pieces; j++)
    {
     for(int k=0; k< inch; k++)
     {
      Feed_In();
      }
      // Move Down the servo to 45 degrees
      //servo.write(45);
      delay(1000);
      Cut_Up();
      // Move Up the servo to 15 degrees
      //servo.write(15);
      delay(1000);
      Feed_Back();

      delay(1000);
      Cut_Down();
      
      delay(1000);
      Feed_Return();

      oled.clear();
      oled.set2X();
      oled.setCursor(22, 25);
      oled.println("CUTTING");
      oled.println(">>> ======");
      oled.setCursor(55, 65);
      oled.println(pieces-1-j, DEC);
      oled.setCursor(5, 65);
      oled.println("====== <<<");
     }
    oled.clear();
    oled.set2X();
    oled.println();
    oled.println();
    oled.setCursor(12, 25);
    oled.println("COMPLETED");
    digitalWrite(Led_Red, LOW);
  inch =0;
  pieces =0;
  state = 0;
}

// Return to State 0, ----- Reset all Values ------

}

//===================================================================================
//-----------------------------------------------------------------------------------
// ------------------------- Functions Construction Section -------------------------
//-----------------------------------------------------------------------------------
//===================================================================================

// Display the first screen
void First_Screen() {
  oled.set2X();
  oled.setCursor(0, 0);
  oled.println("PCM::JMK V1");
  oled.set1X();
  oled.setCursor(0, 13);
  oled.println("---------------------");
}

// Feed in the material
void Feed_In() {
  digitalWrite(MOT1_DIR, HIGH);
  for (int x = 0; x < step_inch; x++) {
    digitalWrite(MOT1_STP, HIGH);
    delayMicroseconds(feed_speed);
    digitalWrite(MOT1_STP, LOW);
    delayMicroseconds(feed_speed);
  }
}

void Feed_In_Manual() {
  digitalWrite(MOT1_DIR, HIGH);
  for (int x = 0; x < 15; x++) {
    digitalWrite(MOT1_STP, HIGH);
    delayMicroseconds(feed_speed);
    digitalWrite(MOT1_STP, LOW);
    delayMicroseconds(feed_speed);
  }
}

// Feed Back the material
void Feed_Back() {
  digitalWrite(MOT1_DIR, LOW);
  for (int x = 0; x < 65; x++) {
    digitalWrite(MOT1_STP, HIGH);
    delayMicroseconds(feed_speed);
    digitalWrite(MOT1_STP, LOW);
    delayMicroseconds(feed_speed);
  }
}

// Feed Return the material
void Feed_Return() {
  digitalWrite(MOT1_DIR, HIGH);
  for (int x = 0; x < 65; x++) {
    digitalWrite(MOT1_STP, HIGH);
    delayMicroseconds(feed_speed);
    digitalWrite(MOT1_STP, LOW);
    delayMicroseconds(feed_speed);
  }
}

// Feed out the material
void Feed_Out() {
  digitalWrite(MOT1_DIR, LOW);
  for (int x = 0; x < step_inch; x++) {
    digitalWrite(MOT1_STP, HIGH);
    delayMicroseconds(feed_speed);
    digitalWrite(MOT1_STP, LOW);
    delayMicroseconds(feed_speed);
  }
}

void Feed_Out_Manual() {
  digitalWrite(MOT1_DIR, LOW);
  for (int x = 0; x < 115; x++) {
    digitalWrite(MOT1_STP, HIGH);
    delayMicroseconds(feed_speed);
    digitalWrite(MOT1_STP, LOW);
    delayMicroseconds(feed_speed);
  }
}
// Cut the material
void Cut_Up() {
  digitalWrite(MOT2_DIR, HIGH);
  for (int x = 0; x < step_cut; x++) 
  {
    if (!digitalRead(es_up)) {
      break; 
    }
    digitalWrite(MOT2_STP, HIGH);
    delayMicroseconds(cut_speed);
    digitalWrite(MOT2_STP, LOW);
    delayMicroseconds(cut_speed);
  }
}

void Cut_Down() {
  digitalWrite(MOT2_DIR, LOW);
  for (int x = 0; x < step_cut; x++) {
    if (!digitalRead(es_down)) {
      break;
    }
    digitalWrite(MOT2_STP, HIGH);
    delayMicroseconds(cut_speed);
    digitalWrite(MOT2_STP, LOW);
    delayMicroseconds(cut_speed);
  }
}


// Display the mode selection screen
void Mode_Selection() {
  oled.set2X();
  oled.setCursor(35, 25);
  oled.println(" AUTO  ");
  oled.setCursor(35, 45);
  oled.println(" MANUAL");
}

// Move the cursor up
void cursorup() {
  oled.set2X();
  oled.setCursor(0, 3);
  oled.println(">>>");
  oled.setCursor(0, 5);
  oled.println("   ");
}

// Move the cursor down
void cursordown() {
  oled.set2X();
  oled.setCursor(0, 3);
  oled.println("   ");
  oled.setCursor(0, 5);
  oled.println(">>>");
}

// Display the manual mode screen
void Manual_Mode() {
  oled.set2X();
  oled.setCursor(20, 65);
  oled.println("   ");
  oled.setCursor(5, 65);
  oled.println();
  oled.println("MANUAL MODE");
  oled.setCursor(5, 10);
  oled.println("<<= ON =>>");
}

// Display the length menu screen
void Length_Menu() {
  oled.set2X();
  oled.println();
  oled.println();
  oled.setCursor(25, 15);
  oled.println("LENGTH");
  oled.setCursor(50, 10);
  //oled.println("INCH");
}

// Display the quantity menu screen
void Quantity_Menu() {
  oled.set2X();
  oled.println();
  oled.println();
  oled.setCursor(18, 15);
  oled.println("QUANTITY");
  oled.setCursor(50, 10);
  //oled.println("PIECES");
}