//
// Copyright 2002,2004 Sony Corporation 
//
// Permission to use, copy, modify, and redistribute this software for
// non-commercial use is hereby granted.
//
// This software is provided "as is" without warranty of any kind,
// either expressed or implied, including but not limited to the
// implied warranties of fitness for a particular purpose.
//

#ifndef OCCIPITAL_H_DEFINED
#define OCCIPITAL_H_DEFINED

using namespace std;

#include <OPENR/OObject.h>
#include <OPENR/OSubject.h>
#include <OPENR/OObserver.h>
#include <OPENR/ODataFormats.h>
#include <OPENR/OFbkImage.h>
#include "def.h"
#include "StuffBall.h"
#include <list>
#include <vector>

/** Stati di occipital*/
enum OccipitalState {
    IDLE,
    SEARCHING,
};

/** Primitiva della telecamera.*/
static const char* const FBK_LOCATOR = "PRM:/r1/c1/c2/c3/i1-FbkImageSensor:F1";
/** Alcune primitive di sensori utili.*/
static const char* const SENSOR_LOCATOR[] = {
    "PRM:/r1/c1/c2-Joint2:12",   // HEAD PAN
    "PRM:/r1/c1/c2/c3-Joint2:13", // HEAD TILT2
    "PRM:/r1/c1-Joint2:11", // HEAD TILT
    "PRM:/r1/c1/c2/c3/p1-Sensor:p1",    // HEAD PSD (NEAR)
    "PRM:/r1/c1/c2/c3/p2-Sensor:p2"    // HEAD PSD (FAR)
};

class Occipital : public OObject {
public:
    Occipital();
    virtual ~Occipital() {}

    OSubject*  subject[numOfSubject];
    OObserver* observer[numOfObserver];

    virtual OStatus DoInit   (const OSystemEvent& event);
    virtual OStatus DoStart  (const OSystemEvent& event);
    virtual OStatus DoStop   (const OSystemEvent& event);
    virtual OStatus DoDestroy(const OSystemEvent& event);


    /** Invocata al ricevimento di una nuova immagine dalla telecamera.*/
    void NotifyImage(const ONotifyEvent& event);
    /** Invocata al ricevimento di nuove informazioni sui sensori.*/
    void NotifySensor(const ONotifyEvent& event);
    /** Invocata quando Motion risulta pronto a ricevere comandi.*/
    void ReadyMotion(const OReadyEvent& event) {};
    /** Invocata al ricevimento di un nuovo comando.*/
    void SearchCommand(const ONotifyEvent& event);

    void Occipital::TimerEnd(void* msg);

private:
    static const size_t      NUM_SENSOR_VECTOR  = 2;
    static const size_t      NUM_SENSORS        = 5;

    enum
      {
	PAN_INDEX= 0,
	TILT2_INDEX= 1,
	TILT_INDEX = 2,
	HEAD_IRSENSOR_NEAR=3,
	HEAD_IRSENSOR_FAR=4,
      };  
   


 private:

    /* Indice del canale di riconoscimento CDT */
    int  ChanIndex;
    int  GateAttraversati;		// Contatore dei Gate attraversati
    int  FrameViewed;			// Contatore dei Frame analizzati
    bool RedMarkerFound;		// Flag per indicare la scoperta di un marker ROSSO
    bool TimerReset;			// Flag per il resettamento del timer
    int  ForceScanNear;			// Per forzare lo Scan della testa in HEAD_SCAN_BALL_NEAR
    static const int FrameTimer = 2;	// Timer di scadenza per la ricerca dei canali CDT in secondi
    static const string colori[];
    static const int threshold[];
    static const unsigned int primitive[];


    /** Apre le primitive.*/
    void OpenPrimitives();


    /** Imposta i parametri della telecamera (gain, wb, e exposition).*/
    void SetCameraParameter();
    /** Inizializzo gli indici dei sensori.*/
    void InitSensorIndex(OSensorFrameVectorData* sensorVec);
    /** Ritorna gli angoli della testa.*/
    void GetPanTiltAngle(longword frameNum, double* pan, double* tilt,  double* tilt2);
    /** Ritorna le letture dai sensori di distanza.*/
    void GetIrHeadDistance(longword frameNum, int* near, int* far);
    /** Converte l'angolo di pan da gradi a ore (in stile militare).*/
    double pan2Hour(double pan){
      return ((pan * -1) * 12) / 360;
    };
    

 private:
    OccipitalState state;
    OPrimitiveID fbkID;
    OPrimitiveID jointID[NUM_SENSORS];
    bool initSensorIndex;
    int sensoridx[NUM_SENSORS];
    list<RCRegion*> sensorRegions;
    double lastRefPan;
    double lastRefTilt;
    Stuff *searchedStuff;
    vector<Stuff*> knownStuff;
};
/*  _____ ___  ____   ___   */
/* |_   _/ _ \|  _ \ / _ \  */
/*   | || | | | | | | | | | */
/*   | || |_| | |_| | |_| | */
/*   |_| \___/|____/ \___/  */

#endif 
