#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <libq.h>
#include "xc.h"
#include "math.h"
#include "config.h"
#include "extern.h"
#include "CalibrationConstants.h"
#include "conv.h"
#include "types.h"


unsigned int AdjustScaleFactor(float N)  
{
    static int last_CAN_packet_rate = 0;  //Ensure setting of this value the first time through.
    static int last_gain = 0;  //Ensure setting of this value the first time through.
    static unsigned long k = 1;
    static float Mlast, Slast;
    float M, S;
    float StdDev;
    int f_StdDev;
    static long delta = 0;
    int local_scale_factor;
    

    if((last_CAN_packet_rate != CAN_packet_rate.Value) || ( last_gain != ScaleIntegrationGain.Value))  //Reset time period if CAN_packet_rate.Value or ScaleIntegrationGain.Value is changed by user.
    {
        k = 1;
        last_CAN_packet_rate = CAN_packet_rate.Value; 
        last_gain = ScaleIntegrationGain.Value;
    }
        
    if (k == 1) //Start of period
    {
        Mlast = N;
        Slast = 0;
    }

    M = Mlast + (N - Mlast) / k;
    S = Slast + (N - Mlast) * (N - M);
    k++;
    Mlast = M;
    Slast = S;
    StdDev = S / k;
    f_StdDev = (int) sqrtf(StdDev);
    //Compute variance of RPM signal over last specified period
    if (k >= 60 * GAIN_SCHEDULE_INTEGRATION_TIME * CAN_packet_rate.Value) //Gain schedule integration period has passed
    {
        k = 1; //N cycles form 0 to GAIN_SCHEDULE_INTEGRATION_TIME-1
        
        if (StatusBitFields.GainScheduleMode) //If in Gain scheduling mode, adjust scale factor according to standard deviation and target.
        {
            if((delta < -1000) || (delta > 1000))
                delta = 0;
            delta += ((long) ScaleIntegrationGain.Value) * (f_StdDev - GainScheduleRPMStdDevTarget.Value)*1000*GAIN_SCHEDULE_INTEGRATION_TIME/(((long) 60)*ConvertEngUnitsToFract(1000, RPMs_cnt, RPMs_eng));
            lTemp1 = delta;
            // Keep delta magnitude smaller than MIN_SCALE_FACTOR to avoid underflow on scale factor which is a char...
            if (delta/1000 > scale_factor.LowerLimit)
                delta = 1000*scale_factor.LowerLimit;
            if (delta/1000 < -scale_factor.LowerLimit)
                delta = -1000*scale_factor.LowerLimit;

            local_scale_factor = scale_factor.Value + delta/1000;
            
            if (local_scale_factor > scale_factor.UpperLimit)
                local_scale_factor = scale_factor.UpperLimit;
            if (local_scale_factor < scale_factor.LowerLimit)
                local_scale_factor = scale_factor.LowerLimit;
            
            scale_factor.Value = local_scale_factor;
            
        }
    }

    return f_StdDev;
}