/* 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 <fcntl.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>

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

#include "PetProxy.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]={           2048,           5047};

/* This ignores the SIGPIPE signal.  This is usually a good idea, since
   the default behaviour is to terminate the application.  SIGPIPE is
   sent when you try to write to an unconnected socket.  You should
   check your return codes to make sure you catch this error! */
void ignore_pipe(void)
{
  struct sigaction sig;

  sig.sa_handler = SIG_IGN;
  sig.sa_flags = 0;
  sigemptyset(&sig.sa_mask);
  sigaction(SIGPIPE,&sig,NULL);
}

PetProxy::PetProxy(void)
{
  ignore_pipe();

  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("PetProxy::PetProxy - Error connecting to client #%d %s port %d.\n",
             i,clients[i],ports[i]);
    }
  }
}

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

bool PetProxy::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("Connect to client failed.\n");
    retval = false;
  }

  if(fcntl(ClientSocket[client_num],F_SETFL,O_NONBLOCK)==-1) {
    perror("Problem making socket non-blocking");
    retval = false;
  }

  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;
    }
  }
}

/* Send the len bytes in buf over the connection */
void PetProxy::sendData(unsigned char *buf, int len)
{
  //dumpBinary(buf,len);

  for(int i=0; i<NumClients; i++) {
    uchar *cur_buf;
    int cur_len;

    cur_buf=buf;
    cur_len=len;
    printf("sending %d bytes of data to client %d\n",cur_len,i);
    if(ClientSocket[i]!=-1) {
      int bytes_sent;
      while(cur_len > 0) {
        bytes_sent=0;
        bytes_sent=send(ClientSocket[i], cur_buf, cur_len, 0);
        if(bytes_sent <= 0)
          break;
        cur_len -= bytes_sent;
        cur_buf += bytes_sent;
      }
      if(cur_len > 0)
        printf("Send to client %d socket fd %d did not complete.\n",i,ClientSocket[i]);
      if(bytes_sent==-1) {
        perror("Error when sending was:");
      }
    } else {
      printf("Trying to send when not connected to client %d.\n",i);
    }
  }
}

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

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

  return oSUCCESS;
}    


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

  return oSUCCESS;
}


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

  return oSUCCESS;
}

void
PetProxy::doNetInput(uchar *data) {
  NetMsgHeader *header;
  ulong size;

  header = (NetMsgHeader *)data;
  
  size = header->totalLength;
  
  switch(header->receiverID) {
  case NetMsgHeader::Linux:
    {
      sendData(data, size);
    }
  break;
  case NetMsgHeader::ERS02:
    {
      switch(header->receiverSubsystem) {
      case NetMsgHeader::Sub_Main:
        {
          // note: this is lossy unless the receiver sets buf cntl
          subject[sbjNetOutputERS02Main]->SetData(data,size);
          subject[sbjNetOutputERS02Main]->NotifyObservers();
        }
        break;
      default:
        printf("unknown receiverSubsystem %d for receiverId %d, dropping message (%lu bytes)\n",
               header->receiverSubsystem,header->receiverID,size);
      }
    }
  break;

  case NetMsgHeader::BCast:
    switch(header->receiverSubsystem) 
      {
      case NetMsgHeader::Sub_Main:

	printf("Broadcast message from %d\n", header->senderID);
	
	// note: this is lossy unless the receiver sets buf cntl
	subject[sbjNetOutputERS01Main]->SetData(data, size);
	subject[sbjNetOutputERS01Main]->NotifyObservers();

	subject[sbjNetOutputERS02Main]->SetData(data, size);
	subject[sbjNetOutputERS02Main]->NotifyObservers();

	subject[sbjNetOutputERS03Main]->SetData(data, size);
	subject[sbjNetOutputERS03Main]->NotifyObservers();

	subject[sbjNetOutputERS04Main]->SetData(data, size);
	subject[sbjNetOutputERS04Main]->NotifyObservers();

	subject[sbjNetOutputERS05Main]->SetData(data, size);
	subject[sbjNetOutputERS05Main]->NotifyObservers();

	subject[sbjNetOutputERS06Main]->SetData(data, size);
	subject[sbjNetOutputERS06Main]->NotifyObservers();

	subject[sbjNetOutputERS07Main]->SetData(data, size);
	subject[sbjNetOutputERS07Main]->NotifyObservers();

	subject[sbjNetOutputERS08Main]->SetData(data, size);
	subject[sbjNetOutputERS08Main]->NotifyObservers();
	
        break;
	
      default:
        printf("unknown receiverSubsystem %d for receiverId %d, dropping message\n",
               header->receiverSubsystem,header->receiverID,size);
	break;
      }
    break;
    
  default:
    {
      printf("unknown receiverID %d, dropping message (%lu bytes)\n",
	     header->receiverID,size);
    }
  }
}

void
PetProxy::NetInput(const ONotifyEvent& event)
{
  uchar *data;

  for(int i=0; i<event.NumOfData(); i++) {
    data = (uchar *)event.Data(i);
    doNetInput(data);
  }

  //printf("assert ready for %d\n",event.ObsIndex());
  observer[event.ObsIndex()]->AssertReady();
}