#include <AccelStepper.h>

#define NUM_PULSE_FIRST_TIME 80               //distance (4cm)
#define NUM_PULSE_NEXT_TIME 20                //distanse (1cm)
#define STEPS_PER_SECOND_SQUARED 400          //accel of motor
#define STEPS_PER_SECOND 500                  //velocity of motor
#define STEPS_PER_SECOND_FIRST_TO_SECOND 400  //velocity of motor from the first to the second sensor
#define DELAY_ENABLE_SIGNAL 10                //delay time between signal from machine and start move motor (ms)
#define CYCLE_NUM 4                           //number cycle after end of a paper
#define PIN_DIR 5                             //dir signal pin
#define PIN_STEP 6                            //step signal pin
#define PIN_SENSOR_ONE 8                      //signal from the first sensor pin
#define PIN_SENSOR_TWO 9                      //signal from the second sensor pin
#define PIN_OUT_ENABLE 11                     //enable output 
#define PIN_ENABLE_SWITCH 12                  //switch pin (schalter)
#define PIN_ENABLE_SIGNAL 13                  //enable signal from machine pin
#define SWITCH_DEBOUNCE_TIME 50               //debounce time for switch
#define DELAY_ENABLE_OUT_SIGNAL 200           //delay for enable output signal


uint32_t EnableSignalTimer = 0;   //
uint8_t numberLogic = 0;          //number logic step
uint8_t cycleNum = 0;             //number of cycle  end of a paper
bool firstSensor;                 //signal from the first sensor
bool secondSensor;                //signal from the second sensor
bool enableSignal;                //enable signal from machine
bool pfEnableSignal = false;      //save pos. front enable signal
bool runFeeder = false;           //run motor 1 or 4 cm
bool firstRun = false;            //run motor the first time
bool enableSignalSwitch = false;  //enable signal from the machine
int switchState = HIGH;
int switchStatePrev = HIGH;
unsigned long lastDebouncestateSwitchEnable = 0;
unsigned long debounceDelaySwitch = 50;

// stepper object
AccelStepper feeder(1, PIN_STEP, PIN_DIR); // pin 6 = step, pin 5 = direction

//called once after power on
void setup() {
  Serial.begin(9600);

  //drive setup
  feeder.setMaxSpeed(STEPS_PER_SECOND);
  feeder.setAcceleration(STEPS_PER_SECOND_SQUARED);

  //pin mode setup
  pinMode(PIN_SENSOR_ONE, INPUT);
  pinMode(PIN_SENSOR_TWO, INPUT);
  pinMode(PIN_ENABLE_SIGNAL, INPUT_PULLUP);
  pinMode(PIN_ENABLE_SWITCH, INPUT_PULLUP);
  pinMode(PIN_OUT_ENABLE, OUTPUT);
}

//main loop
void loop() {
  // read sensors states
  firstSensor = !digitalRead(PIN_SENSOR_ONE);
  secondSensor = !digitalRead(PIN_SENSOR_TWO);
  enableSignal = !digitalRead(PIN_ENABLE_SIGNAL);
  int stateSwitchEnableRead = digitalRead(PIN_ENABLE_SWITCH);
    
  if(stateSwitchEnableRead != switchStatePrev)
   lastDebouncestateSwitchEnable = millis();

 if ((millis() - lastDebouncestateSwitchEnable) > debounceDelaySwitch) {
    // whatever the reading is at, it's been there for longer
    // than the debounce delay, so take it as the actual current state:

    // if the button state has changed:
    if (stateSwitchEnableRead != switchState) {
      switchState = stateSwitchEnableRead;

      // only toggle the LED if the new button state is HIGH
      if (switchState == LOW) {
        Serial.println("Switch is toggled");
        enableSignalSwitch = true;
      }
    }
  }
  
  if (enableSignalSwitch){ // enable switch
  
  // positive edge detection
  if (enableSignal && !pfEnableSignal && millis() - EnableSignalTimer > 100) {
    pfEnableSignal = true;
    runFeeder = true;
    EnableSignalTimer = millis();
  }
  if (!enableSignal && pfEnableSignal && millis() - EnableSignalTimer > 100) {
    pfEnableSignal = false;
    EnableSignalTimer = millis();
  }

  // steps switch
  switch (numberLogic) {
    case 0: //wait paper
      if (firstSensor && !secondSensor) {
        feeder.setSpeed(STEPS_PER_SECOND_FIRST_TO_SECOND);
        numberLogic = 10;
      }
      break;

    case 10: //feed from the first to the second sensor

      if (secondSensor) {
        numberLogic = 11;
        firstRun = true;
      }
      else {
        feeder.runSpeed();
      }
      break;
    case 11: // set enable signal
      digitalWrite(PIN_OUT_ENABLE, HIGH);
      delay(2000);
      numberLogic = 20;      
    break;

    case 20: //wait enable signal
      if (enableSignal && runFeeder && firstSensor) {
        numberLogic = 30;
        digitalWrite(PIN_OUT_ENABLE, LOW);
      }
      if (enableSignal && runFeeder && !firstSensor) {
        cycleNum++;
        numberLogic = 30;
        digitalWrite(PIN_OUT_ENABLE, LOW);
      }
      break;

    case 30: //select distance
      if (firstRun) {
        feeder.move(NUM_PULSE_FIRST_TIME);
      }
      else {
        feeder.move(NUM_PULSE_NEXT_TIME);
      }
      feeder.run();
      numberLogic = 40;
      break;

    case 40: //wait motor move
      if (feeder.distanceToGo()) {
        feeder.run();
      }
      else {
        runFeeder = false;
        firstRun = false;
        numberLogic = 50;
      }
      break;

    case 50: //check end paper
      if (cycleNum < CYCLE_NUM) {
        numberLogic = 11;
      }
      else {
        numberLogic = 0;
        cycleNum = 0;
      }
      break;
  }
  }else{
    numberLogic = 0;
    cycleNum = 0;
    runFeeder = false;
    firstRun = false;
    digitalWrite(PIN_OUT_ENABLE, LOW);
  }
  
  // save the reading.  Next time through the loop,
  switchStatePrev = stateSwitchEnableRead;
  //Serial.print("step");
  //Serial.println(numberLogic);
}

/* 
 *  Changlog: 2022-06-29  Sensor 1 detects the end of paper. When sensor 1 detects black paper again it works only for 4 more steps. 
 *                        Sensor 2 is only for one detection at the beginning.
 *            2022-08-09  Pin 13 is changed to pullup
 *            2022-08-30  Pin 12 enable pin
 *                        Pin 11 enable signal from the board
 *
 */