#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>
#include <LiquidCrystal.h>

int rs=7;
int en=8;
int d4=9;
int d5=10;
int d6=11;
int d7=12;
LiquidCrystal lcd(rs,en,d4,d5,d6,d7);

float q0;  //real part of quaternion (W)
float q1;  //x component of quaternion (x)
float q2;  //y component of quaternion (y)
float q3;  //z component of quaternion (z)

float thetaQ; //pitch calculated from quaternion [degrees]
float phiQ;   //roll calculated from quaternion [degrees]
float psiQ;   //yaw calculated from quaternion [degrees]
float psiQcorr; //yaw corrected to align NORTH to sensor X axis [degrees]


#define BNO055_SAMPLERATE_DELAY_MS (100)   //set sample rate at 10 Hz

Adafruit_BNO055 myIMU = Adafruit_BNO055();  //create IMU object

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);  
  lcd.begin(16,2);
  myIMU.begin();  //start and connect to IMU with default ID and I2C address
  delay(1000);  //delay to give sensor time to boot
  int8_t temp=myIMU.getTemp();
  myIMU.setExtCrystalUse(true);

}

void loop() {
  // put your main code here, to run repeatedly:
  
  uint8_t system, gyro, accel, mg = 0;
  myIMU.getCalibration(&system, &gyro, &accel, &mg);  //get calibration status
  
  // CALIBRATION
  if (accel<3 || mg<3 || gyro<3){
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Please Calibrate");
    lcd.setCursor(0,1);
    lcd.print("A=");
    lcd.print(accel);
    lcd.print("  M=");
    lcd.print(mg);
    lcd.print("  G=");
    lcd.print(gyro);
  
  }  //end if
  
  
   // MEASUREMENT
  if (accel==3 && mg==3 && gyro==3) {
      lcd.clear();
      imu::Quaternion quat=myIMU.getQuat();     //read quaternion from IMU
        
      q0=quat.w();
      q1=quat.x();
      q2=quat.y();
      q3=quat.z();
      thetaQ=-asin(2*(q0*q2-q3*q1))/2/3.141592654*360;                     //calculate pitch angle in degrees
      phiQ=atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2))/2/3.141592654*360;     //calculate roll angle in degrees
      psiQ=-atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3))/2/3.141592654*360;    //calculate yaw angle in degrees

      // Correct azimuth angle to set NORTH in sensor X-direction
      if(psiQ > -180. && psiQ <=90) psiQcorr=psiQ+90.0;
      if(psiQ > 90.0 && psiQ <=180) psiQcorr=psiQ-270.0;
      
   
      lcd.setCursor(0,0);
      lcd.print("Elevation= ");
      lcd.print(round(thetaQ));
      lcd.setCursor(0,1);  
      lcd.print("Azimut= ");
      lcd.print(round(psiQcorr));
     
 
  } //end if
      
 //     Serial.print(accel);
 //     Serial.print(",");
 //     Serial.print(gyro);
 //     Serial.print(",");
 //     Serial.print(mg);
 //     Serial.print(",");
 //     Serial.print(system);
 //     Serial.print(",");
      
 
 //     Serial.print(phiQ);
 //     Serial.print(",");
      

 //     Serial.print(thetaQ);
 //     Serial.print(",");
      
 
 //     Serial.print(psiQcorr);
 //     Serial.println(",");
      
 
      
   delay(BNO055_SAMPLERATE_DELAY_MS);
  } //end loop