/* LICENSE:
  =========================================================================
    CMPack'03 Source Code Release for OPEN-R SDK v1.0
    Copyright (C) 2003 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>

#include "../../../agent/headers/WaveLAN.h"

#include "PetProxyIn.h"

static const int NumClients = 1;

// Calvin 128.2.250.10
// gs18 128.2.203.26
// humuhumu 128.2.220.76
// gs104 128.2.205.116
char *clients[2]={"127.0.0.1","127.0.0.1"};
int   ports  [2]={           5046,           5048};

PetProxyIn::PetProxyIn(void)
{
  for(int i=0; i<NumClients; i++) {
    if(!connectToClient(i,clients[i],ports[i])) {
      // FIXME: We should do real error handling
      // (like ask for a new IP and try again) instead
      // of whining.
      printf("PetProxyIn::PetProxyIn - Error connecting to client #%d %s port %d.\n",
             i,clients[i],ports[i]);
    }
  }

  curBufLoc = buf;
}

/* Close our socket. */
PetProxyIn::~PetProxyIn()
{
  for(int i=0; i<NumClients; i++) {
    if(ClientSocket[i]!=-1)
      close(ClientSocket[i]);
  }

//  if(DatagramInputSocket!=-1)
//    close(DatagramInputSocket);
}

bool PetProxyIn::connectToClient(int client_num,char *client_addr, int client_port)
{
  bool retval = true;

  // Setup an sockaddr_in structure for our client address.
  ClientAddress[client_num].sin_family = AF_INET;
  ClientAddress[client_num].sin_port = htons(client_port);

  if(inet_aton(client_addr, &(ClientAddress[client_num].sin_addr))==0) {
    // FIXME:  we should do something other than whine
    printf("Call to inet_aton failed on \"%s\"\n", client_addr);
    retval = false;
  }
  
  memset(&(ClientAddress[client_num].sin_zero), '\0', 8);

  // Create a TCP stream socket.
  ClientSocket[client_num] = socket(AF_INET, SOCK_STREAM, 0);

  if(ClientSocket[client_num]==-1) {
    // FIXME - actually handle error at some point
    printf("Unable to create socket.\n");
    retval = false;
  }
  
  // Now connect to our client. (it seems weird for a server to
  // poke the client...)
  if(connect(ClientSocket[client_num], (sockaddr *)&ClientAddress[client_num],
	     sizeof(ClientAddress[client_num]))==-1) {
    // FIXME - handle error
    printf("PetProxyIn: Connect to client failed.\n");
    retval = false;
  }

  if(fcntl(ClientSocket[client_num],F_SETFL,O_NONBLOCK)==-1) {
    perror("couldn't make input socket non-blocking");
  }

  return retval;
}

typedef unsigned char uchar;
#include <ctype.h>
void dumpBinary(uchar *data, int length)
{
  int char_cnt = 0;
  bool end_row;
  uchar *asc_data;

  end_row=false;
  asc_data = data;
  while(length > 0) {
    putchar('x');
    unsigned int in=*data;
    unsigned int high=in>>4;
    unsigned int low=in & 0xF;
    putchar((high > 9) ? (high - 10 + 'a') : (high + '0'));
    putchar((low  > 9) ? (low  - 10 + 'a') : (low  + '0'));

    data++;
    length--;

    char_cnt++;
    end_row = end_row || (char_cnt == 30) || (length==0);
    if(end_row) {
      printf(" | ");

      while(asc_data!=data) {
        uchar ch_in=*asc_data;
        asc_data++;
        if(isprint(ch_in))
          putchar(ch_in);
        else
          putchar('.');
      }

      printf("\n");

      end_row = false;
      char_cnt  = 0;
    }
  }
}

/* Receives up to buf_size bytes from the client
   and places them in buf. Returns the actual
   number of bytes received.
*/
int PetProxyIn::recvData(int client_num,unsigned char *buf, int buf_size)
{
  int length = 0;

  if(ClientSocket[client_num]!=-1) {
    length = recv(ClientSocket[client_num], buf, buf_size, 0);
    
    if(length<0 && !(errno==EINTR || errno==EAGAIN)) {
      // FIXME: Should we do something here?
        printf("Error receiving data\n");
        length = 0;
        close(ClientSocket[client_num]);
        ClientSocket[client_num]=-1;
    }
  } else {
    printf("Trying to receive without connection from client %d.\n",client_num);
  }

  return length;
} 

OStatus
PetProxyIn::DoInit(const OSystemEvent& event)
{
  //
  //  New OSubject & OObserver
  //
  NewComData; 
  //
  //  Register services
  //
  SetComData;
    
  return oSUCCESS;
}

OStatus
PetProxyIn::DoStart(const OSystemEvent& event)
{
  //
  //  Start
  //
  for (int i=0; i < numOfObserver; i++) {
    if ( observer[i]->AssertReady() != oSUCCESS ){
      cout << "PetProxyIn::DoStart()" << endl;
      cout << "\tPetProxyIn AssertReady() failed for observer[" << i << "]" << endl;
    }
  }

  processInput();

  return oSUCCESS;
}    


OStatus
PetProxyIn::DoStop(const OSystemEvent& event)
{
  //
  //  Stop
  //
  for (int i=0; i<numOfObserver; i++){
    observer[i]->DeassertReady();
  }

  return oSUCCESS;
}


OStatus
PetProxyIn::DoDestroy(const OSystemEvent& event)
{
  //
  //  Delete OSubject & OObserver
  //
  DeleteComData;

  return oSUCCESS;
}

void
PetProxyIn::ReadyData(const OReadyEvent &event) {
  processInput();
}

void
PetProxyIn::processInput() {
  bool data_available,sent_data;
  int length_read;

  sent_data=false;
  while(!sent_data) {
    bool all_invalid=true;
    for(int i=0; i<NumClients; i++) {
      if(ClientSocket[i]!=-1)
        all_invalid=false;
    }
    if(all_invalid)
      return;
    
    for(int i=0; i<NumClients; i++) {
      while((length_read = recvData(i,curBufLoc,BufSize-(curBufLoc-buf))) > 0) {
        //printf("read %d bytes\n",length_read);
        curBufLoc += length_read;
      }
    }
    data_available = ((curBufLoc-buf) == BufSize);
    
    uchar *start_loc;
    start_loc = (uchar *)memchr(buf,'\x80',curBufLoc-buf);
    if(start_loc!=NULL && curBufLoc-start_loc >= sizeof(NetMsgHeader)) {
      int delete_length;
      delete_length=start_loc+1-buf;
      memmove(buf,&buf[delete_length],BufSize-delete_length);
      curBufLoc -= delete_length;
      NetMsgHeader *header=(NetMsgHeader *)buf;
      ulong length=header->totalLength;
      if(length <= MaxRouteMessageLength) {
        // note: this is lossy if observers have not set buf cntl
        printf("sending %d bytes of data on to PetProxy, ready=%d\n",
               length+sizeof(NetMsgHeader),
               subject[sbjDataForward]->IsReady());
        subject[sbjDataForward]->SetData(buf,length);
        subject[sbjDataForward]->NotifyObservers();
        sent_data=true;
      }
      delete_length=min(length,(ulong)(curBufLoc-buf));
      memmove(buf,&buf[delete_length],BufSize-delete_length);
      curBufLoc -= delete_length;
    } else {
      // drop partial packets
      if(curBufLoc != buf) {
        printf("dropping %d bytes of partial data!!!\n",curBufLoc-buf);
      }
      curBufLoc = buf;
    }

    if(!data_available)
      usleep(10000);
  }
}