/*******************************************************************************
*   モジュール名称      ：app14 Mission TLM (L&M) PI tasks                     *
*   モジュールラベル    ：app14_PIs                                            *
*   タスク区分          ：Application-04  EWO task for Mission TLM (L&M)       *
*   機能                ：                                                     *
*   コーリングシーケンス：void app14_(PI)                                      *
*   引数                ：なし                                                 *
*   戻り値              ：なし                                                 *
*   使用上の注意        ：グローバル変数はアプリケーション０１でまとめて初期化 *
*   エラー処理          ：なし                                                 *
*   注意				：  EDIT権限は、EWO組                                  *
*   作成日・作成者      ：2013/11/23    T.Imachi                               *
*******************************************************************************/

#define __APP_WFC_OFA__
#define __APP_WFC_AM2P__

#include "app_common.h"
#include "app_EWO.h"
#include "dpu_api_proto.h"

// *** EWO: local***
#define		d_AM2P_pacSize		2058							// AM2P Packet size: 127 packets max []				(MAX:262144B)
#define		BITRATE_L	2
#define		BITRATE_M	3
	
// *** AM2P: Macro ***
int		app14_AM2P_readPacket(	unsigned char [], short [], short []);												//	****TMP****
void	app14_AM2P_putTLM	 ( int, unsigned int, int, unsigned char *, int);
// static void EWOB_Data_Setting(short*,short*,int,int);			// ************ COMMENT OUT		110723	YK ********

///Value of PI
static const double __PI=3.1415926535897932384626433832795;

///Look Up Table for the square
static const short pow_2[10]={1,2,4,8,16,32,64,128,256,512};

struct AM2P_TCWord{	// Original name: TCWord
		//Bit 0 de la selection du mode de fonctionnement
		unsigned Operating_Mode :3;
		//Bit 1 de la selection de l'amplitude d'emission
		unsigned Out_level1 :1;
		//Bit 0 de la selection de l'amplitude d'emission
		unsigned Out_level0 :1;
		//Bit 1 de la selection du gain du canal EWO
		unsigned Gain_EWO1 :1;
		//Bit 0 de la selection du gain du canal EWO
		unsigned Gain_EWO0 :1;
		//TM_Rate : Servant a savoir s'il y a un moyennage
		unsigned TM_Rate :2;
		//Bit 0 de la selection du mode de calibration
		unsigned Cal_Mode :3;
		//Bit 1 de la selection de la bande de frequence
		unsigned Freq_Range1 :1;
		//Bit 0 de la selection de la bande de frequence
		unsigned Freq_Range0 :1;
		//Bit Reserved
		unsigned Reserved :1;
		//Bit de Parite
		unsigned Parite :1;
	};

//Union to convert the short parameter in TC_WORD type
union Config_AM2P{
	//Configuration of l'AM2P This is the input of Software
	short Conf;
	//Structure of type TC_WORD to give the detailed configuration of AM2P
	struct AM2P_TCWord Conf_AM2P;
};

//Structure for the return of the DFT result
struct AM2P_result_DFT{
	//Phase result
	double phase;
	//Power result
	double amplitude;
};

//RETURN : Value of sinus in a double type.
static double AM2P_Loop_Sin(short value) 
{	double d_rad=0;
	double Result=0;
	d_rad = ((value)&(0x1FF)) *(0.012271846303085129837744700715936);
	Result = app_math_sin(d_rad);
	Result= Result *32767;
	return Result;
};


//////////////////////////////////////////////////////////////////////////////////////////
//																						//
//								PACKET_SIZE												//
//																						//
//////////////////////////////////////////////////////////////////////////////////////////


int app14_AM2P_Packet_Size ( int part, short TC_Word_tmp)		//Function to give the number of samples to send to Traitement_AM2P
{
	union Config_AM2P TC_Word;     	        //Structure to know the AM2P configuration
	TC_Word.Conf=TC_Word_tmp;	        //Copy the variable into the structure of AM2P configuration
	if(TC_Word.Conf_AM2P.Cal_Mode==3 || TC_Word.Conf_AM2P.Cal_Mode==7)       //Calibration HF for MEFISTO or EWO
	{	if(part == 1)		        //If it's the first called
		{	return 40960;}	        //Return value equal to 40960
		else if (part == 2)	        //If it's the second called
		{	return 30720;}	        //Return value equal to 30720
		else		       	        //If part isn't correct
		{	return 0;}	        //else return value equal to 0
	}
	else if (TC_Word.Conf_AM2P.Cal_Mode==2 || TC_Word.Conf_AM2P.Cal_Mode==6) //Calibration VLF for MEFISTO or EWO
	{	if(part == 1)       	//If the part is less than 11 called
		{	return 5376;}	        //Return value equal to 5376
		else				//If part isn't correct
		{	return 0;}	        //else return value equal to 0
	}
	else if (TC_Word.Conf_AM2P.Cal_Mode == 4)
	{	if(part == 1)				//If the part is 1
		{	return 8064;}			//Return value equal to 8064
		else						//If part isn't correct
		{	return 0;}				//else return value equal to 0
	}
	else if(TC_Word.Conf_AM2P.Operating_Mode==6 || TC_Word.Conf_AM2P.Operating_Mode==4)//Config 2
	{	if(part == 1)		        //If it's the first called
		{	return 69120;}	        //Return value equal to 69120
		else if(part == 2)	        //If it's the second called
		{	return 63744;}	        //Return value equal to 63744
		else				//If part isn't correct
		{	return 0;}	        //else return value equal to 0
	}
	else if(TC_Word.Conf_AM2P.Operating_Mode==0 || TC_Word.Conf_AM2P.Operating_Mode==1 || TC_Word.Conf_AM2P.Operating_Mode==2 || TC_Word.Conf_AM2P.Operating_Mode==3)	//Config 1
	{	if(part == 1)		        //If it's the first called
		{	return 67456;}	        //Return value equal to 67456
		else if(part == 2)	        //If it's the second called
		{	return 53760;}	        //Return value equal to 53760
		else				//If part isn't correct
		{	return 0;}	        //else return value equal to 0
	}
	else 
	{	return 0;}	                //else return value equal to 0
}

//////////////////////////////////////////////////////////////////////////////////////////
//																						//
//								DFT Processing											//
//																						//
//																						//
//////////////////////////////////////////////////////////////////////////////////////////

static struct AM2P_result_DFT
DFT(		short* signal,	//INPUT : Start signal adress
			short sig_size,	//INPUT : Size of band (de 128, 256 ou 512 point per frequency)
			short k			//INPUT : Index of frequency for the process(k*deltaFe)
							// !! It's not the number of frequency (among 90)
							// This index can vary between 1 and 256 because base sinus emit on antenna of 512 points 
							//F_indice=1=Fe/512 (Fe=360kHz or 60kHz)
			)
{
	short i=0;
	double acc_re=0;	//Accumulation variable (64 bits signed)
	double acc_im=0;	//Accumulation variable (64 bits signed) : no overflow possible
	double value=0;		//Variable for the value of phase
	double tmp_short;	//Temporary variable of the result of sinus
	short w=0;			//w = Processing frequency
	struct AM2P_result_DFT resultat_DFT;	//Statement of variable of type struct define in .h
	resultat_DFT.phase=0;		//Initialization of phase part of structure
	resultat_DFT.amplitude=0;	//Initialization of power part of structure

	// Calculation of DFT Real and imaginary part
	for (i=0; i<sig_size; i++)
	{
		w=(short)i*k;	//Frequence ・traiter
		tmp_short=(double)AM2P_Loop_Sin(w+128)/(double)32767;	//Sinus of frequency --- +128 (cosinus in real) --- /32767 because 16 bits coded (short) 
		acc_re=acc_re+(double)signal[i]*tmp_short;	// Accumulation of values of real part

		tmp_short=AM2P_Loop_Sin(w+256)/(double)32767;	//Sinus of frequency ---- +256 (Sinus in imaginary) --- /32767 because 16 bits coded (short) 
		acc_im=acc_im+(double)signal[i]*tmp_short;	//Accumulation of values of imaginary part

	}
	value=app_math_atan(acc_im/acc_re)+(1.5707963267948966192313216916398);		//Calculation of value of phase for this frequency
	if(acc_re<0)
	{
		value=value+3.1415926535897932384626433832795;
	}
	if(value<0.0)			//If this value is negative
	{	
		value=value+((double)6.283185307179586476925286766559);		//turn it of 2Pi
	}
	else if(value>(6.283185307179586476925286766559))
	{	value=value-((double)6.283185307179586476925286766559);
	}
	resultat_DFT.phase=value;		//Storage of value on output variable
	
	// Accumulation variables do in maximum 41 bits
	// For calculate Rｲ+Iｲ we need to pass variable in 30 bits
	// because we need to square on its and add up its
	acc_re=(acc_re/(double)sig_size)*(acc_re/(double)sig_size);
	acc_im=(acc_im/(double)sig_size)*(acc_im/(double)sig_size);
	// acc_ is 16,777,216 times less than real value
	resultat_DFT.amplitude=acc_re+acc_im;	
	return resultat_DFT;
}

static void Traitement_DFT(	short* signal,			//Signal up on which make the process
						struct AM2P_result_DFT * p_DFT_Result_Temp,
						short *boucle,			//Nmoy, index of the start of loop of frequency,index of the end of loop of frequency,Start signal adress,index of the number of acquisition,Ratio multiplier for the Delta_Fe,Ratio subtraction for the Delta_Fe
						double *Valeur_Base
						)
{
	struct AM2P_result_DFT resultat_DFT;	//Statement of variable of type struct define in .h
	resultat_DFT.amplitude=0;	//Initialization of power part of structure
	resultat_DFT.phase=0;		//Initialization of phase part of structure
	if(boucle[7] != 2 && boucle[7] != 3)
		{	resultat_DFT=DFT(signal,(short)boucle[3],(short)(((boucle[1]+1)*boucle[5])-boucle[6]));	//Use the index table of frequency to place on the first sampels of the frequency wanted.
			if(boucle[3]==128)				
			{	resultat_DFT.phase= resultat_DFT.phase+((resultat_DFT.phase>3.1415926535897932384626433832795)? -1 : 1)*boucle[4]*3.1415926535897932384626433832795;}		//if we are in the last frequency band and we are in acquistion unpaired
			p_DFT_Result_Temp->amplitude=p_DFT_Result_Temp->amplitude+resultat_DFT.amplitude/boucle[0];	//Average the power on 4
			if(boucle[4] == 0)
			{	Valeur_Base[0] = resultat_DFT.phase;}
			else
			{	if((Valeur_Base[0]+3.1415926535897932384626433832795)<resultat_DFT.phase)
				{	resultat_DFT.phase = resultat_DFT.phase -6.283185307179586476925286766559;}
				else if((Valeur_Base[0]-3.1415926535897932384626433832795)>resultat_DFT.phase)
				{	resultat_DFT.phase = resultat_DFT.phase +6.283185307179586476925286766559;}
			}
			p_DFT_Result_Temp->phase=p_DFT_Result_Temp->phase+resultat_DFT.phase/boucle[0];						//Average the phase on 4
		}
		else
		{	resultat_DFT=DFT(signal,(short)boucle[3],boucle[5]);	//Use the index table of frequency to place on the first sampels of the frequency wanted.
			p_DFT_Result_Temp->phase=resultat_DFT.phase;							//Storage of the full value (no divide by Nmoy) 
			p_DFT_Result_Temp->amplitude=resultat_DFT.amplitude;				//In the result variable wanted (phase or power)
		}
}


short Calcul_ApoWin(short value)
{
	double d_rad=0;
	double result =0;
	d_rad = (value*0.012271846303085129837744700715936);
	result = app_math_cos(d_rad);
	result = (0.54-(0.46*result))*32767;
	return (short)(result);
}

static void ApoWin_Signal (	short* signal,	//Signal up on which make apodization
						short *boucle,	//Index of end of loop of frequency,index of number of samples,index of number of acquisition,index of start of loop of frequency,Number of acquisition
						char decalage_win	//Index of shift of apodization windows
					)

{
	short j;
	int adresse_depart;
	adresse_depart=(int)signal;
	for(j=0; j<boucle[3]; j++)	//Initialisation of loop for number of point
	{	signal=signal+j;	//Increment of value to apodize		
		*signal=*signal*Calcul_ApoWin(j*decalage_win)>>15;	//16bits*16bits=32bits>>16=16bits
		signal=(short*)adresse_depart;	//Reset of start adress
	}
}


//////////////////////////////////////////////////////////////////////////////////////////
//																						//
//								FFT Processing											//
//																						//
//																						//
//////////////////////////////////////////////////////////////////////////////////////////


// Concatenation on 15 bits and obtaining the power
static void 
//RETURN : Void
Acq_Ampl(	//INPUT : Complex with which we obtain the power
			double *p_real,
			double *p_imag,
			double *amplitude)
{
	double amp_tmp;				//Temporary variable of power
	double tmp_amp;				//Temporary variable of power
	double real_tmp;				//Temporary variable of real
	double imag_tmp;				//Temporary variable of imaginary
	short Mult_Coef =0;
	short Sub_Coef=0;
	short i=0;
	tmp_amp=0;			//Variable initialization
	real_tmp=0;
	imag_tmp=0;
	for (i=0; i<90; i++)
	{	if(i<30)
		{	Mult_Coef = 1;}
		else if(i<60)
		{	Mult_Coef = 2;
			Sub_Coef = 30;}
		else
		{	Mult_Coef = 4;
			Sub_Coef = 150;}
		real_tmp=((p_real[(i+1)*Mult_Coef -Sub_Coef])/512);		//Copy data in temporary variable
		imag_tmp=((p_imag[(i+1)*Mult_Coef -Sub_Coef])/512);		//In dividing by the number of samples because the FFT processing gives us the accumulation		
		tmp_amp=(real_tmp*real_tmp)+(imag_tmp*imag_tmp);	//Aｲ=Iｲ+Rｲ
		tmp_amp=(tmp_amp*2);							//S=|A|ｲ*2		
		if(tmp_amp<1) 
		{	amp_tmp=0;}					//If power is lower than 1 -> log(0.xxx)=-XXX Output power will be at 0
		else
		{	amp_tmp=10*(double)app_math_log10(tmp_amp)+5.3517;}					// N = 10*log(S)
		*amplitude=amp_tmp;		//Addition of the power lost by the windows apodization	
		amp_tmp=0;
		amplitude++;
	}
}



//Function launches the FFT processing on the reference spectrum and adapt the input signal
static void 
//RETURN : Void
Apply_FFT(		//INPUT : Signal on which does the FFT processing
				short * signal,
				//INPUT : Number of acquisition
				short Nmoy,			
				//OUTPUT : Power of signal processing
				unsigned short * Amplitude
				)
{	
	short i;						//Loop variable	
	double *adresse_depart;			//Variable to store signal start adress
	double  amplitude[90];		//Array of 256 double for power(before averaging and selection of frequency)	
	double *p_amplitude;
	//double G14f_AM2P_Signal[512];			//Global variable 20110830
	//double G14f_AM2P_acc_real[512];		//Global variable 20110830
	//double G14f_AM2P_acc_imag[512];		//Global variable 20110830
	double *p_Real;
	double *p_Imag;
	adresse_depart=&G14f_AM2P_Signal[0];	//Memorization of a signal start adress
	p_Real=&G14f_AM2P_acc_real[0];
	p_Imag=&G14f_AM2P_acc_imag[0];
	p_amplitude=&amplitude[0];
	for(i=0; i<90; i++)
	{	amplitude[i]=0;
	}
	for(i=0; i<512; i++)		//Loop by the number of samples
		{
			G14f_AM2P_Signal[i]=(double)signal[i];	//Shift of point adress 
		}
		app_EWO_rfft14(adresse_depart,512,p_Real,p_Imag);
		p_Real=&G14f_AM2P_acc_real[0];
		p_Imag=&G14f_AM2P_acc_imag[0];
		Acq_Ampl(p_Real,p_Imag,p_amplitude);	//Launch of power acquisition
		p_amplitude=&amplitude[0];
	for (i=0; i<90; i++) 
	{	*Amplitude=(unsigned short)(*Amplitude+(49.98779296875*(amplitude[i]/Nmoy)+0.5));	//concatenation on 16 bits
		Amplitude++;	//Incrementation of pointer adress to modify the good value
	}
}


void Init_Boucle(short Channel, short Start_Freq, short Cal, short k, short *boucle)
{
	short Indice_Sub =0;
	short Indice_Mult =0;
	short Nb_Smps =0;
	short part =0;
	short Nmoy =0;
	if(Channel == -1)
	{	Nmoy =4;}
	else
	{	if(Cal == 1)
		{	Nmoy =4;}
		else if(Cal == 0)
		{	Nmoy =2;}
		else
		{	Nmoy =1;}
	}
	if(Cal != 2 && Cal != 3)
	{
		if((Start_Freq <30 && Cal ==0) || (Start_Freq <10 && Cal==1))
		{	
			if(Channel == -1)
			{	part =1;}
			else
			{	if(Start_Freq <29)
				{	part =1;}
				else
				{	part =2;}
			}
			Nb_Smps = 512;
			if(Cal == 0)
			{	Indice_Mult=1;
				Indice_Sub=0;}
			else 
			{	Indice_Mult=3;
				Indice_Sub=2;}

		}
		else if((Start_Freq <60 && Cal ==0) || (Start_Freq <20 && Cal==1))
		{	part =2;
			Nb_Smps = 256;
			if(Cal == 0)
			{	Indice_Mult=2;
				Indice_Sub=30;}
			else
			{	Indice_Mult=6;
				Indice_Sub=34;}
		}
		else if((Start_Freq <90 && Cal ==0) || (Start_Freq <30 && Cal==1))
		{	part =2;
			Nb_Smps = 128;
			if(Cal == 0)
			{	Indice_Mult=4;
				Indice_Sub=150;}
			else
			{	Indice_Mult=12;
				Indice_Sub=158;}
		}
	}
	else
	{	part =1;
		Nmoy = 1;
		if( Start_Freq == 0 )
		{	Indice_Mult = 2;
			Nb_Smps = 512;}
		else if( Start_Freq == 1)
		{	Indice_Mult = 4;
			Nb_Smps = 512;}
		else if( Start_Freq == 2)
		{	Indice_Mult = 8;
			Nb_Smps = 512;}
		else if( Start_Freq == 3)
		{	Indice_Mult = 16;
			Nb_Smps = 512;}
		else if( Start_Freq == 4)
		{	Indice_Mult = 32;
			Nb_Smps = 256;}
		else if( Start_Freq == 5)
		{	Indice_Mult = 64;
			Nb_Smps = 256;}
		else if(  Start_Freq == 6)
		{	Indice_Mult = 130;
			Nb_Smps = 128;}
	}
	boucle[0] = Nmoy;
	boucle[1] = Start_Freq;
	boucle[2] = part;
	boucle[3] = Nb_Smps;
	boucle[4] = k;
	boucle[5] = Indice_Mult;
	boucle[6] = Indice_Sub;
	boucle[7] = Cal;
	boucle[8] = Channel;
}


void Init_FFT(short Nmoy, short channel, short *p_Input, unsigned short * p_Output)
{
	short k=0;
	short boucle[9];
	short i=0;
	short decal_Windows=0;	
	//short G14s_AM2P_Signal[512];		//Global variable 20110830
	unsigned short Amplitude[90];
	unsigned short *p_amplitude;		//Variable for the power of the signal processed
	short *p_signal;		//Variable of the signal to process
	for(i=0; i<512; i++)
	{	G14s_AM2P_Signal[i]=0;}	//Initialization of reference spectrum
	for(i=0; i<90; i++)
	{	Amplitude[i]=0;}
	p_amplitude=&Amplitude[0];	//Memorization of a power start adress
	p_signal=&G14s_AM2P_Signal[0];		//Memorization of a reference spectrum start adress

	for(k=0; k<Nmoy; k++)
	{	/////// 1st Reference spectrum
		decal_Windows= k*512;
		for(i=0; i<512; i++)
		{	if(channel == 0)
			{	G14s_AM2P_Signal[i]=p_Input[(i*2)+128+(decal_Windows*2)];}	//Storage of passive part into its variable
			else if(channel == 1)
			{	G14s_AM2P_Signal[i]=p_Input[(i*2)+1+128+(decal_Windows*2)];}	//Storage of passive part into its variable
			else if(channel == -1)
			{	G14s_AM2P_Signal[i]=p_Input[i+128+decal_Windows];}	//Storage of passive part into its variable
		}
		Init_Boucle(channel,0,0,k,boucle);
		p_signal=&G14s_AM2P_Signal[0];				//Reset of passive part at its start
		ApoWin_Signal(p_signal,boucle,1);					//Apodization of passive part
		p_signal=&G14s_AM2P_Signal[0];				//Reset of passive part at its start
		Apply_FFT(p_signal,Nmoy,p_amplitude);		//FFT processing of the passive part
	}
	for(i=0; i<90; i++)
	{	if(channel != -1)
		{p_Output[i+3+(channel*90)]=Amplitude[i];}				//Storage the result into the output variable
		else
		{	p_Output[i+3]=Amplitude[i];}				//Storage the result into the output variable
	}	
}

//Function conditiones and launches the DFT
static void 
//RETURN : 1 if the function is successfull
Apply_DFT(		//INPUT : Signal up on which make the DFT processing
					short* signal,	
					//INPUT : Number of the part 
					short *boucle,	
					//OUTPUT : Power output
					double* Valeur_Base, 
					//OUTPUT : Result storage
					 struct AM2P_result_DFT * p_Resultat_Temp
					)

{
	struct AM2P_result_DFT * p_DFT_Result_Temp;
	struct AM2P_result_DFT DFT_Result_Temp;
	DFT_Result_Temp.amplitude = 0;
	DFT_Result_Temp.phase = 0;
	p_DFT_Result_Temp = &DFT_Result_Temp;
	char decal;
	if(boucle[3] == 512)
	{	decal = 1;}
	else if(boucle[3] == 256)
	{	decal = 2;}
	else
	{	decal = 4;}
	ApoWin_Signal(signal,boucle,decal);
	Traitement_DFT(signal,p_DFT_Result_Temp,boucle,Valeur_Base);
	p_Resultat_Temp->amplitude = p_Resultat_Temp->amplitude + DFT_Result_Temp.amplitude;
	p_Resultat_Temp->phase = p_Resultat_Temp->phase + DFT_Result_Temp.phase;
}

void DFT_Result(struct AM2P_result_DFT * p_Resultat_Temp, unsigned short *Result)
{
	if(p_Resultat_Temp->amplitude>1)			//if power is positive
	{	Result[0]=(unsigned short)((49.98779296875)*((10*(double)app_math_log10((double)(((p_Resultat_Temp->amplitude)*2))))+5.3517)+0.5);}	// (Nbit_cod馥/Amax)*((10log(Aｲ*2)+5.3517))+0.5
	else		//If power is negative (log(-X)=0.00000xxx
	{	Result[0]=0;}			//So Power output = 0
	if(p_Resultat_Temp->phase < 0)
	{	p_Resultat_Temp->phase = p_Resultat_Temp->phase + 6.283185307179586476925286766559;}
	else if(p_Resultat_Temp->phase > 6.283185307179586476925286766559)
	{	p_Resultat_Temp->phase = p_Resultat_Temp->phase - 6.283185307179586476925286766559;}
	p_Resultat_Temp->phase=((p_Resultat_Temp->phase)*57.295779513082320876798154814105);		//Normalization of phase in degree
	Result[1]=(unsigned short)((p_Resultat_Temp->phase)*11.375+0.5);		//Normalization of phase on 12bits (2^12 = 4096)
	
}


short Index_Calcul (short j, short *boucle)
{
	short Valeur_retour = j+3;
	if(boucle[8] == -1)
	{	Valeur_retour = j+93;}
	else if(boucle[8] ==0)
	{	if(boucle[0] == 2)
		{	Valeur_retour = j+183;}}
	else if(boucle[8] == 1)
	{	if(boucle[0] == 2)
		{	Valeur_retour = j+363;}
		else if(boucle[0] == 4)
		{	Valeur_retour = j+63;}
		else if(boucle[0] == 1)
		{	Valeur_retour = j+17;}
	}
	else if(boucle[8] == 2)
	{	Valeur_retour = j+31;}
	return Valeur_retour;
}

int Calcul_Table_Freq (short *boucle)
{
	int result=0;
	short Coef_FreqB1 = 0;
	short Coef_FreqB2 = 0;
	short Coef_FreqB3 = 0;
	if(boucle[8] ==-1)
	{	Coef_FreqB1 = 2176;
		Coef_FreqB2 = 1152;
		Coef_FreqB3 = 640;}
	else
	{	if(boucle[0] == 2)
		{	Coef_FreqB1 = 1152;
			Coef_FreqB2 = 640;
			Coef_FreqB3 = 384;}
		else if(boucle[0] == 1)
		{	Coef_FreqB1 = 512;
			Coef_FreqB2 = 256;
			Coef_FreqB3 = 128;
		}
	}
	if(boucle[7] == 0)
	{
		if(boucle[1]<30)
		{	result = (boucle[1]+1)*Coef_FreqB1+128;}
		else if(boucle[1]<60)
		{	result = (31*Coef_FreqB1)+((boucle[1]-30)*Coef_FreqB2) + 128;}
		else
		{	result = (31*Coef_FreqB1)+(30*Coef_FreqB2)+((boucle[1]-60)*Coef_FreqB3)+128;}
		return result;
	}
	else if(boucle[7] == 2 ||boucle[7] == 3)
	{
		if(boucle[1]<4)
		{	result = boucle[1]*Coef_FreqB1;}
		else if(boucle[1]<6)
		{	result = (4*Coef_FreqB1)+((boucle[1]-4)*Coef_FreqB2);}
		else
		{	result = 2560;}
		return result;
	}
	else 
	{	return -1;}
}

void Init_Traitement(short *p_Input, short *boucle, struct AM2P_result_DFT * p_Resultat_Temp, double *Valeur_Base)
{
	
	short *p_signal;		//Variable of the signal to process
	//short G14s_AM2P_Signal[512];
	short i=0;
	int Decal_Part =0;
	int Coef_Table = 0;
	short decal_Windows=0;
	p_signal=&G14s_AM2P_Signal[0];		//Memorization of a reference spectrum start adress
	if (boucle[2] == 1) 
	{	Decal_Part =0;}
	else
	{	if(boucle[0] == 4)
		{	if(boucle[7]==0)
			{	Decal_Part =67456;}
			else
			{	Decal_Part = 40960;}
		}
		else
		{	Decal_Part =69120;}
	}
	Coef_Table = Calcul_Table_Freq(boucle);
	for(i=0; i<512; i++)
	{	G14s_AM2P_Signal[i]=0;}	//Initialization of reference spectrum
	decal_Windows = boucle[4] * boucle[3];
	for(i=0;i<boucle[3];i++)
	{	if(boucle[8] != -1)
		{	if(boucle[0] == 2)
			{	G14s_AM2P_Signal[i]=p_Input[(i*2)+boucle[8]+(Coef_Table*2)-Decal_Part+(decal_Windows*2)];}
			else if(boucle[0] == 4)
			{	G14s_AM2P_Signal[i]=p_Input[(i*2)+boucle[8]+(G14i_AM2P_Table_N4_CAL[boucle[1]]*2)-Decal_Part+(decal_Windows*2)];}
			else if(boucle[0] == 1)
			{	G14s_AM2P_Signal[i]=p_Input[i+ Coef_Table + boucle[8]*2688];}
		}
		else
		{	G14s_AM2P_Signal[i]=p_Input[i+Coef_Table-Decal_Part+decal_Windows];}
	}
	p_signal=&G14s_AM2P_Signal[0];
	Apply_DFT(p_signal,boucle,Valeur_Base,p_Resultat_Temp);			
}

//Function to make the DFT processing in config 2
static void Run_Traitement(	int part,					//Input : Number of part of the processing
						unsigned short *p_Output,	//Output : Result of the DFT processing
						short *p_Input,				//Input : Signal to make on the DFT processing
						short Nmoy,					//INPUT : Acquisition Number
						char Cal					//Input : Calibration or not
						)
{
	short j;					//Loop variable
	short boucle[9];
	short k=0;					//Loop variable
	short Chl =0;
	short Channel=0;
	short indice_Result=0;
	short Chl_loop=0;
	unsigned short Result[2];
	struct AM2P_result_DFT *p_Resultat_Temp;	//Statement of variable of type struct define in .h
	struct AM2P_result_DFT Resultat_Temp;
	double Valeur_Base[1];
	short Start_Freq=0;
	short End_Freq=0;
	p_Resultat_Temp = &Resultat_Temp;
	if(part == 1)
	{	Start_Freq = 0;
		if(Nmoy == 4)
		{	if(Cal ==0)
			{	Chl_loop =1;
				End_Freq = 30;}
			else if(Cal ==1)
			{	Chl_loop =2;
				End_Freq = 10;}
		}
		else if(Nmoy == 2)
		{	Chl_loop =2;
			End_Freq = 29;}
		else if(Nmoy == 1)
		{	End_Freq = 7;
			if(Cal == 2)
			{	Chl_loop = 2;}
			else
			{	Chl_loop = 3;}	
		}
	}
	else 
	{	if(Nmoy == 4)
		{	if(Cal ==0)
			{	Chl_loop =1;
				Start_Freq = 30;
				End_Freq = 90;}
			else if(Cal ==1)
			{	Chl_loop =2;
				Start_Freq = 10;
				End_Freq = 30;}
		}
		else if(Nmoy == 2)
		{	Chl_loop =2;
			Start_Freq = 29;
			End_Freq = 90;}
	}

	for(Chl=0; Chl<Chl_loop; Chl++)
	{	
		if(Nmoy==4 && Cal ==0)
		{	Channel =-1;}
		else
		{	Channel = Chl;}
		if(part ==1 && Cal == 0)
		{	Init_FFT(Nmoy,Channel,p_Input,p_Output);}
		for(j=Start_Freq; j<End_Freq; j++)
		{	Resultat_Temp.amplitude = 0;
			Resultat_Temp.phase =0;
			for(k=0; k<Nmoy; k++)
			{
				Init_Boucle(Channel,j,Cal,k,boucle);
				Init_Traitement(p_Input,boucle,p_Resultat_Temp,Valeur_Base);
			}
		
			DFT_Result(p_Resultat_Temp,Result);
			indice_Result = Index_Calcul(j,boucle);
			p_Output[indice_Result]=Result[0];				//Storage the power result in the output variable
			if(Cal == 0)
			{	p_Output[indice_Result+90]=Result[1];}				//Storage the phase result in the output variable
			else if(Cal == 1)
			{	p_Output[indice_Result+30]=Result[1];}				//Storage the phase result in the output variable
			else if(Cal == 2 ||Cal == 3)
			{	p_Output[indice_Result+7]=Result[1];}				//Storage the phase result in the output variable
		}
	}
}

//Function to concatenate Output data result
static void Concatenate (		unsigned short * Output,	//INPUT/OUTPUT : Result of the DFT processing
						short  end_array					//Input : Number of part of the processing
						)
{
	short i=0, q=0;		//Loop variable
	short indice_1=0;		//Index nｰ1 of deconcatenate data 
	short indice_2=0;		//Index nｰ2 of deconcatenate data 
	short indice_3=0;		//Index nｰ3 of deconcatenate data 
	for(i=0; i<end_array; i=i+3)
	{	
		indice_1=i+3;			//Calcul the index of data output
		indice_2=i+3+q;		//Calcul the index of the first deconcatenate data input for this output
		indice_3=i+4+q;		//Calcul the index of the second deconcatenate data input for this output
		Output[indice_1] = (Output[indice_2] << 4) + ((Output[indice_3] >> 8) & 0x000F);//Shift 4 bits the actual result and add the MSB of the next result
		indice_1=i+4;		//Calcul the index of data output
		indice_2=i+4+q;		//Calcul the index of the first deconcatenate data input for this output
		indice_3=i+5+q;		//Calcul the index of the second deconcatenate data input for this output
		Output[indice_1] = (Output[indice_2] << 8) + ((Output[indice_3] >> 4) & 0x00FF);	//Shift 8 bits the actual result and shift of 4  the MSB of the next result
		indice_1=i+5;		//Calcul the index of data output
		indice_2=i+5+q;		//Calcul the index of the first deconcatenate data input for this output
		indice_3=i+6+q;		//Calcul the index of the second deconcatenate data input for this output
		Output[indice_1] = (Output[indice_2] << 12) + ((Output[indice_3]) & 0x0FFF);	//Shift 12 bits the LSB of the actual result and add the next result
		q++;							//Delete the 16 bits lost by concenating				
	}
}


//Function does the process on the input signal (MAIN FUNCTION)
short
//RETURN : Number of bits in the output array
app14_AM2P_Traitement_AM2P( //INPUT : Configuration of AM2P
		unsigned short TC_Word_tmp,
		//INPUT : Give the number of point shifted to do
		int Ui_dataTI,
		//INPUT : Signal to process
		short * p_Input,
		//INPUT : Band number
		int part,
		//OUTPUT : Result signal
		unsigned short *p_Output) {
	union Config_AM2P TC_Word; //Structure to know the AM2P configuration
	TC_Word.Conf = TC_Word_tmp; //Copy the variable into the structure of AM2P configuration
	short nbBytes = -1; // Number of ouput bytes (0 if the ouput is uncomplete, -1 on error)
	short Nmoy = 0;
	char cal = 0;
	short end_array = 0;
	if(TC_Word.Conf_AM2P.Cal_Mode==2 || TC_Word.Conf_AM2P.Cal_Mode==6 || TC_Word.Conf_AM2P.Cal_Mode==4)
	{	if (part == 1)
		{	Nmoy = 1;
			if(TC_Word.Conf_AM2P.Cal_Mode == 4)
			{	cal = 3;
				end_array = 31;//28→31?
				nbBytes = 69;
			}
			else
			{	cal = 2;
				end_array = 21;
				nbBytes = 48; // Number of ouput bytes
			}
		}
	}
	else if(TC_Word.Conf_AM2P.Cal_Mode==3 ||TC_Word.Conf_AM2P.Cal_Mode==7)
	{	if (part == 1 || part == 2) 
		{	Nmoy = 4;
			cal = 1;
			end_array = 90;
			nbBytes = 186; // Number of ouput bytes
		}
	}
	else
	{	if(TC_Word.Conf_AM2P.Operating_Mode==4 || TC_Word.Conf_AM2P.Operating_Mode ==6)
		{	if (part == 1 || part == 2)
			{	Nmoy = 2;
				cal = 0;
				end_array = 405;//403→405?
				nbBytes = 816; // Number of ouput bytes
			}
		}
		else if(TC_Word.Conf_AM2P.Operating_Mode < 4 )//||TC_Word.Conf_AM2P.Operating_Mode==1 ||TC_Word.Conf_AM2P.Operating_Mode==2 ||TC_Word.Conf_AM2P.Operating_Mode==3)
		{	if (part == 1 || part == 2)
			{	Nmoy = 4;
				cal = 0;
				end_array = 202;
				nbBytes = 411; // Number of ouput bytes
				}
		}
	}

	if (Nmoy != 0) {
		Run_Traitement(part, p_Output, p_Input, Nmoy, cal); //Launch the processing on the signal for the config 1
		if (part == 2 || cal == 2 ||cal == 3) //If it's the last part
		{
			TC_Word.Conf_AM2P.TM_Rate = 2; //TM Rate = High Rate (CAL_Mode)
			p_Output[0] = TC_Word.Conf; //Copy the configuration of AM2P into the output
			p_Output[1] = (short) ((Ui_dataTI >> 16) & 0xFFFF); //Copy the TI(MSB) into the output data
			p_Output[2] = (short) (Ui_dataTI & 0xFFFF); //Copy the TI(LSB) into the output data
			Concatenate(p_Output, end_array); //Concatenate the output data
		} else {
			nbBytes = 0; // ouput is not complete
		}
	}
	return nbBytes;
}

//========================================================================
//	AM2P
//		Test (2011.03.02)
//		コマンド対応 (2011.04.19)
//========================================================================
void app14_AM2P( unsigned int ui_sunpulseCnt )
{
	// === BLOCK read ===
	int	       	i_bufId;       		// Buffer ID
	int	       	i_numNewBlock;		// Number of blocks with new data
	unsigned int	ui_timeData;	// Processing data TI
	M_T_BLOCK_INFO	t_blockInfo;
	/* **** M_T_BLOCK_INFO メンバ ******
	unsigned int	ui_createTime	Block 生成開始衛星時刻 (LSB=1.953ms)
	int	  	     	i_replyNum     	Block 登録 read-reply 個数
	char	       	c_invalidFlg	Block 内データ異常の有無 (0:異常なし, 1:異常あり)
	unsinged int	ui_blockIndex	LongBuffer/ShortBuffer 内 Block 特定番号
	******************************** */
	
	// == Time Index ==
	M_T_TIME_INFO	t_timeInfo;
	
	// === Data ===
	int	     	i_dataId;	// EWOE-Header :: DataID	(0:MS, 1:SW)
	int	       	i_dataSize;	// EWOE-Header :: Data Size
	
	unsigned int    ui_AM2P_TI0;    //各partでの統一TI
	unsigned int	ui_AM2P_TI;		// EWOE-Header :: TI
	unsigned int	ui_AM2P_TI1;	// TI
	unsigned char	uc_AM2P_TI2;	// EWO count
	
	// ミッションパケット付帯データ
	static EWO_PACKET_INFO	s_packet_info_AM2P;
	
	// ミッションパケット用バッファのライトポインタ
	static unsigned char	*uc_wp_AM2P;
	// AM2P
	short	       	*s_wpAM2P_signal;
	int	       		i_AM2P_signal_size = 0;
	int	       		i_AM2P_packetSize[3];
	int	       		i_AM2P_nTraitment  = 0;
	int             i_AM2P_ch = 0;
	int             i_AM2P_shiftpoint;
	int             i_AM2P_devide;
	int				i_AM2P_mode = 3;// 暫定値
	unsigned int	ui_AM2P_current_time;
	
	// === General ===
	int		i, j, k,l,m;
	int		i_flag;
	int     i_flag2;
	int     i_old_j;
	
	/*-------------------------*/
	long long 		ll_tempx,ll_tempy;
	short			*s_wpAM2Py_signal;
	int				i_shiftcount = 0;
	int				i_num=0;
	int				i_vlf=0;
	int				i_counter = 0;
	/*-------------------------*/
	
	// For periodical execution
//	G14ui_AM2P_counter++;
//	if( G14uc_AM2P_interval != 0x00 && G14uc_AM2P_interval != 0xff && G14uc_AM2P_stage == 0 ){
	if( G14ui_AM2P_interval != 0x00 && G14ui_AM2P_interval != 0xffff && G14uc_AM2P_stage == 0 ){
		G14ui_AM2P_counter++;
//		if( G14ui_AM2P_counter > (unsigned int)G14uc_AM2P_interval ){
		if( G14ui_AM2P_counter > G14ui_AM2P_interval ){
			G14uc_AM2P_stage = 2;
			G14ui_AM2P_counter = 0;
			G2uc_AM2P_stage_cnt = 0;
		}
	}
	if( G14ui_AM2P_counter >= 0xffffff ) G14ui_AM2P_counter = 0;
	
	// Abort when EWO CAL is running
//	if( G14uc_AM2P_stage == 0 || G14uc_WFC_CAL_stage != 0 ) return;
// 条件緩和（Y. Kasahara, 2013.11.09)
	if( G14uc_WFC_CAL_stage != 0 ) return;
	
	// Appended by T.Imachi, 2011.10.09
	ui_AM2P_TI0 = 0;
	i_dataId = 0;
	s_wpAM2Py_signal = &G14s_AM2P_signal[0];
	
	// -- Set at the buffer with the oldest non-processed block --
	i_bufId = app_CheckNonProcBlock(
			Gd_N_EWO, Gd_D_AM2P,	       	// [INPUT] Node/Data ID
			Gui_TLMm_blockTime[Gd_N_AM2P],	// [INPUT] TI of Processed Block
			&ui_timeData		     	  	// [OUTPUT] TI of Newest Block in the selected buffer
			);
	// i_bufId が no data の場合，処理はここで終了．
	if( i_bufId < 0 ) return;

	// -- Set at the oldest non-processed block --
	i_numNewBlock = app_SetNonProcBlock(
			Gd_N_EWO, Gd_D_AM2P,	       	// [INPUT] Node/Data ID
			i_bufId,		     		  	// [INPUT] Buffer ID
			Gui_TLMm_blockTime[Gd_N_AM2P],	// [INPUT] IT of Processed Block
			&t_blockInfo		      	 	// [OUTPUT] Block info
			);
	if( i_numNewBlock < 1 ){
		Gui_TLMm_blockTime[Gd_N_AM2P] = ui_timeData;
		return;
	}
	
	/*******時間計測用:TMP-START**********/
	//dpu_getTime(&t_timeInfo); Gui_dummy = (t_timeInfo.ui_currentTime);	// unsigned int ui_currentTime;  衛星時刻(1LSB=1.953ms)
	/*******TIME management:TMP-END**********/

	// -- Calculate required packet size --
	i_AM2P_shiftpoint = (0x000f & (int)G14uc_AM2P_shift);

	if((0x0030& G14s_AM2P_TCWord) == 0){
/*	  if((0xE000 & G14s_AM2P_TCWord) == 0 || (0xE000 & G14s_AM2P_TCWord) == 8192 || 
	     (0xE000 & G14s_AM2P_TCWord) == 16384 || (0xE000 &  G14s_AM2P_TCWord) == 24576)//config1 */
	  if((0xE000 & G14s_AM2P_TCWord) == 8192 || (0xE000 &  G14s_AM2P_TCWord) == 24576) //config1, Current Probe
	    i_AM2P_devide = 4;
	  else if((0xE000 & G14s_AM2P_TCWord) == 0 || (0xE000 & G14s_AM2P_TCWord) == 16384){ //config1, MEFISTO differential
	    i_AM2P_devide = 4;
	    i_AM2P_ch     = 2;
	  }  
	  else {										//config2
	    i_AM2P_devide = 2;
	    i_AM2P_shiftpoint *=2;
	  }
	}
	else {//cal-mode
		if((0x0030 & G14s_AM2P_TCWord) == 0x0030){	//cal HF
			i_AM2P_devide = 2;
			i_AM2P_shiftpoint *= 2;
		}
		else{										//cal VLF
			i_AM2P_devide = 4;
			i_vlf = 1;
			i_num -= i_AM2P_shiftpoint;
		}
	}
	 
	if(i_vlf == 0)
  		i_AM2P_signal_size = -i_AM2P_shiftpoint;
  	i_old_j = 0;
  	
	for( l=1; l<=2; l++ ){//part1~part2
		i_AM2P_packetSize[l] = app14_AM2P_Packet_Size( l, G14s_AM2P_TCWord );

	  	if(i_AM2P_packetSize[l] == 0) break;
	 	 i_flag2 = 0;
	 	 i = 0;
	  
		  // -- Fetch AM2P data --//
		  if(i_vlf == 0){
		  	  if(l == 1)
		  	  	s_wpAM2P_signal = &G14s_AM2P_signal[0];
		  	  else
	  			s_wpAM2P_signal = &G14s_AM2P_signal[i_AM2P_shiftpoint];
	  			
	  		if(i_flag2 == 0 && l >= 2){
				for(k = 0;k < i_AM2P_signal_size;k ++)
					*s_wpAM2P_signal++ = G14s_AM2P_signal[i_AM2P_packetSize[l-1] + k + i_AM2P_shiftpoint];
			}
	  			
	  	  }else{
	  	  	  s_wpAM2P_signal = &G14s_AM2P_signal[0];
	  	  	  s_wpAM2Py_signal = &G14s_AM2P_signal[20000];
	  	  }
	  
	  	while(i_AM2P_signal_size < i_AM2P_packetSize[l]){
	  		i++;
	  		if(i > i_numNewBlock)	break;	  	
	    	if( t_blockInfo.ui_createTime > Gui_TLMm_blockTime[Gd_N_AM2P] &&
				t_blockInfo.i_replyNum > 0 &&
				t_blockInfo.c_invalidFlg == 0){
	      		i_flag = app_GetPacket(
				     3,		       	        // [INPUT] App-No.: 3
				     Gd_N_EWO, Gd_D_AM2P,	// [INPUT] Node/Data ID
				     i_bufId,	       	    // [INPUT] Buffer ID
				     &t_blockInfo,	       	// [INPUT] Block info
				     0,		       	        // [INPUT] Packet No. (0:先頭)
				     0,		       	        // [INPUT] OFFSET 読み出し開始番地
				     32,		        	// [INPUT] 読み出しデータ長
				     G14uc_data	       	    // [OUTPUT] 読み出しデータ収納バッファ
				     );
				     
				for( j=i_old_j; j < t_blockInfo.i_replyNum; j++ ){	
					if( i_AM2P_signal_size < i_AM2P_packetSize[l]){
				// 情報取得
						i_flag = app_GetPacket(
								3,		       			// [INPUT] App-No.: 3
								Gd_N_EWO, Gd_D_AM2P,	// [INPUT] Node/Data ID
								i_bufId,	    	   	// [INPUT] Buffer ID
								&t_blockInfo,	       	// [INPUT] Block info
								j,		      		 	// [INPUT] Packet No. (0:先頭)
								0,		      		 	// [INPUT] OFFSET 読み出し開始番地
								136,		   	    	// [INPUT] 読み出しデータ長
								G14uc_data	    	   	// [OUTPUT] 読み出しデータ収納バッファ
								);
						if( ( G14uc_data[22] != 0x10 && G14uc_data[22] != 0x11 ) || G14uc_data[23] != 0 ) continue;
					// トランスミッショントリガ送出前後のパケットを除去
					// データ途中10バイトが全て 0xff でないなら，パケットを処理
						if(		G14uc_data[126] != 0xff ||
								G14uc_data[127] != 0xff ||
								G14uc_data[128] != 0xff ||
								G14uc_data[129] != 0xff ||
								G14uc_data[130] != 0xff ||
								G14uc_data[131] != 0xff ||
								G14uc_data[132] != 0xff ||
								G14uc_data[133] != 0xff ||
								G14uc_data[134] != 0xff ||
								G14uc_data[135] != 0xff
							){				
						// 実データ読み出し
					 		 i_flag = app_GetPacket(
									 3,		     		  	// [INPUT] App-No.: 3
									 Gd_N_EWO, Gd_D_AM2P,	// [INPUT] Node/Data ID
									 i_bufId,	     	  	// [INPUT] Buffer ID
									 &t_blockInfo,	       	// [INPUT] Block info
									 j,		       			// [INPUT] Packet No. (0:先頭)
									 0,						// [INPUT] OFFSET 読み出し開始番地
									 2080,		    	   	// [INPUT] 読み出しデータ長
									 G14uc_data	       		// [OUTPUT] 読み出しデータ収納バッファ
									 );
										
					  		i_dataId = G14uc_data[22];
						  	i_dataSize = ( G14uc_data[24] << 8 ) + G14uc_data[25];
					 		ui_AM2P_TI1 = 0;
							for( k=16; k<=19; k++ ){
								ui_AM2P_TI1 <<= 8;
								ui_AM2P_TI1 += G14uc_data[k];
							}
							uc_AM2P_TI2 = ( G14uc_data[20] << 4 ) + ( G14uc_data[21] >> 4 );
							ui_AM2P_TI = ui_AM2P_TI1 + uc_AM2P_TI2;
		    
		    				if(l == 1 && j == 0)
		    					ui_AM2P_TI0 = ui_AM2P_TI;
							
							/*    				
							if(i_flag2 == 0 && l >= 2){
								for(k = 0;k < i_AM2P_signal_size;k ++)
					    			*s_wpAM2P_signal++ = G14s_AM2P_signal[i_AM2P_packetSize[l-1] + k + i_AM2P_shiftpoint*2];
							}
							*/
							if(i_vlf == 0){
								for( k=26; k < i_dataSize + 26; k+=i_AM2P_devide ){   /* conf1で2byteずらす */
									*s_wpAM2P_signal++ = (short)( ( (unsigned short)G14uc_data[k+i_AM2P_ch] << 8 ) + G14uc_data[k+i_AM2P_ch + 1] );
									if(i_flag2 == 0)
										i_flag2 = 1;
								}
								i_AM2P_signal_size += i_dataSize/i_AM2P_devide;
							}else{//cal VLF
								for(k = 26;k < i_dataSize + 26;k += i_AM2P_devide){
									if(i_AM2P_shiftpoint > i_shiftcount){
										i_shiftcount ++;
									}
									else{
										*s_wpAM2P_signal++ = (short)(((unsigned short)G14uc_data[k] << 8) + G14uc_data[k + 1]);
										*s_wpAM2Py_signal ++ = (short)(((unsigned short)G14uc_data[k + 2] << 8) + G14uc_data[k + 3]);
									}
								}
								i_num += i_dataSize/i_AM2P_devide;
							}
							
							if(i_vlf == 1){//９６点平均化処理
								if(i_num >= 1536){
									s_wpAM2P_signal = &G14s_AM2P_signal[i_counter*16];
									s_wpAM2Py_signal = &G14s_AM2P_signal[i_counter*16 + 20000];
									for(k = 0;k < 1536;k += 96){
										ll_tempx = 0;
										ll_tempy = 0;
										for(m = k;m < k+96;m ++){
											ll_tempx += G14s_AM2P_signal[m + i_counter*16];
											ll_tempy += G14s_AM2P_signal[m + i_counter*16 + 20000];
										}
										
										*s_wpAM2P_signal++ = (short)(ll_tempx/96);
										*s_wpAM2Py_signal++ = (short)(ll_tempy/96);
									}
										
									if(i_AM2P_shiftpoint > 0)//shift分ズレで余った部分を前に寄せる
										for(m = 0;m < (512 - i_AM2P_shiftpoint);m ++){
											*s_wpAM2P_signal++ = G14s_AM2P_signal[m + 1536 + i_counter*16];
											*s_wpAM2Py_signal++ = G14s_AM2P_signal[m + 1536 + i_counter*16 + 20000];
										}							
									i_counter ++;		
									i_num -= 1536;
									i_AM2P_signal_size += (16*2);
								}
							}
						} // end of if
					} // end of if 	
					else break;
				}  // end of for (Packet : for checking 0xff)
				i_old_j = j;
			}  // end of if
	
		/*i_flag = app_NextBlock(
				Gd_N_EWO, Gd_D_AM2P,	// [INPUT] Node/Data ID
				i_bufId,				// [INPUT] Buffer ID
				&t_blockInfo			// [OUTPUT] Block info
				);
		if( i_flag ) break;*/
		} // end of while (Block)
		// -- End of fetching AM2P data --

		i_AM2P_signal_size -= i_AM2P_packetSize[l];
		
		if(i_vlf == 1){
			s_wpAM2Py_signal = &G14s_AM2P_signal[2688];
			for(m = 0;m < 2688;m ++)
				*s_wpAM2Py_signal++ = G14s_AM2P_signal[20000 + m];
		}
		
		// -- Traitment --
		dpu_getTime(&t_timeInfo);
		ui_AM2P_current_time = (t_timeInfo.ui_currentTime);	
		s_packet_info_AM2P.uc_bitrate = BITRATE_L;
		i_AM2P_mode = BITRATE_L;

		s_packet_info_AM2P.uc_interval = 0;    	// 【暫定】0: 1sec (bit-L 時)
		s_packet_info_AM2P.uc_ofa_channel = 0; 	// 【暫定】1: E1
		s_packet_info_AM2P.i_n_spec_data = 0;	// スペクトルデータ点数
			
		/* **************************************************
		// テスト用波形ダンプ
		// 実機用にコメントアウト
		
		//-------------- Waveform full dump
		s_packet_info_AM2P.uc_data_spec = 0x12;
		j = 0;
		while( j < i_AM2P_packetSize[l]){
			uc_wp_AM2P = app14_EWO_packet_init(
					G14uc_AM2P_waveform,	       	// パケットバッファのポインタ
					ui_AM2P_TI0,					// データ先頭の TI
					0x00,			       		// データ先頭の EWO カウンタ
					&s_packet_info_AM2P
					);

			k = 0;
			while( k < 1024 && j < i_AM2P_packetSize[l] ){
				*uc_wp_AM2P++ = (unsigned char)( ( G14s_AM2P_signal[j+i_AM2P_shiftpoint*(1-i_vlf)] & 0xff00 ) >> 8 );
				*uc_wp_AM2P++ = (unsigned char)( ( G14s_AM2P_signal[j+i_AM2P_shiftpoint*(1-i_vlf)] & 0x00ff ) );
				j++;
				k++;
			}
		
			app14_AM2P_putTLM(
				i_dataId, 	       		// [INPUT] Packet-ID	0x11:HF, 0x10:LF
				ui_AM2P_TI0,	    	   	// [INPUT] TI
				i_AM2P_mode,		      		 	// [INPUT] L-mode data
				G14uc_AM2P_waveform, 	// [INPUT] Packet data
				k * 2 + 12	      	 	// [INPUT] Packet size
				);
				
		}
		//------------full dump ここまで
		
		// テスト用波形ダンプ
		// 実機用コメントアウト ここまで
		************************************************** */
		
		// AM2P Process
	
		i_AM2P_nTraitment = app14_AM2P_Traitement_AM2P(
				G14s_AM2P_TCWord,
				ui_AM2P_TI0,
				&G14s_AM2P_signal[i_AM2P_shiftpoint*(1-i_vlf)],
				l,
				&G14us_AM2P_Traitment_result[0]
				);

/*
		s_packet_info_AM2P.uc_data_spec = 0x11;	// 【暫定】0x10:LF, 0x11:HF
		s_packet_info_AM2P.uc_ofa_channel = l & 0xff;
		uc_wp_AM2P = app14_EWO_packet_init(
				G14uc_AM2P,		       	// パケットバッファのポインタ
				ui_AM2P_TI0,		       	// データ先頭の TI
				0x00,			       	// データ先頭の EWO カウンタ
				&s_packet_info_AM2P
				);
*/
		G14uc_AM2P[0] = ( ( Guc_AM2P_WPTP_gain & 0x0f ) << 4 ) | ( Guc_AM2P_EWOE_gain & 0x0f );
		G14uc_AM2P[5] = 0x11;			// 2012.12.19 fix Imachi
		uc_wp_AM2P = &G14uc_AM2P[6];

		for( j=0; j<i_AM2P_nTraitment; j++ ){
			*uc_wp_AM2P++ = (unsigned char)( ( G14us_AM2P_Traitment_result[j] & 0xff00 ) >> 8 );
			*uc_wp_AM2P++ = (unsigned char)( ( G14us_AM2P_Traitment_result[j] & 0x00ff ) );
		}

//			Modified by Y. Kasahara 2013.10.23)
/*		app14_AM2P_putTLM(
				i_dataId, 						// [INPUT] Packet-ID	0x11:HF, 0x10:LF
				ui_AM2P_TI0,						// [INPUT] TI
				i_AM2P_mode,			      			 	// [INPUT] M-mode data
				G14uc_AM2P, 						// [INPUT] Packet data
	    		i_AM2P_nTraitment + 6  	// [INPUT] Packet size 		// 2012.12.19 fix Imachi
			); */
	}//End of part loop
	// app14_AM2P_putTLM was moved outside of the loop. Modified by Y. Kasahara 2013.10.23
	if(i_AM2P_nTraitment != 0 && ui_AM2P_TI0 != 0){
		app14_AM2P_putTLM(
			i_dataId, 						// [INPUT] Packet-ID	0x11:HF, 0x10:LF
			ui_AM2P_TI0,						// [INPUT] TI
			i_AM2P_mode,			      			 	// [INPUT] M-mode data
			G14uc_AM2P, 						// [INPUT] Packet data
    		i_AM2P_nTraitment + 6  	// [INPUT] Packet size 		// 2012.12.19 fix Imachi
				);
//Added by Y. Kasahara (まっとうなTLMが出たら処理終了とする）2013.11.8
		G14uc_AM2P_finished = 1;	// AM2P 処理終了フラグを立てる（ステージは操作しない）
	
		if( Guc_AM2P_cnt == 0xff ) 	Guc_AM2P_cnt = 0;
		else 						Guc_AM2P_cnt++;
	}
	
//	i_bufId = app_CheckNonProcBlock(
//			Gd_N_EWO, Gd_D_AM2P,	       	// [INPUT] Node/Data ID
//			Gui_TLMm_blockTime[Gd_N_AM2P],	// [INPUT] TI of Processed Block
//			&ui_timeData		     	  	// [OUTPUT] TI of Newest Block in the selected buffer
//			);
	
	//Gui_TLMm_blockTime[Gd_N_AM2P] = ui_timeData;
	Gui_TLMm_blockTime[Gd_N_AM2P] = t_blockInfo.ui_createTime; 
	/*******TIME management:TMP**********/
	//dpu_getTime(&t_timeInfo); Gui_dummy = t_timeInfo.ui_currentTime - Gui_dummy;
	/*******TIME management:TMP**********/
	
//	G14uc_AM2P_stage = 0;
//	G14uc_AM2P_finished = 1;	// AM2P 処理終了フラグを立てる（ステージは操作しない）
	
//	if( Guc_AM2P_cnt == 0xff ) 	Guc_AM2P_cnt = 0;
//	else 						Guc_AM2P_cnt++;
		
	return;
}


// ===========================================================================
// app14_AM2P_putTLM
//		[INPUT]		int				i_pacId					Packet ID (user definition)
//					unsigned int	ui_dataTi				Header: TI
//					int				i_drId,					DR-ID = L / M / H0 (for CAT-ID) 	[0:HK 1:OS 2:L 3:M 4-9:H(0-5)]
//					unsigned char	uc_data[]				Data
//					int				i_dataSize				Data Size
// ===========================================================================
void	app14_AM2P_putTLM(	int				i_pacId,				// [INPUT]		Packet ID
							unsigned int	ui_dataTi,				// [INPUT]		TI
							int				i_drId,					// [INPUT]		DR-ID       L/M/H0	(for CAT-ID) [0:HK 1:OS 2:L 3:M 4-9:H(0-5)]
							unsigned char	uc_data[],				// [INPUT]		Data
							int				i_dataSize)				// [INPUT]		Data Size (Byte)
{
	int				i_flag;

	// **** Non-COMP or JPEG-REV ****
	if ( Guc_TLMm_cmp[Gd_N_AM2P] )  Guc_TLMm_cmp[Gd_N_AM2P] = 1;
	i_flag = app_PutTlm( 	14,						// [INPUT]  APP-NUM = 14
							Gd_N_EWO,				// [INPUT]	Node-ID
							Gd_D_AM2P,				// [INPUT]	Data-ID
							Gd_N_AM2P,				// [INPUT]	Header-ID (for PID-ID, CAT-ID setting)
							i_drId,					// [INPUT]	DR-ID     (for CAT-ID) [0:HK 1:OS 2:L 3:M 4-9:H(0-5)]
							Guc_TLMm_cmp[Gd_N_AM2P], // [INPUT]	Comp mode			   [0:non 1:JPEG-rev 2:Math 3-7:After-User-COMP]
							i_pacId, 				// [INPUT]	Packet ID 			<<<< USER-DEF:       0x00-      0xFF >>>>
							ui_dataTi, 				// [INPUT]	Data TI				<<<< USER-DEF: 0x00000000-0xFFFFFFFF >>>>
							Guc_cmdDummy, 			// [INPUT]	Comment				***** CHANGED by YK 110723 *******
							0,				 		// [INPUT]	Comment - Size 		***** CHANGED by YK 110723 *******
							uc_data, 				// [INPUT]	Data				<<<< USER-DEF: <0x1E00               >>>>
							i_dataSize); 			// [INPUT]	Data - Size			<<<< USER-DEF: <0x1E00               >>>>
	return;
}

/* ***********************************************
	unsigned char * app14_EWO_packet_init
	パケットバッファの初期化
--------------------------------------------------
[パラメータ]
	unsigned char 		*uc_packet,		// パケットバッファのポインタ
	unsigned long int 	ui_ti,			// データ先頭の TI
	unsigned char 		uc_ewo_counter,	// データ先頭の EWO カウンタ
	EWO_PACKET_INFO		*s_info,		// パケット情報
		===================================================
		unsigned char	uc_data_spec,	// データ区分
		unsigned char	uc_bitrate,		// ビットレート
		unsigned char	uc_interval,	// データ生成間隔
		unsigned char	uc_ofa_channel,	// OFA チャンネル
		int				i_n_spec_data,	// スペクトルデータ点数
		unsigned char	uc_cal_on,		// CAL ON/OFF
		unsigned char	uc_pre_gain,	// プリアンプゲイン
		unsigned char	uc_ofa_gain		// OFA/WFC ゲイン
		===================================================
--------------------------------------------------
[戻り値]
	データ書き込み位置先頭ポインタ
--------------------------------------------------
	附帯情報：
		TI：					4 Byte
		EWOカウンタ：			1 Byte
		データ区分：			1 Byte
		ビットレート：			Byte0 B7-6
		データ生成間隔：		Byte0 B5-4
		OFAチャンネル：			Byte0 B3-1
		CAL ON/OFF:				Byte0 B0
		プリアンプゲイン：		Byte1 B2
		OFA/WFCゲイン：			Byte1 B1-0
		スペクトルデータ点数：	2 Byte
*********************************************** */

/*	使用取りやめ（2012.11.26 Imachi)
#define	POS_TI	       	0
#define POS_EWO_COUNTER	4
#define POS_DATA_SPEC	5
#define POS_INFO0      	6
#define POS_INFO1      	7
#define POS_N_SPEC_DATA	8
#define POS_DATA_LENGTH	10
#define POS_DATA_TOP	12

unsigned char * app14_EWO_packet_init(
	unsigned char 		*uc_packet,
	unsigned long int 	ui_ti,
	unsigned char 		uc_ewo_counter,
	EWO_PACKET_INFO		*s_info
	)
{
	unsigned char uc_info0;
	unsigned char uc_info1;	
	
	s_info->uc_cal_on = 0;			// 【暫定】0:OFF
	s_info->uc_pre_gain = 0;		// 【暫定】
	s_info->uc_ofa_gain = 0;		// 【暫定】	

	uc_packet[ POS_TI     ] = ( ui_ti >> 24 ) & 0xff;
	uc_packet[ POS_TI + 1 ] = ( ui_ti >> 16 ) & 0xff;
	uc_packet[ POS_TI + 2 ] = ( ui_ti >> 8 ) & 0xff;
	uc_packet[ POS_TI + 3 ] = ( ui_ti      ) & 0xff;
	uc_packet[ POS_EWO_COUNTER ] = uc_ewo_counter;
	uc_packet[ POS_DATA_SPEC ] = s_info->uc_data_spec;
	
	uc_info0 = 0x00;
	uc_info0 |= ( ( s_info->uc_bitrate & 0x03 ) << 6 );
	uc_info0 |= ( ( s_info->uc_interval & 0x03 ) << 4 );
	uc_info0 |= ( ( s_info->uc_ofa_channel & 0x07 ) << 1 );
	uc_info0 |= ( ( s_info->uc_cal_on & 0x01 ) );

	uc_info1 = 0x00;
	uc_info1 |= ( ( s_info->uc_pre_gain & 0x01 ) << 2 );
	uc_info1 |= ( ( s_info->uc_ofa_gain & 0x03 ) );
		
	uc_packet[ POS_INFO0 ] = uc_info0;
	uc_packet[ POS_INFO1 ] = uc_info1;
	
	uc_packet[ POS_N_SPEC_DATA ] = (unsigned char)( s_info->i_n_spec_data >> 8 );
	uc_packet[ POS_N_SPEC_DATA+1 ] = (unsigned char)( s_info->i_n_spec_data & 0xff );
		
	return &uc_packet[ POS_DATA_TOP ];
}
// app14_EWO_packet_init() ここまで
*/