#include "WProgram.h"
void SetupBuckets();
void UpdateBucket(unsigned long range);
void FillString(unsigned long range, char* pRangeString);
#include "LCD4Bit.h"
// Bicycle Rangefinder
// (c) 2008 Timothy Aidley
// Licensed under the Creative Commons Attribution 2.0 License for England and Wales.
// For details, see:  http://creativecommons.org/licenses/by/2.0/uk/legalcode
//
// This program uses a devantech SRF05 ranger connected to an Arduino with a standard LCD
// attached. It makes use of the 4-bit LCD library.
//
// To make sure that you can see the closest that a vehicle has passed recently, the unit
// will display closest a vehicle came in the last few seconds.
// To do this, there are a set of five range buckets. Each bucket holds the closest distance
// detected over the last 10 samples. The display shows the closest distance of any of the 
// buckets.
//
// The program defaults to imperial measures, but metric can be selected by commenting out 
// the #define USEINCHES line.


//create object to control an LCD.  
//number of lines in display=1
LCD4Bit lcd = LCD4Bit(2); 

// The ports we use to communicate with the rangefinder
int triggerPort = 3;
int echoPort = 4;

#define NUM_BUCKETS 5
#define BUCKET_LENGTH 7
#define USEINCHES 
#define MAXRANGE 50000
// We define how far out from the sensor the bike is considered to extend
// Default is 12 inches.
#define BIKEWIDTH (12 * 148)


#ifdef USEINCHES
  #define DIVISOR 148
  char g_major[] = "\'";
  char g_minor[] = "\"";
  #define MAJORMINORDIVISOR 12
#else
  #define DIVISOR 57
  char g_major[] = "m";
  char g_minor[] = "cm";
  #define MAJORMINORDIVISOR 100
#endif

unsigned long g_buckets[NUM_BUCKETS];
unsigned long g_currentBucket;
unsigned long g_currentDataPoint;

// Set up the buckets so that they all hold the maximum detectable range
void SetupBuckets()
{
  g_currentBucket = 0;
  g_currentDataPoint = 0;
  for (int i = 0; i < NUM_BUCKETS; ++i)
  {
    g_buckets[i] = MAXRANGE;
  }
}


// Update the bucket, given a range
// We have a certain number of datapoints per bucket. If ever the range is less than the smallest range
// detected so far, the bucket is updated. Once the set number of datapoints have been used, we move on
// to the next bucket.
void UpdateBucket(unsigned long range)
{
  if (range < g_buckets[g_currentBucket]) 
  {
    g_buckets[g_currentBucket] = range;
  }
  
  g_currentDataPoint++;
  if (g_currentDataPoint >= BUCKET_LENGTH)
  {
    g_currentDataPoint = 0;
    g_currentBucket++;
    if (g_currentBucket >= NUM_BUCKETS)
    {
      g_currentBucket = 0;
    }
    g_buckets[g_currentBucket] = MAXRANGE;
  }
}

// Here we find the smallest distance stored over all the buckets.
unsigned long GetClosestBucket()
{
  unsigned long closest = MAXRANGE;
  for (int i = 0; i < NUM_BUCKETS; ++i)
  {
    if (g_buckets[i] < closest)
    {
      closest = g_buckets[i];
    }
  }
  return closest;
}


void setup() { 
  pinMode(13, OUTPUT);  //we'll use the debug LED to output a heartbeat

  lcd.init();
  pinMode(triggerPort, OUTPUT);
  pinMode(echoPort, INPUT);
  
  SetupBuckets();
}


// Interface with the SRF05
unsigned long rangeFind()
{
  digitalWrite(triggerPort, HIGH);  // trigger the start pulse
  delayMicroseconds(20);            // 20us pause (actually specs say 'at least 10us')
  digitalWrite(triggerPort, LOW);   // end the trigger pulse 
  
  unsigned long timing = pulseIn(echoPort, HIGH);  // time the pulse on the echo port
  
  delayMicroseconds(20);          // Ensure we have a pause (>10us) before the next pulse
  return timing;
}

int g_update = 0;


// Simple string copying function - returns the position of the next free character that we can write to
char* CopyString(char* pDest, char* pSource)
{
  while(*pSource)
  {
    *pDest++ = *pSource++;
  }
  *pDest = 0;
  return pDest;
}

// Write an (optionally) padded number to a string
char* Integer2String(int num, char* pString, int padWidth)
{
  int hitDigit = 0;
  int startPadding = 5 - padWidth;
  for (unsigned long i = 10000; i != 0; i /= 10)
  {
    unsigned long digit = num / i;
    if (startPadding <= 0)
    {
      hitDigit = 1;
    }
    --startPadding;
    if (hitDigit || digit)
    {
      *pString = '0' + digit;
      ++pString;
      hitDigit = 1;
    }
    num -= digit * i;
  }
  *pString = 0;
  return pString;
}

void FillString(unsigned long range, char* pRangeString)
{
  char numberString[9];
  char* pString = numberString;
  if (range < BIKEWIDTH)
  {
    range = 0;
  }
  else
  {
    range -= BIKEWIDTH;
  }
  range /= DIVISOR;
 
 
  unsigned long major = range / MAJORMINORDIVISOR;
  unsigned long minor = range - (major * MAJORMINORDIVISOR);
 
  if (range > 0)
  {
    pString = Integer2String(major, pString, 1);
    pString = CopyString(pString, g_major);
    pString = Integer2String(minor, pString, 2);
    pString = CopyString(pString, g_minor);
  }
  else
  {
    pString = CopyString(pString, "TooClose");
  }
 
  // Here we right-justify the number
  int length = 0;
  for (; numberString[length] != 0; ++length) {}
  int offset = 16 - length;
  
  CopyString(pRangeString + offset, numberString);
}

void loop() {  
 char topString[17] =    "Closest:        ";
 char bottomString[17] = "Current:        ";
 unsigned long time = millis();
 
 unsigned long range;
 unsigned long closeRange;
 
 digitalWrite(13, HIGH);
 range = rangeFind();
 digitalWrite(13, LOW);
 
 UpdateBucket(range);
 closeRange = GetClosestBucket();
 
 FillString(closeRange, topString);
 FillString(range, bottomString);
 
   
 //lcd.clear();          // Better to overwrite as clearing tends to make the display flicker.
 lcd.cursorTo(1, 0); 
 lcd.printIn(topString);
 lcd.cursorTo(2, 0); 
 lcd.printIn(bottomString);
 
 unsigned long timeThisFrame = millis() - time;
 if (timeThisFrame < 50)
 {
   delay(50 - timeThisFrame); 
 }
}
int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}