#include "drift_cornering_ilqr.h"

#include <math.h>
#include <boost/random.hpp>

#include <ros/ros.h>
#include <ilqr_msgs/IlqrInput.h>
#include <ilqr_msgs/IlqrOutput.h>
#include <ilqr_msgs/BoolStamped.h>

#define G 9.81

namespace stationary {
    #include "ilqr_params_stationary.h"
    // extern "C" {
        #include "iLQGStationary/iLQG.h"
        #include "iLQGStationary/iLQG_problem.h"
        #include "iLQGStationary/matMult.h"
    // }
}

using namespace stationary;

template <typename T>
inline bool getRequiredParam(const ros::NodeHandle& n, std::string name, T& value)
{
  if (n.getParam(name, value))
    return true;

  ROS_FATAL("iLQR: Parameter %s is required.", name.c_str());
  return false;
}

static tOptSet* Op_;

void iLQR::_setOpStationary(ros::NodeHandle nh, ros::NodeHandle pnh)
{
    // set values in Op_
    Op_ = (tOptSet *) malloc(sizeof(tOptSet));
    Op_->log_linesearch = NULL;
    Op_->log_z = NULL;
    Op_->log_cost = NULL;
    if (_assignParamsStationary(nh, pnh)) return;
    standard_parameters(Op_);
    Op_->n_hor = HORIZON_STATIONARY;
    Op_->max_iter = MAX_ITER;

    // allocate memory for Op_
    for (int i=0; i<NUMBER_OF_THREADS+1; i++) Op_->trajectories[i].t = (trajEl_t *) malloc(sizeof(trajEl_t)*HORIZON_STATIONARY);
    Op_->multipliers.t = (multipliersEl_t *) malloc(sizeof(multipliersEl_t)*(HORIZON_STATIONARY+1));
    Op_->nominal = (traj_t *) malloc(sizeof(traj_t));
    Op_->nominal->t = (trajEl_t *) malloc(sizeof(trajEl_t)*HORIZON_STATIONARY);
    for (int i=0; i<NUMBER_OF_THREADS; i++) {
        Op_->candidates[i] = (traj_t *) malloc(sizeof(traj_t));
        Op_->candidates[i]->t = (trajEl_t *) malloc(sizeof(trajEl_t)*HORIZON_STATIONARY);
    }

    // wait for equilibrium to be set and set goal in Op_
    ros::Duration(2).sleep();
    Op_->p[P_GOAL_IDX_STATIONARY] = &equilibrium_[2];
}

int iLQR::_assignParamsStationary(ros::NodeHandle nh, ros::NodeHandle pnh)
{
    // get values from rosparam server
    if (!getRequiredParam(nh, "vehicle/mass", _m)) return 1;
    if (!getRequiredParam(nh, "vehicle/I_zz", _I_z)) return 1;
    if (!getRequiredParam(nh, "vehicle/wheelbase_f", _L_f)) return 1;
    if (!getRequiredParam(nh, "vehicle/wheelbase_r", _L_r)) return 1;
    _load_f = _m*G*_L_r/(_L_f+_L_r);
    _load_r = _m*G*_L_f/(_L_f+_L_r);
    if (!getRequiredParam(nh, "vehicle/C_x", _c_x)) return 1;
    if (!getRequiredParam(nh, "vehicle/C_a", _c_a)) return 1;
    if (!getRequiredParam(nh, "vehicle/mu", _mu)) return 1;
    if (!getRequiredParam(nh, "vehicle/mu_s", _mu_s)) return 1;
    if (!getRequiredParam(nh, "vehicle/max_steering_angle", _limstr)) return 1;
    _limSteer[0] = -_limstr;
    _limSteer[1] = _limstr;

    // assign names and values to params
    if ( (_params = (param_t *) malloc(n_const_params*sizeof(param_t))) == NULL) {
        ROS_FATAL("Not enough memory.");
        return 1;
    }
    if ( (Op_->p = (double **) malloc(n_params*sizeof(double *))) == NULL) {
        ROS_FATAL("Not enough memory.");
        return 1;
    }

    int num_set = 0;

    for (int i=0; i<n_const_params; i++)
    {
        _params[i].name = (char *)_param_names[i];
        _params[i].size = 0;

        //// REMEMBER TO CHANGE THESE IF THE iLQG_func.c FILE IS CHANGED!!!!!!!  ////
        ////                     THESE ARE THE CONSTANT PARAMS                   ////

        if (strcmp(_params[i].name , "I_z"      ) == 0) _params[i].data = &_I_z           ;
        if (strcmp(_params[i].name , "L_f"      ) == 0) _params[i].data = &_L_f           ;
        if (strcmp(_params[i].name , "L_r"      ) == 0) _params[i].data = &_L_r           ;
        if (strcmp(_params[i].name , "c_a"      ) == 0) _params[i].data = &_c_a           ;
        if (strcmp(_params[i].name , "c_x"      ) == 0) _params[i].data = &_c_x           ;
        if (strcmp(_params[i].name , "cdu"      ) == 0) _params[i].data = &_cdu[0]        ;
        if (strcmp(_params[i].name , "cf"       ) == 0) _params[i].data = &_cf[0]         ;
        if (strcmp(_params[i].name , "cu"       ) == 0) _params[i].data = &_cu[0]         ;
        if (strcmp(_params[i].name , "cx"       ) == 0) _params[i].data = &_cx[0]         ;
        if (strcmp(_params[i].name , "dt"       ) == 0) _params[i].data = &_dt            ;
        if (strcmp(_params[i].name , "limSteer" ) == 0) _params[i].data = &_limSteer[0]   ;
        if (strcmp(_params[i].name , "limThr"   ) == 0) _params[i].data = &_limThr[0]     ;
        if (strcmp(_params[i].name , "load_f"   ) == 0) _params[i].data = &_load_f        ;
        if (strcmp(_params[i].name , "load_r"   ) == 0) _params[i].data = &_load_r        ;
        if (strcmp(_params[i].name , "m"        ) == 0) _params[i].data = &_m             ;
        if (strcmp(_params[i].name , "mu"       ) == 0) _params[i].data = &_mu            ;
        if (strcmp(_params[i].name , "mu_s"     ) == 0) _params[i].data = &_mu_s          ;
        if (strcmp(_params[i].name , "pf"       ) == 0) _params[i].data = &_pf[0]         ;
        if (strcmp(_params[i].name , "px"       ) == 0) _params[i].data = &_px[0]         ;

        for (int j=0; j<n_params; j++) {
            if (strcmp(_params[i].name,paramdesc[j]->name)==0) {
                _params[i].size = paramdesc[j]->size;
                Op_->p[j] = (double *) malloc(sizeof(double)*paramdesc[j]->size);
                memcpy(Op_->p[j], _params[i].data, _params[i].size*sizeof(double));
                num_set++;
                break;
            }
        }
        if (_params[i].size == 0)
            ROS_ERROR("iLQR parameter %s is not member of parameters struct.", _params[i].name);
    }

    if (num_set != n_const_params)
        ROS_ERROR("%d iLQR parameters not assigned.", n_const_params-num_set);
    else
        return 0;

}

void iLQR::_ilqrStationary(const ilqr_msgs::IlqrInput::ConstPtr& msg)
{
    // get input values to iLQR planner
    x0_[0] = msg->state.pose.x;
    x0_[1] = msg->state.pose.y;
    x0_[2] = msg->state.pose.theta;
    x0_[3] = msg->state.twist.x;
    x0_[4] = msg->state.twist.y;
    x0_[5] = msg->state.twist.theta;
    x0_[6] = msg->previouscommand.speed;
    x0_[7] = msg->previouscommand.steering_angle;

    int i;
    for (i=0;i<msg->remainingcommands.size();i++) {
        u0_stationary_[2*i] = msg->remainingcommands[i].speed;
        u0_stationary_[2*i+1] = msg->remainingcommands[i].steering_angle;
    }
    for (;i<N_-1;i++) {
        u0_stationary_[2*i] = vel_dist(vel_gen);
        u0_stationary_[2*i+1] = steer_dist(steer_gen);
    }

    // assign Op_ values
    Op_->x0 = x0_;

    // outputs
    double x_new[N_*N_X], u_new[(N_-1)*N_U];
    ilqr_msgs::IlqrOutput output;
    output.commands.resize(N_-1);
    output.states.resize(N_);

    if (!init_opt(Op_)) {
        ROS_ERROR("Failed to initialize opt.");
        output.success = 0;
        output.cost = Op_->cost;
    } else {
        for (int c=0; c<N_-1; c++)
            for (int r=0; r<N_U; r++)
                Op_->nominal->t[c].u[r] = u0_stationary_[MAT_IDX(r, c, N_U)];

        if (!forward_pass(Op_->candidates[0], Op_, 0.0, &Op_->cost, 0)) {
            ROS_ERROR("Forward pass failed");
            output.success = 0;
            output.cost = Op_->cost;
        }

        else
        {
            makeCandidateNominal(Op_, 0);
            // ROS_DEBUG("Starting iLQG");
            output.success = iLQG(Op_);
            printf("iLQG %s.\n", output.success?"success":"fail");

            for (int c=0; c<N_-1; c++)
                for (int r=0; r<N_X; r++)
                    x_new[MAT_IDX(r, c, N_X)] = Op_->nominal->t[c].x[r];
            for (int r=0; r<N_X; r++)
                x_new[MAT_IDX(r, N_-1, N_X)] = Op_->nominal->f.x[r];

            for (int c=0; c<N_-1; c++)
                for (int r=0; r<N_U; r++)
                    u_new[MAT_IDX(r, c, N_U)] = Op_->nominal->t[c].u[r];

            output.cost = Op_->cost;

            // copy to ROS message
            for (int i=0; i<N_; i++) {
                output.states[i].pose.x = x_new[MAT_IDX(0, i, N_X)];
                output.states[i].pose.y = x_new[MAT_IDX(1, i, N_X)];
                output.states[i].pose.theta = x_new[MAT_IDX(2, i, N_X)];
                output.states[i].twist.x = x_new[MAT_IDX(3, i, N_X)];
                output.states[i].twist.y = x_new[MAT_IDX(4, i, N_X)];
                output.states[i].twist.theta = x_new[MAT_IDX(5, i, N_X)];
            }
            for (int i=0; i<N_-1; i++) {
                output.commands[i].speed = u_new[MAT_IDX(0, i, N_U)];
                output.commands[i].steering_angle = u_new[MAT_IDX(1, i, N_U)];
            }
        }
    }

    output.header.stamp = ros::Time::now();
    ilqr_pub_.publish(output);
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_ACOTD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACOTD_HPP_INCLUDED

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

 /*!

    @ingroup group-trigonometric
    Function object implementing acotd capabilities

    inverse cotangent in degree.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = acotd(x);
    @endcode

    Returns the arc @c r in the interval
    \f$[0, 180[\f$ such that <tt>cotd(r) == x</tt>.

    @see acot

  **/
  const boost::dispatch::functor<tag::acotd_> acotd = {};
} }
#endif

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

#endif


/*
Copyright 2014, 2015 Rogier van Dalen.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

/** \file
Define helpers for magmas that consist of other magmas.
*/

#ifndef MATH_DETAIL_TUPLE_HELPER_HPP_INCLUDED
#define MATH_DETAIL_TUPLE_HELPER_HPP_INCLUDED

#include <type_traits>

#include <boost/utility/enable_if.hpp>

#include <boost/mpl/and.hpp>
#include <boost/mpl/not.hpp>

#include "meta/vector.hpp"
#include "meta/count.hpp"
#include "meta/all_of_c.hpp"
#include "meta/any_of_c.hpp"

#include "range/core.hpp"
#include "range/equal.hpp"
#include "range/less_lexicographical.hpp"

#include "rime/call_if.hpp"
#include "rime/enable_if_constant.hpp"

#include "../magma.hpp"

namespace math {

/** \struct over
Tag to indicate component types of a composite magma.
*/
template <class ... Components> struct over;

namespace operation {

    namespace tuple_helper {

        /**
        Specialise this for a magma type with an operator() that returns the
        components of the magma as a range.
        */
        template <class TupleTag> struct get_components;

        /**
        Compare a tuple for equality by checking the components in parallel.
        \tparam Equality
            The predicate to use for each component.
        */
        template <class Equality> struct equal_components {
            template <class Left, class Right>
                auto operator() (Left const & left, Right const & right) const
            RETURNS (range::equal (
                get_components <typename magma_tag <Left>::type>() (left),
                get_components <typename magma_tag <Right>::type>() (right),
                Equality()));
        };

        /**
        Compare two tuples for equality, using is_annihilator \<Operation> first.
        If both arguments are annihilators, compare equal.
        If either is an annihilator, compare unequal.
        Otherwise, forward to \a NormalEquality().
        */
        template <class Operation, class NormalEquality>
            class equal_if_annihilator
        {
            struct compare_annihilators {
                template <class Left, class Right>
                auto operator() (Left const & left, Right const & right) const
                RETURNS (math::is_annihilator <Operation> (left) ==
                    math::is_annihilator <Operation> (right));
            };

        public:
            template <class Left, class Right>
            auto operator() (Left const & left, Right const & right) const
            RETURNS (rime::call_if (
                rime::or_ (math::is_annihilator <Operation> (left),
                    math::is_annihilator <Operation> (right)),
                compare_annihilators(), NormalEquality(),
                left, right));
        };

        /**
        Compare two tuples by checking the components in parallel.
        \tparam Compare
            The predicate to use for each component.
        */
        template <class Compare> struct compare_components {
            template <class Left, class Right>
                auto operator() (Left const & left, Right const & right) const
            RETURNS (range::less_lexicographical (
                get_components <typename magma_tag <Left>::type>() (left),
                get_components <typename magma_tag <Right>::type>() (right),
                Compare()));
        };

        /**
        Compare two tuples, ordering annihilators last.
        Annihilators for \a Operation are treated separately.
        Other elements are compared using \a NormalCompare().
        */
        template <class Operation, class NormalCompare>
            class compare_if_annihilator
        {
            // If either left or right is an annihilator:
            // true only if left is not an annihilator (and right is).
            struct not_is_annihilator_left {
                template <class Left, class Right>
                auto operator() (Left const & left, Right const &) const
                RETURNS (!math::is_annihilator <Operation> (left));
            };

        public:
            template <class Left, class Right>
            auto operator() (Left const & left, Right const & right) const
            RETURNS (rime::call_if (
                ! rime::or_ (math::is_annihilator <Operation> (left),
                    math::is_annihilator <Operation> (right)),
                NormalCompare(), not_is_annihilator_left(), left, right));
        };

        /**
        Implement a nullary operation which generates a tuple by calling a
        nullary operation for each element.
        */
        template <class Reassemble, class Operations, class Enable = void>
            struct nullary_operation
        : unimplemented {};

        template <class Reassemble, class ... Operations>
            struct nullary_operation <Reassemble, meta::vector <Operations ...>,
                typename boost::enable_if <meta::all_of_c <
                    is_implemented <Operations>::value ...>>::type>
        : approximate_if <
            meta::any_of_c <is_approximate <Operations>::value ...>>
        {
            typename std::result_of <Reassemble (
                typename std::result_of <Operations()>::type ...)>::type
            operator() () const
            { return Reassemble() (Operations()() ...); }
        };

        /**
        Implement a unary operation which takes the elements of a tuple in
        applies the sequence of operations to each of them, returning the
        results in a new tuple.
        This is implemented iff the operations are all implemented.
        This is marked as approximate iff any of the operations is.

        \tparam Reassemble
            Function class that is passed the new components as arguments.
        \tparam Operations
            Compile-time list of operations that are applied to the elements of
            the tuple.

        \internal
        Indices is a meta::vector of compile-time integrals used by the
        implementation.
        */
        template <class Reassemble, class Operations,
            class Indices =
                typename meta::count <meta::size <Operations>::value>::type,
            class Enable = void>
            struct unary_operation
        : unimplemented {};

        template <class Reassemble, class ... Operations, class ... Indices>
            struct unary_operation <Reassemble,
                meta::vector <Operations ...>, meta::vector <Indices ...>,
                typename boost::enable_if <meta::all_of_c <
                    is_implemented <Operations>::value ...>>::type>
        : approximate_if <
            meta::any_of_c <is_approximate <Operations>::value ...>>
        {
        private:
            // GCC 4.6 crashes if this is a static function instead of a class.
            struct compute {
                /*
                This is a telescoping implementation.
                "compute" is called with the tuple
                Operations is a list of operation functors.
                Indices is a list of compile-time constants 0, 1, 2...
                In the return value this is then expanded as
                Operation0() (components() [0]),
                Operation1() (components() [1]),
                Operation2() (components() [2])
                and so on.
                */
                template <class Components>
                    auto operator() (Components const & components) const
                RETURNS (Reassemble() (
                    Operations() (range::at (components, Indices())) ...));
            };

        public:
            template <class Tuple> auto operator() (Tuple const & tuple) const
            RETURNS (compute() (
                get_components <typename magma_tag <Tuple>::type>() (tuple)));
        };

        /**
        Implement a binary operation which takes the elements of two tuples in
        parallel and applies the sequence of operations to each of them,
        returning the results in a new tuple.
        This is implemented iff the operations are all implemented.
        This is marked as approximate iff any of the operations is.

        \tparam Reassemble
            Function class that is passed the new components as arguments.
        \tparam Operations
            Compile-time list of operations that are applied to the elements of
            the tuples in parallel.

        \internal
        Indices is a meta::vector of compile-time integrals used by the
        implementation.
        */
        template <class Reassemble, class Operations,
            class Indices =
                typename meta::count <meta::size <Operations>::value>::type,
            class Enable = void>
        struct binary_operation
        : unimplemented {};

        template <class Reassemble, class ... Operations, class ... Indices>
            struct binary_operation <Reassemble,
                meta::vector <Operations ...>, meta::vector <Indices ...>,
                typename boost::enable_if <meta::all_of_c <
                    is_implemented <Operations>::value ...>>::type>
        : approximate_if <
            meta::any_of_c <is_approximate <Operations>::value ...>>,
        associative_if <
            meta::all_of_c <is_associative <Operations>::value ...>>,
        commutative_if <
            meta::all_of_c <is_commutative <Operations>::value ...>>,
        idempotent_if <
            meta::all_of_c <is_idempotent <Operations>::value ...>>,
        path_operation_if <
            meta::all_of_c <is_path_operation <Operations>::value ...>>
        {
        private:
            // GCC 4.6 crashes if this is a static function instead of a class.
            struct compute {
                /*
                This implementation is similar to that of unary_operation above,
                but now the left and right components are extracted in parallel.
                */
                template <class LeftComponents, class RightComponents>
                    auto operator() (LeftComponents const & left_components,
                        RightComponents const & right_components) const
                RETURNS (Reassemble() (
                    Operations() (
                        range::at (left_components, Indices()),
                        range::at (right_components, Indices())) ...));
            };

        public:
            template <class Left, class Right> auto operator() (
                Left const & left, Right const & right) const
            RETURNS (compute() (
                get_components <typename magma_tag <Left>::type>() (left),
                get_components <typename magma_tag <Right>::type>() (right)));
        };

        /**
        Implement an operation that takes a stream and tuple, and outputs the
        elements of the tuple to the stream.
        Useful for implementing "print".
        */
        template <class ComponentTags, class Enable = void>
            class print_components
        : unimplemented {};

        template <class ... ComponentTags>
            class print_components <meta::vector <ComponentTags ...>, typename
                boost::enable_if <meta::all_of_c <
                    is_implemented <operation::print <ComponentTags>>::value ...
                >>::type>
        {
            // Known to be empty.
            template <class Stream, class Components, class Enable = typename
                rime::enable_if_constant_true <typename
                    range::result_of <range::callable::empty (Components)>::type
                >::type>
            void print_elements (Stream &, Components const &, bool) const {}

            // Not known to be empty at compile time.
            template <class Stream, class Components, class Enable = typename
                rime::disable_if_constant_true <typename
                    range::result_of <range::callable::empty (Components)>::type
                >::type>
            void print_elements (
                Stream & stream, Components const & components, bool first,
                void * = 0) const
            {
                if (!range::empty (components)) {
                    if (!first)
                        stream << ", ";
                    math::print (stream, range::first (components));
                    print_elements (stream, range::drop (components), false);
                }
            }

        public:
            template <class Stream, class Tuple>
                void operator() (Stream & stream, Tuple const & tuple) const
            {
                stream << '(';
                print_elements (stream, tuple.components(), true);
                stream << ')';
            }
        };

    } // namespace tuple_helper

} // namespace operation

namespace tuple_helper {

    /**
    Evaluate to \c true iff the two meta::vector's of components are of the
    same length, and the types in OtherComponents are explicitly convertible
    from the types in Components, but at least one is not implicitly
    convertible.
    This is useful for providing two overloads of a constructor, one of which
    is implicit and is only available if the conversion of all components is
    implicit, and one which is explicit and only available if the conversion
    is explicit for at least one component.
    */
    template <class Components, class OtherComponents>
        struct components_constructible_only
    : boost::mpl::and_ <
        utility::are_constructible <Components, OtherComponents>,
        boost::mpl::not_ <
            utility::are_convertible <OtherComponents, Components>>>
    {};

} // namespace tuple_helper

} // namespace math

#endif // MATH_DETAIL_TUPLE_HELPER_HPP_INCLUDED


#ifndef RESORTER_HPP
#define RESORTER_HPP

#include "storage/DiskVectorLMDB.hpp"
#include "storage/DiskVector.hpp"
#include "utils.hpp"
#include "config.hpp"
#include <Eigen/Dense>
#include <algorithm>
#include <cmath>

#define MAX_RESORT_BATCH_SIZE 10000 // resort these many at a time
// Similarity Metrics
#define SIM_METRIC_COSINE 1
#define SIM_METRIC_EUCLIDEAN 2

class Resorter {
public:
  float static computeDot(vector<float> a, vector<float> b) {
    float ans = 0;
    for (auto it = a.begin(), it2 = b.begin();
        it != a.end() && it2 != b.end(); it++, it2++) {
      ans += (*it) * (*it2);
    }
    return ans;
  }

  /**
   * When one DiskVector has all the features
   */
  void static resort(const unordered_set<long long int>& matches,
      const std::shared_ptr<DiskVector<vector<float>>>& feats,
      vector<float>& qfeat,
      vector<pair<float, long long int>>& res) {
    res.clear();
    if (matches.size() == 0) {
      cerr << "0 matches input to resorter..";
      return;
    }
    #if NORMALIZE_FEATS == 1
      L2Normalize(qfeat); // no longer required as storing normalized
    #endif
    Eigen::MatrixXf qfeat_mat = Eigen::VectorXf::Map(&qfeat[0], qfeat.size());

    // Batch process the scoring
    int nMatches = matches.size();
    int nBatches = ceil(nMatches * 1.0f / MAX_RESORT_BATCH_SIZE);
    auto match = matches.begin();
    vector<vector<float>> feats_vec;
    vector<pair<float, long long int>> res_batch;

    for (int batch = 0; batch < nBatches; batch++) {
      feats_vec.clear();
      res_batch.clear();
      int batchSize = 0;
      for (; match != matches.end() && batchSize < MAX_RESORT_BATCH_SIZE;
          match++, batchSize++) {
        // TODO: Avoid this, use pre-alloc of memory
        vector<float> temp;
        feats->Get(*match, temp);
        #if NORMALIZE_FEATS == 1
          L2Normalize(temp);
        #endif
        feats_vec.push_back(temp);
        // for output
        res_batch.push_back(make_pair(0.0f, *match));
      }
      cout << "read all"; cout.flush();
      Eigen::MatrixXf feats_mat(matches.size(), qfeat.size());
      for (int i = 0; i < feats_vec.size(); i++) {
        feats_mat.row(i) = Eigen::VectorXf::Map(&feats_vec[i][0], feats_vec[i].size());
      }
      Eigen::MatrixXf cos_scores = qfeat_mat.transpose() * feats_mat.transpose();
      cout << "rescored"; cout.flush();

      for (int i = 0; i < res_batch.size(); i++) {
        res_batch[i].first = cos_scores(i);
      }
      res.insert(res.end(), res_batch.begin(), res_batch.end());
    }
    sort(res.begin(), res.end());
    reverse(res.begin(), res.end());
  }

  /**
   * Multicore reranking
   */
  void static resort_multicore(long long int queryidx,
      const std::shared_ptr<DiskVectorLMDB<vector<float>>>& feats,
      vector<pair<float, long long int>>& res,
      int nMatches,
      int simMetric = SIM_METRIC_COSINE) {
    res.clear();

    cout << "NORMALIZE_FEATS: " << NORMALIZE_FEATS << endl;
    vector<float> qfeat;
    if (!feats->Get((queryidx + 1) * 10000 + 1, qfeat)) {
        cout << "Wrong " << queryidx << endl;
    } else {
      #if NORMALIZE_FEATS == 1
        L2Normalize(qfeat);
      #endif
      Eigen::MatrixXf qfeat_mat = Eigen::VectorXf::Map(&qfeat[0], qfeat.size());
      qfeat_mat.normalize();

      vector<float> scores(nMatches, 0);

      #pragma omp parallel for shared(scores) schedule(dynamic, 1)
      for (int i = 0; i < nMatches; i++) {
        vector<float> temp;
        if (!feats->Get((i + 1) * 10000 + 1, temp)) {
          scores[i] = 0;
          continue;
        }
        #if NORMALIZE_FEATS == 1
          L2Normalize(temp);
        #endif
        Eigen::MatrixXf match_mat = Eigen::VectorXf::Map(&temp[0], temp.size());
        if (simMetric == SIM_METRIC_COSINE) {
          Eigen::MatrixXf sim = qfeat_mat.transpose() * match_mat;
          scores[i] = sim(0);
        } else if (simMetric == SIM_METRIC_EUCLIDEAN) {
          match_mat.normalize();
          // since I need similarity, and 2 is the max distance between
          // any 2 unit vectors under any metric distance (triangle ineq)
          scores[i] = 2.0f - (qfeat_mat - match_mat).norm();
          if (scores[i] < 0) {

          }
        } else {
          cerr << "Distance metric not implemented!" << endl;
          scores[i] = -1;
        }
      }

      for (int i = 0; i < nMatches; i++) {
        res.push_back(make_pair(scores[i], i + 1));
      }

    }
  }

  /**
   * A one-off function to do nxn similarity for all features in an image
   */
  void static computePairwiseSim(
      const std::shared_ptr<DiskVectorLMDB<vector<float>>>& feats,
      int imgid, // 1-indexed
      int numFeat, // num of features in this image
      Eigen::MatrixXf& simsOutput) {
    vector<vector<float>> data;
    for (int i = 1; i <= numFeat; i++) {
      int featid = computeFeatId(imgid, i);
      vector<float> temp;
      if (!feats->Get(featid, temp)) {
        cerr << "Couldn't read featid = " << featid << endl;
        continue;
      }
      data.push_back(temp);
    }
    if (data.size() == 0) return;
    Eigen::MatrixXf featmat(numFeat, data[0].size());
    for (int i =0; i < numFeat; i++) {
      featmat.row(i) = Eigen::VectorXf::Map(&data[i][0], data[i].size());
    }
    simsOutput = featmat * featmat.transpose();
  }

  /**
   * When multiple DiskVector have the features
   */
  // NOT BEING USED currently. TODO: remove it
  void static resort2(const vector<pair<int, int>>& matches,
      const vector<boost::shared_ptr<DiskVector<vector<float>>>>& featstor,
      const vector<float>& qfeat,
      vector<pair<float, pair<int, int>>>& res) {
    res.clear();
    Eigen::MatrixXf qfeat_mat = Eigen::VectorXf::Map(&qfeat[0], qfeat.size());

    // Batch process the scoring
    int nMatches = matches.size();
    int nBatches = ceil(nMatches * 1.0f / MAX_RESORT_BATCH_SIZE);
    auto match = matches.begin();
    vector<vector<float>> feats_vec;
    vector<pair<float, pair<int, int>>> res_batch;

    for (int batch = 0; batch < nBatches; batch++) {
      feats_vec.clear();
      res_batch.clear();
      int batchSize = 0;
      for (; match != matches.end() && batchSize < MAX_RESORT_BATCH_SIZE;
          match++, batchSize++) {
        // TODO: Avoid this, use pre-alloc of memory
        vector<float> temp;
        featstor[match->first]->Get(match->second, temp);

        L2Normalize(temp);
        feats_vec.push_back(temp);
        // for output
        res_batch.push_back(make_pair(0.0f, *match));
      }
      Eigen::MatrixXf feats_mat(matches.size(), qfeat.size());
      for (int i = 0; i < feats_vec.size(); i++) {
        feats_mat.row(i) = Eigen::VectorXf::Map(&feats_vec[i][0], feats_vec[i].size());
      }
      Eigen::MatrixXf cos_scores = qfeat_mat.transpose() * feats_mat.transpose();

      for (int i = 0; i < res_batch.size(); i++) {
        res_batch[i].first = cos_scores(i);
      }
      res.insert(res.end(), res_batch.begin(), res_batch.end());
    }
    sort(res.begin(), res.end());
    reverse(res.begin(), res.end());
  }

};

#endif


#ifndef INDEX_H
#define INDEX_H

#include <math.h>
#include <iostream>
#include <vector>
#include <chrono>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <tbb/scalable_allocator.h>
#include <simdpp/simd.h>

#ifdef INCLUDE_OPENCL
#include <opencl/cl.hpp>
#endif

#include "integrity_violation.hpp"
#include "bitmask.hpp"

// @note: vector type used for representing GPU floating point vector
typedef boost::numeric::ublas::vector< float > blasvector;
// @note: vector type used for representing GPU mask
typedef boost::numeric::ublas::vector< bitblock > blasmask;

// Container used to store prefix sums of vectors which help accelerate our calculations
// multiple vectors are stored in this container so that ranges don't need to be recomputed for each vector
class Index {
public:
    static void precompute();
    void benchmark() const;

    Index();
    // @param source: vector of floating points to sum over (efficiently)
    Index(std::vector< std::vector< float > > const & source);
    ~Index();

    // @param indicator: mask of bits indicating which elements are relevant to the vector sum
    // @returns the total of all elements associated to bits that were set to 1
    void sum(Bitmask const & indicator, float * accumulator) const;

    // @returns string representation of original floating points (used for inspection)
    std::string to_string() const;

private:
    // Copy of the original floating points
    std::vector< float > source;
    // precomputed representation of the floating point vector
    std::vector< std::vector< float > > prefixes;
    // Number of floating points represented in the source vector
    unsigned int size;
    unsigned int width;
    // Threshold of queries beyond which a parallel sum is executed
    unsigned int parallel_threshold;
    // Number of blocks expected in bitmask
    unsigned int num_blocks;

    // Initialize the OpenCL implementation to perform our sum in parallel
    void initialize_kernel();

    // @param indicator: array of blocks of bits indicating which elements are relevant to the vector sum
    // @returns the total of all elements associated to bits that were set to 1
    // @note: This implementation uses look-up to precomputed values of the run-length-code for fast sums
    void block_sequential_sum(bitblock * blocks, float * accumulator) const;
    void block_sequential_sum(rangeblock block, unsigned int offset, float * accumulator) const;

    // @param indicator: array of blocks of bits indicating which elements are relevant to the vector sum
    // @returns the total of all elements associated to bits that were set to 1
    // @note: This implementation computes run-length-code for fast sums
    void bit_sequential_sum(Bitmask const &indicator, float *accumulator) const;

    // @param indicator: array of blocks of bits indicating which elements are relevant to the vector sum
    // @returns the total of all elements associated to bits that were set to 1
    // @note this implementation uses OpenCL kernels to sum in parallel
    void parallel_sum(bitblock * blocks, float * accumulator, bool blocking = true, bool profile = false) const;

    // @param source: The original vector of floats used in computation
    // @modifies prefixes: writes the prefix sums into this vector
    void build_prefixes(std::vector< std::vector< float > > const & source, std::vector< std::vector< float > > & prefixes);
#ifdef INCLUDE_OPENCL
    static cl::Device device;
    static cl::Platform platform;
    static cl::Context context;
    static cl::Program program;
    static cl::CommandQueue queue;
    static unsigned int group_size;

    cl::Buffer data_buffer;

    void initialize_driver(void);

    void set_platform(int index = 0);
    void set_device(cl::Platform platform, int index, bool display = false);
#endif
};
#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2016 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "min_quad_with_fixed.h"

#include "slice.h"
#include "is_symmetric.h"
#include "find.h"
#include "sparse.h"
#include "repmat.h"
#include "EPS.h"
#include "cat.h"

//#include <Eigen/SparseExtra>
// Bug in unsupported/Eigen/SparseExtra needs iostream first
#include <iostream>
#include <unsupported/Eigen/SparseExtra>
#include <cassert>
#include <cstdio>
#include <iostream>

template <typename T, typename Derivedknown>
IGL_INLINE bool igl::min_quad_with_fixed_precompute(
  const Eigen::SparseMatrix<T>& A2,
  const Eigen::MatrixBase<Derivedknown> & known,
  const Eigen::SparseMatrix<T>& Aeq,
  const bool pd,
  min_quad_with_fixed_data<T> & data
  )
{
//#define MIN_QUAD_WITH_FIXED_CPP_DEBUG
  using namespace Eigen;
  using namespace std;
  const Eigen::SparseMatrix<T> A = 0.5*A2;
#ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG
  cout<<"    pre"<<endl;
#endif
  // number of rows
  int n = A.rows();
  // cache problem size
  data.n = n;

  int neq = Aeq.rows();
  // default is to have 0 linear equality constraints
  if(Aeq.size() != 0)
  {
    assert(n == Aeq.cols() && "#Aeq.cols() should match A.rows()");
  }

  assert(A.rows() == n && "A should be square");
  assert(A.cols() == n && "A should be square");

  // number of known rows
  int kr = known.size();

  assert((kr == 0 || known.minCoeff() >= 0)&& "known indices should be in [0,n)");
  assert((kr == 0 || known.maxCoeff() < n) && "known indices should be in [0,n)");
  assert(neq <= n && "Number of equality constraints should be less than DOFs");

  // cache known
  data.known = known;
  // get list of unknown indices
  data.unknown.resize(n-kr);
  std::vector<bool> unknown_mask;
  unknown_mask.resize(n,true);
  for(int i = 0;i<kr;i++)
  {
    unknown_mask[known(i)] = false;
  }
  int u = 0;
  for(int i = 0;i<n;i++)
  {
    if(unknown_mask[i])
    {
      data.unknown(u) = i;
      u++;
    }
  }
  // get list of lagrange multiplier indices
  data.lagrange.resize(neq);
  for(int i = 0;i<neq;i++)
  {
    data.lagrange(i) = n + i;
  }
  // cache unknown followed by lagrange indices
  data.unknown_lagrange.resize(data.unknown.size()+data.lagrange.size());
  // Would like to do:
  //data.unknown_lagrange << data.unknown, data.lagrange;
  // but Eigen can't handle empty vectors in comma initialization
  // https://forum.kde.org/viewtopic.php?f=74&t=107974&p=364947#p364947
  if(data.unknown.size() > 0)
  {
    data.unknown_lagrange.head(data.unknown.size()) = data.unknown;
  }
  if(data.lagrange.size() > 0)
  {
    data.unknown_lagrange.tail(data.lagrange.size()) = data.lagrange;
  }

  SparseMatrix<T> Auu;
  slice(A,data.unknown,data.unknown,Auu);
  assert(Auu.size() != 0 && Auu.rows() > 0 && "There should be at least one unknown.");

  // Positive definiteness is *not* determined, rather it is given as a
  // parameter
  data.Auu_pd = pd;
  if(data.Auu_pd)
  {
    // PD implies symmetric
    data.Auu_sym = true;
    // This is an annoying assertion unless EPS can be chosen in a nicer way.
    //assert(is_symmetric(Auu,EPS<double>()));
    assert(is_symmetric(Auu,1.0) &&
      "Auu should be symmetric if positive definite");
  }else
  {
    // determine if A(unknown,unknown) is symmetric and/or positive definite
    VectorXi AuuI,AuuJ;
    MatrixXd AuuV;
    find(Auu,AuuI,AuuJ,AuuV);
    data.Auu_sym = is_symmetric(Auu,EPS<double>()*AuuV.maxCoeff());
  }

  // Determine number of linearly independent constraints
  int nc = 0;
  if(neq>0)
  {
#ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG
    cout<<"    qr"<<endl;
#endif
    // QR decomposition to determine row rank in Aequ
    slice(Aeq,data.unknown,2,data.Aequ);
    assert(data.Aequ.rows() == neq &&
      "#Rows in Aequ should match #constraints");
    assert(data.Aequ.cols() == data.unknown.size() &&
      "#cols in Aequ should match #unknowns");
    data.AeqTQR.compute(data.Aequ.transpose().eval());
#ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG
    //cout<<endl<<matlab_format(SparseMatrix<T>(data.Aequ.transpose().eval()),"AeqT")<<endl<<endl;
#endif
    switch(data.AeqTQR.info())
    {
      case Eigen::Success:
        break;
      case Eigen::NumericalIssue:
        cerr<<"Error: Numerical issue."<<endl;
        return false;
      case Eigen::InvalidInput:
        cerr<<"Error: Invalid input."<<endl;
        return false;
      default:
        cerr<<"Error: Other."<<endl;
        return false;
    }
    nc = data.AeqTQR.rank();
    assert(nc<=neq &&
      "Rank of reduced constraints should be <= #original constraints");
    data.Aeq_li = nc == neq;
    //cout<<"data.Aeq_li: "<<data.Aeq_li<<endl;
  }else
  {
    data.Aeq_li = true;
  }

  if(data.Aeq_li)
  {
#ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG
    cout<<"    Aeq_li=true"<<endl;
#endif
    // Append lagrange multiplier quadratic terms
    SparseMatrix<T> new_A;
    SparseMatrix<T> AeqT = Aeq.transpose();
    SparseMatrix<T> Z(neq,neq);
    // This is a bit slower. But why isn't cat fast?
    new_A = cat(1, cat(2,   A, AeqT ),
                   cat(2, Aeq,    Z ));

    // precompute RHS builders
    if(kr > 0)
    {
      SparseMatrix<T> Aulk,Akul;
      // Slow
      slice(new_A,data.unknown_lagrange,data.known,Aulk);
      //// This doesn't work!!!
      //data.preY = Aulk + Akul.transpose();
      // Slow
      if(data.Auu_sym)
      {
        data.preY = Aulk*2;
      }else
      {
        slice(new_A,data.known,data.unknown_lagrange,Akul);
        SparseMatrix<T> AkulT = Akul.transpose();
        data.preY = Aulk + AkulT;
      }
    }else
    {
      data.preY.resize(data.unknown_lagrange.size(),0);
    }

    // Positive definite and no equality constraints (Positive definiteness
    // implies symmetric)
#ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG
    cout<<"    factorize"<<endl;
#endif
    if(data.Auu_pd && neq == 0)
    {
#ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG
    cout<<"    llt"<<endl;
#endif
      data.llt.compute(Auu);
      switch(data.llt.info())
      {
        case Eigen::Success:
          break;
        case Eigen::NumericalIssue:
          cerr<<"Error: Numerical issue."<<endl;
          return false;
        default:
          cerr<<"Error: Other."<<endl;
          return false;
      }
      data.solver_type = min_quad_with_fixed_data<T>::LLT;
    }else
    {
#ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG
    cout<<"    ldlt"<<endl;
#endif
      // Either not PD or there are equality constraints
      SparseMatrix<T> NA;
      slice(new_A,data.unknown_lagrange,data.unknown_lagrange,NA);
      data.NA = NA;
      // Ideally we'd use LDLT but Eigen doesn't support positive semi-definite
      // matrices:
      // http://forum.kde.org/viewtopic.php?f=74&t=106962&p=291990#p291990
      if(data.Auu_sym && false)
      {
        data.ldlt.compute(NA);
        switch(data.ldlt.info())
        {
          case Eigen::Success:
            break;
          case Eigen::NumericalIssue:
            cerr<<"Error: Numerical issue."<<endl;
            return false;
          default:
            cerr<<"Error: Other."<<endl;
            return false;
        }
        data.solver_type = min_quad_with_fixed_data<T>::LDLT;
      }else
      {
#ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG
    cout<<"    lu"<<endl;
#endif
        // Resort to LU
        // Bottleneck >1/2
        data.lu.compute(NA);
        //std::cout<<"NA=["<<std::endl<<NA<<std::endl<<"];"<<std::endl;
        switch(data.lu.info())
        {
          case Eigen::Success:
            break;
          case Eigen::NumericalIssue:
            cerr<<"Error: Numerical issue."<<endl;
            return false;
          case Eigen::InvalidInput:
            cerr<<"Error: Invalid Input."<<endl;
            return false;
          default:
            cerr<<"Error: Other."<<endl;
            return false;
        }
        data.solver_type = min_quad_with_fixed_data<T>::LU;
      }
    }
  }else
  {
#ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG
    cout<<"    Aeq_li=false"<<endl;
#endif
    data.neq = neq;
    const int nu = data.unknown.size();
    //cout<<"nu: "<<nu<<endl;
    //cout<<"neq: "<<neq<<endl;
    //cout<<"nc: "<<nc<<endl;
    //cout<<"    matrixR"<<endl;
    SparseMatrix<T> AeqTR,AeqTQ;
    AeqTR = data.AeqTQR.matrixR();
    // This shouldn't be necessary
    AeqTR.prune(static_cast<T>(0.0));
#ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG
    cout<<"    matrixQ"<<endl;
#endif
    // THIS IS ESSENTIALLY DENSE AND THIS IS BY FAR THE BOTTLENECK
    // http://forum.kde.org/viewtopic.php?f=74&t=117500
    AeqTQ = data.AeqTQR.matrixQ();
#ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG
    cout<<"    prune"<<endl;
    cout<<"      nnz: "<<AeqTQ.nonZeros()<<endl;
#endif
    // This shouldn't be necessary
    AeqTQ.prune(static_cast<T>(0.0));
    //cout<<"AeqTQ: "<<AeqTQ.rows()<<" "<<AeqTQ.cols()<<endl;
    //cout<<matlab_format(AeqTQ,"AeqTQ")<<endl;
    //cout<<"    perms"<<endl;
#ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG
    cout<<"      nnz: "<<AeqTQ.nonZeros()<<endl;
    cout<<"    perm"<<endl;
#endif
    SparseMatrix<T> I(neq,neq);
    I.setIdentity();
    data.AeqTE = data.AeqTQR.colsPermutation() * I;
    data.AeqTET = data.AeqTQR.colsPermutation().transpose() * I;
    assert(AeqTR.rows() == nu   && "#rows in AeqTR should match #unknowns");
    assert(AeqTR.cols() == neq  && "#cols in AeqTR should match #constraints");
    assert(AeqTQ.rows() == nu && "#rows in AeqTQ should match #unknowns");
    assert(AeqTQ.cols() == nu && "#cols in AeqTQ should match #unknowns");
    //cout<<"    slice"<<endl;
#ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG
    cout<<"    slice"<<endl;
#endif
    data.AeqTQ1 = AeqTQ.topLeftCorner(nu,nc);
    data.AeqTQ1T = data.AeqTQ1.transpose().eval();
    // ALREADY TRIM (Not 100% sure about this)
    data.AeqTR1 = AeqTR.topLeftCorner(nc,nc);
    data.AeqTR1T = data.AeqTR1.transpose().eval();
    //cout<<"AeqTR1T.size() "<<data.AeqTR1T.rows()<<" "<<data.AeqTR1T.cols()<<endl;
    // Null space
    data.AeqTQ2 = AeqTQ.bottomRightCorner(nu,nu-nc);
    data.AeqTQ2T = data.AeqTQ2.transpose().eval();
#ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG
    cout<<"    proj"<<endl;
#endif
    // Projected hessian
    SparseMatrix<T> QRAuu = data.AeqTQ2T * Auu * data.AeqTQ2;
    {
#ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG
      cout<<"    factorize"<<endl;
#endif
      // QRAuu should always be PD
      data.llt.compute(QRAuu);
      switch(data.llt.info())
      {
        case Eigen::Success:
          break;
        case Eigen::NumericalIssue:
          cerr<<"Error: Numerical issue."<<endl;
          return false;
        default:
          cerr<<"Error: Other."<<endl;
          return false;
      }
      data.solver_type = min_quad_with_fixed_data<T>::QR_LLT;
    }
#ifdef MIN_QUAD_WITH_FIXED_CPP_DEBUG
    cout<<"    smash"<<endl;
#endif
    // Known value multiplier
    SparseMatrix<T> Auk;
    slice(A,data.unknown,data.known,Auk);
    SparseMatrix<T> Aku;
    slice(A,data.known,data.unknown,Aku);
    SparseMatrix<T> AkuT = Aku.transpose();
    data.preY = Auk + AkuT;
    // Needed during solve
    data.Auu = Auu;
    slice(Aeq,data.known,2,data.Aeqk);
    assert(data.Aeqk.rows() == neq);
    assert(data.Aeqk.cols() == data.known.size());
  }
  return true;
}

template <
  typename T,
  typename DerivedB,
  typename DerivedY,
  typename DerivedBeq,
  typename DerivedZ,
  typename Derivedsol>
IGL_INLINE bool igl::min_quad_with_fixed_solve(
  const min_quad_with_fixed_data<T> & data,
  const Eigen::MatrixBase<DerivedB> & B,
  const Eigen::MatrixBase<DerivedY> & Y,
  const Eigen::MatrixBase<DerivedBeq> & Beq,
  Eigen::PlainObjectBase<DerivedZ> & Z,
  Eigen::PlainObjectBase<Derivedsol> & sol)
{
  using namespace std;
  using namespace Eigen;
  typedef Matrix<T,Dynamic,1> VectorXT;
  typedef Matrix<T,Dynamic,Dynamic> MatrixXT;
  // number of known rows
  int kr = data.known.size();
  if(kr!=0)
  {
    assert(kr == Y.rows());
  }
  // number of columns to solve
  int cols = Y.cols();
  assert(B.cols() == 1 || B.cols() == cols);
  assert(Beq.size() == 0 || Beq.cols() == 1 || Beq.cols() == cols);

  // resize output
  Z.resize(data.n,cols);
  // Set known values
  for(int i = 0;i < kr;i++)
  {
    for(int j = 0;j < cols;j++)
    {
      Z(data.known(i),j) = Y(i,j);
    }
  }

  if(data.Aeq_li)
  {
    // number of lagrange multipliers aka linear equality constraints
    int neq = data.lagrange.size();
    // append lagrange multiplier rhs's
    MatrixXT BBeq(B.rows() + Beq.rows(),cols);
    if(B.size() > 0)
    {
      BBeq.topLeftCorner(B.rows(),cols) = B.replicate(1,B.cols()==cols?1:cols);
    }
    if(Beq.size() > 0)
    {
      BBeq.bottomLeftCorner(Beq.rows(),cols) = -2.0*Beq.replicate(1,Beq.cols()==cols?1:cols);
    }

    // Build right hand side
    MatrixXT BBequlcols;
    igl::slice(BBeq,data.unknown_lagrange,1,BBequlcols);
    MatrixXT NB;
    if(kr == 0)
    {
      NB = BBequlcols;
    }else
    {
      NB = data.preY * Y + BBequlcols;
    }

    //std::cout<<"NB=["<<std::endl<<NB<<std::endl<<"];"<<std::endl;
    //cout<<matlab_format(NB,"NB")<<endl;
    switch(data.solver_type)
    {
      case igl::min_quad_with_fixed_data<T>::LLT:
        sol = data.llt.solve(NB);
        break;
      case igl::min_quad_with_fixed_data<T>::LDLT:
        sol = data.ldlt.solve(NB);
        break;
      case igl::min_quad_with_fixed_data<T>::LU:
        // Not a bottleneck
        sol = data.lu.solve(NB);
        break;
      default:
        cerr<<"Error: invalid solver type"<<endl;
        return false;
    }
    //std::cout<<"sol=["<<std::endl<<sol<<std::endl<<"];"<<std::endl;
    // Now sol contains sol/-0.5
    sol *= -0.5;
    // Now sol contains solution
    // Place solution in Z
    for(int i = 0;i<(sol.rows()-neq);i++)
    {
      for(int j = 0;j<sol.cols();j++)
      {
        Z(data.unknown_lagrange(i),j) = sol(i,j);
      }
    }
  }else
  {
    assert(data.solver_type == min_quad_with_fixed_data<T>::QR_LLT);
    MatrixXT eff_Beq;
    // Adjust Aeq rhs to include known parts
    eff_Beq =
      //data.AeqTQR.colsPermutation().transpose() * (-data.Aeqk * Y + Beq);
      data.AeqTET * (-data.Aeqk * Y + Beq.replicate(1,Beq.cols()==cols?1:cols));
    // Where did this -0.5 come from? Probably the same place as above.
    MatrixXT Bu;
    slice(B,data.unknown,1,Bu);
    MatrixXT NB;
    NB = -0.5*(Bu.replicate(1,B.cols()==cols?1:cols) + data.preY * Y);
    // Trim eff_Beq
    const int nc = data.AeqTQR.rank();
    const int neq = Beq.rows();
    eff_Beq = eff_Beq.topLeftCorner(nc,cols).eval();
    data.AeqTR1T.template triangularView<Lower>().solveInPlace(eff_Beq);
    // Now eff_Beq = (data.AeqTR1T \ (data.AeqTET * (-data.Aeqk * Y + Beq)))
    MatrixXT lambda_0;
    lambda_0 = data.AeqTQ1 * eff_Beq;
    //cout<<matlab_format(lambda_0,"lambda_0")<<endl;
    MatrixXT QRB;
    QRB = -data.AeqTQ2T * (data.Auu * lambda_0) + data.AeqTQ2T * NB;
    Derivedsol lambda;
    lambda = data.llt.solve(QRB);
    // prepare output
    Derivedsol solu;
    solu = data.AeqTQ2 * lambda + lambda_0;
    //  http://www.math.uh.edu/~rohop/fall_06/Chapter3.pdf
    Derivedsol solLambda;
    {
      Derivedsol temp1,temp2;
      temp1 = (data.AeqTQ1T * NB - data.AeqTQ1T * data.Auu * solu);
      data.AeqTR1.template triangularView<Upper>().solveInPlace(temp1);
      //cout<<matlab_format(temp1,"temp1")<<endl;
      temp2 = Derivedsol::Zero(neq,cols);
      temp2.topLeftCorner(nc,cols) = temp1;
      //solLambda = data.AeqTQR.colsPermutation() * temp2;
      solLambda = data.AeqTE * temp2;
    }
    // sol is [Z(unknown);Lambda]
    assert(data.unknown.size() == solu.rows());
    assert(cols == solu.cols());
    assert(data.neq == neq);
    assert(data.neq == solLambda.rows());
    assert(cols == solLambda.cols());
    sol.resize(data.unknown.size()+data.neq,cols);
    sol.block(0,0,solu.rows(),solu.cols()) = solu;
    sol.block(solu.rows(),0,solLambda.rows(),solLambda.cols()) = solLambda;
    for(int u = 0;u<data.unknown.size();u++)
    {
      for(int j = 0;j<Z.cols();j++)
      {
        Z(data.unknown(u),j) = solu(u,j);
      }
    }
  }
  return true;
}

template <
  typename T,
  typename DerivedB,
  typename DerivedY,
  typename DerivedBeq,
  typename DerivedZ>
IGL_INLINE bool igl::min_quad_with_fixed_solve(
  const min_quad_with_fixed_data<T> & data,
  const Eigen::MatrixBase<DerivedB> & B,
  const Eigen::MatrixBase<DerivedY> & Y,
  const Eigen::MatrixBase<DerivedBeq> & Beq,
  Eigen::PlainObjectBase<DerivedZ> & Z)
{
  Eigen::Matrix<typename DerivedZ::Scalar, Eigen::Dynamic, Eigen::Dynamic> sol;
  return min_quad_with_fixed_solve(data,B,Y,Beq,Z,sol);
}

template <
  typename T,
  typename Derivedknown,
  typename DerivedB,
  typename DerivedY,
  typename DerivedBeq,
  typename DerivedZ>
IGL_INLINE bool igl::min_quad_with_fixed(
  const Eigen::SparseMatrix<T>& A,
  const Eigen::MatrixBase<DerivedB> & B,
  const Eigen::MatrixBase<Derivedknown> & known,
  const Eigen::MatrixBase<DerivedY> & Y,
  const Eigen::SparseMatrix<T>& Aeq,
  const Eigen::MatrixBase<DerivedBeq> & Beq,
  const bool pd,
  Eigen::PlainObjectBase<DerivedZ> & Z)
{
  min_quad_with_fixed_data<T> data;
  if(!min_quad_with_fixed_precompute(A,known,Aeq,pd,data))
  {
    return false;
  }
  return min_quad_with_fixed_solve(data,B,Y,Beq,Z);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
#if EIGEN_VERSION_AT_LEAST(3,3,0)
#else
template bool igl::min_quad_with_fixed_solve<double, Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<double>, Eigen::Matrix<double, -1, 1, 0, -1, 1> const>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(igl::min_quad_with_fixed_data<double> const&, Eigen::MatrixBase<Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<double>, Eigen::Matrix<double, -1, 1, 0, -1, 1> const> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
template bool igl::min_quad_with_fixed_solve<double, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(igl::min_quad_with_fixed_data<double> const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, Eigen::Matrix<double, -1, 1, 0, -1, 1> > > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
#endif
template bool igl::min_quad_with_fixed<double, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::SparseMatrix<double, 0, int> const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::SparseMatrix<double, 0, int> const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, bool, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template bool igl::min_quad_with_fixed_solve<double, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(igl::min_quad_with_fixed_data<double> const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
template bool igl::min_quad_with_fixed_precompute<double, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::SparseMatrix<double, 0, int> const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::SparseMatrix<double, 0, int> const&, bool, igl::min_quad_with_fixed_data<double>&);
template bool igl::min_quad_with_fixed_solve<double, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(igl::min_quad_with_fixed_data<double> const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
template bool igl::min_quad_with_fixed_solve<double, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(igl::min_quad_with_fixed_data<double> const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template bool igl::min_quad_with_fixed_precompute<double, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::SparseMatrix<double, 0, int> const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::SparseMatrix<double, 0, int> const&, bool, igl::min_quad_with_fixed_data<double>&);
template bool igl::min_quad_with_fixed_solve<double, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(igl::min_quad_with_fixed_data<double> const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template bool igl::min_quad_with_fixed_solve<double, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(igl::min_quad_with_fixed_data<double> const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template bool igl::min_quad_with_fixed_solve<double, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(igl::min_quad_with_fixed_data<double> const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template bool igl::min_quad_with_fixed_solve<double, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(igl::min_quad_with_fixed_data<double> const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template bool igl::min_quad_with_fixed<double, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::SparseMatrix<double, 0, int> const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::SparseMatrix<double, 0, int> const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, bool, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
#endif


/**
* @Author: Marcel Ruhf <marcelruhf>
* @Email:  m.ruhf@protonmail.ch
* @Project: SoccerRL
* @Filename: RobotTracker.cpp
* @License: Licensed under the Apache 2.0 license (see LICENSE.md)
* @Copyright: Copyright (c) 2017 Marcel Ruhf
*/

#include <iostream>
#include <vector>
#include <tuple>
#include <boost/optional.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include "MarkerFinder.hpp"
#include "RobotTracker.hpp"
#include "Constants.hpp"

namespace mr
{
    void RobotTracker::preprocess(cv::Mat image)
    {
        marker.preprocess(image);

        // Determine marker vertices, rotation and translation vectors...
        markerCorners = marker.getCorners(ROBOT_MARKER_ID);
        rvec = marker.getRotationVector(ROBOT_MARKER_ID);
        tvec = marker.getTranslationVector(ROBOT_MARKER_ID);
    }

    boost::optional<cv::Vec3d> RobotTracker::getRotationVector()
    {
        return rvec;
    }

    boost::optional<cv::Vec3d> RobotTracker::getTranslationVector()
    {
        return tvec;
    }

    boost::optional<cv::Point2f> RobotTracker::getCentrePoint()
    {
        if (!markerCorners)
        {
            return boost::optional<cv::Point2f>{};
        }

        // Compute Centroid of the robot...
        cv::Moments mu              = cv::moments(*markerCorners, true);
        cv::Point2f robot_centroid  = cv::Point2f(mu.m10/mu.m00, mu.m01/mu.m00);

        return robot_centroid;
    }
}  // namespace mr


#ifndef __GRAPH_H__
#define __GRAPH_H__

#include <stdio.h>
#include <iostream>
#include <fstream>
#include <map>
#include <string>
#include <set>
#include <vector>
#include <cstdlib>
#include <boost/algorithm/string.hpp>
#include <boost/bimap.hpp>

class Graph
{

public:
	unsigned num_vertex;
	unsigned num_edge;

	unsigned *R;
	unsigned *C;
	unsigned *F;

	boost::bimap<unsigned,std::string> IDs;

	Graph()
	{

	}

	~Graph()
	{
		delete R;
		delete C;
		delete F;
	}

	void parse_edgelist(char *file);

	void print_R();

	void print_number_of_isolated_vertices();

	void print_CSR();

	void print_high_degree_vertices();

	void print_adjacency_list();

	void print_numerical_edge_file(char *outfile);

	void print_BC_scores(const std::vector<float> bc, char *outfile);

};

#endif // __GRAPH_H__


/*
 * Software License Agreement (New BSD License)
 *
 * Copyright (c) 2013, Keith Leung, Felipe Inostroza
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Advanced Mining Technology Center (AMTC), the
 *       Universidad de Chile, 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 AMTC, UNIVERSIDAD DE CHILE, OR THE COPYRIGHT
 * HOLDERS 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 RANDOM_VEC_MATH_TOOLS
#define RANDOM_VEC_MATH_TOOLS

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

#include <math.h>

//double const PI = acos(-1);

/**
 * \class RandomVecMathTools
 * \brief A collection of methods for use on a RandomVec derived class.
 * All methods are static, therefore this class does not need to be
 * instantiated.
 * \tparam RandomVec derived class
 * \author Keith Leung
 */
template< class RandomVecDerived >
class RandomVecMathTools
{
public:

  /**
   * Calculate the sqaured Mahalanobis distance from a random vector,
   * \param[in] x_fm the random vector from which we can calcuating the
   * distance, and of which the covariance is taken for scaling
   * \param[in] x_to the random vector to which we are measuring.
   * The covariance of this is ignored
   * \return mahalanobis distance squared
   */
  static double mahalanobisDist2( RandomVecDerived &x_fm,
				  RandomVecDerived &x_to){
    typename RandomVecDerived::Vec x1;
    x_to.get(x1);
    return mahalanobisDist2( x_fm, x1 );
  }

  /**
   * Calculate the sqaured Mahalanobis distance from a random vector,
   * \param[in] x_fm the random vector from which we can calcuating the
   * distance, and of which the covariance is taken for scaling
   * \param[in] x_to the random vector to which we are measuring.
   * \return mahalanobis distance squared
   */
  static double mahalanobisDist2( RandomVecDerived &x_fm,
				  typename RandomVecDerived::Vec &x_to){
    typename RandomVecDerived::Vec x0;
    typename RandomVecDerived::Mat Sx0Inv;
    x_fm.get(x0);
    x_fm.getCovInv( Sx0Inv );
    typename RandomVecDerived::Vec e = x_to - x0;
    return (e.transpose() * Sx0Inv * e);
  }

  /**
   * Calculate the Mahalanobis distance from a random vector,
   * \param[in] x_fm the random vector from which we can calcuating the
   * distance, and of which the covariance is taken for scaling
   * \param[in] x_to the random vector to which we are measuring.
   * The covariance of this is ignored
   * \return mahalanobis distance
   */
  static double mahalanobisDist( RandomVecDerived &x_fm,
				 RandomVecDerived &x_to){
    return sqrt( mahalanobisDist2( x_fm, x_to ) );
  }

  /**
   * Calculate the Mahalanobis distance from a random vector,
   * \param[in] x_fm the random vector from which we can calcuating the
   * distance, and of which the covariance is taken for scaling
   * \param[in] x_to the random vector to which we are measuring.
   * \return mahalanobis distance
   */
  static double mahalanobisDist( RandomVecDerived &x_fm,
				  typename RandomVecDerived::Vec &x_to){
    return sqrt( mahalanobisDist2( x_fm, x_to ) );
  }

  /**
   * Evaluate the Gaussian likelihood of a evaluation point
   * \param[in] gaussian The Gaussian distribution represented by a random vector
   * \param[in] x_eval evaluation point
   * \param[out] mDist2 if not NULL, stores the squared mahalanobis distance used
   * to calculate the likelihood
   * \return likelihood
   */
  static double evalGaussianLikelihood( RandomVecDerived &gaussian,
					RandomVecDerived &x_eval,
					double* mDist2 = NULL){
    double nDim = gaussian.getNDim();
    double covDet = gaussian.getCovDet();
    double md2 = mahalanobisDist2( gaussian, x_eval );
    double l = ( exp(-0.5 * md2 ) / sqrt( pow( 2*PI, nDim) * covDet ) );
    //If md2 is very large, l will become NAN;
    if( l != l)
      l = 0;
    if(mDist2 != NULL)
      *mDist2 = md2;
    return l;
  }

  /**
   * Evaluate the Gaussian likelihood of a evaluation point
   * \param[in] gaussian The Gaussian distribution represented by a random vector
   * \param[in] x_eval evaluation point
   * \param[out] mDist2 if not NULL, stores the squared mahalanobis distance used
   * to calculate the likelihood
   * \return likelihood
   */
  static double evalGaussianLikelihood( RandomVecDerived &gaussian,
					typename RandomVecDerived::Vec &x_eval,
					double* mDist2 = NULL ){
    double nDim = gaussian.getNDim();
    double covDet = gaussian.getCovDet();
    double md2 = mahalanobisDist2( gaussian, x_eval );
    double l = ( exp(-0.5 * md2 ) / sqrt( pow( 2*PI, nDim) * covDet ) );
    // If md2 is very large, l will become NAN;
    if( l != l)
      l = 0;
    if(mDist2 != NULL)
      *mDist2 = md2;
    return l;
  }

  /**
   * Sample the random vector
   * \param[in] s The random vector with the mean and covariance
   * \param[out] s_sample The sampled vector. The covariance and time
   * of the s are copied.
   */
  static void sample( RandomVecDerived &s,
		      RandomVecDerived &s_sample ){

    typename RandomVecDerived::Vec x, indep_noise, e;
    typename RandomVecDerived::Mat Sx, Sx_L;
    double t;
    s.get(x, Sx, t);

    s.getCovCholeskyDecompLower(Sx_L);

    static boost::mt19937 rng_;
    static boost::normal_distribution<double> nd_;
    static boost::variate_generator< boost::mt19937,
				     boost::normal_distribution<double> >
      gen_(rng_, nd_);

    int n = Sx_L.cols();
    for(int i = 0; i < n; i++){
      indep_noise(i) = gen_();
    }
    e = Sx_L * indep_noise;
    x += e;
    s_sample.set( x, Sx, t );

  }

  /**
   * Sample a random vector
   * \param[in] x The mean of the random vector
   * \param[in] Sx The covariance of the random vector
   * \param[in] Sx_L The lower Cholesky decomposition of the covariance
   * \param[in] t Time
   * \param[out] s_sample The sampled vector. The covariance and time
   * of the s are copied.
   */

  static void sample( typename RandomVecDerived::Vec &x,
		      typename RandomVecDerived::Mat &Sx,
		      typename RandomVecDerived::Mat &Sx_L,
		      double &t,
		      RandomVecDerived &s_sample ){

    typename RandomVecDerived::Vec indep_noise, e;

    static boost::mt19937 rng_;
    static boost::normal_distribution<double> nd_;
    static boost::variate_generator< boost::mt19937,
				     boost::normal_distribution<double> >
      gen_(rng_, nd_);

    int n = Sx_L.cols();
    for(int i = 0; i < n; i++){
      indep_noise(i) = gen_();
    }
    e = Sx_L * indep_noise;
    x += e;
    s_sample.set( x, Sx, t );

  }

};

#endif


#include <dynet/dynet.h>
#include <dynet/expr.h>
#include <dynet/training.h>
#include <dynet/grad-check.h>
#include <boost/test/unit_test.hpp>
#include <stdexcept>

using namespace dynet;
using namespace dynet::expr;
using namespace std;

struct TrainerTest {
  TrainerTest() {
    // initialize if necessary
    if(default_device == nullptr) {
      for (auto x : {"TrainerTest", "--dynet-mem", "10"}) {
        av.push_back(strdup(x));
      }
      char **argv = &av[0];
      int argc = av.size();
      dynet::initialize(argc, argv);
    }
    ones_vals = {1.f,1.f,1.f};
    param_vals = {1.1f,-2.2f,3.3f};
  }
  ~TrainerTest() {
    for (auto x : av) free(x);
  }

  template <class T>
  std::string print_vec(const std::vector<T> vec) {
    ostringstream oss;
    if(vec.size()) oss << vec[0];
    for(size_t i = 1; i < vec.size(); i++)
      oss << ' ' << vec[i];
    return oss.str();
  }

  std::vector<float> ones_vals, param_vals;
  std::vector<char*> av;
};

// define the test suite
BOOST_FIXTURE_TEST_SUITE(trainer_test, TrainerTest);

BOOST_AUTO_TEST_CASE( simple_sgd_direction ) {
  dynet::Model mod;
  dynet::Parameter param = mod.add_parameters({3});
  TensorTools::SetElements(param.get()->values,param_vals);
  SimpleSGDTrainer trainer(&mod);
  dynet::ComputationGraph cg;
  Expression x = parameter(cg, param);
  Expression y = input(cg, {1,3}, ones_vals);
  Expression z = y*x;
  float before = as_scalar(cg.forward(z));
  cg.backward(z);
  trainer.update(0.1);
  float after = as_scalar(cg.forward(z));
  BOOST_CHECK_LT(after, before);
}

BOOST_AUTO_TEST_CASE( momentum_sgd_direction ) {
  dynet::Model mod;
  dynet::Parameter param = mod.add_parameters({3});
  TensorTools::SetElements(param.get()->values,param_vals);
  MomentumSGDTrainer trainer(&mod);
  dynet::ComputationGraph cg;
  Expression x = parameter(cg, param);
  Expression y = input(cg, {1,3}, ones_vals);
  Expression z = y*x;
  float before = as_scalar(cg.forward(z));
  cg.backward(z);
  trainer.update(0.1);
  float after = as_scalar(cg.forward(z));
  BOOST_CHECK_LT(after, before);
}

BOOST_AUTO_TEST_CASE( adagrad_direction ) {
  dynet::Model mod;
  dynet::Parameter param = mod.add_parameters({3});
  TensorTools::SetElements(param.get()->values,param_vals);
  AdagradTrainer trainer(&mod);
  dynet::ComputationGraph cg;
  Expression x = parameter(cg, param);
  Expression y = input(cg, {1,3}, ones_vals);
  Expression z = y*x;
  float before = as_scalar(cg.forward(z));
  cg.backward(z);
  trainer.update(0.1);
  float after = as_scalar(cg.forward(z));
  BOOST_CHECK_LT(after, before);
}

BOOST_AUTO_TEST_CASE( adadelta_direction ) {
  dynet::Model mod;
  dynet::Parameter param = mod.add_parameters({3});
  TensorTools::SetElements(param.get()->values,param_vals);
  AdadeltaTrainer trainer(&mod);
  dynet::ComputationGraph cg;
  Expression x = parameter(cg, param);
  Expression y = input(cg, {1,3}, ones_vals);
  Expression z = y*x;
  float before = as_scalar(cg.forward(z));
  cg.backward(z);
  trainer.update(0.1);
  float after = as_scalar(cg.forward(z));
  BOOST_CHECK_LT(after, before);
}

BOOST_AUTO_TEST_CASE( adam_direction ) {
  dynet::Model mod;
  dynet::Parameter param = mod.add_parameters({3});
  TensorTools::SetElements(param.get()->values,param_vals);
  AdamTrainer trainer(&mod);
  dynet::ComputationGraph cg;
  Expression x = parameter(cg, param);
  Expression y = input(cg, {1,3}, ones_vals);
  Expression z = y*x;
  float before = as_scalar(cg.forward(z));
  cg.backward(z);
  trainer.update(0.1);
  float after = as_scalar(cg.forward(z));
  BOOST_CHECK_LT(after, before);
}

BOOST_AUTO_TEST_SUITE_END()


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2020, LAAS-CNRS, New York University, Max Planck Gesellschaft,
//                          INRIA, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API

#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include <pinocchio/algorithm/kinematics-derivatives.hpp>
#include <pinocchio/algorithm/frames.hpp>

#include "crocoddyl/multibody/impulses/impulse-3d.hpp"
#include "crocoddyl/multibody/impulses/impulse-6d.hpp"

#include "factory/impulse.hpp"
#include "unittest_common.hpp"

using namespace boost::unit_test;
using namespace crocoddyl::unittest;

//----------------------------------------------------------------------------//

void test_construct_data(ImpulseModelTypes::Type impulse_type, PinocchioModelTypes::Type model_type) {
  // create the model
  ImpulseModelFactory factory;
  boost::shared_ptr<crocoddyl::ImpulseModelAbstract> model = factory.create(impulse_type, model_type);

  // create the corresponding data object
  pinocchio::Data pinocchio_data(*model->get_state()->get_pinocchio().get());
  boost::shared_ptr<crocoddyl::ImpulseDataAbstract> data = model->createData(&pinocchio_data);
}

void test_calc_fetch_jacobians(ImpulseModelTypes::Type impulse_type, PinocchioModelTypes::Type model_type) {
  // create the model
  ImpulseModelFactory factory;
  boost::shared_ptr<crocoddyl::ImpulseModelAbstract> model = factory.create(impulse_type, model_type);

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model = model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ImpulseDataAbstract> data = model->createData(&pinocchio_data);

  // Compute the jacobian and check that the impulse model fetch it.
  Eigen::VectorXd x = model->get_state()->rand();
  crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(), &pinocchio_data, x);

  // Getting the jacobian from the model
  Eigen::VectorXd dx;
  model->calc(data, dx);

  // Check that only the Jacobian has been filled
  BOOST_CHECK(!data->Jc.isZero());
  BOOST_CHECK(data->dv0_dq.isZero());
  BOOST_CHECK(data->f.toVector().isZero());
  BOOST_CHECK(data->df_dx.isZero());
}

void test_calc_diff_fetch_derivatives(ImpulseModelTypes::Type impulse_type, PinocchioModelTypes::Type model_type) {
  // create the model
  ImpulseModelFactory factory;
  boost::shared_ptr<crocoddyl::ImpulseModelAbstract> model = factory.create(impulse_type, model_type);

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model = model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ImpulseDataAbstract> data = model->createData(&pinocchio_data);

  // Compute the jacobian and check that the impulse model fetch it.
  Eigen::VectorXd x = model->get_state()->rand();
  crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(), &pinocchio_data, x);

  // Getting the jacobian from the model
  Eigen::VectorXd dx;
  model->calc(data, dx);
  model->calcDiff(data, dx);

  // Check that nothing has been computed and that all value are initialized to 0
  BOOST_CHECK(!data->Jc.isZero());
  BOOST_CHECK(!data->dv0_dq.isZero());
  BOOST_CHECK(data->f.toVector().isZero());
  BOOST_CHECK(data->df_dx.isZero());
}

void test_update_force(ImpulseModelTypes::Type impulse_type, PinocchioModelTypes::Type model_type) {
  // create the model
  ImpulseModelFactory factory;
  boost::shared_ptr<crocoddyl::ImpulseModelAbstract> model = factory.create(impulse_type, model_type);

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model = model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ImpulseDataAbstract> data = model->createData(&pinocchio_data);

  // Create a random force and update it
  Eigen::VectorXd f = Eigen::VectorXd::Random(data->Jc.rows());
  model->updateForce(data, f);
  boost::shared_ptr<crocoddyl::ImpulseModel3D> m = boost::static_pointer_cast<crocoddyl::ImpulseModel3D>(model);

  // Check that nothing has been computed and that all value are initialized to 0
  BOOST_CHECK(data->Jc.isZero());
  BOOST_CHECK(data->dv0_dq.isZero());
  BOOST_CHECK(!data->f.toVector().isZero());
  BOOST_CHECK(data->df_dx.isZero());
}

void test_update_force_diff(ImpulseModelTypes::Type impulse_type, PinocchioModelTypes::Type model_type) {
  // create the model
  ImpulseModelFactory factory;
  boost::shared_ptr<crocoddyl::ImpulseModelAbstract> model = factory.create(impulse_type, model_type);

  // create the corresponding data object
  const boost::shared_ptr<pinocchio::Model>& pinocchio_model = model->get_state()->get_pinocchio();
  pinocchio::Data pinocchio_data(*pinocchio_model.get());
  boost::shared_ptr<crocoddyl::ImpulseDataAbstract> data = model->createData(&pinocchio_data);

  // Create a random force and update it
  Eigen::MatrixXd df_dx = Eigen::MatrixXd::Random(data->df_dx.rows(), data->df_dx.cols());
  model->updateForceDiff(data, df_dx);

  // Check that nothing has been computed and that all value are initialized to 0
  BOOST_CHECK(data->Jc.isZero());
  BOOST_CHECK(data->dv0_dq.isZero());
  BOOST_CHECK(data->f.toVector().isZero());
  BOOST_CHECK(!data->df_dx.isZero());
}

//----------------------------------------------------------------------------//

void register_impulse_model_unit_tests(ImpulseModelTypes::Type impulse_type, PinocchioModelTypes::Type model_type) {
  boost::test_tools::output_test_stream test_name;
  test_name << "test_" << impulse_type << "_" << model_type;
  std::cout << "Running " << test_name.str() << std::endl;
  test_suite* ts = BOOST_TEST_SUITE(test_name.str());
  ts->add(BOOST_TEST_CASE(boost::bind(&test_construct_data, impulse_type, model_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_fetch_jacobians, impulse_type, model_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_diff_fetch_derivatives, impulse_type, model_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_update_force, impulse_type, model_type)));
  ts->add(BOOST_TEST_CASE(boost::bind(&test_update_force_diff, impulse_type, model_type)));
  framework::master_test_suite().add(ts);
}

bool init_function() {
  for (size_t impulse_type = 0; impulse_type < ImpulseModelTypes::all.size(); ++impulse_type) {
    for (size_t model_type = 0; model_type < PinocchioModelTypes::all.size(); ++model_type) {
      register_impulse_model_unit_tests(ImpulseModelTypes::all[impulse_type], PinocchioModelTypes::all[model_type]);
    }
  }
  return true;
}

int main(int argc, char** argv) { return ::boost::unit_test::unit_test_main(&init_function, argc, argv); }


#define DEBUG 1
/**
 * File    : B2.cpp
 * Author  : Kazune Takahashi
 * Created : 11/24/2019, 3:04:17 PM
 * Powered by Visual Studio Code
 */
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <vector>
#include <string>
#include <complex>
#include <tuple>
#include <queue>
#include <stack>
#include <map>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <bitset>
#include <functional>
#include <random>
#include <chrono>
#include <cctype>
#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib>
// ----- boost -----
#include <boost/rational.hpp>
// ----- using directives and manipulations -----
using boost::rational;
using namespace std;
using ll = long long;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{30000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{x % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
ll gcd(ll x, ll y) { return y ? gcd(y, x % y) : x; }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1000000000000000LL};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

// ----- UnionFind -----

class UnionFind
{
  int N;
  vector<int> par;

public:
  UnionFind() : N{0} {}
  UnionFind(int N) : N{N}, par(N, -1) {}

  bool is_same(int x, int y)
  {
    return root(x) == root(y);
  }

  bool merge(int x, int y)
  {
#if DEBUG == 1
    cerr << "merge(" << x << ", " << y << ")" << endl;
#endif
    x = root(x);
    y = root(y);
    if (x == y)
    {
      return false;
    }
    if (par[x] > par[y])
    {
      swap(x, y);
    }
    par[x] += par[y];
    par[y] = x;
    return true;
  }

  int size(int x)
  {
    return -par[root(x)];
  }

  int count_parts()
  {
    int ans{0};
    for (auto i = 0; i < N; i++)
    {
      if (par[i] < 0)
      {
        ++ans;
      }
    }
    return ans;
  }

private:
  int root(int x)
  {
    if (par[x] < 0)
    {
      return x;
    }
    return par[x] = root(par[x]);
  }
};

// ----- main() -----

vector<int> rev(vector<int> const &P)
{
  vector<int> Q(P.size());
  for (auto i = 0; i < static_cast<int>(P.size()); i++)
  {
    Q[P[i]] = i;
  }
  return Q;
}

void flush(int N, int K, UnionFind &uf)
{
  if (uf.size(N - K + 1) == 1)
  {
    cout << uf.count_parts() - 1 << endl;
  }
  else
  {
    cout << uf.count_parts() << endl;
  }
}

vector<int> ascend_sum(int N, int K, vector<int> const &P)
{
  vector<int> ascend(N - 1);
  for (auto i = 0; i < N - 1; i++)
  {
    ascend[i] = P[i] < P[i + 1] ? 0 : 1;
  }
  vector<int> res(N, 0);
  for (auto i = 1; i < N; i++)
  {
    res[i] = res[i - 1] + ascend[i - 1];
  }
  return res;
}

void connect_origin(int N, int K, vector<int> const &P, UnionFind &uf)
{
  vector<int> A{ascend_sum(N, K, P)};
#if DEBUG == 1
  cerr << "A: ";
  for (auto x : A)
  {
    cerr << x;
  }
  cerr << endl;
#endif
  for (auto i = 0; i <= N - K; i++)
  {
    if (A[i + K - 1] - A[i] == 0)
    {
      uf.merge(i, N - K + 1);
    }
  }
}

vector<bool> is_mini(int N, int K, vector<int> const &Q)
{
  vector<bool> res(N, false);
  set<int> S;
  for (auto e : Q)
  {
    S.insert(e);
    if (0 <= e && e <= N - K - 1)
    {
      auto it{S.find(e)};
      ++it;
      if (it == S.end() || *it - e > K)
      {
        res[e] = true;
      }
    }
  }
  return res;
}

vector<bool> is_maxi(int N, int K, vector<int> const &Q)
{
  vector<bool> res(N, false);
  set<int> S;
  for (auto e : Q)
  {
    S.insert(e);
    if (K <= e && e <= N - 1)
    {
      auto it{S.find(e)};
      auto it0{it};
      --it0;
      if (it == S.begin() || e - *it0 > K)
      {
        res[e] = true;
      }
    }
  }
  return res;
}

void connect_adjacent(int N, int K, vector<int> const &P, UnionFind &uf)
{
  vector<int> Q{rev(P)};
  vector<int> R(Q);
  reverse(R.begin(), R.end());
  vector<bool> mini{is_mini(N, K, Q)}, maxi{is_maxi(N, K, R)};
#if DEBUG == 1
  cerr << "mini: ";
  for (auto x : mini)
  {
    cerr << x;
  }
  cerr << endl;
  cerr << "maxi: ";
  for (auto x : maxi)
  {
    cerr << x;
  }
  cerr << endl;
#endif
  for (auto i = 0; i < N - K; i++)
  {
    if (mini[i] && maxi[i + K])
    {
      uf.merge(i, i + 1);
    }
  }
}

int main()
{
  int N, K;
  cin >> N >> K;
  vector<int> P(N);
  for (auto i = 0; i < N; i++)
  {
    cin >> P[i];
  }
  UnionFind uf(N - K + 2); // the node N - K + 1 is origin.
  connect_origin(N, K, P, uf);
  connect_adjacent(N, K, P, uf);
  flush(N, K, uf);
}


#include "RBMGenerator.hpp"
#include "XACC.hpp"
#include "DWQMI.hpp"
#include "DWFunction.hpp"
#include <Eigen/Dense>
#include <typeinfo>

using namespace xacc;

namespace xacc {
namespace rbm {

std::shared_ptr<Function> RBMGenerator::generate(
                        std::map<std::string, InstructionParameter> parameters) {

    if (!parameters.count("visible-units") && !parameters.count("visible_units")) {
        xacc::error("Invalid mapping of parameters for RBM generator, missing visible units key.");
    }

    if (!parameters.count("hidden-units") && !parameters.count("hidden_units")) {
        xacc::error("Invalid mapping of parameters for RBM generator, missing hidden units key.");
    }

    bool hasUnderscore = false;

    if (parameters.count("visible_units")) {
        hasUnderscore = true;
        if (!parameters.count("hidden_units")) xacc::error("RBM Generator missing hidden_units key.");
    }

    if (parameters.count("hidden_units")) {
        hasUnderscore = true;
        if (!parameters.count("visible_units")) xacc::error("RBM Generator missing visible_units key.");
    }

    std::vector<InstructionParameter> params;
    if (hasUnderscore) {
        params.push_back(parameters["visible_units"]);
        params.push_back(parameters["hidden_units"]);
    } else {
        params.push_back(parameters["visible-units"]);
        params.push_back(parameters["hidden-units"]);
    }

    return generate(nullptr, params);
}

std::shared_ptr<Function> RBMGenerator::generate(
		                        std::shared_ptr<AcceleratorBuffer> buffer,
		                        std::vector<InstructionParameter> parameters) {

    xacc::info("Running RBM IRGenerator.");

    int n_visible = parameters[1].as<int>();
    int n_hidden = parameters[0].as<int>();

    auto kernel = std::make_shared<xacc::quantum::DWFunction>("rbm_function");

    for (int i = 0; i < n_visible; i++){
        std::string paramName = "v" + std::to_string(i);
        xacc::InstructionParameter visParam(paramName);
        auto qmi = std::make_shared<xacc::quantum::DWQMI>(i, i, visParam);
        kernel->addInstruction(qmi);
    }

    for (int i = n_visible; i < n_visible + n_hidden; i++){
        std::string paramName = "h" + std::to_string(i);
        xacc::InstructionParameter hidParam(paramName);
        auto qmi = std::make_shared<xacc::quantum::DWQMI>(i, i, hidParam);
        kernel->addInstruction(qmi);
    }
    for (int i = 0; i < n_visible; i++){
        for (int j = n_visible; j < n_visible + n_hidden; j++){
            std::string paramName = "w" + std::to_string(i) + std::to_string(j);
            xacc::InstructionParameter wParam(paramName);
            auto qmi = std::make_shared<xacc::quantum::DWQMI>(i, j, wParam);
            kernel->addInstruction(qmi);
        }
    }
    return kernel;
}
}
}