/*
HC-SR04 Ping distance sensor]
VCC to arduino 5v GND to arduino GND
Echo to Arduino pin 13 Trig to Arduino pin 12
Red POS to Arduino pin 11
Green POS to Arduino pin 10
560 ohm resistor to both LED NEG and GRD power rail
More info at: http://goo.gl/kJ8Gl
Original code improvements to the Ping sketch sourced from Trollmaker.com
Some code and wiring inspired by http://en.wikiversity.org/wiki/User:Dstaub/robotcar
*/
/*********************

Example code for the Adafruit Character LCD Shield and Library

This code displays text on the shield, and also reads the buttons on the keypad.
When a button is pressed.

**********************/

// include the library code:
#include <Wire.h>
#include <Adafruit_RGBLCDShield.h>
#include <utility/Adafruit_MCP23017.h>

// this constant won't change.  It's the pin number
// of the sensor's output:
// We will be using two sensors
// one to correct to unknown variables
// which include humidity, barometric pressure, and temperature
uint8_t trigPin, echoPin;
const int AvgDSz = 8;
float duration1, duration2, avg1, avg2, sum1, sum2;
double cm, cal, inches, usec2cm;

// The shield uses the I2C SCL and SDA pins. On classic Arduinos
// this is Analog 4 and 5 so you can't use those for analogRead() anymore
// However, you can connect other I2C sensors to the I2C bus and share
// the I2C bus.
Adafruit_RGBLCDShield lcd = Adafruit_RGBLCDShield();

float AvgC, TotC, TC, tempC, tempF;  // Temperature variables

uint8_t tempPin = 3,  // A3 => analog pin 3
        AvgTSz = 4;  // number of samples for average
uint8_t buttons;

void setup()
{
  trigPin = 0;
  echoPin = 1;

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  pinMode(trigPin+2, OUTPUT);
  pinMode(echoPin+2, INPUT);

  // Debugging output
  //Serial.begin(9600);
  // set up the LCD's number of columns and rows: 
  lcd.begin(16, 2);

  //time = millis() - time;
  //Serial.print("Took "); Serial.print(time); Serial.println(" ms");
  TC = analogRead(tempPin);
  TotC = TC * AvgTSz;  // form an average
  tempC = TC;
}

void loop()
{
  uint8_t buttons = lcd.readButtons();
  TC = analogRead(tempPin);
  TotC -= TotC / AvgTSz;  // make a hole in average
  TotC += TC;               // fill it
  tempC = TotC / AvgTSz;  // get new average
  lcd.setCursor(10, 0);
  lcd.print(tempC,1);
  tempC = (5.0 * tempC * 100.0)/1024.0;
  tempF = ((tempC*9)/5) + 32;

  lcd.setCursor(0, 0);
  lcd.print(tempC,1);
  lcd.print("'C");

  lcd.setCursor(0, 1);
  lcd.print(tempF,1);
  lcd.print("'F");
  
  // Create a calibration check for 8 inches
  duration1 = ReadPing(trigPin);
  // create new average for calibration
  sum1 = sum1 - avg1;
  sum1 = sum1 + duration1;
  avg1 = sum1 / AvgDSz;
  // now use the calibration value
  duration2 = ReadPing(trigPin+2);
  sum2 = sum2 - avg2;
  sum2 = sum2 + duration2;
  avg2 = sum2 / AvgDSz;

  //if (buttons)
  //{
    //lcd.clear();
    //lcd.setCursor(0,0);
    //if (buttons & BUTTON_UP)
    //{
      //lcd.print("UP ");
      //lcd.setBacklight(RED);
    //}
    //if (buttons & BUTTON_DOWN)
    //{
      //lcd.print("DOWN ");
      //lcd.setBacklight(YELLOW);
    //}
    //if (buttons & BUTTON_LEFT)
    //{
      //lcd.print("LEFT ");
      //trigPin = 0;
      //echoPin = 1;
      //lcd.setBacklight(GREEN);
    //}
    //if (buttons & BUTTON_RIGHT)
    //{
      //lcd.print("RIGHT ");
      //trigPin = 2;
      //echoPin = 3;
      //lcd.setBacklight(TEAL);
    //}
    //if (buttons & BUTTON_SELECT)
    //{
      //lcd.print("SELECT ");
      //lcd.setBacklight(VIOLET);
    //}
  //}
  
  // convert the time into a distance
  //lcd.setCursor(11, 0);
  //lcd.print(sum2);
  //lcd.setCursor(11, 1);
  //lcd.print(avg2);
  usec2cm = CentimetersToMicroseconds(avg2);
  cm = microsecondsToCentimeters(avg1, usec2cm);
  // Cal Reading should be 929.088 for 16 cm times 2
  cal = microsecondsToCentimeters(avg2, 29.034);  // check on original distance standard
  inches = cm/2.54;
  
  if (cm >= 200 || cm <= 0)
  {
    lcd.print("Out of range");
  }
  else
  {
    // set the cursor to column 0, line 1
    // (note: line 1 is the second row, since counting begins with 0):
    lcd.setCursor(0, 1);
    lcd.print(inches);
    lcd.print(" in   ");
    lcd.setCursor(8, 1);
    lcd.print(cm);
    lcd.print(" cm   ");
    //lcd.setCursor(8, 1);
    //lcd.print(cal);
  }

  delay(200);
}
double CentimetersToMicroseconds(long usec2)
{
  // using a know fixed distance calculate the usec per cm
  // using 16 cm find the usec
  return usec2 / 32;
}
double microsecondsToCentimeters(long microseconds, double usec)
{
  // The speed of sound is 340 m/s or 29.034 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return (microseconds / 2.0) / usec;
}

long ReadPing(int iPin)
{
  digitalWrite(iPin, LOW);  // Added this line
  delayMicroseconds(2); // Added this line
  digitalWrite(iPin, HIGH);
  delayMicroseconds(10); // Added this line
  digitalWrite(iPin, LOW);
  return (pulseIn(iPin+1, HIGH));  // design to use trigger and echo = trigger +1
}