#include <math.h>
#include <stdio.h>
#include "nrutil.h"
#include "structs.h"
#include "debug.h"
#include "mymath.h"
#include "const.h"
#define NRANSI

void writefits(char *phead, struct image *img, char *object, int add);
void mkimg (float **array, long naxes[], char *outname, char *tname);
void mkmodel (struct image *model, struct derivs *df, 
	      struct fitpars *fpar, struct convpars *cpar, 
	      struct cons *constr, int output);
int istrunc (char *objtype);

void mrqcof(float **y, float **sig, struct image *psf, float a[], 
	int ia[], int ma, double **alpha, double beta[], double *chisq,
	void (*funcs)(int x, int y, float *, float [], int [], int, 
	struct fitpars *, struct derivs *df), struct fitpars *fpar, 
	struct convpars *cpar, struct cons *constr)
{

	extern struct image *mask;
        extern struct sampars sample;
        extern struct image model;
        extern unsigned long *pos;
        extern struct derivs *df;

	struct fitpars *fptr;
	struct derivs *dptr;
        char name[80];

	int ix, iy, j,k,l,m,mfit=0;
	float ymod,wt,sig2i,dy,*dyda;

	dyda=vector(1,ma);
	for (j=1;j<=ma;j++)
		if (ia[j]==1) mfit++;
	for (j=1;j<=mfit;j++) {
		for (k=1;k<=j;k++) alpha[j][k]=0.0;
		beta[j]=0.0;
	}

	mkmodel (&model, df, fpar, cpar, constr, 0);

#if CKIMG 

    sprintf (model.name, "model.fits");            /*  Output image  */
    writefits ("model.fits", &model, "model", 0);   

    fptr = fpar;
    dptr = df;

    while (fptr != NULL) { 
        for (j=0; j<= fptr->npars; j++){
            if (fptr->ia[j] == 1 || j==0) {
                sprintf (name, "deriv%d.fits", j);
                mkimg (dptr->dpm[j], dptr->naxes, name, "small.fits"); 
            };
        };
        fptr = fptr->next;
        dptr = dptr->next;
    };

#endif

	sample.nmask = 0;
	*chisq=0.0;

	for (iy=1; iy<= model.naxes[2]; iy++) {
	    for (ix=1;ix<=model.naxes[1];ix++) {
	        if (mask->z[iy][ix] < 1.) {        /* Calculate chi^2 only   */
						   /* for unflagged pixels   */
		    (*funcs)(ix,iy,&ymod,dyda,ia,ma,fpar,df); 
		    sig2i=1.0/(sig[iy][ix]*sig[iy][ix]); 
		    dy=y[iy][ix]-ymod;
		    for (j=0,l=1;l<=ma;l++) {
			if (ia[l]==1) {
			    wt=dyda[l]*sig2i;
			    for (j++,k=0,m=1;m<=l;m++)
				if (ia[m]==1) alpha[j][++k] += wt*dyda[m]; 
			    beta[j] += dy*wt;
			};
		    };
		    *chisq += dy*dy*sig2i;
	        } else 
		    sample.nmask++;	   /* count up the number of flagged pixels */
	    };
	};

	for (j=2;j<=mfit;j++)
		for (k=1;k<j;k++) alpha[k][j]=alpha[j][k];
	free_vector(dyda,1,ma);

}

#undef NRANSI


/* --------------------------------------------------------------------- *\
\* --------------------------------------------------------------------- */


void funcs(int x, int y, float *ymodel, float dy[], int ia[], int ma,
    struct fitpars *fpar, struct derivs *df)
{
    int i;
    static int k=0, high = 0;
    extern struct image model;

    if (fpar != NULL) {
	for (i=1; i <= fpar->npars; ++i){
	    if (high == 0 || 
		(high == 1 && !istrunc (fpar->objtype))) {

	        k++;

                if (ia[k] == 1) 
	            dy[k] = df->dpm[i][y][x];
		
	    };
	};

        /* Go into high order list if high == 1 */

        if (high == 1 && fpar->next != NULL)  
            funcs (x, y, ymodel, dy, ia, ma, fpar->next, df->next);

        /*  Check to see if there are high order terms  */

        else if (fpar->high != NULL && high == 0) { 
            high = 1;
            funcs (x, y, ymodel, dy, ia, ma, fpar->high, df->high);
            high = 0;    /*  keep track of when we pop out of this list */
        } ;

        /*  No high order terms or just exiting high order terms  */
        if (high == 0 && fpar->next != NULL)
            funcs (x, y, ymodel, dy, ia, ma, fpar->next, df->next);

    };

    /*  Done, now exit  */

    if (high == 0 && fpar->next == NULL) {
        k = 0;
        *ymodel = model.z[y][x];
    };

}