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

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 xRadioRXSemaphore;
//RingbufHandle_t radio_rx_buf;

extern int8_t EXTERNAL_DATA;

#define WINDOW_SIZE 11
#define SAMPLEFREQUENCY 6000

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

void Radio_Task(void *pvParameters)
{
	size_t p_size;

	// Setup Radio
	xRadioRXSemaphore = xSemaphoreCreateBinary();
	cc1200_radio_init(APRS_RX_SETTINGS, sizeof(APRS_RX_SETTINGS)/sizeof(cc1200_reg_settings_t));
	cc1200_radio_frequency(144390000-6000);

	cc1200_radio_start_APRSRX();

	uint8_t toggle=0;
	uint8_t count = 0;

	double E_s1, E_s2;

	while(1)
	{
//		// setup LEDs for RX mode
//		enable_green_led();
//		disable_red_led();

		//cc1200_radio_start_APRSRX();
		if (xSemaphoreTake(xRadioRXSemaphore, 0) == pdTRUE)
		{
			//gpio_set_level(DEBUG_0, 1);
			gpio_intr_disable(CC1120_GPIO2);
			toggle = toggle ^ 1;
			gpio_set_level(DEBUG_0, toggle);


			//data = cc1200_radio_read_CFM();
			window_add((int) EXTERNAL_DATA);

			E_s1 = goertzelFilter(window, 1200, WINDOW_SIZE);
			E_s2 = goertzelFilter(window, 2200, WINDOW_SIZE);
			//printf("%f\n", (E_s1 - E_s2));

			if (E_s1 > E_s2 )
			{

				enable_green_led();
				disable_red_led();
			}
			else
			{
				enable_red_led();
				disable_green_led();
			}

			//gpio_set_level(DEBUG_0, 0);

		}

		//esp_task_wdt_reset();


		// Transmit Queued Packet
		uint8_t *p = (uint8_t *)xRingbufferReceive(radio_tx_buf, &p_size, 0);	// TODO: Modify to something which will check CC1200 status
		if (p != NULL)
		{
			// disable RX mode
			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);

			cc1200_radio_start_APRSRX();
		}
    }
}

//void UART_Task(void *pvParameters)
//{
//	printf("RSSI: %x", data);
//}



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

	// Create Tasks
	xTaskCreatePinnedToCore(Radio_Task, "Radio_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();
}