/***
 * Description: Just for ...
 * Email: ntamvl@gmail.com
 ***/

#include <AFMotor.h>
#include <Servo.h>

#define TRIG_PIN A4
#define ECHO_PIN A5
#define SERVO_PIN 10
#define MAX_SPEED 150
#define MOTORS_CALIBRATION_OFFSET 3
#define MIN_DISTANCE 25
#define INIT_ANGLE 87

AF_DCMotor leftMotor(4, MOTOR12_8KHZ);
AF_DCMotor rightMotor(3, MOTOR12_8KHZ);
Servo _servo;

String motorSet = "";
int speedSet = 0;

void setup()
{
    Serial.begin(9600);
    pinMode(TRIG_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);
    _servo.attach(SERVO_PIN);
    _servo.write(INIT_ANGLE);
    delay(2000);
}

void loop()
{
    objectDistance();
    Serial.println(motorSet);
    doAutoCar();
}

void doAutoCar()
{
    int distance = 0;
    int distance_90 = 0;
    int maxLeftDistance = -1;
    int maxRightDistance = -1;

    moveForward();

    _servo.write(INIT_ANGLE);
    delay(200);

    distance = objectDistance();
    if (distance <= MIN_DISTANCE)
    {
        moveStop();
        delay(500);

        // begin detect
        for (int pos = 150; pos >= 30; pos -= 10)
        {
            _servo.write(pos);
            delay(90);
            distance = objectDistance();

            if (pos == INIT_ANGLE)
                distance_90 = distance;

            if (pos > INIT_ANGLE)
            {
                if (maxLeftDistance < distance)
                    maxLeftDistance = distance;
            }
            else
            {
                if (maxRightDistance < distance)
                    maxRightDistance = distance;
            }
        }

        for (int pos = 10; pos <= 150; pos += 10)
        {
            _servo.write(pos);
            delay(90);
            distance = objectDistance();

            if (pos > INIT_ANGLE)
            {
                Serial.println("On left");
                if (maxLeftDistance < distance)
                    maxLeftDistance = distance;
                Serial.print("maxLeftDistance: ");
                Serial.print(maxLeftDistance);
            }
            else
            {
                Serial.println("On right");
                if (maxRightDistance < distance)
                    maxRightDistance = distance;
                Serial.print("maxRightDistance: ");
                Serial.print(maxRightDistance);
            }
        }

        if (maxLeftDistance < maxRightDistance)
        {
            moveBackward();
            delay(500);

            Serial.println("Turn right");
            turnRight();
            delay(500);
            moveForward();
        }

        if (maxLeftDistance > maxRightDistance)
        {
            moveBackward();
            delay(500);

            Serial.println("Turn left");
            turnLeft();
            delay(500);
            moveForward();
        }

        if (maxLeftDistance == maxRightDistance)
        {
            Serial.println("Backward");
            moveBackward();
            delay(500);
        }

        delay(200);
        distance = objectDistance();
        if (distance <= MIN_DISTANCE)
        {
            moveBackward();
            delay(500);

            Serial.println("Turn right");
            turnRight();
            delay(500);
        }
        // end detect
    }
}

void moveStop()
{
    leftMotor.run(RELEASE);
    rightMotor.run(RELEASE);
}

void moveForward()
{
    motorSet = "FORWARD";
    leftMotor.run(FORWARD);
    rightMotor.run(FORWARD);
    leftMotor.setSpeed(MAX_SPEED);
    rightMotor.setSpeed(MAX_SPEED);
}

void moveBackward()
{
    motorSet = "BACKWARD";
    leftMotor.run(BACKWARD);
    rightMotor.run(BACKWARD);
    leftMotor.setSpeed(MAX_SPEED);
    rightMotor.setSpeed(MAX_SPEED);
}

void turnRight()
{
    motorSet = "RIGHT";
    leftMotor.run(FORWARD);
    rightMotor.run(RELEASE);
    leftMotor.setSpeed(MAX_SPEED);
    rightMotor.setSpeed(MAX_SPEED);
}

void turnLeft()
{
    motorSet = "LEFT";
    leftMotor.run(RELEASE);
    rightMotor.run(FORWARD);
    leftMotor.setSpeed(MAX_SPEED);
    rightMotor.setSpeed(MAX_SPEED);
}

void backwardRight()
{
    motorSet = "BACKWARD_RIGHT";
    leftMotor.run(BACKWARD);
    rightMotor.run(RELEASE);
    leftMotor.setSpeed(MAX_SPEED);
    rightMotor.setSpeed(MAX_SPEED);
}

void backwardLeft()
{
    motorSet = "BACKWARD_LEFT";
    leftMotor.run(RELEASE);
    rightMotor.run(BACKWARD);
    leftMotor.setSpeed(MAX_SPEED);
    rightMotor.setSpeed(MAX_SPEED);
}

int objectDistance()
{
    unsigned long duration;
    int distance;

    digitalWrite(TRIG_PIN, 0);
    delayMicroseconds(2);

    digitalWrite(TRIG_PIN, 1);
    delayMicroseconds(5);

    digitalWrite(TRIG_PIN, 0);

    duration = pulseIn(ECHO_PIN, HIGH);

    distance = int(duration / 2 / 29.412);

    Serial.print(distance);
    Serial.println(" cm");
    return distance;
}