/*
 * pkw.c
 *
 *  Created on: 19-01-2017
 *      Author: Krzysztof Jakubczyk
 */

#include <math.h>

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

#include "helper.h"
#include "pkw.h"

float pkw_mem[8] = {0,0,0,0,0,0,0,0};

int pkw_initlog(void *arguments, void *logic)
{
	struct pkw_args *args = (struct pkw_args *)arguments;
	struct pkw_logic *log = (struct pkw_logic *)logic;

	if(set_bit_ptr_mask(args->io.Pwe_in,&log->Pwe,&log->Pwe_bit_mask))
		return -1;

	if(set_float_ptr(args->io.I_float_in,&log->kwampl))
		return -1;

	if(set_bit_ptr_mask(args->io.Kas_in,&log->Kas,&log->Kas_bit_mask))
		return -1;

	if(set_bit_ptr_mask(args->io.Pwy_out,&log->Pwy,&log->Pwy_bit_mask))
		return -1;

	if(set_float_ptr(args->io.Licznik_float_out,&log->licznik))
		return -1;

	log->on = args->params.bity & 0x0001;
    log->Wr = args->params.Wr_; // przepisanie wartosci progowej PKW
    log->id = args->params.id & 0x0007;

//	Semaphore_post(spi_semaphore);

	return 0;
}

void pkw(void *arguments, void *logic)
{
	struct pkw_logic *log = (struct pkw_logic *)logic;
	struct pkw_args *args = (struct pkw_args *)arguments;

	if(log->on)   // jesli funkcja przekaznika aktywna
    {
		if (*log->Kas & log->Kas_bit_mask)
			pkw_mem[log->id]=args->params.kas;

		if (*log->Pwe & log->Pwe_bit_mask)
        {
            if (log->later)
            {
				log->prad = *log->kwampl;
				log->zapis = 1;
            }
            log->later = 0;
        }
        else
        {
            log->later = 1;
        }
    }
}

void pkw_20hz(void *arguments, void *logic)
{
	struct pkw_logic *log = (struct pkw_logic *)logic;

	*log->licznik = pkw_mem[log->id];

	if(log->zapis)
	{
		pkw_mem[log->id] += sqrt(log->prad);
		log->zapis = 0;
	}

	log->prad = 0;

	check_and_set_mask(pkw_mem[log->id]>=log->Wr,log->Pwy,log->Pwy_bit_mask);
}
