#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 80
#define CLOSE 40
#define OBS 36   /* 20 */
#define open_amount 36
#define close_amount 36

struct PathType {
  int next; /* next quadrant */
  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[17][17];

/*-----------------Initialize Paths-------------------------------------*/
void init_paths(void)
{
  int i,j;

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

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

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

  /* set all paths from 3 */
  for (i=0; i<16; 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=0; i<=3; i++)
    {
      p[4][i].next=2;
      p[4][i].turn=180;
      p[4][i].stop='r'; 
      p[4][i].tn=-180;
    }
  for (i=5; i<=7; i++)
    {
      p[4][i].next=5;
      p[4][i].turn=0;
      p[4][i].stop='f'; 
      p[4][i].tn=0;
    }
 for (i=8; i<=16; i++)
    {
      p[4][i].next=8;
      p[4][i].turn=-90;
      p[4][i].stop='l'; 
      p[4][i].tn=90;
    }

  /* set all paths from 5 */
  for (i=0; i<=4; i++)
    {
     p[5][i].next=4;
     p[5][i].turn=180;
     p[5][i].stop='l'; 
     p[5][i].tn=-180;
    }
  p[5][6].next=6;
  p[5][6].turn=90;   
  p[5][6].stop='i';      
  p[5][6].tn=-90;
  p[5][7].next=7;
  p[5][7].turn=0;   
  p[5][7].stop='i';      
  p[5][7].tn=0;
  for (i=8; i<=16; i++)
    {
     p[5][i].next=4;
     p[5][i].turn=180;
     p[5][i].stop='l'; 
     p[5][i].tn=-180;
   }

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

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

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

  /* set paths from 11 */
  for (i=0; i<=16; i++)
     {
       p[11][i].next=10;
       p[11][i].turn=180;
       p[11][i].stop='r'; 
       p[11][i].tn=-180;
     }

  /* set paths from 12 */
  for (i=0; i<=16; i++)
     {
       p[12][i].next=10;
       p[12][i].turn=0;
       p[12][i].stop='l'; 
       p[12][i].tn=0;
     }

 /* set paths from 13 */
  for (i=0; i<=12; i++)
     {
       p[13][i].next=10;
       p[13][i].turn=90;
       p[13][i].stop='r'; 
       p[13][i].tn=-90;
     }
 for (i=14; i<=16; i++)
     {
       p[13][i].next=14;
       p[13][i].turn=180;
       p[13][i].stop='r'; 
       p[13][i].tn=-180;
     }

 /* set paths from 14 */
  for (i=0; i<=13; i++)
     {
       p[14][i].next=13;
       p[14][i].turn=0;
       p[14][i].stop='l'; 
       p[14][i].tn=0;
     }
   p[14][15].next=15;
   p[14][15].turn=90;
   p[14][15].stop='i'; 
   p[14][15].tn=-90;
   p[14][16].next=16;
   p[14][16].turn=-90;
   p[14][16].stop='i'; 
   p[14][16].tn=90;

 /* set paths from 15 */
  for (i=0; i<=16; i++)
     {
       p[15][i].next=14;
       p[15][i].turn=-90;
       p[15][i].stop='l'; 
       p[15][i].tn=90;
     }

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

  /* set all paths where from and to are the same */
  for (i=0; i<16; i++)
     {
       p[i][i].next=-1;
       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); 
}
/*----------------------------------------------------------------------*/
int get_shortest_sonar(long *state, int min_dist)
{
  int i;
  int min_value = 255; /*max sonar reading*/
  int min_sonar = -1;
  
  for (i=0; i<=4; i++)
    if (State[STATE_SONAR_0+i]<min_value)
      {
	min_value=State[STATE_SONAR_0+i];
	min_sonar=i;
      }

  for (i=12; i<=15; i++)
    if (State[STATE_SONAR_0+i]<min_value)
      {
	min_value=State[STATE_SONAR_0+i];
	min_sonar=i;
      }
  if (State[STATE_SONAR_0+min_sonar]<=min_dist)
    return(min_sonar);
  else
    return(-1);
}
/*---------------------------------------------------------------------------*/
int sn_avg(int y, long *state){

 int i=0;
 int sn[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
 for (i=0;i<4;i++)
   {
     get_sn();
     sn[0]=sn[0]+State[STATE_SONAR_0];
     sn[1]=sn[1]+State[STATE_SONAR_1];
     sn[4]=sn[4]+State[STATE_SONAR_4];
     sn[12]=sn[12]+State[STATE_SONAR_12];
     sn[15]=sn[15]+State[STATE_SONAR_15];   
  }
printf("%d,  %d",y,sn[y]/4);
return (sn[y]/4);
}
/*-------------------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)
{
    get_sn();
    x=get_shortest_sonar(State,OBS);
    printf("\n SHORTEST SONAR = %d\n", x);
    switch(x)
    {
    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();
    temp=(State[STATE_SONAR_12]+State[STATE_SONAR_13]+State[STATE_SONAR_11])/3;
}
while (!opR)
  {
    get_sn();
    x=get_shortest_sonar(State,OBS);
    printf("\n SHORTEST SONAR = %d\n", x);
    switch(x)
    {
    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();
      saver=(State[STATE_SONAR_12]+State[STATE_SONAR_13]+State[STATE_SONAR_11])/3;
    
      if (saver>=OPEN)
      {
	opR=1;
	printf("\nFound door on right\n");
	scout_vm(50,0);
	sleep(2);
        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)
{
    get_sn();
    x=get_shortest_sonar(State,OBS);
    printf("\n SHORTEST SONAR = %d\n", x);
    switch(x)
    {
    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();
    temp=(State[STATE_SONAR_4]+State[STATE_SONAR_5]+State[STATE_SONAR_3])/3;
}
while (!opL)
  {
    get_sn();
    x=get_shortest_sonar(State,OBS);
    printf("\n SHORTEST SONAR = %d\n", x);
    switch(x)
    {
    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();
      savel=(State[STATE_SONAR_4]+State[STATE_SONAR_5]+State[STATE_SONAR_3])/3;
      if (savel>=OPEN)
      {
	opL=1;
	printf("\nFound opening on left\n");
       	scout_vm(50, 0);
       	sleep(2);
        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)
  {
    get_sn();
    x=get_shortest_sonar(State,OBS);
    printf("\n SHORTEST SONAR = %d\n", x);
    switch(x)
    {
    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;
     
 if (savef<=42)
      {
	wallF=1;
	printf("\nFound wall in front\n");
      } 
   
    
   } /* End of While Loop */
        scout_vm(0,0); 
} /* end of to_wall_in_front */ 
/*---------------------------------Go Inside Opening -------------------*/
void go_inside(long * state)
{
int count, x, prev;

  count=0;
  scout_vm(50,0);
  sleep(4);
  while (count<=200)
  {  
    get_sn();
    x=get_shortest_sonar(State,OBS);
    printf("\n SHORTEST SONAR = %d\n", x);
    switch(x)
    {
    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;
    }
  count=count+1;
  }
  st();
  system("play MAIL.wav");
  sleep(15);
  /* code to go inside an opening goes here */

}  
/*------------------Turn a Given Degrees-------------------------------------*/
void turn_degrees(int turn_amt)
{
  st();      
  scout_vm(0,turn_amt*10);
  sleep(1);
  st();
  printf("\nTurning\n");
}
/*------------------Initial Turn to Face North-------------------------------*/
void turn_north(int 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)
{
  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 temp[10], hold1[3], hold2[3], hold3[3];
  int hold0[16], i, 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();
  /* prompt */
for (i=0; i<=15; i++)
  hold0[i]=0;
strcpy(temp,":)");
i=1;
while ((i<15) && (strcmp(temp,"#")!=0))
{
  printf("Enter the names of the office I will visit:\n");
  scanf("%s", temp);
  if (strcmp(temp,"Martha")==0)
   hold0[i]=15;
  else if (strcmp(temp, "Gelfond")==0)
   hold0[i]=9;
  else if (strcmp(temp, "Bell")==0)
   hold0[i]=6;
  else if (strcmp(temp, "Bernat")==0)
   hold0[i]=7;
  else if (strcmp(temp, "Robotics")==0)
   hold0[i]=11;
  else if (strcmp(temp, "Longpre")==0)
   hold0[i]=3;
  else if (strcmp(temp, "Conf1")==0)
   hold0[i]=1;
  else if (strcmp(temp, "Conf2")==0)
   hold0[i]=12;
  i=i+1;
}
printf("Enter in the office that I am starting from:\n");
scanf("%s", temp);
i=15;
if (strcmp(temp,"Martha")==0)
  hold0[i]=15;
 else if (strcmp(temp, "Gelfond")==0)
  hold0[i]=9;
 else if (strcmp(temp, "Bell")==0)
  hold0[i]=6;
 else if (strcmp(temp, "Bernat")==0)
  hold0[i]=7;
 else if (strcmp(temp, "Robotics")==0)
   hold0[i]=11;
 else if (strcmp(temp, "Longpre")==0)
   hold0[i]=3;
 else if (strcmp(temp, "Conf1")==0)
   hold0[i]=1;
 else if (strcmp(temp, "Conf2")==0)
   hold0[i]=12;
hold0[0]=hold0[i];
drct=8;
while ((drct<0)||(drct>7))
  {
   printf("Direction faced? \n");
   scanf("%s", hold3);
   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);
  from=hold0[15];
  i=0;
while (i<15)
{
  if (i!=0)
    from=hold0[i];
  to=hold0[i+1];
  if (to==0)
  {
    i=15;
    to=hold0[i];
  }
  printf("\nFrom %d\n", from);
  printf("\nTo %d\n", to);
  printNode(from,to);
  while (from!=to) 
	{
		/* get values from array */
		turn=p[from][to].turn;
		printf("\n%d\n", turn);
		stop=p[from][to].stop;
		printf("\n%d\n", stop);
		printf("\n%c\n", stop);
		turn2=p[from][to].tn;
		from=p[from][to].next;
		printf("\nNext Node %d\n",from);
      		turn_degrees(turn); 

		switch(stop)
		{
		case 'l':
		        printf("\nCalling To_Opening_on_Left\n");
			to_opening_on_left(State);
			break;
		case 'r':
		        printf("\nCalling To_Opening_on_Right\n");
			to_opening_on_right(State);
			break;
		case 'i':
		        printf("\nCalling Go_Inside\n");
			go_inside(State);
			break;
		case 'f':
		        printf("\nCalling Wall In Front\n");
			to_wall_in_front(State);
			break;
		case 'e':
			printf("Error From=To");
			break;
		}

	       	turn_degrees(turn2); 
	}	
  i=i+1;  
}
  st();
  printf("\n End of Program. \n");             
  return(0);
}