// -*- C++ -*-
/*!
 * @file  RmpObstAvoid.cpp
 * @brief A simple RMP controller with the Urg
 * $Date$
 *
 * $Id$
 */

#include "RmpObstAvoid.h"
#include "CircTrajMotionPlanner.h"

#include <kalibrary/geometry/point2.h>
#include <kalibrary/geometry/geometryfunctions.h>
#include <kalibrary/geometry/segment2.h>

using namespace RDK2::Geometry;
using namespace std;

// Module specification
// <rtc-template block="module_spec">
static const char* rmpobstavoid_spec[] =
  {
    "implementation_id", "RmpObstAvoid",
    "type_name",         "RmpObstAvoid",
    "description",       "A simple RMP controller with the Urg",
    "version",           "0.1",
    "vendor",            "Daniele Calisi",
    "category",          "Generic",
    "activity_type",     "DataFlowComponent",
    "max_instance",      "",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables

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

RmpObstAvoid::RmpObstAvoid(RTC::Manager* manager)
  : RTC::DataFlowComponentBase(manager),
    // <rtc-template block="initializer">
    m_targetSpeedIn("targetSpeed", m_targetSpeed),
    m_targetTurnIn("targetTurn", m_targetTurn),
    m_SpeedOut("Speed", m_Speed),
    m_TurnOut("Turn", m_Turn),
    m_RmpControllerPortPort("RmpControllerPort"),
    // </rtc-template>
	dummy(0)
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  registerInPort("targetSpeed", m_targetSpeedIn);
  registerInPort("targetTurn", m_targetTurnIn);
  
  // Set OutPort buffer
  registerOutPort("Speed", m_SpeedOut);
  registerOutPort("Turn", m_TurnOut);
  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  m_RmpControllerPortPort.registerConsumer("urg", "UrgServiceInterface", m_urg);
  
  // Set CORBA Service Ports
  registerPort(m_RmpControllerPortPort);
  
  // </rtc-template>

}

RmpObstAvoid::~RmpObstAvoid()
{
}


RTC::ReturnCode_t RmpObstAvoid::onInitialize()
{
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  
  // </rtc-template>
  return RTC::RTC_OK;
}



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

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

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

/*
RTC::ReturnCode_t RmpObstAvoid::onActivated(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t RmpObstAvoid::onDeactivated(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/


RTC::ReturnCode_t RmpObstAvoid::onExecute(RTC::UniqueId ec_id)
{	
  int angleMin = atoi(m_urg->getInfo("AMIN"));
  int angleFront = atoi(m_urg->getInfo("AFRT"));
  double res = 135. / (double) angleFront;
  double startAngle = floor(135. / (double) angleFront * angleMin - 135.);
  
  UnsignedLongSeq* scan = new UnsignedLongSeq();
  double timestamp;
  double target_v,target_w;

  m_targetSpeedIn.read();
  m_targetTurnIn.read();
  target_v =   (double)m_targetSpeed.data /(-332.0);			// see RMP Interface Guide, page 13
  target_w =   (double)m_targetTurn.data /(-180.0 / M_PI * 7.8);	// see RMP Interface Guide, page 13

  if (m_urg->getScan(true, true, scan, timestamp)) {
    double angle = startAngle * M_PI / 180.;
    double stepSize = res * M_PI / 180.;
    vector<Point2d> obstpts;
    double shortestReading = DBL_MAX;
    for (size_t i = 0; i < scan->length(); i++) {
      angle += stepSize;
      if (angle < -M_PI/2 || angle > M_PI/2) continue;
      double reading = (double) (*scan)[i] * 0.001;	// URG output is in millimeters, we want meters
      Point2d locpt(reading * cos(angle), reading * sin(angle));
      obstpts.push_back(locpt);
      if (reading < shortestReading) shortestReading = reading;
      //			if (angle > -M_PI/2 && angle < M_PI/2 && dist < ctrlDist) { ctrlDist = dist; ctrlAngle = angle; }
    }
    vector<Segment2d> obsts = recursiveLineFitting(obstpts, 0.1);
    
    
    double bestSpeed = 0., bestJog = 0.;
    if (shortestReading < 0.20) {
      bestSpeed = 0.; bestJog = 0.;
      printf("Emergency stop activated (at least one URG beam is shorter than 20 cm\n");
    }
    else {
      CircTrajMotionPlanner ctmp;
      
      // some parameters
      ctmp.paramQuickPredict = true;
      ctmp.paramMaxSpeed = 0.40; ctmp.paramMaxJog = deg2rad(40.);
      ctmp.paramMaxASpeed = 0.4; ctmp.paramMaxAJog = deg2rad(180.);
      ctmp.paramRobotSize = 0.39;
      ctmp.paramTargetDistWeight = 0.0; ctmp.paramTargetDirWeight = 0.0;
      ctmp.paramDurationWeight = 0.01;
      ctmp.paramPathLenWeight = 1.0; ctmp.paramObstDistWeight = 0.6;
      ctmp.paramJogWeight = 1.0;
      ctmp.paramIntervalLen = 0.1;
      
      double bestPathCost = DBL_MAX;
      list<Point2od> bestPath;
      double dspeed = ctmp.paramMaxASpeed * ctmp.paramIntervalLen;
      double djog = ctmp.paramMaxAJog * ctmp.paramIntervalLen;
      double pathCost;
      {
	list<Point2od> path; 
	ctmp.computeTrajectory(/*localTargetPose*/ Point2od(0., 0., 0.), target_v, target_w, 0, 0, obsts, path, pathCost);
      }
      if(pathCost < DBL_MAX){
	dspeed /=10;
	djog/=10;
	printf("tracking...");
	if( target_v > lastSpeed+dspeed ){
	  bestSpeed=lastSpeed + dspeed;
	}else if(target_v < lastSpeed-dspeed ){
	  bestSpeed=lastSpeed-dspeed; 
	}else{
	  bestSpeed = target_v;
	}

	if( target_w > lastJog+djog ){
	  bestJog=lastJog + djog;
	}else if(target_w < lastJog-djog ){
	  bestJog=lastJog-djog; 
	}else{
	  bestJog = target_w;
	}
	//	usleep(40000);
      }else{
	printf("avoiding...");
	for (double speed = lastSpeed - dspeed; speed < lastSpeed + dspeed; speed += dspeed / 2.) {
	  for (double jog = lastJog - djog; jog < lastJog + djog; jog += djog / 10.) {
	    if (speed < 0. || speed > ctmp.paramMaxSpeed || speed < -ctmp.paramMaxSpeed
		|| jog > ctmp.paramMaxJog || jog < -ctmp.paramMaxJog) continue;
	    if (fabs(speed) < 0.001 && fabs(jog) < 0.001) continue;
	    
	    list<Point2od> path; 
	    ctmp.computeTrajectory(/*localTargetPose*/ Point2od(0., 0., 0.), speed, jog, 0, 0, obsts, path, pathCost);
	    if(pathCost < DBL_MAX)pathCost += fabs(target_v-speed)*1.0+fabs(target_w-jog)*1.0;
	    //if(fabs(target_v) < fabs(speed))pathCost =DBL_MAX;
	    
	    if (pathCost < bestPathCost) {
	      bestPathCost = pathCost; bestPath = path;
	      bestSpeed = speed; bestJog = jog;
	    }
	  }
	}
	if (bestPathCost == DBL_MAX) {
	  bestSpeed = 0.;
	  bestJog = 0;//deg2rad(2.0);
	}
      }
    }
    
    m_Speed.data = -(int) (bestSpeed * 332);		// see RMP Interface Guide, page 13
    m_Turn.data = -(int) (bestJog * 180 / M_PI * 7.8);	// see RMP Interface Guide, page 13
    
    printf("Speed: %.3f m/s [%.3f m/s], Jog: %.3f deg/s [%.3f deg/s]\n", bestSpeed, target_v,rad2deg(bestJog),target_w);
    
    lastSpeed = bestSpeed;
    lastJog = bestJog;
    
    m_SpeedOut.write();
    m_TurnOut.write();
  }
  else printf("Cannot read from URG service.\n");
  return RTC::RTC_OK;
}


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

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

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

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

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



extern "C"
{
 
  void RmpObstAvoidInit(RTC::Manager* manager)
  {
    RTC::Properties profile(rmpobstavoid_spec);
    manager->registerFactory(profile,
                             RTC::Create<RmpObstAvoid>,
                             RTC::Delete<RmpObstAvoid>);
  }
  
};


