#include <phyphoxBle.h> 
#include <Arduino_LSM9DS1.h>
#include <Arduino_LPS22HB.h>
#include <Arduino_HTS221.h>
#include <Arduino_APDS9960.h>

float choice = 0.0;
float accx, accy, accz;
float accx1, accy1, accz1;
float accx2, accy2, accz2;
float accx3, accy3, accz3;
float accx4, accy4, accz4;
float accx5, accy5, accz5;
float acc1, acc2, acc3, acc4, acc5;
float accxmean, accymean, acczmean;
float gyrx, gyry, gyrz;
float magx, magy, magz;
float pressure;
float temperature, humidity;
int red, green, blue, ambient;
float ambient_float;

void receivedData();

void setup()
{
   if (!IMU.begin()) {
     while (1);
   }
   if (!BARO.begin()) {
     while (1);
   }
   if (!HTS.begin()) {
     while (1);
   }
   if (!APDS.begin()) {
    while (1);
   }
   PhyphoxBLE::start();
   PhyphoxBLE::configHandler=&receivedData;
}

void loop()
{
   if(choice == 1.0){
      if (IMU.accelerationAvailable()) {
        IMU.readAcceleration(accx, accy, accz);
        PhyphoxBLE::write(accx, accy, accz);
      }
   }else if(choice == 2.0){
      if (IMU.gyroscopeAvailable()) {
        IMU.readGyroscope(gyrx, gyry, gyrz);
        PhyphoxBLE::write(gyrx, gyry, gyrz);
      }
   }else if(choice == 3.0){
      if (IMU.magneticFieldAvailable()) {
        IMU.readMagneticField(magx, magy, magz);
        PhyphoxBLE::write(magx, magy, magz);
      }
   }else if(choice == 4.0){
      pressure = BARO.readPressure();
      PhyphoxBLE::write(pressure);
   }else if(choice == 5.0){
      temperature = HTS.readTemperature();
      humidity = HTS.readHumidity();
      PhyphoxBLE::write(temperature, humidity);
   }else if(choice == 6.0){
      if (APDS.colorAvailable()) {
        APDS.readColor(red, green, blue, ambient);
        ambient_float = (float)ambient;
        PhyphoxBLE::write(ambient_float);
      }
   }else if(choice == 7.0) {
      if (IMU.accelerationAvailable()) {
        IMU.readAcceleration(accx1, accy1, accz1);
        while(!IMU.accelerationAvailable()){
        }
        IMU.readAcceleration(accx2, accy2, accz2);
        accxmean = (accx1+accx2)/2;
        accymean = (accy1+accy2)/2;
        acczmean = (accz1+accz2)/2;
        PhyphoxBLE::write(accxmean, accymean, acczmean);
      }
   }else if(choice == 8.0) {
      if (IMU.accelerationAvailable()) {
        IMU.readAcceleration(accx1, accy1, accz1);
        acc1 = sqrt(pow(accx1, 2) + pow(accy1, 2) + pow(accz1, 2));
        while(!IMU.accelerationAvailable()){
        }
        IMU.readAcceleration(accx2, accy2, accz2);
        acc2 = sqrt(pow(accx2, 2) + pow(accy2, 2) + pow(accz2, 2));
        while(!IMU.accelerationAvailable()){
        }
        IMU.readAcceleration(accx3, accy3, accz3);
        acc3 = sqrt(pow(accx3, 2) + pow(accy3, 2) + pow(accz3, 2));
        while(!IMU.accelerationAvailable()){
        }
        IMU.readAcceleration(accx4, accy4, accz4);
        acc4 = sqrt(pow(accx4, 2) + pow(accy4, 2) + pow(accz4, 2));
        while(!IMU.accelerationAvailable()){
        }
        IMU.readAcceleration(accx5, accy5, accz5);
        acc5 = sqrt(pow(accx5, 2) + pow(accy5, 2) + pow(accz5, 2));
        while(!IMU.accelerationAvailable()){
        }
        PhyphoxBLE::write(acc1, acc2, acc3, acc4, acc5);
      }
   }else{
   }
}

void receivedData(){
   PhyphoxBLE::read(choice);
}