text:
// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE autodiff_unittest

#include <boost/test/unit_test.hpp>

#include <iostream>
#include <sstream>

#include <popart/bwdgraphinfo.hpp>
#include <popart/graph.hpp>
#include <popart/ir.hpp>
#include <popart/op.hpp>
#include <popart/op/call.hpp>
#include <popart/op/greater.hpp>
#include <popart/op/if.hpp>
#include <popart/op/loop.hpp>
#include <popart/op/sum.hpp>
#include <popart/sgd.hpp>
#include <popart/tensornames.hpp>
#include <popart/util.hpp>

#define private public
#include <popart/transforms/autodiff.hpp>
#undef private

#include <testutil/irquery/irquery.hpp>

using namespace popart;
using namespace popart::irquery;

// In tests below we use TestOps to build up a popart::Ir object. We apply
// the autodiff transform on this Ir and check that the resulting Ir is
// what we expect.

namespace {

// Forward declarations.
class TestOp;
class SimpleTestOp;
class SimpleTestGradOp;
class AdvTest1Op;
class AdvTest1GradOp;
class AdvTest2Op;
class AdvTest2GradOp;

/**
 * Base class for TestOps in this file.
 **/
class TestOp : public Op {
public:
  TestOp(const OperatorIdentifier &opid, const Op::Settings &settings)
      : Op(opid, settings) {}

  virtual float getSubgraphValue() const override {
    return getLowSubgraphValue();
  }
};

/**
 * An op with the following behaviour:
 *  - an input #0: (details unimportant)
 *  - an output #0: (same tensor info as input)
 *  - calls to getGradOps() return {SimpleTestGradOp}
 *  - otherwise default popart::Op behaviour
 **/
class SimpleTestOp : public TestOp {
public:
  SimpleTestOp(const Op::Settings &settings)
      : TestOp(OperatorIdentifier("TestOps", "SimpleTestOp", 1), settings) {}

  void setup() override { outInfo(0) = inInfo(0); }

  std::unique_ptr<Op> clone() const override {
    return std::make_unique<SimpleTestOp>(*this);
  }

  std::vector<std::unique_ptr<Op>> getGradOps() override {
    std::vector<std::unique_ptr<Op>> grads;
    grads.emplace_back(std::make_unique<SimpleTestGradOp>(*this));
    return grads;
  }
};

/**
 * An op with the following behaviour:
 *  - an input #0: the gradient of a SimpleTestOp's #0 output
 *  - an input #1: the #0 input of a SimpleTestOp
 *  - an output #0: the gradient of a SimpleTestOp's #0 input=
 *  - otherwise default popart::Op behaviour
 **/
class SimpleTestGradOp : public TestOp {
public:
  SimpleTestGradOp(const SimpleTestOp &op)
      : TestOp(OperatorIdentifier("TestOps", "SimpleTestGradOp", 1),
               op.Op::getSettings()) {}

  void setup() override { outInfo(0) = inInfo(0); }

  std::unique_ptr<Op> clone() const override {
    return std::make_unique<SimpleTestGradOp>(*this);
  }

  const std::vector<GradInOutMapper> &gradInputInfo() const override {
    static const std::vector<GradInOutMapper> inInfo = {
        {0, 0, GradOpInType::GradOut}, {1, 0, GradOpInType::In}};
    return inInfo;
  }

  const std::map<int, int> &gradOutToNonGradIn() const override {
    static const std::map<int, int> outInfo = {{0, 0}};
    return outInfo;
  }
};

/**
 * An op with the following behaviour:
 *  - an input #0: (details unimportant)
 *  - an input #1: (details unimportant)
 *  - an output #0: (same tensor info as input #0)
 *  - calls to getGradOps() return {AdvTest1GradOp}
 *  - otherwise default popart::Op behaviour
 **/
class AdvTest1Op : public TestOp {
public:
  AdvTest1Op(const Op::Settings &settings)
      : TestOp(OperatorIdentifier("TestOps", "AdvTest1Op", 1), settings) {}

  void setup() override { outInfo(0) = inInfo(0); }

  std::unique_ptr<Op> clone() const override {
    return std::make_unique<AdvTest1Op>(*this);
  }

  std::vector<std::unique_ptr<Op>> getGradOps() override {
    std::vector<std::unique_ptr<Op>> grads;
    grads.emplace_back(std::make_unique<AdvTest1GradOp>(*this));
    return grads;
  }
};

/**
 * An op with the following behaviour:
 *  - an input #0: the #0 input of a AdvTest1Op
 *  - an input #1: the gradient of a AdvTest1Op's #0 output
 *  - an output #0: the gradient of a AdvTest1Op's #0 input
 *  - no output for AdvTest1Op's #1 input.
 *  - otherwise default popart::Op behaviour
 **/
class AdvTest1GradOp : public TestOp {
public:
  AdvTest1GradOp(const AdvTest1Op &op)
      : TestOp(OperatorIdentifier("TestOps", "AdvTest1GradOp", 1),
               op.Op::getSettings()) {}

  void setup() override { outInfo(0) = inInfo(0); }

  std::unique_ptr<Op> clone() const override {
    return std::make_unique<AdvTest1GradOp>(*this);
  }

  const std::vector<GradInOutMapper> &gradInputInfo() const override {
    static const std::vector<GradInOutMapper> inInfo = {
        {0, 0, GradOpInType::In}, {1, 0, GradOpInType::GradOut}};
    return inInfo;
  }

  const std::map<int, int> &gradOutToNonGradIn() const override {
    static const std::map<int, int> outInfo = {{0, 0}};
    return outInfo;
  }
};

/**
 * An op with the following behaviour:
 *  - an input #0: (details unimportant)
 *  - an output #0: (same tensor info as input #0)
 *  - calls to getGradOps() return {AdvTest2GradOp}
 *  - otherwise default popart::Op behaviour
 **/
class AdvTest2Op : public TestOp {
public:
  AdvTest2Op(const Op::Settings &settings)
      : TestOp(OperatorIdentifier("TestOps", "AdvTest2Op", 1), settings) {}

  void setup() override { outInfo(0) = inInfo(0); }

  std::unique_ptr<Op> clone() const override {
    return std::make_unique<AdvTest2Op>(*this);
  }

  std::vector<std::unique_ptr<Op>> getGradOps() override {
    std::vector<std::unique_ptr<Op>> grads;
    grads.emplace_back(std::make_unique<AdvTest2GradOp>(*this));
    return grads;
  }
};

/**
 * An op with the following behaviour:
 *  - an input #0: the #0 input of a AdvTest2Op
 *  - an input #1: the gradient of a AdvTest2Op's #0 output
 *  - an input #2: the #0 output of a AdvTest2Op
 *  - an output #0: the gradient of a AdvTest2Op's #0 input
 *  - otherwise default popart::Op behaviour
 **/
class AdvTest2GradOp : public TestOp {
public:
  AdvTest2GradOp(const AdvTest2Op &op)
      : TestOp(OperatorIdentifier("TestOps", "AdvTest2GradOp", 1),
               op.Op::getSettings()) {}

  void setup() override { outInfo(0) = inInfo(0); }

  std::unique_ptr<Op> clone() const override {
    return std::make_unique<AdvTest2GradOp>(*this);
  }

  const std::vector<GradInOutMapper> &gradInputInfo() const override {
    static const std::vector<GradInOutMapper> inInfo = {
        {0, 0, GradOpInType::In},
        {1, 0, GradOpInType::GradOut},
        {2, 0, GradOpInType::Out}};
    return inInfo;
  }

  const std::map<int, int> &gradOutToNonGradIn() const override {
    static const std::map<int, int> outInfo = {{0, 0}};
    return outInfo;
  }
};

/**
 * Helper function to print the IR.
 */
void printIr(Ir &ir) {
  std::stringstream ss;
  ir.append(ss);
  std::cout << ss.str() << std::endl;
}

/**
 * Receive a mapping from indices to expected connections and output a list
 * of expected connections.
 **/
ExpectedConnections
sortExpectedConnections(const std::map<int, ExpectedConnection> &map) {

  // Check we got exactly the indices, 0, 1, 2, 3 ... map.size()-1;
  std::vector<int> expectedIndices;
  for (int i = 0; i < map.size(); ++i) {
    expectedIndices.push_back(i);
  }

  std::stringstream ss;
  ss << "Expected " << map << " to have indices " << expectedIndices;

  ExpectedConnections result;
  std::set<int> seenIndices;
  result.resize(map.size());
  for (const auto &kv : map) {
    // Check each index is in the expected range.
    BOOST_REQUIRE_MESSAGE(kv.first >= 0, ss.str());
    BOOST_REQUIRE_MESSAGE(kv.first < map.size(), ss.str());
    // Add to the result.
    result.at(kv.first) = kv.second;
    // Add to set so we can check we have each index.
    seenIndices.insert(kv.first);
  }

  // Unless we've accounted for each index this won't be true.
  BOOST_REQUIRE_MESSAGE(seenIndices.size() == map.size(), ss.str());

  return result;
}

/**
 * Helper function that gets the unique index of an input.
 */
InIndex getUniqInIndex(Op *op, const TensorId &id) {
  const auto &indices = op->input->indices(op->getGraph().getTensors().get(id));
  BOOST_REQUIRE(indices.size() == 1);
  return indices.at(0);
};

/**
 * Add the following to an IR:
 *
 * .--[main graph]-----.
 * |                   |
 * |    ["t0"]         |
 * |      |            |
 * |   #0 v            |
 * | SimpleTestOp      |
 * |   #0 |            |
 * |      v            |
 * |    ["t1"]         |
 * |                   |
 * '-------------------'
 **/
void addTestIr1(Ir &ir) {

  auto &mainGraph       = ir.getMainGraph();
  Op::Settings settings = Op::Settings{mainGraph, "SimpleTestOp"};

  // Add "t0" to the main graph.
  TensorInfo t0Info{DataType::INT32, {}};
  int32_t t0Data[] = {5};
  mainGraph.getTensors().addVarInit("t0", t0Info, static_cast<void *>(&t0Data));

  // Add SimpleTestOp. Connect "t0" to SimpleTestOp's input, create the "t1"
  // output and call setup().
  auto simpleTestOp = mainGraph.createConnectedOp<SimpleTestOp>(
      {{0, "t0"}}, {{0, "t1"}}, settings);
}

/**
 * Add the following to an IR:
 *
 * .--[main graph]----------.
 * |                        |
 * |  ["main_in"]           |
 * |      |                 |
 * |   #0 v                 |
 * |  CallOp<A>             |
 * |   #0 |                 |
 * |      v                 |
 * |  ["main_out"]          |
 * '------------------------'
 *
 *    [A/a_in]
 *      |
 * .----|-[subgraph "A"]----.
 * | #0 v                   |
 * | SimpleTestOp           |
 * | #0 |                   |
 * |    |                   |
 * |  [A/a_tmp]             |
 * |    |                   |
 * | #0 v                   |
 * | SimpleTestOp           |
 * | #0 |                   |
 * |    |                   |
 * |    |                   |
 * '----|-------------------'
 *      v
 *    [A/a_out]
 **/
void addTestIr2(Ir &ir) {
  // Tensor info for tensors in the IR.
  TensorInfo tInfo{DataType::INT32, {}};
  int32_t tData[] = {5};

  // Create the subgraph.
  auto &subgraphA = ir.createGraph(GraphId("A"));

  // Create subgraph A.
  auto a_in  = addScope(subgraphA, "a_in");
  auto a_tmp = addScope(subgraphA, "a_tmp");
  auto a_out = addScope(subgraphA, "a_out");
  subgraphA.addInput(a_in, tInfo);

  // Add SimpleTestOp0.
  Op::Settings settingsSubgraphA = Op::Settings{subgraphA, "SimpleTestOp"};
  auto simpleTestOp0             = subgraphA.createConnectedOp<SimpleTestOp>(
      {{0, a_in}}, {{0, a_tmp}}, settingsSubgraphA);

  // Add SimpleTestOp1.
  auto simpleTestOp1 = subgraphA.createConnectedOp<SimpleTestOp>(
      {{0, a_tmp}}, {{0, a_out}}, settingsSubgraphA);

  // Mark "a_out" as a graph output.
  subgraphA.markAsOutput(a_out);
  // Create the subgraph.
  auto &mainGraph = ir.getMainGraph();

  // Create main graph.
  mainGraph.getTensors().addVarInit(
      "main_in", tInfo, static_cast<void *>(&tData));

  auto callOp = mainGraph.createConnectedOp<CallOp>(
      {{0, "main_in"}},
      {{0, "main_out"}},
      Onnx::AiGraphcore::OpSet1::Call,
      std::ref(subgraphA),
      Op::Settings{mainGraph, "", mainGraph.getScope()});
}

/**
 * Add the following to an IR:
 *
 *     [A/a_in0] [A/a_in1]
 *        |        |
 * .-["A"]|--------|---------.
 * |      |        |         |
 * |      | .------'         |
 * |      | |                |
 * |      | |                |
 * |      | |                |
 * |   #0 v v #1             |
 * |   AdvTest1Op            |
 * |   #0 |                  |
 * |      |                  |
 * '------|- ----------------'
 *        |
 *        v
 *     [A/a_out0]
 **/
void addTestIr3(Ir &ir) {
  // Tensor info for tensors in the IR.
  TensorInfo tInfo{DataType::INT32, {}};
  int32_t tData[] = {5};

  // Create the subgraph.
  auto &mainGraph = ir.getMainGraph();
  auto &subgraphA = ir.createGraph(GraphId("A"));

  // Create subgraph A.
  auto a_in0  = addScope(subgraphA, "a_in0");
  auto a_in1  = addScope(subgraphA, "a_in1");
  auto a_out0 = addScope(subgraphA, "a_out0");
  subgraphA.addInput(a_in0, tInfo);
  subgraphA.addInput(a_in1, tInfo);

  // Add AdvTest1Op.
  Op::Settings settingsSubgraphA = Op::Settings{subgraphA, "AdvTest1Op"};
  auto advTest1Op0               = subgraphA.createConnectedOp<AdvTest1Op>(
      {{0, a_in0}, {1, a_in1}}, {{0, a_out0}}, settingsSubgraphA);

  // Mark "a_out" as a graph output.
  subgraphA.markAsOutput(a_out0);
}

/**
 * Add the following to an IR:
 *
 * .--[main graph]----------.
 * |                        |
 * |  ["main_in"]           |
 * |      |                 |
 * |   #0 v                 |
 * |  CallOp<A>             |
 * |   #0 |                 |
 * |      v                 |
 * |  ["main_out"]          |
 * '------------------------'
 *
 *    [A/in]
 *      |
 * .----|-[subgraph "A"]----.
 * | #0 v                   |
 * | AdvTest2Op             |
 * | #0 |                   |
 * |    |                   |
 * |  [A/tmp]               |
 * |    |                   |
 * | #0 v                   |
 * | AdvTest2Op             |
 * | #0 |                   |
 * |    |                   |
 * |    |                   |
 * '----|-------------------'
 *      v
 *    [A/out]
 **/
void addTestIr4(Ir &ir) {
  // Tensor info for tensors in the IR.
  TensorInfo tInfo{DataType::INT32, {}};
  int32_t tData[] = {5};

  // Create the subgraph.
  auto &subgraphA = ir.createGraph(GraphId("A"));

  // Create subgraph A.
  auto in  = addScope(subgraphA, "in");
  auto tmp = addScope(subgraphA, "tmp");
  auto out = addScope(subgraphA, "out");
  subgraphA.addInput(in, tInfo);

  // Add SimpleTestOp0.
  Op::Settings settingsSubgraphA = Op::Settings{subgraphA, "AdvTest2Op"};
  auto simpleTestOp0             = subgraphA.createConnectedOp<AdvTest2Op>(
      {{0, in}}, {{0, tmp}}, settingsSubgraphA);

  // Add SimpleTestOp1.
  auto simpleTestOp1 = subgraphA.createConnectedOp<AdvTest2Op>(
      {{0, tmp}}, {{0, out}}, settingsSubgraphA);

  // Mark "a_out" as a graph output.
  subgraphA.markAsOutput(out);
  // Create the subgraph.
  auto &mainGraph = ir.getMainGraph();

  // Create main graph.
  mainGraph.getTensors().addVarInit(
      "main_in", tInfo, static_cast<void *>(&tData));

  auto callOp = mainGraph.createConnectedOp<CallOp>(
      {{0, "main_in"}},
      {{0, "main_out"}},
      Onnx::AiGraphcore::OpSet1::Call,
      std::ref(subgraphA),
      Op::Settings{mainGraph, "", mainGraph.getScope()});
}

/**
 * Add the following to an IR:
 *
 * .--[main graph]----------.
 * |                        |
 * |   8       ["main_in"]  |
 * |   |           |        |
 * |   |    .------|        |
 * |   |    |      |        |
 * |#1 v #0 v      |        |
 * |  Greater      |        |
 * |     |         |        |
 * |  #0 v      #1 v        |
 * |  IfOp<Then, Else>      |
 * |   #0 |                 |
 * |      v                 |
 * |  ["main_out"]          |
 * '------------------------'
 *
 *    [Then/in]                   [Else/in]
 *      |                            |
 * .----|-["Then"]----------.   .----|-["Else"]----------.
 * | #0 v                   |   | #0 v                   |
 * | AdvTest2Op             |   | AdvTest2Op             |
 * | #0 |                   |   | #0 |                   |
 * |    |                   |   |    |                   |
 * |  [Then/tmp]            |   |  [Else/tmp]            |
 * |    |                   |   |    |                   |
 * | #0 v                   |   | #0 v                   |
 * | AdvTest2Op             |   | AdvTest2Op             |
 * | #0 |                   |   | #0 |                   |
 * |    |                   |   |    |                   |
 * |    |                   |   |    |                   |
 * '----|-------------------'   '----|-------------------'
 *      v                            v
 *    [Then/out]                   [Else/out]
 **/
void addTestIr5(Ir &ir) {
  // Tensor info for tensors in the IR.
  TensorInfo tInfo{DataType::INT32, {}};
  int32_t tDataZero[]  = {0};
  int32_t tDataEight[] = {8};

  auto addBranch = [&](GraphId branchName) {
    // Create the subgraph.
    auto &branch = ir.createGraph(branchName);

    // Create subgraph.
    auto in  = addScope(branch, "in");
    auto tmp = addScope(branch, "tmp");
    auto out = addScope(branch, "out");
    branch.addInput(in, tInfo);

    // Add SimpleTestOp0.
    Op::Settings branchSettings = Op::Settings{branch, ""};
    auto simpleTestOp0          = branch.createConnectedOp<AdvTest2Op>(
        {{0, in}}, {{0, tmp}}, branchSettings);

    // Add SimpleTestOp1.
    auto simpleTestOp1 = branch.createConnectedOp<AdvTest2Op>(
        {{0, tmp}}, {{0, out}}, branchSettings);

    // Mark "a_out" as a graph output.
    branch.markAsOutput(out);
  };

  addBranch(GraphId("Then"));
  addBranch(GraphId("Else"));

  // Create the subgraph.
  auto &mainGraph        = ir.getMainGraph();
  auto mainGraphSettings = Op::Settings{mainGraph, "", mainGraph.getScope()};

  // Create some tensors.
  mainGraph.getTensors().addVarInit(
      "main_in", tInfo, static_cast<void *>(&tDataZero));
  mainGraph.getTensors().addConstInit(
      "main_const", tInfo, static_cast<void *>(&tDataEight));

  auto condOp = mainGraph.createConnectedOp<GreaterOp>(
      {{0, "main_in"}, {1, "main_const"}},
      {{0, "main_cond"}},
      Onnx::Operators::Greater_1,
      mainGraphSettings);

  auto ifOp = mainGraph.createConnectedOp<IfOp>(
      {{0, "main_cond"}, {1, "main_in"}},
      {{0, "main_out"}},
      Onnx::Operators::If_1,
      BranchInfo{GraphId("Then"), {{1, 0}}, {{0, 0}}},
      BranchInfo{GraphId("Else"), {{1, 0}}, {{0, 0}}},
      mainGraphSettings);
}

} // namespace

BOOST_AUTO_TEST_CASE(autodiff_0) {

  // Check that when given the left hand side of the IR below (from makeTestIr1)
  // autodiff's (`bool apply(Ir&)`) adds the parts of the IR on the right:
  //
  // This is a very basic IR that comprises of only a main graph with one simple
  // test op.
  //
  // .----------------[main graph]-------------------.
  // |                                               |
  // |                ....[added by autodiff]....... |
  // |                :                            : |
  // |    ["t0"]      :        [getGradId("t0")]   : |
  // |      |         :                  ^         : |
  // |      |         :                  |         : |
  // |      |         :                 Sum        : |
  // |      |         :                  ^         : |
  // |      |---------(-----.            |         : |
  // |      |         :     |           [_g0]      : |
  // |      |         :     |            |         : |
  // |   #0 v         :     |         #0 |         : |
  // | SimpleTestOp   :     | SimpleTestGradOp     : |
  // |   #0 |         :     | #1 ^    #0 ^         : |
  // |      |         :     |    |       |         : |
  // |      |         :     '----'       |         : |
  // |      v         :                  |         : |
  // |    ["t1"]      :      [getGradId("t1")]     : |
  // |                :                            : |
  // |                :                            : |
  // |                :............................: |
  // '-----------------------------------------------'
  //
  // Where t1 is marked as the loss and t0 is a variable tensor and
  //
  // - _g0    is an arbitrary edge gradient TensorId.

  Ir ir;

  // First we build up the Ir's left hand side as per the diagram above. Note
  // that we avoid using the builder to do this (which is used in most tests)
  // to try and minimise the amount of production code we are instantiating in
  // this test, making it as close to a unit test as possible.
  addTestIr1(ir);

  // Set "t1" as the loss.
  ir.setFinalLoss("t1");
  ir.getMainGraph().setLoss("t1");

  // NOTE: We need to do a few boilerplate things to apply autodiff. In future,
  // we'd like to refactor autodiff so this isn't the case. We also need an
  // optimizer for autodiff to work.
  ir.setOptimizer(ConstSGD(0.1));
  ir.updateVertices();

  // Now apply the transform.
  ir.applyTransform(Autodiff::id(), ir.getMainGraph());

  // Now check that autodiff added the right things.

  // Start by getting a test wrapper for the main graph.
  IrTestWrapper tw_ir{ir};
  auto tw_mainGraph = tw_ir.hasGraph(ir.getMainGraph().id, Require::MustBeTrue);

  // Now find the SimpleTestGradOp that consumed "t0" and
  // getGradId("t1") and produces _g0.
  auto tw_simpleTestGradOp = tw_mainGraph->ops().hasOp<SimpleTestGradOp>(
      [&](auto &tw_op) -> bool {
        return (tw_op.inputs().hasIdAtIndex(0, getGradId("t1")) &&
                (tw_op.inputs().hasIdAtIndex(1, "t0")) &&
                (tw_op.outputs().hasIndex(0)));
      },
      Require::MustBeTrue);

  // Now, get the value of _g0.
  auto _g0 =
      tw_simpleTestGradOp->outputs().hasIndex(0, Require::MustBeTrue)->id();

  // Find the gradsum op that consumes _g0 and produces getGradId("t0").
  auto tw_gradSumOp = tw_mainGraph->ops().hasOp<SumOp>(
      [&](auto &tw_op) -> bool {
        return (tw_op.inputs().hasExactIds({_g0})) &&
               (tw_op.outputs().hasIdAtIndex(0, getGradId("t0")));
      },
      Require::MustBeTrue);
}

BOOST_AUTO_TEST_CASE(autodiff_1) {

  // Check that autodiff (`bool apply(Ir&)`) applied to the IR generated by
  // `testIr2` adds parts of the IR marked on the right-hand side below:
  //
  // .-----------------------------[main graph]--------------------------------.
  // |                                                                         |
  // |                           ............[added by autodiff].............. |
  // |                           :                                           : |
  // |  ["main_in"]              :       [getGradId("main_in")]              : |
  // |      |                    :                    ^                      : |
  // |      |                    :                    |                      : |
  // |      |                    :                   Sum                     : |
  // |      |                    :                    ^                      : |
  // |      |--------------------(-------.            |                      : |
  // |      |                    :       |          [_g0]                    : |
  // |      |                    :       |            |                      : |
  // |   #0 v                    :       |         #0 |                      : |
  // |  CallOp<A>                :       |     CallOp<_k>                    : |
  // |   #0 |                    :       |  #_x ^ #_y ^                      : |
  // |      |                    :       |      |     |                      : |
  // |      |                    :       '------'     |                      : |
  // |      v                    :                    |                      : |
  // |  ["main_out"]             :      [getGradId("main_out")]              : |
  // |                           :                                           : |
  // |                           :                                           : |
  // |                           :...........................................: |
  // '-------------------------------------------------------------------------'
  //
  //                             ............[added by autodiff]..............
  //                             :                                           :
  //    [A/a_in]                 :    [_k/a_in]         [_k/getGradId(a_in)] :
  //      |                      :  #_x |                       #0 ^         :
  // .----|-[subgraph "A"]----.  : .----|--------[subgraph _k]-----|-------. :
  // |    |                   |  : |    |----------.              Sum      | :
  // |    |                   |  : |    |          |               ^       | :
  // |    |                   |  : |    |          |               |       | :
  // |    |                   |  : |    |          |             [_g2]     | :
  // | #0 v                   |  : | #0 v          |            #0 |       | :
  // | SimpleTestOp           |  : | SimpleTestOp  |    SimpleTestGradOp   | :
  // | #0 |                   |  : | #0 |          |    #1 ^    #0 ^       | :
  // |    |                   |  : |    |          |       |       |       | :
  // |    |                   |  : |    |          '-------'       |       | :
  // |  [A/a_tmp]             |  : | [_k/a_tmp]                    |       | :
  // |    |                   |  : |    |          [_k/getGradId(a_tmp)]   | :
  // |    |                   |  : |    |                          ^       | :
  // |    |                   |  : |    |                          |       | :
  // |    |                   |  : |    |                         Sum      | :
  // |    |                   |  : |    |                          ^       | :
  // |    |                   |  : |    |                          |       | :
  // |    |                   |  : |    |                        [_g1]     | :
  // | #0 v                   |  : |    |                       #0 |       | :
  // | SimpleTestOp           |  : |    |               SimpleTestGradOp   | :
  // | #0 |                   |  : |    |               #1 ^    #0 ^       | :
  // |    |                   |  : |    |                  |       |       | :
  // |    |                   |  : |    '------------------'       |       | :
  // '----|-------------------'  : '-------------------------------|-------' :
  //      v                      :                             #_y |         :
  //    [A/a_out]                :                     [_k/getGradId(a_out)] :
  //                             :...........................................:
  //
  // Where:
  // - _k              is an arbitrary GraphId.
  // - _x, _y          are arbitrary InIndices.
  // - _g0, _g1, _g2   are arbitrary edge gradient TensorIds.

  Ir ir;
  addTestIr2(ir);

  // Set "t1" as the loss.
  ir.setFinalLoss("main_out");
  ir.getMainGraph().setLoss("main_out");

  // NOTE: We need to do a few boilerplate things to apply autodiff. In future,
  // we'd like to refactor autodiff so this isn't the case. We also need an
  // optimizer for autodiff to work.
  ir.setOptimizer(ConstSGD(0.1));
  ir.updateVertices();

  // Now apply the transform.
  ir.applyTransform(Autodiff::id(), ir.getMainGraph());

  // First we check the main graph. We start by getting a test wrapper for the
  // main graph.
  IrTestWrapper tw_ir{ir};
  auto tw_mainGraph = tw_ir.hasGraph(ir.getMainGraph().id, Require::MustBeTrue);

  // Start by checking the main graph now has a CallOp with "main_in" and
  // getGradId("main_out") inputs. This is the backward call op.
  auto tw_bwdCallOp = tw_mainGraph->ops().hasOp<CallOp>(
      [&](auto &tw_op) -> bool {
        return (tw_op.inputs().containsIds(
                   {"main_in", getGradId("main_out")})) &&
               (tw_op.outputs().hasIndex(0));
      },
      Require::MustBeTrue);

  // We will the actual CallOp* to get the called graph ID.
  auto bwdCallOp = tw_bwdCallOp->unwrap();

  // Get the #0 output of bwdCallOp (_g0).
  auto _g0 = tw_bwdCallOp->outputs().hasIndex(0)->id();

  // Find the gradsum op op that takes _g0.
  auto gradSumOp0 = tw_mainGraph->ops().hasOp<SumOp>(
      [&](auto &tw_op) -> bool {
        return (tw_op.inputs().hasExactIds({_g0})) &&
               (tw_op.outputs().hasIdAtIndex(0, getGradId("main_in")));
      },
      Require::MustBeTrue);

  // Now it's time to find and check subgraph bwdGraph with id "_k".
  GraphId _k       = bwdCallOp->getCalledGraph().id;
  auto tw_bwdGraph = tw_ir.hasGraph(_k, Require::MustBeTrue);

  // OK, create expected tensor IDs.
  auto &bwdGraph     = tw_bwdGraph->unwrap().get();
  auto _k_a_in       = addScope(bwdGraph, "a_in");
  auto _k_a_tmp      = addScope(bwdGraph, "a_tmp");
  auto _k_a_in_grad  = addScope(bwdGraph, getGradId("a_in"));
  auto _k_a_tmp_grad = addScope(bwdGraph, getGradId("a_tmp"));
  auto _k_a_out_grad = addScope(bwdGraph, getGradId("a_out"));

  // We don't actually know the values of indices _x and _y, let's determine
  // them. We know CallOps always have contiguous indices from 0 and we know
  // we have two inputs, so it's just working out which is 0 and which is 1.

  // Get the input index of "main_in" in bwdCallOp.
  auto _x =
      tw_bwdCallOp->inputs().hasId("main_in", Require::MustBeTrue)->index();
  // Get the input index of getGradId("main_out").
  auto _y = tw_bwdCallOp->inputs()
                .hasId(getGradId("main_out"), Require::MustBeTrue)
                ->index();

  // Check the subgraphs inputs/outputs.
  tw_bwdGraph->inputs().hasExactIds({_k_a_in, _k_a_out_grad});
  tw_bwdGraph->inputs().hasIdAtIndex(_x, _k_a_in);
  tw_bwdGraph->inputs().hasIdAtIndex(_y, _k_a_out_grad);
  tw_bwdGraph->outputs().hasExactIds({_k_a_in_grad});
  tw_bwdGraph->outputs().hasIdAtIndex(0, _k_a_in_grad);

  // Find the forward SimpleTestOp clone in subgraph bwdGraph that consumes
  // _k/"a_in" and produces _k/"a_tmp".
  auto tw_simpleTestOpClone = tw_bwdGraph->ops().hasOp<SimpleTestOp>(
      [&](auto &tw_op) -> bool {
        return (tw_op.inputs().hasIdAtIndex(0, _k_a_in)) &&
               (tw_op.outputs().hasIdAtIndex(0, _k_a_tmp));
      },
      Require::MustBeTrue);

  // Find the SimpleTestGradOp in subgraph _k that consumes _k/"a_tmp" and
  // _k/getGradId("a_out") and produces _g1.
  auto tw_simpleTestGradOp1 = tw_bwdGraph->ops().hasOp<SimpleTestGradOp>(
      [&](auto &tw_op) -> bool {
        return (tw_op.inputs().hasIdAtIndex(0, _k_a_out_grad)) &&
               (tw_op.inputs().hasIdAtIndex(1, _k_a_tmp)) &&
               (tw_op.outputs().hasIndex(0));
      },
      Require::MustBeTrue);

  // Figure out what _g1 is.
  auto _g1 =
      tw_simpleTestGradOp1->outputs().hasIndex(0, Require::MustBeTrue)->id();

  // Find the gradsum op in subgraph _k that consumes _g1 and produces
  // _k/getGradId("a_tmp").
  auto tw_gradSumOp1 = tw_bwdGraph->ops().hasOp<SumOp>(
      [&](auto &tw_op) -> bool {
        return (tw_op.inputs().hasExactIds({_g1})) &&
               (tw_op.outputs().hasIdAtIndex(0, _k_a_tmp_grad));
      },
      Require::MustBeTrue);

  // Find the SimpleTestGradOp in subgraph _k that consumes
  // _k/"a_in" and _k/getGradId("a_tmp") and produces _g2.
  auto tw_simpleTestGradOp2 = tw_bwdGraph->ops().hasOp<SimpleTestGradOp>(
      [&](auto &tw_op) -> bool {
        return (tw_op.inputs().hasIdAtIndex(0, _k_a_tmp_grad)) &&
               (tw_op.inputs().hasIdAtIndex(1, _k_a_in)) &&
               (tw_op.outputs().hasIndex(0));
      },
      Require::MustBeTrue);

  // Figure out what _g2 is.
  auto _g2 =
      tw_simpleTestGradOp2->outputs().hasIndex(0, Require::MustBeTrue)->id();

  // Find the gradsum op that takes _g2 and produces _k/getGradId("a_in").
  auto tw_gradSumOp2 = tw_bwdGraph->ops().hasOp<SumOp>(
      [&](auto &tw_op) -> bool {
        return (tw_op.inputs().hasExactIds({_g2})) &&
               (tw_op.outputs().hasIdAtIndex(0, _k_a_in_grad));
      },
      Require::MustBeTrue);
}

BOOST_AUTO_TEST_CASE(autodiff_createBwdGraph_0) {

  // Check that the autodiff prototype that operates on subgraph "A" of testIr2
  // `gradsProvidedForTensors` as [A/a_out] returns a graph as per the
  // right-hand side below:
  //
  //                             .........[expected autodiff result]..........
  //                             : bwdGraphId: _k                            :
  //                             : expectedInputs (any order):               :
  //                             :    - _x: (A/a_in, Fwd)                    :
  //                             :    - _y: (A/a_tmp, Fwd)                   :
  //                             :    - _z: (A/a_out, FwdGrad)               :
  //                             : expectedOutputs (any order):              :
  //                             :    - _w: (A/a_in, FwdGrad)                :
  //                             :                                           :
  //    [A/a_in]                 :                      [_k/getGradId(a_in)] :
  //      |                      :                              #0 ^         :
  // .----|-[subgraph "A"]----.  : .-------------[subgraph _k]-----|-------. :
  // |    |                   |  : |                              Sum      | :
  // |    |                   |  : |                               ^       | :
  // |    |                   |  : |                               |       | :
  // |    |                   |  : |                             [_g1]     | :
  // | #0 v                   |  : |                            #0 |       | :
  // | SimpleTestOp           |  : |                    SimpleTestGradOp   | :
  // | #0 |                   |  : |                    #1 ^    #0 ^       | :
  // |    |                   |  : |                       |       |       | :
  // |    |                   |  : |     .-----------------'       |       | :
  // |  [A/a_tmp]             |  : |     |                         |       | :
  // |    |                   |  : |     |         [_k/getGradId(a_tmp)]   | :
  // |    |                   |  : |     |                         ^       | :
  // |    |                   |  : |     |                         |       | :
  // |    |                   |  : |     |                        Sum      | :
  // |    |                   |  : |     |                         ^       | :
  // |    |                   |  : |     |                         |       | :
  // |    |                   |  : |     |                       [_g0]     | :
  // | #0 v                   |  : |     |                      #0 |       | :
  // | SimpleTestOp           |  : |     |              SimpleTestGradOp   | :
  // | #0 |                   |  : |     |              #1 ^    #0 ^       | :
  // |    |                   |  : |     |                 |       |       | :
  // |    |                   |  : |     |          .------'       |       | :
  // '----|-------------------'  : '-----|----------|--------------|-------' :
  //      v                      :   #_x |      #_y |          #_z |         :
  //    [A/a_out]                :       |          |              |         :
  //                             : [_k/a_in] [_k/a_tmp] [_k/getGradId(a_out)]:
  //                             :...........................................:
  //
  // Where:
  // - _k              is an arbitrary GraphId.
  // - _w              is an arbitrary OutIndex.
  // - _x, _y, _z      are arbitrary InIndices.
  // - _g0, _g1        are arbitrary edge gradient TensorIds.

  Ir ir;
  addTestIr2(ir);

  // Get some tensor ids.
  auto &subgraphA = ir.getGraph(GraphId("A"));
  auto a_in       = addScope(subgraphA, "a_in");
  auto a_tmp      = addScope(subgraphA, "a_tmp");
  auto a_out      = addScope(subgraphA, "a_out");

  // Now apply the createBwdGraph function.
  Autodiff autodiff;
  auto result = autodiff.createBwdGraph(std::ref(ir),
                                        GraphId("A"),
                                        Autodiff::TensorIds({a_out}),
                                        Autodiff::TensorIds({}),
                                        FwdGraphToBwdGraphInfo());

  // Now it's time to find and check subgraph bwdGraph with id "_k".
  IrTestWrapper tw_ir{ir};
  GraphId _k       = result.bwdGraphId;
  auto tw_bwdGraph = tw_ir.hasGraph(_k, Require::MustBeTrue);
  auto &bwdGraph   = tw_bwdGraph->unwrap().get();

  // Create some tensors IDs we expect.
  auto _k_a_in       = addScope(bwdGraph, "a_in");
  auto _k_a_tmp      = addScope(bwdGraph, "a_tmp");
  auto _k_a_out      = addScope(bwdGraph, "a_out");
  auto _k_a_in_grad  = addScope(bwdGraph, getGradId("a_in"));
  auto _k_a_tmp_grad = addScope(bwdGraph, getGradId("a_tmp"));
  auto _k_a_out_grad = addScope(bwdGraph, getGradId("a_out"));

  // We don't actually know the values of indices _x, _y, _z. Let's check
  // the inputs are what we expect and determine their values.
  tw_bwdGraph->inputs().hasExactIds({_k_a_in, _k_a_tmp, _k_a_out_grad},
                                    Require::MustBeTrue);
  auto _x = tw_bwdGraph->inputs().hasId(_k_a_in, Require::MustBeTrue)->index();
  auto _y = tw_bwdGraph->inputs().hasId(_k_a_tmp, Require::MustBeTrue)->index();
  auto _z =
      tw_bwdGraph->inputs().hasId(_k_a_out_grad, Require::MustBeTrue)->index();

  // We do know there is only one output, and that output is at index 0.
  tw_bwdGraph->outputs().hasExactIds({_k_a_in_grad}, Require::MustBeTrue);

  // Check that the result.expectedInputs matches the indices we determined.
  BwdGraphInfo expectedBwdGraphInfo{
      _k,
      sortExpectedConnections({{_x, {a_in, ExpectedConnectionType::Fwd}},
                               {_y, {a_tmp, ExpectedConnectionType::Fwd}},
                               {_z, {a_out, ExpectedConnectionType::FwdGrad}}}),
      sortExpectedConnections({{0, {a_in, ExpectedConnectionType::FwdGrad}}})};

  BOOST_REQUIRE_MESSAGE(
      expectedBwdGraphInfo == result,
      logging::format("Expected {}, got {}", expectedBwdGraphInfo, result));

  // Now check the bwd graph is as expected.

  // Find the SimpleTestGradOp in subgraph _k that consumes _k/"a_tmp" and
  // _k/getGradId("a_out") and produces _g0.
  auto tw_simpleTestGradOp1 = tw_bwdGraph->ops().hasOp<SimpleTestGradOp>(
      [&](auto &tw_op) -> bool {
        return (tw_op.inputs().hasIdAtIndex(0, _k_a_out_grad)) &&
               (tw_op.inputs().hasIdAtIndex(1, _k_a_tmp)) &&
               (tw_op.outputs().hasIndex(0));
      },
      Require::MustBeTrue);

  // Figure out what _g0 is.
  auto _g0 =
      tw_simpleTestGradOp1->outputs().hasIndex(0, Require::MustBeTrue)->id();

  // Find the gradsum op in subgraph _k that consumes _g0 and produces
  // _k/getGradId("a_tmp").
  auto tw_gradSumOp1 = tw_bwdGraph->ops().hasOp<SumOp>(
      [&](auto &tw_op) -> bool {
        Op *op = tw_op.unwrap();
        return (tw_op.inputs().hasExactIds({_g0})) &&
               (tw_op.outputs().hasIdAtIndex(0, _k_a_tmp_grad));
      },
      Require::MustBeTrue);

  // Find the SimpleTestGradOp in subgraph _k that consumes
  // _k/"a_in" and _k/getGradId("a_tmp") and produces _g1.
  auto tw_simpleTestGradOp2 = tw_bwdGraph->ops().hasOp<SimpleTestGradOp>(
      [&](auto &tw_op) -> bool {
        return (tw_op.inputs().hasIdAtIndex(0, _k_a_tmp_grad)) &&
               (tw_op.inputs().hasIdAtIndex(1, _k_a_in)) &&
               (tw_op.outputs().hasIndex(0));
      },
      Require::MustBeTrue);

  // Figure out what _g1 is.
  auto _g1 =
      tw_simpleTestGradOp2->outputs().hasIndex(0, Require::MustBeTrue)->id();

  // Find the gradsum op that takes _g2 and produces _k/getGradId("a_in").
  auto tw_gradSumOp2 = tw_bwdGraph->ops().hasOp<SumOp>(
      [&](auto &tw_op) -> bool {
        return (tw_op.inputs().hasExactIds({_g1})) &&
               (tw_op.outputs().hasIdAtIndex(0, _k_a_in_grad));
      },
      Require::MustBeTrue);
}

BOOST_AUTO_TEST_CASE(autodiff_createBwdGraph_1) {

  // Test that, with subgraph A in the left-hand side of the IR below (from
  // addTestIr3):
  //
  // - If a gradient is provided for A/a_out0 and a gradient is required for
  //   for both A/a_in0 and A/a_in1 then an error is raised (because
  //   AdvTest1GradOp can't produce a gradient for that input).
  // - If no gradient is provided and a gradient is required for A/a_in0 then
  //   an error is raised (because AdvTest1GradOp needs the a_out0 gradient
  //   input to produce the gradient for a_in0).
  // - If a gradient is provided for A/a_out0 and a gradient is required for
  //   A/a_in0 then all is well and a gradient for A/a_in0 is available.
  //
  //                             ....[generated by autodiff on success]........
  //                             :                                            :
  //     [A/a_in0] [A/a_in1]     :  [_k/getGradId(a_in0)]                     :
  //        |        |           :        ^                                   :
  // .-["A"]|--------|---------. : .-[_k]-|---------------------------------. :
  // |      |        |         | : |      |                                 | :
  // |      | .------'         | : |      |       .. No output for a_in1's  | :
  // |      | |                | : |      |      :   gradient here, so if   | :
  // |      | |                | : |      |      :   the user requires this | :
  // |      | |                | : |      |      :   it's an error.         | :
  // |   #0 v v #1             | : |   #0 |      :                          | :
  // |   AdvTest1Op            | : |   AdvTest1GradOp                       | :
  // |   #0 |                  | : |   #0 ^   #1 ^                          | :
  // |      |                  | : |      |      |.. If this gradient is not| :
  // |      |                  | : |  .---'      |   provided then the grad | :
  // |      |                  | : |  |          |   for a_in0 can't be     | :
  // |      |                  | : |  |          |   created.               | :
  // '------|- ----------------' : '--|----------|--------------------------' :
  //        |                    :    |          |                            :
  //        v                    :    |          |                            :
  //     [A/a_out0]              :  [_k/a_in0] [_k/getGradId(a_out0)]         :
  //                             :                                            :
  //                             :............................................:

  Ir ir;
  addTestIr3(ir);

  // Get tensorIds.
  auto &subgraphA = ir.getGraph(GraphId("A"));
  auto a_in0      = addScope(subgraphA, "a_in0");
  auto a_in1      = addScope(subgraphA, "a_in1");
  auto a_out0     = addScope(subgraphA, "a_out0");

  auto test = [&](Autodiff::TensorIds provided, Autodiff::TensorIds required) {
    std::cout << "----------------------------------------" << std::endl;
    std::cout << "Testing provided=" << provided << " required=" << required
              << std::endl;

    // Now call `createBwdGraph`.
    Autodiff autodiff;
    auto result = autodiff.createBwdGraph(std::ref(ir),
                                          GraphId("A"),
                                          provided,
                                          required,
                                          FwdGraphToBwdGraphInfo());

    std::cout << "Got bwdGraphId=" << result.bwdGraphId << std::endl;

    ir.logIr();
  };

  // If a gradient is provided for A/a_out0 and a gradient is required for
  // for both A/a_in0 and A/a_in1 then an error is raised (because
  // AdvTest1GradOp can't produce a gradient for that input).
  BOOST_REQUIRE_THROW(test({a_out0}, {a_in0, a_in1}), std::runtime_error);

  // If no gradient is provided and a gradient is required for A/a_in0 then
  // an error is raised (because AdvTest1GradOp needs the a_out0 gradient
  // input to produce the gradient for a_in0).
  BOOST_REQUIRE_THROW(test({}, {a_in0}), std::runtime_error);

  // If a gradient is provided for A/a_out0 and a gradient is required for
  // A/a_in0 then all is well and a gradient for A/a_in0 is available.
  BOOST_REQUIRE_NO_THROW(test({a_out0}, {a_in0}));
}

BOOST_AUTO_TEST_CASE(autodiff_stitch_0) {

  // Test that, with the following IRs (obtained via addTestIr4 for
  // CallOpTestType and addTestIr5 for IfOpTestType and createBwdGraph) and
  // using specific stitch parameters, we get the expected outputs.
  //
  // .--[main graph]----------.
  // |                        |    .--- CallOp for CallOpTestType
  // |     ...                |    |    IfOp   for IfOpTestType
  // |      |                 |    |
  // |      v                 |    |
  // |  Op<_j, ...>  *--------(----'
  // |   #0 |                 |
  // |      v                 |
  // |  ["main_out"]          |
  // '------------------------'
  //
  //      .---------------------------- _j="A"    for CallOpTestType
  //      |                             _j="Then" for IfOpTestType
  //      |
  //      *
  //
  //    [_j/in]                                         [_k/getGradId(in)]
  //      |                                                     #0 ^
  // .----|-[subgraph _j]-----.    .-------------[subgraph _k]-----|-------.
  // |    |                   |    |                              Sum      |
  // |    |                   |    |                               ^       |
  // |    |                   |    |                               |       |
  // |    |                   |    |                             [_g1]     |
  // | #0 v                   |    |                            #0 |       |
  // | AdvTest2Op             |    |                      AdvTest2GradOp   |
  // | #0 |                   |    |                    #1 ^  #2 ^ ^ #0    |
  // |    |                   |    |                       |     | |       |
  // |    |                   |    |     .-----------------'     | |       |
  // |  [A/tmp]               |    |     |        .--------------' |       |
  // |    |                   |    |     |        |                |       |
  // |    |                   |    |     |        |  [_k/getGradId(a_tmp)] |
  // |    |                   |    |     |        |                ^       |
  // |    |                   |    |     |        |                |       |
  // |    |                   |    |     |        |               Sum      |
  // |    |                   |    |     |        |                ^       |
  // |    |                   |    |     |        |                |       |
  // |    |                   |    |     |        |              [_g0]     |
  // | #0 v                   |    |     |        |             #0 |       |
  // | AdvTest2Op             |    |     |        |       AdvTest2GradOp   |
  // | #0 |                   |    |     |        |    #1 ^  #2 ^  ^ #0    |
  // |    |                   |    |     |        |       |     |  |       |
  // |    |                   |    |     |        |-------'     |  '-----. |
  // |    |                   |    |     |        |             |        | |
  // '----|-------------------'    '-----|--------|-------------|--------|-'
  //      v                          #_w |    #_x |         #_y |    #_z |
  //    [_j/out]                         |        |             |        |
  //                               [_k/a_in]   [_k/tmp]      [_k/out]    |
  //                                                     [_k/getGradId(out)]
  //
  // Now, if stitch is called on _k we expect the following result IRs:
  //
  // .--[main graph]----------------.
  // |                              |  C1: Extra output at call site and extra
  // |      ...                     |      subgraph output. Only applies for
  // |       |                      |      CallOpTestType, stitch strategy
  // | ......(..................... |      AddFwdOutputs and when _x is in the
  // | :  #0 v                 C1 : |      stitch index list. Using defaults,
  // | : Op<_j, ...> -------.     : |      this stitch index should be included.
  // | :  #0 |              |     : |  C2: _k/tmp input replaced by
  // | :.....(.........     |     : |      recomputation. Only with recompute
  // |       |        :     |     : |      stitch strategies and when _x is in
  // |       v        :     v     : |      the stitch index list. Using
  // |   ["main_out"] :  ["tmp"]  : |      defaults, this index is included for
  // |                :...........: |      both recomputation strategies.
  // '------------------------------'  C3: _k/out input replaced by
  //                                       recomputation. Only with recompute
  //                                       stitch strategies and when _y is in
  //                                       the stitch index list. Using
  //                                       defaults, this index is included
  //                                       with RecomputeAllNonInputs but not
  //                                       with RecomputeMinimal, as it is
  //                                       an output of _j, so stitching is not
  //                                       required.
  //
  //                                       NOTE: SafeAddFwdOutputs is a hybrid
  //                                       of RecomputeMinmal and AddFwdOutputs.
  //
  //
  //
  //    [_j/in]                                           [_k/getGradId(in)]
  //      |                                                      #0 ^
  // .----|-[subgraph _j]------.    .-------------[subgraph _k]-----|-------.
  // |    |                    |    |                              Sum      |
  // |    |                    |    |                               ^       |
  // |    |                    |    |                               |       |
  // |    |                    |    |                             [_g1]     |
  // | #0 v                    |    |                            #0 |       |
  // | AdvTest2Op              |    |                      AdvTest2GradOp   |
  // | #0 |                    |    |                    #1 ^  #2 ^ ^ #0    |
  // |    |                    |    |                       |     | |       |
  // |  [_j/tmp]               |    |     .-----------------'     | |       |
  // |  ..(...............     |    |     |        .--------------' |       |
  // |  : |           C1 :     |    |     |        |                |       |
  // |  : |              :     |    |     |        |  [_k/getGradId(a_tmp)] |
  // |  : |------------. :     |    |     |        |                ^       |
  // |  : |            | :     |    |     |        |                |       |
  // |  :.(..........  | :     |    |     |        |               Sum      |
  // |    |          : | :     |    |     |        |                ^       |
  // | #0 v          : | :     |    |     |        |                |       |
  // | AdvTest2Op    : | :     |    |     |        |              [_g0]     |
  // | #0 |          : | :     |    |     |        |             #0 |       |
  // |    |      ....: | :.... |    |     |        |       AdvTest2GradOp   |
  // |    |      :     |     : |    |     |        |    #1 ^  #2 ^  ^ #0    |
  // |    |      :     |     : |    |     |        |       |     |  |       |
  // |    |      :     |     : |    |     |        |-------'     |  '-----. |
  // |    |      :     |     : |    |     |        |          [k_out]     | |
  // |    |      :     |     : |    |     |        |             |        | |
  // |    |      :     |     : |    |     |      ..(.............(...     | |
  // |    |      :     |     : |    |     |      : |          #0 |  :     | |
  // |    |      :     |     : |    |     |      : |     AdvTest2Op :     | |
  // |    |      :     |     : |    |     |      : |          #0 ^  :     | |
  // |    |      :     |     : |    |     |      : |-------------'  :     | |
  // |    |      :     |     : |    |     |      :.(........        :     | |
  // |    |      :     |     : |    |     |        |       :        :     | |
  // |    |      :     |     : |    |     |     [_k/tmp]   :        :     | |
  // |    |      :     |     : |    |     |        |       :        :     | |
  // |    |      :     |     : |    |   ..(........(.....  :        :     | |
  // |    |      :     |     : |    |   : | #0     |    :  :        :     | |
  // |    |      :     |     : |    |   : | AdvTest2Op  :  :        :     | |
  // |    |      :     |     : |    |   : |     #0 ^    :  :        :     | |
  // |    |      :     |     : |    |   : |--------'    :  :        :     | |
  // |    |      :     |     : |    |   :.)....         :  :        :     | |
  // '----|------:-----|-----(-'    '-----|---(---------(--(--------(-----|-'
  //      v      :     v     :        #_a |   : (#_b)   :  : (#_c)  : #_d |
  //   [_j/out]  : [_j/tmp]  :        [_k/in] :      C2 :  :     C3 :     |
  //             :...........:                :.........:  :........:     |
  //                                                      [_k/getGradId(out)

  enum class TestType { CallOpTestType = 0, IfOpTestType = 1 };

  enum class ExpectC1 { Yes = 0, No = 1 };

  enum class ExpectC2 { Yes = 0, No = 1 };

  enum class ExpectC3 { Yes = 0, No = 1 };

  struct IrInfo {
    // GraphId values.
    GraphId _j{""};
    GraphId _k{""};

    // Main graph tensor names.
    TensorId in;
    TensorId tmp;
    TensorId out;

    // Backward graph tensor names.
    TensorId _k_in;
    TensorId _k_tmp;
    TensorId _k_out;
    TensorId _k_in_grad;
    TensorId _k_tmp_grad;
    TensorId _k_out_grad;

    // Backward graph input indices (before stitching).
    InIndex _w;
    InIndex _x;
    InIndex _y;
    InIndex _z;

    // Backward graph input indices (after stitching, -1 if not available).
    InIndex _a;
    InIndex _b;
    InIndex _c;
    InIndex _d;
  };

  using OptStitchIndices = nonstd::optional<std::vector<InIndex>>;

  auto test = [&](TestType testType,
                  AutodiffStitchStrategy strategy,
                  // Generate stitchIndices.
                  std::function<OptStitchIndices(IrInfo)> stitchIndicesFun,
                  // Generate expected BwdGraphInfo object.
                  std::function<BwdGraphInfo(IrInfo)> expectedBwdGraphInfoFun,
                  ExpectC1 c1,
                  ExpectC2 c2,
                  ExpectC3 c3) {
    Ir ir;
    IrInfo irInfo;
    if (testType == TestType::CallOpTestType) {
      addTestIr4(ir);
      irInfo._j = GraphId("A");
    } else if (testType == TestType::IfOpTestType) {
      addTestIr5(ir);
      irInfo._j = GraphId("Then");
    }

    // Get some info about IR.
    auto &fwdGraph = ir.getGraph(irInfo._j);
    irInfo.in      = addScope(fwdGraph, "in");
    irInfo.tmp     = addScope(fwdGraph, "tmp");
    irInfo.out     = addScope(fwdGraph, "out");

    // Now apply the createBwdGraph function on _j to generate _k.
    Autodiff autodiff;
    auto createBwdGraphResult =
        autodiff.createBwdGraph(std::ref(ir),
                                irInfo._j,
                                Autodiff::TensorIds({irInfo.out}),
                                Autodiff::TensorIds({}),
                                FwdGraphToBwdGraphInfo());

    // Do some introspection to get _y.
    IrTestWrapper tw_ir{ir};
    irInfo._k = createBwdGraphResult.bwdGraphId;
    auto tw_mainGraph =
        tw_ir.hasGraph(ir.getMainGraph().id, Require::MustBeTrue);
    auto tw_fwdGraph = tw_ir.hasGraph(irInfo._j, Require::MustBeTrue);
    auto tw_bwdGraph = tw_ir.hasGraph(irInfo._k, Require::MustBeTrue);
    auto &bwdGraph   = tw_bwdGraph->unwrap().get();

    // Create some tensors IDs we need.
    irInfo._k_in       = addScope(bwdGraph, "in");
    irInfo._k_tmp      = addScope(bwdGraph, "tmp");
    irInfo._k_out      = addScope(bwdGraph, "out");
    irInfo._k_in_grad  = addScope(bwdGraph, getGradId("in"));
    irInfo._k_tmp_grad = addScope(bwdGraph, getGradId("tmp"));
    irInfo._k_out_grad = addScope(bwdGraph, getGradId("out"));

    // Get _w, _x_, _y and _z.
    irInfo._w =
        tw_bwdGraph->inputs().hasId(irInfo._k_in, Require::MustBeTrue)->index();
    irInfo._x = tw_bwdGraph->inputs()
                    .hasId(irInfo._k_tmp, Require::MustBeTrue)
                    ->index();
    irInfo._y = tw_bwdGraph->inputs()
                    .hasId(irInfo._k_out, Require::MustBeTrue)
                    ->index();
    irInfo._z = tw_bwdGraph->inputs()
                    .hasId(irInfo._k_out_grad, Require::MustBeTrue)
                    ->index();

    // Now stitch it (the bit we're really testing).
    auto stitchResult = autodiff.stitch(ir,
                                        irInfo._j,
                                        createBwdGraphResult,
                                        strategy,
                                        stitchIndicesFun(irInfo));

    // Get _a, _b, _c and _d if available.
    auto getOptInIndex = [&](auto opt) -> InIndex {
      if (opt) {
        return opt->index();
      } else {
        return -1;
      }
    };

    irInfo._a = getOptInIndex(tw_bwdGraph->inputs().hasId(irInfo._k_in));
    irInfo._b = getOptInIndex(tw_bwdGraph->inputs().hasId(irInfo._k_tmp));
    irInfo._c = getOptInIndex(tw_bwdGraph->inputs().hasId(irInfo._k_out));
    irInfo._d = getOptInIndex(tw_bwdGraph->inputs().hasId(irInfo._k_out_grad));

    // Now we do some selective checks to test that the changes are what we
    // anticipate. For the sake of brevity we only for the presence or
    // absence of C1, C2 and C3 and the returned BwdGraphInfo.

    auto getRequire = [](bool require) {
      if (require) {
        // Failure unless it's true.
        return Require::MustBeTrue;
      } else {
        // Failure if it's true.
        return Require::MustBeFalse;
      }
    };

    // C1: Test for additional output in fwdGraph iff we expect C1.
    tw_fwdGraph->outputs().hasId(irInfo.tmp, getRequire(c1 == ExpectC1::Yes));
    // C1: Test the callop in the main graph has an additional output iff we
    // expect C1.
    tw_mainGraph->ops().hasOp<CallOp>(
        [&](auto &op) -> bool {
          return op.outputs().hasIndex(1).operator bool();
        },
        getRequire(c1 == ExpectC1::Yes));

    // C2: Test the bwd graph no longer has the input iff we expect C2.
    tw_bwdGraph->inputs().hasId(irInfo._k_tmp, getRequire(c2 == ExpectC2::No));
    // C2: Test that there is a `AdvTest2Op` in the bwdGraph now that
    // reproduces tmp from in iff we expect C2.
    tw_bwdGraph->ops().hasOp<AdvTest2Op>(
        [&](auto &op) -> bool {
          return (op.inputs().hasId(irInfo._k_in)) &&
                 (op.outputs().hasId(irInfo._k_tmp));
        },
        getRequire(c2 == ExpectC2::Yes));

    // C3: Test the bwd graph no longer has the input iff we expect C3.
    tw_bwdGraph->inputs().hasId(irInfo._k_out, getRequire(c3 == ExpectC3::No));
    // C3: Test that there is a `AdvTest2Op` in the bwdGraph now that
    // reproduces out from tmp iff we expect C3.
    tw_bwdGraph->ops().hasOp<AdvTest2Op>(
        [&](auto &op) -> bool {
          return (op.inputs().hasId(irInfo._k_tmp)) &&
                 (op.outputs().hasId(irInfo._k_out));
        },
        getRequire(c3 == ExpectC3::Yes));

    // Check stitchResult is the expected output.
    auto expectedBwdGraphInfo = expectedBwdGraphInfoFun(irInfo);
    BOOST_REQUIRE_MESSAGE(expectedBwdGraphInfo == stitchResult,
                          logging::format("Expected {}, got {}",
                                          expectedBwdGraphInfo,
                                          stitchResult));
  };

  for (auto testType : {TestType::CallOpTestType, TestType::IfOpTestType}) {

    // Test RecomputeMinimal + default stitch indices. Expect C2 only.
    test(
        testType,
        AutodiffStitchStrategy::RecomputeMinimal,
        [](IrInfo) { return nonstd::nullopt; },
        [](IrInfo irInfo) {
          return BwdGraphInfo{
              irInfo._k,
              sortExpectedConnections(
                  {{irInfo._a, {irInfo.in, ExpectedConnectionType::Fwd}},
                   {irInfo._c, {irInfo.out, ExpectedConnectionType::Fwd}},
                   {irInfo._d, {irInfo.out, ExpectedConnectionType::FwdGrad}}}),
              sortExpectedConnections(
                  {{0, {irInfo.in, ExpectedConnectionType::FwdGrad}}})};
        },
        ExpectC1::No,
        ExpectC2::Yes,
        ExpectC3::No);

    // Test RecomputeMinimal + no stitch indices. Expect nothing.
    test(
        testType,
        AutodiffStitchStrategy::RecomputeMinimal,
        [](IrInfo) { return OptStitchIndices{{}}; },
        [](IrInfo irInfo) {
          return BwdGraphInfo{
              irInfo._k,
              sortExpectedConnections(
                  {{irInfo._a, {irInfo.in, ExpectedConnectionType::Fwd}},
                   {irInfo._b, {irInfo.tmp, ExpectedConnectionType::Fwd}},
                   {irInfo._c, {irInfo.out, ExpectedConnectionType::Fwd}},
                   {irInfo._d, {irInfo.out, ExpectedConnectionType::FwdGrad}}}),
              sortExpectedConnections(
                  {{0, {irInfo.in, ExpectedConnectionType::FwdGrad}}})};
        },
        ExpectC1::No,
        ExpectC2::No,
        ExpectC3::No);

    // Test RecomputeMinimal + unstichable index. Expect exception.
    BOOST_REQUIRE_THROW(
        test(
            testType,
            AutodiffStitchStrategy::RecomputeMinimal,
            [](IrInfo irInfo) { return OptStitchIndices{{irInfo._d}}; },
            [](IrInfo irInfo) {
              return BwdGraphInfo{irInfo._k, {}, {}};
            },
            ExpectC1::No,
            ExpectC2::No,
            ExpectC3::No),
        std::runtime_error);

    // Test RecomputeAllNonInputs + default stitch indices. Expect C2 & C3.
    test(
        testType,
        AutodiffStitchStrategy::RecomputeAllNonInputs,
        [](IrInfo) { return nonstd::nullopt; },
        [](IrInfo irInfo) {
          return BwdGraphInfo{
              irInfo._k,
              sortExpectedConnections(
                  {{irInfo._a, {irInfo.in, ExpectedConnectionType::Fwd}},
                   {irInfo._d, {irInfo.out, ExpectedConnectionType::FwdGrad}}}),
              sortExpectedConnections(
                  {{0, {irInfo.in, ExpectedConnectionType::FwdGrad}}})};
        },
        ExpectC1::No,
        ExpectC2::Yes,
        ExpectC3::Yes);

    // Test RecomputeAllNonInputs + _b. Expect C2 only.
    test(
        testType,
        AutodiffStitchStrategy::RecomputeAllNonInputs,
        [](IrInfo irInfo) { return OptStitchIndices{{irInfo._x}}; },
        [](IrInfo irInfo) {
          return BwdGraphInfo{
              irInfo._k,
              sortExpectedConnections(
                  {{irInfo._a, {irInfo.in, ExpectedConnectionType::Fwd}},
                   {irInfo._c, {irInfo.out, ExpectedConnectionType::Fwd}},
                   {irInfo._d, {irInfo.out, ExpectedConnectionType::FwdGrad}}}),
              sortExpectedConnections(
                  {{0, {irInfo.in, ExpectedConnectionType::FwdGrad}}})};
        },
        ExpectC1::No,
        ExpectC2::Yes,
        ExpectC3::No);

    // Test RecomputeAllNonInputs + unstichable index. Expect exception.
    BOOST_REQUIRE_THROW(
        test(
            testType,
            AutodiffStitchStrategy::RecomputeAllNonInputs,
            [](IrInfo irInfo) { return OptStitchIndices{{irInfo._z}}; },
            [](IrInfo irInfo) {
              return BwdGraphInfo{irInfo._k, {}, {}};
            },
            ExpectC1::No,
            ExpectC2::No,
            ExpectC3::No),
        std::runtime_error);
  }

  // Test CallOp + AddFwdOutputs + default stitch indices. Expect C1.
  test(
      TestType::CallOpTestType,
      AutodiffStitchStrategy::AddFwdOutputs,
      [](IrInfo) { return nonstd::nullopt; },
      [](IrInfo irInfo) {
        return BwdGraphInfo{
            irInfo._k,
            sortExpectedConnections(
                {{irInfo._a, {irInfo.in, ExpectedConnectionType::Fwd}},
                 {irInfo._b, {irInfo.tmp, ExpectedConnectionType::Fwd}},
                 {irInfo._c, {irInfo.out, ExpectedConnectionType::Fwd}},
                 {irInfo._d, {irInfo.out, ExpectedConnectionType::FwdGrad}}}),
            sortExpectedConnections(
                {{0, {irInfo.in, ExpectedConnectionType::FwdGrad}}})};
      },
      ExpectC1::Yes,
      ExpectC2::No,
      ExpectC3::No);

  // Test CallOp + AddFwdOutputs + unstitchable index. Expect exception.
  BOOST_REQUIRE_THROW(
      test(
          TestType::CallOpTestType,
          AutodiffStitchStrategy::AddFwdOutputs,
          [](IrInfo irInfo) { return OptStitchIndices{{irInfo._y}}; },
          [](IrInfo irInfo) {
            return BwdGraphInfo{irInfo._k, {}, {}};
          },
          ExpectC1::No,
          ExpectC2::No,
          ExpectC3::No),
      std::runtime_error);

  // Test with CallOp + AddFwdOutputs + that indices not in the stitch index
  // list don't get stitched.
  test(
      TestType::CallOpTestType,
      AutodiffStitchStrategy::AddFwdOutputs,
      [](IrInfo irInfo) { return OptStitchIndices{{}}; },
      [](IrInfo irInfo) {
        return BwdGraphInfo{
            irInfo._k,
            sortExpectedConnections(
                {{irInfo._a, {irInfo.in, ExpectedConnectionType::Fwd}},
                 {irInfo._b, {irInfo.tmp, ExpectedConnectionType::Fwd}},
                 {irInfo._c, {irInfo.out, ExpectedConnectionType::Fwd}},
                 {irInfo._d, {irInfo.out, ExpectedConnectionType::FwdGrad}}}),
            sortExpectedConnections(
                {{0, {irInfo.in, ExpectedConnectionType::FwdGrad}}})};
      },
      ExpectC1::No,
      ExpectC2::No,
      ExpectC3::No);

  // Test IfOp + AddFwdOutputs + default stitch indices.
  // Does nothing, because this stitch method can't deal with IfOps.
  test(
      TestType::IfOpTestType,
      AutodiffStitchStrategy::AddFwdOutputs,
      [](IrInfo) { return nonstd::nullopt; },
      [](IrInfo irInfo) {
        return BwdGraphInfo{
            irInfo._k,
            sortExpectedConnections(
                {{irInfo._a, {irInfo.in, ExpectedConnectionType::Fwd}},
                 {irInfo._b, {irInfo.tmp, ExpectedConnectionType::Fwd}},
                 {irInfo._c, {irInfo.out, ExpectedConnectionType::Fwd}},
                 {irInfo._d, {irInfo.out, ExpectedConnectionType::FwdGrad}}}),
            sortExpectedConnections(
                {{0, {irInfo.in, ExpectedConnectionType::FwdGrad}}})};
      },
      ExpectC1::No,
      ExpectC2::No,
      ExpectC3::No);

  // -

  // Test CallOp + SafeAddFwdOutputs + default stitch indices. Expect C1 only.
  test(
      TestType::CallOpTestType,
      AutodiffStitchStrategy::SafeAddFwdOutputs,
      [](IrInfo) { return nonstd::nullopt; },
      [](IrInfo irInfo) {
        return BwdGraphInfo{
            irInfo._k,
            sortExpectedConnections(
                {{irInfo._a, {irInfo.in, ExpectedConnectionType::Fwd}},
                 {irInfo._b, {irInfo.tmp, ExpectedConnectionType::Fwd}},
                 {irInfo._c, {irInfo.out, ExpectedConnectionType::Fwd}},
                 {irInfo._d, {irInfo.out, ExpectedConnectionType::FwdGrad}}}),
            sortExpectedConnections(
                {{0, {irInfo.in, ExpectedConnectionType::FwdGrad}}})};
      },
      ExpectC1::Yes,
      ExpectC2::No,
      ExpectC3::No);

  // Test IfOp + SafeAddFwdOutputs + default stitch indices. Expect C2 because
  // we can't do C1 for IfOps.
  test(
      TestType::IfOpTestType,
      AutodiffStitchStrategy::SafeAddFwdOutputs,
      [](IrInfo) { return nonstd::nullopt; },
      [](IrInfo irInfo) {
        return BwdGraphInfo{
            irInfo._k,
            sortExpectedConnections(
                {{irInfo._a, {irInfo.in, ExpectedConnectionType::Fwd}},
                 {irInfo._c, {irInfo.out, ExpectedConnectionType::Fwd}},
                 {irInfo._d, {irInfo.out, ExpectedConnectionType::FwdGrad}}}),
            sortExpectedConnections(
                {{0, {irInfo.in, ExpectedConnectionType::FwdGrad}}})};
      },
      ExpectC1::No,
      ExpectC2::Yes,
      ExpectC3::No);

  // Test CallOp + SafeAddFwdOutputs + unstitchable index. Expect exception.
  BOOST_REQUIRE_THROW(
      test(
          TestType::CallOpTestType,
          AutodiffStitchStrategy::SafeAddFwdOutputs,
          [](IrInfo irInfo) { return OptStitchIndices{{irInfo._z}}; },
          [](IrInfo irInfo) {
            return BwdGraphInfo{irInfo._k, {}, {}};
          },
          ExpectC1::No,
          ExpectC2::No,
          ExpectC3::No),
      std::runtime_error);

  // Test with CallOp + SafeAddFwdOutputs + that indices not in the stitch index
  // list don't get stitched.
  test(
      TestType::CallOpTestType,
      AutodiffStitchStrategy::SafeAddFwdOutputs,
      [](IrInfo irInfo) { return OptStitchIndices{{}}; },
      [](IrInfo irInfo) {
        return BwdGraphInfo{
            irInfo._k,
            sortExpectedConnections(
                {{irInfo._a, {irInfo.in, ExpectedConnectionType::Fwd}},
                 {irInfo._b, {irInfo.tmp, ExpectedConnectionType::Fwd}},
                 {irInfo._c, {irInfo.out, ExpectedConnectionType::Fwd}},
                 {irInfo._d, {irInfo.out, ExpectedConnectionType::FwdGrad}}}),
            sortExpectedConnections(
                {{0, {irInfo.in, ExpectedConnectionType::FwdGrad}}})};
      },
      ExpectC1::No,
      ExpectC2::No,
      ExpectC3::No);
}


text:
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <control_toolbox/pid.h>
#include <realtime_tools/realtime_buffer.h>

#include <pluginlib/class_list_macros.h>
#include <std_msgs/Float64MultiArray.h>
#include <angles/angles.h>

#include <urdf/model.h>

#include <kdl/tree.hpp>
#include <kdl/kdl.hpp>
#include <kdl/chain.hpp>
#include <kdl/chaindynparam.hpp>
#include <kdl_parser/kdl_parser.hpp>

#include <boost/scoped_ptr.hpp>

#include "arm_controllers/ControllerJointState.h"
#include "arm_controllers/PassivityControllerParamsConfig.h"

#define PI 3.141592
#define D2R PI/180.0
#define R2D 180.0/PI

namespace arm_controllers{

	class PassivityController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
	{
		class GainsHandler
		{
		public:
			struct Gains
			{
				Gains() : alpha_(0.0)	{}
				Gains(double alpha) : alpha_(alpha)	{}

				double alpha_;	// it's only one gain, but  make it structure for unity with the controller having multiple gains
			};

			GainsHandler()	{}

			bool initDynamicReconfig(const ros::NodeHandle &node)
			{
				ROS_INFO("Init dynamic reconfig in namespace %s", node.getNamespace().c_str());

				Gains gains;
				if (!node.getParam("alpha", gains.alpha_))
				{
					ROS_ERROR("Could not find gain %s", (node.getNamespace()+"/alpha").c_str());
					return false;
				}

				// Start dynamic reconfigure server
				param_reconfig_server_.reset(new DynamicReconfigServer(param_reconfig_mutex_, node));
				dynamic_reconfig_initialized_ = true;

				setGains(gains);

				// Set Dynamic Reconfigure's gains to Pid's values
				updateDynamicReconfig();

				// Set callback
				param_reconfig_callback_ = boost::bind(&GainsHandler::dynamicReconfigCallback, this, _1, _2);
				param_reconfig_server_->setCallback(param_reconfig_callback_);

				return true;
			}

			void getGains(double &alpha)
			{
				Gains gains = *gains_buffer_.readFromRT();
				alpha     = gains.alpha_;
			}

			Gains getGains()
			{
			  	return *gains_buffer_.readFromRT();
			}

			void setGains(double alpha)
			{
				Gains gains(alpha);

				setGains(gains);
			}

			void setGains(const Gains &gains)
			{
				gains_buffer_.writeFromNonRT(gains);

				updateDynamicReconfig(gains);
			}

			void updateDynamicReconfig()
			{
				// Make sure dynamic reconfigure is initialized
				if(!dynamic_reconfig_initialized_)
					return;

				// Get starting values
				PassivityControllerParamsConfig config;
				getGains(config.alpha);

				updateDynamicReconfig(config);
			}

			void updateDynamicReconfig(Gains gains)
			{
				// Make sure dynamic reconfigure is initialized
				if(!dynamic_reconfig_initialized_)
					return;

				PassivityControllerParamsConfig config;

				// Convert to dynamic reconfigure format
				config.alpha = gains.alpha_;

				updateDynamicReconfig(config);
			}

			void updateDynamicReconfig(PassivityControllerParamsConfig config)
			{
				// Make sure dynamic reconfigure is initialized
				if(!dynamic_reconfig_initialized_)
					return;

				// Set starting values, using a shared mutex with dynamic reconfig
				param_reconfig_mutex_.lock();
				param_reconfig_server_->updateConfig(config);
				param_reconfig_mutex_.unlock();
			}

			void dynamicReconfigCallback(PassivityControllerParamsConfig &config, uint32_t /*level*/)
			{
				ROS_DEBUG_STREAM_NAMED("passivity gain","Dynamics reconfigure callback recieved.");

				// Set the gains
				setGains(config.alpha);
			}

		private:
			// Store the gains in a realtime buffer to allow dynamic reconfigure to update it without
			// blocking the realtime update loop
			realtime_tools::RealtimeBuffer<Gains> gains_buffer_;

			// Dynamics reconfigure
			bool dynamic_reconfig_initialized_;
			typedef dynamic_reconfigure::Server<arm_controllers::PassivityControllerParamsConfig> DynamicReconfigServer;
			boost::shared_ptr<DynamicReconfigServer> param_reconfig_server_;
			DynamicReconfigServer::CallbackType param_reconfig_callback_;

			boost::recursive_mutex param_reconfig_mutex_;
		};

		public:
		~PassivityController()
		{
			command_sub_.shutdown();
		}

		bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle &n)
  		{
			loop_count_ = 0;
			// List of controlled joints
    		if (!n.getParam("joints", joint_names_))
			{
				ROS_ERROR("Could not find joint name");
				return false;
    		}
			n_joints_ = joint_names_.size();

			if(n_joints_ == 0)
			{
				ROS_ERROR("List of joint names is empty.");
				return false;
			}

			// urdf
			urdf::Model urdf;
			if (!urdf.initParam("robot_description"))
			{
				ROS_ERROR("Failed to parse urdf file");
            	return false;
			}

			// joint handle
			for(int i=0; i<n_joints_; i++)
			{
				try
				{
					joints_.push_back(hw->getHandle(joint_names_[i]));
				}
				catch (const hardware_interface::HardwareInterfaceException& e)
				{
					ROS_ERROR_STREAM("Exception thrown: " << e.what());
					return false;
				}

				urdf::JointConstSharedPtr joint_urdf = urdf.getJoint(joint_names_[i]);
				if (!joint_urdf)
				{
					ROS_ERROR("Could not find joint '%s' in urdf", joint_names_[i].c_str());
                	return false;
				}
				joint_urdfs_.push_back(joint_urdf);
			}

			// kdl parser
			if (!kdl_parser::treeFromUrdfModel(urdf, kdl_tree_)){
				ROS_ERROR("Failed to construct kdl tree");
				return false;
			}

			// kdl chain
			std::string root_name, tip_name;
			if (!n.getParam("root_link", root_name))
			{
				ROS_ERROR("Could not find root link name");
				return false;
    		}
			if (!n.getParam("tip_link", tip_name))
			{
				ROS_ERROR("Could not find tip link name");
				return false;
    		}
			if(!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
			{
				ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
				ROS_ERROR_STREAM("  "<<root_name<<" --> "<<tip_name);
				ROS_ERROR_STREAM("  Tree has "<<kdl_tree_.getNrOfJoints()<<" joints");
				ROS_ERROR_STREAM("  Tree has "<<kdl_tree_.getNrOfSegments()<<" segments");
				ROS_ERROR_STREAM("  The segments are:");

				KDL::SegmentMap segment_map = kdl_tree_.getSegments();
            	KDL::SegmentMap::iterator it;

            	for( it=segment_map.begin(); it != segment_map.end(); it++ )
              		ROS_ERROR_STREAM( "    "<<(*it).first);

            	return false;
			}

			gravity_ = KDL::Vector::Zero();
			gravity_(2) = -9.81;
			M_.resize(n_joints_);
			C_.resize(n_joints_);
			G_.resize(n_joints_);

			// inverse dynamics solver
			id_solver_.reset( new KDL::ChainDynParam(kdl_chain_, gravity_) );

			// command and state
			tau_cmd_.data = Eigen::VectorXd::Zero(n_joints_);
			tau_fric_.data = Eigen::VectorXd::Zero(n_joints_);
			q_cmd_.data = Eigen::VectorXd::Zero(n_joints_);
			qdot_cmd_.data = Eigen::VectorXd::Zero(n_joints_);
			qddot_cmd_.data = Eigen::VectorXd::Zero(n_joints_);
			q_cmd_old_.data = Eigen::VectorXd::Zero(n_joints_);
			qdot_cmd_old_.data = Eigen::VectorXd::Zero(n_joints_);
			qdot_ref_.data = Eigen::VectorXd::Zero(n_joints_);
			qddot_ref_.data = Eigen::VectorXd::Zero(n_joints_);

			q_.data = Eigen::VectorXd::Zero(n_joints_);
			qdot_.data = Eigen::VectorXd::Zero(n_joints_);

			q_error_.data = Eigen::VectorXd::Zero(n_joints_);
			q_error_dot_.data = Eigen::VectorXd::Zero(n_joints_);

			// gains
			if (!gains_handler_.initDynamicReconfig(ros::NodeHandle(n, "gains/")) )
			{
				ROS_ERROR_STREAM("Failed to load alpha gain parameter from gains");
				return false;
			}

			pids_.resize(n_joints_);
			for (size_t i=0; i<n_joints_; i++)
			{
				if(!pids_[i].init(ros::NodeHandle(n, "gains/"+joint_names_[i]+"/pid")))
				{
					ROS_ERROR_STREAM("Failed to load PID parameters from " << joint_names_[i] + "/pid");
					return false;
				}
			}

			// command subscriber
			commands_buffer_.writeFromNonRT(std::vector<double>(n_joints_, 0.0));
			command_sub_ = n.subscribe<std_msgs::Float64MultiArray>("command", 1, &PassivityController::commandCB, this);

			// start realtime state publisher
			controller_state_pub_.reset(
				new realtime_tools::RealtimePublisher<arm_controllers::ControllerJointState>(n, "state", 1));

			controller_state_pub_->msg_.header.stamp = ros::Time::now();
			for (size_t i=0; i<n_joints_; i++)
			{
				controller_state_pub_->msg_.name.push_back(joint_names_[i]);
				controller_state_pub_->msg_.command.push_back(0.0);
				controller_state_pub_->msg_.command_dot.push_back(0.0);
				controller_state_pub_->msg_.state.push_back(0.0);
				controller_state_pub_->msg_.state_dot.push_back(0.0);
				controller_state_pub_->msg_.error.push_back(0.0);
				controller_state_pub_->msg_.error_dot.push_back(0.0);
				controller_state_pub_->msg_.effort_command.push_back(0.0);
				controller_state_pub_->msg_.effort_feedforward.push_back(0.0);
				controller_state_pub_->msg_.effort_feedback.push_back(0.0);
			}

   			return true;
  		}

		void starting(const ros::Time& time)
		{
			// get joint positions
			for(size_t i=0; i<n_joints_; i++)
			{
				q_(i) = joints_[i].getPosition();
				qdot_(i) = joints_[i].getVelocity();
			}

			ROS_INFO("Starting Passivity Based Controller");
		}

		void commandCB(const std_msgs::Float64MultiArrayConstPtr& msg)
		{
			if(msg->data.size()!=n_joints_)
			{
			ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!");
			return;
			}
			commands_buffer_.writeFromNonRT(msg->data);
		}

  		void update(const ros::Time& time, const ros::Duration& period)
  		{
			std::vector<double> & commands = *commands_buffer_.readFromRT();
			double dt = period.toSec();
			double q_cmd_old;

			// get joint states
			static double t = 0;
			for (size_t i=0; i<n_joints_; i++)
			{
				q_cmd_(i) = 45*D2R*sin(PI/2*t);
				//q_cmd_(i) = commands[i];

				enforceJointLimits(q_cmd_(i), i);
				q_(i) = joints_[i].getPosition();
				qdot_(i) = joints_[i].getVelocity();

		        // Compute position error
				if (joint_urdfs_[i]->type == urdf::Joint::REVOLUTE)
				{
					angles::shortest_angular_distance_with_limits(
						q_(i),
						q_cmd_(i),
						joint_urdfs_[i]->limits->lower,
						joint_urdfs_[i]->limits->upper,
						q_error_(i));
				}
				else if (joint_urdfs_[i]->type == urdf::Joint::CONTINUOUS)
				{
					q_error_(i) = angles::shortest_angular_distance(q_(i), q_cmd_(i));
				}
				else //prismatic
				{
					q_error_(i) = q_cmd_(i) - q_(i);
				}

				qdot_cmd_(i) = 45*D2R*PI/2*cos(PI/2*t); // (q_cmd_(i) - q_cmd_old_(i)) / period.toSec();;
				qddot_cmd_(i) = -45*D2R*PI*PI/2/2*sin(PI/2*t); // (qdot_cmd_(i) - qdot_cmd_old_(i)) / period.toSec();

				q_error_dot_(i) = qdot_cmd_(i) - qdot_(i);

				q_cmd_old_(i) = q_cmd_(i);
				qdot_cmd_old_(i) = qdot_cmd_(i);

				// friction compensation, to do: implement friction observer
				tau_fric_(i) = 1*qdot_(i) + 1*KDL::sign(qdot_(i));
			}

			t += dt;

			// compute dynamics term
			id_solver_->JntToMass(q_, M_);
			id_solver_->JntToCoriolis(q_, qdot_, C_);
			id_solver_->JntToGravity(q_, G_);

			// torque command
			GainsHandler::Gains gains = gains_handler_.getGains();
			qdot_ref_.data = qdot_cmd_.data + gains.alpha_*q_error_.data;
			qddot_ref_.data = qddot_cmd_.data + gains.alpha_*q_error_dot_.data;

			tau_cmd_.data = M_.data * qddot_ref_.data + C_.data + G_.data + tau_fric_.data;

			for(int i=0; i<n_joints_; i++)
			{
				controller_state_pub_->msg_.effort_feedforward[i] = tau_cmd_(i);
				tau_cmd_(i) += pids_[i].computeCommand(q_error_(i), q_error_dot_(i), period);

				// effort saturation
				if (tau_cmd_(i) >= joint_urdfs_[i]->limits->effort)
					tau_cmd_(i) = joint_urdfs_[i]->limits->effort;

				if (tau_cmd_(i) <= -joint_urdfs_[i]->limits->effort)
					tau_cmd_(i) = -joint_urdfs_[i]->limits->effort;

				joints_[i].setCommand( tau_cmd_(i) );
			}

			// publish
			if (loop_count_ % 10 == 0)
			{
				if (controller_state_pub_->trylock())
				{
					controller_state_pub_->msg_.header.stamp = time;
					for(int i=0; i<n_joints_; i++)
					{
						controller_state_pub_->msg_.command[i] = R2D*q_cmd_(i);
						controller_state_pub_->msg_.command_dot[i] = R2D*qdot_cmd_(i);
						controller_state_pub_->msg_.state[i] = R2D*q_(i);
						controller_state_pub_->msg_.state_dot[i] = R2D*qdot_(i);
						controller_state_pub_->msg_.error[i] = R2D*q_error_(i);
						controller_state_pub_->msg_.error_dot[i] = R2D*q_error_dot_(i);
						controller_state_pub_->msg_.effort_command[i] = tau_cmd_(i);
						controller_state_pub_->msg_.effort_feedback[i] = tau_cmd_(i) - controller_state_pub_->msg_.effort_feedforward[i];
					}
					controller_state_pub_->unlockAndPublish();
				}
			}
  		}

  		void stopping(const ros::Time& time) { }

		void enforceJointLimits(double &command, unsigned int index)
		{
			// Check that this joint has applicable limits
			if (joint_urdfs_[index]->type == urdf::Joint::REVOLUTE || joint_urdfs_[index]->type == urdf::Joint::PRISMATIC)
			{
			if( command > joint_urdfs_[index]->limits->upper ) // above upper limnit
			{
				command = joint_urdfs_[index]->limits->upper;
			}
			else if( command < joint_urdfs_[index]->limits->lower ) // below lower limit
			{
				command = joint_urdfs_[index]->limits->lower;
			}
			}
		}
	private:
		int loop_count_;

		// joint handles
		unsigned int n_joints_;
		std::vector<std::string> joint_names_;
  		std::vector<hardware_interface::JointHandle> joints_;
		std::vector<urdf::JointConstSharedPtr> joint_urdfs_;

		// kdl
		KDL::Tree 	kdl_tree_;
		KDL::Chain	kdl_chain_;
		boost::scoped_ptr<KDL::ChainDynParam> id_solver_;	// inverse dynamics solver
		KDL::JntSpaceInertiaMatrix M_;
		KDL::JntArray C_;
		KDL::JntArray G_;									// gravity torque vector
		KDL::Vector gravity_;

		// cmd, state
		realtime_tools::RealtimeBuffer<std::vector<double> > commands_buffer_;
		KDL::JntArray tau_cmd_, tau_fric_;
		KDL::JntArray q_cmd_, qdot_cmd_, qddot_cmd_, q_cmd_old_, qdot_cmd_old_;
		KDL::JntArray qdot_ref_, qddot_ref_;				// passivity-based control joint reference
		KDL::JntArray q_, qdot_;
		KDL::JntArray q_error_, q_error_dot_;

		// gain
		GainsHandler gains_handler_;				// alpha gain(dynamic reconfigured)
		std::vector<control_toolbox::Pid> pids_; // Internal PID controllers in ros-control

		// topic
		ros::Subscriber command_sub_;
		boost::scoped_ptr<
			realtime_tools::RealtimePublisher<
				arm_controllers::ControllerJointState> > controller_state_pub_;
	};

}

PLUGINLIB_EXPORT_CLASS(arm_controllers::PassivityController, controller_interface::ControllerBase)

ext: cpp

max_stars_repo_name: nicolastrimborn/arm-control-dreamteam

max_issues_repo_name: nicolastrimborn/arm-control-dreamteam

max_forks_repo_name: nicolastrimborn/arm-control-dreamteam


text:
#pragma once
#include "detail/config.hpp"
#include "numeric/sparseref.hpp"

#include <Eigen/SparseCore>

namespace cpb {

template <class scalar_t>
using SparseMatrixX = Eigen::SparseMatrix<scalar_t, Eigen::RowMajor, storage_idx_t>;

using SparseMatrixXf = SparseMatrixX<float>;
using SparseMatrixXcf = SparseMatrixX<std::complex<float>>;
using SparseMatrixXd = SparseMatrixX<double>;
using SparseMatrixXcd = SparseMatrixX<std::complex<double>>;

using num::RealCsrConstRef;
using num::ComplexCsrConstRef;
using num::RealEllConstRef;
using num::ComplexEllConstRef;

/**
 Return a CSR matrix reference
 */
template<class scalar_t>
inline num::CsrConstRef<scalar_t> csrref(SparseMatrixX<scalar_t> const& m) {
    return {static_cast<int>(m.rows()), static_cast<int>(m.cols()), static_cast<int>(m.nonZeros()),
            m.valuePtr(), m.innerIndexPtr(), m.outerIndexPtr()};
};

template<class scalar_t>
class CompressedInserter {
public:
    CompressedInserter(SparseMatrixX<scalar_t>& mat, idx_t size)
        : matrix(mat) { matrix.reserve(size); }

    void start_row() {
        matrix.outerIndexPtr()[row++] = static_cast<storage_idx_t>(idx);
    }

    void start_row(idx_t row_index) {
        while (row <= row_index)
            matrix.outerIndexPtr()[row++] = static_cast<storage_idx_t>(idx);
    }

    void insert(idx_t column, scalar_t value) {
        auto start_idx = matrix.outerIndexPtr()[row-1];
        auto n = idx++;
        while (n > start_idx && matrix.innerIndexPtr()[n - 1] > column) {
            matrix.innerIndexPtr()[n] = matrix.innerIndexPtr()[n - 1];
            matrix.valuePtr()[n] = matrix.valuePtr()[n - 1];
            --n;
        }
        matrix.innerIndexPtr()[n] = static_cast<storage_idx_t>(column);
        matrix.valuePtr()[n] = value;
    }

    void compress() {
        // close outerIndexPtr
        start_row(matrix.outerSize());
        // trim valuePtr and innerIndexPtr
        matrix.resizeNonZeros(idx);
    }

private:
    idx_t idx = 0;
    idx_t row = 0;
    SparseMatrixX<scalar_t>& matrix;
};

template<class scalar_t>
inline CompressedInserter<scalar_t> compressed_inserter(SparseMatrixX<scalar_t>& mat, idx_t size) {
    return {mat, size};
}

template<class SparseMatrix, class Index>
inline auto sparse_row(const SparseMatrix& mat, Index outer_index)
    -> typename SparseMatrix::InnerIterator
{
    return {mat, outer_index};
}

namespace num {

template<class scalar_t>
SparseMatrixX<scalar_t> force_cast(SparseMatrixXcd const& m) { return m.cast<scalar_t>(); }
template<>
inline SparseMatrixX<double> force_cast<double>(SparseMatrixXcd const& m) { return m.real(); }
template<>
inline SparseMatrixX<float> force_cast<float>(SparseMatrixXcd const& m) {
    return m.real().cast<float>();
}

} // namespace num

namespace sparse {

/// SparseMatrix wrapper with several functions for efficient CSR matrix element access
template<class scalar_t>
class Loop {
public:
    Loop(SparseMatrixX<scalar_t> const& matrix)
        : outer_size(matrix.outerSize()), data(matrix.valuePtr()),
          indices(matrix.innerIndexPtr()), indptr(matrix.outerIndexPtr()) {}

    /// Visit each index and value of the sparse matrix:
    ///     lambda(idx_t outer, idx_t inner, scalar_t value)
    template<class F>
    void for_each(F lambda) const {
        for (auto outer = idx_t{0}; outer < outer_size; ++outer) {
            for (auto idx = indptr[outer]; idx < indptr[outer + 1]; ++idx) {
                lambda(outer, indices[idx], data[idx]);
            }
        }
    }

    /// Visit each index and value of the sparse matrix:
    ///     lambda(idx_t outer, idx_t inner, scalar_t value, idx_t buffer_position)
    /// After every 'buffer_size' iterations, the 'process_buffer' function is called:
    ///     process_buffer(idx_t start_outer, idx_t start_data, idx_t last_buffer_size)
    template<class F1, class F2>
    void buffered_for_each(idx_t buffer_size, F1 lambda, F2 process_buffer) const {
        auto n = idx_t{0};
        auto previous_outer = idx_t{0};
        auto previous_idx = static_cast<idx_t>(indptr[0]);

        for (auto outer = idx_t{0}; outer < outer_size; ++outer) {
            for (auto idx = indptr[outer]; idx < indptr[outer + 1]; ++idx, ++n) {
                if (n == buffer_size) {
                    process_buffer(previous_outer, previous_idx, buffer_size);
                    previous_outer = outer;
                    previous_idx = idx;
                    n = 0;
                }

                lambda(outer, indices[idx], data[idx], n);
            }
        }

        process_buffer(previous_outer, previous_idx, n);
    }

    /// Iterate over all elements in a single row (or column) at the 'outer' index:
    ///     lambda(idx_t inner, scalar_t value)
    template<class F>
    void for_each_in_row(idx_t outer, F lambda) const {
        for (auto idx = indptr[outer]; idx < indptr[outer + 1]; ++idx) {
            lambda(indices[idx], data[idx]);
        }
    }

    /// Start iteration from some position given by 'outer' and 'data' indices
    /// and loop for 'slice_size' iterations:
    ///     lambda(idx_t outer, idx_t inner, scalar_t value, idx_t current_iteration)
    template<class F>
    void slice_for_each(idx_t outer, idx_t idx, idx_t slice_size, F lambda) const {
        auto n = idx_t{0};
        for (; outer < outer_size; ++outer) {
            for (; idx < indptr[outer + 1]; ++idx, ++n) {
                if (n == slice_size)
                    return;

                lambda(outer, indices[idx], data[idx], n);
            }
        }
    }

private:
    idx_t const outer_size;
    scalar_t const* const data;
    storage_idx_t const* const indices;
    storage_idx_t const* const indptr;
};

template<class scalar_t>
inline Loop<scalar_t> make_loop(SparseMatrixX<scalar_t> const& m) { return {m}; }

/**
 Return the maximum number of non-zeros per row
 */
template<class scalar_t>
idx_t max_nnz_per_row(SparseMatrixX<scalar_t> const& m) {
    auto max = 0;
    for (auto i = 0; i < m.outerSize(); ++i) {
        auto const nnz = m.outerIndexPtr()[i + 1] - m.outerIndexPtr()[i];
        if (nnz > max) {
            max = nnz;
        }
    }
    return max;
}

}} // namespace cpb::sparse

ext: hpp

max_issues_repo_name: deilynazar/pybinding

max_forks_repo_name: deilynazar/pybinding


text:
#include "stdafx.h"
#include "CResovle.h"
#include <boost\make_shared.hpp>
#include <boost\random.hpp>

#include "CMPP20Head.hpp"
#include "CMPP20Submit.hpp"
#include "CMPP20Connect.hpp"
#include "CMPP20Resps.hpp"
#include "CMPP20Deliver.hpp"

#include "SGIP12Head.hpp"
//#include "SGIP12Deliver.hpp"
//#include "SGIP12Report.hpp"
//#include "SGIP12Submit.hpp"
#include "SGIP12Bind.hpp"
//
#include "SMGP303Head.hpp"
#include "SMGP303Login.hpp"
//#include "SMGP303Submit.hpp"
//#include "SMGP303Deliver.hpp"

#include "SMProxy2.h"
namespace smproxy {
	CResolve::CResolve(Protocol protocol, std::string sp_id):
		protocol_(protocol),
		err_code_(0)
	{
		switch (protocol)
		{
		case SGIP12:
			hd.reset(new SGIP12Head);
			bd.reset(new SGIP12Bind);
			//sb = boost::make_shared<SGIP12Submit>(SGIP12Submit());
			//dlv = boost::make_shared<SGIP12Deliver>(SGIP12Deliver());
			//rp = boost::make_shared<SGIP12Report>(SGIP12Report());
			break;
		case CMPP20:
			hd.reset(new CMPP20Head);
			bd.reset(new CMPP20Connect);
			sb.reset(new CMPP20Submit());
			rsp.reset(new CMPP20Resps());
			dlv.reset(new CMPP20Deliver());
			rp.reset(new CMPP20Deliver());
			break;
		case SMGP303:
			hd.reset(new SMGP303Head);
			bd.reset(new SMGP303Login);
			//sb = boost::make_shared<SMGP303Submit>(SMGP303Submit());
			//dlv = boost::make_shared<SMGP303Deliver>(SMGP303Deliver());
			//rp = boost::make_shared<SMGP303Deliver>(SMGP303Deliver());
			break;
		default:
			break;
		}
		sp_ID_ = sp_id;
	}
	//发送短信 内容为uc2编码的字节串
	bytes CResolve::submitBytes(
		int& serial_numb,
		const char* src_ID,
		std::vector<std::string> &user_num,
		std::vector<uint8_t>& message_content
	)
	{
		hd->setSpId(sp_ID_);
		sb->setSpId(sp_ID_);
		buf_.clear();
		//正常短信
		if (message_content.size() <= 140)
		{
			sb->submit(
				src_ID,
				user_num,
				8,
				message_content
			);
			buf_ = sb->getBuf();
			serial_numb = hd->createSerualNum(serial_numb);
			auto head_buf = hd->header(
				buf_.size(),
				Command::submit,
				hd->getSerialNumb()
			);
			head_buf = hd->header(
				buf_.size()+ head_buf.size(),
				Command::submit,
				hd->getSerialNumb()
			);
			buf_.insert(buf_.begin(), head_buf.begin(), head_buf.end());
		}
		else//长短信
		{
			std::vector<bytes> msgs;
			auto iter_1 = message_content.begin();
			auto iter_2 = message_content.begin();
			while (iter_1 != message_content.end())
			{
				for (size_t i = 0; i < 134 && iter_2 != message_content.end(); i++)
				{
					++iter_2;
				}
				bytes temp_msg;
				temp_msg.assign(iter_1, iter_2);
				msgs.push_back(temp_msg);
				iter_1 = iter_2;
			}
			boost::random::mt19937 gen;
			boost::uniform_int<> real(1, 99);
			uint8_t msg_id = uint8_t(real(gen));
			for (uint8_t i = 0; i < msgs.size(); i++)
			{
				bytes head;
				head.push_back(5);
				head.push_back(0);
				head.push_back(3);
				head.push_back(msg_id);
				head.push_back(uint8_t(msgs.size()));
				head.push_back(i + 1);
				msgs[i].insert(msgs[i].begin(), head.begin(), head.end());
				sb->submit(
					src_ID,
					user_num,
					8,
					msgs[i],
					msgs.size(),
					i+1,
					0,
					1
				);
				auto one_msg_buf = sb->getBuf();

				serial_numb = hd->createSerualNum(serial_numb);
				auto head_buf = hd->header(
					one_msg_buf.size(),
					Command::submit,
					hd->getSerialNumb()
				);
				head_buf = hd->header(
					one_msg_buf.size() + head_buf.size(),
					Command::submit,
					hd->getSerialNumb()
				);
				one_msg_buf.insert(one_msg_buf.begin(), head_buf.begin(), head_buf.end());
				buf_.insert(buf_.end(), one_msg_buf.begin(), one_msg_buf.end());
			}//循环添加长短信切片
		}//是否长短信
		return buf_;
	}

	bytes CResolve::loginBytes(
		int& serial_numb,
		const char* password,
		int login_type
	)
	{
		bd->setType(login_type);
		bd->Binder(sp_ID_, password);
		buf_ = bd->getBuf();
		serial_numb = hd->createSerualNum(serial_numb);
		auto head_buf = hd->header(
			buf_.size(),
			login,
			hd->getSerialNumb()
		);
		hd->setSpId(sp_ID_);
		head_buf = hd->header(
			buf_.size()+ head_buf.size(),
			login,
			hd->getSerialNumb()
		);
		buf_.insert(buf_.begin(), head_buf.begin(), head_buf.end());
		return buf_;
	}

	bytes CResolve::deliverRespBytes(std::string serial_numb, int status, std::string msg_id)
	{
		buf_ = rsp->deliverRespBytes(status, msg_id);
		//serial_numb = hd->createSerualNum(serial_numb);
		auto head_buf = hd->header(
			buf_.size(),
			deliver_resp,
			serial_numb
		);
		hd->setSpId(sp_ID_);
		head_buf = hd->header(
			buf_.size() + head_buf.size(),
			deliver_resp,
			serial_numb
		);
		buf_.insert(buf_.begin(), head_buf.begin(), head_buf.end());
		return buf_;
	}
	bytes CResolve::activeRespBytes(std::string serial_numb, int status)
	{
		buf_ = rsp->activeRespBytes(status);
		auto head_buf = hd->header(
			buf_.size(),
			active_resp,
			serial_numb
		);
		hd->setSpId(sp_ID_);
		head_buf = hd->header(
			buf_.size() + head_buf.size(),
			active_resp,
			serial_numb
		);
		buf_.insert(buf_.begin(), head_buf.begin(), head_buf.end());
		return buf_;
	}

	bytes CResolve::activeBytes(int & serial_numb)
	{
		serial_numb = hd->createSerualNum(serial_numb);
		auto head_buf = hd->header(
			12,
			active,
			hd->getSerialNumb()
		);
		head_buf = hd->header(
			head_buf.size(),
			active,
			hd->getSerialNumb()
		);
		return head_buf;
	}

	void CResolve::resolveBuf(bytes& buf)
	{
		auto iter = hd->recvHead(buf);
		cmd_id_ = hd->getCmdID();
		switch (cmd_id_)
		{
		case login:
		case login_resp:
			rsp->recvLoginResp(buf, iter);
			break;
		case unbind:
		case unbind_resp:
		case Command::submit:

		case submit_resp:
			rsp->recvSubmitResp(buf, iter);
			msg_id_ = rsp->getMsgId();
			break;
		case deliver:
			dlv->recvDeliver(buf, iter);
			msg_id_this_ = dlv->getMsgIdThis();
			src_terminal_id_ = dlv->getSrcTerminalId();
			dest_id_ = dlv->getDestId();
			msg_len_ = dlv->getMsgLen();
			ISMG_id_ = dlv->getISMGId();
			if (dlv->getReportFlag())
			{
				cmd_id_ = report;
				rp->recvDeliver(buf, iter);
				msg_id_ = rp->getMsgId();
			}
			else
			{
				message_content_ = dlv->getMsgContent();
			}
			break;
		case deliver_resp:
		case report:
		case report_resp:
		case active:
			break;
		case active_resp:
			break;
		default:
			break;
		}
	}
}


text:
/*
 * Copyright (C) 2019 Intel Corporation
 *
 * SPDX-License-Identifier: MIT
 *
 */

#ifndef COMPUTE_SAMPLES_USM_MEM_INFO_HPP
#define COMPUTE_SAMPLES_USM_MEM_INFO_HPP

#include <vector>

#include <boost/compute/core.hpp>
#include <boost/compute/intel/usm.hpp>

#include "application/application.hpp"

namespace compute_samples {

class UsmMemInfoApplication : public Application {
private:
  Status run_implementation(std::vector<std::string> &command_line) override;
  struct Arguments {
    bool help = false;
    boost::compute::usm_type type = boost::compute::usm_type::host;
  };
  Arguments
  parse_command_line(const std::vector<std::string> &command_line) const;
};

} // namespace compute_samples

#endif


text: #include <boost/iostreams/char_traits.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
#include <boost/iostreams/device/array.hpp>
#include <boost/iostreams/device/back_inserter.hpp>
#include <boost/iostreams/stream.hpp>
#include <vector>
#include <string>
#include <iostream>

using namespace boost::iostreams;

int main()
{
  std::vector<char> v;
  back_insert_device<std::vector<char>> sink{v};
  stream<back_insert_device<std::vector<char>>> os{sink};
  os << "Boost" << std::endl;

  array_source source{v.data(), v.size()};
  stream<array_source> is{source};
  std::string s;
  is >> s;
  std::cout << s << '\n';
}

max_stars_repo_name: KwangjoJeong/Boost

max_issues_repo_name: KwangjoJeong/Boost

max_forks_repo_name: KwangjoJeong/Boost