#include "lwtnn/LightweightNeuralNetwork.hh"
#include "lwtnn/parse_json.hh"
#include "lwtnn/NanReplacer.hh"

#include <Eigen/Dense>

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

namespace {
  int run_on_files(const lwt::JSONConfig& config,
                   const std::string& vars,
                   const std::string& vals);
  // ramp function so that the inputs _after_ normalization fall on the
  // [-1,1] range, i.e. `np.linspace(-1, 1, n_entries)`
  double ramp(const lwt::Input& in, size_t pos, size_t n_entries);
  int run_on_generated(const lwt::JSONConfig& config);
}

std::vector<std::string> parse_line(std::string& line) {
  std::stringstream          line_stream(line);
  std::string                cell;

  std::vector<std::string>   result;
  while(line_stream >> cell) {
    result.push_back(cell);
  }
  return result;
}

void usage(const std::string& name) {
  std::cout << "usage: " << name << " <nn config> [<labels> <values>]\n"
    "\n"
    "The <nn config> file should be generated by one of the scripts in\n"
    "`converters/`.\n"
    "\n"
    "Both <labels> and <values> should be whitespace separated.\n"
    "In the case of values, there should be one pattern per line.\n"
    "If the labels and values are omitted generate dummy data.\n";
}

int main(int argc, char* argv[]) {
  if (argc > 4 || argc < 2) {
    usage(argv[0]);
    exit(1);
  }
  // Read in the configuration.
  std::string in_file_name(argv[1]);
  std::ifstream in_file(in_file_name);
  auto config = lwt::parse_json(in_file);

  if ( argc == 4) {
    run_on_files(config, argv[2], argv[3]);
  } else if (argc == 2) {
    run_on_generated(config);
  } else {
    usage(argv[0]);
    exit(1);
  }
  return 0;
}
namespace {
  int run_on_generated(const lwt::JSONConfig& config) {
    lwt::LightweightNeuralNetwork tagger(
      config.inputs, config.layers, config.outputs);
    std::map<std::string, double> in_vals;

    const size_t total_inputs = config.inputs.size();
    for (size_t nnn = 0; nnn < total_inputs; nnn++) {
      const auto& input = config.inputs.at(nnn);
      double ramp_val = ramp(input, nnn, total_inputs);
      in_vals[input.name] = ramp_val;
    }
    auto out_vals = tagger.compute(in_vals);
    for (const auto& out: out_vals) {
      std::cout << out.first << " " << out.second << std::endl;
    }
    return 0;
  }
  int run_on_files(const lwt::JSONConfig& config,
                   const std::string& vars,
                   const std::string& vals) {
    // initialize the tagger from the configuration
    lwt::LightweightNeuralNetwork tagger(
      config.inputs, config.layers, config.outputs);
    lwt::NanReplacer replacer(config.defaults, lwt::rep::all);

    // buffer
    std::string val_line;

    // read in input labels and values
    std::ifstream vars_stream( vars );
    std::getline(vars_stream, val_line);
    const auto labels = parse_line(val_line);

    std::ifstream values( vals );
    while (std::getline(values, val_line)) {
      auto val_strings = parse_line(val_line);
      if (val_strings.size() == 0) continue;
      assert(val_strings.size() == labels.size());
      std::map<std::string, double> nn_in;
      for (size_t iii = 0; iii < labels.size(); iii++) {
        nn_in[labels.at(iii)] = std::stof(val_strings.at(iii));
      }
      auto cleaned_inputs = replacer.replace(nn_in);
      auto out_vals = tagger.compute(cleaned_inputs);
      // look at the outputs
      for (const auto& out: out_vals) {
        std::cout << out.first << " " << out.second << std::endl;
      }
    }

    return 0;
  }

  double ramp(const lwt::Input& in, size_t pos, size_t n_entries) {
    double step = 2.0 / (n_entries - 1);
    double x = ( (n_entries == 1) ? 0 : (-1 + pos * step) );
    return x / in.scale - in.offset;
  }

}


// Copyright (C) 2004-2012 The Trustees of Indiana University.

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

//  Authors: Nicholas Edmonds
//           Andrew Lumsdaine

// Example usage of delta_stepping_shortest_paths algorithm

#include <am++/am++.hpp>

// Communicate via MPI
#include <am++/mpi_transport.hpp>

// Enable PBGL interfaces to BGL algorithms
#include <boost/graph/use_mpi.hpp>

// Distributed adjacency list -- NOTE: The graph class needs to be included before any algorithms
#include <boost/graph/distributed/adjacency_list.hpp>

// Dijkstra's single-source shortest paths algorithm
#include <boost/graph/distributed/delta_stepping_shortest_paths.hpp>

// METIS Input
#include <boost/graph/metis.hpp>

// Graphviz Output
#include <boost/graph/distributed/graphviz.hpp>

// Standard Library includes
#include <fstream>
#include <string>

#ifdef BOOST_NO_EXCEPTIONS
void
boost::throw_exception(std::exception const& ex)
{
    std::cout << ex.what() << std::endl;
    abort();
}
#endif

typedef int weight_type;

// Edge property bundle
struct WeightedEdge {
  WeightedEdge(weight_type weight = 0) : weight(weight) { }

  weight_type weight;
};

// Vertex property bundle
struct VertexProperties {
  VertexProperties(weight_type d = 0) : distance(d) { }

  weight_type distance;
};

// Define MPI datatypes for bundled properties so AM++ can send messages containing them
namespace amplusplus {
  template<>
  struct make_mpi_datatype<WeightedEdge> : make_mpi_datatype_base {
    make_mpi_datatype<weight_type> dt1;
    scoped_mpi_datatype dt;
    make_mpi_datatype(): dt1() {
      int blocklengths[1] = {1};
      MPI_Aint displacements[1];
      WeightedEdge test_object;
      MPI_Aint test_object_ptr;
      MPI_Get_address(&test_object, &test_object_ptr);
      MPI_Get_address(&test_object.weight, &displacements[0]);
      displacements[0] -= test_object_ptr;
      MPI_Datatype types[1] = {dt1.get()};
      MPI_Type_create_struct(1, blocklengths, displacements, types, dt.get_ptr());
      MPI_Type_commit(dt.get_ptr());
    }
    MPI_Datatype get() const {return dt;}
  };

  template<>
  struct make_mpi_datatype<VertexProperties> : make_mpi_datatype_base {
    make_mpi_datatype<weight_type> dt1;
    scoped_mpi_datatype dt;
    make_mpi_datatype(): dt1() {
      int blocklengths[1] = {1};
      MPI_Aint displacements[1];
      VertexProperties test_object;
      MPI_Aint test_object_ptr;
      MPI_Get_address(&test_object, &test_object_ptr);
      MPI_Get_address(&test_object.distance, &displacements[0]);
      displacements[0] -= test_object_ptr;
      MPI_Datatype types[1] = {dt1.get()};
      MPI_Type_create_struct(1, blocklengths, displacements, types, dt.get_ptr());
      MPI_Type_commit(dt.get_ptr());
    }
    MPI_Datatype get() const {return dt;}
  };
}

using namespace boost;

/* An undirected graph with distance values stored on the vertices. */
typedef adjacency_list<vecS, distributedS<vecS>, directedS,
                       VertexProperties, WeightedEdge>
  Graph;

int main(int argc, char* argv[])
{
  amplusplus::environment env = amplusplus::mpi_environment(argc, argv);
  amplusplus::transport trans = env.create_transport();

  // Parse command-line options
  const char* filename = "weighted_graph.gr";
  if (argc > 1) filename = argv[1];

  // Open the METIS input file
  std::ifstream in(filename);
  graph::metis_reader reader(in);

  // Load the graph using the default distribution
  Graph g(reader.begin(), reader.end(), reader.weight_begin(),
          reader.num_vertices(), trans);

  // Get vertex 0 in the graph
  graph_traits<Graph>::vertex_descriptor start = vertex(0, g);

  // Initialize delta stepping algorithm
  boost::graph::distributed::delta_stepping_shortest_paths<Graph,
    boost::property_map<Graph, weight_type VertexProperties::*>::type,
    boost::property_map<Graph, weight_type WeightedEdge::*>::type>
      D(g, get(&VertexProperties::distance, g), get(&WeightedEdge::weight, g));

  // Compute shortest paths from vertex 0
  D.run(start);

  // Output a Graphviz DOT file
  std::string outfile = filename;
  if (argc > 2)
    outfile = argv[2];
  else {
    size_t i = outfile.rfind('.');
    if (i > 0 && i != std::string::npos)
      outfile.erase(outfile.begin() + i, outfile.end());
    outfile += "-dijkstra.dot";
  }

  if (trans.rank() == 0) {
    std::cout << "Writing GraphViz output to " << outfile << "... ";
    std::cout.flush();
  }
  write_graphviz(outfile, g,
                 make_label_writer(get(&VertexProperties::distance, g)),
                 make_label_writer(get(&WeightedEdge::weight, g)));
  if (trans.rank() == 0)
    std::cout << "Done." << std::endl;

  return 0;
}


//   -----------------------------------------------------------------------------------------------
//    Copyright 2015 André Bergner. Distributed under the Boost Software License, Version 1.0.
//     (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//      --------------------------------------------------------------------------------------------

#include <iostream>
#include <boost/mpl/int.hpp>
#include <boost/mpl/min_max.hpp>
#include <boost/typeof/typeof.hpp>
#include <boost/proto/core.hpp>
#include <boost/proto/context.hpp>
#include <boost/proto/transform.hpp>
#include <boost/proto/debug.hpp>

#include "callable_decltype.hpp"
#include "tuple_tools.hpp"
#include "demangle.h"

namespace mpl = boost::mpl;
namespace proto = boost::proto;

namespace building_blocks
{
   using namespace proto;

   //  ---------------------------------------------------------------------------------------------
   // definition of the building blocks of the language
   //  ---------------------------------------------------------------------------------------------

   template< typename I >  struct placeholder       { using arity = I; };
   template< typename T >  struct placeholder_arity { using type = typename T::arity; };

   //  ---------------------------------------------------------------------------------------------
   // block composition operators -- TODO needs correct grammar as arguments

   using channel_operator  = comma< _ , _ >;
   using sequence_operator = bitwise_or_assign< _ , _ >;

}

using building_blocks :: placeholder;
using building_blocks :: placeholder_arity;

using building_blocks :: channel_operator;
using building_blocks :: sequence_operator;

BOOST_PROTO_DEFINE_ENV_VAR( current_input_t, current_input );

namespace transforms
{
   using namespace proto;

   //  ---------------------------------------------------------------------------------------------
   // static analysis transforms  --  helpers that inspect structure of expression
   //  ---------------------------------------------------------------------------------------------

   //   output arity of expression
   //
   //   #out_r + max( 0 , #out_l - in_r )
   //
   //   •——→•——→      •——→•——→       •—→•—→        •—→•—→
   //   •——→•         •——→•——→       •—→•—→        •—→•
   //   •——————→      •——————→           —→           •
   //

   struct output_arity;

   struct input_arity : or_
   <  when
      <  terminal< placeholder<_> >
      ,  placeholder_arity<_value>()
      >
   ,  when
      <  terminal<_>
      ,  mpl::int_<0>()
      >
   ,  when
      <  sequence_operator
      ,  mpl::plus
         <  input_arity(_left)
         ,  mpl::max
            <  mpl::int_<0>
            ,  mpl::minus< input_arity(_right) , output_arity(_left) >
            >
         >()
      >
   ,  when
      <  nary_expr<_, vararg<_>>
      ,  fold<_, mpl::int_<0>(), mpl::max<input_arity, _state>()>
      >
   >
   {};

   struct output_arity : or_
   <  when
      <  channel_operator
      ,  mpl::plus< output_arity(_left) , output_arity(_right) >()
      >
   ,  when
      <  sequence_operator
      ,  mpl::plus
         <  output_arity(_right)
         ,  mpl::max
            <  mpl::int_<0>
            ,  mpl::minus< output_arity(_left) , input_arity(_right) >
            >
         >()
      >
   ,  when
      <  _
      ,  mpl::int_<1>()
      >
   >
   {};

   template< typename Expr >
   using input_arity_t = typename boost::result_of<input_arity(Expr)>::type;

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

   struct place_the_holder;
   struct sequence;
   struct collect_wires;

   struct eval_it : or_
   <  when
      <  terminal< placeholder<_> >
      ,  place_the_holder( _value , _state , _env_var<current_input_t> )
      >
   ,  when
      <  sequence_operator
      ,  sequence( _left , _right , _env_var<current_input_t> )
      >
   ,  when
      <  channel_operator    //  grammar rule: only allowed after sequence or recursion at top of sub-tree
      ,  collect_wires( eval_it(_left), eval_it(_right) )
      >
   ,  when
      <  _
      ,  _default< eval_it >
      >
   >
   {};

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

   struct place_the_holder : callable_decltype
   {
      template< typename I , typename S , typename Tuple >
      auto operator()( placeholder<I> const & , S , Tuple const & args ) const
      {
         return std::get<I::value-1>( args );
      }
   };

   struct sequence : callable_decltype
   {
      template< typename LeftExpr , typename RightExpr , typename Input >
      auto operator()( LeftExpr const & l , RightExpr const & r , Input const & input ) const
      {
         auto e = eval_it{};
         auto left_result = flatten_tuple( std::make_tuple(e( l, mpl::int_<0>{}, ( current_input = input ))));
         return e( r, mpl::int_<0>{}, ( current_input = left_result ) );
      }
   };

   struct collect_wires : callable_decltype
   {
      template< typename Left , typename Right >
      auto operator()( Left const & l , Right const & r ) const
      {
         return std::make_tuple( l, r );
      }
   };

}

using  transforms :: input_arity;
using  transforms :: input_arity_t;
using  transforms :: output_arity;
using  transforms :: eval_it;

//  ------------------------------------------------------------------------------------------------
// compile  --  main function of framework
//              • takes an expression
//              • returns clojure
//  ------------------------------------------------------------------------------------------------

template
<  typename Expression
,  size_t   arity
>
struct stateful_lambda
{
private:

   Expression  expr_;

   template< typename... Args >
   auto call_impl( mpl::int_<0> , Args const &... args ) const -> decltype(auto)
   {
      auto result = eval_it{}( expr_, mpl::int_<0>{}, ( current_input = std::make_tuple(args...) ) );
      return flatten_tuple( std::make_tuple( result ));
   }

   template< int arg_diff , typename... Args >
   auto call_impl( mpl::int_<arg_diff> , Args const &... args ) const
   {
      return [ args...
             , expr = *this ]           // TODO should not be a lambda, but a type that is
      ( auto const &... missing_args )  //      aware of the # of args (enable_if them)
      {
         return expr( args..., missing_args... );
      };
   }

public:

   stateful_lambda( Expression expr ) : expr_( std::move(expr)) {}

   template< typename... Args , typename = std::enable_if_t< sizeof...(Args) <= arity > >
   auto operator()( Args const &... args ) const -> decltype(auto)
   {
      return call_impl( mpl::int_< arity - sizeof...(Args) >{} , args... );
   }
};

auto compile = []( auto expr )        // TODO need to define value_type for state
{
   using expr_t = decltype(expr);
   using arity_t = input_arity_t<expr_t>;

   return stateful_lambda< expr_t, arity_t::value>{ expr };
};

const proto::terminal< placeholder< mpl::int_<1> >>::type   _1  = {{}};
const proto::terminal< placeholder< mpl::int_<2> >>::type   _2  = {{}};
const proto::terminal< placeholder< mpl::int_<3> >>::type   _3  = {{}};
const proto::terminal< placeholder< mpl::int_<4> >>::type   _4  = {{}};
const proto::terminal< placeholder< mpl::int_<5> >>::type   _5  = {{}};

int main()
{
   auto print_ins_and_outs = []( auto const & expr )
   {
       std::cout << "--------------------------" << std::endl;
       //proto::display_expr( expr );
       std::cout << "#ins:  " << input_arity{} (expr) << std::endl;
       std::cout << "#outs: " << output_arity{}(expr) << std::endl;
       std::cout << std::endl;
   };

   auto seq_expr =    ( _2 + _1 , _2 )
                   |= ( _1*_2 - 100 )
                   ;

   print_ins_and_outs( seq_expr );
   auto seq = compile( seq_expr );

   auto t = compile( _1 |= _1 );
   t(1);

   std::cout << std::get<0>(seq(1337,3)) << std::endl;
}


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/fundamental_matrix_util.h"

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <glog/logging.h>

#include "theia/math/polynomial.h"
#include "theia/sfm/pose/util.h"

namespace theia {

using Eigen::DiagonalMatrix;
using Eigen::Map;
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Vector3d;

// Given a fundmental matrix, decompose the fundmental matrix and recover focal
// lengths f1, f2 >0 such that diag([f2 f2 1]) F diag[f1 f1 1]) is a valid
// essential matrix. This assumes a principal point of (0, 0) for both cameras.
// Returns true on success, false otherwise.
bool FocalLengthsFromFundamentalMatrix(const double fmatrix[3 * 3],
                                       double* focal_length1,
                                       double* focal_length2) {
  Map<const Matrix3d> fundamental_matrix(fmatrix);

  // Compute the epipoles for each image.
  const Vector3d epipole1 = fundamental_matrix.jacobiSvd(Eigen::ComputeFullV)
                                .matrixV()
                                .rightCols<1>();
  const Vector3d epipole2 = fundamental_matrix.transpose()
                                .jacobiSvd(Eigen::ComputeFullV)
                                .matrixV()
                                .rightCols<1>();
  if (epipole1.x() == 0 || epipole2.x() == 0) {
    VLOG(3) << "Optical axes are collinear. Cannot recover the focal length.";
    return false;
  }

  // Find the rotation that takes epipole1 to (e_0, 0, e_2) and the
  // epipole2 to (e'_0, 0, e'_2). If we form a rotation matrix:
  // R = [ cos x  -sin x  0 ]
  //     [ sin x   cos x  0 ]
  //     [ 0       0      1 ]
  // then we can solve for the angle x such that R * e1 = (e_1, 0, e_3).
  // We can solve this simply be investigating the second row and noting that
  // e1(0) * sin x + e2 * cos x = 0.
  const double theta1 = atan2(-epipole1(1), epipole1(0));
  const double theta2 = atan2(-epipole2(1), epipole2(0));

  Matrix3d rotation1, rotation2;
  rotation1 << cos(theta1), -sin(theta1), 0, sin(theta1), cos(theta1), 0, 0, 0,
      1;
  rotation2 << cos(theta2), -sin(theta2), 0, sin(theta2), cos(theta2), 0, 0, 0,
      1;

  const Matrix3d rotated_fmatrix =
      rotation2 * fundamental_matrix * rotation1.transpose();
  // With the normalized epipoles, the fundamental matrix is now of the form:
  // F = [ e'_2   0    0   ] [ a b a ] [ e_2   0     0  ]
  //     [ 0      1    0   ] [ c d c ] [ 0     1     0  ]
  //     [ 0      0  -e'_1 ] [ a b a ] [ 0     0   -e_1 ]
  const Vector3d rotated_epipole1 = rotation1 * epipole1;
  const Vector3d rotated_epipole2 = rotation2 * epipole2;

  Matrix3d factorized_matrix =
      DiagonalMatrix<double, 3>(rotated_epipole2(2), 1, -rotated_epipole2(0))
          .inverse() *
      rotated_fmatrix *
      DiagonalMatrix<double, 3>(rotated_epipole1(2), 1, -rotated_epipole1(0))
          .inverse();

  // For convenience, as defined above.
  const double a = factorized_matrix(0, 0);
  const double b = factorized_matrix(0, 1);
  const double c = factorized_matrix(1, 0);
  const double d = factorized_matrix(1, 1);

  const double focal_length1_sq =
      (-a * c * rotated_epipole1(0) * rotated_epipole1(0)) /
      (a * c * rotated_epipole1(2) * rotated_epipole1(2) + b * d);
  const double focal_length2_sq =
      (-a * b * rotated_epipole2(0) * rotated_epipole2(0)) /
      (a * b * rotated_epipole2(2) * rotated_epipole2(2) + c * d);

  if (focal_length1_sq < 0 || focal_length2_sq < 0) {
    VLOG(3) << "Real focal length values could not be extracted.";
    return false;
  }

  *focal_length1 = sqrt(focal_length1_sq);
  *focal_length2 = sqrt(focal_length2_sq);
  return true;
}

// Given a fundamental matrix that relates two cameras with the same intrinsics,
// extract the shared focal length. The method is provided in the article:
//   "On Focal Length Calibration from Two Views" by Peter Sturm (CVPR 2001).
bool SharedFocalLengthsFromFundamentalMatrix(const double fmatrix[3 * 3],
                                             double* focal_length) {
  // Construct a "semi-calibrated" matrix G with all intrinsics but focal length
  // accounted for. In our case, the fundamental matrix is equivalent to G
  // because we assume the fundmental matrix was computed with all the known
  // intrinsics effects removed.
  Map<const Matrix3d> G(fmatrix);

  // Compute the SVD of the semi-calibrated fundamental matrix.
  Eigen::JacobiSVD<Eigen::Matrix3d> svd(
      G, Eigen::ComputeFullU | Eigen::ComputeFullV);
  const Eigen::Matrix3d& U = svd.matrixU();
  const Eigen::Matrix3d& V = svd.matrixV();
  const double a = svd.singularValues()(0);
  const double b = svd.singularValues()(1);

  // Construct the quadratic equation that reveals the focal length.
  const double U20 = U(2, 0);
  const double U21 = U(2, 1);
  const double V20 = V(2, 0);
  const double V21 = V(2, 1);
  const double U20_sq = U20 * U20;
  const double U21_sq = U21 * U21;
  const double V20_sq = V20 * V20;
  const double V21_sq = V21 * V21;
  Eigen::VectorXd coeffs(3);
  coeffs(0) = a * a * (1.0 - U20_sq) * (1.0 - V20_sq) -
              b * b * (1.0 - U21_sq) * (1.0 - V21_sq);
  coeffs(1) = a * a * (U20_sq + V20_sq - 2.0 * U20_sq * V20_sq) -
              b * b * (U21_sq + V21_sq - 2.0 * U21_sq * V21_sq);
  coeffs(2) = a * a * U20_sq * V20_sq - b * b * U21_sq * V21_sq;

  // Solve the quadratic equation. The roots provide the square of the focal
  // length.
  Eigen::VectorXd real_roots, imaginary_roots;
  if (!FindPolynomialRoots(coeffs, &real_roots, &imaginary_roots)) {
    return false;
  }

  // If niether root is positive then no valid solution exists. If one of the
  // roots is negative then it leads to an imaginary value for the focal length
  // so we can immediately return the other value as the solution.
  if (real_roots(0) < 0 && real_roots(1) < 0) {
    return false;
  } else if (real_roots(0) < 0) {
    *focal_length = std::sqrt(real_roots(1));
  } else if (real_roots(1) < 0) {
    *focal_length = std::sqrt(real_roots(0));
  } else {
    // If we reach this point then the roots are both positive and so we
    // disambiguate the roots by selecting the root that best satisfies the
    // linear constraint.
    const double c1 =
        a * U20 * U21 * (1.0 - V20_sq) + b * V20 * V21 * (1.0 - U21_sq);
    const double c2 = U21 * V20 * (a * U20 * V20 + b * U21 * V21);
    const double root1_val = c1 * real_roots(0) + c2;
    const double root2_val = c1 * real_roots(1) + c2;
    if (std::abs(root1_val) < std::abs(root2_val)) {
      *focal_length = std::sqrt(real_roots(0));
    } else {
      *focal_length = std::sqrt(real_roots(1));
    }
  }

  return true;
}

void ProjectionMatricesFromFundamentalMatrix(const double fmatrix[3 * 3],
                                             double pmatrix1[3 * 4],
                                             double pmatrix2[3 * 4]) {
  Map<const Matrix3d> fmatrix_map(fmatrix);
  const Vector3d right_epipole =
      fmatrix_map.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();

  Map<Matrix3d>(pmatrix1, 3, 3) = Matrix3d::Identity();
  Map<Vector3d>(pmatrix1 + 9, 3) = Vector3d::Zero();

  Map<Matrix3d>(pmatrix2, 3, 3) =
      CrossProductMatrix(right_epipole) * fmatrix_map.transpose();
  Map<Vector3d>(pmatrix2 + 9, 3) = right_epipole;
}

// Ported from Hartley and Zisserman:
// http://www.robots.ox.ac.uk/~vgg/hzbook/code/vgg_multiview/vgg_F_from_P.m
void FundamentalMatrixFromProjectionMatrices(const double pmatrix1[3 * 4],
                                             const double pmatrix2[3 * 4],
                                             double fmatrix[3 * 3]) {
  Map<const Matrix<double, 3, 4> > projection1(pmatrix1);
  Map<const Matrix<double, 3, 4> > projection2(pmatrix2);
  Map<Matrix3d> fundamental_matrix(fmatrix);

  const int index1[3] = {1, 2, 0};
  const int index2[3] = {2, 0, 1};
  Eigen::Matrix4d temp_mat;
  for (int r = 0; r < 3; r++) {
    temp_mat.row(2) = projection1.row(index1[r]);
    temp_mat.row(3) = projection1.row(index2[r]);
    for (int c = 0; c < 3; c++) {
      temp_mat.row(0) = projection2.row(index1[c]);
      temp_mat.row(1) = projection2.row(index2[c]);
      fundamental_matrix(r, c) = temp_mat.determinant();
    }
  }
}

void EssentialMatrixFromFundamentalMatrix(const double fmatrix[3 * 3],
                                          const double focal_length1,
                                          const double focal_length2,
                                          double ematrix[3 * 3]) {
  const Eigen::Map<const Matrix3d> fundamental_matrix(fmatrix);
  Eigen::Map<Matrix3d> essential_matrix(ematrix);
  essential_matrix =
      Eigen::DiagonalMatrix<double, 3>(focal_length2, focal_length2, 1.0) *
      fundamental_matrix *
      Eigen::DiagonalMatrix<double, 3>(focal_length1, focal_length1, 1.0);
}

void ComposeFundamentalMatrix(const double focal_length1,
                              const double focal_length2,
                              const double rotation[3 * 3],
                              const double translation[3],
                              double fmatrix[3 * 3]) {
  const Matrix3d calibration_inv1 =
      Eigen::DiagonalMatrix<double, 3>(focal_length1, focal_length1, 1.0)
          .inverse();
  const Matrix3d calibration_inv2 =
      Eigen::DiagonalMatrix<double, 3>(focal_length2, focal_length2, 1.0)
          .inverse();

  const Eigen::Map<const Eigen::Matrix3d> rot(rotation);
  const Eigen::Map<const Eigen::Vector3d> trans(translation);

  Eigen::Map<Eigen::Matrix3d> fundamental_matrix(fmatrix);
  fundamental_matrix =
      calibration_inv2 * CrossProductMatrix(trans) * rot * calibration_inv1;
}

}  // namespace theia


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, University of Notre Dame, Notre
// Dame, IN 46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#ifndef BOOST_GRAPH_SEQUENTIAL_VERTEX_COLORING_HPP
#define BOOST_GRAPH_SEQUENTIAL_VERTEX_COLORING_HPP

#include <vector>
#include <boost/graph/graph_traits.hpp>

/* This algorithm is to find coloring of a graph

   Algorithm:
   Let G = (V,E) be a graph with vertices (somehow) ordered v_1, v_2, ...,
   v_n. For k = 1, 2, ..., n the sequential algorithm assigns v_k to the
   smallest possible color.

   Reference:

   Thomas F. Coleman and Jorge J. More, Estimation of sparse Jacobian
   matrices and graph coloring problems. J. Numer. Anal. V20, P187-209, 1983

   v_k is stored as o[k] here.

   The color of the vertex v will be stored in color[v].
   i.e., vertex v belongs to coloring color[v] */

namespace boost {
  template <class VertexListGraph, class OrderPA, class ColorMap>
  typename graph_traits<VertexListGraph>::size_type
  sequential_vertex_coloring(const VertexListGraph& G, OrderPA order,
                             ColorMap color)
  {
    using graph_traits;
    using boost::tie;
    typedef graph_traits<VertexListGraph> GraphTraits;
    typedef typename GraphTraits::vertex_descriptor Vertex;
    typedef typename GraphTraits::size_type size_type;

    size_type max_color = 0;
    const size_type V = num_vertices(G);

    // We need to keep track of which colors are used by
    // adjacent vertices. We do this by marking the colors
    // that are used. The mark array contains the mark
    // for each color. The length of mark is the
    // number of vertices since the maximum possible number of colors
    // is the number of vertices.
    std::vector<size_type> mark(V, numeric_limits_max(max_color));

    //Initialize colors
    typename GraphTraits::vertex_iterator v, vend;
    for (tie(v, vend) = vertices(G); v != vend; ++v)
      put(color, *v, V-1);

    //Determine the color for every vertex one by one
    for ( size_type i = 0; i < V; i++) {
      Vertex current = get(order,i);
      typename GraphTraits::adjacency_iterator v, vend;

      //Mark the colors of vertices adjacent to current.
      //i can be the value for marking since i increases successively
      for (tie(v,vend) = adjacent_vertices(current, G); v != vend; ++v)
        mark[get(color,*v)] = i;

      //Next step is to assign the smallest un-marked color
      //to the current vertex.
      size_type j = 0;

      //Scan through all useable colors, find the smallest possible
      //color which is not used by neighbors.  Note that if mark[j]
      //is equal to i, color j is used by one of the current vertex's
      //neighbors.
      while ( j < max_color && mark[j] == i )
        ++j;

      if ( j == max_color )  //All colors are used up. Add one more color
        ++max_color;

      //At this point, j is the smallest possible color
      put(color, current, j);  //Save the color of vertex current

    }

    return max_color;
  }
}

#endif


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
*
*   Benchmark:  Iterative solver tests (solver.cpp and solver.cu are identical, the latter being required for compilation using CUDA nvcc)
*
*/

#ifndef NDEBUG
 #define NDEBUG
#endif

#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

#define VIENNACL_WITH_UBLAS 1

#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/coordinate_matrix.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/ell_matrix.hpp"
#include "viennacl/sliced_ell_matrix.hpp"
#include "viennacl/hyb_matrix.hpp"
#include "viennacl/context.hpp"

#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/gmres.hpp"

#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/ichol.hpp"
#include "viennacl/linalg/jacobi_precond.hpp"
#include "viennacl/linalg/row_scaling.hpp"

#ifdef VIENNACL_WITH_OPENCL
  #include "viennacl/linalg/mixed_precision_cg.hpp"
#endif

#include "viennacl/io/matrix_market.hpp"

#include <iostream>
#include <vector>
#include "benchmark-utils.hpp"

using namespace boost::numeric;

#define BENCHMARK_RUNS          1

template<typename ScalarType>
ScalarType diff_inf(ublas::vector<ScalarType> & v1, viennacl::vector<ScalarType> & v2)
{
   ublas::vector<ScalarType> v2_cpu(v2.size());
   viennacl::copy(v2.begin(), v2.end(), v2_cpu.begin());

   for (unsigned int i=0;i<v1.size(); ++i)
   {
      if ( std::max( fabs(v2_cpu[i]), fabs(v1[i]) ) > 0 )
         v2_cpu[i] = fabs(v2_cpu[i] - v1[i]) / std::max( fabs(v2_cpu[i]), fabs(v1[i]) );
      else
         v2_cpu[i] = 0.0;
   }

   return norm_inf(v2_cpu);
}

template<typename ScalarType>
ScalarType diff_2(ublas::vector<ScalarType> & v1, viennacl::vector<ScalarType> & v2)
{
   ublas::vector<ScalarType> v2_cpu(v2.size());
   viennacl::copy(v2.begin(), v2.end(), v2_cpu.begin());

   return norm_2(v1 - v2_cpu) / norm_2(v1);
}

template<typename MatrixType, typename VectorType, typename SolverTag, typename PrecondTag>
void run_solver(MatrixType const & matrix, VectorType const & rhs, VectorType const & ref_result, SolverTag const & solver, PrecondTag const & precond, long ops)
{
  Timer timer;
  VectorType result(rhs);
  VectorType residual(rhs);
  viennacl::backend::finish();

  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
  {
    result = viennacl::linalg::solve(matrix, rhs, solver, precond);
  }
  viennacl::backend::finish();
  double exec_time = timer.get();
  std::cout << "Exec. time: " << exec_time << std::endl;
  std::cout << "Est. "; printOps(static_cast<double>(ops), exec_time / BENCHMARK_RUNS);
  residual -= viennacl::linalg::prod(matrix, result);
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(rhs) << std::endl;
  std::cout << "Estimated rel. residual: " << solver.error() << std::endl;
  std::cout << "Iterations: " << solver.iters() << std::endl;
  result -= ref_result;
  std::cout << "Relative deviation from result: " << viennacl::linalg::norm_2(result) / viennacl::linalg::norm_2(ref_result) << std::endl;
}

template<typename ScalarType>
int run_benchmark(viennacl::context ctx)
{
  Timer timer;
  double exec_time;

  ScalarType std_factor1 = static_cast<ScalarType>(3.1415);
  ScalarType std_factor2 = static_cast<ScalarType>(42.0);
  viennacl::scalar<ScalarType> vcl_factor1(std_factor1, ctx);
  viennacl::scalar<ScalarType> vcl_factor2(std_factor2, ctx);

  ublas::vector<ScalarType> ublas_vec1;
  ublas::vector<ScalarType> ublas_vec2;
  ublas::vector<ScalarType> ublas_result;
  unsigned int solver_iters = 100;
  unsigned int solver_krylov_dim = 20;
  double solver_tolerance = 1e-6;

  ublas::compressed_matrix<ScalarType> ublas_matrix;
  if (!viennacl::io::read_matrix_market_file(ublas_matrix, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return EXIT_FAILURE;
  }
  //unsigned int cg_mat_size = cg_mat.size();
  std::cout << "done reading matrix" << std::endl;

  ublas_result = ublas::scalar_vector<ScalarType>(ublas_matrix.size1(), ScalarType(1.0));
  ublas_vec1 = ublas::prod(ublas_matrix, ublas_result);
  ublas_vec2 = ublas_vec1;

  viennacl::compressed_matrix<ScalarType> vcl_compressed_matrix(ublas_vec1.size(), ublas_vec1.size(), ctx);
  viennacl::coordinate_matrix<ScalarType> vcl_coordinate_matrix(ublas_vec1.size(), ublas_vec1.size(), ctx);
  viennacl::ell_matrix<ScalarType> vcl_ell_matrix(ctx);
  viennacl::sliced_ell_matrix<ScalarType> vcl_sliced_ell_matrix(ctx);
  viennacl::hyb_matrix<ScalarType> vcl_hyb_matrix(ctx);

  viennacl::vector<ScalarType> vcl_vec1(ublas_vec1.size(), ctx);
  viennacl::vector<ScalarType> vcl_vec2(ublas_vec1.size(), ctx);
  viennacl::vector<ScalarType> vcl_result(ublas_vec1.size(), ctx);

  //cpu to gpu:
  viennacl::copy(ublas_matrix, vcl_compressed_matrix);
  viennacl::copy(ublas_matrix, vcl_coordinate_matrix);
  viennacl::copy(ublas_matrix, vcl_ell_matrix);
  viennacl::copy(ublas_matrix, vcl_sliced_ell_matrix);
  viennacl::copy(ublas_matrix, vcl_hyb_matrix);
  viennacl::copy(ublas_vec1, vcl_vec1);
  viennacl::copy(ublas_vec2, vcl_vec2);
  viennacl::copy(ublas_result, vcl_result);

  std::cout << "------- Jacobi preconditioner ----------" << std::endl;
  viennacl::linalg::jacobi_precond< ublas::compressed_matrix<ScalarType> >    ublas_jacobi(ublas_matrix, viennacl::linalg::jacobi_tag());
  viennacl::linalg::jacobi_precond< viennacl::compressed_matrix<ScalarType> > vcl_jacobi_csr(vcl_compressed_matrix, viennacl::linalg::jacobi_tag());
  viennacl::linalg::jacobi_precond< viennacl::coordinate_matrix<ScalarType> > vcl_jacobi_coo(vcl_coordinate_matrix, viennacl::linalg::jacobi_tag());

  std::cout << "------- Row-Scaling preconditioner ----------" << std::endl;
  viennacl::linalg::row_scaling< ublas::compressed_matrix<ScalarType> >    ublas_row_scaling(ublas_matrix, viennacl::linalg::row_scaling_tag(1));
  viennacl::linalg::row_scaling< viennacl::compressed_matrix<ScalarType> > vcl_row_scaling_csr(vcl_compressed_matrix, viennacl::linalg::row_scaling_tag(1));
  viennacl::linalg::row_scaling< viennacl::coordinate_matrix<ScalarType> > vcl_row_scaling_coo(vcl_coordinate_matrix, viennacl::linalg::row_scaling_tag(1));

  ///////////////////////////////////////////////////////////////////////////////
  //////////////////////  Incomplete Cholesky preconditioner   //////////////////
  ///////////////////////////////////////////////////////////////////////////////
  std::cout << "------- ICHOL0 on CPU (ublas) ----------" << std::endl;

  timer.start();
  viennacl::linalg::ichol0_precond< ublas::compressed_matrix<ScalarType> >    ublas_ichol0(ublas_matrix, viennacl::linalg::ichol0_tag());
  exec_time = timer.get();
  std::cout << "Setup time: " << exec_time << std::endl;

  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
    ublas_ichol0.apply(ublas_vec1);
  exec_time = timer.get();
  std::cout << "ublas time: " << exec_time << std::endl;

  std::cout << "------- ICHOL0 with ViennaCL ----------" << std::endl;

  timer.start();
  viennacl::linalg::ichol0_precond< viennacl::compressed_matrix<ScalarType> > vcl_ichol0(vcl_compressed_matrix, viennacl::linalg::ichol0_tag());
  exec_time = timer.get();
  std::cout << "Setup time: " << exec_time << std::endl;

  viennacl::backend::finish();
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
    vcl_ichol0.apply(vcl_vec1);
  viennacl::backend::finish();
  exec_time = timer.get();
  std::cout << "ViennaCL time: " << exec_time << std::endl;

  ///////////////////////////////////////////////////////////////////////////////
  //////////////////////           ILU preconditioner         //////////////////
  ///////////////////////////////////////////////////////////////////////////////
  std::cout << "------- ILU0 on with ublas ----------" << std::endl;

  timer.start();
  viennacl::linalg::ilu0_precond< ublas::compressed_matrix<ScalarType> >    ublas_ilu0(ublas_matrix, viennacl::linalg::ilu0_tag());
  exec_time = timer.get();
  std::cout << "Setup time (no level scheduling): " << exec_time << std::endl;
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
    ublas_ilu0.apply(ublas_vec1);
  exec_time = timer.get();
  std::cout << "ublas ILU0 substitution time (no level scheduling): " << exec_time << std::endl;

  std::cout << "------- ILU0 with ViennaCL ----------" << std::endl;

  timer.start();
  viennacl::linalg::ilu0_precond< viennacl::compressed_matrix<ScalarType> > vcl_ilu0(vcl_compressed_matrix, viennacl::linalg::ilu0_tag());
  exec_time = timer.get();
  std::cout << "Setup time (no level scheduling): " << exec_time << std::endl;

  viennacl::backend::finish();
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
    vcl_ilu0.apply(vcl_vec1);
  viennacl::backend::finish();
  exec_time = timer.get();
  std::cout << "ViennaCL ILU0 substitution time (no level scheduling): " << exec_time << std::endl;

  timer.start();
  viennacl::linalg::ilu0_tag ilu0_with_level_scheduling; ilu0_with_level_scheduling.use_level_scheduling(true);
  viennacl::linalg::ilu0_precond< viennacl::compressed_matrix<ScalarType> > vcl_ilu0_level_scheduling(vcl_compressed_matrix, ilu0_with_level_scheduling);
  exec_time = timer.get();
  std::cout << "Setup time (with level scheduling): " << exec_time << std::endl;

  viennacl::backend::finish();
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
    vcl_ilu0_level_scheduling.apply(vcl_vec1);
  viennacl::backend::finish();
  exec_time = timer.get();
  std::cout << "ViennaCL ILU0 substitution time (with level scheduling): " << exec_time << std::endl;

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

  std::cout << "------- Block-ILU0 with ublas ----------" << std::endl;

  ublas_vec1 = ublas_vec2;
  viennacl::copy(ublas_vec1, vcl_vec1);

  timer.start();
  viennacl::linalg::block_ilu_precond< ublas::compressed_matrix<ScalarType>,
                                       viennacl::linalg::ilu0_tag>          ublas_block_ilu0(ublas_matrix, viennacl::linalg::ilu0_tag());
  exec_time = timer.get();
  std::cout << "Setup time: " << exec_time << std::endl;

  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
    ublas_block_ilu0.apply(ublas_vec1);
  exec_time = timer.get();
  std::cout << "ublas time: " << exec_time << std::endl;

  std::cout << "------- Block-ILU0 with ViennaCL ----------" << std::endl;

  timer.start();
  viennacl::linalg::block_ilu_precond< viennacl::compressed_matrix<ScalarType>,
                                       viennacl::linalg::ilu0_tag>          vcl_block_ilu0(vcl_compressed_matrix, viennacl::linalg::ilu0_tag());
  exec_time = timer.get();
  std::cout << "Setup time: " << exec_time << std::endl;

  //vcl_block_ilu0.apply(vcl_vec1);  //warm-up
  viennacl::backend::finish();
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
    vcl_block_ilu0.apply(vcl_vec1);
  viennacl::backend::finish();
  exec_time = timer.get();
  std::cout << "ViennaCL time: " << exec_time << std::endl;

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

  std::cout << "------- ILUT with ublas ----------" << std::endl;

  ublas_vec1 = ublas_vec2;
  viennacl::copy(ublas_vec1, vcl_vec1);

  timer.start();
  viennacl::linalg::ilut_precond< ublas::compressed_matrix<ScalarType> >    ublas_ilut(ublas_matrix, viennacl::linalg::ilut_tag());
  exec_time = timer.get();
  std::cout << "Setup time (no level scheduling): " << exec_time << std::endl;
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
    ublas_ilut.apply(ublas_vec1);
  exec_time = timer.get();
  std::cout << "ublas ILUT substitution time (no level scheduling): " << exec_time << std::endl;

  std::cout << "------- ILUT with ViennaCL ----------" << std::endl;

  timer.start();
  viennacl::linalg::ilut_precond< viennacl::compressed_matrix<ScalarType> > vcl_ilut(vcl_compressed_matrix, viennacl::linalg::ilut_tag());
  exec_time = timer.get();
  std::cout << "Setup time (no level scheduling): " << exec_time << std::endl;

  viennacl::backend::finish();
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
    vcl_ilut.apply(vcl_vec1);
  viennacl::backend::finish();
  exec_time = timer.get();
  std::cout << "ViennaCL ILUT substitution time (no level scheduling): " << exec_time << std::endl;

  timer.start();
  viennacl::linalg::ilut_tag ilut_with_level_scheduling; ilut_with_level_scheduling.use_level_scheduling(true);
  viennacl::linalg::ilut_precond< viennacl::compressed_matrix<ScalarType> > vcl_ilut_level_scheduling(vcl_compressed_matrix, ilut_with_level_scheduling);
  exec_time = timer.get();
  std::cout << "Setup time (with level scheduling): " << exec_time << std::endl;

  viennacl::backend::finish();
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
    vcl_ilut_level_scheduling.apply(vcl_vec1);
  viennacl::backend::finish();
  exec_time = timer.get();
  std::cout << "ViennaCL ILUT substitution time (with level scheduling): " << exec_time << std::endl;

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

  std::cout << "------- Block-ILUT with ublas ----------" << std::endl;

  ublas_vec1 = ublas_vec2;
  viennacl::copy(ublas_vec1, vcl_vec1);

  timer.start();
  viennacl::linalg::block_ilu_precond< ublas::compressed_matrix<ScalarType>,
                                       viennacl::linalg::ilut_tag>          ublas_block_ilut(ublas_matrix, viennacl::linalg::ilut_tag());
  exec_time = timer.get();
  std::cout << "Setup time: " << exec_time << std::endl;

  //ublas_block_ilut.apply(ublas_vec1);
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
    ublas_block_ilut.apply(ublas_vec1);
  exec_time = timer.get();
  std::cout << "ublas time: " << exec_time << std::endl;

  std::cout << "------- Block-ILUT with ViennaCL ----------" << std::endl;

  timer.start();
  viennacl::linalg::block_ilu_precond< viennacl::compressed_matrix<ScalarType>,
                                       viennacl::linalg::ilut_tag>          vcl_block_ilut(vcl_compressed_matrix, viennacl::linalg::ilut_tag());
  exec_time = timer.get();
  std::cout << "Setup time: " << exec_time << std::endl;

  //vcl_block_ilut.apply(vcl_vec1);  //warm-up
  viennacl::backend::finish();
  timer.start();
  for (int runs=0; runs<BENCHMARK_RUNS; ++runs)
    vcl_block_ilut.apply(vcl_vec1);
  viennacl::backend::finish();
  exec_time = timer.get();
  std::cout << "ViennaCL time: " << exec_time << std::endl;

  ///////////////////////////////////////////////////////////////////////////////
  //////////////////////              CG solver                //////////////////
  ///////////////////////////////////////////////////////////////////////////////
  long cg_ops = static_cast<long>(solver_iters * (ublas_matrix.nnz() + 6 * ublas_vec2.size()));

  viennacl::linalg::cg_tag cg_solver(solver_tolerance, solver_iters);

  std::cout << "------- CG solver (no preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, cg_solver, viennacl::linalg::no_precond(), cg_ops);

  std::cout << "------- CG solver (no preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, cg_solver, viennacl::linalg::no_precond(), cg_ops);

#ifdef VIENNACL_WITH_OPENCL
  bool is_double = (sizeof(ScalarType) == sizeof(double));
  if (is_double)
  {
    std::cout << "------- CG solver, mixed precision (no preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
    viennacl::linalg::mixed_precision_cg_tag mixed_precision_cg_solver(solver_tolerance, solver_iters);

    run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, mixed_precision_cg_solver, viennacl::linalg::no_precond(), cg_ops);
    run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, mixed_precision_cg_solver, viennacl::linalg::no_precond(), cg_ops);
  }
#endif

  std::cout << "------- CG solver (no preconditioner) via ViennaCL, coordinate_matrix ----------" << std::endl;
  run_solver(vcl_coordinate_matrix, vcl_vec2, vcl_result, cg_solver, viennacl::linalg::no_precond(), cg_ops);

  std::cout << "------- CG solver (no preconditioner) via ViennaCL, ell_matrix ----------" << std::endl;
  run_solver(vcl_ell_matrix, vcl_vec2, vcl_result, cg_solver, viennacl::linalg::no_precond(), cg_ops);

  std::cout << "------- CG solver (no preconditioner) via ViennaCL, sliced_ell_matrix ----------" << std::endl;
  run_solver(vcl_sliced_ell_matrix, vcl_vec2, vcl_result, cg_solver, viennacl::linalg::no_precond(), cg_ops);

  std::cout << "------- CG solver (no preconditioner) via ViennaCL, hyb_matrix ----------" << std::endl;
  run_solver(vcl_hyb_matrix, vcl_vec2, vcl_result, cg_solver, viennacl::linalg::no_precond(), cg_ops);

  std::cout << "------- CG solver (ICHOL0 preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, cg_solver, ublas_ichol0, cg_ops);

  std::cout << "------- CG solver (ICHOL0 preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, cg_solver, vcl_ichol0, cg_ops);

  std::cout << "------- CG solver (ILU0 preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, cg_solver, ublas_ilu0, cg_ops);

  std::cout << "------- CG solver (ILU0 preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, cg_solver, vcl_ilu0, cg_ops);

  std::cout << "------- CG solver (Block-ILU0 preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, cg_solver, ublas_block_ilu0, cg_ops);

  std::cout << "------- CG solver (Block-ILU0 preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, cg_solver, vcl_block_ilu0, cg_ops);

  std::cout << "------- CG solver (ILUT preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, cg_solver, ublas_ilut, cg_ops);

  std::cout << "------- CG solver (ILUT preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, cg_solver, vcl_ilut, cg_ops);

  std::cout << "------- CG solver (ILUT preconditioner) via ViennaCL, coordinate_matrix ----------" << std::endl;
  run_solver(vcl_coordinate_matrix, vcl_vec2, vcl_result, cg_solver, vcl_ilut, cg_ops);

  std::cout << "------- CG solver (Block-ILUT preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, cg_solver, ublas_block_ilut, cg_ops);

  std::cout << "------- CG solver (Block-ILUT preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, cg_solver, vcl_block_ilut, cg_ops);

  std::cout << "------- CG solver (Jacobi preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, cg_solver, ublas_jacobi, cg_ops);

  std::cout << "------- CG solver (Jacobi preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, cg_solver, vcl_jacobi_csr, cg_ops);

  std::cout << "------- CG solver (Jacobi preconditioner) via ViennaCL, coordinate_matrix ----------" << std::endl;
  run_solver(vcl_coordinate_matrix, vcl_vec2, vcl_result, cg_solver, vcl_jacobi_coo, cg_ops);

  std::cout << "------- CG solver (row scaling preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, cg_solver, ublas_row_scaling, cg_ops);

  std::cout << "------- CG solver (row scaling preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, cg_solver, vcl_row_scaling_csr, cg_ops);

  std::cout << "------- CG solver (row scaling preconditioner) via ViennaCL, coordinate_matrix ----------" << std::endl;
  run_solver(vcl_coordinate_matrix, vcl_vec2, vcl_result, cg_solver, vcl_row_scaling_coo, cg_ops);

  ///////////////////////////////////////////////////////////////////////////////
  //////////////////////           BiCGStab solver             //////////////////
  ///////////////////////////////////////////////////////////////////////////////

  long bicgstab_ops = static_cast<long>(solver_iters * (2 * ublas_matrix.nnz() + 13 * ublas_vec2.size()));

  viennacl::linalg::bicgstab_tag bicgstab_solver(solver_tolerance, solver_iters);

  std::cout << "------- BiCGStab solver (no preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, bicgstab_solver, viennacl::linalg::no_precond(), bicgstab_ops);

  std::cout << "------- BiCGStab solver (no preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, bicgstab_solver, viennacl::linalg::no_precond(), bicgstab_ops);

  std::cout << "------- BiCGStab solver (no preconditioner) via ViennaCL, coordinate_matrix ----------" << std::endl;
  run_solver(vcl_coordinate_matrix, vcl_vec2, vcl_result, bicgstab_solver, viennacl::linalg::no_precond(), bicgstab_ops);

  std::cout << "------- BiCGStab solver (no preconditioner) via ViennaCL, ell_matrix ----------" << std::endl;
  run_solver(vcl_ell_matrix, vcl_vec2, vcl_result, bicgstab_solver, viennacl::linalg::no_precond(), bicgstab_ops);

  std::cout << "------- BiCGStab solver (no preconditioner) via ViennaCL, sliced_ell_matrix ----------" << std::endl;
  run_solver(vcl_sliced_ell_matrix, vcl_vec2, vcl_result, bicgstab_solver, viennacl::linalg::no_precond(), bicgstab_ops);

  std::cout << "------- BiCGStab solver (no preconditioner) via ViennaCL, hyb_matrix ----------" << std::endl;
  run_solver(vcl_hyb_matrix, vcl_vec2, vcl_result, bicgstab_solver, viennacl::linalg::no_precond(), bicgstab_ops);

  std::cout << "------- BiCGStab solver (ILUT preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, bicgstab_solver, ublas_ilut, bicgstab_ops);

  std::cout << "------- BiCGStab solver (ILUT preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, bicgstab_solver, vcl_ilut, bicgstab_ops);

  std::cout << "------- BiCGStab solver (Block-ILUT preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, bicgstab_solver, ublas_block_ilut, bicgstab_ops);

#ifdef VIENNACL_WITH_OPENCL
  std::cout << "------- BiCGStab solver (Block-ILUT preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, bicgstab_solver, vcl_block_ilut, bicgstab_ops);
#endif

//  std::cout << "------- BiCGStab solver (ILUT preconditioner) via ViennaCL, coordinate_matrix ----------" << std::endl;
//  run_solver(vcl_coordinate_matrix, vcl_vec2, vcl_result, bicgstab_solver, vcl_ilut, bicgstab_ops);

  std::cout << "------- BiCGStab solver (Jacobi preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, bicgstab_solver, ublas_jacobi, bicgstab_ops);

  std::cout << "------- BiCGStab solver (Jacobi preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, bicgstab_solver, vcl_jacobi_csr, bicgstab_ops);

  std::cout << "------- BiCGStab solver (Jacobi preconditioner) via ViennaCL, coordinate_matrix ----------" << std::endl;
  run_solver(vcl_coordinate_matrix, vcl_vec2, vcl_result, bicgstab_solver, vcl_jacobi_coo, bicgstab_ops);

  std::cout << "------- BiCGStab solver (row scaling preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, bicgstab_solver, ublas_row_scaling, bicgstab_ops);

  std::cout << "------- BiCGStab solver (row scaling preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, bicgstab_solver, vcl_row_scaling_csr, bicgstab_ops);

  std::cout << "------- BiCGStab solver (row scaling preconditioner) via ViennaCL, coordinate_matrix ----------" << std::endl;
  run_solver(vcl_coordinate_matrix, vcl_vec2, vcl_result, bicgstab_solver, vcl_row_scaling_coo, bicgstab_ops);

  ///////////////////////////////////////////////////////////////////////////////
  ///////////////////////            GMRES solver             ///////////////////
  ///////////////////////////////////////////////////////////////////////////////

  long gmres_ops = static_cast<long>(solver_iters * (ublas_matrix.nnz() + (solver_iters * 2 + 7) * ublas_vec2.size()));

  viennacl::linalg::gmres_tag gmres_solver(solver_tolerance, solver_iters, solver_krylov_dim);

  std::cout << "------- GMRES solver (no preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, gmres_solver, viennacl::linalg::no_precond(), gmres_ops);

  std::cout << "------- GMRES solver (no preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, gmres_solver, viennacl::linalg::no_precond(), gmres_ops);

  std::cout << "------- GMRES solver (no preconditioner) on GPU, coordinate_matrix ----------" << std::endl;
  run_solver(vcl_coordinate_matrix, vcl_vec2, vcl_result, gmres_solver, viennacl::linalg::no_precond(), gmres_ops);

  std::cout << "------- GMRES solver (no preconditioner) on GPU, ell_matrix ----------" << std::endl;
  run_solver(vcl_ell_matrix, vcl_vec2, vcl_result, gmres_solver, viennacl::linalg::no_precond(), gmres_ops);

  std::cout << "------- GMRES solver (no preconditioner) on GPU, sliced_ell_matrix ----------" << std::endl;
  run_solver(vcl_sliced_ell_matrix, vcl_vec2, vcl_result, gmres_solver, viennacl::linalg::no_precond(), gmres_ops);

  std::cout << "------- GMRES solver (no preconditioner) on GPU, hyb_matrix ----------" << std::endl;
  run_solver(vcl_hyb_matrix, vcl_vec2, vcl_result, gmres_solver, viennacl::linalg::no_precond(), gmres_ops);

  std::cout << "------- GMRES solver (ILUT preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, gmres_solver, ublas_ilut, gmres_ops);

  std::cout << "------- GMRES solver (ILUT preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, gmres_solver, vcl_ilut, gmres_ops);

  std::cout << "------- GMRES solver (ILUT preconditioner) via ViennaCL, coordinate_matrix ----------" << std::endl;
  run_solver(vcl_coordinate_matrix, vcl_vec2, vcl_result, gmres_solver, vcl_ilut, gmres_ops);

  std::cout << "------- GMRES solver (Jacobi preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, gmres_solver, ublas_jacobi, gmres_ops);

  std::cout << "------- GMRES solver (Jacobi preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, gmres_solver, vcl_jacobi_csr, gmres_ops);

  std::cout << "------- GMRES solver (Jacobi preconditioner) via ViennaCL, coordinate_matrix ----------" << std::endl;
  run_solver(vcl_coordinate_matrix, vcl_vec2, vcl_result, gmres_solver, vcl_jacobi_coo, gmres_ops);

  std::cout << "------- GMRES solver (row scaling preconditioner) using ublas ----------" << std::endl;
  run_solver(ublas_matrix, ublas_vec2, ublas_result, gmres_solver, ublas_row_scaling, gmres_ops);

  std::cout << "------- GMRES solver (row scaling preconditioner) via ViennaCL, compressed_matrix ----------" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec2, vcl_result, gmres_solver, vcl_row_scaling_csr, gmres_ops);

  std::cout << "------- GMRES solver (row scaling preconditioner) via ViennaCL, coordinate_matrix ----------" << std::endl;
  run_solver(vcl_coordinate_matrix, vcl_vec2, vcl_result, gmres_solver, vcl_row_scaling_coo, gmres_ops);

  return EXIT_SUCCESS;
}

int main()
{
  std::cout << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "               Device Info" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;

#ifdef VIENNACL_WITH_OPENCL
  viennacl::ocl::platform pf = viennacl::ocl::get_platforms()[0];
  std::vector<viennacl::ocl::device> const & devices = pf.devices();

  // Set first device to first context:
  viennacl::ocl::setup_context(0, devices[0]);

  // Set second device for second context (use the same device for the second context if only one device available):
  if (devices.size() > 1)
    viennacl::ocl::setup_context(1, devices[1]);
  else
    viennacl::ocl::setup_context(1, devices[0]);

  std::cout << viennacl::ocl::current_device().info() << std::endl;
  viennacl::context ctx(viennacl::ocl::get_context(1));
#else
  viennacl::context ctx;
#endif

  std::cout << "---------------------------------------------------------------------------" << std::endl;
  std::cout << "---------------------------------------------------------------------------" << std::endl;
  std::cout << " Benchmark for Execution Times of Iterative Solvers provided with ViennaCL " << std::endl;
  std::cout << "---------------------------------------------------------------------------" << std::endl;
  std::cout << " Note that the purpose of this benchmark is not to run solvers until" << std::endl;
  std::cout << " convergence. Instead, only the execution times of a few iterations are" << std::endl;
  std::cout << " recorded. Residual errors are only printed for information." << std::endl << std::endl;

  std::cout << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "## Benchmark :: Solver" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << std::endl;
  std::cout << "   -------------------------------" << std::endl;
  std::cout << "   # benchmarking single-precision" << std::endl;
  std::cout << "   -------------------------------" << std::endl;
  run_benchmark<float>(ctx);
#ifdef VIENNACL_WITH_OPENCL
  if ( viennacl::ocl::current_device().double_support() )
#endif
  {
    std::cout << std::endl;
    std::cout << "   -------------------------------" << std::endl;
    std::cout << "   # benchmarking double-precision" << std::endl;
    std::cout << "   -------------------------------" << std::endl;
    run_benchmark<double>(ctx);
  }
  return 0;
}


#include "test_Prefix.h"

#include <CGAL/boost/graph/Euler_operations.h>
#include <CGAL/boost/graph/IO/OFF.h>

#include <boost/range/distance.hpp>

#include <string>

template < typename Mesh>
typename boost::graph_traits<Mesh>::
halfedge_descriptor find_halfedge(double x1, double y1,
                                  double x2, double y2,
                                  Mesh& m,
                                  bool is_border = false)
{
  typedef typename boost::property_map<Mesh, CGAL::vertex_point_t>::type VPMAP;
  typedef typename boost::property_traits<VPMAP>::value_type Point;

  typedef typename boost::graph_traits<Mesh>::halfedge_descriptor halfedge_descriptor;
  VPMAP vpmap = get(CGAL::vertex_point, m);
  for(halfedge_descriptor h : halfedges(m))
  {
    if(get(vpmap, source(h, m)) == Point(x1,y1,0)
       && get(vpmap, target(h, m)) == Point(x2,y2,0))
    {
      if(is_border == CGAL::is_border(h, m))
        return h;
      else
        return opposite(h, m);
    }
  }
  return boost::graph_traits<Mesh>::null_halfedge();
}

template <typename Mesh>
void
collapse_edge_test()
{
  CGAL_GRAPH_TRAITS_MEMBERS(Mesh);
  typedef typename boost::graph_traits<Mesh>:: vertex_descriptor vertex_descriptor;
  typedef typename boost::graph_traits<Mesh>:: halfedge_descriptor halfedge_descriptor;

  const std::string fname = "data/flat_hexahedron.off";
  Mesh m;
  if(!CGAL::read_OFF(fname, m))
    std::cout << "Error reading file: " << fname << std::endl;

  bool m_is_valid = CGAL::is_valid(m);
  assert(m_is_valid);

  Mesh test_mesh;
  CGAL::copy_face_graph(m, test_mesh);
  m_is_valid = CGAL::is_valid(m);
  assert(m_is_valid);

  //case 1: General Case.
  {
    halfedge_descriptor he = find_halfedge(-0.5,0,
                                           0.5,0,
                                           test_mesh);
    halfedge_descriptor en = next(he, test_mesh);
    halfedge_descriptor eno = opposite(en, test_mesh);
    halfedge_descriptor eno_prime = opposite(next(opposite(he, test_mesh), test_mesh), test_mesh);
    vertex_descriptor v1 = target(he, test_mesh);
    bool ok = CGAL::Euler::collapse_edge(edge(he, test_mesh), test_mesh) == v1;
    assert(ok);
    char found = 0;
    for(halfedge_descriptor it : CGAL::halfedges_around_target(v1,test_mesh))
    {
      if(it == eno
         || it == eno_prime){
        ++found;
      }
    }
    assert(found == 2);
    CGAL::clear(test_mesh);

  }
  //case 2:  collapsing edge is not itself a border, but is incident upon a border edge that is removed.
  {
    CGAL::copy_face_graph(m, test_mesh);
    halfedge_descriptor he = find_halfedge(0,0.5,
                                           -0.75,0.5,
                                           test_mesh);
    CGAL::Euler::remove_face(he, test_mesh);

    he = find_halfedge(-0.5,0,
                       0.5,0,
                       test_mesh);
    halfedge_descriptor en = next(he, test_mesh);
    halfedge_descriptor eno = opposite(en, test_mesh);
    halfedge_descriptor eno_prime = opposite(next(opposite(he, test_mesh), test_mesh), test_mesh);
    vertex_descriptor v1 = target(he, test_mesh);
    bool ok = CGAL::Euler::collapse_edge(edge(he, test_mesh), test_mesh) == v1;
    assert(ok);
    char found = 0;
    for(halfedge_descriptor it : CGAL::halfedges_around_target(v1,test_mesh))
    {
      if(it == eno
         || it == eno_prime){
        ++found;
      }
    }
    assert(found == 2);
    CGAL::clear(test_mesh);
  }
  //case 3: collapsing edge is not itself a border, but is incident upon a border edge that is not removed
  {
    CGAL::copy_face_graph(m, test_mesh);
    halfedge_descriptor he = find_halfedge(1.5,0,
                                           0.75,0.5,
                                           test_mesh);
    CGAL::Euler::remove_face(he, test_mesh);

    he = find_halfedge(-0.5,0,
                       0.5,0,
                       test_mesh);
    halfedge_descriptor en = next(he, test_mesh);
    halfedge_descriptor eno = opposite(en, test_mesh);
    halfedge_descriptor eno_prime = opposite(next(opposite(he, test_mesh), test_mesh), test_mesh);
    vertex_descriptor v1 = target(he, test_mesh);
    bool ok = CGAL::Euler::collapse_edge(edge(he, test_mesh), test_mesh) == v1;
    assert(ok);
    char found = 0;
    for(halfedge_descriptor it : CGAL::halfedges_around_target(v1,test_mesh))
    {
      if(it == eno
         || it == eno_prime){
        ++found;
      }
    }
    assert(found == 2);
    CGAL::clear(test_mesh);
  }
  //case 4: collapsing edge is itself a border
  {
    CGAL::copy_face_graph(m, test_mesh);
    halfedge_descriptor he = find_halfedge(-0.5, 0,
                                           0, -0.5,
                                           test_mesh);
    CGAL::Euler::remove_face(he, test_mesh);
    he = find_halfedge(0, -0.5,
                       -0.5, 0,
                       test_mesh);
    CGAL::Euler::remove_face(he, test_mesh);
    he = find_halfedge(0, -0.5,
                       0.75, -0.5,
                       test_mesh);
    CGAL::Euler::remove_face(he, test_mesh);

    he = find_halfedge(-0.5,0,
                       0.5,0,
                       test_mesh);
    halfedge_descriptor en = next(he, test_mesh);
    halfedge_descriptor eno = opposite(en, test_mesh);
    halfedge_descriptor ep_prime = prev(opposite(he, test_mesh), test_mesh);
    halfedge_descriptor eno_prime = opposite(next(opposite(he, test_mesh), test_mesh), test_mesh);
    vertex_descriptor v1 = target(he, test_mesh);
    bool ok = CGAL::Euler::collapse_edge(edge(he, test_mesh), test_mesh) == v1;
    assert(ok);
    char found = 0;
    for(halfedge_descriptor it : CGAL::halfedges_around_target(v1,test_mesh))
    {
      if(it == eno
         || it == eno_prime
         || it == ep_prime){
        ++found;
      }
    }
    assert(found == 3);
    CGAL::clear(test_mesh);
  }
  //case 5 singular case.
  {
    CGAL::copy_face_graph(m, test_mesh);
    halfedge_descriptor he = find_halfedge(0.75,0.5,
                                           1.5,0,
                                           test_mesh);
    CGAL::Euler::remove_face(he, test_mesh);
    he = find_halfedge(0.75,-0.5,
                       1.5,0,
                       test_mesh);
    CGAL::Euler::remove_face(he, test_mesh);
    he = find_halfedge(0,0.5,
                       0.5,0,
                       test_mesh);
    CGAL::Euler::remove_face(he, test_mesh);
    he = find_halfedge(0.5,0,
                       0,-0.5,
                       test_mesh);
    CGAL::Euler::remove_face(he, test_mesh);

    he = find_halfedge(-0.5,0,
                       0.5,0,
                       test_mesh);
    CGAL::Euler::remove_face(he, test_mesh);
    halfedge_descriptor ep = prev(he, test_mesh);
    halfedge_descriptor eno_prime = opposite(next(opposite(he, test_mesh), test_mesh), test_mesh);
    vertex_descriptor v1 = target(he, test_mesh);
    bool ok = CGAL::Euler::collapse_edge(edge(he, test_mesh), test_mesh) == v1;
    assert(ok);
    char found = 0;
    for(halfedge_descriptor it : CGAL::halfedges_around_target(v1,test_mesh))
    {
      if(it == ep)
        ++found;
      else if( it == eno_prime){
        ++found;
      }
    }
    assert(found == 2);
    CGAL::clear(test_mesh);
  }
}

int main()
{

  collapse_edge_test<Polyhedron>();
  collapse_edge_test<SM>();

#ifdef CGAL_USE_OPENMESH
  collapse_edge_test<OMesh>();
#endif

  std::cerr << "done\n";
  return 0;
}