// Definicoes
#define LEFT_SENSOR 5
#define RIGHT_SENSOR 6

#define LEFT_MOTOR 3
#define RIGHT_MOTOR 1

#define LINHA 1
#define CHAO 0

// Estados
#define INICIAL 0
#define ESQ_LINHA 1
#define DIR_LINHA 2
#define AMBOS_LINHA 3
#define NENHUM_LINHA 4

// Variaveis persistentes
// Representam as cores dos blocos
persistent int L_LINE_SETPOINT;
persistent int L_FLOOR_SETPOINT;
persistent int R_LINE_SETPOINT;
persistent int R_FLOOR_SETPOINT;
persistent int R_DIF;
persistent int L_DIF;

persistent int LEFT_POWER;
persistent int RIGHT_POWER;

// Estados dos sensores
int sensor_esq;
int sensor_dir;

void CalibrarLinha(){

  // Variaveis auxiliares
  int i;

  // Calibra o sensor esquerdo
  // Espera para ser colocado na linha preta
  printf ("\nSensor Esquerdo Linha preta");
  while (!start_button());
  printf ("\nCalibrando..");
  L_LINE_SETPOINT = 0;
  for (i=0; i<50; i++){
      L_LINE_SETPOINT = L_LINE_SETPOINT + analog(LEFT_SENSOR);
      sleep(0.05);
  }
  L_LINE_SETPOINT = (int) L_LINE_SETPOINT / 50;
  printf ("\nLeft Line: %d", L_LINE_SETPOINT);
  while (!start_button());
  sleep(0.5);
  beep();

  // Espera para ser colocado no chao
  printf ("\nSensor Esquerdo Chao");
  while (!start_button());
  printf ("\nCalibrando..");
  L_FLOOR_SETPOINT = 0;
  for (i=0; i<50; i++){
      L_FLOOR_SETPOINT = L_FLOOR_SETPOINT + analog(LEFT_SENSOR);
      sleep(0.05);
  }
  L_FLOOR_SETPOINT= L_FLOOR_SETPOINT/50;
  printf ("\nLeft Floor: %d", L_FLOOR_SETPOINT);
  while (!start_button());
  sleep(0.5);
  beep();

  // Calibra o sensor direito
  // Espera para ser colocado na linha preta
  printf ("\nSensor Direito  Linha preta");
  while (!start_button());
  printf ("\nCalibrando..");
  R_LINE_SETPOINT = 0;
  for (i=0; i<50; i++){
      R_LINE_SETPOINT = R_LINE_SETPOINT + analog(RIGHT_SENSOR);
      sleep(0.05);
  }
  R_LINE_SETPOINT = (int) R_LINE_SETPOINT / 50;
  printf ("\nRight Line: %d", R_LINE_SETPOINT);
  while (!start_button());
  sleep(0.5);
  beep();

  // Espera para ser colocado no chao
  printf ("\nSensor Direito  Chao");
  while (!start_button());
  printf ("\nCalibrando..");
  R_FLOOR_SETPOINT = 0;
  for (i=0; i<50; i++){
      R_FLOOR_SETPOINT = R_FLOOR_SETPOINT + analog(RIGHT_SENSOR);
      sleep(0.05);
  }
  R_FLOOR_SETPOINT = R_FLOOR_SETPOINT/50;
  printf ("\nRight Floor: %d", R_FLOOR_SETPOINT);
  beep();

  // Calcula as diferencas
  L_DIF = (L_LINE_SETPOINT - L_FLOOR_SETPOINT) / 2;
  R_DIF = (R_LINE_SETPOINT - R_FLOOR_SETPOINT) / 2;
}

void SeguirLinha(){

  int pid;

  // Espera precionar start antes de comecar
  sleep(0.5);
  printf ("\nSeguir Linha");
  while(!start_button());
  sleep(0.5);

  pid = start_process(Seguir());

  while(!stop_button());
  kill_process(pid);
  ao();

}

void Seguir(){

  // Variaveis auxiliares
  int estado_anterior = INICIAL;

  // Define as potencias iniciais
  LEFT_POWER = 15;
  RIGHT_POWER = 15;

  // Segue a linha ate o botao stop ser precionado
  while (1){

      // Pega o estado dos sensores
      GetPosicao();

      // Se somente o sensor esq tah nah linha
      if ((sensor_esq == LINHA) && (sensor_dir == CHAO)){
          printf ("\n ESQUERDO LINHA");
          // Vai para frente
          AndarReto();
          // Define o estado anterior
          estado_anterior = ESQ_LINHA;
      }
      else{
          // Se somente o sensor direito tah nah linha
          if ((sensor_esq == CHAO) && (sensor_dir == LINHA)){
              printf ("\n DIREITO LINHA");
              // Vai para frente
              AndarReto();
              // Define o estado anterior
              estado_anterior = DIR_LINHA;
          }
          else{
              // Se ambos estao na linha
              if ((sensor_esq == LINHA) && (sensor_dir == LINHA)){
                  printf ("\n AMBOS LINHA");
                  if (estado_anterior == DIR_LINHA){
                      while ((sensor_esq == LINHA) && (sensor_dir == LINHA) ){
                          // Vira para esquerda
                          VirarEsq();
                          // Pega os novos estados
                          GetPosicao();
                      }
                  }
                  else{
                      if (estado_anterior == ESQ_LINHA){
                          while ((sensor_esq == LINHA) && (sensor_dir == LINHA) ){
                              // Vira para direita
                              VirarDir();
                              // Pega os novos estados
                              GetPosicao();
                          }
                      }
                      else{
                          while ((sensor_esq == LINHA) && (sensor_dir == LINHA) ){
                              // Vira para direita
                              VirarDir();
                              // Pega os novos estados
                              GetPosicao();
                          }
                      }
                  }
                  estado_anterior = AMBOS_LINHA;
              }
              else{
                  printf ("\n NENHUM LINHA");
                  if (estado_anterior == DIR_LINHA){
                      // Vira para direita
                      VirarDir();
                      while ((sensor_esq == CHAO) && (sensor_dir == CHAO) ){
                          // Pega os novos estados
                          GetPosicao();
                      }
                  }
                  else{
                      if (estado_anterior == ESQ_LINHA){
                          // Vira para esquerda
                          VirarEsq();
                          while ((sensor_esq == CHAO) && (sensor_dir == CHAO) ){
                              // Pega os novos estados
                              GetPosicao();
                          }
                      }
                      else{
                          // Faz um espiral
                          motor (LEFT_MOTOR, LEFT_POWER*2);
                          motor (RIGHT_MOTOR, RIGHT_POWER);
                          while ((sensor_esq == CHAO) && (sensor_dir == CHAO) ){
                              // Pega os novos estados
                              GetPosicao();
                          }
                      }
                  }
                  estado_anterior = NENHUM_LINHA;
              }
          }
      }
  }
}

void PararMotores(){
  motor (LEFT_MOTOR, 0);
  motor (RIGHT_MOTOR, 0);
}

// Define os estados dos sensores
void GetPosicao(){

  // Define o estado do sensor direito
  if ((PosicaoDir()+(R_DIF/2)) > R_LINE_SETPOINT)
    sensor_dir = LINHA;
  else{
      if ((PosicaoDir()-(R_DIF/2)) < R_FLOOR_SETPOINT)
        sensor_dir = CHAO;
  }

  // Define o estado do sensor esquerdo
  if ((PosicaoEsq()+(L_DIF/2)) > L_LINE_SETPOINT)
    sensor_esq = LINHA;
  else{
      if ((PosicaoEsq()-(L_DIF/2)) < L_FLOOR_SETPOINT)
        sensor_esq = CHAO;
  }
}

// Calcula a posicao do sensor esquerdo
int PosicaoEsq(){
  int pos;
  pos = analog(LEFT_SENSOR);
  return pos;
}

// Calcula a posicao do sensor direito
int PosicaoDir(){
  int pos;
  pos = analog(RIGHT_SENSOR);
  return pos;
}

// Vira para direita
void VirarDir(){
  motor (LEFT_MOTOR, (int)((float)LEFT_POWER*1.5));
  motor (RIGHT_MOTOR, 0);
}

// Vira para esquerda
void VirarEsq(){
  motor (LEFT_MOTOR, 0);
  motor (RIGHT_MOTOR, (int)((float)RIGHT_POWER*1.5));
}

// Anda Retor
void AndarReto(){
  motor (LEFT_MOTOR, LEFT_POWER);
  motor (RIGHT_MOTOR, RIGHT_POWER);
}