#include <AccelStepper.h>

///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
////////                  VARIABLES TO CHANGE                      ////////
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////

int headShakeDistance = 400; // Ammount the head shakes from left to right in normal head shake
int headLeftPause = 500; // milliseconds pause after moving to the left (0 - 10000)
int headRightPause = 500; // milliseconds pause after moving to the right (0 - 10000)
int headForwardPause = 500; //milliseconds pause looking forward (0 - 10000)
int headMaxSpeed = 1000; // Max speed of the head turn
int headMaxAccelleration = 600; // Max accelleration of the head turn
int pauseAfterMotion = 2000; // Ammount of time in milliseconds to pause after motion is detected (1000 = 1 second)

///////////////////////////////////////////////////////////////////////////
////////                    ARDUINO PINS                           ////////
////////                ONLY CHANGE IF NEEDED                      ////////
///////////////////////////////////////////////////////////////////////////

unsigned char pirPin = 7; // choose the input pin (for PIR sensor / motion sensor)
unsigned char pulPin = 3; // Pull+ pin for the stepper motor driver
unsigned char dirPin = 4; // Dir+ pin for the stepper motor driver


///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
////////                    DON'T CHANGE BELOW                     ////////
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////


// Define a stepper and the pins it will use
// In DRIVER Mode Pin 3 is Pul+ and Pin 4 is Dir+
AccelStepper stepper(AccelStepper::DRIVER, pulPin, dirPin);

unsigned long currentMillis; // keep track of current time in milliseconds
unsigned long randomStepTimer;
unsigned long startTimer;
unsigned long pauseTimer;
unsigned long leftRightTimer;
int distanceFlag = 0;

enum states {
  START,
  SHAKE_HEAD,
  WAIT,
  DETECT_MOTION,
  MOVE_HEAD,
  PAUSE
};
enum states headState = START;

enum moveStates {
  SET_DISTANCE,
  LEFT,
  LEFT_PAUSE,
  RIGHT,
  RIGHT_PAUSE,
  LOOK_FORWARD,
  FORWARD_PAUSE,
  STOP

};
enum moveStates shakeHead = SET_DISTANCE;

int pirState = LOW; // we start, assuming no motion detected
int val = 0; // variable for reading the pin status
int randomFlag = 0;
int randomRate = 1000; // change to increase random range

void setup() {
  stepper.setPinsInverted(false, false, true);
  stepper.setEnablePin(5);
  stepper.enableOutputs();
  pinMode(pirPin, INPUT); // declare sensor as input
  Serial.begin(9600);
  Serial.println("Start Steper Program");

  randomStepTimer = millis(); // setup the random timer to be at the current time
  startTimer = millis();
  pauseTimer = millis();
  leftRightTimer = millis();


  
  stepper.setCurrentPosition(0); //sets starting position of stepper at zero
  stepper.setMaxSpeed(headMaxSpeed); // set max speed of stepper motor
  stepper.setAcceleration(headMaxAccelleration); // set max accelleration of stepper motor
}

void loop() {
  currentMillis = millis(); // set variable to current time in milliseconds
  checkHeadState();
  checkPirSensor();
  //moveHeadRandom(); // uncomment to move head random (remember to comment all other lines in loop)
  //bounceHead();
}

void checkHeadState() {
  switch (headState) {


    case START:
      if (checkTime(startTimer, 1000UL)) {
        Serial.println("Start Complete");
        Serial.println("Switch to WAIT");
        headState = SHAKE_HEAD;
      }
      break;

    case SHAKE_HEAD:
      distanceFlag = 1;
      moveHead(headShakeDistance); //use 400 for microsteps
      break;

    case WAIT:

      headState = SHAKE_HEAD;

      break;

    case DETECT_MOTION:
      break;

    case MOVE_HEAD:
      //moveHeadRandom();
      distanceFlag = 1;
      //bounceHead();

      break;

    case PAUSE:
      if (checkTime(pauseTimer, pauseAfterMotion)) {
        headState = WAIT;
      }
      break;
  }
}

void checkPirSensor() {
  val = digitalRead(pirPin); // read input value
  if (val == HIGH) { // check if the input is HIGH
    if (pirState == LOW) {
      // we have just turned on
      Serial.println("Motion detected!");
      // We only want to print on the output change, not state
      pirState = HIGH;
    }
  } else {
    if (pirState == HIGH) {
      // we have just turned of
      Serial.println("Motion ended!");
      // We only want to print on the output change, not state
      pirState = LOW;
    }
  }
  if (pirState == HIGH) {
    Serial.println("Switch to PAUSE");
    pauseTimer = millis();
    headState = PAUSE;
  }
}

void moveHeadRandom() {
  if (stepper.distanceToGo() == 0 && randomFlag == 0) {
    // Random change to speed, position and acceleration
    // Make sure we dont get 0 speed or accelerations
    stepper.moveTo(rand() % randomRate);
    stepper.setMaxSpeed((rand() % randomRate) + 1);
    stepper.setAcceleration((rand() % randomRate) + 1);
    randomFlag = 1;
  }

  if (stepper.distanceToGo() != 0) {
    stepper.enableOutputs();
    stepper.run();
  }
  if (stepper.distanceToGo() == 0 && randomFlag == 1) {
    stepper.disableOutputs();
    Serial.println("Moved Head Randomly");
    Serial.println("Switch to Pause");
    randomFlag = 0;
    headState = PAUSE;
  }
}

void bounceHead() {
  // If at the end of travel go to the other end
  if (stepper.distanceToGo() == 0) {
    stepper.moveTo(-stepper.currentPosition());
  }
  stepper.run();
}

void moveHead(int steps) {
  stepper.run();
  switch (shakeHead) {
    case SET_DISTANCE:
      if (stepper.currentPosition() != 0) {
        stepper.moveTo(0);
        stepper.runToPosition();
      }
      stepper.moveTo(steps);
      Serial.println("SET_DISTANCE");
      if (distanceFlag == 1) {
        distanceFlag = 0;
        leftRightTimer = millis();
        shakeHead = LEFT;
      }
      break;

    case LEFT:
      if (stepper.distanceToGo() == 0) {
        Serial.println("END LEFT");
        leftRightTimer = millis();
        shakeHead = LEFT_PAUSE;
      }
      break;

    case LEFT_PAUSE:
      if (checkTime(leftRightTimer, headLeftPause)) {
        stepper.moveTo(-stepper.currentPosition());
        Serial.println("END LEFT_PAUSE");
        shakeHead = RIGHT;
      }
      break;

    case RIGHT:
      if (stepper.distanceToGo() == 0) {
        Serial.println("END RIGHT");
        leftRightTimer = millis();
        shakeHead = RIGHT_PAUSE;
      }
      break;

    case RIGHT_PAUSE:
      if (checkTime(leftRightTimer, headRightPause)) {
        Serial.println("END RIGHT_PAUSE");
        stepper.moveTo(0);
        shakeHead = LOOK_FORWARD;
      }
      break;

    case LOOK_FORWARD:
      if (stepper.distanceToGo() == 0) {
        Serial.println("END LOOK_FORWARD");
        leftRightTimer = millis();
        shakeHead = FORWARD_PAUSE;
      }
      break;

    case FORWARD_PAUSE:
      if (checkTime(leftRightTimer, headForwardPause)) {
        Serial.println("END FORWARD_PAUSE");
        shakeHead = STOP;
      }
      break;

    case STOP:
      Serial.println("STOP");
      shakeHead = SET_DISTANCE;
      headState = WAIT;
      break;
  }
}

// variable names modified from original by Larry D for clarity
// timerLength is how long the timer is in Milliseconds
// lastTimerExpiredTime is the time in Milliseconds of the last time the timer expired

// BEGIN CheckTime()
boolean checkTime(unsigned long& lastTimerExpiredTime, unsigned long timerLength) {
  // is the time up for this task?
  if (currentMillis - lastTimerExpiredTime >= timerLength) {
    lastTimerExpiredTime += timerLength; //get ready for the next iteration
    return true;
  }
  return false;
}
//END CheckTime()