ByteOrderConversion.h

Go to the documentation of this file.

//
// © Copyright 2022 SCHUNK Mobile Greifsysteme GmbH, Lauffen/Neckar Germany
// © Copyright 2022 FZI Forschungszentrum Informatik, Karlsruhe, Germany
//
// This file is part of the Schunk SVH Library.
//
// The Schunk SVH Library is free software: you can redistribute it and/or
// modify it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or (at your
// option) any later version.
//
// The Schunk SVH Library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
// Public License for more details.
//
// You should have received a copy of the GNU General Public License along with
// the Schunk SVH Library. If not, see <https://www.gnu.org/licenses/>.
//

//----------------------------------------------------------------------
//----------------------------------------------------------------------
#ifndef DRIVER_SVH_BYTE_ORDER_CONVERISON_H_INCLUDED
#define DRIVER_SVH_BYTE_ORDER_CONVERISON_H_INCLUDED

#include "schunk_svh_library/ImportExport.h"

#include <assert.h>
#include <iomanip>
#include <iostream>
#include <vector>


namespace driver_svh {

template <typename T>
size_t toLittleEndian(const T& data, std::vector<uint8_t>& array, size_t& write_pos)
{
  // Resize the target array in case it it to small to avoid out of bounds acces
  if (write_pos + sizeof(T) > array.size())
  {
    // TODO: Remove Debug
    // std::cout << "To Little Endian has to extend the Array. Current array Size: "<<
    // (int)array.size() << " Pos: " << (int)pos << " Size_T: "<< sizeof(T) << " New array Size: "<<
    // (int)(pos + sizeof(T)) << std::endl;
    array.resize(write_pos + sizeof(T));
  }

  // Endianess Conversion
  for (size_t i = 0; i < sizeof(T); ++i)
  {
    // Copy each byte into the bytearray, always convert byte order to little endian regardles of
    // source architecture
    array[write_pos + i] = static_cast<uint8_t>((data >> (i * 8)) & 0xFF);
  }

  return write_pos + sizeof(T);
}

template <>
DRIVER_SVH_IMPORT_EXPORT size_t toLittleEndian<float>(const float& data,
                                                      std::vector<uint8_t>& array,
                                                      size_t& write_pos);

template <>
DRIVER_SVH_IMPORT_EXPORT size_t toLittleEndian<double>(const double& data,
                                                       std::vector<uint8_t>& array,
                                                       size_t& write_pos);


template <typename T>
size_t fromLittleEndian(T& data, std::vector<uint8_t>& array, size_t& read_pos)
{
  // TODO: Remove once everything is tested :)
  // std::cout << "From Little Endian Called with: "<<" Size_T: "<< sizeof(T) << " Pos: " <<
  // (int)read_pos << "Current Array Size: "<< (int)array.size() << std::endl;

  // Reset data as we only write with or
  data = 0;

  // Check if ArrayBuilder has enough data
  if (read_pos + sizeof(T) > array.size())
  {
    // TODO: better error handling?
    return read_pos;
  }


  // Endianess Conversion
  for (size_t i = 0; i < sizeof(T); ++i)
  {
    // Copy each byte into the bytearray, always convert byte order back from little endian
    data |= (array[read_pos + i] & 0xFF) << (i * 8);
    // std::cout << "Converting Value: 0x" << std::setw(2) << std::setfill('0') << std::hex <<
    // static_cast<int>((array[read_pos+i]& 0xFF) <<(i*8)) << " At the position
    // i="<<static_cast<int>(i) << "resulting in the variable data: " << static_cast<int>(data)  <<
    // std::dec <<" Or on DEC :" << static_cast<int>(data) << std::endl;
  }

  // Note: The Vector still contains the elements at this point maybe we would like to delete that?
  // But its expensive
  return read_pos + sizeof(T);
}

template <>
DRIVER_SVH_IMPORT_EXPORT size_t fromLittleEndian<float>(float& data,
                                                        std::vector<uint8_t>& array,
                                                        size_t& read_pos);

template <>
DRIVER_SVH_IMPORT_EXPORT size_t fromLittleEndian<double>(double& data,
                                                         std::vector<uint8_t>& array,
                                                         size_t& read_pos);

class ArrayBuilder
{
public:
  ArrayBuilder(size_t array_size = 1)
    : write_pos(0)
    , read_pos(0)
    , array(array_size, 0)
  {
  }

  size_t write_pos;

  size_t read_pos;

  std::vector<uint8_t> array;


  void reset(size_t array_size = 1);

  template <typename T>
  void appendWithoutConversion(const T& data)
  {
    // Resize the target array in case it it to small to avoid out of bounds acces
    if (write_pos + sizeof(T) > array.size())
    {
      array.resize(write_pos + sizeof(T));
    }

    // write data to array without conversion
    *(reinterpret_cast<T*>(&array[write_pos])) = data;
    write_pos += sizeof(T);
  }

  template <typename T>
  void appendWithoutConversion(const std::vector<T>& data)
  {
    // Just insert every element of the Vector individually
    for (typename std::vector<T>::const_iterator it = data.begin(); it != data.end(); ++it)
    {
      appendWithoutConversion(*it);
    }
  }

  template <typename T>
  ArrayBuilder& operator<<(const T& data);

  template <typename T>
  ArrayBuilder& operator<<(const std::vector<T>& data);


  template <typename T>
  ArrayBuilder& operator>>(T& data);

  template <typename T>
  ArrayBuilder& operator>>(std::vector<T>& data);

  template <typename T>
  T readBack();
};


template <typename T>
ArrayBuilder& ArrayBuilder::operator<<(const T& data)
{
  // Convert the type to correct encoding and poit it into the Array
  write_pos = toLittleEndian<T>(data, array, write_pos);

  // TODO: Remove debug output
  // std::cout << "Arraybuilder got a generic type of length: "<< sizeof(data) << " and With data
  // Packet: "<< (int)data << std::endl;

  return *this;
}


template <typename T>
ArrayBuilder& ArrayBuilder::operator<<(const std::vector<T>& data)
{
  // Just insert every element of the Vector individually
  for (typename std::vector<T>::const_iterator it = data.begin(); it != data.end(); ++it)
  {
    *this << *it;
  }

  // TODO: Remove Debug output
  // std::cout << "ArrayBuilder got a vector of length: "<< data.size() << std::endl;

  return *this;
}


template <typename T>
ArrayBuilder& ArrayBuilder::operator>>(T& data)
{
  read_pos = fromLittleEndian<T>(data, array, read_pos);
  return *this;
}

template <typename T>
ArrayBuilder& ArrayBuilder::operator>>(std::vector<T>& data)
{
  // Todo: For u_int8 Vectors this could also just be handled by an insert -> faster :)
  // Just insert every element of the Vector individually --> Do it in reverse order as we read from
  // the end of the list :)
  for (typename std::vector<T>::iterator it = data.begin(); it != data.end(); ++it)
  {
    *this >> *it;
  }

  return *this;
}


template <typename T>
T ArrayBuilder::readBack()
{
  T data;
  size_t read_back_pos = write_pos - sizeof(T);
  fromLittleEndian<T>(data, array, read_back_pos);
  return data;
}


std::ostream& operator<<(std::ostream& o, const ArrayBuilder& ab);

} // namespace driver_svh

#endif