// #define ARM_MATH_CM4
/*
 * Copyright (c) 2018 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
 */

/** @file
 *  @brief Nordic UART Bridge Service (NUS) sample
 */
// #include "uart_async_adapter.h"

#include <zephyr/types.h>
#include <zephyr.h>
// #include <drivers/uart.h>
#include <usb/usb_device.h>
// #include <drivers/gpio.h>

#include <random/rand32.h>

#include <device.h>
#include <soc.h>

#include "nrf.h"
#include <nrfx_spim.h>
#include <nrfx_gpiote.h>

#include <bluetooth/bluetooth.h>
#include <bluetooth/uuid.h>
#include <bluetooth/gatt.h>
#include <bluetooth/hci.h>
#include <bluetooth/services/nus.h>

#include <dk_buttons_and_leds.h>
#include <settings/settings.h>

#include <stdio.h>
#include <stdlib.h>
#include <sys/printk.h>

// #include <logging/log.h>

/*--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/

#define NVMC_READY_R  (*((volatile unsigned long*) 0x50039400))
#define NVMC_ICACHECNF_R  (*((volatile unsigned long*) 0x50039540))
#define HFCLKCTL_R                (*((volatile unsigned long *)0x50005558))

#define STACKSIZE CONFIG_BT_NUS_THREAD_STACK_SIZE
#define THREAD1_PRIORITY 7

static K_SEM_DEFINE(ble_init_ok, 0, 1);
static K_SEM_DEFINE(ble_buffer_ok, 1, 1);

static K_FIFO_DEFINE(ble_data);


/*--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/

// Pins for electrical recording and stimulation
#define pin_stim_enable                 NRF_GPIO_PIN_MAP(0, 17)
#define pin_mux_rec_sel0                NRF_GPIO_PIN_MAP(0, 21)
#define pin_mux_rec_sel1                NRF_GPIO_PIN_MAP(0, 16)
#define pin_mux_rec_sel2                NRF_GPIO_PIN_MAP(0, 14)
// #define pin_mux_stim_sel0               NRF_GPIO_PIN_MAP(0, 6)
#define pin_mux_stim_sel0               NRF_GPIO_PIN_MAP(0, 2) // to avoid uart debug
#define pin_mux_stim_sel1               NRF_GPIO_PIN_MAP(0, 24)
#define pin_mux_stim_sel2               NRF_GPIO_PIN_MAP(0, 22)
#define pin_mux_rec_enable              NRF_GPIO_PIN_MAP(0, 20)
// #define pin_mux_stim_enable             NRF_GPIO_PIN_MAP(0, 8) 
#define pin_mux_stim_enable             NRF_GPIO_PIN_MAP(0, 3) // to avoid uart debug
#define pin_vstart						NRF_GPIO_PIN_MAP(0, 26)

// Pins for SPI with variable resistor to control electrical stimulation waveform
#define pin_spim_stim_CS				NRF_GPIO_PIN_MAP(1, 12)
#define pin_spim_stim_MISO				NRF_GPIO_PIN_MAP(1, 3)
#define pin_spim_stim_MOSI				NRF_GPIO_PIN_MAP(1, 7)
#define pin_spim_stim_CLK				NRF_GPIO_PIN_MAP(1, 1)

// Pins for SPI with FPGA which decimates ADC's output
#define pin_spim_adc_CS					NRF_GPIO_PIN_MAP(1, 2)
#define pin_spim_adc_MISO				NRF_GPIO_PIN_MAP(1, 4)
#define pin_spim_adc_MOSI				NRF_GPIO_PIN_MAP(1, 5)
#define pin_spim_adc_CLK				NRF_GPIO_PIN_MAP(1, 6)
#define pin_spim_fpga_ind				NRF_GPIO_PIN_MAP(0, 19)
#define pin_fpga_osc_enable				NRF_GPIO_PIN_MAP(0, 27)


/*--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/

// SPI_STIM variables
nrfx_spim_t spim_stim = NRFX_SPIM_INSTANCE(1);
#define spim_stim_m_length  3
static uint8_t spim_stim_tx_buf[spim_stim_m_length];
static uint8_t spim_stim_rx_buf[spim_stim_m_length+1];
// nrfx_spim_xfer_desc_t spim_stim_desc = NRFX_SPIM_XFER_TX(spim_stim_tx_buf, spim_stim_m_length);
nrfx_spim_xfer_desc_t spim_stim_desc = NRFX_SPIM_XFER_TRX(spim_stim_tx_buf, spim_stim_m_length, spim_stim_rx_buf, spim_stim_m_length);
static volatile bool spim_stim_xfer_done;

// SPI_FPGA variables
nrfx_spim_t spim_adc = NRFX_SPIM_INSTANCE(2);
#define spim_adc_m_length  240
static uint8_t spim_adc_tx_buf[spim_adc_m_length];
static uint8_t spim_adc_rx_buf[spim_adc_m_length+1];
// nrfx_spim_xfer_desc_t spim_adc_desc = NRFX_SPIM_XFER_RX(spim_adc_rx_buf, spim_adc_m_length);
nrfx_spim_xfer_desc_t spim_adc_desc = NRFX_SPIM_XFER_TRX(spim_adc_tx_buf, spim_adc_m_length, spim_adc_rx_buf, spim_adc_m_length);
static volatile bool spim_adc_xfer_done;

/*--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/

#define DEVICE_NAME CONFIG_BT_DEVICE_NAME
#define DEVICE_NAME_LEN (sizeof(DEVICE_NAME) - 1)

// Deprecated LEDs which indicate BLE state
// #define RUN_STATUS_LED DK_LED1
// #define RUN_LED_BLINK_INTERVAL 1000
// #define CON_STATUS_LED DK_LED2

/* The default MTU size before negotiating a longer MTU */
// #define BLE_GATT_ATT_MTU_DEFAULT 23
#define BLE_GATT_ATT_MTU_DEFAULT 247

static const struct bt_data ad[] = {
	BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
	BT_DATA(BT_DATA_NAME_COMPLETE, DEVICE_NAME, DEVICE_NAME_LEN),
};

static const struct bt_data sd[] = {
	BT_DATA_BYTES(BT_DATA_UUID128_ALL, BT_UUID_NUS_VAL),
};

static struct bt_conn *current_conn;

static struct bt_gatt_exchange_params exchange_params;
static uint16_t m_nus_max_send_len = BLE_GATT_ATT_MTU_DEFAULT - 3;

bool is_rcv = false;
char rcv_cmd[3];
// bool is_ble_snd = false;
bool is_ble_snd = true;

/*--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/
//	Function prototypes
/*--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/

// Not required yet


/*--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/
//	Bluetooth functions
/*--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/

static void exchange_func(struct bt_conn *conn, uint8_t err, struct bt_gatt_exchange_params *params)
{
	if (!err) {
		m_nus_max_send_len = bt_nus_get_mtu(conn);
		// LOG_INF("ATT MTU size updated to %d bytes", m_nus_max_send_len + 3);
		err = bt_conn_le_data_len_update(current_conn, BT_LE_DATA_LEN_PARAM_MAX);
		if (err) {
			// LOG_ERR("Failed to update data length (error %u)", err);
		}
	}
}

static void connected(struct bt_conn *conn, uint8_t err)
{
	char addr[BT_ADDR_LE_STR_LEN];

	if (err) {
		// LOG_ERR("Connection failed (err %u)", err);
		return;
	}

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
	// LOG_INF("Connected %s", log_strdup(addr));

	current_conn = bt_conn_ref(conn);

	exchange_params.func = exchange_func;

	err = bt_gatt_exchange_mtu(current_conn, &exchange_params);
	if (err) {
		// LOG_WRN("bt_gatt_exchange_mtu: %d", err);
	}

	// dk_set_led_on(CON_STATUS_LED);
}

static void disconnected(struct bt_conn *conn, uint8_t reason)
{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	// LOG_INF("Disconnected: %s (reason %u)", log_strdup(addr), reason);

	if (current_conn) {
		bt_conn_unref(current_conn);
		current_conn = NULL;
		// dk_set_led_off(CON_STATUS_LED);
	}

	is_rcv = true;
	rcv_cmd[0] = 'S';
	rcv_cmd[0] = 'S';
	rcv_cmd[0] = '0';
	rcv_cmd[0] = '0';
}

static void data_len_updated(struct bt_conn *conn, struct bt_conn_le_data_len_info *info)
{
	// LOG_INF("Data length updated. RX length: %d, TX length: %d", info->rx_max_len,
		// info->tx_max_len);
}

static void phy_updated(struct bt_conn *conn, struct bt_conn_le_phy_info *param)
{
	// LOG_INF("PHY updated. RX PHY: %d, TX PHY: %d", param->rx_phy, param->tx_phy);
}

static struct bt_conn_cb conn_callbacks = {
	.connected = connected,
	.disconnected = disconnected,
	.le_data_len_updated = data_len_updated,
	.le_phy_updated = phy_updated,
};

static void bt_receive_cb(struct bt_conn *conn, const uint8_t *const data, uint16_t len)
{
	// char addr[BT_ADDR_LE_STR_LEN] = { 0 };

	// bt_addr_le_to_str(bt_conn_get_dst(conn), addr, ARRAY_SIZE(addr));

	// LOG_INF("Received data from: %s", log_strdup(addr));

	is_rcv = true;
	rcv_cmd[0] = data[0];
	rcv_cmd[1] = data[1];
	rcv_cmd[2] = data[2];
	rcv_cmd[3] = data[3];
}

static struct bt_nus_cb nus_cb = {
	.received = bt_receive_cb,
};

void error(void)
{
	// dk_set_leds_state(DK_ALL_LEDS_MSK, DK_NO_LEDS_MSK);

	// while (true) {
	// 	/* Spin for ever */
	// 	k_sleep(K_MSEC(1000));
	// }
}

static void mtu_updated(struct bt_conn *conn, uint16_t tx, uint16_t rx)
{
	// LOG_INF("ATT MTU updated. TX: %d, RX: %d", tx, rx);
	/* Actual payload is MTU size minus the 3 byte ATT header */
	m_nus_max_send_len = tx - 3;
}

static struct bt_gatt_cb gatt_callbacks = {
	.att_mtu_updated = mtu_updated,
};

void ble_init(void)
{
	int err = 0;

	bt_conn_cb_register(&conn_callbacks);

	err = bt_enable(NULL);
	if (err) {
		error();
	}
	
	// LOG_INF("Bluetooth initialized");
}

void nus_init(void)
{
	int err = 0;

	err = bt_nus_init(&nus_cb);
	if (err) {
		// LOG_ERR("Failed to initialize UART service (err: %d)", err);
		return;
	}
}

void advertising_init(void)
{
	int err = 0;

	err = bt_le_adv_start(BT_LE_ADV_CONN, ad, ARRAY_SIZE(ad), sd, ARRAY_SIZE(sd));
	if (err) {
		// LOG_ERR("Advertising failed to start (err %d)", err);
		return;
	}
}


/*--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/
//	GPIO & GPIOTE functions
/*--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/

void gpio_pin_init(void)
{
	// V start high
    nrf_gpio_cfg_output(pin_vstart);
    nrf_gpio_pin_write(pin_vstart, 1);
    
    // Stimulation enable
    nrf_gpio_cfg_output(pin_stim_enable);
    nrf_gpio_pin_write(pin_stim_enable, 1); // Init to high = disable

    // MUX enable pins
    nrf_gpio_cfg_output(pin_mux_stim_enable);
    nrf_gpio_cfg_output(pin_mux_rec_enable);
    nrf_gpio_pin_write(pin_mux_stim_enable, 0); // Low = Disable stimulation MUX
    nrf_gpio_pin_write(pin_mux_rec_enable, 1); // High = Enable recording MUX

    // MUX source pins
    nrf_gpio_cfg_output(pin_mux_rec_sel0);
    nrf_gpio_cfg_output(pin_mux_rec_sel1);
    nrf_gpio_cfg_output(pin_mux_rec_sel2);
    nrf_gpio_pin_write(pin_mux_rec_sel0, 0); // All low = connect to T1 rec, T2 stim.
    nrf_gpio_pin_write(pin_mux_rec_sel1, 0);
    nrf_gpio_pin_write(pin_mux_rec_sel2, 0);
    nrf_gpio_cfg_output(pin_mux_stim_sel0);
    nrf_gpio_cfg_output(pin_mux_stim_sel1);
    nrf_gpio_cfg_output(pin_mux_stim_sel2);
    nrf_gpio_pin_write(pin_mux_stim_sel0, 0); // All low = connect to T1 rec, T2 stim.
    nrf_gpio_pin_write(pin_mux_stim_sel1, 0);
    nrf_gpio_pin_write(pin_mux_stim_sel2, 0);

    // V start low
    nrf_gpio_pin_write(pin_vstart, 0);

    // FPGA oscillator start up
    nrf_gpio_cfg_output(pin_fpga_osc_enable);
	nrf_gpio_pin_write(pin_fpga_osc_enable, 0);
	// k_usleep(200);
    // nrf_gpio_pin_write(pin_fpga_osc_enable, 1);
}

void gpiote_handler(nrfx_gpiote_pin_t pin, nrf_gpiote_polarity_t action)
{
	nrfx_err_t err_code = nrfx_spim_xfer(&spim_adc, &spim_adc_desc, 0);
	if (err_code != NRFX_SUCCESS)
	{
		printk("SPI error: %x\n", err_code);
	}
}

void gpiote_init(void)
{
	nrfx_err_t err_code;
	
	// GPIOTE init
    err_code = nrfx_gpiote_init(NRFX_GPIOTE_DEFAULT_CONFIG_IRQ_PRIORITY);
	
	nrfx_gpiote_in_config_t gpiote_cfg = NRFX_GPIOTE_CONFIG_IN_SENSE_LOTOHI(false);

	err_code = nrfx_gpiote_in_init(pin_spim_fpga_ind, &gpiote_cfg, gpiote_handler);
	if (NRFX_SUCCESS != err_code)
	{
		printk("GPIOTE init error\n");
	}
	nrfx_gpiote_in_event_enable(pin_spim_fpga_ind, true);
}


/*--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/
//	SPI functions
/*--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/

void spim_stim_event_handler(nrfx_spim_evt_t const *p_event, void *p_context)
{
	if (p_event->type == NRFX_SPIM_EVENT_DONE)
	{
		spim_stim_xfer_done = true;
	}
}

void spim_adc_event_handler(nrfx_spim_evt_t const *p_event, void *p_context)
{
	if (p_event->type == NRFX_SPIM_EVENT_DONE)
	{
		spim_adc_xfer_done = true;
	}
}

void spi_init(void)
{
	// spim_stim init
	nrfx_spim_config_t spim_stim_config = NRFX_SPIM_DEFAULT_CONFIG(pin_spim_stim_CLK, pin_spim_stim_MOSI, pin_spim_stim_MISO, pin_spim_stim_CS);
	spim_stim_config.frequency = NRF_SPIM_FREQ_8M;
	spim_stim_config.mode = NRF_SPIM_MODE_3;
	// nrfx_spim_init(&spim_stim, &spim_stim_config, spim_stim_event_handler, NULL);
	if (NRFX_SUCCESS != nrfx_spim_init(&spim_stim, &spim_stim_config, spim_stim_event_handler, NULL))
	{
		printk("SPIM_STIM init error\n");
	}
	IRQ_DIRECT_CONNECT(SPIM1_SPIS1_TWIM1_TWIS1_UARTE1_IRQn, IRQ_PRIO_LOWEST, nrfx_spim_1_irq_handler, 0);
	irq_enable(SPIM1_SPIS1_TWIM1_TWIS1_UARTE1_IRQn);

	// spim_adc init
	nrfx_spim_config_t spim_adc_config = NRFX_SPIM_DEFAULT_CONFIG(pin_spim_adc_CLK, pin_spim_adc_MOSI, pin_spim_adc_MISO, pin_spim_adc_CS);
	spim_adc_config.frequency = NRF_SPIM_FREQ_8M;
	spim_adc_config.mode = NRF_SPIM_MODE_3;
	// nrfx_spim_init(&spim_adc, &spim_adc_config, spim_adc_event_handler, NULL);
	if (NRFX_SUCCESS != nrfx_spim_init(&spim_adc, &spim_adc_config, spim_adc_event_handler, NULL))
	{
		printk("SPIM_ADC init error\n");
	}
	IRQ_DIRECT_CONNECT(SPIM2_SPIS2_TWIM2_TWIS2_UARTE2_IRQn, IRQ_PRIO_LOWEST, nrfx_spim_2_irq_handler, 0);
	irq_enable(SPIM2_SPIS2_TWIM2_TWIS2_UARTE2_IRQn);
}


/*--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/
//	Main function
/*--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/

void main(void)
{
	int ble_err_code = 0;
	int deb_i = 0;

	// Set CPU clock to 64 MHz
	HFCLKCTL_R &= ~(0x01);

	// Init
	printk("Start inits\n");
	ble_init();
	nus_init();
	gpio_pin_init();
	spi_init();
	gpiote_init();

	// Load configuration
	if (IS_ENABLED(CONFIG_SETTINGS)) {
		settings_load();
	}

	// Start BLE advertising
	advertising_init();
	
	// Enable cache and hit/miss tracking
    while((NVMC_READY_R&0x1) == 0);
    NVMC_ICACHECNF_R = 0x101;

	printk("Main loop begin\n");
	for(;;)
	{
		// Main loop responses to the BLE RX event after the callback
		if (is_rcv)
		{
			int num_channel;
			switch (rcv_cmd[0])
			{
			case 'r':
				switch(rcv_cmd[1])
                    {
                        case 'm':
                            // 'rm0' = Recording MUX off, 'rm1' = Recording MUX on
                            nrf_gpio_pin_write(pin_mux_rec_enable, atoi(&rcv_cmd[2]));
                            break;
                        case 'c':
                            // 'rc(0~8)' = Recording MUX to specific channel
                            num_channel = atoi(&rcv_cmd[2]);
                            nrf_gpio_pin_write(pin_mux_rec_sel0, (num_channel) & 0x01);
                            nrf_gpio_pin_write(pin_mux_rec_sel1, (num_channel) & 0x02);
                            nrf_gpio_pin_write(pin_mux_rec_sel2, (num_channel) & 0x04);
                            break;
                        default:
                            break;
                    }
				break;
			case 's':
				switch(rcv_cmd[1])
				{
					case 'm':
						// 'sm0' = Stimulation MUX off, 'sm1' = Stimulation MUX on
						nrf_gpio_pin_write(pin_mux_stim_enable, atoi(&rcv_cmd[2]));
						break;
					case 'c':
						// 'sc(0~8)' = Stimulation MUX to specific channel
						num_channel = atoi(&rcv_cmd[2]);
						nrf_gpio_pin_write(pin_mux_stim_sel0, (num_channel) & 0x01);
						nrf_gpio_pin_write(pin_mux_stim_sel1, (num_channel) & 0x02);
						nrf_gpio_pin_write(pin_mux_stim_sel2, (num_channel) & 0x04);
						break;
					case 'o':
						// 'so0' = stimulation on, 'so1' = stimulation off
						nrf_gpio_pin_write(pin_stim_enable, atoi(&rcv_cmd[2]));
						break;
					case 'f':
						// 'sf(char)' = Set stimulation frequency
						spim_stim_tx_buf[0] = 0b10110000;
						spim_stim_tx_buf[1] = rcv_cmd[2];
						spim_stim_tx_buf[2] = rcv_cmd[3];
						spim_stim_xfer_done = false;
						nrfx_spim_xfer(&spim_stim, &spim_stim_desc, 0);
						break;
					case 'a':
						// 'sa(char)' = Set stimulation amplitude
						spim_stim_tx_buf[0] = 0b10110001;
						spim_stim_tx_buf[1] = rcv_cmd[2];
						spim_stim_tx_buf[2] = rcv_cmd[3];
						spim_stim_xfer_done = false;
						nrfx_spim_xfer(&spim_stim, &spim_stim_desc, 0);
						break;
					default:
						break;
				}
				break;
			case 'c':

				break;
			case 'S':
				switch(rcv_cmd[1])
				{
					case 'v':
						// 'Sv1' = Vstart high, 'Sv0' = Vstart low
						nrf_gpio_pin_write(pin_vstart, atoi(&rcv_cmd[2]));
						break;
					case 'S':
						switch(rcv_cmd[2])
						{
							case '0':
								// 'SS0' = sleep
								is_ble_snd = false;
								break;
							case '1':
								// 'SS1' = wake
								is_ble_snd = true;
								break;
							default:
								break;
						}
						break;
					case 'O':
						nrf_gpio_pin_write(pin_fpga_osc_enable, atoi(&rcv_cmd[2]));
						break;
					default:
						break;
				}
				break;
			default:
				break;
			}

			// Reset is_rcv to wait for next BLE CMD
			is_rcv = false;
		}
		if (spim_adc_xfer_done)
		{
			if (is_ble_snd)
			{
				uint16_t length = spim_adc_m_length;
				ble_err_code = bt_nus_send(NULL, spim_adc_rx_buf, length);
				// printk("data sent\n");
				// for(deb_i = 0; deb_i < length; deb_i++)
				// {
				// 	printk("%x ", spim_adc_rx_buf[deb_i]);
				// }
				// printk("\n");
			}
			spim_adc_xfer_done = false;
		}
	}
}