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

#include "eyebot.h"

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


#ifndef PI
#define PI 3.1415926535
#endif

#define DIST    0.36
#define SPEED   DIST
#define ASPEED   PI/2.0
#define THRES   175


PSDHandle psd_front,psd_left,psd_right;
VWHandle  vw;
int       DEBUG;
int       goal_y, goal_x;


/** goal_reached.
  returns 1 if goal position is reached, 0 otherwise.
*/
int goal_reached(int y, int x)
{ return (y==goal_y && x==goal_x);
}


/** turn.
  turn +90, -90, or +180 degrees.
*/
void turn(int change, int *dir)
{  VWDriveTurn(vw, change*PI/2.0, ASPEED);
   VWDriveWait(vw);
   *dir = (*dir+change +4) % 4;
}


/** go_one.
  walk one square in current direction
*/
void go_one(int *x, int *y, int dir)
{ switch (dir)
  { case 0: (*y)++; break; 
    case 1: (*x)--; break; 
    case 2: (*y)--; break; 
    case 3: (*x)++; break; 
  }
  VWDriveStraight(vw, DIST, SPEED);
  VWDriveWait(vw);
}




/**  explore_left.
    find maze goal position from start in (0,0).
    by always following the left wall.
    dir = 0, 1, 2, 3 equals: north, west, south, east.
*/
void explore_left(int x, int y, int dir)
{ int front_open, left_open, right_open;
  PositionType pos;

  while (!goal_reached(y,x))
  { printf("x=%2d y=%2d dir=%1d\n", x,y,dir);
    front_open = PSDGet(psd_front) > THRES;
    left_open  = PSDGet(psd_left)  > THRES; 
    right_open = PSDGet(psd_right) > THRES;
    
    if (DEBUG)
    { VWGetPosition(vw, &pos);
      printf("%5.2f %5.2f %3d\n", pos.x,pos.y, (int) (pos.phi*180.0/PI));
      printf("L=%1d F=%1d R=%1d\n", left_open,front_open, right_open);
      LCDMenu("Next"," "," "," ");
      KEYWait(KEY1);
      LCDMenu(" "," "," "," ");
    }

    if (left_open)         turn(+1, &dir);   /* turn left */
     else if (front_open)  ;                 /* OK, don't turn */ 
      else if (right_open) turn(-1, &dir);   /* turn right */
       else                turn(+2, &dir);   /* dead end - back up */
    go_one(&x,&y,dir);     /* go one step in any case */
  }
}



/** main program.
    find maze goal position from start in (0,0).
    by always following the left wall.
    @AUTHOR    Thomas Braunl, UWA, 1998.
*/
int main ()
{ int start_ori=0, key;
  char dir_names[4][6] = {"north","west","south","east"};

  /* disable input/output buffer */
  setvbuf (stdout,NULL,_IONBF,0);
  setvbuf (stdin,NULL,_IONBF,0);
  LCDMode(SCROLLING|NOCURSOR);

  /* 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);
  vw = VWInit(VW_DRIVE,1);
  VWStartControl(vw,7,0.3,7,0.1);

  printf("MAZE\nfollow left wall\n\n");
  printf("enter goal:\n");
  LCDMenu("Y+","X+"," ","CON");
  goal_y = 0; goal_x = 0;
  do
  { printf("Y %3d  X %3d\n", goal_y,goal_x);
    key = KEYGet();
    if (key == KEY1) goal_y = (goal_y +1) % 16;
    if (key == KEY2) goal_x = (goal_x +1) % 16;
  } while (key != KEY4);

  printf("enter start\norientation\n");
  LCDMenu("OR+"," "," ","CON");
  do
  { printf("ORI %s\n", dir_names[start_ori]);
    key = KEYGet();
    if (key == KEY1) start_ori = (start_ori +1) % 4;
  } while (key != KEY4);


  LCDMenu("GO","DEB"," "," ");
  DEBUG = KEYGet() == KEY2;

  explore_left(0,0,start_ori);  /* start in 0,0 */

  LCDMenu(" "," "," ","END");
  KEYWait(KEY4);
  VWRelease(vw);
  return 0;
}