#include <Arduino.h>
#include <U8g2lib.h>
#include <Wire.h>
#include <XPowersLib.h>
#include <math.h>
#include <stdint.h>
#include <string.h>

#include "SensorQMC6310.hpp"
#include "tbeam_supreme_adapter.h"

namespace {

#ifndef BOARD_ID
#define BOARD_ID "CY"
#endif

#ifndef NODE_LABEL
#define NODE_LABEL "Cy"
#endif

#ifndef FW_BUILD_UTC
#define FW_BUILD_UTC unknown
#endif

#ifndef OLED_SDA
#define OLED_SDA 17
#endif

#ifndef OLED_SCL
#define OLED_SCL 18
#endif

#ifndef OLED_ADDR
#define OLED_ADDR 0x3C
#endif

#ifndef MOTIONCAL_MAG_RANGE_G
#define MOTIONCAL_MAG_RANGE_G 8
#endif

#define STR_INNER(x) #x
#define STR(x) STR_INNER(x)

#if MOTIONCAL_MAG_RANGE_G == 2
static constexpr MagFullScaleRange kMagFullScaleRange = MagFullScaleRange::FS_2G;
static constexpr const char* kMagRangeLabel = "FS2G";
#elif MOTIONCAL_MAG_RANGE_G == 8
static constexpr MagFullScaleRange kMagFullScaleRange = MagFullScaleRange::FS_8G;
static constexpr const char* kMagRangeLabel = "FS8G";
#else
#error "MOTIONCAL_MAG_RANGE_G must be 2 or 8"
#endif

static constexpr const char* kExerciseName = "Exercise 25";
static constexpr const char* kBoardId = BOARD_ID;
static constexpr const char* kNodeLabel = NODE_LABEL;
static constexpr const char* kFirmwareVersion = STR(FW_BUILD_UTC);
static constexpr uint32_t kSampleIntervalMs = 40;
static constexpr uint32_t kDisplayIntervalMs = 250;
static constexpr uint32_t kStatusIntervalMs = 1000;
static constexpr uint8_t kMagCandidateCount = 3;
static constexpr uint8_t kMagCandidates[kMagCandidateCount] = {0x1C, 0x3C, 0x2C};
static constexpr uint16_t kCalibrationPacketSize = 68;
static constexpr uint16_t kCalibrationSignature = 0x5475;
static constexpr size_t kFloatCount = 16;

XPowersLibInterface* g_pmu = nullptr;
U8G2_SH1106_128X64_NONAME_F_HW_I2C g_oled(U8G2_R0, U8X8_PIN_NONE);
SensorQMC6310 g_qmc;

bool g_displayReady = false;
bool g_magReady = false;
uint8_t g_magAddress = 0;
uint8_t g_magChipId = 0;
char g_magLabel[16] = "UNKNOWN";
uint32_t g_lastSampleMs = 0;
uint32_t g_lastDisplayMs = 0;
uint32_t g_lastStatusMs = 0;
uint32_t g_sampleCount = 0;
uint32_t g_readFailCount = 0;
uint32_t g_overflowCount = 0;
int16_t g_lastMagCounts[3] = {0, 0, 0};
float g_lastMagUt[3] = {0.0f, 0.0f, 0.0f};

uint8_t g_calPacket[kCalibrationPacketSize];
uint16_t g_calPacketLen = 0;

uint16_t crc16Update(uint16_t crc, uint8_t data) {
  crc ^= data;
  for (uint8_t i = 0; i < 8; ++i) {
    if ((crc & 1U) != 0U) {
      crc = (crc >> 1U) ^ 0xA001U;
    } else {
      crc >>= 1U;
    }
  }
  return crc;
}

float readFloatLe(const uint8_t* p) {
  union {
    uint32_t n;
    float f;
  } value;
  value.n = ((uint32_t)p[0]) |
            ((uint32_t)p[1] << 8U) |
            ((uint32_t)p[2] << 16U) |
            ((uint32_t)p[3] << 24U);
  return value.f;
}

int16_t clampToInt16(long value) {
  if (value > INT16_MAX) return INT16_MAX;
  if (value < INT16_MIN) return INT16_MIN;
  return (int16_t)value;
}

int16_t microteslaToMotionCalCounts(float uT) {
  return clampToInt16(lroundf(uT * 10.0f));
}

void drawLines(const char* l1,
               const char* l2 = nullptr,
               const char* l3 = nullptr,
               const char* l4 = nullptr,
               const char* l5 = nullptr,
               const char* l6 = nullptr) {
  if (!g_displayReady) {
    return;
  }

  g_oled.clearBuffer();
  g_oled.setFont(u8g2_font_5x8_tf);
  if (l1) g_oled.drawUTF8(0, 10, l1);
  if (l2) g_oled.drawUTF8(0, 20, l2);
  if (l3) g_oled.drawUTF8(0, 30, l3);
  if (l4) g_oled.drawUTF8(0, 40, l4);
  if (l5) g_oled.drawUTF8(0, 50, l5);
  if (l6) g_oled.drawUTF8(0, 60, l6);
  g_oled.sendBuffer();
}

void initDisplay() {
  Wire.begin(OLED_SDA, OLED_SCL);
  Wire.setClock(400000);
  g_oled.setI2CAddress(OLED_ADDR << 1);
  g_oled.setBusClock(400000);
  g_oled.begin();
  g_oled.setPowerSave(0);
  g_displayReady = true;
  drawLines(kExerciseName, kFirmwareVersion, kBoardId, "MotionCal Bridge", "starting...");
}

bool probeI2cAddr(TwoWire& wire, uint8_t addr) {
  wire.beginTransmission(addr);
  return wire.endTransmission() == 0;
}

bool detectMagnetometer() {
  for (uint8_t i = 0; i < kMagCandidateCount; ++i) {
    const uint8_t addr = kMagCandidates[i];
    if (!probeI2cAddr(Wire, addr)) {
      continue;
    }

    g_magAddress = addr;
    if (addr == 0x1C) {
      strlcpy(g_magLabel, "QMC6310U", sizeof(g_magLabel));
    } else if (addr == 0x3C) {
      strlcpy(g_magLabel, "QMC6310N", sizeof(g_magLabel));
    } else if (addr == 0x2C) {
      strlcpy(g_magLabel, "QMC5883P", sizeof(g_magLabel));
    } else {
      strlcpy(g_magLabel, "QST-MAG", sizeof(g_magLabel));
    }
    return true;
  }
  return false;
}

bool initMagnetometer() {
  if (!detectMagnetometer()) {
    return false;
  }

  if (!g_qmc.begin(Wire, g_magAddress, tbeam_supreme::i2cSda(), tbeam_supreme::i2cScl())) {
    return false;
  }

  g_magChipId = g_qmc.getChipID();
  return g_qmc.configMagnetometer(
      OperationMode::CONTINUOUS_MEASUREMENT,
      kMagFullScaleRange,
      100.0f,
      MagOverSampleRatio::OSR_4,
      MagDownSampleRatio::DSR_1);
}

void printBootSummary() {
  Serial.printf("exercise=%s MotionCal T-Beam bridge\r\n", kExerciseName);
  Serial.printf("board_id=%s node_label=%s build=%s\r\n", kBoardId, kNodeLabel, kFirmwareVersion);
  Serial.printf("serial_format=Raw:accel_x,accel_y,accel_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z\r\n");
  Serial.printf("mag_units=MotionCal integer counts, 1 count = 0.1 uT\r\n");
  Serial.printf("mag_range=%s\r\n", kMagRangeLabel);
}

void streamMotionCalRaw(const MagnetometerData& data) {
  const float xUt = data.magnetic_field.x * 100.0f;
  const float yUt = data.magnetic_field.y * 100.0f;
  const float zUt = data.magnetic_field.z * 100.0f;

  g_lastMagUt[0] = xUt;
  g_lastMagUt[1] = yUt;
  g_lastMagUt[2] = zUt;
  g_lastMagCounts[0] = microteslaToMotionCalCounts(xUt);
  g_lastMagCounts[1] = microteslaToMotionCalCounts(yUt);
  g_lastMagCounts[2] = microteslaToMotionCalCounts(zUt);
  ++g_sampleCount;

  Serial.printf("Raw:0,0,8192,0,0,0,%d,%d,%d\r\n",
                (int)g_lastMagCounts[0],
                (int)g_lastMagCounts[1],
                (int)g_lastMagCounts[2]);
}

void updateDisplay() {
  char line3[28];
  char line4[28];
  char line5[28];

  snprintf(line4, sizeof(line4), "%6.1f %6.1f", g_lastMagUt[0], g_lastMagUt[1]);
  snprintf(line5, sizeof(line5), "Z:%6.1f N:%lu", g_lastMagUt[2], (unsigned long)g_sampleCount);
  snprintf(line3, sizeof(line3), "%s %s 0x%02X", kMagRangeLabel, g_magLabel, g_magAddress);
  drawLines(kExerciseName, kFirmwareVersion, line3, line4, line5, "Raw stream");
}

void printCalibrationEcho(const float* values) {
  Serial.printf("Cal1:%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f\r\n",
                values[0], values[1], values[2],
                values[3], values[4], values[5],
                values[6], values[7], values[8], values[9]);
  Serial.printf("Cal2:%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f\r\n",
                values[10], values[13], values[14],
                values[13], values[11], values[15],
                values[14], values[15], values[12]);
}

void handleCalibrationPacket(const uint8_t* packet) {
  const uint16_t signature = (uint16_t)packet[0] | ((uint16_t)packet[1] << 8U);
  if (signature != kCalibrationSignature) {
    return;
  }

  uint16_t crc = 0xFFFF;
  for (uint16_t i = 0; i < kCalibrationPacketSize - 2; ++i) {
    crc = crc16Update(crc, packet[i]);
  }
  const uint16_t got = (uint16_t)packet[66] | ((uint16_t)packet[67] << 8U);
  if (crc != got) {
    Serial.printf("motioncal_calibration_crc=bad expected=0x%04X got=0x%04X\r\n", crc, got);
    return;
  }

  float values[kFloatCount];
  const uint8_t* p = packet + 2;
  for (size_t i = 0; i < kFloatCount; ++i) {
    values[i] = readFloatLe(p);
    p += 4;
  }

  Serial.printf("motioncal_calibration=received hard_iron_uT=%.3f,%.3f,%.3f field_uT=%.3f\r\n",
                values[6], values[7], values[8], values[9]);
  printCalibrationEcho(values);
}

void pollSerialInput() {
  while (Serial.available() > 0) {
    const int c = Serial.read();
    if (c < 0) {
      return;
    }

    if (g_calPacketLen == 0 && (uint8_t)c != 0x75U) {
      continue;
    }

    g_calPacket[g_calPacketLen++] = (uint8_t)c;
    if (g_calPacketLen == 2 && g_calPacket[1] != 0x54U) {
      g_calPacketLen = (g_calPacket[1] == 0x75U) ? 1U : 0U;
      if (g_calPacketLen == 1U) {
        g_calPacket[0] = 0x75U;
      }
      continue;
    }

    if (g_calPacketLen >= kCalibrationPacketSize) {
      handleCalibrationPacket(g_calPacket);
      g_calPacketLen = 0;
    }
  }
}

void appSetup() {
  Serial.begin(115200);
  const uint32_t serialWaitStart = millis();
  while (!Serial && (millis() - serialWaitStart) < 4000) {
    delay(10);
  }
  delay(300);

  printBootSummary();
  initDisplay();

  if (!tbeam_supreme::initPmuForPeripherals(g_pmu, &Serial)) {
    Serial.println("pmu_init=failed");
    drawLines(kExerciseName, kFirmwareVersion, "PMU init failed", kBoardId, "see serial");
    return;
  }
  Serial.println("pmu_init=ok");

  g_magReady = initMagnetometer();
  if (!g_magReady) {
    Serial.println("magnetometer_init=failed");
    drawLines(kExerciseName, kFirmwareVersion, "MAG init failed", kBoardId, "see serial");
    return;
  }

  Serial.printf("magnetometer_init=ok label=%s addr=0x%02X chip=0x%02X\r\n",
                g_magLabel, g_magAddress, g_magChipId);
  drawLines(kExerciseName, kFirmwareVersion, "MotionCal Bridge", g_magLabel, "streaming Raw...");
  g_lastSampleMs = millis();
  g_lastDisplayMs = millis();
  g_lastStatusMs = millis();
}

void appLoop() {
  pollSerialInput();

  if (!g_magReady) {
    delay(100);
    return;
  }

  const uint32_t now = millis();
  if ((uint32_t)(now - g_lastSampleMs) >= kSampleIntervalMs) {
    g_lastSampleMs = now;
    MagnetometerData data;
    if (!g_qmc.readData(data)) {
      ++g_readFailCount;
    } else if (data.overflow) {
      ++g_overflowCount;
    } else {
      streamMotionCalRaw(data);
    }
  }

  if ((uint32_t)(now - g_lastDisplayMs) >= kDisplayIntervalMs) {
    g_lastDisplayMs = now;
    updateDisplay();
  }

  if ((uint32_t)(now - g_lastStatusMs) >= kStatusIntervalMs) {
    g_lastStatusMs = now;
    if (g_readFailCount > 0 || g_overflowCount > 0) {
      Serial.printf("motioncal_status=samples:%lu read_fail:%lu overflow:%lu\r\n",
                    (unsigned long)g_sampleCount,
                    (unsigned long)g_readFailCount,
                    (unsigned long)g_overflowCount);
    }
  }

  delay(1);
}

}  // namespace

void setup() {
  appSetup();
}

void loop() {
  appLoop();
}
