#include "Mezz_send_via_CAN.h"


struct biQuadFilter
{
	float y_0;
	float y_1;
	float y_2;
	float x_0;
	float x_1;
	float x_2;
	float a1;
	float a2;
	float b0;
	float b1;
	float b2;
	//const int16_t maxLen;
	float * buffer;
};
// fs=100Hz 	//	20hz	  // 10Hz
//#define  a1_XL  -0.3695		//-1.143
//#define  a2_XL   0.1958		//0.4128
//#define  b0_XL   0.2066		//0.0675
//#define  b1_XL   0.41314		//0.1349
//#define  b2_XL   0.2066		//0.0675

// fs=100Hz,  fc=10Hz biquad Q=0.707
#define  a1_XL  -1.143
#define  a2_XL   0.4128
#define  b0_XL   0.0675
#define  b1_XL   0.1349
#define  b2_XL   0.0675


uint32_t validity_can_read = 8;
uint8_t  RxData[8];
CAN_RxHeaderTypeDef   RxHeader;

int16_t duty_cycle_Left = 0;
int16_t current_Left = 0;
int32_t erpm_Left = 0;

int16_t duty_cycle_Right = 0;
int16_t current_Right = 0;
int32_t erpm_Right = 0;

// For experimental filtering
float current_y0 = 0;
float current_y1 = 0;
float current_y0_R = 0;
float current_y1_R = 0;
const float alfanky = 0.2;

extern CAN_HandleTypeDef hcan1;

struct biQuadFilter f = {0,0,0,0,0,0,a1_XL,a2_XL,b0_XL,b1_XL,b2_XL};
struct biQuadFilter f_R = {0,0,0,0,0,0,a1_XL,a2_XL,b0_XL,b1_XL,b2_XL};

uint8_t Mezz_read_CAN(int16_t * duty_L, int16_t * curr_L, int32_t * erpm_L, int16_t * duty_R, int16_t * curr_R, int32_t * erpm_R)
{
	*duty_L = duty_cycle_Left;
	*duty_R = duty_cycle_Right;
	*curr_L = current_Left;
	*curr_R = current_Right;
	*erpm_L = erpm_Left;
	*erpm_R = erpm_Right;

return validity_can_read;
}


#ifdef MACRON
/* Function for reading VESC current, ERPM, dutyCycle
 * VESC 0 is Left ESC
 * VESC 1 is Right ESC
 * Suggestions: Optimize this function.
 */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	//HAL_GPIO_TogglePin(GPIOD,LED_RED_Pin);
	if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, RxData) != HAL_OK)
	  Error_Handler();

	  if ((RxHeader.ExtId == 0x900) && (RxHeader.IDE == CAN_ID_EXT) && (RxHeader.DLC == 8))
	  {
		  //HAL_GPIO_TogglePin(GPIOD,LED_RED_Pin);

		  duty_cycle_Left = RxData[6];
		  duty_cycle_Left = (duty_cycle_Left << 8) | RxData[7];

		  current_Left = RxData[4];
		  current_Left = (current_Left << 8) | RxData[5];

		  erpm_Left = RxData[0];
		  erpm_Left = (erpm_Left << 8) | RxData[1];
		  erpm_Left = (erpm_Left << 8) | RxData[2];
		  erpm_Left = (erpm_Left << 8) | RxData[3];

		/*expFil_LP_XL.y_0[var] = (expFil_LP_XL.alfanky * (float_t)dataXLn[var] + ((1.0 - expFil_LP_XL.alfanky) * expFil_LP_XL.y_1[var]));
		  current_y0 = (alfanky * (float)current_Left + ((1.0 - alfanky) * current_y1));
		  current_y1 = current_y0;*/

		f.x_0 = current_Left;
		f.y_0 = f.b0 * f.x_0  +  f.b1 * f.x_1  +  f.b2 * f.x_2  -  f.a1 * f.y_1  -  f.a2 * f.y_2;
		f.x_2 = f.x_1;
		f.x_1 = f.x_0;
		f.y_2 = f.y_1;
		f.y_1 = f.y_0;
		current_y0 = f.y_0;
		current_Left = (int16_t)current_y0;
	  }

	  if ((RxHeader.ExtId == 0x901) && (RxHeader.IDE == CAN_ID_EXT) && (RxHeader.DLC == 8))
	  {
		  //HAL_GPIO_TogglePin(GPIOD,LED_RED_Pin);

		  duty_cycle_Right = RxData[6];
		  duty_cycle_Right = (duty_cycle_Right << 8) | RxData[7];

		  current_Right = RxData[4];
		  current_Right = (current_Right << 8) | RxData[5];

		  erpm_Right = RxData[0];
		  erpm_Right = (erpm_Right << 8) | RxData[1];
		  erpm_Right = (erpm_Right << 8) | RxData[2];
		  erpm_Right = (erpm_Right << 8) | RxData[3];

		/*expFil_LP_XL.y_0[var] = (expFil_LP_XL.alfanky * (float_t)dataXLn[var] + ((1.0 - expFil_LP_XL.alfanky) * expFil_LP_XL.y_1[var]));
		  current_y0 = (alfanky * (float)current_Left + ((1.0 - alfanky) * current_y1));
		  current_y1 = current_y0;*/

  		f_R.x_0 = current_Right;
		f_R.y_0 = f_R.b0 * f_R.x_0  +  f_R.b1 * f_R.x_1  +  f_R.b2 * f_R.x_2  -  f_R.a1 * f_R.y_1  -  f_R.a2 * f_R.y_2;
		f_R.x_2 = f_R.x_1;
		f_R.x_1 = f_R.x_0;
		f_R.y_2 = f_R.y_1;
		f_R.y_1 = f_R.y_0;
		current_y0_R = f_R.y_0;
		current_Right = (int16_t)current_y0_R;
	  }

}
#endif /* MACRO */