//Included for the Adafruit 8x8 LED Backpack
#include <Wire.h>
#include <Adafruit_GFX.h>
#include "Adafruit_LEDBackpack.h"

Adafruit_8x8matrix matrix = Adafruit_8x8matrix();

//Sensor 1 define the pins
const int trigPin = 9;
const int echoPin = 10;

//Sensor 2 define the pins
const int trigPin2 = 11;
const int echoPin2 = 12;

//Sensor 3 define the pins
const int trigPin3 = 7;
const int echoPin3 = 8;

// define the variables

//Sensor 1
long duration;
int distance;
int counter1;

//Sensor 2
long duration2;
int distance2;
int counter2;

//Sesnor 3
long duration3;
int distance3;
int counter5;

//Counter for both sensors when distance equals
int counter3;

//Counter for loop
int counter4;

void setup() {
  // put your setup code here, to run once:
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);

  Serial.begin(9600);

  //Adafruit 8x8 LED Backpack address
  matrix.begin(0x70);

  //Initialize the counters
  counter1 = 0;
  counter2 = 0;
  counter3 = 1; //counter3 is incremented +2 to accommodate difficulties the bot is having with the center sensor which might be due to its proximity between the left and right sensors. It seems to work more reliably.
  counter4 = 0;
  counter5 = 0;

  //Set Adafruit 8x8 LED Backpack orientation
  matrix.setRotation(3);
}

//Bitmaps for eye movement
static const uint8_t PROGMEM
  eye_left[] =
  { B00111100,
    B01111110,
    B11111111,
    B00111111,
    B00111111,
    B11111111,
    B01111110,
    B00111100 },
  eye_center[] =
  { B00111100,
    B01111110,
    B11111111,
    B11100111,
    B11100111,
    B11111111,
    B01111110,
    B00111100 },
  eye_right[] =
  { B00111100,
    B01111110,
    B11111111,
    B11111100,
    B11111100,
    B11111111,
    B01111110,
    B00111100 };

static const uint8_t PROGMEM // Bitmaps are stored in program memory
annoyedBlinkImg[]=   {  // Eye animation frames
  {
    B10000001,         // Fully open annoyed eye
    B01100110,
    B00000000,
    B11111111,
    B11111111,
    B11111111,
    B01111110,
    B00111100       }
  ,
  {
    B10000001,
    B01100110,
    B00000000,
    B11111111,
    B11111111,
    B11111111,
    B01111110,
    B00000000        }
  ,
  {
    B10000001,
    B01100110,
    B00000000,
    B11111111,
    B11111111,
    B01111110,
    B00000000,
    B00000000       }
  ,
  {
    B10000001,
    B01100110,
    B00000000,
    B11111111,
    B01111110,
    B00000000,
    B00000000,
    B00000000        }
  ,
  {
    B10000001,         // Fully closed annoyed eye
    B01100110,
    B00000000,
    B10000001,
    B01111110,
    B00000000,
    B00000000,
    B00000000       }

};

void loop() {

//Increment the loop counter
counter4++;

//Sensor 1
//Clears the trigPin
digitalWrite(trigPin, LOW);
delayMicroseconds(2);

//Sets the trigPin on HIGH state for 10 microseconds
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);

//Reads the echoPin, returns the sound wave travel time in microseconds
duration = pulseIn(echoPin, HIGH);

//Sensor 2
digitalWrite(trigPin2, LOW);
delayMicroseconds(2);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
duration2 = pulseIn(echoPin2, HIGH);

//Sensor 3
digitalWrite(trigPin3, LOW);
delayMicroseconds(2);
digitalWrite(trigPin3, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin3, LOW);
duration3 = pulseIn(echoPin3, HIGH);

//Calculating the distance
distance = duration*0.034/2;
distance2 = duration2*0.034/2;
distance3 = duration3*0.034/2;

//Prints the distanct on the Serial Monitor
/*Serial.print("Distance: ");
Serial.println(distance);
Serial.print("Distance2:             ");
Serial.println(distance2);
*/

if (distance3 < distance && distance3 < distance2)
  {
  counter3++;
  /*Serial.print("MIDDLE");
  Serial.println();
  */
  }
if (distance < distance2)
  {
  counter1++;
  /*Serial.print("RIGHT");
  Serial.println();
  */
  }
if (distance > distance2)
  {
  counter2++;
  /*Serial.print("LEFT");
  Serial.println();
  */
  }
if (counter4 == 5)
  {
  if (counter1 > counter2 && counter1 > counter3)
    {
    Serial.print("RIGHT");
    Serial.println();
    matrix.clear();
    matrix.drawBitmap(0, 0, eye_right, 8, 8, LED_ON);
    matrix.writeDisplay();
    matrix.clear();
    matrix.drawBitmap(0, 0, annoyedBlinkImg, 8, 8, LED_ON);
    matrix.writeDisplay();
    //delay(500);
    }
  if (counter2 > counter1 && counter2 > counter3)
    {
    Serial.print("LEFT");
    Serial.println();
    matrix.clear();
    matrix.drawBitmap(0, 0, eye_left, 8, 8, LED_ON);
    matrix.writeDisplay();
    //delay(500);
    }
  if (counter3 > counter1 && counter3 > counter2)
    {
    Serial.print("MIDDLE");
    Serial.println();
    matrix.clear();
    matrix.drawBitmap(0, 0, eye_center, 8, 8, LED_ON);
    matrix.writeDisplay();
    //delay(500);
    }
    //Reset the counters
    counter4 = 0;
    counter3 = 1; //counter3 is incremented +2 to accommodate difficulties the bot is having with the center sensor which might be due to its proximity between the left and right sensors. It seems to work more reliably.
    counter2 = 0;
    counter1 = 0;
    //counter5 = 0;
  }
//delay(10);
}