/*
 * PWM_algorithms.c
 *
 *  Created on: 07 ���. 2017 �.
 *      Author: Andrey
 */
#include "PlatformMath.h"
#include "PWM_algorithms.h"
#include "math.h"
#define GENERIC_MAX(x,y) ((x)>(y) ?(x) : (y))
#define GENERIC_MIN(x,y) ((x)<(y) ?(x) : (y))
typedef enum 
{
	SV_PWM = 0x1,
	DPWMMAX,
	DPWMMIN,
	DPWM30,
	DPWM60
}PWM_MODE;
PWM_struct MY_PWM = {0};
FAST_RAM unsigned int PWM_proc(PWM_struct * myPWM);
unsigned int PWM_proc(PWM_struct * myPWM)
{



	float VrefA=0,VrefB=0,VrefC=0;
    //double Va=0,Vb=0,Vc=0;
	float X=0,Y=0,Z=0;
	unsigned int A=0,B=0,C=0;
	float t1=0,t2=0;
	float Ta=0,Tb=0,Tc=0;
	float Ualfa_norm=0,Ubeta_norm=0;//modulation index
	//Vdc_pu=(sqrt(myPWM->Ualfa*myPWM->Ualfa+my_PWM.Ubeta*my_PWM.Ubeta))*sqrt(3.)/myPWM->Vm_max;// ������������� �������� �� ����������
	//Vdc_T=myPWM->Trpd_pwm;///myPWM->Vdc;// ���������� �� ���������


	//������������ ��������� �� Udc
	Ualfa_norm=myPWM->Ualfa;//*1.73205/myPWM->Vdc;
	if(Ualfa_norm>1) Ualfa_norm=1.f;
	if(Ualfa_norm<-1) Ualfa_norm=-1.f;
	Ubeta_norm=myPWM->Ubeta;//*1.73205/myPWM->Vdc;
	if(Ubeta_norm>1) Ubeta_norm=1.f;
	if(Ubeta_norm<-1) Ubeta_norm=-1.f;


	// ���������� ������� ���������

	VrefA=Ubeta_norm;
	VrefB=-0.5f*Ubeta_norm+0.8665025f*Ualfa_norm;
	VrefC=-0.5f*Ubeta_norm-0.8665025f*Ualfa_norm;

	//���������� ������� ���������.
	if(VrefA>0) A=1;
		else A=0;
	if(VrefB>0) B=1;
		else B=0;
	if(VrefC>0) C=1;
		else C=0;
	myPWM->sector=A+2*B+4*C;
	// ���������� ����� �������
//	X=Ubeta_norm;
//	Y=0.5f*(1.732f*Ualfa_norm+Ubeta_norm);
//	Z=0.5f*(-1.732f*Ualfa_norm+Ubeta_norm);
	X=VrefA;
	Y=-VrefC;
	Z=-VrefB;
	//
	//X=2*0.57735*Ubeta_norm;
	//Y=(0.57735*Ualfa_norm+Ubeta_norm);
	//Z=(-0.57735*Ualfa_norm+Ubeta_norm);



	// ����� ����������� ����� �������
	switch(myPWM->sector)
	{
	case 1:
		{
			t1=Z;
			t2=Y;
			Tb =(1.f-t1-t2)*0.5f;      				// tbon = (1-t1-t2)/2
      		Ta = Tb+t1;                             // taon = tbon+t1
       		Tc = Ta+t2;                             // tcon = taon+t2

			break;
		}
	case 2:
		{
			t1=Y;
			t2=-X;
			Ta =0.5f*(1.f-t1-t2);      				// taon = (1-t1-t2)/2
      		Tc = Ta+t1;                             // tcon = taon+t1
       		Tb = Tc+t2;                             // tbon = tcon+t2
			break;
		}
	case 3:
		{
			t1=-Z;
			t2=X;
			Ta = 0.5f*(1.f-t1-t2);      // taon = (1-t1-t2)/2
       		Tb = Ta+t1;                             // tbon = taon+t1
       		Tc = Tb+t2;                             // tcon = tbon+t2
			break;
		}
	case 4:
		{
			t1=-X;
			t2=Z;
			Tc = 0.5f*(1.f-t1-t2);      // tcon = (1-t1-t2)/2
      		Tb = Tc+t1;                             // tbon = tcon+t1
       		Ta = Tb+t2;                             // taon = tbon+t2
			break;
		}
	case 5:
		{
			t1=X;
			t2=-Y;
			Tb = 0.5f*(1.f-t1-t2);      // tbon = (1-t1-t2)/2
      		Tc = Tb+t1;                             // tcon = tbon+t1
       		Ta = Tc+t2;                             // taon = tcon+t2
			break;
		}
	case 6:
		{
			t1=-Y;
			t2=-Z;
			Tc = 0.5*(1.f-t1-t2);      				// tcon = (1-t1-t2)/2
       		Ta = Tc+t1;                             // taon = tcon+t1
       		Tb = Ta+t2;                             // tbon = taon+t2
			break;
		}

	case 0:
		{
			t1=0.5f;
			t2=0.5f;
			Ta=0.5f;
			Tb=0.5f;
			Tc=0.5f;
			break;
		}
	default:
		{

			break;
		}

	}//end of switch

	static float Tas = 0,Tbs=0,Tcs=0;
	static float Toffset = 0;
	static float Tmin = 0, Tmax = 0;
	Tmax = GENERIC_MAX(Ta, Tb);
	Tmax = GENERIC_MAX(Tmax, Tc);
	Tmin = GENERIC_MIN(Ta, Tb);
	Tmin = GENERIC_MIN(Tmin, Tc);
	
	
	PWM_MODE pwm_mod =  SV_PWM;
	switch (pwm_mod)
	{
	case SV_PWM:
	{
		Toffset = (1.f-(Tmax-Tmin)) / 2.f - Tmin;
		break;
	}
	case DPWMMAX:
	{
		Toffset = 1.f - Tmax;
		break;
	}
	case DPWMMIN:
	{
		Toffset = -Tmin;
		break;
	}
	case DPWM30:
	{
		if ((2.f*(Tmin + Tmax)-2.f) >= 0)
			Toffset = -(Tmin*2.f-1.f);
		else
		{
			Toffset = 1.0f - (Tmax*2.f-1.f);
		}
		
		break;
	}
	case DPWM60:
	{
		break;
	}
	default:
		break;
	}
	////D�+
	//Toffset = 1 - Tmax;
	////DC-
	//Toffset = -Tmin;
	////Generic
	//static float sigma = toPI/8.f;
	//float mu = 1 - 0.5f * (1 + my_sign(cos(BLDC_drive.Teta_rot_el+sigma),100000));
	//mu = 0.5f;
	//Toffset = (1 - mu) + (mu - 1) * Tmax - mu * Tmin;
	Tas = Ta + Toffset;
	Tbs = Tb + Toffset;
	Tcs = Tc + Toffset;
	//
	if (Tas > 1.0f) Tas = 1.0f;
	if (Tbs > 1.0f) Tbs = 1.0f;
	if (Tcs > 1.0f) Tcs = 1.0f;
	if (Tas < 0.0f) Tas = 0.0f;
	if (Tbs < 0.0f) Tbs = 0.0f;
	if (Tcs < 0.0f) Tcs = 0.0f;
	// ���������� ������ ��������� � ������ ���
	myPWM->Ta=Tas*myPWM->Trpd_pwm;
	myPWM->Tb=Tbs*myPWM->Trpd_pwm;
	myPWM->Tc=Tcs*myPWM->Trpd_pwm;

	return 1;
}