#ifndef MAINNODE_H
#define MAINNODE_H

#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include "boost/thread.hpp"
#include "car/environmentDataMsg.h"

#include "NotifiableThread.h"
#include "NetworkingLib/Networking.h"
#include "PlatoonProtocolLib/Vehicle.h"
#include "PC2CarLib/CommandReceiver.h"

#include "PlatoonController.h"
#include "EgoMotion.h"
// #include "CruiseController.h"

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

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

    MainNode();

    ~MainNode();

private:
    networking::Networking c2cNet;
    networking::Networking pcNet;

    EgoMotion egoMotion;
    
    PlatoonController platoonController;
    NotifiableThread platoonControllerThread;
     
     std::function<void()> platoonControllerNotify =
         [this] { platoonControllerThread.notify(); };
     
    // CruiseController cruiseController;

    platoonProtocol::Vehicle::Ptr c2c;
    pc2car::CommandReceiver::Ptr commandReceiver;
      
    ros::NodeHandle nh;
    std::string name;
    
    ros::Publisher logEnabled;

    ros::Subscriber environmentData;
    void environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg);
};
}
#endif