/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include "permutation.hpp"

#include <numeric>

#include <boost/algorithm/string/join.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/algorithm_ext/push_back.hpp>

#include <core/utils/range_utils.hpp>
#include <reversible/functions/circuit_to_truth_table.hpp>
#include <reversible/simulation/simple_simulation.hpp>

using namespace boost::assign;
using boost::adaptors::transformed;

namespace cirkit
{

permutation_t identity_permutation( unsigned size )
{
  permutation_t perm( size );
  std::iota( perm.begin(), perm.end(), 0u );
  return perm;
}

permutation_t truth_table_to_permutation( const binary_truth_table& spec )
{
  permutation_t perm;

  for ( const auto& row : index( spec ) )
  {
    auto from = row.value.first;
    auto to   = row.value.second;

    assert( truth_table_cube_to_number( binary_truth_table::cube_type( from.first, from.second ) ) == row.index );
    perm += truth_table_cube_to_number( binary_truth_table::cube_type( to.first, to.second ) );
  }

  return perm;
}

permutation_t circuit_to_permutation( const circuit& circ )
{
  binary_truth_table spec;
  circuit_to_truth_table( circ, spec, simple_simulation_func() );
  return truth_table_to_permutation( spec );
}

cycles_t permutation_to_cycles( const permutation_t& perm, bool sort )
{
  cycles_t cycles;

  boost::dynamic_bitset<> vismask = ~boost::dynamic_bitset<>( perm.size() );
  unsigned start, current;

  while ( vismask.any() )
  {
    start = vismask.find_first();
    permutation_t cycle;
    cycle += start;
    current = perm[start];
    vismask.reset( current );

    while ( current != start ) {
      cycle += current;
      current = perm[current];
      vismask.reset( current );
    }

    cycles += cycle;
  }

  assert( vismask.none() );

  if ( sort )
  {
    boost::sort( cycles, []( const std::vector<unsigned>& x1, const std::vector<unsigned>& x2 ) { return x1.size() > x2.size(); } );
  }

  return cycles;
}

std::vector<std::pair<unsigned, unsigned>> permutation_to_transpositions( const permutation_t& perm )
{
  auto perm_copy = perm;
  std::vector<std::pair<unsigned, unsigned>> transpositions;

  for ( int i = perm_copy.size() - 1; i > 0; --i )
  {
    if ( static_cast<int>( perm_copy[i] ) == i ) { continue; }

    /* where is i? */
    auto other = std::distance( perm_copy.begin(), std::find( perm_copy.begin(), perm_copy.begin() + i, i ) );
    transpositions.push_back( {i, other} );

    std::swap( perm_copy[i], perm_copy[other] );
  }

  return transpositions;
}

unsigned permutation_inv( const permutation_t& perm )
{
  unsigned inv = 0u;

  for ( unsigned i = 0u; i < perm.size() - 1u; ++i )
  {
    for ( unsigned j = i + 1u; j < perm.size(); ++j )
    {
      if ( perm[i] > perm[j] )
      {
        ++inv;
      }
    }
  }

  return inv;
}

int permutation_sign( const permutation_t& perm )
{
  return ( permutation_inv( perm ) % 2 == 0u ) ? 1 : -1;
}

permutation_t permutation_multiply( const permutation_t& a, const permutation_t& b )
{
  assert( a.size() == b.size() );
  permutation_t result( a.size() );

  for ( auto i = 0u; i < a.size(); ++i )
  {
    result[i] = b[a[i]];
  }

  return result;
}

permutation_t permutation_invert( const permutation_t& perm )
{
  permutation_t result( perm.size() );

  for ( auto i = 0u; i < perm.size(); ++i )
  {
    result[perm[i]] = i;
  }

  return result;
}

std::vector<unsigned> cycles_type( const cycles_t& cycles )
{
  std::vector<unsigned> type( cycles.size() );
  boost::transform( cycles, type.begin(), []( const permutation_t& cycle ) { return cycle.size(); } );
  boost::sort( type );
  return type;
}

bool is_involution( const permutation_t& perm )
{
  auto c = permutation_to_cycles( perm );
  return boost::find_if( c, []( const std::vector<unsigned>& cycle ) { return cycle.size() > 2u; } ) == c.end();
}

inline unsigned pos( unsigned i, unsigned j, unsigned n )
{
  return i * n + ( i * ( i + 1 ) ) / 2 + j;
}

/* dynamic programming algorithm to check whether a permutation is
   simple.

   [M.H. Albert, M.D. Atkinson, M. Klazar, Journal of Integer Sequences 6 (2003), 03.4.4]
*/
bool is_simple( const permutation_t& perm )
{
  const unsigned n = perm.size();
  const auto sumn  = ( n * ( n + 1 ) ) / 2;

  std::vector<unsigned> m_min( sumn );
  std::vector<unsigned> m_max( sumn );

  auto p = 0u;
  for ( auto i = 0u; i < n; ++i )
  {
    m_min[p] = m_max[p] = perm.at( i );

    for ( auto j = ( i + 1u ); j < n; ++j )
    {
      m_min[p + 1u] = std::min( m_min[p], perm.at( j ) );
      m_max[p + 1u] = std::max( m_max[p], perm.at( j ) );
      ++p;
    }

    ++p;
  }

  p = 0u;
  for ( auto i = 0u; i < ( n - 1u ); ++i )
  {
    ++p;
    for ( auto j = ( i + 1u ); j < n; ++j )
    {
      if ( ( m_max[p] - m_min[p] ) == ( j - i ) && !( ( i == 0u ) && ( j == ( n - 1u ) ) ) )
      {
        return false;
      }
      ++p;
    }
  }

  return true;
}

std::string permutation_to_string( const permutation_t& perm )
{
  return "[" + any_join( perm, " " ) + "]";
}

std::string cycles_to_string( const cycles_t& cycles, bool print_fixpoints )
{
  return boost::join( cycles | transformed( [&print_fixpoints]( const permutation_t& cycle ) {
        return ( cycle.size() > 1u || print_fixpoints ) ? ( "(" + any_join( cycle, " " ) + ")" ) : std::string();
      } ), "" );
}

std::string cycles_to_string( const permutation_t& perm, bool print_fixpoints )
{
  return cycles_to_string( permutation_to_cycles( perm ), print_fixpoints );
}

std::string type_to_string( const std::vector<unsigned>& type )
{
  return "(" + any_join( type, ", " ) + ")";
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


//  Copyright (c) 2019 National Technology & Engineering Solutions of Sandia,
//                     LLC (NTESS).
//  Copyright (c) 2014 Hartmut Kaiser
//  Copyright (c) 2014 Patricia Grubel
//  Copyright (c) 2019 Nikunj Gupta
//
//  SPDX-License-Identifier: BSL-1.0
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// This is the fourth in a series of examples demonstrating the development of
// a fully distributed solver for a simple 1D heat distribution problem.
//
// This example builds on example three. It futurizes the code from that
// example. Compared to example two this code runs much more efficiently. It
// allows for changing the amount of work executed in one HPX thread which
// enables tuning the performance for the optimal grain size of the
// computation. This example is still fully local but demonstrates nice
// scalability on SMP machines.

#include <hpx/local/algorithm.hpp>
#include <hpx/local/init.hpp>
#include <hpx/modules/resiliency.hpp>
#include <hpx/modules/synchronization.hpp>
#include <boost/range/irange.hpp>

#include <atomic>
#include <cstddef>
#include <cstdint>
#include <exception>
#include <iostream>
#include <memory>
#include <random>
#include <utility>
#include <vector>

struct validate_exception : std::exception
{
};

///////////////////////////////////////////////////////////////////////////////
double const pi = std::acos(-1.0);

// Variable to count the number of failed attempts
std::atomic<int> counter(0);

// Variables to generate errors
std::random_device rd;
std::mt19937 gen(rd());

///////////////////////////////////////////////////////////////////////////////
// Our partition data type
struct partition_data
{
public:
    partition_data(std::size_t size)
      : data_(size)
      , size_(size)
    {
    }

    partition_data(std::size_t subdomain_width, double subdomain_index,
        std::size_t subdomains)
      : data_(subdomain_width + 1)
      , size_(subdomain_width + 1)
    {
        for (std::size_t k = 0; k != subdomain_width + 1; ++k)
        {
            data_[k] = std::sin(2 * pi *
                ((0.0 + subdomain_width * subdomain_index + k) /
                    static_cast<double>(subdomain_width * subdomains)));
        }
    }

    partition_data(partition_data&& other)
      : data_(std::move(other.data_))
      , size_(other.size_)
    {
    }

    double& operator[](std::size_t idx)
    {
        return data_[idx];
    }
    double operator[](std::size_t idx) const
    {
        return data_[idx];
    }

    friend std::vector<double>::const_iterator begin(const partition_data& v)
    {
        return begin(v.data_);
    }
    friend std::vector<double>::const_iterator end(const partition_data& v)
    {
        return end(v.data_);
    }

    std::size_t size() const
    {
        return size_;
    }

    void resize(std::size_t size)
    {
        data_.resize(size);
        size_ = size;
    }

private:
    std::vector<double> data_;
    std::size_t size_;
};

std::ostream& operator<<(std::ostream& os, partition_data const& c)
{
    os << "{";
    for (std::size_t i = 0; i != c.size() - 1; ++i)
    {
        if (i != 0)
            os << ", ";
        os << c[i];
    }
    os << "}";
    return os;
}

///////////////////////////////////////////////////////////////////////////////
struct stepper
{
    // Our data for one time step
    typedef hpx::shared_future<partition_data> partition;
    typedef std::vector<partition> space;

    // Our operator
    static double stencil(double left, double center, double right)
    {
        return 0.5 * (0.75) * left + (0.75) * center - 0.5 * (0.25) * right;
    }

    static double left_flux(double left, double center)
    {
        return (0.625) * left - (0.125) * center;
    }

    static double right_flux(double center, double right)
    {
        return 0.5 * (0.75) * center + (1.125) * right;
    }

    // The partitioned operator, it invokes the heat operator above on all
    // elements of a partition.
    static partition_data heat_part(std::size_t sti, double error,
        partition_data const& left_input, partition_data const& center_input,
        partition_data const& right_input)
    {
        static thread_local std::exponential_distribution<> dist_(error);

        double num = dist_(gen);
        bool error_flag = false;

        // Probability of error occurrence is proportional to exp(-error_rate)
        if (num > 1.0)
        {
            error_flag = true;
            ++counter;
        }

        std::size_t const size = center_input.size() - 1;
        partition_data workspace(size + 2 * sti + 1);

        std::copy(
            end(left_input) - sti - 1, end(left_input) - 1, &workspace[0]);
        std::copy(begin(center_input), end(center_input) - 1, &workspace[sti]);
        std::copy(begin(right_input), begin(right_input) + sti + 1,
            &workspace[size + sti]);

        for (std::size_t t = 0; t != sti; ++t)
        {
            for (std::size_t k = 0; k != size + 2 * sti - 1 - 2 * t; ++k)
                workspace[k] =
                    stencil(workspace[k], workspace[k + 1], workspace[k + 2]);
        }

        workspace.resize(size + 1);

        // Artificial error injection to get replay in action
        if (error_flag)
            throw validate_exception();

        return workspace;
    }

    hpx::future<space> do_work(std::size_t subdomains,
        std::size_t subdomain_width, std::size_t iterations, std::size_t sti,
        std::uint64_t nd, std::uint64_t n_value, double error,
        hpx::lcos::local::sliding_semaphore& sem)
    {
        using hpx::resiliency::experimental::dataflow_replicate;
        using hpx::util::unwrapping;

        // U[t][i] is the state of position i at time t.
        std::vector<space> U(2);
        for (space& s : U)
            s.resize(subdomains);

        std::size_t b = 0;
        auto range = boost::irange(b, subdomains);
        hpx::ranges::for_each(hpx::execution::par, range,
            [&U, subdomain_width, subdomains](std::size_t i) {
                U[0][i] = hpx::make_ready_future(
                    partition_data(subdomain_width, double(i), subdomains));
            });

        auto Op = unwrapping(&stepper::heat_part);

        // Actual time step loop
        for (std::size_t t = 0; t != iterations; ++t)
        {
            space const& current = U[t % 2];
            space& next = U[(t + 1) % 2];

            for (std::size_t i = 0; i != subdomains; ++i)
            {
                next[i] = dataflow_replicate(n_value, Op, sti, error,
                    current[(i - 1 + subdomains) % subdomains], current[i],
                    current[(i + 1) % subdomains]);
            }

            // every nd time steps, attach additional continuation which will
            // trigger the semaphore once computation has reached this point
            if ((t % nd) == 0)
            {
                next[0].then([&sem, t](partition&&) {
                    // inform semaphore about new lower limit
                    sem.signal(t);
                });
            }

            // suspend if the tree has become too deep, the continuation above
            // will resume this thread once the computation has caught up
            sem.wait(t);
        }

        // Return the solution at time-step 'iterations'.
        return hpx::when_all(U[iterations % 2]);
    }
};

///////////////////////////////////////////////////////////////////////////////
int hpx_main(hpx::program_options::variables_map& vm)
{
    std::uint64_t n_value =
        vm["n-value"].as<std::uint64_t>();    // Number of partitions.
    std::uint64_t subdomains =
        vm["subdomains"].as<std::uint64_t>();    // Number of partitions.
    std::uint64_t subdomain_width =
        vm["subdomain-width"].as<std::uint64_t>();    // Number of grid points.
    std::uint64_t iterations =
        vm["iterations"].as<std::uint64_t>();    // Number of steps.
    std::uint64_t nd =
        vm["nd"].as<std::uint64_t>();    // Max depth of dep tree.
    std::uint64_t sti =
        vm["steps-per-iteration"]
            .as<std::uint64_t>();    // Number of time steps per iteration
    double error = vm["error-rate"].as<double>();

    // Create the stepper object
    stepper step;

    std::cout << "Starting 1d stencil with dataflow replicate" << std::endl;

    // Measure execution time.
    std::uint64_t t = hpx::chrono::high_resolution_clock::now();

    {
        // limit depth of dependency tree
        hpx::lcos::local::sliding_semaphore sem(nd);

        hpx::future<stepper::space> result = step.do_work(subdomains,
            subdomain_width, iterations, sti, nd, n_value, error, sem);

        stepper::space solution = result.get();
        hpx::wait_all(solution);
    }

    std::cout << "Time elapsed: "
              << static_cast<double>(
                     hpx::chrono::high_resolution_clock::now() - t) /
            1e9
              << std::endl;
    std::cout << "Errors occurred: " << counter << std::endl;

    // for (std::size_t i = 0; i != subdomains; ++i)
    //     std::cout << solution[i].get() << " ";
    // std::cout << std::endl;

    return hpx::local::finalize();
}

int main(int argc, char* argv[])
{
    using namespace hpx::program_options;

    // Configure application-specific options.
    options_description desc_commandline;

    desc_commandline.add_options()(
        "results", "print generated results (default: false)");

    desc_commandline.add_options()("n-value",
        value<std::uint64_t>()->default_value(5), "Number of allowed replays");

    desc_commandline.add_options()("error-rate",
        value<double>()->default_value(5), "Error rate for injecting errors");

    desc_commandline.add_options()("subdomain-width",
        value<std::uint64_t>()->default_value(128),
        "Local x dimension (of each partition)");

    desc_commandline.add_options()("iterations",
        value<std::uint64_t>()->default_value(10), "Number of time steps");

    desc_commandline.add_options()("steps-per-iteration",
        value<std::uint64_t>()->default_value(16),
        "Number of time steps per iterations");

    desc_commandline.add_options()("nd",
        value<std::uint64_t>()->default_value(10),
        "Number of time steps to allow the dependency tree to grow to");

    desc_commandline.add_options()("subdomains",
        value<std::uint64_t>()->default_value(10), "Number of partitions");

    // Initialize and run HPX
    hpx::local::init_params init_args;
    init_args.desc_cmdline = desc_commandline;

    return hpx::local::init(hpx_main, argc, argv, init_args);
}


#include <trajopt/utils.hpp>
#include <trajopt_sco/solver_interface.hpp>
#include <Eigen/Geometry>
using namespace Eigen;

namespace trajopt {

TrajArray getTraj(const DblVec& x, const VarArray& vars) {
  TrajArray out(vars.rows(), vars.cols());
  for (int i=0; i < vars.rows(); ++i) {
    for (int j=0; j < vars.cols(); ++j) {
      out(i,j) = vars(i,j).value(x);
    }
  }
  return out;
}
TrajArray getTraj(const DblVec& x, const AffArray& arr) {
  MatrixXd out(arr.rows(), arr.cols());
  for (int i=0; i < arr.rows(); ++i) {
    for (int j=0; j < arr.cols(); ++j) {
      out(i,j) = arr(i,j).value(x);
    }
  }
  return out;
}

Eigen::Matrix3d toRot(const OR::Vector& rq) {
  Eigen::Affine3d T;
  T = Eigen::Quaterniond(rq[0], rq[1], rq[2], rq[3]);
  return T.rotation();
}

void AddVarArrays(OptProb& prob, int rows, const vector<int>& cols, const vector<string>& name_prefix, const vector<VarArray*>& newvars) {
  int n_arr = name_prefix.size();
  assert(n_arr == newvars.size());

  vector<MatrixXi> index(n_arr);
  for (int i=0; i < n_arr; ++i) {
    newvars[i]->resize(rows, cols[i]);
    index[i].resize(rows, cols[i]);
  }

  vector<string> names;
  int var_idx = prob.getNumVars();
  for (int i=0; i < rows; ++i) {
    for (int k=0; k < n_arr; ++k) {
      for (int j=0; j < cols[k]; ++j) {
        index[k](i,j) = var_idx;
        names.push_back( (boost::format("%s_%i_%i")%name_prefix[k]%i%j).str() );
        ++var_idx;
      }
    }
  }
  prob.createVariables(names); // note that w,r, are both unbounded

  const vector<Var>& vars = prob.getVars();
  for (int k=0; k < n_arr; ++k) {
    for (int i=0; i < rows; ++i) {
      for (int j=0; j < cols[k]; ++j) {
        (*newvars[k])(i,j) = vars[index[k](i,j)];
      }
    }
  }
}

void AddVarArray(OptProb& prob, int rows, int cols, const string& name_prefix, VarArray& newvars) {
  vector<VarArray*> arrs(1, &newvars);
  vector<string> prefixes(1, name_prefix);
  vector<int> colss(1, cols);
  AddVarArrays(prob, rows, colss, prefixes, arrs);
}

}


////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2015 Bryce Adelstein Lelbach aka wash
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
////////////////////////////////////////////////////////////////////////////////

#include "detail/integer_sequence.hpp"

#include <boost/core/lightweight_test.hpp>

using std::true_type;
using std::false_type;
using std::is_same;

using std::experimental::detail::integer_sequence;

using std::experimental::detail::make_integer_sequence;
using std::experimental::detail::make_reversed_integer_sequence;

int main()
{
    ///////////////////////////////////////////////////////////////////////////
    BOOST_TEST_EQ((is_same<
        make_integer_sequence<int, 0>
      , integer_sequence<int>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_integer_sequence<int, 1>
      , integer_sequence<int, 0>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_integer_sequence<int, 2>
      , integer_sequence<int, 0, 1>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_integer_sequence<int, 3>
      , integer_sequence<int, 0, 1, 2>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_integer_sequence<int, 4>
      , integer_sequence<int, 0, 1, 2, 3>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_integer_sequence<int, 5>
      , integer_sequence<int, 0, 1, 2, 3, 4>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_integer_sequence<int, 6>
      , integer_sequence<int, 0, 1, 2, 3, 4, 5>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_integer_sequence<int, 7>
      , integer_sequence<int, 0, 1, 2, 3, 4, 5, 6>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_integer_sequence<int, 8>
      , integer_sequence<int, 0, 1, 2, 3, 4, 5, 6, 7>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_integer_sequence<int, 9>
      , integer_sequence<int, 0, 1, 2, 3, 4, 5, 6, 7, 8>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_integer_sequence<int, 10>
      , integer_sequence<int, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9>
    >::value), true);

    ///////////////////////////////////////////////////////////////////////////
    BOOST_TEST_EQ((is_same<
        make_reversed_integer_sequence<int, 0>
      , integer_sequence<int>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_reversed_integer_sequence<int, 1>
      , integer_sequence<int, 0>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_reversed_integer_sequence<int, 2>
      , integer_sequence<int, 1, 0>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_reversed_integer_sequence<int, 3>
      , integer_sequence<int, 2, 1, 0>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_reversed_integer_sequence<int, 4>
      , integer_sequence<int, 3, 2, 1, 0>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_reversed_integer_sequence<int, 5>
      , integer_sequence<int, 4, 3, 2, 1, 0>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_reversed_integer_sequence<int, 6>
      , integer_sequence<int, 5, 4, 3, 2, 1, 0>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_reversed_integer_sequence<int, 7>
      , integer_sequence<int, 6, 5, 4, 3, 2, 1, 0>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_reversed_integer_sequence<int, 8>
      , integer_sequence<int, 7, 6, 5, 4, 3, 2, 1, 0>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_reversed_integer_sequence<int, 9>
      , integer_sequence<int, 8, 7, 6, 5, 4, 3, 2, 1, 0>
    >::value), true);
    BOOST_TEST_EQ((is_same<
        make_reversed_integer_sequence<int, 10>
      , integer_sequence<int, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0>
    >::value), true);

    return boost::report_errors();
}


// Copyright (C) 2017 Vicente J. Botet Escriba
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// <experimental/strong_ints.hpp>

#include <experimental/strong_ints.hpp>
#include <sstream>

#include <boost/detail/lightweight_test.hpp>

namespace stdex = std::experimental;

using OrangeCount = stdex::strong_integral<class OrangeTag, int>;
using OrangeCountDC = stdex::strong_integral<class OrangeTag, int>;
using AppelCount = stdex::strong_integral<class AppelTag, int>;
using UOrangeCount = stdex::strong_integral<class OrangeTag, unsigned int>;
using UCOrangeCount = stdex::strong_integral<class OrangeTag, unsigned char>;
using SOrangeCount = stdex::strong_integral<class OrangeTag, short>;
using COrangeCount = stdex::strong_integral<class OrangeTag, char>;

static_assert(sizeof(OrangeCount)==sizeof(int), "");

OrangeCount add(OrangeCount x, OrangeCount y){
  return x+y;
}

struct X {
    operator int() { return 1; }
};

struct Y {
    explicit operator int() { return 1; }
};

int main()
{

  {
    auto x = stdex::make_strong_int<OrangeTag>(1);
    BOOST_TEST(x == OrangeCount(1));
  }

  {
      //OrangeCount oc1 = 1;     // error - explicit required
  }
  {
      OrangeCount oc2{1};
      BOOST_TEST(oc2 == OrangeCount{1});
      short s=1;
      OrangeCount oc3 {s};
      BOOST_TEST(oc2 == OrangeCount{1});

      //OrangeCount oc4 {1.0};      // error narrowing conversion

      AppelCount ac1{1};
      (void)ac1;
      OrangeCount oc5 {oc2};
      BOOST_TEST(oc5 == OrangeCount{1});

      //AppelCount ac2=oc2; // error
      //AppelCount ac2{oc2}; // error
      AppelCount ac2{oc2.underlying()};
      BOOST_TEST(ac2 == AppelCount{1});

      //int i = oc2; // error
      //int i{oc2}; // error
      int i{oc2.underlying()};
      BOOST_TEST(i == 1);

      auto oc6 = add(oc2, oc3);
      BOOST_TEST(oc6 == OrangeCount{2});
      //auto oc7 = add(oc2, 1); // error
      BOOST_TEST(oc2 != oc6);
      BOOST_TEST(oc2 < oc6);

      {
          //OrangeCount es {Y{}};        // error
          OrangeCount es {X{}};        // ok, but shouldn't this be forbiden also?
      }
      { // uninitialize default constructor
        OrangeCountDC oc;
        (void)oc;
        //not initialized
        //BOOST_TEST(oc == OrangeCountDC{});
      }
      { // constructor from UT
        OrangeCount oc{1};
        BOOST_TEST(oc == OrangeCount{1});
      }
      { // zero-initializer C++98 default constructor
        OrangeCountDC oc = OrangeCountDC();
        BOOST_TEST(oc == OrangeCountDC{});
      }
      { // zero_initializee C++11 default constructor
        OrangeCountDC oc{};
        BOOST_TEST(oc == OrangeCountDC{});
      }
      { // constructor from UT
        OrangeCount oc{1};
        BOOST_TEST(oc == OrangeCount{1});
      }
      { // constructor from implicitly convertible to UT
        short s2=1;
        OrangeCount oc{s2};
        BOOST_TEST(oc == OrangeCount{1});
      }
      { // copy constructor
        OrangeCount oc1{1};
        OrangeCount oc22{oc1};
        BOOST_TEST(oc1 == oc22);
      }
      { // assignment
        OrangeCount oc1{1};
        OrangeCount oc22;
        oc22 = oc1;
        BOOST_TEST(oc22 == oc1);
      }
#if defined COMPILE_ERROR
      { // assignment from UT
        OrangeCount oc1{1};
        oc1 = 2; // error
      }
#endif
      { // operator+(x)
        OrangeCount oc{1};
        OrangeCount oc22 = +oc;
        BOOST_TEST(oc22 == OrangeCount{1});
      }
      { // operator+(x,y)
        OrangeCount oc1{1};
        OrangeCount oc22{2};
        BOOST_TEST(oc1+oc22 == OrangeCount{3});
      }
      { // operator+(x,y)
        SOrangeCount oc1{1};
        SOrangeCount oc22{2};
        BOOST_TEST(oc1+oc22 == SOrangeCount{3});
      }
      { // operator+(x,y)
        SOrangeCount oc1{1};
        SOrangeCount oc22{2};
        auto oc = oc1+oc22;
        static_assert(std::is_same<decltype(oc), SOrangeCount>::value, "error");

        BOOST_TEST(oc == SOrangeCount{3});
      }

      { // operator+=(x)
        OrangeCount oc1{1};
        OrangeCount oc22{2};
        oc22 += oc1;
        BOOST_TEST(oc22 == OrangeCount{3});
      }
      { // operator++()
        OrangeCount oc1{1};
        auto oc22 = ++oc1;
        BOOST_TEST(oc1 == OrangeCount{2});
        BOOST_TEST(oc22 == OrangeCount{2});
      }
      { // operator++(int)
        OrangeCount oc1{1};
        auto oc22 = oc1++;
        BOOST_TEST(oc1 == OrangeCount{2});
        BOOST_TEST(oc22 == OrangeCount{1});
      }

      { // operator-(x)
        OrangeCount oc{1};
        OrangeCount oc22 = -oc;
        BOOST_TEST(oc22 == OrangeCount{-1});
      }
      { // operator-(x)
        UOrangeCount oc{1};
        UOrangeCount oc22 = -oc;
        BOOST_TEST(oc22 == UOrangeCount{-1u});
      }
      { // operator-(x,y)
        OrangeCount oc1{1};
        OrangeCount oc22{2};
        BOOST_TEST(oc1-oc22 == OrangeCount{-1});
      }
      { // operator-(x,y)
        SOrangeCount oc1{1};
        SOrangeCount oc22{2};
        BOOST_TEST(oc1-oc22 == SOrangeCount{-1});
      }
      { // operator-(x,y)
        SOrangeCount oc1{1};
        SOrangeCount oc22{2};
        auto oc = oc1-oc22;
        static_assert(std::is_same<decltype(oc), SOrangeCount>::value, "error");

        BOOST_TEST(oc == SOrangeCount{-1});
      }
      { // operator-(x,y)
        UOrangeCount oc1{3};
        UOrangeCount oc22{2};
        BOOST_TEST(oc1-oc22 == UOrangeCount{1});
      }
      { // operator-(x,y)
        UOrangeCount oc1{3};
        UOrangeCount oc22{2};
        auto oc = oc1-oc22;
        static_assert(std::is_same<decltype(oc), UOrangeCount>::value, "error");
        BOOST_TEST(oc == UOrangeCount{1});
      }
      { // operator-=(x)
        OrangeCount oc1{1};
        OrangeCount oc22{2};
        oc22 -= oc1;
        BOOST_TEST(oc22 == OrangeCount{1});
      }
      { // operator--()
        OrangeCount oc1{1};
        auto oc22 = --oc1;
        BOOST_TEST(oc1 == OrangeCount{0});
        BOOST_TEST(oc22 == OrangeCount{0});
      }
      { // operator--(int)
        OrangeCount oc1{1};
        auto oc22 = oc1--;
        BOOST_TEST(oc1 == OrangeCount{0});
        BOOST_TEST(oc22 == OrangeCount{1});
      }

      { // operator*(x,y)
        OrangeCount oc1{3};
        OrangeCount oc22{2};
        BOOST_TEST(oc1*oc22 == OrangeCount{6});
      }
      { // operator*=(x)
        OrangeCount oc1{3};
        OrangeCount oc22{2};
        oc22 *= oc1;
        BOOST_TEST(oc22 == OrangeCount{6});
      }

      { // operator/(x,y)
        OrangeCount oc1{6};
        OrangeCount oc22{2};
        BOOST_TEST(oc1/oc22 == OrangeCount{3});
      }
      { // operator/=(x)
        OrangeCount oc1{6};
        OrangeCount oc22{2};
        oc1 /= oc22;
        BOOST_TEST(oc1 == OrangeCount{3});
      }

      { // operator%(x,y)
        OrangeCount oc1{6};
        OrangeCount oc22{5};
        BOOST_TEST(oc1%oc22 == OrangeCount{1});
      }
      { // operator%=(x)
        OrangeCount oc1{6};
        OrangeCount oc22{5};
        oc1 %= oc22;
        BOOST_TEST(oc1 == OrangeCount{1});
      }

      { // operator==(x,y)
        OrangeCount oc1{2};
        OrangeCount oc22{2};
        BOOST_TEST( oc1==oc22 );
      }
      { // operator!=(x,y)
        OrangeCount oc1{2};
        OrangeCount oc22{3};
        BOOST_TEST( oc1!=oc22 );
      }
      { // operator<(x,y)
        OrangeCount oc1{2};
        OrangeCount oc22{5};
        BOOST_TEST( oc1<oc22 );
      }
      { // operator<=(x,y)
        OrangeCount oc1{2};
        OrangeCount oc22{5};
        BOOST_TEST( oc1<=oc22 );
      }
      { // operator<=(x,y)
        OrangeCount oc1{2};
        OrangeCount oc22{2};
        BOOST_TEST( oc1<=oc22 );
      }
      { // operator>(x,y)
        OrangeCount oc1{6};
        OrangeCount oc22{5};
        BOOST_TEST( oc1>oc22 );
      }
      { // operator>=(x,y)
        OrangeCount oc1{6};
        OrangeCount oc22{5};
        BOOST_TEST( oc1>=oc22 );
      }
      { // operator<=(x,y)
        OrangeCount oc1{2};
        OrangeCount oc22{2};
        BOOST_TEST( oc1>=oc22 );
      }
      // swap
      {
        OrangeCount oc1{1};
        OrangeCount oc22{2};
        using std::swap;
        swap(oc1,oc22);
        BOOST_TEST(oc1.underlying()==2);
        BOOST_TEST(oc22.underlying()==1);
      }

      // hash
      {
        OrangeCount oc1{1};
        BOOST_TEST(std::hash<OrangeCount>{}(oc1)==std::hash<int>{}(1));
      }
      // operator <<
      {
          OrangeCount oc{1};
          std::stringstream os;
          os << oc;
          BOOST_TEST_EQ(os.str(), "1");
      }
      // operator >>
      {
          OrangeCount oc;
          std::stringstream s2;
          s2 << 1;
          s2 >> oc;
          BOOST_TEST_EQ(oc, OrangeCount{1});
      }
      {
          SOrangeCount oc;
          std::stringstream s2;
          s2 << 1;
          s2 >> oc;
          BOOST_TEST_EQ(oc, SOrangeCount{1});
      }
      {
          COrangeCount oc;
          std::stringstream s2;
          s2 << COrangeCount{5};
          s2 >> oc;
          BOOST_TEST_EQ(oc, COrangeCount{5});
      }
      {
        //OrangeCount id {&S::foo}; // this fails as expected
      }
      {
        //OrangeCount id  {&f}; // this fails as expected
      }
      {
        //double d;
        //OrangeCount id  {d}; // this fails as expected
      }
      {
        short s2 = 1;
        OrangeCount id  {s2}; // OK
        BOOST_TEST_EQ(id.underlying(), 1);
      }
      {
        //short s = -1;
        //UOrangeCount id  {s}; // this fails as expected - narrowing conversion
      }
      {
        //short s = 1;
        //UCOrangeCount id  {s}; // this fails as expected - narrowing conversion
      }
      {
        //char c = 1;
        //UCOrangeCount id  {c}; // this fails as expected - narrowing conversion
      }
  }
  return ::boost::report_errors();
}


#include <iostream>
//#include <iomanip>
//#include <fstream>

//#include <boost/numeric/ublas/matrix.hpp>
//#include <boost/numeric/ublas/vector.hpp>
//#include <boost/numeric/ublas/io.hpp>

#include <boost/timer.hpp>

#include <vector>
#include <cmath>
#include <cstring>

#include "NRGclasses.hpp"
#include "NRGfunctions.hpp"
#include "SpecFuncClass.hpp"
#include "DM_NRG.hpp"
#include "NRGOpMatRules.hpp"

#ifndef pi
#define pi 3.141592653589793238462643383279502884197169
#endif

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

  CNRGCodeHandler ThisCode;

  //CNRGbasisarray AcutN;
  CNRGbasisarray* AcutN;
  CNRGbasisarray AbasisN;

  //CNRGbasisarray AcutNp1;
  CNRGbasisarray AbasisNp1;

  CNRGbasisarray SingleSite;

  CNRGmatrix** OpArrayN;

  CNRGmatrix* RhoN;
  vector<double> ParamsTemp;
  double betabar=0.727;
    double DM,TM;
  // 0 - betabar; 1 - TM; 2 - DM ?
  double Temp; // In units of the bandwidth (fixed!)
               // Temp/DN = TempBar = 1/betabar
  int Mtemp=1001;

  double twindow=2.0;

  double broadtemp=0.8; // Good for CFS

  char CNsites[8];
  char CNmat[8];
  char arqname[32];
  char ext[32];

  int NshellMax=3;
  int NFermiOps=2;

  // Check time
  boost::timer MyTime;
  double time_elapsed;

  int UseCFS=0;
  int Nw=0; // No of omegas in each shell

  // FDM calculations
  bool UseFDM=false;
  double auxwN=0.0;
  double ZN=0.0;
  double ZTot=0.0;
  double ZNdisc=0.0;
  double TempBar;
  vector<double> wN;

  // Read code parameters, including z_twist
  ThisCode.ReadGenPars(true);

  double bbroad=0.5*log(ThisCode.Lambda);

  // New (2016)
  bool UseGap=false;

  // Command-line: Set Temp
  DM_NRG_CommandLineRead(argc,argv,Mtemp,betabar,twindow,broadtemp,bbroad,UseCFS,UseFDM,UseGap,Nw);

  // Set Temp
  cout << " Mtemp = " << Mtemp
       << " betabar = " << betabar
       << " twindow = " << twindow
       << " dBroad = " << bbroad
       << " dBroadTemp = " << broadtemp << endl
       << " NomegasEachShell = " << Nw << " (if =0 then use standard interpolation) "
       << endl;
  //betabar=0.727;
  Temp=0.0;  // Real Temperature
  //ParamsTemp.push_back(betabar); // Need to fix this today...
  if (Mtemp<ThisCode.Nsitesmax-1) {
    if (!UseFDM) ThisCode.Nsitesmax=Mtemp+1;
    DM=CalcDN(ThisCode.Lambda,Mtemp);
    Temp=DM/betabar;
  }
  else{ // OK, Let me try this. Instead of 0.0, "Temp" will be DM(Nsites+200)/betabar.
    //Temp=0.0;
    Temp=CalcDN(ThisCode.Lambda,ThisCode.Nsitesmax+199)/betabar;
    DM=CalcDN(ThisCode.Lambda,ThisCode.Nsitesmax-1);
  }
  cout << " Temp = "<<Temp
       << " DMtemp = " << DM
       << endl;
  //DM=CalcDN(ThisCode.Lambda,NshellMax);
  ParamsTemp.push_back(Temp/DM);
  // Save in Temp_Mtemp.dat
  ofstream OutFile;
  strcpy(arqname,"Temp_Mtemp.dat");
  OutFile.open(arqname);
  OutFile << Temp << " " << Mtemp << endl;
  OutFile.close();

  cout << "Lambda    = " << ThisCode.Lambda << endl;
  cout << "Nsites0   = " << ThisCode.Nsites0 << endl;
  cout << "Nsitesmax = " << ThisCode.Nsitesmax << endl;
  cout << "NFermiOps = " << ThisCode.NopsSaved << endl;
  cout << "Ext arq   = " << ThisCode.SaveArraysFileName << endl;
  cout << "Symmetry  = " << ThisCode.SymNo << endl;
  if ( (ThisCode.SymNo != 2)&&(ThisCode.SymNo != 3) ){
    cout << " DM-NRG: Only 1chQSz and 1chQ symmetries supported at this point. Testing QS symmetry now..." << endl;
  }
  if (ThisCode.totalS){cout << " SU(2) symmetry detected. " << endl;}
  cout << "Oliveira z = " << ThisCode.code_z_twist << endl;

  if (UseGap){ThisCode.ReadParams((char *)"input_nrg.dat",1);} // Need the gap

  NshellMax=ThisCode.Nsitesmax-1;
  NFermiOps= ThisCode.NopsSaved;

  // April 2010
  // TotS QNs in ThisCode
  ThisCode.SetTotS();
  // Setting SingleSite (depends only on SymNo)
  ThisCode.SetSingleSite(&SingleSite);

  // Allocate matrices: Tricky

  OpArrayN=new CNRGmatrix* [NFermiOps];
  for (int iop=0;iop<NFermiOps;iop++){
    OpArrayN[iop]=new CNRGmatrix [NshellMax+1];
  }

  AcutN = new CNRGbasisarray [NshellMax+1];
  RhoN = new CNRGmatrix [NshellMax+1];

  // Read last Nshell

  cout << " Reading bin Files for NshellMax = " << NshellMax << endl;
  AcutN[NshellMax].Nshell=NshellMax;
  AbasisNp1.Nshell=NshellMax;
  //ThisCode.ReadArrays("test"); // ReadsInto pAcutN
  // Not too handy...

  sprintf(CNsites,"%d",NshellMax);
  //strcpy(ext,"test_N");
  strcpy(ext,ThisCode.SaveArraysFileName);
  strcat(ext,"_N");
  strcat(ext,CNsites);
  strcat(ext,".bin");

  // Read Abasis
  strcpy(arqname,"Abasis_");
  strcat(arqname,ext);
  AbasisNp1.ReadBin(arqname);

  // Read Acut
  strcpy(arqname,"Acut_");
  strcat(arqname,ext);
  AcutN[NshellMax].ReadBin(arqname);

  // Read Operators at NshellMax
  for (int iop=0;iop<NFermiOps;iop++){
    sprintf(CNmat,"%d",iop);
    strcpy(arqname,"Mat");
    strcat(arqname,CNmat);
    strcat(arqname,"_");
    strcat(arqname,ext);
    OpArrayN[iop][NshellMax].ReadBin(arqname);
    // Need a better way to do this but for now it will do:
    // Actually, from Mat block it works!
    //OpArrayN[iop][NshellMax].CheckForMatEl=OneChQSz_cd_check;

  }
  // end read operators

  cout << " NshellMax Files read "<< endl;

  // FDM-NRG: Setting temperature
  DM=CalcDN(ThisCode.Lambda,NshellMax-1); // Why????
  TempBar=Temp/DM;
  ZN=AcutN[NshellMax].PartitionFunc(1.0/TempBar);
  if (!UseFDM)
    ParamsTemp.push_back(ZN); // for NshellMax

  wN.push_back(ZN);
  ZTot=ZN;
  cout << " Temp = " << Temp << " at N= " << NshellMax
       << " TempBar = " << TempBar
       << " DN = " << DM
       << " ZNdisc = ZN = " << ZN
       << " ZwNN = " << ZN*pow(4.0,NshellMax-NshellMax)
       << endl;
  /////////

  // Read AcutN
  for (int Nshell=NshellMax-1;Nshell>=ThisCode.Nsites0;Nshell--){

    cout << " Reading AcutN in Nshell = " << Nshell << endl;

    //AcutN[Nshell].ClearAll();
    //AcutN[Nshell].Nshell=Nshell;

    sprintf(CNsites,"%d",Nshell);
    strcpy(ext,ThisCode.SaveArraysFileName);
    strcat(ext,"_N");
    strcat(ext,CNsites);
    strcat(ext,".bin");

    // Read Acut
    strcpy(arqname,"Acut_");
    strcat(arqname,ext);
    AcutN[Nshell].ReadBin(arqname);

    // FDM-NRG: Setting temperature
    DM=CalcDN(ThisCode.Lambda,Nshell-1);
    TempBar=Temp/DM;
    // Partition function
    ZN=AcutN[Nshell].PartitionFunc(1.0/TempBar);
    // Partition function for discarded states
    ZNdisc=AcutN[Nshell].PartitionFuncDisc(1.0/TempBar);
    wN.push_back(ZNdisc*pow(4.0,NshellMax-Nshell));
    ZTot+=ZNdisc*pow(4.0,NshellMax-Nshell);
    cout << " Temp = " << Temp << " at N= " << Nshell
  	 << " TempBar = " << TempBar
  	 << " DM = " << DM
  	 << " ZN = " << ZN
  	 << " ZNdisc = " << ZNdisc
  	 << " ZwNN = " << ZNdisc*pow(4.0,NshellMax-Nshell)
  	 << endl;
    /////////
  }
  //end loop in Nshell

  cout << "Ztot = " << ZTot << endl;

  // Calculate wN
  double NormwN=0.0;
  for (int ii=0; ii<wN.size(); ii++){
    wN[ii]/=ZTot;
    cout << " wN[" << NshellMax-ii << "] = " << wN[ii] << endl;
    NormwN+=wN[ii];
  }
  cout << " Sum_N wN = " << NormwN << endl;

  // end calculate wn

  if (UseFDM)
    ParamsTemp.push_back(ZTot); // for NshellMax

  // For 1-channel only!! d=4

  // Set density matrix at the LAST NRG iteration

  // Either CalcRhoN AND save it OR read it from file
  // Read RhoNmax
  sprintf(CNsites,"%d",NshellMax);
  strcpy(ext,ThisCode.SaveArraysFileName);
  strcat(ext,"_N");
  strcat(ext,CNsites);
  strcat(ext,".bin");

  strcpy(arqname,"rhoDM_");
  strcat(arqname,ext);

  if (ThisCode.CheckFileExists(arqname)){
    cout << " DM_NRG: Found file " << arqname << endl;
    RhoN[NshellMax].ReadBin(arqname);
  }else{
    DM_NRG_SetRhoNmax(ParamsTemp,&AcutN[NshellMax],&RhoN[NshellMax]);
    RhoN[NshellMax].SaveBin(arqname);
  }

  cout << " trace(Rho[Nshell= " << NshellMax << " ]) = " << RhoN[NshellMax].CalcTrace() << endl;

  //if (NshellMax==48){
  //cout << " Rho Nshell = " << NshellMax << endl;
  //RhoN[NshellMax].PrintAllBlocks();}
  // Debugging
  double qnums[2];
  int iBl=0;

  // Calculate reduced density matrices

  for (int Nshell=NshellMax-1;Nshell>=ThisCode.Nsites0;Nshell--){

    cout << "DM-NRG: working on Nshell = " << Nshell << endl;

    AbasisN.ClearAll();
    AbasisN.Nshell=Nshell;

    // AcutN[Nshell].ClearAll();
    // AcutN[Nshell].Nshell=Nshell;

    sprintf(CNsites,"%d",Nshell);
    //strcpy(ext,"test_N");
    strcpy(ext,ThisCode.SaveArraysFileName);
    strcat(ext,"_N");
    strcat(ext,CNsites);
    strcat(ext,".bin");

    // Read Abasis
    strcpy(arqname,"Abasis_");
    strcat(arqname,ext);
    AbasisN.ReadBin(arqname);

    // Read Acut (read already)
    // strcpy(arqname,"Acut_");
    // strcat(arqname,ext);
    // AcutN[Nshell].ReadBin(arqname);

    // Set ChildStates in AcutN

    DM_NRG_SetChildSt(&AcutN[Nshell],&AbasisNp1);

    // Set RhoN from RhoNp1

    MyTime.restart();

    // FDM-NRG: Setting temperature
    DM=CalcDN(ThisCode.Lambda,Nshell-1);
    TempBar=Temp/DM;
    // Partition function
    ZN=AcutN[Nshell].PartitionFunc(1.0/TempBar);
    // Partition function for discarded states
    ZNdisc=AcutN[Nshell].PartitionFuncDisc(1.0/TempBar);
    cout << " Temp = " << Temp << " at N= " << Nshell
  	 << " TempBar = " << TempBar
  	 << " DN = " << DM
  	 << " ZN = " << ZN
  	 << " ZNdisc = " << ZNdisc
  	 << endl;

    ParamsTemp.clear();
    ParamsTemp.push_back(TempBar);
    //ParamsTemp.push_back(ZN); // NOT ZNdisc!!
    ParamsTemp.push_back(ZTot/pow(4.0,NshellMax-Nshell)); // =ZNdisc/wN !!
    // For 1-channel only!! d=4
    //wN.push_back(ZNdisc*pow(4.0,NshellMax-Nshell));
    /////////

    // Either CalcRhoN AND save it OR read it from file

    // Read RhoN
    strcpy(arqname,"rhoDM_");
    strcat(arqname,ext);

    if (ThisCode.CheckFileExists(arqname)){
      cout << " DM_NRG: Found file " << arqname << endl;
      RhoN[Nshell].ReadBin(arqname);
    }else{
      if (ThisCode.totalS){
	DM_NRG_CalcRhoN_withSU2(ParamsTemp,UseFDM,
				&AcutN[Nshell],&AcutN[Nshell+1],
				&AbasisNp1,&SingleSite,
				&RhoN[Nshell],&RhoN[Nshell+1]);

      }
      else{
	DM_NRG_CalcRhoN(ParamsTemp,UseFDM,
			&AcutN[Nshell],&AcutN[Nshell+1],&AbasisNp1,
			&RhoN[Nshell],&RhoN[Nshell+1]);
      }
      // if SU(2) symmetry
      RhoN[Nshell].SaveBin(arqname);
    }

    time_elapsed=MyTime.elapsed();
    cout << " Rho Nshell = " << Nshell
	 << " completed in " << time_elapsed << " secs " << endl;

    cout << " trace(Rho[Nshell= " << Nshell << " ]) = " << RhoN[Nshell].CalcTrace() << endl;
    // Debugging
//      if (Nshell==1){
//        RhoN[Nshell].PrintAllBlocks();
//     qnums[0]=-2.000;
//     qnums[1]=1.000;
//     iBl=RhoN[Nshell].GetBlockFromQNumbers(qnums);
//     RhoN[Nshell].PrintMatBlock(iBl,iBl);
//     qnums[0]=2.000;
//     qnums[1]=1.000;
//     iBl=RhoN[Nshell].GetBlockFromQNumbers(qnums);
//     RhoN[Nshell].PrintMatBlock(iBl,iBl);
//      }

    // Update AbasisNp1
    AbasisNp1.ClearAll();
    AbasisNp1=AbasisN;

    // Read Operators
    for (int iop=0;iop<NFermiOps;iop++){
      sprintf(CNmat,"%d",iop); // Get all SavedMatrices
      strcpy(arqname,"Mat");
      strcat(arqname,CNmat);
      strcat(arqname,"_");
      strcat(arqname,ext);
      OpArrayN[iop][Nshell].ReadBin(arqname);
      // Need a better way to do this but for now it will do:
      // Not needed!
      //OpArrayN[iop][Nshell].CheckForMatEl=OneChQSz_cd_check;

    }
    // end read operators

    // Debug
//     if ((Nshell==0)||(Nshell==1)){
//       cout << " Op1 : " << endl;
//       OpArrayN[0][Nshell].PrintAllBlocks();
//       cout << " Op2 : " << endl;
//       OpArrayN[1][Nshell].PrintAllBlocks();
//       cout << " DM-NRG: Abasis(Nshell="<<Nshell<<"): " << endl;
//       AbasisN.PrintBasisAll();
//       cout << " Op1 (Nshell="<< Nshell<<"): " << endl;
//       OpArrayN[0][Nshell].PrintAllBlocks();
//     }
    // end debug

  }
  // end loop in Nshell

  // Save RhoN matrices

  // Read RhoN matrices

  // Given AcutN, rhoN and the Operators, calculate the spectral density

  // Calculate rho_0_0 and rho_Costi. Good for debugging
//   DM_NRG_CalcSpecFuncs(&ThisCode,AcutN,RhoN,OpArrayN,0,0);
//   DM_NRG_CalcSpecFuncs(&ThisCode,AcutN,RhoN,OpArrayN,1,1);
//   DM_NRG_CalcSpecFuncs(&ThisCode,AcutN,RhoN,OpArrayN,2,2);

   CSpecFunction spec1;

   spec1.Lambda=ThisCode.Lambda;
   spec1.z_twist=ThisCode.code_z_twist;
   spec1.NshellMax=ThisCode.Nsitesmax;
   spec1.NshellMin=ThisCode.Nsites0;
   spec1.AcutN=AcutN;
   spec1.RhoN=RhoN;
   spec1.UseCFS=UseCFS;

   // Spec Dens
   if (UseCFS==2) spec1.BDelta=LogGaussDelta; // FDM calculation
   else spec1.BDelta=BroadDelta;
   //spec1.dBroad=0.5*log(ThisCode.Lambda);
   spec1.dBroad=bbroad;

   // Finite Temp stuff (need to test this!)
   if (UseCFS==1) spec1.BDeltaTemp=LorentzDeltaAnders;
   else if (UseCFS==2) spec1.BDeltaTemp=GaussDelta;
   else spec1.BDeltaTemp=LorentzDelta;

   //spec1.Temp=0.0;
   //spec1.Mtemp=1000;
   spec1.Temp=Temp;
   spec1.Mtemp=Mtemp;
   spec1.Betabar=betabar;

   spec1.TwindowFac=twindow;
   spec1.dBroadTemp=broadtemp;

   if (UseGap){
     spec1.Gap=ThisCode.dInitParams[3];
     cout << " Using gap = " << spec1.Gap << endl;
   }
   // end setting up the gap]

   // Debug (Mar 2016)
   //DM_NRG_CalcSpecFunc_ij(&spec1,OpArrayN,1,1,UseCFS,UseGap,Nw);

   // Calculate ALL spectral functions!!
   // Jun 2015: Only diagonal functions for now...
   for (int iop=0; iop<NFermiOps; iop++){
     DM_NRG_CalcSpecFunc_ij(&spec1,OpArrayN,iop,iop,UseGap,Nw);
//     if (UseCFS!=0){
//       DM_NRG_CalcSpecFunc_ij(&spec1,OpArrayN,iop,iop,UseCFS,Nw);
//     } else {
//       for (int jop=0; jop<NFermiOps; jop++){
//	 DM_NRG_CalcSpecFunc_ij(&spec1,OpArrayN,iop,jop,UseCFS,Nw);
//       }
//     }
//     // end if CFS
   }
   // end loop in Fermi Ops

   //int iop=0;
   //int jop=0;
   //CFS_CalcSpecFunc_ij(&spec1,OpArrayN,iop,jop);

  delete[] RhoN;
  delete[] AcutN;
  for (int iop=NFermiOps-1;iop>=0;iop--){
    delete[] OpArrayN[iop];
  }
  delete[] OpArrayN;

}
// end MAIN


#include <algorithm>
#include <cmath>
#include <iostream>
#include <fstream>
#include <iterator>

#include <minbase/crossplat.h>
#include <minimgapi/minimgapi-helpers.hpp>
#include <minimgapi/imgguard.hpp>
#include <minimgio/minimgio.h>
#include <mximg/image.h>
#include <mximg/ocv.h>
#include <vi_cvt/std/exception_macros.hpp>
#include <vi_cvt/ocv/image.hpp>

#include <roadseg/road_distance_func.h>
#include <roadseg/road_vertex.h>
#include <remseg/segmentator.hpp>
#include <remseg/utils.h>

#include <opencv2/opencv.hpp>

THIRDPARTY_INCLUDES_BEGIN
#include <boost/filesystem.hpp>
#include <boost/serialization/vector.hpp>
#include <tclap/CmdLine.h>
THIRDPARTY_INCLUDES_END

namespace bfs = boost::filesystem;

using namespace vi::remseg;
using namespace roadseg;

void prepare_image(mximg::PImage &xyz_mximage, std::string const &depth_img_path)
{
  cv::Mat cv_depth_image = cv::imread(depth_img_path, cv::IMREAD_ANYDEPTH);
  DECLARE_GUARDED_MINIMG(xyz_image);
  // 12-channels hack to write 3-channel 32-bit float image as uint_8 one
  NewMinImagePrototype(&xyz_image, cv_depth_image.cols, cv_depth_image.rows, 12, MinTyp::TYP_UINT8);
  if (cv_depth_image.type() != CV_16U)
  {
    throw std::runtime_error("Wrong depth image format. Expected image's type to be uint16_t");
  }
  auto data_path = bfs::path(depth_img_path).parent_path();
  std::string K_path = data_path.append("K").string();
  std::ifstream ifs(K_path);
  std::istream_iterator<float> start(ifs), end;
  std::vector<float> K_coefs(start, end);
  const auto fx = K_coefs[0];
  const auto cx = K_coefs[2];
  const auto fy = K_coefs[4];
  const auto cy = K_coefs[5];
  for (int i = 0; i < cv_depth_image.rows; ++i)
    for (int j = 0; j < cv_depth_image.cols; ++j)
    {
      auto depth = cv_depth_image.at<ushort>(i, j);
      float x = 0;
      float y = 0;
      float z = 0;
      if (depth > 0)
      {
        z = depth / 1000.;
        x = z * (j - cx) / fx;
        y = z * (i - cy) / fy;
      }
      GetMinImageLineAs<float>(&xyz_image, i)[j * 3] = x;
      GetMinImageLineAs<float>(&xyz_image, i)[j * 3 + 1] = y;
      GetMinImageLineAs<float>(&xyz_image, i)[j * 3 + 2] = z;
    }
  xyz_mximage = mximg::createByOwning(xyz_image);
}

Eigen::Vector3d get_eigen_vector(RoadVertex* vertex) {
  auto hs =  vertex->getHelperStats();
  hs.eigSolver.compute(hs.covariance);
  return hs.eigSolver.eigenvectors().col(0);
}

bool check_angles(RoadVertex* a, RoadVertex* b, float cos_angle_thresh) {
  auto vec_a = get_eigen_vector(a);
  auto vec_b = get_eigen_vector(b);

  return std::abs(vec_a.dot(vec_b)) > cos_angle_thresh;
}

bool check_segment_is_plane(RoadVertex *vertex)
{
  auto hs = vertex->getHelperStats();
  auto eig_vals = hs.eigenvalues();
  const float EPS = 1e-7f;
  return (eig_vals[1] > 1e-7f) && (eig_vals[0] / eig_vals[1] < 0.01); // no points or lines segments to merge
}

void prepare_merge_list(std::vector<std::vector<SegmentID>> &segments_to_merge, Segmentator<RoadVertex> &segmentator, float max_angle) {
  auto image_map = segmentator.getImageMap();
  auto stats = image_map.getSegmentStats();
  std::vector<bool> segments_to_check(image_map.getWidth() * image_map.getHeight(), false);
  for (int i = 0; i < image_map.getHeight(); ++i)
    for (int j = 0; j < image_map.getWidth(); ++j){
      auto id = image_map.getSegment({j, i});
      if (check_segment_is_plane(segmentator.vertexById(id))) {
        segments_to_check[id] = true;
      }
    }
  auto cos_angle_thresh = cos(max_angle);

  auto last_segment_checked = std::find(segments_to_check.begin(), segments_to_check.end(), true);
  while (last_segment_checked != segments_to_check.end())
  {
    int start_id = last_segment_checked - segments_to_check.begin();
    segments_to_merge.push_back({start_id});
    segments_to_check[start_id] = false;

    std::queue<SegmentID> bfs_ids;
    bfs_ids.emplace(start_id);
    while (bfs_ids.size())
    {
      auto checking_id = bfs_ids.front();
      bfs_ids.pop();
      for (auto const& neighbour_id : stats[checking_id].neighbours) {
        if (!segments_to_check[neighbour_id]) {
          continue;
        }
        if (check_angles(segmentator.vertexById(checking_id), segmentator.vertexById(neighbour_id), cos_angle_thresh)) {
          segments_to_merge.back().emplace_back(neighbour_id);
          segments_to_check[neighbour_id] = false;
          bfs_ids.emplace(neighbour_id);
        }
      }
    }
    last_segment_checked = std::find(last_segment_checked, segments_to_check.end(), true);
  }
}

void merge_planes(Segmentator<RoadVertex> &segmentator, float max_angle, i8r::PLogger & dbg)
{
  std::vector<std::vector<SegmentID>> segments_to_merge;
  prepare_merge_list(segments_to_merge, segmentator, max_angle);
  for (size_t i = 0; i < segments_to_merge.size(); ++i) {
    if (segments_to_merge[i].size() < 2) {
      continue;
    }
    auto absorbent_vertex = segmentator.vertexById(segments_to_merge[i][0]);
    for (size_t j = 1; j < segments_to_merge[i].size(); ++j) {
      auto vertex = segmentator.vertexById(segments_to_merge[i][j]);
      segmentator.merge(absorbent_vertex, vertex);
    }
    if (dbg->enabled()) {
      segmentator.updateMapping();
      DECLARE_GUARDED_MINIMG(vis);
      visualize(&vis, segmentator.getImageMap());
      dbg->save(std::to_string(i), "plane_merging", &vis, "");
    }
  }
  segmentator.updateMapping();
}

void save_mask(Segmentator<RoadVertex> const& segmentator, std::string const& basename, std::string const& output) {
  SegmentStat max_area_vertex;
  SegmentID max_id = 0;
  const auto map = segmentator.getImageMap();
  const auto stats = map.getSegmentStats();
  int total_area_sum = 0;
  const int max_area = map.getWidth() * map.getHeight();
  for (auto const& stat : stats) {
    if (!total_area_sum || stat.second.area > max_area_vertex.area) {
      max_id = stat.first;
      max_area_vertex = stat.second;
    }
    total_area_sum += stat.second.area;
    if (total_area_sum >  max_area - max_area_vertex.area) {
      break;
    }
  }

  cv::Mat mask(map.getHeight(), map.getWidth(), CV_8UC1);
  for (int i = 0; i < map.getHeight(); ++i) {
    for (int j = 0; j < map.getWidth(); ++j) {
      mask.at<uchar>(i, j) =  (map.getSegment(j , i) == max_id) ? UINT_MAX : 0;
    }
  }
  std::string const mask_filename = bfs::absolute(basename + "_mask.png", output).string();
  cv::imwrite(mask_filename, mask);
}

int main(int argc, const char *argv[])
{
  i8r::AutoShutdown i8r_shutdown;

  TCLAP::CmdLine cmd("Run Range-Based Region Merge Segmentation on a Single image");
  TCLAP::ValueArg<double> errorLimit("e", "error_limit", "average error limit", false, -1, "double", cmd);
  TCLAP::ValueArg<int> segmentsLimit("n", "segm_limit", "segments limit", false, -1, "int", cmd);
  TCLAP::ValueArg<int> blockSize("b", "block_size", "initial size of regions", false, 3, "int", cmd);
  TCLAP::UnlabeledValueArg<std::string> imagePath("image", "path to source xyz-image", true, "", "string", cmd);
  TCLAP::ValueArg<std::string> output("o", "output", "path to output dir", false, ".", "string", cmd);
  TCLAP::SwitchArg debug("d", "debug", "debug mode", cmd, false);
  TCLAP::ValueArg<int> debugIter("i", "debug_iter", "debug iterations", false, 1, "int", cmd);
  TCLAP::ValueArg<int> maxSegments("s", "max_segments", "max segments for debug output", false, -1, "int", cmd);
  TCLAP::ValueArg<double> maxEigVal("v", "max_eigen_val", "maximum eigen val to consider region as a plane", false, 0.1, "double", cmd);
  TCLAP::ValueArg<double> maxAngle("a", "max_angle", "maximum angle to consider ", false, 0, "double", cmd);

  cmd.parse(argc, argv);

  RoadVertex::setMaxFirstEigValThresh(maxEigVal.getValue());

  if (!bfs::is_directory(output.getValue()))
    throw std::runtime_error("Failed to find output directory " + output.getValue());

  if (debug.getValue())
    i8r::configure(output.getValue());

  std::string const basename = bfs::path(imagePath.getValue()).stem().string();
  std::string const imgres_filename = bfs::absolute(basename + ".png", output.getValue()).string();

  try
  {
    mximg::PImage image;
    prepare_image(image, imagePath.getValue());

    auto dbg = i8r::logger("debug." + basename + ".pointlike");

    Segmentator<RoadVertex> segmentatorPlanes(*image, error, criteria, true);
    // group pixels into blocks
    std::cout << "Prepare blocks" << std::endl;
    for (int i = 0; i < (*image)->height; i += blockSize.getValue())
    {
      for (int j = 0; j < (*image)->width; j += blockSize.getValue())
      {
        std::cout << "Block: " << i << ", " << j << "\n";
        for (int ki = 0; ki < std::min<int>(blockSize.getValue(), (*image)->height - i); ++ki)
        {
          for (int kj = 0; kj < std::min<int>(blockSize.getValue(), (*image)->width - j); ++kj)
          {
            if ((ki == 0) && (kj == 0)) {
              continue;
            }
            auto top_left_block_corner = segmentatorPlanes.pointToVertex({j, i});
            auto block_point = segmentatorPlanes.pointToVertex({j + kj, i + ki});
            segmentatorPlanes.merge(top_left_block_corner, block_point);
          }
        }
      }
    }
    segmentatorPlanes.updateMapping();

    segmentatorPlanes.mergeToLimit(-1, errorLimit.getValue(), segmentsLimit.getValue(),
                                   dbg, debugIter.getValue(), maxSegments.getValue());

    merge_planes(segmentatorPlanes, maxAngle.getValue(), dbg);
    const ImageMap &imageMap = segmentatorPlanes.getImageMap();

    DECLARE_GUARDED_MINIMG(imgres);
    visualize(&imgres, imageMap);
    THROW_ON_MINERR(SaveMinImage(imgres_filename.c_str(), &imgres));
    save_mask(segmentatorPlanes, basename, output.getValue());
  }
  catch (std::exception const &e)
  {
    std::cerr << "Exception caught: " << e.what() << "\n";
    return 1;
  }
  catch (...)
  {
    std::cerr << "UNTYPED exception\n";
    return 2;
  }
  return 0;
}


/*
 * Copyright 2015, Andrea Del Prete, LAAS-CNRS
 *
 */

#include <Eigen/Dense>

#include <dynamic-graph/factory.h>
#include <sot/core/debug.hh>
#include <sot/torque_control/joint-torque-controller.hh>
#include <sot/torque_control/commands-helper.hh>

namespace dynamicgraph {
namespace sot {
namespace torque_control {

#define MODEL_INPUT_SIGNALS                                                                                \
  m_motorParameterKt_pSIN << m_motorParameterKt_nSIN << m_motorParameterKf_pSIN << m_motorParameterKf_nSIN \
                          << m_motorParameterKv_pSIN << m_motorParameterKv_nSIN << m_motorParameterKa_pSIN \
                          << m_motorParameterKa_nSIN << m_polySignDqSIN

#define ESTIMATOR_INPUT_SIGNALS                                                                   \
  m_jointsPositionsSIN << m_jointsVelocitiesSIN << m_jointsAccelerationsSIN << m_jointsTorquesSIN \
                       << m_jointsTorquesDerivativeSIN

#define TORQUE_INTEGRAL_INPUT_SIGNALS m_KiTorqueSIN << m_torque_integral_saturationSIN
#define TORQUE_CONTROL_INPUT_SIGNALS \
  m_jointsTorquesDesiredSIN << m_KpTorqueSIN << m_KdTorqueSIN << m_coulomb_friction_compensation_percentageSIN
#define VEL_CONTROL_INPUT_SIGNALS m_dq_desSIN << m_KdVelSIN << m_KiVelSIN

#define ALL_INPUT_SIGNALS                                                                  \
  ESTIMATOR_INPUT_SIGNALS << TORQUE_INTEGRAL_INPUT_SIGNALS << TORQUE_CONTROL_INPUT_SIGNALS \
                          << VEL_CONTROL_INPUT_SIGNALS << MODEL_INPUT_SIGNALS
#define ALL_OUTPUT_SIGNALS m_uSOUT << m_torque_error_integralSOUT << m_smoothSignDqSOUT

namespace dynamicgraph = ::dynamicgraph;
using namespace dynamicgraph;
using namespace dynamicgraph::command;
using namespace std;
using namespace Eigen;

/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef JointTorqueController EntityClassName;

/* --- DG FACTORY ------------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(JointTorqueController, "JointTorqueController");

/* --- CONSTRUCTION ----------------------------------------------------- */
/* --- CONSTRUCTION ----------------------------------------------------- */
/* --- CONSTRUCTION ----------------------------------------------------- */
JointTorqueController::JointTorqueController(const std::string& name)
    : Entity(name),
      CONSTRUCT_SIGNAL_IN(jointsPositions, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(jointsAccelerations, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(jointsTorques, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(jointsTorquesDerivative, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(jointsTorquesDesired, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(dq_des, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(KpTorque, dynamicgraph::Vector)  // proportional gain for torque feedback controller
      ,
      CONSTRUCT_SIGNAL_IN(KiTorque, dynamicgraph::Vector)  // integral gain for torque feedback controller
      ,
      CONSTRUCT_SIGNAL_IN(KdTorque, dynamicgraph::Vector)  // derivative gain for torque feedback controller
      ,
      CONSTRUCT_SIGNAL_IN(KdVel, dynamicgraph::Vector)  // derivative gain for velocity feedback
      ,
      CONSTRUCT_SIGNAL_IN(KiVel, dynamicgraph::Vector)  // integral gain for velocity feedback
      ,
      CONSTRUCT_SIGNAL_IN(torque_integral_saturation, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(coulomb_friction_compensation_percentage, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(motorParameterKt_p, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(motorParameterKt_n, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(motorParameterKf_p, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(motorParameterKf_n, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(motorParameterKv_p, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(motorParameterKv_n, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(motorParameterKa_p, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(motorParameterKa_n, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(polySignDq, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_OUT(u, dynamicgraph::Vector,
                           ESTIMATOR_INPUT_SIGNALS << TORQUE_CONTROL_INPUT_SIGNALS << VEL_CONTROL_INPUT_SIGNALS
                                                   << MODEL_INPUT_SIGNALS << m_torque_error_integralSOUT),
      CONSTRUCT_SIGNAL_OUT(smoothSignDq, dynamicgraph::Vector, m_jointsVelocitiesSIN),
      CONSTRUCT_SIGNAL_OUT(torque_error_integral, dynamicgraph::Vector,
                           m_jointsTorquesSIN << m_jointsTorquesDesiredSIN << TORQUE_INTEGRAL_INPUT_SIGNALS) {
  Entity::signalRegistration(ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);

  /* Commands. */
  addCommand("getTimestep", makeDirectGetter(*this, &m_dt, docDirectGetter("Control timestep", "double")));

  addCommand("init", makeCommandVoid2(*this, &JointTorqueController::init,
                                      docCommandVoid2("Initialize the controller.", "Control timestep [s].",
                                                      "Robot reference (string)")));
  addCommand("reset_integral", makeCommandVoid0(*this, &JointTorqueController::reset_integral,
                                                docCommandVoid0("Reset the integral error.")));
}

/* --- COMMANDS ---------------------------------------------------------- */
/* --- COMMANDS ---------------------------------------------------------- */
/* --- COMMANDS ---------------------------------------------------------- */
void JointTorqueController::init(const double& timestep, const std::string& robot_ref) {
  assert(timestep > 0.0 && "Timestep should be > 0");
  if (!m_jointsVelocitiesSIN.isPlugged())
    return SEND_MSG("Init failed: signal jointsVelocities is not plugged", MSG_TYPE_ERROR);
  if (!m_jointsTorquesSIN.isPlugged())
    return SEND_MSG("Init failed: signal m_jointsTorquesSIN is not plugged", MSG_TYPE_ERROR);
  if (!m_jointsTorquesDesiredSIN.isPlugged())
    return SEND_MSG("Init failed: signal m_jointsTorquesDesiredSIN is not plugged", MSG_TYPE_ERROR);
  if (!m_KpTorqueSIN.isPlugged()) return SEND_MSG("Init failed: signal m_KpTorqueSIN is not plugged", MSG_TYPE_ERROR);
  if (!m_KiTorqueSIN.isPlugged()) return SEND_MSG("Init failed: signal m_KiTorqueSIN is not plugged", MSG_TYPE_ERROR);

  /* Retrieve m_robot_util  informations */
  std::string localName(robot_ref);
  if (isNameInRobotUtil(localName)) {
    m_robot_util = getRobotUtil(localName);
  } else {
    SEND_MSG("You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
    return;
  }

  m_dt = timestep;
  m_tau_star.setZero(m_robot_util->m_nbJoints);
  m_current_des.setZero(m_robot_util->m_nbJoints);
  m_tauErrIntegral.setZero(m_robot_util->m_nbJoints);
  //        m_dqDesIntegral.setZero(m_robot_util->m_nbJoints);
  m_dqErrIntegral.setZero(m_robot_util->m_nbJoints);
}

void JointTorqueController::reset_integral() {
  m_tauErrIntegral.setZero();
  m_dqErrIntegral.setZero();
}

/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */

DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector) {
  // const Eigen::VectorXd& q                  = m_jointsPositionsSIN(iter);
  const Eigen::VectorXd& dq = m_jointsVelocitiesSIN(iter);
  const Eigen::VectorXd& ddq = m_jointsAccelerationsSIN(iter);
  const Eigen::VectorXd& tau = m_jointsTorquesSIN(iter);
  const Eigen::VectorXd& dtau = m_jointsTorquesDerivativeSIN(iter);
  const Eigen::VectorXd& tau_d = m_jointsTorquesDesiredSIN(iter);
  //        const Eigen::VectorXd& dtau_d             = m_jointsTorquesDesiredDerivativeSIN(iter);
  const Eigen::VectorXd& dq_des = m_dq_desSIN(iter);
  const Eigen::VectorXd& kp = m_KpTorqueSIN(iter);
  const Eigen::VectorXd& kd = m_KdTorqueSIN(iter);
  const Eigen::VectorXd& kd_vel = m_KdVelSIN(iter);
  const Eigen::VectorXd& ki_vel = m_KiVelSIN(iter);
  const Eigen::VectorXd& tauErrInt = m_torque_error_integralSOUT(iter);
  const Eigen::VectorXd& colFricCompPerc = m_coulomb_friction_compensation_percentageSIN(iter);
  const Eigen::VectorXd& motorParameterKt_p = m_motorParameterKt_pSIN(iter);
  const Eigen::VectorXd& motorParameterKt_n = m_motorParameterKt_nSIN(iter);
  const Eigen::VectorXd& motorParameterKf_p = m_motorParameterKf_pSIN(iter);
  const Eigen::VectorXd& motorParameterKf_n = m_motorParameterKf_nSIN(iter);
  const Eigen::VectorXd& motorParameterKv_p = m_motorParameterKv_pSIN(iter);
  const Eigen::VectorXd& motorParameterKv_n = m_motorParameterKv_nSIN(iter);
  const Eigen::VectorXd& motorParameterKa_p = m_motorParameterKa_pSIN(iter);
  const Eigen::VectorXd& motorParameterKa_n = m_motorParameterKa_nSIN(iter);
  const Eigen::VectorXd& polySignDq = m_polySignDqSIN(iter);
  //        const Eigen::VectorXd& dq_thr =        m_dq_thresholdSIN(iter);

  m_tau_star = tau_d + kp.cwiseProduct(tau_d - tau) + tauErrInt - kd.cwiseProduct(dtau);

  int offset = 0;
  if (dq.size() == (int)(m_robot_util->m_nbJoints + 6)) offset = 6;

  m_dqErrIntegral += m_dt * ki_vel.cwiseProduct(dq_des - dq);
  const Eigen::VectorXd& err_int_sat = m_torque_integral_saturationSIN(iter);
  // saturate
  bool saturating = false;
  for (int i = 0; i < (int)m_robot_util->m_nbJoints; i++) {
    if (m_dqErrIntegral(i) > err_int_sat(i)) {
      saturating = true;
      m_dqErrIntegral(i) = err_int_sat(i);
    } else if (m_dqErrIntegral(i) < -err_int_sat(i)) {
      saturating = true;
      m_dqErrIntegral(i) = -err_int_sat(i);
    }
  }
  if (saturating) SEND_INFO_STREAM_MSG("Saturate dqErr integral: " + toString(m_dqErrIntegral.head<12>()));

  for (int i = 0; i < (int)m_robot_util->m_nbJoints; i++) {
    m_current_des(i) = motorModel.getCurrent(
        m_tau_star(i),
        dq(i + offset) + kd_vel(i) * (dq_des(i) - dq(i + offset)) +
            m_dqErrIntegral(i),  // ki_vel(i)*(m_dqDesIntegral(i)-q(i)),
        ddq(i + offset), motorParameterKt_p(i), motorParameterKt_n(i), motorParameterKf_p(i) * colFricCompPerc(i),
        motorParameterKf_n(i) * colFricCompPerc(i), motorParameterKv_p(i), motorParameterKv_n(i),
        motorParameterKa_p(i), motorParameterKa_n(i), static_cast<unsigned int>(polySignDq(i)));
  }

  s = m_current_des;
  return s;
}

DEFINE_SIGNAL_OUT_FUNCTION(torque_error_integral, dynamicgraph::Vector) {
  const Eigen::VectorXd& tau = m_jointsTorquesSIN(iter);
  const Eigen::VectorXd& tau_d = m_jointsTorquesDesiredSIN(iter);
  const Eigen::VectorXd& err_int_sat = m_torque_integral_saturationSIN(iter);
  const Eigen::VectorXd& ki = m_KiTorqueSIN(iter);

  // compute torque error integral and saturate
  m_tauErrIntegral += m_dt * ki.cwiseProduct(tau_d - tau);
  for (int i = 0; i < (int)m_robot_util->m_nbJoints; i++) {
    if (m_tauErrIntegral(i) > err_int_sat(i))
      m_tauErrIntegral(i) = err_int_sat(i);
    else if (m_tauErrIntegral(i) < -err_int_sat(i))
      m_tauErrIntegral(i) = -err_int_sat(i);
  }

  s = m_tauErrIntegral;
  return s;
}

DEFINE_SIGNAL_OUT_FUNCTION(smoothSignDq, dynamicgraph::Vector) {
  const Eigen::VectorXd& dq = m_jointsVelocitiesSIN(iter);
  const Eigen::VectorXd& polySignDq = m_polySignDqSIN(iter);
  if (s.size() != (int)m_robot_util->m_nbJoints) s.resize(m_robot_util->m_nbJoints);

  for (int i = 0; i < (int)m_robot_util->m_nbJoints; i++)
    s(i) = motorModel.smoothSign(dq[i], 0.1, static_cast<unsigned int>(polySignDq[i]));
  // TODO Use Eigen binaryexpr
  return s;
}

void JointTorqueController::display(std::ostream& os) const {
  os << "JointTorqueController " << getName() << ":\n";
  try {
    getProfiler().report_all(3, os);
  } catch (ExceptionSignal e) {
  }
}

}  // namespace torque_control
}  // namespace sot
}  // namespace dynamicgraph