#include "ExpressionGraph.hpp"
#include "RandomUniform.hpp"
#include <boost/functional/hash.hpp>
#include <deque>
#include <cassert>
#include <stack>

namespace sdo
{

bool ExpressionGraph::structural_node_eq::operator()( const Node *a, const Node *b ) const
{
   if( a->op != b->op )
   {
      switch( a->op )
      {
      case G:
         return b->op == LE && a->child1 == b->child2 && a->child2 == b->child1;

      case GE:
         return b->op == L && a->child1 == b->child2 && a->child2 == b->child1;

      case L:
         return b->op == GE && a->child1 == b->child2 && a->child2 == b->child1;

      case LE:
         return b->op == G && a->child1 == b->child2 && a->child2 == b->child1;

      default:
         return false;
      }
   }

   switch( a->op )
   {
   case LOOKUP_TABLE:
      return a->lookup_table == b->lookup_table;

   case CONSTANT:
      return a->value == b->value;

   case TIME:
      return true;

   case RANDOM_UNIFORM:
      return false;

   case IF:
   case DELAY_FIXED:
   case PULSE_TRAIN:
   case RAMP:
      return a->child1 == b->child1 && a->child2 == b->child2 && a->child3 == b->child3;

   case MULT:
   case PLUS:
   case MIN:
   case MAX:
   case EQ:
   case NEQ:
   case OR:
   case AND:
      return ( a->child1 == b->child1 && a->child2 == b->child2 ) ||
             ( a->child2 == b->child1 && a->child1 == b->child2 );

   case MINUS:
   case DIV:
   case G:
   case GE:
   case L:
   case LE:
   case POWER:
   case LOG:
   case MODULO:
   case INTEG:
   case ACTIVE_INITIAL:
   case PULSE:
   case STEP:
   case APPLY_LOOKUP:
      return a->child1 == b->child1 && a->child2 == b->child2;

   case UMINUS:
   case SQRT:
   case EXP:
   case LN:
   case ABS:
   case INTEGER:
   case NOT:
   case SIN:
   case COS:
   case TAN:
   case ARCSIN:
   case ARCCOS:
   case ARCTAN:
   case SINH:
   case COSH:
   case TANH:
   case INITIAL:
      return a->child1 == b->child1;

   case CONTROL:
   case NIL:
      return a == b;
   }

   assert( false );
   return false;
}

std::size_t ExpressionGraph::structural_node_hash::operator()( const Node *node ) const
{
   switch( node->op )
   {
   case LOOKUP_TABLE:
      return hash_value( *( node->lookup_table ) );

   case CONSTANT:
      return boost::hash_value( node->value );

   case TIME:
      return boost::hash_value( TIME );

   case RANDOM_UNIFORM:
      return 0;

   case IF:
   case DELAY_FIXED:
   case PULSE_TRAIN:
   case RAMP:
   {
      std::size_t hash = boost::hash_value( node->op );
      boost::hash_combine( hash, reinterpret_cast<uintptr_t>( node->child1 ) );
      boost::hash_combine( hash, reinterpret_cast<uintptr_t>( node->child2 ) );
      boost::hash_combine( hash, reinterpret_cast<uintptr_t>( node->child3 ) );
      return hash;
   }

   case MULT:
   case PLUS:
   case MIN:
   case MAX:
   case EQ:
   case NEQ:
   case OR:
   case AND:
   {
      std::size_t hash = boost::hash_value( node->op );
      boost::hash_combine( hash, reinterpret_cast<uintptr_t>( node->child1 ) ^
                           reinterpret_cast<uintptr_t>( node->child2 ) );
      return hash;
   }

   case G:
   case LE:
   {
      std::size_t hash = boost::hash_value( G );
      boost::hash_combine( hash, boost::hash_value( LE ) );
      boost::hash_combine( hash, reinterpret_cast<uintptr_t>( node->child1 ) ^
                           reinterpret_cast<uintptr_t>( node->child2 ) );
   }

   case L:
   case GE:
   {
      std::size_t hash = boost::hash_value( L );
      boost::hash_combine( hash, boost::hash_value( GE ) );
      boost::hash_combine( hash, reinterpret_cast<uintptr_t>( node->child1 ) ^
                           reinterpret_cast<uintptr_t>( node->child2 ) );
   }

   case MINUS:
   case DIV:
   case POWER:
   case LOG:
   case MODULO:
   case INTEG:
   case APPLY_LOOKUP:
   case ACTIVE_INITIAL:
   case PULSE:
   case STEP:
   {
      std::size_t hash = boost::hash_value( node->op );
      boost::hash_combine( hash, reinterpret_cast<uintptr_t>( node->child1 ) );
      boost::hash_combine( hash, reinterpret_cast<uintptr_t>( node->child2 ) );
      return hash;
   }

   case UMINUS:
   case SQRT:
   case EXP:
   case LN:
   case ABS:
   case INTEGER:
   case NOT:
   case SIN:
   case COS:
   case TAN:
   case ARCSIN:
   case ARCCOS:
   case ARCTAN:
   case SINH:
   case COSH:
   case TANH:
   case INITIAL:
   {
      std::size_t hash = boost::hash_value( node->op );
      boost::hash_combine( hash, reinterpret_cast<uintptr_t>( node->child1 ) );
      return hash;
   }

   case CONTROL:
   case NIL:
      return boost::hash_value( reinterpret_cast<uintptr_t>( node ) );
   }

   assert( false );

   return -1;
}

void ExpressionGraph::addSymbol( const Symbol &s, ExpressionGraph::Node *node )
{
   auto sym = symbol_table.find( s );

   if( sym != symbol_table.end() )
   {
      //if node is not previously undefined do not change it instead return
      if( sym->second->op != NIL )
         return;

      auto eq_range = node_table.equal_range( sym->second );
      std::vector<Symbol> prev_symbols;

      for( auto it = eq_range.first; it != eq_range.second; ++it )
      {
         if( it->second != s )
         {
            prev_symbols.emplace_back( it->second );
            symbol_table[it->second] = node;
         }
      }

      node_table.erase( eq_range.first, eq_range.second );

      for( auto & sym : prev_symbols )
      {
         node_table.emplace( node, std::move( sym ) );
      }

      substituteTmpNode( sym->second, node );
   }

   symbol_table[s] = node;
   node_table.emplace( node, s );
}

ExpressionGraph::Node *ExpressionGraph::getNode( const Symbol &s )
{
   auto i = symbol_table.find( s );

   if( i != symbol_table.end() )
      return i->second;

   Node *n = createTmpNode();
   symbol_table[s] = n;
   node_table.emplace( n, s );
   return n;
}

ExpressionGraph::Node *ExpressionGraph::getNode( Operator op, Node *child )
{
   Node *a = node_pool_.construct();
   a->child1 = child;
   a->op     = op;
   auto b = nodes_.find( a );

   if( b != nodes_.end() )
   {
      node_pool_.free( a );
      return *b;
   }

   if( child->op == NIL )
      temp_node_usages_.emplace( child, &( a->child1 ) );

   nodes_.emplace( a );
   return a;
}

ExpressionGraph::Node *ExpressionGraph::getNode( Operator op, Node *child1, Node *child2 )
{
   Node *a = node_pool_.construct();
   a->child1 = child1;
   a->child2 = child2;
   a->op     = op;
   auto b = nodes_.find( a );

   if( b != nodes_.end() )
   {
      node_pool_.free( a );
      return *b;
   }

   if( child1->op == NIL )
      temp_node_usages_.emplace( child1, &( a->child1 ) );

   if( child2->op == NIL )
      temp_node_usages_.emplace( child2, &( a->child2 ) );

   nodes_.emplace( a );
   return a;
}

ExpressionGraph::Node *ExpressionGraph::getNode( Operator op, Node *child1, Node *child2, Node *child3 )
{
   Node *a = node_pool_.construct();
   a->child1 = child1;
   a->child2 = child2;
   a->child3 = child3;
   a->op     = op;
   auto b = nodes_.find( a );

   if( b != nodes_.end() )
   {
      node_pool_.free( a );
      return *b;
   }

   if( child1 && child1->op == NIL )
      temp_node_usages_.emplace( child1, &( a->child1 ) );

   if( child2 && child2->op == NIL )
      temp_node_usages_.emplace( child2, &( a->child2 ) );

   if( child3 && child3->op == NIL )
      temp_node_usages_.emplace( child3, &( a->child3 ) );

   nodes_.emplace( a );
   return a;
}

ExpressionGraph::Node *ExpressionGraph::getNode( double val )
{
   Node *a = node_pool_.construct();
   a->op    = CONSTANT;
   a->value = val;
   a->type  = CONSTANT_NODE;
   a->init = CONSTANT_INIT;
   a->level = 0;

   if( !unique_constants )
   {
      auto b = nodes_.find( a );

      if( b != nodes_.end() )
      {
         node_pool_.free( a );
         return *b;
      }
   }

   nodes_.emplace( a );
   return a;
}

ExpressionGraph::Node *ExpressionGraph::getTimeNode()
{
   Node *a = node_pool_.construct();
   a->op = TIME;
   auto b = nodes_.find( a );

   if( b != nodes_.end() )
   {
      node_pool_.free( a );
      return *b;
   }

   nodes_.emplace( a );
   return a;
}

LookupTable *ExpressionGraph::createLookupTable()
{
   return lookup_pool_.construct();
}

ExpressionGraph::Node *ExpressionGraph::createTmpNode()
{
   Node *n = node_pool_.construct();
   n->op = NIL;
   return n;
}

void ExpressionGraph::analyze()
{
   std::deque<Node *> node_deque;
   Node *initial_time_node = getNode( Symbol( "INITIAL TIME" ) );
   Node *final_time_node   = getNode( Symbol( "FINAL TIME" ) );
   Node *time_step_node    = getNode( Symbol( "TIME STEP" ) );

   for( std::pair<const Symbol, Node *> &p : symbol_table )
   {
      if( p.second->op == INTEG )
      {
         node_deque.push_back( p.second );
      }
      else if( p.second != initial_time_node && p.second != final_time_node && p.second != time_step_node )
      {
         node_deque.push_front( p.second );
      }
   }

   node_deque.push_back( initial_time_node );
   node_deque.push_back( final_time_node );
   node_deque.push_back( time_step_node );
   node_deque.push_back( getTimeNode() );

   while( !node_deque.empty() )
   {
      Node *node = node_deque.back();

      if( node->type != UNKNOWN )
      {
         node_deque.pop_back();
         continue;
      }

      switch( node->op )
      {
      case INTEG:
         if( node->child2->init == UNKNOWN_INIT )
         {
            node_deque.push_back( node->child2 );
            continue;
         }

         node->type  = DYNAMIC_NODE;
         node->level = node->child2->level + 1;
         node->init  = node->child2->init;

         if( node->init == CONSTANT_INIT )
         {
            node->value = node->child2->value;
         }

         node_deque.pop_back();
         node_deque.push_front( node->child1 );
         continue;

      case IF:
      {
         node->type  = NodeType( node->child1->type | node->child2->type | node->child3->type );
         node->init  = InitialType( node->child1->init | node->child2->init | node->child3->init );
         node->level = std::max( {node->child1->level, node->child2->level, node->child3->level} ) + 1;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = node->child1->value ? node->child2->value : node->child3->value;
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child3 );
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
         }

         continue;
      }

      case ACTIVE_INITIAL:
      {
         node->type  = NodeType( node->child2->type | node->child1->type );
         node->init  = node->child2->init;
         node->level = node->child2->level;

         if( node->init == UNKNOWN_INIT )
         {
            node_deque.push_back( node->child2 );
            continue;
         }

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            if( node->init == CONSTANT_INIT )
            {
               node->value = node->child2->value;
            }

            node->level = std::max( node->child1->level + 1, node->child2->level );
            node_deque.pop_back();
            continue;

         case CONSTANT_NODE:
            error( node->usages, "Use of ACTIVE INITIAL while active equation is constant." );
            node->level = std::max( node->child1->level + 1, node->child2->level );
            node->value = node->child1->value;
            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.pop_back();
            node_deque.push_front( node->child1 );
            node_deque.push_front( node );
         }

         continue;
      }

      case INITIAL:
         if( node->child1->init == UNKNOWN_INIT )
         {
            node_deque.push_back( node->child1 );
            continue;
         }

         node->level = node->child1->level;

         if( node->child1->init == CONSTANT_INIT )
         {
            node->type  = CONSTANT_NODE;
            node->init  = CONSTANT_INIT;
            node->value = node->child1->value;
         }
         else
         {
            node->type = DYNAMIC_NODE;
            node->init = CONTROLED_INIT;
         }

         node_deque.pop_back();
         continue;

      case DELAY_FIXED:
      {
         if( node->child3->type == UNKNOWN )
            node->type = UNKNOWN;
         else
            node->type = NodeType( node->child1->type | node->child2->type | STATIC_NODE );

         if( node->type != UNKNOWN )
         {
            node->init = node->child3->init;

            if( node->child1->type == CONSTANT_NODE )
               warning( node->usages,
                        "DELAY FIXED used with constant input. Consider using STEP instead." );

            if( node->child2->type != CONSTANT_NODE )
               warning( node->usages,
                        "DELAY FIXED used with non constant delay time. Only initial value will be used." );
         }

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level     = std::max( {node->child1->level, node->child2->level, node->child3->level, getTimeNode()->level} ) + 1;

            if( node->init == CONSTANT_INIT )
               node->value = node->child3->value;

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child3 );
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;

         case CONSTANT_NODE:
            assert( false );
         }

         continue;
      }

      case PULSE:
      {
         node->type = NodeType( node->child1->type | node->child2->type | STATIC_NODE );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
            error( node->usages, "Using PULSE with non constant arguments" );

         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level, getTimeNode()->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
               double initial_time = initial_time_node->value;
               double time_step    = time_step_node->value;
               double time_plus    = initial_time + 0.5 * time_step;
               double start        = node->child1->value;
               double width        = node->child2->value;
               node->value          = time_plus > start && time_plus < start + width ? 1. : 0.;
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;

         case CONSTANT_NODE:
            assert( false );
         }

         continue;
      }

      case PULSE_TRAIN:
      {
         node->type = NodeType(
                         node->child1->child1->type | node->child1->child2->type | node->child2->type | node->child3->type | STATIC_NODE );
         node->init = InitialType(
                         node->child1->child1->init | node->child1->child2->init | node->child2->init | node->child3->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
            error( node->usages, "Using PULSE TRAIN with non constant arguments" );

         case STATIC_NODE:
            node->level = std::max( {node->child1->child1->level, node->child1->child2->level, node->child2->level,
                                     node->child3->level, getTimeNode()->level
                                    } ) + 1;

            if( node->init == CONSTANT_INIT )
            {
               double initial_time = initial_time_node->value;
               double time_step    = time_step_node->value;
               double time_plus    = initial_time + 0.5 * time_step;
               double start        = node->child1->child1->value;
               double width        = node->child1->child2->value;
               double tbetween     = node->child2->value;
               double end          = node->child3->value;

               if( tbetween < width )
               {
                  node->value = time_plus > start && time_plus < end ? 1. : 0.;
               }
               else
               {
                  node->value = time_plus > start && time_plus < start + width ? 1. : 0.;
               }
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child3 );
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1->child2 );
            node_deque.push_back( node->child1->child1 );
            continue;

         case CONSTANT_NODE:
            assert( false );
         }

         continue;
      }

      case STEP:
      {
         node->type           = NodeType( node->child1->type | node->child2->type | STATIC_NODE );
         double initial_time = initial_time_node->value;

         if( node->type != UNKNOWN )
         {
            if( node->child2->type != CONSTANT_NODE )
            {
               error( node->usages, "STEP used with non constant step time" );
            }
            else if( node->child2->value <= initial_time )
            {
               warning( node->usages,
                        "Usage of STEP has no effect because step time is at or before initial time" );
               node->init = node->child1->init;

               if( node->init == CONSTANT_INIT )
               {
                  node->value = node->child1->value;
               }
            }
            else
            {
               node->init  = CONSTANT_INIT;
               node->value = 0.0;
            }
         }

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level, getTimeNode()->level} ) + 1;
            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;

         case CONSTANT_NODE:
            assert( false );
         }

         continue;
      }

      case RAMP:
      {
         node->type = NodeType( node->child1->type | node->child2->type | node->child3->type | STATIC_NODE );

         if( node->type != UNKNOWN )
         {
            int non_const_count = ( node->child1->type != CONSTANT_NODE ) +
                                  ( node->child2->type != CONSTANT_NODE ) +
                                  ( node->child3->type != CONSTANT_NODE );

            if( non_const_count )
            {
               std::string msg {"Use of RAMP with "};

               switch( non_const_count )
               {
               case 3:
                  msg += "arguments one, two and three";
                  break;

               case 2:
               {
                  msg += "arguments ";

                  if( node->child1->type == CONSTANT_NODE )
                     msg += "two and three";

                  if( node->child2->type == CONSTANT_NODE )
                     msg += "one and three";

                  if( node->child3->type == CONSTANT_NODE )
                     msg += "one and two";

                  break;
               }

               case 1:
               {
                  msg += "argument ";

                  if( node->child1->type != CONSTANT_NODE )
                     msg += "one";

                  if( node->child2->type != CONSTANT_NODE )
                     msg += "two";

                  if( node->child3->type != CONSTANT_NODE )
                     msg += "three";
               }
               }

               msg += " not constant";
               error( node->usages, msg );
            }

            node->init  = CONSTANT_INIT;
            node->value = 0.0;
            node->level = std::max( {node->child1->level, node->child2->level, node->child3->level, getTimeNode()->level} ) + 1;;
            node_deque.pop_back();
            continue;
         }

         node_deque.push_back( node->child3 );
         node_deque.push_back( node->child2 );
         node_deque.push_back( node->child1 );
         continue;
      }

      case RANDOM_UNIFORM:
      {
         node->type = NodeType( node->child1->type | node->child2->type | STATIC_NODE );

         switch( node->type )
         {
         case DYNAMIC_NODE:
            error( node->usages, "RANDOM UNIFORM used with non constant arguments." );

         case STATIC_NODE:
            node->init  = CONSTANT_INIT;
            node->value = sdo::random_uniform( node->child1->value, node->child2->value );
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;
            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;

         case CONSTANT_NODE:
            assert( false );
         }
      };

      case PLUS:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = node->child1->value + node->child2->value;
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;

         }
      }

      case MINUS:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = node->child1->value - node->child2->value;
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case MULT:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = node->child1->value * node->child2->value;
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case DIV:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = node->child1->value / node->child2->value;
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;

         }
      }

      case G:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = node->child1->value > node->child2->value;
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case GE:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = node->child1->value >= node->child2->value;
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case L:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = node->child1->value < node->child2->value;
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;

         }
      }

      case LE:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = node->child1->value <= node->child2->value;
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;

         }
      }

      case EQ:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = node->child1->value == node->child2->value;
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;

         }
      }

      case NEQ:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = node->child1->value != node->child2->value;
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case AND:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = node->child1->value && node->child2->value;
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case OR:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = node->child1->value || node->child2->value;
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;

         }
      }

      case POWER:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::pow( node->child1->value, node->child2->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case LOG:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::log( node->child1->value ) / std::log( node->child2->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case MIN:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::min( node->child1->value, node->child2->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;

         }
      }

      case MAX:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::max( node->child1->value, node->child2->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;

         }
      }

      case MODULO:
      {
         node->type = NodeType( node->child1->type | node->child2->type );
         node->init = InitialType( node->child1->init | node->child2->init );

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = std::max( {node->child1->level, node->child2->level} ) + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = node->child1->value -
                             node->child2->value * std::floor( node->child1->value / node->child2->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case UMINUS:
      {
         node->type = node->child1->type;
         node->init = node->child1->init;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child1->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = -node->child1->value;
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case SQRT:
      {
         node->type = node->child1->type;
         node->init = node->child1->init;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child1->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::sqrt( node->child1->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case EXP:
      {
         node->type = node->child1->type;
         node->init = node->child1->init;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child1->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::exp( node->child1->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case LN:
      {
         node->type = node->child1->type;
         node->init = node->child1->init;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child1->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::log( node->child1->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case ABS:
      {
         node->type = node->child1->type;
         node->init = node->child1->init;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child1->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::abs( node->child1->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case INTEGER:
      {
         node->type = node->child1->type;
         node->init = node->child1->init;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child1->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::floor( node->child1->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case NOT:
      {
         node->type = node->child1->type;
         node->init = node->child1->init;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child1->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = !( node->child1->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case SIN:
      {
         node->type = node->child1->type;
         node->init = node->child1->init;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child1->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::sin( node->child1->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case COS:
      {
         node->type = node->child1->type;
         node->init = node->child1->init;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child1->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::cos( node->child1->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case TAN:
      {
         node->type = node->child1->type;
         node->init = node->child1->init;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child1->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::tan( node->child1->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case ARCSIN:
      {
         node->type = node->child1->type;
         node->init = node->child1->init;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child1->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::asin( node->child1->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case ARCCOS:
      {
         node->type = node->child1->type;
         node->init = node->child1->init;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child1->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::acos( node->child1->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case ARCTAN:
      {
         node->type = node->child1->type;
         node->init = node->child1->init;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child1->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::atan( node->child1->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case SINH:
      {
         node->type = node->child1->type;
         node->init = node->child1->init;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child1->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::sinh( node->child1->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case COSH:
      {
         node->type = node->child1->type;
         node->init = node->child1->init;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child1->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::cosh( node->child1->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case TANH:
      {
         node->type = node->child1->type;
         node->init = node->child1->init;

         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child1->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = std::tanh( node->child1->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child1 );
            continue;
         }
      }

      case TIME:
      {
         node->type  = STATIC_NODE;
         node->init  = CONSTANT_INIT;
         node->level = 1;
         node->value = initial_time_node->value;
         node_deque.pop_back();
         continue;
      }

      case CONSTANT:
      {
         assert( false );
         node_deque.pop_back();
         continue;
      }

      case CONTROL:
      {
         node->type  = DYNAMIC_NODE;
         node->init = CONTROLED_INIT;
         node->level = 0;
         node_deque.pop_back();
         continue;
      }

      case APPLY_LOOKUP:
      {
         if( node->child1->op != LOOKUP_TABLE )
         {
            error( node->child1->usages, "Symbol not a lookup table" );
         }

         node->type = node->child2->type;
         node->init = node->child2->init;

         bool yConst = true;

         auto yVals = node->child1->lookup_table->getYvals();
         double oldYval = yVals[0];
         double eps = 1e-9;
         for (auto yVal : yVals)
         {
            if( oldYval - eps >= yVal || yVal >= oldYval + eps)
            {
               yConst = false;
               break;
            }
            oldYval = yVal;
         }
         if (yConst == true)
         {
            printf("Detected constant lookup with value %f\n", oldYval);

            node->value = oldYval;
            node->type = CONSTANT_NODE;
            node->init = CONSTANT_INIT;
            continue;
         }
         switch( node->type )
         {
         case DYNAMIC_NODE:
         case STATIC_NODE:
            node->level = node->child2->level + 1;

            if( node->init == CONSTANT_INIT )
            {
            case CONSTANT_NODE:
               node->value = node->child1->lookup_table->operator()( node->child2->value );
            }

            node_deque.pop_back();
            continue;

         case UNKNOWN:
            node_deque.push_back( node->child2 );
            continue;
         }
      }

      case LOOKUP_TABLE:
      {
         assert( false );
         node_deque.pop_back();
         continue;
      }

      case NIL:
      {
         auto s = getSymbol( node );
         std::string msg;

         if( !s.empty() )
         {
            msg += "Use of undefined symbol '";
            msg += s.begin()->second.get();
            msg += "'";
         }
         else
         {
            msg += "Something has gone terribly wrong. NIL node found but it has no symbol attached";
         }

         error( node->usages, msg );
         node->type  = CONSTANT_NODE;
         node->value = 0.0;
         node->init  = CONSTANT_INIT;
         node->level = 0;
         node_deque.pop_back();
         continue;
      }
      }
   }

   if( initial_time_node->type != CONSTANT_NODE )
      error( initial_time_node->usages, "INITIAL TIME is not constant" );

   if( time_step_node->type != CONSTANT_NODE )
      error( time_step_node->usages, "TIME STEP is not constant" );

   if( final_time_node->type != CONSTANT_NODE )
      error( final_time_node->usages, "FINAL TIME is not constant" );

   if( hasErrors() )
      throw parse_error( *this );
}

void ExpressionGraph::useUniqueConstants( bool val )
{
   unique_constants = val;
}

void ExpressionGraph::substituteTmpNode( ExpressionGraph::Node *tmp, ExpressionGraph::Node *subst )
{
   auto eq_range = temp_node_usages_.equal_range( tmp );
   subst->usages.insert( subst->usages.end(), tmp->usages.begin(), tmp->usages.end() );

   if( eq_range.first != eq_range.second )
   {
      if( subst->op == NIL )
      {
         std::vector<Node **> usages;

         for( auto i = eq_range.first; i != eq_range.second; ++i )
         {
            *( i->second ) = subst;
            usages.emplace_back( i->second );
         }

         temp_node_usages_.erase( eq_range.first, eq_range.second );

         for( Node **usg : usages )
            temp_node_usages_.emplace( subst, usg );
      }
      else
      {
         for( auto i = eq_range.first; i != eq_range.second; ++i )
            *( i->second ) = subst;

         temp_node_usages_.erase( eq_range.first, eq_range.second );
      }

      node_pool_.free( tmp );
   }
}

ExpressionGraph::Node *ExpressionGraph::getNode( LookupTable *table )
{
   Node *a = node_pool_.construct();
   a->op           = LOOKUP_TABLE;
   a->lookup_table = table;
   a->type         = CONSTANT_NODE;
   a->level        = 0;
   auto b = nodes_.find( a );

   if( b != nodes_.end() )
   {
      node_pool_.free( a );
      lookup_pool_.destroy( table );
      return *b;
   }

   nodes_.emplace( a );
   return a;
}

double ExpressionGraph::evaluateNode( const Node *node, double time, bool initial ) const
{
   assert( node->type == STATIC_NODE || node->type == CONSTANT_NODE );
   std::stack<const Node *> nodes;
   nodes.push( nullptr );
   std::stack<double> vals;
   std::stack<int> valsoffset;
   valsoffset.push(0);

   auto push_ternary = [&]()
   {
      nodes.push( node );
      valsoffset.push(vals.size() + 2);
      nodes.push( node->child1 );
      valsoffset.push(vals.size() + 1);
      nodes.push( node->child2 );
      valsoffset.push(vals.size());
      node = node->child3;
   };

   auto push_binary = [&]()
   {
      nodes.push( node );
      valsoffset.push(vals.size() + 1);
      nodes.push( node->child1 );
      valsoffset.push(vals.size());
      node = node->child2;
   };

   auto push_unary = [&]()
   {
      nodes.push( node );
      valsoffset.push(vals.size());
      node = node->child1;
   };

   auto pop_node = [&]()
   {
      node = nodes.top();
      valsoffset.pop();
      nodes.pop();
   };

   double time_step = symbol_table.find( Symbol( "TIME STEP" ) )->second->value;
   double time_plus = time + time_step / 2;
   do
   {
      if( node->type == CONSTANT_NODE )
      {
         vals.push( node->value );
         pop_node();
      }
      else
      {
         switch( node->op )
         {
         case DELAY_FIXED:
            //TODO:
            continue;

         case CONTROL:
         case LOOKUP_TABLE:
         case NIL:
         case CONSTANT:
         case INTEG:
            assert( false );
            continue;

         case SQRT:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               push_unary();
            }
            else
            {
               vals.top() = std::sqrt( vals.top() );
               pop_node();
            }

            continue;

         case UMINUS:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               push_unary();
            }
            else
            {
               vals.top() = -vals.top();
               pop_node();
            }

            continue;

         case ACTIVE_INITIAL:
            if( initial )
               node = node->child2;
            else
               node = node->child1;

            continue;

         case APPLY_LOOKUP:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               nodes.push( node );
               valsoffset.push(vals.size());
               node = node->child2;
            }
            else
            {
               auto &lkptbl = *( node->child1->lookup_table );
               vals.top() = lkptbl( vals.top() );
               pop_node();
            }

            continue;

         case ARCCOS:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               push_unary();
            }
            else
            {
               vals.top() = std::acos( vals.top() );
               pop_node();
            }

            continue;

         case ARCSIN:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               push_unary();
            }
            else
            {
               vals.top() = std::asin( vals.top() );
               pop_node();
            }

            continue;

         case ARCTAN:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               push_unary();
            }
            else
            {
               vals.top() = std::atan( vals.top() );
               pop_node();
            }

            continue;

         case SIN:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               push_unary();
            }
            else
            {
               vals.top() = std::sin( vals.top() );
               pop_node();
            }

            continue;

         case COS:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               push_unary();
            }
            else
            {
               vals.top() = std::cos( vals.top() );
               pop_node();
            }

            continue;

         case TAN:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               push_unary();
            }
            else
            {
               vals.top() = std::tan( vals.top() );
               pop_node();
            }

            continue;

         case COSH:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               push_unary();
            }
            else
            {
               vals.top() = std::cosh( vals.top() );
               pop_node();
            }

            continue;

         case SINH:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               push_unary();
            }
            else
            {
               vals.top() = std::sinh( vals.top() );
               pop_node();
            }

            continue;

         case TANH:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               push_unary();
            }
            else
            {
               vals.top() = std::tanh( vals.top() );
               pop_node();
            }

            continue;

         case DIV:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = ( a / vals.top() );
               pop_node();
            }

            continue;

         case EQ:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = ( a == vals.top() );
               pop_node();
            }

            continue;

         case EXP:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               push_unary();
            }
            else
            {
               vals.top() = std::exp( vals.top() );
               pop_node();
            }

            continue;

         case ABS:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               push_unary();
            }
            else
            {
               vals.top() = std::abs( vals.top() );
               pop_node();
            }

            continue;

         case AND:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = a && vals.top();
               pop_node();
            }

            continue;

         case G:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = a > vals.top();
               pop_node();
            }

            continue;

         case GE:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = a >= vals.top();
               pop_node();
            }

            continue;

         case IF:
            if( int(vals.size()) - valsoffset.top() < 3 )
            {
               push_ternary();
            }
            else
            {
               double cond = vals.top();
               vals.pop();
               double thenval = vals.top();
               vals.pop();
               vals.top() = cond ? thenval : vals.top();
               pop_node();
            }

            continue;

         case INITIAL:
            vals.push( node->value );
            pop_node();
            continue;

         case TIME:
            vals.push( time );
            pop_node();
            continue;

         case INTEGER:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               push_unary();
            }
            else
            {
               vals.top() = std::floor( vals.top() );
               pop_node();
            }

            continue;

         case L:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = a < vals.top();
               pop_node();
            }

            continue;

         case LE:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = a <= vals.top();
               pop_node();
            }

            continue;

         case LN:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               push_unary();
            }
            else
            {
               vals.top() = std::log( vals.top() );
               pop_node();
            }

            continue;

         case LOG:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = std::log( a ) / std::log( vals.top() );
               pop_node();
            }

            continue;

         case MAX:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = std::max( a, vals.top() );
               pop_node();
            }

            continue;

         case MIN:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = std::min( a, vals.top() );
               pop_node();
            }

            continue;

         case MINUS:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = a - vals.top();
               pop_node();
            }

            continue;

         case MODULO:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = std::fmod( a, vals.top() );
               pop_node();
            }

            continue;

         case MULT:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = a * vals.top();
               pop_node();
            }

            continue;

         case NEQ:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = a != vals.top();
               pop_node();
            }

            continue;

         case NOT:
            if( int(vals.size()) - valsoffset.top() < 1 )
            {
               push_unary();
            }
            else
            {
               vals.top() = !vals.top();
               pop_node();
            }

            continue;

         case OR:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = a || vals.top();
               pop_node();
            }

            continue;

         case PLUS:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = a + vals.top();
               pop_node();
            }

            continue;

         case POWER:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = std::pow( a, vals.top() );
               pop_node();
            }

            continue;

         case PULSE:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double start = vals.top();
               vals.pop();
               double width = std::max( time_step, vals.top() );
               vals.top() = ( time_plus > start ) && ( time_plus < start + width ) ? 1 : 0;
               pop_node();
            }

            continue;

         case PULSE_TRAIN:
            if( int(vals.size()) - valsoffset.top() < 4 )
            {
               nodes.push( node );
               valsoffset.push(vals.size() + 3);
               nodes.push( node->child1->child1 );
               valsoffset.push(vals.size() + 2);
               nodes.push( node->child1->child2 );
               valsoffset.push(vals.size() + 1);
               nodes.push( node->child2 );
               valsoffset.push(vals.size());
               node = node->child3;
            }
            else
            {
               pop_node();
               double start = vals.top();
               vals.pop();
               double width = std::max( time_step, vals.top() );
               vals.pop();
               double tbetween = vals.top();
               vals.pop();
               double end = vals.top();

               if( time_plus < start || end < time_plus )
               {
                  vals.top() = 0;
                  continue;
               }

               if( tbetween < width )
               {
                  vals.top() = 1;
                  continue;
               }

               double tmodplus = std::fmod( time_plus, tbetween );
               double smod = std::fmod( start, tbetween );

               vals.top() = ( tmodplus > smod ) && ( tmodplus < smod + width ) ? 1 : 0;
            }

            continue;

         case ExpressionGraph::STEP:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double height = vals.top();
               vals.pop();
               vals.top() = time_plus > vals.top() ? height : 0;
               pop_node();
            }

            continue;

         case ExpressionGraph::RAMP:
            if( int(vals.size()) - valsoffset.top() < 3 )
            {
               push_ternary();
            }
            else
            {
               double slope = vals.top();
               vals.pop();
               double start_time = vals.top();
               vals.pop();
               vals.top() = time > start_time ?
                            ( time < vals.top() ?
                              slope * ( time - start_time ) :
                              slope * ( vals.top() - start_time ) )
                            : 0;
               pop_node();
            }

            continue;

         case ExpressionGraph::RANDOM_UNIFORM:
            if( int(vals.size()) - valsoffset.top() < 2 )
            {
               push_binary();
            }
            else
            {
               double a = vals.top();
               vals.pop();
               vals.top() = sdo::random_uniform( a, vals.top() );
               pop_node();
            }

            continue;
         }
      }
   }
   while( node );
   assert(nodes.empty());
   assert(vals.size() == 1 && valsoffset.empty());
   return vals.top();
}

}


#include <stdexcept>
#include <sstream>
#include <limits>
#include <iostream>
#include <cmath>
#include <stdlib.h>
#include <boost/lexical_cast.hpp>
#include <sstream>

#include "shortest_paths.h"
#include "fheap.h"

using namespace std;

shortest_paths::shortest_paths() :
    sssp(sssp_automatic),
    apsp(apsp_automatic),
    integrity_check(true),
    negative_edge_exists(false),
    pairwise_computation_done(false),
    pairwise_tracking_done(false),
    verbose(true),
    directed(true){

}

void shortest_paths::add_node(unsigned int index){

    node to_add = { index };
    if(integrity_check){
        if(node_index_map.find(index) != node_index_map.end()){
            stringstream error;
            error << "node index " << index
                << " occured at least twice";
            throw invalid_argument(error.str());
        }
    }
    node_index_map[index] = nodes.size();
    nodes.push_back(to_add);
    sparse_adjacency_matrix.push_back(map<unsigned int, unsigned int>());
    sparse_transposed_adjacency_matrix.push_back(map<unsigned int, unsigned int>());
    pairwise_computation_done = false;
    pairwise_tracking_done = false;

}

void shortest_paths::add_edge(
        unsigned int index,
        unsigned int source,
        unsigned int target,
        double weight){

    edge to_add = { index, source, target, weight };

    if(integrity_check){
        if(edge_index_map.find(index) != edge_index_map.end()){
            stringstream error;
            error << "edge index " << index
                << " occured at least twice";
            throw invalid_argument(error.str());
        }
        if(node_index_map.find(source) == node_index_map.end()){
            stringstream error;
            error << "source node " << source << " does not exist";
            throw invalid_argument(error.str());
        }
        if(node_index_map.find(target) == node_index_map.end()){
            stringstream error;
            error << "target node " << target << " does not exist";
            throw invalid_argument(error.str());
        }
    }

    unsigned int mapped_source = node_index_map[source];
    unsigned int mapped_target = node_index_map[target];
    unsigned int mapped_index = edges.size();

    map<unsigned int, unsigned int>::iterator old_edge =
        sparse_adjacency_matrix[mapped_source].find(mapped_target);

    // It may be that we have multi edges. In that case, we use the
    // edge with the lowest weight.
    if(old_edge == sparse_adjacency_matrix[mapped_source].end() ||
            edges[old_edge->second].weight > weight){

        sparse_adjacency_matrix[mapped_source][mapped_target] = mapped_index;
        sparse_transposed_adjacency_matrix[mapped_target][mapped_source] = mapped_index;

        if(weight < 0){
            negative_edge_exists = true;
        }

        pairwise_computation_done = false;
        pairwise_tracking_done = false;

    }

    edge_index_map[index] = mapped_index;
    edges.push_back(to_add);

}

void shortest_paths::set_integrity_check(bool integrity_check){

    this->integrity_check = integrity_check;

}

void shortest_paths::graph_from_file(
        const char* filename_nodes,
        const char* filename_edges){

    ifstream nodes_stream(filename_nodes, ios::in),
         edges_stream(filename_edges, ios::in);

    unsigned int node_index,

        edge_index,
        source,
        target,

        line_in_file;

    float weight;

    string line_buffer;

    // let's do the nodes
    if(nodes_stream.fail()){
        stringstream error;
        error << "unable to open file \""
            << filename_nodes << "\" for reading";
        throw invalid_argument(error.str());
    }

    try{
        for(line_in_file=1; getline(nodes_stream, line_buffer);
                line_in_file++){

            // A single whitespace in the template allows any number
            // of whitespaces in the scanned file, including none,
            // so don't remove them!
            if(sscanf(line_buffer.c_str(), " %u ", &node_index) < 1){
                stringstream error;
                error << "malformed input: missing node index";
                throw invalid_argument(error.str());
            }

            add_node(node_index);
        }

    }

    catch(exception e){

        stringstream error;
        error << "line: " << line_in_file << ", file: \""
            << filename_nodes << "\", " << e.what();
        throw invalid_argument(error.str());

    }

    nodes_stream.close();

    if(nodes.size()==0){
        stringstream error;
        error << "file \"" << filename_nodes
            << "\" did not contain any nodes";
        throw invalid_argument(error.str());
    }

    // let's do the edges
    if(edges_stream.fail()){
        stringstream error;
        error << "unable to open file \""
            << filename_edges << "\" for reading";
        throw invalid_argument(error.str());
    }

    try{
        for(line_in_file=1; getline(edges_stream, line_buffer);
                line_in_file++){

            // A single whitespace in the template allows any number
            // of whitespaces in the scanned file, including none,
            // so don't remove them!
            if(sscanf(line_buffer.c_str(), " %u ; %u ; %u ; %f",
                        &edge_index, &source, &target, &weight) < 4){
                stringstream error;
                error << "malformed input: must be like "
                    << "\"index; source; target; weight\"";
                throw invalid_argument(error.str());
            }

            add_edge(edge_index, source, target, (double)weight);
        }

    }

    catch(exception e){

        stringstream error;
        error << "line: " << line_in_file << ", file: \""
            << filename_edges << "\", " << e.what();
        throw invalid_argument(error.str());

    }

    edges_stream.close();

}

void shortest_paths::graph_to_file(
        const char* filename_nodes,
        const char* filename_edges){

    ofstream nodes_stream(filename_nodes, ios::out),
         edges_stream(filename_edges, ios::out);

    if(nodes_stream.fail()){
        stringstream error;
        error << "unable to open file \""
            << filename_nodes << "\" for writing";
        throw invalid_argument(error.str());
    }

    nodes_stream << "# node_index" << endl;

    for(vector<node>::iterator itr = nodes.begin();
            itr != nodes.end(); itr++){

        nodes_stream << itr->index << endl;

    }

    nodes_stream.close();

    if(edges_stream.fail()){
        stringstream error;
        error << "unable to open file \""
            << filename_edges << "\" for writing";
        throw invalid_argument(error.str());
    }

    edges_stream << "# edge_index; from_node; to_node; weight" << endl;

    for(vector<edge>::iterator itr = edges.begin();
            itr != edges.end(); itr++){

        edges_stream << itr->index << "; " << itr->source << "; "
            << itr->target << "; " << itr->weight << endl;

    }

    edges_stream.close();
}

void shortest_paths::compute_path(
        unsigned int path_source,
        unsigned int path_target){

    path_struct tmp = {numeric_limits<double>::infinity(), numeric_limits<unsigned int>::max()};

    d.resize(nodes.size(), vector<path_struct>(nodes.size(), tmp));

    unsigned int mapped_source = node_index_map[path_source];

    if(sssp == sssp_automatic){

        // TODO make differentiation here
        sssp = dijkstra;

    }

    // This part is taken from Wikipedia, Dijkstra's Algorithm.
    if(sssp == dijkstra){

        if(integrity_check && negative_edge_exists){
            stringstream error;
            error << "dijkstra method cannot deal with "
                << "negative edges that exist in the graph.";
            throw invalid_argument(error.str());
        }

        d[mapped_source][mapped_source].distance = 0;

        FHeap heap(nodes.size());

        for(unsigned int i=0; i < nodes.size(); i++){

            heap.insert(i, d[mapped_source][i].distance);

        }

        double minimum = 0;
        unsigned int index = 0;

        for(unsigned int i = 0; i < nodes.size()
                && minimum != numeric_limits<double>::infinity(); i++){

            // Will overwrite minimum and index
            index = heap.deleteMin();
            // if (index == node_index_map[path_target]){
            //     if (d[mapped_source][index].distance < 0){
            //         cout << d[mapped_source][index].distance << endl;
            //       }
            //     break;
            // }
            minimum = d[mapped_source][index].distance;

            for(map<unsigned int, unsigned int>::iterator itr
                    = sparse_adjacency_matrix[index].begin();
                    itr != sparse_adjacency_matrix[index].end();
                    itr++){

                double alt = d[mapped_source][index].distance
                    + edges[itr->second].weight;

                if(alt < d[mapped_source][itr->first].distance){

                    d[mapped_source][itr->first].distance = alt;
                    d[mapped_source][itr->first].predecessor = index;
                    heap.decreaseKey(itr->first, alt);

                }

            }

            if(!directed){

                for(map<unsigned int, unsigned int>::iterator itr
                        = sparse_transposed_adjacency_matrix[index].begin();
                        itr != sparse_transposed_adjacency_matrix[index].end();
                        itr++){

                    double alt = d[mapped_source][index].distance
                        + edges[itr->second].weight;

                    if(alt < d[mapped_source][itr->first].distance){

                        d[mapped_source][itr->first].distance = alt;
                        d[mapped_source][itr->first].predecessor = index;
                        heap.decreaseKey(itr->first, alt);
                    }

                }

            }

        }

    }

    if(sssp == bellman_ford){

        stringstream error;
        error << "bellman_ford not implemented yet";
        throw invalid_argument(error.str());

        // TO BE CONTINUED

        for(map<unsigned int, unsigned int>::iterator itr =
                sparse_adjacency_matrix[path_source].begin();
                itr != sparse_adjacency_matrix[path_source].end();
                itr++){

            d[mapped_source][itr->first].distance = edges[itr->second].weight;
            d[mapped_source][itr->first].predecessor = path_source;

        }

    }

}

void shortest_paths::track_path(
        unsigned int path_source,
        unsigned int path_target){

    unsigned int mapped_source = node_index_map[path_source];
    unsigned int mapped_target = node_index_map[path_target];

    double length = 0.0;

    node_paths.resize(nodes.size(), vector<list<unsigned int> >());
    node_paths[mapped_source].resize(nodes.size(), list<unsigned int>());

    edge_paths.resize(nodes.size(), vector<list<unsigned int> >());
    edge_paths[mapped_source].resize(nodes.size(), list<unsigned int>());

    if(!directed){
        node_paths[mapped_target].resize(nodes.size(), list<unsigned int>());
        edge_paths[mapped_target].resize(nodes.size(), list<unsigned int>());
    }

    unsigned int predecessor = mapped_target;
    unsigned int old_predecessor = d[mapped_source][predecessor].predecessor;

    bool path_exists = false;

    while(old_predecessor != numeric_limits<unsigned int>::max()){

        path_exists = true;

        node_paths[mapped_source][mapped_target].push_front(predecessor);

        map<unsigned int, unsigned int>::iterator itr =
            sparse_adjacency_matrix[old_predecessor].find(predecessor);

        if(!directed && itr == sparse_adjacency_matrix[old_predecessor].end()){
            itr = sparse_adjacency_matrix[predecessor].find(old_predecessor);
        }

        edge_paths[mapped_source][mapped_target].push_front(itr->second);
        length += edges[itr->second].weight;

        predecessor = old_predecessor;
        old_predecessor = d[mapped_source][predecessor].predecessor;

    }

    if(path_exists){

        node_paths[mapped_source][mapped_target].push_front(predecessor);

    }

    // TODO introduce proper epsilon
    if(d[mapped_source][mapped_target].distance != numeric_limits<double>::infinity() &&
            std::abs(length - d[mapped_source][mapped_target].distance) > 0.01){
        stringstream error;
        error << "something is wrong: shortest distance " <<
               d[mapped_source][mapped_target].distance
               << " not matched by path length " << length;
        throw invalid_argument(error.str());
    }

}

void shortest_paths::compute_all_paths(){

	paths.resize(nodes.size(), vector<vector<list<unsigned int> > >());
	for(int ii = 0; ii<nodes.size(); ii++){

		paths[ii].resize(nodes.size(), vector<list<unsigned int> >());

		paths[ii] = get_all_paths_from(ii);

	}
}

vector<vector<list<unsigned int> > > shortest_paths::get_all_paths_from(
        unsigned int path_source){

	unsigned int mapped_source = node_index_map[path_source];

        vector<double> min;
	min.reserve(nodes.size());
	vector<vector<list<unsigned int> > > path;
	path.resize(nodes.size(), vector<list<unsigned int> >());
	vector<bool> visited;
	visited.reserve(nodes.size());

	for(unsigned int ii = 0; ii<nodes.size();ii++){
		visited[ii] = false;
		min[ii] = numeric_limits<double>::infinity();
	}

	min[mapped_source] = 0;
	unsigned int current = mapped_source;

	while(true){
		visited[current] = true;
		for(std::map<unsigned int, unsigned int>::iterator itr = sparse_adjacency_matrix[current].begin(); itr != sparse_adjacency_matrix[current].end(); itr++){
			if(edges[itr->second].weight > 0 && !visited[itr->first]){
				if(edges[itr->second].weight + min[current] < min[itr->first]){
					min[itr->first] = edges[itr->second].weight + min[current];
					path[itr->first].clear();
				}
				if(edges[itr->second].weight + min[current] == min[itr->first]){
					if(path[current].size() == 0){
						path[itr->first].resize(1,list<unsigned int>());
						path[itr->first].back().push_back(edges[itr->second].index);
					} else {
						path[itr->first].resize(path[current].size(),list<unsigned int>());
						for(unsigned int ii = 0;ii< path[current].size(); ii++){
							path[itr->first].push_back(path[current][ii]);
							path[itr->first].back().push_back(edges[itr->second].index);
						}
					}
				}
			}
		}
		for(map<unsigned int, unsigned int>::iterator itr = sparse_transposed_adjacency_matrix[current].begin(); itr != sparse_transposed_adjacency_matrix[current].end() && !directed; itr++){
			if(edges[itr->second].weight > 0 && !visited[itr->first]){
				if(edges[itr->second].weight + min[current] < min[itr->first]){
					min[itr->first] = edges[itr->second].weight + min[current];
					path[itr->first].clear();
				}
				if(edges[itr->second].weight + min[current] == min[itr->first]){
					if(path[current].size() == 0){
						path[itr->first].resize(1,list<unsigned int>());
						path[itr->first].back().push_back(edges[itr->second].index);
					} else {
						path[itr->first].resize(path[current].size(),list<unsigned int>());
						for(unsigned int ii = 0;ii< path[current].size(); ii++){
							path[itr->first].push_back(path[current][ii]);
							path[itr->first].back().push_back(edges[itr->second].index);
						}
					}
				}
			}
		}

		double least = numeric_limits<double>::infinity();
		int k = -1;
		for(unsigned int nodes_count= 0; nodes_count<nodes.size(); nodes_count++){
			if(!visited[nodes_count] && min[nodes_count]<least){
				k = nodes_count;
				least = min[nodes_count];
			}
		}
		if(k==-1)break;
		current = k;
	}
	return path;
}

vector<vector<unsigned int> > shortest_paths::get_all_paths_from_to(
	unsigned int path_source,
	unsigned int path_target){

	vector<vector<unsigned int> > all_paths_remapped;

	all_paths_remapped.reserve(paths[node_index_map[path_source]][node_index_map[path_target]].size());
	for(unsigned int ii = 0; ii<paths[node_index_map[path_source]][node_index_map[path_target]].size();ii++){
		all_paths_remapped.push_back(edge_indices(paths[path_source][path_target][ii]));
	}
	return all_paths_remapped;
}

vector<unsigned int> shortest_paths::edge_indices(const list<unsigned int>& mapped_indices){

    vector<unsigned int> retval;
    retval.reserve(mapped_indices.size());

    for(list<unsigned int>::const_iterator itr = mapped_indices.begin();
            itr != mapped_indices.end(); itr++){

        retval.push_back(edges[*itr].index);

    }

    return retval;

}

vector<unsigned int> shortest_paths::node_indices(const list<unsigned int>& mapped_indices){

    vector<unsigned int> retval;
    retval.reserve(mapped_indices.size());

    for(list<unsigned int>::const_iterator itr = mapped_indices.begin();
            itr != mapped_indices.end(); itr++){

        retval.push_back(nodes[*itr].index);

    }

    return retval;

}

void shortest_paths::compute_pairwise(int start, int end){ // default values of 0 and -1
    if (end == -1) end = nodes.size();
    if(pairwise_computation_done || nodes.size() == 0){
        return;
    }

    if(apsp == floyd_warshall){

        path_struct tmp = {numeric_limits<double>::infinity(),
            numeric_limits<unsigned int>::max()};

        d.resize(nodes.size(), vector<path_struct>(nodes.size(), tmp));

        for(unsigned int i=0; i<sparse_adjacency_matrix.size(); i++){

            for(map<unsigned int, unsigned int>::iterator itr2 =
                    sparse_adjacency_matrix[i].begin();
                    itr2 != sparse_adjacency_matrix[i].end();
                    itr2++){

                d[i][itr2->first].distance = edges[itr2->second].weight;
                d[i][itr2->first].predecessor = i;

                if(!directed){
                    d[itr2->first][i].distance = edges[itr2->second].weight;
                    d[itr2->first][i].predecessor = i;
                }

            }

            d[i][i].distance = 0;

        }

        for(unsigned int k=0; k<nodes.size(); k++){

            if(verbose){
                if(k % 50 == 0){
                    cerr << "node " << k << " of " << nodes.size() << endl;
                }
            }

            for(unsigned int i=0; i<nodes.size(); i++){

                for(unsigned int j=0; j<nodes.size(); j++){

                    double alt = d[i][k].distance + d[k][j].distance;

                    if(alt < d[i][j].distance){

                        d[i][j].distance = alt;
                        d[i][j].predecessor = d[k][j].predecessor;

                    }

                }

            }

        }
        for(unsigned int i=0; i<nodes.size(); i++){

            if(d[i][i].distance < 0){

                stringstream error;
                error << "graph has negative cycles" << endl;
                throw invalid_argument(error.str());

            }

        }

    }

    else {

        for(unsigned int i=start; i < end; i++){

            if(verbose){
                if(i % 50 == 0){
                    cerr << "node " << i - start << " of " << end - start  << endl;
                }
            }
            compute_path(i,i);

        }

    }

    pairwise_computation_done = true;

}

void shortest_paths::track_pairwise(){

    if(pairwise_tracking_done){
        return;
    }

    if(!pairwise_computation_done){

        compute_pairwise();

    }

    for(unsigned int i=0; i<nodes.size(); i++){

        unsigned int startpoint;

        if(directed){
            startpoint = 0;
        }
        else{
            startpoint = i+1;
        }

        for(unsigned int j=startpoint; j<nodes.size(); j++){

            track_path(nodes[i].index, nodes[j].index);
            if(!directed){
                node_paths[j][i] = node_paths[i][j];
                edge_paths[j][i] = edge_paths[i][j];
            }

        }

    }

    pairwise_tracking_done = true;

}

void shortest_paths::distance_to_file(
        const char* filename,
        unsigned int path_source,
        unsigned int path_target){

    if(integrity_check){
        if(node_index_map.find(path_source) == node_index_map.end()){
            stringstream error;
            error << "source node " << path_source << " does not exist";
            throw invalid_argument(error.str());
        }
        if(node_index_map.find(path_target) == node_index_map.end()){
            stringstream error;
            error << "target node " << path_target << " does not exist";
            throw invalid_argument(error.str());
        }
    }

    compute_path(path_source, path_target);

    ofstream filestream(filename, ios::out);

    if(filestream.fail()){
        stringstream error;
        error << "unable to open file \""
            << filename << "\" for writing";
        throw invalid_argument(error.str());
    }

    filestream << "# Automatically created by a shortest_paths object."
        << "# path_source; path_target; distance" << endl
        << path_source << "; " << path_target << "; " <<
        d[node_index_map[path_source]][node_index_map[path_target]].distance
        << endl;

    filestream.close();
}

void shortest_paths::pairwise_distances_to_file(const char* filename) {

    compute_pairwise();

    ofstream filestream(filename, ios::out);

    if(filestream.fail()){
        stringstream error;
        error << "unable to open file \""
            << filename << "\" for writing";
        throw invalid_argument(error.str());
    }

    filestream << "# Automatically created by a shortest_paths object."
        << "# path_source; path_target; distance" << endl;

    for(unsigned int i=0; i < nodes.size(); i++){

        unsigned int startpoint;

        if(directed){
            startpoint = 0;
        }
        else{
            startpoint = i+1;
        }

        for(unsigned int j=startpoint; j < nodes.size(); j++){

            filestream << nodes[i].index << "; "
                << nodes[j].index << "; " << d[i][j].distance << endl;

        }

    }

    filestream.close();
}

void shortest_paths::path_to_file(
        const char* filename,
        unsigned int path_source,
        unsigned int path_target) {

    if(integrity_check){
        if(node_index_map.find(path_source) == node_index_map.end()){
            stringstream error;
            error << "source node " << path_source << " does not exist";
            throw invalid_argument(error.str());
        }
        if(node_index_map.find(path_target) == node_index_map.end()){
            stringstream error;
            error << "target node " << path_target << " does not exist";
            throw invalid_argument(error.str());
        }
    }

    track_path(path_source, path_target);

    ofstream filestream(filename, ios::out);

    if(filestream.fail()){
        stringstream error;
        error << "unable to open file \""
            << filename << "\" for writing";
        throw invalid_argument(error.str());
    }

    filestream << "# Automatically created by a shortest_paths object." << endl;

    if(directed){
        filestream << "# edge_source; edge_target; edge_index" << endl;
    }
    else{
        filestream << "# edge_left_node; edge_right_node; edge_index" << endl;
    }

    list<unsigned int> edge_list = edge_paths[
        node_index_map[path_source]][node_index_map[path_target]];

    for(list<unsigned int>::iterator itr = edge_list.begin();
            itr != edge_list.end(); itr++){

        filestream << edges[*itr].source << "; " << edges[*itr].target << "; "
            << edges[*itr].index << endl;

    }

    filestream.close();

}

void shortest_paths::pairwise_paths_to_file(const char* filename) {

    track_pairwise();

    ofstream filestream(filename, ios::out);

    if(filestream.fail()){
        stringstream error;
        error << "unable to open file \""
            << filename << "\" for writing";
        throw invalid_argument(error.str());
    }

    filestream << "# Automatically created by a shortest_paths object." << endl;

    if(directed){
        filestream << "# path_source; path_target; edge_source; edge_target; edge_index" << endl;
    }
    else{
        filestream << "# path_source; path_target; edge_left_node; edge_right_node; edge_index" << endl;
    }

    for(unsigned int i=0; i < nodes.size(); i++){

        unsigned int startpoint;

        if(directed){
            startpoint = 0;
        }
        else{
            startpoint = i+1;
        }

        for(unsigned int j=startpoint; j < nodes.size(); j++){

            for(list<unsigned int>::iterator
                    itr = edge_paths[i][j].begin();
                    itr != edge_paths[i][j].end(); itr++){

                filestream << nodes[i].index << "; "
                    << nodes[j].index << "; " << edges[*itr].source << "; "
                    << edges[*itr].target << "; " << edges[*itr].index << endl;

            }

        }

    }

    filestream.close();

}

double shortest_paths::get_distance(
        unsigned int path_source,
        unsigned int path_target) {

    if(!pairwise_computation_done){
        compute_path(path_source, path_target);
    }

    return d[node_index_map[path_source]]
        [node_index_map[path_target]].distance;

}

vector<unsigned int> shortest_paths::get_path_nodes(
        unsigned int path_source,
        unsigned int path_target){

    track_path(path_source, path_target);

    return node_indices(node_paths[node_index_map[path_source]][node_index_map[path_target]]);

}

vector<unsigned int> shortest_paths::get_path_edges(
        unsigned int path_source,
        unsigned int path_target){

    track_path(path_source, path_target);

    return edge_indices(edge_paths[node_index_map[path_source]][node_index_map[path_target]]);

}

void shortest_paths::set_sssp_method_dijkstra(){

    sssp = dijkstra;

}

void shortest_paths::set_sssp_method_bellman_ford(){

    sssp = bellman_ford;

}

void shortest_paths::set_sssp_method_automatic(){

    sssp = sssp_automatic;

}

void shortest_paths::set_apsp_method_floyd_warshall(){

    apsp = floyd_warshall;

}

void shortest_paths::set_apsp_method_johnson(){

    apsp = johnson;

}

void shortest_paths::set_verbose(bool verbose){

    this->verbose = verbose;

}

void shortest_paths::set_directed(bool directed){

    this->directed = directed;

}

void shortest_paths::set_apsp_method_automatic(){

    apsp = apsp_automatic;

}