/*
* 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

/*Definition for turn time*/
#define TURN_TIME 2000
#define FORWARD_STEP 500

/*
 * -------------------------------------------------------
 * 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
 */
 /*Function that takes as arguments from->first position and deg->next position */
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 engine in sleep mode
  motor_stop();
  
  Serial.begin(9600); // Starts the serial communication
}


void serialFlush(){
  while(Serial.available() > 0) {
    char t = Serial.read();
    if(t == '\0')
      break;
  }
} 

/*
 * 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 park(int parkingNo) {
  int row = (parkingNo / 2);
  int side = (parkingNo % 2);  //If side is 1 then the parking is to right
                            // else if is 0 the parking is to left.
  
  absolute(255);
  forward();
  delay(row*FORWARD_STEP);
  motor_stop();
  
  //Check the side value to turn.
  if (side == 1) {
    turnRight(255);
  }
  else {
    turnLeft(255);
  }
  forward();
  delay(TURN_TIME);
  absolute(255);
  
  while(getDistance()>10){
    absolute(255);
    forward();
    delay(300);  
  }
  motor_stop();
}


int Serial_Input()
{
    int number;
    
    
    while(!Serial.available()) ;
    serialFlush();
    if (Serial.available() > 0) {
		number = Serial.parseInt();
		while (number <= 0) {
		  // convert the incoming byte to a char and add it to the string:
		  number = Serial.parseInt();
		  //Serial.print("Value:");
		  //Serial.println(number);
		  // clear the string for new input:
      serialFlush();
		}
		// if you get a newline, print the string, then the string's value:
    }
  
    return number;
}

void loop() {
  // Calculating the distance
  //distance= getDistance();
  
  // Prints the distance on the Serial Monitor
  //Serial.print("Distance: ");
  //Serial.println(distance);
  // Read serial input:
  park(Serial_Input());
  //delay(800);
//  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);
}