/*
* Firmware for the ”2WD Ultrasonic Motor Robot Car Kit”
*
* Stephen A. Edwards
*
* Hardware configuration :
* A pair of DC motors driven by an L298N H bridge motor driver
* An HC−SR04 ultrasonic range sensor mounted atop a small hobby servo
*/
#include <Arduino.h>
#include <Servo.h>
// #ifdef VM_DEBUG_GDB
// #include "avr8-stub.h"
// #endif

#define SERIAL_DEBUG TRUE

Servo servo;
// Ultrasonic Module pins
const int trigPin = 13; // 10 microsecond high pulse causes chirp , wait 50 us
const int echoPin = 12; // Width of high pulse indicates distance
// Servo motor that aims ultrasonic sensor .
const int servoPin = 11; // PWM output for hobby servo
// Motor control pins : L298N H bridge
const int enAPin = 6; // Left motor PWM speed control
const int in1Pin = 7; // Left motor Direction 1
const int in2Pin = 5; // Left motor Direction 2
const int in3Pin = 4; // Right motor Direction 1
const int in4Pin = 2; // Right motor Direction 2
const int enBPin = 3; // Right motor PWM speed contro

enum Motor { LEFT, RIGHT };

struct distIndex {
  unsigned int dist;
  unsigned int index;
};

// Set motor speed: 255 full ahead, −255 full reverse , 0 stop
void go( enum Motor m, int speed)
{
digitalWrite (m == LEFT ? in1Pin : in3Pin , speed > 0 ? HIGH : LOW );
digitalWrite (m == LEFT ? in2Pin : in4Pin , speed <= 0 ? HIGH : LOW );
analogWrite(m == LEFT ? enAPin : enBPin, speed < 0 ? -speed : speed );
}

// Initial motor test :
// left motor forward then back
// right motor forward then back
void testMotors ()
{
#ifdef SERIAL_DEBUG
Serial.println("testMotors");
#endif          
static int speed[8] = { 128, 255, 128, 0 ,
-128, -255, -128, 0};
go(RIGHT, 0);
for (unsigned char i = 0 ; i < 8 ; i++)
go(LEFT, speed[i]), delay (200);
for (unsigned char i = 0 ; i < 8 ; i++)
go(RIGHT, speed[i]), delay (200);
}


// Read distance from the ultrasonic sensor , return distance in mm
//
// Speed of sound in dry air , 20C is 343 m/s
// pulseIn returns time in microseconds (10ˆ−6)
// 2d = p * 10ˆ−6 s * 343 m/s = p * 0.00343 m = p * 0.343 mm/us
unsigned int readDistance ()
{
#ifdef SERIAL_DEBUG  
// Serial.println("readDistance");
#endif  
digitalWrite ( trigPin , HIGH );
delayMicroseconds (10);
digitalWrite ( trigPin , LOW );
unsigned long period = pulseIn ( echoPin, HIGH );
return period * 343 / 2000;
}

#define NUMANGLES 6
//unsigned char sensorAngle[NUMANGLES] = { 60, 70, 80, 90, 100, 110, 120 };
unsigned char sensorAngle[NUMANGLES] = { 70, 80, 85, 95, 100, 110 };
unsigned int distance [NUMANGLES];
unsigned int nearDistance [NUMANGLES];
// Scan the area ahead by sweeping the ultrasonic sensor left and right
// and recording the distance observed. This takes a reading , then
// sends the servo to the next angle. Call repeatedly once every 50 ms or so.
struct distIndex readNextDistance ()
{
static unsigned char angleIndex = 0;
static signed char step = 1;

struct distIndex distIndex_instance;    
distance [angleIndex] = readDistance();
angleIndex += step ;
if (angleIndex == NUMANGLES - 1) step = -1;
else if (angleIndex == 0) step = 1;
servo.write ( sensorAngle[angleIndex] );
distIndex_instance.dist = distance [angleIndex];
distIndex_instance.index = angleIndex;
#ifdef SERIAL_DEBUG
Serial.println("  End readNextDistance " +  (String)distIndex_instance.index + " : " +  (String)distIndex_instance.dist);
#endif
return distIndex_instance;
}


// Initial configuration
//
// Configure the input and output pins
// Center the servo
// Turn off the motors
// Test the motors
// Scan the surroundings once
//
void setup () {
// #ifdef VM_DEBUG_GDB
// debug_init();
// #endif
#ifdef SERIAL_DEBUG
  Serial.begin(9600);
  Serial.println("setup");
#endif    
pinMode(trigPin , OUTPUT);
pinMode(echoPin, INPUT);
digitalWrite ( trigPin , LOW);
pinMode(enAPin, OUTPUT);
pinMode(in1Pin, OUTPUT);
pinMode(in2Pin, OUTPUT);
pinMode(in3Pin, OUTPUT);
pinMode(in4Pin, OUTPUT);
pinMode(enBPin, OUTPUT);
servo.attach ( servoPin );
servo.write (90);
go(LEFT, 0);
go(RIGHT, 0);
testMotors ();
// Scan the surroundings before starting
servo.write ( sensorAngle[0] );
delay (500);
for (unsigned char i = 0 ; i < NUMANGLES ; i ++) {
  readNextDistance ();
  delay (500);
}

}


// Main loop:
//
// Get the next sensor reading
// If anything appears to be too close , back up
// Otherwise, go forward
//
void loop () {
// #ifdef VM_DEBUG_GDB
// breakpoint();
// #endif    
#ifdef SERIAL_DEBUG
// Serial.println("loop");
#endif

// See if something is too close at any angle
// unsigned int tooCloseL = 0;
// unsigned int tooCloseR = 0;

unsigned int numCloseL = 0;
unsigned int numCloseR = 0;

struct distIndex distIndex_loop = readNextDistance ();
unsigned int lastDist = distIndex_loop.dist;

if (lastDist < 350) {

  // Something's nearby: Slow down
  go(LEFT, 0);
  go(RIGHT, 0);
  delay (500);

  // Scan the surroundings before choice new direction
  for (unsigned char y = 0 ; y < NUMANGLES*2 - 2 ; y ++) {
    distIndex_loop = readNextDistance ();
    nearDistance [ distIndex_loop.index ] = distIndex_loop.dist;

    delay (450);  // increase to debug
  }

  for (unsigned char y = 0 ; y < NUMANGLES ; y++) {
      if (nearDistance [ y ] < 350) {
        if (y < NUMANGLES/2) {
              numCloseL = numCloseL + nearDistance [ y ];
          }
          else { 
              numCloseR = numCloseR + nearDistance [ y ];            
          }                   
      }
  }

  #ifdef SERIAL_DEBUG
  // Serial.println(" // numCloseL/R " +  (String)numCloseL + " : " + (String)numCloseR);
  #endif  

  if ( (numCloseR > 0) or (numCloseL > 0) ) {
      if (numCloseR > numCloseL) {
          #ifdef SERIAL_DEBUG
          Serial.println("< Something's nearby: back up left  "  +  (String)numCloseL + " : " + (String)numCloseR);
          #endif  
          // Something's nearby: back up Left    
          go(LEFT, 0);
          go(RIGHT, -255);
          delay (500);         
      }
      else {
          #ifdef SERIAL_DEBUG
          Serial.println("> Something's nearby: back up rigth "  +  (String)numCloseL + " : " + (String)numCloseR);
          #endif  
          // Something's nearby: back up rigth
          go(LEFT, -255);
          go(RIGHT, 0);  
          delay (500);  
      }
      // Stop Turn
      go(LEFT, 0);
      go(RIGHT, 0);
      delay (500); 
      // Scan the surroundings before choice new direction
      //for (unsigned char y = 0 ; y < NUMANGLES ; y ++) {
      //  readNextDistance ();
      //  delay (150);
      //}   
  }
} 

  if ( (numCloseL == 0) and (numCloseR == 0) ) {
  // Nothing in our way: go forward

  #ifdef SERIAL_DEBUG
  Serial.println("^ Nothing in our way: go forward " );
  #endif  

  go(LEFT, 255);
  go(RIGHT, 237);
  }
  
// Check the next direction in 50 ms
delay (1500); // increase to debug
}