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(platoonProtocol::VehicleFacade& c2c,
                      pc2car::CommandReceiver::Ptr pc,
                      EgoMotion& egoMotion);
      
    void run();

    float getDesSpeed() { return desSpeed.get(); }
    
    Callback cruiseControllerNotify;
     
    // TODO make values ATOMIC !
    // these value need to be stored and atomic, since they will be pulled from other modules
    PlatoonState curState = PlatoonState::ACC;
    
    platoonProtocol::PlatoonConfig platoonConfig; // TODO needs to be removed, once C2C returns TimedValues

private:
    platoonProtocol::VehicleFacade& c2c;
    pc2car::CommandReceiver::Ptr pc;
    EgoMotion& egoMotion;

    bool c2cAlive = false;
    // void updateC2cConfig(); // once C2C returns TimedValues
    
    void updatePcConfig();
    pc2car::TimedValue<platoonProtocol::PlatoonSpeed>          PS{0.0f};
    pc2car::TimedValue<platoonProtocol::InnerPlatoonDistance> IPD{0.0f};
    
    void updateDesSpeed();
    pc2car::TimedValue<float> desSpeed{0.0f};
    
    void setupC2C();
    
    // METHODS
    void run_ACC();
    void run_CACC_FV();
    void run_CACC_LV();
    
};

}

#endif //CAR_PLATOONCONTROLLER_H