//-- Killer Robot v0.3

//--

//-- ULTRASONIC
#include <NewPing.h>

#define  TRIGGER_PIN  47
#define  ECHO_PIN_RIGHT  53
#define  ECHO_PIN_CENTER  51
#define  ECHO_PIN_LEFT 49

#define MAX_DISTANCE 300

NewPing sonarRight(TRIGGER_PIN, ECHO_PIN_RIGHT, MAX_DISTANCE);
NewPing sonarCenter(TRIGGER_PIN, ECHO_PIN_CENTER, MAX_DISTANCE);
NewPing sonarLeft(TRIGGER_PIN, ECHO_PIN_LEFT, MAX_DISTANCE);

int rangeRight = 0;
int rangeCenter = 0;
int rangeLeft = 0;

int rangeLedRight = 52;
int rangeLedCenter = 50;
int rangeLedLeft = 48;

int creepLed = 46;

int minDistanceSide = 35;
int prefDistanceSide = 50;
int minDistanceFront = 40;
int prefDistanceFront = 100;

long previousMillis = 0;
long sensorInterval = 50;

int sensorCycle = 0;

//-- MOTOR A --
int ENA=5; //Connected to Arudino Pin 5(pwm)
int IN1=2; //Connected to Arudino Pin 2
int IN2=3; //Connected to Arudino Pin 3

//-- MOTOR B --
int IN3=4; //Connected to Arudino Pin 4
int IN4=7; //Connected to Arudino Pin 7
int ENB=6; //Connected to Arudino Pin 6(pwm)

int creepSpeed = 140;

void setup()
{
  //Serial.begin(9600);

  pinMode(ENA,OUTPUT);
  pinMode(ENB,OUTPUT);
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  pinMode(IN3,OUTPUT);
  pinMode(IN4,OUTPUT);

  stopMotors();

  pinMode(rangeLedRight, OUTPUT);
  pinMode(rangeLedCenter, OUTPUT);
  pinMode(rangeLedLeft, OUTPUT);
  pinMode(creepLed, OUTPUT);

  rangeSweep(); // Do a full distance sweep without moving
}

void loop()
{
  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis > sensorInterval)
  {
    previousMillis = currentMillis;
    findRange();
  }
  if (minDistanceSide <= rangeRight && minDistanceSide <= rangeLeft && minDistanceFront <= rangeCenter)
  {
    driveForward();
  }
  else if (minDistanceSide >= rangeRight && prefDistanceSide <= rangeLeft)
  {
    turnLeft();
  }
  else if (prefDistanceSide <= rangeRight && minDistanceSide >= rangeLeft)
  {
    turnRight();
  }
  else
  {
    turnAround();
  }
}

void findRange()
{
  if (sensorCycle == 0) // can't poll all 3 sensors at the same time, interferance issues, thus spaced 50ms apart
  {
    rangeLeft = sonarLeft.ping_cm();
    sensorCycle++;
  }
  else if (sensorCycle == 1)
  {
    rangeCenter = sonarCenter.ping_cm();
    sensorCycle++;
  }
  else if (sensorCycle >= 2) // should be impossible for it to reach 3, but hey
  {
    rangeRight = sonarRight.ping_cm();
    sensorCycle = 0;
  }

  // "0" is no signal, means 3m+, lowest distance it can measure is 2cm
  if (rangeLeft == 0)
    rangeLeft = 300;
  if (rangeRight == 0)
    rangeRight = 300;
  if (rangeCenter == 0)
    rangeCenter = 300;

  if (minDistanceSide >= rangeLeft)
    digitalWrite(rangeLedLeft, HIGH);
  else
    digitalWrite(rangeLedLeft, LOW);

  if (minDistanceFront >= rangeCenter)
    digitalWrite(rangeLedCenter, HIGH);
  else
    digitalWrite(rangeLedCenter, LOW);

  if (minDistanceSide >= rangeRight)
    digitalWrite(rangeLedRight, HIGH);
  else
    digitalWrite(rangeLedRight, LOW);

  //Serial.print("L");
  //Serial.print(rangeLeft);
  //Serial.print(" C");
  //Serial.print(rangeCenter);
  //Serial.print(" R");
  //Serial.println(rangeRight);
}

void rangeSweep() // Do a full distance sweep without moving
{
  delay(50);
  findRange();
  delay(50);
  findRange();
  delay(50);
  findRange();
  delay(50);
}

void driveForward()
{

  motorsForward();

  int speedPot = analogRead(0) / 4;

  if (200 <= speedPot)
    speedPot = 255;
  else if (50 >= speedPot)
    speedPot = 0;

  // Deadzones to overcome math errors, motors don't move robot at too low engine speed anyway

  if (prefDistanceFront <= rangeCenter && prefDistanceSide <= rangeLeft && prefDistanceSide <= rangeRight && creepSpeed >= speedPot)
  {
    analogWrite(ENA, speedPot);
    analogWrite(ENB, speedPot);
    digitalWrite(creepLed, LOW);
  }
  else // Reduce speed if something is detected nearby
  {
    analogWrite(ENA, creepSpeed);
    analogWrite(ENB, creepSpeed);
    digitalWrite(creepLed, HIGH);
  }
}

void turnLeft()
{
  stopMotors();
  delay(500);
  motorsTurnLeft();
  runMotors();
  delay(500);
  stopMotors();
  delay(500);
  // Bought a compass module, will replace delays with actual compass comparison when it arrives
  rangeSweep();
}

void turnRight()
{
  stopMotors();
  delay(500);
  motorsTurnRight();
  runMotors();
  delay(500);
  stopMotors();
  delay(500);
  // Bought a compass module, will replace delays with actual compass comparison when it arrives
  rangeSweep();
}

void turnAround()
{
  stopMotors();
  delay(500);

  if (rangeLeft >= rangeRight)
    motorsTurnLeft();
  else
    motorsTurnRight();

  runMotors();
  delay(1000);
  stopMotors();
  delay(500);
  // Bought a compass module, will replace delays with actual compass comparison when it arrives
  rangeSweep();
}

void stopMotors()
{
  digitalWrite(ENA, LOW);
  digitalWrite(ENB, LOW);
}

void runMotors()
{
  digitalWrite(ENA, HIGH);
  digitalWrite(ENB, HIGH);
}

void motorsForward()
{
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH);
}

void motorsBackward()
{
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
  digitalWrite(IN3,HIGH);
  digitalWrite(IN4,LOW);
}

void motorsTurnLeft()
{
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  digitalWrite(IN3,HIGH);
  digitalWrite(IN4,LOW);
}

void motorsTurnRight()
{
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH);
}