/*
 * FT6336U Capacitive Touch Screen Driver Implementation
 */

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

static ft6336u_config_t g_config_storage;
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) {
    // Retry up to 3 times like Arduino library
    for (int retry = 0; retry < 3; retry++) {
        int result = i2c_write_blocking(g_config->i2c, FT6336U_ADDR, &reg, 1, true);
        if (result != 1) {
            sleep_us(1000);  // 1ms delay before retry
            continue;
        }
        
        // Short settle time between register address write and read.
        // 10ms here heavily throttles touch sampling during hold/move.
        sleep_us(100);  // 0.1ms
        
        result = i2c_read_blocking(g_config->i2c, FT6336U_ADDR, value, 1, false);
        if (result == 1) {
            return true;
        }
        
        sleep_us(1000);  // 1ms delay before retry
    }
    return false;
}

// Helper function to read multiple registers
static bool ft6336u_read_regs(uint8_t reg, uint8_t *buf, size_t len) {
    // Retry up to 3 times
    for (int retry = 0; retry < 3; retry++) {
        int result = i2c_write_blocking(g_config->i2c, FT6336U_ADDR, &reg, 1, true);
        if (result != 1) {
            sleep_us(1000);  // 1ms delay before retry
            continue;
        }
        
        // Short settle time between register address write and read.
        // 10ms here heavily throttles touch sampling during hold/move.
        sleep_us(100);  // 0.1ms
        
        result = i2c_read_blocking(g_config->i2c, FT6336U_ADDR, buf, len, false);
        if (result == (int)len) {
            return true;
        }
        
        sleep_us(1000);  // 1ms delay before retry
    }
    return false;
}

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);
    
    // Copy config to static storage to avoid dangling pointer
    memcpy(&g_config_storage, config, sizeof(ft6336u_config_t));
    g_config = &g_config_storage;
    
    // 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");
    }
    
    // Set CTRL mode to keep active (better interrupt responsiveness)
    printf("[FT6336U] Setting CTRL mode to keep active...\n");
    if (!ft6336u_write_reg(FT6336U_REG_CTRL, FT6336U_CTRL_KEEP_ACTIVE_MODE)) {
        printf("[FT6336U] WARNING: Failed to set CTRL mode\n");
    }
    
    
    // Configure gesture parameters for better detection
    printf("[FT6336U] Configuring gesture detection parameters...\n");
    
    // Radian value: smaller = stricter angles (default ~16, using 10 for easier detection)
    ft6336u_set_radian_value(10);
    
    // Offset values: starting point for gesture detection (using 20 pixels)
    ft6336u_set_offset_left_right(20);
    ft6336u_set_offset_up_down(20);
    
    // Distance thresholds: minimum movement for gesture (using 40 pixels for easier detection)
    ft6336u_set_distance_left_right(40);
    ft6336u_set_distance_up_down(40);
    
    // Zoom distance: minimum pinch/spread distance (using 80)
    ft6336u_set_distance_zoom(80);
    
    // Verify modes and gesture parameters were set
    uint8_t ctrl_mode = ft6336u_get_ctrl_mode();
    uint8_t g_mode = ft6336u_get_g_mode();
    printf("[FT6336U] CTRL_MODE: 0x%02X (%s)\n", 
           ctrl_mode, ctrl_mode == FT6336U_CTRL_KEEP_ACTIVE_MODE ? "keep active" : "monitor");
    printf("[FT6336U] G_MODE: 0x%02X (%s mode)\n", 
           g_mode, g_mode == FT6336U_G_MODE_TRIGGER ? "trigger" : "polling");
    
    // Display gesture parameter values
    printf("[FT6336U] Gesture Parameters:\n");
    printf("  Radian: %d, Offset LR: %d, Offset UD: %d\n",
           ft6336u_get_radian_value(), 
           ft6336u_get_offset_left_right(),
           ft6336u_get_offset_up_down());
    printf("  Distance LR: %d, Distance UD: %d, Zoom: %d\n",
           ft6336u_get_distance_left_right(),
           ft6336u_get_distance_up_down(),
           ft6336u_get_distance_zoom());
    
    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));
    
    // Read gesture ID
    ft6336u_read_reg(FT6336U_REG_GESTURE_ID, &data->gesture);
    
    // Read number of touch points (with retries)
    uint8_t td_status;
    if (!ft6336u_read_reg(FT6336U_REG_TD_STATUS, &td_status)) {
        return false;  // I2C error
    }
    
    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: XH, XL, YH, YL, WEIGHT, MISC)
    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];
        
        if (!ft6336u_read_regs(reg_base, buf, 6)) {
            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];     // Touch pressure/area
        data->points[i].misc = (buf[5] >> 4) & 0x0F;  // Touch area (upper nibble)
        
        // 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;
    
    // In trigger mode, INT pin goes LOW when touched
    // This is faster than reading via I2C
    if (!gpio_get(g_config->gpio_int)) {
        return true;
        //INT pin is LOW, likely touched - confirm with I2C read
        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;
    }
    
    // INT pin is HIGH, no touch
    return false;
}

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;
    
    // In trigger mode:
    // - INT pin goes LOW (falling edge) when touch is detected
    // - INT pin goes HIGH (rising edge) when touch is released
    // Enable interrupts on both edges for full touch lifecycle tracking
    gpio_set_irq_enabled_with_callback(g_config->gpio_int, 
                                       GPIO_IRQ_EDGE_FALL | GPIO_IRQ_EDGE_RISE, 
                                       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;
}

bool ft6336u_set_g_mode(uint8_t mode) {
    if (g_config == NULL) return false;
    return ft6336u_write_reg(FT6336U_REG_G_MODE, mode);
}

uint8_t ft6336u_get_g_mode(void) {
    if (g_config == NULL) return 0xFF;
    
    uint8_t g_mode;
    if (!ft6336u_read_reg(FT6336U_REG_G_MODE, &g_mode)) {
        return 0xFF;
    }
    return g_mode;
}

uint8_t ft6336u_get_power_mode(void) {
    if (g_config == NULL) return 0xFF;
    
    uint8_t pwr_mode;
    if (!ft6336u_read_reg(FT6336U_REG_POWER_MODE, &pwr_mode)) {
        return 0xFF;
    }
    return pwr_mode;
}

bool ft6336u_set_ctrl_mode(uint8_t mode) {
    if (g_config == NULL) return false;
    return ft6336u_write_reg(FT6336U_REG_CTRL, mode);
}

uint8_t ft6336u_get_ctrl_mode(void) {
    if (g_config == NULL) return 0xFF;
    
    uint8_t ctrl_mode;
    if (!ft6336u_read_reg(FT6336U_REG_CTRL, &ctrl_mode)) {
        return 0xFF;
    }
    return ctrl_mode;
}

bool ft6336u_get_int_pin_state(void) {
    if (g_config == NULL) return true;  // Default to HIGH (no interrupt)
    // In trigger mode: LOW = touch active, HIGH = no touch
    return gpio_get(g_config->gpio_int);
}

// Gesture parameter configuration functions
uint8_t ft6336u_get_radian_value(void) {
    if (g_config == NULL) return 0xFF;
    uint8_t value;
    if (!ft6336u_read_reg(FT6336U_REG_RADIAN_VALUE, &value)) {
        return 0xFF;
    }
    return value;
}

bool ft6336u_set_radian_value(uint8_t value) {
    if (g_config == NULL) return false;
    return ft6336u_write_reg(FT6336U_REG_RADIAN_VALUE, value);
}

uint8_t ft6336u_get_offset_left_right(void) {
    if (g_config == NULL) return 0xFF;
    uint8_t value;
    if (!ft6336u_read_reg(FT6336U_REG_OFFSET_LEFT_RIGHT, &value)) {
        return 0xFF;
    }
    return value;
}

bool ft6336u_set_offset_left_right(uint8_t value) {
    if (g_config == NULL) return false;
    return ft6336u_write_reg(FT6336U_REG_OFFSET_LEFT_RIGHT, value);
}

uint8_t ft6336u_get_offset_up_down(void) {
    if (g_config == NULL) return 0xFF;
    uint8_t value;
    if (!ft6336u_read_reg(FT6336U_REG_OFFSET_UP_DOWN, &value)) {
        return 0xFF;
    }
    return value;
}

bool ft6336u_set_offset_up_down(uint8_t value) {
    if (g_config == NULL) return false;
    return ft6336u_write_reg(FT6336U_REG_OFFSET_UP_DOWN, value);
}

uint8_t ft6336u_get_distance_left_right(void) {
    if (g_config == NULL) return 0xFF;
    uint8_t value;
    if (!ft6336u_read_reg(FT6336U_REG_DISTANCE_LEFT_RIGHT, &value)) {
        return 0xFF;
    }
    return value;
}

bool ft6336u_set_distance_left_right(uint8_t value) {
    if (g_config == NULL) return false;
    return ft6336u_write_reg(FT6336U_REG_DISTANCE_LEFT_RIGHT, value);
}

uint8_t ft6336u_get_distance_up_down(void) {
    if (g_config == NULL) return 0xFF;
    uint8_t value;
    if (!ft6336u_read_reg(FT6336U_REG_DISTANCE_UP_DOWN, &value)) {
        return 0xFF;
    }
    return value;
}

bool ft6336u_set_distance_up_down(uint8_t value) {
    if (g_config == NULL) return false;
    return ft6336u_write_reg(FT6336U_REG_DISTANCE_UP_DOWN, value);
}

uint8_t ft6336u_get_distance_zoom(void) {
    if (g_config == NULL) return 0xFF;
    uint8_t value;
    if (!ft6336u_read_reg(FT6336U_REG_DISTANCE_ZOOM, &value)) {
        return 0xFF;
    }
    return value;
}

bool ft6336u_set_distance_zoom(uint8_t value) {
    if (g_config == NULL) return false;
    return ft6336u_write_reg(FT6336U_REG_DISTANCE_ZOOM, value);
}