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

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


#define TICKRATE_HZ		(100)
#define ADC_PIEZO_DC_VAL	(2048)
#define BEEP_NOTIFY	beep_pulse(40, 6000, 30)
#define FFT_LEN			(512)
/*****************************************************************************
 * 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];

uint8_t SendSampledData = 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) {
	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);
		}

		uint16_t 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
		}
	}
}

void AnalyzeMotorSamples(void) {
	// Remove DC value from samples
	uint16_t i;
	//todo: unite buffer size constants

	Tic(1);

	for (i=0; i<ADC_Motor_Piezo_Buffer.buffer_size; i++){
		ADC_Motor_Current_Buffer.p_data[i] = ADC_Motor_Piezo_Buffer.p_data[i];
	}

	for (i=0; i<ADC_Motor_Piezo_Buffer.buffer_size; i++){
		FFT_DataIn[i] = ( (int16_t) ((ADC_Motor_Piezo_Buffer.p_data[i])<<4) ) - (ADC_PIEZO_DC_VAL<<4);
	}
	// Make FFT of motor current
	arm_rfft_instance_q15 RFFT_InstQ15;
	arm_cfft_radix4_instance_q15 CFFT_InstQ15;
	arm_status FFT_Status;

	FFT_Status =  arm_rfft_init_q15 	(
			&RFFT_InstQ15,
			&CFFT_InstQ15,
			512,
			1,
			1
	);
	if (FFT_Status != ARM_MATH_SUCCESS){
		halt();
	}
	arm_rfft_q15(&RFFT_InstQ15,	FFT_DataIn,	FFT_DataOut);

	for (i=0; i<ADC_Motor_Piezo_Buffer.buffer_size; i++){
		ADC_Motor_Piezo_Buffer.p_data[i] = FFT_DataOut[i];
	}

	SendSampledData = 1;

	volatile uint32_t ticks_us = Toc(1);
	nop();
	// Determine motor state and rate of rotation

	// Make FFT of piezo vibration sensor
	// Determine magnitude of component at motor rotation frequency
	// Test for imbalance condition

	// Add current data to statistic
}

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

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

	int8_t xxx = -2;

	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()){
			AnalyzeMotorSamples();
		}

	}
	return 0;
}
