#include "pn532_host/pn532_transport.h"
#include "sdkconfig.h"
#include "driver/gpio.h"
#include "esp_timer.h"
#include "driver/i2c.h"
#include "driver/spi_master.h"
#include "driver/uart.h"
#include "esp_check.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include <string.h>
#include <sys/param.h>

#define PN532_HOST_TO_PN532 0xD4
#define PN532_PN532_TO_HOST 0xD5
#define PN532_TXBUF_MAX 264

static const char *TAG = "pn532_xport";

#if defined(CONFIG_PN532_TRANSPORT_SPI)
static spi_host_device_t pn532_spi_host(void)
{
    return CONFIG_PN532_SPI_HOST == 3 ? SPI3_HOST : SPI2_HOST;
}
#endif

static SemaphoreHandle_t s_bus_mutex;

#if defined(CONFIG_PN532_TRANSPORT_SPI)
static spi_device_handle_t s_spi;
static int s_spi_cs_gpio = CONFIG_PN532_SPI_CS_GPIO;

static esp_err_t spi_wait_ready(int timeout_ms)
{
    uint8_t status = 0;
    const int64_t end = esp_timer_get_time() / 1000 + timeout_ms;
    while ((esp_timer_get_time() / 1000) < (uint64_t)end) {
        gpio_set_level(s_spi_cs_gpio, 0);
        uint8_t cmd = 0x02; /* Status read */
        spi_transaction_t t_cmd = {};
        t_cmd.length = 8;
        t_cmd.tx_buffer = &cmd;
        esp_err_t e = spi_device_polling_transmit(s_spi, &t_cmd);
        if (e == ESP_OK) {
            uint8_t tx = 0x00;
            spi_transaction_t t_status = {};
            t_status.length = 8;
            t_status.tx_buffer = &tx;
            t_status.rx_buffer = &status;
            e = spi_device_polling_transmit(s_spi, &t_status);
        }
        gpio_set_level(s_spi_cs_gpio, 1);
        if (e != ESP_OK) {
            return e;
        }
        if (status & 0x01) {
            return ESP_OK;
        }
        vTaskDelay(pdMS_TO_TICKS(1));
    }
    return ESP_ERR_TIMEOUT;
}

static esp_err_t spi_write_frame(const uint8_t *data, size_t len)
{
    ESP_RETURN_ON_ERROR(spi_wait_ready(200), TAG, "wait before write");
    gpio_set_level(s_spi_cs_gpio, 0);
    vTaskDelay(pdMS_TO_TICKS(2));
    uint8_t hdr = 0x01; /* Data write */
    spi_transaction_t t0 = {};
    t0.length = 8;
    t0.tx_buffer = &hdr;
    ESP_RETURN_ON_ERROR(spi_device_polling_transmit(s_spi, &t0), TAG, "hdr");
    for (size_t i = 0; i < len; i++) {
        spi_transaction_t t = {};
        t.length = 8;
        t.tx_buffer = &data[i];
        ESP_RETURN_ON_ERROR(spi_device_polling_transmit(s_spi, &t), TAG, "w");
    }
    gpio_set_level(s_spi_cs_gpio, 1);
    return ESP_OK;
}

static esp_err_t spi_read_bytes(uint8_t *out, size_t len)
{
    ESP_RETURN_ON_ERROR(spi_wait_ready(300), TAG, "wait read");
    gpio_set_level(s_spi_cs_gpio, 0);
    vTaskDelay(pdMS_TO_TICKS(2));
    uint8_t hdr = 0x03; /* Data read */
    spi_transaction_t t0 = {};
    t0.length = 8;
    t0.tx_buffer = &hdr;
    ESP_RETURN_ON_ERROR(spi_device_polling_transmit(s_spi, &t0), TAG, "rhdr");
    for (size_t i = 0; i < len; i++) {
        spi_transaction_t t = {};
        uint8_t tx = 0xFF;
        t.length = 8;
        t.tx_buffer = &tx;
        t.rx_buffer = &out[i];
        ESP_RETURN_ON_ERROR(spi_device_polling_transmit(s_spi, &t), TAG, "r");
    }
    gpio_set_level(s_spi_cs_gpio, 1);
    return ESP_OK;
}

#elif defined(CONFIG_PN532_TRANSPORT_I2C)
#define PN532_I2C_PORT ((i2c_port_t)CONFIG_PN532_I2C_PORT)

static esp_err_t i2c_wakeup(void)
{
    const uint8_t w[] = {0x55, 0x55, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    i2c_cmd_handle_t c = i2c_cmd_link_create();
    i2c_master_start(c);
    i2c_master_write_byte(c, (CONFIG_PN532_I2C_ADDR << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write(c, w, sizeof(w), I2C_MASTER_LAST_NACK);
    i2c_master_stop(c);
    esp_err_t e = i2c_master_cmd_begin(PN532_I2C_PORT, c, pdMS_TO_TICKS(50));
    i2c_cmd_link_delete(c);
    vTaskDelay(pdMS_TO_TICKS(10));
    return e;
}

static esp_err_t i2c_write_raw(const uint8_t *buf, size_t len)
{
    i2c_cmd_handle_t c = i2c_cmd_link_create();
    i2c_master_start(c);
    i2c_master_write_byte(c, (CONFIG_PN532_I2C_ADDR << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write(c, buf, len, I2C_MASTER_LAST_NACK);
    i2c_master_stop(c);
    esp_err_t e = i2c_master_cmd_begin(PN532_I2C_PORT, c, pdMS_TO_TICKS(200));
    i2c_cmd_link_delete(c);
    return e;
}

static esp_err_t i2c_read_raw(uint8_t *buf, size_t len)
{
    if (!buf || len == 0) {
        return ESP_ERR_INVALID_ARG;
    }
    i2c_cmd_handle_t c = i2c_cmd_link_create();
    i2c_master_start(c);
    i2c_master_write_byte(c, (CONFIG_PN532_I2C_ADDR << 1) | I2C_MASTER_READ, true);
    if (len > 1) {
        i2c_master_read(c, buf, len - 1, I2C_MASTER_ACK);
    }
    i2c_master_read_byte(c, buf + len - 1, I2C_MASTER_NACK);
    i2c_master_stop(c);
    esp_err_t e = i2c_master_cmd_begin(PN532_I2C_PORT, c, pdMS_TO_TICKS(200));
    i2c_cmd_link_delete(c);
    return e;
}

static esp_err_t i2c_read_frame(uint8_t *buf, size_t len)
{
    if (!buf || len == 0) {
        return ESP_ERR_INVALID_ARG;
    }
    uint8_t raw[272];
    if (len + 1 > sizeof(raw)) {
        return ESP_ERR_INVALID_SIZE;
    }
    ESP_RETURN_ON_ERROR(i2c_read_raw(raw, len + 1), TAG, "i2c read");
    if (raw[0] != 0x01) {
        ESP_LOGW(TAG, "unexpected i2c status 0x%02x", raw[0]);
        return ESP_ERR_INVALID_RESPONSE;
    }
    memcpy(buf, raw + 1, len);
    return ESP_OK;
}

static esp_err_t i2c_wait_ready(int timeout_ms)
{
    int64_t t0 = esp_timer_get_time() / 1000;
    while (((esp_timer_get_time() / 1000) - t0) < timeout_ms) {
        uint8_t status = 0;
        if (i2c_read_raw(&status, 1) == ESP_OK && status == 0x01) {
            return ESP_OK;
        }
        vTaskDelay(pdMS_TO_TICKS(2));
    }
    return ESP_ERR_TIMEOUT;
}

#elif defined(CONFIG_PN532_TRANSPORT_HSU)
#define PN532_UART ((uart_port_t)CONFIG_PN532_HSU_UART_NUM)

static esp_err_t hsu_write_raw(const uint8_t *buf, size_t len)
{
    int n = uart_write_bytes(PN532_UART, buf, len);
    return (n == (int)len) ? ESP_OK : ESP_FAIL;
}

static esp_err_t hsu_read_raw(uint8_t *buf, size_t len, int timeout_ms)
{
    size_t got = 0;
    int64_t start = esp_timer_get_time();
    while (got < len) {
        int n = uart_read_bytes(PN532_UART, buf + got, len - got,
                                 pdMS_TO_TICKS(MAX(1, timeout_ms)));
        if (n > 0) {
            got += (size_t)n;
        }
        if ((esp_timer_get_time() - start) / 1000 > timeout_ms) {
            break;
        }
    }
    return got == len ? ESP_OK : ESP_ERR_TIMEOUT;
}
#endif

void pn532_transport_lock(void) { xSemaphoreTake(s_bus_mutex, portMAX_DELAY); }

void pn532_transport_unlock(void) { xSemaphoreGive(s_bus_mutex); }

static esp_err_t read_ack(int timeout_ms)
{
    const uint8_t ack_ok[] = {0x00, 0x00, 0xFF, 0x00, 0xFF, 0x00};
    uint8_t ack[6];
#if defined(CONFIG_PN532_TRANSPORT_SPI)
    ESP_RETURN_ON_ERROR(spi_read_bytes(ack, sizeof(ack)), TAG, "read ack spi");
#elif defined(CONFIG_PN532_TRANSPORT_I2C)
    ESP_RETURN_ON_ERROR(i2c_wait_ready(timeout_ms), TAG, "wait ack i2c");
    ESP_RETURN_ON_ERROR(i2c_read_frame(ack, sizeof(ack)), TAG, "read ack i2c");
#elif defined(CONFIG_PN532_TRANSPORT_HSU)
    ESP_RETURN_ON_ERROR(hsu_read_raw(ack, sizeof(ack), timeout_ms), TAG, "read ack hsu");
#endif
    if (memcmp(ack, ack_ok, 6) != 0) {
        ESP_LOG_BUFFER_HEX_LEVEL(TAG, ack, 6, ESP_LOG_WARN);
        return ESP_ERR_INVALID_RESPONSE;
    }
    return ESP_OK;
}

static esp_err_t read_response_frame(uint8_t *body_out, size_t body_max, size_t *body_len, int timeout_ms)
{
    if (!body_out || !body_len) {
        return ESP_ERR_INVALID_ARG;
    }
    uint8_t hdr[8];
    size_t frame_len = 0;
#if defined(CONFIG_PN532_TRANSPORT_SPI)
    ESP_RETURN_ON_ERROR(spi_read_bytes(hdr, 5), TAG, "hdr spi");
#elif defined(CONFIG_PN532_TRANSPORT_I2C)
    ESP_RETURN_ON_ERROR(i2c_wait_ready(timeout_ms), TAG, "wait frame i2c");
    ESP_RETURN_ON_ERROR(i2c_read_frame(hdr, 5), TAG, "hdr i2c");
#elif defined(CONFIG_PN532_TRANSPORT_HSU)
    ESP_RETURN_ON_ERROR(hsu_read_raw(hdr, 5, timeout_ms), TAG, "hdr hsu");
#endif
    if (hdr[0] != 0x00 || hdr[1] != 0x00 || hdr[2] != 0xFF) {
        ESP_LOG_BUFFER_HEX_LEVEL(TAG, hdr, 5, ESP_LOG_WARN);
        return ESP_ERR_INVALID_RESPONSE;
    }

    if (hdr[3] == 0xFF && hdr[4] == 0xFF) {
#if defined(CONFIG_PN532_TRANSPORT_SPI)
        ESP_RETURN_ON_ERROR(spi_read_bytes(hdr + 5, 3), TAG, "hdr ext spi");
#elif defined(CONFIG_PN532_TRANSPORT_I2C)
        ESP_RETURN_ON_ERROR(i2c_read_frame(hdr + 5, 3), TAG, "hdr ext i2c");
#elif defined(CONFIG_PN532_TRANSPORT_HSU)
        ESP_RETURN_ON_ERROR(hsu_read_raw(hdr + 5, 3, timeout_ms), TAG, "hdr ext hsu");
#endif
        frame_len = (size_t)(((uint16_t)hdr[5] << 8) | hdr[6]);
        if ((uint8_t)(hdr[5] + hdr[6] + hdr[7]) != 0) {
            return ESP_ERR_INVALID_CRC;
        }
    } else {
        frame_len = hdr[3];
        if ((uint8_t)(hdr[3] + hdr[4]) != 0) {
            return ESP_ERR_INVALID_CRC;
        }
    }

    if (frame_len < 2) {
        return ESP_ERR_INVALID_SIZE;
    }

    const size_t read_total = frame_len + 2;
    if (read_total > 272) {
        return ESP_ERR_INVALID_SIZE;
    }
    uint8_t chunk[272];
#if defined(CONFIG_PN532_TRANSPORT_SPI)
    ESP_RETURN_ON_ERROR(spi_read_bytes(chunk, read_total), TAG, "payload spi");
#elif defined(CONFIG_PN532_TRANSPORT_I2C)
    ESP_RETURN_ON_ERROR(i2c_read_frame(chunk, read_total), TAG, "payload i2c");
#elif defined(CONFIG_PN532_TRANSPORT_HSU)
    ESP_RETURN_ON_ERROR(hsu_read_raw(chunk, read_total, timeout_ms), TAG, "payload hsu");
#endif

    if (chunk[0] != PN532_PN532_TO_HOST) {
        return ESP_ERR_INVALID_RESPONSE;
    }
    uint8_t sum = 0;
    for (size_t i = 0; i < frame_len; i++) {
        sum += chunk[i];
    }
    if ((uint8_t)(sum + chunk[frame_len]) != 0) {
        return ESP_ERR_INVALID_CRC;
    }
    if (chunk[frame_len + 1] != 0x00) {
        return ESP_ERR_INVALID_RESPONSE;
    }

    *body_len = frame_len - 1;
    if (*body_len > body_max) {
        return ESP_ERR_INVALID_SIZE;
    }
    memcpy(body_out, chunk + 1, *body_len);
    return ESP_OK;
}

esp_err_t pn532_transport_exchange(const uint8_t *tx_body, size_t tx_body_len, uint8_t *rx_body,
                                   size_t rx_body_max, size_t *rx_body_len, int timeout_ms)
{
    if (!tx_body || !rx_body || !rx_body_len) {
        return ESP_ERR_INVALID_ARG;
    }
    if (tx_body_len == 0 || tx_body_len > 254) {
        return ESP_ERR_INVALID_ARG;
    }
    size_t frame_len = 1 + tx_body_len;
    uint8_t sum = PN532_HOST_TO_PN532;
    for (size_t i = 0; i < tx_body_len; i++) {
        sum += tx_body[i];
    }
    uint8_t dcs = (uint8_t)(0x100 - sum);

    uint8_t frame[PN532_TXBUF_MAX];
    size_t pos = 0;
    frame[pos++] = 0x00;
    frame[pos++] = 0x00;
    frame[pos++] = 0xFF;
    if (frame_len <= 254) {
        uint8_t lcs = (uint8_t)(0x100 - (uint8_t)frame_len);
        frame[pos++] = (uint8_t)frame_len;
        frame[pos++] = lcs;
    } else {
        uint16_t ext_len = (uint16_t)frame_len;
        frame[pos++] = 0xFF;
        frame[pos++] = 0xFF;
        frame[pos++] = (uint8_t)((ext_len >> 8) & 0xFF);
        frame[pos++] = (uint8_t)(ext_len & 0xFF);
        frame[pos++] = (uint8_t)(0x100 - (uint8_t)(((ext_len >> 8) + (ext_len & 0xFF)) & 0xFF));
    }
    frame[pos++] = PN532_HOST_TO_PN532;
    memcpy(frame + pos, tx_body, tx_body_len);
    pos += tx_body_len;
    frame[pos++] = dcs;
    frame[pos++] = 0x00;

#if defined(CONFIG_PN532_TRANSPORT_SPI)
    ESP_RETURN_ON_ERROR(spi_write_frame(frame, pos), TAG, "spi wr");
#elif defined(CONFIG_PN532_TRANSPORT_I2C)
    i2c_wakeup();
    ESP_RETURN_ON_ERROR(i2c_write_raw(frame, pos), TAG, "i2c wr");
#elif defined(CONFIG_PN532_TRANSPORT_HSU)
    ESP_RETURN_ON_ERROR(hsu_write_raw(frame, pos), TAG, "hsu wr");
#endif

    ESP_RETURN_ON_ERROR(read_ack(timeout_ms), TAG, "ack");
    return read_response_frame(rx_body, rx_body_max, rx_body_len, timeout_ms);
}

esp_err_t pn532_transport_init(void)
{
    s_bus_mutex = xSemaphoreCreateMutex();
    if (!s_bus_mutex) {
        return ESP_ERR_NO_MEM;
    }
#if defined(CONFIG_PN532_TRANSPORT_SPI)
    spi_bus_config_t buscfg = {
        .mosi_io_num = CONFIG_PN532_SPI_MOSI_GPIO,
        .miso_io_num = CONFIG_PN532_SPI_MISO_GPIO,
        .sclk_io_num = CONFIG_PN532_SPI_SCLK_GPIO,
        .quadwp_io_num = -1,
        .quadhd_io_num = -1,
        .max_transfer_sz = 512,
    };
    spi_host_device_t host = pn532_spi_host();
    ESP_RETURN_ON_ERROR(spi_bus_initialize(host, &buscfg, SPI_DMA_CH_AUTO), TAG, "spi bus");
    spi_device_interface_config_t devcfg = {
        .clock_speed_hz = CONFIG_PN532_SPI_CLOCK_HZ,
        .mode = 0,
        .spics_io_num = -1, /* manual CS */
        .queue_size = 4,
    };
    ESP_RETURN_ON_ERROR(spi_bus_add_device(host, &devcfg, &s_spi), TAG, "spi dev");
    gpio_reset_pin((gpio_num_t)s_spi_cs_gpio);
    gpio_set_direction((gpio_num_t)s_spi_cs_gpio, GPIO_MODE_OUTPUT);
    gpio_set_level(s_spi_cs_gpio, 1);
#elif defined(CONFIG_PN532_TRANSPORT_I2C)
    i2c_config_t ic = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = CONFIG_PN532_I2C_SDA_GPIO,
        .scl_io_num = CONFIG_PN532_I2C_SCL_GPIO,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master = {.clk_speed = 400000},
    };
    ESP_RETURN_ON_ERROR(i2c_param_config(PN532_I2C_PORT, &ic), TAG, "i2c cfg");
    ESP_RETURN_ON_ERROR(i2c_driver_install(PN532_I2C_PORT, I2C_MODE_MASTER, 0, 0, 0), TAG, "i2c drvr");
#elif defined(CONFIG_PN532_TRANSPORT_HSU)
    uart_config_t uc = {
        .baud_rate = CONFIG_PN532_HSU_BAUD,
        .data_bits = UART_DATA_8_BITS,
        .parity = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
        .source_clk = UART_SCLK_DEFAULT,
    };
    ESP_RETURN_ON_ERROR(uart_driver_install(PN532_UART, 2048, 2048, 0, NULL, 0), TAG, "uart");
    ESP_RETURN_ON_ERROR(uart_param_config(PN532_UART, &uc), TAG, "uart cfg");
    ESP_RETURN_ON_ERROR(uart_set_pin(PN532_UART, CONFIG_PN532_HSU_TX_GPIO, CONFIG_PN532_HSU_RX_GPIO,
                                     UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE),
                        TAG, "uart pins");
#endif
    return ESP_OK;
}