/* modul PWM modulace
   ubytky a mrtve casy

   shodna verze s PWM_FULL_LC

   31.5. 2007

   VER. zmenena metoda vypoctu pily (nove funkce)
   31.5.2007	Doplnena PWM se treti harmonickou - fce pwm_full_new_3h(...) - prepinani mezi sinusovou PWM a 3. harmonickou pomoci priznaku mod!
*/

#define _USE_MATH_DEFINES
#include <math.h>
#include <stdlib.h>
#include "pwm.h"

void pwm_full_init(double Ucn, double fc, double DT);
void pwm_full(double *us, double *u, double *Iz,double Ucn, double Uc, double *i, double fc, double *va_char,unsigned int pocet, double h);

void pwm_full_new_init(double h, double fc, double DT, int citac_init, int D_smer, int D_smer2);
void pwm_full_new(double *us, double *u, double *Iz, double Ucn, double Uc, double *is, double *va_char, unsigned int pocet);

// PWM se treti harmonickou
void pwm_full_new_init_3h(double h, double fc, double DT, int citac_init, int D_smer, int D_smer2);
void pwm_full_new_3h(double *us, double *u, double *Iz, double Ucn, double Uc, double *is, double *va_char, unsigned int pocet, unsigned int mod);

static double ubytek(double *va_char, double I, unsigned int pocet);

static double up, up2, dup;
static int smer, smer2;

static int citac, citac2, citac_PR;

double ladeni_pila, ladeni_ur;

void pwm_full_init(double Ucn, double fc, double DT)
/*
Ucn ... jmenovite napeti kondenzatoru v ss obvodu
fc  ... spinaci frekvence
h   ... krok simulace
DT  ... velikost mrtveho casu
*/
{
  smer=-1; smer2=1;
  up=Ucn/2;
//  up=0.;
  up2=up-2*Ucn*fc*DT;
}

void pwm_full(double *us, double *u, double *Iz,double Ucn, double Uc, double *is, double fc, double *va_char,unsigned int pocet, double h)
/*
u    ... pozadovane napeti
us   ... [usx, usy]...vystupni napeti stridace v (x,y)
Iz   ... proud v ss obvodu
Ucn  ... jmenovite nap. v ss obvodu
Uc   ... skutecne napeti na C
is   ... [isx, isy]...proud motorem
va_char ... VA-char vykonovych spinacu
pocet   ... pocet bodu VA-charky
h    ... krok simulace
*/
{
  unsigned int i;
  double iabc[3], ur[3],ustr[3],ua,ub,uc;
  double dtr[3],dd[3];	      // ubytky na polovodicich

  ladeni_pila=up;

  iabc[0]=*is;
  iabc[1]=(-*is+sqrt(3.)**(is+1))/2.;
  iabc[2]=(-*is-sqrt(3.)**(is+1))/2.;

  ur[0]=*u;
  ur[1]=(-*u+sqrt(3.)**(u+1))/2.;
  ur[2]=(-*u-sqrt(3.)**(u+1))/2.;

  for (i=0;i<3;i++)
  { dtr[i]=ubytek(va_char,fabs(iabc[i]),pocet);
    dd[i]=dtr[i]*.73;
  }

  // implementace ubytku a mrtvych casu
  for (i=0;i<3;i++)
    if (iabc[i]>=0)
      if ((ur[i]>up) && (ur[i]>up2))
	ustr[i]=Uc/2-dtr[i];
      else
	ustr[i]=-(Uc/2+dd[i]);
    else
      if ((ur[i]<up) && (ur[i]<up2))
	ustr[i]=-(Uc/2-dtr[i]);
      else
	ustr[i]=Uc/2+dd[i];

// fazova napeti zateze
  ua=(2*ustr[0]-ustr[1]-ustr[2])/3;
  ub=(2*ustr[1]-ustr[0]-ustr[2])/3;
  uc=(2*ustr[2]-ustr[0]-ustr[1])/3;

// vystupni napeti stridace v (x,y)
  *us=(2.*ua-ub-uc)/3.;
  *(us+1)=(ub-uc)/sqrt(3.);

// vypocet pily pro PWM a pily pro mrtve casy
  dup=2.*Ucn*fc*h;
  up+=smer*dup;
  if (up>Ucn/2)
  { up-=2*dup;
    smer=-1;
  }
  if (up<-Ucn/2)
  { up+=2.*dup;
    smer=1;
  }

  up2+=smer2*dup;
  if (up2>Ucn/2)
  { up2-=2.*dup;
    smer2=-1;
  }
  if (up2<-Ucn/2)
  { up2+=2.*dup;
    smer2=1;
  }

  *Iz=(ustr[0]>0)*iabc[0]+(ustr[1]>0)*iabc[1]+(ustr[2]>0)*iabc[2];
}

static double ubytek(double *va_char, double I, unsigned int pocet)
{
  unsigned int ii;
  double delta_u;

  ii=0;
  while ((*(va_char+ii)<I) && (ii<(pocet-1)))
    ii++;

  if (ii==(pocet-1))  // konec va_char - predpoklad nasyceni charky
    delta_u=*(va_char+ii+pocet);
  else
    if (ii==0)
      delta_u=0;
    else
      delta_u=*(va_char+ii-1+pocet)+(I-*(va_char+ii-1))/(*(va_char+ii)-*(va_char+ii-1))*(*(va_char+ii+pocet)-*(va_char+ii-1+pocet));

  return delta_u;
}


void pwm_full_new(double *us, double *u, double *Iz, double Ucn, double Uc, double *is, double *va_char, unsigned int pocet)
/*
u    ... pozadovane napeti
us   ... [usx, usy]...vystupni napeti stridace v (x,y)
Iz   ... proud v ss obvodu
Ucn  ... jmenovite nap. v ss obvodu
Uc   ... skutecne napeti na C
is   ... [isx, isy]...proud motorem
va_char ... VA-char vykonovych spinacu
pocet   ... pocet bodu VA-charky
t    ... aktualni cas simulace
*/
{
  unsigned int i;
  double iabc[3], ur[3],ustr[3],ua,ub,uc;
  double dtr[3],dd[3];	      // ubytky na polovodicich

  // vypocet pily
  up=(double)citac/citac_PR*Ucn/2.;
  up2=(double)citac2/citac_PR*Ucn/2.;

  ladeni_pila=up;

  iabc[0]=*is;
  iabc[1]=(-*is+sqrt(3.)**(is+1))/2.;
  iabc[2]=(-*is-sqrt(3.)**(is+1))/2.;

  ur[0]=*u;
  ur[1]=(-*u+sqrt(3.)**(u+1))/2.;
  ur[2]=(-*u-sqrt(3.)**(u+1))/2.;

  for (i=0;i<3;i++)
  { dtr[i]=ubytek(va_char,fabs(iabc[i]),pocet);
    dd[i]=dtr[i]*.73;
  }

  // implementace ubytku a mrtvych casu
  for (i=0;i<3;i++)
    if (iabc[i]>=0)
      if ((ur[i]>up) && (ur[i]>up2))
	ustr[i]=Uc/2-dtr[i];
      else
	ustr[i]=-(Uc/2+dd[i]);
    else
      if ((ur[i]<up) && (ur[i]<up2))
	ustr[i]=-(Uc/2-dtr[i]);
      else
	ustr[i]=Uc/2+dd[i];

// fazova napeti zateze
  ua=(2*ustr[0]-ustr[1]-ustr[2])/3;
  ub=(2*ustr[1]-ustr[0]-ustr[2])/3;
  uc=(2*ustr[2]-ustr[0]-ustr[1])/3;

// vystupni napeti stridace v (x,y)
  *us=(2.*ua-ub-uc)/3.;
  *(us+1)=(ub-uc)/sqrt(3.);

  *Iz=(ustr[0]>0)*iabc[0]+(ustr[1]>0)*iabc[1]+(ustr[2]>0)*iabc[2];

  // posun pily ... inkrement citacu
  if ((citac==citac_PR)||(citac==-citac_PR)) smer*=-1;
  if ((citac2==citac_PR)||(citac2==-citac_PR)) smer2*=-1;
  citac+=smer;
  citac2+=smer2;
}

void pwm_full_new_init(double h, double fc, double DT, int citac_init, int D_smer, int D_smer2)
/*
fc  ... spinaci frekvence
h   ... krok simulace
DT  ... velikost mrtveho casu
citac_init ... pocatecni nastaveni citace
D_smer, D_smer1 ... pocatecni nastaveni smeru citani
*/
{
  smer=D_smer; smer2=D_smer2;
  citac=citac_init;
  citac2=citac_init-(int)(DT/h);
  citac_PR=(int)(1./4./fc/h);
}


void pwm_full_new_init_3h(double h, double fc, double DT, int citac_init, int D_smer, int D_smer2)
/*
fc  ... spinaci frekvence
h   ... krok simulace
DT  ... velikost mrtveho casu
citac_init ... pocatecni nastaveni citace
D_smer, D_smer1 ... pocatecni nastaveni smeru citani
*/
{
  smer=D_smer; smer2=D_smer2;
  citac=citac_init;
  citac2=abs(citac_init-(int)(DT/h));
  citac_PR=(int)(1./2./fc/h);
}


void pwm_full_new_3h(double *us, double *u, double *Iz, double Ucn, double Uc, double *is, double *va_char, unsigned int pocet, unsigned int mod)
/*
u    ... pozadovane napeti ve formatu u={Um, beta}
us   ... [usx, usy]...vystupni napeti stridace v (x,y)
Iz   ... proud v ss obvodu
Ucn  ... jmenovite nap. v ss obvodu
Uc   ... skutecne napeti na C
is   ... [isx, isy]...proud motorem
va_char ... VA-char vykonovych spinacu
pocet   ... pocet bodu VA-charky
t    ... aktualni cas simulace
mod ... mod= - sinusova PWM; mod=1 - PWM se 3. harmonickou
*/
{
  unsigned int i;
  double iabc[3], ur[3],ustr[3],ua,ub,uc;
  double dtr[3],dd[3];	      // ubytky na polovodicich
  double Um, beta;
  double U3;				  // korekce 3. harmonickou


  Um=*u;
  beta=*(u+1);

  // vypocet pily
//  up=(double)citac/citac_PR*Ucn/2.;
//  up2=(double)citac2/citac_PR*Ucn/2.;
  // vypocet pily presne jako v procesoru
  up=((double)citac/citac_PR-0.5)*Ucn;
  up2=((double)citac2/citac_PR-0.5)*Ucn;

  ladeni_pila=up;

  iabc[0]=*is;
  iabc[1]=(-*is+sqrt(3.)**(is+1))/2.;
  iabc[2]=(-*is-sqrt(3.)**(is+1))/2.;

  if (mod==0)	// sinusova PWM
  {
    ur[0]=Um*cos(beta);
    ur[1]=Um*cos(beta-2./3.*M_PI);
    ur[2]=Um*cos(beta+2./3.*M_PI);
  }
  else			// PWM se 3. harmonickou
  {
    // dle Karla
//    U3=0.13*cos(3.*beta);
	// moje
	U3=0.17*cos(3.*beta);
    ur[0]=Um*(cos(beta)-U3);
    ur[1]=Um*(cos(beta-2./3.*M_PI)-U3);
    ur[2]=Um*(cos(beta+2./3.*M_PI)-U3);
  }

  ladeni_ur=ur[0];

  for (i=0;i<3;i++)
  { dtr[i]=ubytek(va_char,fabs(iabc[i]),pocet);
    dd[i]=dtr[i]*.73;
  }

  // implementace ubytku a mrtvych casu
  for (i=0;i<3;i++)
    if (iabc[i]>=0)
      if ((ur[i]>up) && (ur[i]>up2))
	ustr[i]=Uc/2-dtr[i];
      else
	ustr[i]=-(Uc/2+dd[i]);
    else
      if ((ur[i]<up) && (ur[i]<up2))
	ustr[i]=-(Uc/2-dtr[i]);
      else
	ustr[i]=Uc/2+dd[i];

// fazova napeti zateze
  ua=(2.*ustr[0]-ustr[1]-ustr[2])/3.;
  ub=(2.*ustr[1]-ustr[0]-ustr[2])/3.;
  uc=(2.*ustr[2]-ustr[0]-ustr[1])/3.;

// vystupni napeti stridace v (x,y)
  *us=(2.*ua-ub-uc)/3.;
  *(us+1)=(ub-uc)/sqrt(3.);

  *Iz=(ustr[0]>0)*iabc[0]+(ustr[1]>0)*iabc[1]+(ustr[2]>0)*iabc[2];

  // posun pily ... inkrement citacu
//  if ((citac==citac_PR)||(citac==-citac_PR)) smer*=-1;
//  if ((citac2==citac_PR)||(citac2==-citac_PR)) smer2*=-1;
  if ((citac==citac_PR)||(citac==0)) smer*=-1;
  if ((citac2==citac_PR)||(citac2==0)) smer2*=-1;
  citac+=smer;
  citac2+=smer2;
}