// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-

// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2022 FZI Forschungszentrum Informatik
// Created on behalf of Universal Robots A/S
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------

//----------------------------------------------------------------------
/*!\file
 *
 * \author  Felix Exner exner@fzi.de
 * \date    2022-12-15
 *
 */
//----------------------------------------------------------------------

#ifndef UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED
#define UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED

#include <string>
#include <chrono>
#include <functional>

#include "ur_client_library/ur/version_information.h"
#include "ur_client_library/ur/datatypes.h"
#ifdef _WIN32

#  define NOMINMAX
#  define WIN32_LEAN_AND_MEAN
#  include <Windows.h>

#  ifdef ERROR
#    undef ERROR
#  endif  // ERROR

#  define SCHED_FIFO (1)

typedef HANDLE pthread_t;

static inline pthread_t pthread_self()
{
  return ::GetCurrentThread();
}

static inline int sched_get_priority_max(int policy)
{
  (void)policy;
  return THREAD_PRIORITY_TIME_CRITICAL;
}

#else  // _WIN32

#  include <pthread.h>

#endif  // _WIN32

/*!
 * \file
 * \brief Portable helpers to temporarily silence deprecation diagnostics.
 *
 * Use \ref URCL_SILENCE_DEPRECATED_BEGIN before and \ref URCL_SILENCE_DEPRECATED_END after the
 * code that must call deprecated APIs (e.g. deprecated library methods implemented for backward
 * compatibility). Expands to nothing on unsupported compilers.
 *
 * Example:
 * \code
 * URCL_SILENCE_DEPRECATED_BEGIN
 * legacy_call();
 * URCL_SILENCE_DEPRECATED_END
 * \endcode
 */

#if defined(_MSC_VER)
#  define URCL_SILENCE_DEPRECATED_BEGIN __pragma(warning(push)) __pragma(warning(disable : 4996))
#  define URCL_SILENCE_DEPRECATED_END __pragma(warning(pop))
#elif defined(__GNUC__)
#  define URCL_SILENCE_DEPRECATED_BEGIN                                                                                \
    _Pragma("GCC diagnostic push") _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"")
#  define URCL_SILENCE_DEPRECATED_END _Pragma("GCC diagnostic pop")
#else
#  define URCL_SILENCE_DEPRECATED_BEGIN
#  define URCL_SILENCE_DEPRECATED_END
#endif

namespace urcl
{
bool setFiFoScheduling(pthread_t& thread, const int priority);

/*!
 * \brief Wait for a condition to be true.
 *
 * This function will wait for a condition to be true. The condition is checked in intervals of \p check_interval.
 * If the condition is not met after \p timeout, the function will throw a urcl::TimeoutException.
 *
 * \param condition The condition to be checked.
 * \param timeout The maximum time to wait for the condition to be true.
 * \param check_interval The interval in which the condition is checked.
 */
void waitFor(std::function<bool()> condition, const std::chrono::milliseconds timeout,
             const std::chrono::milliseconds check_interval = std::chrono::milliseconds(50));

/*!
 * \brief Parses a boolean value from a string.
 *
 * The string can be one of
 *  - true, True, TRUE
 *  - on, On, ON
 *  - yes, Yes, YES
 *  - 1
 *  - false, False, FALSE
 *  - off, Off, OFF
 *  - no, No, NO
 *  - 0
 *
 * \param str string to be parsed
 * \throws urcl::UrException If the string doesn't match one of the options
 * \return The boolean representation of the string
 */
bool parseBoolean(const std::string& str);

/*!
 * \brief Splits a at each delimiter found
 *
 * \param string_to_split String containing the delimiter and other characters
 * \param delimiter Chars at which the string should be split
 *
 * \returns A vector of characters that were between the delimiters
 */
std::vector<std::string> splitString(const std::string& string_to_split, const std::string& delimiter = ",");

/*!
 * \brief Clamps every element of a container to the range [0, 1] in-place.
 *
 * \tparam T The type of the elements in the array.
 * \tparam N The size of the array.
 * \param values The array whose elements will be clamped.
 */
template <typename T, size_t N>
void clampToUnitRange(std::array<T, N>& values)
{
  for (auto& v : values)
  {
    if (v < 0.0)
      v = 0.0;
    else if (v > 1.0)
      v = 1.0;
  }
}

/*!
 * \brief Get the robot series from the robot type and version information.
 *
 * \param type The robot type.
 * \param version The version information of the robot.
 *
 * \returns The robot series corresponding to the given robot type and version information.
 */
RobotSeries robotSeriesFromTypeAndVersion(const RobotType type, const VersionInformation& version);

/*!
 * \brief Get the robot type from a string.
 *
 * The \c RobotType enum has no dedicated entries for UR7 and UR12, so "ur7e" is mapped to
 * \c RobotType::UR5 and "ur12e" is mapped to \c RobotType::UR10, matching what the robot
 * reports over the primary interface.
 *
 * \param robot_type_str The string representation of the robot type as used in the start_ursim.sh
 * script. Must be all lower-case, e.g. "ur3e", "ur5", "ur10e", "ur16e", "ur7e", "ur15", "ur30",
 * "ur8long".
 *
 * \throws std::invalid_argument if \p robot_type_str does not match a known robot type.
 *
 * \returns The robot type corresponding to the given string.
 */
RobotType robotTypeFromString(const std::string& robot_type_str);

}  // namespace urcl
#endif  // ifndef UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED
