/* beacon.c
 *
 * This file contains the code for managing the inputs from the robot's beacon sensors.
 * For a detailed description of the code please refer to the inline comments below. 
 */
 
#include <stdio.h>
#include <shiftreg.h>
#include <ad_lib.h>
#include "constants.h"
#include "beacon.h"

// For debugging purposes, each channel has an associated label indexed by beacon code.
static const char *ChannelNames[]
 = { "Center" , "Right", "Left" };

/*************************************************************************
 * CollectPeakValues
 *
 * Records the peak value seen on each sensor over a set number of samples. Note
 * we have to take multiple readings because the input reading is sinusoidal (and
 * roughly zero otherwise). We would like to know what the peak value of the input
 * is over a certain period to decide if a particular beacon is active.
 *************************************************************************/
void CollectPeakValues(unsigned int beaconSamples[], unsigned int numBeacons)
{
 int i, j, curr_level;
 
 for (i=0; i < numBeacons; i++)
 	 beaconSamples[i] = 0;
 for (i=0; i < kSampleCount; i++)
 {
  for (j=0; j < numBeacons; j++)
  {
   curr_level = AD_Read(j);
   if (curr_level > beaconSamples[j]) {
   	  beaconSamples[j] = curr_level;
   }
  }
 }
}

/*************************************************************************
 * FindActiveBeacon
 *
 * This code simply examines the current beacon readings, and assuming
 * a current beacon is active, examines the remaining beacons to determine
 * which one currently has the highest reading. This beacon is assumed to
 * be active. We are guaranteed that one beacon will be active, and this
 * code determines which beacon it is.
 *************************************************************************/
// Routine to determine which beacon is active
BeaconCode FindActiveBeacon(BeaconCode lastBeacon)
{
 unsigned int beaconSamples[kNumBeacons];
 int i;
 BeaconCode activeBeacon = lastBeacon;
 
 CollectPeakValues(beaconSamples, kNumBeacons);
 for (i=1; i < kNumBeacons; i++)
 {
  if (beaconSamples[(i+lastBeacon) % kNumBeacons] > beaconSamples[activeBeacon] &&
  	  beaconSamples[(i+lastBeacon) % kNumBeacons] > kMinStrength)
  	 activeBeacon = (i+lastBeacon) % kNumBeacons;
 }
 return activeBeacon;
}


/*************************************************************************
 * FindCenter
 *
 * This determines if the center has been found by averaging the peak value
 * readings obtained over several runs to amortize the cost of spurious noise
 * etc. on the line (this was initially a problem, and may have subsided by the
 * end of the project; however, we have left the code in place). If the reading
 * on the center beacon is beyond a threshold level, then we claim to have found
 * the center; otherwise, we report the orientation is off center (forcing the
 * robot to continue rotation).
 *************************************************************************/
Orientation FindCenter(void)
{
 unsigned int beaconSamples[kNumBeacons];
 unsigned int averagePeaks[kNumBeacons];
 int i,j;
 
 for (j=0; j < kNumBeacons; j++)
 	 averagePeaks[j] = 0;
	 
 for (i=0; i < kSampleRuns; i++) {
 	 CollectPeakValues(beaconSamples, kNumBeacons);
        averagePeaks[kCenterBeacon] += beaconSamples[kCenterBeacon];
 }
 averagePeaks[kCenterBeacon] /= kSampleRuns;
  
  if (averagePeaks[kCenterBeacon] >= kMinStrength) {
	return kOrientationCentered;
  }

 return kOrientationSkewed;
}