/*
  Tencent is pleased to support the open source community by making
  Plato available.
  Copyright (C) 2019 THL A29 Limited, a Tencent company.
  All rights reserved.

  Licensed under the BSD 3-Clause License (the "License"); you may
  not use this file except in compliance with the License. You may
  obtain a copy of the License at

  https://opensource.org/licenses/BSD-3-Clause

  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.

  See the AUTHORS file for names of contributors.
*/

#ifndef __PLATO_ALGO_FAST_UNFOLDING_LOUVAIN_HPP__
#define __PLATO_ALGO_FAST_UNFOLDING_LOUVAIN_HPP__

#include <cstdint>
#include <cstdlib>
#include <unordered_map>
#include <algorithm>
#include <utility>
#include <vector>
#include <cmath>
#include <unordered_set>
#include <boost/format.hpp>
#include <boost/lockfree/queue.hpp>

#include "glog/logging.h"

#include "plato/util/perf.hpp"
#include "plato/util/atomic.hpp"
#include "plato/graph/graph.hpp"
#include "plato/engine/dualmode.hpp"

namespace plato { namespace algo {
struct louvain_opts_t {
  int alpha_ = -1;
  bool part_by_in_ = false;
  int outer_iteration_ = 3;
  int inner_iteration_ = 2;
};

template<typename GRAPH>
class louvain_epoch_t {
public:
  using partition_t = typename GRAPH::partition_t;
  using edge_value_t = typename GRAPH::edata_t;
  using louvain_value_t = plato::dense_state_t<edge_value_t, partition_t>;
  using adj_unit_list_spec_t = typename GRAPH::adj_unit_list_spec_t;
  struct epoch_msg_type_t {
    vid_t v_i;
    vid_t from;
    vid_t to;
    edge_value_t ki;
  };

  struct sync_val_msg_type_t{
    vid_t v_i;
    edge_value_t val;
  };

public:
  /**
   * @brief
   * @param graph
   * @param graph_info
   * @param m
   * @param opts
   */
  explicit louvain_epoch_t(
    std::shared_ptr<GRAPH> graph, const graph_info_t& graph_info, double m,
    const louvain_opts_t& opts = louvain_opts_t());
  /**
   * @brief
   */
  ~louvain_epoch_t();

  /**
   * @brief
   */
  void compute();

  /**
   * @brief getter
   * @return
   */
  std::shared_ptr<GRAPH> graph() { return graph_; };
  /**
   * @brief getter
   * @return
   */
  std::vector<vid_t>& labels() { return labels_; };

private:
  std::shared_ptr<GRAPH> graph_;
  graph_info_t graph_info_;
  louvain_value_t ki_;
  std::vector<vid_t> labels_;
  std::vector<edge_value_t> sigma_tot_;
  bitmap_t<> local_bit_;
  double m_;
  louvain_opts_t opts_;
};

template <typename GRAPH>
louvain_epoch_t<GRAPH>::louvain_epoch_t(
  std::shared_ptr<GRAPH> graph,
  const graph_info_t& graph_info, double m, const louvain_opts_t& opts) :
  graph_(graph), graph_info_(graph_info), ki_(graph_info.max_v_i_, graph->partitioner()),
  labels_(graph_info.max_v_i_ + 1), sigma_tot_(graph_info.max_v_i_ + 1, 0),
  local_bit_(graph_info.max_v_i_ + 1), m_(2 * m), opts_(opts) {

  auto& cluster_info = plato::cluster_info_t::get_instance();
  plato::stop_watch_t watch;

  {// init labels
    watch.mark("t0");
#pragma omp parallel for num_threads(cluster_info.threads_)
    for (vid_t v_i = 0; v_i <= graph_info.max_v_i_; ++v_i) {
      labels_[v_i] = v_i;
    }
    LOG(INFO) << "epoch init labels: " << watch.show("t0") / 1000.0;
  }

  {//calc ki_ and sync sigma_tot_
    bitmap_t<> active_all(graph_info.max_v_i_ + 1);
    active_all.fill();
    auto active_view_all = plato::create_active_v_view(graph->partitioner()->self_v_view(), active_all);
    watch.mark("t0");
    edge_value_t max_ki = 0;
    using push_context_t = plato::template mepa_bc_context_t<sync_val_msg_type_t>;
    plato::broadcast_message<sync_val_msg_type_t, vid_t>(
      active_view_all,
      /**
       * @brief
       * @param context
       * @param v_i
       */
      [&](const push_context_t& context, vid_t v_i) {
        edge_value_t local_sum = 0;
        auto neighbours = graph->neighbours(v_i);
        for (auto it = neighbours.begin_; neighbours.end_ != it; ++it) {
          local_sum += it->edata_;
        }
        if (neighbours.begin_ != neighbours.end_) {
          ki_[v_i] = local_sum;
          local_bit_.set_bit(v_i);
          plato::write_max(&max_ki, local_sum);
          context.send(sync_val_msg_type_t{ v_i, local_sum});
        }
      },
      /**
       * @brief
       * @param p_i
       * @param msg
       * @return
       */
      [&](int p_i, sync_val_msg_type_t& msg) {
        sigma_tot_[msg.v_i] = msg.val;
        return 0;
      }
    );
    LOG(INFO) << "epoch sync sigma_tot: " << watch.show("t0") / 1000.0;
    MPI_Allreduce(MPI_IN_PLACE, &max_ki, 1, get_mpi_data_type<edge_value_t>(), MPI_MAX, MPI_COMM_WORLD);
    LOG(INFO) << "max ki: " << max_ki;
  }
}

template <typename GRAPH>
louvain_epoch_t<GRAPH>::~louvain_epoch_t() {
}

template <typename GRAPH>
void louvain_epoch_t<GRAPH>::compute() {
  auto& cluster_info = plato::cluster_info_t::get_instance();
  auto try_change = [&](vid_t v_i, vid_t from, vid_t to, edge_value_t ki_in_from, edge_value_t ki_in_to) {
    double x = (double)ki_[v_i] + (double)sigma_tot_[to] - (double)sigma_tot_[from];
    return (double)ki_in_to - (double)ki_in_from - 2.0 * (double)ki_[v_i] * x / m_;
  };
  auto do_change = [&](epoch_msg_type_t& msg) {
    labels_[msg.v_i] = msg.to;
    plato::write_add(&sigma_tot_[msg.from], -msg.ki);
    plato::write_add(&sigma_tot_[msg.to], msg.ki);
  };

  auto active_view = plato::create_active_v_view(graph_->partitioner()->self_v_view(), local_bit_);
  plato::stop_watch_t watch;
  using push_context_t = plato::template mepa_bc_context_t<epoch_msg_type_t>;
  for (int try_time = 0; try_time < opts_.inner_iteration_; ++try_time){
    watch.mark("t0");
    auto exec_once = [&](std::function<bool(vid_t, vid_t)> condition) {
      plato::broadcast_message<epoch_msg_type_t, vid_t>(
        active_view,
        /**
         * @brief
         * @param context
         * @param v_i
         */
        [&](const push_context_t& context, plato::vid_t v_i) {
          vid_t target = (vid_t)-1;
          vid_t from = labels_[v_i];
          edge_value_t ki_in_from = 0;
          std::unordered_map<vid_t, edge_value_t> ki_in_map;
          auto neighbours = graph_->neighbours(v_i);
          edge_value_t self_cycle = 0;
          for (auto it = neighbours.begin_; neighbours.end_ != it; ++it) {
            vid_t to = labels_[it->neighbour_];
            if (condition(from, to)) {
              ki_in_map[to] += it->edata_;
            } else if (to == from) {
              ki_in_from += it->edata_;
            }

            if (it->neighbour_ == v_i) {
              self_cycle = it->edata_;
            }
          }

          double best_delta = 0;
          for (auto& it: ki_in_map) {
            double delta = try_change(v_i, from, it.first, ki_in_from, it.second + self_cycle);
            if (delta > best_delta) {
              target = it.first;
              best_delta = delta;
            }
          }

          if (target != (vid_t)-1) {
            epoch_msg_type_t msg = epoch_msg_type_t{ v_i, from, target, ki_[v_i] };
            context.send(msg);
            do_change(msg);
          }
        },
        /**
         * @brief
         * @param p_i
         * @param msg
         * @return
         */
        [&](int p_i, epoch_msg_type_t& msg) {
          if (p_i != cluster_info.partition_id_) {
            do_change(msg);
          }
          return 0;
        }
      );
    };

    exec_once([&](vid_t from, vid_t to){
      if (from < to) return true;
      return false;
    });
    exec_once([&](vid_t from, vid_t to){
      if (from > to) return true;
      return false;
    });
    LOG(INFO) << "try_time: "  << try_time << " cost: " << watch.show("t0") / 1000.0;
  }
}

template<typename GRAPH>
class louvain_fast_unfolding_t {
public:
  using partition_t = typename GRAPH::partition_t;
  using edge_value_t = typename GRAPH::edata_t;
  using adj_unit_list_spec_t = plato::adj_unit_list_t<edge_value_t>;
  using louvain_value_t = plato::dense_state_t<vid_t, partition_t>;

  struct edge_sync_msg_type_t {
    vid_t src;
    vid_t dst;
    edge_value_t data;
  };

  struct degree_sync_msg_type_t {
    vid_t src;
    vid_t degree;
  };

public:
  /**
   * @brief
   * @param graph
   * @param graph_info
   * @param opts
   */
  explicit louvain_fast_unfolding_t(
    std::shared_ptr<GRAPH> graph, const graph_info_t& graph_info,
    const louvain_opts_t& opts = louvain_opts_t());

  /**
   * @brief destructor
   */
  ~louvain_fast_unfolding_t();

  /**
   * @brief compute
   */
  void compute();

  /**
   * @brief save to storage.
   * @tparam STREAM
   * @param streams
   */
  template <typename STREAM>
  void save(std::vector<STREAM*>& streams);

private:
  /**
   * @brief
   * @param graph
   * @param labels
   * @return
   */
  std::shared_ptr<GRAPH> rebuild(std::shared_ptr<GRAPH> graph, std::vector<vid_t>& labels);
  /**
   * @brief
   * @param labels
   */
  void update_local_label(std::vector<vid_t>& labels);
private:
  std::shared_ptr<GRAPH> graph_;
  graph_info_t graph_info_;
  louvain_value_t local_label_;
  louvain_value_t local_comm_size_;
  louvain_opts_t opts_;
};

template<typename GRAPH>
louvain_fast_unfolding_t<GRAPH>::louvain_fast_unfolding_t(
  std::shared_ptr<GRAPH> graph, const graph_info_t& graph_info,
  const louvain_opts_t& opts)
  : graph_(graph), graph_info_(graph_info),
    local_label_(graph_info.max_v_i_, graph->partitioner()),
    local_comm_size_(graph_info.max_v_i_, graph->partitioner()), opts_(opts) {
}

template<typename GRAPH>
louvain_fast_unfolding_t<GRAPH>::~louvain_fast_unfolding_t() {
}

template<typename GRAPH>
void louvain_fast_unfolding_t<GRAPH>::compute() {
  //first calc m and init label
  plato::stop_watch_t watch;
  watch.mark("t0");
  double m = local_label_.template foreach<double> (
    /**
     * @brief
     * @param v_i
     * @param pval
     * @return
     */
    [&](vid_t v_i, vid_t* pval) {
      *pval = v_i;
      double local_sum = 0;
      auto neighbours = graph_->neighbours(v_i);
      for (auto it = neighbours.begin_; neighbours.end_ != it; ++it) {
        local_sum += it->edata_;
      }
      return local_sum;
    }
  );
  m /= 2; //undirected
  LOG(INFO) << "calc m: " << m << " cost: " << watch.show("t0") / 1000.0;

  //epoch
  louvain_epoch_t<GRAPH>* cur = new louvain_epoch_t<GRAPH>(graph_, graph_info_, m, opts_);
  for (int epoch = 0; epoch < opts_.outer_iteration_; epoch++) {
    LOG(INFO) << "epoch " << epoch << " begin!";
    watch.mark("compute");
    cur->compute();
    LOG(INFO) << "compute cost: " << watch.show("compute") / 1000.0;
    update_local_label(cur->labels());
    if (epoch == opts_.outer_iteration_ - 1) {
      delete cur;
      break;
    }
    //rebuild from current graph
    watch.mark("rebuild");
    graph_info_t graph_info_next(graph_info_);
    graph_info_next.is_directed_ = true;
    auto graph_next = rebuild(cur->graph(), cur->labels());
    louvain_epoch_t<GRAPH>* nxt = new louvain_epoch_t<GRAPH>(graph_next, graph_info_next, m, opts_);
    LOG(INFO) << "rebuild cost: " << watch.show("rebuild") / 1000.0;
    delete cur;
    cur = nxt;
  }
}

template<typename GRAPH>
std::shared_ptr<GRAPH> louvain_fast_unfolding_t<GRAPH>::rebuild(std::shared_ptr<GRAPH> graph, std::vector<vid_t>& labels) {
  plato::stop_watch_t watch;
  watch.mark("t0");
  eid_t total_edge = local_label_.template foreach<eid_t> (
    [&](vid_t v_i, vid_t* pval) {
      auto neighbours = graph->neighbours(v_i);
      return (eid_t)(neighbours.end_ - neighbours.begin_);
    }
  );
  LOG(INFO) << "rebuild before total edge: " << total_edge << " cost: " << watch.show("t0") / 1000.0;

  watch.mark("t0");
  auto& cluster_info = plato::cluster_info_t::get_instance();
  vid_t v_begin = graph->partitioner()->offset_[cluster_info.partition_id_];
  vid_t v_end = graph->partitioner()->offset_[cluster_info.partition_id_ + 1];
  std::vector<eid_t> edge_idx(v_end - v_begin + 1, 0);
  std::vector<eid_t> tmp_idx(v_end - v_begin + 1, 0);

  bitmap_t<> active_all(graph_info_.max_v_i_ + 1);
  active_all.fill();
  auto active_view_all = plato::create_active_v_view(graph->partitioner()->self_v_view(), active_all);

  {
    //first, calc degree
    using push_context_t = plato::template mepa_sd_context_t<degree_sync_msg_type_t>;
    plato::spread_message<degree_sync_msg_type_t, vid_t>(
      active_view_all,
      /**
       * @brief
       * @param context
       * @param v_i
       */
      [&](const push_context_t& context, vid_t v_i) {
        auto neighbours = graph->neighbours(v_i);
        if (neighbours.begin_ == neighbours.end_) return;
        vid_t src = labels[v_i];
        auto send_to = graph->partitioner()->get_partition_id(src);
        context.send(send_to, degree_sync_msg_type_t { src, (vid_t)(neighbours.end_ - neighbours.begin_) } );
      },
      /**
       * @brief
       * @param msg
       * @return
       */
      [&](degree_sync_msg_type_t& msg) {
        vid_t pos = msg.src - v_begin + 1;
        plato::write_add(&edge_idx[pos], (eid_t)msg.degree);
        return 0;
      }
    );

    for (int i = 1; i < (int)edge_idx.size(); ++i) {
      edge_idx[i] = edge_idx[i - 1] + edge_idx[i];
      tmp_idx[i] = edge_idx[i];
    }

    LOG(INFO) << "rebuild calc degree cost: " << watch.show("t0") / 1000.0;
  }
  eid_t total_local_edge = edge_idx[edge_idx.size() - 1];
  std::vector<std::pair<vid_t, edge_value_t> > edges(total_local_edge);
  LOG(INFO) << "rebuild local edge: " << edges.size();
  watch.mark("t0");
  {
    //second, transfer edge
    using push_context_t = plato::template mepa_sd_context_t<edge_sync_msg_type_t>;
    plato::spread_message<edge_sync_msg_type_t, vid_t>(
      active_view_all,
      /**
       * @brief
       * @param context
       * @param v_i
       */
      [&](const push_context_t& context, vid_t v_i) {
        auto neighbours = graph->neighbours(v_i);
        if (neighbours.begin_ == neighbours.end_) return;
        vid_t src = labels[v_i];
        auto send_to = graph->partitioner()->get_partition_id(src);
        for (auto it = neighbours.begin_; neighbours.end_ != it; ++it) {
          vid_t dst = labels[it->neighbour_];
          context.send(send_to, edge_sync_msg_type_t{ src, dst, it->edata_ });
        }
      },
      /**
       * @brief
       * @param msg
       * @return
       */
      [&](edge_sync_msg_type_t& msg) {
        vid_t pos = msg.src - v_begin;
        vid_t idx = __sync_fetch_and_add(&tmp_idx[pos], (eid_t)1);
        edges[idx].first = msg.dst;
        edges[idx].second = msg.data;
        return 0;
      }
    );
    LOG(INFO) << "rebuild transfer edge cost: " << watch.show("t0") / 1000.0;
  }

  watch.mark("t0");
  edge_cache_t<edge_value_t> edge_cache;
  {
    //third, sort and aggregate
#pragma omp parallel for num_threads(cluster_info.threads_)
    for (vid_t v_i = v_begin; v_i < v_end; ++v_i) {
      vid_t pos = v_i - v_begin;
      eid_t e_start = edge_idx[pos];
      eid_t e_end = edge_idx[pos + 1];
      if (e_start == e_end) continue;
      std::sort(edges.begin() + e_start, edges.begin() + e_end);
      vid_t pre = (vid_t)-1;
      edge_value_t local_sum = 0;
      for (eid_t e = e_start; e < e_end; ++e) {
        if (pre != edges[e].first) {
          if (pre != (vid_t)-1) {
            edge_cache.push_back(edge_unit_t<edge_value_t> { v_i, pre, local_sum });
          }
          pre = edges[e].first;
          local_sum = 0;
        }
        local_sum += edges[e].second;
      }
      if (pre != (vid_t)-1) {
        edge_cache.push_back(edge_unit_t<edge_value_t> { v_i, pre, local_sum });
      }
    }

    eid_t edge_num_new = edge_cache.size();
    MPI_Allreduce(MPI_IN_PLACE, &edge_num_new, 1, get_mpi_data_type<eid_t>(), MPI_SUM, MPI_COMM_WORLD);
    LOG(INFO) << "rebuild new edge num: " << edge_num_new;
    LOG(INFO) << "rebuild aggregate edge cost: " << watch.show("t0") / 1000.0;
  }

  watch.mark("t0");
  graph_info_t graph_info_next(graph_info_);
  graph_info_next.is_directed_ = true;
  std::shared_ptr<GRAPH> pgraph(new GRAPH(graph->partitioner()));
  pgraph->load_from_cache(graph_info_next, edge_cache);
  LOG(INFO) << "rebuild load cache cost: " << watch.show("t0") / 1000.0;

  return pgraph;
}

template<typename GRAPH>
void louvain_fast_unfolding_t<GRAPH>::update_local_label(std::vector<vid_t>& labels) {
  local_label_.template foreach<int> (
    /**
     * @brief
     * @param v_i
     * @param pval
     * @return
     */
    [&] (vid_t v_i, vid_t* pval) {
      if (labels[*pval] != *pval) {
        *pval = labels[*pval];
      }
      return 0;
    }
  );
}

template<typename GRAPH>
template<typename STREAM>
void louvain_fast_unfolding_t<GRAPH>::save(std::vector<STREAM*>& ss) {
  struct louvain_msg_type_t {
    vid_t src;
    vid_t label;
  };
  boost::lockfree::queue<louvain_msg_type_t> que(1024);
  LOG_IF(FATAL, !que.is_lock_free())
  << "boost::lockfree::queue is not lock free\n";

  // start a thread to pop and edge and write to output
  std::atomic<bool> done(false);
  std::thread pop_write([&done, &ss, &que](void) {
#pragma omp parallel num_threads(ss.size())
    {
      int tid = omp_get_thread_num();
      louvain_msg_type_t vb;
      while (!done) {
        if (que.pop(vb)) {
          *ss[tid] << vb.src << "," << vb.label << "\n";
        }
      }

      while (que.pop(vb)) {
        *ss[tid] << vb.src << "," << vb.label << "\n";
      }
    }
  });

  // traverse
  local_label_.template foreach<int> (
    /**
     * @brief
     * @param v_i
     * @param pval
     * @return
     */
    [&] (vid_t v_i, vid_t* pval) {
      while (!que.push(louvain_msg_type_t {v_i, *pval} )) {
      }
      return 0;
    }
  );

  done = true;
  pop_write.join();
}

}  // namespace plato
}  // namespace algo
#endif


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_POLY_HPP
#define BOOST_GEOMETRY_PROJECTIONS_POLY_HPP

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/pj_mlfn.hpp>
#include <boost/geometry/srs/projections/impl/pj_msfn.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct poly {}; // Polyconic (American)

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace poly
    {

            static const double tolerance = 1e-10;
            static const double conv_tolerance = 1e-10;
            static const int n_iter = 10;
            static const int i_iter = 20;
            static const double i_tolerance = 1.e-12;

            template <typename T>
            struct par_poly
            {
                T ml0;
                detail::en<T> en;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_poly_ellipsoid
                : public base_t_fi<base_poly_ellipsoid<T, Parameters>, T, Parameters>
            {
                par_poly<T> m_proj_parm;

                inline base_poly_ellipsoid(const Parameters& par)
                    : base_t_fi<base_poly_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    T  ms, sp, cp;

                    if (fabs(lp_lat) <= tolerance) {
                        xy_x = lp_lon;
                        xy_y = -this->m_proj_parm.ml0;
                    } else {
                        sp = sin(lp_lat);
                        ms = fabs(cp = cos(lp_lat)) > tolerance ? pj_msfn(sp, cp, this->m_par.es) / sp : 0.;
                        xy_x = ms * sin(lp_lon *= sp);
                        xy_y = (pj_mlfn(lp_lat, sp, cp, this->m_proj_parm.en) - this->m_proj_parm.ml0) + ms * (1. - cos(lp_lon));
                    }
                }

                // INVERSE(e_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    xy_y += this->m_proj_parm.ml0;
                    if (fabs(xy_y) <= tolerance) {
                        lp_lon = xy_x;
                        lp_lat = 0.;
                    } else {
                        T r, c, sp, cp, s2ph, ml, mlb, mlp, dPhi;
                        int i;

                        r = xy_y * xy_y + xy_x * xy_x;
                        for (lp_lat = xy_y, i = i_iter; i ; --i) {
                            sp = sin(lp_lat);
                            s2ph = sp * ( cp = cos(lp_lat));
                            if (fabs(cp) < i_tolerance) {
                                BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                            }
                            c = sp * (mlp = sqrt(1. - this->m_par.es * sp * sp)) / cp;
                            ml = pj_mlfn(lp_lat, sp, cp, this->m_proj_parm.en);
                            mlb = ml * ml + r;
                            mlp = this->m_par.one_es / (mlp * mlp * mlp);
                            lp_lat += ( dPhi =
                                ( ml + ml + c * mlb - 2. * xy_y * (c * ml + 1.) ) / (
                                this->m_par.es * s2ph * (mlb - 2. * xy_y * ml) / c +
                                2.* (xy_y - ml) * (c * mlp - 1. / s2ph) - mlp - mlp ));
                            if (fabs(dPhi) <= i_tolerance)
                                break;
                        }
                        if (!i) {
                            BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                        }
                        c = sin(lp_lat);
                        lp_lon = asin(xy_x * tan(lp_lat) * sqrt(1. - this->m_par.es * c * c)) / sin(lp_lat);
                    }
                }

                static inline std::string get_name()
                {
                    return "poly_ellipsoid";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_poly_spheroid
                : public base_t_fi<base_poly_spheroid<T, Parameters>, T, Parameters>
            {
                par_poly<T> m_proj_parm;

                inline base_poly_spheroid(const Parameters& par)
                    : base_t_fi<base_poly_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    T  cot, E;

                    if (fabs(lp_lat) <= tolerance) {
                        xy_x = lp_lon;
                        xy_y = this->m_proj_parm.ml0;
                    } else {
                        cot = 1. / tan(lp_lat);
                        xy_x = sin(E = lp_lon * sin(lp_lat)) * cot;
                        xy_y = lp_lat - this->m_par.phi0 + cot * (1. - cos(E));
                    }
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    T B, dphi, tp;
                    int i;

                    if (fabs(xy_y = this->m_par.phi0 + xy_y) <= tolerance) {
                        lp_lon = xy_x;
                        lp_lat = 0.;
                    } else {
                        lp_lat = xy_y;
                        B = xy_x * xy_x + xy_y * xy_y;
                        i = n_iter;
                        do {
                            tp = tan(lp_lat);
                            lp_lat -= (dphi = (xy_y * (lp_lat * tp + 1.) - lp_lat -
                                .5 * ( lp_lat * lp_lat + B) * tp) /
                                ((lp_lat - xy_y) / tp - 1.));
                        } while (fabs(dphi) > conv_tolerance && --i);
                        if (! i) {
                            BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                        }
                        lp_lon = asin(xy_x * tan(lp_lat)) / sin(lp_lat);
                    }
                }

                static inline std::string get_name()
                {
                    return "poly_spheroid";
                }

            };

            // Polyconic (American)
            template <typename Parameters, typename T>
            inline void setup_poly(Parameters& par, par_poly<T>& proj_parm)
            {
                if (par.es != 0.0) {
                    proj_parm.en = pj_enfn<T>(par.es);
                    proj_parm.ml0 = pj_mlfn(par.phi0, sin(par.phi0), cos(par.phi0), proj_parm.en);
                } else {
                    proj_parm.ml0 = -par.phi0;
                }
            }

    }} // namespace detail::poly
    #endif // doxygen

    /*!
        \brief Polyconic (American) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Conic
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_poly.gif
    */
    template <typename T, typename Parameters>
    struct poly_ellipsoid : public detail::poly::base_poly_ellipsoid<T, Parameters>
    {
        inline poly_ellipsoid(const Parameters& par) : detail::poly::base_poly_ellipsoid<T, Parameters>(par)
        {
            detail::poly::setup_poly(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Polyconic (American) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Conic
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_poly.gif
    */
    template <typename T, typename Parameters>
    struct poly_spheroid : public detail::poly::base_poly_spheroid<T, Parameters>
    {
        inline poly_spheroid(const Parameters& par) : detail::poly::base_poly_spheroid<T, Parameters>(par)
        {
            detail::poly::setup_poly(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::poly, poly_spheroid, poly_ellipsoid)

        // Factory entry(s)
        template <typename T, typename Parameters>
        class poly_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    if (par.es)
                        return new base_v_fi<poly_ellipsoid<T, Parameters>, T, Parameters>(par);
                    else
                        return new base_v_fi<poly_spheroid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        inline void poly_init(detail::base_factory<T, Parameters>& factory)
        {
            factory.add_to_factory("poly", new poly_entry<T, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_POLY_HPP


/*
 *  Copyright (c) 2009, 2010, Universitaet Bremen
 *  All rights reserved.
 *
 *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
 *
 *  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 Universitaet Bremen 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.
 */

#ifndef __UKF_MTKWRAP_HPP__
#define __UKF_MTKWRAP_HPP__

#include <cassert>

#include <Eigen/Core>
//#include <Eigen/LU>

//#include <Eigen/QR>

namespace ukfom {

// import most common Eigen types
USING_PART_OF_NAMESPACE_EIGEN

/**
 * mtkwrap<M> wraps an MTK-Manifold M to a ukf-compatible manifold.
 * M has to have an enum DOF and implement the methods boxplus and boxminus.
 */
template<class M>
struct mtkwrap : public M{
	typedef mtkwrap<M> self;
public:
	typedef double scalar; // MTK only works with double
	typedef scalar scalar_type;

	enum {
		DOF = M::DOF
	};

	typedef Matrix<scalar_type, DOF, 1> vectorized_type;

	mtkwrap(const M &m=M()) : M(m) {}

	/*
	 * manifold operator (+)
	 *
	 */
	self& operator+=(const vectorized_type &delta_state)
	{
		assert(delta_state.stride() == DOF);
		M::boxplus(delta_state.data());
		return *this;
	}

	const self operator+(const vectorized_type &delta_state) const
	{
		self result = *this;
		result += delta_state;

		return result;
	}

	/*
	 * manifold operator (-)
	 */
	const vectorized_type operator-(const self &other) const
	{
		vectorized_type result;
		assert(result.stride()==DOF);
		M::boxminus(result.data(), other);

		return result;
	}

	bool operator==(const self &other) const
	{
		vectorized_type diff = (*this) - other;
		return diff.isZero(1e-12);
	}

	bool operator!=(const self &other) const
	{
		return !(*this == other);
	}

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} // namespace ukfom
#endif /* __UKF_MTKWRAP_HPP__ */


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <functional>
#include <mutex>
#include <Eigen/Geometry>

#ifdef ENABLE_PARALLEL
    #include <omp.h>
#endif

#ifndef SIMPLE_HAUSDORFF_DISTANCE_HPP
#define SIMPLE_HAUSDORFF_DISTANCE_HPP

namespace simple_hausdorff_distance
{
    class SimpleHausdorffDistance
    {
    private:

        SimpleHausdorffDistance() {}

        static inline size_t GetNumOMPThreads()
        {
#ifdef ENABLE_PARALLEL
            size_t num_threads = 0;
            #pragma omp parallel
            {
                num_threads = (size_t)omp_get_num_threads();
            }
            return num_threads;
#else
            return 1;
#endif
        }

    public:

        template<typename Datatype, typename Allocator=std::allocator<Datatype>>
        static double ComputeDistance(const std::vector<Datatype, Allocator>& first_distribution, const std::vector<Datatype, Allocator>& second_distribution, const std::function<double(const Datatype&, const Datatype&)>& distance_fn)
        {
            // Compute the Hausdorff distance - the "maximum minimum" distance
            double maximum_minimum_distance = 0.0;
#ifdef ENABLE_PARALLEL
            std::vector<double> thread_temp_storage(GetNumOMPThreads(), 0.0);
            #pragma omp parallel for schedule(guided)
#endif
            for (size_t idx = 0; idx < first_distribution.size(); idx++)
            {
                const Datatype& first = first_distribution[idx];
                double minimum_distance = INFINITY;
                for (size_t jdx = 0; jdx < second_distribution.size(); jdx++)
                {
                    const Datatype& second = second_distribution[jdx];
                    const double& current_distance = distance_fn(first, second);
                    if (current_distance < minimum_distance)
                    {
                        minimum_distance = current_distance;
                    }
                }
#ifdef ENABLE_PARALLEL
                const size_t current_thread_id = (size_t)omp_get_thread_num();
                if (minimum_distance > thread_temp_storage[current_thread_id])
                {
                    thread_temp_storage[current_thread_id] = minimum_distance;
                }
#else
                if (minimum_distance > maximum_minimum_distance)
                {
                    maximum_minimum_distance = minimum_distance;
                }
#endif
            }
#ifdef ENABLE_PARALLEL
            for (size_t idx = 0; idx < thread_temp_storage.size(); idx++)
            {
                const double& temp_minimum_distance = thread_temp_storage[idx];
                if (temp_minimum_distance > maximum_minimum_distance)
                {
                    maximum_minimum_distance = temp_minimum_distance;
                }
            }
#endif
            return maximum_minimum_distance;
        }
    };
}
#endif // SIMPLE_HAUSDORFF_DISTANCE_HPP


//============================================================================
// Name        : TDEModel.cpp
// Author      : Jordan Frank (jordan.frank@cs.mcgill.ca)
// Copyright   : MIT
// Description : Implementation for the time-delay embedding model.
//============================================================================

#include <stdlib.h>
#include <ctype.h>
#include <iostream>
#include <fstream>
#include <Tisean/tsa.h>
#include <ANN/ANN.h>
#include <opencv/cxcore.h>
#include <boost/random.hpp>
//#include <opencv/cvaux.h>

#include "Utils.h"
#include "BuildTree.h"
#include "TDEModel.h"

using namespace std;
using namespace boost;

TDEModel::TDEModel(Settings* settings) {
    ANNcoord *data, *projecteddata;

    length = settings->length;
    embdim = settings->embdim;
    delay = settings->delay;
    use_pca = settings->pcaembset;

    get_embedding(settings, data, length);

    if (use_pca) {
    	cerr << "Computing PCA bases.\n";
    	computePCABases(data, length, embdim, settings->pcaembdim);
    	projecteddata = projectData(data, length, embdim);
    	delete [] data;
    	data = projecteddata;
    }
    else {
    	avg = NULL;
    	bases = NULL;
    }
    /*
	for (uint i = 0; i < (unsigned)bases->cols; i++) {
		cout << data[i] << " " ;
	}
	cout << endl;
	*/
    get_ann_points(dataPts, data, length, settings->pcaembdim);
    kdTree = new ANNkd_tree(dataPts,length,settings->pcaembdim);
    settings->length = length;
    delete [] data;
}

TDEModel::TDEModel(ifstream* model_file, uint verbosity) {
	int avgsize, basesrows, basescols, i, j;
	*model_file >> delay;
	*model_file >> embdim;
	*model_file >> avgsize;

	if (avgsize > 0) {
		use_pca = 1;
		avg = cvCreateMat(1,avgsize,MAT_TYPE);
		ANNcoord* ptr = (ANNcoord*)avg->data.ptr;
		for (i = 0; i < avgsize; i++) {
			*model_file >> *ptr++;
		}
	}
	else {
		use_pca = 0;
		avg = NULL;
		bases = NULL;
	}
	*model_file >> basesrows >> basescols;
	if (verbosity > 0) {
		if (basescols > 0)
			cerr << "Model params: d=" << delay << " m=" << embdim << " p=" << basescols << endl;
		else
			cerr << "Model params: d=" << delay << " m=" << embdim << endl;
	}
	if (use_pca) {
		bases = cvCreateMat(basesrows,basescols,MAT_TYPE);
		ANNcoord* ptr = (ANNcoord*)bases->data.ptr;
		for (i = 0; i < basesrows; i++) {
			for (j = 0; j < basescols; j++) {
				*model_file >> *ptr++;
			}
		}
	}

	kdTree = new ANNkd_tree(*model_file);
	dataPts = kdTree->thePoints();
	length = kdTree->nPoints();

	if (verbosity > 0)
		cerr << "Loaded " << length << " points." << endl;
	model_file->close();
	delete model_file;
}

TDEModel::~TDEModel() {
	if (avg != NULL) cvReleaseMat(&avg);
	if (bases != NULL) cvReleaseMat(&bases);
	annDeallocPts(dataPts);
	delete kdTree;
}

void TDEModel::DumpTree(char* outfile) {
    ofstream fout(outfile);
    fout << delay << endl;
    fout << embdim << endl;
    if (avg == NULL) {
    	fout << 0 << endl;
    }
    else {
    	fout << avg->cols << endl;
    	for (int i = 0; i < avg->cols; i++) {
    		fout << " " << CV_MAT_ELEM(*avg, ANNcoord, 0, i);
    	}
    }
	fout << endl;
    if (bases == NULL) {
    	fout << "0 0" << endl;
    }
    else {
    	fout << bases->rows << " " << bases->cols << endl;
    	for (int i = 0; i < bases->rows; i++) {
        	fout << CV_MAT_ELEM(*bases, ANNcoord, i, 0);
    		for (int j = 1; j < bases->cols; j++) {
    			fout << " " << CV_MAT_ELEM(*bases, ANNcoord, i, j);
    		}
    		fout << endl;
    	}
    }
    kdTree->Dump(ANNtrue, fout);
    fout.close();
}

void TDEModel::getKNN(ANNpoint ap, uint k, ANNidxArray nn_idx, ANNdistArray dists) {
	kdTree->annkSearch(ap, k, nn_idx, dists);
//    for (uint i = 0; i < k; i++) {
//            cout << "Point " << i+1 << ": [" << dataPts[nn_idx[i]][0] << "," << dataPts[nn_idx[i]][1] << "," << dataPts[nn_idx[i]][2] << "], Dist: " << sqrt(dists[i]) << endl;
//    }
}

void TDEModel::simulateTrajectory(ANNpoint s0, ANNpointArray trajectory, uint dim, ulong N) {
    ANNidxArray nn_idx;
    ANNdistArray dists;
    uint i,j,k;
    variate_generator<mt19937, normal_distribution<> > generator(mt19937(time(0)), normal_distribution<>(0.0,0.1));
    uint neighbours = 4;
	// +1 in case one of the neighbours is the last point in the model.
    nn_idx = new ANNidx[neighbours+1];
    dists = new ANNdist[neighbours+1];

    for (i = 0; i < dim; i++) {
    	trajectory[0][i] = s0[i];
    }

    for (i = 1; i < N; i++) {
    	getKNN(trajectory[i-1], neighbours+1, nn_idx, dists);
    	for (j = 0; j < dim; j++) {
    		trajectory[i][j] = 0.0;
    		for (k = 0; k < neighbours; k++) {
    			if (nn_idx[k] == ANN_NULL_IDX) break;
    			else if (nn_idx[k] == (int)length-1) nn_idx[k] = nn_idx[neighbours];
    			trajectory[i][j] += dataPts[nn_idx[k]+1][j];
    		}
    		trajectory[i][j] = trajectory[i][j] / (ANNcoord)k + generator();
    	}
    }
}

ANNpoint TDEModel::getDataPoint(uint idx) {
	return dataPts[idx];
}

void TDEModel::computePCABases(ANNcoord *data, uint rows, uint cols, uint numbases) {
	CvMat **embedding, *cov, *eigenvectors, *eigenvalues, *vector;
	ANNcoord *basesdata;
	uint i, j, offset;

	cov = cvCreateMat(cols, cols, MAT_TYPE);
	eigenvectors = cvCreateMat(cols,cols,MAT_TYPE);
	eigenvalues = cvCreateMat(cols,cols,MAT_TYPE);
	embedding = new CvMat*[rows];
	for (i = 0; i < rows; i++) {
		vector = cvCreateMatHeader(1, cols, MAT_TYPE);
		cvInitMatHeader(vector, 1, cols, MAT_TYPE, data + i * cols);
		embedding[i] = vector;
	}
	avg = cvCreateMat(1,cols,MAT_TYPE);
	cvCalcCovarMatrix((const CvArr **)embedding, rows, cov, avg, CV_COVAR_NORMAL);
	cvSVD(cov, eigenvalues, eigenvectors, 0, CV_SVD_MODIFY_A);

	basesdata = new ANNcoord[cols*numbases];
	for (i = 0, offset = 0; i < cols; i++) {
		for (j = 0; j < numbases; j++) {
			basesdata[offset++] = ((ANNcoord*)eigenvectors->data.ptr)[i*cols+j];
		}
	}
	bases = cvCreateMatHeader(cols, numbases, MAT_TYPE);
	cvInitMatHeader(bases, cols, numbases, MAT_TYPE, basesdata);

	for (i = 0; i < rows; i++) {
    	cvReleaseMat(&embedding[i]);
    }
    delete [] embedding;
    cvReleaseMat(&cov);
    cvReleaseMat(&eigenvectors);
    cvReleaseMat(&eigenvalues);
}

ANNcoord* TDEModel::projectData(ANNcoord* data, uint rows, uint cols) {
	if (!use_pca) return data;
	ANNcoord* shifteddata = new ANNcoord[rows*cols];
	ANNcoord* projecteddata = new ANNcoord[rows*bases->cols];
	uint i, j, offset;

	for (i = 0, offset = 0; i < rows; i++) {
		for (j = 0; j < cols; j++) {
			shifteddata[offset] = data[offset] - ((ANNcoord*)avg->data.ptr)[j];
			offset++;
		}
	}
	CvMat projected = cvMat(rows, bases->cols, MAT_TYPE, projecteddata);
	CvMat dataMat = cvMat(rows, cols, MAT_TYPE, shifteddata);
	cvGEMM(&dataMat, bases, 1.0, NULL, 0.0, &projected, 0);
	/*
	for (i = 0; i < (unsigned)bases->cols; i++) {
		cout << projecteddata[i] << " " ;
	}
	cout << endl;
	*/
	delete [] shifteddata;
	return projecteddata;
}


/*
 * Copyright (c) 2018 Eliane Briand
 *
 * This file is part of SmolDock.
 *
 * SmolDock is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * SmolDock 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
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with SmolDock.  If not, see <https://www.gnu.org/licenses/>.
 *
 */

#include "VinaLikeRigid.h"

#undef BOOST_LOG

#include <boost/log/trivial.hpp>

#include <cmath>
#include <boost/assert.hpp>
#include <iomanip>
#include <Structures/Atom.h>

#include <Engines/Internals/InternalsUtilityFunctions.h>

#include "VinaLikeCommon.h"

namespace SmolDock {
    namespace Score {

        const std::array<std::string, VinaLikeRigid::numCoefficients>
                VinaLikeRigid::coefficientsNames =  {"Gauss1", "Gauss2", "RepulsionExceptCovalent", "Hydrophobic","Hydrogen"};

        double vina_like_rigid_inter_scoring_func(const iConformer &ligand, iTransform &transform,
                                                  const iProtein &protein) {

            BOOST_ASSERT(!ligand.x.empty());
            BOOST_ASSERT(!protein.x.empty());

            if(std::abs(transform.rota.norm() - 1) > 0.1) {
                transform.rota.normalize();
            }

            double score_raw = 0;

            Eigen::Vector3d ProtCenterPosition = {protein.center_x, protein.center_y, protein.center_z};

            for (unsigned int idxLig = 0; idxLig < ligand.x.size(); idxLig++) {
                for (unsigned int idxProt = 0; idxProt < protein.x.size(); idxProt++) {

                    Eigen::Vector3d LigPosition = {ligand.x[idxLig], ligand.y[idxLig], ligand.z[idxLig]};
                    applyRigidTransformInPlace(LigPosition, transform);

                    Eigen::Vector3d ProtPosition = {protein.x[idxProt], protein.y[idxProt], protein.z[idxProt]};

                    Eigen::Vector3d distToCenterVector = LigPosition - ProtCenterPosition;

                    double distanceToProteinCenter = distToCenterVector.norm();

                    if (distanceToProteinCenter > (protein.radius - 1)) {
                        score_raw += std::pow((distanceToProteinCenter - protein.radius), 4) + 10;
                        continue;
                    }

                    Eigen::Vector3d distVect = ProtPosition - LigPosition;

                    double atomicRadiusLig = ligand.atomicRadius[idxLig];
                    double atomicRadiusProt = protein.atomicRadius[idxProt];
                    double rawDist = distVect.norm();

                    const double cutoff = 8.0;
                    if (rawDist >= cutoff)
                        continue;

                    double radToRemove = (atomicRadiusLig + atomicRadiusProt);
                    double distance = rawDist - radToRemove;

                    score_raw += scoreForAtomCouple(distance, ligand.type[idxLig], ligand.variant[idxLig],
                                                    protein.type[idxProt], protein.variant[idxProt]);
/*
                    // Special rubber band term not in Vina scoring func
                    double x_diff_center = std::pow(LigPosition.x - protein.center_x, 2);
                    double y_diff_center = std::pow(LigPosition.y - protein.center_y, 2);
                    double z_diff_center = std::pow(LigPosition.z - protein.center_z, 2);
                    double rawDistCenter = std::sqrt(x_diff_center + y_diff_center + z_diff_center);

                    score += 0.00001 * rawDistCenter;
*/

                } // for
            } // for
            /*
            BOOST_LOG_TRIVIAL(debug) << "Intermolecular scoring contribution without weighting :";
            BOOST_LOG_TRIVIAL(debug) << "Gauss 1      : " << std::fixed<< std::setprecision(5) << score_gauss1;
            BOOST_LOG_TRIVIAL(debug) << "Gauss 2      : " << std::fixed<< std::setprecision(5) << score_gauss2;
            BOOST_LOG_TRIVIAL(debug) << "Repulsion    : " << std::fixed<< std::setprecision(5) << score_repulsion;
            BOOST_LOG_TRIVIAL(debug) << "Hydrophobic  : " << std::fixed<< std::setprecision(5) << score_hydrophobic;
            BOOST_LOG_TRIVIAL(debug) << "Hydrogen     : " << std::fixed<< std::setprecision(5) << score_hydrogen;
            BOOST_LOG_TRIVIAL(debug) << "------------------------------------------";
            BOOST_LOG_TRIVIAL(debug) << "Raw Score    : " << score_raw;
            BOOST_LOG_TRIVIAL(debug) << "Nrotatable   : " << ligand.num_rotatable_bond;
            //*/
            double final_score = score_raw / (1 + (0.058459999999999998 * ligand.num_rotatable_bond));
            /*
            BOOST_LOG_TRIVIAL(debug) << "------------------------------------------";
            BOOST_LOG_TRIVIAL(debug) << "Final Score  : " << final_score;
            //*/

            return final_score;
        }

        double VinaLikeRigid::Evaluate(const arma::mat &x) {
            BOOST_ASSERT(x.n_rows == 7);

            iTransform tr = this->internalToExternalRepr(x);
            tr.doHousekeeping();

            double score_ = vina_like_rigid_inter_scoring_func(this->startingConformation, tr, this->prot);

            return score_;
        }

        double VinaLikeRigid::EvaluateWithGradient(const arma::mat &x, arma::mat &grad) {

            BOOST_ASSERT(!x.has_nan());
            BOOST_ASSERT(!grad.has_nan());
            BOOST_ASSERT(x.n_rows == 7);
            BOOST_ASSERT(grad.n_rows == 7);

            iTransform tr = this->internalToExternalRepr(x);
            tr.doHousekeeping();

            double score_ = vina_like_rigid_inter_scoring_func(this->startingConformation, tr, this->prot);

            // Translation
            {
                iTransform transform_dx = tr;
                transform_dx.transl.x() += this->differential_epsilon;
                grad[0] = vina_like_rigid_inter_scoring_func(this->startingConformation, transform_dx, this->prot) -
                          score_;
            }

            {
                iTransform transform_dy = tr;
                transform_dy.transl.y() += this->differential_epsilon;
                grad[1] = vina_like_rigid_inter_scoring_func(this->startingConformation, transform_dy, this->prot) -
                          score_;
            }

            {
                iTransform transform_dz = tr;
                transform_dz.transl.z() += this->differential_epsilon;
                grad[2] = vina_like_rigid_inter_scoring_func(this->startingConformation, transform_dz, this->prot) -
                          score_;
            }

            // Rotation

            {
                iTransform transform_ds = tr;
                transform_ds.rota.w() += this->differential_epsilon;
                transform_ds.doHousekeeping();
                grad[3] = vina_like_rigid_inter_scoring_func(this->startingConformation, transform_ds, this->prot) -
                          score_;
            }

            {
                iTransform transform_du = tr;
                transform_du.rota.x() += this->differential_epsilon;
                transform_du.doHousekeeping();
                grad[4] = vina_like_rigid_inter_scoring_func(this->startingConformation, transform_du, this->prot) -
                          score_;
            }

            {
                iTransform transform_dv = tr;
                transform_dv.rota.y() += this->differential_epsilon;
                transform_dv.doHousekeeping();
                grad[5] = vina_like_rigid_inter_scoring_func(this->startingConformation, transform_dv, this->prot) -
                          score_;
            }

            {
                iTransform transform_dt = tr;
                transform_dt.rota.z() += this->differential_epsilon;
                transform_dt.doHousekeeping();
                grad[6] = vina_like_rigid_inter_scoring_func(this->startingConformation, transform_dt, this->prot) -
                          score_;
            }
            /*
            BOOST_LOG_TRIVIAL(debug) << "Transform: " << x.t();
            BOOST_LOG_TRIVIAL(debug) << "Score: " << score_;
            BOOST_LOG_TRIVIAL(debug) << "Gradient";
            BOOST_LOG_TRIVIAL(debug) << "     ds: " << grad[3];
            BOOST_LOG_TRIVIAL(debug) << "     du: " << grad[4] << "   dx: " << grad[0];
            BOOST_LOG_TRIVIAL(debug) << "     dv: " << grad[5] << "   dy: " << grad[1];
            BOOST_LOG_TRIVIAL(debug) << "     dt: " << grad[6] << "   dx: " << grad[2];
            //*/

            BOOST_ASSERT(score_ == score_); // catches NaN
            return score_;
        }

        VinaLikeRigid::VinaLikeRigid(const iConformer &startingConformation_,
                                                                   const iProtein &p,
                                                                   const iTransform &initialTransform_,
                                                                   double differential_epsilon_) :

                startingConformation(startingConformation_),
                prot(p),
                initialTransform(initialTransform_),
                differential_epsilon(differential_epsilon_) {}

        double VinaLikeRigid::getDifferentialEpsilon() const {
            return this->differential_epsilon;
        }

        arma::mat VinaLikeRigid::getStartingConditions() const {
            return this->externalToInternalRepr(this->initialTransform);
        }

        iConformer VinaLikeRigid::getConformerForParamMatrix(const arma::mat &x) {
            BOOST_ASSERT(x.n_rows == 7);

            iTransform tr = this->internalToExternalRepr(x);
            normalizeQuaternionInPlace(
                    tr.rota); //!< Note that the internal representation arma::mat is not by itself normalized
            //! (because we always normalize it in the scoring function, so no constraint)

            iConformer ret = this->startingConformation;
            applyRigidTransformInPlace(ret, tr);

            return ret;
        }

        unsigned int VinaLikeRigid::getParamVectorDimension() const {
            return 7;
        }

        std::vector<std::tuple<std::string, double>> VinaLikeRigid::EvaluateSubcomponents(const arma::mat &x) {
            std::vector<std::tuple<std::string, double>> ret;

            BOOST_ASSERT(x.n_rows == 7);

            iTransform tr = this->internalToExternalRepr(x);
            normalizeQuaternionInPlace(tr.rota);

            BOOST_ASSERT(!this->startingConformation.x.empty());
            BOOST_ASSERT(!this->prot.x.empty());

            if(std::abs(tr.rota.norm() - 1) > 0.1) {
                tr.rota.normalize();
            }

            BOOST_ASSERT(tr.bondRotationsAngles.size() == this->startingConformation.num_rotatable_bond);

            double gauss1_total = 0.0;
            double gauss2_total = 0.0;
            double repulsion_total = 0.0;
            double hydrogen_total = 0.0;
            double hydrophobic_total = 0.0;
            double score_raw = 0.0;

            iConformer ligand = this->startingConformation;
            applyBondRotationInPlace(ligand, tr);

            Eigen::Vector3d ProtCenterPosition = {this->prot.center_x, this->prot.center_y, this->prot.center_z};

            for (unsigned int idxLig = 0; idxLig < ligand.x.size(); idxLig++) {
                for (unsigned int idxProt = 0; idxProt < this->prot.x.size(); idxProt++) {

                    Eigen::Vector3d LigPosition = {ligand.x[idxLig], ligand.y[idxLig], ligand.z[idxLig]};
                    applyRigidTransformInPlace(LigPosition, tr);

                    Eigen::Vector3d ProtPosition = {this->prot.x[idxProt], this->prot.y[idxProt], this->prot.z[idxProt]};
                    Eigen::Vector3d distToCenterVector = LigPosition - ProtCenterPosition;

                    double distanceToProteinCenter = distToCenterVector.norm();

                    if (distanceToProteinCenter > (this->prot.radius - 1)) {
                        //FIXME : temporary disable
                        //score_raw += std::pow((distanceToProteinCenter - this->prot.radius), 4) + 10;
                        continue;
                    }

                    Eigen::Vector3d distVect = ProtPosition - LigPosition;

                    double rawDist = distVect.norm();

                    const double cutoff = 8.0;
                    if (rawDist >= cutoff)
                        continue;

                    double distance = distanceFromRawDistance(rawDist,  ligand.atomicRadius[idxLig], this->prot.atomicRadius[idxProt]);

                    unsigned char atom1AtomicNumber = ligand.type[idxLig];
                    unsigned int atom1AtomVariant = ligand.variant[idxLig];
                    unsigned char atom2AtomicNumber = this->prot.type[idxProt];
                    unsigned int atom2AtomVariant = this->prot.variant[idxProt];

                    gauss1_total += vinaGaussComponent(distance, 0.0, 0.5);
                    gauss2_total += vinaGaussComponent(distance, 3.0, 2.0);
                    repulsion_total += vinaRepulsionComponent(distance, 0.0);
                    hydrogen_total += vinaHydrophobicComponent(distance,
                                                               atom1AtomicNumber, atom1AtomVariant,
                                                               atom2AtomicNumber, atom2AtomVariant);
                    hydrophobic_total += vinaHydrogenComponent(distance,
                                                               atom1AtomicNumber, atom1AtomVariant,
                                                               atom2AtomicNumber, atom2AtomVariant);
                    score_raw += scoreForAtomCouple(distance,
                                                    atom1AtomicNumber, atom1AtomVariant,
                                                    atom2AtomicNumber, atom2AtomVariant);

                } // for
            } // for

            double final_score = score_raw / (1 + (0.058459999999999998 * ligand.num_rotatable_bond));

            ret.emplace_back(std::make_tuple("Gauss1",gauss1_total));
            ret.emplace_back(std::make_tuple("Gauss2",gauss2_total));
            ret.emplace_back(std::make_tuple("Repulsion",repulsion_total));
            ret.emplace_back(std::make_tuple("Hydrophobic",hydrogen_total));
            ret.emplace_back(std::make_tuple("Hydrogen",hydrophobic_total));
            ret.emplace_back(std::make_tuple("ScoreRaw",score_raw));
            ret.emplace_back(std::make_tuple("Score",final_score));
            return ret;
        }

        double VinaLikeRigid::EvaluateOnlyIntermolecular(const arma::mat &x) {
            return this->Evaluate(x);
        }

        unsigned int VinaLikeRigid::getCoefficientsVectorWidth() {
            return this->numCoefficients;
        }

        std::vector<std::string> VinaLikeRigid::getCoefficientsNames() {
            return std::vector<std::string>(this->coefficientsNames.begin(),this->coefficientsNames.end());
        }

        std::vector<double> VinaLikeRigid::getCurrentCoefficients() {
            return {VinaClassic::coeff_gauss1, VinaClassic::coeff_gauss2,
                    VinaClassic::coeff_repulsion, VinaClassic::coeff_hydrophobic,
                    VinaClassic::coeff_hydrogen };
        }

        bool VinaLikeRigid::setNonDefaultCoefficients(std::vector<double> coeffs) {
            BOOST_LOG_TRIVIAL(debug) << "Non default coefficients not supported yet on Vina rigid scoring function.";
            return false;
        }

    }

}


//
//  main.cpp
//  PhyloAcc
//
//  Created by hzr on 3/8/16.
//  Copyright © 2016 hzr. All rights reserved.
//
#include <dirent.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <algorithm>
#include <iomanip>
//#include <omp.h> //libiomp/
#include <string>
#include <armadillo>
#include "profile.h"
#include "newick.h"
#include "newick2.h"
#include "bpp.hpp"
#include "bpp_c.hpp"
#include "utils.h"
#include <math.h>
#include <gsl/gsl_errno.h>

using namespace std;
using namespace arma;

// parameters, input and output files paths
string params_path;
string phytree_path;
string align_path;
string output_path="";
string output_path2="";
string segment_path;
string id_path="";
string result_prefix="test";
string tree_coal_unit="";

//string refspecies;
string outgroup;
string targetspecies;
string conservegroup; // can't be missing in more than 50%
string deepcoal_species = ""; //species in which deep coal can happen
double conserve_prop = 0.8;

int num_thread = 1;

// running parameters
int num_burn = 200;         // num of burn-in updates is num_burn * num_thin
int num_mcmc = 800;         // num of MCMC updates, num_burn * num_mcmc
int num_thin = 1;    // num of updates between two samples, adaptive frequency = 50 * num_thin
int num_chain; // outer loop of updates Q matrix and hyperparameter of substitution rates

double prep_lrate = 0.5;
double prep_lrate2 = 0.0; //0.1
double prep_grate = 0.8; // initalization

double prior_lrate2_a = 0.0,prior_lrate2_b = 1 ; // beta prior for lrate2, 0.5
double prior_lrate_a = 1 ,prior_lrate_b = 1 ; // beta prior for lrate, 1,9
double prior_grate_a = 1,prior_grate_b = 1; // beta prior for grate, 3,1

double ratio0 = 0.5; //initial conserved rate,0.5
double ratio1 = 1; // initial accelerated rate
double missing_thres = 0.8;

double nprior_a = 10, nprior_b = 0.2;  //around 1
double cprior_a = 5, cprior_b = 0.04;  //around ratio
int ropt = 1;
double cub = 1;
double nlb = 0.6;

int batch = -1 ;
int seed = 1;
int seed2 = 1;
double indel = 0;
double indel2;
bool sample_indel = 0;
bool sample_hyper = false;
char gapchar = '-';
bool verbose = 0;
double consToMis = 0.5; //for simulation, 0.01;
int block = 15; // 25; //90;
bool prune=0;
double revgap=0.9;  // 1
int min_length = 50;
bool WL = true;
bool simulate=false;
bool verboseGT = 1; //Han*: output trace_genetrees
double br_sample_cutoff = 10.0;
double theta_cutoff = 1.0;

//Han*: add Dirichlet prior for stationary distribution pi:
//vector<double> prior_dir_par(4,10);
vector<double> prior_dir_par(2,10);  //Han: it's beta prior for pi. Keep the old name.

// load the program parameters
void LoadParams(int argc, char* argv[])
{
    cout << "Loading input data and running parameters......" << endl;

    if (argc > 1)
        params_path = string(argv[1]);
    else
        params_path = "/Users/zhiruihu/Phylogenetics/test_mammalData/param_chr2_Long2.txt"; //params2

    cout << "Loading program configurations from " << params_path << "......" <<endl;

    const int BUFF_SIZE = 1024;
    char line_buff[BUFF_SIZE];

    ifstream in_params(params_path.c_str());
    if (!in_params)
    {
        cerr << "Cannot open the parameters file: " << params_path.c_str() << endl;
        exit(1);
    }
    while(!in_params.eof())
    {
        in_params.getline(line_buff, BUFF_SIZE);
        istringstream line_stream(line_buff);
        string tmp; line_stream >> tmp;

        // simulate or inference
        if (tmp=="SIMULATE")
            line_stream >> simulate;
        // input and output file paths
        else if (tmp=="PHYTREE_FILE")
            line_stream >> phytree_path;
        else if (tmp=="TREE_IN_COALESCENT_UNIT")
            line_stream >> tree_coal_unit;
        else if (tmp=="ALIGN_FILE")
            line_stream >> align_path;
        else if (tmp=="SEG_FILE")
            line_stream >> segment_path;
        else if (tmp=="ID_FILE")
            line_stream >> id_path;
        else if (tmp=="BATCH")
            line_stream >> batch;
        else if (tmp=="RESULT_FOLDER")
            line_stream >> output_path;
        else if (tmp=="PREFIX")
            line_stream >> result_prefix;
        else if (tmp=="SEED")
            line_stream >> seed;
        else if (tmp=="SEEDS")
            line_stream >> seed2;
        else if (tmp=="INIT_CONSERVE_RATE")
            line_stream >> ratio0;
        else if (tmp=="INIT_ACCE_RATE")
            line_stream >> ratio1;
        else if (tmp=="CONSERVE_PRIOR_A")
            line_stream >> cprior_a;
        else if (tmp=="CONSERVE_PRIOR_B")
            line_stream >> cprior_b;
        else if (tmp=="ACCE_PRIOR_A")
            line_stream >> nprior_a;
        else if (tmp=="ACCE_PRIOR_B")
            line_stream >> nprior_b;
        else if (tmp=="ROPT")
            line_stream >> ropt;
        else if (tmp=="CUB")
            line_stream >> cub;
        else if (tmp=="NLB")
            line_stream >> nlb;

        // running parameters
        else if (tmp=="BURNIN")
            line_stream >> num_burn;
        else if (tmp=="MCMC")
            line_stream >> num_mcmc;
        else if (tmp=="THIN") //ADAPT_FREQ
            line_stream >> num_thin;
        else if (tmp=="INIT_LRATE")
            line_stream >> prep_lrate;
        else if (tmp=="INIT_GRATE")
            line_stream >> prep_grate;
        else if (tmp=="HYPER_LRATE_A")
            line_stream >> prior_lrate_a;
        else if (tmp=="HYPER_LRATE_B")
            line_stream >> prior_lrate_b;
        else if (tmp=="HYPER_GRATE_A")
            line_stream >> prior_grate_a;
        else if (tmp=="HYPER_GRATE_B")
            line_stream >> prior_grate_b;
        else if (tmp=="CHAIN")
            line_stream >> num_chain;
        else if (tmp=="WL")
            line_stream >> WL;
        else if (tmp=="BLK_WL")
            line_stream >> block;

        // constraint
        else if (tmp == "OUTGROUP")
            line_stream >> outgroup;
        else if (tmp == "TARGETSPECIES")
            line_stream >> targetspecies;
        else if (tmp == "CONSERVE")
            line_stream >> conservegroup;
        else if (tmp == "CONSERVE_PROP")
            line_stream >> conserve_prop;
        else if (tmp == "GAP_PROP")
            line_stream >> missing_thres;
        //else if (tmp == "REF")
         //   line_stream >> refspecies;
        else if (tmp == "CONSTOMIS")
            line_stream >> consToMis;
        else if (tmp=="BR_SAMPLE_THRESHOLD")
            line_stream >> br_sample_cutoff;
        else if (tmp=="THETA_CUTOFF")
            line_stream >> theta_cutoff;
        else if( tmp=="DEEP_COAL_BRANCH")
            line_stream >> deepcoal_species;

        // treat indel as additional character
        else if (tmp == "GAPCHAR")
            line_stream >> gapchar;
        else if (tmp == "PRUNE_TREE")
            line_stream >> prune;
        else if (tmp == "TRIM_GAP_PERCENT")
            line_stream >> revgap;
        else if (tmp == "MIN_LEN")
            line_stream >> min_length;
        else if (tmp == "INDEL") // not used
            line_stream >> indel;
        else if (tmp == "INDEL2") // not used
            line_stream >> indel2;
        else if(tmp == "SAMPLE_INDEL")  // not used
            line_stream >> sample_indel;
        else if(tmp == "SAMPLE_HYPER")
            line_stream >> sample_hyper;
        else if(tmp == "VERBOSE")
            line_stream >> verbose;
        else if(tmp == "VERBOSE_GENETREE") //Han*: output sampled gene trees
            line_stream>>verboseGT;
        else if(tmp == "NUM_THREAD")
            line_stream >> num_thread;
        else if(tmp != "")
            cout << "Unknown parameter: " << tmp <<endl;

    }

    // trimming file names
    phytree_path = strutils::trim(phytree_path, " \"\t\n");
    align_path = strutils::trim(align_path, " \"\t\n");
    output_path  = strutils::trim(output_path,  " \"\t\n");
    segment_path = strutils::trim(segment_path, " \"\t\n");
    tree_coal_unit = strutils::trim(tree_coal_unit, " \"\t\n");
}

bool DirectoryExists( string pzPath )
{
    if ( pzPath == "") return false;

    DIR *pDir;
    bool bExists = false;

    pDir = opendir (pzPath.c_str());

    if (pDir != NULL)
    {
        bExists = true;
        (void) closedir (pDir);
    }

    return bExists;
}

void DispParams(PhyloProf profile, int seed)
{
    double mean_seg_size = 0;
    for(unsigned int c=0; c<profile.C; c++)
        mean_seg_size += (double)(profile.element_pos[c][1] - profile.element_pos[c][0]) / profile.C;
    cout << "  # total length = " << profile.G << " (" << profile.C << ")" << ". # Species = " << profile.S << ". # elements = " << profile.C << ". Mean gene set size = " << mean_seg_size << "." << endl;
    cout << "# Burn-ins = " << num_burn*num_thin << ". # MCMC Updates = " << num_mcmc*num_thin << ". # thin = " << num_thin << ".  RND SEED = " << seed << "." << endl ; //
    cout << "# Threads = " << num_thread << endl << endl;
}

int main(int argc, char* argv[])
{
    time_t start = time(NULL);

    cout << std::fixed << setprecision(4);
    srand(time(NULL));

    // load the program parameters
    LoadParams(argc, argv);

    // check output path
    if(! DirectoryExists(output_path))
    {
    	cout << "output path doesn't exist or empty!" << endl;
    	return 1;
    }

    // load the phylogenetic profile
    PhyloProf profile = LoadPhyloProfiles(align_path,segment_path);
    // init and display the running parameters
    DispParams(profile, seed);

    PhyloTree_theta tree2;
    if(tree_coal_unit !=""){
        tree2 = LoadPhyloTree_theta(tree_coal_unit);
    }else{
        cout<<"please input a phylogengy with branch length in coalescent unit."<<endl;
        return 1;
    }

    // load the phylogenetic tree
    PhyloTree phytree = LoadPhyloTree(phytree_path); //Han: .subs_rate contains Q

    //get thetas
    double theta_cum=0;
    int count_cum=0;
    int N = phytree.nodes_names.size();
    vector<int> pos_cum=vector<int> (N,0);
    for(int i=0; i<(N-1); i++){
        if(phytree.nodes_names[i]!=tree2.nodes_names[i]){
            cout<<"i="<<i<<". tree1_name="<<phytree.nodes_names[i]<<", tree2_name="<<tree2.nodes_names[i]<<endl;
            cerr<<"two trees do not have the same topology"<<endl;
            exit(1);
        }else{
            if(i<phytree.S){
                phytree.thetas[i]=0;
            }else{
                //cout<<"\ni="<<i<<": ";
                if((tree2.distances[i]!=1.0) && (tree2.distances[i]!= 7.0) && (tree2.distances[i]!= 0)){
                    phytree.thetas[i]=2*phytree.distances[i]/tree2.distances[i];
                    if(phytree.thetas[i]>= theta_cutoff){
                        pos_cum[i]=1;
                    }else{
                        theta_cum+=phytree.thetas[i];
                        count_cum+=1;
                    }
                }else{
                    pos_cum[i]=1;
                }
            }
        }
    }
    double theta_aver=theta_cum/count_cum;
    pos_cum[N-1]=1;
    for(int i=phytree.S; i<N; i++){
        if(pos_cum[i]==1) phytree.thetas[i]=theta_aver;
        //cout<<"node "<<phytree.nodes_names[i]<<" theta="<<phytree.thetas[i]<<endl;
    }

    // create and init the BPP object
    //BPP bpp(0, profile, phytree, output_path, targetspecies, outgroup, conserve_prop, conservegroup, ratio0, ratio1, ropt, cub, nlb, nprior_a, nprior_b, cprior_a, cprior_b, seed, seed2, prep_grate, prep_lrate, prep_lrate2, prior_grate_a, prior_grate_b,prior_lrate_a, prior_lrate_b,prior_lrate2_a, prior_lrate2_b,  indel, indel2, missing_thres, sample_indel,prior_dir_par, br_sample_cutoff);
    BPP bpp(0, profile, phytree, output_path, targetspecies, outgroup, conserve_prop, conservegroup, ratio0, ratio1, ropt, cub, nlb, nprior_a, nprior_b, cprior_a, cprior_b, seed, seed2, prep_grate, prep_lrate, prep_lrate2, prior_grate_a, prior_grate_b,prior_lrate_a, prior_lrate_b,prior_lrate2_a, prior_lrate2_b,  indel, indel2, missing_thres, sample_indel,prior_dir_par, br_sample_cutoff, deepcoal_species);

    //initialize the MCMC sampling
    bpp.InitMCMC(num_burn, num_mcmc, num_thin);

    output_path = output_path + "/" + result_prefix ;
    output_path2 = output_path;
    string outpath_Z0 = output_path + "_rate_postZ_M" +to_string(0) +".txt";
    string outpath_Z1 = output_path + "_rate_postZ_M" +to_string(1) +".txt";
    string outpath_Z2 = output_path + "_rate_postZ_M" +to_string(2) +".txt";

    string outpath_hyper = output_path+"_hyper.txt";
    ofstream out_hyper(outpath_hyper.c_str());
    out_hyper << "iter\tnprior_a\tnprior_b\tcprior_a\tcprior_b\tprior_l_a\tprior_l_b\tprior_g_a\tprior_g_b\n";
    out_hyper << 0 << "\t"<< nprior_a<< "\t"<< nprior_b <<"\t"<< cprior_a << "\t"<< cprior_b << "\t"<< prior_lrate_a << "\t"<< prior_lrate_b << "\t"<< prior_grate_a << "\t"<< prior_grate_b <<endl;

    ofstream out_lik;
    if(sample_hyper) {
        string outpath_elem = output_path+ "_elem_lik.txt";
        out_lik.open(outpath_elem.c_str());
        out_lik.precision(8);
        out_lik << "No.\tID\tloglik_Full\tloglik_Max"<<endl;
    }

    ofstream out_Z0(outpath_Z0.c_str());
    ofstream out_Z1(outpath_Z1.c_str());
    ofstream out_Z2(outpath_Z2.c_str());

    // output species name
    string species_name = output_path+"_species_names.txt";
    ofstream out_species(species_name.c_str());

    out_Z0 << "No.\tn_rate\tc_rate\tg_rate\tl_rate\tl2_rate"; out_Z1 << "No.\tn_rate\tc_rate\tg_rate\tl_rate\tl2_rate"; out_Z2 << "No.\tn_rate\tc_rate\tg_rate\tl_rate\tl2_rate";
    for(int s=0; s<bpp.N;s++){
         for(int k=0;k<4;k++){
            out_Z0 <<"\t"<<bpp.nodes_names[s]<<"_"<<k;
            out_Z1 <<"\t"<<bpp.nodes_names[s]<<"_"<<k;
            out_Z2 <<"\t"<<bpp.nodes_names[s]<<"_"<<k;
         }
         out_species << bpp.nodes_names[s] << endl;
    }
	out_Z0 <<endl; out_Z1 <<endl; out_Z2 <<endl;

    out_species.close();

    // output tree
    outpath_Z0 = output_path + "_tree_M" +to_string(0) +".txt";
    outpath_Z1 = output_path + "_tree_M" +to_string(1) +".txt";
    outpath_Z2 = output_path + "_tree_M" +to_string(2) +".txt";

    ofstream out_tree0(outpath_Z0.c_str());
    ofstream out_tree1(outpath_Z1.c_str());
    ofstream out_tree2(outpath_Z2.c_str());

    out_tree0 << "No.\tprop\tgenetree\n";
    out_tree1 << "No.\tprop\tgenetree\n";
    out_tree2 << "No.\tprop\tgenetree\n";

    double lrate_prop = 0.5, grate_prop = 0.5;

    vector<int> ids;
    if(id_path=="")
    {
        if(batch==-1)
        {
          for(int c =0;c<500;c++) //bpp.C,
          {
            ids.push_back(c);
          }
        }else{
          int temp = ceil(bpp.C/3);
          for(int c =batch*temp ;c< (batch+1)*temp;c++)
          {
            if(c >= bpp.C) break;
            ids.push_back(c);
          }

        }

    }else{
        ifstream in_params(id_path.c_str());
        if (!in_params)
        {
            cerr << "Cannot open the id file: " << id_path.c_str() << endl;
            exit(1);
        }
        while(!in_params.eof())
        {
            const int BUFF_SIZE = 1024;
            char line_buff[BUFF_SIZE];
            in_params.getline(line_buff, BUFF_SIZE);
            istringstream line_stream(line_buff);
            string tmp; line_stream >> tmp;
            tmp = strutils::trim(tmp);
            if(tmp=="") continue;
            ids.push_back(atoi(tmp.c_str()));

        }

    }

    cout << ids.size() << " elements to be computed" << endl;

    if(sample_hyper)
    {
        for(int iter  =0; iter<num_chain; iter++)
        {
            cout << "Running MCMC chain " << iter +1 << " ..." << endl;
            // Gibbs sampling
            #pragma omp parallel for schedule (guided) num_threads(num_thread)
            for(std::size_t i = 0; i < ids.size(); i++ )
            {
                int c = ids[i];
                bool filter = false;

                try{
                    BPP_C bppc(c, profile, bpp, gapchar, missing_thres, filter, verbose, consToMis, block, prune, revgap, min_length);  // for individual element
                    if(filter) {
                        if(verbose) cerr << "filter: "<< c <<endl;
                        continue;
                    }

                    bppc.initMCMC(0,5,bpp,1,prune);
                    bppc.Gibbs(0, 4, bpp,out_Z2,output_path,output_path2,1, true, sample_hyper, lrate_prop, grate_prop, false);  // Gibbs run to get Z for each element

                    if(bppc.verbose || bppc.failure) bppc.Output_sampling(iter, output_path2, bpp, 2);
                    bppc.Output_init(output_path,output_path2,bpp,out_Z2, out_tree2, bppc.verbose); //sort rates!!

                }catch (exception& e){
                    cout << c << " Standard exception: " << e.what() << endl;
                }
            }
            bpp.sample_hyperparam(iter, ids, out_hyper);
            bpp.Output_init0(profile,out_lik, ids);

        }
    }else if(simulate)
    {
        for(std::size_t i = 0; i < ids.size(); i++ )
        {
            int c = ids[i];
            bool filter = false;
            try{
                // accelerate in target species
                BPP_C bppc(c, profile, bpp, gapchar, missing_thres, filter, verbose, consToMis, block, prune, revgap, min_length);  // for individual element
                bppc.simulate(bpp, profile, gapchar,prune);
            }catch (exception& e){
                cout << c << " Standard exception: " << e.what() << endl;
            }
        }

        //write out simulate sequence
        bpp.Output_simu(profile, output_path, ids.size());
    }else{
        // Gibbs sampling
        #pragma omp parallel for schedule (guided) num_threads(num_thread)
        for(std::size_t i = 0; i < ids.size(); i++ ) //
        {
            int c = ids[i];
            bool filter = false;

            try{

                BPP_C bppc(c, profile, bpp, gapchar, missing_thres, filter, verbose, verboseGT, consToMis, block, prune, revgap, min_length);  // for individual element
                cout<<"element "<<to_string(c)<<", number of base pair="<<to_string(bppc.GG)<<endl;
                if(filter) {
                  if(verbose) cerr << "filter: "<< c <<endl;
                  continue;
                }

                int tot = 0;
                if(bppc.idblk_count==0){
                    double nblk=(double)(bppc.GG - 15)/block;
                    int nblk2=(bppc.GG - 15)/block;
                    if( (nblk- nblk2)< ((double) 1.0/3.0)){
                        tot=nblk2;
                    }else{
                        tot=nblk2+1;
                    }
                    //tot=ceil((double)(bppc.GG - 15)/block);  //first bp length is always 15: len={0,15}. If change 15, change in Gibbs as well.
                    //have to make sure 15<=block. So better change 15 to block
                }else{
                    double nblk = (double)(bppc.GG - bppc.idblk_count-15)/block;
                    int nblk2 = (bppc.GG - bppc.idblk_count-15)/block;
                    if((nblk- nblk2)< ((double) 1.0/3.0)){
                        tot = 1+nblk2;
                    }else{
                        tot = 2+nblk2;
                    }
                    //tot=1+ceil((double)(bppc.GG - bppc.idblk_count-15)/block);
                }
                // int tot=ceil((double)(bppc.GG - 15)/block);
                //cout<<"tot ="<<tot<<endl;

                //null model
                cout<<"start null model\n";
                for (int iter = 0; iter <= tot; iter++)
                {
                    // cout<<"start initMCMC"<<endl;
                    bppc.initMCMC(iter, tot, bpp, 0, prune, false);

                    // cout<<"start Gibbs"<<endl;
                    bppc.Gibbs(iter, tot, bpp, out_Z0, output_path, output_path2, 0, true, sample_hyper, lrate_prop, grate_prop, WL); // Gibbs run to get Z for each element
                    if (bppc.verbose || bppc.failure)
                    {
                        bppc.Output_sampling(iter, output_path2, bpp, 0);
                        bppc.Output_tree(iter, output_path2, bpp, 0);
                    }
                }
                bppc.Output_init(output_path,output_path2,bpp,out_Z0, out_tree0, bppc.verbose); //sort rates!!, posterior median of nrate and crate; posterior mean of Z

                // //res model
                cout<<"start restricted model\n";
                for (int iter = 0; iter <= tot; iter++)
                {
                    bppc.initMCMC(iter, tot, bpp, 2, prune, false);
                    // cout<<"start Gibbs"<<endl;
                    bppc.Gibbs(iter, tot, bpp, out_Z2, output_path, output_path2, 2, true, sample_hyper, lrate_prop, grate_prop, WL);
                    if (bppc.verbose || bppc.failure)
                    {
                        bppc.Output_sampling(iter, output_path2, bpp, 1);
                        bppc.Output_tree(iter, output_path2, bpp, 1);
                    }
                }
                bppc.Output_init(output_path,output_path2,bpp,out_Z1, out_tree1, bppc.verbose);

                // full model
                cout<<"start full model\n";
                for (int iter = 0; iter <= tot; iter++)
                {
                    bppc.initMCMC(iter, tot, bpp, 1, prune, false); // not constrain log_prob_back
                    bppc.Gibbs(iter, tot, bpp, out_Z2, output_path, output_path2, 1, true, sample_hyper, lrate_prop, grate_prop, WL);
                    if (bppc.verbose || bppc.failure)
                    {
                        bppc.Output_sampling(iter, output_path2, bpp, 2);
                        bppc.Output_tree(iter, output_path2, bpp, 2);
                    }
                }
                bppc.Output_init(output_path,output_path2,bpp,out_Z2, out_tree2, bppc.verbose);

                cout << c << "\t" << bpp.log_liks_WL[0][c] <<"\t" <<  bpp.log_liks_WL[2][c] <<"\t" <<  bpp.log_liks_WL[1][c] <<endl;
                cout<<"\t" << bpp.log_liks_Z[0][c] << "\t" << bpp.log_liks_Z[2][c]<<"\t" << bpp.log_liks_Z[1][c] << endl;

            }catch (exception& e){
              cout << c << " Standard exception: " << e.what() << endl;
            }
      }

      bpp.Output_init(profile,output_path, ids);
    }

    out_Z0.close();
    out_Z1.close();
    out_Z2.close();
    out_hyper.close();
    out_lik.close();
    out_tree0.close();
    out_tree1.close();
    out_tree2.close();

    cout << endl << endl << "time used:  " << (time(NULL)-start)/60 << " min." << endl << endl;
    return 0;
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file multi_sensor_sigma_point_update_policy.hpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <fl/util/meta.hpp>
#include <fl/util/types.hpp>
#include <fl/util/traits.hpp>
#include <fl/util/descriptor.hpp>
#include <fl/model/sensor/joint_sensor_iid.hpp>
#include <fl/filter/gaussian/transform/point_set.hpp>
#include <fl/filter/gaussian/quadrature/sigma_point_quadrature.hpp>

namespace fl
{
// Forward declarations
template <typename...>
class MultiSensorSigmaPointUpdatePolizzle;

/**
 * \internal
 */
template <typename SigmaPointQuadrature, typename NonJoinSensor>
class MultiSensorSigmaPointUpdatePolizzle<SigmaPointQuadrature,
                                          NonJoinSensor>
{
    static_assert(
        std::is_base_of<internal::JointSensorIidType,
                        NonJoinSensor>::value,
        "\n\n\n"
        "====================================================================\n"
        "= Static Assert: You are using the wrong observation model type    =\n"
        "====================================================================\n"
        "  Observation model type must be a JointSensor<...>.      \n"
        "  For single observation model, use the regular Gaussian filter     \n"
        "  or the regular SigmaPointUpdatePolicy if you are specifying       \n"
        "  the update policy explicitly fo the GaussianFilter.               \n"
        "===================================================================="
        "\n");
};

template <typename SigmaPointQuadrature, typename MultipleOfLocalSensor>
class MultiSensorSigmaPointUpdatePolizzle<
    SigmaPointQuadrature,
    JointSensor<MultipleOfLocalSensor>>
    : public MultiSensorSigmaPointUpdatePolizzle<
          SigmaPointQuadrature,
          NonAdditive<JointSensor<MultipleOfLocalSensor>>>
{
};

template <typename SigmaPointQuadrature, typename MultipleOfLocalSensor>
class MultiSensorSigmaPointUpdatePolizzle<
    SigmaPointQuadrature,
    NonAdditive<JointSensor<MultipleOfLocalSensor>>>
    : public Descriptor
{
public:
    typedef JointSensor<MultipleOfLocalSensor> JointModel;
    typedef typename MultipleOfLocalSensor::Type FeatureSensor;
    typedef typename FeatureSensor::EmbeddedSensor BodyTailModel;
    typedef typename BodyTailModel::BodySensor BodyModel;
    typedef typename BodyTailModel::TailSensor TailModel;

    typedef typename JointModel::State State;
    typedef typename JointModel::Obsrv Obsrv;
    typedef typename JointModel::Noise Noise;

    typedef typename Traits<JointModel>::LocalObsrv LocalFeature;
    typedef Vector1d LocalObsrvNoise;

    template <typename Belief>
    void operator()(JointModel& obsrv_function,
                    const SigmaPointQuadrature& quadrature,
                    const Belief& prior_belief,
                    const Obsrv& y,
                    Belief& posterior_belief)
    {
        auto& feature_model = obsrv_function.local_sensor();
        auto& body_tail_model = feature_model.embedded_sensor();

        /* ------------------------------------------ */
        /* - Determine the number of quadrature     - */
        /* - points needed for the given quadrature - */
        /* - in conjunction with the joint Gaussian - */
        /* - p(State, LocalObsrvNoise)              - */
        /* ------------------------------------------ */
        enum : signed int
        {
            NumberOfPoints = SigmaPointQuadrature::number_of_points(
                JoinSizes<SizeOf<State>::Value,
                          SizeOf<LocalObsrvNoise>::Value>::Size)
        };

        /* ------------------------------------------ */
        /* - PointSets                              - */
        /* - [p_X, p_Q] ~ p(State, LocalObsrvNoise) - */
        /* ------------------------------------------ */
        PointSet<State, NumberOfPoints> p_X;
        PointSet<LocalObsrvNoise, NumberOfPoints> p_R;

        /* ------------------------------------------ */
        /* - Transform p(State, LocalObsrvNoise) to - */
        /* - point sets [p_X, p_Q]                  - */
        /* ------------------------------------------ */
        quadrature.transform_to_points(
            prior_belief,
            Gaussian<LocalObsrvNoise>(feature_model.noise_dimension()),
            p_X,
            p_R);

        auto mu_x = p_X.mean();
        auto X = p_X.centered_points();

        auto W = p_X.covariance_weights_vector().asDiagonal();
        auto c_xx = (X * W * X.transpose()).eval();
        auto c_xx_inv = c_xx.inverse().eval();

        auto C = c_xx_inv;
        auto D = State();
        D.setZero(mu_x.size());

        const int sensor_count = obsrv_function.count_local_models();
        const int dim_y = y.size() / sensor_count;

        auto h_body = [&](const State& x, const typename BodyModel::Noise& w)
        {
            return feature_model.feature_obsrv(
                body_tail_model.body_model().observation(x, w));
        };
        auto h_tail = [&](const State& x, const typename TailModel::Noise& w)
        {
            return feature_model.feature_obsrv(
                body_tail_model.tail_model().observation(x, w));
        };
        PointSet<LocalFeature, NumberOfPoints> p_Y_body;
        PointSet<LocalFeature, NumberOfPoints> p_Y_tail;

        for (int i = 0; i < sensor_count; ++i)
        {
            // validate sensor value, i.e. make sure it is finite
            if (!is_valid(y, i * dim_y, i * dim_y + dim_y)) continue;

            feature_model.id(i);

            /* ------------------------------------------ */
            /* - Integrate body                         - */
            /* ------------------------------------------ */

            quadrature.propagate_points(h_body, p_X, p_R, p_Y_body);
            auto mu_y_body = p_Y_body.mean();

            // validate sensor value, i.e. make sure it is finite
            if (!is_valid(mu_y_body, 0, dim_y)) continue;

            auto Y_body = p_Y_body.centered_points();
            auto c_yy_body = (Y_body * W * Y_body.transpose()).eval();
            auto c_xy_body = (X * W * Y_body.transpose()).eval();

            /* ------------------------------------------ */
            /* - Integrate tail                         - */
            /* ------------------------------------------ */
            quadrature.propagate_points(h_tail, p_X, p_R, p_Y_tail);
            auto mu_y_tail = p_Y_tail.mean();
            auto Y_tail = p_Y_tail.centered_points();
            auto c_yy_tail = (Y_tail * W * Y_tail.transpose()).eval();
            auto c_xy_tail = (X * W * Y_tail.transpose()).eval();

            /* ------------------------------------------ */
            /* - Fuse and center                        - */
            /* ------------------------------------------ */
            auto w = body_tail_model.tail_weight();
            auto mu_y = ((1.0 - w) * mu_y_body + w * mu_y_tail).eval();

            // non centered moments
            auto m_yy_body =
                (c_yy_body + mu_y_body * mu_y_body.transpose()).eval();
            auto m_yy_tail =
                (c_yy_tail + mu_y_tail * mu_y_tail.transpose()).eval();
            auto m_yy = ((1.0 - w) * m_yy_body + w * m_yy_tail).eval();

            // center
            auto c_yy = (m_yy - mu_y * mu_y.transpose()).eval();
            auto c_xy = ((1.0 - w) * c_xy_body + w * c_xy_tail).eval();

            auto c_yx = c_xy.transpose().eval();
            auto A_i = (c_yx * c_xx_inv).eval();
            auto c_yy_given_x = (c_yy - c_yx * c_xx_inv * c_xy).eval();
            auto innovation = (y.middleRows(i * dim_y, dim_y) - mu_y).eval();

            C += A_i.transpose() * solve(c_yy_given_x, A_i);
            D += A_i.transpose() * solve(c_yy_given_x, innovation);
        }

        /* ------------------------------------------ */
        /* - Update belief according to PAPER REF   - */
        /* ------------------------------------------ */
        posterior_belief.dimension(prior_belief.dimension());
        posterior_belief.covariance(C.inverse());
        posterior_belief.mean(mu_x + posterior_belief.covariance() * D);
    }

    virtual std::string name() const
    {
        return "MultiSensorSigmaPointUpdatePolizzle<" +
               this->list_arguments("SigmaPointQuadrature",
                                    "NonAdditive<SensorFunction>") +
               ">";
    }

    virtual std::string description() const
    {
        return "Multi-Sensor Sigma Point based filter update policy "
               "for joint observation model of multiple local observation "
               "models with non-additive noise.";
    }

private:
    /**
     * \brief Checks whether all vector components within the range (start, end)
     *        are finiate, i.e. not NAN nor Inf.
     */
    template <typename Vector>
    bool is_valid(Vector&& vector, int start, int end) const
    {
        for (int k = start; k < end; ++k)
        {
            if (!std::isfinite(vector(k))) return false;
        }

        return true;
    }
};
}