/* Pin Constants (editable) */

//Pins for the ultrasonic sensor
const int pingPin = 12;
const int echoPin = 11;

//Wheels
const int leftWheelEnable = 9;
const int leftWheelForward = 10;
const int leftWheelBackward = 8;
const int rightWheelEnable = 7;
const int rightWheelForward = 5;
const int rightWheelBackward = 6;

//White line detector
const int whiteSensor0 = A0;
const int whiteSensor1 = A1;
const int whiteSensor2 = A2;

//Pins for the binary distance sensors on the side
const int leftFrontSensor = 4;
const int leftBackSensor = 2;
const int rightFrontSensor = A3;
const int rightBackSensor = 3;

/* Structures */

enum State {
  SEARCHING,
  CHASING,
  ON_BORDER,
  AVOIDING
};

enum Direction {
  FORWARD,
  BACKWARD
};

/* Logic Constants */

//When sensor reads over this value, it interprets it as a white line (border)
const int whiteLineThreshold0 = 700;
const int whiteLineThreshold1 = 700;
const int whiteLineThreshold2 = 700;
//When ultrasonic sensor detects object closer than this value then it will interpret it as an enemy;
const int enemySightDistance = 10;

//Globals
State state = SEARCHING;
Direction direction = FORWARD;
unsigned long time;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);

  pinMode(leftWheelForward, OUTPUT);
  pinMode(leftWheelBackward, OUTPUT);
  pinMode(rightWheelForward, OUTPUT);
  pinMode(rightWheelBackward, OUTPUT);

  pinMode(leftWheelEnable, OUTPUT);
  pinMode(rightWheelEnable, OUTPUT);

  digitalWrite(leftWheelEnable, HIGH);
  digitalWrite(rightWheelEnable, HIGH);

  pinMode(pingPin, OUTPUT);
  pinMode(echoPin, INPUT);

  time = millis();
}

void loop() {

  //Update time
  unsigned long deltaTime = millis() - time;
  time = millis();

  int serialState = 0;
  if(Serial.available() > 0) {
     serialState = Serial.read();
  }

  //if(serialState == '1') { Do bluetooth (potem sie zrobi)

    if(!avoidBorder()) {
      if(!avoidEnemiesFromSide()) {
        searchAndChase();
      }
    }

  //}
}

void searchAndChase() {
  if(state == SEARCHING) {
      if(enemyInFront()) {
        state = CHASING;
        direction = FORWARD;
        move();
      }
      else {
        rotateLeft();
      }
    }
  else if(state == CHASING) {
    if(!enemyInFront()) {
      state = SEARCHING;
    }
    else {
      move();
    }
  }
}


bool avoidEnemiesFromSide() {
  if(enemyOnSide(true, true) || enemyOnSide(true, false) || enemyOnSide(false, false) || enemyOnSide(false, true)) {
    direction = BACKWARD;
    state = AVOIDING;
    move();
    return true;
  }
  else {
    if(state = AVOIDING) {
      state = SEARCHING;
    }
    return false;
  }
}

bool avoidBorder() {
  if(state == ON_BORDER) {
    if(onBorder()) {
      move();
      return true;
    }
    else {
      state = SEARCHING;
      return false;
    }
  }
  else {
    if(onBorder()) {
      state = ON_BORDER;
      switchDirection();
      move();
      return true;
    }
    else {
      return false;
    }
  }
}

void switchDirection() {
  direction = FORWARD ? BACKWARD : FORWARD;
}

void move() {
  if(direction == FORWARD) {
    forward();
  }
  else {
    backward();
  }
}

void rotateLeft() {
  digitalWrite(leftWheelForward, HIGH);
  digitalWrite(leftWheelBackward, LOW);
  digitalWrite(rightWheelForward, LOW);
  digitalWrite(rightWheelBackward, HIGH);
}

void rotateRight() {
  digitalWrite(leftWheelForward, HIGH);
  digitalWrite(leftWheelBackward, LOW);
  digitalWrite(rightWheelForward, HIGH);
  digitalWrite(rightWheelBackward, LOW);
}

void forward() {
  digitalWrite(leftWheelForward, HIGH);
  digitalWrite(leftWheelBackward, LOW);
  digitalWrite(rightWheelForward, HIGH);
  digitalWrite(rightWheelBackward, LOW);
}

void backward() {
  digitalWrite(leftWheelForward, LOW);
  digitalWrite(leftWheelBackward, HIGH);
  digitalWrite(rightWheelForward, LOW);
  digitalWrite(rightWheelBackward, HIGH);
}


//Reads distance to the closest object in front of a bot (in centimeters)
long readDistance() {
   digitalWrite(pingPin, LOW);
   delayMicroseconds(2);
   digitalWrite(pingPin, HIGH);
   delayMicroseconds(10);
   digitalWrite(pingPin, LOW);
   long duration = pulseIn(echoPin, HIGH);
   long cm = duration / 29 / 2;
   return cm;
}

bool enemyOnSide(bool left, bool front) {
  if(front) {
    if(left) {
      return digitalRead(leftFrontSensor) == LOW;
    }
    else {
      return analogRead(rightFrontSensor) < 300;
    }
  }
  else {
        if(left) {
      return digitalRead(leftBackSensor) == LOW;
    }
    else {
      return digitalRead(rightBackSensor) == LOW;
    }
  }
}

bool onBorder() {
  int value0 = analogRead(whiteSensor0);
  int value1 = analogRead(whiteSensor1);
  int value2 = analogRead(whiteSensor2);
  if(value0 < whiteLineThreshold0) {
    return true;
  }
  if(value1 < whiteLineThreshold1) {
    return true;
  }
  if(value2 < whiteLineThreshold2) {
    return true;
  }
  return false;
}

bool enemyInFront() {
  return readDistance() < enemySightDistance;
}