#include <AFMotor.h>
#include <NewPing.h>

enum Maneuver {
  LEFT_MVR,
  RIGHT_MVR
};

NewPing sonarL(37,39, 150);
NewPing sonarC(41,43, 150);
NewPing sonarR(40,42, 150);
NewPing sonarB(30,32, 150);
NewPing sonarSL(31,33, 150);
NewPing sonarSR(50,52, 150);

AF_DCMotor m_FL(4, MOTOR34_64KHZ);
AF_DCMotor m_FR(3, MOTOR34_64KHZ);
AF_DCMotor m_BL(1, MOTOR12_64KHZ);
AF_DCMotor m_BR(2, MOTOR12_64KHZ);

float distL, lastDistC, distC, distR, distSideR, distSideL, distB;
unsigned int balancer = 8; //количество циклов прямой езды
Maneuver lastManeuver; //последний манёвр

int direction = 0; //текущий курс относительно начального. Стремится к нулю

void ultrazvuk(){
  lastDistC=distC;

  distL = sonarL.ping_cm();
  if(distL==0) distL=150;
  distC = sonarC.ping_cm();
  if(distC==0) distC=150;
  distR = sonarR.ping_cm();
  if(distR==0) distR=150;
  distB = sonarB.ping_cm();
  if(distB==0) distB=150;
  distSideL = sonarSL.ping_cm();
  if(distSideL==0) distSideL=150;
  distSideR = sonarSR.ping_cm();
  if(distSideR==0) distSideR=150;
}

void dvig (int sFL, int sFR,int sBL,int sBR) {
  if(abs(sFL) > 250) {
    return;
  }
  if (sFL > 0){
    m_FL.run(FORWARD);
  }
  else {
    m_FL.run(BACKWARD);
    sFL=-sFL;
  }
  if (sFR > 0){
    m_FR.run(BACKWARD);
  }
  else {
    m_FR.run(FORWARD);
    sFR=-sFR;
  }
  if (sBL > 0){
    m_BL.run(BACKWARD);
  }
  else {
    m_BL.run(FORWARD);
    sBL=-sBL;
  }
  if (sBR > 0){
    m_BR.run(FORWARD);
  }
  else {
    m_BR.run(BACKWARD);
    sBR=-sBR;
  }
  m_FL.setSpeed(sFL);
  m_FR.setSpeed(sFR);
  m_BL.setSpeed(sBL);
  m_BR.setSpeed(sBR);
}

//Можно ли совершить определённый манёвр. Во избежание неконтролируемых поворотов, нельзя менять направление поворота до прямолинейной траектории
bool canPerfomManeuver(Maneuver maneuver) {
  if(maneuver==LEFT_MVR){
    return lastManeuver!=LEFT_MVR && balancer<8;
  }
  else{
    return lastManeuver!=RIGHT_MVR && balancer<8;
  }
}

//Движение вперёд с поправкой скорости при приближении к препятствию
void straight() {
  //Средняя скорость
  int avgSpeed = 115 - 1000/distC;
  if(distC<20 && lastDistC>distC) {
    Serial.println(min((lastDistC-distC)*3,30));
    avgSpeed-=min((lastDistC-distC)*3,30);
  }

  int distSide = min(distSideL,distSideR);
  if(distSide<6){ //Разные скорости по бокам, если боковое препятствие близко
    int diff = 50/distSide;
    if(distSideL<distSideR) {
      dvig(avgSpeed+diff,avgSpeed-diff,avgSpeed+diff,avgSpeed-diff);
    }
    else {
      dvig(avgSpeed-diff,avgSpeed+diff,avgSpeed-diff,avgSpeed+diff);
    }
  }
  else{
    dvig(avgSpeed,avgSpeed,avgSpeed,avgSpeed);
  }
  balancer++;
}

//Поворот на определённое количество градусов. Положительное направление - по часовой стрелке
void turn(int degree){
  if(degree<0) {
    dvig(-150,150,-150,150);
    direction--;
  }
  else {
    dvig(150,-150,150,-150);
    direction++;
  }
  balancer=0;
  delay(4.2*abs(degree));
}

void m_speed (){
  if(distL > 7 && distC > 7 && distR > 7) { //Путь свободен
    if((distL > 15 || distSideL > 15) && direction>0 && canPerfomManeuver(LEFT_MVR)){ //Возвращение направления при перекосе вправо
      turn(-20);
    }
    else if((distR > 15 || distSideR > 15) && direction<0 && canPerfomManeuver(RIGHT_MVR)) { //Возвращение направления при перекосе влево
      turn(20);
    }
    else { //Движение вперёд, медленно приближаясь к препятствию
      straight();
    }
  }
  else if(distSideL > 15 || distSideR > 15) { //Поворот
    if(canPerfomManeuver(LEFT_MVR)){
      if(distSideL>=distSideR || !canPerfomManeuver(RIGHT_MVR)){
        turn(-20);
      }
      else{
        turn(20);
      }
    }
    else if (canPerfomManeuver(RIGHT_MVR)){
      turn(20);
    }
  }
  else { //Остановка
    dvig(0,0,0,0);
  }
}

void setup() {
  ultrazvuk(); //для вычисление последнего расстояния по центральной оси
  Serial.begin(9600);
}

void loop() {
  ultrazvuk();
  straight();
  //m_speed();
  delay(50);
}