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

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

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

int e_df_dt_initlog(void *arguments, void *logic)
{
	struct e_df_dt_args *args = (struct e_df_dt_args *)arguments;
	struct e_df_dt_logic *log = (struct e_df_dt_logic *)logic;

	u32 *f_ptr;

	f_ptr = (u32 *)(log_manager.nets_data + (args->io.f_ptr_in >> 3));
	if((u8 *)f_ptr > (log_manager.nets_data+sizeof(log_manager.nets_data) - sizeof(void *)))
		return -1;
	log->f=(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 e_df_dt_100hz(void *arguments, void *logic)
{
	struct e_df_dt_logic *log = (struct e_df_dt_logic *)logic;

	struct e_f_wyjscie *w = log->f;
 	uint16_t i;
	uint16_t nr_pp;
    int16_t idxp = w->idx_; // index polozenia drugiej wartosci roznicy (cofniecie o czas T)
	uint16_t OK_ = w->OK_[idxp];
	float f = w->f_[idxp];
	float DT;

    if(OK_) // jezeli pomiar czestotliwosci OK to wyliczenie Df/DT
    {

		//szukanie "drugiego konca" okresu danych
		if (OK_ > log->DT_)
        {
			nr_pp = OK_ - log->DT_;
			for (i=0;i<300;i++)
			{
				idxp--;
				if (idxp<0) idxp +=300;
				if ((w->OK_[idxp]<=nr_pp) && w->OK_[idxp])
				{
					DT = (float) (nr_pp - w->OK_[idxp] + log->DT_) * 0.001f;
					*log->df_ptr =  (f - w->f_[idxp]) / DT;
					*log->ok_ptr|=(1<<log->ok_bit_no);
					break;
				}
				*log->ok_ptr&=~(1<<log->ok_bit_no);
			}
        } else {
			nr_pp = (0xFFFF - OK_) + log->DT_ + 1;
			for (i=0;i<300;i++)
			{
				idxp--;
				if (idxp<0) idxp +=300;
				if ((w->OK_[idxp]<=nr_pp) && (w->OK_[idxp] > 0x7FFF))
				{
					DT = (float)((float)nr_pp - (float)w->OK_[idxp] + *log->df_ptr) * 0.001f;
					*log->df_ptr = (f - w->f_[idxp]) / DT;
					*log->ok_ptr|=(1<<log->ok_bit_no);
					break;
				}
				*log->ok_ptr&=~(1<<log->ok_bit_no);
			}
        }
   		//

    } else {
		*log->ok_ptr&=~(1<<log->ok_bit_no);
		*log->df_ptr = 0;
	}
}

