/*
 *        SEGUIDOR DE LINEA CON ARDUINO
 *                CODIGO BASE
 *  Este código es la base para iniciar el seguidor de linea
 *  aun no esta terminado, pero se ira completando hasta la
 *  version final.
 *  
 *  NOTA: Esta codigo no usa la libreria QTR, por lo que el
 *  procesamieto es manual y se puede ajustar de una mejor manera
 */

//------------------------------------------------------------
//----------------------VARIABLES-----------------------------
//------------------------------------------------------------
int sensores[]={A0,A1,A2,A3,A4,A5,A6,A7};//Sensores
int valores[8];
int umbral[]={1023,1023,1023,1023,1023,1023,1023,1023};
int numSensores=8;//total de sensres en la barra
int ir =2;//encender y apagar la barra

#define PWMA  9
#define AIN2  8
#define AIN1  7
#define BIN1  5
#define BIN2  4
#define PWMB  3


//------------------------------------------------------------
//----------------------INICIALIZACION------------------------
//------------------------------------------------------------
void setup() {
  Serial.begin(115200);
  //Inicializamos los pines de los sensores
  for(int x=0;x<numSensores;x++){
    pinMode(sensores[x],INPUT);
  }
  pinMode(LED_BUILTIN,OUTPUT);
  //pin para controlar el encendido de la barra
  pinMode(ir,OUTPUT);

  //pines para controlar motores
  pinMode(PWMA,OUTPUT);
  pinMode(PWMB,OUTPUT);
  pinMode(AIN1,OUTPUT);
  pinMode(AIN2,OUTPUT);
  pinMode(BIN1,OUTPUT);
  pinMode(BIN2,OUTPUT);
  //establecer direccion de los motores
  digitalWrite(AIN1,HIGH);
  digitalWrite(AIN2,LOW);
  digitalWrite(BIN1,HIGH);
  digitalWrite(BIN2,LOW);
  
  digitalWrite(ir,HIGH);//Encendemos la barra
  calibracion();
}


//------------------------------------------------------------
//---------------------------LOOP-----------------------------
//------------------------------------------------------------
void loop() {
   int pos=getPosicion();

    //PINTAR LOS VALORES
  for(int x=0;x<numSensores;x++){  
    Serial.print(valores[x]);
    Serial.print("\t");
  }
  Serial.println(pos);

  acelerar(50,50);
  delay(100);
}

void acelerar(int motIzq, int motDer){
  analogWrite(PWMA,motIzq);
  analogWrite(PWMA,motDer);
}


/*
 * Funcion que permite la lectura de los sensores de forma
 * analógica sin procesar nada, el resultado queda en el
 * arreglo llamado "valores"
 */
void leerRAW(){
   //LEER LOS VALORES DE LOS SENSORES
    for(int x=0;x<numSensores;x++){
      valores[x]=analogRead(sensores[x]);
    }
}

/*
 * Función que permite mediante un promedio de valores
 * encontrar la posición de la linea negra en todo momento
 * es similar a la función de la libreria QTR, por lo que regresará
 *  un valor entre 0 y 4000
 */
int getPosicion(){
  leerDigital();
  int activos=0;
  int suma=0;
   for(int x=0;x<numSensores;x++){
    suma=suma+valores[x];
    if(valores[x]!=0){
      activos++;
    }
   }
   if(activos==0){
    return 0;
   }else{
    return suma/activos;
   }
  
}

/*
 * Esta función permite leer los valores de los ensores y
 * procesar cada uno de los valores para encontrar la linea
 * negra, similar a la lectura de la libreria QTR, donde cada
 * linea es multipo de 500
 * 
 *      500  1000  1500  2000  2500  3000  3500  4000
 */
void leerDigital(){
   //LEER LOS VALORES DE LOS SENSORES
    for(int x=0;x<numSensores;x++){
      valores[x]=analogRead(sensores[x])>(umbral[x]+70)?(x*500)+500:0;
    }
}


/*
 * Esta funcion tiene como objetivo encontrar el valor mas alto
 * y mas bajo de los sensores, para establecer su umbral, por el momento solo
 * encuentra los valores bajos (blancos) y queda pendiente encontrar
 * los valores negros para obtener su promedio para el umbral.
 * 
 * TODO:reacomodar esta función
 */
void calibracion(){
  digitalWrite(LED_BUILTIN,HIGH);
  for(int y=0;y<400;y++){
  for(int x=0;x<numSensores;x++){
    int v=analogRead(sensores[x]);
    if(umbral[x]>v){
      umbral[x]=v;
    }
  }
  delay(10);
  }
  
  digitalWrite(LED_BUILTIN,LOW);
}