/*
 * Verifier_me.cpp
 *
 *  Created on: 16.02.2011
 *      Author: stephaniebayer
 */

#include "Verifier_me.h"
#include<vector>
#include "Cipher_elg.h"
#include "G_q.h"
#include "Mod_p.h"
#include "Functions.h"
#include "ElGammal.h"
#include "multi_expo.h"
#include "func_ver.h"
#include <fstream>

#include <time.h>
#include <NTL/ZZ.h>
NTL_CLIENT

//extern G_q G;
extern G_q H;
//extern Pedersen Ped;
//extern ElGammal El;
//extern double time_rw_v;

Verifier_me::Verifier_me() {
	// TODO Auto-generated constructor stub

}

Verifier_me::Verifier_me(vector<long> num) {
	// sets the values of the matrix according to the input
	m = num[1]; //number of rows
	n = num[2]; //number of columns
	omega = num[3]; //windowsize for multi-expo-technique
	omega_sw = num[4]; //windowsize for multi-expo-technique sliding window
	omega_LL = num[7]; //windowsize for multi-expo-techniqueof Lim and Lee

	c_A = new vector<Mod_p>(m+1); //allocate the storage for the commitments of Y
	c_B = new vector<Mod_p>(m); //allocate the storage for the commitments of T
	chal_x6 = new vector<ZZ>(2*m);// allocate the storage for the vector of Vandermonde challenges t, ... t^n
	chal_y6 = new vector<ZZ>(n);// allocate the storage for the vector of Vandermonde challenges t, ... t^n
	//allocates the storage needed for the vector e
	chal_x8 = new vector<ZZ>(2*m +1); //challenge from round 8
	basis_chal_x8 = new vector<vector<long>*>(2*m+2);//basis of vector e for multi-expo technique
	mul_chal_x8 = new vector<ZZ>(2*m+2); //shifted vector e, e(0) = 1, used for multi-expo

	//Commitments vectors
	c_Dh= new vector<Mod_p>(m); //commitments to the matrix W
	c_Ds= new vector<Mod_p>(m+1); //contains a_W*t_1
	c_Dl= new vector<Mod_p>(2*m+1); //commitments to the values Cl
	c_a= new vector<Mod_p>(2*m); //commitment to the values in the matrix a
	//Vector of product of the diagonals of permuted Ciphertexts from round 5
	E = new vector<Cipher_elg>(2*m);

	D_h_bar = new vector<ZZ>(n);//sum over the rows in D_h
	d_bar = new vector<ZZ>(n);// chal_x8*D_h(m-1) +d
	Delta_bar = new vector<ZZ>(n);//chal_x8*d_h+Delta
	B_bar  = new vector<ZZ>(n); // sum over the rows in B multiplied by chal^i

	A_bar = new vector<ZZ>(n); //sum over the rows in Y times the challenges
	Ds_bar = new vector<ZZ>(n); // sum over the rows in U times thes challenges

}

Verifier_me::~Verifier_me() {

	delete c_A;
	delete c_B;
	delete chal_x6;
	delete chal_y6;
	delete chal_x8;
	delete mul_chal_x8;
	Functions::delete_vector(basis_chal_x8);
	delete c_Dh;
	delete c_Ds;
	delete c_Dl;
	delete c_a;
	delete E;
	delete D_h_bar;
	delete d_bar;
	delete Delta_bar;
	delete B_bar;
	delete A_bar;
	delete Ds_bar;
}

string Verifier_me::round_2(string in_name){
	long i;
	Mod_p temp;
	string name;
	ZZ ord=H.get_ord();
	time_t rawtime;
	time ( &rawtime );
	//sets a_Y to the values in the file name
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	for (i = 0; i<m; i++){
		ist >> c_A->at(i);
	}
	chal_x2 = RandomBnd(ord);

	name = "round_2 ";
	name = name + ctime(&rawtime);
	ofstream ost(name.c_str());

	ost<<chal_x2;

	return name;
}

string Verifier_me::round_4(string in_name){
	ZZ temp; //temporary value to construct the vector t
	long i;
	Mod_p tem;
	ZZ ord = H.get_ord();
	string name;
	time_t rawtime;
	time ( &rawtime );
	 //sets a_T to the values in the file
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	for (i = 0; i<m; i++){
		ist >> c_B->at(i);
	}

	//Set name of the output file and open stream
	name = "round_4 ";
	name = name + ctime(&rawtime);

	chal_z4 = RandomBnd(ord); //choose value alpha at random
	chal_y4 = RandomBnd(ord); //choose value lamda at random

	ofstream ost(name.c_str());
	ost<< chal_z4<<"\n";
	ost<<chal_y4;
	return name;
}

string Verifier_me::round_6(string in_name){
	ZZ tem;
	ZZ ord = H.get_ord();
	long i,l;
	Mod_p temp;
	string name;
	time_t rawtime;
	time ( &rawtime );

	l=2*m;
	//reads the values out of the file name
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	ist>>c_z;
	for (i = 0; i<m; i++){
		ist >>c_Dh->at(i);
	}
	ist>> c_B0 ;
	for (i = 0; i<l; i++){
		ist >>c_a->at(i);
	}
	for (i = 0; i<l; i++){
		ist >>E->at(i);
	}

	func_ver::fill_vector(chal_x6);
	func_ver::fill_vector(chal_y6);

	name = "round_6 ";
	name = name + ctime(&rawtime);

	ofstream ost(name.c_str());
	for (i=0; i<l; i++){
		ost << chal_x6->at(i)<<" ";
	}
	ost << "\n";
	for (i=0; i<n; i++){
		ost << chal_y6->at(i)<<" ";
	}
	ost << "\n";

	return name;

}

string  Verifier_me::round_8(string in_name){
	long l;;
	long i;

	Mod_p temp;
	Cipher_elg tem;
	string name;
	time_t rawtime;
	time ( &rawtime );
	//reads the values out of the file name

	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	l=2*m;
	for (i = 0; i<=l; i++){
		ist >> c_Dl->at(i) ;
	}
	ist>>c_D0;
	ist>>c_Dm;
	ist>>c_d;
	ist>>c_Delta;
	ist>>c_dh;
	for (i = 0; i<n; i++){
		ist >> B_bar->at(i);
	}
	ist>> r_B_bar;
	ist >>a_bar;
	ist >> r_a_bar;
	ist >> rho_bar;

	//sets chal_x8 as Vandermode vector with value chal
	func_ver::fill_x8(chal_x8, basis_chal_x8, mul_chal_x8, omega);
	l=chal_x8->size();
	name = "round_8 ";
	name = name + ctime(&rawtime);
	ofstream ost(name.c_str());
	for (i = 0; i<l; i++){
		ost << chal_x8->at(i)<< " ";
	}
	return name;

}

ZZ Verifier_me:: round_10(string in_name,vector<vector<Cipher_elg>* >* enc, vector<vector<Cipher_elg>* >* C){
	int b;
	long i;
	string name;
	time_t rawtime;
	time ( &rawtime );

	//reads the values out of the file name
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	for (i = 0; i<n; i++){
		ist >>D_h_bar->at(i);
	}
	ist>>r_Dh_bar;
	for(i=0; i<n; i++){
		ist>>d_bar->at(i);
	}
	ist>> r_d_bar;
	for(i=0; i<n; i++){
		ist>> Delta_bar->at(i);
	}
	ist>>r_Delta_bar;

	for (i = 0; i<n; i++){
		ist >>A_bar->at(i);
	}
	ist>>r_A_bar;
	for(i=0; i<n; i++){
		ist>>Ds_bar->at(i);
	}
	ist>>r_Ds_bar;
	ist>>r_Dl_bar;

	//Check that the Dhi's are constructed correctly
	b=func_ver::check_Dh_op(c_Dh, mul_chal_x8, D_h_bar, r_Dh_bar, omega_LL);
	if(b==1){
		//Check that matrix D is constructed correctly
		b=func_ver::check_D_op(c_D0, c_z, c_A, c_B, chal_x8, chal_y4, A_bar, r_A_bar, n);
		if(b==1){
			//Check that Ds is constructed correctly
			b=func_ver::check_Ds_op(c_Ds, c_Dh, c_Dm, chal_x6, chal_x8, Ds_bar, r_Ds_bar);
			if(b==1){
				//Check that the Dl's are correct
				b=func_ver::check_Dl_op(c_Dl, chal_x8, A_bar, Ds_bar, chal_y6, r_Dl_bar);
				if(b==1){
					//Check that vector d was constructed correctly
					b=func_ver::check_d_op(c_Dh, c_d, chal_x8, d_bar, r_d_bar);
					if(b==1){
						//Check that Deltas are constructed correctly
						b=func_ver::check_Delta_op(c_dh, c_Delta, chal_x8, Delta_bar, d_bar, r_Delta_bar, chal_x2, chal_z4, chal_y4);
						if(b==1 ){
							//Check that the commitments c_B contain the right values
							b=func_ver::check_B_op(c_B, c_B0, chal_x6, B_bar, r_B_bar, omega_LL);
							if(b==1){
								//Check that the reecncryption was done correctly
								b=func_ver::check_a_op(c_a, c_Dl, chal_x6, a_bar, r_a_bar);
								if(b==1){
									//Check that E->at(m) contains c
									b = func_ver::check_c_op(enc, E, chal_x2, omega);
									if(b==1){
										//Check correctness of the ciphertexts
										b =func_ver::check_E_op(C, E, chal_x6, B_bar, a_bar, rho_bar, omega);
										if(b==1 ){
											//cout<<"Accept";
										}
									}
								}
							}
						}
					}
				}
			}
		}
	}
	return to_ZZ(-1);
}


#ifndef DART_NEURAL_SGD_OPTIMIZER_HPP_
#define DART_NEURAL_SGD_OPTIMIZER_HPP_

#include <functional>
#include <memory>
#include <vector>

#include <Eigen/Dense>

#include "dart/trajectory/Optimizer.hpp"
#include "dart/trajectory/Problem.hpp"
#include "dart/trajectory/Solution.hpp"
#include "dart/trajectory/TrajectoryConstants.hpp"

namespace dart {

namespace simulation {
class World;
}

namespace trajectory {

/*
 * IPOPT wants to own the trajectories it's trying to optimize, so we need a way
 * to create a buffer that's possible for IPOPT to own without freeing the
 * underlying trajectory when it's done.
 */
class SGDOptimizer : public Optimizer
{
public:
  SGDOptimizer();

  virtual ~SGDOptimizer() = default;

  std::shared_ptr<Solution> optimize(
      Problem* shot, std::shared_ptr<Solution> warmStart = nullptr) override;

  void setIterationLimit(int iterationLimit);

  void setTolerance(s_t tolerance);

  void setLearningRate(s_t learningRate);

protected:
  int mIterationLimit;
  s_t mTolerance;
  s_t mLearningRate;
};

} // namespace trajectory
} // namespace dart

#endif


#include <iostream>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include "bhtsne.h"

using namespace std;
namespace po = boost::program_options;
namespace fsys = boost::filesystem;

int main(int argc, char **argv) {
  // Declare the supported options
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help", "produce help message")
    ("input-P", po::value<string>()->value_name("FILE")->default_value("P.dat"), "name of binary input file containing P matrix (see ComputeP)")
    ("out-dir", po::value<string>()->value_name("DIR")->default_value("out"), "where to create output files; directory will be created if it does not exist")
    ("out-dim", po::value<int>()->value_name("NUM")->default_value(2), "number of output dimensions")
    ("max-iter", po::value<int>()->value_name("NUM")->default_value(1000), "maximum number of iterations")
    ("rand-seed", po::value<int>()->value_name("NUM")->default_value(-1), "seed for random number generator; to use current time as seed set it to -1")
    ("theta", po::value<double>()->value_name("NUM")->default_value(0.5, "0.5"), "a value between 0 and 1 that controls the accuracy-efficiency tradeoff in SPTree for gradient computation; 0 means exact")
    ("learn-rate", po::value<double>()->value_name("NUM")->default_value(200, "200"), "learning rate for gradient steps")
    ("mom-init", po::value<double>()->value_name("NUM")->default_value(0.5, "0.5"), "initial momentum between 0 and 1")
    ("mom-final", po::value<double>()->value_name("NUM")->default_value(0.8, "0.8"), "final momentum between 0 and 1 (switch point controlled by --mom-switch-iter)")
    ("mom-switch-iter", po::value<int>()->value_name("NUM")->default_value(250), "duration (number of iterations) of initial momentum")
    ("early-exag-iter", po::value<int>()->value_name("NUM")->default_value(250), "duration (number of iterations) of early exaggeration")
    ("skip-random-init", po::bool_switch()->default_value(false), "skip random initialization")
    ("batch-frac", po::value<double>()->value_name("NUM"), "what fraction of points to update for each iteration")
    ("cache-iter", po::value<int>()->value_name("NUM")->default_value(INT_MAX, "INT_MAX"), "After every NUM iterations, write intermediary embeddings to disk. Final embedding is always reported.")
  ;

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).
                options(desc).run(), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << "Usage: RunBhtsne [options]" << endl;
    cout << desc << "\n";
    return 1;
  }

  string infile = vm["input-P"].as<string>();
  string outdir = vm["out-dir"].as<string>();

  fsys::path dir(outdir);
  if (fsys::is_directory(dir)) {
    cout << "Error: Output directory already exists" << endl;
    return 1;
  }
  if (fsys::create_directory(dir)) {
    cout << "Output directory created: " << outdir << endl;
  }

  TSNE *tsne = new TSNE();

  fsys::path paramfile = dir;
  paramfile /= "param.txt";
  ofstream ofs(paramfile.string().c_str());
  ofs << "input-P: " << infile << endl;
  ofs << "out-dir: " << fsys::canonical(dir).string() << endl;

  int no_dims = vm["out-dim"].as<int>(); ofs << "out-dim: " << no_dims << endl;
  double theta = vm["theta"].as<double>(); ofs << "theta: " << theta << endl;
  int rand_seed = vm["rand-seed"].as<int>(); ofs << "rand-seed: " << rand_seed << endl;
  bool skip_random_init = vm["skip-random-init"].as<bool>(); ofs << "skip-random-init: " << skip_random_init << endl;
  int max_iter = vm["max-iter"].as<int>(); ofs << "max-iter: " << max_iter << endl;
  int stop_lying_iter = vm["early-exag-iter"].as<int>(); ofs << "early-exag-iter: " << stop_lying_iter << endl;
  int mom_switch_iter = vm["mom-switch-iter"].as<int>(); ofs << "mom-switch-iter: " << mom_switch_iter << endl;
  double momentum = vm["mom-init"].as<double>(); ofs << "mom-init: " << momentum << endl;
  double final_momentum = vm["mom-final"].as<double>(); ofs << "mom-final: " << final_momentum << endl;
  double eta = vm["learn-rate"].as<double>(); ofs << "learn-rate: " << eta << endl;
  tsne->CACHE_ITER = vm["cache-iter"].as<int>(); ofs << "cache-iter: " << tsne->CACHE_ITER << endl;

  if (vm.count("batch-frac")) {
    double batch_frac = vm["batch-frac"].as<double>(); ofs << "batch-frac: " << batch_frac << endl;
    tsne->BATCH_FLAG = true;
    tsne->BATCH_FRAC = batch_frac;
  }

  ofs.close();

  printf("Loading input similarities...\n");
  int N;
  unsigned int *row_P;
  unsigned int *col_P;
  double *val_P;
  if (!tsne->load_P(infile, N, &row_P, &col_P, &val_P)) {
    cout << "Error: failed to load P from " << infile << endl;
    return 1;
  }

  double *Y = (double *)malloc(N * no_dims * sizeof(double));
  if (Y == NULL) {
    cout << "Error: Memory allocation for the output failed" << endl;
    return 1;
  }

  if (!tsne->run(N, row_P, col_P, val_P, Y, no_dims, theta, rand_seed,
           skip_random_init, max_iter, stop_lying_iter, mom_switch_iter,
           momentum, final_momentum, eta, dir)) {
    return 1;
  }

  free(row_P);
  free(col_P);
  free(val_P);
  free(Y);
  delete(tsne);

  cout << "Done" << endl;

  return 0;
}


/*
@copyright Louis Dionne 2015
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/hana.hpp>

#include <cxxabi.h>
#include <iostream>
#include <memory>
#include <sstream>
#include <string>
#include <typeinfo>
namespace hana = boost::hana;

// Boilerplate for the `print` method
template <typename T, typename = void>
struct print_impl : print_impl<T, hana::when<true>> { };

template <typename T, bool condition>
struct print_impl<T, hana::when<condition>> : hana::default_ {
    template <typename ...Args>
    static constexpr auto apply(Args&& ...) = delete;
};

template <typename T>
constexpr auto print(T const& t) {
    using Print = print_impl<typename hana::tag_of<T>::type>;
    return Print::apply(t);
}

// Define the `Printable` concept
template <typename T>
struct Printable {
    using Tag = typename hana::tag_of<T>::type;
    static constexpr bool value = !hana::is_default<print_impl<Tag>>::value;
};

// model for Sequences
template <typename S>
struct print_impl<S, hana::when<hana::Sequence<S>::value>> {
    template <typename Xs>
    static auto apply(Xs const& xs) {
        std::string result = "(";
        auto comma_separated = hana::intersperse(xs, ", ");
        hana::for_each(comma_separated, [&result](auto const& x) {
            result += print(x);
        });
        result += ")";
        return result;
    }
};

template <typename ...>
struct is_valid {
    static constexpr bool value = true;
};

// model for OutputStreamable types
template <typename S>
struct print_impl<S, hana::when<is_valid<decltype(
    std::declval<std::ostringstream&>() << std::declval<S const&>()
)>::value>> {
    template <typename T>
    static auto apply(T const& t) {
        std::ostringstream os;
        os << t;
        return os.str();
    }
};

// model for hana::maps
template <>
struct print_impl<hana::map_tag> {
    template <typename M>
    static auto apply(M const& map) {
        std::string result = "{";
        auto pairs = hana::transform(hana::to<hana::tuple_tag>(map),
            [](auto const& pair) {
                return print(hana::first(pair)) + " => " + print(hana::second(pair));
            });
        auto comma_separated = hana::intersperse(pairs, ", ");
        hana::for_each(comma_separated, [&result](auto const& element) {
            result += element;
        });
        result += "}";
        return result;
    }
};

// model for Constants holding a `Printable`
template <typename C>
struct print_impl<C, hana::when<
    hana::Constant<C>::value &&
    Printable<typename C::value_type>::value
>> {
    template <typename T>
    static auto apply(T const&) {
        constexpr auto value = hana::value<T>();
        return print(value);
    }
};

// model for Products
template <typename P>
struct print_impl<P, hana::when<hana::Product<P>::value>> {
    template <typename T>
    static auto apply(T const& t) {
        return '(' + print(hana::first(t)) + ", " + print(hana::second(t)) + ')';
    }
};

// model for hana::strings
template <>
struct print_impl<hana::string_tag> {
    template <typename S>
    static auto apply(S const& s) {
        return '"' + std::string{hana::to<char const*>(s)} + '"';
    }
};

// model for hana::sets
template <>
struct print_impl<hana::set_tag> {
    template <typename S>
    static auto apply(S const& set) {
        std::string result = "{";
        auto as_tuple = hana::transform(hana::to<hana::tuple_tag>(set), [](auto const& x) { return print(x); });
        auto comma_separated = hana::intersperse(as_tuple, ", ");
        hana::for_each(comma_separated, [&result](auto const& element) {
            result += element;
        });
        result += "}";
        return result;
    }
};

// model for hana::types
template <>
struct print_impl<hana::type_tag> {
    static std::string demangle(char const* mangled) {
        int status;
        std::unique_ptr<char[], void(*)(void*)> result(
            abi::__cxa_demangle(mangled, 0, 0, &status), std::free
        );
        return result.get() ? result.get() : mangled;
    }

    template <typename T>
    static auto apply(T const&) {
        return "type<" + demangle(typeid(typename T::type).name()) + '>';
    }
};

int main() {
    auto tuple = hana::make_tuple(1, '2', BOOST_HANA_STRING("3456"));

    auto set = hana::make_set(hana::int_c<1>, hana::int_c<2>);

    auto map = hana::make_map(
        hana::make_pair(hana::int_c<1>, hana::type_c<void>),
        hana::make_pair(hana::type_c<decltype(tuple)>, "int")
    );

    auto pair = hana::make_pair(1, hana::int_c<1>);

    std::cout << print(tuple) << std::endl
              << print(set) << std::endl
              << print(map) << std::endl
              << print(pair) << std::endl;
}


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

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

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

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

#ifndef BOOST_GEOMETRY_STRATEGIES_STRATEGY_TRANSFORM_HPP
#define BOOST_GEOMETRY_STRATEGIES_STRATEGY_TRANSFORM_HPP

#include <cstddef>
#include <cmath>
#include <functional>

#include <boost/numeric/conversion/cast.hpp>

#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/strategies/transform.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace transform
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template
<
    typename Src, typename Dst,
    std::size_t D, std::size_t N,
    template <typename> class F
>
struct transform_coordinates
{
    template <typename T>
    static inline void transform(Src const& source, Dst& dest, T value)
    {
        typedef typename select_coordinate_type<Src, Dst>::type coordinate_type;

        F<coordinate_type> function;
        set<D>(dest, boost::numeric_cast<coordinate_type>(function(get<D>(source), value)));
        transform_coordinates<Src, Dst, D + 1, N, F>::transform(source, dest, value);
    }
};

template
<
    typename Src, typename Dst,
    std::size_t N,
    template <typename> class F
>
struct transform_coordinates<Src, Dst, N, N, F>
{
    template <typename T>
    static inline void transform(Src const& , Dst& , T )
    {
    }
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
    \brief Transformation strategy to copy one point to another using assignment operator
    \ingroup transform
    \tparam P point type
 */
template <typename P>
struct copy_direct
{
    inline bool apply(P const& p1, P& p2) const
    {
        p2 = p1;
        return true;
    }
};

/*!
    \brief Transformation strategy to do copy a point, copying per coordinate.
    \ingroup transform
    \tparam P1 first point type
    \tparam P2 second point type
 */
template <typename P1, typename P2>
struct copy_per_coordinate
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        // Defensive check, dimensions are equal, selected by specialization
        assert_dimension_equal<P1, P2>();

        geometry::convert(p1, p2);
        return true;
    }
};

/*!
    \brief Transformation strategy to go from degree to radian and back
    \ingroup transform
    \tparam P1 first point type
    \tparam P2 second point type
    \tparam F additional functor to divide or multiply with d2r
 */
template <typename P1, typename P2, template <typename> class F>
struct degree_radian_vv
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        // Spherical coordinates always have 2 coordinates measured in angles
        // The optional third one is distance/height, provided in another strategy
        // Polar coordinates having one angle, will be also in another strategy
        assert_dimension<P1, 2>();
        assert_dimension<P2, 2>();

        detail::transform_coordinates<P1, P2, 0, 2, F>::transform(p1, p2, math::d2r);
        return true;
    }
};

template <typename P1, typename P2, template <typename> class F>
struct degree_radian_vv_3
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension<P1, 3>();
        assert_dimension<P2, 3>();

        detail::transform_coordinates<P1, P2, 0, 2, F>::transform(p1, p2, math::d2r);
        // Copy height or other third dimension
        set<2>(p2, get<2>(p1));
        return true;
    }
};

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

    /// Helper function for conversion, phi/theta are in radians
    template <typename P, typename T, typename R>
    inline void spherical_polar_to_cartesian(T phi, T theta, R r, P& p)
    {
        assert_dimension<P, 3>();

        // http://en.wikipedia.org/wiki/List_of_canonical_coordinate_transformations#From_spherical_coordinates
        // http://www.vias.org/comp_geometry/math_coord_convert_3d.htm
        // https://moodle.polymtl.ca/file.php/1183/Autres_Documents/Derivation_for_Spherical_Co-ordinates.pdf
        // http://en.citizendium.org/wiki/Spherical_polar_coordinates

        // Phi = first, theta is second, r is third, see documentation on cs::spherical

        // (calculations are splitted to implement ttmath)

        T r_sin_theta = r;
        T r_cos_theta = r;
        r_sin_theta *= sin(theta);
        r_cos_theta *= cos(theta);

        set<0>(p, r_sin_theta * cos(phi));
        set<1>(p, r_sin_theta * sin(phi));
        set<2>(p, r_cos_theta);
    }

    /// Helper function for conversion, lambda/delta (lon lat) are in radians
    template <typename P, typename T, typename R>
    inline void spherical_equatorial_to_cartesian(T lambda, T delta, R r, P& p)
    {
        assert_dimension<P, 3>();

        // http://mathworld.wolfram.com/GreatCircle.html
        // http://www.spenvis.oma.be/help/background/coortran/coortran.html WRONG

        T r_cos_delta = r;
        T r_sin_delta = r;
        r_cos_delta *= cos(delta);
        r_sin_delta *= sin(delta);

        set<0>(p, r_cos_delta * cos(lambda));
        set<1>(p, r_cos_delta * sin(lambda));
        set<2>(p, r_sin_delta);
    }

    /// Helper function for conversion
    template <typename P, typename T>
    inline bool cartesian_to_spherical2(T x, T y, T z, P& p)
    {
        assert_dimension<P, 2>();

        // http://en.wikipedia.org/wiki/List_of_canonical_coordinate_transformations#From_Cartesian_coordinates

#if defined(BOOST_GEOMETRY_TRANSFORM_CHECK_UNIT_SPHERE)
        // TODO: MAYBE ONLY IF TO BE CHECKED?
        T const r = /*sqrt not necessary, sqrt(1)=1*/ (x * x + y * y + z * z);

        // Unit sphere, so r should be 1
        if (geometry::math::abs(r - 1.0) > T(1e-6))
        {
            return false;
        }
        // end todo
#endif

        set_from_radian<0>(p, atan2(y, x));
        set_from_radian<1>(p, acos(z));
        return true;
    }

    template <typename P, typename T>
    inline bool cartesian_to_spherical_equatorial2(T x, T y, T z, P& p)
    {
        assert_dimension<P, 2>();

        set_from_radian<0>(p, atan2(y, x));
        set_from_radian<1>(p, asin(z));
        return true;
    }

    template <typename P, typename T>
    inline bool cartesian_to_spherical3(T x, T y, T z, P& p)
    {
        assert_dimension<P, 3>();

        // http://en.wikipedia.org/wiki/List_of_canonical_coordinate_transformations#From_Cartesian_coordinates
        T const r = sqrt(x * x + y * y + z * z);
        set<2>(p, r);
        set_from_radian<0>(p, atan2(y, x));
        if (r > 0.0)
        {
            set_from_radian<1>(p, acos(z / r));
            return true;
        }
        return false;
    }

	template <typename P, typename T>
	inline bool cartesian_to_spherical_equatorial3(T x, T y, T z, P& p)
	{
		assert_dimension<P, 3>();

		// http://en.wikipedia.org/wiki/List_of_canonical_coordinate_transformations#From_Cartesian_coordinates
		T const r = sqrt(x * x + y * y + z * z);
		set<2>(p, r);
		set_from_radian<0>(p, atan2(y, x));
		if (r > 0.0)
		{
			set_from_radian<1>(p, asin(z / r));
			return true;
		}
		return false;
	}

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
    \brief Transformation strategy for 2D spherical (phi,theta) to 3D cartesian (x,y,z)
    \details on Unit sphere
    \ingroup transform
    \tparam P1 first point type
    \tparam P2 second point type
 */
template <typename P1, typename P2>
struct from_spherical_polar_2_to_cartesian_3
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension<P1, 2>();
        detail::spherical_polar_to_cartesian(get_as_radian<0>(p1), get_as_radian<1>(p1), 1.0, p2);
        return true;
    }
};

template <typename P1, typename P2>
struct from_spherical_equatorial_2_to_cartesian_3
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension<P1, 2>();
        detail::spherical_equatorial_to_cartesian(get_as_radian<0>(p1), get_as_radian<1>(p1), 1.0, p2);
        return true;
    }
};

/*!
    \brief Transformation strategy for 3D spherical (phi,theta,r) to 3D cartesian (x,y,z)
    \ingroup transform
    \tparam P1 first point type
    \tparam P2 second point type
 */
template <typename P1, typename P2>
struct from_spherical_polar_3_to_cartesian_3
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension<P1, 3>();
        detail::spherical_polar_to_cartesian(
                    get_as_radian<0>(p1), get_as_radian<1>(p1), get<2>(p1), p2);
        return true;
    }
};

template <typename P1, typename P2>
struct from_spherical_equatorial_3_to_cartesian_3
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension<P1, 3>();
        detail::spherical_equatorial_to_cartesian(
                    get_as_radian<0>(p1), get_as_radian<1>(p1), get<2>(p1), p2);
        return true;
    }
};

/*!
    \brief Transformation strategy for 3D cartesian (x,y,z) to 2D spherical (phi,theta)
    \details on Unit sphere
    \ingroup transform
    \tparam P1 first point type
    \tparam P2 second point type
    \note If x,y,z point is not lying on unit sphere, transformation will return false
 */
template <typename P1, typename P2>
struct from_cartesian_3_to_spherical_polar_2
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension<P1, 3>();
        return detail::cartesian_to_spherical2(get<0>(p1), get<1>(p1), get<2>(p1), p2);
    }
};

template <typename P1, typename P2>
struct from_cartesian_3_to_spherical_equatorial_2
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension<P1, 3>();
        return detail::cartesian_to_spherical_equatorial2(get<0>(p1), get<1>(p1), get<2>(p1), p2);
    }
};

/*!
    \brief Transformation strategy for 3D cartesian (x,y,z) to 3D spherical (phi,theta,r)
    \ingroup transform
    \tparam P1 first point type
    \tparam P2 second point type
 */
template <typename P1, typename P2>
struct from_cartesian_3_to_spherical_polar_3
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension<P1, 3>();
        return detail::cartesian_to_spherical3(get<0>(p1), get<1>(p1), get<2>(p1), p2);
    }
};

template <typename P1, typename P2>
struct from_cartesian_3_to_spherical_equatorial_3
{
	inline bool apply(P1 const& p1, P2& p2) const
	{
		assert_dimension<P1, 3>();
		return detail::cartesian_to_spherical_equatorial3(get<0>(p1), get<1>(p1), get<2>(p1), p2);
	}
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

/// Specialization for same coordinate system family, same system, same dimension, same point type, can be copied
template <typename CoordSysTag, typename CoordSys, std::size_t D, typename P>
struct default_strategy<CoordSysTag, CoordSysTag, CoordSys, CoordSys, D, D, P, P>
{
    typedef copy_direct<P> type;
};

/// Specialization for same coordinate system family and system, same dimension, different point type, copy per coordinate
template <typename CoordSysTag, typename CoordSys, std::size_t D, typename P1, typename P2>
struct default_strategy<CoordSysTag, CoordSysTag, CoordSys, CoordSys, D, D, P1, P2>
{
    typedef copy_per_coordinate<P1, P2> type;
};

/// Specialization to transform from degree to radian for any coordinate system / point type combination
template <typename CoordSysTag, template<typename> class CoordSys, typename P1, typename P2>
struct default_strategy<CoordSysTag, CoordSysTag, CoordSys<degree>, CoordSys<radian>, 2, 2, P1, P2>
{
    typedef degree_radian_vv<P1, P2, std::multiplies> type;
};

/// Specialization to transform from radian to degree for any coordinate system / point type combination
template <typename CoordSysTag, template<typename> class CoordSys, typename P1, typename P2>
struct default_strategy<CoordSysTag, CoordSysTag, CoordSys<radian>, CoordSys<degree>, 2, 2, P1, P2>
{
    typedef degree_radian_vv<P1, P2, std::divides> type;
};

/// Specialization degree->radian in 3D
template <typename CoordSysTag, template<typename> class CoordSys, typename P1, typename P2>
struct default_strategy<CoordSysTag, CoordSysTag, CoordSys<degree>, CoordSys<radian>, 3, 3, P1, P2>
{
    typedef degree_radian_vv_3<P1, P2, std::multiplies> type;
};

/// Specialization radian->degree in 3D
template <typename CoordSysTag, template<typename> class CoordSys, typename P1, typename P2>
struct default_strategy<CoordSysTag, CoordSysTag, CoordSys<radian>, CoordSys<degree>, 3, 3, P1, P2>
{
    typedef degree_radian_vv_3<P1, P2, std::divides> type;
};

/// Specialization to transform from unit sphere(phi,theta) to XYZ
template <typename CoordSys1, typename CoordSys2, typename P1, typename P2>
struct default_strategy<spherical_polar_tag, cartesian_tag, CoordSys1, CoordSys2, 2, 3, P1, P2>
{
    typedef from_spherical_polar_2_to_cartesian_3<P1, P2> type;
};

/// Specialization to transform from sphere(phi,theta,r) to XYZ
template <typename CoordSys1, typename CoordSys2, typename P1, typename P2>
struct default_strategy<spherical_polar_tag, cartesian_tag, CoordSys1, CoordSys2, 3, 3, P1, P2>
{
    typedef from_spherical_polar_3_to_cartesian_3<P1, P2> type;
};

template <typename CoordSys1, typename CoordSys2, typename P1, typename P2>
struct default_strategy<spherical_equatorial_tag, cartesian_tag, CoordSys1, CoordSys2, 2, 3, P1, P2>
{
    typedef from_spherical_equatorial_2_to_cartesian_3<P1, P2> type;
};

template <typename CoordSys1, typename CoordSys2, typename P1, typename P2>
struct default_strategy<spherical_equatorial_tag, cartesian_tag, CoordSys1, CoordSys2, 3, 3, P1, P2>
{
    typedef from_spherical_equatorial_3_to_cartesian_3<P1, P2> type;
};

/// Specialization to transform from XYZ to unit sphere(phi,theta)
template <typename CoordSys1, typename CoordSys2, typename P1, typename P2>
struct default_strategy<cartesian_tag, spherical_polar_tag, CoordSys1, CoordSys2, 3, 2, P1, P2>
{
    typedef from_cartesian_3_to_spherical_polar_2<P1, P2> type;
};

template <typename CoordSys1, typename CoordSys2, typename P1, typename P2>
struct default_strategy<cartesian_tag, spherical_equatorial_tag, CoordSys1, CoordSys2, 3, 2, P1, P2>
{
    typedef from_cartesian_3_to_spherical_equatorial_2<P1, P2> type;
};

/// Specialization to transform from XYZ to sphere(phi,theta,r)
template <typename CoordSys1, typename CoordSys2, typename P1, typename P2>
struct default_strategy<cartesian_tag, spherical_polar_tag, CoordSys1, CoordSys2, 3, 3, P1, P2>
{
    typedef from_cartesian_3_to_spherical_polar_3<P1, P2> type;
};
template <typename CoordSys1, typename CoordSys2, typename P1, typename P2>
struct default_strategy<cartesian_tag, spherical_equatorial_tag, CoordSys1, CoordSys2, 3, 3, P1, P2>
{
	typedef from_cartesian_3_to_spherical_equatorial_3<P1, P2> type;
};

} // namespace services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::transform

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_STRATEGY_TRANSFORM_HPP


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

#include <iterator>
#include <iostream>
#include <fstream>
#include <limits>
#include "ioMatrix.hpp"
#include "SiconosMatrix.hpp"
#include "SiconosException.hpp"
#include "SimpleMatrix.hpp"
#include "SiconosVector.hpp"
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>

#include "SiconosAlgebra.hpp"

namespace ioMatrix
{
bool read(const std::string& fileName, const std::string& mode, SiconosMatrix& m)
{
  std::ifstream infile;
  if(mode == "ascii")
    infile.open(fileName.c_str(), std::ifstream::in);
  else if(mode == "binary")
    infile.open(fileName.c_str(), std::ifstream::binary);
  else
    THROW_EXCEPTION("incorrect mode for reading");

  if(!infile.good())
    THROW_EXCEPTION("");

  if(infile.peek() == std::ifstream::traits_type::eof())
  {
    THROW_EXCEPTION("the given file is empty!");
  }

  if(m.isBlock())
    THROW_EXCEPTION("not yet implemented for block matrix.");

  infile.precision(15);
  infile.setf(std::ios::scientific);
  DenseMat& p = *m.dense();

  // Dim of the matrix are given in the first line.
  // Just use to check that sizes are consistents.

  unsigned int s1, s2;
  infile >> s1;
  infile >> s2;

  if(s1 != p.size1() || s2 != p.size2())
    p.resize(s1, s2);

  // Note: using istream stl iterator seems to be 2-times faster than << with a loop over matrix data.
  //  copy((std::istream_iterator<double>(infile)), std::istream_iterator<double>(), (p->data()).begin());
  // But it fails with column-major saving ... (ok if user write its matrix in a column-major way)

  DenseMat::iterator1 it;
  DenseMat::iterator2 it2;
  //    std::cout.precision(15);
  //    std::cout.setf(std::ios::scientific);

  for(unsigned int i = 0; i < s1; i++)
  {
    for(unsigned int j = 0; j < s2; j++)
    {
      infile >> p(i, j);
      /* fail on ubuntu 14.04 assert(infile.good());*/
    }
  }

  // Old version: result in Boost format for ouptut
  //  DenseMat * p = m.dense();
  //  infile >> *p;

  infile.close();
  return true;
}

bool write(const std::string& fileName, const std::string& mode, const SiconosMatrix& m, const std::string& outputType)
{
  // Open file and various checks
  std::ofstream outfile;
  if(mode == "ascii")
    outfile.open(fileName.c_str(), std::ofstream::out);
  else if(mode == "binary")
    outfile.open(fileName.c_str(), std::ofstream::binary);
  else
    THROW_EXCEPTION("Incorrect mode for writing");

  if(!outfile.good())
    THROW_EXCEPTION("");

  if(m.isBlock())
    THROW_EXCEPTION("not yet implemented for BlockMatrix");

  outfile.precision(15);
  outfile.setf(std::ios::scientific);
  // Writing

  if(outputType != "noDim")
    outfile << m.size(0) << " " << m.size(1) << std::endl;

  if(m.num() == Siconos::DENSE)
  {
    // DenseMat * p = m.dense();
    DenseMat::iterator1 row;
    DenseMat::iterator2 col;
    double tmp;
    for(unsigned int i = 0; i < m.size(0); i++)
    {
      for(unsigned int j = 0; j < m.size(1); j++)
      {
        tmp = m(i, j);
        if(fabs(tmp) < std::numeric_limits<double>::min()) tmp = 0.0;
        outfile << tmp << " " ;
        assert(outfile.good());
      }
      outfile << std::endl;

    }
  }
  else if(m.num() == Siconos::TRIANGULAR)
  {
    TriangMat * p = m.triang();
    TriangMat::iterator1 row;
    for(row = p->begin1(); row != p->end1() ; ++row)
    {
      std::copy(row.begin(), row.end(), std::ostream_iterator<double>(outfile, " "));
      outfile << std::endl;
    }
  }
  else if(m.num() == Siconos::SYMMETRIC)
  {
    SymMat * p = m.sym();
    SymMat::iterator1 row;
    for(row = p->begin1(); row != p->end1() ; ++row)
    {
      std::copy(row.begin(), row.end(), std::ostream_iterator<double>(outfile, " "));
      outfile << std::endl;
    }
  }
  else if(m.num() == Siconos::SPARSE)
  {
    SparseMat * p = m.sparse();
    SparseMat::iterator1 row;
    for(row = p->begin1(); row != p->end1() ; ++row)
    {
      std::copy(row.begin(), row.end(), std::ostream_iterator<double>(outfile, " "));
      outfile << std::endl;
    }
  }
  else
  {
    BandedMat * p = m.banded();
    BandedMat::iterator1 row;
    for(row = p->begin1(); row != p->end1() ; ++row)
    {
      std::copy(row.begin(), row.end(), std::ostream_iterator<double>(outfile, " "));
      outfile << std::endl;
    }
  }

  outfile.close();
  return true;
}

double compareRefFile(const SimpleMatrix& data, std::string filename, double epsilon,
                      Index index, SP::SimpleMatrix* ref, std::string mode,
                      bool verbose)
{
  SP::SimpleMatrix r;
  if(!ref) ref = &r;
  *ref = std::make_shared<SimpleMatrix>(data);
  (*ref)->zero();
  bool compare = false;

  try
  {
    compare = ioMatrix::read(filename, mode, **ref);
  }
  catch(...)
  {
    if(verbose)
      std::cout << "Warning: reference file " << filename
                << " not found, no comparison performed." << std::endl;
    Siconos::exception::process();
  }
  if(!compare)
    return -1.0;

  if(verbose)
    std::cout << "Comparison with reference file " << filename << std::endl;

  SP::SiconosVector err(new SiconosVector(data.size(1)));
  (data - **ref).normInfByColumn(err);

  if(verbose)
    err->display();

  if(index.size()==0)
    for(unsigned int i = 0; i < err->size(); ++i)
      index.push_back(i);

  /* Scalar error = max of columns */
  double error = 0.0;
  for(unsigned int i = 0; i < index.size(); ++i)
  {
    if(error < (*err)(index[i]))
      error = (*err)(index[i]);
  }

  if(verbose)
    std::cout << "Error = " << error << std::endl;
  if(error > epsilon)
  {
    if(verbose)
    {
      std::cout << "Warning. The results are rather different from the reference file." << std::endl;
      std::cout << "Error = "<< error << std::endl;
    }
  }

  return error;
}

} // namespace ioMatrix

// To be used later ... ?
//   template <class T, class iterator1> friend void write( const T& obj, iterator1 row, std::ofstream outfile)
//   {
//       for(row = obj.begin1(); row!=obj.end1() ; ++row)
//  {
//    std::copy(row.begin(),row.end(),std::ostream_iterator<double>(outfile," "));
//    outfile << std::endl;
//  }
//     }


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include <mitkFiberBundle.h>
#include <mitkCommandLineParser.h>
#include <boost/lexical_cast.hpp>
#include <mitkIOUtil.h>
#include <itkTractClusteringFilter.h>

mitk::FiberBundle::Pointer LoadFib(std::string filename)
{
  std::vector<mitk::BaseData::Pointer> fibInfile = mitk::IOUtil::Load(filename);
  if( fibInfile.empty() )
    std::cout << "File " << filename << " could not be read!";
  mitk::BaseData::Pointer baseData = fibInfile.at(0);
  return dynamic_cast<mitk::FiberBundle*>(baseData.GetPointer());
}

/*!
\brief Spatially cluster fibers
*/
int main(int argc, char* argv[])
{
  mitkCommandLineParser parser;

  parser.setTitle("Fiber Clustering");
  parser.setCategory("Fiber Processing");
  parser.setContributor("MIC");

  parser.setArgumentPrefix("--", "-");
  parser.addArgument("", "i", mitkCommandLineParser::InputFile, "Input:", "input fiber bundle (.fib, .trk, .tck)", us::Any(), false);
  parser.addArgument("", "o", mitkCommandLineParser::OutputDirectory, "Output:", "output root", us::Any(), false);

  parser.addArgument("cluster_size", "", mitkCommandLineParser::Int, "Cluster size:", "", 10);
  parser.addArgument("fiber_points", "", mitkCommandLineParser::Int, "Fiber points:", "", 12);
  parser.addArgument("min_fibers", "", mitkCommandLineParser::Int, "Min. fibers per cluster:", "", 1);
  parser.addArgument("max_clusters", "", mitkCommandLineParser::Int, "Max. clusters:", "");
  parser.addArgument("merge_clusters", "", mitkCommandLineParser::Float, "Merge clusters:", "", -1.0);
  parser.addArgument("output_centroids", "", mitkCommandLineParser::Bool, "Output centroids:", "");
  parser.addArgument("metric", "", mitkCommandLineParser::String, "Metric:", "");
  parser.addArgument("input_centroids", "", mitkCommandLineParser::String, "Input centroids:", "");
  parser.addArgument("scalar_map", "", mitkCommandLineParser::String, "Scalar map:", "");
  parser.addArgument("map_scale", "", mitkCommandLineParser::Float, "Map scale:", "", 0.0);

  std::map<std::string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
  if (parsedArgs.size()==0)
    return EXIT_FAILURE;

  std::string inFileName = us::any_cast<std::string>(parsedArgs["i"]);
  std::string out_root = us::any_cast<std::string>(parsedArgs["o"]);

  int cluster_size = 10;
  if (parsedArgs.count("cluster_size"))
    cluster_size = us::any_cast<int>(parsedArgs["cluster_size"]);

  int fiber_points = 12;
  if (parsedArgs.count("fiber_points"))
    fiber_points = us::any_cast<int>(parsedArgs["fiber_points"]);

  int min_fibers = 1;
  if (parsedArgs.count("min_fibers"))
    min_fibers = us::any_cast<int>(parsedArgs["min_fibers"]);

  int max_clusters = 0;
  if (parsedArgs.count("max_clusters"))
    max_clusters = us::any_cast<int>(parsedArgs["max_clusters"]);

  float map_scale = 0.0;
  if (parsedArgs.count("map_scale"))
    map_scale = us::any_cast<float>(parsedArgs["map_scale"]);

  float merge_clusters = -1.0;
  if (parsedArgs.count("merge_clusters"))
    merge_clusters = us::any_cast<float>(parsedArgs["merge_clusters"]);

  bool output_centroids = false;
  if (parsedArgs.count("output_centroids"))
    output_centroids = us::any_cast<bool>(parsedArgs["output_centroids"]);

  std::string metric = "MDF_STD";
  if (parsedArgs.count("metric"))
    metric = us::any_cast<std::string>(parsedArgs["metric"]);

  std::string input_centroids = "";
  if (parsedArgs.count("input_centroids"))
    input_centroids = us::any_cast<std::string>(parsedArgs["input_centroids"]);

  std::string scalar_map = "";
  if (parsedArgs.count("scalar_map"))
    scalar_map = us::any_cast<std::string>(parsedArgs["scalar_map"]);

  try
  {
    typedef itk::Image< float, 3 > FloatImageType;

    mitk::FiberBundle::Pointer fib = LoadFib(inFileName);

    float max_d = 0;
    int i=1;
    std::vector< float > distances;
    while (max_d < fib->GetGeometry()->GetDiagonalLength()/2)
    {
      distances.push_back(cluster_size*i);
      max_d = cluster_size*i;
      ++i;
    }

    itk::TractClusteringFilter::Pointer clusterer = itk::TractClusteringFilter::New();
    clusterer->SetDistances(distances);
    clusterer->SetTractogram(fib);

    if (input_centroids!="")
    {

      mitk::FiberBundle::Pointer in_centroids = LoadFib(input_centroids);
      clusterer->SetInCentroids(in_centroids);
    }

    if (metric=="MDF")
      clusterer->SetMetric(itk::TractClusteringFilter::Metric::MDF);
    else if (metric=="MDF_STD")
      clusterer->SetMetric(itk::TractClusteringFilter::Metric::MDF_STD);
    else if (metric=="MAX_MDF")
      clusterer->SetMetric(itk::TractClusteringFilter::Metric::MAX_MDF);

    if (scalar_map!="")
    {
      mitk::Image::Pointer mitk_map = dynamic_cast<mitk::Image*>(mitk::IOUtil::Load(scalar_map)[0].GetPointer());
      if (mitk_map->GetDimension()==3)
      {
        FloatImageType::Pointer itk_map = FloatImageType::New();
        mitk::CastToItkImage(mitk_map, itk_map);
        clusterer->SetScalarMap(itk_map);
      }
    }
    clusterer->SetMergeDuplicateThreshold(merge_clusters);
    clusterer->SetScale(map_scale);
    clusterer->SetNumPoints(fiber_points);
    clusterer->SetMaxClusters(max_clusters);
    clusterer->SetMinClusterSize(min_fibers);
    clusterer->Update();
    std::vector<mitk::FiberBundle::Pointer> tracts = clusterer->GetOutTractograms();
    std::vector<mitk::FiberBundle::Pointer> centroids = clusterer->GetOutCentroids();

    MITK_INFO << "Saving clusters";
    std::streambuf *old = cout.rdbuf(); // <-- save
    std::stringstream ss;
    std::cout.rdbuf (ss.rdbuf());       // <-- redirect
    unsigned int c = 0;
    for (auto f : tracts)
    {
      mitk::IOUtil::Save(f, out_root + "Cluster_" + boost::lexical_cast<std::string>(c) + ".fib");

      if (output_centroids)
        mitk::IOUtil::Save(centroids.at(c), out_root + "Centroid_" + boost::lexical_cast<std::string>(c) + ".fib");
      ++c;
    }
    std::cout.rdbuf (old);              // <-- restore

  }
  catch (itk::ExceptionObject e)
  {
    std::cout << e;
    return EXIT_FAILURE;
  }
  catch (std::exception e)
  {
    std::cout << e.what();
    return EXIT_FAILURE;
  }
  catch (...)
  {
    std::cout << "ERROR!?!";
    return EXIT_FAILURE;
  }
  return EXIT_SUCCESS;
}


#include <boost/range/istream_range.hpp>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/algorithm_ext.hpp>
#include <iostream>
#include <unordered_map>

using namespace boost;
using namespace boost::adaptors;

template <typename C, typename T = typename C::value_type>
std::unordered_map<T, unsigned int> count_occurences(const C &collection) {
  std::unordered_map<T, unsigned int> counts{};
  counts.reserve(collection.size() * 5 / 3);
  for_each(collection, [&counts](const T& elem) {
    unsigned int new_count = 1;
    auto search = counts.find(elem);
    if (search != counts.end()) {
      const auto current_count = search->second;
      new_count += current_count;
    }
    counts[elem] = new_count;
  });
  return counts;
}

template <typename C,
          typename T = typename C::value_type,
          typename CIter = typename C::const_iterator,
          typename Iter = typename C::iterator>
class collection_append {
private: C &collection;
public:
  using value_type = T;
  using const_iterator = CIter;
  using iterator = Iter;
  explicit collection_append(C &a_collection) : collection(a_collection) {}
  collection_append() = delete;
  collection_append(const collection_append&) = delete;
  collection_append(collection_append&&) = delete;
  ~collection_append() = default;
  collection_append&operator =(const collection_append&) = delete;
  collection_append&operator =(collection_append&&) = delete;

  collection_append& append(T &&item) {
    this->collection.emplace(this->collection.end(), std::move(item));
    return *this;
  }

  template <typename _InputIterator, typename = std::_RequireInputIter<_InputIterator>>
  void insert(iterator __position, _InputIterator __first, _InputIterator __last) {
    this->collection.insert(__position, __first, __last);
  }

  constexpr inline iterator begin() const noexcept {
    return this->collection.begin();
  }

  constexpr inline iterator end() const noexcept {
    return this->collection.end();
  }
};

template <typename C>
void print_collection(const C& collection) {
  for(const auto &elem : collection) {
    printf("%d: %s\n", elem.first, elem.second.c_str());
  }
}

int main() {
  auto const is_alpha_word = [](const std::string &word) {
    for(const char c : word) {
      if (std::isalpha(c) == 0) { return false; }
    }
    return true;
  };
  auto const to_lower_case = [](const std::string &word) {
    std::string word_copy{word};
    std::transform(word_copy.begin(), word_copy.end(), word_copy.begin(), ::tolower);
    return word_copy;
  };
  auto const flip_pair = [](const std::pair<std::string, unsigned int> &entry) {
    return std::make_pair(entry.second, entry.first);
  };
  using count_pair_t = std::pair<unsigned int, std::string>;
  auto const compare_counts = [](const count_pair_t &x, const count_pair_t &y) {
    return x.first > y.first;
  };
  auto const compare_words = [](const count_pair_t &x, const count_pair_t &y) {
    return x.second.compare(y.second) < 0;
  };
  auto const found_pred = [](const count_pair_t &arg1, const count_pair_t &arg2) { return arg1.first == arg2.first; };

  std::vector<std::string> words;
  words.reserve(64 * 1024);
  collection_append<decltype(words)> add_words{words};
  push_back(add_words,
            istream_range<std::string>(std::cin)
            | filtered(is_alpha_word)
            | transformed(to_lower_case)
  );

  auto counts_map = count_occurences(words);

  std::vector<count_pair_t> count_pairs;
  count_pairs.reserve(counts_map.size());
  std::set<unsigned int> just_counts;
  auto const collect_just_counts = [&just_counts](const count_pair_t &item) { just_counts.emplace(item.first); return true; };
  sort(push_back(count_pairs, counts_map | transformed(flip_pair) | filtered(collect_just_counts)), compare_counts);

  const auto just_counts_rng = adaptors::reverse(make_iterator_range(just_counts.begin(), just_counts.end()));

  std::array<count_pair_t, 1> the_search_item{ count_pair_t{0, std::string{}} };
  auto current = count_pairs.begin();
  const auto very_end = count_pairs.end();
  int check_count = 0, sub_rng_count = 0;
  for(const unsigned int n : just_counts_rng) {
    check_count++;
    the_search_item[0].first = n;
    current = std::find_first_of(current, very_end, the_search_item.begin(), the_search_item.end(), found_pred);
    if (current != very_end) {
      const auto sub_rng_end = std::find_end(current, very_end, the_search_item.begin(), the_search_item.end(), found_pred);
      const auto sub_rng = make_iterator_range(current, sub_rng_end != very_end ? sub_rng_end + 1 : sub_rng_end);
      sort(sub_rng, compare_words);
      if (sub_rng_end != very_end) {
        current = sub_rng_end;
      }
      sub_rng_count++;
    } else {
      break;
    }
  }

  fputs("\nDEBUG: ", stderr);
  for(const unsigned int n : just_counts_rng) {
    fprintf(stderr, "%d, ", n);
  }
  putc('\n', stderr);

  fprintf(stderr, "\nDEBUG: set count: %lu, check count: %d, sub range count: %d\n\n",
          just_counts.size(), check_count, sub_rng_count);

  // main output
  print_collection(count_pairs);

  fputs("\nDEBUG:\n", stderr);
  auto rng2 = boost::unique(sort(words));
  copy(rng2, std::ostream_iterator<std::string>(std::cerr, "\n"));
}


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

#include <Eigen/Dense>
#include <chrono>  // NOLINT [build/c++11]
#include <iostream>
#include "get_valtonenornhag_arxiv_2021.hpp"
#include "relpose.hpp"

int main() {
    /* Timing experiments */
    int nbr_iter = 1e4;

    // Test fEf
    int N = 3;
    Eigen::MatrixXd x1(2, N);
    x1 = Eigen::MatrixXd::Random(2, N);
    Eigen::MatrixXd x2(2, N);
    x2 = Eigen::MatrixXd::Random(2, N);
    Eigen::Matrix3d R1;
    R1 = Eigen::Matrix3d::Random(3, 3);
    Eigen::Matrix3d R2;
    R2 = Eigen::Matrix3d::Random(3, 3);

    auto start = std::chrono::steady_clock::now();
    std::cout << "=== DronePoseLib timings ===" << std::endl;

    std::vector<DronePoseLib::RelPose> poses;

    for (int i = 0; i < nbr_iter; i++) {
        poses = DronePoseLib::ValtonenOrnhagArxiv2021::get_fEf(x1, x2, R1, R2);
    }

    auto end = std::chrono::steady_clock::now();
    std::cout << "Elapsed time (fEf): "
        << std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() / nbr_iter
        << " ns" << std::endl;

    // Test rEr
    double focal_length = 1.0;
    start = std::chrono::steady_clock::now();
    for (int i = 0; i < nbr_iter; i++) {
        poses = DronePoseLib::ValtonenOrnhagArxiv2021Extra::get_rEr(x1, x2, R1, R2, focal_length);
    }
    end = std::chrono::steady_clock::now();

    std::cout << "Elapsed time (rEr): "
        << std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() / nbr_iter
        << " ns" << std::endl;

    // Test frEfr
    N = 4;
    x1 = Eigen::MatrixXd::Random(2, N);
    x2 = Eigen::MatrixXd::Random(2, N);
    R1 = Eigen::Matrix3d::Random(3, 3);
    R2 = Eigen::Matrix3d::Random(3, 3);

    bool use_fast_solver = false;

    start = std::chrono::steady_clock::now();
    for (int i = 0; i < nbr_iter; i++) {
        poses = DronePoseLib::ValtonenOrnhagArxiv2021::get_frEfr(x1, x2, R1, R2, use_fast_solver);
    }
    end = std::chrono::steady_clock::now();

    std::cout << "Elapsed time (frEfr): "
        << std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() / nbr_iter
        << " μs" << std::endl;

    use_fast_solver = true;

    start = std::chrono::steady_clock::now();
    for (int i = 0; i < nbr_iter; i++) {
        poses = DronePoseLib::ValtonenOrnhagArxiv2021::get_frEfr(x1, x2, R1, R2, use_fast_solver);
    }
    end = std::chrono::steady_clock::now();

    std::cout << "Elapsed time (frEfr) fast: "
        << std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() / nbr_iter
        << " μs" << std::endl;

    return 0;
}


//==================================================================================================
/*
  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/meta/is_bitwise_logical.hpp>
#include <boost/simd/logical.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

using namespace boost::simd;

STF_CASE_TPL( "Check is_bitwise_logical", STF_NUMERIC_TYPES )
{
  STF_EXPECT( is_bitwise_logical_t<pack<T>>::value );
  STF_EXPECT( is_bitwise_logical_t<pack<logical<T>>>::value );
}


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

// Copyright (c) 2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#if defined(_MSC_VER)
#pragma warning( disable : 4305 ) // truncation double -> float
#endif // defined(_MSC_VER)

#define BOOST_GEOMETRY_SRS_ENABLE_STATIC_PROJECTION_HYBRID_INTERFACE

#include <geometry_test_common.hpp>

#include <boost/geometry/srs/projection.hpp>

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/algorithms/make.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

namespace srs = bg::srs;
namespace par = bg::srs::par4;

template <typename StaticProj, typename GeoPoint>
void test_forward(std::string const& id, GeoPoint const& geo_point1, GeoPoint const& geo_point2,
        std::string const& parameters, int deviation = 1)
{
    typedef typename bg::coordinate_type<GeoPoint>::type coordinate_type;
    typedef bg::model::d2::point_xy<coordinate_type> cartesian_point_type;
    typedef srs::projection<StaticProj> projection_type;

    try
    {
        // hybrid interface disabled by default
        // static_proj4 default ctor, dynamic parameters passed
        projection_type prj = srs::proj4(parameters);

        cartesian_point_type xy1, xy2;
        prj.forward(geo_point1, xy1);
        prj.forward(geo_point2, xy2);

        // Calculate distances in KM
        int const distance_expected = static_cast<int>(bg::distance(geo_point1, geo_point2) / 1000.0);
        int const distance_found = static_cast<int>(bg::distance(xy1, xy2) / 1000.0);

        int const difference = std::abs(distance_expected - distance_found);
        BOOST_CHECK_MESSAGE(difference <= 1 || difference == deviation,
                " id: " << id
                << " distance found: " << distance_found
                << " expected: " << distance_expected);

// For debug:
//        std::cout << projection_type::get_name() << " " << distance_expected
//            << " " << distance_found
//            << " " << (difference > 1 && difference != deviation ? " *** WRONG ***" : "")
//            << " " << difference
//            << std::endl;
    }
    catch(bg::projection_exception const& e)
    {
        std::cout << "Exception in " << id << " : " << e.code() << std::endl;
    }
    catch(...)
    {
        std::cout << "Exception (unknown) in " << id << std::endl;
    }
}

template <typename Proj, typename Model, typename GeoPoint>
void test_forward(std::string const& id, GeoPoint const& geo_point1, GeoPoint const& geo_point2,
        std::string const& parameters, int deviation = 1)
{
    typedef srs::static_proj4<par::proj<Proj>, par::ellps<Model> > static_proj4;

    test_forward<static_proj4>(id, geo_point1, geo_point2, parameters, deviation);
}

template <typename Proj, typename Model, typename OProj, typename GeoPoint>
void test_forward(std::string const& id, GeoPoint const& geo_point1, GeoPoint const& geo_point2,
        std::string const& parameters, int deviation = 1)
{
    typedef srs::static_proj4<par::proj<Proj>, par::ellps<Model>, par::o_proj<OProj> > static_proj4;

    test_forward<static_proj4>(id, geo_point1, geo_point2, parameters, deviation);
}

template <typename T>
void test_all()
{
    typedef bg::model::point<T, 2, bg::cs::geographic<bg::degree> > geo_point_type;

    geo_point_type amsterdam = bg::make<geo_point_type>(4.8925, 52.3731);
    geo_point_type utrecht   = bg::make<geo_point_type>(5.1213, 52.0907);

    geo_point_type anchorage = bg::make<geo_point_type>(-149.90, 61.22);
    geo_point_type juneau    = bg::make<geo_point_type>(-134.42, 58.30);

    geo_point_type auckland   = bg::make<geo_point_type>(174.74, -36.84);
    geo_point_type wellington = bg::make<geo_point_type>(177.78, -41.29);

    geo_point_type aspen  = bg::make<geo_point_type>(-106.84, 39.19);
    geo_point_type denver = bg::make<geo_point_type>(-104.88, 39.76);

    // IGH (internally using moll/sinu)
    test_forward<par::igh, par::sphere>("igh-au", amsterdam, utrecht, "+ellps=sphere +units=m", 5);
    test_forward<par::igh, par::sphere>("igh-ad", aspen, denver, "+ellps=sphere +units=m", 3);
    test_forward<par::igh, par::sphere>("igh-aw", auckland, wellington, "+ellps=sphere +units=m", 152);
    test_forward<par::igh, par::sphere>("igh-aj", anchorage, juneau, "+ellps=sphere +units=m", 28);

    // Using moll
    //test_forward<par::ob_tran_oblique, par::WGS84>("obto-au", amsterdam, utrecht, "+ellps=WGS84 +units=m +o_proj=moll +o_lat_p=10 +o_lon_p=90 +o_lon_o=11.50", 4);
    //test_forward<par::ob_tran_transverse, par::WGS84>("obtt-au", amsterdam, utrecht, "+ellps=WGS84 +units=m +o_proj=moll +o_lat_p=10 +o_lon_p=90 +o_lon_o=11.50", 5);
    //test_forward<par::ob_tran, par::WGS84, par::moll>("obt-au", amsterdam, utrecht, "+ellps=WGS84 +units=m +o_proj=moll +o_lat_p=10 +o_lon_p=90 +o_lon_o=11.50", 4);
    test_forward<par::ob_tran, par::WGS84, par::moll>("obt-au", amsterdam, utrecht, "+units=m +o_lat_p=10 +o_lon_p=90 +o_lon_o=11.50", 4);
    //test_forward<par::ob_tran_oblique, par::WGS84>("obto-ad", aspen, denver, "+ellps=WGS84 +units=m +o_proj=moll +o_lat_p=10 +o_lon_p=90 +o_lon_o=11.50", 19);
    //test_forward<par::ob_tran_transverse, par::WGS84>("obtt-ad", aspen, denver, "+ellps=WGS84 +units=m +o_proj=moll +o_lat_p=10 +o_lon_p=90 +o_lon_o=11.50", 19);
    //test_forward<par::ob_tran, par::WGS84, par::moll>("obt-ad", aspen, denver, "+ellps=WGS84 +units=m +o_proj=moll +o_lat_p=10 +o_lon_p=90 +o_lon_o=11.50", 19);
    test_forward<par::ob_tran, par::WGS84, par::moll>("obt-ad", aspen, denver, "+units=m +o_lat_p=10 +o_lon_p=90 +o_lon_o=11.50", 19);

    // Using sinu
    //test_forward<par::ob_tran_oblique, par::WGS84>("obto-au", amsterdam, utrecht, "+ellps=WGS84 +units=m +o_proj=sinu +o_lat_p=10 +o_lon_p=90 +o_lon_o=11.50", 5);
    //test_forward<par::ob_tran_transverse, par::WGS84>("obtt-au", amsterdam, utrecht, "+ellps=WGS84 +units=m +o_proj=sinu +o_lat_p=10 +o_lon_p=90 +o_lon_o=11.50", 4);
    //test_forward<par::ob_tran, par::WGS84, par::sinu>("obt-au", amsterdam, utrecht, "+ellps=WGS84 +units=m +o_proj=sinu +o_lat_p=10 +o_lon_p=90 +o_lon_o=11.50", 5);
    test_forward<par::ob_tran, par::WGS84, par::sinu>("obt-au", amsterdam, utrecht, "+units=m +o_lat_p=10 +o_lon_p=90 +o_lon_o=11.50", 5);
    //test_forward<par::ob_tran_oblique, par::WGS84>("obto-ad", aspen, denver, "+ellps=WGS84 +units=m +o_proj=sinu +o_lat_p=10 +o_lon_p=90 +o_lon_o=11.50", 14);
    //test_forward<par::ob_tran_transverse, par::WGS84>("obtt-ad", aspen, denver, "+ellps=WGS84 +units=m +o_proj=sinu +o_lat_p=10 +o_lon_p=90 +o_lon_o=11.50", 6);
    //test_forward<par::ob_tran, par::WGS84, par::sinu>("obt-ad", aspen, denver, "+ellps=WGS84 +units=m +o_proj=sinu +o_lat_p=10 +o_lon_p=90 +o_lon_o=11.50", 14);
    test_forward<par::ob_tran, par::WGS84, par::sinu>("obt-ad", aspen, denver, "+units=m +o_lat_p=10 +o_lon_p=90 +o_lon_o=11.50", 14);

}

int test_main(int, char* [])
{
    test_all<double>();

    return 0;
}


#include "calibrate.hpp"
#include "extract.hpp"
#include "parse.hpp"

#include <cstdint>
#include <fstream>
#include <iostream>
#include <sstream>
#include <vector>

#include <Eigen/Geometry>
#include <ProgramOptions.hxx>
#include <fmt/core.h>

Eigen::Quaternionf parse_calib(std::string const& calib) {
	Eigen::Quaternionf quat;

	std::istringstream ss(calib);
	int index = 0;
	float f;
	while (ss >> f) {
		// Eigen stores quaternion as xyzw internally for some reason
		quat.coeffs()((index + 3) % 4) = f;
		if (ss.peek() == ',')
			ss.ignore();

		++index;
		if (index >= 4)
			break;
	}

	ss.peek();
	if (index != 4 || !ss.eof()) {
		throw std::invalid_argument(
		    "Invalid calib string. Should be 4 comma separated floats.");
	}

	return quat;
}

std::string serialize_calib(Eigen::Quaternionf const& calib) {
	return fmt::format("{:.6f},{:.6f},{:.6f},{:.6f}",
	                   calib.w(), calib.x(), calib.y(), calib.z());
}

void show_session_detail(std::ifstream& recording_infile,
                         int show_session_index, int nr_skip_measurements) {
	log_entry_types entry_type;

	int session_index = -1;
	int photos_in_session = 0;
	std::uint32_t last_photo_time = 0;

	std::vector<Eigen::Vector3f> measurements;
	measurements.reserve(26);

	while (read_next_entry(recording_infile, entry_type)) {
		switch (entry_type) {
		case log_entry_types::photo_event: {
			const auto photo_time = parse_photo(recording_infile).millis;
			if (measurements.size() < 3) {
				measurements.clear();
				break;
			}
			if (session_index == show_session_index) {
				fmt::print("{: >5d}   {: >12d}  {: >9.2f}", photos_in_session,
				           measurements.size(), photo_time / 1000.);

				if (photos_in_session > 0)
					fmt::print("   {: >19.2f}    ",
					           (photo_time - last_photo_time) / 1000.);
				else
					fmt::print("   {: >19}    ", "-");

				Eigen::Vector3f g = Eigen::Vector3f::Zero();
				const int nr_relevant_measurements =
				    measurements.size() - nr_skip_measurements;
				for (int i = 0; i < nr_relevant_measurements; ++i)
					g += measurements[i];
				g.normalize();

				for (int i = 0; i < 3; ++i) {
					fmt::print("{: >7.3f}", g(i));
				}
				fmt::print("\n");

				last_photo_time = photo_time;
				++photos_in_session;
			}
			measurements.clear();
			break;
		}
		case log_entry_types::startup: {
			parse_startup(recording_infile);
			photos_in_session = 0;
			last_photo_time = 0;
			measurements.clear();
			++session_index;
			if (session_index == show_session_index) {
				fmt::print("Session {0}\nphoto   measurements   time [s]   "
				           "time since last [s]   normalized avg gravity\n",
				           show_session_index);
			}
			break;
		}
		case log_entry_types::gravity_vector:
			measurements.push_back(parse_gravity(recording_infile).g);
			break;
		}

		if (session_index > show_session_index)
			break;
	}
}

void list_sessions(std::ifstream& recording_infile) {
	log_entry_types entry_type;

	int session_index = -1;
	int photos_in_session = 0;
	std::uint32_t last_photo_time_in_session;

	const auto print = [&]() {
		fmt::print("{: >7d}   {: >6d}", session_index, photos_in_session);
		if (photos_in_session > 0)
			fmt::print("  {: >9.2f}\n", last_photo_time_in_session / 1000.);
		else
			fmt::print("\n");
	};

	fmt::print("session   photos   time [s]\n");
	while (read_next_entry(recording_infile, entry_type)) {
		switch (entry_type) {
		case log_entry_types::photo_event:
			last_photo_time_in_session = parse_photo(recording_infile).millis;
			++photos_in_session;
			break;
		case log_entry_types::startup:
			parse_startup(recording_infile);

			if (session_index >= 0) {
				print();
			}

			photos_in_session = 0;
			last_photo_time_in_session = 0;
			++session_index;
			break;
		case log_entry_types::gravity_vector:
			parse_gravity(recording_infile);
			break;
		}
	}

	if (session_index >= 0) {
		print();
	}
}

int main(int argc, char **argv) {
	po::parser parser;

	auto& help_option = parser["help"]
	    .abbreviation('h')
	    .description("Print this help screen");

	std::string recording_path = "/run/media/pascal/ARDUINO/GRAVITY.TXT";
	parser["file"]
	    .bind(recording_path)
	    .abbreviation('f')
	    .description("Path to the recorded 'GRAVITY.TXT' file.");

	int nr_of_ignored_measurements = 5;
	parser["ignore"]
	    .bind(nr_of_ignored_measurements)
	    .abbreviation('i')
	    .description("Number of gravity measurements to drop just before every "
	                 "photo. The ideal value depends on the camera model and "
	                 "settings (default: 5).");

	int x_session_index = -1;
	auto& x_session_index_option = parser["x-session-index"]
	    .bind(x_session_index)
	    .abbreviation('x')
	    .description(
	        "Index of session to use for calibrating x axis. See --list for "
	        "indices.");

	int y_session_index = -1;
	auto& y_session_index_option = parser["y-session-index"]
	    .bind(y_session_index)
	    .abbreviation('y')
	    .description(
	        "Index of session to use for calibrating y axis. See --list "
	        "for indices.");

	auto& list_option = parser["list"]
	    .abbreviation('l')
	    .description("List all sessions in the recording file and exit.");

	int show_session_index;
	auto& show_session_option = parser["session"]
	    .bind(show_session_index)
	    .abbreviation('s')
	    .description("Show this session in detail and exit.");

	std::string calibration = "1,0,0,0";
	parser["calibration"]
	    .bind(calibration)
	    .abbreviation('c')
	    .description(
	        "Calibration to apply to the measurements in extract-session.");

	int extract_session_index;
	auto& extract_session_option = parser["extract-session"]
	    .bind(extract_session_index)
	    .abbreviation('e')
	    .description("Extract the data from this session. Needs to be used "
	                 "together with extract-destination.");

	std::string extract_destination;
	auto& extract_destination_option = parser["extract-destination"]
	    .bind(extract_destination)
	    .abbreviation('d')
	    .description("Path to which to extract the gravity vectors. Needs to "
	                 "be used together with extract-session.");

	if (!parser(argc, argv))
		return EXIT_FAILURE;

	if (help_option.was_set()) {
		std::cerr << parser << '\n';
		return EXIT_SUCCESS;
	}

	std::ifstream recording_infile(recording_path, std::ios_base::binary);
	if (!recording_infile) {
		std::cerr << "Could not find recorded file.\n";
		return EXIT_FAILURE;
	}

	if (list_option.was_set()) {
		std::cout << "Listing all sessions in " << recording_path << "\n\n";

		list_sessions(recording_infile);
		return EXIT_SUCCESS;
	}

	if (show_session_option.was_set()) {
		show_session_detail(recording_infile, show_session_index,
		                    nr_of_ignored_measurements);
		return EXIT_SUCCESS;
	}

	if (x_session_index_option.was_set() || y_session_index_option.was_set()) {
		std::cout << "Running Calibration\n\n";

		const Eigen::Quaternionf computed_calib =
		    calibrate(recording_infile, x_session_index, y_session_index,
		              nr_of_ignored_measurements);

		std::cout << "Calibration: " << serialize_calib(computed_calib) << '\n';

		return EXIT_SUCCESS;
	}

	if (extract_session_option.was_set() &&
	    extract_destination_option.was_set()) {
		std::cout << "Extracting data from session " << extract_session_index
		          << "\n\n";

		extract(recording_infile, extract_session_index, extract_destination,
		        nr_of_ignored_measurements, parse_calib(calibration));

		return EXIT_SUCCESS;
	}

	std::cerr << parser << '\n';
}


/*
This file is part of the tug model based diagnosis software for robots
Copyright (c) 2015, Clemens Muehlbacher, Stefan Loigge
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 copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

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

#include <tug_velocity_observer/VelocityConverterTwistStamped.h>
#include <tug_yaml/ProcessYaml.h>
#include <boost/make_shared.hpp>
#include <tug_observer_plugin_utils/differentiation/SimpleLinearRegression.h>
#include <string>

VelocityConverterTwistStamped::VelocityConverterTwistStamped(XmlRpc::XmlRpcValue params,
                                                             boost::function<void(MovementReading)> call_back)
        : VelocityConverter(params, call_back)
{
  unsigned int window_size = ProcessYaml::getValue<unsigned int>("acceleartion_window_size", params, 7);
  x_acceleration_calc_ = boost::make_shared<SimpleLinearRegression<double> >(window_size);
  y_acceleration_calc_ = boost::make_shared<SimpleLinearRegression<double> >(window_size);
  z_acceleration_calc_ = boost::make_shared<SimpleLinearRegression<double> >(window_size);
}

VelocityConverterTwistStamped::VelocityConverterTwistStamped(XmlRpc::XmlRpcValue params,
                                                             boost::function<void(MovementReading)> call_back,
                                                             SubscriberFacade *plugin_base) :
                                                                              VelocityConverter(params, call_back)
{
  topic_ = ProcessYaml::getValue<std::string>("topic", params);
  sub_ = plugin_base->subscribe(topic_, 1, &VelocityConverterTwistStamped::TwistStampedCB, this);

  std::string name = getName();
  if (name.find('/') == 0)
    name = name.substr(1, name.size());
  if (name.find_last_of('/') == (name.size() - 1))
    name = name.substr(0, name.size() - 1);
  twist_pub_ = nh_.advertise<geometry_msgs::TwistStamped>(name + "_twist", 10);
  movement_pub_ = nh_.advertise<sensor_msgs::Imu>(name + "_movement", 10);

  unsigned int window_size = ProcessYaml::getValue<unsigned int>("acceleartion_window_size", params, 7);
  x_acceleration_calc_ = boost::make_shared<SimpleLinearRegression<double> >(window_size);
  y_acceleration_calc_ = boost::make_shared<SimpleLinearRegression<double> >(window_size);
  z_acceleration_calc_ = boost::make_shared<SimpleLinearRegression<double> >(window_size);
}

void VelocityConverterTwistStamped::TwistStampedCB(const geometry_msgs::TwistStamped &msg)
{
  ROS_DEBUG_STREAM(
          "got twist stamped callback with velocity linear x:" << msg.twist.linear.x << " y:" << msg.twist.linear.y <<
          " z:" << msg.twist.linear.z
          << " angular x:" << msg.twist.angular.x << " y:" << msg.twist.angular.y << " z:" << msg.twist.angular.z
          << " at time sec:" << msg.header.stamp.sec << " nsec:" << msg.header.stamp.nsec);

  x_acceleration_calc_->addValue(msg.twist.linear.x, msg.header.stamp);
  y_acceleration_calc_->addValue(msg.twist.linear.y, msg.header.stamp);
  z_acceleration_calc_->addValue(msg.twist.linear.z, msg.header.stamp);

  if (x_acceleration_calc_->hasDifferentiation() && y_acceleration_calc_->hasDifferentiation() &&
      z_acceleration_calc_->hasDifferentiation())
  {
    ros::Time plot_time = ros::Time(ros::WallTime::now().toSec());
    geometry_msgs::TwistStamped new_msg;
    new_msg.twist = msg.twist;
    new_msg.header.stamp = plot_time;

    MovementReading reading;
    reading.plot_time = plot_time;
    reading.reading_time = msg.header.stamp;
    reading.linear.x = x_acceleration_calc_->getDifferentiation();
    reading.linear.y = y_acceleration_calc_->getDifferentiation();
    reading.linear.z = z_acceleration_calc_->getDifferentiation();
    reading.angular.x = msg.twist.angular.x;
    reading.angular.y = msg.twist.angular.y;
    reading.angular.z = msg.twist.angular.z;

    twist_pub_.publish(new_msg);
    movement_pub_.publish(reading.toIMUMsg());

    sendMovement(reading);
  }
}

std::string VelocityConverterTwistStamped::getName()
{
  return topic_;
}


#include "ofApp.h"
#include <boost/variant.hpp>

//
// External events, which are mainly assumed as user operations via javascript.
//
namespace ext_event {
struct set_solution {
  string solution;
};

struct set_input {
  string input;
};

struct set_input_and_solution {
  string input;
  string solution;
};

using info = boost::variant<
  set_solution,
  set_input,
  set_input_and_solution
  >;

queue<info> queue;
mutex queue_mutex;

void push(info i) {
  lock_guard<mutex> l(queue_mutex);
  queue.push(i);
}

void push_set_solution(const string &txt) {
  ext_event::queue.push(ext_event::set_solution{txt});
}

void push_set_input(const string &txt) {
  ext_event::queue.push(ext_event::set_input{txt});
}

void push_set_input_and_solution(const string &input, const string &solution) {
  ext_event::queue.push(ext_event::set_input_and_solution{input, solution});
}
}  // namespace ext_event

#ifdef EMSCRIPTEN
#include <emscripten.h>
#include <emscripten/bind.h>

EMSCRIPTEN_BINDINGS(my_module) {
  emscripten::function("setSolution", &ext_event::push_set_solution);
  emscripten::function("setInput", &ext_event::push_set_input);
  emscripten::function("setInputAndSolution", &ext_event::push_set_input_and_solution);
}
#endif

//
// Official output checker
//
namespace chokudai {
int output_checker(string in_str, string out_str) {
  const int N = 30;

  try{
    // ifstream inp(argv[1]);
    // ifstream ans(argv[3]);
    // ifstream out(argv[2]);
    istringstream inp(in_str);
    istringstream out(out_str);
    int num[N][N] = {};

    int sum = 0;
    for (int i = 0; i < N; i++)
    {
      for (int j = 0; j < N; j++)
      {
        inp >> num[i][j];
        sum += num[i][j];
      }
    }

    int prey = -2;
    int prex = -2;
    int prenum = -1;

    int cnt = 0;
    for (int i = 0; i < sum; i++)
    {
      int y, x;
      out >> y >> x;
      y--; x--;
      if (y < 0 || x < 0 || y >= N || x >= N) continue;

      int d = abs(prey - y) + abs(prex - x);
      if (d != 1 || prenum != num[y][x] + 1) cnt++;

      prey = y;
      prex = x;
      prenum = num[y][x];
      num[y][x]--;
    }

    for (int i = 0; i < N; i++)
    {
      for (int j = 0; j < N; j++)
      {
        if (num[i][j] != 0) return -1;
      }
    }

    int point = 100000 - cnt;

    // printf("IMOJUDGE<<<%d>>>\n", point);
    //cout << "ok" << endl;
    return point;
  }
  catch(char* str){
	cerr << "error: " << str << endl;
    return -1;
  }
}
}  // namespace chokudai

//
// Global variables
//
namespace {
const int N = 30;
vector<vector<int>> A;
string A_str;
vector<pair<int, int>> sol;
vector<pair<int, int>> path;

int scale = 20;
int step;
}  // namespace

void set_input(const string &txt) {
  A.assign(N, vector<int>(N));
  istringstream ss(txt);
  for (int y = 0; y < N; ++y) {
    for (int x = 0; x < N; ++x) {
      if (!(ss >> A[y][x])) {
        cerr << "Error: input too short" << endl;
        return;
      }
    }
  }
  A_str = txt;
  // TODO: check EOF
}

void set_solution(const string &txt) {
  {
    int score = chokudai::output_checker(A_str, txt);
    printf("Score: %d\n", score);
    if (score < 0) return;  // Invalid solution
  }
  {
    istringstream ss(txt);
    sol.clear();
    for (int x, y; ss >> y >> x; ) {
      sol.emplace_back(x - 1, y - 1);
    }
  }
}

void move_step(int target) {
  while (step < target && step < (int)sol.size()) {
    --A[sol[step].second][sol[step].first];
    ++step;
  }
  while (step > target) {
    --step;
    ++A[sol[step].second][sol[step].first];
  }
}

void ofApp::setup(){
  if (A.empty()) {
    ifstream ifs("data/sample_input.txt");
    assert(ifs);
    set_input(string((std::istreambuf_iterator<char>(ifs)),
                     std::istreambuf_iterator<char>()));
  }

  ofSetFrameRate(30);
  ofBackground(255, 255, 255);

  ofTrueTypeFont::setGlobalDpi(72);
  font.load("verdana.ttf", 12, true, true);

  gui.setup();
  gui.add(step_slider.setup("step", 0, 0, sol.size()));
  gui.add(speed_slider.setup("speed", 2.0, -1, 3));
  gui.add(history_slider.setup("history", 100, 1, 1000));
  gui.setPosition(N * scale, 0);

  ofSetWindowShape(N * scale + gui.getWidth(), N * scale);

  step = 0;
}

struct ext_event_handler : boost::static_visitor<void> {
  ext_event_handler(ofApp &app) : app_(app) {}

  void operator()(ext_event::set_solution &t) const {
    move_step(0);
    set_solution(t.solution);
    app_.step_slider.setMax(sol.size());
    app_.step_slider = 0;
  }

  void operator()(ext_event::set_input &t) const {
    move_step(0);
    set_input(t.input);
    set_solution("");
    app_.step_slider.setMax(sol.size());
    app_.step_slider = 0;
  }

  void operator()(ext_event::set_input_and_solution &t) const {
    move_step(0);
    set_input(t.input);
    set_solution(t.solution);
    app_.step_slider.setMax(sol.size());
    app_.step_slider = 0;
  }

  template<typename T>
  void operator()(T &t) const {
    assert(false);
  }

  ofApp &app_;
};

void ofApp::update(){
  // External events
  {
    lock_guard<mutex> l(ext_event::queue_mutex);

    while (!ext_event::queue.empty()) {
      ext_event::info i = ext_event::queue.front();
      ext_event::queue.pop();
      boost::apply_visitor(ext_event_handler(*this), i);
    }
  }

  // Proceed
  if ((int)step_slider < (int)sol.size()) {
    step_slider = min((float)sol.size(), step_slider + pow(10, speed_slider));
  }
  move_step((int)step_slider);
}

void ofApp::draw(){
  ofSetColor(0, 0, 0);

  ofSetLineWidth(1);
  for (int y = 0; y <= N; ++y) ofDrawLine(0, y * scale, N * scale, y * scale);
  for (int x = 0; x <= N; ++x) ofDrawLine(x * scale, 0, x * scale, N * scale);

  for (int y = 0; y < N; ++y) {
    for (int x = 0; x < N; ++x) {
      ofSetColor(ofColor::fromHsb(int((100 - A[y][x]) * 1.7), 255, 255));
      ofDrawRectangle(x * scale, y * scale, scale, scale);
      ofSetColor(0, 0, 0);
      font.drawString(to_string(A[y][x]), x * scale, (y + 1) * scale);
    }
  }

  for (int i = 0; i < history_slider; ++i) {
    int s = step - 1 - i;
    if (s < 0) break;
    int x = sol[s].first, y = sol[s].second;

    ofSetColor(0, 0, 0, 255 * (history_slider - i) / history_slider);
    ofDrawCircle((x + 0.5) * scale, (y + 0.5) * scale, 3);

    if (s > 1) {
      int tx = sol[s - 1].first, ty = sol[s - 1].second;
      if (abs(tx - x) + abs(ty - y) == 1 && A[ty][tx] == A[y][x] + 1) {
        ofVec2f p1((x + 0.5) * scale, (y + 0.5) * scale);
        ofVec2f p2((tx + 0.5) * scale, (ty + 0.5) * scale);
        ofVec2f pm = (p1 + p2) / 2.0;
        ofVec2f v = p1 - p2;

        ofSetLineWidth(max(1, scale / 10));
        ofDrawLine(p1, p2);
        ofDrawLine(pm, pm + v.getRotated(135) * 0.3);
        ofDrawLine(pm, pm + v.getRotated(-135) * 0.3);
      }
    }
  }

  gui.draw();
}

void ofApp::windowResized(int w, int h){
  scale = min((int)((w - gui.getWidth()) / N), h / N);
  gui.setPosition(N * scale, 0);
  font.load("verdana.ttf", scale / 2, true, true);
}

void ofApp::keyPressed(int key){}
void ofApp::keyReleased(int key){}
void ofApp::mouseMoved(int x, int y){}
void ofApp::mouseDragged(int x, int y, int button){}
void ofApp::mousePressed(int x, int y, int button){}
void ofApp::mouseReleased(int x, int y, int button){}
void ofApp::mouseEntered(int x, int y){}
void ofApp::mouseExited(int x, int y){}
void ofApp::gotMessage(ofMessage msg){}
void ofApp::dragEvent(ofDragInfo dragInfo){}


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is licensed under the Apache License,
//  Version 2.0 (the "License"); you may not use this file except in
//  compliance with the License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <string>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <cassert>
#include <list>
#include <map>
#include <algorithm>
#include <utility>

#include <cairomm/context.h>
#include <cairomm/surface.h>

//#include <boost/algorithm/minmax_element.hpp>
#include <boost/program_options.hpp>

#include <vw/Core/Log.h>
#include <vw/Core/ProgressCallback.h>
#include <vw/Image.h>
#include <vw/FileIO.h>

#include <cmath>

#include "contour.h"

namespace po = boost::program_options;

// fwd declaration of function in FitCurves.cpp
BezierContour FitCurve(PointContour &contour, double error);

vw::ImageView<float> load_dem_from_file(std::string file_in) {
    //    Open the file for reading
    vw::ImageView<float> dem;
    vw::DiskImageResource *r = vw::DiskImageResource::open( file_in );
    vw::vw_out(vw::InfoMessage) << "Loading DEM from file" << std::endl;
    vw::vw_out(vw::DebugMessage) << r->rows()
        << "x" << r->cols() << "x" << r->planes()
        << "  " << r->channels() << " channel(s)"
        << " type: " << r->type()
        << " pxfmt: " << r->pixel_format()
        << " chtype: " << r->channel_type()
        << "\n";
    //    Read the data
    //tpc.set_progress_text("Status (loading image):   " );
    vw::read_image(dem, *r);
    delete r;
    return dem;
}

void read_segments_from_file(SegmentList &segment_list,
        std::string file_in, int &rows, int &cols)
{
    vw::vw_out(vw::InfoMessage)
        << "Reading contour segments from file: " << file_in << "\t" << std::endl;
    std::ifstream ifs;
    ifs.open(file_in.c_str(), std::ifstream::in);

    // first line should have image dimensions
    if (ifs.good()) ifs >> std::skipws >> rows >> cols;

    // rest of file should have segment coordinates and level
    while (ifs.good()) {
        float ax,ay,bx,by;
        int level;
        ContourSegment seg;
        ifs >> std::skipws >> ax >> ay >> bx >> by >> level;
        seg.a = ContourPoint(ax,ay);
        seg.b = ContourPoint(bx,by);
        seg.level = level;
        segment_list.push_back(seg);
    }
    ifs.close();
    vw::vw_out(vw::DebugMessage)
        << "\tRead " << segment_list.size() << " segments" << std::endl;
}

void load_segments_into_pcs(SegmentList &segment_list, PointContourSet &cset) {
    SegmentList::iterator seglist_iter;

    // Load segments into PointContourSet
    vw::vw_out(vw::InfoMessage)
        << "Loading segments into PointContourSet" << std::endl;

    for (seglist_iter = segment_list.begin();
         seglist_iter != segment_list.end();
         seglist_iter++) {
        add_segment(cset, *seglist_iter);
    }
}

void fit_Bezier_curves_to_contours(PointContourSet &cset,
        BezierContourSet &bcset, float error)
{
    vw::vw_out(vw::InfoMessage)
        << "Fitting Bezier curves to contours" << std::endl;
    PointContourSet::iterator cset_iter;
    PointContour c;
    int level;

    for (cset_iter = cset.begin(); cset_iter != cset.end(); cset_iter++) {
        level = (*cset_iter).first;
        c = (*cset_iter).second;
        BezierContour bc = FitCurve(c, error);
        bcset.insert(make_pair(level, bc));
        /*
        std::cout << "adding contour: " << bc.size() << " elems" << std::endl;
        for (BezierContour::iterator bciter = bc.begin(); bciter != bc.end(); ++bciter) {
            for (int i = 0; i < 4; i++) {
                std::cout << "\t" << (*bciter)[i] << std::endl;
            }
        }
        */
    }
}

Cairo::RefPtr<Cairo::Context> create_png_surface(int dem_w, int dem_h,
        std::string image_file)
{
    vw::vw_out(vw::InfoMessage) << "Setting up PNG output surface" << std::endl;

    Cairo::RefPtr<Cairo::ImageSurface> surface =
        Cairo::ImageSurface::create_from_png(image_file);
    Cairo::RefPtr<Cairo::Context> cr = Cairo::Context::create(surface);

    int img_w = surface->get_width();
    int img_h = surface->get_height();
    double scale_w = double(img_w)/double(dem_w);
    double scale_h = double(img_h)/double(dem_h);
    cr->scale(scale_w, scale_h);

    vw::vw_out(vw::DebugMessage) <<
        "\tDEM dimensions: " << "(" << dem_w << "x" << dem_h << ")" << std::endl
        << "\tImg dimensions: " << "(" << img_w << "x" << img_h << ")" << std::endl
        << "\tScale factors:  " << "(" << scale_w << "x" << scale_h << ")" << std::endl;

    cr->save();
    return cr;
}

Cairo::RefPtr<Cairo::Context> create_svg_surface(int width_out, int height_out,
        std::string file_out)
{
    vw::vw_out(vw::InfoMessage) << "Setting up SVG output surface " << std::endl;

    Cairo::RefPtr<Cairo::SvgSurface> surface =
        Cairo::SvgSurface::create(file_out, width_out, height_out);
    Cairo::RefPtr<Cairo::Context> cr = Cairo::Context::create(surface);

    cr->save();
    cr->set_source_rgb(1.0, 1.0, 1.0);
    cr->paint();
    //cr->restore();
    cr->save();
    return cr;
}

Cairo::RefPtr<Cairo::Context>
create_output_surface(std::string output_type, int cols, int rows,
        std::string image_file, std::string file_out)
{
    Cairo::RefPtr<Cairo::Context> cr;
    if (output_type == "png") {
        // Create PNG surface
        cr = create_png_surface(cols, rows, image_file);
    } else {
        // Create SVG surface
        cr = create_svg_surface(cols, rows, file_out);
    }
    return cr;
}

void write_output_file(Cairo::RefPtr<Cairo::Context> cr,
        std::string output_type, std::string file_out)
{
    vw::vw_out(vw::InfoMessage) << "Writing output file: " << file_out << std::endl;

    if (output_type == "png") {
        cr->get_target()->write_to_png(file_out);
    } else {
        cr->show_page();
    }
}

void draw_point_contours(PointContourSet cset,
        Cairo::RefPtr<Cairo::Context> cr, float nodataval)
{
    vw::vw_out(vw::InfoMessage) << "Writing point contours to output surface\n";
    PointContourSet::iterator cset_iter;
    double line_width, tmp;
    line_width = 2.0; tmp = 0.0;
    cr->device_to_user_distance(line_width, tmp);
    cr->set_line_width(line_width);
    cr->set_source_rgb(1.0, 0.0, 0.0);

    int level = int(nodataval);
    for (cset_iter = cset.begin(); cset_iter != cset.end(); cset_iter++) {

        int newlevel = (*cset_iter).first;
        if (newlevel != level) {
            level = newlevel;
        }

        PointContour c = (*cset_iter).second;
        PointContour::iterator c_iter = c.begin();
        cr->begin_new_path();
        cr->move_to((*c_iter)[0],(*c_iter)[1]);
        while (++c_iter != c.end()) {
            cr->line_to((*c_iter)[0], (*c_iter)[1]);
        }
        cr->stroke();
    }
}

void draw_Bezier_contours(BezierContourSet bcset, Cairo::RefPtr<Cairo::Context> cr, float nodataval) {
    vw::vw_out(vw::InfoMessage) << "Writing Bezier contours to output surface\n";
    BezierContourSet::iterator bcset_iter;
    double line_width, tmp;
    line_width = 2.0; tmp = 0.0;
    //cr->device_to_user_distance(line_width, tmp);
    cr->set_line_width(line_width);
    cr->set_source_rgb(0.0, 0.0, 1.0);

    int level = int(nodataval);
    for (bcset_iter = bcset.begin(); bcset_iter != bcset.end(); bcset_iter++) {
        int newlevel = (*bcset_iter).first;
        if (newlevel != level) {
            level = newlevel;
        }

        BezierContour contour = (*bcset_iter).second;
        BezierContour::iterator c_iter = contour.begin();
        cr->begin_new_path();
        cr->move_to((*c_iter)[0][0], (*c_iter)[0][1]);
        while (++c_iter != contour.end()) {
            cr->curve_to((*c_iter)[1][0], (*c_iter)[1][1],
                         (*c_iter)[2][0], (*c_iter)[2][1],
                         (*c_iter)[3][0], (*c_iter)[3][1]);
        }
        cr->stroke();
    }
}

void write_points_to_file(std::string file_out, SegmentList segment_list, int rows, int cols) {
    vw::vw_out(vw::InfoMessage) << "Writing contour points to text file\n";
    SegmentList::iterator iter;
    std::ofstream ofs;
    ofs.open(file_out.c_str());
    ofs << rows << "\t" << cols << std::endl;

    for (iter = segment_list.begin(); iter != segment_list.end(); iter++) {
        ContourSegment s = *iter;
        ofs << std::setprecision(8) << std::fixed
            << s.a[0] << "\t" << s.a[1] << "\t" << s.b[0] << "\t"
            << s.b[1] << "\t" << s.level << std::endl;
    }
    ofs.close();
}

int main(int argc, char *argv[])
{
    bool verbose = false;
    std::string file_in;
    std::string file_out;

    // Parse command line options
    po::options_description opts("Allowed options");
    opts.add_options()
        ("help,?", "show help message")
        ("verbose,v", po::bool_switch(&verbose), "set verbose output")
        ("input-type,i", po::value<std::string>()->default_value("tiff"),
            "choose input type")
        ("image-file,m", po::value<std::string>()->default_value(""),
            "image file to write contours on")
        ("output-type,o", po::value<std::string>()->default_value("svg"),
            "choose output type")
        ("no-data-value,n", po::value<float>()->default_value(-10000.0),
            "set \"NO DATA\" value")
        ("contour-interval,c", po::value<int>()->default_value(100),
            "set contour interval")
    ;

    po::options_description hidden("Hidden options");
    hidden.add_options()
        ("input-file", po::value<std::string>(), "input file")
        ("output-file", po::value<std::string>(), "output file")
    ;

    po::options_description cmdline_options;
    cmdline_options.add(opts).add(hidden);

    po::positional_options_description p;
    p.add("input-file", 1);
    p.add("output-file", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).
            options(cmdline_options).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout << opts << std::endl;
        return 1;
    }

    if (vm.count("input-file")) {
        file_in = vm["input-file"].as<std::string>();
    } else {
        vw::vw_out(vw::ErrorMessage)
            << "ERROR: Must specify an input file" << std::endl;
        return 1;
    }

    if (vm.count("output-file")) {
        file_out = vm["output-file"].as<std::string>();
    } else {
        vw::vw_out(vw::ErrorMessage)
            << "ERROR: Must specify an output file" << std::endl;
        return 1;
    }

    std::string input_type = vm["input-type"].as<std::string>();
    std::string image_file = vm["image-file"].as<std::string>();
    std::string output_type = vm["output-type"].as<std::string>();
    float nodataval = vm["no-data-value"].as<float>();
    int cint = vm["contour-interval"].as<int>();

    vw::vw_log().console_log().rule_set().clear();
    vw::vw_log().console_log().rule_set().add_rule(vw::WarningMessage, "console");
    if (verbose) {
        vw::vw_log().console_log().rule_set().add_rule(vw::DebugMessage, "console");
    }

    if (output_type != "text" && output_type != "svg" && output_type != "png") {
        vw::vw_out(vw::ErrorMessage)
            << "ERROR: Output types other than \"text\", \"png\" or \"svg\" "
            << "not currently supported." << std::endl;
        return 1;
    }

    if (input_type != "tiff" && input_type != "text") {
        vw::vw_out(vw::ErrorMessage)
            << "ERROR: Input types other than \"tiff\" or \"text\" "
            << "not currently supported." << std::endl;
        return 1;
    }

    vw::vw_out(vw::DebugMessage) << "Input file: " << file_in << std::endl;
    vw::vw_out(vw::DebugMessage) << "Output file: " << file_out << std::endl;
    vw::vw_out(vw::DebugMessage) << "Input type: " << input_type << std::endl;
    vw::vw_out(vw::DebugMessage) << "Output type: " << output_type << std::endl;
    vw::vw_out(vw::DebugMessage) << "Image file: " << image_file << std::endl;
    vw::vw_out(vw::DebugMessage) << "No-data value: " << nodataval << std::endl;
    vw::vw_out(vw::DebugMessage) << "Contour interval: " << cint << std::endl;

    // done parsing options

    vw::ImageView<float> dem;
    //TerminalProgressCallback tpc();
    PointContourSet cset;
    SegmentList segment_list;
    int rows, cols;
    float error = 1.0e-3;

    if (input_type == "tiff") {
        // 1. Load DEM from file
        dem = load_dem_from_file(file_in);
        rows = dem.rows();
        cols = dem.cols();

        // 2. Run contouring algorithm on DEM
        conrec(dem, cset, cint, nodataval, segment_list);

    }
    else if (input_type == "text") {
        // read contour segments from file
        read_segments_from_file(segment_list, file_in, rows, cols);
    }

    if (output_type == "text") {
        write_points_to_file(file_out, segment_list, rows, cols);

    } else { // SVG or PNG
        // Load segments into point contour set
        load_segments_into_pcs(segment_list, cset);

        // Fit Bezier curves to contours
        BezierContourSet bcset;
        fit_Bezier_curves_to_contours(cset, bcset, error);

        Cairo::RefPtr<Cairo::Context> cr;

        cr = create_output_surface(output_type, cols, rows, image_file, file_out);

        //Draw contours onto surface
        //draw_point_contours(cset, cr, nodataval);
        draw_Bezier_contours(bcset, cr, nodataval);

        write_output_file(cr, output_type, file_out);

    }

    return 0;
}