/*
   Main simulation module

   Simulation of PMSM drive ... floating point

   Illustration demo for object design - project EKF and PF

   Z. Peroutka

REV. 16.3.2008

*/

#include <stdio.h>
#include <math.h>
#include "regulace.h"
#include "simulator.h"

static double param[7]={0.28,0.003465,0.1989,0.0,4,1.5,0.04};
// [Rs, Ls, Fmag, Bf, p, kp, J = 0.04?];

double h, t, t_end, t_sense, h_disp=0;

static int print_counter=0;
static double Ww;

static int k_rampa=1;
static long k_rampa_tmp=0;

FILE *fw;

int main(void)
{
  h=1e-6;

  t_end=4.;    // profil Isq 5s, rozbeh 12s, rozbeh +/- 20s, reverzace 12s
  t_sense=0.0; // 1.
  t=0.;

  fw=fopen("data\\graf1.txt","w");

  set_parameters(*param,*(param+1),*(param+2),*(param+3),*(param+4),*(param+5),*(param+6), 200., 3e-6, h);

  Ww=0.;

  while (t<=t_end)
  {

    Ww+=k_rampa*2.*M_PI*2e-4;    //1000Hz/s
    if (Ww>2.*M_PI*150.) {Ww=2.*M_PI*150.; if (k_rampa_tmp<500000) k_rampa_tmp++; else {k_rampa=-1;k_rampa_tmp=0;}}
    if (Ww<-2.*M_PI*150.) Ww=-2.*M_PI*150.;   /* */

    eval(Ww);

    if (t>=t_sense)
      if (print_counter>199)
      { fprintf(fw,"%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f \n",t,x[0],x[1],x[6],*ladeni_regulace,x[7],*(ladeni_regulace+2),x[4],x[5],x[3],x[2],Ww,*(ladeni_regulace+1),0.95,*(ladeni_regulace+4),*(ladeni_regulace+5),x[10],*(ladeni_regulace+6),200., *(ladeni_regulace+7),0,0,*(ladeni_regulace+8),*(ladeni_regulace+9));
      // t, Isx, Isy, Isd, Isdw, Isq, Isqw, M, Fs, poloha, rychlost, rychlost_w, Urm, Urm_max,
      // Fs_model, M_model, zatezny uhel (beta), zatezny uhel vypocteny z napeti (zbeta), Uc
        print_counter=0;
      }

    if (h_disp>0.2)
    { printf("%f \n",t);
      h_disp=0;
    }

    t+=h;
    h_disp+=h;
    print_counter++;
  }

  fclose(fw);
  return 0;
 }