/* LICENSE: */

#include <stdio.h>

#include "../headers/Util.h"
#include "../headers/SystemUtility.h"
#include "../headers/CircBufPacket.h"

#include "CommMgrObject.h"
#include "BareNet.h"

const bool debug_net_verbose = false;

void CommMgrObject::initNetwork() {
  ipstackRef = antStackRef("IPStack");
}


// takes index in all_peers and initializes the network
// connection. The create_bufs flag indicates whether or
// not we should allocate buffers - when Aperios chokes,
// we close and reopen sockets. There's no need to make
// buffers from scratch if we're just recreating the socket.
bool CommMgrObject::createUDPSender(int ap_index, bool create_bufs) {

  // If we don't already have buffers...
  if(create_bufs) {
    all_peers[ap_index].connection.sendAddress = 
      all_peers[ap_index].ip;
  
    // We went to their_ip:our_id
    all_peers[ap_index].connection.sendPort = 2000 + my_id;
    
    // They send to my_ip:their_id
    all_peers[ap_index].connection.recvPort = 2000 +
      all_peers[ap_index].id;
  
    // No errors [yet] *sigh*
    all_peers[ap_index].connection.recvErrSinceData = 0;
    all_peers[ap_index].connection.sendErrSinceData = 0;

    // ------------
    // We need a send buffer
    antEnvCreateSharedBufferMsg sendBufferMsg(buffer_size);
  
    sendBufferMsg.Call(ipstackRef, sizeof(sendBufferMsg));
    if (sendBufferMsg.error != ANT_SUCCESS) {
      pprintf(TextOutputStream, "createUDPSender: cannot create buffer\n");
      return false;
    }

    all_peers[ap_index].connection.sendBuffer = sendBufferMsg.buffer;
    all_peers[ap_index].connection.sendBuffer.Map();
    all_peers[ap_index].connection.sendData
      = (byte*)(all_peers[ap_index].connection.sendBuffer.GetAddress());
    all_peers[ap_index].connection.sendSize = buffer_size;

    // ------------
    // How about a receive buffer. thanks.
    antEnvCreateSharedBufferMsg recvBufferMsg(buffer_size);
    
    recvBufferMsg.Call(ipstackRef, sizeof(recvBufferMsg));
    if (recvBufferMsg.error != ANT_SUCCESS) {
      pprintf(TextOutputStream, "createUDPSender: cannot create buffer\n");
      return false;
    }

    all_peers[ap_index].connection.recvBuffer = recvBufferMsg.buffer;
    all_peers[ap_index].connection.recvBuffer.Map();
    all_peers[ap_index].connection.recvData
      = (byte*)(all_peers[ap_index].connection.recvBuffer.GetAddress());
    all_peers[ap_index].connection.recvSize = buffer_size;
  }
  
  // Alright, from here on out it doesn't matter if the
  // connection was closed or is new - we need to
  // make a new endpoint/bind it/etc

  // -------------
  // create the endpoint
  antEnvCreateEndpointMsg udpCreateMsg(EndpointType_UDP,
				       buffer_size * 2);
  udpCreateMsg.Call(ipstackRef, sizeof(udpCreateMsg));
  if (udpCreateMsg.error != ANT_SUCCESS) {
    pprintf(TextOutputStream, "couldn't create endpoint\n");
    return false;
  }
  all_peers[ap_index].connection.endpoint = udpCreateMsg.moduleRef;
  
  // --------------
  // bind local address
  UDPEndpointBindMsg bindMsg(all_peers[ap_index].connection.endpoint, 
			     IP_ADDR_ANY,
			     all_peers[ap_index].connection.recvPort);
  bindMsg.Call(ipstackRef,sizeof(antEnvCreateEndpointMsg));
  if (UDP_SUCCESS != bindMsg.error) {
    pprintf(TextOutputStream, "Unable to bind socket\n");
    return false;
  }

  all_peers[ap_index].connection.state = CONNECTION_CONNECTED;

  receive(ap_index);
  
  return true;
}

bool CommMgrObject::send(int index, const void *void_data, int size) {
 #if 0
  // HACK
  static ulong start_time = 0;
  static int bytes_sent = 0;

  if(start_time==0) {
    bytes_sent = size;
    start_time = GetTime();
  } else if(GetTime() - start_time > 3000000) {
    
    FILE *outfile = fopen("/MS/netlog.txt", "at");
    if(outfile) {
      fprintf(outfile, "sent %d bytes in 10 seconds\n", bytes_sent);
      fclose(outfile);
    }

    bytes_sent = 0;
    start_time = 0;
  } else {
    bytes_sent += size;
  }

  // /HACK
#endif
  byte *data = (byte *)void_data;

  if(all_peers[index].connection.state != CONNECTION_CONNECTED)
    return false;

  memcpy(all_peers[index].connection.sendData, data, size);

  UDPEndpointSendMsg sendMsg(all_peers[index].connection.endpoint,
			     all_peers[index].connection.sendAddress,
			     all_peers[index].connection.sendPort,
			     all_peers[index].connection.sendData,
			     size);
  sendMsg.continuation = (void*)index;
  sendMsg.Send(ipstackRef, myOID_,
  	       Extra_Entry[entrySendCont], sizeof(UDPEndpointSendMsg));

  // Hmmm... Blocking case seems to hang the Aibo. Why would a
  // blocking *send* hang the robot? It's not like we're
  // waiting for anything. I heart Aperios.
  //  sendMsg.Call(ipstackRef, sizeof(sendMsg));
                 
  /* In the sample code they do not check the error code
     at this point in time. (with continuation based reporting)
  */
  
  // Forget this; if worst comes to worse, we'll get an error
  // message. Not really caring.
  //  all_peers[index].state = CONNECTION_SENDING;
  
  return true;
}

void
CommMgrObject::SendCont(ANTENVMSG msg)
{
  
  UDPEndpointSendMsg* sendMsg = (UDPEndpointSendMsg*)antEnvMsg::Receive(msg);
  int index = (int)(sendMsg->continuation);
  
  if (sendMsg->error != UDP_SUCCESS) {
    pprintf(TextOutputStream, "SendCont error %d for index %d (count=%d)\n",
	    sendMsg->error, index, all_peers[index].connection.sendErrSinceData);
    
    all_peers[index].connection.sendErrSinceData++;
    if(all_peers[index].connection.sendErrSinceData==10) {
      pprintf(TextOutputStream, "Hey, lots o' send errors\n");
      all_peers[index].connection.sendErrSinceData = 0;
    }
  }
  
  all_peers[index].connection.sendErrSinceData = 0;

  // It seems as if sometimes our receive continuation is
  // not called. Perhaps an aperios message is being dropped
  // between IPStack and CommMgrObject. Once every cycle of poking
  // our peers, let's just reset it.
  receive(index);
}

void
CommMgrObject::receive(int index)
{
  UDPEndpointReceiveMsg receiveMsg(all_peers[index].connection.endpoint,
				   all_peers[index].connection.recvData,
				   buffer_size - 1);
  receiveMsg.continuation = (void*)index;
    
  receiveMsg.Send(ipstackRef, myOID_,
		  Extra_Entry[entryReceiveCont], sizeof(receiveMsg));
}

void
CommMgrObject::ReceiveCont(ANTENVMSG msg)
{
  UDPEndpointReceiveMsg* receiveMsg
    = (UDPEndpointReceiveMsg*)antEnvMsg::Receive(msg);

  int index = (int)(receiveMsg->continuation);

  if (receiveMsg->error != UDP_SUCCESS) {
    // An error occured this time, but let's listen again
    // receive();
    
    all_peers[index].connection.recvErrSinceData++;
    
    // If we've received a long string of errors without
    // any data coming in over the connection, close the
    // socket and create a new one in its place.
    if(all_peers[index].connection.recvErrSinceData==10) {
      all_peers[index].connection.recvErrSinceData = 0;
      
      UDPEndpointCloseMsg closeMsg(all_peers[index].connection.endpoint);
      if(closeMsg.Call(ipstackRef, sizeof(closeMsg))!=ANT_SUCCESS)
	pprintf(TextOutputStream, "close failed. sigh.\n");
      
      // We re-create with our old data so we only need to
      // pass the correct index.
      createUDPSender(index, false);
    }
    
    return;
  }

  all_peers[index].connection.recvErrSinceData = 0; // yay! worked!

  // Date lives in receiveMsg->buffer and is of receiveMsg->size
  if(debug_net_verbose)
    pprintf(TextOutputStream, "received %d bytes\n", 
	    receiveMsg->size);

  // hey, do something with data
  receivedData(index, receiveMsg->buffer, receiveMsg->size);
  
  // Tell aperios to call this method again next time
  // data arrives
  receive(index);
}