/************************************************************************************
HumanRecog RT-Component
Copyright (c) 2010, Segway Japan, Ltd.
All rights reserved.

Contact us if you use this software for sell.
If you use this software not for sell, you can use this software under BSD lisence.
See the files LICENSE.TXT and LICENSE-BSD.TXT for more details.                     
************************************************************************************/
#ifndef HUMANRECOG_H
#define HUMANRECOG_H

#include <rtm/Manager.h>
#include <rtm/DataFlowComponentBase.h>
#include <rtm/CorbaPort.h>
#include <rtm/DataInPort.h>
#include <rtm/DataOutPort.h>
#include <rtm/idl/BasicDataTypeSkel.h>

#include "intellirobotStub.h"

#ifndef WIN32
#include "x11win.h"
#endif

// Service implementation headers
// <rtc-template block="service_impl_h">

// </rtc-template>

// Service Consumer stub headers
// <rtc-template block="consumer_stub_h">

// </rtc-template>

using namespace RTC;

class HumanRecog
  : public RTC::DataFlowComponentBase
{
 public:
  HumanRecog(RTC::Manager* manager);
  ~HumanRecog();

  // The initialize action (on CREATED->ALIVE transition)
  // formaer rtc_init_entry()
 virtual RTC::ReturnCode_t onInitialize();

  // The finalize action (on ALIVE->END transition)
  // formaer rtc_exiting_entry()
  virtual RTC::ReturnCode_t onFinalize();

  // The startup action when ExecutionContext startup
  // former rtc_starting_entry()
  // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);

  // The shutdown action when ExecutionContext stop
  // former rtc_stopping_entry()
  // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);

  // The activated action (Active state entry action)
  // former rtc_active_entry()
  virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);

  // The deactivated action (Active state exit action)
  // former rtc_active_exit()
  virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);

  // The execution action that is invoked periodically
  // former rtc_active_do()
  virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);

  // The aborting action when main logic error occurred.
  // former rtc_aborting_entry()
  // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);

  // The error action in ERROR state
  // former rtc_error_do()
  // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);

  // The reset action that is invoked resetting
  // This is same but different the former rtc_init_entry()
  // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);

  // The state update action that is invoked after onExecute() action
  // no corresponding operation exists in OpenRTm-aist-0.2.0
  // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);

  // The action that is invoked when execution context's rate is changed
  // no corresponding operation exists in OpenRTm-aist-0.2.0
  // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);


 protected:
  // Configuration variable declaration
  // <rtc-template block="config_declare">
  int m_imagew;
  int m_imageh;
  double m_grayS;
  double m_grayK;
  double m_matchvalue;
  int m_matchblur;
  int m_matchw;
  int m_matchh;
  int m_searchw;
  int m_searchh;
  int m_scales;
  int m_scalee;
  int m_addx;
  int m_addy;
  int m_adds;
  double m_maxjump;
  double m_angleofview0;
  double m_angleofview1;
  int m_angleofscan;
  double m_scanradadd;
  std::string m_str_port;
  int m_idIIS;
  // </rtc-template>

  // DataInPort declaration
  // <rtc-template block="inport_declare">
  TimedULong m_Input;
  InPort<TimedULong> m_InputIn;
  TimedULongSeq m_Image;
  InPort<TimedULongSeq> m_ImageIn;
  TimedUShortSeq m_ImageYUYV;
  InPort<TimedUShortSeq> m_ImageYUYVIn;
  TimedLongSeq m_Scan;
  InPort<TimedLongSeq> m_ScanIn;
  IIS::TimedPose2D m_RobotPose2DIIS;
  InPort<IIS::TimedPose2D> m_RobotPose2DIISIn;
  
  // </rtc-template>

  // DataOutPort declaration
  // <rtc-template block="outport_declare">
  IIS::TimedPose2D m_HumanPose2DIIS;
  OutPort<IIS::TimedPose2D> m_HumanPose2DIISOut;
  
  // </rtc-template>

  // CORBA Port declaration
  // <rtc-template block="corbaport_declare">
  
  // </rtc-template>

  // Service declaration
  // <rtc-template block="service_declare">
  
  // </rtc-template>

  // Consumer declaration
  // <rtc-template block="consumer_declare">
  
  // </rtc-template>

 private:
  int dummy;

#ifndef WIN32
  x11win win;
#endif

  //image
  int imagew;
  int imageh;

  unsigned long* image;

  const static int histimageh = 240;
  unsigned long histimage[360 * histimageh];

  float* Hbuf;
  float* Sbuf;
  float* Vbuf;

  float* HbufPtr;
  float* SbufPtr;
  float* VbufPtr;

  float Histbuf[2][360+256];//H(360) + V(256)

  //matching
  int matchw;
  int matchh;
  int searchw;
  int searchh;
  int scales;
  int scalee;
  int addx;
  int addy;
  int adds;

  int matchx;//match result x
  int matchy;//match result y
  int matchs;//match result scale

  //human pos
  float humanx;
  float humany;
  float humanang;

  bool firstdetect;

  float match;
  float matchmin;
  bool matchresult;

  bool getrobotpos;

  bool target;

  //laser
  const static int limagew = 320;
  const static int limageh = 240;
  long scan[1081];
  unsigned long scanimage[limagew*limageh];

  const static int PIXEL_PER_METER = 100;

  void drawpos(float x, float y);
  void drawlaser();
  void drawmatch(unsigned char* buf, bool detecttarget, bool matchnotlost);
  void drawhist(float* histbuf, int y, float scale);
  void drawhistgray(float* histbuf, int y, float scale);

  void clearhist();

  float matching_hist();
  float gethumanang(int matchx, int imagew, float angofview);
  void gethumanpos(float& humanx, float& humany, float humanang, long* scan);

};


extern "C"
{
  void HumanRecogInit(RTC::Manager* manager);
};

#endif // HUMANRECOG_H
