/******************************************************************************
 *
 *      File Name:              fit.c
 *      Version:                1.0
 *      Project Name:           View Non-Linear Fitting Module
 *      Author:                 Harold Levy
 *      Date:                   October 28, 1992
 *      Compiler:               gcc
 *
 *      Utility:                This module provides non-linear fitting via
 *				simplex minimization.
 *
 *      Notes:                  har@caltech.edu
 *                              128-95 Caltech/Pasadena, CA 91125
 *                              Please send questions/bug fixes regarding this
 *                              module to the above addresses.
 *
 ******************************************************************************/
/*
   The licensing agreement for this software is identical to that of the rest
   of the LOG/DigLog/AnaLog source code:

   "LOG", a circuit editing and simulation system,

   "DigLOG", the digital simulator for LOG.

   Copyright (C) 1985, 1990 California Institute of Technology.
   Author: David Gillespie
   Maintainers: John Lazzaro and Dave Gillespie
   Maintainers's address: lazzaro@hobiecat.cs.caltech.edu;
                          CB 425/ CU Boulder/Boulder CO 91125.
                          daveg@csvax.caltech.edu;
                          256-80 Caltech/Pasadena CA 91125.
                          Send questions, bug fixes, to these addresses.

   "AnaLOG", the analog simulator for LOG.
   Copyright (C) 1985, 1990 California Institute of Technology.
   Author: John Lazzaro
   Maintainer: John Lazzaro
   Maintainers's address: lazzaro@hobiecat.cs.caltech.edu;
                          CB 425/ CU Boulder/Boulder CO 91125.
                          Send questions, bug fixes, to this address.


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 (Version 1, 1989).

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; see the file COPYING.  If not, write to
the Free Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/


#include <stdio.h>
#include <sys/param.h>
#include <stdlib.h>
#include <math.h>

#include "global.h"

#ifndef NEWASM_H
#include <p2c/newasm.h>
#endif

#ifndef VIEWMOD_H
#include "viewmod.h"
#endif


void	fit ();
char	ErrorCheck ();
double	func ();

void	malloc_print_error ();
double	*vector ();
void	free_vector ();
double	**matrix ();
void	free_matrix ();
char	simplex ();
double	simplex_try ();

/* Note:  Be sure to update the help information in viewcurves.c
          with changes to DEFAULT_FIT_TOL, now set at 0.001
*/
#define		DEFAULT_FIT_TOL	0.001

#define		sqr(x)		((x)*(x))


void fit (buf_)
  char *buf_;
{
  char		args[256];
  v_curverec	*vcr, *dataVCR, *funcVCR, *paraVCR, *theVCR;
  v_curverec	*tolVCR;
  double	step;
  char		fileName[256], STR1[256];
  FILE		*filePtr;
  long		curwidth;
  int		half, i, j, nPara, nfunc;

  double	**p, *y, fit_tol;

/* Check for file name
*/
  strcpy (args, buf_);
  i = strposc (args, '>', 1L);
  if (i > 0)
  {
    strcpy (fileName, strltrim (strcpy (STR1, args + i)));
    newci_fixfname (fileName, "text", "");
    filePtr = fopen (fileName, "w");
    if (filePtr == NULL)
    {
      printf ("Cannot open \"%s\".\n", fileName);
      return;
    }
    strncpy (args, buf_, i-1);
    args[i-1] = '\0';
  }
  else
    filePtr = NULL;

/* Run error checking on arguments
*/
  if (ErrorCheck (args)) return;
  if (filePtr)
    fprintf (filePtr, "\n");
  else
    printf ("\n");

/* Set fit tolerance
*/
  tolVCR = v_findcurve ("fit_tol");
  if (tolVCR == NULL)
  {
    v_addcurveconst (DEFAULT_FIT_TOL, "", "fit_tol");
    fit_tol = DEFAULT_FIT_TOL;
  }
  else
  {
    if (tolVCR->base != NULL)
    {
      printf ("\n*fit error*\n");
      printf ("  \"fit_tol\" is not a constant.\n");
      if (filePtr) fclose (filePtr);
      return;
    }
    fit_tol = tolVCR->yval;
  }

/* Get curve data structures
*/
  strcpy (STR1, args);
  v_ncurvelist (STR1, &vcr, v_clopt);
  v_checktoomany (STR1);
  if (vcr == NULL)
  {
    printf ("[No matching names]\n");
    if (filePtr) fclose (filePtr);
    return;
  }
  dataVCR = vcr;
  funcVCR = vcr->next2;
  paraVCR = vcr->next2->next2;

/* Show initial conditions
*/
  if (filePtr)
    fprintf (filePtr, "**  Initial Conditions  **\n\n");
  else
    printf ("**  Initial Conditions  **\n\n");
  theVCR = paraVCR;
  nPara = 0;
  while (theVCR != NULL)
  {
    curwidth = nc_curWindow->width - 2;
    half = (int) (curwidth / 2);
    strncpy (STR1, theVCR->name, half - 2);
    if (filePtr)
      fprintf (filePtr, "  %s", STR1);
    else
      printf ("  %s", STR1);
    for (i = 1; i <= half - strlen (theVCR->name); i ++)
      if (filePtr)
        fprintf (filePtr, " ");
      else
        printf (" ");
    if (filePtr)
      fprintf (filePtr, "%-.5e\n", theVCR->yval);
    else
      printf ("%-.5e\n", theVCR->yval);
    nPara ++;
    theVCR = theVCR->next2;
  }
  if (filePtr)
    fprintf (filePtr, "\n");
  else
    printf ("\n");

/* Fitting routine driver
*/
  if (filePtr)
  {
    fprintf (filePtr, "**  Computing Fit  **\n\n");
    fprintf (filePtr, "fit_tol = %-.5e\n", fit_tol);
    fprintf (filePtr, "chisqr:  [0]  = %-.5e\n", func (args));
  }
  else
  {
    printf ("**  Computing Fit  **\n\n");
    printf ("fit_tol = %-.5e\n", fit_tol);
    printf ("chisqr:  [0]  = %-.5e\n", func (args));
  }

/* Setup Simplex
*/
  p = matrix (1, nPara+1, 1, nPara);
  y = vector (1, nPara+1);
  j = 1;
  theVCR = paraVCR;
  while (theVCR != NULL)
  {
    p[1][j] = theVCR->yval;
    j ++;
    theVCR = theVCR->next2;
  }
  for (i = 2; i <= nPara+1; i ++)
  {
    p[i][i-1] = 0.1 * p[1][i-1];
    for (j = 1; j <= nPara; j ++) p[i][j] += p[1][j];
  }

  for (i = 1; i <= nPara+1; i ++)
  {
    theVCR = paraVCR;
    for (j = 1; j <= nPara; j ++)
    {
      v_assigncurveconst (p[i][j], theVCR->name);
      theVCR = theVCR->next2;
    }
    strcpy (STR1, args);
    v_ncurvelist (STR1, &vcr, v_clopt);
    y[i] = func (args);
  }

  if (simplex (args, p, y, nPara, fit_tol, func, &nfunc))
  {
    printf ("\n*fit error*\n");
    printf ("  (simplex):  too many iterations.\n");
    printf ("  Try a closer guess for the paramters.\n\n");
    if (filePtr) fclose (filePtr);
    return;
  }

  theVCR = paraVCR;
  for (j = 1; j <= nPara; j ++)
  {
    v_assigncurveconst (p[1][j], theVCR->name);
    theVCR = theVCR->next2;
  }
  strcpy (STR1, args);
  v_ncurvelist (STR1, &vcr, v_clopt);
  if (filePtr)
    fprintf (filePtr, "chisqr:  [%d] = %-.5e\n\n", nfunc, func (args));
  else
    printf ("chisqr:  [%d] = %-.5e\n\n", nfunc, func (args));

/* Show initial conditions
*/
  if (filePtr)
    fprintf (filePtr, "**  Final Conditions  **\n\n");
  else
    printf ("**  Final Conditions  **\n\n");
  theVCR = paraVCR;
  nPara = 0;
  while (theVCR != NULL)
  {
    curwidth = nc_curWindow->width - 2;
    half = (int) (curwidth / 2);
    strncpy (STR1, theVCR->name, half - 2);
    if (filePtr)
      fprintf (filePtr, "  %s", STR1);
    else
      printf ("  %s", STR1);
    for (i = 1; i <= half - strlen (theVCR->name); i ++)
      if (filePtr)
        fprintf (filePtr, " ");
      else
        printf (" ");
    if (filePtr)
      fprintf (filePtr, "%-.5e\n", theVCR->yval);
    else
      printf ("%-.5e\n", theVCR->yval);
    nPara ++;
    theVCR = theVCR->next2;
  }
  if (filePtr)
    fprintf (filePtr, "\n");
  else
    printf ("\n");

  if (filePtr) fclose (filePtr);
  free_matrix (p, 1, nPara+1, 1, nPara);
  free_vector (y, 1, nPara+1);
}


char ErrorCheck (buf_)
  char *buf_;
{
  char		args[256];
  v_curverec    *vcr, *dataVCR, *funcVCR, *paraVCR, *theVCR;
  v_curverec    *tolVCR;
  double        step;
  char          fileName[256], STR1[256];
  char		dataName[256], funcName[256];

/* Get curve data structures
*/
  strcpy (args, buf_);
  v_ncurvelist (args, &vcr, v_clopt);
  v_checktoomany (args);
  if (vcr == NULL)
  {
    printf ("[No matching names]\n");
    return (1);
  }
  dataVCR = vcr;
  funcVCR = vcr->next2;
  if (funcVCR)
    paraVCR = vcr->next2->next2;
  else
    paraVCR = NULL;
  strcpy (STR1, buf_);
  v_strword (STR1, dataName);
  v_strword (STR1, funcName);

/* Do error checking on the data structures
*/
  if (strcmp (dataVCR->name, dataName))
  {
    printf ("\n*fit error*\n");
    printf ("  <data>:  \"%s\" does not exist.\n\n", dataName);
    return (1);
  }
  if (dataVCR->expr != NULL)
  {
    printf ("\n*fit error*\n");
    printf("  <data>:  \"%s\" must not be a computed curve.\n\n",dataVCR->name);
    return (1);
  }
  if (funcVCR == NULL)
  {
    printf ("\n*fit error*\n");
    printf ("  <function>:  does not exist or degenerate.\n\n");
    return (1);
  }
  if (strcmp (funcVCR->name, funcName))
  {
    printf ("\n*fit error*\n");
    printf ("  <function>:  \"%s\" does not exist.\n\n", funcName);
    return (1);
  }
  if (funcVCR->expr == NULL)
  {
    printf ("\n*fit error*\n");
    printf("  <function>:  \"%s\" must be a computed curve.\n\n",funcVCR->name);
    return (1);
  }

/* Do error checking on the basis coordinates
*/
  if ((dataVCR->base->len != funcVCR->base->len) ||
      (dataVCR->base->vec[0] != funcVCR->base->vec[0]) ||
      (dataVCR->base->vec[dataVCR->base->len-1] !=
       funcVCR->base->vec[funcVCR->base->len-1]))
  {
    printf ("\n*fit error*\n");
    printf ("  \"%s\" and \"%s\" have different basis coordinates.\n",
            dataVCR->name, funcVCR->name);
    step = (dataVCR->base->vec[dataVCR->base->len-1] -
            dataVCR->base->vec[0]) / ((double) dataVCR->base->len - 1);
    printf ("  <data>:      \"%s\"=%g:%g:%g\n", dataVCR->name,
            dataVCR->base->vec[0],
            dataVCR->base->vec[dataVCR->base->len-1],
            step);
    step = (funcVCR->base->vec[funcVCR->base->len-1] -
            funcVCR->base->vec[0]) / ((double) funcVCR->base->len - 1);
    printf ("  <function>:  \"%s\"=%g:%g:%g\n", funcVCR->name,
            funcVCR->base->vec[0],
            funcVCR->base->vec[funcVCR->base->len-1],
            step);
    printf ("\n");
    return (1);
  }

/* Do error checking on the parameters
*/
  theVCR = paraVCR;
  if (theVCR == NULL)
  {
    printf ("\n*fit error*\n");
    printf ("  At least one constant parameter must be specified.\n\n");
    return (1);
  }
  while (theVCR != NULL)
  {
    if (theVCR->base != NULL)
    {
      printf ("\n*fit error*\n");
      printf ("  \"%s\" is not a constant.\n\n", theVCR->name);
      return (1);
    }
    theVCR = theVCR->next2;
  }

  return (0);
}


double func (buf_)
  char *buf_;
{
  char		args[256];
  v_curverec	*vcr, *dataVCR, *funcVCR, *paraVCR;
  double	chisqr;
  int		i;

  strcpy (args, buf_);
  v_ncurvelist (args, &vcr, v_clopt);
  dataVCR = vcr;
  funcVCR = vcr->next2;
  paraVCR = vcr->next2->next2;

  chisqr = 0.0;
  for (i = 0; i < dataVCR->base->len; i ++)
  {
    chisqr += sqr (funcVCR->vec[i] - dataVCR->vec[i]);
  }

  return (chisqr);
}


void malloc_print_error (where)
char *where;
{
	fprintf(stderr,"viewfit malloc_print_error:  %s\n", where);
        exit(1);
}


double *vector(nl,nh)
int nl,nh;
{
        double *v;

        v=(double *)malloc((unsigned) (nh-nl+1)*sizeof(double));
        if (!v) malloc_print_error("failure in vector()");
        return v-nl;
}


void free_vector(v,nl,nh)
double *v;
int nl,nh;
{
        free((char*) (v+nl));
}


double **matrix(nrl,nrh,ncl,nch)
int nrl,nrh,ncl,nch;
{
        int i;
        double **m;

        m=(double **) malloc((unsigned) (nrh-nrl+1)*sizeof(double*));
        if (!m) malloc_print_error("failure 1 in matrix()");
        m -= nrl;

        for(i=nrl;i<=nrh;i++) {
                m[i]=(double *) malloc((unsigned) (nch-ncl+1)*sizeof(double));
                if (!m[i]) malloc_print_error("failure 2 in matrix()");
                m[i] -= ncl;
        }
        return m;
}


void free_matrix(m,nrl,nrh,ncl,nch)
double **m;
int nrl,nrh,ncl,nch;
{
        int i;

        for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
        free((char*) (m+nrl));
}


double simplex_try(buf_,p,y,psum,ndim,funk,ihi,nfunk,fac)
char *buf_;
double **p,*y,*psum,(*funk)(),fac;
int ndim,ihi,*nfunk;
{
        int j;
        double fac1,fac2,ytry,*ptry;

        char            args[256];
        v_curverec      *vcr, *dataVCR, *funcVCR, *paraVCR, *theVCR;
        int             pi;

        strcpy (args, buf_);
        v_ncurvelist (args, &vcr, v_clopt);
        dataVCR = vcr;
        funcVCR = vcr->next2;
        paraVCR = vcr->next2->next2;

        ptry=vector(1,ndim);
        fac1=(1.0-fac)/(double)ndim;
        fac2=fac1-fac;
        for (j=1;j<=ndim;j++) ptry[j]=psum[j]*fac1-p[ihi][j]*fac2;
        theVCR = paraVCR;
        for (pi = 1; pi <= ndim; pi ++)
        {
          v_assigncurveconst (ptry[pi], theVCR->name);
          theVCR = theVCR->next2;
        }
        ytry=(*funk)(buf_);
        ++(*nfunk);
        if (ytry < y[ihi]) {
                y[ihi]=ytry;
                for (j=1;j<=ndim;j++) {
                        psum[j] += ptry[j]-p[ihi][j];
                        p[ihi][j]=ptry[j];
                }
        }
        free_vector(ptry,1,ndim);
        return ytry;
}


#define NMAX 512
#define ALPHA 1.0
#define BETA 0.5
#define GAMMA 2.0

#define GET_PSUM for (j=1;j<=ndim;j++) { for (i=1,sum=0.0;i<=mpts;i++)\
                                                sum += p[i][j]; psum[j]=sum;}

char simplex(buf_,p,y,ndim,ftol,funk,nfunk)
char *buf_;
double **p,y[],ftol,(*funk)();
int ndim,*nfunk;
{
        int i,j,ilo,ihi,inhi,mpts=ndim+1;
        double ytry,ysave,sum,rtol,*psum;

	char args[256];
        v_curverec *vcr, *dataVCR, *funcVCR, *paraVCR, *theVCR;
        int pi;

        strcpy (args, buf_);
        v_ncurvelist (args, &vcr, v_clopt);
        dataVCR = vcr;
        funcVCR = vcr->next2;
        paraVCR = vcr->next2->next2;

        psum=vector(1,ndim);
        *nfunk=0;
        GET_PSUM
        for (;;)
        {
          ilo=1;
          ihi = y[1]>y[2] ? (inhi=2,1) : (inhi=1,2);
          for (i=1;i<=mpts;i++)
          {
            if (y[i] < y[ilo]) ilo=i;
            if (y[i] > y[ihi])
            {
              inhi=ihi;
              ihi=i;
            }
            else if (y[i] > y[inhi])
              if (i != ihi) inhi=i;
          }
          if ((y[ihi]==0.0) && (y[ilo]==0.0))
            rtol = 0.0;
          else
/*
	    This relative tolerance reflects convergence, but not how close
	    that convergence meets the user's goal; hence we blow it off and
            use the brute force approach:
            rtol=2.0*fabs(y[ihi]-y[ilo])/(fabs(y[ihi])+fabs(y[ilo]));
*/
            rtol=fabs(y[ilo]);
          if (rtol < ftol) break;
          if (*nfunk >= NMAX) return (1);
          ytry=simplex_try(buf_,p,y,psum,ndim,funk,ihi,nfunk,-ALPHA);
          if (ytry <= y[ilo])
            ytry=simplex_try(buf_,p,y,psum,ndim,funk,ihi,nfunk,GAMMA);
          else if (ytry >= y[inhi])
          {
            ysave=y[ihi];
            ytry=simplex_try(buf_,p,y,psum,ndim,funk,ihi,nfunk,BETA);
            if (ytry >= ysave)
            {
              for (i=1;i<=mpts;i++)
              {
                if (i != ilo)
                {
                  for (j=1;j<=ndim;j++)
                  {
                    psum[j]=0.5*(p[i][j]+p[ilo][j]);
                    p[i][j]=psum[j];
                  }
		  theVCR = paraVCR;
    	   	  for (pi = 1; pi <= ndim; pi ++)
    		  {
		    v_assigncurveconst (psum[pi], theVCR->name);
      		    theVCR = theVCR->next2;
    	   	  }
		  strcpy (args, buf_);
        	  v_ncurvelist (args, &vcr, v_clopt);
        	  dataVCR = vcr;
        	  funcVCR = vcr->next2;
        	  paraVCR = vcr->next2->next2;
                  y[i]=(*funk)(buf_);
                }
              }
               *nfunk += ndim;
               GET_PSUM
            }
          }
        }
        free_vector(psum,1,ndim);
        return (0);
}

#undef ALPHA
#undef BETA
#undef GAMMA
#undef NMAX
#undef GET_PSUM