array.hpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #ifndef UAVCAN_MARSHAL_ARRAY_HPP_INCLUDED
00006 #define UAVCAN_MARSHAL_ARRAY_HPP_INCLUDED
00007 
00008 #include <cassert>
00009 #include <cstdio>
00010 #include <cstring>
00011 #include <cmath>
00012 #include <uavcan/error.hpp>
00013 #include <uavcan/util/bitset.hpp>
00014 #include <uavcan/util/templates.hpp>
00015 #include <uavcan/build_config.hpp>
00016 #include <uavcan/marshal/type_util.hpp>
00017 #include <uavcan/marshal/integer_spec.hpp>
00018 #include <uavcan/std.hpp>
00019 
00020 #ifndef UAVCAN_CPP_VERSION
00021 # error UAVCAN_CPP_VERSION
00022 #endif
00023 
00024 #ifndef UAVCAN_EXCEPTIONS
00025 # error UAVCAN_EXCEPTIONS
00026 #endif
00027 
00028 #if UAVCAN_EXCEPTIONS
00029 # include <stdexcept>
00030 #endif
00031 
00032 namespace uavcan
00033 {
00034 
00035 enum ArrayMode { ArrayModeStatic, ArrayModeDynamic };
00036 
00037 /**
00038  * Properties of a square matrix; assuming row-major representation.
00039  */
00040 template <unsigned NumElements_>
00041 struct SquareMatrixTraits
00042 {
00043     enum { NumElements = NumElements_ };
00044 
00045     enum { NumRowsCols = CompileTimeIntSqrt<NumElements>::Result };
00046 
00047     enum { NumElementsInTriangle = ((1 + NumRowsCols) * NumRowsCols) / 2 };
00048 
00049     static inline bool isIndexOnDiagonal(unsigned index) { return (index / NumRowsCols) == (index % NumRowsCols); }
00050 
00051     static inline int computeElementIndexAtRowCol(int row, int col) { return row * NumRowsCols + col; }
00052 };
00053 
00054 /**
00055  * This class can be used to detect properties of square matrices.
00056  * Element iterator is a random access forward constant iterator.
00057  */
00058 template <typename ElementIterator, unsigned NumElements>
00059 class SquareMatrixAnalyzer : public SquareMatrixTraits<NumElements>
00060 {
00061     typedef SquareMatrixTraits<NumElements>  Traits ;
00062 
00063     const ElementIterator first_;
00064 
00065 public:
00066     enum PackingMode
00067     {
00068         PackingModeEmpty,
00069         PackingModeScalar,
00070         PackingModeDiagonal,
00071         PackingModeSymmetric,
00072         PackingModeFull
00073     };
00074 
00075     SquareMatrixAnalyzer(ElementIterator first_element_iterator)
00076         : first_(first_element_iterator)
00077     {
00078         StaticAssert<(NumElements > 0)>::check();
00079     }
00080 
00081     ElementIterator accessElementAtRowCol(int row, int col) const
00082     {
00083         return first_ + Traits::computeElementIndexAtRowCol(row, col);
00084     }
00085 
00086     bool areAllElementsNan() const
00087     {
00088         unsigned index = 0;
00089         for (ElementIterator it = first_; index < NumElements; ++it, ++index)
00090         {
00091             if (!isNaN(*it))
00092             {
00093                 return false;
00094             }
00095         }
00096         return true;
00097     }
00098 
00099     bool isScalar() const
00100     {
00101         unsigned index = 0;
00102         for (ElementIterator it = first_; index < NumElements; ++it, ++index)
00103         {
00104             if (!Traits::isIndexOnDiagonal(index) && !isCloseToZero(*it))
00105             {
00106                 return false;
00107             }
00108             if (Traits::isIndexOnDiagonal(index) && !areClose(*it, *first_))
00109             {
00110                 return false;
00111             }
00112         }
00113         return true;
00114     }
00115 
00116     bool isDiagonal() const
00117     {
00118         unsigned index = 0;
00119         for (ElementIterator it = first_; index < NumElements; ++it, ++index)
00120         {
00121             if (!Traits::isIndexOnDiagonal(index) && !isCloseToZero(*it))
00122             {
00123                 return false;
00124             }
00125         }
00126         return true;
00127     }
00128 
00129     bool isSymmetric() const
00130     {
00131         for (int i = 0; i < Traits::NumRowsCols; ++i)
00132         {
00133             for (int k = 0; k < Traits::NumRowsCols; ++k)
00134             {
00135                 // On diagonal comparison is pointless
00136                 if ((i != k) &&
00137                     !areClose(*accessElementAtRowCol(i, k),
00138                               *accessElementAtRowCol(k, i)))
00139                 {
00140                     return false;
00141                 }
00142             }
00143         }
00144         return true;
00145     }
00146 
00147     PackingMode detectOptimalPackingMode() const
00148     {
00149         if (areAllElementsNan())
00150         {
00151             return PackingModeEmpty;
00152         }
00153         if (isScalar())
00154         {
00155             return PackingModeScalar;
00156         }
00157         if (isDiagonal())
00158         {
00159             return PackingModeDiagonal;
00160         }
00161         if (isSymmetric())
00162         {
00163             return PackingModeSymmetric;
00164         }
00165         return PackingModeFull;
00166     }
00167 };
00168 
00169 
00170 template <unsigned Size>
00171 class UAVCAN_EXPORT StaticArrayBase
00172 {
00173 protected:
00174     typedef IntegerSpec<IntegerBitLen<Size>::Result, SignednessUnsigned, CastModeSaturate> RawEncodedSizeType;
00175 
00176 public:
00177     enum { SizeBitLen = 0 };
00178 
00179     typedef typename StorageType<IntegerSpec<IntegerBitLen<EnumMax<Size, 2>::Result>::Result,
00180                                              SignednessUnsigned, CastModeSaturate> >::Type SizeType;
00181 
00182     SizeType size()     const { return SizeType(Size); }
00183     SizeType capacity() const { return SizeType(Size); }
00184 
00185 protected:
00186     StaticArrayBase() { }
00187     ~StaticArrayBase() { }
00188 
00189     SizeType validateRange(SizeType pos) const
00190     {
00191         if (pos < SizeType(Size))
00192         {
00193             return pos;
00194         }
00195 #if UAVCAN_EXCEPTIONS
00196         throw std::out_of_range("uavcan::Array");
00197 #else
00198         UAVCAN_ASSERT(0);
00199         return SizeType(Size - 1U);  // Ha ha
00200 #endif
00201     }
00202 };
00203 
00204 
00205 template <unsigned MaxSize>
00206 class UAVCAN_EXPORT DynamicArrayBase
00207 {
00208 protected:
00209     typedef IntegerSpec<IntegerBitLen<MaxSize>::Result, SignednessUnsigned, CastModeSaturate> RawEncodedSizeType;
00210 public:
00211     typedef typename StorageType<IntegerSpec<IntegerBitLen<EnumMax<MaxSize, 2>::Result>::Result,
00212                                              SignednessUnsigned, CastModeSaturate> >::Type SizeType;
00213 
00214 private:
00215     SizeType size_;
00216 
00217 protected:
00218     DynamicArrayBase() : size_(0) { }
00219     ~DynamicArrayBase() { }
00220 
00221     SizeType validateRange(SizeType pos) const
00222     {
00223         if (pos < size_)
00224         {
00225             return pos;
00226         }
00227 #if UAVCAN_EXCEPTIONS
00228         throw std::out_of_range("uavcan::Array");
00229 #else
00230         UAVCAN_ASSERT(0);
00231         return SizeType((size_ == 0U) ? 0U : (size_ - 1U));
00232 #endif
00233     }
00234 
00235     void grow()
00236     {
00237         if (size_ >= MaxSize)
00238         {
00239             (void)validateRange(MaxSize);  // Will throw, UAVCAN_ASSERT() or do nothing
00240         }
00241         else
00242         {
00243             size_++;
00244         }
00245     }
00246 
00247     void shrink()
00248     {
00249         if (size_ > 0)
00250         {
00251             size_--;
00252         }
00253     }
00254 
00255 public:
00256     enum { SizeBitLen = RawEncodedSizeType::BitLen };
00257 
00258     SizeType size() const
00259     {
00260         UAVCAN_ASSERT(size_ ? ((size_ - 1u) <= (MaxSize - 1u)) : 1); // -Werror=type-limits
00261         return size_;
00262     }
00263 
00264     SizeType capacity() const { return MaxSize; }
00265 
00266     void clear() { size_ = 0; }
00267 };
00268 
00269 /**
00270  * Common functionality for both static and dynamic arrays.
00271  * Static arrays are of fixed size; methods that can alter the size (e.g. push_back() and such) will fail to compile.
00272  * Dynamic arrays contain a fixed-size buffer (it's size is enough to fit maximum number of elements) plus the
00273  * currently allocated number of elements.
00274  */
00275 template <typename T, ArrayMode ArrayMode, unsigned MaxSize>
00276 class UAVCAN_EXPORT ArrayImpl : public Select<ArrayMode == ArrayModeDynamic,
00277                                               DynamicArrayBase<MaxSize>, StaticArrayBase<MaxSize> >::Result
00278 {
00279     typedef ArrayImpl<T, ArrayMode, MaxSize> SelfType;
00280     typedef typename Select<ArrayMode == ArrayModeDynamic,
00281                             DynamicArrayBase<MaxSize>, StaticArrayBase<MaxSize> >::Result Base;
00282 
00283 public:
00284     enum
00285     {
00286         /// True if the array contents can be interpreted as a 8-bit string (ASCII or UTF8).
00287         IsStringLike = IsIntegerSpec<T>::Result && (T::MaxBitLen == 8 || T::MaxBitLen == 7) &&
00288                        (ArrayMode == ArrayModeDynamic)
00289     };
00290 
00291 private:
00292     typedef typename StorageType<T>::Type BufferType[MaxSize + (IsStringLike ? 1 : 0)];
00293     BufferType data_;
00294 
00295     template <typename U>
00296     typename EnableIf<sizeof(U(0) >= U())>::Type initialize(int)
00297     {
00298         if (ArrayMode != ArrayModeDynamic)
00299         {
00300             ::uavcan::fill(data_, data_ + MaxSize, U());
00301         }
00302     }
00303     template <typename> void initialize(...) { }
00304 
00305 protected:
00306     ~ArrayImpl() { }
00307 
00308 public:
00309     typedef typename StorageType<T>::Type ValueType;
00310     typedef typename Base::SizeType SizeType;
00311 
00312     using Base::size;
00313     using Base::capacity;
00314 
00315     ArrayImpl() { initialize<ValueType>(0); }
00316 
00317     /**
00318      * Returns zero-terminated string, same as std::string::c_str().
00319      * This method will compile only if the array can be interpreted as 8-bit string (ASCII of UTF8).
00320      */
00321     const char* c_str() const
00322     {
00323         StaticAssert<IsStringLike>::check();
00324         UAVCAN_ASSERT(size() < (MaxSize + 1));
00325         const_cast<BufferType&>(data_)[size()] = 0;  // Ad-hoc string termination
00326         return reinterpret_cast<const char*>(data_);
00327     }
00328 
00329     /**
00330      * Range-checking subscript.
00331      * If the index is out of range:
00332      * - if exceptions are enabled, std::out_of_range will be thrown.
00333      * - if exceptions are disabled and UAVCAN_ASSERT() is enabled, execution will be aborted.
00334      * - if exceptions are disabled and UAVCAN_ASSERT() is disabled, index will be constrained to
00335      *   the closest valid value.
00336      */
00337     ValueType& at(SizeType pos)             { return data_[Base::validateRange(pos)]; }
00338     const ValueType& at(SizeType pos) const { return data_[Base::validateRange(pos)]; }
00339 
00340     /**
00341      * Range-checking subscript. @ref at()
00342      */
00343     ValueType& operator[](SizeType pos)             { return at(pos); }
00344     const ValueType& operator[](SizeType pos) const { return at(pos); }
00345 
00346     /**
00347      * Standard container methods. Applicable to both dynamic and static arrays.
00348      */
00349     ValueType* begin()             { return data_; }
00350     const ValueType* begin() const { return data_; }
00351     ValueType* end()               { return data_ + Base::size(); }
00352     const ValueType* end()   const { return data_ + Base::size(); }
00353     ValueType& front()             { return at(0U); }
00354     const ValueType& front() const { return at(0U); }
00355     ValueType& back()              { return at((Base::size() == 0U) ? 0U : SizeType(Base::size() - 1U)); }
00356     const ValueType& back()  const { return at((Base::size() == 0U) ? 0U : SizeType(Base::size() - 1U)); }
00357 
00358     /**
00359      * Performs standard lexicographical compare of the elements.
00360      */
00361     template <typename R>
00362     bool operator<(const R& rhs) const
00363     {
00364         return ::uavcan::lexicographical_compare(begin(), end(), rhs.begin(), rhs.end());
00365     }
00366 
00367     /**
00368      * Aliases for compatibility with standard containers.
00369      */
00370     typedef ValueType* iterator;
00371     typedef const ValueType* const_iterator;
00372 };
00373 
00374 /**
00375  * Memory-efficient specialization for bit arrays (each element maps to a single bit rather than single byte).
00376  * This should be compatible with std::bitset.
00377  */
00378 template <unsigned MaxSize, ArrayMode ArrayMode, CastMode CastMode>
00379 class UAVCAN_EXPORT ArrayImpl<IntegerSpec<1, SignednessUnsigned, CastMode>, ArrayMode, MaxSize>
00380     : public BitSet<MaxSize>
00381     , public Select<ArrayMode == ArrayModeDynamic, DynamicArrayBase<MaxSize>, StaticArrayBase<MaxSize> >::Result
00382 {
00383     typedef typename Select<ArrayMode == ArrayModeDynamic,
00384                             DynamicArrayBase<MaxSize>, StaticArrayBase<MaxSize> >::Result ArrayBase;
00385 
00386 public:
00387     enum { IsStringLike = 0 };
00388 
00389     typedef typename BitSet<MaxSize>::Reference Reference;
00390     typedef typename ArrayBase::SizeType SizeType;
00391 
00392     using ArrayBase::size;
00393     using ArrayBase::capacity;
00394 
00395     /**
00396      * Range-checking subscript. Throws if enabled; UAVCAN_ASSERT() if enabled; else constraints the position.
00397      */
00398     Reference at(SizeType pos)  { return BitSet<MaxSize>::operator[](ArrayBase::validateRange(pos)); }
00399     bool at(SizeType pos) const { return BitSet<MaxSize>::operator[](ArrayBase::validateRange(pos)); }
00400 
00401     /**
00402      * @ref at()
00403      */
00404     Reference operator[](SizeType pos)  { return at(pos); }
00405     bool operator[](SizeType pos) const { return at(pos); }
00406 };
00407 
00408 /**
00409  * Zero length arrays are not allowed
00410  */
00411 template <typename T, ArrayMode ArrayMode> class ArrayImpl<T, ArrayMode, 0>;
00412 
00413 /**
00414  * Generic array implementation.
00415  * This class is compatible with most standard library functions operating on containers (e.g. std::sort(),
00416  * std::lexicographical_compare(), etc.).
00417  * No dynamic memory is used.
00418  * All functions that can modify the array or access elements are range checking. If the range error occurs:
00419  * - if exceptions are enabled, std::out_of_range will be thrown;
00420  * - if UAVCAN_ASSERT() is enabled, program will be terminated on UAVCAN_ASSERT(0);
00421  * - otherwise the index value will be constrained to the closest valid value.
00422  */
00423 template <typename T, ArrayMode ArrayMode, unsigned MaxSize_>
00424 class UAVCAN_EXPORT Array : public ArrayImpl<T, ArrayMode, MaxSize_>
00425 {
00426     typedef ArrayImpl<T, ArrayMode, MaxSize_>  Base;
00427     typedef Array<T, ArrayMode, MaxSize_> SelfType;
00428 
00429     static bool isOptimizedTailArray(TailArrayOptimizationMode tao_mode)
00430     {
00431         return (T::MinBitLen >= 8) && (tao_mode == TailArrayOptEnabled);
00432     }
00433 
00434     int encodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, FalseType) const  /// Static
00435     {
00436         UAVCAN_ASSERT(size() > 0);
00437         for (SizeType i = 0; i < size(); i++)
00438         {
00439             const bool last_item = i == (size() - 1);
00440             const int res = RawValueType::encode(Base::at(i), codec, last_item ? tao_mode : TailArrayOptDisabled);
00441             if (res <= 0)
00442             {
00443                 return res;
00444             }
00445         }
00446         return 1;
00447     }
00448 
00449     int encodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, TrueType) const   /// Dynamic
00450     {
00451         StaticAssert<IsDynamic>::check();
00452         const bool self_tao_enabled = isOptimizedTailArray(tao_mode);
00453         if (!self_tao_enabled)
00454         {
00455             const int res_sz =
00456                 Base::RawEncodedSizeType::encode(typename StorageType<typename Base::RawEncodedSizeType>::Type(size()),
00457                                                  codec, TailArrayOptDisabled);
00458             if (res_sz <= 0)
00459             {
00460                 return res_sz;
00461             }
00462         }
00463         if (size() == 0)
00464         {
00465             return 1;
00466         }
00467         return encodeImpl(codec, self_tao_enabled ? TailArrayOptDisabled : tao_mode, FalseType());
00468     }
00469 
00470     int decodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, FalseType)  /// Static
00471     {
00472         UAVCAN_ASSERT(size() > 0);
00473         for (SizeType i = 0; i < size(); i++)
00474         {
00475             const bool last_item = i == (size() - 1);
00476             ValueType value = ValueType();                          // TODO: avoid extra copy
00477             const int res = RawValueType::decode(value, codec, last_item ? tao_mode : TailArrayOptDisabled);
00478             if (res <= 0)
00479             {
00480                 return res;
00481             }
00482             Base::at(i) = value;
00483         }
00484         return 1;
00485     }
00486 
00487 #if __GNUC__
00488 # pragma GCC diagnostic push
00489 # pragma GCC diagnostic ignored "-Wtype-limits"
00490 #endif
00491     int decodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, TrueType)   /// Dynamic
00492     {
00493         StaticAssert<IsDynamic>::check();
00494         Base::clear();
00495         if (isOptimizedTailArray(tao_mode))
00496         {
00497             while (true)
00498             {
00499                 ValueType value = ValueType();
00500                 const int res = RawValueType::decode(value, codec, TailArrayOptDisabled);
00501                 if (res < 0)
00502                 {
00503                     return res;
00504                 }
00505                 if (res == 0)             // Success: End of stream reached (even if zero items were read)
00506                 {
00507                     return 1;
00508                 }
00509                 if (size() == MaxSize_)   // Error: Max array length reached, but the end of stream is not
00510                 {
00511                     return -ErrInvalidMarshalData;
00512                 }
00513                 push_back(value);
00514             }
00515         }
00516         else
00517         {
00518             typename StorageType<typename Base::RawEncodedSizeType>::Type sz = 0;
00519             const int res_sz = Base::RawEncodedSizeType::decode(sz, codec, TailArrayOptDisabled);
00520             if (res_sz <= 0)
00521             {
00522                 return res_sz;
00523             }
00524             // coverity[result_independent_of_operands]
00525             if (static_cast<unsigned>(sz) > MaxSize_)   // False 'type-limits' warning occurs here
00526             {
00527                 return -ErrInvalidMarshalData;
00528             }
00529             resize(sz);
00530             if (sz == 0)
00531             {
00532                 return 1;
00533             }
00534             return decodeImpl(codec, tao_mode, FalseType());
00535         }
00536         UAVCAN_ASSERT(0); // Unreachable
00537         return -ErrLogic;
00538     }
00539 #if __GNUC__
00540 # pragma GCC diagnostic pop
00541 #endif
00542 
00543     template <typename InputIter>
00544     void packSquareMatrixImpl(const InputIter src_row_major)
00545     {
00546         StaticAssert<IsDynamic>::check();
00547 
00548         this->clear();
00549 
00550         typedef SquareMatrixAnalyzer<InputIter, MaxSize> Analyzer;
00551         const Analyzer analyzer(src_row_major);
00552 
00553         switch (analyzer.detectOptimalPackingMode())
00554         {
00555         case Analyzer::PackingModeEmpty:
00556         {
00557             break; // Nothing to insert
00558         }
00559         case Analyzer::PackingModeScalar:
00560         {
00561             this->push_back(ValueType(*src_row_major));
00562             break;
00563         }
00564         case Analyzer::PackingModeDiagonal:
00565         {
00566             for (int i = 0; i < Analyzer::NumRowsCols; i++)
00567             {
00568                 this->push_back(ValueType(*analyzer.accessElementAtRowCol(i, i)));
00569             }
00570             break;
00571         }
00572         case Analyzer::PackingModeSymmetric:
00573         {
00574             for (int row = 0; row < Analyzer::NumRowsCols; row++)
00575             {
00576                 for (int col = row; col < Analyzer::NumRowsCols; col++)
00577                 {
00578                     this->push_back(ValueType(*analyzer.accessElementAtRowCol(row, col)));
00579                 }
00580             }
00581             UAVCAN_ASSERT(this->size() == Analyzer::NumElementsInTriangle);
00582             break;
00583         }
00584         case Analyzer::PackingModeFull:
00585         {
00586             InputIter it = src_row_major;
00587             for (unsigned index = 0; index < MaxSize; index++, it++)
00588             {
00589                 this->push_back(ValueType(*it));
00590             }
00591             break;
00592         }
00593         default:
00594         {
00595             UAVCAN_ASSERT(0);
00596             break;
00597         }
00598         }
00599     }
00600 
00601     template <typename ScalarType, typename OutputIter>
00602     void unpackSquareMatrixImpl(const OutputIter dst_row_major) const
00603     {
00604         StaticAssert<IsDynamic>::check();
00605         typedef SquareMatrixTraits<MaxSize> Traits;
00606 
00607         if (this->size() == Traits::NumRowsCols || this->size() == 1)   // Scalar or diagonal
00608         {
00609             OutputIter it = dst_row_major;
00610             for (unsigned index = 0; index < MaxSize; index++)
00611             {
00612                 if (Traits::isIndexOnDiagonal(index))
00613                 {
00614                     const SizeType source_index = SizeType((this->size() == 1) ? 0 : (index / Traits::NumRowsCols));
00615                     *it++ = ScalarType(this->at(source_index));
00616                 }
00617                 else
00618                 {
00619                     *it++ = ScalarType(0);
00620                 }
00621             }
00622         }
00623         else if (this->size() == Traits::NumElementsInTriangle)         // Symmetric
00624         {
00625             OutputIter it = dst_row_major;
00626             SizeType source_index = 0;
00627             for (int row = 0; row < Traits::NumRowsCols; row++)
00628             {
00629                 for (int col = 0; col < Traits::NumRowsCols; col++)
00630                 {
00631                     if (col >= row)     // Diagonal or upper-right triangle
00632                     {
00633                         *it++ = ScalarType(this->at(source_index));
00634                         source_index++;
00635                     }
00636                     else                // Lower-left triangle
00637                     {
00638                         // Transposing one element, argument swapping is intentional
00639                         // coverity[swapped_arguments]
00640                         *it++ = *(dst_row_major + Traits::computeElementIndexAtRowCol(col, row));
00641                     }
00642                 }
00643             }
00644             UAVCAN_ASSERT(source_index == Traits::NumElementsInTriangle);
00645         }
00646         else if (this->size() == MaxSize)                               // Full - no packing whatsoever
00647         {
00648             OutputIter it = dst_row_major;
00649             for (SizeType index = 0; index < MaxSize; index++)
00650             {
00651                 *it++ = ScalarType(this->at(index));
00652             }
00653         }
00654         else                                                            // Everything else
00655         {
00656             // coverity[suspicious_sizeof : FALSE]
00657             ::uavcan::fill_n(dst_row_major, MaxSize, ScalarType(0));
00658         }
00659     }
00660 
00661 public:
00662     typedef T RawValueType;                           ///< This may be not the same as the element type.
00663     typedef typename StorageType<T>::Type ValueType;  ///< This is the actual stored element type.
00664     typedef typename Base::SizeType SizeType;         ///< Minimal width size type.
00665 
00666     using Base::size;
00667     using Base::capacity;
00668 
00669     enum { IsDynamic = ArrayMode == ArrayModeDynamic };
00670     enum { MaxSize = MaxSize_ };
00671     enum
00672     {
00673         MinBitLen = (IsDynamic == 0)
00674                     ? (static_cast<unsigned>(RawValueType::MinBitLen) * static_cast<unsigned>(MaxSize))
00675                     : 0
00676     };
00677     enum
00678     {
00679         MaxBitLen = static_cast<unsigned>(Base::SizeBitLen) +
00680                     static_cast<unsigned>(RawValueType::MaxBitLen) * static_cast<unsigned>(MaxSize)
00681     };
00682 
00683     /**
00684      * Default constructor zero-initializes the storage even if it consists of primitive types.
00685      */
00686     Array() { }
00687 
00688     /**
00689      * String constructor - only for string-like arrays.
00690      * Refer to @ref operator+=(const char*) for details.
00691      */
00692     Array(const char* str)      // Implicit
00693     {
00694         operator+=(str);
00695     }
00696 
00697     static int encode(const SelfType& array, ScalarCodec& codec, const TailArrayOptimizationMode tao_mode)
00698     {
00699         return array.encodeImpl(codec, tao_mode, BooleanType<IsDynamic>());
00700     }
00701 
00702     static int decode(SelfType& array, ScalarCodec& codec, const TailArrayOptimizationMode tao_mode)
00703     {
00704         return array.decodeImpl(codec, tao_mode, BooleanType<IsDynamic>());
00705     }
00706 
00707     static void extendDataTypeSignature(DataTypeSignature& signature)
00708     {
00709         RawValueType::extendDataTypeSignature(signature);
00710     }
00711 
00712     bool empty() const { return size() == 0; }
00713 
00714     /**
00715      * Only for dynamic arrays. Range checking.
00716      */
00717     void pop_back() { Base::shrink(); }
00718     void push_back(const ValueType& value)
00719     {
00720         Base::grow();
00721         Base::at(SizeType(size() - 1)) = value;
00722     }
00723 
00724     /**
00725      * Only for dynamic arrays. Range checking.
00726      */
00727     void resize(SizeType new_size, const ValueType& filler)
00728     {
00729         if (new_size > size())
00730         {
00731             SizeType cnt = SizeType(new_size - size());
00732             while (cnt-- > 0)
00733             {
00734                 push_back(filler);
00735             }
00736         }
00737         else if (new_size < size())
00738         {
00739             SizeType cnt = SizeType(size() - new_size);
00740             while (cnt-- > 0)
00741             {
00742                 pop_back();
00743             }
00744         }
00745         else
00746         {
00747             ; // Exact size
00748         }
00749     }
00750 
00751     /**
00752      * Only for dynamic arrays. Range checking.
00753      */
00754     void resize(SizeType new_size)
00755     {
00756         resize(new_size, ValueType());
00757     }
00758 
00759     /**
00760      * This operator accepts any container with size() and [].
00761      * Members must be comparable via operator ==.
00762      */
00763     template <typename R>
00764     typename EnableIf<sizeof((reinterpret_cast<const R*>(0))->size()) &&
00765                       sizeof((*(reinterpret_cast<const R*>(0)))[0]), bool>::Type
00766     operator==(const R& rhs) const
00767     {
00768         if (size() != rhs.size())
00769         {
00770             return false;
00771         }
00772         for (SizeType i = 0; i < size(); i++)  // Bitset does not have iterators
00773         {
00774             if (!(Base::at(i) == rhs[i]))
00775             {
00776                 return false;
00777             }
00778         }
00779         return true;
00780     }
00781 
00782     /**
00783      * This method compares two arrays using @ref areClose(), which ensures proper comparison of
00784      * floating point values, or DSDL data structures which contain floating point fields at any depth.
00785      * Please refer to the documentation of @ref areClose() to learn more about how it works and how to
00786      * define custom fuzzy comparison behavior.
00787      * Any container with size() and [] is acceptable.
00788      */
00789     template <typename R>
00790     typename EnableIf<sizeof((reinterpret_cast<const R*>(0))->size()) &&
00791                       sizeof((*(reinterpret_cast<const R*>(0)))[0]), bool>::Type
00792     isClose(const R& rhs) const
00793     {
00794         if (size() != rhs.size())
00795         {
00796             return false;
00797         }
00798         for (SizeType i = 0; i < size(); i++)  // Bitset does not have iterators
00799         {
00800             if (!areClose(Base::at(i), rhs[i]))
00801             {
00802                 return false;
00803             }
00804         }
00805         return true;
00806     }
00807 
00808     /**
00809      * This operator can only be used with string-like arrays; otherwise it will fail to compile.
00810      * @ref c_str()
00811      */
00812     bool operator==(const char* chr) const
00813     {
00814         if (chr == UAVCAN_NULLPTR)
00815         {
00816             return false;
00817         }
00818         return std::strncmp(Base::c_str(), chr, MaxSize) == 0;
00819     }
00820 
00821     /**
00822      * @ref operator==()
00823      */
00824     template <typename R> bool operator!=(const R& rhs) const { return !operator==(rhs); }
00825 
00826     /**
00827      * This operator can only be used with string-like arrays; otherwise it will fail to compile.
00828      * @ref c_str()
00829      */
00830     SelfType& operator=(const char* chr)
00831     {
00832         StaticAssert<Base::IsStringLike>::check();
00833         StaticAssert<IsDynamic>::check();
00834         Base::clear();
00835         if (chr == UAVCAN_NULLPTR)
00836         {
00837             handleFatalError("Array::operator=(const char*)");
00838         }
00839         while (*chr)
00840         {
00841             push_back(ValueType(*chr++));  // Value type is likely to be unsigned char, so conversion may be required.
00842         }
00843         return *this;
00844     }
00845 
00846     /**
00847      * This operator can only be used with string-like arrays; otherwise it will fail to compile.
00848      * @ref c_str()
00849      */
00850     SelfType& operator+=(const char* chr)
00851     {
00852         StaticAssert<Base::IsStringLike>::check();
00853         StaticAssert<IsDynamic>::check();
00854         if (chr == UAVCAN_NULLPTR)
00855         {
00856             handleFatalError("Array::operator+=(const char*)");
00857         }
00858         while (*chr)
00859         {
00860             push_back(ValueType(*chr++));
00861         }
00862         return *this;
00863     }
00864 
00865     /**
00866      * Appends another Array<> with the same element type. Mode and max size can be different.
00867      */
00868     template <uavcan::ArrayMode RhsArrayMode, unsigned RhsMaxSize>
00869     SelfType& operator+=(const Array<T, RhsArrayMode, RhsMaxSize>& rhs)
00870     {
00871         typedef Array<T, RhsArrayMode, RhsMaxSize> Rhs;
00872         StaticAssert<IsDynamic>::check();
00873         for (typename Rhs::SizeType i = 0; i < rhs.size(); i++)
00874         {
00875             push_back(rhs[i]);
00876         }
00877         return *this;
00878     }
00879 
00880     /**
00881      * Formatting appender.
00882      * This method doesn't raise an overflow error; instead it silently truncates the data to fit the array capacity.
00883      * Works only with string-like arrays, otherwise fails to compile.
00884      * @param format    Format string for std::snprintf(), e.g. "%08x", "%f"
00885      * @param value     Arbitrary value of a primitive type (should fail to compile if there's a non-primitive type)
00886      */
00887     template <typename A>
00888     void appendFormatted(const char* const format, const A value)
00889     {
00890         StaticAssert<Base::IsStringLike>::check();
00891         StaticAssert<IsDynamic>::check();
00892 
00893         StaticAssert<sizeof(A() >= A(0))>::check();              // This check allows to weed out most compound types
00894         StaticAssert<(sizeof(A) <= sizeof(long double)) ||
00895                      (sizeof(A) <= sizeof(long long))>::check(); // Another stupid check to catch non-primitive types
00896 
00897         if (!format)
00898         {
00899             UAVCAN_ASSERT(0);
00900             return;
00901         }
00902         // Add some hardcore runtime checks for the format string correctness?
00903 
00904         ValueType* const ptr = Base::end();
00905         UAVCAN_ASSERT(capacity() >= size());
00906         const SizeType max_size = SizeType(capacity() - size());
00907 
00908         // We have one extra byte for the null terminator, hence +1
00909         const int ret = snprintf(reinterpret_cast<char*>(ptr), SizeType(max_size + 1U), format, value);
00910 
00911         for (int i = 0; i < min(ret, int(max_size)); i++)
00912         {
00913             Base::grow();
00914         }
00915         if (ret < 0)
00916         {
00917             UAVCAN_ASSERT(0);    // Likely an invalid format string
00918             (*this) += format;   // So we print it as is in release builds
00919         }
00920     }
00921 
00922     /**
00923      * Converts the string to upper/lower case in place, assuming that encoding is ASCII.
00924      * These methods can only be used with string-like arrays; otherwise compilation will fail.
00925      */
00926     void convertToUpperCaseASCII()
00927     {
00928         StaticAssert<Base::IsStringLike>::check();
00929 
00930         for (SizeType i = 0; i < size(); i++)
00931         {
00932             const int x = Base::at(i);
00933             if ((x <= 'z') && (x >= 'a'))
00934             {
00935                 Base::at(i) = static_cast<ValueType>(x + ('Z' - 'z'));
00936             }
00937         }
00938     }
00939 
00940     void convertToLowerCaseASCII()
00941     {
00942         StaticAssert<Base::IsStringLike>::check();
00943 
00944         for (SizeType i = 0; i < size(); i++)
00945         {
00946             const int x = Base::at(i);
00947             if ((x <= 'Z') && (x >= 'A'))
00948             {
00949                 Base::at(i) = static_cast<ValueType>(x - ('Z' - 'z'));
00950             }
00951         }
00952     }
00953 
00954     /**
00955      * Fills this array as a packed square matrix from a static array.
00956      * Please refer to the specification to learn more about matrix packing.
00957      * Note that matrix packing code uses @ref areClose() for comparison.
00958      */
00959     template <typename ScalarType>
00960     void packSquareMatrix(const ScalarType (&src_row_major)[MaxSize])
00961     {
00962         packSquareMatrixImpl<const ScalarType*>(src_row_major);
00963     }
00964 
00965     /**
00966      * Fills this array as a packed square matrix in place.
00967      * Please refer to the specification to learn more about matrix packing.
00968      * Note that matrix packing code uses @ref areClose() for comparison.
00969      */
00970     void packSquareMatrix()
00971     {
00972         if (this->size() == MaxSize)
00973         {
00974             ValueType matrix[MaxSize];
00975             for (SizeType i = 0; i < MaxSize; i++)
00976             {
00977                 matrix[i] = this->at(i);
00978             }
00979             packSquareMatrix(matrix);
00980         }
00981         else if (this->size() == 0)
00982         {
00983             ; // Nothing to do - leave the matrix empty
00984         }
00985         else
00986         {
00987 #if UAVCAN_EXCEPTIONS
00988             throw std::out_of_range("uavcan::Array::packSquareMatrix()");
00989 #else
00990             UAVCAN_ASSERT(0);
00991             this->clear();
00992 #endif
00993         }
00994 
00995     }
00996 
00997     /**
00998      * Fills this array as a packed square matrix from any container that has the following public entities:
00999      *  - method begin()
01000      *  - method size()
01001      *  - only for C++03: type value_type
01002      * Please refer to the specification to learn more about matrix packing.
01003      * Note that matrix packing code uses @ref areClose() for comparison.
01004      */
01005     template <typename R>
01006     typename EnableIf<sizeof((reinterpret_cast<const R*>(0))->begin()) &&
01007                       sizeof((reinterpret_cast<const R*>(0))->size())>::Type
01008     packSquareMatrix(const R& src_row_major)
01009     {
01010         if (src_row_major.size() == MaxSize)
01011         {
01012             packSquareMatrixImpl(src_row_major.begin());
01013         }
01014         else if (src_row_major.size() == 0)
01015         {
01016             this->clear();
01017         }
01018         else
01019         {
01020 #if UAVCAN_EXCEPTIONS
01021             throw std::out_of_range("uavcan::Array::packSquareMatrix()");
01022 #else
01023             UAVCAN_ASSERT(0);
01024             this->clear();
01025 #endif
01026         }
01027     }
01028 
01029     /**
01030      * Reconstructs full matrix, result will be saved into a static array.
01031      * Please refer to the specification to learn more about matrix packing.
01032      */
01033     template <typename ScalarType>
01034     void unpackSquareMatrix(ScalarType (&dst_row_major)[MaxSize]) const
01035     {
01036         unpackSquareMatrixImpl<ScalarType, ScalarType*>(dst_row_major);
01037     }
01038 
01039     /**
01040      * Reconstructs full matrix in place.
01041      * Please refer to the specification to learn more about matrix packing.
01042      */
01043     void unpackSquareMatrix()
01044     {
01045         ValueType matrix[MaxSize];
01046         unpackSquareMatrix(matrix);
01047 
01048         this->clear();
01049         for (unsigned i = 0; i < MaxSize; i++)
01050         {
01051             this->push_back(matrix[i]);
01052         }
01053     }
01054 
01055     /**
01056      * Reconstructs full matrix, result will be saved into container that has the following public entities:
01057      *  - method begin()
01058      *  - method size()
01059      *  - only for C++03: type value_type
01060      * Please refer to the specification to learn more about matrix packing.
01061      */
01062     template <typename R>
01063     typename EnableIf<sizeof((reinterpret_cast<const R*>(0))->begin()) &&
01064                       sizeof((reinterpret_cast<const R*>(0))->size())>::Type
01065     unpackSquareMatrix(R& dst_row_major) const
01066     {
01067         if (dst_row_major.size() == MaxSize)
01068         {
01069 #if UAVCAN_CPP_VERSION > UAVCAN_CPP03
01070             typedef typename RemoveReference<decltype(*dst_row_major.begin())>::Type RhsValueType;
01071             unpackSquareMatrixImpl<RhsValueType>(dst_row_major.begin());
01072 #else
01073             unpackSquareMatrixImpl<typename R::value_type>(dst_row_major.begin());
01074 #endif
01075         }
01076         else
01077         {
01078 #if UAVCAN_EXCEPTIONS
01079             throw std::out_of_range("uavcan::Array::unpackSquareMatrix()");
01080 #else
01081             UAVCAN_ASSERT(0);
01082 #endif
01083         }
01084     }
01085 
01086     /**
01087      * Aliases for compatibility with standard containers.
01088      */
01089     typedef ValueType value_type;
01090     typedef SizeType size_type;
01091 };
01092 
01093 /**
01094  * These operators will only be enabled if rhs and lhs are different types. This precondition allows to work-around
01095  * the ambiguity arising from the scope containing two definitions: one here and the others in Array<>.
01096  * Refer to https://github.com/UAVCAN/libuavcan/issues/55 for more info.
01097  */
01098 template <typename R, typename T, ArrayMode ArrayMode, unsigned MaxSize>
01099 UAVCAN_EXPORT
01100 inline typename EnableIf<!IsSameType<R, Array<T, ArrayMode, MaxSize> >::Result, bool>::Type
01101 operator==(const R& rhs, const Array<T, ArrayMode, MaxSize>& lhs)
01102 {
01103     return lhs.operator==(rhs);
01104 }
01105 
01106 template <typename R, typename T, ArrayMode ArrayMode, unsigned MaxSize>
01107 UAVCAN_EXPORT
01108 inline typename EnableIf<!IsSameType<R, Array<T, ArrayMode, MaxSize> >::Result, bool>::Type
01109 operator!=(const R& rhs, const Array<T, ArrayMode, MaxSize>& lhs)
01110 {
01111     return lhs.operator!=(rhs);
01112 }
01113 
01114 /**
01115  * Shortcut for string-like array type instantiation.
01116  * The proper way of doing this is actually "template<> using ... = ...", but this feature is not available in
01117  * older C++ revisions which the library has to support.
01118  */
01119 template <unsigned MaxSize>
01120 class MakeString
01121 {
01122     MakeString();       // This class is not instantiatable.
01123 public:
01124     typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, MaxSize> Type;
01125 };
01126 
01127 /**
01128  * YAML streamer specification for any Array<>
01129  */
01130 template <typename T, ArrayMode ArrayMode, unsigned MaxSize>
01131 class UAVCAN_EXPORT YamlStreamer<Array<T, ArrayMode, MaxSize> >
01132 {
01133     typedef Array<T, ArrayMode, MaxSize> ArrayType;
01134 
01135     static bool isNiceCharacter(int c)
01136     {
01137         if (c >= 32 && c <= 126)
01138         {
01139             return true;
01140         }
01141         static const char Good[] = {'\n', '\r', '\t'};
01142         for (unsigned i = 0; i < sizeof(Good) / sizeof(Good[0]); i++)
01143         {
01144             if (Good[i] == c)
01145             {
01146                 return true;
01147             }
01148         }
01149         return false;
01150     }
01151 
01152     template <typename Stream>
01153     static void streamPrimitives(Stream& s, const ArrayType& array)
01154     {
01155         s << '[';
01156         for (typename ArrayType::SizeType i = 0; i < array.size(); i++)
01157         {
01158             YamlStreamer<T>::stream(s, array.at(i), 0);
01159             if ((i + 1) < array.size())
01160             {
01161                 s << ", ";
01162             }
01163         }
01164         s << ']';
01165     }
01166 
01167     template <typename Stream>
01168     static void streamCharacters(Stream& s, const ArrayType& array)
01169     {
01170         s << '"';
01171         for (typename ArrayType::SizeType i = 0; i < array.size(); i++)
01172         {
01173             const int c = array.at(i);
01174             if (c < 32 || c > 126)
01175             {
01176                 char nibbles[2] = {char((c >> 4) & 0xF), char(c & 0xF)};
01177                 for (int k = 0; k < 2; k++)
01178                 {
01179                     nibbles[k] = char(nibbles[k] + '0');
01180                     if (nibbles[k] > '9')
01181                     {
01182                         nibbles[k] = char(nibbles[k] + 'A' - '9' - 1);
01183                     }
01184                 }
01185                 s << "\\x" << nibbles[0] << nibbles[1];
01186             }
01187             else
01188             {
01189                 if (c == '"' || c == '\\')      // YAML requires to escape these two
01190                 {
01191                     s << '\\';
01192                 }
01193                 s << char(c);
01194             }
01195         }
01196         s << '"';
01197     }
01198 
01199     struct SelectorStringLike { };
01200     struct SelectorPrimitives { };
01201     struct SelectorObjects { };
01202 
01203     template <typename Stream>
01204     static void genericStreamImpl(Stream& s, const ArrayType& array, int, SelectorStringLike)
01205     {
01206         bool printable_only = true;
01207         for (typename ArrayType::SizeType i = 0; i < array.size(); i++)
01208         {
01209             if (!isNiceCharacter(array[i]))
01210             {
01211                 printable_only = false;
01212                 break;
01213             }
01214         }
01215         if (printable_only)
01216         {
01217             streamCharacters(s, array);
01218         }
01219         else
01220         {
01221             streamPrimitives(s, array);
01222             s << " # ";
01223             streamCharacters(s, array);
01224         }
01225     }
01226 
01227     template <typename Stream>
01228     static void genericStreamImpl(Stream& s, const ArrayType& array, int, SelectorPrimitives)
01229     {
01230         streamPrimitives(s, array);
01231     }
01232 
01233     template <typename Stream>
01234     static void genericStreamImpl(Stream& s, const ArrayType& array, int level, SelectorObjects)
01235     {
01236         if (array.empty())
01237         {
01238             s << "[]";
01239             return;
01240         }
01241         for (typename ArrayType::SizeType i = 0; i < array.size(); i++)
01242         {
01243             s << '\n';
01244             for (int pos = 0; pos < level; pos++)
01245             {
01246                 s << "  ";
01247             }
01248             s << "- ";
01249             YamlStreamer<T>::stream(s, array.at(i), level + 1);
01250         }
01251     }
01252 
01253 public:
01254     /**
01255      * Prints Array<> into the stream in YAML format.
01256      */
01257     template <typename Stream>
01258     static void stream(Stream& s, const ArrayType& array, int level)
01259     {
01260         typedef typename Select<ArrayType::IsStringLike, SelectorStringLike,
01261                                 typename Select<IsPrimitiveType<typename ArrayType::RawValueType>::Result,
01262                                                 SelectorPrimitives,
01263                                                 SelectorObjects>::Result >::Result Type;
01264         genericStreamImpl(s, array, level, Type());
01265     }
01266 };
01267 
01268 }
01269 
01270 #endif // UAVCAN_MARSHAL_ARRAY_HPP_INCLUDED