#include <Arduino.h>
#include <SPI.h>
#include <U8g2lib.h>
#include <Wire.h>
#include <XPowersLib.h>

#include "Boards.h"
#include "SensorQMI8658.hpp"
#include "tbeam_supreme_adapter.h"

namespace {

constexpr uint32_t kUiUpdateIntervalMs = 100;
constexpr uint32_t kSplashDelayMs = 1200;

U8G2_SH1106_128X64_NONAME_F_HW_I2C g_oled(U8G2_R0, U8X8_PIN_NONE);
XPowersLibInterface* g_pmu = nullptr;
SensorQMI8658 g_qmi;
IMUdata g_accel{0.0f, 0.0f, 0.0f};
IMUdata g_gyro{0.0f, 0.0f, 0.0f};

bool g_sensorReady = false;
bool g_displayReady = false;

uint32_t g_lastUiUpdateMs = 0;
uint32_t g_lastSampleMicros = 0;
float g_lastSamplePeriodMs = 0.0f;
uint32_t g_sampleCount = 0;
float g_temperatureC = 0.0f;
uint32_t g_sensorTimestamp = 0;

void initDisplay() {
  Wire.begin(OLED_SDA, OLED_SCL);
  g_oled.begin();
  g_oled.setI2CAddress(OLED_ADDR << 1);
  g_oled.setBusClock(400000);
  g_oled.clearBuffer();
  g_oled.setFont(u8g2_font_6x10_tf);
  g_oled.drawStr(0, 12, "Exercise 21");
  g_oled.drawStr(0, 28, "QMI8658 Six-Axis");
  g_oled.drawStr(0, 44, "Starting...");
  g_oled.sendBuffer();
  g_displayReady = true;
}

bool initImu() {
  pinMode(SD_CS, OUTPUT);
  digitalWrite(SD_CS, HIGH);

  SPI.begin(SD_CLK, SD_MISO, SD_MOSI, IMU_CS);

  if (!g_qmi.begin(SPI, IMU_CS, SD_MOSI, SD_MISO, SD_CLK)) {
    return false;
  }

  g_qmi.configAccelerometer(
      SensorQMI8658::ACC_RANGE_4G,
      SensorQMI8658::ACC_ODR_250Hz,
      SensorQMI8658::LPF_MODE_1);

  g_qmi.configGyroscope(
      SensorQMI8658::GYR_RANGE_256DPS,
      SensorQMI8658::GYR_ODR_224_2Hz,
      SensorQMI8658::LPF_MODE_3);

  g_qmi.enableGyroscope();
  g_qmi.enableAccelerometer();
  return true;
}

void drawStatus(const char* line1, const char* line2 = nullptr, const char* line3 = nullptr) {
  if (!g_displayReady) {
    return;
  }
  g_oled.clearBuffer();
  g_oled.setFont(u8g2_font_6x10_tf);
  g_oled.drawStr(0, 12, line1);
  if (line2) g_oled.drawStr(0, 28, line2);
  if (line3) g_oled.drawStr(0, 44, line3);
  g_oled.sendBuffer();
}

void sampleImu() {
  if (!g_sensorReady || !g_qmi.getDataReady()) {
    return;
  }

  if (!g_qmi.getAccelerometer(g_accel.x, g_accel.y, g_accel.z)) {
    return;
  }
  if (!g_qmi.getGyroscope(g_gyro.x, g_gyro.y, g_gyro.z)) {
    return;
  }

  const uint32_t nowMicros = micros();
  if (g_lastSampleMicros != 0) {
    g_lastSamplePeriodMs = (nowMicros - g_lastSampleMicros) / 1000.0f;
  }
  g_lastSampleMicros = nowMicros;

  g_temperatureC = g_qmi.getTemperature_C();
  g_sensorTimestamp = g_qmi.getTimestamp();
  ++g_sampleCount;
}

void printSerialFrame() {
  Serial.printf(
      "sample=%lu dt=%.1fms ts=%lu temp=%.2fC "
      "acc[g]={%.4f,%.4f,%.4f} gyro[dps]={%.3f,%.3f,%.3f}\r\n",
      static_cast<unsigned long>(g_sampleCount),
      g_lastSamplePeriodMs,
      static_cast<unsigned long>(g_sensorTimestamp),
      g_temperatureC,
      g_accel.x,
      g_accel.y,
      g_accel.z,
      g_gyro.x,
      g_gyro.y,
      g_gyro.z);
}

void drawFrame() {
  if (!g_displayReady) {
    return;
  }

  char line[32];

  g_oled.clearBuffer();
  g_oled.setFont(u8g2_font_5x8_tf);

  snprintf(line, sizeof(line), "QMI8658  %s", NODE_LABEL);
  g_oled.drawStr(0, 8, line);

  snprintf(line, sizeof(line), "A X:% .3f", g_accel.x);
  g_oled.drawStr(0, 16, line);
  snprintf(line, sizeof(line), "A Y:% .3f", g_accel.y);
  g_oled.drawStr(64, 16, line);

  snprintf(line, sizeof(line), "A Z:% .3f", g_accel.z);
  g_oled.drawStr(0, 24, line);

  snprintf(line, sizeof(line), "G X:% .1f", g_gyro.x);
  g_oled.drawStr(0, 36, line);
  snprintf(line, sizeof(line), "G Y:% .1f", g_gyro.y);
  g_oled.drawStr(64, 36, line);

  snprintf(line, sizeof(line), "G Z:% .1f", g_gyro.z);
  g_oled.drawStr(0, 44, line);

  snprintf(line, sizeof(line), "T:%5.2fC", g_temperatureC);
  g_oled.drawStr(0, 56, line);
  snprintf(line, sizeof(line), "dt:%4.1f", g_lastSamplePeriodMs);
  g_oled.drawStr(60, 56, line);
  snprintf(line, sizeof(line), "N:%lu", static_cast<unsigned long>(g_sampleCount));
  g_oled.drawStr(96, 56, line);

  g_oled.sendBuffer();
}

}  // namespace

void setup() {
  Serial.begin(115200);
  delay(250);

  tbeam_supreme::initPmuForPeripherals(g_pmu, &Serial);
  initDisplay();

  drawStatus("Exercise 21", "QMI8658 live view", "Init IMU...");

  g_sensorReady = initImu();
  if (!g_sensorReady) {
    Serial.println("QMI8658 init failed");
    drawStatus("QMI8658 init", "FAILED", "Check wiring/power");
    return;
  }

  Serial.printf("QMI8658 device ID: 0x%02X\r\n", g_qmi.getChipID());
  Serial.println("Streaming raw accel[g], gyro[dps], temp[C]");
  Serial.println("UI refresh: 100 ms, sensor ODR: accel 250 Hz, gyro 224.2 Hz");

  drawStatus("QMI8658 ready", "Serial + OLED live", "Move the device");
  delay(kSplashDelayMs);
  g_lastUiUpdateMs = millis();
}

void loop() {
  sampleImu();

  const uint32_t now = millis();
  if (now - g_lastUiUpdateMs < kUiUpdateIntervalMs) {
    return;
  }
  g_lastUiUpdateMs = now;

  if (!g_sensorReady) {
    return;
  }

  printSerialFrame();
  drawFrame();
}
