#ifndef JHMI_UTILITY_OCTTREE_HPP_NRC_20160123
#define JHMI_UTILITY_OCTTREE_HPP_NRC_20160123

#include "utility/cube.hpp"
#include "utility/units.hpp"
#include <boost/container/flat_map.hpp>
#include <boost/container/flat_set.hpp>
#include <boost/optional.hpp>
#include <range/v3/to_container.hpp>
#include <range/v3/view.hpp>
#include <tbb/tbb.h>
#include <unordered_set>
#include <deque>

namespace jhmi {
  struct normal_distance_squared {
    template <typename T, typename U> auto operator()(T&& lhs, U&& rhs) {
      auto ds = distance_squared(std::forward<T>(lhs), std::forward<U>(rhs));
      return boost::optional<decltype(ds)>{ds};
    }
  };
  template <typename T>
  class octtree {
    using m_sq = decltype(1_mm * 1_mm);
    using container_t = boost::container::flat_set<T>;
    //using container_t = std::unordered_set<T>;
    struct node {
      friend class octtree;
      //Required since flat_set has an explicit default constructor?!
      node() : objs{}, sub_nodes{nullptr}, ext{} {}
      container_t objs;
      std::unique_ptr<node[]> sub_nodes;
      cube<m3> ext;
    };

    node node_;
    int preferred_;

    void split_node(node& n) {
      assert(!n.sub_nodes);
      n.sub_nodes.reset(new node[8]);
      auto ul = n.ext.ul(), nc = center(n.ext);
      m3 x{nc.x - ul.x, 0_mm, 0_mm};
      m3 y{0_mm, nc.y - ul.y, 0_mm};
      m3 z{0_mm, 0_mm, nc.z - ul.z};
      n.sub_nodes[0].ext = {ul, nc};
      n.sub_nodes[1].ext = {ul+x, nc+x};
      n.sub_nodes[2].ext = {ul+x+y, nc+x+y};
      n.sub_nodes[3].ext = {ul+y, nc+y};
      n.sub_nodes[4].ext = {ul+z, nc+z};
      n.sub_nodes[5].ext = {ul+z+x, nc+z+x};
      n.sub_nodes[6].ext = {nc, n.ext.lr()};
      n.sub_nodes[7].ext = {ul+z+y, nc+z+y};
    }
    static node* find_containing_sub_node(cube<m3> const& ext, node* sub_nodes) {
      if (!sub_nodes)
        return nullptr;
      for (std::int8_t idx = 0; idx < 8; ++idx) {
        if (contains(sub_nodes[idx].ext, ext))
          return &sub_nodes[idx];
      }
      return nullptr;
    }
    struct option_pt {
      boost::optional<std::pair<T,m_sq>> val;
      void consider(T const& t, m_sq d) {
        if (!val || d < val->second)
          val = std::make_pair(t, d);
      }
      bool valid() const { return bool(val); }
      auto extents(m3 const& pt) const {
        return inflate(cube<m3>{pt,pt}, dbl3{1,1,1} * m{sqrt(val->second)});
      }
      option_pt merge(option_pt rhs) const {
        if (val)
          rhs.consider(val->first, val->second);
        return rhs;
      }
    };
    struct best_vector_pts {
      boost::container::flat_map<m_sq, T> objs_;
      int n_;
      explicit best_vector_pts(int n) : n_(n) {}
      void consider(T const& t, m_sq d) {
        objs_.emplace(d,t);
        if (objs_.size() > n_)
          objs_.erase(objs_.begin() + n_, objs_.end());
      }
      bool valid() const { return !objs_.empty(); }
      auto extents(m3 const& pt) const {
        auto last = --objs_.end();
        auto maxext = last->first;
        return inflate(cube<m3>{pt,pt}, dbl3{1,1,1} * m{sqrt(maxext)});
      }
      best_vector_pts merge(best_vector_pts rhs) const {
        rhs.objs.insert(rhs.objs_.end(), objs_.begin(), objs_.end());
        if (rhs.objs_.size() > n_)
          objs_.erase(objs_.begin() + n_, objs_.end());
        return rhs;
      }
    };
    struct vector_pts {
      std::vector<T> objs;
      m_sq dsq;
      explicit vector_pts(m d) : dsq(d*d) {}
      void consider(T const& t, m_sq d) {
        if (d < dsq)
          objs.push_back(t);
      }
      bool valid() const { return true; }
      auto extents(m3 const& pt) const {
        return inflate(cube<m3>{pt,pt}, dbl3{1,1,1} * m{sqrt(dsq)});
      }
      vector_pts merge(vector_pts rhs) const {
        rhs.objs.insert(rhs.objs.end(), objs.begin(), objs.end());
        return rhs;
      }
    };
    template <typename F, typename ValHolder>
    static void find_at_level(m3 const& pt, node const& n, ValHolder& ov, F dist_sq) {
      for (auto&& obj : n.objs) {
        auto new_dist = dist_sq(obj, pt);
        if (new_dist)
          ov.consider(obj, *new_dist);
      }
      if (n.sub_nodes) {
        //We expect the containing node will have the closest match, do this
        // first so we might prune more ocatants in the future.
        auto c = find_containing_sub_node(cube<m3>{pt,pt}, n.sub_nodes.get());
        if (c)
          find_at_level(pt, *c, ov, dist_sq);
        for (int i = 0; i < 8; ++i)
          if (&n.sub_nodes[i] != c)
            if (!ov.valid() || overlaps(n.sub_nodes[i].ext, ov.extents(pt)))
              find_at_level(pt, n.sub_nodes[i], ov, dist_sq);
      }
    }
  public:
    explicit octtree(cube<m3> const& ext, int preferred = 16)
        : node_{}, preferred_{preferred} {
      node_.ext = ext;
    }

    void remove_item(T const& item) {
      auto ext = extents(item);
      auto node = &node_;
      for (auto c = &node_; c; c = find_containing_sub_node(ext, c->sub_nodes.get()))
        node = c;
      assert(node);
      auto it = node->objs.find(item);
      if (it == node->objs.end())
        throw std::runtime_error("removing non-existent octtree item");
      node->objs.erase(it);
    }

    void add_item(T const& item) {
      auto ext = extents(item);
      auto best = &node_;
      if (!contains(best->ext, ext))
        return;
//        throw std::runtime_error("Adding item outside octree bounds");
      while (best->sub_nodes) {
        auto new_best = find_containing_sub_node(ext, best->sub_nodes.get());
        if (new_best)
          best = new_best;
        else
          break;
      }
      best->objs.insert(item);
      if (!best->sub_nodes && best->objs.size() > preferred_) {
        //Split objs into the different octants, unless they overlap.
        split_node(*best);
        container_t ncont;
        for (auto&& repr : best->objs) {
          auto sub_node = find_containing_sub_node(extents(repr), best->sub_nodes.get());
          if (!sub_node)
            ncont.insert(repr);
          else
            sub_node->objs.insert(repr);
        }
        best->objs = std::move(ncont);
      }
    }

    template <typename F = normal_distance_squared>
    std::vector<T> find_nearest_items(m3 const& pt, m dist, F dist_sq = F{}) const {
      auto best = vector_pts{dist};
      find_at_level(pt, node_, best, dist_sq);
      return best.objs;
    }
    template <typename F = normal_distance_squared>
    std::vector<T> find_n_nearest_items(m3 const& pt, int n, F dist_sq = F{}) const {
      auto best = best_vector_pts{n};
      find_at_level(pt, node_, best, dist_sq);
      return best.objs_ | ranges::view::values | ranges::to_vector;
    }

    template <typename F = normal_distance_squared>
    boost::optional<T> find_nearest_item(m3 const& pt, F dist_sq = F{}) const {
      auto best = option_pt{};
      find_at_level(pt, node_, best, dist_sq);
      if (best.valid())
        return best.val->first;
      return boost::none;
    }

    auto get_all() const {
      boost::container::flat_set<decltype(id(std::declval<T>()))> ret;
      std::deque<node const*> nodes{&node_};
      while (!nodes.empty()) {
        auto curr = nodes.front();
        nodes.pop_front();
        for (auto&& repr : curr->objs) {
          ret.insert(id(repr));
        }
        if (curr->sub_nodes) {
          for (int i = 0; i < 8; ++i)
            nodes.insert(nodes.end(), &curr->sub_nodes[i]);
        }
      }
      return ret;
    }
//TODO: Debug only -- non-transitive const problems
    node const& root() const {
      return node_;
    }
  };

  template <typename T>
  cube<m3> extents(octtree<T> const& o) { return o.root().ext; }
}
#endif


#include "../schnorr_proof.h"
#include "../schnorr_prover.h"
#include "../schnorr_verifier.h"
#include "../../streams.h"

#include <boost/test/unit_test.hpp>

namespace lelantus {

class SchnorrProofTests {
public:
    SchnorrProofTests()
    {
        g.randomize();
        h.randomize();
        P.randomize();
        T.randomize();
    }

public:
    GroupElement g, h;
    Scalar P, T;
};

BOOST_FIXTURE_TEST_SUITE(lelantus_schnorr_proof_tests, SchnorrProofTests)

BOOST_AUTO_TEST_CASE(serialization)
{
    SchnorrProver prover(g, h);
    SchnorrProof proof;
    prover.proof(P, T, proof);

    CDataStream serialized(SER_NETWORK, PROTOCOL_VERSION);
    serialized << proof;

    SchnorrProof deserialized;
    serialized >> deserialized;

    BOOST_CHECK(proof.u == deserialized.u);
    BOOST_CHECK(proof.P1 == deserialized.P1);
    BOOST_CHECK(proof.T1 == deserialized.T1);
}

BOOST_AUTO_TEST_CASE(prove_verify)
{
    auto y = LelantusPrimitives::commit(g, P, h, T);

    SchnorrProver prover(g, h);
    SchnorrProof proof;
    prover.proof(P, T, proof);

    SchnorrVerifier verifier(g, h);
    BOOST_CHECK(verifier.verify(y ,proof));
}

BOOST_AUTO_TEST_CASE(fake_prove_not_verify)
{
    auto y = LelantusPrimitives::commit(g, P, h, T);

    SchnorrProver prover(g, h);
    SchnorrProof proof;
    prover.proof(P, T, proof);

    GroupElement fakeY;
    fakeY.randomize();

    SchnorrVerifier verifier(g, h);
    BOOST_CHECK(!verifier.verify(fakeY, proof));

    auto fakeProof = proof;
    fakeProof.P1.randomize();
    BOOST_CHECK(!verifier.verify(y, fakeProof));

    fakeProof = proof;
    fakeProof.T1.randomize();
    BOOST_CHECK(!verifier.verify(y, fakeProof));

    fakeProof = proof;
    fakeProof.u.randomize();
    BOOST_CHECK(!verifier.verify(y, fakeProof));
}

BOOST_AUTO_TEST_SUITE_END()

} // namespace lelantus


#ifndef BOOST_GEOMETRY_PROJECTIONS_CEA_HPP
#define BOOST_GEOMETRY_PROJECTIONS_CEA_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2012 Barend Gehrels, 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)

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

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

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

#include <boost/geometry/extensions/gis/projections/impl/base_static.hpp>
#include <boost/geometry/extensions/gis/projections/impl/base_dynamic.hpp>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>
#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>
#include <boost/geometry/extensions/gis/projections/impl/pj_qsfn.hpp>
#include <boost/geometry/extensions/gis/projections/impl/pj_auth.hpp>

namespace boost { namespace geometry { namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace cea{
            static const double EPS = 1e-10;

            struct par_cea
            {
                double qp;
                double apa[APA_SIZE];
            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_cea_ellipsoid : public base_t_fi<base_cea_ellipsoid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_cea m_proj_parm;

                inline base_cea_ellipsoid(const Parameters& par)
                    : base_t_fi<base_cea_ellipsoid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    xy_x = this->m_par.k0 * lp_lon;
                    xy_y = .5 * pj_qsfn(sin(lp_lat), this->m_par.e, this->m_par.one_es) / this->m_par.k0;
                }

                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    lp_lat = pj_authlat(asin( 2. * xy_y * this->m_par.k0 / this->m_proj_parm.qp), this->m_proj_parm.apa);
                    lp_lon = xy_x / this->m_par.k0;
                }
            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_cea_spheroid : public base_t_fi<base_cea_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_cea m_proj_parm;

                inline base_cea_spheroid(const Parameters& par)
                    : base_t_fi<base_cea_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    xy_x = this->m_par.k0 * lp_lon;
                    xy_y = sin(lp_lat) / this->m_par.k0;
                }

                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    double t;

                    if ((t = fabs(xy_y *= this->m_par.k0)) - EPS <= 1.) {
                        if (t >= 1.)
                            lp_lat = xy_y < 0. ? -HALFPI : HALFPI;
                        else
                            lp_lat = asin(xy_y);
                        lp_lon = xy_x / this->m_par.k0;
                    } else throw proj_exception();;
                }
            };

            // Equal Area Cylindrical
            template <typename Parameters>
            void setup_cea(Parameters& par, par_cea& proj_parm)
            {
                double t = 0;
                if (pj_param(par.params, "tlat_ts").i &&
                    (par.k0 = cos(t = pj_param(par.params, "rlat_ts").f)) < 0.)
                  throw proj_exception(-24);
                if (par.es) {
                    t = sin(t);
                    par.k0 /= sqrt(1. - par.es * t * t);
                    par.e = sqrt(par.es);
                    pj_authset(par.es, proj_parm.apa);
                    proj_parm.qp = pj_qsfn(1., par.e, par.one_es);
                // par.inv = e_inverse;
                // par.fwd = e_forward;
                } else {
                // par.inv = s_inverse;
                // par.fwd = s_forward;
                }
            }

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

    /*!
        \brief Equal Area Cylindrical projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
         - Ellipsoid
         - lat_ts=
        \par Example
        \image html ex_cea.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct cea_ellipsoid : public detail::cea::base_cea_ellipsoid<Geographic, Cartesian, Parameters>
    {
        inline cea_ellipsoid(const Parameters& par) : detail::cea::base_cea_ellipsoid<Geographic, Cartesian, Parameters>(par)
        {
            detail::cea::setup_cea(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Equal Area Cylindrical projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
         - Ellipsoid
         - lat_ts=
        \par Example
        \image html ex_cea.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct cea_spheroid : public detail::cea::base_cea_spheroid<Geographic, Cartesian, Parameters>
    {
        inline cea_spheroid(const Parameters& par) : detail::cea::base_cea_spheroid<Geographic, Cartesian, Parameters>(par)
        {
            detail::cea::setup_cea(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class cea_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    if (par.es)
                        return new base_v_fi<cea_ellipsoid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                    else
                        return new base_v_fi<cea_spheroid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void cea_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("cea", new cea_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_CEA_HPP


/* ----------------------------------------------------------------------- *//**
 *
 * @file MultiResponseGLM_impl.hpp
 *
 *//* ----------------------------------------------------------------------- */

#ifndef MADLIB_MODULES_GLM_MULTIRESPONSEGLM_IMPL_HPP
#define MADLIB_MODULES_GLM_MULTIRESPONSEGLM_IMPL_HPP

#include <dbconnector/dbconnector.hpp>
#include <boost/math/distributions.hpp>
#include <limits>

namespace madlib {

namespace modules {

namespace glm {

// Use Eigen
using namespace dbal::eigen_integration;

template <class Container, class Family, class Link>
inline
MultiResponseGLMAccumulator<Container,Family,Link>::MultiResponseGLMAccumulator(
        Init_type& inInitialization)
  : Base(inInitialization), optimizer(inInitialization) {

    this->initialize();
}

/**
 * @brief Reset the accumulator before accumulating the first tuple
 */
template <class Container, class Family, class Link>
inline
void
MultiResponseGLMAccumulator<Container,Family,Link>::reset() {
    num_rows = 0;
    terminated = false;
    loglik = 0.;
    optimizer.reset();
}

/**
 * @brief Bind all elements of the state to the data in the stream
 *
 * The bind() is special in that even after running operator>>() on an element,
 * there is no guarantee yet that the element can indeed be accessed. It is
 * cruicial to first check this.
 *
 * Provided that this methods correctly lists all member variables, all other
 * methods can, however, rely on that fact that all variables are correctly
 * initialized and accessible.
 */
template <class Container, class Family, class Link>
inline
void
MultiResponseGLMAccumulator<Container,Family,Link>::bind(
        ByteStream_type& inStream) {

    inStream
        >> num_features
        >> num_categories
        >> num_rows
        >> terminated
        >> loglik
        >> optimizer;

    // bind vcov and optimizer.hessian onto the same memory as we don't need them
    // at the same time
    vcov.rebind(optimizer.hessian.memoryHandle(),
                optimizer.hessian.rows(),
                optimizer.hessian.cols());
}

/**
 * @brief Update the accumulation state by feeding a tuple
 */
template <class Container, class Family, class Link>
inline
MultiResponseGLMAccumulator<Container,Family,Link>&
MultiResponseGLMAccumulator<Container,Family,Link>::operator<<(
        const tuple_type& inTuple) {

    const MappedColumnVector& x = std::get<0>(inTuple);
    const int& y = static_cast<int>(std::get<1>(inTuple));
    // convert y to an indicator vector
    ColumnVector vecY(num_categories-1);
    vecY.fill(0);
    if (y != 0) { vecY(y - 1) = 1; }

    // The following checks were introduced with MADLIB-138. It still seems
    // useful to have clear error messages in case of infinite input values.
    if (!dbal::eigen_integration::isfinite(x)) {
        warning("Design matrix is not finite.");
    } else if (x.size() > std::numeric_limits<uint16_t>::max()) {
        warning("Number of independent variables cannot be "
            "larger than 65535.");
    } else if (num_features != static_cast<uint16_t>(x.size())) {
        warning("Inconsistent numbers of independent variables.");
    } else {
        // normal case
        uint16_t N = num_features;
        uint16_t C = static_cast<uint16_t>(num_categories - 1);
        ColumnVector mu(C);
        ColumnVector ita(C);
        Matrix G_prime(C, C);
        Matrix V(C, C);

        // initialize ita and mu in the first iteration
        if (optimizer.beta.norm() == 0.) {
            Link::init(mu);
            Link::link_func(mu,ita);
        } else {
            for (int i = 0; i < ita.size(); i ++) {
                ita(i) = trans(x) * optimizer.beta.segment(i*N, N);
            }
            Link::mean_func(ita, mu);
        }

        // intermediate values
        Link::mean_derivative(ita, G_prime);
        Family::variance(mu, V);

        // log-likelihood
        loglik += Family::loglik(vecY, mu, 1.);

        // calcualte X_trans_W_X
        Matrix GtVinvG(trans(G_prime) * V.inverse() * G_prime);
        Matrix XXt(x * trans(x));
        for (int i = 0; i < C; i ++) {
            for (int j = 0; j < C; j ++) {
                for (int x_i = 0; x_i < num_features; x_i ++) {
                   for (int x_j = 0; x_j < num_features; x_j ++) {
                        optimizer.hessian(i*num_features + x_i,
                                          j*num_features + x_j)
                                += GtVinvG(i,j) * XXt(x_i,x_j);
                   }
                }
            }
        }

        // calculate X_trans_W_Y
        ColumnVector YVinvG(trans(vecY-mu) * V.inverse() * G_prime);
        for (int i = 0; i < C; i ++) {
            optimizer.grad.segment(i*N, N) -= YVinvG(i) * x;
        }
        num_rows ++;
        return *this;
    } // all tests passed and we were in the else

    // error case
    terminated = true;
    return *this;
}

/**
 * @brief Merge with another accumulation state
 */
template <class Container, class Family, class Link>
template <class C, class F, class L>
inline
MultiResponseGLMAccumulator<Container,Family,Link>&
MultiResponseGLMAccumulator<Container,Family,Link>::operator<<(
        const MultiResponseGLMAccumulator<C,F,L>& inOther) {
    if (this->empty()) {
        *this = inOther;
    } else if (inOther.empty()) {
    } else if (num_features != inOther.num_features) {
        warning("Inconsistent numbers of independent variables.");
        terminated = true;
    } else {
        num_rows += inOther.num_rows;
        loglik += inOther.loglik;
        optimizer.grad += inOther.optimizer.grad;
        optimizer.hessian += inOther.optimizer.hessian;
    }

    return *this;
}

/**
 * @brief Copy from a previous state
 */
template <class Container, class Family, class Link>
template <class C, class F, class L>
inline
MultiResponseGLMAccumulator<Container,Family,Link>&
MultiResponseGLMAccumulator<Container,Family,Link>::operator=(
        const MultiResponseGLMAccumulator<C,F,L>& inOther) {
    this->copy(inOther);
    return *this;
}

/**
 * @brief Apply the accumulated intra-state values to inter-state member
 */
template <class Container, class Family, class Link>
inline
void
MultiResponseGLMAccumulator<Container,Family,Link>::apply() {
    if (!dbal::eigen_integration::isfinite(optimizer.hessian) ||
            !dbal::eigen_integration::isfinite(optimizer.grad)) {
        warning("Hessian or gradient is not finite.");
        terminated = true;
    } else {
        optimizer.apply();
    }

}
// -----------------------------------------------------------------------

template <class Container>
MultiResponseGLMResult::MultiResponseGLMResult(const MultiResponseGLMAccumulator<Container>& state) {
    compute(state);
}

/**
 * @brief Transform an accumulation state into a result
 */
template <class Container>
inline
MultiResponseGLMResult&
MultiResponseGLMResult::compute(const MultiResponseGLMAccumulator<Container>& state) {
    typedef Eigen::Map<Matrix> MMap;
    typedef MMap::Scalar Scalar;
    // Vector of coefficients: For efficiency reasons, we want to return this
    // by reference, so we need to bind to db memory
    Allocator& allocator = defaultAllocator();
    Index N = static_cast<Index>(state.num_features);
    Index C = static_cast<Index>(state.num_categories - 1);
    coef.rebind(allocator.allocateArray<double>(C, N), N, C);
    std_err.rebind(allocator.allocateArray<double>(C, N), N, C);
    z_stats.rebind(allocator.allocateArray<double>(C, N), N, C);
    p_values.rebind(allocator.allocateArray<double>(C, N), N, C);

    // computation
    loglik = state.loglik;
    coef = MMap(const_cast<Scalar*>(state.optimizer.beta.data()), N, C);
    Matrix tmp_sd(state.vcov.diagonal().cwiseSqrt());
    tmp_sd.resize(N, C);
    std_err = tmp_sd;
    z_stats = coef.cwiseQuotient(std_err);
    for (Index i = 0; i < N*C; i ++) {
        p_values(i) = 2. * prob::cdf( prob::normal(), -std::abs(z_stats(i)));
    }
    num_rows_processed = state.num_rows;

    return *this;
}

} // namespace glm

} // namespace modules

} // namespace madlib

#endif // defined(MADLIB_MODULES_GLM_MULTIRESPONSEGLM_IMPL_HPP)


//
// Created by eliane on 05/03/19.
//

#include "Evolution.h"

#include <ensmallen.hpp>

#undef BOOST_LOG

#include <boost/log/trivial.hpp>
#include <Engines/GlobalHeuristics/Utilities/MetropolisHastings.h>

namespace SmolDock::Heuristics {

    Evolution::Evolution(Score::ScoringFunction* scorFunc_, Optimizer::Optimizer* optimizer_,
                                                 unsigned int seed_, Evolution::Parameters params_) :
            scorFunc(scorFunc_), optimizer(optimizer_), rndGenerator(seed_), params(params_) {

    }

    bool Evolution::search() {

        arma::mat currentState = scorFunc->getStartingConditions();

        unsigned int populationSize = 500;
        unsigned int maxGenerations = 5000;
        double mutationProb = 0.1;
        double mutationSize = 0.5;
        double selectPercent = 0.2;
        double tolerance = -1;

        ens::CNE CNEvolutionHeur(populationSize, maxGenerations, mutationProb, mutationSize, selectPercent, tolerance);

        CNEvolutionHeur.Optimize(*this->scorFunc, currentState);

        double scoreBeforeLocalOptim = this->scorFunc->Evaluate(currentState);

        this->optimizer->optimize(currentState);
        double scoreAfterLocalOptim = this->optimizer->getScore();

        this->result = this->optimizer->getRawResultMatrix();

        BOOST_LOG_TRIVIAL(debug) << "Evolution: Score after Evolution   = " << scoreBeforeLocalOptim;
        BOOST_LOG_TRIVIAL(debug) << "Evolution: Score after local optim = " << scoreAfterLocalOptim;

        return true;
    }

    arma::mat Evolution::getResultMatrix() {
        return this->result;
    }

}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IF_MINUS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IF_MINUS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-boolean
    This function object conditionally returns @c x-y (respectively @c x)  if @c c is @ref True
    (respectively  @ref False)

    @par Header <boost/simd/function/if_minus.hpp>

    @par Example:

      @snippet if_minus.cpp if_minus

    @par Possible output:

      @snippet if_minus.txt if_minus

  **/
  Value1 if_minus(Value0 const& c, Value1 const& x, Value1 const& y);
} }
#endif

#include <boost/simd/function/scalar/if_minus.hpp>
#include <boost/simd/function/simd/if_minus.hpp>

#endif


#include <vector>
#include <iostream>
#include <memory>
#include<string>
#include <queue>
#include <variant>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <boost/circular_buffer.hpp>

template <typename T> struct Queue{
  explicit Queue(size_t size):_q(size){
  }
  using QT = boost::circular_buffer<T>;
  void push( T t){
    std::unique_lock lc(_m);
    while(_q.full()){ _notFull.wait(lc); }
    _q.push_back(std::move(t));
    _notEmpty.notify_one();
  }

  T pop(){
    std::unique_lock lc(_m);
    while(_q.empty()){ _notEmpty.wait(lc); };
    T t {std::move(_q.front())};
    _q.pop_front();
    _notFull.notify_one();
    return t;
  }
private:
  QT            _q;
  mutable std::mutex    _m;
  std::condition_variable   _notFull;
  std::condition_variable   _notEmpty;
};

//// test
using DT = std::unique_ptr<int>;
Queue<DT> q(10);

void pushAll(){
  for(size_t i = 0; i < 30; ++i ){
    std::this_thread::sleep_for(std::chrono::milliseconds (10));
    q.push(std::make_unique<int>(i));
  }
}

void popAll(){
  for(size_t i = 0; i < 30; ++i ){
    DT d = q.pop();
    std::cout << *d << std::endl;
  }
}

int main(int argc, char** argv) {
  std::thread t1(pushAll); std::thread t2(popAll);
  t1.join(); t2.join();

  return 0;
}


#include <cstdio>
#include <vector>

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <maplab-common/test/testing-entrypoint.h>
#include <vocabulary-tree/distance.h>
#include <vocabulary-tree/simple-kmeans.h>
#include <vocabulary-tree/tree-builder.h>
#include <vocabulary-tree/types.h>

#include "./floating-point-test-helpers.h"

// This tests checks the accelerated search on the kd-tree accelerated
// vocabulary.
TEST(VocabularyTree, VocabularyTree_AcceleratedQuantization) {
  std::mt19937 generator(40);
  static const size_t kNumfeaturesPerCluster = 10;
  static const size_t kNumClusters = 1000;
  static const size_t kNumClustersTree = 10;
  static const size_t kNumLevels = 5;
  DescriptorVector gt_centers;
  DescriptorVector descriptors;
  std::vector<unsigned int> membership;
  std::vector<unsigned int> gt_membership;

  GenerateTestData(
      kNumfeaturesPerCluster, kNumClusters, generator(), &gt_centers,
      &descriptors, &membership, &gt_membership);

  DescriptorType descriptor_zero;
  descriptor_zero.setConstant(
      kDescriptorDimensionality, 1, static_cast<Scalar>(0));

  typedef loop_closure::distance::L2<DescriptorType> DistanceType;
  typedef loop_closure::TreeBuilder<DescriptorType, DistanceType> TreeBuilder;

  TreeBuilder builder(descriptor_zero);
  builder.kmeans().SetRestarts(2);
  builder.Build(descriptors, kNumClustersTree, kNumLevels);

  const TreeBuilder::Tree& tree = builder.tree();
  const DescriptorVector& centers = tree.centers();
  const std::vector<uint8_t>& valid_centers = tree.validCenters();
  const int splits = tree.splits();

  loop_closure::distance::L2<DescriptorType> l2_distance;

  for (const DescriptorType& feature : descriptors) {
    int32_t index = -1;  // Virtual root index, which has no associated center.
    for (unsigned level = 0; level < kNumLevels; ++level) {
      // Calculate the offset to the first child of the current index.
      int32_t first_child = (index + 1) * splits;
      // Find the child center closest to the query.
      int32_t best_child = first_child;
      typename DistanceType::result_type best_distance =
          std::numeric_limits<typename DistanceType::result_type>::max();
      for (int32_t child = first_child; child < first_child + splits; ++child) {
        if (!valid_centers[child])
          break;  // Fewer than splits() children.
        typename DistanceType::result_type child_distance =
            l2_distance(feature, centers[child]);
        if (child_distance < best_distance) {
          best_child = child;
          best_distance = child_distance;
        }
      }
      index = best_child;
    }
    loop_closure::Word word_gt = index - tree.wordstart();
    loop_closure::Word word = tree.Quantize(feature);
    EXPECT_EQ(word_gt, word);
  }
}

MAPLAB_UNITTEST_ENTRYPOINT


#include "fssh.hpp"
#include "electronic.hpp"
#include "constants.hpp"
#include "decoherence_afssh.hpp"
#include "util.hpp"
#include <armadillo>
#include <complex>
#include <cmath>
#include <iostream>

ConfigBlockReader FSSH::setup_reader()
{
    using types = ConfigBlockReader::types;
    ConfigBlockReader reader{"fssh"};
    reader.add_entry("dtc", types::DOUBLE);
    reader.add_entry("delta_e_tol", 1e-4);

    reader.add_entry("amplitude_file", "cs.dat");
    reader.add_entry("decoherence", "");
    // FIXME: make ConfigBlockReader complain if adding the same key twice!

    std::random_device rd; // generate default random seed
    reader.add_entry("random_seed", (size_t) rd());
    {
      std::vector<std::complex<double>> cs {};
      reader.add_entry("amplitudes", cs);
    }
    return reader;
}

void FSSH::get_reader_data(ConfigBlockReader& reader) {
  {
    double in_dtc;
    reader.get_data("dtc", in_dtc);
    // FIXME?: Should input time be in fs as it is (because our
    // interface is general) or in ns to match GMX?
    dtc = in_dtc * (1e-15 / AU2SI_TIME); // fs -> a.u.
  }

  reader.get_data("delta_e_tol", delta_e_tol);
  reader.get_data("amplitude_file", amplitude_file);

  /* added in BOMD::add_qm_keys() */
  reader.get_data("min_state", min_state);

  /*
     Must set rng seed before decoherence because Decoherence will use
     armadillo random number generator
  */
  {
    size_t seed = 0; // a default seed is set in setup_reader(); this value will not be propagated.

    // FIXME: ConfigReader won't throw an error if the wrong type is passed in
    reader.get_data("random_seed", seed);
    arma::arma_rng::set_seed(seed);

    std::cerr << "[FSSH] random_seed = " << seed << std::endl;
  }

  {
    size_t excited_states;
    /* added in BOMD::add_qm_keys() */
    reader.get_data("excited_states", excited_states);

    shstates = excited_states + 1 - min_state;
  }

  if (shstates < 2){
    throw std::logic_error("Cannot run FSSH on a single surface!");
  }

  if (!(min_state <= active_state && active_state <= min_state + shstates)){
    throw std::range_error("Active state not in the range of hopping states!");
  }

  {
    std::string decoherence_in;
    reader.get_data("decoherence", decoherence_in);

    if (decoherence_in == "") {
      // do nothing; already nullptr
    } else if (decoherence_in == "afssh" ||
               decoherence_in == "jain2016" ) {
      decoherence = new AFSSH(&qm, dtc, min_state, shstates, NQM(), NMM());
    }
    else{
      throw std::runtime_error("Decoherence class, '" + decoherence_in + "', not recognized!");
    }
  }

  active_state -= min_state;

  energy.set_size(shstates);

  U.set_size(shstates, shstates);
  T.set_size(shstates, shstates);
  V.set_size(shstates, shstates);

  /*
    FIXME: should be able to configure (multiple) initial electronic
    state(s). Can use DVEC?
  */

  {
    std::vector<std::complex<double>> cs_vec {};
    reader.get_data("amplitudes", cs_vec);
    if (cs_vec.size() > 0){
      if (cs_vec.size() != (arma::uword) shstates){
        throw std::runtime_error("Number of Amplitudes do not match number of hopping surfaces!");
      }
      double norm = 0;
      for (const auto& c: cs_vec){
        norm += std::norm(c);
      }
      std::cerr << "[FSSH] amplitude norm = " << norm << std::endl;
      if (std::abs(1.0 -norm) > 1e-6){
        throw std::runtime_error("Amplitudes are not normed; check your input!");
      }
      arma::cx_vec cs_arma(shstates);
      for (arma::uword i = 0; i < (arma::uword) shstates; i++){
        cs_arma(i) = cs_vec[i];
      }
      c = Electronic(cs_arma);
    }
    else{
      c.reset(shstates, 1, active_state);
    }
  }
}

// For use in the update_gradient() call; Jain step 4
void FSSH::electronic_evolution(void){
  if (hopping){
    throw std::logic_error("Should never be hopping at start of electronic evolution!");
  }

  PropMap props{};
  props.emplace(QMProperty::wfoverlap, &U);
  qm->get_properties(props);

  Electronic::phase_match(U);
  T = arma::real(arma::logmat(U)) / dtc;

  // R.B.: energy was updated in update_gradient()
  V = diagmat(energy);

  /*
    Since the off-diagonal elements of V are always 0 in this
    implementation, we work only with the diagonal of V. Equation 20
    seems to have been developed for a system with positive energy
    eigenvalues. A constant shift of energy scale to the negative (Vii
    = Vii - max(V.diag) can cause Eq. 20 to fail.

    FIXME: construct an alternate to Eq. 20 that is applicable to Vii < 0
  */

  // compute max time step for electronic propagation (eqs. 20, 21)
  {
    double dtq_ = std::min(dtc,
			   std::min(0.02 / T.max(),
				    0.02 / arma::max( V.diag() - arma::mean(V.diag()) )
				    )
			   );

    dtq = dtc / std::round(dtc / dtq_);
  }
  const size_t n_steps = (size_t) dtc / dtq;
  const std::complex<double> I(0,1);

  // Propagate electronic coefficients and compute hopping probabilities for dtc
  for (size_t nt = 0; nt < n_steps; nt++){
    /*
      Propagate all states simultaneously. Recall that
      Electronic::advance(H, dt) uses rk4 to propagate the internally
      held coefficients, c, according the Hamiltonian H for time dt
    */

    // FIXME: should use rk4
    //c.advance_rk4(V - I*T, dtq);
    c.advance_exact(V - I*T, dtq);

    // Check for a hop unless we've already had one
    if (! hopping){
      const arma::uword a = active_state;

      /*
	Compute transition probabilities. Jain (2016) eq. 12 has the
        conjugate on the wrong element; cf. Tully (1990) -- discussion
        with Zeyu Zhou
      */
      arma::vec g = -2 * arma::real(c(a) * arma::conj(c()) % T.col(a)) * dtq / std::norm(c(a));
      // set negative elements to 0
      g.elem( arma::find(g < 0) ).zeros();

      /*
	Ensure that g is normed by adding any residual density to the
	active state. This maintains the correct transition
	probability to all states.
      */
      g(a) += 1.0 - arma::sum(g);

      // randomly select an element from the discrete distribution represented by g
      arma::uword j = util::sample_discrete(g);
      if (a != j){
      	// will update these in the velocity_rescale call
      	hopping = true;
      	target_state = j;
      }
    }  // end hopping check
  }  // end loop over dtq
}

// For use within the velocity_rescale() call; Jain steps 5 & 6
// returns energy gap if the hop succeeds without frustration
double FSSH::hop_and_scale(arma::mat &total_gradient, arma::mat &velocities, const arma::vec &m){
  if (! hopping){
    throw std::logic_error("Should not be attempting a hop right now!");
  }

  bool hop_succeeds = false;

  /*
    Recall that in our implementation, the kinetic energy reservoir to
    balance a hop includes the MM atoms, a region of arbitrary
    size. However, the kinetic energy is only available in proportion
    to the NAC vector, which is properly computed over the MM region
    (Thank you, Ou Qi!)  and, we believe, decays with distance. See
    the calculation of vd below.
  */

  nac.set_size(3, NQM() + NMM());

  arma::mat qmg_new, mmg_new;
  qmg_new.set_size(3,NQM());

  {
    PropMap props{};
    props.emplace(QMProperty::nacvector, {min_state + active_state, min_state + target_state}, &nac);
    props.emplace(QMProperty::qmgradient, {min_state + target_state}, &qmg_new);

    if (NMM() > 0){
      mmg_new.set_size(3, NMM());
      props.emplace(QMProperty::mmgradient, {min_state + target_state}, &mmg_new);
    }

    qm->get_properties(props);
  }

  // Make 3N vector versions of the NAC, velocity, and new
  // gradient. (m comes in as 3N.)
  // FIXME: How do each of these interact with link atoms?
  const arma::vec & nacv = nac.as_col();

  // FIXME: find a safer way to have a writeable velocity view
  arma::vec vel(velocities.memptr(), 3 * (NQM() + NMM()), false, true);

  arma::vec gradv(3 * (NQM() + NMM()));
  gradv.head(3*NQM()) = arma::vectorise(qmg_new);
  if (NMM() > 0){
    gradv.tail(3*NMM()) = arma::vectorise(mmg_new);
  }

  double deltaE = energy(target_state) - energy(active_state);

  /*
    The hopping energy-conservation equations are documented in
    Vale's GQSH notes dated May 14, 2021.
  */

  double vd  = arma::as_scalar(vel.t() * nacv);
  double dmd = arma::as_scalar(nacv.t() * (nacv / m));

  double discriminant = (vd/dmd)*(vd/dmd) - 2*deltaE/dmd;
  if (discriminant > 0){
    hop_succeeds = true;
    std::cerr << "[FSSH] Hop, " << active_state + min_state << "->"
              << target_state + min_state << " succeeds; energy difference = "
              << deltaE << std::endl;

    // test the sign of dmv to pick the root yielding the smallest value of alpha
    double alpha = (vd > 0 ? 1.0 : -1.0) * std::sqrt(discriminant) - (vd/dmd);
    vel = vel + alpha * (nacv / m); // recall the nacv has dimension of momentum
    active_state = target_state;

    // Update the gradient with the new surface so that GMX can take its second step
    auto rows=arma::span(arma::span::all); auto cols = arma::span(0, NQM() - 1);
    total_gradient(rows, cols) += qmg_new - qm_grd;
    qm_grd = qmg_new;

    if (NMM() > 0){
      cols = arma::span(NQM(), NQM() + NMM() - 1);
      total_gradient(rows, cols) += mmg_new - mm_grd;
      mm_grd = mmg_new;
    }
  }
  else{
    hop_succeeds = false;
    std::cerr << "[FSSH] Hop is frustrated---will remain on " << active_state + min_state << "; ";
    /*
      Momentum reversal along nac as per Jasper, A. W.; Truhlar,
      D. G. Chem. Phys. Lett. 2003, 369, 60--67 c.f. eqns. 1 & 2

      In Jain (2016) a second criterion was imposed. But, in January
      2020 A. Jain indicated to JES that this was not necessary. We
      follow the original Jasper-Truhlar prescription in line with
      Jain's updated advice.
    */
    if (arma::as_scalar((-gradv.t() * nacv)*(nacv.t() * (vel % m))) < 0){
      std::cerr << "velocities reversed." << std::endl;
      const arma::vec nacu = arma::normalise(nacv);
      //vel = vel - 2.0 * (nacu * nacu.t() * (vel % m))/m;
      vel = vel - 2.0 * (nacu / m) * (nacu.t() * (vel % m));
    }
    else{
      std::cerr << "velocities, unchanged." << std::endl;
      // Ignore the unsuccessful hop; active_state remains unchanged
    }
  }

  hopping = false;  // update class-level state
  return hop_succeeds ? deltaE : 0;
}

/*
  Call this function when energy fluctuation tolerance in the MD
  driver is exceeded and we've had a hop. Update the current surface
  and to the total gradient, add (new-old). Similarly, take a step in
  velocity space backwards along the old gradient and then forwards
  along the new one.

  N.B.: When working with Gromacs, it is not necessary to do any
  back-propagation in positions, only velocities. Gromacs's
  velocity-Verlet integrator splits velocity integration over 2 steps
  (so that all of the tooling for leap-frog still works) and then does
  position integration in a single step afterwards. See
  gifs/docs/GromacsVVImplementation.jpg for notes.

  Conversation with Amber in Marhc 2021 indicated that it was faster
  and didn't rely on an ad hoc selection of energy tolerance to simply
  converge properties in dtc directly rather than use this scheme.

  void FSSH::backpropagate_gradient_velocities(
    arma::mat &total_gradient,
    arma::mat &velocities,
    arma::vec &masses);
*/

// This is our primary hook into the Gromacs (or other) MD loop
double FSSH::update_gradient(void){
  qm->update();

  // get gradients and energies
  {
    PropMap props{};
    props.emplace(QMProperty::qmgradient, {min_state + active_state}, &qm_grd);
    props.emplace(QMProperty::mmgradient, {min_state + active_state}, &mm_grd);
    props.emplace(QMProperty::energies, util::range(min_state, min_state + shstates), &energy);
    qm->get_properties(props);
  }

  // write amplitudes
  {
    std::ofstream output(amplitude_file, std::ios_base::app);
    output << active_state + min_state << " ";
    c().st().print(output);
    output.close();
  }

  electronic_evolution();

  return energy(active_state);
}

/*
  rescale_velocities() comes after the first MD half-step (and before
  constraint forces are calculated in gromacs)
*/

// FIXME: should alter velocity rescale interface so we take inverse masses as 3N vector
bool FSSH::rescale_velocities(arma::mat &velocities, arma::vec &masses, arma::mat &total_gradient, double total_energy){
  // call parent to update edrift
  BOMD::rescale_velocities(velocities, masses, total_gradient, total_energy);

  // FIXME: don't fire on the first inf
  if (std::abs(edrift) > delta_e_tol){
    std::cerr << "WARNING, energy drift exceeds tolerance!" << std::endl;
  }

  if (masses.has_inf() || arma::any(masses==0)){
    throw std::logic_error("Cannot do surface hopping with massless atoms or momentum sinks!");
  }

  // 3N vector of masses
  arma::vec m (3 * (NQM() + NMM()), arma::fill::zeros);
  for(arma::uword i = 0 ; i < m.n_elem ; i++){
    // no reason to do this without a bounds [] check!
    m(i) =  masses(i/3);
  }

  bool update = hopping; // copy so hopping can be reset
  double deltaE = 0;
  if (hopping){
    std::cerr <<  "[FSSH] Attempting hop: " << active_state + min_state
              << "->" << target_state + min_state << std::endl;

    deltaE = hop_and_scale(total_gradient, velocities, m);

    if (decoherence && deltaE > 0){
      decoherence->hopped(c, active_state);
    }
  }

  if (decoherence){
    decoherence->decohere(c, U, active_state, velocities.as_col(), m);
  }

  // update indicates we need to copy velocity and gradient back to GMX
  return update;
}


#ifndef SHIFT_RC_IMAGE_UTIL_CONVERT_HPP
#define SHIFT_RC_IMAGE_UTIL_CONVERT_HPP

#include <cstdint>
#include <type_traits>
#include <boost/assert.hpp>
#include <shift/core/mpl.hpp>
#include <shift/math/half.hpp>
#include <shift/math/vector.hpp>
#include "shift/rc/image_util/linear_image_view.hpp"
#include "shift/rc/image_util/convert_pixel_component.hpp"

namespace shift::rc::image_util
{
namespace detail
{
  template <typename T, typename PixelChannel>
  struct unorm_converter
  {
    static_assert(std::is_integral_v<T> && std::is_unsigned_v<T>);
    static_assert(!PixelChannel::is_block_format);

    static constexpr float to_float(T value)
    {
      return value / static_cast<float>((1 << PixelChannel::size_in_bits) - 1);
    }

    static constexpr T from_float(float value)
    {
      return static_cast<T>(value * ((1 << PixelChannel::size_in_bits) - 1));
    }
  };

  template <typename T, typename PixelChannel>
  struct snorm_converter
  {
    static_assert(std::is_integral_v<T> && std::is_signed_v<T>);
    static_assert(!PixelChannel::is_block_format);

    static constexpr float to_float(T value)
    {
      static_assert(
        static_cast<T>(std::numeric_limits<std::make_unsigned_t<T>>::max()
                       << 1) == -2);
      // The minimum value of a PixelChannel::size_in_bits sized signed integer,
      // sign extended and stored in a value of type T.
      constexpr T min_value =
        static_cast<T>(std::numeric_limits<std::make_unsigned_t<T>>::max()
                       << (PixelChannel::size_in_bits - 1));
      if (value == min_value)
      {
        return -1.0f;
      }
      else
      {
        return value /
               static_cast<float>((1 << (PixelChannel::size_in_bits - 1)) - 1);
      }
    }

    static constexpr T from_float(float value)
    {
      return static_cast<T>(value *
                            ((1 << (PixelChannel::size_in_bits - 1)) - 1));
    }
  };

  struct srgb_converter
  {
    static constexpr float to_linear(float srgb_value)
    {
      if (srgb_value <= 0.0f)
        return 0.0f;
      else if (srgb_value < 0.04045f)
        return srgb_value / 12.92f;
      else if (srgb_value < 1.0f)
        return std::pow((srgb_value + 0.055f) / 1.055f, 2.4f);
      else
        return 1.0f;
    }

    static constexpr float from_linear(float linear_value)
    {
      if (linear_value <= 0.0f)
        return 0.0f;
      else if (linear_value < 0.0031308f)
        return linear_value * 12.92f;
      else if (linear_value < 1.0f)
        return std::pow(linear_value, 1.0f / 2.4f) * 1.055f - 0.055f;
      else
        return 1.0f;
    }
  };

  template <typename DestinationPixel, typename SourcePixel>
  struct channel_converter
  {
    template <typename DestinationChannel>
    void operator()(const DestinationChannel*,
                    DestinationPixel& destination_pixel,
                    const SourcePixel& source_pixel) const
    {
      using destination_t = typename DestinationPixel::component_t;
      using source_t = typename SourcePixel::component_t;

      constexpr auto destination_channel_index =
        channel_index_v<typename DestinationPixel::channels_t,
                        DestinationChannel>;
      static_assert(destination_channel_index <
                    DestinationPixel::channel_count);
      using destination_channel_t =
        core::get_type_opt_t<destination_channel_index,
                             typename DestinationPixel::channels_t>;

      constexpr auto source_channel_index =
        channel_index_v<typename SourcePixel::channels_t, DestinationChannel>;
      using source_channel_t =
        core::get_type_opt_t<source_channel_index,
                             typename SourcePixel::channels_t>;

      if constexpr (source_channel_index >= SourcePixel::channel_count)
      {
        // There is no matching source channel. We have to fill the destination
        // with a default value.
        destination_pixel[destination_channel_index] = destination_t{};
      }
      else if constexpr ((SourcePixel::data_type ==
                          DestinationPixel::data_type) &&
                         std::is_same_v<source_t, destination_t> &&
                         std::is_same_v<source_channel_t,
                                        destination_channel_t>)
      {
        // Source and destination channels are of the same channel type and
        // data type, so we can perform a direct copy.
        destination_pixel[destination_channel_index] =
          source_pixel[source_channel_index];
      }
      else if constexpr (SourcePixel::data_type == pixel_data_type::unorm)
      {
        if constexpr (DestinationPixel::data_type == pixel_data_type::unorm)
        {
          // Convert unorm to float.
          auto temp = unorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::snorm)
        {
          // Convert unorm to float.
          auto temp = unorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Convert float to snorm.
          destination_pixel[destination_channel_index] =
            snorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                             pixel_data_type::ufloat ||
                           DestinationPixel::data_type ==
                             pixel_data_type::sfloat)
        {
          // Convert unorm to float.
          destination_pixel[destination_channel_index] =
            unorm_converter<source_t, source_channel_t>::to_float(
              source_pixel[source_channel_index]);
        }
        else if constexpr (DestinationPixel::data_type == pixel_data_type::srgb)
        {
          // Convert unorm to float.
          auto temp = unorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Convert color space from linear to sRGB.
          temp = srgb_converter::from_linear(temp);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else
          BOOST_ASSERT(false);
      }
      else if constexpr (SourcePixel::data_type == pixel_data_type::snorm)
      {
        if constexpr (DestinationPixel::data_type == pixel_data_type::unorm)
        {
          // Convert snorm to float.
          auto temp = snorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Clamp values < 0.0f.
          temp = std::max(temp, 0.0f);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::snorm)
        {
          // Convert snorm to float.
          auto temp = snorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Convert float to snorm.
          destination_pixel[destination_channel_index] =
            snorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                             pixel_data_type::ufloat ||
                           DestinationPixel::data_type ==
                             pixel_data_type::sfloat)
        {
          // Convert snorm -> float.
          destination_pixel[destination_channel_index] =
            snorm_converter<source_t, source_channel_t>::to_float(
              source_pixel[source_channel_index]);
        }
        else if constexpr (DestinationPixel::data_type == pixel_data_type::srgb)
        {
          // Convert snorm -> float.
          auto temp = snorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Clamp values < 0.0f.
          temp = std::max(temp, 0.0f);
          // Convert color space from linear to sRGB.
          temp = srgb_converter::from_linear(temp);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else
          BOOST_ASSERT(false);
      }
      else if constexpr (SourcePixel::data_type == pixel_data_type::ufloat)
      {
        if constexpr (DestinationPixel::data_type == pixel_data_type::unorm)
        {
          // Clamp values above 1.0f.
          auto temp = std::min(
            static_cast<float>(source_pixel[source_channel_index]), 1.0f);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::snorm)
        {
          // Clamp values above 1.0f.
          auto temp = std::min(
            static_cast<float>(source_pixel[source_channel_index]), 1.0f);
          // Convert float to snorm.
          destination_pixel[destination_channel_index] =
            snorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::ufloat)
        {
          // Simply copy value.
          destination_pixel[destination_channel_index] =
            static_cast<float>(source_pixel[source_channel_index]);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::sfloat)
        {
          // Simply copy value.
          destination_pixel[destination_channel_index] =
            static_cast<destination_t>(source_pixel[source_channel_index]);
        }
        else if constexpr (DestinationPixel::data_type == pixel_data_type::srgb)
        {
          // Clamp values above 1.0f.
          auto temp = std::min(
            static_cast<float>(source_pixel[source_channel_index]), 1.0f);
          // Convert color space from linear to sRGB.
          temp = srgb_converter::from_linear(temp);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else
          BOOST_ASSERT(false);
      }
      else if constexpr (SourcePixel::data_type == pixel_data_type::sfloat)
      {
        if constexpr (DestinationPixel::data_type == pixel_data_type::unorm)
        {
          // Clamp values between 0.0f and 1.0f.
          auto temp = std::clamp(
            static_cast<float>(source_pixel[source_channel_index]), 0.0f, 1.0f);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::snorm)
        {
          // Clamp values between -1.0f and 1.0f.
          auto temp =
            std::clamp(static_cast<float>(source_pixel[source_channel_index]),
                       -1.0f, 1.0f);
          // Convert float to snorm.
          destination_pixel[destination_channel_index] =
            snorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::ufloat)
        {
          // Clamp values below 0.0f.
          destination_pixel[destination_channel_index] = std::max(
            static_cast<float>(source_pixel[source_channel_index]), 0.0f);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::sfloat)
        {
          // Simply copy value.
          destination_pixel[destination_channel_index] =
            static_cast<destination_t>(source_pixel[source_channel_index]);
        }
        else if constexpr (DestinationPixel::data_type == pixel_data_type::srgb)
        {
          // Clamp values between 0.0f and 1.0f.
          auto temp = std::clamp(
            static_cast<float>(source_pixel[source_channel_index]), 0.0f, 1.0f);
          // Convert color space from linear to sRGB.
          temp = srgb_converter::from_linear(temp);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else
          BOOST_ASSERT(false);
      }
      else if constexpr (SourcePixel::data_type == pixel_data_type::srgb)
      {
        if constexpr (DestinationPixel::data_type == pixel_data_type::unorm)
        {
          // Convert unorm to float.
          auto temp = unorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Convert color space from sRGB to linear.
          temp = srgb_converter::to_linear(temp);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::snorm)
        {
          // Convert unorm to float.
          auto temp = unorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Convert color space from sRGB to linear.
          temp = srgb_converter::to_linear(temp);
          // Convert float to snorm.
          destination_pixel[destination_channel_index] =
            snorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                             pixel_data_type::ufloat ||
                           DestinationPixel::data_type ==
                             pixel_data_type::sfloat)
        {
          // Convert unorm to float.
          auto temp = unorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Convert color space from sRGB to linear.
          destination_pixel[destination_channel_index] =
            srgb_converter::to_linear(temp);
        }
        else if constexpr (DestinationPixel::data_type == pixel_data_type::srgb)
        {
          // Convert unorm to float.
          auto temp = unorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else
          BOOST_ASSERT(false);
      }
      else
        BOOST_ASSERT(false);
    }
  };
}

/// This class converts a single pixel from one format to another.
template <typename DestinationPixel, typename SourcePixel>
class pixel_converter
{
public:
  using destination_pixel_t = DestinationPixel;
  using source_pixel_t = SourcePixel;

  void operator()(destination_pixel_t& destination,
                  const source_pixel_t& source) const
  {
    if constexpr (std::is_same_v<destination_pixel_t, source_pixel_t>)
      std::memcpy(&destination, &source, destination_pixel_t::size_in_bytes);
    else
    {
      // Convert each destination pixel channel separately.
      core::for_each<typename DestinationPixel::channels_t>(
        detail::channel_converter<DestinationPixel, SourcePixel>{}, destination,
        source);
    }
  }
};
}

#endif


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
#include"taskspaceonline3.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<geometry_msgs/Wrench.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"
#include<termios.h>
#include<gazebo_msgs/LinkStates.h>
#include<sensor_msgs/JointState.h>
#include"pidcontroller.h"

using namespace  std;
using namespace  Eigen;

bool left_first=true;//right support in first step
bool backward=false;
bool turning=false;
double TurningRadius=.5;//for on spot .01;
bool sidewalk=false;
int bump_threshold=75;//85;
bool simulation=false;
bool AnkleZAdaptation=!false;
bool LogDataSend=false;

double k_pitch=1.5;//1;0.8;
double pelvis_roll_range=2.5;

double saturate(double a, double min, double max){
    if(a<min){return min;ROS_INFO("subceeding!");}
    else if(a>max){return max;ROS_INFO("exceeding!");}
    else{return a;}
}

void matrix_view(MatrixXd M){

    for (int i = 0; i <M.rows() ; ++i) {
        QString str;
        for (int j = 0; j <M.cols() ; ++j) {
            str+=QString::number(M(i,j));
            str+="   ";
        }
        qDebug()<<str;
    }
    qDebug()<<"";
}

void matrix_view(VectorXd M){
    QString str;
    for (int i = 0; i <M.rows() ; ++i) {str+=QString::number(M(i));str+="   ";}
    qDebug()<<str;
    qDebug()<<"";
}

double move_rest_back(double max,double t_local,double T_start ,double T_move,double T_rest,double T_back){
    double c3=(10*max)/pow(T_move,3);
    double c4=-(15*max)/pow(T_move,4);
    double c5=(6*max)/pow(T_move,5);
    double c3_r=(10*max)/pow(T_back,3);
    double c4_r=-(15*max)/pow(T_back,4);
    double c5_r=(6*max)/pow(T_back,5);
    double T_end=T_start+T_move+T_rest+T_back;
    double theta=0;
    if(t_local<T_start){theta=0;}
    else if (t_local<T_start+T_move){theta=c3*pow(t_local-T_start,3)+c4*pow(t_local-T_start,4)+c5*pow(t_local-T_start,5);}
    else if (t_local<T_start+T_move+T_rest){theta=max;}
    else if (t_local<T_start+T_move+T_rest+T_back){theta=c3_r*pow(T_end-t_local,3)+c4_r*pow(T_end-t_local,4)+c5_r*pow(T_end-t_local,5);}
    return theta;
}

double move2pose(double max,double t_local,double T_start ,double T_end){
    double T_move=T_end-T_start;
    double c3=(10*max)/pow(T_move,3);
    double c4=-(15*max)/pow(T_move,4);
    double c5=(6*max)/pow(T_move,5);
    double theta=0;
    if(t_local<T_start){theta=0;}
    else if (t_local<T_end){theta=c3*pow(t_local-T_start,3)+c4*pow(t_local-T_start,4)+c5*pow(t_local-T_start,5);}
    else{theta=max;}
    return theta;
}

int getch()
{
    static struct termios oldt, newt;
    tcgetattr( STDIN_FILENO, &oldt);           // save old settings
    newt = oldt;
    newt.c_lflag &= ~(ICANON);                 // disable buffering
    tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings

    int c = getchar();  // read character (non-blocking)

    tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
    return c;
}

MinimumJerkInterpolation CoefGen;

//data of left foot sensor
int a,b,c,d,e,f,g,h;

//angles of ankle adaptation

double AnkleZR,AnkleZL;
double AnkleZ_offsetR=0;
double AnkleZ_offsetL=0;

int qc_offset[12];
bool qc_initial_bool;

//

int qra_offset[4];
int qla_offset[4];

int inc_feedback[12];
void qc_initial(const sensor_msgs::JointState & msg){

    if (qc_initial_bool){

        for (int i = 0; i < 12; ++i) {
            qc_offset[i]=int(msg.position[i+1]);

        }

        for (int i = 12; i < 16; ++i){
            qra_offset[i-12]=int(msg.position[i+1]);
        }

        for (int i = 16; i < 20; ++i){
            qla_offset[i-16]=int(msg.position[i+1]);
        }

        qc_initial_bool=false;

        ROS_INFO("Offset=%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\nInitialized!",
                 qc_offset[0],qc_offset[1],qc_offset[2],qc_offset[3],qc_offset[4],
                qc_offset[5],qc_offset[6],qc_offset[7],qc_offset[8],qc_offset[9],
                qc_offset[10],qc_offset[11]);}
}

int abs_feedback[12];
void abs_feedback_func(const sensor_msgs::JointState & msg){
    for (int i = 0; i < 12; ++i) {
        abs_feedback[i]=int(msg.position[i+1]);
    }
}

MatrixXd quater2rot(double w,double x,double y, double z){
    MatrixXd R(3,3);
    R<<w*w+x*x-y*y-z*z,2*x*y-2*w*z,2*x*z+2*w*y,
            2*x*y+2*w*z,w*w-x*x+y*y-z*z,2*y*z-2*w*x,
            2*x*z-2*w*y,2*y*z+2*w*x,w*w-x*x-y*y+z*z;
    return R;
}

//*****quaternion to euler params in ankle
double quaternion2euler_pitch(double q0,double q1,double q2,double q3){
    double R11,R32,R33,R31,theta;
    R31=2*(q1*q3-q0*q2);
    R32=2*(q0*q1+q2*q3);
    R33=q0*q0-q1*q1-q2*q2+q3*q3;
    theta=atan2(-R31,sqrt(R32*R32+R33*R33));
    return theta;
}

double quaternion2euler_roll(double q0,double q1,double q2,double q3){
    double phi,R33,R32;
    R32=2*(q0*q1+q2*q3);
    R33=q0*q0-q1*q1-q2*q2+q3*q3;
    phi=atan2(R32,R33);
    return phi;
}

double GlobalTime;

int bump_pushed[8];
int bump_notpushed[8];
bool bump_initialize;
void receiveFootSensor(const std_msgs::Int32MultiArray& msg)
{
    if (msg.data.size()!=8) {
        qDebug("the size of sensor data is in wrong");
        return;
    }
    if(bump_initialize){
        for (int i = 0; i < 8; ++i) {
            bump_pushed[i]=msg.data[i];
        }
        bump_initialize=false;
    }
    //ROS_INFO("I heard: [%d  %d %d %d %d  %d %d %d]", (int)msg.data[0],(int)msg.data[1],(int)msg.data[2],(int)msg.data[3],(int)msg.data[4],(int)msg.data[5],(int)msg.data[6],(int)msg.data[7]);
    int temp[8];

//    bump_pushed[0]=1094;bump_pushed[1]= 844;bump_pushed[2]=3129;bump_pushed[3]=3005;
//    bump_pushed[4]=3126;bump_pushed[5]=2920;bump_pushed[6]=1212;bump_pushed[7]=920;
//    bump_notpushed[0]=1012;bump_notpushed[1]= 931;bump_notpushed[2]=3037;bump_notpushed[3]=3098;
//    bump_notpushed[4]=3042;bump_notpushed[5]=3009;bump_notpushed[6]=1120;bump_notpushed[7]=1015;

    bump_pushed[0]=1098;bump_pushed[1]= 837;bump_pushed[2]=3129;bump_pushed[3]=3000;
    bump_pushed[4]=3119;bump_pushed[5]=2907;bump_pushed[6]=1205;bump_pushed[7]=916;
    bump_notpushed[0]=1018;bump_notpushed[1]= 930;bump_notpushed[2]=3033;bump_notpushed[3]=3098;
    bump_notpushed[4]=3042;bump_notpushed[5]=3005;bump_notpushed[6]=1115;bump_notpushed[7]=1015;

    temp[0]=msg.data[0]-bump_notpushed[0];
    temp[1]=-1*(msg.data[1]-bump_notpushed[1]);
    temp[2]=msg.data[2]-bump_notpushed[2];
    temp[3]=-1*(msg.data[3]-bump_notpushed[3]);

    //normalizing data of sensors
    a=temp[0]*(100.0/(bump_pushed[0]-bump_notpushed[0]));
    b=temp[1]*(100.0/(bump_notpushed[1]-bump_pushed[1]));
    c=temp[2]*(100.0/(bump_pushed[2]-bump_notpushed[2]));
    d=temp[3]*(100.0/(bump_notpushed[3]-bump_pushed[3]));

    //ROS_INFO("I heard a b c d: [%d  %d %d %d]", a,b,c,d);

    temp[4]=msg.data[4]-bump_notpushed[4];
    temp[5]=-1*(msg.data[5]-bump_notpushed[5]);
    temp[6]=msg.data[6]-bump_notpushed[6];
    temp[7]=-1*(msg.data[7]-bump_notpushed[7]);

    //normalizing data of sensors
    e=temp[4]*(100.0/(bump_pushed[4]-bump_notpushed[4]));
    f=temp[5]*(100.0/(bump_notpushed[5]-bump_pushed[5]));
    g=temp[6]*(100.0/(bump_pushed[6]-bump_notpushed[6]));
    h=temp[7]*(100.0/(bump_notpushed[7]-bump_pushed[7]));

    if (a<0){a=0;} if (b<0){b=0;} if (c<0){c=0;} if (d<0){d=0;}
    if (e<0){e=0;} if (f<0){f=0;} if (g<0){g=0;} if (h<0){h=0;}
    //qDebug()<<"a:"<<a<<"\tb:"<<b<<"\tc:"<<c<<"\td:"<<d<<"\ne:"<<e<<"\tf:"<<f<<"\tg:"<<g<<"\th:"<<h;

//cout<<msg.data[0]<<"\t"<<msg.data[1]<<"\t"<<msg.data[2]<<"\t"<<msg.data[3]<<"\t"<<msg.data[4]
//               <<"\t"<<msg.data[5]<<"\t"<<msg.data[6]<<"\t"<<msg.data[7]<<endl;

}

double teta_motor_L=0;
double teta_motor_R=0;//pitch
double phi_motor_L=0;
double phi_motor_R=0;//roll

PIDController teta_PID_L;
PIDController teta_PID_R;
PIDController phi_PID_L;
PIDController phi_PID_R;

void ankleAdaptation(){
    //parameters of ankle adaptation
    double k1,k2,k3,k4;
    //k1=0.000015;    k2=0.000015;    k3=0.000015;    k4=0.000015;
    k1=0.00004;0.0000215;
    k2=0.00004;0.0000215;
    k3=0.00004;0.0000215;
    k4=0.00004;0.0000215;

    double threshold=4;
    double threshold2=70;

    if(a>threshold2 &&b>threshold2&&c>threshold2&&d>threshold2){

    }
    else if(a<threshold &&b<threshold &&c<threshold &&d<threshold){
      //  theta_motor_L->0,phi_motor_L->0
        teta_motor_L+=phi_PID_L.Calculate(0,teta_motor_L);
        phi_motor_L+=phi_PID_L.Calculate(0,phi_motor_L);
    }

    else if (a>threshold ||b>threshold ||c>threshold ||d>threshold){//left
        //-----------------Pitch left ankle motor control---------------//
        if (abs(b-a)>=abs(c-d)) {
            if (abs(a-b)<120) {teta_motor_L=1*teta_motor_L+k1*(a-b);}
        }
        else {
            if (abs(d-c)<120) {teta_motor_L=1*teta_motor_L+k1*(d-c);}
        }

        //----------------Roll left ankle motor control---------------//
        if (abs(c-b)>=abs(d-a)) {
            if (abs(c-b)<120) {phi_motor_L=1*phi_motor_L+k2*(c-b);}
        }
        else {
            if (abs(a-d)<120) {phi_motor_L=1*phi_motor_L+k2*(d-a);}
        }
    }

    if(e>threshold2 &&f>threshold2&&g>threshold2&&h>threshold2){

    }
    else if(e<threshold &&f<threshold &&g<threshold &&h<threshold){
      //  theta_motor_L->0,phi_motor_L->0
        teta_motor_R+=phi_PID_R.Calculate(0,teta_motor_R);
        phi_motor_R+=phi_PID_R.Calculate(0,phi_motor_R);
    }
    else if(e>threshold ||f>threshold ||g>threshold ||h>threshold){//right

        //-----------------Pitch left ankle motor control---------------//
        if (abs(f-e)>=abs(g-h)) {
            if (abs(e-f)<120) {teta_motor_R=1*teta_motor_R+k3*(e-f);}
        }
        else {
            if (abs(h-g)<120) {teta_motor_R=1*teta_motor_R+k3*(h-g);}
        }

        //----------------Roll left ankle motor control---------------//
        if (abs(g-f)>=abs(h-e)) {
            if (abs(g-f)<120) {phi_motor_R=1*phi_motor_R+k4*(g-f);}
        }
        else {
            if (abs(e-h)<120) {phi_motor_R=1*phi_motor_R+k4*(h-e);}
        }

}

        //------------------------saturation of ankle motors----------------------------//
    phi_motor_L=saturate(phi_motor_L,-M_PI/18,M_PI/18);
    phi_motor_R=saturate(phi_motor_R,-M_PI/18,M_PI/18);
    teta_motor_L=saturate(teta_motor_L,-M_PI/18,M_PI/18);
    teta_motor_R=saturate(teta_motor_R,-M_PI/18,M_PI/18);
//        if ((abs(phi_motor_L))>0.9) {phi_motor_L=0.9;}
//        if ((abs(teta_motor_L))>0.9) {teta_motor_L=0.9;}
//        if ((abs(phi_motor_R))>0.9) {phi_motor_L=0.9;}
//        if ((abs(teta_motor_R))>0.9) {teta_motor_R=0.9;}

        qDebug()<<"a:"<<a<<"\tb:"<<b<<"\tc:"<<c<<"\td:"<<d<<"\ne:"<<e<<"\tf:"<<f<<"\tg:"<<g<<"\th:"<<h;

        qDebug()<<"teta_motor_L:"<<teta_motor_L<<"phi_motor_L:"<<phi_motor_L<<
"\nteta_motor_R:"<<teta_motor_R<<"phi_motor_R:"<<phi_motor_R;
}

int main(int argc, char **argv)
{
double kp=.02;
    teta_PID_L.Init(.005,.9,-.9,kp,0,0);
    teta_PID_R.Init(.005,.9,-.9,kp,0,0);
    phi_PID_L.Init(.005,.9,-.9,kp,0,0);
    phi_PID_R.Init(.005,.9,-.9,kp,0,0);

    qc_initial_bool=!simulation;
    bump_initialize=false;

    vector<double> cntrl(13);
    QCgenerator QC;
    for (int i = 0; i < 12; ++i) {
        qc_offset[i]=0;
    }

    GlobalTime=0;

    //*******************This part of code is for initialization of joints of the robot for walking**********************************
    int count = 0;

    ros::init(argc, argv, "myNode");
    ros::NodeHandle nh;
    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",100);
    ros::Publisher  contact_flag  = nh.advertise<std_msgs::Int32MultiArray>("contact_flag_timing",100);
    ros::Publisher  trajectory_data_pub  = nh.advertise<std_msgs::Float32MultiArray>("my_trajectory_data",100);
    std_msgs::Float32MultiArray trajectory_data;

    ros::Subscriber sub = nh.subscribe("/surena/bump_sensor_state", 1000, receiveFootSensor);
    ros::Subscriber qcinit = nh.subscribe("/surena/inc_joint_state", 1000, qc_initial);

    ros::Rate loop_rate(200);
    std_msgs::Int32MultiArray msg;
    std_msgs::MultiArrayDimension msg_dim;

    msg_dim.label = "joint_position";
    msg_dim.size = 1;
    msg.layout.dim.clear();
    msg.layout.dim.push_back(msg_dim);

    ROS_INFO("press any key to start!");
    getch();
    ROS_INFO("started!");

    while (ros::ok())
    {

        if (qc_initial_bool) {
            ROS_INFO_ONCE("qc is initializing!");
            ros::spinOnce();
            continue;
        }
        ankleAdaptation();
        for (int var = 0; var < 13; ++var) {
           cntrl[var]=0.0;
        }
        cntrl[5]=teta_motor_R;
        cntrl[6]=phi_motor_R;
        cntrl[11]=teta_motor_L;
        cntrl[12]=phi_motor_L;

        vector<int> qref(12);
        qref=QC.ctrldata2qc(cntrl);

        msg.data.clear();

            for(int  i = 0;i < 12;i++)
            {
                msg.data.push_back(qref[i]+qc_offset[i]);
            }

                for(int  i = 12;i < 29;i++)
                {
                    msg.data.push_back(0);
                }

            chatter_pub.publish(msg);

        ros::spinOnce();

        loop_rate.sleep();
        ++count;

    }

    return 0;
}


/*
 * @file
 * state_plugin_estimator.hpp
 *
 *  Created on: 26.07.2018
 *      Author: tomlucas
 */

#ifndef PLUGINS_STATE_PLUGIN_ESTIMATOR_HPP_
#define PLUGINS_STATE_PLUGIN_ESTIMATOR_HPP_

#include "state_plugin_base.hpp"
#include "../Estimators/Estimator.hpp"
#include <OSG_Utils.hpp>

#include <Eigen/Core>
namespace zavi::plugin{

	class StatePluginEstimator : public StatePlugin{
public:

				StatePluginEstimator(std::shared_ptr<estimator::Estimator> estimator):estimator(estimator){

				}
				inline ::osg::Vec3d getPositionOSG(){
						return zavi::osg_viz::eigenToOSGVector(  estimator->getEstimatedPosition());
				}
				inline Eigen::Matrix3d getPositionError(){
						return estimator->getEstimatedPositionError();
				}

				inline ::osg::Quat getRotationOSG(){
					return zavi::osg_viz::eigenToOSGQuat(estimator->getEstimatedOrientation());
				}

private:
				std::shared_ptr<estimator::Estimator> estimator;
	};

}//zavi::plugin

#endif /* PLUGINS_STATE_PLUGIN_ESTIMATOR_HPP_ */