/*
The sensor outputs provided by the library are the raw
16-bit values obtained by concatenating the 8-bit high and
low accelerometer and gyro data registers. They can be
converted to units of g and dps (degrees per second) using
the conversion factors specified in the datasheet for your
particular device and full scale setting (gain).

Example: An LSM6DS33 gives an accelerometer Z axis reading
of 16276 with its default full scale setting of +/- 2 g. The
LA_So specification in the LSM6DS33 datasheet (page 15)
states a conversion factor of 0.061 mg/LSB (least
significant bit) at this FS setting, so the raw reading of
16276 corresponds to 16276 * 0.061 = 992.8 mg = 0.9928 g.
*/



#include <LSM6.h> // Include LSM6.h





LSM6 imu; // Create an LSM6 object called imu


// Store the roll and pitch values
float roll = 0;
float pitch = 0;

// Pins for LEDs
int red = 3;
int yellow = 4;
int green = 5;




// Setup function
void setup()
{
  
  Serial.begin(9600); // Initiate serial communication

  // Set LED pins as outputs
  pinMode(red, OUTPUT);
  pinMode(yellow, OUTPUT);
  pinMode(green, OUTPUT);

  // Check if IMU is working and print an error message if not
  if (!imu.init())
  {
    Serial.println("Failed to detect and initialize IMU!");
    while (1); // do nothing
  }
  imu.enableDefault();

  

  
}

void loop()
{
  imu.read(); // Read values from the accelerometer

  double X_out = imu.a.x/256; // X-axis value
  //For a range of +-2g, we need to divide the raw values by 256, according to the datasheet
  double Y_out = imu.a.y/256; // Y-axis value

  double Z_out = imu.a.z/256; // Z-axis value


  // Calculate Roll and Pitch (rotation around X-axis, rotation around Y-axis)
  double roll = atan(Y_out / sqrt(pow(X_out, 2) + pow(Z_out, 2))) * 180 / PI; // 
  double pitch = atan(-1 * X_out / sqrt(pow(Y_out, 2) + pow(Z_out, 2))) * 180 / PI;


  // Print the roll and pitch values
  Serial.print(roll);
  Serial.print("/");
  Serial.println(pitch);


  if((0 <= pitch and pitch <= 30) or (-90 <= pitch and pitch <= -60)){ // If the roll is between 0 and 30 or -90 and -60, turn the red LED on the and the yellow and green off
    digitalWrite(red, 1);
    digitalWrite(yellow, 0);
    digitalWrite(green, 0);    
  }

  else if((30 < pitch and pitch < 60) or (-60 < pitch and pitch < -30)){ // If the roll is between 30 and 60 or -60 and -30, turn the yellow LED on the and the red and green off
    digitalWrite(red, 0);
    digitalWrite(yellow, 1);
    digitalWrite(green, 0);    
  }

  else if((60 < pitch and pitch < 90) or (-30 < pitch and pitch < 0)){ // If the roll is between 60 and 90 or -30 and 0, turn the green LED on the and the yellow and red off
    digitalWrite(red, 0);
    digitalWrite(yellow, 0);
    digitalWrite(green, 1);    
  }


  delay(10); // repeat every 10 milliseconds
}