/* monx-prims.cc -- the primitives (senses and actions) you find 
   called in the lap script files.  These mostly just call other libraries
   of functions in other files...

   $Log: grasp-prims.cc,v $
   Revision 1.1  1999/12/27 23:48:46  joanna
   Initial revision

   Revision 1.3  1997/09/10 01:36:43  joanna
   works with robs stuff -- fixed loads more chaining problems,
   new rob libraries.  Interactive rob ringrose, not piped yet.

   Revision 1.1  1997/08/10 18:34:11  joanna
   Initial revision

*/

#include <stdio.h>
#include <stream.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <unistd.h>
#include "monx.hh"
#include "brain-objs.hh"

#include "monx-dirattn.hh"

// from string.h
// char *strncat(char *dest, const char *src, size_t n);

typedef struct {
  int num;
  int status;
  int acking;
  int id;
  int port_num;
  int talk_socket, pipe_socket;
} PIPE;

extern "C" {
  void close_all_pipes();
  PIPE *open_client_pipe(char *hostname, int id);
  int send_comm(PIPE *pipe, char *fmt);
  int wait_for_comm(PIPE *pipe, unsigned char *buff, int maxlen, int timeout);
}

extern char *hostname;
PIPE *piper = 0;
void prec (int &val);
void psend (char *words);

static char rcsid[] = "";

/*

When you add a primitive -- 
   1) add it to this list so it can be identified by people
   2) add the code (don't seem to need declarations, just definitions)
   3) add it to initprims at the bottom, so it gets set up

Remember that if a primitive action does not return SUCCESS, the rest
of a fap will not execute!  FAILURE indicates something _very_ drastic
happened, which should abort the next element.  Actually, it may not
make sense to abort except as a result of sensing!

Here are the "grasp" prims: notice we could nuke the learning things
if you just want to make them part of the behavior they follow.

      Basic Actions - (nil, sleep, move_target)
      Grasping Actions - (reach, focus_target, focus_arm, retract, lean)
      Grasping Senses - (grasped, see_target, see_arm, arm_extended)
      Learning Actions - (arm_learn, focus_learn, lean_learn)


For blocked.lap
      Basic Actions - (nil, sleep, move_target)
      Grasping Actions - (reach, focus_target, focus_blockage, focus_arm, lean)
      Grasping Senses - (grasped, see_target, see_arm, happy)
*/


/*  ************  Basic Actions  ************ */
// # Since "life" has no goal, need an action that always fails
lapres nil () {
  return (FAILURE);}

lapres sleep () {
  usleep(10); // sleep a hundred thousandth of a second
  return (SUCCESS);
}

lapres move_target () {
  lapres res = (int) SUCCESS;
  psend ("move_target");
  prec (res);
  return(res);
}

/*  ************  Grasping Actions  ************ */

lapres reach () {
  lapres res = (int) SUCCESS;
  psend ("reach");
  prec (res);
  return(res);
}

lapres focus_target () {
  lapres res = (int) SUCCESS;
  psend ("focus_target");
  prec (res);
  return(res);
}

lapres focus_arm () {
  lapres res = (int) SUCCESS;
  psend ("focus_arm");
  prec (res);
  return(res);
}

lapres focus_blockage () {
  lapres res = (int) SUCCESS;
  psend ("focus_blockage");
  prec (res);
  return(res);
}

lapres retract () {
  lapres res = (int) SUCCESS;
  psend ("retract");
  prec (res);
  return(res);
}

lapres lean () {
  lapres res = (int) SUCCESS;
  psend ("lean");
  prec (res);
  return(res);
}

/*  ************  Grasping Senses  ************ */

int grasped () {
  int res = (int) FAILURE;
  psend ("grasped");
  prec (res);
  return(res);
}

int see_target () {
  int res = (int) FAILURE;
  psend ("see_target");
  prec (res);
  return(res);
}

int see_arm () {
  int res = (int) FAILURE;
  psend ("see_arm");
  prec (res);
  return(res);
}

int arm_extended () {
  int res = (int) FAILURE;
  psend ("arm_extended");
  prec (res);
  return(res);
}

/* grasping isn't good enough unless this is true... */
int happy () {
  int res = (int) FAILURE;
  psend ("happy");
  prec (res);
  return(res);
}

int leaned () {
  int res = (int) FAILURE;
  psend ("leaned");
  prec (res);
  return(res);
}

/*  ************  Learning Actions  ************ */

lapres arm_learn () {
  lapres res = (int) SUCCESS;
  psend ("arm_learn");
  prec (res);
  return(res);
}

lapres focus_learn () {
  lapres res = (int) SUCCESS;
  psend ("focus_learn");
  prec (res);
  return(res);
}

lapres lean_learn () {
  lapres res = (int) SUCCESS;
  psend ("lean_learn");
  prec (res);
  return(res);
}

void
initprims() {
  // basic life
  Act::add_act("nil", nil);
  Act::add_act("sleep", sleep);
  Act::add_act("move_target", move_target);

  // life with reaching
  Act::add_act("reach", reach);
  Act::add_act("focus_target", focus_target);
  Act::add_act("focus_arm", focus_arm);
  Act::add_act("focus_blockage", focus_blockage);
  Act::add_act("retract", retract);
  Act::add_act("lean", lean);
  Sense::add_sense("grasped", grasped);
  Sense::add_sense("see_target", see_target);
  Sense::add_sense("see_arm", see_arm);
  Sense::add_sense("arm_extended", arm_extended);
  Sense::add_sense("happy", happy);
  Sense::add_sense("leaned", leaned);
  Act::add_act("arm_learn", arm_learn);
  Act::add_act("focus_learn", focus_learn);
  Act::add_act("lean_learn", lean_learn);

  /* piggyback a bit to contain the corruption of the client stuff (from Rob)*/
  if (hostname) {
    int res;
    if ((piper = open_client_pipe(hostname, 42)) == NULL) {
      cerr << "grasp: Help!  Can't open to " << hostname 
	   << "/42 getting response " << res << "\n";
      exit (-3);
    }
  } // if hostname
} // initprims

/* I'm assuming I'm just getting 1 or 0 back...  (1 if its an act!) */
void 
prec (int &val) {
  static unsigned char valchar;
  if (hostname) {
    wait_for_comm(piper, &valchar, 1, 600);
    val = (int) valchar;
  }
  else
    if (val == (int) FAILURE) {
      cout << " >>> ";
      cin >> val;
    }
}

/* Just print out the format, there's no args... */
void 
psend (char *words) {
if (hostname)
  send_comm(piper, words);
else
  cout << "  " << words ;
}

