#ifndef EGOMOTION_H
#define EGOMOTION_H

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

#include <functional>
#include <atomic>

#include "car/environmentDataMsg.h"
#include "car/stmDataMsg.h"

namespace car
{
class EgoMotion 
{
  public:
    using Callback = std::function<void()>;
    
    EgoMotion(ros::NodeHandle nh);
    ~EgoMotion(){};

    Callback platoonControllerNotify;
    Callback cruiseControllerNotify;
    
  private:
    std::atomic<float> distance{0};
    std::atomic<float> stmSpeed{0};
    std::atomic<float> relSpeed{0};
    std::atomic<float> prvSpeed{0};
    
    ros::Subscriber environmentData;
    ros::Subscriber stmData;
    
    void environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg);
    void stmDataCallback(const stmDataMsg::ConstPtr & inMsg);
    
    void computeAndNotify();
    float getDistance(){return distance;}
    float getPrvSpeed(){return prvSpeed;}
};
}

#endif