// $ $RCSfile: err_hdl.c,v $ $Revision: 1.26 $ $Date: 2005/03/11 15:08:5
#include "err_hdl.dox"
#include "allnodes.h"
#include "node1.h"
#include <stdlib.h>
#include "configura.h"
#include "cmd_seq.h"
#include "cmd_exec.h"
#include "pubfuncs.h"
#include "MM_MISC.h"
#include "err_hdl.h"

//----- Local type and macros --------------------------------------------------
/*! TC_Compl_ID is a structure type able to hold the information needed to handle
the TC execution completion/failure acknowledgement service. */
typedef struct
{
	unsigned int tc_pkt_id;  // TC packet id
	unsigned int tc_pkt_seq_cont;  // TC packet sequence control
	unsigned int completion_flag;  // to signal if completion ack is requested
} TC_Compl_ID;

#define TC_ACK_ACCEP_MASK   0x00000100  //!< 0000 0001 0000 0000  mask for acceptance ack bit
#define TC_ACK_COMPL_MASK   0x00000800  //!< 0000 1000 0000 0000  mask for completion ack bit

//----- Local variables --------------------------------------------------------
static TC_Compl_ID completion_info = {0, 0, 0};  //!< variable that holds information for TC completion acknowledgement
static unsigned int Service_5_counter[3] = {0,0,0};	 //!< event counters

//----------------------- init_event_counters ---------------------------------------
//! Inits event counters. Called at startup.
void init_event_counters( unsigned int * counters)
{
	Service_5_counter[0] = counters[1];	// 5,1 events
	Service_5_counter[2] = counters[2];	// 5,4 events
	Service_5_counter[1] = counters[3];	// 5,2 events
}

//----------------------- authorise_event ---------------------------------------
/*! This function is to be called before posting a message into the event queue. If the return value is 0
the queue is full and the calling function should NOT post the event. If the return value is 1 the event can be posted. */
int authorise_event (void) 
{
	unsigned int num_event_in_queue, counter;
	struct TM_packet *tm_rep;
	K_BLOCK block;
	EVNT_TM_MSG message;  // queue to tmtc
	
	// read how many events are there in the queue
	num_event_in_queue = KS_FIFOStatus(EVENT_TM_QUEUE);  // EVENT_TM_QUEUE
	
	// if the queue is not overloaded return 1
	if ( num_event_in_queue <= 22 ) return 1;
	
	// if the queue is full simply return 0 
	if ( num_event_in_queue >= 24 ) return 0;

	// there are 23 events in queue. Use the 24th to warn that some messages have been suppressed
	
	// request a block from Event Mempool
	if (get_block(&block, EV_POOL, SHORT_BLOCK_SIZE) != RC_OK) return 0;

	// sending out an event: increase the counter
	counter = Service_5_counter[2]++; 
	* ( (unsigned int pm * ) BOOT_EVENT_54 ) = Service_5_counter[2]-1;
	
	// point to the block just allocated	
	tm_rep = (struct TM_packet *) block.pointer_to_data;

	// Write TM data
	tm_rep->data[0] = RUNTIME_ERROR_EVID;
	tm_rep->data[1] = 0;
	tm_rep->data[2] = Observation_ID >> 16;
	tm_rep->data[3] = Observation_ID & 0x0FFFF;
	UNPACK(&tm_rep->data[4], Cur_BuildingBlock_ID);
	tm_rep->data[6] = counter;
	tm_rep->data[7] = ERR_ERR_HDL_SUPPRESSING_EVENTS;
	tm_rep->data[8] =0;										// no parameters
	
	// prepare header
	prepare_TM_packet((K_BLOCK *)&block, TM_DFHF_REGULAR, APID1024, (((5+(1+8)+1)<<1)-1), TM_TYPE_EVNT, EVENT_EVENT_REPORT);

	// Now prepare and send the message for the event queue
	message.block = block;
	message.offset = TM_HEADER_NWORDS + TM_DATAHEADER_NWORDS;  // offset to data
	message.length = 1 + 7 + 5;  // number of parameters . not relevant for packing 16
	message.count = 0;
	message.packing = PACKING_16;  // 16 bit data
	
	// put in the queue
	if (KS_FIFOPut(EVENT_TM_QUEUE, (void *) &message) == RC_FAIL) release_block(&message.block);  // if the fifo_put fails, we must deallocate the memory block
	
	// done
	return 0;
}

//----------------------- generate_event ---------------------------------------
/*! This function is called when the generation of a TM packet of type 5 (event) is required */
int generate_event (int ev_subtype, int ev_id, int sid, int N_param, int * rep_param)
{
	unsigned int i, counter;
	struct TM_packet *tm_rep;
	K_BLOCK block;
	EVNT_TM_MSG message;  // queue to tmtc
	int aux_length;
	int param_copy[MAX_N_REP_PAR];
	
	// check against event queue overflow
	if (! authorise_event() ) return RC_FAIL;
	
	// Count event types
	switch (ev_subtype)
	{
		case EVENT_SUBTYPE:	
			counter = Service_5_counter[0]++; 
			* ( (unsigned int pm * ) BOOT_EVENT_51 ) = Service_5_counter[0]-1;
			break;  // Event
		case EXCEP_SUBTYPE:	
			counter = Service_5_counter[1]++; 
			* ( (unsigned int pm * ) BOOT_EVENT_52 ) = Service_5_counter[1]-1;
			break;  // Exception
		case ALARM_SUBTYPE:	
			counter = Service_5_counter[2]++; 
			* ( (unsigned int pm * ) BOOT_EVENT_54 ) = Service_5_counter[2]-1;
			break;  // Alarm
		default: counter = 0; break;
	}
	
	// request a block from Event Mempool
	if (get_block(&block, EV_POOL, SHORT_BLOCK_SIZE) != RC_OK) return RC_FAIL;
	
	tm_rep = (struct TM_packet *) block.pointer_to_data;

	// check parameter number: enforce maximum number
	if (N_param > MAX_N_REP_PAR-1) N_param = MAX_N_REP_PAR-1;
	
	// add Number of params as first param
	param_copy[0] = N_param;
	for (i=0; i<N_param; i++) param_copy[1+i] = rep_param[i];
	N_param = N_param+1;

	// the following if is a patch for proper format of runtime errors 
	if (ev_id == RUNTIME_ERROR_EVID)
	{
		// add the error code (contained into argument sid) as first parameter
		tm_rep->data[7] = sid;
		sid = 0;  // sid is always zero in runtime error tm packets
		aux_length = N_param +1;
		// now copy the other parameters
		for (i = 8; i < (N_param + 8); i++)
			tm_rep->data[i] = param_copy[i - 8];
	}
	else
	{
		aux_length = N_param;
		for (i = 7; i < (N_param + 7); i++)
			tm_rep->data[i] = param_copy[i - 7];
	}
	// end of patch
	
	// Write TM data
	tm_rep->data[0] = ev_id;
	tm_rep->data[1] = sid;
	tm_rep->data[2] = Observation_ID >> 16;
	tm_rep->data[3] = Observation_ID & 0x0FFFF;
	UNPACK(&tm_rep->data[4], Cur_BuildingBlock_ID);
	tm_rep->data[6] = counter;
	
	prepare_TM_packet((K_BLOCK *)&block, TM_DFHF_REGULAR, APID1024, (((5+(aux_length+7)+1)<<1)-1), TM_TYPE_EVNT, ev_subtype);

	// Now prepare and send the message for the event queue
	message.block = block;
	message.offset = TM_HEADER_NWORDS + TM_DATAHEADER_NWORDS;  // offset to data
	message.length = aux_length + 7 + 5;  // number of parameters . not relevant for packing 16
	message.count = 0;
	message.packing = PACKING_16;  // 16 bit data
	
	if (KS_FIFOPut(EVENT_TM_QUEUE, (void *) &message) == RC_FAIL)
	{
		release_block(&message.block);  // if the fifo_put fails, we must deallocate the memory block
		return RC_FAIL;
	}
	return RC_OK;
}


//----------------------- generate_peak_up_event -------------------------------
int generate_peak_up_event (int correction_y, int correction_z)
{
	unsigned int counter;
	struct TM_packet *tm_rep;
	K_BLOCK block;
	EVNT_TM_MSG message;  // queue to tmtc
	
	// check against event queue overflow
	if ( ! authorise_event() ) return RC_FAIL;
	
	// Count event types
	counter = Service_5_counter[0]++;
	* ( (unsigned int pm * ) BOOT_EVENT_51 ) = Service_5_counter[0]-1;

	// request a block from Event Mempool
	if (get_block(&block, EV_POOL, SHORT_BLOCK_SIZE) != RC_OK) return RC_FAIL;
	
	tm_rep = (struct TM_packet *) block.pointer_to_data;
	
	// Write TM data
	tm_rep->data[0] = PEAKUP_EVID;
	tm_rep->data[1] = PEAKUP_EVENT_SID;
	tm_rep->data[2] = Observation_ID >> 16;
	tm_rep->data[3] = Observation_ID & 0x0FFFF;
	UNPACK(&tm_rep->data[4], Cur_BuildingBlock_ID);
	tm_rep->data[6] = counter;
	tm_rep->data[7] = 1;  // instrument id HIFI = 1
	tm_rep->data[8] = correction_y;  // microrotation y
	tm_rep->data[9] = correction_z;  // microrotation z
	
	prepare_TM_packet((K_BLOCK *)&block, TM_DFHF_REGULAR, APID1024, (((5+(3+7)+1)<<1)-1), TM_TYPE_EVNT, EVENT_EVENT_REPORT);

	// Now prepare and send the message for the event queue
	message.block = block;
	message.offset = TM_HEADER_NWORDS + TM_DATAHEADER_NWORDS;  // offset to data
	message.length = 3 + 7 + 5;  // number of parameters . not relevant for packing 16
	message.count = 0;
	message.packing = PACKING_16;  // 16 bit data
	
	if (KS_FIFOPut(EVENT_TM_QUEUE, (void *) &message) == RC_FAIL)
	{
		release_block(&message.block);  // if the fifo_put fails, we must deallocate the memory block
		return RC_FAIL;
	}
	return RC_OK;
}

//---------------------- enqueue_ok --------------------------------------------
/*! Produces a telecommand verification TM packet, depending on the subtype parameter either a 
a TM(1,2) (acceptance success) or a TM(1,7) (execution succes) can be produced. This function is to be used 
to produce the execution status for short command (i.e. those that do not require the handling of the 
completion_info structure) */
int enqueue_ok (TC_packet * packet, unsigned int subtype)
{
	TC_verify *tc_verify;
	K_BLOCK block;
	EVNT_TM_MSG message;
	unsigned int tmp; 
	int result = RC_OK;

	// check against event queue overflow
	if ( ! authorise_event() ) return RC_FAIL;
	
	// variable tmp holds the field of the TC packet where ack bits
	tmp = packet->data_field_header[0];
	
	// the following if checks the ack bits in the TC packet, in order to
	// see if for this TC an acceptance/execution success ack is required
	if ( ((subtype == TC_ACCREP_SUCCESS) && (tmp & TC_ACK_ACCEP_MASK)) || ((subtype == TC_EXEC_COMPLETED) && (tmp & TC_ACK_COMPL_MASK)) )
	{
		if (get_block((K_BLOCK *)&block, EV_POOL, SHORT_BLOCK_SIZE) == RC_FAIL) return RC_FAIL;
		
		// Write a TC_verify structure into the block
		tc_verify = (TC_verify *) block.pointer_to_data; 
		// Write TM data
		tc_verify->data_1 = packet->id;
		tc_verify->data_2 = packet->seqctrl;
		prepare_TM_packet((K_BLOCK *)&block, TM_DFHF_REGULAR, APID1024, TM_ACK_LEN, TC_VERIFICATION, subtype);
		
		// Now prepare the message for the queue
		message.block = block;
		message.offset = TM_HEADER_NWORDS + TM_DATAHEADER_NWORDS;  // offset to data (3+5 16-bit words)
		message.length = 2;
		message.count = 0;
		message.packing = PACKING_16;  // 16 bit data
		
		result = KS_FIFOPut(EVENT_TM_QUEUE, (void *) &message);
		// if the fifo_put fails, we must deallocate the memory block
		if (result != RC_OK) release_block(&message.block);
	}
	return result;
}

//----------------------- enqueue_nok ------------------------------------------
/*! Function that produces a telecommand verification TM packet: depending on the subtype either a TM(1,2) (accecptance failure) 
or TM(1,8) (execution failure) can be produced. This function is to be used to produce the execution status for short 
command (i.e. those that do not require the handling of the completion_info structure)  */
int enqueue_nok (TC_packet * packet, unsigned int subtype, unsigned int err_code, int N_param, unsigned int * param_ptr)
{	
	struct TM_packet * tm_packet;
	K_BLOCK block;
	EVNT_TM_MSG message; 
	int i;

	// check against event queue overflow
	if (! authorise_event() ) return RC_FAIL;

	// Get a new free block from pool to put TM data
	if (get_block(&block, EV_POOL, SHORT_BLOCK_SIZE) == RC_FAIL) return RC_FAIL;
	
	// Write a TM_packet structure into the block
	tm_packet = (struct TM_packet *) block.pointer_to_data; 
	
	tm_packet->data[0] = packet->id;
	tm_packet->data[1] = packet->seqctrl;
	tm_packet->data[2] = err_code;
	tm_packet->data[3] = N_param;
	
	// write parameter values
	for (i = 0; i < N_param; i++) 
		tm_packet->data[i + 4] = param_ptr[i];
		
	if (N_param == 0)  // if there are no parameters to be written
	{
		tm_packet->data[4] = 0;  // this because the TM packet specs seem to require at least 32 bits after the error code
		N_param = 1;  // this because of the following use of N_param in the call of prepare_TM_packet
	}

	prepare_TM_packet((K_BLOCK *)&block, TM_DFHF_REGULAR, APID1024, (19 + 2*N_param), TC_VERIFICATION, subtype);						

	// Now prepare the message for the queue
	message.block = block;
	message.offset = TM_HEADER_NWORDS + TM_DATAHEADER_NWORDS;  // offset to data
	message.length = N_param + 4;  // length in words of the application data field without crc
	message.count = 0;
	message.packing = PACKING_16;  // 16 bit data
	if (KS_FIFOPut(EVENT_TM_QUEUE, (void *) &message) == RC_FAIL)
	{
		release_block(&message.block);  // if the fifo_put fails, we must deallocate the memory block
		return RC_FAIL;
	}
	return RC_OK;
}

//----------------------- enqueue_exec_fail ------------------------------------
/*! This function is to be called every time that a telecommand execution failure is detected, in order to 
produce an execution failure TM(1,8) packet. It is to be used for commands that handle the completion_info structure. */
int enqueue_exec_fail (unsigned int err_code, int N_param, unsigned int * param_ptr)
{	
	struct TM_packet * tm_packet;
	K_BLOCK block;
	EVNT_TM_MSG message; 
	int i;

	// check against event queue overflow
	if ( ! authorise_event() ) return RC_FAIL;

	// Get a new free block from pool to put TM data
	if (get_block(&block, EV_POOL, SHORT_BLOCK_SIZE) == RC_FAIL) return RC_FAIL;
	
	// Write a TM_packet structure into the block
	tm_packet = (struct TM_packet *) block.pointer_to_data; 
	
	// access to variable completion_info
	tm_packet->data[0] = completion_info.tc_pkt_id;
	tm_packet->data[1] = completion_info.tc_pkt_seq_cont;
	tm_packet->data[2] = err_code;
	tm_packet->data[3] = N_param;
	
	// reset variable completion_info
	completion_info.tc_pkt_id = 0;  // this is important since we use this field to know if something is going on
	completion_info.tc_pkt_seq_cont = 0;	
	completion_info.completion_flag = 0;
	
	// write parameter values
	for (i = 0; i < N_param; i++) 
		tm_packet->data[i + 4] = param_ptr[i];
	
	if (N_param == 0)  // if there are no parameters to be written
	{
		tm_packet->data[4] = 0;  // this because the TM packet specs seem to require at least 32 bits after the error code
		N_param = 1;  // this because of the following use of N_param in the call of prepare_TM_packet
	}
	
	prepare_TM_packet((K_BLOCK *)&block, TM_DFHF_REGULAR, APID1024, (19 + 2*N_param), TC_VERIFICATION, TC_EXEC_FAILURE);	                    

	// Now prepare the message for the queue
	message.block = block;
	message.offset = TM_HEADER_NWORDS + TM_DATAHEADER_NWORDS;  // offset to data
	message.length = N_param + 4;  // length in words of the application data field without crc
	message.count = 0;
	message.packing = PACKING_16;  // 16 bit data
	if (KS_FIFOPut(EVENT_TM_QUEUE, (void *) &message) == RC_FAIL)
		release_block(&message.block);  // if the fifo_put fails, we must deallocate the memory block
	
	return RC_FAIL;  // this function always returns RC_FAIL
}

//----------------------- enqueue_exec_compl -----------------------------------
/*! This function is to be called when a telecommand execution succes TM(1,7) is to be produced. 
It is to be used for commands that handle the completion_info structure. */
int enqueue_exec_compl (void)
{
	int result = RC_OK;

	if ( completion_info.completion_flag == 1 && authorise_event() )
	{	// as requested, send an execution completion ack
		struct TM_packet * tm_packet;
		K_BLOCK block;
		EVNT_TM_MSG message; 
		
		// Get a new free block from pool to put TM data
		if (get_block(&block, EV_POOL, SHORT_BLOCK_SIZE) == RC_FAIL) 
			result = RC_FAIL;
	
		// Write a TM_packet structure into the block
		tm_packet = (struct TM_packet *) block.pointer_to_data; 
	
		tm_packet->data[0] = completion_info.tc_pkt_id;  // access to the global variable completion_info
		tm_packet->data[1] = completion_info.tc_pkt_seq_cont;
		
		prepare_TM_packet((K_BLOCK *)&block, TM_DFHF_REGULAR, APID1024, TM_ACK_LEN, TC_VERIFICATION, TC_EXEC_COMPLETED);	                            
	
		// Now prepare the message for the queue
		message.block = block;
		message.offset = TM_HEADER_NWORDS + TM_DATAHEADER_NWORDS; // offset to data
		message.length = 2;	 //length in words of the application data field without crc
		message.count = 0;
		message.packing = PACKING_16; // 16 bit data
		if (KS_FIFOPut(EVENT_TM_QUEUE, (void *) &message) == RC_FAIL)
		{
			release_block(&message.block);  // if the fifo_put fails, we must deallocate the memory block
			result = RC_FAIL;
		}
	}
	
	// in any case it is necessary to reset the struct, because the TC execution 
	// has been brought to completion
	completion_info.tc_pkt_id = 0; 
	completion_info.tc_pkt_seq_cont = 0;
	completion_info.completion_flag = 0;
	
	return result;
}

//------------- completion_update ----------------------------------------------
/*! Updates the variable for completion management (named completion_info) therefore it is to be called 
when the execution of a "long" telecommand is started. */
void completion_update (TC_packet * packet)
{
	if ( completion_info.tc_pkt_id != 0 )  // i.e. a previous TC involving a measurement 
		// hasn't been completed and so we have to send an execution failure
		enqueue_exec_fail(EXF_ERR_HDL_MEASUREMENT_ABORTED, 0, NULL);  // execution failure
	
	// now update the variable for completion management (variable local to this file)
	if (packet == NULL)  // argument null is passed when we just want to reset the variable
	{
		completion_info.tc_pkt_id = 0;
		completion_info.tc_pkt_seq_cont = 0;
		completion_info.completion_flag = 0;
	}
	else
	{
		completion_info.tc_pkt_id = packet->id;
		completion_info.tc_pkt_seq_cont = packet->seqctrl;
		completion_info.completion_flag = (packet->data_field_header[0] & TC_ACK_COMPL_MASK) >> 11;
	}
}

//----------------------- generate_report --------------------------------------
/*! This function produces a TM packet (with apid = rep_apid, type = rep_type,
subtype = rep_subtype) containing N_param parameters, each 16-bit long, pointed
by rep_param */
int generate_report (unsigned int rep_apid, int rep_type, int rep_subtype, int N_param, unsigned int * rep_param)
{
	K_BLOCK block;
	HK_TM_MSG message;  // for the queue towards tm_tc
	struct TM_packet *tm_rep;
	unsigned int i;
	
	// Get a new free block from pool to put TM data
	if (get_block(&block, HK_POOL, SHORT_BLOCK_SIZE) == RC_FAIL)
	{
		generate_event(EVENT_EVENT_REPORT, RUNTIME_ERROR_EVID, ERR_ERR_HDL_GET_HK_POOL_BLOCK, 0, NULL);
		return RC_FAIL;
	}
	
	// Write a TM_packet structure into the block
	tm_rep = (struct TM_packet *) block.pointer_to_data;
	// Write TM data
	for (i = 0; i < N_param; i++)
		tm_rep->data[i] = rep_param[i];
	
	prepare_TM_packet(&block, TM_DFHF_REGULAR, rep_apid, ((6+N_param)<<1)-1, rep_type, rep_subtype);
	
	// now prepare the message for the queue
	message.block = block;
	message.offset = TM_HEADER_NWORDS + TM_DATAHEADER_NWORDS; // useless in case of packing 16
	message.length = N_param + 5;  // length in words of the application data field without crc
	message.count = 0;
	message.packing = PACKING_16;  // 16 bit data
	if (KS_FIFOPut(HK_TM_QUEUE, (void *) &message) == RC_FAIL)
	{
		release_block(&block);  // if the fifo_put fails, we must deallocate the memory block
		generate_event(EVENT_EVENT_REPORT, RUNTIME_ERROR_EVID, ERR_ERR_HDL_FIFOPUT_HK_TM_QUEUE, 0, NULL);
		return RC_FAIL;
	}
	return RC_OK;
}