#include <OPENR/OPENRAPI.h>
#include <OPENR/OSyslog.h>
#include <OPENR/core_macro.h>
#include "Occipital.h"
#include <AnalyzerAPI.h>
#include "../Motion/MotionInterface.h"
#include <sstream>
#include "OccipitalInfo.h"


/** 	
***	Blu La porta finale
***	Giallo Il dosso
***	Rosso Uno dei marker
***	Verde L'altro dei marker
**/
const string Occipital::colori[] = {"Blu", "Giallo", "Rosso", "Verde"};
const unsigned int Occipital::primitive[] = {ocdtCHANNEL0, ocdtCHANNEL1, ocdtCHANNEL2, ocdtCHANNEL3}; 
const int Occipital::threshold[] = {35,15, 2, 1};

Occipital::Occipital() : state(IDLE),
			 fbkID(oprimitiveID_UNDEF),
			 initSensorIndex(false),
			 sensorRegions(),
			 lastRefPan(0),
			 lastRefTilt(0)
{
  for (size_t i = 0; i < NUM_SENSORS; i++) 
    {
      jointID[i] = oprimitiveID_UNDEF;
      sensoridx[i] = -1;
    }
  
  knownStuff.push_back(static_cast<Stuff*>(new StuffBall()));
}

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

    NEW_ALL_SUBJECT_AND_OBSERVER;
    REGISTER_ALL_ENTRY;
    SET_ALL_READY_AND_NOTIFY_ENTRY;

    OPENR::SetMotorPower(opowerON);

    OpenPrimitives();

    SetCameraParameter();

    ChanIndex = 3;			// Inizializzo l'indice del canale CDT da usare
    GateAttraversati = 0;		// Inizializzo il contatore di Gate Attraversati
    FrameViewed    = 0;			// Inizializzo il contatore di frame analizzati
    RedMarkerFound = false;		// Inizializzo il flag che indica la scoperta del marker ROSSO
    ForceScanNear  = 0;			// Di deafault non devo fare lo HEAD_SCAN_BALL_NEAR
    TimerReset = false;

    // Inizializzo gli Stuff
    for(size_t i=0; i < knownStuff.size(); i++)
      knownStuff[i]->init(fbkID);
    
    return oSUCCESS;
}

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

    state =   IDLE;

    ENABLE_ALL_SUBJECT;
    ASSERT_READY_TO_ALL_OBSERVER;

    return oSUCCESS;
 }

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

    state = IDLE;

    DISABLE_ALL_SUBJECT;
    DEASSERT_READY_TO_ALL_OBSERVER;

    return oSUCCESS;
}

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


void Occipital::OpenPrimitives(){
    OStatus result;

    for (size_t i = 0; i < NUM_SENSORS; i++) 
      {
        result = OPENR::OpenPrimitive(SENSOR_LOCATOR[i], &jointID[i]);
        if (result != oSUCCESS) 
	  OSYSLOG1((osyslogERROR, "%s : %s %d","Occipital::OpenPrimitives()","OPENR::OpenPrimitive() FAILED", result));
        
      }
    
    result = OPENR::OpenPrimitive(FBK_LOCATOR, &fbkID);
    if (result != oSUCCESS) 
      OSYSLOG1((osyslogERROR, "%s : %s %d","Occipital::OpenPrimitives()","OPENR::OpenPrimitive() FAILED", result));
    
}


//  ____                      _      ____               _ 
// / ___|  ___  __ _ _ __ ___| |__  / ___|_ __ ___   __| |
// \___ \ / _ \/ _` | '__/ __| '_ \| |   | '_ ` _ \ / _` |
//  ___) |  __/ (_| | | | (__| | | | |___| | | | | | (_| |
// |____/ \___|\__,_|_|  \___|_| |_|\____|_| |_| |_|\__,_|
void Occipital::SearchCommand(const ONotifyEvent& event){

  OccipitalInfo::OccipitalCommand* command=(OccipitalInfo::OccipitalCommand*)event.Data(0);
  
  switch(command->_command)
  {
    // Se il comando e' NOTHING non far nulla.
  case OccipitalInfo::NOTHING:
    state = IDLE;
    searchedStuff=0;
    break;


    /** Se è un comando di ricerca stuff */
  case OccipitalInfo::FIND:
    {
    state = SEARCHING;
    searchedStuff = 0;
    FrameViewed = 0;
    ForceScanNear = 0;					// Impedisco lo HEAD_SCAN_BALL_NEAR

    /** Se devo cercare il canale BLU (la porta) e non ho superato almeno tre marker 
	salto al colore successivo (GIALLO) */
    if (ChanIndex == 3 && GateAttraversati <= 2) 
     {
	ChanIndex = (ChanIndex + 2 ) % 4;
     } else {
	ChanIndex = (ChanIndex + 1 ) % 4;
     }
    OSYSDEBUG(("Looking for: %s \n", colori[ChanIndex].c_str()));

    // Se lo stuff e' uno stuff riconsciuto allora lo ricerco.
    for(size_t i=0; i< knownStuff.size(); i++)
      if(knownStuff[i]->getType() == command->_object)
	searchedStuff=knownStuff[i];
    if(searchedStuff == 0)
      state=IDLE;
    }
  break;


    /** Se ho trovato il dosso giallo e mi devo allineare */
  case OccipitalInfo::FIND_YELLOW:
    {
    state = SEARCHING;
    searchedStuff = 0;
    FrameViewed = 0;
    TimerReset = true;
    ForceScanNear = command->_ScanFlag;			// Imposto il flag di Scan così come mi ha detto Orientamento

    // Se lo stuff e' uno stuff riconsciuto allora lo ricerco.
    for(size_t i=0; i< knownStuff.size(); i++)
      if(knownStuff[i]->getType() == command->_object)
	searchedStuff=knownStuff[i];
    if(searchedStuff == 0)
      state=IDLE;
    }
  break;


  default:
    state=IDLE;
    searchedStuff=0;
    break;
  }

  observer[event.ObsIndex()]->AssertReady();
}

/* ___                            
 *|_ _|_ __ ___   __ _  __ _  ___ 
 * | || '_ ` _ \ / _` |/ _` |/ _ \
 * | || | | | | | (_| | (_| |  __/
 *|___|_| |_| |_|\__,_|\__, |\___|
 *                     |___/      
 */
void Occipital::NotifyImage(const ONotifyEvent& event){
 // ##### OFbkImageVectorData* imageVec = reinterpret_cast<OFbkImageVectorData*>(const_cast<void*>(event.Data(0)));

  switch(state)
    {
    case IDLE:
	
    break;

      /** Se sto cercando una stuff */
    case SEARCHING:
      {
      	/**  Perche' creare due variabili con lo stesso cast ? */
	OFbkImageVectorData* imageVec = 
			reinterpret_cast<OFbkImageVectorData*>(const_cast<void*>(event.Data(0)));
	OFbkImageInfo* info = 
			imageVec->GetInfo(ofbkimageLAYER_C);

	if (TimerReset && FrameViewed == ((FrameTimer * 30) - 2)) {

		TimerReset  = false;
		FrameViewed = 0;
	}


	if (FrameViewed >= (FrameTimer * 30)) {
		
		/** Se non trovo piu GIALLO = ho attraversato il dosso faccio qualche passo in avanti e torno al
		    normale scan. */
		if (ForceScanNear){
			OSYSDEBUG(("Timer per il GIALLO scaduto \n"));
			OccipitalInfo::ObjLocation loc;
			loc._object   = searchedStuff->getType();
			loc._state    = 4;		// 4 non è raggiungibile da ChanIndex e identifica lo stato IDLE
			loc._hour     = 0.0;
			loc._distance = 0.0;

			subject[sbjObjectLocation]->SetData(&loc, sizeof(loc));
			subject[sbjObjectLocation]->NotifyObservers();
			state = IDLE;
			ForceScanNear = 0;		// resetto il valore di scan per il Dosso
			RedMarkerFound = false;		// Resetto il flag per non avere problemi futuri
			break;
		}
		/** Se è scaduto il timer per il marker VERDE e ho trovato il marker ROSSO 
		    allora mando lo stesso un messaggio a Orientamento */
		if (primitive[ChanIndex] == ocdtCHANNEL3 && RedMarkerFound) {
			
			OccipitalInfo::ObjLocation loc;
			loc._object   = searchedStuff->getType();
			loc._state    = ChanIndex;
			loc._hour     = 0.0;
			loc._distance = 0.0;

			subject[sbjObjectLocation]->SetData(&loc, sizeof(loc));
			subject[sbjObjectLocation]->NotifyObservers();
			
			state = IDLE;			// Interrompo la ricerca di marker
			RedMarkerFound = false;		// Resetto il flag per non avere problemi futuri
			break;				// Ignoro tutte le istruzioni che seguono
			
		} else {
			/** Se il timer è scaduto incremento l'indice del canale CDT considerando
			    i gate attraversati e resetto il contatore di frame avviato */
			if (ChanIndex == 3 && GateAttraversati <= 2) 
     			 {
			   ChanIndex = (ChanIndex + 2 ) % 4;
     			 } else {
			   ChanIndex = (ChanIndex + 1 ) % 4;
     			 }
			FrameViewed = 0;
			OSYSDEBUG(("Timer expired, try next color\nLooking for: %s \n", 
							colori[ChanIndex].c_str()));
		}
	}

	double pan, tilt, tilt2;
	GetPanTiltAngle(info->frameNumber, &pan, &tilt2, &tilt);

	// Aggiorno i valori degli angoli nella stuff cercata e prendo
	// il comando da mandare a motion
	// Aggiungo il canale da utilizzare per il riconoscimento dell'oggetto desiderato,
	// il threshold per ogni canale ed un flag per l'abilitazione o meno di HEAD_SCAN_BALL_NEAR
	Motion::MotionCommand cmd = 
		searchedStuff->search(*imageVec, pan, tilt2, tilt, 
						primitive[ChanIndex], threshold[ChanIndex], ForceScanNear);
	
	cmd.setFaceLEDs(Motion::LED_GREEN_CYAN, Motion::LED_BRIGHT, Motion::MODE_B);//LED_RED_EYES_HIGH  -  MODE_A
	cmd.mouth=10;
	subject[sbjMotionControl]->SetData(&cmd, sizeof(cmd));
	subject[sbjMotionControl]->NotifyObservers();
	

	/** Se la stuff e' stata individuata */
	if(searchedStuff->isTracked()){

	    // Comunico a Orientamento la posizione dell'oggetto trovato e il codice del colore
	    // che sto cercando.
	    OccipitalInfo::ObjLocation loc;
	    loc._object = searchedStuff->getType();
	    loc._state  = ChanIndex;
	    loc._hour   = pan2Hour(DEG(cmd.head_pan));
	    loc._distance=searchedStuff->getVisualDistance(*imageVec, primitive[ChanIndex]);

	    subject[sbjObjectLocation]->SetData(&loc, sizeof(loc));
	    subject[sbjObjectLocation]->NotifyObservers();
	    
	    /** Se il canale che ho appena elaborato è ROSSO e ho trovato il marker, imposto a true il flag */
	    if (primitive[ChanIndex] == ocdtCHANNEL2) 
	     {	
		RedMarkerFound = true;
		GateAttraversati++;		// Se ho trovato un marker rosso lo considero gate
	     }
	    else
	    	RedMarkerFound = false;

	    searchedStuff = 0;
	    state = IDLE;
	  }
	FrameViewed++;
      }
    break;

    default:
    break;

    }// SWITCH END
  observer[event.ObsIndex()]->AssertReady();
}

void Occipital::SetCameraParameter(){
  OPrimitiveControl_CameraParam shutter(ocamparamSHUTTER_MID);
  OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_SHUTTER_SPEED, &shutter, sizeof(shutter ) , 0 , 0);
  
  OPrimitiveControl_CameraParam gain(ocamparamGAIN_HIGH);
  OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_GAIN, &gain , sizeof(gain), 0,0);
  
  OPrimitiveControl_CameraParam wb(ocamparamWB_INDOOR_MODE) ;
  OPENR::ControlPrimitive (fbkID,oprmreqCAM_SET_WHITE_BALANCE, &wb, sizeof(wb),0,0);
  
  // Bilanciamento del bianco automatico
//  OPENR::ControlPrimitive (fbkID, oprmreqCAM_AWB_ON, 0, 0, 0, 0);
  // Bilanciamento esposizione automatico
  //OPENR::ControlPrimitive (fbkID, oprmreqCAM_AE_ON, 0, 0, 0, 0);
  
}


//  ____                            
// / ___|  ___ _ __  ___  ___  _ __ 
// \___ \ / _ \ '_ \/ __|/ _ \| '__|
//  ___) |  __/ | | \__ \ (_) | |   
// |____/ \___|_| |_|___/\___/|_|                                   
void Occipital::NotifySensor(const ONotifyEvent& event){
  RCRegion* rgn = event.RCData(0);
  
  if (initSensorIndex == false) 
    {
      OSensorFrameVectorData* sv = (OSensorFrameVectorData*)rgn->Base();
      InitSensorIndex(sv);
      initSensorIndex = true;
    }
  
  if (sensorRegions.size() == NUM_SENSOR_VECTOR) 
    {
      sensorRegions.front()->RemoveReference();
      sensorRegions.pop_front();
    }
  rgn->AddReference();
  sensorRegions.push_back(rgn);
  
  observer[event.ObsIndex()]->AssertReady();
}

void Occipital::InitSensorIndex(OSensorFrameVectorData* sensorVec){

  for (size_t i = 0; i < NUM_SENSORS; i++) 
    for (size_t j = 0; j < sensorVec->vectorInfo.numData; j++) 
      {
	OSensorFrameInfo* info = sensorVec->GetInfo(j);
	if (info->primitiveID == jointID[i]) 
	  {
	    sensoridx[i] = j;
	    OSYSDEBUG(("[%2d] %s\n", sensoridx[i], SENSOR_LOCATOR[i]));
	    break;
	  }
      }
}

void Occipital::GetPanTiltAngle(longword frameNum,double* pan, double* tilt2, double* tilt){
  list<RCRegion*>::iterator iter = sensorRegions.begin();
  list<RCRegion*>::iterator last = sensorRegions.end();
  OSensorFrameVectorData* sv;
  OSensorFrameInfo* info;
  OSensorFrameData* data;
  
  while (iter != last) 
    {
      sv = (OSensorFrameVectorData*)(*iter)->Base();
      info = sv->GetInfo(sensoridx[PAN_INDEX]);
      longword fn = info->frameNumber;
      
      for (size_t i = 0; i < info->numFrames; i++) 
	{
	  if (fn == frameNum) 
	    {
	      data = sv->GetData(sensoridx[PAN_INDEX]);
	      *pan = degrees((double)data->frame[i].value / 1000000.0);
	      
	      data = sv->GetData(sensoridx[TILT2_INDEX]);
	      *tilt2 = degrees((double)data->frame[i].value / 1000000.0);
	      
	      data = sv->GetData(sensoridx[TILT_INDEX]);
	      *tilt = degrees((double)data->frame[i].value / 1000000.0);
	      
	      return;
	    }
	  
	  fn++;
	  if (fn > oframeMAX_NUMBER) fn = 1;
	}
        
      ++iter;
    }

  //
  // If sensor data at frameNum is not found, 
  // return latest sensor data.
  //
  
  RCRegion* rgn = sensorRegions.back();
  sv = (OSensorFrameVectorData*)rgn->Base();
  info = sv->GetInfo(sensoridx[PAN_INDEX]);
  
  data = sv->GetData(sensoridx[PAN_INDEX]);
  *pan = degrees((double)data->frame[info->numFrames-1].value / 1000000.0);
  
  data = sv->GetData(sensoridx[TILT2_INDEX]);
  *tilt2 = degrees((double)data->frame[info->numFrames-1].value / 1000000.0);
  
  data = sv->GetData(sensoridx[TILT_INDEX]);
  *tilt = degrees((double)data->frame[info->numFrames-1].value / 1000000.0);
}

void Occipital::GetIrHeadDistance(longword frameNum, int* near, int* far){
  list<RCRegion*>::iterator iter = sensorRegions.begin();
  list<RCRegion*>::iterator last = sensorRegions.end();
  OSensorFrameVectorData* sv;
  OSensorFrameInfo* info;
  OSensorFrameData* data;
  
  while (iter != last) 
    {
      sv = (OSensorFrameVectorData*)(*iter)->Base();
      info = sv->GetInfo(sensoridx[HEAD_IRSENSOR_NEAR]);
      longword fn = info->frameNumber;
      
      for (size_t i = 0; i < info->numFrames; i++) 
	{
	  if (fn == frameNum) 
	    {
	      data = sv->GetData(sensoridx[HEAD_IRSENSOR_NEAR]);
	      *near=(int)data->frame[i].value;
	      data = sv->GetData(sensoridx[HEAD_IRSENSOR_FAR]);
	      *far=(int)data->frame[i].value;
	      
	      return;
	    }
	  
	  fn++;
	  if (fn > oframeMAX_NUMBER) fn = 1;
	}
        
      ++iter;
    }

  //
  // If sensor data at frameNum is not found, 
  // return latest sensor data.
  //
  
  RCRegion* rgn = sensorRegions.back();
  sv = (OSensorFrameVectorData*)rgn->Base();
  
  info = sv->GetInfo(sensoridx[HEAD_IRSENSOR_NEAR]);

  data = sv->GetData(sensoridx[HEAD_IRSENSOR_NEAR]);
  *near=(int)data->frame[info->numFrames-1].value;
  data = sv->GetData(sensoridx[HEAD_IRSENSOR_FAR]);
  *far=(int)data->frame[info->numFrames-1].value;
}

