#include "iostream.h"
#include "math.h"
#include "stdlib.h"
#include "conio.h"

void gaussj(double a[], int n, int ma,double b[])
{
	int i,j,k,l,ll,irow,icol;
	double big,pivinv,dum;
    int ipiv[51], indxr[51], indxc[51];
    for (j=1;j<=n;j++)
	{
        ipiv[j]=0;
    }
    for (i=1;i<=n;i++)
	{
        big=0.0;
        for (j=1;j<=n;j++)
		{
            if(ipiv[j]!=1)
			{
                for(k=1;k<=n;k++)
				{
					if(ipiv[k]==0)
					{
						if(fabs(a[(j-1)*ma+k])>=big)
						{
							big=fabs(a[(j-1)*ma+k]);
							irow=j;
							icol=k;
						}
						else if(ipiv[k]>1)
						{
							cout<<"singular matrix";
						}
					}
                }
            }
        }
        ipiv[icol]=ipiv[icol]+1;
        if(irow!=icol)
		{
            for(l=1;l<=n;l++)
			{
                dum=(a[(irow-1)*ma+l]);
                a[(irow-1)*ma+l]=a[(icol-1)*ma+l];
                a[(icol-1)*ma+l]=dum;
            }
            dum=b[irow];
            b[irow]=b[icol];
            b[icol]=dum;
		}
        indxr[i]=irow;
        indxc[i]=icol;
        if(a[(icol-1)*ma+icol]==0.0)
		{
			cout<< "singular matrix.";
		}
        pivinv=1.0/(a[(icol-1)*ma+icol]);
        a[(icol-1)*ma+icol]=1.0;
        for(l=1;l<=n;l++)
		{
            a[(icol-1)*ma+l]=a[(icol-1)*ma+l]*pivinv;
        }
        b[icol]=b[icol]*pivinv;
        for(ll=1;ll<=n;ll++)
        {
            if(ll!=icol)
			{
                dum=a[(ll-1)*ma+icol];
                a[(ll-1)*ma+icol]=0.0;
                for(l=1;l<=n;l++)
				{
                    a[(ll-1)*ma+l]=a[(ll-1)*ma+l]-a[(icol-1)*ma+l]*dum;
                }
                b[ll]=b[ll]-b[icol]*dum;
            }
        }
	}	
    for(l=n;l>=1;l--)
	{
        if(indxr[l]!=indxc[l])
		{
            for(k=1;k<=n;k++)
			{
                dum=a[(k-1)*ma+indxr[l]];
                a[(k-1)*ma+indxr[l]]=a[(k-1)*ma+indxc[l]];
                a[(k-1)*ma+indxr[l]]=dum;
            }
        }
    }
}

double ran1(long& idum)
{
    int j,iff=-1;
	static long ix1,ix2,ix3;
	static double r[98];
    long m1 = 259200; long m2 = 134456; long m3 = 243000;
	long ia1 = 7141; long ia2 = 8121; long ia3 = 4561;
	long ic1 = 54773; long ic2 = 28411; long ic3 = 51349;
	double rm1 = 0.0000038580247; double rm2 = 0.0000074373773;    
    if (idum < 0 || iff == 0)
	{
        iff = 1;
        ix1 = (ic1 - idum) % m1;
        ix1 = (ia1 * ix1 + ic1) % m1;
        ix2 = ix1 % m2;
        ix1 = (ia1 * ix1 + ic1) % m1;
        ix3 = ix1 % m3;
        for (j = 1; j<=97; j++)
		{
            ix1 = (ia1 * ix1 + ic1) % m1;
            ix2 = (ia2 * ix2 + ic2) % m2;
            r[j] = (double(ix1) + double(ix2) * rm2) * rm1;
        }
        idum = 1;
    }
    ix1 = (ia1 * ix1 + ic1) % m1;
    ix2 = (ia2 * ix2 + ic2) % m2;
    ix3 = (ia3 * ix3 + ic3) % m3;
    j = 1 + int((97 * ix3) / m3);
    if (j > 97 || j < 1)
	{
		cout<<"abnormal exit in ran1"<<endl;
		exit(1);
	}
	double temp=r[j];
    r[j] = (double(ix1) + double(ix2) * rm2) * rm1;
	return temp;
}

double gasdev(long& idum)
{
    static int iset;
	static double gset;
	double v1,v2,r,fac;
    if (iset == 0)
	{
		do
		{
			v1 = 2.0 * ran1(idum) - 1.0;
			v2 = 2.0 * ran1(idum) - 1.0;
			r = v1 * v1 + v2 * v2;
		}while (r >= 1.0 || r == 0);
		fac = sqrt(-2.0 * log(r) / r);
		gset = v1 * fac;
		iset = 1;
		return v2 * fac;
	}
    else
	{
		iset = 0;
		return gset;
    }
}

void covsrt(double covar[], int ncvm, int ma, int lista[], int mfit)
{
	int i,j;
	double swap;
	for (j = 1; j<=ma - 1; j++)
	{
		for (i = j + 1; i<=ma; i++)
		{
			covar[(i-1)*ma+j] = 0.0;
		}
    }
    for (i = 1; i<=mfit - 1; i++)
	{
        for (j = i + 1; j<=mfit; j++)
		{
            if (lista[j] > lista[i])
			{
                covar[(lista[j]-1)*ma+lista[i]] = covar[(i-1)*ma+j];
			}
            else
			{
               covar[(lista[i]-1)*ma+lista[j]] = covar[(i-1)*ma+j];
            }
        }
    }
    swap = covar[1];;
    for (j = 1; j<=ma; j++)
	{
        covar[j] = covar[(j-1)*ma+j];
        covar[(j-1)*ma+j] = 0.0;
    }
    covar[(lista[1]-1)*ma+lista[1]] = swap;
    for (j = 2; j<=mfit; j++)
	{
        covar[(lista[j]-1)*ma+lista[j]] = covar[j];
    }
    for (j = 2; j<=ma; j++)
	{
        for (i = 1; i<=j - 1; i++)
		{
            covar[(i-1)*ma+j] = covar[(j-1)*ma+i];
		}
	}
}

void fgauss(double x, double a[], double& y, double dyda[], int na)
{
	y = 0.0;
	for (int i = 1; i<=na - 1; i=i+3)
	{
		double arg = (x - a[i + 1]) / a[i + 2];
		double ex = exp(-(arg * arg));
		double fac = a[i] * ex * 2.0 * arg;
		y = y + a[i] * ex;
		dyda[i] = ex;
		dyda[i + 1] = fac / a[i + 2];
		dyda[i + 2] = fac * arg / a[i + 2];
	}
}

void mrqcof(double x[], double y[], double sig[], int ndata, double a[],
			int ma, int lista[], int mfit, double alpha[], double beta[],
			int nalp, double& chisq)
{
	int i,j,k;
	double wt,ymod,sig2i,dy;
    double  dyda[21];
    for (j = 1; j<=mfit; j++)
	{
        for (k = 1; k<=j; k++)
		{
            alpha[(j-1)*nalp+k] = 0.0;
        }
        beta[j] = 0.0;
    }
    chisq = 0.0;
    for (i = 1; i<=ndata; i++)
	{
        fgauss(x[i], a, ymod, dyda, ma);
        sig2i = 1.0 / (sig[i] * sig[i]);
        dy = y[i] - ymod;
		for (j = 1; j<=mfit; j++)
		{
            wt = dyda[lista[j]] * sig2i;
			for (k = 1; k<=j; k++)
			{
                alpha[(j-1)*nalp+k] = alpha[(j-1)*nalp+k] + wt * dyda[lista[k]];
            }
            beta[j] = beta[j] + dy * wt;
        }
        chisq = chisq + dy * dy * sig2i;
    }
    for (j = 2; j<=mfit; j++)
	{
        for (k = 1; k<=j - 1; k++)
		{
            alpha[(k-1)*nalp+j] = alpha[(j-1)*nalp+k];
        }
    }
}

void mrqmin(double x[], double y[], double sig[], int ndata, double a[],
			int ma, int lista[], int mfit, double covar[], double alpha[],
			int nca, double& chisq, double& alamda)
{
	static kk,i,j,k,ihit;
    static double atry[21], beta[21], da[21];
	static double ochisq;
    if (alamda < 0.0)
	{
        kk = mfit + 1;
        for (j = 1; j<=ma; j++)
		{
            ihit = 0;
            for (k = 1; k<=mfit; k++)
			{
                if (lista[k] == j)
				{
					ihit = ihit + 1;
				}
            }
            if (ihit == 0)
			{
                lista[kk] = j;
                kk = kk + 1;
			}
            else
			{
				if (ihit > 1)
				{
					cout<< "improper permutation in lista"<<endl;
					return;
				}
            }
        }
        if (kk != ma + 1)
		{
			cout<< "improper permutation in lista"<<endl;
		}
        alamda = 0.001;
        mrqcof(x, y, sig, ndata, a, ma, lista, mfit, alpha, beta, nca, chisq);
        ochisq = chisq;
        for (j = 1; j<=ma; j++)
		{
            atry[j] = a[j];
        }
    }
    for (j = 1; j<=mfit; j++)
	{
        for (k = 1; k<=mfit; k++)
		{
            covar[(j-1)*nca+k] = alpha[(j-1)*nca+k];
        }
        covar[(j-1)*nca+j] = alpha[(j-1)*nca+j] * (1.0 + alamda);
        da[j] = beta[j];
    }
    gaussj(covar, mfit, nca, da);
    if (alamda == 0.0)
	{
        covsrt(covar, nca, ma, lista, mfit);
		for (i=1; i<=20; i++)
		{
			da[i]=0;
			atry[i]=0;
		}
        return;
    }
    for (j = 1; j<=mfit; j++)
	{
        atry[lista[j]] = a[lista[j]] + da[j];
    }
    mrqcof(x, y, sig, ndata, atry, ma, lista, mfit, covar, da, nca, chisq);
    if (chisq < ochisq)
	{
        alamda = 0.1 * alamda;
        ochisq = chisq;
        for (j = 1; j<=mfit; j++)
		{
            for (k = 1; k<=mfit; k++)
			{
                alpha[(j-1)*nca+k] = covar[(j-1)*nca+k];
            }
            beta[j] = da[j];
            a[lista[j]] = atry[lista[j]];
        }
	}
    else
	{
        alamda = 10.0 * alamda;
        chisq = ochisq;
    }
	for (i=1; i<=20; i++)
	{
		da[i]=0;
		atry[i]=0;
	}
}

void main()
{
    //program d9r8a
    //driver for routine mrqmin
    int i,j,npt = 100;
    int mfit,ma = 6;
    double ochisq,chisq,spread = 0.001;
    double x[101], y[101], sig[101], a[7];
	int lista[7];
    double covar[37], alpha[37], gues[7];
    a[1] = 5.0; a[2] = 2.0; a[3] = 3.0; a[4] = 2.0; a[5] = 5.0; a[6] = 3.0;
    gues[1] = 4.5; gues[2] = 2.2; gues[3] = 2.8;
    gues[4] = 2.5; gues[5] = 4.9; gues[6] = 2.8;
    long idum = -911;
    //first try a sum of two gaussians
    for (i = 1; i<=100; i++)
	{
        x[i] = 0.1 * i;
        y[i] = 0.0;
        for (j = 1; j<=4; j=j+3)
		{
            y[i] = y[i] + a[j] * exp(-pow(((x[i] - a[j + 1]) / a[j + 2]), 2));
        }
        y[i] = y[i] * (1.0 + spread * gasdev(idum));
        sig[i] = spread * y[i];
    }
    mfit = ma;
    for (i = 1; i<=mfit; i++)
	{
        lista[i] = i;
    }
    double alamda = -1;
    for (i = 1; i<=ma; i++)
	{
        a[i] = gues[i];
    }
    mrqmin(x, y, sig, npt, a, ma, lista, mfit, covar, alpha, ma, chisq, alamda);
    int k = 1;
    int itst = 0;
	cout.setf(ios::left);
	do
	{
	    cout<<"Iteration #"<<k<<"   Chi-squared: ";
		cout<<chisq;
		cout<<"   alamda: "<<alamda;
		cout<<endl;
		cout<<endl;
		cout<<"  a(1)        a(2)       a(3)       a(4)       a(5)       a(6)"<<endl;
		cout<<endl;
		for (i = 1; i<=ma; i++)
		{
			cout.precision(5);
			cout<<"  ";
			cout.width(9);
			cout<<a[i];
		}
		cout<<endl;
		k = k + 1;
		ochisq = chisq;
		mrqmin(x, y, sig, npt, a, ma, lista, mfit, covar, alpha, ma, chisq, alamda);
		if (chisq > ochisq)
		{
			itst = 0;
		}
		else
		{
			if (fabs(ochisq - chisq) < 0.1)
			{
				itst = itst + 1;
			}
		}
		cout<<"press any key to continue...."<<endl;
		getch();
	}while(itst < 2);
    alamda = 0.0;
    mrqmin(x, y, sig, npt, a, ma, lista, mfit, covar, alpha, ma, chisq, alamda);
    cout<<endl;
	cout<<endl;
	cout<<"Uncertainties:"<<endl;
	cout<<endl;
    for (i = 1; i<=ma; i++)
	{
		cout<<"  ";
		cout.width(9);
		cout.precision(3);
        cout<<sqrt(covar[(i-1)*ma+i]);
    }
	cout<<endl;
	cout<<endl;
    cout<<"Expected results:"<<endl;
	cout<<endl;
    cout<<"  5.0        2.0        3.0        2.0        5.0        3.0"<<endl;
	cout<<endl;
}