// This #include statement was automatically added by the Particle IDE.
#include <Adafruit_BNO055_Photon.h>

// This #include statement was automatically added by the Particle IDE.
#include <Adafruit_Sensor.h>

// This #include statement was automatically added by the Particle IDE.
#include <OneWire.h>





/*************************

   Wiring:

   BNO055 -> Particle
   VIN -> VIN
   GND -> GND
   SDA -> D0
   SCL -> D1

   Ported to Particle devices by Nathan Robinson
   https://github.com/nrobinson2000
   On 2017-1-21

*************************/

//SYSTEM_MODE(MANUAL);
SYSTEM_THREAD(ENABLED);

Adafruit_BNO055 bno = Adafruit_BNO055(55);

uint8_t gyro = 0;
uint8_t accel = 0;
uint8_t mag = 0;
uint8_t sys_val;

int gyro_cloud = 0;
int accel_cloud = 0;
int mag_cloud = 0;

int led = D6;
int A=0;
int vibrator = D4;

double acc_x = 0;
double acc_y = 0;
double acc_z = 0;

void setup()
{
        Serial.begin(9600);
        Serial.println("Orientation Sensor Test");
        Serial.println("");
        Particle.variable("Gyro",gyro_cloud);
        Particle.variable("Accel",accel_cloud);
        Particle.variable("Mag",mag_cloud);
        Particle.variable("X", acc_x ); //euler.x());
        Particle.variable("Y", acc_y );//euler.y());
        Particle.variable("Z", acc_z );//euler.z());
        Particle.variable("A",A);
        digitalWrite (led,LOW);
        digitalWrite (vibrator,LOW);
        pinMode (led, OUTPUT);
        pinMode (vibrator, OUTPUT);
                /* Initialize the sensor */
        if(!bno.begin())
        {
                /* There was a problem detecting the BNO055 ... check your connections */
                Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
                while(1) ;
        }

        delay(1000);

        bno.setExtCrystalUse(true);
}

void loop()
{
        /* Get a new sensor event */
        /*sensors_event_t event;
        bno.getEvent(&event);

        /* Display the floating point data */
        /*Serial.print("X: ");
        Serial.print(event.orientation.x, 4);
        Particle.variable("X", event.orientation.x, INT);
        Serial.print("\tY: ");
        Serial.print(event.orientation.y, 4);
        Serial.print("\tZ: ");
        Serial.print(event.orientation.z, 4);
        Serial.println("");

        /* Display calibration status for each sensor. */
        //uint8_t system, gyro, accel, mag = 0;
        bno.getCalibration(&sys_val, &gyro, &accel, &mag);
        Serial.print("CALIBRATION: Sys=");
        Serial.print(sys_val, DEC);
        
        /* Gryo, Accel, Mag Data */
        Serial.print(" Gyro=");
        Serial.print(gyro, DEC);
        //Particle.variable(" Gyro=", gyro, INT);
        Serial.print(" Accel=");
        Serial.print(accel, DEC);
        //Particle.variable(" Accel=", accel, INT);
        Serial.print(" Mag=");
        Serial.println(mag, DEC);
        // Particle.variable(" Mag=", MAG, INT);
        if(acc_x < 260 && acc_x >230){
            digitalWrite (led,HIGH);
            digitalWrite (vibrator, HIGH);
            A=1;
        }
        else
        {
           digitalWrite (led,LOW);
           digitalWrite (vibrator, LOW);
            A=2;
        }
        


        gyro_cloud = (int)(gyro);
        mag_cloud = (int)(mag);
        accel_cloud = (int)(accel);

        imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);

        acc_x = (double) euler.x();
        acc_y = (double) euler.y();
        acc_z = (double) euler.z();

        /* Display the floating point data */
        Serial.print("X: ");
        Serial.print(euler.x());
        //Particle.variable(" X:", euler.x(), INT);
        Serial.print(" Y: ");
        Serial.print(euler.y());
        //Particle.variable(" Y:", euler.y(), INT);
        Serial.print(" Z: ");
        Serial.print(euler.z());
        //Particle.variable(" Z:", euler.z(), INT);
        Serial.print("\t\t");
        Serial.println();
        Serial.println();

        

        delay(500);
}