#include <LiquidCrystal_I2C.h>

#define DATA_PIN  8  // Pin connected to DS of 74HC595
#define LATCH_PIN 9  // Pin connected to STCP of 74HC595
#define CLOCK_PIN 10 // Pin connected to SHCP of 74HC595

#define NUM_SHIFT_REGS 2

const uint8_t numOfRegisterPins = NUM_SHIFT_REGS * 8;

bool registers[numOfRegisterPins];

LiquidCrystal_I2C lcd(0x27,20,4);
// set the LCD address to 0x27 for a 16 chars and 2 line display

float cm;
float inches;

long readUltrasonicDistance(int triggerPin, int echoPin)
{
	pinMode(triggerPin, OUTPUT);  // Clear the trigger
	digitalWrite(triggerPin, LOW);
	delayMicroseconds(2);
	// Sets the trigger pin to HIGH state for 10 microseconds
	digitalWrite(triggerPin, HIGH);
	delayMicroseconds(10);
	digitalWrite(triggerPin, LOW);
	pinMode(echoPin, INPUT);
	// Reads the echo pin, and returns
	// the sound wave travel time in microseconds
	return pulseIn(echoPin, HIGH);
}



void setup()
{
  Serial.begin(9600);
  lcd.init();	// initialize the lcd
  // Print a message to the LCD.
  lcd.backlight();
  lcd.print("--> Distance <--");
	delay(3000);
	lcd.clear();

	pinMode(DATA_PIN, OUTPUT);
  pinMode(CLOCK_PIN, OUTPUT);
  pinMode(LATCH_PIN, OUTPUT);
  clearRegisters();
  writeRegisters();
}

bool toggle = false;

void loop()
{
  cm = 0.0344/2 * readUltrasonicDistance(3, 2);
	inches = (cm / 2.54);
  /*
	Serial.print("Inches ");
	Serial.print(inches, 1);
	Serial.print("\t");
  Serial.print("cm ");
	Serial.println(cm, 1);
	*/
	lcd.setCursor(0,0);
	lcd.print("Inches");
	lcd.setCursor(4,0);
	lcd.setCursor(12,0);
	lcd.print("cm");
	lcd.setCursor(1,1);
	lcd.print(inches, 1);
	lcd.setCursor(11,1);
	lcd.print(cm, 1);
	lcd.setCursor(14,1);
	delay(2000);
	lcd.clear();

	for (uint8_t i = 0; i < 16; i++) {
    if (i % 2 == 0) {
      setRegisterPin(i, toggle);
    } else {
      setRegisterPin(i, !toggle);
    }
    writeRegisters();
  }
  toggle = !toggle;
  delay(500);
	
}

	void clearRegisters() {
  // Reset all register pins
  for (int i = numOfRegisterPins - 1; i >= 0; i--) {
    registers[i] = LOW;
  }
}

void setRegisterPin(int index, int value) {
  // Set an individual pin HIGH or LOW
  registers[index] = value;
}

void writeRegisters() {
  // Set and display registers
  digitalWrite(LATCH_PIN, LOW);

  for (int i = numOfRegisterPins - 1; i >= 0; i--) {
    digitalWrite(CLOCK_PIN, LOW);
    digitalWrite(DATA_PIN, registers[i]);
    digitalWrite(CLOCK_PIN, HIGH);
  }

  digitalWrite(LATCH_PIN, HIGH);
}