/*
 ===============================================================================
 Name        : main.c
 Author      : $(author)
 Version     :
 Copyright   : $(copyright)
 Description : main definition
 ===============================================================================
 */

#include <cr_section_macros.h>
#include <stdio.h>
#include <string.h>
#include "board.h"
#include "scheduler.h"
#include "gram_usb_cdc_api.h"
#include "gram_pwm_adc_sampling.h"
#include "gram_beep.h"
#include "gram_buffer.h"
#include "gram_aux.h"
#include "stopwatch.h"
#include "tictoc.h"
#include "arm_math.h"

#define INDEX_RE(i)		(2*i)
#define INDEX_IM(i)		(2*i+1)

#define TICKRATE_HZ		(100)
#define ADC_PIEZO_DC_VAL	(2048)
#define BEEP_NOTIFY	beep_pulse(40, 6000, 30)
#define FFT_LEN			(256)

#define MOTOR_POLES_N	(7)
/*****************************************************************************
 * Private types/enumerations/variables
 ****************************************************************************/



/*****************************************************************************
 * Public types/enumerations/variables
 ****************************************************************************/

volatile int USB_int_count = 0;
volatile int USB_tick_count = 0;
uint8_t buf_str[128];

//int16_t FFT_DataIn[FFT_LEN];
//int16_t FFT_DataOut[FFT_LEN];
int16_t FFT_Buffer[FFT_LEN*2];
int16_t FFT_Magnitude[FFT_LEN];
buffer_lin_t FFT_PiezoBuffer = {
		.buffer_size = FFT_LEN,
};

uint8_t SendSampledData = 0;
uint8_t SendFFT_PiezoData = 0;
uint8_t SendFFT_CurrentData = 0;

/*****************************************************************************
 * Private functions
 ****************************************************************************/

/*****************************************************************************
 * Public functions
 ****************************************************************************/


void SysTick_Handler(void) {
	TickCounter++;
}

inline void USB_task(void) {
	//sprintf(str_buf, "USB tick count per second is %d\r\n\0", USB_tick_count);
	//usb_printstr(str_buf);
	//USB_int_count = 0;
	//USB_tick_count = 0;
}

void task_1s(void) {
	sprintf((char *) buf_str, (char *) "USB tick count per second is %d\r\n\0",
			USB_tick_count);
	usb_printstr(buf_str);
}

inline void USB_SendDataTask(void) {
	uint16_t DataCount;
	if (SendSampledData) {
		static uint8_t cmd_report = 1;
		if (cmd_report) {
			cmd_report = 0;
			USB_SPP_SendCmdRxData(SPP_type_uint16_t,
					ADC_Motor_Piezo_Buffer.buffer_size, 5, 31);
			USB_SPP_SendCmdRxData(SPP_type_uint16_t,
					ADC_Motor_Current_Buffer.buffer_size, 6, 30);
		}

		DataCount = BufferLinGetDataCount(&ADC_Motor_Piezo_Buffer);
		if (DataCount) {
			if (DataCount > 40) {
				DataCount = 40;
			}
			if (USB_SPP_send_data16(5,
					&(ADC_Motor_Piezo_Buffer.p_data[ADC_Motor_Piezo_Buffer.data_start_ind]),
					DataCount)) {
				BufferLinDataDeque(&ADC_Motor_Piezo_Buffer, DataCount);
			}
		}
		DataCount = BufferLinGetDataCount(&ADC_Motor_Current_Buffer);
		if (DataCount) {
			if (DataCount > 40) {
				DataCount = 40;
			}
			if (USB_SPP_send_data16(6,
					&(ADC_Motor_Current_Buffer.p_data[ADC_Motor_Current_Buffer.data_start_ind]),
					DataCount)) {
				BufferLinDataDeque(&ADC_Motor_Current_Buffer, DataCount);
			}
		}
		if ( 	!BufferLinGetDataCount(&ADC_Motor_Piezo_Buffer)
				&& !BufferLinGetDataCount(&ADC_Motor_Current_Buffer)){
			SendSampledData = 0;	// All data was send
			cmd_report = 1;			// Next time, start with a command packet
		}
	}

	if (SendFFT_PiezoData) {
		static uint8_t cmd_report = 1;
		if (cmd_report) {		//todo: verify command packet was sent
			if (USB_SPP_SendCmdRxData(SPP_type_uint16_t, FFT_PiezoBuffer.buffer_size, 7, 32)){
				cmd_report = 0;
			}
		}

		DataCount = BufferLinGetDataCount(&FFT_PiezoBuffer);
		if (DataCount) {
			if (DataCount > 40) {
				DataCount = 40;
			}
			if (USB_SPP_send_data16(7,
					&(FFT_PiezoBuffer.p_data[FFT_PiezoBuffer.data_start_ind]),
					DataCount)) {
				BufferLinDataDeque(&FFT_PiezoBuffer, DataCount);
			}
		}
		else{
			SendFFT_PiezoData = 0;	// All data was send
			cmd_report = 1;			// Next time, start with a command packet
		}
	}
}

void AnalyzeMotorSamples(void) {
	arm_cfft_radix4_instance_q15 CFFT_InstQ15;
	arm_status FFT_Status;
	uint16_t i;
	//todo: unite buffer size constants

	Tic(1);

	//*********************************************************
	// Determine motor state and rate of rotation
	//*********************************************************
	// Align ADC result to 1.15 format and copy to complex vector
	for (i=0; i<ADC_Motor_Current_Buffer.buffer_size; i++){
		FFT_Buffer[ INDEX_RE(i) ] = ( (int16_t) (ADC_Motor_Current_Buffer.p_data[i]<<3) );
		FFT_Buffer[ INDEX_IM(i) ] = 0;
	}
	FFT_Status = arm_cfft_radix4_init_q15(&CFFT_InstQ15, FFT_LEN,	0, 1 );
	if (FFT_Status != ARM_MATH_SUCCESS){
		halt();
	}
	arm_cfft_radix4_q15( &CFFT_InstQ15,	FFT_Buffer);
	//scaling
	for (i=0; i<FFT_LEN*2; i++){
		FFT_Buffer[i] = FFT_Buffer[i]<<3;	//todo: solve upscaling
	}
	arm_cmplx_mag_squared_q15( 	FFT_Buffer, FFT_Magnitude, FFT_LEN*2);

	int16_t MagMax;
	uint32_t MagMaxIndex;
	arm_max_q15 (FFT_Buffer, FFT_LEN, &MagMax, &MagMaxIndex);
	uint16_t MotorSpeed = (MagMaxIndex * ADC_PIEZO_SAMPLING_FREQ) / FFT_LEN / MOTOR_POLES_N;


	//*********************************************************
	// Make FFT of piezo vibration sensor
	//*********************************************************
	// Remove virtual ground from samples and copy them into complex vector
	for (i=0; i<ADC_Motor_Piezo_Buffer.buffer_size; i++){
		FFT_Buffer[ INDEX_RE(i) ] = ( (int16_t) (ADC_Motor_Piezo_Buffer.p_data[i]<<3) ) - (ADC_PIEZO_DC_VAL<<3);
		FFT_Buffer[ INDEX_IM(i) ] = 0;
	}
	// Make FFT of piezo samples
	FFT_Status = arm_cfft_radix4_init_q15(&CFFT_InstQ15, FFT_LEN,	0, 1 );
	if (FFT_Status != ARM_MATH_SUCCESS){
		halt();
	}
	arm_cfft_radix4_q15( &CFFT_InstQ15,	FFT_Buffer);
	//scaling
	for (i=0; i<FFT_LEN*2; i++){
		FFT_Buffer[i] = FFT_Buffer[i]<<3;	//todo: solve upscaling
	}
	//arm_cmplx_mag_q15( 	FFT_Buffer, FFT_Magnitude, FFT_LEN*2);
	arm_cmplx_mag_squared_q15( 	FFT_Buffer, FFT_Magnitude, FFT_LEN*2);
	// prepare buffer structure for usb data transmission
	BufferLinSetFull(&FFT_PiezoBuffer);
	FFT_PiezoBuffer.p_data = (uint16_t *) FFT_Magnitude;
	SendFFT_PiezoData = 1;

	//*********************************************************
	// Determine magnitude of component at motor rotation frequency
	//*********************************************************
	// Test for imbalance condition

	// Add current data to statistic

	USB_tick_count = Toc(1);
}

 int main(void) {
	// Read clock settings and update SystemCoreClock variable
	SystemCoreClockUpdate();
	// Initialize GPIO, Buttons, LEDs...
	Board_Init();
	// Initizlize USB stack
	usb_init();
	// Initialize stopwatch module
	TicTocInit(LPC_TIMER3);
	// Initialize systick
	SysTick_Config(SystemCoreClock / TICKRATE_HZ);
	// Initialize beeper (PWM)
	beep_init(10);
	// initialize motor PWM, and sampling = ADC, TMR0, TMR1
	PWM_SamplingInit();
	MotorPWM_DutySet(50);

	// Scheduler counters
	uint16_t timer_led_a;
	ResetDelay(&timer_led_a);
	uint16_t timer_1s_task;
	ResetDelay(&timer_1s_task);
	uint16_t timer_beep;
	ResetDelay(&timer_beep);

	//BEEP_NOTIFY;

	Chip_GPIO_SetPinDIR(LPC_GPIO, 0, 0, 1);
	Chip_GPIO_SetPinDIR(LPC_GPIO, 0, 1, 1);

	SamplingStart();

	while (1) {
		// LED blinky task
		if (HasDelayElapsed(50, &timer_led_a, true)) {
			Board_LED_Toggle(LEDS_LED_A);
		}

		// 1s periodic task for whatever
		if (HasDelayElapsed(100, &timer_1s_task, true)) {
			task_1s();
		}

		// beeper task - 10ms
		if (HasDelayElapsed(1, &timer_beep, true)) {
			beep_task();
		}

		USB_SPP_task();

		USB_SendDataTask();

		if (SamplingIsDone()){
			SendSampledData = 1;
			AnalyzeMotorSamples();

		}

	}
	return 0;
}
