/*
 * e_mem.c
 *
 *  Created on: 22-03-2017
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

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

#include "helper.h"
#include "e_mem.h"

int e_mem_initlog(void *arguments, void *logic)
{
	struct e_mem_args *args = (struct e_mem_args *)arguments;
	struct e_mem_logic *log = (struct e_mem_logic *)logic;

	if(set_float_ptr(args->io.orta_float_in,&log->orta))
		return -1;
	if(set_float_ptr(args->io.ortb_float_in,&log->ortb))
		return -1;
	if(set_float_ptr(args->io.esk_float_in,&log->esk))
		return -1;

	if(set_float_ptr(args->io.orta_float_out,&log->orta_out))
		return -1;
	if(set_float_ptr(args->io.ortb_float_out,&log->ortb_out))
		return -1;
	if(set_float_ptr(args->io.esk_float_out,&log->esk_out))
		return -1;

	if(set_bit_ptr_struct(args->io.memory_ok_out,&log->memory_OK))
		return -1;
	if(set_bit_ptr_struct(args->io.memory_on_out,&log->memory_ON))
		return -1;

    log->nast_.Ur_ = args->params.napiecie_ *  args->params.napiecie_; // wyliczenie wartosci granicznej
    log->nast_.Up_ = log->nast_.Ur_ * (float)0.81; // wyliczenia wartosci powrotu dla wsp 0.9
    log->nast_.licz_t_ = (unsigned short)(args->params.czas_ * 100); // wyliczenie ilosci petli dla czasu pamieci
    log->korekcja_orta = 1;
    log->korekcja_ortb = 0;
	log->U_OK = 0;
    log->idx_buf = log->idx_buf1 = 0;
    log->stan_pamieci = oczekiwanie_na_OK;
    log->licz_pow = 0;
	log->index_buf_T = 0;

	return 0;
}


void oblicz_okres(struct e_mem_logic *log,struct dane_probki* we)
{
	float T;
	float local1;
	u8 OK = 1;
	float local3;


	OK = log->U_OK;
	if (we->flag)
	{

		we->flag = 0;
		T = we->reszta; // pozostalosc z poprzedniego wyliczania
		T += (we->il_p) * 2500 * LOOP_CYCLE_MS;//dodanie calych okresow probkowania
		local1 = (float)((float)(2500*LOOP_CYCLE_MS) * we->minus1 / (we->minus1 - we->bie));//wyliczenie czasu dopelnienia
		T += local1; // wyliczenie okresu


		if (T > 52700) OK = 0; // jesli wynik mniejszy niz 47,5 Hz
		if (T < 47300) OK = 0; // jesli wynik wiekszy niz 52,5 Hz

		local3 = fabs((float)we->T_last - T);
		if (local3>(30*LOOP_CYCLE_MS)) OK = 0; //jesli zmiana wieksza niz 30mHz to traktujemy wynik za niepewny

		we->T_last = T;
		we->reszta = (2500*LOOP_CYCLE_MS) - local1; // wyliczenie reszty do nastepnego okresu
	}
	else
	{

		OK = 0;
		T = 0;
	}

	if (we->flag) OK = 0;
	log->buf_T[log->index_buf_T].T = T;
	log->buf_T[log->index_buf_T].OK = OK;
	//
//	log->buf_T[log->index_buf_T].OK=1;
//	log->buf_T[log->index_buf_T].T = 50000;
	//

	log->index_buf_T++;
	if (log->index_buf_T>19)
	{
		log->index_buf_T = 0;
	}
}

void e_mem(void *arguments, void *logic)
{
	struct e_mem_logic *log = (struct e_mem_logic *)logic;

	short idx_temp;

	log->w.kwampl_ = *log->esk;
	log->w.orta_ = *log->orta;
	log->w.ortb_ = *log->ortb;

//wykrywanie przejscia przez zero

		if ((log->dane.minus1>0)&&(log->w.orta_<=0))	//przejscie z plusa do minusa
		{
			log->dane.plus.flag	= 1;
			log->dane.plus.il_p = log->dane.plus_il_p;
			log->dane.plus_il_p = 0;
			log->dane.plus.bie = log->w.orta_;
			log->dane.plus.minus1 = log->dane.minus1;

		} else {
			log->dane.plus_il_p++;//=LOOP_CYCLE_MS;
		}

		if ((log->dane.minus1<0)&&(log->w.orta_>=0)) //przejscie z minusa do plusa
		{

			log->dane.minus.flag = 1;
			log->dane.minus.il_p = log->dane.minus_il_p;
			log->dane.minus_il_p = 0;
			log->dane.minus.bie = log->w.orta_;
			log->dane.minus.minus1 = log->dane.minus1;
		} else {
			log->dane.minus_il_p++;//=LOOP_CYCLE_MS;
		}
		log->dane.minus1 = log->w.orta_;
//--------------------------------------------------------------------


		sprawdz_P(
				&log->U_OK,
				log->w.kwampl_>log->nast_.Ur_,
				log->w.kwampl_<log->nast_.Up_,
				&log->licz_pow,
				72,2
				);



	log->stan_.memory_ON_ = 0;
	log->stan_.memory_OK_ = 0;

	switch(log->stan_pamieci)
	{
		case oczekiwanie_na_OK:
			//aktualizacja buforow danych
			log->buf_orta[log->idx_buf][0] = log->w.orta_;
			log->buf_ortb[log->idx_buf][0] = log->w.ortb_;
			if(log->f.OK)	log->f_kor = log->f.T;

			if(log->f.OK && log->U_OK)
			{
				log->stan_pamieci = napiecie_OK_0;
				log->stan_.memory_OK_ = 1;
			}
			log->stan_.orta_ = log->w.orta_;
			log->stan_.ortb_ = log->w.ortb_;
			log->stan_.kwampl_ = log->w.kwampl_;
			break;

		case napiecie_OK_0:
			//wystawienie wyjsc
			idx_temp = log->idx_buf - (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS);
			if(idx_temp<0) idx_temp += MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2;
			log->stan_.orta_ = log->buf_orta[idx_temp][0];
			log->stan_.ortb_ = log->buf_ortb[idx_temp][0];
			log->stan_.kwampl_ = (log->stan_.orta_ * log->stan_.orta_ +
					log->stan_.ortb_ * log->stan_.ortb_) * 0.5f;
			//
			//aktualizacja buforow danych
			if (log->U_OK)
			{
				log->buf_orta[log->idx_buf][0] = log->w.orta_;
				log->buf_ortb[log->idx_buf][0] = log->w.ortb_;
				if (log->f.OK)
				{
					log->f_kor = log->f.T;
				}
			} else {
				log->stan_pamieci = napiecie_z_pamieci_0;
				log->idx_buf_start = log->idx_buf;
				log->idx_buf1 = 0;
			}
			log->stan_.memory_OK_ = 1;
			break;

		case napiecie_OK_1:
			//wystawienie wyjsc
			idx_temp = log->idx_buf - (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS);
			if(idx_temp<0) idx_temp +=(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2;
			log->stan_.orta_ = log->buf_orta[idx_temp][1];
			log->stan_.ortb_ = log->buf_ortb[idx_temp][1];
			log->stan_.kwampl_ = (log->stan_.orta_ * log->stan_.orta_ +
					log->stan_.ortb_ * log->stan_.ortb_) * 0.5f;
			//
			//aktualizacja buforow danych
			if (log->U_OK)
			{
				log->buf_orta[log->idx_buf][1] = log->w.orta_;
				log->buf_ortb[log->idx_buf][1] = log->w.ortb_;
				if (log->f.OK)
				{
					log->f_kor = log->f.T;
				}
			} else {
				log->stan_pamieci = napiecie_z_pamieci_1;
				log->idx_buf_start = log->idx_buf;
				log->idx_buf1 = 0;
			}
			log->stan_.memory_OK_ = 1;
			break;

		case napiecie_z_pamieci_0:
			//aktualizacja buforow danych drugiego banku danych
			log->buf_orta[log->idx_buf][1] = log->w.orta_;
			log->buf_ortb[log->idx_buf][1] = log->w.ortb_;
			if(log->f.OK) log->f_kor_new = log->f.T;
			if(log->f.OK && log->U_OK)
			{
				log->stan_pamieci = napiecie_OK_1;
				log->f_kor = log->f_kor_new;
				log->stan_.memory_OK_ = 1;
			}
			//wystawienie wyjsc

			log->idx_buf1++; // przesuniecie pozycji w buforze probek
			if(log->idx_buf1>(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)-1)log->idx_buf1 = 0;
			idx_temp = log->idx_buf_start + log->idx_buf1;
			if(idx_temp>(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2-1) idx_temp -= (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2;
			log->stan_.orta_ = log->buf_orta[idx_temp][0] *log->korekcja_orta
							- log->buf_ortb[idx_temp][0] * log->korekcja_ortb
							; // wyliczenie skladowej ortogonalnej a wyniku
			log->stan_.ortb_ = log->buf_ortb[idx_temp][0] * log->korekcja_orta
							+ log->buf_orta[idx_temp][0] * log->korekcja_ortb
							; // wyliczenie skladowej ortogonalnej b wyniku
			log->stan_.kwampl_ = (log->stan_.orta_ * log->stan_.orta_ +
					log->stan_.ortb_ * log->stan_.ortb_) * 0.5f;

		//
			log->stan_.memory_ON_ = 1;
			break;

		case napiecie_z_pamieci_1:
			//aktualizacja buforow danych drugiego banku danych
			log->buf_orta[log->idx_buf][0] = log->w.orta_;
			log->buf_ortb[log->idx_buf][0] = log->w.ortb_;
			if(log->f.OK) log->f_kor_new = log->f.T;
			if(log->f.OK && log->U_OK)
			{
				log->stan_pamieci = napiecie_OK_0;
				log->f_kor = log->f_kor_new;
				log->stan_.memory_OK_ = 1;
			}
			//wystawienie wyjsc

			log->idx_buf1++; // przesuniecie pozycji w buforze probek
			if(log->idx_buf1>(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)-1)log->idx_buf1 = 0;
			idx_temp = log->idx_buf_start + log->idx_buf1;
			if(idx_temp>(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2-1) idx_temp -= (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2;
			log->stan_.orta_ = log->buf_orta[idx_temp][1] * log->korekcja_orta
							- log->buf_ortb[idx_temp][1] * log->korekcja_ortb
							; // wyliczenie skladowej ortogonalnej a wyniku
			log->stan_.ortb_ = log->buf_ortb[idx_temp][1] * log->korekcja_orta
							+ log->buf_orta[idx_temp][1] * log->korekcja_ortb
							; // wyliczenie skladowej ortogonalnej b wyniku
			log->stan_.kwampl_ = (log->stan_.orta_ * log->stan_.orta_ +
					log->stan_.ortb_ * log->stan_.ortb_) * 0.5f;

		//
			log->stan_.memory_ON_ = 1;
			break;


	}



	log->idx_buf++; // przesuniecie pozycjiw buforze probek
    if(log->idx_buf>(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2-1)log->idx_buf = 0;

    check_and_set_struct(log->stan_.memory_OK_,&log->memory_OK);
    check_and_set_struct(log->stan_.memory_ON_,&log->memory_ON);
    *log->esk_out=log->stan_.kwampl_;
    *log->orta_out=log->stan_.orta_;
    *log->ortb_out=log->stan_.ortb_;
}

void e_mem_100hz(void *arguments, void *logic)
{
	struct e_mem_logic *log = (struct e_mem_logic *)logic;

	float kat_przesuniecia;
	float a = 1;
	float b = 0;
	unsigned short idx = 0;
	float suma = 0;
	unsigned short i;

		//jesli nie wykryto przejsc przez zero zbyt dlugo
		if (log->dane.minus_il_p > 30/LOOP_CYCLE_MS)
		{
			log->dane.minus_il_p = 0;
		}
		if (log->dane.plus_il_p > 30/LOOP_CYCLE_MS)
		{
			log->dane.plus_il_p = 0;
		}
		//

		oblicz_okres(log,&log->dane.plus);
		oblicz_okres(log,&log->dane.minus);

		//wyliczenie jako srenia waona

		for (i=0;i<20;i++)
		{
			if (log->buf_T[i].OK)
			{
				idx++;
				suma += (float)log->buf_T[i].T;
			}
		}
		if (idx>8)
		{
			log->f.T = (float)idx * (2500000) / suma;
			log->f.OK = 1;
		}
		else
		{
			log->f.OK = 0;
			log->f.T = 0;
		}


		if (log->stan_.memory_ON_)
		{
			log->licznik_czasu++; // odliczanie czasu aktywnosci pamieci
			    if(log->licznik_czasu>log->nast_.licz_t_)
				{
			    	log->stan_pamieci = oczekiwanie_na_OK;// wylaczenie pamieci po czasie
			    	log->licz_pow = 0;
				}
			kat_przesuniecia = ((float)3.1416 *log->licznik_czasu + (float)0.62832)*(1-log->f_kor*0.02f);
	        a = cos(kat_przesuniecia);
	        b = sin(kat_przesuniecia);
		} else {
			log->licznik_czasu = 0;
		}
		log->korekcja_orta = a;
		log->korekcja_ortb = b;
}