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

	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;
}