guisdap Code

/****************************************************************************
clutter.c -- decodump routine for removing clutter via fft
*****************************************************************************/

#include <stdlib.h>
#include <string.h>	/* Included as we are using the bzero function */
#include <strings.h>
#include <unistd.h>
#include <math.h>

typedef struct {
	float	re;
	float	im;
}	DSPCMPLX;

typedef struct {
	int16_t	re;
	int16_t	im;
}	DSPCMPLXSHORT;

#include <fftw3.h>
#define CPU_SPEED 1.e9
#include <sys/time.h>
//##ifndef gethrtime /*gcc case*/
#ifndef NANOSEC /*gcc case*/
#include <time.h>
#define NANOSEC 1000000000 
typedef	unsigned long long int hrtime_t;
unsigned long gethrtime(void)
{
	struct timespec ts;
	if (clock_gettime(CLOCK_MONOTONIC_RAW, &ts) != 0) return (-1);
	return ((ts.tv_sec * NANOSEC) + ts.tv_nsec);
}
#endif
#include <pthread.h>
struct pth {
	ulong th;
	ulong k1;
	ulong k2;
	ulong nr_win;
	ulong nr_rep;
	int notch;
	DSPCMPLXSHORT *in;
	DSPCMPLX *out;
	ulong nr_pp;
	ulong nr_lag;
	ulong nr_clutter;
	int fft_clutter;
	ulong debug;
	fftwf_complex *inf;
	fftwf_plan pc;
	fftwf_plan pcb;
};

void *plwin_clutter(struct pth *);
void *plwin_lp(struct pth *);

int clutter_6(ulong nbits,int *par,ulong fbits,float *fpar,DSPCMPLXSHORT *in,
	DSPCMPLX *out,double *upar)
{

/*printf("%ld %ld %ld %ld %ld %ld %ld\n",nbits,par,fbits,fpar,in,out,upar);*/
	ulong	i,j,k,k2,clutthreads,float_notch=0,ncth,nlth,nout;
	int nth;
	struct pth	pth;
	hrtime_t	start[]={0,0,0,0,0};
	start[0]=gethrtime();

	pth.nr_win=par[0];
	pth.nr_rep=par[1];
	pth.nr_pp=par[2];
	pth.nr_clutter=par[3];
	if(par[4]>-1) pth.notch=par[4];
	else pth.notch=(int)upar[0];
	clutthreads=par[5];
	pth.debug=par[6];
	pth.nr_lag=par[7];

	if(pth.notch<0)	float_notch=1;
	pth.fft_clutter=pth.nr_rep/pth.nr_win;

	pthread_attr_t	sched_glob;
	void	*retval;
	pthread_t	*thread=NULL;
	struct pth	*ptth=NULL;
	nth=sysconf(_SC_NPROCESSORS_ONLN);
	ncth=clutthreads;
	if(ncth>pth.nr_clutter) ncth=pth.nr_clutter;
	nlth=pth.nr_lag;
	if(nlth>nth) nlth=nth;
	nth=(nlth>ncth) ? nlth : ncth; /*max threads*/
	if(nth>1) {
		pthread_attr_init(&sched_glob);
		pthread_attr_setscope(&sched_glob,PTHREAD_SCOPE_SYSTEM);
		thread=(pthread_t *)malloc(nth*sizeof(pthread_t));
		ptth=(struct pth *)malloc(nth*sizeof(struct pth));
	}

	nout=pth.nr_pp*pth.nr_lag-pth.nr_lag*(pth.nr_lag-1)/2;
	if(nout==0) nout=1;
	bzero(out,nout*sizeof(DSPCMPLX));
	pth.in=in;
	pth.out=out;
	if(ncth>0) {
		pth.inf=fftwf_malloc(ncth*2*pth.fft_clutter*sizeof(fftwf_complex));
		pth.pc=fftwf_plan_dft_1d(pth.fft_clutter,pth.inf,pth.inf+pth.fft_clutter,FFTW_FORWARD,FFTW_ESTIMATE|FFTW_DESTROY_INPUT);
		pth.pcb=fftwf_plan_dft_1d(pth.fft_clutter,pth.inf+pth.fft_clutter,pth.inf,FFTW_BACKWARD,FFTW_ESTIMATE|FFTW_DESTROY_INPUT);
	}

	start[1]=gethrtime();
	for(k=0,i=0;i<ncth;i++,k=k2) {
		k2=pth.nr_clutter*(i+1)/ncth;
		pth.th=i; pth.k1=k; pth.k2=k2;
		if(ncth>1) {
			memcpy(ptth+i,&pth,sizeof(struct pth));
			pthread_create(thread+i,&sched_glob,(void *(*)(void *)) plwin_clutter,(void *)(ptth+i));
		} else plwin_clutter(&pth);
	}
	if(ncth>1)
		for(i=0;i<ncth;i++) {
			pthread_join(thread[i],&retval);
			if(i==0) pth.notch=ptth[i].notch;
			else pth.notch+=ptth[i].notch;
			if(pth.debug>0) printf("%d",ptth[i].notch);
		}
	if(nlth>0) {
		for(k=0,i=0;i<nlth;i++,k=k2) {
			k2=pth.nr_lag*(i+1)/nlth;
			pth.th=i; pth.k1=k; pth.k2=k2;
			if(nlth>1) {
				memcpy(ptth+i,&pth,sizeof(struct pth));
				pthread_create(thread+i,&sched_glob,(void *(*)(void *)) plwin_lp,(void *)(ptth+i));
			} else plwin_lp(&pth);
		}
		if(nlth>1)
			for(i=0;i<nlth;i++)
				pthread_join(thread[i],&retval);
	}

	if(ncth>0) {
		out[0].im=(float)pth.notch/(float)ncth;
		fftwf_destroy_plan(pth.pc);
		fftwf_destroy_plan(pth.pcb);
		fftwf_free(pth.inf);
	}
	if(nth>1) {
		free(thread); free(ptth);
	}
	if(pth.debug>0) {
		start[4]=gethrtime();
		printf("clutter: [%ld %ld] threads: %.2g s",ncth,nlth,(start[4]-start[0])/CPU_SPEED);
		if(ncth>0 && float_notch)
			printf(" Notch: %.2g",out[0].im);
		if(pth.debug>1)
			for(j=0;j<4;j++)
				printf(" %.2f",(start[j+1]-start[j])/CPU_SPEED);
		printf("\n");
	}
	return 0;
}	/* clutter_6 */

void *plwin_clutter(struct pth *pth)
{
	ulong	k,j,i,win;
	int	notch;
	DSPCMPLXSHORT	*in;
	DSPCMPLX	*clx,*cly;
	float	clf,clm,*clt;

	clx=(DSPCMPLX *)pth->inf+pth->th*pth->fft_clutter*2;
	cly=clx+pth->fft_clutter;
	i=pth->nr_rep/pth->nr_win;
	notch=pth->notch;
	if(notch<0) {
		clt=(float *)calloc(pth->fft_clutter,sizeof(float));
		for(j=pth->k1;j<pth->k2;j++)
			for(win=0;win<pth->nr_win;win++) {
				for(k=win;k<pth->nr_rep;k+=pth->nr_win) {
					in=pth->in+k*pth->nr_pp+j;
					clx[k/pth->nr_win].re=in->re;
					clx[k/pth->nr_win].im=in->im;
				}
				bzero(clx+i,(pth->fft_clutter-i)*sizeof(DSPCMPLX));
				fftwf_execute_dft(pth->pc,(fftwf_complex *)clx,(fftwf_complex *)cly);
				for(k=0;k<pth->fft_clutter;k++)
					clt[k]+=(cly[k].re*cly[k].re+cly[k].im*cly[k].im);
			}
		notch=0; clf=2.*clt[0]; clm=0; j=pth->fft_clutter-1;
		for(k=1;k<pth->fft_clutter;k++) clm+=clt[k];
		while(notch<-pth->notch+1 && clf/2.>clm/j) {
			notch++;
			clf=clt[notch]+clt[pth->fft_clutter-notch];
			clm-=clf; j-=2;
		}
		notch--;
		free(clt);
		pth->notch=notch;
	}
	if(notch>=0) {
		clf=1./sqrt(1.-(float)(2*notch+1)/(float)i)/(float)pth->fft_clutter;
		for(j=pth->k1;j<pth->k2;j++)
			for(win=0;win<pth->nr_win;win++) {
				for(k=win;k<pth->nr_rep;k+=pth->nr_win) {
					in=pth->in+k*pth->nr_pp+j;
					clx[k/pth->nr_win].re=in->re;
					clx[k/pth->nr_win].im=in->im;
				}
				bzero(clx+i,(pth->fft_clutter-i)*sizeof(DSPCMPLX));
				fftwf_execute_dft(pth->pc,(fftwf_complex *)clx,(fftwf_complex *)cly);
				bzero(cly,(notch+1)*sizeof(DSPCMPLX));
				bzero(cly+pth->fft_clutter-notch,notch*sizeof(DSPCMPLX));
				fftwf_execute_dft(pth->pcb,(fftwf_complex *)cly,(fftwf_complex *)clx);
				for(k=win;k<pth->nr_rep;k+=pth->nr_win) {
					in=pth->in+k*pth->nr_pp+j;
					in->re=rint(clx[k/pth->nr_win].re*clf);
					in->im=rint(clx[k/pth->nr_win].im*clf);
	}		}	}
	
	return NULL;
}

void *plwin_lp(struct pth *pth)
{
	ulong	i,k,j;
	DSPCMPLXSHORT	*in;
	DSPCMPLX	*out,s0,s1;

	for(i=0;i<pth->nr_rep;i++) {
		in=pth->in+i*pth->nr_pp;
		out=pth->out+pth->nr_pp*pth->k1-pth->k1*(pth->k1-1)/2;
		for(j=pth->k1;j<pth->k2;j++)
			for(k=0;k<pth->nr_pp-j;k++) {
				s0.re=in[k].re; s0.im=in[k].im;
				s1.re=in[k+j].re; s1.im=in[k+j].im;
				out->re+=(s0.re*s1.re+s0.im*s1.im);
				out->im+=(s0.im*s1.re-s0.re*s1.im);
				out++;
	}		}
	return NULL;
}

void matface(int *par,int *nin,double *in_r,double *in_i,
	int *nout, double *out_r, double *out_i, double *upar) {
	ulong nb=8;
	float *fpar=NULL;
	int i;
	DSPCMPLXSHORT *in;
	DSPCMPLX *out;
	in=(DSPCMPLXSHORT *)malloc(*nin*sizeof(DSPCMPLXSHORT));
	for(i=0;i<*nin;i++) {
		in[i].re=in_r[i];
		in[i].im=in_i[i];
	}
	out=(DSPCMPLX *)malloc(*nout*sizeof(DSPCMPLX));
	clutter_6(nb,par,0,fpar,in,out,upar);
	for(i=0;i<*nout;i++) {
		out_r[i]=out[i].re;
		out_i[i]=out[i].im;
	}
	for(i=0;i<*nin;i++) {
		in_r[i]=in[i].re;
		in_i[i]=in[i].im;
	}
	free(in); free(out);
}