/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#ifndef TURI_FAST_MULTINOMIAL_HPP
#define TURI_FAST_MULTINOMIAL_HPP

#include <vector>
#include <algorithm>

#include <boost/integer.hpp>
#include <boost/random.hpp>

#include <core/parallel/pthread_tools.hpp>
#include <core/parallel/atomic.hpp>

#include <core/generics/float_selector.hpp>

namespace turi {

  /// \ingroup util_internal
  class fast_multinomial {
    // system word length float
    typedef float_selector<sizeof(size_t)>::float_type float_t;

    //! First leaf index
    size_t first_leaf_index;

    //! The number of assignments to the multinomial
    size_t num_asg;

    //! The tree datastructure
    std::vector<float_t> tree;

    //! the number of positive probability elements
    atomic<size_t> num_support;

    // Helper routines
    // ========================================================>

    //! Compute the next power of 2
    //     size_t next_powerof2(size_t val) {
    //       size_t powof2 = 1;
    //       while(powof2 < val) powof2 = powof2 * 2;
    //       return powof2;
    //     }

    //! Clever next power of two bit magic
    uint64_t next_powerof2(uint64_t val) {
      --val;
      val = val | (val >> 1);
      val = val | (val >> 2);
      val = val | (val >> 4);
      val = val | (val >> 8);
      val = val | (val >> 16);
      val = val | (val >> 32);
      return val + 1;
    }

    //! Returns the index of the left child of the supplied index.
    size_t left_child(size_t i) const { return 2 * i + 1; }

    //! Returns the index of the right child of the supplied index.
    size_t right_child(size_t i) const { return 2 * i + 2; }

    //! Returns the index of the parent of the supplied index.
    size_t parent(size_t i) const { return (i-1) / 2; }

    //! returns the sibling of
    size_t sibling(size_t i) const {
      // the binary here is equivalent to (+1) if leaf is odd and (-1) if
      // leaf is even
      return i + (i & 1)*2 - 1;
    }

    //! get the tree location of the assignment
    size_t tree_loc_from_asg(size_t asg) const {
      size_t loc = asg + first_leaf_index;
      assert(loc < tree.size());
      assert(is_leaf(loc));
      return loc;
    }

    //! determine the assignment from the location in the tree
    size_t asg_from_tree_loc(size_t i) const {
      assert(is_leaf(i));
      size_t asg = i - first_leaf_index;
      assert(asg < num_asg);
      return asg;
    }

    //! Returns true if the index corresponds to a leaf
    bool is_leaf(size_t i) const {
      return i >= first_leaf_index;
      //      return left_child(i) > tree.size();
    }

    //! Returns true if the location is the root
    bool is_root(size_t i) const { return i == 0; }

    /// Returns the index of a leaf sampled proportionate to its
    /// priority.  Returns false on failure
    bool try_sample(size_t& asg, size_t cpuid) {
      size_t loc = 0;
      while ( !is_leaf(loc) ) {
        // get the left and right priorities
        float_t left_p = tree.at(left_child(loc));
        float_t right_p = tree.at(right_child(loc));
        // if both are zero, the sample has failed. Return
        if (left_p + right_p == 0) return false;
        else if (right_p == 0) loc = left_child(loc);
        else if (left_p == 0)  loc = right_child(loc);
        else {
          // pick from a bernoulli trial
          float_t childsum = left_p + right_p;
          float_t rndnumber = turi::random::uniform<float_t>(0,1);
          if((childsum * rndnumber)  < left_p)
            loc = left_child(loc);
          else
            loc = right_child(loc);
        }
      }
      assert(is_leaf(loc));
      asg = asg_from_tree_loc(loc);
      assert(asg < num_asg);
      return true;
    } // end of sample index

    /// Propagates a cumulative sum update up the tree.
    void propagate_change(size_t loc) {
      // Loop while the location is not the root each time moving up
      // the tree
      for( ; !is_root(loc); loc = parent(loc) ) {
        // Get the sibbling of this location
        size_t sibling_loc = sibling(loc);
        assert(sibling_loc < tree.size());
        // Get the parent
        size_t parent_loc = parent(loc);
        assert(parent_loc < tree.size());
        // Assert that the sibling is infact the sibling
        assert(parent_loc == parent(sibling_loc));
        // Get the priority of this location and the sibling
        // and the parent
        volatile float_t* sibling1 = &(tree[loc]);
        volatile float_t* sibling2 = &(tree[sibling_loc]);
        volatile float_t* parent = &(tree[parent_loc]);

        // write to the parent. Use a concurrent write mechanism
        // size_t spin_count = 0;
        //float_t old_value = *parent;
        //        float_t new_value = *sibling1 + *sibling2;
        //        while(!atomic_compare_and_swap(tree[parent_loc], old_value, new_value)) {
        //          old_value = *parent;
        //          new_value = *sibling1 + *sibling2;
        // if(++spin_count % 10 == 0) {
        //   std::cerr << "Propagate_change: " << spin_count << std::endl;
        // }
        //      }
        while(true) {
          float_t sum = (*sibling1) + (*sibling2);
          (*parent) = sum;
          __asm("mfence");
          float_t sum2 = (*sibling1) + (*sibling2);
          float_t parentval = (*parent);
          if (sum2 == parentval) break;
        }
        // If the update was successful accomplished by anothe thread
        // than return
        //if(old_value == new_value) return;
      } // end of for loop
    } // end of propagate change

  public:

    /** initialize a fast multinomail */
    fast_multinomial(size_t num_asg,
                     size_t ncpus) :
      first_leaf_index(0),
      num_asg(num_asg) {
      // // initialize the generators
      // for(size_t i = 0; i < rngs.size(); ++i) {
      //   rngs[i].seed(rand());
      //   distributions.push_back(distribution_type(rngs[i]));
      // }
      // Determine the size of the tree
      first_leaf_index = next_powerof2(num_asg) - 1;
      size_t tree_size = first_leaf_index + next_powerof2(num_asg);
      tree.resize(tree_size, 0.0);
    }

    void zero(size_t asg) {
      assert(asg < num_asg);
      size_t loc = tree_loc_from_asg(asg);
      // Use CAS
      float_t old_value = tree[loc];
      float_t new_value = 0;
      while(!atomic_compare_and_swap(tree[loc], old_value, new_value)){
        old_value = tree[loc];
      }
      propagate_change(loc);
      if(old_value > 0) {
        num_support.dec();
      }
    }

    //! Set a leaf value
    void set(size_t asg, float_t value) {
      assert(asg < num_asg);
      assert(value >= 0);
      size_t loc = tree_loc_from_asg(asg);
      // Use atomic compare and swap to update the value
      float_t old_value = tree[loc];
      float_t new_value = value;
      while(!atomic_compare_and_swap(tree[loc], old_value, new_value)){
        old_value = tree[loc];
      }
      if(old_value == 0 && new_value > 0) {
        num_support.inc();
      }
      propagate_change(loc);
      // Update support count
      if(old_value > 0 && new_value == 0) {
        num_support.dec();
      }
    } // end of set

    //! Set a leaf value
    void add(size_t asg, float_t value) {
      assert(asg < num_asg);
      assert(value >= 0);
      size_t loc = tree_loc_from_asg(asg);
      // Use atomic compare and swap to update the value
      float_t old_value = tree[loc];
      float_t new_value = value + old_value;
      while(!atomic_compare_and_swap(tree[loc], old_value, new_value)){
        old_value = tree[loc];
        new_value = value + old_value;
      }
      // Update support count
      if(old_value == 0 && new_value > 0) {
        num_support.inc();
      }
      propagate_change(loc);
    } // end of add

    //! Set a leaf value
    void max(size_t asg, float_t value) {
      assert(asg < num_asg);
      assert(value >= 0);
      size_t loc = tree_loc_from_asg(asg);
      // Use atomic compare and swap to update the value
      float_t old_value = tree[loc];
      float_t new_value = std::max(value, old_value);
      while(!atomic_compare_and_swap(tree[loc], old_value, new_value)){
        old_value = tree[loc];
        new_value = std::max(value, old_value);
      }
      if(old_value == 0 && new_value > 0) {
        num_support.inc();
      }
      propagate_change(loc);
      // Update support count
      if(old_value > 0 && new_value == 0) {
        num_support.dec();
      }
    } // end of set

    /**
     * Try to draw a sample from the multinomial.  If everything has
     * probability zero then return false.
     */
    bool sample(size_t& ret_asg, size_t cpuid) {
      // While there is positive support for some assignment
      // size_t spin_count = 0;
      volatile float_t *root = &(tree[0]);
      while(num_support.value > 0 || (*root) > 0) {
        // Try and get a sample
        if(try_sample(ret_asg, cpuid)) {
          assert(ret_asg < num_asg);
          return true;
        }

        //         if(++spin_count % 10000 == 0) {
        //           std::cerr // << THREAD_ID() << ": "
        //                     << "  Sample: " << spin_count
        //                     << ", " << tree[0]
        //                     << ", " << num_support.value
        //                     << std::endl;
        //           float_t sum = 0;
        //           for(size_t i = first_leaf_index; i < tree.size(); ++i) {
        //             sum += tree[i];
        //           }
        //           std::cerr << "Tree Sum: " << sum << std::endl;
        //           std::getchar();
        //         }

      }  // End of While loop

      //       if(spin_count >= 10){
      //         std::cerr // << THREAD_ID() << ": "
      //                   << "  Sample_recover: " << spin_count << std::endl;
      //       }

      return false;
    } // end of sample

    /**
     * Try to draw a sample from the multinomial and zero out the
     * probability of the element that was drawn.  If everything has
     * probability zero then return false.
     */
    bool pop(size_t& ret_asg, size_t cpuid) {
      if(tree.empty()) return false;
      // While there is positive support for some assignment
      while(num_support.value > 0 || tree[0] > 0) {
        // Try and get a sample
        if(try_sample(ret_asg, cpuid)) {
          assert(ret_asg < num_asg);
          // We have a sample but it is possible that another thread
          // also go this sample so we will use CAS to see who "wins"
          // and gets to keep the sample and who has to try again
          size_t loc = tree_loc_from_asg(ret_asg);
          // Use CAS
          float_t old_value = tree[loc];
          float_t new_value = 0;
          while(!atomic_compare_and_swap(tree[loc], old_value, new_value)){
            old_value = tree[loc];
          }
          // Figure out if we won and get to keep the sample or if
          // some other thread won and zeroed out the sample before we
          // got it.
          if(old_value > 0) {
            // We win!!! and keep the sample :-)
            propagate_change(loc);
            num_support.dec();
            return true;
          }
          // The other thread wins and we have to try agian :-(.
        }
      }

      std::cerr << "Queue emptied!: " << tree[0]
                << ", " << num_support.value << std::endl;
      print_tree();
      return false;
    } // end of pop

    /** Get the number of assignments with positive support */
    size_t positive_support() {
      return num_support.value;
    }

    /** print the tree */
    void print_tree() {
      for (size_t i = 0; i < std::min(tree.size(), size_t(1000)); ++i) {
        if(is_leaf(i)) {
          std::cout << "Leaf(" << asg_from_tree_loc(i)
                    << ", [" << parent(i) << "], "
                    << tree[i] << ") ";
        } else {
          std::cout << "Node(" << i <<  ", "
                    << "[" << left_child(i) << ", "
                    << right_child(i) << "], "
                    << tree[i] << ") ";
        }
      }
      std::cout << std::endl;
    }

    float_t get_weight(size_t asg) const {
      size_t loc = tree_loc_from_asg(asg);
      return tree[loc];
    }

    bool has_support(size_t asg) const {
      size_t loc = tree_loc_from_asg(asg);
      return tree[loc] > 0;
    }

    void clear() {
      // not thread safe
      std::fill(tree.begin(), tree.end(), 0.0);
      num_support.value = 0;
    }
  }; // end of fast_multinomial

} // end of namespace

#undef float_t
#endif


/***********************************************************************

Copyright (c) 2014, Carnegie Mellon University
All rights reserved.

Authors: Jennifer King <jeking04@gmail.com>

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.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*************************************************************************/

#include <vector>
#include <Eigen/Geometry>
#include <ompl/util/RandomNumbers.h>
#include <or_ompl/TSR.h>

using namespace or_ompl;

TSR::TSR()
    : _initialized(false)
{
}

TSR::TSR(
        const Eigen::Affine3d &T0_w,
        const Eigen::Affine3d &Tw_e,
        const Eigen::Matrix<double, 6, 2> &Bw)
    : _T0_w(T0_w)
    , _Tw_e(Tw_e)
    , _Bw(Bw)
    , _manipulator_index(-1)
    , _relative_body_name("NULL")
    , _relative_link_name("")
    , _initialized(true)
{
    _T0_w_inv = _T0_w.inverse();
    _Tw_e_inv = _Tw_e.inverse();
}

bool TSR::deserialize(std::stringstream &ss)
{
    return deserialize(static_cast<std::istream &>(ss));
}

bool TSR::deserialize(std::istream &ss)
{
    // Set _initialized to false in case an error occurs.
    _initialized = false;

    ss >> _manipulator_index
       >> _relative_body_name;

    if(_relative_body_name != "NULL")
        ss >> _relative_link_name;

    // Read in the T0_w matrix
    for(unsigned int c=0; c < 3; c++)
    for(unsigned int r=0; r < 3; r++)
        ss >> _T0_w.matrix()(r,c);

    for(unsigned int idx=0; idx < 3; idx++)
        ss >> _T0_w.translation()(idx);

    // Read in the Tw_e matrix
    for(unsigned int c=0; c < 3; c++)
    for(unsigned int r=0; r < 3; r++)
        ss >> _Tw_e.matrix()(r,c);

    for(unsigned int idx=0; idx < 3; idx++)
        ss >> _Tw_e.translation()(idx);

    // Read in the Bw matrix
    for(unsigned int r=0; r < 6; r++)
    for(unsigned int c=0; c < 2; c++)
        ss >> _Bw(r,c);

    // Check for an error.
    if (!ss)
        return false;

    _T0_w_inv = _T0_w.inverse();
    _Tw_e_inv = _Tw_e.inverse();
    _initialized = true;

    return true;
}

void TSR::serialize(std::ostream& ss)
{
    if (!_initialized)
        throw std::runtime_error("TSR is not initialized.");

    ss << _manipulator_index
       << ' ' << _relative_body_name;

    if(_relative_body_name != "NULL")
        ss << ' ' << _relative_link_name;

    // T0_w matrix
    for(unsigned int c=0; c < 3; c++)
    for(unsigned int r=0; r < 3; r++)
        ss << ' ' << _T0_w.matrix()(r, c);

    for(unsigned int idx=0; idx < 3; idx++)
        ss << ' ' << _T0_w.translation()(idx);

    // Tw_e matrix
    for(unsigned int c=0; c < 3; c++)
    for(unsigned int r=0; r < 3; r++)
        ss << ' ' << _Tw_e.matrix()(r, c);

    for(unsigned int idx=0; idx < 3; idx++)
        ss << ' ' << _Tw_e.translation()(idx);

    // Read in the Bw matrix
    for(unsigned int r=0; r < 6; r++)
    for(unsigned int c=0; c < 2; c++)
        ss << ' ' << _Bw(r, c);
}

Eigen::Matrix<double, 6, 1> TSR::distance(const Eigen::Affine3d &ee_pose) const {
    Eigen::Matrix<double, 6, 1> dist = Eigen::Matrix<double, 6, 1>::Zero();

    // First compute the pose of the w frame in world coordinates, given the ee_pose
    Eigen::Affine3d w_in_world = ee_pose * _Tw_e_inv;

    // Next compute the pose of the w frame relative to its original pose (as specified by T0_w)
    Eigen::Affine3d w_offset = _T0_w_inv * w_in_world;

    // Now compute the elements of the distance matrix
    dist(0,0) = w_offset.translation()(0);
    dist(1,0) = w_offset.translation()(1);
    dist(2,0) = w_offset.translation()(2);
    dist(3,0) = atan2(w_offset.rotation()(2,1), w_offset.rotation()(2,2));
    dist(4,0) = -asin(w_offset.rotation()(2,0));
    dist(5,0) = atan2(w_offset.rotation()(1,0), w_offset.rotation()(0,0));

    return dist;
}

Eigen::Matrix<double, 6, 1> TSR::displacement(const Eigen::Affine3d &ee_pose) const {

    Eigen::Matrix<double, 6, 1> dist = distance(ee_pose);
    Eigen::Matrix<double, 6, 1> disp = Eigen::Matrix<double, 6, 1>::Zero();

    for(unsigned int idx=0; idx < 6; idx++){
        if(dist(idx,0) < _Bw(idx,0)){
            disp(idx,0) = dist(idx,0) - _Bw(idx,0);
        }else if(dist(idx,0) > _Bw(idx,1)){
            disp(idx,0) = dist(idx,0) - _Bw(idx,1);
        }
    }

    return disp;
}

Eigen::Affine3d TSR::sampleDisplacementTransform(void) const {

    // First sample uniformly betwee each of the bounds of Bw
    std::vector<double> d_sample(6);

    ompl::RNG rng;
    for(unsigned int idx=0; idx < d_sample.size(); idx++){
        if(_Bw(idx,1) > _Bw(idx,0)){
            d_sample[idx] = rng.uniformReal(_Bw(idx,0), _Bw(idx,1));
        }
    }

    Eigen::Affine3d return_tf;
    return_tf.translation() << d_sample[0], d_sample[1], d_sample[2];

    // Convert to a transform matrix
    double roll = d_sample[3];
    double pitch = d_sample[4];
    double yaw = d_sample[5];

    double A = cos(yaw);
    double B = sin(yaw);
    double C = cos(pitch);
    double D = sin(pitch);
    double E = cos(roll);
    double F = sin(roll);
    return_tf.linear() << A*C, A*D*F - B*E, B*F + A*D*E,
        B*C, A*E + B*D*F, B*D*E - A*F,
        -D, C*F, C*E;

    return return_tf;
}

Eigen::Affine3d TSR::sample() const {

    Eigen::Affine3d tf = sampleDisplacementTransform();

    return _T0_w * tf * _Tw_e;
}


#ifndef STAN_MATH_PRIM_FUNCTOR_ODE_RK45_HPP
#define STAN_MATH_PRIM_FUNCTOR_ODE_RK45_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/functor/apply.hpp>
#include <stan/math/prim/functor/coupled_ode_system.hpp>
#include <stan/math/prim/functor/ode_store_sensitivities.hpp>
#include <stan/math/prim/fun/Eigen.hpp>
#include <boost/numeric/odeint.hpp>
#include <ostream>
#include <tuple>
#include <vector>

namespace stan {
namespace math {

/**
 * Solve the ODE initial value problem y' = f(t, y), y(t0) = y0 at a set of
 * times, { t1, t2, t3, ... } using the non-stiff Runge-Kutta 45 solver in
 * Boost.
 *
 * If the system of equations is stiff, ode_bdf will likely be
 * faster.
 *
 * \p f must define an operator() with the signature as:
 *   template<typename T_t, typename T_y, typename... T_Args>
 *   Eigen::Matrix<stan::return_type_t<T_t, T_y, T_Args...>, Eigen::Dynamic, 1>
 *     operator()(const T_t& t, const Eigen::Matrix<T_y, Eigen::Dynamic, 1>& y,
 *     std::ostream* msgs, const T_Args&... args);
 *
 * t is the time, y is the vector-valued state, msgs is a stream for error
 * messages, and args are optional arguments passed to the ODE solve function
 * (which are passed through to \p f without modification).
 *
 * @tparam F Type of ODE right hand side
 * @tparam T_y0 Type of initial condition
 * @tparam T_t0 Type of initial time
 * @tparam T_ts Type of output times
 * @tparam T_Args Types of pass-through parameters
 *
 * @param function_name Calling function name (for printing debugging messages)
 * @param f Right hand side of the ODE
 * @param y0_arg Initial state
 * @param t0 Initial time
 * @param ts Times at which to solve the ODE at. All values must be sorted and
 *   greater than t0.
 * @param relative_tolerance Relative tolerance passed to Boost
 * @param absolute_tolerance Absolute tolerance passed to Boost
 * @param max_num_steps Upper limit on the number of integration steps to
 *   take between each output (error if exceeded)
 * @param[in, out] msgs the print stream for warning messages
 * @param args Extra arguments passed unmodified through to ODE right hand side
 * @return Solution to ODE at times \p ts
 */
template <typename F, typename T_y0, typename T_t0, typename T_ts,
          typename... Args, require_eigen_vector_t<T_y0>* = nullptr>
std::vector<Eigen::Matrix<stan::return_type_t<T_y0, T_t0, T_ts, Args...>,
                          Eigen::Dynamic, 1>>
ode_rk45_tol_impl(const char* function_name, const F& f, const T_y0& y0_arg,
                  T_t0 t0, const std::vector<T_ts>& ts,
                  double relative_tolerance, double absolute_tolerance,
                  long int max_num_steps,  // NOLINT(runtime/int)
                  std::ostream* msgs, const Args&... args) {
  using boost::numeric::odeint::integrate_times;
  using boost::numeric::odeint::make_dense_output;
  using boost::numeric::odeint::max_step_checker;
  using boost::numeric::odeint::no_progress_error;
  using boost::numeric::odeint::runge_kutta_dopri5;
  using boost::numeric::odeint::vector_space_algebra;

  using T_y0_t0 = return_type_t<T_y0, T_t0>;

  Eigen::Matrix<T_y0_t0, Eigen::Dynamic, 1> y0
      = y0_arg.template cast<T_y0_t0>();

  check_finite(function_name, "initial state", y0);
  check_finite(function_name, "initial time", t0);
  check_finite(function_name, "times", ts);

  std::tuple<ref_type_t<Args>...> args_ref_tuple(args...);

  apply(
      [&](const auto&... args_ref) {
        // Code from https://stackoverflow.com/a/17340003
        std::vector<int> unused_temp{
            0,
            (check_finite(function_name, "ode parameters and data", args_ref),
             0)...};
      },
      args_ref_tuple);

  check_nonzero_size(function_name, "initial state", y0);
  check_nonzero_size(function_name, "times", ts);
  check_sorted(function_name, "times", ts);
  check_less(function_name, "initial time", t0, ts[0]);

  check_positive_finite(function_name, "relative_tolerance",
                        relative_tolerance);
  check_positive_finite(function_name, "absolute_tolerance",
                        absolute_tolerance);
  check_positive(function_name, "max_num_steps", max_num_steps);

  using return_t = return_type_t<T_y0, T_t0, T_ts, Args...>;
  // creates basic or coupled system by template specializations
  auto&& coupled_system = apply(
      [&](const auto&... args_ref) {
        return coupled_ode_system<F, T_y0_t0, ref_type_t<Args>...>(f, y0, msgs,
                                                                   args_ref...);
      },
      args_ref_tuple);

  // first time in the vector must be time of initial state
  std::vector<double> ts_vec(ts.size() + 1);
  ts_vec[0] = value_of(t0);
  for (size_t i = 0; i < ts.size(); ++i)
    ts_vec[i + 1] = value_of(ts[i]);

  std::vector<Eigen::Matrix<return_t, Eigen::Dynamic, 1>> y;
  y.reserve(ts.size());
  bool observer_initial_recorded = false;
  size_t time_index = 0;

  // avoid recording of the initial state which is included by the
  // conventions of odeint in the output
  auto filtered_observer
      = [&](const std::vector<double>& coupled_state, double t) -> void {
    if (!observer_initial_recorded) {
      observer_initial_recorded = true;
      return;
    }
    apply(
        [&](const auto&... args_ref) {
          y.emplace_back(ode_store_sensitivities(
              f, coupled_state, y0, t0, ts[time_index], msgs, args_ref...));
        },
        args_ref_tuple);
    time_index++;
  };

  // the coupled system creates the coupled initial state
  std::vector<double> initial_coupled_state = coupled_system.initial_state();

  const double step_size = 0.1;
  try {
    integrate_times(
        make_dense_output(absolute_tolerance, relative_tolerance,
                          runge_kutta_dopri5<std::vector<double>, double,
                                             std::vector<double>, double>()),
        std::ref(coupled_system), initial_coupled_state, std::begin(ts_vec),
        std::end(ts_vec), step_size, filtered_observer,
        max_step_checker(max_num_steps));
  } catch (const no_progress_error& e) {
    throw_domain_error(function_name, "", ts_vec[time_index + 1],
                       "Failed to integrate to next output time (",
                       ") in less than max_num_steps steps");
  }

  return y;
}

/**
 * Solve the ODE initial value problem y' = f(t, y), y(t0) = y0 at a set of
 * times, { t1, t2, t3, ... } using the non-stiff Runge-Kutta 45 solver in
 * Boost.
 *
 * If the system of equations is stiff, ode_bdf will likely be
 * faster.
 *
 * \p f must define an operator() with the signature as:
 *   template<typename T_t, typename T_y, typename... T_Args>
 *   Eigen::Matrix<stan::return_type_t<T_t, T_y, T_Args...>, Eigen::Dynamic, 1>
 *     operator()(const T_t& t, const Eigen::Matrix<T_y, Eigen::Dynamic, 1>& y,
 *     std::ostream* msgs, const T_Args&... args);
 *
 * t is the time, y is the vector-valued state, msgs is a stream for error
 * messages, and args are optional arguments passed to the ODE solve function
 * (which are passed through to \p f without modification).
 *
 * @tparam F Type of ODE right hand side
 * @tparam T_0 Type of initial time
 * @tparam T_ts Type of output times
 * @tparam T_Args Types of pass-through parameters
 *
 * @param f Right hand side of the ODE
 * @param y0_arg Initial state
 * @param t0 Initial time
 * @param ts Times at which to solve the ODE at. All values must be sorted and
 *   greater than t0.
 * @param relative_tolerance Relative tolerance passed to Boost
 * @param absolute_tolerance Absolute tolerance passed to Boost
 * @param max_num_steps Upper limit on the number of integration steps to
 *   take between each output (error if exceeded)
 * @param[in, out] msgs the print stream for warning messages
 * @param args Extra arguments passed unmodified through to ODE right hand side
 * @return Solution to ODE at times \p ts
 */
template <typename F, typename T_y0, typename T_t0, typename T_ts,
          typename... Args, require_eigen_vector_t<T_y0>* = nullptr>
std::vector<Eigen::Matrix<stan::return_type_t<T_y0, T_t0, T_ts, Args...>,
                          Eigen::Dynamic, 1>>
ode_rk45_tol(const F& f, const T_y0& y0_arg, T_t0 t0,
             const std::vector<T_ts>& ts, double relative_tolerance,
             double absolute_tolerance,
             long int max_num_steps,  // NOLINT(runtime/int)
             std::ostream* msgs, const Args&... args) {
  return ode_rk45_tol_impl("ode_rk45_tol", f, y0_arg, t0, ts,
                           relative_tolerance, absolute_tolerance,
                           max_num_steps, msgs, args...);
}

/**
 * Solve the ODE initial value problem y' = f(t, y), y(t0) = y0 at a set of
 * times, { t1, t2, t3, ... } using the non-stiff Runge-Kutta 45 solver in Boost
 * with defaults for relative_tolerance, absolute_tolerance, and max_num_steps.
 *
 * If the system of equations is stiff, ode_bdf will likely be
 * faster.
 *
 * \p f must define an operator() with the signature as:
 *   template<typename T_t, typename T_y, typename... T_Args>
 *   Eigen::Matrix<stan::return_type_t<T_t, T_y, T_Args...>, Eigen::Dynamic, 1>
 *     operator()(const T_t& t, const Eigen::Matrix<T_y, Eigen::Dynamic, 1>& y,
 *     std::ostream* msgs, const T_Args&... args);
 *
 * t is the time, y is the vector-valued state, msgs is a stream for error
 * messages, and args are optional arguments passed to the ODE solve function
 * (which are passed through to \p f without modification).
 *
 * @tparam F Type of ODE right hand side
 * @tparam T_y0 Type of initial time
 * @tparam T_ts Type of output times
 * @tparam T_Args Types of pass-through parameters
 *
 * @param f Right hand side of the ODE
 * @param y0 Initial state
 * @param t0 Initial time
 * @param ts Times at which to solve the ODE at. All values must be sorted and
 *   greather than t0.
 * @param[in, out] msgs the print stream for warning messages
 * @param args Extra arguments passed unmodified through to ODE right hand side
 * @return Solution to ODE at times \p ts
 */
template <typename F, typename T_y0, typename T_t0, typename T_ts,
          typename... Args, require_eigen_vector_t<T_y0>* = nullptr>
std::vector<Eigen::Matrix<stan::return_type_t<T_y0, T_t0, T_ts, Args...>,
                          Eigen::Dynamic, 1>>
ode_rk45(const F& f, const T_y0& y0, T_t0 t0, const std::vector<T_ts>& ts,
         std::ostream* msgs, const Args&... args) {
  double relative_tolerance = 1e-6;
  double absolute_tolerance = 1e-6;
  long int max_num_steps = 1e6;  // NOLINT(runtime/int)

  return ode_rk45_tol_impl("ode_rk45", f, y0, t0, ts, relative_tolerance,
                           absolute_tolerance, max_num_steps, msgs, args...);
}

}  // namespace math
}  // namespace stan
#endif


//
//  kfilter.hpp
//  carma_pack
//
//  Created by Brandon Kelly on 6/27/13.
//  Copyright (c) 2013 Brandon Kelly. All rights reserved.
//

#ifndef __carma_pack__kfilter__
#define __carma_pack__kfilter__

#include <armadillo>
#include <utility>
#include <boost/assert.hpp>

// Global random number generator object, instantiated in random.cpp
extern boost::random::mt19937 rng;

// Object containing some common random number generators.
extern RandomGenerator RandGen;

/*
 Abstract base class for the Kalman Filter of a CARMA(p,q) process.
 */

template <class OmegaType>
class KalmanFilter {
public:
    // Kalman mean and variance
    arma::vec mean;
    arma::vec var;

    // Constructor
    KalmanFilter() {};
    KalmanFilter(arma::vec& time, arma::vec& y, arma::vec& yerr) :
    time_(time), y_(y), yerr_(yerr)
    {
        init();
    }

    // Initialize arrays
    void init() {
        int ndata = time_.n_elem;
        dt_ = time_(arma::span(1,ndata-1)) - time_(arma::span(0,ndata-2));

        // Make sure the time vector is strictly increasing
        if (dt_.min() < 0) {
            std::cout << "Time vector is not sorted in increasing order. Sorting the data vectors..."
            << std::endl;
            // Sort the time values such that dt > 0
            arma::uvec sorted_indices = arma::sort_index(time_);
            time_ = time_.elem(sorted_indices);
            y_ = y_.elem(sorted_indices);
            yerr_ = yerr_.elem(sorted_indices);
            dt_ = time_.rows(1,ndata-1) - time_.rows(0,ndata-2);
        }
        // Make sure there are no duplicate values of time
        if (dt_.min() == 0) {
            std::cout << "Found duplicate values of time, removing them..." << std::endl;
            // Find the unique values of time
            arma::uvec unique_values = 1 + arma::find(dt_ != 0);
            // Add extra row in to keep time(0), y(0), yerr(0)
            unique_values.insert_rows(0, arma::zeros<arma::uvec>(1));
            time_ = time_.elem(unique_values);
            y_ = y_.elem(unique_values);
            yerr_ = yerr_.elem(unique_values);
            ndata = time_.n_elem;
            dt_.set_size(ndata-1);
            dt_ = time_.rows(1,time_.n_elem-1) - time_.rows(0,time_.n_elem-2);
        }

        // Set the size of the Kalman Filter mean and variance vectors
        mean.zeros(time_.n_elem);
        var.zeros(time_.n_elem);
    }

    // Methods to set and get the data
    void SetTime(arma::vec& time) {
        time_ = time;
    }
    arma::vec GetTime() {
        return time_;
    }
    void SetTimeSeries(arma::vec& y) {
        y_ = y;
    }
    arma::vec GetTimeSeries() {
        return y_;
    }
    void SetTimeSeriesErr(arma::vec& yerr) {
        yerr_ = yerr;
    }
    arma::vec GetTimeSeriesErr() {
        return yerr_;
    }

    // Methods to set and get the parameter values
    void SetSigsqr(double sigsqr) {
        sigsqr_ = sigsqr;
    }
    double GetSigsqr() {
        return sigsqr_;
    }
    virtual void SetOmega(OmegaType omega) {
        omega_ = omega;
    }
    OmegaType GetOmega() {
        return omega_;
    }
    virtual void SetMA(arma::vec ma_coefs) {}

    double GetConst() { return yconst_; }
    double GetSlope() { return yslope_; }

    std::vector<double> GetMeanSvec() { return arma::conv_to<std::vector<double> >::from(mean); }
    std::vector<double> GetVarSvec() { return arma::conv_to<std::vector<double> >::from(var); }

    /*
     Methods to perform the Kalman Filter operations
     */
    virtual void Reset() = 0;
    virtual void Update() = 0;
    virtual std::pair<double, double> Predict(double time) = 0;

    void Filter() {
        // Run the Kalman Filter
        Reset();
        for (int i=1; i<time_.n_elem; i++) {
            Update();
        }
    }

    // simulate a CARMA process, conditional on the measured time series
    std::vector<double> Simulate(arma::vec time) {
        // first save old values since we will overwrite them later
        arma::vec time0 = time_;
        arma::vec y0 = y_;
        arma::vec yerr0 = yerr_;

        arma::vec ysimulated(time.n_elem);

        time = arma::sort(time);
        unsigned int insert_idx = 0;
        arma::vec tinsert(1);
        arma::vec dt_insert(1);
        arma::vec yinsert(1);
        arma::vec yerr_insert(1);
        yerr_insert(0) = 0.0;
        for (int i=0; i<time.n_elem; i++) {
            insert_idx = 0;
            // first simulate the value at time(i)
            std::pair<double, double> ypredict = this->Predict(time(i));
            ysimulated(i) = RandGen.normal(ypredict.first, sqrt(ypredict.second));
            // find the index where time_[insert_idx-1] < time(i) < time_[insert_idx]
            while (time_(insert_idx) < time(i)) {
                insert_idx++;
                if (insert_idx == (time_.n_elem)) {
                    break;
                }
            }
            // insert the simulated value into the measured time series array. these values are used in subsequent
            // calls to Predict(time(i)).
            tinsert(0) = time(i);
            time_.insert_rows(insert_idx, tinsert);
            dt_ = time_(arma::span(1,time_.n_elem-1)) - time_(arma::span(0,time_.n_elem-2));
            yinsert(0) = ysimulated(i);
            y_.insert_rows(insert_idx, yinsert);
            yerr_.insert_rows(insert_idx, yerr_insert);
            mean.zeros(time_.n_elem);
            var.zeros(time_.n_elem);
        }

        // restore values of measured time series
        time_ = time0;
        dt_ = time_(arma::span(1,time_.n_elem-1)) - time_(arma::span(0,time_.n_elem-2));
        y_ = y0;
        yerr_ = yerr0;
        // restore the original sizes of the kalman mean and variance arrays
        mean.zeros(time_.n_elem);
        var.zeros(time_.n_elem);

        return arma::conv_to<std::vector<double> >::from(ysimulated);
    }

    // Methods needed for interpolation and backcasting
    virtual void InitializeCoefs(double time, unsigned int itime, double ymean, double yvar) = 0;
    virtual void UpdateCoefs() = 0;

protected:
    // Data
    arma::vec time_;
    arma::vec dt_;
    arma::vec y_;
    arma::vec yerr_;
    // The Kalman Filter parameters
    double sigsqr_;
    OmegaType omega_;
    unsigned int current_index_;
    // linear coefficients needed for doing interpolation or backcasting
    double yconst_, yslope_;
};

/*
 Class to perform the Kalman Filter and related operations for a zero-mean CAR(1) process (see below).
 This class will calculate the Kalman Filter, needed to calculate the log-likelihood in the CAR1
 parameter class. It will also provide interpolated, extrapolated, and simulated values given a
 CAR(1) model and a measured time series.
 */

class KalmanFilter1 : public KalmanFilter<double> {
public:
    // Constructors
    KalmanFilter1() : KalmanFilter<double>() {}
    KalmanFilter1(arma::vec& time, arma::vec& y, arma::vec& yerr) : KalmanFilter<double>(time, y, yerr) {}
    KalmanFilter1(arma::vec& time, arma::vec& y, arma::vec& yerr, double sigsqr, double omega) :
        KalmanFilter<double>(time, y, yerr)
    {
        sigsqr_ = sigsqr;
        omega_ = omega;
    }
    KalmanFilter1(std::vector<double> time, std::vector<double> y, std::vector<double> yerr) :
        KalmanFilter<double>()
    {
        arma::vec armatime = arma::conv_to<arma::vec>::from(time);
        arma::vec armay    = arma::conv_to<arma::vec>::from(y);
        arma::vec armady   = arma::conv_to<arma::vec>::from(yerr);
        time_ = armatime;
        y_ = armay;
        yerr_ = armady;
        init();
    }
    KalmanFilter1(std::vector<double> time, std::vector<double> y, std::vector<double> yerr, double sigsqr, double omega) :
        KalmanFilter<double>()
    {
        arma::vec armatime = arma::conv_to<arma::vec>::from(time);
        arma::vec armay    = arma::conv_to<arma::vec>::from(y);
        arma::vec armady   = arma::conv_to<arma::vec>::from(yerr);
        time_ = armatime;
        y_ = armay;
        yerr_ = armady;
        sigsqr_ = sigsqr;
        omega_ = omega;
        init();
    }

    // Methods to perform the Kalman Filter operations
    void Reset();
    void Update();
    std::pair<double, double> Predict(double time);
    void InitializeCoefs(double time, unsigned int itime, double ymean, double yvar);
    void UpdateCoefs();

    std::vector<double> Simulate(std::vector<double> time) {
        arma::vec armatime = arma::conv_to<arma::vec>::from(time);
        arma::vec armasimulate = KalmanFilter<double>::Simulate(armatime);
        std::vector<double> vecsimulate = arma::conv_to<std::vector<double> >::from(armasimulate);
        return vecsimulate;
    }
};

/*
 Same as KalmanFilter1 but for a CARMA(p,q) process
 */

class KalmanFilterp : public KalmanFilter<arma::cx_vec> {
public:
    // Constructors
    KalmanFilterp() : KalmanFilter<arma::cx_vec>() {}
    KalmanFilterp(arma::vec& time, arma::vec& y, arma::vec& yerr) : KalmanFilter<arma::cx_vec>(time, y, yerr) {init();}
    KalmanFilterp(arma::vec& time, arma::vec& y, arma::vec& yerr, double sigsqr, arma::cx_vec& omega, arma::vec& ma_coefs) :
        KalmanFilter<arma::cx_vec>(time, y, yerr)
    {
        sigsqr_ = sigsqr;
        omega_ = omega; // omega are the roots of the AR characteristic polynomial
        p_ = omega_.n_elem;
        if (p_ > ma_coefs.n_elem) {
            ma_coefs.resize(p_);
        }
        SetMA(ma_coefs);

        // set sizes of arrays
        state_vector_.zeros(p_);
        StateVar_.zeros(p_,p_);
        PredictionVar_.zeros(p_,p_);
        kalman_gain_.zeros(p_);
        rho_.zeros(p_);
        state_const_.zeros(p_);
        state_slope_.zeros(p_);
        rotated_ma_coefs_.zeros(p_);
    }
    KalmanFilterp(std::vector<double> time, std::vector<double> y, std::vector<double> yerr) :
        KalmanFilter<arma::cx_vec>()
    {
        arma::vec armatime = arma::conv_to<arma::vec>::from(time);
        arma::vec armay    = arma::conv_to<arma::vec>::from(y);
        arma::vec armady   = arma::conv_to<arma::vec>::from(yerr);
        time_ = armatime;
        y_ = armay;
        yerr_ = armady;
        init();
    }
   KalmanFilterp(std::vector<double> time, std::vector<double> y, std::vector<double> yerr, double sigsqr,
                 std::vector<std::complex<double> > omega, std::vector<double> ma_coefs) :
        KalmanFilter<arma::cx_vec>()
    {
        arma::vec armatime  = arma::conv_to<arma::vec>::from(time);
        arma::vec armay     = arma::conv_to<arma::vec>::from(y);
        arma::vec armady    = arma::conv_to<arma::vec>::from(yerr);
        arma::cx_vec armaomega = arma::conv_to<arma::cx_vec>::from(omega);
        arma::vec armacoefs = arma::conv_to<arma::vec>::from(ma_coefs);
        time_ = armatime;
        y_ = armay;
        yerr_ = armady;
        sigsqr_ = sigsqr;
        omega_ = armaomega;
        p_ = omega_.n_elem;
        if (p_ > armacoefs.n_elem) {
            armacoefs.resize(p_);
        }
        SetMA(armacoefs);

        // set sizes of arrays
        state_vector_.zeros(p_);
        StateVar_.zeros(p_,p_);
        PredictionVar_.zeros(p_,p_);
        kalman_gain_.zeros(p_);
        rho_.zeros(p_);
        state_const_.zeros(p_);
        state_slope_.zeros(p_);
        rotated_ma_coefs_.zeros(p_);

        init();
    }

    // Set and get and moving average terms
    void SetMA(arma::vec ma_coefs) {
        ma_coefs_ = ma_coefs.st();
    }
    arma::vec GetMA() {
        return ma_coefs_.st();
    }

    // set the AR parameters
    void SetOmega(arma::cx_vec omega) {
        omega_ = omega;
        // resize arrays
        p_ = omega_.n_elem;
        state_vector_.zeros(p_);
        StateVar_.zeros(p_,p_);
        PredictionVar_.zeros(p_,p_);
        kalman_gain_.zeros(p_);
        rho_.zeros(p_);
        state_const_.zeros(p_);
        state_slope_.zeros(p_);
        rotated_ma_coefs_.zeros(p_);
    }

    // Methods to perform the Kalman Filter operations
    void Reset();
    void Update();
    std::pair<double, double> Predict(double time);
    void InitializeCoefs(double time, unsigned int itime, double ymean, double yvar);
    void UpdateCoefs();

    std::vector<double> Simulate(std::vector<double> time) {
        arma::vec armatime = arma::conv_to<arma::vec>::from(time);
        arma::vec armasimulate = KalmanFilter<arma::cx_vec>::Simulate(armatime);
        std::vector<double> vecsimulate = arma::conv_to<std::vector<double> >::from(armasimulate);
        return vecsimulate;
    }

private:
    // parameters
    arma::rowvec ma_coefs_; // moving average terms
    unsigned int p_; // the orders of the CARMA process
    // quantities defining the current state of the kalman filter
    arma::cx_vec state_vector_; // current value of the rotated state vector
    arma::cx_mat StateVar_; // stationary covariance matrix of the rotated state vector
    arma::cx_rowvec rotated_ma_coefs_; // rotated moving average coefficients
    arma::cx_mat PredictionVar_; // covariance matrix of the predicted rotated state vector
    arma::cx_vec kalman_gain_;
    arma::cx_vec rho_;
    double innovation_;
    // linear coefficients needed for doing interpolation or backcasting
    arma::cx_vec state_const_;
    arma::cx_vec state_slope_;
};

#endif /* defined(__carma_pack__kfilter__) */


/**  \file boundingbox.hpp \brief Module for box constrain management */
/*
-----------------------------------------------------------------------------
   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
-----------------------------------------------------------------------------
*/
#ifndef  _BOUNDING_BOX_HPP_
#define  _BOUNDING_BOX_HPP_

// BOOST Libraries
#include <boost/numeric/ublas/vector.hpp>
#include "ublas_elementwise.hpp"

namespace bayesopt {

  namespace utils {
    /** Defines a bounding box or axis-alligned bound constraints. */
    template <class V>
    class BoundingBox
    {
    public:
      BoundingBox(const V &lbound, const V &ubound):
	mLowerBound(lbound), mRangeBound(ubound - lbound)
      {};

      virtual ~BoundingBox(){};

      inline V unnormalizeVector( const V &vin )
      {
	return ublas_elementwise_prod(vin,mRangeBound) + mLowerBound;
      };  // unnormalizeVector

      inline V normalizeVector( const V &vin )
      {
	return ublas_elementwise_div(vin - mLowerBound, mRangeBound);
      }  // normalizeVector

    protected:
      V mLowerBound; ///< Lower bound of the input space
      V mRangeBound; ///< Range (up-low) of the input space
    };

  } //namespace utils

} //namespace bayesopt

#endif


/****************************************************************
  compute_unit_tensor_rmes.cpp

  compute relative unit tensor rmes in spncci basis using recurrance

  WARNING: Will not build since requires old version of
  lgi::GenerateLGIExpansion with stream arguments.

  Anna E. McCoy and Mark A. Caprio
  University of Notre Dame

  12/3/16 (aem): Created.  Based on unit_tensor_test.cpp
  1/6/17 (aem) : Added contraction of unit tensors and interaction
  4/20/17 (aem) : Legacy code removed from build
****************************************************************/
#include <cstdio>
#include <ctime>
#include <fstream>
#include <sys/resource.h>

#include <Eigen/Core>
#include <SymEigsSolver.h>  // Also includes <MatOp/DenseSymMatProd.h>

#include "cppformat/format.h"
#include "am/am.h"

#include "lgi/lgi.h"
#include "lgi/lgi_solver.h"
#include "lsu3shell/lsu3shell_basis.h"
#include "lsu3shell/lsu3shell_rme.h"
#include "mcutils/eigen.h"
#include "sp3rlib/u3coef.h"
#include "sp3rlib/vcs.h"
#include "spncci/unit_tensor.h"
#include "spncci/branching_u3s.h"
#include "spncci/branching_u3lsj.h"
#include "spncci/explicit_construction.h"
#include "u3shell/relative_operator.h"
#include "u3shell/upcoupling.h"

namespace spncci
{
  // typedef std:: map< std::pair<int,int>, std::map<std::pair<int,int>,spncci::UnitTensorSectorsCache >
  //                     > LGIUnitTensorSectorCache;
  typedef spncci::UnitTensorMatricesByIrrepFamily LGIUnitTensorSectorCache;

  // void
  // ConstructSpNCCIBasisExplicit(
  //   const spncci::SpNCCISpace& sp_irrep_vector,
  //   basis::MatrixVector& lgi_expansion_matrix_vector,
  //   const u3shell::SpaceU3SPN& space,
  //   const u3shell::SectorsU3SPN& arel_sectors,
  //   basis::MatrixVector& Arel_matrices,
  //   std::unordered_map<u3::U3,vcs::MatrixCache, boost::hash<u3::U3>>& k_matrix_map
  // )
  // {

  //   for(int irrep_index=0; irrep_index<sp_irrep_vector.size(); ++irrep_index)
  //       {
  //         u3shell::U3SPN lgi_labels=sp_irrep_vector[irrep_index];

  //         const u3::U3& sigma=lgi_labels.U3();
  //         Eigen::MatrixXd& lgi_expansion=lgi_expansion_matrix_vector[irrep_index];

  //         const spncci::SpNCCIIrrepFamily& sp_irrep=sp_irrep_vector[irrep_index];
  //         const sp3r::Sp3RSpace& sp_space=sp_irrep.Sp3RSpace();

  //         // Act A on bra lgi
  //         for(int a=0; a<arel_sectors.size(); ++a)
  //           {
  //             auto& sector=arel_sectors.GetSector(a);
  //             int ket_index=sector.ket_subspace_index();
  //             u3shell::U3SPN ket_subspace_labels=space.GetSubspace(ket_index).labels();
  //             u3::U3 omega=ket_subspace_labels.U3();
  //             HalfInt S=ket_subspace_labels.S();

  //             if(
  //                 (not (omega==sigma))
  //                 ||(S!=lgi_labels.S())
  //                 ||(ket_subspace_labels.Sp()!=lgi_labels.Sp())
  //                 ||(ket_subspace_labels.Sn()!=lgi_labels.Sn())
  //               )
  //               continue;

  //             int bra_index=sector.bra_subspace_index();
  //             u3shell::U3SPN bra_subspace_labels=space.GetSubspace(bra_index).labels();
  //             u3::U3 omegap=bra_subspace_labels.U3();

  //             // Check if omegap is in irrep
  //             if(not sp_space.ContainsSubspace(omegap))
  //             {
  //               std::cout<<omegap.Str()<<" is not in irrep "<<sigma.Str()<<std::endl;
  //               continue;
  //             }

  //             HalfInt Sp=bra_subspace_labels.S();
  //             assert(Sp==S);

  //             Eigen::MatrixXd Kmatrix_inverse=k_matrix_map[sigma][omegap].inverse();
  //             double k_inverse=Kmatrix_inverse(0,0);

  //             Eigen::MatrixXd omega_expansion
  //             =ParitySign(u3::ConjugationGrade(omega.SU3())+u3::ConjugationGrade(lgi_labels.U3().SU3()))
  //               *k_inverse
  //               *Arel_matrices[a]*lgi_expansion;

  //             std::cout<<fmt::format("{} in irrep {} ",omegap.Str(), lgi_labels.Str())<<std::endl;
  //             std::cout<<omega_expansion.transpose()*omega_expansion<<std::endl;
  //             // std::cout<<lgi_expansion.transpose()*lgi_expansion<<std::endl;
  //             // std::cout<<Arel_matrices[a].transpose()*Arel_matrices[a]<<std::endl;
  //             // std::cout<<k_matrix_map[sigma][omegap]*k_matrix_map[sigma][omegap]<<std::endl;

  //           }
  //       }
  // }

  // void
  // ComputeUnitTensorSectorsExplicit(
  //   const spncci::SpNCCISpace& sp_irrep_vector,
  //   basis::MatrixVector& lgi_expansion_matrix_vector,
  //   const u3shell::SpaceU3SPN& space,
  //   const u3shell::SectorsU3SPN& sectors,
  //   const u3shell::SectorsU3SPN& arel_sectors,
  //   basis::MatrixVector& Arel_matrices,
  //   std::unordered_map<u3::U3,vcs::MatrixCache, boost::hash<u3::U3>>& k_matrix_map,
  //   const u3shell::RelativeUnitTensorLabelsU3ST& unit_tensor,
  //   basis::MatrixVector& lsu3shell_operator_matrices,
  //   spncci::LGIUnitTensorSectorCache& unit_tensor_sector_cache_explicit,
  //   double zero_threshold
  // )
  // // Compute unit tensor sectors using SpNCCI basis states explicityly constructed in terms of
  // // lsu3shell states.
  // //
  // // Arguemnts:
  // //
  // {
  //   // REMOVE
  //   int d=0;
  //   // Get operator labels
  //   for(int m=0; m<sp_irrep_vector.size(); ++m)
  //     for(int n=0; n<sp_irrep_vector.size(); ++n)
  //       {
  //         // Get lgi expansion
  //         std::pair<int,int> lgi_pair(m,n);
  //         u3shell::U3SPN bra_lgi_labels=sp_irrep_vector[m];
  //         u3shell::U3SPN ket_lgi_labels=sp_irrep_vector[n];

  //         if(not am::AllowedTriangle(ket_lgi_labels.S(),unit_tensor.S0(), bra_lgi_labels.S()))
  //           continue;

  //         const HalfInt& Nsigmap=bra_lgi_labels.N();
  //         const HalfInt& Nsigma=ket_lgi_labels.N();
  //         Eigen::MatrixXd& bra_lgi_expansion=lgi_expansion_matrix_vector[m];
  //         Eigen::MatrixXd& ket_lgi_expansion=lgi_expansion_matrix_vector[n];
  //         // Act A on bra lgi
  //         for(int s=0; s<sectors.size(); ++s)
  //           {
  //             auto& sector=sectors.GetSector(s);
  //             int bra_index=sector.bra_subspace_index();
  //             int ket_index=sector.ket_subspace_index();
  //             u3shell::U3SPN ket_subspace_labels=space.GetSubspace(ket_index).labels();
  //             u3shell::U3SPN bra_subspace_labels=space.GetSubspace(bra_index).labels();
  //             u3::U3 omega=ket_subspace_labels.U3();
  //             u3::U3 omegap=bra_subspace_labels.U3();
  //             HalfInt S=ket_subspace_labels.S();
  //             HalfInt Sp=bra_subspace_labels.S();
  //             // Checking if omegaS and omegapS are contained in irreps in question
  //             if(
  //                 (omegap.N()<Nsigmap)
  //                 ||(Sp!=bra_lgi_labels.S())
  //                 ||(bra_subspace_labels.Sp()!=bra_lgi_labels.Sp())
  //                 ||(bra_subspace_labels.Sn()!=bra_lgi_labels.Sn())
  //                 ||(omega.N()<Nsigma)
  //                 ||(ket_subspace_labels.Sp()!=ket_lgi_labels.Sp())
  //                 ||(ket_subspace_labels.Sn()!=ket_lgi_labels.Sn())
  //                 ||(ket_subspace_labels.S()!=ket_lgi_labels.S())
  //               )
  //               continue;

  //             if((omegap.N()==Nsigmap)&&(not (bra_subspace_labels==bra_lgi_labels)))
  //               continue;

  //             if((omega.N()==Nsigma)&&(not (ket_subspace_labels==ket_lgi_labels)))
  //               continue;

  //             int rho0=sector.multiplicity_index();
  //             spncci::UnitTensorU3Sector unit_U3Sector(omegap,omega,unit_tensor,rho0);
  //             std::pair<int,int> NnpNn(int(omegap.N()-Nsigmap),int(omega.N()-Nsigma));

  //             Eigen::MatrixXd unit_rmes;
  //             Eigen::MatrixXd& operator_matrix=lsu3shell_operator_matrices[s];

  //             // Act A on ket LGI
  //             if(Nsigmap==(Nsigma+unit_tensor.N0()+2))
  //               {

  //                 int arel_index=arel_sectors.LookUpSectorIndex(ket_index,n,1);
  //                 // Check if there is a valid arel sector that will yield the omegap
  //                 // subspace
  //                 if(arel_index==-1)
  //                   if(u3::OuterMultiplicity(ket_lgi_labels.U3().SU3(),u3::SU3(2,0),omega.SU3()))
  //                     {
  //                       // std::cout<<unit_tensor.Str()<<std::endl;
  //                       // std::cout<<bra_lgi_labels.Str()<<"  "<<omegap.Str()<<Sp<<"  "<<ket_lgi_labels.Str()<<"  "<<omega.Str()
  //                       //   <<S<<"  "<<arel_index<<std::endl;
  //                       // std::cout<<ket_index<<"  "<<n<<std::endl;
  //                     }
  //                 if(arel_index!=-1)
  //                   {
  //                     // std::cout<<"hi"<<std::endl;
  //                     Eigen::MatrixXd Kmatrix_inverse=k_matrix_map[ket_lgi_labels.U3()][omega].inverse();
  //                     double k_inverse=Kmatrix_inverse(0,0);
  //                     Eigen::MatrixXd omega_expansion
  //                     =ParitySign(u3::ConjugationGrade(omega.SU3())+u3::ConjugationGrade(ket_lgi_labels.U3().SU3()))
  //                       *k_inverse*Arel_matrices[arel_index]*ket_lgi_expansion;
  //                     // std::cout<<k_inverse
  //                     // <<std::endl<<std::endl
  //                     // <<Arel_matrices[arel_index]<<std::endl<<std::endl<<ket_lgi_expansion<<std::endl;
  //                     // std::cout<<"operator"<<std::endl;
  //                     // std::cout<<operator_matrix<<std::endl;
  //                     unit_rmes
  //                       =bra_lgi_expansion.transpose()*operator_matrix*omega_expansion;
  //                     // std::cout<<"unit_rmes"<<std::endl<<unit_rmes<<std::endl<<std::endl;
  //                   }
  //               }
  //             // Act A on both lgi
  //             if(Nsigmap==(Nsigma+unit_tensor.N0()))
  //               {

  //                 int arel_index_bra=arel_sectors.LookUpSectorIndex(bra_index,m,1);
  //                 int arel_index_ket=arel_sectors.LookUpSectorIndex(ket_index,n,1);

  //                 // if((arel_index_bra==-1)&&(arel_index_ket==-1))
  //                 //   if(u3::OuterMultiplicity(ket_lgi_labels.U3().SU3(),u3::SU3(2,0),omega.SU3()))
  //                 //     if(u3::OuterMultiplicity(bra_lgi_labels.U3().SU3(),u3::SU3(2,0),omegap.SU3()))
  //                 //       {
  //                 //         std::cout<<unit_tensor.Str()<<std::endl;
  //                 //         std::cout<<bra_lgi_labels.Str()<<"  "<<omegap.Str()<<Sp<<"  "<<ket_lgi_labels.Str()
  //                 //         <<"  "<<omega.Str()<<S<<"  "<<arel_index_bra<<"  "<<arel_index_ket<<std::endl;
  //                 //         std::cout<<bra_index<<"   "<<m<<"  "<<ket_index<<"  "<<n<<std::endl;
  //                 //       }

  //                 if((arel_index_bra!=-1)&&(arel_index_ket!=-1))
  //                   {
  //                     Eigen::MatrixXd Kmatrixp_inverse=k_matrix_map[bra_lgi_labels.U3()][omegap].inverse();
  //                     double kp_inverse=Kmatrixp_inverse(0,0);

  //                     Eigen::MatrixXd Kmatrix_inverse=k_matrix_map[ket_lgi_labels.U3()][omega].inverse();
  //                     double k_inverse=Kmatrix_inverse(0,0);

  //                     Eigen::MatrixXd omegap_expansion
  //                     =ParitySign(u3::ConjugationGrade(omegap.SU3())+u3::ConjugationGrade(bra_lgi_labels.U3().SU3()))
  //                       *kp_inverse*Arel_matrices[arel_index_bra]*bra_lgi_expansion;
  //                     Eigen::MatrixXd omega_expansion
  //                     =ParitySign(u3::ConjugationGrade(omega.SU3())+u3::ConjugationGrade(ket_lgi_labels.U3().SU3()))
  //                       *k_inverse*Arel_matrices[arel_index_ket]*ket_lgi_expansion;

  //                     unit_rmes=omegap_expansion.transpose()*operator_matrix*omega_expansion;
  //                     // if(m==0 && n==0 && d<2)
  //                     //   {
  //                     //     std::cout<<omegap.Str()<<"  "<<unit_tensor.Str()<<"  "<<omega.Str()<<std::endl;
  //                     //     std::cout<<"operator"<<std::endl;
  //                     //     std::cout<<operator_matrix<<std::endl<<std::endl;
  //                     //     std::cout<<"bra "<<std::endl;
  //                     //     std::cout<<(Arel_matrices[arel_index_bra]*bra_lgi_expansion).transpose()<<std::endl<<std::endl;
  //                     //     std::cout<<"ket "<<std::endl;
  //                     //     std::cout<<(Arel_matrices[arel_index_ket]*ket_lgi_expansion)<<std::endl<<std::endl;
  //                     //     std::cout<<"rme"<<std::endl;
  //                     //     std::cout<<(Arel_matrices[arel_index_bra]*bra_lgi_expansion).transpose()
  //                     //     *operator_matrix*Arel_matrices[arel_index_ket]*ket_lgi_expansion
  //                     //     <<std::endl<<"  "<<kp_inverse<<"  "<<k_inverse<<std::endl;
  //                     //     std::cout<<"unit tensors"<<std::endl;
  //                     //     std::cout<<unit_rmes<<std::endl<<std::endl;
  //                     //     ++d;
  //                     //   }
  //                   }
  //               }
  //             // Act A on ket lgi
  //             if(Nsigmap==(Nsigma+unit_tensor.N0()-2))
  //               {

  //                 int arel_index=arel_sectors.LookUpSectorIndex(bra_index,m,1);
  //                 if(arel_index!=-1)
  //                   {
  //                     Eigen::MatrixXd Kmatrix_inverse=k_matrix_map[bra_lgi_labels.U3()][omegap].inverse();
  //                     double k_inverse=Kmatrix_inverse(0,0);

  //                     Eigen::MatrixXd omegap_expansion
  //                     =ParitySign(u3::ConjugationGrade(omegap.SU3())+u3::ConjugationGrade(bra_lgi_labels.U3().SU3()))
  //                       *k_inverse*Arel_matrices[arel_index]*bra_lgi_expansion;

  //                     unit_rmes=omegap_expansion.transpose()
  //                               *operator_matrix
  //                               *ket_lgi_expansion;
  //                   }
  //               }
  //                 if(not CheckIfZeroMatrix(unit_rmes,zero_threshold))
  //                   // If nonzero, save unit tensor rmes to cache
  //                   unit_tensor_sector_cache_explicit[lgi_pair][NnpNn][unit_U3Sector]
  //                      =unit_rmes;
  //           }
  //       }

  // }
}// end namespace

int main(int argc, char **argv)
{
      // //REMOVE
      // int testb=3;
      // int testk=3;

  u3::U3CoefInit();
  //unit tensor cache
  u3::UCoefCache u_coef_cache;
  u3::PhiCoefCache phi_coef_cache;

	u3::g_u_cache_enabled = true;
  double zero_threshold=1e-8;
  // For GenerateRelativeUnitTensors
  // should be consistant with lsu3shell tensors
  int T0=0;
  int J0=-1;
  // parse arguments
  if (argc<8)
    {
      std::cout << "Syntax: A twice_Nsigma0 Nsigma0_ex_max N1B Nmax <basis filename> <Nrel filename> <Brel filename> <Arel filename>"
                << std::endl;
      std::exit(1);
    }
  int A = std::stoi(argv[1]);
  int twice_Nsigma0= std::stoi(argv[2]);
  int Nsigma0_ex_max=std::stoi(argv[3]);
  int N1b=std::stoi(argv[4]);
  int Nmax = std::stoi(argv[5]);
  std::string lsu3_filename = argv[6];
  std::string nrel_filename = argv[7];
  std::string brel_filename = argv[8];
  std::string arel_filename = argv[9];

  HalfInt Nsigma_0=HalfInt(twice_Nsigma0,2);
  //initializing map that will store map containing unit tensors {SpIrrep pair : {}
  // inner map is keyed by unit tensor matrix element labels of type UnitTensorRME
  // SpIrrep pair -> NnpNn -> UnitTensorRME -> v'v subsector
  spncci::LGIUnitTensorSectorCache unit_tensor_sector_cache;
  // Texting cache for rme's computed from explicit basis construction
  spncci::LGIUnitTensorSectorCache unit_tensor_sector_cache_explicit;

  //////////////////////////////////////////////////////////////////////////////////////////
  // Get LGI's and populate sector map with LGI rme's
  //////////////////////////////////////////////////////////////////////////////////////////
  // Generating LGI matrix elements
  lsu3shell::LSU3ShellBasisTable basis_table;
  lsu3shell::U3SPNBasisLSU3Labels basis_provenance;
  u3shell::SpaceU3SPN space;
  // Read in lsu3shell basis
  lsu3shell::ReadLSU3ShellBasis(Nsigma_0,lsu3_filename, basis_table, basis_provenance, space);

  // // Writing out lsu3shell basis
  // std::cout<<"lsu3shell basis"<<std::endl;
  // for(int subspace_index=0; subspace_index<space.size(); ++subspace_index)
  //   {
  //     const u3shell::SubspaceU3SPN& subspace = space.GetSubspace(subspace_index);
  //     std::cout
  //       << fmt::format("subspace {} labels {} dim {}",
  //                      subspace_index,
  //                      subspace.U3SPN().Str(),
  //                      subspace.size()
  //         )
  //       << std::endl;
  //   }

  //Get LGI expansion by solving for null space of Brel and Ncm
  std::ifstream is_brel(brel_filename.c_str());
  std::ifstream is_nrel(nrel_filename.c_str());
  lgi::MultiplicityTaggedLGIVector lgi_vector;
  basis::MatrixVector lgi_expansion_matrix_vector;
  lgi::GenerateLGIExpansion(A,Nsigma_0,basis_table,space, is_brel,is_nrel,lgi_vector,lgi_expansion_matrix_vector);
  is_brel.close();
  is_nrel.close();

  // Read in Arel for unit tensor rme check
  // TODO: comment out once rme's are validated
  basis::MatrixVector Arel_matrices;
  u3shell::OperatorLabelsU3ST arel_labels(2,u3::SU3(2,0),0,0,0);
  u3shell::SectorsU3SPN arel_sectors(space,arel_labels,true);

  std::ifstream is_arel(arel_filename.c_str());
  if(not is_arel)
    std::cout<<fmt::format("{} not found",arel_filename)<<std::endl;

  lsu3shell::ReadLSU3ShellRMEs(
    is_arel,arel_labels,basis_table,space,
    arel_sectors,Arel_matrices);
  is_arel.close();

  // for(auto arel : Arel_matrices)
  //   std::cout<<arel<<std::endl<<std::endl;

  // Extract LGI labels and store in lgi_vector
  // lgi::GetLGILabels(Nsigma_0,space,lgi_expansion_matrix_vector, lgi_vector);
  int i=0;
  for(auto lgi_tagged : lgi_vector)
  {
    std::cout<<i<<"  "<<lgi_tagged.Str()<<"  "<<lgi_tagged.tag<<std::endl;
    ++i;
  }

  // Setting up the symplectic basis containers
  spncci::SigmaIrrepMap sigma_irrep_map;
  spncci::SpNCCISpace sp_irrep_vector;
  spncci::NmaxTruncator truncator(Nsigma_0,Nmax);
  // Generating sp3r irreps
  spncci::GenerateSpNCCISpace(lgi_vector,truncator,sp_irrep_vector,sigma_irrep_map);

  //////////////////////////////////////////////////////////////////////////////////////////
  //Precomputing Kmatrices
  //////////////////////////////////////////////////////////////////////////////////////////
  std::unordered_set<u3::U3,boost::hash<u3::U3> >sigma_set;
  for(int l=0; l<sp_irrep_vector.size(); l++)
    {
      sigma_set.insert(sp_irrep_vector[l].sigma());
    }
  std::unordered_map<u3::U3,vcs::MatrixCache, boost::hash<u3::U3>> k_matrix_map;
  for( const auto& s : sigma_set)
    {
      vcs::MatrixCache& k_map=k_matrix_map[s];
      // int Nex=int(s.N()-Nsigma_0);
      // vcs::GenerateKMatricesOpenMP(sigma_irrep_map[s], k_map);
      bool is_intrinsic=true;
      vcs::GenerateKMatrices(sigma_irrep_map[s], k_map,is_intrinsic);

      std::cout<<"kmap "<<s.Str()<<std::endl;
      for(auto it=k_map.begin(); it!=k_map.end(); ++it)
        std::cout<<it->first.Str()<<"  "<<it->second<<std::endl;
    }

  // Labels of relative unit tensors computed between LGI's
  std::vector<u3shell::RelativeUnitTensorLabelsU3ST> LGI_unit_tensor_labels;
  std::cout<<"Nsigma0_ex_max "<<Nsigma0_ex_max<<std::endl;

  //////////////////////////////////////////////////////////////////////////////////////////
  //Explicit construction of basis
  //////////////////////////////////////////////////////////////////////////////////////////
  // std::cout<<"Explicit construction of basis"<<std::endl;
  // basis::MatrixVector spncci_expansions;
  // spncci::BabySpNCCISpace baby_spncci_space2(sp_irrep_vector);
  // ConstructSpNCCIBasisExplicit(space,sp_irrep_vector,lgi_expansion_matrix_vector,
  //     baby_spncci_space2,k_matrix_map,
  //     arel_sectors,Arel_matrices,
  //     spncci_expansions
  //   );

  //////////////////////////////////////////////////////////////////////////////////////////
  //Transform unit tensors
  //////////////////////////////////////////////////////////////////////////////////////////

  // -1 is all J0, T0=0 and false->don't restrict N0 to positive (temp)
  u3shell::GenerateRelativeUnitTensorLabelsU3ST(Nsigma0_ex_max,N1b, LGI_unit_tensor_labels,J0,T0,false);

  // For each operator, transform from lsu3shell basis to spncci basis
  std::cout<<"Number of lgi unit tensors "<<LGI_unit_tensor_labels.size()<<std::endl;

  for(int i=0; i<LGI_unit_tensor_labels.size(); ++i)
    {
      //Get unit tensor labels
      u3shell::RelativeUnitTensorLabelsU3ST unit_tensor(LGI_unit_tensor_labels[i]);
      // std::cout<<"unit tensor "<<i<<"  "<<unit_tensor.Str()<<std::endl;
      u3shell::OperatorLabelsU3ST operator_labels(unit_tensor);
      // Generate sectors from labels
      u3shell::SectorsU3SPN operator_sectors(space,operator_labels,false);
      // Read in lsu3shell rme's of unit tensor
      std::ifstream is_operator(fmt::format("relative_unit_{:06d}.rme",i));
      // If operator is not found, print name and continue;
      if(not is_operator)
        {
          std::cout<<fmt::format("relative_unit_{:06d}.rme not found",i)<<std::endl;
          continue;
        }

      // Read in operator from file
      // std::cout<<fmt::format("Transforming relative_unit_{:06d}.rme",i)<<std::endl;
      // std::cout<< LGI_unit_tensor_labels[i].Str()<<std::endl;
      // std::cout<<"  Reading in rmes"<<std::endl;
      basis::MatrixVector lsu3shell_operator_matrices(operator_sectors.size());
      lsu3shell::ReadLSU3ShellRMEs(
        is_operator,operator_labels, basis_table,space,
        operator_sectors,lsu3shell_operator_matrices
        );

        basis::MatrixVector spncci_operator_matrices;
        lgi::TransformOperatorToSpBasis(
          operator_sectors,lgi_expansion_matrix_vector,
          lsu3shell_operator_matrices,spncci_operator_matrices
          );

      // explicit basis construction unit tensor matrix elements
      // for comparison with Sp-NCCI
      //
      // spncci::ComputeUnitTensorSectorsExplicit(
      //   unit_tensor,space,operator_sectors,lsu3shell_operator_matrices,
      //   baby_spncci_space2,spncci_expansions,unit_tensor_sector_cache_explicit);

      // std::cout<<"traversing the explicit map"<<std::endl;
      //   for(auto it=unit_tensor_sector_cache_explicit.begin(); it!=unit_tensor_sector_cache_explicit.end(); ++it)
      //     {
      //       if((it->first.first!=0)||(it->first.second!=0))
      //         continue;
      //       std::cout<<it->first.first<<"  "<<it->first.second<<std::endl;
      //       for(auto it2=it->second.begin(); it2!=it->second.end(); ++it2)
      //         {
      //           std::cout<<"  "<<it2->first.first<<"  "<<it2->first.second<<std::endl;
      //           for(auto it3=it2->second.begin(); it3!=it2->second.end();++ it3)
      //             {
      //               std::cout<<it3->first.Str()<<std::endl;
      //               std::cout<<it3->second<<std::endl;
      //             }
      //         }
      //     }

      // std::cout<<"lgi expansions"<<std::endl;
      // for(auto matrix :lgi_expansion_matrix_vector)
      //   std::cout<<matrix<<std::endl<<std::endl;

      // Populate unit tensor map with sectors
      // std::cout<<"Populating lgi sectors"<<std::endl;
      std::pair<int,int> N0_pair(0,0);

      for(int s=0; s<operator_sectors.size(); ++s)
        {
          int i=operator_sectors.GetSector(s).bra_subspace_index();
          int j=operator_sectors.GetSector(s).ket_subspace_index();
          int rho0=operator_sectors.GetSector(s).multiplicity_index();
          std::pair<int,int>sp_irrep_pair(i,j);
          u3::U3 sigmap=sp_irrep_vector[i].sigma();
          u3::U3 sigma=sp_irrep_vector[j].sigma();

          // //REMOVE
          // if(sp_irrep_vector[i].Sp()!=HalfInt(1,2) || sp_irrep_vector[i].Sn()!=HalfInt(1,2) || sp_irrep_vector[i].S()!=0)
          //   continue;
          // if(sp_irrep_vector[j].Sp()!=HalfInt(1,2) || sp_irrep_vector[j].Sn()!=HalfInt(1,2) || sp_irrep_vector[j].S()!=0)
          //   continue;

          if(Nmax==0 && sigma.N()>Nsigma_0)
            continue;
          if(Nmax==0 && sigmap.N()>Nsigma_0)
            continue;
          spncci::UnitTensorU3Sector u3_sector(sigmap,sigma,unit_tensor,rho0);
          if(not CheckIfZeroMatrix(spncci_operator_matrices[s],zero_threshold))
            {
              unit_tensor_sector_cache[sp_irrep_pair][N0_pair][u3_sector]
                  =spncci_operator_matrices[s];

              unit_tensor_sector_cache_explicit[sp_irrep_pair][N0_pair][u3_sector]
                  =spncci_operator_matrices[s];

              // if((i==0) && (j==0))
              // {
              // std::cout<<i<<"  "<<j<<std::endl;
              // std::cout<<u3_sector.Str()<<std::endl;
              // std::cout<<spncci_operator_matrices[s]<<std::endl;
              // }
            }
        }
    }

  // spncci::ConstructSpNCCIBasisExplicit(sp_irrep_vector,
  //   lgi_expansion_matrix_vector,
  //   space,
  //   arel_sectors,
  //   Arel_matrices,
  //   k_matrix_map
  // );

  // for(auto it=unit_tensor_sector_cache.begin(); it!=unit_tensor_sector_cache.end(); ++it)
  //   {
  //     std::cout<<"Irreps "<<it->first.first<<" and "<<it->first.second<<std::endl;
  //   }

  // std::cout<<"traversing the map"<<std::endl;
  // auto it_ending=unit_tensor_sector_cache.begin();
  // for(int i=0; i<unit_tensor_sector_cache.size(); ++i)
  //   it_ending++;

  // for(auto it=unit_tensor_sector_cache.begin(); it!=unit_tensor_sector_cache.end(); ++it)
  //   {

  //     // //REMOVE
  //     // if(it->first.first!=0 || it->first.second!=3)
  //     //   continue;
  //     // //

  //     std::cout<<it->first.first<<"  "<<it->first.second<<std::endl;

  //     for(auto it2=it->second.begin(); it2!=it->second.end(); ++it2)
  //       {
  //         std::cout<<"  "<<it2->first.first<<"  "<<it2->first.second<<std::endl;
  //         for(auto it3=it2->second.begin(); it3!=it2->second.end();++ it3)
  //           {
  //             std::cout<<it3->first.Str()<<std::endl;
  //             std::cout<<it3->second<<std::endl;
  //           }
  //       }
  //   }

  std::cout<<"Entering the recurrence"<<std::endl;
  //////////////////////////////////////////////////////////////////////////////////////////
  // Computing unit tensor sectors
  //////////////////////////////////////////////////////////////////////////////////////////
  // if true, unit tensors restricted to N0>=0
  // Nrel_max=Nmax+2N1b

  std::map<int,std::vector<u3shell::RelativeUnitTensorLabelsU3ST>> unit_tensor_labels;
  u3shell::GenerateRelativeUnitTensorLabelsU3ST(Nmax, N1b, unit_tensor_labels, J0,T0, false);
  if(Nmax!=0)
  {
    std::map<int,double> timing_map;
    std::map<int,std::vector<std::pair<int,int>>> lgi_distribution;
    std::map<int,std::vector<int>>  sector_count_map;
    std::map<int,std::vector<double>> individual_times;
    std::map<int,u3::UCoefCache> u_cache_map;
    std::pair<int,int> N0_pair(0,0);
    int num_nodes=1;
    int counter=0;
    for(int n=0; n<num_nodes; ++n)
      timing_map[n]=0;

    for(auto it=unit_tensor_sector_cache.begin(); it!=unit_tensor_sector_cache.end(); ++it)
    {
      clock_t start_time=std::clock();

      // //REMOVE
      // if((it->first.first!=3 ) || (it->first.second!=0))
      //   continue;

      int node=counter%num_nodes;
      assert(node<num_nodes);
      //Timing data
      spncci::GenerateUnitTensorMatrix(
        N1b,Nmax,it->first,sp_irrep_vector,u_cache_map[node], phi_coef_cache,k_matrix_map,
        unit_tensor_labels,unit_tensor_sector_cache);

      double duration=(std::clock()-start_time)/(double) CLOCKS_PER_SEC;
      int num_sector=unit_tensor_sector_cache[it->first][N0_pair].size();

      timing_map[node]+=duration;
      individual_times[node].push_back(duration);
      lgi_distribution[node].push_back(it->first);
      sector_count_map[node].push_back(num_sector);
      counter++;

    }
    // std::cout<<"individual pairs"<<std::endl;
    // for(int n=0; n<num_nodes; ++n)
    //   {
    //     std::cout<<"node"<<n<<std::endl;
    //     int i_stop=lgi_distribution[n].size();
    //     for(int i=0; i<i_stop; ++i)
    //       {
    //         std::cout<<lgi_distribution[n][i].first<<" "<<lgi_distribution[n][i].second<<"  "
    //         <<sector_count_map[n][i]<<"  "<<individual_times[n][i]<<std::endl;
    //       }
    //     std::cout<<"  "<<std::endl;

    //   }

    // std::cout<<"summarizing"<<std::endl;
    // for(int n=0; n<num_nodes; ++n)
    //   {
    //     std::cout<<"node "<<n<<std::endl;
    //     std::cout<<" time "<<timing_map[n]<<std::endl;
    //     std::cout<<" U cache size "<<u_cache_map[n].size()<<std::endl;
    //   }
  }
  bool assert_trap=true;

  std::cout<<"exiting the recurrence"<<std::endl;
  // std::cout<<"traversing the map"<<std::endl;
  // std::cout<<"cache size "<<unit_tensor_sector_cache.size()<<std::endl;
  for(auto it=unit_tensor_sector_cache_explicit.begin(); it!=unit_tensor_sector_cache_explicit.end(); ++it)
    {
      // if(it->first.first!=3 || it->first.second!=0)
      //   continue;
      // std::cout<<it->first.first<<"  "<<it->first.second<<std::endl;
      for(auto it2=it->second.begin(); it2!=it->second.end(); ++it2)
        {
          if(it2->first.first!=0)
            continue;
          if(it2->first.second!=2)
            continue;
          // std::cout<<"  "<<it2->first.first<<"  "<<it2->first.second<<std::endl;
          auto& explicit_cache=unit_tensor_sector_cache[it->first][it2->first];
          for(auto it3=it2->second.begin(); it3!=it2->second.end();++ it3)
            {
              assert(assert_trap);

              int rho0;
              u3shell::RelativeUnitTensorLabelsU3ST tensor;
              std::tie(std::ignore,std::ignore,tensor,rho0)=it3->first.Key();
              // if(not (tensor.x0()==u3::SU3(0,0)))
              //   continue;

              // std::cout<<"     "<<it3->first.Str()<<std::endl;
              // std::cout<<"       "<<it3->second<<std::endl;

              if(explicit_cache.count(it3->first))
              {
                Eigen::MatrixXd& recurrence_matrix=it3->second;

                Eigen::MatrixXd& explicit_matrix=explicit_cache[it3->first];
                if(not mcutils::IsZero(recurrence_matrix-explicit_matrix,1e-3))
                {
                  u3::U3 omega,omegap;
                  std::tie(omegap,omega,std::ignore, std::ignore)=it3->first.Key();
                  // if((omegap.N()==15)&&(omega.N()==15))
                  //   continue;
                  // // std::cout<<it->first.first<<"  "<<it->first.second<<std::endl;

                  // if((it->first.first!=0 )|| (it->first.second!=24))
                  //   continue;

                  // if((it->first.first!=0 )&& (it->first.second!=0))
                  //   continue;
                  // if((it->first.first!=24 )&& (it->first.second!=24))
                  //   continue;

                  std::cout<<it->first.first<<"  "<<it->first.second<<std::endl;
                  std::cout<<"  "<<it2->first.first<<"  "<<it2->first.second<<std::endl;
                  std::cout<<"     "<<it3->first.Str()<<std::endl;

                  std::cout<<"recurrence"<<std::endl;
                  std::cout<<recurrence_matrix<<std::endl<<std::endl;
                  std::cout<<"explicit"<<std::endl;
                  std::cout<<explicit_matrix<<std::endl<<std::endl;

                  Eigen::MatrixXd ratios(recurrence_matrix.rows(),recurrence_matrix.cols());
                  for(int i=0; i<recurrence_matrix.rows(); ++i)
                    for(int j=0; j<recurrence_matrix.cols(); ++j)
                      ratios(i,j)=recurrence_matrix(i,j)/explicit_matrix(i,j);

                  std::cout<<ratios<<std::endl<<std::endl;

                  // assert_trap=false;
                }
              }
              else
              {
                if(mcutils::IsZero(it3->second,1e-3))
                  continue;
                std::cout<<"Explicit sector not found "<<std::endl;
                std::cout<<it->first.first<<"  "<<it->first.second<<std::endl;
                std::cout<<"  "<<it2->first.first<<"  "<<it2->first.second<<std::endl;
                std::cout<<"     "<<it3->first.Str()<<std::endl;
                std::cout<<"     "<<it3->second<<std::endl<<std::endl;
              }
            }
        }
     }

  //////////////////////////////////////////////////////////////////////////////////////////
  // // Getting interaction
  // //////////////////////////////////////////////////////////////////////////////////////////
  // //TODO make input
  // std::string interaction_file="JISP16_u3st.dat";
  // std::string interaction_file= "K2intr_u3st.dat";

  // std::string interaction_file="Tintr_hw20.0_Nmax10_u3st.dat";
  // std::string interaction_file="r2intr_hw20.0_Nmax02_u3st.dat";
  // std::string interaction_file="spin_hw20.0_Nmax04_u3st.dat";
  std::string interaction_file="Hamiltonian_hw20.0_Nmax04_u3st.dat";
  // std::string interaction_file="Nintr_u3st.dat";

  // std::string interaction_file="trel_SU3_Nmax06.dat";
  // std::string interaction_file= "Trel_upcouled";

  // std::string interaction_file="id_SU3_Nmax16.dat";

  // std::string interaction_file="Identity_u3st.dat";

  // std::string interaction_file="unit.dat";
  std::ifstream interaction_stream(interaction_file.c_str());
  assert(interaction_stream);

  std::cout<<"reading in interaction"<<std::endl;
  u3shell::RelativeRMEsU3ST interaction_rme_cache;
  u3shell::ReadRelativeOperatorU3ST(interaction_stream, interaction_rme_cache);

  // for(auto it=interaction_rme_cache.begin(); it!=interaction_rme_cache.end(); ++it)
  //   std::cout<<it->second<<std::endl;

  std::vector<u3shell::IndexedOperatorLabelsU3S> operator_u3s_list;
  u3shell::GetInteractionTensorsU3S(interaction_rme_cache,operator_u3s_list);

  // get baby SpNCCI space
  spncci::BabySpNCCISpace baby_spncci_space(sp_irrep_vector);

  // Get U3S space
  spncci::SpaceU3S u3s_space(baby_spncci_space);
  // Storage for sectors, value gives sector index

  // spncci::SectorLabelsU3SCache u3s_sectors;
  std::vector<spncci::SectorLabelsU3S> u3s_sector_vector;
  // for(auto tensor : operator_u3s_list)
  //   {
  //     int kappa0, L0;
  //     u3shell::OperatorLabelsU3S tensor_u3st;
  //     std::tie(tensor_u3st,kappa0,L0)=tensor;
  //     std::cout<<tensor_u3st.Str()<<"  "<<kappa0<<"  "<<L0<<std::endl;
  //   }

  spncci::GetSectorsU3S(u3s_space,operator_u3s_list,u3s_sector_vector);

  std::cout<<"contracting"<<std::endl;
  //////////////////////////////////////////////////////////////////////////////////////////////
  // Contracting
  //////////////////////////////////////////////////////////////////////////////////////////////
  basis::MatrixVector matrix_vector;
  basis::MatrixVector matrix_vector_explicit;

  spncci::ContractAndRegroupU3S(
    Nmax, N1b,u3s_sector_vector,interaction_rme_cache,baby_spncci_space,
    u3s_space,unit_tensor_sector_cache, matrix_vector);

  // spncci::ContractAndRegroupU3S(
  //   Nmax, N1b,u3s_sector_vector,interaction_rme_cache,baby_spncci_space,
  //   u3s_space,unit_tensor_sector_cache_explicit, matrix_vector_explicit);

  ZeroOutMatrix(matrix_vector,1e-4);
  // ZeroOutMatrix(matrix_vector_explicit,1e-4);

  basis::MatrixVector difference_vector;
  // for(int i=0; i<matrix_vector.size();++i)
  //   difference_vector.push_back(matrix_vector_explicit[i]-matrix_vector[i]);
  // ZeroOutMatrix(difference_vector,1e-4);

  // std::cout<<"printing"<<std::endl;
  // for(int i=0; i<u3s_space.size(); ++i)
  //   std::cout<<i<<"  "<<u3s_space.GetSubspace(i).labels().Str()<<std::endl;
  // for(int s=0; s<matrix_vector.size();  ++s)
  //   {
  //     if (not CheckIfZeroMatrix(matrix_vector[s], 1e-4))
  //     {

  //       std::cout<<u3s_sector_vector[s].Str()<<"  "<<s<<std::endl;
  //       std::cout<<matrix_vector[s]<<std::endl<<std::endl;

  //       // Eigen::MatrixXd difference=matrix_vector_explicit[s]-matrix_vector[s];

  //       // std::cout<<matrix_vector[s]<<std::endl<<std::endl;
  //       // std::cout<<matrix_vector_explicit[s]<<std::endl<<std::endl;

  //       // std::cout<<difference_vector[s]<<std::endl<<std::endl;
  //     }
  //   }

  HalfInt J=1;
  J0=0;
  spncci::SpaceLS space_LS(u3s_space,J);
  std::cout<<"target_space size "<<space_LS.size()<<std::endl;
  basis::MatrixVector sectors_LS;

  std::vector<spncci::OperatorLabelsLS> tensor_labels_LS;
  spncci::GenerateOperatorLabelsLS(J0, tensor_labels_LS);
  std::vector<spncci::SectorLabelsLS> sector_labels_LS;

  // for(auto tensor_labels : tensor_labels_LS)
  //   std::cout<<"tensor "<<tensor_labels.first<<"  "<<tensor_labels.second<<std::endl;

  spncci::GetSectorsLS(space_LS,space_LS, tensor_labels_LS,sector_labels_LS);

  // std::cout<<"sectors"<<std::endl;
  // for(auto sector :target_sector_labels_LS)
  //   std::cout<<sector.Str()<<std::endl;

  spncci::ContractAndRegroupLSJ(
    J,J0,J,
    u3s_space,u3s_sector_vector,matrix_vector,
    space_LS,space_LS,sector_labels_LS,sectors_LS
  );

  // ZeroOutMatrix(sectors_LS,1e-4);
  // std::cout<<"printing LS"<<std::endl;
  // for(int i=0; i<space_LS.size(); ++i)
  //   std::cout<<i<<"  "<<space_LS.GetSubspace(i).Str()<<std::endl;

  // for(int s=0; s<sectors_LS.size();  ++s)
  //   {
  //     if (not CheckIfZeroMatrix(sectors_LS[s], 1e-4))
  //     {
  //       std::cout<<sector_labels_LS[s].Str()<<std::endl;
  //       std::cout<<sectors_LS[s]<<std::endl<<std::endl;
  //     }
  //   }

  Eigen::MatrixXd operator_matrix;

  ConstructOperatorMatrix(space_LS,space_LS,sector_labels_LS,
    sectors_LS,operator_matrix);

  // for(auto it=unit_tensor_labels.begin(); it!=unit_tensor_labels.end(); ++it)
  //   for(auto tensor : it->second)
  //     std::cout<<tensor.Str()<<std::endl;

  // Mask matrix for sparsity
  int num_nonzero=0;
  int total=0;
  // for(int i=0; i<operator_matrix.rows(); ++i)
  //   for(int j=0; j<operator_matrix.cols(); ++j)
  //     {
  //       if(fabs(operator_matrix(i,j))>10e-6)
  //       {
  //         operator_matrix(i,j)=1;
  //         num_nonzero++;
  //       }
  //       else
  //         operator_matrix(i,j)=0;
  //       total++;
  //     }

  // std::cout<<operator_matrix<<std::endl;

  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(operator_matrix);
  for(int i=0; i<10; ++i)
    std::cout<<"eigenvalues are "<<es.eigenvalues()[i]<<std::endl;

    // Spectra::DenseSymMatProd<double> op(operator_matrix);

    // // Construct eigen solver object, requesting the largest three eigenvalues
    // Spectra::SymEigsSolver< double, Spectra::SMALLEST_ALGE, Spectra::DenseSymMatProd<double> > eigs(&op, 5, 6);

    // // Initialize and compute
    // eigs.init();
    // int nconv = eigs.compute();

    // // Retrieve result
    // Eigen::VectorXd evalues;
    // if(eigs.info() == Spectra::SUCCESSFUL)
    //     evalues = eigs.eigenvalues();
    // else
    //   std::cout<<"failed"<<std::endl;

    // std::cout << "Eigenvalues found:\n" << evalues << std::endl;

  // std::cout<<std::endl<<"total "<<total<<"  non-zero "<<num_nonzero<<"  percentage "<<1.*num_nonzero/total<<std::endl;
}


//
// Copyright (c) 2017 CNRS
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//

#ifndef EIQUADPROGFAST_HH_
#define EIQUADPROGFAST_HH_

#include <Eigen/Dense>
#include <Eigen/Sparse>

#define OPTIMIZE_STEP_1_2  // compute s(x) = ci^T * x + ci0
#define OPTIMIZE_COMPUTE_D
#define OPTIMIZE_UPDATE_Z
#define OPTIMIZE_HESSIAN_INVERSE
#define OPTIMIZE_UNCONSTR_MINIM
//#define TRACE_SOLVER 1

//#define USE_WARM_START
//#define PROFILE_EIQUADPROG

//#define DEBUG_STREAM(msg) std::cout<<msg;
#define DEBUG_STREAM(msg)

#ifdef PROFILE_EIQUADPROG
#define START_PROFILER_EIQUADPROG_FAST START_PROFILER
#define STOP_PROFILER_EIQUADPROG_FAST STOP_PROFILER
#else
#define START_PROFILER_EIQUADPROG_FAST
#define STOP_PROFILER_EIQUADPROG_FAST
#endif

#define EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION "EIQUADPROG_FAST Chowlesky dec"
#define EIQUADPROG_FAST_CHOWLESKY_INVERSE "EIQUADPROG_FAST Chowlesky inv"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR "EIQUADPROG_FAST ADD_EQ_CONSTR"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR_2 "EIQUADPROG_FAST ADD_EQ_CONSTR_2"
#define EIQUADPROG_FAST_STEP_1 "EIQUADPROG_FAST STEP_1"
#define EIQUADPROG_FAST_STEP_1_1 "EIQUADPROG_FAST STEP_1_1"
#define EIQUADPROG_FAST_STEP_1_2 "EIQUADPROG_FAST STEP_1_2"
#define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
#define EIQUADPROG_FAST_STEP_2 "EIQUADPROG_FAST STEP_2"
#define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
#define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
#define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C"

#define DEFAULT_MAX_ITER 1000

namespace tsid {

namespace solvers {

/**
 * Possible states of the solver.
 */
enum EiquadprogFast_status {
  EIQUADPROG_FAST_OPTIMAL = 0,
  EIQUADPROG_FAST_INFEASIBLE = 1,
  EIQUADPROG_FAST_UNBOUNDED = 2,
  EIQUADPROG_FAST_MAX_ITER_REACHED = 3,
  EIQUADPROG_FAST_REDUNDANT_EQUALITIES = 4
};

class EiquadprogFast {
 public:
  typedef Eigen::MatrixXd MatrixXd;
  typedef Eigen::VectorXd VectorXd;
  typedef Eigen::VectorXi VectorXi;

  typedef Eigen::SparseMatrix<double> SpMat;
  typedef Eigen::SparseVector<double> SpVec;
  typedef Eigen::SparseVector<int> SpVeci;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  EiquadprogFast();
  virtual ~EiquadprogFast();

  void reset(int dim_qp, int num_eq, int num_ineq);

  int getMaxIter() const { return m_maxIter; }

  bool setMaxIter(int maxIter) {
    if (maxIter < 0) return false;
    m_maxIter = maxIter;
    return true;
  }

  /**
   * @return The size of the active set, namely the number of
   * active constraints (including the equalities).
   */
  int getActiveSetSize() const { return q; }

  /**
   * @return The number of active-set iteratios.
   */
  int getIteratios() const { return iter; }

  /**
   * @return The value of the objective function.
   */
  double getObjValue() const { return f_value; }

  /**
   * @return The Lagrange multipliers
   */
  const VectorXd& getLagrangeMultipliers() const { return u; }
  /**
   * Return the active set, namely the indeces of active constraints.
   * The first nEqCon indexes are for the equalities and are negative.
   * The last nIneqCon indexes are for the inequalities and start from 0.
   * Only the first q elements of the return vector are valid, where q
   * is the size of the active set.
   * @return The set of indexes of the active constraints.
   */
  const VectorXi& getActiveSet() const { return A; }

  /**
   * solves the problem
   * min. x' Hess x + 2 g0' x
   * s.t. CE x + ce0 = 0
   *      CI x + ci0 >= 0
   */
  EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
                                       const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x);
  /**
   * solves the sparse problem
   * min. x' Hess x + 2 g0' x
   * s.t. CE x + ce0 = 0
   *      CI x + ci0 >= 0
   */
  EiquadprogFast_status solve_quadprog_sparse(const SpMat& Hess, const VectorXd& g0, const MatrixXd& CE,
                                              const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0,
                                              VectorXd& x);

  MatrixXd m_J;  // J * J' = Hessian <nVars,nVars>::d
  bool is_inverse_provided_;

 private:
  int m_nVars;
  int m_nEqCon;
  int m_nIneqCon;

  int m_maxIter;   /// max number of active-set iterations
  double f_value;  /// current value of cost function

  Eigen::LLT<MatrixXd, Eigen::Lower> chol_;  // <nVars,nVars>::d
  // Eigen::LLT<MatrixXd,Eigen::Lower> chol_; // <nVars,nVars>::d

  /// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints
  MatrixXd R;  // <nVars,nVars>::d

  /// CI*x+ci0
  VectorXd s;  // <nIneqCon>::d

  /// infeasibility multipliers, i.e. negative step direction in dual space
  VectorXd r;  // <nIneqCon+nEqCon>::d

  /// Lagrange multipliers
  VectorXd u;  // <nIneqCon+nEqCon>::d

  /// step direction in primal space
  VectorXd z;  // <nVars>::d

  /// J' np
  VectorXd d;  //<nVars>::d

  /// current constraint normal
  VectorXd np;  //<nVars>::d

  /// active set (indeces of active constraints)
  /// the first nEqCon indeces are for the equalities and are negative
  /// the last nIneqCon indeces are for the inequalities are start from 0
  VectorXi A;  // <nIneqCon+nEqCon>

  /// initialized as K \ A
  /// iai(i)=-1 iff inequality constraint i is in the active set
  /// iai(i)=i otherwise
  VectorXi iai;  // <nIneqCon>::i

  /// initialized as [1, ..., 1, .]
  /// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set
  /// if adding ineq constraint i fails => iaexcl(i)=0
  /// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active constraints
  /// iaexcl(i)=1 otherwise
  VectorXi iaexcl;  //<nIneqCon>::i

  VectorXd x_old;  // old value of x <nVars>::d
  VectorXd u_old;  // old value of u <nIneqCon+nEqCon>::d
  VectorXi A_old;  // old value of A <nIneqCon+nEqCon>::i

#ifdef OPTIMIZE_ADD_CONSTRAINT
  VectorXd T1;  /// tmp variable used in add_constraint
#endif

  /// size of the active set A (containing the indices of the active constraints)
  int q;

  /// number of active-set iterations
  int iter;

  inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np) {
#ifdef OPTIMIZE_COMPUTE_D
    d.noalias() = J.adjoint() * np;
#else
    d = J.adjoint() * np;
#endif
  }

  inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d, int iq) {
#ifdef OPTIMIZE_UPDATE_Z
    z.noalias() = J.rightCols(z.size() - iq) * d.tail(z.size() - iq);
#else
    z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
#endif
  }

  inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d, int iq) {
    r.head(iq) = d.head(iq);
    R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
  }

  inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, double& R_norm);

  inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, int nEqCon, int& iq, int l);
};

} /* namespace solvers */
} /* namespace tsid */

#endif /* EIQUADPROGFAST_HH_ */