/*
   Simulator of Vector Controlled PMSM Drive

   This module is background for PMSM drive object design and
   introduces basic functions ... set_parameters() and eval().

   Z. Peroutka

Rev. 25.4.2009

*/

#define _USE_MATH_DEFINES

#include <stdio.h>
#include <math.h>
#include <stdlib.h> //na linuxu je abs v stdlib
#include "regulace.h"
#include "simulator.h"


#define REZIM_REGULACE  1       // 0...reg. momentu, 1...reg.rychlosti, 2... Isqw=sqrt(Imax^2-Id^2) - max. moment

void pmsmsim_set_parameters(double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0);
void pmsmsim_step(double Ww);

// local functions
static void pwm(unsigned int mod);
static double ubytek(double I);
static void pmsm_model(unsigned int mod);


// simulator properties ///////////////////////
static double Rs,Ls,Fmag,Bf,p,kp,J;        // parameters of PMSM model
static double Ucn,Uc,DT,U_modulace;        // dc-link voltage and dead-time
static double Urm_max;                     // field weakening
static double h,h_reg,h_reg_real;          // simulation step and sampling of employed loops
unsigned int h_reg_counter,h_reg_counter_mez;       // emulation of DSP operation

static double va_char[16]={0,10,50,100,200,300,500,1000, 0,1,1.8,2.4,3.2,3.8,4.8,6.8};    // ubytky
static unsigned int pocet=8;		// velikost VA-charky

// system state
static double x[9]; // (isx,isy,wme,theta_e,M,Fsd,Isd,Isq,Mz)

// internal variables of PWM module
static int smer, smer2, citac, citac2, citac_PR, modulace;

// internal variables of PMSM model
static double dIsx,dIsx1,dIsx2,dIsx3,dIsy,dIsy1,dIsy2,dIsy3;
static double dTheta,dTheta1,dTheta2,dTheta3;
static double dw,dw1,dw2,dw3;

// system measures
static double Isx, Isy, theta, speed;

// control
static double u[2]={0.,0.};		// format u={Um, beta}
static double us[2]={0.,0.};	// format us={us_alfa, us_beta}

// variables for calculation of mean values of stator voltage components
static double usx_av=0., usy_av=0.,sum_usx_av=0.,sum_usy_av=0.;

// variables for calculation of mean values of stator current components - (alfa, beta)
static double isx_av=0., isy_av=0.,sum_isx_av=0.,sum_isy_av=0.;

// stator voltage components filtering
static double usxf=0.,usyf=0.,Tf=0.01;
static unsigned int start_filter=1;

// output for EKF (voltages and measured currents, which are fed to KalmanObs)
static double KalmanObs[10]={0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};      // usx, usy, Isx, Isy, usx_av, usy_av

// real-time
static double t=0.; //VS removed static due to clash with export in .h

// stator voltage components in alfa beta (inluding impact of the real dc-link voltage)
static double ualfa=0., ubeta=0.;

// PWM - improvement of dead-time effect emulation
static unsigned int start_pwm=1;
static int uact[3]={1,1,1};
static int ureq[3]={1,1,1};
static unsigned int DT_counter[3]={0,0,0};
static unsigned int DT_counter_mez;

// debug
static double debug_pwm;
FILE *fdebug;

/////////////////// POMOCNE FUNKCE //////////////////////////////////
double uhel(double x, double y)
{
  double th;

  if (x==0)
    if (y==0) th=0.;
	else if (y>0) th=M_PI/2.;
	     else th=-M_PI/2.;
  else
    th=atan(y/x);

  if (x<0) th+=M_PI;

  return th;
}
/////////////////////////////////////////////////////////////////////


void pmsmsim_set_parameters(double Rs0, double Ls0, double Fmag0, double Bf0, double p0, double kp0, double J0, double Uc0, double DT0, double dt0)
{
  int tmp_i;

  // simulator parameters setup
  Rs=Rs0;
  Ls=Ls0;
  Fmag=Fmag0;
  Bf=Bf0;
  p=p0;
  kp=kp0;
  J=J0;
  Ucn=600.;
  Uc=Uc0;
  DT=DT0;

  // control setup
  Urm_max=0.95;

  // simulator sampling - fixed setup
  h=dt0;
  h_reg=125e-6;         // fpwm = 4kHz
  h_reg_counter_mez=(int)(h_reg/h);         // emulation of operation of DSP timer
  //h_reg_counter=h_reg_counter_mez;
  h_reg_counter=1;
  h_reg_real=h_reg_counter_mez*h;           // real sampling period

  // reset of the system state variables
  for (tmp_i=0;tmp_i<9;tmp_i++)
    x[tmp_i]=0.;

  // emulation of the first measure
  Isx=0.;Isy=0.;theta=x[3];speed=x[2];

// === init of particular modules of simulator ===
  // PWM init
  smer=-1; smer2=-1;
  citac=0;
  citac2=abs(0-(int)(DT/h)); //VS: oprava, je to spravne?
  citac_PR=h_reg_counter_mez;
  DT_counter_mez = (int)(DT/h);

  // first interrupt occur after first period match => add 1 to both counter registers
  citac++;smer=1;
  citac2--;

  modulace=0;           // THIPWM
  if (modulace==1)
    U_modulace=Ucn/sqrt(3.);
  else
    U_modulace=Ucn/2.;

  // PMSM model init
  dIsx=0;dIsx1=0;dIsx2=0;dIsx3=0;dIsy=0;dIsy1=0;dIsy2=0;dIsy3=0;
  dTheta=0;dTheta1=0;dTheta2=0;dTheta3=0;
  dw=0;dw1=0;dw2=0;dw3=0;

  init_regulace(Ls,Fmag,kp,p,h_reg_real);
  
  // !!d!!
//  fdebug=fopen("data_debug.txt","w");
}


static void pwm(unsigned int mod)
// mod ... mod=0 - sinusoidal PWM; mod=1 - PWM with injected 3rd harmonic
{
  unsigned int i;
  double iabc[3], ur[3],ustr[3],ua,ub,uc;
  double dtr[3],dd[3];
  double Um, beta;
  double U3;
  double up, up2;

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

  // emulation of carrier - timer
  up=((double)citac/citac_PR-0.5)*Ucn;
    
  iabc[0]=*x;
  iabc[1]=(-*x+sqrt(3.)**(x+1))/2.;
  iabc[2]=(-*x-sqrt(3.)**(x+1))/2.;

  if (mod==0)	// sin. 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 with injected 3rd harmonic
  {
    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);
  }

  for (i=0;i<3;i++)
  { dtr[i]=ubytek(fabs(iabc[i]));
    dd[i]=dtr[i]*.73;
  }
  
  // mitigation of the voltage drops
/* for (i=0;i<3;i++)
 {
   dtr[i]=0.;
   dd[i]=0.;
 }
/**/ 
 
 // PWM compare emulation
  for (i=0;i<3;i++)
	  if (ur[i]>up)
		  ureq[i]=1;
      else
		  ureq[i]=-1;

  // First entrace into PWM - actual switching combination is initiated with required switching state
  if (start_pwm == 1)
  {	  
	  for (i=0;i<3;i++)
		  uact[i]=ureq[i];
	  start_pwm=0;
  }
  
  // Dead-time effect has no impact on switching combination, when switching from transistor to diode
  for (i=0;i<3;i++)
  {
	  if ((iabc[i]>0.5) && (uact[i]==1))
	    uact[i]=ureq[i];
	  	  
	  if ((iabc[i]<-0.5) && (uact[i]==-1))
	    uact[i]=ureq[i];
  }
/**/
    
/*  for (i=0;i<3;i++)
	  uact[i]=ureq[i];*/
  
  // Dead-time effect emulation
  for (i=0;i<3;i++)
    if ((uact[i]!=ureq[i]) && (DT_counter[i]<DT_counter_mez))
      DT_counter[i]++;
    else
	{
	  uact[i]=ureq[i];
	  DT_counter[i]=0;
	}
	
	// inverter phase voltage computation
	for (i=0;i<3;i++)
		ustr[i]=uact[i]*Uc/2.;
	
	// implementation of voltage drops
	for (i=0;i<3;i++)
	  if (iabc[i]>0)
		if (uact[i]==1)
		  ustr[i]-=dtr[i];
	    else
		  ustr[i]-=dd[i];
	  else
		if (uact[i]==-1)
		  ustr[i]+=dtr[i];
	    else
		  ustr[i]+=dd[i];
/**/  
  // phase voltages
  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.;

// voltage vector in stationary reference frame (x,y)
  *us=(2.*ua-ub-uc)/3.;
  *(us+1)=(ub-uc)/sqrt(3.);
  
  // sinusoidal inverter!!!!
//  *us=ur[0];
//  *(us+1)=(ur[1]-ur[2])/sqrt(3.);

  // emulation of DSP timers
  if ((citac==citac_PR)||(citac==0))
  {
    smer*=-1;
    // calculation of stator voltage components mean values
    usx_av=h/h_reg*sum_usx_av;
    usy_av=h/h_reg*sum_usy_av;
    // reset of sum accumulators
    sum_usx_av=0.;
    sum_usy_av=0.;

    // stator current components mean values - reference frame (alfa, beta)
    isx_av=h/h_reg*sum_isx_av;
    isy_av=h/h_reg*sum_isy_av;
    // reset of sum accumulators
    sum_isx_av=0.;
    sum_isy_av=0.;
  }
  if ((citac2==citac_PR)||(citac2==0)) smer2*=-1;
  citac+=smer;
  citac2+=smer2;

  // calculation of stator voltage components mean values - sum
  sum_usx_av+=*us;
  sum_usy_av+=*(us+1);

  // stator voltage components filtering
  //if (start_filter==1)
  usxf+=(*us-usxf)*h/h_reg;
  usyf+=(*(us+1)-usyf)*h/h_reg;

  // stator current components mean values - reference frame (alfa, beta)
  sum_isx_av+=*x;
  sum_isy_av+=*(x+1);
  
  debug_pwm = ur[0]; // !!!!
  // !!d!!
//  fprintf(fdebug,"%f %f %f \n",ustr[0],*us,x[0]);
}

static double ubytek(double I)
{
  unsigned int ii;
  double delta_u;

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

  if (ii==(pocet-1))
    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;
}


static void pmsm_model(unsigned int mod)
// mod<5...Euler, mod>4 ... Adams of 4th order
{
  double usx, usy;

  usx=*us;
  usy=*(us+1);

  dIsx=-Rs/Ls*x[0]+Fmag/Ls*x[2]*sin(x[3])+usx/Ls;
  dIsy=-Rs/Ls*x[1]-Fmag/Ls*x[2]*cos(x[3])+usy/Ls;
  dTheta=x[2];

  if (J>0)
    dw=kp*p*p*Fmag/J*(x[1]*cos(x[3])-x[0]*sin(x[3]))-Bf/J*x[2]-p/J*x[8];
  else
    dw=0;

  // integration
  if (mod<5)  // Euler
  { x[0]+=dIsx*h;
    x[1]+=dIsy*h;
    x[2]+=dw*h;
    x[3]+=dTheta*h;
  }
  else			// Adams (4th order)
  { x[0]+=h/24.*(55.*dIsx-59.*dIsx1+37.*dIsx2-9.*dIsx3);
    x[1]+=h/24.*(55.*dIsy-59.*dIsy1+37.*dIsy2-9.*dIsy3);
    x[2]+=h/24.*(55.*dw-59.*dw1+37.*dw2-9.*dw3);
    x[3]+=h/24.*(55.*dTheta-59.*dTheta1+37.*dTheta2-9.*dTheta3);
  }

  // saturation of theta to (-pi,pi)
  if (x[3]>M_PI) x[3]-=(2*M_PI);
  if (x[3]<-M_PI) x[3]+=(2*M_PI);

  // diff. shift - Adams
  dIsx3=dIsx2;dIsx2=dIsx1;dIsx1=dIsx;
  dIsy3=dIsy2;dIsy2=dIsy1;dIsy1=dIsy;
  dTheta3=dTheta2;dTheta2=dTheta1;dTheta1=dTheta;
  dw3=dw2;dw2=dw1;dw1=dw;

  // calculation of Isd, Isq
  x[6]=x[0]*cos(x[3])+x[1]*sin(x[3]);         // Isd
  x[7]=x[1]*cos(x[3])-x[0]*sin(x[3]);         // Isq

  // Fsd ... d-component of stator flux
  x[5]=Ls*x[6]+Fmag;

  // Torque
  x[4]=kp*p*Fmag*(x[1]*cos(x[3])-x[0]*sin(x[3]));
}

//////////////////////////////////////////////////////////////////////////////////////////////////////
void pmsmsim_step(double Ww, double Mz)            // you must link array KalmanObs[] to EKF modul
{
  double Umk, ub, uc;

//  while (t<=t_end)
  {
    pwm(modulace);
//    *us=KalmanObs[0]; *(us+1)=KalmanObs[1];
//     *us=ualfa; *(us+1)=ubeta;
	//Mz
	x[8]= Mz;
	
    pmsm_model(5);

    if (h_reg_counter>=h_reg_counter_mez)           // pocatek ISR
    {
      // voltages and measured currents for EKF
//       Umk=*u*Uc/Ucn;
//       ualfa=Umk*cos(*(u+1));
//       ub=Umk*cos(*(u+1)-2./3.*M_PI);
      KalmanObs[0]=ualfa; //debug_pwm;                     // usx
      //KalmanObs[1]=(ualfa+2.*ub)/sqrt(3.);    // usy
      KalmanObs[1]=ubeta;    // usy
      
      // real sampling - considered transport delay equal to the sampling period
/*     KalmanObs[2]=Isx;
     KalmanObs[3]=Isy;*/
      // ideal sampling
      KalmanObs[2]=x[0];
      KalmanObs[3]=x[1];
      
      // diagnostic - mean values of stator voltage components - pwm()
      KalmanObs[4]=usx_av;
      KalmanObs[5]=usy_av;
      KalmanObs[6]=usxf;
      KalmanObs[7]=usyf;
      KalmanObs[8]=isx_av;
      KalmanObs[9]=isy_av;      

       vektor_regulace(0,0,Urm_max,Ww,u,Isx,Isy,theta,speed,U_modulace,Uc,Ucn,REZIM_REGULACE);	// rezim=1 ... reg. rychlosti, rezim=0 ... reg. momentu
       									                                        // rezim=2 ... Iqw=sqrt(Imax^2-Idw^2)
      
/*	  *u=2*15.0;
	  *(u+1)+=2*0.00157;*/
// emulation of the real sampling of A/D converter
      Isx=x[0];Isy=x[1];speed=x[2];theta=x[3];

      // include ideal commanded stator voltage 
      Umk=*u*Uc/Ucn;// !!!!
      ualfa=Umk*cos(*(u+1));		   // usx = usa
      ub=Umk*cos(*(u+1)-2./3.*M_PI);
      ubeta=(ualfa+2.*ub)/sqrt(3.);    // usy
//       uc=-ualfa-ub;
//       ubeta=(ub-uc)/sqrt(3.);

      h_reg_counter=0;
    }

    t+=h;
    h_reg_counter++;
  }
}

void pmsmsim_noreg_step(double ua, double ub)            // you must link array KalmanObs[] to EKF modul
{
  double kor_Uc;

   *u=sqrt(ua*ua+ub*ub);
   *(u+1)=uhel(ua,ub);
  
/*  *u=5.0;
  *(u+1)+=0.00157;*/
  // uprava velikosti vzhledem k Uc!=Ucn
//  kor_Uc=Ucn/230.;
//  *u*=kor_Uc; /**/

  pwm(modulace);

//  *us=*u*cos(*(u+1));
//  *(us+1)=*u*sin(*(u+1));;

  pmsm_model(5);

  // TODO - check ZP
  double Umk=*u*Uc/Ucn;// !!!!
  ualfa=Umk*cos(*(u+1));		   // usx = usa
  ub=Umk*cos(*(u+1)-2./3.*M_PI);
  ubeta=(ualfa+2.*ub)/sqrt(3.);    // usy
  
  
//  KalmanObs[0]=Umk*cos(*(u+1));		   // usx = usa
//  KalmanObs[1]=(KalmanObs[0]+2.*ub)/sqrt(3.);    // usy

  KalmanObs[0]=ualfa;//Umk*cos(*(u+1));		   // usx = usa
  KalmanObs[1]=ubeta;//(KalmanObs[0]+2.*Umk*cos(*(u+1)-2./3.*M_PI))/sqrt(3.);    // usy
  
  KalmanObs[2]=x[0];
  KalmanObs[3]=x[1];
  
  // diagnostic - mean values of stator voltage components - pwm()
  KalmanObs[4]=usx_av;
  KalmanObs[5]=usy_av;
  KalmanObs[6]=usxf;
  KalmanObs[7]=usyf;
 
  t+=h;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////

// for mexfile
void pmsmsim_fill_parameters(double x_out[10]){
	x_out[0]= Rs;
	x_out[1]= Ls;
	x_out[2]= Fmag;
	x_out[3]= Bf;
	x_out[4]= p;
	x_out[5]= kp;
	x_out[6]= J;
	x_out[7]= Uc;
	x_out[8]= DT;
	x_out[9]= h;
}
void pmsmsim_fill_xy(double xout[9], double yout[6]){
	for (int i=0;i<9;i++){
		xout[i]=x[i];
	}
	for (int i=0;i<6;i++){
		yout[i]=KalmanObs[i];
	}
}
double pmsmsim_get_t()
{
 return t;
}