/************************************************************************************
HumanFollow 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 "HumanFollow.h"
#include "misc.h"


// Module specification
// <rtc-template block="module_spec">
static const char* humanfollow_spec[] =
  {
    "implementation_id", "HumanFollow",
    "type_name",         "HumanFollow",
    "description",       "HumanFollow 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.Kspeed", "1.0",
    "conf.default.Kturn", "3.0",
    "conf.default.MAXspeed", "1.0",
    "conf.default.MAXturn", "1.5",
    "conf.default.Distance", "1.5",
    "conf.default.Kcurve_svec", "1.0",
    "conf.default.Kcurve_evec", "1.0",
    "conf.default.PathVel", "1.0",
    "conf.default.PathTargetR", "0.2",
    "conf.default.PathTurn", "1.570796",
    "conf.default.PathStopR", "1.5",
    "conf.default.PathTime", "2.0",
    "conf.default.IntervalTime", "0.5",
    "conf.default.Mode", "0",
    "conf.default.imagew", "640",
    "conf.default.imageh", "640",

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

HumanFollow::HumanFollow(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_HumanPose2DIISIn("HumanPose2DIIS", m_HumanPose2DIIS),
    m_RobotPose2DIISIn("RobotPose2DIIS", m_RobotPose2DIIS),
    m_VelocityOut("Velocity", m_Velocity),
    m_Velocity2DIISOut("Velocity2DIIS", m_Velocity2DIIS),
    // </rtc-template>
	dummy(0)
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  registerInPort("HumanPose2DIIS", m_HumanPose2DIISIn);
  registerInPort("RobotPose2DIIS", m_RobotPose2DIISIn);
  
  // Set OutPort buffer
  registerOutPort("Velocity", m_VelocityOut);
  registerOutPort("Velocity2DIIS", m_Velocity2DIISOut);
  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  
  // </rtc-template>

  image = NULL;

}

HumanFollow::~HumanFollow()
{
}



RTC::ReturnCode_t HumanFollow::onInitialize()
{
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("Kspeed", m_Kspeed, "1.0");
  bindParameter("Kturn", m_Kturn, "3.0");
  bindParameter("MAXspeed", m_MAXspeed, "1.0");
  bindParameter("MAXturn", m_MAXturn, "1.5");
  bindParameter("Distance", m_Distance, "1.5");
  bindParameter("Mode", m_Mode, "0");
  bindParameter("Kcurve_svec", m_Kcurve_svec, "1.0");
  bindParameter("Kcurve_evec", m_Kcurve_evec, "1.0");
  bindParameter("PathVel", m_PathVel, "1.0");
  bindParameter("PathTargetR", m_PathTargetR, "0.2");
  bindParameter("PathTurn", m_PathTurn, "1.570796");
  bindParameter("PathStopR", m_PathStopR, "1.0");
  bindParameter("PathTime", m_PathTime, "2.0");
  bindParameter("IntervalTime", m_IntervalTime, "0.5");
  bindParameter("imagew", m_imagew, "640");
  bindParameter("imageh", m_imageh, "640");
  
  // </rtc-template>



   return RTC::RTC_OK;
 }

 /*
 RTC::ReturnCode_t HumanFollow::onFinalize()
 {
   return RTC::RTC_OK;
 }
 */

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

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

 RTC::ReturnCode_t HumanFollow::onActivated(RTC::UniqueId ec_id)
 {
   ready_robotpos = false;
   ready_humanpos = false;
   pointlist.clear();

   followid = -1;

   curtime = coil::gettimeofday();
   prevtime = coil::gettimeofday();


   if (m_Mode == MODE_TRACE) {

     if (image == NULL) {
       image = (unsigned long*)  malloc(m_imagew * m_imageh * 4);
       memset(image, 0, m_imagew * m_imageh * 4);
     }
#ifndef WIN32
     win.start(0, NULL, 100,400,m_imagew, m_imageh, "RTC HumanFollow");
#endif
   }

   return RTC::RTC_OK;
 }


 RTC::ReturnCode_t HumanFollow::onDeactivated(RTC::UniqueId ec_id)
 {
   if (m_Mode == MODE_TRACE) {
#ifndef WIN32
     win.end();
#endif
     if (image) {
       free(image);
       image = NULL;
     }
   }

   return RTC::RTC_OK;
 }







void HumanFollow::ExecFOLLOW(double& front, double& steer)
{
  double rx = m_RobotPose2DIIS.data.position.x;
  double ry = m_RobotPose2DIIS.data.position.y;
  double rtheta = m_RobotPose2DIIS.data.heading;

  double hx     = m_HumanPose2DIIS.data.position.x;
  double hy     = m_HumanPose2DIIS.data.position.y;
  double htheta = m_HumanPose2DIIS.data.heading;

  //localize
  const static double pi = 3.141592;
  double x = hx - rx;
  double y = hy - ry;
  double theta = htheta - rtheta + (pi*0.5);
  double lx = x * cos(-rtheta) - y * sin(-rtheta);
  double ly = x * sin(-rtheta) + y * cos(-rtheta);


  double length = sqrtf(lx*lx+ly*ly);
  double speed  = 1.0f * m_Kspeed;

  front  = (length - m_Distance) * speed;
  steer = 1.0f * (theta - 3.141592*0.5) * m_Kturn;

  if (lx < 0.1f) {
    front = 0.0f;
    steer = 0.0f;
  }
  if (fabs(front) > m_MAXspeed) {
    front = (front > 0.0f)? m_MAXspeed : front;
    front = (front < 0.0f)? -m_MAXspeed : front;
  }
  if (fabs(steer) > m_MAXturn) {
    steer = (steer > 0.0f)? m_MAXturn : steer;
    steer = (steer < 0.0f)? -m_MAXturn : steer;
  }

}





bool HumanFollow::IsIntervalTime()
{
  //time interval
  curtime = coil::gettimeofday();
  coil::TimeValue tdiff = curtime - prevtime;
  double sec = (double)((double)tdiff.sec() + (double)tdiff.usec() / 1000000.0);

  if (sec > m_IntervalTime) {
    prevtime = coil::gettimeofday();
    return true;
  }

  return false;
}


void HumanFollow::PathTRACE()
{
  ready_humanpos = (pointlist.size() > 0);
  

  path_point p;

  p.x     = m_HumanPose2DIIS.data.position.x;
  p.y     = m_HumanPose2DIIS.data.position.y;
  p.theta = m_HumanPose2DIIS.data.heading;

  //check point distance 
  if (pointlist.size() > 2) {
    path_point end = pointlist.back();
    
    float tgtx = p.x - end.x;
    float tgty = p.y - end.y;
    float tgtlen = sqrtf(tgtx * tgtx + tgty * tgty);

    if (tgtlen < m_PathTargetR) {
      return;
    }
  }


  if (!IsIntervalTime()) {
    return;
  }

  pointlist.push_back(p);
  
  printf("push_back %f %f len %d\n", (float)p.x, (float)p.y, pointlist.size());


  
  double length = 0.0;
  
  //culc distance and vel from reach time 
  if (pointlist.size() > 1) {
    for (std::list<path_point>::iterator it = targetit;
	 it != pointlist.end();
	 it++) {
      
      std::list<path_point>::iterator itnext = it;
      itnext++;
      if (itnext != pointlist.end()) {
	double lx = itnext->x - it->x;
	double ly = itnext->y - it->y;
	length += sqrt(lx*lx + ly*ly);
      }
    }
  }
  if (m_PathTime > 0.0) {
    m_PathVel = length / m_PathTime;
  }

  if (pointlist.size() == 1) {
    targetit = pointlist.begin();
  }


}


void HumanFollow::ExecTRACE(double& front, double& steer)
{
#if 0
  printf("rpos %f %f %f - hpos %f %f %f\n", 
	    m_RobotPose2DIIS.data.position.x, m_RobotPose2DIIS.data.position.y, m_RobotPose2DIIS.data.heading,
	    m_HumanPose2DIIS.data.position.x, m_HumanPose2DIIS.data.position.y, m_HumanPose2DIIS.data.heading);
#endif


  if (!ready_robotpos ||
      !ready_humanpos) {
    front = 0.0;
    steer = 0.0;
    return;
  }


  std::list<path_point>::iterator oldit = targetit;

  std::list<path_point>::iterator it = targetit;
  while (pointlist.end() != it) {
    float tgtx = it->x - m_RobotPose2DIIS.data.position.x;
    float tgty = it->y - m_RobotPose2DIIS.data.position.y;
    float tgtlen = sqrtf(tgtx * tgtx + tgty * tgty);

    if (tgtlen > m_PathTargetR) {
      targetit = it;

      if (pointlist.end() == it) {
	it--;
	targetit = it;
      }

      break;
    }
    it++;
  }

  if (oldit != targetit) {
    printf("newtarget %f %f\n", targetit->x, targetit->y);
  }

  if (targetit != pointlist.begin()) {
    pointlist.pop_front();
  } 


  float dirx = cosf(m_RobotPose2DIIS.data.heading);
  float diry = sinf(m_RobotPose2DIIS.data.heading);
  float tgtx = targetit->x - m_RobotPose2DIIS.data.position.x;
  float tgty = targetit->y - m_RobotPose2DIIS.data.position.y;
  float tgtlen = sqrtf(tgtx * tgtx + tgty * tgty);
  tgtx /= tgtlen;
  tgty /= tgtlen;



  //---------------------
  //goal check
  //---------------------
  float goalx = m_HumanPose2DIIS.data.position.x - m_RobotPose2DIIS.data.position.x;
  float goaly = m_HumanPose2DIIS.data.position.y - m_RobotPose2DIIS.data.position.y;
  float goallen = sqrtf(goalx * goalx + goaly * goaly);
  if (goallen < m_PathStopR) {
    front = 0.0;
    steer = 0.0;
    return;
  }


  float dot = dirx * tgtx + diry * tgty;

  //turn at the position
  float speed = 0.0f;
  float turn = 1.0f;

  //turn and vel
  if (dot > 0.5) {
    speed = dot - 0.5f;  //0.0 -> 0.5
    turn = 1.0f - dot;   //0.5 -> 0.0
    
    speed *= m_PathVel  * m_Kspeed; //0.0 -> 1.0
    turn  *= m_PathTurn * m_Kturn; //1.0 -> 0.0
  }
  
  //detect turn sign
  float outer = dirx * tgty - diry * tgtx;
  float sign = (outer > 0.0f)? 1.0f : -1.0f;

  turn *= sign;


  front = (double)speed;
  steer = (double)turn;

}


void HumanFollow::drawpath()
{
  memset(image, 0xFF, sizeof(unsigned long) * m_imagew * m_imageh);

  //coord horizontal
  drawline((unsigned char*)image, m_imagew, m_imageh,
	   0, m_imageh/2-PIXEL_PER_METER, m_imagew, m_imageh/2-PIXEL_PER_METER, 0x80, 0x80, 0x80);

  drawline((unsigned char*)image, m_imagew, m_imageh,
	   0, m_imageh/2-PIXEL_PER_METER*2, m_imagew, m_imageh/2-PIXEL_PER_METER*2, 0x80, 0x80, 0x80);

  drawline((unsigned char*)image, m_imagew, m_imageh,
	   0, m_imageh/2,                 m_imagew, m_imageh/2, 0x80, 0x80, 0x80);

  drawline((unsigned char*)image, m_imagew, m_imageh,
	   0, m_imageh/2+PIXEL_PER_METER, m_imagew, m_imageh/2+PIXEL_PER_METER, 0x80, 0x80, 0x80);

  drawline((unsigned char*)image, m_imagew, m_imageh,
	   0, m_imageh/2+PIXEL_PER_METER*2, m_imagew, m_imageh/2+PIXEL_PER_METER*2, 0x80, 0x80, 0x80);

  //coord vertical
  drawline((unsigned char*)image, m_imagew, m_imageh,
	   m_imagew/2-PIXEL_PER_METER, 0, m_imagew/2-PIXEL_PER_METER, m_imageh, 0x80, 0x80, 0x80);

  drawline((unsigned char*)image, m_imagew, m_imageh,
	   m_imagew/2-PIXEL_PER_METER*2, 0, m_imagew/2-PIXEL_PER_METER*2, m_imageh, 0x80, 0x80, 0x80);

  drawline((unsigned char*)image, m_imagew, m_imageh,
	   m_imagew/2, 0, m_imagew/2, m_imageh, 0x80, 0x80, 0x80);

  drawline((unsigned char*)image, m_imagew, m_imageh,
	   m_imagew/2+PIXEL_PER_METER, 0, m_imagew/2+PIXEL_PER_METER, m_imageh, 0x80, 0x80, 0x80);

  drawline((unsigned char*)image, m_imagew, m_imageh,
	   m_imagew/2+PIXEL_PER_METER*2, 0, m_imagew/2+PIXEL_PER_METER*2, m_imageh, 0x80, 0x80, 0x80);


  if (pointlist.size() > 2) {
    std::list<path_point>::iterator it = pointlist.begin();
    float x0, y0, x1, y1;
    
    x0 = it->x;
    y0 = it->y;
    it++;
    x1 = it->x;
    y1 = it->y;
    
    while (it != pointlist.end()) {

      int ix0 =  (int)(x0 * PIXEL_PER_METER) + m_imagew/2;
      int iy0 = -(int)(y0 * PIXEL_PER_METER) + m_imageh/2;
      int ix1 =  (int)(x1 * PIXEL_PER_METER) + m_imagew/2;
      int iy1 = -(int)(y1 * PIXEL_PER_METER) + m_imageh/2;

      drawline((unsigned char*)image, m_imagew, m_imageh, ix0, iy0, ix1, iy1, 0, 0, 0xFF);

      x0 = x1;
      y0 = y1;
      it++;
      x1 = it->x;
      y1 = it->y;
    }
  }  

}



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


  //keep robot pos latest
  if (m_RobotPose2DIISIn.isNew()) {
    while (!m_RobotPose2DIISIn.isEmpty()) {
      m_RobotPose2DIISIn.read();
    }
    ready_robotpos = true;
  }

  if (m_HumanPose2DIISIn.isNew()) {
    while (!m_HumanPose2DIISIn.isEmpty()) {
      m_HumanPose2DIISIn.read();
    }
  }

  {
    //check follow id
    if (m_HumanPose2DIIS.id.length()) {
      int id = m_HumanPose2DIIS.id[0];
      if (followid != id) {
	printf("#Follow Target Changed %d\n", id);
	followid = id;
	pointlist.clear();
      }
    }

    double front;
    double steer;

    if (ready_robotpos) {
      switch (m_Mode) {
      case MODE_FOLLOW:
	ExecFOLLOW(front, steer);
	break;
      case MODE_TRACE:
	PathTRACE();
	ExecTRACE(front, steer);

	if (image) {
	  drawpath();
#ifndef WIN32
	  win.drawlock();
	  win.drawimage((char*)image, 0, 0, m_imagew, m_imageh);
	  win.drawunlock();
#endif
	}
	break;
      }

      double robotdistx = m_HumanPose2DIIS.data.position.x - m_RobotPose2DIIS.data.position.x;
      double robotdisty = m_HumanPose2DIIS.data.position.y - m_RobotPose2DIIS.data.position.y;
      double robotdist = sqrtf(robotdistx * robotdistx + robotdisty * robotdisty);
      if (robotdist < 0.1) {//10cm
	front = 0.0;
	steer = 0.0;
      }

      //---------------------
      //Velocity
      //---------------------
      //     v
      //     | <-w
      //     A
      m_Velocity.v = front;
      m_Velocity.w = steer;
      m_VelocityOut.write();
    
      //---------------------
      //Velocity2DIIS
      //---------------------
      //    vx
      //     | <-w
      //vy<--A
      m_Velocity2DIIS.data.vx = front;
      m_Velocity2DIIS.data.vy = 0.0;
      m_Velocity2DIIS.data.va = steer;
      m_Velocity2DIISOut.write();
    }
  }


  return RTC::RTC_OK;
}


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

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

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

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

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



extern "C"
{
 
  void HumanFollowInit(RTC::Manager* manager)
  {
    RTC::Properties profile(humanfollow_spec);
    manager->registerFactory(profile,
                             RTC::Create<HumanFollow>,
                             RTC::Delete<HumanFollow>);
  }
  
};


