/*******************************************************************************
 * Copyright (c) 2016 - 2017 fortiss GmbH
 * This program and the accompanying materials are made available under the
 * terms of the Eclipse Public License 2.0 which is available at
 * http://www.eclipse.org/legal/epl-2.0.
 *
 * SPDX-License-Identifier: EPL-2.0
 *
 * Contributors:
 *    Ben Schneider
 *      - initial implementation and documentation
 *******************************************************************************/

#include "ROSManager.h"
#include <ros/ros.h>
#include <extevhandlerhelper.h>

#include "TRIGGER_SERVICE_SERVER.h"
#ifdef FORTE_ENABLE_GENERATED_SOURCE_CPP
#include "TRIGGER_SERVICE_SERVER_gen.cpp"
#endif

DEFINE_FIRMWARE_FB(FORTE_TRIGGER_SERVICE_SERVER, g_nStringIdTRIGGER_SERVICE_SERVER)

const CStringDictionary::TStringId FORTE_TRIGGER_SERVICE_SERVER::scm_anDataInputNames[] = { g_nStringIdQI, g_nStringIdNAMESPACE, g_nStringIdSRVNAME, g_nStringIdSUCCESS, g_nStringIdMESSAGE };

const CStringDictionary::TStringId FORTE_TRIGGER_SERVICE_SERVER::scm_anDataInputTypeIds[] = { g_nStringIdBOOL, g_nStringIdSTRING, g_nStringIdSTRING, g_nStringIdBOOL, g_nStringIdSTRING };

const CStringDictionary::TStringId FORTE_TRIGGER_SERVICE_SERVER::scm_anDataOutputNames[] = { g_nStringIdQO, g_nStringIdSTATUS };

const CStringDictionary::TStringId FORTE_TRIGGER_SERVICE_SERVER::scm_anDataOutputTypeIds[] = { g_nStringIdBOOL, g_nStringIdSTRING };

const TForteInt16 FORTE_TRIGGER_SERVICE_SERVER::scm_anEIWithIndexes[] = { 0, 4 };
const TDataIOID FORTE_TRIGGER_SERVICE_SERVER::scm_anEIWith[] = { 0, 1, 2, 255, 0, 3, 4, 255 };
const CStringDictionary::TStringId FORTE_TRIGGER_SERVICE_SERVER::scm_anEventInputNames[] = { g_nStringIdINIT, g_nStringIdRSP };

const TDataIOID FORTE_TRIGGER_SERVICE_SERVER::scm_anEOWith[] = { 0, 1, 255, 0, 1, 255 };
const TForteInt16 FORTE_TRIGGER_SERVICE_SERVER::scm_anEOWithIndexes[] = { 0, 3, -1 };
const CStringDictionary::TStringId FORTE_TRIGGER_SERVICE_SERVER::scm_anEventOutputNames[] = { g_nStringIdINITO, g_nStringIdIND };

const SFBInterfaceSpec FORTE_TRIGGER_SERVICE_SERVER::scm_stFBInterfaceSpec = { 2, scm_anEventInputNames, scm_anEIWith, scm_anEIWithIndexes, 2, scm_anEventOutputNames, scm_anEOWith, scm_anEOWithIndexes, 5, scm_anDataInputNames, scm_anDataInputTypeIds, 2, scm_anDataOutputNames, scm_anDataOutputTypeIds, 0, 0 };

void FORTE_TRIGGER_SERVICE_SERVER::executeEvent(int pa_nEIID){
  switch (pa_nEIID){
    case scm_nEventINITID:
      //initiate
      if(!m_Initiated && QI()){

        m_RosNamespace = getExtEvHandler<CROSManager>(*this).ciecStringToStdString(NAMESPACE());
        m_RosMsgName = getExtEvHandler<CROSManager>(*this).ciecStringToStdString(SRVNAME());
        m_nh = new ros::NodeHandle(m_RosNamespace);
        m_triggerServer = m_nh->advertiseService < FORTE_TRIGGER_SERVICE_SERVER > (m_RosMsgName, &FORTE_TRIGGER_SERVICE_SERVER::triggerCallback, const_cast<FORTE_TRIGGER_SERVICE_SERVER*>(this));
        m_Initiated = true;
        STATUS() = "Server initiated";
        QO() = true;
      }
      //terminate
      else if(m_Initiated && !QI()){
        m_nh->shutdown();
        STATUS() = "Server terminated";
        QO() = false;
      }
      else{
        STATUS() = "initiation or termination failed";
        QO() = false;
      }
      sendOutputEvent(scm_nEventINITOID);
      break;

    case scm_nEventRSPID:
      STATUS() = "Processing service request finished";
      m_ResponseAvailable = true;
      break;

    case cg_nExternalEventID:
      QO() = true;
      sendOutputEvent(scm_nEventINDID);
      break;
  }
}

//TODO use or delete first parameter
bool FORTE_TRIGGER_SERVICE_SERVER::triggerCallback(std_srvs::Trigger::Request &, std_srvs::Trigger::Response &pa_resp){
  setEventChainExecutor(m_poInvokingExecEnv);
  getExtEvHandler<CROSManager>(*this).startChain(this);

  // is a response available
  ros::Rate r(2); //1Hz
  while(!m_ResponseAvailable){
    r.sleep();
  }

  //write response
  pa_resp.success = SUCCESS();
  pa_resp.message = getExtEvHandler<CROSManager>(*this).ciecStringToStdString(MESSAGE());

  m_ResponseAvailable = false;

  return true;
}