#include "stm32f3xx_hal.h"
#include <math.h>
#include "globals.h"
#include "commands.h"


void startRxCmdFlux(void)
{
	
}

extern ADC_HandleTypeDef hadc1;

void measure_pots(uint16_t *raw_pos, uint16_t *raw_speed)
{
	HAL_ADC_Start(&hadc1);
	if (HAL_ADC_PollForConversion(&hadc1,5)==HAL_OK)
	{
		*raw_pos = hadc1.Instance->DR;
	}
	else
	{
		crash(CC_DAC1_CONV1_FAILED);
	}
	if (HAL_ADC_PollForConversion(&hadc1,5)==HAL_OK)
	{
		*raw_speed = hadc1.Instance->DR;
	}	
	else
	{
		crash(CC_DAC1_CONV2_FAILED);
	}
}
// Look at 2 pots and one switch
// POT 1 = POS
// POT 2 = SPEED
// switch = toggle mode
// values are low pass filtered over 10 measures
// in steps of 1/100

#define FILTER_WEIGHT 0.9f
#define FILTER_WEIGHT_BUT 0.7f
#define MIN_STEP (1.0f/100)

extern 	uint32_t currentMode;

uint8_t commandRx(int *cmd_id,void *vData, float real_time)
{
	static float pos_pot = 0, last_sent_pos_pot = -1;
	static float speed_pot = 0, last_sent_speed_pot = -1;
	static float button = 1.0f, last_button = 1.0f;
	uint32_t mode;
	static float time_of_press = 0;
	
	uint16_t raw_pos_pot, raw_speed_pot, raw_button;
/*		
	pos_pot = 0.5;
	speed_pot = 0.5;
		*((float *) vData) = pos_pot;
		*cmd_id = CMD_POS;	
	return 0;
*/
	if (HAL_GPIO_ReadPin(BOUTON_POUSSOIR_GPIO_Port,BOUTON_POUSSOIR_Pin)==GPIO_PIN_SET)
	{
		raw_button = 1;
	}
	else
	{
		raw_button = 0;
	}

	button   = FILTER_WEIGHT_BUT*button   + (1.0f-FILTER_WEIGHT_BUT)*raw_button  ;
	
	if (fabs(button-last_button)>0.8f)
	{
		if (button>0.5f)
			button = 1;
		else
			button = 0;

		extern uint32_t currentMode;
		
		last_button = button;
		if (button == 0) // appui
		{
			if (currentMode==21)
				HAL_NVIC_SystemReset();
			mode = (currentMode+1)%21;
			if (mode==0)
				mode = 1;
			*((uint32_t *) vData) = mode;
			*cmd_id = CMD_MOD;
			time_of_press = real_time;
			return 1;
		}
	}
	// if pressed for 2 sec, go to pseudo mod off
	if (raw_button == 0) // appui
	{
		if (real_time-time_of_press>2)
		{
//			*((uint32_t *) vData) = 21; // black
			*cmd_id = CMD_OFF; //CMD_MOD;
			time_of_press = real_time+100; // so that we don't send the command again
			return 1;
		}
	}
	else
	{
		time_of_press = real_time;
	}
	
  measure_pots(&raw_pos_pot, &raw_speed_pot);	

	pos_pot   = FILTER_WEIGHT*pos_pot   + (1.0f-FILTER_WEIGHT)*raw_pos_pot  /(1<<12);
	speed_pot = FILTER_WEIGHT*speed_pot + (1.0f-FILTER_WEIGHT)*raw_speed_pot/(1<<12);
	
	if (fabs(pos_pot-last_sent_pos_pot)>MIN_STEP)
	{
		last_sent_pos_pot = pos_pot;
//		*((float *) vData) = pos_pot;
//		*cmd_id = CMD_POS;
		*((uint32_t *) vData) = pos_pot*255;
		*cmd_id = CMD_INT;
		return 1;
	}

	if (fabs(speed_pot-last_sent_speed_pot)>MIN_STEP)
	{
		last_sent_speed_pot = speed_pot;
		*((float *) vData) = speed_pot;
		*cmd_id = CMD_SPD;
		return 1;
	}
	
	return 0;
}