/* $Id: Graph.cc,v 1.2 2001/06/04 17:32:07 ingo Exp ingo $
   Darstellung als Ajazenslisten */

#include <ClanLib/core.h>
#include <ClanLib/display.h>
#include <ClanLib/application.h>

#include <cstdio>
#include <functional>
#include <algorithm>
#include <list>
#include <queue>

template<class T>
bool element_of (std::list<T>& lst, T el)
{
  for (std::list<T>::iterator i = lst.begin (); i != lst.end (); ++i)
    {
      if (*i == el)
	return true;
    }
  return false;
}

template<class T>
void add_to_list (std::list<T>& lst, T el)
{
  if (!element_of (lst, el))
    lst.push_back (el);
}

template<class T> class GraphNode;
template<class T> class GraphEdge;

template<class T>
class Graph
{
public:
  typedef std::list<GraphNode<T>* > Nodes;
  typedef std::list<GraphEdge<T>* > Edges;
  typedef Nodes::iterator NodeIter;
  typedef Edges::iterator EdgeIter;

private:
  Nodes nodes;
  Edges edges;

public:

  Nodes get_nodes () {
    return nodes;
  }

  Edges get_edges () {
    return edges;
  }
  
  EdgeIter edges_begin () {
    return edges.begin ();
  }

  EdgeIter edges_end () {
    return edges.end ();
  }

  NodeIter nodes_begin () {
    return nodes.begin ();
  }

  NodeIter nodes_end () {
    return nodes.end ();
  }

  /* nodes_begin (), nodes_end ()
   */
  void add_node (GraphNode<T>* node) 
  {
    nodes.push_back (node);
  }
  
  void add_edge (GraphEdge<T>* edge) 
  {
    edges.push_back (edge);
  }

  ostream& print (ostream& out) 
  {
    out << "Graph:\n";
    for (NodeIter i = nodes.begin (); i != nodes.end (); ++i)
      {
	out << "     ";
	(*i)->print (out) << "\n";
      }
    return out;
  }

  GraphNode<T>* find (const T& value) {
    for (NodeIter i = nodes.begin (); i != nodes.end (); ++i)
      {
	if ((*i)->get () == value)
	  return *i;
      }
    return NULL;
  }

  void connect (const T& a, const T& b, double cost = 0.0f) 
  {
    GraphNode<T>* a_node = find (a);
    GraphNode<T>* b_node = find (b);
    if (a_node && b_node)
      {
	GraphEdge<T>* edge = new GraphEdge<T>(a_node, b_node, cost);
	a_node->connect (edge);
      }
  }

  void connect (GraphNode<T>* a, GraphNode<T>* b)
  {
    GraphEdge<T>* edge (new GraphEdge<T>(a, b));
    a->connect (edge);
    edges.push_back (edge);
  }

  void biconnect (const T& a, const T& b, double cost = 0.0f) 
  {
    GraphNode<T>* a_node = find (a);
    GraphNode<T>* b_node = find (b);

    if (a_node && b_node)
      {
	GraphEdge<T>* a_edge = new GraphEdge<T>(a_node, b_node, cost);
	a_node.connect (a_edge);

	GraphEdge<T>* b_edge = new GraphEdge<T>(b_node, a_node, cost);
	b_node.connect (a_edge);
      }
  }

  std::list<GraphNode<T>*>::iterator& begin () {
    return nodes.begin ();
  }
  std::list<GraphNode<T>*>::iterator& end () {
    return nodes.end ();
  }
};


template<class T>
class GraphEdge			
{
private:
  GraphNode<T>* first;
  GraphNode<T>* second;
  double distance;
public:
  GraphEdge (GraphNode<T>* a, GraphNode<T>* b, double d = 0)
    : first(a), second (b), distance (d)
  {
  }

  GraphNode<T>* get_first () { return first; }
  GraphNode<T>* get_second () { return second; }
  
  double get_distance () { return distance; }
};

template<class T>
class GraphNode
{
private:
  Graph<T>::Edges edges;

  T data;

public:
  GraphNode (const T& d) 
    : data (d)
  {
  }

  T& get () { return data; }

  ostream& print (ostream& out) 
  {
    out << "[Node:" << data << "->";
    
    for (Graph<T>::EdgeIter i = edges.begin (); i != edges.end (); ++i)
      {
	out << "(" << (*i)->get_second ()->get () << ", " << (*i)->get_distance() << ")";
      }
    out << "]";
  }
  
  void connect (GraphEdge<T>* edge) 
  {
    if (edge->get_first () != this)
      {
	std::cout << "Node<" << this << ">: Invalide edge: "
		  << "(" << edge->get_first () << ", " << edge->get_second () << ")"
		  << std::endl;
      }
    edges.push_back (edge);
  }

  Graph<T>::EdgeIter edges_begin () {
    return edges.begin ();
  }

  Graph<T>::EdgeIter edges_end () {
    return edges.end ();
  }
};

template<class T> class Graph;

template<class T>
ostream& operator<< (ostream& out, Graph<T> graph) {
  return graph.print (out);
}


template<class T>
class GraphPath
{
public:
  std::list<GraphNode<T> > nodes;
};

template<class T>
GraphPath<T> find_path (GraphNode<T>* start, GraphNode<T>* stop)
{
  priority_queue<GraphNode<T>*> open_nodes;
  open_nodes.push_back(start);
  
  while (!open_nodes.empty ())
    {
      
    }
}


/** A wrapper around a node which can hold all the extra information
    that is needed */
template<class T>
class DijkstraSearchNode
{
private:
  GraphNode<T>* node;
  GraphNode<T>* parent;
  bool is_finished;
  float distance;
   
public:
  DijkstraSearchNode (GraphNode<T>* n, GraphNode<T>* p) 
    : node (n),
      parent (p),
      is_finished (false),
      distance (0.0f)
  {
  }

  GraphNode<T>* get_node () {
    return node;
  }
};

template<class T>
class DijkstraSearch
{
private:
  Graph<T> graph;
  priority_queue<DijkstraSearchNode<T> > open_nodes;
  std::list<GraphNode<T>*> closed_nodes;
  GraphPath<T> path;
  
public:
  DijkstraSearch (Graph<T> graph, GraphNode<T>* start, GraphNode<T>* finish)
  {
    open_nodes.push (DijkstraSearchNode<T>(start, NULL));

    while (!open_nodes.empty ())
      {
	GraphNode<T>* current_node = open_nodes.top ();
	open_nodes.pop ();
	
	if (current_node == finish)
	  return; // Done, we have found a path

	for (Graph<T>::NodeIter i = current_nodesuccessors_begin ();
	     i != current_node->successors_end (); ++i)
	  {
	    if (!node_is_finished(*i))
	      open_nodes.push (DijkstraSearchNode<T>(*i, current_node));
	  }
	return; // No path found
      }
  }

  ~DijkstraSearch ()
  {
  }

  bool node_is_finished (GraphNode<T>* node)
  {
    return element_of (closed_nodes, node);
  }

  GraphPath<T> get_path ()
  {
    return path;
  }
};

template<class T>
class BreathFirstNode
{
public:
  BreathFirstNode (GraphNode<T>* n)
    : node (n)
  {}

  GraphNode<T>* node;
  bool passed;
};

template<class T>
class Queue
{
private:
  std::list<T> lst;
public:
  void push (T& d) {
    lst.push_back (d);
  }

  void pop () {
    lst.pop_front ();
  }

  T& top () {
    return *lst.begin ();
  }

  bool empty () {
    return lst.empty ();
  }

  std::list<T>::size_type size () {
    return lst.size ();
  }

  // Slow!
  bool has (T& t) 
  {
    for (std::list<T>::iterator i = lst.begin (); i != lst.end (); ++i)
      {
	if (*i == t)
	  return true;
      }
    return false;
  }
};

template<class T>
class BreathFirstSearch 
{
private:
  Graph<T>* graph;
  GraphNode<T>* start;
  GraphNode<T>* finish;
  
  Queue<GraphNode<T>*> open_nodes;
  std::map<GraphNode<T>*, bool> closed_nodes;
  std::map<GraphNode<T>*, GraphNode<T>*> parents;
  typedef std::map<GraphNode<T>*, bool>::iterator CNIter;
public:
  BreathFirstSearch (Graph<T>& g, GraphNode<T>* arg_start, GraphNode<T>* arg_finish)
    : graph (&g), start (arg_start), finish (arg_finish)
  {}

  std::list<GraphNode<T>*> search ()
  {
    open_nodes.push (start);
    
    std::cout << "BreathFirstSearch:start" << std::endl;
    while (!open_nodes.empty ())
      {
	GraphNode<T>* cnode = open_nodes.top ();
	open_nodes.pop ();

	closed_nodes[cnode] = true;
	std::cout << "OpenNodes size:" << open_nodes.size () << std::endl;

	if (cnode == finish) 
	  {
	    std::cout << "Found path, constructing path" << std::endl;
	    std::list<GraphNode<T>*> path;
	    
	    GraphNode<T>* cnode = finish;
	    path.push_front (finish);
	    while (cnode != start) {
	      path.push_front (parents[cnode]);
	      cnode = parents[cnode];
	    }

	    std::cout << "Pathsize: " << path.size () << std::endl;

	    return path;
	  }

	for (Graph<T>::EdgeIter i = cnode->edges_begin (); i != cnode->edges_end (); ++i)
	  {
	    GraphNode<T>* next_node = (*i)->get_second ();

	    CNIter a = closed_nodes.find (next_node);

	    if (a == closed_nodes.end ()
		&& !open_nodes.has (next_node))
	      {
		parents[next_node] = cnode;
		open_nodes.push (next_node);
	      }
	  }
      }
    std::cout << "Found no path" << std::endl;
    return std::list<GraphNode<T>*>();
  }
};

/*
int main ()
{
  std::cout << "Graph Test" << std::endl;
  
  Graph<int> graph;

  graph.add_node (new GraphNode<int>(1));
  graph.add_node (new GraphNode<int>(2));
  graph.add_node (new GraphNode<int>(3));
  graph.add_node (new GraphNode<int>(5));
  graph.add_node (new GraphNode<int>(6));
  graph.add_node (new GraphNode<int>(9));

  graph.connect (5, 1, 1.0);
  graph.connect (6, 2, 2.0);
  graph.connect (1, 9, 2.0);
  graph.connect (5, 9, 2.0);
  graph.connect (9, 2, 2.0);
  graph.connect (2, 9, 2.0);
  graph.connect (5, 2, 5.0);
  graph.connect (2, 3, 2.0);

  std::cout << graph << std::endl;

  return 0;
}
*/

class Position
{
public:
  Position (int arg_x, int arg_y)
    : x (arg_x), y (arg_y)
  {
  }
  int x;
  int y;

  Position operator*(double f) {
    return Position (x * f, y * f);
  }

  Position operator-(Position pos) {
    return Position (x - pos.x, y - pos.y);
  }
  Position operator+(Position pos) {
    return Position (pos.x + x, pos.y + y);
  }
};

bool left_click ()
{
  if (CL_Mouse::left_pressed ())
    {
      while (CL_Mouse::left_pressed ())
	CL_System::keep_alive ();
      return true;
    }
  return false;
}

bool right_click ()
{
  if (CL_Mouse::right_pressed ())
    {
      while (CL_Mouse::right_pressed ())
	CL_System::keep_alive ();
      return true;
    }
  return false;
}

void draw_arrow (Position arg_pos1, Position arg_pos2)
{
  CL_Vector pos1 (arg_pos1.x, arg_pos1.y);
  CL_Vector pos2 (arg_pos2.x, arg_pos2.y);

  CL_Vector diff1 = (pos2 - pos1);
  diff1.normalize ();
  diff1 = diff1.rotate (10.0f, CL_Vector (0.0, 0.0, 1.0)) * 10.0f;

  CL_Vector diff2 = (pos2 - pos1);
  diff2.normalize ();
  diff2 = diff2.rotate (-10.0f, CL_Vector (0.0, 0.0, 1.0)) * 10.0f;

  CL_Vector a = pos2 + diff1;
  CL_Vector b = pos2 + diff2;
  
  CL_Display::draw_line (pos1.x, pos1.y, pos2.x, pos2.y, 0.0, 0.0, 1.0);
  CL_Display::draw_line (pos2.x, pos2.y, a.x, a.y, 0.0, 0.0, 1.0);
  CL_Display::draw_line (pos2.x, pos2.y, b.x, b.y, 0.0, 0.0, 1.0);
}

void draw_edge (Position pos1, Position pos2)
{
  draw_arrow (pos1, pos2);
  draw_arrow (pos1, pos1 + ((pos2 - pos1)*0.7));
}

class GraphTest : public CL_ClanApplication
{
public:
  char* get_title () { return "bla"; }
  
  Graph<Position> graph;

  int main (int argc, char* argv[])
  {
    CL_SetupCore::init ();
    CL_SetupDisplay::init ();

    CL_Display::set_videomode (640, 480, 16, false);

    GraphNode<Position>* current_node = 0;
    GraphNode<Position>* first_node = 0;

    GraphNode<Position>* start_node = 0;
    GraphNode<Position>* finish_node = 0;

    std::list<GraphNode<Position>*> path;

    while (!CL_Keyboard::get_keycode (CL_KEY_ESCAPE))
      {
	Graph<Position>::Nodes nodes = graph.get_nodes ();
	Graph<Position>::Edges edges = graph.get_edges ();

	for (Graph<Position>::EdgeIter i = edges.begin (); i != edges.end (); ++i)
	  {
	    Position& pos1 ((*i)->get_first ()->get ());
	    Position& pos2 ((*i)->get_second ()->get ());

	    draw_edge (pos1, pos2);
	  }

	if (start_node)
	  {
	    Position& pos (start_node->get ());
	    CL_Display::fill_rect (pos.x-4, pos.y-4, pos.x+4, pos.y+4, 0.0f, 1.0f, 0.0f, 0.5f);
	  }

	if (finish_node)
	  {
	    Position& pos (finish_node->get ());
	    CL_Display::fill_rect (pos.x-5, pos.y-5, pos.x+5, pos.y+5, 0.0f, 1.0f, 1.0f, 0.5f);
	  }

	for (Graph<Position>::NodeIter i = nodes.begin (); i != nodes.end (); ++i)
	  {
	    Position& pos = (*i)->get ();
	    //std::cout << "Draw node: "  << pos.x << " " << pos.y << std::endl;
	    CL_Display::fill_rect (pos.x-2, pos.y-2, pos.x+2, pos.y+2, 1.0f, 0.0f, 0.0f);
	  }

	if (left_click ())
	  {
	    Position pos (CL_Mouse::get_x (), CL_Mouse::get_y ());
	    std::cout << "New node: "  << pos.x << " " << pos.y << std::endl;
	    graph.add_node (new GraphNode<Position>(pos));
	  }

	if (CL_Keyboard::get_keycode (CL_KEY_A) && current_node)
	  {	    
	    std::cout << "Got start_node" << std::endl;
	    start_node = current_node;
	  }

	if (CL_Keyboard::get_keycode (CL_KEY_SPACE) && start_node && finish_node)
	  {
	    BreathFirstSearch<Position> graph_search(graph, start_node, finish_node);
	    path = graph_search.search ();
	    while (CL_Keyboard::get_keycode (CL_KEY_SPACE))
	      CL_System::keep_alive ();
	  }

	if (CL_Keyboard::get_keycode (CL_KEY_D))
	  {
	    std::swap (start_node, finish_node);
	    while (CL_Keyboard::get_keycode (CL_KEY_D))
	      CL_System::keep_alive ();
	  }

	if (CL_Keyboard::get_keycode (CL_KEY_S) && current_node)
	  {
	    std::cout << "Got end_node" << std::endl;
	    finish_node = current_node;
	  }

	current_node = NULL;
	for (Graph<Position>::NodeIter i = nodes.begin (); i != nodes.end (); ++i)
	  {
	    Position mpos (CL_Mouse::get_x (), CL_Mouse::get_y ());
	    Position& pos = (*i)->get ();
	    if ((pos.x - 5 < mpos.x && pos.x + 5 > mpos.x)
		&& (pos.y - 5 < mpos.y && pos.y + 5 > mpos.y))
	      {
		current_node = (*i);
	      }
	  }

	if (first_node)
	  {
	    CL_Display::draw_line (first_node->get ().x, first_node->get ().y,
				   CL_Mouse::get_x (), CL_Mouse::get_y (),
				   1.0, 1.0, 0.0, 0.5);
	  }

	if (right_click ())
	  {
	    if (current_node  && !first_node)
	      {
		first_node = current_node;
	      }
	    else if (current_node && first_node && first_node != current_node)
	      {
		std::cout << "Connecting" << std::endl;
		graph.connect (first_node, current_node);
		first_node = NULL;
	      }
	    else if (!current_node)
	      {
		first_node = NULL;
	      }
	  }

	if (current_node)
	  {
	    Position& pos = current_node->get ();
	    CL_Display::draw_rect (pos.x-4, pos.y-4, pos.x+4, pos.y+4, 1.0f, 1.0f, 1.0f);
	  }

	if (!path.empty ())
	  {
	    std::list<GraphNode<Position>*>::iterator last = path.begin ();
	    for (std::list<GraphNode<Position>*>::iterator i = last; i != path.end (); ++i)
	      {
		//std::cout << "Drawing line..." << std::endl;
		CL_Display::draw_line ((*last)->get ().x, (*last)->get ().y,
				       (*i)->get ().x, (*i)->get ().y,
				       1.0, 1.0, 1.0);
		last = i;
	      }
	  }

	CL_Display::flip_display ();
	CL_Display::clear_display ();
	CL_System::keep_alive ();
	CL_System::sleep (20);
      }

    CL_SetupDisplay::deinit ();  
    CL_SetupCore::deinit ();
    return 0;
  }
} app;

// EOF //