/*=====================================================================================
 File name:        SVGEN_MF.C  (IQ version)                  
                    
 Originator:	Digital Control Systems Group
			Texas Instruments

 Description:   Space-vector PWM generation based on magnitude and frequency components

=====================================================================================
 History:
-------------------------------------------------------------------------------------
 04-15-2005	Version 3.20
-------------------------------------------------------------------------------------*/
#include "IQmathLib.h"         // Include header for IQmath library
#include "dmctype.h"


#include "svgen_mf.h"

#include <params.h>


static _iq iq1_0=_IQ(1.0);
static _iq iq6_0=_IQ(6.0);
static _iq iq0_5=_IQ(0.5);
static _iq iq2_0=_IQ(2.0);

static _iq iq3_0=_IQ(3.0);
static _iq iq4_0=_IQ(4.0);
static _iq iq5_0=_IQ(5.0);


//#pragma CODE_SECTION(svgenmf_calc,".fast_run");
void svgenmf_calc(SVGENMF *v)
{	
	_iq StepAngle,EntryOld,dx,dy;
	_iq T = iq1_0;//_IQ(1.0);

	// Normalise the freq input to appropriate step angle
    // Here, 1 pu. = 60 degree
//#ifdef DOUBLE_UPDATE_PWM
//    StepAngle = (_IQmpy(v->Freq,v->FreqMax))>>1; // decrise step /2 for double update
//#else
    StepAngle = _IQmpy(v->Freq,v->FreqMax); // normal step
//#endif

    // Calculate new angle alpha
    EntryOld = v->NewEntry;

    v->Full_Alpha = v->Full_Alpha + StepAngle;

    if (v->Full_Alpha < 0)
      v->Full_Alpha = v->Full_Alpha+iq6_0;

    if (v->Full_Alpha >= iq6_0)
      v->Full_Alpha = v->Full_Alpha-iq6_0;


    //new sector detect
    if (v->Full_Alpha >= iq5_0)
    {
      v->SectorPointer=5;
      v->Alpha = v->Full_Alpha-iq5_0;
    }
    else
    if (v->Full_Alpha >= iq4_0)
    {
      v->SectorPointer=4;
      v->Alpha = v->Full_Alpha-iq4_0;
    }
    else
    if (v->Full_Alpha >= iq3_0)
    {
      v->SectorPointer=3;
      v->Alpha = v->Full_Alpha-iq3_0;
    }
    else
    if (v->Full_Alpha >= iq2_0)
    {
      v->SectorPointer=2;
      v->Alpha = v->Full_Alpha-iq2_0;
    }
    else
    if (v->Full_Alpha >= iq1_0)
    {
      v->SectorPointer=1;
      v->Alpha = v->Full_Alpha-iq1_0;
    }
    else
    {
      v->SectorPointer=0;
      v->Alpha = v->Full_Alpha;
    }


//    v->Alpha = v->Alpha + StepAngle;
//	if (v->Alpha >= iq1_0)
//	  v->Alpha = v->Alpha-iq1_0;


    v->NewEntry = v->Alpha;
    
    dy = _IQsin(_IQmpy(v->NewEntry,PI_THIRD));              // dy = sin(NewEntry)
    dx = _IQsin(PI_THIRD-_IQmpy(v->NewEntry,PI_THIRD));     // dx = sin(60-NewEntry)
  
    // Determine which sector
//    if (v->NewEntry-EntryOld<0)
//    {
//      if (v->SectorPointer==5)
//         v->SectorPointer = 0;
//      else
//         v->SectorPointer = v->SectorPointer + 1;
//    }
 
    if (v->SectorPointer==0)  // Sector 1 calculations - a,b,c --> a,b,c
    {
		v->Ta = _IQmpy(iq0_5,(T-dx-dy));
		v->Tb = v->Ta + dx;
		v->Tc = T - v->Ta; 
    }
    else if (v->SectorPointer==1)  // Sector 2 calculations - a,b,c --> b,a,c  &  dx <--> dy
    {
		v->Tb = _IQmpy(iq0_5,(T-dx-dy));
		v->Ta = v->Tb + dy;
		v->Tc = T - v->Tb; 
    }
    else if (v->SectorPointer==2)  // Sector 3 calculations - a,b,c --> b,c,a
    {
		v->Tb = _IQmpy(iq0_5,(T-dx-dy));
		v->Tc = v->Tb + dx;
	    v->Ta = T - v->Tb; 
    }
    else if (v->SectorPointer==3)  // Sector 4 calculations - a,b,c --> c,b,a  &  dx <--> dy
    {
		v->Tc = _IQmpy(iq0_5,(T-dx-dy));
		v->Tb = v->Tc + dy;
		v->Ta = T - v->Tc; 
    }
    else if (v->SectorPointer==4)  // Sector 5 calculations - a,b,c --> c,a,b
    {
		v->Tc = _IQmpy(iq0_5,(T-dx-dy));
		v->Ta = v->Tc + dx;
		v->Tb = T - v->Tc; 
    }
    else if (v->SectorPointer==5)  // Sector 6 calculations - a,b,c --> a,c,b  &  dx <--> dy
    {
		v->Ta = _IQmpy(iq0_5,(T-dx-dy));
		v->Tc = v->Ta + dy;
		v->Tb = T - v->Ta; 
    }

// Convert the unsigned GLOBAL_Q format (ranged (0,1)) -> signed GLOBAL_Q format (ranged (-1,1))
// Then, multiply with a gain and add an offset.
    v->Ta = _IQmpy(iq2_0,(v->Ta-iq0_5));
    v->Ta = _IQmpy(v->Gain,v->Ta) + v->Offset;

    v->Tb = _IQmpy(iq2_0,(v->Tb-iq0_5));
    v->Tb = _IQmpy(v->Gain,v->Tb) + v->Offset;

    v->Tc = _IQmpy(iq2_0,(v->Tc-iq0_5));
    v->Tc = _IQmpy(v->Gain,v->Tc) + v->Offset;

}