/*
 * 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;

}



/**@}*/
