uint8_t ping = 3; // Trigger Pin of Ultrasonic Sensor
uint8_t echo = 2; // Echo Pin of Ultrasonic Sensor

uint8_t lcd_D7 = 7; //LCD Data pins 4-7
uint8_t lcd_D6 = 6;
uint8_t lcd_D5 = 5;
uint8_t lcd_D4 = 4;

uint8_t lcd_RS = 11; //LCD write vs instructions
uint8_t lcd_E  = 12; //LCD enable pin

//Instruction Codes: found in datasheet of LCD
#define lineOne 0x00 
#define lineTwo 0x40

#define lcd_ClearDisplay    0b00000001 //delay of 5 ms required
#define lcd_ReturnHome      0b00000010 //delay of 5 ms required
#define lcd_EntryModeSet    0b00000110 //delay of 80 µs required
#define lcd_DisplayON       0b00001100 //No delay required
#define lcd_DisplayOFF      0b00001000 //No delay required
#define lcd_FunctionReset   0b00110000 //delay of 80 µs required
#define lcd_FunctionSet4Bit 0b00101000 //delay of 80 µs required
#define lcd_SetCursor       0b10000000 //delay of 80 µs required 

void setup() {
  pinMode(ping, OUTPUT);
  pinMode(echo, INPUT);

  pinMode(lcd_D7, OUTPUT);
  pinMode(lcd_D6, OUTPUT);
  pinMode(lcd_D5, OUTPUT);
  pinMode(lcd_D4, OUTPUT);
  
  pinMode(lcd_RS, OUTPUT);
  pinMode(lcd_E , OUTPUT);

  lcd_Set4BitMode();
}

void loop() {
   long duration, in, cm;
   char in_ch[10], cm_ch[10];
   
   digitalWrite(ping, LOW); //short low pulse to ensure correct functionality
   delayMicroseconds(2);
   digitalWrite(ping, HIGH); //10us high pulse triggers the sensor
   delayMicroseconds(10);
   digitalWrite(ping, LOW);
   
   duration = pulseIn(echo, HIGH); //records duration of pulse in microseconds

   lcd_WriteInstruction(lcd_ClearDisplay);
   delay(5);
   
   if(duration > 130000) //When the pulse doesn't return, the duration returns a very large number which is recognized as failure
   {
    lcd_WriteInstruction(lcd_SetCursor | lineOne);
    delayMicroseconds(80);

    /*The WriteString function requires an input of type uint8_t*. Since we're passing in a char array
     *and char and uint8_t are the same size there is no risk in lying to the compiler and simply 
     *casting one as the other.*/
    lcd_WriteString((uint8_t*)"error"); 

    lcd_WriteInstruction(lcd_SetCursor | lineTwo);
    delayMicroseconds(80);

    lcd_WriteString((uint8_t*)"out of range");
   }
   else
   { 
     //constants found in datasheet of sensor
     in = duration / 148;
     itoa(in, in_ch, 10); //itoa converts a long into an array of ascii characters

     lcd_WriteString((uint8_t*)in_ch);
     lcd_WriteString((uint8_t*)" in    ");
     
     lcd_WriteInstruction(lcd_SetCursor | lineTwo);
     delayMicroseconds(80);
    
     cm = duration / 58;
     itoa(cm, cm_ch, 10);

     lcd_WriteString((uint8_t*)cm_ch);
     lcd_WriteString((uint8_t*)" cm    ");
     
     lcd_WriteInstruction(lcd_SetCursor | lineOne);
     delayMicroseconds(80);
     //print numbers
     delay(100);
   }
}

//Functions for the operation of the LCD

//initializes the LCD to 4 bit mode
void lcd_Set4BitMode(void)
{
  delay(100); //boot up delay

  digitalWrite(lcd_RS, LOW); //LCD is set to write instructions
  digitalWrite(lcd_E, LOW);

  //begin reset sequence
  lcd_Write4Bits(lcd_FunctionReset);
  delay(10);

  lcd_Write4Bits(lcd_FunctionReset);
  delayMicroseconds(200);

  lcd_Write4Bits(lcd_FunctionReset);
  delayMicroseconds(200);

  lcd_Write4Bits(lcd_FunctionSet4Bit); //The LCD will recognize the first 4 bits of the instruction to mean that it is in 4 bit mode
  delayMicroseconds(80);

  lcd_WriteInstruction(lcd_FunctionSet4Bit); //Now that the LCD knows, the rest of the information sets the lines and font to be used
  delayMicroseconds(80);

  lcd_WriteInstruction(lcd_DisplayOFF);
  delayMicroseconds(80);

  lcd_WriteInstruction(lcd_ClearDisplay);
  delay(5);

  lcd_WriteInstruction(lcd_EntryModeSet);
  delayMicroseconds(80);

  lcd_WriteInstruction(lcd_DisplayON);
  delayMicroseconds(80);
}

//Writes a string of type uint8_t to the LCD. No delay required
void lcd_WriteString(uint8_t string[])
{
  /* If this looks just like a for loop, that's because I originally tried using a for loop
   * but for whatever reason it wouldn't work. Only making this dumb while loop got it to work */
  uint8_t i = 0;
  while(string[i] != 0)
  {
    lcd_WriteCharacter(string[i]);
    i++;
    
    delayMicroseconds(80);
  }
}

//Writes a full byte of information as a character command. No delay required.
void lcd_WriteCharacter(uint8_t data)
{
  digitalWrite(lcd_RS, HIGH); //lcd is in write character mode
  digitalWrite(lcd_E, LOW);
  lcd_Write4Bits(data);      //write the first 4 bits
  lcd_Write4Bits(data << 4); //write the last 4 bits
  
}

void lcd_WriteInstruction(uint8_t data)
{
  digitalWrite(lcd_RS, LOW); //lcd is in write instruction mode
  digitalWrite(lcd_E, LOW);
  lcd_Write4Bits(data);      //write the first 4 bits
  lcd_Write4Bits(data << 4); //write the last 4 bits
  
}

//writes the upper 4 bits of information to the data lines and sends it to the arduino. No delay required
void lcd_Write4Bits(uint8_t nibble)
{
  if(nibble & 1<<7) //if the first bit is HIGH
    digitalWrite(lcd_D7, HIGH);
  else
    digitalWrite(lcd_D7, LOW);

  if(nibble & 1<<6) //if the second bit is HIGH
    digitalWrite(lcd_D6, HIGH);
  else
    digitalWrite(lcd_D6, LOW);
    
  if(nibble & 1<<5) //if the third bit is HIGH
    digitalWrite(lcd_D5, HIGH);  
  else
    digitalWrite(lcd_D5, LOW);
    
  if(nibble & 1<<4) //if the fourth bit is HIGH
    digitalWrite(lcd_D4, HIGH);
  else
    digitalWrite(lcd_D4, LOW);

  //to print the data on the LCD E is pulsed high and low quickly
  digitalWrite(lcd_E, HIGH);
  delayMicroseconds(1);
  digitalWrite(lcd_E, LOW);
  delayMicroseconds(1);
}