/*
 * TelepatiUdp ver 1.47
 *
 */

#include "TelepatiUdp.h"

#include <Packages.h>

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

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

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

#include "AnalyzerAPI.h"

TelepatiUdp::TelepatiUdp(){
  resending=0;
  numaibo=0;
  ipstackRef = antStackRef("IPStack");  //creo il riferimento allo stack IP

  // Inizializzo le variabili per le informazioni statistiche
  sendMessage=0;
  sendByte=0;
  totalRecvMessage=0;
  dataRecvMessage=0;
  dataRecvByte=0;
  numOfResending=0;
  numOfAckSended=0;
  startSendTime= new SystemTime();
  sendSpeed= -1;

  // Inizializzo le variabili per la rispedizione dei messaggi non ricevuti
  ackWaitingMsg=0;
  numOfTimerEnd=0;
  timerEndCont=0;
}


TelepatiUdp::~TelepatiUdp(){
  delete []aiboIpActive;
  delete []sendConnection;
  delete startSendTime;
}

/*DoInit: inizializza Telepati*/ 
OStatus TelepatiUdp::DoInit(const OSystemEvent& event){
  OSYSDEBUG((TLPT_NAME"::DoInit()\n"));

  NEW_ALL_SUBJECT_AND_OBSERVER;
  REGISTER_ALL_ENTRY;
  SET_ALL_READY_AND_NOTIFY_ENTRY;

  //Aumento la dimensione standard del buffer di comunicazione interobject
  subject[sbjReceiveMsg]->SetBufferSize(TELEPATI_MAX_MESSAGE_TO_OBSERVER);  

  OStatus retValue=oSUCCESS;
  
  //inizializzo il modello fuzzy
  initFuzzySystem();  
  //parsing del file di configurazione
  if(parseIpConfigFile() != oSUCCESS) 
    retValue=oFAIL;
  //inizializzo le connessioni
  if(initConnections() != oSUCCESS) 
    retValue=oFAIL;
  
  return retValue;
}


/*DoStart: fa partire Telepati*/
OStatus TelepatiUdp::DoStart(const OSystemEvent& event){
  OSYSDEBUG((TLPT_NAME"::DoStart()\n"));
  
  int dummy=0; 
  char* pollingStr="With Polling Disabled";
  char* etherStatStr="With EtherStat Disabled";
  
  //  Partenza Timer periodico di controllo Ack
  EventID ackEvent = EventID();
  RelativeTime period(0,TELEPATI_ACK_TIME);
  TimeEventInfoWithRelativeTime timeInfo(TimeEventInfo::Periodic, period);
  sError error=SetTimeEvent(&timeInfo,myOID_,Extra_Entry[entryTimerEnd],&dummy,sizeof(dummy),&ackEvent);
  if(error!=sSUCCESS) 
    OSYSLOG1((osyslogERROR, TLPT_NAME"::DoStart() : ERROR: AckTimer not started\n"));
  
#ifdef TELEPATI_POLLING
  // Partenza timer periodico di Polling
  EventID eid;  
  RelativeTime pollingPeriod(TELEPATI_BONE_TIME,0);
  TimeEventInfoWithRelativeTime pollingTimeInfo(TimeEventInfo::Periodic, pollingPeriod);
  error=SetTimeEvent(&pollingTimeInfo,myOID_,Extra_Entry[entryBoneTimerEnd],&dummy,sizeof(dummy),&eid);
  if(error!=sSUCCESS) 
    OSYSLOG1((osyslogERROR, TLPT_NAME"::DoStart() : ERROR: BoneTimer not started\n"));  
  pollingStr="With Polling Enabled";
#endif

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

  
  ENABLE_ALL_SUBJECT;
  ASSERT_READY_TO_ALL_OBSERVER;    
  
  receive();  //avvio la ricezione
  
  for(int i=0; i< numaibo; i++) //mando i messaggi di saluto
    sendHiMsg(i);    
    
  OSYSLOG1((osyslogINFO, TLPT_NAME" started. Listening for %d Aibo on port %d\n %s\n %s\n", numaibo, TELEPATI_PORT, pollingStr, etherStatStr));

  return oSUCCESS;
}  //DoStart() END


/*DoStop: arresta TelepatiUdp*/
OStatus TelepatiUdp::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 retValue=oSUCCESS;
  
  for(int i=0; i<2; i++) //stoppo le spedizioni
    if(sendConnection[i].stop() != oSUCCESS) 
      retValue=oFAIL;

  if(recvConnection.stop() != oSUCCESS) //stoppo la ricezione
    retValue=oFAIL;
  
  return retValue;
}  //DoStop() END


/*DoDestroy: libera la memoria occupata da TelepatiUdp*/
OStatus TelepatiUdp::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 TelepatiUdp::checkQueue(){
  OSYSDEBUG((TLPT_NAME"::checkQueue()\n" ));

  if(!sendQueue.empty())
    {
      TelepatiPkg* msg=sendQueue.front();  sendQueue.pop();      
      sendViaData(msg);
    }

  return;
}  //checkQueue() END


/*checkCtrlQueue: controlla la presenza di messaggi di Controllo in coda quindi in attesa di spedizione. Se presenti li spedisce*/
void TelepatiUdp::checkCtrlQueue(){
  OSYSDEBUG((TLPT_NAME"::checkCtrlQueue()\n" ));

  if(!sendCtrlQueue.empty())
    {
      TelepatiPkg* msg=sendCtrlQueue.front();  sendCtrlQueue.pop();      
      sendViaCtrl(msg);
    }

  return;
}  //checkAckQueue() END

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

  for(int i=0; i < event.NumOfData(); i++){  //se ci sono piu' messaggi, scorrili
    
    TelepatiPkg* msg=TelepatiPkg::createPkg((byte*)event.Data(i));
    if(!msg){
      OSYSLOG1((osyslogERROR, TLPT_NAME"::SendNotify() : ERROR: BAD message from subject\n" ));
      continue;
    }

    switch(msg->getType()) 
      {
      case INFO:
	sendAiboInfoMsg();
	delete msg;
	break;
      case COUNTER:
	sendCounterMsg();
      case HI:  //se Hi non far nulla
      case HIREPLY:  //se HiReply non far nulla
      case BYE: //se Bbye non far nulla
      case ACK: //se ACK 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);
		      sendViaData(pkg); 
		    }
		}
	      delete msg;
	    }
	  else if(a > numaibo || aiboIpActive[a] == INACTIVE || aiboIpActive[a] == BYED)
	    {
	      //se l'aibo dest e' errato o inattivo fallisci
	      OSYSLOG1((osyslogERROR, TLPT_NAME"::SendNotify() ERROR : invalid AIBO address or AIBO inactive, %d\n", a));
	      delete msg;
	    }
	  else  //altrimenti manda i messaggio
	    sendViaData(msg); 
	  
	} //END default
      }// END switch
  } //END for
  
  observer[event.ObsIndex()]->AssertReady(); 
  
  return;
}  // SendNotify() END



void TelepatiUdp::sendViaData(TelepatiPkg* msg){
  OSYSDEBUG((TLPT_NAME"::sendViaData() to Aibo %d type %d serial %d (La coda ha %d messaggi)\n", msg->getAibo(), msg->getType(), msg->getSerial(), sendQueue.size() ));
  
  if(sendConnection[DATA_CONNECTION].getState() != CONNECTION_WAITING) //se la connessione non e' in attesa di spedizione
    sendQueue.push(msg);  //accoda il messaggio nella coda
  else
    {
      if(msg->getType() == TIMEINFO)
	{
	  TimeInfoPkg* time=(TimeInfoPkg*)msg;
	  OTime currentTime;
	  OPENR::GetTime(&currentTime);
	  time->setTime(currentTime.GetClock());
	}
      
      //copio il messaggio nel buffer di spedizione      
      byte* vec=msg->getMsg(); 
      size_t vecSize=msg->getSize();
      byte* ptr=sendConnection[DATA_CONNECTION].getSendData();
      memcpy(ptr, vec, vecSize);      
      
      //imposto i valori per il calcolo della velocita' di spedizione
      preComputeSpeed(vecSize); 
      sendByte+=vecSize;
      if(msg->needConfirm())  //se il msg ha bisogno di un ack
	{
	  setTimerEndValue(vecSize); //imposta il timer
	  ackWaitingMsg=msg->clone(); //metti un clone in attesa
	} 
      
      //creo il messaggio per la spedizione sendMsg
      UDPEndpointSendMsg sendMsg(sendConnection[DATA_CONNECTION].getEndpoint(), aiboIp[msg->getAibo()], TELEPATI_PORT, sendConnection[DATA_CONNECTION].getSendData(), vecSize);
      sendMsg.continuation = (void*)DATA_CONNECTION;
      sendConnection[DATA_CONNECTION].setState(CONNECTION_SENDING);
      sendMsg.Send(ipstackRef, myOID_, Extra_Entry[entrySendCont], sizeof(UDPEndpointSendMsg)); //mando il messaggio
      delete msg;
    }

  return;
} //sendViaData() END


void TelepatiUdp::sendViaCtrl(TelepatiPkg* msg){
//   size_t freeMem;
//   AnalyzerGetSizeOfFreeMemory(&freeMem);
//   OSYSDEBUG(("MEMORIA LIBERA %d\n", freeMem));

  OSYSDEBUG((TLPT_NAME"::sendViaCtrl() to Aibo %d type %d serial %d (La coda ha %d messaggi)\n", msg->getAibo(), msg->getType(), msg->getSerial(), sendCtrlQueue.size() ));

  if(sendConnection[CTRL_CONNECTION].getState()  != CONNECTION_WAITING) //se non e' in attesa di spedire 
    sendCtrlQueue.push(msg);  //accoda il messaggio
  else
    {
      if(msg->getType() == TIMEINFO)
	{
	  TimeInfoPkg* time=(TimeInfoPkg*)msg;
	  OTime currentTime;
	  OPENR::GetTime(&currentTime);
	  time->setTime(currentTime.GetClock());
	}

      //copio il messaggio nel buffer di spedizione
      byte* vec=msg->getMsg(); 
      size_t vecSize=msg->getSize();
      byte* ptr=sendConnection[CTRL_CONNECTION].getSendData();
      memcpy(ptr, vec, vecSize);      
            
      //creo il messaggio per la spedizione sendMsg
      UDPEndpointSendMsg sendMsg(sendConnection[CTRL_CONNECTION].getEndpoint(), aiboIp[msg->getAibo()], TELEPATI_PORT,sendConnection[CTRL_CONNECTION].getSendData(), vecSize);
      sendMsg.continuation = (void*)CTRL_CONNECTION;
      sendConnection[CTRL_CONNECTION].setState(CONNECTION_SENDING);

      sendMsg.Send(ipstackRef, myOID_, Extra_Entry[entrySendCont], sizeof(UDPEndpointSendMsg)); //mando il messaggio
      delete msg;
    }

  return;
} //sendViaCtrl() END


void TelepatiUdp::SendCont(ANTENVMSG msg){
  UDPEndpointSendMsg* sendMsg = (UDPEndpointSendMsg*)antEnvMsg::Receive(msg);
  int connID = (int)sendMsg->continuation;

  OSYSDEBUG((TLPT_NAME"::SendCont(%s)\n", ( connID == DATA_CONNECTION ? "DATA_CONNECTION" : "CTRL_CONNECTION")  ));

  if (sendConnection[connID].getState() == CONNECTION_CLOSED) return;  //se e' chiusa i dati non saranno validi  

  UDPEndpointError error=sendMsg->error;
  if (error != UDP_SUCCESS) //se c'e' stato un errore
    {
      char* addr=new char[16];
      addr=sendMsg->address.GetAsString(addr);
      OSYSLOG1((osyslogERROR, "%s : %s %d From IP: %s:%d", TLPT_NAME"::SendCont()", "FAILED. sendMsg->error", error, addr, sendMsg->port));
      delete [] addr;
      if(error == UDP_PROTOCOL_UNREACHABLE || error == UDP_HOST_UNREACHABLE || error == UDP_PORT_UNREACHABLE)
 	{
	  int aibo=aiboFromIp(sendMsg->address);
	  if(setAiboIpActive(aibo, INACTIVE))
 	    sendHiMsg(aibo);  //sonda l'aibo
 	}
    }
  else
    {
      sendMessage++;
      
      switch(connID)
	{
	case CTRL_CONNECTION:
	  sendConnection[CTRL_CONNECTION].setState(CONNECTION_WAITING);
	  checkCtrlQueue();
	  break;
	case DATA_CONNECTION:
	  if(ackWaitingMsg != 0)  //se ci sono messaggi in attesa di Ack
	    {
	      sendConnection[DATA_CONNECTION].setState(CONNECTION_WAITING_ACK);
	    }
	  else //se non ci sono messaggi in attesa di Ack
	    {
	      computeSpeed();
	      sendConnection[DATA_CONNECTION].setState(CONNECTION_WAITING);
	      checkQueue();
	    }
	  break;
	} //switch END
    }
  return;
}  //SendCont() END


void TelepatiUdp::TimerEnd(void* msg){
  if(ackWaitingMsg) //se c'e' un messaggio in attesa di Ack
    {
      if(timerEndCont >= numOfTimerEnd)  //se ho atteso abbastanza
	{
	  timerEndCont=0;
	  if(resending < TELEPATI_MAX_RESENDING)  //se non l'ho rispedito troppe volte
	    {
	      OSYSDEBUG((TLPT_NAME"::TimerEnd() rispedisco messaggio con seriale %d\n", ackWaitingMsg->getSerial()));
	      sendViaCtrl(ackWaitingMsg->clone());
	      numOfResending++;
	      resending++;
	    }
	  else //se l'ho rispedito troppe volte, cancellalo e libera la DATA Connection
	    {
	      OSYSLOG1((osyslogERROR, TLPT_NAME":: ERROR: Cannont send message serial %d to Aibo %d\n", ackWaitingMsg->getSerial(), ackWaitingMsg->getAibo()));
	      eraseFromAckWaitingMsg(ackWaitingMsg->getSerial(), ackWaitingMsg->getAibo());
	    }
	}
      else
	timerEndCont++; //incremento il contatore
    }
  return;
}

/*setTimerEndValue(): imposta i valori di attesa per timerend*/
void TelepatiUdp::setTimerEndValue(size_t dataDimension){
  dimension->value(dataDimension);
  model->evaluate_all();
  model->defuzzy_all();
  double valore=interval->get_value();
  OSYSDEBUG(("IL VALORE FUZZY CALCOLATO: %f DIMENSIONE PACKAGE: %d IL VALORE DI NUMOFTIMEREND SAREBBE %f\n", valore, dataDimension, floor(valore+0.5)));
      
  timerEndCont=0;
  numOfTimerEnd=floor(valore+0.5);
  return;
}

/* receive(): Avvia la ricezione.*/
void TelepatiUdp::receive(){
  
  UDPEndpointReceiveMsg receiveMsg(recvConnection.getEndpoint(), recvConnection.getRecvData(), recvConnection.getRecvSize());
  recvConnection.setState(CONNECTION_LISTENING);
  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 TelepatiUdp::ReceiveCont(ANTENVMSG msg){
  UDPEndpointReceiveMsg* receiveMsg=(UDPEndpointReceiveMsg*)antEnvMsg::Receive(msg);
  
  if(recvConnection.getState() == CONNECTION_CLOSED) return;  //se e' CLOSED i dati non sono validi

  recvConnection.setState(CONNECTION_RECEIVING);
  
  if (receiveMsg->error != UDP_SUCCESS) //se si e' verificato un errore
    {
      char* addr=new char[16];
      addr=receiveMsg->address.GetAsString(addr);
      OSYSLOG1((osyslogERROR, "%s : %s %d From IP: %s:%d", TLPT_NAME"::ReceiveCont()", "FAILED. receiveMsg->error", receiveMsg->error, addr, receiveMsg->port));
      delete [] addr;
      recvConnection.stop(); //fermo la connessione
      recvConnection.initConnection(); //reinizializzo la connessione
      
      Wait(1000000);  //aspetta 1 millisecondo

      receive();  //riattivo la ricezione
      return;
    }
  int aibo=aiboFromIp(receiveMsg->address);

  if(aibo == TelepatiPkg::ADDR_INVALID)  //se il msg arriva da un IP sconosciuto -> scartalo
    OSYSLOG1((osyslogERROR, TLPT_NAME"::ReceiveCont() : WARNING: received message from UNKNOWN address\n"));
  else
    {
      int recvSize=receiveMsg->size;  //dimensione dei dati ricevuti
      int i=0;   //inizializzo l'iteratore
      setAiboIpActive(aibo, ACTIVE);  //imposta l'aibo attivo
     
      while(i < recvSize){  //scorro il buffer 
	TelepatiPkg* recvPkg=TelepatiPkg::createPkg((receiveMsg->buffer)+i);  //ricreo l'oggetto
	if(!recvPkg)  //se recvPkg == 0 -> e' un messaggio errato
	  {
	    OSYSLOG1((osyslogERROR, TLPT_NAME"::ReceiveCont() : ERROR: received BAD message from Aibo %d\n", aibo));
	    return;
	  }
	
	i+=recvPkg->getSize();  //incremento l'iteratore del buffer
	
	totalRecvMessage++; //incremento il numero di msg ricevuti
	
	recvPkg->setAibo(aibo);
	
	OSYSDEBUG((TLPT_NAME"::ReceiveCont() Recieved msg type %d serial %d from Aibo %d\n", recvPkg->getType(), recvPkg->getSerial(), aibo ));

	switch(recvPkg->getType()){
	case HI:
	  sendHiReplyMsg(aibo);  //rispondi al saluto
	  break;
	  
	case HIREPLY:
	  OSYSDEBUG((TLPT_NAME"::ReceiveCont() ricevuto un HIREPLY\n"));
	  break;
	  
	case BYE:
	  setAiboIpActive(aibo, BYED);
	  break;
	  
	case ACK:
	  OSYSDEBUG((TLPT_NAME"::ReceiveCont(): Ricevuto ACK con seriale: %d\n", recvPkg->getSerial()  ));
	  eraseFromAckWaitingMsg(recvPkg->getSerial(), aibo); //cancello il messaggio in attesa di Ack
	  break;

	case INFO:
	  OSYSPRINT((TLPT_NAME"::ReceiveCont() :STRANGE: I've recived an AiboInfoPkg from remote !?\n"));
	  break;
	  
	case TIMEINFO:
	  {
	    TimeInfoPkg* timePkg=(TimeInfoPkg*)recvPkg;
	    OTime time(timePkg->getTime(), otimeTIME_DIF_UNDEF);
	    OStatus error=OPENR::SetTime(time);
	    break;
	  }

	default:
	  {
	    bool isADuplicate=false;
	    
	    if(recvPkg->needConfirm())  //se il messaggio richiede un ack
	      {
		sendViaCtrl( (TelepatiPkg*)new AckPkg(aibo, recvPkg->getSerial()) );  //manda Ack
		numOfAckSended++;
		//se il seriale atteso e' molto piu' grande del
		//seriale ricevuto allora si sono riazzerati i seriali
		//dei pacchetti (ho superato il limite massimo di 2^16
		//come numero di pacchetto, quindi riazzero il seriale
		//atteso.
		if( (expectedSerial[aibo] - recvPkg->getSerial()) > 65000)
		  {
		    expectedSerial[aibo]=0;
		  }
		if(recvPkg->getSerial() >= expectedSerial[aibo])  //se il seriale e' >= del seriale atteso (se non e' un doppione)
		  expectedSerial[aibo] = recvPkg->getSerial()+1;
		else
		  {
		    OSYSDEBUG((TLPT_NAME"::ReceiveCont() : Ho scartato il messaggio di seriale: %d dall'Aibo %d\n", recvPkg->getSerial(), aibo ));
		    isADuplicate=true; 
		  }
	      }	  
	    if(!isADuplicate)  //se non e' un doppione
	      {
		sendToObserver(recvPkg);
		dataRecvMessage++;
		dataRecvByte+=recvPkg->getSize();
	      }
	  }
	} //END switch
	delete recvPkg;

      } //END While
    } //END if(aibo == TelepatiPkg::ADDR_INVALID)
  
  receive();        
  return;
}  //ReceiveCont() END


/* sendToObserver(): manda il messaggio all'observer se pronto a
   riceverlo, altrimenti lo accoda.*/
void TelepatiUdp::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;
}

/*Invocata quando l'observer da un AsserReady*/
void TelepatiUdp::ObserverReady(const OReadyEvent& event){
  OSYSDEBUG((TLPT_NAME"::ObserverReady() dimensione coda %d\n", interObjectQueue.size() ));
  if(!interObjectQueue.empty())
    {
      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

/*BoneTimeEnd(): EXTRA ENTRY POINT, invocata da Open-r allo scadere del timer di rilevamento Aibo*/
void TelepatiUdp::BoneTimerEnd(void* msg){
#ifdef TELEPATI_POLLING
  OSYSDEBUG((TLPT_NAME"::BoneTimerEnd()\n"));

  for(int i=0; i< numaibo; i++){
    if(aiboIpActive[i] == WAITINGREPLY)
      setAiboIpActive(i, INACTIVE);
    sendHiMsg(i);
  }
#endif
  return;
}  //TimerEnd() END

/*EtherStat(): EXTRA ENTRY POINT, invocata da Open-r allo scadere del time fatto paritre allo Start, stampa statistiche sulla rete*/
void TelepatiUdp::EtherStat(void* msg){
#ifdef OPENR_ETHERSTAT
  EtherStat::printStat();
  OSYSPRINT(("Messaggi spediti: %d  Velocita' di spedizione: %d byte/s\n Numero di rispedizioni: %d Numero di Ack spediti: %d\nMessaggi ricevuti: Totale %d - Data %d\nTotale byte Spediti: %u Ricevuti: %u\n", sendMessage, sendSpeed, numOfResending, numOfAckSended, totalRecvMessage, dataRecvMessage, sendByte, dataRecvByte));


#endif  
  return;
}  //TimerEnd() END


/* parseIpConfigFile(): Imposta i valori di: numaibo, aiboIp e aiboIpActive. Scandisce il file di configurazione della rete di AIBO e memorizza gli indirizzi IP nel vettore aiboIp non registrando il proprio.*/
OStatus TelepatiUdp::parseIpConfigFile(){
  OSYSDEBUG((TLPT_NAME"::parseIpConfigFile()\n"));
  
  IPAddress* myIp=retrieveMyIp(); //il mio indirizzo IP
  bool iAmInList=false;  //io sono in AIBONET.CFG
  
  //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\n", AIBONET));
      numaibo=0;
      return oFAIL;
    }

  while( fin.getline(ch, LINEBUFFERSIZE) )
    {
      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)
	{
	  aiboIp.push_back(tmp);
	  OSYSLOG1((osyslogINFO, TLPT_NAME" INFO : Added IP: %s\n", ch ));
	}
      else
	iAmInList=true;
	
    }
  fin.close();
  //fine del parsing
  
  if(iAmInList)
    numaibo=aiboIp.size();
  else
    {
      numaibo=0;
      OSYSLOG1((osyslogERROR, TLPT_NAME"::parseIpConfigFile() : WARNING: My IP isn't listed in %s. I will not comunicate\n", AIBONET));
    }
  
  aiboIpActive=new aiboState[numaibo];
  expectedSerial=new unsigned short[numaibo];
  
  for(int i=0; i<numaibo; i++){
    aiboIpActive[i]=INACTIVE;
    expectedSerial[i]=0;
  }

  delete myIp;
  return oSUCCESS;
} //parseIpConfigFile() END


/*initConnections: inizializza le connessioni. Crea i buffer di spedizione e ricezione, crea gli endpoint UDP e fa il bind delle connessioni. Imposta i valori di: connections(compresi i valori in ogni struttura)*/
OStatus TelepatiUdp::initConnections(){
  OSYSDEBUG((TLPT_NAME"::initConnections()\n"));
  OStatus retValue=oSUCCESS;
  
  /* creo ed inizializzo la connessione di ricezione*/
  recvConnection=UDPRecvConnection(ipstackRef, TELEPATI_BUFFER_SIZE, TELEPATI_PORT);  
  if(recvConnection.initConnection() != oSUCCESS) 
    retValue=oFAIL;
  
  /* creo ed inizializzo le connessioni di spedizione*/
  sendConnection=new UDPSendConnection[2];
  for(int i=0; i<2; i++) {
    sendConnection[i]=UDPSendConnection(ipstackRef, TELEPATI_BUFFER_SIZE);
    if(sendConnection[i].initConnection() != oSUCCESS)
      retValue=oFAIL;
  }

  return retValue;
} //inizialize_connections() END


/*sendAiboInfoMsg: manda un messaggio AiboInfo all'observer*/
void TelepatiUdp::sendAiboInfoMsg(){
  OSYSDEBUG((TLPT_NAME"::sendAiboInfoMsg()\n"));
  
  AiboInfoPkg* info=new AiboInfoPkg(aiboIpActive, numaibo);
  sendToObserver(info);
  delete info;
  return;
}  //sendAiboInfoMsg() END


/*sendCounterMsg: manda un CounterMsg all'observer*/
void TelepatiUdp::sendCounterMsg(){
  OSYSDEBUG((TLPT_NAME"::sendCounterMsg()\n"));
  
  CounterPkg* pkg=new CounterPkg( dataRecvMessage, totalRecvMessage,  sendMessage);
  sendToObserver(pkg);
  delete pkg;
  return;
} //sendCounterMsg() END


/*sendByeMsg(): manda un ByeMsg, messaggio di arrivederci, all'i-esimo Aibo.*/
void TelepatiUdp::sendByeMsg(int i){
  OSYSDEBUG(("Telepati::sendByeMsg(%d)\n",i));
  
  TelepatiPkg* msg=new ByePkg(i);

  //scelgo quale connessione utilizzare.. quella libera
  int use=( sendConnection[CTRL_CONNECTION].getState() == CONNECTION_SENDING ? DATA_CONNECTION : CTRL_CONNECTION );  
  byte* vec=msg->getMsg(); 
  size_t vecSize=msg->getSize();
  byte* ptr=sendConnection[use].getSendData();
  memcpy(ptr, vec, vecSize);      
  //      memcpy(sendConnection[use].getSendData(), msg->getMsg(), vecSize);
   
  //creo il messaggio per la spedizione sendMsg
  UDPEndpointSendMsg sendMsg(sendConnection[use].getEndpoint(), aiboIp[i], TELEPATI_PORT, sendConnection[use].getSendData(), vecSize);
  sendConnection[use].setState(CONNECTION_SENDING);
  sendMsg.Call(ipstackRef,sizeof(UDPEndpointSendMsg)); //mando il messaggio
  sendConnection[use].setState(CONNECTION_WAITING);

  delete msg;
  return;
}


/*retrieveMyIp(): recupera l'indirizzo IP dell'Aibo su cui gira
  Telepati*/
IPAddress* TelepatiUdp::retrieveMyIp(){
  antEnvInitGetParamMsg getParamMsg("ETHER_IP");
  getParamMsg.Call(ipstackRef, sizeof(getParamMsg));
  if (getParamMsg.error == ANT_SUCCESS && getParamMsg.paramType == antEnv_InitParam_String)
    {
      OSYSLOG1((osyslogINFO, TLPT_NAME" INFO : 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


/*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 TelepatiUdp::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


/*aiboFromIp: cerca l'indice dell'Aibo a partire dal suo indirizzo IP. 
  Ritorna: indice dell'Aibo; -1 se non trovato*/
int TelepatiUdp::aiboFromIp(const IPAddress& ip) const{
  int aibo=TelepatiPkg::ADDR_INVALID;
  for(int i=0; i<numaibo; i++)   //trovo l'indice dell'aibo che ha spedito il pacchetto
    if(aiboIp[i] == ip)
      {
	aibo=i;
	break;
      }
  return aibo;
}  //aiboFromIp() END


/*eraseFromAckWaitingMsg(): cancella da ackWaitingMSg il messaggio
  identificato da serial e dall'Aibo di destinazione*/
void TelepatiUdp::eraseFromAckWaitingMsg(unsigned short serial, int aibo){
  OSYSDEBUG((TLPT_NAME"::eraseFromAckWaitingMsg()\n"));
  
  if(ackWaitingMsg)  //se c'e' un messaggio in attesa
    {
      if(ackWaitingMsg->getSerial() == serial && ackWaitingMsg->getAibo() == aibo) //se ho ricevuto l'ack corretto
	{
	  delete ackWaitingMsg; //lo cancello 
	  ackWaitingMsg=0;

	  //azzero i contatori necessari al timer periodico di ACk
	  numOfTimerEnd=0;
	  resending=0;
	  
	  computeSpeed();
	  OSYSDEBUG((TLPT_NAME"::eraseFromAckWaitingMsg(): cancello il messaggio in attesa di Ack con seriale %d\n", serial));
	  if(sendConnection[DATA_CONNECTION].getState() == CONNECTION_WAITING_ACK)
	    {
	      sendConnection[DATA_CONNECTION].setState(CONNECTION_WAITING);  //connessione in attesa
	      checkQueue();  //controlla se c'e' da spedire qualcosa
	    }
	}
      else if(serial > ackWaitingMsg->getSerial())
	{
	  OSYSDEBUG((TLPT_NAME"::WARNING ho ricevuto un Ack con Seriale maggiore dell'atteso\n"));
	}
    }

  return;
}  //eraseFromAckWaitingMsg() END


/*preComputeSpeed(): fa partire il conteggio dei secondi e imposta la
  dimensione dei dati per il calcolo della velocita' di spedizione*/
void TelepatiUdp::preComputeSpeed(size_t dataDimension){
#ifdef OPENR_ETHERSTAT
  sError err=GetSystemTime(startSendTime);  //memorizzo il timestamp di partenza della spedizione
  if(err != sSUCCESS)
    {
      startSendTime= new SystemTime(); //tempo non valido
      startSendTime->MakeInvalid();
    }
  sendConnection[DATA_CONNECTION].setSendSize(dataDimension);
#endif
  return;
}


/*computeSpeed(): calcola la velocita' di spedizione*/
void TelepatiUdp::computeSpeed(){
#ifdef OPENR_ETHERSTAT
  OSYSDEBUG((TLPT_NAME"::computeSpeed()\n"));
  if(ackWaitingMsg == 0)
    {
      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->IsValid())
	{
	  SystemTime elapsed= (*endSendTime - *startSendTime);  //tempo di consegna
	  double time=elapsed.seconds+((double)elapsed.useconds/1000000); //tempo di consegna in secondi
	  double speed=sendConnection[DATA_CONNECTION].getSendSize()/time;  //velocita' byte/secondo
	  sendConnection[DATA_CONNECTION].setSendSize(0);
	  if(sendSpeed == -1)
	    sendSpeed=floor(speed);
	  else
	    sendSpeed=floor((double)(sendSpeed+speed)/2);
	}
      delete endSendTime;
    }
#endif
} //computeSpeed() END


/*initFuzzySystem(): inizializza il modello fuzzy per il calcolo dei tempi di attesa*/
void TelepatiUdp::initFuzzySystem(){
  
  /*Creo le variabili fuzzy*/
  dimension=new FlFVar("Dimensione", 7, 65535);
  /* FlFSetTrapes(a,b, da, db);*/
  dimension->add_value_set(PICCOLA, new FlFSetTrapez(FlString("piccola"),7.0, 2048.0, 0.0 , 6148.0));
  dimension->add_value_set(MEDIA, new FlFSetTrapez(FlString("media"), 6384.0, 24580.0, 2286.0, 16384.0));
  dimension->add_value_set(GRANDE, new FlFSetTrapez(FlString("grande"), 40964.0, 49160.0, 16384.0, 8196.0 ));
  dimension->add_value_set(MOLTO_GRANDE, new FlFSetTrapez(FlString("molto_grande"), 57356.0, 65535.0, 16392.0, 0.0 ));
  
  interval=new FlFVar("Intervallo", 0, 10 );
  interval->add_value_set(PICCOLO, new FlFSetTrapez(FlString("piccolo"), 0.0, 0.0, 0.0, 1.5));
  interval->add_value_set(MEDIO, new FlFSetTrapez(FlString("medio"), 2.0, 2.5, 1.0, 1.0));
  interval->add_value_set(GRANDE, new FlFSetTrapez(FlString("grande"), 4.0, 4.5, 1.0, 1.0));
  interval->add_value_set(MOLTO_GRANDE, new FlFSetTrapez(FlString("molto_grande"), 6.0, 8.0, 1.0, 0.0));
  /* Fine inserimento variabili */
  
  /*Creo le regole/modello fuzzy*/
  model= new FlFRuleBase("mod_timerend");
  model->add_in_var(dimension);
  model->add_out_var(interval);
  
  FlFRule* rule= new FlFRule();
  rule->add_lhs(new FlFClause(dimension, dimension->get_value_set(PICCOLA)));
  rule->add_rhs(new FlFClause(interval, interval->get_value_set(PICCOLO)));
  model->add_rule(rule);
  rule= new FlFRule();
  rule->add_lhs(new FlFClause(dimension, dimension->get_value_set(MEDIA)));
  rule->add_rhs(new FlFClause(interval, interval->get_value_set(MEDIO)));
  model->add_rule(rule);
  rule= new FlFRule();
  rule->add_lhs(new FlFClause(dimension, dimension->get_value_set(GRANDE)));
  rule->add_rhs(new FlFClause(interval, interval->get_value_set(GRANDE)));
  model->add_rule(rule);
  rule= new FlFRule();
  rule->add_lhs(new FlFClause(dimension, dimension->get_value_set(MOLTO_GRANDE)));
  rule->add_rhs(new FlFClause(interval, interval->get_value_set(MOLTO_GRANDE)));
  model->add_rule(rule);
  /* fine inserimento regole*/

  return;
}
