// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_NEW_ALGEBRAIC_CONCEPTS_INCLUDE
#define MTL_NEW_ALGEBRAIC_CONCEPTS_INCLUDE

#include <concepts>
#include <boost/numeric/linear_algebra/intrinsic_concept_maps.hpp>
#include <boost/numeric/linear_algebra/operators.hpp>

namespace math {

concept Commutative<typename Operation, typename Element>
  : std::Callable2<Operation, Element, Element>
{
    axiom Commutativity(Operation op, Element x, Element y)
    {
	op(x, y) == op(y, x);
    }
};

concept SemiGroup<typename Operation, typename Element>
  : std::Callable2<Operation, Element, Element>
{
    axiom Associativity(Operation op, Element x, Element y, Element z)
    {
	op(x, op(y, z)) == op(op(x, y), z);
    }
};

concept Monoid<typename Operation, typename Element>
  : SemiGroup<Operation, Element>
{
    typename identity_result_type;
    identity_result_type identity(Operation, Element);

    axiom Neutrality(Operation op, Element x)
    {
	op( x, identity(op, x) ) == x;
	op( identity(op, x), x ) == x;
    }
};

auto concept Inversion<typename Operation, typename Element>
{
    typename result_type;
    result_type inverse(Operation, Element);

};

concept PIMonoid<typename Operation, typename Element>
  : Monoid<Operation, Element>,
    Inversion<Operation, Element>
{
    bool is_invertible(Operation, Element);

    requires std::Convertible<Inversion<Operation, Element>::result_type, Element>;

    axiom Invertibility(Operation op, Element x)
    {
	// Only for invertible elements:
	if (is_invertible(op, x))
	    op( x, inverse(op, x) ) == identity(op, x);
	if ( is_invertible(op, x) )
	    op( inverse(op, x), x ) == identity(op, x);
    }
}

#if 0
    // Alternative approach to convert the result of inversion to Element
    // Unfortunately, this doesn't compile
    template <typename Operation, typename Element>
        requires PIMonoid<Operation, Element>
    concept_map PIMonoid<Operation, Inversion<Operation, Element>::result_type> {}
#endif

concept Group<typename Operation, typename Element>
  : PIMonoid<Operation, Element>
{
    bool is_invertible(Operation, Element) { return true; }

    // Just in case somebody redefines is_invertible
    axiom AlwaysInvertible(Operation op, Element x)
    {
	is_invertible(op, x);
    }

    // In fact this is implied by AlwaysInvertible and inherited Invertibility axiom
    // Maybe remove
    axiom GlobalInvertibility(Operation op, Element x)
    {
	op( x, inverse(op, x) ) == identity(op, x);
	op( inverse(op, x), x ) == identity(op, x);
    }
};

auto concept AbelianGroup<typename Operation, typename Element>
  : Group<Operation, Element>, Commutative<Operation, Element>
{};

// =======================
// Operator-based concepts
// =======================

concept Additive<typename Element>
  : std::HasPlus<Element>
{
    typename plus_assign_result_type;
    plus_assign_result_type operator+=(Element& x, Element y)
    {
	x= x + y; return x;
    }

    requires std::Convertible<plus_assign_result_type, Element&>;

    // Do we need the opposite conversion too?
    // This line produces a compiler error
    // requires std::Convertible<add<Element>::result_type,
    //                           std::HasPlus<Element>::result_type>;

    axiom Consistency(add<Element> op, Element x, Element y)
    {
	op(x, y) == x + y;
	op(x, y) == (x += y, x);
    }
}

auto concept AdditiveCommutative<typename Element>
  : Additive<Element>,
    Commutative< add<Element>, Element >
{}

auto concept AdditiveSemiGroup<typename Element>
  : Additive<Element>,
    SemiGroup< add<Element>, Element >
{}

#ifdef COMPILER_WITHOUT_OVERLOAD_ERROR  // Uncompilable due to error in compiler
concept AdditiveMonoid<typename Element>
  : AdditiveSemiGroup<Element>,
    Monoid< add<Element>, Element >
{
    Element zero(Element x)
    {
	return identity(add<Element>(), x);
    }

    // If we don't use the default definition
    axiom IdentityConsistency (add<Element> op, Element x)
    {
	zero(x) == identity(op, x);
    }
};

concept AdditivePIMonoid<typename Element>
  : std::HasMinus<Element>, AdditiveMonoid<Element>,
    PIMonoid< add<Element>, Element >
{
    typename minus_assign_result_type;
    minus_assign_result_type operator-=(Element& x, Element y)
    {
	x= x - y; return x;
    }

    requires std::Convertible<minus_assign_result_type, Element&>;

    typename unary_result_type;
    unary_result_type operator-(Element x)
    {
	return zero(x) - x;
    }

    axiom InverseConsistency(add<Element> op, Element x, Element y)
    {
	// consistency between additive and functor concept
	if ( is_invertible(op, y) )
	    op(x, inverse(op, y)) == x - y;
	if ( is_invertible(op, y) )
	    op(x, y) == (x -= y, x);

	// consistency of unary inversion
	if ( is_invertible(op, y) )
	    inverse(op, y) == -y;

	// consistency between unary and binary -
	if ( is_invertible(op, x) )
	    identity(op, x) - x == -x;
    }
}

auto concept AdditiveGroup<typename Element>
  : AdditivePIMonoid<Element>,
    Group< add<Element>, Element >
{};

auto concept AdditiveAbelianGroup<typename Element>
  : AdditiveGroup<Element>,
    Commutative< add<Element>, Element >
{}

#endif

concept Multiplicative<typename Element>
  : std::HasMultiply<Element>
{
    typename times_assign_result_type;
    times_assign_result_type operator*=(Element& x, Element y)
    {
	x= x * y; return x;
    }

    requires std::Convertible<times_assign_result_type, Element&>;

    // Do we need the opposite conversion too?
    // This line produces a compiler error
    // requires std::Convertible<mult<Element>::result_type,
    //                           std::HasMultiply<Element>::result_type>;

    axiom Consistency(mult<Element> op, Element x, Element y)
    {
	op(x, y) == x * y;
	op(x, y) == (x *= y, x);
    }
}

auto concept MultiplicativeCommutative<typename Element>
  : Multiplicative<Element>,
    Commutative< mult<Element>, Element >
{}

auto concept MultiplicativeSemiGroup<typename Element>
  : Multiplicative<Element>,
    SemiGroup< mult<Element>, Element >
{}

#ifdef COMPILER_WITHOUT_OVERLOAD_ERROR  // Uncompilable due to error in compiler
concept MultiplicativeMonoid<typename Element>
  : MultiplicativeSemiGroup<Element>,
    Monoid< mult<Element>, Element >
{
    Element one(Element x)
    {
	return identity(mult<Element>(), x);
    }

    // If we don't use the default definition
    axiom IdentityConsistency (math::mult<Element> op, Element x)
    {
	one(x) == identity(op, x);
    }
};

concept MultiplicativePIMonoid<typename Element>
  : std::HasDivide<Element>, MultiplicativeMonoid<Element>,
    PIMonoid< mult<Element>, Element >
{
    typename divide_assign_result_type;
    divide_assign_result_type operator/=(Element& x, Element y)
    {
	x= x / y; return x;
    }

    requires std::Convertible<divide_assign_result_type, Element&>;

    axiom InverseConsistency(mult<Element> op, Element x, Element y)
    {
	// consistency between multiplicative and functor concept
	if ( is_invertible(op, y) )
	    op(x, inverse(op, y)) == x / y;
	if ( is_invertible(op, y) )
	    op(x, y) == (x /= y, x);
    }
}

auto concept MultiplicativeGroup<typename Element>
  : MultiplicativePIMonoid<Element>,
    Group< mult<Element>, Element >
{};

auto concept MultiplicativeAbelianGroup<typename Element>
  : MultiplicativeGroup<Element>,
    Commutative< mult<Element>, Element >
{}

// ==========================
// Concepts with 2 operations
// ==========================

concept Distributive<typename AddOp, typename MultOp, typename Element>
{
    axiom Distributivity(AddOp add, MultOp mult, Element x, Element y, Element z)
    {
	// From left
	mult(x, add(y, z)) == add(mult(x, y), mult(x, z));
	// from right
	mult(add(x, y), z) == add(mult(x, z), mult(y, z));
    }
}

auto concept Ring<typename AddOp, typename MultOp, typename Element>
  : AbelianGroup<AddOp, Element>,
    SemiGroup<MultOp, Element>,
    Distributive<AddOp, MultOp, Element>
{}

auto concept RingWithIdentity<typename AddOp, typename MultOp, typename Element>
  : Ring<AddOp, MultOp, Element>,
    Monoid<MultOp, Element>
{}

concept DivisionRing<typename AddOp, typename MultOp, typename Element>
  : RingWithIdentity<AddOp, MultOp, Element>,
    Inversion<MultOp, Element>
{
    // 0 != 1, otherwise trivial
    axiom ZeroIsDifferentFromOne(AddOp add, MultOp mult, Element x)
    {
	identity(add, x) != identity(mult, x);
    }

    // Non-zero divisibility from left and from right
    axiom NonZeroDivisibility(AddOp add, MultOp mult, Element x)
    {
	if (x != identity(add, x))
	    mult(inverse(mult, x), x) == identity(mult, x);
	if (x != identity(add, x))
	    mult(x, inverse(mult, x)) == identity(mult, x);
    }
}

auto concept Field<typename AddOp, typename MultOp, typename Element>
  : DivisionRing<AddOp, MultOp, Element>,
    Commutative<MultOp, Element>
{}

auto concept OperatorRing<typename Element>
  : AdditiveAbelianGroup<Element>,
    MultiplicativeSemiGroup<Element>,
    Ring<add<Element>, mult<Element>, Element>
{}

auto concept OperatorRingWithIdentity<typename Element>
  : OperatorRing<Element>,
    MultiplicativeMonoid<Element>,
    RingWithIdentity<add<Element>, mult<Element>, Element>
{}

auto concept OperatorDivisionRing<typename Element>
  : OperatorRingWithIdentity<Element>,
    MultiplicativePIMonoid<Element>,
    DivisionRing<add<Element>, mult<Element>, Element>
{}

auto concept OperatorField<typename Element>
  : OperatorDivisionRing<Element>,
    Field<add<Element>, mult<Element>, Element>
{}

#endif

concept IntrinsicType<typename T> {}

concept IntrinsicArithmetic<typename T> : IntrinsicType<T> {}

concept IntrinsicIntegral<typename T> : IntrinsicArithmetic<T> {}

concept IntrinsicSignedIntegral<typename T>
  : std::SignedIntegralLike<T>,
    IntrinsicIntegral<T>
{}

concept IntrinsicUnsignedIntegral<typename T>
  : std::UnsignedIntegralLike<T>,
    IntrinsicIntegral<T>
{}

concept IntrinsicFloatingPoint<typename T>
  : std::FloatingPointLike<T>,
    IntrinsicArithmetic<T>
{}

#if 0

// ====================
// Default Concept Maps
// ====================

// ==============
// Arithmetic
// ==============

// ----------------
// Signed integrals
// ----------------

template <typename T>
  requires IntrinsicSignedIntegral<T>
concept_map OperatorRingWithIdentity<T> {}

template <typename T>
  requires IntrinsicSignedIntegral<T>
concept_map MultiplicativeCommutative<T> {}

// ------------------
// Unsigned integrals
// ------------------

template <typename T>
  requires IntrinsicUnsignedIntegral<T>
concept_map AdditiveCommutative<T> {}

template <typename T>
  requires IntrinsicUnsignedIntegral<T>
concept_map AdditiveMonoid<T> {}

template <typename T>
  requires IntrinsicUnsignedIntegral<T>
concept_map MultiplicativeCommutative<T> {}

template <typename T>
  requires IntrinsicUnsignedIntegral<T>
concept_map MultiplicativeMonoid<T> {}

// ---------------
// Floationg Point
// ---------------

template <typename T>
  requires IntrinsicFloatingPoint<T>
concept_map Field<T> {}

template <typename T>
  requires IntrinsicFloatingPoint<T>
concept_map Field< std::complex<T> > {}

// ===========
// Min and Max
// ===========

template <typename T>
  requires IntrinsicArithmetic<T>
concept_map Commutative< max<T>, T > {}

template <typename T>
  requires IntrinsicArithmetic<T>
concept_map Monoid< max<T>, T > {}

template <typename T>
  requires IntrinsicArithmetic<T>
concept_map Commutative< min<T>, T > {}

template <typename T>
  requires IntrinsicArithmetic<T>
concept_map Monoid< min<T>, T > {}

// ==========
// And and Or
// ==========

template <typename T>
  requires Intrinsic<T> && std::HasLogicalAnd<T>
concept_map Commutative< std::logical_and<T>, T > {}

template <typename T>
  requires Intrinsic<T> && std::HasLogicalAnd<T>
concept_map Monoid< std::logical_and<T>, T > {}

template <typename T>
  requires Intrinsic<T> && std::HasLogicalOr<T>
concept_map Commutative< std::logical_or<T>, T > {}

template <typename T>
  requires Intrinsic<T> && std::HasLogicalOr<T>
concept_map Monoid< std::logical_or<T>, T > {}

template <typename T>
  requires Intrinsic<T> && std::HasLogicalAnd<T> && std::HasLogicalOr<T>
concept_map Distributive<std::logical_and<T>, std::logical_or<T>, T> {}

template <typename T>
  requires Intrinsic<T> && std::HasLogicalAnd<T> && std::HasLogicalOr<T>
concept_map Distributive<std::logical_or<T>, std::logical_and<T>, T> {}

// ==================
// Bitwise operations
// ==================

// not yet defined

template <typename T>
  requires IntrinsicIntegral<T>
concept_map Commutative< bit_and<T>, T > {}

template <typename T>
  requires IntrinsicIntegral<T>
concept_map Monoid< bit_and<T>, T > {}

template <typename T>
  requires IntrinsicIntegral<T>
concept_map Commutative< bit_or<T>, T > {}

template <typename T>
  requires IntrinsicIntegral<T>
concept_map Monoid< bit_or<T>, T > {}

template <typename T>
  requires IntrinsicIntegral<T>
concept_map Distributive<bit_and<T>, bit_or<T>, T> {}

template <typename T>
  requires IntrinsicIntegral<T>
concept_map Distributive<bit_or<T>, bit_and<T>, T> {}

template <typename T>
  requires IntrinsicIntegral<T>
concept_map Commutative< bit_xor<T>, T > {}

template <typename T>
  requires IntrinsicIntegral<T>
concept_map SemiGroup< bit_xor<T>, T > {}

// ====================
// String concatenation
// ====================

concept_map AdditiveMonoid<std::string> {}

#endif

} // namespace math

#endif // MTL_NEW_ALGEBRAIC_CONCEPTS_INCLUDE


//
// Created by huangkun on 2020/8/26.
//

#include <Eigen/SparseCholesky>
#include <cmath>
#include <algorithm>
#include <numeric>

#include <opengv2/spline/BsplineSO3.hpp>

opengv2::BsplineSO3::BsplineSO3(int p,
                                const std::vector<Sophus::SO3d, Eigen::aligned_allocator<Sophus::SO3d>> &samples,
                                int controlPointsNum, const std::vector<double> &correspondingUs,
                                const std::vector<std::vector<Eigen::Vector3d>> &derivativeSamples,
                                double derivativeWeight) : degree_(p), derivativeWeight_(derivativeWeight) {
    // if dataPoints given, then do approximation
    if (!samples.empty()) {
        samples_ = samples;

        // Data registration
        if (!correspondingUs.empty()) {
            correspondingUs_ = correspondingUs;
        } else {
            // Data registration using chord length, Range: [0,1]
            correspondingUs_.resize(samples_.size());

            double d = 0;
            for (int i = 1; i < samples_.size(); ++i) {
                d += 2 * std::acos((samples_[i] * samples_[i - 1].inverse()).unit_quaternion().w());
            }

            correspondingUs_.front() = 0;
            correspondingUs_.back() = 1;
            for (int i = 1; i < samples_.size() - 1; ++i) {
                correspondingUs_[i] = correspondingUs_[i - 1] +
                                      2 * std::acos((samples_[i] * samples_[i - 1].inverse()).unit_quaternion().w()) /
                                      d;
            }
        }

        // derivative
        if (!derivativeSamples.empty()) {
            derivativeSamples_ = derivativeSamples;
        }

        // initialization
        if (controlPointsNum < 0)
            controlPointsNum = std::max(int(samples_.size() / 3), degree_ + 1);
        if (controlPointsNum <= degree_) {
            std::cerr << "BsplineSO3 Constructor: control points number should greater than degree!" << std::endl;
            return;
        }
        knotSpacing(controlPointsNum);
        initialGuess();
        optimizeCP();
    }
}

void opengv2::BsplineSO3::knotSpacing(int controlPointsNum) {
    /*** knot initialization (9.68) in NURBS book ***/
    knotVector_.resize(controlPointsNum + degree_ + 1);
    std::fill_n(knotVector_.begin(), degree_ + 1, correspondingUs_.front());
    std::fill_n(knotVector_.rbegin(), degree_ + 1, correspondingUs_.back());
    double d = samples_.size() / double(controlPointsNum - degree_);
    for (int j = 1; j <= controlPointsNum - 1 - degree_; j++) {
        int i = floor(j * d);
        double alpha = j * d - i;
        knotVector_[degree_ + j] = (1 - alpha) * correspondingUs_[i - 1] + alpha * correspondingUs_[i];
    }
}

void opengv2::BsplineSO3::derBasisFuns(double u, size_t spanIdx, int derivativeLimit,
                                       std::vector<std::vector<double>> &ders) const {
    // check
    if (knotVector_.empty()) {
        std::cerr << "Function derBasisFuns: knotVector is empty!" << std::endl;
        return;
    }

    ders.resize(derivativeLimit + 1);
    for (auto itr = ders.begin(); itr != ders.end(); itr++) {
        itr->assign(degree_, 0);
    }

    std::vector<std::vector<double>> basisN;

    // 0-th derivative: $\beta_{k,i}(u) = \sum_{j=i}^{k} N_{j,p}(u), i \in [k-p+1, k]$.
    // since \sum_{j=k-p}^{k} N_{j,p}(u) = 1
    basis(u, spanIdx, 0, degree_, basisN);
    ders[0][degree_ - 1] = basisN[0][degree_];
    for (int i = degree_ - 2; i >= 0; i--) {
        ders[0][i] = ders[0][i + 1] + basisN[0][i + 1];
    }

    // \alpha-th derivative, \alpha >= 1
    if (derivativeLimit > 0) {
        basisN.clear();
        basis(u, spanIdx, derivativeLimit - 1, degree_ - 1, basisN);

        for (int alpha = 1; alpha <= derivativeLimit; alpha++) {
            for (int i = spanIdx - degree_ + 1; i <= spanIdx; i++) {
                ders[alpha][i - (spanIdx - degree_ + 1)] =
                        degree_ / (knotVector_[i + degree_] - knotVector_[i]) *
                        basisN[alpha][i - (spanIdx - degree_ + 1)];
            }
        }
    }
}

void opengv2::BsplineSO3::basis(double u, size_t spanIdx, int derivativeLimit, int degree,
                                std::vector<std::vector<double>> &ders) const {
    ders.resize(derivativeLimit + 1);
    for (auto &it:ders) {
        it.assign(degree + 1, 0);
    }

    double ndu[degree + 1][degree + 1]; // store the basis functions and knot differences

    // store (in an alternating fashion) the two most recently computed rows a_{k,j} and a_{k-1,j}
    double a[2][degree + 1];

    std::vector<double> left, right;
    left.resize(degree + 1);
    right.resize(degree + 1);

    ndu[0][0] = 1;
    for (int j = 1; j <= degree; j++) {
        left[j] = u - knotVector_[spanIdx + 1 - j];
        right[j] = knotVector_[spanIdx + j] - u;
        double saved = 0.0;
        for (int r = 0; r < j; ++r) {
            ndu[j][r] = right[r + 1] + left[j - r];
            double temp = ndu[r][j - 1] / ndu[j][r];

            ndu[r][j] = saved + right[r + 1] * temp;
            saved = left[j - r] * temp;
        }
        ndu[j][j] = saved;
    }

    /* Load the basis functions */
    for (int j = 0; j <= degree; j++)
        ders[0][j] = ndu[j][degree];

    if (derivativeLimit > 0) {
        /*** This section computes the derivatives (Eq.[2.9]) ***/
        /* Load over function index */
        for (int r = 0; r <= degree; r++) {
            int s1 = 0, s2 = 1; // Alternate rows in array a
            a[0][0] = 1;

            // loop to compute kth derivative
            for (int k = 1; k <= derivativeLimit; k++) {
                double d = 0;
                int rk = r - k, pk = degree - k;
                if (r >= k) {
                    a[s2][0] = a[s1][0] / ndu[pk + 1][rk];
                    d = a[s2][0] * ndu[rk][pk];
                }

                int j1, j2;
                if (rk >= -1)
                    j1 = 1;
                else
                    j1 = -rk;

                if (r - 1 <= pk)
                    j2 = k - 1;
                else
                    j2 = degree - r;

                for (int j = j1; j <= j2; j++) {
                    a[s2][j] = (a[s1][j] - a[s1][j - 1]) / ndu[pk + 1][rk + j];
                    d += a[s2][j] * ndu[rk + j][pk];
                }

                if (r <= pk) {
                    a[s2][k] = -a[s1][k - 1] / ndu[pk + 1][r];
                    d += a[s2][k] * ndu[r][pk];
                }

                ders[k][r] = d;

                // switch rows
                std::swap(s1, s2);
            }
        }

        /* Multiply through by correct factors (Eq. [2.9]) */
        int r = degree;
        for (int k = 1; k <= derivativeLimit; ++k) {
            for (int j = 0; j <= degree; j++)
                ders[k][j] *= r;

            r *= (degree - k);
        }
    }
}

// TODO: an acceleration penalty factor
void opengv2::BsplineSO3::initialGuess() {
    /*** check ***/
    if (samples_.empty()) {
        std::cerr << "Function initialGuess: dataPoints is empty!" << std::endl;
        return;
    }
    if (knotVector_.empty()) {
        std::cerr << "Function initialGuess: knotVector is empty!" << std::endl;
        return;
    }

    int controlPointsNum = knotVector_.size() - degree_ - 1;

    // initialize controlPoints
    controlPoints_.resize(controlPointsNum);
    controlPoints_.front() = samples_.front();
    controlPoints_.back() = samples_.back();

    /*** Problem ***/
    Eigen::SparseMatrix<double> N(samples_.size(), controlPointsNum);
    Eigen::SparseMatrix<double> M(samples_.size(), controlPointsNum);
    std::vector<std::vector<double>> ders;
    for (int k = 0; k < samples_.size(); ++k) {
        size_t spanIdx = findSpan(correspondingUs_[k]);
        basis(correspondingUs_[k], spanIdx, 0, degree_, ders);

        for (int j = 0; j <= degree_; ++j) {
            if (ders[0][j] != 0)
                N.insert(k, spanIdx - degree_ + j) = ders[0][j]; // N_{i-p+j,p}^{0}(\bar{u}_k)
        }
    }

    // formulating B
    std::vector<Eigen::Matrix<double, 4, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 4, 1>>> B;
    B.assign(controlPointsNum - 2, Eigen::Matrix<double, 4, 1>::Zero());
    for (int l = 1; l <= controlPointsNum - 2; ++l) {
        // for l-th col of N
        for (Eigen::SparseMatrix<double>::InnerIterator it(N, l); it; ++it) {
            // N_{l,p}(\bar{u}_k), valid for column-major
            int k = it.row();
            if (k != 0 && k != samples_.size() - 1) {
                B[l - 1] += it.value() * (samples_[k].unit_quaternion().coeffs() -
                                          N.coeff(k, 0) * samples_.front().unit_quaternion().coeffs() -
                                          N.coeff(k, controlPointsNum - 1) *
                                          samples_.back().unit_quaternion().coeffs());
            }
        }
    }

    // formulating M,N
    Eigen::SparseMatrix<double> Nc = N.block(1, 1, samples_.size() - 2, controlPointsNum - 2);
    Eigen::SparseMatrix<double> A = Nc.transpose() * Nc;

    // solving
    A.makeCompressed();
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
    solver.compute(A);
    if (solver.info() != Eigen::Success) {
        std::cerr << "Function initialGuess: decomposition failed!" << std::endl;
        return;
    }
    Eigen::VectorXd b(controlPointsNum - 2);

    std::vector<Eigen::Matrix<double, 4, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 4, 1>>> cp;
    cp.resize(controlPointsNum - 2);
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < B.size(); ++j) {
            b[j] = B[j][i];
        }
        Eigen::VectorXd x = solver.solve(b);
        if (solver.info() != Eigen::Success) {
            std::cerr << "Function initialGuess: solving failed!" << std::endl;
            return;
        }
        for (int j = 1; j <= controlPointsNum - 2; ++j) {
            cp[j - 1][i] = x[j - 1];
        }
    }
    for (int j = 1; j <= controlPointsNum - 2; ++j) {
        controlPoints_[j].setQuaternion(Eigen::Quaterniond(cp[j - 1]));
    }
}

void opengv2::BsplineSO3::optimizeCP() {
    ceres::Problem problem;
    ceres::LocalParameterization *SO3_parameterization = new LocalParameterizationSO3();

    // Specify local update rule for our parameter
    for (Sophus::SO3d &cp:controlPoints_) {
        problem.AddParameterBlock(cp.data(), Sophus::SO3d::num_parameters, SO3_parameterization);
    }
    problem.SetParameterBlockConstant(controlPoints_.front().data());
    problem.SetParameterBlockConstant(controlPoints_.back().data());

    // pre-calculate spline basis
    std::unordered_map<int, std::shared_ptr<std::vector<std::vector<double>>>> basisFuns;
    std::unordered_map<int, size_t> spanIdxs;
    for (int i = 0; i < samples_.size(); ++i) {
        double u = correspondingUs_[i];
        auto basisFun = std::make_shared<std::vector<std::vector<double>>>();
        size_t spanIdx = findSpan(u);
        derBasisFuns(u, spanIdx, 0, *basisFun);

        basisFuns[i] = basisFun;
        spanIdxs[i] = spanIdx;
    }

    // Create and add cost functions. Derivatives will be evaluated via automatic differentiation
    for (int i = 0; i < samples_.size(); ++i) {
        auto spanIdx = spanIdxs[i];
        ceres::CostFunction *cost_function;
        if (degree_ == 4) {
            cost_function = P4ApproximationError::Create(samples_[i].inverse(), basisFuns[i]);
            problem.AddResidualBlock(cost_function, nullptr,
                                     controlPoints_[spanIdx - 4 + 0].data(),
                                     controlPoints_[spanIdx - 4 + 1].data(),
                                     controlPoints_[spanIdx - 4 + 2].data(),
                                     controlPoints_[spanIdx - 4 + 3].data(),
                                     controlPoints_[spanIdx - 4 + 4].data());
        } else if (degree_ == 3) {
            cost_function = P3ApproximationError::Create(samples_[i].inverse(), basisFuns[i]);
            problem.AddResidualBlock(cost_function, nullptr,
                                     controlPoints_[spanIdx - 3 + 0].data(),
                                     controlPoints_[spanIdx - 3 + 1].data(),
                                     controlPoints_[spanIdx - 3 + 2].data(),
                                     controlPoints_[spanIdx - 3 + 3].data());
        }
    }

    // Set solver options (precision / method)
    ceres::Solver::Options options;
    options.gradient_tolerance = Sophus::Constants<double>::epsilon();
    options.function_tolerance = Sophus::Constants<double>::epsilon();
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

    // Solve
    ceres::Solver::Summary summary;
    Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << std::endl;
}

void opengv2::BsplineSO3::evaluate(double u, int derivativeLimit, Sophus::SO3d &val,
                                   std::vector<Eigen::Vector3d> &Ders) const {
    if (controlPoints_.empty()) {
        std::cerr << "Function evaluate: controlPoints is empty!" << std::endl;
        return;
    }
    if (derivativeLimit > 2) {
        std::cerr << "Function evaluate: derivativeLimit should <= 2!" << std::endl;
        return;
    }
    Ders.clear();

    // compute basis, [k-p+1,k]
    std::vector<std::vector<double>> basis;
    size_t spanIdx = findSpan(u);
    derBasisFuns(u, spanIdx, derivativeLimit, basis);

    // d_{k-p+j}, j \in [1,p]
    std::vector<Eigen::Vector3d> d; // since Sophus::SO3d::DoF = 3
    for (int j = 1; j <= degree_; ++j) {
        d.push_back(
                (controlPoints_[spanIdx - degree_ + j - 1].inverse() * controlPoints_[spanIdx - degree_ + j]).log());
    }

    // A_j(u), j \in [1,p]
    std::vector<Sophus::SO3d, Eigen::aligned_allocator<Sophus::SO3d>> A;
    for (int j = 1; j <= degree_; ++j) {
        int j_idx = j - 1;
        A.push_back(Sophus::SO3d::exp(basis[0][j_idx] * d[j_idx]));
    }

    // derivative = 0
    val = controlPoints_[spanIdx - degree_ + 0];
    for (int j = 1; j <= degree_; ++j) {
        int j_idx = j - 1;
        val *= A[j_idx];
    }

    if (derivativeLimit >= 1) {
        std::vector<std::vector<Eigen::Vector3d>> angVDer; // since Sophus::SO3d::DoF = 3
        angVDer.resize(derivativeLimit);
        for (auto &ad: angVDer) {
            ad.assign(degree_ + 1, Eigen::Vector3d::Zero());
        }

        for (int j = 2; j <= degree_ + 1; j++) {
            int j_idx = j - 1;
            angVDer[0][j_idx] =
                    A[j_idx - 1].inverse().Adj() * angVDer[0][j_idx - 1] + basis[1][j_idx - 1] * d[j_idx - 1];
        }

        Ders.push_back(angVDer[0].back());

        if (derivativeLimit >= 2) {
            for (int j = 2; j <= degree_ + 1; j++) {
                int j_idx = j - 1;
                angVDer[1][j_idx] = basis[1][j_idx - 1] * Sophus::SO3d::lieBracket(angVDer[0][j_idx], d[j_idx - 1]) +
                                    A[j_idx - 1].inverse().Adj() * angVDer[1][j_idx - 1] +
                                    basis[2][j_idx - 1] * d[j_idx - 1];
            }

            Ders.push_back(angVDer[1].back());
        }
    }
}


/*=============================================================================
Copyright (c) 2016 Paul W. Bible

Distributed under the Boost Software License, Version 1.0. (See accompanying
file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
==============================================================================*/
#ifndef ENRICHMENT_TOOLS
#define ENRICHMENT_TOOLS

#include <stdlib.h>

#include <ggtk/AnnotationData.hpp>
#include <ggtk/GoGraph.hpp>
#include <ggtk/SetUtilities.hpp>

#include <boost/math/distributions.hpp>
#include <boost/unordered_map.hpp>

//! The EnrichmentTools namespace provides simple functions for calulating GO term enrichment.
/*!
	This namespace defines free functions that allow enrichment p-values to be calculated.
	These funcitons can serve as the foundation for more sophisticated enrichment anlayis.
*/
namespace EnrichmentTools{

	//! A method for determining which genes are annotated with the given term or a child of that term.
	/*!
		This method calculates the set of the genes annotated with a given term or transatively with a child of that term.
	*/
	inline boost::unordered_set<std::string> getDescendantGenes(GoGraph *go, AnnotationData *data, const std::string &term){
		boost::unordered_set<std::string> descendants = go->getDescendantTerms(term);
		descendants.insert(term);

		boost::unordered_set<std::string>::iterator si;
		boost::unordered_set<std::string> genes;

		for(si = descendants.begin(); si != descendants.end(); ++si){
			std::string currentTerm = *si;
			//std::cout << *si << " " << go->getTermName(*si) << std::endl;
			data->addGenesForGoTerm(currentTerm,genes);
		}
		//std::cout << genes.size() << std::endl;

		return genes;
	};

	//! A method for calculating the result of a hypergeometic test.
	/*!
		This method calculates p-value of a hypergeometice test give 4 values.
		The sample size,         n
		The population success   K
		The the population size  N
		The test value           k

		Answers the question:
		"What is probability of seeing value of k or more successes
		  in a sample of size n, given that the population of size N
		  contains K total successes."
	*/
	inline double oneSidedRawPvalue_hyper(size_t sample, size_t success,size_t population,size_t test_value){
		double sum = 0.0;
		boost::math::hypergeometric dist(sample,success,population);
		for(size_t i = test_value; i <= sample && i <= success; ++i){
			double prob = boost::math::pdf(dist,i);
			sum += prob;
		}
		return sum;
	};

	//! A method to calculate the enrichment of a specific term in a sample of genes.
	/*!
		This method performs a hypergeometic test of enrichment for a term given
		a set of genes that serves as the sample. The population is taken as all genes
		in the annotation database.
	*/
	inline double enrichmentSignificance(GoGraph *go, AnnotationData *data,
											boost::unordered_set<std::string> &genes,
											const std::string &term)
	{
		boost::unordered_set<std::string> termGenes = getDescendantGenes(go, data, term);
		boost::unordered_set<std::string> sharedGenes = SetUtilities::set_intersection(genes,termGenes);

		if(sharedGenes.size() == 0){
			return 1.0;
		}

		size_t sampleSize = genes.size();
		size_t sampleWithTerm = sharedGenes.size();
		size_t populationWithTerm = termGenes.size();
		size_t populationSize = data->getNumGenes();

		return oneSidedRawPvalue_hyper(sampleSize,populationWithTerm,populationSize,sampleWithTerm);
	};

};
#endif


#include <algorithm>
#include <bitset>
// #include <boost/algorithm/string/replace.hpp>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <map>
#include <set>
#include <string>
#include <vector>

using namespace std;

#define FOR(i, j, n) for (int i = j; i < n; i++)
#define OUT(s) cout << s << " ";
#define IN(a) cin >> a;
#define PACK(a, n)                                                             \
  for (int i = 0; i < n; i++)                                                  \
    cin >> a[i];
#define LIST(a, n)                                                             \
  for (int i = 0; i < n; i++)                                                  \
    cout << i << ": " << a[i] << endl;
#define PP(label, obj) cout << "[" << label << "] " << obj << endl;

int main() {
  string S;
  IN(S);

  int len = S.length();
  int even = 0;
  int odd = 0;
  int rpos = -1;
  FOR(i, 0, len) {
    if (i % 2 == 0) {
      even += 1;
    } else {
      odd += 1;
    }
    if ((i + 1 == len || S[i + 1] == 'R') && S[i] == 'L') {
      if (rpos % 2 == 0) {
        OUT(even);
        OUT(odd);
      } else {
        OUT(odd);
        OUT(even);
      }
      FOR(j, 0, i - rpos - 1) OUT(0);
      rpos = -1;
      even = odd = 0;
    } else if (S[i + 1] == 'L' && S[i] == 'R') {
      rpos = i;
    } else {
      if (rpos < 0) {
        OUT(0);
      }
    }
  }
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[tag
//` Shows how tag dispatching essentially works in Boost.Geometry

#include <iostream>

#include <boost/assign.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename Tag> struct dispatch {};

// Specialization for points
template <> struct dispatch<boost::geometry::point_tag>
{
    template <typename Point>
    static inline void apply(Point const& p)
    {
        // Use the Boost.Geometry free function "get"
        // working on all supported point types
        std::cout << "Hello POINT, you are located at: "
            << boost::geometry::get<0>(p) << ", "
            << boost::geometry::get<1>(p)
            << std::endl;
    }
};

// Specialization for polygons
template <> struct dispatch<boost::geometry::polygon_tag>
{
    template <typename Polygon>
    static inline void apply(Polygon const& p)
    {
        // Use the Boost.Geometry manipulator "dsv"
        // working on all supported geometries
        std::cout << "Hello POLYGON, you look like: "
            << boost::geometry::dsv(p)
            << std::endl;
    }
};

// Specialization for multipolygons
template <> struct dispatch<boost::geometry::multi_polygon_tag>
{
    template <typename MultiPolygon>
    static inline void apply(MultiPolygon const& m)
    {
        // Use the Boost.Range free function "size" because all
        // multigeometries comply to Boost.Range
        std::cout << "Hello MULTIPOLYGON, you contain: "
            << boost::size(m) << " polygon(s)"
            << std::endl;
    }
};

template <typename Geometry>
inline void hello(Geometry const& geometry)
{
    // Call the metafunction "tag" to dispatch, and call method (here "apply")
    dispatch
        <
            typename boost::geometry::tag<Geometry>::type
        >::apply(geometry);
}

int main()
{
    // Define polygon type (here: based on a Boost.Tuple)
    typedef boost::geometry::model::polygon<boost::tuple<int, int> > polygon_type;

    // Declare and fill a polygon and a multipolygon
    polygon_type poly;
    boost::geometry::exterior_ring(poly) = boost::assign::tuple_list_of(0, 0)(0, 10)(10, 5)(0, 0);

    boost::geometry::model::multi_polygon<polygon_type> multi;
    multi.push_back(poly);

    // Call "hello" for point, polygon, multipolygon
    hello(boost::make_tuple(2, 3));
    hello(poly);
    hello(multi);

    return 0;
}

//]

//[tag_output
/*`
Output:
[pre
Hello POINT, you are located at: 2, 3
Hello POLYGON, you look like: (((0, 0), (0, 10), (10, 5), (0, 0)))
Hello MULTIPOLYGON, you contain: 1 polygon(s)
]
*/
//]


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef MODULES_WORLD_OPENDRIVE_PLAN_VIEW_HPP_
#define MODULES_WORLD_OPENDRIVE_PLAN_VIEW_HPP_

#include <Eigen/Core>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>

#include "modules/geometry/commons.hpp"
#include "modules/geometry/line.hpp"
#include "modules/world/opendrive/lane.hpp"

namespace modules {
namespace world {
namespace opendrive {

class PlanView {
 public:
  PlanView() : length_(0.0) {}
  ~PlanView() {}

  //! setter functions
  bool add_line(geometry::Point2d start_point, float heading, float length);

  bool add_spiral(geometry::Point2d start_point, float heading, float length, float curvStart, float curvEnd, float s_inc = 2.0f);
  bool add_arc(geometry::Point2d start_point, float heading, float length, float curvature, float s_inc = 2.0f);

  void calc_arc_position(const float s, float initial_heading, float curvature, float &dx, float &dy);

  //! getter functions
  geometry::Line get_reference_line() const { return reference_line_; }

  geometry::Point2d test(geometry::Point2d p) { return p; }

  float get_length() const { return length_; }
  float get_distance( const geometry::Point2d &p) const { return boost::geometry::distance(reference_line_.obj_, p); }

 private:
  geometry::Line reference_line_;  // sequential build up
  float length_;
};

using PlanViewPtr = std::shared_ptr<PlanView>;

}  // namespace opendrive
}  // namespace world
}  // namespace modules

#endif  // MODULES_WORLD_OPENDRIVE_PLAN_VIEW_HPP_


/***********************************************************************
Copyright (c) 2014-2020, Jan Elffers
Copyright (c) 2019-2020, Jo Devriendt
Copyright (c) 2020, Stephan Gocht

Parts of the code were copied or adapted from MiniSat.

MiniSAT -- Copyright (c) 2003-2006, Niklas Een, Niklas Sorensson
           Copyright (c) 2007-2010  Niklas Sorensson

Permission is hereby granted, free of charge, to any person obtaining a
copy of this software and associated documentation files (the
"Software"), to deal in the Software without restriction, including
without limitation the rights to use, copy, modify, merge, publish,
distribute, sublicense, and/or sell copies of the Software, and to
permit persons to whom the Software is furnished to do so, subject to
the following conditions:

The above copyright notice and this permission notice shall be included
in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
***********************************************************************/

#pragma once

#include <boost/multiprecision/cpp_int.hpp>
#if WITHGMP
#include <boost/multiprecision/gmp.hpp>
#endif  // WITHGMP
#include <cassert>
#include <exception>
#include <iostream>
#include <limits>
#include <unordered_map>
#include <vector>

namespace rs {

#if WITHGMP
using int128 = boost::multiprecision::int128_t;  // NOTE: a bit slower than __int128, but plays nice with mpz_int
using bigint = boost::multiprecision::mpz_int;   // NOTE: requires GMP
#else
using int128 = __int128;
using bigint = boost::multiprecision::cpp_int;
#endif  // WITHGMP
using int256 = boost::multiprecision::int256_t;
using BigCoef = bigint;
using BigVal = bigint;

using ID = uint64_t;
const ID ID_Undef = std::numeric_limits<ID>::max();
const ID ID_Unsat = ID_Undef - 1;
const ID ID_Trivial = 1;  // represents constraint 0 >= 0

using Var = int;
using Lit = int;
inline Var toVar(Lit l) { return std::abs(l); }

const int resize_factor = 2;

const int INF = 1e9 + 1;  // 1e9 is the maximum number of variables in the system, anything beyond is infinity
const long long INFLPINT = 1e15 + 1;  // based on max long range captured by double

const int limit32 = 1e9;         // 2^29-2^30
const long long limit64 = 2e18;  // 2^60-2^61
const double limit96 = 8e27;     // 2^92-2^93, so 46 bits is less than half
const double limit128 = 32e36;   // 2^124-2^125, so 62 bits is less than half
const double limit256 = 1e76;    // 2^252-2^253, so 126 bits is less than half
const int conflLimit32 = 14;
const int conflLimit64 = 30;
const int conflLimit96 = 46;
const int conflLimit128 = 62;

using IntVecIt = std::vector<int>::iterator;

using ActValV = long double;
const ActValV actLimitV = (ActValV)1e300 * (ActValV)1e300 * (ActValV)1e300 * (ActValV)1e300 * (ActValV)1e300 *
                          (ActValV)1e300 * (ActValV)1e300 * (ActValV)1e300;  // ~1e2400 << 2^(2^13)
using ActValC = float;
const ActValC actLimitC = 1e30;  // ~1e30 << 2^(2^7)

/*
 * UNKNOWN: uninitialized value
 * FORMULA: original input formula constraints
 * LEARNED: learned from regular conflict analysis
 * FARKAS: LP solver infeasibility witness
 * LEARNEDFARKAS: constraint learned from conflict analysis on FARKAS
 * GOMORY: Gomory cut
 * UPPERBOUND: upper and lower bounds on the objective function
 *
 * max number of types is 16, as the type is stored with 4 bits in Constr
 */
enum class Origin {
  UNKNOWN,
  FORMULA,
  LEARNED,
  FARKAS,
  LEARNEDFARKAS,
  GOMORY,
  UPPERBOUND,
  GAUSS,
};

template <typename SMALL, typename LARGE>
struct ConstrExp;
using ConstrExp32 = ConstrExp<int, long long>;
using ConstrExp64 = ConstrExp<long long, int128>;
using ConstrExp96 = ConstrExp<int128, int128>;
using ConstrExp128 = ConstrExp<int128, int256>;
using ConstrExpArb = ConstrExp<bigint, bigint>;
struct ConstrExpSuper;

template <typename CE>
struct CePtr;
using Ce32 = CePtr<ConstrExp32>;
using Ce64 = CePtr<ConstrExp64>;
using Ce96 = CePtr<ConstrExp96>;
using Ce128 = CePtr<ConstrExp128>;
using CeArb = CePtr<ConstrExpArb>;
using CeSuper = CePtr<ConstrExpSuper>;
using CeNull = CePtr<ConstrExp32>;

template <typename CF, typename DG>
struct ConstrSimple;
using ConstrSimple32 = ConstrSimple<int, long long>;
using ConstrSimple64 = ConstrSimple<long long, int128>;
using ConstrSimple96 = ConstrSimple<int128, int128>;
using ConstrSimple128 = ConstrSimple<int128, int256>;
using ConstrSimpleArb = ConstrSimple<bigint, bigint>;
struct ConstrSimpleSuper;

struct Constr;
struct Clause;
struct Cardinality;

template <typename CF, typename DG>
struct Counting;
using Counting32 = Counting<int, long long>;
using Counting64 = Counting<long long, int128>;
using Counting96 = Counting<int128, int128>;

template <typename CF, typename DG>
struct Watched;
using Watched32 = Watched<int, long long>;
using Watched64 = Watched<long long, int128>;
using Watched96 = Watched<int128, int128>;

template <typename CF, typename DG>
struct CountingSafe;
using CountingSafe32 = CountingSafe<int, long long>;
using CountingSafe64 = CountingSafe<long long, int128>;
using CountingSafe96 = CountingSafe<int128, int128>;
using CountingSafeArb = CountingSafe<bigint, bigint>;

template <typename CF, typename DG>
struct WatchedSafe;
using WatchedSafe32 = WatchedSafe<int, long long>;
using WatchedSafe64 = WatchedSafe<long long, int128>;
using WatchedSafe96 = WatchedSafe<int128, int128>;
using WatchedSafeArb = WatchedSafe<bigint, bigint>;

template <typename CF>
struct Term {
  Term() : c(0), l(0) {}
  Term(const CF& x, Lit y) : c(x), l(y) {}
  CF c;
  Lit l;
};

template <typename CF>
std::ostream& operator<<(std::ostream& o, const Term<CF>& t) {
  return o << t.c << "x" << t.l;
}

inline class AsynchronousInterrupt : public std::exception {
 public:
  virtual const char* what() const throw() { return "Program interrupted by user."; }
} asynchInterrupt;

}  // namespace rs


#pragma once

#include <crab/common/os.hpp>
#include <boost/functional/hash.hpp>
#include <gmp.h>

// TODO: replace ikos with crab namespace. This class has nothing to
// do with the ikos one. Kept for now for compatibility issues with
// some clients.
namespace ikos {

// GMP can convert directly from/to signed/unsigned long and
// signed/unsigned int. However, the C++11 standard only guarantees:
//
//  - unsigned/signed int  >= 16 bits.
//  - unsigned/signed long >= 32 bits.
//

// TODO/FIXME:
//
// We don't have a conversion from GMP numbers to 64-bit integers,
// because GMP cannot convert directly from/to int64_t or
// uint64_t. For that, we need to use mpz_export and mpz_import but
// they are significantly more expensive.
//
// Note that the actual size of **long** integer varies depending on
// the architecture and OS (see e.g.,
// https://en.cppreference.com/w/cpp/language/types). For instance,
// both Linux and mac OS on an Intel 64, the size of long integers is
// 8 bytes. But for Windows on Intel 64, the size is 4 bytes.

class z_number {
  friend class q_number;

private:
  mpz_t _n;

public:

  // overloaded typecast operators
  explicit operator long() const;
  explicit operator int() const;

  z_number();
  z_number(signed long long int n);
  z_number(const std::string& s, unsigned base = 10);

  static z_number from_ulong(unsigned long n);
  static z_number from_slong(signed long n);
  static z_number from_mpz_t(mpz_t n);
  static z_number from_mpz_srcptr(mpz_srcptr n);

  z_number(const z_number& o);
  z_number(z_number&& o);
  z_number& operator=(const z_number& o);
  z_number& operator=(z_number&& o);

  ~z_number();

  mpz_srcptr get_mpz_t() const { return _n; }

  mpz_ptr get_mpz_t() { return _n; }

  std::string get_str(unsigned base = 10) const;

  std::size_t hash() const;

  bool fits_sint() const;

  bool fits_slong() const;

  z_number operator+(z_number x) const;

  z_number operator*(z_number x) const;

  z_number operator-(z_number x) const;

  z_number operator-() const;

  z_number operator/(z_number x) const;

  z_number operator%(z_number x) const;

  z_number& operator+=(z_number x);

  z_number& operator*=(z_number x);

  z_number& operator-=(z_number x);

  z_number& operator/=(z_number x);

  z_number& operator%=(z_number x);

  z_number& operator--();

  z_number& operator++();

  z_number operator++(int);

  z_number operator--(int);

  bool operator==(z_number x) const;

  bool operator!=(z_number x) const;

  bool operator<(z_number x) const;

  bool operator<=(z_number x) const;

  bool operator>(z_number x) const;

  bool operator>=(z_number x) const;

  z_number operator&(z_number x) const;

  z_number operator|(z_number x) const;

  z_number operator^(z_number x) const;

  z_number operator<<(z_number x) const;

  z_number operator>>(z_number x) const;

  z_number fill_ones() const;

  void write(crab::crab_os& o) const;

}; // class z_number

class q_number {

private:
  mpq_t _n;

public:

  q_number();
  q_number(double n);

  q_number(const std::string& s, unsigned base = 10);
  q_number(const z_number& n);
  q_number(const z_number& n, const z_number& d);

  static q_number from_mpq_t(mpq_t n);
  static q_number from_mpz_t(mpz_t n);
  static q_number from_mpq_srcptr(mpq_srcptr q);

  q_number(const q_number& o);
  q_number(q_number&& o);
  q_number& operator=(const q_number& o);
  q_number& operator=(q_number&& o);

  ~q_number();

  mpq_srcptr get_mpq_t() const { return _n; }

  mpq_ptr get_mpq_t() { return _n; }

  double get_double() const;

  std::string get_str(unsigned base = 10) const;

  std::size_t hash() const;

  q_number operator+(q_number x) const;

  q_number operator*(q_number x) const;

  q_number operator-(q_number x) const;

  q_number operator-() const;

  q_number operator/(q_number x) const;

  q_number& operator+=(q_number x);

  q_number& operator*=(q_number x);

  q_number& operator-=(q_number x);

  q_number& operator/=(q_number x);

  q_number& operator--();

  q_number& operator++();

  q_number operator--(int);

  q_number operator++(int);

  bool operator==(q_number x) const;

  bool operator!=(q_number x) const;

  bool operator<(q_number x) const;

  bool operator<=(q_number x) const;

  bool operator>(q_number x) const;

  bool operator>=(q_number x) const;

  z_number numerator() const;

  z_number denominator() const;

  z_number round_to_upper() const;

  z_number round_to_lower() const;

  void write(crab::crab_os& o) const;

}; // class q_number

inline crab::crab_os& operator<<(crab::crab_os& o, const z_number& z) {
  z.write(o);
  return o;
}

inline crab::crab_os& operator<<(crab::crab_os& o, const q_number& q) {
  q.write(o);
  return o;
}

/** for boost::hash_combine **/
inline std::size_t hash_value(const z_number& z) {
  return z.hash();
}

inline std::size_t hash_value(const q_number& q) {
  return q.hash();
}
} //end namespace

/** for specializations of std::hash **/
namespace std {
template<>
struct hash<ikos::z_number> {
  size_t operator()(const ikos::z_number& z) const {
    return z.hash();
  }
};

template<>
struct hash<ikos::q_number> {
  size_t operator()(const ikos::q_number& q) const {
    return q.hash();
  }
};
}


#include <iostream>
#include <algorithm>
#include <cmath>
#include <vector>
#include <map>
#include <set>
#include <tuple>
#include <stdbool.h>
#include <bitset>
#include <string>
#include <boost/multiprecision/cpp_int.hpp>

namespace mp = boost::multiprecision;
using namespace std;

int mizuyari(vector<int> h) {
    // 0が含まれない区間で分割統治する
    // for(const auto& x : h) {
    //     cout << x << ", ";
    // }
    // cout << endl;

    int count = 0;
    for(int i = 0 ; i < h.size() ; ) {
        if (!h[i]) {
            i++;
            continue;
        }
        vector<int> sub; // 分割統治するvector
        int j;
        for(j = i ; j < h.size() ; ++j ) {
            if(!h[j]) {
                break;
            } else {
                sub.push_back(h[j] - 1); // 水やりする
            }
        }
        // (i, j]区間でいける
        count++; // 今回の水やり分
        count += mizuyari(sub); // 水をやった後の残りはまかした
        // 後処理
        i = j;
    }
    return count;
}

int main(void) {
    int n;
    cin >> n;
    vector<int> h(n);
    for(int i = 0 ; i < n ; ++i) {
        cin >> h[i];
    }
    auto ans = mizuyari(h);
    cout << ans << endl;
    return 0;
}


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#ifndef TURI_GRADIENT_DESCENT_H_
#define TURI_GRADIENT_DESCENT_H_

#include <flexible_type/flexible_type.hpp>
#include <Eigen/Core>

#include <optimization/utils.hpp>
#include <optimization/optimization_interface.hpp>
#include <optimization/regularizer_interface.hpp>
#include <optimization/line_search-inl.hpp>
#include <table_printer/table_printer.hpp>

// TODO: List of todo's for this file
//------------------------------------------------------------------------------
// 1. Constant line seach tuning?

namespace turi {

namespace optimization {

/**
 * \ingroup group_optimization
 * \addtogroup gradient_descent Gradient Descent
 * \{
 */

/**
 *
 * Solve a first_order_optimization_iterface model with a gradient descent
 * method.
 *
 * \param[in,out] model  Model with first order optimization interface.
 * \param[in] init_point Starting point for the solver.
 * \param[in,out] opts   Solver options.
 * \returns stats        Solver return stats.
 * \param[in] reg        Shared ptr to an interface to a regularizer.
 * \tparam Vector        Sparse or dense gradient representation.
 *
 *
*/
template <typename Vector = DenseVector>
inline solver_return gradient_descent(first_order_opt_interface& model,
    const DenseVector& init_point,
    std::map<std::string, flexible_type>& opts,
    const std::shared_ptr<regularizer_interface> reg=NULL){

    // Benchmarking utils.
    timer t;
    double start_time = t.current_time();

    logprogress_stream << "Starting Gradient Descent " << std::endl;
    logprogress_stream << "--------------------------------------------------------" << std::endl;
    std::stringstream ss;
    ss.str("");

    // Step 1: Algorithm option init
    // ------------------------------------------------------------------------
    // Check that all solver options are present.
    // Load options
    size_t iter_limit = opts["max_iterations"];
    double convergence_threshold = opts["convergence_threshold"];
    double step_size = opts["step_size"];
    size_t iters = 1;
    solver_return stats;

    // Print progress
    table_printer printer(
        model.get_status_header({"Iteration", "Passes", "Step size", "Elapsed Time"}));
    printer.print_header();

    // First compute the residual. Sometimes, you already have the solution
    // during the starting point. In these settings, you don't want to waste
    // time performing a step of the algorithm.
    DenseVector point = init_point;
    Vector gradient(point.size());
    double func_value;
    model.compute_first_order_statistics(point, gradient, func_value);
    double residual = compute_residual(gradient);

    stats.func_evals++;
    stats.gradient_evals++;

    // Needs to store previous point and gradient information
    DenseVector delta_point = point;
    delta_point.setZero();

    // First iteration will take longer. Warn the user.
    logprogress_stream <<"Tuning step size. First iteration could take longer"
                       <<" than subsequent iterations." << std::endl;

    // Nan Checking!
    if (!std::isfinite(residual)) {
      stats.status = OPTIMIZATION_STATUS::OPT_NUMERIC_OVERFLOW;
    }

    // Step 2: Algorithm starts here
    // ------------------------------------------------------------------------
    // While not converged
    while((residual >= convergence_threshold) && (iters <= iter_limit)){

      // Line search for step size.
      ls_return ls_stats;

      // Pick line search based on regularizers.
      if (reg != NULL){
        step_size  *= 2;
        ls_stats =  backtracking(model,
                                 step_size,
                                 func_value,
                                 point,
                                 gradient,
                                 -gradient,
                                 reg);
      } else {
          ls_stats =  more_thuente(model,
                                   step_size,
                                   func_value,
                                   point,
                                   gradient,
                                   -gradient);
      }

      // Add info from line search
      stats.func_evals += ls_stats.func_evals;
      stats.gradient_evals += ls_stats.gradient_evals;
      step_size = ls_stats.step_size;

      // Line search failed
      if (ls_stats.status == false){
        stats.status = OPTIMIZATION_STATUS::OPT_LS_FAILURE;
        break;
      }

      // \delta x_k = x_{k} - x_{k-1}
      delta_point =  point;
      point = point -step_size * gradient;
      if (reg != NULL)
        reg->apply_proximal_operator(point, step_size);
      delta_point = point - delta_point;

      // Numerical error: Insufficient progress.
      if (delta_point.norm() <= OPTIMIZATION_ZERO){
        stats.status = OPTIMIZATION_STATUS::OPT_NUMERIC_ERROR;
        break;
      }
      // Numerical error: Numerical overflow. (Step size was too large)
      if (!delta_point.array().array().isFinite().all()) {
        stats.status = OPTIMIZATION_STATUS::OPT_NUMERIC_OVERFLOW;
        break;
      }

      // Compute residual norm (to check for convergence)
      model.compute_first_order_statistics(point, gradient, func_value);
      stats.num_passes++;
      residual = compute_residual(gradient);
      iters++;

      // Print progress
      auto stat_info = {std::to_string(iters),
                        std::to_string(stats.num_passes),
                        std::to_string(step_size),
                        std::to_string(t.current_time())};

      auto row = model.get_status(point, stat_info);
      printer.print_progress_row_strs(iters, row);
    }

    printer.print_footer();

    // Step 3: Return optimization model status.
    // ------------------------------------------------------------------------
    if (stats.status == OPTIMIZATION_STATUS::OPT_UNSET) {
      if (iters < iter_limit){
        stats.status = OPTIMIZATION_STATUS::OPT_OPTIMAL;
      } else {
        stats.status = OPTIMIZATION_STATUS::OPT_ITERATION_LIMIT;
      }
    }
    stats.iters = iters;
    stats.residual = residual;
    stats.gradient = gradient;
    stats.func_value = func_value;
    stats.solve_time = t.current_time() - start_time;
    stats.solution = point;
    stats.progress_table = printer.get_tracked_table();

    // Display solver stats
    log_solver_summary_stats(stats);
    return stats;
}

} // optimizaiton

/// \}
} // turicreate

#endif


/** hoNFFT.cpp */

#include "hoNFFT.h"

#include "hoNDFFT.h"
#include "hoNDArray_elemwise.h"
#include "hoNDArray_reductions.h"
#include "hoNDArray_utils.h"

#include "vector_td_utilities.h"
#include "vector_td_operators.h"
#include "vector_td_io.h"

#include <algorithm>
#include <vector>
#include <cmath>
#include <stdexcept>

#include <boost/range/algorithm/transform.hpp>

#include "GadgetronTimer.h"

#include "NFFT.hpp"
#include "NDArray_utils.h"

#include "hoGriddingConvolution.h"

using namespace std;

namespace Gadgetron {

    namespace {

        template<typename T, unsigned int D>
        struct FFTD {
        };

        template<typename T>
        struct FFTD<T, 1> {
            using REAL = typename realType<T>::Type;

            static void fft(hoNDArray<T> &array, NFFT_fft_mode mode, bool do_scale) {
                if (mode == NFFT_fft_mode::FORWARDS) {
                    hoNDFFT<REAL>::instance()->fft1c(array);
                } else {
                    hoNDFFT<REAL>::instance()->ifft1c(array);
                }
                if (!do_scale) array *= std::sqrt(REAL(array.get_size(0)));
            }
        };

        template<typename T>
        struct FFTD<T, 2> {
            using REAL = typename realType<T>::Type;

            static void fft(hoNDArray<T> &array, NFFT_fft_mode mode, bool do_scale ) {
                if (mode == NFFT_fft_mode::FORWARDS) {
                    hoNDFFT<REAL>::instance()->fft2c(array);
                } else {
                    hoNDFFT<REAL>::instance()->ifft2c(array);
                }

                if (!do_scale) array *= std::sqrt(REAL(array.get_size(0)*array.get_size(1)));

            }
        };

        template<typename T>
        struct FFTD<T, 3> {
            using REAL = typename realType<T>::Type;

            static void fft(hoNDArray<T> &array, NFFT_fft_mode mode, bool do_scale) {
                if (mode == NFFT_fft_mode::FORWARDS) {
                    hoNDFFT<REAL>::instance()->fft3c(array);
                } else {
                    hoNDFFT<REAL>::instance()->ifft3c(array);
                }

                if (!do_scale) array *= std::sqrt(REAL(array.get_size(0)*array.get_size(1)*array.get_size(2)));
            }
        };

        template<class REAL, template<class, unsigned int> class K>
        hoNDArray<std::complex<REAL>> compute_deapodization_filter(
            const vector_td<size_t, 1>& image_dims,
            const ConvolutionKernel<REAL, 1, K>& kernel)
        {
            hoNDArray<std::complex<REAL>> deapodization(to_std_vector(image_dims));
            vector_td<REAL,1> image_dims_real(image_dims);
            for (int x = 0; x < image_dims[0]; x++){
                auto offset = x - image_dims_real[0]/2;
                deapodization(x) = kernel.get(offset, 0);
            }
            return deapodization;
        }

        template<class REAL, template<class, unsigned int> class K>
        hoNDArray<std::complex<REAL>> compute_deapodization_filter(
            const vector_td<size_t, 2>& image_dims,
            const ConvolutionKernel<REAL, 2, K>& kernel)
        {
            hoNDArray<std::complex<REAL>> deapodization(to_std_vector(image_dims));
            vector_td<REAL,2> image_dims_real(image_dims);
            for (int y = 0; y < image_dims[1]; y++) {
                auto offset_y = y - image_dims_real[1]/2;
                auto weight_y = kernel.get(offset_y, 1);

                for (int x = 0; x < image_dims[0]; x++) {
                    auto offset_x = x - image_dims_real[0]/2;
                    auto weight_x = kernel.get(offset_x, 0);

                    deapodization(x,y) = weight_x*weight_y;
                }
            }
            return deapodization;
        }

        template<class REAL, template<class, unsigned int> class K>
        hoNDArray<std::complex<REAL>> compute_deapodization_filter(
            const vector_td<size_t, 3>& image_dims,
            const ConvolutionKernel<REAL, 3, K>& kernel)
        {
            hoNDArray<std::complex<REAL>> deapodization(to_std_vector(image_dims));
            vector_td<REAL,3> image_dims_real(image_dims);
            for (int z = 0; z < image_dims[2]; z++) {
                auto offset_z = z - image_dims_real[2]/2;
                auto weight_z = kernel.get(offset_z, 2);

                for (int y = 0; y < image_dims[1]; y++) {
                    auto offset_y = y - image_dims_real[1]/2;
                    auto weight_y = kernel.get(offset_y, 1);

                    for (int x = 0; x < image_dims[0]; x++) {
                        auto offset_x = x - image_dims_real[0]/2;
                        auto weight_x = kernel.get(offset_x, 0);

                        deapodization(x,y,z) = weight_x*weight_y*weight_z;
                    }
                }
            }
            return deapodization;
        }
    }

    template<class REAL, unsigned int D>
    hoNFFT_plan<REAL, D>::hoNFFT_plan(
        const vector_td<size_t, D> &matrix_size,
        const vector_td<size_t, D> &matrix_size_os,
        REAL W)
      : NFFT_plan<hoNDArray,REAL,D>(matrix_size,matrix_size_os,W)
    {
        this->deapodization_filter_IFFT = compute_deapodization_filter(
            this->matrix_size_os_, this->conv_->get_kernel());
        this->deapodization_filter_FFT = deapodization_filter_IFFT;

        FFTD<std::complex<REAL>,D>::fft(
            deapodization_filter_IFFT, NFFT_fft_mode::BACKWARDS, true);
        FFTD<std::complex<REAL>,D>::fft(
            deapodization_filter_FFT, NFFT_fft_mode::FORWARDS, true);

        boost::transform(deapodization_filter_IFFT,
                         deapodization_filter_IFFT.begin(),
                         [](auto val) { return REAL(1)/val; });
        boost::transform(deapodization_filter_FFT,
                         deapodization_filter_FFT.begin(),
                         [](auto val) { return REAL(1)/val; });
    }

    template<class REAL, unsigned int D>
    hoNFFT_plan<REAL, D>::hoNFFT_plan(
        const vector_td<size_t, D> &matrix_size,
        REAL oversampling_factor,
        REAL W)
      : NFFT_plan<hoNDArray, REAL, D>(matrix_size, oversampling_factor, W)
    {
        this->deapodization_filter_IFFT = compute_deapodization_filter(
            this->matrix_size_os_, this->conv_->get_kernel());
        this->deapodization_filter_FFT = deapodization_filter_IFFT;

        FFTD<std::complex<REAL>, D>::fft(
            deapodization_filter_IFFT, NFFT_fft_mode::BACKWARDS, false);
        FFTD<std::complex<REAL>, D>::fft(
            deapodization_filter_FFT, NFFT_fft_mode::FORWARDS, false);

        boost::transform(deapodization_filter_IFFT,
                         deapodization_filter_IFFT.begin(),
                         [](auto val) { return REAL(1) / val; });
        boost::transform(deapodization_filter_FFT,
                         deapodization_filter_FFT.begin(),
                         [](auto val) { return REAL(1) / val; });
    }

    template<class REAL, unsigned int D>
    void hoNFFT_plan<REAL, D>::compute(
            const hoNDArray<ComplexType> &d,
            hoNDArray<ComplexType> &m,
            const hoNDArray<REAL> *dcw,
            NFFT_comp_mode mode
    ) {
        const auto *pd = reinterpret_cast<const hoNDArray<complext<REAL>> *>(&d);
        auto *pm = reinterpret_cast<hoNDArray<complext<REAL>> *>(&m);

        this->compute(*pd, *pm, dcw, mode);
    }

    template<class REAL, unsigned int D>
    void hoNFFT_plan<REAL, D>::compute(
            const hoNDArray<complext<REAL>> &d,
            hoNDArray<complext<REAL>> &m,
            const hoNDArray<REAL> *dcw,
            NFFT_comp_mode mode
    ) {
       NFFT_plan<hoNDArray,REAL,D>::compute(d,m,dcw,mode);
    }

    template<class REAL, unsigned int D>
    void hoNFFT_plan<REAL, D>::mult_MH_M(
            const hoNDArray<complext<REAL>> &in,
            hoNDArray<complext<REAL>> &out,
            const hoNDArray<REAL>* dcw
    ) {
        const hoNDArray<ComplexType> *pin = reinterpret_cast<const hoNDArray<ComplexType> *>(&in);
        hoNDArray<ComplexType> *pout = reinterpret_cast<hoNDArray<ComplexType> *>(&out);

        this->mult_MH_M(*pin, *pout, dcw);
    }

    template<class REAL, unsigned int D>
    void hoNFFT_plan<REAL, D>::mult_MH_M(
            const hoNDArray<ComplexType> &in,
            hoNDArray<ComplexType> &out,
            const hoNDArray<REAL>* dcw
    ) {
        std::vector<size_t> dims = {this->number_of_samples,this->number_of_frames};
        auto batches = in.get_number_of_elements()/(prod(this->matrix_size_)*this->number_of_frames);
        dims.push_back(batches);

        hoNDArray<ComplexType> tmp(dims);
        compute(in, tmp, dcw, NFFT_comp_mode::FORWARDS_C2NC);
        compute(tmp, out,dcw, NFFT_comp_mode::BACKWARDS_NC2C);
    }

    template<class REAL, unsigned int D>
    void hoNFFT_plan<REAL, D>::fft(
            hoNDArray<ComplexType> &d,
            NFFT_fft_mode mode,
            bool do_scale
    ) {
        FFTD<std::complex<REAL>, D>::fft(d, mode,do_scale);
    }

    template<class REAL, unsigned int D>
    void hoNFFT_plan<REAL, D>::fft(
            hoNDArray<complext<REAL>> &d,
            NFFT_fft_mode mode,
            bool do_scale
    ) {
        hoNDArray<ComplexType> *pd = reinterpret_cast<hoNDArray<ComplexType> *>(&d);
        this->fft(*pd,mode,do_scale);
    }

    template<class REAL, unsigned int D>
    void hoNFFT_plan<REAL, D>::deapodize(
            hoNDArray<ComplexType> &d,
            bool fourierDomain
    ) {
        if (fourierDomain){
            d *= deapodization_filter_FFT;
        } else {
            d *= deapodization_filter_IFFT;
        }
    }

    template<class REAL, unsigned int D>
    void hoNFFT_plan<REAL, D>::deapodize(
            hoNDArray<complext<REAL>> &d,
            bool fourierDomain
    ) {
        hoNDArray<ComplexType> *pd = reinterpret_cast<hoNDArray<ComplexType> *>(&d);
        this->deapodize(*pd,fourierDomain);
    }

    template<class REAL, unsigned int D>
    boost::shared_ptr<hoNFFT_plan<REAL,D>> NFFT<hoNDArray,REAL,D>::make_plan(const Gadgetron::vector_td<size_t, D> &matrix_size,
                                        const Gadgetron::vector_td<size_t, D> &matrix_size_os, REAL W) {
        return boost::make_shared<hoNFFT_plan<REAL,D>>(matrix_size,matrix_size_os,W);
    }

}

template
class Gadgetron::hoNFFT_plan<float, 1>;

template
class Gadgetron::hoNFFT_plan<float, 2>;

template
class Gadgetron::hoNFFT_plan<float, 3>;

template
class Gadgetron::hoNFFT_plan<double, 1>;

template
class Gadgetron::hoNFFT_plan<double, 2>;

template
class Gadgetron::hoNFFT_plan<double, 3>;

template class Gadgetron::NFFT<Gadgetron::hoNDArray,float,1>;
template class Gadgetron::NFFT<Gadgetron::hoNDArray,float,2>;
template class Gadgetron::NFFT<Gadgetron::hoNDArray,float,3>;

template class Gadgetron::NFFT<Gadgetron::hoNDArray,double,1>;
template class Gadgetron::NFFT<Gadgetron::hoNDArray,double,2>;
template class Gadgetron::NFFT<Gadgetron::hoNDArray,double,3>;


/* =========================================================================
   Copyright (c) 2012-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
                             -----------------
               ViennaFEM - The Vienna Finite Element Method Library
                             -----------------

   Author:     Karl Rupp                          rupp@iue.tuwien.ac.at

   License:    MIT (X11), see file LICENSE in the ViennaFEM base directory
============================================================================ */

// include necessary system headers
#include <iostream>

// ViennaFEM includes:
#include "viennafem/fem.hpp"
#include "viennafem/io/vtk_writer.hpp"

// ViennaGrid includes:
#include "viennagrid/forwards.hpp"
#include "viennagrid/config/default_configs.hpp"
#include "viennagrid/io/netgen_reader.hpp"

// ViennaData includes:
#include "viennadata/api.hpp"

// ViennaMath includes:
#include "viennamath/expression.hpp"

// Boost.uBLAS includes:
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

//ViennaCL includes:
#ifndef VIENNACL_HAVE_UBLAS
 #define VIENNACL_HAVE_UBLAS
#endif

#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/linalg/prod.hpp"

int main()
{
  typedef viennagrid::tetrahedral_3d_mesh                                                 DomainType;
  typedef viennagrid::result_of::segmentation<DomainType>::type                           SegmentationType;
  typedef viennagrid::result_of::element<DomainType, viennagrid::vertex_tag>::type        VertexType;
  typedef viennagrid::result_of::element_range<DomainType, viennagrid::vertex_tag>::type  VertexContainer;
  typedef viennagrid::result_of::iterator<VertexContainer>::type                          VertexIterator;

  typedef boost::numeric::ublas::compressed_matrix<viennafem::numeric_type>  MatrixType;
  typedef boost::numeric::ublas::vector<viennafem::numeric_type>             VectorType;

  typedef viennamath::function_symbol   FunctionSymbol;
  typedef viennamath::equation          Equation;

  //
  // Create a domain from file
  //
  DomainType my_domain;
  SegmentationType segments(my_domain);

  //
  // Create a storage object
  //
  typedef viennadata::storage<> StorageType;
  StorageType   storage;

  try
  {
    viennagrid::io::netgen_reader my_reader;
    my_reader(my_domain, segments, "../examples/data/sshape3d-pimped.mesh");
  }
  catch (...)
  {
    std::cerr << "File-Reader failed. Aborting program..." << std::endl;
    exit(EXIT_FAILURE);
  }

  //
  // Specify PDE:
  //
  FunctionSymbol u(0, viennamath::unknown_tag<>());   //an unknown function used for PDE specification
  Equation poisson_equ_1 = viennamath::make_equation( viennamath::laplace(u), -1);

  MatrixType system_matrix;
  VectorType load_vector;

  //
  // Setting boundary information on domain (this should come from device specification)
  //
  //setting some boundary flags:
  VertexContainer vertices = viennagrid::elements<VertexType>(my_domain);
  for (VertexIterator vit = vertices.begin();
      vit != vertices.end();
      ++vit)
  {
    //boundary for first equation: Homogeneous Dirichlet everywhere
    if (viennagrid::point(my_domain, *vit)[2] == 3.0 || viennagrid::point(my_domain, *vit)[1] == 3.0 )
      viennafem::set_dirichlet_boundary(storage, *vit, 0.0);
  }

  //
  // Create PDE solver functors: (discussion about proper interface required)
  //
  viennafem::pde_assembler<StorageType> fem_assembler(storage);

  //
  // Solve system and write solution vector to pde_result:
  // (discussion about proper interface required. Introduce a pde_result class?)
  //
  fem_assembler(viennafem::make_linear_pde_system(poisson_equ_1, u),
                my_domain,
                system_matrix,
                load_vector
               );

  VectorType pde_result = viennacl::linalg::solve(system_matrix, load_vector, viennacl::linalg::cg_tag());
  std::cout << "* solve(): Residual: " << norm_2(prod(system_matrix, pde_result) - load_vector) << std::endl;

  //
  // Writing solution back to domain (discussion about proper way of returning a solution required...)
  //
  viennafem::io::write_solution_to_VTK_file(pde_result, "sshape_3d", my_domain, segments, storage, 0);

  std::cout << "*****************************************" << std::endl;
  std::cout << "* Poisson solver finished successfully! *" << std::endl;
  std::cout << "*****************************************" << std::endl;
  return EXIT_SUCCESS;
}


#include <boost/config.hpp>
#include <iostream>
#include <fstream>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <QDebug>
#include "shortestpath.h"

using namespace boost;

bool shortestPath(size_t nodeNum,
    const std::vector<std::pair<size_t, size_t>> &edges,
    const std::vector<int> &weights,
    size_t start,
    size_t stop,
    std::vector<size_t> *path)
{
    typedef adjacency_list < listS, vecS, undirectedS,
        no_property, property < edge_weight_t, int > > graph_t;
    typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
    typedef graph_traits < graph_t >::edge_descriptor edge_descriptor;
    typedef std::pair<size_t, size_t> Edge;

    size_t edgeNum = edges.size();
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
    graph_t g(nodeNum);
    property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);
    for (std::size_t j = 0; j < edgeNum; ++j) {
        edge_descriptor e; bool inserted;
        tie(e, inserted) = add_edge(edges[j].first, edges[j].second, g);
        weightmap[e] = weights[j];
    }
#else
    graph_t g(edges.data(), edges.data() + edgeNum, weights.data(), nodeNum);
    property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);
#endif

    std::vector<vertex_descriptor> p(num_vertices(g));
    std::vector<size_t> d(num_vertices(g));
    vertex_descriptor s = vertex(start, g);

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
    // VC++ has trouble with the named parameters mechanism
    property_map<graph_t, vertex_index_t>::type indexmap = get(vertex_index, g);
    dijkstra_shortest_paths(g, s, &p[0], &d[0], weightmap, indexmap,
                          std::less<size_t>(), closed_plus<size_t>(),
                          (std::numeric_limits<size_t>::max)(), 0,
                          default_dijkstra_visitor());
#else
    dijkstra_shortest_paths(g, s, predecessor_map(&p[0]).distance_map(&d[0]));
#endif

    auto current = stop;
    while (current != start) {
        path->push_back(current);
        size_t next = p[current];
        if (next == current)
            return false;
        current = next;
    }
    path->push_back(current);

    return true;
}


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_MATH_INTERIORPOINTOPTIMIZER_HPP
#define RW_MATH_INTERIORPOINTOPTIMIZER_HPP

#include <rw/math/Math.hpp>

#include <iostream>
#include <boost/function.hpp>
#include <boost/bind.hpp>

namespace rw {
namespace math {

class InteriorPointOptimizer
{
public:

    typedef boost::function<void(const Eigen::VectorXd& x,
                                 double& f,
                                 Eigen::VectorXd& df,
                                 Eigen::MatrixXd& ddf) > ObjectFunction;

    typedef void*(Q& q) ObjectFunction;

    typedef boost::function<void(const Eigen::VectorXd& x,
                                 size_t no,
                                 Eigen::VectorXd& g,
                                 Eigen::MatrixXd& dg,
                                 Eigen::MatrixXd& ddq) > ConstraintFunction;

    InteriorPointOptimizer(size_t n,
                           size_t m,
                           ObjectFunction objectFunction,
                           ConstraintFunction constraintFunction);

    virtual ~InteriorPointOptimizer();

    int solve(const Eigen::VectorXd& x_init);

    void setAccuracy(double accuracy);
    double getAccuracy();

    void verify_user_defined_objective_and_constraints();

protected:
    InteriorPointOptimizer(size_t n, size_t m);

    void initialize();

    virtual void objectFunction(const Eigen::VectorXd& x,
                                double &f,
                                Eigen::VectorXd &df,
                                Eigen::MatrixXd &ddf);

    virtual void constraintFunction(const Eigen::VectorXd& x,
                                    int i,
                                    Eigen::VectorXd &a,
                                    Eigen::MatrixXd &da,
                                    Eigen::MatrixXd &dda);

private:
    ObjectFunction compute_f_info_EXT;
    ConstraintFunction compute_con_info_i_EXT;

    void choleskySolve(int n_e,
                       int bw,
                       Eigen::MatrixXd &A,
                       Eigen::VectorXd &b,
                       Eigen::VectorXd &x);

    void compute_f_info(Eigen::MatrixXd &A,
                        Eigen::VectorXd &RHS);

    void compute_con_info(Eigen::MatrixXd &A,
                          Eigen::VectorXd &RHS);

    void merit_info(Eigen::VectorXd &x,
                    Eigen::VectorXd &s,
                    double &phi,
                    double &eta);

    void Dmerit_info(Eigen::VectorXd &x,
                     Eigen::VectorXd &s,
                     Eigen::VectorXd &dx,
                     Eigen::VectorXd &ds,
                     double &Dphi,
                     double &eta);

    void update(Eigen::VectorXd &x,
                Eigen::VectorXd &dx,
                Eigen::VectorXd &s,
                Eigen::VectorXd &z);

    const size_t N;
    const size_t M;
    double _accuracy;
    Eigen::VectorXd _x;
    Eigen::VectorXd _s;
    Eigen::VectorXd _z;
    double _mu, _eta;

    // objective and derivatives
    double _f;
    Eigen::VectorXd _df;
    Eigen::MatrixXd _ddf;

    // constraints and derivatives (second derivative only stored for one constraint at a time)
    Eigen::VectorXd _a;
    Eigen::MatrixXd _da;
    Eigen::MatrixXd _dda;

};

} //end namespace math
} //end namespace rw

#endif //end include guard


//ros
#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseArray.h>

//ipopt
#include <Eigen/Core>
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>

using CppAD::AD;

class MPC{
public:
    MPC();

    // state, ref_x, ref_y, ref_yaw
    std::vector<double> solve(Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd);

};

class FG_eval{
public:
    FG_eval(Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd);

    typedef CPPAD_TESTVECTOR(AD<double>) ADvector;

    void operator()(ADvector&, const ADvector&);

private:
    Eigen::VectorXd ref_x;
    Eigen::VectorXd ref_y;
    Eigen::VectorXd ref_yaw;

};

class MPCPathTracker
{
public:
    MPCPathTracker(void);

    void path_callback(const nav_msgs::PathConstPtr&);

    void process(void);
    void path_to_vector(void);

private:
    ros::NodeHandle nh;
    ros::Publisher velocity_pub;
    ros::Publisher path_pub;
    ros::Subscriber path_sub;
    MPC mpc;
    nav_msgs::Path path;
    tf::TransformListener listener;
    geometry_msgs::PoseStamped current_pose;
    geometry_msgs::PoseStamped previous_pose;
    tf::StampedTransform _transform;
    geometry_msgs::TransformStamped transform;
    Eigen::VectorXd path_x;
    Eigen::VectorXd path_y;
    Eigen::VectorXd path_yaw;
    bool first_transform = true;
    double last_time;

};

// ホライゾン長さ
int T = 15;
// 周期
double DT = 0.1;// [s]
const double HZ = 10;
// 目標速度
double VREF;// [m/s]
// 最大速度
double MAX_VELOCITY; // [m/s]
// 最大角速度
double MAX_ANGULAR_VELOCITY;// [rad/s]
// ホイール角加速度
double WHEEL_ANGULAR_ACCELERATION_LIMIT;// [rad/s^2]
// ホイール角速度
double WHEEL_ANGULAR_VELOCITY_LIMIT;// [rad/s]
// ホイール半径
double WHEEL_RADIUS;// [m]
// トレッド
double TREAD;// [m]
// グリッドマップ分解能
double RESOLUTION;// [m]

std::string WORLD_FRAME;
std::string ROBOT_FRAME;
std::string VELOCITY_TOPIC_NAME;
std::string INTERMEDIATE_PATH_TOPIC_NAME;

// state
size_t x_start = 0;
size_t y_start = x_start + T;
size_t yaw_start = y_start + T;
size_t v_start = yaw_start + T;
size_t omega_start = v_start + T;
size_t omega_r_start = omega_start + T;
size_t omega_l_start = omega_r_start + T;
// input
size_t domega_r_start = omega_l_start + T;
size_t domega_l_start = domega_r_start + T - 1;

// 最適化失敗時は最後の成功データを使う
int failure_count = 0;
std::vector<double> result;

double min_distance(nav_msgs::Path&, geometry_msgs::PoseStamped&);
double get_distance(geometry_msgs::PoseStamped&, geometry_msgs::PoseStamped&);

int main(int argc, char** argv)
{
    ros::init(argc, argv, "diff_drive_mpc");
    ros::NodeHandle local_nh("~");

    local_nh.getParam("HORIZON_T", T);
    local_nh.getParam("/dynamic_avoidance/VREF", VREF);
    local_nh.getParam("/dynamic_avoidance/MAX_ANGULAR_VELOCITY", MAX_ANGULAR_VELOCITY);
    local_nh.getParam("/diff_drive/MAX_WHEEL_ANGULAR_ACCELERATION", WHEEL_ANGULAR_ACCELERATION_LIMIT);
    local_nh.getParam("/diff_drive/MAX_WHEEL_ANGULAR_VELOCITY", WHEEL_ANGULAR_VELOCITY_LIMIT);
    local_nh.getParam("/diff_drive/WHEEL_RADIUS", WHEEL_RADIUS);
    local_nh.getParam("/diff_drive/TREAD", TREAD);
    local_nh.getParam("/diff_drive/MAX_VELOCITY", MAX_VELOCITY);
    local_nh.getParam("/dynamic_avoidance/RESOLUTION", RESOLUTION);
    local_nh.getParam("/dynamic_avoidance/ROBOT_FRAME", ROBOT_FRAME);
    local_nh.getParam("/dynamic_avoidance/WORLD_FRAME", WORLD_FRAME);
    local_nh.getParam("/dynamic_avoidance/VELOCITY_TOPIC_NAME", VELOCITY_TOPIC_NAME);
    local_nh.getParam("/dynamic_avoidance/INTERMEDIATE_PATH_TOPIC_NAME", INTERMEDIATE_PATH_TOPIC_NAME);

    std::cout << "T: " << T << std::endl;
    std::cout << "VREF: " << VREF << std::endl;
    std::cout << "MAX_VELOCITY: " << MAX_VELOCITY << std::endl;
    std::cout << "MAX_ANGULAR_VELOCITY: " << MAX_ANGULAR_VELOCITY << std::endl;
    std::cout << "WHEEL_ANGULAR_VELOCITY_LIMIT: " << WHEEL_ANGULAR_VELOCITY_LIMIT << std::endl;
    std::cout << "WHEEL_ANGULAR_ACCELERATION_LIMIT: " << WHEEL_ANGULAR_ACCELERATION_LIMIT << std::endl;
    std::cout << "WHEEL_RADIUS: " << WHEEL_RADIUS << std::endl;
    std::cout << "TREAD: " << TREAD << std::endl;
    std::cout << "RESOLUTION: " << RESOLUTION << std::endl;
    std::cout << "ROBOT_FRAME: " << ROBOT_FRAME << std::endl;
    std::cout << "WORLD_FRAME: " << WORLD_FRAME << std::endl;
    std::cout << "VELOCITY_TOPIC_NAME: " << VELOCITY_TOPIC_NAME << std::endl;
    std::cout << "INTERMEDIATE_PATH_TOPIC_NAME: " << INTERMEDIATE_PATH_TOPIC_NAME << std::endl;

    MPCPathTracker mpc_path_tracker;

    ros::Rate loop_rate(HZ);

    while(ros::ok()){
        mpc_path_tracker.process();

        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

MPC::MPC(){}

std::vector<double> MPC::solve(Eigen::VectorXd state, Eigen::VectorXd ref_x, Eigen::VectorXd ref_y, Eigen::VectorXd ref_yaw)
{
    /*
     * state:x, y, yaw, v, omega, omega_r, omega_l
     */
    bool ok = true;
    size_t i;
    typedef CPPAD_TESTVECTOR(double) Dvector;

    double x = state[0];
    double y = state[1];
    double yaw = state[2];
    double v = state[3];
    double omega = state[4];
    double omega_r = state[5];
    double omega_l = state[6];

    /*
    std::cout << "--- state ---" << std::endl;
    std::cout << state << std::endl;
    std::cout << "--- path_x ---" << std::endl;
    std::cout << ref_x << std::endl;
    std::cout << "--- path_y ---" << std::endl;
    std::cout << ref_y << std::endl;
    std::cout << "--- path_yaw ---" << std::endl;
    std::cout << ref_yaw << std::endl;
    */

    // 7(x, y, yaw, v, omega, omega_r, omega_l), 2(domega_r, domega_l)
    size_t n_variables = 7 * T + 2 * (T - 1);

    size_t n_constraints = 7 * T;

    Dvector vars(n_variables);
    for(int i=0;i<n_variables;i++){
        vars[i] = 0.0;
    }

    vars[x_start] = x;
    vars[y_start] = y;
    vars[yaw_start] = yaw;
    vars[v_start] = v;
    vars[omega_start] = omega;
    vars[omega_r_start] = omega_r;
    vars[omega_l_start] = omega_l;

    Dvector vars_lower_bound(n_variables);
    Dvector vars_upper_bound(n_variables);

    for(int i=0;i<v_start;i++){
        // x, y, yaw
        vars_lower_bound[i] = -1.0e19;
        vars_upper_bound[i] = 1.0e19;
    }
    for(int i=v_start;i<omega_start;i++){
        // v
        vars_lower_bound[i] = 0;
        vars_upper_bound[i] = MAX_VELOCITY;
    }
    for(int i=omega_start;i<omega_r_start;i++){
        // omega
        vars_lower_bound[i] = -MAX_ANGULAR_VELOCITY;
        vars_upper_bound[i] = MAX_ANGULAR_VELOCITY;
    }
    for(int i=omega_r_start;i<domega_r_start;i++){
        // omega_r, omega_l
        vars_lower_bound[i] = -WHEEL_ANGULAR_VELOCITY_LIMIT;
        vars_upper_bound[i] = WHEEL_ANGULAR_VELOCITY_LIMIT;
    }
    for(int i=omega_start;i<n_variables;i++){
        // domega_r, domega_l
        vars_lower_bound[i] = -WHEEL_ANGULAR_ACCELERATION_LIMIT;
        vars_upper_bound[i] = WHEEL_ANGULAR_ACCELERATION_LIMIT;
    }

    // 等式制約
    Dvector constraints_lower_bound(n_constraints);
    Dvector constraints_upper_bound(n_constraints);

    for(int i=0;i<n_constraints;i++){
        constraints_lower_bound[i] = 0.0;
        constraints_upper_bound[i] = 0.0;
    }

    // t=0の設定
    constraints_lower_bound[x_start] = x;
    constraints_lower_bound[y_start] = y;
    constraints_lower_bound[yaw_start] = yaw;
    constraints_lower_bound[v_start] = v;
    constraints_lower_bound[omega_start] = omega;
    constraints_lower_bound[omega_r_start] = omega_r;
    constraints_lower_bound[omega_l_start] = omega_l;

    constraints_upper_bound[x_start] = x;
    constraints_upper_bound[y_start] = y;
    constraints_upper_bound[yaw_start] = yaw;
    constraints_upper_bound[v_start] = v;
    constraints_upper_bound[omega_start] = omega;
    constraints_upper_bound[omega_r_start] = omega_r;
    constraints_upper_bound[omega_l_start] = omega_l;

    FG_eval fg_eval(ref_x, ref_y, ref_yaw);

    std::string options;
    options += "Integer print_level  0\n";

    options += "Sparse  true                forward\n";
    options += "Sparse  true                reverse\n";

    options += "Numeric max_cpu_time                    0.5\n";

    CppAD::ipopt::solve_result<Dvector> solution;

    std::cout << "optimization start" << std::endl;
    CppAD::ipopt::solve<Dvector, FG_eval>(
            options, vars, vars_lower_bound, vars_upper_bound, constraints_lower_bound,
            constraints_upper_bound, fg_eval, solution);

    std::cout << "optimization end" << std::endl;
    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
    std::cout << solution.status << std::endl;
    std::cout << ok << std::endl;

    auto cost = solution.obj_value;
    std::cout << "Cost " << cost << std::endl;

    if(ok){
        failure_count = 0;
        result.clear();
        // 何故か0だとうまく行かない
        result.push_back(solution.x[v_start+1]);
        result.push_back(solution.x[omega_start+1]);
        //予測軌道
        for(int i = 0; i < T-1; i++){
            result.push_back(solution.x[x_start+i+1]);
            result.push_back(solution.x[y_start+i+1]);
            result.push_back(solution.x[yaw_start+i+1]);
        }
    }else{
        if(failure_count < T - 1){
            failure_count++;
        }
        result.push_back(solution.x[v_start+1+failure_count]);
        result.push_back(solution.x[omega_start+1+failure_count]);
        result[v_start] = result[v_start + failure_count];
        result[omega_start] = result[omega_start + failure_count];

        //予測軌道
        for(int i = failure_count; i < T-1; i++){
            /*
            result.push_back(solution.x[x_start+i+1]);
            result.push_back(solution.x[y_start+i+1]);
            result.push_back(solution.x[yaw_start+i+1]);
            */
        }
    }
    /*
    std::cout << "--- result ---" << std::endl;
    for(int i=0;i<result.size();i++){
        std::cout << result[i] << std::endl;
    }
    */
    return result;
}

FG_eval::FG_eval(Eigen::VectorXd ref_x, Eigen::VectorXd ref_y, Eigen::VectorXd ref_yaw)
{
    this->ref_x = ref_x;
    this->ref_y = ref_y;
    this->ref_yaw = ref_yaw;
}

void FG_eval::operator()(ADvector& fg, const ADvector& vars)
{
    std::cout << "FG_eval() start" << std::endl;
    // cost
    fg[0] = 0;
    // state
    for(int i=0;i<T-1;i++){
        // pathとの距離
        fg[0] += 0.2 * (CppAD::pow(vars[x_start + i] - ref_x[i], 2) + CppAD::pow(vars[y_start + i] - ref_y[i], 2));
        // 向き
        //fg[0] += 0.1 * CppAD::pow(vars[yaw_start + i] - ref_yaw[i], 2);
        // 速度
        fg[0] += 100 * CppAD::pow(VREF - vars[v_start + i], 2);
        // 角加速度
        fg[0] += 0.1 * CppAD::pow(vars[omega_start + i] - vars[omega_start + i+ 1], 2);
    }
    // input
    for(int i=0;i<T-2;i++){
    }

    std::cout << "constrains start" << std::endl;
    //constraint
    //初期状態
    fg[1 + x_start] = vars[x_start];
    fg[1 + y_start] = vars[y_start];
    fg[1 + yaw_start] = vars[yaw_start];
    fg[1 + v_start] = vars[v_start];
    fg[1 + omega_start] = vars[omega_start];
    fg[1 + omega_r_start] = vars[omega_r_start];
    fg[1 + omega_l_start] = vars[omega_l_start];

    std::cout << "constraints loop start" << std::endl;

    for(int i=0;i<T-1;i++){
        //t+1
        AD<double> x1 = vars[x_start + i + 1];
        AD<double> y1 = vars[y_start + i + 1];
        AD<double> yaw1 = vars[yaw_start + i + 1];
        AD<double> v1 = vars[v_start + i + 1];
        AD<double> omega1 = vars[omega_start + i + 1];
        AD<double> omega_r1 = vars[omega_r_start + i + 1];
        AD<double> omega_l1 = vars[omega_l_start + i + 1];
        //t
        AD<double> x0 = vars[x_start + i];
        AD<double> y0 = vars[y_start + i];
        AD<double> yaw0 = vars[yaw_start + i];
        AD<double> v0 = vars[v_start + i];
        AD<double> omega0 = vars[omega_start + i];
        AD<double> omega_r0 = vars[omega_r_start + i];
        AD<double> omega_l0 = vars[omega_l_start + i];
        //入力ホライゾンはt+1を考慮しない
        AD<double> domega_r0 = vars[domega_r_start + i];
        AD<double> domega_l0 = vars[domega_l_start + i];

        //制約
        fg[2 + omega_r_start + i] = omega_r1 - (omega_r0 + domega_r0 * DT);
        fg[2 + omega_l_start + i] = omega_l1 - (omega_l0 + domega_l0 * DT);
        fg[2 + v_start + i] = v1 - (WHEEL_RADIUS / 2.0) * (omega_r1 + omega_l1);
        fg[2 + omega_start + i] = omega1 - (WHEEL_RADIUS / TREAD) * (omega_r1 - omega_l1);
        fg[2 + x_start + i] = x1 - (x0 + v0 * CppAD::cos(yaw0) * DT);
        fg[2 + y_start + i] = y1 - (y0 + v0 * CppAD::sin(yaw0) * DT);
        fg[2 + yaw_start + i] = yaw1 - (yaw0 + omega0 * DT);
    }
    std::cout << "FG_eval() end" << std::endl;
}

MPCPathTracker::MPCPathTracker(void)
{
    velocity_pub = nh.advertise<geometry_msgs::Twist>(VELOCITY_TOPIC_NAME, 1);
    path_pub = nh.advertise<geometry_msgs::PoseArray>("/mpc_path", 1);
    path_sub = nh.subscribe(INTERMEDIATE_PATH_TOPIC_NAME, 1, &MPCPathTracker::path_callback, this);
    path_x = Eigen::VectorXd::Zero(T);
    path_y = Eigen::VectorXd::Zero(T);
    path_yaw = Eigen::VectorXd::Zero(T);
    std::cout << "=== mpc_path_tracker ===" << std::endl;
}

void MPCPathTracker::path_callback(const nav_msgs::PathConstPtr& msg)
{
    std::cout << "path callback" << std::endl;
    path = *msg;
}

void MPCPathTracker::process(void)
{
    bool transformed = false;
    geometry_msgs::PoseStamped pose;
    try{
        listener.lookupTransform(WORLD_FRAME, ROBOT_FRAME, ros::Time(0), _transform);
        tf::transformStampedTFToMsg(_transform, transform);
        current_pose.header = transform.header;
        current_pose.pose.position.x = transform.transform.translation.x;
        current_pose.pose.position.y = transform.transform.translation.y;
        current_pose.pose.orientation = transform.transform.rotation;
        pose.header = current_pose.header;
        pose.pose.position.x = 0;
        pose.pose.position.y = 0;
        pose.pose.orientation = transform.transform.rotation;
        transformed = true;
    }catch(tf::TransformException &ex){
        std::cout << ex.what() << std::endl;
    }

    if(!path.poses.empty() && transformed){
        std::cout << "=== diff drive mpc ===" << std::endl;
        ros::Time start_time = ros::Time::now();
        if(first_transform){
            last_time = ros::Time::now().toSec();
            first_transform = false;
        }else{
            //std::cout << current_pose << std::endl;
            double current_time = ros::Time::now().toSec();
            double dt = current_time - last_time;
            last_time = current_time;
            double dx = current_pose.pose.position.x - previous_pose.pose.position.x;
            double dy = current_pose.pose.position.y - previous_pose.pose.position.y;
            double dyaw = tf::getYaw(current_pose.pose.orientation) - tf::getYaw(previous_pose.pose.orientation);
            double v = sqrt(dx * dx + dy * dy) / dt;
            double omega = dyaw / dt;
            double omega_r = (v + omega * TREAD / 2.0) / WHEEL_RADIUS;
            double omega_l = (v - omega * TREAD / 2.0) / WHEEL_RADIUS;

            Eigen::VectorXd state(7);
            state << pose.pose.position.x, pose.pose.position.y, tf::getYaw(pose.pose.orientation), v, omega, omega_r, omega_l;
            std::cout << "path to vector" << std::endl;
            path_to_vector();
            std::cout << "solving" << std::endl;
            auto result = mpc.solve(state, path_x, path_y, path_yaw);
            std::cout << "solved" << std::endl;
            geometry_msgs::Twist velocity;
            velocity.linear.x = result[0];
            velocity.angular.z = result[1];
            std::cout << velocity << std::endl;
            velocity_pub.publish(velocity);
            // mpc表示
            geometry_msgs::PoseArray mpc_path;
            mpc_path.header.frame_id = ROBOT_FRAME;
            double yaw0 = tf::getYaw(pose.pose.orientation);
            for(int i=0;i<T-1;i++){
                geometry_msgs::Pose temp;
                temp.position.x = result[2+3*i] * cos(-yaw0) - result[3+3*i] * sin(-yaw0);
                temp.position.y = result[2+3*i] * sin(-yaw0) + result[3+3*i] * cos(-yaw0);
                temp.orientation = tf::createQuaternionMsgFromYaw(result[4+3*i] - yaw0);
                mpc_path.poses.push_back(temp);
            }
            path_pub.publish(mpc_path);
            // ~mpc表示
            path.poses.erase(path.poses.begin());
        }
        std::cout << ros::Time::now() - start_time << "[s]" << std::endl;
    }
    previous_pose = current_pose;
}

void MPCPathTracker::path_to_vector(void)
{
    int m = VREF * DT / RESOLUTION + 1;// TODO:delete 1
    int index = 0;
    for(int i=0;i<T;i++){
        if(i*m<path.poses.size()){
            index = i*m;
            path_x[i] = path.poses[index].pose.position.x;
            path_y[i] = path.poses[index].pose.position.y;
            path_yaw[i] = tf::getYaw(path.poses[index].pose.orientation);
        }else{
            path_x[i] = path.poses[path.poses.size() - 1].pose.position.x;
            path_y[i] = path.poses[path.poses.size() - 1].pose.position.y;
            path_yaw[i] = tf::getYaw(path.poses[path.poses.size() - 1].pose.orientation);
        }
    }
}

double min_distance(nav_msgs::Path& path, geometry_msgs::PoseStamped& pose)
{
    int length = path.poses.size();
    double min_distance = 100;
    for(int i=0;i<length;i++){
        double distance = get_distance(path.poses[i], pose);
        if(min_distance > distance){
            min_distance = distance;
        }
    }
    return min_distance;
}

double get_distance(geometry_msgs::PoseStamped& pose0, geometry_msgs::PoseStamped& pose1)
{
    return sqrt((pose0.pose.position.x - pose1.pose.position.x) * (pose0.pose.position.x - pose1.pose.position.x) + (pose0.pose.position.y - pose1.pose.position.y) * (pose0.pose.position.y - pose1.pose.position.y));
}