<'
# Copyright (c) 2012-2014, 2016-2019, 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 Wed Mar 14 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 <stdio.h>

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


<'foreach s [$component services] {'>
<'  if {[catch {$s task}]} continue'>

/* <"[--- ${comp}_[$s name]_action_cb -----------------------------------]"> */

static void
<"$comp">_<"[$s name]">_action_cb(
  genom_client h, int rqstid,
  actionlib::ActionClient<
    ::<"$comp">_genom::action_<"[$s name]"> >::GoalHandle g)
{
  if (g.getCommState() == actionlib::CommState::ACTIVE) {
    pthread_mutex_lock(&h->activelk);
    if (h->active_rqst[rqstid].status != ROS_CLIENT_WAIT_TERM) {
      h->active_rqst[rqstid].status = ROS_CLIENT_WAIT_TERM;
      h->evpending = 1;
    }
    pthread_mutex_unlock(&h->activelk);

  } else if (g.getCommState() == actionlib::CommState::DONE) {
    const boost::shared_ptr<
      const ::<"$comp">_genom::srv_<"[$s name]">::genom_output> rout =
      g.getResult();

    pthread_mutex_lock(&h->activelk);
    if (rout) {
      if (rout->genom_success)
        h->active_rqst[rqstid].error = genom_ok;
      else {
        const char *json = rout->genom_exdetail.c_str();
        h->active_rqst[rqstid].error =
          genom_json_scanex(&json, &h->active_rqst[rqstid].exdetail, NULL);
        h->active_rqst[rqstid].exfini = genom_tfiniex;
      }

<'  if [llength [$s parameter inout out]] {'>
      auto out = (struct genom_<"$comp">_<"[$s name]">_output *)
                 h->active_rqst[rqstid].output;
<'  }'>
<'  foreach p [$s parameter inout out] {'>
      genom_tcopy_<"[[$p type] mangle]">(
        <"[[$p type] pass reference out->[$p name]]">,
        <"[[$p type] pass value rout->[$p name]]">);
<'  }'>
    } else {
      genom_syserr_detail *d = (genom_syserr_detail *)malloc(sizeof *d);
      if (d) d->code = ENXIO;
      h->active_rqst[rqstid].exdetail = (void *)d;
      h->active_rqst[rqstid].error = genom_syserr_id;
    }
    h->active_rqst[rqstid].status = ROS_CLIENT_TERM;
    h->active_rqst[rqstid].gh.reset();
    h->evpending = 1;
    pthread_mutex_unlock(&h->activelk);
  }
}

<'}'>

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

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

<'foreach s [$component services] {'>

/* <"[--- ${comp}_[$s name]_rqst ----------------------------------------]"> */

genom_public genom_event
genom_<"$comp">_client_<"[$s name]">_rqst(
  genom_client h,
  const struct genom_<"$comp">_<"[$s name]">_input *in,
  <"$comp">_<"[$s name]">_cb sentcb, <"$comp">_<"[$s name]">_cb donecb,
  void *cb_data,
  int *rqstid)
{
<'  set svc [catch {$s task}]'>
  ::<"$comp">_genom::srv_<"[$s name]">::genom_input rin;
  genom_event ex;
  assert(h);

<'  if {!$svc} {'>
  if (!static_cast< ::<"$comp">_genom::connections *>(h->connections)
        -><"[$s name]">_->c->isServerConnected()) {
    genom_syserr_detail d = { ENXIO };
    return genom_syserr(&d, h);
  }
<'  }'>

<'  if {![llength [$s parameter in inout]]} {'>
  (void)in; /* fix -Wunused-parameter */
<'  }'>
<'  foreach p [$s parameter in inout] {'>
  genom_tcopy_<"[[$p type] mangle]">(
    <"[[$p type] pass reference rin.[$p name]]">,
    <"[[$p type] pass value in->[$p name]]">);
<'  }'>

  ex = ros_newrqst(
    h, sizeof(struct genom_<"$comp">_<"[$s name]">_output), *rqstid);
  if (ex) return ex;

  genom_<"$comp">_client_<"[$s name]">_init_output(
    (struct genom_<"$comp">_<"[$s name]">_output *)
    h->active_rqst[*rqstid].output);
  h->active_rqst[*rqstid].info =
    genom_<"$comp">_client_<"[$s name]">_info;
  h->active_rqst[*rqstid].sentcb = (genom_request_cb)sentcb;
  h->active_rqst[*rqstid].donecb = (genom_request_cb)donecb;
  h->active_rqst[*rqstid].cb_data = cb_data;

<'  if {$svc} {'>
  h->active_rqst[*rqstid].status = ROS_CLIENT_WAIT_TERM;
  ::<"$comp">_genom::srv_<"[$s name]">::genom_output rout;
  bool r = static_cast< ::<"$comp">_genom::connections *>(h->connections)
    -><"[$s name]">_.call(rin, rout);
  if (!r) {
    if (!static_cast< ::<"$comp">_genom::connections *>(h->connections)
          -><"[$s name]">_.isValid()) {
      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);
      r = static_cast< ::<"$comp">_genom::connections *>(h->connections)
        -><"[$s name]">_.call(rin, rout);
    }
  }

  pthread_mutex_lock(&h->activelk);
  h->active_rqst[*rqstid].status = ROS_CLIENT_TERM;
  if (!r) {
    pthread_mutex_unlock(&h->activelk);
    ros_clean(h, *rqstid);

    genom_syserr_detail d = { ENXIO };
    return genom_syserr(&d, h);
  } else {
<'    if [llength [$s parameter inout out]] {'>
    auto out = (struct genom_<"$comp">_<"[$s name]">_output *)
               h->active_rqst[*rqstid].output;
<'    }'>
<'    foreach p [$s parameter inout out] {'>
    genom_tcopy_<"[[$p type] mangle]">(
      <"[[$p type] pass reference out->[$p name]]">,
      <"[[$p type] pass value rout.[$p name]]">);
<'    }'>
    if (!rout.genom_success) {
      const char *json = rout.genom_exdetail.c_str();
      h->active_rqst[*rqstid].error =
        genom_json_scanex(&json, &h->active_rqst[*rqstid].exdetail, NULL);
      h->active_rqst[*rqstid].exfini = genom_tfiniex;
    }
  }
  h->evpending = 1;
  pthread_mutex_unlock(&h->activelk);
  ros_evtask_signal(); /* manually signal the callback thread */
<'  } else {'>

  /* no activelk lock here: see #063a4f1 */
  h->active_rqst[*rqstid].status = ROS_CLIENT_WAIT_ACK;
  h->active_rqst[*rqstid].gh =
    static_cast< ::<"$comp">_genom::connections *>(h->connections)
      -><"[$s name]">_->c->sendGoal(
        rin, boost::bind(&<"$comp">_<"[$s name]">_action_cb, h, *rqstid, _1),
        NULL);
<'  }'>

  return genom_ok;
}


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

genom_public genom_event
genom_<"$comp">_client_<"[$s name]">_result(
  genom_client h, int rqstid, genom_event *report,
  struct genom_<"$comp">_<"[$s name]">_output **out, void **exdetail)
{
  genom_client_s::request *a;
  genom_event s;
  void *p;

  pthread_mutex_lock(&h->activelk);
  if (rqstid < 0 || rqstid >= (signed)h->active_rqst.size()) {
    pthread_mutex_unlock(&h->activelk);
    genom_syserr_detail d = { ENOENT };
    return genom_syserr(&d, h);
  }

  a = &h->active_rqst[rqstid];
  if (a->info != genom_<"$comp">_client_<"[$s name]">_info) {
    pthread_mutex_unlock(&h->activelk);
    genom_syserr_detail d = { ENOMSG };
    return genom_syserr(&d, h);
  }
  pthread_mutex_unlock(&h->activelk);

  s = ros_rqstresult(h, rqstid, report, &p, exdetail);
  if (s) return s;
  if (out) *out = (struct genom_<"$comp">_<"[$s name]">_output *)p;
  return genom_ok;
}

<'}'>

<'foreach p [$component ports out] {'>

/* <"[--- ${comp}_[$p name]_port_cb -------------------------------------]"> */

static void
<"$comp">_<"[$p name]">_port_cb(
  gnmix_topic_connection< ::<"$comp">_genom::port_<"[$p name]"> > *p,
    const boost::shared_ptr<
      ::<"$comp">_genom::port_<"[$p name]"> const> &data)
{
  pthread_spin_lock(&p->cblock);
  p->cbdata = data;
  pthread_spin_unlock(&p->cblock);
}

<'}'>
<'foreach p [$component ports out] {'>

/* <"[--- ${comp}_[$p name] reader --------------------------------------]"> */

genom_public genom_event
genom_<"$comp">_client_<"[$p name]">_connect(
<'  if {"multiple" in [$p kind]} {'>
    genom_client h, const char *path
<'  } else {'>
    genom_client h
<'  }'>)
{
  gnmix_topic_connection< ::<"$comp">_genom::port_<"[$p name]"> > *p;
  std::string pname("/" + std::string(h->instance) + "/<"[$p name]">"
<'  if {[$p kind] == "multiple"} {'>
                    "/" + path
<'  }'>
    );
  int delay;

<'  if {[$p kind] == "multiple"} {'>
  auto &map = static_cast< ::<"$comp">_genom::connections *>
              (h->connections)-><"[$p name]">_;
  p = &map[path];
<'  } else {'>
  p = &static_cast< ::<"$comp">_genom::connections *>(h->connections)
    -><"[$p name]">_;
<'  }'>

  /* create subscription */
  pthread_spin_lock(&p->cblock);
  p->cbdata.reset();
  pthread_spin_unlock(&p->cblock);
  p->s = h->node->subscribe< ::<"$comp">_genom::port_<"[$p name]"> >(
    pname, 1,
    boost::bind(&<"$comp">_<"[$p name]">_port_cb, p, _1),
    ros::VoidConstPtr(),
    ros::TransportHints().tcpNoDelay());
  if (!p->s) return genom_no_such_outport(h);

  /* wait until first data is received.
   * this is done with an active polling here, for simplicity. */
  delay = 0;
  pthread_spin_lock(&p->cblock);
  while(!p->cbdata) {
    struct timespec req = {0, 1000000/*1ms*/}, rem;
    pthread_spin_unlock(&p->cblock);

    delay ++;
    if (delay > 3000/*3s*/) {
      genom_mwerr_detail d;
      snprintf(d.what, sizeof(d.what), "timeout reading %s", pname.c_str());
<'  if {[$p kind] == "multiple"} {'>
      map.erase(path);
<'  } else {'>
      /* explicitly shutdown so that we reenter here next time */
      p->s.shutdown();
<'  }'>
      return genom_mwerr(&d, h);
    }
    while (nanosleep(&req, &rem) == EINTR) req = rem;

    pthread_spin_lock(&p->cblock);
  }
  pthread_spin_unlock(&p->cblock);

  return genom_ok;
}

genom_public genom_event
genom_<"$comp">_client_<"[$p name]">_disconnect(
<'  if {"multiple" in [$p kind]} {'>
    genom_client h, const char *name
<'  } else {'>
    genom_client h
<'  }'>)
{
<'  if {[$p kind] == "multiple"} {'>
  auto &map = static_cast< ::<"$comp">_genom::connections *>
              (h->connections)-><"[$p name]">_;
  auto it = map.find(name);

  if (it == map.end()) {
    genom_syserr_detail d = { ENXIO };
    return genom_syserr(&d, h);
  }
  map.erase(it);
<'  } else {'>
  auto &p = static_cast< ::<"$comp">_genom::connections *>(h->connections)
            -><"[$p name]">_;
  if (!p.s) {
    genom_syserr_detail d = { ENXIO };
    return genom_syserr(&d, h);
  }
  p.s.shutdown();
<'  }'>
  return genom_ok;
}

<"[[$p datatype] argument reference]">
genom_<"$comp">_client_<"[$p name]">_data(
<'  if {"multiple" in [$p kind]} {'>
    genom_client h, const char *name
<'  } else {'>
    genom_client h
<'  }'>)
{
<'  if {[$p kind] == "multiple"} {'>
  auto &map = static_cast< ::<"$comp">_genom::connections *>
              (h->connections)-><"[$p name]">_;
  auto it = map.find(name);

  if (it == map.end()) {
    genom_syserr_detail d = { ENXIO };
    genom_syserr(&d, h);
    return NULL;
  }

  return <"[[$p datatype] pass reference it->second.data.v]">;
<'  } else {'>
  auto &p = static_cast< ::<"$comp">_genom::connections *>(h->connections)
            -><"[$p name]">_;
  return <"[[$p datatype] pass reference p.data.v]">;
<'  }'>
}

genom_public genom_event
genom_<"$comp">_client_<"[$p name]">_read(
  genom_client h,
<'  if {[$p kind] == "multiple"} {'>
  const char *path,
<'  }'>
  <"[[$p datatype] argument reference data]">)
{
  gnmix_topic_connection< ::<"$comp">_genom::port_<"[$p name]"> > *p;
  std::string pname("/" + std::string(h->instance) + "/<"[$p name]">"
<'  if {[$p kind] == "multiple"} {'>
                    "/" + path
<'  }'>
    );

<'  if {[$p kind] == "multiple"} {'>
  p = &static_cast< ::<"$comp">_genom::connections *>(h->connections)
    -><"[$p name]">_[path];
<'  } else {'>
  p = &static_cast< ::<"$comp">_genom::connections *>(h->connections)
    -><"[$p name]">_;
<'  }'>
  if (!data)
    data = <"[[$p datatype] pass reference p->data.v]">;

  /* check subscription */
  if (!p->s) {
    genom_event e = genom_<"$comp">_client_<"[$p name]">_connect(h
<'  if {[$p kind] == "multiple"} {'>, path<'  }'>);
    if (e) return e;
  }

  pthread_spin_lock(&p->cblock);
  genom_tcopy_<"[[$p datatype] mangle]">(
    data, <"[[$p datatype] pass value p->cbdata->v]">);
  pthread_spin_unlock(&p->cblock);
  return genom_ok;
}

genom_public genom_event
genom_<"$comp">_client_<"[$p name]">_port(
  genom_client h,
<'  if {[$p kind] == "multiple"} {'>
  const char *path,
<'  }'>
  <"[[$p datatype] argument reference data]">)
  __attribute((alias("genom_<"$comp">_client_<"[$p name]">_read")));

<'}'>
<'foreach p [$component ports in] {'>


/* <"[--- ${comp}_[$p name] publisher -----------------------------------]"> */

genom_public genom_event
genom_<"$comp">_client_<"[$p name]">_open(
  genom_client h, const char *path, const char **name)
{
  std::string fqpn;

  /* get the actual topic name */
  try {
    fqpn = ros::names::resolve(path);
  } catch (const std::exception &e) {
    genom_mwerr_detail d;
    snprintf(d.what, sizeof(d.what), "%s", e.what());
    return genom_mwerr(&d, h);
  }

  /* get or create handle */
  auto &p = static_cast< ::<"$comp">_genom::connections *>
    (h->connections)-><"[$p name]">_[fqpn];

  if (p.p) /* already exists locally */ goto done;

  /* advertise */
  try {
    p.name = fqpn;
    p.p = h->node->advertise<
      ::<"$comp">_genom::port_<"[$p name]"> >(fqpn, 1, true);
    if (!p.p) {
      genom_syserr_detail d = { EINVAL };
      return genom_syserr(&d, h);
    }
  } catch (const std::exception &e) {
    genom_mwerr_detail d;
    snprintf(d.what, sizeof(d.what), "%s", e.what());
    return genom_mwerr(&d, h);
  }

done:
  *name = p.name.c_str();
  return genom_ok;
}

genom_public genom_event
genom_<"$comp">_client_<"[$p name]">_close(
  genom_client h, const char *name)
{
  static_cast< ::<"$comp">_genom::connections *>
    (h->connections)-><"[$p name]">_.erase(name);
  return genom_ok;
}

<"[[$p datatype] argument reference]">
genom_<"$comp">_client_<"[$p name]">_data(
  genom_client h, const char *name)
{
  auto &map = static_cast< ::<"$comp">_genom::connections *>
              (h->connections)-><"[$p name]">_;
  auto it = map.find(name);

  if (it == map.end()) {
    genom_syserr_detail d = { ENXIO };
    genom_syserr(&d, h);
    return NULL;
  }

  return <"[[$p datatype] pass reference it->second.data.v]">;
}

genom_public genom_event
genom_<"$comp">_client_<"[$p name]">_write(
  genom_client h, const char *name,
  const <"[[$p datatype] argument reference data]">)
{
  auto &map = static_cast< ::<"$comp">_genom::connections *>
              (h->connections)-><"[$p name]">_;
  auto it = map.find(name);

  if (it == map.end()) {
    genom_syserr_detail d = { ENXIO };
    genom_syserr(&d, h);
    return NULL;
  }

  if (data)
    genom_tcopy_<"[[$p datatype] mangle]">(
      <"[[$p datatype] pass reference it->second.data.v]">,
      <"[[$p datatype] pass value [[$p datatype] deref reference data]]">);
  it->second.p.publish(it->second.data);
  return genom_ok;
}
<'}'>
