#include <Arduino.h>
#include <FreeRTOS.h>
#include <task.h>
#include "bno085.hpp"

#define USESDK

#ifdef USESDK
#include "hardware/i2c.h"
#else
#include <Wire.h>
#endif

bool bno085mon::waitForI2C(void) {
#ifdef USESDK
  for (uint8_t counter = 0; counter < 100; counter++) {
    if (i2c_get_read_available(i2c0) > 0)
      return true;
    delay(1);
  }
  return false;
#else
  for (uint8_t counter = 0; counter < 100; counter++) {
    if (Wire.available() > 0)
      return true;
    delay(1);
  }
  return false;
#endif
}

bool bno085mon::IIC_Send(uint8_t addr, uint8_t *dat, int len) {
  bool result = false;
  if (len > 0) {
#ifdef USESDK
    result = (i2c_write_blocking(i2c0, addr, dat, len, false) == len);
#else
    Wire.beginTransmission(addr);
    result = (Wire.write(dat, len) == len);
    Wire.endTransmission();
#endif
  }
  return result;
}

int bno085mon::IIC_Receive(uint8_t addr, uint8_t *dat, int buflen) {
  int result = 0;
  memset(dat, 0, buflen);

/*
  TSHTPHeader *h = (TSHTPHeader *)(void *)dat;
  int l = i2c_read_blocking(i2c0, addr, dat, 500, false);
  l = h->Length & ~0x8000;
  if (l > 0 && l <= buflen) {
    result = l;
  }
*/

  // Since the size is included at the beginning of the packet, the end of the packet is normally
  // determined while reading based on the size, but there is no way to achieve this in the API base.
  // However, there is no way to achieve this based on the API.
  // Also, Pico's behavior was suspicious, so we used blocking.
#ifdef USESDK
  TSHTPHeader *h = (TSHTPHeader *)(void *)dat;
  int l = i2c_read_blocking(i2c0, addr, dat, 4, false);
  l = h->Length & ~0x8000;
  if (l > 0 && l <= buflen) {
    l = i2c_read_blocking(i2c0, addr, dat, l, false);
    if (l > 0) {
      l = h->Length & ~0x8000;
      if (l > 0) result = l;
    }
  }
#else
  TSHTPHeader h = { 0 };
  Wire.requestFrom(addr, 4);
  if (!waitForI2C()) return 0;
  for (int i = 0; i < 4; i++) h.b[i] = Wire.read();
  h.Length &= ~0x8000;
  if (h.Length > 0) {
    Wire.requestFrom(addr, h.Length);
    if (!waitForI2C()) return 0;
    for (int i = 0; i < h.Length && (i + 4 < buflen); i++) dat[i] = Wire.read();
    for (int i = 0; i < 4; i++) h.b[i] = dat[i];
    h.Length &= ~0x8000;
    result = h.Length;
  }
#endif

  return result;
}

// 5 HALs required by SH2 library
//-----------------------------
int bno085mon::shtp_i2c_hal_open(sh2_Hal_t *self_) {
  i2c_hal_t *self = (i2c_hal_t *)self_;

  gpio_init(_I2C0_SCL);
  gpio_set_dir(_I2C0_SCL, GPIO_IN);
  gpio_set_pulls(_I2C0_SCL, true, false);
  if (gpio_get(_I2C0_SCL)) {
    gpio_set_dir(_I2C0_SCL, GPIO_OUT);
    for (int i = 0; i < 16; i++) {
      gpio_put(_I2C0_SCL, 0);
      delayMicroseconds(2);
      gpio_put(_I2C0_SCL, 1);
      delayMicroseconds(2);
    }
  }

  gpio_init(_I2C0_SDA);
  gpio_set_dir(_I2C0_SDA, GPIO_IN);
  gpio_set_pulls(_I2C0_SDA, true, false);
  if (gpio_get(_I2C0_SDA)) {
    gpio_set_dir(_I2C0_SDA, GPIO_OUT);
    for (int i = 0; i < 16; i++) {
      gpio_put(_I2C0_SDA, 0);
      delayMicroseconds(2);
      gpio_put(_I2C0_SDA, 1);
      delayMicroseconds(2);
    }
  }

#ifdef USESDK
  i2c_init(i2c0, 400000);
  gpio_set_function(_I2C0_SDA, GPIO_FUNC_I2C);
  gpio_set_function(_I2C0_SCL, GPIO_FUNC_I2C);
  gpio_set_pulls(_I2C0_SDA, true, false);
  gpio_set_pulls(_I2C0_SCL, true, false);
#else
  gpio_pull_up(_I2C0_SDA);
  gpio_pull_up(_I2C0_SCL);
  Wire.setSDA(_I2C0_SDA);
  Wire.setSCL(_I2C0_SCL);
  Wire.begin();
#endif
  uint8_t softreset_pkt[] = { 5, 0, 1, 0, 1 };
  bool success = false;

  for (uint8_t attempts = 0; attempts < 30; attempts++) {
    success = IIC_Send(self->i2c_addr, &softreset_pkt[0], 5);
    delay(10);
  }
  delay(300);

  return success ? SH2_OK : SH2_ERR;
}

void bno085mon::shtp_i2c_hal_close(sh2_Hal_t *self_) {
  (void)self_;
#ifdef USESDK
  i2c_deinit(i2c0);
#else
  Wire.end();
#endif
}

int bno085mon::shtp_i2c_hal_read(sh2_Hal_t *self_, uint8_t *pBuffer, unsigned len, uint32_t *t) {
  int result = 0;
  i2c_hal_t *self = (i2c_hal_t *)self_;
  *t = micros();
  result = IIC_Receive(self->i2c_addr, &pBuffer[0], len);
  // バスがストールしているのでは？としてIICを再初期化
  /////  if (result == 0) shtp_i2c_hal_open (self_);

  return result;
}

int bno085mon::shtp_i2c_hal_write(sh2_Hal_t *self_, uint8_t *pBuffer, unsigned len) {
  i2c_hal_t *self = (i2c_hal_t *)self_;
  int ret = 0;
  if ((pBuffer == 0) || (len == 0)) return SH2_ERR_BAD_PARAM;
  if (IIC_Send(self->i2c_addr, &pBuffer[0], len)) return len;
  return 0;
}

uint32_t bno085mon::shtp_i2c_hal_getTimeUs(sh2_Hal_t *self_) {
  return micros();
}

sh2_Hal_t *bno085mon::shtp_i2c_hal_init(i2c_hal_t *pHal, bool dfu, uint8_t addr) {
  pHal->dfu = dfu;
  pHal->i2c_addr = addr;

  pHal->sh2_hal.open = _shtp_i2c_hal_open;
  pHal->sh2_hal.close = _shtp_i2c_hal_close;
  pHal->sh2_hal.read = _shtp_i2c_hal_read;
  pHal->sh2_hal.write = _shtp_i2c_hal_write;
  pHal->sh2_hal.getTimeUs = _shtp_i2c_hal_getTimeUs;

  return &pHal->sh2_hal;
}





// Handle non-sensor events from the sensor hub
void bno085mon::eventHandler(void *cookie, sh2_AsyncEvent_t *pEvent) {
  (void)cookie;
  // If we see a reset, set a flag so that sensors will be reconfigured.
  if (pEvent->eventId == SH2_RESET) {
    resetOccurred = true;
  }
}

sh2_Hal_t *bno085mon::sh2_hal_init(void) {
  // Get instance of i2c hal suitable for SH2 on FSP201
  return shtp_i2c_hal_init(&sh2_hal, false, _BNO085_SLAVE_ADDR);
}

bool bno085mon::startReports(void) {
  bool result = true;
  static sh2_SensorConfig_t config;
  int status;
  int sensorId;
  static const int enabledSensors[] = {
    SH2_RAW_ACCELEROMETER,
    SH2_ACCELEROMETER,
    SH2_LINEAR_ACCELERATION,
    SH2_GRAVITY,
    SH2_RAW_GYROSCOPE,
    SH2_GYROSCOPE_CALIBRATED,
    SH2_GYROSCOPE_UNCALIBRATED,
    SH2_RAW_MAGNETOMETER,
    SH2_MAGNETIC_FIELD_CALIBRATED,
    SH2_MAGNETIC_FIELD_UNCALIBRATED,

    SH2_ROTATION_VECTOR,

    SH2_GAME_ROTATION_VECTOR,
    SH2_GEOMAGNETIC_ROTATION_VECTOR,
    /*
    SH2_TAP_DETECTOR,
    SH2_STEP_DETECTOR,
    SH2_STEP_COUNTER,
    SH2_SIGNIFICANT_MOTION,
    SH2_STABILITY_CLASSIFIER,
    SH2_SHAKE_DETECTOR,
    SH2_FLIP_DETECTOR,
    SH2_PICKUP_DETECTOR,
    SH2_STABILITY_DETECTOR,
    SH2_PERSONAL_ACTIVITY_CLASSIFIER,
    SH2_SLEEP_DETECTOR,
    SH2_TILT_DETECTOR,
    SH2_POCKET_DETECTOR,
    SH2_CIRCLE_DETECTOR,
    SH2_HEART_RATE_MONITOR,
    SH2_ARVR_STABILIZED_RV,
    SH2_ARVR_STABILIZED_GRV,
    SH2_GYRO_INTEGRATED_RV,
    SH2_IZRO_MOTION_REQUEST,
    SH2_RAW_OPTICAL_FLOW,
    SH2_DEAD_RECKONING_POSE,
*/
  };

  // These sensor options are disabled or not used in most cases
  config.changeSensitivityEnabled = false;
  config.wakeupEnabled = false;
  config.changeSensitivityRelative = false;
  config.alwaysOnEnabled = false;
  config.sniffEnabled = false;
  config.changeSensitivity = 0;
  config.batchInterval_us = 0;
  config.sensorSpecific = 0;

  // Select a report interval.
  //config.reportInterval_us = 100000;  // microseconds (10 Hz)
  //config.reportInterval_us = 20000;  // microseconds (50 Hz)
  //config.reportInterval_us = 10000;  // microseconds (100 Hz)
  config.reportInterval_us = 2500;  // microseconds (400 Hz)
  //config.reportInterval_us = 1000;   // microseconds (1000 Hz)

  for (int n = 0; n < ARRAY_LEN(enabledSensors); n++) {
    // Configure the sensor hub to produce these reports
    sensorId = enabledSensors[n];
    status = sh2_setSensorConfig(sensorId, &config);
    if (status != 0) {
      result = false;
      //      DBG("Error while enabling sensor %d\n", sensorId);
    }
  }
  // Enable dynamic calibration for A, G and M sensors
  status = sh2_setCalConfig(SH2_CAL_ACCEL | SH2_CAL_GYRO | SH2_CAL_MAG);
  status = sh2_setDcdAutoSave(true);

  return result;
}

void bno085mon::SensoorEvent(const sh2_SensorEvent_t *event) {
  int rc;
  sh2_SensorValue_t value;
  //  float scaleRadToDeg = 180.0 / 3.14159265358;

  rc = sh2_decodeSensorEvent(&value, event);
  float t = value.timestamp / 1000000.0;

  if (rc == SH2_OK) {
    switch (value.sensorId) {
      case SH2_RAW_ACCELEROMETER:
        Sensor.rawAccelerometer.timestamp = t;
        Sensor.rawAccelerometer.status = value.status;
        Sensor.rawAccelerometer.value = value.un.rawAccelerometer;
        break;
      case SH2_ACCELEROMETER:
        Sensor.accelerometer.timestamp = t;
        Sensor.accelerometer.status = value.status;
        Sensor.accelerometer.value = value.un.accelerometer;
        break;
      case SH2_LINEAR_ACCELERATION:
        Sensor.linearAcceleration.timestamp = t;
        Sensor.linearAcceleration.status = value.status;
        Sensor.linearAcceleration.value = value.un.linearAcceleration;
        break;
      case SH2_GRAVITY:
        Sensor.gravity.timestamp = t;
        Sensor.gravity.status = value.status;
        Sensor.gravity.value = value.un.gravity;
        break;
      case SH2_RAW_GYROSCOPE:
        Sensor.rawGyroscope.timestamp = t;
        Sensor.rawGyroscope.status = value.status;
        Sensor.rawGyroscope.value = value.un.rawGyroscope;
        break;
      case SH2_GYROSCOPE_CALIBRATED:
        Sensor.gyroscope.timestamp = t;
        Sensor.gyroscope.status = value.status;
        Sensor.gyroscope.value = value.un.gyroscope;
        break;
      case SH2_GYROSCOPE_UNCALIBRATED:
        Sensor.gyroscopeUncal.timestamp = t;
        Sensor.gyroscopeUncal.status = value.status;
        Sensor.gyroscopeUncal.value = value.un.gyroscopeUncal;
        break;
      case SH2_RAW_MAGNETOMETER:
        Sensor.rawMagnetometer.timestamp = t;
        Sensor.rawMagnetometer.status = value.status;
        Sensor.rawMagnetometer.value = value.un.rawMagnetometer;
        break;
      case SH2_MAGNETIC_FIELD_CALIBRATED:
        Sensor.magneticField.timestamp = t;
        Sensor.magneticField.status = value.status;
        Sensor.magneticField.value = value.un.magneticField;
        break;
      case SH2_MAGNETIC_FIELD_UNCALIBRATED:
        Sensor.magneticFieldUncal.timestamp = t;
        Sensor.magneticFieldUncal.status = value.status;
        Sensor.magneticFieldUncal.value = value.un.magneticFieldUncal;
        break;
      case SH2_ROTATION_VECTOR:
        Sensor.rotationVector.timestamp = t;
        Sensor.rotationVector.status = value.status;
        Sensor.rotationVector.value = value.un.rotationVector;
        break;
      case SH2_GAME_ROTATION_VECTOR:
        Sensor.gameRotationVector.timestamp = t;
        Sensor.gameRotationVector.status = value.status;
        Sensor.gameRotationVector.value = value.un.gameRotationVector;
        break;
      case SH2_GEOMAGNETIC_ROTATION_VECTOR:
        Sensor.geoMagRotationVector.timestamp = t;
        Sensor.geoMagRotationVector.status = value.status;
        Sensor.geoMagRotationVector.value = value.un.geoMagRotationVector;
        break;
        //    case SH2_PRESSURE:
        //    case SH2_AMBIENT_LIGHT:
        //    case SH2_HUMIDITY:
        //    case SH2_PROXIMITY:
        //    case SH2_TEMPERATURE:
        //    case SH2_RESERVED:
      case SH2_TAP_DETECTOR:
        Sensor.tapDetector.timestamp = t;
        Sensor.tapDetector.status = value.status;
        Sensor.tapDetector.value = value.un.tapDetector;
        break;
      case SH2_STEP_DETECTOR:
        Sensor.stepDetector.timestamp = t;
        Sensor.stepDetector.status = value.status;
        Sensor.stepDetector.value = value.un.stepDetector;
        break;
      case SH2_STEP_COUNTER:
        Sensor.stepCounter.timestamp = t;
        Sensor.stepCounter.status = value.status;
        Sensor.stepCounter.value = value.un.stepCounter;
        break;
      case SH2_SIGNIFICANT_MOTION:
        Sensor.sigMotion.timestamp = t;
        Sensor.sigMotion.status = value.status;
        Sensor.sigMotion.value = value.un.sigMotion;
        break;
      case SH2_STABILITY_CLASSIFIER:
        Sensor.stabilityClassifier.timestamp = t;
        Sensor.stabilityClassifier.status = value.status;
        Sensor.stabilityClassifier.value = value.un.stabilityClassifier;
        break;
      case SH2_SHAKE_DETECTOR:
        Sensor.shakeDetector.timestamp = t;
        Sensor.shakeDetector.status = value.status;
        Sensor.shakeDetector.value = value.un.shakeDetector;
        break;
      case SH2_FLIP_DETECTOR:
        Sensor.flipDetector.timestamp = t;
        Sensor.flipDetector.status = value.status;
        Sensor.flipDetector.value = value.un.flipDetector;
        break;
      case SH2_PICKUP_DETECTOR:
        Sensor.pickupDetector.timestamp = t;
        Sensor.pickupDetector.status = value.status;
        Sensor.pickupDetector.value = value.un.pickupDetector;
        break;
      case SH2_STABILITY_DETECTOR:
        Sensor.stabilityDetector.timestamp = t;
        Sensor.stabilityDetector.status = value.status;
        Sensor.stabilityDetector.value = value.un.stabilityDetector;
        break;
      case SH2_PERSONAL_ACTIVITY_CLASSIFIER:
        Sensor.personalActivityClassifier.timestamp = t;
        Sensor.personalActivityClassifier.status = value.status;
        Sensor.personalActivityClassifier.value = value.un.personalActivityClassifier;
        break;
      case SH2_SLEEP_DETECTOR:
        Sensor.sleepDetector.timestamp = t;
        Sensor.sleepDetector.status = value.status;
        Sensor.sleepDetector.value = value.un.sleepDetector;
        break;
      case SH2_TILT_DETECTOR:
        Sensor.tiltDetector.timestamp = t;
        Sensor.tiltDetector.status = value.status;
        Sensor.tiltDetector.value = value.un.tiltDetector;
        break;
      case SH2_POCKET_DETECTOR:
        Sensor.pocketDetector.timestamp = t;
        Sensor.pocketDetector.status = value.status;
        Sensor.pocketDetector.value = value.un.pocketDetector;
        break;
      case SH2_CIRCLE_DETECTOR:
        Sensor.circleDetector.timestamp = t;
        Sensor.circleDetector.status = value.status;
        Sensor.circleDetector.value = value.un.circleDetector;
        break;
      case SH2_HEART_RATE_MONITOR:
        Sensor.heartRateMonitor.timestamp = t;
        Sensor.heartRateMonitor.status = value.status;
        Sensor.heartRateMonitor.value = value.un.heartRateMonitor;
        break;
      case SH2_ARVR_STABILIZED_RV:
        Sensor.arvrStabilizedRV.timestamp = t;
        Sensor.arvrStabilizedRV.status = value.status;
        Sensor.arvrStabilizedRV.value = value.un.arvrStabilizedRV;
        break;
      case SH2_ARVR_STABILIZED_GRV:
        Sensor.arvrStabilizedGRV.timestamp = t;
        Sensor.arvrStabilizedGRV.status = value.status;
        Sensor.arvrStabilizedGRV.value = value.un.arvrStabilizedGRV;
        break;
      case SH2_GYRO_INTEGRATED_RV:
        Sensor.gyroIntegratedRV.timestamp = t;
        Sensor.gyroIntegratedRV.status = value.status;
        Sensor.gyroIntegratedRV.value = value.un.gyroIntegratedRV;
        break;
      case SH2_IZRO_MOTION_REQUEST:
        Sensor.izroRequest.timestamp = t;
        Sensor.izroRequest.status = value.status;
        Sensor.izroRequest.value = value.un.izroRequest;
        break;
      case SH2_RAW_OPTICAL_FLOW:
        Sensor.rawOptFlow.timestamp = t;
        Sensor.rawOptFlow.status = value.status;
        Sensor.rawOptFlow.value = value.un.rawOptFlow;
        break;
      case SH2_DEAD_RECKONING_POSE:
        Sensor.deadReckoningPose.timestamp = t;
        Sensor.deadReckoningPose.status = value.status;
        Sensor.deadReckoningPose.value = value.un.deadReckoningPose;
        break;
        //    case SH2_WHEEL_ENCODER:
      default:
        break;
    }
  }
}

// Handle sensor events.
void bno085mon::sensorHandler(void *cookie, sh2_SensorEvent_t *pEvent) {
  (void)cookie;
  SensoorEvent(pEvent);
}

void bno085mon::begin(void) {
  end();
  if (!initialized) {
    IMURESET = false;
    initialized = false;
    anchor = this;
    xTaskCreate([](void *arg) { static_cast<bno085mon *>(arg)->task_imu(); }, nullptr, 256, anchor, 1, &task_imu_handler);
  }
}

void bno085mon::end(void) {
  if (initialized) {
    vTaskDelete(task_imu_handler);
    sh2_close();
    IMURESET = false;
    initialized = false;
    anchor = nullptr;
  }
}

void bno085mon::task_imu() {
  while (1) {
    pSh2Hal = sh2_hal_init();
    int status = sh2_open(pSh2Hal, _eventHandler, NULL);
    if (status == SH2_OK) {
      initialized = true;
      resetOccurred = false;
      sh2_setSensorCallback(_sensorHandler, NULL);
      startReports();

      portTickType t = xTaskGetTickCount();
      while (1) {
        if (resetOccurred) {
          // Restart the flow of sensor reports
          resetOccurred = false;
          startReports();
        }
        // Service the sensor hub
        sh2_service();
        if (IMURESET) break;
        vTaskDelayUntil(&t, 10);
      }
      sh2_close();
      IMURESET = false;
      initialized = false;
    }
    sh2_close();
    IMURESET = false;
    initialized = false;
  }
}

bno085mon *bno085mon::anchor = nullptr;