// Definicoes
#define COLOR_SENSOR 2

// Variaveis persistentes
// Representam as cores dos blocos
persistent int amarelo;
persistent int azul;
persistent int vermelho;
persistent int verde;

void CalibrarCores(){

  // Variaveis auxiliares
  int i;

  // Espera por um bloco amarelo
  printf ("\nBloco amarelo");
  while (!start_button());
  printf ("\nCalibrando..");
  amarelo = 0;
  for (i=0; i<50; i++){
      amarelo = amarelo + analog(COLOR_SENSOR);
      sleep(0.05);
  }
  amarelo = (int) amarelo / 50;
  beep();
  printf ("\nBloco amarelo:  %d", amarelo);
  sleep(1.5);

  // Espera por um bloco azul
  printf ("\nBloco azul");
  while (!start_button());
  printf ("\nCalibrando..");
  azul = 0;
  for (i=0; i<50; i++){
      azul = azul + analog(COLOR_SENSOR);
      sleep(0.05);
  }
  azul = azul/50;
  beep();
  printf ("\nBloco azul:     %d", azul);
  sleep(1.5);

  // Espera por um bloco vermelho
  printf ("\nBloco vermelho");
  while (!start_button());
  printf ("\nCalibrando..");
  vermelho = 0;
  for (i=0; i<50; i++){
      vermelho = vermelho + analog(COLOR_SENSOR);
      sleep(0.05);
  }
  vermelho = vermelho/50;
  beep();
  printf ("\nBloco vermelho: %d", vermelho);
  sleep(1.5);

  // Espera por um bloco verde
  printf ("\nBloco verde");
  while (!start_button());
  printf ("\nCalibrando..");
  verde = 0;
  for (i=0; i<50; i++){
      verde = verde + analog(COLOR_SENSOR);
      sleep(0.05);
  }
  verde = verde/50;
  beep();
  printf ("\nBloco verde:    %d", verde);
  sleep(1.5);

  beep();
  printf ("\nCores Calibradas");
  sleep(1.0);
  printf ("\nBloco amarelo:  %d", amarelo);
  sleep(1.5);
  printf ("\nBloco azul:     %d", azul);
  sleep(1.5);
  printf ("\nBloco vermelho: %d", vermelho);
  sleep(1.5);
  printf ("\nBloco verde:    %d", verde);
  sleep(1.5);

}

/*
 * Metodo para identificar a cor de um determinado bloco
 *
 */
void IdentificarCor(){

  // Variaveis auxiliares
  int tmp_cor, cor;
  int erro_atual, tmp_erro;
  int i;

  // Espera pelo bloco
  printf ("\nMostre o bloco");
  while (!start_button());

  // Calcula a cor
  printf ("\nCalculando..");
  tmp_cor = 0;
  for (i=0; i<50; i++){
      tmp_cor = tmp_cor + analog(COLOR_SENSOR);
      sleep(0.05);
  }
  tmp_cor = tmp_cor/50;

  printf ("\nValor: %d", tmp_cor);
  sleep(1.5);

  // Calcula a variacao para a cor amarela
  erro_atual = tmp_cor - amarelo;
  if (erro_atual < 0)
    erro_atual = erro_atual * (-1);
  // Define a cor do bloco como amarela
  cor = amarelo;

  // Calcula a variacao para a cor azul
  tmp_erro = tmp_cor - azul;
  if (tmp_erro < 0)
    tmp_erro = tmp_erro * (-1);
  // Testa se o erro eh menor
  if (tmp_erro < erro_atual){
      // Define a nova cor
      cor = azul;
      erro_atual = tmp_erro;
  }

  // Calcula a variacao para a cor vermelho
  tmp_erro = tmp_cor - vermelho;
  if (tmp_erro < 0)
    tmp_erro = tmp_erro * (-1);
  // Testa se o erro eh menor
  if (tmp_erro < erro_atual){
      // Define a nova cor
      cor = vermelho;
      erro_atual = tmp_erro;
  }

  // Calcula a variacao para a cor verde
  tmp_erro = tmp_cor - verde;
  if (tmp_erro < 0)
    tmp_erro = tmp_erro * (-1);
  // Testa se o erro eh menor
  if (tmp_erro < erro_atual)
    // Define a nova cor
    cor = verde;

  // Imprime qual eh a cor do bloco
  if (cor == amarelo){
      beep();
      printf ("\nCor: AMARELO");
  }
  else{
      if (cor == azul){
          beep();
          printf ("\nCor: AZUL");
      }
      else{
          if (cor == vermelho){
              beep();
              printf ("\nCor: VERMELHO");
          }
          else{
              beep();
              printf ("\nCor: VERDE");
          }
      }
  }
  sleep(1.5);

}