Lodestar
An integrated real-time control package in C++
SumBlock.hpp
1 //
2 // Created by Hamza El-Kebir on 12/23/21.
3 //
4 
5 #ifndef LODESTAR_SUMBLOCK_HPP
6 #define LODESTAR_SUMBLOCK_HPP
7 
8 
9 #include "Lodestar/blocks/Block.hpp"
10 #include "Lodestar/aux/TemplateTools.hpp"
11 
12 namespace ls {
13  namespace blocks {
14  namespace std {
15  enum class SumBlockOperator {
16  Plus,
17  Minus
18  };
19 
20  struct SumBlockOperatorHelper {
21  template<unsigned int N>
22  static typename ls::aux::TemplateTools::repeat<SumBlockOperator, N, ::std::tuple>::type
23  repeatTuple(SumBlockOperator value = SumBlockOperator::Plus)
24  {
25  typename ls::aux::TemplateTools::repeat<SumBlockOperator, N, ::std::tuple>::type tuple{};
26  repeatTupleImpl<N, N - 1>(tuple, value);
27 
28  return tuple;
29  }
30 
31  template<unsigned int N, int TIdx = N - 1,
32  typename ::std::enable_if<(TIdx >
33  0), bool>::type * = nullptr>
34  static void repeatTupleImpl(
35  typename ls::aux::TemplateTools::repeat<SumBlockOperator, N, ::std::tuple>::type &tuple,
36  SumBlockOperator value)
37  {
38  ::std::get<TIdx>(tuple) = value;
39  return repeatTupleImpl<N, TIdx - 1>(tuple, value);
40  }
41 
42  template<unsigned int N, int TIdx = N - 1,
43  typename ::std::enable_if<(TIdx ==
44  0), bool>::type * = nullptr>
45  static void repeatTupleImpl(
46  typename ls::aux::TemplateTools::repeat<SumBlockOperator, N, ::std::tuple>::type &tuple,
47  SumBlockOperator value)
48  {
49  ::std::get<0>(tuple) = value;
50  }
51 
52  template<unsigned int N, int TIdx = N -
53  1, typename ::std::enable_if<(
54  TIdx <
55  0), bool>::type * = nullptr>
56  static void repeatTupleImpl(
57  typename ls::aux::TemplateTools::repeat<SumBlockOperator, N, ::std::tuple>::type &tuple,
58  SumBlockOperator value)
59  {
60  return;
61  }
62 
63  static inline int interpret(SumBlockOperator value)
64  {
65  switch (value) {
66  default:
67  case SumBlockOperator::Plus:
68  return 1;
69  case SumBlockOperator::Minus:
70  return -1;
71  }
72  }
73  };
74 
75  template<typename TType, unsigned int N>
76  class SumBlock :
77  public Block<
78  typename ls::aux::TemplateTools::repeat<TType, N, ::std::tuple>::type,
79  ::std::tuple<TType>,
80  ::std::tuple<::std::array<SumBlockOperator, N>, TType>
81  > {
82  public:
83  using Base =
84  Block<
85  typename ls::aux::TemplateTools::repeat<TType, N, ::std::tuple>::type,
86  ::std::tuple<TType>,
87  ::std::tuple<::std::array<SumBlockOperator, N>, TType>
88  >;
89 
90  using Ops = SumBlockOperator;
91 
92  static constexpr const SumBlockOperator Plus = SumBlockOperator::Plus;
93  static constexpr const SumBlockOperator Minus = SumBlockOperator::Minus;
94  // template <int TIdx>
95  // using Base::template o<TIdx>;
96 
97  // using Base::getInput;
98 
99  // template <size_t TIdx>
100  // auto i() const
101  // {
102  // auto i_impl = Base::template i<size_t>();
103  //
104  // i_impl();
105  // }
106 
107  SumBlock()
108  {
109  (this->template p<0>()).fill(SumBlockOperator::Plus);
110  bindEquation();
111  // getInput<0>();
112  // getInput<0>();
113  // Base::template i<0>();
114  // this->equation = [&](Base & b) {
115  // b.template o<0>() = sum();
116  // };
117  }
118 
119  template<typename... TOperators>
120  SumBlock(Ops op, TOperators... ops)
121  {
122  (this->template p<0>()).fill(SumBlockOperator::Plus);
123  setOperators(op, ops...);
124  bindEquation();
125  }
126 
127  template<int TIdx = 0, typename... TOperators, typename ::std::enable_if<
128  TIdx < N, bool>::type * = nullptr>
129  void setOperators(Ops op, TOperators... ops)
130  {
131  static_assert(
132  ls::aux::TemplateTraits::allSame<SumBlockOperator, TOperators...>::value,
133  "Operators must all be SumBlockOperator values."
134  );
135 
136  (this->template p<0>())[TIdx] = op;
137 
138  return setOperators<TIdx + 1>(ops...);
139  }
140 
141  template<int TIdx = 0, typename... TOperators,
142  typename ::std::enable_if<
143  TIdx >= N, bool>::type * = nullptr>
144  void setOperators(Ops op, TOperators... ops)
145  {
146  return;
147  }
148 
149  template<int TIdx = 0,
150  typename ::std::enable_if<
151  TIdx < N, bool>::type * = nullptr>
152  void setOperators(Ops op)
153  {
154  (this->template p<0>())[TIdx] = op;
155  }
156 
157  template<int TIdx = 0, typename ::std::enable_if<
158  TIdx >= N, bool>::type * = nullptr>
159  void setOperators(Ops op)
160  {
161  return;
162  }
163 
164  template<int TIdx = 0, typename ::std::enable_if<((TIdx == 0) && (TIdx != N))>::type * = nullptr>
165  void getOperators(::std::vector<SumBlockOperator> &v)
166  {
167  v.clear();
168  v.push_back(this->template p<0>()[0]);
169 
170  return getOperators<TIdx+1>(v);
171  }
172 
173  template<int TIdx = 0, typename ::std::enable_if<((TIdx > 0) && (TIdx < N))>::type * = nullptr>
174  void getOperators(::std::vector<SumBlockOperator> &v)
175  {
176  v.push_back(this->template p<0>()[TIdx]);
177 
178  return getOperators<TIdx+1>(v);
179  }
180 
181  template<int TIdx = 0, typename ::std::enable_if<((TIdx < 0) || (TIdx >= N))>::type * = nullptr>
182  void getOperators(::std::vector<SumBlockOperator> &v)
183  {
184  return;
185  }
186 
187  TType &
188  zero()
189  {
190  return this->template p<1>();
191  }
192 
193 #ifdef LS_USE_GINAC
194 
195  const ::std::array<GiNaC::ex, Base::kIns> &inputSymbols()
196  {
197  if (!this->isInitInput_) {
198  for (int i = 0; i < Base::kIns; i++) {
199  if (ls::aux::TemplateTraits::BinaryOperators::parseMatrixLike<TType>::value) {
200  GiNaC::lst input;
201  for (int ii = 0; ii <
202  ls::aux::TemplateTraits::BinaryOperators::parseMatrixLike<TType>::rows; ii++) {
203  GiNaC::lst row;
204  for (int jj = 0; jj <
205  ls::aux::TemplateTraits::BinaryOperators::parseMatrixLike<TType>::cols; jj++) {
206  GiNaC::symbol entry{
207  "blk" + ::std::to_string(this->id) + "_i_" + ::std::to_string(i) +
208  "_r_" + ::std::to_string(ii) + "_c_" + ::std::to_string(jj),
209  "\\text{BLK}^{i, " + ::std::to_string(i) + ", " + ::std::to_string(ii) +
210  ", " + ::std::to_string(jj) + "}_{" + ::std::to_string(this->id) + "}"};
211 
212  row.append(entry);
213  }
214  input.append(row);
215  }
216 
217  this->inputSymbols_[i] = GiNaC::lst_to_matrix(input);
218  } else {
219  this->inputSymbols_[i] = GiNaC::symbol{"blk" + ::std::to_string(this->id) + "_i_" + ::std::to_string(i),
220  "\\text{BLK}^{i, " + ::std::to_string(i) + "}_{" +
221  ::std::to_string(this->id) +
222  "}"};
223  }
224  }
225 
226  this->isInitInput_ = true;
227  }
228 
229  return this->inputSymbols_;
230  }
231 
232  const ::std::array<GiNaC::ex, Base::kOuts> &outputSymbols()
233  {
234  if (!this->isInitOutput_) {
235  for (int i = 0; i < Base::kOuts; i++) {
236  if (ls::aux::TemplateTraits::BinaryOperators::parseMatrixLike<TType>::value) {
237  GiNaC::lst output;
238  for (int ii = 0; ii <
239  ls::aux::TemplateTraits::BinaryOperators::parseMatrixLike<TType>::rows; ii++) {
240  GiNaC::lst row;
241  for (int jj = 0; jj <
242  ls::aux::TemplateTraits::BinaryOperators::parseMatrixLike<TType>::cols; jj++) {
243  GiNaC::symbol entry{
244  "blk" + ::std::to_string(this->id) + "_o_" + ::std::to_string(i) +
245  "_r_" + ::std::to_string(ii) + "_c_" + ::std::to_string(jj),
246  "\\text{BLK}^{i, " + ::std::to_string(i) + ", " + ::std::to_string(ii) +
247  ", " + ::std::to_string(jj) + "}_{" + ::std::to_string(this->id) + "}"};
248 
249  row.append(entry);
250  }
251  output.append(row);
252  }
253 
254  this->outputSymbols_[i] = GiNaC::lst_to_matrix(output);
255  } else {
256  this->outputSymbols_[i] = GiNaC::symbol{"blk" + ::std::to_string(this->id) + "_o_" + ::std::to_string(i),
257  "\\text{BLK}^{i, " + ::std::to_string(i) + "}_{" +
258  ::std::to_string(this->id) +
259  "}"};
260  }
261  }
262 
263  this->isInitOutput_ = true;
264  }
265 
266  return this->outputSymbols_;
267  }
268 
269  const ::std::array<GiNaC::ex, 1> &parameterSymbols()
270  {
271  if (!this->isInitParameter_) {
272 
273  int i = 0;
274 
275  if (ls::aux::TemplateTraits::BinaryOperators::parseMatrixLike<TType>::value) {
276  GiNaC::lst output;
277  for (int ii = 0;
278  ii < ls::aux::TemplateTraits::BinaryOperators::parseMatrixLike<TType>::rows; ii++) {
279  GiNaC::lst row;
280  for (int jj = 0; jj <
281  ls::aux::TemplateTraits::BinaryOperators::parseMatrixLike<TType>::cols; jj++) {
282  GiNaC::symbol entry{
283  "blk" + ::std::to_string(this->id) + "_p_" + ::std::to_string(i) + "_r_" +
284  ::std::to_string(ii) + "_c_" + ::std::to_string(jj),
285  "\\text{BLK}^{i, " + ::std::to_string(i) + ", " + ::std::to_string(ii) +
286  ", " + ::std::to_string(jj) + "}_{" + ::std::to_string(this->id) + "}"};
287 
288  row.append(entry);
289  }
290  output.append(row);
291  }
292 
293  this->parameterSymbols_[i] = GiNaC::lst_to_matrix(output);
294  } else {
295  this->parameterSymbols_[i] = GiNaC::symbol{"blk" + ::std::to_string(this->id) + "_p_" + ::std::to_string(i),
296  "\\text{BLK}^{i, " + ::std::to_string(i) + "}_{" +
297  ::std::to_string(this->id) +
298  "}"};
299  }
300 
301  Base::isInitParameter_ = true;
302  }
303 
304  return this->parameterSymbols_;
305  }
306 
307 #endif
308 
309  protected:
310 #ifdef LS_USE_GINAC
311  ::std::array<GiNaC::ex, 1> parameterSymbols_;
312 #endif
313 
314  void bindEquation()
315  {
316  this->equation = ::std::bind(
317  &SumBlock<TType, N>::triggerFunction,
318  this,
319  ::std::placeholders::_1);
320 
321 #ifdef LS_USE_GINAC
322  GiNaC::function_options fops("blkf" + ::std::to_string(this->id) + "__", this->blkFunc_NPARAMS);
323  ls::blocks::symbolicEvalFunctionMap[this->id] = [&](const ::std::vector<GiNaC::ex> &exvec) -> GiNaC::ex {
324  // GiNaC::ex res = this->parameterSymbols()[0];
325  GiNaC::ex res = 0;
326  int i = 0;
327 
328  for (auto & ex : exvec) {
329  res += SumBlockOperatorHelper::interpret(this->template p<0>()[i]) * ex;
330 
331  i++;
332  }
333 
334  return res;
335  };
336 
337  fops.eval_func(ls::blocks::symbolicEval);
338 
339 
340  this->serial = GiNaC::function::register_new(
341  fops
342  );
343 #endif
344  }
345 
346  void triggerFunction(Base &b)
347  {
348  b.template o<0>().object = zero();
349  sum(b.template o<0>().object);
350  b.template o<0>().propagate();
351  }
352 
353  template<unsigned int TIdx = Base::kIns - 1>
354  typename ::std::enable_if<(TIdx > 0), void>::type
355  sum(TType &res)
356  {
357  res = res +
358  SumBlockOperatorHelper::interpret(
359  (this->template p<0>())[TIdx]) *
360  this->template i<TIdx>();
361  return sum<TIdx - 1>(res);
362  }
363 
364  template<unsigned int TIdx = Base::kIns - 1>
365  typename ::std::enable_if<(TIdx == 0), void>::type
366  sum(TType &res)
367  {
368  res = res +
369  SumBlockOperatorHelper::interpret((this->template p<0>())[TIdx]) * this->template i<TIdx>();
370  }
371 
372  template<unsigned int TIdx = Base::kIns - 1>
373  typename ::std::enable_if<(TIdx < 0), void>::type
374  sum(TType &res)
375  {
376  return;
377  }
378  };
379  }
380 
381  template<typename TType, unsigned int N>
382  class BlockTraits<std::SumBlock<TType, N>> {
383  public:
384  static constexpr const BlockType blockType = BlockType::SumBlock;
385  enum {
386  directFeedthrough = true
387  };
388 
389  using type = std::SumBlock<TType, N>;
390  using Base = typename type::Base;
391 
392  enum {
393  kIns = Base::kIns,
394  kOuts = Base::kOuts,
395  kPars = Base::kPars
396  };
397 
398  static const ::std::array<::std::string, kIns> inTypes;
399  static const ::std::array<::std::string, kOuts> outTypes;
400  static const ::std::array<::std::string, kPars> parTypes;
401 
402  static const ::std::array<::std::string, 2> templateTypes;
403  };
404 
405  template<typename TType, unsigned int N>
406  const ::std::array<::std::string, BlockTraits<std::SumBlock<TType, N>>::kIns> BlockTraits<std::SumBlock<TType, N>>::inTypes =
407  ls::aux::TemplateTools::create_array<BlockTraits<std::SumBlock<TType, N>>::kIns>(
408  demangle(typeid(TType).name())
409  );
410 
411  template<typename TType, unsigned int N>
412  const ::std::array<::std::string, BlockTraits<std::SumBlock<TType, N>>::kOuts> BlockTraits<std::SumBlock<TType, N>>::outTypes =
413  {demangle(typeid(TType).name())};
414 
415  template<typename TType, unsigned int N>
416  const ::std::array<::std::string, BlockTraits<std::SumBlock<TType, N>>::kPars> BlockTraits<std::SumBlock<TType, N>>::parTypes =
417  {demangle(
418  typeid(::std::array<std::SumBlockOperator, N>).name()
419  ),
420  demangle(typeid(TType).name())};
421 
422  template<typename TType, unsigned int N>
423  const ::std::array<::std::string, 2> BlockTraits<std::SumBlock<TType, N>>::templateTypes =
424  {demangle(typeid(TType).name()), "unsigned int"};
425  }
426 }
427 
428 
429 #endif //LODESTAR_SUMBLOCK_HPP
ls::blocks::BlockTraits::directFeedthrough
static constexpr const bool directFeedthrough
Whether or not the block has direct feedthrough.
Definition: BlockTraits.hpp:44
ls::blocks::std::SumBlockOperatorHelper
Definition: SumBlock.hpp:20
ls::blocks::BlockTraits
A traits object that exposes information about TBlock.
Definition: BlockTraits.hpp:37
ls::blocks::BlockTraits::inTypes
static const ::std::array<::std::string, kIns > inTypes
Input types (as strings).
Definition: BlockTraits.hpp:59
ls::blocks::BlockType
BlockType
Block type information.
Definition: BlockType.hpp:25
ls::aux::TemplateTraits::BinaryOperators::parseMatrixLike
Extracts data from matrix-like types.
Definition: TemplateTraits.hpp:114
ls
Main Lodestar code.
Definition: BilinearTransformation.hpp:12
ls::blocks::BlockTraits::outTypes
static const ::std::array<::std::string, kOuts > outTypes
Output types (as strings).
Definition: BlockTraits.hpp:61
ls::blocks::std::SumBlock
Definition: SumBlock.hpp:76
ls::blocks::BlockType::SumBlock
@ SumBlock
Summation block.
ls::blocks::BlockTraits::blockType
static constexpr const BlockType blockType
Block type.
Definition: BlockTraits.hpp:42
ls::blocks::BlockTraits::templateTypes
static const ::std::array<::std::string, 1 > templateTypes
Template parameter types (as strings).
Definition: BlockTraits.hpp:66
ls::blocks::Block
Generic base template class for all tuple-based Block instances.
Definition: Block.hpp:45
ls::blocks::BlockTraits::kIns
static const constexpr int kIns
Number of input slots.
Definition: BlockTraits.hpp:52
ls::blocks::BlockTraits::parTypes
static const ::std::array<::std::string, kPars > parTypes
Parameter types (as strings).
Definition: BlockTraits.hpp:63
ls::blocks::BlockTraits::kPars
static const constexpr int kPars
Number of parameters.
Definition: BlockTraits.hpp:56
ls::blocks::BlockTraits::kOuts
static const constexpr int kOuts
Number of output slots.
Definition: BlockTraits.hpp:54