#pragma config(Sensor, S1,     LeftLight,      sensorLightActive)
#pragma config(Sensor, S4,     RightColor,     sensorEV3_Color)
#pragma config(Motor,  motorB,          leftMotor,     tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorC,          rightMotor,    tmotorEV3_Large, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int colorSensorCalibrationBlack;
int colorSensorCalibrationWhite;
int lightSensorCalibrationBlack;
int lightSensorCalibrationWhite;
int colorRight;
int lightLeft;
/*
Colour sensor modes
0 - modeEV3Color_Reflected
1 - modeEV3Color_Ambient
2 - modeEV3Color_Color
3 - modeEV3Color_Reflected_Raw
4 - modeEV3Color_RGB_Raw
5 - modeEV3Color_Calibration - Not utilized
*/
int sum_array(int *a, int num_elements){
   int i, sum=0;
   for (i=0; i<num_elements; i++)
   {
	 sum = sum + a[i];
   }
   return(sum/num_elements);
}

void calibrate() {
	int first[26];
	int second[26];
	int i;
	writeDebugStream("Here2");
	while(!getButtonPress(buttonAny)==1){}
	writeDebugStream("Here3");

	for (i = 0; i < 13 ;i+=2) {
		setMotorSpeed(leftMotor, 25);
		setMotorSpeed(rightMotor, -25);
		sleep(100);
		first[i] = SensorValue[RightColor];
		second[i+1] = SensorValue[LeftLight];
		sleep(20);


		setMotorSpeed(leftMotor, -25);
		setMotorSpeed(rightMotor, 25);
		sleep(100);
		first[i] = SensorValue[RightColor];
		second[i+1] = SensorValue[LeftLight];
		sleep(20);
		}
setMotorSpeed(leftMotor,0);
			setMotorSpeed(rightMotor,0);
	colorSensorCalibrationBlack = sum_array(first,26);
	lightSensorCalibrationBlack = sum_array(second,26);
	while(!getButtonPress(buttonAny)==1){}
		for (i = 0; i < 13 ;i+=2) {
			setMotorSpeed(leftMotor, 25);
			setMotorSpeed(rightMotor, -25);
			sleep(100);
			first[i] = SensorValue[RightColor];
			second[i+1] = SensorValue[LeftLight];
			sleep(20);



		setMotorSpeed(leftMotor, -25);
		setMotorSpeed(rightMotor, 25);
		sleep(100);
		first[i] = SensorValue[RightColor];
		second[i+1] = SensorValue[LeftLight];
		sleep(20);
		}
		colorSensorCalibrationWhite = sum_array(first,26);
		lightSensorCalibrationWhite = sum_array(second,26);
		setMotorSpeed(leftMotor,0);
			setMotorSpeed(rightMotor,0);

}

task main()
{

	calibrate();
	colorRight = abs(colorSensorCalibrationBlack-colorSensorCalibrationWhite)/4;
	lightLeft = abs( lightSensorCalibrationBlack- lightSensorCalibrationWhite)/4;
	while (true){
		writeDebugStream("%d,%d/n",abs(SensorValue[LeftLight] - lightSensorCalibrationWhite)<lightLeft,abs(SensorValue[RightColor]-colorSensorCalibrationBlack)<colorRight);
		if(abs(SensorValue[LeftLight] - lightSensorCalibrationWhite)<lightLeft && abs(SensorValue[RightColor]-colorSensorCalibrationBlack)<colorRight){
				setMotorSpeed(leftMotor, 10);
		setMotorSpeed(rightMotor, 5);

		}else if(abs(SensorValue[LeftLight] -lightSensorCalibrationBlack)<lightLeft && abs(SensorValue[RightColor]-colorSensorCalibrationBlack)<colorRight){
			setMotorSpeed(leftMotor, 5);
			setMotorSpeed(rightMotor, 10);


		}else {
			setMotorSpeed(leftMotor, 10);
			setMotorSpeed(rightMotor, 10);
		}
			sleep(100);
		}
}