/*
 * voltage_analyzer.c
 *
 * Created: 14.5.2014 21:44:43
 *  Author: Zdenda
 */ 

#include "voltage_analyzer.h"

#define COEF_SCALE   (1<<14)
const int16_t b0 = (int16_t)(0.99749302132352868 * COEF_SCALE);
const int16_t b1 = (int16_t)(-1.9910493934026068 * COEF_SCALE);
const int16_t b2 = (int16_t)(0.99749302132302764 * COEF_SCALE);

const int16_t a1 = (int16_t)( 1.9910493934026063 * COEF_SCALE);
const int16_t a2 = (int16_t)(-0.9949860426465561 * COEF_SCALE);

// vzorek ve formátu Q11
int16_t filtr3( int16_t sample)
{
    static int16_t ui1 = 0;
    static int16_t ui2 = 0;
    static int16_t yi1 = 0;
    static int16_t yi2 = 0;
    
    int32_t out;

    // koeficienty z Matlab funkce filterbuilder
    // Type : Notch, fc = 50 Hz, fs = 5000 Hz
    out = 0;
    
    out += (int32_t) b0*sample;
    out += (int32_t) b1*ui1;
    out += (int32_t) b2*ui2;
    out += (int32_t) a1*yi1;
    out += (int32_t) a2*yi2;

    out /= COEF_SCALE;
    
    //out = (b0*sample)/factor/2 + (b1*ui1)/factor/2 + (b2*ui2)/factor/2 + (a1*yi1)/factor/2 + (a2*yi2)/factor/2;
    if (out>32000) out = 32000;
    if (out<-32000) out = -32000;

    
    // aktualizace pamet bunek
    yi2 = yi1;
    yi1 = (int16_t) out;

    ui2 = ui1;
    ui1 = sample;

    return (int16_t)out;
}


void distorsion_rms( int16_t sample, int16_t after_filter, int16_t * distorsion, int16_t * line_rms)
{
    //static members
    static unsigned char state_bef = 0;
    static int16_t count = 0;
    static int32_t integral = 0;
    static int32_t integral2 = 0;
    static int16_t output = 0;
    static int16_t output2 = 0;
    
    unsigned char state_act = 0;
   
    // state before
    if (sample > 0) { state_act = 1; }
    else { state_act = 0; }
    
    // zero crossing detection
    if ( (state_bef == 0 ) && (state_act == 1) )
    {
        if (count > 5)
        {
            output = (integral/count)>>11;
            output2 = (integral2/count)>>11;
        }
        
        integral = 0;
        integral2 = 0;
        count = 0;
    }
    
    integral += ((int32_t)sample*sample);
    integral2 += ((int32_t)after_filter*after_filter);

    count++;
   
    state_bef = state_act;
    
    //if(output>(1.5*0.707))
    //{output = -1;}
    
    //return output2;
    if (output>0)
    {
        //*distorsion = ((int32_t)output2*(1<<11))/(output);
		 *distorsion = output2;
    } 
    else
    {
        *distorsion = 0;
    }
    
    *line_rms = output;
}


#define UPPER_TH    (750)
#define LOWER_TH    (500)
#define DIST_TH     (500)

unsigned char kontrola( int16_t distorsion, int16_t rms)
{
   unsigned char fault = 0;
   
   if (distorsion > DIST_TH)
   {
        fault |= DIST_FAULT;
   }
   
   if (rms > UPPER_TH)
   {
        fault |= UPPER_LIMIT_FAULT;
   }

   if (rms < LOWER_TH)
   {
        fault |= LOWER_LIMIT_FAULT;
   }
   
   return fault;
}
