PlatoonController.cpp

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

#include "../../include/mainNode/PlatoonController.h"
#include <iostream>

namespace car
{

PlatoonController::PlatoonController(platoonProtocol::Vehicle::Ptr c2c,
    pc2car::CommandReceiver::Ptr pc, EgoMotion& egoMotion)
    : c2c(c2c)
      , pc(pc)
      , egoMotion(egoMotion)
      , cruiseControllerNotify() {}


void car::PlatoonController::run()
{
  std::cout << "PlatoonController was run." << std::endl;
  
  switch (curState) {
    case State::ACC: {
      run_ACC();
      break;
    }
                     
    case State::CACC_FV: {
      run_CACC_FV();
      break;
    }
                     
    case State::CACC_LV: {
      run_CACC_LV();
      break;
    }
  }
  
}


void car::PlatoonController::run_ACC() {

}

void car::PlatoonController::run_CACC_FV() {
   bool inPlatoon = c2c->isPlatoonRunning(); 
   platoonConfig = c2c->getPlatoonConfig();
   wantsPlatoon = pc->isPlatoonEnabled().get();
   
   
   std::cout << "Running PlatoonController::run_CACC_FV: inPlatoon = " << inPlatoon
     << ", wantsPlatoon = " << wantsPlatoon << std::endl;

   if (inPlatoon && wantsPlatoon) {
    cruiseControllerNotify();
    return;
   } 
   
   if (inPlatoon && !wantsPlatoon) {
     c2c->leavePlatoon();
   }
   
   c2c
   curState = State::ACC;
   cruiseControllerNotify();
   run_ACC();
}

void car::PlatoonController::run_CACC_LV() {

}

}