/*
 * 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 "aprs_decoder.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;
}

int* window_get(void)
{
	return window;
}

int window_get_size(void)
{
	return WINDOW_SIZE;
}

// APRS Decoder


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

// 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 (1)

uint8_t aprs_buf[256];

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

	// algorithm variables
	double E_s1=0, E_s2=0;
	uint8_t raw_bit=0, previous_raw_bit=0;
	int32_t d_pll=0, dpll_error=0, err_div=2;
	int lpf_output = 0;

	aprs_decoder_init();
	frame_buffer_init(aprs_buf, 256);

	// 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)
	{
		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);
			lpf_output += (50000 * (((int)EXTERNAL_DATA) - lpf_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)
			{
				raw_bit = 1;
			}
			else
			{
				raw_bit = 0;
			}

			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:
							(dpll_error + err_div / 2) / err_div;
				}
				else if (dpll_error < (-D_PLL_MARGIN))
				{
					d_pll += get_rx_status() ? D_PLL_MARGIN:
							(dpll_error + err_div / 2) / err_div;
				}
			}

			d_pll += D_PLL_INC;

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

				// 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");
						ESP_LOG_BUFFER_HEXDUMP("APRS RX", aprs_buf, 100, ESP_LOG_INFO);
						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
			}
			previous_raw_bit = raw_bit;
			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_RX2_SETTINGS, sizeof(APRS_RX2_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_isr_register(TIMER_GROUP_0, TIMER_0, rx_timer_isr,
    		(void *) TIMER_0, ESP_INTR_FLAG_IRAM, NULL);

	xTaskCreatePinnedToCore(TX_Task, "TX Task", 1024*4, 0, 2, NULL, 1);
	xTaskCreatePinnedToCore(RX_Task, "RX Task", 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();

	// Setup Kiss Decoder and Encoder
	kiss_init();

	// Initalize BLE
	bt_spp_init();
}