/* Lewis & Clark
 * pg 317, Mobile Robots: Inspiration to Implementation, 
 *    Jones,Flynn,Seiger, 2nd Ed. 1999
 *
 * Purpose: Move and don't get stuck 
 * History: 
 03/31/00 Permission to post granted to Alan McDonley from Klaus Peters
 04/04/00 added calls to tone() when avoiding obstacle 
          changed main() to lewis() - use lewismain.c for standalone

 */

/* UTILITY FUNCTIONS */

/********************** 
	lr_rot(detect): 	Decide direction to spin 
	                        based on IR or bumper hit
   			 	1=>detection on right
			 	2=>detection on left
	returns:   +1 "spin to left"
		   -1 "spin to right"
*/
int lr_rot(int detect) {
	return ((detect & 1) << 1) -1; }


/* BEHAVIORS */

/********************** Cruise Behavior: Always go forward ********/
int cru_trans = 0;	/* Cruise translational velocity command */
int cru_rot = 0;	/* Cruise rotational velocity command */
int cru_act = 0;	/* Cruise active flag */
int cru_def_vel = 100; 	/* Cruise default velocity */

void cruise() 
{ while (1)
  { cru_act = 1;		/* cruise always wants control */
    cru_trans = cru_def_vel;    /* Move at  the standard velocity */
    cru_rot = 0;		/* Don't rotate */
  }
}

/* ********************** Avoid Behavior: IR detect and arc away */
int av_trans = 0;		/* Avoid translational velocity command */
int av_rot = 0;		/* Avoid rotational velocity command */
int av_act = 0;		/* Avoid active flag */
int av_def_trans = 100; /* Avoid default translation velocity */
int av_def_rot = 50;	/* Avoid default rotation velocity */

void avoid()
{int ir_hit = 0;	/* record ir obstacle, 0=none, 3=both sides */
 while(1)
  {
   ir_hit = ir_detect(); 
   if (ir_hit == 0)	/* no ir obstacle detection */
  	av_act = 0;		/* avoid behavior has nothing to say */
   else if (ir_hit == 3)	/* obstacle in front (or both sides) */
	{
		av_act = 1;			/* avoid wants control */
		av_rot = -av_def_rot*2;
		av_trans = 0; 		/* Don't go forward */
		tone(678.48-440.0,.05);   /* beep an e note */
	}	
   else 				/* obstacle only right or left side*/
	{ av_act = 1;		/* avoid wants control */
	  av_trans = 0;		/* don't move forward */
	  av_rot = av_def_rot * lr_rot(ir_hit); /* rotate lft or right */
	  if (ir_hit==1) tone(535.92-440.0,0.1); /* beep a c note */
	  else tone(796.4-660.0,0.1); /* beep a g note */
	}
  } /* end while */
} /* end avoid() */

/*********************Escape Behavior: move away from a collision */
int es_trans = 0;		/* Escape translational velocity command */
int es_rot = 0;		/* Escape rotational velocity command */
int es_act = 0;		/* Escape active flag */
int es_def_trans = 100; /* Escape default translation velocity */
int es_def_rot = 50;	/* Escape default rotation velocity */
float es_bf = 1.0;	/* Backward/Forward time */
float es_spin = 1.0;	/* spin in place time */

void escape()
{ int es_hit = 0;		/* collision direction storage */
  while (1) {
	es_hit = bumper();	/* get and remember bumper state */
	if (es_hit & 3) { 	/* if left or right collision... */
		es_act = 1;		/* escape wants control */
		es_trans = (- es_def_trans); /* backup */
		es_rot = 0;		/* don't turn while backing */
		sleep(es_bf);	/* sleep (while backing) for a while */

		es_trans = 0; 	/* rotate only */
		es_rot = lr_rot(es_hit) * es_def_rot; /* rotate l or r */
		sleep(es_spin);	/* sleep (while spinning) */

		es_trans = es_def_trans;	/* go forward (a bit) */
		es_rot = 0;		/* done spinning */
		sleep(es_bf);	/* sleep (and walk fwd) */
		es_trans = 0;	/* stop going fwd from avoid behavior */
		es_act = 0;		/* we're done, relinquish control */
	}
	else if (es_hit & 4) {	/* bumped from back */
		es_act = 1;		/* we (escape() wants control) */
		es_trans = 0;	/* stop fwd motion */
		es_rot = es_def_rot;	/* spin in place */
		sleep(es_spin);	/* spin a while */
		es_rot = 0;		/* stop spinning */
		es_trans = 0;	/* stop any fwd motion */
		es_act = 0;		/* out a here */
	}
  } /* end while */
} /* end escape() */


/***************** Motor driver behavior: constantly tell motor cmd */
int mot_trans = 0;
int mot_rot = 0;

void motors() {
	while(1) drive(mot_trans, mot_rot);
}

/* ************Drive motor arbitration: prioritize behavior commands */
void arbitrate() {
	while(1) {
		if (es_act) {	/* escape wants control... */
			mot_trans = es_trans;  /* pass along escape's cmds */
			mot_rot = es_rot;
		} else if (av_act) {	/* avoid wants control... */
			mot_trans = av_trans;
			mot_rot = av_rot;
		} else if (cru_act) {	/* cruise wants control... */
			mot_trans = cru_trans;
			mot_rot = cru_rot; 
		} else {
			mot_trans = 0;
			mot_rot = 0;
		}
	} /* end while */
} /* end arbitrate */

/* ************report behavior to LCD ************ */
void report() {
	while (1) {
		printf("L&C  E:%d A:%d C:%d", es_act, av_act, cru_act);
		printf("  Tr:%d Rt:%d\n", mot_trans, mot_rot);
		sleep(0.2);  /* Don't overtax the LCD */
	} /* end while */
}


void lewis() 
{
  printf("Lewis and Clark\n");
  sleep(1.0);	/* give user time to see display */
  start_process(escape());
  start_process(avoid());
  start_process(cruise());
  start_process(motors());
  start_process(arbitrate());
  start_process(report());
		
} /* end lewis */