/*================================================================================
                                                                               
Module Name: test.cpp

General Description: the functions defined in this file is Doppler centroid estimation

/*============================================================================

                       SDSP Lab Confidential Proprietary
                 EE Department of Shanghai Jiaotong University
               (c) Copyright SDSP Lab 200{7}, All Rights Reserved  

 Revision History:
     Author         Modification   Tracking
    (EMail)             Date        Number        Description of Changes
 -----------------  ------------  ----------  --------------------------------
    zhenlin Wang      10/13/2008   LIBDSPFFT       Create of the module


Portability: This module is recommanded to be compiled by Intel C/C++ under platform
             of Windows or Linux/Unix. 
             This module employs OpenMP for parallel computation. 

==================================================================================*/
/*================================================================================
                                 INCLUDE FILES
================================================================================*/


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#define PI 3.141592


/*================================================================================
  This is a test for Doppler centroid estimation 
================================================================================*/


int LOG2(unsigned long num)
{
    int c=-1;
    do
    {
        num>>=1;
        c++;
    }
    while(num!=0);
    return c;
}
 
 void kkfft(double *pr, double *pi,  long n, int k, double *fr, double *fi, char l)
{ 
    int it,m,is,i,j,nv,l0;
    double p,q,s,vr,vi,poddr,poddi;

    for(it=0; it<=n-1; it++)
    {
		m=it;
		is=0;
        for(i=0; i<=k-1; i++)
        {
			j  = m/2;
			is = 2*is+(m-2*j);
			m  = j;
		}
        fr[it]=pr[is];
		fi[it]=pi[is];
    }

    pr[0]=1.0; pi[0]=0.0;
    p=6.283185306/(1.0*n);
    pr[1]=cos(p); pi[1]=-sin(p);

    if (l!=0)
		pi[1]=-pi[1];

    for(i=2; i<=n-1; i++)
    {
		p=pr[i-1]*pr[1]; q=pi[i-1]*pi[1];
        s=(pr[i-1]+pi[i-1])*(pr[1]+pi[1]);
        pr[i]=p-q; pi[i]=s-p-q;
    }

    for(it=0; it<=n-2; it=it+2)
    {
		vr=fr[it]; vi=fi[it];
        fr[it]=vr+fr[it+1]; fi[it]=vi+fi[it+1];
        fr[it+1]=vr-fr[it+1]; fi[it+1]=vi-fi[it+1];
    }

    m=n/2; nv=2;

    for (l0=k-2; l0>=0; l0--)
    {
		m=m/2; nv=2*nv;

        for(it=0; it<=(m-1)*nv; it=it+nv)
		{
            for(j=0; j<=(nv/2)-1; j++)
            {
				p=pr[m*j]*fr[it+j+nv/2];
                q=pi[m*j]*fi[it+j+nv/2];
                s=pr[m*j]+pi[m*j];
                s=s*(fr[it+j+nv/2]+fi[it+j+nv/2]);
                poddr=p-q; poddi=s-p-q;
                fr[it+j+nv/2]=fr[it+j]-poddr;
                fi[it+j+nv/2]=fi[it+j]-poddi;
                fr[it+j]=fr[it+j]+poddr;
                fi[it+j]=fi[it+j]+poddi;
            }
		}
	}

    if (l!=0)
	{
        for (i=0; i<=n-1; i++)
        {
			fr[i]=fr[i]/(1.0*n);
            fi[i]=fi[i]/(1.0*n);
        }
	}

   
    return;
}           



void estimation()
{
	double c=3.0e8;
    double lamd=0.03122;

    double B= 15.55e6;
    double fs=18.97e6;
    double T_tao=37.12e-6;

    double k=B/T_tao;
    double PRF=1674.0;
    double  f_beat=0;
    double doppler_ambiguity=0;

    unsigned long range_samples=1024;
    unsigned long azimuth_samples=512;
    double  aver_center=0.0;

    double band_look=B*2.0/3.0;
    unsigned long band_cut=(((fs-band_look)/(fs/range_samples))+1);
	band_cut=band_cut/2;
    double trans_r=0,trans_i=0;
    double max_mlbf=0;
    unsigned long start_echo=0;
    
    double *tao;
    
    double total_in_echo_r=0;
    double total_in_echo_i=0;
    
    unsigned long i=0,ia=0,ir=0,iw=0;


    double *in_echo_r,*in_echo_i;
    double *fft_in_echo_r,*fft_in_echo_i;
    double *tlook1_r,*tlook1_i,*tlook2_r,*tlook2_i,*fft_look1_r,*fft_look1_i,*fft_look2_r,*fft_look2_i;
    double **mlbf_r,**mlbf_i;
    double *tmlbf_r,*tmlbf_i,*fft_mlbf_r,*fft_mlbf_i;
    double **test_data_r;
    double **test_data_i;
    double **look1_r,**look1_i,**look2_r,**look2_i;
    double *range_filter_r,*range_filter_i,*range_f;

    FILE *fp;

	double* data_tmp = (double*)calloc(2*range_samples*azimuth_samples, sizeof(double));

    in_echo_r= (double *)calloc( (range_samples),sizeof(double ) );   
    in_echo_i= (double *)calloc(  (range_samples),sizeof(double ) );

    fft_in_echo_r=(double *)calloc( (range_samples),sizeof(double ) );
    fft_in_echo_i=(double *)calloc( (range_samples),sizeof(double ) );
    
    tlook1_r=(double*)calloc((range_samples/2-band_cut),sizeof(double));
    tlook1_i=(double*)calloc((range_samples/2-band_cut),sizeof(double));
    tlook2_r=(double*)calloc((range_samples/2-band_cut),sizeof(double));
    tlook2_i=(double*)calloc((range_samples/2-band_cut),sizeof(double));
    fft_look1_r=(double*)calloc((range_samples/2-band_cut),sizeof(double));
    fft_look1_i=(double*)calloc((range_samples/2-band_cut),sizeof(double));
    fft_look2_r=(double*)calloc((range_samples/2-band_cut),sizeof(double));
    fft_look2_i=(double*)calloc((range_samples/2-band_cut),sizeof(double));

    
    tmlbf_r=(double *)calloc( (azimuth_samples),sizeof(double ) );
    tmlbf_i=(double *)calloc( (azimuth_samples),sizeof(double ) );
    fft_mlbf_r=(double *)calloc( (azimuth_samples),sizeof(double ) );
    fft_mlbf_i=(double *)calloc( (azimuth_samples),sizeof(double ) );

    test_data_r= (double **)calloc( (azimuth_samples), sizeof(double *) );   
    test_data_i= (double **)calloc( (azimuth_samples), sizeof(double *) ); 
    for   (i=0;i< (azimuth_samples); i++)     
    {   
          test_data_r[i] = (double*)calloc( (range_samples),sizeof(double ) );  
          test_data_i[i] = (double*)calloc( (range_samples),sizeof(double ) );
      
    }   


    mlbf_r=(double **)calloc( azimuth_samples,sizeof(double *) );
    mlbf_i=(double **)calloc( azimuth_samples,sizeof(double *) );

    look1_r= (double **)calloc( azimuth_samples,sizeof(double *) );   
    look1_i= (double **)calloc( azimuth_samples,sizeof(double *) ); 
    look2_r= (double **)calloc( azimuth_samples,sizeof(double *) ); 
    look2_i= (double **)calloc( azimuth_samples,sizeof(double *) ); 

    for   (i=0;i<(azimuth_samples); i++)     
    {   
          look1_r[i] = (double*)calloc((range_samples/2-band_cut), sizeof(double));  
          look1_i[i] = (double*)calloc((range_samples/2-band_cut), sizeof(double));
          look2_r[i] = (double*)calloc((range_samples/2-band_cut), sizeof(double));
          look2_i[i] = (double*)calloc((range_samples/2-band_cut), sizeof(double));
          mlbf_r[i] = (double*)calloc((range_samples/2-band_cut), sizeof(double));  
          mlbf_i[i] = (double*)calloc((range_samples/2-band_cut), sizeof(double));  
    }

    range_filter_r=(double *)calloc((range_samples), sizeof(double )  );
    range_filter_i=(double *)calloc((range_samples), sizeof(double )  );
    range_f=(double *)calloc((range_samples), sizeof(double )  );

    tao=(double*)calloc((range_samples/2-band_cut),sizeof(double));
    
    for(i=0;i<range_samples/2-band_cut;i++)
		tao[i]=-T_tao/2.0+(double)i*T_tao/(range_samples/2-band_cut);

	//tao=-T_tao/2.+findgen(range_samples/2-band_cut)*T_tao/(range_samples/2-band_cut)
    

    
    for(i=0;i<range_samples;i++)
    {
        range_f[i]=-fs/2.0+(double)i*fs/range_samples;
        range_filter_r[i]=cos(-pow(range_f[i],2)*PI/k);
        range_filter_i[i]=sin(-pow(range_f[i],2)*PI/k);
    }	

	//range_f=-fs/2.+findgen(range_samples)*fs/range_samples
	//range_filter=exp(dcomplex(0,range_f^2*!pi/k))




   if( (fp=fopen("e:\\test\\OnePoint_data_master_00.dat","r")) == NULL )
	
	  return ;

   

	fread(data_tmp,8,2*range_samples*azimuth_samples,fp);

	

	for ( ia =0;ia<=azimuth_samples-1;ia++)
    {
       	
   
		for(i=0;i<range_samples;i++)
		{
			in_echo_r[i]=data_tmp[2*i+2*range_samples*ia];
			in_echo_i[i]=data_tmp[2*i+2*range_samples*ia+1];
		}
		//readu,1,in_echo

    /*	
        for(i=0;i<range_samples;i++)
        {
            total_in_echo_r+=in_echo_r[i];
            total_in_echo_i+=in_echo_i[i];
        }

        for(i=0;i<range_samples;i++)
        {
           in_echo_r[i]=in_echo_r[i]-total_in_echo_r/range_samples;
           in_echo_i[i]=in_echo_i[i]-total_in_echo_i/range_samples;
        }