void sf_putint (sf_file file, const char* key, int par)
/*< put an int parameter to a file >*/
{
    char val[256];
	
    if (NULL == file->dataname) 
	sf_warning("%s: putint to a closed file",__FILE__);
    snprintf(val,256,"%d",par);
    sf_simtab_enter (file->pars,key,val);
}

void hhimc_init(axa ahx_,
		axa ahy_,
		axa ahz_
)
/*< initialize prestack IC >*/
{
    ahx = ahx_;
    ahy = ahy_;
    ahz = ahz_;

    LOx = floor(ahx.o/ahx.d); HIx = LOx + ahx.n;
    LOy = floor(ahy.o/ahy.d); HIy = LOy + ahy.n;
    LOz = floor(ahz.o/ahz.d); HIz = LOz + ahz.n;
    
    sf_warning("LOx=%d HIx=%d",LOx,HIx);
    sf_warning("LOy=%d HIy=%d",LOy,HIy);
    sf_warning("LOz=%d HIz=%d",LOz,HIz);

}

int main (int argc, char *argv[]) 
{    
    int m, k, l, seed;
    sf_file  bshuffle,ashuffle;
    sf_axis ax,at,ap,av;
    int nx,nt,np,nv,iteration,*a1, *a2;
    float ***bsh, ***bsh2;
    sf_init(argc,argv);

    bshuffle=sf_input("in");
    ashuffle=sf_output("out");

    if (!sf_getint("iteration",&iteration)) iteration=1;
    if (!sf_getint("seed",&seed)) seed=2012;
    at=sf_iaxa( bshuffle,1); nt=sf_n(at);
    ax=sf_iaxa( bshuffle,3); nx=sf_n(ax);
    ap=sf_iaxa( bshuffle,2); np=sf_n(ap);
    av=sf_iaxa( bshuffle,4); nv=sf_n(av);
    bsh=sf_floatalloc3(nt,np,nx);
    bsh2=sf_floatalloc3(nt,np,nx);
    a1=sf_intalloc(np);
    a2=sf_intalloc(np);
    sf_warning("ntpx=%d",nt*np*nx);

    srand(seed);


    for (m=0; m<np; m++) {
	a1[m]=rand();
    }

    bubble(a1, a2, np);


    for (k=0; k<nv; k++) {
	sf_floatread(bsh[0][0],nt*np*nx,bshuffle);
	for(l=0; l<nx; l++) {
	    for (m=0; m<np; m++) {
		memcpy(bsh2[l][m], bsh[l][a2[m]], nt*sizeof(float));
	    }
	}
	sf_floatwrite(bsh2[0][0],nt*np*nx,ashuffle);
    }

    free(bsh[0][0]);
    free(bsh[0]);
    free(bsh);
    free(bsh2[0][0]);
    free(bsh2[0]);
    free(bsh2);
    free(a1);
    free(a2);
    exit (0);
}

int mcfft3_init(int pad1           /* padding on the first axis */,
	       int nx,   int ny,  int nz   /* input data size */, 
	       int *nx2, int *ny2, int *nz2 /* padded data size */,
               int *n_local, int *o_local /* local size & start */)
/*< initialize >*/
{
  int cpuid;
  MPI_Comm_rank(MPI_COMM_WORLD, &cpuid);

  if (threads_ok) threads_ok = fftwf_init_threads();

  fftwf_mpi_init();

  if (false)
    sf_warning("Using threaded FFTW3! \n");
  if (threads_ok)
    fftwf_plan_with_nthreads(omp_get_max_threads());

  /* axis 1 */
  nk = n1 = kiss_fft_next_fast_size(nx*pad1);
  /* axis 2 */
  n2 = kiss_fft_next_fast_size(ny);
  /* axis 3 */
  n3 = kiss_fft_next_fast_size(nz);

  alloc_local = fftwf_mpi_local_size_3d(n3, n2, n1, MPI_COMM_WORLD, &local_n0, &local_0_start);

  //cc = sf_complexalloc3(n1,n2,n3);
  cc = sf_complexalloc(alloc_local);

  cfg  = fftwf_mpi_plan_dft_3d(n3,n2,n1,
                               (fftwf_complex *) cc,
                               (fftwf_complex *) cc,
                               MPI_COMM_WORLD,
                               FFTW_FORWARD, FFTW_MEASURE);

  icfg = fftwf_mpi_plan_dft_3d(n3,n2,n1,
                               (fftwf_complex *) cc, 
                               (fftwf_complex *) cc,
                               MPI_COMM_WORLD,
                               FFTW_BACKWARD, FFTW_MEASURE);

  if (NULL == cfg || NULL == icfg) sf_error("FFTW failure.");

  *nx2 = n1;
  *ny2 = n2;
  *nz2 = n3;
  *n_local = (int) local_n0;
  *o_local = (int) local_0_start;
	
  wt =  1.0/(n3*n2*n1);

  return (nk*n2*n3);
}

void sf_putstring (sf_file file, const char* key,const char* par)
/*< put a string parameter to a file >*/
{
    char *val;
    
    if (NULL == file->dataname) 
	sf_warning("%s: putstring to a closed file",__FILE__);
    val = (char*) alloca(strlen(par)+3); 
    sprintf(val,"\"%s\"",par);
    sf_simtab_enter (file->pars,key,val);
}

static void checktree( int ind, const char * str ) 
{
  int k1, k2, k, i,i1=0,i2=0;

  for( k=1; k<=count; k++ ) {
    k1=k*2;
    k2=k1+1;
	i=*(r+k);
    if( k1 <= count ) {
      i1=*(r+k1);
      if( *(t0+i) > *(t0+i1) ) {
	    i=*(r+k);
	    sf_warning("%s",str);
	    sf_warning("at updating the point (%i,  %i)=%i",ind%nx,ind/nx,ind);
	    sf_warning(" parent: (%i, %i)=%i, %.8e",i%nx,i/nx,i, *(t0+i));
	    sf_error("children: %i, %.8e\t %i, %.8e",i1,*(t0+i1),i2,*(t0+i2));
      }
    }
    if( k2 <= count ) {
      i2=*(r+k2);
      if( *(t0+i) > *(t0+i2) ) {
	    i=*(r+k);
	    sf_warning("%s",str);
	    sf_warning("at updating the point (%i, %i)=%i",ind%nx,ind/nx,ind);
	    sf_warning("parent: (%i, %i)=%i, .%.8e",i%nx,i/nx,i,*(t0+i));
	    sf_error("children: %i, %.8e\t %i, %.8e",i1,*(t0+i1),i2,*(t0+i2));
      }
    }
  } 
}

int main(void)
{
	
    int i, j, m, n, order=6, n1x=11, n1y=11, n2x=31, n2y=31; /*for 2D eno, order must be less than 7, or ng=2*(order-1) is too big */
    float **a, **a1, **a2, x, y, f1;   /* a is reference data, a1 is true data, a2 is interpolated data. */
    sf_eno2 map;

    a=sf_floatalloc2(n1x,n1y); /*n1x and n1y are original total grid points*/
    a1=sf_floatalloc2(n2x,n2y);/*n2x and n2y are interpolated total grid points*/
    a2=sf_floatalloc2(n2x,n2y);

    for(i=0;i<n1x;i++)
	for(j=0;j<n1y;j++)
		a[j][i]=sinf(SF_PI/100*i*j)+cosf(SF_PI/100*i*j); /* Calculate original data point value. */
    
    for(i=0;i<n2x;i++)
	for(j=0;j<n2y;j++)
		{a1[j][i]=sinf(SF_PI/100*(0+1.0/3*i)*(0+1.0/3*j))+cosf(SF_PI/100*(0+1.0/3*i)*(0+1.0/3*j));} /* Calculate true fine-grid data point value. */
	
    map = sf_eno2_init(order,n1x,n1y);
    sf_eno2_set (map,a);
    sf_warning("hello1");
    for(i=0;i<n2x;i++)
	for(j=0;j<n2y;j++)
    {
	x=(0+1.0/3*i-0)/1;
	y=(0+1.0/3*j-0)/1;
	m=x;  /* m is grid location in x direction */
	n=y;    /* n is grid location in y direction */
	x-=m; /* x is offset from grid location m */
	y-=n; /* y is offset from grid location n */
        sf_eno2_apply (map, m, n, x, y, &a2[j][i], &f1, FUNC); /* Interpolate fine-grid data point. f1 is the derivative sequence. FUNC is a flag value for eno interpolation */
    }
    sf_warning("hello2");
    for(i=0;i<n2x;i++)
	for(j=0;j<n2y;j++)
		{sf_warning("Interpolation comparison: True[%d][%d]=%f, Inter[%d][%d]=%f",j,i,a1[j][i],j,i,a2[j][i]);}

    exit(0);
}

void wavmod_shot(sf_file dat, sf_file wfl, int ns, int *ps, float **ws)
/*< shot modelling >*/
{
	int it, ig, is;
	int i1, n1, n2, n3;
	float *p;

	n1 = sf_n(vel->z); n2=1; n3=1;
	if(vel->nd >= 2) n2 = sf_n(vel->x);
	if(vel->nd >= 3) n3 = sf_n(vel->y);

	memset(owv, 0, nxyz*sizeof(float));
	memset(wav, 0, nxyz*sizeof(float));
	memset(nwv, 0, nxyz*sizeof(float));
	memset(ud, 0, nxyz*sizeof(float));

	for (it=0; it < nt; it++)
	{
		fd3_laplacian(n1, n2, n3,  wav, ud);

		for (is=0; is<ns; is++)
		ud[ps[is]] += ws[is][it];

#ifdef _OPENMP
#pragma omp parallel for	 \
	schedule(dynamic,n1)	  \
	private(i1)		  
#endif
		for(i1=0; i1<nxyz; i1++)
			nwv[i1] = 2.0*wav[i1] - owv[i1] + ud[i1]*vel->p[i1];
		p = owv;	
		owv = wav;
		wav = nwv;
		nwv = p;

		if(it >= st && (it-st)%jt==0)
		{
#ifdef _OPENMP
#pragma omp parallel for	 \
	schedule(dynamic,8)	  \
	private(ig)		  
#endif
			for(ig=0; ig<ng; ig++)
			data[ig][(it-st)/jt] = wav[pg[ig]];
		}
		if(wfl!=NULL && it>=st && (it-st)%jtm == 0) /* wave */
		sf_floatwrite(wav, nxyz, wfl);
		if(verb) sf_warning("%d of %d;", it, nt);
	}
	/* output seismic data */
	sf_floatwrite(data[0], ntj*ng, dat);
}

int cfft2_init(int pad1           /* padding on the first axis */,
               int nx,   int ny   /* input data size */,
               int *nx2, int *ny2 /* padded data size */)
/*< initialize >*/
{

#ifdef SF_HAS_FFTW
#ifdef _OPENMP
    fftw_init_threads();
    sf_warning("Using threaded FFTW3! \n");
    fftw_plan_with_nthreads(omp_get_max_threads());
#endif
#endif

#ifndef SF_HAS_FFTW
    int i2;
#endif

    nk = n1 = kiss_fft_next_fast_size(nx*pad1);

#ifndef SF_HAS_FFTW
    cfg1  = kiss_fft_alloc(n1,0,NULL,NULL);
    icfg1 = kiss_fft_alloc(n1,1,NULL,NULL);
#endif

    n2 = kiss_fft_next_fast_size(ny);

    cc = sf_complexalloc2(n1,n2);
    dd = sf_complexalloc2(nk,n2);

#ifndef SF_HAS_FFTW
    cfg2  = kiss_fft_alloc(n2,0,NULL,NULL);
    icfg2 = kiss_fft_alloc(n2,1,NULL,NULL);

    tmp =    (kiss_fft_cpx **) sf_alloc(n2,sizeof(*tmp));
    tmp[0] = (kiss_fft_cpx *)  sf_alloc(nk*n2,sizeof(kiss_fft_cpx));
    for (i2=0; i2 < n2; i2++) {
        tmp[i2] = tmp[0]+i2*nk;
    }

    trace2 = sf_complexalloc(n2);
    ctrace2 = (kiss_fft_cpx *) trace2;
#endif

    *nx2 = n1;
    *ny2 = n2;

    wt =  1.0/(n1*n2);

    return (nk*n2);
}

int cfft2_init(int pad1           /* padding on the first axis */,
	       int nx,   int ny   /* input data size */, 
	       int *nx2, int *ny2 /* padded data size */,
               int *n_local, int *o_local /* local size & start */,
               MPI_Comm comm)
/*< initialize >*/
{
  if (threads_ok) threads_ok = fftwf_init_threads();

  fftwf_mpi_init();

  if (false)
    sf_warning("Using threaded FFTW3! \n");
  if (threads_ok)
    fftwf_plan_with_nthreads(omp_get_max_threads());

  nk = n1 = kiss_fft_next_fast_size(nx*pad1);
  n2 = kiss_fft_next_fast_size(ny);

  alloc_local = fftwf_mpi_local_size_2d(n2, n1, comm, &local_n0, &local_0_start);

  //cc = sf_complexalloc2(n1,n2);
  //dd = sf_complexalloc2(nk,n2);
  cc = sf_complexalloc(alloc_local);
  dd = sf_complexalloc(alloc_local);

  cfg = fftwf_mpi_plan_dft_2d(n2,n1,
                              (fftwf_complex *) cc,
                              (fftwf_complex *) dd,
                              comm,
                              FFTW_FORWARD, FFTW_MEASURE);

  icfg = fftwf_mpi_plan_dft_2d(n2,n1,
                               (fftwf_complex *) dd, 
                               (fftwf_complex *) cc,
                               comm,
                               FFTW_BACKWARD, FFTW_MEASURE);

  if (NULL == cfg || NULL == icfg) sf_error("FFTW failure.");

  *nx2 = n1;
  *ny2 = n2;
  *n_local = (int) local_n0;
  *o_local = (int) local_0_start;
	
  wt =  1.0/(n1*n2);
	
  return (nk*n2);
}

void rweone_psc_coef(
    float **aa,
    float **bb,
    float **a0,
    float **b0)
/*< PSC coefficients >*/
{
    float tt,tt2, *tb;
    int ii,ir,it,ig;
    float boa, boa2;
    float aoa;
    float bob;

    if(verb) sf_warning("compute PSC coef");

    /* find max(b0) */
    tb=sf_floatalloc (ar.n*at.n);
    ii=0;
    for(it=0;it<at.n;it++) {
	for(ir=0;ir<ar.n;ir++) {
	    tb[ii] = b0[it][ir];
	    ii++;
	}
    }
    tt = SF_ABS(sf_quantile(ar.n*at.n-1,ar.n*at.n,tb));
    tt2= tt*tt;
    free(tb);

    for(it=0;it<at.n;it++) {
	for(ir=0;ir<ar.n;ir++) {
	    boa = ( b0[it][ir]/tt ) / a0[it][ir];
	    boa2=boa*boa;

	    for(ig=0;ig<ag.n;ig++) {
		aoa = aa[it][ig]/a0[it][ir];
		bob = bb[it][ig]/b0[it][ir];

		m0[it][ir][ig] = 1 / tt2;
		n0[it][ir][ig] = a0[it][ir] * boa2 * (c1*(aoa-1.) - (bob-1.));
		r0[it][ir][ig] =    3 * c2 * boa2;

		m0[it][ir][ig] *= kmu;
		n0[it][ir][ig] *= knu;
		r0[it][ir][ig] *= kro;
	    }
	}
    }
}

int main(int argc, char* argv[])
{
    bool verb;
    int j, k, n, n2, i3, n3, iter, niter;
    sf_complex **a=NULL, *e=NULL;
    float s2;
    sf_file mat=NULL, val=NULL;

    sf_init(argc,argv);
    mat = sf_input("in");
    val = sf_output("out");

    if (SF_COMPLEX != sf_gettype(mat)) sf_error("Need complex input");
    if (!sf_histint(mat,"n1",&n)) sf_error("No n1= in input");
    if (!sf_histint(mat,"n2",&n2) || n2 != n) sf_error("Need n1=n2 in input");
    n3 = sf_leftsize(mat,2);

    sf_putint(val,"n2",1);

    if (!sf_getint("niter",&niter)) niter=10;
    if (!sf_getbool("verb",&verb)) verb=false;

    a = sf_complexalloc2(n,n);
    e = sf_complexalloc(n);
    jacobi2_init(n,verb);

    for (i3=0; i3 < n3; i3++) {
	sf_complexread(a[0],n*n,mat);
	
	for (iter=0; iter < niter; iter++) {
	    s2 = 0.;
	    for (j=0; j < n; j++) {
		for (k=0; k < n; k++) {
		    s2 += jacobi2(a,n,j,k);
		}
	    }
	    sf_warning("iter=%d s2=%g",iter+1,s2);
	}

	for (j=0; j < n; j++) {
	    e[j]=a[j][j];
	}
	
	sf_complexwrite(e,n, val);
    }

    exit(0);
}

int getnumpars(const char* key)
/*< get number of pars >*/
{
  char* par;
  int numpars=0;
  
  sf_warning("in getnumpars call sf_getstring key=%s\n",key);
  if(NULL==(par=sf_getstring(key)))return 0;
   
  numpars=counttokens(par,",");

  fprintf(stderr,"free(par)\n");
  free(par);
  
  return numpars;
}

void sf_putfloats (sf_file file, const char* key, const float* par, size_t n)
/*< put a float array of size n parameter to a file >*/
{
    int i;
    char val[1024], *v;
	
    if (NULL == file->dataname) 
	sf_warning("%s: putfloats to a closed file",__FILE__);
    v = val;
    for (i=0; i < (int) n-1; i++) {
	v += snprintf(v,1024,"%g,",par[i]);
    }
    snprintf(v,1024,"%g",par[n-1]);
	
    sf_simtab_enter (file->pars,key,val);
}

int main (int argc, char* argv[])
{

    int i,n1,n2,n22,n;
    float *A, *x, *d;
    bool verb;
    sf_file in, rhs, out;

    sf_init (argc,argv);
    in = sf_input("in");
    rhs= sf_input("rhs");
    out = sf_output("out");
    if (SF_FLOAT != sf_gettype(in)) sf_error("Need float");

    if(!sf_histint(in,"n1",&n1)) sf_error("No n1 in input matrix!");
    if(!sf_histint(in,"n2",&n2)) sf_error("No n2 in input matrix!");
    if(!sf_histint(rhs,"n2",&n22))sf_error("No n2 in input vector!");
    if(!sf_getbool("verb",&verb)) verb=false;
    if((n1!=n2) || (n1!=n22)) sf_error("Dimension mistake!");
    n=n1;

    x = sf_floatalloc(n);
    A=sf_floatalloc(n*n);
    d=sf_floatalloc(n);

    sf_floatread(A,n*n,in);
    sf_floatread(d,n,rhs);

    trid_init(n, A);
    trid_solve(d, x);
    
    if(verb)
    {
	for(i=0;i<n;i++)
	sf_warning("x[%d]=%f",i+1,x[i]);
    }
    
   sf_putint(out,"n2",n);
   sf_putint(out,"n1",1);
   sf_floatwrite(x,n,out);

   trid_close();
   free(A);
   free(d);
   free(x);
   exit(0);
}

void ceig(int niter      /* number of iterations */, 
	  float tol      /* tolerance */, 
	  int m          /* effective matrix size */,
	  sf_complex** a /* [n][n] matrix */, 
	  sf_complex *e  /* [n] eigenvalues */)
/*< find eigenvalues >*/
{
    int iter, j, k, info;
    float s2,s0=1.;

    if (niter > 0) { /* Jacobi iterations */
	for (iter=0; iter < niter; iter++) {
	    s2 = 0.;
	    for (j=0; j < m; j++) {
		for (k=0; k < m; k++) {
		    s2 += jacobi2(a,m,j,k);
		}
	    }
	    if (verb) sf_warning("iter=%d s2=%g",iter+1,s2);
	    if (0==iter) {
		s0 = s2;
	    } else {
		if (s2 <= s0*tol) break;
	    }
	}
	
	for (j=0; j < m; j++) {
	    e[j]=a[j][j];
	}
    } else {
	for (j=0; j < m; j++) {
	    for (k=0; k < m; k++) {
		b[k+j*m] = a[j][k];
	    }
	}
#ifdef SF_HAS_LAPACK
	cgeev_( "N", "N", &m, b, &m, e, work, &n2, work, &n2, work, &n2, rwork, &info );
	if (info) sf_error("cgeev_ failed");
#else
	sf_error("No LAPACK");
#endif
    }
    for (j=m; j < n-1; j++) {
	e[j]=sf_cmplx(0.,0.);
    }
}

vel_t *clone_vel(float ***vel, int nz, int nx, int ny,
    float oz, float ox, float oy,
    float dz, float dx, float dy,
    float w0, float qfact)
{
  vel_t *v = malloc(sizeof *v);
  v->n1 = nz; v->n2 = nx; v->n3 = ny;
  v->o1 = oz; v->o2 = ox; v->o3 = oy;
  v->d1 = dz; v->d2 = dx; v->d3 = dy;
  v->dat = sf_floatalloc3(nz, nx, ny);
  memcpy(v->dat[0][0], vel[0][0], nz*nx*ny*sizeof(float));
  v->vgamma = sf_floatalloc3(1,1,1);

  gs_w0 = w0; gs_qfact= qfact;
  gs_gamma = 1.0/SF_PI*atan(2*SF_PI/qfact);

  sf_warning("gs_gamma: %f", gs_gamma);
  return v;
}

struct shot_image_par_type shot_image_grab_par(void)
/*< initialize >*/
{
    struct shot_image_par_type image_par;
    float min_x,max_x,min_y,max_y;
    int nhx,nhy,ihx;
    if (!sf_getint("nhx",&nhx) ) nhx=1;
    if (!sf_getint("nhy",&nhy) ) nhy=1;
    if (!sf_getfloat("image_min_x",&min_x)) min_x=1000.0;
    if (!sf_getfloat("image_max_x",&max_x)) max_x=10000.0;
    if (!sf_getfloat("image_min_y",&min_y)) min_y=0.0;
    if (!sf_getfloat("image_max_y",&max_y)) max_y=0.0;
    image_par.nhx=nhx; image_par.nhy=nhy;
    image_par.min_x=min_x; image_par.max_x=max_x;
    image_par.min_y=min_y; image_par.max_y=max_y;
    image_par.sincx_table=sf_floatalloc(8*2001);
    for(ihx=0;ihx<2001;ihx++) mksinc((float)(ihx)*0.0005,8,image_par.sincx_table+ihx*8); 
    sf_warning("Image: minx=%f  maxx=%f\n",image_par.min_x,image_par.max_x);
    return image_par; 
}

void distance (int np         /* number of points */, 
	       float **points /* point coordinates [np][3] */,
	       float* dist    /* distance */, 
	       float* v       /* slowness squared */,
	       int* in                    /* in/front/out flag */, 
	       int n3,int n2,int n1       /* dimensions */,
	       float o3,float o2,float o1 /* origin */,
	       float d3,float d2,float d1 /* sampling */,
	       int order                  /* accuracy order (1,2,3) */)
/*< Run fast marching eikonal solver >*/
{
    float d[3], o[3], *p;
    int n[3], npoints, i;
    
    n[0] = n1; o[0] = o1; d[0] = d1;
    n[1] = n2; o[1] = o2; d[1] = d2;
    n[2] = n3; o[2] = o3; d[2] = d3;

    sf_pqueue_start();
    sf_neighbors_init (in, d, n, order, dist);

    for (npoints =  sf_neighbors_distance (np, v, points, d, o);
	 npoints > 0;
	 npoints -= sf_neighbours(i)) {
	/* Pick smallest value in the NarrowBand
	   mark as good, decrease points_left */

	/* sf_warning("npoints=%d",npoints); */

	p = sf_pqueue_extract();

	if (p == NULL) {
	    sf_warning("%s: heap exausted!",__FILE__);
	    break;
	}

	i = p - dist;

	in[i] = SF_IN;
    }
}

void rweone_xfd_coef(
    float **aa,
    float **bb)
/*< XFD coefficients >*/
{
    int it,ig;

    if(verb) sf_warning("compute XFD coef");

    for(it=0;it<at.n;it++) {
	for(ig=0;ig<ag.n;ig++) {
	    m0[it][0][ig] = 1.;
	    n0[it][0][ig] = - c1 * (bb[it][ig]*bb[it][ig]) /  aa[it][ig];
	    r0[it][0][ig] =   c2 * (bb[it][ig]*bb[it][ig]) / (aa[it][ig] * aa[it][ig]);

	    m0[it][0][ig] *= kmu;
	    n0[it][0][ig] *= knu;
	    r0[it][0][ig] *= kro;
	}
    }
}