/*
 * radio.c
 *
 *  Created on: Oct 11, 2019
 *      Author: curiousmuch
 */

#include <stdio.h>
#include <stdint.h>
#include <math.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"

#include "sdkconfig.h"
#include "esp_log.h"

#include "driver/timer.h"

#include "radio.h"
#include "cc1200.h"
#include "cc1200_protocol.h"

#include "afsk_modulator.h"
#include "afsk_demodulator.h"
#include "aprs_decoder.h"
#include "aprs_encoder.h"

/* Debugging Tag */
#define RADIO_TAG "radio"

/* Data Structures */
typedef enum {
	RADIO_TIMER_NO_CONFIG = 0,
	RADIO_TIMER_TX,
	RADIO_TIMER_RX,
} timer_radio_semaphore_t;

/* Local Global Variables */
static radio_param_t radio_param = { .type = NOT_CONFIGURED,
									 .status = RADIO_IDLE,
									 .rx_cb = NULL,
									 .tx_cb = NULL };

static SemaphoreHandle_t xRadioTXSemaphore;
static SemaphoreHandle_t xRadioRXSemaphore;
static SemaphoreHandle_t xRadioMutex;
static timer_radio_semaphore_t radio_semaphore;

#define DEBUG_0 			16
#define DEBUG_1				4
#define DEBUG_2				32
#define DEBUG_3				33

/* Private Functions */
void IRAM_ATTR radio_timer_isr(void *param)
{
    GPIO.out_w1ts = (1 << DEBUG_0);

	int timer_idx = (int) param; 	// cast to int for timer index
	TIMERG0.int_clr_timers.t0 = 1; 	// clear interrupt
	TIMERG0.hw_timer[0].config.alarm_en = TIMER_ALARM_EN; // re-enable timer alarm

	// provide unblocking semaphore
	static BaseType_t xHigherPriorityTaskWoken = pdFALSE;
	switch(radio_semaphore) {
		case RADIO_TIMER_NO_CONFIG:
			//ESP_LOGE(RADIO_TAG, "no configuration");
			break;
		case RADIO_TIMER_RX:
			xSemaphoreGiveFromISR(xRadioRXSemaphore, &xHigherPriorityTaskWoken);
			break;
		case RADIO_TIMER_TX:
			xSemaphoreGiveFromISR(xRadioTXSemaphore, &xHigherPriorityTaskWoken);
			break;
		default:
			//ESP_LOGE(RADIO_TAG, "invalid configuration");
			break;
	}

	// wake higher priority task if woken
	if (xHigherPriorityTaskWoken == pdTRUE)
	{
		portYIELD_FROM_ISR();
	}

	GPIO.out_w1tc = (1 << DEBUG_0);
}

#define RADIO_TIMER_GROUP TIMER_GROUP_0
#define RADIO_TIMER	TIMER_0

void timer_radio_init(uint32_t sample_rate, void *timer_isr)
{
	// setup timer settings
	timer_config_t timer_config;

	timer_config.divider = 2;
	timer_config.counter_dir = TIMER_COUNT_UP;
	timer_config.counter_en = TIMER_PAUSE;
	timer_config.alarm_en = TIMER_ALARM_EN;
	timer_config.intr_type = TIMER_INTR_LEVEL;
	timer_config.auto_reload = TIMER_AUTORELOAD_EN;

	// calculate value for timer alarm
	// TODO: confirm timer alarm value calculation
	uint32_t timer_alarm;
	timer_alarm = (uint32_t)(40000000 / sample_rate);

	// load timer settings
    timer_init(RADIO_TIMER_GROUP, RADIO_TIMER, &timer_config);
    timer_set_counter_value(RADIO_TIMER_GROUP, RADIO_TIMER, 0x00000000ULL);
    timer_set_alarm_value(RADIO_TIMER_GROUP, RADIO_TIMER, 3030);
    timer_isr_register(RADIO_TIMER_GROUP, RADIO_TIMER, timer_isr,
    		(void *) RADIO_TIMER, ESP_INTR_FLAG_IRAM, NULL);

    // setup timer ISR semaphore type
    radio_semaphore = RADIO_TIMER_NO_CONFIG;
}

void timer_radio_start(timer_radio_semaphore_t type)
{
	// setup timer ISR semaphore type
	radio_semaphore = type;

	// start timer
	timer_enable_intr(RADIO_TIMER_GROUP, RADIO_TIMER);
    timer_set_counter_value(RADIO_TIMER_GROUP, RADIO_TIMER, 0x00000000ULL);
    timer_start(RADIO_TIMER_GROUP, RADIO_TIMER);
}

void timer_radio_stop(void)
{
	// setup timer ISR semaphore type
	radio_semaphore = RADIO_TIMER_NO_CONFIG;

	// stop timer
    timer_disable_intr(RADIO_TIMER_GROUP, RADIO_TIMER);
	timer_pause(RADIO_TIMER_GROUP, RADIO_TIMER);
}

/* HAL Layer */

void radio_rx_task(void *para);

void radio_idle(void)
{
	// disable all radio tasks
	//vTaskSuspend(radio_rx_task);

	// disable all radio timers
	timer_radio_stop();

	cc1200_radio_idle();
	radio_param.status = RADIO_IDLE;
}

void radio_tx(void)
{
	if (radio_param.status != RADIO_IDLE)
		radio_idle();

	cc1200_radio_tx();
	radio_param.status = RADIO_TX;
}

void radio_rx(void)
{
	if (radio_param.status != RADIO_IDLE)
		radio_idle();

	cc1200_radio_rx();
	radio_param.status = RADIO_RX;
}

radio_status_t radio_get_status(void)
{
	return radio_param.status;
}

uint8_t radio_get_cs(void)
{
	return ax25_decoder_get_cs();
}

void radio_packet_tx(uint8_t *data, int32_t len, void *settings)
{


	// configure radio for TX
	switch (radio_param.type) {
		case AX25: {
			ESP_LOGI(RADIO_TAG, "transmitting AX25 packet");

			// load ax25 encoder state machine
			ax25_encoder_encode(data, len);

			// configure radio to send CW
			xSemaphoreTake( xRadioMutex, portMAX_DELAY );	// lock radio
			radio_tx();
			xSemaphoreGive( xRadioMutex );					// unlock radio

			// start sampling timer
			timer_radio_start(RADIO_TIMER_TX);
			break;
		}
		default: {
			ESP_LOGE(RADIO_TAG, "invalid configuration");
			break;
		}
	}

}

void radio_set_frequency(uint32_t freq)
{
	xSemaphoreTake( xRadioMutex, portMAX_DELAY );	// lock radio

	cc1200_radio_frequency(freq);

	xSemaphoreGive( xRadioMutex );

}

void radio_tx_task(void *para)
{
	// variables for transmission
	uint32_t sample_count = 0;
	uint8_t nrzi_bit = 0;
	int8_t amplitude = 0;

	// metrics
	uint32_t tx_count = 0;

	while(1)
	{

		// block until sampling timer is running
		if (xSemaphoreTake(xRadioTXSemaphore, portMAX_DELAY) == pdTRUE)
		{
			if (ax25_encoder_get_status() == READY)
			{
				// reset sample count used per RF symbol
				sample_count = 0;

				// setup first amplitude for tone
				nrzi_bit = ax25_encoder_get_bit();
				amplitude = afsk_get_amplitude(nrzi_bit);
			}
			else
			{
				// process error
			}
		}

		// run until packet is finished
		while(ax25_encoder_get_status() == READY)
		{
			// block until semaphore is given or there has been a timing error
			if (xSemaphoreTake(xRadioTXSemaphore, portMAX_DELAY) == pdTRUE)
			{
				// update carrier for AFSK
				xSemaphoreTake( xRadioMutex, portMAX_DELAY );	// lock radio

				cc1200_radio_write_CFM(amplitude);

				xSemaphoreGive( xRadioMutex );	// unlock radio

				// increment symbol count
				sample_count++;
				if (sample_count >= 11)
				{
				    GPIO.out_w1ts = (1 << DEBUG_1);

					nrzi_bit = ax25_encoder_get_bit();
					sample_count = 0;

				    GPIO.out_w1tc = (1 << DEBUG_1);

				}

				// get amplitude for next nrzi bit
				amplitude = afsk_get_amplitude(nrzi_bit);
			}
			else
			{
				ESP_LOGE(RADIO_TAG, "timing error");
				ESP_LOGI(RADIO_TAG, "canceling transmission");
				break;
			}
		}

		// stop sampling timer
		timer_radio_stop();

		// confirm if TX failed or not
		// ax25_encoder will not reach DONE if their is a timing failure
		switch(ax25_encoder_get_status()) {
			case STOP:
				ESP_LOGI(RADIO_TAG, "transmission complete: [%d]", tx_count++);
				break;
			case READY:
				ESP_LOGE(RADIO_TAG, "transmission failed");
				break;
			case ERROR:
				ESP_LOGE(RADIO_TAG, "encoder error");
				break;
			default:
				ESP_LOGE(RADIO_TAG, "unknown encoder error");
		}

		// active callback function
		if (radio_param.tx_cb != NULL)
		{
			radio_param.tx_cb();
		}

	}
}

void radio_rx_task(void *para)
{
	// analog variables
	int8_t cfm_value=0;

	// demod variables
	uint8_t raw_bit=0, prev_raw_bit=0;

	// pll variables and settings
	int32_t d_pll=0, dpll_error=0, err_div=0;

	// TODO: The following group should be made user setable fed via the settings
	const int32_t SAMPLE_FREQUENCY = 13200;
	const int32_t BAUD_RATE = 1200;
	const int32_t SAMPLES_BIT = SAMPLE_FREQUENCY / BAUD_RATE;
	const int32_t D_PLL_INC = (SAMPLES_BIT * 1);
	const int32_t D_PLL_MAX = (D_PLL_INC * SAMPLES_BIT * 1);
	const int32_t D_PLL_MARGIN = 1;

	// decoder variables
	uint32_t success=0;

	// task
	while(1)
	{
		// block until semphore is given or there has been a timing error
		if (xSemaphoreTake(xRadioRXSemaphore, portMAX_DELAY) == pdTRUE)
		{
			// read register analog value from radio
			xSemaphoreTake( xRadioMutex, portMAX_DELAY );	// lock radio
			cfm_value = cc1200_radio_read_CFM();
			xSemaphoreGive( xRadioMutex );					// unlock radio

			// afsk demod
			raw_bit = afsk_demod_add_sample(cfm_value);

			// clock synchronizer for sampling
			if (raw_bit != prev_raw_bit)
			{
				dpll_error = d_pll - (D_PLL_MAX / 2);
				if (dpll_error > D_PLL_MARGIN)
				{
					d_pll -= ax25_decoder_get_cs() ? D_PLL_MARGIN:
							(err_div + dpll_error); //(dpll_error + err_div / 2) / err_div;

				}
				else if (dpll_error < -D_PLL_MARGIN)
				{
					d_pll += ax25_decoder_get_cs() ? D_PLL_MARGIN :
							-(err_div + dpll_error); // (dpll_error + err_div / 2) / err_div;
				}

			}

			// increment clock
			d_pll += D_PLL_INC;

			// slice
			if (d_pll >= D_PLL_MAX)
			{
				switch (ax25_decoder_feed_bit(raw_bit))
				{
					case NORMAL:
						break;
					case FRAME_DECODED:
						ESP_LOGI("radio", "AX25 transmission received: [%d]", success++);
						// send via KISS TNC to over BLE SPP
						//ESP_LOG_BUFFER_HEXDUMP("APRS RX", aprs_buf, 100, ESP_LOG_INFO);
						//kiss_transmit(KISS_DATAFRAME, v.frame_buffer, v.frame_len);
						if (radio_param.rx_cb != NULL)
							radio_param.rx_cb(ax25_decoder_get_frame(), ax25_decoder_get_frame_len());
						break;
					case ERROR_FCS_MISMATCH:
						ESP_LOGV("radio", "AX25 fcs error");
						break;
					default:
						//printf("Weird Error\n");
						break;
				}
				d_pll -= D_PLL_MAX;
			}
			prev_raw_bit = raw_bit;
		}
		else
		{
			ESP_LOGE(RADIO_TAG, "rx timing error");
		}
	}
}

// Functionality will depend on protocol
// AX.25 - Non-Block and will activate callback function
void radio_packet_rx(void *settings)
{
	switch(radio_param.type)
	{
		case AX25: {
			// reset decoder
			ax25_decoder_reset();	// this is required because sometimes it locks up if stopped
									// by TX Task

			// setup radio
			xSemaphoreTake( xRadioMutex, portMAX_DELAY );	// lock radio
			radio_rx();
			xSemaphoreGive( xRadioMutex );					// unlock radio

			// setup sampling timer
			timer_radio_start(RADIO_TIMER_RX);
			break;
		}
		default: {
			ESP_LOGE(RADIO_TAG, "invalid configuration");
			break;
		}

	}

}

void radio_reinit(radio_config_t type, void *settings)
{
	// lock radio
	xSemaphoreTake(xRadioMutex, portMAX_DELAY);

	// disable radio
	radio_idle();

	// suspend radio tasks
	if (radio_param.type == AX25)
	{
		vTaskSuspend(radio_param.tx_task);
		vTaskSuspend(radio_param.rx_task);
	}

	switch(type)
	{
		case AX25:
			ESP_LOGI(RADIO_TAG, "APRS Mode");
			radio_param.type = AX25;

			// cast setting struct
			ax25_param_t *p = (ax25_param_t *)settings;

			// setup LUT for AFSK demodulator
			afsk_mod_init(p->sample_rate, p->symbol0_freq, p->symbol1_freq);

			// setup frequency detectors for AFSK demodulator
			afsk_demod_init(p->sample_rate, p->symbol0_freq, p->symbol1_freq);

			// setup encoder for ax25
			ax25_encoder_init(p->tx_delay, p->tx_tail);

			// setup ax25 decoder
			ax25_decoder_init(p->rx_buf, p->rx_buf_len);
			radio_param.rx_cb = p->rx_cb;
			radio_param.tx_cb = p->tx_cb;

			// load AX25_settings without reseting / reloading SPI
			cc1200_radio_config(AX25_SETTINGS, sizeof(AX25_SETTINGS)/sizeof(cc1200_reg_settings_t));

			// put chip to idle
			radio_idle();

			// resume radio tasks
			break;
		default:
			ESP_LOGE(RADIO_TAG, "invalid configuration");
			radio_param.type = NOT_CONFIGURED;
			break;
	}

	// resume tasks
	// suspend radio tasks
	if (radio_param.type == AX25)
	{
		vTaskResume(radio_param.tx_task);
		vTaskResume(radio_param.rx_task);
	}

	xSemaphoreGive(xRadioMutex);
}

void radio_init(radio_config_t type, void *settings)
{
	// create mutux so SPI transactions will be multi-thread safe
	// TODO: fix so mutex is only created one to avoid memory leak
	xRadioMutex = xSemaphoreCreateMutex();

	// lock radio
	xSemaphoreTake( xRadioMutex, portMAX_DELAY );

	switch(type)
	{
		case AX25: {
			ESP_LOGI("Radio", "APRS Mode");
			radio_param.type = AX25;

			// cast setting struct
			ax25_param_t *p = (ax25_param_t *)settings;

			// setup LUT for AFSK demodulator
			afsk_mod_init(p->sample_rate, p->symbol0_freq, p->symbol1_freq);

			// setup frequency detectors for AFSK demodulator
			afsk_demod_init(p->sample_rate, p->symbol0_freq, p->symbol1_freq);

			// setup encoder for ax25
			ax25_encoder_init(p->tx_delay, p->tx_tail);

			// setup ax25 decoder
			ax25_decoder_init(p->rx_buf, p->rx_buf_len);
			radio_param.rx_cb = p->rx_cb;
			radio_param.tx_cb = p->tx_cb;

			// setup timer for TX / RX
			timer_radio_init(p->sample_rate, radio_timer_isr);

			// configure CC1200
			cc1200_radio_init(AX25_SETTINGS, sizeof(AX25_SETTINGS)/sizeof(cc1200_reg_settings_t));

			// put chip to idle
			radio_idle();

			// task communication
			xRadioRXSemaphore = xSemaphoreCreateBinary();
			xRadioTXSemaphore = xSemaphoreCreateBinary();

			// create task for sampling
			// TODO: Reduce stack requirements?
			xTaskCreatePinnedToCore(radio_rx_task, "radio rx", 1024*4, 0, 1, &radio_param.tx_task, p->cpu_core);
			xTaskCreatePinnedToCore(radio_tx_task, "radio tx", 1024*4, 0, 2, &radio_param.rx_task, p->cpu_core);
			//vTaskSuspend(radio_rx_task);
			break;
		}
		default: {
			ESP_LOGE(RADIO_TAG, "invalid configuration");
			radio_param.type = NOT_CONFIGURED;
			//TODO: Add assert to stop functionality
			break;
		}
	}

	// unlock radio
	xSemaphoreGive( xRadioMutex );
}