/*
 * Copyright (C) 2003 Carnegie Mellon University and Rutgers University
 *
 * Permission is hereby granted to distribute this software for
 * non-commercial research purposes, provided that this copyright
 * notice is included with any such distribution.
 *
 * THIS SOFTWARE IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
 * EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
 * SOFTWARE IS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU
 * ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
 *
 * $Id: mdpclient.cc,v 1.3 2004/03/10 22:34:41 john Exp $
 */
#include <iostream>
#include <fstream>
#include <sstream>
#include <cerrno>
#include <cstdio>
#include <ctime>
#if defined __GNUC__ && __GNUC__ >= 3 && __GNUC_MINOR__ > 0
#include <ext/stdio_filebuf.h>
#endif
#include <string.h>
#include <stdio.h>

#include <sys/socket.h>
#include <netdb.h>
#include <netinet/in.h>

#include "states.h"
#include "problems.h"
#include "domains.h"
#include "exceptions.h"
#include "actions.h"

#include "client.h"

/* The parse function. */
extern int yyparse();
/* File to parse. */
extern FILE* yyin;
/* Name of current file. */
std::string current_file;
/* Level of warnings. */
int warning_level;
/* Verbosity level. */
int verbosity;

std::string myreplace(std::string source, std::string toreplace, std::string replacement){
  using namespace std;
  
  int pos = source.find (toreplace,0);
  while(pos!=string::npos)
    {
      source = source.replace(pos, toreplace.length(),replacement);
      pos = source.find (toreplace,0);
    }
  
  return source;
}


/* Parses the given file, and returns true on success. */
static bool read_file(const char* name) {
  yyin = fopen(name, "r");
  if (yyin == NULL) {
    std::cerr << PACKAGE << ':' << name << ": " << strerror(errno)
	      << std::endl;
    return false;
  } else {
    current_file = name;
    bool success = (yyparse() == 0);
    fclose(yyin);
    return success;
  }
}

int connect(const char *hostname, int port)
{
  struct hostent *host = ::gethostbyname(hostname);
  if (!host) {
    perror("gethostbyname");
    return -1;
  }

  int sock = ::socket(PF_INET, SOCK_STREAM, 0);
  if (sock == -1) {
    perror("socket");
    return -1;
  }
  
  struct sockaddr_in addr;
  addr.sin_family=AF_INET;
  addr.sin_port=htons(port);
  addr.sin_addr = *((struct in_addr *)host->h_addr);
  memset(&(addr.sin_zero), '\0', 8);

  if (::connect(sock, (struct sockaddr*)&addr, sizeof(addr)) == -1) {
    perror("connect");
    return -1;
  }
  return sock;
  //remember to call close(sock) when you're done
}

class PacPlanner : public Planner
{
public:
  PacPlanner(const Problem& problem) : Planner(problem) {}
  PacPlanner(std::string domain_file, const Problem& problem) : Planner(problem) { 
    domain_file_ = domain_file;
    observations_file_ = "./tmp/observations.txt";
  }
  virtual void initRound();
  virtual ~PacPlanner() {}
  virtual const Action* decideAction(const State&);
  virtual void endRound();

private:  
  std::string domain_file_;
  std::string observations_file_;
  State previous_state_;
  std::vector<std::string> plan_;
  int index_current_action_;

  int plan(std::string domain_file, std::string problem_name, std::string plan_file_name);
  int replan(State state);
  bool repairplan(ActionList actions);
  bool reuseplan(ActionList actions);
  int generate_reproblem(std::string reproblem_domain_file, std::string reproblem_name, State state);
  int print_pending_plan();
  int write_Observation(std::string observations_file_name, std::string action, State dinamic_state, std::string result);
  State get_previous_state();
  void set_previous_state(State state);

};

/* Begin My Functions */
int PacPlanner::plan(std::string domain_file, std::string problem_name, std::string plan_file_name){

  std::cout << "planning... " << std::endl;
  
  // Calling the pac-plan planner
  std::ostringstream command_os;
  command_os << "./pacplan -o " << domain_file<< " -f "<< problem_name << " -m quality -l 1 > " << plan_file_name <<std::endl;

  std::cout << command_os.str().c_str() << std::endl;

  system(command_os.str().c_str());

  std::vector<std::string>::iterator it;
  int i=0;
  for(it=plan_.begin();it!=plan_.end();it++){
    if (i>=index_current_action_) break;
    else i++;
  }

  plan_.erase(it, plan_.end());
  
  // Reading the Proposed plan
  std::ifstream plan_file (plan_file_name.c_str());
  if (!plan_file){
    std::cerr << PACKAGE ": error cant open plan file "<< plan_file_name << std::endl;
    return -1;
  }
  
  std::string line;
  while(!plan_file.eof()){
    // Reading an action and adding it to the plan vector
    getline(plan_file, line);
    int ini_pos,end_pos;
    ini_pos=line.find("(");
    end_pos=line.find(")")+1;
    if ((ini_pos!=std::string::npos)&&(end_pos!=std::string::npos))
      {
	std::string action=line.substr(ini_pos,end_pos-ini_pos);
	plan_.push_back(action);
      }
  }  
  plan_file.close();
  
  return 0;
}

int PacPlanner::generate_reproblem(std::string reproblem_domain_file, std::string reproblem_name, State state){

  //openning the original domain file
  std::ifstream ifs_domain_file (domain_file_.c_str());
  if (!ifs_domain_file){
    std::cerr << PACKAGE ": error cant open domain file "<< domain_file_ << std::endl;
    return -1;
  }
  
  //creating the replanning domain file
  std::ofstream ofs_reproblem_domain_file (reproblem_domain_file.c_str());  
  std::string line;
  while(!ifs_domain_file.eof()){
    // Reading
    getline(ifs_domain_file, line);
    if (line.find("(define (problem")!=std::string::npos) break;
    ofs_reproblem_domain_file << line << std::endl;
  }
  ifs_domain_file.close();

  //creating the reproblem
  ofs_reproblem_domain_file << "(define (problem " << reproblem_name << ")" << std::endl;
  ofs_reproblem_domain_file << "    (:domain " << _problem.domain().name() << ")" << std::endl;
  ofs_reproblem_domain_file << "    (:objects" ;
  for (Type i = _problem.domain().types().first_type(); i <= _problem.domain().types().last_type(); i++) {
    bool bempty = true;
    for (Object j = _problem.terms().first_object();j <= _problem.terms().last_object(); j++) {
      if(_problem.terms().type(j)==i){
	  bempty = false;
	  ofs_reproblem_domain_file << " ";
	  _problem.terms().print_term(ofs_reproblem_domain_file, j);
	}
    }
    if(bempty==false){	
      ofs_reproblem_domain_file << " - ";
      _problem.domain().types().print_type(ofs_reproblem_domain_file, i);
    }
  }
  ofs_reproblem_domain_file << ") " << std::endl;
  
  ofs_reproblem_domain_file << "    (:init ";
  ofs_reproblem_domain_file << std::endl;

    // static state predicates
  std::ostringstream state_os;
  std::ostringstream static_os;
  _problem.domain().predicates().print_static_predicates(static_os);
  for (AtomSet::const_iterator ai = _problem.init_atoms().begin();
       ai != _problem.init_atoms().end(); ai++) {
    std::ostringstream os_aux;    
    (*ai)->print(os_aux, _problem.domain().predicates(), _problem.domain().functions(), _problem.terms());
    
    int end;
    int pos;
    pos=os_aux.str().find(" ");
    if(pos!=std::string::npos) end=pos-1;
    else end = os_aux.str().length();
    std::string aux= os_aux.str().substr(1, end);
    
    if(static_os.str().find(aux)!=std::string::npos){
      state_os << os_aux.str() <<" ";
    }    
  }

  ofs_reproblem_domain_file << state_os.str();

  // dinamic state predicates
  bool first = true;
  for (AtomSet::const_iterator ai = state.atoms().begin(); ai != state.atoms().end(); ai++) {
    if (first) {
      first = false;
    } else {
      ofs_reproblem_domain_file << ' ';
    }
    (*ai)->print(ofs_reproblem_domain_file, _problem.domain().predicates(), _problem.domain().functions(), _problem.terms());
  }
  ofs_reproblem_domain_file << ")" << std::endl;
  
  ofs_reproblem_domain_file << "    (:goal ";
  ofs_reproblem_domain_file << "        "<< std::endl;
  _problem.goal().print(ofs_reproblem_domain_file, _problem.domain().predicates(), _problem.domain().functions(), _problem.terms());
  ofs_reproblem_domain_file << ")" << std::endl;
  ofs_reproblem_domain_file << ")" << std::endl;
  ofs_reproblem_domain_file.close();
  return 0;
}

int PacPlanner::replan(State state){
  // No more actions in the plan, we replan to get a new one
  std::string reproblem_domain_file = "reproblem-domain.pddl";
  std::string reproblem_problem_name ="reproblem";
  generate_reproblem(reproblem_domain_file, reproblem_problem_name, state);
  std::string plan_file_name = "replan.txt";
  plan(reproblem_domain_file, reproblem_problem_name, plan_file_name);  
}

bool PacPlanner::reuseplan(ActionList actions){
  std::cout << "Reusing plan... " << std::endl;

  int old_index=index_current_action_;
  std::string next_action;

  do{
    index_current_action_--;
    //getting the next action from the plan
    next_action=plan_.at(index_current_action_);
    
    //looking for the action in the enabled_actions vector
    for (int i=0; i<actions.size(); i++){
      std::ostringstream os_aux;
      actions[i]->print(os_aux, _problem.terms());
      if(strcasecmp(os_aux.str().c_str(), next_action.c_str())==0) 
	return true;    
    }
  }while (index_current_action_>0);
  
  index_current_action_=old_index;
  return false;
}

bool PacPlanner::repairplan(ActionList actions){
  std::cout << "Repairing plan... " << std::endl;

  //  return reuseplan(actions);
  return false;
}

int PacPlanner::print_pending_plan()
{
  std::vector<std::string>::iterator it;
  int i=index_current_action_;
  std::cout << "The pending plan: " << std::endl;
  for(it=plan_.begin()+index_current_action_;it!=plan_.end();it++){
    std::cout << i<< ": " << *it <<" " << std::endl;
    i++;
  }
  return 0;
}

int PacPlanner::write_Observation(std::string observations_file_name, std::string action, State dinamic_state, std::string result)
{
  using namespace std;

  std::ofstream file_observations;
  file_observations.open(observations_file_name.c_str(), std::ios::app);
  
  // static predicates
  std::ostringstream state_os;
  std::ostringstream static_os;
  _problem.domain().predicates().print_static_predicates(static_os);
  for (AtomSet::const_iterator ai = _problem.init_atoms().begin();
       ai != _problem.init_atoms().end(); ai++) {
    std::ostringstream os_aux;    
    (*ai)->print(os_aux, _problem.domain().predicates(), _problem.domain().functions(), _problem.terms());
    
    int end;
    int pos;
    pos=os_aux.str().find(" ");
    if(pos!=string::npos) end=pos-1;
    else end = os_aux.str().length();
    string aux= os_aux.str().substr(1, end);
    
    if(static_os.str().find(aux)!=string::npos){
      state_os << os_aux.str() <<" ";
    }    
  }

  dinamic_state.print(static_os, _problem.domain().predicates(), _problem.domain().functions(), _problem.terms());
  
  file_observations << action << "|" << result <<"|"<< state_os.str() << "|" << std::endl;  
  file_observations.close();
  return 0;
}

State PacPlanner::get_previous_state(){
  return previous_state_;
}
void PacPlanner::set_previous_state(State state){


  for (AtomSet::const_iterator ai = state.atoms().begin();ai != state.atoms().end(); ai++) {
    const Atom *pa= *ai;
    previous_state_.addAtom(*pa);
  }

  for (ValueMap::const_iterator vi = state.values().begin();vi != state.values().end(); vi++) {
    const Application *app = (*vi).first;
    const Rational *value = &((*vi).second);

    previous_state_.addValueEntry(*app,*value);
  }
}

/* End My Functions */

void PacPlanner::initRound()
{
  std::string plan_file_name = "plan.txt";

  plan_.clear();
  index_current_action_=0;

  plan(domain_file_, _problem.name(), plan_file_name);
  if(plan_.size()==0){
    std::cerr << PACKAGE ": NO plan found for the problem: " << std::endl;
    return;
  }

  // getting the initial State
  State init_state;    
  for (AtomSet::const_iterator ai = _problem.init_atoms().begin(); ai != _problem.init_atoms().end(); ai++) {
    const Atom *pa= *ai;
    init_state.addAtom(*pa);
  }

  set_previous_state(init_state);
}

void PacPlanner::endRound()
{
}

const Action* PacPlanner::decideAction(const State& state)
{
  //getting the enabled actions
  ActionList actions;
  _problem.enabled_actions(actions, state.atoms(), state.values());
  if (actions.empty()) {
    std::cerr << PACKAGE ": Enabled Actions Empty " << std::endl;
    return NULL;
  }

  if(index_current_action_ == plan_.size()){
    if(repairplan(actions)==false){
      replan(state);
      if(plan_.size()==0){
	std::cerr << PACKAGE ": No plan found for the problem: " << std::endl;
	return NULL;
      }
    }
  }
  
  print_pending_plan();
  
  //getting the next action from the plan
  std::string next_action=plan_.at(index_current_action_);
  
  //looking for the action in the enabled_actions vector
  for (int i=0; i<actions.size(); i++){
    std::ostringstream os_aux;
    actions[i]->print(os_aux, _problem.terms());
    if(strcasecmp(os_aux.str().c_str(), next_action.c_str())==0){
      std::cout<< "Sent action " << index_current_action_ << ": " << next_action  << std::endl;

      // ------- The Previous Action was a success ---------
      if(index_current_action_>0){
	std::string previous_action=plan_.at(index_current_action_-1);
	//write_Observation(observations_file_, previous_action, get_previous_state(), "success");
      }

      set_previous_state(state);

      index_current_action_++;
      return actions[i];
    }
  }
  
  std::cout<< "The action " << next_action << " Can't be executed !!" << std::endl;
  // -------- The Previous Action was a failure ---------
  if(index_current_action_>0){
    std::string previous_action=plan_.at(index_current_action_-1);
    //write_Observation(observations_file_, previous_action, get_previous_state(), "failure");
  }

  // The next_action is NOT aplicable, we replan to get a new one 
  if(repairplan(actions)==false){
    index_current_action_--;
    replan(state);
    if(plan_.size()==0){
      std::cerr << PACKAGE << ": No plan found for the new State: " << std::endl;  
      return NULL;
    }
  }
  
  print_pending_plan();
  
  //getting the next action from the plan
  next_action=plan_.at(index_current_action_);
               
  //looking for the action in the enabled_actions vector
  for (int i=0; i<actions.size(); i++){
    std::ostringstream os_aux;
    actions[i]->print(os_aux, _problem.terms());
    if(strcasecmp(os_aux.str().c_str(), next_action.c_str())==0){
      std::cout<< "Sent action: " << next_action  << std::endl;
      index_current_action_++;

      set_previous_state(state);

      return actions[i];
    }
  }
  
  std::cerr << PACKAGE << ": The action: " << next_action << " was not aplicable in the initial replanning state: " << std::endl;  
  return NULL;
}

int main(int argc, char **argv)
{
  /* Set default verbosity. */
  verbosity = 0;
  /* Set default warning level. */
  warning_level = 1;
  if (argc != 4) {
    std::cerr << "Usage: " << *argv << " <host>:<port> <problem file> <problem name>" << std::endl;
    return 1;
  }

  if (!read_file(argv[2])) {
    std::cerr << "Couldn't read problem file " << argv[2] << std::endl;
    return 1;
  }

  const Problem *problem = Problem::find(argv[3]);
  if (!problem) {
    std::cerr << "Problem " << argv[3] << " is not defined in " << argv[2] << std::endl;
    return 1;
  }

  
  char *hostport = argv[1];
  char *host = strtok(hostport, ":");
  char *portstr = strtok(0, ":");
  int port = atoi(portstr);

  int socket = connect(host, port);
  if (socket <= 0) {
    std::cerr << "Could not connect to " << argv[1] << std::endl;
    return 1;
  }
#if defined __GNUC__ && __GNUC__ >= 3 && __GNUC_MINOR__ > 0
  //__gnu_cxx::stdio_filebuf<char> ofbuf(socket, std::ios_base::out, BUFSIZ);
  //__gnu_cxx::stdio_filebuf<char> ifbuf(socket, std::ios_base::in, 1);
  // if above don't compile, try these ...
  __gnu_cxx::stdio_filebuf<char> ofbuf(socket, std::ios_base::out, false, BUFSIZ);
  __gnu_cxx::stdio_filebuf<char> ifbuf(socket, std::ios_base::in, false, 1);
#else
  std::filebuf ofbuf(socket);
  std::filebuf ifbuf(socket);
#endif
  std::ostream os(&ofbuf);
  std::istream is(&ifbuf);
  
  std::string prob_domain_file=argv[2];
  PacPlanner p(prob_domain_file, *problem);

  XMLClient(&p, problem, "results", is, os);
  
  //close(socket);

  return 0;
}