#include<Stepper.h>
#include<NewPing.h>
#include<Math.h>

//Position sensor1 = 0
//Position sensor2 = 305
//Position sensor3 = 610
//Position sensor4 = 908
//Position sensor5 = 1213

#define pinHe 0 //analog
#define pinPir 2
#define trigger 3
#define echo1 4
#define echo2 5
#define echo3 6
#define echo4 7
#define pinM1 8
#define pinM2 9
#define pinM3 10
#define pinM4 11
#define backLights 12
#define movingLights 13
#define maxD 200
#define pos1 0
#define pos2 305
#define pos3 610
#define pos4 908
#define pos5 1213

const int stepsPerRevolution = 200;
int statusPir = 0;
int valueHe = 0;
int dist1 = 0;
int dist2 = 0;
int dist3 = 0;
int dist4 = 0;
int currentPos; //Initial position moving Lights
int beforePos;
int degreesToMove = 0;
int i, j;

Stepper myMotor(stepsPerRevolution, pinM1, pinM2, pinM3, pinM4);
NewPing sonar1(trigger, echo1, maxD);
NewPing sonar2(trigger, echo2, maxD);
NewPing sonar3(trigger, echo3, maxD);
NewPing sonar4(trigger, echo4, maxD);

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  myMotor.setSpeed(30); //config Motor PAP Speed
  valueHe = analogRead(pinHe);
  while(valueHe > 605){
    myMotor.step(-1);
    valueHe = analogRead(pinHe);
  }
  currentPos = pos1;
}

void loop() {
  // put your main code here, to run repeatedly:
  delay(50);
  statusPir = digitalRead(pinPir);

  Serial.print("PIR = ");
  Serial.println(statusPir);
  
  while(statusPir == 1){
    digitalWrite(backLights, HIGH);
    digitalWrite(movingLights, HIGH);

    Serial.print("pos = ");
    Serial.print(currentPos);
    dist1 = sonar1.ping_cm();
    Serial.print("   s1 = ");
    Serial.print(dist1);
    dist2 = sonar2.ping_cm();
    Serial.print("   s2 = ");
    Serial.print(dist2);
    dist3 = sonar3.ping_cm();
    Serial.print("   s3 = ");
    Serial.print(dist3);
    dist4 = sonar4.ping_cm();
    Serial.print("   s4 = ");
    Serial.println(dist4);
    
    if((dist1 < 30) && (dist1 > 0) && (dist2 == 0) && (dist3 == 0) && (dist4 == 0)){
      beforePos = currentPos;
      currentPos = pos2;
      
      degreesToMove = currentPos - beforePos;

      if(degreesToMove > 0){
        for(i = 0; i < (degreesToMove + 1); i++){
          myMotor.step(1);
        }
      }
      else if(degreesToMove < 0){
        for(j = 0; j < (abs(degreesToMove) + 1); j++){
          myMotor.step(-1);
        }
      }
    }
    else if((dist2 < 30) && (dist2 > 0) && (dist1 == 0) && (dist3 == 0) && (dist4 == 0)){
      beforePos = currentPos;
      currentPos = pos3;

      degreesToMove = currentPos - beforePos;

      if(degreesToMove > 0){
        for(i = 0; i < (degreesToMove + 1); i++){
          myMotor.step(1);
        }
      }
      else if(degreesToMove < 0){
        for(j = 0; j < (abs(degreesToMove) + 1); j++){
          myMotor.step(-1);
        }
      }
    }
    else if((dist3 < 30) && (dist3 > 0) && (dist1 == 0) && (dist2 == 0) && (dist4 == 0)){
      beforePos = currentPos;
      currentPos = pos4;
      
      degreesToMove = currentPos - beforePos;

      if(degreesToMove > 0){
        for(i = 0; i < (degreesToMove + 1); i++){
          myMotor.step(1);
        }
      }
      else if(degreesToMove < 0){
        for(j = 0; j < (abs(degreesToMove) + 1); j++){
          myMotor.step(-1);
        }
      }
    }
    else if((dist4 < 30) && (dist4 > 0) && (dist1 == 0) && (dist2 == 0) && (dist3 == 0)){
      beforePos = currentPos;
      currentPos = pos5;
      
      degreesToMove = currentPos - beforePos;

      if(degreesToMove > 0){
        for(i = 0; i < (degreesToMove + 1); i++){
          myMotor.step(1);
        }
      }
      else if(degreesToMove < 0){
        for(j = 0; j < (abs(degreesToMove) + 1); j++){
          myMotor.step(-1);
        }
      }
    }
    delay(3000);
    statusPir = digitalRead(pinPir);
   
  }
  digitalWrite(backLights, LOW);
  digitalWrite(movingLights, LOW);
  delay(2000);
}
