#include <LiquidCrystal.h>
#include <Stepper.h>
#include <Servo.h>

#include "pitches.h"

LiquidCrystal lcd(12, 11, 10, 9, 8, 7);

const int stepsPerRevolution = 200;
const int stepperPin1 = 2;
const int stepperPin2 = 3;
const int stepperPin3 = 4;
const int stepperPin4 = 5;
Stepper stepper(stepsPerRevolution, stepperPin1, stepperPin2, stepperPin3, stepperPin4);

const float pulleyDiameterMM = 30; // mm
const float pulleyCircumferenceM = PI * pulleyDiameterMM / 1000;

Servo servo;
const int servoPin = 6;
const int servoCut = 90;
const int servoHome = 0;

const int upButton = A0;
const int downButton = A1;
const int leftButton = A2;
const int rightButton = A3;
const int enterButton = A4;

const int buzzer = 13;

// Define the variables for storing the settings
int length = 1; // length in cm
int unit = 1; // 0 = cm, 1 = m
int cuts = 5; // number of cuts
bool processing = false;

// Define the state variables for the menu system
int menuState = 0;
int processState = 0; // 0 = idle, 1 = measure, 2 = cut, 3 = end
int subMenuState = 0;

void setup() {
  lcd.begin(16, 2);
  lcd.clear();
  lcd.setCursor(1, 0);
  lcd.print("Thread Cutter!");

  // Set up the buttons as inputs
  pinMode(upButton, INPUT_PULLUP);
  pinMode(downButton, INPUT_PULLUP);
  pinMode(leftButton, INPUT_PULLUP);
  pinMode(rightButton, INPUT_PULLUP);
  pinMode(enterButton, INPUT_PULLUP);

  pinMode(buzzer, OUTPUT);

  stepper.setSpeed(100);

  servo.attach(servoPin);
  servo.write(servoHome);

  delay(3000);
  updateDisplay();
}

void loop() {
  if (digitalRead(upButton) == LOW) {
    handleUpButton();
  }
  if (digitalRead(downButton) == LOW) {
    handleDownButton();
  }
  if (digitalRead(leftButton) == LOW) {
    handleLeftButton();
  }
  if (digitalRead(rightButton) == LOW) {
    handleRightButton();
  }
  if (digitalRead(enterButton) == LOW) {
    handleEnterButton();
  }
  delay(150);

  // Check if the go button has been pressed
  if (processing) {
    startProcess();
  }
}

void handleUpButton() {
  switch (menuState) {
    case 0:
      length++;
      break;
    case 1:
      unit = !unit;
      break;
    case 2:
      cuts++;
      break;
    case 3:
      // do nothing
      break;
  }
  updateDisplay();
}

void handleDownButton() {
  switch (menuState) {
    case 0:
      length--;
      if (length < 1) {
        length = 1;
      }
      break;
    case 1:
      unit = !unit;
      break;
    case 2:
      cuts--;
      if (cuts < 1) {
        cuts = 1;
      }  break;
    case 3:
      // do nothing
      break;
  }
  updateDisplay();
}

void handleLeftButton() {
  menuState--;
  if (menuState < 0) {
    menuState = 3;
  }
  updateDisplay();
}

void handleRightButton() {
  menuState++;
  if (menuState > 3) {
    menuState = 0;
  }
  updateDisplay();
}

void handleEnterButton() {
  if (menuState == 3) {
    processing = true;
  }
}

void updateDisplay() {
  lcd.clear();
  if (processing) {
    switch (processState) {
      case 0: // idle
        break;
      case 1: // measure
        lcd.setCursor(0, 0);
        lcd.print("Midiendo! ");
        lcd.setCursor(0, 1);
        lcd.print("Pendiente: ");
        lcd.setCursor(11, 1);
        lcd.print(cuts);
        break;
      case 2: // cut
        lcd.setCursor(0, 0);
        lcd.print("Cortando! ");
        lcd.setCursor(0, 1);
        lcd.print("Pendiente: ");
        lcd.setCursor(11, 1);
        lcd.print(cuts);
        break;
      case 3: // cut
        lcd.setCursor(0, 0);
        lcd.print("Fin! ");
        break;
    }
  }  else  {
    switch (menuState) {
      case 0:
        lcd.setCursor(0, 0);
        lcd.print("Longitud: ");
        lcd.setCursor(0, 1);
        lcd.print(length);
        lcd.setCursor(5, 1);
        if (unit == 0) {
          lcd.print("Centimetros");
        } else {
          lcd.print("Metros");
        }
        break;
      case 1:
        lcd.setCursor(0, 0);
        lcd.print("Unidad: ");
        lcd.setCursor(0, 2);
        if (unit == 0) {
          lcd.print("Centimetros");
        } else {
          lcd.print("Metros");
        }
        break;
      case 2:
        lcd.setCursor(0, 0);
        lcd.print("Cantidad: ");
        lcd.setCursor(0, 1);
        lcd.print(cuts);
        break;
      case 3:
        lcd.setCursor(0, 0);
        lcd.print("Cortar!");
        String ndl = String(cuts) + " x " + String(length) ;
        ndl += unit == 0 ? " Centimetros" : " Metros";
        lcd.setCursor(0, 1);
        lcd.print(ndl);
        break;
    }
  }
}

void startProcess() {


  tone(buzzer, NOTE_DS5);
  delay(300);
  tone(buzzer, NOTE_D5);
  delay(300);
  tone(buzzer, NOTE_CS5);
  delay(300);
  noTone(buzzer);


  float lengthInMeters = 0.01 * length;
  if (unit == 1) {
    lengthInMeters = length;
  }

  float revolution = lengthInMeters / pulleyCircumferenceM;

  long stepsToTake = revolution * stepsPerRevolution;

  for (; cuts >= 1; cuts--) {
    measure(stepsToTake);
    cut();
  }

  processState = 3;
  updateDisplay();
  // maybe play sound
  delay(5000);

  processing = false;
  cuts = 1;
  updateDisplay();

}

void measure(long stepsToTake) {
  processState = 1;
  updateDisplay();

  // move stepper motor
  stepper.step(stepsToTake);

  processState = 0;
  updateDisplay();

}

void cut() {
  processState = 2;
  updateDisplay();

  // move servo
  servo.write(servoCut);
  delay(500);
  servo.write(servoHome);

  processState = 0;
  updateDisplay();
}