/************************************************************************************
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.                     
************************************************************************************/
#include <math.h>
#include "HumanRecog.h"

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

#include "v4l2.h"
#include "misc.h"

// Module specification
// <rtc-template block="module_spec">
static const char* humanrecog_spec[] =
  {
    "implementation_id", "HumanRecog",
    "type_name",         "HumanRecog",
    "description",       "HumanRecog Component",
    "version",           "1.0.0",
    "vendor",            "SegwayJapan",
    "category",          "Generic",
    "activity_type",     "SPORADIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "10",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    "conf.default.imagew", "640",
    "conf.default.imageh", "480",
    "conf.default.grayS", "0.06",
    "conf.default.grayK", "1.0",
    "conf.default.matchvalue", "0.7",
    "conf.default.matchblur", "1",
    "conf.default.matchw", "128",
    "conf.default.matchh", "192",
    "conf.default.searchw", "80",
    "conf.default.searchh", "40",
    "conf.default.scales", "0",
    "conf.default.scalee", "0",
    "conf.default.addx", "8",
    "conf.default.addy", "8",
    "conf.default.adds", "1",
    "conf.default.maxjump", "0.5",
    "conf.default.angleofview0", "75.0",
    "conf.default.angleofview1", "45.0",
    "conf.default.angleofscan", "40",
    "conf.default.scanradadd", "0.0",
    "conf.default.str_port", "/dev/video0",
    "conf.default.idIIS", "1000",

    ""
  };
// </rtc-template>am

HumanRecog::HumanRecog(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_InputIn("Input", m_Input),
    m_ImageIn("Image", m_Image),
    m_ImageYUYVIn("ImageYUYV", m_ImageYUYV),
    m_ScanIn("Scan", m_Scan),
    m_RobotPose2DIISIn("RobotPose2DIIS", m_RobotPose2DIIS),
    m_HumanPose2DIISOut("HumanPose2DIIS", m_HumanPose2DIIS),
    // </rtc-template>
	dummy(0)
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  registerInPort("Input", m_InputIn);
  registerInPort("Image", m_ImageIn);
  registerInPort("ImageYUYV", m_ImageYUYVIn);
  registerInPort("Scan", m_ScanIn);
  registerInPort("RobotPose2DIIS", m_RobotPose2DIISIn);
  
  // Set OutPort buffer
  registerOutPort("HumanPose2DIIS", m_HumanPose2DIISOut);
  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  
  // </rtc-template>

  matchw = 64;
  matchh = 64;
  searchw = 80;
  searchh = 80;
  scales = 0;
  scalee = 0;
  addx = 4;
  addy = 4;
  adds = 1;

  image = NULL;
  Hbuf = NULL;
  Sbuf = NULL;
  Vbuf = NULL;

}

HumanRecog::~HumanRecog()
{
}



RTC::ReturnCode_t HumanRecog::onInitialize()
{
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("imagew", m_imagew, "640");
  bindParameter("imageh", m_imageh, "480");
  bindParameter("grayS", m_grayS, "0.06");
  bindParameter("grayK", m_grayK, "1.0");
  bindParameter("matchvalue", m_matchvalue, "0.7");
  bindParameter("matchblur", m_matchblur, "1");
  bindParameter("matchw", m_matchw, "128");
  bindParameter("matchh", m_matchh, "192");
  bindParameter("searchw", m_searchw, "80");
  bindParameter("searchh", m_searchh, "40");
  bindParameter("scales", m_scales, "0");
  bindParameter("scalee", m_scalee, "0");
  bindParameter("addx", m_addx, "8");
  bindParameter("addy", m_addy, "8");
  bindParameter("adds", m_adds, "1");
  bindParameter("maxjump", m_maxjump, "0.5");
  bindParameter("angleofview0", m_angleofview0, "75.0");
  bindParameter("angleofview1", m_angleofview1, "45.0");
  bindParameter("angleofscan", m_angleofscan, "40");
  bindParameter("scanradadd", m_scanradadd, "0.0");
  bindParameter("str_port", m_str_port, "/dev/video0");
  bindParameter("idIIS", m_idIIS, "1000");
  // </rtc-template>


  return RTC::RTC_OK;
}




RTC::ReturnCode_t HumanRecog::onFinalize()
{

  return RTC::RTC_OK;
}


/*
RTC::ReturnCode_t HumanRecog::onStartup(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t HumanRecog::onShutdown(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/


RTC::ReturnCode_t HumanRecog::onActivated(RTC::UniqueId ec_id)
{

  memset(scan, 0, sizeof(long)*1081);

  imagew = m_imagew;
  imageh = m_imageh;

  if (image == NULL) {
    image = (unsigned long*) malloc(sizeof(unsigned long) * imagew * imageh);
  }
  if (Hbuf == NULL) {
    Hbuf= (float*) malloc(sizeof(float) * imagew * imageh);
  }
  if (Sbuf == NULL) {
    Sbuf= (float*) malloc(sizeof(float) * imagew * imageh);
  }
  if (Vbuf == NULL) {
    Vbuf= (float*) malloc(sizeof(float) * imagew * imageh);
  }

#ifndef WIN32
  win.start(0, NULL, 100+imagew,100,imagew+360,480+20, "RTC HumanRecog");
#endif

  matchw = m_matchw;
  matchh = m_matchh;
  searchw = m_searchw;
  searchh = m_searchh;
  scales = m_scales;
  scalee = m_scalee;
  addx = m_addx;
  addy = m_addy;
  adds = m_adds;

  matchx = 0;
  matchy = 0;
  matchs = 0;

  HbufPtr = NULL;
  SbufPtr = NULL;
  VbufPtr = NULL;

  firstdetect = true;


  match = 0.0f;
  matchresult = false;

  getrobotpos = false;

  target = false;

  clearhist();

  return RTC::RTC_OK;
}

RTC::ReturnCode_t HumanRecog::onDeactivated(RTC::UniqueId ec_id)
{
#ifndef WIN32
  win.end();
#endif

  if (image) {
    free(image);
    image = NULL;
  }
  if (Hbuf) {
    free(Hbuf);
    Hbuf = NULL;
  }
  if (Sbuf) {
    free(Sbuf);
    Sbuf = NULL;
  }
  if (Vbuf) {
    free(Vbuf);
    Vbuf = NULL;
  }


  return RTC::RTC_OK;
}


void HumanRecog::clearhist()
{
  for (int i = 0; i < 360+256; i++) {
    Histbuf[0][i] = 0.0f;
    Histbuf[1][i] = 0.0f;
  }
}


//--------------------------------
// HSV Converter
//--------------------------------
inline float Max(float a,float b,float c)
{
  if(a >= b && a >= c)
    return a;
  if(b >= a && b >= c)
    return b;
  return c;
}

inline float Min(float a,float b,float c)
{
  if(a <= b && a <= c)
    return a;
  if(b <= a && b <= c)
    return b;
  return c;
}

inline void RGBtoHSV(unsigned long* xrgb, float& H, float& S, float& V)
{
  float	R;
  float	G;
  float	B;
  float	max;
  float	min;
  float	diff;

  //RGB->HSV変換時のR,G,Bは 0.0～1.0
  R = (float) (((*xrgb) & 0x0000FF00) >> 8);
  G = (float) (((*xrgb) & 0x00FF0000) >> 16);
  B = (float) (((*xrgb) & 0xFF000000) >> 24);

  R /= 255.0f;
  G /= 255.0f;
  B /= 255.0f;

  max = Max(R,G,B);
  min = Min(R,G,B);

  if (max == 0) {
      S = 0.0f;
      V = 0.0f;
      return;
  }

  diff = max - min;
  V = max;
  S = diff / max;
  //dS = dMax - dMin;			//円錐色空間時のS

  if (diff == 0) {
    H = 0.0f;
    return;
  }

  if(R == max) {
    H = 60.0f * (G - B) / diff;
  }
  else if(G == max) {
    H = 60.0f * (B - R) / diff + 120.0f;
  }
  else {
    H = 60.0f * (R - G) / diff + 240.0f;
  }

  if (H < 0.0f) {
    H = H + 360.0f;
  }
}


void GetHSV(unsigned long* xrgb, int w, int h, float* Hbuf, float* Sbuf, float* Vbuf)
{
  for (int i = 0; i < h; i++) {
    for (int j = 0; j < w; j++) {

      int idx = w * i + j;

      unsigned long* p = &xrgb[idx];
      float H = 0.0f;
      float S = 0.0f;
      float V = 0.0f;

      RGBtoHSV(p, H, S, V);

      if (Hbuf) {
	Hbuf[idx] = H;
      }
      if (Sbuf) {
	Sbuf[idx] = S;
      }
      if (Vbuf) {
	Vbuf[idx] = V;
      }
    }
  }
}






inline void gethist(float* hist, float* hbuf, float* sbuf, float* vbuf,
		    int x, int y, int w, int h, int iw, int ih,
		    double grayS, double grayK, int matchblur)
{
  memset(hist, 0, sizeof(float)*(360+256));

  int sx = x;
  int sy = y;
  int ex = x+w;
  int ey = y+h;
  sx = (sx < 0)?   0 : sx;
  sx = (sx > iw)? iw : sx;
  sy = (sy < 0)?   0 : sy;
  sy = (sy > ih)? ih : sy;
  ex = (ex < 0)?   0 : ex;
  ex = (ex > iw)? iw : ex;
  ey = (ey < 0)?   0 : ey;
  ey = (ey > ih)? ih : ey;


  for (int i = sy; i < ey; i++) {
    for (int j = sx; j < ex; j++) {

      int idx = iw * i + j;

      if (sbuf && vbuf) {
	//----------------------
	//use s v gray scale
	//----------------------
	float s = sbuf[idx];
	float v = vbuf[idx];

	if (s < (float)grayS) {
	  int vi = (int) (v * 255.0f);

	  hist[360+vi] += (float) grayK;
	}
	else {
	  float h = hbuf[idx];
	  int hi = (int) ceilf(h);
	  hist[hi] += 1.0f;
	}
      }
      else {
	//-------------------
	//use h only
	//-------------------
	float h = hbuf[idx];
      	int hi = (int) ceilf(h);
	hist[hi] += 1.0f;
      }
    }
  }

#if 1
  if (matchblur) {
    float temp[360+256];

    //H color
    for (int i = 0; i < 360; i++) {

      float sum = 0.0f;
      float sumidx = 0.0f;

      for (int idx2 = 360 + i - matchblur; idx2 <= 360 + i + matchblur; idx2++){
	sum += hist[idx2 % 360];
	sumidx += 1.0f;
      }
      temp[i] = sum / sumidx;
    }
    for (int i = 0; i < 360; i++) {
      hist[i] = temp[i];
    }

    //V gray
    for (int i = 360; i < 360+256; i++) {

      float sum = 0.0f;
      float sumidx = 0.0f;

      for (int idx2 = i - matchblur; idx2 <= i + matchblur; idx2++){
	if (idx2 < 360 || idx2 >= 360+256) {
	  continue;
	}
	sum += hist[idx2];
	sumidx += 1.0f;
      }
      temp[i] = sum / sumidx;
    }
    for (int i = 360; i < 360+256; i++) {
      hist[i] = temp[i];
    }

  }
#endif
}

inline float dothist(float* hist0, float* hist1, float k0, float k1)
{
  float sum = 0.0f;
  for (int i = 0; i < 360+256; i++) {
    sum += sqrtf(hist0[i] * hist1[i] * k0 * k1);
  }
  return sum;
}








float SCIP2_TOPURG_ONEANG = 0.25f;
int   SCIP2_TOPURG_SAMPLES = 1081;

float LASER_MAXLEN = 10.0f;
float LASER_MINLEN = 0.05f;


void HumanRecog::drawpos(float x, float y)
{
  x *= PIXEL_PER_METER;
  y *= PIXEL_PER_METER;

  int ix, iy;
  ix = (int)x + limagew/2;
  iy = -(int)y + limageh;

  drawline((unsigned char*)scanimage, limagew, limageh, ix-10, iy-10, ix+10, iy+10, 0xFF, 0, 0);
  drawline((unsigned char*)scanimage, limagew, limageh, ix+10, iy-10, ix-10, iy+10, 0xFF, 0, 0);
}

void HumanRecog::drawlaser()
{
  //draw scan image
  memset(scanimage, 0xFF, sizeof(unsigned long) * limagew * limageh);


  drawline((unsigned char*)scanimage, limagew, limageh,
	   0, limageh-PIXEL_PER_METER, limagew, limageh-PIXEL_PER_METER, 0x80, 0x80, 0x80);

  drawline((unsigned char*)scanimage, limagew, limageh,
	   0, limageh-PIXEL_PER_METER*2, limagew, limageh-PIXEL_PER_METER*2, 0x80, 0x80, 0x80);

  drawline((unsigned char*)scanimage, limagew, limageh,
	   0, limageh-1,                 limagew, limageh-1, 0x80, 0x80, 0x80);

  drawline((unsigned char*)scanimage, limagew, limageh,
	   limagew/2, 0, limagew/2, limageh, 0x80, 0x80, 0x80);

  drawline((unsigned char*)scanimage, limagew, limageh,
	   limagew/2-PIXEL_PER_METER, 0, limagew/2-PIXEL_PER_METER, limageh, 0x80, 0x80, 0x80);

  drawline((unsigned char*)scanimage, limagew, limageh,
	   limagew/2+PIXEL_PER_METER, 0, limagew/2+PIXEL_PER_METER, limageh, 0x80, 0x80, 0x80);



  float oneang = (3.141592 / 180.0) * SCIP2_TOPURG_ONEANG;


  float theta = 0.0f;
  theta += m_scanradadd;

  for (int i = 180; i < SCIP2_TOPURG_SAMPLES-180; i++) {

    float x0, y0, x1, y1;
    float laser0;
    float laser1;

    laser0 = (float)scan[i]   / 1000.0;
    laser1 = (float)scan[i+1] / 1000.0;

    laser0 = (laser0 > LASER_MAXLEN)? LASER_MAXLEN : laser0;
    laser1 = (laser1 > LASER_MAXLEN)? LASER_MAXLEN : laser1;

    laser0 = (laser0 < LASER_MINLEN)? LASER_MAXLEN : laser0;
    laser1 = (laser1 < LASER_MINLEN)? LASER_MAXLEN : laser1;


    x0 = laser0 * cosf(theta) * PIXEL_PER_METER;
    y0 = laser0 * sinf(theta) * PIXEL_PER_METER;
    theta += oneang;

    x1 = laser1 * cosf(theta) * PIXEL_PER_METER;
    y1 = laser1 * sinf(theta) * PIXEL_PER_METER;

    int ix0, iy0, ix1, iy1;
    ix0 = (int)x0 + limagew/2;
    iy0 = -(int)y0 + limageh;
    ix1 = (int)x1 + limagew/2;
    iy1 = -(int)y1 + limageh;

    drawline((unsigned char*)scanimage, limagew, limageh, ix0, iy0, ix1, iy1, 0, 0, 0xFF);
  }
}

void HumanRecog::drawmatch(unsigned char* buf, bool detecttarget, bool matchnotlost)
{
  //--------------------
  //draw match result
  //--------------------
  int matchrect[4][2];
  matchrect[0][0] = imagew/2 - matchw/2 + matchx - matchs;
  matchrect[0][1] = imageh/2 - matchh/2 + matchy - matchs;
  matchrect[1][0] = imagew/2 + matchw/2 + matchx + matchs;
  matchrect[1][1] = imageh/2 - matchh/2 + matchy - matchs;
  matchrect[2][0] = imagew/2 + matchw/2 + matchx + matchs;
  matchrect[2][1] = imageh/2 + matchh/2 + matchy + matchs;
  matchrect[3][0] = imagew/2 - matchw/2 + matchx - matchs;
  matchrect[3][1] = imageh/2 + matchh/2 + matchy + matchs;

  unsigned char r = 255;
  unsigned char g = 0;
  unsigned char b = 0;

  drawline(buf, imagew, imageh, matchrect[0][0], matchrect[0][1], matchrect[1][0], matchrect[1][1], r, g, b);
  drawline(buf, imagew, imageh, matchrect[1][0], matchrect[1][1], matchrect[2][0], matchrect[2][1], r, g, b);
  drawline(buf, imagew, imageh, matchrect[2][0], matchrect[2][1], matchrect[3][0], matchrect[3][1], r, g, b);
  drawline(buf, imagew, imageh, matchrect[3][0], matchrect[3][1], matchrect[0][0], matchrect[0][1], r, g, b);

  if (!matchnotlost) {
    drawline(buf, imagew, imageh, matchrect[0][0], matchrect[0][1], matchrect[2][0], matchrect[2][1], r, g, b);
    drawline(buf, imagew, imageh, matchrect[1][0], matchrect[1][1], matchrect[3][0], matchrect[3][1], r, g, b);
  }

  if (detecttarget) {
    r = 0;
    b = 255;
    matchrect[0][0] = imagew/2 - matchw/2;
    matchrect[0][1] = imageh/2 - matchh/2;
    matchrect[1][0] = imagew/2 + matchw/2;
    matchrect[1][1] = imageh/2 - matchh/2;
    matchrect[2][0] = imagew/2 + matchw/2;
    matchrect[2][1] = imageh/2 + matchh/2;
    matchrect[3][0] = imagew/2 - matchw/2;
    matchrect[3][1] = imageh/2 + matchh/2;

    drawline(buf, imagew, imageh, matchrect[0][0], matchrect[0][1], matchrect[1][0], matchrect[1][1], r, g, b);
    drawline(buf, imagew, imageh, matchrect[1][0], matchrect[1][1], matchrect[2][0], matchrect[2][1], r, g, b);
    drawline(buf, imagew, imageh, matchrect[2][0], matchrect[2][1], matchrect[3][0], matchrect[3][1], r, g, b);
    drawline(buf, imagew, imageh, matchrect[3][0], matchrect[3][1], matchrect[0][0], matchrect[0][1], r, g, b);
  }

}


float HumanRecog::matching_hist()
{

  //----------------------------------------
  //histogram matching
  //----------------------------------------

  float max = 0.0f;
  int finalx = 0;
  int finaly = 0;
  int finals = 0;

  for (int x = matchx - searchw/2; x < matchx + searchw/2; x += addx) {
    for (int y = matchy - searchh/2; y < matchy + searchh/2; y += addy) {
      for (int s = scales; s <= scalee; s += adds) {

	int rectx0 = imagew/2 - matchw/2 + x - s;
	int recty0 = imageh/2 - matchh/2 + y - s;
	int rectx1 = rectx0 + matchw + s;
	int recty1 = recty0 + matchh + s;

	if (rectx0 < 0 || rectx1 >= imagew ||
	    recty0 < 0 || recty1 >= imageh) {
	  continue;
	}

	gethist(Histbuf[1],
		HbufPtr,
		SbufPtr,
		VbufPtr,
		rectx0, recty0,
		matchw + s, matchh + s,
		imagew, imageh,
		m_grayS, m_grayK, m_matchblur);

	float scale = (float)(matchw * matchh) / (float)((matchw + s*2)*(matchh + s*2));

	float dot = dothist(Histbuf[0], Histbuf[1], 1.0f, scale);
	if (dot > max) {
	  finalx = x;
	  finaly = y;
	  finals = s;
	  max = dot;
	}
      }
    }
  }
  if (max > 0.0f) {
    matchx = finalx;
    matchy = finaly;
    matchs = finals;
    //      printf("match %f  xys %d %d %d\n", max, matchx, matchy, matchs);

    //final result
    gethist(Histbuf[1],
	    HbufPtr,
	    SbufPtr,
	    VbufPtr,
	    imagew/2 - matchw/2 + finalx - finals, imageh/2 - matchh/2 + finaly - finals,
	    matchw + finals, matchh + finals,
	    imagew, imageh,
	    m_grayS, m_grayK, m_matchblur);
  }
  return max;

}



//--------------------------------------
// get human pos
//--------------------------------------
float HumanRecog::gethumanang(int ix, int iw, float angofview)
{
  float radview = 3.141592f * angofview / 180.0f;
  float ret = (radview * 0.5f) * ((float)-ix / (float)(iw>>1));
  ret += 3.141592 * 0.5;//shift +90 degree
  return ret;
}

void HumanRecog::gethumanpos(float& x, float& y, float dirang, long* scan)
{
  float oneang = (3.141592 / 180.0) * SCIP2_TOPURG_ONEANG;
  int idx = (int)(dirang / oneang) + 180;


  int angidxw = m_angleofscan * 4; //degree of angle

  int idxstart = idx - (angidxw>>2);
  int idxend   = idx + (angidxw>>2);

  float minlen = 4.0f;
  for (int i = idxstart; i <= idxend; i++) {
    float len = (float)scan[i] / 1000.0f;

    len = (len > LASER_MAXLEN)? LASER_MAXLEN : len;
    len = (len < LASER_MINLEN)? LASER_MAXLEN : len;

    if (len < minlen) {
      minlen = len;
    }
  }

  x = minlen * cosf(dirang);
  y = minlen * sinf(dirang);
}


void HumanRecog::drawhist(float* histbuf, int y, float scale)
{
  float add = 256.0f / 60.0f;

  float r = 255.0f;
  float g = 0.0f;
  float b = 0.0f;
  for (int i = 0; i < 60; i++) {
    drawline((unsigned char*)histimage, 360, histimageh, i, histimageh-y, i, histimageh-y-(int)(histbuf[i] * scale),
	     (unsigned char)r, (unsigned char)g, (unsigned char)0);
    g += add;
  }
  r = 255.0f;
  g = 255.0f;
  b = 0.0f;
  for (int i = 60; i < 120; i++) {
    drawline((unsigned char*)histimage, 360, histimageh, i, histimageh-y, i, histimageh-y-(int)(histbuf[i] * scale),
	     (unsigned char)r, (unsigned char)g, (unsigned char)0);
    r -= add;
  }
  r = 0.0f;
  g = 255.0f;
  b = 0.0f;
  for (int i = 120; i < 180; i++) {
    drawline((unsigned char*)histimage, 360, histimageh, i, histimageh-y, i, histimageh-y-(int)(histbuf[i] * scale),
	     (unsigned char)0, (unsigned char)g, (unsigned char)b);
    b += add;
  }
  r = 0.0f;
  g = 255.0f;
  b = 255.0f;
  for (int i = 180; i < 240; i++) {
    drawline((unsigned char*)histimage, 360, histimageh, i, histimageh-y, i, histimageh-y-(int)(histbuf[i] * scale),
	     (unsigned char)0, (unsigned char)g, (unsigned char)b);
    g -= add;
  }
  r = 0.0f;
  g = 0.0f;
  b = 255.0f;
  for (int i = 240; i < 300; i++) {
    drawline((unsigned char*)histimage, 360, histimageh, i, histimageh-y, i, histimageh-y-(int)(histbuf[i] * scale),
	     (unsigned char)r, (unsigned char)0, (unsigned char)b);
    r += add;
  }
  r = 255.0f;
  g = 0.0f;
  b = 255.0f;
  for (int i = 300; i < 360; i++) {
    drawline((unsigned char*)histimage, 360, histimageh, i, histimageh-y, i, histimageh-y-(int)(histbuf[i] * scale),
	     (unsigned char)r, (unsigned char)0, (unsigned char)b);
    b -= add;
  }

}

void HumanRecog::drawhistgray(float* histbuf, int y, float scale)
{
  for (int i = 0; i < 255; i++) {
    drawline((unsigned char*)histimage, 360, histimageh, i, histimageh-y, i, histimageh-y-(int)(histbuf[i+360] * scale),
	     (unsigned char)i, (unsigned char)i, (unsigned char)i);
  }
}






RTC::ReturnCode_t HumanRecog::onExecute(RTC::UniqueId ec_id)
{


  //---------------------------
  // input
  //---------------------------
  if (m_InputIn.isNew()) {
    while (!m_InputIn.isEmpty()) {
      m_InputIn.read();
      target = (m_Input.data & 0xFF)? true : false;
    }
  }

  //-------------------------------
  // camera image
  //-------------------------------
  if (m_ImageIn.isNew() || m_ImageYUYVIn.isNew()) {

    if (m_ImageIn.isNew()) {
      while (!m_ImageIn.isEmpty()) {
	m_ImageIn.read();
      }
      int imagesize(m_Image.data.length());

      for (int i = 0; i < imagesize; i++) {
	image[i] = m_Image.data[i];
      }
    }
    else if (m_ImageYUYVIn.isNew()) {
      while (!m_ImageYUYVIn.isEmpty()) {
	m_ImageYUYVIn.read();
      }
      int imagesize(m_ImageYUYV.data.length());

      unsigned short* yuyv = (unsigned short*) malloc(imagesize * sizeof(unsigned short));
      for (int i = 0; i < imagesize; i++) {
	yuyv[i] = m_ImageYUYV.data[i];
      }
      unsigned int* rgb24 = (unsigned int*) malloc(imagesize * sizeof(unsigned int));

      yuyv_to_rgb24(imagew, imageh, (unsigned char*) yuyv, (unsigned char*)rgb24);
      rgb24_to_xrgb(imagew, imageh, (unsigned char*) rgb24, (unsigned char*) image);
    }


    GetHSV(image, imagew, imageh, Hbuf, Sbuf, Vbuf);
    HbufPtr = Hbuf;
    SbufPtr = Sbuf;
    VbufPtr = Vbuf;

    if (target) {
      //-----------------
      //update target
      //-----------------
      matchx = 0;
      matchy = 0;
      matchs = 0;

      gethist(Histbuf[0],
	      HbufPtr,
	      SbufPtr,
	      VbufPtr,
	      imagew/2 - matchw/2, imageh/2 - matchh/2,
	      matchw, matchh,
	      imagew, imageh,
	      m_grayS, m_grayK, m_matchblur);

      float dot = dothist(Histbuf[0], Histbuf[0], 1.0f, 1.0f);
      matchmin = dot * m_matchvalue;

      firstdetect = false;
    }

    //-------------------------
    // matching
    //-------------------------
    match = matching_hist();

    //-------------------------
    // human direction
    //-------------------------
    humanang = gethumanang(matchx, imagew, m_angleofview0);

    matchresult = (match > matchmin)? true : false;
    if (!matchresult || firstdetect) {
      matchx = 0;
      matchy = 0;
      matchs = 0;
      humanx = 0.0f;
      humany = 0.0f;
      humanang = 0.0f;
    }
  }



  //--------------------------------
  // draw histogram image
  //--------------------------------
  unsigned int* histimagebuf = (unsigned int*)histimage;
  for (int i = 0; i < 360 * histimageh; i++) {
    histimagebuf[i] = 0x80000000;
  }

  drawhistgray(Histbuf[0], histimageh/4+histimageh/2, 0.3f);
  drawhist(Histbuf[0],     0           +histimageh/2, 0.3f);

  if (!firstdetect) {
    drawhistgray(Histbuf[1], histimageh/4, 0.3f);
    drawhist(Histbuf[1],     1,            0.3f);
  }



  //--------------------------------
  //laser scan
  //--------------------------------
  if (m_ScanIn.isNew()) {
    while (!m_ScanIn.isEmpty()) {
      m_ScanIn.read();
    }
    int scansize(m_Scan.data.length());

    for (int i = 0; i < scansize; i++) {
      scan[i] = m_Scan.data[i];
    }
  }


  //------------------------------
  // update human position
  //------------------------------
  if (matchresult) {
    gethumanpos(humanx, humany, humanang, scan);
  }
  if (target) {
    humanx = 0.0f;
    humany = 0.0f;
    humanang = 0.0f;
  }


  coil::TimeValue tm = coil::gettimeofday();
  m_HumanPose2DIIS.tm.sec = (::CORBA::ULong) tm.sec();
  m_HumanPose2DIIS.tm.nsec = (::CORBA::ULong) (tm.usec() * 1000);
  m_HumanPose2DIIS.id.length(1);
  m_HumanPose2DIIS.id[0] = m_idIIS;

  { 

    if (m_RobotPose2DIISIn.isNew()) {
      while (!m_RobotPose2DIISIn.isEmpty()) {
	m_RobotPose2DIISIn.read();
      }
      getrobotpos = true;
    }
    if (getrobotpos) {

      //globalize
      const static double pi = 3.141592;
      double hx = humanx * cos(m_RobotPose2DIIS.data.heading) - humany * sin(m_RobotPose2DIIS.data.heading);
      double hy = humanx * sin(m_RobotPose2DIIS.data.heading) + humany * cos(m_RobotPose2DIIS.data.heading);

      double nextposx = m_RobotPose2DIIS.data.position.x     + hy;
      double nextposy = m_RobotPose2DIIS.data.position.y     - hx;
      double nextposh = m_RobotPose2DIIS.data.heading + humanang-(pi*0.5);


      //check jump
      double robotdistAx = m_HumanPose2DIIS.data.position.x - m_RobotPose2DIIS.data.position.x;
      double robotdistAy = m_HumanPose2DIIS.data.position.y - m_RobotPose2DIIS.data.position.y;
      double robotdistA = sqrtf(robotdistAx * robotdistAx + robotdistAy * robotdistAy);

      double robotdistBx = nextposx - m_RobotPose2DIIS.data.position.x;
      double robotdistBy = nextposy - m_RobotPose2DIIS.data.position.y;
      double robotdistB = sqrtf(robotdistBx * robotdistBx + robotdistBy * robotdistBy);

      bool firstlastjump = false;
      if (robotdistA < 0.2 || robotdistB < 0.2) {//200mm
	firstlastjump = true;
      }

      {
	double diffx = nextposx - m_HumanPose2DIIS.data.position.x;
	double diffy = nextposy - m_HumanPose2DIIS.data.position.y;
	double jump = sqrtf(diffx * diffx + diffy * diffy);
	if (target || firstlastjump || jump < m_maxjump) {
	  m_HumanPose2DIIS.data.position.x     = nextposx;
	  m_HumanPose2DIIS.data.position.y     = nextposy;
	  m_HumanPose2DIIS.data.heading        = nextposh;
	  m_HumanPose2DIISOut.write();
	}
      }
    
#if 1
      printf("        %f %f\n", (float)hy, (float)-hx);
      printf("RPobIIS %f %f %f\n", (float)m_RobotPose2DIIS.data.position.x, (float)m_RobotPose2DIIS.data.position.y, (float)m_RobotPose2DIIS.data.heading);
      printf("HPosIIS %f %f %f\n", (float)m_HumanPose2DIIS.data.position.x, (float)m_HumanPose2DIIS.data.position.y, (float)m_HumanPose2DIIS.data.heading);
#endif
    }
  }


  drawmatch((unsigned char*)image, target, matchresult);
  drawlaser();
  drawpos(humanx, humany);


#ifndef WIN32
  win.drawlock();
  win.drawimage((char*)image,     0,      0, imagew, imageh);
  win.drawimage((char*)scanimage, imagew, histimageh, 320, 240);
  win.drawimage((char*)histimage, imagew, 0, 360, histimageh);



  char matchstr[64];
  sprintf(matchstr, "match %d  %f / %f", (matchresult)? 1 : 0,  match, matchmin);
  char humanposstr[128];
  sprintf(humanposstr, "human %f %f %f", humanx, humany, humanang);

  win.drawrectfill(imagew, histimageh + limageh, 360, 30);
  win.drawstring(imagew, histimageh + limageh + 10, matchstr);
  win.drawstring(imagew, histimageh + limageh + 20, humanposstr);

  win.drawunlock();
  //win.eventloop();
#else
  printf("human %f %f %f  match %d  %f / %f", humanx, humany, humanang, (matchresult)? 1 : 0,  match, matchmin);
#endif


  return RTC::RTC_OK;
}


/*
RTC::ReturnCode_t HumanRecog::onAborting(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t HumanRecog::onError(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t HumanRecog::onReset(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t HumanRecog::onStateUpdate(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t HumanRecog::onRateChanged(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/



extern "C"
{

  void HumanRecogInit(RTC::Manager* manager)
  {
    RTC::Properties profile(humanrecog_spec);
    manager->registerFactory(profile,
                             RTC::Create<HumanRecog>,
                             RTC::Delete<HumanRecog>);
  }

};


