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

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 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;
            }
        }
    }
}

void funcs(double x, double afunc[], int ma)
{
	int i;
    afunc[1] = 1.0;
    for (i = 2; i<=ma; i++)
	{
        afunc[i] = x * afunc[i - 1];
    }
}

void lfit(double x[], double y[], double sig[], int ndata, double a[],int ma,
		  int lista[], int mfit, double covar[], int ncvm, double& chisq)
{
	int i,j,k,kk,ihit;
	double ym,sig2i,wt,sum;
    double beta[51], afunc[51];
    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 set in lista"<<endl;
			}
        }
    }
    if (kk != (ma + 1))
	{
		cout<<" improper set in lista"<<endl;
	}
    for (j = 1; j<=mfit; j++)
	{
        for (k = 1; k<=mfit; k++)
		{
            covar[(j-1)*ma+k] = 0.0;
        }
        beta[j] = 0.0;
    }
    for (i = 1; i<=ndata; i++)
	{
        funcs(x[i], afunc, ma);
        ym = y[i];
        if (mfit < ma)
		{
            for (j = mfit + 1; j<=ma; j++)
			{
                ym = ym - a[lista[j]] * afunc[lista[j]];
            }
        }
        sig2i = 1.0 / (sig[i] * sig[i]);
        for (j = 1; j<=mfit; j++)
		{
            wt = afunc[lista[j]] * sig2i;
            for (k = 1; k<=j; k++)
			{
                covar[(j-1)*ma+k] = covar[(j-1)*ma+k] + wt * afunc[lista[k]];
			}
            beta[j] = beta[j] + ym * wt;
        }
    }
    if (mfit > 1)
	{
        for (j = 2; j<=mfit; j++)
		{
            for (k = 1; k<=j - 1; k++)
			{
                covar[(k-1)*ma+j] = covar[(j-1)*ma+k];
            }
        }
    }
    gaussj(covar, mfit,ma,beta);
    for (j = 1; j<=mfit; j++)
	{
        a[lista[j]] = beta[j];
    }
    chisq = 0.0;
    for (i = 1; i<=ndata; i++)
	{
        funcs(x[i], afunc, ma);
        sum = 0.0;
        for (j = 1; j<=ma; j++)
		{
            sum = sum + a[j] * afunc[j];
        }
        chisq = chisq + ((y[i] - sum) / sig[i]) * ((y[i] - sum) / sig[i]);
    }
    covsrt(covar, ncvm, ma, lista, mfit);
}

void main()
{
    //program d9r2
    //driver for routine lfit
    int i,j,npt = 100;
    double chisq,spread = 0.1;
    int mfit,nterm = 3;
    double x[101], y[101], sig[101], a[4], covar[10];
	int lista[4];
    long idum = -911;
    for (i = 1; i<=npt; i++)
	{
        x[i] = 0.1 * i;
        y[i] = nterm;
        for (j = nterm - 1; j>=1; j--)
		{
            y[i] = j + y[i] * x[i];
        }
        y[i] = y[i] + spread * gasdev(idum);
        sig[i] = spread;
    }
    mfit = nterm;
    for (i = 1; i<=mfit; i++)
	{
        lista[i] = i;
    }
    lfit(x, y, sig, npt, a, nterm, lista, mfit, covar, nterm, chisq);
    cout<<"Parameter               Uncertainty"<<endl;
	cout.setf(ios::scientific|ios::left);
	cout.precision(4);
    for (i = 1; i<=nterm; i++)
	{
        cout<<"a("<<i<<")= ";
		cout.width(18);
		cout<<a[i];
		cout.width(18);
        cout<<sqrt(covar[(i-1)*nterm+i]);
		cout<<endl;
    }
    cout<<endl;
    cout<<"Chi-squared =  "<<chisq<<endl;
    cout<<endl;
    cout<<"Full covariance matrix"<<endl;
    for (i = 1; i<=nterm; i++)
	{
        for (j = 1; j<=nterm; j++)
		{
			cout.width(18);
            cout<<covar[(i-1)*nterm+j];
        }
		cout<<endl;
    }
    //now test the lista feature
    for (i = 1; i<=nterm; i++)
	{
        lista[i] = nterm + 1 - i;
    }
    lfit(x, y, sig, npt, a, nterm, lista, mfit, covar, nterm, chisq);
    cout<<endl;
    cout<<"Parameter               Uncertainty"<<endl;
    for (i = 1; i<=nterm; i++)
	{
        cout<<"a("<<i<<")= ";
		cout.width(18);
		cout<<a[i];
		cout.width(18);
		cout<<sqrt(covar[(i-1)*nterm+i])<<endl;
    }
    cout<<endl;
    cout<<"Chi-squared =  "<<chisq<<endl;
    cout<<endl;
    cout<<"Full covariance matrix"<<endl;
    for (i = 1; i<=nterm; i++)
	{
        for (j = 1; j<=nterm; j++)
		{
			cout.width(18);
            cout<<covar[(i-1)*nterm+j];
        }
		cout<<endl;
    }
    //now check results of restricting fit parameters
    int ii = 1;
	int aaa;
    for (i = 1; i<=nterm; i++)
	{
        aaa = i - int(i / 2) * 2;
        if (aaa == 1)
		{
            lista[ii] = i;
            ii = ii + 1;
        }
    }
    mfit = ii - 1;
    lfit(x, y, sig, npt, a, nterm, lista, mfit, covar, nterm, chisq);
    cout<<endl;
    cout<<"Parameter               Uncertainty"<<endl;
    for (i = 1; i<=nterm; i++)
	{
        cout<<"a("<<i<<")= ";
		cout.width(18);
		cout<<a[i];
		cout.width(18);
		cout<<sqrt(covar[(i-1)*nterm+i])<<endl;
    }
    cout<<endl;
    cout<<"Chi-squared =  "<<chisq<<endl;
    cout<<endl;
    cout<<"Full covariance matrix"<<endl;
    for (i = 1; i<=nterm; i++)
	{
        for (j = 1; j<=nterm; j++)
		{
			cout.width(18);
            cout<<covar[(i-1)*nterm+j];
        }
		cout<<endl;
    }
}