#include <unistd.h>
#include <stdio.h>
#include "Nclient.h"
#include "mapcode.h"

/* following code taken and modifed from ncs.unix.c */

void InitSonar()
{
    int  i;
    int    order[16];

    /* set sonar fire order */
     order[0] =  0;
     order[1] =  8;
     order[2] =  2;
     order[3] = 10;
     order[4] =  4;
     order[5] = 12;
     order[6] =  6;        /*  I decided this should provide less
     order[7] = 14;           chance of adjacent sensors interfering
     order[8] =  9;           with each other than a circular pattern
     order[9] =  1;                         - M                       */
    order[10] = 11;
    order[11] =  3;
    order[12] = 13;
    order[13] =  5;
    order[14] = 15;
    order[15] =  7;

    conf_sn(2,order);

    for (i = 17; i <= 32; i++)
	Smask[i] = 1;            /* Mask 17 - 32 == 0 - 15 sonar */

    ct();                       /* sends Mask to robot */
}

void InitRobot()
{
    int index;

    if (server_is_running() == 1)
	printf("The server is running\n");
    else{
	printf("The server is not running\n");
	exit(0);
	}
    connect_robot(1);           /* must do */
    sleep(1);
    zr();                       /* reset all motors and sensors */
    dp(0,0);                    /* reset internal displacement */
    ac(100,450,450);            /* set accelerations */
    conf_tm(3);			/* set time out */
				/* set return info flags */
    for (index = 34; index <= 43; index ++)
	Smask[index] = 1;
    Smask[42] = 0;		/* not avaible */
    Smask[33] = 0;		/* not avaible */


    InitSonar();		/* set-up sonar sensors */
    sleep(1);
    ct();
    gs();                       /* update State vector */

}


main()
{
  int i,j;

printf("Init.\n");
  InitRobot();

printf("Press enter to begin.\n");
getchar();

dp(0,0);

  /* First Sequence
      Rotate in place for 5 seconds.
      Map area every second.
      Reverse rotation and repeat.
  */

  map_area();

  printf("Sequence 1\n");
  vm(0, 0, 320);
  for(i = 0; i < 5; i++)
  {
    sleep(1);
    vm(0, 0, 320);
    get_sonar_data();
    vm(0, 0, 320);
    mapper();
    vm(0, 0, 320);
  }
  st();
  ws(1, 1, 1, 5);
  vm(0, 0, -320);
  for(i = 0; i < 5; i++)
  {
    sleep(1);
    vm(0, 0, -320);
    get_sonar_data();
    vm(0, 0, -320);
    mapper();
    vm(0, 0, -320);
  }
  st();
  ws(1, 1, 1, 5);
  print_map();


  /* Second Sequence
      Roll forward for 60 seconds.
      Map area every second.
  */

  printf("Sequence 2\n");
  map_area();
  vm(60, 0, 0);
  for(j = 0; j < 12; i++)
  {
    for(i = 0; i < 5; i++)
    {
      sleep(1);
      vm(60, 0, 0);
      get_sonar_data();
      vm(60, 0, 0);
      mapper();
      vm(60, 0, 0);
    }
    print_map();
  vm(0, 0, 320);
  for(i = 0; i < 5; i++)
  {
    sleep(1);
    vm(0, 0, 320);
    get_sonar_data();
    vm(0, 0, 320);
    mapper();
    vm(0, 0, 320);
  }
  st();
  ws(1, 1, 1, 5);
  vm(0, 0, -320);
  for(i = 0; i < 5; i++)
  {
    sleep(1);
    vm(0, 0, -320);
    get_sonar_data();
    vm(0, 0, -320);
    mapper();
    vm(0, 0, -320);
  }
  st();
  ws(1, 1, 1, 5);
  print_map();
  }
  st();
  ws(1, 1, 1, 5);
  print_map();

  /* Third sequence
      Roll forward for 12 seconds, panning turret left and right.
  */
printf("Sequence 3\n");
  map_area();
  vm(60, 0, 100);
  sleep(1);
  get_sonar_data();
  mapper();
  vm(60, 0, 100);
  sleep(1);
  get_sonar_data();
  mapper();
  vm(60, 0, 100);
  sleep(1);
  get_sonar_data();
  mapper();
  print_map();
  vm(60, 0, -100);
  sleep(1);
  get_sonar_data();
  mapper();
  vm(60, 0, -100);
  sleep(1);
  get_sonar_data();
  mapper();
  vm(60, 0, -100);
  sleep(1);
  get_sonar_data();
  mapper();
  vm(60, 0, -100);
  sleep(1);
  get_sonar_data();
  mapper();
  vm(60, 0, -100);
  sleep(1);
  get_sonar_data();
  mapper();
  vm(60, 0, -100);
  sleep(1);
  get_sonar_data();
  mapper();
  print_map();
  vm(60, 0, 100);
  sleep(1);
  get_sonar_data();
  mapper();
  vm(60, 0, 100);
  sleep(1);
  get_sonar_data();
  mapper();
  vm(60, 0, 100);
  sleep(1);
  get_sonar_data();
  mapper();
  st();
  ws(1, 1, 1, 5);
  print_map();

  printf("End test sequence.\n");
  disconnect_robot(1);
}