#include <AccelStepper.h>
#include <MultiStepper.h>
#include <StateMachine.h>
// define all the pins
//stepper z1
#define z1Dir 22
#define z1Ena 24
#define z1Pwm 2

//stepper z2
#define z2Dir 26
#define z2Ena 28
#define z2Pwm 3

//stepper x
#define xDir 34
#define xEna 36
#define xPwm 5

//stepper y
#define yDir 30
#define yEna 32
#define yPwm 4

//proximity sensors
#define sensor1 38
#define sensor2 40
#define sensor3 42
#define sensor4 44
#define sensor5 46
#define sensor6 48

#define sensorGrip1 50
#define sensorGrip2 52

#define lightGate 53

//buttons
#define startButton 45
#define stopButton 43
#define estopButton 41
#define clearButton 39

//leds
#define redLed 47
#define greenLed 49
#define yellowLed 51

//limit switches
#define z1LimitU 23
#define z1LimitD 25
#define z2LimitU 27
#define z2LimitD 29
#define xLimitU 31
#define xLimitD 33
#define yLimitU 35
#define yLimitD 37

//dc motor
#define dcPWM 6
#define dcForward 14
#define dcBackward 15

// intializing the stepper motors
AccelStepper stepperZ1(AccelStepper::DRIVER, z1Pwm, z1Dir);
AccelStepper stepperZ2(AccelStepper::DRIVER, z2Pwm, z2Dir);
AccelStepper stepperX(AccelStepper::DRIVER, xPwm, xDir);
AccelStepper stepperY(AccelStepper::DRIVER, yPwm, yDir);

// intializing the state machine
StateMachine machine = StateMachine();

State* Off = machine.addState(&stateOff);
State* Idle = machine.addState(&stateIdle);
State* ScreenInstructions = machine.addState(&stateScreenInstructions);
State* SerialIntructions = machine.addState(&stateSerialIntructions);
State* eStop = machine.addState(&stateeStop);
State* previousState;


// global variables
bool error = false;
bool homingZNeeded = false;
bool homingYNeeded = false;
bool homingXNeeded = false;

// constant variables
const int state_delay = 1000;
const int microStepping = 16;
const int minimumPulseWidth = 25;

void setup() {
  // define pin's input / output

  // steppers:
  pinMode(z1Dir, OUTPUT);
  pinMode(z2Dir, OUTPUT);
  pinMode(xDir, OUTPUT);
  pinMode(yDir, OUTPUT);
  pinMode(z1Ena, OUTPUT);
  pinMode(z2Ena, OUTPUT);
  pinMode(xEna, OUTPUT);
  pinMode(yEna, OUTPUT);
  pinMode(z1Pwm, OUTPUT);
  pinMode(z2Pwm, OUTPUT);
  pinMode(xPwm, OUTPUT);
  pinMode(yPwm, OUTPUT);

  //sensors
  pinMode(sensor1, INPUT_PULLUP);
  pinMode(sensor2, INPUT_PULLUP);
  pinMode(sensor3, INPUT_PULLUP);
  pinMode(sensor4, INPUT_PULLUP);
  pinMode(sensor5, INPUT_PULLUP);
  pinMode(sensor6, INPUT_PULLUP);

  pinMode(sensorGrip1, INPUT_PULLUP);
  pinMode(sensorGrip2, INPUT_PULLUP);

  pinMode(lightGate, INPUT_PULLUP);

  //buttons
  pinMode(startButton, INPUT_PULLUP);
  pinMode(stopButton, INPUT_PULLUP);
  pinMode(estopButton, INPUT_PULLUP);
  pinMode(clearButton, INPUT_PULLUP);

  //leds
  pinMode(redLed, OUTPUT);
  pinMode(greenLed, OUTPUT);
  pinMode(yellowLed, OUTPUT);

  //limit switches
  pinMode(z1LimitU, INPUT_PULLUP);
  pinMode(z1LimitD, INPUT_PULLUP);
  pinMode(z2LimitU, INPUT_PULLUP);
  pinMode(z2LimitD, INPUT_PULLUP);
  pinMode(xLimitU, INPUT_PULLUP);
  pinMode(xLimitD, INPUT_PULLUP);
  pinMode(yLimitU, INPUT_PULLUP);
  pinMode(yLimitD, INPUT_PULLUP);

  //dc motor
  pinMode(dcPWM, OUTPUT);
  pinMode(dcForward, OUTPUT);
  pinMode(dcBackward, OUTPUT);

  // set the default parameters for the stepper motors
  stepperZ1.setMaxSpeed(1000 * microStepping);
  stepperZ1.setAcceleration(300 * microStepping);
  stepperZ1.setMinPulseWidth(minimumPulseWidth);
  stepperZ1.setEnablePin(z1Ena);
  stepperZ1.setPinsInverted(false, false, true);
  stepperZ1.disableOutputs();

  stepperZ2.setMaxSpeed(1000 * microStepping);
  stepperZ2.setAcceleration(300 * microStepping);
  stepperZ2.setMinPulseWidth(minimumPulseWidth);
  stepperZ2.setEnablePin(z2Ena);
  stepperZ2.setPinsInverted(false, false, true);
  stepperZ2.disableOutputs();

  stepperX.setMaxSpeed(1000 * microStepping);
  stepperX.setAcceleration(300 * microStepping);
  stepperX.setMinPulseWidth(minimumPulseWidth);
  stepperX.setEnablePin(xEna);
  stepperX.setPinsInverted(false, false, true);
  stepperX.disableOutputs();

  stepperY.setMaxSpeed(1000 * microStepping);
  stepperY.setAcceleration(300 * microStepping);
  stepperY.setMinPulseWidth(minimumPulseWidth);
  stepperY.setEnablePin(yEna);
  stepperY.setPinsInverted(false, false, true);
  stepperY.disableOutputs();

  // interrupts definining
  //attachInterrupt(digitalPinToInterrupt(estopButton), eStop, FALLING);
  //attachInterrupt(digitalPinToInterrupt(z1LimitU), stopZ1Stepper, FALLING);
  //attachInterrupt(digitalPinToInterrupt(z1LimitD), stopZ1Stepper, FALLING);
  //attachInterrupt(digitalPinToInterrupt(z2LimitU), stopZ2Stepper, FALLING);
  //attachInterrupt(digitalPinToInterrupt(z2LimitD), stopZ2Stepper, FALLING);
  //attachInterrupt(digitalPinToInterrupt(xLimitU), stopXStepper, FALLING);
  //attachInterrupt(digitalPinToInterrupt(xLimitD), stopXStepper, FALLING);
  //attachInterrupt(digitalPinToInterrupt(yLimitU), stopYStepper, FALLING);
  //attachInterrupt(digitalPinToInterrupt(yLimitD), stopYStepper, FALLING);

  // setting up state machine
  Off->addTransition(&transitionOffIdle, Idle);
  Idle->addTransition(&transitionIdleScreenInstructions, ScreenInstructions);
  Idle->addTransition(&transitionIdleSerialIntructions, SerialIntructions);
  Idle->addTransition(&transitioneStop, eStop);
  Idle->addTransition(&transitionIdleOff, Off);
  ScreenInstructions->addTransition(&transitioneStop, eStop);
  SerialIntructions->addTransition(&transitioneStop, eStop);
}

void loop() {
  machine.run();
}

void stopZ1Stepper() {
  stepperZ1.stop();
  stepperZ1.disableOutputs();
  stepperZ1.moveTo(stepperZ1.currentPosition());
  homingZNeeded = true;
}

void stopZ2Stepper() {
  stepperZ2.stop();
  stepperZ2.moveTo(stepperZ2.currentPosition());
  homingZNeeded = true;
}

void stopXStepper() {
  stepperX.stop();
  stepperX.moveTo(stepperX.currentPosition());
  homingXNeeded = true;
}

void stopYStepper() {
  stepperY.stop();
  stepperY.moveTo(stepperY.currentPosition());
  homingYNeeded = true;
}

void stopSteppers() {
  stopZ1Stepper();
  stopZ2Stepper();
  stopXStepper();
  stopYStepper();
}

bool checkHoming() {
  if (homingXNeeded || homingYNeeded || homingZNeeded) {
    return true;
  }
  return false;
}

void moveSteppers(int zCoord, int yCoord, int xCoord) {
  stepperZ1.move(zCoord * microStepping);
  stepperZ2.move(zCoord * microStepping);
  stepperX.move(xCoord * microStepping);
  stepperY.move(yCoord * microStepping);

  while (stepperZ1.distanceToGo() != 0 || stepperZ2.distanceToGo() != 0) {
    stepperZ1.run();
    stepperZ2.run();
  }
  stepperZ1.enableOutputs();
  stepperZ2.enableOutputs();

  while (stepperY.distanceToGo() != 0) {
    stepperY.run();
  }
  stepperY.enableOutputs();

  while (stepperX.distanceToGo() != 0) {
    stepperX.run();
  }
  stepperX.enableOutputs();
}

bool buttonPressed(int buttonPin) {
  if (digitalRead(buttonPin)) {
    return false;
  }
  return true;
}

void stateOff() {
  //runs once
  if (machine.executeOnce) {
    digitalWrite(greenLed, LOW);
    digitalWrite(yellowLed, LOW);
    digitalWrite(redLed, HIGH);
  }
  //loops
}

void stateIdle() {
  //runs once
  if (machine.executeOnce) {
    digitalWrite(greenLed, HIGH);
    digitalWrite(yellowLed, LOW);
    digitalWrite(redLed, LOW);
    stepperZ1.enableOutputs();
    stepperZ2.enableOutputs();
    stepperY.enableOutputs();
    stepperX.enableOutputs();
  }
  //loops
}

void stateScreenInstructions() {
  //runs once
  if (machine.executeOnce) {
    digitalWrite(greenLed, HIGH);
    digitalWrite(yellowLed, LOW);
    digitalWrite(redLed, LOW);
    moveSteppers(random(100, 1000), random(100, 1000), random(100, 1000));
    machine.transitionTo(Idle);
  }
  //loops
}

void stateSerialIntructions() {
  //runs once
  if (machine.executeOnce) {
    digitalWrite(greenLed, HIGH);
    digitalWrite(yellowLed, LOW);
    digitalWrite(redLed, LOW);
    machine.transitionTo(Idle);
  }
  //loops
}

void stateeStop() {
  //runs once
  if (machine.executeOnce) {
    digitalWrite(greenLed, LOW);
    digitalWrite(yellowLed, HIGH);
    digitalWrite(redLed, LOW);
    stopSteppers();
    error = true;
  }
  //loops
  if(buttonPressed(clearButton)){
    machine.transitionTo(previousState);
  }
}


bool transitionOffIdle() {
  if (buttonPressed(startButton)) {
    previousState = machine.currentState;
    return true;
  }
  return false;
}

bool transitionIdleScreenInstructions() {
  if (buttonPressed(lightGate)) {
    previousState = machine.currentState;
    return true;
  }
  return false;
}

bool transitionIdleSerialIntructions() {
  return false;
}


bool transitioneStop() {
  if (buttonPressed(estopButton)) {
    previousState = machine.currentState;
    return true;
  }
  return false;  
}

bool transitionIdleOff(){
    if (buttonPressed(stopButton)) {
    previousState = machine.currentState;
    return true;
  }
  return false;  
}