///////////////////////////////////////////////////////////////////////////////
// ROBÔ CONSTRUTOR DE DIQUES
//
///////////////////////////////////////////////////////////////////////////////
// MOVIMENTACAO NO RETICULADO
//     DETECÇÃO DE BRANCO (CHÃO) E PRETO (LINHA)
//     CORREÇÃO DE ALINHAMENTO COM A LINHA PRETA
//     ANDA X QUADRADOS
//     VIRA O ROBO PARA UMA DIREÇÃO E SENTIDO
//     VAI DA POSIÇÃO ATUAL (robo_i, robo_j) ATÉ A POSIÇÃO (obj_i, obj_j)
//                  TERMINANDO NA DIREÇÃO obj_d
// MANIPULAÇÃO DA GARRA COM SENSORIAMENTO DAS CORES DOS TUBOS
//      AGARRA UM TUBO E DEVOLVE A SUA COR UTILIZANDO O SENSOR DE LUZ LOCALIZADO NA GARRA
//      LARGA UM TUBO
//
// SENSORIAMENTO DO ALAGAMENTO
//      DETECTA O ALAGAMENTO DE UM QUADRADO
///////////////////////////////////////////////////////////////////////////////

void corrige ();
void andaQuadrado(int quantidade);
///////////////////////////////////////////////////////////////////////////////


///////////////////////////////////////////////////////////////////////////////
///DEFINES  (essas constantes deverão ser ajustadas por cada grupo)
///////////////////////////////////////////////////////////////////////////////

/* movimentação */

//reto
#define POT_FRENTE                  40
#define ANG_PASSA_FAIXA             -75 //precisa ser testado
#define ANG_VOLTA_MEIO              -47 //precisa ser testado
#define ANG_VOLTA_FAIXA             35  //precisa ser testado

//correção
#define POT_CORRIGE                 15  //precisa ser testado

//giro
#define POT_GIRO                    30  //precisa ser testado
#define ANG_GIRO                    210 //precisa ser testado

//duto
#define TEMPO_AGARRA                750 //precisa ser testado
#define POT_AGARRA                  40  //precisa ser testado
#define TEMPO_LARGA                 750 //precisa ser testado
#define POT_LARGA                   40  //precisa ser testado

/* limites de cor */
#define LIMITE_COR                  550 //precisa ser testado
#define LIMITE_PINO                 550 //precisa ser testado

/* limite de distância */
#define DIS_THRESHOLD               10 //precisa ser testado

/* sensores light */
#define PRETO                       0
#define BRANCO                      1

/* tabuleiro */ //para facilitar na hora de pensar na linha do robô
#define	A	1
#define	B	2
#define	C	3
#define	D	4
#define	E	5
#define	F	6
#define	G	7
#define	H	8
#define	I	9
#define	J	10

/* orientação */
#define NORTE   0
#define LESTE   1
#define SUL     2
#define OESTE   3

/* região */ //para a variável rodo_r
#define DIREITA   0
#define ESQUERDA  1

/* cor do duto */
#define VERDE     0
#define VERMELHO  1

///////////////////////////////////////////////////////////////////////////////
///VARIAVEIS GLOBAIS
///////////////////////////////////////////////////////////////////////////////

/* armazenam a cor que os sensores S1 e S2 (chão) estão lendo no momento
   essas variáveis são atualizadas pelas funções atualizaCor S1 e S2 */
int SENSOR_ESQ = BRANCO;
int SENSOR_DIR = BRANCO;

/* pra testes de multitarefas */
int acabou = false;

int robo_i; //linha atual do robo
int robo_j; //coluna atual do robo
int robo_d; //direcao atual do robo (NORTE, LESTE, SUL ou OESTE)
int robo_r; //regiao do robo (DIREITA OU ESQUERDA)

///////////////////////////////////////////////////////////////////////////////
// MOVIMENTACAO NO RETICULADO
//     DETECÇÃO DE BRANCO (CHÃO) E PRETO (LINHA)
//     CORREÇÃO DE ALINHAMENTO COM A LINHA PRETA
//     ANDA X QUADRADOS
//     VIRA O ROBO PARA UMA DIREÇÃO E SENTIDO
//     VAI DA POSIÇÃO ATUAL (robo_i, robo_j) ATÉ A POSIÇÃO (obj_i, obj_j)
//                  TERMINANDO NA DIREÇÃO obj_d


///////////////////////////////////////////////////////////////////////////////
/// DETECÇÃO DE BRANCO (CHÃO) E PRETO (LINHA)
///////////////////////////////////////////////////////////////////////////////

// Tratamento do sensor da esquerda.
// O sensor da esquerda está na linha preta se
// o valor lido for < LIMITE_COR
// caso contrário, o sensor da esquerda está na região branca.
task atualizaCoresS1()
{
  int leituraAtu;
  long sensor = S1;

  while(!acabou)
  {
    leituraAtu = Sensor(sensor);

    if(leituraAtu < LIMITE_COR) // ficou mais escuro: passou para o preto
    {
      SENSOR_ESQ = PRETO;
    }
    else if(leituraAtu >= LIMITE_COR) // passou para o branco
    {
      SENSOR_ESQ = BRANCO;
    }
  }
}


// Tratamento do sensor da direita.
// O sensor da direita está na linha preta se
// o valor lido for < LIMITE_COR
// caso contrário, o sensor da direita está na região branca.
task atualizaCoresS2()
{
  int leituraAtu;
  long sensor = S2;

  while(!acabou)
  {
    leituraAtu = Sensor(sensor);

    if(leituraAtu < LIMITE_COR) // ficou mais escuro: passou para o preto
    {
      SENSOR_DIR = PRETO;
    }
    else if(leituraAtu >= LIMITE_COR) // passou para o branco
    {
      SENSOR_DIR = BRANCO;
    }
  }
}

/////////////////////////////////////////////////////////////////////////////
// CORREÇÃO DE ALINHAMENTO NA LINHA PRETA
// O ROBÔ ANDA PARA FRENTE ENQUANTO ESTIVER NO BRANCO
// TRATAMENTO DE 3 CASOS:
//   CASO 1: O PRIMEIRO A CHEGAR NA LINHA PRETO É O SENSOR DA ESQUERDA
//   CASO 2: O PRIMEIRO A CHEGAR NA LINHA PRETA É O SENSOR DA DIREITA
//   CASO 3: O DOIS SENSORES CHEGARAM JUNTOS NA LINHA PRETA

void corrige()
{
 SENSOR_ESQ = BRANCO;
 SENSOR_DIR = BRANCO;

 OnRevReg(OUT_BC, POT_FRENTE, OUT_REGMODE_SYNC);
 while(SENSOR_ESQ == BRANCO && SENSOR_DIR == BRANCO){}
 Off(OUT_BC);
 //   CASO 1: O PRIMEIRO A CHEGAR NA LINHA PRETO É O SENSOR DA ESQUERDA
 if(SENSOR_ESQ == PRETO && SENSOR_DIR == BRANCO)
 {
  OnFwd(OUT_C, POT_CORRIGE);
  while(SENSOR_ESQ == PRETO){}
  Off(OUT_C);
  OnRev(OUT_B,POT_CORRIGE);
  while(SENSOR_DIR == BRANCO){}
  Off(OUT_B);
  OnFwd(OUT_B,POT_CORRIGE);
  while(SENSOR_DIR == PRETO){}
  Off(OUT_B);
 }
 //   CASO 2: O PRIMEIRO A CHEGAR NA LINHA PRETA É O SENSOR DA DIREITA
 else if(SENSOR_ESQ == BRANCO && SENSOR_DIR == PRETO)
  {
  OnFwd(OUT_B, POT_CORRIGE);
  while(SENSOR_DIR == PRETO){}
  Off(OUT_B);
  OnRev(OUT_C,POT_CORRIGE);
  while(SENSOR_ESQ == BRANCO){}
  Off(OUT_C);
  OnFwd(OUT_C,POT_CORRIGE);
  while(SENSOR_ESQ == PRETO){}
  Off(OUT_C);
 }
//   CASO 3: O DOIS SENSORES CHEGARAM JUNTOS NA LINHA PRETA (NÃO PRECISA DE CORREÇÃO)
}

///////////////////////////////////////////////////////////////////////////////
// ANDA X QUADRADOS
// Conta a quantidade de quadrados percorridos através das
// linhas pretas detectadas.

void andaQuadrado(int quantidade)
{
  int faixasPassadas = 0;

  if(quantidade == 0)
    return;

  while(faixasPassadas < quantidade)
  {
    corrige();

    RotateMotorEx(OUT_BC, POT_FRENTE, ANG_PASSA_FAIXA, 0, false, false);
    faixasPassadas++;
  }

  corrige();

  RotateMotorEx(OUT_BC, POT_FRENTE, ANG_VOLTA_FAIXA, 0, false, false);
}

///////////////////////////////////////////////////////////////////////////////
 //VIRA O ROBO PARA A ORIENTAÇÃO DESEJADA

void viraRobo(int direcao_final)
{
  int qtoVira90Graus, i;

  qtoVira90Graus = direcao_final - robo_d;

  //corrige rotacao do robo
  if(qtoVira90Graus == -3) qtoVira90Graus = 1;
  else if (qtoVira90Graus == 3) qtoVira90Graus = -1;

  for(i = 1; i <= abs(qtoVira90Graus); i++)
  {
     corrige();

     RotateMotorEx(OUT_BC, POT_FRENTE, ANG_VOLTA_MEIO, 0, false, false);
    
     if(qtoVira90Graus < 0) RotateMotorEx(OUT_BC, POT_GIRO, ANG_GIRO, 100, true, true);
     else RotateMotorEx(OUT_BC, POT_GIRO, ANG_GIRO, -100, true, true);
      
     RotateMotorEx(OUT_BC, POT_FRENTE, 2 * ANG_VOLTA_FAIXA, 0, true, true);

     corrige();

     RotateMotorEx(OUT_BC, POT_FRENTE, ANG_VOLTA_FAIXA, 0, false, false);
  }

  //atualiza a direção do robô
  robo_d = direcao_final;
}

///////////////////////////////////////////////////////////////////////////////
// VAI DA POSIÇÃO ATUAL (robo_i, robo_j) ATÉ A POSIÇÃO (obj_i, obj_j),
// TERMINANDO COM O ROBÔ ORIENTADO NA DIREÇÃO obj_d

void vai(int obj_i, int obj_j, int obj_d)
{
  int direcao_final;

  direcao_final = robo_d;
  
// ANDA NA LINHA
// DEFINE O SENTIDO DA LOCOMOÇÃO NA DIREÇÃO LESTE/OESTE

  if(robo_j < obj_j)
  {
    // A LOCOMOÇÃO DEVE SER PARA LESTE
    direcao_final = LESTE;
  }
  else if(robo_j > obj_j)
  {
     // A LOCOMOÇÃO DEVE SER PARA OESTE
    direcao_final = OESTE;
  }

// VIRA O ROBÔ PARA A DIREÇÃO E SENTIDO CORRETO DA LOCOMOÇÃO
  if(direcao_final != robo_d)
  {
    viraRobo(direcao_final);
  }
  // ANDA X QUADRADOS
  andaQuadrado(abs(obj_j - robo_j));

// ANDA NA COLUNA
// DEFINE O SENTIDO DA LOCOMOÇÃO NA DIREÇÃO NORTE/SUL

  if(robo_i > obj_i)
  {
    // A LOCOMOÇÃO DEVE SER PARA NORTE
    direcao_final = NORTE;
  }
  else if(robo_i < obj_i)
  {
    // A LOCOMOÇÃO DEVE SER PARA SUL
    direcao_final = SUL;
  }

  // VIRA O ROBÔ PARA A DIREÇÃO E SENTIDO CORRETO DA LOCOMOÇÃO
  if(direcao_final != robo_d)
  {
    viraRobo(direcao_final);
  }
  andaQuadrado(abs(obj_i - robo_i));

  // ATUALIZA A POSIÇÃO FINAL DO ROBÔ
  robo_i = obj_i;
  robo_j = obj_j;
  
  // DEPOIS DA LOCOMOÇÃO VIRA O ROBÔ PARA A ORIENTAÇÃO DESTINO //
  if(robo_d != obj_d)
  {
    viraRobo(obj_d);
  }
}

///////////////////////////////////////////////////////////////////////////////
// AGARRA O TUBO E DEVOLVE A SUA COR

int agarra()
{
   SetSensorType(S3, IN_TYPE_LIGHT_ACTIVE); //ativa o sensor de luz da garra
   OnFwd(OUT_A, 100); //abre a garra
   OnFwd(OUT_BC, POT_AGARRA); //gira os motores das rodas
   Wait(TEMPO_AGARRA); //tempo necessario para o robo abrir a garra e andar ate o duto
   Off(OUT_ABC);
   Wait(500); //tempo para estabilizar o robo antes de fazer a leitura do sensor de luz, evitando erros
   if(Sensor(S3)<LIMITE_PINO) return VERDE;
   else return VERMELHO;
}

///////////////////////////////////////////////////////////////////////////////
// LARGA O TUBO
void larga()
{
   SetSensorType(S3, IN_TYPE_LIGHT_INACTIVE); //desativa o sensor de luz da garra
   OnFwd(OUT_A, 100);
   OnRev(OUT_BC, POT_LARGA); //note que aqui é o inverso da função agarra (OnRev ao inves de OnFwd)
   Wait(TEMPO_LARGA);
   Off(OUT_ABC);
}

///////////////////////////////////////////////////////////////////////////////
// SENSORIAMENTO DO ALAGAMENTO
// (essa função você deverá implementar

void alagamento()
{
     SetSensorLowspeed(S4); //define o sensor de distância como sendo o S4
     
     ...
     
     //dica, dar um Wait() para estabilização do robô antes de fazer a leitura

     if(SensorUS(S4)<DIS_THRESHOLD) //tem alagamento
     
     ...
     
     else //não tem alagamento

     ...
     
}


///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
/// MAIN
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////

task teste()
{
   //use esta task para fazer teste de funções

   acabou = true;
}

task robo()
{
   //use esta task para colocar as funções que o robô realizará para resolver o desafio

   acabou = true;
}

task main()
{
   //não se esqueça de inicializar o que for necessário: sensores, posição inicial do robô, etc
   //obs: inicializar os sensores de luz no modo mais preciso:
   SetSensorType(S1, IN_TYPE_LIGHT_ACTIVE);
   SetSensorType(S2, IN_TYPE_LIGHT_ACTIVE);
   
   // recurso do NXT para realizar várias tasks em paralelo
   // com isso, podemos manter o robô constantemente sensoriando o reticulado,
   // enquanto realiza a sua missão
   Precedes(atualizaCoresS1, atualizaCoresS2, teste);
}