#ifndef CRUISECONTROLLER_H
#define CRUISECONTROLLER_H

#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <logging/MessageOStream.h>
#include "PlatoonProtocolLib/VehicleFacade.h"
#include "PlatoonController.h"
#include "EgoMotion.h"
#include "PlatoonState.h"

namespace car
{
class CruiseController
{
public:
    CruiseController(ros::NodeHandle & nh,
                     platoonProtocol::VehicleFacade & c2c,
                     EgoMotion & egoMotion,
                     PlatoonController & platoonController);

    void run();

private:
    MessageOStream messageOStream;
    platoonProtocol::VehicleFacade & c2c;
    EgoMotion & egoMotion;
    PlatoonController & platoonController;

    float min_speed{0};
    float max_speed{20};
    float crash_time{5};
    float stop_dist{30};

    float eps{1e-9};
    float speed{0};
    float units_conv_fac{1};

    float dist_pow{1.5};
    float speed_pow{0.5};

    void run_ACC(float target_speed);

    void run_CACC();

    ros::Publisher ccData;
};
}
#endif