#include <iostream>
#include <random>

#include <boost/tuple/tuple.hpp>

#include "pca.h"
#include "utils.h"

std::string word_vector_file;
std::vector<std::string> search_words;

// main function arguments

void printHelp() {
  std::cout << "c++ pca implementation \n\n";
  std::cout << "Options:\n";
  std::cout << "Parameters for drawing:\n";
  std::cout << "\t-word-vector <file>\n";
  std::cout << "\t\tUse word vector stored in <file>\n";
  std::cout << "\t-draw-words <words>\n";
  std::cout << "\t\tChoose <words> to be drawed\n";
  std::cout << "\nExamples:\n";
  std::cout << "./pca -word-vector data.txt -draw-words king queen prince princess\n";

}

int ArgPos(char *str, int argc, char **argv) {
  int i;
  std::string s_str;
  std::string s_argv[argc];

  for(i = 1; i < argc; i++) s_argv[i] = argv[i];
  s_str = str;

  for(i = 1; i < argc; i++) if(s_str == s_argv[i]) {
      if(i == argc - 1) {
	std::cout << "Argument missing for " << s_str << std::endl;;
	exit(1);
      }
      return i;
  }
  return -1;
}

void ArgPass(int argc, char **argv) {
  int i;
  if ((i = ArgPos((char *)"-word-vector", argc, argv)) > 0) word_vector_file = argv[i + 1];
  if ((i = ArgPos((char *)"-draw-words", argc, argv)) > 0) {
    for(int j = 0; j < argc - 4; j++)
      {
	search_words.push_back(argv[i + 1 + j]);
      }
  }
}

// main function arguments

int main(int argc, char **argv) {
  std::ofstream fout;

  std::string line;
  std::vector<std::string> word;

  std::vector< std::vector<double> > data;
  std::vector<std::string> label;

  fout.open("temp.dat", std::ofstream::out);

  if(argc == 1) {
    printHelp();
    return 0;
  }

  ArgPass(argc, argv);

  std::ifstream inFile;
  inFile.open(word_vector_file, std::ifstream::in);
  if(inFile.fail()) {
    std::cout << "Word vector file not found!\n";
    exit(1);
  }

  std::getline(inFile, line);
  split(line, word);
  int num_records = atoi(word[0].c_str());
  int num_variables = atoi(word[1].c_str());
  stats::pca pca(num_variables);
  pca.set_do_bootstrap(true, 100);

  for(int i = 0; i < num_records; ++i) {
    word.clear();
    std::getline(inFile,line);
    split(line, word);
    std::vector<double> record(num_variables);
    for(int j = 0; j < num_variables; j++) record[j] = atof(word[j+1].c_str());

    pca.add_record(record);
    label.push_back(word[0]);
    data.push_back(record);
  }

  pca.solve();

  /*
  std::cout<<"Energy = "<<pca.get_energy()<<" ("<<
    stats::utils::get_sigma(pca.get_energy_boot())<<")"<<std::endl;;

  const auto eigenvalues = pca.get_eigenvalues();
  std::cout<<"First three eigenvalues = "<<eigenvalues[0]<<", "
      <<eigenvalues[1]<<", "
	   <<eigenvalues[2]<<std::endl;;

  std::cout<<"Orthogonal Check = "<<pca.check_eigenvectors_orthogonal()<<std::endl;;
  std::cout<<"Projection Check = "<<pca.check_projection_accurate()<<std::endl;;
  */

  //pca.save("pca_results");

  std::vector<double> principal_1 = pca.get_principal(0);
  std::vector<double> principal_2 = pca.get_principal(1);

  // Very simple use case of gnuplot

  std::vector< boost::tuple<std::string, double, double> > xy_pts_A;

  double x, y;

  for(int i = 0; i < num_records; i++) {
    x = 0;
    y = 0;
    for(int j = 0; j < num_variables; j++) {
      x += principal_1[j] * data[i][j];
      y += principal_2[j] * data[i][j];
    }
    //xy_pts_A.push_back(boost::make_tuple(label[i],x,y));
  }

  for(int i = 0; i < num_records; i++) {
    for(auto& k: search_words) {
      if( label[i] == (k) ) {
	x = 0;
	y = 0;
	for(int k = 0; k < num_variables; k++) {
	  x += principal_1[k] * data[i][k];
	  y += principal_2[k] * data[i][k];
	}
	xy_pts_A.push_back(boost::make_tuple(label[i],x,y));
      }
    }
  }

  for(const auto& i : xy_pts_A) {
    fout << boost::get<0>(i) << " " << boost::get<1>(i)<< " " << boost::get<2>(i) << std::endl;
  }

  fout.open("script.gp",std::ofstream::out);

  fout << "set term png" << std::endl;
  fout << "set output \"sample.png\"" << std::endl;
  fout << "plot 'temp.dat' using 2:3:1 with labels offset 1 title 'word vectors'" << std::endl;

  fout.close();
  return 0;
}


//
// $Id$
//
//
// Darren Kessner <darren@proteowizard.org>
//
// Copyright 2009 Spielberg Family Center for Applied Proteomics
//   Cedars Sinai Medical Center, Los Angeles, California  90048
//
// 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 "Calibrator.hpp"
#include "ErrorEstimator.hpp"
#include "MassSpread.hpp"
#include "pwiz/utility/chemistry/Ion.hpp"
#include <cmath>
#include <iterator>
#include <stdexcept>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
namespace ublas = boost::numeric::ublas;

using namespace std;
using pwiz::data::CalibrationParameters;

namespace pwiz {
namespace calibration {

class CalibratorImpl : public Calibrator
{
    public:

    CalibratorImpl(const MassDatabase& massDatabase,
                   const vector<Measurement>& measurements,
                   const CalibrationParameters& initialParameters,
                   double initialErrorEstimate,
                   int errorEstimatorIterationCount,
                   const string& outputDirectory);

    virtual void iterate();
    virtual int iterationCount() const {return iterationCount_;}
    virtual const pwiz::data::CalibrationParameters& parameters() const {return parameters_;}
    virtual int measurementCount() const;
    virtual const Measurement* measurement(int index) const;
    virtual const MassSpread* massSpread(int index) const;
    virtual double error() const {return error_;}

    private:

    const MassDatabase& massDatabase_;
    const vector<Measurement>& measurements_;
    pwiz::data::CalibrationParameters parameters_;
    double error_;
    auto_ptr<ErrorEstimator> errorEstimator_;
    int iterationCount_;
    const int errorEstimatorIterationCount_;
    const string outputDirectory_;
    mutable ofstream log_;

    void recalculateParameters();

    void calculateSums(double& sum_z2_f2,
                       double& sum_z2_f3,
                       double& sum_z2_f4,
                       double& sum_z_f,
                       double& sum_z_f2);

    void calculateParametersFromSums(double sum_z2_f2,
                                     double sum_z2_f3,
                                     double sum_z2_f4,
                                     double sum_z_f,
                                     double sum_z_f2);

    void updateLog() const;
};

auto_ptr<Calibrator> Calibrator::create(const MassDatabase& massDatabase,
                                        const vector<Measurement>& measurements,
                                        const CalibrationParameters& initialParameters,
                                        double initialErrorEstimate,
                                        int errorEstimatorIterationCount,
                                        const string& outputDirectory)
{
    return auto_ptr<Calibrator>(new CalibratorImpl(massDatabase,
                                                   measurements,
                                                   initialParameters,
                                                   initialErrorEstimate,
                                                   errorEstimatorIterationCount,
                                                   outputDirectory));
}

CalibratorImpl::CalibratorImpl(const MassDatabase& massDatabase,
                               const vector<Measurement>& measurements,
                               const CalibrationParameters& initialParameters,
                               double initialErrorEstimate,
                               int errorEstimatorIterationCount,
                               const string& outputDirectory)
:   massDatabase_(massDatabase),
    measurements_(measurements),
    parameters_(initialParameters),
    error_(initialErrorEstimate),
    iterationCount_(0),
    errorEstimatorIterationCount_(errorEstimatorIterationCount),
    outputDirectory_(outputDirectory)
{
    if (!outputDirectory_.empty())
    {
        system(("mkdir " + outputDirectory_ + " 2> /dev/null").c_str());
        ostringstream oss;
        oss << outputDirectory << "/log";
        log_.open(oss.str().c_str());
        if (!log_) throw runtime_error(("[Calibrator] Unable to open log file " + oss.str()).c_str());
        updateLog();
    }
}

void CalibratorImpl::iterate()
{
    using namespace proteome; // for Ion

    iterationCount_++;

    // convert frequencies to neutral masses, using current calibration parameters
    vector<double> masses;
    for (vector<Measurement>::const_iterator it=measurements_.begin(); it!=measurements_.end(); ++it)
    {

      double mz = parameters_.mz(it->frequency);
      double neutralMass = Ion::neutralMass(mz, it->charge);
      masses.push_back(neutralMass);
      //     cout<<it->frequency<<" "<<mz<<" "<<it->charge<<endl;
    }

    if (!outputDirectory_.empty())
    {
        ostringstream filename;
        filename << outputDirectory_ << "/ee." << setw(4) << setfill('0') << iterationCount_ << ".txt";
        errorEstimator_ = ErrorEstimator::create(massDatabase_, masses, error_, filename.str().c_str());
    }
    else
    {
        errorEstimator_ = ErrorEstimator::create(massDatabase_, masses, error_); // no logging
    }

    // ErrorEstimator calculates MassSpreads
    for (int i=0; i<errorEstimatorIterationCount_; i++)
        errorEstimator_->iterate();

    // get new Parameter estimate
    bool matrixError = false;
    try{
      recalculateParameters();
    }
    catch(range_error){
      cerr<<"probably had a singular matrix - sucks to be you!"<<endl;
      matrixError = true;
    }
    if(! matrixError){
      error_ = errorEstimator_->error();
    }

    if (log_)
        updateLog();
}

int CalibratorImpl::measurementCount() const
{
    return (int)measurements_.size();
}

const Calibrator::Measurement* CalibratorImpl::measurement(int index) const
{
    if (index >= measurementCount())
        throw out_of_range("[Calibrator::measurement()] Index out of range.");

    return &measurements_[index];
}

const MassSpread* CalibratorImpl::massSpread(int index) const
{
    if (index >= measurementCount())
        throw out_of_range("[Calibrator::massSpread()] Index out of range.");

    return errorEstimator_.get() ? errorEstimator_->massSpread(index) : 0;
}

void CalibratorImpl::recalculateParameters()
{
    double sum_z2_f2 = 0;
    double sum_z2_f3 = 0;
    double sum_z2_f4 = 0;
    double sum_z_f = 0;
    double sum_z_f2 = 0;

    calculateSums(sum_z2_f2, sum_z2_f3, sum_z2_f4, sum_z_f, sum_z_f2);
    cout<<sum_z2_f2<<" "<<sum_z2_f3<<" "<<sum_z2_f4<<" "<<sum_z_f<<" "<<sum_z_f2<<endl;
    calculateParametersFromSums(sum_z2_f2, sum_z2_f3, sum_z2_f4, sum_z_f, sum_z_f2);
}

void CalibratorImpl::calculateSums(double& sum_z2_f2,
                                   double& sum_z2_f3,
                                   double& sum_z2_f4,
                                   double& sum_z_f,
                                   double& sum_z_f2)
{
    using namespace proteome; // for Ion

    if ((int)measurements_.size() != errorEstimator_->measurementCount())
        throw runtime_error("[CalibratorImpl::calculateSums()] Inconsistent measurement counts!");

    for (int i=0; i<errorEstimator_->measurementCount(); i++)
    {
        double f = measurements_[i].frequency;
        int z = measurements_[i].charge;

	cout<<"======================== "<<f<<" "<<z<<" ====================="<<endl;

	//THE PROBLEM IS HERE!!! --BEGIN

        // calculate ion MassSpread from the neutral MassSpread
        auto_ptr<MassSpread> ionMassSpread = MassSpread::create();
        const vector<MassSpread::Pair>& neutralDistribution = errorEstimator_->massSpread(i)->distribution();
        for (vector<MassSpread::Pair>::const_iterator it=neutralDistribution.begin();
             it!=neutralDistribution.end(); ++it)
        {
            MassSpread::Pair pair(Ion::ionMass(it->mass, z), it->probability);
            ionMassSpread->distribution().push_back(pair);
        }
        ionMassSpread->recalculate();

        double pa = ionMassSpread->sumProbabilityOverMass();
        double pa2 = ionMassSpread->sumProbabilityOverMass2();

	cout<<setprecision(10)<<"pa "<<pa<<" "<<pa2<<endl;

	//THE PROBLEM IS HERE!!! --END

        // TODO: uncommenting the following w/O2 gives different output! yuck!
        //errorEstimator_->massSpread(i);

        sum_z2_f2 += z*z*pa2/pow(f,2);
        sum_z2_f3 += z*z*pa2/pow(f,3);
        sum_z2_f4 += z*z*pa2/pow(f,4);
        sum_z_f += z*pa/f;
        sum_z_f2 += z*pa/pow(f,2);

	cout << sum_z2_f2 << "  " << sum_z2_f3 << " | " << sum_z_f << endl;
	cout << sum_z2_f3 << "  " << sum_z2_f4 << " | " << sum_z_f2 << endl;

    }

}

void CalibratorImpl::calculateParametersFromSums(double sum_z2_f2,
                                                 double sum_z2_f3,
                                                 double sum_z2_f4,
                                                 double sum_z_f,
                                                 double sum_z_f2)
{
    ublas::matrix<double> M(2,2);
    ublas::vector<double> p(2);

    M(0,0) = sum_z2_f2;
    M(0,1) = M(1,0) = sum_z2_f3;
    M(1,1) = sum_z2_f4;
    p(0) = sum_z_f;
    p(1) = sum_z_f2;

    ublas::permutation_matrix<size_t> pm(2);
    int singular = lu_factorize(M, pm);
    if (singular)
      throw range_error("[CalibratorImpl::calculateParametersFromSums()] Matrix is singular.");
    //        throw runtime_error("[CalibratorImpl::calculateParametersFromSums()] Matrix is singular.");

    lu_substitute(M, pm, p);

    cout<<"CALIBRATION PARAMS "<<parameters_.A<<" "<<parameters_.B<<endl;
    parameters_.A = p[0];
    parameters_.B = p[1];
    cout<<"CALIBRATION PARAMS "<<parameters_.A<<" "<<parameters_.B<<endl;
}

namespace {
void reportMeasurement(ostream& os,
                       const Calibrator::Measurement& measurement,
                       const CalibrationParameters& parameters)
{
    using namespace proteome; // for Ion

    double mz = parameters.mz(measurement.frequency);
    double mass = Ion::neutralMass(mz, measurement.charge);
    os << measurement.frequency << " " << measurement.charge << " " << mz << " " << mass << endl;
}
}//namespace

void CalibratorImpl::updateLog() const
{
    if (!log_)
    {
        cerr << "[CalibratorImpl::updateLog()] Warning: invalid log stream.\n";
        return;
    }

    log_.precision(12);

    log_ << "#\n";
    log_ << "# iteration: " << iterationCount_ <<
            " error: " << error_ <<
            " A: " << parameters_.A <<
            " B: " << parameters_.B << endl;
    log_ << "#\n";
    log_ << "# f z m/z m\n";

    for (vector<Measurement>::const_iterator it=measurements_.begin(); it!=measurements_.end(); ++it)
        reportMeasurement(log_, *it, parameters_);

    log_ << "\n\n";
}

/* matops matrix implementation
void CalibratorImpl::calculateParametersFromSums(double sum_z2_f2,
                                                 double sum_z2_f3,
                                                 double sum_z2_f4,
                                                 double sum_z_f,
                                                 double sum_z_f2)
{
    vector<vector<double> > M(2); // build matrix from sums (entries)
    M[0] = vector<double>(2);
    M[1] = vector<double>(2);
    M[0][0] = sum_z2_f2;
    M[0][1] = M[1][0] = sum_z2_f3;
    M[1][1] = sum_z2_f4;
    vector<double> v(2); // build vector
    v[0] = sum_z_f;
    v[1] = sum_z_f2;
    vector<double> p = inverse(M)*v; // solution

    parameters_.A = p[0];
    parameters_.B = p[1];
}
*/

} // namespace calibration
} // namespace pwiz


#include <mex.h>
#include <math.h>
#include <Eigen/Dense>
#include <igl/readOBJ.h>
#include <iostream>

using namespace Eigen;
using namespace std;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

    mxArray *vers_mex, *tris_mex, *uvs_mex;

    char *name;
    double *vers, *tris, *uvs;

    name = mxArrayToString(prhs[0]);

    mexPrintf("%s\n", name);

    MatrixXd v, tc, n;
    MatrixXi f, ftc, fn;

    igl::readOBJ(name, v, tc, n, f, ftc, fn);

    mexPrintf("ver #: %d; tri #: %d; uv #: %d.\n", v.rows(), f.rows(), tc.rows());

    vers_mex = plhs[0] = mxCreateDoubleMatrix(v.rows() * 3, 1, mxREAL);
    tris_mex = plhs[1] = mxCreateDoubleMatrix(f.rows() * 3, 1, mxREAL);
    uvs_mex = plhs[2] = mxCreateDoubleMatrix(tc.rows() * 2, 1, mxREAL);

    vers = mxGetPr(vers_mex);
    tris = mxGetPr(tris_mex);
    uvs = mxGetPr(uvs_mex);

    for(int i = 0; i < v.rows(); i++)
    {
        vers[i] = v(i, 0);
        vers[v.rows() + i] = v(i, 1);
        vers[2 * v.rows() + i] = v(i, 2);
    }

    for(int i = 0; i < f.rows(); i++)
    {
        tris[i] = f(i, 0);
        tris[f.rows() + i] = f(i, 1);
        tris[2 * f.rows() + i] = f(i, 2);
    }

    for(int i = 0; i < tc.rows(); i++)
    {
        uvs[i] = tc(i, 0);
        uvs[tc.rows() + i] = tc(i, 1);
    }

    return;

}


#include <I3Test.h>

#include "phys-services/I3Cuts.h"
#include "dataclasses/physics/I3Particle.h"

#include <algorithm>
#include <boost/foreach.hpp>
#include <iostream>

TEST_GROUP(ContainmentSizeTest);

// some functions we need
std::vector<double > myXPos ();
std::vector<double > myYPos ();

// a code copy for debugging
// (is not in use for the test)
double myScaleFunc (double px, double py,
		    std::vector<double> x,
		    std::vector<double> y);

TEST(ContainmentAreaSize){
  // make a first linebreak
  std::cout << std::endl;

  // define a simple detector --------------------
  const std::vector<double > x (myXPos ());
  const std::vector<double > y (myYPos ());

  // print the cornerpoints
  std::cout << "x: " << std::endl;
  BOOST_FOREACH(const double &val, x) {
            std::cout << val << ", ";
  }
  std::cout << std::endl;

  std::cout << "y: " << std::endl;
  BOOST_FOREACH(const double &val, y) {
            std::cout << val << ", ";
  }
  std::cout << std::endl;

  // define the particle -------------------------
  const double px = 450;
  const double py = 0;
  const double detectorPlane = 0;

  I3Particle p (I3Particle::InfiniteTrack);

  p.SetPos (px, py, detectorPlane);

  p.SetDir (0, 0); // vertical track

  std::cout << "particle: (x, y) = ("
	    << px << ", " << py << ")" << std::endl;

  // calculate the scaling factor ----------------
  const double s
    = I3Cuts::ContainmentAreaSize (p, x, y, detectorPlane);

  // do the calculation with own function
  // just for debugging
  // double s = myScaleFunc (px, py, x, y);

  std::cout << "scale factor: " << s << std::endl;
  ENSURE (s < 1, "This should be in the detector.");
}

TEST(ContainmentVolumeSize){
  // make a first linebreak
  std::cout << std::endl;

  // define a simple detector --------------------
  const std::vector<double > x (myXPos ());
  const std::vector<double > y (myYPos ());

  // print the cornerpoints
  std::cout << "x: " << std::endl;
  BOOST_FOREACH(const double &val, x) {
            std::cout << val << ", ";
  }
  std::cout << std::endl;

  std::cout << "y: " << std::endl;
  BOOST_FOREACH(const double &val, y) {
            std::cout << val << ", ";
  }
  std::cout << std::endl;

  // define the particle -------------------------
  const double px = 450;
  const double py = 0;
  const double detectorTop = 500;
  const double detectorBot = -500;
  const double detectorMid = (detectorTop + detectorBot)/2;

  I3Particle p (I3Particle::InfiniteTrack);

  p.SetPos (px, py, detectorMid);
  p.SetDir (0, 0); // vertical track

  std::cout << "particle: (x, y) = ("
	    << px << ", " << py << ")" << std::endl;

  // calculate the scaling factor ----------------
  const double s
    = I3Cuts::ContainmentVolumeSize (p, x, y,
				     detectorTop, detectorBot);

  std::cout << "scale factor: " << s << std::endl;
  ENSURE (s < 1, "This should be in the detector.");
}

// x-xoordnates for a simplified ic-40 geo
std::vector<double > myXPos () {
  std::vector<double > x;
  x.push_back( 400);
  x.push_back( 550);
  //  x.push_back( 350);
  //  x.push_back( 200);
  x.push_back(   0);
  x.push_back(-350);
  x.push_back(-400);
  //  x.push_back(-380);
  //  x.push_back(-220);
  x.push_back(-200);
  //  x.push_back(  50);
  x.push_back( 300);

  return x;
}

// y-xoordnates for a simplified ic-40 geo
std::vector<double > myYPos () {
  std::vector<double > y;
  y.push_back(-200);
  y.push_back( 150);
  //  y.push_back( 400);
  //  y.push_back( 400);
  y.push_back( 500);
  y.push_back( 450);
  y.push_back( 200);
  //  y.push_back( 100);
  //  y.push_back( 100);
  y.push_back(  50);
  //  y.push_back( 100);
  y.push_back(   0);

  return y;
}

// the algorithm with some cout for debugging
// this should be removed later
double myScaleFunc (double px, double py,
		    std::vector<double> x,
		    std::vector<double> y)
{

  using namespace I3Cuts;

  // Error-checking... need at least three strings to have an area
  if (x.size () < 3) {
    std::cout << "ContainmentArea of zero/1/2 strings: will be NAN"
	      << std::endl;
    return NAN;
  }

  // INSERT OLGA'S ALGORITHM HERE!
  // First, compute the center of mass
  double xcm, ycm;
  CMPolygon(x, y, &xcm, &ycm);

  std::cout << "center pos: " << xcm
	    << ", " << ycm << std::endl;

  double xprime = px;
  double yprime = py;

  // Compute the angle of point P:
  I3Direction d(xprime-xcm,yprime-ycm,0);
  double pang = d.CalcPhi();

  std::cout << "pang: " << pang << std::endl;

  //  double pang = angle(xcm,xprime,ycm,yprime);
  int n_more = -1;
  double more = 9999;
  int n_less = -1;
  double less = -9999;

  // Now, compute angles for each corner point,
  // and figure out which two bracket the point
  if (x.size() != y.size()) log_fatal("X and Y are not the same size");
  std::vector<double> ang;
  ang.resize(x.size());

  std::cout << "angle calc ------" << std::endl;

  for (unsigned int i=0; i<x.size(); i++) {
    I3Direction dd(x[i]-xcm,y[i]-ycm,0);
    ang[i] = dd.CalcPhi();
    //ang[i] = angle(xcm, x[i], ycm, y[i]);

    // Compute the difference
    double angdiff = pang-ang[i];
    if (angdiff<-I3Constants::pi) angdiff += 2*I3Constants::pi;
    if (angdiff>I3Constants::pi) angdiff -= 2*I3Constants::pi;

    // Is this the one?
    log_debug ("This (%f, %f) ang = %f, anglediff = %e",
            x[i], y[i], ang[i], angdiff);

    std::cout << "This (" << x[i] << ", " << y[i]
	      << ") ang = " << ang[i]
	      << " , angdiff = " << angdiff << std::endl;

    const double SMALLNUMBER = 1.0e-15;
    if (std::abs(angdiff)<SMALLNUMBER) { // we found an exact match!
      less = 0; n_less = i;
      more = 0; n_more = i;
      std::cout << "Match!" << std::endl;
    }
    else if (angdiff<0 && angdiff>less) { // we found a new less
      less = angdiff;
      n_less = i;
      std::cout << "New less!" << std::endl;
    }
    else if (angdiff>0 && angdiff<more) { // we found a new more
      more = angdiff;
      n_more = i;
      std::cout << "New more!" << std::endl;
    }

  }

  double xi, yi;
  if (less==0 && more==0) {
    log_debug("An exact match was found... %i", n_more);
    xi = x[n_more];
    yi = y[n_more];

  } else {
    log_trace("PAngle = %f, closest above = %f (%i), closest below = %f (%i)",
           pang,
           more, n_more,
           less, n_less);

    // Use Olga's notation
    double x1 = x[n_less]; double y1 = y[n_less];
    double x2 = x[n_more]; double y2 = y[n_more];

    // Compute intersection point
    IntersectionOfTwoLines(x1,y1,x2,y2,xcm,ycm,xprime,yprime,&xi,&yi);

    log_debug("point 1: (%f, %f)", x1, y1);
    log_debug("point 2: (%f, %f)", x2, y2);
    log_debug("CM     : (%f, %f)", xcm, ycm);
    log_debug("the point: (%f, %f)", xprime, yprime);
    log_debug("intersection: (%f, %f)", xi, yi);
  }

  // Compute ratio of distances
  double dprime = sqrt((xprime-xcm)*(xprime-xcm) + (yprime-ycm)*(yprime-ycm));
  double di = sqrt((xi-xcm)*(xi-xcm) + (yi-ycm)*(yi-ycm));
  log_debug("dprime = %f, di = %f", dprime, di);

  return dprime/di;

}


#include <vector>
#include <type_traits>

#include <Eigen/Eigen>
#include <xtensor/xtensor.hpp>
#include <xtensor/xarray.hpp>

#include <iostream>

namespace detail
{
    template <class T>
    struct is_xtensor : std::false_type
    {
    };

    template <class T>
    struct is_xtensor<xt::xarray<T>> : std::true_type
    {
    };

    template <class T, size_t N>
    struct is_xtensor<xt::xtensor<T, N>> : std::true_type
    {
    };

    template <class T, typename = void>
    struct io_impl
    {
        static T load()
        {
            std::cout << "load generic" << std::endl;
            return T();
        }

        static void dump(const T& data){
            std::cout << "dump generic" << std::endl;
        }
    };

    template <class T>
    struct io_impl<T, typename std::enable_if<std::is_base_of<Eigen::DenseBase<T>, T>::value>::type>
    {
        static T load()
        {
            std::cout << "load Eigen::DenseBase" << std::endl;
            return T();
        }

        static void dump(const T& data)
        {
            std::cout << "dump Eigen::DenseBase" << std::endl;
        }
    };

    template <class T>
    struct io_impl<T, typename std::enable_if<is_xtensor<T>::value>::type>
    {
        static T load()
        {
            std::cout << "load xt::xarray<T> or xt::xtensor<T, N>" << std::endl;
            return T();
        }

        static void dump(const T& data){
            std::cout << "dump xt::xarray<T> or xt::xtensor<T, N>" << std::endl;
        }
    };

    template <class T>
    struct io_impl<T, typename std::enable_if<std::is_same<std::vector<typename T::value_type>, T>::value>::type>
    {
        static T load()
        {
            std::cout << "load std::vector" << std::endl;
            return T();
        }

        static void dump(const T& data){
            std::cout << "dump std::vector" << std::endl;
        }
    };
};

template <class T>
inline void dump(const T& data)
{
    return detail::io_impl<std::decay_t<T>>::dump(data);
}

template <class T>
inline T load()
{
    return detail::io_impl<std::decay_t<T>>::load();
}

int main()
{
    Eigen::ArrayXf A = Eigen::ArrayXf::Random(50);
    Eigen::MatrixXd B = Eigen::MatrixXd::Random(20, 5);
    xt::xarray<double> C = {{1, 2, 3}, {4, 5, 6}};
    xt::xtensor<double,2> D = {{1, 2, 3}, {4, 5, 6}};
    std::vector<double> E = {1, 2, 3};
    int F = 0;

    dump(A);
    dump(B);
    dump(C);
    dump(D);
    dump(E);
    dump(F);

    load<decltype(A)>();
    load<decltype(B)>();
    load<decltype(C)>();
    load<decltype(D)>();
    load<decltype(E)>();
    load<decltype(F)>();

    return 0;
}


#pragma once

#include <boost/multiprecision/cpp_dec_float.hpp>

#include "ExecuteOptimization.hpp"

namespace Testing {
using Multiprecision = boost::multiprecision::cpp_dec_float_100;
using MultiprecisionVector = LSLOpt::Vector<Multiprecision>;
}

namespace LSLOpt {

template<>
struct scalar_traits<Testing::Multiprecision> {
    // we use the double epsilon anyway!
    static Testing::Multiprecision epsilon() {return std::numeric_limits<double>::epsilon();}
    static Testing::Multiprecision infinity() {return std::numeric_limits<Testing::Multiprecision>::infinity();}
    static Testing::Multiprecision nan() {return std::numeric_limits<Testing::Multiprecision>::quiet_NaN();}

    static bool is_nan(const Testing::Multiprecision& d) {return !(d == d);}
};

template<>
OptimizationParameters<Testing::Multiprecision> getOptimizationParameters<Testing::Multiprecision>()
{
  OptimizationParameters<Testing::Multiprecision> params;
  params.machine_epsilon = scalar_traits<Testing::Multiprecision>::epsilon();
  params.gradient_tolerance = 1e-6;
  params.alpha_min = params.machine_epsilon;
  params.check_epsilon = 1e-10;
  params.min_change = 0.0;
  params.min_value = -scalar_traits<Testing::Multiprecision>::infinity();
  params.min_grad_param = params.machine_epsilon;

  return params;
}

}


/// \file   variable.hpp
///
/// \brief  Wraps automatically differentiable variables.
///
/// \authors    Maarten P. Scholl
/// \date       2019-04-04
/// \copyright  Copyright 2017-2019 The Institute for New Economic Thinking,
///             Oxford Martin School, University of Oxford
///
///             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.
///
///             You may obtain instructions to fulfill the attribution
///             requirements in CITATION.cff
///
#ifndef ESL_MATHEMATICS_VARIABLE_HPP
#define ESL_MATHEMATICS_VARIABLE_HPP

#include <adept.h>

namespace esl {
    typedef adept::adouble variable;

    template<typename t>
    inline double value(t value)
    {
        return double(value);
    }

    template<>
    inline double value(variable value)
    {
#ifndef ADEPT_NO_AUTOMATIC_DIFFERENTIATION
        return value.value();
#else
        return value;
#endif
    }

}  // namespace esl

#endif  // ESL_MATHEMATICS_VARIABLE_HPP


#pragma once

#include <terark/fstring.hpp>
#include <terark/valvec.hpp>
#include <boost/integer/static_log2.hpp>
#include "entropy_base.hpp"

namespace terark { namespace Huffman {

static constexpr size_t BLOCK_BITS = 12;
static constexpr size_t NORMALISE = 1ull << 15;

struct HuffmanEncSymbol {
    uint16_t bits;
    uint16_t bit_count;
};

class TERARK_DLL_EXPORT encoder {
public:
    encoder();
    encoder(const freq_hist::histogram_t& hist);

    void init(const freq_hist::histogram_t& hist);

    const valvec<byte_t>& table() const;
    void take_table(valvec<byte_t>* ptr);

    EntropyBytes encode(fstring record, TerarkContext* context) const;

    EntropyBits bitwise_encode(fstring record, TerarkContext* context) const;

    HuffmanEncSymbol syms_[256];
    valvec<byte_t> table_;
};

class TERARK_DLL_EXPORT decoder {
public:
    decoder();
    decoder(fstring table, size_t* psize = nullptr);

    void init(fstring table, size_t* psize);

    bool decode(fstring data, valvec<byte_t>* record, TerarkContext* context) const;

    bool bitwise_decode(const EntropyBits& data, valvec<byte_t>* record, TerarkContext* context) const;

private:
    byte_t ari_[1u << BLOCK_BITS];
    uint8_t cnt_[256];
};

class TERARK_DLL_EXPORT encoder_o1 {
public:
    encoder_o1();
    encoder_o1(const freq_hist_o1::histogram_t& hist);

    void init(const freq_hist_o1::histogram_t& hist);

    const valvec<byte_t>& table() const;
    void take_table(valvec<byte_t>* ptr);

    EntropyBytes encode_x1(fstring record, TerarkContext* context) const;
    EntropyBytes encode_x2(fstring record, TerarkContext* context) const;
    EntropyBytes encode_x4(fstring record, TerarkContext* context) const;
    EntropyBytes encode_x8(fstring record, TerarkContext* context) const;

    EntropyBits bitwise_encode_x1(fstring record, TerarkContext* context) const;
    EntropyBits bitwise_encode_x2(fstring record, TerarkContext* context) const;
    EntropyBits bitwise_encode_x4(fstring record, TerarkContext* context) const;
    EntropyBits bitwise_encode_x8(fstring record, TerarkContext* context) const;

private:
    template<size_t N>
    EntropyBits bitwise_encode_xN(fstring record, TerarkContext* context) const;

    HuffmanEncSymbol syms_[257][256];
    valvec<byte_t> table_;
};

class TERARK_DLL_EXPORT decoder_o1 {
public:
    decoder_o1();
    decoder_o1(fstring table, size_t* psize = nullptr);

    void init(fstring table, size_t* psize);

    bool decode_x1(fstring data, valvec<byte_t>* record, TerarkContext* context) const;
    bool decode_x2(fstring data, valvec<byte_t>* record, TerarkContext* context) const;
    bool decode_x4(fstring data, valvec<byte_t>* record, TerarkContext* context) const;
    bool decode_x8(fstring data, valvec<byte_t>* record, TerarkContext* context) const;

    bool bitwise_decode_x1(const EntropyBits& data, valvec<byte_t>* record, TerarkContext* context) const;
    bool bitwise_decode_x2(const EntropyBits& data, valvec<byte_t>* record, TerarkContext* context) const;
    bool bitwise_decode_x4(const EntropyBits& data, valvec<byte_t>* record, TerarkContext* context) const;
    bool bitwise_decode_x8(const EntropyBits& data, valvec<byte_t>* record, TerarkContext* context) const;

private:
    template<size_t N>
    bool bitwise_decode_xN(const EntropyBits& data, valvec<byte_t>* record, TerarkContext* context) const;

    byte_t ari_[257][1u << BLOCK_BITS];
    uint8_t cnt_[257][256];
};

}}


// SPDX-FileCopyrightText: 2015 - 2021 Marcin Łoś <marcin.los.91@gmail.com>
// SPDX-License-Identifier: MIT

#ifndef TUMOR_TUMOR_HPP
#define TUMOR_TUMOR_HPP

#include <cmath>

#include <boost/format.hpp>

#include "ads/executor/galois.hpp"
#include "ads/output_manager.hpp"
#include "ads/simulation.hpp"
#include "params.hpp"
#include "skin.hpp"
#include "state.hpp"
#include "vasculature.hpp"

namespace bsp = ads::bspline;

namespace tumor {

class tumor_2d : public ads::simulation_2d {
private:
    static constexpr std::size_t Dim = 2;
    using Base = ads::simulation_2d;

    state<Dim> now, prev;
    params p;

    vasc::vasculature vasculature;

    ads::output_manager<2> output;

    int save_every;

    int vasc_update_every = 10;

    ads::galois_executor executor{4};

    ads::bspline::eval_ctx xctx;
    ads::bspline::eval_ctx yctx;
    ads::bspline::eval_ders_ctx xdctx;
    ads::bspline::eval_ders_ctx ydctx;

public:
    tumor_2d(const ads::config_2d& config, const params& params, int save_every,
             vasc::vasculature&& vasculature);

private:
    struct constant {
        double value;

        explicit constant(double v)
        : value{v} { }

        double operator()(double, double) const { return value; }
    };

    double init_tumor(double x, double y) {
        double dx = (x - 1500) / 600;
        double dy = (y - 1500) / 600;
        double r2 = std::min(dx * dx + dy * dy, 1.0);
        return 0.8 * (r2 - 1) * (r2 - 1) * (r2 + 1) * (r2 + 1);
    }

    double init_M(double x, double y) const {
        auto lay = p.skin.layer_at(x, y, x);
        return lay == skin_model::layer::dermis ? 1 : p.init_M;
    }

    void dirichlet() {
        x.fix_left();
        x.fix_right();
        y.fix_left();
        y.fix_right();
    }

    void apply_boundary_conditions(vector_type& v) {
        for (int i = 0; i < y.dofs(); ++i) {
            v(0, i) = 0;
            v(x.dofs() - 1, i) = 0;
        }
        for (int i = 0; i < x.dofs(); ++i) {
            v(i, 0) = 0;
            v(i, y.dofs() - 1) = 0;
        }
    }

    point_type normalize(point_type p) const {
        return {(p[0] - x.a) / (x.b - x.a), (p[1] - y.a) / (y.b - y.a)};
    }

    void prepare_matrices() {
        dirichlet();
        Base::prepare_matrices();
    }

    void plot_vasculature(int iter) {
        using boost::format;
        vasculature.plot_veins(str(format("vasculature_%d.data") % iter));
        vasculature.plot_oxygen(str(format("oxygen_%d.data") % iter));
    }

    void before() override;

    void before_step(int /*iter*/, double /*t*/) override {
        using std::swap;
        swap(now, prev);

        now.clear();
    }

    void step(int iter, double /*t*/) override {
        compute_rhs();
        solve_all();
        update_vasculature(iter);
    }

    void save_to_file(int iter);

    void after_step(int iter, double /*t*/) override {
        int next_iter = iter + 1;
        if (next_iter % save_every == 0) {
            save_to_file(next_iter);
        }
    }

    void solve_all();

    value_type ensure_positive(value_type v) const {
        if (v.val < 0) {
            v.val = 0;
        }
        return v;
    }

    void compute_rhs();

    void update_vasculature(int iter);

    double& val(vector_type& v, index_type idx) { return v(idx[0], idx[1]); }
};

}  // namespace tumor

#endif  // TUMOR_TUMOR_HPP


//#define CGAL_INTERSECTION_MAP_FOR_SUPPORTING_CIRCLES

#include <CGAL/Cartesian.h>
#include <CGAL/Handle_for.h>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/MP_Float.h>
#include <CGAL/Gmpz.h>
#include <CGAL/Gmpq.h>
#include <CGAL/Algebraic_kernel_for_circles_2_2.h>
#include <CGAL/intersections.h>
#include <CGAL/Circular_kernel_2.h>
#include <CGAL/Arr_circular_arc_traits_2.h>
#include <CGAL/Lazy_circular_kernel_2.h>
#include <CGAL/Filtered_bbox_circular_kernel_2.h>
#include <CGAL/Arrangement_2.h>
#include <CGAL/Arr_naive_point_location.h>
#include <CGAL/Arr_circular_line_arc_traits_2.h>
#include <CGAL/Timer.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <CGAL/IO/Dxf_variant_reader.h>
#include <fstream>
#include <iomanip>

#include <boost/type_traits.hpp>

// CIRCULAR KERNEL TYPEDEFS
//typedef CGAL::MP_Float RT;
//typedef CGAL::Quotient<RT> NT1;
typedef CGAL::Gmpz RT;
typedef CGAL::Gmpq NT1;
typedef CGAL::Cartesian<NT1> Linear_k1;
typedef CGAL::Algebraic_kernel_for_circles_2_2<NT1> Algebraic_k1;
typedef CGAL::Circular_kernel_2<Linear_k1, Algebraic_k1> CircularKernel;
typedef CGAL::Arr_circular_arc_traits_2<CircularKernel> CircularK_CA_Traits;
typedef CircularKernel::Circular_arc_2 CircularKArc;
typedef std::vector<CircularKArc> CircularKArcContainer;
typedef CircularKernel::Circular_arc_2 Circular_arc_2;
typedef CircularKernel::Line_arc_2 Line_arc_2;
typedef CGAL::Arr_circular_line_arc_traits_2<CircularKernel>   CircularK_Variant_Traits;
typedef boost::variant< Circular_arc_2, Line_arc_2 > CircularKVarArc;
typedef std::vector<CircularKVarArc> CircularKVarArcContainer;

// LAZY KERNEL TYPEDEFS
typedef CGAL::Interval_nt_advanced NT3;
typedef CGAL::Cartesian<NT3> Linear_k3;
typedef CGAL::Algebraic_kernel_for_circles_2_2<NT3> Algebraic_k3;
typedef CGAL::Circular_kernel_2 <Linear_k3,Algebraic_k3> CK3_;
typedef CGAL::Lazy_circular_kernel_2<CircularKernel,CK3_> LazyCurvedK;
typedef CGAL::Arr_circular_arc_traits_2<LazyCurvedK> LazyCurvedK_CA_Traits;
typedef LazyCurvedK::Circular_arc_2 LazyArc;
typedef std::vector<LazyArc> LazyArcContainer;
typedef LazyCurvedK::Circular_arc_2 Circular_arc_3;
typedef LazyCurvedK::Line_arc_2 Line_arc_3;
typedef boost::variant<Circular_arc_3,Line_arc_3 > LazyVarArc;
typedef std::vector<LazyVarArc> LazyVarContainer;
//~ typedef CGAL::Arr_circular_line_arc_traits_2<LazyCurvedK,Line_arc_3,Circular_arc_3> LazyCurvedK_Variant_Traits;
typedef CGAL::Arr_circular_line_arc_traits_2<LazyCurvedK> LazyCurvedK_Variant_Traits;

// BBOX TYPEDEFS
typedef CGAL::Filtered_bbox_circular_kernel_2<CircularKernel>
  BBCircularKernel ;
typedef CGAL::Arr_circular_arc_traits_2<BBCircularKernel>
  BBCircularKernel_CA_Traits;
typedef BBCircularKernel::Circular_arc_2
  BBCircularKernelArc;
typedef std::vector<BBCircularKernelArc>
  BBCircularKernelArcContainer;
typedef BBCircularKernel::Circular_arc_2
  Circular_arc_6;
typedef BBCircularKernel::Line_arc_2
  Line_arc_6;
typedef boost::variant<Circular_arc_6,Line_arc_6 >
  BBCircVarArc;
typedef std::vector<BBCircVarArc>
  BBCircVarContainer;
typedef CGAL::Arr_circular_line_arc_traits_2<BBCircularKernel>  BBCircVariantTraits;

// BBOX(LAZY)
typedef CGAL::Filtered_bbox_circular_kernel_2<LazyCurvedK>
  BBLazyKernel ;
typedef CGAL::Arr_circular_arc_traits_2<BBLazyKernel>
  BBLazyKernel_CA_Traits;
typedef BBLazyKernel::Circular_arc_2
  BBLazyKernelArc;
typedef std::vector<BBLazyKernelArc>
  BBLazyKernelArcContainer;
typedef BBLazyKernel::Circular_arc_2
  Circular_arc_lazybb;
typedef BBLazyKernel::Line_arc_2
  Line_arc_lazybb;
typedef boost::variant<Circular_arc_lazybb,Line_arc_lazybb >
  BBLazyVarArc;
typedef std::vector<BBLazyVarArc>
  BBLazyVarContainer;
typedef CGAL::Arr_circular_line_arc_traits_2<BBLazyKernel>  BBLazyVariantTraits;

template <class CK,class Traits,class ArcContainer>
void do_main(const char *s) {

  // TYPEDEFS
  typedef typename CK::Circular_arc_2      C2;
  typedef typename CK::Line_arc_2          L2;
  typedef typename CGAL::Arrangement_2<Traits>          Pmwx;
  typedef typename CGAL::Arr_naive_point_location<Pmwx> Point_location;

  // LOADING CURVES
  ArcContainer ac;
  std::ifstream fin;
  fin.open (s);
  CGAL::variant_load<CK, C2, L2>(
    fin, std::back_inserter(ac));
  fin.close();

  std::cout << "Size:" << ac.size() << std::endl;

  // BENCHMARKING
  Pmwx _pm;
  Point_location _pl(_pm);
  struct rusage before, after;
  struct timeval utime, stime;
  getrusage(RUSAGE_SELF,&before);
  insert(_pm,ac.begin(),ac.end(),boost::false_type());
  getrusage(RUSAGE_SELF,&after);
  timersub(&(after.ru_utime),&(before.ru_utime),&utime);
  timersub(&(after.ru_stime),&(before.ru_stime),&stime);
  std::cout<<"Time="<< utime.tv_sec<<"."<< std::setw(6) <<
  std::setfill('0')<< utime.tv_usec <<std::endl;

  std::cerr << utime.tv_sec << "." << std::setw(6) <<
  std::setfill('0')<< utime.tv_usec << std::endl;

  std::cout << "The arrangement size:" << std::endl
            << "   V = " << _pm.number_of_vertices()
            << ",  E = " << _pm.number_of_edges()
            << ",  F = " << _pm.number_of_faces() << std::endl;

}

template <class CK,class Traits,class ArcContainer>
void do_main(int k) {

  // TYPEDEFS
  typedef typename CK::Circular_arc_2      C2;
  typedef typename CK::Point_2 Point_2;
  typedef typename CK::FT FT;
  typedef typename CGAL::Arrangement_2<Traits>          Pmwx;
  typedef typename CGAL::Arr_naive_point_location<Pmwx> Point_location;

  // LOADING CURVES
  ArcContainer ac;

  double cx, cy;
  const FT rft(1.0);

  // DENSE
  if(k == 0) {
    for(cx = 0.0; cx <= 10.0; cx += 0.5) {
      for(cy = 0.0; cy <= 10.0; cy += 0.5) {
        ac.push_back(typename CK::Circular_arc_2 ( typename CK::Circle_2(Point_2(cx,cy),rft) ) );
      }
    }
  }

  // VERY DENSE
  if(k == 1) {
    for(cx = 0.0; cx <= 0.2; cx += 0.01) {
      for(cy = 0.0; cy <= 0.2; cy += 0.01) {
        ac.push_back(typename CK::Circular_arc_2 ( typename CK::Circle_2(Point_2(cx,cy),rft) ));
      }
    }
  }

  // ONE CIRCLE
  if(k == 2) {
    ac.push_back(typename CK::Circular_arc_2 ( typename CK::Circle_2(Point_2(0,0),5) ));
  }

  // RANDOM CASE
  if(k == 3) {
    CGAL::Random generatorOfgenerator;
    int random_seed = generatorOfgenerator.get_int(0, 123456);
    CGAL::Random theRandom(random_seed);
    for(int i=0; i<100; i++) {
      double x = theRandom.get_double(0.0,1.0);
      double y = theRandom.get_double(0.0,1.0);
      double r = theRandom.get_double(0.00001,1.0);
      ac.push_back(typename CK::Circular_arc_2 ( typename CK::Circle_2(Point_2(x,y),FT(r)) ));
    }
  }

  // SPARSE
  if(k == 4) {
    double h = (std::sqrt(3.0)+0.01);
    for(cx = 0.0; cx <= 40.4; cx += 2.01) {
      for(cy = h; cy <= h*20; cy += h*2) {
        ac.push_back(typename CK::Circular_arc_2 ( typename CK::Circle_2(Point_2(cx+1.0,cy),rft) ));
      }
      for(cy = 0.0; cy <= h*20.0; cy += h*2) {
        ac.push_back(typename CK::Circular_arc_2 ( typename CK::Circle_2(Point_2(cx,cy),rft)));
      }
    }
  }

  std::cout << "Size:" << ac.size() << std::endl;

  // BENCHMARKING
  Pmwx _pm;
  Point_location _pl(_pm);
  struct rusage before, after;
  struct timeval utime, stime;
  getrusage(RUSAGE_SELF,&before);
  insert(_pm,ac.begin(),ac.end(),boost::false_type());
  getrusage(RUSAGE_SELF,&after);
  timersub(&(after.ru_utime),&(before.ru_utime),&utime);
  timersub(&(after.ru_stime),&(before.ru_stime),&stime);
  std::cout<<"Time="<< utime.tv_sec<<"."<< std::setw(6) <<
  std::setfill('0')<< utime.tv_usec << std::endl;

  std::cerr << utime.tv_sec << "." << std::setw(6) <<
  std::setfill('0')<< utime.tv_usec << std::endl;

  std::cout << "The arrangement size:" << std::endl
            << "   V = " << _pm.number_of_vertices()
            << ",  E = " << _pm.number_of_edges()
            << ",  F = " << _pm.number_of_faces() << std::endl;
}

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

  const char* dxf_filename[] = { "DXF/51.dxf",
                                 "DXF/cad_l1.dxf",
                                 "DXF/cad_l2.dxf",
                                 "DXF/che_mod1.dxf",
                                 "DXF/CIOnZDraw.dxf",
                                 "DXF/mask1.dxf",
                                 "DXF/elekonta.dxf",
                                 "DXF/netlist_signal_1.dxf",
                                 "DXF/painttrack.dxf" };
  if(argc == 3) {
    int i = argv[1][0]-'0';
    int j = argv[2][0]-'0';
    if((j >= 0 && j < 9)) {
      if(i == 1) do_main<BBCircularKernel,BBCircVariantTraits, BBCircVarContainer>(dxf_filename[j]);
      if(i == 2) do_main<LazyCurvedK,LazyCurvedK_Variant_Traits, LazyVarContainer>(dxf_filename[j]);
      if(i == 3) do_main<CircularKernel,CircularK_Variant_Traits, CircularKVarArcContainer>(dxf_filename[j]);
      if(i == 4) do_main<BBLazyKernel,BBLazyVariantTraits, BBLazyVarContainer>(dxf_filename[j]);
      if((i >= 5) || (i <= 0)) std::cout << "INVALID PARAMETERS" << std::endl;
    } else {
      int k = -1;
      if(j == 9) k = 0;
      if(j == ('a'-'0')) k = 1;
      if(j == ('b'-'0')) k = 2;
      if(j == ('c'-'0')) k = 3;
      if(j == ('d'-'0')) k = 4;
      if(i == 1) do_main<BBCircularKernel,BBCircVariantTraits, BBCircVarContainer>(k);
      if(i == 2) do_main<LazyCurvedK,LazyCurvedK_Variant_Traits, LazyVarContainer>(k);
      if(i == 3) do_main<CircularKernel,CircularK_Variant_Traits, CircularKVarArcContainer>(k);
      if(i == 4) do_main<BBLazyKernel,BBLazyVariantTraits, BBLazyVarContainer>(k);
      if(i == 5) do_main<BBCircularKernel,BBCircularKernel_CA_Traits, BBCircularKernelArcContainer>(k);
      if(i == 6) do_main<LazyCurvedK,LazyCurvedK_CA_Traits, LazyArcContainer>(k);
      if(i == 7) do_main<CircularKernel,CircularK_CA_Traits, CircularKArcContainer>(k);
      if(i == 8) do_main<BBLazyKernel,BBLazyKernel_CA_Traits, BBLazyKernelArcContainer>(k);
    }
  } else std::cout << "INVALID PARAMETERS" << std::endl;

  return 0;
}


#include "Heuristics.hpp"
#include "DependencyHelpers.hpp"
#include "TsplpExceptions.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/range/adaptor/reversed.hpp>

#include <unordered_set>

#include <xtensor/xmanipulation.hpp>

std::tuple<std::vector<std::vector<size_t>>, double> tsplp::ExploitFractionalSolution(
    xt::xarray<double> fractionalSolution, xt::xarray<double> weights,
    const xt::xtensor<size_t, 1>& startPositions, const xt::xtensor<size_t, 1>& endPositions,
    const DependencyGraph& dependencies, std::chrono::milliseconds timeout)
{
    const auto A = startPositions.size();
    assert(endPositions.size() == A);

    if (weights.dimension() == 2)
        weights = xt::repeat(xt::view(weights, xt::newaxis(), xt::all()), A, 0);

    const auto [heuristicPaths, _] = NearestInsertion((1 - fractionalSolution) * weights, startPositions, endPositions, dependencies, timeout);

    if (heuristicPaths.empty())
        return { heuristicPaths, 0 };

    double sum = 0.0;
    for (size_t a = 0; a < A; ++a)
        for (size_t i = 1; i < heuristicPaths[a].size(); ++i)
            sum += weights(a, heuristicPaths[a][i - 1], heuristicPaths[a][i]);

    return { heuristicPaths, sum };
}

std::tuple<std::vector<std::vector<size_t>>, double> tsplp::NearestInsertion(
    xt::xarray<double> weights, const xt::xtensor<size_t, 1>& startPositions, const xt::xtensor<size_t, 1>& endPositions,
    const DependencyGraph& dependencies, std::chrono::milliseconds timeout)
{
    const auto startTime = std::chrono::steady_clock::now();

    const auto A = startPositions.size();
    assert(endPositions.size() == A);

    if (weights.dimension() == 2)
        weights = xt::repeat(xt::view(weights, xt::newaxis(), xt::all()), A, 0);

    assert(weights.dimension() == 3);

    const auto N = weights.shape(1);
    assert(weights.shape(2) == N);

    boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS> dependencyGraph(N);
    boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> dependencyGraphUndirected(N);
    for (const auto& [u, v] : dependencies.GetArcs())
    {
        add_edge(u, v, dependencyGraph);
        add_edge(u, v, dependencyGraphUndirected);
    }

    for (size_t a = 0; a < A; ++a)
    {
        add_edge(startPositions[a], endPositions[a], dependencyGraph);
        add_edge(startPositions[a], endPositions[a], dependencyGraphUndirected);
    }

    assert(num_edges(dependencyGraph) >= size(dependencies.GetArcs()));

    std::vector<size_t> componentIds(N);
    const auto numberOfComponents = boost::connected_components(dependencyGraphUndirected, componentIds.data());

    std::vector<size_t> order;
    boost::topological_sort(dependencyGraph, std::back_inserter(order));

    std::vector<size_t> component2AgentMap(numberOfComponents, A);

    auto paths = std::vector<std::vector<size_t>>(A);
    double cost = 0;
    for (size_t a = 0; a < A; ++a)
    {
        assert(componentIds[startPositions[a]] == componentIds[endPositions[a]]);

        paths[a].push_back(startPositions[a]);
        paths[a].push_back(endPositions[a]);

        cost += weights(a, startPositions[a], endPositions[a]);

        if (component2AgentMap[componentIds[startPositions[a]]] != A)
            throw IncompatibleDependenciesException();

        component2AgentMap[componentIds[startPositions[a]]] = a;
    }

    std::vector<size_t> lastInsertPositionOfComponent(numberOfComponents, 0);

    for (const auto n : boost::adaptors::reverse(order))
    {
        if (std::chrono::steady_clock::now() >= startTime + timeout)
            return { std::vector<std::vector<size_t>>{}, 0 };

        if (std::find(startPositions.begin(), startPositions.end(), n) != startPositions.end()
            || std::find(endPositions.begin(), endPositions.end(), n) != endPositions.end())
            continue;

        const auto comp = componentIds[n];

        auto minDeltaCost = std::numeric_limits<double>::max();
        auto minA = std::numeric_limits<size_t>::max();
        auto minI = std::numeric_limits<size_t>::max();

        const auto [aRangeFirst, aRangeLast] = component2AgentMap[comp] == A
            ? std::make_pair(static_cast<size_t>(0), A)
            : std::make_pair(component2AgentMap[comp], component2AgentMap[comp] + 1);

        for (size_t a = aRangeFirst; a < aRangeLast; ++a)
        {
            for (size_t i = 1 + lastInsertPositionOfComponent[comp]; i < paths[a].size(); ++i)
            {
                const auto oldCost = weights(a, paths[a][i - 1], paths[a][i]);
                const auto newCost = weights(a, paths[a][i - 1], n) + weights(a, n, paths[a][i]);
                const auto deltaCost = newCost - oldCost;
                if (deltaCost < minDeltaCost)
                {
                    minDeltaCost = deltaCost;
                    minA = a;
                    minI = i;
                }
            }
        }

        using DiffT = decltype(paths[minA].begin())::difference_type;
        paths[minA].insert(paths[minA].begin() + static_cast<DiffT>(minI), n);
        cost += minDeltaCost;

        component2AgentMap[comp] = minA;
        lastInsertPositionOfComponent[comp] = minI;
    }

    return { paths, cost };
}

std::tuple<std::vector<std::vector<size_t>>, double> tsplp::TwoOptPaths(std::vector<std::vector<size_t>> paths,
    xt::xarray<double> weights, const DependencyGraph& dependencies)
{
    assert(weights.dimension() == 2);

    const auto A = paths.size();
    assert(A > 0);

    bool hasImproved = true;
    double improvementSum = 0.0;

    while (hasImproved)
    {
        hasImproved = false;

        for (size_t a1 = 0; a1 < A; ++a1)
        {
            for (size_t a2 = 0; a2 < A; ++a2)
            {
                for (size_t i = 1; i < paths[a1].size() - 1; ++i)
                {
                    const auto jStart = a1 == a2 ? i + 1 : size_t(1);
                    for (size_t j = jStart; j < paths[a2].size() - 1; ++j)
                    {
                        const auto u = paths[a1][i];
                        const auto v = paths[a2][j];

                        if (a1 != a2 &&
                            (!dependencies.GetIncomingSpan(u).empty() || !dependencies.GetIncomingSpan(v).empty() ||
                                !dependencies.GetOutgoingSpan(u).empty() || !dependencies.GetOutgoingSpan(v).empty()))
                            continue;

                        if (a1 == a2)
                        {
                            bool wouldBreakDependency = false;
                            for (auto k = i; k < j; ++k)
                            {
                                if (dependencies.HasArc(u, paths[a1][k + 1]) || dependencies.HasArc(paths[a1][k], v))
                                {
                                    wouldBreakDependency = true;
                                    break;
                                }
                            }

                            if (wouldBreakDependency)
                                continue;
                        }

                        double improvement = 0.0;

                        if (a1 == a2 && j == i + 1)
                        {
                            const auto before1 = weights(paths[a1][i - 1], paths[a1][i    ]);
                            const auto before2 = weights(paths[a1][i    ], paths[a1][i + 1]);
                            const auto before4 = weights(paths[a1][i + 1], paths[a1][i + 2]);
                            const auto after1  = weights(paths[a1][i - 1], paths[a1][i + 1]);
                            const auto after2  = weights(paths[a1][i + 1], paths[a1][i    ]);
                            const auto after4  = weights(paths[a1][i    ], paths[a1][i + 2]);
                            improvement = before1 + before2 + before4 - after1 - after2 - after4;
                        }
                        else
                        {
                            const auto before1 = weights(paths[a1][i - 1], paths[a1][i    ]);
                            const auto before2 = weights(paths[a1][i    ], paths[a1][i + 1]);
                            const auto before3 = weights(paths[a2][j - 1], paths[a2][j    ]);
                            const auto before4 = weights(paths[a2][j    ], paths[a2][j + 1]);
                            const auto after1  = weights(paths[a1][i - 1], paths[a2][j    ]);
                            const auto after2  = weights(paths[a2][j    ], paths[a1][i + 1]);
                            const auto after3  = weights(paths[a2][j - 1], paths[a1][i    ]);
                            const auto after4  = weights(paths[a1][i    ], paths[a2][j + 1]);
                            const auto a1Imp = before1 + before2 - after1 - after2;
                            const auto a2Imp = before3 + before4 - after3 - after4;
                            improvement = a1Imp + a2Imp;
                        }

                        assert(weights(paths[a1][i], paths[a1][i]) == 0);

                        if (improvement > 0.0)
                        {
                            hasImproved = true;
                            std::swap(paths[a1][i], paths[a2][j]);
                            improvementSum += improvement;
                        }
                    }
                }
            }
        }
    }

    return { paths, improvementSum };
}


#include <boost/accumulators/statistics/weighted_extended_p_square.hpp>


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

#ifndef ROOT_FINDER_H
#define ROOT_FINDER_H

#include <iostream>
#include <cassert>
#include <string>
#include <utility>
#include <Eigen/Core>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>

#include "logger.hpp"
#include "error.hpp"
#include "ewsb_solver.hpp"
#include "gsl_utils.hpp"
#include "gsl_vector.hpp"
#include "wrappers.hpp"

namespace flexiblesusy {

/**
 * @class Root_finder
 * @brief Function root finder
 *
 * The user has to provide the function (of which the root should be
 * found) of the type Function_t.  This function gets as arguments an
 * Eigen vector of lenght `dimension' and returns an Eigen vector of
 * the same length.
 *
 * Example:
 * @code
 * auto parabola = [](const Eigen::Matrix<double,2,1>& x) {
 *    const double y = x(0);
 *    const double z = x(1);
 *    Eigen::Matrix<double,2,1> f;
 *    f << y*(y - 5.0), z*(z - 1.0);
 *    return f;
 * };
 *
 * Root_finder<2> root_finder(parabola, 100, 1.0e-5);
 * const double start[2] = { 10, 10 };
 * const int status = root_finder.find_root(start);
 * @endcode
 */
template <std::size_t dimension>
class Root_finder : public EWSB_solver {
public:
   using Vector_t = Eigen::Matrix<double,dimension,1>;
   using Function_t = std::function<Vector_t(const Vector_t&)>;
   enum Solver_type { GSLHybrid, GSLHybridS, GSLBroyden, GSLNewton };

   Root_finder() = default;
   template <typename F>
   Root_finder(F&&, std::size_t, double, Solver_type solver_type_ = GSLHybrid);
   virtual ~Root_finder() = default;

   template <typename F>
   void set_function(F&& f) { function = std::forward<F>(f); }
   void set_precision(double p) { precision = p; }
   void set_max_iterations(std::size_t n) { max_iterations = n; }
   void set_solver_type(Solver_type t) { solver_type = t; }
   int find_root(const Vector_t&);

   // EWSB_solver interface methods
   virtual std::string name() const override { return "Root_finder<" + solver_type_name() + ">"; }
   virtual int solve(const Eigen::VectorXd&) override;
   virtual Eigen::VectorXd get_solution() const override { return root; }

private:
   std::size_t max_iterations{100};    ///< maximum number of iterations
   double precision{1.e-2};            ///< precision goal
   Vector_t root{Vector_t::Zero()};    ///< the root
   Function_t function{nullptr};       ///< function to minimize
   Solver_type solver_type{GSLHybrid}; ///< solver type

   void print_state(const gsl_multiroot_fsolver*, std::size_t) const;
   std::string solver_type_name() const;
   const gsl_multiroot_fsolver_type* solver_type_to_gsl_pointer() const;
   static int gsl_function(const gsl_vector*, void*, gsl_vector*);
};

/**
 * Constructor
 *
 * @param function_ pointer to the function to minimize
 * @param max_iterations_ maximum number of iterations
 * @param precision_ precision goal
 * @param solver_type_ GSL multiroot solver type
 */
template <std::size_t dimension>
template <typename F>
Root_finder<dimension>::Root_finder(
   F&& function_,
   std::size_t max_iterations_,
   double precision_,
   Solver_type solver_type_
)
   : max_iterations(max_iterations_)
   , precision(precision_)
   , function(std::forward<F>(function_))
   , solver_type(solver_type_)
{
}

/**
 * Start the minimization
 *
 * @param start starting point
 *
 * @return GSL error code (GSL_SUCCESS if minimum found)
 */
template <std::size_t dimension>
int Root_finder<dimension>::find_root(const Vector_t& start)
{
   if (!function)
      throw SetupError("Root_finder: function not callable");

   int status;
   std::size_t iter = 0;
   void* parameters = &function;
   gsl_multiroot_function f = {gsl_function, dimension, parameters};

   gsl_multiroot_fsolver* solver
      = gsl_multiroot_fsolver_alloc(solver_type_to_gsl_pointer(), dimension);

   if (!solver) {
      throw OutOfMemoryError(std::string("Cannot allocate gsl_multiroot_fsolver ") +
                             gsl_multiroot_fsolver_name(solver));
   }

#ifndef ENABLE_DEBUG
   gsl_set_error_handler_off();
#endif

   GSL_vector tmp_root = to_GSL_vector(start);

   gsl_multiroot_fsolver_set(solver, &f, tmp_root.raw());

#ifdef ENABLE_VERBOSE
   print_state(solver, iter);
#endif

   do {
      iter++;
      status = gsl_multiroot_fsolver_iterate(solver);

#ifdef ENABLE_VERBOSE
      print_state(solver, iter);
#endif

      if (status)   // check if solver is stuck
         break;

      status = gsl_multiroot_test_residual(solver->f, precision);
   } while (status == GSL_CONTINUE && iter < max_iterations);

   VERBOSE_MSG("\t\t\tRoot_finder status = " << gsl_strerror(status));

   root = to_eigen_vector_fixed<dimension>(solver->x);

   gsl_multiroot_fsolver_free(solver);

   return status;
}

/**
 * Print state of the root finder
 *
 * @param solver solver
 * @param iteration iteration number
 */
template <std::size_t dimension>
void Root_finder<dimension>::print_state(const gsl_multiroot_fsolver* solver,
                                         std::size_t iteration) const
{
   VERBOSE_MSG("\t\t\tIteration " << iteration
               << ": x = " << GSL_vector(solver->x)
               << ", f(x) = " << GSL_vector(solver->f));
}

template <std::size_t dimension>
int Root_finder<dimension>::gsl_function(const gsl_vector* x, void* params, gsl_vector* f)
{
   if (!is_finite(x)) {
      gsl_vector_set_all(f, std::numeric_limits<double>::max());
      return GSL_EDOM;
   }

   Function_t* fun = static_cast<Function_t*>(params);
   int status = GSL_SUCCESS;
   const Vector_t arg(to_eigen_vector_fixed<dimension>(x));
   Vector_t result;
   result.setConstant(std::numeric_limits<double>::max());

   try {
      result = (*fun)(arg);
      // workaround for intel compiler / eigen bug that causes unexpected behavior
      // of allFinite()
      status = IsFinite(result) ? GSL_SUCCESS : GSL_EDOM;
      //status = result.allFinite() ? GSL_SUCCESS : GSL_EDOM;
   } catch (const flexiblesusy::Error&) {
      status = GSL_EDOM;
   }

   copy(result, f);

   return status;
}

template <std::size_t dimension>
std::string Root_finder<dimension>::solver_type_name() const
{
   switch (solver_type) {
   case GSLHybrid : return "GSLHybrid";
   case GSLHybridS: return "GSLHybridS";
   case GSLBroyden: return "GSLBroyden";
   case GSLNewton : return "GSLNewton";
   default:
      throw SetupError("Unknown root solver type: "
                       + std::to_string(solver_type));
   }

   return "unknown";
}

template <std::size_t dimension>
const gsl_multiroot_fsolver_type* Root_finder<dimension>::solver_type_to_gsl_pointer() const
{
   switch (solver_type) {
   case GSLHybrid : return gsl_multiroot_fsolver_hybrid;
   case GSLHybridS: return gsl_multiroot_fsolver_hybrids;
   case GSLBroyden: return gsl_multiroot_fsolver_broyden;
   case GSLNewton : return gsl_multiroot_fsolver_dnewton;
   default:
      throw SetupError("Unknown root solver type: "
                       + std::to_string(solver_type));
   }

   return nullptr;
}

template <std::size_t dimension>
int Root_finder<dimension>::solve(const Eigen::VectorXd& start)
{
   return (find_root(start) == GSL_SUCCESS ?
           EWSB_solver::SUCCESS : EWSB_solver::FAIL);
}

} // namespace flexiblesusy

#endif


#include <cmath>
#include <exception>
#include <fstream>
#include <iostream>
#include <string>
#include <sstream>

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>
#include <boost/tokenizer.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include "../common/tuple_helper.h"
#include "cmd_helpers.h"
#include "binopt.h"

namespace bpo = boost::program_options;

namespace
{
    bpo::options_description initializeOptionsDescription()
    {
        using namespace std;

        bpo::options_description desc("Options");
        desc.add_options()
            ("help,h", "Print help message")
            ("input", bpo::value<string>(),
                    "Filename of a matrix market vector file "
                    "containing the denoised input data set")
            ("levels", bpo::value<string>(),
                "Filename of a matrix market vector file "
                "containing the level set to culster the datapoints to")
            ("output", bpo::value<string>()->default_value("-"),
                    "Filename of the matrix market vector file "
                    "the denoised data should be written to")
            ("rho-d", bpo::value<double>()->default_value(100.0),
                    "Value of the regularization parameter for the data term")
            ("rho-s", bpo::value<double>()->default_value(10.0),
                    "Value of the regularization parameter for the smoothing term")
            ("rho-p", bpo::value<double>()->default_value(0.0),
                    "Value of the regularization parameter for the prior term")
            ("assignments", "Output assignments to levels and not the whole vector")
            ("maxiter", bpo::value<int>()->default_value(-1),
                    "Number of alpha expansion iterations, if set to -1 (default) "
                    "a backtracking level proposal strategy is used.")
            ("prior-distance", bpo::value<double>(),
                    "The distance of two adjacent steps the prior term should "
                    "NOT penalize")
            ("debug,d", "Turn on debug output if flag is set");
            ("debug-graphstructure",
                    "Turn on debug output of graphs and capacities after "
                    "each graph-cut. The files are in graphviz dot notation.");

        return desc;
    }

    bool tryParseProgramOptions(const bpo::options_description& desc,
                                const int argc,
                                const char *argv[],
                                bpo::variables_map& vm)
    {
        using namespace std;
        using namespace boost::program_options;

        bpo::positional_options_description ppos;
        ppos.add("input", 1);
        ppos.add("levels", 2);
        ppos.add("output", 3);

        try {
            store(parse_command_line(argc, argv, desc), vm); // can throw

            if (cmd::isHelpRequest(desc, vm)) {
                return cmd::SUCCESS;
            }

            bool is_valid = true;
            if (! vm.count("input")) {
                cout << "ERROR: 'input' argument is required" << endl;
                is_valid = false;
            }

            if (! vm.count("levels")) {
                cout << "ERROR: 'levels' argument is required" << endl;
                is_valid = false;
            }

            if (! vm.count("output")) {
                cout << "ERROR: 'output' argument is required" << endl;
                is_valid = false;
            }

            if (vm["rho-p"].as<double>() != 0.0 && ! vm.count("prior-distance")) {
                cout << "ERROR: 'prior-distance' is required, if prior term is activated ";
                cout << "('rho-p' > 0.0)" << endl;
                is_valid = false;
            }

            return is_valid;
        }
        catch (error& e)
        {
            std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
            std::cerr << desc << std::endl;
            return false;
        }
    }

    template <typename VectorType>
    void registerCostFunctions(BinaryOptimization<> &bin_opt,
                               VectorType &data,
                               VectorType &weights,
                               VectorType &labels,
                               VectorType &lambdas,
                               double prior_distance)
    {
        typedef std::tuple<int, int> DataTuple;
        typedef std::tuple<int, int, int, int> SmoothTuple;

        auto data_cost_fn = [&data, &weights, &labels, &lambdas](const DataTuple &t, int)
        {
            double value  = data(get<0>(t));
            double weight = weights(get<0>(t));
            double label_value = labels(get<1>(t));

            double cost  = lambdas[0]
                * (1.0 + weight)
                * abs(value - label_value);

            BOOST_ASSERT(cost >= 0);
            return (long long)(cost);
        };

        auto smooth_cost_fn = [&data, &weights, &labels, &lambdas](const SmoothTuple &t, int)
        {
            double weight_1  = weights(get<0>(t));
            double weight_2  = weights(get<1>(t));
            double label_1   = labels(get<2>(t));
            double label_2   = labels(get<3>(t));

            double cost = lambdas[1]
                * (1.0 + weight_1 + weight_2)
                * (label_1 != label_2 ? 1 : 0);

            BOOST_ASSERT(cost >= 0);
            return (long long)(cost);
        };

        auto label_cost_fn = [&data, &weights, &labels, &prior_distance, &lambdas](const SmoothTuple &t, int)
        {
            double weight_1     = weights(get<0>(t));
            double weight_2     = weights(get<1>(t));
            double label_1      = labels(get<2>(t));
            double label_2      = labels(get<3>(t));
            double label_value1 = labels(label_1);
            double label_value2 = labels(label_2);

            double epsilon = 0.05;
            double delta = abs(prior_distance - abs(label_value1 - label_value2));

            double cost = lambdas[2]
                * (delta > epsilon ? 1 : 0)*(label_1 != label_2 ? 1 : 0);

            BOOST_ASSERT(cost >= 0);
            return (long long)(cost);
        };

        bin_opt.setDataCost(data_cost_fn);
            bin_opt.setSmoothnessCost(smooth_cost_fn);

        if (fabs(lambdas[2]) > std::numeric_limits<double>::epsilon())
            bin_opt.setLabelCost(label_cost_fn);
    }

    bool areAssignmentsRequested(const bpo::variables_map &vm)
    {
        if (vm.count("assignments"))
            return true;

        return false;
    }

    template <typename MatType, typename VectorType>
    void collectAssignments(BinaryOptimization<> &bin_opt,
                            const VectorType &weights,
                            const VectorType &labels,
		    	            MatType &mat)
    {
        using namespace std;

        auto assignments = bin_opt.whichLabels();
        mat.resize(assignments.size(), 2);

        for (size_t row = 0; row < assignments.size(); row++)
        {
            size_t lbl_idx = assignments[row];

            mat(row, 0) = labels(lbl_idx);
            mat(row, 1) = weights(row);
        }
    }

    template <typename VectorType>
    bool saveAssignments(const bpo::variables_map &vm,
                         const VectorType &weights,
                         const VectorType &labels,
		                 BinaryOptimization<> &bin_opt)
    {
        using namespace std;

        boost::numeric::ublas::matrix<double> assignments;
        collectAssignments(bin_opt, weights, labels, assignments);

        return cmd::saveOutputMatrix(vm, assignments);
    }

    template <typename VectorType>
    void postprocessAssignments(const bpo::variables_map &vm,
		    		            BinaryOptimization<> &bin_opt,
				                const VectorType &input,
				                const VectorType &weights,
				                const VectorType &labels,
				                VectorType &output)
    {
        using namespace std;

        output.resize(input.size());

        boost::numeric::ublas::matrix<double> assignments;
        collectAssignments(bin_opt, weights, labels, assignments);

        size_t i = 0;
        double total_weight = 0.0;
        for (size_t j = 0; j < assignments.size1(); j++)
        {
            double val = assignments(j, 0);
            double weight = assignments(j, 1);

            total_weight += weight;

            for (size_t w = 0; w < weight; w++){
                output(i) = val;
                i++;
            }
        }

        //BOOST_LOG_TRIVIAL(debug) << "Total weight: " << total_weight;
        //BOOST_LOG_TRIVIAL(debug) << "Output size:  " << output.size();
    }

    int runProgram(const bpo::options_description& desc,
                   const bpo::variables_map& vm)
    {
        using namespace std;
        typedef boost::numeric::ublas::vector<double> VectorType;

        VectorType input, levels, output;

        if (! cmd::loadInputVectorAndAdjustOthers(vm, input, output))
            return cmd::ERROR_UNHANDLED_EXCEPTION;
        BOOST_LOG_TRIVIAL(debug) << "Loaded input vector with " << input.size() << " samples.";

        if (! cmd::loadLevelsVector(vm, levels))
            return cmd::ERROR_UNHANDLED_EXCEPTION;
        BOOST_LOG_TRIVIAL(debug) << "Loaded levels vector with " << levels.size() << " elements.";

        VectorType data, weights;
        helpers::postprocessTVDNData(input, data, weights);
        BOOST_LOG_TRIVIAL(debug) << "Compressed input vector into " << data.size()
                                 << " (data, weight) tuples.";

        VectorType lambdas;
        cmd::loadLambdas(vm, lambdas);

        double prior_distance = vm.count("prior-distance")
                                    ? vm["prior-distance"].as<double>()
                                    : 0.0;

        BinaryOptimization<> bin_opt(data.size(), levels.size());
        if (vm.count("debug-graphstructure"))
            bin_opt.recordEnergyGraphDumps();

        registerCostFunctions(bin_opt,
                              data,
                              weights,
                              levels,
                              lambdas,
                              prior_distance);

        auto energy = bin_opt.expansion(vm["maxiter"].as<int>());

        if (areAssignmentsRequested(vm)) {
            saveAssignments(vm, weights, levels, bin_opt);
            return cmd::SUCCESS;
        }

        postprocessAssignments(vm, bin_opt, input, weights, levels, output);

        if (! cmd::saveOutputVector(vm, output))
            return cmd::ERROR_UNHANDLED_EXCEPTION;

        return cmd::SUCCESS;
    }
}

int main(const int argc, const char *argv[])
{
    using namespace std;

    auto desc = initializeOptionsDescription();

    try
    {
        // Define and parse the program options
        bpo::variables_map vm;
        if (! tryParseProgramOptions(desc, argc, argv, vm)) {
            return cmd::ERROR_IN_COMMAND_LINE;
        }

        cmd::configureLogging(vm.count("debug"));
        return runProgram(desc, vm);
    }
    catch(std::exception& e)
    {
        cerr << "Unhandled Exception reached the top of main: "
                << e.what() << ", application will now exit" << endl;

        return cmd::ERROR_UNHANDLED_EXCEPTION;
    }

}


// __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__

// TestGeoReference.h
#include <gtest/gtest_VW.h>
#include <test/Helpers.h>

#include <vw/Cartography/PointImageManipulation.h>
#include <boost/math/special_functions/fpclassify.hpp>

using namespace vw;
using namespace vw::cartography;
using namespace vw::test;

// Spherical transform that is ignorant of flattened datums.
TEST( PointImageManipulation, XYZ_to_LonLat ) {
  // Test a full forward and reverse transformation
  Vector3 xyz(-2197110.000000, 1741355.875000, 1898886.875000);
  Vector3 lon_lat_alt = xyz_to_lon_lat_radius(xyz);
  Vector3 xyz2 = lon_lat_radius_to_xyz(lon_lat_alt);

  EXPECT_VECTOR_NEAR( xyz, xyz2, 1e-2 );

  // Test to see if things still work for West positive coordinate
  // systems.
  lon_lat_alt = xyz_to_lon_lat_radius(xyz,false);
  xyz2 = lon_lat_radius_to_xyz(lon_lat_alt,false);

  EXPECT_VECTOR_NEAR( xyz, xyz2, 1e-2 );

  // See if it still works if using 0-360 range
  xyz[1] = -xyz[1];
  lon_lat_alt = xyz_to_lon_lat_radius(xyz,true,false);
  xyz2 = lon_lat_radius_to_xyz(lon_lat_alt);

  EXPECT_VECTOR_NEAR( xyz, xyz2, 1e-2 );
}

// These are the more general operators which can actually handle
// squished datums.
TEST( PointImageManipulation, GeodeticCartesian ) {
  ImageView<Vector3> geodetic(2,2);
  geodetic(0,0) = Vector3( 90, 10, 100 );
  geodetic(0,1) = Vector3(); // An actual valid measurement.
  geodetic(1,0) = Vector3( 10, 89, 9  );
  geodetic(1,1) = Vector3(0, 0, std::numeric_limits<double>::quiet_NaN() ); // invalid measure.

  Datum moon("D_MOON"), earth("WGS84");
  ImageView<Vector3> result_moon = cartesian_to_geodetic(geodetic_to_cartesian(geodetic,moon),moon);
  ImageView<Vector3> result_earth = cartesian_to_geodetic(geodetic_to_cartesian(geodetic,earth),earth);
  EXPECT_RANGE_NEAR( geodetic.begin(), geodetic.begin()+3, result_moon.begin(), result_moon.begin()+3, 1e-9 );
  EXPECT_TRUE( boost::math::isnan(result_moon(1,1).z()) );
  EXPECT_RANGE_NEAR( geodetic.begin(), geodetic.begin()+3, result_earth.begin(), result_earth.begin()+3, 3e-9 ); // earth is bigger, so bigger error too
  EXPECT_TRUE( boost::math::isnan(result_earth(1,1).z()) );
}

TEST( PointImageManipulation, CartesianGeodetic ) {
  ImageView<Vector3> cartesian(2,2);
  cartesian(0,0) = Vector3(1737892,80,5320);
  cartesian(1,0) = Vector3(50,-190,-80);
  cartesian(0,1) = Vector3(33e8, -1e5, 0);
  cartesian(1,1) = Vector3(); // Invalid in for cartesian

  Datum moon("D_MOON"), earth("WGS84");
  ImageView<Vector3> result_moon = geodetic_to_cartesian(cartesian_to_geodetic(cartesian,moon),moon);
  ImageView<Vector3> result_earth = geodetic_to_cartesian(cartesian_to_geodetic(cartesian,earth),earth);
  EXPECT_SEQ_NEAR( cartesian, result_moon, 1e-6 );
  EXPECT_SEQ_NEAR( cartesian, result_earth, 1e-6 );
}


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

//------------------------------------------------------------------------------
// KeypointSpatialIndex.cpp
//
// Allows to construction, query, and removal of Keypoint entries based on pixel position.
//------------------------------------------------------------------------------

#include "KeypointSpatialIndex.h"

#include <boost\function_output_iterator.hpp>
// TODO:  feed back to Boost that 4457 is firing
#pragma warning (push, 1)
#pragma warning (disable:4457)
#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost\geometry\index\rtree.hpp>
#pragma warning (pop)

using namespace cv;
using namespace std;

namespace mage
{
    // TODO PERF optimize this.  Box queries seem to typically return about 16 hits.
    using indexType = boost::geometry::index::rstar<12>;
    using rtree_point = boost::geometry::model::point<float, 3, boost::geometry::cs::cartesian>;
    using rtree_box = boost::geometry::model::box<rtree_point>;
    using rtree_value = tuple<rtree_point, size_t>;
    using rtree = boost::geometry::index::rtree<rtree_value, indexType>;

    struct KeypointSpatialIndex::Impl
    {
        rtree index;

        Impl(gsl::span<const rtree_value> values);
    };

    KeypointSpatialIndex::Impl::Impl(gsl::span<const rtree_value> values) :
        index(values.begin(), values.end())
    {
    }

    KeypointSpatialIndex::KeypointSpatialIndex(gsl::span<const cv::KeyPoint> keypoints)
    {
        // TODO PERF and cleanliness:  use range adapters to create.
        // http://www.boost.org/doc/libs/1_58_0/libs/geometry/doc/html/geometry/spatial_indexes/rtree_examples/range_adaptors.html
        vector<rtree_value> valuesStruct;
        valuesStruct.reserve(keypoints.size());

        for (ptrdiff_t i = 0; i < keypoints.size(); ++i)
        {
            valuesStruct.emplace_back(rtree_point(keypoints[i].pt.x, keypoints[i].pt.y, keypoints[i].octave * octaveSpacing), i);
        }

        m_impl = std::make_unique<KeypointSpatialIndex::Impl>(valuesStruct);
    }

    KeypointSpatialIndex::~KeypointSpatialIndex()
    {}

    struct index_emplace_back
    {
        index_emplace_back& operator =(const rtree_value& value)
        {
            results.emplace_back(get<1>(value));
            return *this;
        }

        index_emplace_back& operator *()
        {
            return *this;
        }

        index_emplace_back& operator++()
        {
            return *this;
        }

        index_emplace_back(temp::vector<size_t>& output)
            : results{ output }
        {}

        temp::vector<size_t>& results;
    };

    void KeypointSpatialIndex::Query(const cv::Point2f& center, int octave, float radius, temp::vector<size_t>& results) const
    {
        rtree_box queryBox{
            rtree_point{ center.x - radius, center.y - radius, octave * octaveSpacing - octaveQueryRange },
            rtree_point{ center.x + radius, center.y + radius, octave * octaveSpacing + octaveQueryRange }
        };

        m_impl->index.query(boost::geometry::index::intersects(queryBox), index_emplace_back{ results });
    }

    void KeypointSpatialIndex::Remove(const cv::KeyPoint& keypoint, size_t value)
    {
        size_t elem = m_impl->index.remove(
            rtree_value(rtree_point(keypoint.pt.x, keypoint.pt.y, keypoint.octave * octaveSpacing), value));
        (void)elem;
        assert(elem == 1);
    }
}


#include <iostream>

#include <type_traits>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/solver/runtime.hpp>
#include <amgcl/make_solver.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/backend/builtin.hpp>
#include <amgcl/adapter/crs_tuple.hpp>

#include "amgcl.h"

#ifdef AMGCL_PROFILING
#include <amgcl/profiler.hpp>
namespace amgcl {
    profiler<> prof;
}
#endif

//---------------------------------------------------------------------------
typedef amgcl::backend::builtin<double>           Backend;
typedef amgcl::amg<Backend, amgcl::runtime::coarsening::wrapper, amgcl::runtime::relaxation::wrapper> AMG;
typedef amgcl::runtime::solver::wrapper<Backend>  ISolver;
typedef amgcl::make_solver<AMG, ISolver>          Solver;
typedef boost::property_tree::ptree               Params;

//---------------------------------------------------------------------------
amgclHandle STDCALL amgcl_params_create() {
    return static_cast<amgclHandle>( new Params() );
}

//---------------------------------------------------------------------------
void STDCALL amgcl_params_seti(amgclHandle prm, const char *name, int value) {
    static_cast<Params*>(prm)->put(name, value);
}

//---------------------------------------------------------------------------
void STDCALL amgcl_params_setf(amgclHandle prm, const char *name, float value) {
    static_cast<Params*>(prm)->put(name, value);
}

//---------------------------------------------------------------------------
void STDCALL amgcl_params_sets(amgclHandle prm, const char *name, const char *value) {
    static_cast<Params*>(prm)->put(name, value);
}

//---------------------------------------------------------------------------
void STDCALL amgcl_params_read_json(amgclHandle prm, const char *fname) {
    read_json(fname, *static_cast<Params*>(prm));
}

//---------------------------------------------------------------------------
void STDCALL amgcl_params_destroy(amgclHandle prm) {
    delete static_cast<Params*>(prm);
}

//---------------------------------------------------------------------------
amgclHandle STDCALL amgcl_precond_create(
        int           n,
        const int    *ptr,
        const int    *col,
        const double *val,
        amgclHandle   prm
        )
{
    auto A = std::make_tuple(n,
            boost::make_iterator_range(ptr, ptr + n + 1),
            boost::make_iterator_range(col, col + ptr[n]),
            boost::make_iterator_range(val, val + ptr[n])
            );

    if (prm)
        return static_cast<amgclHandle>(new AMG(A, *static_cast<Params*>(prm)));
    else
        return static_cast<amgclHandle>(new AMG(A));
}

//---------------------------------------------------------------------------
amgclHandle STDCALL amgcl_precond_create_f(
        int           n,
        const int    *ptr,
        const int    *col,
        const double *val,
        amgclHandle   prm
        )
{
    auto ptr_c = boost::make_transform_iterator(ptr, [](int i){ return i - 1; });
    auto col_c = boost::make_transform_iterator(col, [](int i){ return i - 1; });

    auto A = std::make_tuple(n,
            boost::make_iterator_range(ptr_c, ptr_c + n + 1),
            boost::make_iterator_range(col_c, col_c + ptr[n]),
            boost::make_iterator_range(val, val + ptr[n])
            );

    if (prm)
        return static_cast<amgclHandle>(new AMG(A, *static_cast<Params*>(prm)));
    else
        return static_cast<amgclHandle>(new AMG(A));
}

//---------------------------------------------------------------------------
void STDCALL amgcl_precond_apply(amgclHandle handle, const double *rhs, double *x)
{
    AMG *amg = static_cast<AMG*>(handle);

    size_t n = amgcl::backend::rows(amg->system_matrix());

    boost::iterator_range<double*> x_range =
        boost::make_iterator_range(x, x + n);

    amg->apply(boost::make_iterator_range(rhs, rhs + n), x_range);
}

//---------------------------------------------------------------------------
void STDCALL amgcl_precond_report(amgclHandle handle) {
    std::cout << *static_cast<AMG*>(handle) << std::endl;
}

//---------------------------------------------------------------------------
void STDCALL amgcl_precond_destroy(amgclHandle handle) {
    delete static_cast<AMG*>(handle);
}

//---------------------------------------------------------------------------
amgclHandle STDCALL amgcl_solver_create(
        int           n,
        const int    *ptr,
        const int    *col,
        const double *val,
        amgclHandle   prm
        )
{
    auto A = std::make_tuple(n,
            boost::make_iterator_range(ptr, ptr + n + 1),
            boost::make_iterator_range(col, col + ptr[n]),
            boost::make_iterator_range(val, val + ptr[n])
            );

    if (prm)
        return static_cast<amgclHandle>(new Solver(A, *static_cast<Params*>(prm)));
    else
        return static_cast<amgclHandle>(new Solver(A));
}

//---------------------------------------------------------------------------
amgclHandle STDCALL amgcl_solver_create_f(
        int           n,
        const int    *ptr,
        const int    *col,
        const double *val,
        amgclHandle   prm
        )
{
    auto ptr_c = boost::make_transform_iterator(ptr, [](int i){ return i - 1; });
    auto col_c = boost::make_transform_iterator(col, [](int i){ return i - 1; });

    auto A = std::make_tuple(n,
            boost::make_iterator_range(ptr_c, ptr_c + n + 1),
            boost::make_iterator_range(col_c, col_c + ptr[n]),
            boost::make_iterator_range(val, val + ptr[n])
            );

    if (prm)
        return static_cast<amgclHandle>(new Solver(A, *static_cast<Params*>(prm)));
    else
        return static_cast<amgclHandle>(new Solver(A));
}

//---------------------------------------------------------------------------
void STDCALL amgcl_solver_report(amgclHandle handle) {
    std::cout << static_cast<Solver*>(handle)->precond() << std::endl;
}

//---------------------------------------------------------------------------
void STDCALL amgcl_solver_destroy(amgclHandle handle) {
    delete static_cast<Solver*>(handle);
}

//---------------------------------------------------------------------------
conv_info STDCALL amgcl_solver_solve(
        amgclHandle handle,
        const double *rhs,
        double *x
        )
{
    Solver *slv = static_cast<Solver*>(handle);

    size_t n = slv->size();

    conv_info cnv;

    boost::iterator_range<double*> x_range = boost::make_iterator_range(x, x + n);

    std::tie(cnv.iterations, cnv.residual) = (*slv)(
            boost::make_iterator_range(rhs, rhs + n), x_range
            );

    return cnv;
}

//---------------------------------------------------------------------------
void STDCALL amgcl_solver_solve_f(
        amgclHandle handle,
        const double *rhs,
        double *x,
        conv_info *cnv
        )
{
    *cnv = amgcl_solver_solve(handle, rhs, x);
}

//---------------------------------------------------------------------------
conv_info STDCALL amgcl_solver_solve_mtx(
        amgclHandle handle,
        int    const * A_ptr,
        int    const * A_col,
        double const * A_val,
        const double *rhs,
        double *x
        )
{
    Solver *slv = static_cast<Solver*>(handle);

    size_t n = slv->size();

    conv_info cnv;

    boost::iterator_range<double*> x_range = boost::make_iterator_range(x, x + n);

    std::tie(cnv.iterations, cnv.residual) = (*slv)(
            std::make_tuple(
                n,
                boost::make_iterator_range(A_ptr, A_ptr + n + 1),
                boost::make_iterator_range(A_col, A_col + A_ptr[n]),
                boost::make_iterator_range(A_val, A_val + A_ptr[n])
                ),
            boost::make_iterator_range(rhs, rhs + n), x_range
            );

    return cnv;
}

//---------------------------------------------------------------------------
void STDCALL amgcl_solver_solve_mtx_f(
        amgclHandle handle,
        int    const * A_ptr,
        int    const * A_col,
        double const * A_val,
        const double *rhs,
        double *x,
        conv_info *cnv
        )
{
    Solver *slv = static_cast<Solver*>(handle);

    size_t n = slv->size();

    boost::iterator_range<double*> x_range = boost::make_iterator_range(x, x + n);

    auto ptr_c = boost::make_transform_iterator(A_ptr, [](int i){ return i - 1; });
    auto col_c = boost::make_transform_iterator(A_col, [](int i){ return i - 1; });

    std::tie(cnv->iterations, cnv->residual) = (*slv)(
            std::make_tuple(
                n,
                boost::make_iterator_range(ptr_c, ptr_c + n + 1),
                boost::make_iterator_range(col_c, col_c + A_ptr[n]),
                boost::make_iterator_range(A_val, A_val + A_ptr[n])
                ),
            boost::make_iterator_range(rhs, rhs + n), x_range
            );
}


#include <boost/graph/distributed/eager_dijkstra_shortest_paths.hpp>


#include <boost/accumulators/statistics/weighted_moment.hpp>