//int condEncoder = 0;
#use "utils.ic"


void Rotate(int sentido, int rot){
    int pid;
    while(lockMotor != 0 && lockMotor != 1) defer();
    lockMotor = 1;
    reset_encoder(ENCODER_DIR);
    reset_encoder(ENCODER_ESQ);
    if(sentido == DIR){
        //motor(MOTOR_DIR,-POTENCIA_MOTOR_DIR);
        //motor(MOTOR_ESQ,POTENCIA_MOTOR_ESQ);
        pid = start_process(andaReto(-100,-60,60,100,2));
    }
    else{
        //motor(MOTOR_DIR,POTENCIA_MOTOR_DIR);
        //motor(MOTOR_ESQ,-POTENCIA_MOTOR_ESQ);
        pid = start_process(andaReto(60,100,-100,-60,3));
    }
    while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < rot){
        if(prioridade == 1){
            kill_process(pid);
            ao();
            lockMotor = 2;
            defer();
            while(lockMotor!= 0){
                defer();
            }
            lockMotor = 1;
            if(sentido == DIR){
                pid = start_process(andaReto(-100,-60,60,100,2));
            }
            else{
                pid = start_process(andaReto(60,100,-100,-60,3));
            }
        }
        else defer();
    }
    kill_process(pid);
    ao();
}

void Back(int rots){
    int pidEncoder;
    reset_encoder(ENCODER_DIR);
    reset_encoder(ENCODER_ESQ);
    //anda para tras
    pidEncoder = start_process(andaReto(-100,-60,-100,-60,-1));
    while(read_encoder(ENCODER_DIR)+read_encoder(ENCODER_ESQ) < rots);
    kill_process(pidEncoder);
}

void andaReto(int boundMinD, int boundMaxD, int boundMinE, int boundMaxE, int sentido){
    int countEsq, countDir, erro, erroAnterior, ePositivo;
    int Potencia_Esq;
    int Potencia_Dir;
    float controlV; //manipulated variable
    float razao;
    long tempoAtual;
    long vTempo = 50L;
    
    
    //seta inicio
    if(sentido < 0){
        Potencia_Esq = -70;
        Potencia_Dir = -70;
    }
    else if(sentido == 1){
        Potencia_Esq = 70;
        Potencia_Dir = 70;
    }
    else if(sentido == 2){
        Potencia_Esq = 70;
        Potencia_Dir = -70;
    }
    else if(sentido == 3){
        Potencia_Esq = -70;
        Potencia_Dir = 70;
    }
    reset_encoder(ENCODER_ESQ);
    reset_encoder(ENCODER_DIR);
    erroAnterior = 0;
    //executa trajetoria em linha reta, durante tempo determinado
    while(1){
        if(Potencia_Esq < boundMinE) Potencia_Esq = boundMinE;
        if(Potencia_Dir < boundMinD) Potencia_Dir = boundMinD;
        if(Potencia_Esq > boundMaxE) Potencia_Esq = boundMaxE;
        if(Potencia_Dir > boundMaxD) Potencia_Dir = boundMaxD;
        
        motor(MOTOR_ESQ,Potencia_Esq);
        motor(MOTOR_DIR,Potencia_Dir);
        //reset_encoder(ENCODER_ESQ);
        //reset_encoder(ENCODER_DIR);
        tempoAtual = mseconds();
        while(tempoAtual+vTempo > mseconds());
        //msleep(vTempo);
        printf("\n\npot %d %d count %d %d",Potencia_Esq,Potencia_Dir, countEsq, countDir);
        //condicao inicial para evitar divisao por zero
        while(read_encoder(ENCODER_DIR) < 1 || read_encoder(ENCODER_ESQ) < 1);
        countDir = read_encoder(ENCODER_DIR);
        countEsq = (int)((float)read_encoder(ENCODER_ESQ)*0.63);
        // printf("\n\ndir: %d esq: %d",countDir,countEsq);
        //     ao();
        erro = countDir - countEsq;
        
        //corrige potencia para que as rodas girem o mesmo tanto
        controlV = Kp*(float)erro - Kd*((float)(erroAnterior - erro));
        Potencia_Dir -= (int) controlV;
        Potencia_Esq += (int) controlV;
        
        erroAnterior = erro;
        printf("e: %d\n", erro);
    }
    
}