#include "touch.h"
#include <stdio.h>
#include <unistd.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>

static int i2c_fd = -1;   // I2C file descriptor

void Touch_Reset(void) {
    pinMode(TOUCH_RST_PIN, OUTPUT);
    
    digitalWrite(TOUCH_RST_PIN, HIGH);
    delay(50);
    digitalWrite(TOUCH_RST_PIN, LOW);
    delay(10);
    digitalWrite(TOUCH_RST_PIN, HIGH);
    delay(50);
}

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

    // Initialize I2C
    i2c_fd = wiringPiI2CSetup(CST816D_ADDR);
    if(i2c_fd < 0) {
        printf("Touch: I2C Init Failed\n");
        return;
    }
    
    // Initialize Interrupt Pin
    pinMode(TOUCH_INT_PIN, INPUT);

    Touch_Reset();
    
    printf("Touch: Init Done (FD=%d)\n", i2c_fd);
}

uint8_t Touch_Read_Point(TouchPoint *point) {
    uint8_t buf[6];
    
    if (i2c_fd < 0) return 0;
    
     // CST816D protocol: write register address, then read data
    uint8_t reg = 0x01;
    write(i2c_fd, &reg, 1);
    
    // Read 6 bytes of touch data
    if (read(i2c_fd, buf, 6) != 6) {
        return 0; // Read error
    }
    
    // buf[2] is finger number
    uint8_t finger_num = buf[2];
    
    if (finger_num > 0) {
        // Parse coordinates
        uint16_t x_raw = ((buf[2] & 0x0F) << 8) | buf[3];
        uint16_t y_raw = ((buf[4] & 0x0F) << 8) | buf[5];
        //printf("%d,%d",x_raw,y_raw);
        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;
    }
}
