Lodestar
An integrated real-time control package in C++
NanopbWrapper.hpp
1 //
2 // Created by Hamza El-Kebir on 2/12/22.
3 //
4 
5 #ifndef LODESTAR_NANOPBWRAPPER_HPP
6 #define LODESTAR_NANOPBWRAPPER_HPP
7 
8 #include "Lodestar/io/proto/ls.proto.herald.pb.h"
9 #include "Lodestar/io/proto/ls.proto.types.pb.h"
10 #include "Lodestar/io/proto/ls.proto.matrix.pb.h"
11 #include "Lodestar/io/proto/ls.proto.vector.pb.h"
12 #include "Lodestar/io/proto/ls.proto.array.pb.h"
13 
14 #include <pb_encode.h>
15 #include <pb_decode.h>
16 
17 #include <Lodestar/aux/AlwaysFalse.hpp>
18 #include <Eigen/Dense>
19 #include <libhydrogen/hydrogen.h>
20 
21 namespace ls {
22  namespace io {
23  template<typename T>
24  static void toCharBuffer(void *a, const T &n)
25  {
26  memcpy(a, &n, sizeof(T));
27  }
28 
29  template<typename T>
30  static void fromCharBuffer(const void *a, T &n)
31  {
32  memcpy(&n, a, sizeof(T));
33  }
34 
35  struct MsgInfo {
36  int id = -1;
37  int slot = 0;
38  unsigned long publicKey = 0;
39  bool encrypted = false;
40  ::std::uint8_t sign[64];
41  ls_proto_Type type = ls_proto_Type_unknown_t;
42  };
43 
44  template<typename TType = void>
45  struct NanopbWrapper {
46  static_assert(always_false<TType>::value, "Wrapper not defined for this type.");
47 
48  enum {
49  hasArrayType = false
50  };
51 
52  using arrayType = void;
53 
54  enum {
55  pbType = ls_proto_Type_unknown_t
56  };
57  };
58 
59  template<>
60  struct NanopbWrapper<void> {
61  enum {
62  pbType = ls_proto_Type_unknown_t
63  };
64 
65  enum {
66  hasArrayType = false
67  };
68 
69  };
70 
71  template<>
72  struct NanopbWrapper<bool> {
73  enum {
74  pbType = ls_proto_Type_bool_t
75  };
76 
77  enum {
78  hasArrayType = true
79  };
80  using arrayType = ls_proto_ArrayBool;
81 
82  static ls_proto_Herald makeHerald()
83  {
84  ls_proto_Herald herald = ls_proto_Herald_init_default;
85  herald.msgType = static_cast<ls_proto_Type>(pbType);
86 
87  return herald;
88  }
89  };
90 
91  template<>
92  struct NanopbWrapper<char> {
93  enum {
94  pbType = ls_proto_Type_char_t
95  };
96 
97  enum {
98  hasArrayType = true
99  };
100  using arrayType = ls_proto_ArrayBytes;
101 
102  static ls_proto_Herald makeHerald()
103  {
104  ls_proto_Herald herald = ls_proto_Herald_init_default;
105  herald.msgType = static_cast<ls_proto_Type>(pbType);
106 
107  return herald;
108  }
109  };
110 
111  template<>
112  struct NanopbWrapper<double> {
113  enum {
114  pbType = ls_proto_Type_double_t
115  };
116 
117  enum {
118  hasArrayType = true
119  };
120  using arrayType = ls_proto_ArrayDouble;
121 
122  static ls_proto_Herald makeHerald()
123  {
124  ls_proto_Herald herald = ls_proto_Herald_init_default;
125  herald.msgType = static_cast<ls_proto_Type>(pbType);
126 
127  return herald;
128  }
129  };
130 
131  template<>
132  struct NanopbWrapper<float> {
133  enum {
134  pbType = ls_proto_Type_float_t
135  };
136 
137  enum {
138  hasArrayType = true
139  };
140  using arrayType = ls_proto_ArrayFloat;
141 
142  static ls_proto_Herald makeHerald()
143  {
144  ls_proto_Herald herald = ls_proto_Herald_init_default;
145  herald.msgType = static_cast<ls_proto_Type>(pbType);
146 
147  return herald;
148  }
149  };
150 
151  template<>
152  struct NanopbWrapper<::std::uint32_t> {
153  enum {
154  pbType = ls_proto_Type_uint32_t
155  };
156 
157  enum {
158  hasArrayType = true
159  };
160  using arrayType = ls_proto_ArrayInt32;
161 
162  static ls_proto_Herald makeHerald()
163  {
164  ls_proto_Herald herald = ls_proto_Herald_init_default;
165  herald.msgType = static_cast<ls_proto_Type>(pbType);
166 
167  return herald;
168  }
169  };
170 
171  template<>
172  struct NanopbWrapper<::std::int32_t> {
173  enum {
174  pbType = ls_proto_Type_int32_t
175  };
176 
177  enum {
178  hasArrayType = true
179  };
180  using arrayType = ls_proto_ArrayInt32;
181 
182  static ls_proto_Herald makeHerald()
183  {
184  ls_proto_Herald herald = ls_proto_Herald_init_default;
185  herald.msgType = static_cast<ls_proto_Type>(pbType);
186 
187  return herald;
188  }
189  };
190 
191  template<>
192  struct NanopbWrapper<::std::uint64_t> {
193  enum {
194  pbType = ls_proto_Type_uint64_t
195  };
196 
197  enum {
198  hasArrayType = true
199  };
200  using arrayType = ls_proto_ArrayUInt64;
201 
202  static ls_proto_Herald makeHerald()
203  {
204  ls_proto_Herald herald = ls_proto_Herald_init_default;
205  herald.msgType = static_cast<ls_proto_Type>(pbType);
206 
207  return herald;
208  }
209  };
210 
211  template<>
212  struct NanopbWrapper<::std::int64_t> {
213  enum {
214  pbType = ls_proto_Type_int64_t
215  };
216 
217  enum {
218  hasArrayType = true
219  };
220  using arrayType = ls_proto_ArrayInt64;
221 
222  static ls_proto_Herald makeHerald()
223  {
224  ls_proto_Herald herald = ls_proto_Herald_init_default;
225  herald.msgType = static_cast<ls_proto_Type>(pbType);
226 
227  return herald;
228  }
229  };
230 
231  struct NanopbArg {
232  ::std::function<bool(pb_ostream_t *, const pb_field_t *, void *const *)> *fEncode;
233  ::std::function<bool(pb_istream_t *, const pb_field_t *, void **)> *fDecode;
234  void *data;
235  void const *dataConst;
236  };
237 
238  static bool encode_nanopb(pb_ostream_t *stream, const pb_field_t *field, void *const *arg)
239  {
240  auto mArg = (const NanopbArg *) *arg;
241 
242  return mArg->fEncode->operator()(stream, field, arg);
243  }
244 
245  static bool decode_nanopb(pb_istream_t *stream, const pb_field_t *field, void **arg)
246  {
247  auto mArg = (NanopbArg *) *arg;
248 
249  return mArg->fDecode->operator()(stream, field, arg);
250  }
251 
252  template<typename TScalar, int NRows, int NCols, int NOptions>
253  struct NanopbWrapper<Eigen::Matrix<TScalar, NRows, NCols, NOptions>> {
254  static_assert((NRows >= 0) && (NCols >= 0), "Matrix must be of fixed size.");
255  static_assert(NanopbWrapper<TScalar>::hasArrayType, "An array message must be defined for this type.");
256 
257  enum {
258  pbType = ls_proto_Type_matrix_t
259  };
260 
261  using heraldType = ls_proto_Herald;
262  using subHeraldType = ls_proto_MatrixHerald;
263  using msgType = typename NanopbWrapper<TScalar>::arrayType;
264 
265  enum {
266  hasArrayType = false
267  };
268 
269  static ls_proto_Herald makeHerald()
270  {
271  ls_proto_Herald herald = ls_proto_Herald_init_default;
272  herald.msgType = static_cast<ls_proto_Type>(pbType);
273 
274  return herald;
275  }
276 
277  static ls_proto_MatrixHerald makeSubHerald()
278  {
279  ls_proto_MatrixHerald matrixHerald = ls_proto_MatrixHerald_init_default;
280 
281  matrixHerald.type = static_cast<ls_proto_Type>(NanopbWrapper<TScalar>::pbType);
282  matrixHerald.rows = NRows;
283  matrixHerald.cols = NCols;
284  if ((NOptions & Eigen::StorageOptions::RowMajor) == Eigen::StorageOptions::RowMajor) {
285  matrixHerald.has_ordering = true;
286  matrixHerald.ordering = ls_proto_MatrixOrdering_rowMajor;
287  } else if ((NOptions & Eigen::StorageOptions::ColMajor) == Eigen::StorageOptions::ColMajor) {
288  matrixHerald.has_ordering = true;
289  matrixHerald.ordering = ls_proto_MatrixOrdering_colMajor;
290  }
291 
292  return matrixHerald;
293  }
294 
295  static auto makeEncodingMessage(const Eigen::Matrix<TScalar, NRows, NCols, NOptions> &M, NanopbArg &nArg,
296  const ls_proto_MatrixHerald &mHerald) -> typename NanopbWrapper<TScalar>::arrayType
297  {
298  typename NanopbWrapper<TScalar>::arrayType array;
299 
300  nArg.dataConst = &M;
301  static ::std::function<bool(pb_ostream_t *, const pb_field_t *, void *const *)> fEncode;
302 
303  if (mHerald.type == ls_proto_Type_double_t) {
304  array = ls_proto_ArrayDouble_init_default;
305  if (mHerald.has_ordering) {
306  if (mHerald.ordering == ls_proto_MatrixOrdering_rowMajor) {
307  fEncode = decltype(fEncode){
308  [](pb_ostream_t *stream, const pb_field_t *field, void *const *arg) -> bool {
309  auto mArg = (NanopbArg *) *arg;
310  auto M = (const Eigen::Matrix<TScalar, NRows, NCols, NOptions> *) mArg->dataConst;
311  double value;
312  for (int i = 0; i < NRows; i++) {
313  for (int j = 0; j < NCols; j++) {
314  if (!pb_encode_tag_for_field(stream, field))
315  return false;
316 
317  value = (*M)(i, j);
318 
319  if (!pb_encode_fixed64(stream, &value))
320  return false;
321  }
322  }
323 
324  return true;
325  }};
326  } else {
327  fEncode = decltype(fEncode){
328  [](pb_ostream_t *stream, const pb_field_t *field, void *const *arg) -> bool {
329  auto mArg = (NanopbArg *) *arg;
330  auto M = (const Eigen::Matrix<TScalar, NRows, NCols, NOptions> *) mArg->dataConst;
331  double value;
332  for (int j = 0; j < NCols; j++) {
333  for (int i = 0; i < NRows; i++) {
334  if (!pb_encode_tag_for_field(stream, field))
335  return false;
336 
337  value = (*M)(i, j);
338 
339  if (!pb_encode_fixed64(stream, &value))
340  return false;
341  }
342  }
343 
344  return true;
345  }};
346  }
347  } else {
348  fEncode = decltype(fEncode){
349  [](pb_ostream_t *stream, const pb_field_t *field, void *const *arg) -> bool {
350  auto mArg = (NanopbArg *) *arg;
351  auto M = (const Eigen::Matrix<TScalar, NRows, NCols, NOptions> *) mArg->dataConst;
352  double value;
353  for (int j = 0; j < NCols; j++) {
354  for (int i = 0; i < NRows; i++) {
355  if (!pb_encode_tag_for_field(stream, field))
356  return false;
357 
358  value = (*M)(i, j);
359 
360  if (!pb_encode_fixed64(stream, &value))
361  return false;
362  }
363  }
364 
365  return true;
366  }};
367  }
368  }
369 
370  if (mHerald.type == ls_proto_Type_float_t) {
371  array = ls_proto_ArrayFloat_init_default;
372  if (mHerald.has_ordering) {
373  if (mHerald.ordering == ls_proto_MatrixOrdering_rowMajor) {
374  fEncode = decltype(fEncode){
375  [](pb_ostream_t *stream, const pb_field_t *field, void *const *arg) -> bool {
376  auto mArg = (NanopbArg *) *arg;
377  auto M = (const Eigen::Matrix<TScalar, NRows, NCols, NOptions> *) mArg->dataConst;
378  float value;
379  for (int i = 0; i < NRows; i++) {
380  for (int j = 0; j < NCols; j++) {
381  if (!pb_encode_tag_for_field(stream, field))
382  return false;
383 
384  value = (*M)(i, j);
385 
386  if (!pb_encode_fixed32(stream, &value))
387  return false;
388  }
389  }
390 
391  return true;
392  }};
393  } else {
394  fEncode = decltype(fEncode){
395  [](pb_ostream_t *stream, const pb_field_t *field, void *const *arg) -> bool {
396  auto mArg = (NanopbArg *) *arg;
397  auto M = (const Eigen::Matrix<TScalar, NRows, NCols, NOptions> *) mArg->dataConst;
398  float value;
399  for (int j = 0; j < NCols; j++) {
400  for (int i = 0; i < NRows; i++) {
401  if (!pb_encode_tag_for_field(stream, field))
402  return false;
403 
404  value = (*M)(i, j);
405 
406  if (!pb_encode_fixed32(stream, &value))
407  return false;
408  }
409  }
410 
411  return true;
412  }};
413  }
414  } else {
415  fEncode = decltype(fEncode){
416  [](pb_ostream_t *stream, const pb_field_t *field, void *const *arg) -> bool {
417  auto mArg = (NanopbArg *) *arg;
418  auto M = (const Eigen::Matrix<TScalar, NRows, NCols, NOptions> *) mArg->dataConst;
419  float value;
420  for (int j = 0; j < NCols; j++) {
421  for (int i = 0; i < NRows; i++) {
422  if (!pb_encode_tag_for_field(stream, field))
423  return false;
424 
425  value = (*M)(i, j);
426 
427  if (!pb_encode_fixed32(stream, &value))
428  return false;
429  }
430  }
431 
432  return true;
433  }};
434  }
435  }
436 
437  nArg.fEncode = &fEncode;
438 
439  array.values.funcs.encode = &encode_nanopb;
440  array.values.arg = (void *) &nArg;
441  array.size = NRows * NCols;
442 
443  return array;
444  }
445 
446  struct MatrixCounter {
447  void *M;
448  int i;
449  int j;
450  };
451 
452  static auto
453  makeDecodingMessage(Eigen::Matrix<TScalar, NRows, NCols, NOptions> &M, NanopbArg &nArg, MatrixCounter &MC,
454  const ls_proto_MatrixHerald &mHerald) -> typename NanopbWrapper<TScalar>::arrayType
455  {
456  typename NanopbWrapper<TScalar>::arrayType array;
457  array.size = NRows * NCols;
458 
459 // auto MC = new MatrixCounter{};
460 // MC->M = &M;
461 // MC->i = 0;
462 // MC->j = 0;
463 
464  MC.M = &M;
465  MC.i = 0;
466  MC.j = 0;
467 
468  nArg.data = &MC;
469  static ::std::function<bool(pb_istream_t *, const pb_field_t *, void **)> fDecode;
470 
471  if (mHerald.type == ls_proto_Type_double_t) {
472  array = ls_proto_ArrayDouble_init_default;
473  if (mHerald.has_ordering) {
474  if (mHerald.ordering == ls_proto_MatrixOrdering_rowMajor) {
475  fDecode = decltype(fDecode){
476  [](pb_istream_t *stream, const pb_field_t *field, void **arg) -> bool {
477  auto MC = (MatrixCounter *) ((NanopbArg *) *arg)->data;
478  auto M = (Eigen::Matrix<TScalar, NRows, NCols, NOptions> *) MC->M;
479  double value;
480 
481  if (!pb_decode_fixed64(stream, &value))
482  return false;
483 
484  (*M)(MC->i, MC->j) = value;
485 
486  MC->j++;
487  MC->j = MC->j % NCols;
488 
489  if (MC->j == 0) {
490  MC->i++;
491  MC->i = MC->i % NRows;
492  }
493 
494  return true;
495  }};
496  } else {
497  fDecode = decltype(fDecode){
498  [](pb_istream_t *stream, const pb_field_t *field, void **arg) -> bool {
499  auto MC = (MatrixCounter *) ((NanopbArg *) *arg)->data;
500  auto M = (Eigen::Matrix<TScalar, NRows, NCols, NOptions> *) MC->M;
501  double value = 0;
502 
503  if (!pb_decode_fixed64(stream, &value))
504  return false;
505 
506  (*M)(MC->i, MC->j) = value;
507 
508  MC->i++;
509  MC->i = MC->i % NRows;
510 
511  if (MC->i == 0) {
512  MC->j++;
513  MC->j = MC->j % NCols;
514  }
515 
516  return true;
517  }};
518  }
519  } else {
520  fDecode = decltype(fDecode){
521  [](pb_istream_t *stream, const pb_field_t *field, void **arg) -> bool {
522  auto MC = (MatrixCounter *) ((NanopbArg *) *arg)->data;
523  auto M = (Eigen::Matrix<TScalar, NRows, NCols, NOptions> *) MC->M;
524  double value;
525 
526  if (!pb_decode_fixed64(stream, &value))
527  return false;
528 
529  (*M)(MC->i, MC->j) = value;
530 
531  MC->i++;
532  MC->i = MC->i % NRows;
533 
534  if (MC->i == 0) {
535  MC->j++;
536  MC->j = MC->j % NCols;
537  }
538 
539  return true;
540  }};
541  }
542  }
543 
544  if (mHerald.type == ls_proto_Type_float_t) {
545  array = ls_proto_ArrayFloat_init_default;
546  if (mHerald.has_ordering) {
547  if (mHerald.ordering == ls_proto_MatrixOrdering_rowMajor) {
548  fDecode = decltype(fDecode){
549  [](pb_istream_t *stream, const pb_field_t *field, void **arg) -> bool {
550  auto MC = (MatrixCounter *) ((NanopbArg *) *arg)->data;
551  auto M = (Eigen::Matrix<TScalar, NRows, NCols, NOptions> *) MC->M;
552  float value;
553 
554  if (!pb_decode_fixed32(stream, &value))
555  return false;
556 
557  (*M)(MC->i, MC->j) = value;
558 
559  MC->j++;
560  MC->j = MC->j % NCols;
561 
562  if (MC->j == 0) {
563  MC->i++;
564  MC->i = MC->i % NRows;
565  }
566 
567  return true;
568  }};
569  } else {
570  fDecode = decltype(fDecode){
571  [](pb_istream_t *stream, const pb_field_t *field, void **arg) -> bool {
572  auto MC = (MatrixCounter *) ((NanopbArg *) *arg)->data;
573  auto M = (Eigen::Matrix<TScalar, NRows, NCols, NOptions> *) MC->M;
574  float value;
575 
576  if (!pb_decode_fixed32(stream, &value))
577  return false;
578 
579  (*M)(MC->i, MC->j) = value;
580 
581  MC->i++;
582  MC->i = MC->i % NRows;
583 
584  if (MC->i == 0) {
585  MC->j++;
586  MC->j = MC->j % NCols;
587  }
588 
589  return true;
590  }};
591  }
592  } else {
593  fDecode = decltype(fDecode){
594  [](pb_istream_t *stream, const pb_field_t *field, void **arg) -> bool {
595  auto MC = (MatrixCounter *) ((NanopbArg *) *arg)->data;
596  auto M = (Eigen::Matrix<TScalar, NRows, NCols, NOptions> *) MC->M;
597  float value;
598 
599  if (!pb_decode_fixed32(stream, &value))
600  return false;
601 
602  (*M)(MC->i, MC->j) = value;
603 
604  MC->i++;
605  MC->i = MC->i % NRows;
606 
607  if (MC->i == 0) {
608  MC->j++;
609  MC->j = MC->j % NCols;
610  }
611 
612  return true;
613  }};
614  }
615  }
616 
617  nArg.fDecode = &fDecode;
618 
619  array.values.funcs.decode = &decode_nanopb;
620  array.values.arg = (void *) &nArg;
621 
622  return array;
623  }
624 
625  static bool
626  encode(const Eigen::Matrix<TScalar, NRows, NCols, NOptions> &M, const MsgInfo &info, pb_ostream_t &stream)
627  {
628  static auto herald = makeHerald();
629  static auto subHerald = makeSubHerald();
630  static auto msgArg = new ls::io::NanopbArg{};
631  static auto msg = makeEncodingMessage(M, *msgArg, subHerald);
632 
633  herald.blockId = info.id;
634  herald.slotId = info.slot;
635  herald.msgType = info.type;
636  if (info.encrypted) {
637  herald.has_sign = true;
638  }
639 
640  msgArg->dataConst = &M;
641 
642  if (::std::is_same<TScalar, double>::value)
643  return pb_encode_delimited(&stream, ls_proto_Herald_fields, &herald) &&
644  pb_encode_delimited(&stream, ls_proto_MatrixHerald_fields, &subHerald) &&
645  pb_encode_delimited(&stream, ls_proto_ArrayDouble_fields, &msg);
646  if (::std::is_same<TScalar, float>::value)
647  return pb_encode_delimited(&stream, ls_proto_Herald_fields, &herald) &&
648  pb_encode_delimited(&stream, ls_proto_MatrixHerald_fields, &subHerald) &&
649  pb_encode_delimited(&stream, ls_proto_ArrayFloat_fields, &msg);
650  }
651 
652  static bool decode(Eigen::Matrix<TScalar, NRows, NCols, NOptions> &M, MsgInfo &info, pb_istream_t &stream)
653  {
654  static auto heraldRecv = makeHerald();
655  static auto subHeraldRecv = makeSubHerald();
656  static auto msgArgRecv = new ls::io::NanopbArg{};
657  MatrixCounter MC;
658  static auto msgRecv = makeDecodingMessage(M, *msgArgRecv, MC, subHeraldRecv);
659 
660  auto heraldRes = pb_decode_delimited(&stream, ls_proto_Herald_fields, &heraldRecv);
661 
662  info.id = heraldRecv.blockId;
663  info.slot = heraldRecv.slotId;
664  info.type = heraldRecv.msgType;
665  if (heraldRecv.has_sign) {
666  info.encrypted = true;
667  }
668 
669  if (::std::is_same<TScalar, double>::value)
670  return heraldRes &&
671  pb_decode_delimited(&stream, ls_proto_MatrixHerald_fields, &subHeraldRecv) &&
672  pb_decode_delimited(&stream, ls_proto_ArrayDouble_fields, &msgRecv);
673  if (::std::is_same<TScalar, float>::value)
674  return heraldRes &&
675  pb_decode_delimited(&stream, ls_proto_MatrixHerald_fields, &subHeraldRecv) &&
676  pb_decode_delimited(&stream, ls_proto_ArrayFloat_fields, &msgRecv);
677  }
678 
679  enum {
680  kMessageSize = ls_proto_Herald_size + ls_proto_MatrixHerald_size + NRows * NCols * sizeof(TScalar) + 50
681  };
682  };
683 
684  template<typename TScalar, int NRows>
685  struct NanopbWrapper<Eigen::Vector<TScalar, NRows>> {
686  static_assert((NRows >= 0), "Vector must be of fixed size.");
687  static_assert(NanopbWrapper<TScalar>::hasArrayType, "An array message must be defined for this type.");
688 
689  enum {
690  pbType = ls_proto_Type_vector_t
691  };
692 
693  enum {
694  hasArrayType = false
695  };
696 
697  using heraldType = ls_proto_Herald;
698  using subHeraldType = ls_proto_VectorHerald;
699  using msgType = typename NanopbWrapper<TScalar>::arrayType;
700 
701  static ls_proto_Herald makeHerald()
702  {
703  ls_proto_Herald herald = ls_proto_Herald_init_default;
704  herald.msgType = static_cast<ls_proto_Type>(pbType);
705 
706  return herald;
707  }
708 
709  static ls_proto_VectorHerald makeSubHerald()
710  {
711  ls_proto_VectorHerald vectorHerald = ls_proto_VectorHerald_init_default;
712 
713  vectorHerald.type = static_cast<ls_proto_Type>(NanopbWrapper<TScalar>::pbType);
714  vectorHerald.size = NRows;
715  vectorHerald.has_vectorType = false;
716 
717  return vectorHerald;
718  }
719 
720  static auto makeEncodingMessage(const Eigen::Vector<TScalar, NRows> &v, NanopbArg &nArg,
721  const ls_proto_VectorHerald &mHerald) -> typename NanopbWrapper<TScalar>::arrayType
722  {
723  typename NanopbWrapper<TScalar>::arrayType array;
724 
725  nArg.dataConst = &v;
726  static ::std::function<bool(pb_ostream_t *, const pb_field_t *, void *const *)> fEncode;
727 
728  if (mHerald.type == ls_proto_Type_double_t) {
729  array = ls_proto_ArrayDouble_init_default;
730  fEncode = decltype(fEncode){
731  [](pb_ostream_t *stream, const pb_field_t *field, void *const *arg) -> bool {
732  auto mArg = (NanopbArg *) *arg;
733  auto v = (const Eigen::Vector<TScalar, NRows> *) mArg->dataConst;
734  double value;
735  for (int i = 0; i < NRows; i++) {
736  if (!pb_encode_tag_for_field(stream, field))
737  return false;
738 
739  value = (*v)[i];
740 
741  if (!pb_encode_fixed64(stream, &value))
742  return false;
743  }
744 
745  return true;
746  }};
747  }
748 
749  if (mHerald.type == ls_proto_Type_float_t) {
750  array = ls_proto_ArrayFloat_init_default;
751  fEncode = decltype(fEncode){
752  [](pb_ostream_t *stream, const pb_field_t *field, void *const *arg) -> bool {
753  auto mArg = (NanopbArg *) *arg;
754  auto v = (const Eigen::Vector<TScalar, NRows> *) mArg->dataConst;
755  float value;
756  for (int i = 0; i < NRows; i++) {
757  if (!pb_encode_tag_for_field(stream, field))
758  return false;
759 
760  value = (*v)[i];
761 
762  if (!pb_encode_fixed32(stream, &value))
763  return false;
764  }
765 
766  return true;
767  }};
768  }
769 
770  nArg.fEncode = &fEncode;
771 
772  array.values.funcs.encode = &encode_nanopb;
773  array.values.arg = (void *) &nArg;
774  array.size = NRows;
775 
776  return array;
777  }
778 
779  static ls_proto_ArrayBytes
780  makeEncryptedEncodingMessage(const Eigen::Vector<TScalar, NRows> &v, NanopbArg &nArg,
781  const ls_proto_VectorHerald &mHerald, const ls_proto_Herald &herald)
782  {
783  hydro_init();
784  ls_proto_ArrayBytes array;
785 
786  ::std::uint8_t sk[hydro_secretbox_KEYBYTES] = {0};
787 
788  toCharBuffer(sk, herald.sign.sign0);
789  toCharBuffer(sk + 8, herald.sign.sign1);
790  toCharBuffer(sk + 16, herald.sign.sign2);
791  toCharBuffer(sk + 24, herald.sign.sign3);
792 
793  nArg.dataConst = &v;
794  static ::std::function<bool(pb_ostream_t *, const pb_field_t *, void *const *)> fEncode;
795 
796  array = ls_proto_ArrayBytes_init_default;
797  fEncode = decltype(fEncode){
798  [=](pb_ostream_t *stream, const pb_field_t *field, void *const *arg) -> bool {
799  auto mArg = (NanopbArg *) *arg;
800  auto v = (const Eigen::Vector<TScalar, NRows> *) mArg->dataConst;
801  double value;
802  ::std::uint8_t cipher[NRows * sizeof(TScalar) + hydro_secretbox_HEADERBYTES] = {0};
803  ::std::uint8_t buffer[NRows * sizeof(TScalar)] = {0};
804 
805  for (size_t i = 0; i < NRows; i++)
806  toCharBuffer(buffer + i * sizeof(TScalar), (*v)[i]);
807 
808  if (!pb_encode_tag_for_field(stream, field))
809  return false;
810 
811  hydro_secretbox_encrypt(cipher, buffer, sizeof(buffer), 0, "Lodestar", sk);
812 
813  if (!pb_encode_string(stream, cipher,
814  sizeof(cipher)))
815  return false;
816 
817  return true;
818  }};
819 
820  nArg.fEncode = &fEncode;
821 
822  array.values.funcs.encode = &encode_nanopb;
823  array.values.arg = (void *) &nArg;
824  array.size = NRows * sizeof(TScalar) + hydro_secretbox_HEADERBYTES;
825 
826  return array;
827  }
828 
829  struct VectorCounter {
830  void *v;
831  int i;
832  };
833 
834  static auto makeDecodingMessage(Eigen::Vector<TScalar, NRows> &v, NanopbArg &nArg, VectorCounter &vC,
835  const ls_proto_VectorHerald &vHerald) -> typename NanopbWrapper<TScalar>::arrayType
836  {
837  typename NanopbWrapper<TScalar>::arrayType array;
838  array.size = NRows;
839 
840 // auto vC = new VectorCounter{};
841 // vC->v = &v;
842 // vC->i = 0;
843 
844  vC.v = &v;
845  vC.i = 0;
846 
847  nArg.data = &vC;
848  static ::std::function<bool(pb_istream_t *, const pb_field_t *, void **)> fDecode;
849 
850  if (vHerald.type == ls_proto_Type_double_t) {
851  array = ls_proto_ArrayDouble_init_default;
852 
853  fDecode = decltype(fDecode){[](pb_istream_t *stream, const pb_field_t *field, void **arg) -> bool {
854  auto vC = (VectorCounter *) ((NanopbArg *) *arg)->data;
855  auto v = (Eigen::Vector<TScalar, NRows> *) vC->v;
856  double value = 0;
857 
858  if (!pb_decode_fixed64(stream, &value))
859  return false;
860 
861  (*v)[vC->i] = value;
862 
863  vC->i++;
864  vC->i = vC->i % NRows;
865 
866  return true;
867  }};
868  }
869 
870  if (vHerald.type == ls_proto_Type_float_t) {
871  array = ls_proto_ArrayFloat_init_default;
872  fDecode = decltype(fDecode){[](pb_istream_t *stream, const pb_field_t *field, void **arg) -> bool {
873  auto vC = (VectorCounter *) ((NanopbArg *) *arg)->data;
874  auto v = (Eigen::Vector<TScalar, NRows> *) vC->v;
875  float value;
876 
877  if (!pb_decode_fixed32(stream, &value))
878  return false;
879 
880  (*v)[vC->i] = value;
881 
882  vC->i++;
883  vC->i = vC->i % NRows;
884 
885  return true;
886  }};
887  }
888 
889  nArg.fDecode = &fDecode;
890 
891  array.values.funcs.decode = &decode_nanopb;
892  array.values.arg = (void *) &nArg;
893 
894  return array;
895  }
896 
897  static ls_proto_ArrayBytes
898  makeEncryptedDecodingMessage(Eigen::Vector<TScalar, NRows> &v, NanopbArg &nArg, VectorCounter &vC,
899  const ls_proto_VectorHerald &vHerald, const ls_proto_Herald &herald)
900  {
901  ls_proto_ArrayBytes array;
902  array.size = NRows;
903 
904  ::std::uint8_t sk[hydro_secretbox_KEYBYTES] = {0};
905 
906  toCharBuffer(sk, herald.sign.sign0);
907  toCharBuffer(sk + 8, herald.sign.sign1);
908  toCharBuffer(sk + 16, herald.sign.sign2);
909  toCharBuffer(sk + 24, herald.sign.sign3);
910 
911 
912 // auto vC = new VectorCounter{};
913 // vC->v = &v;
914 // vC->i = 0;
915 
916  vC.v = &v;
917  vC.i = 0;
918 
919  nArg.data = &vC;
920  static ::std::function<bool(pb_istream_t *, const pb_field_t *, void **)> fDecode;
921 
922  array = ls_proto_ArrayBytes_init_default;
923 
924  fDecode = decltype(fDecode){[=](pb_istream_t *stream, const pb_field_t *field, void **arg) -> bool {
925  ::std::uint8_t buffer[NRows * sizeof(TScalar) + hydro_secretbox_HEADERBYTES] = {0};
926  ::std::uint8_t decrypted[NRows * sizeof(TScalar)] = {0};
927 
928  auto vC = (VectorCounter *) ((NanopbArg *) *arg)->data;
929  auto v = (Eigen::Vector<TScalar, NRows> *) vC->v;
930  double value = 0;
931 
932  if (stream->bytes_left > sizeof(buffer) || !pb_read(stream, buffer, sizeof(buffer)))
933  return false;
934 
935  if (hydro_secretbox_decrypt(decrypted, buffer,
936  NRows * sizeof(TScalar) + hydro_secretbox_HEADERBYTES, 0, "Lodestar",
937  sk) != 0)
938  return false;
939 
940  for (size_t i = 0; i < NRows; i++) {
941  (*v)[vC->i] = reinterpret_cast<const TScalar *>(decrypted)[i];
942 
943  vC->i++;
944  vC->i = vC->i % NRows;
945  }
946 
947  return true;
948  }};
949 
950  nArg.fDecode = &fDecode;
951 
952  array.values.funcs.decode = &decode_nanopb;
953  array.values.arg = (void *) &nArg;
954 
955  return array;
956  }
957 
958  static bool encode(const Eigen::Vector<TScalar, NRows> &v, const MsgInfo &info, pb_ostream_t &stream)
959  {
960  static auto herald = makeHerald();
961  static auto subHerald = makeSubHerald();
962  static auto msgArg = new ls::io::NanopbArg{};
963  msgArg->dataConst = &v;
964 
965  herald.blockId = info.id;
966  herald.slotId = info.slot;
967  herald.msgType = info.type;
968 
969  if (info.encrypted) {
970  static auto msg = makeEncryptedEncodingMessage(v, *msgArg, subHerald, herald);
971 
972  herald.has_sign = true;
973  herald.sign.has_sign0 = true;
974  fromCharBuffer(reinterpret_cast<const ::std::uint64_t *>(info.sign), herald.sign.sign0);
975  herald.sign.has_sign1 = true;
976  fromCharBuffer(reinterpret_cast<const ::std::uint64_t *>(info.sign) + 1, herald.sign.sign1);
977  herald.sign.has_sign2 = true;
978  fromCharBuffer(reinterpret_cast<const ::std::uint64_t *>(info.sign) + 2, herald.sign.sign2);
979  herald.sign.has_sign3 = true;
980  fromCharBuffer(reinterpret_cast<const ::std::uint64_t *>(info.sign) + 3, herald.sign.sign3);
981  herald.sign.has_sign4 = true;
982  fromCharBuffer(reinterpret_cast<const ::std::uint64_t *>(info.sign) + 4, herald.sign.sign4);
983  herald.sign.has_sign5 = true;
984  fromCharBuffer(reinterpret_cast<const ::std::uint64_t *>(info.sign) + 5, herald.sign.sign5);
985  herald.sign.has_sign6 = true;
986  fromCharBuffer(reinterpret_cast<const ::std::uint64_t *>(info.sign) + 6, herald.sign.sign6);
987  herald.sign.has_sign7 = true;
988  fromCharBuffer(reinterpret_cast<const ::std::uint64_t *>(info.sign) + 7, herald.sign.sign7);
989 
990  return pb_encode_delimited(&stream, ls_proto_Herald_fields, &herald) &&
991  pb_encode_delimited(&stream, ls_proto_VectorHerald_fields, &subHerald) &&
992  pb_encode_delimited(&stream, ls_proto_ArrayBytes_fields, &msg);
993  } else {
994  static auto msg = makeEncodingMessage(v, *msgArg, subHerald);
995 
996  if (::std::is_same<TScalar, double>::value)
997  return pb_encode_delimited(&stream, ls_proto_Herald_fields, &herald) &&
998  pb_encode_delimited(&stream, ls_proto_VectorHerald_fields, &subHerald) &&
999  pb_encode_delimited(&stream, ls_proto_ArrayDouble_fields, &msg);
1000  if (::std::is_same<TScalar, float>::value)
1001  return pb_encode_delimited(&stream, ls_proto_Herald_fields, &herald) &&
1002  pb_encode_delimited(&stream, ls_proto_VectorHerald_fields, &subHerald) &&
1003  pb_encode_delimited(&stream, ls_proto_ArrayFloat_fields, &msg);
1004  }
1005  }
1006 
1007  static bool decode(Eigen::Vector<TScalar, NRows> &v, MsgInfo &info, pb_istream_t &stream)
1008  {
1009  static auto heraldRecv = makeHerald();
1010  static auto subHeraldRecv = makeSubHerald();
1011  static auto msgArgRecv = new ls::io::NanopbArg{};
1012  VectorCounter vc;
1013 
1014  auto heraldRes = pb_decode_delimited(&stream, ls_proto_Herald_fields, &heraldRecv);
1015 
1016  info.id = heraldRecv.blockId;
1017  info.slot = heraldRecv.slotId;
1018  info.type = heraldRecv.msgType;
1019  if (heraldRecv.has_sign) {
1020  info.encrypted = true;
1021  toCharBuffer(reinterpret_cast<::std::uint64_t *>(info.sign), heraldRecv.sign.sign0);
1022  toCharBuffer(reinterpret_cast<::std::uint64_t *>(info.sign) + 1, heraldRecv.sign.sign1);
1023  toCharBuffer(reinterpret_cast<::std::uint64_t *>(info.sign) + 2, heraldRecv.sign.sign2);
1024  toCharBuffer(reinterpret_cast<::std::uint64_t *>(info.sign) + 3, heraldRecv.sign.sign3);
1025  toCharBuffer(reinterpret_cast<::std::uint64_t *>(info.sign) + 4, heraldRecv.sign.sign4);
1026  toCharBuffer(reinterpret_cast<::std::uint64_t *>(info.sign) + 5, heraldRecv.sign.sign5);
1027  toCharBuffer(reinterpret_cast<::std::uint64_t *>(info.sign) + 6, heraldRecv.sign.sign6);
1028  toCharBuffer(reinterpret_cast<::std::uint64_t *>(info.sign) + 7, heraldRecv.sign.sign7);
1029 
1030  static auto msgRecv = makeEncryptedDecodingMessage(v, *msgArgRecv, vc, subHeraldRecv, heraldRecv);
1031 
1032  return heraldRes &&
1033  pb_decode_delimited(&stream, ls_proto_VectorHerald_fields, &subHeraldRecv) &&
1034  pb_decode_delimited(&stream, ls_proto_ArrayBytes_fields, &msgRecv);
1035  } else {
1036  static auto msgRecv = makeDecodingMessage(v, *msgArgRecv, vc, subHeraldRecv);
1037 
1038  if (::std::is_same<TScalar, double>::value)
1039  return heraldRes &&
1040  pb_decode_delimited(&stream, ls_proto_VectorHerald_fields, &subHeraldRecv) &&
1041  pb_decode_delimited(&stream, ls_proto_ArrayDouble_fields, &msgRecv);
1042  if (::std::is_same<TScalar, float>::value)
1043  return heraldRes &&
1044  pb_decode_delimited(&stream, ls_proto_VectorHerald_fields, &subHeraldRecv) &&
1045  pb_decode_delimited(&stream, ls_proto_ArrayFloat_fields, &msgRecv);
1046  }
1047  }
1048 
1049  enum {
1050  kMessageSize = ls_proto_Herald_size + ls_proto_VectorHerald_size + NRows * sizeof(TScalar) + 50
1051  };
1052  };
1053  }
1054 }
1055 
1056 
1057 #endif //LODESTAR_NANOPBWRAPPER_HPP
ls::io::NanopbArg
Definition: NanopbWrapper.hpp:231
always_false
Definition: AlwaysFalse.hpp:11
ls
Main Lodestar code.
Definition: BilinearTransformation.hpp:12
ls::io::MsgInfo
Definition: NanopbWrapper.hpp:35
ls::io::NanopbWrapper
Definition: NanopbWrapper.hpp:45