/* LVGL + QMI8658 Fixed: Compilation Errors */

#include <lvgl.h>
#include <Arduino_GFX_Library.h>
#include <Adafruit_MCP23X17.h>
#include <Wire.h>
#include "SensorQMI8658.hpp"

// Pin definitions
#define GFX_BL 6
#define SPI_MISO 11
#define SPI_MOSI 13
#define SPI_SCLK 12
#define LCD_CS 10
#define LCD_DC 16
#define LCD_RST 10
#define LCD_HOR_RES 240
#define LCD_VER_RES 240
#define I2C_SDA 8
#define I2C_SCL 7

// Global objects
Adafruit_MCP23X17 mcp;
SensorQMI8658 qmi;
Arduino_DataBus *bus = new Arduino_ESP32SPI(LCD_DC, LCD_CS, SPI_SCLK, SPI_MOSI, SPI_MISO);
Arduino_GFX *gfx = new Arduino_ST7789(bus, LCD_RST, 0, true, LCD_HOR_RES, LCD_VER_RES);

// LVGL display buffer
uint32_t screenWidth, screenHeight, bufSize;
lv_disp_draw_buf_t draw_buf;
lv_color_t *disp_draw_buf1;
lv_color_t *disp_draw_buf2;
lv_disp_drv_t disp_drv;

// UI elements
lv_obj_t *label_accel;
lv_obj_t *label_gyro;
lv_obj_t *label_temp;
lv_obj_t *label_debug;
lv_timer_t *qmi_timer = NULL;

// Sensor data (use float to match library's return type)
struct {
  float x, y, z;  // Changed from double to float
} accel_data = {0}, gyro_data = {0};
float temperature = 0;

/* LVGL display flushing */
void my_disp_flush(lv_disp_drv_t *disp_drv, const lv_area_t *area, lv_color_t *color_p) {
  uint32_t w = (area->x2 - area->x1 + 1);
  uint32_t h = (area->y2 - area->y1 + 1);

#if (LV_COLOR_16_SWAP != 0)
  gfx->draw16bitBeRGBBitmap(area->x1, area->y1, (uint16_t *)&color_p->full, w, h);
#else
  gfx->draw16bitRGBBitmap(area->x1, area->y1, (uint16_t *)&color_p->full, w, h);
#endif

  lv_disp_flush_ready(disp_drv);
}

/* LCD reset */
void lcd_reset(void) {
  mcp.digitalWrite(1, HIGH);
  delay(10);
  mcp.digitalWrite(1, LOW);
  delay(10);
  mcp.digitalWrite(1, HIGH);
  delay(50);  // Reduced delay for faster screen clearing
}

/* Initialize QMI8658 with correct ODR values from library */
void qmi8658_init(void) {
  Wire.end();
  Wire.begin(I2C_SDA, I2C_SCL);
  Wire.setClock(400000);
  delay(100);

  if (!qmi.begin(Wire, QMI8658_L_SLAVE_ADDRESS, I2C_SDA, I2C_SCL)) {
    Serial.println("QMI8658 not found!");
    return;
  }

  // Use ODR values supported by the library (from your original QMI code)
  qmi.configAccelerometer(
    SensorQMI8658::ACC_RANGE_4G, 
    SensorQMI8658::ACC_ODR_125Hz,  // Correct ODR: 125Hz (library-supported)
    SensorQMI8658::LPF_MODE_0
  );
                         
  qmi.configGyroscope(
    SensorQMI8658::GYR_RANGE_64DPS, 
    SensorQMI8658::GYR_ODR_448_4Hz,  // Correct ODR: 448.4Hz (library-supported)
    SensorQMI8658::LPF_MODE_3
  );

  qmi.enableAccelerometer();
  qmi.enableGyroscope();
  Serial.println("QMI8658 initialized");
}

/* Convert float to string (fix LVGL %f issue) */
void float_to_str(char *buf, float value, int decimals) {
  if (value < 0) {
    *buf++ = '-';
    value = -value;
  }
  
  long integer_part = (long)value;
  long fractional_part = (long)((value - integer_part) * pow(10, decimals) + 0.5);
  
  sprintf(buf, "%ld.", integer_part);
  buf += strlen(buf);
  sprintf(buf, "%0*d", decimals, fractional_part);
}

/* Timer callback with fixed data types */
static void qmi_data_callback(lv_timer_t *timer) {
  char buf[32];
  bool accel_ok = false, gyro_ok = false;

  // Read accelerometer (float type matches library)
  accel_ok = qmi.getAccelerometer(accel_data.x, accel_data.y, accel_data.z);
  // Read gyroscope (float type matches library)
  gyro_ok = qmi.getGyroscope(gyro_data.x, gyro_data.y, gyro_data.z);
  // Read temperature
  temperature = qmi.getTemperature_C();

  // Update Accel label
  if (accel_ok) {
    float_to_str(buf, accel_data.x, 2);
    char accel_str[64];
    sprintf(accel_str, "Accel: X:%s Y:", buf);
    
    float_to_str(buf, accel_data.y, 2);
    strcat(accel_str, buf);
    strcat(accel_str, " Z:");
    
    float_to_str(buf, accel_data.z, 2);
    strcat(accel_str, buf);
    
    lv_label_set_text(label_accel, accel_str);
  } else {
    lv_label_set_text(label_accel, "Accel: Read failed");
  }

  // Update Gyro label
  if (gyro_ok) {
    float_to_str(buf, gyro_data.x, 2);
    char gyro_str[64];
    sprintf(gyro_str, "Gyro:  X:%s Y:", buf);
    
    float_to_str(buf, gyro_data.y, 2);
    strcat(gyro_str, buf);
    strcat(gyro_str, " Z:");
    
    float_to_str(buf, gyro_data.z, 2);
    strcat(gyro_str, buf);
    
    lv_label_set_text(label_gyro, gyro_str);
  } else {
    lv_label_set_text(label_gyro, "Gyro:  Read failed");
  }

  // Update Temp label
  float_to_str(buf, temperature, 2);
  char temp_str[32];
  sprintf(temp_str, "Temp:  %s °C", buf);
  lv_label_set_text(label_temp, temp_str);

  // Debug output
  Serial.printf("Accel: %s | Gyro: %s | Temp: %.2f\n",
               accel_ok ? "OK" : "FAIL",
               gyro_ok ? "OK" : "FAIL",
               temperature);
}

/* Initialize UI */
void lvgl_qmi_ui_init(lv_obj_t *parent) {
  lv_obj_set_style_bg_color(parent, lv_color_black(), 0);
  lv_obj_set_style_bg_opa(parent, LV_OPA_COVER, 0);

  static lv_style_t text_style;
  lv_style_init(&text_style);
  lv_style_set_text_color(&text_style, lv_color_white());
  lv_style_set_text_font(&text_style, &lv_font_montserrat_16);
  lv_style_set_text_align(&text_style, LV_TEXT_ALIGN_LEFT);

  // Title
  lv_obj_t *title = lv_label_create(parent);
  lv_obj_add_style(title, &text_style, 0);
  lv_label_set_text(title, "QMI8658 Data");
  lv_obj_align(title, LV_ALIGN_TOP_MID, 0, 5);

  // Accel label
  label_accel = lv_label_create(parent);
  lv_obj_add_style(label_accel, &text_style, 0);
  lv_label_set_text(label_accel, "Accel: X:--.- Y:--.- Z:--.-");
  lv_obj_align(label_accel, LV_ALIGN_TOP_LEFT, 5, 30);

  // Gyro label
  label_gyro = lv_label_create(parent);
  lv_obj_add_style(label_gyro, &text_style, 0);
  lv_label_set_text(label_gyro, "Gyro:  X:--.- Y:--.- Z:--.-");
  lv_obj_align(label_gyro, LV_ALIGN_TOP_LEFT, 5, 60);

  // Temp label
  label_temp = lv_label_create(parent);
  lv_obj_add_style(label_temp, &text_style, 0);
  lv_label_set_text(label_temp, "Temp:  --.- °C");
  lv_obj_align(label_temp, LV_ALIGN_TOP_LEFT, 5, 90);

  // Debug label
  label_debug = lv_label_create(parent);
  lv_obj_add_style(label_debug, &text_style, 0);
  lv_label_set_text(label_debug, "Status: Initializing");
  lv_obj_align(label_debug, LV_ALIGN_BOTTOM_MID, 0, -5);

  // Create timer
  qmi_timer = lv_timer_create(qmi_data_callback, 200, NULL);
}

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

  // Initialize I2C
  Wire.begin(I2C_SDA, I2C_SCL);
  
  // Initialize MCP23017 (for LCD reset)
  if (!mcp.begin_I2C(0x20)) {
    Serial.println("ERROR: MCP23017 initialization failed!");
  } else {
    Serial.println("MCP23017 initialized at address 0x20");
    // Configure LCD reset pin as output
    mcp.pinMode(1, OUTPUT);
  }
  
  lcd_reset();
  
  // Initialize display
  if (!gfx->begin()) {
    Serial.println("Display init failed!");
    while (1);
  }
  gfx->fillScreen(RGB565_BLACK);

  // Backlight on
#ifdef GFX_BL
  pinMode(GFX_BL, OUTPUT);
  digitalWrite(GFX_BL, HIGH);
#endif

  // LVGL init
  lv_init();
  screenWidth = LCD_HOR_RES;
  screenHeight = LCD_VER_RES;
  bufSize = screenWidth * 20;

  // Allocate buffers
  disp_draw_buf1 = (lv_color_t *)malloc(bufSize * sizeof(lv_color_t));
  disp_draw_buf2 = (lv_color_t *)malloc(bufSize * sizeof(lv_color_t));
  if (!disp_draw_buf1 || !disp_draw_buf2) {
    Serial.println("Buffer malloc failed!");
    while (1);
  }
  lv_disp_draw_buf_init(&draw_buf, disp_draw_buf1, disp_draw_buf2, bufSize);

  // Register display driver
  lv_disp_drv_init(&disp_drv);
  disp_drv.hor_res = screenWidth;
  disp_drv.ver_res = screenHeight;
  disp_drv.flush_cb = my_disp_flush;
  disp_drv.draw_buf = &draw_buf;
  lv_disp_drv_register(&disp_drv);

  // Init sensor and UI
  qmi8658_init();
  lvgl_qmi_ui_init(lv_scr_act());

  Serial.println("Setup complete");
}

void loop() {
  lv_timer_handler();
  delay(5);
}