/*------------------------------------------------------------------------
| Filename: map-eve.c
|
| Author:       Nicholas Tay,  UWA 1998
| Changes       Thomas Braunl, UWA 2000
|
| Description:	Map Generation Scheme for the Eyebot.
|
-------------------------------------------------------------------------- */

#include "eyebot.h"

#include <stdio.h>
#include <math.h>
#include <stdlib.h>

typedef struct {
  int x1;
  int y1;
  int x2;
  int y2;
} Segment;	/* Representation of XSegments - stores line-segments */

#define GRID_WIDTH 	80 	/* The width of each cell */
#define GRID_DIMENSIONS 80	/* The dimension of the Grid */
#define GRID_CENTER 	40 	/* Half the GRID_DIMENSIONS */
#define PSD_MAX		210     /* The Maximum PSD range that is reliable */
				/*  OFFSET is included in value */
#define PSD_LIMIT	400
#define OFFSET_RIGHT	38	/* Offset value for Right PSD */
#define OFFSET_LEFT	38	/* Offset value for Left PSD */
#define OFFSET_FRONT	0	/* Offset value for Front PSD */

#ifndef ROBOT_RADIUS
#define ROBOT_RADIUS 75		/* The radius of the robot.
				   Value is approximate because Eyebot is not
				   perfectly circular */
#endif

/* --------------------= VEHStop =---------------------- */

/** Stops the robot.

	@param 	vw_	The VW handle
	@author Nicholas Tay, CIIPS 1998		*/
void VEHStop(VWHandle vw_)
{
  VWSetSpeed(vw_, 0, 0);
}

/* --------------------= MAP_Check =--------------------- */

/** Checks adjacent cells for a match in (value_).

	@param 	map_	The map grid
	@param 	pos_	The index of the grid
	@param 	value_	The value of the cell that the routine tries to match
	@return TRUE, 	If a match is found
	@author Nicholas Tay, CIIPS 1998
	@see	Display_MAP	*/
int MAP_Check(char *map_, int pos_, int value_)  {
  int i, j, temp = 0;

  for(i = -1; i < 2; i++)
    for(j = -1; j < 2; j++)

      /* Evaluates neighbouring cells to determine a match */
      if(map_[pos_ - i * GRID_DIMENSIONS + j] == value_)
        temp = 1;
  return temp;
}

/* --------------------= MAP_Segment =----------------------- */

/** Obtains the equivalent map position for the robot position.

	@param	vals_	The position of the robot 
	@author Nicholas Tay, CIIPS 1998  */
int MAP_Segment(float vals_)  {
  return vals_ * 1000.0 + GRID_WIDTH * GRID_CENTER + 100;
}


/* --------------------= MAP_Pos =------------------------ */

/** Obtains the array index for a particular grid cell.
	Given a particular robot position, returns the cell that 
	represents the robot position.

	@param 	temp	Current position of the robot
	@param	dist	Distance received from the PSD sensor
	@param	offset_	The angle offset (Depends on which PSD used)
	@param  x	The array index
	@param  y 	The array index
	@author Nicholas Tay, CIIPS 1998
	@see	MAP_Locate MAP_Location */
void MAP_Pos(PositionType temp, int dist, float offset_, int *x, int *y)  {
  float a, b;

  a = temp.x + (float)dist*cos(temp.phi + offset_) / 1000.0;
  b = temp.y + (float)dist*sin(temp.phi + offset_) / 1000.0;
  *x = ((int)(a * 1000) / GRID_WIDTH) + GRID_CENTER;
  *y = ((int)(-b * 1000) / GRID_WIDTH) + GRID_CENTER;
  if(a < 0)
    *x -= 1;
  if(b > 0)
    *y -= 1;
}

/* --------------------= MAP_Angle =--------------------------- */

/** Finds the difference between two radians.  The order of the radians
	is important, as the function returns a positive difference.

	@param	x_	The first radians
	@param 	y_	The second radians
	@return	The angle difference in radians.  The angle is positive
	@author Nicholas Tay, CIIPS 1998  	*/
radians MAP_Angle(radians x_, radians y_)  {
  if(x_ < 0)
    x_ += 2 * M_PI;
  if(y_ < 0)
    y_ += 2 * M_PI;
  if(y_ < x_)
    y_ += 2 * M_PI;
  return (y_ - x_);
}

/* ---------------------= MAP_Location =--------------------------- */

/** Obtains the location of a cell in the grid in terms of its robot position.
	Performs the reverse function of "MAP_Pos".  The robot position
	returned is the center of the cell. 

	@param  target	The position (PositionType) of the cell
	@param 	index_	The cell array index
	@return The position of the center of the cell
	@author Nicholas Tay, CIIPS 1998  	
	@see	MAP_Pos */
void MAP_Location(PositionType *target_, int index_)  {
  int x, y;

  y = index_ / GRID_DIMENSIONS;
  x = index_ % GRID_DIMENSIONS;
  target_->x = ((float)x - (float)GRID_CENTER + 0.5) * (float)GRID_WIDTH 
		/ 1000.0; 
  target_->y = ((float)y - (float)GRID_CENTER + 0.5) * (float)GRID_WIDTH
		/ -1000.0; 
}

/* ----------------------= MAP_Position =---------------------------- */

/** VWGetPosition equivalent.  Obtains the position of the robot.  
	However, this function updates the robot position to the grid map.

	@param	vw_ 	The VW handle for the robot
	@param	pos	The position (PositionType) of the robot
	@param  map_	The grid for storage
	@return The position of the robot	
	@author Nicholas Tay, CIIPS 1998	*/
int MAP_Position(VWHandle vw_, PositionType *pos_, char *map_)  {
  int i = 0, j = 0, a, b;

  /* Obtain robot position */
  VWGetPosition(vw_, pos_);
  MAP_Pos(*pos_, 0, 0, &i, &j);

  /* Updating the Grid Map */
  if((map_[j * GRID_DIMENSIONS + i] != 1) && 	
		(map_[j * GRID_DIMENSIONS + i] != 5))
    map_[j * GRID_DIMENSIONS + i] = 0;

  /* Updates its neighbouring cells to "clear" (0) if they were initially
	defined as "clear but unexplored" (6)  */
  for(a = -1; a < 2; a++)
    for (b = -1; b < 2; b++)
      if(map_[(j + a) * GRID_DIMENSIONS + i + b] == 6)
        map_[(j + a) * GRID_DIMENSIONS + i + b] = 0;

  return j * GRID_DIMENSIONS + i;
}

/* ------------------= MAP_Dist =------------------------- */

/** Obtains the distance to the nearest obstacle using the right PSD sensor.
	Records the obstacle and all spaces between the obstacle and the 
	robot into the grid.
	Also checks the left PSD sensor for obstacles.

	@param	vw_	The VW Handle
	@param	psd_	The right PSD sensor
	@param 	psdLeft	The left PSD sensor
	@param  ir_r_	The right infra-red sensor
	@param	map_	The grid for storage
	@return The distance to the obstacle using the right PSD sensor
	@author Nicholas Tay, CIIPS 1998	*/	
int MAP_Dist(VWHandle vw_, PSDHandle psd_, PSDHandle psdLeft, 
	IRHandle ir_r_, char *map_) {
  int i, j, k, x = 0, y = 0, obstacle_, dist;
  PositionType pos_n_;

  dist = PSDGet(psdLeft) - OFFSET_LEFT;
  VWGetPosition(vw_, &pos_n_);

  /* Checks Left PSD Sensors */
  if(dist < PSD_MAX)  {
      /* An Obstacle is detected */

      /* Marks the cell containing the obstacle as "blocked but unexplored" */
      MAP_Pos(pos_n_, dist + ROBOT_RADIUS, M_PI/2, &x, &y);
      if((x >= 0) && (x < GRID_DIMENSIONS) && (y >= 0) && (y < GRID_DIMENSIONS))
        if((map_[y * GRID_DIMENSIONS + x] != 0) &&
              (map_[y * GRID_DIMENSIONS + x] != 1))
          if(!MAP_Check(&map_[0], y * GRID_DIMENSIONS + x, 1))
            map_[y * GRID_DIMENSIONS + x] = 4;

      /* Set the cell beside the robot as "clear" */
      if(dist > 80) {
        MAP_Pos(pos_n_, ROBOT_RADIUS, M_PI/2, &x, &y);
        if((x >= 0) && (x < GRID_DIMENSIONS) && (y >= 0) && 
		(y < GRID_DIMENSIONS))
          map_[y * GRID_DIMENSIONS + x] = 0;
      }
  } else {
    /* Sensors did not detect an obstacle within out given range */

    /* Sets the cell at the limit of PSD range to "clear but unexplored" (6) */
    MAP_Pos(pos_n_, ROBOT_RADIUS + PSD_MAX, M_PI/2, &x, &y);
    if((x >= 0) && (x < GRID_DIMENSIONS) && (y >= 0) && (y < GRID_DIMENSIONS))
      if((map_[y * GRID_DIMENSIONS + x] != 0) &&
            (map_[y * GRID_DIMENSIONS + x] != 1))
    	if(!MAP_Check(&map_[0], y * GRID_DIMENSIONS + x, 1))
          if(!MAP_Check(&map_[0], y * GRID_DIMENSIONS + x, 0))
      	    map_[y * GRID_DIMENSIONS + x] = 6;
  }

  /* Evaluates Right PSD Sensor */
  obstacle_ = PSDGet(psd_) - OFFSET_RIGHT;
  VWGetPosition(vw_, &pos_n_);
  if(obstacle_ < PSD_MAX)  {
    /* An obstacle is detected */

    dist = obstacle_;
    for(k = 0; k < 5; k++)  {
      MAP_Pos(pos_n_, dist + ROBOT_RADIUS, -M_PI/2, &x, &y);
      if((x >= 0) && (x < GRID_DIMENSIONS) && (y >= 0) && 
	      (y < GRID_DIMENSIONS))
      {
	/* The cell containing the obstacle is marked as "blocked" (1) */
        if(dist == obstacle_) {
 	  if(map_[y * GRID_DIMENSIONS + x] != 5)
            map_[y * GRID_DIMENSIONS + x] = 1;

	  /* Neighbouring cells which are unexplored (4) or (6) are cleared */
          for(i = -2; i < 3; i++)
            for(j = -2; j < 3; j++)
	      if((((y + i) * GRID_DIMENSIONS + x + j) > 0) &&
		      ((y + i) * GRID_DIMENSIONS + x + j) < (GRID_DIMENSIONS*
		      GRID_DIMENSIONS))
                if((map_[(y + i) * GRID_DIMENSIONS + x + j] == 4) ||
	              (map_[(y + i) * GRID_DIMENSIONS + x + j] == 6))
	          map_[(y + i) * GRID_DIMENSIONS + x + j] = 3;
        
        /* Sets cells between the robot and obstacle as "clear" (0) */ 
        } else if((map_[y * GRID_DIMENSIONS + x] != 1) 
		&& (map_[y * GRID_DIMENSIONS + x] != 5))
              	map_[y * GRID_DIMENSIONS + x] = 0;
      }

      dist -= obstacle_ / 4;
    }
  }

  /* Returns Right PSD Distance */
  return obstacle_;
}

/* ------------------= MAP_Orient =----------------------- */

/** Obtains the orientation required for the robot to face the target position.
	This routine is required for Path Navigation. 

	@param 	vw_	The VW handle
	@param  current The current position of the robot
	@param  pos_n	The target destination for the robot
	@return The orientation required for the robot (radians)
	@author Nicholas Tay, CIIPS 1998	
	@see	MAP_Navigation MAP_Target */
float MAP_Orient(VWHandle vw_, PositionType current,  PositionType pos_n)  {
  float angle = 0;

  /* Using the robot's position, determines the desired orientation the robot
	must face to reach the destination */
  if(current.x == pos_n.x)
  {
    if(pos_n.y >= current.y)
      return  M_PI / 2;
    else
      return -M_PI / 2;
  }
  angle = atan((pos_n.y - current.y) / fabs(pos_n.x - current.x));
  if(pos_n.x < current.x)
    angle = M_PI - angle;

  return angle;
}  

/* ----------------= MAP_Navigation =-------------------- */

/** Performs a robot navigation routine to reach a destination.
	Includes code for obstacle avoidance and boundary following.
	Adapted from the DistBug architecture.

	@param  vw_	The VW handle
	@param  psd_x	The PSD Handles for the left, front and right PSDs
	@param  if_f	The front infra-red Handle
	@param  if_r	The right infra-red Handle
	@param  if_l	The left infra-red Handle
	@param  target	The target cell in the grid
	@param  map_	The grid map used for storage
	@author Nicholas Tay, CIIPS 1998    	
	@see	Wall_Follow_Right MAP_Target MAP_Orient MAP_Location */
void MAP_Navigation(VWHandle vw_, PSDHandle *psd_x, IRHandle ir_f, 
	IRHandle ir_r, IRHandle ir_l, int target, char *map_)  {
  PositionType dest_n, pos_n_, stored;
  SpeedType stopped_;
  int i, j, dist_ = 0, prev_ = 0, x1, x2, y1, y2, looped = 0;


  MAP_Location(&dest_n, target);
  VWGetPosition(vw_, &stored);
  LCDPutString("Navigating to point\n");

  /* Rotate robot until it faces the correct orientation to target */
  do {
    VWSetSpeed(vw_, 0, 0.2);
    VWGetPosition(vw_, &pos_n_);
  } while(MAP_Angle(pos_n_.phi, MAP_Orient(vw_, pos_n_, dest_n)) 
	> 0.03);
  VEHStop(vw_);

  /* Begins navigation trajectory */
  do {
    VWSetSpeed(vw_, 0.13, 0);
    MAP_Position(vw_, &pos_n_, &map_[0]);
    VWGetSpeed(vw_, &stopped_);

    /* Condition to eliminate the target destination if the destination is
	unreachable (ie. no path exists ).  */
    if(looped != 0)  
      if((fabs(pos_n_.x - stored.x) <= 0.15) && (fabs(pos_n_.y - stored.y) 
		<= 0.15))  {
	map_[target] = 0;	
        return;
      }

    if((fabs(pos_n_.x - dest_n.x) + fabs(pos_n_.y - dest_n.y)) <
	(fabs(stored.x - dest_n.x) + fabs(stored.y - dest_n.y)))  {
      VWGetPosition(vw_, &stored);
      looped = 0;
    }
    if((fabs(pos_n_.x - stored.x) > 0.15) && (fabs(pos_n_.y - stored.y) > 0.15))
      looped = 1;

    /* An obstacle is detected obstructing the robot's path */
    if(((PSDGet(psd_x[1]) - OFFSET_FRONT) < 80) || (IRRead(ir_f) == 0) ||
	((stopped_.v < 0.05) && (stopped_.w == 0))) {
      if((PSDGet(psd_x[0]) - OFFSET_LEFT) < (PSDGet(psd_x[2]) - OFFSET_RIGHT))
      {

	/* The left side of the robot is closer to the obstacle, thus perform
	   Boundary-Following using the left PSD Sensors */
	VWSetSpeed(vw_, 0, -0.2);

	/* Rotate the robot so that the robot is parallel to the left wall */
	do {
	  prev_ = dist_;
          OSWait(1);
          dist_ = PSDGet(psd_x[0]) - OFFSET_LEFT; 
        } while (prev_ >= dist_);
        VEHStop(vw_);
        MAP_Position(vw_, &pos_n_, &map_[0]);

	/* Perform left boundary-following */
        while((MAP_Angle(pos_n_.phi, MAP_Orient(vw_, pos_n_, dest_n)) > 
                0.05) && (MAP_Angle(MAP_Orient(vw_, pos_n_, dest_n),
		pos_n_.phi) > 0.05) && (fabs(pos_n_.x - dest_n.x) +
		fabs(pos_n_.y - dest_n.y) > 0.16)) {

          dist_ = PSDGet(psd_x[0]) - OFFSET_LEFT;
	  MAP_Pos(pos_n_, ROBOT_RADIUS, 0, &x1, &y1);
    	  MAP_Pos(pos_n_, GRID_WIDTH + ROBOT_RADIUS, 0, &x2, &y2);

          VWGetSpeed(vw_, &stopped_);

	  /* Condition for robot stalled (collision) */
          if((stopped_.v < 0.05) && (stopped_.w == 0))
            VWSetSpeed(vw_, 0, -0.3);

	  /* Condition for boundary conditions (reach limits of grid map) */
    	  else if((map_[y1 * GRID_DIMENSIONS + x1] == 5) ||
        	(map_[y2 * GRID_DIMENSIONS + x2] == 5)) 
	    VWSetSpeed(vw_, 0, -0.3);

	  /* Obstacle is detected in front of robot - rotates right to avoid */
          else if(((PSDGet(psd_x[1]) - OFFSET_FRONT) < 50) ||
		(IRRead(ir_f) == 0))
          {
            VEHStop(vw_);
            VWDriveTurn(vw_, -M_PI/4, 0.4);
            while(!VWDriveDone(vw_))
            prev_ = PSDGet(psd_x[0]) - OFFSET_LEFT;
            while((dist_ = PSDGet(psd_x[0]) - OFFSET_LEFT) <= prev_)
            {
              VWSetSpeed(vw_, 0, -0.30);
              prev_ = dist_;
            }

          } else {
	    MAP_Pos(pos_n_, ROBOT_RADIUS, 0, &x1, &y1);
    	    MAP_Pos(pos_n_, GRID_WIDTH + ROBOT_RADIUS, 0, &x2, &y2);

	    /* Condition for boundary limits to the left of robot - must 
		proceed in the same direction */
    	    if((map_[y1 * GRID_DIMENSIONS + x1] == 5) ||
        	(map_[y2 * GRID_DIMENSIONS + x2] == 5)) 
	       VWSetSpeed(vw_, 0.15, 0); 

	    /* Possible collision with obstacle - collision avoidance routine */
            else if(IRRead(ir_l) == 0)
              VWSetSpeed(vw_, 0, -0.2);

	    /* Robot lost the obstacle (outward corner).  Curve left to maintain
		contact with the boundary */
            else if((PSDGet(psd_x[0]) - OFFSET_LEFT) > 100)
            {
              VWSetSpeed(vw_, 0.05, 0.55);
              dist_ = prev_;
            }

	    /* Veering away from obstacle, adjust trajectory */
            else if(dist_ > prev_)
              VWSetSpeed(vw_, 0.15, 0.2);

	    /* Veering too close to obstacle, adjust trajectory */
            else if(dist_ < prev_)
              VWSetSpeed(vw_, 0.15, -0.2);

	    /* Robot should maintain trajectory - moving parallel */
            else VWSetSpeed(vw_, 0.15, 0);
          }
          prev_ = dist_;
          MAP_Position(vw_, &pos_n_, &map_[0]);
        }
      } else {
	/* The right side of the robot is closer top the wall - enact 
	   a Boundary-Following routine using the right sensors */
       
	VWSetSpeed(vw_, 0, 0.2);

	/* Rotates robot so that robot is parallel to right wall */
        do {
	  prev_ = dist_;
          OSWait(1);
          dist_ = PSDGet(psd_x[2]) - OFFSET_RIGHT; 
        } while (prev_ >= dist_);
        VEHStop(vw_);

        MAP_Position(vw_, &pos_n_, &map_[0]);

	/* Begin right wall-following routine */
        while((MAP_Angle(pos_n_.phi, MAP_Orient(vw_, pos_n_, dest_n)) >
                0.05) && (MAP_Angle(MAP_Orient(vw_, pos_n_, dest_n),
		pos_n_.phi) > 0.05) && (fabs(pos_n_.x - dest_n.x) +
		fabs(pos_n_.y - dest_n.y) > 0.16)) {
  
  	  dist_ = PSDGet(psd_x[2]) - OFFSET_RIGHT;
          MAP_Pos(pos_n_, ROBOT_RADIUS, 0, &x1, &y1);
          MAP_Pos(pos_n_, GRID_WIDTH + ROBOT_RADIUS, 0, &x2, &y2);
	  VWGetSpeed(vw_, &stopped_);

	  /* Condition for robot stalled (collision) */ 
  	  if((stopped_.v < 0.05) && (stopped_.w == 0))
      	    VWSetSpeed(vw_, 0, 0.3);

	  /* Condition for reaching boundary limits */
    	  else if((map_[y1 * GRID_DIMENSIONS + x1] == 5) ||
            	(map_[y2 * GRID_DIMENSIONS + x2] == 5)) 
	    VWSetSpeed(vw_, 0, 0.3);

	  /* Condition for obstacle in front of robot - rotate left to avoid */
  	  else if(((PSDGet(psd_x[1]) - OFFSET_FRONT) < 50) ||
		(IRRead(ir_f) == 0))
          {
    	    VEHStop(vw_);
    	    VWDriveTurn(vw_, M_PI/4, 0.4);
    	    while(!VWDriveDone(vw_))
      	    prev_ = PSDGet(psd_x[2]) - OFFSET_RIGHT;
    	    while((dist_ = PSDGet(psd_x[2]) - OFFSET_RIGHT) <= prev_)
      	    {
      	      VWSetSpeed(vw_, 0, 0.30);
      	      prev_ = dist_;
      	    }
    	  }
          else {

            MAP_Pos(pos_n_, ROBOT_RADIUS, -M_PI/2, &x1, &y1);
    	    MAP_Pos(pos_n_, GRID_WIDTH + ROBOT_RADIUS, -M_PI/2, &x2, &y2);

	    /* Condition for boundary limits reached */
    	    if((map_[y1 * GRID_DIMENSIONS + x1] == 5) ||
        	(map_[y2 * GRID_DIMENSIONS + x2] == 5)) 
	      VWSetSpeed(vw_, 0.15, 0);
	
	    /* IR sensors detect an obstacle too close to robot - enact 
		collision avoidance routine */
 	    else if(IRRead(ir_r) == 0)
	      VWSetSpeed(vw_, 0, 0.2);

	    /* No obstacle detected (outward corner) - robot must turn right
	       to relocate obstacle */
  	    else if((PSDGet(psd_x[2]) - OFFSET_RIGHT) > 100)  
            {
              VWSetSpeed(vw_, 0.05, -0.55);
              dist_ = prev_;
            }

	    /* Veering too far from obstacle, adjust trajectory */
  	    else if(dist_ > prev_)  
      	      VWSetSpeed(vw_, 0.15, -0.2);

	    /* Veering too close to obstacle, adjust trajectory */
  	    else if(dist_ < prev_)
      	      VWSetSpeed(vw_, 0.15, 0.2);

	    /* Robot moving parallel to obstacle - maintain trajectory */
            else VWSetSpeed(vw_, 0.15, 0);
 	  }
  	  prev_ = dist_;
          MAP_Position(vw_, &pos_n_, &map_[0]);
        }
      }
    }
  } while(fabs(pos_n_.x - dest_n.x)  + 
		fabs(pos_n_.y - dest_n.y)  > 0.16);

  VEHStop(vw_);
  for(i = -1; i < 2; i++)
    for(j = -1; j < 2; j++)
      if(map_[target + GRID_DIMENSIONS * i + j] == 6)
	map_[target + GRID_DIMENSIONS * i + j] = 0;
  if(map_[target] == 4)
    map_[target] = 3;
  
}

/* ----------------= MAP_Locate =---------------------- */

/** Locates the nearest unexplored obstacle to the robot. And proceeds to that
	obstacle.  The maximum range of operation is the limits to the PSD
	set by PSD_MAX.

	@param	vw_	The VW Handle
	@param	x_	The PSD Handle for the front PSD sensor
	@param  ir_	The forward infra-red sensor
	@param	map_	The address of the occupation grid system
	@author Nicholas Tay, CIIPS 1998   */
int MAP_Locate(VWHandle vw_, PSDHandle x_, IRHandle ir_, char *map_)  {
  int mindist_ = PSD_MAX, obstacle_, p_x, p_y, x = 0, y = 0;
  radians angle_ = 0;
  PositionType pos_n_, stop_;
  SpeedType    stopped_;

  VWDriveTurn(vw_, 2 * M_PI, 0.5);

  /* Rotates around a full circle, locating obstacles */
  while (!VWDriveDone(vw_)) {
    MAP_Position(vw_, &pos_n_, &map_[0]);
    MAP_Pos(pos_n_, 0, 0, &p_x, &p_y);
    obstacle_ = PSDGet(x_) - OFFSET_FRONT;
    if(IRRead(ir_) == 0)
      obstacle_ = 35;

    /* Obstacle detected */
    if(obstacle_ < PSD_MAX)  {
      MAP_Pos(pos_n_, obstacle_ + ROBOT_RADIUS, 0, &x, &y);

      /* Ensure obstacle is not previously explored */
      if((x >= 0) && (x < GRID_DIMENSIONS) && (y >= 0) && (y < GRID_DIMENSIONS))
        if((map_[y * GRID_DIMENSIONS + x] != 0) && 
		(map_[y * GRID_DIMENSIONS + x] != 1)) 
          if(!MAP_Check(&map_[0], y * GRID_DIMENSIONS + x, 1)) {
            map_[y * GRID_DIMENSIONS + x] = 4;
            if(obstacle_ < mindist_)  {
              mindist_  = obstacle_;
              angle_    = pos_n_.phi;
            }
          }

    /* Mark map according for the sensor detecting no obstacles */
    } else if (obstacle_ >= PSD_MAX)  {
      MAP_Pos(pos_n_, PSD_MAX + ROBOT_RADIUS, 0, &x, &y);
      if((x >= 0) && (x < GRID_DIMENSIONS) && (y >= 0) && (y < GRID_DIMENSIONS))
        if((map_[y * GRID_DIMENSIONS + x] != 0) && 
		(map_[y * GRID_DIMENSIONS + x] != 1)) 
      	  if((map_[y * GRID_DIMENSIONS + x] != 0) &&
            	(map_[y * GRID_DIMENSIONS + x] != 1))
        if(!MAP_Check(&map_[0], y * GRID_DIMENSIONS + x, 1))
            map_[y * GRID_DIMENSIONS + x] = 6;
    } 
    
  } 
  VEHStop(vw_);

  if(mindist_ == PSD_MAX)
    return 0;
  
  MAP_Position(vw_, &stop_, &map_[0]);
  VWDriveTurn(vw_, -MAP_Angle(angle_, stop_.phi), M_PI / 3);
  while(!VWDriveDone(vw_));
  VEHStop(vw_);


  /* Drive robot to the nearest undeteected obstacle */
  VWSetSpeed(vw_, 0.15, 0);
  do {
    MAP_Position(vw_, &stop_, &map_[0]);
    VWGetSpeed(vw_, &stopped_);
  } while(IRRead(ir_));
  VEHStop(vw_);

  return 1;
}


/* -------------- Wall_Follow_Right ----------------- */

/** A boundary-following algorithm that tracks the right side of the robot.

 	@param	vw_	The VW Handle  
	@param	psd_	The addresses of the Right, Front and Left PSD sensors
	@param	ir_f_	The front infra-red Handle
	@param	ir_r_	The right middle infra-red Handle (IR_RM)
	@param	map	The address for the occupation grids
	@param	segs	The address for the configuration space
	@param	count	The counter for the number of line-segments
	@author Nicholas Tay, CIIPS 1998  
	@see	MAP_Navigation */
void Wall_Follow_Right(VWHandle vw_, PSDHandle *psd_, IRHandle ir_f_,
	IRHandle ir_r_, char *map_, Segment *segs_, int *count_) 
{
  int 		dist_ = 0, prev_ = -1, looped_ = 0, x1, y1, x2, y2, turn_ = 0;
  int		val_ = 0;
  radians	orient_ = 0;
  SpeedType     stopped_;
  PositionType  pos_n_, start_;

  /* Rotate robot so that it is approximately parallel to obstacle */
  VWDriveTurn(vw_, M_PI / 2, 0.4);
  while(!VWDriveDone(vw_))
    val_ = MAP_Dist(vw_, psd_[2], psd_[0], ir_r_, &map_[0]);
  VEHStop(vw_);
   
  VWSetSpeed(vw_, 0.2, 0);
  MAP_Position(vw_, &pos_n_, &map_[0]);
  MAP_Position(vw_, &start_, &map_[0]);

  /* Store configuration space map */
  if(*count_ < GRID_DIMENSIONS*10) {
    segs_[*count_].x1 = MAP_Segment(start_.x) +
	(val_ + ROBOT_RADIUS) * cos(start_.phi - M_PI/2); 
    segs_[*count_].y1 = MAP_Segment(start_.y) +
	(val_ + ROBOT_RADIUS) * sin(start_.phi - M_PI/2); 
  }

  /* Wall-Following Routine */
  while((looped_ == 0) || ((fabs(start_.x - pos_n_.x) > 0.1) || 
				(fabs(start_.y - pos_n_.y) > 0.1)))  {

    MAP_Pos(pos_n_, ROBOT_RADIUS, 0, &x1, &y1);
    MAP_Pos(pos_n_, GRID_WIDTH + ROBOT_RADIUS, 0, &x2, &y2);
    VWGetSpeed(vw_, &stopped_);
    dist_ = MAP_Dist(vw_, psd_[2], psd_[0], ir_r_, &map_[0]);

    /* Condition for robot stall (collision) */
    if((stopped_.v < 0.025) && (stopped_.w == 0))
      VWSetSpeed(vw_, 0, 0.3);

    /* Condition for grid boundary limit reached */
    else if((map_[y1 * GRID_DIMENSIONS + x1] == 5) ||
	(map_[y2 * GRID_DIMENSIONS + x2] == 5)) {
      VWSetSpeed(vw_, 0, 0.4);

      /* Store configuration space map */
      if((turn_ == 0) && (*count_ < 10*GRID_DIMENSIONS))  {
        segs_[*count_].x2 = MAP_Segment(pos_n_.x) +
                (ROBOT_RADIUS + dist_) * cos(pos_n_.phi - M_PI/2) +
		turn_ * ROBOT_RADIUS * cos(pos_n_.phi);
        segs_[*count_].y2 = MAP_Segment(pos_n_.y) +
                (ROBOT_RADIUS + dist_) * sin(pos_n_.phi - M_PI/2) +
		turn_ * ROBOT_RADIUS * sin(pos_n_.phi);
        *count_ += 1;
      }
      turn_ = 1;

    /* Obstacle detected in front of robot - enact left turn to avoid 
	collision */
    } else if(((PSDGet(psd_[1]) - OFFSET_FRONT) <= GRID_WIDTH) || 
	(IRRead(ir_f_) == 0))
    {
      VEHStop(vw_);
      VWSetSpeed(vw_, 0, 0.30);
      orient_ = pos_n_.phi - M_PI / 5.0;

      /* Rotation until robot reaches corner of the boundary */
      while((dist_ = MAP_Dist(vw_, psd_[2], psd_[0], ir_r_, &map_[0])) >= prev_)
      {
        prev_ = dist_;
        if((dist_ <= 120) && (turn_ == 0) && 
		(*count_ < GRID_DIMENSIONS*10)) {
          MAP_Position(vw_, &pos_n_, &map_[0]);

	  /* Condition to store configuration space map */
          if(MAP_Angle(orient_, pos_n_.phi) >= (M_PI / 12.0))   {
	    orient_ = pos_n_.phi;
            segs_[*count_].x2 = MAP_Segment(pos_n_.x) +
		(ROBOT_RADIUS + dist_) * cos(pos_n_.phi - M_PI/2);
            segs_[*count_].y2 = MAP_Segment(pos_n_.y) +  
		(ROBOT_RADIUS + dist_) * sin(pos_n_.phi - M_PI/2);
            *count_ += 1;
            segs_[*count_].x1 = MAP_Segment(pos_n_.x) + 
		(ROBOT_RADIUS + dist_) * cos(pos_n_.phi - M_PI/2);
            segs_[*count_].y1 = MAP_Segment(pos_n_.y) +
		(ROBOT_RADIUS + dist_) * sin(pos_n_.phi - M_PI/2);
	  }
	}
      }

      orient_ = pos_n_.phi - M_PI / 5.0;

      /* Rotation until robot is parallel to new side of obstacle */
      while((dist_ = MAP_Dist(vw_, psd_[2], psd_[0], ir_r_, &map_[0])) <= prev_)
      {
        prev_ = dist_;
        if((dist_ <= 120) && (turn_ == 0) &&
		(*count_ < GRID_DIMENSIONS*10)) {
          MAP_Position(vw_, &pos_n_, &map_[0]);

	  /* Condition to store configuration space map */
          if(MAP_Angle(orient_, pos_n_.phi) >= (M_PI / 12.0))   {
            orient_ = pos_n_.phi;
            segs_[*count_].x2 = MAP_Segment(pos_n_.x) +
                (ROBOT_RADIUS + dist_) * cos(pos_n_.phi - M_PI/2);
            segs_[*count_].y2 = MAP_Segment(pos_n_.y) +
                (ROBOT_RADIUS + dist_) * sin(pos_n_.phi - M_PI/2);
            *count_ += 1;
            segs_[*count_].x1 = MAP_Segment(pos_n_.x) +
                (ROBOT_RADIUS + dist_) * cos(pos_n_.phi - M_PI/2);
            segs_[*count_].y1 = MAP_Segment(pos_n_.y) +
                (ROBOT_RADIUS + dist_) * sin(pos_n_.phi - M_PI/2);
          }
        }
      }
      VEHStop(vw_);
      turn_ = 0;
    }
    else {

      MAP_Pos(pos_n_, ROBOT_RADIUS, -M_PI/2, &x1, &y1);
      MAP_Pos(pos_n_, GRID_WIDTH + ROBOT_RADIUS, -M_PI/2, &x2, &y2);

      /* Boundary condition reached - robot at end of grid map
	 Robot must maintain trajectory to avoid crossing the boundary */
      if((map_[y1 * GRID_DIMENSIONS + x1] == 5) ||
             (map_[y2 * GRID_DIMENSIONS + x2] == 5))
	VWSetSpeed(vw_, 0.15, 0);

      /* Infra-red sensor to detect collision with the obstacle */
      else if(IRRead(ir_r_) == 0)
        VWSetSpeed(vw_, 0, 0.2);

      /* Obstacle not detected beside robot (outward corner) - 
	 rotate accordingly to locate obstacle */
      else if((PSDGet(psd_[2]) - OFFSET_RIGHT) > 95)  
      {

	/* Store the configuration space map */
        if((prev_ <= 95) && (turn_ == 0) && (*count_ < GRID_DIMENSIONS*10))  { 
          MAP_Position(vw_, &pos_n_, &map_[0]);
          segs_[*count_].x2 = MAP_Segment(pos_n_.x) +
		(ROBOT_RADIUS + prev_) * cos(pos_n_.phi - M_PI/2);
          segs_[*count_].y2 = MAP_Segment(pos_n_.y) +  
		(ROBOT_RADIUS + prev_) * sin(pos_n_.phi - M_PI/2);
          *count_ += 1;
          segs_[*count_].x1 = MAP_Segment(pos_n_.x) + 
		(ROBOT_RADIUS + prev_) * cos(pos_n_.phi - M_PI/2);
          segs_[*count_].y1 = MAP_Segment(pos_n_.y) +
		(ROBOT_RADIUS + prev_) * sin(pos_n_.phi - M_PI/2);
        }
        VWSetSpeed(vw_, 0.05, -0.55);
      }

      /* Veering too far away from obstacle, adjust trajectory */
      else if((prev_ != -1) && (dist_ < prev_))
        VWSetSpeed(vw_, 0.15, 0.2);

      /* Veering too close to obstacle, adjust trajectory */
      else if((prev_ != -1) && (dist_ > prev_))  
        VWSetSpeed(vw_, 0.15, -0.2);

      /* Robot is moving parallel to obstacle - maintain trajectory */
      else 
	VWSetSpeed(vw_, 0.15, 0);	
    }
    prev_ = dist_;
    MAP_Position(vw_, &pos_n_, &map_[0]);

    /* Set condition to determine the end of the wall following routine */
    if((fabs(start_.x - pos_n_.x) > 0.12) || (fabs(start_.y - pos_n_.y) > 0.12))
      looped_ = 1;
  }
  VEHStop(vw_);

  /* Store final point in the configuration space for the obstacle */
  if(*count_ < 10*GRID_DIMENSIONS) {
    segs_[*count_].x2 = MAP_Segment(start_.x) + 
	(ROBOT_RADIUS + val_) * cos(start_.phi-M_PI/2); 
    segs_[*count_].y2 = MAP_Segment(start_.y) + 
	(ROBOT_RADIUS + val_) * sin(start_.phi-M_PI/2); 
    *count_ += 1;
  }
}

/* ---------------= LCD_MAP =---------------------- */

/** Draws the occupational grid onto the LCD display. The map is centred on
	the robot.

	@param vw_ 	The VW Handle
	@param map_	The address of the grid  
	@author Nicholas Tay, CIIPS 1998
	@see	Display_MAP UpLoad_Grid */
void LCD_MAP(VWHandle vw_, char *map_)  {
  int 		i = 0, j = 0, x, y;
  PositionType 	pos_n_;

  /* Centres the map on the robot position */
  VWGetPosition(vw_, &pos_n_);
  MAP_Pos(pos_n_, 0, 0, &x, &y);

  /* Adjusts map if it encounters the end of the grid boundaries */
  if(x < 64)
    x = 64;
  else if(x > (GRID_DIMENSIONS - 64))
    x = GRID_DIMENSIONS - 64;
  if(y < 32)
    y = 32;
  else if(y > (GRID_DIMENSIONS - 32))
    y = GRID_DIMENSIONS - 32;
  LCDClear();

  /* Draws the map on the LCD Display */
  while((j < 64) && ((y + j - 32) < GRID_DIMENSIONS)) {
    i = 0;
    while((i < 128) && ((x + i - 64) < GRID_DIMENSIONS)) {
      if((x + i - 64) >= 0)
	if((y + j - 32) >= 0) 
	  if(map_[(y + j - 32)*GRID_DIMENSIONS+(x + i - 64)] == 1)
            LCDSetPixel(j, i, 1);
      i++;
    }
    j++;
  }
}

/* -----------= Display_MAP =------------- */

/** Displays the map in ascii format (Used for simulator only).

	@param	map_	The address of the grids   
	@author Nicholas Tay, CIIPS 1998
	@see	LCD_MAP UpLoad_Grid */
void Display_MAP(char *map_)  {
  int i, j;
  
  for(i = 0; i < GRID_DIMENSIONS; i++) {
    for(j = 0; j < GRID_DIMENSIONS; j++)
      switch(map_[i * GRID_DIMENSIONS + j])  {
        case 0:
          fprintf(stderr," ");
          break;
        case 1:
 	  fprintf(stderr,"@");
          break;
        case 3:
          fprintf(stderr,":");
          break;
        case 4:
          fprintf(stderr,"X");
          break;
        case 5:
          fprintf(stderr,"*");
          break;
        case 6:
 	  fprintf(stderr,".");
          break;
        default:
          fprintf(stderr,"-");
          break;
        }
    fprintf(stderr,"\n");
  }
} 

/* ------------ MAP_Target --------------------- */

/** Locates an unexplored cell and returns the index as the target.

	@param  map_	The address of the occupational grids  
	@author Nicholas Tay, CIIPS 1998 */
int MAP_Target(char *map_)  {
  
  int i = 0;
 
  while (i < (GRID_DIMENSIONS*GRID_DIMENSIONS))  {
    if((map_[i] == 6) || (map_[i] == 4))
      return i;
    i++;
  }
  return -1;
}

/* ---------------- UpLoad_VG ------------------ */

/** Loads the map through the SERIAL2 link.  Loads the configuration space
	system.

	@param	values	The configuration space address
	@param	v_count	The counter for the configuration space  
	@see	UpLoad_Grid  */
void UpLoad_VG(Segment *values, int v_count)  {
  char string[50], term = 4;
  int i, j, len;

  string[0] = '\0';
  len = sprintf(string, "width %d\n", GRID_DIMENSIONS * GRID_DIMENSIONS + 100);
  for(j = 0; j < len; j++)
    OSSendRS232(&string[j], SERIAL2);
  string[0] = '\0';
  len = sprintf(string, "height %d\n", GRID_DIMENSIONS * GRID_DIMENSIONS + 100);
  for(j = 0; j < len; j++)
    OSSendRS232(&string[j], SERIAL2);
  for(i = 0; i < v_count; i++)  {
    len = sprintf(string, "%d %d %d %d\n", values[i].x1, values[i].y1,
		values[i].x2, values[i].y2);
    for(j = 0; j < len; j++)
      OSSendRS232(&string[j], SERIAL2);
  }
  OSSendRS232(&term, SERIAL2);
  LCDPutString("Map UPLOAD complete!\n");
}

/* ---------------------- UpLoad_Grid ------------------ */

/** Loads the occupational grids system through the SERIAL2 link.

	@param	info	The address of the grids map representation
	@author Nicholas Tay, CIIPS 1998  
	@see	UpLoad_VG  Display_MAP LCD_MAP */
void UpLoad_Grid(char *info)
{
  char term = 4, obs = 'X', clr = ' ', nl = '\n';
  int i, j;

  for(i = 0; i < GRID_DIMENSIONS; i++)  {
    for(j = 0; j < GRID_DIMENSIONS; j++)
      if(info[i*GRID_DIMENSIONS + j] == 1)
        OSSendRS232(&obs, SERIAL2);
      else
        OSSendRS232(&clr, SERIAL2);
    OSSendRS232(&nl, SERIAL2);
  }
  OSSendRS232(&term, SERIAL2);
}

/* ----------------------------------------------------- */



int main ()
{

  /* -----------= Variable declarations =-------------- */
  VWHandle vw;
  Segment data[GRID_DIMENSIONS*10];
  int count = 0, i;
  char     map[GRID_DIMENSIONS*GRID_DIMENSIONS] = {1};

  PSDHandle psd[3];
  /* BumpHandle bump_l, bump_r;*/
  IRHandle ir_rf, ir_rm, ir_lm;  

  
  /* - - - - - - -= PSD Handler =- - - - - - - - */
  
  for(i=GRID_DIMENSIONS;i<GRID_DIMENSIONS*GRID_DIMENSIONS;i++)
    map[i] = 3;
  for(i=0;i<GRID_DIMENSIONS;i++) {
    map[i] = 5;
    map[GRID_DIMENSIONS*GRID_DIMENSIONS - 1 - i] = 5;
    map[GRID_DIMENSIONS*i] = 5;
    map[GRID_DIMENSIONS*i +GRID_DIMENSIONS-1] = 5;
  }
  LCDMode(SCROLLING|NOCURSOR);

  psd[2] = PSDInit(PSD_RIGHT);
  if(psd[2] == 0)
  {
    LCDPutString("PSDInit Error!\n");
    OSWait(20); 
    exit(1);
  }
  psd[0] = PSDInit(PSD_LEFT);
  if(psd[0] == 0)
  {
    LCDPutString("PSDInit Error!\n");
    OSWait(20); exit(1);
  }
  psd[1] = PSDInit(PSD_FRONT);
  if(psd[1] == 0)
  {
    LCDPutString("PSDInit Error!\n");
    OSWait(20); exit(1);
  }
  PSDStart(psd[0] + psd[1] + psd[2], TRUE);
  /*PSDStart(psd[1], TRUE);
  PSDStart(psd[2], TRUE);*/

  
  /* - - - - - - -= IR Handler =- - - - - - - - - - */

    if ((ir_rf = IRInit(IR_RF)) == 0)  {
    LCDPutString("IR Init Error!\n");
    OSWait(20); exit(1);
    }
    if ((ir_rm = IRInit(IR_RM)) == 0)  {
    LCDPutString("IR Init Error!\n");
    OSWait(20); exit(1);
    }

    if ((ir_lm = IRInit(IR_LM)) == 0)  {
    LCDPutString("IR Init Error!\n");
    OSWait(20); exit(0);
    }

    
  /* - - - - - - - - - - - - - - - - - - - - - */

  vw = VWInit(VW_DRIVE,1); /* init v-omega interface */
  if(vw == 0)
    {
      LCDPutString("VWInit Error!\n");
      OSWait(200); exit(1);
    }

  VWSetPosition(vw, 0.0, 0.0, 0.0);
  VWStartControl(vw,4,0.3,4,0.1);


  /* main action loop */ 

  while (TRUE)  {
    LCDPutString("Scanning...\n");
    if(!MAP_Locate(vw, psd[1], ir_rf, &map[0]))  {
      i = MAP_Target(&map[0]);
      if(i != -1)  {
        MAP_Navigation(vw, &psd[0], ir_rf, ir_rm, ir_lm, i, &map[0]);
      } else {
        LCDPrintf("\nMap Generation Finished\n");
        LCDMenu("MAP","GRID","LINE","STOP");
        do {
          i = KEYRead();
          switch (i)  {
	    case KEY1: break;
	    case KEY2: 
	      UpLoad_Grid(&map[0]);
 	      LCDPutString("Loading complete\n");
	      break;
	    case KEY3:
	      UpLoad_VG(&data[0], count);
 	      LCDPutString("Loading complete\n");
  	      break;
	    case KEY4:
	      /* Release Handles */
              IRRelease(ir_rm + ir_lm + ir_rf);
              PSDRelease();
              VWRelease(vw); 
	      exit(0);
	    default:
	      break;
	  }
        } while (i != KEY1);
        LCDMenu("","","","");
        count = 0;
     	for(i=GRID_DIMENSIONS;i<GRID_DIMENSIONS*GRID_DIMENSIONS;i++)
    	  map[i] = 3;
  	for(i=0;i<GRID_DIMENSIONS;i++) {
    	  map[i] = 5;
    	  map[GRID_DIMENSIONS*GRID_DIMENSIONS - 1 - i] = 5;
    	  map[GRID_DIMENSIONS*i] = 5;
    	  map[GRID_DIMENSIONS*i +GRID_DIMENSIONS-1] = 5;
  	}
      }
    } else {
      LCDPutString("Wall-following\n");
      Wall_Follow_Right(vw, &psd[0], ir_rf, ir_rm, &map[0], &data[0], &count);
      LCD_MAP(vw, &map[0]);
      AUBeep();
      KEYWait(ANYKEY);
    }

  } 
  
}