/**
 * Dados dos sensores e motores.
 */

/* Encoders. */
#define SENS_AZUL             2
#define SENS_VERM             3
#define SENS_BRACO            4
#define SENS_BAIXO            6
#define SENS_DIRECAO          5

#define SWITCH_AZUL_FRENTE     10
#define SWITCH_VERMELHO_FRENTE 9
#define SWITCH_AZUL_TRAS       12
#define SWITCH_VERMELHO_TRAS   13

#define SWITCH_CENTRAL_FRENTE   15

#define MOTOR_BRACO           3
#define MOTOR_AZUL            2
#define MOTOR_VERM            1

#define POTENCIA_MINIMA_AZUL  60
#define POTENCIA_MINIMA_VERM  60

#define POTENCIA_MAXIMA_AZUL  100
#define POTENCIA_MAXIMA_VERM  100

/* Potencia dos motores. */
int pot_verm = 75;
int pot_azul = 80;
int pot_braco = 100;

/* Uma variavel booleana para cada thread.
   Representa ativacao. */
int b_pid_correcao = 0;
int b_pid_monitora = 0;
int b_pid_braco = 0;

/**
 * Emergencia -- timer de 60s
 */
void Emergencia(){
    sleep( 60.0 );
    if( b_pid_correcao ) kill_process( pid_correcao );
    if( b_pid_monitora ) kill_process( pid_monitora ); 
    if( b_pid_braco ) kill_process( pid_braco );
    kill_process( pid_estrategia );
    ao();
    
    beep();
    beep();
    msleep(100L);
    beep();
    beep();
}

/**
 * Dados relacionados com Braco.
 */
#define LIMITE_SENS_BRACO_INF 112
#define LIMITE_SENS_BRACO_SUP 112

persistent int levantado = 1;
int n_fatias_braco = 0;
int pid_braco;

void Conta_giros_braco(){
    int cor_atual_braco;
    
    n_fatias_braco = 0;
    
    /* Ve onde o sensor esta inicialmente. */
    if( analog( SENS_BRACO ) > LIMITE_SENS_BRACO_SUP ){
        cor_atual_braco = PRETO;
    }else{
        cor_atual_braco = BRANCO;
    }
    
    while( 1 ){
        if( cor_atual_braco == BRANCO ){
            if( analog( SENS_BRACO ) > LIMITE_SENS_BRACO_SUP ){
                cor_atual_braco = PRETO;
                n_fatias_braco = n_fatias_braco + 1;
            }
        }else{
            if( analog( SENS_BRACO ) < LIMITE_SENS_BRACO_INF ){
                cor_atual_braco = BRANCO;
                n_fatias_braco = n_fatias_braco + 1;
            }
        }
        defer();
    }
}

#define N_FATIAS_BRACO 5
void Levanta_braco(void){
    if( levantado ) return;
    levantado = 1;
    b_pid_braco = 1;
    pid_braco = start_process( Conta_giros_braco() );
    motor( MOTOR_BRACO , -pot_braco );
    while( n_fatias_braco < N_FATIAS_BRACO );
    motor( MOTOR_BRACO , 0 );
    kill_process(pid_braco);
    b_pid_braco = 0;
    n_fatias_braco = 0;
}

void Levanta_parte_braco( int porcentagem ){
    if( porcentagem < 0 ) porcentagem = 0;
    if( porcentagem > 100 ) porcentagem = 100;
    
    b_pid_braco = 1;
    pid_braco = start_process( Conta_giros_braco() );
    motor( MOTOR_BRACO, -pot_braco );
    while( n_fatias_braco < ( N_FATIAS_BRACO * porcentagem )/100 );
    motor(MOTOR_BRACO, 0);
    kill_process(pid_braco);
    b_pid_braco = 0;    
    n_fatias_braco = 0;    
}



void Abaixa_braco(void){
    if( !levantado ) return;
    levantado = 0;    
    
    b_pid_braco = 1;
    pid_braco = start_process( Conta_giros_braco() );
    motor( MOTOR_BRACO, pot_braco );
    while( n_fatias_braco < N_FATIAS_BRACO );
    motor( MOTOR_BRACO , 0 );
    kill_process(pid_braco);
    b_pid_braco = 0;    
    n_fatias_braco = 0;    
}

void Abaixa_parte_braco( int porcentagem ){
    int n_voltas = ( N_FATIAS_BRACO * porcentagem )/100;
    
    if( porcentagem < 0 ) porcentagem = 0;
    if( porcentagem > 100 ) porcentagem = 100;
    
    if (n_voltas == 0) n_voltas = 1;
    
    b_pid_braco = 1;    
    pid_braco = start_process( Conta_giros_braco() );
    motor( MOTOR_BRACO, pot_braco );
    while( n_fatias_braco <  n_voltas );
    motor(MOTOR_BRACO, 0);
    kill_process(pid_braco);
    b_pid_braco = 0;        
    n_fatias_braco = 0;    
}

/**
 * Metodo para medir o valor dos sensores analogicos.
 */
void Medir_sensores_an( void ){
    int sensor = SENS_AZUL;
    
    while ( !stop_button() ){
        if( start_button() ){
            while( start_button() );
            if( sensor == SENS_AZUL ) sensor = SENS_VERM;
            else if ( sensor == SENS_VERM ) sensor = SENS_BRACO;
            else if ( sensor == SENS_BRACO ) sensor = SENS_BAIXO;
            else if ( sensor == SENS_BAIXO ) sensor = SENS_DIRECAO;
            else if ( sensor == SENS_DIRECAO ) sensor = SENS_AZUL;
            
        }
        
        printf("Lendo sensor: %d   %d\n", sensor , analog(sensor) );
        msleep(200L);
    }
    while( stop_button() );
}


/**
 * Metodo para medir o valor dos sensores digitais.
 */
void Medir_sensores_di( void ){
    int sensor = SWITCH_AZUL_FRENTE;
    
    while ( !stop_button() ){
        if( start_button() ){
            while( start_button() );
            if( sensor == SWITCH_AZUL_FRENTE ) sensor = SWITCH_VERMELHO_FRENTE;
            else if ( sensor == SWITCH_VERMELHO_FRENTE ) sensor = SWITCH_AZUL_TRAS;
            else if ( sensor == SWITCH_AZUL_TRAS ) sensor = SWITCH_VERMELHO_TRAS;
            else if ( sensor == SWITCH_VERMELHO_TRAS ) sensor = SWITCH_CENTRAL_FRENTE;                
            else sensor = SWITCH_AZUL_FRENTE;
        }
        printf("Lendo sensor: %d   %d\n", sensor , digital(sensor) );
        msleep(200L);
    }
    while( stop_button() );
}

/**
 * Metodos que monitoram o numero de giros da roda.
 */
/* Dados de sensores. */
int pid_correcao , pid_monitora;    

#define LIMITE_SENS_VERM_INF 115
#define LIMITE_SENS_VERM_SUP 115
#define LIMITE_SENS_AZUL_INF 125
#define LIMITE_SENS_AZUL_SUP 125

#define BRANCO 1
#define PRETO 0

int n_fatias_azul_ant = 0;
int n_fatias_azul = 0;

int n_fatias_verm_ant = 0;
int n_fatias_verm = 0;

int Modulo( int a ){
    if ( a < 0 ) return -a;
    else return a;
}

int Round ( float f ) {
    return (int) ( f + 0.5 );
}

void Monitora_sensores( void ){
    int cor_atual_azul;
    int cor_atual_verm;
    
    n_fatias_azul = 0;
    n_fatias_verm = 0;
    n_fatias_azul_ant = 0;
    n_fatias_verm_ant = 0;
    
    /* Ve onde o sensor esta inicialmente. */
    if( analog( SENS_VERM ) > LIMITE_SENS_VERM_SUP ){
        cor_atual_verm = PRETO;
    }else{
        cor_atual_verm = BRANCO;
    }
    
    if( analog( SENS_AZUL ) > LIMITE_SENS_AZUL_SUP ){
        cor_atual_azul = PRETO;
    }else{
        cor_atual_azul = BRANCO;
    }
    
    while( 1 ){
        n_fatias_verm_ant = n_fatias_verm;
        n_fatias_azul_ant = n_fatias_azul;
        /* Monitora sensor vermelho. */
        if( cor_atual_verm == BRANCO ){
            if( analog( SENS_VERM ) > LIMITE_SENS_VERM_SUP ){
                cor_atual_verm = PRETO;
                n_fatias_verm = n_fatias_verm + 1;
            }
        }else{
            if( analog( SENS_VERM ) < LIMITE_SENS_VERM_INF ){
                cor_atual_verm = BRANCO;
                n_fatias_verm = n_fatias_verm + 1;
            }
        }
        
        /* Monitora sensor azul. */
        if( cor_atual_azul == BRANCO ){
            if( analog(SENS_AZUL) > LIMITE_SENS_AZUL_SUP ){
                cor_atual_azul = PRETO;
                n_fatias_azul = n_fatias_azul + 1;
            }
        }else{
            if( analog(SENS_AZUL) < LIMITE_SENS_AZUL_INF ){
                cor_atual_azul = BRANCO;
                n_fatias_azul = n_fatias_azul + 1;
            }
        }
        defer();
    }
}
int fazendo_curva = 0;
int pot_verm_corrigido = pot_verm;
int pot_azul_corrigido = pot_azul;
int quem_desvia_mais = 0; /* Armazena quem desvia mais, ou seja, se > 0 
                             AZUL desvia mais se < 0 VERM desvia mais. */

#define PGAIN 30
void Corrige_giros( void ){
    int dif;
    
    pot_verm_corrigido = pot_verm;
    pot_azul_corrigido = pot_azul;    
    
    while( 1 ){
        dif = n_fatias_azul - n_fatias_verm ;
        
        /* Azul rodou de mais. */
        if( dif > 0 ){
            pot_verm_corrigido = pot_verm + (PGAIN*dif);
            pot_azul_corrigido = pot_azul - (PGAIN*dif);
            quem_desvia_mais++;
            /* Verm rodou de mais. */            
        }else if( dif < 0 ){
            dif = -dif;
            pot_verm_corrigido = pot_verm - (PGAIN*dif);
            pot_azul_corrigido = pot_azul + (PGAIN*dif);
            quem_desvia_mais--;
        }else{
            pot_verm_corrigido = pot_verm;
            pot_azul_corrigido = pot_azul;
        }
        /*
        if( 0 && !fazendo_curva ){
            if( quem_desvia_mais < 0 ){
                pot_verm_corrigido += PGAIN;
            }else{
                pot_azul_corrigido += PGAIN;                
            }
        }*/
        
        if( pot_azul_corrigido < POTENCIA_MINIMA_AZUL ){
            pot_azul_corrigido = POTENCIA_MINIMA_AZUL;
        }else if( pot_azul_corrigido > POTENCIA_MAXIMA_AZUL ){
            pot_azul_corrigido = POTENCIA_MAXIMA_AZUL;
        }
        
        if( pot_verm_corrigido < POTENCIA_MINIMA_VERM ){
            pot_verm_corrigido = POTENCIA_MINIMA_VERM;
        }else if( pot_verm_corrigido > POTENCIA_MAXIMA_VERM ){
            pot_verm_corrigido = POTENCIA_MAXIMA_VERM;
        }
        
        msleep( 50L );
    }
}



/**
 * Metodo para andar reto um numero de centimetros.
 * Para frente ou para tras. Ateh bater ou nao.
 */
#define PI 3.141592654
#define RAIO 4.9 /* cm */
#define CM_POR_FATIA 0.85714
#define TIME_OUT 400L
void Andar_reto( int distancia , int ate_bater ){
    int inversao = 1;
    int n_fatias_inicial_verm;
    int n_fatias_inicial_azul;
    int motor_azul_parado = 0;
    int motor_verm_parado = 0;
    int n_fatias;
    long segundos = 0L;
    
    fazendo_curva = 0;
    
    if (distancia < 0){
        distancia = -1*distancia;
        inversao = -1;
    }
    printf("Andando reto por %d cm.\n", distancia );    
    
    n_fatias = Round( ( ((float)distancia)/CM_POR_FATIA ) );
    
    b_pid_monitora = 1;
    b_pid_correcao = 1;
    
    pid_monitora = start_process( Monitora_sensores() );
    pid_correcao = start_process( Corrige_giros() );
    
    defer();
    
    n_fatias_inicial_verm = n_fatias_verm;
    n_fatias_inicial_azul = n_fatias_azul;
    
    motor( MOTOR_VERM , inversao * pot_verm_corrigido );
    motor( MOTOR_AZUL , inversao * pot_azul_corrigido );
    
    while( !motor_azul_parado || !motor_verm_parado ){
        
        /* Se eh para esperar ateh bater. */
        if( ate_bater ){
            /* Se vou dar re. */
            if( inversao == -1 ){
                if( !motor_verm_parado && digital( SWITCH_VERMELHO_TRAS ) ){
                    motor( MOTOR_VERM , 0 );
                    motor_verm_parado = 1;        
                    segundos = mseconds();
                }
                
                if( !motor_azul_parado && digital( SWITCH_AZUL_TRAS ) ){
                    motor( MOTOR_AZUL , 0 );
                    motor_azul_parado = 1;
                    segundos = mseconds();                    
                }
                /* Se estou andando para frente. */
            }else{
                if( !motor_verm_parado && digital( SWITCH_VERMELHO_FRENTE ) ){
                    motor( MOTOR_VERM , 0 );
                    motor_verm_parado = 1;                
                    segundos = mseconds();                    
                }
                
                if( !motor_azul_parado && digital( SWITCH_AZUL_FRENTE ) ){
                    motor( MOTOR_AZUL , 0 );
                    motor_azul_parado = 1;                
                    segundos = mseconds();                    
                }
                
                if ( digital(SWITCH_CENTRAL_FRENTE)){
                    motor( MOTOR_VERM , 0 );
                    motor_verm_parado = 1; 
                    motor( MOTOR_AZUL , 0 );
                    motor_azul_parado = 1;   
                }
            }
        }
        
        if( !motor_verm_parado && (n_fatias_verm - n_fatias_inicial_verm) > n_fatias ){
            motor( MOTOR_VERM , 0 );
            motor_verm_parado = 1;
        }
        if( !motor_azul_parado && (n_fatias_azul - n_fatias_inicial_azul) > n_fatias ){
            motor( MOTOR_AZUL , 0 );
            motor_azul_parado = 1;
        }
        
        if(!motor_verm_parado ) motor( MOTOR_VERM , inversao * pot_verm_corrigido );
        if(!motor_azul_parado ) motor( MOTOR_AZUL , inversao * pot_azul_corrigido );
        
        if( segundos != 0L && (mseconds() - segundos) > TIME_OUT ){
            motor( MOTOR_VERM , 0 );
            motor_verm_parado = 1; 
            motor( MOTOR_AZUL , 0 );
            motor_azul_parado = 1;             
        }
        
        defer();
    }
    
    kill_process( pid_correcao );
    kill_process( pid_monitora );
    b_pid_monitora = 0;
    b_pid_correcao = 0;    
}

/**
 * Metodos e dados do Wave Front.
 */
#define N_FATIAS_P_90_GRAUS 13
void Girar_esquerda( void ){
    int motor_azul_parado = 0;
    int motor_verm_parado = 0;
    int n_fatias_inicial_verm ;
    int n_fatias_inicial_azul ;
    
    fazendo_curva = 1;
    
    b_pid_monitora = 1;
    b_pid_correcao = 1;        
    pid_monitora = start_process( Monitora_sensores() );
    pid_correcao = start_process( Corrige_giros() );
    
    n_fatias_inicial_verm = 0;
    n_fatias_inicial_azul = 0;
    
    motor( MOTOR_VERM , -pot_verm_corrigido );
    motor( MOTOR_AZUL , +pot_azul_corrigido );
    
    while( !motor_azul_parado || !motor_verm_parado ){
        
        if( !motor_verm_parado && (n_fatias_verm - n_fatias_inicial_verm) > N_FATIAS_P_90_GRAUS ){
            motor( MOTOR_VERM , 0 );
            motor_verm_parado = 1;
        }
        
        if( !motor_azul_parado && (n_fatias_azul - n_fatias_inicial_azul) > N_FATIAS_P_90_GRAUS ){
            motor( MOTOR_AZUL , 0 );
            motor_azul_parado = 1;
        }
        
        if(!motor_verm_parado ) motor( MOTOR_VERM , -pot_verm_corrigido );
        if(!motor_azul_parado ) motor( MOTOR_AZUL , +pot_azul_corrigido );
        
        defer();
    }
    kill_process( pid_correcao );
    kill_process( pid_monitora );
    b_pid_monitora = 0;
    b_pid_correcao = 0;        
}

void Girar_direita( void ){
    int n_fatias_inicial_verm ;
    int n_fatias_inicial_azul ;
    int motor_azul_parado = 0;
    int motor_verm_parado = 0;
    
    fazendo_curva = 1;    
    
    b_pid_monitora = 1;
    b_pid_correcao = 1;        
    pid_monitora = start_process( Monitora_sensores() );
    pid_correcao = start_process( Corrige_giros() );
    
    n_fatias_inicial_verm = 0;
    n_fatias_inicial_azul = 0;    
    
    motor( MOTOR_VERM , +pot_verm_corrigido );
    motor( MOTOR_AZUL , -pot_azul_corrigido );
    
    while( !motor_azul_parado || !motor_verm_parado ){
        
        if( !motor_verm_parado && (n_fatias_verm - n_fatias_inicial_verm) > N_FATIAS_P_90_GRAUS ){
            motor( MOTOR_VERM , 0 );
            motor_verm_parado = 1;
        }
        
        if( !motor_azul_parado && (n_fatias_azul - n_fatias_inicial_azul) > N_FATIAS_P_90_GRAUS ){
            motor( MOTOR_AZUL , 0 );        
            motor_azul_parado = 1;
        }
        
        if(!motor_verm_parado ) motor( MOTOR_VERM , +pot_verm_corrigido );
        if(!motor_azul_parado ) motor( MOTOR_AZUL , -pot_azul_corrigido );
        defer();
    }
    kill_process( pid_correcao );
    kill_process( pid_monitora );
    b_pid_monitora = 0;
    b_pid_correcao = 0;        
}

/*  Relacao a luz. */
persistent int luz_orientacao = 30;
void Calibrar_iluminacao(void){
    while (!start_button()){
        luz_orientacao = analog(SENS_DIRECAO);
        printf("Direcao: %d \n", luz_orientacao );
        msleep(100L);
    }
    msleep(500L);
}

void Espera_start(){
    printf("Esperando comeco!\n");
    while (analog(SENS_BAIXO)>15){
        msleep(100L);
    }
}

/* Se orienta com sensor do LDR */
#define N_FATIAS_ORIENTA 1
void Orienta(){
    int n_medicoes = 5;
    int valor_lido = 0;
    int i;
    int motor_azul_parado = 0;
    int motor_verm_parado = 0;
    int n_fatias_inicial_verm ;
    int n_fatias_inicial_azul ;
    int n_giros = 0;
    
    fazendo_curva = 1;
    
    b_pid_monitora = 1;
    b_pid_correcao = 1;        
    pid_monitora = start_process( Monitora_sensores() );
    pid_correcao = start_process( Corrige_giros() );
    
    while( 1 ){
        
        valor_lido = 0;
        for (i=0; i < n_medicoes; i++){
            valor_lido = valor_lido + analog(SENS_DIRECAO);
            msleep(25L);
        }
        valor_lido = valor_lido / n_medicoes;
        
        if( Modulo( valor_lido - luz_orientacao) < 45 ) break;
        printf("Orientando %d\n" , valor_lido );        
        
        n_fatias_inicial_verm = 0;
        n_fatias_inicial_azul = 0;
        
        motor( MOTOR_VERM , -pot_verm_corrigido );
        motor( MOTOR_AZUL , +pot_azul_corrigido );
        n_giros += N_FATIAS_ORIENTA;
        
        while( !motor_azul_parado || !motor_verm_parado ){
            
            if( !motor_verm_parado && (n_fatias_verm - n_fatias_inicial_verm) > N_FATIAS_ORIENTA ){
                motor( MOTOR_VERM , 0 );
                motor_verm_parado = 1;
            }
            
            if( !motor_azul_parado && (n_fatias_azul - n_fatias_inicial_azul) > N_FATIAS_ORIENTA ){
                motor( MOTOR_AZUL , 0 );
                motor_azul_parado = 1;
            }
            
            if(!motor_verm_parado ) motor( MOTOR_VERM , -pot_verm_corrigido );
            if(!motor_azul_parado ) motor( MOTOR_AZUL , +pot_azul_corrigido );
            
            defer();
        }
    }
    
    kill_process( pid_correcao );
    kill_process( pid_monitora );
    b_pid_monitora = 0;
    b_pid_correcao = 0;
}