#include <stdio.h>
#include <string.h>
#include <math.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include "esp_log.h"
#include "driver/i2c.h"

#include "u8g2.h"
#include "u8g2_esp32_hal.h"

// ---------- Pins ----------
#define PIN_SDA 8
#define PIN_SCL 9
#define I2C_PORT I2C_NUM_0
#define I2C_FREQ_HZ 400000

// ---------- MPU6050 ----------
#define MPU_ADDR         0x68
#define REG_PWR_MGMT_1   0x6B
#define REG_ACCEL_XOUT_H 0x3B
#define REG_GYRO_XOUT_H  0x43

static const char *TAG = "CREA";

// ---------- I2C low-level ----------
static esp_err_t i2c_init(void) {
  i2c_config_t conf = {
      .mode = I2C_MODE_MASTER,
      .sda_io_num = PIN_SDA,
      .scl_io_num = PIN_SCL,
      .sda_pullup_en = GPIO_PULLUP_ENABLE,
      .scl_pullup_en = GPIO_PULLUP_ENABLE,
      .master.clk_speed = I2C_FREQ_HZ,
  };
  ESP_ERROR_CHECK(i2c_param_config(I2C_PORT, &conf));
  return i2c_driver_install(I2C_PORT, conf.mode, 0, 0, 0);
}

static esp_err_t i2c_write_u8(uint8_t addr, uint8_t reg, uint8_t val) {
  uint8_t data[2] = {reg, val};
  return i2c_master_write_to_device(I2C_PORT, addr, data, sizeof(data), pdMS_TO_TICKS(100));
}

static esp_err_t i2c_read(uint8_t addr, uint8_t reg, uint8_t *out, size_t len) {
  return i2c_master_write_read_device(I2C_PORT, addr, &reg, 1, out, len, pdMS_TO_TICKS(100));
}

static void i2c_scan(void) {
  ESP_LOGI(TAG, "I2C scan...");
  for (int a = 1; a < 0x7F; a++) {
    esp_err_t err = i2c_master_probe(I2C_PORT, a, pdMS_TO_TICKS(30));
    if (err == ESP_OK) ESP_LOGI(TAG, "  found 0x%02X", a);
  }
}

// ---------- MPU reads ----------
static bool mpu_read6(uint8_t start_reg, int16_t *x, int16_t *y, int16_t *z) {
  uint8_t b[6];
  if (i2c_read(MPU_ADDR, start_reg, b, 6) != ESP_OK) return false;
  *x = (int16_t)((b[0] << 8) | b[1]);
  *y = (int16_t)((b[2] << 8) | b[3]);
  *z = (int16_t)((b[4] << 8) | b[5]);
  return true;
}

static bool mpu_read_accel(int16_t *ax, int16_t *ay, int16_t *az) {
  return mpu_read6(REG_ACCEL_XOUT_H, ax, ay, az);
}

static bool mpu_read_gyro(int16_t *gx, int16_t *gy, int16_t *gz) {
  return mpu_read6(REG_GYRO_XOUT_H, gx, gy, gz);
}

// ---------- u8g2 ----------
static u8g2_t u8g2;

static void oled_init(void) {
  u8g2_esp32_hal_t hal = U8G2_ESP32_HAL_DEFAULT;
  hal.sda = PIN_SDA;
  hal.scl = PIN_SCL;
  u8g2_esp32_hal_init(hal);

  // SSD1306 128x64 I2C, address 0x3C (u8g2 setzt 0x78/0x7A als 8-bit addr intern)
  u8g2_Setup_ssd1306_i2c_128x64_noname_f(
      &u8g2,
      U8G2_R0,
      u8g2_esp32_i2c_byte_cb,
      u8g2_esp32_gpio_and_delay_cb);

  u8g2_InitDisplay(&u8g2);
  u8g2_SetPowerSave(&u8g2, 0);
  u8g2_ClearBuffer(&u8g2);
  u8g2_SendBuffer(&u8g2);
}

void app_main(void) {
  ESP_LOGI(TAG, "Starting (OLED always on, read MPU accel/gyro)");

  ESP_ERROR_CHECK(i2c_init());
  i2c_scan();

  // wake MPU
  ESP_ERROR_CHECK(i2c_write_u8(MPU_ADDR, REG_PWR_MGMT_1, 0x00));

  oled_init();
  u8g2_SetFont(&u8g2, u8g2_font_6x10_tf);

  while (1) {
    int16_t ax, ay, az, gx, gy, gz;
    bool okA = mpu_read_accel(&ax, &ay, &az);
    bool okG = mpu_read_gyro(&gx, &gy, &gz);

    // in deg/s (bei default ±250 dps): raw/131
    int gdps_x = gx / 131;
    int gdps_y = gy / 131;
    int gdps_z = gz / 131;

    ESP_LOGI(TAG, "acc ok=%d ax=%d ay=%d az=%d | gyro ok=%d gx=%d gy=%d gz=%d (dps %d %d %d)",
             okA, ax, ay, az, okG, gx, gy, gz, gdps_x, gdps_y, gdps_z);

    char l0[32], l1[32], l2[32], l3[32];
    snprintf(l0, sizeof(l0), "MPU ok A:%d G:%d", okA ? 1 : 0, okG ? 1 : 0);
    snprintf(l1, sizeof(l1), "A %6d %6d %6d", ax, ay, az);
    snprintf(l2, sizeof(l2), "G %6d %6d %6d", gx, gy, gz);
    snprintf(l3, sizeof(l3), "dps %4d %4d %4d", gdps_x, gdps_y, gdps_z);

    u8g2_ClearBuffer(&u8g2);
    u8g2_DrawStr(&u8g2, 0, 12, l0);
    u8g2_DrawStr(&u8g2, 0, 28, l1);
    u8g2_DrawStr(&u8g2, 0, 44, l2);
    u8g2_DrawStr(&u8g2, 0, 60, l3);
    u8g2_SendBuffer(&u8g2);

    vTaskDelay(pdMS_TO_TICKS(250));
  }
}