// ESP32 Dev Module

#include <Wire.h>

// ID kegla mora bit unikaten za vsakega! (se poslje poleg parametrov)
#define KEGEL_ID 1

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

//#include "RTMath.h"


#include <esp_now.h>
#include <WiFi.h>

uint8_t sprejemnikMac[] = {0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C };

typedef struct sensor_msg {
  int id;
  RTFLOAT aX;
  RTFLOAT aY;
  RTFLOAT aZ;
  RTFLOAT qX;
  RTFLOAT qY;
  RTFLOAT qZ;
  RTFLOAT qW;
}sensor_msg;

sensor_msg odcitek;
esp_now_peer_info_t peerInfo;

#define DISPLAY_INTERVAL  5                         // interval between pose displays

// 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

void setup() {
  int errcode;

  // Basic(debug) serial init
  Serial.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;

  // WIFI init
  WiFi.mode(WIFI_STA);
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }

  memcpy(peerInfo.peer_addr, sprejemnikMac, 6);
  peerInfo.channel = 0;  
  peerInfo.encrypt = false;

  if (esp_now_add_peer(&peerInfo) != ESP_OK){
    Serial.println("WIFI registracija ni uspela");
    return;
  }
}

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;
    }

    if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
      lastDisplay = now;

      odcitek.id = KEGEL_ID;
      odcitek.aX = realAccel.x();
      odcitek.aY = realAccel.y();
      odcitek.aZ = realAccel.z();
      odcitek.qX = fusion.getFusionQPose().x();
      odcitek.qY = fusion.getFusionQPose().y();
      odcitek.qZ = fusion.getFusionQPose().z();
      odcitek.qW = fusion.getFusionQPose().scalar();

      esp_err_t result = esp_now_send(sprejemnikMac, (uint8_t *) &odcitek, sizeof(odcitek));

      if (result == ESP_OK) {
        Serial.println("Uspesno poslano");
      } else {
        Serial.println("Napaka pri posiljanju");
      }
    }
  }
}