Simplex

#ifndef DBL_MAX
  /*!
   * \brief A macro (constant) for maximal positive double value.
   */
  #define DBL_MAX 1.7976931348623157E+308
#endif

#ifndef DBL_NMIN
  /*!
   * \brief A macro (constant) for minimal negative double value.
   */
  #define DBL_NMIN -DBL_MAX
#endif


struct SegmentData {
  int unixTime; /*!< Time of data measurement (in unix form). */
  double time; /*!< Time of data measurement (in rational form). */
  double blood; /*!< Measured glucose in blood. */
  double ist; /*!< Measured ist - glucose measured under the skin. */
};

struct Segment {
  unsigned int id; /*!< Identifier of measured segment. */
  std::vector<SegmentData> data; /*!< Measured segment data. */
  double timeFrom; /*!< Time of segment beginning (in rational form). */
  double timeTo; /*!< Time of segment end (in rational form). */
};

struct FilteredSegment : Segment {
  std::vector<SegmentData> filteredData; /*!< Filtered data which contain only data with measured blood and ist. */
};


static double evaluateFi(Segment& segment, unsigned int dataId, double *params) {
  double t = segment.data[dataId].time;
  double h = params[PARAM_H_INDEX];

  // prevent divining by 0
  if (h == 0.0) {
    return DBL_NMIN;
  }

  double ist = getIstValueAt(segment, t - h);
  if (ist == DBL_NMIN) {
    return ist; // returns DBL_NMIN
  }

  return t + params[PARAM_DT_INDEX] + params[PARAM_K_INDEX] * ((segment.data[dataId].ist - ist) / h);
}

static double evaluateBeta(double ist, double *params) {
  return params[PARAM_P_INDEX] - params[PARAM_CG_INDEX] * ist;
}

static double evaluateGama(Segment& segment, unsigned int dataId, double *params) {
  double fi = evaluateFi(segment, dataId, params);
  if (fi == DBL_NMIN) {
    return fi; // returns DBL_NMIN
  }

  double ist = getIstValueAt(segment, fi);
  if (ist == DBL_NMIN) {
    return ist; // returns DBL_NMIN
  }

  return params[PARAM_C_INDEX] - ist;
}

static double evaluateD(Segment& segment, unsigned int dataId, double *params) {
  double alpha = params[PARAM_CG_INDEX];
  double beta = evaluateBeta(segment.data[dataId].ist, params);
  double gama = evaluateGama(segment, dataId, params);
  if (gama == DBL_NMIN) {
    return gama; // returns DBL_NMIN
  }

  return pow(beta, 2) - 4 * alpha * gama;
}

double evaluateTheoreticalBlood(Segment& segment, unsigned int dataId, double *params) {
  double alpha = params[PARAM_CG_INDEX];
  // prevent divining by 0
  if (alpha == 0.0) {
    return DBL_NMIN;
  }

  double d = evaluateD(segment, dataId, params);
  if (d < 0) {
    return DBL_NMIN; // minimal double value = unknown value, there is no sense to continue
  }
  if (d == DBL_NMIN) {
    return d; // returns DBL_NMIN
  }

  double beta = evaluateBeta(segment.data[dataId].ist, params);

  return (-1 * beta + sqrt(d)) / (2 * alpha);
}

const Fitness fitness(FilteredSegment& segment, vector<double>& params) {
  Fitness f; // result of fitness function
  f.dataCount = segment.filteredData.size();

  double epsilon = 0.0; // summary of relative mistakes e(t) for computed b(t)
  double epsilonSqr = 0.0; // summary of squared e(t) [e(t)^2]

  vector<SegmentData> data = segment.filteredData;
  for (size_t i = 0; i < data.size(); i++) {
    double blood = data[i].blood;
    double theoretical = evaluateTheoreticalBlood(segment, i, &params[0]);
    if (theoretical == DBL_NMIN) {
      continue;
    }

    //printf("b(t) = %0.5f, blood = %0.5f\n", theoretical, blood);

    double e = abs((blood - theoretical) / blood); // e = (x - x0) / x0
    epsilon += e;
    epsilonSqr += pow(e, 2);
    f.calcCount++;
  }

  f.epsilon = epsilon / (double)f.calcCount; // average of e(t): x = 1/N * sum(x_i)
  double averageSqr = epsilonSqr / (double)f.calcCount; // average of squared e(t): 1/N * sum(x_i^2)
  f.deviation = sqrt(averageSqr - pow(f.epsilon, 2)); // o = sqrt((1/N * sum(x_i^2)) - x^2)

  unsigned int minData = (unsigned int)(getAppConfig().minDataCoef * f.dataCount);
  if (f.calcCount >= minData && f.calcCount > 0) {
    f.fitness = f.epsilon + f.deviation;
  }
  else {
    f.fitness = DBL_MAX;
  }

  return f;
}

const SimplexResult simplex(Segment& segment, Bounds& bounds) {
  printf("Starting simplex optimalization for segment %d\n", segment.id);
  high_resolution_clock::time_point start = high_resolution_clock::now(); // start timer

  const Config appConfig = getAppConfig(); // configuration of application

  const double epsilon = appConfig.epsilon * numeric_limits<double>::epsilon(); // termination criteria
  const int iterations = appConfig.iterations;

  const int N = PARAMS_COUNT; // N: space dimension

  // filters segment
  FilteredSegment fSegment = filterSegment(segment);

  // initializes the simplex
  vector<vector<double>> x = vector<vector<double>>(N + 1); // x: the Simplex
  for (size_t i = 0; i < x.size(); i++) {
    for (int j = 0; j < appConfig.initIterations; j++) {
      vector<double> xi = generateParameters(bounds);
      Fitness fi = fitness(fSegment, xi);
      if (fi.fitness != DBL_MAX) {
        x[i] = xi;
        break;
      }
    }
  }

  vector<double> xcentroidOld(N, 0); // simplex center * (N+1)
  vector<double> xcentroidNew(N, 0); // simplex center * (N+1)

  vector<Fitness> vf(N + 1); // fitness evaluated at simplex vertexes

  size_t x1 = 0;   // x1:   f(x1) = min { f(x1), f(x2)...f(x_{n+1} }
  size_t xn = 0;   // xnp1: f(xnp1) = max { f(x1), f(x2)...f(x_{n+1} }
  size_t xnp1 = 0; // xn:   f(xn)<f(xnp1) && f(xn)> all other f(x_i)

  int it = 0; // iteration step number
  // optimization begins
  for (it = 0; it < iterations; it++) {
    for (int i = 0; i < N + 1; i++) {
      vf[i] = fitness(fSegment, x[i]);
    }

    // find index of max, second max, min of vf.
    x1 = 0; xn = 0; xnp1 = 0;

    for (size_t i = 0; i < vf.size(); i++) {
      if (vf[i] < vf[x1]) {
        x1 = i;
      }
      if (vf[i] > vf[xnp1]) {
        xnp1 = i;
      }
    }

    xn = x1;

    for (size_t i = 0; i < vf.size(); i++) {
      if (vf[i]<vf[xnp1] && vf[i]>vf[xn]) {
        xn = i;
      }
    }

    // x1, xn, xnp1 are found

    vector<double> xg(N, 0); // xg: centroid of the N best vertexes
    for (size_t i = 0; i < x.size(); i++) {
      if (i != xnp1) {
        transform(xg.begin(), xg.end(), x[i].begin(), xg.begin(), plus<double>());
      }
    }

    transform(xg.begin(), xg.end(), x[xnp1].begin(), xcentroidNew.begin(), plus<double>());
    transform(xg.begin(), xg.end(), xg.begin(), bind2nd(divides<double>(), N));
    // xg found, xcentroid_new updated

    // termination condition
    if (appConfig.epsilonEnabled) {
      double diff = 0; //calculate the difference of the simplex centers see if the difference is less than the termination criteria
      for (int i = 0; i < N; i++) {
        diff += fabs(xcentroidOld[i] - xcentroidNew[i]);
      }

      // the final solution cannot have this fitness
      if ((diff / N < epsilon) && vf[x1].fitness != DBL_MAX) {
        break; //terminate the optimizer
      }
    }

    xcentroidOld.swap(xcentroidNew); //update simplex center

    // reflection:
    vector<double> xr(N, 0);
    for (int i = 0; i < N; i++) {
      xr[i] = xg[i] + A * (xg[i] - x[xnp1][i]);
    }
    // reflection, xr found

    parametersCorrection(bounds, xr); // correct parameters
    Fitness fxr = fitness(fSegment, xr); // record function at xr
    if (vf[x1] <= fxr && fxr <= vf[xn]) {
      copy(xr.begin(), xr.end(), x[xnp1].begin());
    }

    //expansion:
    else if (fxr < vf[x1]) {
      vector<double> xe(N, 0);
      for (int i = 0; i < N; i++) {
        xe[i] = xr[i] + B * (xr[i] - xg[i]);
      }

      parametersCorrection(bounds, xe); // correct parameters
      if (fitness(fSegment, xe) < fxr) {
        copy(xe.begin(), xe.end(), x[xnp1].begin());
      }
      else {
        copy(xr.begin(), xr.end(), x[xnp1].begin());
      }
    }
    // expansion finished, xe is not used outside the scope

    // contraction:
    else if (fxr > vf[xn]) {
      vector<double> xc(N, 0);
      for (int i = 0; i < N; i++) {
        xc[i] = xg[i] + G * (x[xnp1][i] - xg[i]);
      }

      parametersCorrection(bounds, xc); // correct parameters
      if (fitness(fSegment, xc) < vf[xnp1]) {
        copy(xc.begin(), xc.end(), x[xnp1].begin());
      }
      else {
        for (size_t i = 0; i < x.size(); i++) {
          if (i != x1) {
            for (int j = 0; j < N; j++) {
              x[i][j] = x[x1][j] + H * (x[i][j] - x[x1][j]);
            }
            parametersCorrection(bounds, x[i]); // correct parameters
          }
        }
      }
    } // contraction finished, xc is not used outside the scope
  } // optimization is finished

  high_resolution_clock::time_point end = high_resolution_clock::now(); // stop timer
  long long duration = duration_cast<milliseconds>(end - start).count();

  if (it == iterations && appConfig.epsilonEnabled) { //max number of iteration achieves before tol is satisfied
    printf("Warning: Iteration limit achieves, result may not be optimal\n");
  }

  printf("Found parameters for segment %d in %d iterations [%lld ms]\n", segment.id, it, duration);
  printParametersWithFitness(x[x1], vf[x1], false);

  // preparation of result
  SimplexResult result;
  result.segmentId = segment.id;
  result.params = x[x1];
  result.fitness = vf[x1].fitness;
  result.duration = duration;

  return result;
}