#ifndef STAN_MATH_TORSTEN_EVENT_SOLVER_HPP
#define STAN_MATH_TORSTEN_EVENT_SOLVER_HPP

#include <stan/math/prim/fun/multiply.hpp>
#include <stan/math/rev/fun/multiply.hpp>
#include <stan/math/torsten/dsolve/pk_vars.hpp>
#include <stan/math/torsten/dsolve/pmx_ode_integrator.hpp>
#include <stan/math/torsten/ev_manager.hpp>
#include <stan/math/torsten/model_factory.hpp>
#include <stan/math/torsten/event.hpp>
#include <stan/math/torsten/mpi/session.hpp>
#include <stan/math/torsten/mpi/my_worker.hpp>
#include <stan/math/torsten/mpi/precomputed_gradients.hpp>
#include <Eigen/Dense>
#include <vector>

namespace torsten{

  template<typename T_model, typename T_em>
  struct EventSolver;

  /*
   * the solver wrapper is aware of @c T_model so it build model
   * accordingly.
   */
  template<typename T_model, typename T_event_record,
           typename T0, typename T4, template<typename...> class theta_container,
           typename... event_ctrl_type,
           typename... ode_data_type>
  struct EventSolver<T_model,
                     EventsManager<T_event_record, NonEventParameters<T0, T4, theta_container,
                                                                      std::tuple<event_ctrl_type...>,
                                                                      ode_data_type...>> > {
    using EM = EventsManager<T_event_record, NonEventParameters<T0, T4, theta_container,
                                                                std::tuple<event_ctrl_type...>,
                                                                ode_data_type...>>;
    using T_param = NonEventParameters<T0, T4, theta_container,
                                       std::tuple<event_ctrl_type...>,
                                       ode_data_type...>;
    /*
     * Data used to fill the results when computation throws exception.
     */
    static constexpr double invalid_res_d = std::numeric_limits<double>::quiet_NaN();

    /*
     * calculate the number of parameters according to the
     * model type.
     *
     * @tparam T_er type of events record
     * @param id subject id among a population
     * @param rec events record for the population
     */
    static int system_size(int id, const T_event_record& rec,
                           const std::vector<theta_container<T4>>& theta) {
      const int ncmt = rec.ncmt;
      const int npar = theta[0].size();
      const int nvar = pmx_model_nvars<T_model,
                                       typename EM::T_time,
                                       typename EM::T_scalar,
                                       typename EM::T_rate>::nvars(ncmt, npar);
      const int nvar_ss = pmx_model_nvars_ss<T_model,
                                             typename EM::T_amt,
                                             typename EM::T_par_rate,
                                             typename EM::T_par_ii>::nvars(npar);
      return rec.has_ss_dosing(id) ? pk_nsys(ncmt, nvar, nvar_ss) : pk_nsys(ncmt, nvar);
    }

    /**
     * Every Torsten function calls Pred.
     *
     * Predicts the amount in each compartment for each event,
     * given the event schedule and the parameters of the model.
     *
     * Proceeds in two steps. First, computes all the events that
     * are not included in the original data set but during which
     * amounts in the system get updated. Secondly, predicts
     * the amounts in each compartment sequentially by going
     * through the augmented schedule of events. The returned pred
     * Matrix only contains the amounts in the event originally
     * specified by the users.
     *
     * This function is valid for all models. What changes from one
     * model to the other are the Pred1 and PredSS functions, which
     * calculate the amount at an individual event.
     *
     * @tparam T_em type the @c EventsManager
     * @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 (starts at 1)
     * @param[in] addl additional dosing at each event
     * @param[in] ss steady state approximation at each event
     * (0: no, 1: yes)
     * @param[in] pMatrix parameters at each event
     * @param[in] addParm additional parameters at each event
     * @parem[in] model basic info for ODE model and evolution operators
     * @param[in] SystemODE matrix describing linear ODE system that
     * defines compartment model. Used for matrix exponential solutions.
     * Included because it may get updated in modelParameters.
     * @return a matrix with predicted amount in each compartment
     * at each event.
     */
    template<typename integrator_type, typename... scalar_pars_type>
    void pred(int id,
              const T_event_record& events_rec,
              Eigen::Matrix<typename EM::T_scalar, -1, -1>& res,
              const integrator_type& integrator,
              const std::vector<theta_container<T4>>& theta,
              const std::vector<std::vector<event_ctrl_type>>&... event_ctrl,
              const std::vector<std::vector<ode_data_type>>&... ode_data,
              const scalar_pars_type... scalar_pars) {
      using Eigen::Matrix;
      using Eigen::Dynamic;
      using std::vector;
      using torsten::PKRec;

      int nCmt = EM::nCmt(events_rec);

      res.resize(nCmt, events_rec.num_event_times(id));
      PKRec<typename EM::T_scalar> init(nCmt);
      init.setZero();

      try {
        EM em(id, events_rec, theta, event_ctrl..., ode_data...);
        int ikeep = 0, iev = 0;

        while(ikeep < em.nKeep) {
          stepper(iev, init, em, integrator, scalar_pars...);
          if(em.events().keep(iev)) {
            res.col(ikeep) = init;
            ikeep++;
          }
          iev++;
        }
      } catch (const std::exception& e) {
        throw;
      }
    }

    /*
     * Step through a range of events.
     */
    template<typename integrator_type, typename... scalar_pars_type>
    void stepper(int i, PKRec<typename EM::T_scalar>& init, const EM& em,
                 const integrator_type& integrator,
                 const scalar_pars_type... scalar_pars) {
      auto& events = em.events();
      using scalar = typename EM::T_scalar;
      typename EM::T_time tprev = i == 0 ? events.time(0) : events.time(i-1);

      T_model pkmodel {model_factory<T_model, EM, scalar_pars_type...>::model(em, i,
                                                                              par_index_seq<ode_data_type...>{},
                                                                              scalar_pars...)};
      auto ev = em.event(i);
      ev(init, pkmodel, integrator);
    }

    /**
     * Solve an event from event manager and store the result in both
     * data format(solution & gradient) and var format.
     *
     * @param i id of the event to be solved
     * @param init var solution
     * @param sol_d data solution
     * @param em event manager
     * @param integrator numerical integrator for ODE
     * @param scalar_pars params needed to construst the PMX model,
     *        such as functor & dimension of ODE system
     */
    template<typename integrator_type, typename... scalar_pars_type>
    void stepper_solve(int i, torsten::PKRec<typename EM::T_scalar>& init,
                       torsten::PKRec<double>& sol_d,
                       const EM& em,
                       const integrator_type& integrator,
                       const scalar_pars_type... scalar_pars) {
      using std::vector;
      using stan::math::var;

      auto& events = em.events();

      typename EM::T_time tprev = i == 0 ? events.time(0) : events.time(i-1);

      T_model pkmodel {model_factory<T_model, EM, scalar_pars_type...>::model(em, i,
                                                                              par_index_seq<ode_data_type...>{},
                                                                              scalar_pars...)};
      auto ev = em.event(i);
      ev(sol_d, init, pkmodel, integrator, scalar_pars...);
    }

    template<typename integrator_type, typename... scalar_pars_type>
    /**
     * For MPI solutions, after each rank solves its corresponding
     * subjects, all ranks sync their results among each other.
     *
     * @param i id of the event to be synced
     * @param init var solution
     * @param sol_d data solution
     * @param em event manager
     * @param integrator numerical integrator for ODE
     * @param scalar_pars params needed to construst the PMX model,
     *        such as functor & dimension of ODE system
     */
    void stepper_sync(int i, torsten::PKRec<typename EM::T_scalar>& init,
                      torsten::PKRec<double>& sol_d,
                      const EM& em,
                      const integrator_type& integrator,
                      const scalar_pars_type... scalar_pars) {
      using std::vector;
      using stan::math::var;

      auto& events = em.events();

      typename EM::T_time tprev = i == 0 ? events.time(0) : events.time(i-1);

      if (events.is_reset(i)) {
        init.setZero();
      } else if (events.is_ss_dosing(i)) {  // steady state event
        T_model pkmodel {model_factory<T_model, EM, scalar_pars_type...>::model(em, i,
                                                                                par_index_seq<ode_data_type...>{},
                                                                                scalar_pars...)};
        auto curr_amt = em.fractioned_amt(i);
        vector<var> v_i(dsolve::pk_vars(curr_amt, events.rate(i), events.ii(i), pkmodel.par()));
        int nsys = torsten::pk_nsys(em.ncmt, v_i.size());
        if (events.ss(i) == 2)
          init += torsten::mpi::precomputed_gradients(sol_d.segment(0, nsys), v_i);  // steady state without reset
        else
          init = torsten::mpi::precomputed_gradients(sol_d.segment(0, nsys), v_i);  // steady state with reset (ss = 1)
      } else if (events.time(i) > tprev) {
        // auto curr_rates = stan::math::value_of(em.fractioned_rates(i));
        auto curr_rates = em.fractioned_rates(i);
          T_model pkmodel {model_factory<T_model, EM, scalar_pars_type...>::model(em, i,
                                                                                par_index_seq<ode_data_type...>{},
                                                                                scalar_pars...)};
          vector<var> v_i =
            pmx_model_vars<T_model>::vars(events.time(i), init, curr_rates, pkmodel.par());
          int nsys = torsten::pk_nsys(em.ncmt, v_i.size());
          init = torsten::mpi::precomputed_gradients(sol_d.segment(0, nsys), v_i);
      }

      if (events.is_bolus_dosing(i)) {
        init(events.cmt(i) - 1) += em.fractioned_amt(i);
      }
    }

#ifdef TORSTEN_MPI

    /*
     * MPI solution when the population
     * information passed in as ragged arrays. The overloading occurs
     * on res arg for results. Here the result is var.
     *
     * @param events_rec event record
     * @param res solution
     * @param integrator ODE integrator
     * @param theta PMX parameters passed into ODE function
     * @param event_ctrl optional PMX parameters: bioavailability & tlag
     * @param ode_data optional ODE parameters: real & integer data
     * @param scalar_pars params needed to construst the PMX model,
     *        such as functor & dimension of ODE system
     */
    template<typename integrator_type, typename... scalar_pars_type>
    void pred(const T_event_record& events_rec,
              Eigen::Matrix<stan::math::var, -1, -1>& res,
              const integrator_type& integrator,
              const std::vector<theta_container<T4>>& theta,
              const std::vector<std::vector<event_ctrl_type>>&... event_ctrl,
              const std::vector<std::vector<ode_data_type>>&... ode_data,
              const scalar_pars_type... scalar_pars) {
      using Eigen::Matrix;
      using Eigen::MatrixXd;
      using Eigen::VectorXd;
      using Eigen::Dynamic;
      using std::vector;
      using::stan::math::var;
      using torsten::PKRec;

      using scalar = typename EM::T_scalar;

      const int nCmt = EM::nCmt(events_rec);
      const int np = events_rec.num_subjects();
      bool is_invalid = false;
      std::ostringstream rank_fail_msg;

      const MPI_Comm& comm = torsten::mpi::Session::pmx_parm_comm().comm();
      int rank = torsten::mpi::Session::pmx_parm_comm().rank();
      int size = torsten::mpi::Session::pmx_parm_comm().size();

      std::vector<MPI_Request> req(np);
      vector<MatrixXd> res_d(np);

      res.resize(nCmt, events_rec.total_num_event_times);

      PKRec<scalar> init(nCmt);
      PKRec<double> pred1;
      for (int id = 0; id < np; ++id) {

        /* For every rank */

        const int nKeep = events_rec.num_event_times(id);

        int nev = EM::num_events(id, events_rec, theta, event_ctrl..., ode_data...);
        res_d[id].resize(system_size(id, events_rec, theta), nev);
        res_d[id].setConstant(0.0);

        int my_worker_id = torsten::mpi::my_worker(id, np, size);

        /* only solver rank */

        if (rank == my_worker_id) {
          if (is_invalid) {
            res_d[id].setConstant(invalid_res_d);
          } else {
            try {
              EM em(id, events_rec, theta, event_ctrl..., ode_data...);
              auto& events = em.events();
              assert(nev == events.size());
              assert(nKeep == em.nKeep);
              init.setZero();

              int ikeep = 0, iev = 0;
              while(ikeep < em.nKeep) {
                stepper_solve(iev, init, pred1, em, integrator, scalar_pars...);
                res_d[id].col(iev).segment(0, pred1.size()) = pred1;
                if(events.keep(iev)) {
                  res.col(EM::begin(id, events_rec) + ikeep) = init;
                  ikeep++;
                }
                iev++;
              }
            } catch (const std::exception& e) {
              is_invalid = true;
              res_d[id].setConstant(invalid_res_d);
              rank_fail_msg << "Rank " << rank << " failed to solve id " << id << ": " << e.what();
            }
          }
        }
        MPI_Ibcast(res_d[id].data(), res_d[id].size(), MPI_DOUBLE, my_worker_id, comm, &req[id]);
      }

      // int finished = 0;
      // int index;
      // int flag = 0;
      // for(int id = 0; id < np; ++id) {
      //   MPI_Wait(&req[id], MPI_STATUS_IGNORE);

      //   if (is_invalid) continue;
      //   if (std::isnan(res_d[id](0))) {
      //     assert(rank != torsten::mpi::my_worker(id, np, size));
      //     is_invalid = true;
      //     rank_fail_msg << "Rank " << rank << " received invalid data for id " << id;
      //   } else {
      //     EM em(id, events_rec);
      //     auto& events = em.events();
      //     PKRec<scalar> init(nCmt); init.setZero();
      //     PKRec<double> pred1 = VectorXd::Zero(res_d[id].rows());

      //     int ikeep = 0, iev = 0;
      //     while(ikeep < em.nKeep) {
      //       pred1 = res_d[id].col(iev);
      //       stepper_sync(iev, init, pred1, em, integrator, scalar_pars...);
      //       if(events.keep(iev)) {
      //         res.col(EM::begin(id, events_rec) + ikeep) = init;
      //         ikeep++;
      //       }
      //       iev++;
      //     }
      //   }
      // }

      int finished = 0;
      int index = 0;
      int flag = 0;
      while (finished < np) {
        MPI_Test(&req[index], &flag, MPI_STATUS_IGNORE);
        if (flag) {
          int id = index;
          index++;
          finished++;
          if (is_invalid) continue;
          if (std::isnan(res_d[id](0))) {
            // assert(rank != torsten::mpi::my_worker(id, np, size));
            is_invalid = true;
            rank_fail_msg << "Rank " << rank << " received invalid data for id " << id;
          } else {
            EM em(id, events_rec, theta, event_ctrl..., ode_data...);
            auto& events = em.events();
            PKRec<scalar> init(nCmt); init.setZero();
            PKRec<double> pred1 = VectorXd::Zero(res_d[id].rows());

            int ikeep = 0, iev = 0;
            while(ikeep < em.nKeep) {
              pred1 = res_d[id].col(iev);
              stepper_sync(iev, init, pred1, em, integrator, scalar_pars...);
              if(events.keep(iev)) {
                res.col(EM::begin(id, events_rec) + ikeep) = init;
                ikeep++;
              }
              iev++;
            }
          }
        }
      }

      MPI_Barrier(comm);

      if(is_invalid) {
        throw std::runtime_error(rank_fail_msg.str());
      }
    }

    /*
     * Data-only MPI solver that takes ragged arrays as input.
     */
    template<typename integrator_type, typename... scalar_pars_type>
    void pred(const T_event_record& events_rec, Eigen::MatrixXd& res,
              const integrator_type& integrator,
              const std::vector<theta_container<T4>>& theta,
              const std::vector<std::vector<event_ctrl_type>>&... event_ctrl,
              const std::vector<std::vector<ode_data_type>>&... ode_data,
              const scalar_pars_type... scalar_pars) {
      using Eigen::Matrix;
      using Eigen::MatrixXd;
      using Eigen::VectorXd;
      using Eigen::Dynamic;
      using std::vector;
      using::stan::math::var;
      using torsten::PKRec;

      using ER = NONMENEventsRecord<double, double, double, double>;
      using EM_d = EventsManager<ER, NonEventParameters<T0, T4, theta_container, std::tuple<event_ctrl_type...>, ode_data_type...>>;

      const int nCmt = EM_d::nCmt(events_rec);
      const int np = events_rec.num_subjects();
      bool is_invalid = false;
      std::ostringstream rank_fail_msg;

      const MPI_Comm& comm = torsten::mpi::Session::pmx_data_comm().comm();
      int rank = torsten::mpi::Session::pmx_data_comm().rank();
      int size = torsten::mpi::Session::pmx_data_comm().size();

      std::vector<MPI_Request> req(np);

      res.resize(nCmt, events_rec.total_num_event_times);

      PKRec<double> init(nCmt);
      for (int id = 0; id < np; ++id) {

        /* For every rank */
        int nKeep = events_rec.num_event_times(id);
        int my_worker_id = torsten::mpi::my_worker(id, np, size);
        int begin_id = EM_d::begin(id, events_rec) * nCmt;
        int size_id = nKeep * nCmt;

        /* only solver rank */
        if (rank == my_worker_id) {
          try {
            EM em(id, events_rec, theta, event_ctrl..., ode_data...);
            auto& events = em.events();
            init.setZero();
            int ikeep = 0, iev = 0;
            while(ikeep < em.nKeep) {
              stepper(iev, init, em, integrator, scalar_pars...);
              if(events.keep(iev)) {
                res.col(EM_d::begin(id, events_rec) + ikeep) = init;
                ikeep++;
              }
              iev++;
            }
          } catch (const std::exception& e) {
            is_invalid = true;
            res(begin_id) = invalid_res_d;
            rank_fail_msg << "Rank " << rank << " failed to solve id " << id << ": " << e.what();
          }
        }

        MPI_Ibcast(res.data() + begin_id, size_id, MPI_DOUBLE, my_worker_id, comm, &req[id]);
      }

      // make sure every rank throws in case any rank fails
      int finished = 0;
      int index = 0;
      int flag = 0;
      while (finished < np && size > 1) {
        MPI_Testany(np, req.data(), &index, &flag, MPI_STATUS_IGNORE);
        if (flag) {
          finished++;
          if(is_invalid) continue;
          int id = index;
          int begin_id = EM_d::begin(id, events_rec) * nCmt;
          if (std::isnan(res(begin_id))) {
            is_invalid = true;
            rank_fail_msg << "Rank " << rank << " received invalid data for id " << id;
          }
        }
      }

      MPI_Barrier(comm);

      if(is_invalid) {
        MPI_Barrier(comm);
        throw std::runtime_error(rank_fail_msg.str());
      }
    }
#else

    /*
     * For population input in the form of ragged arrays,
     * addional information of the size of each individual
     * is required to locate the data in a single array for population.
     */
    template<typename integrator_type, typename... scalar_pars_type> //NOLINT
    void pred(const T_event_record& events_rec,
              Eigen::Matrix<typename EM::T_scalar, -1, -1>& res,
              const integrator_type& integrator,
              const std::vector<theta_container<T4>>& theta,
              const std::vector<std::vector<event_ctrl_type>>&... event_ctrl,
              const std::vector<std::vector<ode_data_type>>&... ode_data,
              const scalar_pars_type... scalar_pars) {
      using ER = T_event_record;

      const int nCmt = EM::nCmt(events_rec);
      const int np = events_rec.num_subjects();

      res.resize(nCmt, events_rec.total_num_event_times);

      static bool has_warning = false;
      if (!has_warning) {
        std::cout << "Torsten Population PK solver " << "running sequentially" << "\n";
        has_warning = true;
      }

      for (int id = 0; id < np; ++id) {
        const int nKeep = events_rec.num_event_times(id);
        Eigen::Matrix<typename EM::T_scalar, -1, -1> res_id(nCmt, nKeep);
        pred(id, events_rec, res_id, integrator, theta,
             event_ctrl..., ode_data..., scalar_pars...);
        for (int j = 0; j < nKeep; ++j) {
          res.col(EM::begin(id, events_rec) + j) = res_id.col(j);
        }
      }
    }
#endif
  };

  template<typename T_model, typename T_event_record,
           typename T0, typename T4, template<typename...> class theta_container,
           typename... event_ctrl_type,
           typename... ode_data_type>
  constexpr double EventSolver<T_model,
                               EventsManager<T_event_record, NonEventParameters<T0, T4, theta_container,
                                                                                std::tuple<event_ctrl_type...>,
                                                                                ode_data_type...>> >::invalid_res_d;
}
#endif


#pragma once

#define BOOST_PARAMETER_MAX_ARITY 17
// Following defines move parts of SVG++ code to svgpp_parser_impl.cpp file
// reducing compiler memory requirements
#define SVGPP_USE_EXTERNAL_PATH_DATA_PARSER
#define SVGPP_USE_EXTERNAL_TRANSFORM_PARSER
#define SVGPP_USE_EXTERNAL_PRESERVE_ASPECT_RATIO_PARSER
#define SVGPP_USE_EXTERNAL_PAINT_PARSER
#define SVGPP_USE_EXTERNAL_MISC_PARSER
#define SVGPP_USE_EXTERNAL_COLOR_PARSER
#define SVGPP_USE_EXTERNAL_LENGTH_PARSER

#if defined(RENDERER_AGG)
#include <agg_color_rgba.h>
#include <agg_trans_affine.h>
#elif defined(RENDERER_GDIPLUS)
#include <windows.h>
#include <gdiplus.h>
#undef min
#undef max
#undef small
#include <vector>
#elif defined(RENDERER_SKIA)
#include <SkPaint.h>
#endif

#include <svgpp/factory/integer_color.hpp>
#include <svgpp/factory/unitless_length.hpp>
#if defined(SVG_PARSER_MSXML)
# include "parser_msxml.hpp"
#elif defined(SVG_PARSER_RAPIDXML_NS)
# include "parser_rapidxml_ns.hpp"
#elif defined(SVG_PARSER_LIBXML)
# include "parser_libxml.hpp"
#elif defined(SVG_PARSER_XERCES)
# include "parser_xerces.hpp"
#else
#error One of XML parsers must be set with SVG_PARSER_xxxx macro
#endif
#include <boost/function.hpp>
#include <boost/tuple/tuple.hpp>
#include <map>

#if defined(RENDERER_AGG)
typedef double number_t;
typedef agg::trans_affine transform_t;
typedef agg::rgba8 color_t;

inline color_t BlackColor() { return color_t(0, 0, 0); }
inline color_t TransparentBlackColor() { return color_t(0, 0, 0, 0); }
inline color_t TransparentWhiteColor() { return color_t(255, 255, 255, 0); }

struct color_factory_base_t
{
  typedef agg::rgba8 color_type;

  static color_type create(unsigned char r, unsigned char g, unsigned char b)
  {
    return color_type(r, g, b);
  }
};
typedef svgpp::factory::color::percentage_adapter<color_factory_base_t> color_factory_t;
#elif defined(RENDERER_GDIPLUS)
typedef Gdiplus::REAL number_t;
typedef Gdiplus::Matrix transform_t;
typedef Gdiplus::Color color_t;

inline color_t BlackColor() { return color_t(0, 0, 0); }
inline color_t TransparentBlackColor() { return color_t(0, 0, 0, 0); }
inline color_t TransparentWhiteColor() { return color_t(0, 255, 255, 255); }

struct color_factory_base_t
{
  typedef Gdiplus::Color color_type;

  static color_type create(unsigned char r, unsigned char g, unsigned char b)
  {
    return color_type(r, g, b);
  }
};
typedef svgpp::factory::color::percentage_adapter<color_factory_base_t> color_factory_t;
#elif defined(RENDERER_SKIA)
typedef SkScalar number_t;
typedef SkMatrix transform_t;
typedef SkColor color_t;

inline color_t BlackColor() { return SK_ColorBLACK; }
inline color_t TransparentBlackColor() { return SK_ColorTRANSPARENT; }
inline color_t TransparentWhiteColor() { return 0x00FFFFFF; }

struct color_policy
{
  static const SkColor preset_bits = 0xFF000000;
  static const SkColor r_offset = 16;
  static const SkColor g_offset = 8;
  static const SkColor b_offset = 0;
};

typedef svgpp::factory::color::integer<SkColor, color_policy> color_factory_t;
#else
#error One of renderers must be set with RENDERER_xxxx macro
#endif

typedef svgpp::factory::length::unitless<number_t, number_t> length_factory_t;
typedef boost::tuple<double, double, double, double> bounding_box_t;
typedef boost::function<bounding_box_t()> get_bounding_box_func_t;

#if defined(RENDERER_GDIPLUS)
inline void AssignMatrix(Gdiplus::Matrix & dest, Gdiplus::Matrix const & src)
{
  Gdiplus::REAL m[6];
  src.GetElements(m);
  dest.SetElements(m[0], m[1], m[2], m[3], m[4], m[5]);
}

class PathStorage
{
public:
  void path_move_to(number_t x, number_t y, svgpp::tag::coordinate::absolute const &)
  {
    path_points_.push_back(Gdiplus::PointF(x, y));
    path_types_.push_back(Gdiplus::PathPointTypeStart);
  }

  void path_line_to(number_t x, number_t y, svgpp::tag::coordinate::absolute const &)
  {
    path_points_.push_back(Gdiplus::PointF(x, y));
    path_types_.push_back(Gdiplus::PathPointTypeLine);
  }

  void path_cubic_bezier_to(
    number_t x1, number_t y1,
    number_t x2, number_t y2,
    number_t x, number_t y,
    svgpp::tag::coordinate::absolute const &)
  {
    // TODO:
    path_points_.push_back(Gdiplus::PointF(x1, y1));
    path_types_.push_back(Gdiplus::PathPointTypeBezier);
    path_points_.push_back(Gdiplus::PointF(x2, y2));
    path_types_.push_back(Gdiplus::PathPointTypeBezier);
    path_points_.push_back(Gdiplus::PointF(x, y));
    path_types_.push_back(Gdiplus::PathPointTypeBezier);
  }

  void path_close_subpath()
  {
    if (!path_types_.empty())
      path_types_.back() |= Gdiplus::PathPointTypeCloseSubpath;
  }

  void path_exit()
  {}

  std::vector<Gdiplus::PointF> path_points_;
  std::vector<BYTE> path_types_;
};
#endif


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

#include <map>
#include <cmath>
#include <algorithm> //find
#include <boost/math/special_functions/binomial.hpp>

#include <iostream>

using namespace std;

namespace OpenMS
{

  AScore::AScore()
  {
  }

  AScore::~AScore()
  {
  }

  PeptideHit AScore::compute(PeptideHit & hit, RichPeakSpectrum & real_spectrum, double fmt, Int number_of_phospho_sites)
  {
    String without_phospho_str(hit.getSequence().toString());
    size_t found = without_phospho_str.find("(Phospho)");
    while (found != string::npos)
    {
      without_phospho_str.erase(found, String("(Phospho)").size());
      found = without_phospho_str.find("(Phospho)");
    }
    AASequence without_phospho = AASequence::fromString(without_phospho_str);
    String umps = without_phospho.toUnmodifiedString(); // unmodified phosphostring
    Int number_of_STY = Int(std::count(umps.begin(), umps.end(), 'S') + std::count(umps.begin(), umps.end(), 'T') + std::count(umps.begin(), umps.end(), 'Y'));
    if (real_spectrum.empty() || number_of_phospho_sites < 1 || number_of_STY == 0)
    {
      return PeptideHit(-1, 0, hit.getCharge(), without_phospho);
    }
    TheoreticalSpectrumGenerator spectrum_generator;
    vector<RichPeakSpectrum> th_spectra;     //typedef MSSpectrum<RichPeak1D> RichPeakSpectrum;
    ///produce theoretical spectra
    if (number_of_STY < number_of_phospho_sites)
    {
      number_of_phospho_sites = number_of_STY;
    }
    //Int number_of_permutations = (Int)boost::math::binomial_coefficient<double>((double)number_of_STY, number_of_phospho_sites);
    vector<Size> tupel(computeTupel_(without_phospho));
    vector<vector<Size> > permutations(computePermutations_(tupel, number_of_phospho_sites));
    //cout<<"number of permutations "<<permutations.size();//<< " - " << number_of_permutations;
    th_spectra.resize(permutations.size());
    for (Size i = 0; i < permutations.size(); ++i)
    {
      AASequence temp(without_phospho);
      Size permu = 0;
      for (Size as = 0; as < temp.size(); ++as)
      {
        if (as == permutations[i][permu])
        {
          temp.setModification(as, "Phospho");
          ++permu;
        }
        if (permu == permutations[i].size())
          break;
      }
      spectrum_generator.addPeaks(th_spectra[i], temp, Residue::BIon, hit.getCharge());
      spectrum_generator.addPeaks(th_spectra[i], temp, Residue::YIon, hit.getCharge());
      th_spectra[i].setName(temp.toString());
    }
    ///produce theoretical spectra - END

    if (!real_spectrum.isSorted())
    {
      real_spectrum.sortByPosition();
    }
    vector<vector<double> > peptide_site_scores(th_spectra.size());
    vector<RichPeakSpectrum> windows_with_all_peak_depths;
    ///prepare peak depth for all windows in the actual spectrum
    {
      double biggest_window = real_spectrum[real_spectrum.size() - 1].getMZ();
      Size number_of_windows = 1;
      if (biggest_window > 100)
        number_of_windows = ceil(biggest_window / 100);
      windows_with_all_peak_depths.resize(number_of_windows);
      RichPeakSpectrum::Iterator begin_window = real_spectrum.MZBegin(0);
      RichPeakSpectrum::Iterator end_window = real_spectrum.MZBegin(100);
      for (Size current_window = 0; current_window < number_of_windows; ++current_window)
      {
        RichPeakSpectrum real_window;
        RichPeakSpectrum::Iterator runner = begin_window;
        while (runner <= end_window)
        {
          real_window.push_back(*runner);
          ++runner;
        }

        real_window.sortByIntensity(true);
        for (Size i = 0; i < 10; ++i)
        {
          if (i < real_window.size())
            windows_with_all_peak_depths[current_window].push_back(real_window[i]);
        }
        begin_window = end_window + 1;
        end_window = real_spectrum.MZBegin((current_window + 1) * 100);
      }
    }
    ///prepare peak depth for all windows in the actual spectrum - END
    UInt N;
    vector<vector<double> >::iterator side_scores = peptide_site_scores.begin();
    for (vector<RichPeakSpectrum>::iterator it = th_spectra.begin(); it < th_spectra.end(); ++it, ++side_scores)    //each theoretical spectrum
    {
      N = UInt(it->size());       //real or theo!!
      UInt i = 1;
      side_scores->resize(10);
      while (i <= 10)
      {
        //Auslagern
        UInt n = 0;
        for (Size depth = 0; depth <  windows_with_all_peak_depths.size(); ++depth)         //each 100 m/z window
        {
          n += numberOfMatchedIons_(*it, windows_with_all_peak_depths[depth], i, fmt);
        }
        //auslagern_end
        double p = (double)i / 100;
        double cum_socre = computeCumulativeScore(N, n, p);
        (*side_scores)[i - 1] = (-10 * log10(cum_socre));    //computeCumulativeScore(N,n,p);
        ++i;
      }
    }
    vector<ProbablePhosphoSites> highest_peptides;
    PeptideHit phospho;
    if (permutations[0].size() < permutations.size())
    {
      computeHighestPeptides(peptide_site_scores, highest_peptides, permutations);
      phospho.setScore(peptideScore_(peptide_site_scores[highest_peptides[0].seq_1]));
      //phospho.setSequence(AASequence(th_spectra[highest_peptides[0].seq_1].getName()));
    }
    {
      multimap<double, Size> ranking;
      for (Size it = 0; it < peptide_site_scores.size(); ++it)
      {
        double current_score = peptideScore_(peptide_site_scores[it]);
        ranking.insert(pair<double, Size>(current_score, it));
      }
      phospho.setScore(ranking.rbegin()->first);
      phospho.setSequence(AASequence::fromString(th_spectra[ranking.rbegin()->second].getName()));
    }
    phospho.setCharge(hit.getCharge());
    phospho.setMetaValue("Search_engine_sequence", hit.getSequence().toString());
    ///Calculate AScore
    //Int add_ascores = (Int)highest_peptides.size()/permutations[0].size();
    //Int count_ascores = 0;
    //vector<ProbablePhosphoSites>::iterator winner = highest_peptides.begin();
    //vector<ProbablePhosphoSites>::iterator actual = winner;
    //double ascore_addition = 0.0;
    //double highest_addition = 0.0;
    Int rank = 1;
    for (vector<ProbablePhosphoSites>::iterator hp = highest_peptides.begin(); hp < highest_peptides.end(); ++hp)
    {
      vector<RichPeakSpectrum> site_determining_ions;
      compute_site_determining_ions(th_spectra, *hp, hit.getCharge(), site_determining_ions);
      N = UInt(site_determining_ions[0].size());       // all possiblities have the same number
      double p = (double)hp->peak_depth / 100;
      Int n = 0;
      //Auslagern
      for (Size depth = 0; depth <  windows_with_all_peak_depths.size(); ++depth)       //each 100 m/z window
      {
        n += numberOfMatchedIons_(site_determining_ions[0], windows_with_all_peak_depths[depth], (Size)p, fmt);
      }
      //auslagern_end
      double P_first = computeCumulativeScore(N, n, p);
      Int n2 = 0;
      //Auslagern
      for (Size depth = 0; depth <  windows_with_all_peak_depths.size(); ++depth)       //each 100 m/z window
      {
        n2 += numberOfMatchedIons_(site_determining_ions[1], windows_with_all_peak_depths[depth], (Size)p, fmt);
      }
      //auslagern_end
      double P_second = computeCumulativeScore(N, n2, p);
      double score_first = -10 * log10(P_first);
      double score_second = -10 * log10(P_second);
      double AScore_first = score_first - score_second;
      phospho.setMetaValue("AScore_" + String(rank), AScore_first);
      ++rank;
      /*    hp->AScore = AScore_first;
          ++count_ascores;
          ascore_addition += AScore_first;
          if(count_ascores == add_ascores)
          {

              count_ascores = 0;
              if(ascore_addition > highest_addition)
              {
                  highest_addition = ascore_addition;
                  winner = actual;
              }
              actual = hp;
              ascore_addition = 0;
          }*/
    }
    return phospho;
  }

  double AScore::computeCumulativeScore(UInt N, UInt n, double p)
  {
    if (n > N)
    {
      return -1.0;
    }
    double score = 0.0;
    for (UInt k = n; k <= N; ++k)
    {
      double coeff = boost::math::binomial_coefficient<double>((double)N, k);
      double pow1 = pow((double)p, (int)k);
      double pow2 = pow(double(1 - p), double(N - k));
      score += coeff * pow1 * pow2;
    }
    if (score == 0.0)
    {
      return 1.0;
    }
    return score;
  }

  void AScore::computeHighestPeptides(std::vector<std::vector<double> > & peptide_site_scores, std::vector<ProbablePhosphoSites> & sites, vector<vector<Size> > & permutations)
  {
    sites.clear();
    sites.resize(permutations[0].size());
    multimap<double, Size> ranking;
    for (Size it = 0; it < peptide_site_scores.size(); ++it)
    {
      double current_score = peptideScore_(peptide_site_scores[it]);
      ranking.insert(pair<double, Size>(current_score, it));
    }
    vector<Size> & hps = permutations[ranking.rbegin()->second /*it->second*/];       //highest peptide score
    for (Size i = 0; i < hps.size(); ++i)
    {
      multimap<double, Size>::reverse_iterator rev = ranking.rbegin();
      sites[i].first = hps[i];
      sites[i].seq_1 = rev->second;          //it->second;
      bool peptide_not_found = true;
      do
      {
        ++rev;
        for (Size j = 0; j < hps.size(); ++j)
        {
          if (j == i)
          {
            if (find(permutations[rev->second].begin(), permutations[rev->second].end(), hps[j]) != permutations[rev->second].end())
            {
              peptide_not_found = true;
              break;
            }
            else
            {
              peptide_not_found = false;
            }
          }
          else if (find(permutations[rev->second].begin(), permutations[rev->second].end(), hps[j]) == permutations[rev->second].end())
          {
            peptide_not_found = true;
            break;
          }
          else
          {
            peptide_not_found = false;
          }
        }
      }
      while (peptide_not_found);
      sites[i].seq_2 = rev->second;
      for (Size j = 0; j < permutations[sites[i].seq_2].size(); ++j)
      {
        if (find(permutations[sites[i].seq_1].begin(), permutations[sites[i].seq_1].end(), permutations[sites[i].seq_2][j]) == permutations[sites[i].seq_1].end())
        {
          sites[i].second = permutations[sites[i].seq_2][j];
          break;
        }
      }
    }
    for (Size i = 0; i < sites.size(); ++i)
    {
      double current_peak_depth = 0.0;
      sites[i].peak_depth = 1;
      vector<double>::iterator first_it = peptide_site_scores[sites[i].seq_1].begin();
      Size depth = 1;
      for (vector<double>::iterator second_it = peptide_site_scores[sites[i].seq_2].begin(); second_it < peptide_site_scores[sites[i].seq_2].end(); ++second_it, ++first_it)
      {
        if ((*first_it - *second_it) > current_peak_depth)
        {
          current_peak_depth = *first_it - *second_it;
          sites[i].peak_depth = depth;
        }
        ++depth;
      }
    }
  }

  void AScore::compute_site_determining_ions(vector<RichPeakSpectrum> & th_spectra, ProbablePhosphoSites & candidates, Int charge, vector<RichPeakSpectrum> & site_determining_ions)
  {
    site_determining_ions.clear();
    site_determining_ions.resize(2);
    TheoreticalSpectrumGenerator spectrum_generator;
    AASequence pref, suf, pref_with_phospho_first, pref_with_phospho_second, suf_with_phospho_first, suf_with_phospho_second;
    RichPeakSpectrum prefix, suffix, prefix_with_phospho_first, prefix_with_phospho_second, suffix_with_phospho_second, suffix_with_phospho_first;
    AASequence first(AASequence::fromString(th_spectra[candidates.seq_1].getName()));
    AASequence second(AASequence::fromString(th_spectra[candidates.seq_2].getName()));
    if (candidates.first < candidates.second)
    {
      pref = AASequence::fromString(first.getPrefix(candidates.first + 1).toString());
      suf = AASequence::fromString(second.getSuffix(second.size() - candidates.second - 1).toString());
      pref_with_phospho_first = AASequence::fromString(first.getPrefix(candidates.second + 1).toString());
      pref_with_phospho_second = AASequence::fromString(second.getPrefix(candidates.second + 1).toString());
      suf_with_phospho_first = AASequence::fromString(first.getSuffix(first.size() - candidates.first).toString());
      suf_with_phospho_second = AASequence::fromString(second.getSuffix(second.size() - candidates.first).toString());
    }
    else
    {
      pref = AASequence::fromString(second.getPrefix(candidates.second + 1).toString());
      suf = AASequence::fromString(first.getSuffix(first.size() - candidates.first - 1).toString());
      pref_with_phospho_first = AASequence::fromString(first.getPrefix(candidates.first + 1).toString());
      pref_with_phospho_second = AASequence::fromString(second.getPrefix(candidates.first + 1).toString());
      suf_with_phospho_first = AASequence::fromString(first.getSuffix(first.size() - candidates.second).toString());
      suf_with_phospho_second = AASequence::fromString(second.getSuffix(first.size() - candidates.second).toString());
    }
    spectrum_generator.addPeaks(prefix, pref, Residue::BIon, charge);
    spectrum_generator.addPeaks(suffix, suf, Residue::YIon, charge);
    spectrum_generator.addPeaks(prefix_with_phospho_first, pref_with_phospho_first, Residue::BIon, charge);
    spectrum_generator.addPeaks(prefix_with_phospho_second, pref_with_phospho_second, Residue::BIon, charge);
    spectrum_generator.addPeaks(suffix_with_phospho_first, suf_with_phospho_first, Residue::YIon, charge);
    spectrum_generator.addPeaks(suffix_with_phospho_second, suf_with_phospho_second, Residue::YIon, charge);
    if (!prefix.empty())
    {
      for (RichPeakSpectrum::iterator it = prefix_with_phospho_first.begin(); it < prefix_with_phospho_first.end(); ++it)
      {
        if (it->getMZ() > prefix[prefix.size() - 1].getMZ())
          site_determining_ions[0].push_back(*it);
      }
      for (RichPeakSpectrum::iterator it = prefix_with_phospho_second.begin(); it < prefix_with_phospho_second.end(); ++it)
      {
        if (it->getMZ() > prefix[prefix.size() - 1].getMZ())
          site_determining_ions[1].push_back(*it);
      }
    }
    else
    {
      for (RichPeakSpectrum::iterator it = prefix_with_phospho_first.begin(); it < prefix_with_phospho_first.end(); ++it)
      {
        site_determining_ions[0].push_back(*it);
      }
      for (RichPeakSpectrum::iterator it = prefix_with_phospho_second.begin(); it < prefix_with_phospho_second.end(); ++it)
      {
        site_determining_ions[1].push_back(*it);
      }
    }
    if (!suffix.empty())
    {
      for (RichPeakSpectrum::iterator it = suffix_with_phospho_first.begin(); it < suffix_with_phospho_first.end(); ++it)
      {
        if (it->getMZ() > suffix[suffix.size() - 1].getMZ())
          site_determining_ions[0].push_back(*it);
      }
      for (RichPeakSpectrum::iterator it = suffix_with_phospho_second.begin(); it < suffix_with_phospho_second.end(); ++it)
      {
        if (it->getMZ() > suffix[suffix.size() - 1].getMZ())
          site_determining_ions[1].push_back(*it);
      }
    }
    else
    {
      RichPeakSpectrum::iterator it1 = suffix_with_phospho_first.begin();
      RichPeakSpectrum::iterator it2 = suffix_with_phospho_second.begin();
      if (!suf.empty())
      {
        ++it1;
        ++it2;
      }
      for (; it1 < suffix_with_phospho_first.end(); ++it1)
      {
        site_determining_ions[0].push_back(*it1);
      }
      for (; it2 < suffix_with_phospho_second.end(); ++it2)
      {
        site_determining_ions[1].push_back(*it2);
      }
    }
    site_determining_ions[0].sortByPosition();
    site_determining_ions[1].sortByPosition();
  }

  Int AScore::numberOfMatchedIons_(const RichPeakSpectrum & th, const RichPeakSpectrum & windows, Size depth, double fmt)
  {
    Int n = 0;
    for (Size i = 0; i < windows.size() && i <= depth; ++i)
    {
      Size nearest_peak = th.findNearest(windows[i].getMZ());
      if (nearest_peak < th.size() && fabs(th[nearest_peak].getMZ() - windows[i].getMZ()) < fmt)
        ++n;
    }
    return n;
  }

  double AScore::peptideScore_(std::vector<double> & scores)
  {
    return (scores[0] * 0.5
            + scores[1] * 0.75
            + scores[2]           //*1
            + scores[3]           //*1
            + scores[4]           //*1
            + scores[5]           //*1
            + scores[6] * 0.75
            + scores[7] * 0.5
            + scores[8] * 0.25
            + scores[9] * 0.25)
           / 10;
  }

  vector<Size> AScore::computeTupel_(AASequence & without_phospho)
  {
    vector<Size> tupel;
    String unmodified = without_phospho.toUnmodifiedString();
    for (Size i = 0; i < unmodified.size(); ++i)
    {
      if (unmodified[i] == 'Y' || unmodified[i] == 'T' || unmodified[i] == 'S')
      {
        tupel.push_back(i);
      }
    }
    return tupel;
  }

  vector<vector<Size> > AScore::computePermutations_(vector<Size> tupel, Int number_of_phospho_sites)
  {
    if (number_of_phospho_sites == 1)
    {
      vector<vector<Size>  > permutations;
      for (Size i = 0; i < tupel.size(); ++i)
      {
        vector<Size> temp;
        temp.push_back(tupel[i]);
        permutations.push_back(temp);
      }
      return permutations;
    }
    else if (tupel.size() == (Size)number_of_phospho_sites)
    {
      vector<vector<Size> > permutations;
      permutations.push_back(tupel);
      return permutations;
    }
    else
    {
      vector<vector<Size> > permutations;
      vector<Size> head;
      vector<vector<Size> > tail;
      head.push_back(tupel[0]);
      vector<Size> tupel_left(++tupel.begin(), tupel.end());
      Int tail_phospho_sites = number_of_phospho_sites - 1;
      tail = computePermutations_(tupel_left, tail_phospho_sites);
      for (vector<vector<Size> >::iterator it = tail.begin(); it < tail.end(); ++it)
      {
        vector<Size> temp(head);
        temp.insert(temp.end(), it->begin(), it->end());
        permutations.push_back(temp);
      }
      vector<vector<Size> > other_possibilities(computePermutations_(tupel_left, number_of_phospho_sites));
      permutations.insert(permutations.end(), other_possibilities.begin(), other_possibilities.end());
      return permutations;
    }
  }

} // namespace OpenMS


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_PACK_NUMERIC_HPP
#define CRYPTO3_PACK_NUMERIC_HPP

#include <boost/assert.hpp>
#include <boost/static_assert.hpp>

#include <boost/multiprecision/cpp_int.hpp>

namespace boost {
    namespace crypto3 {
        namespace detail {
            using namespace boost::multiprecision;

            template<typename Endianness, int InValueBits, int OutValueBits, typename InputIterator, typename Backend,
                     expression_template_option ExpressionTemplates>
            inline void pack(InputIterator first, InputIterator last, number<Backend, ExpressionTemplates> &out) {
                import_bits(out, first, last);
                BOOST_ASSERT(msb(out) == OutValueBits);
            }

            template<typename Endianness, int OutValueBits, typename InputType, typename Backend,
                     expression_template_option ExpressionTemplates>
            inline void pack(const InputType &in, number<Backend, ExpressionTemplates> &out) {
                import_bits(out, in.begin(), in.end());
                BOOST_ASSERT(msb(out) == OutValueBits);
            }

            template<typename Endianness, int OutValueBits, typename OutputType, typename Backend,
                     expression_template_option ExpressionTemplates>
            inline void pack(const number<Backend, ExpressionTemplates> &in, OutputType &out) {
                export_bits(in, out);
                BOOST_ASSERT(msb(out) == OutValueBits);
            }
        }    // namespace detail
    }        // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_BLOCK_PACK_HPP


#include <MeshFEM/MeshIO.hh>
#include <MeshFEM/MSHFieldWriter.hh>
#include <MeshFEM/LinearElasticity.hh>
#include <MeshFEM/Materials.hh>
#include <MeshFEM/PeriodicHomogenization.hh>
#include <MeshFEM/OrthotropicHomogenization.hh>
#include <MeshFEM/GlobalBenchmark.hh>
#include <MeshFEM/TensorProjection.hh>
#include <vector>

#include <queue>
#include <iostream>
#include <iomanip>
#include <memory>
#include <cmath>

#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

namespace po = boost::program_options;
using namespace std;
using namespace PeriodicHomogenization;

[[ noreturn ]] void usage(int exitVal, const po::options_description &visible_opts) {
    cout << "Usage: OptimizeLinkage_cli [options] mesh" << endl;
    cout << visible_opts << endl;
    exit(exitVal);
}

po::variables_map parseCmdLine(int argc, const char *argv[])
{
    po::options_description hidden_opts("Hidden Arguments");
    hidden_opts.add_options()
        ("mesh",       po::value<string>(),                     "input mesh")
        ;
    po::positional_options_description p;
    p.add("mesh",                1);

    po::options_description visible_opts;
    visible_opts.add_options()("help", "Produce this help message")
        ("material,m", po::value<string>(),                 "base material")
        ("degree,d",   po::value<int>()->default_value(2), "degree of finite elements")
        ("fieldOutput,o",po::value<string>(),              "Dump fluctuation stress and strain fields to specified msh file")
        ("centerFluctuationDisplacements,c",               "Shift each fluctuation displacement so that it averages to zero")
        ("fullDegreeFieldOutput,D",                        "Output full-degree nodal fields (don't do piecewise linear subsample)")
        ("distanceToIsotropy",                             "Output the distance to the closest isotropic tensor")
        ("distanceToMaterial", po::value<string>(),        "Output the distance to a particular material")
        ("ignorePeriodicMismatch",                         "Ignore mismatched nodes on the periodic faces (useful for voxel grids)")
        ("manualPeriodicVertices", po::value<string>(),    "Manually specify identified periodic vertices using a hacky file format (see PeriodicCondition constructor)")
        ("orthotropicCell,O",                              "Analyze the orthotropic symmetry base cell only")
        ;

    po::options_description cli_opts;
    cli_opts.add(visible_opts).add(hidden_opts);

    po::variables_map vm;
    try {
        po::store(po::command_line_parser(argc, argv).
                  options(cli_opts).positional(p).run(), vm);
        po::notify(vm);
    }
    catch (std::exception &e) {
        cout << "Error: " << e.what() << endl << endl;
        usage(1, visible_opts);
    }

    bool fail = false;
    if (vm.count("mesh") == 0) {
        cout << "Error: must specify input mesh" << endl;
        fail = true;
    }

    int d = vm["degree"].as<int>();
    if (d < 1 || d > 2) {
        cout << "Error: FEM Degree must be 1 or 2" << endl;
        fail = true;
    }

    if (fail || vm.count("help"))
        usage(fail, visible_opts);

    return vm;
}

// Sum the values appearing on periodically-identified vertices.
// First sum onto the reduced DoFs, then redistribute.
template<class Sim, class VField>
VField sumIdentifiedValues(const Sim &sim, VField v) {
    const auto &mesh = sim.mesh();
    if (v.domainSize() != mesh.numNodes())
        throw std::runtime_error("Expected per-node vector field");

    VField dofField(sim.numDoFs());
    dofField.clear();
    for (size_t i = 0; i < mesh.numNodes(); ++i)
        dofField(sim.DoF(i)) +=  v(i);
    for (size_t i = 0; i < mesh.numNodes(); ++i)
        v(i) = dofField(sim.DoF(i));

    return v;
}

template<size_t _N>
using HMG = LinearElasticity::HomogenousMaterialGetter<Materials::Constant>::template Getter<_N>;

template<size_t _N, size_t _FEMDegree>
void execute(const po::variables_map &args,
             const vector<MeshIO::IOVertex> &inVertices,
             const vector<MeshIO::IOElement> &inElements) {
    auto &mat = HMG<_N>::material;
    if (args.count("material")) mat.setFromFile(args["material"].as<string>());

    typedef LinearElasticity::Mesh<_N, _FEMDegree, HMG> Mesh;
    typedef LinearElasticity::Simulator<Mesh> Simulator;
    Simulator sim(inElements, inVertices);
    typedef typename Simulator::ETensor ETensor;
    typedef typename Simulator::VField  VField;

    BENCHMARK_START_TIMER_SECTION("Cell Problems");
    std::vector<VField> w_ij;
    std::unique_ptr<PeriodicCondition<_N>> pc;
    ETensor Eh;
    for (size_t it = 0; it < 20; ++it) {
        if (args.count("manualPeriodicVertices"))
            pc = Future::make_unique<PeriodicCondition<_N>>(sim.mesh(), args["manualPeriodicVertices"].as<string>());
        if (args.count("orthotropicCell") == 0) {
            solveCellProblems(w_ij, sim, 1e-7, args.count("ignorePeriodicMismatch"), std::move(pc));
        }
        else {
            auto systems = PeriodicHomogenization::Orthotropic::solveCellProblems(w_ij, sim, 1e-7);
            cout << systems.size() << endl;
        }

        BENCHMARK_STOP_TIMER_SECTION("Cell Problems");

        BENCHMARK_START_TIMER_SECTION("Compute Tensor");
        // ETensor Eh = homogenizedElasticityTensor(w_ij, sim);
        if (args.count("orthotropicCell") == 0)   Eh = homogenizedElasticityTensorDisplacementForm(w_ij, sim);
        else Eh = PeriodicHomogenization::Orthotropic::homogenizedElasticityTensorDisplacementForm(w_ij, sim);
        BENCHMARK_STOP_TIMER_SECTION("Compute Tensor");

        cout << setprecision(16);
        cout << "Homogenized elasticity tensor:" << endl;
        cout << Eh << endl << endl;

        auto eigs = Eh.computeEigenstrains();
        // Make all eigenstrains positive
        if (eigs.strains(0, 0) < 0) eigs.strains.col(0) *= -1;
        if (eigs.strains(0, 1) < 0) eigs.strains.col(1) *= -1;
        if (eigs.strains(0, 2) < 0) eigs.strains.col(2) *= -1;

        cout << "Minimum Eh eigenvalue " << eigs.lambdas[0] << " for eigenstrain: "
             << eigs.strains.col(0).transpose() << endl;

        SymmetricMatrixValue<Real, _N> minEigenstrain(eigs.strains.col(0));
        SymmetricMatrixValue<Real, _N> midEigenstrain(eigs.strains.col(1));
        SymmetricMatrixValue<Real, _N> maxEigenstrain(eigs.strains.col(2));

        ETensor Eh_pinv = Eh.pseudoinverse();
        // cout << "Eh : Eh_pinv: " << endl << Eh.doubleContract(Eh_pinv) << endl << endl;
        // cout << "Eh : Eh_pinv : Eh " << endl << Eh.doubleContract(Eh_pinv).doubleContract(Eh) << endl << endl;

        // Compute discrete shape derivative of minimum eigenvalue and
        // one of the eigenstrain components
        // (to steer away from the uniform expansion mode).
        auto dEh = homogenizedElasticityTensorDiscreteDifferential(w_ij, sim);
        // Work with vector fields instead of OneForms; we probably can't compute a
        // proper Riesz representative on non-manifold meshes anyway, so don't
        // worry about distinction between one-forms and vector fields.
        VField dMinEigenvalue = sumIdentifiedValues(sim, compose([&](const ETensor &E) { return minEigenstrain.doubleContract(E.doubleContract(minEigenstrain)); }, dEh).asVectorField());
        VField dMidEigenvalue = sumIdentifiedValues(sim, compose([&](const ETensor &E) { return midEigenstrain.doubleContract(E.doubleContract(midEigenstrain)); }, dEh).asVectorField());
        VField dMaxEigenvalue = sumIdentifiedValues(sim, compose([&](const ETensor &E) { return maxEigenstrain.doubleContract(E.doubleContract(maxEigenstrain)); }, dEh).asVectorField());

        VField dMinEigenstrainC0 = sumIdentifiedValues(sim, compose([&](const ETensor &E) { return -Eh_pinv.doubleContract(E.doubleContract(minEigenstrain))(0, 0); }, dEh).asVectorField());
        VField dMinEigenstrainC1 = sumIdentifiedValues(sim, compose([&](const ETensor &E) { return -Eh_pinv.doubleContract(E.doubleContract(minEigenstrain))(1, 1); }, dEh).asVectorField());
        VField dMinEigenstrainC2 = sumIdentifiedValues(sim, compose([&](const ETensor &E) { return -Eh_pinv.doubleContract(E.doubleContract(minEigenstrain))(0, 1); }, dEh).asVectorField());

        bool linearSubsampleFields = args.count("fullDegreeFieldOutput") == 0;
        MSHFieldWriter writer("vertical_linkage_it" + std::to_string(it) + ".msh", sim.mesh(), linearSubsampleFields);
        writer.addField("dE00", sumIdentifiedValues(sim, compose([&](const ETensor &E) { return E.D(0,0); }, dEh).asVectorField()), DomainType::PER_NODE);
        writer.addField("dE01", sumIdentifiedValues(sim, compose([&](const ETensor &E) { return E.D(0,1); }, dEh).asVectorField()), DomainType::PER_NODE);
        writer.addField("dE11", sumIdentifiedValues(sim, compose([&](const ETensor &E) { return E.D(1,1); }, dEh).asVectorField()), DomainType::PER_NODE);
        writer.addField("dE22", sumIdentifiedValues(sim, compose([&](const ETensor &E) { return E.D(2,2); }, dEh).asVectorField()), DomainType::PER_NODE);

        writer.addField("dMinEigenvalue", dMinEigenvalue, DomainType::PER_NODE);
        writer.addField("dMidEigenvalue", dMidEigenvalue, DomainType::PER_NODE);
        writer.addField("dMaxEigenvalue", dMaxEigenvalue, DomainType::PER_NODE);

        writer.addField("dMinEigenstrainC0", dMinEigenstrainC0, DomainType::PER_NODE);
        writer.addField("dMinEigenstrainC1", dMinEigenstrainC1, DomainType::PER_NODE);
        writer.addField("dMinEigenstrainC2", dMinEigenstrainC2, DomainType::PER_NODE);

        VField descentStep = dMinEigenstrainC1;
        descentStep.maxColumnNormalize();
        descentStep *= 0.01;
        std::vector<MeshIO::IOVertex> pts(sim.mesh().numVertices());
        for (auto v : sim.mesh().vertices()) {
            pts[v.index()] = v.node()->p;
            pts[v.index()].point += padTo3D(PointND<_N>(descentStep(v.index())));
        }
        sim.updateMeshNodePositions(pts);
    }

    ETensor S = Eh.inverse();
    cout << "Homogenized compliance tensor:" << endl;
    cout << S << endl;
    vector<Real> moduli(flatLen(_N));

    // Shear moduli are multiplied by 4 in flattened compliance tensor...
    for (size_t i = 0; i < flatLen(_N); ++i)
        moduli[i] = ((i < _N) ? 1.0 : 0.25) / S.D(i, i);

    vector<Real> poisson;
    if (_N == 2) poisson = { -S.D(0, 1) / S.D(1, 1),   // v_yx
                             -S.D(1, 0) / S.D(0, 0) }; // v_xy
    else         poisson = { -S.D(0, 1) / S.D(1, 1),   // v_yx
                             -S.D(0, 2) / S.D(2, 2),   // v_zx
                             -S.D(1, 2) / S.D(2, 2),   // v_zy
                             -S.D(1, 0) / S.D(0, 0),   // v_xy
                             -S.D(2, 0) / S.D(0, 0),   // v_xz
                             -S.D(2, 1) / S.D(1, 1) }; // v_zy

    if (_N == 2)  {
        cout << "Approximate Young moduli:\t"  << moduli[0] << "\t" << moduli[1] << endl;
        cout << "Approximate shear modulus:\t" << moduli[2] << endl;

        cout << "v_yx, v_xy:\t" << poisson[0] << "\t" << poisson[1] << endl;
    }
    else {
        cout << "Approximate Young moduli:\t" << moduli[0] << "\t" << moduli[1] << "\t"
             << moduli[2] << endl;
        cout << "Approximate shear moduli:\t" << moduli[3] << "\t" << moduli[4] << "\t"
             << moduli[5] << endl;

        cout << "v_yx, v_zx, v_zy:\t" << poisson[0] << "\t" << poisson[1] << "\t" << poisson[2] << endl;
        cout << "v_xy, v_xz, v_yz:\t" << poisson[3] << "\t" << poisson[4] << "\t" << poisson[5] << endl;
    }

    cout << "Anisotropy:\t" << Eh.anisotropy() << endl;

    if (args.count("fieldOutput")) {
        bool linearSubsampleFields = args.count("fullDegreeFieldOutput") == 0;
        MSHFieldWriter writer(args["fieldOutput"].as<string>(), sim.mesh(),
                              linearSubsampleFields);
        if (args.count("centerFluctuationDisplacements")) {
            for (size_t i = 0; i < w_ij.size(); ++i) {
                auto &w = w_ij[i];
                VectorND<_N> total(VectorND<_N>::Zero());
                for (size_t ii = 0; ii < w.domainSize(); ++ii) total += w(ii);
                total *= 1.0 / w.domainSize();
                for (size_t ii = 0; ii < w.domainSize(); ++ii) w(ii) -= total;
            }
        }
        for (size_t i = 0; i < w_ij.size(); ++i) {
            writer.addField("load_ij " + to_string(i), sim.dofToNodeField(sim.constantStrainLoad(-Simulator::SMatrix::CanonicalBasis(i))), DomainType::PER_NODE);
            writer.addField("w_ij " + to_string(i), w_ij[i], DomainType::PER_NODE);
            if ((Simulator::Strain::Deg == 0) || linearSubsampleFields) {
                // Output constant (average) strain when we're outputting piecewise
                // linear solutions.
                writer.addField("strain w_ij " + to_string(i),
                        sim.averageStrainField(w_ij[i]), DomainType::PER_ELEMENT);
            }
            else {
                // Output full-degree per-element strain. (Wasteful since
                // strain fields are of degree - 1, but Gmsh/MSHFieldWriter
                // only supports full-degree ElementNodeData).
                auto strainField = sim.strainField(w_ij[i]);
                typedef SymmetricMatrixInterpolant<typename Simulator::SMatrix,
                                               _N, _FEMDegree> UpsampledStrain;
                vector<UpsampledStrain> upsampledStrainField;
                upsampledStrainField.reserve(strainField.size());
                for (const auto s: strainField)
                    upsampledStrainField.emplace_back(s);
                writer.addField("strain w_ij " + to_string(i),
                                upsampledStrainField, DomainType::PER_ELEMENT);
            }
        }
    }

    if (args.count("distanceToIsotropy")) {
        auto isoFit = closestIsotropicTensor(Eh);
        cout << endl;
        cout << "(Sq Rel Frob) Distance to Isotropy:\t" << (isoFit - Eh).frobeniusNormSq() / isoFit.frobeniusNormSq() << endl;
        cout << "Closest isotropic tensor:" << endl << isoFit << endl;
        cout << endl;
    }

    if (args.count("distanceToMaterial")) {
        Materials::Constant<_N> targetMat(args.at("distanceToMaterial").as<string>());
        auto tgtE = targetMat.getTensor();
        cout << "(Sq Rel Frob) Distance to Specified Tensor:\t" << (Eh - tgtE).frobeniusNormSq() / tgtE.frobeniusNormSq() << endl;
    }

    BENCHMARK_REPORT();
}

////////////////////////////////////////////////////////////////////////////////
/*! Program entry point
//  @param[in]  argc    Number of arguments
//  @param[in]  argv    Argument strings
//  @return     status  (0 on success)
*///////////////////////////////////////////////////////////////////////////////
int main(int argc, const char *argv[])
{
    po::variables_map args = parseCmdLine(argc, argv);

    vector<MeshIO::IOVertex>  inVertices;
    vector<MeshIO::IOElement> inElements;
    string meshPath = args["mesh"].as<string>();
    auto type = load(meshPath, inVertices, inElements, MeshIO::FMT_GUESS,
                     MeshIO::MESH_GUESS);

    // Infer dimension from mesh type.
    size_t dim;
    if      (type == MeshIO::MESH_TET) dim = 3;
    else if (type == MeshIO::MESH_TRI) dim = 2;
    else    throw std::runtime_error("Mesh must be triangle or tet.");

    // Look up and run appropriate homogenizer instantiation.
    int deg = args["degree"].as<int>();
    auto exec = (dim == 3) ? ((deg == 2) ? execute<3, 2> : execute<3, 1>)
                           : ((deg == 2) ? execute<2, 2> : execute<2, 1>);

    exec(args, inVertices, inElements);

    return 0;
}


#pragma once

#include <memory>

#include <Eigen/Dense>
#include <Eigen/Geometry>
#define _USE_MATH_DEFINES
#include <cmath>

#include "math/IWorldAdapter.hpp"
#include "math/IMatrixFactory.hpp"
#include "math/IVectorFactory.hpp"

namespace northstar {
    namespace math {
        class CWorldAdapter : public IWorldAdapter {
        public:
            CWorldAdapter(
                std::shared_ptr<IMatrixFactory> pMatrixFactory,
                std::shared_ptr<IVectorFactory> pVectorFactory);

            virtual types::SPose FromStructureSensorPoseToOpenVRSpace(const ST::XRPose& Pose) override final;
            virtual types::Vector3d FromStructureSensorLinearVectorArrayToOpenVRSpace(const std::array<double, 3>& Vector) override final;
            virtual types::Vector3d FromStructureSensorAngularVectorArrayToOpenVRSpace(const std::array<double, 3>& Vector) override final;
            virtual types::AffineMatrix4d ConversionMatrixFromLeapMotionTrackingSpaceToHMDRelativeSpace(const types::Vector3d& v3dHeadRelativeLeapPosition, const types::Quaterniond& qdHeadRelativeLeapOrientation) override final;
            virtual types::AffineMatrix4d ConversionMatrixFromHMDSpaceToOpenVRWorldSpace(const vr::DriverPose_t& sOVRPose) override final;
            virtual types::Vector3d FromLeapMotionVelocityToOpenVRVelocity(const types::AffineMatrix4d& m4dConversionMatrix, const types::Vector3d& v3dLeapVelocity) override final;
            virtual types::Quaterniond FromUnityQuaternionToOpenVRQuaternion(const types::Quaterniond& qdUnityRotation) override final;
            virtual types::Vector3d FromUnityPositionToOpenVRPosition(const types::Vector3d& v3dUnityPosition) override final;
            virtual types::Vector4d FromUnityProjectionExtentsLRTBToOpenVRProjectionExtentsLRTB(const types::Vector4d& v4dUnityProjectionExtentsLRTB) override final;
            virtual types::AffineMatrix4d FromUnityMatrix4dToOpenVRMatrix4d(const types::AffineMatrix4d& v4dUnityPose) override final;
            virtual types::Vector2d FromOpenGLUVToUnityUV(const types::Vector2d& v2dOpenGLUV) override final;
            virtual types::Vector2d FromUnityUVToOpenGLUV(const types::Vector2d& v2dUnityUV) override final;

        private:
            static constexpr double x_dMillimetersToMeters = 0.001;
            std::shared_ptr<IMatrixFactory> m_pMatrixFactory;
            std::shared_ptr<IVectorFactory> m_pVectorFactory;
        };
    }
}


#include "decoder.h"
#include <fstream>
#include <iostream>
#include <boost/timer/timer.hpp>

// Mainly for profiling purposes

int main(int argc, char** argv)
{
    int N = 32;
    int T = 648;
    double data[N*T];

    std::ifstream is("probs.dat", std::ios::binary);
    is.read(reinterpret_cast<char*>(data), is.tellg());
    is.close();

    // TODO FIXME PARAM Import
    std::string CHARMAP_PATH = "/scail/group/deeplearning/speech/zxie/kaldi-stanford/kaldi-trunk/egs/wsj/s6/ctc-utils";
    PrefixTree ptree(CHARMAP_PATH + "/chars.txt",
                     CHARMAP_PATH + "/wordlist",
                     CHARMAP_PATH + "/lm_bg.arpa");

    // Decode!
    PrefixKey best_prefix;
    int beam = 40;
    double alpha = 1.0;
    double beta = 0.0;
    boost::timer::cpu_timer timer;
    double best_score = decode_bg_lm(data, N, T, ptree, beam, alpha, beta, best_prefix);
    boost::timer::cpu_times elapsed = timer.elapsed();
    std::cout << "decoding time (wall): " << elapsed.wall / 1e9 << std::endl;

    for (int k = 0; k < best_prefix.size(); k++)
    {
        std::cout << best_prefix[k] << " ";
    }
    std::cout << std::endl;

    return 0;
}


/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, Daichi Yoshikawa
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Daichi Yoshikawa 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 OWNER 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.
 *
 * Author: Daichi Yoshikawa
 *
 *********************************************************************/

#ifndef __AHL_ROBOT_CONTROLLER_PARAM_HPP
#define __AHL_ROBOT_CONTROLLER_PARAM_HPP

#include <memory>
#include <mutex>
#include <Eigen/Dense>
#include <dynamic_reconfigure/server.h>
#include <ahl_robot/robot/robot.hpp>
#include <ahl_utils/scoped_lock.hpp>
#include "ahl_robot_controller/param_base.hpp"
#include "ahl_robot_controller/ParamConfig.h"

namespace ahl_ctrl
{

  class Param : public ParamBase
  {
  public:
    explicit Param(const ahl_robot::RobotPtr& robot);

    virtual const Eigen::MatrixXd& getKpJoint() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return Kp_joint_;
    }

    virtual const Eigen::MatrixXd& getKvJoint() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return Kv_joint_;
    }

    virtual const Eigen::MatrixXd& getKpTask() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return Kp_task_;
    }

    virtual const Eigen::MatrixXd& getKiTask() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return Ki_task_;
    }

    virtual const Eigen::MatrixXd& getKvTask() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return Kv_task_;
    }

    virtual const Eigen::Vector3d& getIClippingTaskPos() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return i_clipping_task_pos_;
    }

    virtual const Eigen::Vector3d& getIClippingTaskOri() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return i_clipping_task_ori_;
    }

    virtual const Eigen::MatrixXd& getKvDamp() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return Kv_damp_;
    }

    virtual const Eigen::MatrixXd& getKpLimit() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return Kp_limit_;
    }

    virtual const Eigen::MatrixXd& getKvLimit() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return Kv_limit_;
    }

    virtual double getJointErrorMax() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return joint_error_max_;
    }

    virtual double getPosErrorMax() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return pos_error_max_;
    }

    virtual double getOriErrorMax() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return ori_error_max_;
    }

    virtual double getDqMax() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return dq_max_;
    }

    virtual double getVxMax() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return vx_max_;
    }

    virtual const Eigen::Vector3d& getG() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return g_;
    }

    virtual const Eigen::MatrixXd& getB() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return b_;
    }

    virtual double getKpWheel() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return kp_wheel_;
    }

    virtual double getKvWheel() override
    {
      ahl_utils::ScopedLock lock(mutex_);
      return kv_wheel_;
    }

  private:
    void update(ahl_robot_controller::ParamConfig& config, uint32_t level);
    using ParamConfigServer = dynamic_reconfigure::Server<ahl_robot_controller::ParamConfig>;
    using ParamConfigServerPtr = std::shared_ptr<ParamConfigServer>;

    std::mutex mutex_;

    ParamConfigServerPtr server_;
    dynamic_reconfigure::Server<ahl_robot_controller::ParamConfig>::CallbackType f_;

    uint32_t dof_;
    uint32_t macro_dof_;

    Eigen::MatrixXd Kp_joint_;
    Eigen::MatrixXd Kv_joint_;
    Eigen::MatrixXd Kp_task_;
    Eigen::MatrixXd Ki_task_;
    Eigen::MatrixXd Kv_task_;
    Eigen::Vector3d i_clipping_task_pos_;
    Eigen::Vector3d i_clipping_task_ori_;
    Eigen::MatrixXd Kv_damp_;
    Eigen::MatrixXd Kp_limit_;
    Eigen::MatrixXd Kv_limit_;
    double joint_error_max_;
    double pos_error_max_;
    double ori_error_max_;
    double dq_max_;
    double vx_max_;
    double kp_wheel_;
    double kv_wheel_;
    Eigen::Vector3d g_;
    Eigen::MatrixXd b_;
  };

  using ParamPtr = std::shared_ptr<Param>;

} // namespace ahl_ctrl

#endif // __AHL_ROBOT_CONTROLLER_PARAM_HPP


// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

/*
/// \file StereoSessionIsis.cc
///

// Vision Workbench
#include <vw/Core/Settings.h>
#include <vw/Core/Log.h>
#include <vw/Math/Functors.h>
#include <vw/Image/Algorithms.h>
#include <vw/Image/EdgeExtension.h>
#include <vw/Image/ImageViewRef.h>
#include <vw/Image/Manipulation.h>
#include <vw/Image/ImageMath.h>
#include <vw/Image/MaskViews.h>
#include <vw/Image/Statistics.h>
#include <vw/Image/Transform.h>
#include <vw/FileIO/DiskImageView.h>
#include <vw/FileIO/DiskImageResourceGDAL.h>
#include <vw/FileIO/DiskImageResourceOpenEXR.h>
#include <vw/Camera/CameraModel.h>
#include <vw/Stereo/DisparityMap.h>
#include <vw/InterestPoint/Descriptor.h>
#include <vw/InterestPoint/Detector.h>
#include <vw/InterestPoint/Matcher.h>
#include <vw/InterestPoint/MatrixIO.h>
#include <vw/Cartography/Datum.h>

// Stereo Pipeline
#include <asp/Core/InterestPointMatching.h>
#include <asp/Core/AffineEpipolar.h>
#include <asp/Core/PhotometricOutlier.h>
#include <asp/Sessions/StereoSessionIsis.h>
#include <asp/IsisIO/IsisCameraModel.h>
#include <asp/IsisIO/DiskImageResourceIsis.h>
#include <asp/IsisIO/Equation.h>

// Boost
#include <boost/filesystem/operations.hpp>
#include <boost/math/special_functions/next.hpp> // boost::float_next
#include <boost/shared_ptr.hpp>
namespace fs = boost::filesystem;

#include <algorithm>

using namespace vw;
using namespace vw::camera;
using namespace asp;

#if defined(ASP_HAVE_PKG_ISISIO) && ASP_HAVE_PKG_ISISIO == 1

// Allows FileIO to correctly read/write these pixel types
namespace vw {
  template<> struct PixelFormatID<Vector3>   { static const PixelFormatEnum value = VW_PIXEL_GENERIC_3_CHANNEL; };
}

#endif  // ASP_HAVE_PKG_ISISIO
*/


/***************************************************************************
 *   Copyright (C) Preparatory Commission for the Comprehensive            *
 *   Nuclear-Test-Ban Treaty Organization (CTBTO).                         *
 *                                                                         *
 *   You can redistribute and/or modify this program under the             *
 *   terms of the SeisComP Public License.                                 *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   SeisComP Public License for more details.                             *
 ***************************************************************************/

#define SEISCOMP_COMPONENT Magnitudes/ML_idc

#include "ML_idc_private.h"

#include <seiscomp3/logging/log.h>
#include <seiscomp3/system/environment.h>

#include <boost/thread/mutex.hpp>

#include <cmath> // log10
#include <vector> // log10
#include <cfloat>
#include <fstream>

#define AMP_TYPE "SBSNR"
#define MAG_TYPE "ML(IDC)"

#define MINIMUM_DISTANCE 0.0   // in degrees
#define MAXIMUM_DISTANCE 20.0  // in degrees

#define MAXIMUM_DEPTH    40.0  // in km

using namespace std;

namespace {

std::string ExpectedAmplitudeUnit = "nm";

Util::TabValues tableA;
bool validTableA = false;
bool readTableA = false;
boost::mutex mutexTableA;
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
Magnitude_ML_idc::Magnitude_ML_idc()
: MagnitudeProcessor(MAG_TYPE) {}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
std::string Magnitude_ML_idc::amplitudeType() const {
	return AMP_TYPE;
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
bool Magnitude_ML_idc::setup(const Settings &settings) {
	_attenuation.reset();

	if ( !MagnitudeProcessor::setup(settings) )
		return false;

	try {
		string tablePath = Environment::Instance()->absolutePath(settings.getString("magnitudes.ML(IDC).A"));
		size_t p;

		p = tablePath.find("{net}");
		if ( p != string::npos ) {
			tablePath.replace(p, 5, settings.networkCode);
		}

		p = tablePath.find("{sta}");
		if ( p != string::npos ) {
			tablePath.replace(p, 5, settings.stationCode);
		}

		p = tablePath.find("{loc}");
		if ( p != string::npos ) {
			tablePath.replace(p, 5, settings.locationCode);
		}

		SEISCOMP_DEBUG("Read station specific ML(IDC) attentuation table at %s",
		               tablePath.c_str());

		_attenuation = new Util::TabValues;
		bool validLocalTable = _attenuation->read(tablePath);
		if ( !validLocalTable ) {
			_attenuation.reset();
			SEISCOMP_ERROR("Failed to read A values from: %s", tablePath.c_str());
			return false;
		}
	}
	catch ( ... ) {}

	if ( !_attenuation ) {
		mutexTableA.lock();

		if ( !readTableA ) {
			string tablePath = Environment::Instance()->absolutePath("@DATADIR@/magnitudes/IDC/global.ml");
			SEISCOMP_DEBUG("Read global ML(IDC) attentuation table at %s",
			               tablePath.c_str());

			validTableA = tableA.read(tablePath);
			if ( !validTableA ) {
				SEISCOMP_ERROR("Failed to read A values from: %s", tablePath.c_str());
			}

			readTableA = true;
		}
		else if ( !validTableA ) {
			SEISCOMP_ERROR("Invalid A value table");
		}

		mutexTableA.unlock();
		return validTableA;
	}

	return true;
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
MagnitudeProcessor::Status
Magnitude_ML_idc::computeMagnitude(double amplitude,
                                   const std::string &unit,
                                   double period, double,
                                   double delta, double depth,
                                   const DataModel::Origin *,
                                   const DataModel::SensorLocation *,
                                   const DataModel::Amplitude *,
                                   double &value) {
	if ( !_attenuation && !validTableA )
		return IncompleteConfiguration;

	if ( (delta < MINIMUM_DISTANCE) || (delta > MAXIMUM_DISTANCE) )
		return DistanceOutOfRange;

	if ( depth > MAXIMUM_DEPTH )
		return DepthOutOfRange;

	if ( !convertAmplitude(amplitude, unit, ExpectedAmplitudeUnit) )
		return InvalidAmplitudeUnit;

	double x_1st_deriv, z_1st_deriv, x_2nd_deriv, z_2nd_deriv;
	double interpolated_value;
	int interp_err;
	bool ok;

	if ( _attenuation ) {
		ok = _attenuation->interpolate(interpolated_value, false, true,
		                               delta, 0, &x_1st_deriv, &z_1st_deriv,
		                               &x_2nd_deriv, &z_2nd_deriv, &interp_err);
	}
	else {
		mutexTableA.lock();
		ok = tableA.interpolate(interpolated_value, false, true,
		                        delta, 0, &x_1st_deriv, &z_1st_deriv,
		                        &x_2nd_deriv, &z_2nd_deriv, &interp_err);
		mutexTableA.unlock();
	}

	if ( !ok ) {
		SEISCOMP_ERROR("Failed to interpolate attentuation value");
		return Error;
	}

	if ( interp_err ) {
		SEISCOMP_ERROR("Error on attentuation interpolation: %d", interp_err);
		return Error;
	}

	value = correctMagnitude(log10(amplitude) + double(interpolated_value));

	return OK;
}
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
REGISTER_MAGNITUDEPROCESSOR(Magnitude_ML_idc, MAG_TYPE);
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>}
}


//-*- mode: c++; indent-tabs-mode: nil; tab-width: 2 -*-

#include "ug_bitext.h"
#include <algorithm>
#include <boost/math/distributions/binomial.hpp>

namespace sapt
{

  float
  lbop(size_t const tries, size_t const succ, float const confidence)
  {
    return (confidence == 0
            ? float(succ)/tries
            : (boost::math::binomial_distribution<>::
               find_lower_bound_on_p(tries, succ, confidence)));
  }

  void
  snt_adder<L2R_Token<SimpleWordId> >::
  operator()()
  {
    typedef L2R_Token<SimpleWordId> tkn;
    std::vector<id_type> sids; sids.reserve(snt.size());
    BOOST_FOREACH(std::string const& foo, snt)
      {
        sids.push_back(track ? track->size() : 0);
        std::istringstream buf(foo);
        std::string w;
        std::vector<tkn> s; s.reserve(100);
        while (buf >> w) s.push_back(tkn(V[w]));
        track = append(track,s);
      }
    if (index)
      index.reset(new imTSA<tkn>(*index,track,sids,V.tsize()));
    else
      index.reset(new imTSA<tkn>(track,NULL,NULL));
  }

  snt_adder<L2R_Token<SimpleWordId> >::
  snt_adder(std::vector<std::string> const& s, TokenIndex& v,
            SPTR<imTtrack<L2R_Token<SimpleWordId> > >& t,
            SPTR<imTSA<L2R_Token<SimpleWordId> > >& i)
    : snt(s), V(v), track(t), index(i)
  { }

  bool
  expand_phrase_pair
  (std::vector<std::vector<ushort> >& a1,
   std::vector<std::vector<ushort> >& a2,
   ushort const s2, // next word on in target side
   ushort const L1, ushort const R1, // limits of previous phrase
   ushort & s1, ushort & e1, ushort& e2) // start/end src; end trg
  {
    if (a2[s2].size() == 0)
      {
        std::cout << __FILE__ << ":" << __LINE__ << std::endl;
        return false;
      }
    bitvector done1(a1.size());
    bitvector done2(a2.size());
    std::vector<std::pair<ushort,ushort> > agenda;
    // x.first:  side (1 or 2)
    // x.second: word position
    agenda.reserve(a1.size() + a2.size());
    agenda.push_back(std::pair<ushort,ushort>(2,s2));
    e2 = s2;
    s1 = e1 = a2[s2].front();
    if (s1 >= L1 && s1 < R1)
      {
        std::cout << __FILE__ << ":" << __LINE__ << std::endl;
        return false;
      }
    agenda.push_back(std::pair<ushort,ushort>(2,s2));
    while (agenda.size())
      {
        ushort side = agenda.back().first;
        ushort p    = agenda.back().second;
        agenda.pop_back();
        if (side == 1)
          {
            done1.set(p);
            BOOST_FOREACH(ushort i, a1[p])
              {
                if (i < s2)
                  {
                    // cout << __FILE__ << ":" << __LINE__ << endl;
                    return false;
                  }
                if (done2[i]) continue;
                for (;e2 <= i;++e2)
                  if (!done2[e2])
                    agenda.push_back(std::pair<ushort,ushort>(2,e2));
              }
          }
        else
          {
            done2.set(p);
            BOOST_FOREACH(ushort i, a2[p])
              {
                if ((e1 < L1 && i >= L1) ||
                    (s1 >= R1 && i < R1) ||
                    (i >= L1 && i < R1))
                  {
                    // cout << __FILE__ << ":" << __LINE__ << " "
                    // << L1 << "-" << R1 << " " << i << " "
                    // << s1 << "-" << e1<< endl;
                    return false;
                  }

                if (e1 < i)
                  {
                    for (; e1 <= i; ++e1)
                      if (!done1[e1])
                        agenda.push_back(std::pair<ushort,ushort>(1,e1));
                  }
                else if (s1 > i)
                  {
                    for (; i <= s1; ++i)
                      if (!done1[i])
                        agenda.push_back(std::pair<ushort,ushort>(1,i));
                  }
              }
          }
      }
    ++e1;
    ++e2;
    return true;
  }

  void
  print_amatrix(std::vector<std::vector<ushort> > a1, uint32_t len2,
                ushort b1, ushort e1, ushort b2, ushort e2)
  {
    using namespace std;
    std::vector<bitvector> M(a1.size(),bitvector(len2));
    for (ushort j = 0; j < a1.size(); ++j)
      {
        BOOST_FOREACH(ushort k, a1[j])
          M[j].set(k);
      }
    cout << b1 << "-" << e1 << " " << b2 << "-" << e2 << endl;
    cout << "   ";
    for (size_t c = 0; c < len2;++c)
      cout << c%10;
    cout << endl;
    for (size_t r = 0; r < M.size(); ++r)
      {
        cout << setw(3) << r << " ";
        for (size_t c = 0; c < M[r].size(); ++c)
          {
            if ((b1 <= r) && (r < e1) && b2 <= c && c < e2)
              cout << (M[r][c] ? 'x' : '-');
            else cout << (M[r][c] ? 'o' : '.');
          }
        cout << endl;
      }
    cout  << std::string(90,'-') << endl;
  }

  void
  write_bitvector(bitvector const& v, std::ostream& out)
  {
    for (size_t i = v.find_first(); i < v.size();)
      {
        out << i;
        if ((i = v.find_next(i)) < v.size()) out << ",";
      }
  }

}