/*
 * e_df_3f.c
 *
 *  Created on: 30-06-2016
 *      Author: Krzysztof Jakubczyk
 */

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

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

int e_df_3f_initlog(void *arguments, void *logic)
{
	struct e_df_3f_args *args = (struct e_df_3f_args *)arguments;
	struct e_df_3f_logic *log = (struct e_df_3f_logic *)logic;

	u32 *f_ptr;

	f_ptr = (u32 *)(log_manager.nets_data + (args->io.f1_ptr_in >> 3));
	if((u8 *)f_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *)))
		return -1;
	log->f1=(struct e_f_wyjscie *)*f_ptr;

	if(!*f_ptr)
		return -1;

	f_ptr = (u32 *)(log_manager.nets_data + (args->io.f2_ptr_in >> 3));
	if((u8 *)f_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *)))
		return -1;
	log->f2=(struct e_f_wyjscie *)*f_ptr;

	if(!*f_ptr)
		return -1;

	f_ptr = (u32 *)(log_manager.nets_data + (args->io.f3_ptr_in >> 3));
	if((u8 *)f_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *)))
		return -1;
	log->f3=(struct e_f_wyjscie *)*f_ptr;

	if(!*f_ptr)
		return -1;

	log->df_ptr = (float *)(log_manager.nets_data + (args->io.df_float_out >> 3));
	if((u8 *)log->df_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(float)))
		return -1;

	log->ok_ptr = log_manager.nets_data + (args->io.ok_out >> 3);
	log->ok_bit_no = args->io.ok_out & 0x07;

	if(log->ok_ptr >= (log_manager.nets_data+sizeof(log_manager.nets_data)))
		return -1;

	return 0;
}

struct e_df_3f_wyjscie wylicz_df(struct e_f_wyjscie *f)
{
    int16_t idxp;
    float df;
    float dt;
    struct e_df_3f_wyjscie temp;
    int16_t idx = f->idx_;
    uint16_t temp_OK;
    temp.OK_ = 0;

    if (f->OK_[f->idx_]) // jesli ostatni pomiar czestotliwosci poprawny
    {
        //wyznaczenie indeksu wartosci z poprzedniej iteracji dla okna 1 okres
        idxp = f->idx_ - 2;
        if(idxp<0) idxp +=300;
        //
        if (f->OK_[idxp]) // jesli pomiar czestotliwosci poprawny na koncu okna pomiarowego
        {
            //wyliczenie roznicy czestotliwosci dla okna 1 okres
            df = f->f_[f->idx_] - f->f_[idxp];
            //sprawdzenie wielkosci wyniku, jezeli dostatecznie duzy to wyliczanie pochodnej
            // jezeli za maly to wyliczenie z wiekszego okna
            if(fabs(df)>0.08)
            {
                if (f->OK_[f->idx_ - 1])
                {
                    temp.df_ = df * f->f_[f->idx_];
                } else {
                    if (f->OK_[f->idx_] > f->OK_[idxp])
                    {
                        dt = (float)(f->OK_[f->idx_] - f->OK_[idxp]) * 0.001f;
                    } else {
                        dt = (float)((0xFFFF - f->OK_[idxp]) + f->OK_[f->idx_] + 1) * 0.001f;
                    }
                    temp.df_ = df / dt;
               }
                temp.OK_ = 1;
            }
        }


        if (!temp.OK_)
        {
            //wyznaczenie indeksu wartosci z poprzedniej iteracji dla okna 2 okresy
            idxp = f->idx_ - 4;
            if(idxp<0) idxp +=300;
            //
            if (f->OK_[idxp]) // jesli pomiar czestotliwosci poprawny na koncu okna pomiarowego
            {

                //wyliczenie roznicy czestotliwosci dla okna 2 okresy
                df = f->f_[f->idx_] - f->f_[idxp];
                //sprawdzenie wielkosci wyniku, jezeli dostatecznie duzy to wyliczanie pochodnej
                // jezeli za maly to wyliczenie z wiekszego okna
                if(fabs(df)>0.08)
                {
                    idx = f->idx_;
                    temp_OK = 1;
                    while(idx!=idxp) // sprawdzenie poprawnosci pomiarow w zakresie sprawdzanej czestotliwosci
                    {
                        idx--;
                        if(idx<0) idx +=300;
                        if (!f->OK_[idx]) temp_OK = 0;
                    }

                    if (temp_OK)
                    {
                        temp.df_ = (float)(0.25 * df * (f->f_[f->idx_] + f->f_[idxp]));
                    } else {
                        if (f->OK_[f->idx_] > f->OK_[idxp])
                        {
                            dt = (float)(f->OK_[f->idx_] - f->OK_[idxp]) * 0.001f;
                        } else {
                            dt = (float)((0xFFFF - f->OK_[idxp]) + f->OK_[f->idx_] + 1) * 0.001f;
                        }
                        temp.df_ = df / dt;
                   }
                    temp.OK_ = 1;
                }
            }
        }


        if (!temp.OK_)
        {
            //wyznaczenie indeksu wartosci z poprzedniej iteracji dla okna 4 okresy
            idxp = f->idx_ - 8;
            if(idxp<0) idxp +=300;
            //
            if (f->OK_[idxp]) // jesli pomiar czestotliwosci poprawny na koncu okna pomiarowego
            {

                //wyliczenie roznicy czestotliwosci dla okna 4 okresy
                df = f->f_[f->idx_] - f->f_[idxp];
                //sprawdzenie wielkosci wyniku, jezeli dostatecznie duzy to wyliczanie pochodnej
                // jezeli za maly to wyliczenie z wiekszego okna
                if(fabs(df)>0.08)
                {
                    idx = f->idx_;
                    temp_OK = 1;
                    while(idx!=idxp) // sprawdzenie poprawnosci pomiarow w zakresie sprawdzanej czestotliwosci
                    {
                        idx--;
                        if(idx<0) idx +=300;
                        if (!f->OK_[idx]) temp_OK = 0;
                    }

                    if (temp_OK)
                    {
                        temp.df_ = (float)(0.125 * df * (f->f_[f->idx_] + f->f_[idxp]));
                    } else {
                        if (f->OK_[f->idx_] > f->OK_[idxp])
                        {
                            dt = (float)(f->OK_[f->idx_] - f->OK_[idxp]) * 0.001f;
                        } else {
                            dt = (float)((0xFFFF - f->OK_[idxp]) + f->OK_[f->idx_] + 1) * 0.001f;
                        }
                        temp.df_ = df / dt;
                   }
                    temp.OK_ = 1;
                }
            }
        }




        if (!temp.OK_)
        {
            //wyznaczenie indeksu wartosci z poprzedniej iteracji dla okna 7 okresow
            idxp = f->idx_ - 14;
            if(idxp<0) idxp +=300;
            //
            if (f->OK_[idxp]) // jesli pomiar czestotliwosci poprawny na koncu okna pomiarowego
            {

                //wyliczenie roznicy czestotliwosci dla okna 7 okresow
                df = f->f_[f->idx_] - f->f_[idxp];
                //sprawdzenie wielkosci wyniku, jezeli dostatecznie duzy to wyliczanie pochodnej
                // jezeli za maly to wyliczenie z wiekszego okna
                if(fabs(df)>0.08)
                {
                    idx = f->idx_;
                    temp_OK = 1;
                    while(idx!=idxp) // sprawdzenie poprawnosci pomiarow w zakresie sprawdzanej czestotliwosci
                    {
                        idx--;
                        if(idx<0) idx +=300;
                        if (!f->OK_[idx]) temp_OK = 0;
                    }

                    if (temp_OK)
                    {
                        temp.df_ = (float)(0.07143 * df * (f->f_[f->idx_] + f->f_[idxp]));
                    } else {
                        if (f->OK_[f->idx_] > f->OK_[idxp])
                        {
                            dt = (float)(f->OK_[f->idx_] - f->OK_[idxp]) * 0.001f;
                        } else {
                            dt = (float)((0xFFFF - f->OK_[idxp]) + f->OK_[f->idx_] + 1) * 0.001f;
                        }
                        temp.df_ = df / dt;
                   }
                    temp.OK_ = 1;
                }
            }
        }


        if (!temp.OK_)
        {
            //wyznaczenie indeksu wartosci z poprzedniej iteracji dla okna 10 okresy
            idxp = f->idx_ - 20;
            if(idxp<0) idxp +=300;
            //
            if (f->OK_[idxp]) // jesli pomiar czestotliwosci poprawny na koncu okna pomiarowego
            {

                //wyliczenie roznicy czestotliwosci dla okna 10 okresow
                df = f->f_[f->idx_] - f->f_[idxp];

                // wyliczenie pochodnej dla okna 10 okresow
                idx = f->idx_;
                temp_OK = 1;
                while(idx!=idxp) // sprawdzenie poprawnosci pomiarow w zakresie sprawdzanej czestotliwosci
                {
                    idx--;
                    if(idx<0) idx +=300;
                    if (!f->OK_[idx]) temp_OK = 0;
                }

                if (temp_OK)
                {
                    temp.df_ = (float)(0.05 * df * (f->f_[f->idx_] + f->f_[idxp]));
                } else {
                    if (f->OK_[f->idx_] > f->OK_[idxp])
                    {
                        dt = (float)(f->OK_[f->idx_] - f->OK_[idxp]) * 0.001f;
                    } else {
                        dt = (float)((0xFFFF - f->OK_[idxp]) + f->OK_[f->idx_] + 1) * 0.001f;
                    }
                    temp.df_ = df / dt;
                }
                temp.OK_ = 1;
            }
        }
    }

    return(temp);
}

void e_df_3f_100hz(void *arguments, void *logic)
{
	struct e_df_3f_logic *log = (struct e_df_3f_logic *)logic;

    float temp = 0;
    int16_t temp1 = 0;
    struct e_df_3f_wyjscie temp2;

	temp2 = wylicz_df(log->f1);
    if(temp2.OK_) //jesli pomiar w kanale 1 sprawny
    {
        temp += temp2.df_;
        temp1 +=1;
    }
    temp2 = wylicz_df(log->f2);
    if(temp2.OK_) //jesli pomiar w kanale 2 sprawny
    {
        temp += temp2.df_;
        temp1 +=1;
    }
    temp2 = wylicz_df(log->f3);
    if(temp2.OK_) //jesli pomiar w kanale 3 sprawny
    {
        temp += temp2.df_;
        temp1 +=1;
    }
    if(temp1)//jesli co najmniej jeden pomiar sprawny
    {
    	*log->df_ptr=temp/temp1;
    	*log->ok_ptr|=(1<<log->ok_bit_no);
    } else {
    	*log->df_ptr=temp/temp1;
    	*log->ok_ptr&=~(1<<log->ok_bit_no);
    	//pomiar niesprawny
    }
}

