#include "touch.h"
#include <stdio.h>
#include <unistd.h>
#include "sg_lgpio.h"

extern int I2C_handle; 
extern int Gpio_handle;

// Reset the CST816D touch controller
void Touch_Reset(void) {
    // Configure RST pin as output
    Lgpio_set_pinmode(TOUCH_RST_PIN, LGPIO_OUTPUT);
    
    // Reset timing: HIGH -> LOW -> HIGH
    Lgpio_write_pin(TOUCH_RST_PIN, LGPIO_HIGH);
    delay(50);
    Lgpio_write_pin(TOUCH_RST_PIN, LGPIO_LOW);
    delay(10);
    Lgpio_write_pin(TOUCH_RST_PIN, LGPIO_HIGH);
    delay(50);
}

void Touch_Init(void) {
    printf("Touch: Init Start...\n");

     // Initialize I2C (using I2C-1, address 0x15)
    LG_I2C_Init(CST816D_ADDR);
    
    // Initialize interrupt pin as input
    Lgpio_set_pinmode(TOUCH_INT_PIN, LGPIO_INPUT);

    // Reset the touch chip
    Touch_Reset();
    
    printf("Touch: Init Done (BCM RST=%d, INT=%d)\n", TOUCH_RST_PIN, TOUCH_INT_PIN);
}

uint8_t Touch_Read_Point(TouchPoint *point) {
    uint8_t buf[7];
    int res;

    // Read 7 registers starting from address 0x01
    res = LG_I2C_Read_Regs(0x01, buf, 7);
    
    if (res < 0) {
        // I2C read failed
        return 0;
    }
    // buf[2] contains the number of fingers
    uint8_t finger_num = buf[2];
    
    if (finger_num > 0) {
        // Parse raw coordinates (12-bit values)
        uint16_t x_raw = ((buf[3] & 0x0F) << 8) | buf[4];
        uint16_t y_raw = ((buf[5] & 0x0F) << 8) | buf[6];
        
        point->x = y_raw;
        point->y = x_raw;

        if (point->x >= 320) point->x = 319;
        if (point->y >= 240) point->y = 239;

        point->pressed = 1;
        
        return 1;
    } else {
        point->pressed = 0;
        return 0;
    }
}
