#include "touch.h"
#include "pins.h"
#include "lcd.h"

#include "hardware/gpio.h"
#include "hardware/spi.h"
#include "pico/stdlib.h"

#include <stdlib.h>

#define LCD_SPI_HZ (16 * 1000 * 1000)

/* PD1=PD0=0 → power-down after conversion, PENIRQ enabled */
#define XPT_CMD_X   0xD0
#define XPT_CMD_Y   0x90
#define XPT_CMD_Z1  0xB0
#define XPT_CMD_PWR 0x80

#define RAW_X_MIN  250
#define RAW_X_MAX  3900
#define RAW_Y_MIN  200
#define RAW_Y_MAX  3800

#define TOUCH_JUMP_REJECT 1800
#define TOUCH_Z_MIN       300

static uint16_t s_prev_rx, s_prev_ry;
static bool s_have_prev;

static inline void bb_delay(void)
{
    busy_wait_us(2);
}

static void spi_pins_to_gpio(void)
{
    gpio_init(PIN_T_CLK);
    gpio_set_dir(PIN_T_CLK, GPIO_OUT);
    gpio_put(PIN_T_CLK, 0);

    gpio_init(PIN_T_DIN);
    gpio_set_dir(PIN_T_DIN, GPIO_OUT);
    gpio_put(PIN_T_DIN, 0);

    gpio_init(PIN_T_DO);
    gpio_set_dir(PIN_T_DO, GPIO_IN);
}

static void spi_pins_restore(void)
{
    gpio_set_function(PIN_T_CLK, GPIO_FUNC_SPI);
    gpio_set_function(PIN_T_DIN, GPIO_FUNC_SPI);
    gpio_set_function(PIN_T_DO, GPIO_FUNC_SPI);
    spi_set_baudrate(TOUCH_SPI_INST, LCD_SPI_HZ);
}

static void bb_clk_pulse(void)
{
    gpio_put(PIN_T_CLK, 1);
    bb_delay();
    gpio_put(PIN_T_CLK, 0);
    bb_delay();
}

static void bb_write8(uint8_t v)
{
    for (int i = 7; i >= 0; i--) {
        gpio_put(PIN_T_DIN, (v >> i) & 1u);
        bb_delay();
        bb_clk_pulse();
    }
}

/* DOUT valid after falling CLK — sample with CLK low. */
static uint16_t bb_read12(void)
{
    uint16_t v = 0;
    bb_clk_pulse(); /* busy */
    for (int i = 0; i < 12; i++) {
        gpio_put(PIN_T_CLK, 1);
        bb_delay();
        gpio_put(PIN_T_CLK, 0);
        bb_delay();
        v = (uint16_t)((v << 1) | (gpio_get(PIN_T_DO) ? 1u : 0u));
    }
    bb_clk_pulse();
    bb_clk_pulse();
    bb_clk_pulse();
    return v;
}

static uint16_t xpt_sample(uint8_t cmd)
{
    bb_write8(cmd);
    return bb_read12();
}

static uint16_t xpt_read_avg(uint8_t cmd)
{
    (void)xpt_sample(cmd);
    busy_wait_us(100);

    uint32_t sum = 0;
    for (int i = 0; i < 3; i++) {
        sum += xpt_sample(cmd);
        busy_wait_us(30);
    }
    return (uint16_t)(sum / 3u);
}

static void xpt_power_down_penirq(void)
{
    /* Must end with PD1=PD0=0 or PENIRQ stays dead until reset. */
    bb_write8(XPT_CMD_PWR);
    for (int i = 0; i < 16; i++) {
        bb_clk_pulse();
    }
}

void touch_init(void)
{
    gpio_init(PIN_T_CS);
    gpio_set_dir(PIN_T_CS, GPIO_OUT);
    gpio_put(PIN_T_CS, 1);

    gpio_init(PIN_T_IRQ);
    gpio_set_dir(PIN_T_IRQ, GPIO_IN);
    gpio_pull_up(PIN_T_IRQ);

    gpio_init(PIN_SD_CS);
    gpio_set_dir(PIN_SD_CS, GPIO_OUT);
    gpio_put(PIN_SD_CS, 1);

    gpio_put(PIN_LCD_CS, 1);
    gpio_set_function(PIN_T_DO, GPIO_FUNC_SPI);

    /* Ensure chip starts with PENIRQ armed. */
    spi_pins_to_gpio();
    gpio_put(PIN_T_CS, 0);
    busy_wait_us(5);
    xpt_power_down_penirq();
    gpio_put(PIN_T_CS, 1);
    spi_pins_restore();
}

bool touch_pressed(void)
{
    return gpio_get(PIN_T_IRQ) == 0;
}

bool touch_read_raw_ex(uint16_t *out_x, uint16_t *out_y, uint16_t *out_z)
{
    if (!out_x || !out_y) {
        return false;
    }

    /*
     * Do NOT disable IRQs for the whole transfer — that starved TinyUSB
     * (serial dropped) and could abort mid-frame leaving PENIRQ disabled.
     */
    gpio_put(PIN_LCD_CS, 1);
    gpio_put(PIN_SD_CS, 1);

    spi_pins_to_gpio();
    gpio_put(PIN_T_CS, 0);
    busy_wait_us(10);

    uint16_t z1 = xpt_read_avg(XPT_CMD_Z1);
    uint16_t x = xpt_read_avg(XPT_CMD_X);
    uint16_t y = xpt_read_avg(XPT_CMD_Y);

    xpt_power_down_penirq();

    gpio_put(PIN_T_CS, 1);
    spi_pins_restore();

    if (out_z) {
        *out_z = z1;
    }
    *out_x = x;
    *out_y = y;
    return true;
}

bool touch_read_raw(uint16_t *out_x, uint16_t *out_y)
{
    uint16_t z = 0;
    return touch_read_raw_ex(out_x, out_y, &z);
}

static int16_t map_clamp(int32_t v, int32_t in_min, int32_t in_max,
                         int32_t out_min, int32_t out_max)
{
    if (in_max == in_min) {
        return (int16_t)out_min;
    }
    if (v < in_min) {
        v = in_min;
    }
    if (v > in_max) {
        v = in_max;
    }
    /* out_max < out_min is fine (axis flip). */
    return (int16_t)(out_min + (v - in_min) * (out_max - out_min) / (in_max - in_min));
}

static void map_to_screen(uint16_t rx, uint16_t ry, int16_t *sx, int16_t *sy)
{
    /*
     * Landscape + MADCTL_MV: screen X ← touch raw_y, screen Y ← touch raw_x.
     * Mirrors match this panel (X was reversed in bring-up).
     */
    int16_t x = map_clamp(ry, RAW_Y_MIN, RAW_Y_MAX, 0, LCD_WIDTH - 1);
    int16_t y = map_clamp(rx, RAW_X_MIN, RAW_X_MAX, 0, LCD_HEIGHT - 1);

#if TOUCH_MIRROR_X
    x = (int16_t)((LCD_WIDTH - 1) - x);
#endif
#if TOUCH_MIRROR_Y
    y = (int16_t)((LCD_HEIGHT - 1) - y);
#endif
    if (sx) {
        *sx = x;
    }
    if (sy) {
        *sy = y;
    }
}

bool touch_read_screen(int16_t *out_x, int16_t *out_y)
{
    uint16_t rx, ry;
    if (!touch_read_raw(&rx, &ry)) {
        return false;
    }
    map_to_screen(rx, ry, out_x, out_y);
    return true;
}

bool touch_read(uint16_t *raw_x, uint16_t *raw_y, int16_t *sx, int16_t *sy)
{
    return touch_read_diag(raw_x, raw_y, NULL, sx, sy);
}

bool touch_read_diag(uint16_t *raw_x, uint16_t *raw_y, uint16_t *z,
                     int16_t *sx, int16_t *sy)
{
    uint16_t rx = 0, ry = 0, zz = 0;
    if (!touch_read_raw_ex(&rx, &ry, &zz)) {
        return false;
    }

    if (zz < TOUCH_Z_MIN) {
        s_have_prev = false;
        return false;
    }

    if (s_have_prev) {
        int dx = (int)rx - (int)s_prev_rx;
        int dy = (int)ry - (int)s_prev_ry;
        if (dx < 0) {
            dx = -dx;
        }
        if (dy < 0) {
            dy = -dy;
        }
        if (dx > TOUCH_JUMP_REJECT || dy > TOUCH_JUMP_REJECT) {
            return false;
        }
    }
    s_prev_rx = rx;
    s_prev_ry = ry;
    s_have_prev = true;

    if (raw_x) {
        *raw_x = rx;
    }
    if (raw_y) {
        *raw_y = ry;
    }
    if (z) {
        *z = zz;
    }
    map_to_screen(rx, ry, sx, sy);
    return true;
}

void touch_filter_reset(void)
{
    s_have_prev = false;
}