#pragma config(Sensor, S1,     L,                   sensorLightActive)
#pragma config(Sensor, S4,     T,                   sensorTouch)
#pragma config(Motor,  motorA,          M1,            tmotorNormal, openLoop, encoder)

//Função: separador de cores com lego mindstorms nxt
//Código feito por Ivan Seidel
//http://techlego.blogspot.com/

//Peço que deixem meus créditos apenas!

#include "MV-lib.c"

#define SpeedNormal           56
#define DifMaxLight           50
#define RotateAngleFirst      200
#define RotateAngleMultiple   180
#define RotateAngleBack       150
#define RotateAngleAfterT     0
#define MaxColors             7

int ArrayLights[10];
int ColorsFound = 0;
int LightValue;

//Le o valor do sensor deluz, e checa se esta no range de algum valor do array
//Reads light sensor and checks if that value is in the range of somevalue in the array
int IsInRange(int CheckValue){
  int i=0,found=-1;
  while(i<ColorsFound){
    if(CheckValue<=ArrayLights[i]+DifMaxLight && CheckValue>=ArrayLights[i]-DifMaxLight && ArrayLights[i]!=0){
      found=i;
      break;
    }
    i++;
  }
  return found;
}

void InsertValue(int InsertValue){
  ArrayLights[ColorsFound]=InsertValue;
  ColorsFound++;
}

task main(){
  int KeepRuning=true;
  while(KeepRuning){

    //Reseta todos os valores e vai para o comeco
     //Resets position and all states to 0
     while(SensorValue[T]==0){
       motor[M1]=SpeedNormal;
     }
     MV_StopMotors();
     MV_Vira(-SpeedNormal,RotateAngleAfterT,M1);
     wait10Msec(50);
     //Inicia o programaprincial de separar as cores
     //Starts the main program of sorting coloros
    int samples=10,LightValue;
     for(int y=0;y<samples;y++){
       LightValue+=SensorRaw[L];
     }
     LightValue=LightValue/samples;
     MV_Vira(SpeedNormal/1.5,RotateAngleFirst,M1);
     bool looping=true;
     while(looping){
       int foundColorPosition=IsInRange(LightValue);
       if(foundColorPosition!=-1){
           for (int i=0;i < foundColorPosition+1;i++){
                  PlayTone(500,5);
                wait10Msec(10);
            }
            wait10Msec(50);
         MV_Vira(SpeedNormal,(foundColorPosition+1)*RotateAngleMultiple,M1);
         MV_Vira(-SpeedNormal*1.6,RotateAngleBack,M1);
         wait10Msec(50);
         looping=false;
        }else{
          InsertValue(LightValue);
        }
      }
  }
}

//----------------------------------------------//
//----------------------------------------------//
//Library que possui as funcoes de mover motores//
//----------------------------------------------//
//----------------------------------------------//
#define MV_EMotor    motorA
#define MV_DMotor    motorC
#define MV_GarraMotor     motorB
#define MV_TimeLimit 70 //5 secs

void MV_StopMotors(){
   motor[MV_EMotor] = 0;
  motor[MV_DMotor] = 0;
  motor[MV_GarraMotor]  = 0;
}

void MV_Reto_Unlimited(int forca,int steering=0){
   motor[MV_EMotor]=forca+steering;
   motor[MV_DMotor]=forca-steering;
}
void MV_Spin_Unlimited(int forca){
   motor[MV_EMotor]=forca;
   motor[MV_DMotor]=-forca;
}

void MV_Vira(int forca, int degree,byte lado, int tempo=0){
   time100[T2]=0;
   if(degree>0){
      nMotorEncoder[lado]=0;
      while(nMotorEncoder[lado]<degree && nMotorEncoder[lado]>-degree && time100[T2]<MV_TimeLimit){
         motor[lado]=forca;
      }
      MV_StopMotors();
   }else if(tempo>0){
      time100[T1]=0;
      while(time100[T1]<tempo){
         motor[lado]=forca;
         motor[lado]=forca;
      }
      MV_StopMotors();
   }
}

void MV_Spin(int forca, int degree, int tempo=0){
   time100[T2]=0;
   nMotorEncoder[MV_EMotor]=0;
   nMotorEncoder[MV_DMotor]=0;
   if(degree>0){
      while(nMotorEncoder[MV_EMotor]<degree && nMotorEncoder[MV_EMotor]>-degree && time100[T2]<MV_TimeLimit){
         motor[MV_EMotor]=forca;
         motor[MV_DMotor]=-forca;
      }
      MV_StopMotors();
   }else if(tempo>0){
      time100[T1]=0;
      while(time100[T1]<tempo){
         motor[MV_EMotor]=forca;
         motor[MV_DMotor]=-forca;
      }
      MV_StopMotors();
   }
}