/* Subroutine for averaging phase shifter encoder readout */

#include <epicsStdlib.h>
#include <epicsStdioRedirect.h>
#include <epicsMath.h>
#include <dbAccess.h>
#include <dbEvent.h>
#include <dbDefs.h>
#include <recGbl.h>
#include <dbCommon.h>
#include <registryFunction.h>
#include <epicsExport.h>
#include <recSup.h>
#include <aSubRecord.h>

static int encAverage(struct aSubRecord *pasub)
{
	double val = *(double *)pasub->a;
        static double samples[10]; 
        double average = 0;
        static double sum = 0;
        static int i = 0;
        int N = 10;
        static int round = 0;
        
        /* Debugging*/
        /*printf("val = %f, i = %d, round = %d\n", val, i, round);*/

 
        /* Fill the array one time through */
        while(round == 0){
           if(i < (N-1)) {
             samples[i] = val;
             /* Basic running average while array is not full */
             sum += val;
             i++;
             average = sum/(double)i;
             *(double *)pasub->vala = average;
             /*printf("round 0 average = %f i = %d sum = %f\n", average, i, sum);*/
             return(0);
           }
           else {
             round = 1;
             break;
           }
        }
  

        /* Rewind the buffer if needed */
        if (i > (N-1)) {
            i = 0;
        }

        /* Round robin moving average after array has been filled */
        /* Subtract oldest sample */
        sum -= samples[i];
        /* add new sample */
        sum += val;
        samples[i] = val;
        /* new average */
        i++;
        average = sum/(double)N;
        *(double *)pasub->vala = average;
        /*printf("round 1 average = %f i = %d sum = %f\n", average, i, sum);*/

return(0);
}

epicsRegisterFunction(encAverage);