/* $Header: https://svn.mpia.de/gulli/TwiceAsNice/branch/unstable/TwiceAsNice/nice/src/libNice/Nice/Alg/Alg.cc 15141 2018-02-21 14:14:57Z mathar $
*/

#include <Nice/Alg/Alg.h>

/**
* @brief linear least-square solver using normal equations
*
* Given a set of data points _x[0..ndat-1], _y[0..ndat-1] with inidividual
* standard deviations sig[0..ndat-1], use chi-squared minimization to fit for some
* or all of the coefficeints a[0..ma-1] of a function that depends linearly on a,
* y=sum_i a_i*afunc_i(x). The boolean input array ia[0..ma-1] indicates by
* true entries those components of a that should be fitted for, and by false
* entries those components that should be held fixed at their input values.
* The program returns values for a[0..ma-1], chi-square=chisq, and the covariance
* matrix covar[0..ma-1][0..ma-1]. (Parameters held fixed will return zero
* covariances.) The user supplies a routine funcs(x,afunc) that returns the
* ma basis functions evaluated at x=x in the array afunc[0..ma-1]).
* (Excerpt of numerical recipies section 15.4)
* @param[in] _x     data points (x)
* @param[in] _y     data points (y)
* @param[in] _sig   standard deviations of the data points
* @param[out] _a     coefficients
* @param[in] _ia    flags for each coefficient. if false, coefficient will be kept constant
* @param[out] _covar covariance matrix will be returned
* @param[out] _chisq chi^2 as return value
* @param[in] _funcs the function that is linear to the coefficients a
*/
void Nice::Alg::linearFit ( boost::numeric::ublas::vector<double> &_x,
                            boost::numeric::ublas::vector<double> &_y,
                            boost::numeric::ublas::vector<double> &_sig,
                            boost::numeric::ublas::vector<double> &_a,
                            boost::numeric::ublas::vector<bool> &_ia,
                            boost::numeric::ublas::matrix<double> &_covar,
                            double &_chisq,
                            void _funcs ( const double, boost::numeric::ublas::vector<double> & ) )
{
    int i,j,k,l,m,mfit=0;
    double ym,wt,sum,sig2i;

    int ndat=(int)_x.size();
    int ma=(int)_a.size();
    boost::numeric::ublas::vector<double> afunc ( ma );
    boost::numeric::ublas::matrix<double> beta ( ma,1 );
    for ( j=0;j<ma;j++ )
        if ( _ia[j] ) mfit++;

    if ( mfit == 0 )
    {
        std::string error ( "Fit parameter are missing." );
        E_LOG ( error );
        E_THROW ( ::Nice::Algorithm::NoFitParamsException, error );
    }

    for ( j=0;j<mfit;j++ )
    {
        for ( k=0;k<mfit;k++ ) _covar ( j,k ) =0.0;
        beta ( j,0 ) =0.0;
    }
    for ( i=0;i<ndat;i++ )
    {
        _funcs ( _x[i],afunc );
        ym=_y[i];
        if ( mfit < ma )
        {
            for ( j=0;j<ma;j++ )
                if ( !_ia[j] ) ym -= _a[j]*afunc[j];
        }
        sig2i=1.0/SQR ( _sig[i] );
        for ( j=0,l=0;l<ma;l++ )
        {
            if ( _ia[l] )
            {
                wt=afunc[l]*sig2i;
                for ( k=0,m=0;m<=l;m++ )
                    if ( _ia[m] ) _covar ( j,k++ ) += wt*afunc[m];
                beta ( j++,0 ) += ym*wt;
            }
        }
    }
    for ( j=1;j<mfit;j++ )
        for ( k=0;k<j;k++ )
            _covar ( k,j ) =_covar ( j,k );
    boost::numeric::ublas::matrix<double> temp ( mfit,mfit );
    for ( j=0;j<mfit;j++ )
        for ( k=0;k<mfit;k++ )
            temp ( j,k ) =_covar ( j,k );
    gaussj ( temp,beta );
    for ( j=0;j<mfit;j++ )
        for ( k=0;k<mfit;k++ )
            _covar ( j,k ) =temp ( j,k );
    for ( j=0,l=0;l<ma;l++ )
        if ( _ia[l] ) _a[l]=beta ( j++,0 );
    _chisq=0.0;
    for ( i=0;i<ndat;i++ )
    {
        _funcs ( _x[i],afunc );
        sum=0.0;
        for ( j=0;j<ma;j++ ) sum += _a[j]*afunc[j];
        _chisq += SQR ( ( _y[i]-sum ) /_sig[i] );
    }
    covsrt ( _covar,_ia,mfit );
} /* linearFit */


/** @brief linear least-square solver using normal equations
  *
  * Given a set of data points _x[0..ndat-1], _y[0..ndat-1] with inidividual
  * standard deviations sig[0..ndat-1], use chi-squared minimization to fit for some
  * or all of the coefficeints a[0..ma-1] of a function that depends linearly on a,
  * y=sum_i a_i*afunc_i(x). The boolean input array ia[0..ma-1] indicates by
  * true entries those components of a that should be fitted for, and by false
  * entries those components that should be held fixed at their input values.
  * The program returns values for a[0..ma-1], chi-square=chisq, and the covariance
  * matrix covar[0..ma-1][0..ma-1]. (Parameters held fixed will return zero
  * covariances.) The user supplies a routine funcs(x,afunc) that returns the
  * ma basis functions evaluated at x=x in the array afunc[0..ma-1]).
  * (Excerpt of numerical recipies section 15.4)
  * @param[in] _x     data points (x)
  * @param[in] _y     data points (y)
  * @param[in] sigmas   standard deviations of the data points
  * @param[out] coeffs     coefficients
  * @param[in] fitCoeffs    flags for each coefficient. if false, coefficient will be kept constant
  * @param[out] _covar covariance matrix will be returned
  * @param[out] _chisq chi^2 as return value
  * @param[in] _funcs the function that is linear to the coefficients a */
void Nice::Alg::shinsLinearFit(boost::numeric::ublas::vector<double> &_x,
                               boost::numeric::ublas::vector<double> &_y,
                               boost::numeric::ublas::vector<double> &sigmas,
                               boost::numeric::ublas::vector<double> &coeffs,
                               boost::numeric::ublas::vector<bool> &fitCoeffs,
                               boost::numeric::ublas::matrix<double> &_covar,
                               double &_chisq,
                               void base_poly_3 ( const double, boost::numeric::ublas::vector<double> & ) )
{
    int i, j, k, l, m, mfit = 0;
    double ym, wt, sum, sig2i;

    int num_dat = (int)_x.size();
    int num_coeff = (int)coeffs.size();
    boost::numeric::ublas::vector<double> afunc(num_coeff);
    boost::numeric::ublas::matrix<double> beta(num_coeff, 1);
    for(j = 0; j < num_coeff; j++)
    {
        if(fitCoeffs.size() > j && fitCoeffs[j])mfit++;
    }

    if(mfit == 0 || _y.size() != num_dat)
    {
        std::string error;
        if(mfit == 0)error = "Fit parameter are missing.";
        else if(_x.size() != _y.size())error = "Vector X ad Y must have the same size.";
        E_LOG ( error );
        E_THROW ( ::Nice::Algorithm::NoFitParamsException, error );
    }

    for(j = 0; j < mfit; j++)
    {
        for(k = 0; k < mfit; k++)
        {
            _covar(j, k) = 0.0;
        }
        beta(j, 0) = 0.0;
    }

    for(i = 0; i < num_dat; i++)
    {
        base_poly_3(_x[i], afunc);
        ym = _y[i];
        if(mfit < num_coeff)
        {
            for(j = 0; j < num_coeff; j++)
            {
                if(!fitCoeffs[j])
                    ym -= coeffs[j]*afunc[j];
            }
        }
        sig2i = 1.0/SQR(sigmas[i]);
        for(j = 0, l = 0; l < num_coeff; l++)
        {
            if(fitCoeffs[l])
            {
                wt = afunc[l]*sig2i;
                for(k = 0 , m = 0; m <= l; m++)
                {
                    if(fitCoeffs[m])
                        _covar(j, k++) += wt*afunc[m];
                }
                beta(j++, 0) += ym*wt;
            }
        }
    }
    for(j = 1; j < mfit; j++)
    {
        for(k = 0; k < j; k++)
        {
            _covar(k, j) = _covar (j, k);
        }
    }
    boost::numeric::ublas::matrix<double> temp(mfit, mfit);
    for(j = 0; j < mfit; j++)
    {
        for(k = 0; k < mfit; k++)
        {
            temp(j, k) = _covar(j, k);
        }
    }
    gaussj(temp, beta);
    for(j = 0; j < mfit; j++)
    {
        for(k = 0; k < mfit; k++)
        {
            _covar( j, k) = temp(j, k);
        }
    }
    for(j = 0, l = 0; l < num_coeff; l++)
    {
        if(fitCoeffs[l])
            coeffs[l] = beta(j++, 0);
    }
    _chisq = 0.0;
    for(i = 0; i < num_dat; i++)
    {
        base_poly_3(_x[i], afunc);
        sum = 0.0;
        for(j = 0; j < num_coeff; j++)
        {
            sum += coeffs[j]*afunc[j];
        }
        _chisq += SQR((_y[i] - sum)/sigmas[i]);
    }
    covsrt(_covar, fitCoeffs, mfit);
} /* shinsLinearFit */

/** @brief weighted cubic least-square solver in closed form
  * Given a set of data points _x[0..ndat-1], _y[0..ndat-1] with inidividual sigmas
  * returns constants A B C D for the best y = A + B*x + C*x^2 + D*x^3 */
int Nice::Alg::shinsFit4(const std::vector<double> & x, const std::vector<double> & y, const std::vector<double> & sigmas,
                         double & AA, double & BB, double & CC, double & DD)
{
    double y1, x1, x2, x3, x4, x5, x6, w0;
    double w = 0, wx = 0, wx2 = 0, wx3 = 0, wx4 = 0, wx5 = 0, wx6 = 0;
    double wy = 0, wxy = 0, wx2y = 0, wx3y = 0;
    double Delta;
    int i;
    if(x.size() != y.size() || x.size() != sigmas.size() || x.size() < 4)return EXIT_FAILURE;
    for(i = 0; i < x.size(); i++)
    {
        x1 = x[i];
        x2 = x1*x1;
        x3 = x2*x1;
        x4 = x3*x1;
        x5 = x4*x1;
        x6 = x5*x1;
        y1 = y[i];
        w0 = 1/(sigmas[i]*sigmas[i]);
        w += w0;
        wx += w0*x1;
        wx2 += w0*x2;
        wx3 += w0*x3;
        wx4 += w0*x4;
        wx5 += w0*x5;
        wx6 += w0*x6;
        wy += w0*y1;
        wxy += w0*x1*y1;
        wx2y += w0*x2*y1;
        wx3y += w0*x3*y1;
    }

    Delta = wx3*wx3*wx3*wx3 + wx2*wx2*wx4*wx4 - w*wx4*wx4*wx4 + wx*wx*wx5*wx5 - wx2*wx2*wx2*wx6 - wx*wx*wx4*wx6 \
            - wx3*wx3*(3*wx2*wx4 + 2*wx*wx5 + w*wx6) + wx2*(-2*wx*wx4*wx5 - w*wx5*wx5 + w*wx4*wx6) + 2*wx3*((wx2*wx2 + \
            w*wx4)*wx5 + wx*(wx4*wx4 + wx2*wx6));

    AA = (wx3*wx3*wx3*wx3y + wx*wx3y*wx4*wx4 - wx*wx2y*wx4*wx5 + wx2*wx2*(wx3y*wx5 - \
    wx2y*wx6) + wx*wx5*wx5*wxy - wx*wx4*wx6*wxy - wx4*wx4*wx4*wy + \
    wx3*(-(wx*wx3y*wx5) + wx*wx2y*wx6 + wx4*wx4*wxy + wx2*(-2*wx3y*wx4 + \
    wx2y*wx5 + wx6*wxy) + 2*wx4*wx5*wy) - wx3*wx3*(wx2y*wx4 + wx5*wxy + \
    wx6*wy) + wx2*(wx2y*wx4*wx4 - wx4*wx5*wxy - wx5*wx5*wy + \
    wx4*wx6*wy))/Delta;

    BB = (wx*wx3*wx3y*wx4 - w*wx3y*wx4*wx4 + w*wx3*wx3y*wx5 + wx2y*(wx3*wx3*wx3 + \
    w*wx4*wx5 + wx*wx2*wx6 - wx3*(wx2*wx4 + wx*wx5 + w*wx6)) - \
    wx3*wx3*wx4*wxy - w*wx5*wx5*wxy + w*wx4*wx6*wxy + wx2*wx2*(wx3y*wx4 - \
    wx6*wxy) + wx3*wx4*wx4*wy - wx3*wx3*wx5*wy + wx*wx5*wx5*wy - wx*wx4*wx6*wy \
    - wx2*(wx3*wx3*wx3y + wx*wx3y*wx5 - 2*wx3*wx5*wxy + wx4*wx5*wy - \
    wx3*wx6*wy))/Delta;

    CC = (w*wx3*wx3y*wx4 - w*wx2y*wx4*wx4 + wx*wx*(wx3y*wx5 - wx2y*wx6) + \
    wx3*wx3*wx3*wxy + w*wx4*wx5*wxy - w*wx3*wx6*wxy - wx3*wx3*wx4*wy + \
    wx2*(-(wx2y*wx3*wx3) - wx*wx3y*wx4 - w*wx3y*wx5 + w*wx2y*wx6 - \
    wx3*wx4*wxy + wx*wx6*wxy + wx4*wx4*wy + wx3*wx5*wy) + wx2*wx2*(wx3*wx3y - \
    wx6*wy) - wx*(wx3*wx3*wx3y - 2*wx2y*wx3*wx4 + wx3*wx5*wxy + wx4*wx5*wy \
    - wx3*wx6*wy))/Delta;

    DD = (-(wx2*wx2*wx2*wx3y) - w*wx3*wx3*wx3y + w*wx2y*wx3*wx4 + wx*wx*(-(wx3y*wx4) + \
    wx2y*wx5) - w*wx4*wx4*wxy + w*wx3*wx5*wxy + wx3*wx3*wx3*wy + wx2*(w*wx3y*wx4 \
    - w*wx2y*wx5 - wx3*wx3*wxy + wx*(2*wx3*wx3y - wx2y*wx4 - wx5*wxy) - \
    2*wx3*wx4*wy) + wx2*wx2*(wx2y*wx3 + wx4*wxy + wx5*wy) + \
    wx*(-(wx2y*wx3*wx3) + wx3*wx4*wxy + wx4*wx4*wy - wx3*wx5*wy))/Delta;

    // std::stringstream incoming_data;
    // for(int ii = 0; ii < x.size(); ii++)
    // {
    //     incoming_data << x[ii] << "\t" <<  y[ii] << "\t" << sigmas[ii] << "\t"; 
    // }
    // U9_LOG(incoming_data.str());
    // U9_LOG(__FUNCTION__ << ": " << AA << "\t" << BB << "\t" << CC << "\t" << DD << "\t" << Delta << "\t" << (int)AA << "\t" << (int)(BB*65536) << "\t" << (int)(CC*2*65536) << "\t" << (int)(DD*6*65536*65536));
    return EXIT_SUCCESS;

} /* shinsFit4 */


/** @brief cubic polynomial passing for first and last point of data
  * returns constants A B given C and D*/
int Nice::Alg::shinsFitF(const std::vector<double> & x, const std::vector<double> & y, int cc, int dd, int & AA, int & BB)
{
    const double encode16 = 65536.;
    const double encode32 = 65536.*65536;
    const double c2 = 1/2.;
    const double c3 = 1/6.;
    double CCC = c2*cc/encode16;
    double DDD = c3*dd/encode32;
    int xsize = (int)x.size();
    double x1 = x[0], x2 = x[xsize - 1];
    double y1 = y[0], y2 = y[xsize - 1];
    double sqx1 = x1*x1;
    double sqx2 = x2*x2;
    AA = (int)((CCC*(-(sqx1*x2) + x1*sqx2))/(-x1 + x2) + (DDD*(-(x1*sqx1*x2) + x1*x2*sqx2))/(-x1 + x2) + (x2*y1 - x1*y2)/(-x1 + x2));
    BB = (int)(((CCC*(sqx1 - sqx2))/(-x1 + x2) + (DDD*(x1*sqx1 - x2*sqx2))/(-x1 + x2) + (-y1 + y2)/(-x1 + x2))*encode16);

    return EXIT_SUCCESS;
}

/** @brief returns the best pos, vel, acc, jerk ready to be passed to MoCon. */
int Nice::Alg::shinsBestFitF(const std::vector<double> & x, const std::vector<double> & y, int & pos, int & vel, int & acc, int & jerk, double & maxdelta)
{
    maxdelta = 0;
    if(x.size() > 3)
    {
        return shinsBestFitF4(x, y, pos, vel, acc, jerk, maxdelta);
    }
    else if(x.size() == 3)
    {
        return shinsBestFitF3(x, y, pos, vel, acc, jerk);
    }
    else if(x.size() == 2)
    {
       return shinsBestFitF2(x, y, pos, vel, acc, jerk);
    }

    return EXIT_FAILURE;
}

/** @brief returns the best pos, vel, acc, jerk ready to be passed to MoCon. */
int Nice::Alg::shinsBestFitF4(const std::vector<double> & x, const std::vector<double> & y, int & pos, int & vel, int & acc, int & jerk, double & maxdelta)
{
    std::vector<double> sigmas(x.size(), 1);
    sigmas[0] = sigmas[x.size() - 1] = 0.01;
//    sigmas[x.size() - 2] = 0.1;
    double AAA, BBB, CCC, DDD;
    shinsFit4(x, y, sigmas, AAA, BBB, CCC, DDD);
    const double encode16 = 65536.;
    const double encode32 = 65536.*65536;
    maxdelta = 10000000.;
    double maxdf = 0;
    const double c2 = 1/2.;
    const double c3 = 1/6.;
    acc = (int)(CCC*encode16/c2);
    jerk = (int)(DDD*encode32/c3);
    if(false && abs(acc) > 100 && abs(jerk) > 300)
    {
        pos = (int)AAA;
        vel = (int)(BBB*encode16);
        AAA = pos;
        BBB = vel/encode16;
        CCC = c2*acc/encode16;
        DDD = c3*jerk/encode32;
        for(int k = 0; k < x.size(); k++)
        {
            double x1 = x[k];
            double sqx = x1*x1;
            double y1 = y[k];
            double df = abs(y1 - (AAA + BBB*x1 + CCC*sqx + DDD*x1*sqx));
            if(df > maxdf)maxdf = df;
        }
        maxdelta = maxdf;
        return EXIT_SUCCESS;
    }
    else
    {
        int wpos, wvel;
        double minchi2 = -1;
        const double c3 = 1/6.;
        const double c2 = 1/2.;
        int bestAcc = 0, bestJerk = 0, bestPos = (int)AAA, bestVel = (int)(BBB*encode16);
        for(int iteration = 0; iteration < 2; iteration++)
        {
            int accmin = acc - ((1 - iteration)*100 + iteration*15);
            int accmax = acc + ((1 - iteration)*100 + iteration*15);
            int dacc = ((1 - iteration)*5 + iteration*1);
            int jerkmin = jerk - ((1 - iteration)*1000 + iteration*50);
            int jerkmax = jerk + ((1 - iteration)*1000 + iteration*50);
            int djerk = ((1 - iteration)*5 + iteration*1);
            for(int wJerk = jerkmin; wJerk <= jerkmax; wJerk += djerk)
            {
                for(int wAcc = accmin; wAcc <= accmax; wAcc += dacc)
                {
                    double chi2 = 0;
                    shinsFitF(x, y, wAcc, wJerk, wpos, wvel);
                    AAA = wpos;
                    BBB = wvel/encode16;
                    CCC = c2*wAcc/encode16;
                    DDD = c3*wJerk/encode32;
                    maxdf = 0;
                    for(int k = 0; k < x.size(); k++)
                    {
                        double x1 = x[k];
                        double sqx = x1*x1;
                        double y1 = y[k];
                        double df = abs(y1 - (AAA + BBB*x1 + CCC*sqx + DDD*x1*sqx));
                        if(df > maxdf)maxdf = df;
                        chi2 += df*df;
                    }
                    if(minchi2 == -1)minchi2 = chi2;
                    if(chi2 < minchi2)
                    {
                        bestAcc = wAcc;
                        bestJerk = wJerk;
                        bestPos = wpos;
                        bestVel = wvel;
                        maxdelta = maxdf;
                        minchi2 = chi2;
                    }
                }
            }
            acc = bestAcc;
            jerk = bestJerk;
        }
        pos = bestPos;
        vel = bestVel;
        return EXIT_SUCCESS;
    }
}

/** @brief To be devoloped. Will return the best pos, vel, acc, jerk ready to be passed to MoCon. */
int Nice::Alg::shinsBestFitF3(const std::vector<double> & x, const std::vector<double> & y, int & pos, int & vel, int & acc, int & jerk)
{
    return EXIT_FAILURE;
}

/** @brief To be devoloped. Will return the best pos, vel, acc, jerk ready to be passed to MoCon. */
int Nice::Alg::shinsBestFitF2(const std::vector<double> & x, const std::vector<double> & y, int & pos, int & vel, int & acc, int & jerk)
{
    return EXIT_FAILURE;
}

/** @brief weighted linear least-square solver in closed form
  * Given a set of data points _x[0..ndat-1], _y[0..ndat-1] with inidividual sigmas
  * returns constants A B for the best y = A + B*x */
int Nice::Alg::shinsFit2(const std::vector<double> & x, const std::vector<double> & y, const std::vector<double> & sigmas,
                                double & M, double & N)
{
    int num_val = (int)x.size();
    double wx = 0, wy = 0, wx2 = 0, wxy = 0, w = 0;

    // y = A + B x
    double A, B, D;

    for(int i = 0; i < num_val; i++)
    {
        double wi = 1/(sigmas[i]*sigmas[i]);
        wx += wi*x[i];
        wy += wi*y[i];
        wx2 += wi*x[i]*x[i];
        wxy += wi*x[i]*y[i];
        w += wi;
    }

    D = w*wx2 - wx*wx;
    A = (wx2*wy - wx*wxy)/D;
    B = (w*wxy - wx*wy)/D;

    double sigmaa, sigmab;
    sigmaa = sqrt(wxy/D);
    sigmab = sqrt(w/D);

    M = B;
    N = A;

    return EXIT_SUCCESS;
} /* shinsLinearFit2 */


/**
* @brief Gauss-Jordan elimination
*
* Used e.g. by lfit to solve for the coefficients (and to find the covariance matrix)
*
* @param _a
* @param _b
*/
void Nice::Alg::gaussj ( boost::numeric::ublas::matrix<double> &a, boost::numeric::ublas::matrix<double> &b )
{
    int i,icol=0,irow=0,j,k,l,ll;
    double big,dum,pivinv;

    int n=(int)a.size1();
    int m=(int)b.size2();
    boost::numeric::ublas::vector<int> indxc ( n ),indxr ( n ),ipiv ( n );
    for ( j=0;j<n;j++ ) ipiv[j]=0;
    for ( i=0;i<n;i++ )
    {
        big=0.0;
        for ( j=0;j<n;j++ )
            if ( ipiv[j] != 1 )
                for ( k=0;k<n;k++ )
                {
                    if ( ipiv[k] == 0 )
                    {
                        if ( fabs ( a ( j,k ) ) >= big )
                        {
                            big=fabs ( a ( j,k ) );
                            irow=j;
                            icol=k;
                        }
                    }
                }
        ++ ( ipiv[icol] );
        if ( irow != icol )
        {
            for ( l=0;l<n;l++ ) SWAP ( a ( irow,l ),a ( icol,l ) );
            for ( l=0;l<m;l++ ) SWAP ( b ( irow,l ),b ( icol,l ) );
        }
        indxr[i]=irow;
        indxc[i]=icol;

        if ( a ( icol,icol ) == 0.0 )
        {
            std::string error ( "gauss j singular matrix exception." );
            E_LOG ( error );
            E_THROW ( ::Nice::Algorithm::GaussjSingularMatrixException, error );
        }

        pivinv=1.0/a ( icol,icol );
        a ( icol,icol ) =1.0;
        for ( l=0;l<n;l++ ) a ( icol,l ) *= pivinv;
        for ( l=0;l<m;l++ ) b ( icol,l ) *= pivinv;
        for ( ll=0;ll<n;ll++ )
            if ( ll != icol )
            {
                dum=a ( ll,icol );
                a ( ll,icol ) =0.0;
                for ( l=0;l<n;l++ ) a ( ll,l ) -= a ( icol,l ) *dum;
                for ( l=0;l<m;l++ ) b ( ll,l ) -= b ( icol,l ) *dum;
            }
    }
    for ( l=n-1;l>=0;l-- )
    {
        if ( indxr[l] != indxc[l] )
            for ( k=0;k<n;k++ )
                SWAP ( a ( k,indxr[l] ),a ( k,indxc[l] ) );
    }
} /* gaussj */

/**
* @brief Sorts covariance matrix
*
* @param _covar
* @param _ia
* @param _mfit
*/
void Nice::Alg::covsrt ( boost::numeric::ublas::matrix<double> &covar, boost::numeric::ublas::vector<bool> &ia, const int mfit )
{
    int i,j,k;

    int ma=(int)ia.size();
    for ( i=mfit;i<ma;i++ )
        for ( j=0;j<i+1;j++ ) covar ( i,j ) =covar ( j,i ) =0.0;
    k=mfit-1;
    for ( j=ma-1;j>=0;j-- )
    {
        if ( ia[j] )
        {
            for ( i=0;i<ma;i++ ) SWAP ( covar ( i,k ),covar ( i,j ) );
            for ( i=0;i<ma;i++ ) SWAP ( covar ( k,i ),covar ( j,i ) );
            k--;
        }
    }
} /* covsrt */


#ifndef M_SQRT3_2
/** Altitude in a regular triangle of unit side length.
* sqrt(3)/2 = 0.5*1.7320508075.
*/
#define M_SQRT3_2 0.866025403784438646763723170755
#endif

/*!******************************************
* @brief construct a chain of points on a c.c.w. spiral.
*
* @param c A circle which will contain the spiral.
*   The center of the circle defines the starting point of the spiral.
*   The radius of the circle defines how far the spiral radius may reach out from the center.
* @param type Type of the discret underlying lattice.
*   If type=0 the points are on a simple 2D quadratic lattice.
*   If type=1 the points are on a simple triangular (a.k.a. hexagonal) lattice.
*   There is no type for centered square lattices, because these are the
*   same as quadratic lattices rotated by 45 degrees and shrunk by a factor sqrt(2).
*   (So they may be derived by rotation and shrinking the vector of points returned by this function.)
* @param latConst The lattice constant in the same units as the circle center and radius.
*   This is the shortest distance between two distinct points on the lattice.
*   That value should not be an integer multiple of the lattice constant (to avoid fuzzy interpretation
*   of which points on the spiral stay within the circle)
* @return A sequence of points with increasing distance from the circle center.
* @since 2017-11-02
* @author R. J. Mathar
*/
Nice::SeqPoint Nice::Alg::generateSpiral(const Nice::Circle & c, int type, double latConst)
{
	/* the list of all points to be returned */
	Nice::SeqPoint spir ;

	/* the current tail of the list */
	Nice::Point tail(c.getCenter()) ;

	/* the current heading of the spiral's end, starting horizontal to +x */
	Nice::Point delta(latConst,0.) ;

	switch (type)
	{
	case 0:
		spir.push_back(tail) ;
		/* each generation means walking along the perimeter of a square
		*/
		for(int gen =1;; gen++)
		{
			/* distances will all be beyond the circle radius: stop iteration
			*/
			if ( gen*latConst > c.r)
				break ;

			/* one step to the right */
			tail += delta ;
			spir.push_back(tail) ;

			/* 2*gen-1 steps up */
			delta = Nice::Point(0.,latConst) ;
			for(int s = 0; s < 2*gen-1 ; s++)
			{
				tail += delta ;
				if ( c.contains(tail)) 
					spir.push_back(tail) ;
			}

			/* 2*gen steps left */
			delta = Nice::Point(-latConst,0.) ;
			for(int s = 0; s < 2*gen ; s++)
			{
				tail += delta ;
				if ( c.contains(tail)) 
					spir.push_back(tail) ;
			}

			/* 2*gen steps down */
			delta = Nice::Point(0.,-latConst) ;
			for(int s = 0; s < 2*gen ; s++)
			{
				tail += delta ;
				if ( c.contains(tail)) 
					spir.push_back(tail) ;
			}

			/* 2*gen steps right */
			delta = Nice::Point(latConst,0.) ;
			for(int s = 0; s < 2*gen ; s++)
			{
				tail += delta ;
				if ( c.contains(tail)) 
					spir.push_back(tail) ;
			}
		}
		break ;
	case 1:
		spir.push_back(tail) ;
		/* each generation means walking along the perimeter of a regular hexagon
		* where the furthest distance on the hexagon (occuring 6 times) to the center is gen*latConst.
		*/
		for(int gen =1;; gen++)
		{
			/* distances will all be beyond the circle radius: stop iteration
			* Here the left hand side is the distance to any of the 6 sides of the hexagon.
			*/
			if ( M_SQRT3_2* gen*latConst > c.r)
				break ;

			/* one step to the right */
			tail += delta ;
			spir.push_back(tail) ;

			/* gen-1 steps right up */
			delta = Nice::Point(0.5*latConst,M_SQRT3_2*latConst) ;
			for(int s = 0; s < gen-1 ; s++)
			{
				tail += delta ;
				if ( c.contains(tail)) 
					spir.push_back(tail) ;
			}

			/* gen steps left up */
			delta = Nice::Point(-0.5*latConst,M_SQRT3_2*latConst) ;
			for(int s = 0; s < gen ; s++)
			{
				tail += delta ;
				if ( c.contains(tail)) 
					spir.push_back(tail) ;
			}

			/* gen steps left */
			delta = Nice::Point(-latConst,0.) ;
			for(int s = 0; s < gen ; s++)
			{
				tail += delta ;
				if ( c.contains(tail)) 
					spir.push_back(tail) ;
			}

			/* gen steps left down */
			delta = Nice::Point(-0.5*latConst,-M_SQRT3_2*latConst) ;
			for(int s = 0; s < gen ; s++)
			{
				tail += delta ;
				if ( c.contains(tail)) 
					spir.push_back(tail) ;
			}

			/* gen steps right down */
			delta = Nice::Point(0.5*latConst,-M_SQRT3_2*latConst) ;
			for(int s = 0; s < gen ; s++)
			{
				tail += delta ;
				if ( c.contains(tail)) 
					spir.push_back(tail) ;
			}

			/* gen steps right */
			delta = Nice::Point(latConst,0.) ;
			for(int s = 0; s < gen ; s++)
			{
				tail += delta ;
				if ( c.contains(tail)) 
					spir.push_back(tail) ;
			}
		}
		break ;
	}
	return spir ;
} /* generateSpiral */

/*!******************************************
* @brief construct subsets of size m out of n elements
*
* This is the standard combinatorial sub-set construction.
* For N=5 , m=3 we construct <0,1,2>, <0,1,3>, <0,1,4>,
*  <0,2,3>, <0,2,4>...., <2,3,4>.
*
* @param N The number of elements to choose from.
* @param m The number of elements selected in each element of the result.
* @return ordered lists of numbers in the range 0..n-1 where
*  each list has m elements.
*  The cardinality of the outer vector is biomial(N,m).
* @since 2017-03-30
* @author R. J. Mathar
*/
template <typename T> std::vector< std::vector<int> > Nice::Alg::subSets(T N, T m)
{
        std::vector<int> fxd ;
        return subSets(N,m,fxd,(T)0) ;
} /* subSets */

/*!******************************************
* @brief construct subsets of size m out of n elements
*
* @param N The number of elements to coose from.
* @param m The number of elements selected in each element of the result.
* @param[in] fxd The sublist of indices which are already fixed.
* @param[in] minm A lower bound of the indices to be taken freely.
* @return ordered lists of numbers in the range 0..n-1 where
* @since 2017-03-30
* @author R. J. Mathar
*/
template <typename T> std::vector< std::vector<int> > Nice::Alg::subSets(T N, T m,
        std::vector<int> fxd, T minm)
{
        std::vector< std::vector<int> > listofm ;
        if ( (int)fxd.size() < m)
        {
                /* still some slots to be filled; attach numbers c to new
                * partial lists of selected elements
                */
                for(T c = minm ; c < N ; c++)
                {
                        std::vector<int> nxtl = fxd ;
                        nxtl.push_back((int)c) ;
                        /* recursive call, one more element fixed now
                        */
                        std::vector< std::vector<int> > sols = subSets(N,m, nxtl,(T)(c+1)) ;
                        /* append all solutions to the existing solutions
                        */
                        listofm.insert(listofm.end(), sols.begin(), sols.end()) ;
                }
        }
        else
                /* all elements fixed, so this is the only choice
                */
                listofm.push_back(fxd) ;

        return listofm ;
} /* subSets */

/* explicit template instantiations
*/
template std::vector< std::vector<int> > Nice::Alg::subSets<short>(short N, short m) ;
template std::vector< std::vector<int> > Nice::Alg::subSets<int>(int N, int m) ;
