/*
 * dev_ctrl.c
 *
 *  Created on: 07-02-2017
 *      Author: Krzysztof Jakubczyk
 */

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

#include "dev_ctrl.h"
#include "helper.h"

u32 dev_ctrl_state = 0;
u32 powered_off=220;
u32 service_mode = 0;

int dev_ctrl_initlog(void *arguments, void *logic)
{
	struct dev_ctrl_args *args = (struct dev_ctrl_args *)arguments;
	struct dev_ctrl_logic *log = (struct dev_ctrl_logic *)logic;

	if(set_bit_ptr_mask(args->io.test_we_in,&log->test_we,&log->test_we_bit_mask))
		return -1;
	if(set_bit_ptr_mask(args->io.test_wy_in,&log->test_wy,&log->test_wy_bit_mask))
		return -1;
	if(set_bit_ptr_mask(args->io.test_log_in,&log->test_log,&log->test_log_bit_mask))
		return -1;
	if(set_bit_ptr_mask(args->io.blok_wy_in,&log->blok_wy,&log->blok_wy_bit_mask))
		return -1;
	if(set_bit_ptr_mask(args->io.test_an_in,&log->test_an,&log->test_an_bit_mask))
		return -1;
	if(set_bit_ptr_struct(args->io.pps2_in,&log->pps2))
		return -1;
	if(set_bit_ptr_struct(args->io.service_mode_in,&log->service))
		return -1;
	if(set_bit_ptr_mask(args->io.pwr1_ok_out,&log->pwr1_ok,&log->pwr1_ok_bit_mask))
		return -1;
	if(set_bit_ptr_mask(args->io.pwr2_ok_out,&log->pwr2_ok,&log->pwr2_ok_bit_mask))
		return -1;
	if(set_bit_ptr_mask(args->io.pwr2_ok_out,&log->pwr2_ok,&log->pwr2_ok_bit_mask))
		return -1;
	if(set_bit_ptr_mask(args->io.nowe_nast_out,&log->nowe_nast,&log->nowe_nast_bit_mask))
		return -1;
	if(set_bit_ptr_mask(args->io.blad_konf_out,&log->blad_konf,&log->blad_konf_bit_mask))
		return -1;
	if(set_bit_ptr_mask(args->io.blad_sd_out,&log->blad_sd,&log->blad_sd_bit_mask))
		return -1;

	if(set_bit_ptr_struct(args->io.mgb_ch1_act_out,&log->mgb_ch[0]))
		return -1;
	if(set_bit_ptr_struct(args->io.mgb_ch2_act_out,&log->mgb_ch[1]))
		return -1;
	if(set_bit_ptr_struct(args->io.mgb_ch3_act_out,&log->mgb_ch[2]))
		return -1;
	if(set_bit_ptr_struct(args->io.mgb_ch4_act_out,&log->mgb_ch[3]))
		return -1;
	if(set_bit_ptr_struct(args->io.mgb_ch5_act_out,&log->mgb_ch[4]))
		return -1;
	if(set_bit_ptr_struct(args->io.mgb_ch6_act_out,&log->mgb_ch[5]))
		return -1;
	if(set_bit_ptr_struct(args->io.mgb_ch7_act_out,&log->mgb_ch[6]))
		return -1;
	if(set_bit_ptr_struct(args->io.mgb_ch8_act_out,&log->mgb_ch[7]))
		return -1;

	if(set_bit_ptr_struct(args->io.mki_zp6_act_out,&log->mki_zp6))
		return -1;
	if(set_bit_ptr_struct(args->io.mki_iec1_act_out,&log->mki_iec[0]))
		return -1;
	if(set_bit_ptr_struct(args->io.mki_iec2_act_out,&log->mki_iec[1]))
		return -1;
	if(set_bit_ptr_struct(args->io.mki_iec3_act_out,&log->mki_iec[2]))
		return -1;
	if(set_bit_ptr_struct(args->io.mki_iec4_act_out,&log->mki_iec[3]))
		return -1;

	if(set_bit_ptr_struct(args->io.pps_ok_out,&log->pps_ok))
		return -1;
	if(set_bit_ptr_struct(args->io.ptp_ok_out,&log->ptp_ok))
		return -1;
	if(set_bit_ptr_struct(args->io.ntp_ok_out,&log->ntp_ok))
		return -1;
	if(set_bit_ptr_struct(args->io.irigb_fix_ok_out,&log->irigb_fix_ok))
		return -1;
	if(set_bit_ptr_struct(args->io.iec_err_out,&log->iec_err))
		return -1;
	if(set_bit_ptr_struct(args->io.pps_sw_out,&log->pps_sw))
		return -1;
	if(set_float_ptr(args->io.sat_cnt_float_out,&log->sat_cnt))
		return -1;

	dev_ctrl_state&=~DEV_CTRL_STATE_SD_ERR;
	dev_ctrl_state&=~DEV_CTRL_STATE_PPS_OK;
	dev_ctrl_state&=~DEV_CTRL_STATE_PTP_OK;
	dev_ctrl_state&=~DEV_CTRL_STATE_IRIGB_FIX_OK;

	return 0;
}

void dev_ctrl(void *arguments, void *logic)
{
	struct dev_ctrl_logic *log = (struct dev_ctrl_logic *)logic;

	if(powered_off)
	{
		if(powered_off>=LOOP_CYCLE_MS)
			powered_off-=LOOP_CYCLE_MS;
		else
			powered_off=0;
	}
	else
	{
		check_and_set_mask(!(IN_DATA01 & (1<<12)),log->pwr1_ok,log->pwr1_ok_bit_mask);
		check_and_set_mask(!(IN_DATA01 & (1<<5)),log->pwr2_ok,log->pwr2_ok_bit_mask);
	}

	if(check_struct(&log->service))
		service_mode=1;
	else
		service_mode=0;

	if(*log->test_we & log->test_we_bit_mask)
		dev_ctrl_state|=DEV_CTRL_STATE_TEST_IN;
	else
		dev_ctrl_state&=~DEV_CTRL_STATE_TEST_IN;

	if(*log->test_wy & log->test_wy_bit_mask)
		dev_ctrl_state|=DEV_CTRL_STATE_TEST_OUT;
	else
		dev_ctrl_state&=~DEV_CTRL_STATE_TEST_OUT;

	if(*log->test_log & log->test_log_bit_mask)
		dev_ctrl_state|=DEV_CTRL_STATE_TEST_LOG;
	else
		dev_ctrl_state&=~DEV_CTRL_STATE_TEST_LOG;

	if(*log->blok_wy & log->blok_wy_bit_mask)
		dev_ctrl_state|=DEV_CTRL_STATE_BLOCK_OUT;
	else
		dev_ctrl_state&=~DEV_CTRL_STATE_BLOCK_OUT;

	if(*log->test_an & log->test_an_bit_mask)
		dev_ctrl_state|=DEV_CTRL_STATE_TEST_AN;
	else
		dev_ctrl_state&=~DEV_CTRL_STATE_TEST_AN;


	check_and_set_mask(dev_ctrl_state & DEV_CTRL_STATE_CFG_CHANGE,log->nowe_nast,log->nowe_nast_bit_mask);
	dev_ctrl_state&=~DEV_CTRL_STATE_CFG_CHANGE;
	check_and_set_mask(dev_ctrl_state & DEV_CTRL_STATE_CFG_ERR,log->blad_konf,log->blad_konf_bit_mask);
	check_and_set_mask(dev_ctrl_state & DEV_CTRL_STATE_SD_ERR,log->blad_sd,log->blad_sd_bit_mask);
}

void dev_ctrl_20hz(void *arguments, void *logic)
{
	struct dev_ctrl_logic *log = (struct dev_ctrl_logic *)logic;
	struct time_data irigb_time;
	u8 i;

	if(ic->sync_reg & 0xC000) // IRIG-B or PPS
	{
		log->pps_timeout_cnt2=0;

		if(log->pps_timeout_cnt>100)
			dev_ctrl_state|=DEV_CTRL_STATE_PPS_OK;
		else
			log->pps_timeout_cnt++;
	}
	else
	{
		log->pps_timeout_cnt=0;
		if(log->pps_timeout_cnt2>100)
			dev_ctrl_state&=~DEV_CTRL_STATE_PPS_OK;
		else
			log->pps_timeout_cnt2++;
	}

	if(ic->sync_reg & 0x4000)
	{
		irigb_time=irigb_process_frame();
		if(timesync_method==SYNC_METHOD_IRIG_B || timesync_method==SYNC_METHOD_IRIG_B_ZPRAE || (timesync_method==SYNC_METHOD_CUSTOM && (timesync_bits & CFG_TSYNC_FROM_DSP)))
		{
			if(irigb_time.fix_sat & 0x10)
			{
				if(cur_time.tv_usec>50 && cur_time.tv_usec<950)
					cur_time.tv_sec = irigb_time.secs;

				if(cur_time_sw.tv_usec>50 && cur_time_sw.tv_usec<950)
					cur_time_sw.tv_sec = irigb_time.secs;
			}
		}

		if(irigb_time.fix_sat & 0x10)
		{
			log->fix_timeout_cnt2=0;
			if(log->fix_timeout_cnt>100)
				dev_ctrl_state|=DEV_CTRL_STATE_IRIGB_FIX_OK;
			else
				log->fix_timeout_cnt++;
// zrobione juz wyzej - bez rozdzialu na irigb i irigb_zprae
//			if((timesync_method==SYNC_METHOD_IRIG_B_ZPRAE || (timesync_method==SYNC_METHOD_CUSTOM && (timesync_bits & CFG_TSYNC_FROM_DSP))) && cur_time.tv_usec>50 && cur_time.tv_usec<950)
//				cur_time.tv_sec = irigb_time.secs;

			*log->sat_cnt=irigb_time.fix_sat & 0x0f;
		}
		else
		{
			*log->sat_cnt=0;
			log->fix_timeout_cnt=0;
			if(log->fix_timeout_cnt2>100)
				dev_ctrl_state&=~DEV_CTRL_STATE_IRIGB_FIX_OK;
			else
				log->fix_timeout_cnt2++;
		}
	}
	else
	{
		*log->sat_cnt=0;
		log->fix_timeout_cnt=0;
		if(log->fix_timeout_cnt2>100)
			dev_ctrl_state&=~DEV_CTRL_STATE_IRIGB_FIX_OK;
		else
			log->fix_timeout_cnt2++;
	}

	if(pps3_timeout_cnt>60000)
		clear_struct(&log->pps_sw);
	else
		set_struct(&log->pps_sw);

	if(check_struct(&log->pps2))
	{
		if(!(ic->sync_reg & MKI_PPS_IN))
			ic->sync_reg|=MKI_PPS_IN;
	}
	else
	{
		if(ic->sync_reg & MKI_PPS_IN)
			ic->sync_reg&=~MKI_PPS_IN;
	}

	check_and_set_struct(dev_ctrl_state&DEV_CTRL_STATE_PPS_OK,&log->pps_ok);
	check_and_set_struct(dev_ctrl_state&DEV_CTRL_STATE_PTP_OK,&log->ptp_ok);
	check_and_set_struct(dev_ctrl_state&DEV_CTRL_STATE_NTP_OK,&log->ntp_ok);
	check_and_set_struct(dev_ctrl_state&DEV_CTRL_STATE_IRIGB_FIX_OK,&log->irigb_fix_ok);
	for(i=0;i<8;i++)
		check_and_set_struct(dev_ctrl_state & (DEV_CTRL_STATE_MGB_CHAN1<<i),&log->mgb_ch[i]);

	check_and_set_struct(dev_ctrl_state & (DEV_CTRL_STATE_MKI_CHAN_ZP),&log->mki_zp6);
	check_and_set_struct(dev_ctrl_state & (DEV_CTRL_STATE_MKI_IEC_ERR),&log->iec_err);

	check_and_set_struct(dev_ctrl_state & (DEV_CTRL_STATE_MKI_IEC_CHAN1),&log->mki_iec[0]);
	check_and_set_struct(dev_ctrl_state & (DEV_CTRL_STATE_MKI_IEC_CHAN2),&log->mki_iec[1]);
	check_and_set_struct(dev_ctrl_state & (DEV_CTRL_STATE_MKI_IEC_CHAN3),&log->mki_iec[2]);
	check_and_set_struct(dev_ctrl_state & (DEV_CTRL_STATE_MKI_IEC_CHAN4),&log->mki_iec[3]);

}
