///                            ____                   ______            __          __
///                           / __ `____  ___  ____  / ____/_  ______  / /_  ____  / /
///                          / / / / __ `/ _ `/ __ `/ /   / / / / __ `/ __ `/ __ `/ /
///                         / /_/ / /_/ /  __/ / / / /___/ /_/ / /_/ / / / / /_/ / /
///                         `____/ .___/`___/_/ /_/`____/`__, / .___/_/ /_/`__,_/_/
///                             /_/                     /____/_/
///
/// A demo application showcasing the implementation of a UDRAL servo network service.
/// This application is intended to run on GNU/Linux but it is trivially adaptable to baremetal environments.
/// Please refer to the enclosed README for details.
///
/// This software is distributed under the terms of the MIT License.
/// Copyright (C) 2021 OpenCyphal <maintainers@opencyphal.org>
/// Author: Pavel Kirienko <pavel@opencyphal.org>

#define NUNAVUT_ASSERT(x) assert(x)
#include "canard.h"
#include "esp32-hal.h"
#include "pin_def.h"

// #include "socketcan.h"
#include <assert.h>
#include <o1heap.h>
#include <reg/udral/physics/dynamics/rotation/PlanarTs_0_1.h>
#include <reg/udral/physics/dynamics/rotation/Planar_0_1.h>
#include <reg/udral/physics/electricity/PowerTs_0_1.h>
#include <reg/udral/service/actuator/common/Feedback_0_1.h>
#include <reg/udral/service/actuator/common/Status_0_1.h>
#include <reg/udral/service/actuator/common/_0_1.h>
#include <reg/udral/service/common/Readiness_0_1.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <uavcan/_register/Access_1_0.h>
#include <uavcan/_register/List_1_0.h>
#include <uavcan/_register/Value_1_0.h>
#include <uavcan/node/ExecuteCommand_1_1.h>
#include <uavcan/node/GetInfo_1_0.h>
#include <uavcan/node/Heartbeat_1_0.h>
#include <uavcan/node/port/List_0_1.h>
#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
#include <uavcan/primitive/array/Real32_1_0.h>
#include <uavcan/primitive/scalar/Real32_1_0.h>
#include <unistd.h>

#include "USB.h"
#include "esp32twaican.h"
#include "register.hpp"
extern USBCDC usbserial;

#define KILO 1000L
#define MEGA ((int64_t)KILO * KILO)

#define CAN_REDUNDANCY_FACTOR 1
/// For CAN FD the queue can be smaller.
#define CAN_TX_QUEUE_CAPACITY 100

const unsigned int hz = 50;

/// We keep the state of the application here. Feel free to use static variables
/// instead if desired.

struct UdralServoState {
  /// Whether the servo is supposed to actuate the load or to stay idle (safe
  /// low-power mode).
  struct {
    struct {
      bool armed;
      CanardMicrosecond last_update_at;
    } arming;

    float vcc_volts;
    float current;

    float curr_position;      ///< [meter]                [radian]
    float curr_velocity;      ///< [meter/second]         [radian/second]
    float curr_acceleration;  ///< [(meter/second)^2]     [(radian/second)^2]
    float curr_force;         ///< [newton]               [netwon*meter]

    float motor_temperature;
    float controller_temperature;
  };

  struct {
    /// Setpoint & motion profile (unsupported constraints are to be ignored).
    /// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl
    /// As described in the linked documentation, there are two kinds of servos
    /// supported: linear and rotary. Units per-kind are:   LINEAR ROTARY
    float target_position;      ///< [meter]                [radian]
    float target_velocity;      ///< [meter/second]         [radian/second]
    float target_acceleration;  ///< [(meter/second)^2]     [(radian/second)^2]
    float target_force;         ///< [newton]               [netwon*meter]

    uavcan_primitive_array_Real32_1_0 pid_position;
    uavcan_primitive_array_Real32_1_0 pid_velocity;
  };
};

typedef void (*state_sync_f)(UdralServoState *const, const float hz);

struct UdralServoInternalState {
  CanardMicrosecond started_at;

  O1HeapInstance *heap;
  struct CanardInstance canard;
  struct CanardTxQueue canard_tx_queues[CAN_REDUNDANCY_FACTOR];

  /// The state of the business logic.
  UdralServoState servo;

  /// These values are read from the registers at startup. You can also
  /// implement hot reloading if desired. The subjects of the servo network
  /// service are defined in the UDRAL data type definitions here:
  /// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl
  struct {
    struct {
      CanardPortID servo_feedback;  //< reg.udral.service.actuator.common.Feedback
      CanardPortID servo_status;    //< reg.udral.service.actuator.common.Status
      CanardPortID servo_power;     //< reg.udral.physics.electricity.PowerTs
      CanardPortID servo_dynamics;  //< (timestamped dynamics)
    } pub;
    struct {
      CanardPortID servo_setpoint;   //< (non-timestamped dynamics)
      CanardPortID servo_readiness;  //< reg.udral.service.common.Readiness
    } sub;
  } port_id;

  /// A transfer-ID is an integer that is incremented whenever a new message is
  /// published on a given subject. It is used by the protocol for
  /// deduplication, message loss detection, and other critical things. For CAN,
  /// each value can be of type uint8_t, but we use larger types for genericity
  /// and for statistical purposes, as large values naturally contain the number
  /// of times each subject was published to.
  struct {
    uint64_t uavcan_node_heartbeat;
    uint64_t uavcan_node_port_list;
    uint64_t uavcan_pnp_allocation;
    // Messages published synchronously can share the same transfer-ID:
    uint64_t servo_fast_loop;
    uint64_t servo_1Hz_loop;
  } next_transfer_id;

  state_sync_f user_state_sync_f;
};

/// This flag is raised when the node is requested to restart.
static volatile bool g_restart_required = false;

/// A deeply embedded system should sample a microsecond-resolution non-overflowing 64-bit timer.
/// Here is a simple non-blocking implementation as an example:
/// https://github.com/PX4/sapog/blob/601f4580b71c3c4da65cc52237e62a/firmware/src/motor/realtime/motor_timer.c#L233-L274
/// Mind the difference between monotonic time and wall time. Monotonic time never changes rate or makes leaps,
/// it is therefore impossible to synchronize with an external reference. Wall time can be synchronized and therefore
/// it may change rate or make leap adjustments. The two kinds of time serve completely different purposes.
static CanardMicrosecond getMonotonicMicroseconds() { return micros(); }

typedef enum SubjectRole {
  SUBJECT_ROLE_PUBLISHER,
  SUBJECT_ROLE_SUBSCRIBER,
} SubjectRole;

/// Reads the port-ID from the corresponding standard register. The standard register schema is documented in
/// the Cyphal Specification, section for the standard service uavcan.register.Access. You can also find it here:
/// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/register/384.Access.1.0.dsdl
/// A very hands-on demo is available in Python: https://pycyphal.readthedocs.io/en/stable/pages/demo.html
static CanardPortID getSubjectID(const SubjectRole role, const char *const port_name, const char *const type_name) {
  // Deduce the register name from port name.
  const char *const role_name = (role == SUBJECT_ROLE_PUBLISHER) ? "pub" : "sub";
  char register_name[uavcan_register_Name_1_0_name_ARRAY_CAPACITY_] = {0};
  snprintf(&register_name[0], sizeof(register_name), "uavcan.%s.%s.id", role_name, port_name);

  // Set up the default value. It will be used to populate the register if it doesn't exist.
  uavcan_register_Value_1_0 val = {0};
  uavcan_register_Value_1_0_select_natural16_(&val);
  val.natural16.value.count = 1;
  val.natural16.value.elements[0] = UINT16_MAX;  // This means "undefined", per Specification, which is the default.

  // Read the register with defensive self-checks.
  registerRead(&register_name[0], &val);
  assert(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == 1));
  const uint16_t result = val.natural16.value.elements[0];

  // This part is NOT required but recommended by the Specification for enhanced introspection capabilities. It is
  // very cheap to implement so all implementations should do so. This register simply contains the name of the
  // type exposed at this port. It should be immutable, but it is not strictly required so in this implementation
  // we take shortcuts by making it mutable since it's behaviorally simpler in this specific case.
  snprintf(&register_name[0], sizeof(register_name), "uavcan.%s.%s.type", role_name, port_name);
  uavcan_register_Value_1_0_select_string_(&val);
  val._string.value.count = nunavutChooseMin(strlen(type_name), uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_);
  memcpy(&val._string.value.elements[0], type_name, val._string.value.count);
  registerWrite(&register_name[0], &val);  // Unconditionally overwrite existing value because it's read-only.

  return result;
}

static void send(struct UdralServoInternalState *const state, const CanardMicrosecond tx_deadline_usec,
                 const CanardTransferMetadata *const metadata, const size_t payload_size,
                 const void *const payload_data, const CanardMicrosecond now_usec) {
  for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) {
    const struct CanardPayload payload = {.size = payload_size, .data = payload_data};
    (void)canardTxPush(&state->canard_tx_queues[ifidx], &state->canard, tx_deadline_usec, metadata, payload, now_usec,
                       NULL);
  }
}

static void sendResponse(struct UdralServoInternalState *const state,
                         const struct CanardRxTransfer *const original_request_transfer, const size_t payload_size,
                         const void *const payload_data, const CanardMicrosecond now_usec) {
  CanardTransferMetadata meta = original_request_transfer->metadata;
  meta.transfer_kind = CanardTransferKindResponse;
  send(state, original_request_transfer->timestamp_usec + MEGA, &meta, payload_size, payload_data, now_usec);
}

/// Invoked at the rate of the fastest loop.
static void handleFastLoop(struct UdralServoInternalState *const state, const CanardMicrosecond now_usec) {
  // Apply control inputs if armed.
  if (state->servo.arming.armed) {
    // char buf[255];
    // snprintf(buf, sizeof(buf),

    Serial.printf("\rp=%.3f m    v=%.3f m/s    a=%.3f (m/s)^2    F=%.3f N    \r", (double)state->servo.target_position,
                  (double)state->servo.target_velocity, (double)state->servo.target_acceleration,
                  (double)state->servo.target_force);
  } else {
    // fprintf(stderr, "\rDISARMED    \r");
  }

  const bool anonymous = state->canard.node_id > CANARD_NODE_ID_MAX;
  const uint64_t servo_transfer_id = state->next_transfer_id.servo_fast_loop++;

  // Publish feedback if the subject is enabled and the node is non-anonymous.
  if (!anonymous && (state->port_id.pub.servo_feedback <= CANARD_SUBJECT_ID_MAX)) {
    reg_udral_service_actuator_common_Feedback_0_1 msg = {0};
    msg.heartbeat.readiness.value = state->servo.arming.armed ? reg_udral_service_common_Readiness_0_1_ENGAGED
                                                              : reg_udral_service_common_Readiness_0_1_STANDBY;
    // If there are any hardware or configuration issues, report them here:
    msg.heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL;
    // Serialize and publish the message:
    uint8_t serialized[reg_udral_service_actuator_common_Feedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
    size_t serialized_size = sizeof(serialized);
    const int8_t err =
        reg_udral_service_actuator_common_Feedback_0_1_serialize_(&msg, &serialized[0], &serialized_size);
    assert(err >= 0);
    if (err >= 0) {
      const CanardTransferMetadata transfer = {
          .priority = CanardPriorityHigh,
          .transfer_kind = CanardTransferKindMessage,
          .port_id = state->port_id.pub.servo_feedback,
          .remote_node_id = CANARD_NODE_ID_UNSET,
          .transfer_id = (CanardTransferID)servo_transfer_id,
      };
      send(state, now_usec + 10 * KILO, &transfer, serialized_size, &serialized[0], now_usec);
    }
  }

  // Publish dynamics if the subject is enabled and the node is non-anonymous.
  if (!anonymous && (state->port_id.pub.servo_dynamics <= CANARD_SUBJECT_ID_MAX)) {
    reg_udral_physics_dynamics_rotation_PlanarTs_0_1 msg = {0};
    // Our node does not synchronize its clock with the network, so we cannot timestamp our publications:
    msg.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN;

    msg.value.kinematics.angular_position.radian = state->servo.curr_position;
    msg.value.kinematics.angular_velocity.radian_per_second = state->servo.curr_velocity;
    msg.value.kinematics.angular_acceleration.radian_per_second_per_second = state->servo.curr_acceleration;
    msg.value._torque.newton_meter = state->servo.curr_force;
    // Serialize and publish the message:
    uint8_t serialized[reg_udral_physics_dynamics_rotation_PlanarTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
    size_t serialized_size = sizeof(serialized);
    const int8_t err =
        reg_udral_physics_dynamics_rotation_PlanarTs_0_1_serialize_(&msg, &serialized[0], &serialized_size);
    if (err >= 0) {
      const CanardTransferMetadata transfer = {
          .priority = CanardPriorityHigh,
          .transfer_kind = CanardTransferKindMessage,
          .port_id = state->port_id.pub.servo_dynamics,
          .remote_node_id = CANARD_NODE_ID_UNSET,
          .transfer_id = (CanardTransferID)servo_transfer_id,
      };
      send(state, now_usec + 10 * KILO, &transfer, serialized_size, &serialized[0], now_usec);
    }
  }

  // Publish power if the subject is enabled and the node is non-anonymous.
  if (!anonymous && (state->port_id.pub.servo_power <= CANARD_SUBJECT_ID_MAX)) {
    reg_udral_physics_electricity_PowerTs_0_1 msg = {0};
    // Our node does not synchronize its clock with the network, so we cannot timestamp our publications:
    msg.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN;
    // TODO populate real values:
    msg.value.current.ampere = state->servo.current;
    msg.value.voltage.volt = state->servo.vcc_volts;
    // Serialize and publish the message:
    uint8_t serialized[reg_udral_physics_electricity_PowerTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
    size_t serialized_size = sizeof(serialized);
    const int8_t err = reg_udral_physics_electricity_PowerTs_0_1_serialize_(&msg, &serialized[0], &serialized_size);
    if (err >= 0) {
      const CanardTransferMetadata transfer = {
          .priority = CanardPriorityHigh,
          .transfer_kind = CanardTransferKindMessage,
          .port_id = state->port_id.pub.servo_power,
          .remote_node_id = CANARD_NODE_ID_UNSET,
          .transfer_id = (CanardTransferID)servo_transfer_id,
      };
      send(state, now_usec + 10 * KILO, &transfer, serialized_size, &serialized[0], now_usec);
    }
  }
}

/// Invoked every second.
static void handle1HzLoop(struct UdralServoInternalState *const state, const CanardMicrosecond now_usec) {
  const bool anonymous = state->canard.node_id > CANARD_NODE_ID_MAX;
  // Publish heartbeat every second unless the local node is anonymous. Anonymous nodes shall not publish heartbeat.
  if (!anonymous) {
    Serial.println("Heartbeat");
    uavcan_node_Heartbeat_1_0 heartbeat = {0};
    heartbeat.uptime = (uint32_t)((now_usec - state->started_at) / MEGA);
    heartbeat.mode.value = uavcan_node_Mode_1_0_OPERATIONAL;
    const O1HeapDiagnostics heap_diag = o1heapGetDiagnostics(state->heap);
    if (heap_diag.oom_count > 0) {
      heartbeat.health.value = uavcan_node_Health_1_0_CAUTION;
    } else {
      heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL;
    }

    uint8_t serialized[uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
    size_t serialized_size = sizeof(serialized);
    const int8_t err = uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, &serialized[0], &serialized_size);
    assert(err >= 0);
    if (err >= 0) {
      const CanardTransferMetadata transfer = {
          .priority = CanardPriorityNominal,
          .transfer_kind = CanardTransferKindMessage,
          .port_id = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
          .remote_node_id = CANARD_NODE_ID_UNSET,
          .transfer_id = (CanardTransferID)(state->next_transfer_id.uavcan_node_heartbeat++),
      };
      send(state,
           now_usec + MEGA,  // Set transmission deadline 1 second, optimal for heartbeat.
           &transfer, serialized_size, &serialized[0], now_usec);
    }
  } else  // If we don't have a node-ID, obtain one by publishing allocation request messages until we get a response.
  {
    // The Specification says that the allocation request publication interval shall be randomized.
    // We implement randomization by calling rand() at fixed intervals and comparing it against some threshold.
    // There are other ways to do it, of course. See the docs in the Specification or in the DSDL definition here:
    // https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/pnp/8165.NodeIDAllocationData.2.0.dsdl
    // Note that a high-integrity/safety-certified application is unlikely to be able to rely on this feature.
    if (rand() > RAND_MAX / 2)  // NOLINT
    {
      // Note that this will only work over CAN FD. If you need to run PnP over Classic CAN, use message v1.0.
      uavcan_pnp_NodeIDAllocationData_1_0 msg = {0};
      msg.allocated_node_id.elements[0].value = UINT16_MAX;
      msg.allocated_node_id.count = 0;
      uint8_t id[128 / 8];
      getUniqueID(id);
      memcpy((uint8_t *)&msg.unique_id_hash, id, sizeof(msg.unique_id_hash));
      Serial.print("hash: ");
      Serial.println(msg.unique_id_hash, HEX);
      uint8_t serialized[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
      size_t serialized_size = sizeof(serialized);
      const int8_t err = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, &serialized[0], &serialized_size);
      assert(err >= 0);
      if (err >= 0) {
        const CanardTransferMetadata transfer = {
            .priority = CanardPrioritySlow,
            .transfer_kind = CanardTransferKindMessage,
            .port_id = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_,
            .remote_node_id = CANARD_NODE_ID_UNSET,
            .transfer_id = (CanardTransferID)(state->next_transfer_id.uavcan_pnp_allocation++),
        };
        send(state,  // The response will arrive asynchronously eventually.
             now_usec + MEGA, &transfer, serialized_size, &serialized[0], now_usec);
      }
    }
  }

  const uint64_t servo_transfer_id = state->next_transfer_id.servo_1Hz_loop++;

  if (!anonymous) {
    // Publish the servo status -- this is a low-rate message with low-severity diagnostics.
    reg_udral_service_actuator_common_Status_0_1 msg = {0};
    // TODO: POPULATE THE MESSAGE: temperature, errors, etc.
    msg.motor_temperature.kelvin = state->servo.motor_temperature;
    msg.controller_temperature.kelvin = state->servo.controller_temperature;
    uint8_t serialized[reg_udral_service_actuator_common_Status_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
    size_t serialized_size = sizeof(serialized);
    const int8_t err = reg_udral_service_actuator_common_Status_0_1_serialize_(&msg, &serialized[0], &serialized_size);
    assert(err >= 0);
    if (err >= 0) {
      const CanardTransferMetadata transfer = {
          .priority = CanardPriorityNominal,
          .transfer_kind = CanardTransferKindMessage,
          .port_id = state->port_id.pub.servo_status,
          .remote_node_id = CANARD_NODE_ID_UNSET,
          .transfer_id = (CanardTransferID)servo_transfer_id,
      };
      send(state, now_usec + MEGA, &transfer, serialized_size, &serialized[0], now_usec);
    }
  }

  // Disarm automatically if the arming subject has not been updated in a while.
  if (state->servo.arming.armed && ((now_usec - state->servo.arming.last_update_at) >
                                    (uint64_t)(reg_udral_service_actuator_common___0_1_CONTROL_TIMEOUT * MEGA))) {
    state->servo.arming.armed = false;
    puts("Disarmed by timeout ");
  }
}

/// This is needed only for constructing uavcan_node_port_List_0_1.
static void fillSubscriptions(const struct CanardTreeNode *const tree,  // NOLINT(misc-no-recursion)
                              uavcan_node_port_SubjectIDList_0_1 *const obj) {
  if (NULL != tree) {
    fillSubscriptions(tree->lr[0], obj);
    const struct CanardRxSubscription *crs = (const struct CanardRxSubscription *)tree;
    assert(crs->port_id <= CANARD_SUBJECT_ID_MAX);
    assert(obj->sparse_list.count < uavcan_node_port_SubjectIDList_0_1_sparse_list_ARRAY_CAPACITY_);
    obj->sparse_list.elements[obj->sparse_list.count++].value = crs->port_id;
    fillSubscriptions(tree->lr[1], obj);
  }
}

/// This is needed only for constructing uavcan_node_port_List_0_1.
static void fillServers(const struct CanardTreeNode *const tree,  // NOLINT(misc-no-recursion)
                        uavcan_node_port_ServiceIDList_0_1 *const obj) {
  if (NULL != tree) {
    fillServers(tree->lr[0], obj);
    const struct CanardRxSubscription *crs = (const struct CanardRxSubscription *)tree;
    assert(crs->port_id <= CANARD_SERVICE_ID_MAX);
    (void)nunavutSetBit(&obj->mask_bitpacked_[0], sizeof(obj->mask_bitpacked_), crs->port_id, true);
    fillServers(tree->lr[1], obj);
  }
}

/// Invoked every 10 seconds.
static void handle01HzLoop(struct UdralServoInternalState *const state, const CanardMicrosecond now_usec) {
  // Publish the recommended (not required) port introspection message. No point publishing it if we're anonymous.
  // The message is a bit heavy on the stack (about 2 KiB) but this is not a problem for a modern MCU.
  if (state->canard.node_id <= CANARD_NODE_ID_MAX) {
    uavcan_node_port_List_0_1 m = {0};
    uavcan_node_port_List_0_1_initialize_(&m);
    uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&m.publishers);
    uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&m.subscribers);

    // Indicate which subjects we publish to. Don't forget to keep this updated if you add new publications!
    {
      size_t *const cnt = &m.publishers.sparse_list.count;
      m.publishers.sparse_list.elements[(*cnt)++].value = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_;
      m.publishers.sparse_list.elements[(*cnt)++].value = uavcan_node_port_List_0_1_FIXED_PORT_ID_;
      if (state->port_id.pub.servo_feedback <= CANARD_SUBJECT_ID_MAX) {
        m.publishers.sparse_list.elements[(*cnt)++].value = state->port_id.pub.servo_feedback;
      }
      if (state->port_id.pub.servo_status <= CANARD_SUBJECT_ID_MAX) {
        m.publishers.sparse_list.elements[(*cnt)++].value = state->port_id.pub.servo_status;
      }
      if (state->port_id.pub.servo_power <= CANARD_SUBJECT_ID_MAX) {
        m.publishers.sparse_list.elements[(*cnt)++].value = state->port_id.pub.servo_power;
      }
      if (state->port_id.pub.servo_dynamics <= CANARD_SUBJECT_ID_MAX) {
        m.publishers.sparse_list.elements[(*cnt)++].value = state->port_id.pub.servo_dynamics;
      }
    }

    // Indicate which servers and subscribers we implement.
    // We could construct the list manually but it's easier and more robust to just query libcanard for that.
    fillSubscriptions(state->canard.rx_subscriptions[CanardTransferKindMessage], &m.subscribers);
    fillServers(state->canard.rx_subscriptions[CanardTransferKindRequest], &m.servers);
    fillServers(state->canard.rx_subscriptions[CanardTransferKindResponse], &m.clients);  // For regularity.

    // Serialize and publish the message. Use a smaller buffer if you know that message is always small.
    uint8_t serialized[uavcan_node_port_List_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
    size_t serialized_size = uavcan_node_port_List_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
    if (uavcan_node_port_List_0_1_serialize_(&m, &serialized[0], &serialized_size) >= 0) {
      const CanardTransferMetadata transfer = {
          .priority = CanardPriorityOptional,  // Mind the priority.
          .transfer_kind = CanardTransferKindMessage,
          .port_id = uavcan_node_port_List_0_1_FIXED_PORT_ID_,
          .remote_node_id = CANARD_NODE_ID_UNSET,
          .transfer_id = (CanardTransferID)(state->next_transfer_id.uavcan_node_port_list++),
      };
      send(state, now_usec + MEGA, &transfer, serialized_size, &serialized[0], now_usec);
    }
  }
}

/// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl
static void processMessageServoSetpoint(struct UdralServoInternalState *const state,
                                        const reg_udral_physics_dynamics_rotation_Planar_0_1 *const msg) {
  // Serial.println("processMessageServoSetpoint");
  state->servo.target_position = msg->kinematics.angular_position.radian;
  state->servo.target_velocity = msg->kinematics.angular_velocity.radian_per_second;
  state->servo.target_acceleration = msg->kinematics.angular_acceleration.radian_per_second_per_second;
  state->servo.target_force = msg->_torque.newton_meter;
}

/// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/common/Readiness.0.1.dsdl
static void processMessageServiceReadiness(struct UdralServoInternalState *const state,
                                           const reg_udral_service_common_Readiness_0_1 *const msg,
                                           const CanardMicrosecond monotonic_time) {
  // Serial.println("processMessageServiceReadiness");
  state->servo.arming.armed = msg->value >= reg_udral_service_common_Readiness_0_1_ENGAGED;
  state->servo.arming.last_update_at = monotonic_time;
}

static void processMessagePlugAndPlayNodeIDAllocation(struct UdralServoInternalState *const state,
                                                      const uavcan_pnp_NodeIDAllocationData_1_0 *const msg) {
  uint8_t uid[uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_] = {0};
  getUniqueID(uid);
  if ((msg->allocated_node_id.elements[0].value <= CANARD_NODE_ID_MAX) && (msg->allocated_node_id.count > 0) &&
      (memcmp(uid, (uint8_t *)&msg->unique_id_hash, 6) == 0)) {
    Serial.print("Got processMessagePlugAndPlayNodeIDAllocation response ");
    Serial.println(msg->allocated_node_id.elements[0].value);
    // printf("Got PnP node-ID allocation: %d\n", msg->allocated_node_id.elements[0].value);
    state->canard.node_id = (CanardNodeID)msg->allocated_node_id.elements[0].value;
    // Store the value into the non-volatile storage.
    uavcan_register_Value_1_0 reg = {0};
    uavcan_register_Value_1_0_select_natural16_(&reg);
    reg.natural16.value.elements[0] = msg->allocated_node_id.elements[0].value;
    reg.natural16.value.count = 1;
    registerWrite("uavcan.node.id", &reg);
    // We no longer need the subscriber, drop it to free up the resources (both memory and CPU time).
    (void)canardRxUnsubscribe(&state->canard, CanardTransferKindMessage,
                              uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_);
  } else {
    Serial.println("Ignoring processMessagePlugAndPlayNodeIDAllocation response");
  }
  // Otherwise, ignore it: either it is a request from another node or it is a response to another node.
}

static uavcan_node_ExecuteCommand_Response_1_1 processRequestExecuteCommand(
    const uavcan_node_ExecuteCommand_Request_1_1 *req) {
  uavcan_node_ExecuteCommand_Response_1_1 resp = {0};
  switch (req->command) {
    case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_BEGIN_SOFTWARE_UPDATE: {
      Serial.println("uavcan_node_ExecuteCommand_Request_1_1_COMMAND_BEGIN_SOFTWARE_UPDATE");
      char file_name[uavcan_node_ExecuteCommand_Request_1_1_parameter_ARRAY_CAPACITY_ + 1] = {0};
      memcpy(file_name, req->parameter.elements, req->parameter.count);
      file_name[req->parameter.count] = '\0';
      // TODO: invoke the bootloader with the specified file name. See https://github.com/Zubax/kocherga/
      // printf("Firmware update request; filename: '%s' \n", &file_name[0]);
      resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_BAD_STATE;  // This is a stub.
      break;
    }
    case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_FACTORY_RESET: {
      Serial.println("uavcan_node_ExecuteCommand_Request_1_1_COMMAND_FACTORY_RESET");
      registerDoFactoryReset();
      resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS;
      break;
    }
    case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_RESTART: {
      Serial.println("uavcan_node_ExecuteCommand_Request_1_1_COMMAND_RESTART");
      g_restart_required = true;
      resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS;
      break;
    }
    case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_STORE_PERSISTENT_STATES: {
      Serial.println("uavcan_node_ExecuteCommand_Request_1_1_COMMAND_STORE_PERSISTENT_STATES");
      // If your registers are not automatically synchronized with the non-volatile storage, use this command
      // to commit them to the storage explicitly. Otherwise, it is safe to remove it.
      // In this demo, the registers are stored in files, so there is nothing to do.
      resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS;
      break;
    }
      // You can add vendor-specific commands here as well.
    default: {
      Serial.println("uavcan_node_ExecuteCommand_Response_1_1_STATUS_BAD_COMMAND");
      resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_BAD_COMMAND;
      break;
    }
  }
  return resp;
}

/// Performance notice: the register storage may be slow to access depending on its implementation (e.g., if it is
/// backed by an uncached filesystem). If your register storage implementation is slow, this may disrupt real-time
/// activities of the device. To avoid this, you can employ either measure:
///
/// - Load registers to memory at startup, synchronize with the storage at reboot/power-down.
///   To implement fast register access you can use https://github.com/pavel-kirienko/cavl.
///   See also uavcan.node.ExecuteCommand.COMMAND_STORE_PERSISTENT_STATES.
///
/// - If an RTOS is used (not a baremetal system), you can run a separate Cyphal processing task for
///   soft-real-time blocking operations (this approach is used in PX4).
///
/// - Document an operational limitation that the register interface should not be accessed while ENGAGED (armed).
///   Cyphal networks usually have no service traffic while the vehicle is operational.
///
static uavcan_register_Access_Response_1_0 processRequestRegisterAccess(const uavcan_register_Access_Request_1_0 *req) {
  char name[uavcan_register_Name_1_0_name_ARRAY_CAPACITY_ + 1] = {0};
  assert(req->name.name.count < sizeof(name));
  memcpy(&name[0], req->name.name.elements, req->name.name.count);
  name[req->name.name.count] = '\0';

  uavcan_register_Access_Response_1_0 resp = {0};
  // Serial.println("processRequestRegisterAccess");
  // Serial.print("name: ");
  // Serial.println((char *)&req->name.name);

  // If we're asked to write a new value, do it now:
  if (!uavcan_register_Value_1_0_is_empty_(&req->value)) {
    uavcan_register_Value_1_0_select_empty_(&resp.value);
    registerRead(&name[0], &resp.value);
    // If such register exists and it can be assigned from the request value:
    if (!uavcan_register_Value_1_0_is_empty_(&resp.value) && registerAssign(&resp.value, &req->value)) {
      registerWrite(&name[0], &resp.value);
    }
  }

  // Regardless of whether we've just wrote a value or not, we need to read the current one and return it.
  // The client will determine if the write was successful or not by comparing the request value with response.
  uavcan_register_Value_1_0_select_empty_(&resp.value);
  registerRead(&name[0], &resp.value);

  // Currently, all registers we implement are mutable and persistent. This is an acceptable simplification,
  // but more advanced implementations will need to differentiate between them to support advanced features like
  // exposing internal states via registers, perfcounters, etc.
  resp._mutable = true;
  resp.persistent = true;

  // Our node does not synchronize its time with the network so we can't populate the timestamp.
  resp.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN;

  return resp;
}

/// Constructs a response to uavcan.node.GetInfo which contains the basic information about this node.
static uavcan_node_GetInfo_Response_1_0 processRequestNodeGetInfo() {
  Serial.println("processRequestNodeGetInfo");
  uavcan_node_GetInfo_Response_1_0 resp = {0};
  resp.protocol_version.major = CANARD_CYPHAL_SPECIFICATION_VERSION_MAJOR;
  resp.protocol_version.minor = CANARD_CYPHAL_SPECIFICATION_VERSION_MINOR;

  // The hardware version is not populated in this demo because it runs on no specific hardware.
  // An embedded node like a servo would usually determine the version by querying the hardware.

  resp.software_version.major = VERSION_MAJOR;
  resp.software_version.minor = VERSION_MINOR;
  resp.software_vcs_revision_id = VCS_REVISION_ID;

  getUniqueID(resp.unique_id);

  // The node name is the name of the product like a reversed Internet domain name (or like a Java package).
  resp.name.count = strlen(NODE_NAME);
  memcpy(&resp.name.elements, NODE_NAME, resp.name.count);

  // The software image CRC and the Certificate of Authenticity are optional so not populated in this demo.
  return resp;
}

static void processReceivedTransfer(struct UdralServoInternalState *const state,
                                    const struct CanardRxTransfer *const transfer, const CanardMicrosecond now_usec) {
  if (transfer->metadata.transfer_kind == CanardTransferKindMessage) {
    size_t size = transfer->payload.size;
    if (transfer->metadata.port_id == state->port_id.sub.servo_setpoint) {
      reg_udral_physics_dynamics_rotation_Planar_0_1 msg = {0};
      if (reg_udral_physics_dynamics_rotation_Planar_0_1_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >=
          0) {
        processMessageServoSetpoint(state, &msg);
      }
    } else if (transfer->metadata.port_id == state->port_id.sub.servo_readiness) {
      reg_udral_service_common_Readiness_0_1 msg = {0};
      if (reg_udral_service_common_Readiness_0_1_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >= 0) {
        processMessageServiceReadiness(state, &msg, transfer->timestamp_usec);
      }
    } else if (transfer->metadata.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) {
      uavcan_pnp_NodeIDAllocationData_1_0 msg = {0};
      if (uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >= 0) {
        processMessagePlugAndPlayNodeIDAllocation(state, &msg);
      }
    } else {
      assert(false);  // Seems like we have set up a port subscription without a handler -- bad implementation.
    }
  } else if (transfer->metadata.transfer_kind == CanardTransferKindRequest) {
    if (transfer->metadata.port_id == uavcan_node_GetInfo_1_0_FIXED_PORT_ID_) {
      // The request object is empty so we don't bother deserializing it. Just send the response.
      const uavcan_node_GetInfo_Response_1_0 resp = processRequestNodeGetInfo();
      uint8_t serialized[uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
      size_t serialized_size = sizeof(serialized);
      const int8_t res = uavcan_node_GetInfo_Response_1_0_serialize_(&resp, &serialized[0], &serialized_size);
      if (res >= 0) {
        sendResponse(state, transfer, serialized_size, &serialized[0], now_usec);
      } else {
        assert(false);
      }
    } else if (transfer->metadata.port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) {
      uavcan_register_Access_Request_1_0 req = {0};
      size_t size = transfer->payload.size;
      if (uavcan_register_Access_Request_1_0_deserialize_(&req, (uint8_t *)transfer->payload.data, &size) >= 0) {
        const uavcan_register_Access_Response_1_0 resp = processRequestRegisterAccess(&req);
        uint8_t serialized[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
        size_t serialized_size = sizeof(serialized);
        if (uavcan_register_Access_Response_1_0_serialize_(&resp, &serialized[0], &serialized_size) >= 0) {
          sendResponse(state, transfer, serialized_size, &serialized[0], now_usec);
        }
      }
    } else if (transfer->metadata.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
      uavcan_register_List_Request_1_0 req = {0};
      size_t size = transfer->payload.size;

      Serial.println("uavcan_register_List_1_0_FIXED_PORT_ID_");
      if (uavcan_register_List_Request_1_0_deserialize_(&req, (uint8_t *)transfer->payload.data, &size) >= 0) {
        const uavcan_register_List_Response_1_0 resp = {.name = registerGetNameByIndex(req.index)};
        uint8_t serialized[uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
        size_t serialized_size = sizeof(serialized);
        if (uavcan_register_List_Response_1_0_serialize_(&resp, &serialized[0], &serialized_size) >= 0) {
          sendResponse(state, transfer, serialized_size, &serialized[0], now_usec);
        }
      }
      Serial.println("uavcan_register_List_1_0_FIXED_PORT_ID_ send");
    } else if (transfer->metadata.port_id == uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_) {
      uavcan_node_ExecuteCommand_Request_1_1 req = {0};
      size_t size = transfer->payload.size;
      Serial.println("uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_");
      if (uavcan_node_ExecuteCommand_Request_1_1_deserialize_(&req, (uint8_t *)transfer->payload.data, &size) >= 0) {
        const uavcan_node_ExecuteCommand_Response_1_1 resp = processRequestExecuteCommand(&req);
        uint8_t serialized[uavcan_node_ExecuteCommand_Response_1_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
        size_t serialized_size = sizeof(serialized);
        if (uavcan_node_ExecuteCommand_Response_1_1_serialize_(&resp, &serialized[0], &serialized_size) >= 0) {
          sendResponse(state, transfer, serialized_size, &serialized[0], now_usec);
        }
      }
    } else {
      assert(false);  // Seems like we have set up a port subscription without a handler -- bad implementation.
    }
  } else {
    assert(false);  // Bad implementation -- check your subscriptions.
  }
}

static void *canardAllocate(void *const user_reference, const size_t amount) {
  O1HeapInstance *const heap = ((struct UdralServoInternalState *)user_reference)->heap;
  assert(o1heapDoInvariantsHold(heap));
  return o1heapAllocate(heap, amount);
}

static void canardDeallocate(void *const user_reference, const size_t amount, void *const pointer) {
  (void)amount;
  O1HeapInstance *const heap = ((struct UdralServoInternalState *)user_reference)->heap;
  o1heapFree(heap, pointer);
}

int udral_loop(state_sync_f servo_state_sync_f) {
  srand(micros());
  struct UdralServoInternalState state{
      .user_state_sync_f = servo_state_sync_f,
  };
  // A simple application like a servo node typically does not require more than 20 KiB of heap and 4 KiB of stack.
  // For the background and related theory refer to the following resources:
  // - https://github.com/OpenCyphal/libcanard/blob/master/README.md
  // - https://github.com/pavel-kirienko/o1heap/blob/master/README.md
  // - https://forum.opencyphal.org/t/uavcanv1-libcanard-nunavut-templates-memory-usage-concerns/1118/4
  _Alignas(O1HEAP_ALIGNMENT) static uint8_t heap_arena[1024 * 20] = {0};
  state.heap = o1heapInit(heap_arena, sizeof(heap_arena), NULL, NULL);
  assert(NULL != state.heap);
  struct CanardMemoryResource canard_memory = {
      .user_reference = &state,
      .deallocate = canardDeallocate,
      .allocate = canardAllocate,
  };

  // The libcanard instance requires the allocator for managing protocol states.
  state.canard = canardInit(canard_memory);
  state.canard.user_reference = &state;  // Make the state reachable from the canard instance.

  // Restore the node-ID from the corresponding standard register. Default to anonymous.
  uavcan_register_Value_1_0 val = {0};
  uavcan_register_Value_1_0_select_natural16_(&val);
  val.natural16.value.count = 1;
  val.natural16.value.elements[0] = UINT16_MAX;  // This means undefined (anonymous), per Specification/libcanard.
  registerRead("uavcan.node.id", &val);  // The names of the standard registers are regulated by the Specification.
  assert(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == 1));
  state.canard.node_id = (val.natural16.value.elements[0] > CANARD_NODE_ID_MAX)
                             ? CANARD_NODE_ID_UNSET
                             : (CanardNodeID)val.natural16.value.elements[0];

  // The description register is optional but recommended because it helps constructing/maintaining large networks.
  // It simply keeps a human-readable description of the node that should be empty by default.
  uavcan_register_Value_1_0_select_string_(&val);
  val._string.value.count = 0;
  registerRead("uavcan.node.description", &val);  // We don't need the value, we just need to ensure it exists.

  // The UDRAL cookie is used to mark nodes that are auto-configured by a specific auto-configuration authority.
  // We don't use this value, it is managed by remote nodes; our only responsibility is to persist it across reboots.
  // This register is entirely optional though; if not provided, the node will have to be configured manually.
  uavcan_register_Value_1_0_select_string_(&val);
  val._string.value.count = 0;  // The value should be empty by default, meaning that the node is not configured.
  registerRead("udral.pnp.cookie", &val);

  // Announce which UDRAL network services we support by populating appropriate registers. They are supposed to be
  // immutable (read-only), but in this simplified demo we don't support that, so we make them mutable (do fix this).
  uavcan_register_Value_1_0_select_string_(&val);
  strcpy((char *)val._string.value.elements, "servo");  // The prefix in port names like "servo.feedback", etc.
  val._string.value.count = strlen((const char *)val._string.value.elements);
  registerWrite("reg.udral.service.actuator.servo", &val);

  // PID
  state.user_state_sync_f(&state.servo, hz);

  uavcan_register_Value_1_0_select_real32_(&val);
  val.real32 = state.servo.pid_position;
  registerRead("reg.pid_position", &val);
  if (!uavcan_register_Value_1_0_is_real32_(&val) || val.real32.value.count < state.servo.pid_position.value.count) {
    for (int i = 0; i < uavcan_register_Value_1_0_is_real32_(&val) ? val.real32.value.count : 0; i += 1) {
      state.servo.pid_position.value.elements[i] = val.real32.value.elements[i];
    }
    uavcan_register_Value_1_0_select_real32_(&val);
    val.real32 = state.servo.pid_position;
    registerWrite("reg.pid_position", &val);
  }
  state.servo.pid_position = val.real32;

  uavcan_register_Value_1_0_select_real32_(&val);
  val.real32 = state.servo.pid_velocity;
  registerRead("reg.pid_velocity", &val);
  if (!uavcan_register_Value_1_0_is_real32_(&val) || val.real32.value.count < state.servo.pid_velocity.value.count) {
    for (int i = 0; i < uavcan_register_Value_1_0_is_real32_(&val) ? val.real32.value.count : 0; i += 1) {
      state.servo.pid_velocity.value.elements[i] = val.real32.value.elements[i];
    }
    uavcan_register_Value_1_0_select_real32_(&val);
    val.real32 = state.servo.pid_velocity;
    registerWrite("reg.pid_velocity", &val);
  }
  state.servo.pid_velocity = val.real32;

  // Configure the transport by reading the appropriate standard registers.
  uavcan_register_Value_1_0_select_natural16_(&val);
  val.natural16.value.count = 1;
  val.natural16.value.elements[0] = CANARD_MTU_CAN_CLASSIC;
  registerRead("uavcan.can.mtu", &val);
  assert(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == 1));
  // We also need the bitrate configuration register. In this demo we can't really use it but an embedded application
  // shall define "uavcan.can.bitrate" of type natural32[2]; the second value is zero/ignored if CAN FD not supported.
  const int sock[CAN_REDUNDANCY_FACTOR] = {//    socketcanOpen("vcan0", val.natural16.value.elements[0])  //
                                           esp32twaicanOpen(CAN_TX, CAN_RX, NULL)};

  for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) {
    state.canard_tx_queues[ifidx] = canardTxInit(CAN_TX_QUEUE_CAPACITY, val.natural16.value.elements[0], canard_memory);
  }

  // Load the port-IDs from the registers. You can implement hot-reloading at runtime if desired. Specification here:
  // https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl
  // https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/README.md
  // As follows from the Specification, the register group name prefix can be arbitrary; here we just use "servo".
  // Publications:
  state.port_id.pub.servo_feedback =  // High-rate status information: all good or not, engaged or sleeping.
      getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.feedback",
                   reg_udral_service_actuator_common_Feedback_0_1_FULL_NAME_AND_VERSION_);
  state.port_id.pub.servo_status =  // A low-rate high-level status overview: temperatures, fault flags, errors.
      getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.status",
                   reg_udral_service_actuator_common_Status_0_1_FULL_NAME_AND_VERSION_);
  state.port_id.pub.servo_power =  // Electric power input measurements (voltage and current).
      getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.power",
                   reg_udral_physics_electricity_PowerTs_0_1_FULL_NAME_AND_VERSION_);
  state.port_id.pub.servo_dynamics =  // Position/speed/acceleration/force feedback.
      getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.dynamics",
                   reg_udral_physics_dynamics_rotation_PlanarTs_0_1_FULL_NAME_AND_VERSION_);
  // Subscriptions:
  state.port_id.sub.servo_setpoint =  // This message actually commands the servo setpoint with the motion profile.
      getSubjectID(SUBJECT_ROLE_SUBSCRIBER, "servo.setpoint",
                   reg_udral_physics_dynamics_rotation_Planar_0_1_FULL_NAME_AND_VERSION_);
  state.port_id.sub.servo_readiness =  // Arming subject: whether to act upon the setpoint or to stay idle.
      getSubjectID(SUBJECT_ROLE_SUBSCRIBER, "servo.readiness",
                   reg_udral_service_common_Readiness_0_1_FULL_NAME_AND_VERSION_);

  // Set up subject subscriptions and RPC-service servers.
  // Message subscriptions:
  static const CanardMicrosecond servo_transfer_id_timeout = 100 * KILO;
  if (state.canard.node_id > CANARD_NODE_ID_MAX) {
    static struct CanardRxSubscription rx;
    const int8_t res =  //
        canardRxSubscribe(&state.canard, CanardTransferKindMessage, uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_,
                          uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
                          &rx);
    if (res < 0) {
      return -res;
    }
  }
  if (state.port_id.sub.servo_setpoint <= CANARD_SUBJECT_ID_MAX)  // Do not subscribe if not configured.
  {
    static struct CanardRxSubscription rx;
    const int8_t res =  //
        canardRxSubscribe(&state.canard, CanardTransferKindMessage, state.port_id.sub.servo_setpoint,
                          reg_udral_physics_dynamics_rotation_Planar_0_1_EXTENT_BYTES_, servo_transfer_id_timeout, &rx);
    if (res < 0) {
      return -res;
    }
  }
  if (state.port_id.sub.servo_readiness <= CANARD_SUBJECT_ID_MAX)  // Do not subscribe if not configured.
  {
    static struct CanardRxSubscription rx;
    const int8_t res =  //
        canardRxSubscribe(&state.canard, CanardTransferKindMessage, state.port_id.sub.servo_readiness,
                          reg_udral_service_common_Readiness_0_1_EXTENT_BYTES_, servo_transfer_id_timeout, &rx);
    if (res < 0) {
      return -res;
    }
  }
  // Service servers:
  {
    static struct CanardRxSubscription rx;
    const int8_t res =  //
        canardRxSubscribe(&state.canard, CanardTransferKindRequest, uavcan_node_GetInfo_1_0_FIXED_PORT_ID_,
                          uavcan_node_GetInfo_Request_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx);
    if (res < 0) {
      return -res;
    }
  }
  {
    static struct CanardRxSubscription rx;
    const int8_t res =  //
        canardRxSubscribe(&state.canard, CanardTransferKindRequest, uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_,
                          uavcan_node_ExecuteCommand_Request_1_1_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
                          &rx);
    if (res < 0) {
      return -res;
    }
  }
  {
    static struct CanardRxSubscription rx;
    const int8_t res =  //
        canardRxSubscribe(&state.canard, CanardTransferKindRequest, uavcan_register_Access_1_0_FIXED_PORT_ID_,
                          uavcan_register_Access_Request_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
                          &rx);
    if (res < 0) {
      return -res;
    }
  }
  {
    static struct CanardRxSubscription rx;
    const int8_t res =  //
        canardRxSubscribe(&state.canard, CanardTransferKindRequest, uavcan_register_List_1_0_FIXED_PORT_ID_,
                          uavcan_register_List_Request_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &rx);
    if (res < 0) {
      return -res;
    }
  }

  // Now the node is initialized and we're ready to roll.
  state.started_at = getMonotonicMicroseconds();
  const CanardMicrosecond fast_loop_period = MEGA / hz;
  CanardMicrosecond next_fast_iter_at = state.started_at + fast_loop_period;
  CanardMicrosecond next_1_hz_iter_at = state.started_at + MEGA;
  CanardMicrosecond next_01_hz_iter_at = state.started_at + MEGA * 10;
  do {
    vTaskDelay(1);
    // Run a trivial scheduler polling the loops that run the business logic.
    CanardMicrosecond now_usec = getMonotonicMicroseconds();
    if (now_usec >= next_fast_iter_at) {
      state.user_state_sync_f(&state.servo, hz);
      next_fast_iter_at += fast_loop_period;
      handleFastLoop(&state, now_usec);
    }
    if (now_usec >= next_1_hz_iter_at) {
      next_1_hz_iter_at += MEGA;
      handle1HzLoop(&state, now_usec);
    }
    if (now_usec >= next_01_hz_iter_at) {
      next_01_hz_iter_at += MEGA * 10;
      handle01HzLoop(&state, now_usec);
    }

    // Manage CAN RX/TX per redundant interface.
    for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) {
      // Transmit pending frames from the prioritized TX queues managed by libcanard.
      struct CanardTxQueue *const que = &state.canard_tx_queues[ifidx];
      struct CanardTxQueueItem *tqi = canardTxPeek(que);  // Find the highest-priority frame.
      while (tqi != NULL) {
        // Attempt transmission only if the frame is not yet timed out while waiting in the TX queue.
        // Otherwise just drop it and move on to the next one.
        if ((tqi->tx_deadline_usec == 0) || (tqi->tx_deadline_usec > now_usec)) {
          const struct CanardFrame canard_frame = {
              .extended_can_id = tqi->frame.extended_can_id,
              .payload = {.size = tqi->frame.payload.size, .data = tqi->frame.payload.data}};
          // const int16_t result = socketcanPush(sock[ifidx], &canard_frame, 0);  // Non-blocking write attempt.
          const int16_t result = esp32twaicanPush(
              &canard_frame,
              0);  // Non-blocking write attempt.
                   // digitalWrite(38, 0); /*enable*/delay(1000); digitalWrite(38, 1); /*disable*/delay(1000);

          // const int16_t result = 0;
          if (result == 0) {
            break;  // The queue is full, we will try again on the next iteration.
          }
          if (result < 0) {
            return -result;  // SocketCAN interface failure (link down?)
          }
        }

        struct CanardTxQueueItem *const mut_tqi = canardTxPop(que, tqi);
        canardTxFree(que, &state.canard, mut_tqi);

        tqi = canardTxPeek(que);
      }

      // Process received frames by feeding them from SocketCAN to libcanard.
      // The order in which we handle the redundant interfaces doesn't matter -- libcanard can accept incoming
      // frames from any of the redundant interface in an arbitrary order.
      // The internal state machine will sort them out and remove duplicates automatically.
      struct CanardFrame frame = {0};
      uint8_t buf[CANARD_MTU_CAN_CLASSIC] = {0};
      const int16_t recieve_result = esp32twaicanPop(&frame, NULL, sizeof(buf), buf, 0, NULL);

      if (recieve_result == 0)  // The read operation has timed out with no frames, nothing to do here.
      {
        break;
      }
      if (recieve_result < 0)  // The read operation has failed. This is not a normal condition.
      {
        return -recieve_result;
      }
      // The SocketCAN adapter uses the wall clock for timestamping, but we need monotonic.
      // Wall clock can only be used for time synchronization.
      const CanardMicrosecond timestamp_usec = getMonotonicMicroseconds();
      struct CanardRxTransfer transfer;
      memset(&transfer, 0, sizeof transfer);
      const int8_t canard_result = canardRxAccept(&state.canard, timestamp_usec, &frame, ifidx, &transfer, NULL);
      if (canard_result > 0) {
        processReceivedTransfer(&state, &transfer, now_usec);
        state.canard.memory.deallocate(state.canard.memory.user_reference, transfer.payload.allocated_size,
                                       transfer.payload.data);
      } else if ((canard_result == 0) || (canard_result == -CANARD_ERROR_OUT_OF_MEMORY)) {
        (void)0;  // The frame did not complete a transfer so there is nothing to do.
                  // OOM should never occur if the heap is sized correctly. You can track OOM errors via heap API.
      } else {
        assert(false);  // No other error can possibly occur at runtime.
      }
    }
  } while (!g_restart_required);
  return 0;
}