/*********************** Graphplan **************************************/


/***** planner.c: do the planning part.*********************************/


#include "graphplan.h"
#include <sys/time.h>

extern hashtable_t *fact_table, *op_table;  /* the arrays of hash tables */
extern int do_minexp, do_remove, do_heuristic_value, print_horizon, debugging_mode, print_path;
extern fact_list the_goals;
extern struct timeval start;


/*stuff for completeness */
extern int same_as_prev_flag, first_full_time, num_hashes[2];

/* for heuristic value */
extern int *value_of_null;

void insert(goal_arr a, int num, int time, double prob);
int setup_forward(int maxtime);
void insert_forward(fact_arr a, vertex_t op, int num, int time, double prob);
hash_entry lookup_forward(fact_arr a, int num, int time);

double runbasicplan(int numgoals, int time);

int hash_inserts = 0, hash_hits = 0, hash_ctr = 0,
  hash_numctr = 0, subset_hits = 0;

int number_of_actions_tried = 0;
int number_of_state_steps = 0;
int linenumber = 0;
int global_maxtime;  /* global variable containing maxtime */
extern int num_goals;  /* how many goals there are */
extern int global_needed_vec; /* vector showing needed goals */

/*****use for storing current facts and ops*****/
fact_arr *facts_true_at = NULL;
goal_arr *goals_at = NULL;
op_arr *ops_at = NULL;

/*** hash table stuff ***/
set_table the_table, the_forward_table;

void print_plan(int maxtime, double utility, int num_facts);

/* return the expected utility of solution.  We will do forward-chaining */
double do_plan(int maxtime)
{
  int num_facts;
  double result;
  num_facts = setup_forward(maxtime);
  global_maxtime = maxtime;
  result = runbasicplan(num_facts, maxtime);
  if (result > util_of_failing) print_plan(maxtime, result, num_facts);
  return result;
}



/* return the value of a given set of facts at a given time.  The "given
   facts" are those in facts_true_at[time].  The value is utilities[time]
   if the goals are solved, and util_of_failing if not  */

double value_of(int numfacts, int time)
{
  vertex_t v;
  fact_list temp;
  int i;
  static int flag = 0, ngoals;
  static char str[MAXGOALS][100];

  if (flag == 0) {    /* set up str */
    flag = 1;
    for(ngoals=0, temp=the_goals; temp; ngoals++, temp = temp->next) {
      instantiate_into_string(temp->item, NULL, str[ngoals],1);
    }
  }
  for(i=0; i < ngoals; ++i) {
    if ((v = lookup_from_table(fact_table[time],str[i])) == NULL) 
      return util_of_failing;
    if (! v->is_true) return util_of_failing;
  }
  return utilities[time];
}

/* sum up the heuristic values of the nodes in this set, and add in
   value_of_null[time]. If we are keeping track of pairwise values,
   then do the slower pairwise method */
int heuristic_value_of(int numfacts, int time)
{
  int value = value_of_null[time];
  int i;
  for(i=0; i < numfacts; ++i) 
    value += facts_true_at[time][i]->heuristic_value;
  return value;
}

/* see if all goals are represented */
int not_all_goals_represented(int numfacts, int time)
{
  unsigned int vec = 0;
  int i;
  for(i=0; i < numfacts; ++i) 
    vec |= facts_true_at[time][i]->needed_vec;
  if ((vec & global_needed_vec) != global_needed_vec) return 1;
  return 0;
}


/* uses the "requirement" field to remove vertices in facts_true_at
   that are irrelevant.  Returns the new numfacts.  */
int remove_unsupported_vertices(int numfacts, int time)
{
  int i;
  edgelist_t e;
  for(i=0; i < numfacts; ++i) {
    for(e = facts_true_at[time][i]->requirements; e; e = e->next) {
      if (!e->endpt->is_true) {  /* can get rid of ith elt of array */
	facts_true_at[time][i]->is_true = 0;
	facts_true_at[time][i] = facts_true_at[time][numfacts-1];
	--numfacts;
	i = 0;  /* start over */
	break;
      }
    }
  }
  return numfacts;
}

/* print line number and blank spaces in front of line */
void printlinenumber (int time)
{
  int i;

  printf("%d",linenumber);
  if (linenumber < 10) printf("  ");
  else if (linenumber < 100) printf(" ");

  for(i=0; i < time; ++i) printf(" ");

  linenumber++;
}

/* print out the plan.  Current state in facts_true_at[time] */
void print_plan_recursive(int time, int maxtime, int numfacts)
{
  hash_entry elt;
  double result, randval;
  vertex_t v;
  edgelist_t e;
  int i, which, newnum, value, thecase, testvalue;
  incl_list incl;

  if (time > print_horizon) return;  /* don't print if not desired */

  printlinenumber(time);

  /* base case */
  result = value_of(numfacts, time);
  if (time == maxtime || result == utilities[time]) {
    if (result == utilities[time]) printf("success\n");
    else printf("fail\n");
    return;
  }

  /* if heuristic values tell us we cannot hope to succeed, then we fail */
  if (do_heuristic_value) {
    value = heuristic_value_of(numfacts, time);
    testvalue = time;
    if (value < testvalue) {
      printf("fail\n");
      return;
    }
  }

  /* if not all goals represented, then we fail */
  if (do_remove)
    if (not_all_goals_represented(numfacts, time)) {
      printf("fail\n");
      return;
    }
    
  if (do_remove > 1) numfacts = remove_unsupported_vertices(numfacts,time);

  elt = lookup_forward(facts_true_at[time], numfacts, time);
  if (!elt) do_error("element is NULL in print_plan???");

  /* one more base case */
  if (elt->util == util_of_failing) {
    printf("fail\n");
    return;
  }

  /* if we've seen this state before, can just do the same thing */
  if (elt->linenumber > 0 && !print_path) {
    printf("See line number %d\n",elt->linenumber);
    return;
  }

  /* save the line number in case we get to same state again */
  elt->linenumber = linenumber-1;

  /* if just printing one path, figure out which to do */
  if (print_path) {
    randval = (random() % 10000) / 10000.0;
    for(which=0, incl = elt->op->inclusions; incl && incl->prob < randval; 
	which++, incl=incl->next) randval -= incl->prob;
    if (!incl) do_error("weird probabilities in print_plan");
    thecase = which;
  }

  printf("Do: %s\n", elt->op->name);

  for(which = 0,incl = elt->op->inclusions; incl; which++, incl=incl->next) {
    if (print_path && which != thecase) continue;

    /* if more than one case, say which case it is */
    if (which > 0 || incl->next != NULL) {
      printlinenumber(time);
      printf("case %d (%.0f %%) [ ", which, 100*incl->prob);
    }
    
    /* set up facts_true_at[time+1] */
    newnum = 0;
    for(e = elt->op->out_edges; e; e = e->next) {
      if (e->incl != incl) continue;  /* a different outcome */
      if (do_remove && !e->endpt->needed) continue;/* not needed */
      facts_true_at[time+1][newnum++] = e->endpt;  /* store it */
      e->endpt->is_true = 1;                       /* mark in the graph */
      if (which > 0 || incl->next != NULL ) printf("%s ",e->endpt->name);
    }
    for(i=0; i < numfacts; ++i) {         /* copy over facts not deleted */
      v = facts_true_at[time][i]->next_time;
      if (do_remove && !v->needed) continue;       /* not needed */
      for(e = elt->op->del_edges; e; e = e->next) {
	if (e->incl != incl) continue; /* a different outcome */	
	if (e->endpt == v) break;
      }
      if (e != NULL) continue;            /* this fact WAS deleted */
      if (v->is_true) continue;           /* this fact is already listed */

      facts_true_at[time+1][newnum++] = v;         /* store it */
      v->is_true = 1;                              /* mark in the graph */
    }
    if (do_remove > 1) {
      newnum = remove_unsupported_vertices(newnum, time+1);
    }
    if (which > 0 || incl->next != NULL ) printf("]\n");
    
    print_plan_recursive(time+1, maxtime, newnum);
    
    /* clean up */
    for(i=0; i < newnum; ++i) facts_true_at[time+1][i]->is_true = 0;
  }

}

int srandom( unsigned seed);  /* comment this out depending on compiler */

/* Print out the plan */
void print_plan(int maxtime, double utility, int num_facts)
{
  if (!do_minexp) {  /* utilities correspond to prob of success */
    printf("found plan with success probability %f\n\n",utility);
  } else {
    printf("found plan with -utility (expected time) %f\n\n",-utility);
  }
  if (print_horizon < 0) print_horizon = maxtime;
  if (print_path) {
    printf("printing random paths\n");
    srandom((unsigned) start.tv_usec);
  }
  do {
    linenumber = 0;
    print_plan_recursive(0, maxtime, num_facts);
  } while(--print_path > 0);

  printf("%d entries in hash table, ",hash_inserts);
  if (hash_ctr == 0) printf("\n");
  else printf("%d hash hits, avg set size %d.\n",
		hash_hits, hash_numctr/hash_ctr);

  /* printf("%d actions tried\n", number_of_actions_tried); */
  printf("%d state steps performed\n", number_of_state_steps);
}


/* recursive forward-chaining planner.  Returns utility.
   Given the facts listed in facts_true_at[time], does a recursive
   forward-chaining search.

   There's an assumption that if value of state equals utilities[time]
   then we can return right now.

   the facts in facts_true_at[time] should be listed as is_true
 */

double basic_forward_plan(int numfacts, int time, int maxtime)
{
  double best_util = util_of_failing, current_util, result, prob_sum;
  vertex_t op, best_op,v;
  edgelist_t e;
  incl_list incl;
  int i, whichop, newnum, value;
  hash_entry elt;
  
  result = value_of(numfacts, time);
  if (result == utilities[time]) 
    return result; /* we reached the goals */
  if (time == maxtime) return util_of_failing;  /* value for failing */

  /* if heuristic values tell us we cannot hope to succeed, then return */
  if (do_heuristic_value) {
    value = heuristic_value_of(numfacts, time);
    if (value < time) return util_of_failing;
  }

  /* if needed_vec tells us we cannot hope to succeed, then return */
  if (do_remove) {
    if (not_all_goals_represented(numfacts, time)) {
      if (DEBUG_FLAG>1) printf("returning because not all goals represented\n");
      return util_of_failing;
    }
  }


  /* if we've been memoized, then we're done */
  if ((elt = lookup_forward(facts_true_at[time], numfacts, time)) != NULL)
      return elt->util;

  /* if in debugging mode, ask user for the operator to do */
  if (debugging_mode) {
    printf("time %d, choose an operator:\n", time);
    for(whichop = 0; (op = ops_at[time][whichop]) != NULL; ++whichop) {
      /* see if op is applicable */
      for(e = op->in_edges; e; e = e->next) 
	if (!e->endpt->is_true) break;   /* op not applicable */
      if (e != NULL) continue;           /* op not applicable */

      printf("  %d: %s\n",whichop, op->name);
    }
    scanf("%d",&whichop);
    best_op = ops_at[time][whichop];
  }
  
  /* go through all the non-noop operators */
  for(whichop=0; (op = ops_at[time][whichop]) != NULL; ++whichop) {
    /* if in debugging mode, only do what user wants */
    if (debugging_mode && op != best_op) continue;

    /* see if op is applicable */
    for(e = op->in_edges; e; e = e->next) 
      if (!e->endpt->is_true) break;   /* op not applicable */
    if (e != NULL) continue;           /* op not applicable */

    /* find expected utility if this op is used.  This is a weighted
       average over all possible outcomes of the op */
    current_util = 0.0;
    prob_sum = 0.0;                 /* this is for verification */
    if (op->inclusions == NULL) {
      printf("op %s has no inclusions at time %d\n",op->name, time);
      do_error("inclusion error");
    }
    for(incl = op->inclusions; incl; incl = incl->next) {

      /* set up facts_true_at[time+1] for this outcome */
      newnum = 0;
      for(e = op->out_edges; e; e = e->next) {
	if (e->incl != incl) continue;  /* a different outcome */
	if (do_remove && !e->endpt->needed) continue;/* not needed */
	facts_true_at[time+1][newnum++] = e->endpt;  /* store it */
	e->endpt->is_true = 1;                       /* mark in the graph */
      }
      for(i=0; i < numfacts; ++i) {         /* copy over facts not deleted */
	v = facts_true_at[time][i]->next_time;
	if (do_remove && !v->needed) continue;       /* not needed */
	for(e = op->del_edges; e; e = e->next) {
	  if (e->incl != incl) continue; /* a different outcome */
	  if (e->endpt == v) break;
	}
	if (e != NULL) continue;            /* this fact WAS deleted */
	if (v->is_true) continue;           /* this fact is already listed */

	facts_true_at[time+1][newnum++] = v;         /* store it */
	v->is_true = 1;                              /* mark in the graph */
      }
      /* use the pairwise "requirement" relation */
      if (do_remove > 1) {
	newnum = remove_unsupported_vertices(newnum, time+1);
      }
 	
      /* do the recursive call */
      if (DEBUG_FLAG > 1) {
	printlinenumber(time);
	printf("time %d, trying %s, event with prob %.3f\n",time, op->name,
	       incl->prob);
      }
      ++number_of_state_steps;
      if (number_of_state_steps % 100000 == 0)
	printf("...%d state steps so far\n",number_of_state_steps);

      result = basic_forward_plan(newnum, time+1, maxtime);

      if (DEBUG_FLAG > 1) {
	printlinenumber(time);
	printf("event returned with success %.3f\n",result);
      }

      current_util += (incl->prob)*result;
      prob_sum +=incl->prob;
      /* clean up */
      for(i=0; i < newnum; ++i) facts_true_at[time+1][i]->is_true = 0;
    }
    if (prob_sum < 0.99999 || prob_sum > 1.000001) {
      printf("prob sum is %f\n", prob_sum);
      do_error("illegal value");
    }

    /* if expected utility is highest so far,then remember this op */
    if (current_util >= best_util) {
      best_util = current_util;
      best_op = op;
    }
    /* utilities[time+1] is best possible, ASSUMING that solving earlier
       is better than solving later.  Would need to change this
       if have different kind of utility */
    if (current_util >= utilities[time+1]) break; /* we did best possible */
  }

  /* now, memoize us and return */
  insert_forward(facts_true_at[time], best_op, numfacts, time, best_util);
  return best_util;
}


/* helper for setting up
   - need to clean out forward hash table if we want to do automatic time.
 */
double runbasicplan(int numfacts, int maxtime)
{
  double result;
  result = basic_forward_plan(numfacts, 0, maxtime);
  return result;
}

double get_prob(incl_list e)
{
  if (e==NULL)
    return 1;
  else
    return e->prob;
}


/*****************************************************************/
/* These are the functions for memoizing.  Idea: store in a hash table
   the sets of goals that have been seen so far.
   Since we're just doing lookup, we might as well use a big table.

   For hash table, look at rand1 values. Just add the 
   values to hash since in fact you WANT to have lists that are same 
   as sets but different as sequences to collide.  Will also store the actual
   sum, since it's likely will only have equal sums when really are same
   sets.

   NEW: store sets as bit-vectors of length MAXNODES. That way can compare
   with just a few equality or bitwise AND tests. (also need to check they
   are at the same time)

 */

/* sum instead of xor since doesn't seem to matter */
long sum_arr(goal_arr a, int num)
{
  int i;
  unsigned long sum;
  for(i=0,sum=0; i < num; i++)  sum = sum + a[i]->rand1;
  return sum;
}

/* turn a goal array into a bit vector. Assume there is space in v. */
/* NOTE: if levels are equal, then uid_*'s should be equal too */
void make_bit_vector(goal_arr a, int len, bit_vector v)
{
  int i;
  vertex_t w;
  for(i=0; i < bvec_len; ++i) v[i] = 0;  /* zero out first */
  for(i=0; i < len; ++i) {
    w = a[i];
    v[w->uid_block] = v[w->uid_block] | w->uid_mask;
  }
}

/* Find the probability of a given set of facts in the initial conditions */
double find_probability(goal_arr a, int num)
{
  printf("You shouldn't be calling this function\n");
  return 0.0;
}



/* doing lookup with verification.  Other option is to hash both
   random values and to use other hash for probabilistic verification

   storing as bit vectors so can check equality easily.

   return the entry in the table, or NULL
 */
hash_entry lookup_forward(fact_arr a, int num, int time)
{
  long sum;
  static bit_vector query;
  int index, i;
  hash_entry elt;

  ++hash_ctr;  /* just for info */
  hash_numctr += num; /* just for info */
  sum = sum_arr(a, num);
  index = (int) (sum & P_HASH);  /* BITWISE AND */
  /*  if (index < 0) index += P_HSIZE; */
  make_bit_vector(a, num, query);   /* "query" is what we actually check*/
  for(elt = the_forward_table[index]; elt; elt = elt->next) {
    if (elt->sum != sum) continue;   /* not same */
    if (elt->time != time) continue; /* not same */
    /* if get here, probably the same, but need to verify */
    for(i=0; i < bvec_len; ++i) {
      if (query[i] != elt->item[i]) break;
    }
    if (i == bvec_len) {
      if (DEBUG_FLAG > 1) printf("hash table hit.\n");
      ++hash_hits; /* just for info */
      return elt;
    }
  }
  return NULL;
}


/* insert into table.  Return pointer */
hash_entry set_insert(set_table table, goal_arr a, int num, int time, 
		      double util)
{
  long sum;
  int index;
  static int mem_flag = 0;
  hash_entry elt;
  sum = sum_arr(a, num);
  index = (int) (sum & P_HASH);
  /* if (index < 0) index += P_HSIZE; */
  elt = (struct SET_ELT *) malloc(sizeof(struct SET_ELT));
  if (elt == NULL) { /* out of memory */
    if (mem_flag == 0)
      ++mem_flag, printf("no memory at insert %d\n",hash_inserts);
    return NULL;
  }
  elt->num = num;
  elt->sum = sum;
  elt->time = time;
  elt->util = util;
  make_bit_vector(a, num, elt->item);
  elt->next = table[index];
  table[index] = elt;
  return elt;
}


/* insert into the_forward_table */
void insert_forward(fact_arr a, vertex_t op, int num, int time, double util)
{
  hash_entry elt = set_insert(the_forward_table, a, num, time, util);
  ++hash_inserts;  /* global: just for info */
  elt->op = op;
}



/* insert in to hash table the_table.  This is a collection of goal-sets,
   with attached probabilities. */

void insert(goal_arr a, int num, int time, double prob)
{
  set_insert(the_table, a, num, time, prob);
  ++hash_inserts;  /* global: just for info */

}


/******************** set up ****************************/
void handle_events(void);
void do_graph_created(void);

/* set up fact_array facts_true_at.  Set up op_arr ops_at.
   Return number of facts */
int setup_forward(int maxtime)
{
  vertex_t v;
  int i, t, n;

  if (facts_true_at != NULL) free(facts_true_at);
  if (ops_at != NULL) free(ops_at);
  facts_true_at = (fact_arr *) calloc(maxtime+1,sizeof(fact_arr));
  ops_at = (op_arr *) calloc(maxtime,sizeof(op_arr));

  get_next(fact_table[0],0);
  for(n=0; (v = get_next(fact_table[0],1)) != NULL; n++) {
    facts_true_at[0][n] = v;
    v->is_true = 1;
  }
  for(t=0; t < maxtime; ++t) {
    get_next(op_table[t], 0);
    for(i=0; (v = get_next(op_table[t],1)) != NULL;) {
      if (v->is_noop) continue;                   /* don't include noops */
      if (do_remove && !v->needed)
	continue;                              /* don't include unneeded */
      ops_at[t][i++] = v;
    }
    if (i >= MAXOPS) do_error("too many ops. need to change MAXOPS in gp.h");
    ops_at[t][i] = NULL;  /* guarantee that last one is NULL */
  }
  return n;
}