#include <loopTimer.h>
#include <IRremote.hpp>
#include "FastAccelStepper.h"

/*-----( Pins )-----*/
#define dirPinStepper 32  // Pin connected to DIR pin of Driver
//#define enablePinStepper 6  //Disabled
#define stepPinStepper 33  // Pin connected to STEP pin of Driver
#define receiver 34  // Signal Pin of IR receiver Digital Pin
#define home_switch 35  // Pin connected to Home Switch (MicroSwitch)
//#define beep_pin 10  // Digital Pin used for different Beep Tones

/*-----( Constants )-----*/
const int stepsPerMM = 200;  // Steps per MM of linear travel, [(Steps per rotation)/(Leadscrew Lead in MM)]
const int maxLength = 400;  // Maximum linear movement in MM, Software endstop
const int speedLimit = 3000;  // Top limit for speed
const byte incriment = 10;  // Number of steps to use for the IR Remote controlled speed and length

/*-----( Variables )-----*/
float maxSteps = (maxLength*stepsPerMM);  // Max Length in steps, taken from constants
byte program = 0;  // Program number to start running
bool isHomed = false;
int remoteLength = 5;  // Used to store the length setting, set by IR Remote, from 0-10
float currentLength = -((maxSteps/incriment)*remoteLength);  // Set current stroke length, in steps, using remoteLength as a scale
int remoteSpeed = 10;  // Used to store the speed setting, set by IR Remote, from 0-10
float currentSpeed = (speedLimit/incriment)*remoteSpeed;  // Set current speed, using remoteSpeed as a scale
float currentAccel = 2000;  // Acceleration to use

FastAccelStepperEngine engine = FastAccelStepperEngine();
FastAccelStepper *stepper = NULL;

void setup() {
  pinMode(home_switch, INPUT_PULLUP);  // Set Home switch pin
  Serial.begin(115200); // Enable serial comms
  IrReceiver.begin(receiver, true);  // Start the IR receiver, set the date pin, set led feedback
  engine.init();
  stepper = engine.stepperConnectToPin(stepPinStepper);
  if (stepper) {
    stepper->setDirectionPin(dirPinStepper);
    //stepper->setEnablePin(enablePinStepper);
    stepper->setAutoEnable(false);
    stepper->setSpeedInHz(currentSpeed);
    stepper->setAcceleration(currentAccel);
    stepper->setJumpStart(250);
  }
  homeZero();  // Home to find first Zero
}

void loop() {
  loopTimer.check(Serial);  // Print Looptimer
  homeButtonCheck();  // Check if homing button is pressed
  homeZero();  // Home if needed
  checkIR();  // Recieve IR signal
  runProgram();
}

void runProgram(){
  switch(program){
    case 0:   // Program 0: Stop movement
      if(stepper->isRunning() == true){
        stepper->stopMove();
      }
      break;
    case 1:   // Program 1: Following adjustable speed and length tables
      if(stepper->isRunning() == false){
        if (stepper->getCurrentPosition() == 0){
          stepper->setSpeedInHz(currentSpeed);
          stepper->moveTo(currentLength, false);
        }
        else{
          stepper->setSpeedInHz(currentSpeed);
          stepper->moveTo(0, false);
        }
        break;
      }
      break;
    default:  // Otherwise set to program 0: stop
      program = 0;
      break;
  }
}

void homeButtonCheck(){     // Stops stepper, flags needing homed
  if(!digitalRead(home_switch)){
    stepper->forceStop();  // Stop
    isHomed = false;  // Home is not true
  }
}
void homeZero(){
  if(isHomed == false){
    stepper->setSpeedInHz(400); // Slow Down
    if(digitalRead(home_switch)){       // If home not pressed
      stepper->runForward();           // Run Backwards
      while(digitalRead(home_switch));  // Until home switch is pressed
    }
    stepper->forceStop();  // Stop
    while(!digitalRead(home_switch)){
     stepper->backwardStep(true);    // While home is pressed, step forwards slowly
     delay(5);
   }
    stepper->forceStopAndNewPosition(0);  // Abrupt stop, set home
    stepper->setSpeedInHz(currentSpeed);  // Return to normal speed
    isHomed = true;
  }
}
void checkIR(){
  if (IrReceiver.decode()) {  // Check if IR code has been received
    // IrReceiver.printIRResultShort(&Serial); // Print complete received data in one line
    switch (IrReceiver.decodedIRData.command){
      case 0x22: // Button Test
        isHomed = false;
        break;
      case 0xE0: // Button Left  <<-
        remoteLength = remoteLength-1;
        if(remoteLength < 1){
          remoteLength = 1;
        }
        currentLength = -((maxSteps/incriment)*remoteLength);
        break;
      case 0x90: // Button Right ->>
        remoteLength = remoteLength+1;
        if(remoteLength > 10){
          remoteLength = 10;
        }
        currentLength = -((maxSteps/incriment)*remoteLength);
        break;
      case 0x2: // Button Up +
        remoteSpeed = remoteSpeed+1;
        if(remoteSpeed > 10){
          remoteSpeed = 10;
        }
        currentSpeed = (speedLimit/incriment)*remoteSpeed;
        stepper->applySpeedAcceleration();
        break;
      case 0x98: // Button Down -
        remoteSpeed = remoteSpeed-1;
        if(remoteSpeed < 1){
          remoteSpeed = 1;
        }
        currentSpeed = (speedLimit/incriment)*remoteSpeed;
        stepper->applySpeedAcceleration();
        break;
      case 0x68: // Button 0
        program = 0;
        break;
      case 0x30: // Button 1
        program = 1;
      //Serial.print("Program ");
      //Serial.println(program);
        break;
      case 0x18: // Button 2
        program = 2;
        break;
      case 0x7A: // Button 3
        program = 3;
        break;
      case 0x10: // Button 4
        program = 4;
        break;
      case 0x38: // Button 5
        program = 5;
        break;
      case 0x5A: // Button 6
        program = 6;
        break;
      default:  // If button is not recognised
        IrReceiver.printIRResultShort(&Serial);
        break;
    }
    IrReceiver.resume(); // Enable receiving of the next value
  }
}