// $ $RCSfile: hs1.c,v $ $Revision: 1.27 $ $Date: 2005/03/11 15:08:57
//! Contains task hs1 which receives a FIFO frame from hs0, process the data, and pass a SD_PKT to datapack
#include "hs1.dox"
#include "allnodes.h"
#include "node1.h"
#include "configura.h"
#include "pubfuncs.h"
#include "err_hdl.h"
#include "hs0.h"
#include "hifi_pool.h"
#include "cmd_seq.h"
#include "hs_hdl.h"
#include "data_hdl.h"
#include "hk_ask.h"
#include "cmd_exec.h"
#include "time_tsk.h"
#include "MM_misc.h"
#include "hs1.h"

//---- Local functions -------------------------------------------------------------------//
void hs1( void );
void process_fifo(unsigned int fifo_id, int block_nr);
void process_sd_frame(int fifo_id, int *pdata);
void process_hk_frame(int fifo_id, int* pdata);
void total_power(int fifo_id, int *pdata);
void total_power_wbs(int fifo_id, int *source);
void total_power_hrs(int fifo_id, int *source);
int deliver_sd_packet (int fifo_id);
void update_counters(int fifo_id);
void fast_chop_update_counters(int fifo_id);
void increment_transfer_cnt (int fifo_id);

//----- Local variables --------------------------------------------------------
static K_BLOCK sd_block[4][2];  //!< blocks to hold coadditions: i.e. science data buffers
static int coadded[4][2];  //!< coadditions made per block
static int current_buffer[4];  //!< current buffer, can be 0 or 1	
static int frame_counter[4];  //!< counter for the frames, used to decide the buffer
static int scan_counter[4][2];  //!< scan_counter buffers
static unsigned int transfer_counter[4] = {0,0,0,0};  //!< counters of the frames received from each spectrometer; index 0 is for wbsh, .., index 3 is for hrsv
START_FRAME_DATA last_frame_TS; //!< time stamps for the last TS 

//-------- hs1 -----------------------------------------------------------------
//! Task hs1: handles the SD frame sent from hs0. Makes coaddition and basic data processing. Sends a packet to data_hdl.
void hs1 (void)
{
	FRAME_MSG msg;  // To host data from the FRAME_QUEUE

	// Wait loop on the FRAME_QUEUE
	for(;;)
	{
		// Wait for message
		KS_FIFOGetW(FRAME_QUEUE, (void*) &msg);

 		// Process fifo
		process_fifo(msg.fifo_id, msg.block_nr);
		
		// Signal to data_hdl that the frame counter can be resetted - abort if problems
		if ( deliver_frame_completed_pkt(msg.fifo_id) == RC_FAIL ) abort_measurement();

		// Update buffers and counters 
		if (AID_spectroscopy == TC_FUNMAN_SPCTRSC_AID || AID_spectroscopy == TC_FUNMAN_SPCTRFS_AID) update_counters(msg.fifo_id);
		if (AID_spectroscopy == TC_FUNMAN_SPCTRFC_AID) fast_chop_update_counters(msg.fifo_id);
	}
}

//-------- init_hs1 ------------------------------------------------------------
//! Function init_hs1: Allocates memory for hs1 and sets the static variables
void init_hs1 (void)
{
	int i, j;
	
	// request from memory pool one block of buffer for each FIFO
	for (i = 0; i < 4; i++) for (j = 0; j < 2; j++)
		get_block(&sd_block[i][j], SD_POOL, SD_BLOCK_MAX_BYTES);
	
	// inits static vars
	reset_hs1();
}

//---- reset_hs1 ---------------------------------------------------------------
//! Function reset_hs1: resets static variables of this file
void reset_hs1 (void)
{
	int i, j;
	
	for (i = 0; i < 4; i++) for (j = 0; j < 2; j++)
	{
		coadded[i][j] = 0;
		current_buffer[i] = 0;
		frame_counter[i] = 0;
		scan_counter[i][j] = 0;
	}
}

//---- update_counters ---------------------------------------------------------
//! Updates the counters in order to keep track if the current frame is from buffer (phase) A or B 
void update_counters (int fifo_id)
{
	unsigned int mult;
	
	// compute period multiple
	switch( fifo_id )
	{
		case WBS_V_ID:
		case WBS_H_ID:
			mult = 1;
			break;
		case HRS_V_ID:
		case HRS_H_ID:
			mult = get_hrs_frames_per_wbs();
			break;
	}

	// increase counter
	frame_counter[fifo_id]++;
	
	// wrap counter
	frame_counter[fifo_id] = IFSI_MOD(frame_counter[fifo_id], (4*mult));

	// select buffer
	switch ( frame_counter[fifo_id] / mult )
	{
		case 0:
		case 3:
			current_buffer[fifo_id] = 0;
			break;
		case 1:
		case 2:
			current_buffer[fifo_id] = 1;
			break;
	}
}

//---- fast_chop_update_counters -----------------------------------------------
//! For fast chop only: Updates the counters in order to keep track if the current frame is from buffer (phase) A or B 
void fast_chop_update_counters (int fifo_id)
{
	current_buffer[fifo_id]++;
	current_buffer[fifo_id] = IFSI_MOD(current_buffer[fifo_id], 2);
}

//----------------- process_fifo -----------------------------------------------
//! Process the data, cheks if a TM packet ready and if yes send it, check if measurement is over.
void process_fifo (unsigned int fifo_id, int block_nr)
{
	int * pdata;
	int pool_id;

	// Get the pool id
	switch (fifo_id)
	{
		case HRS_V_ID:
		case HRS_H_ID:
			pool_id = HRS_FRAME_POOL;
			break;
		case WBS_V_ID:
		case WBS_H_ID:
			pool_id = WBS_FRAME_POOL;
			break;
	}

	// get pointer to data
	pdata = hifi_pool_get_pointer(pool_id, block_nr);

	// update the frame counter used to link hk to science
	increment_transfer_cnt(fifo_id);
	
	// process frame
	process_sd_frame(fifo_id, pdata);
	process_hk_frame(fifo_id, pdata);

	// data have been transferred: we can free the Frame Pool block
	hifi_pool_free_block(pool_id, block_nr);

	// check if we are ready to produce TM packets: if so send data to task data_hdl
	// note: in case the frame was requested only for the hk, it is not forwarded to
	// data_hdl (function pkt_complete() returns 0 thanks to the default values of
	// the variables involved)
	if ( pkt_complete(fifo_id, coadded[fifo_id][current_buffer[fifo_id]]) ) 
		if (deliver_sd_packet(fifo_id) == RC_FAIL)  // deliver packet
		{
			enqueue_exec_fail(EXF_HS1_DELIVER_SD_PKT, 0, NULL);
			abort_measurement();
		}
}

//------------------- process_hk_frame -----------------------------------------
//! Processes the HK part of the fifo frame
void process_hk_frame (int fifo_id, int* pdata)
{
	// Process hrs or wbs
	switch (fifo_id)
	{
		case HRS_V_ID:
		case HRS_H_ID:
			scan_counter[fifo_id][current_buffer[fifo_id]] = 0;  // always zero for hrs
			hk_process_hrs(fifo_id, pdata);		
			break;
		case WBS_V_ID:
		case WBS_H_ID:
			scan_counter[fifo_id][current_buffer[fifo_id]] += (*(pdata + WBS_FRAME_SD_LEN));  // save the scan_count
			hk_process_wbs(fifo_id, pdata);
			// the following is for wbs calibration, first part
			store_wbs_att_sett(fifo_id, pdata);
			break;
	}
}

//---- process_sd_frame --------------------------------------------------------
//! Processes the SD part of the fifo frame
void process_sd_frame (int fifo_id, int *pdata)
{
	// Processing is different depending on the measurement
	switch (AID_spectroscopy)
	{
		// If no measurement is running simply return
		case REQUIRING_HK:
		case STOPPING_HK:
		case TC_FUNMAN_SPCTRSM_AID:
		case STOPPING_VM:
		case STOPPING_OBS:
		case START_EEPROM_WRITE:
			get_TS( &(last_frame_TS.time_stamp) );	// get a dummy time stamp: should be useless but is safer
			return;
		// Process the data
		case HRS_FUNCTEST:		// hrs functional test
		case HRS_TUNE_INIT:		// hrs init tune
		case HRS_ATTSETT_ID:  // hrs attenuators setting 1st loop
		case HRS_ATTSETT2_ID:  // hrs attenuators setting 2nd loop
		case TUNE_HMIXMAG_ID:  // mixer magnet tuning with hrs
		case DIP_SCAN_IF_ID:  // diplexer scan with if power
		case WBS_ATTSETT_ID:  // IF power measurement in WBS
		case WBS_ATTSETT2_ID:  // IF power measurement in WBS
		case WBS_ATTSETT3_ID:  // IF power measurement in WBS
		case TUNE_WMIXMAG_ID:  // mixer magnet tuning with wbs
		case ZSCAN_ID:  // IF power measurement in WBS
		case WBS_COMB_STEP1_ID:  // wbs calibration: first step
		case WBS_COMB_STEP2_ID:  // wbs calibration: second step
		case SPECTROSCOPY_WBS_ZERO_ID:  // wbs zero measurement
		case PEAKUP_ACQUIRE_HRS:  // peakup acquire hrs
		case PEAKUP_ACQUIRE_WBS:  // peakup acquire wbs
		case TC_FUNMAN_SPCTRTP_AID:  // total power
		case TC_FUNMAN_SPCTRSC_AID:  // slow chop
		case TC_FUNMAN_SPCTRFC_AID:  // fast chop
		case TC_FUNMAN_SPCTRFS_AID:  // frequency switch
		case NEW_CALIB_RUN_AID:  // new wbs comb
		case NEW_CALIB_CLOSE_AID:  // new wbs comb
		case NEW_CALIB_INIT_AID:  // new wbs comb		
			pop_TS( &last_frame_TS, fifo_id);	// get the time stamp of the correct fifo
			total_power(fifo_id, pdata);
			break;
		default:
			generate_event(EVENT_EVENT_REPORT, RUNTIME_ERROR_EVID, ERR_HS1_SPECT_AID, 0, NULL);
			return;
	}
}
//---- total_power -------------------------------------------------------------
//! Computes the total power of the frame
void total_power (int fifo_id, int *pdata)
{
	// different for hrs or wbs
	switch( fifo_id )
	{
		case HRS_V_ID:
		case HRS_H_ID:
			total_power_hrs(fifo_id, pdata);
			break;
		case WBS_V_ID:
		case WBS_H_ID:
			total_power_wbs(fifo_id, pdata);
			break;
	}
}

//---------------- total_power_hrs ---------------------------------------------
//! Computes the total power of the HRS frame
void total_power_hrs (int fifo_id, int *source)
{
	int * dest;
	int i;

	// Init dest, add data and increase coadded
	dest = (int*) sd_block[fifo_id][current_buffer[fifo_id]].pointer_to_data + SD_TM_HEADER;
	if ( coadded[fifo_id][current_buffer[fifo_id]] > 0 )
	{
		int k;
		for (k=0; k<HRS_BLOCK_NUM * 2; k++)		// cycle over the subblocks
		{
			// the first data of each subblock is not to be coadded
			*(dest++) = *(source++);
			// coadd each 
			for(i=1; i< HRS_SUBBLOCK_DATA_LEN; i++)	*(dest++) += *(source++);
		}
	}
	else
		for(i=0; i< HRS_FRAME_SD_LEN; i++)	*(dest++) = *(source++);
	coadded[fifo_id][current_buffer[fifo_id]]++;
}

//---------------- total_power_wbs ---------------------------------------------
//! Computes the total power of the WBS frame
void total_power_wbs (int fifo_id, int *source)
{
	int i;
	int * dest1;
	int * dest2;
	int * dest3;
	int * dest4;

	// Init dest, add data and increase coadded
	dest1 = (int*) sd_block[fifo_id][current_buffer[fifo_id]].pointer_to_data + SD_TM_HEADER;
	dest2 = dest1 + WBS_BLOCK_DATA_LEN;
	dest3 = dest2 + WBS_BLOCK_DATA_LEN;
	dest4 = dest3 + WBS_BLOCK_DATA_LEN;

	if ( coadded[fifo_id][current_buffer[fifo_id]] > 0 )
	{
		for(i=0; i< WBS_BLOCK_DATA_LEN; i++)
		{	
			*(dest1++) += *(source++);
			*(dest2++) += *(source++);
		}
		for(i=0; i< WBS_BLOCK_DATA_LEN; i++)
		{
			*(dest3++) += *(source++);
			*(dest4++) += *(source++);
		}
	}
	else
	{
		for(i=0; i< WBS_BLOCK_DATA_LEN; i++)
		{	
			*(dest1++) = *(source++);
			*(dest2++) = *(source++);
		}
		for(i=0; i< WBS_BLOCK_DATA_LEN; i++)
		{
			*(dest3++) = *(source++);
			*(dest4++) = *(source++);
		}
	}
	
	coadded[fifo_id][current_buffer[fifo_id]]++;
}

//---- deliver_sd_packet -------------------------------------------------------
//! Sends a packet to the SD_PKT_QUEUE containing the SD frame
int deliver_sd_packet (int fifo_id)
{
	static SD_PKT_MSG sd_out_msg;	// structure which holds the message to data pkt task
	int return_value;
	
	// init return
	return_value = RC_OK;

	// Prepare message
	sd_out_msg.int_scan_cnt = scan_counter[fifo_id][current_buffer[fifo_id]];
	sd_out_msg.subsysID = fifo_id;
	sd_out_msg.measID = AID_spectroscopy;
	sd_out_msg.phase = current_buffer[fifo_id];  // values: 0 for buffer 1, 1 for buffer 2
	sd_out_msg.block = sd_block[fifo_id][current_buffer[fifo_id]];
	sd_out_msg.transfer_cnt = transfer_cnt_value(fifo_id);
	sd_out_msg.time_position = put_TS_in_pool( last_frame_TS );	// put the TS in the TSD pool and store the addres to pass it to data_hdl

	// send the packet after a check on scan count;
	// in case of a wbs frame, a scan count equal to zero is an error; instead for
	// a hrs frame scan count is always zero: see function process_hk_frame())
	if ((sd_out_msg.int_scan_cnt == 0) && ((fifo_id == WBS_H_ID) || (fifo_id == WBS_V_ID)))
	{
		// problems: do not forward the packet and issue an error
		release_block(&sd_out_msg.block);
		generate_event(EVENT_EVENT_REPORT, RUNTIME_ERROR_EVID, ERR_HS1_SCAN_COUNT, 0, NULL);
		return_value = RC_FAIL;
	}
	else
		if (KS_FIFOPut(SD_PKT_QUEUE, (void *) &sd_out_msg) == RC_FAIL)
		{
			release_block(&sd_out_msg.block);
			generate_event(EVENT_EVENT_REPORT, RUNTIME_ERROR_EVID, ERR_HS1_FIFOPUT_SD_PKT, 0, NULL);
			return_value = RC_FAIL;
		}
	
	// Allocate new block for the next tm packet
	coadded[fifo_id][current_buffer[fifo_id]] = 0;
	scan_counter[fifo_id][current_buffer[fifo_id]] = 0;
	if (get_block(&sd_block[fifo_id][current_buffer[fifo_id]], SD_POOL, SD_BLOCK_MAX_BYTES) == RC_FAIL)
	{
		generate_event(EVENT_EVENT_REPORT, RUNTIME_ERROR_EVID, ERR_HS1_SD_GET_BLOCK, 0, NULL);
		return_value = RC_FAIL;  // returns RC_FAIL od RC_OK depending on the block allocation
	}
	
	// done
	return return_value;
}

//------- deliver_frame_completed_pkt ------------------------------------------
//! this function pushes into fifo SD_PKT_QUEUE a special packet: when data_hdl
// receives this message, the corresponding fifo_pending counter is decremented
int deliver_frame_completed_pkt (int fifo_id)
{
	static SD_PKT_MSG sd_out_msg;	// structure which holds the message to data pkt task
	int code;
	
	// prepare code to pass to data handler
	switch (fifo_id)
	{
		case WBS_H_ID: code = FRAME_WBS_H_ID; break;
		case WBS_V_ID: code = FRAME_WBS_V_ID; break;
		case HRS_H_ID: code = FRAME_HRS_H_ID; break;
		case HRS_V_ID: code = FRAME_HRS_V_ID; break;
	}
			
	// Prepare message
	sd_out_msg.measID = code;
	
	// send a message in the Science Data Packet queue
	if ( KS_FIFOPut(SD_PKT_QUEUE, (void *) &sd_out_msg) == RC_FAIL )
	{
		generate_event(EVENT_EVENT_REPORT, RUNTIME_ERROR_EVID, ERR_HS1_FRAME_COMPLETE_FAIL, 0, NULL);
		return RC_FAIL;
	}
	else
		return RC_OK;  // add an error generation
}

//------------- transfer_cnt_value ---------------------------------------------
//! this function returns the transfer counter value for the fifo identified by fifo_id
unsigned int transfer_cnt_value (int fifo_id)
{
	return transfer_counter[fifo_id];
}

//------------- increment_transfer_cnt -----------------------------------------
//! this function increments the transfer counter for the fifo identified by argument fifo_id
void increment_transfer_cnt (int fifo_id)
{
	// update the transfer counter (top level variable of file hs1.c);
	// it is a 16-bit counter: the AND with 0xFFFF is equivalent to the use of a "mod 16536"
	transfer_counter[fifo_id] = (transfer_counter[fifo_id] + 1) & 0xFFFF;
}

//------------- reset_transfer_cnt ---------------------------------------------
//! this function resets the transfer counters for all the four fifos
void reset_transfer_cnt (void)
{
	int fifo_id;
	
	for (fifo_id = 0; fifo_id < 4; fifo_id++)
		transfer_counter[fifo_id] = 0;
}