#include <stdlib.h>
#include <stdio.h>
#include "Nclient.h"
#define ROBOT_ID 10
#define MAXLENGTH 65

void turn_front_sonar_on(void)
{
  int sn_order[16] = {0, 2, 15, 1, 14, 3, 13, 4, 12, 5, 11, 6, 10, 7, 9, 8};  

  /* turn on 5 front sonars and set them to fire in certain order */
  conf_sn(15, sn_order);
  conf_tm(2); 
}
void turn_right()
{
 int x;
 for(x=0;x<=14;x++)
   vm(-25,25,0);
}

void turn_left()
{
 int x;
 for(x=0;x<=13;x++)
   vm(25,-25,0);
}
 
void go_straight()
{
 int x;
 for(x=0;x<=60;x++)
   vm(40,40,0);
}

void turn_robot(int direction)
{
 int x;
 switch(direction)
   {
   case 1:{
           turn_left(); 
            break; //north
          }
   case 2:{
           turn_left();
           vm(40,40,0);
           turn_left();
           break;
           }
   case 3:{ //south
          turn_right();
           break;
           }
   case 4:{ //west
           break;
          }
   }//end of switch
}
void follow_left_wall()
     //follow left wall until a blank space
{
 int found = 0,left,front,x;
 while (!found)
   {
    vm(20,20,0);
    gs();
    left = State[STATE_SONAR_4];
    front = State[STATE_SONAR_0];
    if (left = front) //at a corner
      turn_right();
    if (left<=15)
     {
       gs();
       left = State[STATE_SONAR_4];
       if (left<=10)
	 {
          for(x=0;x<=3;x++)
	    vm(-20,20,0);
	 }
       if (left >= 20)
        {
         for(x=0;x<=3;x++)
	   vm(20,-20,0);
        }
     }
    if (left>=25)
      found = 1; //exit loop at a door way
   }
    //turn_left(); //go into the door way
    //go_straight();
}
void main(void)
{
  int i, min_dist,  angle, found = 0, z;
  int side=1, distance, reading_big = 0;           //1 = right, 0 left
  int count = 0, num_openings = 2, prev_max = 0;   //nu
  int section = 1, f_room = 4, direction = 2;
  int f_section = 2, room = 3;
  int left,right,front;
  //1= north, 2 = east, 3 = south, 4 = west


  connect_robot (1,MODEL_SCOUT2,"/dev/ttyS0",38400);

  turn_front_sonar_on();            //fires the sonar in a specific order

 /* initialize robot */
  conf_tm(2);
  zr();
  ac(50, 100, 100);
  sp(100, 200, 200);
  turn_robot(direction);
  gs();                           //update state vector (sonars and bumpers)
  while(!found)
  {
    gs();
    vm(40,40,0);
    left = State[STATE_SONAR_4];
    right = State[STATE_SONAR_12];
    front = State[STATE_SONAR_0];
    if (front <=20)
      {
	turn_right();
	follow_left_wall();
      }  
  }
  disconnect_robot(ROBOT_ID);
}