/*
 * Project: Arrow
 * Author: 	curiousmuch
 * Description: This project is an APRS transceiver based on the ESP32 and CC1200.
 * TODO: Refactor Code to isolate into seperate components
 * TODO: Isolate BT_SPP.C functionality
 * TODO: Isolate KISS.C functionality
 * TODO: Remove any dependancies on Direwolf.
 * TODO: Transfer DSP functions to APRS folder
 * TODO: Abstract includes to be nicer (#include aprs.h) for example
 * TODO: Isolate CC1200 functions from other parts / maybe integrate most of TX scheme into task.
 */
#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "freertos/ringbuf.h"

#include "driver/gpio.h"
#include "sdkconfig.h"
#include "esp_task_wdt.h"

#include "cc1200.h"
#include "cc1200_protocol.h"
#include "board.h"

#include "nvs.h"
#include "nvs_flash.h"
#include "esp_log.h"
#include "esp_bt.h"
#include "esp_bt_main.h"
#include "esp_gap_bt_api.h"
#include "esp_bt_device.h"
#include "esp_spp_api.h"
#include "bt_spp.h"

#include "ax25_pad2.h"
#include "ax25_pad.h"
#include "fcs_calc.h"


#include "math.h"

#include "driver/timer.h"
#include "aprs_decoder.h"
#include "kiss.h"

#include "afsk_demodulator.h"

//#include "esp_dsp.h"

RingbufHandle_t radio_tx_buf;
RingbufHandle_t radio_rx_buf;

SemaphoreHandle_t xRadioRXISRSemaphore;
SemaphoreHandle_t xRadioRXSemaphore;
SemaphoreHandle_t xRadioTXSemaphore;
SemaphoreHandle_t xRadioMutex;

//RingbufHandle_t radio_rx_buf;

extern int8_t EXTERNAL_DATA;
extern decoder_varibles_t v;

// Timer Functions
void IRAM_ATTR rx_timer_isr(void *para)
{

    //GPIO.out_w1ts = (1 << DEBUG_0);

	int timer_idx = (int) para;

    /* Clear the interrupt
         and update the alarm time for the timer with without reload */
    TIMERG0.int_clr_timers.t0 = 1;

    // after the alarm has been triggered
    //  we need enable it again, so it is triggered the next time
    TIMERG0.hw_timer[0].config.alarm_en = TIMER_ALARM_EN;

	static BaseType_t xHigherPriorityTaskWoken = pdFALSE;

    xSemaphoreGiveFromISR(xRadioRXISRSemaphore, &xHigherPriorityTaskWoken);
	if (xHigherPriorityTaskWoken == pdTRUE)
	{
		portYIELD_FROM_ISR( );
	}

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

}

void Radio_Task(void *pvParameters)
{
	size_t p_size;

    // Setup Timer for TX Task
	timer_config_t config;
    config.divider = 2;
    config.counter_dir = TIMER_COUNT_UP;
    config.counter_en = TIMER_PAUSE;
    config.alarm_en = TIMER_ALARM_EN;
    config.intr_type = TIMER_INTR_LEVEL;
    config.auto_reload = TIMER_AUTORELOAD_EN;
    timer_init(TIMER_GROUP_0, TIMER_1, &config);
    timer_set_counter_value(TIMER_GROUP_0, TIMER_1, 0x00000000ULL);
    timer_set_alarm_value(TIMER_GROUP_0, TIMER_1, 3030);
    timer_isr_register(TIMER_GROUP_0, TIMER_1, cc1200_aprs_tx_isr,
    		(void *) TIMER_1, ESP_INTR_FLAG_IRAM, NULL);

	// Setup Sampling Timer for RX Task
	timer_config_t rx_config;
	rx_config.divider = 2;
	rx_config.counter_dir = TIMER_COUNT_UP;
	rx_config.counter_en = TIMER_PAUSE;
	rx_config.alarm_en = TIMER_ALARM_EN;
	rx_config.intr_type = TIMER_INTR_LEVEL;
	rx_config.auto_reload = TIMER_AUTORELOAD_EN;
    timer_init(TIMER_GROUP_0, TIMER_0, &rx_config);
    timer_set_counter_value(TIMER_GROUP_0, TIMER_0, 0x00000000ULL);
    timer_set_alarm_value(TIMER_GROUP_0, TIMER_0, 3030);
    timer_isr_register(TIMER_GROUP_0, TIMER_0, rx_timer_isr,
    		(void *) TIMER_0, ESP_INTR_FLAG_IRAM, NULL);

	// Initialize Radio
	cc1200_radio_init(APRS_RX2_SETTINGS, sizeof(APRS_RX2_SETTINGS)/sizeof(cc1200_reg_settings_t));
	cc1200_radio_frequency(144390000);


	while(1)
	{
		// setup LEDs for RX mode
		disable_red_led();
		enable_green_led();
		cc1200_radio_start_APRSRX();
	    timer_enable_intr(TIMER_GROUP_0, TIMER_0);
	    timer_set_counter_value(TIMER_GROUP_0, TIMER_0, 0x00000000ULL);
	    timer_start(TIMER_GROUP_0, TIMER_0);


		// block until packet is in queue
		uint8_t *p = (uint8_t *)xRingbufferReceive(radio_tx_buf, &p_size, portMAX_DELAY);

		// send packet
		if (p != NULL)
		{

			// disable RX mode
		    timer_disable_intr(TIMER_GROUP_0, TIMER_0);
			timer_pause(TIMER_GROUP_0, TIMER_0);
			cc1200_radio_stop_APRSRX();

			// setup LEDs for TX mode
			enable_red_led();
			disable_green_led();

			cc1200_radio_APRSTXPacket(p, p_size, 4, 1);

			vRingbufferReturnItem(radio_tx_buf, (void *)p);

		}
	}
}


// Phase Locked Loop (PLL) Parameters
#define BAUD_RATE (1200)
#define SAMPLES_BIT (SAMPLEFREQUENCY / BAUD_RATE)
#define D_PLL_INC (SAMPLES_BIT * 1)
#define D_PLL_MAX (D_PLL_INC * SAMPLES_BIT *1)
#define D_PLL_MARGIN (dpll_margin)

uint8_t aprs_buf[256];

void dsps_biquad_gen_lpf_f32(float *coeffs, float f, float qFactor)
{
    if (qFactor <= 0.0001) {
        qFactor = 0.0001;
    }
    float Fs = 1;

    float w0 = 2 * M_PI * f / Fs;
    float c = cosf(w0);
    float s = sinf(w0);
    float alpha = s / (2 * qFactor);

    float b0 = (1 - c) / 2;
    float b1 = 1 - c;
    float b2 = b0;
    float a0 = 1 + alpha;
    float a1 = -2 * c;
    float a2 = 1 - alpha;

    coeffs[0] = b0 / a0;
    coeffs[1] = b1 / a0;
    coeffs[2] = b2 / a0;
    coeffs[3] = a1 / a0;
    coeffs[4] = a2 / a0;
    return;
}

void dsps_biquad_f32_ansi(const float *input, float *output, int len, float *coef, float *w)
{
    for (int i = 0 ; i < len ; i++) {
        float d0 = input[i] - coef[3] * w[0] - coef[4] * w[1];
        output[i] = coef[0] * d0 +  coef[1] * w[0] + coef[2] * w[1];
        w[1] = w[0];
        w[0] = d0;
    }
    return;
}

float lpf_coef0[5];
float lpf_coef1[5];
float lpf_delay0[2];
float lpf_delay1[2];

void lpf_init(void)
{
	// generate filter coef
	dsps_biquad_gen_lpf_f32(lpf_coef0, 0.10, 0.54119610);
	dsps_biquad_gen_lpf_f32(lpf_coef1, 0.10, 1.3065630);


	// setup delay line and lpf_input window
	int i;
	for(i=0;i<2;i++)
	{
		lpf_delay0[i] = 0;
		lpf_delay1[i] = 0;
	}
}

float lpf_baseband(float input)
{
	float output, temp;
	dsps_biquad_f32_ansi(&input, &temp, 1, lpf_coef0, lpf_delay0);
	dsps_biquad_f32_ansi(&temp, &output, 1, lpf_coef1, lpf_delay1);

	return output;
}


void RX_DSP_Task(void *pvParameters)
{
	// algorithm variables
	volatile float E_s1=0, E_s2=0, lpf_output;
	volatile uint8_t raw_bit=0, previous_raw_bit=1;
	volatile int32_t d_pll=0, dpll_error=0, err_div=0, dpll_margin = 1;
	uint32_t success = 0;
	float coeff0, coeff1;

	volatile uint32_t count_start, count_stop; // needed for debuging


	aprs_decoder_init();
	frame_buffer_init(aprs_buf, 256);
	goertzel_init(1200, 2200, &coeff0, &coeff1);
	lpf_init();

	// Sampling Semaphore
	xRadioRXISRSemaphore = xSemaphoreCreateBinary();

	while(1)
	{
		count_start = xthal_get_ccount();
		if( xSemaphoreTake(xRadioRXISRSemaphore, portMAX_DELAY) == pdTRUE)
		{

			//enable_debug_IO(DEBUG_0);
			if (xSemaphoreTake(xRadioMutex, 0) == pdTRUE)
			{
				//disable_debug_IO(DEBUG_0);
				//enable_debug_IO(DEBUG_0);
				EXTERNAL_DATA = (int)cc1200_radio_read_CFM();
				//EXTERNAL_DATA = gpio_get_level(CC1120_GPIO2);
				xSemaphoreGive(xRadioMutex);
				//disable_debug_IO(DEBUG_0);
			}
			else
			{
				ESP_LOGE("Radio", "Sampling Failure");
			}
			//disable_debug_IO(DEBUG_0);
			//enable_debug_IO(DEBUG_0);

			window_add(EXTERNAL_DATA);
			int8_t *window = window_get();
			E_s1 = goertzel_filter(window, coeff0, WINDOW_SIZE);
			E_s2 = goertzel_filter(window, coeff1, WINDOW_SIZE);
			//E_s1 = goertzelFilter(window, 1200, WINDOW_SIZE);
			//E_s1 = goertzelFilter(window, 2200, WINDOW_SIZE);
			lpf_output = lpf_baseband((E_s1 - E_s2));

			if (lpf_output > 0)
			{
				raw_bit = 1;
				enable_debug_IO(DEBUG_2);

			}
			else
			{
				raw_bit = 0;
				disable_debug_IO(DEBUG_2);
			}

			//disable_debug_IO(DEBUG_0);
			//enable_debug_IO(DEBUG_0);

			// DPLL for sampling
			if (raw_bit != previous_raw_bit)
			{
				dpll_error = d_pll - (D_PLL_MAX / 2);
				if (dpll_error >  (D_PLL_MARGIN))
				{
					d_pll -= get_rx_status()  ? D_PLL_MARGIN:
							(err_div + dpll_error);//(dpll_error + err_div / 2) / err_div;
				}
				else if (dpll_error < (-D_PLL_MARGIN))
				{
					d_pll += get_rx_status()  ? D_PLL_MARGIN:
							-(err_div + dpll_error); //(dpll_error + err_div / 2) / err_div;
				}
			}

			d_pll += D_PLL_INC;

			if (d_pll >= D_PLL_MAX)
			{
				// set clock debug I/O high
				enable_debug_IO(DEBUG_3);

				// feed bit to aprs decoder algorithm
				switch(aprs_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);
						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;
			}
			else
			{
				// set clock debug I/O low
				disable_debug_IO(DEBUG_3);
			}
			count_stop = xthal_get_ccount();
			previous_raw_bit = raw_bit;
		}
	}
}


void radio_init()
{
	// Setup Task Communications
	radio_tx_buf = xRingbufferCreate(1028, RINGBUF_TYPE_NOSPLIT);
	radio_rx_buf = xRingbufferCreate(1028, RINGBUF_TYPE_NOSPLIT);
	xRadioRXSemaphore = xSemaphoreCreateBinary();
	xRadioTXSemaphore = xSemaphoreCreateBinary();
	xRadioMutex = xSemaphoreCreateMutex();

	xTaskCreatePinnedToCore(Radio_Task, "Radio Controller", 1024*4, 0, 2, NULL, 1);
	xTaskCreatePinnedToCore(RX_DSP_Task, "Radio DSP", 1024*4, 0, 1, NULL, 1);

}

void IRAM_ATTR app_main()
{
	// Initialize Flash
	esp_err_t ret = nvs_flash_init();
	if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
		ESP_ERROR_CHECK(nvs_flash_erase());
		ret = nvs_flash_init();
	}
	ESP_ERROR_CHECK( ret );

	// Board IO Initialize
	board_init();

	// Radio Task Initialize
	radio_init();

	// AFSK Decoder and Encoder


	// Kiss Decoder and Encoder
	kiss_init();

	// BLE and SPP
	bt_spp_init();
}