/* Thomas Braunl/Birgit Graf, UWA, 1998 */
/* Thomas Braunl, UWA, 2000             */

#include "eyebot.h"

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

#ifndef RIGHT
#define RIGHT 1
#endif

#ifndef LEFT
#define LEFT -1
#endif

#define MIDDLE 0


#define THRES	    240		/* open way to next field? */
/* #define THRES_WALL 95 */
#define THRES_WALL  120		/* stop in front of wall */
#define PSDSIDE	    100		/* PSD-value on side if robot in middle of corridor [mm] */
#define MAZESIZE    8

/* Define the places output can go */
#define	LCD	    0
#define CONSOLE	1

/* pos.0,0 is bottom left, dir. 0 is facing up (north) */
int mark[MAZESIZE][MAZESIZE];     /* 1 if visited */
int wall[MAZESIZE+1][MAZESIZE+1][2];  /* 1 if wall, 0 if free, -1 if unknown */
/*  BOTTOM: wall[x][y][0]  LEFT: wall[x][y][1] */
int map [MAZESIZE][MAZESIZE];     /* distance to goal */
int nmap[MAZESIZE][MAZESIZE];     /* copy */
int path[MAZESIZE*MAZESIZE];      /* shortest path */

/** handles for PSD sensors */
PSDHandle psd_front,psd_left,psd_right;

/** handle for motors */
VWHandle  vw;
int       GRAPH,DEBUG,DEBUG2;

/** robo position and orientation.
   dir = 0, 1, 2, 3 equals: north, west, south, east.
*/
int rob_x, rob_y, rob_dir;

/** flag, set to end driving */
int end_drive=FALSE;

meterPerSec lin_speed=0.2;	/* linear speed [m/s] */
radPerSec rot_speed=1.0;	/* rotational speed [rad/s] */
float dist;			/* length of one field (depends on lin. speed) */

/** where output should go */
int output;

/** play tune.
    Play tune when reached target
*/
void tune()
{
  AUTone(1046.5, 200);
  while(!AUCheckTone()){}
  AUTone(1396.9, 200);
  while(!AUCheckTone()){}
  AUTone(1046.5, 200);
  while(!AUCheckTone()){}
  AUTone(698.5, 200);
  while(!AUCheckTone()){}
}  


void set_speed()
{
  float boc[2];
  int ind = 0;
  int end_proc=FALSE;

  LCDClear();
  LCDMenu(" + "," - ","Nxt","OK");
  LCDPrintf("Set speed:\n");

  boc[0]=lin_speed;
  boc[1]=rot_speed;
  
  while (!end_proc)
  {
    LCDSetPos(2,0);
    LCDPrintf("lin. spd.: %1.2f\n",lin_speed);
    LCDPrintf("rot. spd.: %1.2f\n",rot_speed);
      
    LCDSetChar(2+ind,15,'*');
    
    switch (KEYGet())
    {
    case KEY1:
      boc[ind]+=0.02;
      break;
      
    case KEY2:
        boc[ind]-=0.02;
      break;

    case KEY3:
      LCDSetChar(2+ind,15,' ');
      ind++;
      if (ind>1) ind=0;
      break;
      
    case KEY4:
      LCDClear(); end_proc=TRUE; break; 
      
    default: break;
    }
    
    lin_speed=boc[0];
    rot_speed=boc[1];
  }
}


/** print mark on LCD (6 lines).
    print positions that robot has already visited.
*/
void print_mark()
{ int i,j;

  if( output == LCD )
  {
    LCDSetPos(1,0);
    for (i=5; i>=0; i--)
    { for (j=0; j<14; j++)
	if (mark[i][j]) LCDPutChar('x'); else LCDPutChar('.');
      if (i>0) LCDPutChar('\n');
    }
  }
  else
  {
    fprintf(stderr,"MARK\n");
    for (i=MAZESIZE-1; i>=0; i--)
    { for (j=0; j<MAZESIZE; j++)
	if (mark[i][j]) fprintf(stderr,"x "); else fprintf(stderr,". ");
      fprintf(stderr,"\n");
    }
    fprintf(stderr,"\n");
  }
}
 

/** print full maze
    print maze as explored by robot.
*/
void print_maze()
{ int i,j;

  if( output == LCD )
  {
    LCDSetPos(1,0);
    for (i=5; i>=0; i--)
    { for (j=0; j<=6; j++)
      { if      (wall[i][j][1]==1) LCDPutChar('|');  /* left   */
	else if (wall[i][j][1]==0) LCDPutChar(' '); else LCDPutChar('.');
	if      (wall[i][j][0]==1) LCDPutChar('_');  /* bottom */
	else if (wall[i][j][0]==0) LCDPutChar(' '); else LCDPutChar('.');
      }
      if (i>0) LCDPutChar('\n');
    }
  }
  else
  {
    fprintf(stderr,"MAZE\n");
    for (i=MAZESIZE; i>=0; i--)
    { for (j=0; j<=MAZESIZE; j++)
      { if      (wall[i][j][1]==1) fprintf(stderr,"|");  /* left */
	else if (wall[i][j][1]==0) fprintf(stderr," "); else fprintf(stderr,".");
	if      (wall[i][j][0]==1) fprintf(stderr,"_"); /* top */
	else if (wall[i][j][0]==0) fprintf(stderr," "); else fprintf(stderr,".");
      }
      fprintf(stderr,"\n");
    }
    fprintf(stderr,"\n");
  }
}


void wall_set(int *w, int v)
{ if (*w == -1) *w = v;  /* not seen before, set value */
   else if (*w != v)     /* seen before and CONTRADITION */
        { *w = 1;        /* assume wall to be safe */
          LCDSetString(0,0,"CONTRADICTION\n\a");
        }
}


/** maze_entry.
    enter recognized walls or doors.
*/
void maze_entry(int x, int y, int dir, int open)
{ switch(dir)  /* record bottom or left wall per square */
  { case 0: wall_set(&wall[y+1][x  ][0], !open); break;  /* top = bottom of next */
    case 2: wall_set(&wall[y  ][x  ][0], !open); break;
    case 1: wall_set(&wall[y  ][x  ][1], !open); break;
    case 3: wall_set(&wall[y  ][x+1][1], !open); break;  /* right = left of next */
  }
}

/** init_maze.
    inits internal map of maze.
    set marks to 0 and walls to -1 (unknown).
*/
void init_maze()
{ int i,j;
  for (i=0; i<MAZESIZE; i++) for (j=0; j<MAZESIZE; j++)
    mark[i][j] = 0;
  for (i=0; i<MAZESIZE+1; i++) for (j=0; j<MAZESIZE+1; j++)
  { wall[i][j][0] = -1; wall[i][j][1] = -1; }
}


int xneighbor(int x, int dir)
{ switch (dir)
  { case 0: return x;   /* north */
    case 1: return x-1; /* west  */
    case 2: return x;   /* south */
    case 3: default: return x+1; /* east  */
  }
}

int yneighbor(int y, int dir)
{ switch (dir)
  { case 0: return y+1; /* north */
    case 1: return y;   /* west  */
    case 2: return y-1; /* south */
    case 3: default: return y;   /* east  */
  }
}


int unmarked(int y, int x, int dir)
{ dir = (dir+4) % 4;
  return !mark [yneighbor(y,dir)] [xneighbor(x,dir)];
}


/** sign.
    Return -1 for input<0, 1 for input>0 and 0 for input=0.
*/

int sign(float number)
{
  if (number!=0.0)
    return (int) number/fabs(number);
  else
    return 0;
}


/** Read PSD.
    Read PSD value for specified sensor 3 times and return median
*/
int ReadPSD(int side)
{
  int i,val[3];
    
  switch (side)
  {
  case RIGHT:
    for (i=0;i<3;i++)
      val[i]=PSDGet(psd_right);
    break;
  case LEFT:
    for (i=0;i<3;i++)
      val[i]=PSDGet(psd_left);
    break;
  case MIDDLE:
    for (i=0;i<3;i++)
      val[i]=PSDGet(psd_front);
    break;
  }
  
  if (val[0]<val[1])
    if (val[1]<val[2])
      return val[1];
    else
      if (val[0]<val[2])
	return val[2];
      else
	return val[0];
  else
    if (val[1]<val[2])
      if (val[0]<val[2])
	return val[2];
      else
	return val[0];
    else
      return val[1];
}


/** go_to.
    walk one square in current direction - drive curve if robot isn't in the middle
    of the field or heading isn't straight.
*/
void go_to(int dir)
{
  int turn;
  int old_l,old_r,act_l,act_r;
  float old_pos,pos_diff;
  PositionType pos;

  int epsilon=50;
  float c=0.0015;
  
  dir = (dir+4) % 4;  /* keep goal dir in 0..3 */
  turn = dir - rob_dir;
  if (turn == 3) turn = -1;  /* turn shorter angle */
    else if (turn == -3) turn =  1;
  if (turn && !end_drive)
  {
    VWDriveTurn(vw, turn*M_PI/2.0-sign(turn)*rot_speed/10, rot_speed);  /* turn */
    while (!VWDriveDone(vw)&&!end_drive)
      end_drive=(KEYRead()==KEY3);
    VWSetSpeed(vw,0,0);
  }

  if(!end_drive)
  {
    VWGetPosition(vw,&pos);
    if (dir%2)			/* heading west or east */
      old_pos=pos.y;
    else			/* heading north or south */
      old_pos=pos.x;

    act_r=ReadPSD(RIGHT);
    act_l=ReadPSD(LEFT);

    do
    {
      old_r=act_r;
      old_l=act_l;

      act_r=ReadPSD(RIGHT);
      act_l=ReadPSD(LEFT);

      if (abs(act_r+act_l-old_r-old_l)<epsilon)
      {
	if(act_l<2*PSDSIDE&&act_r<2*PSDSIDE) /* walls on both sides */
	  VWSetSpeed(vw,lin_speed,c*(act_l-act_r));
	else
	  if (act_l<2*PSDSIDE&&act_r>2*PSDSIDE)	/* wall on left side */
	    VWSetSpeed(vw,lin_speed,2*c*(act_l-PSDSIDE));
	  else
	    if (act_l>2*PSDSIDE&&act_r<2*PSDSIDE) /* wall on right side */
	    VWSetSpeed(vw,lin_speed,2*c*(PSDSIDE-act_r));
	    else		/* no walls */
	      VWSetSpeed (vw,lin_speed,0);
      }
      end_drive=(KEYRead()==KEY3);
      
      VWGetPosition(vw,&pos);
      if (dir%2)		/* heading west or east */
	pos_diff=fabs(old_pos-pos.y);
      else			/* heading north or south */
	pos_diff=fabs(old_pos-pos.x);
      
    } while ((pos_diff<dist)&&!end_drive&&!(ReadPSD(MIDDLE)<THRES_WALL));

    rob_dir = dir;
    rob_x   = xneighbor(rob_x,rob_dir);
    rob_y   = yneighbor(rob_y,rob_dir);
    LCDSetPos(0,0);
    LCDPrintf("Pos[%2d,%2d,%1d]", rob_x,rob_y,rob_dir);
    LCDSetPos(1,0);
    if (DEBUG)
      VWSetSpeed(vw,0,0);
  }
}



/** check_mark.
    if ALL walls of a square are known, mark square as visited.
    this avoids unnecessary exploration.
*/
void check_mark()
{ int i,j;
  for (i=1; i<MAZESIZE; i++) for (j=0; j<MAZESIZE-1; j++)
  /* careful: watch boundaries!! i from 1 / j until size-1 */
  { if (wall[i  ][j][0] != -1 && wall[i][j  ][1] != -1 &&  /* bottom / left  */
        wall[i+1][j][0] != -1 && wall[i][j+1][1] != -1)    /* top    / right */
      mark[i][j] = 1;
  }
}



/**  explore.
    search maze goal from given start position and orientation.
    if more than one possible way: search all recursively.
    mark all visited maze squares.
*/
void explore()
{
  int front_open, left_open, right_open, old_dir,key;

  if(!end_drive) 
  {
    end_drive=(KEYRead()==KEY3);
    mark[rob_y][rob_x] = 1;   /* mark current square */
    front_open = ReadPSD(MIDDLE) > THRES;
    left_open  = ReadPSD(LEFT) > THRES; 
    right_open = ReadPSD(RIGHT) > THRES;
    maze_entry(rob_x,rob_y,rob_dir,       front_open);
    maze_entry(rob_x,rob_y,(rob_dir+1)%4, left_open);
    maze_entry(rob_x,rob_y,(rob_dir+3)%4, right_open);
    check_mark();
    old_dir = rob_dir;
    if (GRAPH)
    { LCDSetPos(0,0);
      LCDPrintf("Pos[%2d,%2d,%1d]", rob_x,rob_y,rob_dir);
      if (left_open)  LCDSetChar(0,13,'<');
                 else LCDSetChar(0,13,'|');
      if (front_open) LCDSetChar(0,14,'^');
                 else LCDSetChar(0,14,'-');
      if (right_open) LCDSetChar(0,15,'>');
                 else LCDSetChar(0,15,'|');
      print_maze();
    }
 
    if (DEBUG)
    { LCDMenu("Next"," ","STOP"," ");
      key=KEYGet();
      end_drive=(key==KEY3);
    }
    
    if (!end_drive)
    { LCDMenu(" "," ","STOP"," ");
      if (front_open  && unmarked(rob_y,rob_x,old_dir)) /* then go straight */ 
      { go_to(old_dir);   /* go 1 forward, 0 if first choice */
        explore();        /* recursive call */
        go_to(old_dir+2); /* go 1 back */
      }
      
      if (left_open && unmarked(rob_y,rob_x,old_dir+1))  /* then turn left */
      { go_to(old_dir+1); /* go 1 left */
        explore();        /* recursive call */
        go_to(old_dir-1); /* go 1 right, -1 = +3 */
      }
      
      if (right_open && unmarked(rob_y,rob_x,old_dir-1)) /* then turn right */
      { go_to(old_dir-1); /* go 1 right, -1 = +3 */
        explore();        /* recursive call */
        go_to(old_dir+1); /* go 1 left */
      }
    }
  }
  else
    VWSetSpeed(vw,0,0);
}


/** print shortest distances from start on LCD (6 lines).
*/
void print_map()
{ int i,j;
  if( output == LCD )
  {
    LCDClear();
    LCDPutString("Map distances\n");
    for (i=5; i>=0; i--)
    { for (j=0; j<4; j++)
	LCDPrintf("%3d",map[i][j]);
      if (i>0) LCDPutChar('\n');
    }
  }
  else
  {
    fprintf(stderr,"MAP\n");
    for (i=MAZESIZE-1; i>=0; i--)
    { for (j=0; j<MAZESIZE; j++)
	fprintf(stderr,"%3d",map[i][j]);
      fprintf(stderr,"\n");
    }
    fprintf(stderr,"\n");
  }
}


/** shortest_path.
    analyze shortest path after maze has been searched.
    returns path length to goal.
    or -1 if no path could be found.
*/
int shortest_path(int goal_y, int goal_x)
{ int i,j,iter;

  LCDSetString(0,0,"                "); /* clear top line */
  LCDSetPos(0,0); LCDPutString("Map..");
  for (i=0; i<MAZESIZE; i++) for (j=0; j<MAZESIZE; j++)
  {  map [i][j] = -1;  /* init */
     nmap[i][j] = -1;
  }
  map [0][0] = 0;
  nmap[0][0] = 0;
  iter=0;

  do
  { iter++;
    for (i=0; i<MAZESIZE; i++) for (j=0; j<MAZESIZE; j++)
    { if (map[i][j] == -1)
      { if (i>0)
          if (!wall[i][j][0]   && map[i-1][j] != -1)
            nmap[i][j] = map[i-1][j] + 1;
        if (i<MAZESIZE-1)
          if (!wall[i+1][j][0] && map[i+1][j] != -1)
            nmap[i][j] = map[i+1][j] + 1;
        if (j>0)
          if (!wall[i][j][1]   && map[i][j-1] != -1)
            nmap[i][j] = map[i][j-1] + 1;
        if (j<MAZESIZE-1)
          if (!wall[i][j+1][1] && map[i][j+1] != -1)
            nmap[i][j] = map[i][j+1] + 1;
      }
    }

    for (i=0; i<MAZESIZE; i++) for (j=0; j<MAZESIZE; j++)
      map[i][j] = nmap[i][j];  /* copy back */

    if (DEBUG2)
    { print_map();
      LCDMenu("Next"," "," "," ");
      KEYWait(KEY1);
      LCDMenu(" "," "," "," ");
    }
  } while (map[goal_y][goal_x] == -1  && iter < (MAZESIZE*MAZESIZE) );
  return map[goal_y][goal_x];
}


/** build path.
  build shortest path after finding it.
  uses map and wall.
  sets path.
*/
void build_path(int i, int j, int len)
{ int k;

  LCDSetString(0,0,"                "); /* clear top line */
  LCDSetString(0,0,"Path..");
  if (i<=5 && j<=6) LCDSetChar(6-i,2*j+1,'G'); /* mark goal */
  for (k = len-1; k>=0; k--)
  {
    if (i>0 && !wall[i][j][0] && map[i-1][j] == k)
    { i--;
      path[k] = 0; /* north */
    }
   else
    if (i<MAZESIZE-1  && !wall[i+1][j][0] && map[i+1][j] == k)
    { i++;
      path[k] = 2; /* south */
    }
   else
    if (j>0  && !wall[i][j][1] && map[i][j-1] == k)
    { j--;
      path[k] = 3; /* east */
    }
  else
    if (j<MAZESIZE-1  && !wall[i][j+1][1] && map[i][j+1] == k)
    { j++;
      path[k] = 1; /* west */
    }
  else
    { LCDPutString("ERROR\a");
      KEYWait(ANYKEY);
    }

   /* mark path in maze on LCD */
   if (i<=5 && j<=6) { if (k>0) LCDSetChar(6-i,2*j+1,'*'); /* path  */
                           else LCDSetChar(6-i,2*j+1,'S'); /* start */
                     }
   if (DEBUG2) fprintf(stderr,"path %3d:%1d\n", k, path[k]);
  }
  LCDSetString(0,6,"done");
}


/** drive path.
  drive path after building it.
  parametere specifies start to finish (0).
  or finish to start (1).
*/
void drive_path(int len, int reverse)
{ int i;
  if (reverse)
  { for (i=len-1; i>=0; i--) go_to(path[i]+2);

    if (rob_dir != 0) /* back in start field */
    { VWDriveTurn(vw, -rob_dir*M_PI/2.0, rot_speed);  /* turn */
      VWDriveWait(vw);
      rob_dir = 0;
    }
  }

  else
    for (i=0; i<len; i++)    go_to(path[i]);
}


/** main program.
    search maze from start in (0,0).
    search whole maze and generate map.
    @AUTHOR    Thomas Braunl/Birgit Graf, UWA, 1998.
*/
int main ()
{ int key,path_len,trash,i;
  int goalY, goalX, incr;

  /* ask where to send output to */
  LCDPrintf( "Format output\nfor?" );
  LCDMenu( "LCD", "", "", "SIM" );
  do { key = KEYGet(); }
  while( key != KEY1 && key != KEY4 );
  if( key == KEY1 ) output = LCD;
  else output = CONSOLE;
  
  LCDClear();
  LCDPrintf("MAZE\nfull search\n\n");
  init_maze();
  
  /* init PSD sensors and motors */
  psd_front = PSDInit(PSD_FRONT);
  psd_left  = PSDInit(PSD_LEFT);
  psd_right = PSDInit(PSD_RIGHT);
  PSDStart(psd_front|psd_left|psd_right,TRUE);
  for (i=0;i<5;i++)		/* get bad initial PSD-values and throw them away */
  {
    trash=ReadPSD(RIGHT);
    trash=ReadPSD(LEFT);
    trash=ReadPSD(MIDDLE);
  }
  vw = VWInit(VW_DRIVE,1);
  VWStartControl(vw,7,0.3,7,0.1);

  set_speed();
  dist=0.375-lin_speed/10;		/* distance from one field to next [m] */

  LCDPrintf("Put robot to\n(0,0,north)\n");
  end_drive=FALSE;
 
  LCDMenu("GO","Fst","Dbg","Db2");
  key = KEYGet();
  GRAPH  =  key != KEY2;
  DEBUG  =  key == KEY3;
  DEBUG2 = (key == KEY3) || (key == KEY4);
  
  LCDMenu(" "," ","STOP"," ");

  rob_x = 0; rob_y=0; rob_dir = 0;  /* start in pos. 0,0, facing north */
  explore();

  /* back in [0,0] turn robot to original direction */
  if (rob_dir != 0)
  { VWDriveTurn(vw, -rob_dir*M_PI/2.0, rot_speed);  /* turn */
    VWDriveWait(vw);
    rob_dir = 0;
  }
 
  VWSetSpeed(vw,0,0);
  end_drive=FALSE;


  do
  { print_maze();
    LCDMenu("Y+-","X+-","+/-","GO");
    incr = 1;
    goalX=0; goalY=0;
    LCDMode(SCROLLING|CURSOR);
    do
    { LCDSetPos(0,0);
      LCDPrintf("GOAL y,x: %2d %2d\a", goalY, goalX);
      if (goalY<=5 && goalX <=6) LCDSetPos(6-goalY,2*goalX+1);
      switch (key = KEYGet())
      { case KEY1: goalY = (goalY+incr+MAZESIZE) % MAZESIZE; break;
        case KEY2: goalX = (goalX+incr+MAZESIZE) % MAZESIZE; break;
        case KEY3: incr = -incr; break;
      }
    } while (key != KEY4);
    LCDMode(SCROLLING|NOCURSOR);
    LCDMenu(" "," "," "," ");

    path_len = shortest_path(goalY,goalX);  /* from start 0,0 to goal */
    if (path_len != -1) /* if path exists */
    {
      LCDSetPos(0,0); LCDPrintf("Path length: %3d", path_len);
      build_path(goalY,goalX,path_len);

      do  /* optional data display */
      { LCDMenu("Map","Mrk","Maz","DRV");
        switch (key = KEYGet())
        { case KEY1: print_map() ; break;
          case KEY2: print_mark(); break;
          case KEY3: print_maze(); break;
        }
      } while (key != KEY4);
 
      if(!end_drive)
      { LCDMenu(" "," ","STOP"," ");
        drive_path(path_len,0); /* drive start to finish */
        if(!end_drive)
        { VWSetSpeed(vw,0,0);
          LCDPrintf("reached target\n\n");
          tune();
          drive_path(path_len,1); /* drive finish to start */
          if(!end_drive)
          { VWSetSpeed(vw,0,0);
            LCDPrintf("reached start\n");
            tune(); tune();      
          }
        }
      }
    }
    else { LCDSetPos(0,0); LCDPrintf("No path exists! \a"); }  

    end_drive=FALSE;
    LCDMenu("REP"," "," ","END");
  } while (KEYGet()!=KEY4);

  VWRelease(vw);
  PSDRelease();
  return 0;
}