/**
 * @file icm42688_spi.cpp
 * @brief ICM-42688/ICM-42670 IMU 驱动 - GPIO 模拟 SPI 实现
 * @date 2025-11-27
 * @platform Avaota F1 (V821 / RISC-V)
 * 
 * 硬件连接：
 * - VCC        → 3.3V
 * - GND        → GND
 * - SCL/SCLK   → PD3 (GPIO 99)
 * - SDA/MOSI   → PD2 (GPIO 98)
 * - AD0/MISO   → PD4 (GPIO 100)
 * - CS         → PD5 (GPIO 101)
 * 
 * 使用 GPIO 模拟 SPI，速度约 1MHz
 */

#include "icm42688.h"
#include "../utils/logger.h"

#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <linux/gpio.h>
#include <cstring>

// GPIO 引脚定义
#define GPIO_SCLK  99   // PD3 - SPI 时钟
#define GPIO_MOSI  98   // PD2 - 主→从数据
#define GPIO_MISO  100  // PD4 - 从→主数据
#define GPIO_CS    101  // PD5 - 片选
#define GPIO_CHIP  "/dev/gpiochip0"
#define SPI_DELAY_US 1  // 1us 延迟，约 500kHz

// GPIO 句柄
static int gpio_chip_fd = -1;
static int sclk_fd = -1;
static int mosi_fd = -1;
static int miso_fd = -1;
static int cs_fd = -1;

// GPIO 控制函数
static bool gpio_init() {
    gpio_chip_fd = open(GPIO_CHIP, O_RDWR);
    if (gpio_chip_fd < 0) {
        LOG_ERROR("[IMU] Failed to open GPIO chip %s", GPIO_CHIP);
        return false;
    }
    
    // 请求 SCLK (输出)
    struct gpiohandle_request req_sclk;
    memset(&req_sclk, 0, sizeof(req_sclk));
    req_sclk.lineoffsets[0] = GPIO_SCLK;
    req_sclk.flags = GPIOHANDLE_REQUEST_OUTPUT;
    req_sclk.default_values[0] = 0;  // 默认低电平
    req_sclk.lines = 1;
    strcpy(req_sclk.consumer_label, "icm_sclk");
    
    if (ioctl(gpio_chip_fd, GPIO_GET_LINEHANDLE_IOCTL, &req_sclk) < 0) {
        LOG_ERROR("[IMU] Failed to request SCLK GPIO %d", GPIO_SCLK);
        close(gpio_chip_fd);
        return false;
    }
    sclk_fd = req_sclk.fd;
    
    // 请求 MOSI (输出)
    struct gpiohandle_request req_mosi;
    memset(&req_mosi, 0, sizeof(req_mosi));
    req_mosi.lineoffsets[0] = GPIO_MOSI;
    req_mosi.flags = GPIOHANDLE_REQUEST_OUTPUT;
    req_mosi.default_values[0] = 0;
    req_mosi.lines = 1;
    strcpy(req_mosi.consumer_label, "icm_mosi");
    
    if (ioctl(gpio_chip_fd, GPIO_GET_LINEHANDLE_IOCTL, &req_mosi) < 0) {
        LOG_ERROR("[IMU] Failed to request MOSI GPIO %d", GPIO_MOSI);
        close(sclk_fd);
        close(gpio_chip_fd);
        return false;
    }
    mosi_fd = req_mosi.fd;
    
    // 请求 MISO (输入)
    struct gpiohandle_request req_miso;
    memset(&req_miso, 0, sizeof(req_miso));
    req_miso.lineoffsets[0] = GPIO_MISO;
    req_miso.flags = GPIOHANDLE_REQUEST_INPUT;
    req_miso.lines = 1;
    strcpy(req_miso.consumer_label, "icm_miso");
    
    if (ioctl(gpio_chip_fd, GPIO_GET_LINEHANDLE_IOCTL, &req_miso) < 0) {
        LOG_ERROR("[IMU] Failed to request MISO GPIO %d", GPIO_MISO);
        close(mosi_fd);
        close(sclk_fd);
        close(gpio_chip_fd);
        return false;
    }
    miso_fd = req_miso.fd;
    
    // 请求 CS (输出，默认高电平 = 未选中)
    struct gpiohandle_request req_cs;
    memset(&req_cs, 0, sizeof(req_cs));
    req_cs.lineoffsets[0] = GPIO_CS;
    req_cs.flags = GPIOHANDLE_REQUEST_OUTPUT;
    req_cs.default_values[0] = 1;  // 默认高电平（未选中）
    req_cs.lines = 1;
    strcpy(req_cs.consumer_label, "icm_cs");
    
    if (ioctl(gpio_chip_fd, GPIO_GET_LINEHANDLE_IOCTL, &req_cs) < 0) {
        LOG_ERROR("[IMU] Failed to request CS GPIO %d", GPIO_CS);
        close(miso_fd);
        close(mosi_fd);
        close(sclk_fd);
        close(gpio_chip_fd);
        return false;
    }
    cs_fd = req_cs.fd;
    
    LOG_INFO("[IMU] GPIO SPI initialized: SCLK=PD3(GPIO%d), MOSI=PD2(GPIO%d), MISO=PD4(GPIO%d), CS=PD5(GPIO%d)", 
             GPIO_SCLK, GPIO_MOSI, GPIO_MISO, GPIO_CS);
    return true;
}

static void sclk_high() {
    struct gpiohandle_data data;
    data.values[0] = 1;
    ioctl(sclk_fd, GPIOHANDLE_SET_LINE_VALUES_IOCTL, &data);
    usleep(SPI_DELAY_US);
}

static void sclk_low() {
    struct gpiohandle_data data;
    data.values[0] = 0;
    ioctl(sclk_fd, GPIOHANDLE_SET_LINE_VALUES_IOCTL, &data);
    usleep(SPI_DELAY_US);
}

static void mosi_high() {
    struct gpiohandle_data data;
    data.values[0] = 1;
    ioctl(mosi_fd, GPIOHANDLE_SET_LINE_VALUES_IOCTL, &data);
}

static void mosi_low() {
    struct gpiohandle_data data;
    data.values[0] = 0;
    ioctl(mosi_fd, GPIOHANDLE_SET_LINE_VALUES_IOCTL, &data);
}

static bool miso_read() {
    struct gpiohandle_data data;
    ioctl(miso_fd, GPIOHANDLE_GET_LINE_VALUES_IOCTL, &data);
    return data.values[0];
}

static void cs_select() {
    struct gpiohandle_data data;
    data.values[0] = 0;  // 低电平 = 选中
    ioctl(cs_fd, GPIOHANDLE_SET_LINE_VALUES_IOCTL, &data);
    usleep(SPI_DELAY_US);
}

static void cs_deselect() {
    struct gpiohandle_data data;
    data.values[0] = 1;  // 高电平 = 未选中
    ioctl(cs_fd, GPIOHANDLE_SET_LINE_VALUES_IOCTL, &data);
    usleep(SPI_DELAY_US);
}

// SPI 传输一个字节（Mode 0: CPOL=0, CPHA=0）
static uint8_t spi_transfer_byte(uint8_t tx_byte) {
    uint8_t rx_byte = 0;
    
    for (int i = 7; i >= 0; i--) {
        // 设置 MOSI（时钟下降沿前设置数据）
        if (tx_byte & (1 << i)) {
            mosi_high();
        } else {
            mosi_low();
        }
        
        // 上升沿：从设备采样 MOSI
        sclk_high();
        
        // 读取 MISO（时钟上升沿后读取数据）
        if (miso_read()) {
            rx_byte |= (1 << i);
        }
        
        // 下降沿：从设备改变 MISO
        sclk_low();
    }
    
    return rx_byte;
}

// 构造函数
ICM42688::ICM42688(const std::string& i2c_device, uint8_t address) 
    : m_i2c_device(i2c_device), m_address(address), m_fd(-1),
      m_ax(0), m_ay(0), m_az(0), 
      m_gx(0), m_gy(0), m_gz(0), 
      m_temp(0),
      m_accel_scale(0.0f), m_gyro_scale(0.0f) {
}

// 析构函数
ICM42688::~ICM42688() {
    if (cs_fd >= 0) close(cs_fd);
    if (miso_fd >= 0) close(miso_fd);
    if (mosi_fd >= 0) close(mosi_fd);
    if (sclk_fd >= 0) close(sclk_fd);
    if (gpio_chip_fd >= 0) close(gpio_chip_fd);
}

// 初始化 IMU
bool ICM42688::init() {
    // 1. 初始化 GPIO
    if (!gpio_init()) {
        LOG_ERROR("[IMU] GPIO initialization failed");
        return false;
    }
    
    m_fd = 1;  // 标记已初始化
    
    // 等待芯片上电稳定
    LOG_INFO("[IMU] Waiting for chip power-up (100ms)...");
    usleep(100000);
    
    // 2. 读取 WHO_AM_I 验证设备
    LOG_INFO("[IMU] Reading WHO_AM_I register...");
    uint8_t who_am_i = read_register(ICM42688_WHO_AM_I);
    if (who_am_i != ICM42688_DEVICE_ID && who_am_i != ICM42670_DEVICE_ID) {
        LOG_ERROR("[IMU] ICM42688 not found, WHO_AM_I = 0x%02X (expected 0x47 or 0x67)", who_am_i);
        return false;
    }
    LOG_INFO("[IMU] ICM42688 detected, WHO_AM_I = 0x%02X", who_am_i);
    
    // 3. 软复位
    write_register(ICM42688_PWR_MGMT0, 0x01);  // SOFT_RESET
    usleep(100000);  // 等待 100ms
    
    // 4. 设置加速度计和陀螺仪为待机
    write_register(ICM42688_PWR_MGMT0, 0x1F);
    usleep(1000);
    
    // 5. 配置加速度计（±16g, 1kHz ODR）
    write_register(ICM42688_ACCEL_CONFIG0, 
                   (uint8_t)AccelScale::AFS_16G << 5 | (uint8_t)ODR::ODR_1KHZ);
    m_accel_scale = 16.0f / 32768.0f;  // g
    
    // 6. 配置陀螺仪（±2000°/s, 1kHz ODR）
    write_register(ICM42688_GYRO_CONFIG0, 
                   (uint8_t)GyroScale::GFS_2000DPS << 5 | (uint8_t)ODR::ODR_1KHZ);
    m_gyro_scale = 2000.0f / 32768.0f;  // °/s
    
    // 7. 启动加速度计和陀螺仪
    write_register(ICM42688_PWR_MGMT0, 0x0F);  // ACCEL + GYRO ON
    usleep(100000);  // 等待 100ms
    
    LOG_INFO("[IMU] ICM42688 initialized successfully");
    return true;
}

// 读取传感器数据
bool ICM42688::read_sensor() {
    uint8_t data[14];
    
    if (!read_registers(ICM42688_TEMP_DATA1, 14, data)) {
        LOG_ERROR("[IMU] Failed to read sensor data");
        return false;
    }
    
    // 解析数据（大端序）
    m_temp = (int16_t)(data[0] << 8 | data[1]);
    m_ax   = (int16_t)(data[2] << 8 | data[3]);
    m_ay   = (int16_t)(data[4] << 8 | data[5]);
    m_az   = (int16_t)(data[6] << 8 | data[7]);
    m_gx   = (int16_t)(data[8] << 8 | data[9]);
    m_gy   = (int16_t)(data[10] << 8 | data[11]);
    m_gz   = (int16_t)(data[12] << 8 | data[13]);
    
    return true;
}

// 获取数据
float ICM42688::get_accel_x() { return (float)m_ax * m_accel_scale * G; }
float ICM42688::get_accel_y() { return (float)m_ay * m_accel_scale * G; }
float ICM42688::get_accel_z() { return (float)m_az * m_accel_scale * G; }
float ICM42688::get_gyro_x() { return (float)m_gx * m_gyro_scale; }
float ICM42688::get_gyro_y() { return (float)m_gy * m_gyro_scale; }
float ICM42688::get_gyro_z() { return (float)m_gz * m_gyro_scale; }
float ICM42688::get_temperature() { return ((float)m_temp / TEMP_SCALE) + TEMP_OFFSET; }

// 写入寄存器（SPI）
bool ICM42688::write_register(uint8_t reg, uint8_t value) {
    cs_select();
    
    // 写操作：寄存器地址最高位为 0
    spi_transfer_byte(reg & 0x7F);
    spi_transfer_byte(value);
    
    cs_deselect();
    return true;
}

// 读取单个寄存器（SPI）
uint8_t ICM42688::read_register(uint8_t reg) {
    cs_select();
    
    // 读操作：寄存器地址最高位为 1
    spi_transfer_byte(reg | 0x80);
    uint8_t value = spi_transfer_byte(0x00);  // 发送 dummy 字节读取数据
    
    cs_deselect();
    return value;
}

// 读取多个寄存器（SPI）
bool ICM42688::read_registers(uint8_t reg, uint8_t count, uint8_t* data) {
    cs_select();
    
    // 读操作：寄存器地址最高位为 1
    spi_transfer_byte(reg | 0x80);
    
    for (uint8_t i = 0; i < count; i++) {
        data[i] = spi_transfer_byte(0x00);  // 发送 dummy 字节读取数据
    }
    
    cs_deselect();
    return true;
}

// 读取所有传感器数据（便捷方法）
bool ICM42688::read_sensors(float& temp_c, float& ax, float& ay, float& az, 
                             float& gx, float& gy, float& gz) {
    if (!read_sensor()) {
        return false;
    }
    
    temp_c = get_temperature();
    ax = get_accel_x();
    ay = get_accel_y();
    az = get_accel_z();
    gx = get_gyro_x();
    gy = get_gyro_y();
    gz = get_gyro_z();
    
    return true;
}
