//2013.10.24
/*******************************************************************************
*   モジュール名称      ：アプリケーション02 - HK read & anaylses - MPPE/MIA   *
*   参考                ：  app02_DPU.c を参照                                 *
*   注意				：  EDIT権限は、MPPE/MIA組                             *
*   作成日・作成者      ：2011/10/08    Y.K.                                   *
*******************************************************************************/
#include 	"app_common.h"
#include 	"app_MPPE-MIA.h"

#define		d_SIZEhk_MIA		128		// HK-size	

int app02_MIA_setTable0();
int app02_MIA_setTable1();
int app02_MIA_readE2PROM();
int app02_genSWHK_MIA(unsigned char*);
// ********************************************************************
// *** MIA :  HK analyses, HW CMD production, HK packet production ***
// ********************************************************************
int		app02_MIA  ( unsigned char  uc_dataHK[], unsigned int ui_counter, unsigned char uc_link )
{
	unsigned char	uc_dpuSwHk[0x100];
	int				i_dpuSwHK_size = 0;
	int				i;
	int				i_flag;

	struct _s_MIA_CtrlParam		*pCtrl;		//制御用のフラグ及びパラメータ
	struct _s_MIA_Command		*pCmd;		//コマンド
	struct _s_MIA_Product		*pDat;		//プロダクト
	pCtrl = &Gst_MIA_ctrl;
	pCmd  = &Gst_MIA_cmd;
	pDat  = &Gst_MIA_dat;
	unsigned char uc_EMER2[4];
	
	i_flag = 0;
	//
	//	コマンド対応
	//
	// # Set Lookup TABLE
	if (pCtrl->uc_settbl_n == 1) {
		app02_MIA_setTable0();
		pCtrl->uc_settbl_n = 2;
	}
	else if (pCtrl->uc_settbl_n == 2) {
		app02_MIA_setTable1();
		pCtrl->uc_settbl_n = 0;
	}
	// # read E2PROM 
	if (pCtrl->uc_readrom) {
		i_flag = app02_MIA_readE2PROM();
		pCtrl->uc_readrom = FALSE;
		pCtrl->uc_settbl_n = 0x10;
	}
	else if (pCtrl->uc_settbl_n == 0x10) {
		app02_MIA_setTable1();
		pCtrl->uc_settbl_n = 0;
		pCtrl->uc_got_e2p_prm = TRUE;
	}
	
	// *************************
	// ***	HW HK analyses   ***
	// *************************

	if ( uc_link ) {
	
		//MPPE_MIA_EMER2_EXEC
		uc_EMER2[0] = 0x02; uc_EMER2[1] = 0xff;
		uc_EMER2[2] = 0xff; uc_EMER2[3] = 0xff;
	
		// --- HW-ON  ---
		
		//HK monitor ENA HKによってHV OFFを行う。
		//MHV
		if (pCtrl->uc_HKmon_ena & 0x01)
			if (uc_dataHK[0x06] > pCtrl->uc_MHV_lmt[1] || uc_dataHK[0x06] < pCtrl->uc_MHV_lmt[0]) {
				i_flag = app_PiCmd(Gd_N_MIA, uc_EMER2, 4 );//HV off
				pCtrl->uc_HK_anmly |= 0x01;						//HK
				if (!i_flag) pCtrl->uc_HKmon_ena &= 0xfe;		//CMD正常受付時にフラグを倒す
			}
		//SVS
		if (pCtrl->uc_HKmon_ena >> 1 & 0x01)
			if (uc_dataHK[0x08] > pCtrl->uc_SVS_lmt[1] || uc_dataHK[0x08] < pCtrl->uc_SVS_lmt[0]) {
				i_flag = app_PiCmd(Gd_N_MIA, uc_EMER2, 4 );//HV off
				pCtrl->uc_HK_anmly |= 0x02;						//HK
				if (!i_flag) pCtrl->uc_HKmon_ena &= 0xfd;	//CMD正常受付時にフラグを倒す
			}
		//SVG
		if (pCtrl->uc_HKmon_ena >> 2 & 0x01)
			if (uc_dataHK[0x0a] > pCtrl->uc_SVG_lmt[1] || uc_dataHK[0x0a] < pCtrl->uc_SVG_lmt[0]) {
				i_flag = app_PiCmd(Gd_N_MIA, uc_EMER2, 4 );//HV off
				pCtrl->uc_HK_anmly |= 0x04;						//HK
				if (!i_flag) pCtrl->uc_HKmon_ena &= 0xfb;	//CMD正常受付時にフラグを倒す
			}
		//MCP-I
		if (pCtrl->uc_HKmon_ena >> 3 & 0x01)
			if (uc_dataHK[0x11] > pCtrl->uc_MCPI_lmt[1] || uc_dataHK[0x11] < pCtrl->uc_MCPI_lmt[0]) {
				i_flag = app_PiCmd(Gd_N_MIA, uc_EMER2, 4 );//HV off
				pCtrl->uc_HK_anmly |= 0x08;						//HK
				if (!i_flag) pCtrl->uc_HKmon_ena &= 0xf7;	//CMD正常受付時にフラグを倒す
			}
		
		//以下はMIAのOFFまで行う。
		//ASIC-T
		if (pCtrl->uc_HKmon_ena >> 4 & 0x01)
			if (uc_dataHK[0x0f] > pCtrl->uc_ASCT_lmt[1] || uc_dataHK[0x0f] < pCtrl->uc_ASCT_lmt[0]) {
				i_flag = app_PiCmd(Gd_N_MIA, uc_EMER2, 4 );//HV off
				pCtrl->uc_HK_anmly |= 0x10;						//HK
				if (!i_flag) {
					pCtrl->uc_HKmon_ena &= 0xef;	//CMD正常受付時にフラグを倒す
					pCtrl->uc_HK_delay = 1;					//MIA PSU off カウントダウン開始
				}
			}
		//IF-T
		if (pCtrl->uc_HKmon_ena >> 5 & 0x01)
			if (uc_dataHK[0x10] > pCtrl->uc_IFT_lmt[1] || uc_dataHK[0x10] < pCtrl->uc_IFT_lmt[0]) {
				i_flag = app_PiCmd(Gd_N_MIA, uc_EMER2, 4 );//HV off
				pCtrl->uc_HK_anmly |= 0x20;						//HK
				if (!i_flag) {
					pCtrl->uc_HKmon_ena &= 0xdf;	//CMD正常受付時にフラグを倒す
					pCtrl->uc_HK_delay = 1;					//MIA PSU off カウントダウン開始
				}
			}
		//ラッチアップ
		if (pCtrl->uc_HKmon_ena >> 6 & 0x01)
			if (uc_dataHK[2] & 0x01) {//ASICのラッチアップ検出
				i_flag = app_PiCmd(Gd_N_MIA, uc_EMER2, 4 );//HV off
				pCtrl->uc_HK_anmly |= 0x40;						//HK
				if (!i_flag) {
					pCtrl->uc_HKmon_ena &= 0xbf;			//CMD正常受付時にフラグを倒す
					pCtrl->uc_HK_delay = 1;					//MIA PSU off カウントダウン開始
				}
			}
		
		//MIA PSU OFF
		if (pCtrl->uc_HK_delay)
			if (pCtrl->uc_HK_delay++ > d_Lcl_MIA_DELAY) {
				i_flag = app_DmcRequestCmd(0x0a);
				pCtrl->uc_HK_anmly |= 0x80;					//HK
				if (!i_flag) pCtrl->uc_HK_delay = 0;
			}

	} else {
		// --- HW-OFF ---
		for (i=0; i<d_SIZEhk_MIA; i++)  uc_dataHK[i] = 0;
	}
	
	if (i_flag) pCmd->uc_err_cnt++;

	// *************************
	// ***	SW/HK production ***
	// *************************
	if ( uc_link ) {// --- HW-ON  ---
		i_dpuSwHK_size = app02_genSWHK_MIA(uc_dpuSwHk);

	} else {
		// --- HW-OFF ---
	}

	// *****************************
	// ***	HK Packet production ***
	// *****************************
	if ( i_dpuSwHK_size > 0)	memcpy( &uc_dataHK[d_SIZEhk_MIA], uc_dpuSwHk, i_dpuSwHK_size );
	else						i_dpuSwHK_size = 0;

	// ******************************
	// ***	HK Production in 'OFF ***
	// ******************************
	if ( uc_link == 0 ) return( 0 );
	else				return( d_SIZEhk_MIA+i_dpuSwHK_size );
}

int app02_genSWHK_MIA(unsigned char uc_SwHK[])
{
	struct _s_MIA_CtrlParam		*pCtrl;		//制御用のフラグ及びパラメータ
	struct _s_MIA_Command		*pCmd;		//コマンド

	pCtrl = &Gst_MIA_ctrl;
	pCmd  = &Gst_MIA_cmd;

	//SOFTWARE HK
	uc_SwHK[0] = pCmd->uc_cnt;//コマンドカウンタ
	uc_SwHK[1] = pCmd->uc_err_cnt;//エラーカウンタ
	uc_SwHK[2] = pCmd->uc_ans[0];//コマンドアンサ0-3
	uc_SwHK[3] = pCmd->uc_ans[1];
	uc_SwHK[4] = pCmd->uc_ans[2];
	uc_SwHK[5] = pCmd->uc_ans[3];
	uc_SwHK[6] = (pCtrl->uc_snap_ave & 0x01)	+ (pCtrl->uc_eng_sci << 1 & 0x02)
												+ (pCtrl->uc_mode    << 2 & 0xfc);
	uc_SwHK[7] = pCtrl->uc_HKmon_ena;
	memcpy(&uc_SwHK[8],  pCtrl->uc_MHV_lmt,  2);
	memcpy(&uc_SwHK[10], pCtrl->uc_SVS_lmt,  2);
	memcpy(&uc_SwHK[12], pCtrl->uc_SVG_lmt,  2);
	memcpy(&uc_SwHK[14], pCtrl->uc_MCPI_lmt, 2);
	memcpy(&uc_SwHK[16], pCtrl->uc_ASCT_lmt, 2);
	memcpy(&uc_SwHK[18], pCtrl->uc_IFT_lmt,  2);
	memcpy(&uc_SwHK[20], &pCtrl->ui_eng_vmf, 4);
	uc_SwHK[24] = pCtrl->uc_HK_anmly;
	uc_SwHK[25] = (pCtrl->uc_got_e2p_prm & 0x01) + (pCtrl->uc_swcal_prm[0] << 1 & 0x02)
												 + (pCtrl->uc_vet_cmp_ena  << 2 & 0x04);
//												 + (pCtrl->uc_watchman     << 3 & 0x08);
	uc_SwHK[26] = d_Lcl_MIA_TLMID0;
	
	uc_SwHK[27] = d_Lcl_MIA_TLMID1;
	
	return (28); //Size of Software HK
}

int app02_MIA_readE2PROM()
{
	int		i_flag;
	struct	_s_MIA_CtrlParam		*pCtrl;		//制御用のフラグ及びパラメータ
	struct	_s_MIA_ProcParam		*pP;		//解析用パラメータ E2PROMからデータを読み込む

	pCtrl = &Gst_MIA_ctrl;
	pP =  &Gst_MIA_proc;
	i_flag = 0;
	
	//HKモニタの制限値の読み込み
	i_flag +=  app_readROM(Gd_MIA_RomAdr,     2, pCtrl->uc_MHV_lmt);
	i_flag +=  app_readROM(Gd_MIA_RomAdr + 2, 2, pCtrl->uc_SVS_lmt);
	i_flag +=  app_readROM(Gd_MIA_RomAdr + 4, 2, pCtrl->uc_SVG_lmt);
	i_flag +=  app_readROM(Gd_MIA_RomAdr + 6, 2, pCtrl->uc_MCPI_lmt);
	i_flag +=  app_readROM(Gd_MIA_RomAdr + 8, 2, pCtrl->uc_ASCT_lmt);
	i_flag +=  app_readROM(Gd_MIA_RomAdr +10, 2, pCtrl->uc_IFT_lmt);		
	//VM計算用のパラメータ読み込み
	i_flag +=  app_readROM(Gd_MIA_RomAdr +12,      128, (unsigned char*)pP->us_gf);		//(0 Max)/Max x ffff
	//i_flag +=  app_readROM(Gd_MIA_RomAdr +12+ 144, 144, (unsigned char*)pP->us_dE);
	//i_flag +=  app_readROM(Gd_MIA_RomAdr +12+ 288, 144, (unsigned char*)pP->us_sr);
	i_flag +=  app_readROM(Gd_MIA_RomAdr +12 +128,    128, (unsigned char*)pP->s_csp);		//(-90 +90) /180 x 7fff
	i_flag +=  app_readROM(Gd_MIA_RomAdr +12 +256,    128, (unsigned char*)pP->s_csa);		//(-180 +180) /180 x 7fff
	i_flag +=  app_readROM(Gd_MIA_RomAdr +12 +384,    832, (unsigned char*)pP->us_vlc);	//(0 Max)/Max x ffff
	i_flag +=  app_readROM(Gd_MIA_RomAdr +12 +384+832, 64, (unsigned char*)pP->uc_vf);	//(0 Max)/Max x ffff
	return (i_flag);
}

int app02_MIA_setTable0() 
{
	int i;
	unsigned char uc[32];
	struct _s_MIA_ProcParam		*pP;
	double f_nrm;
	
	f_nrm = 1/180.*d_Lcl_MIA_TBL_RD0;	//規格化パラメータ
	pP    = &Gst_MIA_proc;
	

	//GF & FACTOR of Vtable
	for (i =  0; i < 16; ++i)	{ pP->us_gf[i] = 0x7ff;		pP->uc_vf[i] = 0x7f; }	//SW16ch GFoff
	for (i = 16; i < 32; ++i)	{ pP->us_gf[i] = 0x4fff;	pP->uc_vf[i] = 0x7f; }	//SW16ch GFON
	for (i = 32; i < 40; ++i)	{ pP->us_gf[i] = 0x2ff;		pP->uc_vf[i] = 0x7f; }	//AntiSW 8ch GFoff
	pP->us_gf[35] /= 2; pP->us_gf[36] /= 2;										//CHの大きさが半分
	for (i = 40; i < 48; ++i)	{ pP->us_gf[i] = 0x7fff;	pP->uc_vf[i] = 0x7f; }	//AntiSW 8ch GFON
	pP->us_gf[43] /= 2; pP->us_gf[44] /= 2;										//CHの大きさが半分
	for (i = 48; i < 64; ++i)	{ pP->us_gf[i] = 0x7fff;	pP->uc_vf[i] = 0x7f; }	//AntiSW 16ch GFoff
	pP->us_gf[55] /= 2; pP->us_gf[56] /= 2;										//CHの大きさが半分
	
	//
	//for (i =  0; i < 64; ++i) pP->us_gf[i] = 0xffff; //debug
	//
	
	
	//cosAlpha, sinAlpha
	for (i =  0; i < 16; ++i)
		pP->s_csp[i] = pP->s_csp[i+16] = (short) ( (42.1875 - i*5.625)*f_nrm );
	for (i =  32; i < 40; ++i)
		pP->s_csp[i] = pP->s_csp[i+8] = (short) ( (-78.75 + (i-32)*22.5)*f_nrm );
	for (i = 48; i < 64; ++i)
		pP->s_csp[i] = (short) ( (-84.375 + (i-48)*11.25)*f_nrm );
	
	//cosBeta, sinBeta
	for (i = 0; i < 16; ++i)
		pP->s_csa[i] = pP->s_csa[i+16] = (short)( 137.8125*f_nrm );	//-45 + 180 + 90/32 deg.
	//SWとは180度の差異がある
	for (i =  32; i < 40; ++i)
		pP->s_csa[i] = pP->s_csa[i+8] = (short)( -78.75*f_nrm );	//-90 + 90/8 deg.
	for (i = 48; i < 64; ++i)
		pP->s_csa[i] = (short)( -84.375*f_nrm );						//-90 + 90/16 deg.

	//Velocity
	//for (i = 0; i < 16; ++i) { uc[i] = 1 + 2*i; uc[i+16] = 30 - 2*i; }
	for (i = 0; i < 16; ++i) { uc[2*i] = 31 - i; uc[2*i + 1] = i; }
	//0 = 0
	//0xffff = Max
	pP->us_vlc[uc[0]] = pP->us_vlc[uc[0] +128] = 2676;//T0&T1 100-10keV
	for (i = 1; i < 32; ++i) pP->us_vlc[uc[i]    ] = pP->us_vlc[uc[i]     +128] = pP->us_vlc[uc[i-1]]*1.0752;
	for (i = 0; i < 32; ++i) pP->us_vlc[uc[i] +32] = pP->us_vlc[uc[i] +32 +128] = pP->us_vlc[uc[i]]*1.0183;
	for (i = 0; i < 32; ++i) pP->us_vlc[uc[i] +64] = pP->us_vlc[uc[i] +64 +128] = pP->us_vlc[uc[i]]*1.0369;
	for (i = 0; i < 32; ++i) pP->us_vlc[uc[i] +96] = pP->us_vlc[uc[i] +96 +128] = pP->us_vlc[uc[i]]*1.0559;
	
	pP->us_vlc[uc[0] +256] = 598;	//T2 5-300eV
	for (i = 1; i < 32; ++i) pP->us_vlc[uc[i] +256] = pP->us_vlc[uc[i-1] +256]*1.0683;
	pP->us_vlc[uc[0] +288] = 14652;	//T2 3k-30keV
	for (i = 1; i < 32; ++i) pP->us_vlc[uc[i] +288] = pP->us_vlc[uc[i-1] +288]*1.0338;
	
	pP->us_vlc[uc[0] +320] = 598;//T3 5-30keV
	for (i = 1; i < 32; ++i) pP->us_vlc[uc[i] +320] = pP->us_vlc[uc[i-1] +320]*1.1506;
	pP->us_vlc[uc[0] +352] = 598;//T4 5-5keV
	for (i = 1; i < 32; ++i) pP->us_vlc[uc[i] +352] = pP->us_vlc[uc[i-1] +352]*1.1179;
	pP->us_vlc[uc[0] +384] = 598;//T5 5-30keV
	for (i = 1; i < 32; ++i) pP->us_vlc[uc[i] +384] = pP->us_vlc[uc[i-1] +384]*1.1506;
	
	return TRUE;
}

//解析用パラメータの角度情報からcos, sinテーブルを作成し、と一部をセクター方向に展開する
int app02_MIA_setTable1()
{
	int		i, j;
	struct	_s_MIA_ProcParam		*pP;		//解析用パラメータ E2PROMからデータを読み込む
	double	f_sctr;
	double f_nrm;

	pP =  &Gst_MIA_proc;
	f_nrm = MATH_PI/d_Lcl_MIA_TBL_RD0;	//規格化パラメータ

	for (i = 0; i < 16*2 + 8*2 + 16; ++i) {
		pP->s_snp[i] =(short) (app_math_sin( pP->s_csp[i]*f_nrm )*d_Lcl_MIA_TBL_RD1);
		pP->s_csp[i] =(short) (app_math_cos( pP->s_csp[i]*f_nrm )*d_Lcl_MIA_TBL_RD1);
	}
	//RAD -> sin, cos, セクター方向にも展開
	//再配置
	memcpy(&pP->s_csa[256], &pP->s_csa[16], 16*2);
	memcpy(&pP->s_csa[512], &pP->s_csa[32],  8*2);
	memcpy(&pP->s_csa[640], &pP->s_csa[40],  8*2);
	memcpy(&pP->s_csa[768], &pP->s_csa[48], 16*2);
	f_sctr = MATH_PI/32;
	for (i = 0; i < 16; ++i) {
		for (j = 15; j >= 0; --j) {
			pP->s_sna[i+16*j] =    (short)(app_math_sin(pP->s_csa[i]    *f_nrm + j*f_sctr)*d_Lcl_MIA_TBL_RD1);
			pP->s_csa[i+16*j] =    (short)(app_math_cos(pP->s_csa[i]    *f_nrm + j*f_sctr)*d_Lcl_MIA_TBL_RD1);
			pP->s_sna[i+16*j+256] =(short)(app_math_sin(pP->s_csa[i+256]*f_nrm + j*f_sctr)*d_Lcl_MIA_TBL_RD1);
			pP->s_csa[i+16*j+256] =(short)(app_math_cos(pP->s_csa[i+256]*f_nrm + j*f_sctr)*d_Lcl_MIA_TBL_RD1);
	}}
	f_sctr = MATH_PI/8;
	for (i = 0; i < 8; ++i) {
		for (j = 15; j >= 0; --j) {
			pP->s_sna[i+8*j+512] =(short)(app_math_sin( pP->s_csa[i+512]*f_nrm + j*f_sctr)*d_Lcl_MIA_TBL_RD1);
			pP->s_csa[i+8*j+512] =(short)(app_math_cos( pP->s_csa[i+512]*f_nrm + j*f_sctr)*d_Lcl_MIA_TBL_RD1);
			pP->s_sna[i+8*j+640] =(short)(app_math_sin( pP->s_csa[i+640]*f_nrm + j*f_sctr)*d_Lcl_MIA_TBL_RD1);
			pP->s_csa[i+8*j+640] =(short)(app_math_cos( pP->s_csa[i+640]*f_nrm + j*f_sctr)*d_Lcl_MIA_TBL_RD1);
	}}
	f_sctr = MATH_PI/16;
	for (i = 0; i < 16; ++i) {
		for (j = 31; j >= 0; --j) {
			pP->s_sna[i+16*j+768] = (short)(app_math_sin(pP->s_csa[i+768]*f_nrm + j*f_sctr)*d_Lcl_MIA_TBL_RD1);
			pP->s_csa[i+16*j+768] = (short)(app_math_cos(pP->s_csa[i+768]*f_nrm + j*f_sctr)*d_Lcl_MIA_TBL_RD1);
	}}
	
	return TRUE;
}