/*
 * Project: Arrow
 * Author: 	curiousmuch
 */
#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.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.c"

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

#include "math.h"

#include "driver/timer.h"


//#include "esp_dsp.h"

uint8_t APRS_TEST_PACKET[] = { 0x82, 0x98, 0x98, 0x40, 0x40, 0x40, 0xe0, 0x96, 0x84, 0x66, 0xaa, 0x96, 0xac, 0xe0, 0xae, 0x92,
									 0x88, 0x8a, 0x62, 0x40, 0x62, 0xae, 0x92, 0x88, 0x8a, 0x64, 0x40, 0x65, 0x03, 0xf0, 0x3a, 0x4b,
									 0x42, 0x33, 0x55, 0x4b, 0x56, 0x2d, 0x32, 0x20, 0x3a, 0x48, 0x69, 0x21, 0x20, 0x54, 0x68, 0x69, 0x73,
									 0x20, 0x69, 0x73, 0x20, 0x61, 0x20, 0x54, 0x65, 0x73, 0x74, 0x7b, 0x31, 0xad, 0xa1 };

RingbufHandle_t radio_tx_buf;
SemaphoreHandle_t xRadioRXISRSemaphore;
SemaphoreHandle_t xRadioRXSemaphore;
SemaphoreHandle_t xRadioTXSemaphore;

//RingbufHandle_t radio_rx_buf;

extern int8_t EXTERNAL_DATA;

#define WINDOW_SIZE 6
#define SAMPLEFREQUENCY 6000

int window[WINDOW_SIZE];
int lpf_window[WINDOW_SIZE];

double goertzelFilter(int samples[], double freq, int N)
{
    double s_prev = 0.0;
    double s_prev2 = 0.0;
    double coeff,normalizedfreq,power,s;
    int i;
    normalizedfreq = freq / SAMPLEFREQUENCY;
    coeff = 2*cos(2*M_PI*normalizedfreq);
    for (i=0; i<N; i++) {
        s = samples[i] + coeff * s_prev - s_prev2;
        s_prev2 = s_prev;
        s_prev = s;
    }
    power = s_prev2*s_prev2+s_prev*s_prev-coeff*s_prev*s_prev2;
    return power;
}

//double calculate_symbols_energy(int samples[], int N)
//{
//	double E_s1, E_s2;
//	E_s1 = goertzelFilter(samples, 1200, N);
//	E_s2 = goertzelFilter(samples, 2200, N);
//	return (E_s1 - E_s2);
//}

void window_init(void)
{
	for(int i=0;i<WINDOW_SIZE;i++)
	{
		window[i] = 0;
	}
}

void window_add(int sample)
{
	for(int i=0;i<(WINDOW_SIZE-1);i++)
	{
		window[(WINDOW_SIZE-1)-i] = window[(WINDOW_SIZE-2)-i];
	}
	window[0] = sample;
}

uint8_t * window_get(void)
{
	return window;
}

int window_get_size(void)
{
	return WINDOW_SIZE;
}


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

}

// The TX Task should have the highest
void TX_Task(void *pvParameters)
{
	size_t p_size;

	while(1)
	{

		// 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_pause(TIMER_GROUP_0, TIMER_0);
		    timer_disable_intr(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, 2, 0);

			vRingbufferReturnItem(radio_tx_buf, (void *)p);

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


void RX_Task(void *pvParameters)
{
	// debugging variables
	uint8_t toggle = 0;
	uint8_t count = 0;

	// algorithm variables
	double E_s1, E_s2, result;
	E_s1 = 0; E_s2 = 0;
	int f_output = 0;

	// Sampling Semaphore
	xRadioRXISRSemaphore = xSemaphoreCreateBinary();

	// setup LEDs for RX mode
	enable_green_led();
	disable_red_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);


	while(1)
	{

		//ulNotificationValue = ulTaskNotifyTake(pdTRUE, 0);
		if( xSemaphoreTake(xRadioRXISRSemaphore, portMAX_DELAY) == pdTRUE)
		{

			//enable_debug_IO(DEBUG_0);
			EXTERNAL_DATA = (int)cc1200_radio_read_CFM();
			//disable_debug_IO(DEBUG_0);
			//enable_debug_IO(DEBUG_0);
			//f_output += (50000 * (((int)EXTERNAL_DATA) - f_output)) / 100000;
			window_add((int) EXTERNAL_DATA);
			E_s1 = goertzelFilter(window, 1200, WINDOW_SIZE);
			E_s2 = goertzelFilter(window, 2200, WINDOW_SIZE);
			if (E_s1 > E_s2)
			{
				enable_debug_IO(DEBUG_0);
			}
			else
			{
				disable_debug_IO(DEBUG_0);
			}
			//disable_debug_IO(DEBUG_0);
		}
	}
}

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

	// Initialize Radio
	cc1200_radio_init(APRS_RX_SETTINGS, sizeof(APRS_RX_SETTINGS)/sizeof(cc1200_reg_settings_t));
	cc1200_radio_frequency(144390000-6000);

	// Setup Sampling Timer
	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_0, &config);
    timer_set_counter_value(TIMER_GROUP_0, TIMER_0, 0x00000000ULL);
    timer_set_alarm_value(TIMER_GROUP_0, TIMER_0, 6666);
    //timer_enable_intr(TIMER_GROUP_0, TIMER_0);
    timer_isr_register(TIMER_GROUP_0, TIMER_0, rx_timer_isr,
    		(void *) TIMER_0, ESP_INTR_FLAG_IRAM, NULL);
    //timer_start(TIMER_GROUP_0, TIMER_0);


	// Create Tasks
//	xTaskCreatePinnedToCore(Radio_Controller_Task, "Radio_Controller", 1024*4, 0, 10, NULL, 0);
	xTaskCreatePinnedToCore(TX_Task, "TX Task", 1024*4, 0, 2, NULL, 1);
	xTaskCreatePinnedToCore(RX_Task, "RX Task", 1024*4, 0, 1, NULL, 1);


//	xTaskCreatePinnedToCore(UART_Task, "UART Task", 1024*4, 0, 5, NULL, 0);
}

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

	// Setup Kiss Decoder and Encoder
	kiss_init();

	// Initalize BLE
	bt_spp_init();
}