humotion

#pragma once

#include <humotion/server/joint_interface.h>

#include <boost/bimap.hpp>

#include <yarp/dev/PolyDriver.h>

#include <yarp/dev/IControlLimits2.h>

#include <yarp/dev/ControlBoardInterfaces.h>

#include <yarp/os/Time.h>

#include <yarp/sig/Vector.h>

#include "icub_jointinterface.h"

#include <humotion/server/server.h>

#include <yarp/os/Network.h>

#include <yarp/os/RateThread.h>

#include <yarp/os/Time.h>

#include <yarp/os/Property.h>

#include <yarp/dev/ControlBoardInterfaces.h>

#include "icub_jointinterface.h"

//forward declaration to solve loop

class iCubJointInterface;

class iCubDataReceiver : public yarp::os::RateThread{

public:

    iCubDataReceiver(int period, iCubJointInterface *icub_jointinterface);

    bool threadInit();

    void threadRelease();

    void run();

private:

    void store_incoming_position(int icub_id, double value, double timestamp);

    void store_incoming_velocity(int icub_id, double velocity, double timestamp);

    yarp::sig::Vector calculate_velocities(yarp::sig::Vector positions,

                              yarp::sig::Vector timestamps);

    void dump_incoming_data();

    float target_eye_pan_;

    float target_eye_vergence_;

    float target_eye_pan_velocity_;

    float target_eye_vergence_velocity_;

    yarp::sig::Vector positions_;

    yarp::sig::Vector timestamps_;

    yarp::sig::Vector velocities_;

    yarp::sig::Vector commands_;

    yarp::sig::Vector previous_positions_;

    yarp::sig::Vector previous_timestamps_;

    iCubJointInterface *icub_jointinterface_;

    yarp::dev::IEncodersTimed *iencs_;

};