/********* file nav.c *************/

/* note: largest possible grid with existing memory = 15 x 40 */

#define GRID_SIZE_X 10  
#define GRID_SIZE_Y 37  
#define EMPTY 0
#define FULL 1
#define BIG_COST 500.
#define MAX_VELOCITY 0.1
#define SQRT2 1.4142136
#define Pi 3.14159


/***********************

Globals 

***********************/

char occupancy[GRID_SIZE_X][GRID_SIZE_Y];
float cost[GRID_SIZE_X][GRID_SIZE_Y];
int robot_x, robot_y;
char goal_x, goal_y;
float x_part, y_part;

float x1;
float x0;
float y1;
float y0;
float theta_0;
float theta_1;

/* the following are for robot discovered obstacles */

persistent char new_obstacle=0;
persistent char A1=0;
persistent char B1=0;
persistent char C1=0;
persistent char D=0;
persistent char E=0;
persistent char F=0;
persistent char G=0;
persistent char H=0;
persistent char I=0;
persistent char J=0;
persistent char K=0;
persistent char L=0;
persistent char M=0;
persistent char N=0;
persistent char O=0;
persistent char P=0;
persistent char Q=0;
persistent char R1=0;
persistent char S=0;
persistent char T=0;
persistent char U=0;
persistent char V=0;
persistent char W=0;
persistent char X=0;
persistent char Y=0;
persistent char Z=0;


void save_new_obstacle(int obstacle_x, int obstacle_y)

  {if(new_obstacle==1)
    {A1=obstacle_x;
     B1=obstacle_y;}
  else if (new_obstacle==2)
    {C1=obstacle_x;
     D=obstacle_y;}
  else if (new_obstacle==3)
    {E=obstacle_x;
     F=obstacle_y;}
  else if (new_obstacle==4)
    {G=obstacle_x;
     H=obstacle_y;}
  else if (new_obstacle==5)
    {I=obstacle_x;
     J=obstacle_y;}
  else if (new_obstacle==6)
    {K=obstacle_x;
     L=obstacle_y;}
  else if (new_obstacle==7)
    {M=obstacle_x;
     N=obstacle_y;}
  else if (new_obstacle==8)
    {O=obstacle_x;
     P=obstacle_y;}
  else if (new_obstacle==9)
    {Q=obstacle_x;
     R1=obstacle_y;}
  else if (new_obstacle==10)
    {S=obstacle_x;
     T=obstacle_y;}
  else if (new_obstacle==11)
    {U=obstacle_x;
     V=obstacle_y;}
  else if (new_obstacle==12)
    {W=obstacle_x;
     X=obstacle_y;}
  else if (new_obstacle==13)
    {Y=obstacle_x;
     Z=obstacle_y;}
  
}

/*******************************************************

       set up everything 

*******************************************************/

void set_up()
 {init();
   load_map();
    replan();}
 

/*****************************************************

      load the map into memory (partly provided to the robot by humans)

******************************************************/


void load_map()
{
/* robot-found obstacles........... */
        occupancy[A1][B1]=FULL;
        occupancy[C1][D]=FULL;
        occupancy[E][F]=FULL;
        occupancy[G][H]=FULL;
        occupancy[I][J]=FULL;
        occupancy[K][L]=FULL;        
        occupancy[M][N]=FULL;
        occupancy[O][P]=FULL;
        occupancy[Q][R1]=FULL;
        occupancy[S][T]=FULL;
        occupancy[U][V]=FULL;
        occupancy[W][X]=FULL;
        occupancy[Y][Z]=FULL;
        
/* existing obstacles......................*/
        occupancy[1][28]=FULL;
        occupancy[2][28]=FULL; 	
        occupancy[3][28]=FULL;
        occupancy[4][28]=FULL;
        occupancy[3][29]=FULL;
        occupancy[3][30]=FULL; 	
        occupancy[3][31]=FULL;
        occupancy[3][32]=FULL;
        occupancy[4][33]=FULL;
        occupancy[1][1]=FULL;
        occupancy[1][2]=FULL; 	
        occupancy[1][3]=FULL;
        occupancy[1][8]=FULL;
        occupancy[1][9]=FULL;
        occupancy[3][13]=FULL;
        occupancy[3][14]=FULL; 	
        occupancy[3][15]=FULL;
        occupancy[3][16]=FULL;
        occupancy[4][16]=FULL;
        occupancy[5][16]=FULL; 	
        occupancy[6][16]=FULL;
        occupancy[7][16]=FULL;
        occupancy[2][19]=FULL;
        occupancy[4][10]=FULL;
        occupancy[5][10]=FULL; 	
        occupancy[6][10]=FULL;
        occupancy[7][10]=FULL;
        occupancy[6][8]=FULL;
        occupancy[7][8]=FULL; 	
        occupancy[6][9]=FULL;
        occupancy[7][9]=FULL;
        occupancy[1][21]=FULL;
        occupancy[1][20]=FULL;
        occupancy[2][22]=FULL;
        occupancy[1][22]=FULL;
        occupancy[2][21]=FULL;
        occupancy[2][20]=FULL;
        occupancy[4][11]=FULL;
        occupancy[4][12]=FULL; 	
        occupancy[4][13]=FULL;
        occupancy[5][23]=FULL;
        occupancy[6][23]=FULL; 	
        occupancy[7][23]=FULL;
        occupancy[3][26]=FULL;
        occupancy[3][25]=FULL;
        occupancy[4][25]=FULL;  
        occupancy[5][25]=FULL;
        occupancy[6][25]=FULL; 	
        occupancy[7][25]=FULL;
        occupancy[3][24]=FULL;
        occupancy[5][24]=FULL;
}



/*************************

      init()

Initialize the occupancy and cost grids

*****************************/

void init()

 {int i,j;
  for (i=0; i < GRID_SIZE_X; i++)
    {for (j=0; j< GRID_SIZE_Y; j++)
      {occupancy[i][j] = EMPTY;
       cost[i][j] = BIG_COST;
      
      }
    }
}

/********************************

	cell_cost()

Calculate the cost of traveling from a cell to 
the goal.  Returns 0 if no change, 1 otherwise.

**********************************/

int cell_cost(int x, int y)
{
int i, j, low_x, low_y;
float temp_cost, low_cost = BIG_COST;

if (occupancy[x][y] == FULL)
	{cost[x][y] = BIG_COST;
         return 0;}

/* check to see if its the goal*/

if ((goal_x == x) && (goal_y == y)) 
	{cost[x][y] = 0.;
         return 0;}

low_x = x;
low_y = y;
low_cost = cost[x][y];	
for (i=(x-1);i<=(x+1); i++)
	{for (j=(y-1);j<=(y+1); j++)
		{if ((i==x) || (j==y)) 
                   temp_cost=cost[i][j] + 1.;
                 else
                   temp_cost=cost[i][j] + SQRT2;
		 if (temp_cost<low_cost)
 		   low_cost=temp_cost;
		   low_x = i;
		   low_y = j;
                }
	}


	if (cost[x][y] != low_cost) 
            
                 {cost[x][y] = low_cost;
		 return 1;}
        else
                 return 0;}


/**************************************
	replan()

compute the cost grid based on the map represented in the occupancy grid.

****************************************/

void replan()
{int i, j, count=1;
  enable_encoders=0; /*shut off the encoders in the system interrupt so this will go faster */
  while(count != 0)
   	{count=0;
         for (i=1; i < GRID_SIZE_X-1; i++)
		{
                          for (j=1; j < GRID_SIZE_Y-1; j++)
                          count = count + cell_cost(i,j);
                        }
              }
   enable_encoders = 1;
}


/*********************************************

	check_plan()

Look at the cost grid to decide which way to go.  Returns a heading
in radians between 0 and 2*Pi. East (+x) is 0, north is (+y) is Pi/2.

**********************************************/

void check_plan(int x, int y)
{int i,j;
 float low_x=0., low_y=1., low_cost;
   x_part=0.;
   y_part=0.;

/* Find the x and y parts of the gradient */

for(i=(x-1); i<=(x+1); i++)
 {for(j=(y-1); j<=(y+1); j++)
   {if (i<x) x_part += cost[i][j];
    if (i>x) x_part -= cost[i][j];
    if (j<y) y_part += cost[i][j];
    if (j>y) y_part -= cost[i][j];
   }
 }
        low_cost = cost[x][y];
	x_part = 0.;
	y_part = 1.;

	for(i=(x-1); i<=(x+1); i++)
           {for(j=(y-1); j<=(y+1); j++) 
               if(cost[i][j]<low_cost)
		{low_cost = cost[i][j];
		 low_x=(float)(i);
                 low_y=(float)(j);
                         }
              }

	/* Use the direction towards the lowest cost cell
           as the heading.  */

	x_part = low_x - (float)(x);
            y_part = low_y - (float)(y);
         }
       



/***********************************************************

       run the course

**********************************************************/


void do_course()
	
         {set_up();
            while(1)
             {
                 if(round(x0/12.)==goal_x && round(y0/12.)==goal_y)
                  break;

                  if(y0 == 12.* 11. || y0 == 12.*14. || y0 == 12.*17.|| y0 == 12.*20. || y0 == 12.*23.|| y0 == 12.*26. || y0 == 12.*29. || y0 == 12.*31.)
                  align_to_compass();  /* do this every three feet */
	       		
                  check_plan(round(x0/12.),round(y0/12.));
                  if  (x_part < -1.)
            	{x1 = x0-12.;
                         x_part=-1.;}
                  else if (x_part > 1.)
            	{x1 = x0+12.;
                         x_part=1.;} 
                 else x1 = (x0 + (x_part*12.));

                 if  (y_part < -1.)
                      {y1 = y0-12.;
                         y_part=-1.;}
	    else if (y_part > 1.)
                      {y1 = y0+12.; 
                        y_part=1.;}
                 else y1 = (y0 + (y_part*12.));
	
                 nav();} /* take the solution and run with it */
		 
                 sleep(.5);
               }
                 

/*******************************************************

        engine room

********************************************************/

/* motor 0 is on the right, Encoder2 & 1 is on the right*/
/*motor 0 is the fast one*/

#define tilt servo_a7_pulse
#define pan servo_a3_pulse 
#define left_motor 1
#define right_motor 0
#define range analog(4)


void servo3_on()
 {servo_a3_init(1);
 }

void servo3_off()
 {servo_a3_init(0);}

void servo7_on()
 {servo_a7_init(1);
  }

void servo7_off()
 {servo_a7_init(0);
  pokeword(0x1016,0); /*do this to renable motor speed*/
}

/*********************************

nav routines

************************************/
 
  int diagonal;
 float delta_x;
 float delta_y;

void nav()
 
{int obstacle_x, obstacle_y;

 float R_temp;
 float x2,y2;
 int R;
 float theta;
 
delta_x=x1-x0;
delta_y=y1-y0;
R_temp=sqrt(delta_x*delta_x + delta_y*delta_y);
R=round(R_temp);

         if (delta_x == 0. && delta_y > 0.)
                {theta = Pi / 2.;}
    
         else  if (delta_x == 0. && delta_y < 0.)
                {theta = -Pi / 2.;}

         else if (delta_x > 0. && delta_y >= 0.)
 		{theta = atan(delta_y/delta_x);}

         else if (delta_x < 0. && delta_y >= 0.)
 		{theta = Pi + atan(delta_y/delta_x);}

         else if (delta_x < 0. && delta_y < 0.)
 		{theta = Pi + atan(delta_y/delta_x);} 
         else 
 		{theta = 2.*Pi + atan(delta_y/delta_x);}
   
         theta_1 = theta - theta_0;
    
         theta_1 = theta_1 * 57.29578; /*convert to degrees*/

/*is the robot now aligned on a diagonal? */

          if ( (int)delta_x == 0 || (int)delta_y == 0)
             diagonal = 0;
          else 
             diagonal = 1;
          
         turn (round(theta_1)*-1);} 

         if (diagonal == 1)
            { scan_diagonal();} /*look around with IR ranger to see if anything is out there that shouldn't be */
         else
            { scan_aligned();} /*ditto*/

         theta_0=theta; /*already at new angle, so this is done no matter what */

         if(hit_zone == 0) /*meaning there are no new obstacles*/
            {go(R); /* so go ahead and go */
             x0=x1;
             y0=y1;
             sleep(.5);}

         else if(hit_zone == 1) /* something is out there!  works for both scan_diagonal and scan_aligned */
            {occupancy[obstacle_x=(int)(x1/12.)][obstacle_y=(int)(y1/12.)]=FULL;
             save_new_obstacle(obstacle_x, obstacle_y);
            set_up();
             }

             else
                {go(R);
                 x0=x1;
                 y0=y1;}}

              
/*********************************
 translation directive go()
 
**********************************/

float clicks=0.0;
int left_offset=0;
int right_offset=0;

void go(int inches)
   {int speed=60;
     clicks=20.16*(float)(inches)*1.34375; /* fiddled with this during development, could be one constant */
     encoder0_counts=encoder1_counts=encoder2_counts=encoder3_counts=0; /*zero the encoders*/
     left_encoder= left_offset + left_turn_offset; /* the offsets are corrections from previous overshoots */
     right_encoder= right_offset + right_turn_offset;
    
     motor(right_motor,86);
     while (right_encoder+22<round(clicks))
     { if(right_encoder+28 >= round(clicks))
         speed=10;  /*deacellerate*/
     
       {if (left_encoder<right_encoder)
         {motor(right_motor,10); }
        else
          motor(right_motor,speed);}

       {if(left_encoder>right_encoder)
        {motor(left_motor,10);}
        else
        motor(left_motor,speed);}}

        motor(right_motor,0);
        sleep(.3);
        motor(left_motor,0);
        sleep(1.0);
      
 while (right_encoder<round(clicks) || left_encoder<round(clicks))
     {
     if (left_encoder<round(clicks) && left_encoder<=right_encoder)
         {jog_left(.04,1);}

     if (right_encoder<round(clicks) && right_encoder<=left_encoder)
         {jog_right(.04,1);}

     sleep(.25);}
     
     sleep(1.); 
     left_offset= left_encoder-round(clicks); /*these are overshoot numbers to use next time*/
     right_offset= right_encoder-round(clicks);
     right_turn_offset=left_turn_offset=0; /*only use once after a turn */       
       }


/*****************************************

    Rotation directive turn()

******************************************/

      int left_turn_offset=0;
      int right_turn_offset=0;

void turn(int degrees)
      {
        int adder;  /* correct a turn deficiency at 45 degrees */
        int speed = 45;
        float turn_cal=1.0;
        float right_turn_cal=45./43.26; /*41.5   43.26*/
        float left_turn_cal=45./43.26;
        int dir;
      
        if (degrees==45 || degrees==-45)
          adder=-2; 
          else if (degrees==180 || degrees==-180)
          adder=-5; 
          else adder=0;

        if (degrees > 180)
             degrees= (degrees-360); /*don't go the long way*/
        if (degrees < -180)
             degrees= (degrees+360); /* ditto */
	
        {if(degrees < 0)
        	{dir=-1;
              turn_cal=left_turn_cal;
              degrees=-degrees;
                 }
   	else
	{dir=1;
	  turn_cal=right_turn_cal;}}

              clicks = 2. *.65946 * (float)(degrees) * turn_cal*1.34375 ;
              encoder0_counts=encoder1_counts=encoder2_counts=encoder3_counts=0; /*zero the encoders*/

            if (dir==1) /*clockwise turn */
           {left_encoder=(left_turn_offset+left_offset-adder);
	 right_encoder=-(right_turn_offset+right_offset+adder);}

            else
            {left_encoder= -(left_turn_offset+left_offset+adder);
	  right_encoder= (right_turn_offset+right_offset-adder);}

        motor(left_motor,dir*speed);
        motor(right_motor,-dir*speed);

        while (right_encoder+12<round(clicks))
       {if(right_encoder+23 >= round(clicks))
         speed=10;  /*deacellerate*/
  
       {if(right_encoder<left_encoder)
        motor(left_motor,dir*7);
        else
        motor(left_motor,dir*speed);}

       {if(right_encoder>left_encoder)
        motor(right_motor,-dir*7);
        else
        motor(right_motor,-dir*speed);}}
     
       stop();
       sleep(.5);
      
       while (right_encoder<round(clicks) || left_encoder<round(clicks))
     {
        if (right_encoder<round(clicks) && right_encoder<=left_encoder)
        {jog_right(.04,-dir);}

     if (left_encoder<round(clicks) && left_encoder <= right_encoder)
         {jog_left(.04,dir);}
          sleep(.25);}
          sleep(1.0);  /*let wheels get stopped*/

          left_turn_offset = dir*(left_encoder-round(clicks)); /* overshoot corrections */
          right_turn_offset = -dir*(right_encoder-round(clicks));
          left_offset=right_offset=0; /*already used these from last translation, kill 'em */

       }
	
/*************************************

   IR scan functions

**************************************/ 

int hit_zone=0;

void scan_aligned() /* aligned with x or y axis, not on a diagonal track */
   {int deg=40;
    int k;
    float look[10];
    int hit=0;
    hit_zone=0;
    pan_deg(deg);
    sleep(.4); /*allow extra sweep time */
   
    for(k=1;k<10;k++)
     {look[k]=range_avg();
        if (look[k] > 0.)
         {hit=1;}
      deg=deg-10;
      pan_deg(deg);}
      
      pan_deg(0); /*center servo*/
    
    while(hit) /* only look at an area covered by the square directly ahead */
     {if((look[1]>0. && look[1]<=9.31) || (look[9]>0. && look[9]<=9.31) || (look[2]>0. && look[2]<=12.0) || (look[8]>0. && look[8]<=12.0) || (look[3]>0. && look[3]<=17.54) || (look[7]>0. && look[7]<=17.54) || (look[4]>0. && look[4]<=17.54) || (look[6]>0. && look[6]<=17.54) || (look[5]>0. && look[5]<=17.54))
         {hit_zone=1;
          new_obstacle++;}
      
         hit=0;}}



void scan_diagonal() /* look at an area of a square directly diagonal to the square we are on */
   {int deg=40;
    int k;
    float look[10];
    int hit=0;
    
    hit_zone=0;
    pan_deg(deg);
     sleep(.4); /*allow extra sweep time */

    for(k=1;k<10;k++)
     {look[k]=range_avg();
        if (look[k] > 0.)
         {hit=1;}
      deg=deg-10;
      pan_deg(deg);
      }
    pan_deg(0); /*center servo*/
 
    while(hit)
     {if((look[1]>0. && look[1]<=14.61) || (look[9]>0. && look[9]<=14.61) || (look[2]>0. && look[2]<=15.07) || (look[8]>0. && look[8]<=15.07) || (look[3]>0. && look[3]<=16.05) || (look[7]>0. && look[7]<=16.05) || (look[4]>0. && look[4]<=17.77) || (look[6]>0. && look[6]<=17.77) || (look[5]>0. && look[5]<=20.58))
         {hit_zone=1;
       new_obstacle++;}
       hit=0;}}


/***********************************************

           JOG motors to finish move command

***********************************************/
        

void jog_right(float on,int dir)
  {motor (right_motor,dir*30);
   sleep (on);
   motor (right_motor,0);
   }


void jog_left(float on, int dir)
  {motor (left_motor,dir*30);
   sleep (on);
   motor (left_motor,0);
   }



/***********************************************

           ALIGN_TO_COMPASS

***********************************************/

void align_to_compass()
      {float correct_theta_0;
        int local_deviation;
       int mag_heading;

        if (theta_0 < 0.)
        correct_theta_0= theta_0 + (2. * Pi);
        else correct_theta_0=theta_0;

	if(correct_theta_0 + .1 < Pi/4. )  /*.1 added to cure rounding error */
		mag_heading=76;

   	else if(correct_theta_0 + .1 <  Pi/2.) 
		mag_heading=31;  /* these constants represent headings spaced 45 degrees around the compass */

	else if(correct_theta_0 + .1 < 3.*Pi/4.)   
		mag_heading=344;

   	else if(correct_theta_0 + .1 <  Pi)
		mag_heading=304;

   	else if(correct_theta_0 + .1 < 5.*Pi/4.)
		mag_heading=260;

   	else if(correct_theta_0 + .1 < 3.*Pi/2.)
		mag_heading=212;

   	else if(correct_theta_0 + .1 < 7.*Pi/4.)
		mag_heading=159; 

   	else mag_heading=120;

/* make the corrections for mag deviation based on location during compass read */

	if (y0==12.*11.)
      		{if (x0==12.*3.)
                   local_deviation=-28; 
		else if (x0==12.*2.)
                   local_deviation=-24;  
	            else if (x0==12.*1.)
                   local_deviation=-24;}

	else if (y0==12.*14.)
      		{if (x0==12.*2.)
                   local_deviation=-19;
		else if (x0==12.*1.)
                   local_deviation=-29;}

	else if (y0==12.*17.)
      		{if (x0==12.*4.)
                   local_deviation=-12;
		else if (x0==12.*3.)
                   local_deviation=-15; 
		else if (x0==12.*2.)
                   local_deviation=-20; 
		else if (x0==12.*1.)
                   local_deviation=-22;}

	else if (y0==12.*20.)
      		{if (x0==12.*5.)
                   local_deviation=-11;
		else if (x0==12.*4.)
                   local_deviation=-11;
		else if (x0==12.*3.)
                   local_deviation=-10;} 

	else if (y0==12.*23.)
      		{if (x0==12.*2.)
                   local_deviation=-2;
		else if (x0==12.*3.)
                   local_deviation=-2;
		else if (x0==12.*4.)
                   local_deviation=-2;}

	else if (y0==12.*26.)
      		{if (x0==12.*1.)
                   local_deviation=3;
		else if (x0==12.*2.)
                   local_deviation=3;}

	else if (y0==12.*29.)
      		{if (x0==12.*4.)
                   local_deviation=-2;
		else if (x0==12.*5.)
                   local_deviation=1;
		else if (x0==12.*6.)
                   local_deviation=-2;
		else if (x0==12.*7.)
                   local_deviation=-2;}


	else if (y0==12.*31.)
      		{if (x0==12.*4.)
                   local_deviation=-2;
		else if (x0==12.*5.)
                   local_deviation=-3;
		else if (x0==12.*6.)
                   local_deviation=-2;
		else if (x0==12.*7.)
                   local_deviation=-2;}

        while(1)
		{
                   if (read() < (mag_heading - 1 + local_deviation))
                    {jog_left(.04,1);}
                     
                   else if (read() > (mag_heading + 1 + local_deviation))
                   {jog_right(.04,1);}

                   else break;}

        left_offset=right_offset=left_turn_offset=right_turn_offset=0;  /* corrections for any overshoots */
           }


/**************************************

      servo control 

**************************************/

void pan_deg(int deg)
  {float pan_temp;
   pan_temp=3035. + ((float)(deg)*18.30);  /* constants determined by experiment */
   pan=(int)(pan_temp);
   servo3_on();
   pan=(int)(pan_temp);
   sleep(1.0);
   pan=(int)(pan_temp);
   sleep (.4); 
   servo3_off();
   }

/***********************************

   master control executive-- the main() function

***********************************/

int q;

void main()
{
    tilt=3600;
    sleep(.1);
    servo7_on();
    sleep(.8);
    servo7_off();
    pan_deg(0);
    servo3_on();
    sleep(1.);

    encoder_low_threshold=75;
    encoder_high_threshold=150;

    reset_compass();

/* set initial nav variables  */
    x0=5.;/*initial position is x = 5*/
    y0=9.;/*initial position is y = 9*/
    goal_x=3; /*to first waypoint*/
    goal_y=20; /*to first waypoint*/
    theta_0=3.*Pi/2.; /*initial orientation */

    x1=0.;
    y1=0.;
    theta_1=0.;
    robot_x=(int)x0;
    robot_y=(int)y0;
    x0=x0* 12.; /*change to inches*/
    y0=y0* 12.; /*change to inches*/

/* set ranging variables */
   deg_to_rad = 57.2957795131;
   A=40./deg_to_rad;

    do_course();

/* correct the heading and distance from the landmark using IR */

    correct_heading(1,0,18,1);  /*side to look (+is right),angle s/b,normal distance s/b */ 
    printf("At waypoint 1.\n");

/* set couse to next waypoint */
    goal_x=5;
    goal_y=31;

    do_course();

    correct_heading(1,0,12,1);
    printf("At first goal.\n");

    goal_x=3;
    goal_y=22;
    
   do_course();

    correct_heading(-1,0,18,0); 
    printf("At second waypoint.\n");

    goal_x=3;
    goal_y=9;

   do_course();

   printf("You miss me?\n");

}