#include "Wire.h"
#include <MPU6050_light.h>
#include <LiquidCrystal.h>

MPU6050 mpu(Wire);
unsigned long timer = 0;
// initialize the library by associating any needed LCD interface pin
// with the Arduino pin number it is connected to
const int rs = 7, en = 8, d4 = 9, d5 = 10, d6 = 11, d7 = 12;

LiquidCrystal lcd(7,8,9,10,11,12);
//                rs,e,d4,d5,d6,d7

//  levelLED
int levelLED_bb20 = 6;
int levelLED_bb15 = 5;
int levelLED_bb00 = 4;
int levelLED_sb15 = 3;
int levelLED_sb20 = 2;
// relay 
int relay = 13;


void setup() {
//set up the lcd number of coloms and rows;
 lcd.begin(16,2);
  Serial.begin(9600);
  Wire.begin();
  // Get tilt angles on X and Y, and rotation angle on Z;
  byte status = mpu.begin();
  lcd.print(F("MPU6050 status: "));
  lcd.println(status);
  while(status!=0){ } // stop everything if could not connect to MPU6050
  
  lcd.println(F("Calculating offsets, do not move MPU6050"));
  delay(1000);
  // mpu.upsideDownMounting = true; // uncomment this line if the MPU6050 is mounted upside-down
  mpu.calcOffsets(); // gyro and accelero
  lcd.println("Done!\n");

// set levelLED
pinMode(levelLED_bb20, OUTPUT);
pinMode(levelLED_bb15, OUTPUT);
pinMode(levelLED_bb00, OUTPUT);
pinMode(levelLED_sb15, OUTPUT);
pinMode(levelLED_sb20, OUTPUT);

// set relay
pinMode(relay, OUTPUT);


}

void loop() {
  // put your main code here, to run repeatedly:
  mpu.update();
  lcd.clear();  

  lcd.setCursor(0,0);
  lcd.print("Pitch : ");
  
  
  lcd.setCursor(0,1);
  lcd.print("Roll : ");
  mpu.update();
  
   if((millis()-timer)>1000){ // print data every 1000ms
  lcd.setCursor(7,0);
  lcd.print(mpu.getAngleX());
  lcd.setCursor(7,1);
  lcd.print(mpu.getAngleY());


  // check angles for levelLEDS
  if(mpu.getAngleY() <-19){
    // turn on level led 20graden bakboord
    digitalWrite(levelLED_bb20, HIGH);
    digitalWrite(levelLED_bb15, HIGH);
    digitalWrite(levelLED_bb00, LOW);
    digitalWrite(levelLED_sb15, LOW);
    digitalWrite(levelLED_sb20, LOW);

  }else   if((mpu.getAngleY() >-19) && (mpu.getAngleY()<-1)){
    // turn on level led 15graden bakboord
    digitalWrite(levelLED_bb20, LOW);
    digitalWrite(levelLED_bb15, HIGH);
    digitalWrite(levelLED_bb00, HIGH);
    digitalWrite(levelLED_sb15, LOW);
    digitalWrite(levelLED_sb20, LOW);
//
    }else   if((mpu.getAngleY() >=-5) && (mpu.getAngleY()<=5)){
    // turn on level led recht
    digitalWrite(levelLED_bb20, LOW);
    digitalWrite(levelLED_bb15, LOW);
    digitalWrite(levelLED_bb00, HIGH);
    digitalWrite(levelLED_sb15, LOW);
    digitalWrite(levelLED_sb20, LOW);


  }else   if((mpu.getAngleY() >19) && (mpu.getAngleY()<1)){
    // turn on level led 15graden stuurboord
    digitalWrite(levelLED_bb20, LOW);
    digitalWrite(levelLED_bb15, LOW);
    digitalWrite(levelLED_bb00, HIGH);
    digitalWrite(levelLED_sb15, HIGH);
    digitalWrite(levelLED_sb20, LOW);


  }else   if(mpu.getAngleY() >19) {
    // turn on level led 20graden stuurboord
    digitalWrite(levelLED_bb20, LOW);
    digitalWrite(levelLED_bb15, LOW);
    digitalWrite(levelLED_bb00, LOW);
    digitalWrite(levelLED_sb15, HIGH);
    digitalWrite(levelLED_sb20, HIGH);
  }
   if(mpu.getAngleY() <-19){
     // turn on relay
    digitalWrite(relay, HIGH);}

      if(mpu.getAngleY() <19){
     // turn on relay
    digitalWrite(relay, HIGH);}



   }

  }