#include <stdio.h>
#include <stdlib.h>
#include "Nclient.h"

#define ROTATION_CONSTANT    0.118597  /* inches/degree (known to 100 ppm) */

#define RIGHT(trans, steer)  (trans + (int)((float)steer*ROTATION_CONSTANT))
#define LEFT(trans, steer)   (trans - (int)((float)steer*ROTATION_CONSTANT))

#define scout_vm(trans, steer)   vm(RIGHT(trans, steer), LEFT(trans, steer), 0)
#define scout_pr(trans, steer)   pr(RIGHT(trans, steer), LEFT(trans, steer), 0)
#define OPEN /*24*/24
#define CLOSE /*18*/15
#define open_amount 36
#define close_amount 36

struct PathType {
  int next; /* next quadrant in maze*/
  int turn; /* degrees to turn before moving */
  char stop; /* stopping condition r=opening on right, l=opening on left */
             /* f=wall in front, a=walls in front and both sides, e=error */
  int tn;   /* degrees to turn in order to leave robot facing North */
};

struct PathType p[13][13];

/*-----------------Initialize Paths-------------------------------------*/
/* Initialize the paths in the array for the maze. */
/* The maze consists of 12 quadrents, numbered 1-12.*/
/* Sets paths from any of the quadrents to any other quadrent*/
/* Any element in the array is p[from][to] where from is the quadrent that the */
/* robot is currently at and to is the destination quadrent*/

void init_paths(void)
{
  int i,j;

  /* no zero quadrents, want to begin with 1 not 0 */
  for (i=0; i<13; i++)
    {
      p[i][0].next=0;
      p[i][0].turn=0;
      p[i][0].stop='e'; 
      p[i][0].tn=0;
    }
  for (i=0; i<13; i++)
    {
      p[0][i].next=0;
      p[0][i].turn=0;
      p[0][i].stop='e'; 
      p[0][i].tn=0;
    }

  /* set all paths from 1 */
  for (i=2; i<13; i++)
    {
     p[1][i].next=2;     
     p[1][i].turn=90;
     p[1][i].stop='r'; 
     p[1][i].tn=-90;
    }

  /* set all paths from 2 */
  p[2][1].next=1;
  p[2][1].turn=-90;
  p[2][1].stop='r'; 
  p[2][1].tn=90;
  p[2][3].next=3;
  p[2][3].turn=90;
  p[2][3].stop='a'; 
  p[2][3].tn=-90;
  for (i=4; i<=12; i++)
    {
      p[2][i].next=5;
      p[2][i].turn=0;
      p[2][i].stop='r'; 
      p[2][i].tn=0;
    }

  /* set all paths from 3 */
  for (i=1; i<13; i++)
    {
      p[3][i].next=2;
      p[3][i].turn=-90;
      p[3][i].stop='l'; 
      p[3][i].tn=90;
    }

  /* set all paths from 4 */
  for (i=1; i<13; i++)
    {
      p[4][i].next=5;
      p[4][i].turn=90;
      p[4][i].stop='l'; 
      p[4][i].tn=-90;
    }

  /* set all paths from 5 */
  for (i=1; i<=3; i++)
    {
     p[5][i].next=2;
     p[5][i].turn=180;
     p[5][i].stop='f'; 
     p[5][i].tn=-180;
    }
  p[5][4].next=4;
  p[5][4].turn=-90;   
  p[5][4].stop='a';      
  p[5][4].tn=90;
  for (i=6; i<=12; i++)
    {
     p[5][i].next=6;
     p[5][i].turn=90;
     p[5][i].stop='f'; 
     p[5][i].tn=-90;
   }

  /* set all paths from 6 */
  for (i=1; i<=5; i++)
    {
     p[6][i].next=5;
     p[6][i].turn=-90;
     p[6][i].stop='r'; 
     p[6][i].tn=90;
    }
  for (i=7; i<=12; i++)
    {
     p[6][i].next=9;
     p[6][i].turn=0;
     p[6][i].stop='f'; 
     p[6][i].tn=0;
    }

  /* set all paths from 7 */
  for (i=1; i<=12; i++)
    {
      p[7][i].next=8;
      p[7][i].turn=90;
      p[7][i].stop='r'; 
      p[7][i].tn=-90;
    }

  /* set all paths from 8 */
  for (i=1; i<=6; i++)
    {
      p[8][i].next=9;
      p[8][i].turn=90;
      p[8][i].stop='f'; 
      p[8][i].tn=-90;
    }
  p[8][7].next=7;
  p[8][7].turn=-90;
  p[8][7].stop='f'; 
  p[8][7].tn=90;
  p[8][9].next=9;
  p[8][9].turn=90;
  p[8][9].stop='f';     
  p[8][9].tn=-90;
  for (i=10; i<=12; i++)
     {
       p[8][i].next=11;
       p[8][i].turn=0;
       p[8][i].stop='r'; 
       p[8][i].tn=0;
     }

  /* set paths from 9 */
  for (i=1; i<=6; i++)
     {
       p[9][i].next=6;
       p[9][i].turn=180;
       p[9][i].stop='f'; 
       p[9][i].tn=-180;
     }
  for (i=7; i<=12; i++)
     {
       p[9][i].next=8;
       p[9][i].turn=-90;
       p[9][i].stop='l'; 
       p[9][i].tn=90;
     }
  
  /* set paths from 10 */
  for (i=1; i<=12; i++)
     {
       p[10][i].next=11;
       p[10][i].turn=90;
       p[10][i].stop='l'; 
       p[10][i].tn=-90;
     }

  /* set paths from 11 */
  for (i=1; i<=9; i++)
     {
       p[11][i].next=8;
       p[11][i].turn=180;
       p[11][i].stop='l'; 
       p[11][i].tn=-180;
     }
  p[11][10].next=10;
  p[11][10].turn=-90;
  p[11][10].stop='f'; 
  p[11][10].tn=90;
  p[11][12].next=12;
  p[11][12].turn=90;
  p[11][12].stop='f'; 
  p[11][12].tn=-90;


  /* set paths from 12 */
  for (i=1; i<=11; i++)
     {
       p[12][i].next=11;
       p[12][i].turn=-90;
       p[12][i].stop='r'; 
       p[12][i].tn=90;
     }

  /* set all paths where from and to are the same */
  for (i=1; i<13; i++)
     {
       p[i][i].next=0;
       p[i][i].turn=0;
       p[i][i].stop='e'; 
       p[i][i].tn=0;
     }
 
}

/*----------------Turn Sonar On----------------------------------------*/
void turn_sonar_on(void)
{
  int sn_order[16] = {0, 4, 8, 12, 15, 3, 7, 11, 14, 2, 6, 10, 13, 1, 5, 9};
  /* Remainder are irrelevant */
  
  /* turn on sonars and set them to fire in certain order */
  conf_sn(15, sn_order);
  
  conf_tm(5); 
}
/*--------------------Get Shortest Sonar--------------------------------*/
int get_shortest_sonar(long *state, int min_dist)
{/*only return shortest sonar if it is less than min_dist*/
  int i;
  int min_value = 255; /*max sonar reading*/
  int min_sonar = -1;
  
  for (i=0; i<=4; i++) /* check sonars on the left */
    if (State[STATE_SONAR_0+i]<min_value)
      {/* if the sonar reading is less than the current min_value*/
	min_value=State[STATE_SONAR_0+i]; /* new min_value */
	min_sonar=i; /* remember sonar number*/
      }

  for (i=12; i<=15; i++) /* check sonars on the right */
    if (State[STATE_SONAR_0+i]<min_value) 
      {/* if the sonar reading is less than the current min_value*/
	min_value=State[STATE_SONAR_0+i]; /* new min_value */
	min_sonar=i; /* remember sonar number*/
      }
   /* check the from sonar */
  if (State[STATE_SONAR_0+min_sonar]<=min_dist)
    /* if the minimum sonar value is less than the min_dist */
    return(min_sonar);  /* return that sonar number*/
  else
    return(-1); /* no sonar was found with a value less than min_dist */
}
/*-------------------Go Forward Until first opening on right-----------------*/
void to_opening_on_right(long * state)
{ /* move forward until see an opening on the right */
int x, saver, opR=0, prev=1, temp=OPEN+1;

while (temp>=OPEN)
{/* while at an opening */
    get_sn();
    x=get_shortest_sonar(State,12);
    printf("\n SHORTEST SONAR = %d\n", x);
    switch(x)
    { /* obstacle avoid according to shortest sonar */
    case -1:
      scout_vm(75, 0);
      break;
    case 0:
      st();
      if (prev == 1)
	scout_vm(-15, 125);
      else
	scout_vm(-15, -125);
      sleep(3);
      break;
    case 1:
      prev=0;
      scout_vm(15, -188);
      break;
    case 2:
      prev=0;
      scout_vm(20, -125);
      break;
    case 3:
      prev=0;
      scout_vm(25, -63);
      break;
    case 4:
      prev=0;
      scout_vm(30, -31);
      break;
    case 15:
      prev=1;
      scout_vm(15, 188);
      break;
    case 14:
      prev=1;
      scout_vm(20, 125);
      break;
    case 13:
      prev=1;
      scout_vm(25, 63);
      break;
    case 12:
      prev=1;
      scout_vm(30, 31);
      break;
    } /* End of Case Statement */
    get_sn();
    temp=(State[STATE_SONAR_11]+State[STATE_SONAR_12]+State[STATE_SONAR_10])/3;
    /* get the average of the sonars on the right */
}
/* at this point there is not an opening on the right */
while (!opR) /* while there is no opening on the right */
  {
    get_sn();
    x=get_shortest_sonar(State,12);
    printf("\n SHORTEST SONAR = %d\n", x);
    switch(x)
    { /* obstacle avoid according to shortest sonar */
    case -1:
      scout_vm(75, 0);
      break;
    case 0:
      st();
      if (prev == 1)
	scout_vm(-15, 125);
      else
	scout_vm(-15, -125);
      sleep(3);
      break;
    case 1:
      prev=0;
      scout_vm(15, -188);
      break;
    case 2:
      prev=0;
      scout_vm(20, -125);
      break;
    case 3:
      prev=0;
      scout_vm(25, -63);
      break;
    case 4:
      prev=0;
      scout_vm(30, -31);
      break;
    case 15:
      prev=1;
      scout_vm(15, 188);
      break;
    case 14:
      prev=1;
      scout_vm(20, 125);
      break;
    case 13:
      prev=1;
      scout_vm(25, 63);
      break;
    case 12:
      prev=1;
      scout_vm(30, 31);
      break;
    } /* End of Case Statement */
    
      get_sn();
      /* get average of the sonars on the right */
      saver=(State[STATE_SONAR_11]+State[STATE_SONAR_12]+State[STATE_SONAR_10])/3;
    
      if (saver>=OPEN)
      { /* if the average of the sonars on the right is larger than an opening value */
	opR=1; /* found an opening on the right */
	printf("\nFound door on right\n");
        scout_vm(30, 0); /* move forward to the middle of the opening */
        sleep(3);
        st();
      }
   } /* End of While Loop */
	scout_vm(0,0); /* stop moving */
} /* end of to_opening on right */ 
/*-----------------------Find first opening on Left -------------------------*/
void to_opening_on_left(long * state)
{ /* move forward until see an opening on the left */
  int x, savel, opL=0, prev=1, temp=OPEN+1;;

while (temp>=OPEN)
{ /* while there is an opening on the left */
    get_sn();
    x=get_shortest_sonar(State,12);
    printf("\n SHORTEST SONAR = %d\n", x);
    switch(x)
    {/* obstacle avoid according to the shortest sonar reading */
    case -1:
      scout_vm(75, 0);
      break;
    case 0:
      st();
      if (prev == 1)
	scout_vm(-15, 125);
      else
	scout_vm(-15, -125);
      sleep(3);
      break;
    case 1:
      prev=0;
      scout_vm(15, -188);
      break;
    case 2:
      prev=0;
      scout_vm(20, -125);
      break;
    case 3:
      prev=0;
      scout_vm(25, -63);
      break;
    case 4:
      prev=0;
      scout_vm(30, -31);
      break;
    case 15:
      prev=1;
      scout_vm(15, 188);
      break;
    case 14:
      prev=1;
      scout_vm(20, 125);
      break;
    case 13:
      prev=1;
      scout_vm(25, 63);
      break;
    case 12:
      prev=1;
      scout_vm(30, 31);
      break;
    } /* End of Case Statement */
    get_sn();
    /* get the average of the sonars on the left */
    temp=(State[STATE_SONAR_5]+State[STATE_SONAR_4]+State[STATE_SONAR_6])/3;
}

/* at this point there is not an opening on the left */
while (!opL)
{ /* while there is not an opening on the left */ 
    get_sn();
    x=get_shortest_sonar(State,12);
    printf("\n SHORTEST SONAR = %d\n", x);
    switch(x)
    { /* obstacle avoid while there is NOT an opening on the left */
    case -1:
      scout_vm(75, 0);
      break;
    case 0:
      st();
      if (prev == 1)
	scout_vm(-15, 125);
      else
	scout_vm(-15, -125);
      sleep(3);
      break;
    case 1:
      prev=0;
      scout_vm(15, -188);
      break;
    case 2:
      prev=0;
      scout_vm(20, -125);
      break;
    case 3:
      prev=0;
      scout_vm(25, -63);
      break;
    case 4:
      prev=0;
      scout_vm(30, -31);
      break;
    case 15:
      prev=1;
      scout_vm(15, 188);
      break;
    case 14:
      prev=1;
      scout_vm(20, 125);
      break;
    case 13:
      prev=1;
      scout_vm(25, 63);
      break;
    case 12:
      prev=1;
      scout_vm(30, 31);
      break;
    } /* End of Case Statement */
    
      get_sn();
      /* get the average of the sonars on the left */
      savel=(State[STATE_SONAR_6]+State[STATE_SONAR_4]+State[STATE_SONAR_5])/3;
    
      if (savel>=OPEN)
      { /* if the average of the sonars on the left is greater than opening value */
	opL=1; /* found an opening on the left */
	printf("\nFound opening on left\n");
        scout_vm(30, 0); /* move forward to the middle of the opening */
        sleep(3);
        st();
      }
   } /* End of While Loop */
	scout_vm(0,0); /* stop moving */
} /* end of to_opening on left */ 
/*-------------------Go Forward Until Wall in Front--------------------------*/
void to_wall_in_front(long * state)
{ /* move forward until hit a wall in the front */
  int x, savef, wallF=0, prev=1;

while (!wallF)
{ /* while there is NOT a wall in front */
    get_sn();
    x=get_shortest_sonar(State,12);
   
    printf("\n SHORTEST SONAR = %d\n", x);
    switch(x)
    { /* obstacle avoid according to shortest sonar */
    case -1:
      scout_vm(100, 0);
      break;
    case 0:
      st();
      if (prev == 1)
	scout_vm(-15, 125);
      else
	scout_vm(-15, -125);
      sleep(3);
      break; 
    case 1:
      prev=0;
      scout_vm(15, -188);
      break;
    case 2:
      prev=0;
      scout_vm(20, -125);
      break;
    case 3:
      prev=0;
      scout_vm(25, -63);
      break;
    case 4:
      prev=0;
      scout_vm(30, -31);
      break;
    case 15:
      prev=1;
      scout_vm(15, 188);
      break;
    case 14:
      prev=1;
      scout_vm(20, 125);
      break;
    case 13:
      prev=1;
      scout_vm(25, 63);
      break;
    case 12:
      prev=1;
      scout_vm(30, 31);
      break;
    } /* End of Case Statement */
    
      get_sn();
      /* get the average of the 3 front sonars */
      savef=(State[STATE_SONAR_15]+State[STATE_SONAR_0]+State[STATE_SONAR_1])/3;
     
 if (savef<=18)
 { /* if the average of the 3 front sonars is less than 18 */
	wallF=1; /* there is a wall in front */
	printf("\nFound wall in front\n");
      } 
   
    
   } /* End of While Loop */
        scout_vm(0,0); 
} /* end of to_wall_in_front */ 
/*------------------Forward Until Wall in front and left and right ----------*/
void to_wall_all(long * state)
{/* move forward until front, left, and right are all looking at a wall*/
  int x, saveA, savef,savel,saver, wallA=0, prev=1;

while (!wallA)
{ /* while there is not a wall in the front, on the left, and on the right */
    get_sn();
    x=get_shortest_sonar(State,12);
    printf("\n SHORTEST SONAR = %d\n", x);
    switch(x)
    {/* obstacle avoid according to shortest sonar */
    case -1:
      scout_vm(100, 0);
      break;
    case 0:
      st();
      if (prev == 1)
	scout_vm(-15, 125);
      else
	scout_vm(-15, -125);
      sleep(3);
      break; 
    case 1:
      prev=0;
      scout_vm(15, -188);
      break;
    case 2:
      prev=0;
      scout_vm(20, -125);
      break;
    case 3:
      prev=0;
      scout_vm(25, -63);
      break;
    case 4:
      prev=0;
      scout_vm(30, -31);
      break;
    case 15:
      prev=1;
      scout_vm(15, 188);
      break;
    case 14:
      prev=1;
      scout_vm(20, 125);
      break;
    case 13:
      prev=1;
      scout_vm(25, 63);
      break;
    case 12:
      prev=1;
      scout_vm(30, 31);
      break;
    } /* End of Case Statement */
    
      get_sn();
      savef=(State[STATE_SONAR_15]+State[STATE_SONAR_0]+State[STATE_SONAR_1])/3;
      savel=(State[STATE_SONAR_3]+State[STATE_SONAR_4]+State[STATE_SONAR_5])/3;
      saver=(State[STATE_SONAR_11]+State[STATE_SONAR_12]+State[STATE_SONAR_13])/3;
			
      /* get the average of the front, left, and right sonars */
      saveA=(savef+savel+saver)/3;

      if (saveA<=CLOSE)
      {/* if the average of the front, left, and right sonars is less than CLOSE */
	wallA=1;/* there is a wall all around (front, left, and right) */
	printf("\nFound wall in front\n");
      }
   } /* End of While Loop */
	scout_vm(0,0); /* stop moving */
} /* end of wall all  */ 
/*------------------Turn a Given Degrees-------------------------------------*/
void turn_degrees(int turn_amt)
{ /* turn_amt is in 0.1 degrees */
  /* for example: turn_degrees(900) will turn the robot 90 to the left */
  st();      
  scout_vm(0,turn_amt*10); 
  sleep(1);
  st();
  printf("\nTurning\n");
}
/*------------------Initial Turn to Face North-------------------------------*/
void turn_north(int facing)
{/* turn the robot to face north from whatever position it is currently facing */
  switch(facing)
  { 
    case 0: /* N */
      turn_degrees(0);
      break;
    case 1: /* NE */
      turn_degrees(45);
      break;
    case 2: /* E */
      turn_degrees(90);
      break;
    case 3: /* SE */
      turn_degrees(135);
      break;
    case 4: /* S */
      turn_degrees(180);
      break;
    case 5: /* SW */
      turn_degrees(-135);
      break;
    case 6: /* W */
      turn_degrees(-90);
      break;
    case 7: /* NW */
      turn_degrees(-45);
      break;
    }
}
/*---------------------Print Node Info---------------------------------------*/
void printNode(int i, int j)
{/* display the information about an element in the array */
  printf("\nNext From %d\n", p[i][j].next);
  printf("Turn %d\n", p[i][j].turn);
  printf("Stop %c\n", p[i][j].stop);
  printf("Turn to North %d\n", p[i][j].tn);
}
/*---------------------------------------------------------------------------*/
/*                      main                                                 */
/*-------------------------------------------------------------------------- */
main(void)
{
  short c;
  char hold1[3], hold2[3], hold3[3];
  int from, to, turn, turn2, drct;
  char stop;

  init_paths();
  /* connect to the robot */
  connect_robot(1, MODEL_SCOUT2, "/dev/ttyS0", 38400);

  /* turn on the sonars */
  turn_sonar_on();
  from=0;
  /* prompt */
while ((from<1)||(from>12)) 
{ /* keep asking until from = a valid quadrent */
   printf("From? \n");
   scanf("%s", hold1);
   /* convert the string entered by the user to an int */
   if (strcmp(hold1, "1")==0)
       from=1;
   else if (strcmp(hold1, "2")==0)
       from=2;
   else if (strcmp(hold1, "3")==0)
       from=3;
   else if (strcmp(hold1, "4")==0)
       from=4;
   else if (strcmp(hold1, "5")==0)
       from=5;
   else if (strcmp(hold1, "6")==0)
       from=6;
   else if (strcmp(hold1, "7")==0)
       from=7;
   else if (strcmp(hold1, "8")==0)
       from=8;
   else if (strcmp(hold1, "9")==0)
       from=9;
   else if (strcmp(hold1, "10")==0)
       from=10;
   else if (strcmp(hold1, "11")==0)
       from=11;
   else if (strcmp(hold1, "12")==0)
       from=12;
  }
while ((to<1)||(to>12))
{ /* keep asking until a valid destination quadrent is entered */
   printf("To? \n");
   scanf("%s", hold2);
   /* convert the string entered by the user to an int */
   if (strcmp(hold2, "1")==0)
       to=1;
   else if (strcmp(hold2, "2")==0)
       to=2;
   else if (strcmp(hold2, "3")==0)
       to=3;
   else if (strcmp(hold2, "4")==0)
       to=4;
   else if (strcmp(hold2, "5")==0)
       to=5;
   else if (strcmp(hold2, "6")==0)
       to=6;
   else if (strcmp(hold2, "7")==0)
       to=7;
   else if (strcmp(hold2, "8")==0)
       to=8;
   else if (strcmp(hold2, "9")==0)
       to=9;
   else if (strcmp(hold2, "10")==0)
       to=10;
   else if (strcmp(hold2, "11")==0)
       to=11;
   else if (strcmp(hold2, "12")==0)
       to=12;
  }
drct=8; /* drct is an int that holds an int 
       corresponding to the direction the robot is facing */
while ((drct<0)||(drct>7))
{ /* ask the suer for what the starting direction is */
   printf("Direction faced? \n");
   scanf("%s", hold3);
   /* if a valid direction is entered, set drct */
   if (strcmp(hold3, "n")==0) 
       drct=0;
   else if (strcmp(hold3, "ne")==0)
       drct=1;
   else if (strcmp(hold3, "e")==0)
       drct=2;
   else if (strcmp(hold3, "se")==0)
       drct=3;
   else if (strcmp(hold3, "s")==0)
       drct=4;
   else if (strcmp(hold3, "sw")==0)
       drct=5;
   else if (strcmp(hold3, "w")==0)
       drct=6;
   else if (strcmp(hold3, "nw")==0)
       drct=7;
  }
  turn_north(drct); /* turn the robot until it faces north
                        depending on what direction it is currently facing */
  printf("\nFrom %d\n", from);/* display the quadrent the robot is currently at */
  printf("\nTo %d\n", to); /* display the destination quadrent, where it's trying to get to*/
  printNode(from,to); /* display the path information stored in the array*/
  while (from!=to) 
	{/* while the robot is not at the destination quadrent */
		/* get values from array */
		turn=p[from][to].turn; /*turn the specified number of degrees 
                             to get to the next node in the path */
		printf("\n%d\n", turn);
		stop=p[from][to].stop;  /*Get stopping condition from the array */
                            /*This is the cond that tells the robot it has reached the next quadrant */
		printf("\n%d\n", stop);
		printf("\n%c\n", stop);
		turn2=p[from][to].tn; /* the the turn needed to make the robot face north after it's reached the next quadrant */
		from=p[from][to].next; 
		printf("\nNext Node %d\n",from);
		turn=turn;
		turn_degrees(turn); /* turn to face the direction to go to the next quadrant */

		switch(stop)
		{
		case 'l': /* stop until find an opening on the left */
		        printf("\nCalling To_Opening_on_Left\n");
			to_opening_on_left(State);
			break;
		case 'r': /* stop until find an opening on the right */
		        printf("\nCalling To_Opening_on_Right\n");
			to_opening_on_right(State);
			break;
		case 'f': /* stop until find a wall */
			to_wall_in_front(State);
			break;
		case 'a': /* stop until find walls in front, left, and right */
			to_wall_all(State);
			break;
		case 'e': /* Error something went wrong, trying to get to a node that doesn't exist */
			printf("Error From=0");
			break;
		}

	       	turn_degrees(turn2);  /* turn to end facing north, to be ready for next position */
	}	
  
  st();
  printf("\n End of Program. \n");             
  return(0);
}