text: 
  
  #include "QLearningActionSelector.h"
  
  #include "RewardFunction.h"
  
  #include <cstdlib>
  #include <iterator>
  #include <algorithm>
  
  #include <sstream>
  #include <iostream>
  
  #include <ros/console.h>
  
  #define EPSILON 0.1
  
  using namespace actasp;
  using namespace std;
  
  namespace bwi_krexec {
  
  struct CompareValues {
  
    CompareValues(QLearningActionSelector::ActionValueMap& value) : value(value) {}
  
    bool operator()(const AspFluent& first, const AspFluent& second) {
      return value[first] < value[second];
    }
  
    QLearningActionSelector::ActionValueMap& value;
  };
  
  QLearningActionSelector::QLearningActionSelector(double alpha, RewardFunction<State> *reward, 
                                                   actasp::AspKR *reasoner, DefaultActionValue *defval) :
    reasoner(reasoner),
    defval(defval),
    alpha(alpha),
    reward(reward),
    value(),
    initial(),
    final(),
    previousAction("noaction(0)"),
    count(0)  {}
  
  
  struct CompareSecond {
    bool operator()(const pair<AspFluent, double>& first, const pair<AspFluent, double>& second) {
      return first.second < second.second;
    }
  };
  
  actasp::ActionSet::const_iterator QLearningActionSelector::choose(const actasp::ActionSet& options) throw() {
  
    if (!(initial.empty() || final.empty())) {
  
      ActionValueMap::const_iterator bestValuePair = max_element(value[final].begin(), value[final].end(),CompareSecond());
      
      double bestValue = 0;
      if(bestValuePair != value[final].end())
        bestValue = bestValuePair->second;
  
      double rew = reward->r(initial,previousAction,final);
  
  
      ROS_INFO_STREAM("old value: " << value[initial][previousAction]);
      ROS_INFO_STREAM("reward: " << rew);
  
      value[initial][previousAction] = (1 - alpha) * value[initial][previousAction] + alpha * (rew + bestValue);
  
      ROS_INFO_STREAM("new value: " << value[initial][previousAction]);
  
      initial.clear();
      final.clear();
  
    }
  
  
    stringstream ss;
    ss << "Evaluating options: ";
    copy(options.begin(), options.end(), ostream_iterator<string>(ss, " "));
    ss << endl;
  
    AnswerSet currentState = reasoner->currentStateQuery(vector<AspRule>());
  
    ActionSet::const_iterator optIt = options.begin();
    for (; optIt != options.end(); ++optIt) {
      ActionValueMap &thisState = value[currentState.getFluents()];
      
      if(thisState.find(*optIt) == thisState.end()) {
        //initialize to default
        thisState[*optIt] = defval->value(*optIt);
      }
      ss << value[currentState.getFluents()][*optIt] << " ";
    }
  
    ROS_INFO_STREAM(ss.str());
  
    double prob = EPSILON;
  
    if (rand() <= prob * RAND_MAX) { //random
      ActionSet::const_iterator chosen =  options.begin();
      advance(chosen, rand() % options.size());
  //     std::cout << "choosing random " << std::endl;
      return chosen;
    }
  
    actasp::ActionSet::const_iterator best = max_element(options.begin(), options.end(),CompareValues(value[currentState.getFluents()]));
  
    return best;
  
  }
  
  void QLearningActionSelector::actionStarted(const AspFluent&) throw() {
  
    initial = reasoner->currentStateQuery(vector<AspRule>()).getFluents();
  }
  
  
  void QLearningActionSelector::actionTerminated(const AspFluent& action) throw() {
    final = reasoner->currentStateQuery(vector<AspRule>()).getFluents();
    previousAction = action;
  }
  
  void QLearningActionSelector::episodeEnded() {
    if(initial.empty())
      return;
      
    ROS_INFO_STREAM("old value: " << value[initial][previousAction]);
    value[initial][previousAction] = (1 - alpha) * value[initial][previousAction] + alpha * reward->r(initial,previousAction,final);
    ROS_INFO_STREAM("new value: " << value[initial][previousAction]);
  
    initial.clear();
    final.clear();
    ++count;
  }
  
  
  void QLearningActionSelector::readFrom(std::istream & fromStream) throw() {
  
    ROS_DEBUG("Loading value function");
  
    const string whiteSpaces(" \t");
  
    value.clear();
  
    while (fromStream.good() &&  !fromStream.eof()) {
  
      string stateLine;
      getline(fromStream,stateLine);
  
      size_t firstChar = min(stateLine.find_first_of(whiteSpaces),static_cast<size_t>(0));
      size_t lastChar = min(stateLine.find_last_not_of(whiteSpaces),stateLine.size());
      stateLine = stateLine.substr(firstChar,lastChar-firstChar+1);
  
      stringstream stateStream(stateLine);
  
  
  
      if (stateLine.empty())
        return;
  
      State state;
      copy(istream_iterator<string>(stateStream), istream_iterator<string>(), inserter(state, state.begin()));
  
  
      string actionLine;
      getline(fromStream,actionLine);
  
      while (actionLine.find("-----") == string::npos) {
  
        size_t firstChar = min(actionLine.find_first_of(whiteSpaces), static_cast<size_t>(0));
        size_t lastChar = min(actionLine.find_last_not_of(whiteSpaces),actionLine.size());
        actionLine = actionLine.substr(firstChar,lastChar-firstChar+1);
  
        stringstream actionStream(actionLine);
  
        double actionValue;
        string fluentString;
  
        actionStream >> actionValue >> fluentString;
  
        AspFluent action(fluentString);
  
        value[state].insert(make_pair(action,actionValue));
  
        getline(fromStream,actionLine);
      }
  
    }
  }
  
  
  void QLearningActionSelector::writeTo(std::ostream & toStream) throw() {
  
    ROS_DEBUG("Storing value function");
  
    StateActionMap::const_iterator stateIt = value.begin();
    //for each state
    for (; stateIt != value.end(); ++stateIt) {
  
      //write the state in a line
      copy(stateIt->first.begin(), stateIt->first.end(), ostream_iterator<string>(toStream, " "));
  
      //for each action
      ActionValueMap::const_iterator actionIt = stateIt->second.begin();
      for (; actionIt != stateIt->second.end(); ++actionIt) {
  
        toStream << endl;
  
        //write the value, then the action
  
        toStream <<  actionIt->second << " " << actionIt->first.toString();
  
      }
  
      //a separator for the next state
      toStream << endl << "-----" << endl;
    }
  
  }
  }
  