#ifndef STAN_MATH_TORSTEN_EVENTS_MANAGER_HPP
#define STAN_MATH_TORSTEN_EVENTS_MANAGER_HPP

#include <stan/math/torsten/dsolve/pk_vars.hpp>
#include <stan/math/torsten/ev_history.hpp>
#include <stan/math/torsten/ev_record.hpp>
#include <stan/math/torsten/event.hpp>
#include <boost/math/tools/promotion.hpp>
#include <Eigen/Dense>
#include <string>
#include <vector>

namespace torsten {
  template <typename T_event_record, typename T_params>
  struct EventsManager;

  template <typename T0, typename T1, typename T2, typename T3, typename T4,
            template<class...> class theta_container, typename... tuple_pars_t, typename... Ts>
  struct EventsManager<NONMENEventsRecord<T0, T1, T2, T3>,
                       NonEventParameters<T0, T4, theta_container, std::tuple<tuple_pars_t...>, Ts...> > {
    using param_t = NonEventParameters<T0, T4, theta_container, std::tuple<tuple_pars_t...>, Ts...>;
    using ER = NONMENEventsRecord<T0, T1, T2, T3>;
    using T_scalar = typename stan::return_type_t<typename ER::T_scalar, T4, tuple_pars_t..., Ts...>;
    using T_time   = typename stan::return_type_t<typename ER::T_time, typename param_t::lag_t>;
    using T_rate   = typename stan::return_type_t<typename ER::T_rate, typename param_t::biovar_t>;
    using T_amt    = typename stan::return_type_t<typename ER::T_amt, typename param_t::biovar_t>;
    using T_par    = T4;
    using T_par_rate = T2;
    using T_par_ii   = T3;

    param_t params;
    EventHistory<T0, T1, T2, T3, typename param_t::lag_t> event_his;

    int nKeep;
    int ncmt;

    static int nCmt(const ER& rec) {
      return rec.ncmt;
    }

    /*
     * the index in the result/input where subject @c id begins.
     */
    static int begin(int id, const ER& rec) {
      return rec.begin_.at(id);
    }

    /*
     * For population models, we need generate events using
     * ragged arrays.
     */
    template<typename... Tss>
    EventsManager(const ER& rec,
                  const std::vector<theta_container<T4>>& theta,
                  const std::vector<std::vector<Tss>>&... non_event_params) :
      EventsManager(0, rec, theta, non_event_params...)
    {}

    /*
     * For population models, we need generate events using
     * ragged arrays.
     */
    template<typename... Tss>
    EventsManager(int id, const ER& rec,
                  const std::vector<theta_container<T4>>& theta,
                  const std::vector<std::vector<Tss>>&... non_event_params) :
      params(id, rec, theta, non_event_params...),
      event_his(rec.ncmt, rec.begin_[id], rec.len_[id], rec.time_, rec.amt_, rec.rate_, rec.ii_, rec.evid_, rec.cmt_, rec.addl_, rec.ss_)
    {
      ncmt = rec.ncmt;

      attach_event_parameters();
      insert_lag_dose();
      event_his.generate_rates(ncmt);
      attach_event_parameters();

      nKeep = event_his.num_event_times;
    }

    template<typename... Tss>
    EventsManager(int id, const ER& rec,
                  int ibegin_theta, int isize_theta,
                  int ibegin_biovar, int isize_biovar,
                  int ibegin_tlag, int isize_tlag,
                  const std::vector<theta_container<T4>>& theta,
                  const std::vector<std::vector<Tss>>&... non_event_params) :
      params(id, rec, ibegin_theta, isize_theta, ibegin_biovar, isize_biovar, ibegin_tlag, isize_tlag, theta, non_event_params...),
      event_his(rec.ncmt, rec.begin_[id], rec.len_[id], rec.time_, rec.amt_, rec.rate_, rec.ii_, rec.evid_, rec.cmt_, rec.addl_, rec.ss_)
    {
      ncmt = rec.ncmt;

      attach_event_parameters();
      insert_lag_dose();
      event_his.generate_rates(ncmt);
      attach_event_parameters();

      nKeep = event_his.num_event_times;
    }

    /**
     * Implement absorption lag times by modifying the times of the dosing events.
     * Two cases: parameters are either constant or vary with each event.
     * Function sorts events at the end of the procedure.
     * The old event is set with a special EVID = 9 and it introduces no action.
     */
    void insert_lag_dose() {
      // reverse loop so we don't process same lagged events twice
      int nEvent = event_his.size();
      int iEvent = nEvent - 1;
      while (iEvent >= 0) {
        if (event_his.is_dosing(iEvent)) {
          if (params.lag_time(iEvent, event_his.cmt(iEvent) - 1) > ER::lag_time_min) {
            event_his.insert_event(iEvent);
            event_his.gen_time.back() += params.lag_time(iEvent, event_his.cmt(iEvent) - 1);
            event_his.idx[iEvent][2] = 9;
          }
        }
        iEvent--;
      }
      event_his.sort_state_time();
    }

    const EventHistory<T0, T1, T2, T3, typename param_t::lag_t>& events() const {
      return event_his;
    }

    void attach_event_parameters() {
      int nEvent = event_his.size();
      assert(nEvent > 0);
      int len_Parameters = params.size();  // numbers of events for which parameters are determined
      assert(len_Parameters > 0);

      if (!params.is_ordered()) params.sort();
      params.pars.resize(nEvent);

      int iEvent = 0;
      for (int i = 0; i < len_Parameters - 1; ++i) {
        while (event_his.isnew(iEvent)) iEvent++;  // skip new events
        assert(params.get_par_time(i) == event_his.time(iEvent));  // compare time of "old' events to time of parameters.
        iEvent++;
      }

      if (len_Parameters == 1)  {
        for (int i = 0; i < nEvent; ++i) {
          params.set_par_time(i, stan::math::value_of(event_his.time(i)));
          params.set_par_array(i, params.get_par_array(0));
          event_his.idx[i][3] = 0;
        }
      } else {  // parameters are event dependent.
        std::vector<double> times(nEvent, 0);
        for (int i = 0; i < nEvent; ++i) times[i] = params.pars[i].first;
        iEvent = 0;

        using par_t = typename param_t::par_t;
        par_t newParameter;
        int j = 0;
        typename std::vector<par_t>::const_iterator lower = params.pars.begin();
        typename std::vector<par_t>::const_iterator it_param_end = params.pars.begin() + len_Parameters;
        for (int iEvent = 0; iEvent < nEvent; ++iEvent) {
          if (event_his.isnew(iEvent)) {
            // Find the index corresponding to the time of the new event in the
            // times vector.
            const double t = stan::math::value_of(event_his.time(iEvent));
            lower = std::lower_bound(lower, it_param_end, t,
                                     [](const par_t& t1, const double& t2) {return t1.first < t2;});
            newParameter = lower == (it_param_end) ? params.pars[len_Parameters-1] : *lower;
            newParameter.first = t;
            params.pars[len_Parameters + j] = newParameter;
            event_his.idx[iEvent][3] = 0; /**< item is "new" no more, set "isnew" false */
            j++;
          }
        }
      }
      params.sort();
    }

    /**
     * Get model param for certain subject
     *
     * @param i subject id
     *
     * @return model param
     */
    inline const theta_container<T4>& theta(int i) const {
      return params.theta(i);
    }

    template<size_t Is>
    inline auto& get_model_array_1d_param(int i) const {
      return params.template get_model_array_1d_param<Is>(i);
    }

    /**
     * Get dosing rate adjusted with bioavailability for a subject
     *
     * @param i subject id
     *
     * @return dosing rate
     */
    inline std::vector<T_rate> fractioned_rates(int i) const {
      const int n = event_his.rates[0].second.size();
      const std::vector<T2>& r = event_his.rates[event_his.rate_index[i]].second;
      std::vector<T_rate> res(r.size());
      for (size_t j = 0; j < r.size(); ++j) {
        res[j] = r[j] * params.bioavailability(i, j);
      }
      return res;
    }

    /**
     * Get dosing amount adjusted with bioavailability for a subject
     *
     * @param i subject id
     *
     * @return dosing amount
     */
    inline T_amt fractioned_amt(int i) const {
      // return bioavailability(i, cmt(i) - 1) * amt(i);
      return params.bioavailability(i, event_his.cmt(i) - 1) * event_his.amt(i);
    }

    Event<T_time, T3, T_amt, T_rate, T2> event(int i) const {
      int id;
      switch (event_his.evid(i)) {
      case 2:                   // "other" type given "-cmt" indicates turn-off/reset
        if (event_his.cmt(i) < 0) {
            id = 5;
        } else {
          id = 0;
        }
        break;
      case 3:                   // reset
        id = 1;
        break;
      case 4:                   // reset + dosing
        if (event_his.is_ss_dosing(i)) {
          // since it's reset, SS reset is irrelevant
          id = 4;
        } else {
          id = 2;
        }
        break;
      case 8:                   // mrgsolve: "evid=9" overwrite cmt
        if (event_his.cmt(i) > 0) {
          id = 6;
        }
        break;
      default:
        if (event_his.is_ss_dosing(i)) {
          if (event_his.ss(i) == 2) {
            id = 3;
          } else {
            id = 4;
          }
        } else {
          id = 0;
        }
      }

      T_time t0, t1;
      if (event_his.is_ss_dosing(i)) {
        // t0 = event_his.time(i);
        // t1 = event_his.ii(i);
        t0 = i == 0 ? event_his.time(0) : event_his.time(i-1);
        t1 = event_his.time(i);
      } else {
        t0 = i == 0 ? event_his.time(0) : event_his.time(i-1);
        t1 = event_his.time(i);
      }
      PKRec<T_amt> amt = PKRec<T_amt>::Zero(ncmt);
      if (event_his.is_bolus_dosing(i) || event_his.is_ss_dosing(i)) {
        amt(event_his.cmt(i) - 1) = fractioned_amt(i);
      }
      std::vector<T_rate> rate(fractioned_rates(i));
      return {id, t0, t1, event_his.ii(i),
              amt, rate, event_his.rate(i), event_his.cmt(i)};
    }

    /*
     * number of events for a sinlge individual
     */
    template<typename... Tss>
    static int num_events(const ER& rec,
                          const std::vector<theta_container<T4>>& theta,
                          const std::vector<std::vector<Tss>>&... non_event_params) {
      return num_events(0, rec, theta, non_event_params...);
    }

    static int num_events(int id, const ER& rec,
                          const std::vector<theta_container<T4>>& theta) {
      int res;
      int n = rec.len_[id];
      for (int i = rec.begin_[id]; i < rec.begin_[id] + rec.len_[id]; ++i) {
        if (rec.evid_[i] == 1 || rec.evid_[i] == 4) {      // is dosing event
          if (rec.addl_[i] > 0 && rec.ii_[i] > 0) {        // has addl doses
            if (rec.rate_[i] > 0 && rec.amt_[i] > 0) {
              n++;                               // end event for original IV dose
              n += 2 * rec.addl_[i];                  // end event for addl IV dose
            } else {
              n += rec.addl_[i];
            }
          } else if (rec.rate_[i] > 0 && rec.amt_[i] > 0) {
            n++;                                 // end event for IV dose
          }
        }
      }
      res = n;
      return res;
    }

    template<typename T5>
    static int num_events(int id, const ER& rec,
                          const std::vector<theta_container<T4>>& theta,
                          const std::vector<std::vector<T5> >& biovar) {
      return num_events(id, rec, theta);
    }

    template<typename T5, typename T6, typename... Tss>
    static int num_events(int id, const ER& rec,
                          const std::vector<theta_container<T4>>& theta,
                          const std::vector<std::vector<T5> >& biovar,
                          const std::vector<std::vector<T6> >& tlag,
                          const std::vector<std::vector<Tss>>&... non_event_params) {
      int res;
      bool has_lag = rec.has_positive_param(id, tlag);

      if (!has_lag) {
        res = num_events(id, rec, theta);
      } else if (rec.len_param(id, tlag) == 1) {
        int n = rec.len_[id];
        std::vector<std::tuple<double, int>> dose;
        dose.reserve(rec.len_[id]);
        for (int i = rec.begin_[id]; i < rec.begin_[id] + rec.len_[id]; ++i) {
          if (rec.evid_[i] == 1 || rec.evid_[i] == 4) {      // is dosing event
            if (tlag[rec.begin_param(id, tlag)][rec.cmt_[i] - 1] > 0.0) {       // tlag dose
              n++;
            }
            if (rec.addl_[i] > 0 && rec.ii_[i] > 0) {        // has addl doses
              if (rec.rate_[i] > 0 && rec.amt_[i] > 0) {
                n++;                               // end ev for IV dose
                n += 2 * rec.addl_[i];                  // end ev for addl IV dose
              } else {
                n += rec.addl_[i];
              }
              if (tlag[rec.begin_param(id, tlag)][rec.cmt_[i] - 1] > 0.0) {     // tlag dose
                n += rec.addl_[i];
              }
            } else if (rec.rate_[i] > 0 && rec.amt_[i] > 0) {
              n++;                                 // end event for IV dose
            }
          }
        }
        res = n;
      } else {
        // FIXME not to use brute force
        res = EventsManager(id, rec, theta, biovar, tlag, non_event_params...).events().size();
      }

      return res;
    }
  };

}

#endif


//  Copyright John Maddock 2005-2006.
//  Use, modification and distribution are subject to 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 BOOST_MATH_TOOLS_PRECISION_INCLUDED
#define BOOST_MATH_TOOLS_PRECISION_INCLUDED

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/limits.hpp>
#include <boost/assert.hpp>
#include <boost/static_assert.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/if.hpp>
#include <boost/math/policies/policy.hpp>

// These two are for LDBL_MAN_DIG:
#include <limits.h>
#include <math.h>

namespace boost{ namespace math
{
namespace tools
{
// If T is not specialized, the functions digits, max_value and min_value,
// all get synthesised automatically from std::numeric_limits.
// However, if numeric_limits is not specialised for type RealType,
// for example with NTL::RR type, then you will get a compiler error
// when code tries to use these functions, unless you explicitly specialise them.

// For example if the precision of RealType varies at runtime,
// then numeric_limits support may not be appropriate,
// see boost/math/tools/ntl.hpp  for examples like
// template <> NTL::RR max_value<NTL::RR> ...
// See  Conceptual Requirements for Real Number Types.

template <class T>
inline BOOST_MATH_CONSTEXPR int digits(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(T)) BOOST_NOEXCEPT
{
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   BOOST_STATIC_ASSERT( ::std::numeric_limits<T>::is_specialized);
   BOOST_STATIC_ASSERT( ::std::numeric_limits<T>::radix == 2 || ::std::numeric_limits<T>::radix == 10);
#else
   BOOST_ASSERT(::std::numeric_limits<T>::is_specialized);
   BOOST_ASSERT(::std::numeric_limits<T>::radix == 2 || ::std::numeric_limits<T>::radix == 10);
#endif
   return std::numeric_limits<T>::radix == 2
      ? std::numeric_limits<T>::digits
      : ((std::numeric_limits<T>::digits + 1) * 1000L) / 301L;
}

template <class T>
inline BOOST_MATH_CONSTEXPR T max_value(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE(T))  BOOST_MATH_NOEXCEPT(T)
{
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   BOOST_STATIC_ASSERT( ::std::numeric_limits<T>::is_specialized);
#else
   BOOST_ASSERT(::std::numeric_limits<T>::is_specialized);
#endif
   return (std::numeric_limits<T>::max)();
} // Also used as a finite 'infinite' value for - and +infinity, for example:
// -max_value<double> = -1.79769e+308, max_value<double> = 1.79769e+308.

template <class T>
inline BOOST_MATH_CONSTEXPR T min_value(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE(T)) BOOST_MATH_NOEXCEPT(T)
{
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   BOOST_STATIC_ASSERT( ::std::numeric_limits<T>::is_specialized);
#else
   BOOST_ASSERT(::std::numeric_limits<T>::is_specialized);
#endif
   return (std::numeric_limits<T>::min)();
}

namespace detail{
//
// Logarithmic limits come next, note that although
// we can compute these from the log of the max value
// that is not in general thread safe (if we cache the value)
// so it's better to specialise these:
//
// For type float first:
//
template <class T>
inline BOOST_MATH_CONSTEXPR T log_max_value(const boost::integral_constant<int, 128>& BOOST_MATH_APPEND_EXPLICIT_TEMPLATE_TYPE(T)) BOOST_MATH_NOEXCEPT(T)
{
   return 88.0f;
}

template <class T>
inline BOOST_MATH_CONSTEXPR T log_min_value(const boost::integral_constant<int, 128>& BOOST_MATH_APPEND_EXPLICIT_TEMPLATE_TYPE(T)) BOOST_MATH_NOEXCEPT(T)
{
   return -87.0f;
}
//
// Now double:
//
template <class T>
inline BOOST_MATH_CONSTEXPR T log_max_value(const boost::integral_constant<int, 1024>& BOOST_MATH_APPEND_EXPLICIT_TEMPLATE_TYPE(T)) BOOST_MATH_NOEXCEPT(T)
{
   return 709.0;
}

template <class T>
inline BOOST_MATH_CONSTEXPR T log_min_value(const boost::integral_constant<int, 1024>& BOOST_MATH_APPEND_EXPLICIT_TEMPLATE_TYPE(T)) BOOST_MATH_NOEXCEPT(T)
{
   return -708.0;
}
//
// 80 and 128-bit long doubles:
//
template <class T>
inline BOOST_MATH_CONSTEXPR T log_max_value(const boost::integral_constant<int, 16384>& BOOST_MATH_APPEND_EXPLICIT_TEMPLATE_TYPE(T)) BOOST_MATH_NOEXCEPT(T)
{
   return 11356.0L;
}

template <class T>
inline BOOST_MATH_CONSTEXPR T log_min_value(const boost::integral_constant<int, 16384>& BOOST_MATH_APPEND_EXPLICIT_TEMPLATE_TYPE(T)) BOOST_MATH_NOEXCEPT(T)
{
   return -11355.0L;
}

template <class T>
inline T log_max_value(const boost::integral_constant<int, 0>& BOOST_MATH_APPEND_EXPLICIT_TEMPLATE_TYPE(T))
{
   BOOST_MATH_STD_USING
#ifdef __SUNPRO_CC
   static const T m = boost::math::tools::max_value<T>();
   static const T val = log(m);
#else
   static const T val = log(boost::math::tools::max_value<T>());
#endif
   return val;
}

template <class T>
inline T log_min_value(const boost::integral_constant<int, 0>& BOOST_MATH_APPEND_EXPLICIT_TEMPLATE_TYPE(T))
{
   BOOST_MATH_STD_USING
#ifdef __SUNPRO_CC
   static const T m = boost::math::tools::min_value<T>();
   static const T val = log(m);
#else
   static const T val = log(boost::math::tools::min_value<T>());
#endif
   return val;
}

template <class T>
inline BOOST_MATH_CONSTEXPR T epsilon(const boost::true_type& BOOST_MATH_APPEND_EXPLICIT_TEMPLATE_TYPE(T)) BOOST_MATH_NOEXCEPT(T)
{
   return std::numeric_limits<T>::epsilon();
}

#if defined(__GNUC__) && ((LDBL_MANT_DIG == 106) || (__LDBL_MANT_DIG__ == 106))
template <>
inline BOOST_MATH_CONSTEXPR long double epsilon<long double>(const boost::true_type& BOOST_MATH_APPEND_EXPLICIT_TEMPLATE_TYPE(long double)) BOOST_MATH_NOEXCEPT(long double)
{
   // numeric_limits on Darwin (and elsewhere) tells lies here:
   // the issue is that long double on a few platforms is
   // really a "double double" which has a non-contiguous
   // mantissa: 53 bits followed by an unspecified number of
   // zero bits, followed by 53 more bits.  Thus the apparent
   // precision of the type varies depending where it's been.
   // Set epsilon to the value that a 106 bit fixed mantissa
   // type would have, as that will give us sensible behaviour everywhere.
   //
   // This static assert fails for some unknown reason, so
   // disabled for now...
   // BOOST_STATIC_ASSERT(std::numeric_limits<long double>::digits == 106);
   return 2.4651903288156618919116517665087e-32L;
}
#endif

template <class T>
inline T epsilon(const boost::false_type& BOOST_MATH_APPEND_EXPLICIT_TEMPLATE_TYPE(T))
{
   // Note: don't cache result as precision may vary at runtime:
   BOOST_MATH_STD_USING  // for ADL of std names
   return ldexp(static_cast<T>(1), 1-policies::digits<T, policies::policy<> >());
}

template <class T>
struct log_limit_traits
{
   typedef typename mpl::if_c<
      (std::numeric_limits<T>::radix == 2) &&
      (std::numeric_limits<T>::max_exponent == 128
         || std::numeric_limits<T>::max_exponent == 1024
         || std::numeric_limits<T>::max_exponent == 16384),
      boost::integral_constant<int, (std::numeric_limits<T>::max_exponent > INT_MAX ? INT_MAX : static_cast<int>(std::numeric_limits<T>::max_exponent))>,
      boost::integral_constant<int, 0>
   >::type tag_type;
   BOOST_STATIC_CONSTANT(bool, value = tag_type::value ? true : false);
   BOOST_STATIC_ASSERT(::std::numeric_limits<T>::is_specialized || (value == 0));
};

template <class T, bool b> struct log_limit_noexcept_traits_imp : public log_limit_traits<T> {};
template <class T> struct log_limit_noexcept_traits_imp<T, false> : public boost::integral_constant<bool, false> {};

template <class T>
struct log_limit_noexcept_traits : public log_limit_noexcept_traits_imp<T, BOOST_MATH_IS_FLOAT(T)> {};

} // namespace detail

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4309)
#endif

template <class T>
inline BOOST_MATH_CONSTEXPR T log_max_value(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE(T)) BOOST_NOEXCEPT_IF(detail::log_limit_noexcept_traits<T>::value)
{
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   return detail::log_max_value<T>(typename detail::log_limit_traits<T>::tag_type());
#else
   BOOST_ASSERT(::std::numeric_limits<T>::is_specialized);
   BOOST_MATH_STD_USING
   static const T val = log((std::numeric_limits<T>::max)());
   return val;
#endif
}

template <class T>
inline BOOST_MATH_CONSTEXPR T log_min_value(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE(T)) BOOST_NOEXCEPT_IF(detail::log_limit_noexcept_traits<T>::value)
{
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   return detail::log_min_value<T>(typename detail::log_limit_traits<T>::tag_type());
#else
   BOOST_ASSERT(::std::numeric_limits<T>::is_specialized);
   BOOST_MATH_STD_USING
   static const T val = log((std::numeric_limits<T>::min)());
   return val;
#endif
}

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

template <class T>
inline BOOST_MATH_CONSTEXPR T epsilon(BOOST_MATH_EXPLICIT_TEMPLATE_TYPE_SPEC(T)) BOOST_MATH_NOEXCEPT(T)
{
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   return detail::epsilon<T>(boost::integral_constant<bool, ::std::numeric_limits<T>::is_specialized>());
#else
   return ::std::numeric_limits<T>::is_specialized ?
      detail::epsilon<T>(boost::true_type()) :
      detail::epsilon<T>(boost::false_type());
#endif
}

namespace detail{

template <class T>
inline BOOST_MATH_CONSTEXPR T root_epsilon_imp(const boost::integral_constant<int, 24>&) BOOST_MATH_NOEXCEPT(T)
{
   return static_cast<T>(0.00034526698300124390839884978618400831996329879769945L);
}

template <class T>
inline BOOST_MATH_CONSTEXPR T root_epsilon_imp(const T*, const boost::integral_constant<int, 53>&) BOOST_MATH_NOEXCEPT(T)
{
   return static_cast<T>(0.1490116119384765625e-7L);
}

template <class T>
inline BOOST_MATH_CONSTEXPR T root_epsilon_imp(const T*, const boost::integral_constant<int, 64>&) BOOST_MATH_NOEXCEPT(T)
{
   return static_cast<T>(0.32927225399135962333569506281281311031656150598474e-9L);
}

template <class T>
inline BOOST_MATH_CONSTEXPR T root_epsilon_imp(const T*, const boost::integral_constant<int, 113>&) BOOST_MATH_NOEXCEPT(T)
{
   return static_cast<T>(0.1387778780781445675529539585113525390625e-16L);
}

template <class T, class Tag>
inline T root_epsilon_imp(const T*, const Tag&)
{
   BOOST_MATH_STD_USING
   static const T r_eps = sqrt(tools::epsilon<T>());
   return r_eps;
}

template <class T>
inline T root_epsilon_imp(const T*, const boost::integral_constant<int, 0>&)
{
   BOOST_MATH_STD_USING
   return sqrt(tools::epsilon<T>());
}

template <class T>
inline BOOST_MATH_CONSTEXPR T cbrt_epsilon_imp(const boost::integral_constant<int, 24>&) BOOST_MATH_NOEXCEPT(T)
{
   return static_cast<T>(0.0049215666011518482998719164346805794944150447839903L);
}

template <class T>
inline BOOST_MATH_CONSTEXPR T cbrt_epsilon_imp(const T*, const boost::integral_constant<int, 53>&) BOOST_MATH_NOEXCEPT(T)
{
   return static_cast<T>(6.05545445239333906078989272793696693569753008995e-6L);
}

template <class T>
inline BOOST_MATH_CONSTEXPR T cbrt_epsilon_imp(const T*, const boost::integral_constant<int, 64>&) BOOST_MATH_NOEXCEPT(T)
{
   return static_cast<T>(4.76837158203125e-7L);
}

template <class T>
inline BOOST_MATH_CONSTEXPR T cbrt_epsilon_imp(const T*, const boost::integral_constant<int, 113>&) BOOST_MATH_NOEXCEPT(T)
{
   return static_cast<T>(5.7749313854154005630396773604745549542403508090496e-12L);
}

template <class T, class Tag>
inline T cbrt_epsilon_imp(const T*, const Tag&)
{
   BOOST_MATH_STD_USING;
   static const T cbrt_eps = pow(tools::epsilon<T>(), T(1) / 3);
   return cbrt_eps;
}

template <class T>
inline T cbrt_epsilon_imp(const T*, const boost::integral_constant<int, 0>&)
{
   BOOST_MATH_STD_USING;
   return pow(tools::epsilon<T>(), T(1) / 3);
}

template <class T>
inline BOOST_MATH_CONSTEXPR T forth_root_epsilon_imp(const T*, const boost::integral_constant<int, 24>&) BOOST_MATH_NOEXCEPT(T)
{
   return static_cast<T>(0.018581361171917516667460937040007436176452688944747L);
}

template <class T>
inline BOOST_MATH_CONSTEXPR T forth_root_epsilon_imp(const T*, const boost::integral_constant<int, 53>&) BOOST_MATH_NOEXCEPT(T)
{
   return static_cast<T>(0.0001220703125L);
}

template <class T>
inline BOOST_MATH_CONSTEXPR T forth_root_epsilon_imp(const T*, const boost::integral_constant<int, 64>&) BOOST_MATH_NOEXCEPT(T)
{
   return static_cast<T>(0.18145860519450699870567321328132261891067079047605e-4L);
}

template <class T>
inline BOOST_MATH_CONSTEXPR T forth_root_epsilon_imp(const T*, const boost::integral_constant<int, 113>&) BOOST_MATH_NOEXCEPT(T)
{
   return static_cast<T>(0.37252902984619140625e-8L);
}

template <class T, class Tag>
inline T forth_root_epsilon_imp(const T*, const Tag&)
{
   BOOST_MATH_STD_USING
   static const T r_eps = sqrt(sqrt(tools::epsilon<T>()));
   return r_eps;
}

template <class T>
inline T forth_root_epsilon_imp(const T*, const boost::integral_constant<int, 0>&)
{
   BOOST_MATH_STD_USING
   return sqrt(sqrt(tools::epsilon<T>()));
}

template <class T>
struct root_epsilon_traits
{
   typedef boost::integral_constant<int, (::std::numeric_limits<T>::radix == 2) && (::std::numeric_limits<T>::digits != INT_MAX) ? std::numeric_limits<T>::digits : 0> tag_type;
   BOOST_STATIC_CONSTANT(bool, has_noexcept = (tag_type::value == 113) || (tag_type::value == 64) || (tag_type::value == 53) || (tag_type::value == 24));
};

}

template <class T>
inline BOOST_MATH_CONSTEXPR T root_epsilon() BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && detail::root_epsilon_traits<T>::has_noexcept)
{
   return detail::root_epsilon_imp(static_cast<T const*>(0), typename detail::root_epsilon_traits<T>::tag_type());
}

template <class T>
inline BOOST_MATH_CONSTEXPR T cbrt_epsilon() BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && detail::root_epsilon_traits<T>::has_noexcept)
{
   return detail::cbrt_epsilon_imp(static_cast<T const*>(0), typename detail::root_epsilon_traits<T>::tag_type());
}

template <class T>
inline BOOST_MATH_CONSTEXPR T forth_root_epsilon() BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && detail::root_epsilon_traits<T>::has_noexcept)
{
   return detail::forth_root_epsilon_imp(static_cast<T const*>(0), typename detail::root_epsilon_traits<T>::tag_type());
}

} // namespace tools
} // namespace math
} // namespace boost

#endif // BOOST_MATH_TOOLS_PRECISION_INCLUDED


// D3DDlg.cpp : implementation file
//

#include "pch.h"
#include "FaceMorph.h"
#include "DxDlg.h"
#include "vertex.h"
#include "FileHandle.h"

//#include <iostream>
//#include <Eigen/Dense>
const int LINE_BUFF_SIZE = 4096;
// CDxWnd

IMPLEMENT_DYNAMIC(CDxWnd, CWnd)

CDxWnd::CDxWnd()
{
	m_nNumVertices = 0;
	m_bWireframe = false;
	m_pVB = nullptr;
	m_pIB = nullptr;
	g_hWnd = NULL;
	g_hMainDlg = NULL;
	g_pD3D = nullptr;
	g_pD3DDevice = nullptr;
	g_pTexture = nullptr;
	g_fScale = 1.0f;
	m_pVertexPNTDecl = nullptr;
	g_pEffect = nullptr;
	m_pFaceMesh = nullptr;
	m_hTech = NULL;
	m_hWVP = NULL;
	m_hWorldInvTrans = NULL;
	m_hLightVecW = NULL;
	m_hDiffuseMtrl = NULL;
	m_hDiffuseLight = NULL;
	m_hAmbientMtrl = NULL;
	m_hAmbientLight = NULL;
	m_hSpecularMtrl = NULL;
	m_hSpecularLight = NULL;
	m_hSpecularPower = NULL;
	m_hEyePos = NULL;
	m_hWorld = NULL;
	m_hTex = NULL;
	m_LightVecW = D3DXVECTOR3(0.0, 0.8f, 1.0f);

	D3DXVec3Normalize(&m_LightVecW, &m_LightVecW);

	m_DiffuseMtrl = D3DXCOLOR(1.0f, 1.0f, 1.0f, 1.0f);
	m_DiffuseLight = D3DXCOLOR(1.0f, 1.0f, 1.0f, 1.0f);
	m_AmbientMtrl = D3DXCOLOR(1.0f, 1.0f, 1.0f, 1.0f);
	m_AmbientLight = D3DXCOLOR(0.4f, 0.4f, 0.4f, 1.0f);
	m_SpecularMtrl = D3DXCOLOR(0.8f, 0.8f, 0.8f, 1.0f);
	m_SpecularLight = D3DXCOLOR(1.0f, 1.0f, 1.0f, 1.0f);
	m_SpecularPower = 8.0f;
}
//---------------------------------------------------------------//
CDxWnd::~CDxWnd()
{
	ShutDown();
}
//---------------------------------------------------------------//
BEGIN_MESSAGE_MAP(CDxWnd, CWnd)
	ON_WM_TIMER()
END_MESSAGE_MAP()
//---------------------------------------------------------------//
// CDxWnd message handlers
bool CDxWnd::Initialize(CWnd* pParent)
{
	bool bResult = SUCCEEDED(InitD3D());
	if (bResult)
	{
		InitAllVertexDeclarations(g_pD3DDevice);
		{
			HRESULT hr = S_OK;
			if (m_pVB == nullptr)
			{
				hr = g_pD3DDevice->CreateVertexBuffer((UINT)m_nNumVertices * sizeof(VertexPNT), D3DUSAGE_WRITEONLY, 0, D3DPOOL_MANAGED, &m_pVB, 0);
			}
			if (m_pIB == nullptr)
			{
				hr = g_pD3DDevice->CreateIndexBuffer((UINT)m_vIndices.size() * sizeof(DWORD), D3DUSAGE_WRITEONLY, D3DFMT_INDEX32, D3DPOOL_MANAGED, &m_pIB, 0);
			}
			CEigenValues c;//default mesh to mean mesh only i.e. all eigenvalues set to zero)
			RecalcMesh(c);
		}
	}
	else
	{
		ATLASSERT(!L"InitD3D failed");
	}
	return bResult;
}
//---------------------------------------------------------------//
void CDxWnd::OnTimer(UINT_PTR nIDEvent)
{
	CWnd::OnTimer(nIDEvent);
}
//---------------------------------------------------------------//
void CDxWnd::RecalcMesh(CEigenValues& e)
{
	if (m_nNumVertices == 0) return;
	int nNumVerticesXYZ = m_nNumVertices * 3;
	//the following could be vectorized with Intel IPP, but seems to run fast enough
	for (int j = 0; j < nNumVerticesXYZ; j++)
	{
		float r = m_AveFace[j] / 8.0f;
		for (int h = 0; h < NUM_EIGENS; h++)
		{
			r = r + e.m_Next[h] * m_Eigen[h][j];
		}
		m_Mesh[j] = r;
	}

	if (m_pVB && m_pIB)
	{
		VertexPNT* v = 0;
		HRESULT hr = m_pVB->Lock(0, 0, (void**)&v, 0);
		for (int j = 0; j < m_nNumVertices; j++)
		{
			float x = m_Mesh[j];
			float y = m_Mesh[m_nNumVertices + j];
			float z = m_Mesh[2 * m_nNumVertices + j];
			v[j] = VertexPNT(x, y, z, 0, 0, 0, 0, 0);
		}
		{
			DWORD* k = 0;
			HRESULT hr = m_pIB->Lock(0, 0, (void**)&k, 0);
			size_t nNumTriangles = m_vIndices.size();
			for (int j = 0; j < nNumTriangles; j++)
			{
				k[j] = m_vIndices[j];
			}

			//compute normals (or at least approximate them)
			for (int j = 0; j < nNumTriangles; j++)
			{
				if (j > 1 && j < nNumTriangles - 2)
				{
					int vip = k[j - 1];//prev
					int vic = k[j];//current
					int vin = k[j + 1];//next
					D3DXVECTOR3 va = v[vip].pos;
					D3DXVECTOR3 vb = v[vic].pos;
					D3DXVECTOR3 vc = v[vin].pos;
					D3DXVECTOR3 cross;
					D3DXVECTOR3 vc1 = vb - va;
					D3DXVECTOR3 vc2 = vc - va;
					D3DXVec3Cross(&cross, &vc1, &vc2);
					D3DXVec3Normalize(&cross, &cross);
					v[vip].normal = cross;
					v[vic].normal = cross;
					v[vin].normal = cross;
				}
			}
			hr = m_pIB->Unlock();
		}
		hr = m_pVB->Unlock();
	}
}
//---------------------------------------------------------------//
void CDxWnd::Render()
{
	if (g_pD3DDevice == 0) return;

	HRESULT hr = S_OK;

	if (FAILED(g_pD3DDevice->BeginScene()))
		return;

	g_pD3DDevice->Clear(0, NULL, D3DCLEAR_TARGET | D3DCLEAR_ZBUFFER, D3DCOLOR_ARGB(0, 45, 170, 50), 1.0f, 0);
	g_pD3DDevice->SetRenderState(D3DRS_FILLMODE, D3DFILL_SOLID);
	// Draw the background gradient.
	RECT rClient;
	GetClientRect(&rClient);

	D3DXCOLOR c1(0.2f, 0.2f, 0.2f, 1.0f);
	D3DXCOLOR c2(0.45f, 0.45f, 0.45f, 1.0f);
	g_pD3DDevice->SetRenderState(D3DRS_ZWRITEENABLE, FALSE);
	DrawTransformedQuad(g_pD3DDevice, -0.5f, -0.5f, 0.f,
		(FLOAT)(rClient.right - rClient.left),
		(FLOAT)(rClient.bottom - rClient.top),
		c1, c1, c2, c2);
	g_pD3DDevice->SetRenderState(D3DRS_ZWRITEENABLE, TRUE);

	g_pD3DDevice->SetRenderState(D3DRS_CULLMODE, D3DCULL_CCW);
	if (m_bWireframe)
	{
		g_pD3DDevice->SetRenderState(D3DRS_FILLMODE, D3DFILL_WIREFRAME);
	}

	D3DXMatrixIdentity(&m_World);
	D3DXVECTOR3 vEye(0, 0, 10);
	float nRenderFOV = 12.59f;
	D3DXMatrixLookAtLH(&m_View, &vEye, &(D3DXVECTOR3(0, 0, 0)), &(D3DXVECTOR3(0, 1, 0)));
	D3DXMatrixPerspectiveFovLH(&m_Proj, nRenderFOV, 1, 1, 50);

	m_WVP = m_World * m_View * m_Proj;
	D3DXMATRIX WI;
	D3DXMatrixInverse(&WI, NULL, &m_World);
	D3DXMatrixTranspose(&m_WIT, &WI);
	hr = g_pEffect->SetTechnique(m_hTech);

	hr = g_pD3DDevice->SetVertexDeclaration( VertexPNT::Decl);
	UINT numPasses = 0;

	hr = g_pEffect->Begin(&numPasses, 0);
	for (UINT k = 0; k < numPasses; ++k)
	{
		hr = g_pEffect->BeginPass(k);
		hr = g_pEffect->SetMatrix(m_hWorldInvTrans, &m_WIT);
		hr = g_pEffect->SetMatrix(m_hWVP, &m_WVP);
		hr = g_pEffect->SetValue(m_hLightVecW, &m_LightVecW, sizeof(D3DXVECTOR3));
		hr = g_pEffect->SetValue(m_hDiffuseMtrl, &m_DiffuseMtrl, sizeof(D3DXCOLOR));
		hr = g_pEffect->SetValue(m_hDiffuseLight, &m_DiffuseLight, sizeof(D3DXCOLOR));
		hr = g_pEffect->SetValue(m_hAmbientMtrl, &m_AmbientMtrl, sizeof(D3DXCOLOR));
		hr = g_pEffect->SetValue(m_hAmbientLight, &m_AmbientLight, sizeof(D3DXCOLOR));
		hr = g_pEffect->SetValue(m_hSpecularLight, &m_SpecularLight, sizeof(D3DXCOLOR));
		hr = g_pEffect->SetValue(m_hSpecularMtrl, &m_SpecularMtrl, sizeof(D3DXCOLOR));
		hr = g_pEffect->SetFloat(m_hSpecularPower, m_SpecularPower);
		hr = g_pEffect->SetMatrix(m_hWorld, &m_World);
		hr = g_pEffect->SetTexture(m_hTex, g_pTexture);
		hr = g_pEffect->SetValue(m_hEyePos, &vEye, sizeof(D3DXVECTOR3));
		g_pEffect->CommitChanges();
		hr = g_pD3DDevice->SetStreamSource(0, m_pVB, 0, sizeof(VertexPNT));
		hr = g_pD3DDevice->SetIndices(m_pIB);
		hr = g_pD3DDevice->DrawIndexedPrimitive(D3DPT_TRIANGLELIST, 0, 0, m_nNumVertices, 0, (UINT)m_vIndices.size() / 3);
		if (FAILED(hr))
		{
			ATLASSERT(0);
		}
		hr = g_pEffect->EndPass();
	}
	g_pD3DDevice->EndScene();
	g_pD3DDevice->Present(NULL, NULL, NULL, NULL);
}
//---------------------------------------------------------------//
// Initialize Direct3D stuff.
HRESULT CDxWnd::InitD3D()
{
	HRESULT hr;

	g_pD3D = Direct3DCreate9(D3D_SDK_VERSION);

	if (!g_pD3D)
		return E_FAIL;

	D3DDISPLAYMODE d3ddm;

	g_pD3D->GetAdapterDisplayMode(D3DADAPTER_DEFAULT, &d3ddm);

	D3DPRESENT_PARAMETERS d3dpp;
	ZeroMemory(&d3dpp, sizeof(d3dpp));

	d3dpp.Windowed = TRUE;
	d3dpp.SwapEffect = D3DSWAPEFFECT_DISCARD;
	d3dpp.BackBufferFormat = d3ddm.Format;
	d3dpp.EnableAutoDepthStencil = TRUE;
	d3dpp.AutoDepthStencilFormat = D3DFMT_D16;
	d3dpp.PresentationInterval = D3DPRESENT_INTERVAL_ONE; // v-sync on - No need to burn the CPU and GPU over nothing.
	d3dpp.hDeviceWindow = m_hWnd;

	if (FAILED(hr = g_pD3D->CreateDevice(D3DADAPTER_DEFAULT, D3DDEVTYPE_HAL,
		m_hWnd, D3DCREATE_HARDWARE_VERTEXPROCESSING, &d3dpp, &g_pD3DDevice)))
		return hr;

	return OnCreateDevice();
}
//---------------------------------------------------------------//
// This is called after the device has been created.
HRESULT CDxWnd::OnCreateDevice()
{
	HRESULT hr;
	CString strFileName;
	TCHAR szFilePath[MAX_PATH + _ATL_QUOTES_SPACE];
	DWORD dwFLen = ::GetModuleFileName(NULL, szFilePath + 0, MAX_PATH);
	if (dwFLen == 0 || dwFLen == MAX_PATH)
	{
		ATLASSERT(0);
		return E_FAIL;
	}
	else
	{
		strFileName = CString(szFilePath);
		bool bFoundFX = false;
		int nNumAttempts = 0;
		//search for the .fx file. Might be better to embed it as a resource
		while (!bFoundFX && nNumAttempts < 3)
		{
			long nRight = strFileName.ReverseFind(_T('\\'));//move one folder up
			strFileName = strFileName.Left(nRight);
			nNumAttempts++;
			if (CFileHelper::DoesFileExist(strFileName + L"\\FaceMorph.fx")) {
				bFoundFX = true;
				strFileName.Append(L"\\FaceMorph.fx");
			}
			else if (CFileHelper::DoesFileExist(strFileName + L"\\FaceMorph\\FaceMorph.fx")) {
				bFoundFX = true;
				strFileName.Append(L"\\FaceMorph\\FaceMorph.fx");
			}
		}
	}
	// Load the effect from file.
	LPD3DXBUFFER pErrors = NULL;
	hr = D3DXCreateEffectFromFile(g_pD3DDevice, strFileName, NULL, NULL, 0, NULL, &g_pEffect, &pErrors);
	if (FAILED(hr))
	{
		if (pErrors)
		{
			CA2T tErrors = (char*)pErrors->GetBufferPointer();
			AfxMessageBox((LPCTSTR)tErrors, MB_OK | MB_ICONSTOP);
			pErrors->Release();
		}
		return hr;
	}
	strFileName.Replace(_T("FaceMorph.fx"), _T("texture.png"));
	hr = D3DXCreateTextureFromFile(g_pD3DDevice, strFileName, &g_pTexture);

	m_hTech				= g_pEffect->GetTechniqueByName("DirLightTexTech");
	m_hWVP				= g_pEffect->GetParameterByName(0, "gWVP");
	m_hWorldInvTrans	= g_pEffect->GetParameterByName(0, "gWorldInvTrans");
	m_hLightVecW		= g_pEffect->GetParameterByName(0, "gLightVecW");
	m_hDiffuseMtrl		= g_pEffect->GetParameterByName(0, "gDiffuseMtrl");
	m_hDiffuseLight		= g_pEffect->GetParameterByName(0, "gDiffuseLight");
	m_hAmbientMtrl		= g_pEffect->GetParameterByName(0, "gAmbientMtrl");
	m_hAmbientLight		= g_pEffect->GetParameterByName(0, "gAmbientLight");
	m_hSpecularMtrl		= g_pEffect->GetParameterByName(0, "gSpecularMtrl");
	m_hSpecularLight	= g_pEffect->GetParameterByName(0, "gSpecularLight");
	m_hSpecularPower	= g_pEffect->GetParameterByName(0, "gSpecularPower");
	m_hEyePos			= g_pEffect->GetParameterByName(0, "gEyePosW");
	m_hWorld			= g_pEffect->GetParameterByName(0, "gWorld");
	m_hTex				= g_pEffect->GetParameterByName(0, "gTex");

	D3DVERTEXELEMENT9 VertexPNTElements[] =
	{
		{0, 0,  D3DDECLTYPE_FLOAT3, D3DDECLMETHOD_DEFAULT, D3DDECLUSAGE_POSITION, 0},
		{0, 12, D3DDECLTYPE_FLOAT3, D3DDECLMETHOD_DEFAULT, D3DDECLUSAGE_NORMAL, 0},
		{0, 24, D3DDECLTYPE_FLOAT2, D3DDECLMETHOD_DEFAULT, D3DDECLUSAGE_TEXCOORD, 0},
		D3DDECL_END()
	};
	hr = g_pD3DDevice->CreateVertexDeclaration(VertexPNTElements, &m_pVertexPNTDecl);

	return S_OK;
}
//---------------------------------------------------------------//
void CDxWnd::ShutDown()
{
	SAFE_RELEASE(g_pTexture);

	SAFE_RELEASE(g_pEffect);
	SAFE_RELEASE(m_pFaceMesh);
	SAFE_RELEASE(m_pVB);
	SAFE_RELEASE(m_pIB);

	SAFE_RELEASE(g_pD3DDevice);
	SAFE_RELEASE(g_pD3D);
}
//---------------------------------------------------------------//
HRESULT CDxWnd::DrawTransformedQuad(LPDIRECT3DDEVICE9 pDevice,
	FLOAT x, FLOAT y, FLOAT z,
	FLOAT width, FLOAT height,
	D3DCOLOR c1,
	D3DCOLOR c2,
	D3DCOLOR c3,
	D3DCOLOR c4)
{
	return DrawTransformedQuad(pDevice, x, y, z, width, height,
		D3DXVECTOR2(0, 0), D3DXVECTOR2(1, 0), D3DXVECTOR2(0, 1), D3DXVECTOR2(1, 1),
		c1, c2, c3, c4);
}
//---------------------------------------------------------------//
HRESULT CDxWnd::DrawTransformedQuad(LPDIRECT3DDEVICE9 pDevice,
	FLOAT x, FLOAT y, FLOAT z,
	FLOAT width, FLOAT height,
	D3DXVECTOR2 uvTopLeft, D3DXVECTOR2 uvTopRight,
	D3DXVECTOR2 uvBottomLeft, D3DXVECTOR2 uvBottomRight,
	D3DCOLOR c1, D3DCOLOR c2, D3DCOLOR c3, D3DCOLOR c4)
{
	struct
	{
		float pos[4];
		D3DCOLOR color;
		float uv[2];
	} quad[] =
	{
		x,			y,				z, 1.f,		c1, uvTopLeft.x,		uvTopLeft.y,
		x + width,	y,				z, 1.f,		c2, uvBottomRight.x,	uvTopLeft.y,
		x,			y + height,		z, 1.f,		c3, uvTopLeft.x,		uvBottomRight.y,
		x + width,	y + height,		z, 1.f,		c4, uvBottomRight.x,	uvBottomRight.y,
	};
	pDevice->SetFVF(D3DFVF_XYZRHW | D3DFVF_DIFFUSE | D3DFVF_TEX1 | D3DFVF_TEXCOORDSIZE2(0));
	return pDevice->DrawPrimitiveUP(D3DPT_TRIANGLESTRIP, 2, quad, sizeof(quad[0]));
}
//---------------------------------------------------------------//


#include "Expression.h"
#include "mapnikvt/StringUtils.h"

#include <stdexcept>
#include <utility>

#include <boost/lexical_cast.hpp>

namespace {
    using carto::css::Color;
    using carto::css::Value;

    struct NotOp {
        Value operator() (bool val) const { return Value(!val); }
        template <typename T> Value operator() (T val) const { throw std::invalid_argument("Unexpected type in ! operator"); }
    };

    struct NegOp {
        Value operator() (long long val) const { return Value(-val); }
        Value operator() (double val) const { return Value(-val); }
        template <typename T> Value operator() (T val) const { throw std::invalid_argument("Unexpected type in - operator"); }
    };

    struct AndOp {
        Value operator() (bool val1, bool val2) const { return Value(val1 && val2); }
        template <typename S, typename T> Value operator() (S val1, T val2) const { throw std::invalid_argument("Unexpected types in binary && operator"); }
    };

    struct OrOp {
        Value operator() (bool val1, bool val2) const { return Value(val1 || val2); }
        template <typename S, typename T> Value operator() (S val1, T val2) const { throw std::invalid_argument("Unexpected types in binary || operator"); }
    };

    template <template <typename> class OpImpl, bool NullResult, bool MismatchResult>
    struct CompOp  {
        Value operator() (std::monostate, std::monostate) const { return Value(NullResult); }
        Value operator() (bool val1, long long val2) const { return Value(OpImpl<long long>()(static_cast<long long>(val1), val2)); }
        Value operator() (bool val1, double val2) const { return Value(OpImpl<double>()(static_cast<double>(val1), val2)); }
        Value operator() (long long val1, bool val2) const { return Value(OpImpl<long long>()(val1, static_cast<long long>(val2))); }
        Value operator() (long long val1, double val2) const { return Value(OpImpl<double>()(static_cast<double>(val1), val2)); }
        Value operator() (double val1, bool val2) const { return Value(OpImpl<double>()(val1, static_cast<double>(val2))); }
        Value operator() (double val1, long long val2) const { return Value(OpImpl<double>()(val1, static_cast<double>(val2))); }
        template <typename T> Value operator() (T val1, T val2) const { return Value(OpImpl<T>()(val1, val2)); }
        template <typename S, typename T> Value operator() (S val1, T val2) const { return Value(MismatchResult); }
    };

    struct MatchOp {
        Value operator() (const std::string& val1, const std::string& val2) const { return Value(carto::mvt::regexMatch(val1, val2)); }
        template <typename S, typename T> Value operator() (S val1, T val2) const { return Value(false); }
    };

    struct AddOp {
        Value operator() (const std::string& val1, const std::string& val2) const { return Value(val1 + val2); }
        Value operator() (const std::string& val1, std::monostate) const { return Value(val1); }
        Value operator() (std::monostate, const std::string& val2) const { return Value(val2); }
        template <typename T> Value operator() (const std::string& val1, T val2) const { return Value(val1 + boost::lexical_cast<std::string>(val2)); }
        template <typename S> Value operator() (S val1, const std::string& val2) const { return Value(boost::lexical_cast<std::string>(val1) + val2); }
        Value operator() (long long val1, long long val2) const { return Value(val1 + val2); }
        Value operator() (long long val1, double val2) const { return Value(static_cast<double>(val1) + val2); }
        Value operator() (double val1, long long val2) const { return Value(val1 + static_cast<double>(val2)); }
        Value operator() (double val1, double val2) const { return Value(val1 + val2); }
        Value operator() (Color val1, Color val2) const { return Value(val1 + val2); }
        template <typename S, typename T> Value operator() (S val1, T val2) const { throw std::invalid_argument("Unexpected types in binary + operator"); }
    };

    struct SubOp {
        Value operator() (long long val1, long long val2) const { return Value(val1 - val2); }
        Value operator() (long long val1, double val2) const { return Value(static_cast<double>(val1) - val2); }
        Value operator() (double val1, long long val2) const { return Value(val1 - static_cast<double>(val2)); }
        Value operator() (double val1, double val2) const { return Value(val1 - val2); }
        Value operator() (Color val1, Color val2) const { return Value(val1 - val2); }
        template <typename S, typename T> Value operator() (S val1, T val2) const { throw std::invalid_argument("Unexpected types in binary - operator"); }
    };

    struct MulOp {
        Value operator() (long long val1, long long val2) const { return Value(val1 * val2); }
        Value operator() (long long val1, double val2) const { return Value(static_cast<double>(val1) * val2); }
        Value operator() (double val1, long long val2) const { return Value(val1 * static_cast<double>(val2)); }
        Value operator() (double val1, double val2) const { return Value(val1 * val2); }
        Value operator() (Color val1, Color val2) const { return Value(val1 * val2); }
        Value operator() (Color val1, long long val2) const { return Value(val1 * static_cast<float>(val2)); }
        Value operator() (Color val1, double val2) const { return Value(val1 * static_cast<float>(val2)); }
        Value operator() (long long val1, Color val2) const { return Value(static_cast<float>(val1) * val2); }
        Value operator() (double val1, Color val2) const { return Value(static_cast<float>(val1) * val2); }
        template <typename S, typename T> Value operator() (S val1, T val2) const { throw std::invalid_argument("Unexpected types in binary * operator"); }
    };

    struct DivOp {
        Value operator() (long long val1, long long val2) const { return val2 == 0 ? Value() : Value(static_cast<double>(val1) / static_cast<double>(val2)); }
        Value operator() (long long val1, double val2) const { return Value(static_cast<double>(val1) / val2); }
        Value operator() (double val1, long long val2) const { return Value(val1 / static_cast<double>(val2)); }
        Value operator() (double val1, double val2) const { return Value(val1 / val2); }
        Value operator() (Color val1, Color val2) const { return Value(val1 / val2); }
        Value operator() (Color val1, long long val2) const { return Value(val1 * (1.0f / static_cast<float>(val2))); }
        Value operator() (Color val1, double val2) const { return Value(val1 * (1.0f / static_cast<float>(val2))); }
        template <typename S, typename T> Value operator() (S val1, T val2) const { throw std::invalid_argument("Unexpected types in binary / operator"); }
    };
}

namespace carto { namespace css {
    Value UnaryExpression::applyOp(Op op, const Value& val) {
        switch (op) {
        case Op::NOT:
            return std::visit(NotOp(), val);
        case Op::NEG:
            return std::visit(NegOp(), val);
        }
        throw std::invalid_argument("Unsupported unary operation");
    }

    Value BinaryExpression::applyOp(Op op, const Value& val1, const Value& val2) {
        switch (op) {
        case Op::AND:
            return std::visit(AndOp(), val1, val2);
        case Op::OR:
            return std::visit(OrOp(), val1, val2);
        case Op::EQ:
            return std::visit(CompOp<std::equal_to, true, false>(), val1, val2);
        case Op::NEQ:
            return std::visit(CompOp<std::not_equal_to, false, true>(), val1, val2);
        case Op::LT:
            return std::visit(CompOp<std::less, false, false>(), val1, val2);
        case Op::LTE:
            return std::visit(CompOp<std::less_equal, true, false>(), val1, val2);
        case Op::GT:
            return std::visit(CompOp<std::greater, false, false>(), val1, val2);
        case Op::GTE:
            return std::visit(CompOp<std::greater_equal, true, false>(), val1, val2);
        case Op::MATCH:
            return std::visit(MatchOp(), val1, val2);
        case Op::ADD:
            return std::visit(AddOp(), val1, val2);
        case Op::SUB:
            return std::visit(SubOp(), val1, val2);
        case Op::MUL:
            return std::visit(MulOp(), val1, val2);
        case Op::DIV:
            return std::visit(DivOp(), val1, val2);
        }
        throw std::invalid_argument("Unsupported binary operation");
    }

    Value FunctionExpression::applyFunc(const std::string& func, const std::vector<Value>& vals) {
        if (func == "url" && vals.size() == 1) {
            return Value(getString(vals[0]));
        }
        else if (func == "rgb" && vals.size() == 3) {
            std::array<float, 3> components;
            std::transform(vals.begin(), vals.end(), components.begin(), [](const Value& val) { return getFloat(val) / 255.0f; });
            Color color = Color::fromRGBA(components[0], components[1], components[2], 1.0f);
            return Value(color);
        }
        else if (func == "rgba" && vals.size() == 4) {
            std::array<float, 4> components;
            std::transform(vals.begin(), vals.begin() + 3, components.begin(), [](const Value& val) { return getFloat(val) / 255.0f; });
            components[3] = getFloat(vals[3]);
            Color color = Color::fromRGBA(components[0], components[1], components[2], components[3]);
            return Value(color);
        }
        else if (func == "mix" && vals.size() == 3) {
            Color color = Color::mix(getColor(vals[0]), getColor(vals[1]), getFloat(vals[2]));
            return Value(color);
        }
        else if (func == "lighten" && vals.size() == 2) {
            Color color = Color::lighten(getColor(vals[0]), getFloat(vals[1]));
            return Value(color);
        }
        else if (func == "darken" && vals.size() == 2) {
            Color color = Color::lighten(getColor(vals[0]), -getFloat(vals[1]));
            return Value(color);
        }
        else if (func == "saturate" && vals.size() == 2) {
            Color color = Color::saturate(getColor(vals[0]), getFloat(vals[1]));
            return Value(color);
        }
        else if (func == "desaturate" && vals.size() == 2) {
            Color color = Color::saturate(getColor(vals[0]), -getFloat(vals[1]));
            return Value(color);
        }
        else if (func == "fadein" && vals.size() == 2) {
            Color color = Color::fade(getColor(vals[0]), getFloat(vals[1]));
            return Value(color);
        }
        else if (func == "fadeout" && vals.size() == 2) {
            Color color = Color::fade(getColor(vals[0]), -getFloat(vals[1]));
            return Value(color);
        }
        return Value();
    }

    Color FunctionExpression::getColor(const Value& value) {
        if (auto colorVal = std::get_if<Color>(&value)) {
            return *colorVal;
        }
        throw std::invalid_argument("Wrong type, expecting color");
    }

    float FunctionExpression::getFloat(const Value& value) {
        if (auto longVal = std::get_if<long long>(&value)) {
            return static_cast<float>(*longVal);
        }
        else if (auto doubleVal = std::get_if<double>(&value)) {
            return static_cast<float>(*doubleVal);
        }
        throw std::invalid_argument("Wrong type, expecting float");
    }

    std::string FunctionExpression::getString(const Value& value) {
        if (auto strVal = std::get_if<std::string>(&value)) {
            return *strVal;
        }
        throw std::invalid_argument("Wrong type, expecting string");
    }
} }


#include "shape_reconstruction/ShapeRecPlotter.h"

#include <pcl/conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter_indices.h>
#include <sensor_msgs/Image.h>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>

#include <pcl_conversions/pcl_conversions.h>

#include <ros/package.h>

#include <cmath>

#include <ros/console.h>

#include "shape_reconstruction/RangeImagePlanar.hpp"

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/passthrough.h>

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

#include <std_msgs/Bool.h>

#include <iostream>
#include <pcl/common/common.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/point_types.h>
#include <pcl/surface/mls.h>
#include <pcl/surface/poisson.h>
#include <pcl/surface/marching_cubes.h>
#include <pcl/surface/marching_cubes_rbf.h>
#include <pcl/surface/marching_cubes_hoppe.h>
#include <pcl/surface/ear_clipping.h>
#include <pcl/surface/vtk_smoothing/vtk_mesh_smoothing_laplacian.h>
#include <pcl/surface/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.h>
#include <pcl/surface/vtk_smoothing/vtk_mesh_subdivision.h>
#include <pcl/filters/passthrough.h>

#include <pcl/io/vtk_lib_io.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <vcg/complex/complex.h> //class UpdateCurvature
#include <vcg/complex/all_types.h>     //class UpdateNormals
#include <vcg/complex/algorithms/clean.h> //class UpdateCurvature

#include <vcg/complex/algorithms/update/bounding.h> //class UpdateCurvature
#include <vcg/complex/algorithms/update/normal.h> //class UpdateCurvature
#include <vcg/complex/algorithms/update/topology.h> //class UpdateCurvature

#include <pcl/surface/convex_hull.h>
#include <pcl/surface/concave_hull.h>

#include <pcl/filters/normal_refinement.h>

#include <pcl/surface/simplification_remove_unused_vertices.h>

#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>

#include <pcl_ros/transforms.h>

using namespace omip;

ShapeRecPlotter::ShapeRecPlotter()
{
    std::string topic_name = std::string("");
    //this->_node_handle.getParam("/surface_smoother/topic_name", topic_name);
    ROS_INFO_STREAM_NAMED("ShapeRecPlotter","topic_name: " << topic_name);
    this->_pc_subscriber = this->_node_handle.subscribe(topic_name, 1,
                                                        &ShapeRecPlotter::InputPCmeasurementCallback, this);

    std::string topic_name2 = std::string("moved_pc");
    this->_pc_publisher = this->_node_handle.advertise<sensor_msgs::PointCloud2>(topic_name2,10, true);

    this->_current_pc.reset(new SRPointCloud());

    _tf_listener = new tf::TransformListener();

}

ShapeRecPlotter::~ShapeRecPlotter()
{
}

void ShapeRecPlotter::plotShape()
{
    if(_current_pc->size())
    {
    sensor_msgs::PointCloud2 pc2, pcout;

    pcl::toROSMsg(*this->_current_pc, pc2);

    _tf_listener->waitForTransform("/camera_rgb_optical_frame", "/ip/rb2", ros::Time(0), ros::Duration(0.1));

    bool nooo = false;
    tf::StampedTransform tfTransform;
    try{

    _tf_listener->lookupTransform ("/camera_rgb_optical_frame", "/ip/rb2",ros::Time(0), tfTransform);

    }catch(...){
        nooo = true;
        std::cout << "." << std::endl ;
    }

    if(!nooo)
    {
        std::cout << std::endl ;
    Eigen::Matrix4f transform;
    transform(0,0) = tfTransform.getBasis()[0][0];
    transform(0,1) = tfTransform.getBasis()[0][1];
    transform(0,2) = tfTransform.getBasis()[0][2];
    transform(1,0) = tfTransform.getBasis()[1][0];
    transform(1,1) = tfTransform.getBasis()[1][1];
    transform(1,2) = tfTransform.getBasis()[1][2];
    transform(2,0) = tfTransform.getBasis()[2][0];
    transform(2,1) = tfTransform.getBasis()[2][1];
    transform(2,2) = tfTransform.getBasis()[2][2];
    transform(0,3) = tfTransform.getOrigin()[0];
    transform(1,3) = tfTransform.getOrigin()[1];
    transform(2,3) = tfTransform.getOrigin()[2];
    transform(3,0) = 0;
    transform(3,1) = 0;
    transform(3,2) = 0;
    transform(3,3) = 1;

    std::cout << transform << std::endl;
    pcl_ros::transformPointCloud(transform, pc2, pcout);

    _pc_publisher.publish(pcout);
    }else{
        std::cout << std::endl ;
    Eigen::Matrix4f transform;
    transform(0,0) = 1;
    transform(0,1) = 0;
    transform(0,2) = 0;
    transform(1,0) = 0;
    transform(1,1) = 1;
    transform(1,2) = 0;
    transform(2,0) = 0;
    transform(2,1) = 0;
    transform(2,2) = 1;
    transform(0,3) = 0;
    transform(1,3) = 0;
    transform(2,3) = 0;
    transform(3,0) = 0;
    transform(3,1) = 0;
    transform(3,2) = 0;
    transform(3,3) = 1;
    pcl_ros::transformPointCloud(transform, pc2, pcout);

    _pc_publisher.publish(pcout);
    }
    }

}

void ShapeRecPlotter::InputPCmeasurementCallback(const sensor_msgs::PointCloud2ConstPtr &pc_msg)
{
    std::cout << "received" << std::endl;
    // Convert ROS PC message into a pcl point cloud
    pcl::fromROSMsg(*pc_msg, *this->_current_pc);

}

// Main program
int main(int argc, char** argv)
{
    ros::init(argc, argv, "ShapeRecPlotter");
    ShapeRecPlotter sr_node;

    ros::Rate r(30); // 10 hz
    while (ros::ok())
    {
        ros::spinOnce();
        sr_node.plotShape();
        r.sleep();
    }

    std::cout << " Shutting down ShapeRecPlotter " << std::endl;

    return (0);
}


// Copyright (c) 2015-2018 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#ifndef cell_caller_hpp
#define cell_caller_hpp

#include <vector>
#include <string>
#include <memory>
#include <functional>

#include <boost/optional.hpp>

#include "config/common.hpp"
#include "basics/phred.hpp"
#include "core/types/haplotype.hpp"
#include "core/types/genotype.hpp"
#include "core/models/mutation/coalescent_model.hpp"
#include "core/models/genotype/genotype_prior_model.hpp"
#include "core/models/genotype/single_cell_model.hpp"
#include "caller.hpp"

namespace octopus {

class GenomicRegion;
class ReadPipe;
class Variant;
class HaplotypeLikelihoodArray;
class VariantCall;

class CellCaller : public Caller
{
public:
    using Caller::CallTypeSet;

    struct Parameters
    {
        unsigned ploidy;
        boost::optional<CoalescentModel::Parameters> prior_model_params;
        Phred<double> min_variant_posterior, min_refcall_posterior;
        bool deduplicate_haplotypes_with_prior_model = false;
        unsigned max_clones;
        unsigned max_genotypes, max_joint_genotypes;
        double dropout_concentration;
        DeNovoModel::Parameters mutation_model_parameters;
        boost::optional<unsigned> max_vb_seeds = boost::none; // Use default if none
    };

    CellCaller() = delete;

    CellCaller(Caller::Components&& components,
               Caller::Parameters general_parameters,
               Parameters specific_parameters);

    CellCaller(const CellCaller&)            = delete;
    CellCaller& operator=(const CellCaller&) = delete;
    CellCaller(CellCaller&&)                 = delete;
    CellCaller& operator=(CellCaller&&)      = delete;

    ~CellCaller() = default;

private:
    class Latents;
    friend Latents;

    Parameters parameters_;

    std::string do_name() const override;
    CallTypeSet do_call_types() const override;
    unsigned do_min_callable_ploidy() const override;
    unsigned do_max_callable_ploidy() const override;

    std::size_t do_remove_duplicates(std::vector<Haplotype>& haplotypes) const override;

    std::unique_ptr<Caller::Latents>
    infer_latents(const std::vector<Haplotype>& haplotypes,
                  const HaplotypeLikelihoodArray& haplotype_likelihoods) const override;

    boost::optional<double>
    calculate_model_posterior(const std::vector<Haplotype>& haplotypes,
                              const HaplotypeLikelihoodArray& haplotype_likelihoods,
                              const Caller::Latents& latents) const override;

    boost::optional<double>
    calculate_model_posterior(const std::vector<Haplotype>& haplotypes,
                              const HaplotypeLikelihoodArray& haplotype_likelihoods,
                              const Latents& latents) const;

    std::vector<std::unique_ptr<VariantCall>>
    call_variants(const std::vector<Variant>& candidates, const Caller::Latents& latents) const override;

    std::vector<std::unique_ptr<VariantCall>>
    call_variants(const std::vector<Variant>& candidates, const Latents& latents) const;

    std::vector<std::unique_ptr<ReferenceCall>>
    call_reference(const std::vector<Allele>& alleles, const Caller::Latents& latents,
                   const ReadPileupMap& pileup) const override;

    std::vector<std::unique_ptr<ReferenceCall>>
    call_reference(const std::vector<Allele>& alleles, const Latents& latents,
                   const ReadPileupMap& pileup) const;

    std::unique_ptr<GenotypePriorModel> make_prior_model(const std::vector<Haplotype>& haplotypes) const;
};

class CellCaller::Latents : public Caller::Latents
{
public:
    using Caller::Latents::HaplotypeProbabilityMap;
    using Caller::Latents::GenotypeProbabilityMap;

    Latents() = delete;

    Latents(const CellCaller& caller,
            std::vector<Haplotype> haplotypes,
            std::vector<Genotype<Haplotype>> genotypes,
            std::vector<model::SingleCellModel::Inferences> inferences);

    std::shared_ptr<HaplotypeProbabilityMap> haplotype_posteriors() const noexcept override;
    std::shared_ptr<GenotypeProbabilityMap> genotype_posteriors() const noexcept override;

private:
    mutable std::shared_ptr<GenotypeProbabilityMap> genotype_posteriors_;
    mutable std::shared_ptr<HaplotypeProbabilityMap> haplotype_posteriors_;

    const CellCaller& caller_;
    std::vector<Haplotype> haplotypes_;
    std::vector<Genotype<Haplotype>> genotypes_;
    std::vector<model::SingleCellModel::Inferences> phylogeny_inferences_;
    std::vector<double> phylogeny_posteriors_;

    friend CellCaller;
};

} // namespace octopus

#endif


#include <iostream>
#include <fstream>
#include <vector>
#include <ctime>
#include <stdlib.h>
#include <time.h>
#include <algorithm>
#include <deque>
#include <unordered_set>
#include <math.h>
//PCL
#include <pcl/common/transforms.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/impl/passthrough.hpp>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/normal_3d.h>

#include <pcl/visualization/impl/point_cloud_geometry_handlers.hpp>//自定义点云类型时要加

//Eigen
#include <Eigen/Dense>
#include <queue>

#include <unordered_map>

#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>

//ROS
#include <ros/ros.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/Image.h>

using namespace std;

#include "jpc_groundremove.h"

int get_quadrant(pcl::PointXYZI point)
{
	int res = 0;
	float x = point.x;
	float y = point.y;
	if (x > 0 && y >= 0)
		res = 1;
	else if (x <= 0 && y > 0)
		res = 2;
	else if (x < 0 && y <= 0)
		res = 3;
	else if (x >= 0 && y < 0)
		res = 4;
	return res;
}

void add_ring_info(pcl::PointCloud<pcl::PointXYZI>& input, pcl::PointCloud<PointXYZIR>& output)
{
	int previous_quadrant = 0;
	uint16_t ring_ = (uint16_t)64-1;
	for (pcl::PointCloud<pcl::PointXYZI>::iterator pt = input.points.begin(); pt < input.points.end()-1; ++pt){
		int quadrant = get_quadrant(*pt);
		if (quadrant == 1 && previous_quadrant == 4 && ring_ > 0)
			ring_ -= 1;
		PointXYZIR point;
		point.x = pt->x;
		point.y = pt->y;
		point.z = pt->z;
		point.intensity = pt->intensity;
		point.ring = ring_;
		output.push_back(point);
		previous_quadrant = quadrant;
	}
}

class SubscribeAndPublish {
public:
	SubscribeAndPublish(ros::NodeHandle nh, std::string lidar_topic_name);

	~SubscribeAndPublish(){
	}

	void callback(const sensor_msgs::PointCloud2ConstPtr& cloudmsg) {

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

   		pcl::fromROSMsg(*cloudmsg, *cloud);

		pcl::PointCloud<PointXYZIR>::Ptr cloud_ring(new pcl::PointCloud<PointXYZIR>);
		add_ring_info(*cloud, *cloud_ring);

		ROS_INFO("Reciving data!");

		pcl::PointCloud<PointXYZIR>::Ptr cloud_gr(new pcl::PointCloud<PointXYZIR>);
		pcl::PointCloud<PointXYZIR>::Ptr cloud_ob(new pcl::PointCloud<PointXYZIR>);
		cv::Mat range_image;

		JpcGroundRemove groundremove;
		groundremove.GroundRemove(*cloud_ring, *cloud_gr, *cloud_ob, range_image);
		ROS_INFO("segmentation");

		sensor_msgs::ImagePtr imgmsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8",range_image).toImageMsg();
		pubimage_.publish(imgmsg);

		sensor_msgs::PointCloud2 ros_cloud2;
		pcl::toROSMsg(*cloud_gr, ros_cloud2);
		ros_cloud2.header.frame_id = "global_init_frame";
		pub_.publish(ros_cloud2);

		sensor_msgs::PointCloud2 ros_cloud3;
		pcl::toROSMsg(*cloud_ob, ros_cloud3);
		ros_cloud3.header.frame_id = "global_init_frame";
		pub2_.publish(ros_cloud3);

	}
private:
	ros::NodeHandle n_;
	ros::Publisher pub_;
	ros::Publisher pub2_;
	ros::Subscriber sub_;
  	image_transport::Publisher pubimage_;
};

SubscribeAndPublish::SubscribeAndPublish(ros::NodeHandle nh,
		std::string lidar_topic_name) :
		n_(nh){

	pub_  = nh.advertise < sensor_msgs::PointCloud2 > ("/ground_point", 1);
	pub2_ = nh.advertise < sensor_msgs::PointCloud2 > ("/obstacle_point", 1);
	sub_ = nh.subscribe(lidar_topic_name, 10, &SubscribeAndPublish::callback, this);
  	image_transport::ImageTransport it(nh);
	pubimage_ = it.advertise("/seg_image", 1);
}

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

 	ros::init(argc, argv, "jpc_seg_node");
	SubscribeAndPublish SAPObject(ros::NodeHandle(), "pointcloud");
	ROS_INFO("waiting for data!");
	ros::spin();
	return 0;
}


#include <iostream>
#include <sstream>
#include <sys/resource.h>
#include <thread>
#include <algorithm>
//#include <boost/asio/thread_pool.hpp>
//#include <boost/asio/post.hpp>
//#include <boost/bind.hpp>
#include <mutex>
#include "emissionprobabilitycomputer.hpp"
#include "copynumber.hpp"
#include "variantreader.hpp"
#include "uniquekmercomputer.hpp"
#include "hmm.hpp"
#include "commandlineparser.hpp"
#include "timer.hpp"
#include "threadpool.hpp"
#include <cassert>

using namespace std;

// TODO
/** version of the main algorithm that uses only path information for genotyping and ignores kmers **/

struct Results {
	mutex result_mutex;
	map<string, vector<GenotypingResult>> result;
	map<string, double> runtimes;
};

void run_genotyping_paths(string chromosome, KmerCounter* genomic_kmer_counts, KmerCounter* read_kmer_counts, VariantReader* variant_reader, ProbabilityTable* probs, size_t kmer_abundance_peak, bool only_genotyping, bool only_phasing, long double effective_N, Results* results) {
	Timer timer;
	// determine sets of kmers unique to each variant region
	UniqueKmerComputer kmer_computer(genomic_kmer_counts, read_kmer_counts, variant_reader, chromosome, kmer_abundance_peak);
	std::vector<UniqueKmers*> unique_kmers;
	kmer_computer.compute_empty(&unique_kmers);
	// construct HMM and run genotyping/phasing
	HMM hmm(&unique_kmers, probs, !only_phasing, !only_genotyping, 1.26, false, effective_N);
	// store the results
	{
		lock_guard<mutex> lock (results->result_mutex);
		results->result.insert(pair<string, vector<GenotypingResult>> (chromosome, move(hmm.get_genotyping_result())));
	}
	// destroy unique kmers
	for (size_t i = 0; i < unique_kmers.size(); ++i) {
		delete unique_kmers[i];
		unique_kmers[i] = nullptr;
	}
	lock_guard<mutex> lock (results->result_mutex);
	results->runtimes.insert(pair<string,double>(chromosome, timer.get_total_time()));
}

int main (int argc, char* argv[])
{
	Timer timer;
	double time_preprocessing;
	double time_writing;
	double time_total;

	cerr << endl;
	cerr << "program: PanGenie-paths - genotyping and phasing based on known haplotype paths." << endl;
	cerr << "author: Jana Ebler" << endl << endl;
	string reffile = "";
	string vcffile = "";
	string outname = "result";
	string sample_name = "sample";
	size_t nr_core_threads = 1;
	bool only_genotyping = false;
	bool only_phasing = false;
	double effective_N = 0.00001L;
	bool add_reference = true;

	// parse the command line arguments
	CommandLineParser argument_parser;
	argument_parser.add_command("PanGenie-paths [options] -r <reference.fa> -v <variants.vcf>");
	argument_parser.add_mandatory_argument('r', "reference genome in FASTA format");
	argument_parser.add_mandatory_argument('v', "variants in VCF format");
	argument_parser.add_optional_argument('o', "result", "prefix of the output files");
	argument_parser.add_optional_argument('s', "sample", "name of the sample (will be used in the output VCFs)");
	argument_parser.add_optional_argument('t', "1", "number of threads to use for core algorithm. Largest number of threads possible is the number of chromosomes given in the VCF");
	argument_parser.add_optional_argument('n', "0.00001", "effective population size");
	argument_parser.add_flag_argument('g', "only run genotyping (Forward backward algorithm)");
	argument_parser.add_flag_argument('p', "only run phasing (Viterbi algorithm)");
	argument_parser.add_flag_argument('d', "do not add reference as additional path.");

	try {
		argument_parser.parse(argc, argv);
	} catch (const runtime_error& e) {
		argument_parser.usage();
		cerr << e.what() << endl;
		return 1;
	} catch (const exception& e) {
		return 0;
	}
	reffile = argument_parser.get_argument('r');
	vcffile = argument_parser.get_argument('v');
	outname = argument_parser.get_argument('o');
	sample_name = argument_parser.get_argument('s');
	nr_core_threads = stoi(argument_parser.get_argument('t'));
	only_genotyping = argument_parser.get_flag('g');
	only_phasing = argument_parser.get_flag('p');
	effective_N = stold(argument_parser.get_argument('n'));
	add_reference = !argument_parser.get_flag('d');

	// print info
	cerr << "Files and parameters used:" << endl;
	argument_parser.info();

	// read allele sequences and unitigs inbetween, write them into file
	cerr << "Determine allele sequences ..." << endl;
	VariantReader variant_reader (vcffile, reffile, 31, add_reference, sample_name);

	// determine chromosomes present in VCF
	vector<string> chromosomes;
	variant_reader.get_chromosomes(&chromosomes);
	cerr << "Found " << chromosomes.size() << " chromosome(s) in the VCF." << endl;

	// TODO: only for analysis
	struct rusage r_usage0;
	getrusage(RUSAGE_SELF, &r_usage0);
	cerr << "#### Memory usage until now: " << (r_usage0.ru_maxrss / 1E6) << " GB ####" << endl;

	// prepare output files
	if (! only_phasing) variant_reader.open_genotyping_outfile(outname + "_genotyping.vcf");
	if (! only_genotyping) variant_reader.open_phasing_outfile(outname + "_phasing.vcf");

	time_preprocessing = timer.get_interval_time();

	cerr << "Construct HMM and run core algorithm ..." << endl;
	ProbabilityTable probabilities;

	// determine max number of available threads (at most one thread per chromosome possible)
	size_t available_threads = min(thread::hardware_concurrency(), (unsigned int) chromosomes.size());
	if (nr_core_threads > available_threads) {
		cerr << "Warning: set nr_core_threads to " << available_threads << "." << endl;
		nr_core_threads = available_threads;
	}
	Results results;
	{
		// create thread pool
		ThreadPool threadPool (nr_core_threads);
		for (auto chromosome : chromosomes) {
			KmerCounter* genomic = nullptr;
			VariantReader* variants = &variant_reader;
			Results* r = &results;
			ProbabilityTable* probs = &probabilities;
			function<void()> f_genotyping = bind(run_genotyping_paths, chromosome, genomic, nullptr, variants, probs, 28, only_genotyping, only_phasing, effective_N, r);
			threadPool.submit(f_genotyping);
		}
	}

/**	boost::asio::thread_pool threadPool(nr_core_threads);
	for (auto chromosome : chromosomes) {
		boost::asio::post(threadPool, boost::bind(run_genotyping_paths, chromosome, nullptr, nullptr, &variant_reader, 28, only_genotyping, only_phasing, effective_N, &results));
	}
	threadPool.join();
**/
	timer.get_interval_time();

	// output VCF
	cerr << "Write results to VCF ..." << endl;
	assert (results.result.size() == chromosomes.size());
	// write VCF
	for (auto it = results.result.begin(); it != results.result.end(); ++it) {
		if (!only_phasing) {
			// output genotyping results
			variant_reader.write_genotypes_of(it->first, it->second);
		}
		if (!only_genotyping) {
			// output phasing results
			variant_reader.write_phasing_of(it->first, it->second);
		}
	}

	if (! only_phasing) variant_reader.close_genotyping_outfile();
	if (! only_genotyping) variant_reader.close_phasing_outfile();

	time_writing = timer.get_interval_time();
	time_total = timer.get_total_time();

	cerr << endl << "###### Summary ######" << endl;
	// output times
	cerr << "time spent reading input files:\t" << time_preprocessing << " sec" << endl;
	// output per chromosome time
	double time_hmm = time_writing;
	for (auto chromosome : chromosomes) {
		double time_chrom = results.runtimes.at(chromosome);
		cerr << "time spent genotyping chromosome " << chromosome << ":\t" << time_chrom << endl;
		time_hmm += time_chrom;
	}
	cerr << "total running time:\t" << time_preprocessing + time_hmm << " sec"<< endl;
	cerr << "total wallclock time: " << time_total  << " sec" << endl;

	// memory usage
	struct rusage r_usage;
	getrusage(RUSAGE_SELF, &r_usage);
	cerr << "Total maximum memory usage: " << (r_usage.ru_maxrss / 1E6) << " GB" << endl;

	return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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 BOOST_GEOMETRY_GEOMETRY_HPP
#define BOOST_GEOMETRY_GEOMETRY_HPP

// Shortcut to include all header files

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tag_cast.hpp>
#include <boost/geometry/core/tags.hpp>

// Core algorithms
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/interior_rings.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/topological_dimension.hpp>

#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>

#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/algorithms/append.hpp>
#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/buffer.hpp>
#include <boost/geometry/algorithms/centroid.hpp>
#include <boost/geometry/algorithms/clear.hpp>
#include <boost/geometry/algorithms/comparable_distance.hpp>
#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/algorithms/convex_hull.hpp>
#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/algorithms/covered_by.hpp>
#include <boost/geometry/algorithms/difference.hpp>
#include <boost/geometry/algorithms/disjoint.hpp>
#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/algorithms/envelope.hpp>
#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/algorithms/expand.hpp>
#include <boost/geometry/algorithms/for_each.hpp>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/algorithms/intersects.hpp>
#include <boost/geometry/algorithms/length.hpp>
#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/algorithms/num_geometries.hpp>
#include <boost/geometry/algorithms/num_interior_rings.hpp>
#include <boost/geometry/algorithms/num_points.hpp>
#include <boost/geometry/algorithms/overlaps.hpp>
#include <boost/geometry/algorithms/perimeter.hpp>
#include <boost/geometry/algorithms/reverse.hpp>
#include <boost/geometry/algorithms/simplify.hpp>
#include <boost/geometry/algorithms/sym_difference.hpp>
#include <boost/geometry/algorithms/touches.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/algorithms/union.hpp>
#include <boost/geometry/algorithms/unique.hpp>
#include <boost/geometry/algorithms/within.hpp>

// Include multi a.o. because it can give weird effects
// if you don't (e.g. area=0 of a multipolygon)
#include <boost/geometry/multi/multi.hpp>

// check includes all concepts
#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/util/for_each_coordinate.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_most_precise.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>
#include <boost/geometry/io/dsv/write.hpp>

#include <boost/geometry/views/box_view.hpp>
#include <boost/geometry/views/segment_view.hpp>

#include <boost/geometry/io/io.hpp>
#include <boost/geometry/io/svg/svg_mapper.hpp>

#endif // BOOST_GEOMETRY_GEOMETRY_HPP