/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */
#ifndef INCLUDED_CommMgr_h
#define INCLUDED_CommMgr_h

#include <OPENR/OObject.h>
#include <OPENR/ObjcommTypes.h>
#include <OPENR/OSubject.h>
#include <OPENR/OObserver.h>
#include <map>
#include <set>
#include <vector>

#include "../headers/AperiosSharedMem.h"
#include "../headers/system_config.h"
#include "../headers/Dictionary.h"
#include "../headers/DogTypes.h"
#include "../headers/SharedMem.h"
#include "../headers/SPOutEncoder.h"
#include "BareNet.h"
#include "BoundedQueue.h"
#include "CommMgrInterface.h"

class Peer;
struct PeerStatus;
class CommMgrObject;

// number of offsets to store in each peer's queue.
static const int QUEUELEN = 5; 

// number of synch packets we keep history for.
static const int SEQTIMEHISTORYLEN = 30;

// Associates the sequence # of a sync packet we sent with our time at
// which we sent it.
struct SeqTime {
  ulong seq;
  ulong time;
};

struct Peer {

  Peer() {
    ip = IPAddress(0);
    for(int i=0; i<6; i++)
      mac[i] = 0;
    id = rank = goodWait = 0;
    last_recv = 0;
    initialTime = 0;
    initialLocalTime = 0;
    status = INITIAL;
    error = 0;
    offsetEstimate = 0;
    onewayLatency = 0;
    offsets = new BoundedQueue<unsigned long>(QUEUELEN);
  }
  
  // We get crashes without a copy constructor. Cool.
  Peer(const Peer &other) {
    ip = other.ip;
    for(int i=0; i<6; i++)
      mac[i] = other.mac[i];

    id = other.id;
    rank = other.rank;
    goodWait = other.goodWait;
    last_recv = other.last_recv;
    initialTime = other.initialTime;
    offsetEstimate = other.offsetEstimate;
    initialLocalTime = other.initialLocalTime;
    error = other.error;
    status = other.status;
    onewayLatency = other.onewayLatency;
    offsets = new BoundedQueue<unsigned long>(other.offsets);
    connection = other.connection;
  }

  ~Peer() {
    delete offsets;
    offsets = NULL;
  }

  enum Status { INITIAL, OFF, DISCONNECTED, POOR, GOOD };
  Status status;

  IPAddress ip; // from /MS/config/macaddr.cfg
  int mac[6];
  
  int id;   // integer ID used to identify a peer

  unsigned int rank;   // peer's index in the all_peers vector

  // estimate of this peer's offset, in microseconds
  long offsetEstimate; 

  // estimate of the one-way latency between ourselves and this
  // peer. (Equal to the roundtrip delay divided by 2.)
  ulong onewayLatency;

  // error in offset estimate from the last synch packet received
  long error; 

  // queue containing the last `queuelen' offset measurements
  BoundedQueue<unsigned long>* offsets;

  // time we last got a message from this peer
  ulong last_recv;

  // the time specified in the first synch packet we ever received
  // from this peer
  ulong initialTime;

  // our local time when we received our first synch packet from this
  // peer
  ulong initialLocalTime;

  // connection used to talk to this peer
  UDPConnection connection;

  // number of synch packets needed for this peer to go from "poor" to
  // "good" status
  int goodWait;
};

/*
  A utility class to automatically discover which robots are active
  and connect to them. This class also handles all the synchronization
  and network reliability classification. There are two ways to
  identify a robot - its ID and its rank. ID refers to the number
  pulled from the macaddr.cfg file. (e.g. ERS01 = 1, ERS02 = 2, etc)
  Rank refers to the index of a peer in our list of live
  connections. We need the concept of rank so we can individually
  address every one of our peers without knowing their ID in
  behaviors. (And of course we can use rank to figure out which IDs
  we're connected to if we care about ID in behaviors)
*/
class CommMgrObject : public OObject {

 public:
  
  // Threshold (in microseconds) used to determine the difference
  // between "poor" and "good" status.
  static const int SIGMA = 33;

  // Number of consecutive synch packets we need "good" readings on,
  // in order to go from "poor" status to "good" status.
  static const int GOOD_HYSTERESIS = 10;

  // If we don't hear a message from a peer in DISCONNECT_TIME
  // seconds, that peer's status is set to "disconnected".
  static const int DISCONNECT_TIME = 10;

  // maximum number of bots in the macaddr.cfg file
  static const int MAX_BOTS = 25;

 private:

  int my_id;  // our robot id
  int my_rank;   // where are we in all_peers

  // keeps track of the times that we sent the last SEQTIMEHISTORYLEN
  // sync packets
  SeqTime *send_times;

  // the sequence number that we will stamp our next synch packet
  // with.
  int next_seq_num;

  // Vector containing all peers (dead or alive)
  std::vector<Peer> all_peers;

  // Maps robot IDs to indexes in all_peers
  static const int MAX_ID = (256);
  int idToRankMap[MAX_ID+1];
  
 public:

  CommMgrObject();
  ~CommMgrObject();
  
  virtual OStatus DoInit   (const OSystemEvent& event);
  virtual OStatus DoStart  (const OSystemEvent& event);
  virtual OStatus DoStop   (const OSystemEvent& event);
  virtual OStatus DoDestroy(const OSystemEvent& event);

  OSubject*  subject[numOfSubject];
  OObserver* observer[numOfObserver];

  // Aperios communication stuff

  // We don't handle ready messages, so we only have a single,
  // generic handler to ignore them.
  void ReadyGeneric(const OReadyEvent &event);
  void BCastMessage(const ONotifyEvent &event);
  void SendMessage(const ONotifyEvent &event);

  // Sends a synch packet to every connected host, and randomly to
  // disconnected hosts (to check whether they've recently become
  // alive). This function should really be private, but is public
  // because the Aperios TimeEvent stuff can't call private functions.
  void SyncTimer(void* unused);

  // Our current classification of the overall status of the network.
  CommMgr::NetStatus getNetStatus();

  // Our current classification of our connection to the robot with
  // the given ID.
  CommMgr::NetStatus getPeerStatus(int id);

 private:
  // timestamp conversion functions
  unsigned long convert(Peer* peer, unsigned long t, bool addoffset);
  unsigned long convertto(Peer* peer, unsigned long t);
  unsigned long convertfrom(Peer* peer, unsigned long t);

  // Starts up our periodic timer to send synch packets and do other
  // periodic calculations
  void startTimer();

  // Loads information from dictionary file and populates all_peers
  void loadDictionary();

  // Loads dictionary, then finds our ID and rank
  void init();

  // display the MAC address and ID of this robot
  void dumpInfo();

  // Our current classification of our connection to the given peer.
  CommMgr::NetStatus getPeerStatus(Peer *p);

  // Uses idToRankMap to grab a Peer* out of all_peers
  Peer* getPeerByID(int id);

  // For TextOutputStream
  SMMSharedMemRegion outMemRgn;

  // Sends the given message to the given peer.
  //  void sendToPeer(Peer *p, const char* msg);
  // Sends the given message to the robot with the given ID.
  //void sendToPeer(int id, const char* msg);

  // Sends the given message to every peer that we currently believe
  // is "alive".
  //void sendToLivePeers(const char *msg);


  // Sends a single synch packet to the given peer
  void sendSynchPacket(int ap_index, const char* msg);

  // we have received data from a peer
  void receivedData(int ap_index, byte *data, int bytes);

  // ----------------------------------------------------------
  // Network stuff
  // Callbacks so that IPStack can poke use after we
  // send messages to it

  void initNetwork();

 public:
  void SendCont   (ANTENVMSG msg);
  void ReceiveCont(ANTENVMSG msg);

 private:
  
  /* Returns an integer index that can be used to
     send to the specified address in the future.
     Returns a negative value on failure.
  */
  bool createUDPSender(int ap_index, bool create_bufs);
  bool send(int index, const void *data, int size);
  void receive(int index = -1);

 private:
  antStackRef    ipstackRef;

};

struct PeerStatus {
  int peerID;
  CommMgr::NetStatus status;
};

struct PeerMsgHeader {
  int recipientID;
};

#endif