#include "common.h"
#include "config.h"
#include "fpga.h"

#include <driver/gpio.h>
#include <driver/spi_common.h>
#include <driver/spi_master.h>

#define PIN_FPGA_INT	 9
#define PIN_FPGA_CS	10
#define PIN_FPGA_IO0	11
#define PIN_FPGA_CLK	12
#define PIN_FPGA_IO1	13

#define FPGA_SPI_HOST	FSPI	/* SPI2 */

#define FPGA_PRIORITY	3

static spi_bus_config_t spi_bus_config = {
    .data0_io_num    = PIN_FPGA_IO0,
    .data1_io_num    = PIN_FPGA_IO1,
    .sclk_io_num     = PIN_FPGA_CLK,
    .data2_io_num    = -1,
    .data3_io_num    = -1,
    .data4_io_num    = -1,
    .data5_io_num    = -1,
    .data6_io_num    = -1,
    .data7_io_num    = -1,
    .max_transfer_sz = 4096,
    .flags           = SPICOMMON_BUSFLAG_MASTER | SPICOMMON_BUSFLAG_DUAL
};

static const spi_device_interface_config_t spi_device_interface_config = {
    .command_bits     =  8,
    .address_bits     = 32,
    .dummy_bits       =  0,
    .mode             =  0,
    .cs_ena_pretrans  =  0,
    .cs_ena_posttrans =  0,
    .clock_speed_hz   = SPI_MASTER_FREQ_40M,
    .spics_io_num     = PIN_FPGA_CS,
    .flags            = SPI_DEVICE_HALFDUPLEX,
    .queue_size       =  4	/* Maybe? */
};

static spi_device_handle_t spi_handle;
static TaskHandle_t fpga_task;
static SemaphoreHandle_t spi_mutex;

#define NOTIFY_INDEX 0

static void ARDUINO_ISR_ATTR fpga_interrupt(void)
{
    BaseType_t do_wakeup = pdFALSE;

    if (!fpga_task)
	return;

    xTaskNotifyIndexedFromISR(fpga_task, NOTIFY_INDEX, 1, eSetBits, &do_wakeup);
    if (do_wakeup)
	portYIELD_FROM_ISR(do_wakeup);
}

static void fpga_service_task(void *);

int fpga_service_start(void)
{
    esp_err_t err;

    pinMode(PIN_FPGA_INT, INPUT_PULLUP);

    if (!spi_mutex) {
	spi_mutex = xSemaphoreCreateMutex();
	if (!spi_mutex)
	    goto failed;
    }

    xSemaphoreTake(spi_mutex, portMAX_DELAY);

    err = spi_bus_initialize(FPGA_SPI_HOST, &spi_bus_config, SPI_DMA_CH_AUTO);
    if (err)
	goto failed;

    err = spi_bus_add_device(FPGA_SPI_HOST, &spi_device_interface_config,
			     &spi_handle);
    if (err)
	goto failed;

    /* Only device on this bus, so acquire it permanently */
    spi_device_acquire_bus(spi_handle, portMAX_DELAY);

    xSemaphoreGive(spi_mutex);

    if (!fpga_task) {
	if (xTaskCreate(fpga_service_task, "fpga_svc", 4096, NULL,
			FPGA_PRIORITY, &fpga_task) != pdPASS)
	    goto failed;

	attachInterrupt(PIN_FPGA_INT, fpga_interrupt, FALLING);
	esp_register_shutdown_handler(fpga_service_stop);
    }

    /* All good! */
    printf("[FPGA] FPGA services started\n");
    return 0;

failed:
    printf("[FPGA] Failed to initialize FPGA SPI bus\n");
    if (spi_mutex)
	xSemaphoreGive(spi_mutex);

    fpga_service_stop();
    return -1;
}

void fpga_service_stop(void)
{
    if (spi_mutex) {
	xSemaphoreTake(spi_mutex, portMAX_DELAY);

	if (fpga_task) {
	    esp_unregister_shutdown_handler(fpga_service_stop);
	    detachInterrupt(PIN_FPGA_INT);
	    vTaskDelete(fpga_task);
	    fpga_task = NULL;
	}

	if (spi_handle) {
	    spi_device_release_bus(spi_handle);
	    spi_bus_remove_device(spi_handle);
	    spi_bus_free(FPGA_SPI_HOST);
	    spi_handle = NULL;
	}

	xSemaphoreGive(spi_mutex);
    }
    printf("[FPGA] FPGA services stopped\n");
}

#define FPGA_CMD_IRQ1	(1 << 0)
#define FPGA_CMD_IRQ2	(2 << 0)
#define FPGA_CMD_IRQ3	(3 << 0)
#define FPGA_CMD_ACK1	(1 << 2)
#define FPGA_CMD_ACK2	(2 << 2)
#define FPGA_CMD_ACK3	(3 << 2)
#define FPGA_CMD_WR	(0 << 4)
#define FPGA_CMD_RD	(1 << 4)

#define FPGA_HDR_ADDR	0x40000000

static esp_err_t fpga_io(uint8_t cmd, uint32_t addr, void *data, size_t len)
{
    spi_transaction_ext_t trans;
    esp_err_t err;

    memset(&trans, 0, sizeof trans);
    trans.base.flags   =
	SPI_TRANS_MODE_DIO |
	SPI_TRANS_VARIABLE_DUMMY |
	SPI_TRANS_MULTILINE_CMD |
	SPI_TRANS_MULTILINE_ADDR;
    trans.command_bits =  8;
    trans.address_bits = 32;

    trans.base.cmd    = cmd;
    trans.base.addr   = addr;
    if (cmd & FPGA_CMD_RD) {
	trans.dummy_bits     = 16; /* 16 cycles = 32 bits */
	trans.base.rx_buffer = data;
	trans.base.rxlength  = len << 3;
    } else {
	trans.base.tx_buffer = data;
	trans.base.length    = len << 3;
    }

    xSemaphoreTake(spi_mutex, portMAX_DELAY);
    err = spi_device_transmit(spi_handle, (spi_transaction_t *)&trans);
    xSemaphoreGive(spi_mutex);

    return err;
}

static void fpga_service_task(void *dummy)
{
    (void)dummy;
    uint32_t head[8];

    while (1) {
	vTaskDelay(10 * configTICK_RATE_HZ);

	fpga_io(FPGA_CMD_RD|FPGA_CMD_ACK1, FPGA_HDR_ADDR,
		head, sizeof head);

	for (unsigned int i = 0; i < ARRAY_SIZE(head); i++) {
	    printf("[FPGA] head[%u] = 0x%08x (%u)\n",
		   i, head[i], head[i]);
	}
    }

    while (1) {
	while (!digitalRead(PIN_FPGA_INT)) {
	    printf("[FPGA] FPGA signals ready\n");

	}

	/* Wait until an interrupt is received */
	xTaskNotifyWaitIndexed(NOTIFY_INDEX, 0, 1, NULL, portMAX_DELAY);
    }
}