//
//=======================================================================
// Copyright (c) 2006 Detlef Mages
//
// Distributed under the Boost Software License (all versions). (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//


#include "astar_search.hpp"
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <iostream>

using namespace std;
using namespace boost;


/**
 *  Simple graph with bundled properties
 */

struct VProp {
  VProp() : m_heuristic( 1.0 ), m_distance(.0) {}
  VProp( size_t invar_id, double heuristic )
    : m_heuristic( heuristic ), m_distance( .0 ) {}

  double  m_heuristic;            // a really simple heuristic
  double  m_distance;             // altered by the algo: this id is the distance 
  boost::default_color_type m_color;
};

struct EProp {
  EProp() : m_weight( .0 ) {}
  EProp( double weight ) : m_weight( weight ) {}

  double  m_weight;
};

typedef boost::adjacency_list< boost::vecS, boost::vecS, 
			       boost::undirectedS, VProp, EProp > G;
typedef G::vertex_descriptor V;
typedef G::edge_descriptor E;



/**
 *  Helper classes:
 *    GoalVisitor throws an exeption if the goal is examined (==found)
 *    DistanceHeuristic returns a constant value that is defined in VProp
 */

struct found_goal {};

template< class Vertex, class Edge >
class GoalVisitor : public boost::default_astar_visitor {
public:
  GoalVisitor(Vertex goal) : m_goal(goal) {}

  template <class Graph>
  void examine_vertex(Vertex u, Graph& g) {
    cout << "examine:              " << u
	 << endl;
    if(u == m_goal) throw found_goal();
  }

  template <class Graph>
  void discover_vertex(Vertex u, Graph& g) {
    cout << "discover:             " << u
	 << endl;
  }
      
  template <class Graph>
  void black_target(Edge u, Graph& g) {
    cout << "black_target:         " << boost::source( u, g ) 
	 << " -- " 
	 << boost::target( u, g ) 
	 << endl;
  }

  template <class Graph>
  void edge_relaxed(Edge u, Graph& g) {
    cout << "edge_relaxed:         " << boost::source( u, g ) 
	 << " -- " 
	 << boost::target( u, g ) 
	 << endl;
  }


  template <class Graph>
  void edge_not_relaxed(Edge u, Graph& g) {
    cout << "edge_not_relaxed:     " << boost::source( u, g ) 
	 << " -- " 
	 << boost::target( u, g ) 
	 << endl;
  }

private:
  Vertex m_goal;
};


template< class Graph, class CostType  >
class DistanceHeuristic : public boost::astar_heuristic<Graph, CostType > {
public:
  DistanceHeuristic( Graph & graph ) : m_graph(graph) {}
  
  template< class Vertex >
  CostType operator()( Vertex u) {
    return m_graph[u].m_heuristic;
  }
private:
  Graph & m_graph;
};



/**
 *
 * EdgeWeightWriter is used to output the graph by graphviz_write
 *
 */

class EdgeWeightWriter {
public:
  EdgeWeightWriter( G & graph ): m_graph( graph ) {}
  
  template< class OStream >
  void operator()( OStream& out, V v ) {
    //    out << " [label= \"" << v <<"\"]";
  }
  template< class OStream >
  void operator()( OStream& out, E v ) {
    out << " [label= \"" << m_graph[v].m_weight <<"\"]";
  }
  
private:
  G & m_graph;
};




int main() {
  G g;

  V ar[181];
  for( unsigned int i = 0; i < 5; ++i ) {
    ar[i] = add_vertex( VProp(i, 1.0), g );
  }

  /**
   * This graph is sensible to a horrible case described 
   * in astar_search.hpp happening on x86 machines running 
   * linux
   * If this horrible case is given and Doug Gregor's fix is not applied, 
   * this code results in an infinite loop if compiled with 
   * enabled optimization (-O2)
   */
  add_edge( ar[1], ar[2], EProp( 2.20 ), g );
  add_edge( ar[0], ar[3], EProp( 0.93 ), g );
  add_edge( ar[3], ar[4], EProp( 2.72 ), g );
  add_edge( ar[0], ar[2], EProp( 3.29 ), g );

  map< V, V > predecessor;
  associative_property_map< map< V, V > > predecessorMap( predecessor );
  
  /**
   * solution to this should be: 1->2->0
   */
  try {
    astar_search(g, ar[0], DistanceHeuristic<G, double>( g ), 
		 predecessor_map( predecessorMap ).
		 weight_map( get(&EProp::m_weight, g) ).
		 color_map( get(&VProp::m_color, g) ).
		 distance_map( get(&VProp::m_distance, g) ).
		 visitor(GoalVisitor<V, E>( ar[1] ) ) );
  } catch ( found_goal ) {
    cout << "found" << endl;
    for(V v = ar[1];; v = predecessor[v]) {
      cout << v;
      if(predecessor[v] == v) break;
      cout << "->";
    }
    cout << endl;
  }
  
  boost::write_graphviz( std::cerr, g,
			 EdgeWeightWriter( g ), EdgeWeightWriter( g ),
			 default_writer() );
}