/*
 * e_f.c
 *
 *  Created on: 28-06-2016
 *      Author: Krzysztof Jakubczyk
 */

#include "tdefs.h"
#include "misc.h"

#include "e_f.h"
#include "analog_in.h"
#include <math.h>

int e_f_initlog(void *arguments, void *logic)
{
	struct e_f_args *args = (struct e_f_args *)arguments;
	struct e_f_logic *log = (struct e_f_logic *)logic;
	struct analog_in_params *an_params;
	u32 *an_params_ptr;
	u32 *buf_ptr;
	u32 *f_ptr;

	an_params_ptr = (u32 *)(log_manager.nets_data + (args->io.param_an_ptr_in >> 3));
	if((u8*)an_params_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *)))
		return -1;

	if(!*an_params_ptr) // param in not connected or wrong elements order (analog_in should be run before)
		return -1;

	an_params=(struct analog_in_params *)*an_params_ptr;

	buf_ptr = (u32 *)(log_manager.nets_data + (args->io.buf_u16_ptr_in >> 3));
	if((u8*)buf_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *)))
		return -1;

	if(!*buf_ptr) // param in not connected or wrong elements order (analog_in should be run before)
		return -1;

	log->buf_ptr=(u16*)*buf_ptr;

	f_ptr = (u32 *)(log_manager.nets_data + (args->io.f_ptr_out >> 3));
	if((u8 *)f_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *)))
		return -1;

	*f_ptr = (u32)&log->wy;

	log->nast_.Ublr_ = args->params.Ubl_;/// nast_.w->nast->mnoznik;
	log->nast_.Ublr_ *= log->nast_.Ublr_;
    log->nrprobki = 1;
	log->nast_.ofset = 32767 + an_params->offset;
	log->nast_.mnoznik1 = an_params->multiplier;
	log->nast_.mnoznik = an_params->multiplier;
	log->nast_.mnoznik *= log->nast_.mnoznik;

	log->mn[0]=1.0f;
	log->mn[1]=0.5f;
	log->mn[2]=0.33333333f;

	return 0;
}

/**
Wylicza czestotliwosc przebiegu z danych pozyskanych od funkcji przelicz_1000Hz
\param: dane_wew& d - dane do obliczen wedlug struktury
        WA::Wyjscie* w - wskaznik do danych probek
*/
void e_f_wylicz_f(struct e_f_dane_do_wyliczenia *d, struct e_f_logic *log)
{
    float dopelnienie;
    float temp, tempmax, tempmin,p1temp;
    uint16_t temp1;
	int i1;
    float okres;
    float probka_minus1,probka;
    float il_p;
    u8 cnt;

	log->idx_f++;
	if(log->idx_f>4)
		log->idx_f = 0;
    log->f_cz.f_OK_[log->idx_f] = 1;


//szukanie probek przejscia przez zero
	cnt=0;

	do
	{
		cnt++;
		i1 = d->nrp - cnt;

		if (i1<0)
			i1 += SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2;

		if(cnt==1)
			probka = (float)log->buf_ptr[d->nrp] - log->nast_.ofset;
		else
			probka = probka_minus1;

		probka_minus1 = (float)log->buf_ptr[i1] - log->nast_.ofset;
	}
	while((probka * probka_minus1) > 0 && (d->skok-cnt>=0));

	if ((probka * probka_minus1) > 0)
		log->f_cz.f_OK_[log->idx_f] = 0;
	else
		il_p = d->skok - cnt;

	//wyliczenie pojedynczej iteracji
	d->il_p_ += il_p;

    dopelnienie = probka_minus1 / (probka_minus1 - probka);
    okres = dopelnienie + d->reszta_ + (float)d->il_p_;
	p1temp = 1 / okres;
    temp = (float)(SAMPLES_PER_MS*1000) * p1temp;
    //

    log->f_cz.f_[log->idx_f] = temp; // zapisanie czestotliwosci pojedynczej iteracji do bufora podrecznego

    if ((temp < 38) || (temp > 72))
    	log->f_cz.f_OK_[log->idx_f] = 0; // sprawdzenie warunku zakresu zmiennosci

	int16_t n = okres;

	d->war_sk_= 0;
	d->war_sr_= 0;

	i1 = d->nrp - 1;

	int i;

	for(i=0;i<n;i++)
	{
		if (i1<0) i1 += SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2;
		probka = (float)log->buf_ptr[i1] - log->nast_.ofset;
		d->war_sk_ += probka * probka;
		d->war_sr_ += fabs(probka);
		i1--;
	}
	d->war_sk_ *= log->nast_.mnoznik;
	d->war_sr_ *= log->nast_.mnoznik1;

	d->war_sk_ *= p1temp;// wyliczenie wartosci skutecznej
    d->war_sr_ *= p1temp;// wyliczenie wartosci sredniej wyprostowanej

    //sprawdzenie poprawnosci wyniku czastkowego
    if(d->war_sk_ < log->nast_.Ublr_)
    	log->f_cz.f_OK_[log->idx_f] = 0;    //sprawdzenie warunku napiecia blokady

    temp = fabs(d->war_sk_ - d->war_sk_pop_);
    if(temp > 0.1 * d->war_sk_) log->f_cz.f_OK_[log->idx_f] = 0; //sprawdzenie warunku zmiennosci - blokada pomiaru jesli zmiana amplitudy

	temp = (d->war_sr_ * d->war_sr_); //wspolczynnik ksztaltu powinien byc 1.1107 * 1.1107 = 1.2337
    if ((d->war_sk_ < (float)1.20 * temp) || (d->war_sk_ > (float)1.27 * temp))
	{
		log->f_cz.f_OK_[log->idx_f] = 0; // sprawdzenie wspolczynnika ksztaltu sinusoidy
	}

	if(fabs(okres - log->okres_n_minus1) > (float)2.0)
		log->f_cz.f_OK_[log->idx_f] = 0; //kiedy duza roznica to blad pomiaru

	// wyliczenie czestotliwosci z pieciu iteracji i zapisanie stanu na wyjscie
    temp = 0;
    temp1 = 0;
    tempmin = 10000.0;
    tempmax = 0;

    for (i=0;i<5;i++)   //usrednienie pomiaru z odrzuceniem dwoch skrajnych najgorszych wynikow
    {
        if(log->f_cz.f_OK_[i])
        {
            temp1++;
            temp += log->f_cz.f_[i];
            if(tempmin > log->f_cz.f_[i]) tempmin = log->f_cz.f_[i];
            if(tempmax < log->f_cz.f_[i]) tempmax = log->f_cz.f_[i];
        }
    }

    log->wy.idx_++;
    if(log->wy.idx_>299)
    	log->wy.idx_ = 0;

    if(temp1>2) //jesli co najmniej 3 pomiary OK to funkcja wystawia ze czestotliwosc jest OK
    {
        log->wy.OK_[log->wy.idx_] = d->nrprobki;
		log->wy.f_[log->wy.idx_] = (temp - tempmin - tempmax) * log->mn[temp1-3];
    } else
    {
        log->wy.OK_[log->wy.idx_] = 0;
		log->wy.f_[log->wy.idx_] = 0;
    }

    d->reszta_ = 1 - dopelnienie;
    d->war_sk_pop_ = d->war_sk_; //zapamietanie poprzedniej wartosci skutecznej
    d->war_sk_ = 0; //zerowanie licznika wartosci skutecznej przy przejsciu przez zero
    d->war_sr_ = 0; //zerowanie licznika wartosci sredniej wyprostowanej przy przejsciu przez zero
    d->il_p_ = d->skok - 1 - il_p;//zerowanie licznika pelnych probek(odliczanie nowego okresu)
    log->okres_n_minus1 = okres;
}

void e_f(void *arguments, void *logic)
{
	struct e_f_logic *log = (struct e_f_logic *)logic;

    u8 przejscie_z_plusa = 0;
    u8 przejscie_z_minusa = 0;

 	log->nrp =  bus_an_cur_sample_num;
	log->skok = log->nrp - log->nrp_minus1;

	if(log->skok<0)
		log->skok +=SAMPLES_PER_MS*MAIN_FREQ_PERIOD_MS*2;

//    probka_n_ = w.probka_;
	log->probka_n_ = (float)log->buf_ptr[log->nrp] - log->nast_.ofset;

    //szukanie przejsc przez zero

		if ((log->probka_n_minus1 <0 ) && (log->probka_n_ >= 0))
			przejscie_z_minusa = 1;
		if ((log->probka_n_minus1 >0 ) && (log->probka_n_ <= 0))
			przejscie_z_plusa = 1;


        //


        if(przejscie_z_plusa && (log->plus.il_p_ > 35*SAMPLES_PER_MS/3) && !log->d_plus.wylicz)
        {
			log->d_plus.il_p_ += log->plus.il_p_;
			log->plus.il_p_ = 0;
			log->d_plus.wylicz = 1;
			log->d_plus.skok = log->skok;
			log->d_plus.nrp = log->nrp;
			log->d_plus.nrprobki = log->nrprobki;
        }
        else
        {
            log->plus.il_p_ += log->skok; //wyliczanie ilosci pelnych probek w okresie dla okresu od plusa
        }

        if(przejscie_z_minusa && (log->minus.il_p_ > 35*SAMPLES_PER_MS/3) && !log->d_minus.wylicz)
        {
			log->d_minus.il_p_ += log->minus.il_p_;
			log->minus.il_p_ = 0;
			log->d_minus.wylicz = 1;
			log->d_minus.skok = log->skok;
			log->d_minus.nrp = log->nrp;
			log->d_minus.nrprobki = log->nrprobki;
        }
        else
        {
            log->minus.il_p_ += log->skok; //wyliczanie ilosci pelnych probek w okresie dla okresu od minusa
        }

		log->probka_n_minus1 = log->probka_n_;//zapamietanie poprzedniej probki
		log->nrp_minus1 = log->nrp;
        log->nrprobki++;
        if(!(log->nrprobki))
        	log->nrprobki++;


        {
            if ((log->plus.il_p_ > 100) || (log->minus.il_p_ > 100)) // jezeli brak probkowania
            {
                log->plus.il_p_ = 0;
                log->minus.il_p_ = 0;
                log->wy.OK_[log->wy.idx_] = 0;
                log->wy.idx_++;
                if(log->wy.idx_>299)
                	log->wy.idx_ = 0;
                log->wy.OK_[log->wy.idx_] = 0;
            }

        	if(log->d_plus.wylicz)
        	{
        		e_f_wylicz_f(&log->d_plus,log);
        		log->d_plus.wylicz = 0;
        	}

        	if(log->d_minus.wylicz)
        	{
        		e_f_wylicz_f(&log->d_minus,log);
        		log->d_minus.wylicz = 0;
        	}
        }
}
/*
void e_f_100hz(void *arguments, void *logic)
{
	struct e_f_logic *log = (struct e_f_logic *)logic;

    if ((log->plus.il_p_ > 100) || (log->minus.il_p_ > 100)) // jezeli brak probkowania
    {
        log->plus.il_p_ = 0;
        log->minus.il_p_ = 0;
        log->wy.OK_[log->wy.idx_] = 0;
        log->wy.idx_++;
        if(log->wy.idx_>299)
        	log->wy.idx_ = 0;
        log->wy.OK_[log->wy.idx_] = 0;
    }

	if(log->d_plus.wylicz)
	{
		e_f_wylicz_f(&log->d_plus,log);
		log->d_plus.wylicz = 0;
	}

	if(log->d_minus.wylicz)
	{
		e_f_wylicz_f(&log->d_minus,log);
		log->d_minus.wylicz = 0;
	}
}
*/
