#include "StuffBall.h"
#include <math.h>
#include <OPENR/OPENRAPI.h>
#include <OPENR/OSyslog.h>
#include "MCOOP.h"


void StuffBall::init(OPrimitiveID& fbkID){
  OStatus result;
  MemoryRegionID  cdtVecID;
  OCdtVectorData* cdtVec;
  OCdtInfo*       cdt;

  // 
  // Imposto i canali CDT per la Aibo Ball
  //
  
  result = OPENR::NewCdtVectorData(&cdtVecID, &cdtVec);
  if (result != oSUCCESS) 
    {
      OSYSLOG1((osyslogERROR, "%s : %s %d", "ImageObserver::SetCdtVectorDataOfPinkBall()", "OPENR::NewCdtVectorData() FAILED", result));
      return;
    }
  
  cdtVec->SetNumData(4);
  
  cdt = cdtVec->GetInfo(0);
  cdt->Init(fbkID, ocdtCHANNEL0);
  
  //
  // cdt->Set(Y_segment, Cr_max,  Cr_min, Cb_max, Cb_min)
  //
  // Blu
  cdt->Set( 0, 114, 1, 250, 138);
  cdt->Set( 1, 114, 1, 250, 138);
  cdt->Set( 2, 114, 1, 250, 138);
  cdt->Set( 3, 114, 1, 250, 138);
  cdt->Set( 4, 114, 1, 250, 138);
  cdt->Set( 5, 114, 1, 250, 138);
  cdt->Set( 6, 114, 1, 250, 138);
  cdt->Set( 7, 114, 1, 250, 138);
  cdt->Set( 8, 114, 1, 250, 138);
  cdt->Set( 9, 114, 1, 250, 138);
  cdt->Set(10, 114, 1, 250, 138);
  cdt->Set(11, 114, 1, 250, 138);
  cdt->Set(12, 114, 1, 250, 138);
  cdt->Set(13, 114, 1, 250, 138);
  cdt->Set(14, 114, 1, 250, 138);
  cdt->Set(15, 114, 1, 250, 138);
  cdt->Set(16, 114, 1, 250, 138);
  cdt->Set(17, 114, 1, 250, 138);
  cdt->Set(18, 114, 1, 250, 138);
  cdt->Set(19, 114, 1, 250, 138);
  cdt->Set(20, 114, 1, 250, 138);
  cdt->Set(21, 114, 1, 250, 138);
  cdt->Set(22, 114, 1, 250, 138);
  cdt->Set(23, 114, 1, 250, 138);
  cdt->Set(24, 114, 1, 250, 138);
  cdt->Set(25, 114, 1, 250, 138);
  cdt->Set(26, 114, 1, 250, 138);
  cdt->Set(27, 114, 1, 250, 138);
  cdt->Set(28, 114, 1, 250, 138);
  cdt->Set(29, 114, 1, 250, 138);
  cdt->Set(30, 114, 1, 250, 138);
  cdt->Set(31, 114, 1, 250, 138);


  cdt = cdtVec->GetInfo(1);
  cdt->Init(fbkID, ocdtCHANNEL1);
  
  //
  // cdt->Set(Y_segment, Cr_max,  Cr_min, Cb_max, Cb_min)
  //
  // Giallo
  cdt->Set( 0, 164, 110, 85, 1);
  cdt->Set( 1, 164, 110, 85, 1);
  cdt->Set( 2, 164, 110, 85, 1);
  cdt->Set( 3, 164, 110, 85, 1);
  cdt->Set( 4, 164, 110, 85, 1);
  cdt->Set( 5, 164, 110, 85, 1);
  cdt->Set( 6, 164, 110, 85, 1);
  cdt->Set( 7, 164, 110, 85, 1);
  cdt->Set( 8, 164, 110, 85, 1);
  cdt->Set( 9, 164, 110, 85, 1);
  cdt->Set(10, 164, 110, 85, 1);
  cdt->Set(11, 164, 110, 85, 1);
  cdt->Set(12, 164, 110, 85, 1);
  cdt->Set(13, 164, 110, 85, 1);
  cdt->Set(14, 164, 110, 85, 1);
  cdt->Set(15, 164, 110, 85, 1);
  cdt->Set(16, 164, 110, 85, 1);
  cdt->Set(17, 164, 110, 85, 1);
  cdt->Set(18, 164, 110, 85, 1); 
  cdt->Set(19, 164, 110, 85, 1);
  cdt->Set(20, 164, 110, 85, 1);
  cdt->Set(21, 164, 110, 85, 1);
  cdt->Set(22, 164, 110, 85, 1);
  cdt->Set(23, 164, 110, 85, 1);
  cdt->Set(24, 164, 110, 85, 1);
  cdt->Set(25, 164, 110, 85, 1);
  cdt->Set(26, 164, 110, 85, 1);
  cdt->Set(27, 164, 110, 85, 1);
  cdt->Set(28, 164, 110, 85, 1);
  cdt->Set(29, 164, 110, 85, 1);
  cdt->Set(30, 164, 110, 85, 1);
  cdt->Set(31, 164, 110, 85, 1);
  

  cdt = cdtVec->GetInfo(2);
  cdt->Init(fbkID, ocdtCHANNEL2);
  
  //
  // cdt->Set(Y_segment, Cr_max,  Cr_min, Cb_max, Cb_min)
  //
  //
  // rosso
  cdt->Set( 0, 225, 175,130, 78);
  cdt->Set( 1, 225, 175,130, 78);
  cdt->Set( 2, 225, 175,130, 78);
  cdt->Set( 3, 225, 175,130, 78);
  cdt->Set( 4, 225, 175,130, 78);
  cdt->Set( 5, 225, 175,130, 78);
  cdt->Set( 6, 225, 175,130, 78);
  cdt->Set( 7, 225, 175,130, 78);
  cdt->Set( 8, 225, 175,130, 78);
  cdt->Set( 9, 225, 175,130, 78);
  cdt->Set(10, 225, 175,130, 78);
  cdt->Set(11, 225, 175,130, 78);
  cdt->Set(12, 225, 175,130, 78);
  cdt->Set(13, 225, 175,130, 78);
  cdt->Set(14, 225, 175,130, 78);
  cdt->Set(15, 225, 175,130, 78);
  cdt->Set(16, 225, 175,130, 78);
  cdt->Set(17, 225, 175,130, 78);
  cdt->Set(18, 225, 175,130, 78);
  cdt->Set(19, 225, 175,130, 78);
  cdt->Set(20, 225, 175,130, 78);
  cdt->Set(21, 225, 175,130, 78);
  cdt->Set(22, 225, 175,130, 78);
  cdt->Set(23, 225, 175,130, 78);
  cdt->Set(24, 225, 175,130, 78);
  cdt->Set(25, 225, 175,130, 78);
  cdt->Set(26, 225, 175,130, 78);
  cdt->Set(27, 225, 175,130, 78);
  cdt->Set(28, 225, 175,130, 78);
  cdt->Set(29, 225, 175,130, 78);
  cdt->Set(30, 225, 175,130, 78);
  cdt->Set(31, 225, 175,130, 78);


  cdt = cdtVec->GetInfo(3);
  cdt->Init(fbkID, ocdtCHANNEL3);
  
  //
  // cdt->Set(Y_segment, Cr_max,  Cr_min, Cb_max, Cb_min)
  //
  // verde
  cdt->Set( 0, 100, 75, 99, 65);
  cdt->Set( 1, 100, 75, 99, 65);
  cdt->Set( 2, 100, 75, 99, 65);
  cdt->Set( 3, 100, 75, 99, 65);
  cdt->Set( 4, 100, 75, 99, 65);
  cdt->Set( 5, 100, 75, 99, 65);
  cdt->Set( 6, 100, 75, 99, 65);
  cdt->Set( 7, 100, 75, 99, 65);
  cdt->Set( 8, 100, 75, 99, 65);
  cdt->Set( 9, 100, 75, 99, 65);
  cdt->Set(10, 100, 75, 99, 65);
  cdt->Set(11, 100, 75, 99, 65);
  cdt->Set(12, 100, 75, 99, 65);
  cdt->Set(13, 100, 75, 99, 65);
  cdt->Set(14, 100, 75, 99, 65);
  cdt->Set(15, 100, 75, 99, 65);
  cdt->Set(16, 100, 75, 99, 65);
  cdt->Set(17, 100, 75, 99, 65);
  cdt->Set(18, 100, 75, 99, 65);
  cdt->Set(19, 100, 75, 99, 65);
  cdt->Set(20, 100, 75, 99, 65);
  cdt->Set(21, 100, 75, 99, 65);
  cdt->Set(22, 100, 75, 99, 65);
  cdt->Set(23, 100, 75, 99, 65);
  cdt->Set(24, 100, 75, 99, 65);
  cdt->Set(25, 100, 75, 99, 65);
  cdt->Set(26, 100, 75, 99, 65);
  cdt->Set(27, 100, 75, 99, 65);
  cdt->Set(28, 100, 75, 99, 65);
  cdt->Set(29, 100, 75, 99, 65);
  cdt->Set(30, 100, 75, 99, 65);
  cdt->Set(31, 100, 75, 99, 65);



  result = OPENR::SetCdtVectorData(cdtVecID);
  if (result != oSUCCESS) 
    OSYSLOG1((osyslogERROR, "%s : %s %d","ImageObserver::SetCdtVectorDataOfPinkBall()", "OPENR::SetCdtVectorData() FAILED", result));
  
  result = OPENR::DeleteCdtVectorData(cdtVecID);
  if (result != oSUCCESS) 
    OSYSLOG1((osyslogERROR, "%s : %s %d", "ImageObserver::SetCdtVectorDataOfPinkBall()", "OPENR::DeleteCdtVectorData() FAILED", result));
  
}

Motion::MotionCommand StuffBall::search(OFbkImageVectorData& imageVec, double pan, double tilt2, double tilt, OCdtChannel channel, int threshold, int ForceScanNear){

  Motion::MotionCommand cmd;
  memset(&cmd, 0, sizeof(cmd));

  OFbkImageInfo* info = imageVec.GetInfo(ofbkimageLAYER_C);
  byte*          data = imageVec.GetData(ofbkimageLAYER_C);
  OFbkImage cdtImage(info, data, ofbkimageBAND_CDT);

  switch(_state){

  case IDLE:
  break;

  case TRACKED:
    _state=SEARCHING;

  case SEARCHING:
    {

      dTrackAngles = 1024;
      cmd.motion_cmd = Motion::MOTION_STAND_NEUTRAL;
      cmd.tail_cmd   = Motion::TAIL_FOLLOW;

      if (ForceScanNear == 2){

         // Imposto la testa ferma rivolta verso il basso
	 cmd.head_cmd = Motion::HEAD_ANGLES;
	 cmd.bound_mode=Motion::BOUND_GOTO_EGO;
	 cmd.head_tilt_offset=RAD(0);
	 cmd.head_tilt  = RAD(0);
	 cmd.head_pan   = RAD(0);
	 cmd.head_roll  = RAD(-60);
	 cmd.head_tilt2 = RAD(-60);
      } else if (channel == ocdtCHANNEL1) {	// Se cerco il GIALLO

	// Se cerco il GIALLO (Dosso) guardo in basso
         cmd.head_cmd = Motion::HEAD_SCAN_BALL_NEAR;
      } else if ( channel == ocdtCHANNEL0) {	// Se cerco il BLU

	// Se cerco il BLU (Porta) guardo ad un'altezza media
         cmd.head_cmd = Motion::HEAD_SCAN_OBSTACLES;
      } else {					// Se cerco ROSSO o VERDE

	// Se cerco il Rosso o il Verde guardo in alto
	 cmd.head_cmd = Motion::HEAD_SCAN_MARKERS_HIGH;
      }

	/**  Se trovo una grande quantità di pixel nell'immagine ipotizzo che il mio marker sia vicino */
      if (cdtImage.ColorFrequency(channel) >= (threshold * 3)){
	 found = 10;
	 OSYSDEBUG(("Found a near marker\n"));
      } else if (cdtImage.ColorFrequency(channel) >= threshold){
	 found++;
	 OSYSDEBUG(("pixel trovati: %d\n", cdtImage.ColorFrequency(channel)));
      } else {
	found = 0;
      }

      if (found >= FOUND_THRESHOLD) 
	{
	  found = 0;
	  _state=TRACKING;
	}
    break;
    }

    // Se ho individuato la palla nell'immagine
  case TRACKING:
    {
	
      if (cdtImage.ColorFrequency(channel) >= threshold) 
      {
	  if (!ForceScanNear)	// Se non sto sul Dosso calcolo il dTrack al solito modo
	  	dTrackAngles = fabs(pan-oldPan) + fabs(tilt - oldTilt) + fabs(tilt2-oldTilt2);
	  else 			// Altrimenti lo calcolo non considerando il tilt2
		dTrackAngles = fabs(pan-oldPan) + fabs(tilt - oldTilt);

	  lost=0;
	  double dPan, dTilt2;

	  if (ForceScanNear > 0 && BumpCentroid(cdtImage, &dPan, &dTilt2, channel)){
	     // Se entro qua non devo calcolare il centroide al modo classico ma considerando che ci sono due strisce

	     pan+=dPan;
	     tilt2+=dTilt2;

	     if(tilt2 <= AiboErs7Spec::HEAD_TILT2_MIN)
		tilt-=5;
	     if(tilt2 >= AiboErs7Spec::HEAD_TILT2_MAX)
		tilt+=5;

	     cmd.motion_cmd = Motion::MOTION_STAND_NEUTRAL;
	     cmd.tail_cmd   = Motion::TAIL_FOLLOW;
	     cmd.bound_mode = Motion::BOUND_GOTO_EGO;
	     cmd.head_cmd   = Motion::HEAD_ANGLES;
	     cmd.head_tilt_offset=RAD(0);
	     cmd.head_tilt=RAD(tilt); 
	     cmd.head_pan=RAD(pan);  
	     cmd.head_roll=RAD(tilt2); 
	     cmd.head_tilt2=RAD(tilt2);

	     oldPan=pan;
	     oldTilt=tilt;
	     oldTilt2=tilt2;

	     isTrackedBump();
	     break;
	  }

	  if (CentroidAndDeltaAngle(cdtImage, &dPan, &dTilt2, channel)){

	      pan+=dPan;
	      tilt2+=dTilt2;

	      if(tilt2 <= AiboErs7Spec::HEAD_TILT2_MIN)
		tilt-=5;
	      if(tilt2 >= AiboErs7Spec::HEAD_TILT2_MAX)
		tilt+=5;

	      cmd.motion_cmd=Motion::MOTION_STAND_NEUTRAL; 
	      cmd.head_cmd=Motion::HEAD_ANGLES;
	      cmd.tail_cmd=Motion::TAIL_FOLLOW;
	      cmd.bound_mode=Motion::BOUND_GOTO_EGO;
	      cmd.head_tilt_offset=RAD(0);
	      cmd.head_tilt=RAD(tilt);
	      cmd.head_pan=RAD(pan);
	      cmd.head_roll=RAD(tilt2);
	      cmd.head_tilt2=RAD(tilt2);

	      oldPan=pan;
	      oldTilt=tilt;
	      oldTilt2=tilt2;

	      if(isTracked())
		_state=TRACKED;
	      break;
	   }
      }

      lost++;
      cmd.motion_cmd=Motion::MOTION_STAND_NEUTRAL;
      cmd.head_cmd=Motion::HEAD_NO_CMD;
      cmd.tail_cmd=Motion::TAIL_FOLLOW;

      if (lost == LOST_THRESHOLD) 
	{
	  lost = 0;
	  _state=SEARCHING;
	  Wait(WAIT_AFTER_LOST_BALL);
	}
    }
  break;
  }

  return cmd;
}




bool StuffBall::isTracked(){
  if(_state == TRACKING)
    if((dTrackAngles/3) < 1.5)
      _state=TRACKED;
  return (_state == TRACKED);
}

bool StuffBall::isTrackedBump(){
  if(_state == TRACKING)
    if((dTrackAngles/2) < 1.8)
      _state=TRACKED;
  return (_state == TRACKED);
}

double StuffBall::getVisualDistance(OFbkImageVectorData& imageVec, OCdtChannel channel){
  //
  // Implementazione DUMMY... da completare.
  //
  double distance;
  if(_state == TRACKED)
    {
      OFbkImageInfo* info = imageVec.GetInfo(ofbkimageLAYER_C);
      byte*          data = imageVec.GetData(ofbkimageLAYER_C);
      OFbkImage cdtImage(info, data, ofbkimageBAND_CDT);
      byte* ptr = cdtImage.Pointer();
      
      int w     = cdtImage.Width();
      int h     = cdtImage.Height()-1;
      int diameters[w];
      memset(diameters, 0, sizeof(diameters));
      
      for (int y = 0; y < h; y++) 
	for (int x = 0; x < w; x++) 
	  if (*ptr++ & channel) 
	    diameters[y]++;

      int maxDiameter=0;
      for(int i=0; i < w; i++)
	if(diameters[i] > maxDiameter)
	  maxDiameter=diameters[i];
      
      // 
      // Fai i vari calcoli 
      //
      OSYSDEBUG(("Diametro calcolato: %d (%dx%d)\n", maxDiameter, w,h+1));

      distance=125;
    }
  else
    distance=-1;
  
  return distance;
}


bool StuffBall::CentroidAndDeltaAngle(const OFbkImage& cdtImage, double* pDeltaPan, double* pDeltaTilt2, OCdtChannel channel){
  int w     = cdtImage.Width();
  int h     = cdtImage.Height();
  byte* ptr = cdtImage.Pointer();
  
  int xsum   = 0;
  int ysum    = 0;
  int npixels = 0;
  
  // Last line is not used because of TagInfo.  Scorro l'immagine
  // come gli elettroni nel tubo catodico
  for (int y = 0; y < h-1; y++) 
    for (int x = 0; x < w; x++) 
      if (*ptr++ & channel) 
	{ 
	  // Se il pixel e' del colore giusto
	  xsum += x;
	  ysum += y;
	  npixels++;
	}
  
  if (npixels != 0) 
    {
      // praticamente calcola le medie dei punti in x e in y
      int xcentroid  = xsum / npixels; 
      int ycentroid  = ysum / npixels;
      
      // Ora in xcentroid e ycentroid ho piu' o meno le coordinate
      // del centro della palla espresse in x e y
       
      //(xcentroid - (w/2): prendo il centroid, quindi divido la
      //width dell'immagine per due (significa, la testa e' a meta'
      //del sistema di riferimento) a questo punto se e' positivo e'
      //alla sua dx, altrimenti e' alla sua sx.
      *pDeltaPan  = -1.0 * AiboErs7Spec::FIELD_VIEW_H * (xcentroid - w/2.0) / w;
      *pDeltaTilt2 = -1.0 * AiboErs7Spec::FIELD_VIEW_V * (ycentroid - h/2.0) / h;
  
      return true;
    } 
  else 
    {
      *pDeltaPan  = 0.0;
      *pDeltaTilt2 = 0.0;
      return false;
    }
}


bool StuffBall::BumpCentroid(const OFbkImage& cdtImage, double* pDeltaPan, double* pDeltaTilt2, OCdtChannel channel){
  int w     = cdtImage.Width();
  int h     = cdtImage.Height();
  byte* ptr = cdtImage.Pointer();
  
  int xsum    = 0;
  int ysum    = 0;
  int npix_x  = 0;
  int npix_y  = 0;

  int tollB   = 11;				// Tolleranza in pixel prima di considerare terminata la fascia
  int tollY   = 5;				// Tolleranza di pixel prima di considerare visto il GIALLO
  int flag    = 0;				// Numero di fasce GIALLE analizzate
  int x_2     = 0;				// Coordinata x della seconda fascia GIALLA
  int blackP  = 0;				// Numero di pixel Neri visti
  int yelloP  = 0;				// Numero di pixel GIALLI visti

  bool YCount = true;				// Completatore della prima fascia GIALLA

  // Last line is not used because of TagInfo.  Scorro l'immagine
  // come gli elettroni nel tubo catodico
  for (int y = (h / 3); y < h-1; y++){		// Altezza

	for (int x = 0; x < w; x++){		// Larghezza

		if ((*ptr & channel) && flag == 0 && yelloP > tollY){	
		  // Se è GIALLO e non avevo mai visto GIALLI

			flag++;			// Ho visto un pixel GIALLO
			yelloP = 0;		// Resetto il numero di Pixel GIALLI visti
			YCount = false;
		} else if ((*ptr & channel) && blackP > tollB && yelloP > tollY && flag == 1){
		  // Se è GIALLO, è il primo pixel ed è la seconda fascia

		 	flag++;			// Ho visto un pixel GIALLO
		 	x_2 = x - yelloP;	// Mi segno la posizione del pixel finale
			while (x < (w - 1)){
				x++;		// Forzo l'uscita dal for
				*ptr++;		// Aggiorno contestualmente anche ptr altrimenti punta ad un pixel errato
			}
			/** Quando sia x che ptr hanno lo stesso valore di w-1 allora termino il for incrementando 
			    un'ultima volta x e ptr, x rivalutata sara = w e al prossimo y, x=0 ptr=w*y */
			
		} else if (!(*ptr & channel) && flag == 1){

			blackP++;		// Ho visto un pixel Nero
			//yelloP = 0;
		} else if ((*ptr & channel) && flag == 1 && blackP <= tollB){

			blackP = 0;		// Ho trovato un pixel giallo, resetto il conteggio dei pixel neri
		} else if ((*ptr & channel) && yelloP <= tollY && YCount){ // && flag == 0 
		  // Se ho visto un pixel GIALLO e i pixel visti sono minori della tolleranza
			yelloP++;		// Ho visto un pixel GIALLO
		}
		
		if (blackP > tollB) // Superata la tolleranza dei neri riabilito il contatore dei GIALLI
			YCount = true;
		*ptr++;
	} // END secondo FOR
	if (flag == 2){				// Se ho trovato due fasce di colore GIALLO

		npix_y++;			// Conto quante righe sto elaborando
		ysum += y;			// Sommo le coordinate delle y
		npix_x++;			// Conto quante colonne sto elaborando
		xsum += ((x_2 - blackP) + x_2) / 2;	// Faccio la somma delle due Coordinate x diviso 2
	} // END IF

	flag   = 0;
	x_2    = 0;
	yelloP = 0;
	blackP = 0;
  }  // END primo FOR
  
  if (xsum != 0) {

      // praticamente calcola le medie dei punti in x e in y
      int xcentroid  = xsum / npix_x; 
      int ycentroid  = ysum / npix_y;
      //xcentroid = xcentroid - 20;		// Correggo la tendenza del centroide ad essere verso Dx
      OSYSDEBUG(("INFO: Centroid coordinates: x %d  y %d\n", xcentroid, ycentroid));
      // Ora in xcentroid e ycentroid ho piu' o meno le coordinate
      // del centro della palla espresse in x e y
      //(xcentroid - (w/2): prendo il centroid, quindi divido la
      //width dell'immagine per due (significa, la testa e' a meta'
      //del sistema di riferimento) a questo punto se e' positivo e'
      //alla sua dx, altrimenti e' alla sua sx.
      *pDeltaPan  = -1.0 * AiboErs7Spec::FIELD_VIEW_H * (xcentroid - w/2.0) / w;
      *pDeltaTilt2 = -1.0 * AiboErs7Spec::FIELD_VIEW_V * (ycentroid - h/2.0) / h;
  
      return true;
    } else {
	OSYSDEBUG(("WARNING: No centroid found\n"));
      *pDeltaPan  = 0.0;
      *pDeltaTilt2 = -2.0;
      return false;
    }
}
