//only reads right toe accurately, need method to represent accurate left toe

#include <LiquidCrystal.h>
const int rs = 12, en = 11, d4 = 5, d5 = 4, d6 = 3, d7 = 2;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);

#define ADC_ref 5
#define zero 0

unsigned int rawLeft;
unsigned int rawRight;
unsigned int rawBottom;
float distLeft;
float distRight;
float distBottom;
float distTarget;
float toe;
float cam;

void setup()
{

  lcd.clear();
  lcd.begin(16,2);

}

void loop (){
  //sensor input from 0-1023
  rawLeft = analogRead(A0);
  rawRight = analogRead(A1);
  rawBottom = analogRead(A2);
  //calculated distance in inches
  distLeft=(rawLeft/1024.0*ADC_ref+zero)*10.23622047244094;
  distRight=(rawRight/1024.0*ADC_ref+zero)*10.23622047244094;
  distBottom=(rawBottom/1024.0*ADC_ref+zero)*10.23622047244094;
  //calculated angles in degrees
  toe=atan2(24,(distLeft-distRight))*57.2957795-90;
  cam=atan2(8,(distBottom-distRight))*57.2957795-90;
  //calculated distance from target in inches, might need for later
  distTarget=(distLeft+distRight)/2;

  //out of range warning or regular operation, distances in inches
  if (distLeft >= 32 || distLeft <= 4 || distRight >= 32 || distRight <= 4 || distBottom >= 32 || distBottom <= 4)
    { 
      lcd.setCursor(0,0);
      lcd.print("     Out Of     ");
      lcd.setCursor(0,1);
      lcd.print("     Range      ");
      }
      else
      {
      lcd.setCursor(0,0);
      lcd.print("Toe: ");
      lcd.print(abs(toe),3);
        if (toe <0)
          {
            lcd.print(" Out  ");
          }
          else
          {
            lcd.print(" In   ");
          }
      lcd.setCursor(0,1);
      lcd.print("Cam: ");
      lcd.print(cam);
      lcd.print(" deg  ");
      }
}