/*
 * FT6336U Capacitive Touch Screen Driver Implementation
 */

#include "ft6336u.h"
#include "hardware/gpio.h"
#include <string.h>
#include <stdio.h>

static const ft6336u_config_t *g_config = NULL;

// Helper function to write a register
static bool ft6336u_write_reg(uint8_t reg, uint8_t value) {
    uint8_t buf[2] = {reg, value};
    int result = i2c_write_blocking(g_config->i2c, FT6336U_ADDR, buf, 2, false);
    return result == 2;
}

// Helper function to read a register
static bool ft6336u_read_reg(uint8_t reg, uint8_t *value) {
    int result = i2c_write_blocking(g_config->i2c, FT6336U_ADDR, &reg, 1, true);
    if (result != 1) {
        printf("[FT6336U] I2C write failed: result=%d (expected 1)\n", result);
        if (result == PICO_ERROR_GENERIC) printf("  Error: PICO_ERROR_GENERIC\n");
        if (result == PICO_ERROR_TIMEOUT) printf("  Error: PICO_ERROR_TIMEOUT\n");
        return false;
    }
    
    result = i2c_read_blocking(g_config->i2c, FT6336U_ADDR, value, 1, false);
    if (result != 1) {
        printf("[FT6336U] I2C read failed: result=%d (expected 1)\n", result);
        return false;
    }
    return true;
}

// Helper function to read multiple registers
static bool ft6336u_read_regs(uint8_t reg, uint8_t *buf, size_t len) {
    int result = i2c_write_blocking(g_config->i2c, FT6336U_ADDR, &reg, 1, true);
    if (result != 1) return false;
    
    result = i2c_read_blocking(g_config->i2c, FT6336U_ADDR, buf, len, false);
    return result == (int)len;
}

bool ft6336u_init(const ft6336u_config_t *config) {
    if (config == NULL) {
        printf("[FT6336U] ERROR: config is NULL\n");
        return false;
    }
    
    printf("[FT6336U] Initializing touch controller...\n");
    printf("[FT6336U] Pins: SDA=%d, SCL=%d, RST=%d, INT=%d\n",
           config->gpio_sda, config->gpio_scl, config->gpio_rst, config->gpio_int);
    
    g_config = config;
    
    // Initialize I2C
    printf("[FT6336U] Initializing I2C at 400 kHz...\n");
    uint actual_freq = i2c_init(config->i2c, 400000);  // 400 kHz
    printf("[FT6336U] I2C actual frequency: %u Hz\n", actual_freq);
    
    gpio_set_function(config->gpio_sda, GPIO_FUNC_I2C);
    gpio_set_function(config->gpio_scl, GPIO_FUNC_I2C);
    gpio_pull_up(config->gpio_sda);
    gpio_pull_up(config->gpio_scl);
    printf("[FT6336U] I2C pins configured\n");
    
    // Initialize reset pin
    gpio_init(config->gpio_rst);
    gpio_set_dir(config->gpio_rst, GPIO_OUT);
    printf("[FT6336U] Reset pin configured\n");
    
    // Initialize interrupt pin (input with pull-up)
    gpio_init(config->gpio_int);
    gpio_set_dir(config->gpio_int, GPIO_IN);
    gpio_pull_up(config->gpio_int);
    printf("[FT6336U] Interrupt pin configured\n");
    
    // Reset the touch controller
    printf("[FT6336U] Resetting touch controller...\n");
    gpio_put(config->gpio_rst, 0);
    sleep_ms(10);
    gpio_put(config->gpio_rst, 1);
    sleep_ms(300);  // Wait for chip to initialize
    printf("[FT6336U] Reset complete, reading chip ID...\n");
    
    // Verify chip ID
    uint8_t chip_id = ft6336u_get_chip_id();
    printf("[FT6336U] Chip ID read attempt 1: 0x%02X (expected 0x64)\n", chip_id);
    
    if (chip_id != 0x64) {
        // Try again - sometimes first read fails
        printf("[FT6336U] First read failed, retrying...\n");
        sleep_ms(100);
        chip_id = ft6336u_get_chip_id();
        printf("[FT6336U] Chip ID read attempt 2: 0x%02X\n", chip_id);
        
        if (chip_id != 0x64) {
            printf("[FT6336U] ERROR: Invalid chip ID! Check I2C wiring and address.\n");
            printf("[FT6336U] Possible issues:\n");
            printf("  - I2C pins not connected properly\n");
            printf("  - Touch controller not powered\n");
            printf("  - Wrong I2C address (using 0x%02X)\n", FT6336U_ADDR);
            printf("  - I2C pull-up resistors missing\n");
            return false;
        }
    }
    
    printf("[FT6336U] Chip ID verified successfully!\n");
    
    // Set to normal operating mode
    printf("[FT6336U] Setting normal operating mode...\n");
    if (!ft6336u_write_reg(FT6336U_REG_DEVICE_MODE, 0x00)) {
        printf("[FT6336U] WARNING: Failed to set device mode\n");
    }
    
    printf("[FT6336U] Initialization complete!\n");
    return true;
}

bool ft6336u_read_touch(ft6336u_touch_data_t *data) {
    if (data == NULL || g_config == NULL) return false;
    
    memset(data, 0, sizeof(ft6336u_touch_data_t));
    
    // Small delay to ensure touch controller is ready
    sleep_us(100);
    
    // Read gesture ID (optional, skip if causing issues)
    ft6336u_read_reg(FT6336U_REG_GESTURE_ID, &data->gesture);
    
    // Read number of touch points - retry on failure
    uint8_t td_status;
    int retry_count = 3;
    bool success = false;
    
    for (int retry = 0; retry < retry_count; retry++) {
        if (ft6336u_read_reg(FT6336U_REG_TD_STATUS, &td_status)) {
            success = true;
            break;
        }
        sleep_us(500);  // Brief delay before retry
    }
    
    if (!success) {
        return false;  // Failed after retries
    }
    
    data->touch_count = td_status & 0x0F;
    if (data->touch_count > FT6336U_MAX_TOUCH_POINTS) {
        data->touch_count = FT6336U_MAX_TOUCH_POINTS;
    }
    
    // If no touches, return early
    if (data->touch_count == 0) {
        return true;
    }
    
    // Read touch point data (6 bytes per point)
    for (int i = 0; i < data->touch_count; i++) {
        uint8_t reg_base = (i == 0) ? FT6336U_REG_P1_XH : FT6336U_REG_P2_XH;
        uint8_t buf[6];
        
        // Retry read if it fails
        bool read_success = false;
        for (int retry = 0; retry < 3; retry++) {
            if (ft6336u_read_regs(reg_base, buf, 6)) {
                read_success = true;
                break;
            }
            sleep_us(200);
        }
        
        if (!read_success) {
            return false;
        }
        
        // Parse touch point data
        data->points[i].event = (buf[0] >> 6) & 0x03;
        uint16_t raw_x = ((buf[0] & 0x0F) << 8) | buf[1];
        uint16_t raw_y = ((buf[2] & 0x0F) << 8) | buf[3];
        data->points[i].id = (buf[2] >> 4) & 0x0F;
        data->points[i].weight = buf[4];
        
        // Apply coordinate transformations
        if (g_config->swap_xy) {
            uint16_t temp = raw_x;
            raw_x = raw_y;
            raw_y = temp;
        }
        
        if (g_config->invert_x) {
            raw_x = g_config->screen_width - 1 - raw_x;
        }
        
        if (g_config->invert_y) {
            raw_y = g_config->screen_height - 1 - raw_y;
        }
        
        data->points[i].x = raw_x;
        data->points[i].y = raw_y;
        
        // Ensure coordinates are within screen bounds
        if (data->points[i].x >= g_config->screen_width) {
            data->points[i].x = g_config->screen_width - 1;
        }
        if (data->points[i].y >= g_config->screen_height) {
            data->points[i].y = g_config->screen_height - 1;
        }
    }
    
    return true;
}

bool ft6336u_is_touched(void) {
    if (g_config == NULL) return false;
    
    // More reliable: Read TD_STATUS register directly
    // INT pin can be unreliable due to timing/noise
    uint8_t td_status;
    if (!ft6336u_read_reg(FT6336U_REG_TD_STATUS, &td_status)) {
        return false;  // I2C error, assume not touched
    }
    
    uint8_t touch_count = td_status & 0x0F;
    return touch_count > 0;
}

uint8_t ft6336u_get_chip_id(void) {
    if (g_config == NULL) return 0xFF;
    
    uint8_t chip_id;
    if (!ft6336u_read_reg(FT6336U_REG_CHIPID, &chip_id)) {
        return 0xFF;
    }
    return chip_id;
}

uint8_t ft6336u_get_firmware_version(void) {
    if (g_config == NULL) return 0xFF;
    
    uint8_t fw_ver;
    if (!ft6336u_read_reg(FT6336U_REG_FIRMID, &fw_ver)) {
        return 0xFF;
    }
    return fw_ver;
}

void ft6336u_set_interrupt_callback(void (*callback)(uint gpio, uint32_t events)) {
    if (g_config == NULL || callback == NULL) return;
    
    // Enable interrupt on falling edge (touch detected)
    gpio_set_irq_enabled_with_callback(g_config->gpio_int, 
                                       GPIO_IRQ_EDGE_FALL, 
                                       true, 
                                       callback);
}

bool ft6336u_test_i2c(void) {
    if (g_config == NULL) {
        printf("[FT6336U] Test failed: not initialized\n");
        return false;
    }
    
    printf("[FT6336U] Testing I2C communication...\n");
    
    // Test 1: Read chip ID
    uint8_t chip_id = ft6336u_get_chip_id();
    printf("  Chip ID: 0x%02X (expected 0x64) - %s\n", 
           chip_id, chip_id == 0x64 ? "PASS" : "FAIL");
    
    // Test 2: Read firmware version
    uint8_t fw_ver = ft6336u_get_firmware_version();
    printf("  Firmware: 0x%02X - %s\n", 
           fw_ver, fw_ver != 0xFF ? "PASS" : "FAIL");
    
    // Test 3: Read TD_STATUS multiple times
    int success_count = 0;
    const int test_count = 10;
    for (int i = 0; i < test_count; i++) {
        uint8_t status;
        if (ft6336u_read_reg(FT6336U_REG_TD_STATUS, &status)) {
            success_count++;
        }
        sleep_ms(10);
    }
    printf("  TD_STATUS reads: %d/%d successful - %s\n", 
           success_count, test_count, 
           success_count == test_count ? "PASS" : "WARN");
    
    bool overall = (chip_id == 0x64) && (fw_ver != 0xFF) && (success_count >= test_count - 2);
    printf("[FT6336U] I2C test: %s\n", overall ? "PASS" : "FAIL");
    
    return overall;
}