// ESP32 Dev Module

#include <Wire.h>

// Parameters for device
#define BTNAME "kegel 2"

// IMU libraries
#include "I2Cdev.h"
#include "RTIMUSettings.h"
#include "RTIMU.h"
#include "RTFusionRTQF.h" 
#include "CalLib.h"
#include <EEPROM.h>

#include <OSCBundle.h>
#include <OSCBoards.h>

#define BT_OSC

#define DISPLAY_INTERVAL  5                         // interval between pose displays


// Bluetooth
#ifdef BT_OSC
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif
  
#include <SLIPEncodedSerial.h>
#include "BluetoothSerial.h"
#include "SLIPEncodedBluetoothSerial.h"
  
BluetoothSerial SerialBT;
SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT);
#endif


// SERIAL
#ifdef BOARD_HAS_USB_SERIAL
#include <SLIPEncodedUSBSerial.h>
SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB );
#else
#include <SLIPEncodedSerial.h>
SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial
#endif

// Motion sensor objects
RTIMU *imu;                                           // the IMU object
RTFusionRTQF fusion;                                  // the fusion object
RTIMUSettings settings;                               // the settings object

unsigned long lastDisplay;
unsigned long lastRate;
int sampleCount;
RTQuaternion gravity;

bool reset; // For quaternion calibration

/* OSC MSG channels */
OSCBundle bundle;

void setup() {
  int errcode;
  // Basic(debug) serial init
  SLIPSerial.begin(115200);
  //Serial.begin(115200);   // set this as high as you can reliably run on your platform
  Serial.println("Starting up...");
  
  // Init EEPROM based on magnet calibration size requirement
  EEPROM.begin(512);
  
  // I2C init
  Wire.begin();
  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties

  // create the imu object
  imu = RTIMU::createIMU(&settings);

  Serial.print("ArduinoIMU starting using device "); Serial.println(imu->IMUName());
  if ((errcode = imu->IMUInit()) < 0) {
    Serial.print("Failed to init IMU: "); Serial.println(errcode);
  }

  
  if (imu->getCalibrationValid())
    Serial.println("Using compass calibration");
  else
    Serial.println("No valid compass calibration data");
  
  // Gravity obj
  gravity.setScalar(0);
  gravity.setX(0);
  gravity.setY(0);
  gravity.setZ(1);
/*
  fusion.setSlerpPower(0.02);
  fusion.setGyroEnable(true);
  fusion.setAccelEnable(true);
  fusion.setCompassEnable(true);
  */

  lastDisplay = lastRate = millis();
  sampleCount = 0;

#ifdef BT_OSC
  SerialBT.begin(BTNAME);
#endif
}

void loop() {
  unsigned long now = millis();
  unsigned long delta;
  RTVector3 realAccel;
  RTQuaternion rotatedGravity;
  RTQuaternion fusedConjugate;
  RTQuaternion qTemp;
  int loopCount = 0;

  // get the latest data if ready yet
  while (imu->IMURead()) {          
    // this flushes remaining data in case we are falling behind
    if (++loopCount >= 10)
      continue;

    fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
    
    //  do gravity rotation and subtraction
    
    // create the conjugate of the pose
    fusedConjugate = fusion.getFusionQPose().conjugate();
  
    // now do the rotation - takes two steps with qTemp as the intermediate variable
    qTemp = gravity * fusion.getFusionQPose();
    rotatedGravity = fusedConjugate * qTemp;
  
    // now adjust the measured accel and change the signs to make sense
    realAccel.setX(-(imu->getAccel().x() - rotatedGravity.x()));
    realAccel.setY(-(imu->getAccel().y() - rotatedGravity.y()));
    realAccel.setZ(-(imu->getAccel().z() - rotatedGravity.z()));
  
    sampleCount++;
    if ((delta = now - lastRate) >= 1000) {
    
      //Serial.print("Sample rate: "); Serial.print(sampleCount);
      if (!imu->IMUGyroBiasValid()) {
      //  Serial.println(", calculating gyro bias");
      } else {
      //  Serial.println();
      }
      
      sampleCount = 0;
      lastRate = now;
    }
  
    //RTMath::display("Accel:", realAccel);
    //Serial.println();
  
    // Euler - rotacija
    //eulerVector = eulerFromQuaternion(q);
    //bundle.add("/euler").add(eulerVector(0)).add(eulerVector(1)).add(eulerVector(2)); // X Y Z
    //bundle.add("/euler").add(fusion.getFusionPose().x()).add(fusion.getFusionPose().y()).add(fusion.getFusionPose().z());
    // Quaterion difference - rotacijska razlika (prejsnji reading - trenutni reading)
    //bundle.add("/quaternionDiff").add(diff.w).add(diff.y * -1).add(diff.z).add(diff.x * -1); // W X Y Z
  
    // Rotation diff value in euler angle
    //eulerDiffVector = eulerFromQuaternion(diff);
    //bundle.add("/eulerDiff").add(eulerDiffVector(0)).add(eulerDiffVector(1)).add(eulerDiffVector(2)); // X Y Z

    if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
      lastDisplay = now;
      // Accelerometer
      bundle.add("/accel").add(realAccel.x()).add(realAccel.y()).add(realAccel.z());
      
      // Quaternion - rotacija
      bundle.add("/quaternion").add(fusion.getFusionQPose().scalar()).add(fusion.getFusionQPose().x()).add(fusion.getFusionQPose().y()).add(fusion.getFusionQPose().z()); // W X Y Z

      SLIPSerial.beginPacket();
      bundle.send(SLIPSerial);
      SLIPSerial.endPacket();
  
      #ifdef BT_OSC
        SLIPBTSerial.beginPacket();
        bundle.send(SLIPBTSerial);
        SLIPBTSerial.endPacket();
      #endif
    
      bundle.empty();
    }
  }
}