#define Itens_Menu 5

float MAX = 100.0 ,MIN = -100.0, Kp = 7.0, Kd = 10.0;

float vel8 = 0.0, vel9 = 0.0;

int cont8 = 0;

int cont9 = 0;

float power8, power9;

float var30 = 30.0;

float var90g = 7.0;

int velm1 = 100, velm2 = 93;

float tempo1 = 9.0 , tempo2 = 9.0;

int a1 = 0, a2 = 0, a3 = 0, a4 = 0, a5 = 0, a6 = 0, a7 = 0,a8=0;

int pid1, pid2, pid3, pid4, pid5, pidm1, pidm2,pid6,pid7,pid8;

int Parede_D = 14, Parede_E=4,Luz_H=2,Luz_V=6,Luz_Partida=9;

persistent int ponto_escuro = 100;

persistent int ponto_claro = 100;

persistent int media_cor[5][3];

persistent int dv[5][3];

persistent int VparedeD, VparedeE,pertoD,pertoE,longeD,longeE,Luz_Escolha;

char Menu[Itens_Menu][40] = {"0- Inicia_Partida Derick", "1- Inicia_Partida2 Flavio", "2- flavio calibrando", "3- calibra tudo", "4- polar"};

char bloco_cor[6][10] = {"Preto", "Amarelo", "azul", "verde", "vermelho","Erro"};

void velo8(){
    int cont_ant = 0, te = 0;
    vel8 = 0.0;
    
    while(1){
        
        if(cont8 != cont_ant){
            vel8 = ((float)cont8 - (float)cont_ant)/((float)te*0.1);
            cont_ant = cont8;
            te = 1;
        }
        else { 
            te = te + 1;
            if(te == 5) vel8 = 0.0;
        }
        
        
        
        sleep(0.1);
    }
    
}

void velo9(){
    int cont_ant2 = 0, te2 = 0;
    vel9 = 0.0;
    if(cont9 != cont_ant2){
        vel9 = ((float)cont9 - (float)cont_ant2)/((float)te2*0.1);
        cont_ant2 = cont9;
        te2 = 1;
    }
    else { 
        te2 = te2 + 1;
        if(te2 == 5) vel9 = 0.0;
    }
    
}
void count8(){
    int est_ant = digital(8);
    int est_ant2 = digital(7);
    cont8 = 0;
    cont9 = 0;
    while(1){
        if (est_ant == 0 && digital(8) == 1){
            if(velm1>=0){
                cont8 = cont8 + 1;
            }
            else { 
                cont8 = cont8 - 1;
            }
        }
        est_ant = digital(8);
        
        if (est_ant2 == 0 && digital(7) == 1){
            if(velm2>=0){
                cont9 = cont9 + 1;
            }
            else { 
                cont9 = cont9 - 1;
            }
        }
        est_ant2 = digital(7);
        
        sleep(0.004);
    }
}



void start_menu(){
    int i = 0;
    int option;
    
    while(1){
        while((!start_button()) || (!stop_button())){    
            printf("%s\n",Menu[i]);
            msleep(100L);
            
            if (start_button()){
                executeItem(i);
                while (start_button());
            }
            if (stop_button()){
                i = (i + 1)% Itens_Menu;  
                while (stop_button());
            }
        }   
    }    
}


int rodar(float temp ,int vel, int mot){
    motor(mot,vel);
    sleep(temp);
    off(mot);
}
void stop(){
	motor(0,0);
	motor(2,0);
    off(0);
    off(2);
}

void executa_teste(float ref){
    
    int con;
    float eps = 0.2;
    while(!stop_button()){
        
        con = cont8;
        power8 = Kp*( ref - (float)con);
        if(power8 > MAX){
            power8= MAX;
        }
        else if(power8< MIN){
            power8= MIN;
        }
        
        motor(0,(int)(power8+eps));
        //printf("p = %f c = %d v = %f\n", power8, con, vel8);
    }
}

void executa_teste2(float ref){
    
    int con;
    while(!stop_button()){
        
        con = cont9;
        power9 = Kd*( ref - (float)con);
        if(power9 > MAX){
            power9= MAX;
        }
        else if(power9< MIN){
            power9= MIN;
        }
        motor(2,(int)power9);
    }
}

int executa_andar(float ref1, float ref2){
    
    int wall_D, cor;
    float t1, t2;
    cont8 = 0;
    cont9 = 0;
    executa_andar2(100);
    
    wall_D = digital(Parede_D);
    if (wall_D == 1){
        executa_braco2();
        wall_D= digital(Parede_D);
    }
    
    cont8 = 0;
    cont9 = 0;
    
    executa_andar2(-9);

        rodar(0.4, -70, 3);
        sleep(1.0);
        off(3);

    executa_andar2(-8);
	
    executa_braco3();
	executa_andar2(20);
    wall_D = digital(Parede_D);
    if (wall_D == 1){
        wall_D= digital(Parede_D);
    }
	executa_braco();
    cor=executa_cor();
	//se nao preto: pega, da re ate a base
	if(cor!=0){
		executa_andar2(-40);
		return 0;
	}
	else{
		executa_andar2(-8);
		executa_rodar(90);
		executa_andar2(8);
		executa_rodar(-90);
		return 1;
	}
	
    stop();
    
}

void executa_andar_old(){
    pidm1 = start_process(rodar(tempo1, velm1, 0));
    pidm2 = start_process(rodar(tempo2, velm2, 2));
    
    sleep(tempo1);
    kill_process(pidm1);
    off(0);
    kill_process(pidm2);
    off(2);
    
}

void executa_andar2(int ref){
    int wall_D;
    cont8 = 0;
    cont9 = 0;
    
    if (ref < 0){
        velm1 = -velm1;
        velm2 = -velm2;
        pidm1 = start_process(rodar(30.0, velm1, 0));
        pidm2 = start_process(rodar(30.0, velm2, 2));
        
    }
    else{
        pidm1 = start_process(rodar(30.0, velm1, 0));
        pidm2 = start_process(rodar(30.0, velm2, 2));
    }
    
    wall_D = digital(Parede_D);
    if (ref < 0){
        while(cont8 > ref){
        }    
    }
    else while(cont8 < ref && wall_D == 0){
        wall_D = digital(Parede_D);
    }
    
    motor(0,0);
    motor(2,0);
    kill_process(pidm1);
    off(0);
    kill_process(pidm2);
    off(2);
    if (ref <= 0){
        velm1 = -velm1;
        velm2 = -velm2;
    }
    
}

void execua_re(float tempo){
    pidm1 = start_process(rodar(tempo, -70, 0));
    pidm2 = start_process(rodar(tempo, -70, 2));
    sleep(tempo);
    kill_process(pidm1);
    off(0);
    kill_process(pidm2);
    off(2);
}

void executa_rodar(int graus){
    float temp1;
    if(graus==90||graus==-90){
        temp1=2.3;
    }else{
        if(graus==15){
            temp1=0.3; 
        }else{
            temp1=4.5;
        }
        
        
    }
    if(graus>0){
    pidm1 = start_process(rodar(temp1, -velm1, 0));
    pidm2 = start_process(rodar(temp1, velm2, 2));
    }else{
	pidm1 = start_process(rodar(temp1, velm1, 0));
    pidm2 = start_process(rodar(temp1, -velm2, 2));
	}
	
    sleep(temp1);
    kill_process(pidm1);
    off(0);
    kill_process(pidm2);
    off(2);
    
}

//Calibra linha escura e clara//

void calibra_linha(){
    int new;
    int cont=0;
    int soma=0;
    while(cont<10) {
        new = sensor_linha();
        printf("Linha escura=%d\n",new);
        sleep(0.2);
        soma =soma+new;
        sleep(0.2);
        cont++;
    }
    ponto_escuro=soma/10;
    printf("Linha escura=%d\n",ponto_escuro);
    sleep(1.0);
    printf("Start para ponto branco");
    while(!start_button()){
        
    }
    cont=0;
    soma=0;
    while(cont<10) {
        new = sensor_linha();
        printf("Linha clara=%d\n",new);
        sleep(0.2);
        soma =soma+new;
        sleep(0.2);
        cont++;
    }
    ponto_claro=soma/10;
    printf("Linha clara=%d\n", ponto_claro);
    sleep(1.0);
    
}

void calibra_cor(){
    int new;
    int soma=0;
    int i, j, k, maior = 0, menor = 256;
    poke(0x1009, 0x3c);
    for (i = 0; i < 5;i ++){
        sleep(0.5);
        printf("Start para %s\n",bloco_cor[i]);
        while(!start_button()){
        }
        for (j = 0; j < 3; j ++){
            soma = 0;
            for (k = 0; k < 2; k ++){
                acende_led(j);
                sleep(0.2);
                new = sensor_cor();
                if (new > maior) maior = new;
                if (new < menor) menor = new;
                printf("%s = %d\n", bloco_cor[i], new);
                soma = soma + new;
                sleep(0.1);
            }
            apaga_led(j);
            media_cor[i][j] = soma/2;
            dv[i][j] = maior- menor;
        }
    }
}

int executa_cor(){
    int i,j,k,numCor=5;
    int cor[3];
    int soma,new;
    printf("Executando leitura\n");
    sleep(0.5);
    for (j = 0; j < 3; j ++){
        soma = 0;
        for (k = 0; k < 3; k ++){
            acende_led(j);
            sleep(0.2);
            new = sensor_cor();
            soma = soma + new;
            sleep(0.1);
        } 
        apaga_led(j);
        cor[j]=soma/3;
    }
    for(i=0;i<5;i++){
        if(cor[0]>=(media_cor[i][0]-4)&&cor[0]<=(media_cor[i][0]+4)&&cor[1]>=(media_cor[i][1]-4)&&cor[1]<=(media_cor[i][1]+4)&&cor[2]>=(media_cor[i][2]-4)&&cor[2]<=(media_cor[i][2]+4)){
            numCor=i;
        }
    }
    
    printf("Cor= %s\n", bloco_cor[numCor]);
    sleep(0.5);
	if(numCor==5){
      return(0);
	}else{
	  return(numCor);	
	}
	
}

//Executa andar na linha//

void executa_linha(){
    while(1) {
        andar_direita();
        na_linha();
        fora_linha();
        para_motor();
        sleep(0.1);
        andar_esquerda();
        na_linha();
        fora_linha();
    }
    
}

//motor direita: 2 motor esquerda:0//

void andar_reto(){
    motor(0,95);
    motor(2,95);
}

void andar_esquerda(){
    //motor(0,0);
    //motor(2,90);
    //sleep(0.5);
    motor(0,30);
    motor(2,90);
}


void andar_direita(){
    //motor(0,80);
    //motor(2,0);
    //sleep(0.4);
    motor(0,75);
    motor(2,20);
}


void para_motor(){
    motor(0,0);
    motor(2,0);
}

//Calibra Parede//

void calibra_parede(){
    
    int wall= analog(Parede_E);
    
    sleep(1.0);
    while(!start_button()){
        wall= analog(Parede_E);
        printf("Start para Parede_E= %d\n",wall);
        sleep(0.5);
    }
    printf("Parede_E= %d\n",wall);
    sleep(1.0);
    VparedeE= wall; 
    beep(); 
    
    sleep(1.0); /* give a pause for the display */
    
    
    wall= digital(Parede_D);
    
    while(!start_button()){
        wall= digital(Parede_D);
        printf("Start para Parede_D= %d\n",wall);
        sleep(0.5);
    }
    printf("Parede_D= %d\n",wall);
    sleep(1.0);
    VparedeD= wall; 
    beep();
    printf("Fim da calibragem\n");
    sleep(1.0); /* give a pause for the display */
}


void calibra_paredeGentle(){
    int wall= analog(Parede_E);
    
    while(!start_button()){
        wall= analog(Parede_E);
        printf("Start para Parede_E perto= %d\n",wall);
        sleep(0.5);
    }
    pertoE= wall;
    printf("pertoE %d\n", pertoE); 
    beep(); 
    sleep(1.0);
    
    
    
    while(!start_button()){
        wall= analog(Parede_E);
        printf("Start para Parede_E longe= %d\n",wall);
    }
    longeE= wall;
    printf("longeE %d\n", longeE); 
    beep(); 
    sleep(1.0);
    
    
    //parede da direita//
    
    while(!start_button()){
        wall= digital(Parede_D);
        printf("Start para Parede_D perto= %d\n",wall);
    }
    pertoD= wall;
    printf("pertoD %d\n", pertoD); 
    beep(); 
    sleep(1.0);
    
    while(!start_button()){
        wall= digital(Parede_D);
        printf("Start para Parede_D longe= %d\n",wall);
    }
    longeD= wall;
    printf("longeD %d\n", longeD); 
    beep(); 
    sleep(1.0);
    
    
    printf("Fim da Calibracao.\n");
    beep();
    sleep(0.5);
    
}

//Segue parede simples//
void Parede_Simples(){
    //x= 0;
    
    while (1) {
        int wall_D=digital(Parede_D);
        int wall_E=analog(Parede_E);
        printf("wall_D:%d wall_E: %d\n", wall_D, wall_E);
        
        if (wall_E > VparedeE) andar_esquerda();  /*longe */
        else andar_direita();             /* perto */ 
        
        if (wall_D < VparedeD){
            execua_re(1.3);
            executa_rodar(90);  /* longe */
        }
        
        
        
    }
}

//Segue parede Gentle//
void Parede_Gentle(){
    while (1) {
        //int x =0;
        int wall_D= digital(Parede_D);
        int wall_E=analog(Parede_E);
        printf("wall_D:%d wall_E: %d\n", wall_D, wall_E);
        
        if (wall_E >= longeE){ /* too far */
            andar_esquerda();
        }else{
            if (wall_E <= pertoE){  /* too close */
                andar_direita();
            }else{
                if(wall_D >= longeD){
                    andar_reto();
                }
            }
        }
        
        if (wall_D < longeD){ /* too close */
            execua_re(1.3);
            executa_rodar(90);
        }  
        
    }
}

void alinhar(){
	if(Luz_Escolha==Luz_H){
		int luzh = digital(Luz_H);
		int luzv = digital(Luz_V);
	}else{
		int luzv = digital(Luz_H);
		int luzh = digital(Luz_V);
	}
    
    if(!luzv() && luzh()){
        kill_process(pidm1);
        off(0);
        kill_process(pidm2);
        off(2); 
    }else{
        pidm1 = start_process(rodar(10.0, -velm1, 0));
        pidm2 = start_process(rodar(10.0, velm2, 2));
        while(1){
            if(!luzv() && luzh()){
                kill_process(pidm1);
                off(0);
                kill_process(pidm2);
                off(2); 
                break;
            }
            //printf("luz:%d%d\n", luzv, luzh);
            sleep(0.5);
        }
    }
    beep();
    sleep(0.5);
}

void alinhar2(){
	if(Luz_Escolha==Luz_H){
		int luzh = digital(Luz_H);
		int luzv = digital(Luz_V);
	}else{
		int luzv = digital(Luz_H);
		int luzh = digital(Luz_V);
	}
    
    if(!luzv2() && luzh2()){
        kill_process(pidm1);
        off(0);
        kill_process(pidm2);
        off(2); 
    }else{
        pidm1 = start_process(rodar(10.0, -velm1, 0));
        pidm2 = start_process(rodar(10.0, velm2, 2));
        while(1){
            if(!luzv2() && luzh2()){
                kill_process(pidm1);
                off(0);
                kill_process(pidm2);
                off(2); 
                break;
            }
            sleep(0.5);
        }
    }
    beep();
    sleep(0.5);
}

void escohe_luzV(){
    Luz_Escolha=Luz_V;
    printf("Vertical");
    sleep(1.5);
}

void escohe_luzH(){
    Luz_Escolha=Luz_H;
    printf("Horizonal");
    sleep(1.5);
}



void na_linha(){
    while(sensor_linha() < ponto_escuro);    
}

void fora_linha(){
    while(sensor_linha() > ponto_claro);
}

int sensor_linha(){
    return analog(2);
}
int sensor_cor(){
    return analog(3);
}

void executa_reta(){
    executa_andar(var30, var30);
    
    executa_andar(var90g,-var90g);
    executa_andar(var90g,-var90g);
    
    executa_andar(var30, var30);
    
}

void acende_led(int cor){
    if (cor == 0){
        bit_set(0x1008, 4);//led vermelho
    }
    else if(cor == 1){
        bit_set(0x1008, 8);//led verde
    }
    else if(cor == 2){
        bit_set(0x1008, 16);//led amarelo
    }   
}

void apaga_led(int cor){
    if (cor == 0)
      bit_clear(0x1008, 4);
    else if(cor == 1)
      bit_clear(0x1008, 8);
    else if(cor == 2)
      bit_clear(0x1008, 16);  
}

void executa_luzes(){ 
    int tempo;
    tempo=0;
    
    while(tempo<8){
        bit_set(0x1008, 4);//led vermelho//
        sleep(1.0);
        bit_clear(0x1008, 4);
        
        bit_set(0x1008, 8);//led verde//
        sleep(1.0);
        bit_clear(0x1008, 8);
        
        bit_set(0x1008, 16);//led amarelo//
        sleep(1.0);
        bit_clear(0x1008, 16);
        tempo++;
    }//end while//
}

void executa_musica2(){
    
}

void executa_musica(){
    int pidt1, pidt2,pidt3;
    pidt1 = start_process(executa_luzes());
    pidt2 = start_process(rodar(20.0 ,50, 0));
    pidt3 = start_process(executa_musica2());
    sleep(20.0);
    motor(0,0);
    off(0);
    kill_process(pidt2);
    kill_process(pidt3);
    tone(380.0, 0.01);
    bit_clear(0x1008, 4);
    bit_clear(0x1008, 8);
    bit_clear(0x1008, 16);
    kill_process(pidt1);
    
}

int andar_frente(){
    int pidteste2 = start_process(count8());
    int reu; 
    reu = executa_andar(30.0, 30.0);
    kill_process(pidteste2);
	return (reu);
}

int naobate(){
	 while (1) {
        int wall_D= digital(Parede_D);
        int wall_E=analog(Parede_E);
        printf("wall_D:%d wall_E: %d\n", wall_D, wall_E);
        if (wall_E <= pertoE){  /* too close */
                andar_direita();
            }else{
                if(wall_D == 0){
                    andar_reto();
                }
            }
        
        if (wall_D == 1 || sensor_cor()>240){ /* too close */
            stop();
			break;
        }  
	}
}

void voltabase(){
	execua_re(1.5);
	while(1){
	if (analog(Luz_Escolha)<50){
		break;
	}else{
		execua_re(1.5);
		alinhar2();
	}
	}
}

void tatica1(){
	int cor=5;
	while(1){
	naobate();
	//verificacor//
	cor=executa_cor();
	//se nao preto: pega, da re ate a base//
	if(cor!=0){
		executa_braco();
		execua_re(2.0);
		voltabase();
		executa_rodar(90);
		executa_braco3();
		executa_rodar(-90);
	}else{//se preto: vira a direita e não bate. //
		execua_re(1.3);
        executa_rodar(-90);
	}
	}
}

void executa_taticas(int p){
	int reu = 1;
	if(p == 1){
		tatica1();
	}
	else{
		while(reu == 1){
			reu = andar_frente();
		}
	}
}

void begin(int p){
	calibra_luz();
	executa_taticas(p);
}
int luzv(){
	int luzv = analog(Luz_V);
	if(luzv<=35){
		return(1);
	}else{
		return(0);
	}
}
int luzh(){
	int luzh = analog(Luz_H);
	if(luzh<=75){
		return(1);
	}else{
		return(0);
	}
}
void calibra_luz(){
    if(!luzv() && luzh()){
        off(0);
        off(2); 
		Luz_Escolha=Luz_H;
    }else if(luzv() && !luzh()){
        off(0);
        off(2); 
		Luz_Escolha=Luz_V;
	}else{
        pidm1 = start_process(rodar(10.0, -velm1, 0));
        pidm2 = start_process(rodar(10.0, velm2, 2));
        while(1){
            if(!luzv() && luzh()){
                kill_process(pidm1);
                off(0);
                kill_process(pidm2);
                off(2); 
				Luz_Escolha=Luz_H;
                break;
            }else if(luzv() && !luzh()){
				kill_process(pidm1);
                off(0);
                kill_process(pidm2);
                off(2); 
				Luz_Escolha=Luz_V;
                break;
			}else{
            sleep(1.0);
	    }   
	}
	}
  }

  int luzv2(){
	int luzv = analog(Luz_V);
	if(luzv<=100){
		return(1);
	}else{
		return(0);
	}
}
int luzh2(){
	int luzh = analog(Luz_H);
	if(luzh<=150){
		return(1);
	}else{
		return(0);
	}
}
  void calibra_luz2(){
    if(!luzv2() && luzh2()){
        off(0);
        off(2); 
		Luz_Escolha=Luz_H;
    }else if(luzv2() && !luzh2()){
        off(0);
        off(2); 
		Luz_Escolha=Luz_V;
	}else{
        pidm1 = start_process(rodar(10.0, -velm1, 0));
        pidm2 = start_process(rodar(10.0, velm2, 2));
        while(1){
            if(!luzv2() && luzh2()){
                kill_process(pidm1);
                off(0);
                kill_process(pidm2);
                off(2); 
				Luz_Escolha=Luz_H;
                break;
            }else if(luzv2() && !luzh2()){
				kill_process(pidm1);
                off(0);
                kill_process(pidm2);
                off(2); 
				Luz_Escolha=Luz_V;
                break;
			}else{
            sleep(1.0);
	    }
	}	
  }
}



void Calibra_tudo(){
	calibra_cor();
	calibra_paredeGentle();
}

void inicia_Partida2(int parametro){
	int luz= analog(5);
	poke(0x1009, 0x3c);
	while(1){
        if(luz < 5){
            int pidP = start_process(begin(parametro));
			sleep(60.0);
			stop();
			off(3);
            break;
        }
        luz= analog(5);
        printf("Esperando parida %d\n",luz);
        sleep(0.2);
    }
}
void inicia_Partida(int parametro){
    int luz= analog(5);//digital(Luz_Partida);
	int calibra_cor=0;
	poke(0x1009, 0x3c);
	sleep(1.5);
	while(1){
		printf("Start-Caibra Stop-No\n" );
		if(!stop_button()){
			calibra_cor=1;
			break;
		}else if(!start_button()){
			break;
		}
			
	}
	if(calibra_cor==1){
	Calibra_tudo();
	}
    while(1){
        if(luz < 5){
             int pidP = start_process(begin(parametro));
			sleep(60.0);
			stop();
			motor(3,0);
			off(3);
            break;
        }
        luz= analog(5);//digital(Luz_Partida);
        printf("Esperando parida %d\n",luz);
        sleep(0.2);
    }
    
}

void executa_braco(){
    rodar(0.8, -70, 3);
    sleep(1.0);
    off(3);
    off(3);
}

void executa_braco2(){
    rodar(0.65, -70, 3);
    sleep(0.8);
    off(3);
    off(3);
}

void executa_braco3(){
    rodar(0.65, 85, 3);
    sleep(1.0);
    off(3);
}

void polar(){
	int luzv,luzh,ldr;
	while(!stop_button()){
	ldr=sensor_cor();
	luzv = analog(Luz_V);
	luzh= analog(Luz_H);
	printf("luz:V:%dH:%d %d\n", luzv, luzh, ldr);
	sleep(1.0);
	}
}

void executeItem(int MenuOption){
    int lixo = 1;
    if (MenuOption == 0){
        inicia_Partida2(2);
    }
	if (MenuOption == 1){
        inicia_Partida2(1);
    }
    if( MenuOption == 2){
        inicia_Partida(2);
    }
    
    if( MenuOption == 3){
        Calibra_tudo();
    }
    
    if( MenuOption == 4){ 
        polar();
    }  
    
    
}