/*
 * e_du_dt.c
 *
 *  Created on: 06-07-2016
 *      Author: Krzysztof Jakubczyk
 *
 *      Wylicza wartosc pochodnej wartosci skutecznej napiecia
 *
 *
 */

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

#include "e_du_dt.h"
#include <math.h>

int e_du_dt_initlog(void *arguments, void *logic)
{
	struct e_du_dt_args *args = (struct e_du_dt_args *)arguments;
	struct e_du_dt_logic *log = (struct e_du_dt_logic *)logic;

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

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

	log->start=1;

	return 0;
}

void e_du_dt_100hz(void *arguments, void *logic)
{
	struct e_du_dt_logic *log = (struct e_du_dt_logic *)logic;

    int16_t idx_n,idx_n1;
    float temp;
	float dU_;

    log->index++;
    if (log->index>99)
    {
        log->index = 0;
        log->start = 0;
    }

	log->srednia_z_10ms[log->index] = sqrt(*log->esk_ptr);

    idx_n = log->index; // polozenie biezacej danej w buforze

	if (log->srednia_z_10ms[idx_n] > (float)0.05)
	{
		idx_n1 = idx_n - 99;
		if (idx_n1 < 0) idx_n1 += 100; // polozenie poprzedniej probki w buforze
		if (log->srednia_z_10ms[idx_n1] > (float) 0.05)
		{
			dU_ = (log->srednia_z_10ms[idx_n] - log->srednia_z_10ms[idx_n1]) * 1.010101f;/// (float)0.99; //pochodna z oknem 990 ms
		} else {
			dU_ = 0;
		}


		idx_n1 = idx_n - 20;
		if(idx_n1 < 0) idx_n1 += 100; // polozenie poprzedniej probki w buforze
		temp = log->srednia_z_10ms[idx_n] - log->srednia_z_10ms[idx_n1]; //pochodna z oknem 200 ms
        if((fabs(temp) > 0.02) && (log->srednia_z_10ms[idx_n1] > (float)0.05))
        {
            dU_ = temp * 5.0f;//  / (float)0.2; // jesli dostatecznie duza roznica (1%) to wynik pochodnej z 200ms
		}

		idx_n1 = idx_n - 4;
		if(idx_n1 < 0) idx_n1 += 100; // polozenie poprzedniej probki w buforze
		temp = log->srednia_z_10ms[idx_n] - log->srednia_z_10ms[idx_n1]; //pochodna z oknem 200 ms
        if((fabs(temp) > 0.02) && (log->srednia_z_10ms[idx_n1] > (float)0.05))
        {
            dU_ = temp * 25.0f;// / (float)0.04; // jesli dostatecznie duza roznica (1%) to wynik pochodnej z 40ms
		}

		idx_n1 = idx_n - 1;
		if(idx_n1 < 0) idx_n1 += 100; // polozenie poprzedniej probki w buforze
		temp = log->srednia_z_10ms[idx_n] - log->srednia_z_10ms[idx_n1]; //pochodna z oknem 10 ms
        if((fabs(temp) > 0.02) && (log->srednia_z_10ms[idx_n1] > (float)0.05))
        {
            dU_ = temp * 100.0f;// / (float)0.01; // jesli dostatecznie duza roznica (1%) to wynik pochodnej z 10ms
		}


	} else {
		dU_ = 0;
	}

    if(log->start) dU_ = 0;// zerowanie wyjscia po bootowaniu do czasu napelnienia buforu

    *log->du_ptr=dU_;
}
