#include "foc/svpwm.h"
#include "math/fast_math.h"

void svpwm(alpha_beta_t *alpha_beta, float vbus, uint32_t PWM_half_period, phase_time_t *phase_out, u8 *sector_out){
	float alpha = alpha_beta->alpha / (3.0f/2.0f * vbus);
	float beta  = alpha_beta->beta / (3.0f/2.0f * vbus);
	uint32_t sector = 0xFF;

	if (beta >= 0.0f) {
		if (alpha >= 0.0f) {
			//quadrant I
			if (ONE_BY_SQRT3 * beta > alpha) {
				sector = SECTOR_2;
			} else {
				sector = SECTOR_1;
			}
		} else {
			//quadrant II
			if (-ONE_BY_SQRT3 * beta > alpha) {
				sector = SECTOR_3;
			} else {
				sector = SECTOR_2;
			}
		}
	} else {
		if (alpha >= 0.0f) {
			//quadrant IV5
			if (-ONE_BY_SQRT3 * beta > alpha) {
				sector = SECTOR_5;
			} else {
				sector = SECTOR_6;
			}
		} else {
			//quadrant III
			if (ONE_BY_SQRT3 * beta > alpha) {
				sector = SECTOR_4;
			} else {
				sector = SECTOR_5;
			}
		}
	}
	// PWM timings
	u32 tA, tB, tC;
	u32 low, midle, high;
	switch (sector) {
		// sector 1-2
	case SECTOR_1: {
		// Vector on-times
		uint32_t t1 = (alpha - ONE_BY_SQRT3 * beta) * PWM_half_period;
		uint32_t t2 = (TWO_BY_SQRT3 * beta) * PWM_half_period;
	
		// PWM timings
		tA = (PWM_half_period - t1 - t2) / 2;
		tB = tA + t1;
		tC = tB + t2;
		low = tA;
        midle = tB;
        high = tC;
		break;
	}
	// sector 2-3
	case SECTOR_2: {
		// Vector on-times
		uint32_t t2 = (alpha + ONE_BY_SQRT3 * beta) * PWM_half_period;
		uint32_t t3 = (-alpha + ONE_BY_SQRT3 * beta) * PWM_half_period;
	
		// PWM timings
		tB = (PWM_half_period - t2 - t3) / 2;
		tA = tB + t3;
		tC = tA + t2;
		low = tB;
        midle = tA;
        high = tC;	
		break;
	}
	
	// sector 3-4
	case SECTOR_3: {
		// Vector on-times
		uint32_t t3 = (TWO_BY_SQRT3 * beta) * PWM_half_period;
		uint32_t t4 = (-alpha - ONE_BY_SQRT3 * beta) * PWM_half_period;
	
		// PWM timings
		tB = (PWM_half_period - t3 - t4) / 2;
		tC = tB + t3;
		tA = tC + t4;
		low = tB;
        midle = tC;
        high = tA;
		break;
	}
	
	// sector 4-5
	case SECTOR_4: {
		// Vector on-times
		uint32_t t4 = (-alpha + ONE_BY_SQRT3 * beta) * PWM_half_period;
		uint32_t t5 = (-TWO_BY_SQRT3 * beta) * PWM_half_period;
	
		// PWM timings
		tC = (PWM_half_period - t4 - t5) / 2;
		tB = tC + t5;
		tA = tB + t4;
		low = tC;
        midle = tB;
        high = tA;	
		break;
	}
	
	// sector 5-6
	case SECTOR_5: {
		// Vector on-times
		uint32_t t5 = (-alpha - ONE_BY_SQRT3 * beta) * PWM_half_period;
		uint32_t t6 = (alpha - ONE_BY_SQRT3 * beta) * PWM_half_period;
	
		// PWM timings
		tC = (PWM_half_period - t5 - t6) / 2;
		tA = tC + t5;
		tB = tA + t6;
		low = tC;
        midle = tA;
        high = tB;	
		break;
	}
	
	// sector 6-1
	case SECTOR_6: {
		// Vector on-times
		uint32_t t6 = (-TWO_BY_SQRT3 * beta) * PWM_half_period;
		uint32_t t1 = (alpha + ONE_BY_SQRT3 * beta) * PWM_half_period;

		// PWM timings
		tA = (PWM_half_period - t6 - t1) / 2;
		tC = tA + t1;
		tB = tC + t6;
		low = tA;
        midle = tC;
        high = tB;	
		break;
	}
	}
	
	phase_out->A = tA;
	phase_out->B = tB;
	phase_out->C = tC;
	phase_out->low = low;
	phase_out->midle = midle;
	phase_out->high = high;
	*sector_out = sector;
#if 0
	static int tet_p = 0;
	if (tet_p++ % 5 == 0) {
		printf("$%d %d %d;", tA, tB, tC);
	}
#endif		
}

#if 0
static u8 __inline Calc_N(alpha_beta_t *alpha_beta){
	float sqr_alpha = alpha_beta->alpha * SQRT3_BY_2;
	float half_beta = 0.5f * alpha_beta->beta;
	return (((sqr_alpha - half_beta > 0.0f) << 1) + (alpha_beta->beta > 0.0f)) + ((-(sqr_alpha + half_beta) > 0.0f) << 2);
}

static void __inline Calc_XYZ(alpha_beta_t *alpha_beta, float vbus, uint32_t PWM_Period, float *XYZ_Out) {
	float modu = (1.0f / vbus) * (float)PWM_Period;
	float sqr_beta = SQRT3_BY_2 * alpha_beta->beta;
	float alpha = 1.5f * alpha_beta->alpha;

	XYZ_Out[0] = SQRT3 * alpha_beta->beta * modu;
	XYZ_Out[1] = (sqr_beta + alpha) * modu;
	XYZ_Out[2] = (sqr_beta - alpha) * modu;
}
#endif
static void __inline ModuleTime(u32 *T4, u32 *T6, u32 PWM_Period) {
	u32 period = PWM_Period * 95 / 100; //95%�ĵ���
	if (*T4 + *T6 > period){
		float ration = ((float)period)/((float)*T4 + (float)*T6);
		*T4 *= ration;
		*T6 *= ration;
	}
}
void SVPWM_ST(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out){
	u32 PWM_Period = PWM_half_period * 2;
	float wAlpha = SQRT3 * alpha_beta->alpha * 2.0f;
	float wBeta = -alpha_beta->beta * 2.0f;
	float X = wBeta * PWM_Period/vbus;
	float Y = (wBeta + wAlpha)*PWM_Period/vbus/2.0f;
	float Z = (wBeta - wAlpha)*PWM_Period/vbus/2.0f;	
	s32 tA, tB, tC;
	s32 low, midle, high;
	if (Y < 0) {
    	if (Z < 0) {
        	*sector_out = 5;
        	tA = PWM_Period/4 + (Y - Z)/4;
        	tB = tA + Z/2;
        	tC = tA - Y/2;
      		low = tC;
      		midle = tA;
      		high = tB;			
    	}else {
        	if (X <= 0 ) {
            	*sector_out = 4;
            	tA = PWM_Period/4 + (X - Z)/4;
            	tB = tA + Z/2;
            	tC = tB - X/2;
				low = tC;
      			midle = tB;
      			high = tA;
        	}else {
            	*sector_out = 3;
            	tA = PWM_Period/4 + (Y - X)/4;
            	tC = tA - Y/2;
            	tB = tC + X/2;
      			low = tB;
      			midle = tC;
      			high = tA;				
        	}
    	}
	}else {
    	if (Z >= 0) {
        	*sector_out = 2;
        	tA = PWM_Period/4 + (Y - Z)/4;
        	tB= tA + Z/2;
        	tC = tA - Y/2;
      		low = tB;
      		midle = tC;
      		high = tA;			
    	}else {
        	if (X <= 0 ) {
            	*sector_out = 6;
            	tA = PWM_Period/4 + (Y - X)/4;
            	tC = tA - Y/2;    
            	tB = tC + X/2;
	      		low = tA;
      			midle = tC;
      			high = tB;			
        	} else {
            	*sector_out = 1;
            	tA = PWM_Period/4 + (X - Z)/4;
            	tB = tA + Z/2;
            	tC = tB - X/2;
	      		low = tA;
      			midle = tB;
      			high = tC;
        	}
    	}
	}
	phase_out->A = ( uint16_t )tA;
	phase_out->B = ( uint16_t )tB;
	phase_out->C = ( uint16_t )tC;
	phase_out->low = low;
	phase_out->midle = midle;
	phase_out->high = high;	
}

void SVPWM_7(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out) {
	float alpha = alpha_beta->alpha * 2.0f / 3.0f;
	float beta  = alpha_beta->beta  * 2.0f / 3.0f;
	u8 sector = 0xFF;
	u32 A_duty, B_duty, C_duty;
	u32 low, midle, high;
	u32 T1, T2;
	float X, Y, Z;
	float modu = (float)(PWM_half_period) / vbus;

	if (beta >= 0.0f) {
		if (alpha >= 0.0f) {
			//quadrant I
			if (ONE_BY_SQRT3 * beta > alpha) {
				sector = SECTOR_2;
			} else {
				sector = SECTOR_1;
			}
		} else {
			//quadrant II
			if (-ONE_BY_SQRT3 * beta > alpha) {
				sector = SECTOR_3;
			} else {
				sector = SECTOR_2;
			}
		}
	} else {
		if (alpha >= 0.0f) {
			//quadrant IV5
			if (-ONE_BY_SQRT3 * beta > alpha) {
				sector = SECTOR_5;
			} else {
				sector = SECTOR_6;
			}
		} else {
			//quadrant III
			if (ONE_BY_SQRT3 * beta > alpha) {
				sector = SECTOR_4;
			} else {
				sector = SECTOR_5;
			}
		}
	}
	//X = SQRT3 * beta * modu;
	X = TWO_BY_SQRT3 * beta * modu;
	//Y = (1.5f * alpha + SQRT3_BY_2 * beta) * modu;
	Y = (alpha + ONE_BY_SQRT3 * beta) * modu;
	//Z = (-1.5f * alpha + SQRT3_BY_2 * beta) * modu;
	Z = (-alpha + ONE_BY_SQRT3 * beta) * modu;
	switch(sector) {
		case SECTOR_1: // 3
			T1 = -Z;
			T2 = X;
			break;		
		case SECTOR_2: // 1
			T1 = Z;
			T2 = Y;			
			break;
		case SECTOR_3: // 5
			T1 = X;
			T2 = -Y;		
			break;
		case SECTOR_4: // 4
			T1 = -X;
			T2 = Z;		
			break;
		case SECTOR_5: // 6
			T1 = -Y;
			T2 = -Z;		
			break;			
		case SECTOR_6: // 2
			T1 = Y;
			T2 = -X;		
			break;
		default:
			break;
	}
	ModuleTime(&T1, &T2, PWM_half_period);
  /*	
	u32 ta = (PWM_half_period - T1 - T2) / 2;
	u32 tb = ta + T1 ;
	u32 tc = tb + T2 ; */
	switch(sector) {
		case SECTOR_1: // 3
			A_duty = (PWM_half_period - T1 - T2) / 2;
			B_duty = A_duty + T1;
			C_duty = B_duty + T2;

			low = C_duty;
			midle = B_duty;
			high = A_duty;
			break;		
		case SECTOR_2: // 1
			B_duty = (PWM_half_period - T1 - T2) / 2;
			A_duty = B_duty + T1;
			C_duty = A_duty + T2;

			low = C_duty;
			midle = A_duty;
			high = B_duty;			
			break;
		case SECTOR_3: // 5
			B_duty = (PWM_half_period - T1 - T2) / 2;
			C_duty = B_duty + T1;
			A_duty = C_duty + T2;

			low = A_duty;
			midle = C_duty;
			high = B_duty;			
			break;
		case SECTOR_4: // 4
			C_duty = (PWM_half_period - T1 - T2) / 2;
			B_duty = C_duty + T1;
			A_duty = B_duty + T2;

			low = A_duty;
			midle = B_duty;
			high = C_duty;			
			break;
		case SECTOR_5: // 6
			C_duty = (PWM_half_period - T1 - T2) / 2;
			A_duty = C_duty + T1;
			B_duty = A_duty + T2;

			low = B_duty;
			midle = A_duty;
			high = C_duty;			
			break;			
		case SECTOR_6: // 2
			A_duty = (PWM_half_period - T1 - T2) / 2;
			C_duty = A_duty + T1;
			B_duty = C_duty + T2;

			low = B_duty;
			midle = C_duty;
			high = A_duty;			
			break;
		default:
			break;
	}
	
	phase_out->A = A_duty;
	phase_out->B = B_duty;
	phase_out->C = C_duty;
	phase_out->low = low;
	phase_out->midle = midle;
	phase_out->high = high;
	*sector_out = sector;
#if 0
	static int tet_p = 0;
	if (tet_p++ % 10 == 0) {
		printf("$%d %d %d;", A_duty, B_duty, C_duty);
	}
#endif		
//	printf("3sec %d, A:%d, B:%d, C:%d\n", sector, A_duty, B_duty, C_duty);
}

/* 7段式SVPWM
 * 返回设置3相PWM的3个CCR寄存器的值
 * 这里使用的是stm32的PWM mode1,在向上计数时，一旦TIMx_CNT<TIMx_CCR1时通道1为有效电平，否则为无效电平
 * 在向下计数时，一旦TIMx_CNT>TIMx_CCR1时通道1为无效电平(OC1REF=0)，否则为有效 电平(OC1REF=1)。
 * 整个时间的计算，前面X,Y,Z都是一样的，后面计算ABC三相的pwm CCR寄存器值的时候，需要注意，很多网络包括书本的资料都是用PWM2模式的
   就是高电平的时间 pwm_period - ccr,我们用PWM1模式,所以最后abc的计算稍微有些不一样
*/

void SVM_Get_Phase_Time(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out) {
	float alpha = alpha_beta->alpha * SQRT3_BY_2;
	float beta  = alpha_beta->beta  * SQRT3_BY_2;
	u32   PWM_Period = PWM_half_period * 2;
	u8 sector = 0xFF;
	u32 tA, tB, tC;
	u32 low, midle, high;
	float X, Y, Z;
	float modu = (float)(PWM_Period) / vbus;

	if (beta >= 0.0f) {
		if (alpha >= 0.0f) {
			//quadrant I
			if (ONE_BY_SQRT3 * beta > alpha) {
				sector = SECTOR_2;
			} else {
				sector = SECTOR_1;
			}
		} else {
			//quadrant II
			if (-ONE_BY_SQRT3 * beta > alpha) {
				sector = SECTOR_3;
			} else {
				sector = SECTOR_2;
			}
		}
	} else {
		if (alpha >= 0.0f) {
			//quadrant IV5
			if (-ONE_BY_SQRT3 * beta > alpha) {
				sector = SECTOR_5;
			} else {
				sector = SECTOR_6;
			}
		} else {
			//quadrant III
			if (ONE_BY_SQRT3 * beta > alpha) {
				sector = SECTOR_4;
			} else {
				sector = SECTOR_5;
			}
		}
	}
	//X = SQRT3 * beta * modu;
	X = TWO_BY_SQRT3 * beta * modu;
	//Y = (1.5f * alpha + SQRT3_BY_2 * beta) * modu;
	Y = (alpha + ONE_BY_SQRT3 * beta) * modu;
	//Z = (-1.5f * alpha + SQRT3_BY_2 * beta) * modu;
	Z = (-alpha + ONE_BY_SQRT3 * beta) * modu;
	switch(sector) {
		case SECTOR_1: // 3
		{	u32 T4 = -Z;
			u32 T6 = X;
			tC = (PWM_Period - T4 - T6)/4;
        	tB = tC + T6/2;
        	tA = tB + T4/2;
			low = tA;
			midle = tB;
			high = tC;
			break;
		}
		case SECTOR_2: // 1
		{
			u32 T6 = Y;
			u32 T2 = Z;
			tC = (PWM_Period - T6 - T2)/4;
        	tA = tC + T6/2;
        	tB = tA + T2/2;
			low = tB;
			midle = tA;
			high = tC;			
			break;
		}
		case SECTOR_3: // 5
		{
			u32 T2 = X;
			u32 T3 = -Y;
			tA = (PWM_Period - T2 - T3)/4;
			tC = tA + T3/2;
			tB = tC + T2/2;
			low = tB;
			midle = tC;
			high = tA;			
			break;
		}
		case SECTOR_4: // 4
		{
			u32 T1 = -X;
			u32 T3 = Z;
			tA = (PWM_Period - T1 - T3)/4;
			tB = tA + T3/2;
			tC = tB + T1/2;
			low = tC;
			midle = tB;
			high = tA;			
			break;
		}
		case SECTOR_5: // 6
		{
			u32 T1 = -Y;
			u32 T5 = -Z;
			tB = (PWM_Period - T1 - T5)/4;
			tA = tB + T5/2;
			tC = tA + T1/2;
			low = tC;
			midle = tA;
			high = tB;			
			break;
		}					
		case SECTOR_6: // 2
		{
			u32 T4 = Y;
			u32 T5 = -X;
			tB = (PWM_Period - T4 - T5)/4;
			tC = tB + T5/2;
			tA = tC + T4/2;
			low = tA;
			midle = tC;
			high = tB;			
			break;
		}
		default:
			break;
	}	
	phase_out->A = tA;
	phase_out->B = tB;
	phase_out->C = tC;
	phase_out->low = low;
	phase_out->midle = midle;
	phase_out->high = high;
	*sector_out = sector;
#if 0
	static int tet_p = 0;
	if (tet_p++ % 10 == 0) {
		printf("$%d %d %d;", tA, tB, tC);
	}
#endif		
//	printf("3sec %d, A:%d, B:%d, C:%d\n", sector, A_duty, B_duty, C_duty);
}


#if 0

void XYZ_step(void)
{
  real_T rtb_Product1;
  real_T rtb_Product2;
  real_T rtb_Product3;

  /* Product: '<S1>/Product' incorporates:
   *  Inport: '<Root>/Ts'
   *  Inport: '<Root>/Udc'
   *  Math: '<S1>/Math Function'
   *
   * About '<S1>/Math Function':
   *  Operator: reciprocal
   */
  rtb_Product1 = 1.0 / rtU.Udc * rtU.Ts;

  /* Gain: '<S1>/Gain2' incorporates:
   *  Inport: '<Root>/Ubeta'
   */
  rtb_Product2 = 0.8660254037844386 * rtU.Ubeta;

  /* Gain: '<S1>/Gain' incorporates:
   *  Inport: '<Root>/Ualpha'
   */
  rtb_Product3 = 1.5 * rtU.Ualpha;

  /* Outport: '<Root>/XYZ' incorporates:
   *  Gain: '<S1>/Gain1'
   *  Inport: '<Root>/Ubeta'
   *  Product: '<S1>/Product1'
   *  Product: '<S1>/Product2'
   *  Product: '<S1>/Product3'
   *  Sum: '<S1>/Add'
   *  Sum: '<S1>/Add1'
   */
  rtY.XYZ_d[2] = (rtb_Product2 - rtb_Product3) * rtb_Product1;
  rtY.XYZ_d[1] = (rtb_Product2 + rtb_Product3) * rtb_Product1;
  rtY.XYZ_d[0] = 1.7320508075688772 * rtU.Ubeta * rtb_Product1;
}




/* Model step function */
void T1T2_step(void)
{
  real_T rtb_Subtract;
  real_T rtb_T1;
  real_T rtb_T2;

  /* MultiPortSwitch: '<S1>/Multiport Switch' incorporates:
   *  Gain: '<S1>/Gain'
   *  Gain: '<S1>/Gain1'
   *  Gain: '<S1>/Gain2'
   *  Inport: '<Root>/N'
   *  Inport: '<Root>/XYZ'
   */
  switch ((int32_T)rtU.N) {
   case 1:
    rtb_T1 = rtU.XYZ[2];

    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
     *  Inport: '<Root>/XYZ'
     */
    rtb_T2 = rtU.XYZ[1];
    break;

   case 2:
    rtb_T1 = rtU.XYZ[1];

    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
     *  Gain: '<S1>/Gain'
     *  Inport: '<Root>/XYZ'
     */
    rtb_T2 = -rtU.XYZ[0];
    break;

   case 3:
    rtb_T1 = -rtU.XYZ[2];

    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
     *  Gain: '<S1>/Gain2'
     *  Inport: '<Root>/XYZ'
     */
    rtb_T2 = rtU.XYZ[0];
    break;

   case 4:
    rtb_T1 = -rtU.XYZ[0];

    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
     *  Gain: '<S1>/Gain'
     *  Inport: '<Root>/XYZ'
     */
    rtb_T2 = rtU.XYZ[2];
    break;

   case 5:
    rtb_T1 = rtU.XYZ[0];

    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
     *  Gain: '<S1>/Gain1'
     *  Inport: '<Root>/XYZ'
     */
    rtb_T2 = -rtU.XYZ[1];
    break;

   default:
    rtb_T1 = -rtU.XYZ[1];

    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
     *  Gain: '<S1>/Gain1'
     *  Gain: '<S1>/Gain2'
     *  Inport: '<Root>/XYZ'
     */
    rtb_T2 = -rtU.XYZ[2];
    break;
  }

  /* End of MultiPortSwitch: '<S1>/Multiport Switch' */

  /* Sum: '<S1>/Subtract' */
  rtb_Subtract = rtb_T1 + rtb_T2;

  /* Switch: '<S1>/Switch' incorporates:
   *  Inport: '<Root>/Tpwm'
   *  Sum: '<S1>/Subtract1'
   *  Switch: '<S1>/Switch1'
   */
  if (rtU.Tpwm - rtb_Subtract > 0.0) {
    /* Outport: '<Root>/T1 ' */
    rtY.T1 = rtb_T1;

    /* Outport: '<Root>/T2' */
    rtY.T2 = rtb_T2;
  } else {
    /* Outport: '<Root>/T1 ' incorporates:
     *  Product: '<S1>/Divide'
     *  Product: '<S1>/Product'
     */
    rtY.T1 = rtb_T1 * rtU.Tpwm / rtb_Subtract;

    /* Outport: '<Root>/T2' incorporates:
     *  Product: '<S1>/Divide1'
     *  Product: '<S1>/Product1'
     */
    rtY.T2 = 1.0 / rtb_Subtract * (rtb_T2 * rtU.Tpwm);
  }

  /* End of Switch: '<S1>/Switch' */
}

#endif