#ifndef COMPLIB_SERVER_MATHUTILS_HPP
#define COMPLIB_SERVER_MATHUTILS_HPP

#include <cstdint>
#include <cmath>

namespace mathUtils {
  inline int int_from_bytes(uint8_t *data, int length) {
    int ret = 0;

    int i = 0;
    for (int j = length - 1; j >= 0; j--) {
      ret = ret | (data[i] << (j * 8));
      i++;
    }
    return ret;
  }

  template<class T>
  inline T from_bytes(const uint8_t *data, int length) {
    T ret = 0;

    int i = 0;
    for (int j = length - 1; j >= 0; j--) {
      ret = ret | (data[i] << (j * 8));
      i++;
    }
    return ret;
  }

  inline void bytes_from_int(int data, int length, uint8_t *result) {
    int i = 0;
    for (int j = length - 1; j >= 0; j--) {
      result[i] = ((data >> (j * 8)) & 0xffu);
      i++;
    }
  }

  template<class T>
  inline T wrap_angle_to_pi(T angle) {
    return atan2(sin(angle), cos(angle));
  }
}

#endif // COMPLIB_SERVER_MATHUTILS_HPP