#include "resistive_touch.h"

#include <math.h>

#include "esp_err.h"
#include "driver/gpio.h"
#include "driver/spi_master.h"

#define TOUCH_SPI_HOST SPI2_HOST

#define TOUCH_CMD_READ_X 0x90
#define TOUCH_CMD_READ_Y 0xD0
#define TOUCH_READ_TIMES 5
#define TOUCH_LOST_VALUES 1
#define TOUCH_ERR_RANGE 50

/*
 * Default calibration from the vendor resistive-touch example for 320x240
 * landscape mode. If your panel is offset, adjust these four constants.
 */
#define TOUCH_X_FACTOR 0.089750f
#define TOUCH_X_OFFSET (-14.0f)
#define TOUCH_Y_FACTOR (-0.064668f)
#define TOUCH_Y_OFFSET 251.0f

static spi_device_handle_t touch_spi;

static void touch_spi_bus_init(void)
{
    spi_bus_config_t bus_config = {
        .mosi_io_num = TOUCH_PIN_MOSI,
        .miso_io_num = TOUCH_PIN_MISO,
        .sclk_io_num = TOUCH_PIN_SCLK,
        .quadwp_io_num = -1,
        .quadhd_io_num = -1,
        .max_transfer_sz = 3,
    };

    esp_err_t err = spi_bus_initialize(TOUCH_SPI_HOST, &bus_config, SPI_DMA_CH_AUTO);
    if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {
        ESP_ERROR_CHECK(err);
    }
}

static uint16_t clamp_u16(float value, uint16_t max)
{
    if (value <= 0.0f) {
        return 0;
    }
    if (value >= (float)max) {
        return max;
    }
    return (uint16_t)lroundf(value);
}

static uint16_t touch_read_adc(uint8_t command)
{
    uint8_t tx[3] = {command, 0x00, 0x00};
    uint8_t rx[3] = {0};
    spi_transaction_t transaction = {
        .length = 24,
        .tx_buffer = tx,
        .rx_buffer = rx,
    };

    spi_device_polling_transmit(touch_spi, &transaction);
    return (uint16_t)((((uint16_t)rx[1] << 8) | rx[2]) >> 3) & 0x0fff;
}

static uint16_t touch_read_filtered_axis(uint8_t command)
{
    uint16_t values[TOUCH_READ_TIMES];

    for (int i = 0; i < TOUCH_READ_TIMES; i++) {
        values[i] = touch_read_adc(command);
    }

    for (int i = 0; i < TOUCH_READ_TIMES - 1; i++) {
        for (int j = i + 1; j < TOUCH_READ_TIMES; j++) {
            if (values[i] > values[j]) {
                uint16_t tmp = values[i];
                values[i] = values[j];
                values[j] = tmp;
            }
        }
    }

    uint32_t sum = 0;
    for (int i = TOUCH_LOST_VALUES; i < TOUCH_READ_TIMES - TOUCH_LOST_VALUES; i++) {
        sum += values[i];
    }

    return (uint16_t)(sum / (TOUCH_READ_TIMES - 2 * TOUCH_LOST_VALUES));
}

static bool touch_read_filtered_raw(uint16_t *x, uint16_t *y)
{
    uint16_t x1 = touch_read_filtered_axis(TOUCH_CMD_READ_X);
    uint16_t y1 = touch_read_filtered_axis(TOUCH_CMD_READ_Y);
    uint16_t x2 = touch_read_filtered_axis(TOUCH_CMD_READ_X);
    uint16_t y2 = touch_read_filtered_axis(TOUCH_CMD_READ_Y);

    if ((abs((int)x1 - (int)x2) >= TOUCH_ERR_RANGE) ||
        (abs((int)y1 - (int)y2) >= TOUCH_ERR_RANGE)) {
        return false;
    }

    *x = (uint16_t)((x1 + x2) / 2);
    *y = (uint16_t)((y1 + y2) / 2);
    return true;
}

bool resistive_touch_is_pressed(void)
{
    return gpio_get_level(TOUCH_PIN_IRQ) == 0;
}

bool resistive_touch_read_raw(uint16_t *x, uint16_t *y)
{
    if (!x || !y || !resistive_touch_is_pressed()) {
        return false;
    }

    return touch_read_filtered_raw(x, y);
}

bool resistive_touch_read(resistive_touch_point_t *point)
{
    uint16_t raw_x;
    uint16_t raw_y;

    if (!point) {
        return false;
    }

    point->pressed = false;
    if (!resistive_touch_read_raw(&raw_x, &raw_y)) {
        return false;
    }

    point->raw_x = raw_x;
    point->raw_y = raw_y;
    point->x = clamp_u16(TOUCH_X_FACTOR * raw_x + TOUCH_X_OFFSET, TOUCH_WIDTH - 1);
    point->y = clamp_u16(TOUCH_Y_FACTOR * raw_y + TOUCH_Y_OFFSET, TOUCH_HEIGHT - 1);
    point->pressed = true;
    return true;
}

void resistive_touch_init(void)
{
    gpio_config_t gpio_config_data = {
        .pin_bit_mask = 1ULL << TOUCH_PIN_IRQ,
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = GPIO_PULLUP_ENABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_DISABLE,
    };
    gpio_config(&gpio_config_data);

    touch_spi_bus_init();

    spi_device_interface_config_t device_config = {
        .clock_speed_hz = 2000000,
        .mode = 0,
        .spics_io_num = TOUCH_PIN_CS,
        .queue_size = 1,
    };

    ESP_ERROR_CHECK(spi_bus_add_device(TOUCH_SPI_HOST, &device_config, &touch_spi));
}