/*
  Teleled.cc
*/

#include "../lib/Packages/Packages.h"
#include "TeleLed.h"
#include <OPENR/OSyslog.h>
#include <OPENR/core_macro.h>
#include <OPENR/OPENRAPI.h>
#include <MoNetData.h>
#include "entry.h"
#define MSGTOSEND 2000

TeleLed::TeleLed(){
  aibos=0;
  rubbia=0;
  canSend=0;
  msgToSend=0;
  sendMsg=0;
  recvMsg=0;
  info= new AiboInfoPkg();
  for (int i = 0; i < NUM_LEDS; i++) ledID[i] = oprimitiveID_UNDEF;
  for (int i = 0; i < NUM_COMMAND_VECTOR; i++) region[i] = 0;
}

TeleLed::~TeleLed(){
}


/*DoInit: inizializza Telepati; fa il parse degli indirizzi IP nel file di configurazione CFGKEYWORD e inizializza le connessioni*/
OStatus TeleLed::DoInit(const OSystemEvent& event){
  OSYSDEBUG((TLLD_NAME"::DoInit()\n"));
  
  NEW_ALL_SUBJECT_AND_OBSERVER;
  REGISTER_ALL_ENTRY;
  SET_ALL_READY_AND_NOTIFY_ENTRY;

  subject[sbjTesterSend]->SetBufferSize(5);  

  OpenPrimitives();
  NewCommandVectorData();

  return oSUCCESS;
}


/*DoStart: fa partire Telepati*/
OStatus TeleLed::DoStart(const OSystemEvent& event){

  OSYSDEBUG((TLLD_NAME"::DoStart()\n"));

  ENABLE_ALL_SUBJECT;
  ASSERT_READY_TO_ALL_OBSERVER;

  OStatus st = OPENR::SetMotorPower(opowerON);   
  
  subject[sbjTesterSend]->SetData(info->getMsg(), info->getSize());
  subject[sbjTesterSend]->NotifyObservers();
  
  return oSUCCESS;
}  //DoStart() END


/*DoStop: arresta Telepati*/
OStatus TeleLed::DoStop(const OSystemEvent& event){
  
  OSYSDEBUG((TLLD_NAME"::DoStop() Totale messaggi ricevuti: %d\n", recvMsg ));

  DISABLE_ALL_SUBJECT;
  DEASSERT_READY_TO_ALL_OBSERVER;
  OStatus st = OPENR::SetMotorPower(opowerOFF); 

  return oSUCCESS;
}  //DoStop() END


/*DoDestroy: libera la memoria occupata da Telepati*/
OStatus TeleLed::DoDestroy(const OSystemEvent& event){

  OSYSDEBUG((TLLD_NAME"::DoDestroy\()\n"));

  DELETE_ALL_SUBJECT_AND_OBSERVER;

  return oSUCCESS;
}  //DoDestroy() END


void TeleLed::ObjReady(const OReadyEvent& event){ 
  //  int arr[]={20, 100, 1024, 2048, 4000, 10000, 16000, 22000, 32000, 4500, 5600, 6400, 3000, 6000, 5500, 12000, 3300, 20, 2500, 3, 4800, 200, 5400, 6900, 8000, 18000, 2900, 25, 8000, 15000, 3500 };
  //int arr[]={20, 25, 25, 22, 20, 25, 25, 22,20, 25, 25, 22,20, 25, 25, 22,20, 25, 25, 22,20, 25, 25, 22, 20, 25, 54, 27, 28, 30 };
  
//   static int i=0;
  
//   if(info->isAllActive() && i < MSGTOSEND)
//     {
//       TelepatiPkg* pkg=new DummyPkg(TelepatiPkg::ADDR_BROADCAST, true);
//       subject[sbjTesterSend]->SetData(pkg->getMsg(), pkg->getSize());
//       delete pkg;
//       i++;
//       sendMsg+=aibos;
//     }
  
  //   for(int i=0; i<aibos; i++){
  //     if(msgToSend[i] >0 && info->isActive(i))  //se devo mandargli i messaggi ed e' attivo
//       {
// 	if(subject[sbjTesterSend]->RemainBuffer() == 0)
// 	  OSYSDEBUG(("REMAIN BUFFER UGUALE A 0, CAZZO \n"));
  
// 	OSYSDEBUG((TLLD_NAME"::MANDO IL MESSAGGIO ALL'AIBO %d\n", i));
// 	TelepatiPkg* pkg=new DummyPkg(i, true);
// 	subject[sbjTesterSend]->SetData(pkg->getMsg(), pkg->getSize());
// 	msgToSend[i]--;
// 	sendMsg++;
//       }
//   }

//   subject[sbjTesterSend]->NotifyObservers();
  
  if(sendMsg >= MSGTOSEND*aibos){
    OSYSDEBUG((TLLD_NAME"::ObjReady() : Messaggi spediti: %d\n", sendMsg));
  }
  
  
  return;
}


void TeleLed::Ready(const OReadyEvent& event){
  OSYSDEBUG((TLLD_NAME"::Ready(): Totale messaggi ricevuti %d\n", recvMsg));
  return;
}

/* receive(): avviata al ricevimento dell'AR spedito dall'observer.*/
void TeleLed::Receive(const ONotifyEvent& event){ 
  static bool ledOn=false;
  
  for(int j=0; j < event.NumOfData(); j++)
    {
      TelepatiPkg* msg=TelepatiPkg::createPkg((byte*)event.Data(j));
    
      pkg_type type=msg->getType();
    
      switch(type)
	{
	case INFO:
	  {  //se ricevo un AiboInfoPkg
	    delete info;
	    info=(AiboInfoPkg*)msg;
	    aibos=info->getNumOfAibo();
	    if(msgToSend == 0) //se non e' inizializzato l'array lo inizializzo
	      {
		msgToSend=new int[aibos];
		for(int i=0; i<aibos; i++)
		  msgToSend[i]=MSGTOSEND;
	      }
	    if(canSend == 0) //se non e' inizializzato l'array lo inizializzo
	      {
		canSend=new bool[aibos];
		for(int i=0; i<aibos; i++)
		  canSend[i]=true;
	      }
	    break;
	  }
	case DUMMYLED: 
	  {  //se ricevi un DummyLedPkg
	    recvMsg++;
	    if(!ledOn)
	      {

		DummyLedPkg* dummypkg=(DummyLedPkg*)msg;
		RCRegion* rgn = FindFreeRegion();
		SetLED3Value(rgn, dummypkg->getColor(), oled3_MODE_B);
		subject[sbjBlink]->SetData(rgn);
		subject[sbjBlink]->NotifyObservers();
		ledOn=true;
		delete dummypkg;
	      }
	    break;
	  }
	case COUNTER: 
	  {  //se ricevo un counter
	    CounterPkg* contpkg=(CounterPkg*)msg;
	    OSYSDEBUG((TLLD_NAME"::Telepati ha risposto di aver ricevuto: %d messaggi di cui %d messaggi di dati  e di averne spediti: %d\n", contpkg->getTotalRecvMessage(),contpkg->getDataRecvMessage(),contpkg->getSendMessage() ));
	    delete contpkg;
	    break;
	  }
	default: 
	  {
	    recvMsg++;
	    delete msg;
	  }
	}  //END switch

//       for(int i=0; i<aibos; i++){
// 	if(info->isActive(i) && canSend[i])
// 	  {
// 	    OSYSDEBUG((TLLD_NAME": Mando un DummyLed all'Aibo %d\n", i));
// 	    TelepatiPkg* pkg=new DummyLedPkg(i, 50, true);
// 	    subject[sbjTesterSend]->SetData(pkg->getMsg(), pkg->getSize());
// 	    canSend[i]=false;
// 	  }
//       }
      if(info->isAllActive() && canSend[0])
	{
	  OSYSDEBUG((TLLD_NAME": Mando un DummyLed all'Aibo \n"));
	  //	  TelepatiPkg* pkg=new DummyLedPkg(TelepatiPkg::ADDR_BROADCAST, 10, true);
	  TelepatiPkg* pkg=new DummyPkg(TelepatiPkg::ADDR_BROADCAST, true);
	  subject[sbjTesterSend]->SetData(pkg->getMsg(), pkg->getSize());
	  subject[sbjTesterSend]->NotifyObservers();
	  delete pkg;
	  
	  canSend[0]=false;
	}
    } //big for END
  

  observer[obsTesterReceive]->AssertReady();

  OSYSDEBUG((TLLD_NAME": Totale messaggi ricevuti %d\n", recvMsg));
  return;
}


void TeleLed::OpenPrimitives(){
  for (int i = 0; i < NUM_LEDS; i++) {
    OStatus result = OPENR::OpenPrimitive(LED3_LOCATOR[i], &ledID[i]);
    if (result != oSUCCESS) {
      OSYSLOG1((osyslogERROR, "%s : %s %d",
		TLLD_NAME"::OpenPrimitives()",
		"OPENR::OpenPrimitive() FAILED", result));
    }
  }
}


void TeleLed::NewCommandVectorData(){
  OStatus result;
  MemoryRegionID      cmdVecDataID;
  OCommandVectorData* cmdVecData;
  
  for (int i = 0; i < NUM_COMMAND_VECTOR; i++) {
    
    result = OPENR::NewCommandVectorData(NUM_LEDS, 
					 &cmdVecDataID, &cmdVecData);
    if (result != oSUCCESS) {
      OSYSLOG1((osyslogERROR, "%s : %s %d",
		TLLD_NAME"::NewCommandVectorData()",
		"OPENR::NewCommandVectorData() FAILED", result));
    }
    
    region[i] = new RCRegion(cmdVecData->vectorInfo.memRegionID,
			     cmdVecData->vectorInfo.offset,
			     (void*)cmdVecData,
			     cmdVecData->vectorInfo.totalSize);
    
    cmdVecData->SetNumData(NUM_LEDS);
    
    for (int j = 0; j < NUM_LEDS; j++) {
      OCommandInfo* info = cmdVecData->GetInfo(j);
      info->Set(odataLED_COMMAND3, ledID[j], ocommandMAX_FRAMES);
    }
  }
}


void TeleLed::SetLED3Value(RCRegion* rgn, sword intensity, OLED3Mode mode){
  OCommandVectorData* cmdVecData = (OCommandVectorData*)rgn->Base();
  
  for (int i = 0; i < NUM_LEDS; i++) {
    OCommandData* data = cmdVecData->GetData(i);
    OLEDCommandValue3* val = (OLEDCommandValue3*)data->value;
    for (int j = 0; j < ocommandMAX_FRAMES; j++) {
      val[j].intensity = intensity;
      val[j].mode      = mode;
      val[j].period    = 100;
    }
  }
}


RCRegion* TeleLed::FindFreeRegion(){
    for (int i = 0; i < NUM_COMMAND_VECTOR; i++) {
        if (region[i]->NumberOfReference() == 1) return region[i];
    }

    return 0;
}


void TeleLed::MoResult(const ONotifyEvent& event){ 
  OSYSDEBUG((TLLD_NAME"::MoNetResult()\n"));
  //  stato=new canSend();
  
  MoNetResult* result = (MoNetResult*)event.Data(0);
  
  if (result->status != monetCOMPLETION && result->status != monetBUSY) {
    OSYSLOG1((osyslogERROR,
	      "%s : MoNetResult error [commandID %d,status %d]",
	      TLLD_NAME"::NotifyResult()",
	      result->commandID, result->status));
  }
  OSYSDEBUG((TLLD_NAME":Allora, esto es rubbia: %d\n", rubbia));

  if(rubbia == 0)
    { //vai in stand
      MoNetCommand cmd(2);
      subject[sbjCommand]->SetData(&cmd, sizeof(cmd));
      subject[sbjCommand]->NotifyObservers();
      rubbia++;
    }
  else if(rubbia == 1 )
    {
      MoNetCommand cmd(10);
      subject[sbjCommand]->SetData(&cmd, sizeof(cmd));
      subject[sbjCommand]->NotifyObservers();
      rubbia++;
    }
  else if(rubbia == 2)
    {
      MoNetCommand cmd(1);
      subject[sbjCommand]->SetData(&cmd, sizeof(cmd));
      subject[sbjCommand]->NotifyObservers();
      rubbia++;
    }
  observer[event.ObsIndex()]->AssertReady();
  return;
}
