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

/*
 * 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 6
#define MAX_DISTANCE 200
/*
 * -------------------------------------------------------
*/

/*
 * Define Ultrasonic Pins
 */
const int trigPin = 12;
const int echoPin = 11;
/*
 * For Motor Driver Pis.
*/
const int in1 = 2;
const int in2 = 4;
const int in3 = 7;
const int in4 = 8;
const int rightSpeed = 5;
const int leftSpeed = 3;

/*
 * For Servo Motor Pins
*/
const int 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();
  forward();
  
  Serial.begin(9600); // Starts the serial communication
  
  // Clears the trigPin
  analogWrite(trigPin, LOW);
  delayMicroseconds(2);
}

/*
 * 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(int echoPin, int trigPin){
  do{
    int uS1 = sonar1.ping();
    distanceCm = uS1 / US_ROUNDTRIP_CM;
  } while(distanceCm == 0);

  return distanceCm;
}

/*
 * Left turn walk
 */
void turnLeft(){
  analogWrite(leftSpeed, 255);
  analogWrite(rightSpeed, 0);
}

/*
 * Right turn walk
 */
void turnRight(){
  analogWrite(leftSpeed, 0);
  analogWrite(rightSpeed, 255);
}

/*
 * Straight walk
 */
void absolute(){
  analogWrite(leftSpeed, 255);
  analogWrite(rightSpeed, 255);
}

/*
 * 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(){
  if (pos == 1){
    absolute();
  } else if (pos == 2){
    turnLeft();
  } else if (pos == 3){
    absolute();
  } else if (pos == 4){
    turnRight();
  }
}

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

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

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

void loop() {
  // Calculating the distance
  distance= getDistance(echoPin, trigPin);
  
  // Prints the distance on the Serial Monitor
  Serial.print("Distance: ");
  Serial.println(distance);
  if (distance < 6){
    motor_stop();
    // Set Ultrasonic to next step
    next_step();
    // Change direction to motors
    turn();
    // Move car
    moveCar();
    delay(1000);
  }
  delay(100);
}