#include<Wire.h>
#include <LiquidCrystal_I2C.h>
#define LED_PIN 13
#define TRIG_PIN A0
#define PUSH_PIN A1
const int MPU=0x68; 
LiquidCrystal_I2C lcd(0x27,  16, 2);
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
//Pin connected to ST_CP of 74HC595
int latchPin = 11;
//Pin connected to SH_CP of 74HC595
int clockPin = 9;
////Pin connected to DS of 74HC595
int dataPin = 12;
int32_t leds;

int32_t get_color_bits(char c){
  if (c == 'r') {
    return 0b001;
  } 
  else if (c == 'g') {
    return 0b010;
  }
  else if (c == 'b') {
    return 0b100;
  }
  else if (c == 'y') {
    return 0b011;
  }
  else if (c == 'w') {
    return 0b111;
  }
  else {
    return 0b000;
  }
}

void  setup(){
  Serial.begin(9600);
  pinMode(LED_PIN, OUTPUT);
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  
  Wire.write(0);    
  Wire.endTransmission(true);
  //set pins to output so you can control the shift register
  pinMode(latchPin, OUTPUT);
  pinMode(clockPin, OUTPUT);
  pinMode(dataPin, OUTPUT);
  pinMode(TRIG_PIN, INPUT);
  pinMode(PUSH_PIN, INPUT);
  lcd.init();
  lcd.backlight();
}
void  loop(){
  Serial.println(" ");
  Serial.println("Trigger pin:");
  Serial.println(analogRead(TRIG_PIN));
  Serial.println("Push pin:");
  Serial.println(analogRead(PUSH_PIN));
  digitalWrite(LED_PIN, HIGH);
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,12,true);  
  AcX=Wire.read()<<8|Wire.read();    
  AcY=Wire.read()<<8|Wire.read();  
  AcZ=Wire.read()<<8|Wire.read();  
  Tmp=Wire.read()<<8|Wire.read();  
  GyX=Wire.read()<<8|Wire.read();  
  GyY=Wire.read()<<8|Wire.read();  
  GyZ=Wire.read()<<8|Wire.read();  

  Serial.println("Gyro T  = "); Serial.print(Tmp);
  Serial.println("Accelerometer: ");
  Serial.print("X = "); Serial.print(AcX);
  Serial.print(" | Y = "); Serial.print(AcY);
  Serial.print(" | Z = ");  Serial.println(AcZ); 
  
  Serial.println("Gyroscope: ");
  Serial.print("T  = " + String(Tmp));
  Serial.print(" | X = "); Serial.print(GyX);
  Serial.print(" | Y = "); Serial.print(GyY);
  Serial.print(" | Z = "); Serial.print(GyZ);
  int32_t speed_led = get_color_bits('r');
  int32_t force_led = get_color_bits('y');
  int32_t acc_led = get_color_bits('g');
  int32_t gyro_led = get_color_bits('g');
  int32_t result_led = get_color_bits('y');
  leds = (result_led << 12) | (gyro_led << 9) | (acc_led << 6) | (force_led << 3) | speed_led;
  shiftOut(dataPin, clockPin, MSBFIRST, (leds >> 8) & 0xFF);
  shiftOut(dataPin, clockPin, MSBFIRST, leds & 0xFF); 
  digitalWrite(latchPin, LOW);
  digitalWrite(latchPin, HIGH);
  delay(500);
}