#ifndef BEZIER_COM_TRAJ_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH
#define BEZIER_COM_TRAJ_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH

/*
 * Copyright 2018, LAAS-CNRS
 * Author: Pierre Fernbach
 */

#include <hpp/bezier-com-traj/data.hh>

namespace bezier_com_traj {
namespace c0_dc0_ddc0_j0_j1_ddc1_dc1_c1 {

static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC |
                                   END_VEL | END_POS | INIT_JERK | END_JERK;
static const size_t DIM_VAR = 3;
static const size_t DIM_POINT = 3;
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND
/// ACCELERATION AND JERK (DEGREE = 8)
///
/**
 * @brief evaluateCurveAtTime compute the expression of the point on the curve
 * at t, defined by the waypoint pi and one free waypoint (x)
 * @param pi constant waypoints of the curve, assume pi[8] pi[1] pi[2] pi[3] x
 * pi[4] pi[5] pi[6] pi[7]
 * @param t param (normalized !)
 * @return the expression of the waypoint such that wp.first . x + wp.second =
 * point on curve
 */
inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
  coefs_t wp;
  const double t2 = t * t;
  const double t3 = t2 * t;
  const double t4 = t3 * t;
  const double t5 = t4 * t;
  const double t6 = t5 * t;
  const double t7 = t6 * t;
  const double t8 = t7 * t;
  // equation found with sympy
  wp.first = 70.0 * t8 - 280.0 * t7 + 420.0 * t6 - 280.0 * t5 + 70.0 * t4;
  wp.second = 1.0 * pi[8] * t8 - 8.0 * pi[8] * t7 + 28.0 * pi[8] * t6 -
              56.0 * pi[8] * t5 + 70.0 * pi[8] * t4 - 56.0 * pi[8] * t3 +
              28.0 * pi[8] * t2 - 8.0 * pi[8] * t + 1.0 * pi[8] -
              8.0 * pi[1] * t8 + 56.0 * pi[1] * t7 - 168.0 * pi[1] * t6 +
              280.0 * pi[1] * t5 - 280.0 * pi[1] * t4 + 168.0 * pi[1] * t3 -
              56.0 * pi[1] * t2 + 8.0 * pi[1] * t + 28.0 * pi[2] * t8 -
              168.0 * pi[2] * t7 + 420.0 * pi[2] * t6 - 560.0 * pi[2] * t5 +
              420.0 * pi[2] * t4 - 168.0 * pi[2] * t3 + 28.0 * pi[2] * t2 -
              56.0 * pi[3] * pow(t, 8) + 280.0 * pi[3] * t7 -
              560.0 * pi[3] * t6 + 560.0 * pi[3] * t5 - 280.0 * pi[3] * t4 +
              56.0 * pi[3] * t3 - 56.0 * pi[5] * t8 + 168.0 * pi[5] * t7 -
              168.0 * pi[5] * t6 + 56.0 * pi[5] * pow(t, 5) +
              28.0 * pi[6] * t8 - 56.0 * pi[6] * t7 + 28.0 * pi[6] * t6 -
              8.0 * pi[7] * t8 + 8.0 * pi[7] * t7 + 1.0 * pi[8] * t8;
  return wp;
}

inline coefs_t evaluateVelocityCurveAtTime(const std::vector<point_t>& pi,
                                           double T, double t) {
  coefs_t wp;
  const double alpha = 1. / (T);
  const double t2 = t * t;
  const double t3 = t2 * t;
  const double t4 = t3 * t;
  const double t5 = t4 * t;
  const double t6 = t5 * t;
  const double t7 = t6 * t;
  // equation found with sympy
  wp.first =
      (560.0 * t7 - 1960.0 * t6 + 2520.0 * t5 - 1400.0 * t4 + 280.0 * t3) *
      alpha;
  wp.second = (8.0 * pi[8] * t7 - 56.0 * pi[8] * t6 + 168.0 * pi[8] * t5 -
               280.0 * pi[8] * t4 + 280.0 * pi[8] * t3 - 168.0 * pi[8] * t2 +
               56.0 * pi[8] * t - 8.0 * pi[8] - 64.0 * pi[1] * t7 +
               392.0 * pi[1] * t6 - 1008.0 * pi[1] * t5 + 1400.0 * pi[1] * t4 -
               1120.0 * pi[1] * t3 + 504.0 * pi[1] * t2 - 112.0 * pi[1] * t +
               8.0 * pi[1] + 224.0 * pi[2] * t7 - 1176.0 * pi[2] * t6 +
               2520.0 * pi[2] * t5 - 2800.0 * pi[2] * t4 + 1680.0 * pi[2] * t3 -
               504.0 * pi[2] * t2 + 56.0 * pi[2] * t - 448.0 * pi[3] * t7 +
               1960.0 * pi[3] * t6 - 3360.0 * pi[3] * t5 + 2800.0 * pi[3] * t4 -
               1120.0 * pi[3] * t3 + 168.0 * pi[3] * t2 - 448.0 * pi[5] * t7 +
               1176.0 * pi[5] * t6 - 1008.0 * pi[5] * t5 + 280.0 * pi[5] * t4 +
               224.0 * pi[6] * t7 - 392.0 * pi[6] * t6 + 168.0 * pi[6] * t5 -
               64.0 * pi[7] * t7 + 56.0 * pi[7] * t6 + 8.0 * pi[8] * t7) *
              alpha;
  return wp;
}

inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
                                               double T, double t) {
  coefs_t wp;
  const double alpha = 1. / (T * T);
  const double t2 = t * t;
  const double t3 = t2 * t;
  const double t4 = t3 * t;
  const double t5 = t4 * t;
  const double t6 = t5 * t;
  // equation found with sympy
  wp.first =
      ((3920.0 * t6 - 11760.0 * t5 + 12600.0 * t4 - 5600.0 * t3 + 840.0 * t2)) *
      alpha;
  wp.second = (56.0 * pi[8] * t6 - 336.0 * pi[8] * t5 + 840.0 * pi[8] * t4 -
               1120.0 * pi[8] * t3 + 840.0 * pi[8] * t2 - 336.0 * pi[8] * t +
               56.0 * pi[8] - 448.0 * pi[1] * t6 + 2352.0 * pi[1] * t5 -
               5040.0 * pi[1] * t4 + 5600.0 * pi[1] * t3 - 3360.0 * pi[1] * t2 +
               1008.0 * pi[1] * t - 112.0 * pi[1] + 1568.0 * pi[2] * t6 -
               7056.0 * pi[2] * t5 + 12600.0 * pi[2] * t4 -
               11200.0 * pi[2] * t3 + 5040.0 * pi[2] * t2 - 1008.0 * pi[2] * t +
               56.0 * pi[2] - 3136.0 * pi[3] * t6 + 11760.0 * pi[3] * t5 -
               16800.0 * pi[3] * t4 + 11200.0 * pi[3] * t3 -
               3360.0 * pi[3] * t2 + 336.0 * pi[3] * t - 3136.0 * pi[5] * t6 +
               7056.0 * pi[5] * t5 - 5040.0 * pi[5] * t4 + 1120.0 * pi[5] * t3 +
               1568.0 * pi[6] * t6 - 2352.0 * pi[6] * t5 + 840.0 * pi[6] * t4 -
               448.0 * pi[7] * t6 + 336.0 * pi[7] * t5 + 56.0 * pi[8] * t6) *
              alpha;
  return wp;
}

inline coefs_t evaluateJerkCurveAtTime(const std::vector<point_t>& pi, double T,
                                       double t) {
  coefs_t wp;
  const double alpha = 1. / (T * T * T);
  const double t2 = t * t;
  const double t3 = t2 * t;
  const double t4 = t3 * t;
  const double t5 = t4 * t;
  // equation found with sympy
  wp.first =
      (23520.0 * t5 - 58800.0 * t4 + 50400.0 * t3 - 16800.0 * t2 + 1680.0 * t) *
      alpha;
  wp.second =
      1.0 *
      (336.0 * pi[0] * t5 - 1680.0 * pi[0] * t4 + 3360.0 * pi[0] * t3 -
       3360.0 * pi[0] * t2 + 1680.0 * pi[0] * t - 336.0 * pi[0] -
       2688.0 * pi[1] * t5 + 11760.0 * pi[1] * t4 - 20160.0 * pi[1] * t3 +
       16800.0 * pi[1] * t2 - 6720.0 * pi[1] * t + 1008.0 * pi[1] +
       9408.0 * pi[2] * t5 - 35280.0 * pi[2] * t4 + 50400.0 * pi[2] * t3 -
       33600.0 * pi[2] * t2 + 10080.0 * pi[2] * t - 1008.0 * pi[2] -
       18816.0 * pi[3] * t5 + 58800.0 * pi[3] * t4 - 67200.0 * pi[3] * t3 +
       33600.0 * pi[3] * t2 - 6720.0 * pi[3] * t + 336.0 * pi[3] -
       18816.0 * pi[5] * t5 + 35280.0 * pi[5] * t4 - 20160.0 * pi[5] * t3 +
       3360.0 * pi[5] * t2 + 9408.0 * pi[6] * t5 - 11760.0 * pi[6] * t4 +
       3360.0 * pi[6] * t3 - 2688.0 * pi[7] * t5 + 1680.0 * pi[7] * t4 +
       336.0 * pi[8] * t5) *
      alpha;
  return wp;
}
inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi,
                                              double t) {
  coefs_t coef = evaluateCurveAtTime(pi, t);
  waypoint_t wp;
  wp.first = Matrix3::Identity() * coef.first;
  wp.second = coef.second;
  return wp;
}
inline waypoint_t evaluateVelocityCurveWaypointAtTime(
    const std::vector<point_t>& pi, double T, double t) {
  coefs_t coef = evaluateVelocityCurveAtTime(pi, T, t);
  waypoint_t wp;
  wp.first = Matrix3::Identity() * coef.first;
  wp.second = coef.second;
  return wp;
}
inline waypoint_t evaluateAccelerationCurveWaypointAtTime(
    const std::vector<point_t>& pi, double T, double t) {
  coefs_t coef = evaluateAccelerationCurveAtTime(pi, T, t);
  waypoint_t wp;
  wp.first = Matrix3::Identity() * coef.first;
  wp.second = coef.second;
  return wp;
}

inline waypoint_t evaluateJerkCurveWaypointAtTime(
    const std::vector<point_t>& pi, double T, double t) {
  coefs_t coef = evaluateJerkCurveAtTime(pi, T, t);
  waypoint_t wp;
  wp.first = Matrix3::Identity() * coef.first;
  wp.second = coef.second;
  return wp;
}

inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
                                                     double T) {
  // equation for constraint on initial and final position and velocity and
  // initial acceleration(degree 5, 5 constant waypoint and one free (pi[3]))
  // first, compute the constant waypoints that only depend on pData :
  double n = 8.;
  std::vector<point_t> pi;
  pi.push_back(pData.c0_);
  pi.push_back((pData.dc0_ * T / n) + pData.c0_);
  pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
               (2 * pData.dc0_ * T / n) +
               pData.c0_);  // * T because derivation make a T appear
  pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) +
               (3 * pData.ddc0_ * T * T / (n * (n - 1))) +
               (3 * pData.dc0_ * T / n) + pData.c0_);
  pi.push_back(point_t::Zero());
  pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) +
               (3 * pData.ddc1_ * T * T / (n * (n - 1))) -
               (3 * pData.dc1_ * T / n) + pData.c1_);  // * T ??
  pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) -
               (2 * pData.dc1_ * T / n) + pData.c1_);  // * T ??
  pi.push_back((-pData.dc1_ * T / n) + pData.c1_);     // * T ?
  pi.push_back(pData.c1_);
  return pi;
}

inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
                                                double T) {
  bezier_wp_t::t_point_t wps;
  const int DIM_POINT = 6;
  const int DIM_VAR = 3;
  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
  std::vector<Matrix3> Cpi;
  for (std::size_t i = 0; i < pi.size(); ++i) {
    Cpi.push_back(skew(pi[i]));
  }
  const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
  const Matrix3 Cg = skew(g);
  const double T2 = T * T;
  const double alpha = 1 / (T2);

  // equation of waypoints for curve w found with sympy
  waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
  w0.second.head<3>() = (30 * pi[0] - 60 * pi[1] + 30 * pi[2]) * alpha;
  w0.second.tail<3>() =
      1.0 *
      (1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.0 * Cpi[0] * pi[2]) *
      alpha;
  wps.push_back(w0);
  waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
  w1.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
  w1.first.block<3, 3>(3, 0) = 13.3333333333333 * Cpi[0] * alpha;
  w1.second.head<3>() =
      1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
  w1.second.tail<3>() = 1.0 *
                        (0.333333333333333 * Cg * T2 * pi[0] +
                         0.666666666666667 * Cg * T2 * pi[1] -
                         30.0 * Cpi[0] * pi[2] + 20.0 * Cpi[1] * pi[2]) *
                        alpha;
  wps.push_back(w1);
  waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
  w2.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
  w2.first.block<3, 3>(3, 0) =
      1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
  w2.second.head<3>() =
      1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
  w2.second.tail<3>() =
      1.0 *
      (0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
       0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] -
       20.0 * Cpi[1] * pi[2]) *
      alpha;
  wps.push_back(w2);
  waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
  w3.first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
  w3.first.block<3, 3>(3, 0) = 1.0 *
                               (0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] +
                                14.2857142857143 * Cpi[2]) *
                               alpha;
  w3.second.head<3>() = 1.0 *
                        (3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] -
                         14.2857142857143 * pi[2] + 7.85714285714286 * pi[4] +
                         1.42857142857143 * pi[5]) *
                        alpha;
  w3.second.tail<3>() =
      1.0 *
      (0.0119047619047619 * Cg * T2 * pi[0] +
       0.214285714285714 * Cg * T2 * pi[1] +
       0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
       1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
      alpha;
  wps.push_back(w3);
  waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
  w4.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
  w4.first.block<3, 3>(3, 0) =
      1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
  w4.second.head<3>() = 1.0 *
                        (1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] -
                         3.57142857142857 * pi[2] + 5.0 * pi[4] +
                         4.28571428571429 * pi[5] + 0.238095238095238 * pi[6]) *
                        alpha;
  w4.second.tail<3>() =
      1.0 *
      (0.0476190476190471 * Cg * T2 * pi[1] +
       0.357142857142857 * Cg * T2 * pi[2] +
       0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] +
       0.238095238095238 * Cpi[0] * pi[6] - 12.8571428571429 * Cpi[1] * pi[4] +
       5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
      alpha;
  wps.push_back(w4);
  waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
  w5.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
  w5.first.block<3, 3>(3, 0) =
      1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
  w5.second.head<3>() = 1.0 *
                        (0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] +
                         5.0 * pi[2] - 3.57142857142857 * pi[4] +
                         7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
                        alpha;
  w5.second.tail<3>() =
      1.0 *
      (+0.11904761904762 * Cg * T2 * pi[2] +
       0.357142857142857 * Cg * T2 * pi[4] +
       0.0476190476190476 * Cg * T2 * pi[5] -
       0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
       1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] +
       12.8571428571429 * Cpi[2] * pi[5]) *
      alpha;
  wps.push_back(w5);
  waypoint_t w6 = initwp(DIM_POINT, DIM_VAR);
  w6.first.block<3, 3>(0, 0) = -5.71428571428571 * alpha * Matrix3::Identity();
  w6.first.block<3, 3>(3, 0) = 1.0 *
                               (0.238095238095238 * Cg * T2 +
                                14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) *
                               alpha;
  w6.second.head<3>() = 1.0 *
                        (1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] -
                         14.2857142857143 * pi[4] + 7.14285714285715 * pi[5] +
                         3.57142857142857 * pi[6]) *
                        alpha;
  w6.second.tail<3>() =
      1.0 *
      (0.535714285714286 * Cg * T2 * pi[4] +
       0.214285714285714 * Cg * T2 * pi[5] +
       0.0119047619047619 * Cg * T2 * pi[6] -
       1.42857142857143 * Cpi[1] * pi[6] - 12.8571428571429 * Cpi[2] * pi[5] +
       5.0 * Cpi[2] * pi[6]) *
      alpha;
  wps.push_back(w6);
  waypoint_t w7 = initwp(DIM_POINT, DIM_VAR);
  w7.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
  w7.first.block<3, 3>(3, 0) =
      1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
  w7.second.head<3>() =
      1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
  w7.second.tail<3>() =
      1.0 *
      (0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
       0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] +
       20.0 * Cpi[4] * pi[5]) *
      alpha;
  wps.push_back(w7);
  waypoint_t w8 = initwp(DIM_POINT, DIM_VAR);
  w8.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
  w8.first.block<3, 3>(3, 0) = 1.0 * (13.3333333333333 * Cpi[6]) * alpha;
  w8.second.head<3>() =
      1.0 *
      (-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) *
      alpha;
  w8.second.tail<3>() = 1.0 *
                        (0.666666666666667 * Cg * T2 * pi[5] +
                         0.333333333333333 * Cg * T2 * pi[6] -
                         20.0 * Cpi[4] * pi[5] + 30.0 * Cpi[4] * pi[6]) *
                        alpha;
  wps.push_back(w8);
  waypoint_t w9 = initwp(DIM_POINT, DIM_VAR);
  w9.second.head<3>() = (30 * pi[4] - 60 * pi[5] + 30 * pi[6]) * alpha;
  w9.second.tail<3>() =
      1.0 *
      (1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) *
      alpha;
  wps.push_back(w9);
  return wps;
}

std::vector<waypoint_t> computeVelocityWaypoints(
    const ProblemData& pData, const double T,
    std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);

  std::vector<waypoint_t> wps;
  assert(pi.size() == 9);

  double alpha = 1. / (T);
  waypoint_t w = initwp(DIM_POINT, DIM_VAR);
  // assign w0:
  w.second = alpha * 8 * (-pi[0] + pi[1]);
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w1:
  w.second = alpha * 8 * (-pi[1] + pi[2]);
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w2:
  w.second = alpha * 8 * (-pi[2] + pi[3]);
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w3:
  w.first = 8 * alpha * Matrix3::Identity();
  w.second = alpha * -8 * pi[3];
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w4:
  w.first = -8 * alpha * Matrix3::Identity();
  w.second = alpha * 8 * pi[5];
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w5:
  w.second = alpha * 8 * (-pi[5] + pi[6]);
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w6:
  w.second = alpha * 8 * (-pi[6] + pi[7]);
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w7:
  w.second = alpha * 8 * (-pi[7] + pi[8]);
  wps.push_back(w);
  return wps;
}

std::vector<waypoint_t> computeAccelerationWaypoints(
    const ProblemData& pData, const double T,
    std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);

  std::vector<waypoint_t> wps;
  assert(pi.size() == 9);
  double alpha = 1. / (T * T);

  waypoint_t w = initwp(DIM_POINT, DIM_VAR);

  // assign w0:
  w.second = 56 * alpha * (pi[0] - 2 * pi[1] + pi[2]);
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w1:
  w.second = 56 * alpha * (pi[1] - 2 * pi[2] + pi[3]);
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w2:
  w.first = 56 * alpha * Matrix3::Identity();
  w.second = (56 * pi[2] - 112 * pi[3]) * alpha;
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w3:
  w.first = -112 * alpha * Matrix3::Identity();
  w.second = (56 * pi[3] + 56 * pi[8]) * alpha;
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w4:
  w.first = 56 * alpha * Matrix3::Identity();
  w.second = (-112 * pi[5] + 56 * pi[6]) * alpha;
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w5:
  w.second = 56 * alpha * (pi[5] - 2 * pi[6] + pi[7]);
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w5:
  w.second = 56 * alpha * (pi[6] - 2 * pi[7] + pi[8]);
  wps.push_back(w);
  return wps;
}

std::vector<waypoint_t> computeJerkWaypoints(
    const ProblemData& pData, const double T,
    std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);

  std::vector<waypoint_t> wps;
  assert(pi.size() == 9);

  double alpha = 1. / (T * T * T);

  waypoint_t w = initwp(DIM_POINT, DIM_VAR);

  // assign w0:
  w.second = 336 * (-pi[0] + 3 * pi[1] - 3 * pi[2] + pi[3]) * alpha;
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w1:
  w.first = 336 * alpha * Matrix3::Identity();
  w.second = 336 * (-pi[1] + 3 * pi[2] - 3 * pi[3]) * alpha;
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w2:
  w.first = -3 * 336 * alpha * Matrix3::Identity();
  w.second = 336 * (-pi[2] + 3 * pi[3] + pi[5]) * alpha;
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w3:
  w.first = 3 * 336 * alpha * Matrix3::Identity();
  w.second = 336 * (-pi[3] - 3 * pi[5] + pi[6]) * alpha;
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w4:
  w.first = -336 * alpha * Matrix3::Identity();
  w.second = 336 * (3 * pi[5] - 3 * pi[6] + pi[7]) * alpha;
  wps.push_back(w);
  w = initwp(DIM_POINT, DIM_VAR);
  // assign w5:
  w.second = 336 * (-pi[5] + 3 * pi[6] - 3 * pi[7] + pi[8]) * alpha;
  wps.push_back(w);
  return wps;
}

inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
  coefs_t v;
  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
  // equation found with sympy
  v.first = 0.;
  v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
  return v;
}

inline std::pair<MatrixXX, VectorX> computeVelocityCost(
    const ProblemData& pData, double T,
    std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
  MatrixXX H = MatrixXX::Zero(3, 3);
  VectorX g = VectorX::Zero(3);
  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);

  g = (-7.8321678321748 * pi[0] - 7.83216783237586 * pi[1] +
       9.13752913728184 * pi[3] + 9.13752913758454 * pi[5] -
       7.83216783216697 * pi[7] - 7.83216783216777 * pi[8]) /
      (2 * T);
  H = Matrix3::Identity() * 6.52680652684107 / (T);

  double norm = H.norm();
  H /= norm;
  g /= norm;

  return std::make_pair(H, g);
}

}  // namespace c0_dc0_ddc0_j0_j1_ddc1_dc1_c1

}  // namespace bezier_com_traj

#endif  // WAYPOINTS_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH
