#include "CST816D.h"
#include "driver/i2c.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

static const char *TAG = "CST816D";

static void i2c_master_init(CST816D_t *dev) {
    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = dev->sda_pin,
        .scl_io_num = dev->scl_pin,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = 100000  // 100kHz
    };
    i2c_param_config(dev->i2c_port, &conf);
    i2c_driver_install(dev->i2c_port, conf.mode, 0, 0, 0);
}
static esp_err_t mpu9250_register_read(uint8_t reg_addr, uint8_t *data, size_t len)
{
    return i2c_master_write_read_device(0, I2C_ADDR_CST816D , &reg_addr, 1, data, len, 1000 / portTICK_PERIOD_MS);
}
void CST816D_init(CST816D_t *dev) {
    // Initialize I2C
    i2c_master_init(dev);

    // Configure reset pin
    if (dev->rst_pin != -1) {
        gpio_set_direction(dev->rst_pin, GPIO_MODE_OUTPUT);
        gpio_set_level(dev->rst_pin, 0);
        vTaskDelay(pdMS_TO_TICKS(10));
        gpio_set_level(dev->rst_pin, 1);
        vTaskDelay(pdMS_TO_TICKS(300));
    }

    // Configure interrupt pin
    if (dev->int_pin != -1) {
        gpio_set_direction(dev->int_pin, GPIO_MODE_OUTPUT);
        gpio_set_level(dev->int_pin, 1);
        vTaskDelay(pdMS_TO_TICKS(1));
        gpio_set_level(dev->int_pin, 0);
        vTaskDelay(pdMS_TO_TICKS(1));
    }

    // Initialize touch
    uint8_t data = 0xFF;
    i2c_master_write_to_device(dev->i2c_port, I2C_ADDR_CST816D, &data, 1, pdMS_TO_TICKS(1000));
    printf("init touch\n");
}

bool CST816D_getTouch(CST816D_t *dev, uint16_t *x, uint16_t *y, uint8_t *gesture) {
    uint8_t FingerIndex = 0;
    uint8_t reg1=0x02;
    mpu9250_register_read(0x02,&FingerIndex,1);
    FingerIndex = (bool)FingerIndex;
    // uint8_t *gesture = NULL;
    mpu9250_register_read(0x01,gesture,1);
    if (!(*gesture == SlideUp || *gesture == SlideDown))
    {
        *gesture = None;
    }
    uint8_t data[4];
    mpu9250_register_read(0x03,data,4);
    *x = ((data[0] & 0x0f) << 8) | data[1];
    printf("x: %d\n",*x);
    *y = ((data[2] & 0x0f) << 8) | data[3];
    printf("y: %d\n",*y);
    return FingerIndex;
    // uint8_t data[4];
    // uint8_t gesture_val = 0;
    // // printf("get touch\n");
    // // Read gesture
    // i2c_master_read_from_device(dev->i2c_port, I2C_ADDR_CST816D, &gesture_val, 1, pdMS_TO_TICKS(1000));
    // *gesture = gesture_val;

    // // Read touch coordinates
    // i2c_master_read_from_device(dev->i2c_port, I2C_ADDR_CST816D, data, 4, pdMS_TO_TICKS(1000));
    // *x = ((data[0] & 0x0F) << 8) | data[1];
    // *y = ((data[2] & 0x0F) << 8) | data[3];

    // return (*gesture != None);
}