/*
 * e_df_dt_3f.c
 *
 *  Created on: 05-07-2016
 *      Author: Krzysztof Jakubczyk
 */

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

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

int e_df_dt_3f_initlog(void *arguments, void *logic)
{
	struct e_df_dt_3f_args *args = (struct e_df_dt_3f_args *)arguments;
	struct e_df_dt_3f_logic *log = (struct e_df_dt_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;

	log->DT_ = 1000 * args->params.DT_;

	return 0;
}

void przelicz1f(struct e_df_dt_3f_logic *log)
{
	int i;

	log->idxp = log->w->idx_; // index polozenia drugiej wartosci roznicy (cofniecie o czas T)
	log->OK_ = log->w->OK_[log->idxp];
	log->f = log->w->f_[log->idxp];

    if(log->OK_) // jezeli pomiar czestotliwosci OK to wyliczenie Df/DT
    {
		//szukanie "drugiego konca" okresu danych
		if (log->OK_ > log->DT_)
        {
			log->nr_pp = log->OK_ - log->DT_;
			for (i=0;i<300;i++)
			{
				log->idxp--;
				if (log->idxp<0) log->idxp +=300;
				if ((log->w->OK_[log->idxp]<=log->nr_pp) && log->w->OK_[log->idxp])
				{
					log->DT = (float)((float)log->nr_pp - (float)log->w->OK_[log->idxp] + (float)log->DT_) * 0.001f;
					log->wyDf = (log->f - log->w->f_[log->idxp]) / log->DT;
					log->wyOK = 1;
					break;
				}
				log->wyOK  = 0;
			}
        } else {
			log->nr_pp = (0xFFFF - log->OK_) + log->DT_ + 1;
			for (i=0;i<300;i++)
			{
				log->idxp--;
				if (log->idxp<0) log->idxp +=300;
				if ((log->w->OK_[log->idxp]<=log->nr_pp) && (log->w->OK_[log->idxp] > 0x7FFF))
				{
					log->DT = (float)((float)log->nr_pp - (float)log->w->OK_[log->idxp] + (float)log->wyDf) * 0.001f;
					log->wyDf = (log->f - log->w->f_[log->idxp]) / log->DT;
					log->wyOK = 1;
					break;
				}
				log->wyOK  = 0;
			}
        }
   		//
    } else {
        log->wyOK  = 0;
		log->wyDf = 0;
	}
}

void e_df_dt_3f_100hz(void *arguments, void *logic)
{
	struct e_df_dt_3f_logic *log = (struct e_df_dt_3f_logic *)logic;
    float wynik;
    uint16_t n;

    n = 0;
    wynik = 0;

	log->w = log->f1;
	przelicz1f(log);
    if(log->wyOK) // jesli pomiar wejscia 1 poprawny
    {
        wynik = log->wyDf;
        n++;
    }

	log->w = log->f2;
	przelicz1f(log);
    if(log->wyOK) // jesli pomiar wejscia 2 poprawny
    {
        wynik += log->wyDf;
        n++;
    }
	log->w = log->f3;
	przelicz1f(log);
    if(log->wyOK) // jesli pomiar wejscia 3 poprawny
    {
        wynik += log->wyDf;
        n++;
    }


    if(n)  //jesli rozna od zera to co najmniej jeden pomiar poprawny
    {
    	*log->df_ptr=wynik/n; // wyliczenie sredniej z poprawwnie zmierzonych wejsc
		*log->ok_ptr|=(1<<log->ok_bit_no); // ustawienie stanu sprawnosci pomiaru
    }
    else
    	*log->ok_ptr&=~(1<<log->ok_bit_no);
}

