//defines gerais
#define MOTOR_DIR 0
#define MOTOR_ESQ 1
#define MOTOR_BLOCO 2
#define MOTOR_EXTENSAO 3
#define ENCODER_DIR 0
#define ENCODER_ESQ 1
#define TOQUE_DIR 10
#define TOQUE_ESQ 12
//#define TOQUE_FRENTE TOQUE_DIR || TOQUE_ESQ
#define TOQUE_FRENTE 15
#define BREAK_BEAM 6

#define DIR 0
#define ESQ 1

#define ROT_45 4
#define ROT_90 7
#define ROT_140 9
#define ROT_180 14
#define ROT_360 26
#define POTENCIA_MOTOR 60
#define POTENCIA_MOTOR_DIR 60
#define POTENCIA_MOTOR_ESQ 80
//#define POTENCIA_MOTOR_DIR 50
//#define POTENCIA_MOTOR_ESQ 70
#define POTENCIA_ROTACAO 75
//55
#define POTENCIA_BLOCO 100

//defines e variaveis da localizacao
#define DIF_SENSOR 4
#define NUM_VAL_LUZ 10
#define MIN_LUZ1 132
#define MAX_LUZ2 123


//defines e variaveis do segue linha
#define LIGHT_SENSOR_DIR 5
#define LIGHT_SENSOR_ESQ 2



//defines das cores
#define COLOR_SENSOR 3
#define RED_LED 0x08
#define GREEN_LED 0x06
#define BLUE_LED 0xA0
#define PESO_DIST 1
#define PESO_IND 0

//defines e variaveis da linha reta
#define POT_MIN 60
#define POT_MAX 100
#define Kp 2.0
#define Kd 0.8
//0.8

int lockMotor=0;
int prioridade=0;
int carregandoObjeto = 0;

//#use "musictmpl.c"



int getRGB(int sensor, int valor[]){
    //filtro de luz ambiente
    int vSemLuz = getMediaLightValue(sensor);
    
    //int vSemLuz = 0;
    valor[0] = getLedRGB(sensor,RED_LED) - vSemLuz;
    valor[1] = getLedRGB(sensor,GREEN_LED) - vSemLuz;
    valor[2] = getLedRGB(sensor,BLUE_LED)- vSemLuz;
    //garante intervalo entre o proximo calculo de luz
    printf("\n\nvSemLuz: %d\n", valor[0], valor[1], valor[2]);
    sleep(1.1);
    //notifica que nao detectou objeto
    if(valor[0] < 5 && valor[1] < 5 && valor[2] < 5) return -1;
    else if(valor[0] < 1 || valor[1] < 1 || valor[2] < 1) return -1;
    
    else return 0;
}

int getLedRGB(int sensor, int led){
    int value;
    
    poke(0x1008, led);
    sleep(0.1);
    value = getMediaLightValue(sensor);
    //sleep(1.0);
    poke(0x1008, 0x00);
    
    //printf("\n\nv1: %d v2: %d v3: %d",values[0],values[1],values[2]);
    
    return value;
}



int getLightValue(int sensor){
    return 255 - analog(sensor);
}

int getMediaLightValue(int sensor){
    int valor=0;
    int i;
    for(i = 1; i <= 10; i++){
        valor += getLightValue(sensor);
    }
    return valor / 10;
}

/*float getMediaLightValueF(int sensor){
    float valor=0.0;
    int i;
    for(i = 1; i <= 10; i++){
        valor += (float)getLightValue(sensor);
    }
    return valor / 10.0;
}*/



float pow(float n, int expo){
    int i;
    float result=1.0;
    for(i=0;i<expo;i++){
        result *= n;
    }
    return result;
}

float square(float n){
    return n*n;
}

int squareI(int n){
    return n*n;
}