#ifndef STUFFBALL_H_DEFINED
#define STUFFBALL_H_DEFINED
#include "Stuff.h"
#include "AiboErs7Spec.h"

/** Oggetto per la ricerca della AiboBall. Implementa la classe Stuff
    come interfaccia per l'oggetto Open-R Occipital. Ricerca la palla
    effettuando (nella versione attuale) una ricerca in base al colore
    della palla.*/
class StuffBall : public Stuff{
public:
  /** Costruttore di default.*/
  StuffBall() : Stuff(), lost(0), found(0), oldPan(-1024), oldTilt2(-1024), oldTilt(-1024), dTrackAngles(1024) { _type=BALL; _state=SEARCHING; };
  /** Distruttore di default.*/
  virtual ~StuffBall() {};

  /** Effettua alcune inizializzazioni necessarie alla richerca della
      stuff.*/
  void init(OPrimitiveID& fbkID);
  /** Dati gli angoli attuali della testa ricerca la palla e ritorna
      il comando da mantare a Motion per puntare lo stuff.*/
  Motion::MotionCommand search(OFbkImageVectorData& imageVec, double pan, double tilt2, double tilt, OCdtChannel channel, int threshold, int ForceScanNear);
  /** Controlla se la stuff e' stata correttamente individuata.*/
  bool isTracked();
  /** Controlla se la stuff e' stata correttamente individuata nel Dosso.*/
  bool isTrackedBump();
  /** Ritorna la distanza dalla stuff calcolata in base all'immagine
      (DUMMY).*/
  double getVisualDistance(OFbkImageVectorData& imageVec, OCdtChannel channel);

  /** Calcola la distanza tra le due bande gialle e ne restituisce il pan del centro */
  bool BumpCentroid(const OFbkImage& cdtImage, double* pDeltaPan, double* pDeltaTilt2, OCdtChannel channel);

private:
  /* Numero di frame in cui la palla dev'essere stata vista per essere
     considerata trovata. Serve per il passaggio dallo stato SEARCHING allo stato TRACKING.*/
  static const int         FOUND_THRESHOLD    = 3;
  /* Numero di frame in cui la palla non dev'essere piu' vista
     nell'angolo di visone della telecamera prima di dichiararla
     persa. Serve per il passaggio dallo stato TRACKING allo stato
     SEARCHING.*/
  static const int         LOST_THRESHOLD     = 10;
  /* Width dell'immagine in Layer C*/
  static const int         CAMERA_IMAGE_W     = 104;
  /* Height dell'immagine in Layer C*/
  static const int         CAMERA_IMAGE_H     = 80;
  /* Tempo di attesa dopo aver perso la palla in nano secondi. Serve
     per evitare di incominicare immediatamente dopo aver perso la
     palla un ciclo di ricerca (comando HEAD_SCAN_BALL).*/
  static const longword  WAIT_AFTER_LOST_BALL = 1000000;

  /* Contatore che memorizza il numero di volte in cui la palla non e'
     stata vista. #see LOST_THRESHOLD.*/
  int lost;
  /* Contatore che memorizza il numero di volte in cui la palla e'
     stata vista. @see FOUND_THRESHOLD.*/
  int found;


  /* Vecchio valore del joint pan della testa. Usato per calcolare gli
     spostamenti effettuati sugli angoli della testa.*/
  double oldPan;
  /* Vecchio valore del joint tilt2 (o roll) della testa.Usato per
     calcolare gli spostamenti effettuati sugli angoli della testa.*/
  double oldTilt2;
  /* Vecchio valore del joint tilt della testa. Usato per calcolare
     gli spostamenti effettuati sugli angoli della testa.*/
  double oldTilt;

  /* Memorizza la somma delle differenze tra gli angoli attualmente
     mandati alla testa dell'Aibo e i vecchi angoli mandati alla
     testa. Serve a calcolare quanto i nuovi comandi siano diversi dai
     precedenti permettendo di capire se la palla e' stata trovata o
     meno.*/
  double dTrackAngles;
private:
  /* Funzione principale per l'individuazione della palla. Calcola il
     centroide del'immagine riconosciuta in base ai colori impostati
     per la palla, sulla base di tale valore calcola quando bisogna
     spostare il joint pan e il joint tilt per centrare l'oggetto al
     centro dell'immagine. @param cdtImage l'immagine. @param
     pDeltaPan restituira' il delta da applicare al joint pan per
     centrare l'immagine. @param pDeltaTil2 restituira' il delta da
     applicare al joint tilt2 (o roll) per centrare
     l'immagine. @return true se l'oggetto cercato e' stato
     trovato. @false altrimenti.*/
  bool CentroidAndDeltaAngle(const OFbkImage& cdtImage, double* pDeltaPan, double* pDeltaTilt2, OCdtChannel channel);

};


#endif
