#ifndef POLYNOMIALP_HPP
#define POLYNOMIALP_HPP

#include <algebra/TinyVector.hpp>

template <size_t N, size_t Dimension>
class PolynomialP
{
 private:
  static constexpr size_t size_coef = [] {
    if constexpr (Dimension == 1) {
      return N + 1;
    } else if constexpr (Dimension == 2) {
      return (N + 1) * (N + 2) / 2;
    } else {
      static_assert(Dimension == 3);
      return (N + 1) * (N + 2) * (N + 3) / 6;
    }
  }();

  using Coefficients = TinyVector<size_coef, double>;
  Coefficients m_coefficients;
  static_assert((N >= 0), "PolynomialP degree must be non-negative");
  static_assert((Dimension > 0), "PolynomialP dimension must be positive");
  static_assert((Dimension <= 3), "PolynomialP dimension must no greater than three");

 public:
  PUGS_INLINE
  constexpr size_t
  degree() const
  {
    return N;
  }

  constexpr size_t
  dim() const
  {
    return Dimension;
  }

  PUGS_INLINE
  constexpr const TinyVector<size_coef, double>&
  coefficients() const
  {
    return m_coefficients;
  }

  PUGS_INLINE
  constexpr TinyVector<size_coef, double>&
  coefficients()
  {
    return m_coefficients;
  }

  PUGS_INLINE constexpr bool
  operator==(const PolynomialP& q) const
  {
    return m_coefficients == q.m_coefficients;
  }

  PUGS_INLINE constexpr PolynomialP(const TinyVector<size_coef, double>& coefficients) noexcept
    : m_coefficients{coefficients}
  {}

  PUGS_INLINE
  constexpr PolynomialP(TinyVector<size_coef, double>&& coefficients) noexcept : m_coefficients{coefficients} {}

  PUGS_INLINE
  constexpr bool
  operator!=(const PolynomialP& q) const
  {
    return not this->operator==(q);
  }

  PUGS_INLINE constexpr PolynomialP
  operator+(const PolynomialP Q) const
  {
    PolynomialP<N, Dimension> P(m_coefficients);
    for (size_t i = 0; i < size_coef; ++i) {
      P.coefficients()[i] += Q.coefficients()[i];
    }
    return P;
  }

  PUGS_INLINE constexpr PolynomialP
  operator-() const
  {
    PolynomialP<N, Dimension> P;
    P.coefficients() = -coefficients();
    return P;
  }

  PUGS_INLINE constexpr PolynomialP() noexcept = default;
  ~PolynomialP()                               = default;
};

//   template <size_t M>
//   PUGS_INLINE constexpr PolynomialP<std::max(M, N)>
//   operator-(const PolynomialP<M>& Q) const
//   {
//     PolynomialP<std::max(M, N)> P;
//     if constexpr (M > N) {
//       P.coefficients() = -Q.coefficients();
//       for (size_t i = 0; i <= N; ++i) {
//         P.coefficients()[i] += coefficients()[i];
//       }
//     } else {
//       P.coefficients() = coefficients();
//       for (size_t i = 0; i <= M; ++i) {
//         P.coefficients()[i] -= Q.coefficients()[i];
//       }
//     }
//     return P;
//   }

//   template <size_t M>
//   PUGS_INLINE constexpr PolynomialP<N>&
//   operator=(const PolynomialP<M>& Q)
//   {
//     coefficients() = zero;
//     for (size_t i = N + 1; i <= M; ++i) {
//       Assert(Q.coefficients()[i] == 0, "degree of polynomialP to small in assignation");
//     }
//     //    static_assert(N >= M, "degree of polynomialP to small in assignation");
//     for (size_t i = 0; i <= std::min(M, N); ++i) {
//       coefficients()[i] = Q.coefficients()[i];
//     }
//     return *this;
//   }

//   template <size_t M>
//   PUGS_INLINE constexpr PolynomialP<N>&
//   operator+=(const PolynomialP<M>& Q)
//   {
//     static_assert(N >= M, "PolynomialP degree to small in affectation addition");
//     for (size_t i = 0; i <= M; ++i) {
//       coefficients()[i] += Q.coefficients()[i];
//     }
//     return *this;
//   }

//   template <size_t M>
//   PUGS_INLINE constexpr PolynomialP<N>&
//   operator-=(const PolynomialP<M>& Q)
//   {
//     static_assert(N >= M, "PolynomialP degree to small in affectation addition");
//     for (size_t i = 0; i <= M; ++i) {
//       coefficients()[i] -= Q.coefficients()[i];
//     }
//     return *this;
//   }

//   template <size_t M>
//   PUGS_INLINE constexpr PolynomialP<M + N>
//   operator*(const PolynomialP<M>& Q) const
//   {
//     PolynomialP<M + N> P;
//     P.coefficients() = zero;
//     for (size_t i = 0; i <= N; ++i) {
//       for (size_t j = 0; j <= M; ++j) {
//         P.coefficients()[i + j] += coefficients()[i] * Q.coefficients()[j];
//       }
//     }
//     return P;
//   }

//   template <size_t M>
//   PUGS_INLINE constexpr PolynomialP<N>&
//   operator*=(const PolynomialP<M>& Q)
//   {
//     static_assert(N >= M, "Degree to small in affectation product");
//     for (size_t i = N - M + 1; i <= N; ++i) {
//       Assert(coefficients()[i] == 0, "Degree of affectation product greater than the degree of input polynomialP");
//     }
//     PolynomialP<N> P(zero);
//     for (size_t i = 0; i <= N - M; ++i) {
//       for (size_t j = 0; j <= M; ++j) {
//         P.coefficients()[i + j] += coefficients()[i] * Q.coefficients()[j];
//       }
//     }
//     coefficients() = P.coefficients();
//     return *this;
//   }

//   PUGS_INLINE
//   constexpr PolynomialP<N>
//   operator*(const double& lambda) const
//   {
//     TinyVector<N + 1> mult_coefs = lambda * m_coefficients;
//     PolynomialP<N> M(mult_coefs);
//     return M;
//   }

//   template <size_t M>
//   PUGS_INLINE constexpr PolynomialP<M * N>
//   compose(const PolynomialP<M>& Q) const
//   {
//     PolynomialP<M * N> P;
//     P.coefficients() = zero;
//     PolynomialP<M * N> R;
//     R.coefficients() = zero;

//     for (size_t i = 0; i <= N; ++i) {
//       R = Q.template pow<N>(i) * coefficients()[i];
//       P += R;   // R;
//     }
//     return P;
//   }

//   template <size_t M, size_t I>
//   PUGS_INLINE constexpr PolynomialP<M * N>
//   power(const PolynomialP<M>& Q) const
//   {
//     return coefficients()[I] * Q.template pow<N>(I);
//   }

//   template <size_t M, size_t... I>
//   PUGS_INLINE constexpr PolynomialP<M * N>
//   compose2(const PolynomialP<M>& Q, std::index_sequence<I...>) const
//   {
//     PolynomialP<M * N> P;
//     P.coefficients() = zero;
//     P                = (power<M, I>(Q) + ...);
//     return P;
//   }

//   template <size_t M>
//   PUGS_INLINE constexpr PolynomialP<M * N>
//   compose2(const PolynomialP<M>& Q) const
//   {
//     PolynomialP<M * N> P;
//     P.coefficients()    = zero;
//     using IndexSequence = std::make_index_sequence<N + 1>;
//     return compose2<M>(Q, IndexSequence{});
//     //    for (size_t i = 0; i <= N; ++i) {
//     //      P += Q.template pow<N>(i) * coefficients()[i];
//     //    }
//     return P;
//   }

//   template <size_t M>
//   PUGS_INLINE constexpr PolynomialP<M * N>
//   operator()(const PolynomialP<M>& Q) const
//   {
//     PolynomialP<M * N> P;
//     P.coefficients() = zero;
//     PolynomialP<M * N> R;
//     R.coefficients() = zero;

//     for (size_t i = 0; i <= N; ++i) {
//       R = Q.template pow<N>(i) * coefficients()[i];
//       P += R;   // R;
//     }
//     return P;
//   }

//   template <size_t M>
//   PUGS_INLINE constexpr PolynomialP<M * N>
//   pow(size_t power) const
//   {
//     Assert(power <= M, "You declared a polynomialP of degree too small for return of the pow function");
//     PolynomialP<M * N> R;
//     R.coefficients() = zero;
//     if (power == 0) {
//       R.coefficients()[0] = 1;
//     } else {
//       R = *this;
//       for (size_t i = 1; i < power; ++i) {
//         R = R * *this;
//       }
//     }
//     return R;
//   }

//   PUGS_INLINE
//   constexpr friend PolynomialP<N>
//   operator*(const double& lambda, const PolynomialP<N> P)
//   {
//     return P * lambda;
//   }
//   // evaluation using Horner's method https://en.wikipedia.org/wiki/Horner's_method
//   PUGS_INLINE
//   constexpr double
//   evaluate(const double& x) const
//   {
//     TinyVector<N + 1> coefs = this->coefficients();
//     double bcoef            = coefs[N];
//     for (size_t i = N; i > 0; --i) {
//       bcoef *= x;
//       bcoef += coefs[i - 1];
//     }
//     return bcoef;
//   }

//   PUGS_INLINE
//   constexpr double
//   operator()(const double x) const
//   {
//     TinyVector<N + 1> coefs = this->coefficients();
//     double bcoef            = coefs[N];
//     for (size_t i = N; i > 0; --i) {
//       bcoef *= x;
//       bcoef += coefs[i - 1];
//     }
//     return bcoef;
//   }

//   template <size_t M>
//   PUGS_INLINE constexpr friend void
//   divide(const PolynomialP<N>& P1, const PolynomialP<M>& P2, PolynomialP<N>& Q, PolynomialP<N>& R)
//   {
//     const size_t Nr  = P1.realDegree();
//     const size_t Mr  = P2.realDegree();
//     R.coefficients() = P1.coefficients();
//     Q.coefficients() = zero;
//     for (ssize_t k = Nr - Mr; k >= 0; --k) {
//       Q.coefficients()[k] = R.coefficients()[Mr + k] / P2.coefficients()[Mr];
//       for (ssize_t j = Mr + k; j >= k; --j) {
//         R.coefficients()[j] -= Q.coefficients()[k] * P2.coefficients()[j - k];
//       }
//     }
//     for (size_t j = Mr; j <= Nr; ++j) {
//       R.coefficients()[j] = 0;
//     }
//   }

//   PUGS_INLINE
//   constexpr friend PolynomialP<N + 1>
//   primitive(const PolynomialP<N>& P)
//   {
//     TinyVector<N + 2> coefs;
//     for (size_t i = 0; i < N + 1; ++i) {
//       coefs[i + 1] = P.coefficients()[i] / double(i + 1);
//     }
//     coefs[0] = 0;
//     return PolynomialP<N + 1>{coefs};
//   }

//   PUGS_INLINE
//   constexpr friend std::ostream&
//   operator<<(std::ostream& os, const PolynomialP<N>& P)
//   {
//     //    os << "P(x) = ";
//     bool all_coef_zero = true;
//     if (N == 0) {
//       os << P.coefficients()[0];
//       return os;
//     }
//     if (N != 1) {
//       if (P.coefficients()[N] != 0.) {
//         if (P.coefficients()[N] < 0.) {
//           os << "- ";
//         }
//         if (P.coefficients()[N] != 1 && P.coefficients()[N] != -1) {
//           os << std::abs(P.coefficients()[N]);
//         }
//         os << "x^" << N;
//         all_coef_zero = false;
//       }
//     }
//     for (size_t i = N - 1; i > 1; --i) {
//       if (P.coefficients()[i] != 0.) {
//         if (P.coefficients()[i] > 0.) {
//           os << " + ";
//         } else if (P.coefficients()[i] < 0.) {
//           os << " - ";
//         }
//         if (P.coefficients()[i] != 1 && P.coefficients()[i] != -1) {
//           os << std::abs(P.coefficients()[i]);
//         }
//         os << "x^" << i;
//         all_coef_zero = false;
//       }
//     }
//     if (P.coefficients()[1] != 0.) {
//       if (P.coefficients()[1] > 0. && N != 1) {
//         os << " + ";
//       } else if (P.coefficients()[1] < 0.) {
//         os << " - ";
//       }
//       if (P.coefficients()[1] != 1 && P.coefficients()[1] != -1) {
//         os << std::abs(P.coefficients()[1]);
//       }
//       os << "x";
//       all_coef_zero = false;
//     }
//     if (P.coefficients()[0] != 0. || all_coef_zero) {
//       if (P.coefficients()[0] > 0.) {
//         os << " + ";
//       } else if (P.coefficients()[0] < 0.) {
//         os << " - ";
//       }
//       os << std::abs(P.coefficients()[0]);
//     }
//     return os;
//   }

//   PUGS_INLINE
//   constexpr friend void
//   lagrangeBasis(const TinyVector<N + 1> zeros, TinyVector<N + 1, PolynomialP<N>>& basis)
//   {
//     PolynomialP<N> lj;
//     for (size_t j = 0; j < N + 1; ++j) {
//       basis[j] = lagrangePolynomialP(zeros, j);
//     }
//   }

//   PUGS_INLINE
//   constexpr friend PolynomialP<N>
//   lagrangeToCanonical(const TinyVector<N + 1> lagrange_coefs, const TinyVector<N + 1, PolynomialP<N>>& basis)
//   {
//     PolynomialP<N> P(zero);
//     //    lagrangeBasis({0, 0, 0}, basis);
//     for (size_t j = 0; j < N + 1; ++j) {
//       P += basis[j] * lagrange_coefs[j];
//     }
//     return P;
//   }

// template <size_t N>
// PUGS_INLINE constexpr PolynomialP<N> lagrangePolynomialP(const TinyVector<N + 1> zeros, const size_t k);

// template <size_t N>
// PUGS_INLINE constexpr TinyVector<N, PolynomialP<N - 1>>
// lagrangeBasis(const TinyVector<N>& zeros)
// {
//   static_assert(N >= 1, "invalid degree");
//   TinyVector<N, PolynomialP<N - 1>> basis;
//   PolynomialP<N - 1> lj;
//   for (size_t j = 0; j < N; ++j) {
//     basis[j] = lagrangePolynomialP<N - 1>(zeros, j);
//   }
//   return basis;
// }

// template <size_t N>
// PUGS_INLINE constexpr double
// integrate(const PolynomialP<N>& P, const double& xinf, const double& xsup)
// {
//   PolynomialP<N + 1> Q = primitive(P);
//   return (Q(xsup) - Q(xinf));
// }

// template <size_t N>
// PUGS_INLINE constexpr double
// symmetricIntegrate(const PolynomialP<N>& P, const double& delta)
// {
//   Assert(delta > 0, "A positive delta is needed for symmetricIntegrate");
//   double integral = 0.;
//   for (size_t i = 0; i <= N; ++i) {
//     if (i % 2 == 0)
//       integral += 2. * P.coefficients()[i] * std::pow(delta, i + 1) / (i + 1);
//   }
//   return integral;
// }

// template <size_t N>
// PUGS_INLINE constexpr auto
// derivative(const PolynomialP<N>& P)
// {
//   if constexpr (N == 0) {
//     return PolynomialP<0>(0);
//   } else {
//     TinyVector<N> coefs;
//     for (size_t i = 0; i < N; ++i) {
//       coefs[i] = double(i + 1) * P.coefficients()[i + 1];
//     }
//     return PolynomialP<N - 1>(coefs);
//   }
// }

// template <size_t N>
// PUGS_INLINE constexpr PolynomialP<N>
// lagrangePolynomialP(const TinyVector<N + 1> zeros, const size_t k)
// {
//   for (size_t i = 0; i < N; ++i) {
//     Assert(zeros[i] < zeros[i + 1], "Interpolation values must be strictly increasing in Lagrange polynomialPs");
//   }
//   PolynomialP<N> lk;
//   lk.coefficients()    = zero;
//   lk.coefficients()[0] = 1;
//   for (size_t i = 0; i < N + 1; ++i) {
//     if (k == i)
//       continue;
//     double factor = 1. / (zeros[k] - zeros[i]);
//     PolynomialP<1> P({-zeros[i] * factor, factor});
//     lk *= P;
//   }
//   return lk;
// }

#endif   // POLYNOMIALP_HPP