#define INTERVAL 33
#define P_TURN 300 // en pour mille

#define PINDIR1 4
#define PINPWM1 5
#define PINDIR2 2
#define PINPWM2 3
#define PINTRIG 12
#define PINECHO 11



/* Constantes pour le timeout */
const unsigned long MEASURE_TIMEOUT = 25000UL; // 25ms = ~8m à 340m/s

typedef struct {
  bool far_l, far_r, close_l, close_r;
} IR_Capture;

typedef enum { INIT, RUN, LEFT, RIGHT, PAUSE, STOP } State ;
bool line_l, line_r; // ligne sur la gauche/droite (capteur infrarouge)
bool obstacle_f; // obstacle devant (capteur ultrason)
bool turn, turn_l; // y a une proba de tourner au cours de la course (marche aléatoire), et une proba pour savoir si on tourne à gauche ou droite
bool setting_up, fatal_error, success, start;
int speedL=0;
int speedR=0;
bool dirL = true;
bool dirR = true;
double prop =1.100;
bool led=false;
int vit=128;
float rand_turn_duration = 200;
int compteur_tour = 0;
const int distance_lim = 1500;

State state;

void setup() {
  // port série
  Serial.begin(115200);
    //on initialise les pins du moteur 1
  pinMode(PINDIR1, OUTPUT);
  pinMode(PINPWM1, OUTPUT);
  pinMode(PINDIR2, OUTPUT);
  pinMode(PINPWM2, OUTPUT);

  digitalWrite(PINDIR1,LOW);
  digitalWrite(PINDIR2,LOW);

  pinMode(PINTRIG, OUTPUT);
  digitalWrite(PINTRIG, LOW); // La broche TRIGGER doit être à LOW au repos
  pinMode(PINECHO, INPUT);

  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);

  state=INIT;
}

IR_Capture detect_IR()
{
  // capteurs infrarouges
  IR_Capture cap;

  return cap;
}

void detect_US()
{
  // capteurs ultrasons
  digitalWrite(PINTRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PINTRIG, LOW);
  long measure = pulseIn(PINECHO, HIGH, MEASURE_TIMEOUT);
  //Serial.println(measure);
  obstacle_f = (measure!=0 && measure<distance_lim);
}

void refresh()
{
  // met à jour les globals (conditions)
  detect_IR();
  detect_US();
  if (turn) {
    if(compteur_tour<int(rand_turn_duration/INTERVAL)) compteur_tour++;
    else {
      turn=false;
      compteur_tour=0;
    }
    Serial.println(compteur_tour);
  }
  else {
    turn = (random(1000)<P_TURN);
    turn_l = (random(2)==1); }
}

void transition()
{
  if (fatal_error || success)
    {
      state = STOP;
      return;
    }
  switch (state)
  {
    case INIT:
      if (setting_up) {
        state = INIT;
      }
      else {
        if (!obstacle_f && !line_l && !line_r && !turn) {
          state = RUN;
        }
        else if (turn && turn_l) {
          state = LEFT;
        }
        else if (turn && !turn_l) {
          state = RIGHT;
        } else
        {
          state = PAUSE;
        }
      }
      break;
    case RUN:
      if (!line_l && !line_r && !turn && !obstacle_f) {
        state = RUN;
      }
      else if (line_r || (!line_l && !line_r && turn && turn_l) && !obstacle_f) {
        state = LEFT;
      }
      else if (line_l || (!line_l && !line_r && turn && !turn_l) && !obstacle_f) {
        state = RIGHT;
      }
      else if (obstacle_f) {
        state = PAUSE;
      }
      else {
        state = STOP;
      }
      break;
    case LEFT:
      if (!line_l && !line_r && !obstacle_f) {
        state = RUN;
      }
      else if (line_l || line_r && !obstacle_f) {
        state = LEFT;
      }
      else if (obstacle_f) {
        state = PAUSE;
      }
      else {
        state = STOP;
      }
      break;
    case RIGHT:
      if (!line_l && !line_r && !obstacle_f) {
        state = RUN;
      }
      else if (line_l || line_r && !obstacle_f) {
        state = RIGHT;
      }
      else if (obstacle_f) {
        state = PAUSE;
      }
      else {
        state = STOP;
      }
      break;
    case PAUSE:
      if (!line_l && !line_r && !obstacle_f) {
        state = RUN;
      }
      else if (line_r && !obstacle_f) {
        state = LEFT;
      }
      else if (line_l && !obstacle_f) {
        state = RIGHT;
      }
      else if (obstacle_f) {
        state = PAUSE;
      }
      break;
    case STOP:
      if (start) {
        state = INIT;
      }
      else {
        state = STOP;
      }
      break;
    default:
      state = STOP;
      break;
  }

}

void loop() {
  // put your main code here, to run repeatedly:
  unsigned long t0=millis();
  refresh();
  transition();
  //vit=(vit+7)%255;
  //speedL=250;

  switch (state)
  {
    case INIT:
      Serial.println(F("INIT"));
      state=RUN;
      break;
    case RUN:
      Serial.println(F("RUN"));
      led=false;
      dirL=true;
      dirR=true;
      speedL=vit;
      speedR=vit;
      break;
    case LEFT:
      Serial.println(F("LEFT"));
      led=false;
      dirL=false;
      dirR=true;
      speedL=vit;
      speedR=vit;
      break;
    case RIGHT:
      Serial.println(F("RIGHT"));
      led=false;
      dirL=true;
      dirR=false;
      speedL=vit;
      speedR=vit;
      break;
    case PAUSE:
      Serial.println(F("PAUSE"));
      led=true;
      speedL=0;
      speedR=0;
      break;
    case STOP:
      Serial.println(F("STOP"));
      led=false;
      speedL=0;
      speedR=0;
      break;
    default:
      break;
  }

  digitalWrite(PINDIR1, (dirL) ? LOW : HIGH);
  digitalWrite(PINDIR1, (dirR) ? LOW : HIGH);
  analogWrite(PINPWM1,digitalRead(!PINDIR1 )?(int)(prop*speedL):(int)prop*(255-speedL));
  analogWrite(PINPWM2,digitalRead(!PINDIR2 )?speedR:255-speedR);
  digitalWrite(LED_BUILTIN, (led)?HIGH:LOW);

  unsigned long elapsed=millis()-t0;
  if (elapsed>INTERVAL) printf("Excessive execution time\n");
  else delay(INTERVAL-elapsed);

}