#include "cst820_touch.h"
#include <array>

namespace {
constexpr int I2C_TIMEOUT_MS = 50;
// Touch point count has a retry loop in the reference driver because the chip can
// briefly report more points than the panel supports during a transition.
constexpr int MULTI_POINT_RETRIES = 3;
}

esp_err_t Cst820Touch::read_registers(uint8_t reg, uint8_t* data, size_t size) {
    if (!device_ || !data || !size) return ESP_ERR_INVALID_STATE;
    return i2c_master_transmit_receive(device_, &reg, 1, data, size, I2C_TIMEOUT_MS);
}

esp_err_t Cst820Touch::write_register(uint8_t reg, uint8_t value) {
    if (!device_) return ESP_ERR_INVALID_STATE;
    const std::array<uint8_t, 2> wire{reg, value};
    return i2c_master_transmit(device_, wire.data(), wire.size(), I2C_TIMEOUT_MS);
}

esp_err_t Cst820Touch::begin(i2c_master_bus_handle_t bus) {
    if (!bus) return ESP_ERR_INVALID_ARG;
    i2c_device_config_t config{};
    config.dev_addr_length = I2C_ADDR_BIT_LEN_7;
    config.device_address = ADDRESS;
    config.scl_speed_hz = 400000;
    auto result = i2c_master_bus_add_device(bus, &config, &device_);
    if (result != ESP_OK) { device_ = nullptr; return result; }

    // Chip ID read doubles as presence detection; a missing chip is nonfatal.
    uint8_t chip[3] = {};
    result = read_registers(REG_CHIP_ID, chip, sizeof(chip));
    if (result != ESP_OK) {
        i2c_master_bus_rm_device(device_);
        device_ = nullptr;
        return result;
    }
    // Verified init tail from Touch_CST816S::_check_init: exit command mode,
    // then emit an INT pulse on touch change with a ~2 ms low pulse. A failed
    // write means the controller is not usable; report nonfatal FAIL rather
    // than claiming ready.
    const struct { uint8_t reg, value; } init_writes[] = {{0x00, 0x00}, {0xFA, 0x20}, {0xED, 20}};
    for (const auto& write : init_writes) {
        result = write_register(write.reg, write.value);
        if (result != ESP_OK) {
            i2c_master_bus_rm_device(device_);
            device_ = nullptr;
            return result;
        }
    }
    return ESP_OK;
}

bool Cst820Touch::read(uint16_t& raw_x, uint16_t& raw_y) {
    if (!device_) return false;
    uint8_t data[6] = {};
    uint8_t points = 0;
    int retry = MULTI_POINT_RETRIES;
    do {
        // No I2C response means the finger is not on the panel.
        if (read_registers(REG_TOUCH, data, sizeof(data)) != ESP_OK) return false;
        points = data[0] & 0x0F;
    } while (points > 1 && --retry);
    if (!points) return false;
    // Verified bit layout from Touch_CST816S::getTouchRaw.
    raw_x = uint16_t(data[2] | (uint16_t(data[1] & 0x0F) << 8));
    raw_y = uint16_t(data[4] | (uint16_t(data[3] & 0x0F) << 8));
    return true;
}