/*
 * TelepatiTcp ver 1.5
 *
 */

#include "TelepatiTcp.h"

#include <Packages.h>

#include <OPENR/OSyslog.h>
#include <OPENR/core_macro.h>
#include <OPENR/OPENRAPI.h>

#include <ant.h>
#include <EndpointTypes.h>
#include <TCPEndpointMsg.h>

#include <vector>
#include <fstream>
#include "entry.h"
#include <string.h>
#include <math.h>
#include <OPENR/OTime.h>

#include "EtherStat.h"
#include "AnalyzerAPI.h"

TelepatiTcp::TelepatiTcp(){
  numaibo=0;
  sendMessage=0;
  totalRecvMessage=0;
  dataRecvMessage=0;
  sendSpeed= -1;
  byteSended=0;
  byteReceived=0;
  
  ipstackRef = antStackRef("IPStack");  //creo il riferimento allo stack IP
}

TelepatiTcp::~TelepatiTcp(){
  delete [] aiboIpActive;
  delete [] sendConnection;
  delete [] recvConnection;
  delete [] numOfReconnect;
  delete [] *startSendTime;
  delete myIp;
}

/*DoInit: inizializza TelepatiTcp; fa il parse degli indirizzi IP nel file di configurazione e inizializza le connessioni*/
OStatus TelepatiTcp::DoInit(const OSystemEvent& event){
  OSYSDEBUG((TLPT_NAME"::DoInit()\n"));

  NEW_ALL_SUBJECT_AND_OBSERVER;
  REGISTER_ALL_ENTRY;
  SET_ALL_READY_AND_NOTIFY_ENTRY;
  
  myIp=retrieveMyIp();
  subject[sbjReceiveMsg]->SetBufferSize(TELEPATI_MAX_MESSAGE_TO_OBSERVER);

  if(parseIpConfigFile() != oSUCCESS) return oFAIL;
  
  return oSUCCESS;
}


/*DoStart: fa partire TelepatiTcp*/
OStatus TelepatiTcp::DoStart(const OSystemEvent& event){
  OSYSDEBUG((TLPT_NAME"::DoStart()\n"));
  OStatus retVal=oSUCCESS;

 //metto in ascolto le connessione receive
  for(int i=0; i<numaibo; i++){
    if(listen(i) != oSUCCESS) 
      retVal=oFAIL;
  }

  //connetto le connessioni in spedizione
  for(int i=0; i<numaibo; i++){
    if(connect(i) != oSUCCESS) 
      retVal=oFAIL;
  }

  ENABLE_ALL_SUBJECT;
  ASSERT_READY_TO_ALL_OBSERVER;  

  int dummy=0; 
  char* etherStatStr="With EtherStat Disabled";

#ifdef OPENR_ETHERSTAT
  //parte il timer per la stampa statistiche rete
  EventID etherEid;  
  RelativeTime etherPeriod(5,0);
  TimeEventInfoWithRelativeTime etherTimeInfo(TimeEventInfo::Periodic, etherPeriod);
  sError etherError=SetTimeEvent(&etherTimeInfo,myOID_,Extra_Entry[entryEtherStat],&dummy,sizeof(dummy),&etherEid);
  if(etherError!=sSUCCESS) 
    OSYSLOG1((osyslogERROR, TLPT_NAME"::DoStart() : ERROR: EtherStatTimer not started\n"));  
  etherStatStr="With EtherStat Enabled";  
#endif  

  OSYSLOG1((osyslogINFO, TLPT_NAME" : INFO: started. Listening for %d Aibo from port %d\n %s\n %s\n", numaibo, TELEPATI_BASE_PORT, etherStatStr));

  return retVal;
}  //DoStart() END


/*DoStop: arresta TelepatiTcp*/
OStatus TelepatiTcp::DoStop(const OSystemEvent& event){
  OSYSDEBUG((TLPT_NAME"::DoStop()\n"));

  DISABLE_ALL_SUBJECT;
  DEASSERT_READY_TO_ALL_OBSERVER;

  for(int i=0; i< numaibo; i++)  //informa gli altri Aibo della chiusura
    sendByeMsg(i);    
  
  OStatus retVal=oSUCCESS;
  for(int i=0; i< numaibo; i++){  //chiudo le connessioni
    if(recvConnection[i].stop() != oSUCCESS) 
      retVal=oFAIL;
    if(sendConnection[i].stop() != oSUCCESS) 
      retVal=oFAIL;
  }
  
  return retVal;
}  //DoStop() END


/*DoDestroy: distrugge le strutture di comunicazione inter-object TelepatiTcp*/
OStatus TelepatiTcp::DoDestroy(const OSystemEvent& event){
  OSYSDEBUG((TLPT_NAME"::DoDestroy()\n"));
  
  DELETE_ALL_SUBJECT_AND_OBSERVER;
  
  return oSUCCESS;
}  //DoDestroy() END


/*checkQueue: controlla la presenza di messaggi in coda quindi in
  attesa di spedizione. Se presenti li spedisce*/
void TelepatiTcp::checkQueue(){
  OSYSDEBUG((TLPT_NAME"::checkQueue()\n"));
  
  if(!sendQueue.empty())
    {
      TelepatiPkg* msg=sendQueue.front();
      sendQueue.pop();      
      send(msg);
    }
  
  return;
}  //checkQueue() END


/* send(): ENTRY POINT (Obs) invocata al ricevimento di un messaggio da parte del relativo subject.*/
void TelepatiTcp::SendNotify(const ONotifyEvent& event){
  OSYSDEBUG((TLPT_NAME"::SendNotify()\n"));

  for(int i=0; i < event.NumOfData(); i++){
    TelepatiPkg* msg=TelepatiPkg::createPkg((byte*)event.Data(i));
    if(!msg) //se msg == 0 -> errore nella creazione del messaggio
      {
	OSYSLOG1((osyslogERROR, TLPT_NAME"::SendNotify() : ERROR: BAD message from subject\n"));
	break;
      }
    
    switch(msg->getType())
      {
      case INFO:
	sendAiboInfoMsg();
	break;
      case COUNTER:
	sendCounterMsg();
      case HI:  //se Hi non far nulla
      case HIREPLY:  //se HiReply non far nulla
      case ACK: //se ACK non far nulla (per compatibilita' con libPackages)
      case BYE: //se Bye non far nulla
	delete msg;
	break;
      
      default: 
	{
	  int a=msg->getAibo();
	  
	  if(a == TelepatiPkg::ADDR_BROADCAST)  //se e' un messaggio di broadcast
	    {
	      for(int j=0; j< numaibo; j++)
		{
		  if(aiboIpActive[j] == ACTIVE || aiboIpActive[j] == WAITINGREPLY)
		    {
		      TelepatiPkg* pkg=msg->clone();
		      pkg->setAibo(j);
		      send(pkg); 
		    }
		}
	      delete msg;
	    }
	  else if(a > numaibo ||  aiboIpActive[a] == BYED || aiboIpActive[a] == INACTIVE) //se l'aibo dest e' errato o inattivo fallisci
	    {
	      delete msg;
	      OSYSLOG1((osyslogERROR, TLPT_NAME"::SendNotify() : ERROR: invalid AIBO address or AIBO inactive, %d\n", a));
	    }
	  else  //altrimenti manda il messaggio
	    send(msg); 
	  
	} // default END
      }  //switch END
  }// for END
  
  observer[event.ObsIndex()]->AssertReady(); 
  
  return;
}  // SendNotify() END


/*send: Manda il package da spedire a SendCont per effettuare la spedizione*/
void TelepatiTcp::send(TelepatiPkg* msg){
  OSYSDEBUG((TLPT_NAME"::send() to Aibo %d (La coda ha %d messaggi)\n", msg->getAibo(), sendQueue.size()));

  // size_t freeMem;
//   AnalyzerGetSizeOfFreeMemory(&freeMem);
//   OSYSDEBUG(("MEMORIA LIBERA %d\n", freeMem));
  
  int aibo=msg->getAibo();   
  
  if(sendConnection[aibo].getState() != CONNECTION_WAITING)  //se la connessione non e' in attesa
    sendQueue.push(msg);  //allora accoda il messaggio
  else
    {
      if(msg->getType() == TIMEINFO)
	{
	  TimeInfoPkg* time=(TimeInfoPkg*)msg;
	  OTime currentTime;
	  OPENR::GetTime(&currentTime);
	  time->setTime(currentTime.GetClock());
	}

      byte* vec=msg->getMsg(); 
      size_t vecSize=msg->getSize();
      byte* ptr=sendConnection[aibo].getSendData();
      memcpy(ptr, vec, vecSize);      
      //memcpy(sendConnection[aibo].getSendData(), msg->getMsg(), vecSize);
      byteSended+=vecSize;

      TCPEndpointSendMsg sendMsg(sendConnection[aibo].getEndpoint(), sendConnection[aibo].getSendData(), vecSize);
      sendMsg.continuation = (void*)aibo;
      sendConnection[aibo].setState(CONNECTION_SENDING);
      
      sendMsg.Send(ipstackRef, myOID_, Extra_Entry[entrySendCont], sizeof(TCPEndpointSendMsg));  //mandalo ad ANT
      delete msg;
    }
  
  return;
}  //send() END


/* SendCont(): ENTRY POINT si occupa della spedizione dei messaggi. Lavora senza bloccare l'esecuzione dell'oggetto*/
void TelepatiTcp::SendCont(ANTENVMSG msg){
  TCPEndpointSendMsg* sendMsg = (TCPEndpointSendMsg*)msg;  
  
  int aibo = (int)sendMsg->continuation;
  OSYSDEBUG((TLPT_NAME"::SendCont(%d)\n", aibo));

  TCPEndpointError error=sendMsg->error;
  
  if (sendConnection[aibo].getState() == CONNECTION_CLOSED)
    return;
  
  if (error != TCP_SUCCESS)  //se c'e' un errore
    {
      OSYSLOG1((osyslogERROR, "%s : %s %d From Connection: %d", TLPT_NAME"::SendCont()", "FAILED. sendMsg->error", error, aibo));
      if(error == TCP_PROTOCOL_UNREACHABLE || error == TCP_HOST_UNREACHABLE || error == TCP_PORT_UNREACHABLE)
	{
	  setAiboIpActive(aibo, INACTIVE);  //imposto l'aibo come inattivo
	  sendConnection[aibo].stop();  //chiudo la connessione
	  sendConnection[aibo].initConnection();  //la reinizzializzo
	  connect(aibo);  //e la riconnetto
	}
    }
  else  //se non ci sono errori
    {
      sendMessage++;
      sendConnection[aibo].setState(CONNECTION_WAITING);  
      checkQueue();  //controlla se c'e' altro da spedire
    }
  
  return;
}  //SendCont() END


/* receive(): Avvia la ricezione sulla i-esima connessione.*/
void TelepatiTcp::receive(int i){
  
  TCPEndpointReceiveMsg receiveMsg(recvConnection[i].getEndpoint(), recvConnection[i].getRecvData(), 1, recvConnection[i].getRecvSize());
  receiveMsg.continuation = (void*)i;
  recvConnection[i].setState(CONNECTION_CONNECTED);
  receiveMsg.Send(ipstackRef, myOID_, Extra_Entry[entryReceiveCont], sizeof(receiveMsg));
  
  return;
}  //receive() END


/* ReceiveCont(): ENTRY POINT si occupa della ricezione dei messaggi. Lavora senza bloccare l'esecuzione dell'oggetto*/
void TelepatiTcp::ReceiveCont(ANTENVMSG msg){
  //faccio il cast del parametro msg
  TCPEndpointReceiveMsg* receiveMsg=(TCPEndpointReceiveMsg*)antEnvMsg::Receive(msg);
  
  // size_t freeMem;
//   AnalyzerGetSizeOfFreeMemory(&freeMem);
//   OSYSDEBUG(("MEMORIA LIBERA %d\n", freeMem));

  int aibo = (int)(receiveMsg->continuation);
  OSYSDEBUG((TLPT_NAME"::ReceiveCont(%d)\n", aibo));
  
  if (recvConnection[aibo].getState() == CONNECTION_CLOSED) return;  //se e' CLOSED i dati non sono validi
  
  recvConnection[aibo].setState(CONNECTION_RECEIVING);  
  
  if (receiveMsg->error != TCP_SUCCESS) //se c'e' un errore
    {
      OSYSLOG1((osyslogERROR, "%s : %s %d From Connection: %d", TLPT_NAME"::ReceiveCont()", "FAILED. receiveMsg->error", receiveMsg->error, aibo));
      if(receiveMsg->error == TCP_CONNECTION_CLOSED || receiveMsg->error == TCP_FAIL) 
	{
	  setAiboIpActive(aibo, INACTIVE);
	  recvConnection[aibo].stop(); //chiudo la connessione
	  recvConnection[aibo].initConnection();  //la reinizzializzo
	  listen(aibo);  //e lo rimetto in ascolto
	}
      else
	receive(aibo);  //riattivo la connessione
    }
  else //se non c'e' un errore
    {
      int i=0;  //iteratore
      int recvSize=(receiveMsg->sizeMin)+chunkSize[aibo];
      byte* msgBuffer=mergeWithChunk(receiveMsg, aibo);  //buffer contenente i dati ricevuti;
      OSYSDEBUG((TLPT_NAME"::receiveCont() ho ricevuto %d byte\n", recvSize));
      while( i < recvSize )
	{  //scorro il buffer 
	  int restSize=recvSize-i; //dati rimamenti da elaborare
	  
	  if( (restSize < TelepatiPkg::STD_HEADER_SIZE) || (TelepatiPkg::getSizeOfPkg(msgBuffer+i) > restSize))
	    { // se i byte rimanenti sono pochi o il pkg e' tranciato
	      saveInChunk(msgBuffer, i, restSize, aibo);
	      OSYSDEBUG((TLPT_NAME"::salvo nel chunck %d byte\n", restSize));
	      break;
	    }
	  else //altrimenti se i dati sono corretti proseguo
	    {
	      TelepatiPkg* recvPkg=TelepatiPkg::createPkg(msgBuffer+i);  //ricreo l'oggetto
	      if(!recvPkg)  //se recvPkg == 0 -> errore nella creazione del package
		{
		  OSYSLOG1((osyslogERROR, TLPT_NAME"::ReceiveCont() : ERROR: I've received a BAD message from Aibo %d\n", aibo));
		  return;
		}
	      
	      totalRecvMessage++;  //incremento il numero di msg ricevuti
	      i+=recvPkg->getSize();  //incremento l'iteratore del buffer
	      
	      recvPkg->setAibo(aibo);  //imposto l'indice corretto dell'Aibo nel package
	      
	      OSYSDEBUG((TLPT_NAME": Recieved msg type %d from Aibo %d\n", recvPkg->getType(), aibo));
	      switch(recvPkg->getType())
		{
		case HI:
		  OSYSDEBUG((TLPT_NAME"::ReceiveCont() ricevuto un HI dall'Aibo %d\n", aibo));
		  sendHiReplyMsg(aibo);  //rispondi al saluto
		  break;
		case HIREPLY:
		  OSYSDEBUG((TLPT_NAME"::ReceiveCont() ricevuto un HIREPLY dall'Aibo %d\n", aibo));
		  setAiboIpActive(aibo, ACTIVE);
		  break;
		case BYE:
		  setAiboIpActive(aibo, BYED);
		  recvConnection[aibo].stop(); //chiudo la connessione di ricezione dall'aibo
		  sendConnection[aibo].stop(); //chiudo la connessione di spedizione all'aibo
		  break;
		  
		case INFO:
		  OSYSPRINT(("TelepatiTcp::ReceiveCont()", "STRANGE. I've recived an AiboInfoPkg from remote !?\n"));
		  break;
		  
		case ACK:  //nel caso riceva un ACK scartalo
		  break;

		case TIMEINFO:
		  {
		    TimeInfoPkg* timePkg=(TimeInfoPkg*)recvPkg;
		    OTime time(timePkg->getTime(), otimeTIME_DIF_UNDEF);
		    OStatus error=OPENR::SetTime(time);
		    break;
		  }
		  
		default:
		  dataRecvMessage++;
		  byteReceived+=recvPkg->getSize();
		  sendToObserver(recvPkg);
		} //switch END
	      delete recvPkg;
	    }
	} //while END
      
      OSYSDEBUG((TLPT_NAME"::Messaggi ricevuti: Totali %d - Data %d\n",totalRecvMessage, dataRecvMessage));  
      if(msgBuffer != (receiveMsg->buffer)) //se il msgbuffer ha subito un merge
	{
	  OSYSDEBUG((TLPT_NAME"::per capire cancello msgBuffer\n"));
	  delete [] msgBuffer;
	}

      receive(aibo);  //riattivo la ricezione
    } // else se non c'e' errore END

  return;
}  //ReceiveCont() END

  
/*mergeWithChunk(): unisce il buffer dei dati ricevuti con i dati
  contenuti nel chunk, se presenti.*/
byte* TelepatiTcp::mergeWithChunk(const TCPEndpointReceiveMsg* msg, int connID){
  byte* ptr;

  if(chunkSize[connID] > 0)
    {
      ptr=new byte[(msg->sizeMin)+chunkSize[connID]];  //creo il buffer dei dati ricevuti
      memcpy(ptr, chunk[connID], chunkSize[connID]); //ci copio il chunk precedente
      memcpy(ptr+chunkSize[connID], msg->buffer, msg->sizeMin); //ci accodo i nuovi dati ricevuti
      
      delete [] chunk[connID];
      chunkSize[connID]=0;  //azzero la dimensione del chunk
    }
  else
    ptr=msg->buffer;  //buffer contenente i dati ricevuti  

  return ptr;
}


/*saveInChunk(): salva nel chunk i size byte contenuti in ptr a partire dall'offset offset.*/
void TelepatiTcp::saveInChunk(const byte* ptr, int offset, int size, int connID){
  chunk[connID]=new byte[size];
  chunkSize[connID]=size;
  memcpy(chunk[connID], ptr+offset, size); //copio in chunk i byte rimanenti
  return;
}
/*Invocata quando l'observer manda un AsserReady*/
void TelepatiTcp::ObserverReady(const OReadyEvent& event){
  OSYSDEBUG((TLPT_NAME"::ObserverReady()\n"));
  
  if(!interObjectQueue.empty()) //se la coda non e' vuota
    {
      TelepatiPkg* msg=interObjectQueue.front();  interObjectQueue.pop();      
      subject[sbjReceiveMsg]->SetData(msg->getMsg(), msg->getSize());  //lo mando all'observer
      subject[sbjReceiveMsg]->NotifyObservers();
      delete msg;
    }
  return;
} //ObserverReady() END


/*EtherStat(): EXTRA ENTRY POINT, invocata da Open-r allo scadere del timer fatto paritre allo Start, stampa statistiche sulla rete*/
void TelepatiTcp::EtherStat(void* msg){
#ifdef OPENR_ETHERSTAT
  EtherStat::printStat();
  OSYSPRINT(("Messaggi spediti: %d  -- Messaggi ricevuti: Totali %d - Data %d\nVelocita' di spedizione: %d byte/s\n",sendMessage, totalRecvMessage, dataRecvMessage, sendSpeed));

#endif  
  return;
}  //TimerEnd() END


/* parseIpConfigFile(): Imposta i valori di: numaibo, aiboIp e
   aiboIpActive e imposta le porte TCP delle connessioni. Scandisce il
   file di configurazione della rete di AIBO, memorizza gli indirizzi
   IP nel vettore aiboIp non registrando il proprio e imposta, in base
   al numero di aibo e alla posizione nel file di configurazione
   dell'i-esimo Aibo, la porta tcp dell'i-esima connessione di
   spedizione/ricezione.*/
OStatus TelepatiTcp::parseIpConfigFile(){
  OSYSDEBUG((TLPT_NAME"::parseIpConfigFile()\n"));
  int line;
  int myLine;
  vector<int> lines;
  bool myIpFound=false;

  //faccio il parsing del file di configurazione
  char ch[LINEBUFFERSIZE];
  ifstream fin(AIBONET);
  if( !fin )
    {
      OSYSLOG1((osyslogERROR, TLPT_NAME"::parseIpConfigFile() : ERROR opening IP config file: %s", AIBONET));
      numaibo=0;
      return oFAIL;
    }
  
  //scandisco le linee
  for(line=0; fin.getline(ch, LINEBUFFERSIZE); line++ )
    {
      size_t IPLenght=strlen(ch);
      if( IPLenght > 15 || IPLenght < 7 )  //se la riga non e' un IP
	continue;   //skip
      IPAddress tmp(ch);
      
      if(tmp != *myIp)  //se l'IP non e' il mio
	{
	  aiboIp.push_back(tmp); //salvalo nell'array
	  lines.push_back(line); //salva il numero di riga
	  OSYSDEBUG(("Added IP: %s\n", ch ));
	}
      else //se e' il mio IP
	{
	  myIpFound=true;
	  myLine=line;  //il mio numero di riga
	}
    }
  fin.close();
  //fine del parsing
  
  if(myIpFound) //se ho trovato il mio IP nel file di configurazione
    numaibo=aiboIp.size();
  else //se non l'ho trovato
    {
      numaibo=0; //non inizializzare le connessioni
      OSYSLOG1((osyslogERROR, TLPT_NAME"::parseIpConfig() : ERROR: my IP not found in %s, can't comunicate",AIBONET));
    }

  //inizializzo gli array 
  aiboIpActive=new aiboState[numaibo];
  numOfReconnect=new byte[numaibo];
  startSendTime=new SystemTime*[numaibo];
  chunk=new byte*[numaibo];
  chunkSize=new size_t[numaibo];
  
  for(int i=0; i<numaibo; i++){
    aiboIpActive[i]=INACTIVE;
    numOfReconnect[i]=0;
    chunk[i]=0;
    chunkSize[i]=0;
  }

  if(initConnections() != oSUCCESS) return oFAIL;  //inizializzo le connessioni  

  //Imposto le porte
  for(int i=0; i<numaibo; i++){
    recvConnection[i].setPort(TELEPATI_BASE_PORT+(lines[i]*line)+myLine);
    sendConnection[i].setPort(TELEPATI_BASE_PORT+(myLine*line)+lines[i]);
  }
     
  return oSUCCESS;
} //parseIpConfigFile() END


/*initConnections: inizializza le connessioni. Crea i buffer di
  spedizione e ricezione, crea gli endpoint TCP. Imposta i valori di:
  connections(compresi i valori in ogni struttura)*/
OStatus TelepatiTcp::initConnections(){
  OSYSDEBUG((TLPT_NAME"::initConnections() number of connection %d\n", numaibo*2));
  
  recvConnection=new TCPRecvConnection[numaibo]; //creo array connessioni in ricezione
  sendConnection=new TCPSendConnection[numaibo]; //creo array connessioni in spedizione

  OStatus retVal=oSUCCESS;
  
  for(int i=0; i<numaibo; i++){
    recvConnection[i]=TCPRecvConnection(ipstackRef, TELEPATI_BUFFER_SIZE);
    if(recvConnection[i].initConnection() != oSUCCESS)
      retVal=oFAIL;

    sendConnection[i]=TCPSendConnection(ipstackRef, TELEPATI_BUFFER_SIZE);
    if(sendConnection[i].initConnection() != oSUCCESS)
      retVal=oFAIL;
  }

  return retVal;
} //initConnections() END


/*Attiva la ricezione sulla i-esima connessione di ricezione in ascolo
  sulla porta recvPort*/
OStatus TelepatiTcp::listen(int i){
  OSYSDEBUG((TLPT_NAME"::listen(%d) on port %d stato %d\n ", i, recvConnection[i].getPort(), recvConnection[i].getState() ));
  if (recvConnection[i].getState() != CONNECTION_CLOSED) return oFAIL;

  TCPEndpointListenMsg listenMsg(recvConnection[i].getEndpoint(), IP_ADDR_ANY, recvConnection[i].getPort());
  listenMsg.continuation = (void*)i;

  recvConnection[i].setState(CONNECTION_LISTENING);    
  listenMsg.Send(ipstackRef, myOID_, Extra_Entry[entryListenCont], sizeof(listenMsg));
  
  return oSUCCESS;
} //listen() END


void TelepatiTcp::ListenCont(ANTENVMSG msg){
  TCPEndpointListenMsg* listenMsg=(TCPEndpointListenMsg*)antEnvMsg::Receive(msg);
  int index = (int)listenMsg->continuation;
  
  OSYSDEBUG((TLPT_NAME"::ListenCont(%d) on port %d\n", index, recvConnection[index].getPort()));
  
  if (listenMsg->error != TCP_SUCCESS) //se c'e' stato un errore
    {
      OSYSLOG1((osyslogERROR, "%s : %s %d", TLPT_NAME"::ListenRecvCont()", "FAILED. listenMsg->error", listenMsg->error));	
      recvConnection[index].stop(); //blocco la connessione
      recvConnection[index].initConnection();  //la reinizializzo
      if(listenMsg->error != TCP_FAIL)  //se non e' un Fail
	listen(index);  //ritento l'ascolto
    }
  else  //se non ci sono stati errori
    {
      recvConnection[index].setState(CONNECTION_CONNECTED);
      char* addr=new char[16];
      addr=aiboIp[index].GetAsString(addr);
      OSYSLOG1((osyslogINFO, TLPT_NAME" : INFO: is listening on port: %d to Aibo %d: IP: %s\n", recvConnection[index].getPort(), index, addr));
      delete [] addr;
      
      receive(index);  //attivo la ricezione
    }
} //ListenCont() END


/*Connette l'Aibo con l'i-esimo Aibo alla porta sendPort*/
OStatus TelepatiTcp::connect(int i){
  OSYSDEBUG((TLPT_NAME"::Connect(%d) to port %d\n",i, sendConnection[i].getPort()));
  
  if (sendConnection[i].getState() != CONNECTION_CLOSED) return oFAIL;
  
  TCPEndpointConnectMsg connectMsg(sendConnection[i].getEndpoint(), *myIp, IP_PORT_ANY, aiboIp[i], sendConnection[i].getPort());
  connectMsg.continuation = (void*)i;
  
  sendConnection[i].setState(CONNECTION_CONNECTING);
  connectMsg.Send(ipstackRef, myOID_, Extra_Entry[entryConnectCont], sizeof(connectMsg));
  
  return oSUCCESS;
} //connect() END


void TelepatiTcp::ConnectCont(ANTENVMSG msg){
  TCPEndpointConnectMsg* connectMsg= (TCPEndpointConnectMsg*)antEnvMsg::Receive(msg);
  int index = (int)connectMsg->continuation;
  
  OSYSDEBUG((TLPT_NAME"::ConnectCont(%d) to port %d\n", index, sendConnection[index].getPort()));
  if (connectMsg->error != TCP_SUCCESS)  //se c'e' stato un errore
    {
      OSYSLOG1((osyslogERROR, "%s : %s %d", TLPT_NAME"::ConnectCont()", "FAILED. connectMsg->error", connectMsg->error));
      if(numOfReconnect[index] < TELEPATI_MAX_CONNECT_ERROR)
	{
	  sendConnection[index].stop();  //chiudo la connessione
	  sendConnection[index].initConnection();  //la reinizializzo
	  
	  Wait(TELEPATI_RECONNECT_TIME);  //aspetta.. un attimo
	  numOfReconnect[index]++;	    
	  connect(index);
	}
    }
  else  //se non ci sono errori (mi sono connesso correttamente)
    {
      sendConnection[index].setState(CONNECTION_WAITING);
      char* addr=new char[16];
      addr=aiboIp[index].GetAsString(addr);
      OSYSLOG1((osyslogINFO, TLPT_NAME" : INFO: is connected to Aibo %d IP: %s to port: %d", index, addr, sendConnection[index].getPort()));
      delete [] addr;
      numOfReconnect[index]=0;  //il numero di errori viene riazzerato	    
      sendHiMsg(index); //saluto l'Aibo appena contattato
    }

    return;
} //ConnectCont() END


/* sendToObserver(): manda il messaggio all'observer se pronto a
   riceverlo, altrimenti lo accoda.*/
void TelepatiTcp::sendToObserver(TelepatiPkg* pkg){
  if((subject[sbjReceiveMsg]->IsAnyReady() == 0) || (subject[sbjReceiveMsg]->RemainBuffer() == 0))
    interObjectQueue.push(pkg->clone());
  else
    {
      subject[sbjReceiveMsg]->SetData(pkg->getMsg(), pkg->getSize());  //lo mando all'observer
      subject[sbjReceiveMsg]->NotifyObservers();
    }
  return;
}

/*sendAiboInfoMsg: manda un messaggio AiboInfo all'observer*/
void TelepatiTcp::sendAiboInfoMsg(){
  OSYSDEBUG((TLPT_NAME"::sendAiboInfoMsg()\n"));

  AiboInfoPkg* info=new AiboInfoPkg(aiboIpActive, numaibo);
  sendToObserver(info);
  delete info;
  return;
}  //sendAiboInfoMsg() END


void TelepatiTcp::sendCounterMsg(){
  OSYSDEBUG((TLPT_NAME"::sendCounterMsg()\n"));
  
  CounterPkg* pkg=new CounterPkg(dataRecvMessage, totalRecvMessage, sendMessage);
  sendToObserver(pkg);
  delete pkg;
  return;
} //sendCounterMsg() END


/*retrieveMyIp: restituisce il mio indirizzo IP.*/
IPAddress* TelepatiTcp::retrieveMyIp(){
  antEnvInitGetParamMsg getParamMsg("ETHER_IP");
  getParamMsg.Call(ipstackRef, sizeof(getParamMsg));

  if (getParamMsg.error == ANT_SUCCESS && getParamMsg.paramType == antEnv_InitParam_String)
    OSYSDEBUG(("My IP is': %s\n", getParamMsg.value.str));
  else
    {
      OSYSDEBUG(("Errore nel recupero del mio indirizzo IP\n"));
      return new  IPAddress("127.0.0.1");
    }

  return new IPAddress(getParamMsg.value.str); //il mio indirizzo IP
}  //retrieveMyIP() END

/*sendByeMsg: manda un messaggio di arrivederci all'i-esimo Aibo
  bloccando l'esecuzione fino al completamento della spedizione.*/
void TelepatiTcp::sendByeMsg(int i){
  OSYSDEBUG((TLPT_NAME"::sendByMsg(%d)\n", i));
  
  if (sendConnection[i].getState() != CONNECTION_CLOSED) 
    {
      TelepatiPkg* msg=new ByePkg(i);
      
      byte* vec=msg->getMsg(); 
      size_t vecSize=msg->getSize();
      byte* ptr=sendConnection[i].getSendData();
      memcpy(ptr, vec, vecSize);      
      //memcpy(sendConnection[i].getSendData(), msg->getMsg(), vecSize);

      TCPEndpointSendMsg sendMsg(sendConnection[i].getEndpoint(), sendConnection[i].getSendData(), vecSize);
      
      sendConnection[i].setState(CONNECTION_SENDING);
      sendMsg.Call(ipstackRef, sizeof(TCPEndpointSendMsg));  //mandalo ad ANT
      sendConnection[i].setState(CONNECTION_WAITING);
      delete msg;
    }
  
  return;
}  //sendByeMsg() END


/*setAiboIpActive: imposta aiboIpActive per l'Aibo aibo con il valore
  newVal. Se il nuovo valore imposta lo stato dell'Aibo da un Attivo
  ad un passivo allora manda un AiboInfoPkg all'observer.  Ritorna:
  true se il valore in aiboIpActive e' cambiato, false altrimenti*/
bool TelepatiTcp::setAiboIpActive(int aibo, aiboState newVal){
  aiboState statoCorrente=aiboIpActive[aibo];
  aiboIpActive[aibo]=newVal;
  if(statoCorrente == ACTIVE || statoCorrente == WAITINGREPLY)  //se Aibo e' attivo
    {
      if(newVal == INACTIVE || newVal == BYED)  //e diventa intattivo
	sendAiboInfoMsg();          //informo l'observer
    }
  else //se Aibo e' intattivo
    {
      if(newVal == ACTIVE || newVal == WAITINGREPLY)  //e diventa attivo
	sendAiboInfoMsg();          //informo l'observer
    }
  
  if(statoCorrente != newVal)  //se e' cambiato lo stato dell'Aibo
    return true;
  else
    return false;
  
}  //setAiboIpActive() END


void TelepatiTcp::computeSpeed(int aibo){
  SystemTime* endSendTime=new SystemTime();
  sError err=GetSystemTime(endSendTime);  //timestamp attuale
  if(err != sSUCCESS)
    {
      endSendTime= new SystemTime(); //tempo non valido
      endSendTime->MakeInvalid();
    }

  if(endSendTime->IsValid() && startSendTime[aibo]->IsValid())
    {
      OSYSDEBUG(("LA DIMENSIONE DEI DATI E'%d\n ", sendConnection[aibo].getSendSize()));
      SystemTime elapsed= (*endSendTime - *startSendTime[aibo]);  //tempo di consegna
      double time=elapsed.seconds+((double)elapsed.useconds/1000000); //tempo di consegna in secondi
      double speed=sendConnection[aibo].getSendSize()/time;  //velocita' byte/secondo
      sendConnection[aibo].setSendSize(0);
      
      if(sendSpeed == -1)
	sendSpeed=floor(speed);
      else
	sendSpeed=floor((double)(sendSpeed+speed)/2);
    }
  delete endSendTime;
  OSYSDEBUG((TLPT_NAME": Velocita' di spedizione %d B/s\n", sendSpeed));
}  //computeSpeed END
