#include <Servo.h>
#include <AccelStepper.h>

#define dirPin 12
#define stepPin 13
#define motorInterfaceType 1

Servo myservoRight;  // create servo object to control a servo
Servo myservoLeft;  // create servo object to control a servo
#define servoPinRight 3 //~
#define servoPinLeft 6 //~
#define pushButtonPin 2

int led = 5;         // the PWM pin the LED is attached to
int brightness = 0;  // how bright the LED is
int fadeAmount = 5;  // how many points to fade the LED by

int angleRight = 179;    // initial angle  for Right servo (beteen 1 and 179)
int angleLeft = 179;    // initial angle  for Left servo (beteen 1 and 179)

int servoAngleMax = 45;    // Minimum angle for stepper (instead of 0)
int angleStep = 1;
int stepperMotorPosition = 0;
int stepperLowestLevel = -18000;  // lowest level of stepper motor originally at -22000
int stepperHighestLevel = 22000;  // highest level of stepper motor originally at 28000
int stepperMaxSpeed = 2500.0; // max speed of stepper motor

unsigned long startMillis;
unsigned long currentMillis;
unsigned long timeSetToOff;
const unsigned long period = 40;

// initialize the stepper library on pins 8 through 11:
AccelStepper myStepper = AccelStepper(motorInterfaceType, stepPin, dirPin);

void setup() {
  myStepper.setCurrentPosition(stepperLowestLevel);
  // set the speed at 30 rpm:
  //myStepper.setMaxSpeed(3000);       //set max speed the motor will turn (steps/second)
 // myStepper.setAcceleration(900);  //set acceleration (steps/second^2)

  // declare pin 5 to be an output:
  pinMode(led, OUTPUT);

  // initialize the serial port:
  Serial.begin(9600);          //  setup serial
  myservoRight.attach(servoPinRight);  // attaches the servo on pin 3 to the servo object
  myservoLeft.attach(servoPinLeft);  // attaches the servo on pin 6 to the servo object
  pinMode(pushButtonPin, INPUT_PULLUP);
  startMillis = millis();
  timeSetToOff = millis();

  myservoRight.write(angleRight);
  myservoLeft.write(angleLeft);
  //Serial.println("Servo Button ");
  //myStepper.moveTo(stepperLowestLevel);

}

void loop() {
  //myStepper.run();

  if (digitalRead(pushButtonPin) == LOW) {  //switch is ON

    currentMillis = millis();  //get the current time
    if (currentMillis - startMillis >= period)  //test whether the period has elapsed
    {
      analogWrite(led, brightness);    //set the brightness of the LED
      brightness = brightness + fadeAmount;
      if (brightness <= 0 || brightness >= 120) {
        fadeAmount = -fadeAmount;
      }
      startMillis = currentMillis;  //IMPORTANT to save the start time of the current LED brightness
      if ((currentMillis - timeSetToOff >= 3000) || (angleRight < 179)) { //add a bit of delay before the action
        if (angleLeft > servoAngleMax && angleLeft <= 180 ) {

          angleLeft = angleLeft - angleStep;

          if (angleLeft < servoAngleMax) {
            angleLeft = servoAngleMax;
          }

          else {
            myservoLeft.write(angleLeft); // move the left servo to desired angle: open doors
          }

        }

        //wait for Servo right to reach 140 degree
        if ((angleRight > servoAngleMax && angleRight <= 180) && (angleLeft <= 110 )) {

          angleRight = angleRight - angleStep;

          if (angleRight < servoAngleMax) {
            angleRight = servoAngleMax;
          }

          else {
            myservoRight.write(angleRight); // move the right servo to desired angle: open doors
          }
          //end of Servo Left logic
        }
      }
    }
    // wait for 20 milliseconds to see the dimming effect
    //delay(20);

    if ((angleLeft == servoAngleMax) && (angleRight == servoAngleMax)) { //if door is fully open raise platform
      myStepper.moveTo(stepperHighestLevel);
      //myStepper.runToPosition(stepperHighestLevel);
      myStepper.setMaxSpeed(stepperMaxSpeed);//set desired speed of stepper motor
      //myStepper.setSpeed(stepperMaxSpeed);
      myStepper.setAcceleration(900);
      myStepper.run();
      stepperMotorPosition = myStepper.currentPosition();

      //Serial.println("raising platform ");
      //Serial.println(stepperMotorPosition);

      //Serial.println(stepperMotorPosition);
    }

    //this part to switch off LEDs once doors are open
    if (stepperMotorPosition == stepperHighestLevel) {
      brightness = 0;
    }
    //end of turning OFF LEDs

    //delay(20); // waits for the servo to get there

  }


  if (digitalRead(pushButtonPin) == HIGH) { //switch is OFF
    timeSetToOff = millis();
    currentMillis = millis();
    // wait for 20 milliseconds to see the dimming effect
    //delay(20);

    myStepper.moveTo(stepperLowestLevel);
    myStepper.setMaxSpeed(stepperMaxSpeed);//set desired speed of stepper motor
    myStepper.setAcceleration(900);
    myStepper.run();
    stepperMotorPosition = myStepper.currentPosition();

    if (angleLeft >= 179 && angleRight >= 179) {
      analogWrite(led, 0); //begin LED dimming code
    }
    else {
      //get the current time
      //analogWrite(led, brightness); //begin LED dimming code
      if (currentMillis - startMillis >= period) {
        analogWrite(led, brightness); //begin LED dimming code
        // change the brightness for next time through the loop:
        brightness = brightness + fadeAmount;

        // reverse the direction of the fading at the ends of the fade:
        if (brightness <= 0 || brightness >= 120) {
          fadeAmount = -fadeAmount;
        }
        startMillis = currentMillis;

        if (stepperMotorPosition == stepperLowestLevel) {
          if (angleRight >= servoAngleMax && angleRight <= 180)  {
            angleRight = angleRight + angleStep;
            if (angleRight > 180) {
              angleRight = 179; //was set to 180 should be same as starting position
            }

            else {
              //myservoRight.write(angleRight); // move the Right servo to desired angle
              myservoRight.write(angleRight); // move the Right servo to desired angle
            }
          }

          //Wait for Left motor to move

          if ((angleLeft >= servoAngleMax && angleLeft <= 180) && (angleRight >= 100 )) {
            angleLeft = angleLeft + angleStep;
            if (angleLeft > 180) {
              angleLeft = 179; //was set to 180 should be same as starting position
            }

            else {
              myservoLeft.write(angleLeft); // move the Left servo to desired angle
              //myservoLeft.write(angleLeft); // move the Left servo to desired angle
            }
          }

        }

      }

      //Serial.println("lowering platform ");
      //Serial.println(stepperMotorPosition);

      //Serial.println("Switch is OFF");
      //Serial.println(stepperMotorPosition);

      // Servo button demo by Robojax.com


    }
    //delay(20); // waits for the servo to get there, to keep same speed this has to be the total
    //of dimming LED delay and Servo delay when switch is in ON position
  }


}