#include "main.h"
#pragma once

#define DEF_MAX_CURRENT	(1.00f)

typedef enum {

	MOTOR_STATE_IDLE	 	   	 	= 0,

	MOTOR_STATE_GOTO_ZERO		 	= 1,
	MOTOR_STATE_GOTO_ZERO_FINISH  	= 2,

	MOTOR_STATE_STABILIZATION 		= 3,

	MOTOR_STATE_ERROR				= 4,

	MOTOR_STATE_TURN_OFF			= 5,
	MOTOR_STATE_TURN_OFF_FINISH  	= 6,

	MOTOR_STATE_EMERGENCY_BRAKE  	= 7,

	MOTOR_STATE_FLUX_ALIGN			=10,

	MOTOR_STATE_TEST_PID	 		=20,

	MOTOR_STATE_TEST_MOTION	 		=30,

} ENUM_MOTOR_STATES;

#pragma pack(push,1)

typedef struct {
	uint32_t u32CpuID;
	float f32TetaCorrection;
	float f32MinAngle_deg;
	float f32MaxAngle_deg;
	float f32IcogCompCoef;
	float f32IaccelCompFactor;
	float f32PositionZero_deg;
	float f32PidPositionKd;
	float f32PidPositionKi;
	float f32PidPositionKp;
	float f32PidPositionTfilt_d;
	float f32UpRateZero;
	float f32DownRateZero;
	float f32UpRateStab;
	float f32DownRateStab;
} TPARAMS_USED_BY_SERVO;

#pragma pack(pop)


extern TPARAMS_USED_BY_SERVO stUsedParameters;


extern uint32_t CPU_ID;
/*TEST pins define section*/
#define TESTPIN_ENABLE 			 0
#define TESTPIN_UART_ISR_ENABLE (1)
//#define TEST_UART 0
#define FSAMPLE_FAST (10000)
#define TSAMPLE_FAST (1.0f/FSAMPLE_FAST)
#define FSAMPLE_SLOW (1000)
#define	TSAMPLE_SLOW (1.0f/FSAMPLE_SLOW)
static inline float ISR_TIME_CALC(float start,float stop)
{
	static float systemTimeStep = -1.f;
	if(systemTimeStep <0.f){
		systemTimeStep=1.f/SystemCoreClock;
		return 0;
	}
	if (start > stop)
		    {
		  	  return  ((start- stop)*(systemTimeStep));
		    }
		    else
		    {
		    	uint32_t counter = ((0xFFFFFF - stop) + start) + 1;
		    	return  ( (counter)*(systemTimeStep));
		    }
}

static inline uint16_t GET_PWM_Period(uint16_t ID)
{
	return((uint16_t)LL_TIM_GetAutoReload(TIM1));
}
static inline void PWM_OUT_ENABLE(uint16_t ID)
{
	  LL_GPIO_SetOutputPin(EN_U_GPIO_Port,EN_U_Pin);
	  LL_GPIO_SetOutputPin(EN_W_GPIO_Port,EN_W_Pin);
	  LL_GPIO_SetOutputPin(EN_V_GPIO_Port,EN_V_Pin);
	  LL_TIM_EnableAllOutputs(TIM1);
}
static inline void PWM_OUT_DISABLE(uint16_t ID)
{
	  LL_GPIO_ResetOutputPin(EN_U_GPIO_Port,EN_U_Pin);
	  LL_GPIO_ResetOutputPin(EN_W_GPIO_Port,EN_W_Pin);
	  LL_GPIO_ResetOutputPin(EN_V_GPIO_Port,EN_V_Pin);
	  LL_TIM_DisableAllOutputs(TIM1);
}
static inline void PWM_COMPARE_SET(uint16_t ID,uint16_t A,uint16_t B,uint16_t C)
{
	LL_TIM_OC_SetCompareCH1(TIM1, A);
	LL_TIM_OC_SetCompareCH2(TIM1, B);
	LL_TIM_OC_SetCompareCH3(TIM1, C);

}
static uint16_t hw_err_state = 0;
static inline uint16_t GET_HWfault_state(uint16_t ID)
{
	if(hw_err_state) LL_TIM_DisableIT_BRK(TIM1);
	return hw_err_state;
}
static inline void CLEAR_HWfault(uint16_t ID)
{
	hw_err_state = 0;
	LL_TIM_EnableIT_BRK(TIM1);
}


static inline uint32_t GET_ADC_data(uint16_t ID,uint16_t *ADC_array,uint16_t *error_code)
{
	*error_code = 0;
	uint32_t adc_wait1 = 0;
	while((!LL_ADC_IsActiveFlag_JEOS(ADC1))){
		if(adc_wait1++ > 1000) return (*error_code = 0x1);
	};

	/*Channel 1 ADC 1- Current A*/
	ADC_array[0] = LL_ADC_INJ_ReadConversionData12(ADC1,LL_ADC_INJ_RANK_1);
	/*Channel 2 ADC 1- Current B*/
	ADC_array[2] = LL_ADC_INJ_ReadConversionData12(ADC1,LL_ADC_INJ_RANK_2);
	/*Channel 4 ADC 1 - Vdc bs*/
	ADC_array[5] = LL_ADC_INJ_ReadConversionData12(ADC1,LL_ADC_INJ_RANK_4);
	/*Clear flags*/
	LL_ADC_ClearFlag_JEOS(ADC1);
	return (0x0);

}
static inline uint16_t GET_ENCODER_counter(uint16_t ID)
{
	return ( LL_TIM_GetCounter(TIM3));
}
static inline void SET_ENCODER_counter(uint16_t ID,uint16_t value)
{
	LL_TIM_SetCounter(TIM3, value);
}