tgo.c

Go to the documentation of this file.

 
/*****************************************************************************/
#include "tpcclibConfig.h"
/*****************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <string.h>
/*****************************************************************************/
#include "tpcextensions.h"
#include "tpcrand.h"
#include "tpcstatist.h"
/*****************************************************************************/
#include "tpcnlopt.h"
/*****************************************************************************/
 
/*****************************************************************************/
#ifndef MAX_PARAMETERS
#define MAX_PARAMETERS 50
#endif
typedef struct {
  int topomin;
  double fvalue;
  double par[MAX_PARAMETERS];
} TGO_POINT;
/*****************************************************************************/
 
/*****************************************************************************/
int nloptITGO1(
  NLOPT *nlo,
  const int doLocal,
  unsigned int tgoNr,
  unsigned int sampleNr,
  unsigned int neighNr,
  TPCSTATUS *status
) {
  int verbose=0; if(status!=NULL) verbose=status->verbose;
  if(verbose>0) {
    printf("%s(NLOPT, %d, %u, %u, %u, status)\n", __func__, doLocal, tgoNr, sampleNr, neighNr);
    fflush(stdout);
  }
  if(nlo==NULL ) {
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
    return TPCERROR_FAIL;
  }
  if(nlo->totalNr<1 || nlo->xfull==NULL || nlo->_fun==NULL) {
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_NO_DATA);
    return TPCERROR_NO_DATA;
  }
  if(nlo->totalNr>MAX_PARAMETERS) {
    if(verbose>0) {fprintf(stderr, "Error: too many dimensions to optimize.\n"); fflush(stderr);}
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
    return TPCERROR_FAIL;
  }
 
  /* Check if any of the parameters are fixed */
  unsigned int fixedParNr=nloptLimitFixedNr(nlo);
  if(verbose>1 && fixedParNr>0) printf("fixedParNr := %d\n", fixedParNr);
  unsigned int fittedParNr=nlo->totalNr-fixedParNr;
  if(verbose>1) printf("fittedParNr := %d\n", fittedParNr);
  if(fittedParNr<1) {
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_NO_DATA);
    return TPCERROR_NO_DATA;
  }
 
  /* Check the tolerations */
  for(unsigned int i=0; i<nlo->totalNr; i++) {
    if(nlo->xlower[i]>=nlo->xupper[i]) {nlo->xtol[i]=0.0; continue;}
    if(!(nlo->xtol[i]>0.0)) {
      if(verbose>0) {fprintf(stderr, "Error: invalid xtol[].\n"); fflush(stderr);}
      statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_NO_DATA);
      return TPCERROR_NO_DATA;
    }
  }
 
  /* Continue input checking */
  if(sampleNr<10) sampleNr=50*fittedParNr;
  if(sampleNr&1) sampleNr++; // If number is odd, then add 1
  if(neighNr<2) neighNr=sampleNr/2;
  else if(neighNr>sampleNr-1) neighNr=sampleNr-1; // Make sure "neighNr" isn't too big
  if(tgoNr==0) tgoNr=1+fittedParNr;
  if(verbose>2) {
    printf("sampleNr := %d\n", sampleNr);
    printf("neighNr := %d\n", neighNr);
    printf("tgoNr := %d\n", tgoNr);
    fflush(stdout);
  }
 
  /* Check if initial guess is valid */
  int initGuessAvailable=1;
  double initGuessCost=nan("");
  for(unsigned int i=0; i<nlo->totalNr; i++) {
    if(isnan(nlo->xfull[i])) {initGuessAvailable=0; break;}
    if(nlo->xfull[i]<nlo->xlower[i]) {initGuessAvailable=0; break;}
    if(nlo->xfull[i]>nlo->xupper[i]) {initGuessAvailable=0; break;}
  }
  if(initGuessAvailable) {
    initGuessCost=nlo->funval=(*nlo->_fun)(nlo->totalNr, nlo->xfull, nlo->fundata);
    nlo->funCalls++;
    if(!isfinite(initGuessCost)) initGuessAvailable=0;
    else if(verbose>2) {
      printf("valid initial guess with cost=%g\n", initGuessCost);
    }
  }
 
  /* Allocate memory */
  TGO_POINT *points=(TGO_POINT*)malloc(sampleNr*sizeof(TGO_POINT));
  if(points==NULL) {
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_OUT_OF_MEMORY);
    return TPCERROR_OUT_OF_MEMORY;
  }
  for(unsigned int i=0; i<sampleNr; i++) {
    points[i].topomin=0;
    points[i].fvalue=0.0;
  }
 
  /*
   *  Iterative TGO, or non-iterative if tgoNr==1
   */
  unsigned int topoNr=0;
  for(unsigned int tgoi=0; tgoi<tgoNr; tgoi++) {
 
    if(verbose>3 && tgoNr>1) {printf("TGO iteration # %d: \n", 1+tgoi); fflush(stdout);}
 
    /* Sample N points in the feasible region and compute the object function values for 
       those points which do not already have it. */
    unsigned int badNr=0;
    for(unsigned int si=0; si<sampleNr; si++) if(points[si].topomin==0) {
      if(tgoi==0 && si==0 && initGuessAvailable) {
        /* If initial guess was given, then use it as the first point */
        for(unsigned int i=0; i<nlo->totalNr; i++) points[si].par[i]=nlo->xfull[i];
        points[si].fvalue=initGuessCost;
        continue;
      }
      nloptRandomPoint(points[si].par, nlo->xlower, nlo->xupper, nlo->totalNr, NULL);
      points[si].fvalue=(*nlo->_fun)(nlo->totalNr, points[si].par, nlo->fundata);
      nlo->funCalls++;
      /* If function return value was not valid, then we'll try twice with new guesses */
      int tries=0;
      while(!isfinite(points[si].fvalue) && tries<2) {
        if(verbose>8) {
          printf("this point did not give normal return value:\n");
          for(unsigned int i=0; i<nlo->totalNr; i++) printf("  %10.2e", points[si].par[i]);
          printf("\n");
        }
        nloptRandomPoint(points[si].par, nlo->xlower, nlo->xupper, nlo->totalNr, NULL);
        points[si].fvalue=(*nlo->_fun)(nlo->totalNr, points[si].par, nlo->fundata);
        nlo->funCalls++;
        tries++;
      }
      if(!isfinite(points[si].fvalue)) badNr++;
    }
    if(verbose>4 && badNr>0) printf("Number of bad points: %d\n", badNr);
    /* Object functions values must be good for at least NeighNr points */
    if(tgoi==0 && (sampleNr-badNr)<=neighNr) {
      if(verbose>0) {
        fprintf(stderr, "Error: invalid values inside parameter range.\n"); fflush(stderr);}
      free(points);
      statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_BAD_FIT);
      return TPCERROR_BAD_FIT;
    }
    /* Print sampled points */
    if(verbose>10) {
      printf("Sampled points:\n");
      for(unsigned int si=0; si<sampleNr; si++) {
        printf("%d\t", 1+si);
        for(unsigned int i=0; i<nlo->totalNr; i++) printf("%e ", points[si].par[i]);
        printf("=> %e\n", points[si].fvalue);
      }
      fflush(stdout);
    }
 
    /* For each point i, find out if it is a "topographic minimum", that is, 
       better than k neighbour points. */
    topoNr=0;
    for(unsigned int si=0; si<sampleNr; si++) {
      points[si].topomin=0;
 
      /* Compute the distances, scaled using xtol[] */
      double sdist[sampleNr];
      for(unsigned int sj=0; sj<sampleNr; sj++) {
        sdist[sj]=0.0;
        if(si==sj) {sdist[sj]=1.0E+99; continue;}
        for(unsigned int k=0; k<nlo->totalNr; k++) {
          if(!(nlo->xtol[k]>0.0)) continue;
          double d=(points[si].par[k]-points[sj].par[k])/nlo->xtol[k];
          if(isfinite(d)) sdist[sj]+=d*d;
        }
        /* Distance should be computed as square root, but it does not affect
           the order of distances; since sqrt() is slow, it is not done. */
        // sdist[sj]=sqrt(sdist[sj]);
      }
 
      /* Find the closest neighbours for sample i */
      unsigned int neighs[neighNr]; // collect here the neighbours
      unsigned int ni=0;
      for(ni=0; ni<neighNr; ni++) {
        unsigned int mini=0;
        double minv=sdist[mini];
        for(unsigned int sj=0; sj<sampleNr; sj++) {
          if(sdist[sj]<minv) {minv=sdist[sj]; mini=sj;}
        }
        sdist[mini]=1.0E+99; // not used again
        /* If point i is worse than any of the closest neighbours, then point i 
           is certainly not a topographic minimum; then stop this loop and go to
           the next point i+1 */
        if(!(points[si].fvalue<points[mini].fvalue)) break;
        neighs[ni]=mini;
      }
      if(ni<neighNr) continue; // continue with the next i
      /* otherwise mark this as topographic minimum (TM) */
      points[si].topomin=1; topoNr++;
      /* Print TM point */
      if(verbose>6) {
        printf("TM point %d:\n", topoNr);
        printf("%d\t", 1+si);
        for(unsigned int i=0; i<nlo->totalNr; i++) printf("%e ", points[si].par[i]);
        printf("=> %e\n", points[si].fvalue);
        if(verbose>7) {
          printf("  points neighbours:");
          for(ni=0; ni<neighNr; ni++) printf(" %d", 1+neighs[ni]);
          printf("\n");
        }
        fflush(stdout);
      }
 
    }
    if(verbose>4) {printf("  %d topographical minima\n", topoNr); fflush(stdout);}
    if(topoNr==0) {
      if(verbose>0) {fprintf(stderr, "Warning: no topographical minima found\n"); fflush(stderr);}
      continue;
    }
 
  } // end of iTGO
 
 
  /* If requested, do local optimization for the TMs */
  if(topoNr==0) {
    if(verbose>0) {fprintf(stderr, "Error: no topographical minima found\n"); fflush(stderr);}
    free(points);
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_BAD_FIT);
    return TPCERROR_BAD_FIT;
  }
  for(unsigned int si=0; si<sampleNr; si++) if(points[si].topomin>0) {
    if(verbose>2) printf("LO for TM point %d\n", 1+si);
    NLOPT lnlo; nloptInit(&lnlo);
    nloptDuplicate(nlo, &lnlo);
    for(unsigned int i=0; i<nlo->totalNr; i++) lnlo.xfull[i]=points[si].par[i];
    lnlo.funval=points[si].fvalue;
    for(unsigned int i=0; i<nlo->totalNr; i++) lnlo.xdelta[i]=100.0*lnlo.xtol[i];
    lnlo.maxFunCalls=2000*nlo->totalNr; lnlo.funCalls=0;
    int ret=nloptSimplex(&lnlo, 100*nlo->totalNr, status);
    if(ret==TPCERROR_OK) {
      for(unsigned int i=0; i<nlo->totalNr; i++) points[si].par[i]=lnlo.xfull[i];
      points[si].fvalue=lnlo.funval;
      if(verbose>6) {
        printf("TM point %d after LO:\n", 1+si);
        printf("%d\t", 1+si);
        for(unsigned int i=0; i<nlo->totalNr; i++) printf("%e ", points[si].par[i]);
        printf("=> %e\n", points[si].fvalue); fflush(stdout);
      }
    } else if(verbose>7) {printf("  LO failed\n"); fflush(stdout);}
    nlo->funCalls+=lnlo.funCalls;
    nloptFree(&lnlo);
  }
 
  /* Find the best minimum and return it */
  {
    unsigned int mini=0;
    double minv=points[mini].fvalue;
    for(unsigned int si=1; si<sampleNr; si++)
      if(!(points[si].fvalue>minv)) {minv=points[si].fvalue; mini=si;}
    for(unsigned int i=0; i<nlo->totalNr; i++) nlo->xfull[i]=points[mini].par[i];
    nlo->funval=points[mini].fvalue;
  }
 
  free(points);
  statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_OK);
  return TPCERROR_OK;
}
/*****************************************************************************/
 
/*****************************************************************************/
int nloptITGO2(
  NLOPT *nlo,
  const int doLocal,
  unsigned int tgoNr,
  unsigned int sampleNr,
  unsigned int neighNr,
  TPCSTATUS *status
) {
  int verbose=0; if(status!=NULL) verbose=status->verbose;
  if(verbose>0) {
    printf("%s(NLOPT, %d, %u, %u, %u, status)\n", __func__, doLocal, tgoNr, sampleNr, neighNr);
    fflush(stdout);
  }
  if(nlo==NULL ) {
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
    return TPCERROR_FAIL;
  }
  if(nlo->totalNr<1 || nlo->xfull==NULL || nlo->_fun==NULL) {
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_NO_DATA);
    return TPCERROR_NO_DATA;
  }
  if(nlo->totalNr>MAX_PARAMETERS) {
    if(verbose>0) {fprintf(stderr, "Error: too many dimensions to optimize.\n"); fflush(stderr);}
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
    return TPCERROR_FAIL;
  }
 
  /* Check if any of the parameters are fixed */
  unsigned int fixedParNr=nloptLimitFixedNr(nlo);
  if(verbose>1 && fixedParNr>0) printf("fixedParNr := %d\n", fixedParNr);
  unsigned int fittedParNr=nlo->totalNr-fixedParNr;
  if(verbose>1) printf("fittedParNr := %d\n", fittedParNr);
  if(fittedParNr<1) {
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_NO_DATA);
    return TPCERROR_NO_DATA;
  }
 
  /* Check the tolerations */
  for(unsigned int i=0; i<nlo->totalNr; i++) {
    if(nlo->xlower[i]>=nlo->xupper[i]) {nlo->xtol[i]=0.0; continue;}
    if(!(nlo->xtol[i]>0.0)) {
      if(verbose>0) {fprintf(stderr, "Error: invalid xtol[].\n"); fflush(stderr);}
      statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_NO_DATA);
      return TPCERROR_NO_DATA;
    }
  }
 
  /* Continue input checking */
  if(sampleNr<10) sampleNr=50*fittedParNr;
  if(sampleNr&1) sampleNr++; // If number is odd, then add 1
  if(neighNr<2) neighNr=sampleNr/2;
  else if(neighNr>sampleNr-1) neighNr=sampleNr-1; // Make sure "neighNr" isn't too big
  if(tgoNr==0) tgoNr=1+fittedParNr;
  if(verbose>2) {
    printf("sampleNr := %d\n", sampleNr);
    printf("neighNr := %d\n", neighNr);
    printf("tgoNr := %d\n", tgoNr);
    fflush(stdout);
  }
 
  /* Check if initial guess is valid */
  int initGuessAvailable=1;
  double initGuessCost=nan("");
  for(unsigned int i=0; i<nlo->totalNr; i++) {
    if(isnan(nlo->xfull[i])) {initGuessAvailable=0; break;}
    if(nlo->xfull[i]<nlo->xlower[i]) {initGuessAvailable=0; break;}
    if(nlo->xfull[i]>nlo->xupper[i]) {initGuessAvailable=0; break;}
  }
  if(initGuessAvailable) {
    initGuessCost=nlo->funval=(*nlo->_fun)(nlo->totalNr, nlo->xfull, nlo->fundata);
    nlo->funCalls++;
    if(!isfinite(initGuessCost)) initGuessAvailable=0;
    else if(verbose>2) {
      printf("valid initial guess with cost=%g\n", initGuessCost);
    }
  }
 
  /* Allocate memory */
  TGO_POINT *points=(TGO_POINT*)malloc(sampleNr*sizeof(TGO_POINT));
  if(points==NULL) {
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_OUT_OF_MEMORY);
    return TPCERROR_OUT_OF_MEMORY;
  }
  for(unsigned int i=0; i<sampleNr; i++) {
    points[i].topomin=0;
    points[i].fvalue=0.0;
  }
  TGO_POINT *gpoints=(TGO_POINT*)malloc(sampleNr*sizeof(TGO_POINT));
  if(gpoints==NULL) {
    free(points);
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_OUT_OF_MEMORY);
    return TPCERROR_OUT_OF_MEMORY;
  }
  for(unsigned int i=0; i<sampleNr; i++) {
    gpoints[i].topomin=0;
    gpoints[i].fvalue=0.0;
  }
 
  /*
   *  Iterative TGO, or non-iterative if tgoNr==1
   */
  unsigned int topoNr=0;
  if(initGuessAvailable) {
    /* If initial guess was given, then use it as the first G point */
    for(unsigned int i=0; i<nlo->totalNr; i++) gpoints[0].par[i]=nlo->xfull[i];
    gpoints[0].fvalue=initGuessCost;
    topoNr=1;
  }
  for(unsigned int tgoi=0; tgoi<tgoNr; tgoi++) {
 
    if(verbose>2 && tgoNr>1) {printf("TGO iteration # %d: \n", 1+tgoi); fflush(stdout);}
 
    while(topoNr<sampleNr) {
      if(verbose>3) {printf("  topoNr := %d\n", topoNr); fflush(stdout);}
 
      /* Sample N points and compute the object function values for all points. */
      unsigned int badNr=0;
      for(unsigned int si=0; si<sampleNr; si++) {
        points[si].topomin=0;
        nloptRandomPoint(points[si].par, nlo->xlower, nlo->xupper, nlo->totalNr, NULL);
        points[si].fvalue=(*nlo->_fun)(nlo->totalNr, points[si].par, nlo->fundata);
        nlo->funCalls++;
        /* If function return value was not valid, then we'll try twice with new guesses */
        int tries=0;
        while(!isfinite(points[si].fvalue) && tries<2) {
          if(verbose>8) {
            printf("this point did not give normal return value:\n");
            for(unsigned int i=0; i<nlo->totalNr; i++) printf("  %10.2e", points[si].par[i]);
            printf("\n");
          }
          nloptRandomPoint(points[si].par, nlo->xlower, nlo->xupper, nlo->totalNr, NULL);
          points[si].fvalue=(*nlo->_fun)(nlo->totalNr, points[si].par, nlo->fundata);
          nlo->funCalls++;
          tries++;
        }
        if(!isfinite(points[si].fvalue)) badNr++;
      }
      if(verbose>4 && badNr>0) printf("Number of bad points: %d\n", badNr);
      /* Object functions values must be good for at least NeighNr points */
      if(tgoi==0 && (sampleNr-badNr)<=neighNr) {
        if(verbose>0) {
          fprintf(stderr, "Error: invalid values inside parameter range.\n"); fflush(stderr);}
        free(points); free(gpoints);
        statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_BAD_FIT);
        return TPCERROR_BAD_FIT;
      }
 
      /* For each point i, find out if it is a "topographic minimum", that is, 
         better than k neighbour points. */
      for(unsigned int si=0; si<sampleNr; si++) {
        points[si].topomin=0;
 
        /* Compute the distances, scaled using xtol[] */
        double sdist[sampleNr];
        for(unsigned int sj=0; sj<sampleNr; sj++) {
          sdist[sj]=0.0;
          if(si==sj) {sdist[sj]=1.0E+99; continue;}
          for(unsigned int k=0; k<nlo->totalNr; k++) {
            if(!(nlo->xtol[k]>0.0)) continue;
            double d=(points[si].par[k]-points[sj].par[k])/nlo->xtol[k];
            if(isfinite(d)) sdist[sj]+=d*d;
          }
          /* Distance should be computed as square root, but it does not affect
             the order of distances; since sqrt() is slow, it is not done. */
          // sdist[sj]=sqrt(sdist[sj]);
        }
 
        /* Find the closest neighbours for sample i */
        unsigned int ni=0;
        for(ni=0; ni<neighNr; ni++) {
          unsigned int mini=0;
          double minv=sdist[mini];
          for(unsigned int sj=0; sj<sampleNr; sj++) {
            if(sdist[sj]<minv) {minv=sdist[sj]; mini=sj;}
          }
          sdist[mini]=1.0E+99; // not used again
          /* If point i is worse than any of the closest neighbours, then point i is certainly not 
             a topographic minimum; then stop this loop and go to the next point i+1 */
          if(!(points[si].fvalue<points[mini].fvalue)) break;
        }
        if(ni<neighNr) continue; // continue with the next sample i
        /* otherwise mark this as topographic minimum (TM) */
        points[si].topomin=1;
        /* and copy it to G point list (if there is space left) */
        if(topoNr<sampleNr) {
          for(unsigned int i=0; i<nlo->totalNr; i++) gpoints[topoNr].par[i]=points[si].par[i];
          gpoints[topoNr++].fvalue=points[si].fvalue;
        }
        /* Print TM point */
        if(verbose>6) {
          printf("TM point %d\t", 1+si);
          for(unsigned int i=0; i<nlo->totalNr; i++) printf("%e ", points[si].par[i]);
          printf("=> %e\n", points[si].fvalue);
          fflush(stdout);
        }
      }
    } // inner loop 
 
 
    if(verbose>3) {printf("  process gpoints.\n"); fflush(stdout);}
    /* For each point i, find out if it is a "topographic minimum", that is, 
       better than k/2 neighbour points. */
    for(unsigned int si=0; si<sampleNr; si++) {
      gpoints[si].topomin=0;
 
      /* Compute the distances, scaled using xtol[] */
      double sdist[sampleNr];
      for(unsigned int sj=0; sj<sampleNr; sj++) {
        sdist[sj]=0.0;
        if(si==sj) {sdist[sj]=1.0E+99; continue;}
        for(unsigned int k=0; k<nlo->totalNr; k++) {
          if(!(nlo->xtol[k]>0.0)) continue;
          double d=(gpoints[si].par[k]-gpoints[sj].par[k])/nlo->xtol[k];
          if(isfinite(d)) sdist[sj]+=d*d;
        }
      }
 
      /* Find the closest neighbours for sample i */
      unsigned int ni=0;
      for(ni=0; ni<neighNr; ni++) {
        unsigned int mini=0;
        double minv=sdist[mini];
        for(unsigned int sj=0; sj<sampleNr; sj++) {
          if(sdist[sj]<minv) {minv=sdist[sj]; mini=sj;}
        }
        sdist[mini]=1.0E+99; // not used again
        /* If point i is worse than any of the closest neighbours, then point i is certainly not 
           a topographic minimum; then stop this loop and go to the next point i+1 */
        if(!(gpoints[si].fvalue<gpoints[mini].fvalue)) break;
      }
      if(ni<neighNr) continue; // continue with the next sample i
      /* otherwise mark this as topographic minimum (TM) */
      gpoints[si].topomin=1;
      /* Print TM point */
      if(verbose>6) {
        printf("GTM point %d\t", 1+si);
        for(unsigned int i=0; i<nlo->totalNr; i++) printf("%e ", gpoints[si].par[i]);
        printf("=> %e\n", gpoints[si].fvalue);
        fflush(stdout);
      }
    }
    /* Copy TMs into the start of gpoint list and set topoNr accordingly */
    topoNr=0;
    for(unsigned int si=0; si<sampleNr; si++) if(gpoints[si].topomin>0) {
      if(si>topoNr) {
        for(unsigned int i=0; i<nlo->totalNr; i++) gpoints[topoNr].par[i]=gpoints[si].par[i];
        gpoints[topoNr].fvalue=gpoints[si].fvalue;
      }
      topoNr++;
    }
    if(verbose>3) {printf("  %d topographical minima\n", topoNr); fflush(stdout);}
    if(topoNr==0) {
      if(verbose>0) {fprintf(stderr, "Warning: no topographical minima found\n"); fflush(stderr);}
      continue;
    }
 
 
  } // end of iTGO
 
 
  /* If requested, do local optimization for the TMs */
  if(topoNr==0) {
    if(verbose>0) {fprintf(stderr, "Error: no topographical minima found\n"); fflush(stderr);}
    free(points); free(gpoints);
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_BAD_FIT);
    return TPCERROR_BAD_FIT;
  }
  for(unsigned int si=0; si<sampleNr; si++) if(gpoints[si].topomin>0) {
    if(verbose>2) printf("LO for TM point %d\n", 1+si);
    if(verbose>3) {
      printf("TM point %d before LO:\n", 1+si);
      printf("%d\t", 1+si);
      for(unsigned int i=0; i<nlo->totalNr; i++) printf("%e ", gpoints[si].par[i]);
      printf("=> %e\n", gpoints[si].fvalue); fflush(stdout);
    }
    NLOPT lnlo; nloptInit(&lnlo);
    nloptDuplicate(nlo, &lnlo);
    for(unsigned int i=0; i<nlo->totalNr; i++) lnlo.xfull[i]=gpoints[si].par[i];
    lnlo.funval=points[si].fvalue;
    for(unsigned int i=0; i<nlo->totalNr; i++) lnlo.xdelta[i]=100.0*lnlo.xtol[i];
    lnlo.maxFunCalls=2000*nlo->totalNr; lnlo.funCalls=0;
    int ret=nloptSimplex(&lnlo, 100*nlo->totalNr, status);
    if(ret==TPCERROR_OK) {
      for(unsigned int i=0; i<nlo->totalNr; i++) gpoints[si].par[i]=lnlo.xfull[i];
      gpoints[si].fvalue=lnlo.funval;
      if(verbose>3) {
        printf("TM point %d after LO:\n", 1+si);
        printf("%d\t", 1+si);
        for(unsigned int i=0; i<nlo->totalNr; i++) printf("%e ", gpoints[si].par[i]);
        printf("=> %e\n", gpoints[si].fvalue); fflush(stdout);
      }
    } else if(verbose>7) {printf("  LO failed\n"); fflush(stdout);}
    nlo->funCalls+=lnlo.funCalls;
    nloptFree(&lnlo);
  }
 
  /* Find the best minimum and return it */
  {
    unsigned int mini=0;
    double minv=gpoints[mini].fvalue;
    for(unsigned int si=1; si<sampleNr; si++)
      if(!(gpoints[si].fvalue>minv)) {minv=gpoints[si].fvalue; mini=si;}
    for(unsigned int i=0; i<nlo->totalNr; i++) nlo->xfull[i]=gpoints[mini].par[i];
    nlo->funval=gpoints[mini].fvalue;
  }
 
  free(points); free(gpoints);
  statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_OK);
  return TPCERROR_OK;
}
/*****************************************************************************/
 
/*****************************************************************************/
int nloptIATGO(
  NLOPT *nlo,
  const int doLocal,
  unsigned int maxIterNr,
  double neighFract,
  TPCSTATUS *status
) {
  FILE *fp=stdout;
  int verbose=0; if(status!=NULL) {verbose=status->verbose; fp=status->fp;}
  if(verbose>1) {
    fprintf(fp, "%s(NLOPT, %d, %u, %g, status)\n", __func__, doLocal, maxIterNr, neighFract);
    fflush(fp);
  }
  if(nlo==NULL ) {
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL);
    return TPCERROR_FAIL;
  }
  if(nlo->totalNr<1 || nlo->xfull==NULL || nlo->_fun==NULL) {
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_NO_DATA);
    return TPCERROR_NO_DATA;
  }
 
  unsigned int dim=nlo->totalNr;
 
  if(verbose>4) {
    fprintf(fp, "\nInitial limits and tolerances:\n");
    for(unsigned int i=0; i<dim; i++)
      fprintf(fp, "\t%g\t%g\t%e\n", nlo->xlower[i], nlo->xupper[i], nlo->xtol[i]);
  }
 
  /* Check if any of the parameters are fixed */
  unsigned int fixedParNr=nloptLimitFixedNr(nlo);
  if(verbose>2 && fixedParNr>0) fprintf(fp, "fixedParNr := %d\n", fixedParNr);
  unsigned int fittedParNr=nlo->totalNr-fixedParNr;
  if(verbose>2) {fprintf(fp, "fittedParNr := %d\n", fittedParNr); fflush(fp);}
  if(fittedParNr<1) {
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_NO_DATA);
    return TPCERROR_NO_DATA;
  }
 
  /* Check the tolerations */
  for(unsigned int i=0; i<dim; i++) {
    if(nlo->xlower[i]>=nlo->xupper[i]) {nlo->xtol[i]=0.0; continue;}
    if(!(nlo->xtol[i]>0.0)) {
      if(verbose>0) {fprintf(stderr, "Error: invalid xtol[].\n"); fflush(stderr);}
      statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_NO_DATA);
      return TPCERROR_NO_DATA;
    }
  }
 
  /* Continue input checking */
  if(neighFract<0.04) neighFract=0.04; else if(neighFract>0.97) neighFract=0.97;
  unsigned int sampleNr=50*fittedParNr/neighFract;
  if(maxIterNr==0) maxIterNr=5+2*fittedParNr;
  unsigned int neighNr=neighFract*sampleNr;
  if(verbose>2) {
    fprintf(fp, "sampleNr := %u\n", sampleNr);
    fprintf(fp, "neighFract := %g\n", neighFract);
    fprintf(fp, "neighNr := %u\n", neighNr);
    fprintf(fp, "maxIterNr := %u\n", maxIterNr);
    fflush(stdout);
  }
 
 
  /*
   *  Initialize the sample list with random points
   */
  if(verbose>3) {fprintf(fp, "Making initial random samples\n"); fflush(fp);}
 
  /* Allocate memory */
  nlo->usePList=1; // All points are stored
  nlo->funCalls=0; 
  nlo->plist=(double*)malloc(sampleNr*(dim+1)*sizeof(double));
  if(nlo->plist==NULL) { // will be freed with nloptFree()
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_OUT_OF_MEMORY);
    return TPCERROR_OUT_OF_MEMORY;
  }
 
  /* Fill the list with random points and compute the object function values */
  {
    unsigned int badNr=0;
    /* Add the random points */
    for(unsigned int si=0; si<sampleNr; si++) {
      if(nloptRandomPoint(&nlo->plist[si*(dim+1)], nlo->xlower, nlo->xupper, dim, NULL)) {
        statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_NO_DATA);
        return TPCERROR_NO_DATA;
      }
      nlo->plist[si*(dim+1)+dim]=(*nlo->_fun)(nlo->totalNr, &nlo->plist[si*(dim+1)], nlo->fundata);
      nlo->funCalls++;
      if(!isfinite(nlo->plist[si*(dim+1)+dim])) badNr++;
    }
    if(verbose>3 && badNr>0) fprintf(fp, "Number of bad points: %d\n", badNr);
    /* Not all object function values can be bad */
    if((sampleNr-badNr)<10) {
      if(verbose>0) {
        fprintf(stderr, "Error: invalid values inside parameter range.\n"); fflush(stderr);}
      statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_BAD_FIT);
      return TPCERROR_BAD_FIT;
    }
    /* Try new points to replace the failed ones */
    if(badNr>0) {
      badNr=0;
      for(unsigned int si=0; si<sampleNr; si++) if(!isfinite(nlo->plist[si*(dim+1)+dim])) {
        nloptRandomPoint(&nlo->plist[si*(dim+1)], nlo->xlower, nlo->xupper, dim, NULL);
        nlo->plist[si*(dim+1)+dim]=(*nlo->_fun)(nlo->totalNr, &nlo->plist[si*(dim+1)], nlo->fundata);
        //nlo->funCalls++;
        if(!isfinite(nlo->plist[si*(dim+1)+dim])) badNr++;
      }
      if(verbose>4 && badNr>0) fprintf(fp, "Number of bad points after retry: %d\n", badNr);
    }
  }
 
  /* If initial guess is valid, add it to the list */
  int initGuessAvailable=1;
  double initGuessCost=nan("");
  for(unsigned int i=0; i<dim; i++) {
    if(isnan(nlo->xfull[i])) {initGuessAvailable=0; break;}
    if(nlo->xfull[i]<nlo->xlower[i]) {initGuessAvailable=0; break;}
    if(nlo->xfull[i]>nlo->xupper[i]) {initGuessAvailable=0; break;}
  }
  if(initGuessAvailable) {
    initGuessCost=nlo->funval=(*nlo->_fun)(nlo->totalNr, nlo->xfull, nlo->fundata);
    if(isfinite(initGuessCost)) {
      nlo->funCalls++; 
      nloptAddP(nlo, nlo->xfull, initGuessCost);
    } else {
      initGuessAvailable=0;
    }
  }
  if(verbose>2) {
    if(initGuessAvailable) fprintf(fp, "valid initial guess with cost=%g\n", initGuessCost);
    else fprintf(fp, "no valid initial guess provided.\n");
  }
 
 
  /*
   *  Start iterations
   */
  if(verbose>2) {fprintf(fp, "\nStarting TGO iterations\n"); fflush(fp);}
  unsigned int iterNr=0, stopCounter=0;
  double prevBest=nan("");
  while(iterNr<maxIterNr && nlo->funCalls<nlo->maxFunCalls) {
    iterNr++;
    unsigned int sampleNr=nlo->funCalls; // LOCAL sampleNr, increasing in each iteration
    if(verbose>3) {fprintf(fp, "\nIteration %d with %d samples\n", iterNr, sampleNr); fflush(fp);}
 
    /* Sort samples based on the evaluated function value */
    if(nloptSortP(nlo)!=TPCERROR_OK) {
      statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL); return TPCERROR_FAIL;}
 
    /* Print sampled points */
    if(verbose>15) {fprintf(fp, "Points so far:\n"); nloptPrintP(nlo, 0, fp);}
    else if(verbose>6) {fprintf(fp, "Best points so far:\n"); nloptPrintP(nlo, 10, fp);}
    else if(verbose>4) {
      fprintf(fp, "best point so far:");
      for(unsigned int i=0; i<dim; i++) fprintf(fp, " %e", nlo->plist[i]);
      fprintf(fp, " => %e\n", nlo->plist[dim]); fflush(fp);
    }
 
    /* If SD of the best points is minimal, and fit is not improving, then stop */
    if(verbose>4 && !isnan(prevBest))
      fprintf(fp, "dif_to_prev_point :=  %e\n", prevBest-nlo->plist[dim]);
    if(!isnan(prevBest) && (prevBest-nlo->plist[dim])<1.0E-20) {
      if(verbose>4) fprintf(fp, "Best point did not improve.\n");
      if(nloptMeanP(nlo, neighNr, nlo->xfull, nlo->xdelta)==TPCERROR_OK) {
        unsigned int i=0;
        for(i=0; i<dim; i++) if(fabs(nlo->xdelta[i])>nlo->xtol[i]) break;
        if(i==dim) {
          if(verbose>2) fprintf(fp, "Small SD of best points.\n");
          stopCounter++;
        } else stopCounter=0;
      }
    } else stopCounter=0;
    prevBest=nlo->plist[dim];
 
    /* Stop, if stopping rules bumped into several times */
    if(verbose>4) fprintf(fp, "stopCounter := %d\n", stopCounter);
    if(stopCounter>2) {
      if(verbose>2) fprintf(fp, "\n Required tolerance reached.\n");
      break;
    }
 
    /* Find topographic minima (TM) */
 
    /* Allocate a list specifying whether point is local minimum (1) or not (0) */
    /* Allocate a list specifying whether point is part of topographic minimum (>0) or not (0) */
    unsigned int tmlist[sampleNr];
    for(unsigned int i=0; i<sampleNr; i++) tmlist[i]=0;
 
    unsigned int topoNr=0;
    if(verbose>3) {fprintf(fp, "neighNr := %d\n", neighNr); fflush(fp);}
 
    /* For each point i, find out if it is a "topographic minimum", that is, 
       better than k neighbour points. */
    for(unsigned int si=0; si<sampleNr-neighNr; si++) if(tmlist[si]==0) {
      
      /* If function value is invalid, then this cannot be accepted as TM */
      if(!isfinite(nlo->plist[si*(dim+1)+dim])) continue;
 
      /* Compute the distances to other points, scaled using xtol[] */
      double sdist[sampleNr];
      for(unsigned int sj=0; sj<sampleNr; sj++) {
        if(si==sj || !isfinite(nlo->plist[sj*(dim+1)+dim])) { // point itself not included and
          sdist[sj]=nan(""); continue;}                       // valid function value required
        sdist[sj]=0.0;
        for(unsigned int k=0; k<dim; k++) {
          if(!(nlo->xtol[k]>0.0)) continue;
          double d=(nlo->plist[si*(dim+1)+k]-nlo->plist[sj*(dim+1)+k])/nlo->xtol[k];
          /*if(isfinite(d))*/ sdist[sj]+=d*d;
        }
        /* If scaled distance is less than 1, the points are practically equal: leave out */
        //if(sdist[sj]<1.0) sdist[sj]=nan(""); // would lead to multiple local minima at same point
      }
      double sdist2[sampleNr]; // make copy for printing
      for(unsigned int sj=0; sj<sampleNr; sj++) sdist2[sj]=sdist[sj];
 
      /* Find its closest neighbours that are not any better */
      unsigned int nn=0; double prev_mind=nan("");
      while(1) {
        /* Find the closest neighbour that was not already found */
        unsigned int mini=0;
        double mind=nan("");
        for(unsigned int sj=0; sj<sampleNr; sj++) {
          if(!isfinite(mind) || mind>sdist[sj]) {mind=sdist[sj]; mini=sj;}
        }
        if(!isfinite(mind)) break;
        sdist[mini]=nan(""); // this point not used again in search of neighbours
        if(verbose>100) fprintf(fp, "  min_distance[%u] := %g , with fval := %g\n",
                                1+nn, mind, nlo->plist[mini*(dim+1)+dim]);
        /* If this neighbour is better than the tentative TM, then stop */
        if(nlo->plist[mini*(dim+1)+dim] < nlo->plist[si*(dim+1)+dim]) break;
        /* Otherwise, count this as a worse neighbour, belonging to this TM */
        nn++; tmlist[mini]=1+si;
        // we do not want to include more than neighNr points into this TM,
        // but if distance is less than one or not larger than previously then 
        // this is essentially the same point
        if(isnan(prev_mind)) prev_mind=mind;
        if(nn>=neighNr && mind>1.0 && mind>prev_mind) break;
        prev_mind=mind;
      }
      /* If the number of worse neighbours is less than required, then stop considering this
         as a local optimum, and continue with the next sample */
      if(nn<neighNr) {
        /* its neighbours then do not belong to TM either */
        for(unsigned int ni=0; ni<sampleNr; ni++) if(tmlist[ni]==1+si) tmlist[ni]=0;
        continue;
      }
      /* otherwise mark this as topographic minimum (TM) */
      tmlist[si]=1+si;
      topoNr++;
      /* Print TM point */
      if(verbose>5) {
        fprintf(fp, "TM point %d\t", 1+si);
        for(unsigned int i=0; i<dim; i++) fprintf(fp, "%e ", nlo->plist[si*(dim+1)+i]);
        fprintf(fp, "=> %e\n", nlo->plist[si*(dim+1)+dim]);
        if(verbose>14) {
          fprintf(fp, "and its %u neighbours:\n", nn);
          for(unsigned int sj=0; sj<sampleNr; sj++) if(sj!=si && tmlist[sj]==1+si) {
            for(unsigned int i=0; i<dim; i++) fprintf(fp, "%e ", nlo->plist[sj*(dim+1)+i]);
            fprintf(fp, "=> %e\t(%g)\n", nlo->plist[sj*(dim+1)+dim], sdist2[sj]);
          }
        }
        fflush(fp);
      }
    }
    if(verbose>3) {fprintf(fp, "topographic minima: %d\n", topoNr); fflush(fp);}
    if(topoNr==0) {
      if(verbose>0) {fprintf(stderr, "Error: no topographical minima found\n"); fflush(stderr);}
      statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_BAD_FIT);
      return TPCERROR_BAD_FIT;
    }
 
    /* 
     *  Local optimization from each TM
     */
    for(unsigned int si=0; si<sampleNr; si++) if(tmlist[si]==1+si) {
      if(verbose>5) {fprintf(fp, "LO for TM point %d\n", 1+si); fflush(fp);}
      if(verbose>6) {
        fprintf(fp, "TM point %d before LO:\n", 1+si);
        for(unsigned int i=0; i<dim; i++) fprintf(fp, " %e", nlo->plist[si*(dim+1)+i]);
        fprintf(fp, " => %e\n", nlo->plist[si*(dim+1)+dim]); fflush(fp);
      }
      /* Compute the mean and SD of points belonging to this TM */
      double meanp[dim], sdp[dim]; unsigned int nn=0;
      for(unsigned int i=0; i<dim; i++) {
        if(nlo->xupper[i]>nlo->xlower[i]) {
          double x[sampleNr];
          nn=0;
          for(unsigned int sj=0; sj<sampleNr; sj++)
            if(tmlist[sj]==1+si) x[nn++]=nlo->plist[sj*(dim+1)+i];
          statMeanSD(x, nn, meanp+i, sdp+i, NULL);
        } else {
          meanp[i]=nlo->xlower[i]; sdp[i]=0.0;
        }
      }
      if(verbose>7) {
        fprintf(fp, "TM point (n=%u) mean and SD:\n", nn);
        for(unsigned int i=0; i<dim; i++) fprintf(fp, "\t%g\t%g\n", meanp[i], sdp[i]);
        fflush(fp);
      }
      /* Make sure that SD is not zero, unless parameter is fixed */
      for(unsigned int i=0; i<dim; i++) if(nlo->xupper[i]>nlo->xlower[i]) {
        if(sdp[i]<0.1*nlo->xtol[i]) sdp[i]=drandExponential(0.05*nlo->xtol[i]);
      }
      if(doLocal==1) { // downhill simplex from the best point so far
        doubleCopy(nlo->xfull, &nlo->plist[si*(dim+1)], dim);
        doubleCopy(nlo->xdelta, sdp, dim);
        int ret=nloptSimplex(nlo, 100*dim, status);
        if(ret!=TPCERROR_OK) {
          if(verbose>1) {fprintf(fp, "  LO failed: %s\n", errorMsg(ret)); fflush(fp);}
        } else {
          if(verbose>6) {
            fprintf(fp, "Point after LO:");
            for(unsigned int i=0; i<dim; i++) fprintf(fp, " %e ", nlo->xfull[i]);
            fprintf(fp, " => %e\n", nlo->funval); fflush(fp);
          }
        }
      } else { // Add random points with Gaussian distribution
        unsigned int bi=0; double bval=nan("");
        for(unsigned int ni=0; ni<neighNr; ni++) {
          double p[dim];
          nloptGaussianPoint(p, &nlo->plist[si*(dim+1)], sdp, nlo->xlower, nlo->xupper, dim, NULL);
          double fval=(*nlo->_fun)(dim, p, nlo->fundata);
          if(isnan(bval) || bval>fval) {bval=fval; bi=nlo->funCalls;}
          if(isfinite(fval)) {nlo->funCalls++; nloptAddP(nlo, p, fval);}
        }
        if(verbose>6) {
          fprintf(fp, "Best TM point %d after LO:\n", 1+si);
          for(unsigned int i=0; i<dim; i++) fprintf(fp, " %e", nlo->plist[bi*(dim+1)+i]);
          fprintf(fp, " => %e\n", nlo->plist[bi*(dim+1)+dim]); fflush(fp);
        }
      }
    } // next TM
 
  } // end of iterations
 
  /* Check the reason for loop exit */
  if(verbose>1) {
    if(iterNr>=maxIterNr)
      fprintf(fp, "\n Exceeded the maximum number for loops.\n");
    if(nlo->funCalls>=nlo->maxFunCalls)
      fprintf(fp, "\n Exceeded the maximum number for function calls.\n");
    fflush(fp);
  }
 
  /* Sort samples based on the evaluated function value */
  if(nloptSortP(nlo)!=TPCERROR_OK) {
    statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_FAIL); return TPCERROR_FAIL;}
  /* Get the best point so far */
  for(unsigned int i=0; i<dim; i++) nlo->xfull[i]=nlo->plist[i];
  nlo->funval=nlo->plist[dim];
 
 
  statusSet(status, __func__, __FILE__, __LINE__, TPCERROR_OK);
  return TPCERROR_OK;
}
/*****************************************************************************/
 
/*****************************************************************************/