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

#define ROBOT_ID 1
#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); 
}

int get_leftside_max_dist(void)
{
  int max_value = 0;

  max_value = State[STATE_SONAR_4];
  return(max_value);
}

int get_rightside_max_dist(void)
{
  int max_value = 0;

  max_value = State[STATE_SONAR_12];
  return(max_value);
}

int check_front_sonar_dist(void)
{
  int counter = 0, i;

  if (State[STATE_SONAR_1] > 50)
      counter++;
  
  if (State[STATE_SONAR_15] > 50)
      counter++;

  return(counter);
}

void steer(void)
{
  int j;

  if (State[STATE_SONAR_1] >= State[STATE_SONAR_15])
  {
    for (j = 0; j < 40; j++)
      vm(40,10,0);
    for (j = 0; j<60; j++)
      vm(50,50,0);
    for (j = 0; j < 40; j++)
      vm(10,40,0);
  }
  else
  {
    for (j = 0; j < 40; j++)
      vm(10,40,0);
    for (j=0; j<60; j++)
      vm(50,50,0);
    for (j = 0; j < 40; j++)
      vm(40,10,0);
  }
}

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, finish_room = 4, direction = 1;
  //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);

  gs();                              //update state vector (sonars and bumpers)
  while(!found)
  {
    vm(40,40, 0);                   // move straight
    gs();

    //need to check side sensors for an opening
    if (side == 0)
      distance = get_leftside_max_dist();
    else
      distance = get_rightside_max_dist();

    if (distance >= MAXLENGTH)
    {
      if (prev_max < MAXLENGTH)
      {
    	count++;
        if (count == num_openings+1)
	{
	  found = 1;
	  break;
        }
      }
    }
    prev_max = distance;
    min_dist = State[STATE_SONAR_0];       //check front sensor
    if (min_dist <= 35)                    // close to a wall or obstacle
    {  
      // check other front sensors to determine if wall or obstacle
      // wall = all sensors should be pretty close in distance
      // obstacle = one or more of the sensors give a big reading
      
      reading_big = check_front_sonar_dist();
      if (reading_big == 0)               // wall - need to turn around
        for (z = 0; z < 120; z++)
          vm(25,-25,0);
      else
      {
	//st();                //obstacle - need to steer in open direction
	//found = 1;
	// need to find the best angle to steer
	steer();
        st();
      }
    }
  }
  disconnect_robot(ROBOT_ID);
}