diff --git a/atintegrators/BeamLoadingCavityPass.c b/atintegrators/BeamLoadingCavityPass.c
index f3f7f30a97..5642550a0f 100644
--- a/atintegrators/BeamLoadingCavityPass.c
+++ b/atintegrators/BeamLoadingCavityPass.c
@@ -1,7 +1,9 @@
 #include "atconstants.h"
 #include "atelem.c"
 #include "atimplib.c"
+#include "atfeedbacklib.c"
 #include "attrackfunc.c"
+#include <complex.h>
 
 /*
  * BeamLoadingCavity pass method by Simon White.  
@@ -16,16 +18,16 @@ struct elem
   int fbmode;
   int buffersize;
   int windowlength;
+  int openloop;
   double normfact;
-  double phasegain;
-  double voltgain;
+  double tunergain;
+  double *gain;
   double *turnhistory;
   double *z_cuts;
   double Length;
   double Voltage;
   double Energy;
   double Frequency;
-  double HarmNumber;
   double TimeLag;
   double Qfactor;
   double Rshunt;
@@ -42,7 +44,30 @@ struct elem
   double *vbunch_buffer;
   int system_harmonic;
   double ts;
-}; 
+  int every;
+  int delay;
+  int samplenum;
+  int ff;
+  double cutoff;
+  int recordsize;
+  double *Ig2Vg_vec;
+  double *Ig2Vg_tmp;
+  double *ig_phasor;
+  double *ig_phasor_record;
+  double *dot_output;
+  double *generator_phasor_record;
+  double *beam_phasor_record;
+  double *cavity_phasor_record;
+  double *Ig2Vg_mat;
+  double *vc_previous;
+  double *diff_record;
+  double *samplelist;
+  double *vc_list;
+  double *I_record;
+  double *FFconst;
+  double *IIRout;
+  double *IIRcoef;
+  }; 
 
 
 void write_buffer(double *data, double *buffer, int datasize, int buffersize){
@@ -58,6 +83,7 @@ void BeamLoadingCavityPass(double *r_in, int num_particles, int nbunch,
                            double *fillpattern,
                            double circumference,
                            int nturn, double energy, int harmonic_number,
+                           int iturn,
                            struct elem *Elem) {
   
     long cavitymode = Elem->cavitymode;
@@ -71,57 +97,129 @@ void BeamLoadingCavityPass(double *r_in, int num_particles, int nbunch,
     double normfact = Elem->normfact;  
     double le = Elem->Length;
     double rffreq = Elem->Frequency;
-    double harmn = Elem->HarmNumber;
+    int harmn = rffreq * circumference / C0 ;    // cavity harmonic number
+    int open = Elem->openloop;
+    
     int ring_harmn = harmonic_number;
     double tlag = Elem->TimeLag;
     double qfactor = Elem->Qfactor;
     double rshunt = Elem->Rshunt;
     double beta = Elem->Beta;
-    double phasegain = Elem->phasegain;
-    double voltgain = Elem->voltgain;
+    double tunergain = Elem->tunergain;
+    //if fb mode is PROP then gain[0] is Voltgain and gain[1] is PhaseGain
+    //if fb mode is PROP_INTEGRAL then gain[0] is Prop gain and gain[1] is integral gain
+    double *gain = Elem->gain;
+
     double ts = Elem->ts;
-    
+    double *vgen_arr = Elem->vgen; /* [vgen, thetag, psi, vgr] */
+        
     double *turnhistory = Elem->turnhistory;
     double *vgen_buffer = Elem->vgen_buffer;
     double *vbeam_buffer = Elem->vbeam_buffer;
     double *vbunch_buffer = Elem->vbunch_buffer;
     
+    
+    size_t sztmp1 = sizeof(double)*buffersize*3; //vgen, theta_g, psi
+    void *set_params = atMalloc(sztmp1); // This is a buffer of the actual params to set. Good for PID
+    
+    double cutoff = Elem->cutoff;  
+    int delay = Elem->delay; 
+    int every = Elem->every; 
+    int FF = Elem->ff; 
+    int samplenum = Elem->samplenum; 
+    int record_size = Elem->recordsize;
+    int samplelist_length = ring_harmn/every + 1;
+    printf("AAAAAAAAAA %d \n", samplelist_length);
+    /* Here we have to declare empty pointers for the PI Loop
+    They have to be defined outside of an if statement*/
+    
+    double *Ig2Vg_vec_real = Elem->Ig2Vg_vec;
+    double *Ig2Vg_vec_imag = Elem->Ig2Vg_vec + ring_harmn;
+    double *Ig2Vg_tmp_real = Elem->Ig2Vg_tmp;
+    double *Ig2Vg_tmp_imag = Elem->Ig2Vg_tmp + ring_harmn;
+    double *ig_phasor_real = Elem->ig_phasor;
+    double *ig_phasor_imag = Elem->ig_phasor + ring_harmn;
+    double *ig_phasor_record_real = Elem->ig_phasor_record;
+    double *ig_phasor_record_imag = Elem->ig_phasor_record + ring_harmn;
+    double *dot_output_real = Elem->dot_output;
+    double *dot_output_imag = Elem->dot_output + ring_harmn;
+                      
+
+    double *generator_phasor_record_real = Elem->generator_phasor_record;
+    double *generator_phasor_record_imag = Elem->generator_phasor_record + ring_harmn;
+    double *beam_phasor_record_real = Elem->beam_phasor_record;
+    double *beam_phasor_record_imag = Elem->beam_phasor_record + ring_harmn;
+    double *cavity_phasor_record_real = Elem->cavity_phasor_record;
+    double *cavity_phasor_record_imag = Elem->cavity_phasor_record + ring_harmn;
+
+    double *Ig2Vg_mat_real = Elem->Ig2Vg_mat;
+    double *Ig2Vg_mat_imag = Elem->Ig2Vg_mat + ring_harmn*ring_harmn;
+
+    
+    double *vc_previous_real = Elem->vc_previous; 
+    double *vc_previous_imag = Elem->vc_previous + samplenum;
+    double *diff_record_real = Elem->diff_record; 
+    double *diff_record_imag = Elem->diff_record + record_size;
+    double *samplelist = Elem->samplelist;
+    double *vc_list_real = Elem->vc_list; 
+    double *vc_list_imag = Elem->vc_list + ring_harmn + samplenum;
+    
+    //double *Ig_modulation_signal_real; double *Ig_modulation_signal_imag; 
+
+    double *I_record = Elem->I_record;            
+    double *FFconst = Elem->FFconst;
+    double *IIRout = Elem->IIRout;
+    double *IIRcoef = Elem->IIRcoef;
+
     double *z_cuts = Elem->z_cuts;
     double *vbunch = Elem->vbunch;
-    double *vbeam_phasor = Elem->vbeam_phasor;
     double *vbeam = Elem->vbeam;
     double *vcav_set = Elem->vcav; /* Vcav set points amplitude, phase */
-
+    double *vbeam_phasor = Elem->vbeam_phasor;
     double feedback_angle_offset = Elem->feedback_angle_offset;
     
+    
+        
+        
     double vbeam_set[] = {vbeam[0], vbeam[1]};
+    double vcav_meas[] = {0.0, 0.0, 0.0};
     double ave_vbeam[] = {0.0, 0.0};
     double tot_current = 0.0;
+
     
     int i;
     size_t sz = nslice*nbunch*sizeof(double) + num_particles*sizeof(int);
     int c;
     int *pslice;
     double *vbeam_kicks; /* This used to be kz, it is the kick that is applied */
-    double *vgen_arr = Elem->vgen; /* [vgen, thetag, psi, vgr] */
+
     
     double vgen = vgen_arr[0];
     double gen_phase = vgen_arr[1];
-
-    double delta = pow(rffreq * tan(vgen_arr[2]) / qfactor, 2) + 4 * pow(rffreq,2);
-    double freqres = (rffreq * tan(vgen_arr[2]) / qfactor + sqrt(delta)) / 2;
+    double psi = vgen_arr[2];
+    double delta = pow(rffreq * tan(psi) / qfactor, 2) + 4 * pow(rffreq,2);
+    double freqres = (rffreq * tan(psi) / qfactor + sqrt(delta)) / 2;
 
     double tot_lag_phase = (tlag+ts)*rffreq*TWOPI/C0;
-    
+    double filling_time = 2*qfactor / (TWOPI * freqres);
+    double T1 = 1/rffreq;
+    double kloss = rshunt * TWOPI * freqres / (2 * qfactor);
+
+    double vcav_phasor[] = {0.0, 0.0}; 
+    set_cavity_phasor(vgen, gen_phase, vbeam_phasor, vcav_phasor);
+    printf("starting cav phasor %f \t %f \n", vcav_phasor[0], vcav_phasor[1]);
+
     for(i=0;i<nbunch;i++){
         tot_current += bunch_currents[i];
     }
-        
-        
+    
+
+    
     /*Track RF cavity is always done. */
     trackRFCavity(r_in, le, vgen/energy, rffreq, harmn, tlag, -gen_phase - tot_lag_phase, nturn, circumference/C0, num_particles);
+    
     /*Only allocate memory if current is > 0*/
-    if(tot_current>0){
+    if(tot_current>0 && rshunt > 0){
         void *buffer = atMalloc(sz);
         
         double *dptr = (double *) buffer;
@@ -136,9 +234,9 @@ void BeamLoadingCavityPass(double *r_in, int num_particles, int nbunch,
                     bunch_currents, turnhistory, pslice, z_cuts);
         compute_kicks_phasor(nslice, nbunch, nturnsw, turnhistory, normfact, vbeam_kicks,
                              freqres, qfactor, rshunt, vbeam_phasor, circumference, energy,
-                             beta, ave_vbeam, vbunch, bunch_spos, ring_harmn, fillpattern, ts);                        
-
-                
+                             beta, ave_vbeam, vbunch, bunch_spos, ring_harmn, fillpattern, ts);             
+                                        
+        
         /*apply kicks*/
         for (c=0; c<num_particles; c++) {
             double *r6 = r_in+c*6;
@@ -148,27 +246,105 @@ void BeamLoadingCavityPass(double *r_in, int num_particles, int nbunch,
             }
         }
         
+               
         // First write the values to the buffer
         if(buffersize>0){
             write_buffer(vbeam, vbeam_buffer, 2, buffersize);
             write_buffer(vgen_arr, vgen_buffer, 4, buffersize);
-            write_buffer(vbunch, vbunch_buffer, 2*nbunch, buffersize);
+            write_buffer(vbunch, vbunch_buffer, 2*ring_harmn, buffersize);
         }   
 
         update_vbeam_set(fbmode, vbeam_set, ave_vbeam, vbeam_buffer,
                              buffersize, windowlength);
-        
+                             
+        compute_set_params(vbeam_set, vgen_arr, feedback_angle_offset, vcav_set[1], vcav_meas);
+
         if(cavitymode==1){
-            update_vgen(vbeam_set, vcav_set, vgen_arr, voltgain, phasegain, feedback_angle_offset); 
+            // If CavityMode=ACTIVE
+            if(tunergain>0){
+                vgen_arr[2] += tunergain * (vcav_meas[2] - vgen_arr[2]);
+            }
+            if(fbmode==1){
+                // If FBMode=PROP
+                update_vgen(vcav_set, vgen_arr, vcav_meas, gain[0], gain[1], feedback_angle_offset);
+            }
+            if(fbmode==2){
+                if(iturn==0){
+                    init_sample_list(samplelist, ring_harmn, every, samplelist_length); 
+                             
+                    init_phasor_arrays(vgen, gen_phase,
+                                       ig_phasor_real, ig_phasor_imag,
+                                       ig_phasor_record_real, ig_phasor_record_imag,
+                                       ring_harmn, rshunt, psi,
+                                       generator_phasor_record_real, generator_phasor_record_imag);
+
+                    init_IIR(cutoff, IIRcoef, IIRout, T1, every, vcav_set[0]);
+                    
+                    init_FFconst(FF,
+                                 ig_phasor_real, ig_phasor_imag,
+                                 ring_harmn, FFconst);
+                                 
+                    init_Ig2Vg_matrix(ring_harmn,
+                                      Ig2Vg_vec_real, Ig2Vg_vec_imag,
+                                      Ig2Vg_tmp_real, Ig2Vg_tmp_imag,
+                                      filling_time, psi, T1, 
+                                      Ig2Vg_mat_real, Ig2Vg_mat_imag);
+
+                    I_record[0] = 0.0; I_record[1] = 0.0;
+
+
+                    set_cavity_phasor(vgen, gen_phase, ave_vbeam, vcav_phasor);
+
+                    init_vc_previous(vc_previous_real, vc_previous_imag, samplenum, vcav_phasor);    
+
+                };
+
+                init_cavity_record_phasor_array(vbunch,
+                                                beam_phasor_record_real, beam_phasor_record_imag,
+                                                cavity_phasor_record_real, cavity_phasor_record_imag,
+                                                generator_phasor_record_real, generator_phasor_record_imag,
+                                                ring_harmn); 
+
+                if(iturn>=1 && tunergain>0){
+                    init_Ig2Vg_matrix(ring_harmn,
+                                      Ig2Vg_vec_real, Ig2Vg_vec_imag,
+                                      Ig2Vg_tmp_real, Ig2Vg_tmp_imag,
+                                      filling_time, psi, T1,
+                                      Ig2Vg_mat_real, Ig2Vg_mat_imag);
+                }
 
+                printf("1,2,3,4,5 %d \t %d \t %d \t %d \n", (int)samplelist[0], (int)samplelist[1], (int)samplelist[2], (int)samplelist[3]);
+                track_PIL(vc_previous_real, vc_previous_imag,
+                          cavity_phasor_record_real, cavity_phasor_record_imag,
+                          ig_phasor_real, ig_phasor_imag,
+                          samplelist, samplenum, record_size, samplelist_length,
+                          diff_record_real, diff_record_imag,
+                          FFconst, gain, I_record,
+                          rffreq,
+                          vcav_set[0], vcav_set[1],
+                          generator_phasor_record_real, generator_phasor_record_imag,
+                          Ig2Vg_vec_real, Ig2Vg_vec_imag,
+                          Ig2Vg_mat_real, Ig2Vg_mat_imag,
+                          ig_phasor_record_real, ig_phasor_record_imag,
+                          dot_output_real, dot_output_imag,
+                          kloss, T1, ring_harmn, vgen_arr,
+                          IIRout, IIRcoef,
+                          vc_list_real, vc_list_imag,
+                          every,
+                          psi, rshunt,
+                          open
+                          );    
+
+            }
         }else if(cavitymode==3){     
-            update_passive_frequency(vbeam_set, vcav_set, vgen_arr, phasegain);
+            update_passive_frequency(vbeam_set, vcav_set, vgen_arr, tunergain);
         }
 
         vbeam[0] = ave_vbeam[0];
         vbeam[1] = ave_vbeam[1];
         atFree(buffer);
     }
+    
 }
 
 
@@ -178,30 +354,47 @@ ExportMode struct elem *trackFunction(const atElem *ElemData,struct elem *Elem,
 {
     double rl = Param->RingLength;
     double energy;
-    int nturn=Param->nturn;
+    int nturn = Param->nturn;
     if (!Elem) {
-        long nslice,nturns,cavitymode,fbmode, buffersize, windowlength, system_harmonic;
-        double wakefact;
-        double normfact, phasegain, voltgain;
+        long nslice, nturns, cavitymode, fbmode, buffersize, windowlength, system_harmonic;
+        long delay, every, samplenum, ff, recordsize, openloop;
+        double wakefact, Energy, Frequency, TimeLag, Length, feedback_angle_offset;
+        double normfact, tunergain, qfactor, rshunt, beta, phis, ts, cutoff;
+        double *gain;
         double *turnhistory;
         double *vgen_buffer;
         double *vbeam_buffer;
         double *vbunch_buffer;
         double *z_cuts;
-        double Energy, Frequency, TimeLag, Length, feedback_angle_offset;
-        double qfactor,rshunt,beta;
         double *vbunch;
         double *vbeam_phasor;
         double *vbeam;
         double *vgen;
         double *vcav;
-        double phis;
-        double ts;
 
+        double *Ig2Vg_vec;
+        double *Ig2Vg_tmp;
+        double *ig_phasor;
+        double *ig_phasor_record;
+        double *dot_output;
+        double *generator_phasor_record;
+        double *beam_phasor_record;
+        double *cavity_phasor_record;
+        double *Ig2Vg_mat;
+        double *vc_previous;
+        double *diff_record;
+        double *samplelist;
+        double *vc_list;
+        double *I_record;
+        double *FFconst;
+        double *IIRcoef;
+        double *IIRout;
+        
         /*attributes for RF cavity*/
         Length=atGetDouble(ElemData,"Length"); check_error();
         Frequency=atGetDouble(ElemData,"Frequency"); check_error();
         TimeLag=atGetOptionalDouble(ElemData,"TimeLag",0); check_error();
+        
         /*attributes for resonator*/
         nslice=atGetLong(ElemData,"_nslice"); check_error();
         nturns=atGetLong(ElemData,"_nturns"); check_error();
@@ -214,8 +407,10 @@ ExportMode struct elem *trackFunction(const atElem *ElemData,struct elem *Elem,
         rshunt=atGetDouble(ElemData,"Rshunt"); check_error();
         beta=atGetDouble(ElemData,"_beta"); check_error();
         normfact=atGetDouble(ElemData,"NormFact"); check_error();
-        phasegain=atGetDouble(ElemData,"PhaseGain"); check_error();
-        voltgain=atGetDouble(ElemData,"VoltGain"); check_error();
+        
+        gain=atGetDoubleArray(ElemData,"Gain"); check_error();
+        tunergain=atGetDouble(ElemData,"TunerGain"); check_error();
+        
         turnhistory=atGetDoubleArray(ElemData,"_turnhistory"); check_error();
         vbunch=atGetDoubleArray(ElemData,"_vbunch"); check_error();
         vbeam=atGetDoubleArray(ElemData,"_vbeam"); check_error();
@@ -225,10 +420,41 @@ ExportMode struct elem *trackFunction(const atElem *ElemData,struct elem *Elem,
         vgen_buffer=atGetDoubleArray(ElemData,"_vgen_buffer"); check_error();
         vbeam_buffer=atGetDoubleArray(ElemData,"_vbeam_buffer"); check_error();
         vbunch_buffer=atGetDoubleArray(ElemData,"_vbunch_buffer"); check_error();
+        
         phis=atGetDouble(ElemData,"_phis"); check_error();
         system_harmonic=atGetLong(ElemData,"system_harmonic"); check_error();
         ts=atGetDouble(ElemData,"_ts"); check_error();
-        /*optional attributes*/
+        
+        openloop=atGetLong(ElemData,"OpenLoop"); check_error();
+       
+        /*optional attributes*/        
+        delay=atGetOptionalLong(ElemData,"delay",1); check_error();
+        every=atGetOptionalLong(ElemData,"every",1); check_error();
+        samplenum=atGetOptionalLong(ElemData,"samplenum",1); check_error();        
+        cutoff=atGetOptionalDouble(ElemData,"cutoff",0); check_error();        
+        ff=atGetOptionalLong(ElemData,"FF",1); check_error();
+        recordsize=atGetOptionalLong(ElemData,"recordsize",1); check_error();
+
+        Ig2Vg_vec=atGetOptionalDoubleArray(ElemData,"_Ig2Vg_vec"); check_error();
+        Ig2Vg_tmp=atGetOptionalDoubleArray(ElemData,"_Ig2Vg_tmp"); check_error();
+        ig_phasor=atGetOptionalDoubleArray(ElemData,"_ig_phasor"); check_error();
+        ig_phasor_record=atGetOptionalDoubleArray(ElemData,"_ig_phasor_record"); check_error();
+        dot_output=atGetOptionalDoubleArray(ElemData,"_dot_output"); check_error();
+        generator_phasor_record=atGetOptionalDoubleArray(ElemData,"_generator_phasor_record"); check_error();
+        beam_phasor_record=atGetOptionalDoubleArray(ElemData,"_beam_phasor_record"); check_error();
+        cavity_phasor_record=atGetOptionalDoubleArray(ElemData,"_cavity_phasor_record"); check_error();
+
+        Ig2Vg_mat=atGetOptionalDoubleArray(ElemData,"_Ig2Vg_mat"); check_error();
+        vc_previous=atGetOptionalDoubleArray(ElemData,"_vc_previous"); check_error();
+        diff_record=atGetOptionalDoubleArray(ElemData,"_diff_record"); check_error();        
+        samplelist=atGetOptionalDoubleArray(ElemData,"_samplelist"); check_error();        
+        vc_list=atGetOptionalDoubleArray(ElemData,"_vc_list"); check_error();        
+        I_record=atGetOptionalDoubleArray(ElemData,"_I_record"); check_error();
+        FFconst=atGetOptionalDoubleArray(ElemData,"_FFconst"); check_error();
+        IIRcoef=atGetOptionalDoubleArray(ElemData,"_IIRcoef"); check_error();
+        IIRout=atGetOptionalDoubleArray(ElemData,"_IIRout"); check_error();
+        
+                
         Energy=atGetOptionalDouble(ElemData,"Energy",Param->energy); check_error();
         z_cuts=atGetOptionalDoubleArray(ElemData,"ZCuts"); check_error();
         feedback_angle_offset=atGetOptionalDouble(ElemData,"feedback_angle_offset", 0.0); check_error();
@@ -238,14 +464,13 @@ ExportMode struct elem *trackFunction(const atElem *ElemData,struct elem *Elem,
 
         int dimsth[] = {Param->nbunch*nslice*nturns, 4};
         atCheckArrayDims(ElemData,"_turnhistory", 2, dimsth); check_error();
-        int dimsvb[] = {Param->nbunch, 2};
+        int dimsvb[] = {Param->harmonic_number, 2};
         atCheckArrayDims(ElemData,"_vbunch", 2, dimsvb); check_error();
        
         Elem = (struct elem*)atMalloc(sizeof(struct elem));
         
         Elem->Length=Length;
         Elem->Frequency=Frequency;
-        Elem->HarmNumber=round(Frequency*rl/C0);
         Elem->Energy = Energy;
         Elem->TimeLag=TimeLag;   
         Elem->nslice=nslice;
@@ -255,13 +480,14 @@ ExportMode struct elem *trackFunction(const atElem *ElemData,struct elem *Elem,
         Elem->Qfactor = qfactor;
         Elem->Rshunt = rshunt;
         Elem->Beta = beta;
-        Elem->z_cuts=z_cuts;
+        Elem->z_cuts = z_cuts;
         Elem->vbunch = vbunch;
         Elem->vbeam = vbeam;
         Elem->vgen = vgen;
         Elem->vcav = vcav;
-        Elem->phasegain = phasegain;
-        Elem->voltgain = voltgain;
+        Elem->tunergain = tunergain;
+        Elem->gain = gain;
+        Elem->openloop = openloop;
         Elem->vbeam_phasor = vbeam_phasor;
         Elem->cavitymode = cavitymode;
         Elem->buffersize = buffersize;
@@ -274,6 +500,29 @@ ExportMode struct elem *trackFunction(const atElem *ElemData,struct elem *Elem,
         Elem->phis = phis;
         Elem->ts = ts;
         Elem->system_harmonic = system_harmonic;
+        Elem->every=every;
+        Elem->delay=delay;
+        Elem->samplenum=samplenum;
+        Elem->cutoff=cutoff;
+        Elem->ff=ff;
+        Elem->recordsize=recordsize;
+        Elem->I_record=I_record;
+        Elem->Ig2Vg_vec=Ig2Vg_vec;
+        Elem->Ig2Vg_tmp=Ig2Vg_tmp;
+        Elem->ig_phasor=ig_phasor;
+        Elem->ig_phasor_record=ig_phasor_record;
+        Elem->dot_output=dot_output;
+        Elem->generator_phasor_record=generator_phasor_record;
+        Elem->beam_phasor_record=beam_phasor_record;
+        Elem->cavity_phasor_record=cavity_phasor_record;
+        Elem->Ig2Vg_mat=Ig2Vg_mat;
+        Elem->vc_previous=vc_previous;
+        Elem->diff_record=diff_record;
+        Elem->samplelist=samplelist;
+        Elem->vc_list=vc_list;
+        Elem->FFconst=FFconst;
+        Elem->IIRcoef=IIRcoef;
+        Elem->IIRout=IIRout;
     }
     energy = atEnergy(Param->energy, Elem->Energy); check_error();
 
@@ -285,7 +534,9 @@ ExportMode struct elem *trackFunction(const atElem *ElemData,struct elem *Elem,
     if(Elem->cavitymode==0 || Elem->cavitymode>=4){
         atError("Unknown cavitymode provided."); check_error();
     } 
-
+    if(Elem->fbmode>=3){
+        atError("Unknown fbmode provided."); check_error();
+    } 
 
     #ifdef _MSC_VER
     atError("Beam loading module not implemented in Windows."); check_error();
@@ -293,7 +544,7 @@ ExportMode struct elem *trackFunction(const atElem *ElemData,struct elem *Elem,
     
     BeamLoadingCavityPass(r_in,num_particles,Param->nbunch,Param->bunch_spos,
                           Param->bunch_currents, Param->fillpattern, rl, 
-                          nturn, energy, Param->harmonic_number, Elem);
+                          nturn, energy, Param->harmonic_number, Param->nturn, Elem);
     return Elem;
 }
 
@@ -311,14 +562,14 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
       const mxArray *ElemData = prhs[0];
       int num_particles = mxGetN(prhs[1]);
       struct elem El, *Elem=&El;
-      
+
       long nslice, nturns, cavitymode, fbmode, buffersize, windowlength, system_harmonic;
-      double wakefact, phis, ts;
-      double normfact, phasegain, voltgain;
+      long delay, every, samplenum, ff, recordsize, openloop;
+      double wakefact, Energy, Frequency, TimeLag, Length, feedback_angle_offset;
+      double normfact, tunergain, qfactor, rshunt, beta, phis, ts, cutoff;
+      double *gain;
       double *turnhistory;
       double *z_cuts;
-      double Energy, Frequency, TimeLag, Length, feedback_angle_offset;
-      double qfactor,rshunt,beta;
       double *vbunch;
       double *vbeam_phasor;
       double *vbeam;
@@ -327,6 +578,12 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
       double *vgen_buffer;
       double *vbeam_buffer;
       double *vbunch_buffer;
+      double *I_record;
+      double *FFconst;
+      double *IIRcoef;
+      double *IIRout;
+      
+      
       /*attributes for RF cavity*/
       Length=atGetDouble(ElemData,"Length"); check_error();
       Frequency=atGetDouble(ElemData,"Frequency"); check_error();
@@ -344,7 +601,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
       beta=atGetDouble(ElemData,"_beta"); check_error();
       normfact=atGetDouble(ElemData,"NormFact"); check_error();
       phasegain=atGetDouble(ElemData,"PhaseGain"); check_error();
-      voltgain=atGetDouble(ElemData,"VoltGain"); check_error();
+      gain=atGetDouble(ElemData,"Gain"); check_error();
       turnhistory=atGetDoubleArray(ElemData,"_turnhistory"); check_error();
       vbunch=atGetDoubleArray(ElemData,"_vbunch"); check_error();
       vbeam=atGetDoubleArray(ElemData,"_vbeam"); check_error();
@@ -357,17 +614,45 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
       phis=atGetDouble(ElemData,"_phis"); check_error();
       system_harmonic=atGetLong(ElemData,"system_harmonic"); check_error();
       ts=atGetDouble(ElemData,"_ts"); check_error();
+      openloop=atGetLong(ElemData,"OpenLoop"); check_error();
       
       /*optional attributes*/
       Energy=atGetOptionalDouble(ElemData,"Energy",0.0); check_error();
       z_cuts=atGetOptionalDoubleArray(ElemData,"ZCuts"); check_error();
       feedback_angle_offset=atGetOptionalDouble(ElemData,"feedback_angle_offset",0.0); check_error();
       
+      
+      Ig2Vg_vec=atGetOptionalDoubleArray(ElemData,"_Ig2Vg_vec"); check_error();
+      Ig2Vg_tmp=atGetOptionalDoubleArray(ElemData,"_Ig2Vg_tmp"); check_error();
+      ig_phasor=atGetOptionalDoubleArray(ElemData,"_ig_phasor"); check_error();
+      ig_phasor_record=atGetOptionalDoubleArray(ElemData,"_ig_phasor_record"); check_error();
+      dot_output=atGetOptionalDoubleArray(ElemData,"_dot_output"); check_error();
+      generator_phasor_record=atGetOptionalDoubleArray(ElemData,"_generator_phasor_record"); check_error();
+      beam_phasor_record=atGetOptionalDoubleArray(ElemData,"_beam_phasor_record"); check_error();
+      cavity_phasor_record=atGetOptionalDoubleArray(ElemData,"_cavity_phasor_record"); check_error();
+
+
+      delay=atGetOptionalLong(ElemData,"delay",1); check_error();
+      every=atGetOptionalLong(ElemData,"every",1); check_error();
+      samplenum=atGetOptionalLong(ElemData,"samplenum",1); check_error();        
+      cutoff=atGetOptionalDouble(ElemData,"cutoff",0); check_error();        
+      ff=atGetOptionalLong(ElemData,"FF",1); check_error();
+      recordsize=atGetOptionalLong(ElemData,"recordsize",1); check_error();  
+        
+      Ig2Vg_mat=atGetOptionalDoubleArray(ElemData,"_Ig2Vg_mat"); check_error();
+      vc_previous=atGetOptionalDoubleArray(ElemData,"_vc_previous"); check_error();
+      diff_record=atGetOptionalDoubleArray(ElemData,"_diff_record"); check_error();        
+      samplelist=atGetOptionalDoubleArray(ElemData,"_samplelist"); check_error();        
+      vc_list=atGetOptionalDoubleArray(ElemData,"_vc_list"); check_error();        
+      I_record=atGetOptionalDoubleArray(ElemData,"_I_record"); check_error();
+      FFconst=atGetOptionalDoubleArray(ElemData,"_FFcont"); check_error();
+      IIRcoef=atGetOptionalDoubleArray(ElemData,"_IIRcoef"); check_error()
+      IIRout=atGetOptionalDoubleArray(ElemData,"_IIRout"); check_error();
+      
       Elem = (struct elem*)atMalloc(sizeof(struct elem));
       Elem->Length=Length;
       Elem->cavitymode=cavitymode;
       Elem->Frequency=Frequency;
-      Elem->HarmNumber=1;
       Elem->Energy = Energy;
       Elem->TimeLag=TimeLag;   
       Elem->nslice=nslice;
@@ -382,8 +667,9 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
       Elem->vbeam = vbeam;
       Elem->vgen = vgen;
       Elem->vcav = vcav;
-      Elem->phasegain = phasegain;
-      Elem->voltgain = voltgain;
+      Elem->gain = gain;
+      Elem->openloop = openloop;
+      Elem->tunergain = tunergain;
       Elem->vbeam_phasor = vbeam_phasor;
       Elem->buffersize = buffersize;
       Elem->windowlength = windowlength;
@@ -394,6 +680,30 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
       Elem->phis = phis;
       Elem->ts = ts;
       Elem->system_harmonic = system_harmonic;
+      Elem->every=every;
+      Elem->delay=delay;
+      Elem->samplenum=samplenum;
+      Elem->cutoff=cutoff;
+      Elem->ff=ff;
+      Elem->recordsize=recordsize;
+      
+      Elem->Ig2Vg_vec=Ig2Vg_vec;
+      Elem->Ig2Vg_tmp=Ig2Vg_tmp;
+      Elem->ig_phasor=ig_phasor;
+      Elem->ig_phasor_record=ig_phasor_record;
+      Elem->dot_output=dot_output;
+      Elem->generator_phasor_record=generator_phasor_record;
+      Elem->beam_phasor_record=beam_phasor_record;
+      Elem->cavity_phasor_record=cavity_phasor_record;
+      Elem->Ig2Vg_mat=Ig2Vg_mat;
+      Elem->vc_previous=vc_previous;
+      Elem->diff_record=diff_record;
+      Elem->samplelist=samplelist;
+      Elem->vc_list=vc_list;
+      Elem->I_record=I_record;
+      Elem->FFconst=FFconst;
+      Elem->IIRcoef=IIRcoef;
+      Elem->IIRout=IIRout;
       
       Elem->fbmode = fbmode;
       if (nrhs > 2) atProperties(prhs[2], &Energy, &rest_energy, &charge);
@@ -406,11 +716,11 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
       double bspos = 0.0;
       double bcurr = 0.0;
       double fillp = 0.0;
-      BeamLoadingCavityPass(r_in, num_particles, 1, &bspos, &bcurr, &fillp, 1, 0, Energy, 1, Elem);
+      BeamLoadingCavityPass(r_in, num_particles, 1, &bspos, &bcurr, &fillp, 1, 0, Energy, 1, 1, Elem);
   }
   else if (nrhs == 0)
   {   /* return list of required fields */
-      plhs[0] = mxCreateCellMatrix(28,1);
+      plhs[0] = mxCreateCellMatrix(29,1);
       mxSetCell(plhs[0],0,mxCreateString("Length"));
       mxSetCell(plhs[0],1,mxCreateString("Energy"));
       mxSetCell(plhs[0],2,mxCreateString("Frequency"));
@@ -423,7 +733,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
       mxSetCell(plhs[0],9,mxCreateString("Rshunt"));
       mxSetCell(plhs[0],10,mxCreateString("_beta"));
       mxSetCell(plhs[0],11,mxCreateString("NormFact"));
-      mxSetCell(plhs[0],12,mxCreateString("PhaseGain"));
+      mxSetCell(plhs[0],12,mxCreateString("Gain"));
       mxSetCell(plhs[0],13,mxCreateString("VoltGain"));
       mxSetCell(plhs[0],14,mxCreateString("_turnhistory"));
       mxSetCell(plhs[0],15,mxCreateString("_vbunch"));
@@ -439,12 +749,41 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
       mxSetCell(plhs[0],25,mxCreateString("_phis"));
       mxSetCell(plhs[0],26,mxCreateString("system_harmonic")); 
       mxSetCell(plhs[0],27,mxCreateString("_ts"));     
+      mxSetCell(plhs[0],28,mxCreateString("OpenLoop"));     
+    
+      
+                                
       if(nlhs>1) /* optional fields */
       {
-          plhs[1] = mxCreateCellMatrix(3,1);
+          plhs[1] = mxCreateCellMatrix(26,1);
           mxSetCell(plhs[1],0,mxCreateString("TimeLag"));
           mxSetCell(plhs[1],1,mxCreateString("ZCuts"));
           mxSetCell(plhs[1],2,mxCreateString("feedback_angle_offset"));
+
+          mxSetCell(plhs[1],3,mxCreateString("Delay"));           
+          mxSetCell(plhs[1],4,mxCreateString("Every"));           
+          mxSetCell(plhs[1],5,mxCreateString("SampleNum"));            
+          mxSetCell(plhs[1],6,mxCreateString("Delay"));  
+          mxSetCell(plhs[1],7,mxCreateString("FF"));   
+          
+          mxSetCell(plhs[1],8,mxCreateString("Ig2Vg_vec"));   
+          mxSetCell(plhs[1],9,mxCreateString("Ig2Vg_tmp"));   
+          mxSetCell(plhs[1],10,mxCreateString("ig_phasor"));   
+          mxSetCell(plhs[1],11,mxCreateString("ig_phasor_record"));   
+          mxSetCell(plhs[1],12,mxCreateString("dot_output"));   
+          mxSetCell(plhs[1],13,mxCreateString("generator_phasor_record"));   
+          mxSetCell(plhs[1],14,mxCreateString("beam_phasor_record"));   
+          mxSetCell(plhs[1],15,mxCreateString("cavity_phasor_record"));   
+          mxSetCell(plhs[1],16,mxCreateString("Ig2Vg_mat"));   
+          mxSetCell(plhs[1],17,mxCreateString("vc_previous"));   
+          mxSetCell(plhs[1],18,mxCreateString("diff_record"));   
+          mxSetCell(plhs[1],19,mxCreateString("samplelist"));   
+          mxSetCell(plhs[1],20,mxCreateString("vc_list"));   
+          mxSetCell(plhs[1],21,mxCreateString("I_record"));   
+          mxSetCell(plhs[1],22,mxCreateString("FFconst"));   
+          mxSetCell(plhs[1],23,mxCreateString("IIRcoef"));   
+          mxSetCell(plhs[1],24,mxCreateString("IIRout"));   
+          mxSetCell(plhs[1],25,mxCreateString("RecordSize"));   
       }
   }
   else
diff --git a/atintegrators/atfeedbacklib.c b/atintegrators/atfeedbacklib.c
new file mode 100644
index 0000000000..d286cc4b5a
--- /dev/null
+++ b/atintegrators/atfeedbacklib.c
@@ -0,0 +1,713 @@
+#include "atconstants.h"
+#include "atelem.c"
+#include <math.h>
+#include <float.h>
+#include <complex.h>
+#ifdef MPI
+#include <mpi.h>
+#include <mpi4py/mpi4py.h>
+#endif
+
+static void init_IIR(double cutoff, double *IIRcoef, double *IIRout, double T1, int every, double Vc){
+    
+    if(cutoff==0){
+        IIRcoef[0] =  1.0;
+    }else{
+        double omega = TWOPI * cutoff;
+        double T = T1 * every;
+        double alpha = cos(omega * T) - 1;
+        double tmp = alpha*alpha - 2*alpha;
+        if(tmp > 0){
+            IIRcoef[0] = alpha + sqrt(tmp);
+        }else{
+            IIRcoef[0] = T * cutoff * TWOPI;
+        }
+    IIRout[0] = Vc;
+    IIRout[1] = 0.0;
+    }
+}
+
+static void IIR(double complex input, double *IIRcoef, double *IIRout){
+    /*
+
+    """Return IIR filter output."""
+    self.IIRout = (1 - self.IIRcoef) * self.IIRout + self.IIRcoef * input
+    return self.IIRout
+    
+    */
+
+    
+    IIRout[0] = (1 - IIRcoef[0]) * IIRout[0] + IIRcoef[0] * creal(input);
+    IIRout[1] = (1 - IIRcoef[0]) * IIRout[1] + IIRcoef[0] * cimag(input);
+    
+
+
+}
+        
+
+static double complex Vg2Ig_real(double vgen, double thetag, double psi, double RL){
+    /*
+    Return Ig from Vg (assuming constant Vg).
+
+    Eq.25 of ref [2] assuming the dVg/dt = 0.
+    */
+    double complex vgen_phasor = vgen * cexp(_Complex_I * (thetag + TWOPI/4)); // phase shift needed for vgen def
+    double complex Ig = (vgen_phasor / RL) * (1 - _Complex_I * tan(psi));
+    printf("some parameters %f \t %f \t %f \t %f \n", vgen, thetag, RL, psi);
+    return creal(Ig);
+}
+
+static double complex Vg2Ig_imag(double vgen, double thetag, double psi, double RL){
+    /*
+    Return Ig from Vg (assuming constant Vg).
+
+    Eq.25 of ref [2] assuming the dVg/dt = 0.
+    */
+    double complex vgen_phasor = vgen * cexp(_Complex_I * (thetag + TWOPI/4));  // phase shift needed for vgen def
+    
+    double complex Ig = (vgen_phasor / RL) * (1 - _Complex_I * tan(psi));
+    return cimag(Ig);
+}
+
+
+        
+        
+static void init_Ig2Vg_matrix(int ring_harmn, double *Ig2Vg_vec_real, double *Ig2Vg_vec_imag, double *Ig2Vg_tmp_real, double *Ig2Vg_tmp_imag, double filling_time, double psi, double T1, double *Ig2Vg_mat_real, double *Ig2Vg_mat_imag){
+    /*
+    Initialize matrix for Ig2Vg_matrix.
+
+    Shoud be called before first use of Ig2Vg_matrix and after each cavity
+    parameter change.
+    */
+
+    /*
+        k = np.arange(0, self.ring.h)
+        self.Ig2Vg_vec = np.exp(-1 / self.cav_res.filling_time *
+                                (1 - 1j * np.tan(self.cav_res.psi)) *
+                                self.ring.T1 * (k+1))
+        tempV = np.exp(-1 / self.cav_res.filling_time * self.ring.T1 * k *
+                       (1 - 1j * np.tan(self.cav_res.psi)))
+        for idx in np.arange(self.ring.h):
+           self.Ig2Vg_mat[idx:, idx] = tempV[:self.ring.h - idx]
+    */                   
+
+    int idx=0;
+
+    for(idx=0;idx<ring_harmn;idx++){
+        Ig2Vg_vec_real[idx] = 0.0;
+        Ig2Vg_vec_imag[idx] = 0.0;
+        Ig2Vg_tmp_real[idx] = 0.0;
+        Ig2Vg_tmp_imag[idx] = 0.0;
+    }
+    
+    for(idx=0;idx<ring_harmn*ring_harmn;idx++){
+        Ig2Vg_mat_real[idx] = 0.0;
+        Ig2Vg_mat_imag[idx] = 0.0;            
+    }
+    
+    
+    int ibucket=0;
+    for(ibucket=0;ibucket<ring_harmn;ibucket++){
+        double complex tmp1 = cexp(-1.0 / filling_time * (1.0 - _Complex_I * tan(psi)) * T1 * ( (double) ibucket + 1.0));
+        Ig2Vg_vec_real[ibucket] = creal(tmp1);
+        Ig2Vg_vec_imag[ibucket] = cimag(tmp1);
+        
+        double complex tmp2 = cexp((-1.0 / filling_time) * (T1 * (double) ibucket * (1.0 - _Complex_I * tan(psi))));
+        Ig2Vg_tmp_real[ibucket] = creal(tmp2);
+        Ig2Vg_tmp_imag[ibucket] = cimag(tmp2);
+    }
+   
+
+    
+    for(idx=0;idx<ring_harmn;idx++){
+        int tempV_start_ind = 0;
+        int tempV_end_ind = ring_harmn - idx;
+                
+        int idtmpv = 0;
+        for(idtmpv=tempV_start_ind;idtmpv<tempV_end_ind;idtmpv++){
+        
+            Ig2Vg_mat_real[idx + idtmpv + ring_harmn*idx] = Ig2Vg_tmp_real[idtmpv];
+            Ig2Vg_mat_imag[idx + idtmpv + ring_harmn*idx] = Ig2Vg_tmp_imag[idtmpv];
+            
+        }
+    }        
+}
+
+static void init_FFconst(bool FF, double *ig_phasor_real, double *ig_phasor_imag, int ring_harmn, double *FFconst){
+    //Initialize feedforward constant
+    double FFconst_real=0.0;
+    double FFconst_imag=0.0;
+    int idx=0;
+    if(FF){
+        for(idx=0;idx<ring_harmn;idx++){
+            FFconst_real += ig_phasor_real[idx];
+            FFconst_imag += ig_phasor_imag[idx];
+        }
+        FFconst_real /= ring_harmn;
+        FFconst_imag /= ring_harmn;
+    }
+    FFconst[0] = FFconst_real;
+    FFconst[1] = FFconst_imag;
+    
+}
+
+static void init_phasor_arrays(double vgen, double thetag, double *ig_phasor_real, double *ig_phasor_imag, double *ig_phasor_record_real, double *ig_phasor_record_imag, int ring_harmn, double RL, double psi, double *generator_phasor_record_real, double *generator_phasor_record_imag){
+
+    double ig_real = Vg2Ig_real(vgen, thetag, psi, RL);
+    double ig_imag = Vg2Ig_imag(vgen, thetag, psi, RL);
+    double Vg_real = -vgen*sin(thetag);
+    double Vg_imag = vgen*cos(thetag);
+    int idx=0;
+    for(idx=0;idx<ring_harmn;idx++){
+        ig_phasor_real[idx]=ig_real;
+        ig_phasor_imag[idx]=ig_imag;
+        ig_phasor_record_real[idx]=ig_real;
+        ig_phasor_record_imag[idx]=ig_imag;
+        generator_phasor_record_real[idx]=Vg_real;
+        generator_phasor_record_imag[idx]=Vg_imag;
+    }
+}
+
+static void init_cavity_record_phasor_array(double *vbunch, 
+                                            double *beam_phasor_record_real, double *beam_phasor_record_imag, 
+                                            double *cavity_phasor_record_real, double *cavity_phasor_record_imag,
+                                            double *generator_phasor_record_real, double *generator_phasor_record_imag,
+                                            int ring_harmn){
+        int idx;
+        double complex tmp = 0 + _Complex_I*0;
+        
+        for(idx=0;idx<ring_harmn;idx++){
+            tmp = vbunch[idx]*cexp(_Complex_I*vbunch[idx+ring_harmn]);
+            beam_phasor_record_real[idx] = creal(tmp);
+            beam_phasor_record_imag[idx] = cimag(tmp);
+        }
+        
+        for(idx=0;idx<ring_harmn;idx++){
+            cavity_phasor_record_real[idx] = beam_phasor_record_real[idx] + generator_phasor_record_real[idx];
+            cavity_phasor_record_imag[idx] = beam_phasor_record_imag[idx] + generator_phasor_record_imag[idx];
+        }
+}
+
+static void set_cavity_phasor(double vgen, double thetag, double *vbeam_phasor, double *vcav_phasor){
+    // BE AWARE THAT MBTRACK2 INCLUDES LOSS FACTOR IN ADDITION TO u0. THIS MAY RESULT IN NUMBERS BEING SLIGHTLY DIFFERENT
+    double complex generator_phasor = vgen*cexp(_Complex_I*(thetag+TWOPI/4));
+    double complex beam_phasor = vbeam_phasor[0]*cexp(_Complex_I*vbeam_phasor[1]);
+    double complex cavity_phasor = generator_phasor + beam_phasor;
+    printf("beam_phasor %f \t %f \n", creal(beam_phasor), cimag(beam_phasor));
+    vcav_phasor[0] = creal(cavity_phasor);
+    vcav_phasor[1] = cimag(cavity_phasor);
+
+}
+
+
+static void mat_dot(double *Ig2Vg_mat_real, double *Ig2Vg_mat_imag, double *ig_phasor_record_real, double *ig_phasor_record_imag, double *dot_output_real, double *dot_output_imag, int ring_harmn){	
+    /*
+    For whatever stupud reason I have mIg2V to be the inverse matrix in the memory. So the logic
+    here is kind of inverted with i and j
+    
+    The problem here is the fucking complex numbers. 
+    */
+    int i,j;
+    double val_real=0.0;
+    double val_imag=0.0;
+    double complex tmp=0;
+    for (i=0;i<ring_harmn;i++){ 
+        val_real = 0.0;
+        val_imag = 0.0;
+        for(j=0;j<ring_harmn;j++){
+            tmp = (Ig2Vg_mat_real[ring_harmn*j + i] + _Complex_I*Ig2Vg_mat_imag[ring_harmn*j + i]) * (ig_phasor_record_real[j] + _Complex_I*ig_phasor_record_imag[j]);
+            val_real += creal(tmp);
+            val_imag += cimag(tmp);
+        }
+        dot_output_real[i] = val_real;
+        dot_output_imag[i] = val_imag;
+        
+    }   
+
+}
+
+
+static void concat_vc_list(double *vc_previous_real, double *vc_previous_imag, double *vc_list_real, double *vc_list_imag, double *cavity_phasor_record_real, double *cavity_phasor_record_imag, int ring_harmn, int samplenum){
+
+    int idx=0;
+    for(idx=0;idx<samplenum;idx++){
+        vc_list_real[idx] = vc_previous_real[idx];
+        vc_list_imag[idx] = vc_previous_imag[idx];
+    }
+    for(idx=0;idx<ring_harmn;idx++){
+        vc_list_real[idx + samplenum] = cavity_phasor_record_real[idx];
+        vc_list_imag[idx + samplenum] = cavity_phasor_record_imag[idx];
+
+    }    
+
+} 
+
+
+
+
+static void init_sample_list(double *sample_list, int ring_harmn, int every, int samplelist_length){
+    /*
+    self.sample_list = range(0, self.ring.h, self.every)
+    
+    but then later
+
+    self.sample_list = range(index + self.every - self.ring.h, self.ring.h,
+                         self.every)
+    */
+    
+   
+    int idx=0;
+
+    for(idx=0;idx<samplelist_length;idx++){
+        sample_list[idx] = 0.0;
+    }
+    
+    for(idx=0;idx<samplelist_length;idx++){
+        sample_list[idx] = idx*every;
+    }
+}
+
+void update_sample_list(int *sample_list, int index, int every, int ring_harmn){
+    int idx=0;
+    int tt = 0;
+    for(idx=index+every-ring_harmn;idx<ring_harmn;idx=idx+every){
+        sample_list[tt] = idx;
+        tt += 1;
+    }   
+    printf("DO I EVEN GET HERE[0] %d \n", sample_list[0]);
+}
+
+
+static void Ig2Vg_matrix(double *generator_phasor_record_real, double *generator_phasor_record_imag,
+                         double *Ig2Vg_vec_real, double *Ig2Vg_vec_imag,
+                         double *Ig2Vg_mat_real, double *Ig2Vg_mat_imag,
+                         double *ig_phasor_record_real, double *ig_phasor_record_imag,
+                         double *dot_output_real, double *dot_output_imag,
+                         double kloss, double T1, int ring_harmn){
+    /*
+    Return Vg from Ig using matrix formalism.
+    Warning: self.init_Ig2Vg should be called after each CavityResonator
+    parameter change.
+
+
+    generator_phasor_record = (
+        self.Ig2Vg_vec * self.cav_res.generator_phasor_record[-1] +
+        self.Ig2Vg_mat.dot(self.ig_phasor_record) *
+        kloss * T1)
+        
+    */
+    int idx;
+    double complex tmp=0;
+    mat_dot(Ig2Vg_mat_real, Ig2Vg_mat_imag,
+            ig_phasor_record_real, ig_phasor_record_imag,
+            dot_output_real, dot_output_imag,
+            ring_harmn);
+            
+    for(idx=0;idx<ring_harmn;idx++){
+        tmp =  (Ig2Vg_vec_real[idx] + _Complex_I*Ig2Vg_vec_imag[idx]) * 
+               (generator_phasor_record_real[ring_harmn-1] + _Complex_I * generator_phasor_record_imag[ring_harmn-1]);
+               
+        generator_phasor_record_real[idx] = creal(tmp) +
+                                            dot_output_real[idx] * kloss * T1;       
+        generator_phasor_record_imag[idx] = cimag(tmp) +
+                                            dot_output_imag[idx] * kloss * T1;     
+    }        
+}
+
+
+static void Ig2Vg(double *generator_phasor_record_real, double *generator_phasor_record_imag,
+                 double *Ig2Vg_vec_real, double *Ig2Vg_vec_imag,
+                 double *Ig2Vg_mat_real, double *Ig2Vg_mat_imag,
+                 double *ig_phasor_record_real, double *ig_phasor_record_imag,
+                 double *dot_output_real, double *dot_output_imag,
+                 double kloss, double T1, int ring_harmn, double *vgen_arr){
+    /*
+    def Ig2Vg(self):
+        """
+        Go from Ig to Vg.
+
+        Apply new values to cav_res.generator_phasor_record, cav_res.Vg and
+        cav_res.theta_g from ig_phasor_record.
+        """
+        self.cav_res.generator_phasor_record = self.Ig2Vg_matrix()
+        self.cav_res.Vg = np.mean(np.abs(self.cav_res.generator_phasor_record))
+        self.cav_res.theta_g = np.mean(
+            np.angle(self.cav_res.generator_phasor_record))
+    */
+
+    Ig2Vg_matrix(generator_phasor_record_real, generator_phasor_record_imag,
+                 Ig2Vg_vec_real, Ig2Vg_vec_imag,
+                 Ig2Vg_mat_real, Ig2Vg_mat_imag,
+                 ig_phasor_record_real, ig_phasor_record_imag,
+                 dot_output_real, dot_output_imag,
+                 kloss, T1, ring_harmn);
+                 
+    int idx;
+    double mean_vg = 0.0;
+    double mean_thetag=0.0;
+    for(idx=0;idx<ring_harmn;idx++){
+        mean_vg += sqrt(generator_phasor_record_real[idx]*generator_phasor_record_real[idx] + 
+                        generator_phasor_record_imag[idx]*generator_phasor_record_imag[idx]);
+        mean_thetag += -atan2(generator_phasor_record_real[idx], generator_phasor_record_imag[idx]);
+    }
+    mean_vg /= ring_harmn;
+    mean_thetag /= ring_harmn;
+    printf("mean_vg %f \t %f \n", mean_vg, mean_thetag);
+    vgen_arr[0] = mean_vg;
+    vgen_arr[1] = mean_thetag;
+}
+
+
+
+static void track_PIL(double *vc_previous_real, double *vc_previous_imag,
+                      double *cavity_phasor_record_real, double *cavity_phasor_record_imag,
+                      double *ig_phasor_real, double *ig_phasor_imag,
+                      double *sample_list, int samplenum, int record_size, int samplelist_length,
+                      double *diff_record_real, double *diff_record_imag,
+                      double *FFconst, double *gain, double *I_record,
+                      double frf,
+                      double Vc, double theta,
+                      double *generator_phasor_record_real, double *generator_phasor_record_imag,
+                      double *Ig2Vg_vec_real, double *Ig2Vg_vec_imag,
+                      double *Ig2Vg_mat_real, double *Ig2Vg_mat_imag,
+                      double *ig_phasor_record_real, double *ig_phasor_record_imag,
+                      double *dot_output_real, double *dot_output_imag,
+                      double kloss, double T1, int ring_harmn, double *vgen_arr,
+                      double *IIRout, double *IIRcoef,
+                      double *vc_list_real, double *vc_list_imag,
+                      int every,
+                      double psi, double rshunt,
+                      int open
+                      ){
+    /*
+    def track(self, apply_changes: bool = True):
+        """
+        Tracking method for the Cavity PI control feedback.
+
+        Returns
+        -------
+        None.
+
+        """
+        vc_list = np.concatenate([
+            self.vc_previous, self.cav_res.cavity_phasor_record
+        ])  #This line is slowing down the process.
+        self.ig_phasor.fill(self.ig_phasor[-1])
+
+        for index in self.sample_list:
+            # 2) updating Ig using last item of the list
+            diff = self.diff_record[-1] - self.FFconst
+            self.I_record += diff / self.ring.f1
+            fb_value = self.gain[0] * diff + self.gain[1] * self.I_record
+            self.ig_phasor[index:] = self.Vg2Ig(fb_value) + self.FFconst
+            # Shift the record
+            self.diff_record = np.roll(self.diff_record, 1)
+            # 1) recording diff as a first item of the list
+            mean_vc = np.mean(vc_list[index:self.sample_num + index]) * np.exp(
+                -1j * self.cav_res.theta)
+            self.diff_record[0] = self.cav_res.Vc - self.IIR(mean_vc)
+        # update sample_list for next turn
+        self.sample_list = range(index + self.every - self.ring.h, self.ring.h,
+                                 self.every)
+        # update vc_previous for next turn
+        self.vc_previous = self.cav_res.cavity_phasor_record[-self.sample_num:]
+
+        self.ig_phasor = self.ig_phasor + self.ig_modulation_signal
+        self.ig_phasor_record = self.ig_phasor
+
+        if apply_changes:
+            self.Ig2Vg()
+            
+    */            
+
+    int idx=0;
+    float index_tmp=0;
+    int index=0;
+    int index2=0;
+    int sample=0;
+    
+    double diff_real, diff_imag, fb_value_real, fb_value_imag;
+    double fb_amp, fb_phase, fb_real, fbr, fbi;
+    double mean_vc_arr[] = {0.0, 0.0};
+    double complex mean_vc = 0.0 + _Complex_I * 0.0;
+    double complex fb_value;
+
+    concat_vc_list(vc_previous_real, vc_previous_imag, vc_list_real, vc_list_imag, cavity_phasor_record_real, cavity_phasor_record_imag, ring_harmn, samplenum);
+
+    
+    printf("1,2,3,4,5 %d \t %d \t %d \t %d \n", (int)sample_list[0], (int)sample_list[1], (int)sample_list[2], (int)sample_list[3]);
+    for(idx=0;idx<samplelist_length;idx++){
+        index_tmp = sample_list[idx];
+        index = (int)index_tmp;
+        printf("LOOK AT THIS ONE THE INDEX IS %d \t %d \t %f \t %f \n", (int)index, idx, index, idx);
+        diff_real = diff_record_real[record_size-1] - FFconst[0];
+        diff_imag = diff_record_imag[record_size-1] - FFconst[1];
+        printf("diff_real[0] %f \n", diff_real);
+        printf("FFconst %f\t%f\n", FFconst[0], FFconst[1]);
+
+        I_record[0] += diff_real * T1;
+        I_record[1] += diff_imag * T1;
+
+        fb_value = gain[0] * (diff_real + _Complex_I * diff_imag) 
+                 + gain[1] * (I_record[0] + _Complex_I * I_record[1]);
+        
+        fb_value_real = creal(fb_value);
+        fb_value_imag = cimag(fb_value);
+        printf("fb real imag before Vg2Ig %f \t %f \n", fb_value_real, fb_value_imag);
+
+                
+        fb_amp = sqrt(fb_value_real*fb_value_real + fb_value_imag*fb_value_imag);
+        fb_phase = -atan2(fb_value_real, fb_value_imag);
+        
+        fbr = Vg2Ig_real(fb_amp, fb_phase, psi, rshunt);
+        fbi = Vg2Ig_imag(fb_amp, fb_phase, psi, rshunt);
+        printf("fb real imag after Vg2Ig [1e9] %f \t %f \n", fbr*1e9, fbi*1e9);
+        printf("fb real imag after Vg2Ig  with FFconst %f \t %f \n", fbr+FFconst[0], fbi+FFconst[1]);        
+        for(index2=index;index2<ring_harmn;index2++){
+            ig_phasor_real[index2] = fbr + FFconst[0];
+            ig_phasor_imag[index2] = fbi + FFconst[1];
+            }
+
+        printf("\n ig_phasor_real %f \t imag %f \n", ig_phasor_real[0], ig_phasor_imag[0]);
+
+        roll_array(diff_record_real, record_size);
+        roll_array(diff_record_imag, record_size);
+        printf("\n\n vc list %f \t %f \n\n", vc_list_real[0], vc_list_imag[0]);
+        compute_mean_vc(vc_list_real, vc_list_imag, mean_vc_arr, index, samplenum);
+        printf("\n mean_vc arr %f \t %f \n\n", mean_vc_arr[0], mean_vc_arr[1]);
+        mean_vc = (mean_vc_arr[0] + _Complex_I * mean_vc_arr[1])*cexp(-_Complex_I * (theta+TWOPI/4));
+        printf("\n mean_vc %f \t %f \n\n", creal(mean_vc), cimag(mean_vc));
+        
+        IIR(mean_vc, IIRcoef, IIRout);
+
+        diff_record_real[0] = Vc - IIRout[0];
+        diff_record_imag[0] = IIRout[1];
+        };
+        printf("diff_record_real[0] %f \n", diff_record_real[0]); 
+
+
+        update_sample_list(sample_list, index, every, ring_harmn);
+        
+        
+        update_vc_previous(vc_previous_real, vc_previous_imag,
+                           samplenum, ring_harmn, 
+                           cavity_phasor_record_real, cavity_phasor_record_imag);
+                           
+        update_ig_phasor(ig_phasor_real, ig_phasor_imag, ig_phasor_record_real, ig_phasor_record_imag, ring_harmn);
+                           
+        if(open==0){                          
+            Ig2Vg(generator_phasor_record_real, generator_phasor_record_imag,
+                         Ig2Vg_vec_real, Ig2Vg_vec_imag,
+                         Ig2Vg_mat_real, Ig2Vg_mat_imag,
+                         ig_phasor_record_real, ig_phasor_record_imag,
+                         dot_output_real, dot_output_imag,
+                         kloss, T1, ring_harmn, vgen_arr);     
+        }                    
+    };
+
+void update_ig_phasor(double *ig_phasor_real, double *ig_phasor_imag, double *ig_phasor_record_real, double *ig_phasor_record_imag, int ring_harmn){
+    int idx=0;
+    for(idx=0;idx<ring_harmn;idx++){
+        ig_phasor_record_real[idx] = ig_phasor_real[idx];
+        ig_phasor_record_imag[idx] = ig_phasor_imag[idx];
+    }    
+
+}
+
+static void init_vc_previous(double *vc_previous_real, double *vc_previous_imag, int samplenum, double *vcav_phasor){
+    /*
+    self.vc_previous = np.ones(
+    self.sample_num) * self.cav_res.cavity_phasor
+    */
+    int idx=0;
+    for(idx=0;idx<samplenum;idx++){
+        vc_previous_real[idx] = vcav_phasor[0];
+        vc_previous_imag[idx] = vcav_phasor[1];
+    }
+
+}
+
+
+
+void update_vc_previous(double *vc_previous_real, double *vc_previous_imag, int samplenum, int ring_harmn, double *cavity_phasor_record_real, double *cavity_phasor_record_imag){
+    /*
+    
+    */
+    int idx=0;
+    for(idx=0;idx<samplenum;idx++){
+        vc_previous_real[idx] = cavity_phasor_record_real[ring_harmn-samplenum+idx];
+        vc_previous_imag[idx] = cavity_phasor_record_imag[ring_harmn-samplenum+idx];
+    }    
+}
+
+
+void compute_mean_vc(double *vc_list_real, double *vc_list_imag, double *vc_mean, int index, int samplenum){
+    int idx=0;
+    vc_mean[0] = 0.0;
+    vc_mean[1] = 0.0;
+    printf("compute mean the index is %d \n", index);
+    for(idx=index;idx<index+samplenum;idx++){
+        vc_mean[0] += vc_list_real[idx]/samplenum;
+        vc_mean[1] += vc_list_imag[idx]/samplenum;
+    }
+}
+
+
+
+
+void roll_array(double *arr, int arr_len){
+
+
+    if(arr_len>1){
+        int idx = 0;
+        double tmp=0.0;
+        double tmp2=0.0;    
+        tmp = arr[arr_len-1];
+       
+        for(idx=0;idx<arr_len;idx++){
+            tmp2 = arr[idx];
+            arr[idx] = tmp;
+            tmp = tmp2;
+        }
+    }    
+}
+
+
+static void update_vbeam_set(long fbmode, double *vbeam_set,
+                             double *vbeamk, double *vbeam_buffer,
+                             long buffersize, long windowlength){
+    int bufferlengthnow = 0;
+    // If FBMode is set to ONETURN, then set the vbeam and move on
+    if(windowlength==1){
+        vbeam_set[0] = vbeamk[0];
+        vbeam_set[1] = vbeamk[1];        
+    }
+    // If FBMode is set to WINDOW, compute the vbeam_set from the buffer
+    
+    else if(windowlength>=2){
+        // Compute the length of the buffer as we will not act until 
+        // the buffer is full. (2 arrays of vbeam and psi)
+            
+        bufferlengthnow = check_buffer_length(vbeam_buffer, buffersize, 2);
+
+        if(bufferlengthnow >= windowlength){
+            compute_buffer_mean(vbeam_set, vbeam_buffer, windowlength, buffersize, 2);
+        } 
+    }
+}
+
+
+void compute_buffer_mean(double *out_array, double *buffer, long windowlength, long buffersize, long numcolumns){
+
+    int c,p,offset;
+    offset = buffersize - windowlength;
+
+    for (p=0; p<numcolumns; p++) {
+        out_array[p] = 0.0;
+    }
+    
+    for (c=offset; c<buffersize; c++) {
+        for (p=0; p<numcolumns; p++) {
+            out_array[p] += buffer[2*c+p];
+        }
+    }
+    
+    for (p=0; p<numcolumns; p++) {
+        out_array[p] /= windowlength ; 
+    }
+}
+
+int check_buffer_length(double *buffer, long buffersize, long numcolumns){
+    int c;
+    int bufferlengthnow=0;
+    for (c=0; c<numcolumns*buffersize; c++){
+        if (buffer[c]!=0.0){
+            bufferlengthnow += 1;
+        }
+    }
+    bufferlengthnow /= numcolumns;
+    return bufferlengthnow;
+}
+
+
+
+static void compute_set_params(double *vbeam, double *vgen, double detune_angle, double phis, double *vgen_set){
+
+    double vbeamr_meas = vbeam[0]*cos(vbeam[1]);
+    double vbeami_meas = vbeam[0]*sin(vbeam[1]);
+    
+    double vgenr_meas = -vgen[0]*sin(vgen[1]);
+    double vgeni_meas = vgen[0]*cos(vgen[1]);      
+    
+    double vcavr_meas = vgenr_meas + vbeamr_meas;
+    double vcavi_meas = vgeni_meas + vbeami_meas;   
+
+    double vcav_meas = sqrt(vcavr_meas*vcavr_meas + vcavi_meas*vcavi_meas); 
+    double phis_meas = -atan2(vcavr_meas, vcavi_meas);
+
+    
+    /* This computes the delta psi */
+    double meas_psi = vgen[1] - phis + detune_angle;
+
+    vgen_set[0] = vcav_meas;
+    vgen_set[1] = phis_meas;
+    vgen_set[2] = meas_psi;
+
+}
+static void update_vgen(double *vcav, double *vgen, double *vcav_meas, double voltgain,
+                        double phasegain, double detune_angle){
+
+    /* This computes the delta theta g*/
+    double phis = vcav[1];   
+    double ptmp = vcav_meas[1] - phis; 
+
+    /* This computes the delta psi */
+
+    double dtmp = vcav[0] / vcav_meas[0];
+
+    vgen[3] *= pow(dtmp,voltgain);
+    vgen[1] -= ptmp*phasegain;
+    vgen[0] = vgen[3]*cos(vgen[2]);
+    
+}
+
+
+static void update_passive_frequency(double *vbeam, double *vcav, double *vgen, double phasegain){
+    /* The cavity voltage is
+    V(t) = 2*I0*rs*cos(psi)*exp(i(wt+psi))
+    We save the amplitude of vbeam, so the exponent goes to 1.
+    Therefore vbeam[0] = 2*I0*rs*cos(psi) which is the cavity voltage.
+    */
+    double vset = vcav[0];
+    double psi = vgen[2];
+    double vpeak = vbeam[0]; /* Peak amplitude of cavity voltage */
+    double delta_v = vset - vpeak;
+    double grad = vbeam[0]*sin(psi)/cos(psi); 
+    /*vbeam amp contains cos(psi). So replace with sin(psi)
+    to get get the gradient */
+    
+    double delta_psi = delta_v / grad; /*linear extrapolation*/
+
+    
+    /* If the cavity is detuned positively, the psi needs to
+    be increased to reduce the voltage. Likewise, if the cavity
+    is detuned negatively, the psi needs to be decreased to reduce
+    the voltage.
+    */
+        
+    int sg = (psi<0) - (psi>0);
+
+    /* This is to avoid setting a value if grad is 0, as then
+    delta_psi is inf, which even when multiplied by 0 gives nan
+    */
+    if (grad!=0.0){
+        vgen[2] += sg*delta_psi*phasegain;
+    }
+}
+
+
+
+
+
+
+
diff --git a/atintegrators/atimplib.c b/atintegrators/atimplib.c
index b1d32b206c..83f65090ac 100644
--- a/atintegrators/atimplib.c
+++ b/atintegrators/atimplib.c
@@ -319,36 +319,42 @@ static void compute_kicks_phasor(int nslice, int nbunch, int nturns, double *tur
     double kloss = rshunt*omr/(2*qfactor);
     double bc = beta*C0;
     double *vbr = vbunch;
-    double *vbi = vbunch+nbunch;
+    double *vbi = vbunch+ring_harmn;
     int ibunch, islice, total_slice_counter;
     int bunch_counter = 0;
-    double bucket_curr = 0.0;
+    double is_filled = 0.0;
     double main_bucket = circumference / (double) ring_harmn;
     double ave_vbeam_ri[] = {0.0, 0.0};
     
     
     
     for (i=0;i<nslice*nbunch;i++) {
-        ibunch = (int)(i/nslice);
         vbeam_kicks[i] = 0.0;
+    }
+
+    for (ibunch=0;ibunch<ring_harmn;ibunch++){
         vbr[ibunch] = 0.0;
         vbi[ibunch] = 0.0;
+    
     }
 
-    
     /* The vbeam_complex will always be sent to the center of the next bucket */
-    
+
+    double bucket_z_center = 0.0;
     for(ibunch=0; ibunch<ring_harmn; ibunch++){
-        bucket_curr = fillpattern[ibunch];
-        if(bucket_curr!=0.0){
+        is_filled = fillpattern[ibunch]; 
+
+        bucket_z_center = ibunch*main_bucket;
+        
+        if(is_filled!=0.0){
             for(islice=0; islice<nslice; islice++){
                 total_slice_counter = islice + nslice*bunch_counter; 
                 wi = turnhistoryW[total_slice_counter];
                 selfkick = normfact*wi*kloss*energy; /*normfact*energy is -t0 . This number comes out to be negative, which is correct*/       
                 if(islice==0){
-                    /* TurnhistoryZ goes from -bucket991 to bucket0 */
-                    dt = (turnhistoryZ[total_slice_counter] + bunch_spos[nbunch-1-bunch_counter] - bunch_spos[0])/bc;
-                    
+                    // TurnhistoryZ goes from -bucket991 to bucket0 
+                    // so the bucket center in the turnhistory reference is one bucket shifted,
+                    dt = (turnhistoryZ[total_slice_counter] + circumference - bucket_z_center - main_bucket)/bc; 
                 }else{
                     /* This is dt between each slice*/
                     dt = (turnhistoryZ[total_slice_counter]-turnhistoryZ[total_slice_counter-1])/bc;
@@ -357,28 +363,24 @@ static void compute_kicks_phasor(int nslice, int nbunch, int nturns, double *tur
                 /* track the dt */
                 vbeam_complex *= cexp((_Complex_I*omr-omr/(2*qfactor))*dt);
                 vbeam_kicks[total_slice_counter] = creal((vbeam_complex + selfkick)/energy);
-                
                 vbeam_complex += 2*selfkick;    
                
             }
             /* back to the center of the bucket */
-            dt = -(turnhistoryZ[total_slice_counter] + bunch_spos[nbunch - 1 - bunch_counter] - bunch_spos[0])/bc;
-            vbeam_complex *= cexp((_Complex_I*omr-omr/(2*qfactor))*dt);
+            dt = -(turnhistoryZ[total_slice_counter] + circumference - bucket_z_center - main_bucket)/bc;
             
-            /* move to ts_central time */
-            dt = -ts_central_z/bc;
             vbeam_complex *= cexp((_Complex_I*omr-omr/(2*qfactor))*dt);
-            
-            vbr[bunch_counter] = cabs(vbeam_complex);
-            vbi[bunch_counter] = carg(vbeam_complex);
-                        
             bunch_counter += 1;
-        }else{
-            /* move to ts_central time */
-            dt = -ts_central_z/bc;
-            vbeam_complex *= cexp((_Complex_I*omr-omr/(2*qfactor))*dt);     
         }
+        
+        /* move to ts_central time */
+        dt = -ts_central_z/bc;
+        vbeam_complex *= cexp((_Complex_I*omr-omr/(2*qfactor))*dt);
+                       
 
+        vbr[ibunch] = cabs(vbeam_complex);
+        vbi[ibunch] = carg(vbeam_complex);
+            
         ave_vbeam_ri[0] += creal(vbeam_complex)/ring_harmn;
         ave_vbeam_ri[1] += cimag(vbeam_complex)/ring_harmn;
 
@@ -390,7 +392,7 @@ static void compute_kicks_phasor(int nslice, int nbunch, int nturns, double *tur
        
         dt = main_bucket/bc;
         vbeam_complex *= cexp((_Complex_I*omr-omr/(2*qfactor))*dt);
-        
+
     }
     /* store the phasor for the next turn */
     vbeam_phasor[0] = cabs(vbeam_complex);
@@ -402,124 +404,3 @@ static void compute_kicks_phasor(int nslice, int nbunch, int nturns, double *tur
     #endif    
 };
 
-
-static void update_vgen(double *vbeam,double *vcav,double *vgen, double voltgain,double phasegain,double detune_angle){
-
-    double vbeamr_meas = vbeam[0]*cos(vbeam[1]);
-    double vbeami_meas = vbeam[0]*sin(vbeam[1]);
-    
-    double vgenr_meas = -vgen[0]*sin(vgen[1]);
-    double vgeni_meas = vgen[0]*cos(vgen[1]);      
-    
-    double vcavr_meas = vgenr_meas + vbeamr_meas;
-    double vcavi_meas = vgeni_meas + vbeami_meas;   
-
-    double vcav_meas = sqrt(vcavr_meas*vcavr_meas + vcavi_meas*vcavi_meas); 
-    double phis_meas = -atan2(vcavr_meas, vcavi_meas);
-
-    double phis = vcav[1];   
-    /* This computes the delta theta g*/
-    double ptmp = phis_meas - phis; 
-    
-    /* This computes the delta psi */
-    double dttmp = vgen[1] - vgen[2] - phis + detune_angle;
-
-    double dtmp = vcav[0] / vcav_meas;
-    
-    vgen[3] *= pow(dtmp,voltgain);
-    vgen[2] += dttmp*phasegain; 
-    vgen[1] -= ptmp*phasegain;
-    vgen[0] = vgen[3]*cos(vgen[2]);
-}
-
-
-static void update_passive_frequency(double *vbeam, double *vcav, double *vgen, double phasegain){
-    /* The cavity voltage is
-    V(t) = 2*I0*rs*cos(psi)*exp(i(wt+psi))
-    We save the amplitude of vbeam, so the exponent goes to 1.
-    Therefore vbeam[0] = 2*I0*rs*cos(psi) which is the cavity voltage.
-    */
-    double vset = vcav[0];
-    double psi = vgen[2];
-    double vpeak = vbeam[0]; /* Peak amplitude of cavity voltage */
-    double delta_v = vset - vpeak;
-    double grad = vbeam[0]*sin(psi)/cos(psi); 
-    /*vbeam amp contains cos(psi). So replace with sin(psi)
-    to get get the gradient */
-    
-    double delta_psi = delta_v / grad; /*linear extrapolation*/
-
-    
-    /* If the cavity is detuned positively, the psi needs to
-    be increased to reduce the voltage. Likewise, if the cavity
-    is detuned negatively, the psi needs to be decreased to reduce
-    the voltage.
-    */
-        
-    int sg = (psi<0) - (psi>0);
-
-    /* This is to avoid setting a value if grad is 0, as then
-    delta_psi is inf, which even when multiplied by 0 gives nan
-    */
-    if (grad!=0.0){
-        vgen[2] += sg*delta_psi*phasegain;
-    }
-}
-
-static void compute_buffer_mean(double *out_array, double *buffer, long windowlength, long buffersize, long numcolumns){
-
-    int c,p,offset;
-    offset = buffersize - windowlength;
-
-    for (p=0; p<numcolumns; p++) {
-        out_array[p] = 0.0;
-    }
-    
-    for (c=offset; c<buffersize; c++) {
-        for (p=0; p<numcolumns; p++) {
-            out_array[p] += buffer[2*c+p];
-        }
-    }
-    
-    for (p=0; p<numcolumns; p++) {
-        out_array[p] /= windowlength ; 
-    }
-}
-
-int check_buffer_length(double *buffer, long buffersize, long numcolumns){
-    int c;
-    int bufferlengthnow=0;
-    for (c=0; c<numcolumns*buffersize; c++){
-        if (buffer[c]!=0.0){
-            bufferlengthnow += 1;
-        }
-    }
-    bufferlengthnow /= numcolumns;
-    return bufferlengthnow;
-}
-
-
-static void update_vbeam_set(long fbmode, double *vbeam_set,
-                             double *vbeamk, double *vbeam_buffer,
-                             long buffersize, long windowlength){
-    int bufferlengthnow = 0;
-    // If FBMode is set to ONETURN, then set the vbeam and move on
-    if(fbmode==1){
-        vbeam_set[0] = vbeamk[0];
-        vbeam_set[1] = vbeamk[1];        
-    }
-    // If FBMode is set to WINDOW, compute the vbeam_set from the buffer
-    
-    else if(fbmode==2){
-        // Compute the length of the buffer as we will not act until 
-        // the buffer is full. (2 arrays of vbeam and psi)
-        
-        bufferlengthnow = check_buffer_length(vbeam_buffer, buffersize, 2);
-
-        if(bufferlengthnow >= windowlength){
-            compute_buffer_mean(vbeam_set, vbeam_buffer, windowlength, buffersize, 2);
-        } 
-    }
-}
-
-
diff --git a/pyat/at/collective/beam_loading.py b/pyat/at/collective/beam_loading.py
index 7b9229eb75..a4d8da2b81 100644
--- a/pyat/at/collective/beam_loading.py
+++ b/pyat/at/collective/beam_loading.py
@@ -34,17 +34,16 @@ class CavityMode(IntEnum):
 
 class FeedbackMode(IntEnum):
     """
-    FeedbackMode.ONETURN means the feedback is only using the most recent
+    FeedbackMode.PROP means the feedback is only using the most recent
     turn, and the delta is applied each turn multiplied by VoltGain or
     PhaseGain.
 
-    FeedbackMode.WINDOW means the beam voltage passed to the feedback is
-    the average of the WINDOW. For this method, buffersize and windowlength
-    must also be provided.
+    FeedbackMode.PROP_INTEGRAL means
+    
     """
 
-    ONETURN = 1
-    WINDOW = 2
+    PROP = 1
+    PROP_INTEGRAL = 2
 
 
 def add_beamloading(
@@ -158,8 +157,11 @@ class BeamLoadingElement(RFCavity, Collective):
         Rshunt=float,
         Qfactor=float,
         NormFact=float,
-        PhaseGain=float,
-        VoltGain=float,
+        TunerGain=float,
+        Gain=lambda v: _array(v, shape=(2,)),
+        #PhaseGain=float,
+        #VoltGain=float,
+        IIR_cutoff=float,
         _beta=float,
         _wakefact=float,
         _nslice=int,
@@ -172,6 +174,9 @@ class BeamLoadingElement(RFCavity, Collective):
         _vbeam=lambda v: _array(v, shape=(2,)),
         _vcav=lambda v: _array(v, shape=(2,)),
         _vgen=lambda v: _array(v, shape=(4,)),
+        delay=int,
+        every=int,
+        samplenum=int,
     )
 
     def __init__(
@@ -185,8 +190,8 @@ def __init__(
         rshunt: float,
         detune: float | None = 0.0,
         cavitymode: CavityMode | None = CavityMode.ACTIVE,
-        fbmode: FeedbackMode | None = FeedbackMode.ONETURN,
-        buffersize: int | None = 0,
+        fbmode: FeedbackMode | None = FeedbackMode.PROP,
+        IIR_cutoff: float = 0.0,
         **kwargs,
     ):
         r"""
@@ -211,10 +216,12 @@ def __init__(
                 passive_voltage
             passive_voltage [V] (float): Voltage setpoint with the passive
                 cavity with feedback.
-            PhaseGain (float):  Used for cavity feedbacks. States the gain on
-                the phase correction factor to be applied.
-            VoltGain (float):  Used for cavity feedbacks. States the gain on
-                the phase correction factor to be applied.
+            TunerGain (float): Used for detuning of the cavities. States the gain
+                of the correction factor to be applied.
+            Gain ([float, float]):  Used for cavity feedbacks. If FBMode is PROP
+                then Gain[0] is the amplitude gain, and Gain[1] is the phase gain.
+                If FBMode is PROP_INTEGRAL, then Gain[0] is Prop gain and Gain[1]
+                is Integral gain. 
             buffersize (int):  Size of the history buffer for vbeam, vgen,
                 vbunch (default 0)
             feedback_angle_offset:      Fixed detuning from optimal tuning
@@ -238,48 +245,82 @@ def __init__(
                 for the given system. e.g. third of fourth harmonic of
                 rf_frequency. If None, then will be computed to the nearest
                 integer multiple of rf_frequency.
+            IIR_cutoff: cutoff frequency of the IIR filter [Hz]. If 0,
+                a cutoff frequency of infinity is assumed. 
+            Delay: Loop delay [buckets]
+            Every: Every what
+            samplenum: Sample whhatt?
         Returns:
             bl_elem (Element): beam loading element
         """
         kwargs.setdefault("PassMethod", self.default_pass[True])
-        if not isinstance(cavitymode, CavityMode):
-            raise TypeError("cavitymode has to be an " + "instance of CavityMode")
+
 
         zcuts = kwargs.pop("ZCuts", None)
-        ts = kwargs.pop("ts", None)
-        self.system_harmonic = kwargs.pop(
-            "system_harmonic", int(np.round(frequency / ring.rf_frequency))
-        )
-        self.detune = detune
+        if zcuts is not None:
+            self.ZCuts = zcuts
+            
 
-        check_frequency = np.abs(frequency - self.system_harmonic * ring.rf_frequency)
 
-        if check_frequency > 1.0:  # 1 Hz is the limit for the float check
 
-            error_string = (
-                "Cavity must be an integer of rf_frequency, otherwise"
-                "the phi_s computation will be wrong. Please use the detune"
-                "argument when adding beamloading to a cavity that is an"
-                "integer harmonic."
-            )
-            raise AtError(error_string)
 
         self.circumference = ring.circumference
         self.bunch_spos = ring.bunch_spos
         energy = ring.energy
-        harmonic_number = self.system_harmonic * ring.harmonic_number
+        self.system_harmonic = kwargs.pop(
+            "system_harmonic", int(np.round(frequency / ring.rf_frequency))
+        )
+        harmonic_number = self.system_harmonic * ring.harmonic_number #cavity harmonic number
+        self.ring_harmonic_number = ring.harmonic_number #ring harmonic number (nbuckets)        
+        self._nbunch = ring.nbunch
+        
+        
+        self._passive_vset = kwargs.pop("passive_voltage", 0.0)
+        self._beta = ring.beta
+        self._wakefact = -ring.circumference / (clight * ring.energy * ring.beta**3)
+        self._nslice = kwargs.pop("Nslice", 101)
+        self._nturns = kwargs.pop("Nturns", 1)
+        self._nbunch = ring.nbunch
+        self._turnhistory = None  # Defined here to avoid warning
+        self._vbunch = None
+
+
         self.feedback_angle_offset = kwargs.pop("feedback_angle_offset", 0)
         self.Rshunt = rshunt
         self.Qfactor = qfactor
         self.NormFact = kwargs.pop("NormFact", 1.0)
-        self.PhaseGain = kwargs.pop("PhaseGain", 1.0)
-        self.VoltGain = kwargs.pop("VoltGain", 1.0)
+        
+        self.Gain = kwargs.pop("Gain", [1e-3,1e-3])
+        self.TunerGain = kwargs.pop("TunerGain", 1.0)
+        #self.PhaseGain = kwargs.pop("PhaseGain", 1.0)
+        #self.VoltGain = kwargs.pop("VoltGain", 1.0)
+        self.OpenLoop = int(kwargs.pop("OpenLoop", 0))
+
+        self.delay = kwargs.pop("delay", 1)
+        self.every = kwargs.pop("every", 1)
+        self.samplenum = kwargs.pop("samplenum", 1)
+        self.samplelist_length = int(np.ceil(ring.harmonic_number/self.every))
+        self.recordsize = int(np.ceil(self.delay / self.every))
+
+        self.cutoff = kwargs.pop("IIRcutoff",0.0)
+        self.FF = kwargs.pop("FF", 1) #bool
+        self._IIRcoef = np.zeros(1)
+        self._IIRout = np.zeros(2)
+        self._FFconst = np.zeros(2)        
+        self._I_record = np.zeros(2)
+            
         self._cavitymode = int(cavitymode)
 
+        ####################################
+        ### Next we perform all the checks #
+        ####################################
+        if not isinstance(cavitymode, CavityMode):
+            raise TypeError("cavitymode has to be an " + "instance of CavityMode")
+
         if self._cavitymode == 1:
             if not isinstance(fbmode, FeedbackMode):
                 err_string = (
-                    "For an active cavity, fbmode has to be defined and an "
+                    "For an active cavity, fbmode has to be defined as an "
                     "instance of FeedbackMode"
                 )
                 raise TypeError(err_string)
@@ -287,6 +328,9 @@ def __init__(
         else:
             self._fbmode = 0
 
+
+        # Here we make checks relating to the passive cavity with FB
+        self.detune = detune
         if self.detune == 0 and self._cavitymode == 3:
             err_string = (
                 "Cannot start passive cavity feedback from zero detuning."
@@ -296,35 +340,46 @@ def __init__(
             )
             raise AtError(err_string)
 
-        self._passive_vset = kwargs.pop("passive_voltage", 0.0)
-        self._beta = ring.beta
-        self._wakefact = -ring.circumference / (clight * ring.energy * ring.beta**3)
-        self._nslice = kwargs.pop("Nslice", 101)
-        self._nturns = kwargs.pop("Nturns", 1)
-        self._nbunch = ring.nbunch
-        self._turnhistory = None  # Defined here to avoid warning
-        self._vbunch = None
-        self._buffersize = buffersize
-        self._windowlength = kwargs.pop("windowlength", 0)
+            
+        check_frequency = np.abs(frequency - self.system_harmonic * ring.rf_frequency)
+        if check_frequency > 1.0:  # 1 Hz is the limit for the float check
+            error_string = (
+                "Cavity must be an integer of rf_frequency, otherwise"
+                "the phi_s computation will be wrong. Please use the detune"
+                "argument but keep the resonant frequency on resonance."
+            )
+            raise AtError(error_string)
 
+        # buffer size and windowlength verification
+        self._windowlength = kwargs.pop("windowlength", 0)
+        self._buffersize = kwargs.pop("buffersize", 0) #is it still needed?
         if self._windowlength > self._buffersize:
             err_string = "The windowlength must be smaller than the buffersize"
             raise ValueError(err_string)
+
+
+
         self._vgen_buffer = np.zeros(1)
         self._vbeam_buffer = np.zeros(1)
         self._vbunch_buffer = np.zeros(1)
+        
 
-        if zcuts is not None:
-            self.ZCuts = zcuts
 
         super().__init__(
             family_name, length, voltage, frequency, harmonic_number, energy, **kwargs
         )
-
+        
+        ts = kwargs.pop("ts", None)
         if ts is None:
             _, ts = get_timelag_fromU0(ring)
         self._ts = ts
 
+        # this one has to come after the super in order to properly initialise
+        cavity_voltage = self.Voltage
+        if self._cavitymode == 3:
+            cavity_voltage = self._passive_vset
+
+
         self._phis = 2 * np.pi * self.Frequency * (-self._ts - self.TimeLag) / clight
 
         # The below is needed because atan2 returns phases between -pi and pi
@@ -340,30 +395,45 @@ def __init__(
         self._vbeam = np.zeros(2)
         self._vgen = np.zeros(4)
 
-        cavity_voltage = self.Voltage
-        if self._cavitymode == 3:
-            cavity_voltage = self._passive_vset
-
+        # Here we define the cavity setpoints, finally
         self._vcav = np.array([cavity_voltage, self._phis])
         self.clear_history(ring=ring)
 
+
+        self._Ig2Vg_vec = np.zeros(ring.harmonic_number*2)
+        self._Ig2Vg_tmp = np.zeros(ring.harmonic_number*2)
+        self._ig_phasor = np.zeros(ring.harmonic_number*2)
+        self._ig_phasor_record = np.zeros(ring.harmonic_number*2)
+        self._dot_output = np.zeros(ring.harmonic_number*2)
+        self._generator_phasor_record = np.zeros(ring.harmonic_number*2)
+        self._beam_phasor_record = np.zeros(ring.harmonic_number*2)                
+        self._cavity_phasor_record = np.zeros(ring.harmonic_number*2)        
+        
+        self._Ig2Vg_mat = np.zeros(ring.harmonic_number**2 * 2)
+        self._vc_previous = np.zeros(self.samplenum*2)
+        self._diff_record = np.zeros(self.recordsize*2)
+        self._samplelist = np.zeros(self.samplelist_length)
+
+        self._vc_list = np.zeros((ring.harmonic_number + self.samplenum)*2)        
+
+        
     def is_compatible(self, other):
         return False
 
     def clear_history(self, ring=None):
         if ring is not None:
-            self._nbunch = ring.nbunch
             current = ring.beam_current
-            nbunch = ring.nbunch
-            self._vbunch = np.zeros((nbunch, 2), order="F")
+            self._vbunch = np.zeros((self.ring_harmonic_number, 2), order="F")
             self._init_bl_params(current)
         tl = self._nturns * self._nslice * self._nbunch
         self._turnhistory = np.zeros((tl, 4), order="F")
+
+
         if self._buffersize > 0:
             self._vgen_buffer = np.zeros((4, self._buffersize), order="F")
             self._vbeam_buffer = np.zeros((2, self._buffersize), order="F")
             self._vbunch_buffer = np.zeros(
-                (self._nbunch, 2, self._buffersize), order="F"
+                (self.ring_harmonic_number, 2, self._buffersize), order="F"
             )
 
     def _init_bl_params(self, current):