/*

  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 11)

  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 <Wire.h>

#include <LSM6.h>



int ledR = 9;           // the PWM pin the LED is attached to

int ledG = 6;           // the PWM pin the LED is attached to

int ledB = 10;           // the PWM pin the LED is attached to






int gyro2LedX = 0;  //Red being fed into gyroX r
int gyro2LedY = 0; //Green being fed into gyroY g
int gyro2LedZ = 0; //Blue being fed into gyroZ b


int brightnessX = 0; //brightness for X
int brightnessY = 0; //brightness for Y
int brightnessZ = 0; //brightness for Z

int fadeAmount = 45;    // how many points to fade the LED by


LSM6 imu;




char report[80]; //



void setup()

{



  pinMode(ledR, OUTPUT);

  pinMode(ledG, OUTPUT);

  pinMode(ledB, OUTPUT);



  // pinMode(ledR_a, OUTPUT);
  // pinMode(ledB_a, OUTPUT);



  Serial.begin(9600);

  Wire.begin();




  if (!imu.init())

  {

    Serial.println("Failed to detect and initialize IMU!");

    while (1);

  } else {
    Serial.println("IMU OK!");
    
  }

  imu.enableDefault();

}



void loop()

{

  imu.read();


  int gyroX = imu.a.x;

  int gyroY = imu.a.y;

  int gyroZ = imu.a.z;


  int gyroX_a = imu.a.x;
  int gyroY_a = imu.a.y;
  int gyroZ_a = imu.a.z;



  gyroX = map(gyroX, -20000, 20000, 0, 255);

  gyroY = map(gyroY, -20000, 20000, 0, 255);

  gyroZ = map(gyroZ, -20000, 20000, 0, 255);


  //gyroX_a = map(gyroX_a, -20000, 20000, 0, 255);

  //gyroY_a = map(gyroY_a, -20000, 20000, 0, 255);

  //gyroZ_a = map(gyroZ_a, -20000, 20000, 0, 255);



  // Serial.println((imu.a.x), (imu.a.y), (imu.a.z));

  Serial.print("GyroX:");
  Serial.println(gyroX);

  Serial.print("GyroY:");
  Serial.println(gyroY);

  Serial.print("GyroZ:");
  Serial.println(gyroZ);



  /*

    Serial.print("GyroX_a:");
    Serial.println(gyroX_a);

    Serial.print("GyroY_a:");
    Serial.println(gyroY_a);

    Serial.print("GyroZ_a:");
    Serial.println(gyroZ_a);
  */



  gyro2LedX = gyroX;
  gyro2LedY = gyroY;
  gyro2LedZ = gyroZ;


  brightnessX = gyro2LedX;
  brightnessY = gyro2LedY;
  brightnessZ = gyro2LedZ;


  // change the brightness for next time through the loop:

  brightnessX = brightnessX + fadeAmount;
  brightnessY = brightnessY + fadeAmount;
  brightnessZ = brightnessZ + fadeAmount;



  // reverse the direction of the fading at the ends of the fade for X,Y,Z:

  if (brightnessX <= 0 || brightnessX >= 255)
  {

    fadeAmount = -fadeAmount;
  }


  if (brightnessY <= 0 || brightnessY >= 255)
  {

    fadeAmount = -fadeAmount;
  }


  if (brightnessZ <= 0 || brightnessZ >= 255)
  {

    fadeAmount = -fadeAmount;
  }


  // set the brightness of pin 9, 6, 10:

  analogWrite(ledR, brightnessX);

  analogWrite(ledG, brightnessY);

  analogWrite(ledB, brightnessZ);


  //      analogWrite(ledR_a, gyroX_a);

  //  analogWrite(ledB_a, gyroY_a);

  //   analogWrite(ledR_a, gyroZ_a);



  delay(150);



  void red();
  {
    gyro2LedX = 255;
    gyro2LedY = 0;
    gyro2LedZ = 0;
  }

  void green();
  {
    gyro2LedX = 0;
    gyro2LedY = 255;
    gyro2LedZ = 0;
  }

  void blue();
  {
    gyro2LedX = 0;
    gyro2LedY = 0;
    gyro2LedZ = 255;
  }

  void purple();
  {
    gyro2LedX = 141;
    gyro2LedY = 62;
    gyro2LedZ = 255;
  }

  void yellow();
  {
    gyro2LedX = 232;
    gyro2LedY = 219;
    gyro2LedZ = 64;
  }

  void aqua();
  {
    gyro2LedX = 75;
    gyro2LedY = 255;
    gyro2LedZ = 250;
  }

  void orange();
  {
    gyro2LedX = 255;
    gyro2LedY = 83;
    gyro2LedZ = 13;
  }

  void white();
  {
    gyro2LedX = 0;
    gyro2LedY = 0;
    gyro2LedZ = 0;
  }

}