/*
  Acquire sensor data using the library provided by CEVA, Inc. for BNO08x.
  The BNO08x data output method is perverse and troublesome to acquire.
  The task is generated in a deliberate manner for the data collection loop.

  Copyright (C) 2023 T.Uemitsu
*/

#pragma once

#include <Arduino.h>
#include <task.h>
extern "C" {
#include "src/sh2.h"
#include "src/sh2_err.h"
#include "src/sh2_hal.h"
#include "src/sh2_SensorValue.h"
#include "src/sh2_util.h"
#include "src/shtp.h"
}

class bno085mon {
  const uint8_t _BNO085_SLAVE_ADDR = 0x4A;
  const uint8_t _I2C0_SDA = 0;
  const uint8_t _I2C0_SCL = 1;

  bool initialized;

  static bno085mon *anchor;

  // Header section of received packet
  typedef union {
    struct {
      uint16_t Length;
      uint8_t Channel;
      uint8_t SeqNum;
    } __attribute__((packed));
    uint8_t b[4];
  } TSHTPHeader;

  typedef struct i2c_hal_s {
    sh2_Hal_t sh2_hal;  // Must be first so we can cast (sh2_hal_t *) to (i2c_hal_t *)
    bool dfu;
    uint8_t i2c_addr;
  } i2c_hal_t;

  bool resetOccurred = false;
  i2c_hal_t sh2_hal;
  sh2_Hal_t *pSh2Hal = nullptr;
  xTaskHandle task_imu_handler;

  bool IIC_Send(uint8_t addr, uint8_t *dat, int len);
  bool waitForI2C(void);
  int IIC_Receive(uint8_t addr, uint8_t *dat, int buflen);

  int shtp_i2c_hal_open(sh2_Hal_t *self_);
  void shtp_i2c_hal_close(sh2_Hal_t *self_);
  int shtp_i2c_hal_read(sh2_Hal_t *self_, uint8_t *pBuffer, unsigned len, uint32_t *t);
  int shtp_i2c_hal_write(sh2_Hal_t *self_, uint8_t *pBuffer, unsigned len);
  uint32_t shtp_i2c_hal_getTimeUs(sh2_Hal_t *self_);
  sh2_Hal_t *shtp_i2c_hal_init(i2c_hal_t *pHal, bool dfu, uint8_t addr);

  static int _shtp_i2c_hal_open(sh2_Hal_t *self_) { return anchor->shtp_i2c_hal_open (self_); }
  static void _shtp_i2c_hal_close(sh2_Hal_t *self_) { anchor->shtp_i2c_hal_close (self_); }
  static int _shtp_i2c_hal_read(sh2_Hal_t *self_, uint8_t *pBuffer, unsigned len, uint32_t *t) { return anchor->shtp_i2c_hal_read (self_, pBuffer, len, t); }
  static int _shtp_i2c_hal_write(sh2_Hal_t *self_, uint8_t *pBuffer, unsigned len) { return anchor->shtp_i2c_hal_write(self_, pBuffer, len); }
  static uint32_t _shtp_i2c_hal_getTimeUs(sh2_Hal_t *self_) { return anchor->shtp_i2c_hal_getTimeUs (self_); }

  sh2_Hal_t *sh2_hal_init(void);
  void eventHandler(void *cookie, sh2_AsyncEvent_t *pEvent);
  void SensoorEvent(const sh2_SensorEvent_t *event);
  void sensorHandler(void *cookie, sh2_SensorEvent_t *pEvent);
  bool startReports(void);

  static void _eventHandler(void *cookie, sh2_AsyncEvent_t *pEvent) { anchor->eventHandler(cookie, pEvent); }
  static void _SensoorEvent(const sh2_SensorEvent_t *event) { anchor->SensoorEvent(event); }
  static void _sensorHandler(void *cookie, sh2_SensorEvent_t *pEvent) { anchor->sensorHandler(cookie, pEvent); }

  //void task_imu();
  void task_imu();

public:
  typedef struct {
    struct { float timestamp; uint8_t status; sh2_RawAccelerometer_t value; } rawAccelerometer;
    struct { float timestamp; uint8_t status; sh2_Accelerometer_t value; } accelerometer;
    struct { float timestamp; uint8_t status; sh2_Accelerometer_t value; } linearAcceleration;
    struct { float timestamp; uint8_t status; sh2_Accelerometer_t value; } gravity;
    struct { float timestamp; uint8_t status; sh2_RawGyroscope_t value; } rawGyroscope;
    struct { float timestamp; uint8_t status; sh2_Gyroscope_t value; } gyroscope;
    struct { float timestamp; uint8_t status; sh2_GyroscopeUncalibrated_t value; } gyroscopeUncal;
    struct { float timestamp; uint8_t status; sh2_RawMagnetometer_t value; } rawMagnetometer;
    struct { float timestamp; uint8_t status; sh2_MagneticField_t value; } magneticField;
    struct { float timestamp; uint8_t status; sh2_MagneticFieldUncalibrated_t value; } magneticFieldUncal;
    struct { float timestamp; uint8_t status; sh2_RotationVectorWAcc_t value; } rotationVector;
    struct { float timestamp; uint8_t status; sh2_RotationVector_t value; } gameRotationVector;
    struct { float timestamp; uint8_t status; sh2_RotationVectorWAcc_t value; } geoMagRotationVector;
//    sh2_Pressure_t pressure;
//    sh2_AmbientLight_t ambientLight;
//    sh2_Humidity_t humidity;
//    sh2_Proximity_t proximity;
//    sh2_Temperature_t temperature;
//    sh2_Reserved_t reserved;
    struct { float timestamp; uint8_t status; sh2_TapDetector_t value; } tapDetector;
    struct { float timestamp; uint8_t status; sh2_StepDetector_t value; } stepDetector;
    struct { float timestamp; uint8_t status; sh2_StepCounter_t value; } stepCounter;
    struct { float timestamp; uint8_t status; sh2_SigMotion_t value; } sigMotion;
    struct { float timestamp; uint8_t status; sh2_StabilityClassifier_t value; } stabilityClassifier;
    struct { float timestamp; uint8_t status; sh2_ShakeDetector_t value; } shakeDetector;
    struct { float timestamp; uint8_t status; sh2_FlipDetector_t value; } flipDetector;
    struct { float timestamp; uint8_t status; sh2_PickupDetector_t value; } pickupDetector;
    struct { float timestamp; uint8_t status; sh2_StabilityDetector_t value; } stabilityDetector;
    struct { float timestamp; uint8_t status; sh2_PersonalActivityClassifier_t value; } personalActivityClassifier;
    struct { float timestamp; uint8_t status; sh2_SleepDetector_t value; } sleepDetector;
    struct { float timestamp; uint8_t status; sh2_TiltDetector_t value; } tiltDetector;
    struct { float timestamp; uint8_t status; sh2_PocketDetector_t value; } pocketDetector;
    struct { float timestamp; uint8_t status; sh2_CircleDetector_t value; } circleDetector;
    struct { float timestamp; uint8_t status; sh2_HeartRateMonitor_t value; } heartRateMonitor;
    struct { float timestamp; uint8_t status; sh2_RotationVectorWAcc_t value; } arvrStabilizedRV;
    struct { float timestamp; uint8_t status; sh2_RotationVector_t value; } arvrStabilizedGRV;
    struct { float timestamp; uint8_t status; sh2_GyroIntegratedRV_t value; } gyroIntegratedRV;
    struct { float timestamp; uint8_t status; sh2_IZroRequest_t value; } izroRequest;
    struct { float timestamp; uint8_t status; sh2_RawOptFlow_t value; } rawOptFlow;
    struct { float timestamp; uint8_t status; sh2_DeadReckoningPose_t value; } deadReckoningPose;
//    sh2_WheelEncoder_t wheelEncoder;
  } __attribute__((packed)) Tbno085;

  bool IMURESET;

  Tbno085 Sensor;
  void begin(void);
  void end(void);
};