#include "tkdCmdParser.h"
#include "itkImage.h"

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

#include <boost/random.hpp>
//#include <boost/test/unit_test.hpp>
//#include <boost/test/floating_point_comparison.hpp>

#include <boost/accumulators/numeric/functional/vector.hpp>
#include <boost/accumulators/numeric/functional/complex.hpp>
#include <boost/accumulators/numeric/functional/valarray.hpp>

#include <boost/accumulators/accumulators.hpp>

#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/density.hpp>
#include <boost/accumulators/statistics/p_square_cumulative_distribution.hpp>

#include "graphCommon.h"

using namespace boost;
using namespace boost::accumulators;

/**
 * @author: W.M. Otte
 * @date: 19-11-2009
 * @aim: Determine histogram from given input numbers.
 * To determine image histograms instead of list histograms, use imagehistogram.
 */
class Histogram
{

public:

	typedef double PixelType;
	typedef vnl_vector< PixelType > VectorType;
	typedef vnl_matrix< PixelType > MatrixType;

	/**
	 * Run.
	 */
	void run( const std::string& input, const std::string& output,
					bool cumulative, bool reverse, unsigned int bins )
	{
		// [ 1 ] Read time-series.
		// --------------------------------------
		VectorType inputs;
		graph::Graph< PixelType >::GetVectorFromFile( input, inputs );

		// [ 2 ] get histogram.
		// --------------------------------------
		VectorType positions( bins );
		VectorType values( bins );

		getHistogram( positions, values, inputs, bins, cumulative, reverse );

		// [ 3 ] write to disk.
		// --------------------------------------

		graph::Graph< PixelType >::WriteVectorToFile( output + "_density.txt", values );

		graph::Graph< PixelType >::WriteVectorToFile( output + "_positions.txt", positions );

		exit( EXIT_SUCCESS );
	}

protected:

	/**
	 * Convert input vector into histogram.
	 *
	 * The histogram density estimator returns a histogram of the sample distribution.
	 * The positions and sizes of the bins are determined
	 * using a specifiable number of cached samples (cache_size).
	 *
	 * The range between the minimum and the maximum of the cached samples
	 * is subdivided into a specifiable number of bins (num_bins) of same size.
	 * Additionally, an under- and an overflow bin is added to
	 * capture future under- and overflow samples.
	 * Once the bins are determined, the cached samples and all
	 * subsequent samples are added to the correct bins.
	 * At the end, a range of std::pair is return,
	 * where each pair contains the position of the bin (lower bound)
	 * and the samples count (normalized with the total number of samples).
	 *
	 * density_cache_size: Number of first samples used to determine min and max.
	 * density_num_bins: Number of bins (two additional bins collect under- and overflow samples).
	 */
	void getHistogram( VectorType& positions, VectorType& values,
							const VectorType& input, unsigned int bins,
								bool cumulative, bool reverse )
	{
		typedef accumulator_set< PixelType, stats< tag::density > > acc_density_t;
		typedef accumulator_set< PixelType, stats< tag::p_square_cumulative_distribution > > acc_cumul_t;
		typedef iterator_range< std::vector< std::pair< PixelType, PixelType > >::iterator > iterator_type;

		if ( ! cumulative )
		{
			// define accumulator with density feature.
			acc_density_t acc( tag::density::cache_size = input.size(), tag::density::num_bins = bins );

			// fill accumulator
			for ( unsigned int i = 0; i < input.size(); i++ )
			{
				acc( input[i] );
			}

			// get densities
			iterator_type densities = density( acc );

			positions = VectorType( densities.size() );
			values    = VectorType( densities.size() );

			// insert densities in output vector.
			for( unsigned int i = 0; i < densities.size(); i++ )
			{
				positions[i] = densities[i].first;
				values[i]    = densities[i].second;
			}

		}
		else // cumulative
		{
			acc_cumul_t acc( p_square_cumulative_distribution_num_cells = bins );

			// fill accumulator
			for ( unsigned int i = 0; i < input.size(); i++ )
			{
				acc( input[i] );
			}
			iterator_type histogram = p_square_cumulative_distribution( acc );

			// insert histogram in output vector.
			for( unsigned int i = 0; i < histogram.size(); i++ )
			{
				positions[i] = i;
				values[i]    = histogram[i].first;
			}

		}

		if ( reverse )
		{
			values.flip();
		}
	}
};

/**
 * Main hierarchy.
 */
int main( int argc, char ** argv )
{
	tkd::CmdParser p( "histogram", "Construct (cumulative) histogram from given list of numbers." );

	std::string input;
	std::string output;

	bool cumulative = false;
	bool reverse = false;
	int bins = 128;

	p.AddArgument( input, "input" )
		->AddAlias( "i" )
		->SetInput( "filename" )
		->SetDescription( "List input (column format)" )
		->SetRequired( true )
		-> SetMinMax( 1, 1 );

	p.AddArgument( output, "output" )
		->AddAlias( "o" )
		->SetInput( "filename" )
		->SetDescription( "Rootname histogram output (column format)" )
		->SetRequired( true )
		-> SetMinMax( 1, 1 );

	p.AddArgument( cumulative, "cumulative" )
		->AddAlias( "c" )
		->SetInput( "bool" )
		->SetDescription( "Cumulative histogram [ default: false ]" )
		->SetRequired( false );

	p.AddArgument( reverse, "reverse" )
		->AddAlias( "r" )
		->SetInput( "bool" )
		->SetDescription( "Reverse histogram [ default: false ]" )
		->SetRequired( false );

	p.AddArgument( bins, "bins" )
		->AddAlias( "b" )
		->SetInput( "uint" )
		->SetDescription( "Number of histogram bins [ default: 128 ]" )
		->SetRequired( false );

	if ( !p.Parse( argc, argv ) )
	{
		p.PrintUsage( std::cout );
		return EXIT_FAILURE;
	}

	Histogram histogram;

	histogram.run( input, output, cumulative, reverse, ( unsigned ) bins );

	return EXIT_SUCCESS;
}


/**
 * @file  CableTensionFactor.cpp
 * @brief Cable tension factor: relates cable tension, two mounting points, and
 * resultant forces
 * @author Frank Dellaert
 * @author Gerry Chen
 */

#include "CableTensionFactor.h"

#include <gtdynamics/utils/DynamicsSymbol.h>

#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>

#include <boost/optional.hpp>
#include <iostream>
#include <string>

using namespace gtsam;

namespace gtdynamics {

/******************************************************************************/
Vector6 CableTensionFactor::computeWrench(
    double t, const Pose3 &wTx, boost::optional<Matrix &> H_t,
    boost::optional<Matrix &> H_wTx) const {
  // Jacobians: cable direction
  Matrix33 dir_H_wPb;
  Matrix36 wPb_H_wTx;
  // Jacobians: force to wrench conversion
  Matrix31 wf_H_t;
  Matrix33 wf_H_dir;
  Matrix33 xf_H_wf;
  Matrix33 xf_H_wRx;
  Matrix63 H_xf;
  Matrix33 xm_H_xf;  // = H_xf.topRows<3>(); TODO(gerry): pointer?

  // cable direction
  Point3 wPb = wTx.transformFrom(xPb_, H_wTx ? &wPb_H_wTx : 0);
  Vector3 dir = normalize(wPb - wPa_, H_wTx ? &dir_H_wPb : 0);
  // force->wrench
  Vector3 wf = -t * dir;
  if (H_t) wf_H_t = -dir;
  if (H_wTx) wf_H_dir = -t * I_3x3;
  Vector3 xf = wTx.rotation().unrotate(wf,  // force in the EE frame
                                       H_wTx ? &xf_H_wRx : 0,
                                       (H_t || H_wTx) ? &xf_H_wf : 0);
  Vector3 xm = cross(xPb_, xf,     // moment in the EE frame
                     boost::none,  //
                     (H_t || H_wTx) ? &xm_H_xf : 0);

  Vector6 F = (Vector6() << xm, xf).finished();
  if (H_t || H_wTx) H_xf << xm_H_xf, I_3x3;
  if (H_t) *H_t = H_xf * xf_H_wf * wf_H_t;
  if (H_wTx) {
    *(H_wTx) = H_xf * xf_H_wf * wf_H_dir * dir_H_wPb * wPb_H_wTx;
    H_wTx->leftCols<3>() += H_xf * xf_H_wRx;
  }
  return F;
}

/******************************************************************************/
Vector6 CableTensionFactor::computeWrenchUsingAdjoint(
    double t, const Pose3 &wTx, boost::optional<Matrix &> H_t,
    boost::optional<Matrix &> H_wTx) const {
  // Jacobians: cable direction
  Matrix33 dir_H_wPb;
  Matrix36 wPb_H_wTx;
  // Jacobians: force to wrench conversion
  Matrix61 wF_H_t;
  Matrix63 wF_H_dir;

  // cable direction
  Point3 wPb = wTx.transformFrom(xPb_, H_wTx ? &wPb_H_wTx : 0);
  Vector3 dir = normalize(wPb - wPa_, H_wTx ? &dir_H_wPb : 0);
  // force->wrench
  Pose3 bTx = Pose3(wTx.rotation().inverse(), xPb_).inverse();
  Vector6 wF = (Vector6() << 0, 0, 0, -t * dir).finished();
  if (H_t) wF_H_t = (Matrix61() << 0, 0, 0, -dir).finished();
  if (H_wTx) wF_H_dir = (Matrix63() << Z_3x3, -t * I_3x3).finished();
  Vector6 F = bTx.AdjointMap().transpose() * wF;
  // TODO(gerry): find jacobian of adjoint map
  return F;
}

}  // namespace gtdynamics


#include "wali/xfa/Xfa.hpp"
#include "wali/xfa/ast.hpp"
#include "wali/xfa/RelationMaker.hpp"
#include "wali/wpds/WPDS.hpp"

#include <boost/lexical_cast.hpp>

#include <iostream>
#include <vector>

#include "wali/domains/genkill/GenKillXformerTemplate.hpp"

using namespace wali::xfa;
using namespace wali;
using namespace wali::wfa;
using namespace wali::wpds;

struct VarSet : public std::set<std::string>
{
    static VarSet & UniverseSet()
    {
        static VarSet u;
        return u;
    }

    static VarSet EmptySet()
    {
        return VarSet();
    }

    static bool Eq( const VarSet& x, const VarSet& y ) {
        return x == y;
    }

    static VarSet Diff( const VarSet& x, const VarSet& y,
                        bool UNUSED_PARAMETER(normalizing) = false )
    {
        VarSet ret;
        std::set_difference(x.begin(), x.end(),
                            y.begin(), y.end(),
                            std::inserter(ret, ret.begin()));
        return ret;
    }

    static VarSet Union( const VarSet& x, const VarSet& y )
    {
        VarSet ret;
        std::set_union(x.begin(), x.end(),
                       y.begin(), y.end(),
                       std::inserter(ret, ret.begin()));
        return ret;
    }

    static VarSet Intersect( const VarSet& x, const VarSet& y )
    {
        VarSet ret;
        std::set_intersection(x.begin(), x.end(),
                              y.begin(), y.end(),
                              std::inserter(ret, ret.begin()));
        return ret;
    }

    std::ostream& print(std::ostream& o) const
    {
        o << "{";
        for (VarSet::const_iterator iter = this->begin();
             iter != this->end(); ++iter)
        {
            o << *iter << ", ";
        }
        o << "}";
        return o;
    }

};

typedef GenKillTransformer_T<VarSet> GenKillWeight;

sem_elem_t
makeGKW(VarSet const & kill, VarSet const & gen)
{
    return GenKillWeight::makeGenKillTransformer_T(kill, gen);
}

namespace wali {
    namespace xfa {

        namespace details {

        std::string
        var_name(int id,
                 std::string const & domain_var_name_prefix)
        {
            return domain_var_name_prefix + "var" + boost::lexical_cast<std::string>(id);
        }

        void
        register_vars(ast::XfaAst const & ast,
                      std::string const & prefix)
        {
            VarSet & all_vars = VarSet::UniverseSet();

            for (auto ast_trans = ast.transitions.begin(); ast_trans != ast.transitions.end(); ++ast_trans) {
                ast::ActionList & acts = (*ast_trans)->actions;
                for (auto act_it = acts.begin(); act_it != acts.end(); ++act_it) {
                    auto & act = **act_it;
                    if (act.action_type == "fire"
                        || act.action_type == "alert"
                        || act.action_type == "reject")
                    {
                        continue;
                    }
                    assert (act.action_type == "ctr2");
                    all_vars.insert(var_name(act.operand_id, prefix));
                }
            }

        }

        struct ReadTransitionException {
            sem_elem_t back_weight, bit1_weight, init_weight;

            ReadTransitionException(sem_elem_t init, sem_elem_t back, sem_elem_t bit1)
                : back_weight(back)
                , bit1_weight(bit1)
                , init_weight(init)
            {}
        };

        struct WeightedTransition {
            Key source;
            Key symbol;
            Key target;
            sem_elem_t weight;

            WeightedTransition(Key src,
                               Key sym,
                               Key tgt,
                               sem_elem_t w)
                : source(src), symbol(sym), target(tgt), weight(w)
            {}
        };

        typedef std::vector<WeightedTransition> TransList;

        sem_elem_t
        get_relation(ast::Action const & act,
                     std::string const & prefix)
        {
            VarSet empty;
            sem_elem_t zero = makeGKW(empty, empty);

            if (act.action_type == "fire"
                || act.action_type == "alert")
            {
                std::cerr << "    fire or alert\n";
                // Hmmm.
                return zero;
            }

            assert(act.command);
            auto cmd = *act.command;

            if (cmd.name == "read") {
                VarSet killed;
                killed.insert(var_name(act.operand_id, prefix));

                sem_elem_t kill_weight = makeGKW(killed, empty);

                throw ReadTransitionException(kill_weight, zero, zero);
            }

            if (cmd.name == "reset") {
                VarSet killed;
                assert(cmd.arguments.size() == 1u);
                killed.insert(var_name(act.operand_id, prefix));
                sem_elem_t kill_weight = makeGKW(killed, empty);
                return kill_weight;
            }

            if (cmd.name == "incr") {
                VarSet saved;
                saved.insert(var_name(act.operand_id, prefix));
                sem_elem_t save_weight = makeGKW(empty, saved);
                return save_weight;
            }

            if (cmd.name == "testnectr2") {
                assert(cmd.arguments.size() == 1u);
                assert(cmd.consequent);
                assert(!cmd.alternative);
                assert(cmd.consequent->action_type == "reject");

                int rhs_id = boost::lexical_cast<int>(cmd.arguments[0]);
                std::string lhs_name = var_name(act.operand_id, prefix);
                std::string rhs_name = var_name(rhs_id, prefix);

                VarSet saved;
                saved.insert(lhs_name);
                saved.insert(rhs_name);
                sem_elem_t save_weight = makeGKW(empty, saved);

                return save_weight;
            }

            assert(false);
        }

        TransList
        get_transitions(ast::Transition const & trans,
                        std::string const & prefix)
        {
            Key source = getKey(trans.source);
            Key dest = getKey(trans.dest);
            Key eps(wali::WALI_EPSILON);

            TransList ret;
            VarSet empty;
            sem_elem_t rel = makeGKW(empty, empty);

            try {
                if (trans.actions.size() == 0u) {
                    //rel->is_effectively_one = true;
                }
                else if (trans.actions.size() == 1u) {
                    rel = get_relation(*trans.actions[0], prefix);
                }
                else {
                    assert(false);
                }

                auto const & syms = trans.symbols;
                for (auto sym = syms.begin(); sym != syms.end(); ++sym) {
                    auto name = dynamic_cast<ast::Name*>(sym->get());
                    assert(name);
                    if (name->name == "epsilon") {
                        ret.push_back(WeightedTransition(source, eps, dest, rel));
                    }
                    else {
                        ret.push_back(WeightedTransition(source, getKey(name->name), dest, rel));
                    }
                }
            }
            catch (ReadTransitionException & e) {
                auto const & syms = trans.symbols;
                for (auto sym = syms.begin(); sym != syms.end(); ++sym) {
                    auto name = dynamic_cast<ast::Name*>(sym->get());
                    assert(name->name != "epsilon");

                    // source ---> intermediate_name ---> dest
                    //                               <---
                    std::stringstream intermediate_name;
                    intermediate_name << trans.source << "__" << name->name;
                    std::stringstream bit0_name, bit1_name;;
                    bit0_name << name->name << "__bit_is_0";
                    bit1_name << name->name << "__bit_is_1";
                    Key startbits = getKey("__startbits");
                    Key bit0 = getKey(bit0_name.str());
                    Key bit1 = getKey(bit1_name.str());
                    Key intermediate = getKey(intermediate_name.str());

                    // source --> intermediate has identity weight, symbol '__startbits'
                    ret.push_back(WeightedTransition(source, startbits, intermediate, e.init_weight));

                    // intermediate --> dest has two transitions:
                    //     '__bit_0' has identity weight
                    //     '__bit_1' has +1 weight
                                  ret.push_back(WeightedTransition(intermediate, bit0, dest, e.init_weight->one()));
                    ret.push_back(WeightedTransition(intermediate, bit1, dest, e.bit1_weight));

                    // dest --> intermediate has epsilon and weight *2
                    ret.push_back(WeightedTransition(dest, eps, intermediate, e.back_weight));
                }
            }

            return ret;
        }

        wali::wfa::WFA
        create_live_var_wfa(ast::XfaAst const & ast,
                            std::string const & domain_var_name_prefix)
        {
            register_vars(ast, domain_var_name_prefix);

            wali::wfa::WFA ret;

            VarSet empty;
            sem_elem_t zero = makeGKW(empty, empty);
            zero = zero->zero();

            for (auto ast_state = ast.states.begin(); ast_state != ast.states.end(); ++ast_state) {
                Key state = getKey((*ast_state)->name);
                ret.addState(state, zero);
            }

            for (auto ast_trans = ast.transitions.begin(); ast_trans != ast.transitions.end(); ++ast_trans) {
                TransList transs = get_transitions(**ast_trans, domain_var_name_prefix);
                for (auto trans = transs.begin(); trans != transs.end(); ++trans) {
                    ret.addTrans(trans->source, trans->symbol, trans->target, trans->weight);
                }
            }

            ret.setInitialState(getKey(ast.start_state));

            return ret;
        }

        struct VarKiller : TransFunctor
        {
            WFA * gen_kill_results;
            Key p_state;
            RelationMaker * maker;

            VarSet get_live(Key state) {
                TransSet matches = gen_kill_results->match(p_state, state);
                if (matches.size() == 0) {
                    std::cerr << "WARNING: no prestar result for " << key2str(state) << "\n";
                    return VarSet();
                }
                if (matches.size() > 1) {
                    assert(false);
                }
                sem_elem_t w = (*(matches.begin()))->weight();
                GenKillWeight * gkw = dynamic_cast<GenKillWeight *>(w.get_ptr());
                //assert(gkw->getKill().empty());
                return gkw->getGen();
            }

            void
            operator() (ITrans * trans) {
                VarSet
                    source_live = get_live(trans->from()),
                    target_live = get_live(trans->to());

                VarSet killed = VarSet::Diff(source_live, target_live);

                for (VarSet::const_iterator v = killed.begin(); v != killed.end(); ++v) {
                    std::cout << "==== Killing variable " << *v << " on transition "
                              << key2str(trans->from()) << "--" << key2str(trans->stack()) << "--> "
                              << key2str(trans->to()) << "\n";

                    sem_elem_t kill_relation = maker->initialize_variable_to_val(*v, 0);
                    sem_elem_t new_weight = trans->weight()->extend(kill_relation);

                    trans->setWeight(new_weight);
                }
            }
        };

        bool
        always_include_trans(ITrans const * UNUSED_PARAMETER(t))
        {
            return true;
        }

        void
        kill_variables(ast::XfaAst const & ast,
                       wali::xfa::Xfa & xfa,
                       RelationMaker & maker,
                       std::string const & domain_var_name_prefix)
        {
            WFA wfa_genkill = create_live_var_wfa(ast, domain_var_name_prefix);

            // do prestar from the final state
            WFA query;
            Key p_state = getKey("__p");
            Key accept = getKey("__accept");
            Key target_accept = getKey("accept");

            query.addTrans(p_state, target_accept, accept,
                           wfa_genkill.getSomeWeight()->one());

            WPDS wpds;
            wfa_genkill.toWpds(p_state, &wpds, always_include_trans);
            WFA result = wpds.prestar(query);

            //
            VarKiller killer;
            killer.gen_kill_results = &result;
            killer.p_state = p_state;
            killer.maker = &maker;

            xfa.for_each(killer);
        }

        }

    }
}


#ifndef LEPP3_FILTER_CLOUD_POST_PT1_FILTER_H__
#define LEPP3_FILTER_CLOUD_POST_PT1_FILTER_H__

#include "lepp3/filter/cloud/post/CloudPostFilter.hpp"
#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>

namespace lepp {

/**
 * An implementation of a `CloudFilter` where points are included only
 * if they have been seen in a certain percentage of frames of the last N
 * frames.
 */
template<class PointT>
class Pt1Filter : public CloudPostFilter<PointT> {
  using MapPoint = lepp::cloud_post_filter::MapPoint;

public:
  virtual void newFrame() override;
  virtual void newPoint(PointT& p, PointCloudT& filtered) override;
  virtual void getFiltered(PointCloudT& filtered) override;

private:
  boost::unordered_set<MapPoint> this_frame_;
  boost::unordered_map<MapPoint, float> all_points_;
};
}

template<class PointT>
void lepp::Pt1Filter<PointT>::newFrame() {
  this_frame_.clear();
}

template<class PointT>
void lepp::Pt1Filter<PointT>::newPoint(PointT& p, PointCloudT& filtered) {
  MapPoint map_point = MapPoint(p.x * 100, p.y * 100, p.z * 100);
  float& ref = all_points_[map_point];
  // TODO Factor out the parameter value to a member constant.
  ref = 0.9*ref + 0.1*10;
  this_frame_.insert(map_point);
}

template<class PointT>
void lepp::Pt1Filter<PointT>::getFiltered(PointCloudT& filtered) {
  boost::unordered_map<MapPoint, float>::iterator it =
      all_points_.begin();
  while (it != all_points_.end()) {
    if (this_frame_.find(it->first) == this_frame_.end()) {
      it->second = 0.9*it->second + 0.1*0.;
    }
    if (it->second >= 4.) {
      PointT pt;
      pt.x = it->first.x / 100.;
      pt.y = it->first.y / 100.;
      pt.z = it->first.z / 100.;
      filtered.push_back(pt);
    }
    ++it;
  }
}

#endif


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

#include <vector>
#include <string>
#include <map>

#include <boost/lexical_cast.hpp>

#include <hashclash/saveload_gz.hpp>
#include <hashclash/md5detail.hpp>
#include <hashclash/differentialpath.hpp>
#include <hashclash/booleanfunction.hpp>

#include "main.hpp"

using namespace hashclash;
using namespace std;

void md5_backward_thread::md5_backward_differential_step(const differentialpath& path, path_container_autobalance& outpaths)
{
	const unsigned t = outpaths.t;
	const unsigned& maxcond = outpaths.maxcond;
	const unsigned& maxsdrs = outpaths.maxsdrs;
	const unsigned& maxweight = outpaths.maxweight;
	const unsigned& minweight = outpaths.minweight;

	const uint32 m_diff_t = outpaths.m_diff[md5_wt[t]];

	booleanfunction* F = 0;
	if (t < 16) F = &MD5_F_data;
	else if (t < 32) F = &MD5_G_data;
	else if (t < 48) F = &MD5_H_data;
	else if (t < 64) F = &MD5_I_data;

	newpath = path;
	newpath[int(t)-3].clear();
	newpath[t+1];

	unsigned totprecond = 0;
	unsigned totcond = 0;
	for (int k = int(t)+1; k <= min(outpaths.tend,newpath.tend()+1) && k <= 64; ++k)
		totprecond += newpath[k].hw();
	totcond = totprecond + newpath[int(t)].hw() + newpath[int(t)-1].hw();

	unsigned minextracond = 0;
	for (unsigned b = 0; b < 32; ++b)
	{
		Qtb[b] = newpath(int(t),b);
		Qtm1b[b] = newpath(int(t)-1,b);
		bf_outcome outconstant = F->outcome( Qtb[b], Qtm1b[b], bc_constant );
		bf_outcome outplus = F->outcome( Qtb[b], Qtm1b[b], bc_plus );
		bf_outcome outminus = F->outcome( Qtb[b], Qtm1b[b], bc_minus );
		if (outconstant.size() > 1 && outplus.size() > 1 && outminus.size() > 1)
			++minextracond;
	}
	uint32 Qtm2diff = newpath[int(t)-2].diff();
	unsigned Qtm2_hwnaf = hwnaf(Qtm2diff);
	if (totcond + Qtm2_hwnaf + minextracond > maxcond) return;
	wordconditions& newpathQt = newpath[t];
	wordconditions& newpathQtm1 = newpath[int(t)-1];
	wordconditions& newpathQtm2 = newpath[int(t)-2];
	wordconditions& newpathQtm3 = newpath[int(t)-3];
	uint32 dR = newpath[t+1].diff() - newpath[t].diff();

	rotate_difference(dR, 32-md5_rc[t], rotateddiff);
	double bestprob = 0;
	for (unsigned pr = 0; pr < rotateddiff.size(); ++pr)
		if (rotateddiff[pr].second > bestprob)
			bestprob = rotateddiff[pr].second;

	for (unsigned pr = 0; pr < rotateddiff.size(); ++pr)
	{
		if (rotateddiff[pr].second < bestprob * 0.75) continue;
		uint32 dT2 = rotateddiff[pr].first - m_diff_t;
		unsigned w = Qtm2_hwnaf+1;
		if (w < minweight) w = minweight;
		unsigned mincount = 0;
		if (minweight > 0)
			mincount = count_sdrs(Qtm2diff,minweight-1);
		while (
				(w < 33)
				&& (w+1 <= maxweight)
				&& (totcond + w + 1 + minextracond <= maxcond)
				&& (count_sdrs(Qtm2diff, w+1)-mincount <= maxsdrs)
			)
			++w;
		table_sdrs(sdrs, Qtm2diff, w);

		vector<sdr>::const_iterator cit = sdrs.begin(), citend = sdrs.end();
		for (; cit != citend; ++cit)
		{
			sdr sdrQtm2 = *cit;
			unsigned hwQtm2 = sdrQtm2.hw();
			if (hwQtm2 < minweight) continue;
			if (totcond + hwQtm2 + minextracond > maxcond)
				continue;
			newpathQtm2 = sdrQtm2;

			uint32 cnt = 1;
			uint32 dF_fixed = 0;
			unsigned maxextracond = 0;
			bval.clear();
			for (unsigned b = 0; b < 32; ++b)
			{
				Qtm2b[b] = newpathQtm2.get(b);
				foutcomes[b] = F->outcome( Qtb[b], Qtm1b[b], Qtm2b[b] );
				unsigned fsize = foutcomes[b].size();
				if (fsize > 1)
				{
					fdiv[b] = cnt;
					if (fsize == 2) cnt <<= 1;
					else if (fsize == 3) { cnt += cnt<<1; ++ maxextracond; }
					bval.push_back(b);
				} else
					dF_fixed += foutcomes[b](0,b);
			}
			if (totcond + hwQtm2 + bval.size() > maxcond)
				continue;
			if (outpaths.estimatefactor != 0)
			{
				outpaths.estimate(totcond + hwQtm2 + bval.size() + ((maxextracond+1)>>1), cnt);
				continue;
			}

			newpathQt = path[t];
			newpathQtm1 = path[int(t)-1];

			std::reverse(bval.begin(), bval.end());
			for (uint32 k = 0; k < cnt; ++k)
			{
				uint32 m = k;
				uint32 dF = dF_fixed;
				for (unsigned j = 0; j < bval.size(); ++j)
				{
					const unsigned b = bval[j];
					unsigned i = 0;
					while (m >= fdiv[b])
					{
						m -= fdiv[b];
						++i;
					}
					const bf_conditions& newconditions = F->backwardconditions( Qtb[b], Qtm1b[b], Qtm2b[b], foutcomes[b][i] );
					newpathQt.set(b, newconditions.first);
					newpathQtm1.set(b, newconditions.second);
					newpathQtm2.set(b, newconditions.third);
					dF += foutcomes[b](i,b);
				}
				newpathQtm3 = naf(dT2 - dF);
				unsigned ncond = totprecond + newpathQt.hw() + newpathQtm1.hw() + newpathQtm2.hw();
				if (outpaths.includenaf)
				{
					if (outpaths.halfnafweight)
						ncond += (newpathQtm3.hw()>>1);
					else
						ncond += newpathQtm3.hw();
				}
				if (ncond <= maxcond)
					outpaths.push_back(newpath, ncond);
			} // for cnt
		} // for sdrs
	} // for rotateddiff
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/rem_pio2_cephes.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/pio_4.hpp>

STF_CASE_TPL (" rem_pio2_cephes",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::rem_pio2_cephes;
  using iT = bd::as_integer_t<T>;
  using r_t = decltype(rem_pio2_cephes(T()));

  STF_EXPR_IS( (rem_pio2_cephes(T()))
                  , (std::pair<iT,T>)
                  );

  {
    r_t res = rem_pio2_cephes(bs::Zero<T>());
    STF_ULP_EQUAL( res.first, bs::Zero<iT>(), 0.5);
    STF_ULP_EQUAL( res.second, bs::Zero<T>(), 0.5);
  }
  {
    r_t res = rem_pio2_cephes(bs::Pio_4<T>());
    STF_ULP_EQUAL( res.first, bs::Zero<iT>(), 0.5);
    STF_ULP_EQUAL( res.second, bs::Pio_4<T>(), 0.5);
  }
} // end of test for floating_


#include <ros/ros.h>
#include <stdio.h>
#include <Eigen/Dense>
#include <cmath>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <sstream>

using namespace Eigen;

//const float SAMPLE_TIME_S       = 0.01;
//const float DEFAULT_SPEED_MPS   = 0.2;

class ObstacleSimulator{

    public:

    float ned_x = 0;
    float ned_y = 0;
    float yaw = 0;
    float challenge = 0;
    ros::Publisher marker_pub; //= n.advertise<visualization_msgs::Marker>("Obstacle_markers",1000);

    void ned_to_body(float ned_x2, float ned_y2, float* x2, float* y2 )
    {

    }

    Matrix2d rotation_matrix(float angle)
    {
        Matrix2d a;
        //a.resize(2,2);
        a << cos(angle), -1*sin(angle),
             sin(angle), cos(angle);
        return a;
    }

    void rviz_markers(ros::NodeHandle *nh)
    {
            this->marker_pub = nh->advertise<visualization_msgs::Marker>("Obstacle_markers",1000);
            visualization_msgs::Marker marker1,marker2,marker3;
            visualization_msgs::MarkerArray marker_array;
            if (this->challenge == 0){
                //Primer poste gate
                marker1.header.frame_id  = "/world";
                marker1.header.stamp = ros::Time();
                marker1.type = visualization_msgs::Marker::CYLINDER;
                marker1.action = visualization_msgs::Marker::ADD;
                marker1.pose.position.x = 1;
                marker1.pose.position.y = -0.5;
                marker1.pose.position.z = -1.5;
                marker1.pose.orientation.x = 0.0;
                marker1.pose.orientation.y = 0.0;
                marker1.pose.orientation.z = 0.0;
                marker1.pose.orientation.w = 1.0;
                marker1.scale.x = 0.762;
                marker1.scale.y = 0.762;
                marker1.scale.z = 1;
                marker1.color.a = 1.0;
                marker1.color.r = 0.0;
                marker1.color.g = 1.0;
                marker1.color.b = 0.0;

                //Segundo poste gate
                marker2.header.frame_id  = "/world";
                marker2.header.stamp = ros::Time();
                marker2.type = visualization_msgs::Marker::CYLINDER;
                marker2.action = visualization_msgs::Marker::ADD;
                marker2.pose.position.x = 1;
                marker2.pose.position.y = 0.5;
                marker2.pose.position.z = -1.5;
                marker2.pose.orientation.x = 0.0;
                marker2.pose.orientation.y = 0.0;
                marker2.pose.orientation.z = 0.0;
                marker2.pose.orientation.w = 1.0;
                marker2.scale.x = 0.762;
                marker2.scale.y = 0.762;
                marker2.scale.z = 1;
                marker2.color.a = 1.0;
                marker2.color.r = 0.0;
                marker2.color.g = 1.0;
                marker2.color.b = 0.0;

                this->marker_pub.publish(marker1);
                //this->marker_pub.publish(marker2);

            }

    }

};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "uuv_obstacle_simulation_node");
    ros::NodeHandle nh;
    ros::Rate loop_rate(10);
    ObstacleSimulator obstacleSim;
    while(ros::ok()){
        obstacleSim.rviz_markers(&nh);
        ros::spinOnce();
        loop_rate.sleep();

    }
    return 0;
}


/*
Copyright (C) 2012 Mathias Eitz and Ronald Richter.
All rights reserved.

This file is part of the imdb library and is made available under
the terms of the BSD license (see the LICENSE file).
*/

#ifndef DESCRIPTORS__GIST_HPP
#define DESCRIPTORS__GIST_HPP

#include <string>
#include <complex>

#include <boost/function.hpp>

#include "../util/types.hpp"
#include "../descriptors/generator.hpp"

namespace imdb
{

/**
 * @ingroup generators
 * @brief The gist_generator class
 */
class gist_generator : public Generator
{
    typedef float                 float_t;
    typedef std::complex<float_t> complex_t;

    public:

    gist_generator(const ptree& params);

    void compute(anymap_t& data) const;

    private:

    void init_filter();

    const size_t _padding;

    const size_t _realwidth;
    const size_t _realheight;

    const size_t _num_x_tiles;
    const size_t _num_y_tiles;

    const size_t _num_freqs;
    const size_t _num_orients;

    const double _max_peak_freq;
    const double _delta_freq_oct;
    const double _bandwidth_oct;
    const double _angle_factor;

    const bool   _polar;

    const std::string _prefilter_str;

    const size_t _width;
    const size_t _height;

    boost::function<void (cv::Mat&)> _prefilter_ocv;
    std::vector<cv::Mat_<complex_t> > _filters;
};

} // namespace imdb

#endif // DESCRIPTORS__GIST_HPP


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Battin, R.H. An Introduction to the Mathematics and Methods of Astrodynamics,
 *          AIAA Education Series, 1999.
 *      Izzo, D. lambert_problem.h, keptoolbox.
 *
 *    Notes
 *      This code is an implementation of the method developed by Dario Izzo from ESA/ACT and
 *      publicly available at: http://keptoolbox.sourceforge.net/.
 *      After verification and validation, it was proven that this algorithm is faster and more
 *      robust than the implemented Lancaster & Blanchard and Gooding method. Notably, this method
 *      does not suffer from the near-pi singularity (pi-transfers are by nature singular).
 *
 */

#include "Tudat/Astrodynamics/MissionSegments/lambertTargeterIzzo.h"
#include "Tudat/Astrodynamics/MissionSegments/lambertRoutines.h"

#include <Eigen/Geometry>

namespace tudat
{
namespace mission_segments
{

//! Execute Lambert targeting solver.
void LambertTargeterIzzo::execute( )
{
    // Call Izzo's Lambert targeting routine.
    solveLambertProblemIzzo( cartesianPositionAtDeparture, cartesianPositionAtArrival,
                             timeOfFlight, gravitationalParameter, cartesianVelocityAtDeparture,
                             cartesianVelocityAtArrival, isRetrograde_, convergenceTolerance_,
                             maximumNumberOfIterations_ );
}

//! Get radial velocity at departure.
double LambertTargeterIzzo::getRadialVelocityAtDeparture( )
{
    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture
            = cartesianPositionAtDeparture.normalized( );

    // Compute radial velocity at departure.
    return cartesianVelocityAtDeparture.dot( radialUnitVectorAtDeparture );
}

//! Get radial velocity at arrival.
double LambertTargeterIzzo::getRadialVelocityAtArrival( )
{
    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute radial velocity at arrival.
    return cartesianVelocityAtArrival.dot( radialUnitVectorAtArrival );
}

//! Get transverse velocity at departure.
double LambertTargeterIzzo::getTransverseVelocityAtDeparture( )
{
    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtDeparture.cross( cartesianVelocityAtDeparture );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture
            = cartesianPositionAtDeparture.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtDeparture =
                angularMomentumUnitVector.cross( radialUnitVectorAtDeparture );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtDeparture.dot( tangentialUnitVectorAtDeparture );
}

//! Get transverse velocity at arrival.
double LambertTargeterIzzo::getTransverseVelocityAtArrival( )
{
    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtArrival.cross( cartesianVelocityAtArrival );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtArrival
            = angularMomentumUnitVector.cross( radialUnitVectorAtArrival );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtArrival.dot( tangentialUnitVectorAtArrival );
}

//! Get semi-major axis.
double LambertTargeterIzzo::getSemiMajorAxis( )
{
    // Compute specific orbital energy: eps = v^2/ - mu/r.
    const double specificOrbitalEnergy = cartesianVelocityAtDeparture.squaredNorm( ) / 2.0
            - gravitationalParameter / cartesianPositionAtDeparture.norm( );

    // Compute semi-major axis: a = -mu / 2*eps.
    return -gravitationalParameter / ( 2.0 * specificOrbitalEnergy );
}

} // namespace mission_segments
} // namespace tudat


#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
#include <OpenMesh/Core/IO/reader/OBJReader.hh>
#include <OpenMesh/Core/IO/writer/OBJWriter.hh>
#include <iostream>
#include <Eigen/Dense>
#include <stdio.h>
#include <math.h>
#include "meshmonk.hpp"

using namespace std;

typedef OpenMesh::DefaultTraits MyTraits;
typedef OpenMesh::TriMesh_ArrayKernelT<MyTraits>  TriMesh;
typedef Eigen::Matrix< int, Eigen::Dynamic, 3> FacesMat; //matrix Mx3 of type unsigned int
typedef Eigen::VectorXf VecDynFloat;
typedef Eigen::Matrix< float, Eigen::Dynamic, 6> FeatureMat; //matrix Mx6 of type float

int main()
{

    /*
    ############################################################################
    ##############################  INPUT  #####################################
    ############################################################################
    */
    //# IO variables
    //const string floatingDir = "/home/jonatan/projects/meshmonk/examples/data/karlijne/Normaal/Cleaned/CL0009A_Clean.obj";
//    const string floatingDir = "/home/jonatan/projects/meshmonk/examples/data/karlijne/Normaal/Cleaned/rigidResult.obj";
//    const string targetDir = "/home/jonatan/projects/meshmonk/examples/data/karlijne/Normaal/Cleaned/CL0013A_Clean.obj";
//    const string floatingDir = "/home/jonatan/projects/meshmonk/examples/faceTemplate.obj";
//    const string targetDir = "/home/jonatan/projects/meshmonk/examples/faceTarget.obj";
//    const string resultDir = "/home/jonatan/projects/meshmonk/examples/data/karlijne/Normaal/Cleaned/resultexample.obj";
//    const string floatingDir = "/home/jonatan/projects/meshmonk/examples/data/bunny.obj";
//    const string targetDir = "/home/jonatan/projects/meshmonk/examples/data/bunny2.obj";
//    const string resultDir = "/home/jonatan/projects/meshmonk/examples/data/bunnyResult.obj";

    //# My Matlab Data
//    const string floatingDir = "/home/jonatan/projects/meshmonk/examples/faceTemplate.obj";
//    const string targetDir = "/home/jonatan/projects/meshmonk/examples/faceTarget.obj";

    //# Peter's Matlab Data
    const string floatingDir = "/Users/maartenhuijsmans/GitProjects/meshmonk/meshmonk/floating.obj";
    const string targetDir = "/Users/maartenhuijsmans/GitProjects/meshmonk/meshmonk/target.obj";

    const string resultDir = "/home/jonatan/projects/meshmonk/examples/matlabResult.obj";

    //# Load meshes and convert to feature matrices
    FeatureMat floatingFeatures;
    FeatureMat targetFeatures;
    FacesMat floatingFaces;
    FacesMat targetFaces;
    meshmonk::read_obj_files(floatingDir, targetDir,
                    floatingFeatures, targetFeatures,
                    floatingFaces, targetFaces);

    const size_t numFloatingVertices = floatingFeatures.rows();
    const size_t numTargetVertices = targetFeatures.rows();
    VecDynFloat floatingFlags = VecDynFloat::Ones(numFloatingVertices);
    VecDynFloat targetFlags = VecDynFloat::Ones(numTargetVertices);
//
//
//
//    /*
//    ############################################################################
//    ##############################  REGISTRATION  ##############################
//    ############################################################################
////    */
    //# Parameters
    //## Pyramid
    const size_t numIterations = 60;
    const size_t numRigidIterations = 20;
    const size_t numPyramidLayers = 1;
    const float downsampleFloatStart = 0;
    const float downsampleTargetStart = 0;
    const float downsampleFloatEnd = 0;
    const float downsampleTargetEnd = 0;

    //## Correspondences
    const bool correspondencesSymmetric = true;
    const size_t correspondencesNumNeighbours = 5;

    //## Inliers
    const float inlierKappa = 4.0f;
    const bool inlierUseOrientation = false;

    //## Transformation
    const float transformSigma = 3.0f;
    const size_t transformNumViscousIterationsStart = 55;
    const size_t transformNumViscousIterationsEnd = 2;
    const size_t transformNumElasticIterationsStart = 56;
    const size_t transformNumElasticIterationsEnd = 3;

    //# Rigid registration
    //meshmonk::rigid_registration(floatingFeatures, targetFeatures, floatingFaces, targetFaces,
//                        floatingFlags, targetFlags, numRigidIterations,
//                        correspondencesSymmetric, correspondencesNumNeighbours, inlierKappa);
//
//
    //DEBUG
        std::cout << " -- Floating Features --\n" << floatingFeatures.topRows(10) << std::endl;
        std::cout << " -- Target Features --\n" << targetFeatures.topRows(10) << std::endl;
        std::cout << " -- Floating Faces --\n" << floatingFaces.topRows(10) << std::endl;
        std::cout << " -- Target Faces --\n" << targetFaces.topRows(10) << std::endl;
        std::cout << " -- Floating Flags --\n" << floatingFlags.topRows(10) << std::endl;
        std::cout << " -- Target Flags --\n" << targetFlags.topRows(10) << std::endl;
        //END DEBUG

        //# Call pyramid_registration()
        //DEBUG
        std::cout << "Num Floating Elements              - " << floatingFeatures.rows() << std::endl;
        std::cout << "Num Target Elements                - " << targetFeatures.rows() << std::endl;
        std::cout << "Num Floating Faces                 - " << floatingFaces.rows() << std::endl;
        std::cout << "Num Target Faces                   - " << targetFaces.rows() << std::endl;
        std::cout << "Num Iterations                     - " << numIterations << std::endl;
        std::cout << "Num Pyramid Layers                 - " << numPyramidLayers << std::endl;
        std::cout << "Downsample Float Start             - " << downsampleFloatStart << std::endl;
        std::cout << "Downsample Target Start            - " << downsampleTargetStart << std::endl;
        std::cout << "Downsample Float End               - " << downsampleFloatEnd << std::endl;
        std::cout << "Downsample Target End              - " << downsampleTargetEnd << std::endl;
        std::cout << "Symmetric Correspondences          - " << correspondencesSymmetric << std::endl;
        std::cout << "Num Neighbours                     - " << correspondencesNumNeighbours << std::endl;
        std::cout << "Inlier Kappa                       - " << inlierKappa << std::endl;
        std::cout << "Transform Sigma                    - " << transformSigma << std::endl;
        std::cout << "transformNumViscousIterationsStart - " << transformNumViscousIterationsStart << std::endl;
        std::cout << "transformNumViscousIterationsEnd   - " << transformNumViscousIterationsEnd << std::endl;
        std::cout << "transformNumElasticIterationsStart - " << transformNumElasticIterationsStart << std::endl;
        std::cout << "transformNumElasticIterationsEnd   - " << transformNumElasticIterationsEnd << std::endl;
        //END DEBUG
//
//
    //# Nonrigid registration
    //DEBUG
    //## Initialize variables
//    floatingFeatures = FeatureMat::Zero(3,6);
//    floatingFeatures << 1.0, 1.0, 1.0, 1.0, 0.0, 0.0,
//                        1.0, 1.0, 0.0, 1.0, 0.0, 0.0,
//                        1.0, 0.0, 0.0, 1.0, 0.0, 0.0;
//    targetFeatures = FeatureMat::Zero(3,6);
//    targetFeatures << 1.1, 1.0, 1.0, 1.0, 0.0, 0.0,
//                      0.9, 1.0, 0.0, 1.0, 0.0, 0.0,
//                      1.2, 0.0, 0.0, 1.0, 0.0, 0.0;
//    floatingFaces = FacesMat::Zero(1,3);
//    floatingFaces << 0, 1, 2;
//    targetFaces = FacesMat::Zero(1,3);
//    targetFaces << 0, 1, 2;
//    floatingFlags = VecDynFloat::Ones(3);
//    targetFlags = VecDynFloat::Ones(3);
//
//
//    std::cout << "floatingFeatures: " << floatingFeatures << std::endl;
//    std::cout << "targetFeatures: " << targetFeatures << std::endl;
//    std::cout << "floatingFaces: " << floatingFaces << std::endl;
//    std::cout << "targetFaces: " << targetFaces << std::endl;
//    std::cout << "floatingFlags: " << floatingFlags << std::endl;
//    std::cout << "targetFlags: " << targetFlags << std::endl;
//
//    //## Convert to arrays
//    float floatingFeaturesArray[18];
//    Eigen::Map<FeatureMat>(floatingFeaturesArray, floatingFeatures.rows(), floatingFeatures.cols()) = floatingFeatures;
//    float targetFeaturesArray[18];
//    Eigen::Map<FeatureMat>(targetFeaturesArray, targetFeatures.rows(), targetFeatures.cols()) = targetFeatures;
//
//    int floatingFacesArray[18];
//    Eigen::Map<FacesMat>(floatingFacesArray, floatingFaces.rows(), floatingFaces.cols()) = floatingFaces;
//    int targetFacesArray[18];
//    Eigen::Map<FacesMat>(targetFacesArray, targetFaces.rows(), targetFaces.cols()) = targetFaces;
//
//    float floatingFlagsArray[18];
//    Eigen::Map<VecDynFloat>(floatingFlagsArray, floatingFlags.rows(), floatingFlags.cols()) = floatingFlags;
//    float targetFlagsArray[18];
//    Eigen::Map<VecDynFloat>(targetFlagsArray, targetFlags.rows(), targetFlags.cols()) = targetFlags;
//
//
//
//    std::cout << "floatingFeaturesArray: " << std::endl;
//    for (int i = 0 ; i < 3 ; i++){
//        for (int j = 0 ; j < 6 ; j++) {
//            std::cout << floatingFeaturesArray[6*i + j] << " | ";
//        }
//        std::cout << std::endl;
//    }
//
//    std::cout << "targetFeaturesArray: " << targetFeaturesArray << std::endl;
//    std::cout << "floatingFacesArray: " << floatingFacesArray << std::endl;
//    std::cout << "targetFacesArray: " << targetFacesArray << std::endl;
//    std::cout << "floatingFlagsArray: " << floatingFlagsArray << std::endl;
//    std::cout << "targetFlagsArray: " << targetFlagsArray << std::endl;
    //END DEBUG

    meshmonk::pyramid_registration(floatingFeatures, targetFeatures,
                                floatingFaces, targetFaces,
                                floatingFlags, targetFlags,
                                numIterations, numPyramidLayers,
                                downsampleFloatStart, downsampleTargetStart,
                                downsampleFloatEnd, downsampleTargetEnd,
                                correspondencesSymmetric, correspondencesNumNeighbours,
                                inlierKappa, inlierUseOrientation,
                                transformSigma,
                                transformNumViscousIterationsStart, transformNumViscousIterationsEnd,
                                transformNumElasticIterationsStart, transformNumElasticIterationsEnd);
//    meshmonk::nonrigid_registration_mex(floatingFeaturesArray, targetFeaturesArray,
//                                3, 3,
//                                floatingFacesArray, targetFacesArray,
//                                1, 1,
//                                floatingFlagsArray, targetFlagsArray,
//                                numIterations,
//                                correspondencesSymmetric, correspondencesNumNeighbours,
//                                inlierKappa, inlierUseOrientation,
//                                transformSigma,
//                                transformNumViscousIterationsStart, transformNumViscousIterationsEnd,
//                                transformNumElasticIterationsStart, transformNumElasticIterationsEnd);

//    /*
//    ############################################################################
//    ##############################  OUTPUT #####################################
//    ############################################################################
//    */
//    //# Write result to file
//    std::cout << "Writing results to file..." << std::endl;
//    //DEBUG
//    std::cout << "Num Floating Elements              - " << floatingFeatures.rows() << std::endl;
//    std::cout << "Num Target Elements                - " << targetFeatures.rows() << std::endl;
//    std::cout << "Num Floating Faces                 - " << floatingFaces.rows() << std::endl;
//    std::cout << "Num Target Faces                   - " << targetFaces.rows() << std::endl;
//    //END DEBUG
//    meshmonk::write_obj_files(floatingFeatures,floatingFaces, resultDir);
//    std::cout << "Process finished." << std::endl;

    return 0;
}


#include "stdafx.h"
#include "Graph.h"
#include <sstream>
#include <boost/algorithm/string.hpp>

using std::vector;
using std::string;
using std::stringstream;

namespace
{

const char C_INPUT_1[] = R"***(
5 5 1
1 2 2
2 3 3
2 4 -3
3 1 -4
4 5 2
)***";

const char C_OUTPUT_1[] = R"***(
0 0
2 1 2
5 2 2 3
-1 2 2 4
1 3 2 4 5
)***";

const char C_INPUT_2[] = R"***(
4 7 1
1 2 3
1 3 7
2 3 2
2 4 1
3 2 2
3 4 2
4 3 2
)***";

const char C_OUTPUT_2[] = R"***(
0 0
3 1 2
5 2 2 3
4 2 2 4
)***";

const char C_INPUT_3[] = R"***(
4 2 1
1 2 3
3 4 4
)***";

const char C_OUTPUT_3[] = R"***(
0 0
3 1 2
No
No
)***";

const char C_INPUT_4[] = R"***(
4 4 1
1 2 2
2 3 4
3 1 -9
2 4 5
)***";

const char C_OUTPUT_4[] = R"***(
No
4 2 3 1 2
)***";

// Используем split для игнорирования пробелов и переносов строк при проверке.
vector<string> SplitWords(string const& text)
{
	std::string trimmed = boost::trim_copy(text);

	vector<string> words;
	boost::split(words, trimmed, boost::is_space(), boost::token_compress_on);
	return words;
}

}

BOOST_AUTO_TEST_SUITE(Graph_with_Bellmand_Ford_algorithm)
	BOOST_AUTO_TEST_CASE(finds_correct_shortest_paths)
	{
		CGraph graph;
		stringstream input(C_INPUT_1);
		stringstream output;
		BOOST_CHECK(graph.ReadText(input));
		graph.RunBellmanFord(output);
		vector<string> words = SplitWords(output.str());
		vector<string> checkWords = SplitWords(C_OUTPUT_1);
		BOOST_CHECK(words == checkWords);
	}

	BOOST_AUTO_TEST_CASE(has_no_negative_loop_false_positives)
	{
		CGraph graph;
		stringstream input(C_INPUT_2);
		stringstream output;
		BOOST_CHECK(graph.ReadText(input));
		graph.RunBellmanFord(output);
		vector<string> words = SplitWords(output.str());
		vector<string> checkWords = SplitWords(C_OUTPUT_2);
		BOOST_CHECK(words == checkWords);
	}

	BOOST_AUTO_TEST_CASE(prints_No_if_no_path)
	{
		CGraph graph;
		stringstream input(C_INPUT_3);
		stringstream output;
		BOOST_CHECK(graph.ReadText(input));
		graph.RunBellmanFord(output);
		vector<string> words = SplitWords(output.str());
		vector<string> checkWords = SplitWords(C_OUTPUT_3);
		BOOST_CHECK(words == checkWords);
	}

	BOOST_AUTO_TEST_CASE(finds_negative_loop_and_prints_it)
	{
		CGraph graph;
		stringstream input(C_INPUT_4);
		stringstream output;
		BOOST_CHECK(graph.ReadText(input));
		graph.RunBellmanFord(output);
		vector<string> words = SplitWords(output.str());
		vector<string> checkWords = SplitWords(C_OUTPUT_4);
		BOOST_CHECK(words == checkWords);
	}
BOOST_AUTO_TEST_SUITE_END()


#ifndef INPUT_OUTPUT_HPP
#define INPUT_OUTPUT_HPP

#include <iostream>
#include <fstream>
#include <vector>
#include <memory>

#include <Eigen/Dense>
#include "dist/json/json.h"
#include "options.hpp"

using namespace Eigen;

class InputOutput{

public:

	std::string config_name;

	//constructor
	InputOutput(std::string cfg): config_name(cfg){}

	//destructor
	~InputOutput(){}

	//write to file
	//writes data in columns with given headers
	void WriteResults(const std::string& filename,const std::vector<std::string>& headers,
	const MatrixXf& data){
		std::ofstream myfile;
		std::string filetype = ".txt";

		myfile.open(filename+filetype);
		for(auto it = headers.begin(); it!=headers.end(); it++){
			myfile << *it<< ",";
		}
		myfile << std::endl;
		for(int i = 0; i < data.rows(); i++){
			for(int j =0; j < data.cols(); j++){
				myfile << data(i,j) << ", ";
			}
			myfile << std::endl;

		}
		myfile.close();
	}

	//read configuration given configuration object
	Options ReadConfig(){

		Options opt;

		std::ifstream ifs(config_name);
		Json::Reader reader;
		Json::Value obj;
		reader.parse(ifs, obj); // reader can also read strings

		//the save file name
		std::string save_name = obj["save_file"].asString();
		float U = obj["U"].asFloat();
		float t = obj["t"].asFloat();
		float k_min = obj["k_min"].asFloat();
		float k_max = obj["k_max"].asFloat();
		int n_T = obj["n_T"].asInt();
		opt.mu_min = obj["mu_min"].asFloat();
		opt.mu_max = obj["mu_max"].asFloat();
		opt.n_mu = obj["n_mu"].asInt();
		opt.plotting = obj["plotting"].asInt();
		opt.max_iter = obj["max_iter"].asInt();
		opt.xDelta = obj["xDelta"].asFloat();
		opt.fDelta = obj["fDelta"].asFloat();
		opt.condition = obj["condition"].asFloat();
		opt.delta0 = obj["delta0"].asFloat();

		//saving the basis vectors from config
		Json::Value& basis = obj["basis_vectors"];

		int dim = basis.size();

		MatrixXf basis_vectors(dim,dim );
		for (int i = 0; i < basis.size(); i++) {
			float x = basis[i]["x"].asFloat();
			float y = basis[i]["y"].asFloat();
			basis_vectors.col(i) << x,y;
		}

		//saving t_matrix
		Json::Value& t_mat = obj["t_matrix"];
		int bands = t_mat.size();
		MatrixXf t_matrix(bands,bands );
		for (int i = 0; i < bands; i++) {
			for (int j = 0; j < bands; j++){
				t_matrix(i,j) = t_mat[i][j].asFloat();
			}
		}

		//saving R matrix
		Json::Value& R_mat = obj["r_matrix"];

		/* for lattices that have n different couplings between same lattice sites,
		  the different couplings are stored in r_matrix and all the different couplings
		  are stored in r_matrix_vector
		 */
		std::vector<std::shared_ptr<MatrixXf>> r_matrix_vector;

		MatrixXf r_matrix(bands*dim,bands );
		//i goes from 0 to bands
		int most_comps = 0;
		for (int i = 0; i < bands; i++) {
			//j goes from 0 to bands
			for (int j = 0; j < bands; j++){

				Json::Value& components = R_mat[i][j];
				for(int n=0; n < components.size(); n++){

					//the length of r_matrix_vector is increased when we notice more couplings exist
					//initialize it to zeros
					if( n == most_comps) {
						std::shared_ptr<MatrixXf> point (new MatrixXf(bands*dim,bands ));
						*point = MatrixXf::Zero(bands*dim,bands);
						r_matrix_vector.push_back(point);
						most_comps += 1;
					}

					//r_matrix = MatrixXf::Zero(bands*dim,bands);
					MatrixXf vec = MatrixXf::Zero(dim,1);

					for(int m=0; m < components[n].size(); m++){
						float factor = components[n][m]["factor"].asFloat();
						vec += factor*basis_vectors.col( components[n][m]["b_vec"].asInt()-1 ) ;
					}
					(*r_matrix_vector[n]).block(i*dim,j,dim,1) << vec;

					//r_matrix.block(i*dim,j,dim,1) << vec;
					//std::shared_ptr<MatrixXf> point (new MatrixXf(bands*dim,bands ));
					//*point = r_matrix;
					//r_matrix_vector.push_back(point);
				}
			}
		}
		std::cout << "r_matriisit, koko: " << r_matrix_vector.size() << std::endl;
		for (int i = 0; i < r_matrix_vector.size(); i++){
			std::cout << *r_matrix_vector[i] << std::endl;
		}

		//saving read values to option object
		opt.save_file = save_name;
		opt.U = U;
		opt.t = t;
		opt.dim = dim;
		opt.bands = bands;
		opt.k_min = k_min;
		opt.k_max = k_max;
		opt.n_T = n_T;

		opt.basis = basis_vectors;
		opt.R = r_matrix_vector;
		opt.t_matrix = t_matrix;

		return opt;

		//std::cout << "basis\n" << basis_vectors << std::endl;
		//std::cout << "t_matrix\n" << t_matrix << std::endl;
		//std::cout << "R matrix\n" << r_matrix << std::endl;
	}

};

#endif


/*
Copyright (c) 2017 Theodoros Chondrogiannis
*/

#ifndef GRAPH_HPP
#define GRAPH_HPP

#include <iostream>
#include <fstream>
#include <queue>
#include <unordered_set>
#include <unordered_map>

#include <boost/functional/hash.hpp>

using namespace std;

typedef unsigned int NodeID;
typedef pair<NodeID,NodeID> Edge;

typedef unordered_map<NodeID,int> EdgeList;

class RoadNetwork {
public:
    unsigned int numNodes;
    unsigned int numEdges;
   	vector<EdgeList> adjListOut;
   	vector<EdgeList> adjListInc;

    RoadNetwork(const char *filename);
    int getEdgeWeight(NodeID lnode, NodeID rnode);
    void print();
    RoadNetwork(){};
    ~RoadNetwork();

    EdgeList ougoingEdgesOf(NodeID);
    EdgeList incomingEdgesOf(NodeID);
};

// This is to ensure that edges are considered in a bidirectional fashion for the computation of the overlap.
bool operator==(const Edge& le, const Edge& re);

class Path {
public:
	vector<NodeID> nodes;
	unordered_set<Edge,boost::hash<Edge>> edges;
	int length;

	Path() {
		length = -1;
	}

	bool containsEdge(Edge e);
	double overlap_ratio(RoadNetwork *rN, Path &path2);
};

bool operator==(const Path& lp, const Path& rp);

#endif


#pragma once
///////////////////////////////////////////////////////////////////////////////
// calculator_grammar.hpp
#ifndef CALCULATOR_GRAMMAR_HPP__
#define CALCULATOR_GRAMMAR_HPP__

#include <stdexcept>
#include <ostream>

#pragma warning(push,3)

#define BOOST_SPIRIT_USE_PHOENIX_V3
//#define BOOST_RESULT_OF_USE_DECLTYPE

#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_statement.hpp>
#include <boost/spirit/include/phoenix_bind.hpp>
#include <boost/phoenix/object/construct.hpp>
#include <boost/fusion/include/std_pair.hpp>

#pragma warning(pop)

#include "symbol_table.hpp"
namespace qi = boost::spirit::qi;
namespace phx = boost::phoenix;

template <typename Iterator>
struct calculator_interpreter
    : qi::grammar<Iterator, int(), qi::space_type>
{
    explicit
    calculator_interpreter(std::ostream& errors_stream)
        : calculator_interpreter::base_type(expr, "expr")
        , serr(errors_stream)
    {
        using namespace qi;
        using qi::_1;
        using qi::_2;
        using qi::_3;
        using qi::_4;
        using qi::_val;

        expr %= add | sub | mult | div | let
            ;

        add = lit("add")   > '(' > factor[_val = _1] > ',' > factor[_val += _1] > ')'
            ;

        sub = lit("sub")   > '(' > factor[_val = _1] > ',' > factor[_val -= _1] > ')'
            ;

        mult = lit("mult") > '(' > factor[_val = _1] > ',' > factor[_val *= _1] > ')'
            ;

        div = lit("div")   > '(' > factor[_val = _1] > ',' > factor[
                _val = phx::if_else(
                    _1,
                    _val /= _1,
                    (phx::throw_(std::runtime_error("division by zero")), _1)
                )]
            > ')'
            ;

        let =
            lit("let")     > '('
            > var_declaration[ phx::bind(&symbols_table::push, phx::ref(stable), _1) ]
            > ',' > expr[ _val = _1, phx::bind(&symbols_table::pop, phx::ref(stable)) ]
            > ')'
        ;

        var_declaration = (+alpha > ',' > factor)
            ;

        variable =
            qi::as_string[+alpha][_pass = phx::bind(&symbols_table::lookup, phx::ref(stable), _1, _val)]
        ;

        factor %=
            qi::int_
            | expr
            | variable
        ;

        /// Errors handlign

        expr.name("expr");
        add.name("add");
        sub.name("sub");
        mult.name("mult");
        div.name("div");
        let.name("let");
        factor.name("factor");
        var_declaration.name("var_declaration");
        variable.name("variable");

        on_error<fail>
            (
                expr
                , serr
                << phx::val("Error! Expecting ")
                << _4                               // what failed?
                << phx::val(" here: \"")
                << phx::construct<std::string>(_3, _2)   // iterators to error-pos, end
                << phx::val("\"")
                << std::endl
            );
    }

    qi::rule<Iterator, int(), qi::space_type>
        expr, add, sub, mult, div, let
        , factor, variable;

    qi::rule<Iterator, std::pair<std::string, int>(), qi::space_type>
        var_declaration;

    symbols_table stable;

    std::ostream& serr;

};

#endif


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __TTT_MPL_NAMING_HPP__
#define __TTT_MPL_NAMING_HPP__

#include <libv/lma/global.hpp>
#include "../traits/naming.hpp"
#include <boost/mpl/is_sequence.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/deref.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/int.hpp>
#include <boost/utility/enable_if.hpp>

namespace ttt
{
  template<class A, class B> struct DispTypeVector
  {
    static std::string name()
    {
      static std::string delimitor = boost::is_same<typename boost::mpl::next<A>::type,B>::value ? "" : ",";
      return ttt::name<typename boost::mpl::deref<A>::type>() + delimitor + DispTypeVector<typename boost::mpl::next<A>::type,B>::name();
    }
  };

  template<class A> struct DispTypeVector<A,A>
  {
    static std::string name()
    {
      return ttt::name<typename boost::mpl::deref<A>::type>();
    }
  };

  template<class Vector> struct Name<Vector,typename boost::enable_if<boost::mpl::is_sequence<Vector>>::type>
  {
    static std::string name() { return color.green() + "mpl::vector<" + color.reset() + DispTypeVector<typename boost::mpl::begin<Vector>::type, typename boost::mpl::end<Vector>::type>::name() + color.green() + ">" + color.reset(); }
  };

  template<> struct Name<boost::mpl::void_>
  {
    static std::string name() { return ""; }
  };
  template<int I> struct Name<mpl::int_<I>>
  {
    static std::string name() { return std::string("Int<") + lma::to_string(I) + ">";}
  };

  template<> struct Name<mpl_::na> { static std::string name(){return "mpl::na";}};
}

#endif


#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <iostream>
#include <random>  // Requires C++ 11

#include <Spectra/GenEigsSolver.h>
#include <Spectra/MatOp/DenseGenMatProd.h>
#include <Spectra/MatOp/SparseGenMatProd.h>

using namespace Spectra;

#define CATCH_CONFIG_MAIN
#include "catch.hpp"

typedef Eigen::MatrixXd Matrix;
typedef Eigen::VectorXd Vector;
typedef Eigen::MatrixXcd ComplexMatrix;
typedef Eigen::VectorXcd ComplexVector;
typedef Eigen::SparseMatrix<double> SpMatrix;

// Traits to obtain operation type from matrix type
template <typename MatType>
struct OpTypeTrait
{
    typedef DenseGenMatProd<double> OpType;
};

template <>
struct OpTypeTrait<SpMatrix>
{
    typedef SparseGenMatProd<double> OpType;
};

// Generate random sparse matrix
SpMatrix gen_sparse_data(int n, double prob = 0.5)
{
    SpMatrix mat(n, n);
    std::default_random_engine gen;
    gen.seed(0);
    std::uniform_real_distribution<double> distr(0.0, 1.0);
    for (int i = 0; i < n; i++)
    {
        for (int j = 0; j < n; j++)
        {
            if (distr(gen) < prob)
                mat.insert(i, j) = distr(gen) - 0.5;
        }
    }
    return mat;
}

template <typename MatType, int SelectionRule>
void run_test(const MatType& mat, int k, int m, bool allow_fail = false)
{
    typename OpTypeTrait<MatType>::OpType op(mat);
    GenEigsSolver<double, SelectionRule, typename OpTypeTrait<MatType>::OpType>
        eigs(&op, k, m);
    eigs.init();
    // maxit = 500 to reduce running time for failed cases
    int nconv = eigs.compute(500);
    int niter = eigs.num_iterations();
    int nops = eigs.num_operations();

    if (allow_fail)
    {
        if (eigs.info() != SUCCESSFUL)
        {
            WARN("FAILED on this test");
            std::cout << "nconv = " << nconv << std::endl;
            std::cout << "niter = " << niter << std::endl;
            std::cout << "nops  = " << nops << std::endl;
            return;
        }
    }
    else
    {
        INFO("nconv = " << nconv);
        INFO("niter = " << niter);
        INFO("nops  = " << nops);
        REQUIRE(eigs.info() == SUCCESSFUL);
    }

    ComplexVector evals = eigs.eigenvalues();
    ComplexMatrix evecs = eigs.eigenvectors();

    ComplexMatrix resid = mat * evecs - evecs * evals.asDiagonal();
    const double err = resid.array().abs().maxCoeff();

    INFO("||AU - UD||_inf = " << err);
    REQUIRE(err == Approx(0.0).margin(1e-9));
}

template <typename MatType>
void run_test_sets(const MatType& A, int k, int m)
{
    SECTION("Largest Magnitude")
    {
        run_test<MatType, LARGEST_MAGN>(A, k, m);
    }
    SECTION("Largest Real Part")
    {
        run_test<MatType, LARGEST_REAL>(A, k, m);
    }
    SECTION("Largest Imaginary Part")
    {
        run_test<MatType, LARGEST_IMAG>(A, k, m);
    }
    SECTION("Smallest Magnitude")
    {
        run_test<MatType, SMALLEST_MAGN>(A, k, m, true);
    }
    SECTION("Smallest Real Part")
    {
        run_test<MatType, SMALLEST_REAL>(A, k, m);
    }
    SECTION("Smallest Imaginary Part")
    {
        run_test<MatType, SMALLEST_IMAG>(A, k, m, true);
    }
}

TEST_CASE("Eigensolver of general real matrix [10x10]", "[eigs_gen]")
{
    std::srand(123);

    const Matrix A = Eigen::MatrixXd::Random(10, 10);
    int k = 3;
    int m = 6;

    run_test_sets(A, k, m);
}

TEST_CASE("Eigensolver of general real matrix [100x100]", "[eigs_gen]")
{
    std::srand(123);

    const Matrix A = Eigen::MatrixXd::Random(100, 100);
    int k = 10;
    int m = 20;

    run_test_sets(A, k, m);
}

TEST_CASE("Eigensolver of general real matrix [1000x1000]", "[eigs_gen]")
{
    std::srand(123);

    const Matrix A = Eigen::MatrixXd::Random(1000, 1000);
    int k = 20;
    int m = 50;

    run_test_sets(A, k, m);
}

TEST_CASE("Eigensolver of sparse real matrix [10x10]", "[eigs_gen]")
{
    std::srand(123);

    const SpMatrix A = gen_sparse_data(10, 0.5);
    int k = 3;
    int m = 6;

    run_test_sets(A, k, m);
}

TEST_CASE("Eigensolver of sparse real matrix [100x100]", "[eigs_gen]")
{
    std::srand(123);

    const SpMatrix A = gen_sparse_data(100, 0.5);
    int k = 10;
    int m = 20;

    run_test_sets(A, k, m);
}

TEST_CASE("Eigensolver of sparse real matrix [1000x1000]", "[eigs_gen]")
{
    std::srand(123);

    const SpMatrix A = gen_sparse_data(1000, 0.5);
    int k = 20;
    int m = 50;

    run_test_sets(A, k, m);
}


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// 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 University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  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 OWNER 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.

/// @author vsevolod vlaskine

#include <algorithm>
#include <cmath>
#include <deque>
#include <functional>
#include <iostream>
#include <vector>
#include <boost/scoped_ptr.hpp>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <comma/application/command_line_options.h>
#include <comma/base/types.h>
#include <comma/csv/stream.h>
#include <comma/string/string.h>
#include "../../visiting/traits.h"
#include "../rotation_matrix.h"

void usage( bool verbose )
{
    std::cerr << std::endl;
    std::cerr << "simple wrapper for eigen library operations" << std::endl;
    std::cerr << std::endl;
    std::cerr << "usage: cat sample.csv | math-eigen <operation> [<options>]" << std::endl;
    std::cerr << std::endl;
    std::cerr << "<operation>: eigen, rotation" << std::endl;
    std::cerr << std::endl;
    std::cerr << "operations" << std::endl;
    std::cerr << "    eigen (default): calculate eigen vector and eigen values on a sample" << std::endl;
    std::cerr << "        fields" << std::endl;
    std::cerr << "            block: block number; output eigen vectors and eigen values for each" << std::endl;
    std::cerr << "                   contiguous block of samples with the same block id" << std::endl;
    std::cerr << "            data: sample data" << std::endl;
    std::cerr << "            default: data" << std::endl;
    std::cerr << "        output" << std::endl;
    std::cerr << "            default output fields" << std::endl;
    std::cerr << "                one eigen vector per line; if block field present: vector,value,block" << std::endl;
    std::cerr << "                                           if no block field present: vector,value" << std::endl;
    std::cerr << "            binary format: 32-bit unsigned integer for block, doubles for other output fields" << std::endl;
    std::cerr << "        options" << std::endl;
    std::cerr << "            --normalize,-n: output normalized eigen values" << std::endl;
    std::cerr << "            --rsort,--descending: output eigen vectors and values in descending order of eigen values" << std::endl;
    std::cerr << "            --sort,-s,--ascending: output eigen vectors and values in ascending order of eigen values" << std::endl;
    std::cerr << "            --single-line-output,--single-line,--single: output eigen vectors and eigen values all as one line:" << std::endl;
    std::cerr << "                                                         vector[0],vector[1],...,value[0],value[1],...,block" << std::endl;
    std::cerr << "            --size: a hint of number of elements in the data vector, ignored, if data indices" << std::endl;
    std::cerr << "                    specified, e.g. data[0],data[1],data[2]" << std::endl;
    std::cerr << std::endl;
    std::cerr << "    fit plane: fit plane to dataset, output its mean and normal" << std::endl;
    std::cerr << "        fields" << std::endl;
    std::cerr << "            block: block number; output eigen vectors and eigen values for each" << std::endl;
    std::cerr << "                   contiguous block of samples with the same block id" << std::endl;
    std::cerr << "            data: sample data" << std::endl;
    std::cerr << "            default: data" << std::endl;
    std::cerr << "        output" << std::endl;
    std::cerr << "            output fields: mean,normal[,block]" << std::endl;
    std::cerr << "            binary format: 32-bit unsigned integer for block, doubles for other output fields" << std::endl;
    std::cerr << "        options" << std::endl;
    std::cerr << "            --size: a hint of number of elements in the data vector, ignored, if data indices" << std::endl;
    std::cerr << "                    specified, e.g. data[0],data[1],data[2]" << std::endl;
    std::cerr << std::endl;
    std::cerr << "    rotation: convert rotation from representation to another, append the result to stdin data, output to stdout" << std::endl;
    std::cerr << "        options" << std::endl;
    std::cerr << "            --from=<what>: input representation; default euler" << std::endl;
    std::cerr << "            --to=<what>: output representation; default euler" << std::endl;
    std::cerr << "                <what>" << std::endl;
    std::cerr << "                    euler;rpy;roll,pitch,yaw: euler angles, i.e. roll, pitch, yaw" << std::endl;
    std::cerr << "                    axis-angle,angle-axis: angle-axis" << std::endl;
    std::cerr << "                    axis-angle-scaled: rotation axis with the norm equal to rotation angle" << std::endl;
    std::cerr << "                    quaternion: quaternion" << std::endl;
    std::cerr << "                    rotation-matrix,matrix: rotation matrix" << std::endl;
    std::cerr << "            --input-fields: print input fields to stdout and exit" << std::endl;
    std::cerr << "            --output-fields: print output fields to stdout and exit" << std::endl;
    std::cerr << "            --output-format: print binary output format to stdout and exit" << std::endl;
    std::cerr << std::endl;
    std::cerr << "options" << std::endl;
    std::cerr << "    --help,-h: show this help; --help --verbose: more help" << std::endl;
    if( verbose ) { std::cerr << std::endl << "csv options" << std::endl << comma::csv::options::usage() << std::endl; }
    std::cerr << std::endl;
    std::cerr << "examples" << std::endl;
    std::cerr << "    eigen" << std::endl;
    std::cerr << "        cat sample.csv | math-eigen" << std::endl;
    std::cerr << "        cat sample.csv | math-eigen --fields=block,data" << std::endl;
    std::cerr << "        cat sample.csv | math-eigen --fields=block,,,data --size=4" << std::endl;
    std::cerr << "        cat sample.csv | math-eigen --fields=block,,data[0],,data[1],,data[2],,data[3]" << std::endl;
    std::cerr << "        cat sample.bin | math-eigen --fields=block,data --binary=ui,6d" << std::endl;
    std::cerr << std::endl;
    exit( 0 );
}

static boost::optional< unsigned int > size;

namespace snark { namespace eigen {

struct input_t
{
    std::vector< double > data;
    comma::uint32 block;

    input_t() : data( *size ), block( 0 ) {}
};

struct output_t
{
    std::vector< double > vector;
    double value;
    comma::uint32 block;

    output_t() : vector( *size ), value( 0 ), block( 0 ) {}
};

struct single_line_output_t
{
    std::vector< double > vectors; // quick and dirty
    std::vector< double > values;
    comma::uint32 block;

    single_line_output_t(): vectors( *size * *size ), values( *size ), block( 0 ) {}
};

struct fit_plane_output_t
{
    std::vector< double > mean;
    std::vector< double > normal;
    comma::uint32 block;

    fit_plane_output_t(): mean( *size ), normal( *size ), block( 0 ) {}
};

} } // namespace snark { namespace eigen {

namespace comma { namespace visiting {

template <> struct traits< snark::eigen::input_t >
{
    template < typename K, typename V > static void visit( const K&, snark::eigen::input_t& p, V& v )
    {
        v.apply( "data", p.data );
        v.apply( "block", p.block );
    }

    template < typename K, typename V > static void visit( const K&, const snark::eigen::input_t& p, V& v )
    {
        v.apply( "data", p.data );
        v.apply( "block", p.block );
    }
};

template <> struct traits< snark::eigen::output_t >
{
    template < typename K, typename V > static void visit( const K&, const snark::eigen::output_t& p, V& v )
    {
        v.apply( "vector", p.vector );
        v.apply( "value", p.value );
        v.apply( "block", p.block );
    }
};

template <> struct traits< snark::eigen::single_line_output_t >
{
    template < typename K, typename V > static void visit( const K&, const snark::eigen::single_line_output_t& p, V& v )
    {
        v.apply( "vectors", p.vectors );
        v.apply( "values", p.values );
        v.apply( "block", p.block );
    }
};

template <> struct traits< snark::eigen::fit_plane_output_t >
{
    template < typename K, typename V > static void visit( const K&, const snark::eigen::fit_plane_output_t& p, V& v )
    {
        v.apply( "mean", p.mean );
        v.apply( "normal", p.normal );
        v.apply( "block", p.block );
    }
};

} } // namespace comma { namespace visiting {

namespace rotation {

template < typename T > struct traits;

template <> struct traits< snark::roll_pitch_yaw >
{
    static snark::roll_pitch_yaw zero() { return snark::roll_pitch_yaw(); }
    static snark::roll_pitch_yaw get( const snark::rotation_matrix& m ) { return m.roll_pitch_yaw(); }
};

template < typename T > struct traits< Eigen::AngleAxis< T > >
{
    static Eigen::AngleAxis< T > zero() { Eigen::AngleAxis< T > a( 0, Eigen::Matrix< T, 3, 1 >::Zero() ); return a; }
    static Eigen::AngleAxis< T > get( const snark::rotation_matrix& m ) { return Eigen::AngleAxis< T >( m.rotation() ); }
};

template <> struct traits< Eigen::Vector3d >
{
    static Eigen::Vector3d zero() { return Eigen::Vector3d::Zero(); }
    static Eigen::Vector3d get( const snark::rotation_matrix& m ) { const Eigen::AngleAxis< double > a( m.rotation() ); return a.axis() * a.angle(); }
};

template < typename T > struct traits< Eigen::Quaternion< T > >
{
    static Eigen::Quaternion< T > zero() { Eigen::Quaternion< T > q( 0, 0, 0, 0 ); return q; }
    static Eigen::Quaternion< T > get( const snark::rotation_matrix& m ) { return m.quaternion(); }
};

template <> struct traits< Eigen::Matrix< double, 3, 3 > >
{
    static Eigen::Matrix< double, 3, 3 > zero() { return Eigen::Matrix< double, 3, 3 >::Zero(); }
    static Eigen::Matrix< double, 3, 3 > get( const snark::rotation_matrix& m ) { return Eigen::Matrix< double, 3, 3 >( m.rotation() ); }
};

template < typename From, typename To >
static int run( const comma::command_line_options& options )
{
    if( options.exists( "--input-fields" ) ) { std::cout << comma::join( comma::csv::names< From >( false ), ',' ) << std::endl; return 0; }
    if( options.exists( "--output-fields" ) ) { std::cout << comma::join( comma::csv::names< To >( false ), ',' ) << std::endl; return 0; }
    if( options.exists( "--output-format" ) ) { std::cout << comma::csv::format::value< To >() << std::endl; return 0; }
    comma::csv::options csv( options );
    csv.full_xpath = true;
    comma::csv::input_stream< From > istream( std::cin, csv, rotation::traits< From >::zero() );
    comma::csv::options output_csv;
    if( csv.binary() ) { output_csv.format( comma::csv::format::value< To >() ); }
    output_csv.full_xpath = true;
    comma::csv::output_stream< To > ostream( std::cout, output_csv );
    while( istream.ready() || std::cin.good() )
    {
        const From* p = istream.read();
        if( !p ) { break; }
        comma::csv::append( istream, ostream, rotation::traits< To >::get( snark::rotation_matrix( *p ) ) );
        if( csv.flush ) { ostream.flush(); }
    }
    return 0;
}

template < typename From >
static int run( const comma::command_line_options& options, const std::string& to )
{
    if( to == "euler" || to == "rpy" || to == "roll,pitch,yaw" ) { return rotation::run< From, snark::roll_pitch_yaw >( options ); }
    if( to == "angle-axis" || to == "axis-angle" ) { return rotation::run< From, Eigen::AngleAxis< double > >( options ); }
    if( to == "axis-angle-scaled" ) { return rotation::run< From, Eigen::Vector3d >( options ); }
    if( to == "quaternion" ) { return rotation::run< From, Eigen::Quaternion< double > >( options ); }
    if( to == "rotation-matrix" || to == "matrix" ) { return rotation::run< From, Eigen::Matrix< double, 3, 3 > >( options ); }
    std::cerr << "math-eigen: rotation: expected valid value for --to; got --to=\"" << to << "\"" << std::endl;
    return 1;
}

} // namespace rotation {

int main( int ac, char** av )
{
    try
    {
        comma::command_line_options options( ac, av, usage );
        const std::vector< std::string >& unnamed = options.unnamed( "--flush,--normalize,-n,--sort,-s,--ascending,--rsort,--descending,--single-line-output,--single-line,--single,--verbose,-v" );
        std::string operation = unnamed.empty() ? std::string( "eigen" ) : unnamed[0];
        if( operation == "eigen" || operation == "fit-plane" )
        {
            comma::csv::options csv( options );
            size = options.optional< unsigned int >( "--size" );
            if( csv.fields.empty() ) { csv.fields = "data"; }
            std::string first;
            if( !size )
            {
                const std::vector< std::string >& fields = comma::split( csv.fields, ',' );
                if( csv.has_field( "data" ) )
                {
                    unsigned int count;
                    if( csv.binary() )
                    {
                        count = csv.format().count();
                    }
                    else
                    {
                        while( std::cin.good() && first.empty() ) { std::getline( std::cin, first ); }
                        count = comma::split( first, csv.delimiter ).size(); // quick and dirty, wasteful
                    }
                    size = count - fields.size() + 1;
                }
                else
                {
                    unsigned int max = 0;
                    for( unsigned int i = 0; i < fields.size(); ++i )
                    {
                        if( fields[i].substr( 0, 5 ) == "data[" && *fields[i].rbegin() == ']' ) { unsigned int k = boost::lexical_cast< unsigned int >( fields[i].substr( 5, fields[i].size() - 6 ) ) + 1; if( k > max ) { max = k; } }
                    }
                    if( max == 0 ) { std::cerr << "math-eigen: please specify valid data fields" << std::endl; return 1; }
                    size = max;
                }
            }

            bool has_block = csv.has_field( "block" );
            std::deque< snark::eigen::input_t > buffer;
            if( !first.empty() ) { buffer.push_back( comma::csv::ascii< snark::eigen::input_t >( csv ).get( first ) ); }
            comma::csv::input_stream< snark::eigen::input_t > istream( std::cin, csv );
            std::vector< unsigned int > indices( *size ); // quick and dirty
            for( unsigned int i = 0; i < indices.size(); indices[i] = i, ++i );
            if( operation == "eigen" )
            {
                bool single_line_output = options.exists( "--single-line-output,--single-line,--single" );
                bool normalize = options.exists( "--normalize,-n" );
                bool sort = options.exists( "--sort,--ascending,-s,--descending,--rsort" );
                bool ascending = sort && !options.exists( "--descending,--rsort" );
                comma::csv::options output_csv;
                output_csv.fields = single_line_output ? has_block ? "vectors,values,block" : "vectors,values"
                                                       : has_block ? "vector,value,block" : "vector,value";
                if( csv.binary() )
                {
                    std::string s = boost::lexical_cast< std::string >( single_line_output ? *size * ( *size + 1 ) : ( *size + 1 ) );
                    output_csv.format( has_block ? s + "d,ui" : s + "d" );
                }
                boost::scoped_ptr< comma::csv::output_stream< snark::eigen::output_t > > ostream;
                boost::scoped_ptr< comma::csv::output_stream< snark::eigen::single_line_output_t > > single_line_ostream;
                if( single_line_output ) { single_line_ostream.reset( new comma::csv::output_stream< snark::eigen::single_line_output_t >( std::cout, output_csv ) ); }
                else { ostream.reset( new comma::csv::output_stream< snark::eigen::output_t >( std::cout, output_csv ) ); }
                while( true )
                {
                    const snark::eigen::input_t* p = istream.read();
                    if( !p || ( !buffer.empty() && buffer.front().block != p->block ) )
                    {
                        typedef Eigen::Matrix< double, -1, -1, Eigen::RowMajor > matrix_t;
                        matrix_t sample( buffer.size(), *size );
                        for( std::size_t i = 0; i < buffer.size(); ++i ) { ::memcpy( &sample( i, 0 ), &buffer[i].data[0], *size * sizeof( double ) ); } // dodgy?
                        //if( buffer.size() == 1 ) { std::cerr << "math-eigen: on block " << buffer.front().block << ": expected block with at least two entries, got only one" << std::endl; return 1; }
                        matrix_t covariance = sample.adjoint() * sample;
                        covariance = covariance / ( sample.rows() - 1 );
                        Eigen::SelfAdjointEigenSolver< matrix_t > solver( covariance );
                        Eigen::VectorXd values = solver.eigenvalues();
                        if( normalize ) { values = values / solver.eigenvalues().sum(); }
                        const matrix_t& vectors = solver.eigenvectors().transpose();
                        if( sort )
                        {
                            std::map< double, unsigned int > m; // quick and dirty, watch performance
                            for( unsigned int i = 0; i < indices.size(); m[ ascending ? values[i] : -values[i] ] = i, ++i );
                            unsigned int i = 0;
                            for( std::map< double, unsigned int >::const_iterator it = m.begin(); it != m.end(); indices[i] = it->second, ++it, ++i );
                        }
                        if( single_line_output )
                        {
                            snark::eigen::single_line_output_t output;
                            output.block = buffer.front().block;
                            if( sort )
                            {
                                for( unsigned int i = 0; i < indices.size(); ++i )
                                {
                                    output.values[i] = values[ indices[i] ];
                                    ::memcpy( &output.vectors[ i * *size ], &vectors( indices[i], 0 ), *size * sizeof( double ) ); // quick and dirty
                                }
                            }
                            else
                            {
                                ::memcpy( &output.vectors[0], &vectors( 0, 0 ), *size * *size * sizeof( double ) ); // quick and dirty
                                ::memcpy( &output.values[0], &values[0], *size * sizeof( double ) );
                            }
                            single_line_ostream->write( output );
                            if( csv.flush ) { single_line_ostream->flush(); }
                        }
                        else
                        {
                            for( std::size_t i = 0; i < *size; ++i )
                            {
                                snark::eigen::output_t output;
                                output.block = buffer.front().block;
                                ::memcpy( &output.vector[0], &vectors( indices[i], 0 ), *size * sizeof( double ) );
                                output.value = values[ indices[i] ];
                                ostream->write( output );
                                if( csv.flush ) { ostream->flush(); }
                            }
                        }
                        buffer.clear();
                    }
                    if( !p ) { break; }
                    buffer.push_back( *p );
                }
                return 0;
            }
            if( operation == "fit-plane" ) // see http://math.stackexchange.com/questions/99299/best-fitting-plane-given-a-set-of-points
            {
                comma::csv::options output_csv;
                output_csv.fields = has_block ? "mean,normal,block" : "mean,normal";
                if( csv.binary() )
                {
                    std::string s = boost::lexical_cast< std::string >( *size );
                    output_csv.format( has_block ? s + "d," + s + "d,ui" : s + "d," + s + "d" );
                }
                comma::csv::output_stream< snark::eigen::fit_plane_output_t > ostream( std::cout, output_csv );
                while( true )
                {
                    const snark::eigen::input_t* p = istream.read();
                    if( !p || ( !buffer.empty() && buffer.front().block != p->block ) )
                    {
                        typedef Eigen::Matrix< double, -1, -1 > matrix_t;
                        matrix_t sample( *size, buffer.size() );
                        for( std::size_t i = 0; i < buffer.size(); ++i ) // todo: watch performance, cache misses; use tbb::parallel_for?
                        {
                            for( std::size_t j = 0; j < *size; ++j ) { sample.row( j )[i] = buffer[i].data[j]; }
                        }
                        Eigen::VectorXd mean( *size );
                        for( std::size_t i = 0; i < *size; ++i )
                        {
                            mean( i ) = sample.row( i ).mean();
                            sample.row( i ).array() -= mean( i );
                        }
                        Eigen::VectorXd normal = sample.jacobiSvd( Eigen::ComputeThinU | Eigen::ComputeThinV ).matrixU().rightCols< 1 >();
                        snark::eigen::fit_plane_output_t output;
                        ::memcpy( &output.mean[0], &mean[0], *size * sizeof( double ) ); // quick and dirty
                        ::memcpy( &output.normal[0], &normal[0], *size * sizeof( double ) ); // quick and dirty
                        output.block = buffer.front().block;
                        ostream.write( output );
                        buffer.clear();
                    }
                    if( !p ) { break; }
                    buffer.push_back( *p );
                }
                return 0;
            }

        }
        if( operation == "rotation" )
        {
            std::string from = options.value< std::string >( "--from", "euler" );
            std::string to = options.value< std::string >( "--to", "euler" );
            if( from == "euler" || from == "rpy" || from == "roll,pitch,yaw" ) { return rotation::run< snark::roll_pitch_yaw >( options, to ); }
            if( from == "angle-axis" || from == "axis-angle" ) { return rotation::run< Eigen::AngleAxis< double > >( options, to ); }
            if( from == "axis-angle-scaled" ) { return rotation::run< Eigen::Vector3d >( options, to ); }
            if( from == "quaternion" ) { return rotation::run< Eigen::Quaternion< double > >( options, to ); }
            if( from == "rotation-matrix" || from == "matrix" ) { return rotation::run< Eigen::Matrix< double, 3, 3 > >( options, to ); }
            std::cerr << "math-eigen: rotation: expected valid value for --from; got --from=\"" << from << "\"" << std::endl;
            return 1;
        }
        std::cerr << "math-eigen: expected operation, got \"" << operation << "\"" << std::endl;
    }
    catch( std::exception& ex ) { std::cerr << "math-eigen: " << ex.what() << std::endl; }
    catch( ... ) { std::cerr << "math-eigen: unknown exception" << std::endl; }
    return 1;
}


#include <fstream>
#include <iostream>
#include <random>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include "envitools/CSVIO.hpp"
#include "envitools/EnviUtils.hpp"

namespace fs = boost::filesystem;
namespace po = boost::program_options;
namespace et = envitools;

int main(int argc, char** argv) {
    fs::path inputPath, csvPath;

    // clang-format off
    po::options_description options("Options");
    options.add_options()
        ("help,h", "Show this message")
        ("input-file,i", po::value<std::string>()->required(),
            "Input file path of ROIs")
        ("output-file,o", po::value<std::string>()->required(),
            "Output file path")
        ("point-count,p", po::value<int>()->required(),
            "total number of points to generate");
    // clang-format on

    // parsedOptions will hold the value of all parsed options as a Map
    po::variables_map parsedOptions;
    po::store(
        po::command_line_parser(argc, argv).options(options).run(),
        parsedOptions);

    // show the help message
    if (parsedOptions.count("help") || argc < 4) {
        std::cout << options << std::endl;
        return EXIT_SUCCESS;
    }

    // warn of missing options
    try {
        po::notify(parsedOptions);
    } catch (po::error& e) {
        std::cerr << "Error: " << e.what() << std::endl;
        return EXIT_FAILURE;
    }

    // Get the ROI CSV file path
    inputPath = parsedOptions["input-file"].as<std::string>();

    if (!(boost::filesystem::exists(inputPath))) {
        std::cerr << "Error: File path does not exist." << std::endl;
        return EXIT_FAILURE;
    }

    // Get the output path
    csvPath = parsedOptions["output-file"].as<std::string>();

    if (boost::filesystem::exists(csvPath)) {
        std::cerr
            << "Error: File path already exists. Cannot overwrite output file."
            << std::endl;
        return EXIT_FAILURE;
    }

    // Read ROIs into vector
    auto vecROIs = et::CSVIO::ReadROICSV(inputPath);

    // Get number of points to generate
    auto numPoints = parsedOptions["point-count"].as<int>();

    std::cout << "X,Y" << std::endl;
    std::vector<cv::Vec2i> vec;
    for (int i = 0; i < numPoints; i++) {
        auto roi = vecROIs[envitools::RandomInt(0, vecROIs.size() - 1)];
        auto x = envitools::RandomInt(roi.xmin, roi.xmax);
        auto y = envitools::RandomInt(roi.ymin, roi.ymax);
        vec.emplace_back(x, y);
        std::cout << x << "," << y << std::endl;
    }

    envitools::CSVIO::WriteCSV(csvPath, vec);

    return 0;
}


#include <stan/math/mix/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/fpclassify.hpp>
#include <math/rev/scal/util.hpp>
#include <math/mix/scal/fun/nan_util.hpp>
#include <limits>

TEST(AgradFwdAbs, FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using std::abs;

  fvar<var> x(2.0, 1.0);
  fvar<var> a = abs(x);

  EXPECT_FLOAT_EQ(2.0, a.val_.val());
  EXPECT_FLOAT_EQ(1.0, a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.val_.grad(y, g);
  EXPECT_FLOAT_EQ(1.0, g[0]);
}
TEST(AgradFwdAbs, FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using std::abs;

  fvar<var> x(2.0, 1.0);
  fvar<var> a = abs(x);

  AVEC z = createAVEC(x.val_);
  VEC h;
  a.d_.grad(z, h);
  EXPECT_FLOAT_EQ(0.0, h[0]);
}

TEST(AgradFwdAbs, FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using std::abs;

  fvar<fvar<var> > y;
  y.val_ = fvar<var>(4.0, 1.0);

  fvar<fvar<var> > b = abs(y);

  EXPECT_FLOAT_EQ(4.0, b.val_.val_.val());
  EXPECT_FLOAT_EQ(1.0, b.val_.d_.val());
  EXPECT_FLOAT_EQ(0.0, b.d_.val_.val());
  EXPECT_FLOAT_EQ(0.0, b.d_.d_.val());

  AVEC z = createAVEC(y.val_.val_);
  VEC h;
  b.val_.val_.grad(z, h);
  EXPECT_FLOAT_EQ(1.0, h[0]);
}
TEST(AgradFwdAbs, FvarFvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using std::abs;

  fvar<fvar<var> > y;
  y.val_ = fvar<var>(4.0, 1.0);

  fvar<fvar<var> > b = abs(y);

  AVEC z = createAVEC(y.val_.val_);
  VEC h;
  b.val_.d_.grad(z, h);
  EXPECT_FLOAT_EQ(0.0, h[0]);
}
TEST(AgradFwdAbs, FvarFvarVar_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using std::abs;

  fvar<fvar<var> > y;
  y.val_.val_ = 4.0;
  y.val_.d_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > b = abs(y);

  AVEC z = createAVEC(y.val_.val_);
  VEC h;
  b.d_.d_.grad(z, h);
  EXPECT_FLOAT_EQ(0.0, h[0]);
}

struct abs_fun {
  template <typename T0>
  inline T0 operator()(const T0& arg1) const {
    return abs(arg1);
  }
};

TEST(AgradFwdAbs, abs_NaN_1) {
  abs_fun abs_;
  test_nan_mix(abs_, false);
}