#ifndef ENVIRONMENT_H
#define ENVIRONMENT_H

#include <nodelet/nodelet.h>
#include <ros/ros.h>

#include "car/ccDataMsg.h"
#include "car/laneDataMsg.h"
#include "car/rcEnabledMsg.h"

#include "boost/thread.hpp"
#include "NetworkingLib/Networking.h"
#include "VeloxProtocolLib/Connection.h"

namespace car
{
class MavLink : public nodelet::Nodelet
{
public:
    virtual void onInit();

    MavLink(ros::NodeHandle & nh, std::string & name);

    MavLink();

    ~MavLink();

private:
    ros::NodeHandle nh_;
    std::string name_;

    ros::Publisher stmData;
    ros::Subscriber ccData;
    ros::Subscriber laneData;
    ros::Subscriber rcEnabled;

    void ccDataCallback(const ccDataMsg::ConstPtr & inMsg);

    void laneDataCallback(const laneDataMsg::ConstPtr & inMsg);

    void rcEnabledCallback(const rcEnabledMsg::ConstPtr & inMsg);

    void onStmDataReceived();

    float convertSpeedToStm(float speed) const;

    float convertSpeedFromStm(float speed) const;

    networking::Networking net;
    veloxProtocol::Connection::Ptr veloxConnection;
};

}
#endif