#define ADC_OUT  0          // ulaz feedbacka visokog AC napona

#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))

#define NPWM  200         
//#define PPWM  665       // 60.062 Hz - izaberi ovaj, ili ovaj ispod. nikako oba.
#define PPWM  799         // 50.002 Hz 799  49.939 Hz za 800

      
uint8_t uf,oen,v1low, lv_stopped,int_f;
uint16_t pcount;
uint32_t vpwr;
int sst;
uint16_t l[NPWM+1];
float pwr;
uint8_t dbounce;
double Input, Output, Setpoint;
double errSum, lastErr,error,dErr;
double kp, ki, kd;
float xv[5],yv[5];
float ac_output, ac_setpoint;

void init_vars(void);

long pcount_acc;
int pcount_delta, phase_error, old_phase_error;
int pcint,new_delta;

uint8_t init_done; 

void setup()
  {
  int i;
  float t,u;

  for(t=0.0,i=0; i <= NPWM; i++,t +=   3.14159/ (float) (NPWM+1))
    {   
    u = 65535.0 * sin(t);         // 1/2 sinusne lookup table, skalirana na (16 bits - 1)
    l[i] =  (int)u;  
    }
 
  pinMode(9, OUTPUT);             // V1 Spwm
  pinMode(10, OUTPUT);            // V2 Spwm
  pinMode(8,INPUT);               // uklj/isklj pin
  
// u daljem kodu ne smem dozvoliti da arduino poziva pinMode() f-ju.. utvrdio sam da zeza d5 koji mora biti niske impedanse po butovanju...
  DDRD = 0xfb;                // postavi D2 kao ulaz INT0 
  pinMode(2,INPUT_PULLUP);
  noInterrupts();

  TCCR1A = _BV(COM1A1) | _BV(COM1B0) | _BV(COM1B1) | _BV(WGM11);
  TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS10);
  OCR1A = 10;
  ICR1 = PPWM;
  TIMSK1 |= (1 << TOIE1);

  EICRA = 3;               // interrupt na silaznoj ivici signala D2
  EIMSK |= bit (INT0);     // uključi ga  
  cbi(PORTD,4);            // nema sinh.
  cbi(PORTD,5);           // isključi izlaze spwm. 
  oen = 0;                // uklj/isklj invertora. = 1 za uklj, = 0 za isklj
  init_vars();
  init_done = 1;
  
  interrupts();             // omomgući interapte
  TIMSK0 = 0;               // osim arduino clock-a

  sbi(ADCSRA,ADPS2) ;       // ADC clock priskejler = 16 sad. štedimo 100us u glavnom loop-u uz sitna odricanja
  cbi(ADCSRA,ADPS1) ;
  cbi(ADCSRA,ADPS0) ;  

  Serial.begin(115200);
  }

//
// kod za sync. 
// ovo se mora brzo izvršavati
// trebalo bi za manje od 6uS
// kreće na početku poluperiode, gde će PWM duty% uvek biti mali
// otprilike na prolasku kroz nulu.  6uS turbulencije će biti vrlo male, možda neprimetne.
//

ISR (INT0_vect)
{
if  (int_f == 0) 
{
  sbi(PORTD,7);
  cbi(PORTD,7);
  return;  // 
}
int_f = 0;
//sbi(PORTD,7);
//cbi(PORTD,7);
pcint = pcount +  (v1low == 1?200:0) - 200;   // nađi poziciju unutar periode, -200 to 200 
                                              
phase_error = (pcint - 0);    
                              
new_delta =  (phase_error/4 + (phase_error - old_phase_error)); 
if (new_delta > 10) 
  {
    
    new_delta = 10;       
  }
if (new_delta < -10) 
  {
    
    new_delta = -10;     // trebalo bi da se usyncuje unutar pola sekunde    
  }
pcount_delta = pcount_delta - new_delta;  // primeni ono što si pronašao na generisani spwm
if(pcount_delta < 200) pcount_delta = 200;  
if(pcount_delta > 300) pcount_delta = 300;  
old_phase_error = phase_error;
if (phase_error < 3)
   sbi(PORTD,4);              // upali led ako već misliš da si u sync.
}  

//
// 20khz pwm. ovaj kod se mora izvršiti unutar 50 mikrosec. mislim da se izvršava unutar 10mikrosec.
//
ISR(TIMER1_OVF_vect)        
 {   
  long c;
  c = (l[pcount] * vpwr) >> 16;   // reskaliraj vrednosti sinusoide
  if (v1low == 1)                 // svičuj između dva komparatora. generiši obe poluperiode.
    OCR1B = c;                    //  1/2 ovde
  else
    OCR1A = c;                    // i još 1/2 ovde.
  pcount_acc += pcount_delta;     // broji kroz 1/2 snusoide, 200 unosa. Na 20KHz, = 50Hz izlaz 
  pcount = (pcount_acc >> 8);     // dodaj mogućnost variranja +-8Hz.
  if(pcount >= NPWM)          
    {
    pcount = 0;               // resetuj brojac
    pcount_acc = 0;           
    uf=1;                     
    if (v1low == 1)           // ako 1/2 ..
      {
       v1low = 0;               // onda druga polovina half wave-a
       TCCR1A =  _BV(COM1A1) | _BV(WGM11); 
       //sbi(PORTD,7);          // sync za osciloskop ko voli :)   
       //cbi(PORTD,7); 
       if (int_f == 1)            // nema ulaznog sync napona ?
        {
          cbi(PORTD,4);         // reci ljudima nema sync-a
          pcount_delta = 256;   // ako nema synca ili nikad nije ni uspostavljen - vidi jel sve u redu sa f-jom izlaza.
        }
       int_f = 1;               // par stvari da osigura da ovaj kod prvi ide
      }                         // i da ga ne ometaju bilo kakvi interapti
    else
      {
       v1low = 1;                             // prebaci na prvi half wave
       TCCR1A =  _BV(COM1B1) | _BV(WGM11);    // konf O.C. da paše....
      }
    }
}


void init_vars()      
  {
  ICR1 = PPWM;
  lv_stopped = 0;  
  sst = 0;      // soft start 0 - 251
  v1low = 1;    // fleg koji pokazuje đe smo, u prvoj ili drugoj poluperiodi, 0 or 1
  pcount = 0;   // brojač lookup tabele
  uf = 0;       // nešto oko feedbacka
  pwr = 0.0;    // PWM duty , 0.0 do 1.0
  vpwr= 0;      // integer ekvivalent snage, 0 do PPWM
  dbounce = 0;  // 
  kp = 0.1;     // setup PID za AC izlaz 
  ki = 0.0;
  kd = 0.01;
  errSum = 0.0;
  lastErr = 0.0;
  int_f = 0;          
  pcount_acc = 0;      
  pcount_delta = 256;  
  old_phase_error = 0; 
  cbi(PORTD,4);  
  }


void do_pid()
{   
   double timeChange = 0.01;    
   error = Setpoint - Input;
   errSum += (error * timeChange);
   dErr = (error - lastErr) / timeChange;
   Output = kp * error + ki * errSum + kd * dErr;
   lastErr = error;
}


void loop()
{
  float ch0;
  ch0 = (float)analogRead(ADC_OUT) / 1024.0; // reskaliraj 0-5V na 0.0-1.0V
  // filtriraj AC izlaz , Fc = 10, 4 pole Bessel LP
  // pokupio od  https://www-users.cs.york.ac.uk/~fisher/mkfilter/trad.html
  xv[0] = xv[1]; xv[1] = xv[2]; xv[2] = xv[3]; xv[3] = xv[4];
  xv[4] = ch0 / 2.259747797e+05;
  yv[0] = yv[1]; yv[1] = yv[2]; yv[2] = yv[3]; yv[3] = yv[4];
  yv[4] =   (xv[0] + xv[4]) + 4 * (xv[1] + xv[3]) + 6 * xv[2]
                     + ( -0.7428578687 * yv[0]) + (  3.1954036268 * yv[1])
                     + ( -5.1599230905 * yv[2]) + (  3.7073065280 * yv[3]);
  ac_output = yv[4];

  if(uf == 1  )                   // ovo se izvršava na 100Hz
      {                              // radi kad prolazi sinusoida kroz nulu       
      uf=0; 
      ac_setpoint = 0.56;           // pravimo da bude kompatibilno Vfb kao na EG8010. 
      if (oen == 1) sst++;          // slow start. 
      if (sst > 251)sst = 251;   
      if (oen == 0) sst--;         // ako se gasi invertor, isto sporo gasi
      if (sst <= 0)             
        {
        sst = 0;
        cbi(PORTD,5);            // pull down IR2184 shutdown LOW da bi IC shvatio da nećemo sad spwm
        }
      else
        sbi(PORTD,5);            // pull HIGH, kao gore samo obrnuto

      if(sst > 0 && sst < 250)      
        {                          
        if (oen == 1)              
          { 
          if(ac_output < ac_setpoint)
            pwr += 1.0/250.0;      
          else                     
            pwr -= 1.0/250.0;      
          }
        else
          pwr -= 1.0 /250.0;      
        }
        
      if (sst == 251)       
        {                   
        Setpoint = ac_setpoint;     
        Input = ac_output;  
        do_pid();
        pwr = pwr + Output;
        }
        
      if (pwr > 0.99) pwr = 0.99;       
      if (pwr < 0.01) pwr = 0.01;       
      vpwr = (int)((float)PPWM * pwr);     

      check_switch_cont();
      
      Serial.print((pcount_delta-256)*10);      
      Serial.print(" -150 150 ");
      Serial.println(pcint*10);
      
      }
}

void check_switch_cont()
{   
  if ((PINB & 0x01) == 0x01)     
    {
    if (init_done == 0)
      {
      init_vars();
      init_done = 1;
      }  
    oen = 1;                                                    // startuj polako
    }
      
  if (((PINB & 0x01) == 0) || ((PIND & 0x40) == 0x40))     
    {
    init_done = 0;
    oen = 0;                                                   // stopiraj polako
    }
          
}