/******************************************************************
 * DPUSER - The Next Generation
 *
 * File:     libfits/fits_red.cpp
 * Purpose:  Fits class methods doing data reduction
 * Author:   Thomas Ott
 *
 * Revision history
 * ================
 * 13.08.1999: file created
 ******************************************************************/

#include "fits.h"
#include "math_utils.h"
#include "fitting.h"
//@#include <rfftw.h>
#include <fftw3.h>

#ifdef WIN
#pragma warning (disable: 4018) // disable warning for comparing signed and unsigned
#endif /* WIN */

bool Fits::wien(const Fits & dbeam, float max) {
	double total;

// Check that the psf and the dirty image match and are 2-dimensional
	if ((naxis[0] != 2) || (dbeam.naxis[0] != 2)) {
		dp_output("Can only wiener deconvolve 2-dimensional images.\n");
		return FALSE;
	}
	if ((naxis[1] != dbeam.naxis[1]) || (naxis[2] != dbeam.naxis[2])) {
		dp_output("The two images do not match.\n");
		return FALSE;
	}

	Fits psf;
	if (!psf.copy(dbeam)) return FALSE;
	total = get_flux();
	if (!fft()) return FALSE;
	psf.norm();
	Fits delta;
	if (!psf.fft()) return FALSE;

	if (!delta.delta(naxis[1], naxis[2], naxis[1]/2+1, naxis[2]/2+1)) return FALSE;
	delta *= max;
	if (!delta.fft()) return FALSE;
	Fits delta_power;
	if (!delta_power.copy(delta)) return FALSE;
	delta_power.conj();
	delta_power *= delta;
	
	Fits psf_power;
	if (!psf_power.copy(psf)) return FALSE;
	psf_power.conj();
	*this *= psf_power;
	psf_power *= psf;
	psf_power += delta_power;
	*this /= psf_power;	
	if (!fft()) return FALSE;
	if (!reass()) return FALSE;
		
	total /= get_flux();	
	*this *= total;
	
	return TRUE;
}

bool Fits::quick_wien(const Fits & dbeam, float max) {
//@	fftw_real *a = NULL, *b = NULL, *c = NULL;
    double *a = NULL, *b = NULL, *c = NULL;
    fftw_complex *A, *B, *C;
	Fits delta;
//@	rfftwnd_plan p = NULL, pinv = NULL;
    fftw_plan pA = NULL, pB = NULL, pC = NULL, pinv = NULL;
    long M, N, L, i, x, y, z, n, inc;
	double nomre, nomim, den;

// Check that the psf and the dirty image match
	if (!matches(dbeam)) {
		dp_output("LUCY: Dirty map and beam do not match in size.\n");
		return FALSE;
	}
	n = naxis[0];
	N = Naxis(1);
	M = Naxis(2);
	L = Naxis(3);
	inc = (N-1) % 2 + 1;
	if (n == 2)
		delta.delta(M, N, M / 2 + 1, N / 2 + 1);
	else {
		delta.create(N, M, L, I1);
		delta.i1data[delta.F_I(M / 2 + 1, N / 2 + 1, L / 2 + 1)] = 1;
	}
	delta *= max;

	setType(R4);

	if (n == 2) {
//@		a = (fftw_real *)malloc(M * (N / 2 + 1) * sizeof(fftw_complex));
//@		b = (fftw_real *)malloc(M * (N / 2 + 1) * sizeof(fftw_complex));
//q		c = (fftw_real *)malloc(M * (N / 2 + 1) * sizeof(fftw_complex));
        a = (double *)fftw_malloc(M * (N / 2 + 1) * sizeof(fftw_complex));
        b = (double *)fftw_malloc(M * (N / 2 + 1) * sizeof(fftw_complex));
        c = (double *)fftw_malloc(M * (N / 2 + 1) * sizeof(fftw_complex));
    } else if (n == 3) {
//@        a = (fftw_real *)malloc(L * M * (N / 2 + 1) * sizeof(fftw_complex));
//@		b = (fftw_real *)malloc(L * M * (N / 2 + 1) * sizeof(fftw_complex));
//@		c = (fftw_real *)malloc(L * M * (N / 2 + 1) * sizeof(fftw_complex));
        a = (double *)fftw_malloc(L * M * (N / 2 + 1) * sizeof(fftw_complex));
        b = (double *)fftw_malloc(L * M * (N / 2 + 1) * sizeof(fftw_complex));
        c = (double *)fftw_malloc(L * M * (N / 2 + 1) * sizeof(fftw_complex));
    }
	if ((a == NULL) || (b == NULL) || (c == NULL)) {
		if (a != NULL) free(a);
		if (b != NULL) free(b);
		if (c != NULL) free(c);
		dp_output("Fits::quick_wien: Could not allocate memory.\n");
		return FALSE;
	}
	A = (fftw_complex *)a;
	B = (fftw_complex *)b;
	C = (fftw_complex *)c;
	if (n == 2) {
        pA = fftw_plan_dft_r2c_2d(M, N, a, A, FFTW_ESTIMATE);
        pB = fftw_plan_dft_r2c_2d(M, N, b, B, FFTW_ESTIMATE);
        pC = fftw_plan_dft_r2c_2d(M, N, c, C, FFTW_ESTIMATE);
        pinv = fftw_plan_dft_c2r_2d(M, N, A, a, FFTW_ESTIMATE);
//@        p = rfftw2d_create_plan(M, N, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE | FFTW_IN_PLACE);
//@		pinv = rfftw2d_create_plan(M, N, FFTW_COMPLEX_TO_REAL, FFTW_ESTIMATE | FFTW_IN_PLACE);
    } else if (n == 3) {
        pA = fftw_plan_dft_r2c_3d(M, N, L, a, A, FFTW_ESTIMATE);
        pB = fftw_plan_dft_r2c_3d(M, N, L, b, B, FFTW_ESTIMATE);
        pC = fftw_plan_dft_r2c_3d(M, N, L,c, C, FFTW_ESTIMATE);
        pinv = fftw_plan_dft_c2r_3d(M, N, L, A, a, FFTW_ESTIMATE);
//@        p = rfftw3d_create_plan(L, M, N, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE | FFTW_IN_PLACE);
//@		pinv = rfftw3d_create_plan(L, M, N, FFTW_COMPLEX_TO_REAL, FFTW_ESTIMATE | FFTW_IN_PLACE);
    }
	i = 0;

	if (n == 2) {
		for (y = 0; y < M; y++) {
			for (x = 0; x < N; x++) {
				a[i] = ValueAt(C_I(x, y));
				b[i] = dbeam.ValueAt(C_I(x, y));
				c[i] = delta.ValueAt(C_I(x, y));
				i++;
			}
			i += inc;
		}
	} else if (n == 3) {
		for (z = 0; z < L; z++) {
			for (y = 0; y < M; y++) {
				for (x = 0; x < N; x++) {
					a[i] = ValueAt(C_I(x, y, z));
					b[i] = dbeam.ValueAt(C_I(x, y, z));
					c[i] = delta.ValueAt(C_I(x, y, z));
					i++;
				}
				i += inc;
			}
		}
	}
    fftw_execute(pA);
    fftw_execute(pB);
    fftw_execute(pC);
//@	rfftwnd_one_real_to_complex(p, a, NULL);
//@	rfftwnd_one_real_to_complex(p, b, NULL);
//q	rfftwnd_one_real_to_complex(p, c, NULL);
	if (n == 2) {
		for (i = 0; i < M * (N / 2 + 1); i++) {
            nomre = A[i][0] * B[i][0] + A[i][1] * B[i][1];
            nomim = A[i][1] * B[i][0] - A[i][0] * B[i][1];
            den = B[i][0] * B[i][0] + B[i][1] * B[i][1];
            den += C[i][0] * C[i][0] + C[i][1] * C[i][1];
            A[i][0] = nomre / den;
            A[i][1] = nomim / den;
//            nomre = A[i].re * B[i].re + A[i].im * B[i].im;
//			nomim = A[i].im * B[i].re - A[i].re * B[i].im;
//			den = B[i].re * B[i].re + B[i].im * B[i].im;
//			den += C[i].re * C[i].re + C[i].im * C[i].im;
//			A[i].re = nomre / den;
//			A[i].im = nomim / den;
        }
	} else if (n == 3) {
		for (i = 0; i < L * M * (N / 2 + 1); i++) {
            nomre = A[i][0] * B[i][0] + A[i][1] * B[i][1];
            nomim = A[i][1] * B[i][0] - A[i][0] * B[i][1];
            den = B[i][0] * B[i][0] + B[i][1] * B[i][1];
            den += C[i][0] * C[i][0] + C[i][1] * C[i][1];
            A[i][0] = nomre / den;
            A[i][1] = nomim / den;
//            nomre = A[i].re * B[i].re + A[i].im * B[i].im;
//			nomim = A[i].im * B[i].re - A[i].re * B[i].im;
//			den = B[i].re * B[i].re + B[i].im * B[i].im;
//			den += C[i].re * C[i].re + C[i].im * C[i].im;
//			A[i].re = nomre / den;
//			A[i].im = nomim / den;
        }
	}
    fftw_execute(pinv);
//@	rfftwnd_one_complex_to_real(pinv, A, NULL);
    fftw_destroy_plan(pA);
    fftw_destroy_plan(pB);
    fftw_destroy_plan(pC);
//@    rfftwnd_destroy_plan(p);
    fftw_destroy_plan(pinv);
//@    fftwnd_destroy_plan(pinv);
    i = 0;

	if (n == 2) {
		for (y = 0; y < M; y++) {
			for (x = 0; x < N; x++) {
				r4data[C_I(x, y)] = a[i];
				i++;
			}
			i += inc;
		}
	} else if (n == 3) {
		for (z = 0; z < L; z++) {
			for (y = 0; y < M; y++) {
				for (x = 0; x < N; x++) {
					r4data[C_I(x, y, z)] = a[i];
					i++;
				}
				i += inc;
			}
		}
	}
    fftw_free(a);
    fftw_free(b);
    fftw_free(c);
//@    free(a);
//@	free(b);
//@	free(c);
    reass();

	return TRUE;
}

bool Fits::lucy(const Fits & psf, int niter, double thresh) {
    dpint64 i, iter;
	double flux, sum, sukn, suk, xmax, th;
	Fits dbeam;

// Check that the psf and the dirty image match
	if (naxis[0] != psf.naxis[0]) {
		dp_output("LUCY: Dirty map and beam do not match in size.\n");
		return FALSE;
	}
	if (naxis[0] == 1) {
		if (naxis[1] != psf.naxis[1]) {
			dp_output("LUCY: Dirty map and beam do not match in size.\n");
			return FALSE;
		}
	} else if (naxis[0] == 2) {
		if ((naxis[1] != psf.naxis[1]) || (naxis[2] != psf.naxis[2])) {
			dp_output("LUCY: Dirty map and beam do not match in size.\n");
			return FALSE;
		}
	} else if (naxis[0] == 3) {
		if ((naxis[1] != psf.naxis[1]) || (naxis[2] != psf.naxis[2]) || (naxis[3] != psf.naxis[3])) {
			dp_output("LUCY: Dirty map and beam do not match in size.\n");
			return FALSE;
		}
	} else {
		dp_output("LUCY: Dirty map and beam must be 1-, 2-, or 3-dimensional.\n");
		return FALSE;
	}

	if (!setType(R4)) return FALSE;
	
	clip(0.0, -1.0);
	flux = get_flux();
	Fits xmap;
	
	if (!xmap.copy(*this)) return FALSE;
	if (!dbeam.copy(psf)) return FALSE;
	dbeam.norm();
	sum = dbeam.get_flux();
	xmax = get_max();
	dbeam /= sum;
	if (!dbeam.fft()) return FALSE;
//	dbeam.conj();
	
	if (fabs(thresh) < EPS) thresh = -0.1;
	th = fabs(thresh) * xmax;
	dp_output("th = %f\n", th);
	
	Fits w;
	if (!w.copy(*this)) return FALSE;
	
	for (i = 0; i < n_elements; i++)
		if (r4data[i] > th) w.r4data[i] = 1.0;
		else w.r4data[i] = 0.0;
	
// should compute new threshold here

	if (thresh < 0.0) {
		double er;
		
		sum = suk = sukn = 0.0;
		for (i = 0; i < w.n_elements; i++) {
			if (w.r4data[i] < 0.5) {
				sum += r4data[i];
				suk += sqr(r4data[i]);
				sukn++;
			}
		}
		dp_output("%f %f %f\n", sukn, sum*sum/sukn, suk);
		er = sqrt(fabs(suk-sum*sum/sukn) / (sukn - 1.0));
		sum /= sukn;
		dp_output("%f %f\n", sum, er);
		thresh = sum + 3.0 * er;
		dp_output("%f\n", thresh);
		for (i = 0; i < n_elements; i++)
			if (r4data[i] > th) w.r4data[i] = 1.0;
	}
	
	Fits imap;
	if (!imap.copy(dbeam)) return FALSE;
	imap = 0.0;
	Fits cxmap;
	if (!cxmap.copy(xmap)) return FALSE;
	
	Fits lastxmap;
	if (!lastxmap.copy(xmap)) return FALSE;
	if (!cxmap.copy(xmap)) return FALSE;
	if (!cxmap.fft()) return FALSE;
	
    for (iter = 0; iter <	(dpint64)niter; iter++) {
		if (FitsInterrupt) {
			dp_output("lucy interrupted after %li iterations\n", iter);
			break;
		}
		lastxmap.copy(xmap);
			
		imap.copy(cxmap);
		imap *= dbeam;
		if (!imap.fft()) return FALSE;
		if (!imap.reass()) return FALSE;
		sum = imap.get_flux();
		imap *= flux/sum;
		
//	computing ratio map in area where weighting function is unity

		for (i = 0; i < imap.n_elements; i++) {
			if ((w.r4data[i] < 0.5) || (imap.r4data[i] < 1e-6))
				imap.r4data[i] = 1.0;
			else
				imap.r4data[i] = r4data[i] / imap.r4data[i];
		}
		
//	convolving ratio map with dirty beam

		if (!imap.fft()) return FALSE;
		imap *= dbeam;
		if (!imap.fft()) return FALSE;
		if (!imap.reass()) return FALSE;
		imap /= (float)(imap.n_elements);
		
//	multiplying current lucy map with convolved ratio map

		xmap *= imap;
		sum = xmap.get_flux();
		xmap *= flux/sum;
		
// convolving new iteration with dirty beam

		imap.copy(xmap);
		if (!imap.fft()) return FALSE;
		
		cxmap.copy(imap);
		
		imap *= dbeam;
		if (!imap.fft()) return FALSE;
		if (!imap.reass()) return FALSE;
		imap /= (float)(imap.n_elements);
		
//	renormalizing reconvolved map to flux of input dirty map
		
		sum = imap.get_flux();
		imap *= flux/sum;
		
		sum = sukn = 0.0;
		for (i = 0; i < imap.n_elements; i++) {
			sum += w.r4data[i]*sqr(r4data[i]-imap.r4data[i]);
			sukn += w.r4data[i];
		}
		sum = sqrt(sum/(sukn-1));
		
		dp_output("lucy ===> niter = %li,  sum = %f %f\n", iter+1, sum, sum/thresh);
		
		if (FitsInterrupt) {
			dp_output("lucy interrupted after %li iterations\n", iter+1);
			break;
		}
	}
	*this = xmap;

	return TRUE;
}

bool Fits::quick_lucy(const Fits & psf, int niter, double thresh) {

    dpint64 i, iter;
    dpint64 ii;
	double flux, sum, sukn, suk, xmax, th, re;
	long M, N, L, x, y, z, NE = 0, n;
	Fits xmap, w, imap;
    double *tmp;
	fftw_complex *TMP, *DBEAM;
//@	rfftwnd_plan p = NULL, pinv = NULL;
    fftw_plan p_tmp_DBEAM = NULL, p_xmap_TMP = NULL, p_TMP_imap = NULL, p_imap_TMP;
	static double value, vvalue;
	
// Check that the psf and the dirty image match
	if (!matches(psf)) {
		dp_output("LUCY: Dirty map and beam do not match in size.\n");
		return FALSE;
	}
	if ((naxis[0] < 2) || (naxis[0] > 3)) return lucy(psf, niter, thresh);
	n = naxis[0];
	N = Naxis(1);
	M = Naxis(2);
	L = Naxis(3);
	if (n == 2)
		NE = M * (N / 2 + 1);
	else if (n == 3)
		NE = L * M * (N / 2 + 1);
    tmp = (double *)fftw_malloc(NE * sizeof(fftw_complex));
	if (tmp == NULL) return FALSE;
    DBEAM = (fftw_complex *)fftw_malloc(NE * sizeof(fftw_complex));
	if (DBEAM == NULL) {
		free(tmp);
		return FALSE;
	}
	TMP = (fftw_complex *)tmp;
	if (n == 2) {
        p_tmp_DBEAM = fftw_plan_dft_r2c_2d(M, N, tmp, DBEAM, FFTW_ESTIMATE);
//@		p = rfftw2d_create_plan(M, N, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE | FFTW_USE_WISDOM);
//@		pinv = rfftw2d_create_plan(M, N, FFTW_COMPLEX_TO_REAL, FFTW_ESTIMATE | FFTW_USE_WISDOM);
	} else if (n == 3) {
        p_tmp_DBEAM = fftw_plan_dft_r2c_3d(M, N, L, tmp, DBEAM, FFTW_ESTIMATE);
//@        p = rfftw3d_create_plan(L, M, N, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE | FFTW_USE_WISDOM);
//@		pinv = rfftw3d_create_plan(L, M, N, FFTW_COMPLEX_TO_REAL, FFTW_ESTIMATE | FFTW_USE_WISDOM);
	}

	if (!setType(R4)) return FALSE;
	
	clip(0.0, -1.0);
	flux = get_flux();
	
	if (!xmap.copy(*this)) return FALSE;
	xmap.setType(R8);

	i = 0;
	sum = 0.0;
	if (n == 2) {
		for (y = 0; y < M; y++) {
			for (x = 0; x < N; x++) {
				tmp[i] = psf.ValueAt(C_I(x, y));
				sum += tmp[i];
				i++;
			}
		}
	} else if (n == 3) {
		for (z = 0; z < L; z++) {
			for (y = 0; y < M; y++) {
				for (x = 0; x < N; x++) {
					tmp[i] = psf.ValueAt(C_I(x, y, z));
					sum += tmp[i];
					i++;
				}
			}
		}
	}
    fftw_execute(p_tmp_DBEAM);
//@	rfftwnd_one_real_to_complex(p, tmp, DBEAM);

	xmax = get_max();
	
	if (fabs(thresh) < EPS) thresh = -0.1;
	th = fabs(thresh) * xmax;
	dp_output("th = %f\n", th);
	
	if (!w.copy(*this)) return FALSE;
	
	for (ii = 0; ii < n_elements; ii++)
		if (r4data[ii] > th) w.r4data[ii] = 1.0;
		else w.r4data[ii] = 0.0;
	
// should compute new threshold here

	if (thresh < 0.0) {
		double er;
		
		sum = suk = sukn = 0.0;
		for (ii = 0; ii < w.n_elements; ii++) {
			if (w.r4data[ii] < 0.5) {
				sum += r4data[ii];
				suk += sqr(r4data[ii]);
				sukn++;
			}
		}
		dp_output("%f %f %f\n", sukn, sum*sum/sukn, suk);
		er = sqrt(fabs(suk-sum*sum/sukn) / (sukn - 1.0));
		sum /= sukn;
		dp_output("%f %f\n", sum, er);
		thresh = sum + 3.0 * er;
		dp_output("%f\n", thresh);
		for (ii = 0; ii < n_elements; ii++)
			if (r4data[ii] > th) w.r4data[ii] = 1.0;
	}
    if (n == 2) {
		imap.create(Naxis(1), Naxis(2), R8);
        p_xmap_TMP = fftw_plan_dft_r2c_2d(M, N, xmap.r8data, TMP, FFTW_ESTIMATE);
        p_imap_TMP = fftw_plan_dft_r2c_2d(M, N, imap.r8data, TMP, FFTW_ESTIMATE);
        p_TMP_imap = fftw_plan_dft_c2r_2d(M, N, TMP, imap.r8data, FFTW_ESTIMATE);
    } else if (n == 3) {
		imap.create(Naxis(1), Naxis(2), Naxis(3), R8);
        p_xmap_TMP = fftw_plan_dft_r2c_3d(M, N, L, xmap.r8data, TMP, FFTW_ESTIMATE);
        p_imap_TMP = fftw_plan_dft_r2c_3d(M, N, L, imap.r8data, TMP, FFTW_ESTIMATE);
        p_TMP_imap = fftw_plan_dft_c2r_3d(M, N, L, TMP, imap.r8data, FFTW_ESTIMATE);
    }

    fftw_execute(p_xmap_TMP);
//@	rfftwnd_one_real_to_complex(p, xmap.r8data, TMP);
	for (i = 0; i < NE; i++) {
        value = TMP[i][0] * DBEAM[i][0] - TMP[i][1] * DBEAM[i][1];
        TMP[i][1] = TMP[i][1] * DBEAM[i][0] + TMP[i][0] * DBEAM[i][1];
        TMP[i][0] = value;
	}
    fftw_execute(p_TMP_imap);
//@	rfftwnd_one_complex_to_real(pinv, TMP, imap.r8data);
	if (!imap.reass()) return FALSE;
	sum = imap.get_flux();
	imap *= flux/sum;
	
	dpProgress(-niter, "Lucy Deconvolution");

	for (iter = 0; iter < niter; iter++) {

//	computing ratio map in area where weighting function is unity

		for (ii = 0; ii < imap.n_elements; ii++) {
			if ((w.r4data[ii] < 0.5) || (imap.r8data[ii] < 1e-6))
				imap.r8data[ii] = 1.0;
			else
				imap.r8data[ii] = r4data[ii] / imap.r8data[ii];
		}
		
//	convolving ratio map with dirty beam

        fftw_execute(p_imap_TMP);
//@		rfftwnd_one_real_to_complex(p, imap.r8data, TMP);
		for (i = 0; i < NE; i++) {
            value = TMP[i][0] * DBEAM[i][0] - TMP[i][1] * DBEAM[i][1];
            TMP[i][1] = TMP[i][1] * DBEAM[i][0] + TMP[i][0] * DBEAM[i][1];
            TMP[i][0] = value;
		}
        fftw_execute(p_TMP_imap);
//@		rfftwnd_one_complex_to_real(pinv, TMP, imap.r8data);
		if (!imap.reass()) return FALSE;
//		imap /= (float)(imap.n_elements);
		
//	multiplying current lucy map with convolved ratio map

		sum = 0.0;
		for (ii = 0; ii < xmap.Nelements(); ii++) {
			value = xmap.r8data[ii] * imap.r8data[ii];
			xmap.r8data[ii] = value;
			sum += value;
		}
//		xmap *= imap;
//		sum = xmap.get_flux();
		xmap *= flux/sum;
		
// convolving new iteration with dirty beam

//@		rfftwnd_one_real_to_complex(p, xmap.r8data, TMP);
        fftw_execute(p_xmap_TMP);
		for (i = 0; i < NE; i++) {
            re = TMP[i][0] * DBEAM[i][0] - TMP[i][1] * DBEAM[i][1];
            TMP[i][1] = TMP[i][1] * DBEAM[i][0] + TMP[i][0] * DBEAM[i][1];
            TMP[i][0] = re;
		}
        fftw_execute(p_TMP_imap);
//@		rfftwnd_one_complex_to_real(pinv, TMP, imap.r8data);
		
		if (!imap.reass()) return FALSE;
//		imap /= (float)(imap.n_elements);
		
//	renormalizing reconvolved map to flux of input dirty map
		
		sum = imap.get_flux();
		imap *= flux/sum;
		
		sum = sukn = 0.0;
		for (ii = 0; ii < imap.n_elements; ii++) {
			vvalue = w.r4data[ii];
			value = r4data[ii]-imap.r8data[ii];
			sum += vvalue*value*value;
			sukn += vvalue;
		}
		sum = sqrt(sum/(sukn-1));
		
		dp_output("lucy ===> niter = %li,  sum = %f %f\n", iter+1, sum, sum/thresh);
		if (FitsInterrupt) {
			char msg[256];
			sprintf(msg, "lucy interrupted after %li iterations\n", iter+1);
			dp_output("lucy interrupted after %li iterations\n", iter+1);
			dpProgress(0, msg);
			break;
		} else {
			char msg[256];
			sprintf(msg, "lucy ===> niter = %li,  sum = %f %f\n", iter+1, sum, sum/thresh);
			dpProgress(iter+1, msg);
		}
	}

    fftw_destroy_plan(p_tmp_DBEAM);
    fftw_destroy_plan(p_xmap_TMP);
    fftw_destroy_plan(p_TMP_imap);
    fftw_destroy_plan(p_imap_TMP);
//@	rfftwnd_destroy_plan(p);
//@	rfftwnd_destroy_plan(pinv);
    fftw_free(tmp);
    fftw_free(DBEAM);
	*this = xmap;
	setType(R4);

	return TRUE;
}

void Fits::clean(Fits &dbeam, int niter, float gain, Fits *s) {
	Fits submap, deltamap, psf;
	int i, xmax, ymax, psfmaxx, psfmaxy;
	float flux, max;

	setType(R4);
	if (s != NULL) {
		s->copy(*this);
	} else {
		submap.copy(*this);
	}
/*	dbeam.limits(&xlow, &xhigh, &ylow, &yhigh);
	dbeam.extractRange(psf, xlow, xhigh, ylow, yhigh, 1, 1);*/
	psf.copy(dbeam);
	psf.setType(R4);
	psf.norm();
	psf *= gain;
	flux = psf.get_flux();
	psf.max_pos(&psfmaxx, &psfmaxy);

	*this = 0.0;

	for (i = 0; i < niter; i++) {
		if (s != NULL)
			max = s->max_pos(&xmax, &ymax);
		else
			max = submap.max_pos(&xmax, &ymax);
		setValue(ValueAt(F_I(xmax, ymax)) + flux * max, xmax, ymax);
/*		xlow = xmax - psfmaxx + 1;
		xhigh = xlow + psf.Naxis(1) - 1;
		ylow = ymax - psfmaxy + 1;
		yhigh = ylow + psf.Naxis(2) - 1;
		n = 0;
		for (x = xlow; x <= xhigh; x++) {
			for (y = ylow; y <= yhigh; y++) {
				if ((x > 0) && (x <= Naxis(1)) && (y > 0) && (y <= Naxis(2))) {
					if (s != NULL) s->r4data[s->F_I(x, y)] -= psf.r4data[n] * max;
					else submap.r4data[submap.F_I(x, y)] -= psf.r4data[n] * max;
				}
				n++;
			}
		}*/
		psf.copy(dbeam);
		psf.setType(R4);
		psf.norm();
		psf *= gain * max;
		psf.shift(xmax - psfmaxx, ymax - psfmaxy, 2);
		if (s != NULL) *s -= psf;
		else submap -= psf;

		dp_output("clean: niter = %5i: max = %f at [%i,%i]\n", i+1, max, xmax, ymax);
	}
}

double Fits::ap_phot(int x, int y, int radius, int find_max, int search_box) {
	int i, j;
	int true_x = x, true_y = y;
	double rv = 0.0, rr = (double)(radius * radius);
	
	if ((x < 1) || (y < 1) || (x > (int)naxis[1]) || (y > (int)naxis[2])) return 0.0;
	if (find_max == 1) {
		double max = ValueAt(F_I(true_x, true_y));
		for (i = x-search_box; i <= x+search_box; i++) {
			for (j = y-search_box; j <= y+search_box; j++) {
				if ((i > 0) && (i <= (int)naxis[1]) && (j > 0) && (j <= (int)naxis[2])) {
					if (ValueAt(F_I(i, j)) > max) {
						true_x = i;
						true_y = j;
						max = ValueAt(F_I(i, j));
					}
				}
			}
		}
	}
	for (i = true_x - radius; i <= true_x + radius; i++) {
		for (j = true_y - radius; j <= true_y + radius; j++) {
			if ((i > 0) && (i <= (int)naxis[1]) && (j > 0) && (j <= (int)naxis[2])) {
				if ((double)((i-true_x)*(i-true_x)) + (double)((j-true_y)*(j-true_y)) <= rr) {
					rv += ValueAt(F_I(i, j));
				}
			}
		}
	}
	return(rv);
}

bool Fits::maxEntropy(const Fits & dbeam, int niter, float thresh) {
	Fits rim, newim, transpsf, lambda, psf, _tmp;
	double /*S,*/ error, scale;
	int n, x, y;

	clip(1e-6, -99.);

	if (!psf.copy(dbeam)) return FALSE;
	if (!psf.setType(R8)) return FALSE;
	psf.norf(0);
	
	if (!transpsf.copy(psf)) return FALSE;
	transpsf.rot90(90);
	transpsf.flip(1);
	transpsf.max_pos(&x, &y);
	transpsf.shift(transpsf.Naxis(1) / 2 + 1 - x, transpsf.Naxis(2) / 2 + 1 - y, 2);

	if (!lambda.copy(*this)) return FALSE;
	if (!lambda.setType(R8)) return FALSE;
	lambda = 0.0;
	
	for (n = 0; n < niter; n++) {
		if (!rim.copy(transpsf)) return FALSE;
		if (!rim.convolve(lambda)) return FALSE;
		rim.Exp();
		rim.norf(0);
		
		if (!newim.copy(psf)) return FALSE;
		if (!newim.convolve(rim)) return FALSE;
		scale = get_flux() / newim.get_flux();
		
		if (!_tmp.copy(*this)) return FALSE;
		_tmp /= scale;
		_tmp -= newim;
		_tmp.Abs();
		error = _tmp.get_flux();
		
		newim.clip(1e-15, -99.);
		newim.Ln();
		if (!_tmp.copy(*this)) return FALSE;
		_tmp /= scale;
		_tmp.clip(1e-15, -99.);
		_tmp.Ln();
		lambda += _tmp;
		lambda -= newim;
		
		if (FitsInterrupt) {
			dp_output("maxentropy interrupted after %li iterations\n", n);
			break;
		}
		dp_output("Maximum Entropy: niter = %7i, error = %f\n", n, error);
	}
	if (!copy(rim)) return FALSE;
	mul(scale);

	return TRUE;
}

void Fits::multiple_phot(int *x, int *y, int radius, int n, double *pdata, int find_max, int search_box) {
	int i;
	
	for (i = 0; i < n; i++) {
		pdata[i] = ap_phot(x[i], y[i], radius, find_max, search_box);
	}
}

bool Fits::multiple_phot(Fits &positions, int radius, Fits *result, int find_max, int search_box) {
	int i;

	if (positions.Naxis(1) != 2) {
		dp_output("Fits::multiple_phot: positions must have NAXIS1 == 2\n");
		return FALSE;
	}
	if (!result->create(1, positions.Naxis(2), R8)) return FALSE;
	for (i = 0; i < positions.Naxis(2); i++) {
		result->r8data[i] = ap_phot(nint(positions.ValueAt(positions.C_I(0, i))), nint(positions.ValueAt(positions.C_I(1, i))), radius, find_max, search_box);
	}
	return TRUE;
}

inline void smooth1d_helper(float *r4data, double *a, fftw_complex *A, fftw_complex *B, fftw_plan *pa, fftw_plan *pA, long z1, long N, long inc) {
    double re;
    long z, i;

    for (z = 0; z < N; z++) a[z] = r4data[z1 + z*inc];
    fftw_execute(*pa);
//@    rfftwnd_one_real_to_complex(*p, a, NULL);
    for (i = 0; i < 1 * (N / 2 + 1); i++) {
        re = A[i][0] * B[i][0] - A[i][1] * B[i][1];
        A[i][1] = A[i][1] * B[i][0] + A[i][0] * B[i][1];
        A[i][0] = re;
    }
    fftw_execute(*pA);
//@    rfftwnd_one_complex_to_real(*pinv, A, NULL);
    for (z = 0; z < N; z++) r4data[z1 + z*inc] = a[z]; // gausstotal;
}

bool Fits::smooth1d(float fwhm, int axis) {
    double *a, *b;
    fftw_complex *A, *B;
    fftw_plan pa, pb, pA;
//@    rfftwnd_plan p, pinv;
    long N, M, i, x, y, z, z1, inc;
    double re, gausstotal;
    double sigma, dy2, dx2, d2, xcen, ycen;

    N = Naxis(axis);
    M = 1;

    setType(R4);

    a = (double *)fftw_malloc(M * (N / 2 + 1) * sizeof(fftw_complex));
    b = (double *)fftw_malloc(M * (N / 2 + 1) * sizeof(fftw_complex));
    if ((a == NULL) || (b == NULL)) {
        if (a != NULL) free(a);
        if (b != NULL) free(b);
        dp_output("Fits::smooth1d: Could not allocate memory.\n");
        return FALSE;
    }

    // Create the gaussian
    xcen = (double)(N / 2);
    sigma = -FOUR_LN2 / (fwhm * fwhm);

    gausstotal = 0.0;
    for (x = 0; x < N; x++) {
        dx2 = ((double)x - xcen);
        d2 = exp(sqr(dx2) * sigma);
        gausstotal += d2;
        b[x] = d2;
    }

    A = (fftw_complex *)a;
    B = (fftw_complex *)b;
    pa = fftw_plan_dft_r2c_2d(M, N, a, A, FFTW_ESTIMATE);
    pb = fftw_plan_dft_r2c_2d(M, N, b, B, FFTW_ESTIMATE);
    pA = fftw_plan_dft_c2r_2d(M, N, A, a, FFTW_ESTIMATE);
//    p = rfftw2d_create_plan(M, N, FFTW_REAL_TO_COMPLEX, FFTW_ESTIMATE | FFTW_IN_PLACE);
//    pinv = rfftw2d_create_plan(M, N, FFTW_COMPLEX_TO_REAL, FFTW_ESTIMATE | FFTW_IN_PLACE);

    fftw_execute(pb);
//    rfftwnd_one_real_to_complex(p, b, NULL);
    gausstotal *= (double)N;

    inc = 1;
    if (axis > 1) inc = Naxis(1);
    if (axis > 2) inc *= Naxis(2);

    z = 0;

    switch (axis) {
        case 1:
            for (x = 0; x < Naxis(2); x++) {
                for (y = 0; y < Naxis(3); y++) {
                    z1 = C_I(0, x, y);
                    smooth1d_helper(r4data, a, A, B, &pa, &pA, z1, N, inc);
                }
            }
            wrap(N / 2 + N % 2, 0, 0, FALSE);
            break;
        case 2:
            for (x = 0; x < Naxis(1); x++) {
                for (y = 0; y < Naxis(3); y++) {
                    z1 = C_I(x, 0, y);
                    smooth1d_helper(r4data, a, A, B, &pa, &pA, z1, N, inc);
                }
            }
            wrap(0, N / 2 + N % 2, 0, FALSE);
            break;
        case 3:
            for (x = 0; x < Naxis(1); x++) {
                for (y = 0; y < Naxis(2); y++) {
                    z1 = C_I(x, y, 0);
                    smooth1d_helper(r4data, a, A, B, &pa, &pA, z1, N, inc);
                }
            }
            wrap(0, 0, N / 2 + N % 2, FALSE);
            break;
        default:
            break;
    }

//    rfftwnd_destroy_plan(p);
//    rfftwnd_destroy_plan(pinv);
    fftw_destroy_plan(pa);
    fftw_destroy_plan(pb);
    fftw_destroy_plan(pA);
    fftw_free(a);
    fftw_free(b);

    div(gausstotal);

    return TRUE;
}

bool Fits::smooth(double fwhm, bool toR8) {
    double *a, *b;
	fftw_complex *A, *B;
    fftw_plan pa, pb, pinv;
	long M, N, i, x, y, z, z1, inc;
	double re, gausstotal;
	double sigma, dy2, dx2, d2, xcen, ycen;

	N = Naxis(1);
	M = Naxis(2);
	inc = (N-1) % 2 + 1;

	if (toR8)
		setType(R8);
	else
		setType(R4);

    a = (double *)fftw_malloc(M * (N / 2 + 1) * sizeof(fftw_complex));
    b = (double *)fftw_malloc(M * (N / 2 + 1) * sizeof(fftw_complex));
	if ((a == NULL) || (b == NULL)) {
		if (a != NULL) free(a);
		if (b != NULL) free(b);
		dp_output("Fits::smooth: Could not allocate memory.\n");
		return FALSE;
	}

// Create the gaussian
	xcen = (double)(naxis[1] / 2);
	ycen = (double)(naxis[2] / 2);
	sigma = -FOUR_LN2 / (fwhm * fwhm);

	i = 0;
	gausstotal = 0.0;
	for (y = 0; y < M; y++) {
		for (x = 0; x < N; x++) {
			dx2 = ((double)x - xcen);
			dy2 = ((double)y - ycen);
			d2 = exp((sqr(dx2) + sqr(dy2)) * sigma);
			gausstotal += d2;
			b[i] = d2;
			i++;
		}
		i += inc;
	}

	A = (fftw_complex *)a;
	B = (fftw_complex *)b;
    pa = fftw_plan_dft_r2c_2d(M, N, a, A, FFTW_ESTIMATE);
    pb = fftw_plan_dft_r2c_2d(M, N, b, B, FFTW_ESTIMATE);
    pinv = fftw_plan_dft_c2r_2d(M, N, A, a, FFTW_ESTIMATE);

    fftw_execute(pb);
//@	rfftwnd_one_real_to_complex(p, b, NULL);
	gausstotal *= (double)(Naxis(1) * Naxis(2));
	
	z1 = Naxis(3);
	if (z1 < 1) z1 = 1;

	Fits _tmp;
	
	if (toR8)
		_tmp.create(Naxis(1), Naxis(2), R8);
	else
		_tmp.create(Naxis(1), Naxis(2));
	for (z = 0; z < z1; z++) {
		extractRange(_tmp, 1, Naxis(1), 1, Naxis(2), z+1, z+1);
		i = 0;
		for (y = 0; y < M; y++) {
			for (x = 0; x < N; x++) {
				a[i] = _tmp.ValueAt(C_I(x, y));
				i++;
			}
			i += inc;
		}
        fftw_execute(pa);
//@		rfftwnd_one_real_to_complex(p, a, NULL);
		for (i = 0; i < M * (N / 2 + 1); i++) {
            re = A[i][0] * B[i][0] - A[i][1] * B[i][1];
            A[i][1] = A[i][1] * B[i][0] + A[i][0] * B[i][1];
            A[i][0] = re;
		}
        fftw_execute(pinv);
//@		rfftwnd_one_complex_to_real(pinv, A, NULL);

		i = 0;
		for (y = 0; y < M; y++) {
			for (x = 0; x < N; x++) {
				if (toR8)
					_tmp.r8data[_tmp.C_I(x, y)] = a[i] / gausstotal;
				else
					_tmp.r4data[_tmp.C_I(x, y)] = a[i] / gausstotal;
				i++;
			}
			i += inc;
		}
		_tmp.reass();
		setRange(_tmp, 1, Naxis(1), 1, Naxis(2), z+1, z+1);
	}

    fftw_destroy_plan(pa);
    fftw_destroy_plan(pb);
    fftw_destroy_plan(pinv);
//	rfftwnd_destroy_plan(p);
//	rfftwnd_destroy_plan(pinv);
    fftw_free(a);
    fftw_free(b);

	return TRUE;
}

bool Fits::velmap(Fits &cube, double center, double fwhm, double threshold, int method) {
	double centralwave, centralpix, dispersion;
	double median, max, min, cenx, ceny;
	long zc, zw, z1, z2, x, y, z;
	Fits line, err, xvals, estimate, average;
	Fits xvalues, yvalues, errors, fit;
        Fits flippedcube;
	double chi, stddev;
	int xmax, ymax, i, counter;
        bool flipped = FALSE;
        bool logwave = cube.isAxisLog(3);
	
        if (!(cube.Naxis(3) > 3)) {
            dp_output("velmap: 3rd axis must have at least 4 elements");
            return FALSE;
        }

        if (cube.getCDELT(3) < 0.0) {
            if (!flippedcube.copy(cube)) return FALSE;
            flippedcube.flip(3);
            centralwave = flippedcube.getCRVAL(3);
            centralpix = flippedcube.getCRPIX(3);
            dispersion = flippedcube.getCDELT(3);
            flipped = TRUE;
        } else {
            centralwave = cube.getCRVAL(3);
            centralpix = cube.getCRPIX(3);
            dispersion = cube.getCDELT(3);
        }
//	if (!cube.GetFloatKey("CRVAL3", &centralwave)) return FALSE;
//	if (!cube.GetFloatKey("CRPIX3", &centralpix)) return FALSE;
//	if (!cube.GetFloatKey("CDELT3", &dispersion)) return FALSE;

	dpProgress(-cube.Naxis(1), "Creating Velocity Map...");
//	zc = nint((center-centralwave)/dispersion + centralpix);
    zc = wavepix(center, centralpix, centralwave, dispersion, logwave);
	zw = nint(fwhm / dispersion * 5.);
	dp_debug("zc = %li zw = %li\n", zc, zw);
	z1 = zc - zw;
	if (z1 < 1) z1 = 1;
	z2 = zc + zw;
        if (z2 - z1 < 4) z2 = z1 + 4;
	if (z2 > cube.Naxis(3)) z2 = cube.Naxis(3);
        if (z2 - z1 < 4) {
            dp_output("velmap: input parameters for central wavelength and FWHM results in too narrow slices");
            return FALSE;
        }

        if (flipped) {
            flippedcube.extractRange(line, 1, 1, 1, 1, z1, z2);
        } else {
            cube.extractRange(line, 1, 1, 1, 1, z1, z2);
        }
        centralwave = line.getCRVAL(1);
        centralpix = line.getCRPIX(1);
//	dispersion = line.getCDELT(1);
//	if (!line.GetFloatKey("CRVAL1", &centralwave)) return FALSE;
//	if (!line.GetFloatKey("CRPIX1", &centralpix)) return FALSE;
//	if (!line.GetFloatKey("CDELT1", &dispersion)) return FALSE;
	
	create(cube.Naxis(1), cube.Naxis(2), 9, R8);
        CopyHeader(cube);
        DeleteKey("CRVAL3");
        DeleteKey("CDELT3");
        DeleteKey("CRPIX3");
        DeleteKey("CTYPE3");
        DeleteKey("CD3_3");
	err.create(line.Naxis(1), 1, R8);
	err += 1.;
	xvals.create(line.Naxis(1), 1, R8);
	for (x = 1; x <= xvals.Nelements(); x++) {
        xvals.r8data[x-1] = pixwave(x, centralpix, centralwave, dispersion, logwave); //centralwave + ((double)x - centralpix) * dispersion;
	}

	cube_avg(cube, average);
	if (threshold > 0.0) {
		min = average.get_max() * threshold / 100.;
	} else {
		min = average.get_min() - 1.0;
	}
	dp_debug("threshold is %f\n", min);
	estimate.create(8, 1, R8);
	for (x=1; x <= cube.Naxis(1); x++) {
		char msg[256];
		sprintf(msg, "velmap: at row %i", x);
		dpProgress(x, msg);
		if (FitsInterrupt) {
			sprintf(msg, "velmap interrupted after %i rows", x);
			dpProgress(0, msg);
			return TRUE;
		}
		for (y=1; y <= cube.Naxis(2); y++) {
			if (average.ValueAt(average.F_I(x, y)) > min) {
                            if (flipped) {
                                flippedcube.extractRange(line, x, x, y, y, z1, z2);
                            } else {
				cube.extractRange(line, x, x, y, y, z1, z2);
                            }
				switch (method) {
					case 0: // gaussfit
						median = line.get_median();
						estimate.r8data[0] = median;
						max = line.max_pos(&xmax, &ymax);
						estimate.r8data[1] = max - median;
                        estimate.r8data[2] = pixwave(xmax, centralpix, centralwave, dispersion, logwave); //centralwave + ((double)xmax - centralpix) * dispersion;
						estimate.r8data[3] = fwhm;
						chi = gaussfit(estimate, xvals, line, err);
						for (z = 1; z <= 8; z++) {
							r8data[F_I(x, y, z)] = estimate.r8data[z-1];
						}
						r8data[F_I(x, y, 9)] = chi;
						break;
					case 1: // centroid
						line.setType(R8);
						median = line.get_median();
						r8data[F_I(x, y, 1)] = median;
						max = line.max_pos(&xmax, &ymax);
						r8data[F_I(x, y, 2)] = max - median;
						line.centroid(&cenx, &ceny);
                        r8data[F_I(x, y, 3)] = pixwave(cenx, centralpix, centralwave, dispersion, logwave); //centralwave + (cenx - centralpix) * dispersion;
						r8data[F_I(x, y, 4)] = line.get_fwhm((int)(cenx + .5), 1, (z2-z1) / 2);

/* Now fit a straight line to the profile, neglecting the region around xmax - fwhm to xmax + fwhm */
						xvalues.create(line.Nelements(), 1, R8);
						yvalues.create(line.Nelements(), 1, R8);
						counter = 0;
						for (i = 0; i < line.Nelements(); i++) {
							if ((i < xmax - fwhm/dispersion) || (i > xmax + fwhm/dispersion)) {
								xvalues.r8data[counter] = i;
								yvalues.r8data[counter] = line[i];
								counter++;
							}
						}
						if (counter > 5) {
							xvalues.resize(counter);
							yvalues.resize(counter);
							errors.create(counter, 1, R8);
							errors += 1.0;
							polyfit1d(fit, xvalues, yvalues, errors, 1);
							for (i = 0; i < counter; i++) {
								yvalues.r8data[i] -= fit[0] + xvalues[i] * fit[1];
							}
							stddev = yvalues.get_stddev();
							for (i = 0; i < line.Nelements(); i++) {
								line.r8data[i] -= fit[0] + i * fit[1];
								if (line.r8data[i] < stddev) line.r8data[i] = 0.0;
							}

							max = line.max_pos(&xmax, &ymax);
// Calculate centroid
							line.centroid(&cenx, &ceny);
                            r8data[F_I(x, y, 3)] = pixwave(cenx, centralpix, centralwave, dispersion, logwave); //centralwave + (cenx - centralpix) * dispersion;
							r8data[F_I(x, y, 4)] = line.get_fwhm((int)(cenx + .5), 1, (z2-z1) / 2) * dispersion;
						} else {
							r8data[F_I(x, y, 3)] = 0.0;
							r8data[F_I(x, y, 4)] = 0.0;
						}
						break;
					default:
						break;
				}
			}
		}
	    dp_output("velmap: at row %i\n", x);
	}
	return TRUE;
}
