#ifndef TAYLOR_POLYNOMIAL_HPP
#define TAULOR_POLYNOMIAL_HPP

#include <algebra/TinyVector.hpp>
#include <analysis/CubeGaussQuadrature.hpp>
#include <analysis/GaussQuadratureDescriptor.hpp>
#include <analysis/QuadratureManager.hpp>
#include <analysis/SquareGaussQuadrature.hpp>
#include <analysis/TriangleGaussQuadrature.hpp>
#include <geometry/SquareTransformation.hpp>
#include <geometry/TriangleTransformation.hpp>
#include <utils/Exceptions.hpp>

template <size_t N, size_t Dimension>
class TaylorPolynomial
{
 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;
  TinyVector<Dimension> m_x0;
  static_assert((N >= 0), "TaylorPolynomial degree must be non-negative");
  static_assert((Dimension > 0), "TaylorPolynomial dimension must be positive");
  static_assert((Dimension <= 3), "TaylorPolynomial 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 const TinyVector<Dimension, double>&
  x0() const
  {
    return m_x0;
  }

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

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

  PUGS_INLINE constexpr TaylorPolynomial(const TinyVector<size_coef, double>& coefficients,
                                         const TinyVector<Dimension, double>& x0) noexcept
    : m_coefficients{coefficients}, m_x0(x0)
  {}

  PUGS_INLINE
  constexpr TaylorPolynomial(TinyVector<size_coef, double>&& coefficients,
                             const TinyVector<Dimension, double>&& x0) noexcept
    : m_coefficients{coefficients}, m_x0(x0)
  {}

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

  PUGS_INLINE constexpr TaylorPolynomial
  operator+(const TaylorPolynomial Q) const
  {
    Assert(m_x0 == Q.m_x0, "You cannot add Taylor polynomials with different origins");
    TaylorPolynomial<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 TaylorPolynomial
  operator-() const
  {
    TaylorPolynomial<N, Dimension> P;
    P.coefficients() = -coefficients();
    return P;
  }

  PUGS_INLINE constexpr TaylorPolynomial
  operator-(const TaylorPolynomial Q) const
  {
    Assert(m_x0 == Q.m_x0, "You cannot subtract Taylor polynomials with different origins");
    TaylorPolynomial<N, Dimension> P(m_coefficients);
    P = P + (-Q);
    return P;
  }

  template <size_t M, size_t Dim>
  PUGS_INLINE constexpr TaylorPolynomial&
  operator=(const TaylorPolynomial<M, Dim>& Q)
  {
    coefficients() = zero;
    for (size_t i = 0; i < size_coef; ++i) {
      coefficients()[i] = Q.coefficients()[i];
    }
    m_x0 = Q.m_x0;
    return *this;
  }

  PUGS_INLINE constexpr TaylorPolynomial&
  operator+=(const TaylorPolynomial& Q)
  {
    Assert(m_x0 == Q.m_x0, "You cannot add Taylor polynomials with different origins");
    m_coefficients += Q.coefficients();
    return *this;
  }

  template <size_t M>
  PUGS_INLINE constexpr TaylorPolynomial&
  operator-=(const TaylorPolynomial& Q)
  {
    Assert(m_x0 == Q.m_x0, "You cannot subtract Taylor polynomials with different origins");
    m_coefficients -= Q.coefficients();
    return *this;
  }

  PUGS_INLINE
  constexpr TaylorPolynomial
  operator*(const double& lambda) const
  {
    TinyVector<size_coef> mult_coefs = lambda * m_coefficients;
    TaylorPolynomial<N, Dimension> M(mult_coefs);
    return M;
  }

  PUGS_INLINE
  constexpr friend TaylorPolynomial<N, Dimension>
  operator*(const double& lambda, const TaylorPolynomial<N, Dimension> P)
  {
    return P * lambda;
  }

  PUGS_INLINE
  constexpr double
  operator[](const TinyVector<Dimension, size_t> relative_pos) const
  {
    size_t total_degree = 0;
    for (size_t i = 0; i < Dimension; ++i) {
      Assert((relative_pos[i] <= N),
             "You are looking for a coefficient of order greater than the degree of the polynomial");
      total_degree += relative_pos[i];
    }
    Assert((total_degree <= N), "The sum of the degrees of the coefficient you are looking for is greater than the "
                                "degree of the polynomial");
    TinyVector<size_coef> absolute_coefs = this->coefficients();
    size_t absolute_position             = 0;
    if constexpr (Dimension == 1) {
      absolute_position = relative_pos[0];
    } else if constexpr (Dimension == 2) {
      size_t total_degree = relative_pos[0] + relative_pos[1];
      absolute_position   = total_degree * (total_degree + 1) / 2 + relative_pos[1];
    } else {
      // throw NotImplementedError("Not yet Available in 3D");
      static_assert(Dimension == 3);
      size_t total_degree     = relative_pos[0] + relative_pos[1] + relative_pos[2];
      size_t total_sub_degree = relative_pos[1] + relative_pos[2];
      return total_degree * (total_degree + 1) * (total_degree + 2) / 6 +
             total_sub_degree * (total_sub_degree + 1) / 2 + relative_pos[2];
    }

    return absolute_coefs[absolute_position];
  }

  PUGS_INLINE
  constexpr double
  operator[](const TinyVector<Dimension, size_t> relative_pos)
  {
    size_t total_degree = 0;
    for (size_t i = 0; i < Dimension; ++i) {
      Assert((relative_pos[i] <= N),
             "You are looking for a coefficient of order greater than the degree of the polynomial");
      total_degree += relative_pos[i];
    }
    Assert((total_degree <= N), "The sum of the degrees of the coefficient you are looking for is greater than the "
                                "degree of the polynomial");
    TinyVector<size_coef> absolute_coefs = this->coefficients();
    size_t absolute_position             = 0;
    if constexpr (Dimension == 1) {
      absolute_position = relative_pos[0];
    } else if constexpr (Dimension == 2) {
      size_t total_degree = relative_pos[0] + relative_pos[1];
      absolute_position   = total_degree * (total_degree + 1) / 2 + relative_pos[1];
    } else {
      // throw NotImplementedError("Not yet Available in 3D");
      static_assert(Dimension == 3);
      size_t total_degree     = relative_pos[0] + relative_pos[1] + relative_pos[2];
      size_t total_sub_degree = relative_pos[1] + relative_pos[2];
      absolute_position       = total_degree * (total_degree + 1) * (total_degree + 2) / 6 +
                          total_sub_degree * (total_sub_degree + 1) / 2 + relative_pos[2];
    }

    return absolute_coefs[absolute_position];
  }

  PUGS_INLINE
  constexpr double
  absolute_position(const TinyVector<Dimension, size_t> relative_pos) const
  {
    size_t total_degree = 0;
    for (size_t i = 0; i < Dimension; ++i) {
      Assert((relative_pos[i] <= N),
             "You are looking for a coefficient of order greater than the degree of the polynomial");
      total_degree += relative_pos[i];
    }
    Assert((total_degree <= N), "The sum of the degrees of the coefficient you are looking for is greater than the "
                                "degree of the polynomial");
    size_t abs_pos = 0;
    if constexpr (Dimension == 1) {
      abs_pos = relative_pos[0];
    } else if constexpr (Dimension == 2) {
      abs_pos = total_degree * (total_degree + 1) / 2 + relative_pos[1];
    } else {
      static_assert(Dimension == 3);
      size_t total_degree     = relative_pos[0] + relative_pos[1] + relative_pos[2];
      size_t total_sub_degree = relative_pos[1] + relative_pos[2];
      abs_pos                 = total_degree * (total_degree + 1) * (total_degree + 2) / 6 +
                total_sub_degree * (total_sub_degree + 1) / 2 + relative_pos[2];
      // throw NotImplementedError("Not yet Available in 3D");
    }

    return abs_pos;
  }

  PUGS_INLINE
  constexpr double
  operator()(const TinyVector<Dimension> x) const
  {
    const auto& P = *this;
    double value  = 0.;
    if constexpr (Dimension == 1) {
      value = m_coefficients[N];
      for (size_t i = N; i > 0; --i) {
        value *= x - m_x0;
        value += m_coefficients[i - 1];
      }
    } else if constexpr (Dimension == 2) {
      TinyVector<Dimension, size_t> relative_pos(0, N);
      value = P[relative_pos];
      for (size_t i = N; i > 0; --i) {
        value *= (x[1] - m_x0[1]);
        relative_pos  = TinyVector<Dimension, size_t>(N - i + 1, i - 1);
        double valuex = P[relative_pos];
        for (size_t j = N - i + 1; j > 0; --j) {
          valuex *= (x[0] - m_x0[0]);
          relative_pos = TinyVector<Dimension, size_t>(j - 1, i - 1);
          valuex += P[relative_pos];
        }
        value += valuex;
      }
    } else {
      throw NotImplementedError("Not yet Available in 3D");
    }

    return value;
  }

  PUGS_INLINE size_t
  find_size_coef(const size_t degree)
  {
    if constexpr (Dimension == 1) {
      return degree + 1;
    } else if constexpr (Dimension == 2) {
      return (degree + 1) * (degree + 2) / 2;
    } else {
      static_assert(Dimension == 3);
      return (degree + 1) * (degree + 2) * (degree + 3) / 6;
    }
  }

  PUGS_INLINE constexpr TaylorPolynomial<N, Dimension>
  derivative(const size_t var) const
  {
    const auto P = *this;
    TinyVector<size_coef> coefs(zero);
    TaylorPolynomial<N, Dimension> Q(coefs);
    if constexpr (N != 0) {
      Assert(var < Dimension,
             "You can not derive a polynomial with respect to a variable of rank greater than the dimension");
      if constexpr (Dimension == 1) {
        for (size_t i = 0; i < size_coef - 1; ++i) {
          coefs[i] = double(i + 1) * P.coefficients()[i + 1];
        }
      } else if constexpr (Dimension == 2) {
        if (var == 0) {
          for (size_t i = 0; i < N; ++i) {
            for (size_t j = 0; j < N - i; ++j) {
              TinyVector<Dimension, size_t> relative_pos(i, j);
              TinyVector<Dimension, size_t> relative_posp(i + 1, j);
              size_t absolute_position            = Q.absolute_position(relative_pos);
              size_t absolute_positionp           = P.absolute_position(relative_posp);
              Q.coefficients()[absolute_position] = double(i + 1) * m_coefficients[absolute_positionp];
            }
          }
        } else {
          for (size_t i = 0; i < N; ++i) {
            for (size_t j = 0; j < N - i; ++j) {
              TinyVector<Dimension, size_t> relative_pos(i, j);
              TinyVector<Dimension, size_t> relative_posp(i, j + 1);
              size_t absolute_position            = Q.absolute_position(relative_pos);
              size_t absolute_positionp           = P.absolute_position(relative_posp);
              Q.coefficients()[absolute_position] = double(j + 1) * m_coefficients[absolute_positionp];
            }
          }
        }
      } else {
        static_assert(Dimension == 3);
        if (var == 0) {
          for (size_t i = 0; i < N; ++i) {
            for (size_t j = 0; j < N - i; ++j) {
              for (size_t k = 0; k < N - i - j; ++k) {
                TinyVector<Dimension, size_t> relative_pos(i, j, k);
                TinyVector<Dimension, size_t> relative_posp(i + 1, j, k);
                size_t absolute_position            = Q.absolute_position(relative_pos);
                size_t absolute_positionp           = P.absolute_position(relative_posp);
                Q.coefficients()[absolute_position] = double(i + 1) * m_coefficients[absolute_positionp];
              }
            }
          }
        } else if (var == 1) {
          for (size_t i = 0; i < N; ++i) {
            for (size_t j = 0; j < N - i; ++j) {
              for (size_t k = 0; k < N - i - j; ++k) {
                TinyVector<Dimension, size_t> relative_pos(i, j, k);
                TinyVector<Dimension, size_t> relative_posp(i, j + 1, k);
                size_t absolute_position            = Q.absolute_position(relative_pos);
                size_t absolute_positionp           = P.absolute_position(relative_posp);
                Q.coefficients()[absolute_position] = double(j + 1) * m_coefficients[absolute_positionp];
              }
            }
          }
        } else {
          for (size_t i = 0; i < N; ++i) {
            for (size_t j = 0; j < N - i; ++j) {
              for (size_t k = 0; k < N - i - j; ++k) {
                TinyVector<Dimension, size_t> relative_pos(i, j, k);
                TinyVector<Dimension, size_t> relative_posp(i, j, k + 1);
                size_t absolute_position            = Q.absolute_position(relative_pos);
                size_t absolute_positionp           = P.absolute_position(relative_posp);
                Q.coefficients()[absolute_position] = double(k + 1) * m_coefficients[absolute_positionp];
              }
            }
          }
        }
        // throw NotImplementedError("Not yet Available in 3D");
      }
    }
    return Q;
  }

  PUGS_INLINE
  constexpr friend std::ostream&
  operator<<(std::ostream& os, const TaylorPolynomial<N, Dimension>& P)
  {
    //    os << "P(x) = ";
    bool all_coef_zero = true;
    if (N == 0) {
      os << P.coefficients()[0];
      return os;
    }
    if constexpr (Dimension == 1) {
      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 - " << P.x0()[0] << ")"
             << "^" << 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 - " << P.x0()[0] << ")"
             << "^" << 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 - " << P.x0()[0] << ")";
        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;
    } else if constexpr (Dimension == 2) {
      size_t i = 0;
      size_t j = N;
      TinyVector<Dimension, size_t> rel_pos(i, j);
      double coef = P[rel_pos];
      if (coef != 0.) {
        if (coef < 0.) {
          os << " - ";
        }
        if (coef != 1 && coef != -1) {
          os << std::abs(coef);
        }
        os << "(y - " << P.x0()[1] << ")"
           << "^" << N;
      }
      size_t degree = N;
      for (size_t k = size_coef - 1; k > 0; --k) {
        if (j > 0) {
          j--;
          i++;
        } else {
          degree--;
          j = degree;
          i = 0;
        }
        rel_pos     = TinyVector<Dimension, size_t>(i, j);
        double coef = P[rel_pos];
        if (coef != 0.) {
          if (coef > 0.) {
            os << " + ";
          } else if (coef < 0.) {
            os << " - ";
          }
          if ((coef != 1 && coef != -1) || (i == 0 && j == 0)) {
            os << std::abs(coef);
          }
          if (i == 0 && j == 0)
            continue;
          if (i == 0) {
            if (j != 1) {
              os << "(y - " << P.x0()[1] << ")"
                 << "^" << j;
            } else {
              os << "y";
            }
          } else if (j == 0) {
            if (i == 1) {
              os << "(x - " << P.x0()[0] << ")";
            } else {
              os << "(x - " << P.x0()[0] << ")"
                 << "^" << i;
            }
          } else {
            if (i == 1 && j == 1) {
              os << "(x - " << P.x0()[0] << ")"
                 << "(y - " << P.x0()[1] << ")";
            } else if (i == 1) {
              os << "(x - " << P.x0()[0] << ")"
                 << "(y - " << P.x0()[1] << ")^" << j;
            } else if (j == 1) {
              os << "(x - " << P.x0()[0] << ")"
                 << "^i"
                 << "(y - " << P.x0()[1] << ")";
            } else {
              os << "(x - " << P.x0()[0] << ")"
                 << "^i"
                 << "(y - " << P.x0()[1] << ")^" << j;
            }
          }
          all_coef_zero = false;
        }
      }

      return os;
    } else {
      // size_t i = 0;
      // size_t j = 0;
      // size_t k = N;
      // TinyVector<Dimension, size_t> rel_pos(i, j, k);
      // double coef = P[rel_pos];
      // if (coef != 0.) {
      //   if (coef < 0.) {
      //     os << " - ";
      //   }
      //   if (coef != 1 && coef != -1) {
      //     os << std::abs(coef);
      //   }
      //   os << "z^" << N;
      // }
      // size_t degree = N;
      // for (size_t l = size_coef - 1; l > 0; --l) {
      //   if (k > 0) {
      //     k--;
      //     if (j < k) {
      //       j++;
      //     } else {
      //       j--;
      //       i++;
      //     }
      //   } else {
      //     degree--;
      //     k = degree;
      //     i = 0;
      //     j = 0;
      //   }

      //   rel_pos     = TinyVector<Dimension, size_t>(i, j, k);
      //   double coef = P[rel_pos];
      //   if (coef != 0.) {
      //     if (coef > 0.) {
      //       os << " + ";
      //     } else if (coef < 0.) {
      //       os << " - ";
      //     }
      //     if ((coef != 1 && coef != -1) || (i == 0 && j == 0 && k == 0)) {
      //       os << std::abs(coef);
      //     }
      //     if (i == 0 && j == 0 && k == 0)
      //       continue;
      //     if (i == 0 && j == 0) {
      //       if (k != 1) {
      //         os << "z^" << j;
      //       } else {
      //         os << "z";
      //       }
      //     } else if (i == 0 && k == 0) {
      //       if (j == 1) {
      //         os << "y";
      //       } else {
      //         os << "y^" << i;
      //       }
      //     } else if (j == 0 && k == 0) {
      //       if (i == 1) {
      //         os << "x";
      //       } else {
      //         os << "x^" << i;
      //       }
      //     } else {
      //       if (i == 1 && j == 1 && k == 1) {
      //         os << "xyz";
      //       } else if (i == 1) {
      //         os << "x"
      //            << "y^" << j << "z^" << k;
      //       } else if (j == 1) {
      //         os << "x^" << i << "y"
      //            << "z^" << k;
      //       } else if (k == 1) {
      //         os << "x^" << i << "y^" << j << "z";
      //       } else {
      //         os << "x^" << i << "y^" << j << "z^" << k;
      //       }
      //     }
      //     all_coef_zero = false;
      //   }
      //
      for (size_t l = 0; l < size_coef; ++l) {
        double coef = P.coefficients()[l];
        os << coef << " ";
      }
      return os;
      //      throw NotImplementedError("Not yet Available in 3D");
    }
  }

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

template <size_t N, size_t Dimension, size_t P>
PUGS_INLINE double
integrate(const TaylorPolynomial<N, Dimension> Q, const std::array<TinyVector<Dimension>, P>& positions)
{
  double integral = 0.;
  static_assert(P > 1, "For the integration, number of positions should be greater or equal to 2");
  static_assert(N >= 0, "invalid degree");
  if constexpr (Dimension == 1) {
    static_assert(P == 2, "In 1D number of positions should be 2");
    throw NotImplementedError("Not yet Available in 1D");
  } else if constexpr (Dimension == 2) {
    static_assert(P <= 4, "In 2D number of positions should be lesser or equal to 4");
    if constexpr (P == 2) {
    } else if constexpr (P == 3) {
      const QuadratureFormula<2>& lN = QuadratureManager::instance().getTriangleFormula(GaussQuadratureDescriptor(N));
      auto point_list                = lN.pointList();
      auto weight_list               = lN.weightList();
      TriangleTransformation<2> t{positions[0], positions[1], positions[2]};
      auto value = weight_list[0] * Q(t(point_list[0]));
      for (size_t i = 1; i < weight_list.size(); ++i) {
        value += weight_list[i] * Q(t(point_list[i]));
      }
      integral = value;
    } else {
      const QuadratureFormula<2>& lN = QuadratureManager::instance().getSquareFormula(GaussQuadratureDescriptor(N));
      auto point_list                = lN.pointList();
      auto weight_list               = lN.weightList();
      SquareTransformation<2> s{positions[0], positions[1], positions[2], positions[3]};
      auto value = weight_list[0] * Q(s(point_list[0]));
      for (size_t i = 1; i < weight_list.size(); ++i) {
        value += weight_list[i] * Q(s(point_list[i]));
      }
      integral = value;
    }
  } else {
    static_assert(Dimension == 3, "Dimension should be <=3");
    throw NotImplementedError("Not yet Available in 3D");
  }
  return integral;
}

#endif   // POLYNOMIALP_HPP