/*
 * under_1ph.c
 *
 *  Created on: 12-05-2014
 *      Author: Krzysztof Jakubczyk
 */

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

#include "under_1ph.h"

int under_1ph_initlog(void *arguments, void *logic)
{
	struct under_1ph_args *args = (struct under_1ph_args *)arguments;
	struct under_1ph_logic *log = (struct under_1ph_logic *)logic;

	log->action_ptr = log_manager.nets_data + (args->io.action_out >> 3);
	log->action_bit_no = args->io.action_out & 0x07;

	if(log->action_ptr > (log_manager.nets_data + sizeof(log_manager.nets_data) - sizeof(u8)))
		return -1;

	log->trig_ptr = log_manager.nets_data + (args->io.trig_out >> 3);
	log->trig_bit_no = args->io.trig_out & 0x07;

	if(log->trig_ptr > (log_manager.nets_data + sizeof(log_manager.nets_data) - sizeof(u8)))
		return -1;

	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->limit_val1 = args->params.trig_val * args->params.trig_val;
	log->limit_val2 = log->limit_val1 * args->params.kp * args->params.kp;

	return 0;
}

void under_1ph(void *arguments, void *logic)
{
	struct under_1ph_args *args = (struct under_1ph_args *)arguments;
	struct under_1ph_logic *log = (struct under_1ph_logic *)logic;

	if(*log->trig_ptr & (1<<log->trig_bit_no))
	{
		if(!(*log->action_ptr & (1<<log->action_bit_no)))
		{
			log->time_cnt++;
			if(log->time_cnt>args->params.t_delay)
				*log->action_ptr|=(1<<log->action_bit_no);
		}

		if(*log->esk_ptr > log->limit_val2)
		{
			*log->action_ptr&=~(1<<log->action_bit_no);
			*log->trig_ptr&=~(1<<log->trig_bit_no);
		}
	}
	else
	{
		if(*log->esk_ptr < log->limit_val1)
		{
			*log->trig_ptr|=(1<<log->trig_bit_no);
			log->time_cnt=0;
		}
	}
}
