#ifndef MAINNODE_H
#define MAINNODE_H

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

#include "NotifiableThread.h"
#include "NetworkingLib/Networking.h"

#include "PlatoonProtocolLib/VehicleFacade.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:
    ros::NodeHandle nh;
    std::string name;
    
    // OLD needs to be removed after test! networking::Networking c2cNet; // thread  + event queue
    // OLD needs to be removed after test! platoonProtocol::Vehicle::Ptr c2c;
    platoonProtocol::
    
     
    networking::Networking pcNet; // thread  + event queue
    pc2car::CommandReceiver::Ptr pc;

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