#include <Servo.h>

//1
enum pinNum {pin1 = 1, pinInfraredProximitySensor_L, pinInfraredProximitySensor_R, pinUltraSonicSensor, pinServo, pinBackMotor_E, pinBuzzer, pinFrontMotor_L, pinFrontMotor_R, pin10=10, pinBackMotor_F, pinBackMotor_B};

//enum direction {left=-1, straight, right, backward=-1, stop, forward, slow=0};
//enum direction {straight, left, right, stop=0, backward, forward, slow=0};pin10=10
enum direction {dirLeft, dirMiddle, dirRight, dirBackward = 0, dirStay, dirForward};
enum speed {spdFull = 255, spdHalf = 127, spdStop = 0};
enum distance {disDanger = 30, disYell = 70};
enum pitch {tnCheck = 250, tnReady = 320, tnYell = 700, tnScream = 850};
enum distanceFlag {dfSafe, dfYell, dfDanger};

int distanceFlagLog[3]; //D=Danger Y=Yell S=Safe
int checkSeq[4] = {dirMiddle, dirRight, dirMiddle, dirLeft};

const bool debug = true;
const int svAngleDefault = 90;
const int svAngleChange = 45;
const int serialPort = 9600;
int currentXdir = -1;
int currentYdir = -1;
int currentSpeed = -1;

Servo _servo;
//int svdir = dirLeft;

void setup() {
  //tone(pinBuzzer, tnReady, 500);
  if (debug) {
    Serial.begin(serialPort);
    Serial.println(); // cursor to home
    Serial.println();
    Serial.println();
    Serial.println();
    Serial.println("*******");
    Serial.println("*******");
  }
  _servo.attach(pinServo); //*************
  //pinMode(pinInfraredProximitySensor_L, INPUT);
  //pinMode(pinInfraredProximitySensor_R, INPUT);
  firstCheck();
  turnSV(dirMiddle);
  tone(pinBuzzer, tnReady, 500);
  debugPrint("0********", 0);
  delay(5000);
  // attachInterrupt(digitalPinToInterrupt(pinInfraredProximitySensor_L), blink, FALLING);
  // attachInterrupt(digitalPinToInterrupt(pinInfraredProximitySensor_R), blink, FALLING);
}

//44000102110895
void loop() {
  for (int i = 0; i < 4; i++) {
    turnSV(checkSeq[i]);
    smartCar();
    debugPrint("Check: ", i);
    if ( busyIRCheck(3, 300) == false) {
      logDistance(checkSeq[i]);
    }
    //delay(100000);
  }
}

void firstCheck() {
  for (int i = 0; i < 3; i++) {
    turnSV(i);
    delay(500);
    logDistance(i);
    tone(pinBuzzer, tnCheck, 150);
    debugPrint("FirstCheck: ", i);
  }
}

void smartCar() {
  int xdir = dirMiddle;
  int ydir = dirStay;
  int spd = spdStop;
  switch (distanceFlagLog[1]) {
    case dfYell:
      spd = spdHalf;
      ydir = dirForward;
      xdir = pickDirection(false);
      tone(pinBuzzer, tnYell, 500);
      debugPrint("smartCar Yell ", ydir);
      break;
    case dfDanger:
      spd = spdHalf;
      ydir = dirBackward;
      xdir = pickDirection(true);
      tone(pinBuzzer, tnScream, 500);
      debugPrint("smartCar Danger ", ydir);
      break;
    default:
      spd = spdFull;
      ydir = dirForward;
      debugPrint("smartCar Safe ", ydir);
  }
  if(currentXdir != xdir || currentYdir != ydir || currentSpeed != spd){
    debugPrint("smartCar Adjust ", 0);
    runMotors (xdir, ydir, spd);
  }
  currentXdir = xdir;
  currentYdir = ydir;
  currentSpeed = spd;
  return;
}

int pickDirection(bool mustTurn) {
  if (distanceFlagLog[0] > distanceFlagLog[2]) {
    return dirRight;
  } else if (distanceFlagLog[0] < distanceFlagLog[2]) {
    return dirLeft;
  }
  if (mustTurn) {
    return round((double)rand() / (double)RAND_MAX) * 2;
  }
  return 0;
}

/*void StillCheck(int dir) {
  turnSV(dir);
  delay(400);
  logDistance(dir);
  tone(pinBuzzer, tnReady, 150);
  debugPrint("Check: ", dir);
}*/

void logDistance(int direction) {
  int currentDistance = getDistance(pinUltraSonicSensor);
  debugPrint("Distance:", currentDistance);
  if (currentDistance <= disDanger) {
    distanceFlagLog[direction] = dfDanger;
  } else if (currentDistance <= disYell) {
    distanceFlagLog[direction] = dfYell;
  } else {
    distanceFlagLog[direction] = dfSafe;
  }
}

bool busyIRCheck(int counter, int waitTime) {
  int irL = HIGH;
  int irR = HIGH;
  for (int j = 0; j < counter; j++) {
    irL = getIRPS(pinInfraredProximitySensor_L);
    irR = getIRPS(pinInfraredProximitySensor_R);
    if (irL && irR) {
      distanceFlagLog[dirMiddle] = dfDanger;
      debugPrint("busyIRCheck Danger ", j);
      return true;
    } else if (irL == true && irR == false) {
      distanceFlagLog[dirRight] = dfDanger;
      debugPrint("busyIRCheck R Danger ", j);
      return true;
    } else if (irL == false && irR == true) {
      distanceFlagLog[dirLeft] = dfDanger;
      debugPrint("busyIRCheck L Danger ", j);
      return true;
    }
    debugPrint("busyIRCheck Safe ", j);
    delay(waitTime / counter);
  }
  debugPrint("busyIRCheck wait ", waitTime);
  return false;
}

//***************
//Motors
//***************
void runMotors (int xdir, int ydir, unsigned int spd) { //00 straight 01 left 10 right 0-255 speed

  digitalWrite(pinFrontMotor_L, xdir==0);
  digitalWrite(pinFrontMotor_R, xdir==2);
  debugPrint("Turn:", xdir);
  debugPrint("pinFrontMotor_L PIN:", pinFrontMotor_L);
  debugPrint("pinFrontMotor_R PIN:", pinFrontMotor_R);
  debugPrint("pinFrontMotor_L:", xdir==0);
  debugPrint("pinFrontMotor_R:", xdir==2);

  analogWrite(pinBackMotor_E, spd);
  digitalWrite(pinBackMotor_F, ydir==2);
  digitalWrite(pinBackMotor_B, ydir==0);
  debugPrint("pinBackMotor_E PIN:", pinBackMotor_E);
  debugPrint("pinBackMotor_F PIN:", pinBackMotor_F);
  debugPrint("pinBackMotor_B PIN:", pinBackMotor_B);
  debugPrint("pinBackMotor_F:", ydir==2);
  debugPrint("pinBackMotor_B:", ydir==0);
  debugPrint("Run:", ydir);
  debugPrint("Speed:", spd);
//delay(2000);
  //digitalWrite(backMotor_F, -1 * dir + abs(dir));
  //digitalWrite(backMotor_B, dir + abs(dir));
}


//***************
//Servo
//***************
void turnSV(int dir) {
  //int angle = SV_S_Angle + (dir & 2 - 1) * SV_T_Angle;
  int angle = svAngleDefault + (dir - 1) * svAngleChange;
  //turnSV(angle);
  _servo.write(angle);
  debugPrint("Servo turn:", dir);
  debugPrint("Angle:", angle);
}

//***************
//3Pin Ultrasonic Sensor
//***************
int getDistance(int pinNum) {
  long duration = 0;
  long distance = 0;
  pinMode(pinNum, OUTPUT);
  digitalWrite(pinNum, LOW);
  delayMicroseconds(2);
  digitalWrite(pinNum, HIGH);
  delayMicroseconds(10);
  digitalWrite(pinNum, LOW);
  pinMode(pinNum, INPUT);
  duration = pulseIn(pinNum, HIGH);
  if (duration <= 0) {
    distance = 99999;
  }
  distance = duration * 0.034 / 2;
  debugPrint("Duration:", duration);
  debugPrint("Distance:", distance);
  return (int) distance;
}

//***************
//Infrared Proximity Sensor
//***************
bool getIRPS(int pinNum) {
  pinMode(pinNum, INPUT);
  if (digitalRead(pinNum) == LOW) {
    debugPrint("IRPS ALARM ", pinNum);
    return true;
  }
  debugPrint("IRPS OK ", pinNum);
  return false;
}

//***************
//Debug
//***************
void debugPrint(String msg, int data) {
  msg += data;
  if (debug) {
    Serial.println(msg);
  }
}

//***************
//ActiveBuzzer
//***************
/*void activeBuzz(int pinNum, unsigned duriation, unsigned int highDuration, unsigned int lowDuration) {
  if (debug){
    Serial.println("BUZZ " + pinNum + " " + duriation + " " + highDuration + " " + lowDuration);
  }
  if(abs(lowDuration) + abs(highDuration) ==0 ){
    return;
  }
  int cycle = round(abs(duriation) / (abs(lowDuration) + abs(highDuration)));
  if (debug){
    Serial.println("BUZZ Cycle " + cycle);
  }
  if(cycle == 0){
    return;
  }
  int i = 0;
  for(i=0;i<cycle;i++) {
    digitalWrite(pinNum,HIGH);
    delay(highDuration);
    digitalWrite(pinNum,LOW);
    delay(lowDuration);
  }
  }*/