#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseCholesky>

#include <vector>
#include <memory>

#include "SDOT/SemiDiscreteOT.h"
#include "SDOT/Distances/Wasserstein2.h"

#include "SDOT/PolygonRasterize.h"
#include "SDOT/RegularGrid.h"
#include "SDOT/DiscretizedDistribution.h"

using namespace sdot;
using namespace sdot::distances;

void LaguerreDiagramTest()
{
  int numPts = 2;

  Eigen::VectorXd prices = Eigen::VectorXd::Ones(numPts);
  prices << 1, 0.873;//, 0.978674;//, 0.851588;//, 0.996466;

  Eigen::Matrix2Xd pts(2,numPts);
  pts << 0.823295, 0.329554,
         0.604897, 0.536459;
  //pts = Eigen::Matrix2Xd::Random(2,numPts).cwiseAbs();

  Eigen::VectorXd discrProbs(numPts);
  discrProbs = (1.0/numPts)*Eigen::VectorXd::Ones(numPts);

  LaguerreDiagram lagDiag(0, 1, 0, 1, pts, prices);

  LaguerreDiagram::Point_2 startPt = std::get<1>(lagDiag.InternalEdges(0).at(0));
  LaguerreDiagram::Point_2 endPt = std::get<2>(lagDiag.InternalEdges(0).at(0));

  std::cout << "Dividing edge = (" << CGAL::to_double(startPt.x()) << "," << CGAL::to_double(startPt.y()) << ") -> (" << CGAL::to_double(endPt.x()) << "," << CGAL::to_double(endPt.y()) << ")" << std::endl;
  LaguerreDiagram::Point_2 midPt = startPt + 0.5*(endPt-startPt);

  double dist1 = std::sqrt( CGAL::to_double(CGAL::squared_distance(startPt,midPt)));
  double dist2 = std::sqrt( CGAL::to_double(CGAL::squared_distance(endPt,midPt)));

  assert( std::abs( (dist1 - prices(0)) - (dist2 - prices(0)))<1e-14 );
}

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

  int numPts = 100;

  Eigen::VectorXd prices = Eigen::VectorXd::Ones(numPts);

  Eigen::Matrix2Xd pts = Eigen::Matrix2Xd::Random(2,numPts).cwiseAbs();

  std::cout << "Points = \n";
  std::cout << "[[" << pts(0,0) << "," << pts(1,0) << "]";
  for(int i=1; i<numPts; ++i){
    std::cout << ", [" << pts(0,i) << "," << pts(1,i) << "]";
  }
  std::cout << "]" << std::endl;

  Eigen::VectorXd discrProbs(numPts);
  discrProbs << (1.0/numPts)*Eigen::VectorXd::Ones(numPts);

  Eigen::Matrix2Xd domain(2,4);
  domain << 0.0, 1.0, 1.0, 0.0,
            0.0, 0.0, 1.0, 1.0;

  // Construct the continuous distribution
  auto grid = std::make_shared<RegularGrid>(domain(0,0),domain(1,0), domain(0,2), domain(1,2), 10,10);

  // Unnormalized density.  Will be normalized in DiscretizedDistribution constructor
  Eigen::MatrixXd density = Eigen::MatrixXd::Ones(grid->NumCells(0), grid->NumCells(1));

  auto dist = std::make_shared<DiscretizedDistribution>(grid, density);

  // Evalaute the SDOT objective
  auto sdot = std::make_shared<SemidiscreteOT<Wasserstein2>>(dist, pts, discrProbs);

  Eigen::VectorXd optPrices;
  double optVal;
  std::tie(optPrices,optVal) = sdot->Solve(Eigen::VectorXd::Ones(numPts));
  std::cout << "Optimal prices = " << optPrices.transpose() << std::endl;

  std::shared_ptr<LaguerreDiagram> lagDiag = sdot->Diagram();

  std::cout << "Laguerre cells = " << std::endl;
  for(int polyInd=0; polyInd<numPts; ++polyInd){
    std::shared_ptr<PolygonRasterizeIter::Polygon_2> poly = lagDiag->GetCell(polyInd)->ToCGAL();

    auto vertIt = poly->vertices_begin();
    std::cout << "[[" << vertIt->x() << "," << vertIt->y() << "]";
    vertIt++;
    for(;  vertIt != poly->vertices_end(); ++vertIt){
      std::cout << ", [" << vertIt->x() << "," << vertIt->y() << "]";
    }
    std::cout << "]" << std::endl;
  }

  return 0;
}


#ifndef CX_SVD_HPP
#define CX_SVD_HPP

#include "print.hpp"
//#include "cx_modify.hpp"
#include "sub.hpp"
#include "../tools.hpp"
// #include "cx_multiply.hpp"
// #include "cx_special.hpp"

#include <mkl.h>

#include <boost/timer/timer.hpp>
#include <boost/chrono.hpp>

#include <vector>
#include <utility>
#include <complex>

// inline bool comparator ( const max_pair& l, const max_pair& r) { return l.first > r.first; }

namespace dqmc {
    namespace la {

	inline perm_mat cx_find_col_perm(cx_mat&__restrict in,
					 cx_mat&__restrict out) {
	    using namespace std;
	    using namespace Eigen;
	    mat_t::Index min_col;
	    ivec col_ids(in.cols());
	    for (int i = 0; i < in.rows(); ++i) {
		in.col(i).cwiseAbs().minCoeff(&min_col);
		cout << min_col << endl;
		col_ids[i] = int(min_col) - 1;
	    }
	    // for (std::vector<max_pair>::iterator it=min_indices.begin();
	    // 	 it != min_indices.end(); ++it) {
	    // 	col_ids((*it).second) = std::distance(min_indices.begin(), it);
	    // 	// cout << row_ids((*it).second) <<  " ";
	    // }
	    // // cout << endl;
	    perm_mat col_perm(col_ids);

	    out = in * col_perm.transpose();
	    return col_perm;
	}

	inline perm_mat cx_row_sort(cx_mat&__restrict in, cx_mat&__restrict out) {
	    using namespace std;
	    using namespace Eigen;
	    vector<int> max_index(in.rows());
	    vector<max_pair> max_indices;
	    vec max_vals(in.rows());
	    mat A = in.cwiseAbs().real().cast<double>();

	    for(int i = 0; i < in.rows(); ++i) {
		max_vals(i) = A.cwiseAbs().real().row(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec row_ids(in.rows());
	    for (std::vector<max_pair>::iterator it=max_indices.begin(); it != max_indices.end(); ++it) {
		row_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    perm_mat row_perm(row_ids);

	    out = row_perm * in;
	    return row_perm;
	}

	inline perm_mat cx_col_sort(cx_mat&__restrict in,
				    cx_mat&__restrict out) {
	    using namespace std;
	    using namespace Eigen;
	    vector<int> max_index(in.cols());
	    vector<max_pair> max_indices;
	    vec max_vals(in.cols());
	    mat A = in.cwiseAbs().real().cast<double>();
	    for(int i = 0; i < in.cols(); ++i) {
		max_vals(i) = A.col(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec col_ids(in.cols());
	    for (std::vector<max_pair>::iterator it=max_indices.begin();
		 it != max_indices.end(); ++it) {
		col_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    perm_mat col_perm(col_ids);

	    out = in * col_perm;
	    return col_perm;
	}

	inline void decompose_svd(cx_mat&__restrict__ in,
				  cx_mat&__restrict__ U,
				  vec&__restrict__ D,
				  cx_mat&__restrict__ T) {
	    int lwork = -1;
	    int m = in.rows();
	    int n = in.cols();
	    int lda = m;
	    int ldu = m;
	    int ldvt = n;
	    cx_double wkopt;
	    vec_t rwork(5 * m);
	    int info = -1;

	    cx_mat_t U_temp(m, m);
	    vec D_temp(m);
	    vec_t superb(std::min(m, n) - 1);
	    info = LAPACKE_zgesvd( LAPACK_COL_MAJOR, 'A', 'A', m, n, in.data(), m,
				   D_temp.data(), U_temp.data(), m, T.data(), n, superb.data());
	    U = U_temp.block(0, 0, m, n);
	    D = D_temp.segment(0, n);
	    // lwork = -1;
	    // zgesvd( "All", "All", &m, &n, in.data(), &lda, D.data(), U.data(), &ldu, T.data(),
	    // 	    &ldvt, &wkopt, &lwork,
	    // 	    rwork.data(), &info );
	    // lwork = int(wkopt.real());
	    // cx_vec_t work(lwork);
	    // /* Compute SVD */
	    // zgesvd( "All", "All", &m, &n, in.data(), &lda, D.data(), U.data(), &ldu, T.data(), &ldvt, work.data(), &lwork,
	    // 	    rwork.data(), &info );

	}

	inline void decompose_udt_full_piv(cx_mat&__restrict__ in,
					   cx_mat&__restrict__ U,
					   vec&__restrict__ D,
					   cx_mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    if (U.rows() != in.rows() || U.cols() != in.cols()) {
		cout << U.rows() << " vs. " << in.rows() << "\tand\t"
		     << U.cols() << " vs. " << in.cols() << endl;
		throw std::runtime_error("dimensions of U are wrong");
	    }

	    if (D.size() != in.cols()) {
		cout << D.size() << endl;
		throw std::runtime_error("dimensions of D are wrong");
	    }

	    if (T.rows() != in.cols() || U.cols() != in.cols()) {
		cout << T.rows() << " vs. " << in.cols() << "\tand\t"
		     << U.cols() << " vs. " << in.cols() << endl;
		throw std::runtime_error("dimensions of T are wrong");
	    }

	    cx_mat_t scaled_in = in;
	    // perm_mat row_perm = dqmc::la::cx_row_sort(in, scaled_in);
	    // perm_mat col_perm = dqmc::la::cx_col_sort(scaled_in, in);

	    Eigen::FullPivHouseholderQR<cx_mat> qr(in);

	    qr.compute(in);
	    // U = row_perm.inverse() * qr.matrixQ().block(0, 0, in.rows(), in.cols());
	    U = qr.matrixQ().block(0, 0, in.rows(), in.cols());

	    cx_mat upper = qr.matrixQR().triangularView<Upper>();// .block(0, 0, in.cols(), in.cols());
	    // cout << "upper" << endl << upper << endl << endl;
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal().real().cast<double>();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    // std::cout << "culprit " << D(i) << " was "
		    // 	      << upper(i, i) << std::endl;
		    dqmc::tools::abort("Invalid upper triangle"); }
		    // throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) { D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }
	    // cout << "D " << endl << D.transpose() << endl << endl;
	    // cout << "inv D " << endl << d_inv.transpose() << endl << endl;
	    T = (d_inv.asDiagonal() * upper.block(0, 0, in.cols(), in.cols()))
	    	* qr.colsPermutation().transpose(); // * col_perm.inverse();
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}

	inline void decompose_udt_full_sort(cx_mat&__restrict__ in,
					    cx_mat&__restrict__ U,
					    vec&__restrict__ D,
					    cx_mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    if (U.rows() != in.rows() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of U are wrong");

	    if (D.size() != in.cols()) {
		cout << D.size() << endl;
		throw std::runtime_error("dimensions of D are wrong");
	    }

	    if (T.rows() != in.cols() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of T are wrong");

	    cx_mat_t scaled_in;// = cx_mat_t::Zero(in.rows(), in.cols());
	    scaled_in.fill(cx_double(0., 0.));
	    perm_mat row_perm = dqmc::la::cx_row_sort(in, scaled_in);
	    perm_mat col_perm = dqmc::la::cx_col_sort(scaled_in, in);

	    // Very crude scaling...
	    //	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.5);
	    double scale = 1.;

	    cx_vec_t taus(in.cols());
	    ivec iwork(in.cols());

	    // vec work(in.rows() * in.rows() * in.rows());
	    int lwork = -1;
	    // int info = 0;
	    int M = in.rows();
	    int N = in.cols();

	    scaled_in.block(0, 0, in.rows(), in.cols()) = in;

	    iwork.setZero();
	    lapack_int info = LAPACKE_zgeqp3( LAPACK_COL_MAJOR, M, N, scaled_in.data(),
					      M, iwork.data(), taus.data() );

	    // cout << iwork.size() << " " << work.size() << " " << taus.size() << endl;
	    // lwork = -1;
	    // zgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
	    // 	    taus.data(), taus.data(),
	    // 	    &lwork, &info);
	    // lwork = taus(0);
	    // cx_vec_t work(lwork);
	    // work.setZero();
	    // taus.setZero();

	    // zgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
	    // 	    taus.data(), work.data(),
	    // 	    &lwork, &info);

	    cx_mat_t upper = scaled_in.triangularView<Upper>();
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal().real().cast<double>();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    // std::cout << "culprit " << D(i) << " was "
		    // 	      << upper(i, i) << std::endl;
		    dqmc::tools::abort("Invalid upper triangle"); }
		    // throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) { D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }
	    lwork = -1;

	    info = LAPACKE_zungqr( LAPACK_COL_MAJOR, M, N, N,
				   scaled_in.data(), M, taus.data() );

	    // zungqr_(&M, &N, &N, scaled_in.data(), &M, taus.data(),
	    // 	    work.data(), &lwork, &info);
	    // // if (work(0) > work.size()) {
	    // // 	cout << "Need more lwork for dorgqr" << endl;
	    // // }
	    // lwork = work(0);
	    // work.resize(lwork);
	    // // cout << "in" << endl << scaled_in << endl << endl;
	    // zungqr_(&M, &N, &N, scaled_in.data(), &M, taus.data(),
	    // 	    work.data(), &lwork, &info);
	    // cout << "out" << endl << scaled_in << endl << endl;

	    U = row_perm.inverse() * scaled_in.block(0, 0, in.rows(), in.cols());
	    ivec col_ids(in.cols());
	    for (int i = 0; i < in.cols(); ++i) {
		col_ids(i) = iwork(i) - 1;
	    }

	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> lapack_col_perm(col_ids);
	    T = (d_inv.cast<cx_double_t>().asDiagonal()
		 * upper.block(0, 0, in.cols(), in.cols()))
		* lapack_col_perm.transpose() * col_perm.transpose();
	    D *= scale;
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}

	inline void decompose_udt_col_piv(cx_mat&__restrict__ in,
					    cx_mat&__restrict__ U,
					    vec&__restrict__ D,
					    cx_mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;
	    if (U.rows() != in.rows() || U.cols() != in.cols() || U.cols() == 0) {
		cout << U.rows() << " " << U.cols() << endl;
		throw std::runtime_error("dimensions of U are wrong");
	    }

	    if (D.size() != in.cols() || D.size() == 0) {
		cout << D.size() << endl;
		throw std::runtime_error("dimensions of D are wrong");
	    }

	    if (T.rows() != in.cols() || T.cols() == 0)
		throw std::runtime_error("dimensions of T are wrong");

	    cx_mat_t scaled_in = cx_mat_t::Zero(in.rows(), in.cols());
	    scaled_in.fill(cx_double(0., 0.));

	    cx_vec_t taus(in.cols());
	    ivec iwork(in.cols());

	    int lwork = -1;
	    int M = in.rows();
	    int N = in.cols();

	    perm_mat row_sort_perm = cx_row_sort(in, scaled_in);
	    // scaled_in.block(0, 0, in.rows(), in.cols()) = in;
	    iwork.setZero();
	    lapack_int info = LAPACKE_zgeqp3( LAPACK_COL_MAJOR, M, N, scaled_in.data(),
					      M, iwork.data(), taus.data() );

	    cx_mat_t upper = scaled_in.triangularView<Upper>();
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal().real().cast<double>();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    // std::cout << "culprit " << D(i) << " was "
		    // 	      << upper(i, i) << std::endl;
		    dqmc::tools::abort("Invalid upper triangle");
		    // throw std::runtime_error("Invalid upper triangle");
		}
		d_inv(i) = 1./D(i);
		if (D(i) < 0) { D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }
	    lwork = -1;

	    info = LAPACKE_zungqr( LAPACK_COL_MAJOR, M, N, N,
				   scaled_in.data(), M, taus.data() );

	    U = (row_sort_perm.inverse() * scaled_in).block(0, 0, in.rows(), in.cols());
	    ivec col_ids(in.cols());
	    for (int i = 0; i < in.cols(); ++i) {
		col_ids(i) = iwork(i) - 1;
	    }

	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> lapack_col_perm(col_ids);
	    T = (d_inv.cast<cx_double_t>().asDiagonal()
		 * upper.block(0, 0, in.cols(), in.cols()))
		* lapack_col_perm.transpose();
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}

	inline void decompose_udt(cx_mat&__restrict__ in,
				  cx_mat&__restrict__ U,
				  vec&__restrict__ D,
				  cx_mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;
	    if (U.rows() != in.rows() || U.cols() != in.cols() || U.cols() == 0) {
		cout << U.rows() << " " << U.cols() << endl;
		throw std::runtime_error("dimensions of U are wrong");
	    }

	    if (D.size() != in.cols() || D.size() == 0) {
		cout << D.size() << endl;
		throw std::runtime_error("dimensions of D are wrong");
	    }

	    if (T.rows() != in.cols() || T.cols() == 0)
		throw std::runtime_error("dimensions of T are wrong");

	    cx_mat_t scaled_in = cx_mat_t::Zero(in.rows(), in.cols());
	    scaled_in.fill(cx_double(0., 0.));

	    cx_vec_t taus(in.cols());
	    ivec iwork(in.cols());

	    int lwork = -1;
	    int M = in.rows();
	    int N = in.cols();

	    scaled_in.block(0, 0, in.rows(), in.cols()) = in;
	    iwork.setZero();

	    lapack_int info = LAPACKE_zgeqrf( LAPACK_COL_MAJOR, M, N,
					      scaled_in.data(),
					      M, taus.data() );

	    cx_mat_t upper = scaled_in.triangularView<Upper>();
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal().real().cast<double>();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    dqmc::tools::abort("Invalid upper triangle");
		}
		    // throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) { D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }
	    lwork = -1;

	    info = LAPACKE_zungqr( LAPACK_COL_MAJOR, M, N, N,
				   scaled_in.data(), M, taus.data() );

	    U = scaled_in.block(0, 0, in.rows(), in.cols());
	    ivec col_ids(in.cols());
	    for (int i = 0; i < in.cols(); ++i) {
		col_ids(i) = iwork(i) - 1;
	    }

	    T = (d_inv.cast<cx_double_t>().asDiagonal()
		 * upper.block(0, 0, in.cols(), in.cols()));
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}
    }
}
#endif


// -*- mode: c++; indent-tabs-mode: nil; -*-
//
// Copyright (c) 2009-2013 Illumina, Inc.
//
// This software is provided under the terms and conditions of the
// Illumina Open Source Software License 1.
//
// You should have received a copy of the Illumina Open Source
// Software License 1 along with this program. If not, see
// <https://github.com/sequencing/licenses/>
//

/// \file

/// \author Chris Saunders
///
#include "blt_util/stat_util.hh"

#include <boost/math/distributions/chi_squared.hpp>

bool
is_chi_sqr_reject(const double xsq,
                  const unsigned df,
                  const double alpha) {

    assert(xsq>=0);
    assert(df>0);

    boost::math::chi_squared dist(df);
    return ((1.-boost::math::cdf(dist,xsq)) < alpha);

#if 0
    // alternate implementation (is one faster?):
    const double xsq_crit_val(boost::math::quantile(dist,1.-alpha));
    return xsq>xsq_crit_val;
#endif
}

bool
is_lrt_reject_null(const double null_loghood,
                   const double alt_loghood,
                   const unsigned df,
                   const double alpha) {

    if (df == 0) return false;
    if (null_loghood>alt_loghood) return false;

    const double log_lrt(-2.*(null_loghood-alt_loghood));

    return is_chi_sqr_reject(log_lrt,df,alpha);
}


#include <vector>
#include <string>
#include <iostream>
#include <boost/tuple/tuple.hpp>
#include <set>

int findBestPack( const std::vector<boost::tuple<std::string , int , int> > & ,
      std::set<int> & , const int  ) ;

int main( ) {
   std::vector<boost::tuple<std::string , int , int> > items ;
   //===========fill the vector with data====================
   items.push_back( boost::make_tuple( "" , 0  ,  0 ) ) ;
   items.push_back( boost::make_tuple( "map" , 9 , 150 ) ) ;
   items.push_back( boost::make_tuple( "compass" , 13 , 35 ) ) ;
   items.push_back( boost::make_tuple( "water" , 153 , 200 ) ) ;
   items.push_back( boost::make_tuple( "sandwich", 50 , 160 ) ) ;
   items.push_back( boost::make_tuple( "glucose" , 15 , 60 ) ) ;
   items.push_back( boost::make_tuple( "tin", 68 , 45 ) ) ;
   items.push_back( boost::make_tuple( "banana", 27 , 60 ) ) ;
   items.push_back( boost::make_tuple( "apple" , 39 , 40 ) ) ;
   items.push_back( boost::make_tuple( "cheese" , 23 , 30 ) ) ;
   items.push_back( boost::make_tuple( "beer" , 52 , 10 ) ) ;
   items.push_back( boost::make_tuple( "suntan creme" , 11 , 70 ) ) ;
   items.push_back( boost::make_tuple( "camera" , 32 , 30 ) ) ;
   items.push_back( boost::make_tuple( "T-shirt" , 24 , 15 ) ) ;
   items.push_back( boost::make_tuple( "trousers" , 48 , 10 ) ) ;
   items.push_back( boost::make_tuple( "umbrella" , 73 , 40 ) ) ;
   items.push_back( boost::make_tuple( "waterproof trousers" , 42 , 70 ) ) ;
   items.push_back( boost::make_tuple( "waterproof overclothes" , 43 , 75 ) ) ;
   items.push_back( boost::make_tuple( "note-case" , 22 , 80 ) ) ;
   items.push_back( boost::make_tuple( "sunglasses" , 7 , 20 ) ) ;
   items.push_back( boost::make_tuple( "towel" , 18 , 12 ) ) ;
   items.push_back( boost::make_tuple( "socks" , 4 , 50 ) ) ;
   items.push_back( boost::make_tuple( "book" , 30 , 10 ) ) ;
   const int maximumWeight = 400 ;
   std::set<int> bestItems ; //these items will make up the optimal value
   int bestValue = findBestPack( items , bestItems , maximumWeight ) ;
   std::cout << "The best value that can be packed in the given knapsack is " <<
      bestValue << " !\n" ;
   int totalweight = 0 ;
   std::cout << "The following items should be packed in the knapsack:\n" ;
   for ( std::set<int>::const_iterator si = bestItems.begin( ) ;
	 si != bestItems.end( ) ; si++ ) {
      std::cout << (items.begin( ) + *si)->get<0>( ) << "\n" ;
      totalweight += (items.begin( ) + *si)->get<1>( ) ;
   }
   std::cout << "The total weight of all items is " << totalweight << " !\n" ;
   return 0 ;
}

int findBestPack( const std::vector<boost::tuple<std::string , int , int> > & items ,std::set<int> & bestItems , const int weightlimit ) {
   //dynamic programming approach sacrificing storage space for execution
   //time , creating a table of optimal values for every weight and a
   //second table of sets with the items collected so far in the knapsack
   //the best value is in the bottom right corner of the values table,
   //the set of items in the bottom right corner of the sets' table.
   const int n = items.size( ) ;
   int bestValues [ n ][ weightlimit ] ;
   std::set<int> solutionSets[ n ][ weightlimit ] ;
   std::set<int> emptyset ;
   for ( int i = 0 ; i < n ; i++ ) {
      for ( int j = 0 ; j < weightlimit  ; j++ ) {
	 bestValues[ i ][ j ] = 0 ;
	 solutionSets[ i ][ j ] = emptyset ;
       }
    }
    for ( int i = 0 ; i < n ; i++ ) {
       for ( int weight = 0 ; weight < weightlimit ; weight++ ) {
	  if ( i == 0 )
	     bestValues[ i ][ weight ] = 0 ;
	  else  {
	     int itemweight = (items.begin( ) + i)->get<1>( ) ;
	     if ( weight < itemweight ) {
		bestValues[ i ][ weight ] = bestValues[ i - 1 ][ weight ] ;
		solutionSets[ i ][ weight ] = solutionSets[ i - 1 ][ weight ] ;
	     } else { // weight >= itemweight
		if ( bestValues[ i - 1 ][ weight - itemweight ] +
		   (items.begin( ) + i)->get<2>( ) >
		        bestValues[ i - 1 ][ weight ] ) {
		   bestValues[ i ][ weight ] =
		       bestValues[ i - 1 ][ weight - itemweight ] +
	        	(items.begin( ) + i)->get<2>( ) ;
		  solutionSets[ i ][ weight ] =
		      solutionSets[ i - 1 ][ weight - itemweight ] ;
		  solutionSets[ i ][ weight ].insert( i ) ;
	     }
	     else {
		bestValues[ i ][ weight ] = bestValues[ i - 1 ][ weight ] ;
		solutionSets[ i ][ weight ] = solutionSets[ i - 1 ][ weight ] ;
	     }
	  }
       }
      }
    }
    bestItems.swap( solutionSets[ n - 1][ weightlimit - 1 ] ) ;
    return bestValues[ n - 1 ][ weightlimit - 1 ] ;
}


#pragma once

#include <armadillo>
#include <iostream>
#include "shannonEntropy.hpp"
#include <Eigen/Dense>

// using Eigen::ArrayXi;

using ArrayXL = Eigen::Array<int64_t, Eigen::Dynamic, 1>;

using namespace std;
// using namespace arma;

// static_assert(sizeof(sword) ==8); //findHFPair works on the assumption that arma::sword is 64 bit

struct ETC {

    union pair {
        struct {
            uint64_t i1;
            uint64_t i2;
        } __attribute__((packed));
        __int128 i128;

        bool operator==(const pair& other)
        {
            return i128 == other.i128;
        }
    };

   static ETC::pair makeETCPair (uint64_t a, uint64_t b) {ETC::pair p; p.i1 = a; p.i2 = b; return p;};

   typedef unordered_map<__int128, unsigned int> pairFreqTable;

    static ETC::pair findHFPair(const ArrayXL &seq) {
        /*
         this implementation works very slightly differently from the matlab original where more that one pair wins the highest frequency
         - the matlab version depends on arbitrary behaviour of max(x)
         [m,indx]=max(Count_Array(:));
         i.e. chooses winner based on first position in 2d frequency matrix
         whereas this version chooses the first winner in the order of the array, which is slightly faster
         tests show this occasionally makes very minor differences in the results
         */
        ETC::pairFreqTable histo;
        ETC::pair winner;
        unsigned int highScore=0;

        unsigned int seqPos = 0;
        while (seqPos < seq.size()-1) {
            ETC::pair currPair = ETC::makeETCPair(seq[seqPos], seq[seqPos+1]);
            ETC::pairFreqTable::iterator it = histo.find(currPair.i128);
            unsigned int score;
            if (it == histo.end()) {
                histo.insert(std::make_pair(currPair.i128, 1));
                score=1;
            }else{
                score = it->second + 1;
               it->second = score;
            }
            if (score > highScore) {
                highScore = score;
                winner = currPair;
            }
            if (currPair.i1 == currPair.i2) {
                if (seqPos < seq.size()-2) {
                    if (seq[seqPos+2] == seq[seqPos]) {
                        seqPos++;
                    }
                }
            }
            seqPos++;
        }
        return winner;
    }

    static auto substitute(const ArrayXL &seq, ETC::pair p) {
        int64_t replacementSymbol = seq.maxCoeff() + 1;
        ArrayXL newSeq(seq.size());
        size_t src=0, dest=0;
        size_t replaceCount=0;
        while(src < seq.size()) {
            if (src < seq.size()-1) {
                if (seq[src] == p.i1 && seq[src+1] == p.i2) {
                    newSeq[dest] = replacementSymbol;
                    src++;
                    replaceCount++;
                } else {
                    newSeq[dest] = seq[src];
                }
            }else{
                newSeq[dest] = seq[src];
            }
            src++;
            dest++;
        }
        ArrayXL finalSeq = newSeq.head(dest);
        return std::make_tuple(finalSeq, replaceCount, replacementSymbol);
    }

    // static double calcOld(const ivec &seq) {
    //     double N = 0; //ETC measure
    //     double Hnew = shannonEntropy::calc(seq);
    //     ivec newSeq = seq;

    //     //todo: this can be optimised by continually editing the shannon histo instead of redoing it every time
    //     while(Hnew >1e-6 && newSeq.size() > 1) {
    //         ETC::pair hfPair = ETC::findHFPair(newSeq);
    //         auto [newSeqRepl, replaceCount, replaceSym] = ETC::substitute(newSeq, hfPair);
    //         Hnew = shannonEntropy::calc(newSeqRepl);
    //         newSeq = newSeqRepl;
    //         N++;
    //         // cout << newSeq << endl;
    //         // cout << N << ", " << Hnew << endl;
    //     }

    //     return N;
    // }

    static double calc(const ArrayXL &seq) {
        double N = 0; //ETC measure
//        cout << seq << endl;
        if (seq.size() > 1) {
            shannonEntropy::histoMap histo = shannonEntropy::calcDistribution(seq);
            double Hnew = shannonEntropy::calcProbability(histo, seq);

            ArrayXL newSeq = seq;

            while(Hnew >1e-6 && newSeq.size() > 1) {
                ETC::pair hfPair = ETC::findHFPair(newSeq);
                auto [newSeqRepl, replaceCount, replaceSym] = ETC::substitute(newSeq, hfPair);
                //reduce counts of replacement pair
                shannonEntropy::histoMap::iterator it = histo.find(hfPair.i1);
                it->second -= replaceCount;
                if (it->second == 0) {
                    //remove from the histo
                    histo.erase(it);
                }
                it = histo.find(hfPair.i2);
                it->second -= replaceCount;
                if (it->second == 0) {
                    //remove from the histo
                    histo.erase(it);
                }
                //add the new symbol into the histogram
                auto histoEntry = make_pair(replaceSym, replaceCount);
                histo.insert(histoEntry);

                Hnew = shannonEntropy::calcProbability(histo, newSeqRepl);
                newSeq = newSeqRepl;
                N++;
            }
            N /= (seq.size() -1);
        }
        return N;
    }

    static double calcJoint(const ArrayXL& seq1, const ArrayXL& seq2) {

        ArrayXL combSeq(seq1.size());
        for (size_t i=0; i < seq1.size(); i++) {
            combSeq[i] =(uint64_t)( seq1[i] | (seq2[i] << 32));
        }
        return ETC::calc(combSeq);
    }

};


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

**/
//==================================================================================================
#ifndef BOOST_SIMD_HYPERBOLIC_HPP_INCLUDED
#define BOOST_SIMD_HYPERBOLIC_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-hyperbolic  Hyperbolic functions

    These functions provides scalar and SIMD version of
    hyperbolic  and inverse hyperbolic functions.

    - Direct hyperbolic functors

         | name          | name         | name           | name         |
         |:-------------:|:------------:|:--------------:|:------------:|
         | @ref cosh     | @ref csch    | @ref sinh      | @ref sinhcosh|
         | @ref coth     | @ref sech    | @ref sinhc     | @ref tanh    |

          @ref sinhc is the hyperbolic sinus cardinal function
          (\f$\sinh x/x\f$).

          @ref sinhcosh  allows the simultaneous
          computation of hyperbolic sine and cosine at lower cost.

          @ref sech and @ref csch are the hyperbolic secant (inverse of
          the hyperbolic cosine) and cosecant (inverse of the hyperbolic sine)

    - Inverse hyperbolic functors

         | name        | name         | name        |
         |:-----------:|:------------:|:-----------:|
         | @ref acosh  | @ref acsch   | @ref asinh  |
         | @ref acoth  | @ref asech   | @ref atanh  |

    **/

} }

#include <boost/simd/function/acosh.hpp>
#include <boost/simd/function/acoth.hpp>
#include <boost/simd/function/acsch.hpp>
#include <boost/simd/function/asech.hpp>
#include <boost/simd/function/asinh.hpp>
#include <boost/simd/function/atanh.hpp>
#include <boost/simd/function/cosh.hpp>
#include <boost/simd/function/coth.hpp>
#include <boost/simd/function/csch.hpp>
#include <boost/simd/function/sech.hpp>
#include <boost/simd/function/sinhc.hpp>
#include <boost/simd/function/sinhcosh.hpp>
#include <boost/simd/function/sinh.hpp>
#include <boost/simd/function/tanh.hpp>

#endif


#include <boost/config.hpp>
#include <boost/version.hpp>
#include <fstream>

// CGAL headers
#include <CGAL/Cartesian.h>
#include <CGAL/MP_Float.h>
#include <CGAL/Algebraic_kernel_for_circles_2_2.h>
#include <CGAL/Circular_kernel_2.h>
#include <CGAL/intersections.h>
#include <CGAL/Circular_kernel_2.h>
#include <CGAL/Object.h>
#include <CGAL/IO/WKT.h>

// Qt headers
#include <QtGui>
#include <QString>
#include <QActionGroup>
#include <QFileDialog>
#include <QInputDialog>

// GraphicsView items and event filters (input classes)
#include <CGAL/Qt/GraphicsViewCircularArcInput.h>
#include "ArcsGraphicsItem.h"

// the two base classes
#include "ui_Circular_kernel_2.h"
#include <CGAL/Qt/DemosMainWindow.h>

typedef CGAL::Quotient<CGAL::MP_Float>                       NT;
typedef CGAL::Cartesian<NT>                                 Linear_k;

typedef CGAL::Algebraic_kernel_for_circles_2_2<NT>          Algebraic_k;
typedef CGAL::Circular_kernel_2<Linear_k,Algebraic_k>       CircularKernel;

typedef CircularKernel::Point_2                                 Point_2;
typedef CircularKernel::Segment_2                               Segment_2;
typedef CircularKernel::Line_arc_2                              Line_arc_2;
typedef CircularKernel::Circular_arc_2                          Circular_arc_2;
typedef CircularKernel::Circular_arc_point_2                    Circular_arc_point_2;

typedef CGAL::Qt::ArcsGraphicsItem<CircularKernel>                 ArcsGraphicsItem;

typedef std::vector<CGAL::Object>                           ArcContainer;

class MainWindow :
  public CGAL::Qt::DemosMainWindow,
  public Ui::Circular_kernel_2
{
  Q_OBJECT

private:
  ArcContainer arcs;
  ArcContainer intersections;
  QGraphicsScene scene;

  ArcsGraphicsItem * agi;

  CGAL::Qt::GraphicsViewCircularArcInput<CircularKernel> * cai;

public:
  MainWindow();

public Q_SLOTS:

  virtual void open(QString);

  void processInput(CGAL::Object o);

  void on_actionInsertCircularArc_toggled(bool checked);

  void on_actionClear_triggered();

  void on_actionLoadLineAndCircularArcs_triggered();

  void on_actionRecenter_triggered();

Q_SIGNALS:
  void changed();
};

MainWindow::MainWindow()
  : DemosMainWindow()
{
  setupUi(this);

  // Add a GraphicItem for the Circular triangulation
  agi = new CGAL::Qt::ArcsGraphicsItem<CircularKernel>(arcs, intersections);

  agi->setIntersectionsPen(QPen(Qt::red, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));

  QObject::connect(this, SIGNAL(changed()),
                   agi, SLOT(modelChanged()));

  agi->setInputPen(QPen(Qt::black, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.addItem(agi);
  agi->hide();

  // Setup input handlers. They get events before the scene gets them
  // and the input they generate is passed to the triangulation with
  // the signal/slot mechanism
  cai = new CGAL::Qt::GraphicsViewCircularArcInput<CircularKernel>(this, &scene);

  QObject::connect(cai, SIGNAL(generate(CGAL::Object)),
                   this, SLOT(processInput(CGAL::Object)));

  // Manual handling of actions
  //
  QObject::connect(this->actionQuit, SIGNAL(triggered()),
                   qApp, SLOT(quit()));

  // Check two actions
  this->actionInsertCircularArc->setChecked(true);

  //
  // Setup the scene and the view
  //
  scene.setItemIndexMethod(QGraphicsScene::NoIndex);
  scene.setSceneRect(-100, -100, 100, 100);
  this->graphicsView->setScene(&scene);

  // Uncomment the following line to get antialiasing by default.
//   actionUse_Antialiasing->setChecked(true);

  // Turn the vertical axis upside down
  this->graphicsView->scale(1, -1);

  // The navigation adds zooming and translation functionality to the
  // QGraphicsView
  this->addNavigation(this->graphicsView);

  this->setupStatusBar();
  this->setupOptionsMenu();
  this->addAboutDemo(":/cgal/help/about_Circular_kernel_2.html");
  this->addAboutCGAL();
  this->addRecentFiles(this->menuFile, this->actionQuit);
  connect(this, SIGNAL(openRecentFile(QString)),
          this, SLOT(open(QString)));
}

void
MainWindow::processInput(CGAL::Object o)
{
  Circular_arc_2 ca;
  Line_arc_2 la;
  bool is_circular = false;

  if(assign(ca, o)){
    is_circular = true;
  } else if(! assign(la, o)){
    std::cerr << "unknown object" << std::endl;
    return;
  }

  for(std::vector<CGAL::Object>::iterator it = arcs.begin(); it != arcs.end(); ++it){
    Circular_arc_2 vca;
    Line_arc_2 vla;
    if(assign(vca, *it)){
      if(is_circular){
        CGAL::intersection(ca, vca, std::back_inserter(intersections));
      } else {
        CGAL::intersection(la, vca, std::back_inserter(intersections));
      }
    } else if(assign(vla, *it)){
      if(is_circular){
        CGAL::intersection(ca, vla, std::back_inserter(intersections));
      } else {
        CGAL::intersection(la, vla, std::back_inserter(intersections));
      }
    }
  }
  arcs.push_back(o);
  Q_EMIT( changed());
}

/*
 *  Qt Automatic Connections
 *  https://doc.qt.io/qt-5/designer-using-a-ui-file.html#automatic-connections
 *
 *  setupUi(this) generates connections to the slots named
 *  "on_<action_name>_<signal_name>"
 */

void
MainWindow::on_actionInsertCircularArc_toggled(bool checked)
{
  if(checked){
    scene.installEventFilter(cai);
  } else {
    scene.removeEventFilter(cai);
  }
}

void
MainWindow::on_actionClear_triggered()
{
  arcs.clear();
  intersections.clear();
  Q_EMIT( changed());
}

void
MainWindow::on_actionLoadLineAndCircularArcs_triggered()
{
  QString fileName = QFileDialog::getOpenFileName(this,
                                                  tr("Open Line and Circular Arc File"),
                                                  ".",
                                                  tr("Edge files (*.arc)\n"
                                                     "WKT files (*.wkt *.WKT)\n"
                                                     ));
  if(! fileName.isEmpty()){
    open(fileName);
    this->addToRecentFiles(fileName);
  }
}

void
MainWindow::open(QString fileName)
{
    std::ifstream ifs(qPrintable(fileName));

    char c;
    double x,y;
    if(fileName.endsWith(".wkt", Qt::CaseInsensitive))
    {
      //read pairs as Line_arc_2 and triplets as circular_arc_2
      do
      {
        std::vector<Point_2> multi_points;
        CGAL::IO::read_multi_point_WKT(ifs, multi_points);
        if(multi_points.size() == 2)
        {
          Line_arc_2 la(Segment_2(multi_points[0],
                        multi_points[1]));
          for(std::vector<CGAL::Object>::iterator it = arcs.begin(); it != arcs.end(); ++it){
            Circular_arc_2 vca;
            Line_arc_2 vla;
            if(assign(vca, *it)){
              CGAL::intersection(la, vca, std::back_inserter(intersections));
            } else if(assign(vla, *it)){
              CGAL::intersection(la, vla, std::back_inserter(intersections));
            }
          }
          arcs.push_back(make_object(la));
        }
        else if(multi_points.size() == 3)
        {
          Circular_arc_2 ca(multi_points[0],
                            multi_points[1],
                            multi_points[2]);
          for(std::vector<CGAL::Object>::iterator it = arcs.begin(); it != arcs.end(); ++it){
            Circular_arc_2 vca;
            Line_arc_2 vla;
            if(assign(vca, *it)){
              CGAL::intersection(ca, vca, std::back_inserter(intersections));
            } else if(assign(vla, *it)){
              CGAL::intersection(ca, vla, std::back_inserter(intersections));
            }
          }
          arcs.push_back(make_object(ca));
        }
        else if(multi_points.size()>0)
        {
          std::cerr<<"unreadable object."<<std::endl;
        }
      }while(ifs.good() && !ifs.eof());
      ifs.close();
    }
    else
    {
      while(ifs >> c){
        if(c == 's'){
          ifs >> x >> y;
          Point_2 p(x,y);
          ifs >> x >> y;
          Point_2 q(x,y);

          Line_arc_2 la(Segment_2(p,q));
          for(std::vector<CGAL::Object>::iterator it = arcs.begin(); it != arcs.end(); ++it){
            Circular_arc_2 vca;
            Line_arc_2 vla;
            if(assign(vca, *it)){
              CGAL::intersection(la, vca, std::back_inserter(intersections));
            } else if(assign(vla, *it)){
              CGAL::intersection(la, vla, std::back_inserter(intersections));
            }
          }
          arcs.push_back(make_object(la));
        } else if(c == 'c'){
          ifs >> x >> y;
          Point_2 p(x,y);
          ifs >> x >> y;
          Point_2 q(x,y);
          ifs >> x >> y;
          Point_2 r(x,y);
          Circular_arc_2 ca(p,q,r);
          for(std::vector<CGAL::Object>::iterator it = arcs.begin(); it != arcs.end(); ++it){
            Circular_arc_2 vca;
            Line_arc_2 vla;
            if(assign(vca, *it)){
              CGAL::intersection(ca, vca, std::back_inserter(intersections));
            } else if(assign(vla, *it)){
              CGAL::intersection(ca, vla, std::back_inserter(intersections));
            }
          }
          arcs.push_back(make_object(ca));
        }
      }
    }
    Q_EMIT( changed());
}

void
MainWindow::on_actionRecenter_triggered()
{
  this->graphicsView->setSceneRect(agi->boundingRect());
  this->graphicsView->fitInView(agi->boundingRect(), Qt::KeepAspectRatio);
}

#include "Circular_kernel_2.moc"
#include <CGAL/Qt/resources.h>

int main(int argc, char **argv)
{
  QApplication app(argc, argv);

  app.setOrganizationDomain("geometryfactory.com");
  app.setOrganizationName("GeometryFactory");
  app.setApplicationName("Circular_kernel_2 demo");

  // Import resources from libCGAL (Qt5).
  CGAL_QT_INIT_RESOURCES;

  MainWindow mainWindow;
  mainWindow.show();
  return app.exec();
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

// Defines the class BulgedCube.

#pragma once

#include <array>
#include <boost/optional.hpp>
#include <cstddef>
#include <limits>

#include "DataStructures/Tensor/TypeAliases.hpp"
#include "Utilities/TypeTraits/RemoveReferenceWrapper.hpp"

/// \cond
namespace PUP {
class er;
}  // namespace PUP
/// \endcond

namespace domain {
namespace CoordinateMaps {

/*!
 *  \ingroup CoordinateMapsGroup
 *
 *  \brief Three dimensional map from the cube to a bulged cube.
 *  The cube is shaped such that the surface is compatible
 *  with the inner surface of Wedge3D.
 *  The shape of the object can be chosen to be cubical,
 *  if the sphericity is set to 0, or to a sphere, if
 *  the sphericity is set to 1. The sphericity can
 *  be set to any number between 0 and 1 for a bulged cube.
 *
 *  \details The volume map from the cube to a bulged cube is obtained by
 *  interpolating between six surface maps, twelve bounding curves, and
 *  eight corners. The surface map for the upper +z axis is obtained by
 *  interpolating between a cubical surface and a spherical surface. The
 *  two surfaces are chosen such that the latter circumscribes the former.
 *
 *  We make a choice here as to whether we wish to use the logical coordinates
 *  parameterizing these surface as they are, in which case we have the
 *  equidistant choice of coordinates, or whether to apply a tangent map to them
 *  which leads us to the equiangular choice of coordinates. In terms of the
 *  logical coordinates, the equiangular coordinates are:
 *
 *  \f[\textrm{equiangular xi} : \Xi(\xi) = \textrm{tan}(\xi\pi/4)\f]
 *
 *  \f[\textrm{equiangular eta}  : \mathrm{H}(\eta) = \textrm{tan}(\eta\pi/4)\f]
 *
 *  With derivatives:
 *
 *  \f[\Xi'(\xi) = \frac{\pi}{4}(1+\Xi^2)\f]
 *
 *  \f[\mathrm{H}'(\eta) = \frac{\pi}{4}(1+\mathrm{H}^2)\f]
 *
 *  The equidistant coordinates are:
 *
 *  \f[ \textrm{equidistant xi}  : \Xi = \xi\f]
 *
 *  \f[ \textrm{equidistant eta}  : \mathrm{H} = \eta\f]
 *
 *  with derivatives:
 *
 *  \f$\Xi'(\xi) = 1\f$, and \f$\mathrm{H}'(\eta) = 1\f$
 *
 *  We also define the variable \f$\rho\f$, given by:
 *
 *  \f[\rho = \sqrt{1+\Xi^2+\mathrm{H}^2}\f]
 *
 *  ### The Spherical Face Map
 *  The surface map for the spherical face of radius \f$R\f$ lying in the
 *  \f$+z\f$
 *  direction in either choice of coordinates is then given by:
 *
 *  \f[
 *  \vec{\sigma}_{spherical}(\xi,\eta) =
 *  \begin{bmatrix}
 *  x(\xi,\eta)\\
 *  y(\xi,\eta)\\
 *  z(\xi,\eta)\\
 *  \end{bmatrix}  = \frac{R}{\rho}
 *  \begin{bmatrix}
 *  \Xi\\
 *  \mathrm{H}\\
 *  1\\
 *  \end{bmatrix}
 *  \f]
 *
 *  ### The Cubical Face Map
 *  The surface map for the cubical face of side length \f$2L\f$ lying in the
 *  \f$+z\f$ direction is given by:
 *
 *  \f[
 *  \vec{\sigma}_{cubical}(\xi,\eta) =
 *  \begin{bmatrix}
 *  x(\xi,\eta)\\
 *  y(\xi,\eta)\\
 *  L\\
 *  \end{bmatrix}  = L
 *  \begin{bmatrix}
 *  \Xi\\
 *  \mathrm{H}\\
 *  1\\
 *  \end{bmatrix}
 *  \f]
 *
 *  ### The Bulged Face Map
 *  To construct the bulged map we interpolate between a cubical face map of
 *  side length \f$2L\f$ and a spherical face map of radius \f$R\f$, with the
 *  interpolation parameter being \f$s\f$, the `sphericity`.
 *  The surface map for the bulged face lying in the \f$+z\f$ direction is then
 *  given by:
 *
 *  \f[
 *  \vec{\sigma}_{+\zeta}(\xi,\eta) = \left\{(1-s)L + \frac{sR}{\rho}\right\}
 *  \begin{bmatrix}
 *  \Xi\\
 *  \mathrm{H}\\
 *  1\\
 *  \end{bmatrix}
 *  \f]
 *
 *  This equation defines the upper-z map \f$\vec{\sigma}_{+\zeta}\f$, and we
 *  similarly define the other five surface maps \f$\vec{\sigma}_{+\eta}\f$,
 *  \f$\vec{\sigma}_{+\xi}\f$, and so on by appropriate rotations.
 *  We constrain L by demanding that the spherical face circumscribe the cube.
 *  With this condition, we have \f$L = R/\sqrt3\f$.
 *
 *  ### The General Formula for 3D Isoparametric Maps
 *  The general formula is given by Eq. 1 in section 2.1 of Hesthaven's paper
 *  "A Stable Penalty Method For The Compressible Navier-Stokes Equations III.
 *  Multidimensional Domain Decomposition Schemes" available
 *   here .
 *
 *  Hesthaven's formula is general in the degree of the shape functions used,
 *  so for our purposes we take the special case where the shape functions are
 *  linear in the interpolation variable, and define new variables accordingly.
 *  However, our interpolation variables do not necessarily have to be the
 *  logical coordinates themselves, though they often are. To make this
 *  distinction clear, we will define the new interpolation variables
 *  \f$\{\tilde{\xi},\tilde{\eta},\tilde{\zeta}\}\f$, which may either be the
 *  logical coordinates themselves or a invertible transformation of them. For
 *  the purposes of the bulged cube map, this transformation will be the same
 *  transformation that takes the logical coordinates into the equiangular
 *  coordinates. We will later see how this choice can lead to simplifications
 *  in the final map.
 *
 *  We define the following variables for
 *  \f$\alpha, \beta, \gamma \in\{\tilde{\xi},\tilde{\eta},\tilde{\zeta}\}\f$:
 *
 *  \f[
 *  f^{\pm}_{\alpha} = \frac{1}{2}(1\pm\alpha)\\
 *  f^{\pm\pm}_{\alpha \ \beta} = \frac{1}{4}(1\pm\alpha)(1\pm\beta)\\
 *  f^{\pm\pm\pm}_{\alpha \ \beta \ \gamma} =
 *  \frac{1}{8}(1\pm\alpha)(1\pm\beta)(1\pm\gamma)
 *  \f]
 *
 *  The formula involves six surfaces, which we will denote by
 *  \f$\vec{\sigma}\f$, twelve curves, denoted by \f$\vec{\Gamma}\f$, and eight
 *  vertices, denoted by \f$\vec{\pi}\f$, with the subscripts denoting which
 *  face(s) these objects belong to. The full volume map is given by:
 *
 *  \f{align*}
 *  \vec{x}(\xi,\eta,\zeta) = &
 *  f^{+}_{\tilde{\zeta}}\vec{\sigma}_{+\zeta}(\xi, \eta)+
 *  f^{-}_{\tilde{\zeta}}\vec{\sigma}_{-\zeta}(\xi, \eta)\\
 *  &+ f^{+}_{\tilde{\eta}}\vec{\sigma}_{+\eta}(\xi, \zeta)+
 *  f^{-}_{\tilde{\eta}}\vec{\sigma}_{-\eta}(\xi, \zeta)+
 *  f^{+}_{\tilde{\xi}}\vec{\sigma}_{+\xi}(\eta, \zeta)+
 *  f^{-}_{\tilde{\xi}}\vec{\sigma}_{-\xi}(\eta, \zeta)\\
 *  &- f^{++}_{\tilde{\xi} \ \tilde{\eta}}\vec{\Gamma}_{+\xi +\eta}(\zeta)-
 *  f^{-+}_{\tilde{\xi} \ \tilde{\eta}}\vec{\Gamma}_{-\xi +\eta}(\zeta)-
 *  f^{+-}_{\tilde{\xi} \ \tilde{\eta}}\vec{\Gamma}_{+\xi -\eta}(\zeta)-
 *  f^{--}_{\tilde{\xi} \ \tilde{\eta}}\vec{\Gamma}_{-\xi -\eta}(\zeta)\\
 *  &- f^{++}_{\tilde{\xi} \ \tilde{\zeta}}\vec{\Gamma}_{+\xi +\zeta}(\eta)-
 *  f^{-+}_{\tilde{\xi} \ \tilde{\zeta}}\vec{\Gamma}_{-\xi +\zeta}(\eta)-
 *  f^{+-}_{\tilde{\xi} \ \tilde{\zeta}}\vec{\Gamma}_{+\xi -\zeta}(\eta)-
 *  f^{--}_{\tilde{\xi} \ \tilde{\zeta}}\vec{\Gamma}_{-\xi -\zeta}(\eta)\\
 *  &- f^{++}_{\tilde{\eta} \ \tilde{\zeta}}\vec{\Gamma}_{+\eta +\zeta}(\xi)-
 *  f^{-+}_{\tilde{\eta} \ \tilde{\zeta}}\vec{\Gamma}_{-\eta +\zeta}(\xi)-
 *  f^{+-}_{\tilde{\eta} \ \tilde{\zeta}}\vec{\Gamma}_{+\eta -\zeta}(\xi)-
 *  f^{--}_{\tilde{\eta} \tilde{\zeta}}\vec{\Gamma}_{-\eta -\zeta}(\xi)\\
 *  &+ f^{+++}_{\tilde{\xi} \ \tilde{\eta} \ \tilde{\zeta}}\vec{\pi}_{+\xi +\eta
 * +\zeta}+ f^{-++}_{\tilde{\xi} \ \tilde{\eta} \ \tilde{\zeta}}\vec{\pi}_{-\xi
 * +\eta +\zeta}+ f^{+-+}_{\tilde{\xi} \ \tilde{\eta} \
 * \tilde{\zeta}}\vec{\pi}_{+\xi -\eta +\zeta}+
 *  f^{--+}_{\tilde{\xi} \ \tilde{\eta} \ \tilde{\zeta}}\vec{\pi}_{-\xi -\eta
 * +\zeta}\\
 *  &+ f^{++-}_{\tilde{\xi} \ \tilde{\eta} \ \tilde{\zeta}}\vec{\pi}_{+\xi +\eta
 * -\zeta}+ f^{-+-}_{\tilde{\xi} \ \tilde{\eta} \ \tilde{\zeta}}\vec{\pi}_{-\xi
 * +\eta -\zeta}+ f^{+--}_{\tilde{\xi} \ \tilde{\eta} \
 * \tilde{\zeta}}\vec{\pi}_{+\xi -\eta -\zeta}+ f^{---}_{\tilde{\xi} \
 * \tilde{\eta} \ \tilde{\zeta}}\vec{\pi}_{-\xi -\eta -\zeta} \f}
 *
 *
 *  ### The Special Case for Octahedral Symmetry
 *  The general formula is for the case in which there are six independently
 *  specified bounding surfaces. In our case, the surfaces are obtained by
 *  rotations and reflections of the upper-\f$\zeta\f$ face.
 *
 * We define the matrices corresponding to these transformations to be:
 *
 * \f[
 * S_{xy} =
 *  \begin{bmatrix}
 *  0 & 1 & 0\\
 *  1 & 0 & 0\\
 *  0 & 0 & 1\\
 *  \end{bmatrix},\
 *
 * S_{xz} =
 *  \begin{bmatrix}
 *  0 & 0 & 1\\
 *  0 & 1 & 0\\
 *  1 & 0 & 0\\
 *  \end{bmatrix},\
 *
 * S_{yz} =
 *  \begin{bmatrix}
 *  1 & 0 & 0\\
 *  0 & 0 & 1\\
 *  0 & 1 & 0\\
 *  \end{bmatrix}\f]
 *
 * \f[C_{zxy} =
 *  \begin{bmatrix}
 *  0 & 0 & 1\\
 *  1 & 0 & 0\\
 *  0 & 1 & 0\\
 *  \end{bmatrix},\
 *
 * C_{yzx} =
 *  \begin{bmatrix}
 *  0 & 1 & 0\\
 *  0 & 0 & 1\\
 *  1 & 0 & 0\\
 *  \end{bmatrix}\f]
 *
 * \f[N_{x} =
 *  \begin{bmatrix}
 *  -1 & 0 & 0\\
 *  0 & 1 & 0\\
 *  0 & 0 & 1\\
 *  \end{bmatrix},\
 *
 * N_{y} =
 *  \begin{bmatrix}
 *  1 & 0 & 0\\
 *  0 & -1 & 0\\
 *  0 & 0 & 1\\
 *  \end{bmatrix},\
 *
 * N_{z} =
 *  \begin{bmatrix}
 *  1 & 0 & 0\\
 *  0 & 1 & 0\\
 *  0 & 0 & -1\\
 *  \end{bmatrix}
 *  \f]
 *
 * The surface maps can now all be written in terms of
 * \f$\vec{\sigma}_{+\zeta}\f$ and these matrices:
 *
 * \f$\vec{\sigma}_{-\zeta}(\xi, \eta) = N_z\vec{\sigma}_{+\zeta}(\xi, \eta)\\
 * \vec{\sigma}_{+\eta}(\xi, \zeta) = S_{yz}\vec{\sigma}_{+\zeta}(\xi, \zeta)\\
 * \vec{\sigma}_{-\eta}(\xi, \zeta) = N_yS_{yz}\vec{\sigma}_{+\zeta}(\xi,
 * \zeta)\\
 * \vec{\sigma}_{+\xi}(\eta, \zeta) = C_{zxy}\vec{\sigma}_{+\zeta}(\eta,
 * \zeta)\\
 * \vec{\sigma}_{-\xi}(\eta, \zeta) = N_xC_{zyx}\vec{\sigma}_{+\zeta}(\eta,
 * \zeta)\f$
 *
 *
 * The four bounding curves \f$\vec{\Gamma}\f$ on the \f$+\zeta\f$ face are
 * given by:
 *
 *
 * \f$\vec{\Gamma}_{+\xi,+\zeta}(\eta) = \vec{\sigma}_{+\zeta}(+1,\eta)\\
 * \vec{\Gamma}_{-\xi,+\zeta}(\eta) = \vec{\sigma}_{+\zeta}(-1,\eta)
 * = N_x\vec{\sigma}_{+\zeta}(+1, \eta)\\
 * \vec{\Gamma}_{+\eta,+\zeta}(\xi) = \vec{\sigma}_{+\zeta}(\xi,+1)
 * = S_{xy}\vec{\sigma}_{+\zeta}(+1, \xi)\\
 * \vec{\Gamma}_{-\eta,+\zeta}(\xi) = \vec{\sigma}_{+\zeta}(\xi,-1)
 * = N_yS_{xy}\vec{\sigma}_{+\zeta}(+1,\xi)\f$
 *
 *
 * The bounding curves on the other surfaces can be obtained by transformations
 * on the \f$+\zeta\f$ face:
 *
 *
 * \f$\vec{\Gamma}_{+\xi,-\zeta}(\eta) = N_z\vec{\sigma}_{+\zeta}(+1,\eta)\\
 * \vec{\Gamma}_{-\xi,-\zeta}(\eta) = N_z\vec{\sigma}_{+\zeta}(-1,\eta)
 * = N_zN_x\vec{\sigma}_{+\zeta}(+1,\eta)\\
 * \vec{\Gamma}_{+\eta,-\zeta}(\xi) = N_z\vec{\sigma}_{+\zeta}(\xi,+1)
 * = N_zS_{xy}\vec{\sigma}_{+\zeta}(+1, \xi)\\
 * \vec{\Gamma}_{-\eta,-\zeta}(\xi) = N_z\vec{\sigma}_{+\zeta}(\xi,-1)
 * = N_zN_yS_{xy}\vec{\sigma}_{+\zeta}(+1, \xi)\\
 * \vec{\Gamma}_{+\xi,+\eta}(\zeta) =
 * C_{zxy}\vec{\sigma}_{+\zeta}(+1,\zeta)\\
 * \vec{\Gamma}_{-\xi,+\eta}(\zeta) =
 * N_xC_{zxy}\vec{\sigma}_{+\zeta}(+1,\zeta)\\
 * \vec{\Gamma}_{+\xi,-\eta}(\zeta) = C_{zxy}\vec{\sigma}_{+\zeta}(-1,\zeta)
 * = C_{zxy}N_x\vec{\sigma}_{+\zeta}(+1,\zeta)\\
 * \vec{\Gamma}_{-\xi,-\eta}(\zeta) = N_xC_{zxy}\vec{\sigma}_{+\zeta}(-1,\zeta)
 * = N_xC_{zxy}N_x\vec{\sigma}_{+\zeta}(+1,\zeta)\f$
 *
 *
 * Now we can write the volume map in terms of
 * \f$\vec{\sigma}_{+\zeta}\f$ only:
 * \f{align*}\vec{x}(\xi,\eta,\zeta) = &
 * (f^{+}_{\tilde{\zeta}} + f^{-}_{\tilde{\zeta}}N_z)
 * \vec{\sigma}_{+\zeta}(\xi, \eta)\\
 * &+ (f^{+}_{\tilde{\eta}} + f^{-}_{\tilde{\eta}}N_y)
 * S_{yz}\vec{\sigma}_{+\zeta}(\xi, \zeta)\\
 * &+ (f^{+}_{\tilde{\xi}} + f^{-}_{\tilde{\xi}}N_x)
 * C_{zxy}\vec{\sigma}_{+\zeta}(\eta, \zeta)\\
 * &- (f^{+}_{\tilde{\xi}}+f^{-}_{\tilde{\xi}}N_x)
 * (f^{+}_{\tilde{\eta}}+f^{-}_{\tilde{\eta}}N_y)
 * C_{zxy}\vec{\sigma}_{+\zeta}(+1, \zeta)\\
 * &- (f^{+}_{\tilde{\zeta}}+f^{-}_{\tilde{\zeta}}N_z)\left\{
 * (f^{+}_{\tilde{\xi}}+f^{-}_{\tilde{\xi}}N_x)\vec{\sigma}_{+\zeta}(+1, \eta)+
 * (f^{+}_{\tilde{\eta}}+f^{-}_{\tilde{\eta}}N_y)S_{xy}\vec{\sigma}_{+\zeta}(+1,
 * \xi)\right\}\\
 * &+ \frac{r}{\sqrt{3}}\vec{\tilde{\xi}}
 *  \f}
 *
 * Note that we can now absorb all of the \f$f\f$s into the matrix prefactors
 * in the above equation and obtain a final set of matrices. We define the
 * following *blending matrices*:
 *
 *  \f[
 *  B_{\tilde{\xi}} =
 *  \begin{bmatrix}
 *  0 & 0 & \tilde{\xi}\\
 *  1 & 0 & 0\\
 *  0 & 1 & 0\\
 *  \end{bmatrix},\
 *
 *  B_{\tilde{\eta}} =
 *  \begin{bmatrix}
 *  1 & 0 & 0\\
 *  0 & 0 & \tilde{\eta}\\
 *  0 & 1 & 0\\
 *  \end{bmatrix},\
 *
 *  B_{\tilde{\zeta}} =
 *  \begin{bmatrix}
 *  1 & 0 & 0\\
 *  0 & 1 & 0\\
 *  0 & 0 & \tilde{\zeta}\\
 *  \end{bmatrix}\\
 *
 *  B_{\tilde{\xi}\tilde{\eta}} =
 *  \begin{bmatrix}
 *  0 & 0 & \tilde{\xi}\\
 *  \tilde{\eta} & 0 & 0\\
 *  0 & 1 & 0\\
 *  \end{bmatrix},\
 *
 *  B_{\tilde{\xi}\tilde{\zeta}} =
 *  \begin{bmatrix}
 *  \tilde{\xi} & 0 & 0\\
 *  0 & 1 & 0\\
 *  0 & 0 & \tilde{\zeta}\\
 *  \end{bmatrix},\
 *
 *  B_{\tilde{\eta}\tilde{\zeta}} =
 *  \begin{bmatrix}
 *  0 & 1 & 0\\
 *  \tilde{\eta} & 0 & 0\\
 *  0 & 0 & \tilde{\zeta}\\
 *  \end{bmatrix}\\
 *
 *  B_{\tilde{\xi}\tilde{\eta}\tilde{\zeta}} =
 *  \begin{bmatrix}
 *  \tilde{\xi} & 0 & 0\\
 *  0 & \tilde{\eta} & 0\\
 *  0 & 0 & \tilde{\zeta}\\
 *  \end{bmatrix}
 *  \f]
 *
 *  Now we can write the volume map in these terms:
 *
 * \f{align*}
 * \vec{x}(\xi,\eta,\zeta) = &
 * B_{\tilde{\zeta}}
 * \vec{\sigma}_{+\zeta}(\xi, \eta)\\& +
 * B_{\tilde{\eta}}
 * \vec{\sigma}_{+\zeta}(\xi, \zeta)+
 * B_{\tilde{\xi}}
 * \vec{\sigma}_{+\zeta}(\eta, \zeta)\\& -
 * B_{\tilde{\xi} \tilde{\eta}}
 * \vec{\sigma}_{+\zeta}(+1, \zeta)-
 * B_{\tilde{\xi} \tilde{\zeta}}
 * \vec{\sigma}_{+\zeta}(+1, \eta)+
 * B_{\tilde{\eta} \tilde{\zeta}}
 * \vec{\sigma}_{+\zeta}(+1, \xi)\\& +
 * B_{\tilde{\xi} \tilde{\eta} \tilde{\zeta}}
 * \vec{\sigma}_{+\zeta}(+1, +1)
 * \f}
 *
 * ### The Bulged Cube Map
 * We now use the result above to provide the mapping for the bulged cube.
 * First we will define the variables \f$\rho_A\f$ and \f$\rho_{AB}\f$, for
 * \f$A, B \in \{\Xi,\mathrm{H}, \mathrm{Z}\} \f$, where \f$\mathrm{Z}\f$
 * is \f$\tan(\zeta\pi/4)\f$ in the equiangular case and \f$\zeta\f$ in the
 * equidistant case:
 *
 * \f[
 * \rho_A = \sqrt{2 + A^2}\\
 * \rho_{AB} = \sqrt{1 + A^2 + B^2}
 * \f]
 * The final mapping is then:
 * \f[
 * \vec{x}(\xi,\eta,\zeta) = \frac{(1-s)R}{\sqrt{3}}
 * \begin{bmatrix}
 * \Xi\\
 * \mathrm{H}\\
 * \mathrm{Z}\\
 * \end{bmatrix} +
 * \frac{sR}{\sqrt{3}}
 * \begin{bmatrix}
 * \tilde{\xi}\\
 * \tilde{\eta}\\
 * \tilde{\zeta}\\
 * \end{bmatrix} + sR
 * \begin{bmatrix}
 * \tilde{\xi} & \Xi & \Xi\\
 * \mathrm{H} & \tilde{\eta} &\mathrm{H}\\
 * \mathrm{Z} & \mathrm{Z} & \tilde{\zeta}\\
 * \end{bmatrix}
 * \begin{bmatrix}
 * 1/\rho_{\mathrm{H}\mathrm{Z}}\\
 * 1/\rho_{\Xi\mathrm{Z}}\\
 * 1/\rho_{\Xi\mathrm{H}}\\
 * \end{bmatrix} - sR
 * \begin{bmatrix}
 * \Xi & \tilde{\xi} & \tilde{\xi}\\
 * \tilde{\eta} & \mathrm{H} &\tilde{\eta}\\
 * \tilde{\zeta} & \tilde{\zeta} & \mathrm{Z}\\
 * \end{bmatrix}
 * \begin{bmatrix}
 * 1/\rho_{\Xi}\\
 * 1/\rho_{\mathrm{H}}\\
 * 1/\rho_{\mathrm{Z}}\\
 * \end{bmatrix}
 * \f]
 *
 * Recall that the lower case Greek letters with tildes are the variables
 * used for the linear interpolation between the six bounding surfaces, and
 * that the upper case Greek letters are the coordinates along these surfaces -
 * both of which can be specified to be either
 * equidistant or equiangular. In the case where the
 * interpolation variable is chosen to match that of the
 * coordinates along the surface, we have \f$\tilde{\xi} = \Xi\f$, etc. In this
 * case, the formula reduces further. The reduced formula below is the one used
 * for this CoordinateMap. It is given by:
 *
 * \f[
 * \vec{x}(\xi,\eta,\zeta) =
 * \left\{
 * \frac{R}{\sqrt{3}}
 * + sR
 * \left(
 * 1/\rho_{\mathrm{H}\mathrm{Z}}+
 * 1/\rho_{\Xi\mathrm{Z}}+
 * 1/\rho_{\Xi\mathrm{H}}-
 * 1/\rho_{\Xi}-
 * 1/\rho_{\mathrm{H}}-
 * 1/\rho_{\mathrm{Z}}
 * \right)
 * \right\}
 * \begin{bmatrix}
 * \Xi\\
 * \mathrm{H}\\
 * \mathrm{Z}\\
 * \end{bmatrix}
 * \f]
 *
 * The inverse mapping is analytic in the angular directions. A root find
 * must be performed for the inverse mapping in the radial direction. This
 * one-dimensional formula is obtained by taking the magnitude of both sides
 * of the mapping, and changing variables from \f$\xi, \eta, \zeta\f$ to
 * \f$x, y, z\f$ and introducing \f$\rho^2 := \sqrt{\xi^2+\eta^2+\zeta^2}\f$.
 */
class BulgedCube {
 public:
  static constexpr size_t dim = 3;
  BulgedCube(double radius, double sphericity,
             bool use_equiangular_map) noexcept;
  BulgedCube() noexcept = default;
  ~BulgedCube() noexcept = default;
  BulgedCube(BulgedCube&&) noexcept = default;
  BulgedCube(const BulgedCube&) noexcept = default;
  BulgedCube& operator=(const BulgedCube&) noexcept = default;
  BulgedCube& operator=(BulgedCube&&) noexcept = default;

  template <typename T>
  std::array<tt::remove_cvref_wrap_t<T>, 3> operator()(
      const std::array<T, 3>& source_coords) const noexcept;

  boost::optional<std::array<double, 3>> inverse(
      const std::array<double, 3>& target_coords) const noexcept;

  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame> jacobian(
      const std::array<T, 3>& source_coords) const noexcept;

  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame> inv_jacobian(
      const std::array<T, 3>& source_coords) const noexcept;

  // clang-tidy: google runtime references
  void pup(PUP::er& p) noexcept;  // NOLINT

  bool is_identity() const noexcept { return is_identity_; }

 private:
  template <typename T>
  std::array<tt::remove_cvref_wrap_t<T>, 3> xi_derivative(
      const std::array<T, 3>& source_coords) const noexcept;
  friend bool operator==(const BulgedCube& lhs, const BulgedCube& rhs) noexcept;

  double radius_{std::numeric_limits<double>::signaling_NaN()};
  double sphericity_{std::numeric_limits<double>::signaling_NaN()};
  bool use_equiangular_map_ = false;
  bool is_identity_ = false;
};

bool operator!=(const BulgedCube& lhs, const BulgedCube& rhs) noexcept;
}  // namespace CoordinateMaps
}  // namespace domain


#include <iomanip>
#include <iostream>

#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/cbrt.hpp>
#include <boost/math/special_functions/gamma.hpp>

#include <boost/math/bindings/e_float.hpp>
#include <boost/multiprecision/number.hpp>

#include "examples.h"

typedef boost::multiprecision::number<boost::math::ef::e_float,
                                      boost::multiprecision::et_off>
e_float_type;

void examples::nr_021::boost_bindings()
{
  const e_float_type y(char(1));
  const e_float_type x(3.0L);
  const e_float_type z(y / x);

  const e_float_type a(z);

  const e_float_type lg_max = log((std::numeric_limits<e_float_type>::max)());
  const e_float_type eps    = std::numeric_limits<e_float_type>::epsilon();

  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << e_float_type(0.5F)                 << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << z                                  << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << sin  (z)                           << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << floor(e_float_type(1.5F))          << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << ceil (e_float_type(1.5F))          << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << sqrt (e_float_type(2))             << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << boost::math::cbrt(e_float_type(2)) << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << exp  (z)                           << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << atan2(y, x)                        << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << atan (z)                           << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << log  (e_float_type(2))             << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << lg_max                             << std::endl;

  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << eps                    << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << sqrt(eps)              << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << boost::math::cbrt(eps) << std::endl;

  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << boost::math::tgamma(e_float_type(0.5F))          << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << sqrt(boost::math::constants::pi<e_float_type>()) << std::endl;

  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << boost::math::cyl_bessel_j(e_float_type(1) / 7, e_float_type(2.5F))  << std::endl;

  int nexp;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << frexp(e_float_type(2), &nexp) << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10) << ldexp(e_float_type(1), 2)     << std::endl;

  std::cout << std::boolalpha << (x < y) << std::endl;
  std::cout << std::boolalpha << (y > x) << std::endl;

  std::cout << std::boolalpha << (lg_max > (std::numeric_limits<std::int64_t>::max)()) << std::endl;
  std::cout << std::boolalpha << (lg_max > (std::numeric_limits<std::int64_t>::min)()) << std::endl;

  const e_float_type a3(("33." + std::string(std::size_t(std::numeric_limits<e_float_type>::digits10 - 2), char('3')) + std::string(16U, char('4'))).c_str());
  const e_float_type b3(e_float_type(100U) / 3);

  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10 + 4) << a3 << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10 + 4) << b3 << std::endl;

  std::cout << std::boolalpha << (a3 == b3) << std::endl;
  std::cout << std::boolalpha << (a3 >  b3) << std::endl;
  std::cout << std::boolalpha << (a3 <  b3) << std::endl;

  const e_float_type c3(("33." + std::string(120U, char('3'))).c_str());
  const e_float_type d3(e_float_type(100U) / 3);

  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10 + 4) << c3 << std::endl;
  std::cout << std::setprecision(std::numeric_limits<e_float_type>::digits10 + 4) << d3 << std::endl;

  std::cout << std::boolalpha << (c3 == d3) << std::endl;
  std::cout << std::boolalpha << (c3 >  d3) << std::endl;
  std::cout << std::boolalpha << (c3 <  d3) << std::endl;
}


//------------------------------------------------------------------------------
//  Copyright 2007-2011 by Jyh-Ming Lien and George Mason University
//  See the file "LICENSE" for more information
//------------------------------------------------------------------------------

#include "polygon.h"
#include "intersection.h"
#include <vector>
#include <cmath>

#include <boost/polygon/polygon.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>
#include <boost/geometry/algorithms/equals.hpp>

#include "adapt_boost_poly.h"

using namespace std;
namespace bp = boost::polygon;
namespace bg = boost::geometry;

#ifdef WIN32
extern "C"{
#include "triangulate.h"
}
#else
#include "triangulate.h"
#endif

template<typename Pt_>
ply_vertex<Pt_>::~ply_vertex()
{
    //doing nothing for now
}

// - compute normal
// - check if the vertex is reflex or not
template<typename Pt_>
void
ply_vertex<Pt_>::computeExtraInfo()
{
    //compute normal direction
    point_type v= next->pos;
    bg::subtract_point(v, pos);
    if( bg::get<0>(v) == 0 ){
        if (bg::get<1>(v)>0) { bg::set<0>(normal,1); bg::set<1>(normal,0); }
        else { bg::set<0>(normal,-1); bg::set<1>(normal,0); }
    }
    else if( bg::get<0>(v)>0 ){
      bg::set<1>(normal, -1);
      bg::set<0>(normal, bg::get<1>(v) / bg::get<0>(v));
    }
    else{//get<0>(v)<0
      bg::set<1>(normal, 1);
      bg::set<0>(normal, -(bg::get<1>(v) / bg::get<0>(v)));
    }

    // normalize
    normalize(normal);

    // compute left or right turn
    reflex = !leftTurn(pre->pos, pos, next->pos);
}

template<typename Pt_>
void
ply_vertex<Pt_>::negate()
{
    bg::multiply_value(normal, -1);
    bg::multiply_value(pos, -1);
}

template<typename Pt_>
void
ply_vertex<Pt_>::reverse()
{
    swap(next,pre);
    //normal=-normal;
    computeExtraInfo();
}

template<typename Pt_>
void
ply_vertex<Pt_>::copy(ply_vertex * other)
{
    pos=other->pos;
    normal=other->normal;
    reflex=other->reflex;
    vid=other->vid;
}

///////////////////////////////////////////////////////////////////////////////

//copy from the given ply
template<typename Pt_>
void
c_ply<Pt_>::copy(const c_ply<Pt_>& other)
{
    destroy();//detroy myself first

    vertex_type* ptr=other.head;
    beginPoly();
    do{
        vertex_type * v=new vertex_type();
        assert(v); //check for memory
        v->copy(ptr);
        addVertex(v);
        ptr=ptr->getNext();
    }while( ptr!=other.head );

    //endPoly();
    //finish up
    tail->setNext(head);

    //copy extra info
    area=other.area;
    bg::assign_point(center, other.center);
    radius=other.radius;
    type=other.type;
    triangulation=other.triangulation;
    extra_info=other.extra_info;
}

// clean up the space allocated
template<typename Pt_>
void
c_ply<Pt_>::destroy()
{
    if( head==NULL ) return;
    vertex_type* ptr=head;
    do{
        vertex_type * n=ptr->getNext();
        delete ptr;
        ptr=n;
    }while( ptr!=head );
    head=tail=NULL;

    all.clear();
    triangulation.clear();
}

// Create a empty polygon
template<typename Pt_>
void
c_ply<Pt_>::beginPoly()
{
    head=tail=NULL;
    all.clear();
    triangulation.clear();
}

// Add a vertex to the polygonal chian
template<typename Pt_>
void
c_ply<Pt_>::addVertex(
    typename c_ply<Pt_>::coordinate_type x,
    typename c_ply<Pt_>::coordinate_type y,
    bool remove_duplicate )
{
    point_type pt(x,y);

    if(tail!=NULL){
        if(bg::equals(tail->getPos(), pt) && remove_duplicate) return; //don't add
    }

    vertex_type * v=new vertex_type(pt);
    if( tail!=NULL ){
        tail->setNext(v);
    }
    tail=v;
    if( head==NULL ) head=tail;
    v->setVID(all.size()); //id of the vertex in this ply
	all.push_back(v);

}

// Add a vertex to the polygonal chian
template<typename Pt_>
void
c_ply<Pt_>::addVertex( vertex_type * v )
{
    if( tail!=NULL ){
        tail->setNext(v);
    }
    tail=v;
    if( head==NULL ) head=tail;
    v->setVID(all.size()); //id of the vertex in this ply
    all.push_back(v);
}

// finish building the polygon
template<typename Pt_>
void
c_ply<Pt_>::endPoly(bool remove_duplicate)
{
    if(head!=NULL && tail!=NULL){
        if(remove_duplicate){
            if(bg::equals(head->getPos(), tail->getPos())){ //remove tail..
                delete tail;
                all.pop_back();
                tail=all.back();
            }
        }//
    }

    tail->setNext(head);
    doInit();
}

// initialize property of the this polychain
// Compute normals and find reflective vertices
template<typename Pt_>
void
c_ply<Pt_>::doInit()
{
    //compute area
    getArea();
    if(this->area<0 && type==POUT){
       //cerr<<"! Warning: polygon type is POUT but has negative area. Reverse the vertex ordering."<<endl;
       reverse();
    }
    else if(this->area>0 && type==PIN){
       //cerr<<"! Warning: polygon type is PIN but has positive area. Reverse the vertex ordering."<<endl;
       reverse();
    }

    //compute normals
    vertex_type* ptr=head;
    do{
        ptr->computeExtraInfo();
        ptr=ptr->getNext();
    }while( ptr!=head );
}

template<typename Pt_>
const typename c_ply<Pt_>::point_type&
c_ply<Pt_>::getCenter()
{
    if(radius<0){
        center = point_type(0,0);
        vertex_type * ptr=head;
        const point_type& first=ptr->getPos();
        uint size=0;
        do{
            size++;
            point_type v = ptr->getPos();
            bg::subtract_point(v, first);
            bg::add_point(center, v);
            ptr=ptr->getNext();
        }while(ptr!=head); //end while
        bg::divide_value(center, size);
        bg::add_point(center, first);

        radius=0;
    }

    return center;
}

///////////////////////////////////////////////////////////////////////////
template<typename Pt_>
void
c_ply<Pt_>::negate()
{
    vertex_type * ptr=head;
    do{
        ptr->negate();
        ptr=ptr->getNext();
    }while(ptr!=head); //end while
}

//reverse the order of the vertices
template<typename Pt_>
void
c_ply<Pt_>::reverse()
{
    vertex_type * ptr=head;
    do{
        ptr->reverse();
        ptr=ptr->getNext();
    }
    while(ptr!=head); //end while

    this->area=-this->area;
    all.clear();
    triangulation.clear();
}

///////////////////////////////////////////////////////////////////////////
template<typename Pt_>
void
c_ply<Pt_>::translate(const point_type& p)
{
    vertex_type * ptr=head;
    do{
        ptr->translate(p);
        ptr=ptr->getNext();
    }while(ptr!=head); //end while

    //translate box
    extra_info.box[0] += bg::get<0>(p);
    extra_info.box[1] += bg::get<0>(p);
    extra_info.box[2] += bg::get<1>(p);
    extra_info.box[3] += bg::get<1>(p);
}

///////////////////////////////////////////////////////////////////////////
//compute the Radius of the poly chain
template<typename Pt_>
typename c_ply<Pt_>::coordinate_type
c_ply<Pt_>::getRadius()
{
    if(radius<0) getCenter();

    if(radius==0){
        vertex_type * ptr=head;
        do{
            point_type v = center;
            bg::subtract_point(v, ptr->getPos());
            coordinate_type d;
            normalize(v, d);
            if(d>radius) radius=d;
            ptr=ptr->getNext();
        }while(ptr!=head); //end while
        radius=sqrt(radius);
    }

    return radius;
}

template<typename Pt_>
typename c_ply<Pt_>::coordinate_type
c_ply<Pt_>::getArea()
{
    if (area == numeric_limits<coordinate_type>::lowest())
      area = bp::area(*this);

    return area;
}

template<typename Pt_>
bool
c_ply<Pt_>::enclosed(const typename c_ply<Pt_>::point_type& p)
{
    if(triangulation.empty())
        triangulate(triangulation);

    //find the largest triangle
    for(uint i=0;i<triangulation.size();i++){
        triangle & tri=triangulation[i];
        const point_type& p1=all[tri.v[0]]->getPos();
        const point_type& p2=all[tri.v[1]]->getPos();
        const point_type& p3=all[tri.v[2]]->getPos();
        coordinate_type area1=Area(p1,p2,p);
        coordinate_type area2=Area(p2,p3,p);
        coordinate_type area3=Area(p3,p1,p);
        if(area1>=0 && area2>=0 && area3>=0) return true; //in
        if(area1<=0 && area2<=0 && area3<=0) return true; //in
    }

    return false; //out
}

template<typename Pt_>
typename c_ply<Pt_>::point_type
c_ply<Pt_>::findEnclosedPt()
{
    if(triangulation.empty())
        triangulate(triangulation);

    if(triangulation.empty()){ //too few points...
        const point_type& p1=head->getPos();
        const point_type& p2=head->getNext()->getPos();
        point_type pt;
        bg::assign_point(pt, p1);
        bg::add_point(pt, p2);
        bg::divide_value(pt, 2);
        return pt;
    }

    //find the largest triangle
    coordinate_type largest_area=-1;
    uint   largest_tri=0;
    for(uint i=0;i<triangulation.size();i++){
        triangle & tri=triangulation[i];
        const point_type& p1=all[tri.v[0]]->getPos();
        const point_type& p2=all[tri.v[1]]->getPos();
        const point_type& p3=all[tri.v[2]]->getPos();
        coordinate_type area=fabs(Area(p1,p2,p3));
        if(area>largest_area){
            largest_area=area;
            largest_tri=i;
        }
    }

    //find a node near the vertex of the triangle
    triangle & tri=triangulation[largest_tri];
    const point_type& p1=(*this)[tri.v[0]]->getPos();
    const point_type& p2=(*this)[tri.v[1]]->getPos();
    const point_type& p3=(*this)[tri.v[2]]->getPos();

    point_type pt;
    bg::assign_point(pt, p1);
    bg::add_point(pt, p2);
    bg::add_point(pt, p3);
    bg::divide_value(pt, 3);

    return pt;
}

template<typename Pt_>
void
c_ply<Pt_>::triangulate(vector<triangle>& tris)
{
     indexing();
     if(triangulation.empty()){

         const point_type& O=getHead()->getPos();

         int * ringVN=new int[1];     //number of vertices for each ring
         assert(ringVN);
         ringVN[0]=getSize();
         int vN=ringVN[0];             //total number of vertices

         if( vN<3 ){
             triangle tri;
             for(short i=0;i<3;i++) tri.v[i]=i;
             tris.push_back(tri);
             return;
         }

         //more than 3 vertices
         int tN=(vN-2);                   //# of triangles
         double * V = new double[vN*2]; //to hold vertices pos
         int *T=new int[3*tN];            //to hold resulting triangles
         assert(T&&V);

         //copy vertices
         {
             int i=0;
             vertex_type * ptr=getHead();
             do{
                 point_type pt=ptr->getPos();
                 V[i*2]=bg::get<0>(pt)-bg::get<0>(O);    // potential implicit conversion
                 V[i*2+1]=bg::get<1>(pt)-bg::get<1>(O);
                 ptr=ptr->getNext();
                 i++;
             }while( ptr!=getHead() );
         }

         FIST_PolygonalArray(1, ringVN, (double (*)[2])V, &tN, (int (*)[3])T);

         for(int i=0;i<tN;i++){
             triangle tri;
             for(int j=0;j<3;j++){
                 tri.v[j]=T[i*3+j];
             }//end j
             triangulation.push_back(tri);
         }//end i

         delete [] ringVN;
         delete [] V;
         delete [] T;
     }//end if

     tris=triangulation;
}

//check if convex
template<typename Pt_>
bool
c_ply<Pt_>::is_convex() const
{
    vertex_type * ptr=head;
    do{
        if(ptr->isReflex()) return false;
        ptr=ptr->getNext();
    }while(ptr!=head); //end while

    return true;
}

template<typename Pt_>
void
c_ply<Pt_>::delete_vertex(vertex_type * v)
{
    vertex_type *pre=v->getPre();
    vertex_type *next=v->getNext();

    pre->setNext(next);
    next->setPre(pre);
    pre->computeExtraInfo(); //recompute info

    if(head==v){
        head=next;
        tail=pre;
    }
    delete v;

    triangulation.clear(); //not valid anymore
    all.clear(); //not valid anymore
}

template<typename Pt_>
void
c_ply<Pt_>::indexing()
{
    uint vid=0;
    all.clear();
    vertex_type * ptr=head;
    do{
        ptr->setVID(vid++);
        all.push_back(ptr);
        ptr=ptr->getNext();
    }while(ptr!=head); //end while
}

template<typename Pt_>
bool
c_ply<Pt_>::identical(const c_ply<Pt_>& other)
{

    //do some basic checks
    if((*this)==other) return true; //same head ptr
    if(type!=other.type) return false;
    if(getSize()!=other.getSize()) return false;
    if( fabs(getArea()-other.getArea())>SMALLNUMBER ) return false;

    //find the first match
    const vertex_type * o_ptr=other.head;
    bool found_first_match=false;
    do{
        const point_type& pos=o_ptr->getPos();
        coordinate_type dx=fabs(bg::get<0>(pos)-bg::get<0>(head->getPos()));
        coordinate_type dy=fabs(bg::get<1>(pos)-bg::get<0>(head->getPos()));

        if( dx<SMALLNUMBER && dy<SMALLNUMBER){
            found_first_match=true;
            break;
        }

        o_ptr=o_ptr->getNext();
    }while(o_ptr!=other.head); //end while

    if(!found_first_match) return false;

    //check the rest of the match
    const vertex_type * ptr=head;

    //since we are sure that ptr matches to o_ptr, we first advance
    ptr=ptr->getNext();
    o_ptr=o_ptr->getNext();

    do
    {
        const point_type& o_pos=o_ptr->getPos();
        const point_type& pos=ptr->getPos();

        coordinate_type dx=fabs(bg::get<0>(pos)-bg::get<0>(o_pos));
        coordinate_type dy=fabs(bg::get<1>(pos)-bg::get<1>(o_pos));

        if( dx>SMALLNUMBER || dy>SMALLNUMBER){
            return false; //don't match
        }

        //advance
        ptr=ptr->getNext();
        o_ptr=o_ptr->getNext();

    }
    while(ptr!=head);

    return true;
}

//
//
// Compute the center and the box of a list of plys
//
//
template<typename Pt_>
void
c_plylist<Pt_>::buildBoxAndCenter()
{
    box[0] = box[2] = std::numeric_limits<coordinate_type>::max();
    box[0] = box[3] = std::numeric_limits<coordinate_type>::lowest();
    for(auto i=begin();i!=end();i++){

        const vertex_type * ptr=i->getHead();
        do{
            const point_type& p=ptr->getPos();
            if(bg::get<0>(p)<box[0]) box[0]= bg::get<0>(p);
            if(bg::get<0>(p)>box[1]) box[1]= bg::get<0>(p);
            if(bg::get<1>(p)<box[2]) box[2]= bg::get<1>(p);
            if(bg::get<1>(p)>box[3]) box[3]= bg::get<1>(p);
            ptr=ptr->getNext();
        }
        while(ptr!=i->getHead()); //end while
    }

    bg::set<0>(center, (box[0]+box[1])/2);
    bg::set<1>(center, (box[2]+box[3])/2);

    is_buildboxandcenter_called=true;
}

template<typename Pt_>
void
c_plylist<Pt_>::translate(const typename c_plylist<Pt_>::point_type& p)
{
    for (auto i=begin();i!=end();i++) i->translate(p);
}

template<typename Pt_>
void
c_polygon<Pt_>::reverse()
{
    for(auto i=begin();i!=end();i++) i->reverse();
    triangulation.clear();
    all.clear();
}

template<typename Pt_>
bool
c_polygon<Pt_>::valid() const
{
    if(empty()) return false;
    if(front().getType()!=ring_type::POUT) return false;
    for(auto i=++begin();i!=end();i++)
      if(i->getType()!=ring_type::PIN)
        return false;

    return true;
}

//copy from the given polygon
template<typename Pt_>
void
c_polygon<Pt_>::copy(const c_polygon& other)
{
    destroy();

    for(auto i=other.cbegin();i!=other.cend();i++){
        this->emplace_back(i->getType());
        this->back().copy(*i);
    }
    indexing();
}

//check if a point is enclosed
//the behavior is unknown if pt is on the boundary of the polygon
template<typename Pt_>
bool
c_polygon<Pt_>::enclosed(const typename c_polygon<Pt_>::point_type& p) const
{
    //find the largest triangle
    for(uint i=0;i<triangulation.size();i++){
        const triangle & tri=triangulation[i];
        const point_type& p1=(*this)[tri.v[0]]->getPos();
        const point_type& p2=(*this)[tri.v[1]]->getPos();
        const point_type& p3=(*this)[tri.v[2]]->getPos();
        coordinate_type area1=Area(p1,p2,p);
        coordinate_type area2=Area(p2,p3,p);
        coordinate_type area3=Area(p3,p1,p);
        if(area1>=0 && area2>=0 && area3>=0) return true; //in
        if(area1<=0 && area2<=0 && area3<=0) return true; //in
    }

    return false; //out
}

template<typename Pt_>
typename c_polygon<Pt_>::point_type
c_polygon<Pt_>::findEnclosedPt()
{
    if(triangulation.empty())
        triangulate(triangulation);

    if(triangulation.empty())
        return front().findEnclosedPt();

    //find the largest triangle
    coordinate_type largest_area=-1;
    uint   largest_tri=0;
    for(uint i=0;i<triangulation.size();i++){
        triangle & tri=triangulation[i];
        const point_type& p1=(*this)[tri.v[0]]->getPos();
        const point_type& p2=(*this)[tri.v[1]]->getPos();
        const point_type& p3=(*this)[tri.v[2]]->getPos();
        coordinate_type area=fabs(Area(p1,p2,p3));
        if(area>largest_area){
            largest_area=area;
            largest_tri=i;
        }
    }

    //find a node near the vertex of the triangle
    triangle & tri=triangulation[largest_tri];
    const point_type& p1=(*this)[tri.v[0]]->getPos();
    const point_type& p2=(*this)[tri.v[1]]->getPos();
    const point_type& p3=(*this)[tri.v[2]]->getPos();

    point_type pt;
    bg::assign_point(pt, p1);
    bg::add_point(pt, p2);
    bg::add_point(pt, p3);
    bg::divide_value(pt, 3);

    return pt;
}

template<typename Pt_>
void
c_polygon<Pt_>::triangulate(vector<triangle>& tris)
{
    if(triangulation.empty())
    {
         const point_type& O=front().getHead()->getPos();

         int ringN=size();             //number of rings
         int * ringVN=new int[ringN];     //number of vertices for each ring
         assert(ringVN);

         int vN=0;             //total number of vertices
         {
             int i=0;
             for(auto ip=begin();ip!=end();ip++,i++){
                 vN+=ip->getSize();
                 ringVN[i]=ip->getSize();
             }
         }

         if( vN<3 ){
//             triangle tri;
//             for(short i=0;i<3;i++) tri.v[i]=i;
//             tris.push_back(tri);
             return;
         }

         int tN=(vN-2)+2*(ringN-1);       //# of triangles, (n-2)+2*(#holes)
         double * V=new double[vN*2];     //to hold vertices pos
         int *T=new int[3*tN];            //to hold resulting triangles
         assert(T&&V);

         //copy vertices
         int i=0;
         for(auto ip=begin();ip!=end();ip++){
             vertex_type * ptr=ip->getHead();
             do{
                 point_type pt=ptr->getPos();
                 V[i*2]=bg::get<0>(pt)-bg::get<0>(O);    // potential implicit conversion to double
                 V[i*2+1]=bg::get<1>(pt)-bg::get<1>(O);
                 ptr=ptr->getNext();
                 i++;
             }while( ptr!=ip->getHead() );
         }

         FIST_PolygonalArray(ringN, ringVN,
             (double (*)[2])V, &tN, (int (*)[3])T);

         for(int i=0;i<tN;i++){
             triangle tri;
             for(int j=0;j<3;j++){
                 tri.v[j]=T[i*3+j];
             }//end j
             triangulation.push_back(tri);
         }//end i

    }

    tris=triangulation;
}

template<typename Pt_>
void
c_polygon<Pt_>::pop_front()
{
  ring_type &ply = front();
  ply.indexing(); // reset vertex IDs
  c_plylist<Pt_>::pop_front();
  /* These must be recomputed */
  all.clear();
  triangulation.clear();
  indexing();
}

template<typename Pt_>
void
c_polygon<Pt_>::destroy()
{
    for(auto i=begin();i!=end();i++){
        i->destroy();
    }
    clear(); //remove all ply from this list
    all.clear();
    triangulation.clear();
}

template<typename Pt_>
bool
c_polygon<Pt_>::is_convex() const
{
    if(size()>1) return false; //contains hole
    return front().is_convex();
}

template<typename Pt_>
void
c_polygon<Pt_>::indexing()
{
    uint vid=0;
    for(auto i=begin();i!=end();i++){
        uint vsize=i->getSize();
        for(uint j=0;j<vsize;j++){
            (*i)[j]->setVID(vid++);       //id of vertex in the polygons
            all.push_back((*i)[j]);
        }
    }//end for i
}

template<typename Pt_>
typename c_polygon<Pt_>::coordinate_type
c_polygon<Pt_>::getArea()
{
  if (area == 0) {
    area = bp::area(*this);
  }

  return area;
}

template<typename Pt_>
bool
c_polygon<Pt_>::identical(c_polygon& p)
{
    if(size()!=p.size()) return false;

    coordinate_type a1=getArea();
    coordinate_type a2=p.getArea();
    if( fabs(a1-a2)>SMALLNUMBER ) return false;

    for(auto i=begin();i!=end();i++)
    {
        bool match=false;
        for(auto j=p.begin();j!=p.end();j++){
            if( i->identical(*j) ){
                match=true;
                break;
            }
        }
        if(!match) return false;
    }

    return true;
}

template<typename Pt_>
istream& operator>>( istream& is, c_ply<Pt_>& poly)
{
    typedef typename c_ply<Pt_>::coordinate_type coordinate_type;
    typedef typename c_ply<Pt_>::point_type point_type;

    int vsize; string str_type;
    is>>vsize>>str_type;

    if( str_type.find("out")!=string::npos )
        poly.type=c_ply<Pt_>::POUT;
    else poly.type=c_ply<Pt_>::PIN;

    poly.beginPoly();
    //read in all the vertices
    int iv;
    vector<point_type> pts; pts.reserve(vsize);
    for( iv=0;iv<vsize;iv++ ){
        coordinate_type x,y;
        is>>x>>y;
        pts.emplace_back(x,y);
        //coordinate_type d=x*x+y*y;
    }
    int id;
    for( iv=0;iv<vsize;iv++ ){
        is>>id; id=id-1;
        poly.addVertex(bg::get<0>(pts[id]), bg::get<1>(pts[id]));
    }

    poly.endPoly();
    return is;
}

template<typename Pt_>
istream& operator>>( istream& is, c_plylist<Pt_>& p)
{
    //remove header commnets
    do{
        char tmp[1024];
        char c=is.peek();
        if(isspace(c)) is.get(c); //eat it
        else if(c=='#') {
            is.getline(tmp,1024);
        }
        else break;
    }while(true);

    //start reading
    uint size;
    is>>size;
    uint vid=0;
    for(uint i=0;i<size;i++){
        c_ply<Pt_> poly(c_ply<Pt_>::UNKNOWN);
        is>>poly;
        p.push_back(poly);
        uint vsize=poly.getSize();
        for(uint j=0;j<vsize;j++){
            poly[j]->setVID(vid++);       //id of vertex in the polygons
            //p.all.push_back(poly[j]);
        }
    }
    return is;
}

template<typename Pt_>
ostream&
operator<<( ostream& os, const c_ply<Pt_>& p)
{
    os<<p.getSize()<<" "<<((p.type==c_ply<Pt_>::PIN)?"PIN  ":"POUT ");
    const typename c_ply<Pt_>::vertex_type * ptr=p.head;
    os << "< " << ptr->getPos();
    ptr = ptr->getNext();
    while(ptr!=p.head)
    {
        os<< ", " << ptr->getPos();
        ptr=ptr->getNext();
    }

    return os;
}

template<typename Pt_>
ostream&
operator<<( ostream& out, const c_plylist<Pt_>& p)
{
    out<<p.size()<<"\n";
    for(auto i=p.cbegin();i!=p.cend();i++) out<<*i;
    return out;
}

template<typename Pt_>
ostream &
operator<<(ostream &os, const c_polygon<Pt_> &p)
{
  unsigned int idx = 0u;
  os << "c_polygon({" << endl;
  for (auto ply_it = p.cbegin(); ply_it != p.cend(); ++ply_it)
    os << "    [" << setw(2) << idx++ << "] " << *ply_it << endl;
  os << "})";
  return os;
}

template<typename Pt_>
ostream &
operator<<(ostream &os, const vector<c_polygon<Pt_> > &v)
{
  os << "vector<c_polygon>({" << endl;
  unsigned int idx = 0u;
  for (auto it = v.begin(); it != v.end(); ++it)
    os << "  [" << setw(2) << idx++ << "] " << *it << endl;
  os << "})";
  return os;
}

template<typename Tp_>
ostream & operator<<(ostream &os,
    boost::geometry::model::d2::point_xy<Tp_> const&p)
{
  os << "(" << bg::get<0>(p) << "," << bg::get<1>(p) << ")";
  return os;
}

PLY_INSTANTIATE(boost::geometry::model::d2::point_xy<float>, );
PLY_INSTANTIATE(boost::geometry::model::d2::point_xy<double>, );

template std::ostream &operator<< <float>(std::ostream&,
    boost::geometry::model::d2::point_xy<float> const&);
template std::ostream &operator<< <double>(std::ostream&,
    boost::geometry::model::d2::point_xy<double> const&);


/*
  Total variation denoising based on the paper
  "The Split Bregman Method for L1-Regularized Problems" by Tom Goldstein and Stanley Osher.
  Siam J. Imaging Sciences. Vol. 2, No. 2, pp. 323-343.
*/

// Gadgetron includes
#include "cuNDArray.h"
#include "hoNDArray_fileio.h"
#include "cuSbCgSolver.h"
#include "cuCgSolver.h"
#include "identityOperator.h"
#include "cuPartialDerivativeOperator.h"
#include "parameterparser.h"
#include "cuNDDWT.h"
#include "cuDWTOperator.h"
#include <boost/make_shared.hpp>
// Std includes
#include <iostream>

using namespace std;
using namespace Gadgetron;

// Define desired precision
typedef float _real;

int main(int argc, char** argv)
{
  //
  // Parse command line
  //

  ParameterParser parms;
  parms.add_parameter( 'd', COMMAND_LINE_STRING, 1, "Noisy image file name (.real)", true );
  parms.add_parameter( 'r', COMMAND_LINE_STRING, 1, "Result file name", true, "denoised_image_TV.real" );
  parms.add_parameter( 'i', COMMAND_LINE_INT,    1, "Number of cg iterations", true, "20" );
  parms.add_parameter( 'I', COMMAND_LINE_INT,    1, "Number of sb inner iterations", true, "1" );
  parms.add_parameter( 'O', COMMAND_LINE_INT,    1, "Number of sb outer iterations", true, "10" );
  parms.add_parameter( 'l', COMMAND_LINE_FLOAT,  1, "Total variation weight (lambda)", true, "50.0" );
  parms.add_parameter( 'm', COMMAND_LINE_FLOAT,  1, "Regularization weight (mu)", true, "25.0" );
  parms.add_parameter('w', COMMAND_LINE_FLOAT, 1, "Wavelet weight" ,true, "0");

  parms.parse_parameter_list(argc, argv);
  if( parms.all_required_parameters_set() ){
    cout << " Running denoising with the following parameters: " << endl;
    parms.print_parameter_list();
  }
  else{
    cout << " Some required parameters are missing: " << endl;
    parms.print_parameter_list();
    parms.print_usage();
    return 1;
  }

  // Load sample data from disk
  boost::shared_ptr< hoNDArray<_real> > host_data =
    read_nd_array<_real>((char*)parms.get_parameter('d')->get_string_value());

  if( !host_data.get() ){
    cout << endl << "Input image not found. Quitting!\n" << endl;
    return 1;
  }

  if( host_data->get_number_of_dimensions() != 2 ){
    cout << endl << "Input image is not two-dimensional. Quitting!\n" << endl;
    return 1;
  }

  // Upload host data to device
  cuNDArray<_real> data(host_data.get());

  _real mu = (_real) parms.get_parameter('m')->get_float_value();
  _real lambda = (_real)parms.get_parameter('l')->get_float_value();

  if( mu <= (_real) 0.0 ) {
    cout << endl << "Regularization parameter mu should be strictly positive. Quitting!\n" << endl;
    return 1;
  }

  unsigned int num_cg_iterations = parms.get_parameter('i')->get_int_value();
  unsigned int num_inner_iterations = parms.get_parameter('I')->get_int_value();
  unsigned int num_outer_iterations = parms.get_parameter('O')->get_int_value();

 // Define encoding operator (identity)
  boost::shared_ptr< identityOperator<cuNDArray<_real> > > E( new identityOperator<cuNDArray<_real> >() );
  E->set_weight( mu );
  E->set_domain_dimensions(data.get_dimensions().get());
  E->set_codomain_dimensions(data.get_dimensions().get());

  // Setup split-Bregman solver
  cuSbCgSolver<_real> sb;
  sb.set_encoding_operator( E );
  sb.set_max_outer_iterations(num_outer_iterations);
  sb.set_max_inner_iterations(num_inner_iterations);
  sb.set_output_mode( cuCgSolver<_real>::OUTPUT_VERBOSE );
   // Setup regularization operators

  if (lambda > 0){
  boost::shared_ptr< cuPartialDerivativeOperator<_real,2> > Rx( new cuPartialDerivativeOperator<_real,2>(0) );
  Rx->set_weight( lambda );
  Rx->set_domain_dimensions(data.get_dimensions().get());
  Rx->set_codomain_dimensions(data.get_dimensions().get());

  boost::shared_ptr< cuPartialDerivativeOperator<_real,2> > Ry( new cuPartialDerivativeOperator<_real,2>(1) );
  Ry->set_weight( lambda );
  Ry->set_domain_dimensions(data.get_dimensions().get());
  Ry->set_codomain_dimensions(data.get_dimensions().get());
  //sb.add_regularization_operator( Rx ); // Anisotropic denoising
  //sb.add_regularization_operator( Ry ); // Anisotropic denoising
  sb.add_regularization_group_operator( Rx ); // Isotropic denoising
  sb.add_regularization_group_operator( Ry); // Isotropic denoising
  sb.add_group();
  }

  _real wavelet = parms.get_parameter('w')->get_float_value();
  if (wavelet > 0){
	  auto dwt = boost::make_shared<cuDWTOperator<_real,2>>();
	  dwt->set_levels(3);
	  dwt->set_weight(wavelet);
	  sb.add_regularization_operator(dwt);
	  dwt->set_domain_dimensions(data.get_dimensions().get());
	  dwt->set_codomain_dimensions(data.get_dimensions().get());
	  dwt->use_random(true);
  }

  // Setup inner conjugate gradient solver
  sb.get_inner_solver()->set_max_iterations( num_cg_iterations );
  sb.get_inner_solver()->set_tc_tolerance( 1e-4 );
  sb.get_inner_solver()->set_output_mode( cuCgSolver<_real>::OUTPUT_WARNINGS );

  // Run split-Bregman solver
  boost::shared_ptr< cuNDArray<_real> > sbresult = sb.solve(&data);

  /*
  boost::shared_ptr< cuNDArray<_real> > sbresult(new cuNDArray<_real>(data.get_dimensions()));
  clear(sbresult.get());

  vector_td<float,4> daubechies4({0.6830127f,1.1830127f,0.3169873f,-0.1830127f});
  vector_td<float,2> haahr(1.0f,1.0f);
  vector_td<float,6> daubechies6{0.47046721f,1.14111692f,0.650365f,-0.19093442f, -0.12083221f,0.0498175f};

  cuDWTOperator<float,2> dwt;
  dwt.set_levels(3);
  dwt.mult_M(&data,sbresult.get());
  //data = *sbresult;
  shrink1(sbresult.get(),30.0f,&data);
  dwt.mult_MH(&data,sbresult.get());*/
  //clear(sbresult.get());
 // All done, write out the result
  boost::shared_ptr< hoNDArray<_real> > host_result = sbresult->to_host();
  write_nd_array<_real>(host_result.get(), (char*)parms.get_parameter('r')->get_string_value());

  return 0;
}


/*
 * Copyright (C) 2015, Nils Moehrle
 * TU Darmstadt - Graphics, Capture and Massively Parallel Computing
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD 3-Clause license. See the LICENSE.txt file for details.
 */

#include <cstdint>
#include <iostream>

#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <math/vector.h>

#include "poisson_blending.h"

typedef Eigen::SparseMatrix<float> SpMat;

math::Vec3f simple_laplacian(int i, mve::FloatImage::ConstPtr img) {
  const int width = img->width();
  assert(i > width + 1 && i < img->get_pixel_amount() - width - 1);

  return -4.0f * math::Vec3f(&img->at(i, 0))
         + math::Vec3f(&img->at(i - width, 0)) + math::Vec3f(&img->at(i - 1, 0))
         + math::Vec3f(&img->at(i + 1, 0))
         + math::Vec3f(&img->at(i + width, 0));
}

bool valid_mask(mve::ByteImage::ConstPtr mask) {
  const int width = mask->width();
  const int height = mask->height();

  for (int x = 0; x < width; ++x)
    if (mask->at(x, 0, 0) == 255 || mask->at(x, height - 1, 0) == 255)
      return false;

  for (int y = 0; y < height; ++y)
    if (mask->at(0, y, 0) == 255 || mask->at(width - 1, y, 0) == 255)
      return false;

  // TODO check for sane boundary conditions...

  return true;
}

void poisson_blend(
    mve::FloatImage::ConstPtr src,
    mve::ByteImage::ConstPtr mask,
    mve::FloatImage::Ptr dest,
    float alpha) {
  assert(src->width() == mask->width() && mask->width() == dest->width());
  assert(src->height() == mask->height() && mask->height() == dest->height());
  assert(src->channels() >= 3 && dest->channels() == src->channels());
  assert(mask->channels() == 1);
  assert(valid_mask(mask));

  const int n = dest->get_pixel_amount();
  const int width = dest->width();
  const int height = dest->height();
  // only do poisson on the color channels--first three
  // TODO dwh: get rid of hard coded color channels=3
  const int channels = std::min(dest->channels(), 3);

  mve::Image<int>::Ptr indices = mve::Image<int>::create(width, height, 1);
  indices->fill(-1);
  int index = 0;
  for (int i = 0; i < n; ++i) {
    if (mask->at(i) != 0) {
      indices->at(i) = index;
      index += 1;
    }
  }
  const int nnz = index;

  std::vector<math::Vec3f> coefficients_b;
  coefficients_b.resize(nnz);
  // std::cout << "blend 0 " << std::endl;
  std::vector<
      Eigen::Triplet<float, int>,
      Eigen::aligned_allocator<Eigen::Triplet<float, int>>>
      coefficients_A;
  coefficients_A.reserve(nnz);  // TODO better estimate...
  // std::cout << "blend A " << std::endl;
  for (int i = 0; i < n; ++i) {
    const int row = indices->at(i);
    if (mask->at(i) == 128 || mask->at(i) == 64) {
      Eigen::Triplet<float, int> t(row, row, 1.0f);
      coefficients_A.push_back(t);

      coefficients_b[row] = math::Vec3f(&dest->at(i, 0));
    }

    if (mask->at(i) == 255) {
      const int i01 = indices->at(i - width);
      const int i10 = indices->at(i - 1);
      const int i11 = indices->at(i);
      const int i12 = indices->at(i + 1);
      const int i21 = indices->at(i + width);

      /* All neighbours should be eighter border conditions or part of the
       * optimization. */
      assert(i01 != -1 && i10 != -1 && i11 != -1 && i12 != -1 && i21 != -1);

      Eigen::Triplet<float, int> t01(row, i01, 1.0f);

      Eigen::Triplet<float, int> t10(row, i10, 1.0f);
      Eigen::Triplet<float, int> t11(row, i11, -4.0f);
      Eigen::Triplet<float, int> t12(row, i12, 1.0f);

      Eigen::Triplet<float, int> t21(row, i21, 1.0f);

      Eigen::Triplet<float, int> triplets[] = {t01, t10, t11, t12, t21};

      coefficients_A.insert(coefficients_A.end(), triplets, triplets + 5);

      math::Vec3f l_d = simple_laplacian(i, dest);
      math::Vec3f l_s = simple_laplacian(i, src);

      coefficients_b[row] = (alpha * l_s + (1.0f - alpha) * l_d);
    }
  }
  // std::cout << "blend B " << std::endl;
  SpMat A(nnz, nnz);
  A.setFromTriplets(coefficients_A.begin(), coefficients_A.end());
  // std::cout << "blend C " << std::endl;
  Eigen::SparseLU<SpMat, Eigen::COLAMDOrdering<int>> solver;
  // std::cout << "blend Cp " << std::endl;
  // std::cout << nnz << std::endl;
  solver.compute(A);
  // std::cout << "blend D " << std::endl;
  for (int channel = 0; channel < channels; ++channel) {
    Eigen::VectorXf b(nnz);
    for (std::size_t i = 0; i < coefficients_b.size(); ++i)
      b[i] = coefficients_b[i][channel];

    Eigen::VectorXf x(n);
    x = solver.solve(b);

    for (int i = 0; i < n; ++i) {
      int index = indices->at(i);
      if (index != -1)
        dest->at(i, channel) = x[index];
    }
  }
  // std::cout << "blend E " << std::endl;
}