/*******************************************************************************
 * Copyright (c) 2021, Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
 *
 * This program and the accompanying materials are made
 * available under the terms of the Eclipse Public License 2.0
 * which is available at https://www.eclipse.org/legal/epl-2.0/
 *
 * SPDX-License-Identifier: EPL-2.0
 *******************************************************************************/

//-----------------------------------------------------------------------------
/** @file  orientation.h */
//-----------------------------------------------------------------------------

#ifndef MANTLEAPI_COMMON_ORIENTATION_H
#define MANTLEAPI_COMMON_ORIENTATION_H

#include <MantleAPI/Common/floating_point_helper.h>
#include <units.h>

namespace units
{
UNIT_ADD(angular_acceleration,
         radians_per_second_squared,
         radians_per_second_squared,
         rad_per_s_sq,
         compound_unit<angle::radians, inverse<squared<time::seconds>>>)

namespace category
{
typedef base_unit<detail::meter_ratio<0>, std::ratio<0>, std::ratio<-2>, std::ratio<1>> angular_acceleration_unit;
}

UNIT_ADD_CATEGORY_TRAIT(angular_acceleration)

UNIT_ADD(angular_jerk,
         radians_per_second_cubed,
         radians_per_second_cubed,
         rad_per_s_cu,
         compound_unit<angle::radians, inverse<cubed<time::seconds>>>)

namespace category
{
typedef base_unit<detail::meter_ratio<0>, std::ratio<0>, std::ratio<-3>, std::ratio<1>> angular_jerk_unit;
}

UNIT_ADD_CATEGORY_TRAIT(angular_jerk)

}  // namespace units

namespace mantle_api
{
template <typename T,
          class = typename std::enable_if_t<units::traits::is_angle_unit<T>::value ||
                                            units::traits::is_angular_velocity_unit<T>::value ||
                                            units::traits::is_angular_acceleration_unit<T>::value>>
struct Orientation3
{
  Orientation3() = default;

  Orientation3(T yaw_in, T pitch_in, T roll_in)
      : yaw{yaw_in}, pitch{pitch_in}, roll{roll_in}
  {
  }

  T yaw{};
  T pitch{};
  T roll{};
};

template <typename T>
inline bool operator==(const Orientation3<T>& lhs, const Orientation3<T>& rhs) noexcept
{
  return IsEqual(lhs.yaw, rhs.yaw) && IsEqual(lhs.pitch, rhs.pitch) && IsEqual(lhs.roll, rhs.roll);
}

template <typename T>
inline bool operator!=(const Orientation3<T>& lhs, const Orientation3<T>& rhs) noexcept
{
  return !(lhs == rhs);
}

template <typename T>
inline Orientation3<T> operator+(const Orientation3<T>& lhs, const Orientation3<T>& rhs) noexcept
{
  return Orientation3<T>{lhs.yaw + rhs.yaw, lhs.pitch + rhs.pitch, lhs.roll + rhs.roll};
}

template <typename T>
inline Orientation3<T> operator-(const Orientation3<T>& lhs, const Orientation3<T>& rhs) noexcept
{
  return Orientation3<T>{lhs.yaw - rhs.yaw, lhs.pitch - rhs.pitch, lhs.roll - rhs.roll};
}

}  // namespace mantle_api

#endif  // MANTLEAPI_COMMON_ORIENTATION_H