#include <stdio.h>
#include <libplayerc/playerc.h>

int main(int argc, const char **argv)
{
  int i;
  playerc_client_t *client;
  playerc_position2d_t *position;
  playerc_laser_t *laser;
  playerc_blobfinder_t *blobfinder;

  double vI;
  int robotport;
  int proxy_handle;

  // lab2
  int state;
  int j;
  int found_object;
  int found_obstacle;

  state = 2;

  if (argc != 4)
  {
    printf("usage: client port_number proxy_handle speed_constant\n");
    printf("example: client 6665 0 1\n");
  }
  else
  {
    vI = (double)atoi(argv[3]);
    proxy_handle = (double)atoi(argv[2]);
    robotport = (double)atoi(argv[1]);
  }

  // Create a client object and connect to the server; the server must
  // be running on "localhost" at port 6665
  client = playerc_client_create(NULL, "localhost", robotport);
  if (playerc_client_connect(client) != 0)
  {
    fprintf(stderr, "error: %s\n", playerc_error_str());
    return -1;
  }

  // Create a position proxy (device id "position:0") and susbscribe
  // in read/write mode
  position = playerc_position2d_create(client, proxy_handle);
  if (playerc_position2d_subscribe(position, PLAYERC_OPEN_MODE) != 0)
  {
    fprintf(stderr, "error: %s\n", playerc_error_str());
    return -1;
  }

  // Create a laser proxy (device id "laser:0") and susbscribe
  // in read/write mode
  laser = playerc_laser_create(client, proxy_handle);
  if (playerc_laser_subscribe(laser, PLAYERC_OPEN_MODE) != 0)
  {
    fprintf(stderr, "error: %s\n", playerc_error_str());
    return -1;
  }

  // Create a blobfinder proxy (device id "blobfinder:0") and susbscribe
  // in read/write mode
  blobfinder = playerc_blobfinder_create(client, proxy_handle);
  if (playerc_blobfinder_subscribe(blobfinder, PLAYERC_OPEN_MODE) != 0)
  {
    fprintf(stderr, "error: %s\n", playerc_error_str());
    return -1;
  }


  // Enable the robots motors
  playerc_position2d_enable(position, 1);

  // Start the robot moving
  playerc_position2d_set_cmd_vel(position, vI * .25, 0, 0, 1); 


  // YOUR CONTROL CODE GOES HERE

  while (1 == 1)
  {
      // Read data from the server and display current robot position
      playerc_client_read(client);

      // see if an obstacle in near by
      found_obstacle = -1;
      for(j=0;j<laser->scan_count;j++) {
         if (laser->scan[j][0] < 1.0)
         found_obstacle = j;
      }
	
      // see if an object is in view
      found_object = -1;
      for(j=0;j<blobfinder->blobs_count;j++) {
         if (blobfinder->blobs[j].color == 0x000000FF)
         found_object = j;
      }

      if (found_obstacle > 0) {
         printf("avoiding obstacle\n");
         playerc_position2d_set_cmd_vel(position, 0, 0, vI * .25, 1); 
      }
      else if (found_object > 0) {
         printf("tracking blue object\n");
         playerc_position2d_set_cmd_vel(position, vI * .25, 0, vI * .25 * (( (((double)blobfinder->width/2.0)-(double)blobfinder->blobs[found_object].x)/((double)blobfinder->width/2.0))), 1); 
      }
      else {
            printf("wandering\n");
            playerc_position2d_set_cmd_vel(position, vI * .25, 0, 0, 1); 
      }
  }




  // Shutdown and tidy up
  // !!! need to unsubscribe and destroy laser and blobfinder
  playerc_blobfinder_unsubscribe(blobfinder);
  playerc_blobfinder_destroy(blobfinder);
  playerc_laser_unsubscribe(laser);
  playerc_laser_destroy(laser);
  playerc_position2d_unsubscribe(position);
  playerc_position2d_destroy(position);
  playerc_client_disconnect(client);
  playerc_client_destroy(client);

  return 0;

}