// r_c_shortest_paths.hpp header file

// Copyright Michael Drexl 2005, 2006.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GRAPH_R_C_SHORTEST_PATHS_HPP
#define BOOST_GRAPH_R_C_SHORTEST_PATHS_HPP

#include <map>
#include <queue>
#include <vector>
#include <list>

#include <boost/make_shared.hpp>
#include <boost/enable_shared_from_this.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/property_map/property_map.hpp>

namespace boost {

// r_c_shortest_paths_label struct
template<class Graph, class Resource_Container>
struct r_c_shortest_paths_label : public boost::enable_shared_from_this<r_c_shortest_paths_label<Graph, Resource_Container> >
{
  r_c_shortest_paths_label
  ( const unsigned long n,
    const Resource_Container& rc = Resource_Container(),
    const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > pl = boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> >(),
    const typename graph_traits<Graph>::edge_descriptor& ed = graph_traits<Graph>::edge_descriptor(),
    const typename graph_traits<Graph>::vertex_descriptor& vd = graph_traits<Graph>::vertex_descriptor() )
  : num( n ),
    cumulated_resource_consumption( rc ),
    p_pred_label( pl ),
    pred_edge( ed ),
    resident_vertex( vd ),
    b_is_dominated( false ),
    b_is_processed( false )
  {}

  r_c_shortest_paths_label& operator=( const r_c_shortest_paths_label& other )
  {
    if( this == &other )
      return *this;
    this->~r_c_shortest_paths_label();
    new( this ) r_c_shortest_paths_label( other );
    return *this;
  }
  const unsigned long num;
  Resource_Container cumulated_resource_consumption;
  const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > p_pred_label;
  const typename graph_traits<Graph>::edge_descriptor pred_edge;
  const typename graph_traits<Graph>::vertex_descriptor resident_vertex;
  bool b_is_dominated;
  bool b_is_processed;
}; // r_c_shortest_paths_label

template<class Graph, class Resource_Container>
inline bool operator==
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return
    l1.cumulated_resource_consumption == l2.cumulated_resource_consumption;
}

template<class Graph, class Resource_Container>
inline bool operator!=
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return
    !( l1 == l2 );
}

template<class Graph, class Resource_Container>
inline bool operator<
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return
    l1.cumulated_resource_consumption < l2.cumulated_resource_consumption;
}

template<class Graph, class Resource_Container>
inline bool operator>
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return
    l2.cumulated_resource_consumption < l1.cumulated_resource_consumption;
}

template<class Graph, class Resource_Container>
inline bool operator<=
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return
    l1 < l2 || l1 == l2;
}

template<class Graph, class Resource_Container>
inline bool operator>=
( const r_c_shortest_paths_label<Graph, Resource_Container>& l1,
  const r_c_shortest_paths_label<Graph, Resource_Container>& l2 )
{
  return l2 < l1 || l1 == l2;
}

template<typename Graph, typename Resource_Container>
inline bool operator<
        ( const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > &t,
          const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > &u) {
    return *t < *u;
}

template<typename Graph, typename Resource_Container>
inline bool operator<=( const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > &t,
                        const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > &u ) {
    return *t <= *u;
}

template<typename Graph, typename Resource_Container>
inline bool operator>
        (
          const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > &t,
          const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > &u ) {
    return *t > *u;
}

template<typename Graph, typename Resource_Container>
inline bool operator>=
        (
          const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > &t,
          const boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > &u) {
    return *t >= *u;
}

namespace detail {

// r_c_shortest_paths_dispatch function (body/implementation)
template<class Graph,
         class VertexIndexMap,
         class EdgeIndexMap,
         class Resource_Container,
         class Resource_Extension_Function,
         class Dominance_Function,
         class Label_Allocator,
         class Visitor>
void r_c_shortest_paths_dispatch
( const Graph& g,
  const VertexIndexMap& vertex_index_map,
  const EdgeIndexMap& /*edge_index_map*/,
  typename graph_traits<Graph>::vertex_descriptor s,
  typename graph_traits<Graph>::vertex_descriptor t,
  // each inner vector corresponds to a pareto-optimal path
  std::vector
    <std::vector
      <typename graph_traits
        <Graph>::edge_descriptor> >& pareto_optimal_solutions,
  std::vector
    <Resource_Container>& pareto_optimal_resource_containers,
  bool b_all_pareto_optimal_solutions,
  // to initialize the first label/resource container
  // and to carry the type information
  const Resource_Container& rc,
  Resource_Extension_Function& ref,
  Dominance_Function& dominance,
  // to specify the memory management strategy for the labels
  Label_Allocator /*la*/,
  Visitor vis )
{
  pareto_optimal_resource_containers.clear();
  pareto_optimal_solutions.clear();

  size_t i_label_num = 0;
#if defined(BOOST_NO_CXX11_ALLOCATOR)
  typedef
    typename
      Label_Allocator::template rebind
        <r_c_shortest_paths_label
          <Graph, Resource_Container> >::other LAlloc;
#else
  typedef
     typename
     std::allocator_traits<Label_Allocator>::template rebind_alloc
     <r_c_shortest_paths_label
     <Graph, Resource_Container> > LAlloc;
  typedef std::allocator_traits<LAlloc> LTraits;
#endif
  LAlloc l_alloc;
  typedef
    boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > Splabel;
  std::priority_queue<Splabel, std::vector<Splabel>, std::greater<Splabel> >
    unprocessed_labels;

  bool b_feasible = true;
  Splabel splabel_first_label = boost::allocate_shared<r_c_shortest_paths_label<Graph, Resource_Container> >(
          l_alloc,
          i_label_num++,
          rc,
          boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> >(),
          typename graph_traits<Graph>::edge_descriptor(),
          s );

  unprocessed_labels.push( splabel_first_label );
  std::vector<std::list<Splabel> > vec_vertex_labels_data( num_vertices( g ) );
  iterator_property_map<typename std::vector<std::list<Splabel> >::iterator,
                        VertexIndexMap>
    vec_vertex_labels(vec_vertex_labels_data.begin(), vertex_index_map);
  vec_vertex_labels[s].push_back( splabel_first_label );
  typedef
    std::vector<typename std::list<Splabel>::iterator>
    vec_last_valid_positions_for_dominance_data_type;
  vec_last_valid_positions_for_dominance_data_type
    vec_last_valid_positions_for_dominance_data( num_vertices( g ) );
  iterator_property_map<
      typename vec_last_valid_positions_for_dominance_data_type::iterator,
      VertexIndexMap>
    vec_last_valid_positions_for_dominance
      (vec_last_valid_positions_for_dominance_data.begin(),
       vertex_index_map);
  BGL_FORALL_VERTICES_T(v, g, Graph) {
    put(vec_last_valid_positions_for_dominance, v, vec_vertex_labels[v].begin());
  }
  std::vector<size_t> vec_last_valid_index_for_dominance_data( num_vertices( g ), 0 );
  iterator_property_map<std::vector<size_t>::iterator, VertexIndexMap>
    vec_last_valid_index_for_dominance
      (vec_last_valid_index_for_dominance_data.begin(), vertex_index_map);
  std::vector<bool>
    b_vec_vertex_already_checked_for_dominance_data( num_vertices( g ), false );
  iterator_property_map<std::vector<bool>::iterator, VertexIndexMap>
    b_vec_vertex_already_checked_for_dominance
      (b_vec_vertex_already_checked_for_dominance_data.begin(),
       vertex_index_map);

  while( !unprocessed_labels.empty()  && vis.on_enter_loop(unprocessed_labels, g) )
  {
    Splabel cur_label = unprocessed_labels.top();
    unprocessed_labels.pop();
    vis.on_label_popped( *cur_label, g );
    // an Splabel object in unprocessed_labels and the respective Splabel
    // object in the respective list<Splabel> of vec_vertex_labels share their
    // embedded r_c_shortest_paths_label object
    // to avoid memory leaks, dominated
    // r_c_shortest_paths_label objects are marked and deleted when popped
    // from unprocessed_labels, as they can no longer be deleted at the end of
    // the function; only the Splabel object in unprocessed_labels still
    // references the r_c_shortest_paths_label object
    // this is also for efficiency, because the else branch is executed only
    // if there is a chance that extending the
    // label leads to new undominated labels, which in turn is possible only
    // if the label to be extended is undominated
    if( !cur_label->b_is_dominated )
    {
      typename boost::graph_traits<Graph>::vertex_descriptor
        i_cur_resident_vertex = cur_label->resident_vertex;
      std::list<Splabel>& list_labels_cur_vertex =
        get(vec_vertex_labels, i_cur_resident_vertex);
      if( list_labels_cur_vertex.size() >= 2
          && vec_last_valid_index_for_dominance[i_cur_resident_vertex]
               < list_labels_cur_vertex.size() )
      {
        typename std::list<Splabel>::iterator outer_iter =
          list_labels_cur_vertex.begin();
        bool b_outer_iter_at_or_beyond_last_valid_pos_for_dominance = false;
        while( outer_iter != list_labels_cur_vertex.end() )
        {
          Splabel cur_outer_splabel = *outer_iter;
          typename std::list<Splabel>::iterator inner_iter = outer_iter;
          if( !b_outer_iter_at_or_beyond_last_valid_pos_for_dominance
              && outer_iter ==
                   get(vec_last_valid_positions_for_dominance,
                       i_cur_resident_vertex) )
            b_outer_iter_at_or_beyond_last_valid_pos_for_dominance = true;
          if( !get(b_vec_vertex_already_checked_for_dominance, i_cur_resident_vertex)
              || b_outer_iter_at_or_beyond_last_valid_pos_for_dominance )
          {
            ++inner_iter;
          }
          else
          {
            inner_iter =
              get(vec_last_valid_positions_for_dominance,
                  i_cur_resident_vertex);
            ++inner_iter;
          }
          bool b_outer_iter_erased = false;
          while( inner_iter != list_labels_cur_vertex.end() )
          {
            Splabel cur_inner_splabel = *inner_iter;
            if( dominance( cur_outer_splabel->
                             cumulated_resource_consumption,
                           cur_inner_splabel->
                             cumulated_resource_consumption ) )
            {
              typename std::list<Splabel>::iterator buf = inner_iter;
              ++inner_iter;
              list_labels_cur_vertex.erase( buf );
              if( cur_inner_splabel->b_is_processed )
              {
                cur_inner_splabel.reset();
              }
              else
                cur_inner_splabel->b_is_dominated = true;
              continue;
            }
            else
              ++inner_iter;
            if( dominance( cur_inner_splabel->
                             cumulated_resource_consumption,
                           cur_outer_splabel->
                             cumulated_resource_consumption ) )
            {
              typename std::list<Splabel>::iterator buf = outer_iter;
              ++outer_iter;
              list_labels_cur_vertex.erase( buf );
              b_outer_iter_erased = true;
              if( cur_outer_splabel->b_is_processed )
              {
                cur_outer_splabel.reset();
              }
              else
                cur_outer_splabel->b_is_dominated = true;
              break;
            }
          }
          if( !b_outer_iter_erased )
            ++outer_iter;
        }
        if( list_labels_cur_vertex.size() > 1 )
          put(vec_last_valid_positions_for_dominance, i_cur_resident_vertex,
            (--(list_labels_cur_vertex.end())));
        else
          put(vec_last_valid_positions_for_dominance, i_cur_resident_vertex,
            list_labels_cur_vertex.begin());
        put(b_vec_vertex_already_checked_for_dominance,
            i_cur_resident_vertex, true);
        put(vec_last_valid_index_for_dominance, i_cur_resident_vertex,
          list_labels_cur_vertex.size() - 1);
      }
    }
    if( !b_all_pareto_optimal_solutions && cur_label->resident_vertex == t )
    {
      // the devil don't sleep
      if( cur_label->b_is_dominated )
      {
        cur_label.reset();
      }
      while( unprocessed_labels.size() )
      {
        Splabel l = unprocessed_labels.top();
        unprocessed_labels.pop();
        // delete only dominated labels, because nondominated labels are
        // deleted at the end of the function
        if( l->b_is_dominated )
        {
          l.reset();
        }
      }
      break;
    }
    if( !cur_label->b_is_dominated )
    {
      cur_label->b_is_processed = true;
      vis.on_label_not_dominated( *cur_label, g );
      typename graph_traits<Graph>::vertex_descriptor cur_vertex =
        cur_label->resident_vertex;
      typename graph_traits<Graph>::out_edge_iterator oei, oei_end;
      for( boost::tie( oei, oei_end ) = out_edges( cur_vertex, g );
           oei != oei_end;
           ++oei )
      {
        b_feasible = true;
        Splabel new_label = boost::allocate_shared<r_c_shortest_paths_label<Graph, Resource_Container> >(
                l_alloc,
                i_label_num++,
                cur_label->cumulated_resource_consumption,
                cur_label,
                *oei,
                target( *oei, g ) );
        b_feasible =
          ref( g,
               new_label->cumulated_resource_consumption,
               new_label->p_pred_label->cumulated_resource_consumption,
               new_label->pred_edge );

        if( !b_feasible )
        {
          vis.on_label_not_feasible( *new_label, g );
          new_label.reset();
        }
        else
        {
          vis.on_label_feasible( *new_label, g );
          vec_vertex_labels[new_label->resident_vertex].
            push_back( new_label );
          unprocessed_labels.push( new_label );
        }
      }
    }
    else
    {
      vis.on_label_dominated( *cur_label, g );
      cur_label.reset();
    }
  }
  std::list<Splabel> dsplabels = get(vec_vertex_labels, t);
  typename std::list<Splabel>::const_iterator csi = dsplabels.begin();
  typename std::list<Splabel>::const_iterator csi_end = dsplabels.end();
  // if d could be reached from o
  if( !dsplabels.empty() )
  {
    for( ; csi != csi_end; ++csi )
    {
      std::vector<typename graph_traits<Graph>::edge_descriptor>
        cur_pareto_optimal_path;
      boost::shared_ptr<r_c_shortest_paths_label<Graph, Resource_Container> > p_cur_label = *csi;
      pareto_optimal_resource_containers.
        push_back( p_cur_label->cumulated_resource_consumption );
      while( p_cur_label->num != 0 )
      {
        cur_pareto_optimal_path.push_back( p_cur_label->pred_edge );
        p_cur_label = p_cur_label->p_pred_label;

        // assertion b_is_valid beyond this point is not correct if the domination function
        // requires resource levels to be strictly greater than existing values
        //
        // Example
        // Customers
        // id   min_arrival   max_departure
        //  2             0             974
        //  3             0             972
        //  4             0             964
        //  5           678             801
        //
        // Path A: 2-3-4-5 (times: 0-16-49-84-678)
        // Path B: 3-2-4-5 (times: 0-18-51-62-678)
        // The partial path 3-2-4 dominates the other partial path 2-3-4,
        // though the path 3-2-4-5 does not strictly dominate the path 2-3-4-5
      }
      pareto_optimal_solutions.push_back( cur_pareto_optimal_path );
      if( !b_all_pareto_optimal_solutions )
        break;
    }
  }

  BGL_FORALL_VERTICES_T(i, g, Graph) {
    std::list<Splabel>& list_labels_cur_vertex = vec_vertex_labels[i];
    typename std::list<Splabel>::iterator si = list_labels_cur_vertex.begin();
    const typename std::list<Splabel>::iterator si_end = list_labels_cur_vertex.end();
    for(; si != si_end; ++si )
    {
      (*si).reset();
    }
  }
} // r_c_shortest_paths_dispatch

} // detail

// default_r_c_shortest_paths_visitor struct
struct default_r_c_shortest_paths_visitor
{
  template<class Label, class Graph>
  void on_label_popped( const Label&, const Graph& ) {}
  template<class Label, class Graph>
  void on_label_feasible( const Label&, const Graph& ) {}
  template<class Label, class Graph>
  void on_label_not_feasible( const Label&, const Graph& ) {}
  template<class Label, class Graph>
  void on_label_dominated( const Label&, const Graph& ) {}
  template<class Label, class Graph>
  void on_label_not_dominated( const Label&, const Graph& ) {}
  template<class Queue, class Graph>
  bool on_enter_loop(const Queue& queue, const Graph& graph) {return true;}
}; // default_r_c_shortest_paths_visitor

// default_r_c_shortest_paths_allocator
typedef
  std::allocator<int> default_r_c_shortest_paths_allocator;
// default_r_c_shortest_paths_allocator

// r_c_shortest_paths functions (handle/interface)
// first overload:
// - return all pareto-optimal solutions
// - specify Label_Allocator and Visitor arguments
template<class Graph,
         class VertexIndexMap,
         class EdgeIndexMap,
         class Resource_Container,
         class Resource_Extension_Function,
         class Dominance_Function,
         class Label_Allocator,
         class Visitor>
void r_c_shortest_paths
( const Graph& g,
  const VertexIndexMap& vertex_index_map,
  const EdgeIndexMap& edge_index_map,
  typename graph_traits<Graph>::vertex_descriptor s,
  typename graph_traits<Graph>::vertex_descriptor t,
  // each inner vector corresponds to a pareto-optimal path
  std::vector<std::vector<typename graph_traits<Graph>::edge_descriptor> >&
    pareto_optimal_solutions,
  std::vector<Resource_Container>& pareto_optimal_resource_containers,
  // to initialize the first label/resource container
  // and to carry the type information
  const Resource_Container& rc,
  const Resource_Extension_Function& ref,
  const Dominance_Function& dominance,
  // to specify the memory management strategy for the labels
  Label_Allocator la,
  Visitor vis )
{
  r_c_shortest_paths_dispatch( g,
                               vertex_index_map,
                               edge_index_map,
                               s,
                               t,
                               pareto_optimal_solutions,
                               pareto_optimal_resource_containers,
                               true,
                               rc,
                               ref,
                               dominance,
                               la,
                               vis );
}

// second overload:
// - return only one pareto-optimal solution
// - specify Label_Allocator and Visitor arguments
template<class Graph,
         class VertexIndexMap,
         class EdgeIndexMap,
         class Resource_Container,
         class Resource_Extension_Function,
         class Dominance_Function,
         class Label_Allocator,
         class Visitor>
void r_c_shortest_paths
( const Graph& g,
  const VertexIndexMap& vertex_index_map,
  const EdgeIndexMap& edge_index_map,
  typename graph_traits<Graph>::vertex_descriptor s,
  typename graph_traits<Graph>::vertex_descriptor t,
  std::vector<typename graph_traits<Graph>::edge_descriptor>&
    pareto_optimal_solution,
  Resource_Container& pareto_optimal_resource_container,
  // to initialize the first label/resource container
  // and to carry the type information
  const Resource_Container& rc,
  const Resource_Extension_Function& ref,
  const Dominance_Function& dominance,
  // to specify the memory management strategy for the labels
  Label_Allocator la,
  Visitor vis )
{
  // each inner vector corresponds to a pareto-optimal path
  std::vector<std::vector<typename graph_traits<Graph>::edge_descriptor> >
    pareto_optimal_solutions;
  std::vector<Resource_Container> pareto_optimal_resource_containers;
  r_c_shortest_paths_dispatch( g,
                               vertex_index_map,
                               edge_index_map,
                               s,
                               t,
                               pareto_optimal_solutions,
                               pareto_optimal_resource_containers,
                               false,
                               rc,
                               ref,
                               dominance,
                               la,
                               vis );
  if (!pareto_optimal_solutions.empty()) {
    pareto_optimal_solution = pareto_optimal_solutions[0];
    pareto_optimal_resource_container = pareto_optimal_resource_containers[0];
  }
}

// third overload:
// - return all pareto-optimal solutions
// - use default Label_Allocator and Visitor
template<class Graph,
         class VertexIndexMap,
         class EdgeIndexMap,
         class Resource_Container,
         class Resource_Extension_Function,
         class Dominance_Function>
void r_c_shortest_paths
( const Graph& g,
  const VertexIndexMap& vertex_index_map,
  const EdgeIndexMap& edge_index_map,
  typename graph_traits<Graph>::vertex_descriptor s,
  typename graph_traits<Graph>::vertex_descriptor t,
  // each inner vector corresponds to a pareto-optimal path
  std::vector<std::vector<typename graph_traits<Graph>::edge_descriptor> >&
    pareto_optimal_solutions,
  std::vector<Resource_Container>& pareto_optimal_resource_containers,
  // to initialize the first label/resource container
  // and to carry the type information
  const Resource_Container& rc,
  const Resource_Extension_Function& ref,
  const Dominance_Function& dominance )
{
  r_c_shortest_paths_dispatch( g,
                               vertex_index_map,
                               edge_index_map,
                               s,
                               t,
                               pareto_optimal_solutions,
                               pareto_optimal_resource_containers,
                               true,
                               rc,
                               ref,
                               dominance,
                               default_r_c_shortest_paths_allocator(),
                               default_r_c_shortest_paths_visitor() );
}

// fourth overload:
// - return only one pareto-optimal solution
// - use default Label_Allocator and Visitor
template<class Graph,
         class VertexIndexMap,
         class EdgeIndexMap,
         class Resource_Container,
         class Resource_Extension_Function,
         class Dominance_Function>
void r_c_shortest_paths
( const Graph& g,
  const VertexIndexMap& vertex_index_map,
  const EdgeIndexMap& edge_index_map,
  typename graph_traits<Graph>::vertex_descriptor s,
  typename graph_traits<Graph>::vertex_descriptor t,
  std::vector<typename graph_traits<Graph>::edge_descriptor>&
    pareto_optimal_solution,
  Resource_Container& pareto_optimal_resource_container,
  // to initialize the first label/resource container
  // and to carry the type information
  const Resource_Container& rc,
  const Resource_Extension_Function& ref,
  const Dominance_Function& dominance )
{
  // each inner vector corresponds to a pareto-optimal path
  std::vector<std::vector<typename graph_traits<Graph>::edge_descriptor> >
    pareto_optimal_solutions;
  std::vector<Resource_Container> pareto_optimal_resource_containers;
  r_c_shortest_paths_dispatch( g,
                               vertex_index_map,
                               edge_index_map,
                               s,
                               t,
                               pareto_optimal_solutions,
                               pareto_optimal_resource_containers,
                               false,
                               rc,
                               ref,
                               dominance,
                               default_r_c_shortest_paths_allocator(),
                               default_r_c_shortest_paths_visitor() );
  if (!pareto_optimal_solutions.empty()) {
    pareto_optimal_solution = pareto_optimal_solutions[0];
    pareto_optimal_resource_container = pareto_optimal_resource_containers[0];
  }
}
// r_c_shortest_paths

// check_r_c_path function
template<class Graph,
         class Resource_Container,
         class Resource_Extension_Function>
void check_r_c_path( const Graph& g,
                     const std::vector
                       <typename graph_traits
                         <Graph>::edge_descriptor>& ed_vec_path,
                     const Resource_Container& initial_resource_levels,
                     // if true, computed accumulated final resource levels must
                     // be equal to desired_final_resource_levels
                     // if false, computed accumulated final resource levels must
                     // be less than or equal to desired_final_resource_levels
                     bool b_result_must_be_equal_to_desired_final_resource_levels,
                     const Resource_Container& desired_final_resource_levels,
                     Resource_Container& actual_final_resource_levels,
                     const Resource_Extension_Function& ref,
                     bool& b_is_a_path_at_all,
                     bool& b_feasible,
                     bool& b_correctly_extended,
                     typename graph_traits<Graph>::edge_descriptor&
                       ed_last_extended_arc )
{
  size_t i_size_ed_vec_path = ed_vec_path.size();
  std::vector<typename graph_traits<Graph>::edge_descriptor> buf_path;
  if( i_size_ed_vec_path == 0 )
    b_feasible = true;
  else
  {
    if( i_size_ed_vec_path == 1
        || target( ed_vec_path[0], g ) == source( ed_vec_path[1], g ) )
      buf_path = ed_vec_path;
    else
      for( size_t i = i_size_ed_vec_path ; i > 0; --i )
        buf_path.push_back( ed_vec_path[i - 1] );
    for( size_t i = 0; i < i_size_ed_vec_path - 1; ++i )
    {
      if( target( buf_path[i], g ) != source( buf_path[i + 1], g ) )
      {
        b_is_a_path_at_all = false;
        b_feasible = false;
        b_correctly_extended = false;
        return;
      }
    }
  }
  b_is_a_path_at_all = true;
  b_feasible = true;
  b_correctly_extended = false;
  Resource_Container current_resource_levels = initial_resource_levels;
  actual_final_resource_levels = current_resource_levels;
  for( size_t i = 0; i < i_size_ed_vec_path; ++i )
  {
    ed_last_extended_arc = buf_path[i];
    b_feasible = ref( g,
                      actual_final_resource_levels,
                      current_resource_levels,
                      buf_path[i] );
    current_resource_levels = actual_final_resource_levels;
    if( !b_feasible )
      return;
  }
  if( b_result_must_be_equal_to_desired_final_resource_levels )
    b_correctly_extended =
     actual_final_resource_levels == desired_final_resource_levels ?
       true : false;
  else
  {
    if( actual_final_resource_levels < desired_final_resource_levels
        || actual_final_resource_levels == desired_final_resource_levels )
      b_correctly_extended = true;
  }
} // check_path

} // namespace

#endif // BOOST_GRAPH_R_C_SHORTEST_PATHS_HPP


// Copyright 2016. 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.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: ryan.latture@gmail.com (Ryan Latture)

#include "mesh.h"
#include <Eigen/Dense>
#include <iostream>

namespace explicit_fea {
    Mesh::Mesh(const std::vector<Node> &nodes,
               const std::vector<std::unique_ptr<BeamElement>>& elems,
               BCList bcs) :
        bcs(std::move(bcs))
    {
        assemble_matrices(nodes, elems);
        apply_bcs();
    }

    SparseMatrix const& Mesh::get_global_stiffness_matrix() const {
        return global_stiffness_matrix;
    }

    SparseMatrix const& Mesh::get_inv_mass_matrix() const {
        return inverse_mass_matrix;
    }

    SparseMatrix const& Mesh::get_mass_matrix() const {
        return mass_matrix;
    }

    BCList const& Mesh::get_bcs() const {
        return bcs;
    }

    void Mesh::assemble_matrices(const std::vector<Node> &nodes, const std::vector<std::unique_ptr<BeamElement>>& elems) {
        int n = DOF::NUM_DOFS * nodes.size();
        global_stiffness_matrix.resize(n, n);
        mass_matrix.resize(n, n);
        inverse_mass_matrix.resize(n, n);

        std::vector<Eigen::Triplet<double> > stiffness_triplets, mass_triplets, inv_mass_triplets;
        stiffness_triplets.reserve(40 * elems.size());
        mass_triplets.reserve(40 * elems.size());
        inv_mass_triplets.reserve(40 * elems.size());
        LocalMatrix inv_mass, mass;

        for (size_t i = 0; i < elems.size(); ++i) {
            inv_mass = elems[i]->calculate_inv_mass_matrix(nodes);
            mass = inv_mass.inverse();
            append_triplets(*elems[i], stiffness_triplets, elems[i]->calculate_stiffness_matrix(nodes));
            append_triplets(*elems[i], mass_triplets, mass);
            append_triplets(*elems[i], inv_mass_triplets, inv_mass);
        }
        global_stiffness_matrix.setFromTriplets(stiffness_triplets.begin(), stiffness_triplets.end());
        inverse_mass_matrix.setFromTriplets(inv_mass_triplets.begin(), inv_mass_triplets.end());
        mass_matrix.setFromTriplets(mass_triplets.begin(), mass_triplets.end());

        global_stiffness_matrix.prune(1.e-14);
        mass_matrix.prune(1.e-14);
        inverse_mass_matrix.prune(1.e-14);

        global_stiffness_matrix.makeCompressed();
        mass_matrix.makeCompressed();
        inverse_mass_matrix.makeCompressed();
    }

    void Mesh::append_triplets(const BeamElement& elem, std::vector<Eigen::Triplet<double> > &triplets, const LocalMatrix& mat) {
        const int nn1 = elem.get_node_numbers()[0];
        const int nn2 = elem.get_node_numbers()[1];
        size_t row, col;
        const unsigned int dofs_per_elem = DOF::NUM_DOFS;

        SparseMatrix sparse_mat = mat.sparseView();

        for (size_t j = 0; j < sparse_mat.outerSize(); ++j) {
            for (SparseMatrix::InnerIterator it(sparse_mat, j); it; ++it) {
                row = it.row();
                col = it.col();

                // check position in local matrix and update corresponding global position
                if (row < dofs_per_elem) {
                    // top left
                    if (col < dofs_per_elem) {
                        triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * nn1 + row,
                                                                  dofs_per_elem * nn1 + col,
                                                                  it.value()));
                    }
                        // top right
                    else {
                        triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * nn1 + row,
                                                                  dofs_per_elem * (nn2 - 1) + col,
                                                                  it.value()));
                    }
                }
                else {
                    // bottom left
                    if (col < dofs_per_elem) {
                        triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * (nn2 - 1) + row,
                                                                  dofs_per_elem * nn1 + col,
                                                                  it.value()));
                    }
                        // bottom right
                    else {
                        triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * (nn2 - 1) + row,
                                                                  dofs_per_elem * (nn2 - 1) + col,
                                                                  it.value()));
                    }
                }
            }
        }
    }

    void Mesh::apply_bcs() {
        BcsPruneFunctor prune_functor(bcs);
        inverse_mass_matrix.prune(prune_functor);
        for (std::set<int>::const_iterator it = prune_functor.bcs_ind_set.begin();
             it != prune_functor.bcs_ind_set.end(); ++it) {
            inverse_mass_matrix.insert(*it, *it) = 1.0;
        }
    }

    Mesh::BcsPruneFunctor::BcsPruneFunctor(const BCList &bcs){
        for (BCList::const_iterator it = bcs.begin(); it != bcs.end(); ++it) {
            bcs_ind_set.insert((*it)->global_index);
        }
    }

    bool Mesh::BcsPruneFunctor::operator()(int row, int col, double value) const {
        bool row_exists = bcs_ind_set.find(row) != bcs_ind_set.end();
        if (row_exists)
            return false;

        bool col_exists = bcs_ind_set.find(col) != bcs_ind_set.end();
        return !col_exists;
    }

} // namespace explicit_fea


/* $Id: step-48.cc 27687 2012-11-26 14:37:31Z kronbichler $ */
/* Author: Katharina Kormann, Martin Kronbichler, Uppsala University, 2011-2012 */

/*    $Id: step-48.cc 27687 2012-11-26 14:37:31Z kronbichler $       */
/*                                                                */
/*    Copyright (C) 2011-2012 by the deal.II authors             */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// The necessary files from the deal.II library.
#include <deal.II/base/logstream.h>
#include <deal.II/base/utilities.h>
#include <deal.II/base/function.h>
#include <deal.II/base/conditional_ostream.h>
#include <deal.II/base/timer.h>
#include <deal.II/lac/vector.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_boundary_lib.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/compressed_simple_sparsity_pattern.h>
#include <deal.II/lac/trilinos_vector.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/distributed/tria.h>

// This includes the data structures for the efficient implementation of
// matrix-free methods.
#include <deal.II/lac/parallel_vector.h>
#include <deal.II/matrix_free/matrix_free.h>
#include <deal.II/matrix_free/fe_evaluation.h>

#include <fstream>
#include <iostream>
#include <iomanip>

namespace Step48
{
  using namespace dealii;

  // We start by defining two global variables to collect all parameters
  // subject to changes at one place: One for the dimension and one for the
  // finite element degree. The dimension is used in the main function as a
  // template argument for the actual classes (like in all other deal.II
  // programs), whereas the degree of the finite element is more crucial, as
  // it is passed as a template argument to the implementation of the
  // Sine-Gordon operator. Therefore, it needs to be a compile-time constant.
  const unsigned int dimension = 2;
  const unsigned int fe_degree = 4;

  // @sect3{SineGordonOperation}

  // The SineGordonOperation class implements the cell-based
  // operation that is needed in each time step. This nonlinear operation can
  // be implemented straight-forwardly based on the MatrixFree
  // class, in the same way as a linear operation would be treated by this
  // implementation of the finite element operator application. We apply two
  // template arguments to the class, one for the dimension and one for the
  // degree of the finite element. This is a difference to other functions in
  // deal.II where only the dimension is a template argument. This is
  // necessary to provide the inner loops in @p FEEvaluation with information
  // about loop lengths etc., which is essential for efficiency. On the other
  // hand, it makes it more challenging to implement the degree as a run-time
  // parameter.
  template <int dim, int fe_degree>
  class SineGordonOperation
  {
  public:
    SineGordonOperation(const MatrixFree<dim,double> &data_in,
                        const double                  time_step);

    void apply (parallel::distributed::Vector<double>                     &dst,
                const std::vector<parallel::distributed::Vector<double>*> &src) const;

  private:
    const MatrixFree<dim,double>         &data;
    const VectorizedArray<double>         delta_t_sqr;
    parallel::distributed::Vector<double> inv_mass_matrix;

    void local_apply (const MatrixFree<dim,double>               &data,
                      parallel::distributed::Vector<double>      &dst,
                      const std::vector<parallel::distributed::Vector<double>*> &src,
                      const std::pair<unsigned int,unsigned int> &cell_range) const;
  };

  // @sect4{SineGordonOperation::SineGordonOperation}

  // This is the constructor of the SineGordonOperation class. It receives a
  // reference to the MatrixFree holding the problem information and the time
  // step size as input parameters. The initialization routine sets up the
  // mass matrix. Since we use Gauss-Lobatto elements, the mass matrix is a
  // diagonal matrix and can be stored as a vector. The computation of the
  // mass matrix diagonal is simple to achieve with the data structures
  // provided by FEEvaluation: Just loop over all (macro-) cells and integrate
  // over the function that is constant one on all quadrature points by using
  // the integrate function with @p true argument at the slot for
  // values. Finally, we invert the diagonal entries since we have to multiply
  // by the inverse mass matrix in each time step.
  template <int dim, int fe_degree>
  SineGordonOperation<dim,fe_degree>::
  SineGordonOperation(const MatrixFree<dim,double> &data_in,
                      const double                  time_step)
    :
    data(data_in),
    delta_t_sqr(make_vectorized_array(time_step *time_step))
  {
    VectorizedArray<double> one = make_vectorized_array (1.);

    data.initialize_dof_vector (inv_mass_matrix);

    FEEvaluationGL<dim,fe_degree> fe_eval(data);
    const unsigned int            n_q_points = fe_eval.n_q_points;

    for (unsigned int cell=0; cell<data.n_macro_cells(); ++cell)
      {
        fe_eval.reinit(cell);
        for (unsigned int q=0; q<n_q_points; ++q)
          fe_eval.submit_value(one,q);
        fe_eval.integrate (true,false);
        fe_eval.distribute_local_to_global (inv_mass_matrix);
      }

    inv_mass_matrix.compress();
    for (unsigned int k=0; k<inv_mass_matrix.local_size(); ++k)
      if (inv_mass_matrix.local_element(k)>1e-15)
        inv_mass_matrix.local_element(k) = 1./inv_mass_matrix.local_element(k);
      else
        inv_mass_matrix.local_element(k) = 0;
  }

  // @sect4{SineGordonOperation::local_apply}

  // This operator implements the core operation of the program, the
  // integration over a range of cells for the nonlinear operator of the
  // Sine-Gordon problem. The implementation is based on the FEEvaluationGL
  // class since we are using the cell-based implementation for Gauss-Lobatto
  // elements.

  // The nonlinear function that we have to evaluate for the time stepping
  // routine includes the value of the function at the present time @p current
  // as well as the value at the previous time step @p old. Both values are
  // passed to the operator in the collection of source vectors @p src, which
  // is simply an STL vector of pointers to the actual solution vectors. This
  // construct of collecting several source vectors into one is necessary as
  // the cell loop in @p MatrixFree takes exactly one source and one
  // destination vector, even if we happen to use many vectors like the two in
  // this case. Note that the cell loop accepts any valid class for input and
  // output, which does not only include vectors but general data
  // types. However, only in case it encounters a
  // parallel::distributed::Vector<Number> or an STL vector collecting these
  // vectors, it calls functions that exchange data at the beginning and the
  // end of the loop. In the loop over the cells, we first have to read in the
  // values in the vectors related to the local values. Then, we evaluate the
  // value and the gradient of the current solution vector and the values of
  // the old vector at the quadrature points. Then, we combine the terms in
  // the scheme in the loop over the quadrature points. Finally, we integrate
  // the result against the test function and accumulate the result to the
  // global solution vector @p dst.
  template <int dim, int fe_degree>
  void SineGordonOperation<dim, fe_degree>::
  local_apply (const MatrixFree<dim>                      &data,
               parallel::distributed::Vector<double>      &dst,
               const std::vector<parallel::distributed::Vector<double>*> &src,
               const std::pair<unsigned int,unsigned int> &cell_range) const
  {
    AssertDimension (src.size(), 2);
    FEEvaluationGL<dim,fe_degree> current (data), old (data);
    for (unsigned int cell=cell_range.first; cell<cell_range.second; ++cell)
      {
        current.reinit (cell);
        old.reinit (cell);

        current.read_dof_values (*src[0]);
        old.read_dof_values     (*src[1]);

        current.evaluate (true, true, false);
        old.evaluate (true, false, false);

        for (unsigned int q=0; q<current.n_q_points; ++q)
          {
            const VectorizedArray<double> current_value = current.get_value(q);
            const VectorizedArray<double> old_value     = old.get_value(q);

            current.submit_value (2.*current_value - old_value -
                                  delta_t_sqr * std::sin(current_value),q);
            current.submit_gradient (- delta_t_sqr *
                                     current.get_gradient(q), q);
          }

        current.integrate (true,true);
        current.distribute_local_to_global (dst);
      }
  }

  //@sect4{SineGordonOperation::apply}

  // This function performs the time stepping routine based on the cell-local
  // strategy. First the destination vector is set to zero, then the cell-loop
  // is called, and finally the solution is multiplied by the inverse mass
  // matrix. The structure of the cell loop is implemented in the cell finite
  // element operator class. On each cell it applies the routine defined as
  // the local_apply() method of the class
  // SineGordonOperation, i.e., this. One could also
  // provide a function with the same signature that is not part of a class.
  template <int dim, int fe_degree>
  void SineGordonOperation<dim, fe_degree>::
  apply (parallel::distributed::Vector<double>                     &dst,
         const std::vector<parallel::distributed::Vector<double>*> &src) const
  {
    dst = 0;
    data.cell_loop (&SineGordonOperation<dim,fe_degree>::local_apply,
                    this, dst, src);
    dst.scale(inv_mass_matrix);
  }

  //@sect3{Equation data}

  // We define a time-dependent function that is used as initial
  // value. Different solutions can be obtained by varying the starting
  // time. This function has already been explained in step-25.
  template <int dim>
  class ExactSolution : public Function<dim>
  {
  public:
    ExactSolution (const unsigned int n_components = 1,
                   const double time = 0.) : Function<dim>(n_components, time) {}
    virtual double value (const Point<dim> &p,
                          const unsigned int component = 0) const;
  };

  template <int dim>
  double ExactSolution<dim>::value (const Point<dim> &p,
                                    const unsigned int /* component */) const
  {
    double t = this->get_time ();

    const double m = 0.5;
    const double c1 = 0.;
    const double c2 = 0.;
    const double factor = (m / std::sqrt(1.-m*m) *
                           std::sin(std::sqrt(1.-m*m)*t+c2));
    double result = 1.;
    for (unsigned int d=0; d<dim; ++d)
      result *= -4. * std::atan (factor / std::cosh(m*p[d]+c1));
    return result;
  }

  // @sect3{SineGordonProblem class}

  // This is the main class that builds on the class in step-25.  However, we
  // replaced the SparseMatrix<double> class by the MatrixFree class to store
  // the geometry data. Also, we use a distributed triangulation in this
  // example.
  template <int dim>
  class SineGordonProblem
  {
  public:
    SineGordonProblem ();
    void run ();

  private:
    ConditionalOStream pcout;

    void make_grid_and_dofs ();
    void oldstyle_operation ();
    void assemble_system ();
    void output_results (const unsigned int timestep_number) const;

#ifdef DEAL_II_USE_P4EST
    parallel::distributed::Triangulation<dim>   triangulation;
#else
    Triangulation<dim>   triangulation;
#endif
    FE_Q<dim>            fe;
    DoFHandler<dim>      dof_handler;
    ConstraintMatrix     constraints;
    IndexSet             locally_relevant_dofs;

    MatrixFree<dim,double> matrix_free_data;

    parallel::distributed::Vector<double> solution, old_solution, old_old_solution;

    const unsigned int n_global_refinements;
    double time, time_step;
    const double final_time;
    const double cfl_number;
    const unsigned int output_timestep_skip;
  };

  //@sect4{SineGordonProblem::SineGordonProblem}

  // This is the constructor of the SineGordonProblem class. The time interval
  // and time step size are defined here. Moreover, we use the degree of the
  // finite element that we defined at the top of the program to initialize a
  // FE_Q finite element based on Gauss-Lobatto support points. These points
  // are convenient because in conjunction with a QGaussLobatto quadrature
  // rule of the same order they give a diagonal mass matrix without
  // compromising accuracy too much (note that the integration is inexact,
  // though), see also the discussion in the introduction.
  template <int dim>
  SineGordonProblem<dim>::SineGordonProblem ()
    :
    pcout (std::cout,
           Utilities::System::get_this_mpi_process(MPI_COMM_WORLD)==0),
#ifdef DEAL_II_USE_P4EST
    triangulation (MPI_COMM_WORLD),
#endif
    fe (QGaussLobatto<1>(fe_degree+1)),
    dof_handler (triangulation),
    n_global_refinements (10-2*dim),
    time (-10),
    final_time (10),
    cfl_number (.1/fe_degree),
    output_timestep_skip (200)
  {}

  //@sect4{SineGordonProblem::make_grid_and_dofs}

  // As in step-25 this functions sets up a cube grid in dim
  // dimensions of extent $[-15,15]$. We refine the mesh more in the center of
  // the domain since the solution is concentrated there. We first refine all
  // cells whose center is within a radius of 11, and then refine once more
  // for a radius 6.  This is simple ad-hoc refinement could be done better by
  // adapting the mesh to the solution using error estimators during the time
  // stepping as done in other example programs, and using
  // parallel::distributed::SolutionTransfer to transfer the solution to the
  // new mesh.
  template <int dim>
  void SineGordonProblem<dim>::make_grid_and_dofs ()
  {
    GridGenerator::hyper_cube (triangulation, -15, 15);
    triangulation.refine_global (n_global_refinements);
    {
      typename Triangulation<dim>::active_cell_iterator
      cell = triangulation.begin_active(),
      end_cell = triangulation.end();
      for ( ; cell != end_cell; ++cell)
        if (cell->is_locally_owned())
          if (cell->center().norm() < 11)
            cell->set_refine_flag();
      triangulation.execute_coarsening_and_refinement();

      cell = triangulation.begin_active();
      end_cell = triangulation.end();
      for ( ; cell != end_cell; ++cell)
        if (cell->is_locally_owned())
          if (cell->center().norm() < 6)
            cell->set_refine_flag();
      triangulation.execute_coarsening_and_refinement();
    }

    pcout << "   Number of global active cells: "
#ifdef DEAL_II_USE_P4EST
          << triangulation.n_global_active_cells()
#else
          << triangulation.n_active_cells()
#endif
          << std::endl;

    dof_handler.distribute_dofs (fe);

    pcout << "   Number of degrees of freedom: "
          << dof_handler.n_dofs()
          << std::endl;

    // We generate hanging node constraints for ensuring continuity of the
    // solution. As in step-40, we need to equip the constraint matrix with
    // the IndexSet of locally relevant degrees of freedom to avoid it to
    // consume too much memory for big problems. Next, the  MatrixFree
    //  for the problem is set up. Note that we specify the MPI
    // communicator which we are going to use, and that we also want to use
    // shared-memory parallelization (hence one would use multithreading for
    // intra-node parallelism and not MPI; note that we here choose the
    // standard option — if we wanted to disable shared memory
    // parallelization, we would choose @p none). Finally, three solution
    // vectors are initialized. MatrixFree stores the layout that is to be
    // used by distributed vectors, so we just ask it to initialize the
    // vectors.
    DoFTools::extract_locally_relevant_dofs (dof_handler,
                                             locally_relevant_dofs);
    constraints.clear();
    constraints.reinit (locally_relevant_dofs);
    DoFTools::make_hanging_node_constraints (dof_handler, constraints);
    constraints.close();

    QGaussLobatto<1> quadrature (fe_degree+1);
    typename MatrixFree<dim>::AdditionalData additional_data;
    additional_data.mpi_communicator = MPI_COMM_WORLD;
    additional_data.tasks_parallel_scheme =
      MatrixFree<dim>::AdditionalData::partition_partition;

    matrix_free_data.reinit (dof_handler, constraints,
                             quadrature, additional_data);

    matrix_free_data.initialize_dof_vector (solution);
    old_solution.reinit (solution);
    old_old_solution.reinit (solution);
  }

  //@sect4{SineGordonProblem::output_results}

  // This function prints the norm of the solution and writes the solution
  // vector to a file. The norm is standard (except for the fact that we need
  // to be sure to only count norms on locally owned cells), and the second is
  // similar to what we did in step-40. However, we first need to generate an
  // appropriate vector for output: The ones we used during time stepping
  // contained information about ghosts dofs that one needs write access to
  // during the loops over cell. However, that is not the same as needed when
  // outputting. So we first initialize a vector with locally relevant degrees
  // of freedom by copying the solution (note how we use the function @p
  // copy_from to transfer data between vectors with the same local range, but
  // different layouts of ghosts). Then, we import the values on the ghost
  // DoFs and then distribute the constraints (as constraints are zero in the
  // vectors during loop over all cells).
  template <int dim>
  void
  SineGordonProblem<dim>::output_results (const unsigned int timestep_number) const
  {
    parallel::distributed::Vector<double> locally_relevant_solution;
    locally_relevant_solution.reinit (dof_handler.locally_owned_dofs(),
                                      locally_relevant_dofs,
                                      MPI_COMM_WORLD);
    locally_relevant_solution.copy_from (solution);
    locally_relevant_solution.update_ghost_values ();
    constraints.distribute (locally_relevant_solution);

    Vector<float> norm_per_cell (triangulation.n_active_cells());
    VectorTools::integrate_difference (dof_handler,
                                       locally_relevant_solution,
                                       ZeroFunction<dim>(),
                                       norm_per_cell,
                                       QGauss<dim>(fe_degree+1),
                                       VectorTools::L2_norm);
    const double solution_norm =
      std::sqrt(Utilities::MPI::sum (norm_per_cell.norm_sqr(), MPI_COMM_WORLD));

    pcout << "   Time:"
          << std::setw(8) << std::setprecision(3) << time
          << ", solution norm: "
          << std::setprecision(5) << std::setw(7) << solution_norm
          << std::endl;

    DataOut<dim> data_out;

    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (locally_relevant_solution, "solution");
    data_out.build_patches ();

    const std::string filename =
      "solution-" + Utilities::int_to_string (timestep_number, 3) +
      "." + Utilities::int_to_string (Utilities::MPI::
                                      this_mpi_process(MPI_COMM_WORLD),4);

    std::ofstream output ((filename + ".vtu").c_str());
    data_out.write_vtu (output);

    if (Utilities::MPI::this_mpi_process(MPI_COMM_WORLD) == 0)
      {
        std::vector<std::string> filenames;
        for (unsigned int i=0;
             i<Utilities::MPI::n_mpi_processes (MPI_COMM_WORLD); ++i)
          filenames.push_back ("solution-" +
                               Utilities::int_to_string (timestep_number, 3) +
                               "." +
                               Utilities::int_to_string (i, 4) +
                               ".vtu");

        std::ofstream master_output ((filename + ".pvtu").c_str());
        data_out.write_pvtu_record (master_output, filenames);
      }
  }

  // @sect4{SineGordonProblem::run}

  // This function is called by the main function and calls the subroutines of
  // the class.
  //
  // The first step is to set up the grid and the cell operator. Then, the
  // time step is computed from the CFL number given in the constructor and
  // the finest mesh size. The finest mesh size is computed as the diameter of
  // the last cell in the triangulation, which is the last cell on the finest
  // level of the mesh. This is only possible for Cartesian meshes, otherwise,
  // one needs to loop over all cells). Note that we need to query all the
  // processors for their finest cell since the not all processors might hold
  // a region where the mesh is at the finest level. Then, we readjust the
  // time step a little to hit the final time exactly if necessary.
  template <int dim>
  void
  SineGordonProblem<dim>::run ()
  {
    make_grid_and_dofs();

    const double local_min_cell_diameter =
      triangulation.last()->diameter()/std::sqrt(dim);
    const double global_min_cell_diameter
      = -Utilities::MPI::max(-local_min_cell_diameter, MPI_COMM_WORLD);
    time_step = cfl_number * global_min_cell_diameter;
    time_step = (final_time-time)/(int((final_time-time)/time_step));
    pcout << "   Time step size: " << time_step << ", finest cell: "
          << global_min_cell_diameter << std::endl << std::endl;

    // Next the initial value is set. Since we have a two-step time stepping
    // method, we also need a value of the solution at time-time_step. For
    // accurate results, one would need to compute this from the time
    // derivative of the solution at initial time, but here we ignore this
    // difficulty and just set it to the initial value function at that
    // artificial time.

    // We create an output of the initial value. Then we also need to collect
    // the two starting solutions in an STL vector of pointers field and to
    // set up an instance of the  SineGordonOperation class
    // based on the finite element degree specified at the top of this file.
    VectorTools::interpolate (dof_handler,
                              ExactSolution<dim> (1, time),
                              solution);
    VectorTools::interpolate (dof_handler,
                              ExactSolution<dim> (1, time-time_step),
                              old_solution);
    output_results (0);

    std::vector<parallel::distributed::Vector<double>*> previous_solutions;
    previous_solutions.push_back(&old_solution);
    previous_solutions.push_back(&old_old_solution);

    SineGordonOperation<dim,fe_degree> sine_gordon_op (matrix_free_data,
                                                       time_step);

    // Now loop over the time steps. In each iteration, we shift the solution
    // vectors by one and call the  apply  function of the
    // SineGordonOperator . Then, we write the solution to a file. We
    // clock the wall times for the computational time needed as wall as the
    // time needed to create the output and report the numbers when the time
    // stepping is finished.
    //
    // Note how this shift is implemented: We simply call the swap method on
    // the two vectors which swaps only some pointers without the need to copy
    // data around. Obviously, this is a more efficient way to move data
    // around. Let us see what happens in more detail: First, we exchange
    // old_solution with old_old_solution, which
    // means that old_old_solution gets
    // old_solution, which is what we expect. Similarly,
    // old_solution gets the content from solution
    // in the next step. Afterward, solution holds
    // old_old_solution, but that will be overwritten during this
    // step.
    unsigned int timestep_number = 1;

    Timer timer;
    double wtime = 0;
    double output_time = 0;
    for (time+=time_step; time<=final_time; time+=time_step, ++timestep_number)
      {
        timer.restart();
        old_old_solution.swap (old_solution);
        old_solution.swap (solution);
        sine_gordon_op.apply (solution, previous_solutions);
        wtime += timer.wall_time();

        timer.restart();
        if (timestep_number % output_timestep_skip == 0)
          output_results(timestep_number / output_timestep_skip);

        output_time += timer.wall_time();
      }
    timer.restart();
    output_results(timestep_number / output_timestep_skip + 1);
    output_time += timer.wall_time();

    pcout << std::endl
          << "   Performed " << timestep_number << " time steps."
          << std::endl;

    pcout << "   Average wallclock time per time step: "
          << wtime / timestep_number << "s" << std::endl;

    pcout << "   Spent " << output_time << "s on output and "
          << wtime << "s on computations." << std::endl;
  }
}

// @sect3{The main function}

// This is as in all other programs:
int main (int argc, char **argv)
{
  using namespace Step48;
  using namespace dealii;

  Utilities::System::MPI_InitFinalize mpi_initialization(argc, argv);

  try
    {
      deallog.depth_console (0);

      SineGordonProblem<dimension> sg_problem;
      sg_problem.run ();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#include <string>
#include <memory>
#include <tuple>
#include <stdexcept>
#include <algorithm>
#include <cmath>
#include <map>

#include <boost/thread/mutex.hpp>
#include <boost/thread/locks.hpp>
#include <boost/thread/lock_guard.hpp>

#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <string.h>

#include "cpl_serv.h"
#include "tiff.h"
#include "libxtiff/xtiffio.h"
#include "geotiffio.h"
#include "geovalues.h"
#include "geo_normalize.h"

#include "rastermap.h"
#include "map_geotiff.h"
#include "projection.h"
#include "util.h"

const char * const DEFAULT_ENCODING = "UTF-8";

// Map between the unit square (x and y in [0, 1]) and a general
// quadrilateral using bilinear interpolation.
// Normalized coordinates are given as UnitSquareCoord.
// Quadrilateral coordinatese are given as MapPixelDelta.
class BilinearInterpolator {
    public:
        // Initialize with the four corners of the general quadrilateral.
        // The coordinates are given in clock-wise order.
        BilinearInterpolator(const MapPixelDelta &P00,
                             const MapPixelDelta &P10,
                             const MapPixelDelta &P11,
                             const MapPixelDelta &P01);

        // Map from input (within the unit square) to the general quad.
        // Example: forward(UnitSquareCoord(0.25, 0)) -> P00 * 0.75 + P10 * 0.25
        MapPixelDelta forward(const UnitSquareCoord &input);

        // Map from the general quad back to the unit square.
        // This is the inverse transformation of forward().
        UnitSquareCoord inverse(const MapPixelDelta &X);
    private:
        template <typename T>
        T lerp(double f, const T &A, const T &B) {
            return (1-f)*A + f*B;
        }
        const MapPixelDelta A, B, C, D;
        const MapPixelDelta E, F, G;
};

BilinearInterpolator::BilinearInterpolator(const MapPixelDelta &P00,
                                           const MapPixelDelta &P10,
                                           const MapPixelDelta &P11,
                                           const MapPixelDelta &P01)
: A(P00), B(P10), C(P11), D(P01), E(B-A), F(D-A), G(A-B-D+C)
{}

MapPixelDelta BilinearInterpolator::forward(const UnitSquareCoord &input) {
    // Straight-forward bilinear interpolation.
    return lerp(input.y, lerp(input.x, A, B), lerp(input.x, D, C));
}

UnitSquareCoord BilinearInterpolator::inverse(const MapPixelDelta &X) {
    // Inverse bilinear interpolation involves solving a quadratic equation.
    // Cf. http://www.iquilezles.org/www/articles/ibilinear/ibilinear.htm
    MapPixelDelta H = X-A;
    double k2 = G.x*F.y - G.y*F.x;
    double k1 = E.x*F.y - E.y*F.x + H.x*G.y - H.y*G.x;
    double k0 = H.x*E.y - H.y*E.x;
    double v = (-k1 + sqrt(k1*k1 - 4*k0*k2)) / (2 * k2);
    if (v < 0 || v > 1) {
        v = (-k1 - sqrt(k1*k1 - 4*k0*k2)) / (2 * k2);
    }
    double u = (H.x - F.x*v)/(E.x + G.x*v);
    return UnitSquareCoord(u, v);
}

class TiffHandle {
    public:
        explicit TiffHandle(const std::wstring &fname)
        // Pass "m" flag to disable memory-mapped IO.
        // Otherwise we fill up the whole virtual address space with mmapped
        // TIF files and run out of space to load libraries or map other data.
        // This gives hard-to-diagnose OOM errors (debug with VMMap on Win32).
        : m_tiff(XTIFFOpenW(fname.c_str(), "rm"))
        {
            if (!m_tiff) {
                throw std::runtime_error("File not found");
            }
        };
        ~TiffHandle() {
            if (m_tiff) {
                XTIFFClose(m_tiff);
                m_tiff = NULL;
            }
        }
        TIFF *GetTIFF() { return m_tiff; };
    private:
        DISALLOW_COPY_AND_ASSIGN(TiffHandle);
        TIFF* m_tiff;
};

class GeoTiffHandle {
    public:
        explicit GeoTiffHandle(TiffHandle &tiffhandle)
            : m_gtif(GTIFNew(tiffhandle.GetTIFF()))
        {
            if (!m_gtif)
                throw std::runtime_error("Opening GeoTiff failed.");
        };
        ~GeoTiffHandle() {
            if (m_gtif) {
                GTIFFree(m_gtif);
                m_gtif = NULL;
            }
        };
        GTIF *GetGTIF() { return m_gtif; };
    private:
        DISALLOW_COPY_AND_ASSIGN(GeoTiffHandle);
        GTIF *m_gtif;
};

class Tiff {
    public:
        explicit Tiff(const std::wstring &fname);
        virtual ~Tiff() { };
        TIFF *GetTIFF() { return m_rawtiff; };
        unsigned int GetWidth() const { return m_width; };
        unsigned int GetHeight() const { return m_height; };
        unsigned int GetBitsPerSample() const { return m_bitspersample; };
        unsigned int GetSamplesPerPixel() const { return m_samplesperpixel; };
        PixelBuf GetRegion(
                const class MapPixelCoordInt &pos,
                const class MapPixelDeltaInt &size) const;

        template <typename T>
        std::tuple<unsigned int, const T*>
        GetField(ttag_t field) const;

        const std::wstring &GetFilename() const { return m_fname; };
        const std::wstring &GetTitle() const { return m_title; };
        const std::wstring &GetDescription() const { return m_description; };
    protected:
        const std::wstring m_fname;
        std::wstring m_title;
        std::wstring m_description;
        TiffHandle m_tiffhandle;
        TIFF *m_rawtiff;

        virtual void Hook_TIFFRGBAImageGet(TIFFRGBAImage &img) const {};
    private:
        unsigned int m_width, m_height;
        unsigned short int m_bitspersample, m_samplesperpixel;

        PixelBuf DoGetRegion(
                const class MapPixelCoordInt &pos,
                const class MapPixelDeltaInt &size) const;
};

class GeoTiff : public Tiff {
    public:
        explicit GeoTiff(const std::wstring &fname);
        virtual ~GeoTiff() { };

        bool CheckVersion() const;
        bool LoadCoordinates();

        bool PixelToPCS(double *x, double *y) const;
        bool PCSToPixel(double *x, double *y) const;
        const std::string &GetProj4String() const { return m_proj; };
        GeoDrawable::DrawableType GetType() const { return m_type; };

        template <typename T>
        std::tuple<unsigned int, std::shared_ptr<T>>
        GetKey(geokey_t key) const;

        template <typename T>
        bool GetKeySingle(geokey_t key, T *result) const;

        bool HasKey(geokey_t) const;

        geocode_t GetModel() const { return m_model; }

    protected:
        GeoTiffHandle m_gtifhandle;
        GTIF *m_rawgtif;

        virtual void Hook_TIFFRGBAImageGet(TIFFRGBAImage &img) const;
    private:
        bool CheckDHMValid() const;

        geocode_t m_model;
        const double *m_tiepoints;
        const double *m_pixscale;
        const double *m_transform;
        unsigned short int m_ntiepoints, m_npixscale, m_ntransform;

        std::string m_proj;
        GeoDrawable::DrawableType m_type;
};

static void put16bitbw_DHM(
    TIFFRGBAImage* img,
    uint32* cp,                     // ptr to write target in TO (dest buf)
    uint32 x, uint32 y,             // x,y offset of write target within TO
    uint32 w, uint32 h,             // pixel count of one row; number of rows
    int32 fromskew, int32 toskew,   // offsets on row change in FROM/TO
    unsigned char* pp)              // ptr to read location in FROM
{
    int samplesperpixel = img->samplesperpixel;
    int16 *wp = reinterpret_cast<int16*>(pp);
    while (h-- > 0) {
        for (x = w; x-- > 0;) {
            // Copy the 16 bit heightmap value over into the RGBA image
            // Take care to sign-extend
            *cp++ = (uint32)*wp;
            wp += samplesperpixel;
        }
        cp += toskew;
        wp += fromskew;
    }
}

Tiff::Tiff(const std::wstring &fname)
    : m_fname(fname), m_title(), m_description(), m_tiffhandle(fname),
      m_rawtiff(m_tiffhandle.GetTIFF())
{
    if (!TIFFGetField(m_rawtiff, TIFFTAG_IMAGEWIDTH, &m_width) ||
        !TIFFGetField(m_rawtiff, TIFFTAG_IMAGELENGTH, &m_height)) {
        throw std::runtime_error("Failed getting TIF dimensions.");
    }

    if (!TIFFGetField(m_rawtiff, TIFFTAG_BITSPERSAMPLE, &m_bitspersample) ||
        !TIFFGetField(m_rawtiff, TIFFTAG_SAMPLESPERPIXEL, &m_samplesperpixel))
    {
        throw std::runtime_error("Failed getting TIF pixel format.");
    }
    char *title;
    if (TIFFGetField(m_rawtiff, TIFFTAG_DOCUMENTNAME, &title)) {
        m_title = WStringFromString(title, DEFAULT_ENCODING);
    }
    char *description;
    if (TIFFGetField(m_rawtiff, TIFFTAG_IMAGEDESCRIPTION, &description)) {
        m_description = WStringFromString(description, DEFAULT_ENCODING);
    }
};

PixelBuf
Tiff::DoGetRegion(const MapPixelCoordInt &pos,
                  const MapPixelDeltaInt &size) const
{
    MapPixelCoordInt endpos = pos + size;
    if (endpos.x <= 0 || endpos.y <= 0 ||
        pos.x >= static_cast<int>(m_width) ||
        pos.y >= static_cast<int>(m_height))
    {
        return PixelBuf(size.x, size.y);
    }

    TIFFRGBAImage img;
    char emsg[1024] = "";
    if (!TIFFRGBAImageOK(m_rawtiff, emsg) ||
        !TIFFRGBAImageBegin(&img, m_rawtiff, 0, emsg)) {
        throw std::runtime_error("TIFF RGBA access not possible.");
    }

    img.col_offset = pos.x;
    img.row_offset = pos.y;

    // Give derived classes a chance to influence TIFFRGBAGetImage
    Hook_TIFFRGBAImageGet(img);

    PixelBuf result(size.x, size.y);
    int ok = TIFFRGBAImageGet(&img, result.GetRawData(), size.x, size.y);
    TIFFRGBAImageEnd(&img);

    if (!ok) {
        throw std::runtime_error("Loading TIFF data failed.");
    }
    return result;
}

PixelBuf
Tiff::GetRegion(const MapPixelCoordInt &pos,
                const MapPixelDeltaInt &size) const
{
    if (TIFFIsTiled(m_rawtiff))
        return DoGetRegion(pos, size);

    // Handle stripped images:
    // TIFFRGBAImageGet ignores img.col_offset for stripped images.
    // It always returns data starting from the first column of the image.
    //
    // So, fetch a full-width strip and copy the relevant portion into the
    // output PixelBuf.
    auto result = PixelBuf(size.x, size.y);
    MapPixelCoordInt end = pos + size;

    int strip_size = -1;
    if (!TIFFGetFieldDefaulted(m_rawtiff, TIFFTAG_ROWSPERSTRIP, &strip_size)) {
        throw std::runtime_error("Failed getting TIF dimensions.");
    }
    if (strip_size < 0) {
        throw std::runtime_error("Stripped TIF image with strip height < 0?!");
    }
    if (static_cast<unsigned int>(strip_size) > m_height) {
        // TIFFRGBAImageGet crashes when reading from an image where
        // rows_per_strip > total_rows_in_image if more than
        // total_rows_in_image rows are requested.
        //
        // Cap the number of rows we try to read, in this case.
        strip_size = m_height;
    }

    int first_ty = pos.y / strip_size;
    int last_ty = (end.y - 1) / strip_size;
    for (int ty = pos.y / strip_size; ty*strip_size < end.y; ty++) {
        PixelBuf tile = DoGetRegion(
                MapPixelCoordInt(0, ty*strip_size),
                MapPixelDeltaInt(m_width, strip_size));
        if (!tile.GetData()) {
            continue;
        }
        auto insert_pos = PixelBufCoord(
                -pos.x,
                (last_ty - ty + first_ty) * strip_size - pos.y);
        result.Insert(insert_pos, tile);
    }
    return result;
}

template <typename T>
std::tuple<unsigned int, const T*>
Tiff::GetField(ttag_t field) const {
    const T *data;
    unsigned int length;
    if (!TIFFGetField(m_rawtiff, field, &length, &data)) {
        length = 0;
        data = NULL;
    }
    return std::make_tuple(length, data);
}

/**
 * Internal callback to resolve csv data filenames to full paths.
 *
 * @locking Acquires an internal ``std::mutex``. No external calls are made
 * with that mutex held.
 */
static const char *CSVFileOverride(const char * input_fname) {
    static boost::mutex strmap_mutex;
    static std::map<std::string, const char *> strmap;

    {
        boost::lock_guard<boost::mutex> lock(strmap_mutex);
        auto result = strmap.find(input_fname);
        if (result != strmap.end()) {
            return result->second;
        }
    }

    std::string csvdir = GetModuleDir_char() + "csv" + ODM_PathSep_char;
    std::string fpath = csvdir + input_fname;
    if (!FileExists(fpath)) {
        throw std::runtime_error("CSV file does not exist: " + fpath);
    }

    // Intentionally leak this memory.
    // The buffer address must never change, which STL containers don't
    // guarantee. Only ~40 input file names are possible, so cache per input
    // filename and stop worrying.
    const char * const fpath_cstr = _strdup(fpath.c_str());
    {
        boost::lock_guard<boost::mutex> lock(strmap_mutex);
        strmap[input_fname] = fpath_cstr;
    }
    return fpath_cstr;
}

// http://rocky.ess.washington.edu/data/raster/geotiff/docs/manual.txt
// ftp://kratmos.gsfc.nasa.gov/pub/jim/imager/latest_version/TIFF_reader2.c
// http://svn.osgeo.org/fdocore/tags/3.2.x_G052/Thirdparty/GDAL1.3/src/frmts/gtiff/geotiff.cpp
// http://ojs.klaki.net/geotiff2ncdf/gt2nc.c

GeoTiff::GeoTiff(const std::wstring &fname)
    : Tiff(fname), m_gtifhandle(m_tiffhandle),
      m_rawgtif(m_gtifhandle.GetGTIF()),
      m_tiepoints(NULL), m_pixscale(NULL), m_transform(NULL),
      m_ntiepoints(0), m_npixscale(0), m_ntransform(0),
      m_proj(), m_type()
{
    SetCSVFilenameHook(&CSVFileOverride);
    if (!CheckVersion()) {
        throw std::runtime_error("GeoTIFF version not supported.");
    }
    if (!LoadCoordinates()) {
        m_type = RasterMap::TYPE_IMAGE;
    }
};

bool GeoTiff::CheckVersion() const {
    enum GTIFVersion { VERSION = 0, REV_MAJOR, REV_MINOR };
    int versions[3];
    GTIFDirectoryInfo(m_rawgtif, versions, NULL);
    return versions[VERSION] <= GvCurrentVersion &&
           versions[REV_MAJOR] <= GvCurrentRevision;
}

bool GeoTiff::LoadCoordinates() {
    if (!GetKeySingle<geocode_t>(GTModelTypeGeoKey, &m_model)) {
        return false;
    }

    // The rest is not implemented
    if (m_model != ModelTypeProjected && m_model != ModelTypeGeographic) {
        throw std::runtime_error("Map type not supported yet");
    }

    GTIFDefn    defn;
    if(!GTIFGetDefn(m_rawgtif, &defn)) {
        return false;
    }

    std::shared_ptr<char> proj_str(GTIFGetProj4Defn(&defn),
                                   [](char* mem) { GTIFFreeMemory(mem); });
    if (!proj_str)
        return false;

    m_proj = proj_str.get();
    if (!m_proj.length() || !m_proj[0])
        return false;

    using std::tie;
    tie(m_ntiepoints, m_tiepoints) = GetField<double>(TIFFTAG_GEOTIEPOINTS);
    tie(m_npixscale, m_pixscale) = GetField<double>(TIFFTAG_GEOPIXELSCALE);
    tie(m_ntransform, m_transform) = GetField<double>(TIFFTAG_GEOTRANSMATRIX);

    unsigned int sample_fmt = 0;
    TIFFGetFieldDefaulted(m_rawtiff, TIFFTAG_SAMPLEFORMAT, &sample_fmt);
    if (HasKey(VerticalUnitsGeoKey)) {
        m_type = RasterMap::TYPE_DHM;
    } else if (GetSamplesPerPixel() == 1 && GetBitsPerSample() == 16 &&
               sample_fmt == SAMPLEFORMAT_INT)
    {
        // Some DHM's don't set VerticalUnitsGeoKey, unfortunately.
        // Use this heuristic to catch those, SAMPLEFORMAT_INT should be
        // a pretty good indicator.
        m_type = RasterMap::TYPE_DHM;
    } else {
        m_type = RasterMap::TYPE_MAP;
    }

    return true;
}

template <typename T>
std::tuple<unsigned int, std::shared_ptr<T>>
GeoTiff::GetKey(geokey_t key) const
{
    int size;
    tagtype_t type;
    unsigned int count = GTIFKeyInfo(m_rawgtif, key, &size, &type);
    if (!count) {
        return make_tuple(0, std::shared_ptr<T>());
    }
    if (sizeof(T) != size) {
        throw std::runtime_error("GeoTIFF key malformed.");
    }

    std::shared_ptr<T> buffer(new T[count](), ArrayDeleter<T>());
    if (GTIFKeyGet(m_rawgtif, key, buffer.get(), 0, count) != count) {
        throw std::runtime_error("GeoTIFF key read error.");
    }
    return std::make_tuple(count, buffer);
};

template <typename T>
bool GeoTiff::GetKeySingle(geokey_t key, T *output) const {
    auto result = GetKey<T>(key);
    if (std::get<0>(result) != 1) {
        return false;
    }
    *output = *std::get<1>(result).get();
    return true;
}

bool GeoTiff::HasKey(geokey_t key) const {
    return 0 != GTIFKeyInfo(m_rawgtif, key, NULL, NULL);
}

bool GeoTiff::PixelToPCS(double *x, double *y) const {
    if (m_type == RasterMap::TYPE_IMAGE)
        return false;

    if (m_ntiepoints > 6 && m_npixscale == 0) {
        // Interpolate between multiple tiepoints
        if (m_ntiepoints != 4*6) {
            // Currently, we only support 4 tiepoints.
            return false;
        };

        unsigned int w = GetWidth();
        unsigned int h = GetHeight();
        std::unique_ptr<MapPixelDelta> p00, p10, p01, p11;

        for (unsigned int i=0; i < m_ntiepoints; i += 6) {
            if (m_tiepoints[i+0] == 0 && m_tiepoints[i+1] == 0)
                p00.reset(new MapPixelDelta(m_tiepoints[i+3], m_tiepoints[i+4]));
            else if (m_tiepoints[i+0] == w && m_tiepoints[i+1] == 0)
                p10.reset(new MapPixelDelta(m_tiepoints[i+3], m_tiepoints[i+4]));
            else if (m_tiepoints[i+0] == 0 && m_tiepoints[i+1] == h)
                p01.reset(new MapPixelDelta(m_tiepoints[i+3], m_tiepoints[i+4]));
            else if (m_tiepoints[i+0] == w && m_tiepoints[i+1] == h)
                p11.reset(new MapPixelDelta(m_tiepoints[i+3], m_tiepoints[i+4]));
            else {
                // For now, we require tie points to be at the image corners.
                return false;
            }
        }
        if (!p00 || !p10 || !p01 || !p11) {
            return false;
        }
        auto interpolator = BilinearInterpolator(*p00, *p10, *p11, *p01);
        auto res = interpolator.forward(UnitSquareCoord(*x/w, *y/h));
        *x = res.x;
        *y = res.y;
    }
    else if (m_ntransform == 16) {
        // Use matrix for transformation
        double x_in = *x, y_in = *y;
        *x = x_in * m_transform[0] + y_in * m_transform[1] + m_transform[3];
        *y = x_in * m_transform[4] + y_in * m_transform[5] + m_transform[7];
    }
    else if (m_npixscale >= 3 && m_ntiepoints >= 6) {
        // Use one tiepoint + pixscale
        *x = (*x - m_tiepoints[0]) * m_pixscale[0] + m_tiepoints[3];
        *y = (*y - m_tiepoints[1]) * (-1 * m_pixscale[1]) + m_tiepoints[4];
    }
    else {
        throw std::runtime_error("Couldn't find GeoTIFF coordinates.");
    }
    return true;
}

bool GeoTiff::PCSToPixel(double *x, double *y) const {
    if (m_type == RasterMap::TYPE_IMAGE)
        return false;

    if (m_ntiepoints > 6 && m_npixscale == 0) {
        // Interpolate between multiple tiepoints
        if (m_ntiepoints != 4*6) {
            // Currently, we only support 4 tiepoints.
            return false;
        }

        unsigned int w = GetWidth();
        unsigned int h = GetHeight();
        std::unique_ptr<MapPixelDelta> p00, p10, p01, p11;

        for (unsigned int i=0; i < m_ntiepoints; i += 6) {
            if (m_tiepoints[i+0] == 0 && m_tiepoints[i+1] == 0)
                p00.reset(new MapPixelDelta(m_tiepoints[i+3], m_tiepoints[i+4]));
            else if (m_tiepoints[i+0] == w && m_tiepoints[i+1] == 0)
                p10.reset(new MapPixelDelta(m_tiepoints[i+3], m_tiepoints[i+4]));
            else if (m_tiepoints[i+0] == 0 && m_tiepoints[i+1] == h)
                p01.reset(new MapPixelDelta(m_tiepoints[i+3], m_tiepoints[i+4]));
            else if (m_tiepoints[i+0] == w && m_tiepoints[i+1] == h)
                p11.reset(new MapPixelDelta(m_tiepoints[i+3], m_tiepoints[i+4]));
            else {
                // For now, we require tie points to be at the image corners.
                return false;
            }
        }
        if (!p00 || !p10 || !p01 || !p11) {
            return false;
        }
        auto interpolator = BilinearInterpolator(*p00, *p10, *p11, *p01);
        auto res = interpolator.inverse(MapPixelDelta(*x, *y));
        *x = res.x * w;
        *y = res.y * h;
    }
    else if (m_ntransform == 16) {
        // Use inverse matrix for transformation.
        const double *mat = m_transform;
        double x_in = *x, y_in = *y;
        double denom = (mat[0] * mat[5] - mat[1] * mat[4]);
        *x = (+(x_in - mat[3]) * mat[5] - (y_in - mat[7]) * mat[1]) / denom;
        *y = (-(x_in - mat[3]) * mat[4] + (y_in - mat[7]) * mat[0]) / denom;
    }
    else if (m_npixscale >= 3 && m_ntiepoints >= 6) {
        // Use one tiepoint + pixscale
        *x = (*x - m_tiepoints[3]) / m_pixscale[0] + m_tiepoints[0];
        *y = (*y - m_tiepoints[4]) / (-1 * m_pixscale[1]) + m_tiepoints[1];
    }
    else {
        throw std::runtime_error("Couldn't find GeoTIFF coordinates.");
    }
    return true;
}

bool GeoTiff::CheckDHMValid() const {
    short int vert_units;
    if (!GetKeySingle<short int>(VerticalUnitsGeoKey, &vert_units))
        return false;

    // Other vertical formats not supported at the moment
    return vert_units == Linear_Meter;
}

// If we have a DHM map, the pixel values typically represent height in m
// The default TIFFRGBAImageGet turns this (typically 16 bit) value into
// a grayscale rgb image (thus truncating the lower 8 bits).
// Prevent this and simply store the 16 bit value within the 32 bit RGBA.
void GeoTiff::Hook_TIFFRGBAImageGet(TIFFRGBAImage &img) const {
    // Only hook DHM maps
    if (GetType() != RasterMap::TYPE_DHM)
        return;

    if (GetBitsPerSample() != 16) {
        assert(false);  // not implemented
    }
    if (!img.isContig) {
        assert(false);  // not implemented
    }
    if (img.photometric != PHOTOMETRIC_MINISWHITE &&
        img.photometric != PHOTOMETRIC_MINISBLACK)
    {
        assert(false);  // not implemented
    }
    img.put.contig = put16bitbw_DHM;
}

TiffMap::TiffMap(const wchar_t *fname)
    : m_geotiff(new GeoTiff(fname)), m_proj(m_geotiff->GetProj4String())
{}

GeoDrawable::DrawableType TiffMap::GetType() const {
    return m_geotiff->GetType();
}
unsigned int TiffMap::GetWidth() const { return m_geotiff->GetWidth(); };
unsigned int TiffMap::GetHeight() const { return m_geotiff->GetHeight(); };
MapPixelDeltaInt TiffMap::GetSize() const {
    return MapPixelDeltaInt(m_geotiff->GetWidth(), m_geotiff->GetHeight());
}

PixelBuf TiffMap::GetRegion(
                      const MapPixelCoordInt &pos,
                      const MapPixelDeltaInt &size) const
{
    auto fixed_bounds_pb = GetRegion_BoundsHelper(*this, pos, size);
    if (fixed_bounds_pb.GetData())
        return fixed_bounds_pb;

    boost::lock_guard<boost::mutex> lock(m_getregion_mutex);
    return m_geotiff->GetRegion(pos, size);
}

bool TiffMap::PixelToPCS(double *x, double *y) const
    { return m_geotiff->PixelToPCS(x, y); }
bool TiffMap::PCSToPixel(double *x, double *y) const
    { return m_geotiff->PCSToPixel(x, y); }
const std::wstring &TiffMap::GetFname() const
    { return m_geotiff->GetFilename(); }
const std::wstring &TiffMap::GetTitle() const
    { return m_geotiff->GetTitle(); }
const std::wstring &TiffMap::GetDescription() const
    { return m_geotiff->GetDescription(); }
Projection TiffMap::GetProj() const
    { return m_proj; }

bool TiffMap::PixelToLatLon(const MapPixelCoord &pos, LatLon *result) const {
    double x = pos.x;
    double y = pos.y;
    if (!PixelToPCS(&x, &y))
        return false;

    if (m_geotiff->GetModel() == ModelTypeProjected) {
        if (!GetProj().PCSToLatLong(x, y)) {
            return false;
        }
    }
    *result = LatLon(y, x);
    return true;
}

bool TiffMap::LatLonToPixel(const LatLon &pos, MapPixelCoord *result) const {
    double x = pos.lon;
    double y = pos.lat;
    if (m_geotiff->GetModel() == ModelTypeProjected) {
        if (!GetProj().LatLongToPCS(x, y)) {
            return false;
        }
    }
    if (!PCSToPixel(&x, &y))
        return false;

    *result = MapPixelCoord(x, y);
    return true;
}


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

#include "sbacam.h"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "g2o/stuff/macros.h"
#include "g2o/stuff/misc.h"

namespace g2o {

// initialize an object
SBACam::SBACam() {
  setKcam(1, 1, cst(0.5), cst(0.5), 0);  // unit image projection
}

// set the object pose
SBACam::SBACam(const Quaternion& r_, const Vector3& t_) : SE3Quat(r_, t_) {
  Kcam.setZero();
  setTransform();
  setProjection();
  setDr();
}

SBACam::SBACam(const SE3Quat& p) : SE3Quat(p) {
  Kcam.setZero();
  setTransform();
  setProjection();
  setDr();
}

// update from the linear solution
// defined in se3quat
void SBACam::update(const Vector6& update) {
  // position update
  _t += update.head(3);
  // small quaternion update
  Quaternion qr;
  qr.vec() = update.segment<3>(3);
  qr.w() =
      sqrt(cst(1.0) - qr.vec().squaredNorm());  // should always be positive
  _r *= qr;                                     // post-multiply
  _r.normalize();
  setTransform();
  setProjection();
  setDr();
}

// transforms
void SBACam::transformW2F(Eigen::Matrix<number_t, 3, 4>& m,
                          const Vector3& trans, const Quaternion& qrot) {
  m.block<3, 3>(0, 0) = qrot.toRotationMatrix().transpose();
  m.col(3).setZero();  // make sure there's no translation
  Vector4 tt;
  tt.head(3) = trans;
  tt[3] = 1.0;
  m.col(3) = -m * tt;
}

void SBACam::transformF2W(Eigen::Matrix<number_t, 3, 4>& m,
                          const Vector3& trans, const Quaternion& qrot) {
  m.block<3, 3>(0, 0) = qrot.toRotationMatrix();
  m.col(3) = trans;
}

// set up camera matrix
void SBACam::setKcam(number_t fx, number_t fy, number_t cx, number_t cy,
                     number_t tx) {
  Kcam.setZero();
  Kcam(0, 0) = fx;
  Kcam(1, 1) = fy;
  Kcam(0, 2) = cx;
  Kcam(1, 2) = cy;
  Kcam(2, 2) = cst(1.0);
  baseline = tx;
  setTransform();
  setProjection();
  setDr();
}

// sets angle derivatives
void SBACam::setDr() {
  // inefficient, just for testing
  // use simple multiplications and additions for production code in calculating
  // dRdx,y,z
  Matrix3 dRidx, dRidy, dRidz;
  dRidx << cst(0.0), cst(0.0), cst(0.0), cst(0.0), cst(0.0), cst(2.0), cst(0.0),
      cst(-2.0), cst(0.0);
  dRidy << cst(0.0), cst(0.0), cst(-2.0), cst(0.0), cst(0.0), cst(0.0),
      cst(2.0), cst(0.0), cst(0.0);
  dRidz << cst(0.0), cst(2.0), cst(0.0), cst(-2.0), cst(0.0), cst(0.0),
      cst(0.0), cst(0.0), cst(0.0);

  // for dS'*R', with dS the incremental change
  dRdx = dRidx * w2n.block<3, 3>(0, 0);
  dRdy = dRidy * w2n.block<3, 3>(0, 0);
  dRdz = dRidz * w2n.block<3, 3>(0, 0);
}

// human-readable SBACam object
std::ostream& operator<<(std::ostream& out_str, const SBACam& cam) {
  out_str << cam.translation().transpose() << std::endl;
  out_str << cam.rotation().coeffs().transpose() << std::endl << std::endl;
  out_str << cam.Kcam << std::endl << std::endl;
  out_str << cam.w2n << std::endl << std::endl;
  out_str << cam.w2i << std::endl << std::endl;
  return out_str;
}

}  // namespace g2o


//
//=======================================================================
// 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_RELAX_HPP
#define BOOST_GRAPH_RELAX_HPP

#include <functional>
#include <boost/limits.hpp> // for numeric limits
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map.hpp>

namespace boost {

    // The following version of the plus functor prevents
    // problems due to overflow at positive infinity.

    template <class T>
    struct closed_plus
    {
      // std::abs just isn't portable :(
      template <class X>
      inline X my_abs(const X& x) const { return x < 0 ? -x : x; }

      T operator()(const T& a, const T& b) const {
        using namespace std;
        T inf = numeric_limits<T>::max();
        if (b > 0 && my_abs(inf - a) < b)
          return inf;
        return a + b;
      }
    };

    template <class Graph, class WeightMap,
            class PredecessorMap, class DistanceMap,
            class BinaryFunction, class BinaryPredicate>
    bool relax(typename graph_traits<Graph>::edge_descriptor e,
               const Graph& g, WeightMap w,
               PredecessorMap p, DistanceMap d,
               BinaryFunction combine, BinaryPredicate compare)
    {
      typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
      Vertex u = source(e, g), v = target(e, g);
      typedef typename property_traits<DistanceMap>::value_type D;
      typedef typename property_traits<WeightMap>::value_type W;
      D d_u = get(d, u), d_v = get(d, v);
      W w_e = get(w, e);

      if ( compare(combine(d_u, w_e), d_v) ) {
        put(d, v, combine(d_u, w_e));
        put(p, v, u);
        return true;
      } else
        return false;
    }

    template <class Graph, class WeightMap,
      class PredecessorMap, class DistanceMap>
    bool relax(typename graph_traits<Graph>::edge_descriptor e,
               const Graph& g, WeightMap w, PredecessorMap p, DistanceMap d)
    {
      typedef typename property_traits<DistanceMap>::value_type D;
      typedef closed_plus<D> Combine;
      typedef std::less<D> Compare;
      return relax(e, g, w, p, d, Combine(), Compare());
    }

} // namespace boost

#endif /* BOOST_GRAPH_RELAX_HPP */