/**
 *  staticplan.cpp
 *
 *  Generate a static plan for a planar vehicle to satisfy a DBA
 *  by using abstraction-based control synthesis.
 *
 *  Created by Yinan Li on Feb. 8, 2021.
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <cstdlib>
#include <sys/stat.h>
#include <boost/algorithm/string.hpp>

#include "src/DBAparser.h"
#include "src/abstraction.hpp"

#include "src/bsolver.hpp"
#include "src/patcher.h"

#include "src/hdf5io.h"

#include "car.hpp"

int main(int argc, char *argv[])
{
    /**
     * Default values
     **/
    std::string specfile;
    double eta[]{0.2, 0.2, 0.2}; /* partition precision */

    /* Input arguments:
     * carAbst dbafile precision(e.g. 0.2 0.2 0.2)
     */
    if (argc < 2 || argc > 5) {
	std::cout << "Improper number of arguments.\n";
	std::exit(1);
    }
    specfile = std::string(argv[1]);
    if (argc > 2 && argc < 4) {
	std::cout << "Input precision should be of 3-dim, e.g. 0.2 0.2 0.2.\n";
	std::exit(1);
    }
    if (argc == 5) {
	for(int i = 2; i < 5; ++i)
	    eta[i-2] = std::atof(argv[i]);
    }
    std::cout << "Partition precision: " << eta[0] << ' '
	      << eta[1] << ' ' << eta[2] << '\n';

    clock_t tb, te;
    /* set the state space */
    const double theta = 3.5;
    double xlb[] = {0, 0, -theta};
    double xub[] = {10, 10, theta};
    /* set the control values */
    double ulb[] = {-1.0, -1.0};
    double uub[] = {1.0, 1.0};
    double mu[] = {0.3, 0.3};
    /* define the control system */
    rocs::DTCntlSys<carde> car("DBA", tau, carde::n, carde::m);
    car.init_workspace(xlb, xub);
    car.init_inputset(mu, ulb, uub);

    /**
     * Construct and save the abstraction
     */
    // const double eta[] = {0.2, 0.2, 0.2}; /* set precision */
    rocs::abstraction<rocs::DTCntlSys<carde>> abst(&car);
    abst.init_state(eta, xlb, xub);
    std::cout << "The number of abstraction states: " << abst._x._nv << '\n';
    /* Assign the label of avoid area to -1 */
    rocs::UintSmall nAvoid = 6;
    double obs[6][4] = {
	{0.0, 1.0, 4.6, 6.4},
	{2.0, 3.0, 0.0, 3.6},
	{3.5, 4.5, 8.5, 10.0}, //{3.5, 4.5, 7.5, 10.0},
	{5.5, 6.5, 0.0, 1.0},
	{5.5, 6.5, 3.4, 5.5}, //{5.5, 6.5, 3.4, 6.5},
	{6.5, 10.0, 4.5, 5.5} //{6.5, 10.0, 5.5, 6.5}
    };
    auto label_avoid = [&obs, &nAvoid, &abst, &eta](size_t i) {
    		     std::vector<double> x(abst._x._dim);
    		     abst._x.id_to_val(x, i);
    		     double c1= eta[0]/2.0; //+1e-10;
    		     double c2= eta[1]/2.0; //+1e-10;
    		     for(size_t i = 0; i < nAvoid; ++i) {
    			 if ((obs[i][0]-c1) <= x[0] && x[0] <= (obs[i][1]+c1) &&
    			     (obs[i][2]-c2) <= x[1] && x[1] <= (obs[i][3]+c2))
    			     return -1;
    		     }
    		     return 0;
    		 };
    abst.assign_labels(label_avoid);
    abst.assign_label_outofdomain(-1); // out of domain is banned
    std::vector<size_t> obstacles;
    for (size_t i = 0; i < abst._x._nv; ++i) {
    	if (abst._labels[i] < 0)
    	    obstacles.push_back(i);
    }
    /* Compute abstraction */
    std::string suffix;
    for(auto &item : eta) {
	std::stringstream ss;
    	ss << std::setprecision(1);
	ss << item;
	suffix += '-';
	suffix += ss.str();
    }
    std::string transfile = "abstfull" + suffix + ".h5";
    struct stat buffer;
    float tabst;
    if(stat(transfile.c_str(), &buffer) == 0) {
	/* Read from a file */
	std::cout << "Transitions have been computed. Reading transitions...\n";
	rocs::h5FileHandler transRdr(transfile, H5F_ACC_RDONLY);
	tb = clock();
	transRdr.read_transitions(abst._ts);
	te = clock();
	tabst = (float)(te - tb)/CLOCKS_PER_SEC;
	std::cout << "Time of reading abstraction: " << tabst << '\n';
	std::cout << "# of transitions: " << abst._ts._ntrans << '\n';
    } else {
	std::cout << "Transitions haven't been computed. Computing transitions...\n";
	/* Robustness margins */
	double e1[] = {0,0,0};
	double e2[] = {0,0,0};
	tb = clock();
	abst.assign_transitions(e1, e2);
	// abst.assign_transitions();
	te = clock();
	tabst = (float)(te - tb)/CLOCKS_PER_SEC;
	std::cout << "Time of computing abstraction: " << tabst << '\n';
	std::cout << "# of transitions: " << abst._ts._ntrans << '\n';
	/* Write abstraction to file */
	rocs::h5FileHandler transWtr(transfile, H5F_ACC_TRUNC);
	transWtr.write_transitions(abst._ts);
	transWtr.write_array<size_t>(obstacles, "obs");
	transWtr.write_array<double>(eta, carde::n, "eta");
	transWtr.write_2d_array<double>(abst._x._data, "xgrid");
	transWtr.write_problem_setting< rocs::DTCntlSys<carde> >(car);
    }

    /**
     * Read DBA from dba*.txt file
     */
    std::cout << "Reading the specification...\n";
    rocs::UintSmall nAP = 0, nNodes = 0, q0 = 0;
    std::vector<rocs::UintSmall> acc;
    std::vector<std::vector<rocs::UintSmall>> arrayM;
    if (!rocs::read_spec(specfile, nNodes, nAP, q0, arrayM, acc))
	std::exit(1);
    std::vector<std::string> tokens;
    boost::split(tokens, specfile, boost::is_any_of("."));

    /*
     * Assign labels to states: has to be consistent with the dba file.
     */
    rocs::UintSmall nGoal = 3;
    double e = tau/2.0;
    double goal[3][4] = {{0.5+e, 2.0-e, 7.5+e, 9.5-e},
			 {8.0, 0.8-e, 8.0, 0.8-e}, // {5.5, 0.5-e, 8.5, 0.5-e},
			 {7.5+e, 9.5-e, 0.8+e, 3.0-e}}; //{7.5+e, 9.5-e, 0.8+e, 4.0-e}
    auto label_target = [&goal, &nGoal, &abst, &eta](size_t i) {
		      std::vector<double> x(abst._x._dim);
		      abst._x.id_to_val(x, i);
		      // double c1= eta[0]/2.0; //+1e-10;
		      // double c2= eta[1]/2.0; //+1e-10;
		      double xl = x[0] - eta[0]/2.;
		      double xr = x[0] + eta[0]/2.;
		      double yl = x[1] - eta[1]/2.;
		      double yr = x[1] + eta[1]/2.;
		      boost::dynamic_bitset<> label(nGoal, false); // n is the number of goals
		      for(rocs::UintSmall i = 0; i < nGoal; ++i) {
			  if(i != 1) {
			  label[2-i] = (goal[i][0] <= xl && xr <= goal[i][1] &&
					goal[i][2] <= yl && yr <= goal[i][3])
			      ? true: false;
			  } else {
			      double rxr = xr - goal[i][0];
			      double rxl = xl - goal[i][0];
			      double ryr = yr - goal[i][2];
			      double ryl = yl - goal[i][2];
			      double xsqr = (rxr*rxr) < (rxl*rxl) ? (rxl*rxl) : (rxr*rxr);
			      double ysqr = (ryr*ryr) < (ryl*ryl) ? (ryl*ryl) : (ryr*ryr);
			      label[2-i] = (xsqr+ysqr)<goal[i][1]*goal[i][3] ? true : false;
			  }
		      }
		      return label.to_ulong();
		  };
    abst.assign_labels(label_target);

    std::cout << "Save labels to file.\n";
    std::string labelfile = "labels_" + tokens[0] + "_" + transfile;
    rocs::h5FileHandler labelWtr(labelfile, H5F_ACC_TRUNC);
    labelWtr.write_array<int>(abst._labels, "labels");
    std::cout << "Specification assignment is done.\n";

    /**
     * Solve a Buchi game on the product of NTS and DBA.
     */
    std::cout << "Start solving a Buchi game on the product of the abstraction and DBA...\n";
    rocs::BSolver solver; // memories will be allocated for psolver
    solver.construct_dba((int)nAP, (int)nNodes, (int)q0, acc, arrayM);
    tb = clock();
    solver.load_abstraction(abst);
    solver.generate_product(abst);
    solver.solve_buchigame_on_product();
    te = clock();
    float tsyn = (float)(te - tb)/CLOCKS_PER_SEC;
    std::cout << "Time of synthesizing controller: " << tsyn << '\n';

    /**
     * Display and save memoryless controllers.
     */
    std::string datafile = "controller_" + tokens[0] + "_";
    for(int i = 0; i < 3; ++i) {
    	std::stringstream ss;
    	ss << std::setprecision(1);
    	ss << eta[i];
    	datafile += ss.str();
    	if (i < 2)
    	    datafile += "-";
    }
    datafile += ".h5";
    std::cout << "Writing the controller...\n";
    // solver.write_controller_to_txt(const_cast<char*>(datafile.c_str()));
    rocs::h5FileHandler ctlrWtr(datafile, H5F_ACC_TRUNC);
    ctlrWtr.write_problem_setting< rocs::DTCntlSys<carde> >(car);
    // ctlrWtr.write_2d_array<double>(targetPts, "G");
    ctlrWtr.write_array<double>(eta, carde::n, "eta");
    ctlrWtr.write_2d_array<double>(abst._x._data, "xgrid");
    ctlrWtr.write_discrete_controller(&(solver._sol));
    std::cout << "Controller writing is done.\n";

    std::cout << "Total time of used (abstraction+synthesis): " << tabst+tsyn << '\n';

    /**
     * Create a patcher and save the winning graph
     */
    std::string winfile = "gwin.h5";
    rocs::Patcher local;
    std::cout << "Extracting the winning graph...\n";
    tb = clock();
    local.initialize_winning_graph(solver._sol);
    te = clock();
    std::cout << "Time of extracting the winning graph: " << (float)(te - tb)/CLOCKS_PER_SEC << '\n';

    /* Save the winning graph to a file */
    std::cout << "Writing the winning graph to file...\n";
    rocs::h5FileHandler graphWtr(winfile, H5F_ACC_TRUNC);
    tb = clock();
    if(graphWtr.write_winning_graph(local)) {
    	std::cout << "Error in saving the winning graph to file.\n";
    	return 1;
    }
    te = clock();
    std::cout << "Time of writing the winning graph: " << (float)(te - tb)/CLOCKS_PER_SEC << '\n';

    return 0;
}


/**
 * Copyright (c) 2012 Andrew Prock. All rights reserved.
 */
#include "card_distribution.h"

#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/math/special_functions/binomial.hpp>

#include <nit/error.h>
#include <nit/eval/card.h>
#include <nit/util/combinations.h>

namespace nit {

CardDistribution::CardDistribution() : m_handList(1, CardSet()), m_weights() {
  m_weights[CardSet()] = 1.0;
}

CardDistribution::CardDistribution(const CardSet& cs)
    : m_handList(1, cs), m_weights() {
  m_weights[cs] = 1.0;
}

CardDistribution::CardDistribution(const CardDistribution& cd) { *this = cd; }

CardDistribution& CardDistribution::operator=(const CardDistribution& other) =
    default;

size_t CardDistribution::size() const { return m_handList.size(); }

std::string CardDistribution::str() const {
  std::string ret;
  for (size_t i = 0; i < m_handList.size(); i++) {
    const CardSet& hand = m_handList[i];
    ret += (i > 0 ? "," : "") +
           (boost::format("%s=%.3f") % hand.str() % weight(hand)).str();
  }
  return ret;
}

void CardDistribution::clear() {
  m_handList.clear();
  m_weights.clear();
}

CardDistribution CardDistribution::data() const { return *this; }

void CardDistribution::fill(int n) {
  CardSet cards;
  cards.fill();
  fill(cards, n);
}

void CardDistribution::fill(const CardSet& cs, int n) {
  std::vector<Card> cards = cs.cards();
  auto setsize = static_cast<int>(cards.size());
  combinations hands(setsize, n);
  int vsize = boost::math::binomial_coefficient<double>(setsize, n);
  clear();
  m_handList.reserve(vsize);

  do {
    CardSet cs_i;
    for (int i = 0; i < n; i++)
      cs_i.insert(cards[hands[i]]);
    m_handList.push_back(cs_i);
    m_weights[cs_i] = 1.0;
  } while (hands.next());
}

const CardSet& CardDistribution::operator[](size_t index) const {
  if (index >= m_handList.size())
    throw LogicError("CardDistribution::operator: bounds error");
  return m_handList[index];
}

const double& CardDistribution::operator[](const CardSet& hand) const {
  static const double kStaticZero = 0.0;  // for hands not in distribution
  auto it = m_weights.find(hand);
  if (it == m_weights.end())
    return kStaticZero;
  return it->second;
}

double& CardDistribution::operator[](const CardSet& hand) {
  return m_weights[hand];
}

// this is a simple wrapper to make reading the code easier
const double& CardDistribution::weight(const CardSet& hand) const {
  return (*this)[hand];
}

bool CardDistribution::parse(const std::string& input) {
  clear();

  // trap the special "unrealized/empty" distribution which
  // has no specific cards, and is equivalent to a "random" dist
  // TODO: verify that this is the internal representation we want for a
  // random hand
  if (input == ".") {
    CardSet hand;
    if (hand.size() != 0)
      return false;
    m_weights[hand] = 1.0;
    m_handList.push_back(hand);
    return true;
  }

  std::vector<std::string> hands;
  boost::split(hands, input, boost::is_any_of(","));
  for (const std::string& h : hands) {
    // handle the weight
    double weight = 1.0;
    if (boost::contains(h, "=")) {
      // trap for the case where the input ends with "="
      std::string::size_type weightPos = h.rfind("=") + 1;
      if (weightPos == h.size())
        return false;
      weight = boost::lexical_cast<double>(h.substr(weightPos));
    }

    // handle the hand one card at a time.
    std::string handstr = h.substr(0, h.rfind("="));
    if (handstr.size() % 2 != 0)
      return false;
    CardSet hand;
    for (size_t i = 0; i < handstr.size(); i += 2) {
      Card c;
      if (!c.fromString(handstr.substr(i, i + 2)))
        return false;
      if (hand.contains(c))
        return false;
      hand.insert(c);
    }

    // final check and
    if (hand.size() == 0)
      return false;
    m_weights[hand] = weight;
    m_handList.push_back(hand);
  }
  return true;
}

std::string CardDistribution::display() const { return str(); }

void CardDistribution::removeCards(const CardSet& dead) {
  for (auto& i : m_handList)
    if (i.intersects(dead))
      m_weights[i] = 0.0;
}

double CardDistribution::weight() const {
  double total = 0.0;
  for (const std::pair<CardSet, double>& w : m_weights)
    total += w.second;
  return total;
}

}  // namespace nit


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_UTIL_SELECT_COORDINATE_TYPE_HPP
#define BOOST_GEOMETRY_UTIL_SELECT_COORDINATE_TYPE_HPP

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

/*!
    \brief Meta-function selecting the most precise coordinate type
        of two geometries
    \ingroup utility
 */
template <typename T1, typename T2>
struct select_coordinate_type
{
    typedef typename select_most_precise
        <
            typename coordinate_type<T1>::type,
            typename coordinate_type<T2>::type
        >::type type;
};

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_UTIL_SELECT_COORDINATE_TYPE_HPP


//==================================================//
///
/// @file   MyFFT.cpp
/// @brief  Implementation of MyFFT.
/// @author cogoto
///
/// [MyAll]
/// Copyright (c) 2021 cogoto
/// Released under the MIT license
/// https://opensource.org/licenses/mit-license.php
///
//==================================================//

#include "MyFFT.hpp"

#include <algorithm>

#include <chrono>

#include <cstdint>

#include <ctime>

//#include <execution>

#include <iostream>

#include <iomanip>

//#include <Eigen/Dense>

#include <complex>

#include <thread>

#include <time.h>

//

namespace MyAll {

//

using namespace std;

//using namespace Eigen;

//using fint = int_fast32_t;

//using ufint = uint_fast32_t;

//--------------------------------------------------//
///
/// @brief  First process. Initialize member variables.
///
//--------------------------------------------------//

MyFFT::MyFFT(const int &n_data__, const int &n_thread__) {

    print_ = false;

    MyFFT::Init(n_data__, n_thread__);

};

//--------------------------------------------------//
///
/// @brief   End process.
///
//--------------------------------------------------//

MyFFT::~MyFFT() {

};

//--------------------------------------------------//
///
/// @brief   Initialize calculation parameters.
/// @return  None.
///
//--------------------------------------------------//

void MyFFT::Init(const int &n_data__, const int &n_thread__) {

    if(!SetN(n_data__)) { exit(0); }

    //

    double theta = 2 * PI_ / n_;

    complex<double> Wo = complex<double> (cos(theta), -sin(theta));

    W_0_.reserve(n_half_);
    W_0_.push_back(1.0L);
    for(uint_fast32_t i=1;i<n_half_;i++) { W_0_.push_back(W_0_[i-1] * Wo); }

    //

    n_thread_ = n_thread__;

    int n_cpu = static_cast<int>(thread::hardware_concurrency());

    if(n_thread__ <= 0) {
        n_thread_ = n_cpu - 1;
        if(n_thread_ <= 0) { n_thread_ = 1; }
    }
    else if(n_thread_ > n_cpu) {
        n_thread_ = n_cpu;
    }

    if(print_) { cout << "[MyFFT] n_thread = " << n_thread_ << endl; }

    //cout << "[MyFFT] n_thread = " << n_thread_ << endl;

    //

    thread_exec_.reserve(n_thread_);
    //for(int i=0;i<n_thread_;i++) { thread_exec_.push_back(0); }
    for(int i=0;i<n_thread_;i++) { thread_exec_.push_back(-1); }

    //

    n_stream_ = static_cast<uint_fast32_t>(static_cast<double>(n_half_) / static_cast<double>(n_thread_));
    n_stream_ += 1;

    //cout << ":: " << n_ << " : " << n_thread_ << " : " << n_half_ << " : " << n_step_ << " : " << endl;

    //

    vector<vector<uint_fast32_t>>().swap(iW_);
    iW_.resize(n_step_);
    for(auto &&it:iW_) {
        //it.resize(n_);
        it.reserve(n_);
        for(uint_fast32_t i=0;i<n_;i++) { it.push_back(0); }
    }

    //iW_.resize(n_);

    //W_.reserve(n_); // cannot resize the object! (It will occur core-dump)
    //for(uint_fast32_t i=0;i<n_;i++) { W_.push_back(1.0L); }

    //

    vector<vector<uint_fast32_t>>().swap(m_);
    m_.resize(n_step_);
    for(auto &&it:m_) {
        //it.resize(n_half_);
        it.reserve(n_half_);
        for(uint_fast32_t i=0;i<n_half_;i++) { it.push_back(0); }
    }

    //m_.reserve(n_half_);
    //for(uint_fast32_t i=0;i<n_half_;i++) { m_.push_back(0); }

    n_block_half_ = n_;

    uint_fast32_t i0, sss;

    for(uint_fast32_t s=0;s<n_step_;s++) {

        n_block_ = n_block_half_;
        n_block_half_ >>= 1;

        i0 = 0;

        for(uint_fast32_t i1=0; i1<n_block_half_; i1++) {

            sss = (i1 << s);

            for(uint_fast32_t i2=i1; i2<n_; i2+=n_block_) {
                m_[s][i0] = i2;
                iW_[s][i2] = sss;
                i0++;
            }

        }

    }

    //

    i_bit_rev_.resize(n_);

    uint_fast32_t i2;

    for(uint_fast32_t i1=0;i1<n_;i1++) {

        i2 = i1;

        i2 = (((i2 & 0xaaaaaaaa) >> 1) | ((i2 & 0x55555555) << 1));
        i2 = (((i2 & 0xcccccccc) >> 2) | ((i2 & 0x33333333) << 2));
        i2 = (((i2 & 0xf0f0f0f0) >> 4) | ((i2 & 0x0f0f0f0f) << 4));
        i2 = (((i2 & 0xff00ff00) >> 8) | ((i2 & 0x00ff00ff) << 8));
        i2 = ((i2 >> 16) | (i2 << 16)) >> (32 - n_step_);

        if(i2 > i1) {
            i_bit_rev_[i1] = i2;
        }
        else {
            i_bit_rev_[i1] = i1;
            //i_bit_rev_[i1] = -1;
        }

    }

    //

    window_func_name_ = "";

    //SetWindow(""); // rectangular window function

    //

    /*
    uint_fast32_t thread_no = 0;

    vector<thread> ths( n_thread_ );

    for (auto &&th : ths) {

        th = thread(
            &MyFFT::_calc_butterfly,
            this,
            thread_no
        );

        thread_no++;

    }

    for (auto &&th : ths) {
        try {
            if(th.joinable()) {
                th.join();
            }
        } catch(const std::system_error& e) {
            cout << "Caught system_error with code " << e.code() << " meaning " << e.what() << endl;
        }
    }
    */

    //

    return;

};

//--------------------------------------------------//
///
/// @brief   Sets the number of data.
/// @return  bool.
///
//--------------------------------------------------//

bool MyFFT::SetN(const int &n_data__) {

    int n_check_pre = 0;
    int n_check_now = 1;

    for(int i=0;i<32;i++) {

        n_check_pre = n_check_now;

        n_check_now = (n_check_now << 1);

        if(n_check_now == n_data__) { break; }

        if(n_check_pre < n_data__ && n_data__ < n_check_now) {
            cerr << "[ERROR][MyFFT] The number of data is not a multiply of 2." << endl;
            return false;
        }

    }

    n_ = n_data__;

    n_half_ = (n_ >> 1);

    n_step_ = static_cast<uint_fast32_t>(log2(n_));

    //coeff_amp_ = 2.0 / n_;

    //coeff_amp_ = complex<double> (1.0 / n_, 0.0);

    return true;

}

//--------------------------------------------------//
///
/// @brief     Control standard outputs of current status.
/// @param[in] use__ : Print stdout? (yes=true, no=false)
/// @return    None.
///
//--------------------------------------------------//

void MyFFT::Print(const bool &use__){

    print_ = use__;

};

//--------------------------------------------------//
///
/// @brief   Gets frequency values.
/// @return  None.
///
//--------------------------------------------------//

std::vector<double> MyFFT::F(const double &df__, const double &offset__, const bool &wrap_back__) {

    df_ = df__;

    dt_ = 1 / (n_ * df_);

    vector<double> f;

    if(wrap_back__) { f.reserve(n_); }
    else            { f.reserve(n_half_+1); }

    for(uint_fast32_t i=0;i<=n_half_;i++) {
        f.push_back(df_ * i + offset__);
    }

    //f[n_half_] *= -1;

    if(wrap_back__) {

        for(int i=(n_half_-1);i>0;i--) {
            f.push_back(-1 * f[i]);
        }

    }

    return f;

}

//--------------------------------------------------//
///
/// @brief   Gets time values.
/// @return  None.
///
//--------------------------------------------------//

std::vector<double> MyFFT::T(const double &dt__, const double &offset__) {

    dt_ = dt__;

    df_ = 1 / (n_ * dt_);

    vector<double> t;

    t.reserve(n_);

    for(uint_fast32_t i=0;i<n_;i++) {
        t.push_back(dt_ * i + offset__);
    }

    return t;

}

//--------------------------------------------------//
///
/// @brief   Executing FFT
/// @return  None.
///
//--------------------------------------------------//

bool MyFFT::_FFT_CT_multi(const vector<double> &t__, vector<complex<double>> &f__) {

    if(n_ != static_cast<uint_fast32_t>(t__.size())) { return false; }

    chrono::system_clock::time_point start, end;
    //chrono::system_clock::time_point start1, end1;

    //struct timespec ts;
    //ts.tv_sec  = 0;
    //ts.tv_nsec = 0;

    start = chrono::system_clock::now();

    //f_ = &f__;

    vector<complex<double>> ().swap(f_);
    f_.reserve(n_);
    for(const auto &it:t__) {
        f_.push_back(complex<double> (it, 0.0));
    }

    //end = chrono::system_clock::now();
    //cout << "[FFT_CT] [init val] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end-start).count() << endl;

    //

    //start = chrono::system_clock::now();
    cout << ":" << endl;
    vector<thread> ths( n_thread_ );

    //end = chrono::system_clock::now();
    //cout << "[FFT_CT] [prep ths] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end-start).count() << endl;

    //

    //for (int i=0;i<n_thread_;i++) { thread_exec_[i] = 0; }

    //start = chrono::system_clock::now();

    uint_fast32_t thread_no = 0;

    thread_init_cnt_ = 0;

    for (auto &&th : ths) {

        uint_fast32_t i_begin = n_stream_ * thread_no;
        uint_fast32_t i_end   = i_begin + n_stream_;

        if(i_end > n_half_) { i_end = n_half_; }

        th = thread(
            &MyFFT::_calc_butterfly,
            this,
            thread_no,
            i_begin,
            i_end
        );

        //nanosleep(&ts, NULL);

        thread_no++;

    }

    //end = chrono::system_clock::now();
    //cout << "[FFT_CT] [make ths] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end-start).count() << endl;

    //

    //start = chrono::system_clock::now();

    //while(thread_init_cnt_ < n_thread_) {
    //    continue;
    //}

    while(1) {
        {
            lock_guard<mutex> lock(mtx_thread_);
            //lock_guard<recursive_mutex> lock(mtx_thread_);
            if(thread_init_cnt_ == n_thread_) { break; }
        }
    }

    //end = chrono::system_clock::now();
    //cout << "[FFT_CT] [wait ini] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end-start).count() << endl;

    //

    start = chrono::system_clock::now();

    n_block_half_ = n_;

    thread_cnt_ = 0;

    for(s_=0;s_<n_step_;s_++) {
    //for(uint_fast32_t s=1;s<=n_step_;s++) {

        n_block_ = n_block_half_;
        n_block_half_ >>= 1;

        //s_ = s;

        //

        //start1 = chrono::system_clock::now();

        /*{
            lock_guard<mutex> lock(mtx_thread_);
            //lock_guard<recursive_mutex> lock(mtx_thread_);
            n_calc_ = 0;
            for (int i=0;i<n_thread_;i++) {
                thread_exec_[i] = 1;
            }
        }*/

        {
            unique_lock<std::mutex> wait_lock(mtx_wait_);
            thread_ready_ = true;
        }

        cond_.notify_all();

        //end1 = chrono::system_clock::now();
        //cout << "[FFT_CT] [loop] [exec ths] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end1-start1).count() << endl;

        //

        /*
        start1 = chrono::system_clock::now();

        //int i_test = 0;

        for(;;) {
            {
                lock_guard<mutex> lock(mtx_thread_);
                if(thread_cnt_ >= 0) {
                    cout << ":: " << thread_cnt_ << endl;
                    break;
                }
            }
        }

        end1 = chrono::system_clock::now();
        cout << "[FFT_CT] [loop] [test] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end1-start1).count() << endl;
        */

        //

        //{
            //lock_guard<mutex> lock(mtx_thread_);
            //start1 = chrono::system_clock::now();
            //cout << "[FFT_CT] [loop] wait st = " << time_.HMS_nsec() << endl;
        //}

        int i = 0;
        //while(1) {
        for(;;) {
            {
                lock_guard<mutex> lock(mtx_thread_);
                //lock_guard<recursive_mutex> lock(mtx_thread_);
                //cout << thread_cnt_ << ":" << n_thread_ << endl;
                if(thread_cnt_ == n_thread_) { break; }
            }
            //if(thread_cnt_ >= n_thread_) {
                //cout << "wait count = " << i << endl;
            //    break;
            //}
            i++;
            //if(i==30) {cout << thread_cnt_ << endl;}
            //cout << thread_cnt_ << " " ;
            //cout << i << " " ;
            //nanosleep(&ts, NULL);
            //continue;
        }

        thread_cnt_ = 0;

        thread_ready_ = false;

        /*{
            lock_guard<mutex> lock(mtx_thread_);
            //cout << "[FFT_CT] [loop] wait et = " << time_.HMS_nsec() << endl;
            cout << "wait loop n = " << i << endl ;
            //end1 = chrono::system_clock::now();
            //cout << "[FFT_CT] [loop] [wait ths] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end1-start1).count() << endl;
        }*/

    }

    //

    {
        lock_guard<mutex> lock(mtx_thread_);
        //lock_guard<recursive_mutex> lock(mtx_thread_);
        for (int i=0;i<n_thread_;i++) {
            thread_exec_[i] = -1;
        }
    }

    //

    for (auto &&th : ths) {
        try {
            if(th.joinable()) {
                th.join();
            }
        } catch(const std::system_error& e) {
            cerr << "[FFT_CT] Caught system_error with code " << e.code() << " meaning " << e.what() << endl;
            return false;
        }
    }

    //end = chrono::system_clock::now();
    //cout << "[FFT_CT] [exec fft] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end-start).count() << endl;

    //
    // Reverse bits
    //

    //start = chrono::system_clock::now();

    complex<double> tmp_swap;

    int i1 = 0;

    for(const auto &i2 : i_bit_rev_) {
        tmp_swap = f_[i1];
        f_[i1] = f_[i2];
        f_[i2] = tmp_swap;
        i1++;
    }

    /*
    uint_fast32_t i2;

    for(uint_fast32_t i1=0;i1<n_;i1++) {
        if(i_bit_rev_[i1] != -1) {
            i2 = i_bit_rev_[i1];
            tmp_swap = f_[i1];
            f_[i1] = f_[i2];
            f_[i2] = tmp_swap;
        }
    }
    */

    /*
    uint_fast32_t i2;

    for(uint_fast32_t i1=0;i1<n_;i1++) {

        i2 = i1;

        i2 = (((i2 & 0xaaaaaaaa) >> 1) | ((i2 & 0x55555555) << 1));
        i2 = (((i2 & 0xcccccccc) >> 2) | ((i2 & 0x33333333) << 2));
        i2 = (((i2 & 0xf0f0f0f0) >> 4) | ((i2 & 0x0f0f0f0f) << 4));
        i2 = (((i2 & 0xff00ff00) >> 8) | ((i2 & 0x00ff00ff) << 8));
        i2 = ((i2 >> 16) | (i2 << 16)) >> (32 - n_step_);

        if(i2 > i1) {
            tmp_swap = f_[i1];
            f_[i1] = f_[i2];
            f_[i2] = tmp_swap;
        }

    }
    */

    //end = chrono::system_clock::now();
    //cout << "[FFT_CT] [bit rev.] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end-start).count() << endl;

    //

    //start = chrono::system_clock::now();

    f__ = f_;

    //end = chrono::system_clock::now();
    //cout << "[FFT_CT] [copy val] = " << fixed << setw(9) << right << chrono::duration_cast<chrono::nanoseconds>(end-start).count() << endl;

    //

    return true;

};

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

void MyFFT::_calc_butterfly(
    uint_fast32_t thread_no__,
    uint_fast32_t i_begin__,
    uint_fast32_t i_end__
) {

    //chrono::system_clock::time_point start, end;

    uint_fast32_t m1, m2, n_step;

    complex<double> tmp;

    //uint_fast32_t n_block_half = n_block_half_;

    {
        lock_guard<mutex> lock(mtx_thread_);
        //lock_guard<recursive_mutex> lock(mtx_thread_);
        //unique_lock<mutex> lock(mtx_thread_);
        thread_exec_[thread_no__] = 0;
        thread_init_cnt_++;
        //cout << "[_calc_butterfly] init end : " << thread_no__ << endl;
        n_step = n_step_;
    }

    //sleep(1);
    //this_thread::sleep_for(chrono::nanoseconds(1));

    //

    //int thread_exec = 0;

    //uint_fast32_t n_calc;

    uint_fast32_t n_block_half;

    for(uint_fast32_t s=0;s<n_step;s++) {

    //while(thread_exec_[thread_no__] >= 0) {
        //while(1) {

        //this_thread::sleep_for(chrono::nanoseconds(1));

        //cout << "th " << thread_no__ << " sts = " << thread_exec_[thread_no__] << endl;

        /*
        {
            lock_guard<mutex> lock(mtx_thread_);
            //lock_guard<recursive_mutex> lock(mtx_thread_);
            thread_exec = thread_exec_[thread_no__];
        }
        */

        {
            unique_lock<std::mutex> wait_lock(mtx_wait_);

            // データの準備ができるまで待機してから処理する
            cond_.wait(wait_lock, [this] { return thread_ready_; });

        //if(thread_exec_[thread_no__] == 1) {

            //start = chrono::system_clock::now();

            {
                lock_guard<mutex> lock(mtx_thread_);
                thread_exec_[thread_no__] = 0;
                n_block_half = n_block_half_;
                //cout << "[CALC] " << thread_no__ << " : st = " << time_.HMS_nsec() << endl;
            }

            //{
            //    lock_guard<mutex> lock(mtx_thread_);
            //    //lock_guard<recursive_mutex> lock(mtx_thread_);
            //    thread_exec_[thread_no__] = 0;
            //}

            for(uint_fast32_t i=i_begin__;i<i_end__;i++) {
                m1 = m_[s_][i];
                m2 = m1 + n_block_half;
                tmp = f_[m1] - f_[m2];
                f_[m1] += f_[m2];
                f_[m2] = tmp * W_0_[iW_[s_][m1]];
            }

            /*
            while(1) {

                {
                    lock_guard<mutex> lock(mtx_thread_);
                    n_calc = n_calc_;
                    n_calc_++;
                    if(n_calc_ >= n_half_) { break; }
                }

                m1 = m_[s_][n_calc];
                m2 = m1 + n_block_half;
                tmp = f_[m1] - f_[m2];
                f_[m1] += f_[m2];
                f_[m2] = tmp * W_0_[iW_[s_][m1]];

            }
            */

            //end = chrono::system_clock::now();

            {
                lock_guard<mutex> lock(mtx_thread_);
                //lock_guard<recursive_mutex> lock(mtx_thread_);
                //unique_lock<mutex> lock(mtx_thread_);
                thread_cnt_++;
                //cout << "[CALC] " << thread_no__ << " : et = " << time_.HMS_nsec() << endl;
                //cout << "[CALC] " << thread_no__ << " : dt =  " << fixed << setw(15) << right << chrono::duration_cast<chrono::nanoseconds>(end-start).count() << endl;
            }

            //thread_ready_ = false;

        }

        //if(thread_exec < 0) { break; }

        //this_thread::sleep_for(chrono::nanoseconds(100));

    }

    return;

};

/*
void MyFFT::_calc_butterfly(
    uint_fast32_t i_begin__,
    uint_fast32_t i_end__,
    uint_fast32_t &Nb_half__,
    vector<complex<double>> &f__,
    vector<complex<double>> &W__,
    vector<uint_fast32_t> &w__,
    vector<uint_fast32_t> &m__
) {

    uint_fast32_t i1;
    uint_fast32_t i2;
    complex<double> tmp;

    //cout << "[begin, end] = [" << i_begin__ << ", " << i_end__ << "]" << endl;

    if(calc_butterfly_) {

        for(uint_fast32_t i=i_begin__;i<i_end__;i++) {
            i1 = m__[i];
            i2 = i1 + Nb_half__;
            tmp = f__[i1] - f__[i2];
            f__[i1] += f__[i2];
            f__[i2] = tmp * W__[w__[i1]];
        }

    }

    return;

};
*/

//--------------------------------------------------//
///
/// @brief   Implementation of multi-thread FFT with a number of data.
/// @return  None.
///
//--------------------------------------------------//

void MyFFT::_calc_fft(
    const uint_fast32_t &thread_no__,
    vector<vector<complex<double>>> &x__,
    const int &n_set__,
    int &i_set__
) {

    int i_set;
    uint_fast32_t n;
    vector<int> i_bit_rev;

    {
        unique_lock<std::mutex> wait_lock(mtx_wait_);
        i_bit_rev = i_bit_rev_;
        n = n_;
    }

    while(1) {

        {
            unique_lock<std::mutex> wait_lock(mtx_wait_);

            i_set__++;
            i_set = i_set__;

            if(i_set__ >= n_set__) { break; }

            //cout << "[_calc_fft] th.no = " << thread_no__ << ", i_set = " << i_set__;
            //cout << ", size = " << x__.size() << ", " <<  x__[i_set].size() << endl;

        }

        _FFT_CT(x__[i_set], n, i_bit_rev);

        //cout << "[_calc_fft] fin " << endl;

    }

    return;

}

//--------------------------------------------------//
///
/// @brief   Executing FFT with a number of data.
/// @return  None.
///
//--------------------------------------------------//

bool MyFFT::FFT(
    const vector<complex<double>> &t__,
    vector<vector<complex<double>>> &f__,
    bool zero_padding__
) {

    int n_set = static_cast<int>(t__.size()/n_);

    if(zero_padding__) { n_set++; }

    vector<vector<complex<double>>> ().swap(f__);
    vector<complex<double>> f_part;

    f__.reserve(n_set);
    f_part.resize(n_);

    uint_fast32_t i1 = 0;

    for(const auto &it : t__) {

        f_part[i1] = it;

        i1++;

        if(i1 >= n_) {
            f__.push_back(f_part);
            i1 = 0;
        }

    }

    //cout << "size = " << f__.size() << endl;
    //cout << "part size =";
    //for(const auto &it:f__) { cout << " " << it.size() << ","; }
    //cout << endl;

    if(zero_padding__) {
        for(uint_fast32_t i2=i1;i2<n_;i2++) {
            f_part[i2] = complex<double> (0.0, 0.0);
        }
        f__.push_back(f_part);
    }

    //

    if(window_func_name_ != "") {
        for(auto &&it : f__) {
            ApplyWindow(it);
        }
    }

    //

    vector<thread> ths( n_thread_ );

    uint_fast32_t thread_no = 0;

    int i_set = -1;

    for (auto &&th : ths) {

        th = thread(
            &MyFFT::_calc_fft,
            this,
            thread_no,
            std::ref(f__),
            std::ref(n_set),
            std::ref(i_set)
        );

        thread_no++;

    }

    for (auto &&th : ths) {
        try {
            if(th.joinable()) {
                th.join();
            }
        } catch(const std::system_error& e) {
            cout << "Caught system_error with code " << e.code() << " meaning " << e.what() << endl;
        }
    }

    return true;

}

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

bool MyFFT::FFT(
    const vector<double> &t__,
    vector<vector<complex<double>>> &f__,
    bool zero_padding__
) {

    vector<complex<double>> t;
    t.reserve(t__.size());
    for(const auto &it:t__) {
        t.push_back(complex<double> (it, 0.0));
    }

    return FFT(t, f__);

}

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

/*
vector<vector<complex<double>>> MyFFT::FFT(
    const vector<double> &t__,
    bool zero_padding__
) {

    vector<complex<double>> t;
    t.reserve(t__.size());
    for(const auto &it:t__) {
        t.push_back(complex<double> (it, 0.0));
    }

    vector<vector<complex<double>>> f;

    FFT(t, f);

    return f;

}
*/

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

bool MyFFT::FFT(
    const vector<complex<double>> &t__,
    vector<complex<double>> &f__,
    bool zero_padding__
) {

    vector<vector<complex<double>>> f;

    FFT(t__, f);

    vector<complex<double>> ().swap(f__);
    f__.resize(n_);
    complex<double> zero(0.0, 0.0);

    for(auto &&it : f__) { it = zero; }

    for(auto &&it1 : f) {

        int i = 0;

        for(auto &&it2 : it1) {

            f__[i] += it2;
            i++;

        }

    }

    return true;

}

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

bool MyFFT::FFT(
    const vector<double> &t__,
    vector<complex<double>> &f__,
    bool zero_padding__
) {

    vector<complex<double>> t;
    t.reserve(t__.size());
    for(const auto &it:t__) {
        t.push_back(complex<double> (it, 0.0));
    }

    return FFT(t, f__);

}

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

vector<complex<double>> MyFFT::FFT(
    const vector<double> &t__,
    bool zero_padding__
) {

    vector<complex<double>> t;
    t.reserve(t__.size());
    for(const auto &it:t__) {
        t.push_back(complex<double> (it, 0.0));
    }

    vector<complex<double>> f;

    FFT(t, f);

    return f;

}

//--------------------------------------------------//
///
/// @brief   Executing FFT
/// @return  None.
///
//--------------------------------------------------//

bool MyFFT::_FFT_CT(
    vector<complex<double>> &x__,
    uint_fast32_t &n__,
    vector<int> &i_bit_rev__
) {

    double thetaT = PI_ / n__;;
    uint_fast32_t k = n__;

    uint_fast32_t n;
    complex<double> T;
    complex<double> tmp;
    complex<double> phiT = complex<double>(cos(thetaT), -sin(thetaT));

    //

    while(k > 1) {

        n = k;
        k >>= 1;
        phiT = phiT * phiT;
        T = 1.0L;

        for (uint_fast32_t l = 0; l < k; l++) {

            for (uint_fast32_t a = l; a < n__; a += n) {

                uint_fast32_t b = a + k;
                tmp = x__[a] - x__[b];
                x__[a] += x__[b];
                x__[b] = tmp * T;

            }

            T *= phiT;

        }

    }

    //
    // Reverse bits
    //

    complex<double> tmp_swap;

    int i1 = 0;

    for(const auto &i2 : i_bit_rev__) {
        tmp_swap = x__[i1];
        x__[i1] = x__[i2];
        x__[i2] = tmp_swap;
        i1++;
    }

    return true;

}

//--------------------------------------------------//
///
/// @brief   Executing FFT with the specified number of data, sequentialy.
/// @return  None.
///
//--------------------------------------------------//

bool MyFFT::FFT_CT_seq(const vector<complex<double>> &t__, vector<complex<double>> &f__) {

    if(static_cast<uint_fast32_t>(t__.size()) != n_) {
        cerr << "[ERROR][MyFFT] The number of input data is not a multiply of 2." << endl;
        return false;
    }

    //

    f__ = t__;

    if(window_func_name_ != "") { ApplyWindow(f__); }

    if(_FFT_CT_seq(f__)) {

        for(auto &&it : f__) {
            it /= n_; // Correcting the amplitude.
        }

        return true;

    } else {

        return false;

    }

}

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

bool MyFFT::FFT_CT_seq(const vector<double> &t__, vector<complex<double>> &f__) {

    if(static_cast<uint_fast32_t>(t__.size()) != n_) {
        cerr << "[ERROR][MyFFT] The number of input data is not a multiply of 2." << endl;
        return false;
    }

    //

    vector<complex<double>> ().swap(f__);
    f__.reserve(n_);
    for(const auto &it:t__) {
        f__.push_back(complex<double> (it, 0.0));
    }

    if(window_func_name_ != "") { ApplyWindow(f__); }

    if(_FFT_CT_seq(f__)) {

        for(auto &&it : f__) {
            it /= n_; // Correcting the amplitude.
        }

        return true;

    } else {

        return false;

    }

}

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

vector<complex<double>> MyFFT::FFT_CT_seq(const vector<double> &t__) {

    vector<complex<double>> f;

    if(static_cast<uint_fast32_t>(t__.size()) != n_) {
        cerr << "[ERROR][MyFFT] The number of input data is not a multiply of 2." << endl;
        return f;
    }

    //

    if(FFT_CT_seq(t__, f)) {

        for(auto &&it : f) {
            it /= n_; // Correcting the amplitude.
        }

    } else {

        vector<complex<double>> ().swap(f);

    }

    return f;

}

//--------------------------------------------------//
///
/// @brief   Executing inverted FFT
/// @return  None.
///
//--------------------------------------------------//

bool MyFFT::IFFT_CT_seq(const vector<complex<double>> &f__, vector<complex<double>> &t__) {

    if(!Preparing_IFFT(f__, t__)) { return false; }

    //

    if(_FFT_CT_seq(t__)) {

        for(auto &&it : t__) { it = conj(it); }

        return true;

    } else {

        return false;

    }

}

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

bool MyFFT::IFFT_CT_seq(const vector<complex<double>> &f__, vector<double> &t__) {

    vector<complex<double>> t;

    if(!Preparing_IFFT(f__, t)) { return false; }

    //

    if(_FFT_CT_seq(t)) {

        for(auto &&it : t) { it = conj(it); }

    } else {

        return false;

    }

    //

    vector<double> ().swap(t__);
    t__.reserve(n_);

    for(const auto &it : t) {
        t__.push_back(it.real());
    }

    return true;

}

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

vector<complex<double>> MyFFT::IFFT_CT_seq(const vector<complex<double>> &f__) {

    vector<complex<double>> t;

    if(!Preparing_IFFT(f__, t)) { return t; }

    //

    if(_FFT_CT_seq(t)) {

        for(auto &&it : t) { it = conj(it); }

    } else {

        vector<complex<double>> ().swap(t);

    }

    return t;

}

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

bool MyFFT::Preparing_IFFT(const vector<complex<double>> &f__, vector<complex<double>> &t__) {

    if(static_cast<uint_fast32_t>(f__.size()) == n_) {

        t__ = f__;

    }
    else if(static_cast<uint_fast32_t>(f__.size()) == (n_half_ + 1)) {

        vector<complex<double>> ().swap(t__);
        t__.reserve(n_);

        for(const auto &it : f__) {
            t__.push_back(it);
        }

        for(int i=n_half_; i>0; i--) {
            t__.push_back(t__[i]);
        }

    }
    else {

        cerr << "[ERROR][MyFFT] The number of input data is irregular. : " << f__.size() << endl;
        return false;

    }

    //

    for(auto &&it : t__) { it = conj(it); }

    return true;

}

//--------------------------------------------------//
///
/// @brief   Executing FFT
/// @return  None.
///
//--------------------------------------------------//

bool MyFFT::_FFT_CT_seq(vector<complex<double>> &x__) {

    uint_fast32_t n;

    uint_fast32_t N;
    double thetaT;

    {
        unique_lock<std::mutex> wait_lock(mtx_wait_);

        N = n_;
        thetaT = PI_ / N;
    }

    uint_fast32_t k = N;

    complex<double> T;
    complex<double> tmp;
    complex<double> phiT = complex<double>(cos(thetaT), -sin(thetaT));

    //

    while(k > 1) {

        n = k;
        k >>= 1;
        phiT = phiT * phiT;
        T = 1.0L;

        for (uint_fast32_t l = 0; l < k; l++) {

            for (uint_fast32_t a = l; a < N; a += n) {

                uint_fast32_t b = a + k;

                tmp = x__[a] - x__[b];
                x__[a] += x__[b];
                x__[b] = tmp * T;

            }

            T *= phiT;

        }

    }

    //
    // Reverse bits
    //

    complex<double> tmp_swap;

    int i1 = 0;

    {
        unique_lock<std::mutex> wait_lock(mtx_wait_);

        for(const auto &i2 : i_bit_rev_) {
            tmp_swap = x__[i1];
            x__[i1] = x__[i2];
            x__[i2] = tmp_swap;
            i1++;
        }
    }

    return true;

}

//--------------------------------------------------//
///
/// @brief     GSts the window function.
/// @param[in] window_func_name__ : name of window function
///            @arg "hanning"  = Hanning(Hann) window function
///            @arg "hamming"  = Hamming window function
///            @arg "blackman" = Blackman window function
///            @arg "flat-top" = Flat top window function
///            @arg "" or others = rectangular window function
/// @return    None.
///
//--------------------------------------------------//

void MyFFT::SetWindow(const string &window_func_name__, const bool &use_acf__) {

    window_func_name_ = window_func_name__;

    use_acf_ = use_acf__;

    //

    vector<double> ().swap(wf_);
    wf_.resize(n_);

    double coeff = 2 * PI_ / n_;

    double sum = 0;

    int i = 0;

    if(window_func_name_ == "hanning") {

        for(auto &&it : wf_) {
            it = 0.5 - 0.5 * cos(coeff * i);
            sum += it;
            i++;
        }
        acf_ = n_ / sum;

    }
    else if(window_func_name_ == "hamming") {

        for(auto &&it : wf_) {
            it = 0.54 - 0.46 * cos(coeff * i);
            sum += it;
            i++;
        }
        acf_ = n_ / sum;

    }
    else if(window_func_name_ == "blackman") {

        for(auto &&it : wf_) {
            it = 0.42 - 0.5 * cos(coeff * i) + 0.08 * cos(2 * coeff * i);
            sum += it;
            i++;
        }
        acf_ = n_ / sum;

    }
    else if(window_func_name_ == "flat-top") {

        for(auto &&it : wf_) {
            it   = 1 - 1.93 * cos(coeff * i);
            it  +=  1.29  * cos(2 * coeff * i);
            it  += -0.388 * cos(3 * coeff * i);
            it  +=  0.032 * cos(4 * coeff * i);
            sum += it;
            i++;
        }
        acf_ = n_ / sum;

    }
    else {

        for(auto &&it : wf_) { it = 1.0; i++; }
        acf_ = 1.0;

    }

    return;

}

//--------------------------------------------------//
///
/// @brief      Gets the window function.
/// @param[out] wf__ : the window function array
/// @return     None.
///
//--------------------------------------------------//

void MyFFT::GetWindow(std::vector<double> &wf__) {

    wf__ = wf_;

    return;

}

//--------------------------------------------------//
///
/// @brief      Applys the window function to time data.
/// @param[out] w__ : the window function array
/// @return     None.
///
//--------------------------------------------------//

void MyFFT::ApplyWindow(vector<complex<double>> &t__) {

    uint_fast32_t n = t__.size();

    if(n >= n_) {

        if(use_acf_) {
            for(uint_fast32_t i=0;i<n_;i++) { t__[i] = acf_ * wf_[i] * t__[i]; }
        } else {
            for(uint_fast32_t i=0;i<n_;i++) { t__[i] = wf_[i] * t__[i]; }
        }

        if(t__.size() > n_) {
            t__.resize(n_);
        }

    }

    return;

}

/*
//--------------------------------------------------//
///
/// @brief   Flat top window function
/// @return  None.
///
//--------------------------------------------------//

void MyFFT::GetWF_FlatTop(std::vector<double> &w__) {

    vector<double> ().swap(w__);
    w__.resize(n_);

    double coeff = 2 * PI_ / n_;

    int i = 0;

    for(auto &&it : w__) {
        it  = 1 - 1.93 * cos(coeff * i)
        it +=  1.29  * cos(2 * coeff * i)
        it += -0.388 * cos(3 * coeff * i)
        it += +0.032 * cos(4 * coeff * i)
        i++;
    }

    return;

}
*/

//--------------------------------------------------//
///
/// @brief   Converting data.
/// @return  None.
///
//--------------------------------------------------//

bool MyFFT::C2RI(const vector<complex<double>> &c__, vector<double> &c_real__, vector<double> &c_imag__) {

    int n = c__.size();

    if(n == 0 || (n%2) != 0) {
        return false;
    }

    vector<double> ().swap(c_real__);
    vector<double> ().swap(c_imag__);

    c_real__.reserve(n);
    c_imag__.reserve(n);

    for(const auto &it : c__) {
        c_real__.push_back(it.real());
        c_imag__.push_back(it.imag());
    }

    return true;

}

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

bool MyFFT::RI2C(const vector<double> &c_real__, const vector<double> &c_imag__, vector<complex<double>> &c__) {

    int n_real = c_real__.size();
    int n_imag = c_imag__.size();

    if(n_real == 0 || n_real != n_imag) {
        return false;
    }

    vector<complex<double>> ().swap(c__);

    c__.reserve(n_real);

    for(int i=0;i<n_real;i++) {
        c__.push_back(complex<double> (c_real__[i], c_imag__[i]));
    }

    return true;

}

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

bool MyFFT::C2AP(const vector<complex<double>> &c__, vector<double> &c_amp__, vector<double> &c_phas__) {

    int n = c__.size();
    int n_half = n / 2;

    if(n == 0 || (n%2) != 0) {
        return false;
    }

    vector<double> ().swap(c_amp__);
    vector<double> ().swap(c_phas__);

    c_amp__.reserve(n_half + 1);
    c_phas__.reserve(n_half + 1);

    for(int i=0;i<=n_half;i++) {
        c_amp__.push_back(abs(c__[i]));
        c_phas__.push_back(arg(c__[i]));
    }

    for(int i=0;i<n_half;i++) {
        c_amp__[n_half-i] += abs(c__[n_half+i]);
    }

    return true;

}

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

bool MyFFT::AP2C(const vector<double> &c_amp__, const vector<double> &c_phas__, vector<complex<double>> &c__) {

    int n_amp  = c_amp__.size();
    int n_phas = c_phas__.size();

    if(n_amp == 0 || n_amp != n_phas) {
        return false;
    }

    int n = (n_amp -1) * 2;

    vector<complex<double>> ().swap(c__);

    c__.reserve(n);

    c__.push_back(complex<double> (c_amp__[0]*cos(c_phas__[0]), c_amp__[0]*sin(c_phas__[0])));

    for(int i=1;i<n_amp;i++) {
        c__.push_back(complex<double> (0.5 * c_amp__[i]*cos(c_phas__[i]), 0.5 * c_amp__[i]*sin(c_phas__[i])));
    }

    //cout << "00" << endl;

    for(int i=n_amp-2;i>0;i--) {
        c__.push_back(conj(c__[i]));
    }

    //cout << "01 : " << c__.size() << endl;

    return true;

}

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

};


// Header
#include <iostream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <ctime>
#include <unordered_map>
#include <string>
#include <regex>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string/join.hpp>
#include <boost/bimap/bimap.hpp>
#include <boost/regex.hpp>
#include "json.hpp"
using json = nlohmann::json;

// Using bimap
typedef boost::bimaps::bimap<std::string,std::string> bimap;
typedef bimap::value_type bimap_val;

//// REVISED ////////

// Utility Function
//std::string split
std::vector<std::string> &Split(const std::string &s, char delim, std::vector<std::string> &elems) {
    std::stringstream ss(s);
    std::string item;
    while (std::getline(ss, item, delim)) {
        elems.push_back(item);}
    return elems;}

std::vector<std::string> Split(const std::string &s, char delim) {
    std::vector<std::string> elems;
    Split(s, delim, elems);
    return elems;}

std::vector<std::string> SplitDqcsv(std::string str){
    std::vector<std::string> v1;
    const char *mystart=str.c_str();    // prepare to parse the line - start is position of begin of field
    bool instring{false};
    for (const char* p=mystart; *p; p++) {  // iterate through the string
        if (*p=='"')                        // toggle flag if we're btw double quote
            instring = !instring;
        else if (*p==',' && !instring) {    // if comma OUTSIDE double quote
            v1.push_back(std::string(mystart,p-mystart));  // keep the field
            mystart=p+1;                    // and start parsing next one
        }
    }
    v1.push_back(std::string(mystart));   // last field delimited by end of line instead of comma
    return(v1);
}

std::vector<int> Split2IntVector(std::string line){
    std::vector<std::string> elements0 = Split(line,',');
    std::vector<int> elements;
    for(int i=0;i<elements0.size();++i){
        elements.push_back(boost::lexical_cast<int>(elements0[i]));
    }
    return(elements);
}

/*
std::vector<int64_t> Split2LongVector(std::string line){
    std::vector<std::string> elements0 = Split(line,',');
    std::vector<int64_t> elements;
    for(int i=0;i<elements0.size();++i){
        elements.push_back(boost::lexical_cast<int64_t>(elements0[i]));
    }
    return(elements);
}
/****/

std::vector<long long> Split2LongVector(std::string line){
    std::vector<std::string> elements0 = Split(line,',');
    std::vector<long long> elements;
    for(int i=0;i<elements0.size();++i){
        elements.push_back(boost::lexical_cast<long long>(elements0[i]));
    }
    return(elements);
}
/****/

std::string Trimstring(const std::string s) {//stringの左右両端の、タブ、改行を消す
    if(s.length() == 0)
        return s;
    int b = s.find_first_not_of(" \t\r\n,");
    int e = s.find_last_not_of(" \t\r\n,");
    if(b == -1)// 左右両端に、スペース、タブ、改行がない。
        return "";
    return std::string(s, b, e - b + 1);}

std::string Trimquote(const std::string s) {//trimquote
    if(s.length() == 0)
        return s;
    int b = s.find_first_not_of("\"");
    int e = s.find_last_not_of("\"");
    if(b == -1)
        return "";
    return std::string(s, b, e - b + 1);}

float Sigmoid(float x){
    return( float(1.0) / (float(1.0) + exp(-x)));
    //return(0.5 * (x / (1 + abs(x)) + 0.5);
}

float DerSigmoid(float x){
    return(Sigmoid(x) * (float(1.0) - Sigmoid(x)));
}

float TanH(float x){
    float numer = exp(x) - exp(-x);
    float denom = exp(x) + exp(-x);
    return(numer/denom);
}

float DerTanH(float x){
    float out = float(1.0) - (TanH(x)*TanH(x));
    return(out);
}

float SigmoidFast(float x){
    //return(1 / ( 1+exp(-x)));
    return(0.5 * (x / (1 + abs(x)) + 0.5));
}

float Relu(float x){
    return(log(1 + exp(x)));
}

double ContorPair(double x1,double x2){
    x1 += 1.0;
    x2 += 1.0;
    return((x1+x2)*(x1+x2+1.0)/2.0 + x2);
}

double ContorTuple(double x1,double x2,double x3){
    x1 += 1.0;
    x2 += 1.0;
    x3 += 1.0;
    return(ContorPair(ContorPair(x1,x2),x3));
}

// Draw Dirichlet Sample
void RandomDirichlet(std::mt19937_64 &mt,std::vector<float> &x, const float alpha) {
    std::gamma_distribution<float> gamma(alpha, 1.0);
    float sum_y = 0;
    for(int i = 0; i < x.size(); i++){
        float y = gamma(mt);
            sum_y += y;
            x[i] = y;}
    std::for_each(x.begin(), x.end(), [sum_y](float &v) { v /= sum_y; });
}

int EditDistance(const std::string &s1, const std::string &s2){
  	// To change the type this function manipulates and returns, change
  	// the return type and the types of the two variables below.
  	int s1len = s1.size();
  	int s2len = s2.size();
  	auto column_start = (decltype(s1len))1;
  	auto column = new decltype(s1len)[s1len + 1];
  	std::iota(column + column_start - 1, column + s1len + 1, column_start - 1);
  	for (auto x = column_start; x <= s2len; x++) {
  		column[0] = x;
  		auto last_diagonal = x - column_start;
  		for (auto y = column_start; y <= s1len; y++) {
  			auto old_diagonal = column[y];
  			auto possibilities = {
  				column[y] + 1,
  				column[y - 1] + 1,
  				last_diagonal + (s1[y - 1] == s2[x - 1]? 0 : 1)
  			};
  			column[y] = std::min(possibilities);
  			last_diagonal = old_diagonal;
  		}
  	}
  	auto result = column[s1len];
  	delete[] column;
  	return result;
}

// trimquote
std::string TrimSemi(const std::string s) {//trimquote
  if(s.length() == 0)
    return s;
  int b = s.find_first_not_of(";");
  int e = s.find_last_not_of(";");
  if(b == -1)
    return "";
  return std::string(s, b, e - b + 1);
}

// trimquote
std::string TrimComma(const std::string s) {//trimquote
  if(s.length() == 0)
    return s;
  int b = s.find_first_not_of(",");
  int e = s.find_last_not_of(",");
  if(b == -1)
    return "";
  return std::string(s, b, e - b + 1);
}

// std::string replace
void StrReplace(std::string &str,const std::string &from,const std::string &to){
  std::string::size_type pos = 0;
  while(pos = str.find(from,pos),pos!=std::string::npos){
      str.replace(pos,from.length(),to);
      pos += to.length();}
}

bool BothAreSpaces(char lhs, char rhs) {
  return (lhs == rhs) && (lhs == ' ');
}

void RemoveDoubleSpaces(std::string &story){
    std::string::iterator new_end = std::unique(story.begin(),story.end(), BothAreSpaces);
    story.erase(new_end,story.end());
}

//// NOT YET REVISED ////////

// sort indexes
template <typename T>
std::vector<size_t> sort_indexes(const std::vector<T> &v) {
/*
 * Descr: place any std::vector and it would return an indices
 * Input: std::vector
 * Output: Indices vector
 /***/
    // initialize original index locations
    std::vector<size_t> idx(v.size());
    iota(idx.begin(), idx.end(), 0);
    // sort indexes based on comparing values in v
    sort(idx.begin(), idx.end(),
       [&v](size_t i1, size_t i2) {return v[i1] < v[i2];});
    return idx;
}

// Remove control sequence
std::string remove_ctrl(std::string s){
	std::string result;
	for(int i=0;i<s.length();++i){
		if(s[i] >=0x20){
			result = result + s[i];}}
	return result;}

// join std::string
std::string join_str(std::vector<std::string> v1,const char* delim){
/* Descr: C++ substitute of python's str.join method
 * Input: vector of std::string
 * Output: joined string
/***/
    std::ostringstream  os;
    std::copy(v1.begin(), v1.end(), std::ostream_iterator<std::string>(os,delim));
    std::string outstr = Trimstring(os.str());
    return(outstr);
}

// convert date
std::string conv_date (std::string str){
    std::string y = "";
    std::string m = "";
    std::string d = "";
    for(int i=0;i<4;++i) y += str[i];
    for(int i=4;i<6;++i) m += str[i];
    for(int i=6;i<8;++i) d += str[i];
    std::string ymd = y + "-" + m + "-" + d;
    return ymd;
}

// lastquart
std::string lastquart(std::string str){
    std::vector<std::string> v1 = Split(str,'-');
    int y = boost::lexical_cast<int>(v1[0]);
    int m = boost::lexical_cast<int>(v1[1]);
    m = m - 3;
    if(m < 1){
        m = 12 + m;
        y = y - 1;}
    std::string mstr = "";
    if(m < 10){
        mstr = "0" + boost::lexical_cast<std::string>(m);
    }else{
        mstr = boost::lexical_cast<std::string>(m);}
    std::string dstr="";
    if(m==1 || m==3 || m==5 || m==7 || m==8 || m==10 || m==12) dstr = "31";
    if(m==4 || m==6 || m==9 || m==11) dstr="30";
    if(m==2 && y % 4 !=0) dstr="28";
    if(m==2 && y % 4 ==0) dstr="29";
    std::string outstr = boost::lexical_cast<std::string>(y) + "-" + mstr + "-" + dstr;
    return(outstr);
}

//returns current time in seconds
double current_time () {
    timeval tv;
    gettimeofday(&tv, NULL);
    double rtn_value = (double) tv.tv_usec;
    rtn_value /= 1e6;
    rtn_value += (double) tv.tv_sec;
    return rtn_value;}

// No Double Space
std::string no_d_space(std::string line){
    int loc = 0;
    while((loc = line.find("  ",loc)) != std::string::npos) //Two spaces here
    {
       line.replace(loc,2," "); //Single space in quotes
    }
    return line;
}

std::string no_quote (std::string line){//FAST
    int loc = 0;
    while((loc = line.find("\"",loc)) != std::string::npos) line.replace(loc,1,"");
    return line;
}

// Probably Used Only Here
std::string no_quote_plus (std::string str){
    std::regex re("\\\\|\"|,|;");
    str = std::regex_replace(str,re,"");
    return str;
}

// Probably Used Only Here
std::string no_quote_plus2 (std::string str){
    std::regex re("\"|\'|\\(|\\)|\\[|\\]");
    str = std::regex_replace(str,re,"");
    return str;
}

// To cope with text problems creates by windows
std::string remove_r(std::string str){
    str.erase( std::remove(str.begin(), str.end(), '\r'), str.end() );
    return(str);
}

// Probably Used Only Here
std::string no_http (std::string str){
    std::regex re("http://|https://");
    str = std::regex_replace(str,re,"");
    return str;
}


// Copyright (c) 2016 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE FullyConnectedTest
#include <poplibs_support/TestDevice.hpp>

#include <Winograd.hpp>
#include <boost/random.hpp>
#include <boost/test/unit_test.hpp>
#include <poplar/Engine.hpp>
#include <poplin/Convolution.hpp>
#include <poplin/codelets.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>
#include <random>
#include <string>

using namespace poplar;
using namespace poplar::program;
using namespace poputil;
using namespace poplibs_support;

const OptionFlags engineOptions;

namespace utf = boost::unit_test;
namespace fpc = boost::test_tools::fpc;

static unsigned filterLengthPre(unsigned a, unsigned kernel) {
  unsigned hLen = (kernel - 1) / 2;
  unsigned filtLen = hLen;

  if (a < hLen)
    filtLen -= hLen - a;

  return filtLen;
}

static unsigned filterLengthPost(unsigned a, unsigned kernel, unsigned aLim) {
  unsigned hLen = (kernel - 1) / 2;
  unsigned filtLen = hLen;

  if (a + hLen >= aLim)
    filtLen -= a + hLen - aLim + 1;

  return filtLen;
}

/* Reference convolution layer implementation using naive convolution method */
static void computeReference(Tensor in, Tensor weights, Tensor activations,
                             const float *inpBuffer, const float *weightBuffer,
                             float *outBuffer, unsigned paddingY,
                             unsigned paddingX) {

  unsigned numInpChanGroups = in.dim(1);
  unsigned numInpChansInGroup = in.dim(4);
  unsigned featureX = in.dim(3);
  unsigned featureY = in.dim(2);
  unsigned kernelX = weights.dim(3);
  unsigned kernelY = weights.dim(2);

  unsigned numInpChansInGroupWeight = weights.dim(5);
  unsigned numOutChansInGroupWeight = weights.dim(4);
  unsigned numInpChanGroupsWeight = weights.dim(1);

  unsigned numOutChansInGroup = activations.dim(4);
  unsigned numOutChanGroups = activations.dim(1);

  for (unsigned ozg = 0; ozg < numOutChanGroups; ++ozg) {
    for (unsigned ozc = 0; ozc < numOutChansInGroup; ++ozc) {

      const auto wozc =
          (ozg * numOutChansInGroup + ozc) % numOutChansInGroupWeight;
      const auto wozg =
          (ozg * numOutChansInGroup + ozc) / numOutChansInGroupWeight;
      const auto outIdx =
          ozg * (numOutChansInGroup * featureX * featureY) + ozc;

      for (unsigned y = 0; y < featureY; ++y) {
        for (unsigned x = 0; x < featureX; ++x) {

          /* pre and post filter segments from centre tap: needed to include
           * padding
           */
          const unsigned filtLenLX = filterLengthPre(x, kernelX);
          const unsigned filtLenLY = filterLengthPre(y, kernelY);
          const unsigned filtLenRX = filterLengthPost(x, kernelX, featureX);
          const unsigned filtLenRY = filterLengthPost(y, kernelY, featureY);

          float outRes = 0;

          for (unsigned izg = 0; izg < numInpChanGroups; ++izg) {
            for (unsigned izc = 0; izc < numInpChansInGroup; ++izc) {
              const auto wizc =
                  (izg * numInpChansInGroup + izc) % numInpChansInGroupWeight;
              const auto wizg =
                  (izg * numInpChansInGroup + izc) / numInpChansInGroupWeight;

              const auto wIdx =
                  wozg * (numInpChanGroupsWeight * kernelX * kernelY *
                          numOutChansInGroupWeight * numInpChansInGroupWeight) +
                  wizg * (kernelX * kernelY * numOutChansInGroupWeight *
                          numInpChansInGroupWeight) +
                  wozc * (numInpChansInGroupWeight) + wizc;

              const auto inIdx =
                  izg * (featureX * featureY * numInpChansInGroup) + izc;
              float acc = 0;

              for (unsigned ix = (kernelX - 1) / 2 - filtLenLX;
                   ix <= (kernelX - 1) / 2 + filtLenRX; ++ix) {
                for (unsigned iy = (kernelY - 1) / 2 - filtLenLY;
                     iy <= (kernelY - 1) / 2 + filtLenRY; ++iy) {
                  const auto finIdx =
                      inIdx +
                      (x + ix - (kernelX - 1) / 2) * numInpChansInGroup +
                      (y + iy - (kernelY - 1) / 2) * featureX *
                          numInpChansInGroup;

                  const auto fwIdx =
                      wIdx +
                      ix * numOutChansInGroupWeight * numInpChansInGroupWeight +
                      iy * kernelX * numOutChansInGroupWeight *
                          numInpChansInGroupWeight;

                  acc += inpBuffer[finIdx] * weightBuffer[fwIdx];
                }
              }
              outRes += acc;
            }
          }

          const auto foIdx = outIdx + x * numOutChansInGroup +
                             y * numOutChansInGroup * featureX;
          outBuffer[foIdx] = outRes;
        }
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(WinogradConvolution,
                     *utf::tolerance<float>(fpc::percent_tolerance<float>(1))) {
  auto device = createTestDevice(TEST_TARGET, 1, 24);
  Graph graph(device.getTarget());
  popops::addCodelets(graph);
  poplin::addCodelets(graph);

  /* Test configuration */

  const auto dType = FLOAT;
  const unsigned numOutPartialChanGroups = 128 / 8;
  const unsigned numOutPartialChansInGroup = 8;
  const unsigned numInpChanGroups = 32 / 16;
  const unsigned featureX = 13;
  const unsigned featureY = 13;
  const unsigned numInpChansInGroup = 16;
  const unsigned kernelSizeX = 3;
  const unsigned kernelSizeY = 3;
  const unsigned numOutChanGroups = 128 / 8;
  const unsigned numOutChansInGroup = 8;
  const unsigned patchSizeX = 4;
  const unsigned patchSizeY = 4;
  const unsigned paddingY = 1;
  const unsigned paddingX = 1;
  const float mean = 0.2;
  const float stdDev = 1.0;

  auto in = graph.addVariable(
      dType, {1, numInpChanGroups, featureY, featureX, numInpChansInGroup},
      "in");
  auto weights = graph.addVariable(
      dType,
      {numOutPartialChanGroups, numInpChanGroups, kernelSizeY, kernelSizeX,
       numOutPartialChansInGroup, numInpChansInGroup},
      "weights");
  auto activations = graph.addVariable(
      dType, {1, numOutChanGroups, featureY, featureX, numOutChansInGroup},
      "activations");
  Tensor residual;

  mapTensorLinearly(graph, in);
  mapTensorLinearly(graph, activations);

  const std::size_t inSize =
      numInpChanGroups * featureY * featureX * numInpChansInGroup;
  const std::size_t wSize = numOutPartialChanGroups * numInpChanGroups *
                            kernelSizeY * kernelSizeX *
                            numOutPartialChansInGroup * numInpChansInGroup;
  const std::size_t outSize =
      numOutChanGroups * featureX * featureY * numOutChansInGroup;

  std::vector<float> inBuffer(inSize);
  std::vector<float> outBuffer(outSize);
  std::vector<float> outBufferRef(outSize);
  std::vector<float> weightsBuffer(wSize);
  std::vector<float> debugBuffer(inSize * 16);
  std::fill(debugBuffer.begin(), debugBuffer.end(), 0);

  std::mt19937 randomEngine;
  boost::random::normal_distribution<> dist(mean, stdDev);

  for (unsigned i = 0; i < inSize; ++i) {
    inBuffer[i] = dist(randomEngine);
  }

  for (unsigned i = 0; i < wSize; ++i) {
    weightsBuffer[i] = dist(randomEngine);
  }

  const auto &target = graph.getTarget();
  poplin::WinogradParams params({paddingY, paddingX}, {paddingY, paddingX},
                                {1, 1});
  poplin::WinogradOptions options(target.getNumIPUs(), target.getTilesPerIPU());
  auto wgdConv =
      poplin::winogradConvolution(graph, params, options, in, weights,
                                  activations, patchSizeX, patchSizeY, FLOAT);

  graph.createHostWrite("in", in);
  graph.createHostWrite("weights", weights);
  graph.createHostRead("out", activations);

  Engine eng(graph, wgdConv, engineOptions);
  device.bind([&](const Device &d) {
    eng.load(d);
    eng.writeTensor("in", inBuffer.data(), inBuffer.data() + inBuffer.size());
    eng.writeTensor("weights", weightsBuffer.data(),
                    weightsBuffer.data() + weightsBuffer.size());
    eng.run();
    eng.readTensor("out", outBuffer.data(),
                   outBuffer.data() + outBuffer.size());
  });

  computeReference(in, weights, activations, &inBuffer[0], &weightsBuffer[0],
                   &outBufferRef[0], paddingX, paddingY);

  for (unsigned i = 0; i < outSize; ++i) {
    // float relDiff = fabs((outBuffer[i]-outBufferRef[i])/outBufferRef[i]*100);
    // std::cout << outBuffer[i] << "  " << outBufferRef[i] << "  ";
    // std::cout << relDiff << std::endl;
    // if (relDiff > 0.01)
    // std::cout << " error ... " << i << std::endl;
    BOOST_TEST(outBuffer[i] == outBufferRef[i]);
  }
}


#define BOOST_TEST_MODULE lue core time tick_period
#include <boost/test/unit_test.hpp>
#include "lue/core/time.hpp"

BOOST_AUTO_TEST_CASE(seconds)
{
    namespace ldm = lue::data_model;

    // Every 5 * (1 seconds) a tick
    std::size_t const nr_seconds = 5;

    ldm::time::TickPeriod<ldm::time::Second> tick_period(nr_seconds);

    BOOST_CHECK_EQUAL(tick_period.nr_units(), nr_seconds);
}


#pragma once

#include <crab/common/bignums.hpp>
#include <crab/domains/intervals.hpp>
#include <crab/common/debug.hpp>
#include <crab/cfg/cfg.hpp>
#include <crab/iterators/wto.hpp>

#include <boost/unordered_map.hpp>
#include <boost/range/iterator_range.hpp>

#include <algorithm>
#include <climits>

namespace crab {

   namespace iterators {

     /**
	 Class that represents a thresholds used by the widening operator
     **/
     template<typename Number>
     class thresholds {

      private:

       /// XXX: internal representation of a threshold
       typedef ikos::bound <Number> bound_t;

       std::vector<bound_t> m_thresholds;
       unsigned int m_size;

       template<class B1, class B2>
       static B2 convert_bounds_impl (B1 b1) {
	 B2 b2 (0); // some initial value it doesn't matter which one
	 ikos::bounds_impl::convert_bounds (b1, b2);
	 return b2;
       }

      public:

       thresholds (int size = UINT_MAX) : m_size (size) {
         m_thresholds.push_back (bound_t::minus_infinity ());
         m_thresholds.push_back (0);
	 // useful thresholds for wrapped domains
	 #if 1
	 m_thresholds.push_back (bound_t("-2147483648"));
	 m_thresholds.push_back (bound_t("-32768"));
	 m_thresholds.push_back (bound_t("-128"));
	 m_thresholds.push_back (bound_t("127"));
	 m_thresholds.push_back (bound_t("255"));
	 m_thresholds.push_back (bound_t("32767"));
	 m_thresholds.push_back (bound_t("65535"));
	 m_thresholds.push_back (bound_t("2147483647"));
	 #endif
         m_thresholds.push_back (bound_t::plus_infinity ());
       }

       unsigned size (void) const { return m_thresholds.size(); }

       template<typename N>
       void add (ikos::bound<N> v1) {
         if (m_thresholds.size () < m_size) {
       	   bound_t v = convert_bounds_impl<ikos::bound<N>, bound_t> (v1);
           if (std::find
	       (m_thresholds.begin (), m_thresholds.end (), v) == m_thresholds.end ()) {
             auto ub = std::upper_bound (m_thresholds.begin (), m_thresholds.end (), v);
             m_thresholds.insert (ub, v);
           }
         }
       }

       template<typename N>
       ikos::bound<N> get_next (ikos::bound<N> v1) const {
	 if (v1.is_plus_infinity()) return v1;
	 bound_t v = convert_bounds_impl<ikos::bound<N>, bound_t> (v1);
	 bound_t t = m_thresholds [m_thresholds.size () - 1];
         auto ub = std::upper_bound (m_thresholds.begin (), m_thresholds.end (), v);
         if (ub != m_thresholds.end ())
	   t = *ub;
	 return convert_bounds_impl<bound_t, ikos::bound<N> > (t);
       }

       template<typename N>
       ikos::bound<N> get_prev (ikos::bound<N> v1) const {
	 if (v1.is_minus_infinity()) return v1;
	 bound_t v = convert_bounds_impl<ikos::bound<N>, bound_t> (v1);
         auto lb = std::lower_bound (m_thresholds.begin (), m_thresholds.end (), v);
         if (lb != m_thresholds.end ()) {
           --lb;
           if (lb != m_thresholds.end ()) {
	     return convert_bounds_impl<bound_t, ikos::bound<N> > (*lb);
	   }
         }
	 return convert_bounds_impl<bound_t, ikos::bound<N> > (m_thresholds [0]);
       }

       void write (crab_os &o) const {
         o << "{";
         for (typename std::vector<bound_t>::const_iterator it = m_thresholds.begin (),
                  et= m_thresholds.end (); it != et; ) {
           bound_t b (*it);
           b.write (o);
           ++it;
           if (it != m_thresholds.end ())
             o << ",";
         }
         o << "}";
       }

     };

     template<typename Number>
     crab_os& operator<< (crab_os& o, const thresholds<Number>& t) {
       t.write (o);
       return o;
     }

     // explicit instantiation
     typedef thresholds<ikos::q_number> thresholds_t;

     /**
	Collect thresholds per wto cycle  (i.e. loop)
     **/
     template< typename NodeName, typename CFG>
     class wto_thresholds: public ikos::wto_component_visitor< NodeName, CFG > {

     public:

       typedef ikos::wto_vertex< NodeName, CFG > wto_vertex_t;
       typedef ikos::wto_cycle< NodeName, CFG > wto_cycle_t;
       typedef crab::iterators::thresholds_t thresholds_t;
       typedef boost::unordered_map<NodeName, thresholds_t> thresholds_map_t;

     private:

       // the cfg
       CFG m_cfg;
       // maximum number of thresholds
       size_t m_max_size;
       // keep a set of thresholds per wto head
       thresholds_map_t m_head_to_thresholds;
       // the top of the stack is the current wto head
       std::vector<NodeName> m_stack;

       typedef typename CFG::basic_block_t basic_block_t;
       typedef typename CFG::number_t number_t;
       typedef typename CFG::varname_t varname_t;
       typedef ikos::bound<number_t> bound_t;

       typedef crab::cfg::assume_stmt<number_t,varname_t> assume_t;
       typedef crab::cfg::select_stmt<number_t,varname_t> select_t;
       typedef crab::cfg::assignment<number_t,varname_t>  assign_t;

       void get_thresholds(const basic_block_t &bb, thresholds_t &thresholds) {
	 for (auto const&i : boost::make_iterator_range (bb.begin (), bb.end ())) {
	   bound_t t = bound_t::plus_infinity ();
	   if (i.is_assume ()) {
	     auto a = static_cast<const assume_t*> (&i);
	     t = bound_t(-(a->constraint ().expression ().constant ()));
	   }
	   else if (i.is_select ()) {
	     auto s = static_cast<const select_t*> (&i);
	     t = bound_t(-(s->cond ().expression ().constant ()));
	   }
	   else if (i.is_assign ()) {
	     auto a = static_cast<const assign_t*> (&i);
	     if (a->rhs ().is_constant ())
	       t = bound_t(-(a->rhs().constant ()));
	   }

	   if (t != bound_t::plus_infinity ()) {
	     // XXX: for code pattern like this "while(x<t) {x+=k;}"
	     // note that the condition (x<t) is translated to
	     // (x<=t-1) so an useful threshold would be t+1+k.
	     // Since we don't keep track of how x is incremented or
	     // decremented we choose arbitrarily k=1.
	     thresholds.add(t+2);
	   }
	 }
       }

     public:

       wto_thresholds(CFG cfg, size_t max_size)
	 : m_cfg(cfg), m_max_size (max_size) { }

       void visit(wto_vertex_t& vertex) {
	 if (m_stack.empty()) return;

	 NodeName head = m_stack.back();
	 auto it = m_head_to_thresholds.find(head);
	 if (it != m_head_to_thresholds.end()) {
	   thresholds_t & thresholds = it->second;
	   typename CFG::basic_block_t & bb = m_cfg.get_node(vertex.node());
	   get_thresholds(bb, thresholds);
	 } else {
	   CRAB_ERROR("No head found while gathering thresholds");
	 }
       }

       void visit(wto_cycle_t& cycle) {
	 thresholds_t thresholds(m_max_size);
	 typename CFG::basic_block_t & bb = m_cfg.get_node(cycle.head());
	 get_thresholds(bb, thresholds);

	 #if 1
	 // XXX: if we want to consider constants from loop
	 // initializations
	 for (auto pre : boost::make_iterator_range(bb.prev_blocks())) {
	   if (pre != cycle.head()) {
	     typename CFG::basic_block_t & pred_bb = m_cfg.get_node(pre);
	     get_thresholds(pred_bb, thresholds);
	   }
	 }
	 #endif

	 m_head_to_thresholds.insert(std::make_pair(cycle.head(), thresholds));
	 m_stack.push_back(cycle.head());
	 for (typename wto_cycle_t::iterator it = cycle.begin(); it != cycle.end(); ++it) {
	   it->accept(this);
	 }
	 m_stack.pop_back();
       }

       const thresholds_map_t& get_thresholds_map() const {
	 return m_head_to_thresholds;
       }

       void write (crab::crab_os& o) const {
	 for (auto &kv: m_head_to_thresholds) {
	   o << crab::cfg_impl::get_label_str(kv.first) << "=" << kv.second << "\n";
	 }
       }

     }; // class wto_thresholds

     template<typename NodeName, typename CFG>
     crab::crab_os& operator<<(crab::crab_os& o, const wto_thresholds<NodeName, CFG>& t) {
       t.write (o);
       return o;
     }

   } // end namespace iterators
} // end namespace crab


/*
    Copyright 2016 Emanuele Vespa, Imperial College London
    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are met:

    1. Redistributions of source code must retain the above copyright notice, this
    list of conditions and the following disclaimer.

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

    3. Neither the name of the copyright holder nor the names of its contributors
    may be used to endorse or promote products derived from this software without
    specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
    ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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.

*/
#ifndef OCTANT_OPS_HPP
#define OCTANT_OPS_HPP
#include "utils/morton_utils.hpp"
#include "utils/math_utils.h"
#include "octree_defines.h"
#include <iostream>
#include <bitset>
#include <Eigen/Dense>

namespace se {
  namespace keyops {

    inline se::key_t code(const se::key_t octant_key) {
      return octant_key & ~SCALE_MASK;
    }

    inline int depth(const se::key_t octant_key) {
      return octant_key & SCALE_MASK;
}

    inline se::key_t encode(const int x, const int y, const int z,
        const int depth, const int voxel_depth) {
      const int offset = MAX_BITS - voxel_depth + depth - 1;
      return (compute_morton(x, y, z) & MASK[offset] & ~SCALE_MASK) | depth;
    }

    inline Eigen::Vector3i decode(const se::key_t octant_key) {
      return unpack_morton(octant_key & ~SCALE_MASK);
    }
  }

/*
 * Algorithm 5 of p4est paper: https://epubs.siam.org/doi/abs/10.1137/100791634
 */
inline Eigen::Vector3i face_neighbour(const se::key_t octant_key,
    const unsigned int face, const unsigned int l,
    const unsigned int voxel_depth) {
  Eigen::Vector3i octant_coord = se::keyops::decode(octant_key);
  const unsigned int octant_size = 1 << (voxel_depth - l);
  octant_coord.x() = octant_coord.x() + ((face == 0) ? -octant_size : (face == 1) ? octant_size : 0);
  octant_coord.y() = octant_coord.y() + ((face == 2) ? -octant_size : (face == 3) ? octant_size : 0);
  octant_coord.z() = octant_coord.z() + ((face == 4) ? -octant_size : (face == 5) ? octant_size : 0);
  return {octant_coord.x(), octant_coord.y(), octant_coord.z()};
}

/*
 * \brief Return true if node is a descendant of ancestor
 * \param[in] octant_key
 * \param[in] ancestor_key
 * \param[in] voxel_depth  Max/voxel depth of the tree the keys refer to.
 * \return True if octant_key is a descendant of the ancestor_key or if
 *         they are equal.
 */
inline bool descendant(const se::key_t octant_key,
                       const se::key_t ancestor_key,
                       const int       voxel_depth) {
  const int depth = se::keyops::depth(octant_key);
  const int ancestor_depth = se::keyops::depth(ancestor_key);
  // MAX_BITS is used for the maximum voxel depth given the size of the se::key_t
  const int idx = MAX_BITS - voxel_depth + ancestor_depth - 1;
  const se::key_t ancestor_key_code = se::keyops::code(ancestor_key);
  const se::key_t octant_key_code = se::keyops::code(octant_key) & MASK[idx];
  return (depth >= ancestor_depth) && (ancestor_key_code ^ octant_key_code) == 0;
}

/*
 * \brief Computes the parent's morton code of a given octant
 * \param octant_key
 * \param voxel_depth max depth of the tree on which the octant lives
 */
inline se::key_t parent(const se::key_t& octant_key, const int voxel_depth) {
  const int depth = se::keyops::depth(octant_key) - 1;
  const int idx = MAX_BITS - voxel_depth + depth - 1;
  return (octant_key & MASK[idx]) | depth;
}

/*
 * \brief Computes the octants's id in its local brotherhood
 * \param octant_key
 * \param depth of octant
 * \param voxel_depth max depth of the tree on which the octant lives
 */
inline int child_idx(se::key_t octant_key, const int depth,
    const int voxel_depth) {
  int shift = voxel_depth - depth;
  octant_key = se::keyops::code(octant_key) >> shift*3;
  int idx = (octant_key & 0x01) | (octant_key & 0x02) | (octant_key & 0x04);
  return idx;
}

/*
 * \brief Computes the octants's id in its local brotherhood
 * \param octant_key
 * \param depth of octant
 * \param voxel_depth max depth of the tree on which the octant lives
 */
inline int child_idx(se::key_t octant_key, const int voxel_depth) {
  int shift = voxel_depth - se::keyops::depth(octant_key);
  octant_key = se::keyops::code(octant_key) >> shift*3;
  int idx = (octant_key & 0x01) | (octant_key & 0x02) | (octant_key & 0x04);
  return idx;
}

/*
 * \brief Computes the octants's corner which is not shared with its siblings
 * \param octant_key
 * \param depth of octant
 * \param voxel_depth max depth of the tree on which the octant lives
 */
inline Eigen::Vector3i far_corner(const se::key_t octant_key, const int depth,
    const int voxel_depth) {
  const unsigned int octant_size = 1 << (voxel_depth - depth);
  const int child_idx = se::child_idx(octant_key, depth, voxel_depth);
  const Eigen::Vector3i octant_coord = se::keyops::decode(octant_key);
  return Eigen::Vector3i(octant_coord.x() + ( child_idx & 1)       * octant_size,
                         octant_coord.y() + ((child_idx & 2) >> 1) * octant_size,
                         octant_coord.z() + ((child_idx & 4) >> 2) * octant_size);
}

/*
 * \brief Computes the non-sibling neighbourhood around an octants. In the
 * special case in which the octant lies on an edge, neighbour are duplicated
 * as movement outside the enclosing cube is forbidden.
 * \param result 7-vector containing the neighbours
 * \param octant_key
 * \param depth of octant
 * \param voxel_depth max depth of the tree on which the octant lives
 */
inline void exterior_neighbours(se::key_t result[7],
    const se::key_t octant_key, const int depth, const int voxel_depth) {

  const int child_idx = se::child_idx(octant_key, depth, voxel_depth);
  Eigen::Vector3i dir = Eigen::Vector3i((child_idx & 1) ? 1 : -1,
                                        (child_idx & 2) ? 1 : -1,
                                        (child_idx & 4) ? 1 : -1);
  Eigen::Vector3i base_coord = far_corner(octant_key, depth, voxel_depth);
  dir.x() = se::math::in(base_coord.x() + dir.x() , 0, (1 << voxel_depth) - 1) ? dir.x() : 0;
  dir.y() = se::math::in(base_coord.y() + dir.y() , 0, (1 << voxel_depth) - 1) ? dir.y() : 0;
  dir.z() = se::math::in(base_coord.z() + dir.z() , 0, (1 << voxel_depth) - 1) ? dir.z() : 0;

 result[0] = se::keyops::encode(base_coord.x() + dir.x(), base_coord.y() + 0, base_coord.z() + 0,
     depth, voxel_depth);
 result[1] = se::keyops::encode(base_coord.x() + 0, base_coord.y() + dir.y(), base_coord.z() + 0,
     depth, voxel_depth);
 result[2] = se::keyops::encode(base_coord.x() + dir.x(), base_coord.y() + dir.y(), base_coord.z() + 0,
     depth, voxel_depth);
 result[3] = se::keyops::encode(base_coord.x() + 0, base_coord.y() + 0, base_coord.z() + dir.z(),
     depth, voxel_depth);
 result[4] = se::keyops::encode(base_coord.x() + dir.x(), base_coord.y() + 0, base_coord.z() + dir.z(),
     depth, voxel_depth);
 result[5] = se::keyops::encode(base_coord.x() + 0, base_coord.y() + dir.y(), base_coord.z() + dir.z(),
     depth, voxel_depth);
 result[6] = se::keyops::encode(base_coord.x() + dir.x(), base_coord.y() + dir.y(),
     base_coord.z() + dir.z(), depth, voxel_depth);
}

/*
 * \brief Computes the six face neighbours of an octant. These are stored in an
 * 4x6 matrix in which each column represents the homogeneous coordinates of a
 * neighbouring octant. The neighbours along the x axis come first, followed by
 * neighbours along the y axis and finally along the z axis. All coordinates are
 * clamped to be in the range between [0, max_size] where max size is given
 * by pow(2, voxel_depth).
 * \param res 4x6 matrix containing the neighbours
 * \param octant_coord octant coordinates
 * \param depth depth of the octant
 * \param voxel_depth max depth of the tree on which the octant lives
 */

static inline void one_neighbourhood(Eigen::Ref<Eigen::Matrix<int, 4, 6>> res,
    const Eigen::Vector3i& octant_coord, const int depth, const int voxel_depth) {
  const Eigen::Vector3i base_coord = octant_coord;
  const int size = 1 << voxel_depth;
  const int step = 1 << (voxel_depth - depth);
  Eigen::Matrix<int, 4, 6> cross;
  res <<
    -step, step,     0,    0,     0,    0,
        0,    0, -step, step,     0,    0,
        0,    0,     0,    0, -step, step,
        0,    0,     0,    0,     0,    0;
    res.colwise() += base_coord.homogeneous();
    res = res.unaryExpr([size](const int a) {
        return std::max(std::min(a, size-1), 0);
        });
}

/*
 * \brief Computes the six face neighbours of an octant. These are stored in an
 * 4x6 matrix in which each column represents the homogeneous coordinates of a
 * neighbouring octant. The neighbours along the x axis come first, followed by
 * neighbours along the y axis and finally along the z axis. All coordinates are
 * clamped to be in the range between [0, max_size] where max size is given
 * by pow(2, voxel_depth).
 * \param res 4x6 matrix containing the neighbours
 * \param octant octant key
 * \param voxel_depth max depth of the tree on which the octant lives
 */

static inline void one_neighbourhood(Eigen::Ref<Eigen::Matrix<int, 4, 6>> res,
    const se::key_t octant_key, const int voxel_depth) {
  one_neighbourhood(res, se::keyops::decode(octant_key), se::keyops::depth(octant_key),
      voxel_depth);
}

/*
 * \brief Computes the morton number of all siblings around an octant,
 * including itself.
 * \param result 8-vector containing the neighbours
 * \param octant
 * \param voxel_depth max depth of the tree on which the octant lives
 */
inline void siblings(se::key_t result[8],
    const se::key_t octant_key, const int voxel_depth) {
  const int depth = (octant_key & SCALE_MASK);
  const int shift = 3 * (voxel_depth - depth);
  const se::key_t parent_key = parent(octant_key, voxel_depth) + 1; // set-up next depth
  for(int i = 0; i < 8; ++i) {
    result[i] = parent_key | (i << shift);
  }
}
}
#endif


/*
 * Copyright 2014-2017, Andrea Del Prete, Rohan Budhiraja LAAS-CNRS
 *
 */

#include <Eigen/Dense>

#include <dynamic-graph/factory.h>
#include <sot/core/debug.hh>
#include <sot/torque_control/torque-offset-estimator.hh>
#include <sot/torque_control/commands-helper.hh>
#include <sot/torque_control/motor-model.hh>

namespace dynamicgraph {
namespace sot {
namespace torque_control {

#define ALL_INPUT_SIGNALS m_base6d_encodersSIN << m_accelerometerSIN << m_jointTorquesSIN << m_gyroscopeSIN

#define ALL_OUTPUT_SIGNALS m_jointTorquesEstimatedSOUT

namespace dynamicgraph = ::dynamicgraph;
using namespace dynamicgraph;
using namespace dynamicgraph::command;
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 TorqueOffsetEstimator EntityClassName;
typedef int dummy;

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

/* --- CONSTRUCTION ----------------------------------------------------- */
/* --- CONSTRUCTION ----------------------------------------------------- */
/* --- CONSTRUCTION ----------------------------------------------------- */
TorqueOffsetEstimator::TorqueOffsetEstimator(const std::string& name)
    : Entity(name),
      CONSTRUCT_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(accelerometer, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(gyroscope, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_IN(jointTorques, dynamicgraph::Vector),
      CONSTRUCT_SIGNAL_INNER(collectSensorData, dynamicgraph::Vector, ALL_INPUT_SIGNALS),
      CONSTRUCT_SIGNAL_OUT(jointTorquesEstimated, dynamicgraph::Vector,
                           m_collectSensorDataSINNER << ALL_INPUT_SIGNALS),
      sensor_offset_status(PRECOMPUTATION),
      current_progress(0) {
  Entity::signalRegistration(ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);
  addCommand("init", makeCommandVoid5(*this, &TorqueOffsetEstimator::init,
                                      docCommandVoid5("Initialize the estimator", "urdfFilePath",
                                                      "Homogeneous transformation from chest frame to IMU frame",
                                                      "Maximum angular velocity allowed in either axis",
                                                      "Freeflyer joint name", "chest joint name")));
  addCommand("computeOffset",
             makeCommandVoid2(*this, &TorqueOffsetEstimator::computeOffset,
                              docCommandVoid2("Compute the offset for sensor calibration",
                                              "Number of iteration to average over.", "Maximum allowed offset")));
  addCommand("getSensorOffsets", makeDirectGetter(*this, &jointTorqueOffsets,
                                                  docDirectGetter("Return the computed sensor offsets", "vector")));

  // encSignals.clear();
  // accSignals.clear();
  // gyrSignals.clear();
  // tauSignals.clear();
  // stdVecJointTorqueOffsets.clear();
}

/* --- COMMANDS ---------------------------------------------------------- */
/* --- COMMANDS ---------------------------------------------------------- */
/* --- COMMANDS ---------------------------------------------------------- */
void TorqueOffsetEstimator::init(const std::string& robotRef, const Eigen::Matrix4d& m_torso_X_imu_,
                                 const double& gyro_epsilon_, const std::string& ffJointName,
                                 const std::string& torsoJointName) {
  try {
    /* Retrieve m_robot_util  informations */
    std::string localName(robotRef);
    if (isNameInRobotUtil(localName)) {
      m_robot_util = getRobotUtil(localName);
      std::cerr << "m_robot_util:" << m_robot_util << std::endl;
    } else {
      SEND_MSG("You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
      return;
    }

    pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(), m_model);
    // assert(m_model.nq == N_JOINTS+7);
    // assert(m_model.nv == N_JOINTS+6);

    jointTorqueOffsets.resize(m_model.nv - 6);
    jointTorqueOffsets.setZero();
    ffIndex = m_model.getJointId(ffJointName);
    torsoIndex = m_model.getJointId(torsoJointName);
  } catch (const std::exception& e) {
    std::cout << e.what();
    SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
    return;
  }
  m_data = new pinocchio::Data(m_model);
  m_torso_X_imu.rotation() = m_torso_X_imu_.block<3, 3>(0, 0);
  m_torso_X_imu.translation() = m_torso_X_imu_.block<3, 1>(0, 3);
  gyro_epsilon = gyro_epsilon_;
}

void TorqueOffsetEstimator::computeOffset(const int& nIterations, const double& epsilon_) {
  if (sensor_offset_status == PRECOMPUTATION) {
    SEND_MSG("Starting offset computation with no. iterations:" + nIterations, MSG_TYPE_DEBUG);
    n_iterations = nIterations;
    epsilon = epsilon_;
    sensor_offset_status = INPROGRESS;
  } else if (sensor_offset_status == INPROGRESS) {
    SEND_MSG("Collecting input signals. Please keep the graph running", MSG_TYPE_WARNING);
  } else {  // sensor_offset_status == COMPUTED
    SEND_MSG("Recomputing offset with no. iterations:" + nIterations, MSG_TYPE_DEBUG);

    // stdVecJointTorqueOffsets.clear();
    current_progress = 0;
    jointTorqueOffsets.setZero();

    n_iterations = nIterations;
    epsilon = epsilon_;
    sensor_offset_status = INPROGRESS;
  }
  return;
}

DEFINE_SIGNAL_INNER_FUNCTION(collectSensorData, dummy) {
  if (sensor_offset_status == INPROGRESS) {
    const Eigen::VectorXd& gyro = m_gyroscopeSIN(iter);
    if (gyro.array().abs().maxCoeff() >= gyro_epsilon) {
      SEND_MSG("Very High Angular Rotations.", MSG_TYPE_ERROR_STREAM);
    }

    // Check the current iteration status
    int i = current_progress;

    if (i < n_iterations) {
      SEND_MSG("Collecting signals for iteration no:" + i, MSG_TYPE_DEBUG_STREAM);

      const Eigen::VectorXd& sot_enc = m_base6d_encodersSIN(iter);
      const Eigen::VectorXd& IMU_acc = m_accelerometerSIN(iter);
      const Eigen::VectorXd& tau = m_jointTorquesSIN(iter);

      Eigen::VectorXd enc(m_model.nq);
      // joints_sot_to_urdf(sot_enc.tail(m_model.nv-6), enc.tail(m_model.nv-6));
      enc.tail(m_model.nv - 6) = sot_enc.tail(m_model.nv - 6);
      // base_sot_to_urdf(sot_enc.head<6>(), enc.head<7>());
      enc.head<6>().setZero();
      enc[6] = 1.0;

      // Get the transformation from ff(f) to torso (t) to IMU(i) frame:
      // fMi = oMf^-1 * fMt * tMi
      pinocchio::forwardKinematics(m_model, *m_data, enc);
      // pinocchio::SE3 fMi = m_data->oMi[ffIndex].inverse()*m_data->oMi[torsoIndex]*m_torso_X_imu;
      pinocchio::SE3 oMimu = m_data->oMi[torsoIndex] * m_torso_X_imu;

      // Move the IMU signal to the base frame.
      // angularAcceleration is zero. Intermediate frame acc and velocities are zero
      const dynamicgraph::Vector acc = oMimu.rotation() * IMU_acc;  // Torso Acceleration

      // Deal with gravity predefined in robot model. Robot Z should be pointing upwards
      m_model.gravity.linear() = acc;
      // Set fixed for DEBUG
      // m_model.gravity.linear() = m_model.gravity981;

      const Eigen::VectorXd& tau_rnea =
          pinocchio::rnea(m_model, *m_data, enc, Eigen::VectorXd::Zero(m_model.nv), Eigen::VectorXd::Zero(m_model.nv));
      const Eigen::VectorXd current_offset = tau - tau_rnea.tail(m_model.nv - 6);
      if (current_offset.array().abs().maxCoeff() >= epsilon) {
        SEND_MSG("Too high torque offset estimated for iteration" + i, MSG_TYPE_ERROR);
        assert(false);
      }
      // encSignals.push_back(_enc);
      // accSignals.push_back(_acc);
      // gyrSignals.push_back(_gyr);
      // tauSignals.push_back(_tau);
      jointTorqueOffsets += current_offset;
      current_progress++;
    } else if (i == n_iterations) {
      // Find the mean, enough signals collected
      jointTorqueOffsets /= n_iterations;
      sensor_offset_status = COMPUTED;
    } else {  // i > n_iterations
      // Doesn't reach here.
      assert(false && "Error in collectSensorData. ");
    }
  }
  //        else { // sensor_offset_status == COMPUTED
  //        }
  return s;
}

DEFINE_SIGNAL_OUT_FUNCTION(jointTorquesEstimated, dynamicgraph::Vector) {
  m_collectSensorDataSINNER(iter);

  if (s.size() != m_model.nv - 6) s.resize(m_model.nv - 6);

  if (sensor_offset_status == PRECOMPUTATION || sensor_offset_status == INPROGRESS) {
    s = m_jointTorquesSIN(iter);
  } else {  // sensor_offset_status == COMPUTED
    const dynamicgraph::Vector& inputTorques = m_jointTorquesSIN(iter);
    s = inputTorques - jointTorqueOffsets;
  }
  return s;
}

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

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