void* comm_send_thread_start( void* data ) {
  thread_t* my_info = reinterpret_cast<thread_t*>(data);
  msg_t outgoing;
  int error = 0;

  // Send loop
  // Communication hasn't been stopped
  while ( my_info->state == T_STATE_ACTIVE ) {
    comm_send_q.wait_and_pop( outgoing );
    
    if ( outgoing.msg_type == MSG_T_TERMINATE ) {
      break;
    }
    
    contact_list_t::iterator contact = comm_contacts.find( outgoing.dest_ID );
    if ( contact == comm_contacts.end() ) {
      cout << "comm_send_thread_start Unknown destination ID " << outgoing.dest_ID << endl;
      continue;
    }
    
    comm_print_send_msg( outgoing );
    
    error = ::send( contact->second.sockfd, &outgoing, sizeof(outgoing), 0 );
    if ( error == -1 ) {
      cout << "comm_send_thread_start Couldn't send message:\n\t";
      comm_print_send_msg( outgoing );
    }
  }

  return NULL;
}
void comm_print_contacts() {
  gettimeofday(&(TIMER), NULL);
  cout << TIMER.tv_sec << "." << TIMER.tv_usec << " (" << comm_seq_num.num << "): " << "Contacts:" << endl;
  for ( contact_list_t::iterator it = comm_contacts.begin(); it != comm_contacts.end(); it++ ) {
    cout << "\t" << it->first << " @ " << it->second.address << " : " << it->second.sockfd << endl;
  }
}
void comm_handle_write( const msg_t& incoming ) {
  // Stringstream to hold contents to be written to file
  stringstream contents;

  contact_list_t::iterator contact = comm_contacts.find( incoming.send_ID );
  string hostname = ( contact == comm_contacts.end() ? "" : contact->second.address );
  
  contents << "<" << incoming.send_ID << ", " << incoming.ts << ", "
    << hostname << ">" << endl;

  // open up file of "replica_<nodeID>.txt"
  string output_path = "replica_" + int_to_str( NODE_INFO.id ) + ".txt";
  ofstream out_file( output_path, ios::out | ios::app );
  //ofstream out_file( output_path, ios::out );
  
  // write string and close file.
  if ( out_file.is_open() ) {
    out_file << contents.str();
    out_file.close();
  }
  
  else {
    cout << contents.str();
  }
}
int comm_initiate_contacts() {
  int error = 0;

  for ( contact_list_t::iterator node = comm_contacts.begin();
        node != comm_contacts.end();
        node++ ) {
    // cout << "attempting to open connection to" << endl;
    // print_contact( node->second );
    
    switch ( node->second.type ) {
      case SOCK_STREAM:
        error = comm_open_tcp_connection_until( node->second );
        break;
      
      case SOCK_DGRAM:
        error = comm_open_udp_connection_until( node->second );
        break;
      
      default:
        cout << "Unknown socket type for contact ";
        print_contact( node->second );
    }
    
    if ( error != 0 ) {
      cout << "comm_initiate_contacts Couldn't connect to contact";
      print_contact( node->second );
      break;
    }
  }

  return error;
}
int comm_open_tcp_connection_once( const node_contact_t& contact ) {
  int error = 0;
  contact_list_t::iterator node = comm_contacts.find( contact.id );

  if ( node == comm_contacts.end() ) {
    cout << "comm_open_tcp_connection_once No known id " << contact.id << endl;
    error = -1;
  }

  else if ( node->second.address.compare( contact.address ) != 0 ) {
    cout << "comm_open_tcp_connection_once Mismatched address for supplied contact and stored contact" << endl;
    error = -2;
  }

  else {
    struct addrinfo hints, *server_info, *p;
    string comm_port_str = int_to_str( comm_port );

    memset( &hints, 0, sizeof(hints) );
    hints.ai_family   = AF_UNSPEC;         // IPv4 or IPv6 don't care
    hints.ai_socktype = SOCK_STREAM;       // TCP

    // Get address info of remote address
    error = getaddrinfo( node->second.address.c_str(), comm_port_str.c_str(), &hints, &server_info );
    if ( error != 0 ) {
      ::freeaddrinfo( server_info );
      cout << "comm_open_tcp_connection_once Couldn't get info for remote address "
        << node->second.address << endl;
      return error;
    }

    // loop through all the results and connect to the first we can
    for( p = server_info; p != NULL; p = p->ai_next ) {
      node->second.sockfd = ::socket( p->ai_family, p->ai_socktype, p->ai_protocol );
      if ( node->second.sockfd == -1 ) {
        continue;
      }

      if ( ::connect( node->second.sockfd, p->ai_addr, p->ai_addrlen ) == -1 ) {
        continue;
      }

      break;
    }

    ::freeaddrinfo( server_info );

    if ( p == NULL ) {
      error = -1;
      cout << "comm_open_tcp_connection_once Couldn't connect to remote address "
        << node->second.address << endl;
      return error;
    }
  }

  return error;
}
void print_contact_list( const contact_list_t& contact_list ) {
  cout << "Contacts:" << endl;
  for ( contact_list_t::const_iterator it = contact_list.begin();
        it != contact_list.end();
        it++ ) {
    cout << it->first << ": ";
    print_contact( it->second );
  }
}
int comm_open_udp_connection_until( const node_contact_t& contact ) {
  int error = 0;
  contact_list_t::iterator node = comm_contacts.find( contact.id );

  if ( node == comm_contacts.end() ) {
    cout << "comm_open_udp_connection_until No known id " << contact.id << endl;
    error = -1;
  }

  else if ( node->second.address.compare( contact.address ) != 0 ) {
    cout << "comm_open_udp_connection_until Mismatched address for supplied contact and stored contact" << endl;
    error = -2;
  }

  else {
    struct addrinfo hints, *p;
    string comm_port_str = int_to_str( comm_port );

    memset( &hints, 0, sizeof(hints) );
    hints.ai_family   = AF_UNSPEC;         // IPv4 or IPv6 don't care
    hints.ai_socktype = SOCK_DGRAM;        // UDP 

    do {
      error = 0;

      // Get address info of remote address
      error = getaddrinfo( node->second.address.c_str(), comm_port_str.c_str(), &hints, &(node->second.server_info) );
      if ( error != 0 ) {
        cout << "comm_open_udp_connection_until Couldn't get info for remote address "
          << node->second.address << endl;
          wait_s_duration( 1 );
      }
    } while ( error != 0 );

    do {
      error = 0;

      // loop through all the results and connect to the first we can
      for( p = node->second.server_info; p != NULL; p = p->ai_next ) {
        node->second.sockfd = ::socket( p->ai_family, p->ai_socktype, p->ai_protocol );
        if ( node->second.sockfd != -1 ) {
          break;
        }
      }

      if ( p == NULL ) {
        error = -1;
        cout << "comm_open_udp_connection_until Couldn't connect to remote address "
          << node->second.address << endl;
        wait_s_duration( 1 );
      }
    } while ( error != 0 );
  }

  return error;
}
void* comm_send_thread_fun( void* data ) {
  msg_t outgoing;
  int error = 0;

  // Send loop
  // Communication hasn't been stopped
  while ( comm_send_thread.state == T_STATE_ACTIVE ) {
    comm_send_q.wait_and_pop( outgoing );

    if ( outgoing.header == MSG_T_HALT ) {
      break;
    }
    
    if ( !comm_up && (outgoing.header != MSG_T_UNREACHABLE) ) {
      cout << "comm_send_thread_fun Comms are down, message not sent" << endl;
      continue;
    }

    // Check to make sure outgoing destination is a known location and
    // a socket is open for sending
    contact_list_t::iterator contact = comm_contacts.find( outgoing.dest_id );
    if ( contact == comm_contacts.end() ) {
      cout << "comm_send_thread_fun Unknown destination ID " << outgoing.dest_id << endl;
      continue;
    }

    else if ( contact->second.sockfd < 0 ) {
      cout << "comm_send_thread_fun No open connection to destination ID " << outgoing.dest_id << endl;
      continue;
    }

    comm_print_send_msg( outgoing );

    if ( contact->second.type == SOCK_STREAM ) {
      error = ::send( contact->second.sockfd, &outgoing, sizeof(outgoing), 0 );
      if ( error == -1 ) {
        cout << "comm_send_thread_fun Couldn't send tcp message" << endl;
      }
    }
    
    else if ( contact->second.type == SOCK_DGRAM ) {
      error = ::sendto( contact->second.sockfd, &outgoing, sizeof(outgoing), 0, 
        contact->second.server_info->ai_addr, contact->second.server_info->ai_addrlen );
      if ( error == -1 ) {
        cout << "comm_send_thread_fun Couldn't send udp message" << endl;
      }
    }
    
    else {
      cout << "comm_send_thread_fun Unknown socket type " << contact->second.type << endl;
    }
  }

  return NULL;
}
int comm_open_connections() {
  for ( contact_list_t::iterator node = comm_contacts.begin();
        node != comm_contacts.end();
        node++ ) {
    if ( node->first == NODE_INFO.id ) {
      continue;
    }
    
    node->second.sockfd = comm_open_connection_until( node->second.address );
  }
  
  return 0;
}
int comm_close_connection( unsigned int id ) {
  contact_list_t::iterator node = comm_contacts.find( id );
  
  if ( node == comm_contacts.end() ) {
    cout << "comm_close_connection No known id " << id << endl;
    return -1;
  }
  
  ::close( node->second.sockfd );
  node->second.sockfd = -1;

  return 0;
}
int comm_broadcast( msg_t msg ) {
  int error = 0;

  for ( contact_list_t::iterator it = comm_contacts.begin();
        it != comm_contacts.end();
        it++ ) {
    msg.dest_id = it->first;
    error = comm_send( msg );
    if ( error < 0 ) {
      cout << "comm_broadcast Couldn't send to id " << msg.dest_id << endl;
    }
  }

  return error;
}
int comm_delete_contact( unsigned int id ) {
  int error = 0;

  comm_close_connection( comm_contacts[id] );
  
  error = comm_contacts.erase( id );

  return error;
}
int comm_stop() {
  comm_tcp_recv_threads.mutex.lock();

  // Mark all threads STOP
  comm_tcp_listen_thread.state = T_STATE_STOP;
  comm_udp_recv_thread.state = T_STATE_STOP;
  comm_recv_parse_thread.state = T_STATE_STOP;
  comm_send_thread.state = T_STATE_STOP;
  for ( list<recv_thread_t>::iterator it = comm_tcp_recv_threads.thread_list.begin();
        it != comm_tcp_recv_threads.thread_list.end();
        it++ ) {
    it->state = T_STATE_STOP;
  }

  // Close sockets threads are working with.
  // This gets the receiver threads and the listener thread off the blocking
  // accept() and recv() calls
  ::close( comm_tcp_listen_thread.sockfd );
  ::close( comm_udp_recv_thread.sockfd );
  for ( contact_list_t::iterator it = comm_contacts.begin();
        it != comm_contacts.end();
        it++ ) {
    comm_close_connection( it->second );
  }
  for ( list<recv_thread_t>::iterator it = comm_tcp_recv_threads.thread_list.begin();
        it != comm_tcp_recv_threads.thread_list.end();
        it++ ) {
    ::close( it->sockfd );
  }
  
  comm_tcp_recv_threads.mutex.unlock();

  // Push an execution STOP message into the send and recv queues
  // This gets the receiver parser thread and the sending thread off the blocked
  // wait_and_pop() calls
  // msg_t terminate = create_msg( MSG_T_FINAL, 0, 0, 0, "" );
  msg_t terminate;
  terminate.header = MSG_T_HALT;
  comm_recv_q.push( terminate );
  comm_send_q.push( terminate );

  return 0;
}
int comm_close_connection( const node_contact_t& contact ) {
  int error = 0;
  contact_list_t::iterator node = comm_contacts.find( contact.id );

  if ( node == comm_contacts.end() ) {
    cout << "comm_close_connection No known id " << contact.id << endl;
    error = -1;
  }

  else if ( node->second.address.compare( contact.address ) != 0 ) {
    cout << "comm_close_connection Mismatched address for supplied contact and stored contact" << endl;
    error = -2;
  }

  else {
    ::close( node->second.sockfd );
    node->second.sockfd = -1;
  }

  return error;
}
int comm_stop() {
  // Mark all threads STOP
  comm_listen_thread_id.state = T_STATE_STOP;
  comm_recv_parse_thread_id.state = T_STATE_STOP;
  comm_send_thread_id.state = T_STATE_STOP;
  for ( unsigned int i = 0; i < comm_recv_thread_ids.size(); i++ ) {
    comm_recv_thread_ids[i].state = T_STATE_STOP;
  }

  // Close sockets threads are working with.
  // This gets the receiver threads and the listener thread off the blocking
  // accept() and recv() calls
  ::close( comm_listen_thread_id.sockfd );
  for ( unsigned int i = 0; i < comm_recv_thread_ids.size(); i++ ) {
    ::close( comm_recv_thread_ids[i].sockfd );
  }
  for ( contact_list_t::iterator it = comm_contacts.begin();
        it != comm_contacts.end();
        it++ ) {
    comm_close_connection( it->first );     
  }

  // Push an execution STOP message into the send and recv queues
  // This gets the receiver parser thread and the sending thread off the blocked
  // wait_and_pop() calls
  msg_t terminate;
  terminate.msg_type = MSG_T_TERMINATE;
  comm_recv_q.push( terminate );
  comm_send_q.push( terminate );

  // Join all threads
  pthread_join( comm_listen_thread_id.id, NULL );
  pthread_join( comm_recv_parse_thread_id.id, NULL );
  pthread_join( comm_send_thread_id.id, NULL );
  for ( unsigned int i = 0; i < comm_recv_thread_ids.size(); i++ ) {
    pthread_join( comm_recv_thread_ids[i].id, NULL );
  }

  return 0;
}
int comm_delete_contact( unsigned int id ) {
  // Close connection
  comm_close_connection( id );
  
  // Remove from all groups except BASE_GROUP
  for ( group_map_t::iterator it = comm_groups.begin(); it != comm_groups.end(); it++ ) {
    comm_remove_contact( id, it->first );
  }
  
  // Remove from BASE_GROUP
  comm_groups[BASE_GROUP].cs.cs_map.erase( id );
  
  // Remove from contacts
  comm_contacts.erase( id );
  
  return 0;
}
// Communication server setup
int comm_start( const char* config_file_path ) {
  int error = 0;

  if ( config_file_path == NULL ) {
    cout << "comm_start Invalid config file path " << config_file_path << endl;
    error = -1;
    return error;
  }

  // Open configuration file, read only
  string line;
  ifstream config_file( config_file_path, ios::in );

  if ( config_file.is_open() ) {
    // Read comms port
    std::getline( config_file, line );
    comm_port = str_to_uint( line );

    // Read coordinator ID
    std::getline( config_file, line );
    NODE_NETWORK_MASTER = str_to_uint( line );

    // Contact to add after each read from file
    node_contact_t contact;

    // All new contacts have no open sockets, so -1
    contact.sockfd = -1;

    // No nodes in the network yet
    NODE_NETWORK_SIZE = 0;

    // Read <node ID, node address>
    while ( config_file.good() ) {
      // Read node ID
      std::getline( config_file, line );
      contact.id = str_to_uint( line );

      // Read node address
      std::getline( config_file, line );
      contact.address = line;

      // Use TCP to talk to master, use UDP elsewise
      contact.type = ( contact.id == NODE_NETWORK_MASTER ? SOCK_STREAM : SOCK_DGRAM );

      error = comm_add_contact( contact );
      if ( error != 0 ) {
        cout << "comm_start Couldn't add contact "
          << contact.id << "@" << contact.address << endl;
        return error;
      }

      NODE_NETWORK_SIZE++;
    }
    
    // print_contact_list( comm_contacts );

    // The subnet of udp servers is all of th eservers minus the one master
    NODE_SUBNET_SIZE = NODE_NETWORK_SIZE - 1;

    // Finished reading from file
    config_file.close();

    // Initialize this node's vn info
    comm_vns[NODE_INFO.id].vn = 0;
    comm_vns[NODE_INFO.id].ru = NODE_SUBNET_SIZE;
    strncpy( comm_vns[NODE_INFO.id].text, "", sizeof("") );

    comm_print_vns();
    
    // Initialize the distributed file
    comm_file.path = "replica_" + uint_to_str( NODE_INFO.id ) + ".txt";
    comm_file.delimiters = "|";

    comm_file.print();
    
    // Initialize the distinguished object
    comm_distinguished.m = 0;
    comm_distinguished.n = 0;

    // Initialize the reachability list to all UDP nodes
    for ( contact_list_t::iterator it = comm_contacts.begin();
          it != comm_contacts.end();
          it++ ) {
      if ( it->first != NODE_NETWORK_MASTER ) {
        comm_reachability_list.insert( it->first );
      }
    }
    
    comm_print_reachability_list();

    // all other attrs default to OK values

    // Spawn tcp listener thread
    error = pthread_create( &(comm_tcp_listen_thread.id),
                            NULL,
                            comm_tcp_listen_thread_fun,
                            NULL );
    if ( error != 0 ) {
      cout << "comm_start Couldn't create TCP listen thread" << endl;
      return error;
    }
    comm_tcp_listen_thread.state = T_STATE_ACTIVE;

    // Spawn UDP receiver thread
    error = pthread_create( &(comm_udp_recv_thread.id),
                            NULL, 
                            comm_udp_recv_thread_fun,
                            NULL );
    if ( error != 0 ) {
      cout << "comm_start Couldn't create UDP recveive thread" << endl;
      return error;
    }
    comm_udp_recv_thread.state = T_STATE_ACTIVE;                        
    
    // Spawn receive parser thread
    error = pthread_create( &(comm_recv_parse_thread.id),
                            NULL,
                            comm_recv_parse_thread_fun,
                            NULL );
    if ( error != 0 ) {
      cout << "comm_start Couldn't create receive parse thread" << endl;
      return error;
    }
    comm_recv_parse_thread.state = T_STATE_ACTIVE;

    // Open a connection to all remote addresses previously added using an
    // comm_add_contact() call
    error = comm_initiate_contacts();
    if ( error != 0 ) {
      cout << "comm_start Couldn't connect to all remote addresses" << endl;
      return error;
    }
    
    // print_contact_list( comm_contacts );    

    // Spawn sender thread
    error = pthread_create( &(comm_send_thread.id),
                            NULL,
                            comm_send_thread_fun,
                            NULL );
    if ( error != 0 ) {
      cout << "comm_start Couldn't create send thread" << endl;
      return error;
    }
    comm_send_thread.state = T_STATE_ACTIVE;

    error = comm_barrier();
    if ( error != 0 ) {
      cout << "comm_start Couldn't join the network" << endl;
    }
  }

  else {
    error = -1;
    cout << "comm_start Couldn't open config file" << endl;
  }

  cout << "Initial node configuration: " << endl;
  print_contact_list( comm_contacts ); 
  
  // msg_t test = create_msg( MSG_T_FINAL, 0, 0, 0, "" );
  msg_t test;
  test.header = MSG_T_FINAL;
  comm_broadcast( test );

  return error;
}