/*
Pins for the CNC Shield v3

 X_STP_PIN           2
 Y_STP_PIN           3
 Z_STP_PIN           4

 X_DIR_PIN           5
 Y_DIR_PIN           6
 Z_DIR_PIN           7

 SHIELD_EN_PIN       8 pos useage for turning steppers off for realligment
*/


// INCLUDES
#include 
#include 

// Define the stepper motor connections
const int Z_DIR_PIN = 7;   //  Z Takeup Reels Direction Pin
const int Z_STP_PIN = 4;  //  Z Takeup Reels Step Pin
const int X_DIR_PIN = 5;   //  X Capstans Direction Pin
const int X_STP_PIN = 2;  //  X Capstans Step Pin

const int stepsPerRev = 200;   // Number of steps for a full rotation(360) // SPROCKET(CAPSTAN)
const int stepsPerRev2 = 200;  // Number of steps for a full rotation(360) // Takeup Reel
const int microSteps = 16;     // 16th microstepping
//const int takeupMaxSpeed = 250;     // Takeup Reel Max Speed
//const int takeupSpeed = 250;        // Takeup Reel Speed
//const int capstanMaxSpeed = 600;    // Capstan Reel Max Speed
//const int capstanSpeed = 600;       // Capstan Reel Speed 120
//const int capstanAccel = 99999999;  // * Sets the steppers acceleration to a high number so that the stepper
//const int takeupAccel = 99999999;   // * number so that the stepper Doesn't acceralate. 99999999

const int takeupMaxSpeed = 450;   // Takeup Reel Max Speed
const int takeupSpeed = 450;      // Takeup Reel Speed
const int capstanMaxSpeed = 900;  // Capstan Reel Max Speed
const int capstanSpeed = 900;     // Capstan Reel Speed 120

const int capstanAccel = 800;      // * Sets the steppers acceleration to a high number so that the stepper
const int takeupAccel = 99999999;  // * number so that the stepper Doesn't acceralate. 99999999

int steps = 800;  //  Number of steps to move x amount of 8mm cine film frames.
/*
__________________
| STEPS | FRAMES |
|-------|--------|
|  100  |   6    |
|  200  |   12   | 1
|  300  |   18   |
|  400  |   24   | 2
|  500  |   30   |
|  600  |   36   | 3
|  700  |   42   |
|  800  |   48   | 4
|  900  |   54   |
| 1000  |   60   | 5
*/

// Create two new instances of the AccelStepper class
AccelStepper takeup(AccelStepper::DRIVER, Z_STP_PIN, Z_DIR_PIN);   // Runs all the time unless switch is pressed!
AccelStepper capstan(AccelStepper::DRIVER, X_STP_PIN, X_DIR_PIN);  // Runs only when told to via serial coms

// SWITCH SETUP
ezButton tensionSW(9);   // Create ezButton object that attach to pin 7;
ezButton stopButton(11);  // Create ezButton object that attach to pin 3;

bool isTakeupActive = false;       // Flag to track the status of the takeup stepper
bool isCapstanContinuous = false;  // Flag to indicate capstan continuous movement

void setup() {
  Serial.begin(9600);  // Initialize serial communication at 9600 baud

  Serial.println("");
  Serial.println("M = Move");  //
  Serial.println("T = Activate Takeup Stepper");  //
  Serial.println("X = enable capstan continuous movement");  //
  Serial.println("");

  tensionSW.setDebounceTime(50);  // set debounce time to 50 milliseconds

  takeup.setMaxSpeed(takeupMaxSpeed);   // Set the maximum speed of the stepper motor
  takeup.setSpeed(takeupSpeed);         // Set the speed of the stepper motor
  takeup.setAcceleration(takeupAccel);  // Set the acceleration of the stepper motor

  capstan.setMaxSpeed(capstanMaxSpeed);   // Set the maximum speed of the stepper motor
  capstan.setSpeed(capstanSpeed);         // Set the speed of the stepper motor
  capstan.setAcceleration(capstanAccel);  // Set the acceleration of the stepper motor

  takeup.move(999999999);  // * Tells the Takeup stepper to to move x steps
                           // * when takeup.run() is called in the loop.
}

void loop() {
  serialCheck();
  tensionSW.loop();  // Checks that status of the switch
  stopButton.loop();
  switchReleased();  // code for when the switch is released
  switchPressed();   // code for when the switch is pressed

  if (isTakeupActive) {
    takeup.run();
  }

  if (isCapstanContinuous) {
    capstan.move(steps * microSteps);
    capstan.run();
    if (stopButton.isPressed()) {
      isCapstanContinuous = false;  // Stop the capstan continuous movement
      capstan.stop();
      Serial.println("Capstan movement stopped.");
    }
  } else {
    capstan.run();
  }
}

void serialCheck() {
  // Note: we shall process one command at a time! This means: reset the
  // following variable to zero as soon as the command is done processing.
  static char current_command;

  // Do not proceed with another command until we're done with the current one!
  if (current_command == 0) {
    if (Serial.available() > 0) {
      current_command = Serial.read();
      // Perform a single action upon receiving a command. Makes code easier to read.
      switch (current_command) {
        default:
          current_command = 0;
          break;  // Ignore unknown commands
        case 'M':
          capstan.move(steps * 16);
          break;   // Move the motor 100 steps
        case 'T':  // Activate the takeup stepper
          isTakeupActive = true;
          break;
        case 'A':  // Activate the takeup stepper
          isTakeupActive = false; //not sure this is working!
          break;
        case 'X':  // enable capstan continuous movement
          isCapstanContinuous = true;
          break;
      }
    }
  } else {
    // Send a message when the motor is done moving:
    if (capstan.isRunning() == false) {
      current_command = 0;  // Reset the current_command
      Serial.println("S");  // Send message "We're done!"
    }
  }
}

void switchReleased() {

  if (tensionSW.isReleased()) {
    Serial.println("MOVING - Trigger Released");
    takeup.move(999999999);
  }
}

void switchPressed() {
  if (tensionSW.isPressed()) {
    Serial.println("STOPPED - Trigger Pressed");
    takeup.stop();
  }
}