// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2013.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// --------------------------------------------------------------------------
// $Maintainer: Alexandra Zerck $
// $Authors: $
// --------------------------------------------------------------------------
//

#include <boost/math/special_functions/acosh.hpp>

#include <OpenMS/TRANSFORMATIONS/RAW2PEAK/OptimizePeakDeconvolution.h>
#include <OpenMS/MATH/MISC/MathFunctions.h>
#include <OpenMS/DATASTRUCTURES/ListUtils.h>
#include <limits>
#include <boost/math/special_functions/fpclassify.hpp>

#ifdef DEBUG_DECONV
#include <iostream>
#include <fstream>
#endif

namespace OpenMS
{

  const DoubleReal OptimizePeakDeconvolution::dist_ = 1.003;
  namespace OptimizationFunctions
  {
    // Evaluation of the target function for nonlinear optimization.
    Int residualDC(const gsl_vector * x, void * params, gsl_vector * f)
    {
      // According to the gsl conventions, x contains the parameters to be optimized.
      // The first two entries are the left and right width, respectively.They are equal
      // for all peaks. Then the height and position of all peaks are stored.
      //
      // Params might contain any additional parameters. We handle these using class members
      // instead.
      // The vector f is supposed to contain the result when we return from this function.
      // Note: GSL wants the values for each data point i as one component of the results vector
      std::vector<DoubleReal> & signal = static_cast<OptimizePeakDeconvolution::Data *>(params)->signal;
      std::vector<DoubleReal> & positions = static_cast<OptimizePeakDeconvolution::Data *>(params)->positions;
      std::vector<PeakShape> & peaks = static_cast<OptimizePeakDeconvolution::Data *>(params)->peaks;
      OptimizationFunctions::PenaltyFactorsIntensity & penalties = static_cast<OptimizePeakDeconvolution::Data *>(params)->penalties;
      Int charge = static_cast<OptimizePeakDeconvolution::Data *>(params)->charge;
      DoubleReal leftwidth = gsl_vector_get(x, 0);
      DoubleReal rightwidth = gsl_vector_get(x, 1);
      //DoubleReal posP1 = gsl_vector_get(x,2);

      // iterate over all points of the signal
      for (Size current_point = 0; current_point < positions.size(); current_point++)
      {
        DoubleReal computed_signal     = 0.;
        DoubleReal current_position    = positions[current_point];
        DoubleReal experimental_signal = signal[current_point];

        //iterate over all peaks
        for (Size current_peak = 0; current_peak < peaks.size(); current_peak++)
        {
          //Store the current parameters for this peak
          DoubleReal p_height        = gsl_vector_get(x, 2 + 2 * current_peak);
          DoubleReal p_position    = gsl_vector_get(x, 2 + 2 * current_peak + 1);
          DoubleReal p_width         = (current_position <= p_position) ? leftwidth : rightwidth;

          //is it a Lorentz or a Sech - Peak?
          if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
          {
            computed_signal += p_height / (1. + pow(p_width * (current_position - p_position), 2));
          }
          else                   // It's a Sech - Peak
          {
            computed_signal += p_height / pow(cosh(p_width * (current_position - p_position)), 2);
          }
        }
        gsl_vector_set(f, current_point, computed_signal - experimental_signal);
      }

      // penalties : especially negative heights have to be penalised
      DoubleReal penalty = 0.;

      DoubleReal penalty_pos    = penalties.pos;
      DoubleReal penalty_lwidth = penalties.lWidth;
      DoubleReal penalty_rwidth = penalties.rWidth;
      DoubleReal penalty_intensity = penalties.height;

      //iterate over all peaks again to compute the penalties
      for (Size current_peak = 0; current_peak < peaks.size(); current_peak++)
      {
        DoubleReal p_position = gsl_vector_get(x, 2 + 2 * current_peak + 1);
        if (current_peak < peaks.size() - 1)
        {

          DoubleReal next_p_position  = gsl_vector_get(x, 2 + 2 * current_peak + 3);
          // if distance between peaks does not match the peptide mass rule
          if (fabs(fabs(p_position - next_p_position) - 1.003 / charge) > 0.05)
          {
            // penalize it
            penalty +=  penalty_pos * 10000
                       * pow(fabs(fabs(p_position - next_p_position) - 1.003 / charge), 2);
          }
        }
        DoubleReal old_position   = peaks[current_peak].mz_position;
        DoubleReal old_width_l  = peaks[current_peak].left_width;
        DoubleReal old_width_r    = peaks[current_peak].right_width;
        DoubleReal old_height    = peaks[current_peak].height;

        DoubleReal p_width_l    = gsl_vector_get(x, 0);
        DoubleReal p_width_r    = gsl_vector_get(x, 1);
        DoubleReal p_height     = gsl_vector_get(x, 2 + 2 * current_peak);

        if (p_height <  1)
        {
          penalty += 100000 * penalty_intensity * pow(fabs(p_height - old_height), 2);

        }
        if (p_width_l < 0)
        {
          penalty += penalty_lwidth * peaks.size() * 10000 * pow(fabs(p_width_l - old_width_l), 2);
        }
        else if (p_width_l < 1.5)
          penalty += 10000 * pow(fabs(p_width_l - old_width_l), 2);
        if (p_width_r < 0)
        {
          penalty += penalty_rwidth * peaks.size() * 10000 * pow(fabs(p_width_r - old_width_r), 2);
        }
        else if (p_width_r < 1.5)
          penalty += 10000 * pow(fabs(p_width_r - old_width_r), 2);
        if (fabs(old_position - p_position) > 0.1)
        {
          penalty += 10000 * penalty_pos * pow(fabs(old_position - p_position), 2);
        }

      }
      gsl_vector_set(f, f->size - 1, penalty);
      return GSL_SUCCESS;
    }

    /** Compute the Jacobian of the residual, where each row of the matrix corresponds to a
     *  point in the data.
     */
    Int jacobianDC(const gsl_vector * x, void * params, gsl_matrix * J)
    {
      // For the conventions on x and params c.f. the commentary in residual()
      //
      // The matrix J is supposed to contain the result when we return from this function.
      // Note: GSL expects the Jacobian as follows:
      //                    - each row corresponds to one data point
      //                    - each column corresponds to one parameter

      std::vector<DoubleReal> & positions = static_cast<OptimizePeakDeconvolution::Data *>(params)->positions;
      std::vector<PeakShape> & peaks = static_cast<OptimizePeakDeconvolution::Data *>(params)->peaks;
      OptimizationFunctions::PenaltyFactorsIntensity & penalties = static_cast<OptimizePeakDeconvolution::Data *>(params)->penalties;
      Int charge = static_cast<OptimizePeakDeconvolution::Data *>(params)->charge;

      DoubleReal leftwidth = gsl_vector_get(x, 0);
      DoubleReal rightwidth = gsl_vector_get(x, 1);

      gsl_matrix_set_zero(J);

      // iterate over all points of the signal
      for (Size current_point = 0; current_point < positions.size(); current_point++)
      {
        DoubleReal current_position    = positions[current_point];

        // iterate over all peaks
        for (Size current_peak = 0; current_peak < peaks.size(); current_peak++)
        {

          //Store the current parameters for this peak
          DoubleReal p_height        = gsl_vector_get(x, 2 + 2 * current_peak);
          DoubleReal p_position    = gsl_vector_get(x, 2 + 2 * current_peak + 1);
          DoubleReal p_width          = (current_position <= p_position) ? leftwidth : rightwidth;

          //is it a Lorentz or a Sech - Peak?
          if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
          {
            DoubleReal diff      = current_position - p_position;
            DoubleReal denom_inv = 1. / (1. + pow(p_width * diff, 2));

            DoubleReal ddl_left  = (current_position <= p_position)
                                   ? -2 * p_height * pow(diff, 2) * p_width * pow(denom_inv, 2) :
                                     0;

            DoubleReal ddl_right = (current_position  > p_position)
                                   ? -2 * p_height * pow(diff, 2) * p_width * pow(denom_inv, 2) :
                                     0;

            // left and right width are the same for all peaks,
            // the sums of the derivations over all peaks are stored in the first two columns
            gsl_matrix_set(J, current_point, 0, gsl_matrix_get(J, current_point, 0) + ddl_left);
            gsl_matrix_set(J, current_point, 1, gsl_matrix_get(J, current_point, 1) + ddl_right);

            DoubleReal ddx0    = 2 * p_height * pow(p_width, 2) * diff * pow(denom_inv, 2);

            // partial derivation with respect to intensity
            gsl_matrix_set(J, current_point, 2 + 2 * current_peak, denom_inv);

            // partial derivation with respect to the mz-position
            gsl_matrix_set(J, current_point, 2 + 2 * current_peak + 1, ddx0);
          }
          else                   // It's a Sech - Peak
          {
            DoubleReal diff      = current_position - p_position;
            DoubleReal denom_inv = 1. / cosh(p_width * diff);

            // The remaining computations are not stable if denom_inv == 0. In that case, we are far away from the peak
            // and can assume that all derivatives vanish
            DoubleReal sinh_term = (fabs(denom_inv) < 1e-6) ? 0.0 : sinh(p_width * diff);

            DoubleReal ddl_left  = (current_position <= p_position)
                                   ? -2 * p_height * sinh_term * diff * pow(denom_inv, 3) :
                                     0;
            DoubleReal ddl_right = (current_position  > p_position)
                                   ? -2 * p_height * sinh_term * diff * pow(denom_inv, 3) :
                                     0;

            gsl_matrix_set(J, current_point, 0, gsl_matrix_get(J, current_point, 0) + ddl_left);
            gsl_matrix_set(J, current_point, 1, gsl_matrix_get(J, current_point, 1) + ddl_right);

            DoubleReal ddx0      = 2 * p_height * p_width * sinh_term * pow(denom_inv, 3);

            gsl_matrix_set(J, current_point, 2 + 2 * current_peak, pow(denom_inv, 2));
            gsl_matrix_set(J, current_point, 2 + 2 * current_peak + 1, ddx0);
          }
        }
      }

      /** Now iterate over all peaks again to compute the
       *  penalties.
       */

      for (Size current_peak = 0; current_peak < peaks.size(); current_peak++)
      {

        DoubleReal penalty_p = 0;
        DoubleReal p_position = gsl_vector_get(x, 2 + 2 * current_peak + 1);
        if (current_peak < peaks.size() - 1)
        {

          DoubleReal next_p_position  = gsl_vector_get(x, 2 + 2 * current_peak + 3);
          // if distance between peaks does not match the peptide mass rule
          if (fabs(fabs(p_position - next_p_position) - 1.003 / charge) > 0.05)
          {
            // penalize it
            penalty_p += penalties.pos * 20000
                         * fabs(fabs(p_position - next_p_position) - 1.003 / charge);

          }
        }
        //  std::cout << "penalty_p "<<penalty_p<<std::endl;
        DoubleReal p_width_left = gsl_vector_get(x, 0);
        DoubleReal p_width_right = gsl_vector_get(x, 1);
        DoubleReal p_height   = gsl_vector_get(x, 2 + 2 * current_peak);

        DoubleReal old_position    = peaks[current_peak].mz_position;
        DoubleReal old_width_left  = peaks[current_peak].left_width;
        DoubleReal old_width_right = peaks[current_peak].right_width;
        DoubleReal old_height      = peaks[current_peak].height;

        DoubleReal penalty_h = 0., penalty_l = 0., penalty_r = 0.;
        if (p_height < 1)
        {
          penalty_h += 100000 * 2 * penalties.height * (fabs(p_height) - fabs(old_height));
        }

        if (p_width_left < 0)
        {
          penalty_l += peaks.size() * 2 * penalties.lWidth * 10000 * (fabs(p_width_left - old_width_left));
        }
        else if (p_width_left < 1.5)
          penalty_l += 2 * penalties.lWidth * 10000 * pow(fabs(p_width_left - old_width_left), 2);
        if (p_width_right < 0)
        {
          penalty_r += peaks.size() * 2 * penalties.rWidth * 10000 * (fabs(p_width_right - old_width_right));
        }
        else if (p_width_right < 1.5)
          penalty_r += 2 * penalties.rWidth * 10000 * pow(fabs(p_width_right - old_width_right), 2);
        if (fabs(old_position - p_position) > 0.1)
        {
          penalty_p += 10000 * penalties.pos * 2 * fabs(old_position - p_position);
        }

        gsl_matrix_set(J, positions.size(), 2 + 2 * current_peak, 100 * penalty_h);
        gsl_matrix_set(J, positions.size(), 0, 100 * penalty_l);
        gsl_matrix_set(J, positions.size(), 1, 100 * penalty_r);
        gsl_matrix_set(J, positions.size(), 2 + 2 * current_peak + 1, 100 * penalty_p);
      }

      return GSL_SUCCESS;
    }

    // Driver function for the evaluation of function and jacobian.
    Int evaluateDC(const gsl_vector * x, void * params, gsl_vector * f, gsl_matrix * J)
    {
      residualDC(x, params, f);
      jacobianDC(x, params, J);

      return GSL_SUCCESS;
    }

  } // namespace OptimizationFunctions

  OptimizePeakDeconvolution::OptimizePeakDeconvolution() :
    DefaultParamHandler("OptimizePeakDeconvolution"), charge_(1)
  {

    defaults_.setValue("max_iteration", 10, "maximal number of iterations for the fitting step");
    defaults_.setValue("eps_abs", 1e-04, "if the absolute error gets smaller than this value the fitting is stopped", ListUtils::create<String>("advanced"));
    defaults_.setValue("eps_rel", 1e-04, "if the relative error gets smaller than this value the fitting is stopped", ListUtils::create<String>("advanced"));

    defaults_.setValue("penalties:left_width", 0.0, "penalty term for the fitting of the left width:" \
                                                    "If the left width gets too broad or negative during the fitting it can be penalized.");
    defaults_.setValue("penalties:right_width", 0.0, "penalty term for the fitting of the right width:" \
                                                     "If the right width gets too broad or negative during the fitting it can be penalized.");
    defaults_.setValue("penalties:height", 0.0, "penalty term for the fitting of the intensity:" \
                                                "If it gets negative during the fitting it can be penalized.");
    defaults_.setValue("penalties:position", 0.0, "penalty term for the fitting of the peak position:" \
                                                  "If the position changes more than 0.5Da during the fitting it can be penalized as well as " \
                                                  "discrepancies of the peptide mass rule.");

    defaults_.setValue("fwhm_threshold", 1.0, "If a peaks is broader than fwhm_threshold, it is assumed that it contains another peaks and an additional peak is added.");

    defaultsToParam_();
  }

  void OptimizePeakDeconvolution::updateMembers_()
  {
    penalties_.rWidth = (float)param_.getValue("penalties:right_width");
    penalties_.lWidth = (float)param_.getValue("penalties:left_width");
    penalties_.height = (float)param_.getValue("penalties:height");
    penalties_.pos    = (float)param_.getValue("penalties:position");

  }

  bool OptimizePeakDeconvolution::optimize(std::vector<PeakShape> & peaks, Data & data)
  {

    if (peaks.empty())
      return true;

#ifdef DEBUG_DECONV
    std::cout << "peaksanzahl:" << peaks.size();
    std::cout << "\tpeaks[0].mz_position:" << peaks[0].mz_position << std::endl;

    for (Size j = 0; j < peaks.size(); ++j)
    {
      std::cout << "\tpeaks[j].mz_position:" << peaks[j].mz_position;
      std::cout << "\tpeaks[j].height:" << peaks[j].height << std::endl;
      std::cout << "\tpeaks[j].left_width:" << peaks[j].left_width;
      std::cout << "\tpeaks[j].right_width:" << peaks[j].right_width << std::endl << std::endl;
    }

    for (Size j = 0; j < data.positions.size(); ++j)
    {
      std::cout << "positions[" << j << "]=" << data.positions[j] << std::endl;
    }

#endif

    // the input peaks are stored in a temporary vector
    std::vector<PeakShape> temp_shapes = peaks;

    Size global_peak_number = 0;

    DoubleReal min(std::numeric_limits<double>::max());
    Int best_charge;
    Size num_peaks;
    Size best_num_peaks;
    gsl_vector * best_result = gsl_vector_alloc((Int)(2 + 2 * data.peaks.size()));

    // try three different charge states : charge-1, charge, charge +1
    // take the best solution
    Int l = (charge_ - 1 > 1) ? charge_ - 1 : charge_;
    Int start_l = l;
#ifdef DEBUG_DECONV
    std::cout << "charge " << l << " max_charge" << charge_ + 1
              << "\tpeaks.size() " << peaks.size() << std::endl;
#endif
    best_charge = l;
    best_num_peaks = peaks.size();
    num_peaks = peaks.size();
    for (; l < charge_ + 2; ++l)
    {

      num_peaks = getNumberOfPeaks_(l, temp_shapes, data);
#ifdef DEBUG_DECONV
      std::cout << "charge " << l << " #peaks " << num_peaks << "\tpeaks.size() "
                << data.peaks.size() << std::endl;
#endif
      gsl_vector * start_value;
      // the vector storing the start values for the parameters has to be filled
      // differently depending on the usage of the peptide mass rule
      start_value = gsl_vector_alloc(2 + 2 * data.peaks.size());
      for (Size i = 0; i < data.peaks.size(); i++)
      {
        gsl_vector_set(start_value, 2 + 2 * i, data.peaks[i].height);
        gsl_vector_set(start_value, 3 + 2 * i, data.peaks[i].mz_position);
      }

      // Initialize the parameters for the optimization

      // all peaks shall have the same width
      DoubleReal wl = data.peaks[0].left_width;
      DoubleReal wr = data.peaks[0].right_width;
      if (boost::math::isnan(wl))
      {
        for (Size i = 0; i < data.peaks.size(); ++i)
        {
          data.peaks[i].left_width = 1;
        }
        wl = 1.;
      }
      if (boost::math::isnan(wr))
      {
        for (Size i = 0; i < data.peaks.size(); ++i)
        {
          data.peaks[i].right_width = 1;
        }
        wr = 1.;
      }

      gsl_vector_set(start_value, 0, wl);
      gsl_vector_set(start_value, 1, wr);

      // The gsl algorithms require us to provide function pointers for the evaluation of
      // the target function.
      gsl_multifit_function_fdf fit_function;

      fit_function.f      = OptimizationFunctions::residualDC;
      fit_function.df     = OptimizationFunctions::jacobianDC;
      fit_function.fdf    = OptimizationFunctions::evaluateDC;

      fit_function.n      = std::max(data.positions.size() + 1,
                                     2 + 2 * data.peaks.size());

      fit_function.p    = 2 + 2 * data.peaks.size();
      data.penalties = penalties_;
// fit_function.params = &penalties_;
      data.charge = l;
      fit_function.params = &data;
#ifdef DEBUG_DECONV
      std::cout << "fit_function.p " << fit_function.p << "\t fit_function.n " << fit_function.n << std::endl;
      std::cout << "peaks.size() " << data.peaks.size() << std::endl;
#endif
      const gsl_multifit_fdfsolver_type * type = gsl_multifit_fdfsolver_lmsder;
      gsl_multifit_fdfsolver * fit;
      fit = gsl_multifit_fdfsolver_alloc(type,
                                         std::max(data.positions.size() + 1,
                                                  2 + 2 * data.peaks.size()),
                                         2 + 2 * data.peaks.size());

      gsl_multifit_fdfsolver_set(fit, &fit_function, start_value);

#ifdef DEBUG_DECONV
      // initial norm
      std::cout << "Before optimization: ||f|| = " << gsl_blas_dnrm2(fit->f) << std::endl;
#endif
      // Iteration
      Int iteration = 0;
      Int status;

      do
      {
        iteration++;
        status = gsl_multifit_fdfsolver_iterate(fit);

#ifdef DEBUG_DECONV
        std::cout << "Iteration " << iteration << "; Status " << gsl_strerror(status) << "; " << std::endl;
        std::cout << "||f|| = " << gsl_blas_dnrm2(fit->f) << std::endl;
        std::cout << "Number of parms: " << data.peaks.size() + 3 << std::endl;
        std::cout << "Delta: " << gsl_blas_dnrm2(fit->dx) << std::endl;
#endif
        if (boost::math::isnan(gsl_blas_dnrm2(fit->dx)))
        {
#ifdef DEBUG_DECONV
          std::cout << "norm is not a number" << std::endl;
#endif
          break;
        }
        status = gsl_multifit_test_delta(fit->dx, fit->x, (float)param_.getValue("eps_abs"),
                                         (float)param_.getValue("eps_rel"));

        if (status != GSL_CONTINUE)
        {
#ifdef DEBUG_DECONV
          std::cout << "gsl status != GSL_CONTINUE" << std::endl;
#endif
          break;
        }
        //      if(!checkFWHM_(peaks,fit) && failure <1)
//                      {
// #ifdef DEBUG_DECONV
//                              std::cout << "fwhm differ"<<std::endl;
// #endif
//                              return false;
//                      }
      }
      while (status == GSL_CONTINUE && iteration < (Int)param_.getValue("max_iteration"));

      DoubleReal chi = gsl_blas_dnrm2(fit->f);
#ifdef DEBUG_DECONV
      std::cout << "Finished! Charge " << l << "\tIterations: " << iteration << std::endl;
      std::cout << "Delta: " << gsl_blas_dnrm2(fit->dx) << std::endl;

      std::cout << "chisq/dof = " << pow(chi, 2.0) / (data.positions.size()
                                                      - (3 + data.peaks.size()));
      std::cout << "\nAfter optimization: ||f|| = " << gsl_blas_dnrm2(fit->f) << std::endl;
#endif
      if ((l == start_l) || (chi < min))
      {
        if (l != start_l)
          gsl_vector_free(best_result);
        best_result = gsl_vector_alloc(2 + 2 * data.peaks.size());
        gsl_vector_memcpy(best_result, fit->x);
        min = chi;
        best_charge = l;
        best_num_peaks = data.peaks.size();
      }
      iteration = 0;

      gsl_multifit_fdfsolver_free(fit);
      gsl_vector_free(start_value);

    }
    global_peak_number += best_num_peaks;
    // iterate over all peaks and store the optimized values in peaks
    if (best_num_peaks > 0)
    {
      peaks.resize(best_num_peaks);
      for (Size current_peak = 0; current_peak < best_num_peaks; current_peak++)
      {

        // Store the current parameters for this peak

        peaks[current_peak].left_width  = gsl_vector_get(best_result, 0);
        peaks[current_peak].right_width = gsl_vector_get(best_result, 1);

        peaks[current_peak].height      = gsl_vector_get(best_result, 2 + 2 * current_peak);
        peaks[current_peak].mz_position = gsl_vector_get(best_result, 2 + 2 * current_peak + 1);

        // compute the area
        // is it a Lorentz or a Sech - Peak?
        if (peaks[current_peak].type == PeakShape::LORENTZ_PEAK)
        {
          PeakShape p = peaks[current_peak];
          DoubleReal x_left_endpoint = p.mz_position + 1 / p.left_width * sqrt(p.height / 1 - 1);
          DoubleReal x_right_endpoint = p.mz_position + 1 / p.right_width * sqrt(p.height / 1 - 1);
#ifdef DEBUG_DECONV
          std::cout << "x_left_endpoint " << x_left_endpoint << " x_right_endpoint " << x_right_endpoint << std::endl;
          std::cout << "p.height" << p.height << std::endl;
#endif
          DoubleReal area_left = -p.height / p.left_width * atan(p.left_width * (x_left_endpoint - p.mz_position));
          DoubleReal area_right = -p.height / p.right_width * atan(p.right_width * (p.mz_position - x_right_endpoint));
          peaks[current_peak].area = area_left + area_right;

        }
        else                  //It's a Sech - Peak
        {
          PeakShape p = peaks[current_peak];
          DoubleReal x_left_endpoint = p.mz_position + 1 / p.left_width * boost::math::acosh(sqrt(p.height / 0.001));
          DoubleReal x_right_endpoint = p.mz_position + 1 / p.right_width * boost::math::acosh(sqrt(p.height / 0.001));
#ifdef DEBUG_DECONV
          std::cout << "x_left_endpoint " << x_left_endpoint << " x_right_endpoint " << x_right_endpoint << std::endl;
          std::cout << "p.height" << p.height << std::endl;
#endif
          DoubleReal area_left = -p.height / p.left_width * (sinh(p.left_width * (p.mz_position - x_left_endpoint))
                                                             / cosh(p.left_width * (p.mz_position - x_left_endpoint)));
          DoubleReal area_right = -p.height / p.right_width * (sinh(p.right_width * (p.mz_position - x_right_endpoint))
                                                               / cosh(p.right_width * (p.mz_position - x_right_endpoint)));
          peaks[current_peak].area = area_left + area_right;

        }

      }
    }
    charge_ = best_charge;
    gsl_vector_free(best_result);

    return true;
  }

  bool OptimizePeakDeconvolution::checkFWHM_(std::vector<PeakShape> & peaks, gsl_multifit_fdfsolver * & fit)
  {
    DoubleReal fwhm_threshold = (DoubleReal)param_.getValue("fwhm_threshold");

    PeakShape p;
    for (Size current_peak = 0; current_peak < peaks.size(); current_peak++)
    {
      p.left_width  = gsl_vector_get(fit->x, 0);
      p.right_width = gsl_vector_get(fit->x, 1);
      p.type        = peaks[current_peak].type;
#ifdef DEBUG_DECONV
      std::cout << "fwhm: " << p.getFWHM() << " > " << fwhm_threshold << " ?" << std::endl;
#endif
      if (p.getFWHM() > fwhm_threshold)
        return false;
    }

    return true;
  }

  Size OptimizePeakDeconvolution::getNumberOfPeaks_(Int charge, std::vector<PeakShape> & temp_shapes, Data & data)
  {
    DoubleReal dist = dist_ / charge;

    data.peaks.clear();

    Size shape = 0;
#ifdef DEBUG_DECONV
    std::cout << "temp_shapes[0].mz_position " << temp_shapes[0].mz_position
              << "\t dist " << dist << "\tp_index " << shape << std::endl;
#endif
    // while the peak's position is smaller than the last considered position
    // take the peak for optimization
    while ((temp_shapes[0].mz_position + shape * dist <
            data.positions[data.positions.size() - 1]) &&
           (shape < temp_shapes.size()))
    {
      data.peaks.push_back(temp_shapes[shape]);
#ifdef DEBUG_DECONV
      std::cout << "temp_shapes[0].mz_position + p_index*dist = " << temp_shapes[0].mz_position + shape * dist << std::endl;
#endif
      ++shape;
    }

    return shape;

  }

}


#include <boost/filesystem.hpp>
#include <fftw3.h>

#include "Simulation.h"

template <int dim>
Simulation<dim>::Simulation(
		json settings, double lc_thickness, unsigned int N_lc_steps,
		std::shared_ptr<CubicInterpolatedMapping<dim,3,double> > &n_field) :
	mat_properties(settings.at("Material properties")),
	N_lc_steps(N_lc_steps),
	lc_thickness(lc_thickness),
	n_field(n_field) {

	// We initialize the wavelength array
	auto &j = settings.at("Light source");
	if(j.find("Wavelengths") != j.end()) {
		wavelengths = j.at("Wavelengths").get<std::vector<double> >();
		N_wavelengths = int(wavelengths.size());
	}
	else {
		N_wavelengths = j.at("N wavelengths");
		double mean_lambda = j.at("Mean wavelength");
		double fwhm = j.at("Spectral FWHM");

		if(N_wavelengths==1)
			wavelengths.push_back(j.at("Mean wavelength"));
		else {
			for(int i=0; i<N_wavelengths; i++) {
				wavelengths.push_back(mean_lambda - 0.5*fwhm + fwhm*i/(N_wavelengths-1));
			}
		}
	}

	// We initialize the ODE for the isotropic layers
	lower_iso_thickness = std::accumulate(
		mat_properties.e_lower_layers.begin(),
		mat_properties.e_lower_layers.end(), 0.);
	upper_iso_thickness = std::accumulate(
		mat_properties.e_upper_layers.begin(),
		mat_properties.e_upper_layers.end(), 0.);

	lower_iso_odes.push_back(std::make_shared<IsotropicODEFunction<dim> >(
		std::pow(mat_properties.n_out, 2.),
		std::make_shared<SlabDomain<dim> >(
			BasisVector<dim>(dim-1),
			-lower_iso_thickness-lc_thickness*1.5,
			-lower_iso_thickness-lc_thickness/2.)));

	double z_interface = -lower_iso_thickness-lc_thickness/2.;
	for(int i=0; i<mat_properties.n_lower_layers.size(); i++) {
		lower_iso_odes.push_back(std::make_shared<IsotropicODEFunction<dim> >(
			std::pow(mat_properties.n_lower_layers[i],2.),
			std::make_shared<SlabDomain<dim> >(
				BasisVector<dim>(dim-1), z_interface,
				z_interface + mat_properties.e_lower_layers[i])));
		z_interface += mat_properties.e_lower_layers[i];
	}

	z_interface = lc_thickness/2;
	for(int i=0; i<mat_properties.e_upper_layers.size(); i++) {
		upper_iso_odes.push_back(std::make_shared<IsotropicODEFunction<dim> >(
			std::pow(mat_properties.n_upper_layers[i],2.),
			std::make_shared<SlabDomain<dim> >(
				BasisVector<dim>(dim-1), z_interface,
				z_interface + mat_properties.e_upper_layers[i])));
		z_interface += mat_properties.e_upper_layers[i];
	}
	upper_iso_odes.push_back(std::make_shared<IsotropicODEFunction<dim> >(
		std::pow(mat_properties.n_out,2.),
		std::make_shared<SlabDomain<dim> >(
			BasisVector<dim>(dim-1), z_interface,
			z_interface+lc_thickness)));

	// We initalize the other parameters
	N_refinement_cycles =
		settings.at("InverseScreenMap parameters").at("N refinement cycles");
	hc_parameters = settings.at("InverseScreenMap parameters");

	viz_settings = settings.at("Visualisation");
	numerical_aperture = viz_settings.at("Screen output").at("Numerical aperture");

	bulk_basename = viz_settings.at("Bulk output").at("Base name");
	screen_basename = viz_settings.at("Screen output").at("Base name");
	basedir = viz_settings.at("Results folder name");

	bulk_ray_output = viz_settings.at("Bulk output").at("Export ray data");
	bulk_fields_output = viz_settings.at("Bulk output").at("Export reconstructed fields");
	screen_ray_output = viz_settings.at("Screen output").at("Export ray data");
	screen_fields_output = viz_settings.at("Screen output").at("Export reconstructed fields");

	auto widths = parse_Vector<dim-1,double>(viz_settings, "Target output widths");
	auto dims = parse_Vector<dim-1,unsigned long>(viz_settings, "Target N pixels per dim");
	auto origin = -widths/2;

	Vector<dim-1,unsigned long> coarse_dims;
	for(int i=0; i<dim-1; i++) {
		coarse_dims(i) =
			std::floor(1+(dims(i)-1)/std::pow(2,N_refinement_cycles));
		dims(i) = 1+(coarse_dims(i)-1)*std::pow(2,N_refinement_cycles);
	}
	coarse_horiz_mesh = std::make_shared<CartesianMesh<dim-1> >(
		origin, widths, coarse_dims, false);
	full_horiz_mesh = std::make_shared<CartesianMesh<dim-1> >(
		origin, widths, dims, false);

	switch(dim) {
		case 2:
			typical_length = widths(0);
			break;
		case 3:
			typical_length = std::max(widths(0),widths(1));
			break;
	}

	double bulk_z_length = lc_thickness;
	if(settings.at("Geometry").at("Sample type")=="Droplet")
		bulk_z_length -= 2*settings.at("Geometry").at("Droplet sample parameters").at(
			"Distance from upper sample plate").get<double>();
	z_step = bulk_z_length / (N_lc_steps-1);

	double n_above_lc =
		mat_properties.n_upper_layers.size()>0 ?
		mat_properties.n_upper_layers[0] : mat_properties.n_out;
	z_foc_1 =
		bulk_z_length*(1-n_above_lc/mat_properties.n_lc)/2. +
		(lc_thickness-bulk_z_length)*(1-n_above_lc/mat_properties.n_host)/2.;
	z_foc_2 =
		bulk_z_length*(1-mat_properties.n_out/mat_properties.n_lc)/2. +
		(lc_thickness-bulk_z_length)*(1-mat_properties.n_out/mat_properties.n_host)/2.;
	for(int i=0; i<mat_properties.e_upper_layers.size(); i++)
		z_foc_2 +=
			mat_properties.e_upper_layers[i] *
			(1-mat_properties.n_out/mat_properties.n_upper_layers[i]);

	switch(dim) {
		case 2:
			N_horiz_pixels = dims(0);
			if(bulk_ray_output) {
				bulk_extra_data = std::make_shared<RayData>(
					1, dims(0), N_lc_steps,
					0., widths(0)/(dims(0)-1), z_step);
				bulk_ordi_data = std::make_shared<RayData>(
					1, dims(0), N_lc_steps,
					0., widths(0)/(dims(0)-1), z_step);
			}
			if(bulk_fields_output)
				bulk_fields_data = std::make_shared<FieldsData>(
					1, dims(0), N_lc_steps,
					0., widths(0)/(dims(0)-1), z_step,
					wavelengths);
			if(screen_ray_output) {
				screen_extra_data = std::make_shared<RayData>(
					1, dims(0), 1,
					0., widths(0)/(dims(0)-1), 0.);
				screen_ordi_data = std::make_shared<RayData>(
					1, dims(0), 1,
					0., widths(0)/(dims(0)-1), 0.);
			}
			if(screen_fields_output)
				screen_fields_data = std::make_shared<FieldsData>(
					1, dims(0), 1,
					0., widths(0)/(dims(0)-1), 0.,
					wavelengths);
			break;

		case 3:
			N_horiz_pixels = dims(0)*dims(1);
			if(bulk_ray_output) {
				bulk_extra_data = std::make_shared<RayData>(
					dims(0), dims(1), N_lc_steps,
					widths(0)/(dims(0)-1), widths(1)/(dims(1)-1), z_step);
				bulk_ordi_data = std::make_shared<RayData>(
					dims(0), dims(1), N_lc_steps,
					widths(0)/(dims(0)-1), widths(1)/(dims(1)-1), z_step);
			}
			if(bulk_fields_output)
				bulk_fields_data = std::make_shared<FieldsData>(
					dims(0), dims(1), N_lc_steps,
					widths(0)/(dims(0)-1), widths(1)/(dims(1)-1), z_step,
					wavelengths);
			if(screen_ray_output) {
				screen_extra_data = std::make_shared<RayData>(
					dims(0), dims(1), 1,
					widths(0)/(dims(0)-1), widths(1)/(dims(1)-1), 0.);
				screen_ordi_data = std::make_shared<RayData>(
					dims(0), dims(1), 1,
					widths(0)/(dims(0)-1), widths(1)/(dims(1)-1), 0.);
			}
			if(screen_fields_output)
				screen_fields_data = std::make_shared<FieldsData>(
					dims(0), dims(1), 1,
					widths(0)/(dims(0)-1), widths(1)/(dims(1)-1), 0.,
					wavelengths);
			break;
	}
}

template <int dim>
void Simulation<dim>::apply_fourier_iso_layer_filter() {

	int dims[3]; 		screen_fields_data->vti_data->GetDimensions(dims);
	double spacings[3];	screen_fields_data->vti_data->GetSpacing(spacings);

	fftw_complex *in = static_cast<fftw_complex*>(
		fftw_malloc(sizeof(fftw_complex) * dims[0]*dims[1]));
	fftw_complex *out = static_cast<fftw_complex*>(
		fftw_malloc(sizeof(fftw_complex) * dims[0]*dims[1]));
	fftw_plan forward_plan = (dim==3) ?
		fftw_plan_dft_2d(dims[1], dims[0], in, out, FFTW_FORWARD, FFTW_ESTIMATE) :
		fftw_plan_dft_1d(dims[1], in, out, FFTW_FORWARD, FFTW_ESTIMATE);
	fftw_plan backward_plan = (dim==3) ?
		fftw_plan_dft_2d(dims[1], dims[0], in, out, FFTW_BACKWARD, FFTW_ESTIMATE) :
		fftw_plan_dft_1d(dims[1], in, out, FFTW_BACKWARD, FFTW_ESTIMATE);

	#pragma omp parallel for
	for(int wave_idx=0; wave_idx<N_wavelengths; wave_idx++) {
		// First we assemble the filter
		auto iso_filter =
			std::make_shared<std::vector<std::complex<double> > >(dims[0]*dims[1]);
		double k0 = 2*PI/wavelengths[wave_idx];

		// transfer matrix in column-major order
		std::vector<std::complex<double> > tmat(4), prev_tmat(4);
		std::complex<double> t1, t2;

		for(int iy=0; iy<dims[1]; iy++) {
			for(int ix=0; ix<dims[0]; ix++) {
				double kx = (ix<double(dims[0]/2.)) ?
					2*PI*ix/(spacings[0]*(dims[0]-1)) :
					-2*PI*(dims[0]-ix)/(spacings[0]*(dims[0]-1));
				if(dim==2)
					kx = 0;
				double ky = (iy<double(dims[1]/2.)) ?
					2*PI*iy/(spacings[1]*(dims[1]-1)) :
					-2*PI*(dims[1]-iy)/(spacings[1]*(dims[1]-1));
				double k = std::sqrt(kx*kx+ky*ky);

				if(k<k0*numerical_aperture) {
					tmat[0] = 1;
					tmat[1] = 0;
					tmat[2] = 0;
					tmat[3] = 1;

					prev_tmat = tmat;

					double kN = std::sqrt(k0*k0-k*k);

					for(int p=0; p<mat_properties.e_upper_layers.size(); p++) {
						double np = mat_properties.n_upper_layers[p];
						double ep = mat_properties.e_upper_layers[p];
						double kp = std::sqrt(std::pow(k0*np,2.)-k*k);
						double next_kp = (p+1<mat_properties.e_upper_layers.size()) ?
							std::sqrt(std::pow(k0*mat_properties.n_upper_layers[p+1],2.)-k*k) :
							std::sqrt(std::pow(k0*mat_properties.n_out,2.)-k*k);

						t1 = 0.5*(1+kp/next_kp)*std::exp(std::complex<double>(0,(kp-kN)*ep));
						t2 = 0.5*(1-kp/next_kp)*std::exp(std::complex<double>(0,(kp-kN)*ep));

						tmat[0] = t1*prev_tmat[0] + std::conj(t2)*prev_tmat[1];
						tmat[1] = t2*prev_tmat[0] + std::conj(t1)*prev_tmat[1];
						tmat[2] = t1*prev_tmat[2] + std::conj(t2)*prev_tmat[3];
						tmat[3] = t2*prev_tmat[2] + std::conj(t1)*prev_tmat[3];

						prev_tmat = tmat;
					}
					iso_filter->at(ix+dims[0]*iy) =
						(tmat[0]-tmat[1]*tmat[2]/tmat[3]) *
						std::exp(std::complex<double>(0,kN*z_foc_2));
				}
				else
					iso_filter->at(ix+dims[0]*iy) = 0;
			}
		}

		// FFTW arrays for this thread
		fftw_complex *field = static_cast<fftw_complex*>(
			fftw_malloc(sizeof(fftw_complex) * dims[0]*dims[1]));
		fftw_complex *fft_field = static_cast<fftw_complex*>(
			fftw_malloc(sizeof(fftw_complex) * dims[0]*dims[1]));

		for(int pol_idx=0; pol_idx<2; pol_idx++) {
			for(int comp=0; comp<2; comp++) {
				// First, we fill the fftw input array with our data
				for(int i=0; i<dims[0]*dims[1]; i++) {
					field[i][0] =
						screen_fields_data->E_real[pol_idx][wave_idx]->GetComponent(i,comp);
					field[i][1] =
						screen_fields_data->E_imag[pol_idx][wave_idx]->GetComponent(i,comp);
				}

				// We switch to fourrier space and apply the iso filter
				fftw_execute_dft(forward_plan, field, fft_field);
				for(int i=0; i<dims[0]*dims[1]; i++) {
					auto val = std::complex<double>(fft_field[i][0], fft_field[i][1]);
					fft_field[i][0] = std::real(val * iso_filter->at(i));
					fft_field[i][1] = std::imag(val * iso_filter->at(i));
				}

				// We come back to real space and save the transformed
				// data
				fftw_execute_dft(backward_plan, fft_field, field);

				for(int i=0; i<dims[0]*dims[1]; i++) {
					screen_fields_data->E_real[pol_idx][wave_idx]->SetComponent(
						i, comp, field[i][0]/(dims[0]*dims[1]));
					screen_fields_data->E_imag[pol_idx][wave_idx]->SetComponent(
						i, comp, field[i][1]/(dims[0]*dims[1]));
				}
			}
		}

		// We free the FFTW pointers
		fftw_free(field);
		fftw_free(fft_field);
	}

	// We free the FFTW pointers
	fftw_destroy_plan(forward_plan);
	fftw_destroy_plan(backward_plan);
	fftw_free(in);
	fftw_free(out);
}

template class Simulation<2>;
template class Simulation<3>;


/**
BSD 3-Clause License

This file is part of the code accompanying the paper
From Planes to Corners: Multi-Purpose Primitive Detection in Unorganized 3D Point Clouds
by C. Sommer, Y. Sun, L. Guibas, D. Cremers and T. Birdal,
accepted for Publication in IEEE Robotics and Automation Letters (RA-L) 2020.

Copyright (c) 2019, Christiane Sommer.
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

// includes
#include <iostream>
#include <fstream>
#include <string>
#include "definitions.h"
// libraries
#include <Eigen/Dense>
#include <opencv2/core/core.hpp>
#include <CLI/CLI.hpp>
// classes
#include "Timer.h"
#include "Plane.h"
#include "graph/PlaneGraph.h"
#include "graph/ParallelPlaneGraph.h"
#include "PPF/PairDetector.h"
// function includes
#include "io/load_ply_cloud.h"
#include "visualize/pcshow.h"

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <experimental/filesystem>

namespace fs = std::experimental::filesystem;

/*
 * main
 */
int main(int argc, char** argv) {

  int nr_volume=0;
  int nr_pointclouds=0;

  std::string string_pointcloud;
  std::ifstream MyReadFile2("../data/main_programs/config_files/input_file_plane_dataset.txt");
  std::getline (MyReadFile2, string_pointcloud);

  std::ofstream myfile("/home/alex-pop/Desktop/Doctorat/Side_projects/Volume_box_batch_processing/orthogonal-planes/data/output_volumes.txt");

  myfile<<"Hello"<<'\n';

	//fs::path path("/home/alex-pop/Desktop/Doctorat/Side_projects/Volume_box_batch_processing/orthogonal-planes/data/Pointcloud_recordings");
  fs::path path(string_pointcloud);
	for (auto& p : fs::directory_iterator(path))
	{

	std::stringstream ss;

    ss << p ;

   std::string str = ss.str();

   str.erase(std::remove(str.begin(), str.end(), '"'), str.end());

   std::cout<<str<<'\n';

   pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);

    if (pcl::io::loadPCDFile<pcl::PointNormal> (str, *cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
    return (-1);
  }
  std::cout << "Loaded "
            << cloud->width * cloud->height
            << " data points from "<<str<<".pcd with the following fields: "
            << std::endl;

  std::vector<Eigen::Vector3f> points;
  std::vector<Eigen::Vector3f> normals;

  nr_pointclouds++;

  for (int i = 0; i < cloud->size(); i++)
    {

        Eigen::Vector3f position_aux= Eigen::Vector3f( (float) cloud->points[i].x,(float) cloud->points[i].y,(float) cloud->points[i].z);
        points.push_back(position_aux);

        Eigen::Vector3f normal_aux= Eigen::Vector3f( (float) cloud->points[i].normal_x,(float) cloud->points[i].normal_y,(float) cloud->points[i].normal_z);
        normals.push_back(normal_aux);
    }

     std::cout << points.size() << " points loaded." << std::endl;
     std::cout << normals.size() << " normals loaded." << std::endl;
    std::cout<<"\n";

    Timer T;

    // parse setttings from command line
    std::string ply_file;
    int min_votes = 5;
    double d_min = .1, d_max = 1.;
    int sampling = 10;

    std::ifstream MyReadFile("../data/main_programs/config_files/input_file_plane.txt");

    std::ifstream MyReadFile2("../data/main_programs/config_files/input_file_plane_dataset.txt");

    std::string min_votes_text;
    std::string d_min_text;
    std::string d_max_text;
    std::string sampling_text;
    std::string threshold_text;

    std::getline (MyReadFile, min_votes_text);
    std::getline (MyReadFile, d_min_text);
    std::getline (MyReadFile, d_max_text);
    std::getline (MyReadFile, sampling_text);
    std::getline (MyReadFile, threshold_text);

    min_votes=std::stoi(min_votes_text);
    std::cout<<"min_votes="<<min_votes<< std::endl;
    d_min=std::stod(d_min_text);
     std::cout<<"d_min="<<d_min<< std::endl;
     d_max=std::stod(d_max_text);
     std::cout<<"d_max="<<d_max<< std::endl;
     sampling=std::stoi(sampling_text);
     std::cout<<"sampling_text="<<sampling<< std::endl;
     double threshold=std::stod(threshold_text);
     std::cout<<"threshold lines="<<threshold<< std::endl;

     for(int nr_exec=0;nr_exec<3;nr_exec++)
     {

    // pairing
    ppf::PairDetector pairDet(d_max, d_min, min_votes);
    PlaneGraph planeMap(pairDet.para_thresh(), pairDet.distance_bin());
    T.tic();
    pairDet.detect_ortho_pairs(points, normals, planeMap);
    T.toc("time pairing PPF ");
    //planeMap.print_info();

    std::cout << std::endl << "Thresholds:\tAngle:\t" << pairDet.para_thresh() << "\tDistance:\t" << pairDet.distance_bin() << std::endl << std::endl;

    // clustering & filtering
    T.tic();
    planeMap.cluster_graph_vertices();
    T.toc("clustering planes");
    //planeMap.print_info();
    //planeMap.print_parameters();
    //planeMap.print_edges();

    // find triangles
    std::vector<Graph<Plane>::Triangle> triangles;
    T.tic();
    planeMap.find_triangles(triangles);
    T.toc("Finding triangles");
    //planeMap.print_triangles(triangles);

    // reduce graph to ParallelPlaneGraph
    T.tic();
    ParallelPlaneGraph redMap = planeMap.reduce_graph();
    T.toc("graph reduction");
    //redMap.print_info();
    //redMap.print_edges();
    T.tic();
    redMap.triangle_reduce();
    T.toc("triangle reduction");
    //redMap.print_info();
    //redMap.print_parameters();
    //redMap.print_edges();

    T.tic();
    redMap.filter_outliers(points, normals, pairDet.distance_bin(), .5*sampling);
    T.toc("outlier filtering");
    //redMap.print_info();
    //redMap.print_parameters();
    //redMap.print_edges();

    // CERES problem setup and solving
    double lambda = 0.01 * points.size();
    T.tic();
    redMap.refine_coarse_fine(points, normals, lambda, pairDet.distance_bin(), sampling);
    T.toc("Multi-plane parameter optimization");
    //redMap.print_info();
    //redMap.print_parameters();
    //redMap.print_edges();

    T.tic();
    redMap.filter_outliers(points, normals, pairDet.distance_bin(), .5*sampling);
    T.toc("outlier filtering");
    //redMap.print_info();
    //redMap.print_parameters();
    //redMap.print_edges();

    // From here on visualization / debugging

//    // get points located on planes and not-planes
    PlaneGraph finalMap = PlaneGraph::from_ppg(redMap, 2*pairDet.para_thresh()*pairDet.para_thresh()-1, pairDet.distance_bin()); // double angle
    std::vector<Eigen::Matrix<float, 6, 1>> labeled;
    T.tic();
    finalMap.go_thr_pcl(points, normals, labeled);
    T.toc("labeling planes");

    // visualize
   // pcshow(labeled, "Result");

   std::vector<Vec6> lines;
   // line extraction and visualization
   T.tic();
   lines = finalMap.extract_lines(points, normals);
   T.toc("get lines");
   //std::cout << lines.size() << " lines found." << std::endl;

   std::vector<Vec6> new_lines;

   std::vector<Vec6> final_lines;
   std::vector<double> distances;

   std::vector<double> Volume_edge;

   std::vector<int> positions_wrong;
   std::vector<double> Volumes;

    std::cout<<'\n';

  if (lines.size()>=3){
    for(int i = 0; i != lines.size()-2; i++) {
    //std::cout<< lines[i][0]<<" "<<lines[i][1]<<" "<<lines[i][2]<<" "<<lines[i][3]<<" "<<lines[i][4]<<" "<<lines[i][5]<<" "<<'\n';
      for(int j = i+1; j != lines.size()-1; j++) {
          for(int k = j+1; k != lines.size(); k++) {

                bool ok_ij=1;
                bool ok_jk=1;
                bool ok_ki=1;

                double average_0,average_1,average_2;

                if( (lines[i][0]==lines[j][0]) && (lines[i][1]==lines[j][1]) && (lines[i][2]==lines[j][2])) {ok_ij=0;}
                if( (lines[j][0]==lines[k][0]) && (lines[j][1]==lines[k][1]) && (lines[j][2]==lines[k][2])) {ok_jk=0;}
                if( (lines[k][0]==lines[i][0]) && (lines[k][1]==lines[i][1]) && (lines[k][2]==lines[i][2])) {ok_ki=0;}

                if (ok_ij && ok_jk && ok_ki){

              average_0=(lines[i][0]+lines[j][0]+lines[k][0]) / 3;
              average_1=(lines[i][1]+lines[j][1]+lines[k][1]) /3 ;
              average_2=(lines[i][2]+lines[j][2]+lines[k][2]) /3;

              double dist_i_average= sqrt (  (lines[i][0]-average_0)*(lines[i][0]-average_0)  +  (lines[i][1]-average_1)*(lines[i][1]-average_1) +(lines[i][2]-average_2)*(lines[i][2]-average_2)   );
              double dist_j_average= sqrt (  (lines[j][0]-average_0)*(lines[j][0]-average_0)  +  (lines[j][1]-average_1)*(lines[j][1]-average_1) +(lines[j][2]-average_2)*(lines[j][2]-average_2)   );
              double dist_k_average= sqrt (  (lines[k][0]-average_0)*(lines[k][0]-average_0)  +  (lines[k][1]-average_1)*(lines[k][1]-average_1) +(lines[k][2]-average_2)*(lines[k][2]-average_2)   );

              if(  (dist_i_average<threshold) && (dist_j_average<threshold) &&(dist_k_average<threshold) )
              {
                  /*
                  std::cout<< lines[i][0]<<" "<<lines[i][1]<<" "<<lines[i][2]<<" "<<'\n';
                  std::cout<< lines[j][0]<<" "<<lines[j][1]<<" "<<lines[j][2]<<" "<<'\n';
                  std::cout<< lines[k][0]<<" "<<lines[k][1]<<" "<<lines[k][2]<<" "<<'\n';
                  */

                  distances.push_back(dist_i_average);
                  distances.push_back(dist_j_average);
                  distances.push_back(dist_k_average);

                  /*
                  std::cout<<"Average:"<<average_0<<" "<<average_1<<" "<<average_2<<"\n";
                  std::cout<<"\n";
                  */
                  /*
                std::cout<< lines[i][0]<<" "<<lines[i][1]<<" "<<lines[i][2]<<" "<<lines[i][3]<<" "<<lines[i][4]<<" "<<lines[i][5]<<" "<<'\n';
                std::cout<< lines[j][0]<<" "<<lines[j][1]<<" "<<lines[j][2]<<" "<<lines[j][3]<<" "<<lines[j][4]<<" "<<lines[j][5]<<" "<<'\n';
                std::cout<< lines[k][0]<<" "<<lines[k][1]<<" "<<lines[k][2]<<" "<<lines[k][3]<<" "<<lines[k][4]<<" "<<lines[k][5]<<" "<<'\n';
                */
                double latura_i = sqrt (  (lines[i][0]-lines[i][3])*(lines[i][0]-lines[i][3])  +  (lines[i][1]-lines[i][4])*(lines[i][1]-lines[i][4]) +(lines[i][2]-lines[i][5])*(lines[i][2]-lines[i][5])   );
                double latura_j = sqrt (  (lines[j][0]-lines[j][3])*(lines[j][0]-lines[j][3])  +  (lines[j][1]-lines[j][4])*(lines[j][1]-lines[j][4]) +(lines[j][2]-lines[j][5])*(lines[j][2]-lines[j][5])   );
                double latura_k = sqrt (  (lines[k][0]-lines[k][3])*(lines[k][0]-lines[k][3])  +  (lines[k][1]-lines[k][4])*(lines[k][1]-lines[k][4]) +(lines[k][2]-lines[k][5])*(lines[k][2]-lines[k][5])   );

                double Volum = latura_i*latura_j*latura_k;
                 /*
                std::cout<<"Latura 1: "<<latura_1<<'\n';
                std::cout<<"Latura 2: "<<latura_3<<'\n';
                std::cout<<"Latura 3: "<<latura_3<<'\n';
                */

                //std::cout<<"Volum="<<Volum<<'\n';

                new_lines.push_back(lines[i]);
                new_lines.push_back(lines[j]);
                new_lines.push_back(lines[k]);

                Volume_edge.push_back(latura_i);
                Volume_edge.push_back(latura_j);
                Volume_edge.push_back(latura_k);

                Volumes.push_back(Volum);
                Volumes.push_back(Volum);
                Volumes.push_back(Volum);
              }

              }
            }
          }

       }

   if (new_lines.size()>1){
    for(int i = 0; i != new_lines.size(); i++) {
    //std::cout<< new_lines[i][0]<<" "<<new_lines[i][1]<<" "<<new_lines[i][2]<<" "<<new_lines[i][3]<<" "<<new_lines[i][4]<<" "<<new_lines[i][5]<<" "<<'\n';
    }

    for(int i = 0; i != new_lines.size()-1; i++) {

      for(int j = i+1; j != new_lines.size(); j++) {
          if( ( (new_lines[i][0]==new_lines[j][0])  && (new_lines[i][1]==new_lines[j][1]) && (new_lines[i][2]==new_lines[j][2]) && (new_lines[i][3]==new_lines[j][3]) && (new_lines[i][4]==new_lines[j][4]) && (new_lines[i][5]==new_lines[j][5]) ) ||
              ( (new_lines[i][0]==new_lines[j][3])  && (new_lines[i][1]==new_lines[j][4])&&  (new_lines[i][2]==new_lines[j][5]) && (new_lines[i][3]==new_lines[j][0]) && (new_lines[i][4]==new_lines[j][1]) && (new_lines[i][5]==new_lines[j][2]) ) )
               {
            //std::cout<<"Dublura gasita la"<<i<<" "<<j<<'\n';

            double total_distance_i;
            double total_distance_j;

            int offset_i= (i % 3);
            int offset_j= (j % 3);

            total_distance_i=distances[i - offset_i]+distances[i+1 - offset_i]+distances[i+2 - offset_i];
            total_distance_j=distances[j - offset_j]+distances[j+1 - offset_j]+distances[j+2 - offset_j];

            //std::cout<<"Distance i: "<<total_distance_i<<"\n";
           // std::cout<<"Distance j: "<<total_distance_j<<"\n";

            if(total_distance_i>total_distance_j){
              positions_wrong.push_back(i - offset_i);
            }
            else
            {
               positions_wrong.push_back(j - offset_j);
            }

          }

      }
    }
   }

  /*
  std::cout<<"Positions wrong:"<<'\n';

   for(int i = 0; i != positions_wrong.size(); i++) {
    std::cout<<positions_wrong[i]<<'\n';
    }

    */
    if (new_lines.size()>=3){
      nr_volume++;

       //myfile<<str<<" "<<'\n';

       //myfile<<nr_exec<<" iteration"<<'\n';

    for(int i = 0; i < new_lines.size(); i=i+3) {
      bool ok_position=1;
      for(int j = 0; j != positions_wrong.size(); j++) {
        if(i==positions_wrong[j]){
          ok_position=0;
        }
      }
      if(ok_position){
        final_lines.push_back(new_lines[i]);
        final_lines.push_back(new_lines[i+1]);
        final_lines.push_back(new_lines[i+2]);
        std::cout<<"Volum "<<(i/3)<<": "<<Volumes[i]<<" cm^3"<<'\n';

        myfile<<Volumes[i]<<" "<<nr_pointclouds<<'\n';

        /*
        std::cout<< "("<<new_lines[i][0]<<" , "<<new_lines[i][1]<<" , "<<new_lines[i][2]<<" , "<<new_lines[i][3]<<" , "<<new_lines[i][4]<<" , "<<new_lines[i][5]<<")"<<" =>distance1= "<<Volume_edge[i] <<'\n';
        std::cout<< "("<<new_lines[i+1][0]<<" , "<<new_lines[i+1][1]<<" , "<<new_lines[i+1][2]<<" , "<<new_lines[i+1][3]<<" , "<<new_lines[i+1][4]<<" , "<<new_lines[i+1][5]<<")"<<" =>distance2= "<<Volume_edge[i+1] <<'\n';
        std::cout<< "("<<new_lines[i+2][0]<<" , "<<new_lines[i+2][1]<<" , "<<new_lines[i+2][2]<<" , "<<new_lines[i+2][3]<<" , "<<new_lines[i+2][4]<<" , "<<new_lines[i+2][5]<<")"<<" =>distance3= "<<Volume_edge[i+2] <<'\n';
        */

        std::cout<< "distance1= "<<Volume_edge[i] <<" cm"<<'\n';
        std::cout<< "distance2= "<<Volume_edge[i+1]<<" cm" <<'\n';
        std::cout<< "distance3= "<<Volume_edge[i+2]<<" cm" <<'\n';

        std::cout<<'\n';

      }
    }
   }

   //pcshow_lines(points, lines, "Orth Plane Intersections");

   //pcshow_lines(points, new_lines, "Volume Computation Edges");

   //pcshow_lines(points, final_lines, "Volume Final Edges");

  }
     }
  }
   std::cout<<nr_pointclouds<<" pointclouds found"<<'\n';
    std::cout<<nr_volume<<" pointclouds with volumes"<<'\n';

}


// Copyright (C) 2011-2012 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "piecewise_polynomial_discontinuous_scalar_space.hpp"

#include "space_helper.hpp"

#include "../assembly/discrete_sparse_boundary_operator.hpp"
#include "../common/acc.hpp"
#include "../common/boost_make_shared_fwd.hpp"
#include "../common/bounding_box_helpers.hpp"
#include "../fiber/explicit_instantiation.hpp"
#include "../grid/entity.hpp"
#include "../grid/entity_iterator.hpp"
#include "../grid/geometry.hpp"
#include "../grid/grid.hpp"
#include "../grid/grid_view.hpp"
#include "../grid/grid_segment.hpp"
#include "../grid/mapper.hpp"
#include "../grid/vtk_writer.hpp"

#include <stdexcept>
#include <iostream>

#include <boost/array.hpp>

namespace Bempp {

template <typename BasisFunctionType>
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    PiecewisePolynomialDiscontinuousScalarSpace(
        const shared_ptr<const Grid> &grid, int polynomialOrder)
    : ScalarSpace<BasisFunctionType>(grid), m_polynomialOrder(polynomialOrder),
      m_flatLocalDofCount(0) {
  initialize(GridSegment::wholeGrid(*grid));
}

template <typename BasisFunctionType>
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    PiecewisePolynomialDiscontinuousScalarSpace(
        const shared_ptr<const Grid> &grid, int polynomialOrder,
        const GridSegment &segment, int dofMode)
    : ScalarSpace<BasisFunctionType>(grid), m_polynomialOrder(polynomialOrder),
      m_flatLocalDofCount(0) {
  initialize(segment, dofMode);
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::initialize(
    const GridSegment &segment, int dofMode) {
  const int gridDim = this->grid()->dim();
  if (gridDim != 2)
    throw std::invalid_argument("PiecewisePolynomialDiscontinuousScalarSpace::"
                                "initialize(): "
                                "only 2-dimensional grids are supported");
  if (!(dofMode & (REFERENCE_POINT_ON_SEGMENT | ELEMENT_ON_SEGMENT)))
    throw std::invalid_argument("PiecewisePolynomialDiscontinuousScalarSpace::"
                                "initialize(): invalid dofMode");
  m_view = this->grid()->leafView();
  if (m_polynomialOrder == 0)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 0>());
  else if (m_polynomialOrder == 1)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 1>());
  else if (m_polynomialOrder == 2)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 2>());
  else if (m_polynomialOrder == 3)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 3>());
  else if (m_polynomialOrder == 4)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 4>());
  else if (m_polynomialOrder == 5)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 5>());
  else if (m_polynomialOrder == 6)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 6>());
  else if (m_polynomialOrder == 7)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 7>());
  else if (m_polynomialOrder == 8)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 8>());
  else if (m_polynomialOrder == 9)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 9>());
  else if (m_polynomialOrder == 10)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 10>());
  else
    throw std::invalid_argument(
        "PiecewisePolynomialDiscontinuousScalarSpace::"
        "PiecewisePolynomialDiscontinuousScalarSpace(): "
        "polynomialOrder must be >= 0 and <= 10");
  assignDofsImpl(segment, dofMode);
}

template <typename BasisFunctionType>
PiecewisePolynomialDiscontinuousScalarSpace<
    BasisFunctionType>::~PiecewisePolynomialDiscontinuousScalarSpace() {}

template <typename BasisFunctionType>
int PiecewisePolynomialDiscontinuousScalarSpace<
    BasisFunctionType>::domainDimension() const {
  return this->grid()->dim();
}

template <typename BasisFunctionType>
int PiecewisePolynomialDiscontinuousScalarSpace<
    BasisFunctionType>::codomainDimension() const {
  return 1;
}

template <typename BasisFunctionType>
const Fiber::Shapeset<BasisFunctionType> &
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::shapeset(
    const Entity<0> &element) const {
  if (elementVariant(element) == 3)
    return *m_triangleShapeset;
  throw std::logic_error(
      "PiecewisePolynomialDiscontinuousScalarSpace::shapeset(): "
      "invalid element variant, this shouldn't happen!");
}

template <typename BasisFunctionType>
bool PiecewisePolynomialDiscontinuousScalarSpace<
    BasisFunctionType>::spaceIsCompatible(const Space<BasisFunctionType> &other)
    const {

  typedef PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>
  thisSpaceType;

  if (other.grid().get() != this->grid().get())
    return false;

  if (other.spaceIdentifier() == this->spaceIdentifier()) {
    // Try to typecast the other space down.
    const thisSpaceType &temp = dynamic_cast<const thisSpaceType &>(other);
    if (this->m_polynomialOrder == temp.m_polynomialOrder)
      return true;
    else
      return false;
  } else
    return false;
}

template <typename BasisFunctionType>
ElementVariant
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::elementVariant(
    const Entity<0> &element) const {
  GeometryType type = element.type();
  if (type.isLine())
    return 2;
  else if (type.isTriangle())
    return 3;
  else if (type.isQuadrilateral())
    return 4;
  else
    throw std::runtime_error("PiecewisePolynomialDiscontinuousScalarSpace::"
                             "elementVariant(): invalid geometry type, "
                             "this shouldn't happen!");
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<
    BasisFunctionType>::setElementVariant(const Entity<0> &element,
                                          ElementVariant variant) {
  if (variant != elementVariant(element))
    // for this space, the element variants are unmodifiable,
    throw std::runtime_error("PiecewisePolynomialDiscontinuousScalarSpace::"
                             "setElementVariant(): invalid variant");
}

template <typename BasisFunctionType>
shared_ptr<const Space<BasisFunctionType>>
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    discontinuousSpace(const shared_ptr<const Space<BasisFunctionType>> &self)
    const {
  if (self.get() != this)
    throw std::invalid_argument(
        "PiecewisePolynomialDiscontinuousScalarSpace::discontinuousSpace(): "
        "argument should be a shared pointer to *this");
  return self;
}

template <typename BasisFunctionType>
bool PiecewisePolynomialDiscontinuousScalarSpace<
    BasisFunctionType>::isDiscontinuous() const {
  return true;
}

template <typename BasisFunctionType>
void
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::assignDofsImpl(
    const GridSegment &segment, int dofMode) {
  const int gridDim = this->domainDimension();
  if (gridDim != 2)
    throw std::runtime_error("PiecewisePolynomialDiscontinuousScalarSpace::"
                             "assignDofsImpl(): only 2-dimensional grids "
                             "are supported at present");
  const int vertexCodim = 2, edgeCodim = 1, elementCodim = 0;
  const Mapper &elementMapper = m_view->elementMapper();
  const IndexSet &indexSet = m_view->indexSet();

  int elementCount = m_view->entityCount(0);

  const int localDofCountPerTriangle =
      (m_polynomialOrder + 1) * (m_polynomialOrder + 2) / 2;
  const int localDofCountPerQuad =
      (m_polynomialOrder + 1) * (m_polynomialOrder + 1);

  BoundingBox<CoordinateType> model;
  model.lbound.x = std::numeric_limits<CoordinateType>::max();
  model.lbound.y = std::numeric_limits<CoordinateType>::max();
  model.lbound.z = std::numeric_limits<CoordinateType>::max();
  model.ubound.x = -std::numeric_limits<CoordinateType>::max();
  model.ubound.y = -std::numeric_limits<CoordinateType>::max();
  model.ubound.z = -std::numeric_limits<CoordinateType>::max();
  m_globalDofBoundingBoxes.reserve(localDofCountPerQuad * elementCount);

  // (Re)initialise DOF maps
  m_local2globalDofs.clear();
  m_local2globalDofs.resize(elementCount);
  m_global2localDofs.clear();
  // estimated number of global DOFs
  m_global2localDofs.reserve(localDofCountPerQuad * elementCount);

  // Fill in global<->local dof maps
  std::unique_ptr<EntityIterator<0>> it = m_view->entityIterator<0>();
  arma::Mat<CoordinateType> vertices;
  arma::Col<CoordinateType> dofPosition;
  GlobalDofIndex globalDofCount = 0;
  while (!it->finished()) {
    const Entity<0> &element = it->entity();
    EntityIndex elementIndex = elementMapper.entityIndex(element);
    bool elementContained = !(dofMode & ELEMENT_ON_SEGMENT) ||
                            segment.contains(elementCodim, elementIndex);
    typedef arma::Col<CoordinateType> Col;
    const Geometry &geo = element.geometry();
    geo.getCorners(vertices);
    int vertexCount = vertices.n_cols;
    int localDofCount =
        vertexCount == 3 ? localDofCountPerTriangle : localDofCountPerQuad;

    // List of global DOF indices corresponding to the local DOFs of the
    // current element
    std::vector<GlobalDofIndex> &globalDofs = m_local2globalDofs[elementIndex];
    globalDofs.resize(localDofCount);
    // GlobalDofIndex gdofStart = globalDofCount;
    // for (int i = 0; i < localDofCount; ++i) {
    //     globalDofs.push_back(globalDofCount);
    //     std::vector<LocalDof> localDofs(1, LocalDof(elementIndex, i));
    //     m_global2localDofs.push_back(localDofs);
    //     ++globalDofCount;
    // }
    // GlobalDofIndex gdofEnd = globalDofCount;

    // Bounding boxes
    BoundingBox<CoordinateType> bbox = model;
    extendBoundingBox(bbox, vertices);
    // m_globalDofBoundingBoxes.insert(m_globalDofBoundingBoxes.end(),
    //                                 localDofCount, bbox);
    if (vertexCount == 3) {
      int subEntityIndex;
      int ldof;

      if (m_polynomialOrder == 0) {
        ldof = 0;
        if (segment.contains(0, elementIndex)) {
          acc(globalDofs, ldof) = globalDofCount;
          std::vector<LocalDof> localDofs(1, LocalDof(elementIndex, ldof));
          m_global2localDofs.push_back(localDofs);
          m_globalDofBoundingBoxes.push_back(bbox);
          setBoundingBoxReference<CoordinateType>(
              acc(m_globalDofBoundingBoxes, globalDofCount),
              (vertices.col(0) + vertices(1) + vertices(2)) / 3);
          ++globalDofCount;
        } else
          acc(globalDofs, ldof) = -1;
      } else {
        // vertex dofs
        ldof = 0;
        subEntityIndex = indexSet.subEntityIndex(element, 0, vertexCodim);
        if (elementContained &&
            (!(dofMode & REFERENCE_POINT_ON_SEGMENT) ||
             segment.contains(vertexCodim, subEntityIndex))) {
          acc(globalDofs, ldof) = globalDofCount;
          std::vector<LocalDof> localDofs(1, LocalDof(elementIndex, ldof));
          m_global2localDofs.push_back(localDofs);
          m_globalDofBoundingBoxes.push_back(bbox);
          setBoundingBoxReference<CoordinateType>(
              acc(m_globalDofBoundingBoxes, globalDofCount), vertices.col(0));
          ++globalDofCount;
        } else
          acc(globalDofs, ldof) = -1;

        ldof = m_polynomialOrder;
        subEntityIndex = indexSet.subEntityIndex(element, 1, vertexCodim);
        if (elementContained &&
            (!(dofMode & REFERENCE_POINT_ON_SEGMENT) ||
             segment.contains(vertexCodim, subEntityIndex))) {
          acc(globalDofs, ldof) = globalDofCount;
          std::vector<LocalDof> localDofs(1, LocalDof(elementIndex, ldof));
          m_global2localDofs.push_back(localDofs);
          m_globalDofBoundingBoxes.push_back(bbox);
          setBoundingBoxReference<CoordinateType>(
              acc(m_globalDofBoundingBoxes, globalDofCount), vertices.col(1));
          ++globalDofCount;
        } else
          acc(globalDofs, ldof) = -1;

        ldof = localDofCount - 1;
        subEntityIndex = indexSet.subEntityIndex(element, 2, vertexCodim);
        if (elementContained &&
            (!(dofMode & REFERENCE_POINT_ON_SEGMENT) ||
             segment.contains(vertexCodim, subEntityIndex))) {
          acc(globalDofs, ldof) = globalDofCount;
          std::vector<LocalDof> localDofs(1, LocalDof(elementIndex, ldof));
          m_global2localDofs.push_back(localDofs);
          m_globalDofBoundingBoxes.push_back(bbox);
          setBoundingBoxReference<CoordinateType>(
              acc(m_globalDofBoundingBoxes, globalDofCount), vertices.col(2));
          ++globalDofCount;
        } else
          acc(globalDofs, ldof) = -1;

        // edge dofs
        if (m_polynomialOrder >= 2) {

          subEntityIndex = indexSet.subEntityIndex(element, 0, edgeCodim);
          if (elementContained &&
              (!(dofMode & REFERENCE_POINT_ON_SEGMENT) ||
               segment.contains(edgeCodim, subEntityIndex))) {
            dofPosition = 0.5 * (vertices.col(0) + vertices.col(1));
            for (int ldof = 1; ldof < m_polynomialOrder; ++ldof) {
              acc(globalDofs, ldof) = globalDofCount;
              std::vector<LocalDof> localDofs(1, LocalDof(elementIndex, ldof));
              m_global2localDofs.push_back(localDofs);
              m_globalDofBoundingBoxes.push_back(bbox);
              setBoundingBoxReference<CoordinateType>(
                  acc(m_globalDofBoundingBoxes, globalDofCount), dofPosition);
              ++globalDofCount;
            }
          } else
            for (int ldof = 1; ldof < m_polynomialOrder; ++ldof) {
              acc(globalDofs, ldof) = -1;
            }

          subEntityIndex = indexSet.subEntityIndex(element, 1, edgeCodim);
          if (elementContained &&
              (!(dofMode & REFERENCE_POINT_ON_SEGMENT) ||
               segment.contains(edgeCodim, subEntityIndex))) {
            dofPosition = 0.5 * (vertices.col(0) + vertices.col(2));
            for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy) {
              int ldof =
                  ldofy * (m_polynomialOrder + 1) - ldofy * (ldofy - 1) / 2;
              acc(globalDofs, ldof) = globalDofCount;
              std::vector<LocalDof> localDofs(1, LocalDof(elementIndex, ldof));
              m_global2localDofs.push_back(localDofs);
              m_globalDofBoundingBoxes.push_back(bbox);
              setBoundingBoxReference<CoordinateType>(
                  acc(m_globalDofBoundingBoxes, globalDofCount), dofPosition);
              ++globalDofCount;
            }
          } else
            for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy) {
              int ldof =
                  ldofy * (m_polynomialOrder + 1) - ldofy * (ldofy - 1) / 2;
              acc(globalDofs, ldof) = -1;
            }

          subEntityIndex = indexSet.subEntityIndex(element, 2, edgeCodim);
          if (elementContained &&
              (!(dofMode & REFERENCE_POINT_ON_SEGMENT) ||
               segment.contains(edgeCodim, subEntityIndex))) {
            dofPosition = 0.5 * (vertices.col(1) + vertices.col(2));
            for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy) {
              int ldof = ldofy * (m_polynomialOrder + 1) -
                         ldofy * (ldofy - 1) / 2 + (m_polynomialOrder - ldofy);
              acc(globalDofs, ldof) = globalDofCount;
              std::vector<LocalDof> localDofs(1, LocalDof(elementIndex, ldof));
              m_global2localDofs.push_back(localDofs);
              m_globalDofBoundingBoxes.push_back(bbox);
              setBoundingBoxReference<CoordinateType>(
                  acc(m_globalDofBoundingBoxes, globalDofCount), dofPosition);
              ++globalDofCount;
            }
          } else
            for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy) {
              int ldof = ldofy * (m_polynomialOrder + 1) -
                         ldofy * (ldofy - 1) / 2 + (m_polynomialOrder - ldofy);
              acc(globalDofs, ldof) = -1;
            }

          // dofPosition = 0.5 * (vertices.col(0) + vertices.col(1));
          // for (int ldof = 1; ldof < m_polynomialOrder; ++ldof){
          //     setBoundingBoxReference<CoordinateType>(
          //         acc(m_globalDofBoundingBoxes, gdofStart + ldof),
          // dofPosition);
          // }
          // dofPosition = 0.5 * (vertices.col(0) + vertices.col(2));
          // for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy) {
          //     int ldof = ldofy * (m_polynomialOrder + 1) -
          //         ldofy * (ldofy - 1) / 2;
          //     setBoundingBoxReference<CoordinateType>(
          //         acc(m_globalDofBoundingBoxes, gdofStart + ldof),
          // dofPosition);
          // }
          // dofPosition = 0.5 * (vertices.col(1) + vertices.col(2));
          // for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy) {
          //     int ldof = ldofy * (m_polynomialOrder + 1) -
          //         ldofy * (ldofy - 1) / 2 + (m_polynomialOrder - ldofy);
          //     setBoundingBoxReference<CoordinateType>(
          //         acc(m_globalDofBoundingBoxes, gdofStart + ldof),
          // dofPosition);
          // }
        }
        // bubble dofs
        if (m_polynomialOrder >= 3) {
          if (segment.contains(elementCodim, elementIndex)) {
            dofPosition =
                (vertices.col(0) + vertices.col(1) + vertices.col(2)) / 3.;
            for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy)
              for (int ldofx = 1; ldofx + ldofy < m_polynomialOrder; ++ldofx) {
                int ldof = ldofy * (m_polynomialOrder + 1) -
                           ldofy * (ldofy - 1) / 2 + ldofx;
                acc(globalDofs, ldof) = globalDofCount;
                std::vector<LocalDof> localDofs(1,
                                                LocalDof(elementIndex, ldof));
                m_global2localDofs.push_back(localDofs);
                m_globalDofBoundingBoxes.push_back(bbox);
                setBoundingBoxReference<CoordinateType>(
                    acc(m_globalDofBoundingBoxes, globalDofCount), dofPosition);
                ++globalDofCount;
              }
          } else
            for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy)
              for (int ldofx = 1; ldofx + ldofy < m_polynomialOrder; ++ldofx) {
                int ldof = ldofy * (m_polynomialOrder + 1) -
                           ldofy * (ldofy - 1) / 2 + ldofx;
                acc(globalDofs, ldof) = -1;
              }

          // dofPosition = (vertices.col(0) + vertices.col(1) +
          //                vertices.col(2)) / 3.;
          // for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy)
          //     for (int ldofx = 1; ldofx + ldofy < m_polynomialOrder; ++ldofx)
          // {
          //         int ldof = ldofy * (m_polynomialOrder + 1) -
          //             ldofy * (ldofy - 1) / 2 + ldofx;
          //         setBoundingBoxReference<CoordinateType>(
          //             acc(m_globalDofBoundingBoxes, gdofStart + ldof),
          //             dofPosition);
          //     }
        }
      }
    }

    it->next();
  }

  // Initialize the container mapping the flat local dof indices to
  // local dof indices
  SpaceHelper<BasisFunctionType>::initializeLocal2FlatLocalDofMap(
      m_flatLocalDofCount, m_local2globalDofs, m_flatLocal2localDofs);

#ifndef NDEBUG
  for (size_t i = 0; i < m_globalDofBoundingBoxes.size(); ++i) {
    const BoundingBox<CoordinateType> &bbox = acc(m_globalDofBoundingBoxes, i);

    assert(bbox.reference.x >= bbox.lbound.x);
    assert(bbox.reference.y >= bbox.lbound.y);
    assert(bbox.reference.z >= bbox.lbound.z);
    assert(bbox.reference.x <= bbox.ubound.x);
    assert(bbox.reference.y <= bbox.ubound.y);
    assert(bbox.reference.z <= bbox.ubound.z);
  }
#endif // NDEBUG
}

template <typename BasisFunctionType>
size_t
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::globalDofCount()
    const {
  return m_global2localDofs.size();
}

template <typename BasisFunctionType>
size_t PiecewisePolynomialDiscontinuousScalarSpace<
    BasisFunctionType>::flatLocalDofCount() const {
  return m_flatLocalDofCount;
}

template <typename BasisFunctionType>
void
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::getGlobalDofs(
    const Entity<0> &element, std::vector<GlobalDofIndex> &dofs) const {
  const Mapper &mapper = m_view->elementMapper();
  EntityIndex index = mapper.entityIndex(element);
  dofs = m_local2globalDofs[index];
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    global2localDofs(const std::vector<GlobalDofIndex> &globalDofs,
                     std::vector<std::vector<LocalDof>> &localDofs) const {
  localDofs.resize(globalDofs.size());
  for (size_t i = 0; i < globalDofs.size(); ++i)
    localDofs[i] = m_global2localDofs[globalDofs[i]];
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    flatLocal2localDofs(const std::vector<FlatLocalDofIndex> &flatLocalDofs,
                        std::vector<LocalDof> &localDofs) const {
  localDofs.resize(flatLocalDofs.size());
  for (size_t i = 0; i < flatLocalDofs.size(); ++i)
    localDofs[i] = m_flatLocal2localDofs[flatLocalDofs[i]];
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    getGlobalDofPositions(std::vector<Point3D<CoordinateType>> &positions)
    const {
  positions.resize(m_globalDofBoundingBoxes.size());
  for (size_t i = 0; i < m_globalDofBoundingBoxes.size(); ++i)
    acc(positions, i) = acc(m_globalDofBoundingBoxes, i).reference;
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    getFlatLocalDofPositions(std::vector<Point3D<CoordinateType>> &positions)
    const {
  getGlobalDofPositions(positions);
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    getGlobalDofBoundingBoxes(std::vector<BoundingBox<CoordinateType>> &bboxes)
    const {
  bboxes = m_globalDofBoundingBoxes;
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    getFlatLocalDofBoundingBoxes(
        std::vector<BoundingBox<CoordinateType>> &bboxes) const {
  getGlobalDofBoundingBoxes(bboxes);
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    getGlobalDofNormals(std::vector<Point3D<CoordinateType>> &normals) const {
  SpaceHelper<BasisFunctionType>::getGlobalDofNormals_defaultImplementation(
      *m_view, m_global2localDofs, normals);
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    getFlatLocalDofNormals(std::vector<Point3D<CoordinateType>> &normals)
    const {
  getGlobalDofNormals(normals);
}

template <typename BasisFunctionType>
void
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::dumpClusterIds(
    const char *fileName,
    const std::vector<unsigned int> &clusterIdsOfDofs) const {
  dumpClusterIdsEx(fileName, clusterIdsOfDofs, GLOBAL_DOFS);
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<
    BasisFunctionType>::dumpClusterIdsEx(const char *fileName,
                                         const std::vector<unsigned int> &
                                             clusterIdsOfDofs,
                                         DofType dofType) const {
  throw std::runtime_error("PiecewisePolynomialDiscontinuousScalarSpace::"
                           "dumpClusterIdsEx(): not implemented yet");
}

FIBER_INSTANTIATE_CLASS_TEMPLATED_ON_BASIS(
    PiecewisePolynomialDiscontinuousScalarSpace);

} // namespace Bempp


#ifndef STAN_MATH_TORSTEN_MIXODE1CPTMODEL_RK45_HPP
#define STAN_MATH_TORSTEN_MIXODE1CPTMODEL_RK45_HPP

#include <Eigen/Dense>
#include <stan/math/torsten/PKModel/PKModel.hpp>
#include <stan/math/torsten/PKModel/functors/mix1_functor.hpp>
#include <stan/math/torsten/PKModel/Pred/Pred1_mix1.hpp>
#include <stan/math/torsten/PKModel/Pred/PredSS_mix1.hpp>
// #include <stan/math/torsten/PKModel/Pred/PredSS_err.hpp>
#include <boost/math/tools/promotion.hpp>
#include <vector>

namespace torsten {

/**
 * Compute the predicted amounts in each compartment at each event
 * of an ODEs model. The model contains a base 1 Compartment PK
 * component which gets solved analytically, while the other ODEs
 * are solved numerically using stan::math::integrate_ode_rk45. This
 * amounts to using the mixed solver method.
 *
 * <b>Warning:</b> This prototype does not handle steady state events.
 *
 * @tparam T0 type of scalar for time of events.
 * @tparam T1 type of scalar for amount at each event.
 * @tparam T2 type of scalar for rate at each event.
 * @tparam T3 type of scalar for inter-dose inteveral at each event.
 * @tparam T4 type of scalars for the model parameters.
 * @tparam T5 type of scalars for the bio-variability parameters.
 * @tparam T6 type of scalars for the model tlag parameters.
 * @tparam F type of ODE system function.
 * @param[in] f functor for base ordinary differential equation
 *            which gets solved numerically.
 * @param[in] nOde number of ODEs we solve numerically.
 * @param[in] time times of events
 * @param[in] amt amount at each event
 * @param[in] rate rate at each event
 * @param[in] ii inter-dose interval at each event
 * @param[in] evid event identity:
 *                    (0) observation
 *                    (1) dosing
 *                    (2) other
 *                    (3) reset
 *                    (4) reset AND dosing
 * @param[in] cmt compartment number at each event
 * @param[in] addl additional dosing at each event
 * @param[in] ss steady state approximation at each event (0: no, 1: yes)
 * @param[in] theta vector of ODE parameters
 * @param[in] biovar bio-availability in each compartment
 * @param[in] tlag lag time in each compartment
 * @param[in] rel_tol relative tolerance for the Boost ode solver
 * @param[in] abs_tol absolute tolerance for the Boost ode solver
 * @param[in] max_num_steps maximal number of steps to take within
 *            the Boost ode solver
 * @return a matrix with predicted amount in each compartment
 *         at each event.
 *
 * FIX ME: msg should be passed on to functor (allows use of
 * print statement inside ODE system).
 */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
mixOde1CptModel_rk45(const F& f,
                     const int nOde,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<std::vector<T4> >& theta,
                     const std::vector<std::vector<T5> >& biovar,
                     const std::vector<std::vector<T6> >& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  using std::vector;
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using boost::math::tools::promote_args;

  int nPK = 2;

  // check arguments
  static const char* function("mixOde1CptModel_rk45");
  torsten::pmetricsCheck(time, amt, rate, ii, evid, cmt, addl, ss,
                theta, biovar, tlag, function);

  // Construct dummy array of matrix for last argument of pred
  Matrix<T4, Dynamic, Dynamic> dummy_system;
  vector<Matrix<T4, Dynamic, Dynamic> > dummy_systems(1, dummy_system);

  typedef mix1_functor<F> F0;

  return Pred(time, amt, rate, ii, evid, cmt, addl, ss,
              theta, biovar, tlag, nPK + nOde, dummy_systems,
              Pred1_mix1<F0>(F0(f), rel_tol, abs_tol, max_num_steps, msgs,
                             "rk45"),
              PredSS_mix1<F0>(F0(f), rel_tol, abs_tol, max_num_steps, msgs,
                              "rk45", nOde));
}

/**
 * Overload function to allow user to pass an std::vector for
 * theta.
 */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
  typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
mixOde1CptModel_rk45(const F& f,
                     const int nOde,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<T4>& theta,
                     const std::vector<std::vector<T5> >& biovar,
                     const std::vector<std::vector<T6> >& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T4> > vec_theta(1, theta);

  return mixOde1CptModel_rk45(f, nOde,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              vec_theta, biovar, tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
 * Overload function to allow user to pass an std::vector for
 * theta and biovar.
 */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
mixOde1CptModel_rk45(const F& f,
                     const int nOde,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<T4>& theta,
                     const std::vector<T5>& biovar,
                     const std::vector<std::vector<T6> >& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T4> > vec_theta(1, theta);
  std::vector<std::vector<T5> > vec_biovar(1, biovar);

  return mixOde1CptModel_rk45(f, nOde,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              vec_theta, vec_biovar, tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
 * Overload function to allow user to pass an std::vector for
 * theta, biovar, and tlag.
 */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
mixOde1CptModel_rk45(const F& f,
                     const int nOde,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<T4>& theta,
                     const std::vector<T5>& biovar,
                     const std::vector<T6>& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T4> > vec_theta(1, theta);
  std::vector<std::vector<T5> > vec_biovar(1, biovar);
  std::vector<std::vector<T6> > vec_tlag(1, tlag);

  return mixOde1CptModel_rk45(f, nOde,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              vec_theta, vec_biovar, vec_tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
 * Overload function to allow user to pass an std::vector for
 * theta and tlag.
 */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
mixOde1CptModel_rk45(const F& f,
                     const int nOde,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<T4>& theta,
                     const std::vector<std::vector<T5> >& biovar,
                     const std::vector<T6>& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T4> > vec_theta(1, theta);
  std::vector<std::vector<T6> > vec_tlag(1, tlag);

  return mixOde1CptModel_rk45(f, nOde,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              vec_theta, biovar, vec_tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
 * Overload function to allow user to pass an std::vector for
 * biovar.
 */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
mixOde1CptModel_rk45(const F& f,
                     const int nOde,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<std::vector<T4> >& theta,
                     const std::vector<T5>& biovar,
                     const std::vector<std::vector<T6> >& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T5> > vec_biovar(1, biovar);

  return mixOde1CptModel_rk45(f, nOde,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              theta, vec_biovar, tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
 * Overload function to allow user to pass an std::vector for
 * biovar and tlag.
 */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
mixOde1CptModel_rk45(const F& f,
                     const int nOde,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<std::vector<T4> >& theta,
                     const std::vector<T5>& biovar,
                     const std::vector<T6>& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T5> > vec_biovar(1, biovar);
  std::vector<std::vector<T6> > vec_tlag(1, tlag);

  return mixOde1CptModel_rk45(f, nOde,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              theta, vec_biovar, vec_tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
 * Overload function to allow user to pass an std::vector for
 * tlag.
 */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
mixOde1CptModel_rk45(const F& f,
                     const int nOde,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<std::vector<T4> >& theta,
                     const std::vector<std::vector<T5> >& biovar,
                     const std::vector<T6>& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T6> > vec_tlag(1, tlag);

  return mixOde1CptModel_rk45(f, nOde,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              theta, biovar, vec_tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

}
#endif