#include "foc/core/svpwm.h"
#include "math/fast_math.h"
#include "libs/logger.h"

#if 0
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;

//	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 = TWO_BY_SQRT3 * beta * modu;
	Y = (alpha + ONE_BY_SQRT3 * 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;
}
#endif

#if 0
void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
	s16q5_t alpha = S16_mul(alb->a, SQRT3_BY_2_Q14, 14);
	s16q5_t beta  = S16_mul(alb->b, SQRT3_BY_2_Q14, 14);
	u32   PWM_Period = PWM_half_period * 2;
	u8 sector = 0xFF;
	u32 tA, tB, tC;
	u32 low, midle;
	s32 X, Y, Z;
	//static int _g_count = 0;
#if 1
	s16q5_t mod = ((PWM_Period << 10) / vbus);
#else
	float mod =  (float)(PWM_Period) / S16Q5toF(vbus);
#endif
	if (beta >= 0) {
		if (alpha >= 0) {
			//quadrant I
			if (S16_mul(ONE_BY_SQRT3_Q14, beta, 14) > alpha) {
				sector = SECTOR_2;
			} else {
				sector = SECTOR_1;
			}
		} else {
			//quadrant II
			if (-S16_mul(ONE_BY_SQRT3_Q14, beta, 14) > alpha) {
				sector = SECTOR_3;
			} else {
				sector = SECTOR_2;
			}
		}
	} else {
		if (alpha >= 0.0f) {
			//quadrant IV5
			if (-S16_mul(ONE_BY_SQRT3_Q14 ,beta, 14) > alpha) {
				sector = SECTOR_5;
			} else {
				sector = SECTOR_6;
			}
		} else {
			//quadrant III
			if (S16_mul(ONE_BY_SQRT3_Q14, beta, 14) > alpha) {
				sector = SECTOR_4;
			} else {
				sector = SECTOR_5;
			}
		}
	}
#if 1
	s16q5_t temp = S16_mul(ONE_BY_SQRT3_Q14, beta, 14);
	X = S16_mul(S16_mul(TWO_BY_SQRT3_Q14, beta, 14), mod, 10);
	Y = S16_mul((alpha + temp), mod, 10);
	Z = S16_mul((-alpha + temp) ,mod, 10);
#else
	float temp =  ONE_BY_SQRT3 * S16Q5toF(beta);
	X = TWO_BY_SQRT3 * S16Q5toF(beta) * (mod);
	Y = (S16Q5toF(alpha) + temp) * (mod);
	Z = (-S16Q5toF(alpha) + temp) * (mod);
#endif
	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;
	}	
	out->n_Duty[0] = tA;
	out->n_Duty[1] = tB;
	out->n_Duty[2] = tC;
	out->n_lowDuty = low;
	out->n_midDuty = midle;
	out->n_Sector = sector;
#if 0
	if (_g_count++ % 10 == 0) {
		plot_3data16(X, Y, Z);
	}
#endif
}

#else
#if 1
#define Duty_Sat(max, t1, t2) \
	do { \
		if (t1+t2 > max) { \
			float r = (float)max/(float)(t1 + t2); \
			t1 = r * t1; \
			t2 = r * t2; \
		} \
	}while(0);
#else
#define Duty_Sat(max, t1, t2) 
#endif
void SVM_Duty_Fix(AB_t *alb, float vbus, u32 PWM_half_period, FOC_OutP *out) {
	float alpha = (alb->a);// * SQRT3_BY_2;
	float beta  = (alb->b);//  * SQRT3_BY_2;
	s32   PWM_Period = PWM_half_period * 2;
	u8    sector = 0xFF;
	s32   tA, tB, tC;
	u32   low, midle;
	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
		{	s32 T4 = -Z;
			s32 T6 = X;
			Duty_Sat(PWM_Period, T4, T6);
			tC = (PWM_Period - T4 - T6)/4;
        	tB = tC + T6/2;
        	tA = tB + T4/2;
			low = tA;
			midle = tB;
			break;
		}
		case SECTOR_2: // 1
		{
			s32 T6 = Y;
			s32 T2 = Z;
			Duty_Sat(PWM_Period, T2, T6);
			tC = (PWM_Period - T6 - T2)/4;
        	tA = tC + T6/2;
        	tB = tA + T2/2;
			low = tB;
			midle = tA;
			break;
		}
		case SECTOR_3: // 5
		{
			s32 T2 = X;
			s32 T3 = -Y;
			Duty_Sat(PWM_Period, T2, T3);
			tA = (PWM_Period - T2 - T3)/4;
			tC = tA + T3/2;
			tB = tC + T2/2;
			low = tB;
			midle = tC;
			break;
		}
		case SECTOR_4: // 4
		{
			s32 T1 = -X;
			s32 T3 = Z;
			Duty_Sat(PWM_Period, T1, T3);
			tA = (PWM_Period - T1 - T3)/4;
			tB = tA + T3/2;
			tC = tB + T1/2;
			low = tC;
			midle = tB;
			break;
		}
		case SECTOR_5: // 6
		{
			s32 T1 = -Y;
			s32 T5 = -Z;
			Duty_Sat(PWM_Period, T1, T5);
			tB = (PWM_Period - T1 - T5)/4;
			tA = tB + T5/2;
			tC = tA + T1/2;
			low = tC;
			midle = tA;
			break;
		}					
		case SECTOR_6: // 2
		{
			s32 T4 = Y;
			s32 T5 = -X;
			Duty_Sat(PWM_Period, T4, T5);
			tB = (PWM_Period - T4 - T5)/4;
			tC = tB + T5/2;
			tA = tC + T4/2;
			low = tA;
			midle = tC;
			break;
		}
		default:
			break;
	}	
	out->n_Duty[0] = sclamp(tA, 0, (s32)PWM_half_period);
	out->n_Duty[1] = sclamp(tB, 0, (s32)PWM_half_period);
	out->n_Duty[2] = sclamp(tC, 0, (s32)PWM_half_period);
	out->n_lowDuty = low;
	out->n_midDuty = midle;
	out->n_Sector = sector;
#if 0	
	if (_g_count++ % 10 == 0) {
		plot_3data16(X, Y, Z);
	}
#endif	
}
#endif