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

/* 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 svpwm_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;
#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);
}