PlatoonController.h

//
// Created by philipp on 12.02.18.
//

#ifndef CAR_PLATOONCONTROLLER_H
#define CAR_PLATOONCONTROLLER_H

#include <functional>

#include "PlatoonProtocolLib/VehicleFacade.h"
#include "PlatoonProtocolLib/Protocol.h"
#include "PC2CarLib/CommandReceiver.h"
#include "EgoMotion.h"
#include "PlatoonState.h"
#include <atomic>
#include <chrono>

namespace car
{

class PlatoonController
{
public:
    using Callback = std::function<void()>;

    PlatoonController(ros::NodeHandle & nh,
                      platoonProtocol::VehicleFacade & c2c,
                      pc2car::CommandReceiver::Ptr pc,
                      EgoMotion & egoMotion);

    void run();

    void setCruiseControllerNotify(const Callback & cruiseControllerNotify)
    { this->cruiseControllerNotify = cruiseControllerNotify; }

    float getDesSpeed()
    { return desSpeed.get(); }

    PlatoonState getCurrState() const
    { return currState; }

    networking::time::TimedValue<platoonProtocol::PlatoonConfig> getPlatoonConfig()
    { return platoonConfig.getNonAtomicCopy(); }

private:
    float maxPlatoonRange = 100.0; // cm
    
    MessageOStream messageOStream;
    Callback cruiseControllerNotify = [] {};
    platoonProtocol::VehicleFacade & c2c;
    pc2car::CommandReceiver::Ptr pc;
    EgoMotion & egoMotion;
    std::atomic<PlatoonState> currState{PlatoonState::ACC};
    networking::time::TimedAtomicValue<platoonProtocol::PlatoonConfig> platoonConfig;
    bool c2cAlive = false;
    // void updateC2cConfig(); // once C2C returns TimedValues

    void updatePcConfig();

    networking::time::TimedValue<platoonProtocol::PlatoonSpeed> PS{0.0f};
    networking::time::TimedValue<platoonProtocol::InnerPlatoonDistance> IPD{0.0f};

    void updateDesSpeed();

    networking::time::TimedValue<float> desSpeed{0.0f};

    void setupC2C();

    // METHODS
    void run_ACC();

    void run_CACC_FV();

    void run_CACC_LV();

};

}

#endif //CAR_PLATOONCONTROLLER_H