PlatoonController.h

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

#ifndef CAR_PLATOONCONTROLLER_H
#define CAR_PLATOONCONTROLLER_H

#include <functional>

#include "PlatoonProtocolLib/Vehicle.h"
#include "PlatoonProtocolLib/Protocol.h"
#include "PC2CarLib/CommandReceiver.h"
#include "EgoMotion.h"

namespace car
{

enum class State { ACC, CACC_FV, CACC_LV };

class PlatoonController
{
public:
    using Callback = std::function<void()>;
    
    PlatoonController(platoonProtocol::Vehicle::Ptr c2c,
                      pc2car::CommandReceiver::Ptr pc,
                      EgoMotion& egoMotion);
      
    void run();
    
    Callback cruiseControllerNotify;

private:

    platoonProtocol::Vehicle::Ptr c2c;
    pc2car::CommandReceiver::Ptr pc;
    EgoMotion& egoMotion;
    
    State curState = State::CACC_FV;

    bool wantsPlatoon = false;

    // TODO check these values!
    platoonProtocol::PlatoonConfig platoonConfig {50.0, 0.0}; // IPD=50.0 PS=0.0
    float desSpeed = 0.0;

    // METHODS

    void run_ACC();
    void run_CACC_FV();
    void run_CACC_LV();
    
};

}

#endif //CAR_PLATOONCONTROLLER_H