/*============================= Include File ================================*/


#include <src/INFN/LU_SourceFileID_INFN.h>
#define __FILEID__ _CAL_Calorimeter_INFN__c

#include <src/INFN/PRH_ParamHandler_INFN.h>
#include <src/INFN/LU_LogUtility_INFN.h>
#include <src/INFN/PRH_ParamHandler_INFN_auto.h>
LU_DECL_MASK();

#include <src/INFN/DAQ_IDAQ_INFN.h>
#include <src/INFN/CM_Common_INFN.h>
#include <src/INFN/CH_CommandHandler_INFN.h>
#include <src/INFN/CAL_Calorimeter_INFN.h>
#include <src/INFN/KHB_Driver_INFN.h>
#include <src/INFN/PM_PamManager_INFN.h>
#include <src/INFN/TRG_Trigger_INFN.h>
#include <src/INFN/HK_Manager_INFN.h>
#include <src/INFN/ALM_Alarm_INFN.h>

/*============================ Global define ================================*/


/*============================== global types  ==============================*/





/*=========================== Structure define ==============================*/


/*============================ Enumerate define =============================*/


/*============================== global variables  ==============================*/

DAQ_DECLBUF(CAL_TempPif0,CAL_MAXTEMPBUFFER);
DAQ_DECLBUF(CAL_TempPif1,CAL_MAXTEMPBUFFER);
DAQ_DECLBUF(CAL_TempPif2,CAL_MAXTEMPBUFFER);
DAQ_DECLBUF(CAL_TempPif3,CAL_MAXTEMPBUFFER);

extern BOOL  RM_Init_Before_Calib;

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I211;

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I221;

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I231;

static DAQ_CMD_BUF CAL_WRITE_DSP_MEM_I41;

static DAQ_CMD_BUF CAL_WRITE_DSP_MEM_I42;

static DAQ_CMD_BUF CAL_WRITE_DSP_MEM_I43;

static DAQ_CMD_BUF CAL_READ_DSP_MEM_C31;

static DAQ_CMD_BUF CAL_READ_DSP_MEM_C32;

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_OFF;

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I241[4+1];

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I251[4+1];

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I261[4+1];

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I260[4+1];

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I311[4+1];

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I310[4+1];

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I321;

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I331[4+1];

static DAQ_CMD_BUF CAL_WRITE_FPGA_REG_I341[4+1];

DAQ_DECLBUF(daq_reply,128*1024);
DAQ_DECLBUF(daq_temp,128*1024);
DAQ_DECLBUF(daq_command,1024);
DAQ_DECLBUF(daq_calo,1024);

#define CAL_CAL_TRIGGER_DELAY 10

PRH_VAR_TYPE CAL_DSP_MASK;
PRH_VAR_TYPE CAL_FE_MASK;

#define CAL_STRIP_SKIP_LEN 66*16
#define CAL_SKIP_HEADER SM_PKTHEADER_LEN

static UINT32 STRIP_SKIP[CAL_STRIP_SKIP_LEN];
static UINT16 CAL_CRC[4][4]={
  {0x635a,0x6700,0x6bee,0x6fb4},
  {0xba7c,0xbe26,0xb2c8,0xb692},
  {0x723d,0x7667,0x7a89,0x7ed3},
  {0xab1b,0xaf41,0xa3af,0xa7f5}
};

status_code CAL_Init_ETOATO()
{
  int i;
  
  CH_SetAtoEto(CH_CAL_RESET_FE,1,2);
  CH_SetAtoEto(CH_CAL_READ_ALARM,1,2);
  
  for(i=0; i < 7; i++)
    CH_SetAtoEto(CAL_CAL_READ_FPGA_REG_BASE_ID+i,1,1);


  CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_OFF,1,2);
  CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I211,1,2);
  CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I221,1,2);
  CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I231,1,2);
  
  for(i=1; i < 5; i++)
    {
      CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I241[i],1,2);
      CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I251[i],1,2);
      CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I261[i],1,2);
      CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I260[i],1,2);
      CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I311[i],1,2);
      CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I310[i],1,2);
      CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I331[i],1,2);
      CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_FPGA_REG_I341[i],1,2);
    }

  CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_DSP_MEM_I41,1,7);  
  CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_DSP_MEM_I42,1,7);
  CH_PARAMBUF_SET_ATO_ETO(CAL_WRITE_DSP_MEM_I43,1,2);
  CH_PARAMBUF_SET_ATO_ETO(CAL_READ_DSP_MEM_C31,1,8);
  CH_PARAMBUF_SET_ATO_ETO(CAL_READ_DSP_MEM_C32,1,8);
  
  CH_SetAtoEto(CH_CAL_READ_HK,1,7);
  CH_SetAtoEto(CH_CAL_READ_EVENT_DATA,1,7);
  CH_SetAtoEto(CH_CAL_FULL,1,2);
  CH_SetAtoEto(CH_CAL_COMPRESS,1,2);

  return CM_RC_SUCCESSFUL;
}

void CAL_Init_ParamBuffers() {
  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_WRITE_FPGA_REG_I211);
  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_WRITE_FPGA_REG_I221);
  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_WRITE_FPGA_REG_I231);

  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_WRITE_DSP_MEM_I41);
  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_WRITE_DSP_MEM_I42);
  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_WRITE_DSP_MEM_I43);

  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_READ_DSP_MEM_C31);
  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_READ_DSP_MEM_C32);

  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I241,1);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I241,2);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I241,3);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I241,4);  

  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I251,1);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I251,2);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I251,3);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I251,4);  
  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I261,1);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I261,2);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I261,3);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I261,4);  

  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I260,1);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I260,2);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I260,3);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I260,4);  

  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I311,1);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I311,2);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I311,3);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I311,4);  

  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I310,1);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I310,2);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I310,3);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I310,4);  

  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_WRITE_FPGA_REG_I321);
  DAQ_INIT_BUF_FROM_PARAMBUF(CAL_WRITE_FPGA_REG_OFF);

  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I331,1);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I331,2);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I331,3);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I331,4);  

  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I341,1);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I341,2);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I341,3);  
  DAQ_INIT_BUF_FROM_PARAMBUF_AS_ARR(CAL_WRITE_FPGA_REG_I341,4);  

}

status_code CAL_Init() 
{
  DAQ_BUFFER_INIT(daq_reply);
  DAQ_BUFFER_INIT(daq_temp);
  DAQ_BUFFER_INIT(daq_command);
  DAQ_BUFFER_INIT(daq_calo);
  DAQ_BUFFER_INIT(CAL_TempPif0);
  DAQ_BUFFER_INIT(CAL_TempPif1);
  DAQ_BUFFER_INIT(CAL_TempPif2);
  DAQ_BUFFER_INIT(CAL_TempPif3);

  CAL_DSP_MASK=PRH_VAR_CAL_UPLOAD_CAL_DSP_MASK;
  CAL_FE_MASK =PRH_VAR_CAL_UPLOAD_CAL_FE_MASK;

  CAL_Init_ParamBuffers();
  CAL_Init_ETOATO();
  return CM_RC_SUCCESSFUL;
}

status_code CAL_SwitchDCDC_On()
{
  return CM_RC_SUCCESSFUL;
}


status_code CAL_SendTrigger(SM_PAGEID page,UINT16 n) {
  int i;
  status_code status;
  for (i=0; i < n; i++)
    {
      status = DAQ_SendCmdQueue2CMDIF(page);
      LU_EXIT_IFERROR(status);
      status = RM_TaskSuspendUnlessSINT(CAL_CAL_TRIGGER_DELAY);
      LU_EXIT_IFERROR(status);
    }
}


status_code CAL_Initialization()
{
  DAQ_FE idaqfe;
  BYTE cal_loop1,cal_loop2,*ptr,dummy;
  UINT16 temp,temp1,i,fe,alarm_reg,j;
  RM_ACQ_SETTING sector;
  status_code status=CM_RC_SUCCESSFUL;
  DAQ_CMD_BUF* auto_buf;
  static int debug_cal=0;

  DAQ_ResetPIF(DAQ_DEFAULT_ETO_TICS,DAQ_MAX_DTO_TICS,SM_DAQSETUP,HB_PKTTYPE_CAL_INIT);

  CAL_FE_MASK = PRH_VAR_CAL_UPLOAD_CAL_FE_MASK; 
  CAL_DSP_MASK = PRH_VAR_CAL_UPLOAD_CAL_DSP_MASK;
  
  for(idaqfe=DAQ_FE_CAL_1,fe = 0x0001,i=0; idaqfe <= DAQ_FE_CAL_4; idaqfe++ , fe <<=1,i++) 
    {
      if(fe & CAL_FE_MASK) 
	{
	  cal_loop1=cal_loop2=0; 
	  
	START_ACQ:
	  
	  DAQ_Format_CMD_Empty(&daq_temp);
	  DAQ_Format_CMD_Empty(&CAL_TempPif0);
	  DAQ_Format_CMD_Empty(&CAL_TempPif1);
	  DAQ_Format_CMD_Empty(&CAL_TempPif2);
	  DAQ_Format_CMD_Empty(&CAL_TempPif3);

	  CH_GetBuf(CH_CAL_READ_ALARM,&auto_buf);
/* ZULLO 06 Jul 2004 Fe2Daq --> Fe2Ram */
/* 	  DAQ_Format_Fe2Daq(&CAL_TempPif1,auto_buf,idaqfe); 
	  DAQ_Format_Fe2Ram(&CAL_TempPif0,auto_buf,idaqfe); 
*/

	  if(debug_cal)
	    DAQ_Format_Fe2Daq(&CAL_TempPif0,auto_buf,idaqfe);
	  else
	    DAQ_Format_Cmd2Fe(&CAL_TempPif0,auto_buf,idaqfe); 
	  
	  status = DAQ_StoreCmdQueue2PIF(SM_CALEXP0,&CAL_TempPif0);
	  LU_EXIT_IFERROR(status);

	  if(debug_cal)
	    status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP0,TRUE,&daq_reply,TRUE);
	  else
	    status = DAQ_SendCmdQueue2CMDIF(SM_CALEXP0);
	  

	  LU_EXIT_IFERROR(status);

	  status = RM_TaskSuspendUnlessSINT(1);
	  LU_EXIT_IFERROR(status);
	  
	  CH_GetBuf(CH_CAL_RESET_FE,&auto_buf);
	  DAQ_Format_Fe2Daq(&CAL_TempPif1,auto_buf,idaqfe);
	 
	  if (RM_Init_Before_Calib)
	    DAQ_Format_Fe2Daq(&CAL_TempPif1,&CAL_WRITE_FPGA_REG_I211,idaqfe);
	  else
	    {
	      CH_GetBuf(CH_CAL_WRITE_FPGA_REG_RAW,&auto_buf);
	      DAQ_Format_Fe2Daq(&CAL_TempPif1,auto_buf,idaqfe);
	    }
	  
	  DAQ_Format_Fe2Daq(&CAL_TempPif1,&CAL_WRITE_FPGA_REG_I221,idaqfe);
	  DAQ_Format_Fe2Daq(&CAL_TempPif1,&CAL_WRITE_FPGA_REG_I231,idaqfe);
	  DAQ_Format_Fe2Daq(&CAL_TempPif1,&CAL_WRITE_FPGA_REG_I241[i+1],idaqfe);
	  DAQ_Format_Fe2Daq(&CAL_TempPif1,&CAL_WRITE_FPGA_REG_I251[i+1],idaqfe);
	  
	  if (RM_Init_Before_Calib)
	    DAQ_Format_Fe2Daq(&CAL_TempPif1,&CAL_WRITE_FPGA_REG_I261[i+1],idaqfe);
	  else
	    DAQ_Format_Fe2Daq(&CAL_TempPif1,&CAL_WRITE_FPGA_REG_I260[i+1],idaqfe);
	  
	  status = DAQ_StoreCmdQueue2PIF(SM_CALEXP1,&CAL_TempPif1);
	  LU_EXIT_IFERROR(status);
	  status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP1,TRUE,&daq_reply,TRUE);
	  LU_EXIT_IFERROR(status);

	  status = RM_TaskSuspendUnlessSINT(100);
	  LU_EXIT_IFERROR(status);

	  DAQ_Format_AppendBuffer_offset(&daq_temp,&daq_reply,CAL_SKIP_HEADER);

	  
	  if (RM_Init_Before_Calib)
	    {
	      DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_WRITE_FPGA_REG_I311[i+1],idaqfe);
	      DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_WRITE_FPGA_REG_I321,idaqfe);
	    }
	  else
	    {
	      DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_WRITE_FPGA_REG_I310[i+1],idaqfe);
	      CH_GetBuf(CH_CAL_WRITE_FPGA_REG_RAW,&auto_buf);
	      DAQ_Format_Fe2Daq(&CAL_TempPif2,auto_buf,idaqfe);
	    }
	  
	  DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_WRITE_FPGA_REG_I331[i+1],idaqfe);

	  DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_WRITE_FPGA_REG_I341[i+1],idaqfe);

	  status = DAQ_StoreCmdQueue2PIF(SM_CALEXP2,&CAL_TempPif2);
	  LU_EXIT_IFERROR(status);
	  status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP2,TRUE,&daq_reply,TRUE);
	  LU_EXIT_IFERROR(status);

	  DAQ_Format_AppendBuffer_offset(&daq_temp,&daq_reply,CAL_SKIP_HEADER);

	  DAQ_Format_CMD_Empty(&CAL_TempPif2);

	  if (RM_Init_Before_Calib)
	    {
	      if(fe & CAL_DSP_MASK) 
		{
		  DAQ_Format_CMD_Empty(&CAL_TempPif2);
		  DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_WRITE_DSP_MEM_I41,idaqfe);
		  DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_WRITE_DSP_MEM_I42,idaqfe);
		  DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_WRITE_DSP_MEM_I43,idaqfe);
		  status = DAQ_StoreCmdQueue2PIF(SM_CALEXP3,&CAL_TempPif2);
		  LU_EXIT_IFERROR(status);
		  status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP3,TRUE,&daq_reply,TRUE);
		  LU_EXIT_IFERROR(status);
		  DAQ_Format_AppendBuffer_offset(&daq_temp,&daq_reply,CAL_SKIP_HEADER);
		}
	    }
	  else
	    CAL_DSP_MASK &=~fe;
	  	  
	  if (daq_temp.len < 2); // go to reset 
	  
	  for (j=1; j <=10; j++)
	    {
	      ptr = (BYTE*)&daq_temp.buf[j*2];
	      CM_READ_BE_UINT16(ptr,temp,dummy);
	      if ((temp & 0x4007) !=  0x4000)
		goto INC_LOOP1;
	    }
	  
	  if (fe & CAL_DSP_MASK)
	    {
	      for (j=11; j <= 13 ; j++)
		{
		  ptr = (BYTE*)&daq_temp.buf[j*2];
		  CM_READ_BE_UINT16(ptr,temp,dummy);
		  if ((temp & 0x8007) !=  0x8000)
		    goto INC_LOOP2;
		}
	    }
	  
	  continue;
	  
	INC_LOOP2:
	  if (++cal_loop2 < PRH_ARR_CAL_COUNT[1])
	    goto START_ACQ;
	  
	  DAQ_Format_CMD_Empty(&CAL_TempPif3);
	  CH_GetBuf(CH_CAL_RESET_FE,&auto_buf);
	  DAQ_Format_Fe2Daq(&CAL_TempPif3,auto_buf,idaqfe);
	  CH_GetBuf(CH_CAL_WRITE_FPGA_REG_RAW,&auto_buf);
	  DAQ_Format_Fe2Daq(&CAL_TempPif3,auto_buf,idaqfe);

	  status = DAQ_StoreCmdQueue2PIF(SM_CALEXP3,&CAL_TempPif3);
	  LU_EXIT_IFERROR(status);
	  status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP3,TRUE,&daq_reply,FALSE);
	  LU_EXIT_IFERROR(status);
	  
	  CAL_DSP_MASK&=~fe;
	  continue;
	  
	INC_LOOP1:      
	  if (++cal_loop1 < PRH_ARR_CAL_COUNT[0])
	    goto START_ACQ;
	  
	  CAL_FE_MASK&=~fe;	  
	}

      CAL_DSP_MASK&=~fe;
    }

  return status;
} 

BOOL CAL_CheckCRC(int i,int val)
{
  int j;
  
  for (j=0; j < 4; j++)
    if (val == CAL_CRC[i][j])
      return(TRUE);
  
  return(FALSE);
}


int CAL_GetWriteFPGARegChannel(int x)
{
  int i=1;
  
  if (x == 0)
    return(CH_CAL_WRITE_FPGA_REG_0000);
  
  if (x < 0x8005)
    while (x = (x >> 1)) i++;
  else
    {
      switch(x)
	{
	case 0x8005: i = 17;
	  break;
	case 0x8015: i = 18;
	  break;
	case 0xc2:   i = 19;
	}
    }
  return(i+CH_CAL_WRITE_FPGA_REG_0000);
}

status_code CAL_Pulse(int cal_dsp_mask,int channel,int cal_vcal)
{
  UINT16 fe,i;
  DAQ_FE idaqfe;
  status_code status;
  DAQ_CMD_BUF* auto_buf;

  for(idaqfe=DAQ_FE_CAL_1,fe=0x0001; idaqfe <= DAQ_FE_CAL_4; idaqfe++ , fe <<=1) 
    {
      if (fe & cal_dsp_mask)
	{
	  DAQ_Format_CMD_Empty(&CAL_TempPif1);
	  
	  CH_GetBuf(CH_CAL_RESET_FE,&auto_buf);
	  DAQ_Format_Fe2Daq(&CAL_TempPif1,auto_buf,idaqfe);
	  CH_GetBuf(CAL_GetWriteFPGARegChannel(channel),&auto_buf);
	  DAQ_Format_Fe2Daq(&CAL_TempPif1,auto_buf,idaqfe);
	  CH_GetBuf(CAL_GetWriteFPGARegChannel(cal_vcal),&auto_buf);
	  DAQ_Format_Fe2Daq(&CAL_TempPif1,auto_buf,idaqfe);
	  
	  DAQ_StoreCmdQueue2PIF(SM_CALEXP1,&CAL_TempPif1);
	  status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP1,TRUE,&daq_reply,FALSE);
	  LU_EXIT_IFERROR(status);	  		  
	}
    }

  //  status = DAQ_StoreCmdQueue2PIF(SM_CALEXP0,&CAL_TempPif0);
  //  LU_EXIT_IFERROR(status);
  status = CAL_SendTrigger(SM_CALEXP0,130);
  LU_EXIT_IFERROR(status);
  
  return(CM_RC_SUCCESSFUL);  
}


status_code CAL_ReadPulseData(UINT16 fe_mask)
{
  DAQ_FE idaqfe;
  DAQ_CMD_BUF *auto_buf;
  status_code status = CM_RC_SUCCESSFUL;
  UINT16 fe;

  for(idaqfe=DAQ_FE_CAL_1,fe = 0x0001; idaqfe <= DAQ_FE_CAL_4; idaqfe++ , fe <<=1) 
    {
      if (fe & fe_mask)
	{
	  DAQ_Format_CMD_Empty(&CAL_TempPif2);
	  CH_GetBuf(CH_CAL_READ_EVENT_DATA,&auto_buf);
	  DAQ_Format_Fe2Daq(&CAL_TempPif2,auto_buf,idaqfe);
	  DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_READ_DSP_MEM_C32,idaqfe);
	  status = DAQ_StoreCmdQueue2PIF(SM_CALEXP2,&CAL_TempPif2);
	  LU_EXIT_IFERROR(status);
	  status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP2,TRUE,&daq_reply,FALSE);
	  LU_EXIT_IFERROR(status);	  		  
	}
    }

  return status;
}

BOOL CAL_Pack(DAQ_CMD_BUF *buffer) 
{
  int i;
  UINT16 temp,dummy;
  long int chped,chped2,chthr2,chthr;
  BYTE *ptr;
  UINT32 w1,w2;
  
  chped2 = 0; chthr2 =0;
  temp = 0;
    
  ptr = &buffer->buf[CAL_SKIP_HEADER+6*2];

  for(i=0; i < 1056; i++)
    {
      CM_READ_BE_UINT16(ptr,temp,dummy);
      ptr += 2;
      chped2+=(short int)temp;
    }

  CM_READ_BE_UINT16(ptr,w1,dummy); ptr += 2;
  CM_READ_BE_UINT16(ptr,w2,dummy); ptr += 2;

  chped = (w1 & 0x0000ffff) | (w2 << 16);
  
   for (i=0; i < 66; i++)
    {
      CM_READ_BE_UINT16(ptr,temp,dummy);
      ptr += 2;
      chthr2+=(short int)temp;
    }

  CM_READ_BE_UINT16(ptr,w1,dummy); ptr += 2;
  CM_READ_BE_UINT16(ptr,w2,dummy); 
  
  chthr = (w1 & 0xffff) | (w2 << 16);
  
  return((chped == chped2) && (chthr2 == chthr));
}



status_code CAL_Calibration() 
{
  DAQ_FE idaqfe;
  UINT16 fe_mask,cal_raw_mask,cal_loop3,cal_loop4,repeat_fe,fe,data[6],dummy,alarm_reg;
  BYTE *ptr;
  int i,j;
  status_code status;
  DAQ_CMD_BUF* auto_buf;

  DAQ_ResetPIF(DAQ_DEFAULT_ETO_TICS,DAQ_MAX_DTO_TICS,SM_DAQSETUP,HB_PKTTYPE_RSV_CAL_CALIB);

  fe_mask = CAL_DSP_MASK;
  cal_raw_mask = cal_loop3 = repeat_fe = 0; 

 REDO_CALIB:
  cal_loop4 = 0;  
  DAQ_Format_CMD_Empty(&daq_temp);
  DAQ_Format_CMD_Empty(&CAL_TempPif3);
  PM_pi_SetPktType(HB_PKTTYPE_CALIB_CAL);

  for(fe = 1,idaqfe=DAQ_FE_CAL_1; idaqfe <= DAQ_FE_CAL_4; idaqfe++ , fe <<=1 ) 
    {
      if (fe & fe_mask)
	{
	  DAQ_Format_CMD_Empty(&CAL_TempPif1);   
	  CH_GetBuf(CH_CAL_RESET_FE,&auto_buf);
	  DAQ_Format_Fe2Daq(&CAL_TempPif1,auto_buf,idaqfe);
	  DAQ_Format_Fe2Daq(&CAL_TempPif1,&CAL_WRITE_DSP_MEM_I41,idaqfe);
	  CH_GetBuf(CH_CAL_WRITE_FPGA_REG_C2,&auto_buf);
	  DAQ_Format_Fe2Daq(&CAL_TempPif1,auto_buf,idaqfe);

	  status = DAQ_StoreCmdQueue2PIF(SM_CALEXP1,&CAL_TempPif1);
	  LU_EXIT_IFERROR(status);
	  status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP1,DAQ_STORE_MM,&daq_reply,DAQ_NO_STORE_RAM);
	  LU_EXIT_IFERROR(status);
	}
    }

  DAQ_Format_CMD_Empty(&CAL_TempPif0);
  CH_GetBuf(CH_TB_GIVE_ME_TRIGGER,&auto_buf );
  DAQ_Format_Cmd2Fe(&CAL_TempPif0,auto_buf,DAQ_SELECT_TRIG(PRH_VAR_TB_LINK)); 
  
  status = DAQ_StoreCmdQueue2PIF(SM_CALEXP0,&CAL_TempPif0);
  LU_EXIT_IFERROR(status);


  status = CAL_SendTrigger(SM_CALEXP0,1850);
  LU_EXIT_IFERROR(status);

  fe = 0x0001;
  
  for(idaqfe=DAQ_FE_CAL_1, j=0; idaqfe <= DAQ_FE_CAL_4; idaqfe++ , fe <<=1, j++ ) 
    {
      if (fe & fe_mask)
	{
	  CH_GetBuf(CH_CAL_READ_EVENT_DATA,&auto_buf);
	  DAQ_Format_CMD_Empty(&CAL_TempPif2);
	  DAQ_Format_Fe2Daq(&CAL_TempPif2,auto_buf,idaqfe);
	  DAQ_Format_Fe2Daq(&CAL_TempPif2,&CAL_READ_DSP_MEM_C31,idaqfe);
  
	  PM_pi_SetPktType(HB_PKTTYPE_CALIB_CAL_PED);
	  status = DAQ_StoreCmdQueue2PIF(SM_CALEXP2,&CAL_TempPif2);
	  LU_EXIT_IFERROR(status);
	  status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP2,DAQ_STORE_MM,&daq_reply,DAQ_STORE_RAM);
	  LU_EXIT_IFERROR(status);
	  
	  for (i = 0, ptr = &daq_reply.buf[CAL_SKIP_HEADER]; i < 6; i++)
	    CM_READ_BE_UINT16(ptr,data[i],dummy);
	  
	  if (!( 
	      ((data[0] & 0xe047) == 0xe000) && 
	      (data[1]==2) && 
	      (data[2]==0) && 
	      (CAL_CheckCRC(j,data[3])) && 
	      ((data[4] & 0xe047)==0xa000) &&
	      (data[5]==0x1215) && 
	      CAL_Pack(&daq_reply)
	      ))
	    {
	      repeat_fe+=fe;
	      cal_loop4 = 1;
	    }
	}
    }

  if (cal_loop4)
    {
      if (cal_loop3 == PRH_ARR_CAL_COUNT[2])
	{
	  for(idaqfe=DAQ_FE_CAL_1, fe = 1; idaqfe <= DAQ_FE_CAL_4; idaqfe++ , fe <<=1) 
	    {
	      if (fe & repeat_fe)
		{
		  CH_GetBuf(CH_CAL_RESET_FE,&auto_buf);
		  DAQ_Format_CMD_Empty(&CAL_TempPif3);
		  DAQ_Format_Fe2Daq(&CAL_TempPif3,auto_buf,idaqfe);
		  CH_GetBuf(CH_CAL_WRITE_FPGA_REG_RAW,&auto_buf);
		  DAQ_Format_Fe2Daq(&CAL_TempPif3,auto_buf,idaqfe);
		  CAL_DSP_MASK &=~fe;
		  status = DAQ_StoreCmdQueue2PIF(SM_CALEXP3,&CAL_TempPif3);	    
		  LU_EXIT_IFERROR(status);
		  status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP3,DAQ_STORE_MM,&daq_reply,DAQ_NO_STORE_RAM);
		  LU_EXIT_IFERROR(status);	  		  	  
		}
	    }
	}
      else
	{
	  cal_loop3++;
	  fe_mask = repeat_fe;
	  repeat_fe=0;
	  goto REDO_CALIB;
	}
    }

  PM_pi_SetPktType(HB_PKTTYPE_CALIB_CAL_PULSE1);
  
  status = CAL_Pulse(CAL_DSP_MASK,0,PRH_VAR_CAL_VCAL);
  LU_EXIT_IFERROR(status);	  
  
  status = CAL_ReadPulseData(CAL_DSP_MASK);
  LU_EXIT_IFERROR(status);	  

  PM_pi_SetPktType(HB_PKTTYPE_CALIB_CAL_PULSE2);

  status = CAL_Pulse(CAL_DSP_MASK,PRH_VAR_CAL_CH,PRH_VAR_CAL_VCAL);
  LU_EXIT_IFERROR(status);	  
  status = CAL_ReadPulseData(CAL_DSP_MASK);
  LU_EXIT_IFERROR(status);	  
  
  if (PRH_VAR_CAL_CH == 0x8000)
    {
      PRH_VAR_CAL_CH = 0x0001;
      
      if (PRH_VAR_CAL_VCAL == 0x8005)
	PRH_VAR_CAL_VCAL = 0x8015;
      else
	PRH_VAR_CAL_VCAL = 0x8005;
    }
  else
    PRH_VAR_CAL_CH*=2;

  return status;
}

status_code CAL_Error_Handler()
{
  DAQ_FE idaqfe;
  int i,bit_count;
  BYTE *ptr,dummy;
  UINT16 idaq_status,fe,data[4];
  UINT32 alarm_reg;
  UINT32 cal_count=0;
  status_code status = CM_RC_SUCCESSFUL;
  DAQ_CMD_BUF* auto_buf;
  BOOL check_ok;

  DAQ_ResetPIF(DAQ_DEFAULT_ETO_TICS,DAQ_DEFAULT_DTO_TICS,SM_DAQSETUP,HB_PKTTYPE_CAL_ALARM);

  status = HK_ReadKHBAlarmRegister(&alarm_reg);
  LU_EXIT_IFERROR(status);
  
  alarm_reg&=(KHB_ALARM_CALO_1 | KHB_ALARM_CALO_2 | KHB_ALARM_CALO_3 | KHB_ALARM_CALO_4);

  alarm_reg>>=4;
  

  for(idaqfe=DAQ_FE_CAL_1,fe=0x0001; idaqfe <= DAQ_FE_CAL_4; idaqfe++, fe <<=1) 
    {
      if (fe & alarm_reg)
	{
	  DAQ_Format_CMD_Empty(&CAL_TempPif0);
	  CH_GetBuf(CH_CAL_READ_ALARM,&auto_buf);
	  DAQ_Format_Fe2Daq(&CAL_TempPif0,auto_buf,idaqfe);
  
	  for(i=0; i < 7; i++)
	    {
	      CH_GetBuf(CAL_CAL_READ_FPGA_REG_BASE_ID+i,&auto_buf);
	      DAQ_Format_Fe2Daq(&CAL_TempPif0,auto_buf,idaqfe);
	    }

	  status = DAQ_StoreCmdQueue2PIF(SM_CALEXP0,&CAL_TempPif0);	    
	  if (status) 
	    {
	      status = CM_RC_DO_RESET_IF_ALM;
	      LU_EXIT_IFERROR(status);
	    }

	  cal_count = 0; check_ok = FALSE;

	  while ((cal_count < PRH_ARR_CAL_COUNT[3]) && (status == CM_RC_SUCCESSFUL) && !check_ok)
	    {
	      status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP0,TRUE,&daq_reply,DAQ_STORE_RAM);
  
	      if (status == CM_RC_SUCCESSFUL) 
		{
		  if (daq_reply.len < 2 + CAL_SKIP_HEADER)
		    status = CM_RC_DO_RESET_IF_ALM;
		  else
		    {
		      ptr = &daq_reply.buf[CAL_SKIP_HEADER];
		      CM_READ_BE_UINT16(ptr,data[0],dummy);
		      CM_READ_BE_UINT16(ptr,data[1],dummy); 
		      CM_READ_BE_UINT16(ptr,data[2],dummy);
		      CM_READ_BE_UINT16(ptr,data[3],dummy); 
	      	      
		      if (((data[0] & 0xE007) == 0x2000) && (data[1] == 3))
			check_ok = TRUE;
		    }
	  
		  cal_count++;
		}
	      else
		status = CM_RC_DO_RESET_IF_ALM;
	    }
  
	  LU_EXIT_IFERROR(status);
  
	  if (check_ok)
	    {
	      if ((data[0] & 0x0020) == 0x0020)
		{
		  bit_count = 0;
	  
		  for(i=0; i < 11; i++)
		    {
		      if (data[3] & 1)
			bit_count++;
	      
		      data[3]>>=1;
		    }
	      
		  if (bit_count > PRH_VAR_CAL_TEMP)
		    {
		      DAQ_Format_CMD_Empty(&CAL_TempPif0);		      
		      
		      CH_GetBuf(CH_CAL_RESET_FE,&auto_buf);
		      DAQ_Format_Fe2Daq(&CAL_TempPif0,auto_buf,idaqfe);
		      
		      DAQ_Format_Fe2Daq(&CAL_TempPif0,&CAL_WRITE_FPGA_REG_OFF,idaqfe);
		      		      
		      status = DAQ_StoreCmdQueue2PIF(SM_CALEXP0,&CAL_TempPif0);
		      if (status) 
			{
			  status = CM_RC_DO_RESET_IF_ALM;
			  LU_EXIT_IFERROR(status);
			}
		      
		      status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP0,TRUE,&daq_reply,TRUE);
		      if (status) 
			{
			  status = CM_RC_DO_RESET_IF_ALM;
			  LU_EXIT_IFERROR(status);
			}
		      
		      status = RM_TaskSuspendUnlessSINT(100);
		      if (status) 
			{
			  status = CM_RC_DO_RESET_IF_ALM;
			  LU_EXIT_IFERROR(status);
			}

		      DAQ_Format_CMD_Empty(&CAL_TempPif0);		      
		      CH_GetBuf(CH_CAL_READ_ALARM,&auto_buf);
		      DAQ_Format_Fe2Daq(&CAL_TempPif0,auto_buf,idaqfe);
		      
		      status = DAQ_StoreCmdQueue2PIF(SM_CALEXP0,&CAL_TempPif0);
		      if (status) 
			{
			  status = CM_RC_DO_RESET_IF_ALM;
			  LU_EXIT_IFERROR(status);
			}
		      
		      status = DAQ_SendCmd2PIFWaitDAQReply(SM_CALEXP0,TRUE,&daq_reply,TRUE);
		      if (status)
			{
			  status = CM_RC_DO_RESET_IF_ALM;
			  LU_EXIT_IFERROR(status);
			}	      		      
		      CAL_FE_MASK &=~fe;
		      CAL_DSP_MASK &=~fe;
		    }
		}
	    }
	  else 
	    // CAL_FE_MASK&=~fe;
	    ;
	}
    }
  
  status = DAQ_ReadStatusWord(&idaq_status);
  if (status) 
    status = CM_RC_DO_RESET_IF_ALM;
     
  return status;
}