/*
* Athors: Oulis Evnagelos, Oulis Nikolaos, Katsibras Drosos
* Ultrasonic Sensor HC-SR04 and Arduino
* Dual Stepper Motor Shiled 1.1
* Subject: Autonomous Car Parking
*/

/*
 * Include all libraries.
 * 
*/

/*
 * Include the Arduino Servo Library
 */
#include <Servo.h>

/*
 * Include the Ping Ulptrasonic Library
 */
#include <NewPing.h>

/*
 * -------------------------------------------------------
*/

/*
 * All Definitions.
*/
#define ULTRASONIC_TIMEOUT 100000
#define SERVO_SPEED 1
#define MAX_DISTANCE 200
/*
 * Definitions for Motor Driver Pins.
*/
#define IN1 2
#define IN2 4
#define IN3 7
#define IN4 8
#define RIGHTSPEED 5
#define LEFTSPEED 3

#define MAX_WHEEL_SPEED 255
/*
 * Definitions for Ultrasonic Pins
*/
#define TRIGPIN 12
#define ECHOPIN 11
/*
 * Definitions for Servo Motor Pins.
*/
#define SERVOPIN 6

/*
 * -------------------------------------------------------
 * Define Servo Data Struct
 */
Servo myServo;

/*
 * Define Ultrasonic Struct
 */
NewPing sonar1 (TRIGPIN, ECHOPIN, MAX_DISTANCE);

/*
 * Define all useful variables
 */
long duration;
float distance, distanceCm;
int pos, dir;

/*
 * Turn Servo using custom Speed
 */
void turnServo(int from, int deg){
  int d;
  if (from > deg){
    for(d=from; d>=deg; d--){
      myServo.write(d);
      delay(SERVO_SPEED);
    }
  } else{
    for(d=from; d<=deg; d++){
      myServo.write(d);
      delay(SERVO_SPEED);
    }
  }
}

/*
 * Setup Wiring Components
 */
void setup() {
  //pinMode(TRIGPIN, OUTPUT); // Sets the TRIGPIN as an Output
  //pinMode(ECHOPIN, INPUT); // Sets the ECHOPIN as an Input
  
  //pinMode(response, OUTPUT); // Sets to led aoutput.
  
  pinMode(IN1, OUTPUT); //Motor Driver 1st Pin
  pinMode(IN2, OUTPUT); //Motor Driver 2nd Pin
  pinMode(IN3, OUTPUT); //Motor Driver 3rd Pin
  pinMode(IN4, OUTPUT); //Motor Driver 4th Pin
  
  pinMode(RIGHTSPEED, OUTPUT);
  pinMode(LEFTSPEED, OUTPUT);

  //Initialize Servo Motor
  myServo.attach(SERVOPIN);
  pos = 1;
  dir = 1;

  //Initialize car direction and make walk
  absolute(MAX_WHEEL_SPEED);
  forward();
  
  Serial.begin(9600); // Starts the serial communication
}

/*
 * Stepper Function for Servo.
 * 1st Step: straight
 * 2nd Step: left
 * 3rd Step: straight
 * 4th Step: right
 */
void next_step()
{
  if(pos == 1)
  { //Servo move to right
    turnServo(90, 30);
    pos = 2;
  }
  else if(pos ==2)
  { //Servo move to center
    turnServo(30, 90);
    pos = 3;
  }
  else if(pos == 3)
  { //Servo move to left
    turnServo(90, 150);
    pos = 4;
  }
  else if(pos == 4)
  { //Servo move to center
    turnServo(150, 90);
    pos = 1;
  }
}

/*
 * Distance meter via Ultrasonic
 */

float getDistance(){
  do{
    int uS1 = sonar1.ping();
    distanceCm = uS1 / US_ROUNDTRIP_CM;
    delayMicroseconds(2);
  } while(distanceCm == 0);

  return distanceCm;
}

/*
 * Left turn walk
 */
void turnLeft(const int wheelSpeed){
  analogWrite(LEFTSPEED, wheelSpeed);
  analogWrite(RIGHTSPEED, 0);
}

/*
 * Right turn walk
 */
void turnRight(const int wheelSpeed){
  analogWrite(LEFTSPEED, 0);
  analogWrite(RIGHTSPEED, wheelSpeed);
}

/*
 * Straight walk
 */
void absolute(const int wheelSpeed){
  analogWrite(LEFTSPEED, wheelSpeed);
  analogWrite(RIGHTSPEED, wheelSpeed);
}

/*
 * Backward walk
 */
void backward(){
  // Set Motor A backward
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  // Set Motor B backward
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

/*
 * Forward walk
 */
void forward(){
  // Set Motor A forward
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  // Set Motor B forward
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}

/*
 * Stop motors
 */
void motor_stop(){
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

void turn(const int wheelSpeed){
  if (pos == 1){
    absolute(wheelSpeed);
  } else if (pos == 2){
    turnLeft(wheelSpeed);
  } else if (pos == 3){
    absolute(wheelSpeed);
  } else if (pos == 4){
    turnRight(wheelSpeed);
  }
}

void moveCar(){
  if (pos == 3){
    if (dir == 1){
      turnRight(MAX_WHEEL_SPEED);
      backward();

      dir = 2;
    } else{
      turnLeft(MAX_WHEEL_SPEED);
      backward();

      dir = 1;
    }
    delay(1500);
    forward();
  } else{
    forward();
  }
}

void loop() {
  // Calculating the distance
  distance= getDistance();
  
  // Prints the distance on the Serial Monitor
  //Serial.print("Distance: ");
  //Serial.println(distance);

  if (distance <15) {
    // Change direction to motors
    turn(225);
  } else if (distance <13){
    turn(200);
  } else {
    // Change direction to motors
    turn(MAX_WHEEL_SPEED);
  }
  if (distance < 6){
    motor_stop();
    // Set Ultrasonic to next step
    next_step();
    // Move car
    moveCar();
  }
  delay(100);
}