/*
 * This file is part of the ESO SINFONI Pipeline
 * Copyright (C) 2004,2005 European Southern Observatory
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, 51 Franklin St, Fifth Floor, Boston, MA  02111-1307  USA
 */
#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif
#include <sinfo_fit.h>
#include <sinfo_msg.h>
#include <stdio.h>
#include <math.h>
/**@{*/
/**
 * @defgroup sinfo_fit Fit functions
 *
 * TBD
 */

static double
sinfo_spline(double x, double cons[], double ak[], double *sp, double *spp,
             double *sppp, int n);

/**
 @brief perform a amoeba minimization of the function ftbm(d2,ncon)
 @param    d0 has nvar values, it is the average excluding the highest of d1
 @param    d1 has nvar x nvar+1 values.  it is the simplex array of values
 @param    d2 has ncons values at all times, it is the interface with ftbm
 @param    value is the last nvar+1 values from ftbm
 @param    range range of parameter varaiability
 @param    tol   tolerance
 @param    ivorf is 0 for fixed parameters, 1 for those varied.
 @param    ncon
 @param    nref is the maximum number of reflections
 @param    ftbm function to be fit
 @note     parameters begin varying as d2(i)+-range(i)
 */
double
sinfo_amsub(double d0[], double d1[], double d2[], double value[],
            double range[], double tol, int ivorf[], int ncon, int nref, double
            (*ftbm)(double[], int ncon))

{
    double alpha = 1.0, loc_gamma = 1.5;
    double sf, bsave, temp, sum, cval, ccval, beta;
    int idone, nvar, nvec, nrefl, i, j, k, kd1, kval, isign, jvar, imin, imax,
                    i2max, it1, it2, itemp;
    idone = 0;
    isign = 1;
    // we require that nvec=nvar+1, define nvar
    nvar = 0;
    for (i = 0; i < ncon; ++i) {
        if (ivorf[i] == 1) {
            nvar = nvar + 1;
        }
    }
    nvec = nvar + 1;
    // sf is the 'shrink' factor
    sf = 1e5 * nvec;
    nrefl = 0;
    value[0] = (*ftbm)(d2, ncon);
    // sinfo_msg("value[0] = %lg",value[0]);
    // initial and shrink calculation of the d1 array, uses range
    // set d2 in the first position -- using Fortran convention of 1st
    // array element moving first
    cont20: kd1 = -1;
    for (i = 0; i < ncon; ++i) {
        if (ivorf[i] == 1) {
            kd1 = kd1 + 1;
            d1[kd1] = d2[i];
        }
    }
    // now for the next nvar values
    kval = 0;
    for (jvar = 0; jvar < ncon; ++jvar) {
        if (ivorf[jvar] == 1) {
            kval = kval + 1;
            bsave = d2[jvar];
            isign = -isign;
            d2[jvar] = d2[jvar] + isign * range[jvar];
            value[kval] = (*ftbm)(d2, ncon);
            for (i = 0; i < ncon; ++i) {
                if (ivorf[i] == 1) {
                    kd1 = kd1 + 1;
                    d1[kd1] = d2[i];
                }
            }
            d2[jvar] = bsave;
        }
    }
    // sinfo_msg(" d1 ");
    // for (j=0;j<nvec;++j) {
    //   for(i=0;i<nvar;++i) {
    //     sinfo_msg("%12.4lg ",d1[i+j*nvar]);
    //   }
    // }
    /* find highest, second highest, and minimum values
     imax points to the vector with the largest value
     i2max points to the vector with the second largest value
     imin points to the vector with the smallest value  */
    cont40: imin = 1;
    if (value[0] > value[1]) {
        imax = 0;
        i2max = 1;
    }
    else {
        imax = 1;
        i2max = 0;
    }
    for (i = 0; i < nvec; ++i) {
        if (value[i] < value[imin])
            imin = i;
        if (value[i] > value[imax]) {
            i2max = imax;
            imax = i;
        }
        else if ((value[i] > value[i2max]) && (i != imax)) {
            i2max = i;
        }
    }
    // sinfo_msg(" values after sorting ");
    // for(i=0;i<nvec;++i)sinfo_msg("%12.4lg ",value[i]);
    // sinfo_msg("imin %d,i2max %d,imax %d",imin,i2max,imax);
    // scanf("%d",&itest);

    // check if done

    if (nrefl >= nref) {
        sinfo_msg(" maximum number of reflection reached");
        idone = 1;
        goto cont400;
    }
    if (value[imin] != 0.0) {
        temp = (value[imax] - value[imin]) / value[imin];
        if (fabs(temp) <= tol) {
            sinfo_msg(" reached tolerance %lg temp %lg tol", temp, tol);
            idone = 1;
            goto cont400;
        }
    }
    if (value[imax] - value[imin] <= tol) {
        sinfo_msg("value[max]-value[min]<=tol");
        idone = 1;
        goto cont400;
    }

    // *** form d0 the average of all but imax
    for (j = 0; j < nvar; ++j) {
        sum = 0.0;
        for (i = 0; i < nvec; ++i) {
            if (i != imax)
                sum = sum + d1[i * nvar + j];
        }
        d0[j] = sum / (nvec - 1);
    }
    // sinfo_msg(" D0 values ");
    // for(i=0;i<nvar;++i)sinfo_msg("%12.4lg ",d0[i]);
    // scanf("%d",&itest);
    // reflection

    nrefl = nrefl + 1;
    k = -1;
    for (j = 0; j < ncon; ++j) {
        if (ivorf[j] == 1) {
            k = k + 1;
            it1 = imax * nvar + k;
            d2[j] = (1 + alpha) * d0[k] - alpha * d1[it1];
        }
    }

    // sinfo_msg(" refl d2 ");
    // for(i=0;i<nvar;++i) sinfo_msg("%12.4lg ",d2[i]);

    cval = (*ftbm)(d2, ncon);
    // sinfo_msg("refl ftbm %lg",cval);

    // value is higher than i2max so do contraction
    if (cval >= value[i2max])
        goto cont200;

    // value is less than i2max - normal - update d1 and value

    value[imax] = cval;
    k = -1;
    for (j = 0; j < ncon; ++j) {
        if (ivorf[j] == 1) {
            k = k + 1;
            it1 = imax * nvar + k;
            d1[it1] = d2[j];
        }
    }

    // value is less than imin, try expansion
    if (cval < value[imin])
        goto cont300;
    goto cont40;

    // contraction
    cont200:
    // sinfo_msg(" contraction ");
    beta = 0.75;
    for (itemp = 0; itemp < 3; ++itemp) {
        if (cval <= value[imax]) {
            value[imax] = cval;
            k = -1;
            for (j = 0; j < ncon; ++j) {
                if (ivorf[j] == 1) {
                    k = k + 1;
                    it1 = imax * nvar + k;
                    d1[it1] = d2[j];
                }
            }
        }
        k = -1;
        for (j = 0; j < ncon; ++j) {
            if (ivorf[j] == 1) {
                k = k + 1;
                it1 = imax * nvar + k;
                d2[j] = beta * d1[it1] + (1. - beta) * d0[k];
            }
        }
        cval = ftbm(d2, ncon);

        // sinfo_msg(" contraction beta %lg cval %lg ",beta,cval);
        // value is better
        if (cval < value[i2max]) {
            value[imax] = cval;
            k = -1;
            for (j = 0; j < ncon; ++j) {
                if (ivorf[j] == 1) {
                    k = k + 1;
                    it1 = imax * nvar + k;
                    d1[it1] = d2[j];
                }
            }
            if (cval < value[imin])
                sinfo_msg(" contraction minimum %lg", cval);
            goto cont40;
        }
        beta = beta - 0.25;
    }
    sinfo_msg(" contraction failed  ==>shrink");
    // scanf("%d",&itest);
    // value is worse so shrink it

    goto cont400;

    // expansion

    cont300:
    sinfo_msg(" reflection min %lg \n", cval);
    k = -1;
    for (j = 0; j < ncon; ++j) {
        if (ivorf[j] == 1) {
            k = k + 1;
            d2[j] = loc_gamma * d2[j] + (1. - loc_gamma) * d0[k];
        }
    }
    ccval = (*ftbm)(d2, ncon);
    // value is higher than reflected value ==> discard
    if (ccval > cval)
        goto cont40;
    // value is better so use it rather than the reflected point
    sinfo_msg(" expansion minimum %lg \n", ccval);
    value[imax] = ccval;
    k = -1;
    for (j = 0; j < ncon; ++j) {
        if (ivorf[j] == 1) {
            k = k + 1;
            it1 = imax * nvar + k;
            d1[it1] = d2[j];
        }
    }
    goto cont40;

    cont400:
    // sinfo_msg(" following cont400 ");
    // scanf("%d",&itest);
    // recalculate d2 and range
    // the range is the average of dist**2 from d1 with min value
    k = -1;
    for (j = 0; j < ncon; ++j) {
        if (ivorf[j] == 1) {
            k = k + 1;
            it1 = imin * nvar + k;
            d2[j] = d1[it1];
            sum = 0.0;
            for (i = 0; i < nvec; ++i) {
                it1 = i * nvar + k;
                it2 = imin * nvar + k;
                sum = sum + (d1[it1] - d1[it2]) * (d1[it1] - d1[it2]);
            }
            range[j] = sf * sqrt(sum / (nvec - 1));
        }
    }
    value[1] = value[imin];
    sf = .75 * sf;
    if (sf < 0.1)
        idone = 1;
    sinfo_msg(" shrink factor %lg ", sf);
    if (idone != 1)
        goto cont20;
    return value[1];

}

static double
sinfo_spline(double x, double cons[], double ak[], double *sp, double *spp,
             double *sppp, int n)
{
    double retval = 0;
    double xm = 0;
    double xm2 = 0;
    double xm3 = 0;

    int i = 0;

    *sp = 0;
    *spp = 0;
    *sppp = 0;

    for (i = 0; i < n; ++i) {
        if (ak[i] >= x) {
            xm = ak[i] - x;
            xm2 = xm * xm;
            xm3 = xm * xm2;
            sinfo_msg("cons=%g", cons[i]);
            retval += cons[i] * xm3;
            *sp -= 3 * cons[i] * xm2;
            *spp += 6 * cons[i] * xm;
            *sppp -= 6 * cons[i];
        }
    }
    sinfo_msg("1x=%g retval=%g", x, retval);
    return retval;

}

double
sinfo_ftbm(const double x, double cons[])
{
    double retval = 0;
    double ak[4] =
        { -1, -.666666666666666, -.333333333333, 0 };
    double sm1 = 0;
    double spm1 = 0;
    double sppm1 = 0;
    double spppm1 = 0;

    int n = 4;

    sm1 = sinfo_spline(x, cons, ak, &spm1, &sppm1, &spppm1, n) - 1;
    sinfo_msg("x=%g val=%g", x, sm1 + 1);

    retval = sm1 * sm1 + spm1 * spm1 + sppm1 * sppm1 + spppm1 * spppm1;
    sinfo_msg("fitbm: x=%g retval=%g", x, retval);

    return retval;

}

/**@}*/
