
#include <driver/gpio.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>

#include "deca_device_api.h"
#include "dw3000_hw.h"
#include "dw3000_spi.h"
#include "log.h"

static const char* LOG_TAG = "DW3000";
static bool dw3000_interrupt_enabled;


int dw3000_hw_init(DW3000_PIN_CONFIG* dw3000_pin_config)
{
	pin_config = dw3000_pin_config;

	LOG_INF("HW Init (RESET:%d WAKEUP:%d IRQ:%d)", pin_config->dw3000_pin_reset,
		pin_config->dw3000_pin_wakeup, pin_config->dw3000_pin_irq);

	/*
	 * RESET: output low, open drain, no pull-up
	 * normally used as input to see when DW3000 is ready
	 */
	if (pin_config->dw3000_pin_reset != 0) {
		gpio_config_t io_conf_reset = {
			.mode = GPIO_MODE_INPUT,
			.pin_bit_mask = (uint64_t)1 << pin_config->dw3000_pin_reset,
		};
		gpio_config(&io_conf_reset);

		/* check reset state */
		int timeout = 1000;
		while (!gpio_get_level(pin_config->dw3000_pin_reset) && --timeout > 0) {
			vTaskDelay(1);
		}
		if (timeout <= 0) {
			LOG_ERR("did not come out of reset");
			return ESP_ERR_TIMEOUT;
		}
	}

	/*
	 * WAKEUP: output high
	 */
	if (pin_config->dw3000_pin_wakeup != 0) {
		gpio_config_t io_conf_wakeup = {
			.mode = GPIO_MODE_OUTPUT,
			.pin_bit_mask = (uint64_t)1 << pin_config->dw3000_pin_wakeup,
		};
		gpio_config(&io_conf_wakeup);
		gpio_set_level(pin_config->dw3000_pin_wakeup, 0);
	}

	return dw3000_spi_init();
}

static void dw3000_isr(void* args)
{
	while (gpio_get_level(pin_config->dw3000_pin_irq)) {
		dwt_isr();
	}
}

int dw3000_hw_init_interrupt(void)
{
	if (pin_config->dw3000_pin_irq == 0) {
		LOG_ERR("IRQ pin is not defined");
		return ESP_ERR_NOT_SUPPORTED;
	}

	gpio_config_t io_conf = {
		.intr_type = GPIO_INTR_POSEDGE,
		.pin_bit_mask = (uint64_t)1 << pin_config->dw3000_pin_irq,
		.mode = GPIO_MODE_INPUT,
		.pull_down_en = true,
	};

	gpio_config(&io_conf);
	gpio_install_isr_service(0);
	return gpio_isr_handler_add(pin_config->dw3000_pin_irq, dw3000_isr, NULL);
}

void dw3000_hw_interrupt_enable(void)
{
	if (pin_config->dw3000_pin_irq != 0) {
		gpio_intr_enable(pin_config->dw3000_pin_irq);
		dw3000_interrupt_enabled = true;
	}
}

void dw3000_hw_interrupt_disable(void)
{
	if (pin_config->dw3000_pin_irq == 0) {
		LOG_ERR("IRQ pin is not defined");
		return;
	}

	gpio_intr_disable(pin_config->dw3000_pin_irq);
	dw3000_interrupt_enabled = false;
}

bool dw3000_hw_interrupt_is_enabled(void)
{
	return dw3000_interrupt_enabled;
}

void dw3000_hw_fini(void)
{
	LOG_INF("HW fini");

	if (pin_config->dw3000_pin_irq != 0) {
		if (dw3000_interrupt_enabled) {
			gpio_intr_disable(pin_config->dw3000_pin_irq);
		}
		gpio_isr_handler_remove(pin_config->dw3000_pin_irq);
	}

	dw3000_spi_fini();
}

void dw3000_hw_reset(void)
{
	if (pin_config->dw3000_pin_reset == 0) {
		LOG_ERR("Reset pin is not defined");
		return;
	}

	LOG_INF("HW reset");
	gpio_set_direction(pin_config->dw3000_pin_reset, GPIO_MODE_OUTPUT);
	gpio_set_level(pin_config->dw3000_pin_reset, 0);
	vTaskDelay(1); // 10 us?
	gpio_set_level(pin_config->dw3000_pin_reset, 1);
	vTaskDelay(2); // From sample platform code
	gpio_set_direction(pin_config->dw3000_pin_reset, GPIO_MODE_INPUT);
}

/** wakeup either using the WAKEUP pin or SPI CS */
void dw3000_hw_wakeup(void)
{
	if (pin_config->dw3000_pin_wakeup != 0) {
		/* Use WAKEUP pin if available */
		LOG_INF("dw3000_hw_wakeup(): WAKEUP PIN");
		gpio_set_level(pin_config->dw3000_pin_wakeup, 1);
		vTaskDelay(1); // 500 usec
		gpio_set_level(pin_config->dw3000_pin_wakeup, 0);
	}
	else {
		/* Use SPI CS pin */
		LOG_INF("dw3000_hw_wakeup():WAKEUP CS");
		// TODO: set from SPI to GPIO output
		gpio_set_level(pin_config->dw3000_pin_cs, 0);
		vTaskDelay(1); // 500 usec
		gpio_set_level(pin_config->dw3000_pin_cs, 1);
	}
	vTaskDelay(1);
}

/** set WAKEUP pin low if available */
void dw3000_hw_wakeup_pin_low(void)
{
	if (pin_config->dw3000_pin_wakeup != 0) {
		gpio_set_level(pin_config->dw3000_pin_wakeup, 0);
	}
	else {
		gpio_set_level(pin_config->dw3000_pin_cs, 0);
	}
}
