/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 * Copyright Projet JRL-Japan, 2008
 *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * File:      PatternGenerator.cpp
 * Project:   SOT
 * Author:    Olivier Stasse
 *
 * Version control
 * ===============
 *
 *  $Id$
 *
 * Description
 * ============
 *
 *
 * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

//#define VP_DEBUG
//#define VP_DEBUG_MODE 45
#include <sot/core/debug.hh>
#ifdef VP_DEBUG
class sotPG__INIT
{
  public:sotPG__INIT( void ) { dynamicgraph::sot::DebugTrace::openFile(); }
};
sotPG__INIT sotPG_initiator;
#endif //#ifdef VP_DEBUG

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>

#include <jrl/mal/matrixabstractlayer.hh>
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"

#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
#include <sot/core/matrix-geometry.hh>

#include <sot-pattern-generator/pg.h>

using namespace std;
namespace dynamicgraph {
  namespace sot {

    DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PatternGenerator,"PatternGenerator");

    PatternGenerator::
    PatternGenerator( const std::string & name )
      :Entity(name)
      ,m_PGI(0)
      ,m_PreviewControlParametersFile()
      ,m_urdfFile("")
      ,m_srdfFile("")
      ,m_xmlRankFile("")
      ,m_soleLength(0)
      ,m_soleWidth(0)
      ,m_init(false)
      ,m_InitPositionByRealState(true)
      ,firstSINTERN( boost::bind(&PatternGenerator::InitOneStepOfControl,this,_1,_2),
                     sotNOSIGNAL,"PatternGenerator("+name+")::intern(dummy)::init" )

      ,OneStepOfControlS( boost::bind(&PatternGenerator::OneStepOfControl,this,_1,_2),
                          firstSINTERN << jointPositionSIN ,"PatternGenerator("+name+")::onestepofcontrol" )

      ,m_dataInProcess(0)
      ,m_rightFootContact(true) // It is assumed that the robot is standing.
      ,m_leftFootContact(true)
      ,jointPositionSIN(NULL,"PatternGenerator("+name+")::input(vector)::position")

      ,motorControlJointPositionSIN(NULL,"PatternGenerator("+name+")::input(vector)::motorcontrol")

      ,ZMPPreviousControllerSIN(NULL,"PatternGenerator("+name+")::input(vector)::zmppreviouscontroller")

      ,ZMPRefSOUT( boost::bind(&PatternGenerator::getZMPRef,this,_1,_2),
                   OneStepOfControlS,
                   "PatternGenerator("+name+")::output(vector)::zmpref" )

      ,CoMRefSOUT( boost::bind(&PatternGenerator::getCoMRef,this,_1,_2),
                   OneStepOfControlS,
                   "PatternGenerator("+name+")::output(matrix)::comref" )

      ,dCoMRefSOUT( boost::bind(&PatternGenerator::getdCoMRef,this,_1,_2),
                    OneStepOfControlS,
                    "PatternGenerator("+name+")::output(matrix)::dcomref" )

      ,comSIN(NULL,"PatternGenerator("+name+")::input(vector)::com")

      ,comStateSIN(NULL,"PatternGenerator("+name+")::input(vector)::comStateSIN")

      ,zmpSIN(NULL,"PatternGenerator("+name+")::input(vector)::zmpSIN")

      ,forceSIN(NULL,"PatternGenerator("+name+")::input(vector)::forceSIN")

      ,forceSOUT(boost::bind(&PatternGenerator::getExternalForces,this,_1,_2),
                 OneStepOfControlS,
                 "PatternGenerator("+name+")::output(matrix)::forceSOUT" )

      ,velocitydesSIN(NULL,"PatternGenerator("+name+")::input(vector)::velocitydes")

      ,LeftFootCurrentPosSIN(NULL,"PatternGenerator("+name+")::input(homogeneousmatrix)::leftfootcurrentpos")

      ,RightFootCurrentPosSIN(NULL,"PatternGenerator("+name+")::input(homogeneousmatrix)::rightfootcurrentpos")

      ,LeftFootRefSOUT( boost::bind(&PatternGenerator::getLeftFootRef,this,_1,_2),
                        OneStepOfControlS,
                        "PatternGenerator("+name+")::output(homogeneousmatrix)::leftfootref" )

      ,RightFootRefSOUT( boost::bind(&PatternGenerator::getRightFootRef,this,_1,_2),
                         OneStepOfControlS,
                         "PatternGenerator("+name+")::output(homogeneousmatrix)::rightfootref" )
      ,dotLeftFootRefSOUT( boost::bind(&PatternGenerator::getdotLeftFootRef,this,_1,_2),
                           OneStepOfControlS,
                           "PatternGenerator("+name+")::output(homogeneousmatrix)::dotleftfootref" )

      ,dotRightFootRefSOUT( boost::bind(&PatternGenerator::getdotRightFootRef,this,_1,_2),
                            OneStepOfControlS,
                            "PatternGenerator("+name+")::output(homogeneousmatrix)::dotrightfootref" )

      ,FlyingFootRefSOUT( boost::bind(&PatternGenerator::getFlyingFootRef,this,_1,_2),
                          OneStepOfControlS,
                          "PatternGenerator("+name+")::output(homogeneousmatrix)::flyingfootref" )

      ,SupportFootSOUT( boost::bind(&PatternGenerator::getSupportFoot,this,_1,_2),
                        OneStepOfControlS,
                        "PatternGenerator("+name+")::output(uint)::SupportFoot" )
      ,jointWalkingErrorPositionSOUT(boost::bind(&PatternGenerator::getjointWalkingErrorPosition,this,_1,_2),
                                     OneStepOfControlS,
                                     "PatternGenerator("+name+")::output(vector)::walkingerrorposition")

      ,comattitudeSOUT(boost::bind(&PatternGenerator::getComAttitude,this,_1,_2),
                       OneStepOfControlS,
                       "sotPatternGenerator("+name+")::output(vectorRPY)::comattitude")
      ,dcomattitudeSOUT(boost::bind(&PatternGenerator::getdComAttitude,this,_1,_2),
                        OneStepOfControlS,
                        "sotPatternGenerator("+name+")::output(vectorRPY)::dcomattitude")

      ,waistattitudeSOUT(boost::bind(&PatternGenerator::getWaistAttitude,this,_1,_2),
                         OneStepOfControlS,
                         "PatternGenerator("+name+")::output(vectorRPY)::waistattitude")
      ,waistattitudeabsoluteSOUT(boost::bind(&PatternGenerator::getWaistAttitudeAbsolute,this,_1,_2),
                                 OneStepOfControlS,
                                 "PatternGenerator("+name+")::output(vectorRPY)::waistattitudeabsolute")

      ,waistpositionSOUT(boost::bind(&PatternGenerator::getWaistPosition,this,_1,_2),
                         OneStepOfControlS,
                         "PatternGenerator("+name+")::output(vector)::waistposition")
      ,waistpositionabsoluteSOUT(boost::bind(&PatternGenerator::getWaistPositionAbsolute,this,_1,_2),
                                 OneStepOfControlS,
                                 "PatternGenerator("+name+")::output(vector)::waistpositionabsolute")

      ,dataInProcessSOUT(boost::bind(&PatternGenerator::getDataInProcess, this, _1, _2),
                         OneStepOfControlS,
                         "PatternGenerator("+name+")::output(bool)::inprocess")
      ,InitZMPRefSOUT( boost::bind(&PatternGenerator::getInitZMPRef,this,_1,_2),
                       OneStepOfControlS,
                       "PatternGenerator("+name+")::output(vector)::initzmpref" )

      ,InitCoMRefSOUT( boost::bind(&PatternGenerator::getInitCoMRef,this,_1,_2),
                       OneStepOfControlS,
                       "PatternGenerator("+name+")::output(matrix)::initcomref" )

      ,InitWaistPosRefSOUT( boost::bind(&PatternGenerator::getInitWaistPosRef,this,_1,_2),
                            OneStepOfControlS,
                            "PatternGenerator("+name+")::output(vector)::initwaistposref" )

      ,InitWaistAttRefSOUT( boost::bind(&PatternGenerator::getInitWaistAttRef,this,_1,_2),
                            OneStepOfControlS,
                            "PatternGenerator("+name+")::output(vectorRPY)::initwaistattref" )

      ,InitLeftFootRefSOUT( boost::bind(&PatternGenerator::getInitLeftFootRef,this,_1,_2),
                            OneStepOfControlS,
                            "PatternGenerator("+name+")::output(homogeneousmatrix)::initleftfootref" )

      ,InitRightFootRefSOUT( boost::bind(&PatternGenerator::getInitRightFootRef,this,_1,_2),
                             OneStepOfControlS,
                             "PatternGenerator("+name+")::output(homogeneousmatrix)::initrightfootref" )
      ,leftFootContactSOUT( boost::bind(&PatternGenerator::getLeftFootContact,this,_1,_2),
                            OneStepOfControlS,
                            "PatternGenerator("+name+")::output(bool)::leftfootcontact" )
      ,rightFootContactSOUT( boost::bind(&PatternGenerator::getRightFootContact,this,_1,_2),
                             OneStepOfControlS,
                             "PatternGenerator("+name+")::output(bool)::rightfootcontact")

    {
      m_MotionSinceInstanciationToThisSequence.setIdentity();

      m_LocalTime = 0;
      m_TimeStep = 0.005;
      m_DoubleSupportPhaseState = false;
      m_forceFeedBack = false ;
      m_feedBackControl = false ;

      m_ZMPRefPos.resize(4);
      m_ZMPRefPos.fill(0.0);
      m_ZMPRefPos(3) = 1.0;
      m_COMRefPos.resize(3);
      m_COMRefPos.fill(0.0);
      m_ZMPPrevious.resize(4);
      m_ZMPPrevious(3) = 1.0;
      m_dCOMRefPos.resize(3);
      m_dCOMRefPos.fill(0.0);
      m_InitZMPRefPos.resize(3);
      m_InitZMPRefPos.fill(0);
      m_InitCOMRefPos.resize(3);
      m_InitCOMRefPos.fill(0);
      m_InitWaistRefPos.resize(3);
      m_InitWaistRefPos.fill(0);
      m_InitWaistRefAtt.resize(3);
      m_InitWaistRefAtt.fill(0);
      m_dComAttitude.resize(3);
      m_dComAttitude.fill(0);
      m_VelocityReference.resize(3);
      m_VelocityReference.fill(0.0);
      m_WaistAttitude.resize(3);
      m_WaistAttitude.fill(0);
      m_ComAttitude.resize(3);
      m_ComAttitude.fill(0);
      m_WaistPosition.resize(3);
      m_WaistPosition.fill(0);
      m_WaistAttitudeAbsolute.resize(3);
      m_WaistAttitudeAbsolute.fill(0);
      m_WaistPositionAbsolute.resize(3);
      m_WaistPositionAbsolute.fill(0);

      m_k_Waist_kp1.setIdentity();

      m_SupportFoot = 1; // Means that we do not know which support foot it is.
      m_ReferenceFrame = WORLD_FRAME;

      sotDEBUGIN(5);

      firstSINTERN.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
      // TODO: here, the 'setConstant' destroy the pointer toward
      // function initOneStepOfControl. By calling firstSINTERN(t), whatever t,
      // nothing will happen (well, it will just return 0).
      // To initialize firstSINTERN (without destroying the pointer), use
      // firstSINTERN.setReady() instead.
      // TODO: Remove the next line: // firstSINTERN.setConstant(0);
      firstSINTERN.setReady(true);

      //OneStepOfControlS.setDependencyType(TimeDependency<int>::ALWAYS_READY);
      //  OneStepOfControlS.setConstant(0);

      OneStepOfControlS.addDependency( LeftFootCurrentPosSIN  );
      OneStepOfControlS.addDependency( RightFootCurrentPosSIN );
      OneStepOfControlS.addDependency( velocitydesSIN );
      OneStepOfControlS.addDependency( firstSINTERN );
      OneStepOfControlS.addDependency( motorControlJointPositionSIN );
      OneStepOfControlS.addDependency( comSIN );
      OneStepOfControlS.addDependency( comStateSIN );
      OneStepOfControlS.addDependency( zmpSIN );
      OneStepOfControlS.addDependency( forceSIN );

      // For debug, register OSOC (not relevant for normal use).
      signalRegistration( OneStepOfControlS );

#if 0

      signalRegistration( jointPositionSIN <<
                          motorControlJointPositionSIN <<
                          ZMPPreviousControllerSIN <<
                          ZMPRefSOUT <<
                          CoMRefSOUT <<
                          dCoMRefSOUT);
      signalRegistration( dataInProcessSOUT <<
                          LeftFootCurrentPosSIN <<
                          RightFootCurrentPosSIN <<
                          LeftFootRefSOUT <<
                          RightFootRefSOUT);

      signalRegistration( SupportFootSOUT <<
                          jointWalkingErrorPositionSOUT <<
                          waistattitudeSOUT <<
                          waistpositionSOUT <<
                          waistattitudeabsoluteSOUT <<
                          waistpositionabsoluteSOUT);

      signalRegistration( comattitudeSOUT <<
                          dcomattitudeSOUT );

      signalRegistration( dotLeftFootRefSOUT <<
                          dotRightFootRefSOUT);

      signalRegistration( InitZMPRefSOUT <<
                          InitCoMRefSOUT <<
                          InitWaistPosRefSOUT <<
                          InitWaistAttRefSOUT <<
                          InitLeftFootRefSOUT <<
                          InitRightFootRefSOUT <<
                          comSIN <<
                          velocitydesSIN);
#else
      signalRegistration( dataInProcessSOUT );

      signalRegistration( jointPositionSIN <<
                          motorControlJointPositionSIN <<
                          ZMPPreviousControllerSIN <<
                          ZMPRefSOUT <<
                          CoMRefSOUT <<
                          dCoMRefSOUT);

      signalRegistration( comStateSIN << zmpSIN << forceSIN << forceSOUT );

      signalRegistration(comSIN <<
                         velocitydesSIN <<
                         LeftFootCurrentPosSIN <<
                         RightFootCurrentPosSIN <<
                         LeftFootRefSOUT <<
                         RightFootRefSOUT);

      signalRegistration( SupportFootSOUT <<
                          jointWalkingErrorPositionSOUT <<
                          comattitudeSOUT <<
                          dcomattitudeSOUT <<
                          waistattitudeSOUT );

      signalRegistration( waistpositionSOUT <<
                          waistattitudeabsoluteSOUT <<
                          waistpositionabsoluteSOUT);

      signalRegistration( dotLeftFootRefSOUT <<
                          dotRightFootRefSOUT);

      signalRegistration( InitZMPRefSOUT <<
                          InitCoMRefSOUT <<
                          InitWaistPosRefSOUT <<
                          InitWaistAttRefSOUT <<
                          InitLeftFootRefSOUT <<
                          InitRightFootRefSOUT );

      signalRegistration( leftFootContactSOUT <<
                          rightFootContactSOUT);

#endif
      initCommands();

      // init filter for force signals "to be removed"
      m_bufferForce.clear();
      int n=10;
      double sum=0,tmp=0;
      m_filterWindow.resize(n+1);
      for(int i=0;i<n+1;i++)
      {
        tmp =sin((M_PI*i)/n);
        m_filterWindow[i]=tmp*tmp;
      }

      for(int i=0;i<n+1;i++)
        sum+= m_filterWindow[i];

      for(int i=0;i<n+1;i++)
        m_filterWindow[i]/= sum;

      m_initForce.resize(6);
      m_currentForces.resize(6);
      //dataInProcessSOUT.setReference( &m_dataInProcess );
      //m_wrml2urdfIndex.clear();

      sotDEBUGOUT(5);
    }

    bool PatternGenerator::InitState(void)
    {

      sotDEBUGIN(5);
      // TODO
      // Instead of (0) ie .access(0), it could be rather used:
      // .accessCopy()
      // Instead of copy value (ml::Vector pos) it could be rather
      // used reference (const ml::Vector & post)
      Vector res;
	  MAL_VECTOR_TYPE(double) lWaistPosition;
      if (m_InitPositionByRealState)
	{
	  const Vector& pos = jointPositionSIN(m_LocalTime);

      MAL_VECTOR_RESIZE(lWaistPosition, 6);
      for(unsigned int i = 0; i < 6; ++i)
      {
        lWaistPosition(i) = pos(i);
      }
	  //  m_ZMPPrevious[2] =m_AnkleSoilDistance; // Changed the reference frame.

	  res.resize( pos.size()-6);

	  for(unsigned i=0;i<res.size();i++)
	    res(i) = pos(i+6);

	  Vector lZMPPrevious = ZMPPreviousControllerSIN(m_LocalTime);
	  for(unsigned int i=0;i<3;i++)
	    m_ZMPPrevious[i] = lZMPPrevious(i);
	}
      else
	{
	  res = motorControlJointPositionSIN(m_LocalTime);
	  //for(unsigned i=0;i<res.size();i++)
	  //res(m_wrml2urdfIndex[i]) = res(i);
	}

      Vector com = comSIN(m_LocalTime);

      m_JointErrorValuesForWalking.resize(res.size());

      sotDEBUG(5) << "m_LocalTime:" << m_LocalTime << endl;
      sotDEBUG(5) << "Joint Values:" << res << endl;

      try
	{
	  boost::numeric::ublas::vector<double> bres; bres.resize(res.size());
	  for(int i=0;i<res.size();i++) bres(i) = res(i);
	  m_PGI->SetCurrentJointValues(bres);
	  //      m_ZMPPrevious[2] = -m_AnkleSoilDistance; // Changed the reference frame.

	  // Evaluate current position of the COM, ZMP and feet
	  // according to the state of the robot.
	  PatternGeneratorJRL::COMState lStartingCOMState;
	  MAL_S3_VECTOR_TYPE(double) lStartingZMPPosition;
	  PatternGeneratorJRL::FootAbsolutePosition InitLeftFootAbsPos;
	  PatternGeneratorJRL::FootAbsolutePosition InitRightFootAbsPos;

	  m_PGI->EvaluateStartingState(lStartingCOMState,
				       lStartingZMPPosition,
				       lWaistPosition,
				       InitLeftFootAbsPos,
				       InitRightFootAbsPos);

	  // Put inside sotHomogeneous representation
	  m_InitCOMRefPos(0) = lStartingCOMState.x[0];
	  m_InitCOMRefPos(1) = lStartingCOMState.y[0];
	  m_InitCOMRefPos(2) = lStartingCOMState.z[0];

	  m_InitZMPRefPos(0) = lStartingCOMState.x[0];
	  m_InitZMPRefPos(1) = lStartingCOMState.y[0];
	  m_InitZMPRefPos(2) = 0;

	  if (m_InitPositionByRealState)
	    {
	      m_ZMPPrevious[0] = lStartingCOMState.x[0];
	      m_ZMPPrevious[1] = lStartingCOMState.y[0];
	      m_ZMPPrevious[2] = 0;
	    }
	  sotDEBUG(5) << "InitZMPRefPos :" <<m_InitZMPRefPos<< endl;

	  m_InitWaistRefPos(0) =
	    m_WaistPositionAbsolute(0) = lWaistPosition(0);
	  m_InitWaistRefPos(1) =
	    m_WaistPositionAbsolute(1) = lWaistPosition(1);
	  m_InitWaistRefPos(2) =
	    m_WaistPositionAbsolute(2) = lWaistPosition(2);

	  m_InitWaistRefAtt(0) =
	    m_WaistAttitudeAbsolute(0) = lWaistPosition(3);
	  m_InitWaistRefAtt(1) =
	    m_WaistAttitudeAbsolute(1) = lWaistPosition(4);
	  m_InitWaistRefAtt(2) =
	    m_WaistAttitudeAbsolute(2) = lWaistPosition(5);

	  FromAbsoluteFootPosToDotHomogeneous(InitRightFootAbsPos,
					      m_InitRightFootPosition,
					      m_dotRightFootPosition);
	  FromAbsoluteFootPosToDotHomogeneous(InitLeftFootAbsPos,
					      m_InitLeftFootPosition,
					      m_dotLeftFootPosition);
	  Eigen::Matrix<double, 4,1> newtmp,oldtmp;
	  oldtmp(0) =  m_InitCOMRefPos(0); oldtmp(1) =  m_InitCOMRefPos(1);
	  oldtmp(2) =  m_InitCOMRefPos(2); oldtmp(3)=1.0;
	  newtmp = m_MotionSinceInstanciationToThisSequence* oldtmp;
	  m_InitCOMRefPos(0) = newtmp(0);	m_InitCOMRefPos(1) = newtmp(1);
	  m_InitCOMRefPos(2) = newtmp(2);

	  oldtmp(0) =  m_InitZMPRefPos(0); oldtmp(1) =  m_InitZMPRefPos(1);
	  oldtmp(2) =  m_InitZMPRefPos(2);
	  newtmp = m_MotionSinceInstanciationToThisSequence* oldtmp;
	  m_InitZMPRefPos(0) = newtmp(0); m_InitZMPRefPos(1) = newtmp(1);
	  m_InitZMPRefPos(2) = newtmp(2);

	  if (!m_InitPositionByRealState)
	    {

	      MatrixHomogeneous invInitLeftFootRef;
	      invInitLeftFootRef = m_InitLeftFootPosition.inverse();
	      m_k_Waist_kp1 = m_k_Waist_kp1 * invInitLeftFootRef;
	      m_MotionSinceInstanciationToThisSequence =
		m_MotionSinceInstanciationToThisSequence * m_k_Waist_kp1;
	    }

	  m_k_Waist_kp1 = m_InitLeftFootPosition;

	  m_InitLeftFootPosition = m_MotionSinceInstanciationToThisSequence*
	    m_InitLeftFootPosition;
	  m_InitRightFootPosition = m_MotionSinceInstanciationToThisSequence*
	    m_InitRightFootPosition;

	  m_LeftFootPosition = m_InitLeftFootPosition;
	  m_RightFootPosition = m_InitRightFootPosition;

	  sotDEBUG(5) << "m_InitCOMRefPos: " << m_InitCOMRefPos;

	  sotDEBUG(5) << "m_InitZMPRefPos: " << m_InitZMPRefPos;
	  sotDEBUG(5) << "m_LeftFootPosition: " << m_LeftFootPosition;
	  sotDEBUG(5) << "m_RightFootPosition: " << m_RightFootPosition;
	  sotDEBUG(5) << "m_MotionSinceInstanciationToThisSequence" <<
	    m_MotionSinceInstanciationToThisSequence<< std::endl;

	  sotDEBUG(5) << " Init Waist Ref. Position " << m_InitWaistRefPos << endl;
	  sotDEBUG(5) << " Init Waist Ref. Attitude " << m_InitWaistRefAtt << endl;

	  sotDEBUG(5) << "ILF :" <<m_InitLeftFootPosition << " "
		      << "LRF :" <<m_InitRightFootPosition << endl;

	}
      catch(...)
	{
	  SOT_THROW ExceptionPatternGenerator( ExceptionPatternGenerator::PATTERN_GENERATOR_JRL,
					       "Error while setting the current joint values of the WPG.");
	  return false;
	}

      m_InitPositionByRealState = false;
      sotDEBUGOUT(5);
      return true;
    }

    bool PatternGenerator::buildModel( void )
    {
      bool ok=true;
      // Parsing the file.
      se3::urdf::buildModel(m_urdfFile, se3::JointModelFreeFlyer(), m_robotModel);

      m_robotData = new se3::Data(m_robotModel) ;
      // Creating the humanoid robot.
      m_PR = new pg::PinocchioRobot() ;
      m_PR->initializeRobotModelAndData(&m_robotModel,m_robotData);
      // Read xml/srdf stream
      std::ifstream srdf_stream(m_srdfFile.c_str());
      using boost::property_tree::ptree;
      ptree pt;
      try{
        read_xml(srdf_stream, pt);
        // Initialize the Right Foot
        pg::PRFoot aFoot ;
        string path = "robot.specificities.feet.right.size" ;
        BOOST_FOREACH(const ptree::value_type & v, pt.get_child(path.c_str()))
        {
          aFoot.soleHeight = v.second.get<double>("height");
          aFoot.soleWidth  = v.second.get<double>("width");
          aFoot.soleDepth  = v.second.get<double>("depth");
        } // BOOST_FOREACH
        path = "robot.specificities.feet.right.anklePosition" ;
        BOOST_FOREACH(const ptree::value_type & v, pt.get_child(path.c_str()))
        {
          aFoot.anklePosition(0) = v.second.get<double>("x");
          aFoot.anklePosition(1) = v.second.get<double>("y");
          aFoot.anklePosition(2) = v.second.get<double>("z");
        } // BOOST_FOREACH
        se3::FrameIndex ra = m_robotModel.getFrameId("r_ankle");
        aFoot.associatedAnkle = m_robotModel.frames.at(ra).parent ;
        m_PR->initializeRightFoot(aFoot);
        // Initialize the Left Foot
        path = "robot.specificities.feet.left.size" ;
        BOOST_FOREACH(const ptree::value_type & v, pt.get_child(path.c_str()))
        {
          aFoot.soleHeight = v.second.get<double>("height");
          aFoot.soleWidth  = v.second.get<double>("width");
          aFoot.soleDepth  = v.second.get<double>("depth");
        } // BOOST_FOREACH
        path = "robot.specificities.feet.left.anklePosition" ;
        BOOST_FOREACH(const ptree::value_type & v, pt.get_child(path.c_str()))
        {
          aFoot.anklePosition(0) = v.second.get<double>("x");
          aFoot.anklePosition(1) = v.second.get<double>("y");
          aFoot.anklePosition(2) = v.second.get<double>("z");
        } // BOOST_FOREACH
        se3::FrameIndex la = m_robotModel.getFrameId("l_ankle");
        aFoot.associatedAnkle = m_robotModel.frames.at(la).parent ;
        m_PR->initializeLeftFoot(aFoot);
      }catch(...)
      {
        cerr << "problem while reading the srdf file. File corrupted?" << endl;
        ok=false;
      }

      if (m_PR!=0)
    {
      pg::PRFoot * rightFoot = m_PR->rightFoot();
      if (rightFoot!=0)
        {
          vector3d AnkleInFoot;
          AnkleInFoot = rightFoot->anklePosition ;
          m_AnkleSoilDistance = fabs(AnkleInFoot(2));
        }
      else ok=false;
    }
      else ok=false;

      if (!ok)
    {
	  SOT_THROW ExceptionPatternGenerator( ExceptionPatternGenerator::PATTERN_GENERATOR_JRL,
					       "Error while creating humanoid robot dynamical model.",
					       "(PG creation process for object %s).",
					       getName().c_str());
	}
      try
	{
      m_PGI = PatternGeneratorJRL::patternGeneratorInterfaceFactory(m_PR);
	}

      catch (...)
	{
	  SOT_THROW ExceptionPatternGenerator( ExceptionPatternGenerator::PATTERN_GENERATOR_JRL,
					       "Error while allocating the Pattern Generator.",
					       "(PG creation process for object %s).",
					       getName().c_str());
	}
      m_init = true;
      return false;
    }

    PatternGenerator::
    ~PatternGenerator( void )
    {
      sotDEBUGIN(25);
      if( 0!=m_PR )
    {
      delete m_PR;
      m_PR = 0;
    }
      if( 0!=m_PGI )
    {
      delete m_PGI;
      m_PGI = 0;
    }
      if( 0!=m_robotData )
	{
      delete m_robotData;
      m_robotData = 0;
    }
      sotDEBUGOUT(25);
      return;
    }

    /* --- CONFIG --------------------------------------------------------------- */
    /* --- CONFIG --------------------------------------------------------------- */
    /* --- CONFIG --------------------------------------------------------------- */
    /* --- CONFIG --------------------------------------------------------------- */
    void PatternGenerator::
    setParamPreviewFile( const std::string& filename )
    {
      m_PreviewControlParametersFile = filename;
    }

    void PatternGenerator::
    setURDFFile( const std::string& filename )
    {
      m_urdfFile = filename;
    }
    void PatternGenerator::
    setSRDFFile( const std::string& filename )
    {
      m_srdfFile = filename;
    }
    void PatternGenerator::
    setXmlRankFile( const std::string& filename )
    {
      m_xmlRankFile = filename;
    }
    void PatternGenerator::
    addJointMapping(const std::string &link, const std::string &repName)
    {
      specialJoints_[link] = repName;
    }

    /* --- COMPUTE -------------------------------------------------------------- */
    /* --- COMPUTE -------------------------------------------------------------- */
    /* --- COMPUTE -------------------------------------------------------------- */

#include <jrl/mal/boostspecific.hh>

    Vector & PatternGenerator::
    getZMPRef(Vector & ZMPRefval, int time)
    {
      sotDEBUGIN(5);

      OneStepOfControlS(time);

      ZMPRefval.resize(3);
      ZMPRefval(0) = m_ZMPRefPos(0);
      ZMPRefval(1) = m_ZMPRefPos(1);
      ZMPRefval(2) = m_ZMPRefPos(2);
      sotDEBUG(5) << "ZMPRefPos transmitted" << m_ZMPRefPos
		  << " " << ZMPRefval << endl;

      sotDEBUGOUT(5);
      return ZMPRefval;
    }

    Vector & PatternGenerator::
    getCoMRef(Vector & CoMRefval, int time)
    {
      sotDEBUGIN(25);

      OneStepOfControlS(time);
      CoMRefval = m_COMRefPos;

      sotDEBUGOUT(25);
      return CoMRefval;
    }

    Vector & PatternGenerator::
    getdCoMRef(Vector & CoMRefval, int time)
    {
      sotDEBUGIN(25);

      OneStepOfControlS(time);
      CoMRefval = m_dCOMRefPos;

      sotDEBUGOUT(25);
      return CoMRefval;
    }

    Vector & PatternGenerator::
    getExternalForces(Vector & forces, int time)
    {
      sotDEBUGIN(25);

      OneStepOfControlS(time);
      forces = m_currentForces;

      sotDEBUGOUT(25);
      return forces;
    }

    Vector & PatternGenerator::
    getInitZMPRef(Vector & InitZMPRefval, int /*time*/)
    {
      sotDEBUGIN(25);

      sotDEBUG(25) << "InitZMPRefPos transmitted" << m_InitZMPRefPos
		   << " " << InitZMPRefval << std::endl;
      InitZMPRefval.resize(3);
      InitZMPRefval(0) = m_InitZMPRefPos(0);
      InitZMPRefval(1) = m_InitZMPRefPos(1);
      InitZMPRefval(2) = m_InitZMPRefPos(2);

      sotDEBUGOUT(25);
      return InitZMPRefval;
    }

    Vector & PatternGenerator::
    getInitCoMRef(Vector & InitCoMRefval, int /*time*/)
    {
      sotDEBUGIN(25);

      InitCoMRefval.resize(3);
      InitCoMRefval(0) = m_InitCOMRefPos(0);
      InitCoMRefval(1) = m_InitCOMRefPos(1);
      InitCoMRefval(2) = m_InitCOMRefPos(2);

      sotDEBUGOUT(25);
      return InitCoMRefval;
    }

    Vector & PatternGenerator::
    getInitWaistPosRef(Vector & InitWaistRefval, int /*time*/)
    {
      sotDEBUGIN(25);

      InitWaistRefval = m_InitWaistRefPos;

      sotDEBUGOUT(25);
      return InitWaistRefval;
    }
    VectorRollPitchYaw & PatternGenerator::
    getInitWaistAttRef(VectorRollPitchYaw & InitWaistRefval, int /*time*/)
    {
      sotDEBUGIN(25);

      for(unsigned int i=0;i<3;++i)
	InitWaistRefval(i) = m_InitWaistRefAtt(i);

      sotDEBUGOUT(25);
      return InitWaistRefval;
    }

    MatrixHomogeneous & PatternGenerator::
    getLeftFootRef(MatrixHomogeneous & LeftFootRefVal, int time)
    {
      sotDEBUGIN(25);

      OneStepOfControlS(time);
      LeftFootRefVal = m_LeftFootPosition;
      sotDEBUGOUT(25) ;
      return LeftFootRefVal;
    }
    MatrixHomogeneous & PatternGenerator::
    getRightFootRef(MatrixHomogeneous & RightFootRefval, int time)
    {
      sotDEBUGIN(25);

      OneStepOfControlS(time);

      RightFootRefval = m_RightFootPosition;
      sotDEBUGOUT(25);
      return RightFootRefval;
    }
    MatrixHomogeneous & PatternGenerator::
    getdotLeftFootRef(MatrixHomogeneous & LeftFootRefVal, int time)
    {
      sotDEBUGIN(25);

      OneStepOfControlS(time);
      LeftFootRefVal = m_dotLeftFootPosition;
      sotDEBUGOUT(25) ;
      return LeftFootRefVal;
    }
    MatrixHomogeneous & PatternGenerator::
    getdotRightFootRef(MatrixHomogeneous & RightFootRefval, int time)
    {
      sotDEBUGIN(25);

      OneStepOfControlS(time);

      RightFootRefval = m_dotRightFootPosition;
      sotDEBUGOUT(25);
      return RightFootRefval;
    }

    MatrixHomogeneous & PatternGenerator::
    getInitLeftFootRef(MatrixHomogeneous & LeftFootRefVal, int /*time*/)
    {
      sotDEBUGIN(25);

      LeftFootRefVal = m_InitLeftFootPosition;
      sotDEBUGOUT(25) ;
      return LeftFootRefVal;
    }
    MatrixHomogeneous & PatternGenerator::
    getInitRightFootRef(MatrixHomogeneous & RightFootRefval, int /*time*/)
    {
      sotDEBUGIN(25);

      RightFootRefval = m_InitRightFootPosition;
      sotDEBUGOUT(25);
      return RightFootRefval;
    }

    MatrixHomogeneous & PatternGenerator::
    getFlyingFootRef(MatrixHomogeneous & FlyingFootRefval, int time)
    {
      sotDEBUGIN(25);
      OneStepOfControlS(time);
      FlyingFootRefval = m_FlyingFootPosition;
      sotDEBUGOUT(25);
      return FlyingFootRefval;
    }

    bool & PatternGenerator ::
    getLeftFootContact(bool &res, int time)
    {
      sotDEBUGIN(25);
      OneStepOfControlS(time);
      res = m_leftFootContact;
      sotDEBUGOUT(25);
      return res;

    }

    bool & PatternGenerator ::
    getRightFootContact(bool &res, int time)
    {
      sotDEBUGIN(25);
      OneStepOfControlS(time);
      res = m_rightFootContact;
      sotDEBUGOUT(25);
      return res;

    }

    int &PatternGenerator::
    InitOneStepOfControl(int &dummy, int /*time*/)
    {
      sotDEBUGIN(15);
      // TODO: modified first to avoid the loop.
      firstSINTERN.setReady(false);
      //  buildModel();
      // Todo: modified the order of the calls
      //OneStepOfControlS(time);
      sotDEBUGIN(15);
      return dummy;
    }

    void PatternGenerator::getAbsoluteWaistPosAttHomogeneousMatrix(MatrixHomogeneous &aWaistMH)
    {

      const double cr = cos(m_WaistAttitudeAbsolute(0)); // ROLL
      const double sr = sin(m_WaistAttitudeAbsolute(0));
      const double cp = cos(m_WaistAttitudeAbsolute(1)); // PITCH
      const double sp = sin(m_WaistAttitudeAbsolute(1));
      const double cy = cos(m_WaistAttitudeAbsolute(2)); // YAW
      const double sy = sin(m_WaistAttitudeAbsolute(2));

      aWaistMH.matrix().setZero();

      aWaistMH(0,0) = cy*cp;
      aWaistMH(0,1) = cy*sp*sr-sy*cr;
      aWaistMH(0,2) = cy*sp*cr+sy*sr;
      aWaistMH(0,3) = m_WaistPositionAbsolute(0);

      aWaistMH(1,0) = sy*cp;
      aWaistMH(1,1) = sy*sp*sr+cy*cr;
      aWaistMH(1,2) = sy*sp*cr-cy*sr;
      aWaistMH(1,3) = m_WaistPositionAbsolute(1);

      aWaistMH(2,0) = -sp;
      aWaistMH(2,1) = cp*sr;
      aWaistMH(2,2) = cp*cr;
      aWaistMH(2,3) = m_WaistPositionAbsolute(2);

      aWaistMH(3,3) = 1.0;

    }

    void PatternGenerator::FromAbsoluteFootPosToDotHomogeneous(pg::FootAbsolutePosition aFootPosition,
							       MatrixHomogeneous &aFootMH,
							       MatrixHomogeneous &adotFootMH)
    {
      MatrixRotation dRot,Twist,Rot;
      adotFootMH.setIdentity();
      FromAbsoluteFootPosToHomogeneous(aFootPosition,aFootMH);

      for(unsigned int i=0;i<3;i++)
	for(unsigned int j=0;j<3;j++)
	  Rot(i,j) = aFootMH(i,j);

      Twist(0,0)=0.0; Twist(0,1)= -aFootPosition.dtheta; Twist(0,2) = aFootPosition.domega;
      Twist(1,0)= aFootPosition.dtheta; Twist(1,1)= 0.0; Twist(1,2) = aFootPosition.domega2;
      Twist(2,0)= -aFootPosition.domega; Twist(2,1)= -aFootPosition.domega2; Twist(2,2) = 0.0;

      dRot = Twist * Rot;

      for(unsigned int i=0;i<3;i++)
	for(unsigned int j=0;j<3;j++)
	  adotFootMH(i,j) = dRot(i,j);

      adotFootMH(0,3) = aFootPosition.dx;
      adotFootMH(1,3) = aFootPosition.dy;
      adotFootMH(2,3) = aFootPosition.dz;

    }

    void PatternGenerator::FromAbsoluteFootPosToHomogeneous(pg::FootAbsolutePosition aFootPosition,
							    MatrixHomogeneous &aFootMH)
    {
      double c,s,co,so;
      c = cos(aFootPosition.theta*M_PI/180.0);
      s = sin(aFootPosition.theta*M_PI/180.0);

      co = cos(aFootPosition.omega*M_PI/180.0);
      so = sin(aFootPosition.omega*M_PI/180.0);

      aFootMH(0,0) = c*co;        aFootMH(0,1) = -s;       aFootMH(0,2) = c*so;
      aFootMH(1,0) = s*co;        aFootMH(1,1) =  c;       aFootMH(1,2) = s*so;
      aFootMH(2,0) = -so ;        aFootMH(2,1) =  0;       aFootMH(2,2) =   co;
      aFootMH(3,0) = 0 ;          aFootMH(3,1) =  0;       aFootMH(3,2) =   0;

      aFootMH(0,3) = aFootPosition.x + m_AnkleSoilDistance*so;
      aFootMH(1,3) = aFootPosition.y;
      aFootMH(2,3) = aFootPosition.z + m_AnkleSoilDistance*co;
      aFootMH(3,3) = 1.0;
    }

    int &PatternGenerator::
    OneStepOfControl(int &dummy, int time)
    {
      m_LocalTime=time;
      int lSupportFoot; // Local support foot.
      // Default value
      m_JointErrorValuesForWalking.fill(0.0);
      const int robotSize = m_JointErrorValuesForWalking.size()+6;

      try
	{
	  for(unsigned int i=0;i<3;i++)
	    m_ZMPRefPos(i) = m_ZMPPrevious[i];
	}
      catch(...)
	{ m_ZMPRefPos(0) = m_ZMPRefPos(1) = m_ZMPRefPos(2) = 0.0;
	  m_ZMPRefPos(3) = 1.0;};
      //  m_WaistAttitudeAbsolute.fill(0);
      //  m_WaistPositionAbsolute.fill(0);

      try
	{
	  m_LeftFootPosition = LeftFootCurrentPosSIN(time);
	  m_RightFootPosition = RightFootCurrentPosSIN(time);
	}
      catch (...)
	{ };

      try
	{
	  m_VelocityReference = velocitydesSIN(time);
	}
      catch(...)
	{ };

      sotDEBUG(25) << "LeftFootCurrentPos:  " << m_LeftFootPosition << endl;
      sotDEBUG(25) << "RightFootCurrentPos:  " << m_RightFootPosition << endl;

      sotDEBUGIN(15);
      if (m_PGI!=0)
	{
	  // TODO: Calling firstSINTERN may cause an infinite loop
	  // since the function initonestepofcontrol calls without
	  // control this actual function. 'Hopefully', the function
	  // pointer of firstSINTERN has been earlier destroyed
	  // by setconstant(0).
	  firstSINTERN(time);
	  Vector CurrentState = motorControlJointPositionSIN(time);
	  assert( CurrentState.size() == robotSize );

	  /*! \brief Absolute Position for the left and right feet. */
	  pg::FootAbsolutePosition lLeftFootPosition,lRightFootPosition;
	  lLeftFootPosition.x=0.0;lLeftFootPosition.y=0.0;lLeftFootPosition.z=0.0;
	  lRightFootPosition.x=0.0;lRightFootPosition.y=0.0;lRightFootPosition.z=0.0;
	  /*! \brief Absolute position of the reference CoM. */
	  pg::COMState lCOMRefState;
	  sotDEBUG(45) << "mc = " << CurrentState << std::endl;

	  MAL_VECTOR_DIM(CurrentConfiguration,double,robotSize);
	  MAL_VECTOR_DIM(CurrentVelocity,double,robotSize);
	  MAL_VECTOR_DIM(CurrentAcceleration,double,robotSize);
	  MAL_VECTOR_DIM(ZMPTarget,double,3);
	  MAL_VECTOR_FILL(ZMPTarget,0);

	  sotDEBUG(25) << "Before One Step of control " << lCOMRefState.x[0] << " "
		       << lCOMRefState.y[0] << " " << lCOMRefState.z[0] << endl;
	  sotDEBUG(4) << " VelocityReference " << m_VelocityReference << endl;

	  m_PGI->setVelocityReference(m_VelocityReference(0),
				      m_VelocityReference(1),
				      m_VelocityReference(2));

      try{
        if(m_feedBackControl)
        {
          Vector curCoMState (3) ;
          Vector curZMP (3) ;

          curCoMState = comStateSIN(time);
          curZMP = zmpSIN(time);

          lCOMRefState.x[0] = curCoMState(0) ;
          lCOMRefState.y[0] = curCoMState(1) ;
          lCOMRefState.z[0] = curCoMState(2) ;
          lCOMRefState.x[2] = (lCOMRefState.x[0]-curZMP(0))*9.81/lCOMRefState.z[0];
          lCOMRefState.y[2] = (lCOMRefState.y[0]-curZMP(1))*9.81/lCOMRefState.z[0];
          lCOMRefState.z[2] = 0.0 ;

//          for (unsigned i=0 ; i<3 ; ++i)
//            lCOMRefState.x[i] = curCoMState(i) ;
//          for (unsigned i=0 ; i<3 ; ++i)
//            lCOMRefState.y[i] = curCoMState(i+3) ;
//          for (unsigned i=0 ; i<3 ; ++i)
//            lCOMRefState.z[i] = curCoMState(i+6) ;

//          ZMPTarget(0) = lCOMRefState.x[0]-lCOMRefState.x[2]*lCOMRefState.z[0]/9.81 ;
//          ZMPTarget(1) = lCOMRefState.y[0]-lCOMRefState.y[2]*lCOMRefState.z[0]/9.81 ;
//          ZMPTarget(2) = 0.0; //to be fixed considering the support foot
        }
      }catch(...)
      {
        cout << "problems with signals reading" << endl;
        useFeedBackSignals(false);
      };

      try{
          if(m_forceFeedBack)
          {
        Vector extForce (3);
        extForce = forceSIN(time);
        if(time<50*0.005)
        {
          m_initForce=extForce;
        }
        extForce -= m_initForce;
        unsigned int n=321;
        if(m_bufferForce.size()<n-1)
        {
          m_bufferForce.push_back(extForce);
        }
        else
        {
          m_bufferForce.push_back(extForce);
          double ltmp1(0.0), ltmp2(0.0), ltmp3(0.0) ;
          for(unsigned int k=0;k<m_filterWindow.size();k++)
          {
            ltmp1 += m_filterWindow[k]*m_bufferForce[n-1-k](0);
            ltmp2 += m_filterWindow[k]*m_bufferForce[n-1-k](1);
            ltmp3 += m_filterWindow[k]*m_bufferForce[n-1-k](2);
          }
          extForce(0) = ltmp1 ;
          extForce(1) = ltmp2 ;
          extForce(2) = ltmp3 ;
          m_bufferForce.pop_front();
        }
        double threshold = 7.0;
        double thresholdy = 4.0;
        if(extForce(0)>threshold)
          extForce(0)=threshold;
        if(extForce(0)<-threshold)
          extForce(0)=-threshold;

        if(extForce(1)>thresholdy)
          extForce(1)=thresholdy;
        if(extForce(1)<-thresholdy)
          extForce(1)=-thresholdy;

        if(extForce(2)>threshold)
          extForce(2)=threshold;
        if(extForce(2)<-threshold)
          extForce(2)=-threshold;

        if((extForce(0)*extForce(0)+extForce(1)*extForce(1)) < 100)
        {
          extForce(0)=0.0;
          extForce(1)=0.0;
        }
        m_currentForces = extForce ;
        ostringstream oss ("");
        //oss << ":perturbationforce " << extForce(0) << " " << extForce(1) << " " << extForce(2);
        oss << ":perturbationforce " << -m_currentForces(1) << " " << /*m_currentForces(0)*/0.0 << " " << m_currentForces(2);
        // cout << oss.str() << endl ;
        pgCommandLine(oss.str());
          }
      }catch(...)
      {
        //cout << "problems with force signals reading" << endl;
      };

      // Test if the pattern value has some value to provide.
	  if (m_PGI->RunOneStepOfTheControlLoop(CurrentConfiguration,
						CurrentVelocity,
						CurrentAcceleration,
						ZMPTarget,
						lCOMRefState,
						lLeftFootPosition,
						lRightFootPosition))
	    {
	      sotDEBUG(25) << "After One Step of control " << endl
			   << "CurrentState:" << CurrentState << endl
			   << "CurrentConfiguration:" << CurrentConfiguration << endl;

	      m_ZMPRefPos(0) = ZMPTarget[0];
	      m_ZMPRefPos(1) = ZMPTarget[1];
	      m_ZMPRefPos(2) = ZMPTarget[2];
	      m_ZMPRefPos(3) = 1.0;
	      sotDEBUG(2) << "ZMPTarget returned by the PG: "<< m_ZMPRefPos <<endl;
	      for(int i=0;i<3;i++)
		{
		  m_WaistPositionAbsolute(i) = CurrentConfiguration(i);
		  m_WaistAttitudeAbsolute(i) = CurrentConfiguration(i+3);
		}
	      m_COMRefPos(0) = lCOMRefState.x[0];
	      m_COMRefPos(1) = lCOMRefState.y[0];
	      m_COMRefPos(2) = lCOMRefState.z[0];
	      sotDEBUG(2) << "COMRefPos returned by the PG: "<< m_COMRefPos <<endl;
	      m_dCOMRefPos(0) = lCOMRefState.x[1];
	      m_dCOMRefPos(1) = lCOMRefState.y[1];
	      m_dCOMRefPos(2) = lCOMRefState.z[1];

	      m_ComAttitude(0) = lCOMRefState.roll[0];
	      m_ComAttitude(1) = lCOMRefState.pitch[0];
	      m_ComAttitude(2) = lCOMRefState.yaw[0];

	      m_dComAttitude(0) = lCOMRefState.roll[1];
	      m_dComAttitude(1) = lCOMRefState.pitch[1];
	      m_dComAttitude(2) = lCOMRefState.yaw[1];

	      sotDEBUG(2) << "dCOMRefPos returned by the PG: "<< m_dCOMRefPos <<endl;
	      sotDEBUG(2) << "CurrentState.size()"<< CurrentState.size()<<endl;
	      sotDEBUG(2) << "CurrentConfiguration.size()"<< CurrentConfiguration.size()<<endl;
	      sotDEBUG(2) << "m_JointErrorValuesForWalking.size(): "<< m_JointErrorValuesForWalking.size() <<endl;

	      // In this setting we assume that there is a proper mapping between
	      // CurrentState and CurrentConfiguration.
	      unsigned int SizeCurrentState = CurrentState.size();
	      unsigned int SizeCurrentConfiguration = CurrentConfiguration.size()-6;
	      unsigned int MinSize = std::min(SizeCurrentState,SizeCurrentConfiguration);

	      if (m_JointErrorValuesForWalking.size()>=MinSize)
		{
		  for(unsigned int li=0;li<MinSize;li++)
		    m_JointErrorValuesForWalking(li)= (CurrentConfiguration(li+6)- CurrentState(li) )/m_TimeStep;
		}
	      else
		{
		  std::cout <<"The state of the robot and the one return by the WPG are different" << std::endl;
		  sotDEBUG(25) << "Size not coherent between CurrentState and m_JointErrorValuesForWalking: "
			       << CurrentState.size()<< " "
			       << m_JointErrorValuesForWalking.size()<< " "
			       << endl;
		}
	      sotDEBUG(2) << "Juste after updating m_JointErrorValuesForWalking" << endl;

	      sotDEBUG(1) << "lLeftFootPosition : "
			  << lLeftFootPosition.x << " "
			  << lLeftFootPosition.y << " "
			  << lLeftFootPosition.z << " "
			  << lLeftFootPosition.theta << endl;
	      sotDEBUG(1) << "lRightFootPosition : "
			  << lRightFootPosition.x << " "
			  << lRightFootPosition.y << " "
			  << lRightFootPosition.z << " "
			  << lRightFootPosition.theta << endl;

	      sotDEBUG(25) << "lCOMPosition : "
			   << lCOMRefState.x[0] << " "
			   << lCOMRefState.y[0] << " "
			   << lCOMRefState.z[0] <<  endl;

	      /* Fill in the homogeneous matrix using the world reference frame*/
	      FromAbsoluteFootPosToDotHomogeneous(lLeftFootPosition,
						  m_LeftFootPosition,
						  m_dotLeftFootPosition);
	      FromAbsoluteFootPosToDotHomogeneous(lRightFootPosition,
						  m_RightFootPosition,
						  m_dotRightFootPosition);

	      /* We assume that the left foot is always the origin of the new frame. */
	      m_LeftFootPosition = m_MotionSinceInstanciationToThisSequence * m_LeftFootPosition;
	      m_RightFootPosition = m_MotionSinceInstanciationToThisSequence * m_RightFootPosition;

	       Eigen::Matrix<double, 4,1> newRefPos, oldRefPos;
	      oldRefPos(0) = m_COMRefPos(0); oldRefPos(1) = m_COMRefPos(1);
	      oldRefPos(2) = m_COMRefPos(2); oldRefPos(3) = 1.0;
	      newRefPos = m_MotionSinceInstanciationToThisSequence * oldRefPos;
	      m_COMRefPos(0) = newRefPos(0);
	      m_COMRefPos(1) = newRefPos(1);
	      m_COMRefPos(2) = newRefPos(2);

	      oldRefPos(0) = m_ZMPRefPos(0); oldRefPos(1) = m_ZMPRefPos(1);
	      oldRefPos(2) = m_ZMPRefPos(2); oldRefPos(3) = 1.0;
	      newRefPos = m_MotionSinceInstanciationToThisSequence * oldRefPos;
	      m_ZMPRefPos(0) = newRefPos(0);
	      m_ZMPRefPos(1) = newRefPos(1);
	      m_ZMPRefPos(2) = newRefPos(2);

	      sotDEBUG(25) << "lLeftFootPosition.stepType: " << lLeftFootPosition.stepType
			   << " lRightFootPosition.stepType: " << lRightFootPosition.stepType <<endl;
	      // Find the support foot feet.
	      m_leftFootContact = true;
	      m_rightFootContact = true;
	      if (lLeftFootPosition.stepType==-1)
		{
		  lSupportFoot=1; m_leftFootContact = true;
		  if (lRightFootPosition.stepType!=-1)
		    m_rightFootContact = false;
		  m_DoubleSupportPhaseState = 0;
		}
	      else if (lRightFootPosition.stepType==-1)
		{
		  lSupportFoot=0; m_rightFootContact = true;
		  if (lLeftFootPosition.stepType!=-1)
		    m_leftFootContact = false;
		  m_DoubleSupportPhaseState = 0;
		}
	      else /* m_LeftFootPosition.z ==m_RightFootPosition.z
		      We keep the previous support foot half the time of the double support phase..
		   */
		{
		  lSupportFoot=m_SupportFoot;
		}

	      /* Update the class related member. */
	      m_SupportFoot = lSupportFoot;

	      if ((m_ReferenceFrame==EGOCENTERED_FRAME) ||
		  (m_ReferenceFrame==LEFT_FOOT_CENTERED_FRAME) ||
		  (m_ReferenceFrame==WAIST_CENTERED_FRAME))
		{
		  sotDEBUG(25) << "Inside egocentered frame " <<endl;
		  MatrixHomogeneous PoseOrigin,iPoseOrigin, WaistPoseAbsolute;

		  getAbsoluteWaistPosAttHomogeneousMatrix(WaistPoseAbsolute);

		  if (m_ReferenceFrame==EGOCENTERED_FRAME)
		    {
		      if (m_SupportFoot==1)
			PoseOrigin = m_LeftFootPosition;
		      else
			PoseOrigin = m_RightFootPosition;
		    }
		  else if (m_ReferenceFrame==LEFT_FOOT_CENTERED_FRAME)
		    {
		      PoseOrigin = m_LeftFootPosition;
		    }
		  else if (m_ReferenceFrame==WAIST_CENTERED_FRAME)
		    {
		      PoseOrigin = WaistPoseAbsolute;
		    }
		  iPoseOrigin = PoseOrigin.inverse();

		  sotDEBUG(25) << "Old ComRef:  " << m_COMRefPos << endl;
		  sotDEBUG(25) << "Old LeftFootRef:  " << m_LeftFootPosition << endl;
		  sotDEBUG(25) << "Old RightFootRef:  " << m_RightFootPosition << endl;
		  sotDEBUG(25) << "Old PoseOrigin:  " << PoseOrigin << endl;

		  Eigen::Matrix<double, 4,1> lVZMPRefPos, lV2ZMPRefPos;
		  Eigen::Matrix<double, 4,1> lVCOMRefPos, lV2COMRefPos;

		  for(unsigned int li=0;li<3;li++)
		    {
		      lVZMPRefPos(li) = m_ZMPRefPos(li);
		      lVCOMRefPos(li) = m_COMRefPos(li);
		    }
		  lVZMPRefPos(3) = lVCOMRefPos(3) = 1.0;

		  // We do not touch to ZMP.
		  lV2ZMPRefPos = iPoseOrigin * (WaistPoseAbsolute * lVZMPRefPos);

		  // Put the CoM reference pos in the Pos Origin reference frame.
		  lV2COMRefPos = iPoseOrigin * lVCOMRefPos;

		  MatrixHomogeneous lMLeftFootPosition = m_LeftFootPosition;
		  MatrixHomogeneous lMRightFootPosition = m_RightFootPosition;

		  m_LeftFootPosition = iPoseOrigin * lMLeftFootPosition;
		  m_RightFootPosition = iPoseOrigin * lMRightFootPosition;

		  for(unsigned int i=0;i<3;i++)
		    {
		      m_ZMPRefPos(i) = lV2ZMPRefPos(i);
		      m_COMRefPos(i) = lV2COMRefPos(i);
		    }

		  MatrixHomogeneous lWaistPoseAbsoluste = WaistPoseAbsolute;
		  WaistPoseAbsolute = iPoseOrigin * WaistPoseAbsolute;

		  MatrixRotation newWaistRot;
		  newWaistRot = WaistPoseAbsolute.linear();
		  VectorRollPitchYaw newWaistRPY;
		  newWaistRPY = (newWaistRot.eulerAngles(2,1,0)).reverse();
		  m_WaistAttitude = newWaistRPY;

		  m_WaistPosition = WaistPoseAbsolute.translation();

		  sotDEBUG(25) << "ComRef:  " << m_COMRefPos << endl;
		  sotDEBUG(25) << "iPoseOrigin:  " << iPoseOrigin << endl;
		}
	      sotDEBUG(25) << "After egocentered frame " << endl;

	      sotDEBUG(25) << "ComRef:  " << m_COMRefPos << endl;
	      sotDEBUG(25) << "LeftFootRef:  " << m_LeftFootPosition << endl;
	      sotDEBUG(25) << "RightFootRef:  " << m_RightFootPosition << endl;
	      sotDEBUG(25) << "ZMPRefPos:  " << m_ZMPRefPos << endl;
	      sotDEBUG(25) << "m_MotionSinceInstanciationToThisSequence" <<
		m_MotionSinceInstanciationToThisSequence<< std::endl;

	      for(unsigned int i=0;i<3;i++)
		m_ZMPPrevious[i] = m_ZMPRefPos(i);

	      m_dataInProcess = 1;
	    }
	  else
	    {
	      sotDEBUG(1) << "Error while compute one step of PG."
			  << m_dataInProcess << std::endl;
	      // TODO: SOT_THROW
	      if (m_dataInProcess==1)
		{
		  MatrixHomogeneous invInitLeftFootRef,Diff;
		  invInitLeftFootRef = m_InitLeftFootPosition.inverse();
		  Diff = invInitLeftFootRef * m_LeftFootPosition;

		  m_k_Waist_kp1 = m_k_Waist_kp1 * Diff;

		}
	      m_dataInProcess = 0;
	    }
	  sotDEBUG(25) << "After computing error " << m_JointErrorValuesForWalking << endl;
	}
      else
	{
	  m_COMRefPos = comSIN.access(time);
	  m_ZMPRefPos(0) = m_COMRefPos(0);
	  m_ZMPRefPos(1) = m_COMRefPos(1);
	  m_ZMPRefPos(2) = 0.0;
	  m_ZMPRefPos(3) = 1.0;
	}
      sotDEBUG(25) << "LeftFootRef:  " << m_LeftFootPosition << endl;
      sotDEBUG(25) << "RightFootRef:  " << m_RightFootPosition << endl;
      sotDEBUG(25) << "COMRef:  " << m_COMRefPos << endl;

      sotDEBUGOUT(15);
      return dummy;
    }

    /* --- PARAMS --------------------------------------------------------------- */
    /* --- PARAMS --------------------------------------------------------------- */

    void PatternGenerator::
    initCommands( void )
    {
      using namespace command;
      addCommand("setURDFpath",
         makeCommandVoid1(*this,&PatternGenerator::setURDFFile,
                  docCommandVoid1("Set URDF directory+name.",
						  "string (path name)")));
      addCommand("setSRDFpath",
         makeCommandVoid1(*this,&PatternGenerator::setSRDFFile,
                  docCommandVoid1("Set SRDF directory+name.",
						  "string (file name)")));

      addCommand("setXmlRank",
         makeCommandVoid1(*this,&PatternGenerator::setXmlRankFile,
                  docCommandVoid1("Set XML rank file directory+name.",
                          "string (file name)")));

      std::string docstring = "    \n"
        "    Set foot parameters\n"
        "      Input:\n"
        "        - a floating point number: the sole length,\n"
        "        - a floating point number: the sole width,\n"
        "    \n";
      addCommand("setSoleParameters",
		 makeCommandVoid2(*this,&PatternGenerator::setSoleParameters,
                        docstring));

      addCommand("addJointMapping",
		 makeCommandVoid2(*this,&PatternGenerator::addJointMapping,
				  docCommandVoid1("Map link names.",
						  "string (link name)"
						  "string (rep name)")));

      addCommand("setParamPreview",
		 makeCommandVoid1(*this,&PatternGenerator::setParamPreviewFile,
				  docCommandVoid1("Set [guess what!] file",
						  "string (path/filename)")));
      // for the setFiles, need to implement the makeCmdVoid5... later
      // displayfiles... later too
      addCommand("buildModel",
       		 makeCommandVoid0(*this,
				  (void (PatternGenerator::*) (void))&PatternGenerator::buildModel,
				  docCommandVoid0("From the files, parse and build.")));
      addCommand("initState",
       		 makeCommandVoid0(*this,
				  (void (PatternGenerator::*) (void))&PatternGenerator::InitState,
				  docCommandVoid0("From q and model, compute the initial geometry.")));
      addCommand("frameReference",
       		 makeCommandVoid1(*this,
				  &PatternGenerator::setReferenceFromString,
				  docCommandVoid1("Set the reference.",
						  "string among "
						  "World|Egocentered|LeftFootcentered|Waistcentered")));

      addCommand("getTimeStep",
		 makeDirectGetter(*this,&m_TimeStep,docDirectGetter("timestep","double")));
      addCommand("setTimeStep",
		 makeDirectSetter(*this,&m_TimeStep,docDirectSetter("timestep","double")));

      addCommand("getInitByRealState",
		 makeDirectGetter(*this,&m_InitPositionByRealState,
				  docDirectGetter("initByRealState","bool")));
      addCommand("setInitByRealState",
		 makeDirectSetter(*this,&m_InitPositionByRealState,
				  docDirectSetter("initByRealState","bool")));

      addCommand("addOnLineStep",
       		 makeCommandVoid3(*this,&PatternGenerator::addOnLineStep,
				  docCommandVoid3("Add a step on line.",
						  "double (x)","double (y)","double (theta)")));
      addCommand("addStep",
       		 makeCommandVoid3(*this,&PatternGenerator::addOnLineStep,
				  docCommandVoid3("Add a step in the stack.",
						  "double (x)","double (y)","double (theta)")));
      addCommand("parseCmd",
       		 makeCommandVoid1(*this,&PatternGenerator::pgCommandLine,
				  docCommandVoid1("Send the command line to the internal pg object.",
						  "string (command line)")));

      addCommand("feedBackControl",
             makeCommandVoid1(*this,&PatternGenerator::useFeedBackSignals,
                  docCommandVoid1("Enable or disable the use of the CoMfullState Signal inside the pg.",
                          "string (true or false)")));

      addCommand("dynamicFilter",
             makeCommandVoid1(*this,&PatternGenerator::useDynamicFilter,
                  docCommandVoid1("Enable or disable the use of the CoMfullState Signal inside the pg.",
                          "string (true or false)")));

      // Change next step : todo (deal with FootAbsolutePosition...).

     addCommand("debug",
       		 makeCommandVoid0(*this,
				  (void (PatternGenerator::*) (void))&PatternGenerator::debug,
				  docCommandVoid0("Launch a debug command.")));

    }

    void PatternGenerator::debug(void)
    {
      std::cout << "t = " << dataInProcessSOUT.getTime() << std::endl;
      std::cout << "deptype = " << dataInProcessSOUT.dependencyType << std::endl;
      std::cout << "child = " << dataInProcessSOUT.updateFromAllChildren << std::endl;
      std::cout << "last = " << dataInProcessSOUT.lastAskForUpdate << std::endl;

      std::cout << "inprocess = " << dataInProcessSOUT.needUpdate(40) << std::endl;
      std::cout << "onestep = " << OneStepOfControlS.needUpdate(40) << std::endl;

      dataInProcessSOUT.Signal<unsigned int,int>::access(1);
    }

    void PatternGenerator::addOnLineStep( const double & x, const double & y, const double & th)
    {
      assert( m_PGI!=0 );
      m_PGI->AddOnLineStep(x,y,th);
    }
    void PatternGenerator::addStep( const double & x, const double & y, const double & th)
    {
      assert( m_PGI!=0 );
      m_PGI->AddStepInStack(x,y,th);
    }
    void PatternGenerator::pgCommandLine( const std::string & cmdline )
    {
      assert( m_PGI!=0 );
      std::istringstream cmdArgs( cmdline );
      m_PGI->ParseCmd(cmdArgs);
    }

    void PatternGenerator::useFeedBackSignals( const bool & feedBack )
    {
      m_feedBackControl = feedBack ;
      string cmdBool = feedBack?"true":"false" ;
      assert( m_PGI!=0 );
      std::istringstream cmdArgs( ":feedBackControl " + cmdBool );
      m_PGI->ParseCmd(cmdArgs);
    }

    void PatternGenerator::useDynamicFilter(const bool & dynamicFilter )
    {
      m_feedBackControl = dynamicFilter ;
      string cmdBool = dynamicFilter?"true":"false" ;
      assert( m_PGI!=0 );
      std::istringstream cmdArgs( ":useDynamicFilter " + cmdBool );
      m_PGI->ParseCmd(cmdArgs);
    }

    int PatternGenerator::
    stringToReferenceEnum( const std::string & FrameReference )
    {
      if (FrameReference=="World") return WORLD_FRAME;
      else if (FrameReference=="Egocentered") return EGOCENTERED_FRAME;
      else if (FrameReference=="LeftFootcentered")return LEFT_FOOT_CENTERED_FRAME;
      else if (FrameReference=="Waistcentered")return WAIST_CENTERED_FRAME;
      assert( false && "String name should be in the list "
	      "World|Egocentered|LeftFootcentered|Waistcentered" );
      return 0;
    }

    void PatternGenerator::
    setReferenceFromString( const std::string & str )
    {
      m_ReferenceFrame = stringToReferenceEnum( str );
    }

    Vector & PatternGenerator::getjointWalkingErrorPosition(Vector &res,int time)
    {
      sotDEBUGIN(5);

      OneStepOfControlS(time);

      res=m_JointErrorValuesForWalking;

      sotDEBUGOUT(5);

      return res;
    }

    unsigned int & PatternGenerator::
    getSupportFoot(unsigned int &res, int /*time*/)
    {
      res = m_SupportFoot;
      return res;
    }

    VectorRollPitchYaw & PatternGenerator::
    getWaistAttitude( VectorRollPitchYaw&res, int time)
    {
      sotDEBUGIN(5);
      OneStepOfControlS(time);
      for( unsigned int i=0;i<3;++i ) { res(i) = m_WaistAttitude(i); }
      sotDEBUG(5) << "WaistAttitude: " << m_WaistAttitude << endl;
      sotDEBUGOUT(5);
      return res;
    }

    dynamicgraph::Vector & PatternGenerator::
    getdComAttitude( dynamicgraph::Vector& res, int time)
    {
      sotDEBUGIN(5);
      OneStepOfControlS(time);
      res.resize(3);
      for( unsigned int i=0;i<3;++i ) { res(i) = m_dComAttitude(i); }
      sotDEBUG(5) << "ComAttitude: " << m_dComAttitude << endl;
      sotDEBUGOUT(5);
      return res;
    }

    dynamicgraph::Vector& PatternGenerator::
    getComAttitude( dynamicgraph::Vector& res, int time)
    {
      sotDEBUGIN(5);
      OneStepOfControlS(time);
      res.resize(3);
      for( unsigned int i=0;i<3;++i ) { res(i) = m_ComAttitude(i); }
      sotDEBUG(5) << "ComAttitude: " << m_ComAttitude << endl;
      sotDEBUGOUT(5);
      return res;
    }

    VectorRollPitchYaw & PatternGenerator::getWaistAttitudeAbsolute(VectorRollPitchYaw &res, int time)
    {
      sotDEBUGIN(5);
      OneStepOfControlS(time);
      sotDEBUG(15) << "I survived one step of control" << std::endl;
      for( unsigned int i=0;i<3;++i ) { res(i) = m_WaistAttitudeAbsolute(i); }
      sotDEBUG(5) << "WaistAttitude: " << m_WaistAttitudeAbsolute << endl;
      sotDEBUGOUT(5);
      return res;
    }

    Vector & PatternGenerator::
    getWaistPosition(Vector &res, int time)
    {
      sotDEBUGIN(5);
      OneStepOfControlS(time);
      res = m_WaistPosition;
      sotDEBUG(5) << "WaistPosition: " << m_WaistPosition << endl;
      sotDEBUGOUT(5);
      return res;
    }
    Vector & PatternGenerator::
    getWaistPositionAbsolute(Vector &res, int time)
    {
      sotDEBUGIN(5);
      OneStepOfControlS(time);
      res = m_WaistPositionAbsolute;
      /* ARGH ! ->  res(2) =0*/
      sotDEBUG(5) << "WaistPosition: " << m_WaistPositionAbsolute << endl;
      sotDEBUGOUT(5);
      return res;
    }

    unsigned & PatternGenerator::
    getDataInProcess(unsigned &res, int time)
    {
      sotDEBUGIN(5);
      OneStepOfControlS(time);
      res = m_dataInProcess;
      sotDEBUG(5) << "DataInProcess: " << m_dataInProcess << endl;
      sotDEBUGOUT(5);
      return res;
    }

    void PatternGenerator::
    setSoleParameters(const double& inSoleLength, const double& inSoleWidth)
    {
      m_soleLength = inSoleLength;
      m_soleWidth  = inSoleWidth;
    }
  } // namespace dg
} // namespace sot


#include "kick.hh"

#include "../MotionScript/motionscript.hh"
#include "../util/log.hh"

#include <Eigen/Core>

using namespace bold;
using namespace Eigen;
using namespace std;

std::vector<std::shared_ptr<Kick const>> Kick::d_allKicks;

void Kick::loadAll()
{
  // TODO load from config
  // TODO produce kick params experimentally using kick-learner role

  d_allKicks.push_back(make_shared<Kick const>(
    "forward-right",
    "./motionscripts/kick-right.json",
    Bounds2d(Vector2d(0, 0), Vector2d(0.1, 0.2)),
    Vector2d(0.0, 1.853),
    Vector2d(0.065, 0.106)
  ));

  d_allKicks.push_back(make_shared<Kick const>(
    "forward-left",
    "./motionscripts/kick-left.json",
    Bounds2d(Vector2d(-0.1, 0), Vector2d(0, 0.2)),
    Vector2d(0.0, 1.853),
    Vector2d(-0.065, 0.106)
  ));

  d_allKicks.push_back(make_shared<Kick const>(
    "cross-left",
    "./motionscripts/kick-cross-left.json",
    Bounds2d(Vector2d(-0.1, 0), Vector2d(0.02, 0.16)),
    Vector2d(1.002, 1.152),
    Vector2d(-0.006, 0.105)
  ));

  d_allKicks.push_back(make_shared<Kick const>(
    "cross-right",
    "./motionscripts/kick-cross-right.json",
    Bounds2d(Vector2d(-0.02, 0), Vector2d(0.1, 0.16)),
    Vector2d(-1.002, 1.152),
    Vector2d(0.006, 0.105)
  ));

  d_allKicks.push_back(make_shared<Kick const>(
    "cross-left-80",
    "./motionscripts/kick-cross-left-80.json",
    Bounds2d(Vector2d(-0.1, 0), Vector2d(0.02, 0.2)),
    Vector2d(1.0, 0.1),
    Vector2d(-0.006, 0.105)
  ));

  d_allKicks.push_back(make_shared<Kick const>(
    "cross-right-80",
    "./motionscripts/kick-cross-right-80.json",
    Bounds2d(Vector2d(-0.02, 0), Vector2d(0.1, 0.2)),
    Vector2d(-1.0, 0.1),
    Vector2d(0.006, 0.105)
  ));
}

shared_ptr<Kick const> Kick::getById(string id)
{
  auto it = std::find_if(
    d_allKicks.begin(),
    d_allKicks.end(),
    [id](shared_ptr<Kick const> const& kick) { return kick->getId() == id; });

  if (it == d_allKicks.end())
  {
    log::error("Kick::getById") << "Requested kick with unknown id: " << id;
    throw runtime_error("Requested kick with unknown id");
  }

  return *it;
}

Kick::Kick(string id, string scriptPath, Bounds2d ballBounds, Vector2d endPos, Vector2d idealBallPos)
: d_id(id),
  d_motionScript(MotionScript::fromFile(scriptPath)),
  d_ballBounds(ballBounds),
  d_endPos(endPos),
  d_idealBallPos(idealBallPos)
{}

Maybe<Vector2d> Kick::estimateEndPos(Vector2d const& ballPos) const
{
  // TODO end pos depends upon start pos -- need means of modelling this
  // TODO model probabilistically, not absolutely, to allow learning risk/reward

  if (!d_ballBounds.contains(ballPos))
    return Maybe<Vector2d>::empty();

  return Maybe<Vector2d>(d_endPos);
}


#include "srs.h"
#include "ChiSquared.h"
#include "binheap.h"
#include "gendef.h"
#include "rand.h"
#include <cmath>
#include <string.h>

//#include <boost/math/distributions/chi_squared.hpp>

// to handle large files
#define _LARGEFILE_SOURCE
#define _LARGEFILE64_SOURCE
#define _FILE_OFFSET_BITS 64
#define O_LARGEFILE 0

SRS::SRS() {
  d = -1;
  n = -1;
  L = -1;
  B = -1;
  m = -1;
  numUsedTrees = -1;
  trees = NULL;
  a_array = NULL;
}

SRS::~SRS() {
  if (a_array) {
    delete[] a_array;
    a_array = NULL;
  }

  int l = L;
  if (numUsedTrees != -1) {
    l = numUsedTrees;
  }

  if (trees) {
    for (int i = 0; i < l; i++) {
      trees[i]->close();
      delete trees[i];
      trees[i] = NULL;
    }

    delete[] trees;
    trees = NULL;
  }
}

void SRS::init(int _d, int _n, int _B, int _L, int _m) {
  d = _d;
  n = _n;
  B = _B;
  L = _L;
  m = _m;

  gen_vectors();
}

void SRS::gen_vectors() {
  int i = -1;

  a_array = new float[m * L * d];
  for (i = 0; i < m * L * d; i++)
    a_array[i] = gaussian(0, 1);
}

/*****************************************************************
builds SRS tree(s) from a dataset. data format:
id coordinate_1 _2 ... _dim

-para-
dsPath			the dataset path
indexPath	  the folder containing the trees

-return-
0				success
1				failure

-prior-
all tree parameters properly set.
*****************************************************************/

int SRS::buildFromFile(char *_dsPath, char *_indexPath) {
  int ret = 0;

  char fname[100];
  int cnt = -1;
  int i = -1;
  int *key = NULL;
  int son = -1;
  FILE *fp = NULL;

  fp = fopen64(_dsPath, "r");
  BlockFile *d_blockFile = NULL;
  char *blk = NULL;
  int blk_pos = -1;

  if (!fp) {
    printf("Could not open the source file.\n");
    ret = 1;

    goto recycle;
  }

  fclose(fp);

  strcpy(dsPath, _dsPath);
  getFNameFromPath(dsPath, dsName);

  strcpy(indexPath, _indexPath);
  if (indexPath[strlen(indexPath) - 1] != '/')
    strcat(indexPath, "/");

  strcpy(dName, indexPath);
  strcat(dName, "data");

  strcpy(fname, indexPath);
  strcat(fname, "para");

  if (writeParaFile(fname)) {
    ret = 1;
    goto recycle;
  }

  key = new int[d];
  trees = new RtreePtr[L];

  for (i = 0; i < L; i++) {
    printf("Tree %d (out of %d)\n", i + 1, L);

    trees[i] = new ptRTree();

    getTreeFname(i, fname);
    trees[i]->init(fname, 4 * B, NULL, m);

    cnt = 0;
    // if (isBinary)
    //   fp = fopen64(dsPath, "rb");
    // else
    fp = fopen64(dsPath, "r");

    while (!feof(fp) && cnt < n) {
      freadNextEntry(fp, &son, key);
      if (son <= 0) {
        printf("Sorry no negative id please.\n");

        ret = 1;
        goto recycle;
      }
      insert(i, son, key);
      cnt++;

      // if (cnt % 1000 == 0) {
      //   printf("\tInserted %d (%d%%)\n", cnt, cnt * 100 / n);
      // }
    }

    if (cnt == n && !feof(fp)) {
      printf("The dataset is larger than you said. Giving up...\n", n);
      fclose(fp);
      break;
    }
    fclose(fp);
  }

  for (i = 0; i < L; i++) {
    trees[i]->close();
    delete trees[i];
    trees[i] = NULL;
  }

  delete[] trees;
  trees = NULL;

recycle:
  delete d_blockFile;
  delete[] blk;
  delete[] key;

  return ret;
}

/*****************************************************************
Build a binary file for a dataset. data format:
id coordinate_1 _2 ... _dim

-para-
dsPath      the dataset path
indexPath  the folder containing the binary file (same as the one containing the
trees)

-return-
0       success
1       failure
*****************************************************************/

int SRS::buildBinFromFile(char *_dsPath, char *_indexPath) {
  int ret = 0;
  char fname[100];
  int cnt = -1;
  int i = -1;
  int *key = NULL;
  int son = -1;
  FILE *fp = NULL;

  fp = fopen64(_dsPath, "r");
  BlockFile *d_blockFile = NULL;
  char *blk = NULL;
  int blk_pos = -1;

  if (!fp) {
    printf("Could not open the source file.\n");
    ret = 1;

    goto recycle;
  }

  fclose(fp);

  strcpy(dsPath, _dsPath);
  getFNameFromPath(dsPath, dsName);

  strcpy(indexPath, _indexPath);
  if (indexPath[strlen(indexPath) - 1] != '/')
    strcat(indexPath, "/");

  strcpy(dName, indexPath);
  strcat(dName, "data");

  d_blockFile = new BlockFile(dName, 4 * B);
  blk = new char[d_blockFile->blocklength];
  blk_pos = 0;

  strcpy(fname, indexPath);
  strcat(fname, "para");

  key = new int[d];
  cnt = 0;
  fp = fopen64(dsPath, "r");

  while (!feof(fp) && cnt < n) {
    freadNextEntry(fp, &son, key);
    for (int j = 0; j < d; ++j) {
      memcpy(&blk[blk_pos], &key[j], sizeof(int));
      blk_pos += sizeof(int);
      if (blk_pos == d_blockFile->blocklength) {
        blk_pos = 0;
        d_blockFile->append_block(blk);
      }
    }

    if (son <= 0) {
      printf("Sorry no negative id please.\n");

      ret = 1;
      goto recycle;
    }
    cnt++;
  }
  if (blk_pos != 0) {
    d_blockFile->append_block(blk); // Thus the last block is not clear!!!
  }

  if (cnt == n && !feof(fp)) {
    printf("The dataset is larger than you said. Giving up...\n", n);

    ret = 1;
    goto recycle;
  }

  fclose(fp);

recycle:
  delete d_blockFile;
  delete[] blk;

  return ret;
}

/*****************************************************************
continue builds SRS tree(s) from a dataset. data format:
id coordinate_1 _2 ... _dim

-para-
dsPath      the dataset path
indexPath   the folder containing the trees

-return-
0       success
1       failure

-prior-
all tree parameters properly set
*****************************************************************/

int SRS::ctBuildFromFile(char *_dsPath, char *_indexPath) {
  int ret = 0;

  char fname[100];
  int cnt = -1;
  int i = -1;
  int *key = NULL;
  // float key = -0.0;
  int son = -1;
  FILE *fp = NULL;

  fp = fopen64(_dsPath, "r");
  BlockFile *d_blockFile = NULL;
  char *blk = NULL;

  if (!fp) {
    printf("Could not open the source file.\n");

    ret = 1;

    goto recycle;
  }

  fclose(fp);

  strcpy(dsPath, _dsPath);
  getFNameFromPath(dsPath, dsName);

  strcpy(indexPath, _indexPath);
  if (indexPath[strlen(indexPath) - 1] != '/')
    strcat(indexPath, "/");

  strcpy(dName, indexPath);
  strcat(dName, "data");

  strcpy(fname, indexPath);
  strcat(fname, "para");

  key = new int[d];
  trees = new RtreePtr[L];

  for (i = 0; i < L; i++) {
    printf("Tree %d (out of %d)\n", i + 1, L);

    trees[i] = new ptRTree();

    getTreeFname(i, fname);
    trees[i]->init(fname, NULL);

    cnt = 0;
    fp = fopen64(dsPath, "r");

    while (!feof(fp) && cnt < n) {
      freadNextEntry(fp, &son, key);

      if (son <= 0) {
        printf("Sorry no negative id please.\n");

        ret = 1;
        goto recycle;
      }
      insert(i, son, key);
      cnt++;

      if (cnt % 1000 == 0) {
        printf("\tInserted %d (%d%%)\n", cnt, cnt * 100 / n);
      }
    }

    if (cnt == n && !feof(fp)) {
      printf("The dataset is larger than you said. Giving up...\n", n);

      ret = 1;
      goto recycle;
    }

    fclose(fp);
  }

  for (i = 0; i < L; i++) {
    trees[i]->close();
    delete trees[i];
    trees[i] = NULL;
  }

  delete[] trees;
  trees = NULL;

recycle:
  delete d_blockFile;
  delete[] blk;

  return ret;
}

/*****************************************************************
write the para file to the disk

-para-
fname		path of the file

-return-
0			success
1			failure
*****************************************************************/

int SRS::writeParaFile(char *_fname) {
  int ret = 0;

  int i = -1;
  int u = -1;
  FILE *fp = NULL;
  float *aVector = NULL;

  fp = fopen64(_fname, "r");

  if (fp) {
    printf("index exists. ");

    ret = 1;

    goto recycle;
  }

  fp = fopen64(_fname, "w");
  if (!fp) {
    printf("I could not create %s.\n", _fname);
    printf("Perhaps no such folder %s?\n", indexPath);

    ret = 1;

    goto recycle;
  }

  fprintf(fp, "%s\n", dsPath);
  fprintf(fp, "B = %d\n", B);
  fprintf(fp, "n = %d\n", n);
  fprintf(fp, "d = %d\n", d);
  fprintf(fp, "l = %d\n", L);
  fprintf(fp, "m = %d\n", m);

  for (i = 0; i < m * L; i++) {
    getHashPara(i, &aVector);
    fprintf(fp, "%f", aVector[0]);
    for (u = 1; u < d; u++) {
      fprintf(fp, " %f", aVector[u]);
    }
    fprintf(fp, "\n");
  }

recycle:
  if (fp)
    fclose(fp);

  return ret;
}

/*****************************************************************
get the file name of a srs-tree

-para-
i		  which tree it is, from 0 to L- 1
fname	(out) the file name
*****************************************************************/

void SRS::getTreeFname(int _i, char *_fname) {
  char c[100];

  strcpy(_fname, indexPath);
  sprintf(c, "%d", _i); // modified by Yifang
  strcat(_fname, c);
}

/*****************************************************************
insert a point into the srs-trees.

-para-
treeID	into which tree are we inserting (from 0 to L-1)
son		  the object's id
key		  its coordinates

 -return-
0		success
1		failure
*****************************************************************/

int SRS::insert(int _treeID, int _son, int *_key) {
  int ret = 0;
  Entry *e = NULL;

  e = new ptEntry();
  e->init(trees[_treeID]);
  e->level = 0;
  e->son = _son;

  for (int i = 0; i < m; ++i) {
    e->bounces[2 * i] = e->bounces[2 * i + 1] =
        getHashValue(_treeID * m + i, _key);
  }
  trees[_treeID]->insert(e);

  return ret;
}

/*****************************************************************
returns the u-th random projection vector.

-para-
u			    see above
a_vector	(out) starting address of the a-vector, which is an array of
size d.
*****************************************************************/

void SRS::getHashPara(int _u, float **_a_vector) {
  (*_a_vector) = &(a_array[_u * d]);
}

/*****************************************************************
gets the inner product of key and the u-th random vector.

-para-
u		  see above
key		raw coordinates

-return-
see above
*****************************************************************/

float SRS::getHashValue(int _u, int *_key) {
  float ret = 0;
  float *a_vector = NULL;

  getHashPara(_u, &a_vector);

  for (int i = 0; i < d; i++) {
    ret += a_vector[i] * _key[i];
  }
  return ret;
}

/*****************************************************************
load an existing srs index.

-para-
paraPath		    The full path of the parameter file
start_tree_ID   the first tree that we are going to use (default: 0)
numTrees        the number of trees that we are going to use (default: 1)

-return-
0		success
1		failure
*****************************************************************/

int SRS::restore(char *_paraPath, int _start_tree_ID, int _numTrees) {
  int ret = 0;

  char fname[100];
  int i = -1;
  int len = -1;
  numUsedTrees = _numTrees;

  strcpy(indexPath, _paraPath);

  len = strlen(indexPath);
  if (indexPath[len - 1] != '/' && indexPath[len - 1] != '\\')
    strcat(indexPath, "/");

  strcpy(dName, indexPath);
  strcat(dName, "data");

  strcpy(fname, indexPath);
  strcat(fname, "para");

  if (readParaFile(fname)) {
    ret = 1;
    goto recycle;
  }

  if (_start_tree_ID + _numTrees > L) {
    ret = 1;
    goto recycle;
  }

  trees = new RtreePtr[_numTrees];

  for (i = 0; i < _numTrees; i++) {
    getTreeFname(_start_tree_ID + i, fname);

    trees[i] = new ptRTree();
    trees[i]->init(fname, NULL);
  }

recycle:
  return ret;
}

/*****************************************************************
read the parameter file.

-para-
fname		full path of the para file

-return-
0		success
1		failure
*****************************************************************/

int SRS::readParaFile(char *_fname) {
  int ret = 0;

  FILE *fp = NULL;
  int cnt = 0;
  int i = -1;
  int j = -1;
  int k = -1;

  fp = fopen64(_fname, "r");

  if (!fp) {
    printf("Could not open %s.\n", _fname);

    ret = 1;
    goto recycle;
  }

  fscanf(fp, "%s\n", dsPath);
  getFNameFromPath(dsPath, dsName);

  fscanf(fp, "B = %d\n", &B);
  fscanf(fp, "n = %d\n", &n);
  fscanf(fp, "d = %d\n", &d);
  // fscanf(fp, "t = %d\n", &t);
  fscanf(fp, "l = %d\n", &L);
  fscanf(fp, "m = %d\n", &m);

  a_array = new float[m * L * d];

  cnt = 0;

  for (i = 0; i < L * m; i++) {
    for (k = 0; k < d; k++) {
      fscanf(fp, "%f", &a_array[cnt]);
      cnt++;
    }
    fscanf(fp, "\n");
  }

recycle:

  if (fp)
    fclose(fp);

  return ret;
}

/*****************************************************************
reads the next pt from the original data file.

-para-
fp		handle of the file
son		(out) pt id
key		pt coordinates
*****************************************************************/

void SRS::freadNextEntry(FILE *_fp, int *_son, int *_key) {
  int i;
  fscanf(_fp, "%d", _son);
  for (i = 0; i < d; i++) {
    fscanf(_fp, " %d", &(_key[i]));
  }

  fscanf(_fp, "\n");
}

/*****************************************************************
reads the pt with given id from the binary data file.

-para-
pt    (out) pt coordinates
bf   block file
id   pt id

-return-
number of I/O that cost in this step
*****************************************************************/

int SRS::readData(int *_pt, BlockFile *_bf, int _id) {
  int ret = 0;
  int blk_pos = ((long long)d * sizeof(int) * (_id - 1)) % _bf->blocklength;
  int blk_id = (long long)d * sizeof(int) * (_id - 1) / _bf->blocklength;
  char *blk = new char[_bf->blocklength];
  // printf("%d %d\n",blk_pos,blk_id);

  _bf->read_block(blk, blk_id);
  ret++;
  for (int i = 0; i < d; ++i) {
    if (blk_pos == _bf->blocklength) {
      blk_id++;
      _bf->read_block(blk, blk_id);
      ret++;
      blk_pos = 0;
    }
    memcpy(&_pt[i], &blk[blk_pos], sizeof(int));
    blk_pos += sizeof(int);
  }

  delete[] blk;
  return ret;
}

/*----------------------------------------------------------------
  auxiliary function called by SRS:knn.
  ----------------------------------------------------------------*/

int SRS_hcomp(const void *_e1, const void *_e2) {
  SRS_Hentry *e1 = (SRS_Hentry *)_e1;
  SRS_Hentry *e2 = (SRS_Hentry *)_e2;

  int ret = 0;

  if (e1->gap < e2->gap)
    ret = -1;
  else if (e1->gap > e2->gap)
    ret = 1;

  return ret;
}

/*----------------------------------------------------------------
  auxiliary function called by SRS:knn.
  ----------------------------------------------------------------*/

void SRS_hdestroy(const void *_e) {
  SRS_Hentry *e = (SRS_Hentry *)_e;

  delete e;
  e = NULL;
}

/*----------------------------------------------------------------
  auxiliary function called by SRS:knn.
  ----------------------------------------------------------------*/

float SRS::updateknn(SRS_Hentry *_rslt, SRS_Hentry *_he, int _k) {
  float ret = -1;

  int i = -1;
  int pos = -1;
  bool alreadyIn = false;

  for (i = 0; i < _k; i++) {
    if (_he->id == _rslt[i].id) {
      alreadyIn = true;
      break;
    } else if (compfloats(_he->dist, _rslt[i].dist) == -1) {
      break;
    }
  }

  pos = i;

  if (!alreadyIn && pos < _k) {
    for (i = _k - 1; i > pos; i--)
      _rslt[i].setto(&(_rslt[i - 1]));

    _rslt[pos].setto(_he);
  }
  ret = _rslt[_k - 1].dist;

  return ret;
}

/*****************************************************************
finds the top-k c-approximate nearest neighbors.

-para-
q             query
k             k of top-k
rslt          (out) top-k cANN
numTrees      number of trees that used (default 1) (only report the case of
"numTrees = 1" in the paper) start_tree_id id of the first tree (default 0) c
approximation ratio that will be used in the early termination condition tau
threshold in Algorithm 2 in the paper t             maximum proportion of points
that will be verified (i.e., T/n) early_stop    whether the early termination
condition is applied or not

-return-
number of I/O
*****************************************************************/
int SRS::knn(int *_q, int _k, SRS_Hentry *_rslt, int _start_tree_ID,
             int _numTrees, float _c, float _tau, float _t, bool early_stop) {
  int ret = 0; // total I/O
  int cnt = 0;
  int treeId = 0;
  int son = 0;
  bool again = true;
  float *qk = NULL;
  BinHeap *hp = NULL;
  BinHeapEntry *bhe = NULL;
  BinHeapEntry *bhe_top = NULL;
  SRS_Hentry *he = NULL;
  BlockFile *d_blockFile = NULL;
  float knnDist = -1;
  // boost::math::chi_squared chi(m);
  float *d_array = NULL;
  int limit = -1;

  SRS_Hentry *inner_rslt = new SRS_Hentry[_k];
  for (int i = 0; i < _k; i++) {
    inner_rslt[i].d = d;
  }

  // inner_rslt is the top-k NN found through R-tree; rslt also consider the
  // points that in the same page when reading a point from the binary file
  for (int i = 0; i < _k; i++) {
    _rslt[i].id = -1;
    _rslt[i].dist = (float)MAXREAL;
    inner_rslt[i].id = -1;
    inner_rslt[i].dist = (float)MAXREAL;
  }

  hp = new BinHeap();
  hp->compare_func = &SRS_hcomp;
  hp->destroy_data = &SRS_hdestroy;

  d_blockFile = new BlockFile(dName, 4 * B);
  knnDist = (float)MAXREAL;

  limit = (int)ceil(_t * n) + _k - 1;
  if (limit > n)
    limit = n;

  qk = new float[m * _numTrees];

  // d_array records all the read points, so if the r-tree returns a point that
  // is already read, we can save a I/O
  d_array = new float[n + 1];
  for (int i = 0; i <= n; ++i) {
    d_array[i] = -1.0;
  }

  for (int i = 0; i < _numTrees; ++i) {
    he = new SRS_Hentry();
    he->id = trees[i]->root;
    he->gap = 0;
    he->level = 1;
    he->d = d;
    he->treeId = i;
    he->dist = -1;

    bhe = new BinHeapEntry();
    bhe->data = he;
    hp->insert(bhe);

    for (int j = 0; j < m; ++j) {
      qk[i * m + j] = getHashValue((_start_tree_ID + i) * m + j, _q);
#ifdef DEBUG
      printf("%.6f ", qk[i * m + j]);
#endif
    }
#ifdef DEBUG
    printf("\n");
#endif
  }

  while (again) {
    bhe_top = hp->remove();
    if (!bhe_top) {
      again = false;
    } else {
      he = (SRS_Hentry *)bhe_top->data;
      treeId = he->treeId;
      son = he->id;

      if (he->level == 0) {
        cnt++;

        if (d_array[son] < 0) {
          int *pt = new int[d];
          int ret_temp = 0;
          // locate the page that contains point son
          int blk_pos = ((long long)d * sizeof(int) * (son - 1)) %
                        d_blockFile->blocklength;
          int blk_id =
              (long long)d * sizeof(int) * (son - 1) / d_blockFile->blocklength;
          char *blk = new char[d_blockFile->blocklength];

          d_blockFile->read_block(blk, blk_id);
          ret++;

          // read points in the page before son
          for (int i = 1;; i++) {
            int tmp_pos = blk_pos - d * sizeof(int) * i;
            if (tmp_pos < 0 || son - i < 1) {
              break;
            }

            for (int j = 0; j < d; ++j) {
              memcpy(&pt[j], &blk[tmp_pos], sizeof(int));
              tmp_pos += sizeof(int);
            }
            d_array[son - i] = l2_dist_int(pt, _q, d);

#ifndef DEBUG
            /* We comment out this block for debugging purpose. As the size of
               each point is different in compact mode and un-compact mode, the
               results for them under the same t is different (more data points
               would be checked under compact mode==>compact mode would be
               better). To make sure they will produce the same result when
               using the same parameter, we comment out this block. */
            SRS_Hentry *tmp_e = new SRS_Hentry();
            tmp_e->id = son - i;
            tmp_e->dist = d_array[son - i];
            updateknn(_rslt, tmp_e, _k);
            delete tmp_e;
#endif
          }

          // read point son
          for (int i = 0; i < d; ++i) {
            if (blk_pos == d_blockFile->blocklength) {
              blk_id++;
              d_blockFile->read_block(blk, blk_id);
              ret++;
              blk_pos = 0;
            }
            memcpy(&pt[i], &blk[blk_pos], sizeof(int));
            blk_pos += sizeof(int);
          }
          d_array[son] = l2_dist_int(pt, _q, d);

          // read point in the page after son
          for (int i = 1;; ++i) {
            int tmp_pos = blk_pos + d * sizeof(int) * (i - 1);
            if ((tmp_pos + d * sizeof(int)) > d_blockFile->blocklength ||
                son + i > n) {
              break;
            }

            for (int j = 0; j < d; ++j) {
              memcpy(&pt[j], &blk[tmp_pos], sizeof(int));
              tmp_pos += sizeof(int);
            }
            d_array[son + i] = l2_dist_int(pt, _q, d);
            if (son + i > n) {
              printf("%d %d\n", son, i);
            }

#ifndef DEBUG
            /* We comment out this block for debugging purpose. As the size of
               each point is different in compact mode and un-compact mode, the
               results for them under the same t is different (more data points
               would be checked under compact mode==>compact mode would be
               better). To make sure they will produce the same result when
               using the same parameter, we comment out this block. */
            SRS_Hentry *tmp_e = new SRS_Hentry();
            tmp_e->id = son + i;
            tmp_e->dist = d_array[son + i];
            updateknn(_rslt, tmp_e, _k);
            delete tmp_e;
#endif
          }
          delete[] pt;
          delete[] blk;
        }

        he->dist = d_array[son];
        updateknn(_rslt, he, _k);
        knnDist = updateknn(inner_rslt, he, _k);

        // early termination condition
        //        if (early_stop && (compfloats(knnDist, 0.0) == 0 || 1 - pow(1
        //        - boost::math::cdf(chi, _c * _c * he->gap / knnDist /
        //        knnDist), (double) _numTrees) >= _tau)) {
        if (early_stop &&
            (compfloats(knnDist, 0.0) == 0 ||
             (knnDist * knnDist) > (he->gap * chi2inv(_tau, m) / _c / _c))) {
          again = false;
        }

      } else {
        ret++;
        RTNode *child = new ptRTNode();
        child->init(trees[treeId], son);
        for (int i = 0; i < child->num_entries; i++) {
          he = new SRS_Hentry();
          he->id = child->entries[i]->son;
          he->level = child->level;
          he->gap = MINDIST(qk, child->entries[i]->bounces, m, treeId);
          he->treeId = treeId;
          he->d = d;

          bhe = new BinHeapEntry();
          bhe->data = he;
          hp->insert(bhe);
        }
        delete child;
      }
      // normal termination condition (i.e., verified T points)
      if (cnt >= limit) {
        again = false;
      }
    }
  }

recycle:
  if (d_blockFile) {
    delete d_blockFile;
  }

  if (qk) {
    delete[] qk;
  }

  if (d_array) {
    delete[] d_array;
  }

  delete[] inner_rslt;

  if (hp->root)
    hp->root->recursive_data_wipeout(hp->destroy_data);
  delete hp;

  return ret;
}