/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file    stm32f3xx_it.c
* @brief   Interrupt Service Routines.
******************************************************************************
*
* COPYRIGHT(c) 2019 STMicroelectronics
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*   1. Redistributions of source code must retain the above copyright notice,
*      this list of conditions and the following disclaimer.
*   2. Redistributions in binary form must reproduce the above copyright notice,
*      this list of conditions and the following disclaimer in the documentation
*      and/or other materials provided with the distribution.
*   3. Neither the name of STMicroelectronics nor the names of its contributors
*      may be used to endorse or promote products derived from this software
*      without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "stm32f3xx_it.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "math.h"
#include "User_data_types.h"
#include "User_Variables.h"
#include "User_Constants.h"
#include "User_Macros.h"
#include "User_MotorControl_Functions.h"
#include "VS_SimpleP36.h"
#include "MotorData.h"

#include "pScope.h"
#include "pScope_var_collector.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN TD */

/* USER CODE END TD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/* External variables --------------------------------------------------------*/
extern TIM_HandleTypeDef htim1;
extern DMA_HandleTypeDef hdma_usart2_rx;
extern DMA_HandleTypeDef hdma_usart2_tx;
/* USER CODE BEGIN EV */

/* USER CODE END EV */

/******************************************************************************/
/*           Cortex-M4 Processor Interruption and Exception Handlers          */ 
/******************************************************************************/
/**
  * @brief This function handles Non maskable interrupt.
  */
void NMI_Handler(void)
{
  /* USER CODE BEGIN NonMaskableInt_IRQn 0 */

  /* USER CODE END NonMaskableInt_IRQn 0 */
  /* USER CODE BEGIN NonMaskableInt_IRQn 1 */

  /* USER CODE END NonMaskableInt_IRQn 1 */
}

/**
  * @brief This function handles Hard fault interrupt.
  */
void HardFault_Handler(void)
{
  /* USER CODE BEGIN HardFault_IRQn 0 */

  /* USER CODE END HardFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_HardFault_IRQn 0 */
    /* USER CODE END W1_HardFault_IRQn 0 */
  }
}

/**
  * @brief This function handles Memory management fault.
  */
void MemManage_Handler(void)
{
  /* USER CODE BEGIN MemoryManagement_IRQn 0 */

  /* USER CODE END MemoryManagement_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
    /* USER CODE END W1_MemoryManagement_IRQn 0 */
  }
}

/**
  * @brief This function handles Pre-fetch fault, memory access fault.
  */
void BusFault_Handler(void)
{
  /* USER CODE BEGIN BusFault_IRQn 0 */

  /* USER CODE END BusFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_BusFault_IRQn 0 */
    /* USER CODE END W1_BusFault_IRQn 0 */
  }
}

/**
  * @brief This function handles Undefined instruction or illegal state.
  */
void UsageFault_Handler(void)
{
  /* USER CODE BEGIN UsageFault_IRQn 0 */

  /* USER CODE END UsageFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_UsageFault_IRQn 0 */
    /* USER CODE END W1_UsageFault_IRQn 0 */
  }
}

/**
  * @brief This function handles System service call via SWI instruction.
  */
void SVC_Handler(void)
{
  /* USER CODE BEGIN SVCall_IRQn 0 */

  /* USER CODE END SVCall_IRQn 0 */
  /* USER CODE BEGIN SVCall_IRQn 1 */

  /* USER CODE END SVCall_IRQn 1 */
}

/**
  * @brief This function handles Debug monitor.
  */
void DebugMon_Handler(void)
{
  /* USER CODE BEGIN DebugMonitor_IRQn 0 */

  /* USER CODE END DebugMonitor_IRQn 0 */
  /* USER CODE BEGIN DebugMonitor_IRQn 1 */

  /* USER CODE END DebugMonitor_IRQn 1 */
}

/**
  * @brief This function handles Pendable request for system service.
  */
void PendSV_Handler(void)
{
  /* USER CODE BEGIN PendSV_IRQn 0 */

  /* USER CODE END PendSV_IRQn 0 */
  /* USER CODE BEGIN PendSV_IRQn 1 */

  /* USER CODE END PendSV_IRQn 1 */
}

/**
  * @brief This function handles System tick timer.
  */
void SysTick_Handler(void)
{
  /* USER CODE BEGIN SysTick_IRQn 0 */

  /* USER CODE END SysTick_IRQn 0 */
  HAL_IncTick();
  /* USER CODE BEGIN SysTick_IRQn 1 */

  /* USER CODE END SysTick_IRQn 1 */
}

/******************************************************************************/
/* STM32F3xx Peripheral Interrupt Handlers                                    */
/* Add here the Interrupt Handlers for the used peripherals.                  */
/* For the available peripheral interrupt handler names,                      */
/* please refer to the startup file (startup_stm32f3xx.s).                    */
/******************************************************************************/

/**
  * @brief This function handles DMA1 channel6 global interrupt.
  */
void DMA1_Channel6_IRQHandler(void)
{
  /* USER CODE BEGIN DMA1_Channel6_IRQn 0 */

  /* USER CODE END DMA1_Channel6_IRQn 0 */
  HAL_DMA_IRQHandler(&hdma_usart2_rx);
  /* USER CODE BEGIN DMA1_Channel6_IRQn 1 */

  /* USER CODE END DMA1_Channel6_IRQn 1 */
}

/**
  * @brief This function handles DMA1 channel7 global interrupt.
  */
void DMA1_Channel7_IRQHandler(void)
{
  /* USER CODE BEGIN DMA1_Channel7_IRQn 0 */

  /* USER CODE END DMA1_Channel7_IRQn 0 */
  HAL_DMA_IRQHandler(&hdma_usart2_tx);
  /* USER CODE BEGIN DMA1_Channel7_IRQn 1 */

  /* USER CODE END DMA1_Channel7_IRQn 1 */
}

/**
  * @brief This function handles TIM1 break and TIM15 interrupts.
  */
void TIM1_BRK_TIM15_IRQHandler(void)
{
  /* USER CODE BEGIN TIM1_BRK_TIM15_IRQn 0 */
	State=0;
	pwm_stop=1;
	TIM1->SR&=~(1<<7); //clean break interrupt flag 
  /* USER CODE END TIM1_BRK_TIM15_IRQn 0 */
  HAL_TIM_IRQHandler(&htim1);
  /* USER CODE BEGIN TIM1_BRK_TIM15_IRQn 1 */

  /* USER CODE END TIM1_BRK_TIM15_IRQn 1 */
}
/**
  * @brief This function handles TIM1 update and TIM16 interrupts.
  */
void TIM1_UP_TIM16_IRQHandler(void)
{
  /* USER CODE BEGIN TIM1_UP_TIM16_IRQn 0 */
#ifdef SIM //--- Simulink Code ---//
	
	isabc.a 	= U(0);		// Phase current a
	isabc.b 	= U(1);   // Phase current b
	isabc.c 	= U(2);   // Phase current c
	vdc 			= U(3);   // DC-link voltage
	theta_r		= U(4);	  // Motor position
  n_ref_in  = U(5);		// Mechanical reference speed [rpm]
//T_ref  		= U(5);  	// Torque reference 
  Ctrl_type = U(6);   // Control Type defined on Simulink : 0 -> V/Hz , 1 -> I-Hz , >2 -> FOC
  Reset 		= U(7);		// Black button
  Go  		  = U(8);		// Blue  button 
	
	//------------------------- Lettura Encoder ----------------------------------//
	theta_and_speed_compute_sc(&theta_r, &omega_r, PP, &SinCos_r, &SinCos_r_old, &SinCos_r_elt, &SinCos_r_elt_old);
	cut_off_freq = 100.0f; 
	K_filtro = TWOPI*cut_off_freq*Ts;
	Filter(omega_r, omega_r_filt, K_filtro);
	n_encoder = omega_r * RAD2RPM;
	n_encoder_filt = omega_r_filt * RAD2RPM;
	
#else
	
	/* USER CODE BEGIN TIM1_UP_TIM16_IRQn 0 */
	GPIOC->ODR^=(1<<3);
	IWDG->KR = 0xAAAA;		// reload watchdog TIMER (IDWG RLR)
	
	//--- Lettura Encoder ---//
	theta_encoder = ((float)TIM2->CNT)*ENC_FAC;    																																			// Angle Computation from Encoder
	ReadLut(&THETA_R_LUT[0], theta_encoder, THETA_MAX, THETA_MIN, DTHETA, INV_DTHETA, &theta_r); 												// Real-time Angle Compensation
//ReadLut(&THETA_R_LUT_MATLAB[0], theta_encoder, THETA_MAX_v2, THETA_MIN_v2, DTHETA_v2, INV_DTHETA_v2, &theta_r);     // Offline Angle Compensation 
	theta_and_speed_compute_sc(&theta_r, &omega_r, PP, &SinCos_r, &SinCos_r_old, &SinCos_r_elt, &SinCos_r_elt_old); 		// Speed Computation
	cut_off_freq = 100.0f;                          
	K_filter = TWOPI * cut_off_freq * Ts;
	Filter(omega_r, omega_r_filt, K_filter);  
	n_encoder = omega_r * RAD2RPM;
	n_encoder_filt = omega_r_filt * RAD2RPM;

	//--- Feedback Acquisition ---//
	while(((ADC1->ISR)&(1<<6)) == 0){} 													// Wait end of ADC conversions
	ADC1->ISR|=((1<<6));																				// Clear flag JEOC (the flag JEOC is cleared by setting the correspondent bit to 1)
	input.ch0 = (float)ADC1->JDR1;
	input.ch1 = (float)ADC1->JDR2;
	input.ch2 = (float)ADC1->JDR3;
	isabc.a = -(input.ch0 - offset_current_a) * current_scale; 	// Phase current a
	isabc.b = -(input.ch1 - offset_current_b) * current_scale; 	// Phase current b
	isabc.c = -(input.ch2 - offset_current_c) * current_scale; 	// Phase current c
	vdc 		=  ((int)ADC1->JDR4) * voltage_scale;               // DC-link voltage

	if(((GPIOC->IDR)& (1<<13)) == 0 && Go_flag == 1){						// Read Blue Button State
		Go = 1.0f;																								// Go Button
		Go_flag = 0;
	}else	Go = 0.0f;
	if ((GPIOC->IDR)& (1<<13)) Go_flag = 1;
#endif
	// Over current protection
	CurrentProtection(isabc,&State,&pwm_stop);
	// Clarke transformation (a,b,c) --> (alpha,beta)
  _clarke(isabc, isab);
	// Voltage drop and Deadtime compensation
	VoltageDrop(&TR_par, &D_par, isabc, duty_abc, &v_error_voltage_drop, &enable_voltage_drop);
	DTComp_IM(duty_abc, duty_km1, isabc, vdc, &vsabc, &v_error_deadtime, &enable_deadtime);
	VoltageCompensation(duty_abc, &vsabc, vdc, &v_error, v_error_deadtime, v_error_voltage_drop, enable_voltage_drop, enable_deadtime);
	_clarke(vsabc,vsab);

	switch(State){
		
	case ERROR:
		
		// Variables Initialization
		pwm_stop = 1;
		counter = 0;
		duty_abc.a = 0.0f;
		duty_abc.b = 0.0f;
		duty_abc.c = 0.0f;
		 
		offset_current_a = 2120;
		offset_current_b = 2120;
		offset_current_c = 2120;
		offset_in.ch0 = 0.0f;
		offset_in.ch1 = 0.0f;
		offset_in.ch2 = 0.0f;
		offset_in.ch3 = 0.0f;
		
		SinCos_ref_commissioning.sin = 0.0f;
		SinCos_ref_commissioning.cos = 1.0f;
		SinCos_ref_commissioning_old.sin = 0.0f;
		SinCos_ref_commissioning_old.cos = 1.0f;
		SinCos_ref.sin = 0.0f;
		SinCos_ref.cos = 1.0f;
		SinCos_FOC.sin = 0.0f;
		SinCos_FOC.cos = 1.0f;
		SinCos_IndirectFOC.sin = 0.0f;
		SinCos_IndirectFOC.cos = 1.0f;
		SinCos_VIpole.sin = 0.0f;
		SinCos_VIpole.cos = 1.0f;
		SinCos_VIpole_old.sin = 0.0f;
		SinCos_VIpole_old.cos = 1.0f;
		SinCos_Iw.sin = 0.0f;
		SinCos_Iw.cos = 1.0f;
		SinCos_Itheta.sin = 0.0f;
		SinCos_Itheta.cos = 1.0f;
		SinCos_VI_w.sin = 0.0f;
		SinCos_VI_w.cos = 1.0f;
		SinCos_VI_theta.sin = 0.0f;
		SinCos_VI_theta.cos = 1.0f;
		SinCos_Sensorless.sin = 0.0f;
		SinCos_Sensorless.cos = 1.0f;
		SinCos_desired.sin = 0.0f;
		SinCos_desired.cos = 1.0f;
		
		// Inverter's parameters 
		TR_par.vON1 = vON1_TR;
    TR_par.Rd1 = Rd1_TR;
    TR_par.Iknee = Iknee_TR;
    TR_par.vON2 = vON2_TR;
    TR_par.Rd2 = Rd2_TR;
    TR_par.mdl = TR_mdl;
    D_par.vON1 = vON1_D;
    D_par.Rd1 = Rd1_D;
    D_par.Iknee = Iknee_D;
    D_par.vON2 = vON2_D;
    D_par.Rd2 = Rd2_D;
    D_par.mdl = D_mdl;
		
		// Control Technique:  0 -> V/Hz    1 -> I-HZ    >2 -> FOC  
		Ctrl_type = 0;
		
		flux_nom = LAMBDAS_NOM;  // Rated Stator Flux [Vs] 
		v0 = V0;                 // Phase DC Voltage [V]
 		n_ref_in = 1000.0f;      // Reference Speed [rpm]
		omega_ref_in = 0.0f;
		omega_ref_ramp = 0.0f;
		accel = 1000.0f;	       // Acceleration [rpm/s] 

		isdq_ref.d = ISD;     	 // Magnetizing Current for Rotor Excitation
		g = 10.0f;               // Gain, to be tuned for each FOC control
		
		// Current Loop Parameters
		fbi = 150.0f;               
		wbi = TWOPI*fbi;
		id_par.kp = SIGMALS*wbi;
		id_par.ki = id_par.kp*INV_TAU_EL*Ts;         
		iq_par.kp = SIGMALS*wbi;
		iq_par.ki = iq_par.kp*INV_TAU_EL*Ts;
		
	  // Speed Loop Parameters
		fbw = 5.0f;              
		wbw = TWOPI*fbw;
		sp_par.kp = wbw*Jeq;                        
		sp_par.ki = 0.2f*sp_par.kp*wbw*Ts;   			 
		
		// PLL Parameters Bandwidth suggerita tra 100 e 500 Hz 
		fb_PLL = 100.0f;        
		wb_PLL = TWOPI*fb_PLL;
		damping = 0.7f;                     // damping = 0.7 è una scelta che solitamente garantisce equilibrio tra risposta rapida e stabilità. Il valore ideale dipende dalla dinamica del motore e dalla velocità di risposta desiderata. Se si vuole che il PLL segua velocemente i cambiamenti di velocità angolare, si può ridurre il valore di smorzamento a 0.5 ; se si desidera una risposta più stabile e meno sensibile al rumore si può aumentare a 0.8
		PLL_par.kp = 2.0f*damping*wb_PLL;    
		PLL_par.ki = wb_PLL*wb_PLL*Ts; 
		theta_PLL.cos = 1.0f;
		theta_PLL.sin = 0.0f;
		
		//per compensazione in real time nel COMMISSIONING
		sample_index = 0; 
		read_counter = 0;  // Contatore per gestire la decimazione
	
		if(Go) State = WAKE_UP;	// Wait for Go State
		
		break;
		
	case WAKE_UP:
		
		pwm_stop = 0;				// PWM is ON 
		duty_abc.a = 0.2f;
		duty_abc.b = 0.2f;
		duty_abc.c = 0.2f;

		// Current Offset Accumulation
		if (counter > 100){
			offset_in.ch0 += input.ch0;
			offset_in.ch1 += input.ch1;
			offset_in.ch2 += input.ch2;
		}
		// Offset Computation and Bootsrap Pre-load
		if (counter == (100+400)) {
			offset_current_a = (float)(offset_in.ch0/400.0f);
			offset_current_b = (float)(offset_in.ch1/400.0f);
			offset_current_c = (float)(offset_in.ch2/400.0f);
		}
		// Switch to READY state
		if (counter > 500) {
			counter = 0;
			State = READY;
		}
		counter++;
		
		break;
		
	case READY:
		
		duty_abc.a = 0.5f;
		duty_abc.b = 0.5f;
		duty_abc.c = 0.5f;
		counter = 0;
	
		if (Go)	State = COMMISSIONING;  
		
		break;

	case COMMISSIONING: 

		if(counter < BLANKTIME){
			isdq_ref.q = 0.0f;
			omega_ref_ramp_commissioning = 0.0f;
			Gen_theta_ref(omega_ref_ramp_commissioning, &theta_ref_commissioning, &SinCos_ref_commissioning);  // theta_ref_commissioning = 0 
			counter++;
		}
		else { 
			turns = 5.0f;                         			// 5 Mechanical Turns  
			electrical_turns = PP*turns; 								// 5 Mehcanical Turns = PP*5 Electrical Turns (without considering slip and friction)
			freq = 10.0f; 												 			// Commissioning Speed [Hz]
			revolution_samples = PP/(freq*Ts);      		 
			samples_min = revolution_samples - 100.0f;  
			samples_max = revolution_samples + 100.0f;
			delta = TWOPI*freq*Ts; 				
			ramp(TWOPI*electrical_turns, delta, &theta_ref_commissioning); 
			SinCos_ref_commissioning.sin = sinf(theta_ref_commissioning);
			SinCos_ref_commissioning.cos = cosf(theta_ref_commissioning);
			speed_compute_sc(SinCos_ref_commissioning, &SinCos_ref_commissioning_old, &omega_ref_ramp_commissioning);

			margin = 0.1f;	
			if ((RECORDING_COMPLETE == FALSE) && (sample_index < LUT_SAMPLES)) {  // Encoder Reading
				delta_theta = theta_encoder - theta_encoder_old;
				if (delta_theta < (-margin)) {      					
					if (first_reset_found == FALSE) {
							first_reset_found = TRUE;
					} else if (second_reset_found == FALSE) {
							second_reset_found = TRUE;
							recording = TRUE;												// Angle Data Recording Begins
							sample_index = 0;
					} else if (third_reset_found == FALSE) {
							third_reset_found = TRUE;
							recording = FALSE; 											// Angle Data Recording Ends
							RECORDING_COMPLETE = TRUE;
					}
					sawtooth_increasing = TRUE;                 // Rotation Check
					
				} else if (delta_theta > margin){
						if (first_reset_found == FALSE) {
								first_reset_found = TRUE;
						} else if (second_reset_found == FALSE) {
								second_reset_found = TRUE;
								recording = TRUE;                     // Angle Data Recording Begins
								sample_index = 0; 
						} else if (third_reset_found == FALSE) {
								third_reset_found = TRUE;
								recording = FALSE;                    // Angle Data Recording Ends
								RECORDING_COMPLETE = TRUE;
						}
						sawtooth_increasing = FALSE;              // Rotation Check
					}
				decimation_factor = 1; 												// To reduce samples acquisition if necessary (i.e. commissioning speed < 2Hz or PWM frequency > 5 kHz)
				if (recording == TRUE) {
						read_counter++; 
						if (read_counter >= decimation_factor) {
							theta_encoder_valid[sample_index] = theta_encoder; 	
							sample_index++; 
						}
				}
				theta_encoder_old = theta_encoder;
			}

			if(theta_ref_commissioning == (TWOPI*electrical_turns)) 	
				TURNS_DONE = TRUE; 	
	
			if((TURNS_DONE == TRUE) && (RECORDING_COMPLETE == TRUE)){
				cnt++;
				valid_data_count = sample_index;
				if((valid_data_count < samples_min) || (valid_data_count > samples_max)){   	
					LUT_ERROR = TRUE;
					State = ERROR;
				}
			}

			if ((cnt > 1) && (LUT_GENERATION == FALSE)){
				if (i < valid_data_count){
					if(sawtooth_increasing == TRUE){
						theta_r_ideal[i] = (TWOPI / (1.0f*valid_data_count - 1.0f)) * i;           		// Ideal Sawtooth from 0 to TWOPI 
					} else {
							theta_r_ideal[i] = TWOPI - (TWOPI / (1.0f*valid_data_count - 1.0f)) * i; 		// Ideal Sawtooth from TWOPI to 0 
					}
					i++;
				} 
				if (i == valid_data_count){
					i = 0;		 											
					LUT_GENERATION = TRUE; 			
				}
			}
		
			if((LUT_GENERATION == TRUE) && (LUT_COMPLETE == FALSE)){
				
				if (i < valid_data_count){
					
					xi = theta_r_ideal[i]; 
					
					if (sawtooth_increasing == TRUE) {  // Rotation Check
						
						if (xi < theta_encoder_valid[0]) {  
								// Linear extrapolation below minimum 
								x0 = theta_encoder_valid[0];
								x1 = theta_encoder_valid[1];
								y0 = theta_r_ideal[0];
								y1 = theta_r_ideal[1];
								THETA_R_LUT[i] = y0 + (xi - x0) * (y1 - y0) / (x1 - x0);
							
						} else if (xi > theta_encoder_valid[valid_data_count - 1]) { 
							  // Linear extrapolation above maximum 
								x0 = theta_encoder_valid[valid_data_count - 2];
								x1 = theta_encoder_valid[valid_data_count - 1];
								y0 = theta_r_ideal[valid_data_count - 2];
								y1 = theta_r_ideal[valid_data_count - 1];
								THETA_R_LUT[i] = y0 + (xi - x0) * (y1 - y0) / (x1 - x0);
							
							} else {
									idx = binary_search(theta_encoder_valid, valid_data_count - 1, xi, sawtooth_increasing); // Binary search to find adjacent indexes
									if (idx != -1) {
											// Linear Interpolation
											x0 = theta_encoder_valid[idx];
											x1 = theta_encoder_valid[idx + 1];
											y0 = theta_r_ideal[idx];
											y1 = theta_r_ideal[idx + 1];
											THETA_R_LUT[i] = y0 + (xi - x0) * (y1 - y0) / (x1 - x0);
										
									} else {
											THETA_R_LUT[i] = 0.0f; // Fallback for binary search error
										}
								}
					} else {  // Handle Opposite Rotation 
							
							if (xi > theta_encoder_valid[0]) {
									// Linear extrapolation above maximum 
									x0 = theta_encoder_valid[0];
									x1 = theta_encoder_valid[1];
									y0 = theta_r_ideal[0];
									y1 = theta_r_ideal[1];
									THETA_R_LUT[i] = y0 + (xi - x0) * (y1 - y0) / (x1 - x0);
							} else if (xi < theta_encoder_valid[valid_data_count - 1]) {
									// Linear extrapolation below minimum 
									x0 = theta_encoder_valid[valid_data_count - 2];
									x1 = theta_encoder_valid[valid_data_count - 1];
									y0 = theta_r_ideal[valid_data_count - 2];
									y1 = theta_r_ideal[valid_data_count - 1];
									THETA_R_LUT[i] = y0 + (xi - x0) * (y1 - y0) / (x1 - x0);
							} else { 
									idx = binary_search(theta_encoder_valid, valid_data_count - 1, xi, sawtooth_increasing); // Binary search to find adjacent indexes
									if (idx != -1) {
										  // Linear Interpolation 
											x0 = theta_encoder_valid[idx];
											x1 = theta_encoder_valid[idx + 1];
											y0 = theta_r_ideal[idx];
											y1 = theta_r_ideal[idx + 1];
											THETA_R_LUT[i] = y0 + (xi - x0) * (y1 - y0) / (x1 - x0);
									} else {
											THETA_R_LUT[i] = 0.0f; // Fallback for binary search error
									}
							}
						}
        i++;
				}
				if (i == valid_data_count){
						if (sawtooth_increasing == FALSE){  
								if (j < valid_data_count / 2){
									temp = THETA_R_LUT[j];
									THETA_R_LUT[j] = THETA_R_LUT[valid_data_count - 1 - j];
									THETA_R_LUT[valid_data_count - 1 - j] = temp;
									j++;
								}else 	
									LUT_VECTOR_POSITIVE = TRUE;
						}
						if (sawtooth_increasing == TRUE)		
									LUT_VECTOR_POSITIVE = TRUE;
					}
					if (LUT_VECTOR_POSITIVE == TRUE){ 		// Computation of LUT Parameters
						THETA_MIN = THETA_R_LUT[0];
						THETA_MAX = THETA_R_LUT[valid_data_count - 1];
						DTHETA = (THETA_MAX - THETA_MIN) / (1.0f*valid_data_count - 1.0f);
						INV_DTHETA = 1.0f / DTHETA;
						LUT_COMPLETE = TRUE;
						computational_time = cnt;
					}
		  }		
			// Keep the motor still for 2 seconds after the electrical turns are completed 
			if (cnt == 10000) 	
				State = START; 		// End of COMMISSIONING, Go in START State
		}
		vsdq_ref.d = v0 + flux_nom*(omega_ref_ramp_commissioning);    // V/Hz Control 
		_invrot(vsdq_ref, SinCos_ref_commissioning, vsab_ref);
		_invclarke(vsab_ref, vsabc_ref); 
		PWMduty(vsabc_ref, vdc, &duty_abc);
		
		if(LUT_COMPLETE == TRUE){ 
			duty_abc.a = 0.5f;
			duty_abc.b = 0.5f;
			duty_abc.c = 0.5f;
		}

		break;
		
	case START: 		
		
			speed_request = RAMP;
		//speed_request = SINUSOIDAL;
		//speed_request = TRAPEZOIDAL;
	
			n_ref_in 	= 1000.0f;							 		// Reference Speed [rpm]
			accel 		= 1000.0f;               		// Acceleration [rpm/s]
			omega_ref_in = n_ref_in*RPM2RAD; 		
			HOLD_TIME = 5000; 								 		// 1 Second
	
		switch (speed_request){
						
			case RAMP:	
				
				ramp(omega_ref_in, accel*RPM2RAD*Ts, &omega_ref_ramp);

			break;
			
			case SINUSOIDAL:

				amplitude = omega_ref_in;     			// Sinusoidal Wave Amplitude
				wave_freq = 5.0f;                   // Sinusoidal Wave Frequency
				desired_offset = .0f*omega_ref_in;  // Sinusoidal Wave Offset
				ramp(desired_offset , accel*RPM2RAD*Ts, &wave_offset);
				if(wave_offset < desired_offset) 
					omega_ref_ramp = wave_offset;
				else{
					if((holdCounter < HOLD_TIME) && (desired_offset  > 0.0f)){ 
						omega_ref_ramp = wave_offset;
						holdCounter++; 
					}
					else{
						omega_ref_ramp = wave_offset + amplitude*sinf(wt);
						wt += TWOPI*wave_freq*Ts;
						if(wt >= TWOPI)		wt -= TWOPI;
						if(wt < 0.0f)	    wt += TWOPI;
					}
				}
				
			break;
			
			case TRAPEZOIDAL:
				
				if(rampUpComplete == FALSE){																		 // Ramp Up
					omega_ref_in = n_ref_in*RPM2RAD;
					ramp(omega_ref_in, accel*RPM2RAD*Ts, &omega_ref_ramp);
					if(omega_ref_ramp == omega_ref_in){
						rampUpComplete = TRUE;
						holdCounter = 0;
					}
				}else if((rampUpComplete == TRUE)&&(rampDownComplete == FALSE)){ // Hold 
						if(holdCounter < HOLD_TIME) {
						omega_ref_in = n_ref_in*RPM2RAD;
						omega_ref_ramp = omega_ref_in;
						holdCounter++; 
						}else{ 																											 // Ramp Down 
								omega_ref_in = -n_ref_in * RPM2RAD;
								ramp(omega_ref_in, accel * RPM2RAD * Ts, &omega_ref_ramp);
								if(omega_ref_ramp == omega_ref_in){
									rampDownComplete = TRUE;
									holdCounter2 = 0;
								}
							}
				}else if((rampDownComplete == TRUE)&&(holdCounter2 < HOLD_TIME)){ // Hold
						omega_ref_in = -n_ref_in * RPM2RAD;
						omega_ref_ramp = omega_ref_in;
						holdCounter2++;
					}else{                                                          // To 0  
							omega_ref_in = 0.0f;
							ramp(omega_ref_in, accel*RPM2RAD*Ts, &omega_ref_ramp);
					 }
		}
			
	 
		
		// Speed Request Computation (ramp, sinusoidal, trapezoidal)
		
		n_ref_ramp 	 = omega_ref_ramp * RAD2RPM;
		
		if(counter < BLANKTIME){ 								// Ignore Reference Speed during start-up  
			omega_ref_ramp = 0.0f;
			n_ref_ramp = 0.0f;
			isdq_ref.q = 0.0f;
			wt = 0.0f; 
			wave_offset = 0.0f;
			counter++;
		}	

		Gen_theta_ref(omega_ref_ramp*PP, &theta_ref, &SinCos_ref);   
		
		switch(Ctrl_type) {
			
		case 0: // V/Hz Control 
			
			vsdq_ref.d = v0 + flux_nom*(PP*omega_ref_ramp);     
			
			_invrot(vsdq_ref, SinCos_ref, vsab_ref);       
		
		break;
				
		case 1:	// I-Hz Control
		
			isdq_ref.d = ISD;    				 
			isdq_ref.q = 0.0f;               
			_rot(isab, SinCos_ref, isdq);
		
			// Feed-forward for Dynamic Decoupling
		  vsdq_ref_ffw.d = 1.0f*(RS*isdq_ref.d - PP*omega_r_filt*SIGMALS*isdq_ref.q);
		  vsdq_ref_ffw.q = 1.0f*(RS*isdq_ref.q + PP*omega_r_filt*(SIGMALS*isdq_ref.d + Kr*LAMBDAR_NOM));
		
			CurrentLoop_d_priority(vdc, CRT_MAX, &isdq_ref, isdq, &id_par, &iq_par, &id_var, &iq_var, vsdq_ref_ffw, &vsdq_ref); 
			
			_invrot(vsdq_ref, SinCos_ref, vsab_ref);
			
			// Flux Estimators and Observers
			g = 10.0f;
		
			VI_pole_flux_estimator(&lambdaS_VIpole, &lambdaR_VIpole , &SinCos_VIpole, vsab, isab, g);
			VI_theta_flux_observer(&lambdaR_Itheta_rotor, &lambdaR_Itheta, isab, &isab_rotor, SinCos_r_elt, SinCos_r_elt_old, &SinCos_Itheta, vsab, &lambdaS_VIpole_theta, &lambdaR_VI_theta, &SinCos_VI_theta, g);
			VI_omega_flux_observer(&lambdaR_Iw_old, &lambdaR_Iw, &SinCos_Iw, vsab, isab, PP, omega_r_filt, &lambdaS_VIpole_w, &lambdaR_VI_w, &SinCos_VI_w, g);
			
			Sensorless_flux_observer(vsab, isab, isdq_ref, &isab_est, &esab_est, &lambdaS_Sensorless, &lambdaR_Sensorless, &SinCos_Sensorless, PP, &omega_slip_est, &omega_elt_est, &omega_r_est, g);
			cut_off_freq_Sensorless = 20.0f; 
			K_filtro_Sensorless = TWOPI*cut_off_freq_Sensorless*Ts;
	    Filter(omega_r_est, omega_r_est_filt, K_filtro_Sensorless);
			
		break;
			
		case 2: // Indirect FOC			
		
			// Speed Regulator
			Tlim = T_MAX;									// Max Torque Reference
			sp_par.lim = Tlim;
			sp_var.ref = omega_ref_ramp;
			sp_var.fbk = omega_r_filt;
			PIReg(&sp_par, &sp_var);
			T_ref = sp_var.out;   
		
			//T_ref = 0.05f;      				// Bypass the Speed Regulator
		
			isdq_ref.d = ISD;             // Motor Excitation
			isdq_ref.q = INV_KT * T_ref;  // T = 3/2 * PP * Kr * LambdaR,nom * is,q  = KT * is,q 
			if(counter<BLANKTIME)
				isdq_ref.q = 0.0f;
			
			if(fabs(isdq_ref.d) > 0.0f)   
				theta_slip += Ts * INV_TAUR * isdq_ref.q / isdq_ref.d;
			
			if(theta_slip >= TWOPI)	theta_slip -= TWOPI;
			if(theta_slip < 0.0f)	  theta_slip += TWOPI;
			
			theta_IndirectFOC = PP*theta_r + theta_slip;
			while(theta_IndirectFOC >= TWOPI)		theta_IndirectFOC -= TWOPI; 
			while(theta_IndirectFOC < 0.0f)	  	theta_IndirectFOC += TWOPI; 

			SinCos_IndirectFOC.sin = sinf(theta_IndirectFOC);
			SinCos_IndirectFOC.cos = cosf(theta_IndirectFOC);
			_rot(isab, SinCos_IndirectFOC, isdq);
			
			// Feed-forward for Dynamic Decoupling
		  vsdq_ref_ffw.d = 1.0f*(RS*isdq_ref.d - PP*omega_r_filt*SIGMALS*isdq_ref.q);
		  vsdq_ref_ffw.q = 1.0f*(RS*isdq_ref.q + PP*omega_r_filt*(SIGMALS*isdq_ref.d + Kr*LAMBDAR_NOM));
		
		  CurrentLoop_d_priority(vdc, CRT_MAX, &isdq_ref, isdq, &id_par, &iq_par, &id_var, &iq_var, vsdq_ref_ffw, &vsdq_ref); 
      
			_invrot(vsdq_ref, SinCos_IndirectFOC, vsab_ref);		
			
		break;
		
		case 3: // Direct FOC 
			
			Iw_flux_estimator(&lambdaR_old, &lambdaR, &SinCos_FOC, isab, PP, omega_r_filt);
			Itheta_flux_estimator(&lambdaR_rotor, &lambdaR, isab, &isab_rotor, SinCos_r_elt , SinCos_r_elt_old, &SinCos_FOC);
			g = 5.0f;
			VI_pole_flux_estimator(&lambdaS, &lambdaR, &SinCos_FOC, vsab_ref, isab, g);
			VI_theta_flux_observer(&lambdaR_rotor, &lambdaR_Itheta, isab, &isab_rotor, SinCos_r_elt, SinCos_r_elt_old, &SinCos_Itheta, vsab_ref, &lambdaS, &lambdaR, &SinCos_FOC, g);
			VI_omega_flux_observer(&lambdaR_Iw_old, &lambdaR_Iw, &SinCos_Iw, vsab_ref, isab, PP, omega_r_filt, &lambdaS, &lambdaR, &SinCos_FOC, g);
			Sensorless_flux_observer(vsab_ref, isab, isdq_ref, &isab_est, &esab_est, &lambdaS, &lambdaR, &SinCos_FOC, PP, &omega_slip_est, &omega_elt_est, &omega_r_est, g);
			cut_off_freq_Sensorless = 20.0f; 
			K_filter_Sensorless = TWOPI*cut_off_freq_Sensorless*Ts;
	    Filter(omega_r_est, omega_r_est_filt, K_filter_Sensorless);

			// Speed Regulator
			Tlim = T_MAX;										// Max Torque Reference
			sp_par.lim = Tlim;
			sp_var.ref = omega_ref_ramp;
			sp_var.fbk = omega_r_est_filt;  // Sensorless
			sp_var.fbk = omega_r_filt;
			PIReg(&sp_par, &sp_var);
			T_ref = sp_var.out;
			
			isdq_ref.q = INV_KT * T_ref;
			if(counter<BLANKTIME){
				SinCos_FOC.cos = 1.0f;
				SinCos_FOC.sin = 0.0f;
				isdq_ref.q = 0.0f;
			}
			_rot(isab, SinCos_FOC, isdq);
		
			// Feed-forward for Dynamic Decoupling
			vsdq_ref_ffw.d = 1.0f*(RS*isdq_ref.d - PP*omega_r_filt*SIGMALS*isdq_ref.q);
			vsdq_ref_ffw.q = 1.0f*(RS*isdq_ref.q + PP*omega_r_filt*(SIGMALS*isdq_ref.d + Kr*LAMBDAR_NOM));
					
			CurrentLoop_d_priority(vdc, CRT_MAX, &isdq_ref, isdq, &id_par, &iq_par, &id_var, &iq_var, vsdq_ref_ffw, &vsdq_ref); 
			
		  _invrot(vsdq_ref, SinCos_FOC, vsab_ref); 
			
		}
	  _invclarke(vsab_ref, vsabc_ref);
		PWMduty(vsabc_ref, vdc, &duty_abc);	
	}
	
	//duty cycles saturation
	if (duty_abc.a > 0.95f) duty_abc.a = 0.95f;
	if (duty_abc.b > 0.95f) duty_abc.b = 0.95f;
	if (duty_abc.c > 0.95f) duty_abc.c = 0.95f;
	
	if (duty_abc.a < 0.05f) duty_abc.a = 0.05f;
	if (duty_abc.b < 0.05f) duty_abc.b = 0.05f;
	if (duty_abc.c < 0.05f) duty_abc.c = 0.05f;
	
	if(pwm_stop){
		duty_abc.a = 0.0f;
		duty_abc.b = 0.0f;
		duty_abc.c = 0.0f;
	}
	
#ifndef SIM
	TIM1->CCR1 = TIM1_cycle*duty_abc.a;
	TIM1->CCR2 = TIM1_cycle*duty_abc.b;
	TIM1->CCR3 = TIM1_cycle*duty_abc.c;
	
	//Virtual Scope

	//	VS_simpleP36_3f(isabc.a,isabc.b,isabc.c);
	//	VS_simpleP36_3f(duty_abc.a,duty_abc.b,duty_abc.c);
	//	VS_simpleP36_2f(isab.alpha, isab.beta);
	//	VS_simpleP36_2f(vsab_ref.alpha,vsab_ref.beta);
	//  VS_simpleP36_3f(vsabc_ref.a,vsabc_ref.b,vsabc_ref.c);
	//  VS_simpleP36_3f(isab.alpha,isab.beta,theta_ref);
	//	VS_simpleP36_2f(duty_abc.a,isabc.a);
	//	VS_simpleP36_2f(isab.alpha,isab.beta);
	//  VS_simpleP36_2f(SinCos_r.sin,SinCos_r_elt.sin);
	//  VS_simpleP36_2f(SinCos_ref.sin,SinCos_r_elt.sin);
	//  VS_simpleP36_3f(theta_r, omega_r,omega_r_filt);
	//  VS_simpleP36_2f(omega_r,omega_r_filt);
	//  VS_simpleP36_3f(n_ref_in, n_encoder, n_encoder_filt);
  //  VS_simpleP36_1f(theta_r);
  //  VS_simpleP36_3f(lambdaR_VIpole.amp, lambdaR_VIpole.alpha, lambdaR_VIpole.beta);
  //  VS_simpleP36_3f(lambdaR_Iw.amp, lambdaR_Iw.alpha, lambdaR_Iw.beta);
  //  VS_simpleP36_3f(lambdaR_Itheta.amp, lambdaR_Itheta.alpha, lambdaR_Itheta.beta);
  //  VS_simpleP36_3f(lambdaR_VI_w.amp, lambdaR_VI_w.alpha, lambdaR_VI_w.beta);
	//  VS_simpleP36_3f(lambdaR_VI_theta.amp, lambdaR_VI_theta.alpha, lambdaR_VI_theta.beta);
  //  VS_simpleP36_4f(lambdaR_VIpole.amp,lambdaR_Itheta.amp, lambdaR_VI_w.amp, lambdaR_VI_theta.amp);
		
					
	// Utilizzando applicazione pScope su MATLAB, variabili da esportare sono definite nel file trace.h 
	pScope_task();
		

#endif
  /* USER CODE END TIM1_UP_TIM16_IRQn 0 */
  HAL_TIM_IRQHandler(&htim1);
  /* USER CODE BEGIN TIM1_UP_TIM16_IRQn 1 */

  /* USER CODE END TIM1_UP_TIM16_IRQn 1 */
}

/**
  * @brief This function handles EXTI line[15:10] interrupts.
  */
void EXTI15_10_IRQHandler(void)
{
  /* USER CODE BEGIN EXTI15_10_IRQn 0 */
	//if PB10 input is 1 reset the counter (encoder Z signal)
	if(GPIOB->IDR & (1<<10)){
	TIM2->CNT=0;
	}
  /* USER CODE END EXTI15_10_IRQn 0 */
  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_10);
  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_13);
  /* USER CODE BEGIN EXTI15_10_IRQn 1 */

  /* USER CODE END EXTI15_10_IRQn 1 */
}

/* USER CODE BEGIN 1 */

/* USER CODE END 1 */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
