/*
 * 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"

/* Local Global Variables */
static radio_param_t radio_param;				// radio configuraiton
static SemaphoreHandle_t xRadioSemaphore;		// semphore for sample timing

/* 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;
	xSemaphoreGiveFromISR(xRadioSemaphore, &xHigherPriorityTaskWoken);
	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)
{
	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;


	uint32_t timer_alarm;
	timer_alarm = (uint32_t)(40000000 / sample_rate);

	// TODO: confirm timer_alarm calculation

    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);
}

void timer_radio_start(void)
{
	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)
{
    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;
}

int8_t radio_get_cs()
{
	if (radio_param.status != RADIO_RX)
		return 0;
	else
		return ax25_decoder_get_cs();
}

void radio_init(radio_config_t type, void *settings)
{
	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;

			// 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
			xRadioSemaphore = xSemaphoreCreateBinary();

			// create task for sampling
			// TODO: Reduce stack requirements?
			xTaskCreatePinnedToCore(radio_rx_task, "radio rx", 1024*4, 0, 1, NULL, 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;
		}
	}
}

// this function will block and configure
// the cc1200 for tx using the protocol setup by "radio_init"
// the buffer holding the packet isn't copied and therefore must be held until
// the function returns
// TODO: Should this be non-blocking and start up a task / wait for an interrupt
// then fire a callback function?
void radio_packet_tx(uint8_t *data, int32_t len, void *settings)
{
	switch(radio_param.type)
	{
		case AX25: {
			ESP_LOGV(RADIO_TAG, "transmitting AX25 packet");

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

			// variables for symbol timing (11 samples per symbol)
			uint32_t sample_count = 0; int8_t tone;
			uint8_t bit = ax25_encoder_get_bit();

			// configure radio to send CW
			radio_tx();

			// start sampling timer
			timer_radio_start();

			while(ax25_encoder_get_status() == READY)
			{
				// TODO: Do we need to feed the WDT here?

				// get amplitude for AFSK
				tone = afsk_get_amplitude(bit);

				// pause for semphore
				if (xSemaphoreTake(xRadioSemaphore, portMAX_DELAY) == pdTRUE)
				{
					// update carrier for AFSK
					cc1200_radio_write_CFM(tone);

					// inc symbol count
					sample_count++;
					if (sample_count > 11)
					{
						bit = ax25_encoder_get_bit();
						sample_count = 0;
					}
				}
				else
				{
					ESP_LOGE(RADIO_TAG, "sampling timeout error");
					break;
				}

			}
			radio_idle();
			ESP_LOGV(RADIO_TAG, "transmit complete");
			break;
		}
		default: {
			ESP_LOGE(RADIO_TAG, "invalid configuration");
			break;
		}
	}
}

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 = 132000;
	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(xRadioSemaphore, portMAX_DELAY) == pdTRUE)
		{
			// read register analog value from radio
			cfm_value = cc1200_radio_read_CFM();

			// 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("APRS RX", "AX.25 Frame 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("APRS RX", "AX.25 FCS Error\n");
						break;
					default:
						//printf("Weird Error\n");
						break;
				}
				d_pll -= D_PLL_MAX;
			}
			prev_raw_bit = raw_bit;
		}
		else
		{
			ESP_LOGE(RADIO_TAG, "sampling timeout 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: {
			// setup radio
			radio_rx();

			// setup sampling timer
			timer_radio_start();

			// unsuspend task
			vTaskResume(radio_rx_task); //??????? should this be top or bottom

			break;
		}
		default: {
			ESP_LOGE(RADIO_TAG, "invalid configuration");
			break;
		}

	}
}