#ifndef STAN_MATH_TORSTEN_SOLVE_ODE_HPP
#define STAN_MATH_TORSTEN_SOLVE_ODE_HPP

#include <Eigen/Dense>
#include <stan/math/torsten/to_array_2d.hpp>
#include <stan/math/torsten/ev_manager.hpp>
#include <stan/math/torsten/pmx_population_check.hpp>
#include <stan/math/torsten/ev_solver.hpp>
#include <stan/math/torsten/pmx_ode_model.hpp>
#include <stan/math/torsten/pmx_check.hpp>
#include <vector>

namespace torsten {
/**
 * simpily pmx function declare
 *
 */
#define TORSTEN_PMX_FUNC_EVENTS_ARGS 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

  template<typename integrator_type>
  struct PMXSolveODE {

    /// default tolerances & max steps for
    /// differential eq(DE) and algebra solver(AS)
    static constexpr double RTOL_DE = 1.e-6;
    static constexpr double ATOL_DE = 1.e-6;
    static constexpr int MAXSTEP_DE = 1e6;
    static constexpr double RTOL_AS = 1.e-6;
    static constexpr double ATOL_AS = 1.e-6;
    static constexpr int MAXSTEP_AS = 1e2;

    /**
     * Computes the predicted amounts in each compartment at each event
     * for a general compartment model, defined by a system of ordinary
     * differential equations.
     *
     * <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 that defines
     *            compartment model.
     * @param[in] nCmt number of compartments in model
     * @param[in] pMatrix parameters at each event
     * @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] 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
     * @param[in] as_rel_tol relative tolerance for the algebra solver
     * @param[in] as_abs_tol absolute tolerance for the algebra solver
     * @param[in] as_max_num_steps maximal number of steps to take within
     *            the algebra solver
     * @return a matrix with predicted amount in each compartment
     *         at each event.
     *
     * FIX ME: currently have a dummy msgs argument. Makes it easier
     * to expose to stan grammar files, because I can follow more closely
     * what was done for the ODE integrator. Not ideal.
     */
    template <typename T0, typename T1, typename T2, typename T3, typename T4,
              typename T5, typename T6, typename F>
    static stan::matrix_return_t<T0, T1, T2, T3, T4, T5, T6>
    solve(const F& f,
          const int nCmt,
          TORSTEN_PMX_FUNC_EVENTS_ARGS,
          const std::vector<std::vector<T4> >& pMatrix,
          const std::vector<std::vector<T5> >& biovar,
          const std::vector<std::vector<T6> >& tlag,
          double rel_tol,
          double abs_tol,
          long int max_num_steps,
          double as_rel_tol,
          double as_abs_tol,
          long int as_max_num_steps,
          std::ostream* msgs) {
      using std::vector;
      using Eigen::Dynamic;
      using Eigen::Matrix;

      // check arguments
      static const char* function("PMX SOLVE ODE");
      torsten::pmx_check(time, amt, rate, ii, evid, cmt, addl, ss, pMatrix, function);
      torsten::pmx_check(time.size(), nCmt, biovar, tlag);

      using ER = NONMENEventsRecord<T0, T1, T2, T3>;
      using EM = EventsManager<ER, NonEventParameters<T0, T4, std::vector, std::tuple<T5, T6> >>;
      const ER events_rec(nCmt, time, amt, rate, ii, evid, cmt, addl, ss);

      Matrix<typename EM::T_scalar, -1, -1> pred(EM::nCmt(events_rec), events_rec.num_event_times());

      using model_type = torsten::PKODEModel<typename EM::T_par, F>;

      integrator_type integrator(rel_tol, abs_tol, max_num_steps, as_rel_tol, as_abs_tol, as_max_num_steps, msgs);
      EventSolver<model_type, EM> pr;

      pr.pred(0, events_rec, pred, integrator, pMatrix, biovar, tlag, nCmt, f);
      return pred;
    }

    /*
     * Overload with default ODE & algebra solver controls
     */
    template <typename T0, typename T1, typename T2, typename T3, typename T4,
              typename T5, typename T6, typename F>
    static stan::matrix_return_t<T0, T1, T2, T3, T4, T5, T6>
    solve(const F& f,
               const int nCmt,
               TORSTEN_PMX_FUNC_EVENTS_ARGS,
               const std::vector<std::vector<T4> >& pMatrix,
               const std::vector<std::vector<T5> >& biovar,
               const std::vector<std::vector<T6> >& tlag,
               std::ostream* msgs) {
      return solve(f, nCmt,
                   time, amt, rate, ii, evid, cmt, addl, ss,
                   pMatrix, biovar, tlag,
                   RTOL_DE, ATOL_DE, MAXSTEP_DE,
                   RTOL_AS, ATOL_AS, MAXSTEP_AS,
                   msgs);
    }

    /*
     * Overload with default algebra solver controls
     */
    template <typename T0, typename T1, typename T2, typename T3, typename T4,
              typename T5, typename T6, typename F>
    static stan::matrix_return_t<T0, T1, T2, T3, T4, T5, T6>
    solve(const F& f,
               const int nCmt,
               TORSTEN_PMX_FUNC_EVENTS_ARGS,
               const std::vector<std::vector<T4> >& pMatrix,
               const std::vector<std::vector<T5> >& biovar,
               const std::vector<std::vector<T6> >& tlag,
               double rel_tol,
               double abs_tol,
               long int max_num_steps,
               std::ostream* msgs) {
      return solve(f, nCmt,
                   time, amt, rate, ii, evid, cmt, addl, ss,
                   pMatrix, biovar, tlag,
                   rel_tol, abs_tol, max_num_steps,
                   RTOL_AS, ATOL_AS, MAXSTEP_AS,
                   msgs);
    }

    /**
     * Overload function to allow user to pass an std::vector for
     * pMatrix/bioavailability/tlag
     */
    template <typename T0, typename T1, typename T2, typename T3,
              typename T_par, typename T_biovar, typename T_tlag,
              typename F,
              typename... Ts,
              typename = require_any_not_std_vector_t<T_par, T_biovar, T_tlag>,
              typename = require_all_not_std_vector_t<Ts...> >
    static stan::matrix_return_t<T0, T1, T2, T3, T_par, T_biovar, T_tlag>
    solve(const F& f,
          const int nCmt,
          TORSTEN_PMX_FUNC_EVENTS_ARGS,
          const std::vector<T_par>& pMatrix,
          const std::vector<T_biovar>& biovar,
          const std::vector<T_tlag>& tlag,
          Ts... solver_ctrl) {
      return solve(f, nCmt,
                   time, amt, rate, ii, evid, cmt, addl, ss,
                   torsten::to_array_2d(pMatrix),
                   torsten::to_array_2d(biovar),
                   torsten::to_array_2d(tlag),
                   solver_ctrl...);
    }

    /**
     * Overload: omitting lag time, with full tolerance spec
     *
     */
    template <typename T0, typename T1, typename T2, typename T3, typename T4,
              typename T5, typename F>
    static stan::matrix_return_t<T0, T1, T2, T3, T4, T5>
    solve(const F& f,
               const int nCmt,
               TORSTEN_PMX_FUNC_EVENTS_ARGS,
               const std::vector<std::vector<T4> >& pMatrix,
               const std::vector<std::vector<T5> >& biovar,
               double rel_tol,
               double abs_tol,
               long int max_num_steps,
               double as_rel_tol,
               double as_abs_tol,
               long int as_max_num_steps,
               std::ostream* msgs) {
      static const char* function("solve");
      torsten::pmx_check(time, amt, rate, ii, evid, cmt, addl, ss, pMatrix, function);
      torsten::pmx_check(time.size(), nCmt, biovar);

      using ER = NONMENEventsRecord<T0, T1, T2, T3>;
      using EM = EventsManager<ER, NonEventParameters<T0, T4, std::vector, std::tuple<T5>>>;
      const ER events_rec(nCmt, time, amt, rate, ii, evid, cmt, addl, ss);

      Matrix<typename EM::T_scalar, -1, -1> pred(EM::nCmt(events_rec), events_rec.num_event_times());

      using model_type = torsten::PKODEModel<typename EM::T_par, F>;

      integrator_type integrator(rel_tol, abs_tol, max_num_steps, as_rel_tol, as_abs_tol, as_max_num_steps, msgs);
      EventSolver<model_type, EM> pr;

      pr.pred(0, events_rec, pred, integrator, pMatrix, biovar, nCmt, f);
      return pred;
    }

    /**
     * Overload: omitting lag time, with default ode & algebra solver spec
     *
     */
    template <typename T0, typename T1, typename T2, typename T3, typename T4,
              typename T5, typename F>
    static stan::matrix_return_t<T0, T1, T2, T3, T4, T5>
    solve(const F& f,
          const int nCmt,
          TORSTEN_PMX_FUNC_EVENTS_ARGS,
          const std::vector<std::vector<T4> >& pMatrix,
          const std::vector<std::vector<T5> >& biovar,
          std::ostream* msgs) {
      return solve(f, nCmt,
                   time, amt, rate, ii, evid, cmt, addl, ss,
                   pMatrix, biovar,
                   RTOL_DE, ATOL_DE, MAXSTEP_DE,
                   RTOL_AS, ATOL_AS, MAXSTEP_AS,
                   msgs);
    }

    /**
     * Overload: omitting lag time, with default algebra solver spec
     *
     */
    template <typename T0, typename T1, typename T2, typename T3, typename T4,
              typename T5, typename F>
    static stan::matrix_return_t<T0, T1, T2, T3, T4, T5>
    solve(const F& f,
          const int nCmt,
          TORSTEN_PMX_FUNC_EVENTS_ARGS,
          const std::vector<std::vector<T4> >& pMatrix,
          const std::vector<std::vector<T5> >& biovar,
          double rel_tol,
          double abs_tol,
          long int max_num_steps,
          std::ostream* msgs) {
      return solve(f, nCmt,
                   time, amt, rate, ii, evid, cmt, addl, ss,
                   pMatrix, biovar,
                   rel_tol, abs_tol, max_num_steps,
                   RTOL_AS, ATOL_AS, MAXSTEP_AS,
                   msgs);
    }

    /**
     * Overload: omitting lag time, allow population-wise 1d array,
     * with full ode & algebra solver spec
     *
     */
    template <typename T0, typename T1, typename T2, typename T3,
              typename T_par, typename T_biovar, typename F,
              typename... Ts,
              typename = require_any_not_std_vector_t<T_par, T_biovar>,
              typename = require_all_not_std_vector_t<Ts...> >
    static stan::matrix_return_t<T0, T1, T2, T3, T_par, T_biovar>
    solve(const F& f,
          const int nCmt,
          TORSTEN_PMX_FUNC_EVENTS_ARGS,
          const std::vector<T_par>& pMatrix,
          const std::vector<T_biovar>& biovar,
          Ts... solver_ctrl) {
      return solve(f, nCmt,
                   time, amt, rate, ii, evid, cmt, addl, ss,
                   torsten::to_array_2d(pMatrix), torsten::to_array_2d(biovar),
                   solver_ctrl...);
    }

    /**
     * Overload: omitting bioavailability & lag time,
     * with full ode & algebra solver spec
     *
     */
    template <typename T0, typename T1, typename T2, typename T3, typename T4,
              typename F>
    static stan::matrix_return_t<T0, T1, T2, T3, T4>
    solve(const F& f,
               const int nCmt,
               TORSTEN_PMX_FUNC_EVENTS_ARGS,
               const std::vector<std::vector<T4> >& pMatrix,
               double rel_tol,
               double abs_tol,
               long int max_num_steps,
               double as_rel_tol,
               double as_abs_tol,
               long int as_max_num_steps,
               std::ostream* msgs) {
      // check arguments
      static const char* function("solve");
      torsten::pmx_check(time, amt, rate, ii, evid, cmt, addl, ss, pMatrix, function);

      using ER = NONMENEventsRecord<T0, T1, T2, T3>;
      using EM = EventsManager<ER, NonEventParameters<T0, T4, std::vector, std::tuple<>>>;
      const ER events_rec(nCmt, time, amt, rate, ii, evid, cmt, addl, ss);

      Matrix<typename EM::T_scalar, -1, -1> pred(EM::nCmt(events_rec), events_rec.num_event_times());

      using model_type = torsten::PKODEModel<typename EM::T_par, F>;

      integrator_type integrator(rel_tol, abs_tol, max_num_steps, as_rel_tol, as_abs_tol, as_max_num_steps, msgs);
      EventSolver<model_type, EM> pr;

      pr.pred(0, events_rec, pred, integrator, pMatrix, nCmt, f);
      return pred;
    }

    /**
     * Overload: omitting bioavailability & lag time,
     * with default ode & algebra solver spec
     *
     */
    template <typename T0, typename T1, typename T2, typename T3, typename T4,
              typename F>
    static stan::matrix_return_t<T0, T1, T2, T3, T4>
    solve(const F& f,
          const int nCmt,
          TORSTEN_PMX_FUNC_EVENTS_ARGS,
          const std::vector<std::vector<T4> >& pMatrix,
          std::ostream* msgs) {
      return solve(f, nCmt,
                   time, amt, rate, ii, evid, cmt, addl, ss,
                   pMatrix,
                   RTOL_DE, ATOL_DE, MAXSTEP_DE,
                   RTOL_AS, ATOL_AS, MAXSTEP_AS,
                   msgs);
    }

    /**
     * Overload: omitting bioavailability & lag time,
     * with default algebra solver spec
     *
     */
    template <typename T0, typename T1, typename T2, typename T3, typename T4,
              typename F>
    static stan::matrix_return_t<T0, T1, T2, T3, T4>
    solve(const F& f,
          const int nCmt,
          TORSTEN_PMX_FUNC_EVENTS_ARGS,
          const std::vector<std::vector<T4> >& pMatrix,
          double rel_tol,
          double abs_tol,
          long int max_num_steps,
          std::ostream* msgs) {
      return solve(f, nCmt,
                   time, amt, rate, ii, evid, cmt, addl, ss,
                   pMatrix,
                   rel_tol, abs_tol, max_num_steps,
                   RTOL_AS, ATOL_AS, MAXSTEP_AS,
                   msgs);
    }

    /**
     * Overload: omitting bioavailability & lag time, allow
     * population-wise 1d array,
     * with full algebra solver spec
     *
     */
    template <typename T0, typename T1, typename T2, typename T3,
              typename T_par, typename F,
              typename... Ts,
              typename = require_any_not_std_vector_t<T_par>,
              typename = require_all_not_std_vector_t<Ts...> >
    static stan::matrix_return_t<T0, T1, T2, T3, T_par>
    solve(const F& f,
          const int nCmt,
          TORSTEN_PMX_FUNC_EVENTS_ARGS,
          const std::vector<T_par>& pMatrix,
          Ts... solver_ctrl) {
      return solve(f, nCmt,
                   time, amt, rate, ii, evid, cmt, addl, ss,
                   torsten::to_array_2d(pMatrix),
                   solver_ctrl...);
    }

    /**
     * Overload: additional real data for ODE, with full ode & algebra
     * solver spec
     *
     */
    template <typename T0, typename T1, typename T2, typename T3, typename T4,
              typename T5, typename T6, typename F>
    static stan::matrix_return_t<T0, T1, T2, T3, T4, T5, T6>
    solve(const F& f,
          const int nCmt,
          TORSTEN_PMX_FUNC_EVENTS_ARGS,
          const std::vector<std::vector<T4> >& pMatrix,
          const std::vector<std::vector<T5> >& biovar,
          const std::vector<std::vector<T6> >& tlag,
          const std::vector<std::vector<double> >& x_r,
          double rel_tol,
          double abs_tol,
          long int max_num_steps,
          double as_rel_tol,
          double as_abs_tol,
          long int as_max_num_steps,
          std::ostream* msgs) {
      // check arguments
      static const char* function("solve");
      torsten::pmx_check(time, amt, rate, ii, evid, cmt, addl, ss, pMatrix, function);
      torsten::pmx_check(time.size(), nCmt, biovar, tlag);

      using ER = NONMENEventsRecord<T0, T1, T2, T3>;
      using EM = EventsManager<ER, NonEventParameters<T0, T4, std::vector, std::tuple<T5, T6>, double>>;
      const ER events_rec(nCmt, time, amt, rate, ii, evid, cmt, addl, ss);

      Matrix<typename EM::T_scalar, -1, -1> pred(EM::nCmt(events_rec), events_rec.num_event_times());

      using model_type = torsten::PKODEModel<typename EM::T_par, F>;

      integrator_type integrator(rel_tol, abs_tol, max_num_steps, as_rel_tol, as_abs_tol, as_max_num_steps, msgs);
      EventSolver<model_type, EM> pr;

      pr.pred(0, events_rec, pred, integrator, pMatrix, biovar, tlag, x_r, nCmt, f);
      return pred;
    }

    /**
     * Overload: additional real data for ODE, with default ode & algebra
     * solver spec
     *
     */
    template <typename T0, typename T1, typename T2, typename T3, typename T4,
              typename T5, typename T6, typename F>
    static stan::matrix_return_t<T0, T1, T2, T3, T4, T5, T6>
    solve(const F& f,
          const int nCmt,
          TORSTEN_PMX_FUNC_EVENTS_ARGS,
          const std::vector<std::vector<T4> >& pMatrix,
          const std::vector<std::vector<T5> >& biovar,
          const std::vector<std::vector<T6> >& tlag,
          const std::vector<std::vector<double> >& x_r,
          std::ostream* msgs) {
      return solve(f, nCmt,
                   time, amt, rate, ii, evid, cmt, addl, ss,
                   pMatrix, biovar, tlag, x_r,
                   RTOL_DE, ATOL_DE, MAXSTEP_DE,
                   RTOL_AS, ATOL_AS, MAXSTEP_AS,
                   msgs);
    }

    /**
     * Overload: additional real data for ODE, with default algebra
     * solver spec
     *
     */
    template <typename T0, typename T1, typename T2, typename T3, typename T4,
              typename T5, typename T6, typename F>
    static stan::matrix_return_t<T0, T1, T2, T3, T4, T5, T6>
    solve(const F& f,
          const int nCmt,
          TORSTEN_PMX_FUNC_EVENTS_ARGS,
          const std::vector<std::vector<T4> >& pMatrix,
          const std::vector<std::vector<T5> >& biovar,
          const std::vector<std::vector<T6> >& tlag,
          const std::vector<std::vector<double> >& x_r,
          double rel_tol,
          double abs_tol,
          long int max_num_steps,
          std::ostream* msgs) {
      return solve(f, nCmt,
                   time, amt, rate, ii, evid, cmt, addl, ss,
                   pMatrix, biovar, tlag, x_r,
                   rel_tol, abs_tol, max_num_steps,
                   RTOL_AS, ATOL_AS, MAXSTEP_AS,
                   msgs);
    }

    // Overload: with real data, PMX params can be either 1d or 2d arrays.
    template <typename T0, typename T1, typename T2, typename T3,
              typename T_par, typename T_biovar, typename T_tlag,
              typename F,
              typename... Ts,
              typename = require_any_not_std_vector_t<T_par, T_biovar, T_tlag>,
              typename = require_all_not_std_vector_t<Ts...> >
    static stan::matrix_return_t<T0, T1, T2, T3, T_par, T_biovar, T_tlag>
    solve(const F& f,
          const int nCmt,
          TORSTEN_PMX_FUNC_EVENTS_ARGS,
          const std::vector<T_par>& pMatrix,
          const std::vector<T_biovar>& biovar,
          const std::vector<T_tlag>& tlag,
          const std::vector<std::vector<double> >& x_r,
          Ts... solver_ctrl) {
      return solve(f, nCmt,
                   time, amt, rate, ii, evid, cmt, addl, ss,
                   torsten::to_array_2d(pMatrix),
                   torsten::to_array_2d(biovar),
                   torsten::to_array_2d(tlag), x_r,
                   solver_ctrl...);
    }

    /**
     * Overload: additional real & int data for ODE, with full ode & algebra
     * solver spec
     *
     */
    template <typename T0, typename T1, typename T2, typename T3, typename T4,
              typename T5, typename T6, typename F>
    static stan::matrix_return_t<T0, T1, T2, T3, T4, T5, T6>
    solve(const F& f,
          const int nCmt,
          TORSTEN_PMX_FUNC_EVENTS_ARGS,
          const std::vector<std::vector<T4> >& pMatrix,
          const std::vector<std::vector<T5> >& biovar,
          const std::vector<std::vector<T6> >& tlag,
          const std::vector<std::vector<double> >& x_r,
          const std::vector<std::vector<int> >& x_i,
          double rel_tol,
          double abs_tol,
          long int max_num_steps,
          double as_rel_tol,
          double as_abs_tol,
          long int as_max_num_steps,
          std::ostream* msgs) {
      // check arguments
      static const char* function("solve");
      torsten::pmx_check(time, amt, rate, ii, evid, cmt, addl, ss, pMatrix, function);
      torsten::pmx_check(time.size(), nCmt, biovar, tlag);

      using ER = NONMENEventsRecord<T0, T1, T2, T3>;
      using EM = EventsManager<ER, NonEventParameters<T0, T4, std::vector, std::tuple<T5, T6>, double, int>>;
      const ER events_rec(nCmt, time, amt, rate, ii, evid, cmt, addl, ss);

      Matrix<typename EM::T_scalar, -1, -1> pred(EM::nCmt(events_rec), events_rec.num_event_times());

      using model_type = torsten::PKODEModel<typename EM::T_par, F>;

      integrator_type integrator(rel_tol, abs_tol, max_num_steps, as_rel_tol, as_abs_tol, as_max_num_steps, msgs);
      EventSolver<model_type, EM> pr;

      pr.pred(0, events_rec, pred, integrator, pMatrix, biovar, tlag, x_r, x_i, nCmt, f);
      return pred;
    }

    /**
     * Overload: additional real & int data for ODE, with default ode & algebra
     * solver spec
     *
     */
    template <typename T0, typename T1, typename T2, typename T3, typename T4,
              typename T5, typename T6, typename F>
    static stan::matrix_return_t<T0, T1, T2, T3, T4, T5, T6>
    solve(const F& f,
          const int nCmt,
          TORSTEN_PMX_FUNC_EVENTS_ARGS,
          const std::vector<std::vector<T4> >& pMatrix,
          const std::vector<std::vector<T5> >& biovar,
          const std::vector<std::vector<T6> >& tlag,
          const std::vector<std::vector<double> >& x_r,
          const std::vector<std::vector<int> >& x_i,
          std::ostream* msgs) {
      return solve(f, nCmt,
                   time, amt, rate, ii, evid, cmt, addl, ss,
                   pMatrix, biovar, tlag, x_r, x_i,
                   RTOL_DE, ATOL_DE, MAXSTEP_DE,
                   RTOL_AS, ATOL_AS, MAXSTEP_AS,
                   msgs);
    }

    /**
     * Overload: additional real & int data for ODE, with default algebra
     * solver spec
     *
     */
    template <typename T0, typename T1, typename T2, typename T3, typename T4,
              typename T5, typename T6, typename F>
    static stan::matrix_return_t<T0, T1, T2, T3, T4, T5, T6>
    solve(const F& f,
               const int nCmt,
               TORSTEN_PMX_FUNC_EVENTS_ARGS,
               const std::vector<std::vector<T4> >& pMatrix,
               const std::vector<std::vector<T5> >& biovar,
               const std::vector<std::vector<T6> >& tlag,
               const std::vector<std::vector<double> >& x_r,
               const std::vector<std::vector<int> >& x_i,
               double rel_tol,
               double abs_tol,
               long int max_num_steps,
               std::ostream* msgs) {
      return solve(f, nCmt,
                   time, amt, rate, ii, evid, cmt, addl, ss,
                   pMatrix, biovar, tlag, x_r, x_i,
                   rel_tol, abs_tol, max_num_steps,
                   RTOL_AS, ATOL_AS, MAXSTEP_AS,
                   msgs);
    }

    // Overload: with real & int data, PMX params can be either 1d or 2d arrays.
    template <typename T0, typename T1, typename T2, typename T3,
              typename T_par, typename T_biovar, typename T_tlag,
              typename F,
              typename... Ts,
              typename = require_any_not_std_vector_t<T_par, T_biovar, T_tlag>,
              typename = require_all_not_std_vector_t<Ts...> >
    static stan::matrix_return_t<T0, T1, T2, T3, T_par, T_biovar, T_tlag>
    solve(const F& f,
          const int nCmt,
          TORSTEN_PMX_FUNC_EVENTS_ARGS,
          const std::vector<T_par>& pMatrix,
          const std::vector<T_biovar>& biovar,
          const std::vector<T_tlag>& tlag,
          const std::vector<std::vector<double> >& x_r,
          const std::vector<std::vector<int> >& x_i,
          Ts... solver_ctrl) {
      return solve(f, nCmt,
                   time, amt, rate, ii, evid, cmt, addl, ss,
                   torsten::to_array_2d(pMatrix),
                   torsten::to_array_2d(biovar),
                   torsten::to_array_2d(tlag), x_r, x_i,
                   solver_ctrl...);
    }
  };
}
#endif


// starting month is fixed to dec

#include<stdio.h>
#include <stdlib.h>
#include <string>
#include <math.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <algorithm>
#include <cassert>
#include <omp.h>
#include "ANIMALS.h"
#include "Misc.h"
#include "DefineParameters.h"
#include "DefinePriors.h"
#include "eigenmvn.h"
#include <ctime>
#include "Dist.h"
#include<sys/types.h>
#include <random>
#include <thread>
#include <vector>
#include "ProcessHeader.h"
#include <boost/uuid/seed_rng.hpp>
#include <boost/random/mersenne_twister.hpp>

using namespace std;

//typedef boost::lagged_fibonacci607 base_generator_type;

const int get_int(const string& s) {
	stringstream ss(s);
	int ret;
	ss >> ret;
	return ret;
}

int count_Data(ifstream &filename)// struct weatherdata *enviroData)
{
	int numrec = 0;
	string lines;
	if (filename){
		getline(filename, lines); //get rid of the first line,
		// ------------ read and store data into enviroData-------------
		while (getline(filename, lines))
		{
			numrec++;
		}

	}
	else{ cout << "Problem in opening file.\n"; exit(0); }

	return numrec;
}

double get_double(const string& s) {
	stringstream ss(s);
	double ret;
	ss >> ret;
	return  ret;
}

/*
void get_chlo_legth_dat(ifstream &filename, struct chlo_length *data){

int i = 0;
string lines;

if (filename){
getline(filename, lines);
i = 0;
while (getline(filename, lines)){
stringstream ss(lines);
vector<std::string> dat;
string field;
while (getline(ss, field, ',')) {
if (field.empty()){
field = "-9999";// missing value
}
dat.push_back(field);
}
data[i].chro = get_double(dat[0]);
data[i].p_len = roundf(get_double(dat[1]) * 100) / 100;;
vector<string>().swap(dat);
i++;
}
}
else{ cout << "Problem in opening CHLOROPHYLL BASED LENGTH file\n"; exit(0); }
}
*/

void get_old_params(ifstream &filename, struct parameters *params, vector<double> &rho){
	// update to mixute on 07/09
	int i = 0;
	string lines;

	if (filename){
		getline(filename, lines);
		i = 0;
		while (getline(filename, lines)){
			stringstream ss(lines);
			vector<std::string> dat;
			string field;
			while (getline(ss, field, '\t')) {
				if (field.empty()){
					cout << "There is missing value in param.txt" << endl;
					exit(0);
				}
				dat.push_back(field);
			}

			rho[i] = get_double(dat[1]);
			params[i].alpha[0] = get_double(dat[2]);
			params[i].alpha[1] = get_double(dat[3]);
			params[i].alpha[2] = get_double(dat[4]);
			params[i].beta = get_double(dat[5]);
			params[i].beta_sigma = get_double(dat[6]);
			params[i].detect[0] = get_double(dat[7]);
			params[i].detect[1] = get_double(dat[8]);
			params[i].detect[2] = get_double(dat[9]);
			params[i].error_N = get_double(dat[10]);
			params[i].jv_sur = get_double(dat[11]);
			params[i].k = get_double(dat[12]);
			params[i].larv_mor = get_double(dat[13]);
			params[i].x0 = get_double(dat[14]);
			vector<string>().swap(dat);
			i++;
		}
	}
	else{ cout << "Problem in opeing params.txt .\n"; exit(0); }
}

void get_ini_leng_dat(ifstream &filename, struct ini_length *ini_leng){
	// update to mixute on 07/09
	int i = 0;
	string lines;

	if (filename){
		getline(filename, lines);
		i = 0;
		while (getline(filename, lines)){
			stringstream ss(lines);
			vector<std::string> dat;
			string field;
			while (getline(ss, field, ' ')) {
				if (field.empty()){
					field = "-9999";// missing value
				}
				dat.push_back(field);
			}
			ini_leng[i].prop = get_double(dat[0]);
			ini_leng[i].mean = get_double(dat[1]);
			ini_leng[i].std = get_double(dat[2]);
			vector<string>().swap(dat);
			i++;
		}
	}
	else{ cout << "Problem in relative frequency among reefs file.\n"; exit(0); }
}
void get_ini_dist(ifstream &filename, struct reef_month *out){

	int i = 0;
	string lines;

	if (filename){
		getline(filename, lines);
		i = 0;
		while (getline(filename, lines)){
			stringstream ss(lines);
			vector<std::string> dat;
			string field;
			while (getline(ss, field, ' ')) {
				if (field.empty()){
					field = "-9999";// missing value
				}
				dat.push_back(field);
			}
			out[i].REEF_ID = dat[0];
			out[i].shelf = dat[1];
			out[i].lat = get_double(dat[2]);
			out[i].longi = get_double(dat[3]);
			out[i].rel = get_double(dat[4]);
			out[i].ini_obs = get_int(dat[5]);
			vector<string>().swap(dat);
			i++;
		}
	}
	else{ cout << "Problem in relative frequency among reefs file.\n"; exit(0); }

}
void get_coral_coverage(ifstream &filename, struct coral_cov *out, int s_yr, int s_mth){

	int i = 0;
	string lines;

	if (filename){
		getline(filename, lines);
		i = 0;
		while (getline(filename, lines)){
			stringstream ss(lines);
			vector<std::string> dat;
			string field;
			while (getline(ss, field, ' ')) {
				if (field.empty()){
					field = "-9999";// missing value
				}
				dat.push_back(field);
			}
			out[i].REEF_ID = dat[0];
			out[i].s_day = convert_yr_mth_time(s_yr, s_mth, get_int(dat[1]), get_int(dat[2]));
			out[i].cov = get_double(dat[3]);
			vector<string>().swap(dat);
			i++;
		}
	}
	else{ cout << "Problem in opening coral coverage.\n"; exit(0); }

}
void get_chlorophyll_dat(ifstream &filename, struct chloro_ori *out, int s_yr, int s_mth){

	int i = 0;
	string lines;

	if (filename){
		getline(filename, lines);
		i = 0;
		while (getline(filename, lines)){
			stringstream ss(lines);
			vector<std::string> dat;
			string field;
			while (getline(ss, field, ' ')) {
				if (field.empty()){
					field = "-9999";// missing value
				}
				dat.push_back(field);
			}
			out[i].REEF_ID = dat[0];
			out[i].s_day = convert_yr_mth_time(s_yr, s_mth, get_int(dat[1]), get_int(dat[2]));
			out[i].chlo = get_double(dat[3]);
			vector<string>().swap(dat);
			i++;
		}
	}
	else{ cout << "Problem in opening chlorophyll data.\n"; exit(0); }

}
void get_connect_dat(ifstream &filename, struct connect_ori *out){

	int i = 0;
	string lines;

	if (filename){
		getline(filename, lines);
		i = 0;
		while (getline(filename, lines)){
			stringstream ss(lines);
			vector<std::string> dat;
			string field;
			while (getline(ss, field, ' ')) {
				if (field.empty()){
					field = "-9999";// missing value
				}
				dat.push_back(field);
			}
			out[i].year = get_int(dat[0]);
			out[i].from = dat[1];
			out[i].to = dat[2];
			out[i].connect = get_double(dat[3]);
			vector<string>().swap(dat);
			i++;
		}
	}
	else{ cout << "Problem in opening connectivity data.\n"; exit(0); }

}
void get_obs_dat(ifstream &filename, struct obs_abund *out){

	int i = 0;
	string lines;

	if (filename){
		getline(filename, lines);
		i = 0;
		while (getline(filename, lines)){
			stringstream ss(lines);
			vector<std::string> dat;
			string field;
			while (getline(ss, field, ' ')) {
				if (field == "NA"){
					field = "-9999";// missing value
				}
				dat.push_back(field);
			}

			out[i].reef_ID = dat[0];
			out[i].year = get_int(dat[1]);
			out[i].mth = get_int(dat[2]);
			out[i].pz = get_double(dat[3]);
			out[i].CPUE = get_double(dat[4]);
			vector<string>().swap(dat);
			i++;
		}
	}
	else{ cout << "Problem in opening COTS abundance data.\n"; exit(0); }

}
void get_reef_size_dat(ifstream &filename, struct reef_size *out){

	int i = 0;
	string lines;

	if (filename){
		getline(filename, lines);
		i = 0;
		while (getline(filename, lines)){
			stringstream ss(lines);
			vector<std::string> dat;
			string field;
			while (getline(ss, field, '\t')) {
				if (field == "NA"){
					field = "-9999";// missing value
				}
				dat.push_back(field);
			}

			out[i].reef_ID = dat[1];
			out[i].size = get_double(dat[5]);

			vector<string>().swap(dat);
			i++;
		}
	}
	else{ cout << "Problem in opening REEF SIZE data.\n"; exit(0); }

}
void get_wtemp_dat(ifstream &filename, struct w_temp *out, int s_yr, int s_mth){

	int i = 0;
	string lines;

	if (filename){
		getline(filename, lines);
		i = 0;
		while (getline(filename, lines)){
			stringstream ss(lines);
			vector<std::string> dat;
			string field;
			while (getline(ss, field, ' ')) {
				if (field == "NA"){
					field = "-9999";// missing value
				}
				dat.push_back(field);
			}

			out[i].sim = convert_yr_mth_time(s_yr, s_mth, get_int(dat[0]), get_int(dat[1]));
			out[i].wTemp = get_int(dat[2]);
			vector<string>().swap(dat);
			i++;
		}
	}
	else{ cout << "Problem in opening WATER temperature data.\n"; exit(0); }

}
void usage()
{
	printf("Command line should have \n 1) simulation period (unit: Years) \n 2) starting population size  \n 3) Initial Sex ratio \n 4) Starting Year \n 5)Minimum tolerance value \n 6)Proportion of particles dropped per iteration \n 7) Number of total particles \n 8) Starting Tolerance value \n 9) Option (0:start new, 1:Continue) \n");
	exit(1);
}

int main(int argc, char **argv)
{
	ifstream aFile, bFile, cFile;
	ofstream myFile;
	int no_chl_rec, bins, bins_zero, coral_cov_rec;
	int i, p;
	int j, k, l;
	int chlo_rec;
	int n_conn; // number of connecticity records
	int no_loc;
	int SIM_LENGTH, INI_POP_SIZE, START_YR, SIM_YEARS;
	double INI_SEX_RAT;
	int DIM;

	double RATE;
	int PARTICLE_SIZE;
	double TOLERANCE_S, START_S;
	int OPTION;
	///---------------------------------------end very temp
	if (argc != 10) usage();
	SIM_YEARS = get_int(argv[1]);
	SIM_LENGTH = SIM_YEARS * 12;
	INI_POP_SIZE = get_int(argv[2]);
	INI_SEX_RAT = get_double(argv[3]);
	START_YR = get_int(argv[4]);
	TOLERANCE_S = get_double(argv[5]);// miniMUM TOLERANCE
	RATE = get_double(argv[6]);// Proportion of samples to be dropped
	PARTICLE_SIZE = get_int(argv[7]);// Number of particles to keep
	START_S = get_double(argv[8]);// miniMUM TOLERANCE
	OPTION = get_int(argv[9]); // 0: start from new; 1: provide the keep
	// store the parameters

	auto start = std::chrono::system_clock::now();

	// read in
	//1) annual manta-tow data
	//2) AMPTO data
	//-------------------Begin read in data--------------------------------------

	///////////////////////////////////////////
	/*	aFile.open("predlengthchro.txt", ios::in);
		no_chl_rec = count_Data(aFile);
		cout << "Nrow Chlorophyll Length data " << no_chl_rec << endl;
		aFile.clear();
		aFile.seekg(0);
		struct chlo_length *chlo_length;
		chlo_length = new struct chlo_length[no_chl_rec];
		get_chlo_legth_dat(aFile, &(*chlo_length));
		aFile.close();
		*/

	// for location where no COTS were seen on the first two years.
	/*
	cFile.open("ini_size_stru_fisk_zero.txt", ios::in);
	bins_zero = count_Data(cFile);
	cout << "Number of Length bin: " << bins_zero << endl;
	cFile.clear();
	cFile.seekg(0);

	struct ini_length * ini_leng_zero;
	ini_leng_zero = new struct ini_length[bins_zero];
	get_ini_leng_dat(cFile, ini_leng_zero);
	cFile.close();
	*/

	//////////////////////////////////////////////
	bFile.open("initial_length_v2.txt", ios::in);
	bins = count_Data(bFile);
	cout << "Number of Length bin: " << bins << endl;
	bFile.clear();
	bFile.seekg(0);

	struct ini_length * ini_leng;
	ini_leng = new struct ini_length[bins];
	get_ini_leng_dat(bFile, ini_leng);
	bFile.close();

	// Read in Locations and relative abundance
	aFile.open("starting_wt.txt", ios::in);
	no_loc = count_Data(aFile);
	aFile.clear();
	aFile.seekg(0);
	struct reef_month *temp_mth_reef;
	temp_mth_reef = new struct reef_month[no_loc];
	get_ini_dist(aFile, temp_mth_reef);
	aFile.close();

	aFile.open("MonthlyCoralAdjCycle.txt", ios::in);
	coral_cov_rec = count_Data(aFile);
	aFile.clear();
	aFile.seekg(0);
	struct coral_cov *CC;
	CC = new struct coral_cov[coral_cov_rec];
	get_coral_coverage(aFile, CC, START_YR, START_MTH);
	aFile.close();

	// assign CC to mth_reef
	convert_coralcover_foo(CC, temp_mth_reef, no_loc, SIM_LENGTH, coral_cov_rec);
	delete[] CC;
	CC = nullptr;

	bFile.open("chlorophylldata_all.txt", ios::in);
	chlo_rec = count_Data(bFile);
	cout << "Chlorophyll data " << chlo_rec << endl;
	bFile.clear();
	bFile.seekg(0);
	struct chloro_ori *chlo_temp;
	chlo_temp = new struct chloro_ori[chlo_rec];
	get_chlorophyll_dat(bFile, chlo_temp, START_YR, START_MTH);
	bFile.close();

	struct chlorophyll *chlo_dat;
	chlo_dat = new struct chlorophyll[no_loc];

	for (i = 0; i < no_loc; i++){
		chlo_dat[i].REEF_ID = temp_mth_reef[i].REEF_ID;
	}

	format_chloro(chlo_temp, chlo_dat, no_loc, SIM_LENGTH, chlo_rec);

	delete[] chlo_temp;
	chlo_temp = nullptr;

	aFile.open("Connect_09.txt", ios::in);
	n_conn = count_Data(aFile);
	aFile.clear();
	aFile.seekg(0);
	struct connect_ori *con_temp = new struct connect_ori[n_conn];
	get_connect_dat(aFile, con_temp);
	aFile.close();

	// convert struct of connect_ori to
	struct con *connect = new struct con[no_loc];

	int ctr1, yr;
	int s_yr;

	// assign soucing reef
	for (i = 0; i < no_loc; i++){
		s_yr = START_YR + 1; // recruitment starts in the following year
		connect[i].REEF_ID = temp_mth_reef[i].REEF_ID;
		ctr1 = 0;
		for (j = 0; j < (no_loc - 1)*(SIM_YEARS + 1); j++){
			connect[i].year[j] = s_yr;
			if (j == ((no_loc - 1) * (ctr1 + 1) - 1)){
				s_yr++;
				ctr1++;
			}

		}
		// assign sinking reefs
		l = 0;
		for (yr = 0; yr < SIM_YEARS + 1; yr++){
			for (k = 0; k < no_loc; k++){
				if (connect[i].REEF_ID != temp_mth_reef[k].REEF_ID){
					connect[i].to[l] = temp_mth_reef[k].REEF_ID;
					l++;
				}
			}
		}
	}
	fomat_connect(con_temp, connect, no_loc, n_conn, SIM_YEARS);

	delete[] con_temp;
	con_temp = nullptr;

	// read in reef size

	int nr;
	/*aFile.open("areaofReef.txt", ios::in);
	nr = count_Data(aFile);
	struct reef_size *size = new struct reef_size[nr];
	aFile.clear();
	aFile.seekg(0);
	get_reef_size_dat(aFile, size);
	aFile.close();

	for (i = 0; i < nr; i++){
	for (j = 0; j < no_loc; j++){
	if (temp_mth_reef[j].REEF_ID == size[i].reef_ID){
	temp_mth_reef[j].size = size[i].size;
	}
	}
	}

	delete[] size;
	size = nullptr;
	*/
	///---- read water temp data------
	bFile.open("Complete Pooled water temp.txt", ios::in);
	nr = count_Data(bFile);
	struct w_temp *wat_temp = new struct w_temp[nr];
	bFile.clear();
	bFile.seekg(0);
	get_wtemp_dat(bFile, wat_temp, START_YR, START_MTH);
	bFile.close();

	///// ---------------------- read the catch data

	int no_obs_rec;

	/*change to distribution on 08/09/2015
	aFile.open("LTMP_abun.txt", ios::in);
	no_obs_rec = count_Data(aFile);
	aFile.clear();
	aFile.seekg(0);
	struct obs_abund *obs_abundance = new struct obs_abund[no_obs_rec];
	get_obs_dat(aFile, obs_abundance);
	aFile.close();
	*/
	aFile.open("COT_abund_dis_cpue.txt", ios::in);
	no_obs_rec = count_Data(aFile);
	aFile.clear();
	aFile.seekg(0);
	struct obs_abund *obs_abundance = new struct obs_abund[no_obs_rec];
	get_obs_dat(aFile, obs_abundance);
	aFile.close();
	//-------------------------------end read in data--------------------------------

	///------ remove data

	// remove files
	remove("res_out.txt");
	remove("pop_str.txt");
	remove("rho_out.txt");
	remove("walltime.txt");
	remove("params.txt");
	remove("acc_rat.txt");
	//remove("annual.txt");

	/// assign universal initial structure
	// out put sort
	myFile.open("res_out.txt", ios::app);
	// just checking if struct2mat works
	myFile << "particle" << "\t" << "Loc" << "\t" << "Year" << "\t" << "Mth" << "\t" << "n_cots" << "\t" << "no_female" << "\t" << "no_male" << "\t" << "L15" << "\t" << "L1525" << "\t" << "L2540" << "\t" << "L40p" << endl;
	myFile.close();

	// Of a fixed number of COTS the follow functiond distribute COTS number according to the relative weight
	ini_pop_distribution(temp_mth_reef, no_loc, INI_POP_SIZE);

	myFile.open("params.txt", ios::app);
	//myFile << "ITI" << "\t" << "Par" << "\t" << "Summary_stat" << "\t" << "Beta" << "\t" << "detect" << "\t" << "jv_sur" << "\t" << "k" << "\t" << "larv_mor" << "\t" << "x0" << endl;
	myFile << "Par" << "\t" << "Summary_stat" << "\t";
	for (i = 0; i < no_loc; i++){
		myFile << "alpha_" << i << "\t";
	}
	myFile << "Beta" << "\t" << "Beta_sigma" << "\t";
	for (i = 0; i < no_loc; i++){
		myFile << "detect_" << i << "\t";
	}
	myFile << "Error" << "\t" << "jv_sur" << "\t" << "k" << "\t" << "larv_mor" << "\t" << "x0" << "\t" << endl;
	myFile.close();

	myFile.open("acc_rat.txt", ios::app);
	myFile << "ITI ratio i_acc R p_acc RN" << endl;
	myFile.close();

	///////////OMP////////////////////
	omp_set_dynamic(2);
	unsigned short max_thread = omp_get_max_threads();
	base_generator_type eng[max_thread];
	omp_set_num_threads(max_thread);

	struct parameters *params = new struct parameters[PARTICLE_SIZE];

	// add two elements to parameters

	// first runs
	Context dist;
	vector<double> rho(PARTICLE_SIZE);

	double e_max; // first epi_max
	int N_drop = (int)round(PARTICLE_SIZE* RATE); // numebr of partcles to be dropped
	struct parameters *param_keepers = new struct parameters[PARTICLE_SIZE - N_drop];

	if (OPTION == 0){

#pragma omp parallel for schedule(dynamic, 3) private(i, j, p) shared(rho,params, START_YR, no_loc, obs_abundance, PARTICLE_SIZE, bins, no_obs_rec,  temp_mth_reef,ini_leng, SIM_LENGTH, INI_SEX_RAT, eng)
		for (p = 0; p < PARTICLE_SIZE; p++){

			double sum_stats = 100000;
			int flg = 1; // flag ==1 means population extinct
			int seed = boost::uuids::detail::seed_rng()();
			struct mth_out *out = new struct mth_out[SIM_LENGTH*no_loc]; // store the monthly records

			eng[omp_get_thread_num()].seed(seed);
			while ((sum_stats >= START_S) || (flg == 1)){
				//eng[omp_get_thread_num()].seed(static_cast<unsigned int>((seed++)*p + time(0))*max_thread*getpid());
				int inini;
				do{
					params[p].beta = dist.uniform_real_dis(BETA_MIN, BETA_MAX, eng[omp_get_thread_num()]);
					params[p].beta_sigma = dist.lognormal_dis(BETA_SIG_MEAN, BETA_SIG_STD, eng[omp_get_thread_num()]);
					params[p].jv_sur = dist.lognormal_dis(JUV_SUR_MEAN, JUV_SUR_STD, eng[omp_get_thread_num()]);
					params[p].k = dist.lognormal_dis(ADULT_MOR_K_MEAN, ADULT_MOR_K_STD, eng[omp_get_thread_num()]);
					params[p].larv_mor = 1 - dist.lognormal_dis(LAR_DAILY_MOR_MEAN, LAR_DAILY_MOR_STD, eng[omp_get_thread_num()]);
					params[p].x0 = dist.lognormal_dis(ADULT_MOR_X0_MEAN, ADULT_MOR_X0_STD, eng[omp_get_thread_num()]);
					params[p].error_N = dist.uniform_real_dis(VAR_ERROR_MIN, VAR_ERROR_MAX, eng[omp_get_thread_num()]);

					for (j = 0; j < no_loc; j++){
						params[p].detect[j] = dist.uniform_real_dis(DETECT_LO, DETECT_HI, eng[omp_get_thread_num()]);
						params[p].alpha[j] = dist.lognormal_dis(DIS_MEAN, DIS_STD, eng[omp_get_thread_num()]);
					}
					inini= check_ini_para(params, p, DIM, no_loc);

				} while (inini == 0);

					//----------------------------------call the process model here!!
					//setting up starting reef matrix
				struct reef_month *mth_reef = new struct reef_month[no_loc];

				for (i = 0; i < no_loc; i++){
					mth_reef[i] = temp_mth_reef[i];
					mth_reef[i].yr = START_YR;
					mth_reef[i].mth = START_MTH;
				}

				ProcessModel(&params[p], mth_reef, ini_leng, obs_abundance, bins, no_loc, SIM_LENGTH, INI_SEX_RAT, max_thread, no_obs_rec, sum_stats, flg, eng[omp_get_thread_num()], START_S, START_S, TOLERANCE_S, out);

				delete[]mth_reef;
				mth_reef = nullptr;

			}
			rho[p] = sum_stats;
			cout << p << " " << rho[p] << " flag " << flg << endl;

		}// end particles of the first iteration

		e_max = *max_element(rho.begin(), rho.end()); // first epi_max

		cout << "First e max " << e_max << endl;

		/*myFile.open("params.txt", ios::app);

		for (p = 0; p < PARTICLE_SIZE; p++){
		myFile << p << "\t" << rho[p] << "\t" << params[p].beta << "\t" << params[p].beta_sigma << "\t" << params[p].jv_sur << "\t" << params[p].k << "\t" << params[p].larv_mor << "\t" << params[p].x0 << "\t" << params[p].error_N << "\t";
		for (i = 0; i < no_loc; i++){
		myFile << params[p].alpha[i] << "\t" << params[p].detect[i] << "\t";
		}
		myFile << endl;
		}
		myFile.close();
		*/
	}

	else{ // read in the params[p] data

		aFile.open("params_old.txt", ios::in);
		get_old_params(aFile, params, rho);
		aFile.close();
		e_max = *max_element(rho.begin(), rho.end()); // first epi_max

		cout << "First e max " << e_max << endl;
	}

	double e_next;
	long long R, RN;
	R = INITIAL_R;
	long double p_acc;
	int tck;
	int u = 0;

	do{// et to e_max
		int ctr = 0;
		// ----- adaptive design
		RN = R* N_drop;
		int i_acc = 0;
		tck = 0;
		// --- adaptive
		// assign the rank to params
		for (auto i : sort_indexes(rho)) {
			params[i].rank = ctr;
			//cout << i << " " << rho[i] << " " << params[i].rank << endl; // this is just to show values from largest to smallest; and the location of value (i) in the vector
			ctr++;
		}
		//for checking if the code works
		/*for (p = 0; p < PARTICLE_SIZE; p++){
		cout << rho[p] << " " << params[p].rank << endl;
		}*/
		// next step-replenish
		//----------------- keeppers

		int ctr_cc = 0;

		for (p = 0; p < PARTICLE_SIZE; p++){
			if (params[p].rank >= N_drop){
				param_keepers[ctr_cc] = params[p];
				ctr_cc++;
			}
			if (params[p].rank == N_drop){
				e_next = rho[p];
			}
		}

		cout << "next " << e_next << endl;
		/*
		myFile.open("params.txt", ios::app);

		for (i = 0; i < ctr_cc; i++){
		myFile << i << "\t" << param_keepers[i].rank << "\t" << param_keepers[i].beta << "\t" << param_keepers[i].beta_sigma << "\t" << param_keepers[i].jv_sur << "\t" << param_keepers[i].k << "\t" << param_keepers[i].larv_mor << "\t" << param_keepers[i].x0 << "\t" << param_keepers[i].error_N << "\t";
		for (j = 0; j < no_loc; j++){
		myFile << param_keepers[i].alpha[j] << "\t" << param_keepers[i].detect[j] << "\t";
		}
		myFile << endl;
		}
		myFile.close();
		*/
		// replensihing

		// parallel this part
		DIM = 13;

		// variance-covariance of

		MatrixXd S(DIM, DIM);
		MatrixXd temp_mat(PARTICLE_SIZE - N_drop, DIM);
		struct2mat(param_keepers, temp_mat, PARTICLE_SIZE - N_drop);

		MatrixXd x_bar = temp_mat.rowwise() - temp_mat.colwise().mean();
		S = 2 * (x_bar.adjoint() * x_bar) / double(temp_mat.rows());
		// end of mean and variance-cov matrix

		// code is fine to here
#pragma omp parallel for schedule(dynamic, 1) private(i,p) firstprivate(ctr_cc)shared(START_YR, no_loc, chlo_dat, obs_abundance, PARTICLE_SIZE, bins, no_obs_rec, temp_mth_reef,ini_leng, SIM_LENGTH, INI_SEX_RAT, DIM, S, N_drop, param_keepers, params, e_max, e_next, tck, RN,i_acc, rho, eng)
		for (p = 0; p < PARTICLE_SIZE; p++){

			double sum_stats;
			int flg = 0; // flg ==1 means population extinct
			struct parameters *temp_params = new struct parameters[1];
			struct mth_out *out = new struct mth_out[SIM_LENGTH*no_loc]; // store the monthly records

			//cout << "bf " << rho[p] << endl;
			//int seed = boost::uuids::detail::seed_rng()();

			//eng[omp_get_thread_num()].seed(seed);

			if (params[p].rank <= N_drop){// && tck< RN){ // replenish N_drop
				int flag = 0;
				int k = 0;

				do{// line 2.5 to 2.12
#pragma omp atomic
					k++;
#pragma omp atomic
					tck++;
					// step 1: select from keepers
					int oo;
					// q(.|theta)- line 2.4
					// mean vector
					// draws - equal probability
					VectorXd mean(DIM);

					oo = dist.uniform_int_dis(0, ctr_cc - 1, eng[omp_get_thread_num()]);// ctr-1 is total number of particles kept
					//#pragma omp critical
					struct2array(param_keepers, mean, oo);

					// draw theta **

					EigenMultivariateNormal<double> normX(mean, S, boost::uuids::detail::seed_rng()());
					int flgg = 0;
					MatrixXd temp(DIM, 1);
					while (flgg == 0){
						temp = normX.samples(1);
						flgg = check_array(temp, DIM);
					}
					/*
#pragma omp critical
					{
					cout << "p " << p << " prop " << temp << endl;
					cout << "oo " << oo << "mean" << mean << endl;
					}
					*/

					//cout << "mean" <<  mean << "\t" << "temp" << temp << endl;
					// work out the MH ratio
					double MH;
					// order is extremely important

					double top = lognormal_pdf_ln(DIS_MEAN, DIS_STD, temp(0)) + lognormal_pdf_ln(DIS_MEAN, DIS_STD, temp(1)) + lognormal_pdf_ln(DIS_MEAN, DIS_STD, temp(2)) + unif_pdf_ln(BETA_MIN, BETA_MAX, temp(3)) + lognormal_pdf_ln(BETA_SIG_MEAN, BETA_SIG_STD, temp(4)) - 3 * log(DETECT_HI - DETECT_LO) + unif_pdf_ln(VAR_ERROR_MIN, VAR_ERROR_MAX, temp(8)) + lognormal_pdf_ln(JUV_SUR_MEAN, JUV_SUR_STD, temp(9)) + lognormal_pdf_ln(ADULT_MOR_K_MEAN, ADULT_MOR_K_STD, temp(10)) + lognormal_pdf_ln(LAR_DAILY_MOR_MEAN, LAR_DAILY_MOR_STD, 1 - temp(11)) + lognormal_pdf_ln(ADULT_MOR_X0_MEAN, ADULT_MOR_X0_STD, temp(12)) + multivariate_normal_ln_pdf(temp, S, mean);

					double bot = lognormal_pdf_ln(DIS_MEAN, DIS_STD, mean(0)) + lognormal_pdf_ln(DIS_MEAN, DIS_STD, mean(1)) + lognormal_pdf_ln(DIS_MEAN, DIS_STD, mean(2)) + unif_pdf_ln(BETA_MIN, BETA_MAX, mean(3)) + lognormal_pdf_ln(BETA_SIG_MEAN, BETA_SIG_STD, mean(4)) - 3 * log(DETECT_HI - DETECT_LO) + unif_pdf_ln(VAR_ERROR_MIN, VAR_ERROR_MAX, mean(8)) + lognormal_pdf_ln(JUV_SUR_MEAN, JUV_SUR_STD, mean(9)) + lognormal_pdf_ln(ADULT_MOR_K_MEAN, ADULT_MOR_K_STD, mean(10)) + lognormal_pdf_ln(LAR_DAILY_MOR_MEAN, LAR_DAILY_MOR_STD, 1 - mean(11)) + lognormal_pdf_ln(ADULT_MOR_X0_MEAN, ADULT_MOR_X0_STD, mean(12)) + multivariate_normal_ln_pdf(mean, S, temp);

					MH = exp(top - bot);
					/*
#pragma omp critical
					{
					cout << " t " << top << " b " << bot;
					cout << " p " << p << " MH " << MH << endl;
					}
					*/

					if (MH > 1){ MH = 1; }// else MH is MH

					if (MH > dist.uniform_real_dis(0, 1, eng[omp_get_thread_num()])){
						array2struct(temp_params, temp, 0);
						// simulate data
						struct reef_month *mth_reef = new struct reef_month[no_loc];

						for (i = 0; i < no_loc; i++){
							mth_reef[i] = temp_mth_reef[i];
							mth_reef[i].yr = START_YR;
							mth_reef[i].mth = START_MTH;
						}
						/*
#pragma omp critical
						{
						cout << params[p].beta << " " << params[p].beta_sigma << " " << params[p].error_N << " " << params[p].jv_sur << " " << params[p].k << " " << params[p].larv_mor << " " << params[p].rank << " ";

						for (j = 0; j < no_loc; j++){
						cout << params[p].detect[j] << " " << params[p].alpha[j] << " ";
						}
						cout << endl;
						}
						*/
						ProcessModel(&temp_params[0], mth_reef, ini_leng, obs_abundance, bins, no_loc, SIM_LENGTH, INI_SEX_RAT, max_thread, no_obs_rec, sum_stats, flg, eng[omp_get_thread_num()], e_max, e_next, TOLERANCE_S, out);

						// if summary stat is less than E_Max
						if (sum_stats < e_next && flg == 0){
#pragma omp critical
							{
								cout << " p " << p << " sum_stat " << sum_stats << " flag " << flg << endl;
							}
							if (e_next < TOLERANCE_S){
#pragma omp critical
								{
									myFile.open("res_out.txt", ios::app);
									for (j = 0; j < no_loc*SIM_LENGTH; j++){
										myFile << p << "\t" << out[j].reef_ID << "\t" << out[j].year << "\t" << out[j].mth << "\t" << out[j].pop_size << "\t" << out[j].no_female << "\t" << out[j].no_male << "\t" << out[j].l15 << "\t" << out[j].l1525 << "\t" << out[j].l2540 << "\t" << out[j].l40p << endl;

									}
									myFile.close();

								}

							}
							flag = 1;
							array2struct(params, temp, p);
							rho[p] = sum_stats;
#pragma omp atomic
							i_acc++;
						}
						else{
							flag = 0;
						}
						delete[]mth_reef;
						mth_reef = nullptr;
					}
					else{ // MH else
						flag = 0;
					}
				}while(flag == 0 && k < R);

			} // end if: replenish N_drop

			delete[] temp_params;
			temp_params = nullptr;
			delete[] out;
			out = nullptr;
		} // end of p iteration- replenishing
		cout << " icc " << i_acc << endl;

		p_acc = (double)i_acc / RN;
		R = log(0.1) / log(1 - p_acc);

		// update e_max;
		cout << "acceptance ratio " << p_acc << endl;
		cout << " R " << R << endl;
		cout << endl;
		u++;
		double uu = (double)i_acc / tck;
		myFile.open("acc_rat.txt", ios::app);
		myFile << u << " " << uu << " " << i_acc << " " << R << " " << p_acc << " "<< RN << endl;
		myFile.close();

		e_max = *max_element(rho.begin(), rho.end());
		cout << "e_max" << e_max << endl;

	} while (e_max > TOLERANCE_S);// && tck < RN);

	auto end = std::chrono::system_clock::now();
	auto elapsed = end - start;
	myFile.open("walltime.txt", ios::app);
	myFile << elapsed.count() << '\n';
	myFile.close();

	////-----------------
	myFile.open("params.txt", ios::app);

	for (p = 0; p < PARTICLE_SIZE; p++){
		myFile << p << "\t" << rho[p] << "\t";

		for (i = 0; i < no_loc; i++){
			myFile << params[p].alpha[i] << "\t";
		}

		myFile << params[p].beta << "\t" << params[p].beta_sigma << "\t";
		for (i = 0; i < no_loc; i++){
			myFile << params[p].detect[i] << "\t";
		}
		myFile << params[p].error_N << "\t" << params[p].jv_sur << "\t" << params[p].k << "\t" << params[p].larv_mor << "\t" << params[p].x0 << "\t" << endl;
	}
	myFile.close();

	cout << "Congratulations XXOO!!" << endl;

	delete[] ini_leng;
	delete[] temp_mth_reef;
	delete[] params;
	delete[] wat_temp;
	delete[] connect;
	delete[] chlo_dat;
	delete[] obs_abundance;
	delete[] param_keepers;

	param_keepers = nullptr;
	obs_abundance = nullptr;
	chlo_dat = nullptr;

	connect = nullptr;
	wat_temp = nullptr;
	params = nullptr;

	ini_leng = nullptr;

	return 0;
}


/*
 *
 * Copyright (C) 2015 Mohammad Javad Dousti, Qing Xie, and Massoud Pedram, SPORT lab,
 * University of Southern California. All rights reserved.
 *
 * Please refer to the LICENSE file for terms of use.
 *
*/
#include "headers/rc_utils.h"
#include <omp.h>
//#include <boost/numeric/odeint/external/openmp/openmp.hpp>
//#include <thrust/device_vector.h>
//#include <boost/numeric/odeint/external/thrust/thrust_algebra.hpp>
//#include <boost/numeric/odeint/external/thrust/thrust_operations.hpp>
//#include <boost/numeric/odeint/external/mkl/mkl_operations.hpp>

//double **RCutils::matrix;
//double *RCutils::invC_vector;
//double **RCutils::A_matrix;

double *RCutils::P_vector;
int RCutils::n;
int** RCutils::a_matrix_indices;
double** RCutils::a_matrix_values;

/* For Eigen */
//SparseMatrix<double> RCutils::a_matrix;
//VectorXd RCutils::p_vector;

#if USE_GPU==1
void RCutils::checkGPUStatus(culaStatus status){
    char buf[256];

    if(!status)
        return;

    culaGetErrorInfoString(status, culaGetErrorInfo(), buf, sizeof(buf));
    cout<<buf<<endl;

    culaShutdown();
    exit(-1);
}

void RCutils::gpuSolveSteady(double **matrix, int N, double* P_vector, double *T_vector){
    int NRHS = 1;

    culaStatus status;

    culaDouble* A = NULL;
    culaInt* IPIV = NULL;

    cout<<"--------------------"<<endl;
    cout<<" Calling GPU Solver"<<endl;
    cout<<"--------------------"<<endl;

    cout<<"Allocating Matrices..."<<endl;
    A = (culaDouble*)malloc(N*N*sizeof(culaDouble));
    IPIV = (culaInt*)malloc(N*sizeof(culaInt));
    if(!A || !IPIV){
        cerr<<"Cannot allocate contiguous memory for solving the linear set of equations using GPU."<<endl;
    	exit(-1);
    }

    cout<<"Initializing CULA..."<<endl;
    status = culaInitialize();
    RCutils::checkGPUStatus(status);

	for (int i = 0; i < N; i++){
	    memmove(&A[i*N], matrix[i], sizeof(double) * N);
	}

    //Making a copy of P_vector into T_vector; This is required per specification
    memcpy(T_vector, P_vector, N*sizeof(culaDouble));

    //memset(IPIV, 0, N*sizeof(culaInt));

    cout<<"Calling the solver..."<<endl;
    //General Solver
//	status = culaDgesv(N, NRHS, A, N, IPIV, T_vector, N);
    //Assuming that A is a positive-definite matrix
    status = culaDposv('U', N, NRHS, A, N, T_vector, N);
    if(status == culaInsufficientComputeCapability){
        cout<<"No Double precision support available."<<endl;
        delete(A);
        delete(IPIV);
        culaShutdown();
        return;
    }
    RCutils::checkGPUStatus(status);

    cout<<"Shutting down CULA..."<<endl<<endl;
    culaShutdown();

    delete(A);
    delete(IPIV);
}
#else

/**
 * This solver uses Eigen library to solve the linear set of equations
 */
void RCutils::cpuSolveSteady(double **matrix, int N, double* P_vector, vector<double>& T_vector){
//	Eigen::setNbThreads(8);
//	MatrixXd A = MatrixXd(N,N);

	SparseMatrix<double> A(N,N);
	vector<Triplet<double> > triplet;

	VectorXd b = VectorXd(N);
	//SparseVector<double> b(N);
	for (int i = 0; i < N; i++){
		for(int j=0; j<N; j++){
			//A(i,j)=matrix[i][j];
			if (matrix[i][j]!=0)
				triplet.push_back(Triplet<double> (i,j,matrix[i][j]));
		}
		b(i)=P_vector[i];
	}
	A.setFromTriplets(triplet.begin(), triplet.end());

	cout<<"Calling the solver..."<<endl;
	SimplicialCholesky<SparseMatrix<double> > chol(A);

	b = chol.solve(b);

	//b=A.llt().solve(b);
//	VectorXd x =  b;

	for (int i = 0; i < N; i++){
		T_vector[i]=b(i);
	}
}

void RCutils::thermEq(const state_type &T_vector, state_type &dTdt, double t){
// Code written for Eigen
//   const double *PtState = &T_vector[0];
//   double *PtChange = &dTdt[0];
//
//   typedef Map<const VectorXd> MapTypeConst; // a read-only map
//   typedef Map<VectorXd> MapType;
//
//
//   MapTypeConst States(PtState, n);
//   MapType Changes(PtChange, n);
//
//   Changes = p_vector - (a_matrix * States);

	#pragma omp parallel for schedule(auto) num_threads(4)
//	#pragma omp parallel for schedule(runtime)
	for(int i=0; i<n; i++){
		dTdt[i]=(P_vector[i]);

		for(int j=0; j<n; j++){
			if (a_matrix_indices[i][j]!=-1){
				dTdt[i]-= a_matrix_values[i][j] * T_vector[a_matrix_indices[i][j]];
			}else
				break;
		}

	}
	/*
	 * GPU:
	 *  	typedef thrust::device_vector<double> state_type;
	 * 		runge_kutta4< state_type , double , state_type , double , thrust_algebra ,	thrust_operations > rk4;
	 *
	 */

	//		for(int j=0; j<n; j++){
	//			if (A_matrix[i][j]!=0){
	//				dTdt[i]-=(A_matrix[i][j]*T_vector[j]);
	//			}
	//		}

}

void RCutils::writeTemp( const state_type &T_vector , const double t ){
	cout<<t<<":\t"<<T_vector[0]-273.15<<" C"<<"\n";
}

//template<std::size_t element_no>
void RCutils::cpuSolveTransient(int n, double ** A_matrix, int** a_matrix_indices, double** a_matrix_values,
		double *invC_vector, double* P_vector, vector<double> &T_vector, double time_start, double time_end){

	RCutils::a_matrix_indices=a_matrix_indices;
	RCutils::a_matrix_values=a_matrix_values;
	RCutils::n = n;

	/* Calculating P' = C^-1 * P  */
	RCutils::P_vector = Utils::vectorAlloc(n);
	Utils::diagmatvectmult(RCutils::P_vector, invC_vector, &P_vector[0], n);

	cout<<T_vector[0]-273.15<<" C"<<endl;

	// For Eigen
//	a_matrix=SparseMatrix<double>(n,n);
//	p_vector = VectorXd(n);
//
//	vector<Triplet<double> > triplet;
//	for (int i = 0; i < n; i++){
//		for(int j=0; j<n; j++){
//			if (A_matrix[i][j]!=0){
//				triplet.push_back(Triplet<double> (i, j, A_matrix[i][j]));
//			}
//		}
//		p_vector(i)=RCutils::P_vector[i];
//	}
//	a_matrix.setFromTriplets(triplet.begin(), triplet.end());

	//integrate( system , x0 , t0 , t1 , dt )
	//http://headmyshoulder.github.io/odeint-v2/doc/boost_numeric_odeint/odeint_in_detail/integrate_functions.html

	/* use the scratch pad vector to find (inv_A)*POWER */
	//vdMul(n, &P_vector[0], invC_vector, &P_vector[0]);

	double dt = 0.00001;

//	runge_kutta_cash_karp54 is better than runge_kutta_dopri5
	typedef  runge_kutta_cash_karp54 < state_type > error_stepper_type;
	typedef controlled_runge_kutta< error_stepper_type > controlled_stepper_type;
	controlled_stepper_type controlled_stepper;

//    adams_bashforth_moulton< 2 , state_type, double, state_type, double, openmp_range_algebra > stepper_adams;

//    int chunk_size = T_vector.size()/omp_get_max_threads();
//    omp_set_schedule( omp_sched_static , chunk_size );

	struct timespec start, end;

	cout<<"Calling the solver..."<<endl;
	clock_gettime(CLOCK_MONOTONIC, &start);

    integrate_adaptive( make_controlled( 5E-1 , 1E-1 , error_stepper_type() ) , thermEq , T_vector , time_start, time_end , dt);

//	integrate_adaptive(stepper_adams , thermEq , T_vector , time_start, time_end , dt);

//	integrate_adaptive( make_controlled<error_stepper_type>( 1E-3 , 1E-3) , thermEq , T_vector , time_start, time_end , dt);
//  integrate_adaptive(stepper_mew , thermEq , T_vector , time_start, time_end , dt);
//	integrate_adaptive(controlled_stepper, thermEq , T_vector , time_start, time_end , dt);

	clock_gettime(CLOCK_MONOTONIC, &end);
	double elapsed = (end.tv_sec - start.tv_sec);
	elapsed += (end.tv_nsec - start.tv_nsec) / 1e9;
	cout<<endl<<"ODE runtime: "<<elapsed<<" s"<<endl;

	cout<<T_vector[0]-273.15<<" C"<<endl;

	Utils::vectorDealloc(RCutils::P_vector);

//	Baseline: HotSpot code
//	double *power = Utils::vectorAlloc(n);
//	/* use the scratch pad vector to find (inv_A)*POWER */
//    diagmatvectmult(power, invC_vector, &P_vector[0], n);
//
//    /* Obtain temp at time (t+time_elapsed).
//     * Instead of getting the temperature at t+time_elapsed directly, we do it
//     * in multiple steps with the correct step size at each time
//     * provided by rk4
//     */
//	int i=0;
//	double h, time_elapsed = 0.1;
//
//	for (double t = 0, new_h = 1e-7 ; t < time_elapsed && new_h >= 1e-7*1.0e-6; t+=h) {
//        h = new_h;
//
//        /* the slope function callback is typecast accordingly */
//        new_h = rk4(A_matrix, &T_vector[0], &P_vector[0], n, &h, &T_vector[0], (slope_fn_ptr) slope_fn_block);
//        new_h = std::min(new_h, time_elapsed-t-h);
//    	cout << t << ":\t" << T_vector[0]-273.15<<" C"<<endl;
//        i++;
//    }
}

#endif
double RCutils::calcThermalConductivity(double k, double thickness, double area){
	if (thickness==0){
		cerr<<"The thickness of an element cannot be zero."<<endl;
		exit(-1);
	}
	return k * area / thickness;
}

double RCutils::calcSubComponentCapacitance(SubComponent *sc){
	double volume = sc->getLength() * sc->getWidth() * sc->getHeight();

	if (volume==0)
		cout<<sc->getName() << " has nil volume."<<endl;

	else if (sc->getComponent()->getMaterial()->getSpecificHeat()==0)
		cout<<sc->getName() << " has nil specific heat."<<endl;

	else if (sc->getComponent()->getMaterial()->getDensity()==0)
		cout<<sc->getName() << " has nil density."<<endl;

	return C_FACTOR * sc->getComponent()->getMaterial()->getSpecificHeat() *
			sc->getComponent()->getMaterial()->getDensity() * volume;
}

double RCutils::calcAmbientResistance(double h, double area){
	return 1/(h * area);
}

double RCutils::overallParallelConductivity(double k1, double k2){
	return (k1 * k2)/( k1 + k2 );
}

bool RCutils::touchesAirInXDir(SubComponent *sc, Device* device){
	if (Utils::eq(sc->getX(), device->getX()) ||
			Utils::eq(sc->getX() +  sc->getLength(), device->getX() +  device->getLength()))
		return true;
	else
		return false;
}

bool RCutils::touchesAirInYDir(SubComponent *sc, Device* device){
	if (Utils::eq(sc->getY(), device->getY()) ||
			Utils::eq(sc->getY() +  sc->getWidth(), device->getY() +  device->getWidth())){
		return true;
	}else{
		return false;
	}
}

bool RCutils::touchesAirFromTopBot(SubComponent *sc, Device* device){
	if (Utils::eq(sc->getZ() , device->getZ()) ||
			Utils::eq(sc->getZ() + sc->getHeight() , device->getZ() +  device->getHeight()))
		return true;
	else
		return false;
}

double RCutils::calcConductanceToAmbient(SubComponent *sc, Device* device){
	double commonArea;
	double t1=0;
	double k;
	double K=0;
	double Kx=0, Ky=0, Kz=0, Rx, Ry, Rz;
//	double h = 10 * 1.15; //	W/m^2/K, from <http://www.engineeringtoolbox.com/convective-heat-transfer-d_430.html>
	double h = 10 * 1.15;
	if (touchesAirFromTopBot(sc, device)){	//Touches air from top or bottom
		t1 = sc->getHeight() / 2;
		commonArea = sc->getLength() * sc->getWidth();
//		cout<<p1->getName() << " and "<<" have XY common area to ambient: "<<commonArea<<endl;

		k = sc->getComponent()->getMaterial()->getNormalConductivity();

		Kz= RCutils::calcThermalConductivity(k, t1, commonArea);
//		cout<<RCutils::calcThermalConductivity(k1, t1, commonArea)<<endl;
		Rz = RCutils::calcAmbientResistance(h, commonArea);
		Kz = RCutils::overallParallelConductivity(Kz, 1/Rz);
	}
	if (touchesAirInXDir(sc, device)){	//Touches air from the X side
		t1 = sc->getLength() / 2;
		commonArea = sc->getWidth() * sc->getHeight();
//		cout<<p1->getName() << " and "<<" have YZ common area to ambient: "<<commonArea<<endl;

		//Setting the k1 value to the proper value if the planar conductivity differs from the normal conductivity
		if (sc->getComponent()->getMaterial()->hasPlanarConductivity())
			k = sc->getComponent()->getMaterial()->getPlanarConductivity();
		else
			k = sc->getComponent()->getMaterial()->getNormalConductivity();

		Kx = RCutils::calcThermalConductivity(k, t1, commonArea);
		Rx = RCutils::calcAmbientResistance(h, commonArea);
		Kx = RCutils::overallParallelConductivity(Kx, 1/Rx);
	}
	if (touchesAirInYDir(sc , device)){		//Touches air from the Y side

		t1 = sc->getWidth() / 2;
		commonArea = sc->getLength() * sc->getHeight();
//		cout<<p1->getName() << " and "<<" have XZ common area to ambient: "<<commonArea<<endl;

		//Setting the k1 value to the proper value if the planar conductivity differs from the normal conductivity
		if (sc->getComponent()->getMaterial()->hasPlanarConductivity())
			k = sc->getComponent()->getMaterial()->getPlanarConductivity();
		else
			k = sc->getComponent()->getMaterial()->getNormalConductivity();

		Ky = RCutils::calcThermalConductivity(k, t1, commonArea);
		Ry = RCutils::calcAmbientResistance(h, commonArea);
		Ky = RCutils::overallParallelConductivity(Ky, 1/Ry);
	}
	return K= Kx + Ky + Kz;
}

double RCutils::calcCommonConductance(SubComponent *sc1, SubComponent *sc2){
	double commonArea;
	double t1=0, t2=0;
	double k1, k2;

	// common area in the Y & Z planes
	double commonX, commonY, commonZ;

	commonX= min(sc1->getX()+sc1->getLength() , sc2->getX()+sc2->getLength()) - max(sc1->getX() , sc2->getX());
	commonY= min(sc1->getY()+sc1->getWidth() , sc2->getY()+sc2->getWidth()) - max(sc1->getY() , sc2->getY());
	commonZ= min(sc1->getZ()+sc1->getHeight() , sc2->getZ()+sc2->getHeight()) - max(sc1->getZ() , sc2->getZ());

	if ((Utils::eq(sc1->getX() + sc1->getLength() , sc2->getX()) ||
		Utils::eq(sc2->getX() + sc2->getLength() , sc1->getX())) && commonZ>0 && commonY>0){
		commonArea = commonY * commonZ;
		 t1 = sc1->getLength() / 2;
		 t2 = sc2->getLength() / 2;

		 //using planar thermal conductivity if the material has different value for it
		 if (sc1->getComponent()->getMaterial()->hasPlanarConductivity()){
			 k1 = sc1->getComponent()->getMaterial()->getPlanarConductivity();
		 }else{
			 k1 = sc1->getComponent()->getMaterial()->getNormalConductivity();
		 }
		 if (sc2->getComponent()->getMaterial()->hasPlanarConductivity()){
			 k2 = sc2->getComponent()->getMaterial()->getPlanarConductivity();
		 }else{
			 k2 = sc2->getComponent()->getMaterial()->getNormalConductivity();
		 }
//		 cout<<"Common area (YZ) btn "<<p1->getName()<<" and "<<p2->getName()<<" is "<<commonArea<<endl;
	}else if ((Utils::eq(sc1->getY() + sc1->getWidth() , sc2->getY()) ||
			Utils::eq(sc2->getY() + sc2->getWidth() , sc1->getY())) && commonX>0 && commonZ>0){
		commonArea = commonX * commonZ;
		 t1 = sc1->getWidth() / 2;
		 t2 = sc2->getWidth() / 2;

		 //using planar thermal conductivity if the material has different value for it
		 if (sc1->getComponent()->getMaterial()->hasPlanarConductivity()){
			 k1 = sc1->getComponent()->getMaterial()->getPlanarConductivity();
		 }else{
			 k1 = sc1->getComponent()->getMaterial()->getNormalConductivity();
		 }
		 if (sc2->getComponent()->getMaterial()->hasPlanarConductivity()){
			 k2 = sc2->getComponent()->getMaterial()->getPlanarConductivity();
		 }else{
			 k2 = sc2->getComponent()->getMaterial()->getNormalConductivity();
		 }
//		 cout<<"Common area (XZ) btn "<<p1->getName()<<" and "<<p2->getName()<<" is "<<commonArea<<endl;
	}else if ((Utils::eq(sc1->getZ() + sc1->getHeight() , sc2->getZ()) ||
			Utils::eq(sc2->getZ() + sc2->getHeight(), sc1->getZ())) && commonX>0 && commonY>0){
		commonArea = commonX * commonY;
		t1 = sc1->getHeight() / 2;
		t2 = sc2->getHeight() / 2;

		//using normal conductivity since it is in the vertical direction
		 k1 = sc1->getComponent()->getMaterial()->getNormalConductivity();
		 k2 = sc2->getComponent()->getMaterial()->getNormalConductivity();

//		 cout<<"Common area (XY) btn "<<p1->getName()<<" and "<<p2->getName()<<" is "<<commonArea<<endl;
	}else{
		commonArea = 0;
		return 0;
	}
	//cout<<p1->getName() << " and "<<p2->getName() <<" have common area: "<<commonArea<<endl;

	double K1 = RCutils::calcThermalConductivity(k1, t1, commonArea);
	double K2 = RCutils::calcThermalConductivity(k2, t2, commonArea);

	return RCutils::overallParallelConductivity(K1,K2);
}

/* compute the slope vector dy for the transient equation
 * dy + cy = p. useful in the transient solver
 */
void slope_fn_block(double *y, double *p, double *dy, double **c, int n)
{
    /* for our equation, dy = p - cy */
    int i;
    double *t = Utils::vectorAlloc(n);
    Utils::matvectmult(t, c, y, n);
    for (i = 0; i < n; i++)
        dy[i] = p[i]-t[i];
    Utils::vectorDealloc(t);
}

/* slope function pointer - used as a call back by the transient solver */
typedef void (*slope_fn_ptr)(void *y, void *p, void *dy, double **c, int n);

/* core of the 4th order Runge-Kutta method, where the Euler step
 * (y(n+1) = y(n) + h * k1 where k1 = dydx(n)) is provided as an input.
 * to evaluate dydx at different points, a call back function f (slope
 * function) is also passed as a parameter. Given values for y, and k1,
 * this function advances the solution over an interval h, and returns
 * the solution in yout. For details, see the discussion in "Numerical
 * Recipes in C", Chapter 16, from
 * http://www.nrbook.com/a/bookcpdf/c16-1.pdf
 */
void rk4_core(double **c, double *y, double *k1, void *p, int n, double h, double *yout, slope_fn_ptr f)
{
	int i;
	double *t, *k2, *k3, *k4;
	k2 = Utils::vectorAlloc(n);
	k3 = Utils::vectorAlloc(n);
	k4 = Utils::vectorAlloc(n);
	t = Utils::vectorAlloc(n);

	/* k2 is the slope at the trial midpoint (t) found using
	 * slope k1 (which is at the starting point).
	 */
	/* t = y + h/2 * k1 (t = y; t += h/2 * k1) */
	for(i=0; i < n; i++)
		t[i] = y[i] + h/2.0 * k1[i];
	/* k2 = slope at t */
	(*f)(t, p, k2, c, n);

	/* k3 is the slope at the trial midpoint (t) found using
	 * slope k2 found above.
	 */
	/* t =  y + h/2 * k2 (t = y; t += h/2 * k2) */
	for(i=0; i < n; i++)
		t[i] = y[i] + h/2.0 * k2[i];
	/* k3 = slope at t */
	(*f)(t, p, k3, c, n);

	/* k4 is the slope at trial endpoint (t) found using
	 * slope k3 found above.
	 */
	/* t =  y + h * k3 (t = y; t += h * k3) */
	for(i=0; i < n; i++)
		t[i] = y[i] + h * k3[i];
	/* k4 = slope at t */
	(*f)(t, p, k4, c, n);

	/* yout = y + h*(k1/6 + k2/3 + k3/3 + k4/6)	*/

	for (i =0; i < n; i++)
		yout[i] = y[i] + h * (k1[i] + 2*k2[i] + 2*k3[i] + k4[i])/6.0;

	Utils::vectorDealloc(k2);
	Utils::vectorDealloc(k3);
	Utils::vectorDealloc(k4);
	Utils::vectorDealloc(t);
}

void copy_dvector (double *dst, double *src, int n){
    memmove(dst, src, sizeof(double) * n);
}

#define RK4_SAFETY      0.95
#define RK4_MAXUP       5.0
#define RK4_MAXDOWN     10.0
#define RK4_PRECISION   0.01
double rk4(double **c, double *y, void *p, int n, double *h, double *yout, slope_fn_ptr f)
{
    int i;
    double *k1, *t1, *t2, *ytemp, max, new_h = (*h);

    k1 = Utils::vectorAlloc(n);
    t1 = Utils::vectorAlloc(n);
    t2 = Utils::vectorAlloc(n);
    ytemp = Utils::vectorAlloc(n);

    /* evaluate the slope k1 at the beginning */
    (*f)(y, p, k1, c, n);

    /* try until accuracy is achieved   */
    do {
        (*h) = new_h;

        /* try RK4 once with normal step size   */
        rk4_core(c, y, k1, p, n, (*h), ytemp, f);

        /* repeat it with two half-steps    */
        rk4_core(c, y, k1, p, n, (*h)/2.0, t1, f);

        /* y after 1st half-step is in t1. re-evaluate k1 for this  */
        (*f)(t1, p, k1, c, n);

        /* get output of the second half-step in t2 */
        rk4_core(c, t1, k1, p, n, (*h)/2.0, t2, f);

        /* find the max diff between these two results:
         * use t1 to store the diff
         */
		for(i=0; i < n; i++)
			t1[i] = fabs(ytemp[i] - t2[i]);
		max = t1[0];
		for(i=1; i < n; i++)
			if (max < t1[i])
				max = t1[i];

	       /*
	         * compute the correct step size: see equation
	         * 16.2.10  in chapter 16 of "Numerical Recipes
	         * in C"
	         */
	        /* accuracy OK. increase step size  */
	        if (max <= RK4_PRECISION) {
	            new_h = RK4_SAFETY * (*h) * pow(fabs(RK4_PRECISION/max), 0.2);
	            if (new_h > RK4_MAXUP * (*h))
	                new_h = RK4_MAXUP * (*h);
	        /* inaccuracy error. decrease step size and compute again */
	        } else {
	            new_h = RK4_SAFETY * (*h) * pow(fabs(RK4_PRECISION/max), 0.25);
	            if (new_h < (*h) / RK4_MAXDOWN)
	                new_h = (*h) / RK4_MAXDOWN;
	        }

	    } while (new_h < (*h));

	    /* commit ytemp to yout */
	    copy_dvector(yout, ytemp, n);

	    /* clean up */
	    Utils::vectorDealloc(k1);
	    Utils::vectorDealloc(t1);
	    Utils::vectorDealloc(t2);
	    Utils::vectorDealloc(ytemp);

	    /* return the step-size */
	    return new_h;
}

/*
 * LUP decomposition from the pseudocode given in the CLRS book
 * ('Introduction to Algorithms'). The matrix 'a' is
 * transformed into an in-place lower/upper triangular matrix
 * and the vector'p' carries the permutation vector such that
 * Pa = lu, where 'P' is the matrix form of 'p'.
 */

void RCutils::lupdcmp(double**a, int n, int *p){
	int i, j, k, pivot=0;
	double max = 0;

	/* start with identity permutation	*/
	for (i=0; i < n; i++)
		p[i] = i;

	for (k=0; k < n; k++)	 {
		max = 0;
		for (i = k; i < n; i++)	{
			if (fabs(a[i][k]) > max) {
				max = fabs(a[i][k]);
				pivot = i;
			}
		}
		if (Utils::eq (max, 0)){
			cerr<<"Singular matrix in lupdcmp."<<endl;
			exit (-1);
		}

		/* bring pivot element to position	*/
		swap(p[k], p[pivot]);
		for (i=0; i < n; i++){
			swap(a[k][i], a[pivot][i]);
		}

		for (i=k+1; i < n; i++) {
			a[i][k] /= a[k][k];
			for (j=k+1; j < n; j++)
				a[i][j] -= a[i][k] * a[k][j];
		}
	}
}

/*
 * the matrix a is an in-place lower/upper triangular matrix
 * the following macros split them into their constituents
 */

#define LOWER(a, i, j)		((i > j) ? a[i][j] : 0)
#define UPPER(a, i, j)		((i <= j) ? a[i][j] : 0)

/*
 * LU forward and backward substitution from the pseudocode given
 * in the CLRS book ('Introduction to Algorithms'). It solves ax = b
 * where, 'a' is an in-place lower/upper triangular matrix. The vector
 * 'x' carries the solution vector. 'p' is the permutation vector.
 */

void RCutils::lusolve(double **a, int n, int *p, double *b, double *x){
	int i, j;
	double *y = new double[n];
	double sum;

	/* forward substitution	- solves ly = pb	*/
	for (i=0; i < n; i++) {
		for (j=0, sum=0; j < i; j++)
			sum += y[j] * LOWER(a, i, j);
		y[i] = b[p[i]] - sum;
	}

	/* backward substitution - solves ux = y	*/
	for (i=n-1; i >= 0; i--) {
		for (j=i+1, sum=0; j < n; j++)
			sum += x[j] * UPPER(a, i, j);
		x[i] = (y[i] - sum) / UPPER(a, i, i);
	}

	delete[] y;
}


#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include "Graph.h"

namespace GraphLib
{
#ifdef GRAPH_ENABLE_DEBUG
	size_t Graph::instanceCount = 0;
#endif

	double Graph::dampingFactor = 0.85;

	Graph::Graph(bool directed)
	{
		this->directed = directed;
		adjMatrix = NULL;
		comment = NULL;
#ifdef GRAPH_ENABLE_DEBUG
		std::cout << "Creating an instance of Graph\n";
		instanceCount++;
#endif
	}

	Graph::~Graph()
	{
		for(std::size_t i=0; i<vertexes.size(); i++)
		{
			Node* n = vertexes[i];
			delete n;
		}

		delete adjMatrix;

		vertexes.clear();
		edges.clear();

#ifdef GRAPH_ENABLE_DEBUG
		std::cout << "Removing an instance of Graph\n";
		instanceCount--;
#endif
	}

	Graph::EdgeSet Graph::getEdges()
	{
		return edges;
	}

	/* number of nodes */
	std::size_t Graph::getNodesCount()const
	{
		return vertexes.size();
	}

	/* number of edges */
	std::size_t Graph::getEdgesCount()const
	{
		return edges.size();
	}

	size_t& Graph::createNode()
	{
		Node* n = new Node();
		addNode(n);
		return n->getIndex();
	}

	Node* Graph::nodeAt(std::size_t n)
	{
		assert(n<vertexes.size());
		return vertexes[n];
	}

	bool Graph::link(std::size_t n1, std::size_t n2)
	{
		assert((n1<vertexes.size()) && (n2<vertexes.size()));
		assert(n1!=n2);

		Node *node1 = vertexes[n1], *node2 = vertexes[n2];
		Edge *e = NULL;

		e = new Edge(node1, node2);

		if(!addEdge(e))
		{
			delete e;
			return false;
		}
		return true;
	}

	bool Graph::unlink(std::size_t n1, std::size_t n2)
	{
		assert(n1<vertexes.size());
		assert(n2<vertexes.size());
		assert(n1!=n2);

		Node *node1 = vertexes[n1], *node2 = vertexes[n2];

		Edge *e = NULL;

		e = new Edge(vertexes[n1], vertexes[n2]);

		EdgeSet::const_iterator it = edges.find(e);

		//std::cout << (it==edges.end()) << " X\n";

		if(it == edges.end())
			return false;

		edges.erase(e);
		delete e;

		return true;
	}

	bool Graph::areLinked(std::size_t n1, std::size_t n2)
	{
		assert(n1<vertexes.size());
		assert(n2<vertexes.size());
		assert(n1!=n2);

		Edge *e = NULL;

		e = new Edge(vertexes[n1], vertexes[n2]);

		EdgeSet::const_iterator it = edges.find(e);
		if(it == edges.end())
			return false;

		return true;

	}

	void Graph::removeNode(size_t ref)
	{
		assert(ref<vertexes.size());
		Node *n = vertexes[ref];
		removeNode(n);
	}

	/* Add a node to the graph, used internally */
	bool Graph::addNode(Node *node)
	{
		node->setIndex(vertexes.size());
		for( std::size_t i = 0; i < vertexes.size(); i++)
			if (node == vertexes[i])
				return false;

		vertexes.push_back(node);
		return true;
	}

	/* Removes a node from the graph, used internally  */
	void Graph::removeNode(Node *node)
	{
		std::size_t size = vertexes.size();
		std::size_t pos = node->getIndex();

		vertexes.erase(vertexes.begin()+pos);

		std::vector<Edge*> toRemove;

		for(auto e: edges)
		{
			if(e->contains(node))
				toRemove.push_back(e);
		}

		std::size_t s = toRemove.size();

		for(std::size_t i=0; i<s; i++)
		{
			Edge *e  = toRemove[i];
			edges.erase(e);
			delete e;
		}

		/* Update other nodes indexes */
		for(std::size_t i=pos; i<size-1; i++)
			vertexes[i]->setIndex(i);

		node->setIndex(-1);
		delete node;
	}

	bool Graph::addEdge(Edge *edge)
	{
		EdgeSet::const_iterator e = edges.find(edge);

		if(e != edges.end())
			return false;

		std::size_t start = edge->start->getIndex();
		std::size_t end   = edge->end->getIndex();

		if (start == -1 || end == -1)
			return false;

		edges.insert(edge);
		return true;
	}

	bool Graph::removeEdge(Edge *edge)
	{
		EdgeSet::const_iterator e = edges.find(edge);
		if(e == edges.end())
			return false;

		edges.erase(edge);
		return true;
	}

	void Graph::calculateAjdMatrix()
	{
		std::size_t SIZE = vertexes.size();

		adjMatrix = new MatrixXb(SIZE, SIZE);

		for(std::size_t i = 0; i < SIZE; i++)
		{
			for(std::size_t j = 0; j < SIZE; j++)
			{
				(*adjMatrix)(i, j) = false;
			}
		}

		for(const auto& e: edges)
		{
			std::size_t i1 = e->start->getIndex();
			std::size_t i2 = e->end->getIndex();
			(*adjMatrix)(i1, i2) = true;
			if(!this->directed)
				(*adjMatrix)(i2, i1) = true;
		}
	}

	void Graph::printAdjMatrix()
	{
		assert(adjMatrix != NULL);

		auto adjMat = *adjMatrix;

		std::size_t SIZE = vertexes.size();
		for( std::size_t i = 0; i < SIZE; i++)
		{
			for(std::size_t j = 0; j < SIZE; j++)
			{
				std::cout << "\t" << adjMat(i, j);
			}
			std::cout << "\n";
		}
	}

	bool Graph::get(std::size_t n1, std::size_t n2)
	{
		assert(adjMatrix != NULL);
		assert(n1<vertexes.size());
		assert(n2<vertexes.size());

		return (*adjMatrix)(n1, n2);
	}

	/* Initializes every node with a rank value 1/numberOfNodes */

	void Graph::calculatePageRank()
	{
		assert(adjMatrix != NULL);

		auto adjMat = *adjMatrix;

		/* initial value */
		double ival = 1.f/vertexes.size();

		/* This vector will contain the number of edges in which each node participates */
		std::vector <std::size_t> nodeEdgesCount;

		/* Step 0 (Optimisation) */
		for(std::size_t i = 0; i < vertexes.size(); i++)
		{
			nodeEdgesCount.push_back(0);

			for(std::size_t j = 0; j < vertexes.size(); j++)
			{
				if (adjMat(i, j)){
					nodeEdgesCount[i] ++;
				}
			}
		}

		/* Step 1 Initialize all nodes PR to 1/numberOfNodes */
		for(std::size_t i = 0; i < vertexes.size(); i++)
		{
			vertexes[i]->setRank(ival);
		}

		/* Step 2 Loop 100x to get a good page rank value. */
		for(std::size_t k = 0; k <100; k++)
		{
			/* Calculate PR of all nodes */
			for(std::size_t i = 0; i < vertexes.size(); i++)
			{

				double sum = 0;

				/* calculate the sum of RP(Ti)/L(Ti) where Ti links to the current node */
				for(std::size_t j = 0; j < vertexes.size(); j++)
				{
					/* node i must be targeted not the source */
					if (adjMat(j, i))
					{
						sum += vertexes[j]->getRank()/nodeEdgesCount[j];
					}
				}
				vertexes[i]->setRank( (1-dampingFactor) + dampingFactor * sum );
			}

		}
	}

	void Graph::calculatePageRankMatrix()
	{
		assert(adjMatrix != NULL);

		auto adjMat = *adjMatrix;

		std::vector<double> R;
		std::size_t SIZE = vertexes.size();
		Eigen::MatrixXd M(SIZE, SIZE);

		/* This vector will contain the number of edges in which each node participates */
		std::vector <std::size_t> nodeEdgesCount;

		/* Step 0 (Optimisation) */
		for(std::size_t i = 0; i < SIZE; i++)
		{
			nodeEdgesCount.push_back(0);

			for(std::size_t j = 0; j < SIZE; j++)
			{
				if (adjMat(i, j)){
					nodeEdgesCount[i] ++;
				}
			}
		}

		/* Creating M matrix */
		for(std::size_t i = 0; i < SIZE; i++)
		{
			for(std::size_t j = 0; j < SIZE; j++)
			{
				M(i, j) = 0;
				if(adjMat(j,i))
					M(i, j) = (1.0f/nodeEdgesCount[j]);
			}
		}

		/* Creating R matrix */
		for(std::size_t i = 0; i < SIZE; i++)
		{
			R.push_back(1./SIZE);
		}

		for(std::size_t k = 0; k < 100; k++)
		{
			std::vector<double> tmp(SIZE);

			for(std::size_t i = 0; i < SIZE; i++)
			{
				double value = 0;
				for(std::size_t j = 0; j < SIZE; j++)
				{
					value += M(i, j) * R[j];
				}
				tmp[i] = value *dampingFactor +(1.0f-dampingFactor);
			}
			R = tmp;
		}

		for(std::size_t i = 0; i < R.size(); i++)
		{
			vertexes[i]->setRank(R[i]);
		}

	}

	double Graph::getRank(std::size_t n)
	{
		assert(n<vertexes.size());
		double rank = vertexes[n]->getRank();
		/* To make sure this is called after calculating PR */
		assert(rank >= 0);
		return rank;
	}

	void Graph::printPageRank() const
	{
		double sum = 0;
		for( std::size_t i = 0; i < vertexes.size(); i++)
		{
			std::cout << "\tNode " << i << ":\t\t" << vertexes[i]->getRank() << "\n";
			sum += vertexes[i]->getRank();
		}
		std::cout << "\n\n\tTotal PR: " << sum << "\n\n";
	}

	void Graph::saveGML(wchar_t *url)
	{
		std::cout << "\n\tSaving GML to \""<< url << "\"...\n";
		std::ofstream out;
		out.open(url);
		out << "graph [";

		if(comment != NULL)
			out << "\tcomment \"" << comment << "\"\n";
		out << "\tdirected 0\n";

		for(std::size_t i = 0; i < vertexes.size(); i++)
		{
			out << "\tnode [";
			out << "\t\tid " << i << '\n';
			out << "\t\tlabel \"" << i << "\"\n";
			out << "\t]\n";
		}

		for(auto e: edges)
		{
			out << "\tedge [";
			out << "\t\tsource " << e->start->getIndex() << '\n';
			out << "\t\ttarget " << e->end->getIndex() << '\n';
			out << "\t]\n";
		}

		out << "]";

		out.close();
	}

	void Graph::saveDOT(wchar_t *url)
	{
		std::cout << "\n\tSaving DOT to \""<< url << "\"...\n";
		std::ofstream out;
		out.open(url);
		out << "graph g{\n";
		for(auto e: edges)
		{
			out << "\t" << e->start->getIndex() << " -- " << e->end->getIndex() << ";\n";
		}

		out << "}";

		out.close();
	}

	void Graph::saveCSV(wchar_t *url)
	{
		std::cout << "saving file\n";
		std::ofstream out;
		out.open(url);
		out << "X, ";
		for(std::size_t i = 0; i < vertexes.size(); i++)
		{
			out << "Node " << i << ',';
		}

		for(std::size_t i = 0; i < vertexes.size(); i++)
		{
			out << "\nNode " << i << ',';
			for(std::size_t j = 0; j < vertexes.size(); j++)
			{
				out << (*adjMatrix)(i, j) << ',';
			}
		}
		out << "\n\n";

		double sum = 0;
		for( std::size_t i = 0; i < vertexes.size(); i++)
		{
			out << "Node " << i << ',' << vertexes[i]->getRank() << '\n';
			sum += vertexes[i]->getRank();
		}

		out << "\n\nTotal PR," << sum << "\n\n";

		out.close();
	}

}


/*
 * Copyright (c) 2019, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. 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.
 *
 *    3. 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.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

///////////////////////////////////////////////////////////////////////////////
//
// Check whether or not a particular set of strategies is a local Nash
// equilibrium.
//
///////////////////////////////////////////////////////////////////////////////

#include <ilqgames/cost/player_cost.h>
#include <ilqgames/dynamics/multi_player_flat_system.h>
#include <ilqgames/dynamics/multi_player_integrable_system.h>
#include <ilqgames/utils/compute_strategy_costs.h>
#include <ilqgames/utils/operating_point.h>
#include <ilqgames/utils/quadratic_cost_approximation.h>
#include <ilqgames/utils/strategy.h>
#include <ilqgames/utils/types.h>

#include <glog/logging.h>
#include <Eigen/Dense>
#include <random>
#include <vector>

namespace ilqgames {

bool NumericalCheckLocalNashEquilibrium(
    const std::vector<PlayerCost>& player_costs,
    const std::vector<Strategy>& strategies,
    const OperatingPoint& operating_point,
    const MultiPlayerIntegrableSystem& dynamics, const VectorXf& x0,
    Time time_step, float max_perturbation, bool open_loop) {
  CHECK_EQ(strategies.size(), player_costs.size());
  CHECK_EQ(strategies.size(), dynamics.NumPlayers());
  CHECK_EQ(x0.size(), dynamics.XDim());

  const size_t num_time_steps = strategies[0].Ps.size();
  CHECK_EQ(num_time_steps, strategies[0].alphas.size());

  // Compute nominal equilibrium cost and be sure to use only 1-step Euler
  // integration.
  const bool was_integrating_using_euler =
      MultiPlayerIntegrableSystem::IntegrationUsesEuler();
  if (!was_integrating_using_euler)
    MultiPlayerIntegrableSystem::IntegrateUsingEuler();
  const std::vector<float> nominal_costs =
      ComputeStrategyCosts(player_costs, strategies, operating_point, dynamics,
                           x0, time_step, open_loop);

  // For each player, perturb strategies with Gaussian noise a bunch of times
  // and if cost decreases then return false.
  std::vector<Strategy> perturbed_strategies_lower(strategies);
  std::vector<Strategy> perturbed_strategies_upper(strategies);
  for (PlayerIndex ii = 0; ii < dynamics.NumPlayers(); ii++) {
    for (size_t kk = 0; kk < num_time_steps - 1; kk++) {
      VectorXf& alphak_lower = perturbed_strategies_lower[ii].alphas[kk];
      VectorXf& alphak_upper = perturbed_strategies_upper[ii].alphas[kk];

      for (size_t jj = 0; jj < alphak_lower.size(); jj++) {
        alphak_lower(jj) -= max_perturbation;
        alphak_upper(jj) += max_perturbation;

        // Compute new costs.
        const std::vector<float> perturbed_costs_lower = ComputeStrategyCosts(
            player_costs, perturbed_strategies_lower, operating_point, dynamics,
            x0, time_step, open_loop);
        const std::vector<float> perturbed_costs_upper = ComputeStrategyCosts(
            player_costs, perturbed_strategies_upper, operating_point, dynamics,
            x0, time_step, open_loop);

        // Check Nash condition.
        if (std::min(perturbed_costs_lower[ii], perturbed_costs_upper[ii]) <
            nominal_costs[ii]) {
          // std::printf(
          //     "player %hu, timestep %zu: nominal %f > perturbed %f\n ", ii,
          //     kk, nominal_costs[ii], std::min(perturbed_costs_lower[ii],
          //     perturbed_costs_lower[ii]));
          // std::cout << "nominal u: " <<
          // operating_point.us[kk][ii].transpose()
          //           << ", alpha original: "
          //           << strategies[ii].alphas[kk].transpose()
          //           << ", vs. perturbed " << alphak_lower.transpose()
          //           << std::endl;

          // Other users will likely want RK4 integration.
          if (!was_integrating_using_euler)
            MultiPlayerIntegrableSystem::IntegrateUsingRK4();
          return false;
        }

        // Reset this alpha.
        alphak_lower = strategies[ii].alphas[kk];
        alphak_upper = strategies[ii].alphas[kk];
      }
    }
  }

  // Other users will likely want RK4 integration.
  MultiPlayerIntegrableSystem::IntegrateUsingRK4();
  return true;
}

bool CheckSufficientLocalNashEquilibrium(
    const std::vector<PlayerCost>& player_costs,
    const OperatingPoint& operating_point, Time time_step,
    const std::shared_ptr<const MultiPlayerFlatSystem>& dynamics) {
  // Unpack number of players and number of time steps.
  const PlayerIndex num_players = player_costs.size();
  const size_t num_time_steps = operating_point.xs.size();
  const Dimension xdim = operating_point.xs[0].size();

  // Set up quadratic cost approximations.
  std::vector<QuadraticCostApproximation> quadraticization(
      num_players, QuadraticCostApproximation(xdim));

  // Quadraticize costs and check PSD conditions.
  for (size_t kk = 0; kk < num_time_steps; kk++) {
    const Time t = operating_point.t0 + static_cast<Time>(kk) * time_step;
    VectorXf x = operating_point.xs[kk];
    std::vector<VectorXf> us = operating_point.us[kk];

    if (dynamics.get()) {
      // Previous x, us are actually xi, vs.
      x = dynamics->FromLinearSystemState(x.eval());
      us = dynamics->LinearizingControls(x, std::vector<VectorXf>(us));
    }

    std::transform(player_costs.begin(), player_costs.end(),
                   quadraticization.begin(),
                   [&t, &x, &us](const PlayerCost& cost) {
                     return cost.Quadraticize(t, x, us);
                   });

    // Check if Q, Rs PSD.
    constexpr float kErrorMargin = 1e-4;
    for (const auto& q : quadraticization) {
      const auto eig_Q = Eigen::SelfAdjointEigenSolver<MatrixXf>(q.state.hess);
      if (eig_Q.eigenvalues().minCoeff() < -kErrorMargin) {
        // std::cout << "Failed at timestep " << kk << std::endl;
        // std::cout << "Q is: \n" << q.Q << std::endl;
        // std::cout << "Q evals are: " << eig_Q.eigenvalues().transpose()
        //           << std::endl;
        return false;
      }

      for (const auto& entry : q.control) {
        const auto eig_R =
            Eigen::SelfAdjointEigenSolver<MatrixXf>(entry.second.hess);
        if (eig_R.eigenvalues().minCoeff() < -kErrorMargin) return false;
      }
    }
  }

  return true;
}

}  // namespace ilqgames


#ifndef STAN_MCMC_HMC_HAMILTONIANS_DIAG_E_METRIC_HPP
#define STAN_MCMC_HMC_HAMILTONIANS_DIAG_E_METRIC_HPP

#include <stan/callbacks/logger.hpp>
#include <stan/mcmc/hmc/hamiltonians/base_hamiltonian.hpp>
#include <stan/mcmc/hmc/hamiltonians/diag_e_point.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>

namespace stan {
  namespace mcmc {

    // Euclidean manifold with diagonal metric
    template <class Model, class BaseRNG>
    class diag_e_metric: public base_hamiltonian<Model, diag_e_point, BaseRNG> {
    public:
      explicit diag_e_metric(const Model& model)
        : base_hamiltonian<Model, diag_e_point, BaseRNG>(model) {}

      double T(diag_e_point& z) {
        return 0.5 * z.p.dot( z.inv_e_metric_.cwiseProduct(z.p) );
      }

      double tau(diag_e_point& z) {
        return T(z);
      }

      double phi(diag_e_point& z) {
        return this->V(z);
      }

      double dG_dt(diag_e_point& z, callbacks::logger& logger) {
        return 2 * T(z) - z.q.dot(z.g);
      }

      Eigen::VectorXd dtau_dq(diag_e_point& z, callbacks::logger& logger) {
        return Eigen::VectorXd::Zero(this->model_.num_params_r());
      }

      Eigen::VectorXd dtau_dp(diag_e_point& z) {
        return z.inv_e_metric_.cwiseProduct(z.p);
      }

      Eigen::VectorXd dphi_dq(diag_e_point& z, callbacks::logger& logger) {
        return z.g;
      }

      void sample_p(diag_e_point& z, BaseRNG& rng) {
        boost::variate_generator<BaseRNG&, boost::normal_distribution<> >
          rand_diag_gaus(rng, boost::normal_distribution<>());

        for (int i = 0; i < z.p.size(); ++i)
          z.p(i) = rand_diag_gaus() / sqrt(z.inv_e_metric_(i));
      }
    };

  }  // mcmc
}  // stan
#endif