#ifndef MAINNODE_H
#define MAINNODE_H

#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <logging/MessageOStream.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:
    MainNode();

    void onInit() override;

private:
    ros::NodeHandle nh;
    MessageOStream messageOStream;
    
    networking::Networking c2cNet; // thread  + event queue
    platoonProtocol::VehicleFacade c2c;
     
    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(); };

    platoonProtocol::VehicleId readVehicleId();
    platoonProtocol::Vehicle::VehicleEndpoints readVehicleEndpoints();
};
}
#endif