/* Sweep
 by BARRAGAN <http://barraganstudio.com>
 This example code is in the public domain.

 modified 8 Nov 2013
 by Scott Fitzgerald
 https://www.arduino.cc/en/Tutorial/LibraryExamples/Sweep
*/

#include <Servo.h>

#define LS_TRAY A3              // Limit switch input to provide feedback for Tray Movement
#define SERVO_PIN 13            // Servo motor signal attached to digital pin 13 of Arduino Uno
#define RETRACT_DUR 2000         // Duration for which servo moves to attain retracted position from resting
#define EJECT_DUR  2000          // Duration fro which servo moves to attain ejecred position from resting 
#define SERVO_SPEED 250         // Speed of servo motor
#define BUTTON_A 12             // sets the buttonA input on pin 12
#define BUTTON_B 11             // sets the buttonB input on pin 11

enum position{RETRACTED, RESTING, EJECTED};
enum position servo_position;
enum position req_pos;

int prev_ls_tray_state = 0;
uint32_t servo_timer = 0;


Servo servo_motor;  // create servo object to control a servo
// twelve servo objects can be created on most boards

int pos = 0;    // variable to store the servo position

void setup() {
  Serial.begin(9600);
  pinMode(LS_TRAY, INPUT);
  pinMode(SERVO_PIN, OUTPUT);
  pinMode(BUTTON_A, INPUT_PULLUP);
  pinMode(BUTTON_B, INPUT_PULLUP);
  servo_motor.attach(SERVO_PIN);
  req_pos = RESTING;
  delay(1000);
}

void loop() {
  if ( digitalRead(BUTTON_A) == 0 ) {
    req_pos = RETRACTED;
  }
  if ( digitalRead(BUTTON_B) == 0 ) {
    req_pos = EJECTED;
  }
  move_servo();
  Serial.print("\tButtonA:");Serial.print(digitalRead(BUTTON_A));
  Serial.print("\tButtonB:");Serial.print(digitalRead(BUTTON_B));
  //Serial.print("\tLS POSITION: ");Serial.print(digitalRead(LS_TRAY));
  Serial.print("\tServo Position");Serial.print(servo_position);
  Serial.print("\tRequired: ");Serial.println(req_pos);
  delay(200);
}

int move_to_resting () {
    Serial.print("PTRAY: ");Serial.print(prev_ls_tray_state);
    Serial.print("\tTRAY: ");Serial.print(digitalRead(LS_TRAY));
    
    if ( prev_ls_tray_state == 0 ) {
      if ( prev_ls_tray_state == digitalRead(LS_TRAY) ) {
          servo_motor.writeMicroseconds(1500 + SERVO_SPEED);
          return 0;
      } else {
          servo_motor.writeMicroseconds(1500);
          return 1;
      }
    } else {
      if ( prev_ls_tray_state == digitalRead(LS_TRAY) ) {
          servo_motor.writeMicroseconds(1500 - SERVO_SPEED);
          return 0;
      } else {
          servo_motor.writeMicroseconds(1500);
          return 1;
      }
    }
}

void move_servo () {
    int t_req_pos = req_pos;
    if ( req_pos != servo_position ) {
      if (servo_position == EJECTED ) {
        if ( req_pos == RETRACTED ) {
          t_req_pos = RESTING;
          servo_timer = millis();
        }
      } else if ( servo_position == RETRACTED ) {
          if (req_pos = EJECTED ) {
            t_req_pos = RESTING;
            servo_timer = millis();
          }
      }
      switch (t_req_pos) {
          case RETRACTED:
            Serial.print("\tRETRACTING\t");
            if ( millis() - servo_timer < RETRACT_DUR ) {
                servo_motor.writeMicroseconds(1500 - SERVO_SPEED);
            } else {
                servo_position = RETRACTED;
                servo_motor.writeMicroseconds(1500);
            }
          break;
          
          case RESTING:
          Serial.print("\RESTING\t");
              if ( move_to_resting() == 1 ) {
                  servo_position = RESTING;
                  servo_motor.writeMicroseconds(1500);
              }
          break;

          case EJECTED:
          Serial.print("\tEJECTING\t");
              if ( millis() - servo_timer < EJECT_DUR ) {
                servo_motor.writeMicroseconds(1500 + SERVO_SPEED);
              } else {
                servo_position = EJECTED;
                servo_motor.writeMicroseconds(1500);
              }
          break;
      }
    } else {
      servo_timer = millis ();
      prev_ls_tray_state = digitalRead(LS_TRAY);
    }
}