text: #pragma config(Sensor, in1,    vis1, sensorReflection)
  #pragma config(Sensor, in2,    inf1, sensorLineFollower)
  #pragma config(Sensor, in3,    inf2, sensorLineFollower)
  #pragma config(Sensor, dgtl11, enc1, sensorQuadEncoder)
  #pragma config(Motor,  port1,  led1, tmotorVexFlashlight, openLoop, reversed)
  #pragma config(Motor,  port2,  ser1, tmotorServoStandard, openLoop)
  #pragma config(Motor,  port3,  ser2, tmotorServoStandard, openLoop)
  #pragma config(Motor,  port10, mot1, tmotorVex269_HBridge, openLoop)
  
  #define square(n) n*n
  
  int type;
  
  int dist;
  int minDist;
  
  enum sensor { Vis, Inf, nSensors };
  enum material { Empty, Blue, Metal, White, Bite, Wood, Clear, nMaterials };
  
  // Known material data to match against
  int known[nMaterials][nSensors] = {
  	{  107,   84},
  	{  578, 1423},
  	{ 2609, 1119},
  	{  875,  957},
  	{  416, 1145},
  	{ 2742,  623},
  	{  112, 1229},
  };
  // Normalization factors for converting sensor data to same range as knowns
  int norm[nSensors] = { 300, 4000 };
  
  // Averaging an array of sensor values
  #define nSamples 200
  int arr[nSamples][nSensors];
  int index;
  int avg[nSensors];
  int sum[nSensors];
  
  int i;
  
  task main()
  { while(1) {
  	// Raise scanner to catch ball
  	while(SensorValue[enc1] < 0) motor[mot1] = 128;
  	wait(0.1);
  	motor[mot1] = 0;
  
  	// Reset encoder
  	SensorValue[enc1] = 0;
  
  	// Release ball from queue
  	motor[ser1] = -50;
  	wait(0.11);
  	motor[ser1] = 10;
  	wait(1);
  
  	// Turn on flashlight
  	motor[led1] = -127;
  	type = 0;
  	// Sample until type is not 0, but have a minimum of samples first
  	for(index = type = 0; type == 0 || index < nSamples; index++)
  	{
  		// Add new samples to arrays
  		arr[index % nSamples][Vis] = SensorValue[vis1];
  		arr[index % nSamples][Inf] = SensorValue[inf1] + SensorValue[inf2];
  
  		// Find sums of arrays
  		sum[Vis] = 0;
  		sum[Inf] = 0;
  		for(i = 0; i < nSamples; i++)
  		{
  			sum[Vis] += arr[i][Vis];
  			sum[Inf] += arr[i][Inf];
  		}
  
  		// Take averages
  		avg[Vis] = sum[Vis] * 1000 / norm[Vis] / nSamples;
  		avg[Inf] = sum[Inf] * 1000 / norm[Inf] / nSamples;
  
  		// Find best type match
  		minDist = 1000000;
  		type = 0;
  		for(i = 0; i < (int)nMaterials; i++)
  		{
  			dist = square(avg[Vis] - known[i][Vis]) + square(avg[Inf] - known[i][Inf]);
  
  			if(dist > minDist) continue;
  
  			minDist = dist;
  			type = i;
  		}
  
  		// Adjust redirector
  		motor[ser2] = 10*(type - (int)nMaterials);
  	}
  
  	// Turn off flashlight
  	motor[led1] = 0;
  
  	// Lower scanner to drop ball
  	while(SensorValue[enc1] > -200) motor[mot1] = -128;
  	motor[mot1] = 0;
  	wait(1);
  } }
  