#include <OPENR/OPENRAPI.h>
#include <OPENR/OSyslog.h>
#include <OPENR/core_macro.h>
#include "Orientamento.h"        
#include <AnalyzerAPI.h>
#include "../Motion/MotionInterface.h"
#include "../Ocipitale/OccipitalInfo.h"
#include <sstream>
                              //questo codice non va.....
                              // variando il parametro della velocita di rotazione non cambia molto
			     //il parametro migliore e 4.80 
Orientamento::Orientamento() 
  : mState(IDLE),
    locationInfo(0)
{};

OStatus Orientamento::DoInit(const OSystemEvent& event)
{
    OSYSDEBUG(("Orientamento::DoInit()\n"));

    NEW_ALL_SUBJECT_AND_OBSERVER;
    REGISTER_ALL_ENTRY;
    SET_ALL_READY_AND_NOTIFY_ENTRY;
    
    RedMarkerHour = 0.0;
    TimerDuration = 3980;
    AlignmentHour = 0.0;
    AlignmentFlag = true;
    AcceptHourVal = 0.20;		// Valore iniziale del cono di accettamento del dosso
    ForceScanFlag = 0;			// Di default non devo attraaversare il Dosso
    onBump        = false;		// Di default non mi trovo sopra il Dosso
    GG            = true;
    

    //Inizializzo anche il sensore della distanza
    //const string LOCATOR = "PRM:/p1-Sensor:p1";  //il sensore del petto (10/90 cm)
    //const string LOCATOR = "PRM:/r1/c1/c2/c3/p2-Sensor:p2";  //il sensore della testa (far) (20/150 cm)
    const string LOCATOR = "PRM:/r1/c1/c2/c3/p1-Sensor:p1";  //il sensore della testa (near) (5/50 cm)
    OStatus result = OPENR::OpenPrimitive(LOCATOR.c_str(), &sensorID);    //apro la primitiva e memorizzo il tutto in sensorID
    if (result != oSUCCESS)
    {
      OSYSPRINT(("Orientamento::DoInit(): ERROR - sensore non aperto\n"));
      return 0;
    }

    return oSUCCESS;
}

OStatus Orientamento::DoStart(const OSystemEvent& event)
{
    OSYSDEBUG(("Orientamento::DoStart()\n"));


    ENABLE_ALL_SUBJECT;
    ASSERT_READY_TO_ALL_OBSERVER;

    derPlus  = -5.0;						// Compenso la deriva se mi muovo verso Dx
    derMinus = -4.2;						// Compenso la deriva se mi muovo verso Sx o in avanti

    //
    // Mando a occipital il comando di ricercare la palla.
    //
    mState=IDLE;

    OccipitalInfo::OccipitalCommand cmd;
    cmd._command=OccipitalInfo::FIND;
    cmd._object=Stuff::BALL;
    cmd._ScanFlag=0;						// Non abilito il flag per l'attraversamento del Dosso
    subject[sbjSearchCommand]->SetData(&cmd, sizeof(cmd));
    subject[sbjSearchCommand]->NotifyObservers();

    /** Operazioni necessarie all'apertura del sensore IR */
    testTimer= true;						// Controlla che il timer della distanza
    int dummy=0;
    EventID sincroEvent = EventID();
    RelativeTime period(0,50);
    TimeEventInfoWithRelativeTime timeInfo(TimeEventInfo::Periodic, period);
    sError error=SetTimeEvent(&timeInfo,myOID_,Extra_Entry[entryTimerDist],&dummy,sizeof(dummy),&sincroEvent);
    if(error!=sSUCCESS)
      OSYSPRINT(("Orientamento::TimerDist(): ERROR - TimerDist non è partito\n"));

    return oSUCCESS;
 }

OStatus Orientamento::DoStop(const OSystemEvent& event)
{
    OSYSDEBUG(("Orientamento::DoStop()\n"));

    DISABLE_ALL_SUBJECT;
    DEASSERT_READY_TO_ALL_OBSERVER;
    OPENR::ClosePrimitive(sensorID);				// Chiudo il sensore IR
    return oSUCCESS;
}

OStatus Orientamento::DoDestroy(const OSystemEvent& event)
{
    DELETE_ALL_SUBJECT_AND_OBSERVER;
    return oSUCCESS;
}


void Orientamento::TimerEnd(void* msg){
  //OSYSDEBUG(("Orientamento::TimerEnd()\n"));
  // Allo scadere del timer controllo lo stato.
  manageState();
}

void Orientamento::TimerWalk(void* msg)				//invocata allo scadere del timer di camminata
{
  OSYSDEBUG(("Orientamento::TimerWalk()\n"));
  
  Motion::MotionCommand stop;					//mi fermo
  memset(&stop, 0, sizeof(stop));
  stop.motion_cmd=Motion::MOTION_STAND_NEUTRAL;
  subject[sbjMotionControl]->SetData(&stop, sizeof(stop));
  subject[sbjMotionControl]->NotifyObservers();

  if (mState != IDLE){						//se non sono arrivato
  	mState = BLU;						//cerco di nuovo
	locationInfo = NULL;
  }
  manageState();
}

void Orientamento::TimerDist(void* msg)
{
   if (testTimer) {						// Controllo distanza del goal con infrarossi

	OPENR::GetSensorValue(sensorID, &sensorValue);		// Recupero il valore del sensore sensorID
	slongword dist = sensorValue.value;			// Slongword è il tipo dati originale del .value, dovrebbe corrispondere a un long

	if (dist < DMIN && mState==BLU){			// Sono arrivato al goal? Lo cerco quando cammino...

		Motion::MotionCommand goal;			// Creo il messaggio di esultanza
		memset(&goal, 0, sizeof(goal));
		goal.motion_cmd=Motion::MOTION_GOAL_HAPPY1;	// MOTION_GOAL_HAPPY3
		goal.setFaceLEDs(Motion::LED_RED_EYES_HIGH, Motion::LED_BRIGHT, Motion::MODE_A);
		subject[sbjMotionControl]->SetData(&goal, sizeof(goal));
		subject[sbjMotionControl]->NotifyObservers();

		testTimer = false;
		mState=IDLE;					// Fermo tutto
	}
   }
}

void Orientamento::manageState(){
  switch(mState)
    {
    case IDLE:
{
    OSYSDEBUG(("GOAL!!! ;-)\n"));
    Motion::MotionCommand goal;					// Metto l'Aibo in Stand-By
    memset(&goal, 0, sizeof(goal));
    goal.setFaceLEDs(Motion::LED_RED_EYES_HIGH, Motion::LED_BRIGHT, Motion::MODE_A);
    subject[sbjMotionControl]->SetData(&goal, sizeof(goal));
    subject[sbjMotionControl]->NotifyObservers();
    break;
}

    case BLU:
	if(locationInfo)		//se ho informazioni sulla posizione dell'oggetto richiesto
	{
	  if( fabs(locationInfo->_hour) <= 0.3 )	//se la palla e' circa difronte
	    {
	      OSYSPRINT(("\n"));

                Motion::MotionCommand go;
                memset(&go, 0, sizeof(go));
                go.setFaceLEDs(Motion::LED_RED_EYES_HIGH, Motion::LED_BRIGHT, Motion::MODE_A);
	        go.mouth=10;
                go.motion_cmd=Motion::MOTION_WALK_TROT;
             	go.vx=100;				//velocità in avanti in mm/s
 		go.head_cmd=Motion::HEAD_ANGLES;	//abbasso la testa per usare meglio il sensore
                go.head_tilt2 = -15;			//testa su giù (-15,45)*/
                subject[sbjMotionControl]->SetData(&go, sizeof(go));
	        subject[sbjMotionControl]->NotifyObservers();

                int dummy = 0;
                EventID sincroEvent = EventID();	//"di poco interesse pratico"
                RelativeTime period(4, 0);		//tempo del timer: (s, ms)
                TimeEventInfoWithRelativeTime timeInfo(TimeEventInfo::NonPeriodic, period); //info del timer: non periodico di tempo period
                sError error = SetTimeEvent(	//imposta e fa partire il timer
                             &timeInfo,		//carica timeinfo
                              myOID_,		//roba di Open-R...
                              Extra_Entry[entryTimerWalk],  //funz. invocata allo scadere del timer
                             &dummy,	//parametro della funzione invocata alla fine del timer
                              sizeof(dummy),	//sizeof...
                             &sincroEvent	//lo usa Aperios
                                           );
                if(error!=sSUCCESS)
                  OSYSPRINT(("ERROR: TimerWalk is not started\n"));
	    }
          else {			//se la palla non è di fronte allineo l'AIBO

	    OSYSPRINT(("Found BLU goal...alignment\n"));
	    Motion::MotionCommand motCmd;
	    memset(&motCmd, 0, sizeof(motCmd));
	    motCmd.setFaceLEDs(Motion::LED_RED_EYES_HIGH, Motion::LED_BRIGHT, Motion::MODE_A);
	    motCmd.mouth=10;
            motCmd.motion_cmd=Motion::MOTION_WALK_TROT;
            motCmd.vx=0;
            //(hour * 360 ) / 12 = quanti gradi al secondi per raggiungere la posizione.
            motCmd.va= RAD((locationInfo->_hour * 30) * -1);
            motCmd.vy=0;
            subject[sbjMotionControl]->SetData(&motCmd, sizeof(motCmd));
            subject[sbjMotionControl]->NotifyObservers();

            mState=BLU;			// Controllo ancora il BLU
	    locationInfo = NULL;	// Forzo il controllo del colore

            //Faccio partire il timer (che alla fine riattiva manageState())
            EventID eid;
            RelativeTime period(0, 990);
            TimeEventInfoWithRelativeTime timeInfo(TimeEventInfo::NonPeriodic, period);
            SetTimeEvent(&timeInfo,myOID_,Extra_Entry[entryTimerEnd],&mState,sizeof(mState),&eid);
         
	  }
	}
        else {				//se non ho informazioni sulla posizione dell'oggetto richiesto dico a Occipital di ricercarlo

	  OSYSPRINT(("Right re-Check BLU position is a good choice\n"));
	  OccipitalInfo::OccipitalCommand cmd;
	  cmd._command=OccipitalInfo::FIND_YELLOW;
	  cmd._object=Stuff::BALL;
	  cmd._ScanFlag=0;
	  subject[sbjSearchCommand]->SetData(&cmd, sizeof(cmd));
	  subject[sbjSearchCommand]->NotifyObservers();
	}
    break;


    case GIALLO:
	/**
	*** Trovato un segmento giallo avvio la computazione per l'allineamento alle bande gialle e poi 
	*** per l'attraversamento del dosso.
	***
	*** Se il flag di controllo è vero non ero in fase si allineamento altrimenti mi sono allineato e devo
	*** ricontrollare la mia posizione del dosso attraverso ricerca CDT.
	***
	*** Il cono visivo ristretto = se il centroide è nei 4.5° (a destra o a sinistra) della punta
	*** dell'Aibo.
	**/
	
 	 ForceScanFlag = 1;					// Abilito la ricerca del centroide di due bande
	
	if (AlignmentFlag) {
		/**
		*** Se il centroide del dosso è dirca all'interno del cono visivo ristretto a 9° OK, altrimenti muovi 
		*** sull'asse y l'Aibo e ricontrolla la posizione.
		**/
		if (fabs(locationInfo->_hour) <= AcceptHourVal) {

			OSYSDEBUG(("Found the Yellow bump...it's in front of me\n"));
			if (AcceptHourVal >= 0.20){		// Se sto per entrare nella rampa cammino 
			  TimerDuration = 2000;			// Cammino per 1,5sec, piu' aggressivo ??
			} else {				// Se è una delle successive
			  TimerDuration = 1100;			// per circa un secondo
			}
			AcceptHourVal = 0.18;			// Restringo a metà il cono visivo, ormai sono sul dosso
			RedMarkerHour = 0.1;			// Angolo in ore per correggere la divergenza dell'Aibo
			MoveAhead();				// Faccio muovere l'Aibo con le impostazioni qui sopra
			ForceScanFlag = 2;			// Sul Dosso devo usare mantenere la testa ferma
			onBump = true;				// Sono salito sopra il Dosso
		} else {

			OSYSDEBUG(("Found the Yellow bump...alignment\n"));
			AlignmentHour = locationInfo->_hour;
			if (AcceptHourVal >= 0.20)		// Di default il tempo per il movimento è di 990 ms
			   TimerDuration = 1500;
			else					// Se sono sul dosso il movimento deve essere ridotto
			   TimerDuration = 650;
			MoveSide();				// Muovo l'Aibo sull'asse y
		}
		mState = GIALLO;				// Forzo lo stato a GIALLO
		AlignmentFlag = false;				// Variabile semaforica
	} else {
		
		OccipitalInfo::OccipitalCommand cmd;		// Creo un nuovo messaggio per avviare la ricerca
		cmd._command=OccipitalInfo::FIND_YELLOW;	// Imposto il case speciale per la ricerca del GIALLO
		cmd._object=Stuff::BALL;
		cmd._ScanFlag=ForceScanFlag;			// Imposto il flag per l'attraversamento del Dosso
		subject[sbjSearchCommand]->SetData(&cmd, sizeof(cmd));	// Invio il comando a Occipital
		subject[sbjSearchCommand]->NotifyObservers();
		AlignmentFlag = true;
	}
    break;


    case ROSSO:
     {  /**
     	*** Trovato il marker ROSSO memorizzo la posizione (_hour) in una variabile globale
     	*** e riprendo la ricerca del prossimo colore (VERDE) 
     	**/
	OSYSDEBUG(("Found RED marker...looking for GREEN one\n"));
	RedMarkerHour = locationInfo->_hour;
	
	OccipitalInfo::OccipitalCommand cmd;			// Creo un messaggio comando
        cmd._command=OccipitalInfo::FIND;			// Imposto in ricerca il messaggio comando
        cmd._object=Stuff::BALL;				// Imposto la Stuff da cercare
        cmd._ScanFlag=0;					// Non abilito il flag per l'attraversamento del Dosso
        subject[sbjSearchCommand]->SetData(&cmd, sizeof(cmd));	// Invio il comando
        subject[sbjSearchCommand]->NotifyObservers();		// Notifico l'ObServer del messaggio inviato
     }
    break;


    case VERDE:
	/** Devo discriminare dalla situazione in cui ho trovato il marker verde da quella in cui non l'ho trovato 
	***
	*** Entro in VERDE per 3 motivi:
	*** 1. Ho trovato il marker ROSSO e quello VERDE (ottimale)
	*** 2. Ho trovato il marker ROSSO ma non quello VERDE
	*** 3. Ho trovato il marker VERDE ma non quello ROSSO
	***
	**/
	if (locationInfo->_hour) {
	
		if (RedMarkerHour) { /** Caso 1. */
			
			OSYSDEBUG(("Found GREEN marker...let's go!\n"));
			RedMarkerHour = (locationInfo->_hour + RedMarkerHour) / 2;
			TimerDuration = 4200;
			MoveAhead();	// Faccio muovere il cane, l'angolo RedMarkerHour

		} else { /** Caso 3. */

			OSYSDEBUG(("WARNING: No RED marker found...using only GREEN marker's infos\n"));
			RedMarkerHour = locationInfo->_hour - 1.25;
			TimerDuration = 4200;
			MoveAhead();	// Faccio muovere il cane, l'angolo RedMarkerHour
		}
	} else { /** Caso 2 */

		OSYSDEBUG(("WARNING: No GREEN marker found...using only RED marker's infos\n"));
		RedMarkerHour += 1;
		TimerDuration = 4200;
		MoveAhead();		// Faccio muovere il cane, l'angolo RedMarkerHour
	}
	mState = CHECK;			// Effettuato il movimento torno in CHECK per proseguire
	RedMarkerHour = 0;		// Resetto il valore non piu necessario
     break;


     case CHECK:
	{ /**
	  *** Avvia la ricerca dei colori del percorso ad ostacoli 
	  ***/
	OccipitalInfo::OccipitalCommand cmd;
	cmd._command=OccipitalInfo::FIND;
	cmd._object=Stuff::BALL;
	cmd._ScanFlag=0;					// Non abilito il flag per l'attraversamento del Dosso
	subject[sbjSearchCommand]->SetData(&cmd, sizeof(cmd));
	subject[sbjSearchCommand]->NotifyObservers();
	}
     break;


     default:
      OSYSDEBUG(("WARNING: Exception on manageState(), force CHECK state.\n"));
      mState = CHECK;			// Setta lo stato in CHECK
      manageState();			// Riavvia lo switch in manageState()
     break;
    }
}


/** Muove l'Aibo in avanti con un angolo di sterzata calcolato a partire da RedMarkerHour */
void Orientamento::MoveAhead(){

   Motion::MotionCommand motCmd;			// Creo un messaggio per il movimento
   memset(&motCmd, 0, sizeof(motCmd));
	 // NOTA (hour * 360 ) / 12 = quanti gradi
   motCmd.head_cmd=Motion::HEAD_NO_CMD;
   motCmd.motion_cmd=Motion::MOTION_WALK_TROT;		// Imposto il movimento da effettuare
   motCmd.vx = 180;					// Velocità sull'asse X
   motCmd.va = RAD( (((RedMarkerHour * 30) * -1) / 4.5) + (derMinus + 2));	// Velocità di rotazione  4.80
   motCmd.vy = 0;					// Velocità sull'asse Y
   motCmd.setFaceLEDs(Motion::LED_GREEN_CYAN, Motion::LED_BRIGHT, Motion::MODE_B);//LED_RED_EYES_HIGH  -  MODE_A
   motCmd.mouth=10;
   subject[sbjMotionControl]->SetData(&motCmd, sizeof(motCmd));
   subject[sbjMotionControl]->NotifyObservers();
		
   EventID eid;						// Creo un timer per eseguire il movimento
   RelativeTime period(0, TimerDuration);		// Imposto il tempo di durata del timer (~4sec)
   TimeEventInfoWithRelativeTime timeInfo(TimeEventInfo::NonPeriodic, period);
   SetTimeEvent(&timeInfo,myOID_,Extra_Entry[entryTimerEnd],&mState,sizeof(mState),&eid);
} // END of MoveAhead()


/** Muove l'Aibo sul fianco, in entrambe le direzioni, basandosi sull'angolo in AlignmentHour */
void Orientamento::MoveSide(){

   /** Se non sono sul Dosso mi allineo spostandomi sul fianco (movimento su asse y) */
   if (!onBump && GG){
   	double Compensazione;
      /** In base alla distanza in ore dal dosso e al segno dell'ora imposto velocità e direzione.
      *** In base a dove mi diriggo utilizzo un differente fattore di compensazione */
   	if (fabs(AlignmentHour) >= 0.7){
    		if (AlignmentHour > 0){
    		   AlignmentHour = -150;
		   Compensazione = derPlus;
    		} else {
		   AlignmentHour = 150;
		   Compensazione = derMinus + 2;
		}
   	} else {
    		if (AlignmentHour > 0){
    		   AlignmentHour = -80;
		   Compensazione = derPlus;
    		} else {
		   AlignmentHour = 80;
		   Compensazione = derMinus + 2;
		}
   	}

	Motion::MotionCommand motCmd;		// Creo un messaggio per il movimento
	memset(&motCmd, 0, sizeof(motCmd));

	motCmd.head_cmd=Motion::HEAD_NO_CMD;
	motCmd.motion_cmd=Motion::MOTION_WALK_TROT;// Imposto il movimento da effettuare
	motCmd.vx = 0;				// Velocità sull'asse X
	motCmd.va = RAD(Compensazione);		// correzione deriva
	motCmd.vy = AlignmentHour;		// Velocità sull'asse Y
	motCmd.setFaceLEDs(Motion::LED_GREEN_CYAN, Motion::LED_BRIGHT, Motion::MODE_B);//LED_RED_EYES_HIGH  -  MODE_A
	motCmd.mouth=10;
	subject[sbjMotionControl]->SetData(&motCmd, sizeof(motCmd));
	subject[sbjMotionControl]->NotifyObservers();
		
	EventID eid;				// Creo un timer per eseguire il movimento
	RelativeTime period(0, TimerDuration);	// Imposto il tempo di durata del timer (~4sec)
	TimeEventInfoWithRelativeTime timeInfo(TimeEventInfo::NonPeriodic, period);
	SetTimeEvent(&timeInfo,myOID_,Extra_Entry[entryTimerEnd],&mState,sizeof(mState),&eid);
	GG = false;
   } else {

   /** Se sono sul Dosso invece di spostarmi ruoto su me stesso evitando di cadere */
	Motion::MotionCommand motCmd;		// Creo un messaggio per il movimento
	memset(&motCmd, 0, sizeof(motCmd));

	motCmd.head_cmd=Motion::HEAD_NO_CMD;
	motCmd.motion_cmd=Motion::MOTION_WALK_TROT;// Imposto il movimento da effettuare
	motCmd.vx = 40;				// Velocità sull'asse X
	motCmd.va = RAD(((AlignmentHour * 30) * -1));
	motCmd.vy = 0;				// Velocità sull'asse Y
	motCmd.setFaceLEDs(Motion::LED_GREEN_CYAN, Motion::LED_BRIGHT, Motion::MODE_B);//LED_RED_EYES_HIGH  -  MODE_A
	motCmd.mouth=10;
	subject[sbjMotionControl]->SetData(&motCmd, sizeof(motCmd));
	subject[sbjMotionControl]->NotifyObservers();
		
	EventID eid;				// Creo un timer per eseguire il movimento
	RelativeTime period(0, TimerDuration);	// Imposto il tempo di durata del timer (~4sec)
	TimeEventInfoWithRelativeTime timeInfo(TimeEventInfo::NonPeriodic, period);
	SetTimeEvent(&timeInfo,myOID_,Extra_Entry[entryTimerEnd],&mState,sizeof(mState),&eid);
	GG = true;
   }
} // END of MoveSide()


/** Riceve i messaggi dall'oggetto Occipital controlla lo stato inviato e imposta lo stato (mState) locale corretto */
void Orientamento::ObjectLocation(const ONotifyEvent& event){
  OSYSDEBUG(("Orientamento::ObjectLocation(const ONotifyEvent& event)\n"));
  locationInfo = reinterpret_cast<OccipitalInfo::ObjLocation*>(const_cast<void*>(event.Data(0)));
  
  observer[event.ObsIndex()]->AssertReady();
//OSYSDEBUG(("NOTE: Channel Found: %d\n",locationInfo->_state));
  /** Utilizzo il ChanIndex di Occipital per conoscere lo stato in cui andare */
  switch(locationInfo->_state) 
  {
     case 0:
  	mState=BLU;
     break;

     case 1:
  	mState=GIALLO;
     break;

     case 2:
  	mState=ROSSO;
     break;

     case 3:
  	mState=VERDE;
     break;

     case 4:
  	mState=CHECK;
     break;
  }

  if (mState != CHECK)
     manageState();					// Invoca la procedura principale di controllo
  else{
     RedMarkerHour = 0;
     TimerDuration = 2500;
     onBump = false;					// Sono sceso dal dosso
     ForceScanFlag = 0;
     MoveAhead();
  }

}
