#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));
  }

  template<class T>
  inline T limit_min(T val, T min) {
    if (val < 0.0 && val > -min) {
      return -min;
    } else if (val > 0.0 && val < min) {
      return min;
    } else if (val == 0) {
      return min;
    }
    return val;
  }

  template<class T>
  inline T limit_max(T val, T min, T max) {
    return fmax(min, fmin(max, val));
  }
}

#endif // COMPLIB_SERVER_MATHUTILS_HPP