/*******************************************************************************
 * 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  spline.h */
//-----------------------------------------------------------------------------

#ifndef MANTLEAPI_COMMON_SPLINE_H
#define MANTLEAPI_COMMON_SPLINE_H

#include <MantleAPI/Common/floating_point_helper.h>
#include <MantleAPI/Common/orientation.h>
#include <MantleAPI/Common/time_utils.h>
#include <MantleAPI/Common/vector.h>
#include <units.h>

#include <tuple>

using namespace units;

namespace units
{
UNIT_ADD(jerk,
         meters_per_second_cubed,
         meters_per_second_cubed,
         mps_cu,
         compound_unit<velocity::meters_per_second, inverse<squared<time::seconds>>>)

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

UNIT_ADD_CATEGORY_TRAIT(jerk)

UNIT_ADD(jerk_acceleration,
         meters_per_second_to_the_power_of_four,
         meters_per_second_to_the_power_of_four,
         mps_pow4,
         compound_unit<velocity::meters_per_second, inverse<cubed<time::seconds>>>)

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

UNIT_ADD_CATEGORY_TRAIT(jerk_acceleration)

}  // namespace units

namespace mantle_api
{
template <typename T>
struct SplineSegment
{
  Vec3<T> a;
  Vec3<T> b;
  Vec3<T> c;
  Vec3<T> d;
};

template <typename T, class = typename std::enable_if_t<traits::is_unit<T>::value>>
struct SplineSection
{
  Time start_time{0};
  Time end_time{0};
  /// @brief Represents the polynomial.
  ///
  /// The tuple stores in format \f$[a_3, a_2, a_1, a_0]\f$ for a polynomial in form
  /// \f[
  ///   P(x) = \sum_{i=0}^{3} a_{i} x^{i} = a_3 x^3 + a_2 x^2 + a_1 x + a_0
  /// \f]
  std::tuple<unit_t<compound_unit<T, inverse<cubed<time::second>>>>,
             unit_t<compound_unit<T, inverse<squared<time::second>>>>,
             unit_t<compound_unit<T, inverse<time::second>>>,
             unit_t<T>>
      polynomial{
          unit_t<compound_unit<T, inverse<cubed<time::second>>>>(0),
          unit_t<compound_unit<T, inverse<squared<time::second>>>>(0),
          unit_t<compound_unit<T, inverse<time::second>>>(0),
          unit_t<T>(0)};
};

/// @brief Equality comparison for SplineSection.
template <typename T, class = typename std::enable_if_t<units::traits::is_unit<T>::value>>
inline bool operator==(const SplineSection<T>& lhs, const SplineSection<T>& rhs) noexcept
{
  return lhs.start_time == rhs.start_time && lhs.end_time == rhs.end_time && IsEqual(std::get<0>(lhs.polynomial), std::get<0>(rhs.polynomial)) && IsEqual(std::get<1>(lhs.polynomial), std::get<1>(rhs.polynomial)) && IsEqual(std::get<2>(lhs.polynomial), std::get<2>(rhs.polynomial)) && IsEqual(std::get<3>(lhs.polynomial), std::get<3>(rhs.polynomial));
}

}  // namespace mantle_api

#endif  // MANTLEAPI_COMMON_SPLINE_H