/*************************************************************
 * Name: commsExample.cc
 * Description: Illustrates how to use communicate.h for 
 *              communicating between robot processes.
 *
 * REFER TO FUNCTION HEADER COMMENTS FOR INTERPRETATION
 *
 * To run this code in 2 processes, give the following commands
 * in 2 separate windows:
 * 
 * In window #1:
 *   linux>./commsExample 1 6000 2 6001
 * 
 * In window #2:
 *   linux>./commsExample 2 6001 1 6001
 *
 * The first command runs this program (commsExample), setting
 * the current robot's ID to 1, using port 6000, and setting the 
 * robot friend's ID to 2, using port 6001.  The second command
 * does the opposite:  it sets the current robot's ID to 2, using
 * port 6001, and it sets the robot friend's ID to 1, using port
 * 6000.
**************************************************************/

#include <iostream>
#include <string>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <stdlib.h>
#include "communicate.h"

using namespace std;

/**********************************************************************
 * Utility to convert an integer base 10 to ascii (string)
 *********************************************************************/
void itoa(int n, char s[])
{ 
  short i, j, k;
  int c, sign;

  if((sign = n) < 0) n = -n;
  i = 0;
  do { 
    s[i++] = n % 10 + '0';
  } while((n/=10) > 0);

  if(sign < 0) s[i++] = '-';
  s[i] = '\0';
  k = i-1;
  for(j=0; j<k; j++) {
    c = s[j];
    s[j] = s[k];
    s[k] = c;
    k--;
  }
}

/**********************************************************************
 * Send a message.  Here, the message format is:
 
 T:  1 character message type
 S:  ID of sender (converted to characters)
 $:  special delimiter symbol separating fields
 R:  ID of recipient (converted to characters)
 !:  special delimiter symbol denoting end of message
 
 Here, the implemented  message types are:
   
   'F':  Target found
   'C':  Application is complete; stop the program.

 You can implement your own message format (such as passing along the
 x and y values of the target, which aren't included here yet.)
 *********************************************************************/ 
void send_cmd(int from_id, int to_id, int to_port, char *type)
{
  char *msg = NULL;
  char *from = NULL, *to = NULL;

  msg = (char *) malloc(50*sizeof(int));
  from = (char *) malloc(15*sizeof(int));
  to = (char *) malloc(15*sizeof(int));

  itoa(from_id, from);
  itoa(to_id, to);

  strcpy(msg, type);
  strcat(msg, from);
  strcat(msg, "$");
  strcat(msg, to);

  // HERE, ADD ADDITIONAL FIELDS AS NEEDED, SEPARATED BY "$", DEPENDING
  // ON YOUR APPLICATION.  (E.G., YOU'LL WANT TO ADD THE TARGET X and Y
  // POSITIONS TO YOUR MESSAGE)

  strcat(msg, "!");

  printf("msg: %s\n", msg);
  //  send_msg(rfd, msg, to_port);
  send_msg(rfd, msg);
}

/**********************************************************************
 * Receive commands from other robots
 *
 * This function always checks (1) to make sure that the received message 
 * is from our "friend", and (2) that this message is intended for me.  
 * If not, the message is ignored.
 *
 * The "meat" of the message processing here should mirror the message
 * format set up in the send_cmd function.
 *********************************************************************/
int recv_cmd(int my_id, int friend_id)
{
  char *token, *ptr;
  char msg[MAXSIZE];
  int nbytes = 0;
  int from, to;

  while (1) {
    nbytes = recv_msg(rfd, msg);
    if (nbytes == 0) return 0; 
    printf("recv_msg: %s\n", msg);

    token = strtok(msg, "!");
    while (token != NULL) {
      if (token[0] == 'F') { // Target found message
        ptr = strstr(token, "F");
	ptr++;
	from = atoi(ptr);

	// Ignore messages if they aren't from our friend
	if (from != friend_id) continue;   

	ptr = strstr(ptr, "$");
	ptr++;
	to = atoi(ptr);
   
        // Ignore messages if they aren't for me
	if (to != my_id) continue;
	printf("%d receives a target found message from %d\n", to, from);

	// HERE, YOU ADD YOUR OWN CODE TO DECIPHER THE X, Y POSITION OF THE TARGET.
        // CONSIDER USING ATOF FUNCTION


	break;
      } 
      else if (token[0] == 'C') { // Complete; stop the program
        ptr = strstr(token, "C");
	ptr++;
	from = atoi(ptr);

	// Ignore messages if they aren't from our friend
	if (from != friend_id) continue;
	ptr = strstr(ptr, "$");
	ptr++;
	to = atoi(ptr);

        // Ignore messages if they aren't for me
	if (to != my_id) continue;
	printf("%d gets a Complete message from %d\n", to, from);
	exit(0);
      }
      token = strtok(NULL, "!");
    }
  }
  return nbytes;
}

/**********************************************************************
 * helper function to filter input arguments 
 *********************************************************************/
void parse_comms_args(int argc, char **argv, 
		      int *my_id, int *my_port, int *friend_id, int *friend_port)
{
  if (argc < 5) {
    printf("USAGE:commsExample my_id my_port friend_id friend_port\n");
    exit(1);
  }
  *my_id = atoi(argv[1]);
  *my_port = atoi(argv[2]);
  *friend_id = atoi(argv[3]);
  *friend_port = atoi(argv[4]);
  

  // init communication
  rfd = open_socket(*my_port, *friend_port);
  bind_socket(rfd, *my_port);
  fcntl(rfd, F_SETFL, O_NONBLOCK);

  return;
}


/********** The main program **************/

int main(int argc, char **argv)
{ 
  int my_id, my_port, friend_id, friend_port;

  parse_comms_args(argc, argv, &my_id, &my_port, &friend_id, &friend_port);

  cout << "my_id = " << my_id << " my_port = " << my_port 
       << " friend_id = " << friend_id << " friend_port = " << friend_port << endl;

  while (1) {

    // YOU'LL NEED TO ADD CODE TO PROVIDE THE X, Y POSITIONS OF THE TARGET
    // IN THE MESSAGE YOU SEND
    send_cmd(my_id, friend_id, friend_port, "F");
    recv_cmd(my_id, friend_id);
    sleep(2);
  }
}