<'
# Copyright (c) 2012-2022, 2024-2026 LAAS/CNRS
# All rights reserved.
#
# Redistribution and use  in source  and binary  forms,  with or without
# modification, are permitted provided that the following conditions are
# met:
#
#   1. Redistributions of  source  code must retain the  above copyright
#      notice and this list of conditions.
#   2. Redistributions in binary form must reproduce the above copyright
#      notice and  this list of  conditions in the  documentation and/or
#      other materials provided with the distribution.
#
#                                      Anthony Mallet on Thu Aug  2 2012
#

if {[llength $argv] != 1} { error "expected arguments: component" }
lassign $argv component

# compute handy shortcuts
set comp [$component name]
set COMP [string toupper [$component name]]

lang c
'>
#include "autoconf/acheader.h"

#include <cerrno>
#include <cstdlib>
#include <cstdio>
#include <cstring>

#include <vector>

#include "ros/master.h"
#include "ros/topic_manager.h"

#include "<"$comp">/c/client.h"
#include "<"$comp">_msglib.h"
#include "private.h"

const struct genom_client_option genom_<"$comp">_options[] = {
  { "-i|--name", "instance name" },
  { "-t|--topic_timeout", "timeout subsciptions after that many seconds" },
  { NULL, NULL}
};


/* <"[--- ${comp}_init --------------------------------------------------]"> */

genom_public genom_client
genom_<"$comp">_client_init(int argc, char *argv[],
                   genom_event *ex, const void **exdetail)
{
  static char progname[] = "<"$comp">";
  genom_state_component *state;
  struct ros_options opts;
  char *largv[argc+1];
  genom_client h;
  int i, e;

  h = ros_newclient();
  if (!h) {
    if (ex) *ex = genom_fatal;
    if (exdetail) *exdetail = NULL;
    return NULL;
  }

  /* must shift args so that getopt() works (argv[0] must be progname) */
  largv[0] = progname;
  for(i=0; i<argc; i++) {
    largv[i+1] = argv[i];
  }

  if (ros_getopt(h, argc+1, largv, &opts)) {
    if (ex) *ex = h->ex;
    if (exdetail) *exdetail = h->exdetail;
    free(h);
    return NULL;
  }

  if (ros_init(h, argc+1, largv)) {
    if (ex) *ex = h->ex;
    if (exdetail) *exdetail = h->exdetail;
    pthread_mutex_destroy(&h->activelk);
    delete h;
    return NULL;
  }

  h->instance = strdup(opts.instance?opts.instance:"<"$comp">");
  h->topics_timeout = opts.topics_timeout;
  try {
    /* lookup server genom_state topic */
    ros::master::V_TopicInfo topics;
    if (!ros::master::getTopics(topics)) {
      genom_mwerr_detail d;
      snprintf(d.what, sizeof(d.what), "cannot list published topics");
      genom_mwerr(&d, h);
      goto error;
    }
    ros::master::V_TopicInfo::iterator it;
    for (e = 0, it = topics.begin(); it != topics.end(); it++) {
      if (it->name == "/" + std::string(h->instance) + "/genom_state") {
        e = 1;
        break;
      }
    }
    if (!e) {
      genom_mwerr_detail d;
      snprintf(d.what, sizeof(d.what),
               "%s node does not seem to be running", h->instance);
      genom_mwerr(&d, h);
      goto error;
    }

    h->node = new ros::NodeHandle;
  }
  catch (ros::Exception &e) {
    genom_mwerr_detail d;
    snprintf(d.what, sizeof(d.what), "%s", e.what());
    genom_mwerr(&d, h);
    goto error;
  }
  h->connections = new <"$comp">_genom::connections;

  /* check digest */
  if (genom_<"$comp">_client_genom_state_read(h, NULL)) goto error;
  state = genom_<"$comp">_client_genom_state_data(h);
  genom_<"$comp">_client_genom_state_disconnect(h);
  if (!state) goto error;

  if (strcmp(state->digest, "<"[$component digest]">")) {
    genom_incompatible_digest_detail d;
    strcpy(d.server.version, state->version);
    strcpy(d.server.date, state->date);
    snprintf(d.client.version, sizeof(d.client.version), "%s",
             "<"[$component name]-[$component version]">");
    snprintf(d.client.date, sizeof(d.client.date), "%s",
             "<"[clock format [clock sec]]">");
    genom_incompatible_digest(&d, h);
    goto error;
  }

  /* connect to services */
<'foreach s [$component services] {'>
<'  if {[catch {$s task}]} {'>
  static_cast< ::<"$comp">_genom::connections *>(h->connections)
    -><"[$s name]">_ = h->node->serviceClient<
      ::<"$comp">_genom::srv_<"[$s name]">::genom_input,
      ::<"$comp">_genom::srv_<"[$s name]">::genom_output
      >("/" + std::string(h->instance) + "/<"[$s name]">", true);
<'  } else {'>
  /* prevent dropping actionlib "results" by using an infinite subscription
   * queue length */
  h->node->setParam("/" + std::string(h->instance) +
                    "/<"[$s name]">/actionlib_client_sub_queue_size", 0);
  static_cast< ::<"$comp">_genom::connections *>(h->connections)
    -><"[$s name]">_ =
    new <"$comp">_genom::actionclient<
      ::<"$comp">_genom::action_<"[$s name]"> >(
      *h->node, "/" + std::string(h->instance) + "/<"[$s name]">");
<'  }'>
<'}'>

  /* check connections */
<'foreach s [$component services] {'>
<'  if {[catch {$s task}]} {'>
  if (!static_cast< ::<"$comp">_genom::connections *>(h->connections)
        -><"[$s name]">_.exists()) {
    genom_mwerr_detail d;
    snprintf(d.what, sizeof(d.what),
             "cannot connect to service %s", "<"[$s name]">");
    genom_mwerr(&d, h);
    goto error;
  }
<'  } else {'>
  for(i = 0; i < 10 * h->topics_timeout; i++) {
    if (static_cast< ::<"$comp">_genom::connections *>(h->connections)
        -><"[$s name]">_->c->isServerConnected()) {
      i = 0;
      break;
    }
    usleep(100000/*100ms*/);
  }
  if (i) {
    genom_mwerr_detail d;
    snprintf(d.what, sizeof(d.what),
             "cannot connect to action %s", "<"[$s name]">");
    genom_mwerr(&d, h);
    goto error;
  }
<'  }'>
<'}'>

  return h;

error:
  if (ex) *ex = h->ex;
  if (exdetail) *exdetail = h->exdetail;
  ros_delclient(h);
  return NULL;
}


/* <"[--- ${comp}_fini --------------------------------------------------]"> */

genom_public void
genom_<"$comp">_client_fini(genom_client h)
{
  ros_delclient(h);
}


/* <"[--- ${comp}_context -----------------------------------------------]"> */

genom_public genom_context
genom_<"$comp">_client_context(genom_client h)
{
  return h;
}


/* <"[--- ${comp}_name --------------------------------------------------]"> */

genom_public const char *
genom_<"$comp">_client_instance(genom_client h)
{
  return h->instance;
}


/* <"[--- ${comp}_eventfd -----------------------------------------------]"> */

genom_public int
genom_<"$comp">_client_eventfd(genom_client h)
{
  return h->evpipe[0];
}


/* <"[--- ${comp}_service_info ------------------------------------------]"> */

genom_public const struct genom_service_info *
genom_<"$comp">_client_service_info(genom_client h, int rqstid)
{
  return ros_service_info(h, rqstid);
}


/* <"[--- ${comp}_done --------------------------------------------------]"> */

genom_public int
genom_<"$comp">_client_done(genom_client h, int rqstid)
{
  return ros_done(h, rqstid);
}


/* <"[--- ${comp}_wait --------------------------------------------------]"> */

genom_public genom_event
genom_<"$comp">_client_wait(genom_client h, int rqstid)
{
  return ros_waitterm(h, rqstid);
}


/* <"[--- ${comp}_result ------------------------------------------------]"> */

genom_event
genom_<"$comp">_client_result(genom_client h, int rqstid,
                     genom_event *report, void **output, void **exdetail)
{
  return ros_rqstresult(h, rqstid, report, output, exdetail);
}


/* <"[--- ${comp}_clean -------------------------------------------------]"> */

genom_public genom_event
genom_<"$comp">_client_clean(genom_client h, int rqstid)
{
  return ros_clean(h, rqstid);
}


/* <"[--- ${comp}_doevents -----------------------------------------------]"> */

genom_public genom_event
genom_<"$comp">_client_doevents(genom_client h)
{
  return ros_doevents(h);
}
