/**
 * \file
 * \author Thomas Fischer
 * \date   2010-03-17
 * \brief  Implementation of analytical geometry functions.
 *
 * \copyright
 * Copyright (c) 2012-2020, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "AnalyticalGeometry.h"

#include <algorithm>
#include <cmath>
#include <limits>

#include <Eigen/Dense>

#include "BaseLib/StringTools.h"

#include "Polyline.h"
#include "PointVec.h"

#include "MathLib/GeometricBasics.h"

extern double orient2d(double *, double *, double *);
extern double orient2dfast(double*, double*, double*);

namespace ExactPredicates
{
double getOrientation2d(MathLib::Point3d const& a,
    MathLib::Point3d const& b, MathLib::Point3d const& c)
{
    return orient2d(const_cast<double*>(a.getCoords()),
        const_cast<double*>(b.getCoords()),
        const_cast<double*>(c.getCoords()));
}

double getOrientation2dFast(MathLib::Point3d const& a,
                            MathLib::Point3d const& b,
                            MathLib::Point3d const& c)
{
    return orient2dfast(const_cast<double*>(a.getCoords()),
                        const_cast<double*>(b.getCoords()),
                        const_cast<double*>(c.getCoords()));
}
}  // namespace ExactPredicates

namespace GeoLib
{
Orientation getOrientation(MathLib::Point3d const& p0,
                           MathLib::Point3d const& p1,
                           MathLib::Point3d const& p2)
{
    double const orientation = ExactPredicates::getOrientation2d(p0, p1, p2);
    if (orientation > 0)
    {
        return CCW;
    }
    if (orientation < 0)
    {
        return CW;
    }
    return COLLINEAR;
}

Orientation getOrientationFast(MathLib::Point3d const& p0,
                               MathLib::Point3d const& p1,
                               MathLib::Point3d const& p2)
{
    double const orientation =
        ExactPredicates::getOrientation2dFast(p0, p1, p2);
    if (orientation > 0)
    {
        return CCW;
    }
    if (orientation < 0)
    {
        return CW;
    }
    return COLLINEAR;
}

bool parallel(MathLib::Vector3 v, MathLib::Vector3 w)
{
    const double eps(std::numeric_limits<double>::epsilon());

    // check degenerated cases
    if (v.getLength() < eps)
    {
        return false;
    }

    if (w.getLength() < eps)
    {
        return false;
    }

    v.normalize();
    w.normalize();

    bool parallel(true);
    if (std::abs(v[0] - w[0]) > eps)
    {
        parallel = false;
    }
    if (std::abs(v[1] - w[1]) > eps)
    {
        parallel = false;
    }
    if (std::abs(v[2] - w[2]) > eps)
    {
        parallel = false;
    }

    if (! parallel) {
        parallel = true;
        // change sense of direction of v_normalised
        v *= -1.0;
        // check again
        if (std::abs(v[0] - w[0]) > eps)
        {
            parallel = false;
        }
        if (std::abs(v[1] - w[1]) > eps)
        {
            parallel = false;
        }
        if (std::abs(v[2] - w[2]) > eps)
        {
            parallel = false;
        }
    }

    return parallel;
}

bool lineSegmentIntersect(GeoLib::LineSegment const& s0,
                          GeoLib::LineSegment const& s1,
                          GeoLib::Point& s)
{
    GeoLib::Point const& a{s0.getBeginPoint()};
    GeoLib::Point const& b{s0.getEndPoint()};
    GeoLib::Point const& c{s1.getBeginPoint()};
    GeoLib::Point const& d{s1.getEndPoint()};

    if (!isCoplanar(a, b, c, d))
    {
        return false;
    }

    // handle special cases here to avoid computing intersection numerical
    if (MathLib::sqrDist(a, c) < std::numeric_limits<double>::epsilon() ||
        MathLib::sqrDist(a, d) < std::numeric_limits<double>::epsilon()) {
        s = a;
        return true;
    }
    if (MathLib::sqrDist(b, c) < std::numeric_limits<double>::epsilon() ||
        MathLib::sqrDist(b, d) < std::numeric_limits<double>::epsilon()) {
        s = b;
        return true;
    }

    MathLib::Vector3 const v(a, b);
    MathLib::Vector3 const w(c, d);
    MathLib::Vector3 const qp(a, c);
    MathLib::Vector3 const pq(c, a);

    auto isLineSegmentIntersectingAB = [&v](MathLib::Vector3 const& ap,
                                            std::size_t i)
    {
        // check if p is located at v=(a,b): (ap = t*v, t in [0,1])
        return 0.0 <= ap[i] / v[i] && ap[i] / v[i] <= 1.0;
    };

    if (parallel(v,w)) { // original line segments (a,b) and (c,d) are parallel
        if (parallel(pq,v)) { // line segment (a,b) and (a,c) are also parallel
            // Here it is already checked that the line segments (a,b) and (c,d)
            // are parallel. At this point it is also known that the line
            // segment (a,c) is also parallel to (a,b). In that case it is
            // possible to express c as c(t) = a + t * (b-a) (analog for the
            // point d). Since the evaluation of all three coordinate equations
            // (x,y,z) have to lead to the same solution for the parameter t it
            // is sufficient to evaluate t only once.

            // Search id of coordinate with largest absolute value which is will
            // be used in the subsequent computations. This prevents division by
            // zero in case the line segments are parallel to one of the
            // coordinate axis.
            std::size_t i_max(std::abs(v[0]) <= std::abs(v[1]) ? 1 : 0);
            i_max = std::abs(v[i_max]) <= std::abs(v[2]) ? 2 : i_max;
            if (isLineSegmentIntersectingAB(qp, i_max)) {
                s = c;
                return true;
            }
            MathLib::Vector3 const ad(a, d);
            if (isLineSegmentIntersectingAB(ad, i_max)) {
                s = d;
                return true;
            }
            return false;
        }
        return false;
    }

    // general case
    const double sqr_len_v(v.getSqrLength());
    const double sqr_len_w(w.getSqrLength());

    Eigen::Matrix2d mat;
    mat(0,0) = sqr_len_v;
    mat(0,1) = -1.0 * MathLib::scalarProduct(v,w);
    mat(1,1) = sqr_len_w;
    mat(1,0) = mat(0,1);

    Eigen::Vector2d rhs;
    rhs << MathLib::scalarProduct(v, qp), MathLib::scalarProduct(w, pq);

    rhs = mat.partialPivLu().solve(rhs);

    // no theory for the following tolerances, determined by testing
    // lower tolerance: little bit smaller than zero
    const double l(-1.0*std::numeric_limits<float>::epsilon());
    // upper tolerance a little bit greater than one
    const double u(1.0+std::numeric_limits<float>::epsilon());
    if (rhs[0] < l || u < rhs[0] || rhs[1] < l || u < rhs[1]) {
        return false;
    }

    // compute points along line segments with minimal distance
    GeoLib::Point const p0(a[0]+rhs[0]*v[0], a[1]+rhs[0]*v[1], a[2]+rhs[0]*v[2]);
    GeoLib::Point const p1(c[0]+rhs[1]*w[0], c[1]+rhs[1]*w[1], c[2]+rhs[1]*w[2]);

    double const min_dist(sqrt(MathLib::sqrDist(p0, p1)));
    double const min_seg_len(std::min(sqrt(sqr_len_v), sqrt(sqr_len_w)));
    if (min_dist < min_seg_len * 1e-6) {
        s[0] = 0.5 * (p0[0] + p1[0]);
        s[1] = 0.5 * (p0[1] + p1[1]);
        s[2] = 0.5 * (p0[2] + p1[2]);
        return true;
    }

    return false;
}

bool lineSegmentsIntersect(const GeoLib::Polyline* ply,
                           GeoLib::Polyline::SegmentIterator &seg_it0,
                           GeoLib::Polyline::SegmentIterator &seg_it1,
                           GeoLib::Point& intersection_pnt)
{
    std::size_t const n_segs(ply->getNumberOfSegments());
    // Neighbouring segments always intersects at a common vertex. The algorithm
    // checks for intersections of non-neighbouring segments.
    for (seg_it0 = ply->begin(); seg_it0 != ply->end() - 2; ++seg_it0)
    {
        seg_it1 = seg_it0+2;
        std::size_t const seg_num_0 = seg_it0.getSegmentNumber();
        for ( ; seg_it1 != ply->end(); ++seg_it1) {
            // Do not check first and last segment, because they are
            // neighboured.
            if (!(seg_num_0 == 0 && seg_it1.getSegmentNumber() == n_segs - 1)) {
                if (lineSegmentIntersect(*seg_it0, *seg_it1, intersection_pnt)) {
                    return true;
                }
            }
        }
    }
    return false;
}

void rotatePoints(MathLib::DenseMatrix<double> const& rot_mat, std::vector<GeoLib::Point*> &pnts)
{
    rotatePoints(rot_mat, pnts.begin(), pnts.end());
}

MathLib::DenseMatrix<double> rotatePointsToXY(std::vector<GeoLib::Point*>& pnts)
{
    return rotatePointsToXY(pnts.begin(), pnts.end(), pnts.begin(), pnts.end());
}

std::unique_ptr<GeoLib::Point> triangleLineIntersection(
    MathLib::Point3d const& a, MathLib::Point3d const& b,
    MathLib::Point3d const& c, MathLib::Point3d const& p,
    MathLib::Point3d const& q)
{
    const MathLib::Vector3 pq(p, q);
    const MathLib::Vector3 pa(p, a);
    const MathLib::Vector3 pb(p, b);
    const MathLib::Vector3 pc(p, c);

    double u (MathLib::scalarTriple(pq, pc, pb));
    if (u < 0)
    {
        return nullptr;
    }
    double v (MathLib::scalarTriple(pq, pa, pc));
    if (v < 0)
    {
        return nullptr;
    }
    double w (MathLib::scalarTriple(pq, pb, pa));
    if (w < 0)
    {
        return nullptr;
    }

    const double denom (1.0/(u+v+w));
    u*=denom;
    v*=denom;
    w*=denom;
    return std::make_unique<GeoLib::Point>(u * a[0] + v * b[0] + w * c[0],
                                           u * a[1] + v * b[1] + w * c[1],
                                           u * a[2] + v * b[2] + w * c[2]);
}

void computeAndInsertAllIntersectionPoints(GeoLib::PointVec &pnt_vec,
    std::vector<GeoLib::Polyline*> & plys)
{
    auto computeSegmentIntersections = [&pnt_vec](GeoLib::Polyline& poly0,
                                                  GeoLib::Polyline& poly1)
    {
        for (auto seg0_it(poly0.begin()); seg0_it != poly0.end(); ++seg0_it)
        {
            for (auto seg1_it(poly1.begin()); seg1_it != poly1.end(); ++seg1_it)
            {
                GeoLib::Point s(0.0, 0.0, 0.0, pnt_vec.size());
                if (lineSegmentIntersect(*seg0_it, *seg1_it, s))
                {
                    std::size_t const id(
                        pnt_vec.push_back(new GeoLib::Point(s)));
                    poly0.insertPoint(seg0_it.getSegmentNumber() + 1, id);
                    poly1.insertPoint(seg1_it.getSegmentNumber() + 1, id);
                }
            }
        }
    };

    for (auto it0(plys.begin()); it0 != plys.end(); ++it0) {
        auto it1(it0);
        ++it1;
        for (; it1 != plys.end(); ++it1) {
            computeSegmentIntersections(*(*it0), *(*it1));
        }
    }
}

GeoLib::Polygon rotatePolygonToXY(GeoLib::Polygon const& polygon_in,
    MathLib::Vector3 & plane_normal)
{
    // 1 copy all points
    auto* polygon_pnts(new std::vector<GeoLib::Point*>);
    for (std::size_t k(0); k < polygon_in.getNumberOfPoints(); k++)
    {
        polygon_pnts->push_back(new GeoLib::Point(*(polygon_in.getPoint(k))));
    }

    // 2 rotate points
    double d_polygon (0.0);
    GeoLib::getNewellPlane (*polygon_pnts, plane_normal, d_polygon);
    MathLib::DenseMatrix<double> rot_mat(3,3);
    GeoLib::computeRotationMatrixToXY(plane_normal, rot_mat);
    GeoLib::rotatePoints(rot_mat, *polygon_pnts);

    // 3 set z coord to zero
    std::for_each(polygon_pnts->begin(), polygon_pnts->end(),
        [] (GeoLib::Point* p) { (*p)[2] = 0.0; }
    );

    // 4 create new polygon
    GeoLib::Polyline rot_polyline(*polygon_pnts);
    for (std::size_t k(0); k < polygon_in.getNumberOfPoints(); k++)
    {
        rot_polyline.addPoint(k);
    }
    rot_polyline.addPoint(0);
    return GeoLib::Polygon(rot_polyline);
}

std::vector<MathLib::Point3d> lineSegmentIntersect2d(
    GeoLib::LineSegment const& ab, GeoLib::LineSegment const& cd)
{
    GeoLib::Point const& a{ab.getBeginPoint()};
    GeoLib::Point const& b{ab.getEndPoint()};
    GeoLib::Point const& c{cd.getBeginPoint()};
    GeoLib::Point const& d{cd.getEndPoint()};

    double const orient_abc(getOrientation(a, b, c));
    double const orient_abd(getOrientation(a, b, d));

    // check if the segment (cd) lies on the left or on the right of (ab)
    if ((orient_abc > 0 && orient_abd > 0) || (orient_abc < 0 && orient_abd < 0)) {
        return std::vector<MathLib::Point3d>();
    }

    // check: (cd) and (ab) are on the same line
    if (orient_abc == 0.0 && orient_abd == 0.0) {
        double const eps(std::numeric_limits<double>::epsilon());
        if (MathLib::sqrDist2d(a, c) < eps && MathLib::sqrDist2d(b, d) < eps)
        {
            return {{a, b}};
        }
        if (MathLib::sqrDist2d(a, d) < eps && MathLib::sqrDist2d(b, c) < eps)
        {
            return {{a, b}};
        }

        // Since orient_ab and orient_abd vanish, a, b, c, d are on the same
        // line and for this reason it is enough to check the x-component.
        auto isPointOnSegment = [](double q, double p0, double p1)
        {
            double const t((q - p0) / (p1 - p0));
            return 0 <= t && t <= 1;
        };

        // check if c in (ab)
        if (isPointOnSegment(c[0], a[0], b[0])) {
            // check if a in (cd)
            if (isPointOnSegment(a[0], c[0], d[0])) {
                return {{a, c}};
            }
            // check b == c
            if (MathLib::sqrDist2d(b,c) < eps) {
                return {{b}};
            }
            // check if b in (cd)
            if (isPointOnSegment(b[0], c[0], d[0])) {
                return {{b, c}};
            }
            // check d in (ab)
            if (isPointOnSegment(d[0], a[0], b[0])) {
                return {{c, d}};
            }
            std::stringstream err;
            err.precision(std::numeric_limits<double>::digits10);
            err << ab << " x " << cd;
            OGS_FATAL(
                "The case of parallel line segments ({:s}) is not handled yet. "
                "Aborting.",
                err.str());
        }

        // check if d in (ab)
        if (isPointOnSegment(d[0], a[0], b[0])) {
            // check if a in (cd)
            if (isPointOnSegment(a[0], c[0], d[0])) {
                return {{a, d}};
            }
            // check if b==d
            if (MathLib::sqrDist2d(b, d) < eps) {
                return {{b}};
            }
            // check if b in (cd)
            if (isPointOnSegment(b[0], c[0], d[0])) {
                return {{b, d}};
            }
            // d in (ab), b not in (cd): check c in (ab)
            if (isPointOnSegment(c[0], a[0], b[0])) {
                return {{c, d}};
            }

            std::stringstream err;
            err.precision(std::numeric_limits<double>::digits10);
            err << ab << " x " << cd;
            OGS_FATAL(
                "The case of parallel line segments ({:s}) "
                "is not handled yet. Aborting.",
                err.str());
        }
        return std::vector<MathLib::Point3d>();
    }

    // precondition: points a, b, c are collinear
    // the function checks if the point c is onto the line segment (a,b)
    auto isCollinearPointOntoLineSegment = [](MathLib::Point3d const& a,
                                              MathLib::Point3d const& b,
                                              MathLib::Point3d const& c) {
        if (b[0] - a[0] != 0)
        {
            double const t = (c[0] - a[0]) / (b[0] - a[0]);
            return 0.0 <= t && t <= 1.0;
        }
        if (b[1] - a[1] != 0)
        {
            double const t = (c[1] - a[1]) / (b[1] - a[1]);
            return 0.0 <= t && t <= 1.0;
        }
        if (b[2] - a[2] != 0)
        {
            double const t = (c[2] - a[2]) / (b[2] - a[2]);
            return 0.0 <= t && t <= 1.0;
        }
        return false;
    };

    if (orient_abc == 0.0) {
        if (isCollinearPointOntoLineSegment(a, b, c))
        {
            return {{c}};
        }
        return std::vector<MathLib::Point3d>();
    }

    if (orient_abd == 0.0) {
        if (isCollinearPointOntoLineSegment(a, b, d))
        {
            return {{d}};
        }
        return std::vector<MathLib::Point3d>();
    }

    // check if the segment (ab) lies on the left or on the right of (cd)
    double const orient_cda(getOrientation(c, d, a));
    double const orient_cdb(getOrientation(c, d, b));
    if ((orient_cda > 0 && orient_cdb > 0) || (orient_cda < 0 && orient_cdb < 0)) {
        return std::vector<MathLib::Point3d>();
    }

    // at this point it is sure that there is an intersection and the system of
    // linear equations will be invertible
    // solve the two linear equations (b-a, c-d) (t, s)^T = (c-a) simultaneously
    Eigen::Matrix2d mat;
    mat(0,0) = b[0]-a[0];
    mat(0,1) = c[0]-d[0];
    mat(1,0) = b[1]-a[1];
    mat(1,1) = c[1]-d[1];
    Eigen::Vector2d rhs{c[0] - a[0], c[1] - a[1]};

    rhs = mat.partialPivLu().solve(rhs);
    if (0 <= rhs[1] && rhs[1] <= 1.0) {
        return { MathLib::Point3d{std::array<double,3>{{
                c[0]+rhs[1]*(d[0]-c[0]), c[1]+rhs[1]*(d[1]-c[1]),
                c[2]+rhs[1]*(d[2]-c[2])}} } };
    }
    return std::vector<MathLib::Point3d>();  // parameter s not in the valid
                                             // range
}

void sortSegments(
    MathLib::Point3d const& seg_beg_pnt,
    std::vector<GeoLib::LineSegment>& sub_segments)
{
    double const eps(std::numeric_limits<double>::epsilon());

    auto findNextSegment = [&eps](
                               MathLib::Point3d const& seg_beg_pnt,
                               std::vector<GeoLib::LineSegment>& sub_segments,
                               std::vector<GeoLib::LineSegment>::iterator&
                                   sub_seg_it) {
        if (sub_seg_it == sub_segments.end())
        {
            return;
        }
        // find appropriate segment for the given segment begin point
        auto act_beg_seg_it = std::find_if(
            sub_seg_it, sub_segments.end(),
            [&seg_beg_pnt, &eps](GeoLib::LineSegment const& seg)
            {
                return MathLib::sqrDist(seg_beg_pnt, seg.getBeginPoint()) < eps ||
                       MathLib::sqrDist(seg_beg_pnt, seg.getEndPoint()) < eps;
            });
        if (act_beg_seg_it == sub_segments.end())
        {
            return;
        }
        // if necessary correct orientation of segment, i.e. swap beg and end
        if (MathLib::sqrDist(seg_beg_pnt, act_beg_seg_it->getEndPoint()) <
            MathLib::sqrDist(seg_beg_pnt, act_beg_seg_it->getBeginPoint()))
        {
            std::swap(act_beg_seg_it->getBeginPoint(),
                      act_beg_seg_it->getEndPoint());
        }
        assert(sub_seg_it != sub_segments.end());
        // exchange segments within the container
        if (sub_seg_it != act_beg_seg_it)
        {
            std::swap(*sub_seg_it, *act_beg_seg_it);
        }
    };

    // find start segment
    auto seg_it = sub_segments.begin();
    findNextSegment(seg_beg_pnt, sub_segments, seg_it);

    while (seg_it != sub_segments.end())
    {
        MathLib::Point3d & new_seg_beg_pnt(seg_it->getEndPoint());
        seg_it++;
        if (seg_it != sub_segments.end())
        {
            findNextSegment(new_seg_beg_pnt, sub_segments, seg_it);
        }
    }
}

} // end namespace GeoLib


#include <state_estimation/filters/ukf_vs.h>
#include <state_estimation/utilities/data_subset_utilities.h>
#include <state_estimation/utilities/logging.h>
#include <Eigen/Dense>

namespace state_estimation {

UKFVS::UKFVS(system_models::NonlinearSystemModel* system_model)
    : FilterBase::FilterBase(system_model) {
    initializeSigmaPointParameters();
}

UKFVS::UKFVS(system_models::NonlinearSystemModel* system_model, const Eigen::VectorXd& x,
             const Eigen::MatrixXd& cov, double timestamp)
    : FilterBase::FilterBase(system_model, x, cov, timestamp) {
    initializeSigmaPointParameters();
}

void UKFVS::setSigmaPointParameters(double alpha, double kappa, double beta) {
    uint32_t n = system_model_->activeStateSize();
    num_sigma_pts_ = 2 * n + 1;

    // Compute our lambda value
    lambda_ = pow(alpha, 2) * (n + kappa) - n;

    // Compute our weight vectors
    w_mean_.resize(num_sigma_pts_);
    w_cov_.resize(num_sigma_pts_);

    const double init_w = 0.5 / (n + lambda_);
    w_mean_ = Eigen::VectorXd::Constant(num_sigma_pts_, init_w);
    w_cov_ = Eigen::VectorXd::Constant(num_sigma_pts_, init_w);

    w_mean_(0) = lambda_ / (n + lambda_);
    w_cov_(0) = w_mean_(0) + 1 - pow(alpha, 2) + beta;

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "UKF sigma point initialization" << std::endl
              << "alpha=" << alpha << std::endl
              << "kappa=" << kappa << std::endl
              << "lambda=" << lambda_ << std::endl
              << "Initialized mean weights to [" << w_mean_.transpose() << "]" << std::endl
              << "Initialized covariance weights to [" << w_cov_.transpose() << "]" << std::endl;
#endif
}

void UKFVS::initializeSigmaPointParameters() {
    setSigmaPointParameters(0.001, 0, 2);
}

void UKFVS::myPredict(const Eigen::VectorXd& u, double dt) {
    // We will only be updating a subset of the state. All the computationally expensive linear
    // algebra operations will happen on the subset of the state for efficiency. Since the models
    // still operate on the full state vector the sigma points vectors will be of the full
    // dimensionality, but the number of sigma points will still be determined by the number of
    // active states. This only adds a slight additional memory cost.

    // Generate the sigma offsets only using the subset of the covariance matrix we are inerested
    // for efficiency
    const Eigen::MatrixXd cov_subset = getSubset(
        filter_state_.covariance, system_model_->activeStates(), system_model_->activeStates());
    const Eigen::MatrixXd sigma_offset_subset =
        ((system_model_->activeStateSize() + lambda_) * cov_subset).llt().matrixL();

    Eigen::MatrixXd sigma_pts(system_model_->stateSize(), num_sigma_pts_);

    // Run all the sigma points through the model
    system_model_->update(filter_state_.x, u, dt);
    sigma_pts.col(0) = system_model_->g();

    Eigen::VectorXd offset = Eigen::VectorXd::Zero(system_model_->stateSize());
    for (uint32_t i = 0; i < system_model_->activeStateSize(); ++i) {
        const uint32_t i_high = i + 1;
        const uint32_t i_low = i + 1 + system_model_->activeStateSize();
        convertSubsetToFull(sigma_offset_subset.col(i), &offset, system_model_->activeStates());

        const Eigen::VectorXd x_high = system_model_->addVectors(filter_state_.x, offset);
        system_model_->update(x_high, u, dt);
        sigma_pts.col(i_high) = system_model_->g();

        const Eigen::VectorXd x_low = system_model_->subtractVectors(filter_state_.x, offset);
        system_model_->update(x_low, u, dt);
        sigma_pts.col(i_low) = system_model_->g();
    }

    // Compute the weighted mean
    filter_state_.x = system_model_->weightedSum(w_mean_, sigma_pts);

    // Initialze the covariance with the process and control noise
    const Eigen::MatrixXd Rc_subset =
        getSubset(system_model_->Rc(), system_model_->activeControls());
    const Eigen::MatrixXd P_subset =
        getSubset(system_model_->P(), system_model_->activeStates(), {});
    const Eigen::MatrixXd V_subset = getSubset(system_model_->V(), system_model_->activeStates(),
                                               system_model_->activeControls());

    Eigen::MatrixXd cov_prime_subset = P_subset * system_model_->Rp() * P_subset.transpose() +
                                       V_subset * Rc_subset * V_subset.transpose();

    // Add the weighted sample covariance
    for (uint32_t i = 0; i < num_sigma_pts_; ++i) {
        const Eigen::VectorXd dx_full =
            system_model_->subtractVectors(sigma_pts.col(i), filter_state_.x);
        const Eigen::VectorXd dx_subset = getSubset(dx_full, system_model_->activeStates());

        cov_prime_subset += w_cov_(i) * dx_subset * dx_subset.transpose();
    }

    // Update the full covariance matrix with the subset we calculated
    convertSubsetToFull(cov_prime_subset, &filter_state_.covariance, system_model_->activeStates());

#ifdef DEBUG_STATE_ESTIMATION
    Eigen::MatrixXd sigma_offset =
        Eigen::MatrixXd::Zero(system_model_->stateSize(), system_model_->stateSize());
    convertSubsetToFull(sigma_offset_subset, &sigma_offset, system_model_->activeStates());

    std::cout << "UKF predicition update:" << std::endl
              << "Sigma offsets=" << std::endl
              << printMatrix(sigma_offset) << std::endl
              << "Sigma points=" << std::endl
              << printMatrix(sigma_pts) << std::endl
              << "P=" << std::endl
              << printMatrix(system_model_->P()) << std::endl
              << "V=" << std::endl
              << printMatrix(system_model_->V()) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

void UKFVS::myCorrect(const Eigen::VectorXd& z,
                      measurement_models::NonlinearMeasurementModel* model, double dt) {
    // We will only be updating a subset of the state. All the computationally expensive linear
    // algebra operations will happen on the subset of the state for efficiency. Since the models
    // still operate on the full state vector the sigma points vectors will be of the full
    // dimensionality, but the number of sigma points will still be determined by the number of
    // active states. This only adds a slight additional memory cost.

    // Generate the sigma offsets only using the subset of the covariance matrix we are inerested
    // for efficiency
    const Eigen::MatrixXd cov_subset =
        getSubset(filter_state_.covariance, system_model_->activeStates());
    const Eigen::MatrixXd sigma_offset_subset =
        ((system_model_->activeStateSize() + lambda_) * cov_subset).llt().matrixL();

    Eigen::MatrixXd state_sigma_pts(system_model_->stateSize(), num_sigma_pts_);
    Eigen::MatrixXd meas_sigma_pts(model->measurementSize(), num_sigma_pts_);

    // Run all the sigma points through the model
    state_sigma_pts.col(0) = filter_state_.x;
    model->update(filter_state_.x, dt);
    meas_sigma_pts.col(0) = model->h();

    Eigen::VectorXd offset = Eigen::VectorXd::Zero(system_model_->stateSize());
    for (uint32_t i = 0; i < system_model_->activeStateSize(); ++i) {
        const uint32_t i_high = i + 1;
        const uint32_t i_low = i + 1 + system_model_->activeStateSize();
        convertSubsetToFull(sigma_offset_subset.col(i), &offset, system_model_->activeStates());

        state_sigma_pts.col(i_high) = system_model_->addVectors(filter_state_.x, offset);
        model->update(state_sigma_pts.col(i_high), dt);
        meas_sigma_pts.col(i_high) = model->h();

        state_sigma_pts.col(i_low) = system_model_->subtractVectors(filter_state_.x, offset);
        model->update(state_sigma_pts.col(i_low), dt);
        meas_sigma_pts.col(i_low) = model->h();
    }

    // Compute the weighted mean for the predicted measurement
    Eigen::VectorXd z_pred = model->weightedSum(w_mean_, meas_sigma_pts);

    // Compute the gain (only using the subset of the states)
    Eigen::MatrixXd S = getSubset(model->covariance(), model->activeMeasurements());
    for (uint32_t i = 0; i < num_sigma_pts_; ++i) {
        const Eigen::VectorXd dz_full = model->subtractVectors(meas_sigma_pts.col(i), z_pred);
        const Eigen::VectorXd dz_subset = getSubset(dz_full, model->activeMeasurements());
        S += w_cov_(i) * dz_subset * dz_subset.transpose();
    }

    Eigen::MatrixXd cross_covariance =
        Eigen::MatrixXd::Zero(system_model_->activeStateSize(), model->activeMeasurementSize());
    for (uint32_t i = 0; i < num_sigma_pts_; ++i) {
        const Eigen::VectorXd dx_full =
            system_model_->subtractVectors(state_sigma_pts.col(i), state_sigma_pts.col(0));
        const Eigen::VectorXd dx_subset = getSubset(dx_full, system_model_->activeStates());

        const Eigen::VectorXd dz_full = model->subtractVectors(meas_sigma_pts.col(i), z_pred);
        const Eigen::VectorXd dz_subset = getSubset(dz_full, model->activeMeasurements());

        cross_covariance += w_cov_(i) * dx_subset * dz_subset.transpose();
    }

    const Eigen::MatrixXd K = cross_covariance * S.inverse();

    // Perform the mean update
    const Eigen::VectorXd dz_full = model->subtractVectors(z, z_pred);
    const Eigen::VectorXd dz_subset = getSubset(dz_full, model->activeMeasurements());
    const Eigen::VectorXd dx_subset = K * dz_subset;
    const Eigen::VectorXd dx_full = convertSubsetToFullZeroed(
        dx_subset, system_model_->activeStates(), system_model_->stateSize());
    filter_state_.x = system_model_->addVectors(filter_state_.x, dx_full);

    // Perform the covariance update
    const Eigen::MatrixXd cov_prime_subset = cov_subset - K * S * K.transpose();
    convertSubsetToFull(cov_prime_subset, &filter_state_.covariance, system_model_->activeStates());

#ifdef DEBUG_STATE_ESTIMATION
    const Eigen::MatrixXd sigma_offset = convertSubsetToFullZeroed(
        sigma_offset_subset, system_model_->activeStates(), system_model_->stateSize());

    const Eigen::MatrixXd S_full =
        convertSubsetToFullZeroed(S, model->activeMeasurements(), model->measurementSize());

    const Eigen::MatrixXd cross_full = convertSubsetToFullZeroed(
        cross_covariance, system_model_->activeStates(), model->activeMeasurements(),
        system_model_->stateSize(), model->measurementSize());

    const Eigen::MatrixXd K_full =
        convertSubsetToFullZeroed(K, system_model_->activeStates(), model->activeMeasurements(),
                                  system_model_->stateSize(), model->measurementSize());

    std::cout << "UKF measurement update:" << std::endl
              << "Sigma offsets=" << std::endl
              << printMatrix(sigma_offset) << std::endl
              << "State sigma points=" << std::endl
              << printMatrix(state_sigma_pts) << std::endl
              << "Measurement sigma points=" << std::endl
              << printMatrix(meas_sigma_pts) << std::endl
              << "z_pred=" << printMatrix(z_pred) << std::endl
              << "Q=" << std::endl
              << printMatrix(model->covariance()) << std::endl
              << "S=" << std::endl
              << printMatrix(S_full) << std::endl
              << "Cross Covariance=" << std::endl
              << printMatrix(cross_full) << std::endl
              << "K=" << std::endl
              << printMatrix(K_full) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

}  // namespace state_estimation


#include <ros/ros.h>
#include <std_msgs/String.h>
#include <model_based_shared_control/State.h>
#include <model_based_shared_control/Control.h>
#include "robotlib/dynamicalSystems/koopman/koopman_operator.hpp"
#include "robotlib/dynamicalSystems/koopman/basis_functions/linear_basis.hpp"
#include "robotlib/dynamicalSystems/koopman/basis_functions/nonlinear_basis.hpp"
#include <armadillo>

class HumanRobot {

public:

  // publishers and subscribers
  ros::Subscriber state_sub;
  ros::Subscriber shutdown_sub;
  bool has_initialized = false;

  // messages
  model_based_shared_control::State state;
  model_based_shared_control::Control control;

  // data vectors
  arma::vec current_state;
  arma::vec dataIn;
  arma::vec dataOut;
  arma::vec cdataIn;
  arma::vec hdataIn;
  arma::vec hdataOut;

  // Koopman models
  KoopmanOperator* linear_koopman_operator;
  KoopmanOperator* nonlinear_koopman_operator;

  HumanRobot(ros::Rate* loop_rate) {

    ros::NodeHandle nh;

    // set up subscribers
    state_sub = nh.subscribe("/state", 1, &HumanRobot::get_state, this);
    shutdown_sub = nh.subscribe("/shutdown", 1, &HumanRobot::get_shutdown, this);

    // set up data vectors
    dataIn = arma::zeros<arma::vec>(6);
    dataOut = arma::zeros<arma::vec>(6);
    cdataIn = arma::zeros<arma::vec>(2);
    hdataIn = arma::zeros<arma::vec>(2);
    hdataOut = arma::zeros<arma::vec>(2);
    current_state = arma::zeros<arma::vec>(6);

    // set up Linear Koopman
    linear_koopman_operator = new KoopmanOperator(new LinearBasisFunction());

    // set up Non Linear Koopman
    nonlinear_koopman_operator = new KoopmanOperator(new NonLinearBasisFunction());

  }

  void get_state(const model_based_shared_control::State::ConstPtr& msg) {
    if (has_initialized == false) {
      dataOut[0] = msg->x;
      dataOut[1] = msg->y;
      dataOut[2] = msg->theta;
      dataOut[3] = msg->x_dot;
      dataOut[4] = msg->y_dot;
      dataOut[5] = msg->theta_dot;
      hdataOut[0] = msg->u_1;
      hdataOut[1] = msg->u_2;
      has_initialized = true;
    } else {
       if (std::abs(dataOut[0] - msg->x) + std::abs(dataOut[1] - msg->y) < 0.3){
        dataIn = dataOut;
        hdataIn = hdataOut;
        dataOut[0] = msg->x;
        dataOut[1] = msg->y;
        dataOut[2] = msg->theta;
        dataOut[3] = msg->x_dot;
        dataOut[4] = msg->y_dot;
        dataOut[5] = msg->theta_dot;
        hdataOut[0] = msg->u_1;
        hdataOut[1] = msg->u_2;
        linear_koopman_operator->gradStep(dataIn, hdataIn, dataOut, hdataOut);
        nonlinear_koopman_operator->gradStep(dataIn, hdataIn, dataOut, hdataOut);
        current_state = dataOut;
      } else {
        has_initialized = false;
      }
		}
	}

  void get_shutdown(const std_msgs::String::ConstPtr& msg) {
    std::string filePath = msg->data;
    std::string linearFilePath = filePath + "-koopman-linear";
    std::string nonlinearFilePath = filePath + "-koopman-nonlinear";
    linear_koopman_operator->saveOperator(linearFilePath);
    nonlinear_koopman_operator->saveOperator(nonlinearFilePath);
  }

};

int main(int argc, char** argv) {
  ros::init(argc, argv,"human_robot");
  ros::NodeHandle nh;
  ros::Rate loop_rate(10);
  HumanRobot sys(&loop_rate);

  while (ros::ok()) {
    loop_rate.sleep();
    ros::spinOnce();
  }

  return 0;
}


/**
 * @file calc-characteristic.cpp
 *
 * @brief calculate characteristic polynomial.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <iostream>
#include <iomanip>
#include <sstream>
#include <string>
#include <inttypes.h>
#include <stdint.h>
#include "SFMText.hpp"
#include "SFMT-calc-jump.hpp"
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/GF2XFactoring.h>

namespace sfmt {
    using namespace NTL;
    using namespace std;
    static void calc_minimal(GF2X& minimal, SFMText& sfmt, int bitpos)
    {
	uint32_t mask[4];
	for (int i = 0; i < 4; i++) {
	    mask[i] = 0;
	}
	int pos = bitpos / 32;
	uint32_t m = 1 << (bitpos % 32);
	mask[pos] = m;
	int maxdegree = sfmt.get_maxdegree();
	vec_GF2 seq;
	seq.SetLength(2 * maxdegree);
	for (int i = 0; i < 2 * maxdegree; i++) {
	    seq[i] = sfmt.next(mask);
	}
	MinPolySeq(minimal, seq, maxdegree);
#ifdef DEBUG
	if (deg(minimal) == 0) {
	    cout << "deg minimal:" << dec << deg(minimal) << endl;
	    cout << "minimal:" << minimal << endl;
	    cout << "seq:" << seq << endl;
	}
#endif
    }

    static void LCM(GF2X& lcm, const GF2X& x, const GF2X& y) {
	GF2X gcd;
	mul(lcm, x, y);
	GCD(gcd, x, y);
	lcm /= gcd;
    }

    void get_characteristic(GF2X& lcmpoly, SFMText& sfmt) {
	GF2X minimal;
	GF2X tmp;
	int maxdegree = sfmt.get_maxdegree();
	sfmt.seeding(1234);
	for (int bitpos = 0; bitpos < 128; bitpos++) {
	    calc_minimal(minimal, sfmt, bitpos);
	    LCM(tmp, lcmpoly, minimal);
	    lcmpoly = tmp;
	    if (deg(lcmpoly) == maxdegree) {
		return;
	    }
	}
	for (int i = 0; i < maxdegree; i++) {
	    sfmt.init_basis();
	    for (int bitpos = 0; bitpos < 128; bitpos++) {
		calc_minimal(minimal, sfmt, bitpos);
		LCM(tmp, lcmpoly, minimal);
		lcmpoly = tmp;
		if (deg(lcmpoly) == maxdegree) {
		    return;
		}
	    }
	}
	cerr << "deg:" << deg(lcmpoly) << endl;
	throw new logic_error("can't find lcm");
    }

    static void check(SFMText& sfmt) {
	sfmt.seeding(1234);

	for (int i = 0; i < 10; i++) {
	    w128_t x = sfmt.next();
	    for (int j = 0; j < 4; j++) {
		cout << dec << x.u[j] << endl;
	    }
	}
    }

    static int has_large_irreducible(GF2X& fpoly, int degree) {
	static const GF2X t2(2, 1);
	static const GF2X t1(1, 1);
	GF2X t2m;
	GF2X t;
	GF2X alpha;
	int m;

	t2m = t2;
	if (deg(fpoly) < degree) {
	    return 0;
	}
	t = t1;
	t += t2m;

	for (m = 1; deg(fpoly) > degree; m++) {
	    for(;;) {
		GCD(alpha, fpoly, t);
		if (IsOne(alpha)) {
		    break;
		}
		fpoly /= alpha;
		if (deg(fpoly) < degree) {
		    return 0;
		}
	    }
	    t2m *= t2m;
	    t2m %= fpoly;
	    add(t, t2m, t1);
	}
	if (deg(fpoly) != degree) {
	    return 0;
	}
	return IterIrredTest(fpoly);
    }
}
#if defined(MAIN)
using namespace sfmt;
using namespace NTL;
using namespace std;

int main(int argc, char *argv[]) {
    if (argc < 11) {
	cout << argv[0]
	     << " mexp sl1 sl2 sr1 sr2 pos1 mask1 mask2 mask3 mask4"
	     << " parity1 parity2 parity3 parity4"
	     << endl;
	return -1;
    }
    int mexp = strtol(argv[1], NULL, 10);
    int pos1 = strtol(argv[2], NULL, 10);
    int sl1 = strtol(argv[3], NULL, 10);
    int sl2 = strtol(argv[4], NULL, 10);
    int sr1 = strtol(argv[5], NULL, 10);
    int sr2 = strtol(argv[6], NULL, 10);
    uint32_t mask[4];
    mask[0] = strtoull(argv[7], NULL, 16);
    mask[1] = strtoull(argv[8], NULL, 16);
    mask[2] = strtoull(argv[9], NULL, 16);
    mask[3] = strtoull(argv[10], NULL, 16);
    uint32_t parity[4];
    parity[0] = strtoull(argv[11], NULL, 16);
    parity[1] = strtoull(argv[12], NULL, 16);
    parity[2] = strtoull(argv[13], NULL, 16);
    parity[3] = strtoull(argv[14], NULL, 16);
#if defined(DEBUG)
    cout << "mexp:" << dec << mexp << endl;
    cout << "pos1:" << dec << pos1 << endl;
    cout << "sl1:" << dec << sl1 << endl;
    cout << "sl2:" << dec << sl2 << endl;
    cout << "sr1:" << dec << sr1 << endl;
    cout << "sr2:" << dec << sr2 << endl;
    cout << "mask1:" << hex<< mask[0] << endl;
    cout << "mask2:" << hex << mask[1] << endl;
    cout << "mask3:" << hex << mask[2] << endl;
    cout << "mask4:" << hex << mask[3] << endl;
    cout << "parity1:" << hex<< parity[0] << endl;
    cout << "parity2:" << hex << parity[1] << endl;
    cout << "parity3:" << hex << parity[2] << endl;
    cout << "parity4:" << hex << parity[3] << endl;
#endif
    SFMText sfmt(mexp, sl1, sl2, sr1, sr2, pos1,
		 mask, parity);
    GF2X characteristic(0, 1);
#if defined(DEBUG)
    check(sfmt);
#endif
    get_characteristic(characteristic, sfmt);
    GF2X work;
    work = characteristic;
#if defined(DEBUG)
    cout << "degree:" << deg(characteristic) << endl;
    cout << characteristic << endl;
#endif
    if (!has_large_irreducible(characteristic, mexp)) {
	cout << "error?" << endl;
	return -1;
    }
    string x;
    polytostring(x, work);
#if defined(DEBUG)
    cout << "characteristic:" << x << endl;
#endif
    cout << "#" << dec << mexp;
    cout << "," << dec << pos1;
    cout << "," << dec << sl1;
    cout << "," << dec << sl2;
    cout << "," << dec << sr1;
    cout << "," << dec << sr2;
    cout << "," << hex << mask[0];
    cout << "," << hex << mask[1];
    cout << "," << hex << mask[2];
    cout << "," << hex << mask[3];
    cout << "," << hex << parity[0];
    cout << "," << hex << parity[1];
    cout << "," << hex << parity[2];
    cout << "," << hex << parity[3];
    cout << endl;
    cout << x << endl;
    cout << dec << flush;
    return 0;
}
#endif


//[ LazyVector
///////////////////////////////////////////////////////////////////////////////
//  Copyright 2008 Eric Niebler. 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)
//
// This example constructs a mini-library for linear algebra, using
// expression templates to eliminate the need for temporaries when
// adding vectors of numbers.
//
// This example uses a domain with a grammar to prune the set
// of overloaded operators. Only those operators that produce
// valid lazy vector expressions are allowed.

#include <vector>
#include <iostream>
#include <boost/mpl/int.hpp>
#include <boost/proto/core.hpp>
#include <boost/proto/context.hpp>
namespace mpl = boost::mpl;
namespace proto = boost::proto;
using proto::_;

// This grammar describes which lazy vector expressions
// are allowed; namely, vector terminals and addition
// and subtraction of lazy vector expressions.
struct LazyVectorGrammar
  : proto::or_<
        proto::terminal< std::vector<_> >
      , proto::plus< LazyVectorGrammar, LazyVectorGrammar >
      , proto::minus< LazyVectorGrammar, LazyVectorGrammar >
    >
{};

// Expressions in the lazy vector domain must conform
// to the lazy vector grammar
struct lazy_vector_domain;

// Here is an evaluation context that indexes into a lazy vector
// expression, and combines the result.
template<typename Size = std::size_t>
struct lazy_subscript_context
{
    lazy_subscript_context(Size subscript)
      : subscript_(subscript)
    {}

    // Use default_eval for all the operations ...
    template<typename Expr, typename Tag = typename Expr::proto_tag>
    struct eval
      : proto::default_eval<Expr, lazy_subscript_context>
    {};

    // ... except for terminals, which we index with our subscript
    template<typename Expr>
    struct eval<Expr, proto::tag::terminal>
    {
        typedef typename proto::result_of::value<Expr>::type::value_type result_type;

        result_type operator ()( Expr const & expr, lazy_subscript_context & ctx ) const
        {
            return proto::value( expr )[ ctx.subscript_ ];
        }
    };

    Size subscript_;
};

// Here is the domain-specific expression wrapper, which overrides
// operator [] to evaluate the expression using the lazy_subscript_context.
template<typename Expr>
struct lazy_vector_expr
  : proto::extends<Expr, lazy_vector_expr<Expr>, lazy_vector_domain>
{
    typedef proto::extends<Expr, lazy_vector_expr<Expr>, lazy_vector_domain> base_type;

    lazy_vector_expr( Expr const & expr = Expr() )
      : base_type( expr )
    {}

    // Use the lazy_subscript_context<> to implement subscripting
    // of a lazy vector expression tree.
    template< typename Size >
    typename proto::result_of::eval< Expr, lazy_subscript_context<Size> >::type
    operator []( Size subscript ) const
    {
        lazy_subscript_context<Size> ctx(subscript);
        return proto::eval(*this, ctx);
    }
};

// Here is our lazy_vector terminal, implemented in terms of lazy_vector_expr
template< typename T >
struct lazy_vector
  : lazy_vector_expr< typename proto::terminal< std::vector<T> >::type >
{
    typedef typename proto::terminal< std::vector<T> >::type expr_type;

    lazy_vector( std::size_t size = 0, T const & value = T() )
      : lazy_vector_expr<expr_type>( expr_type::make( std::vector<T>( size, value ) ) )
    {}

    // Here we define a += operator for lazy vector terminals that
    // takes a lazy vector expression and indexes it. expr[i] here
    // uses lazy_subscript_context<> under the covers.
    template< typename Expr >
    lazy_vector &operator += (Expr const & expr)
    {
        std::size_t size = proto::value(*this).size();
        for(std::size_t i = 0; i < size; ++i)
        {
            proto::value(*this)[i] += expr[i];
        }
        return *this;
    }
};

// Tell proto that in the lazy_vector_domain, all
// expressions should be wrapped in laxy_vector_expr<>
struct lazy_vector_domain
  : proto::domain<proto::generator<lazy_vector_expr>, LazyVectorGrammar>
{};

int main()
{
    // lazy_vectors with 4 elements each.
    lazy_vector< double > v1( 4, 1.0 ), v2( 4, 2.0 ), v3( 4, 3.0 );

    // Add two vectors lazily and get the 2nd element.
    double d1 = ( v2 + v3 )[ 2 ];   // Look ma, no temporaries!
    std::cout << d1 << std::endl;

    // Subtract two vectors and add the result to a third vector.
    v1 += v2 - v3;                  // Still no temporaries!
    std::cout << '{' << v1[0] << ',' << v1[1]
              << ',' << v1[2] << ',' << v1[3] << '}' << std::endl;

    // This expression is disallowed because it does not conform
    // to the LazyVectorGrammar
    //(v2 + v3) += v1;

    return 0;
}
//]


#include "libsnark/gadgetlib1/gadgets/basic_gadgets.hpp"
#include "libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp"
#include "libsnark/common/default_types/r1cs_ppzksnark_pp.hpp"
#include "libff/common/utils.hpp"
#include <boost/optional.hpp>

using namespace libsnark;
using namespace std;

#include "payment_in_out_gadget.hpp"
#include "payment_multi_gadget.hpp"

template<typename ppzksnark_ppT>
r1cs_ppzksnark_keypair<ppzksnark_ppT> generate_keypair()
{
    typedef Fr<ppzksnark_ppT> FieldT;

    protoboard<FieldT> pb;
    payment_in_out_gadget<FieldT> g(pb);
    g.generate_payment_in_out_constraints();
    const r1cs_constraint_system<FieldT> constraint_system = pb.get_constraint_system();

    cout << "Number of R1CS constraints: " << constraint_system.num_constraints() << endl;

    return r1cs_ppzksnark_generator<ppzksnark_ppT>(constraint_system);
}

template<typename ppzksnark_ppT>
r1cs_ppzksnark_keypair<ppzksnark_ppT> generate_keypair_multi()
{
    typedef Fr<ppzksnark_ppT> FieldT;

    protoboard<FieldT> pb;
    payment_multi_gadget<FieldT> g(pb);
    g.generate_payment_multi_constraints();
    const r1cs_constraint_system<FieldT> constraint_system = pb.get_constraint_system();

    cout << "Number of R1CS constraints: " << constraint_system.num_constraints() << endl;

    return r1cs_ppzksnark_generator<ppzksnark_ppT>(constraint_system);
}

template<typename ppzksnark_ppT>
boost::optional<r1cs_ppzksnark_proof<ppzksnark_ppT>> generate_payment_in_out_proof(r1cs_ppzksnark_proving_key<ppzksnark_ppT> proving_key,
                                                                   const bit_vector &h_startbalance,
                                                                   const bit_vector &h_endbalance,
                                                                   const bit_vector &h_incoming,
                                                                   const bit_vector &h_outgoing,
                                                                   const bit_vector &r_startbalance,
                                                                   const bit_vector &r_endbalance,
                                                                   const bit_vector &r_incoming,
                                                                   const bit_vector &r_outgoing
                                                                   )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    protoboard<FieldT> pb;
    payment_in_out_gadget<FieldT> g(pb);
    g.generate_payment_in_out_constraints();
    g.generate_payment_in_out_witness(h_startbalance, h_endbalance, h_incoming, h_outgoing, r_startbalance, r_endbalance, r_incoming, r_outgoing);

    if (!pb.is_satisfied()) {
      std::cout << "System not satisfied!" << std::endl;
        return boost::none;
    }

    return r1cs_ppzksnark_prover<ppzksnark_ppT>(proving_key, pb.primary_input(), pb.auxiliary_input());
}

template<typename ppzksnark_ppT>
boost::optional<r1cs_ppzksnark_proof<ppzksnark_ppT>> generate_payment_multi_proof(r1cs_ppzksnark_proving_key<ppzksnark_ppT> proving_key,
                                                            const bit_vector &h_startbalance,
                                                            const bit_vector &h_endbalance,
                                                            const bit_vector *h_incoming,
                                                            const bit_vector *h_outgoing,
                                                            const bit_vector &r_startbalance,
                                                            const bit_vector &r_endbalance,
                                                            const bit_vector *r_incoming,
                                                            const bit_vector *r_outgoing
                                                            )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    protoboard<FieldT> pb;
    payment_multi_gadget<FieldT> g(pb);
    g.generate_payment_multi_constraints();
    g.generate_payment_multi_witness(h_startbalance, h_endbalance, h_incoming, h_outgoing, r_startbalance, r_endbalance, r_incoming, r_outgoing);

    if (!pb.is_satisfied()) {
      std::cout << "System not satisfied!" << std::endl;
        return boost::none;
    }

    return r1cs_ppzksnark_prover<ppzksnark_ppT>(proving_key, pb.primary_input(), pb.auxiliary_input());
}

template<typename ppzksnark_ppT>
bool verify_payment_in_out_proof(r1cs_ppzksnark_verification_key<ppzksnark_ppT> verification_key,
                  r1cs_ppzksnark_proof<ppzksnark_ppT> proof,
                  const bit_vector &h_startbalance,
                  const bit_vector &h_endbalance,
                  const bit_vector &h_incoming,
                  const bit_vector &h_outgoing
                 )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    const r1cs_primary_input<FieldT> input = l_input_map<FieldT>(h_startbalance, h_endbalance, h_incoming, h_outgoing);

    std::cout << "**** After l_input_map *****" << std::endl;

    return r1cs_ppzksnark_verifier_strong_IC<ppzksnark_ppT>(verification_key, input, proof);

}

template<typename ppzksnark_ppT>
bool verify_payment_multi_proof(r1cs_ppzksnark_verification_key<ppzksnark_ppT> verification_key,
                  r1cs_ppzksnark_proof<ppzksnark_ppT> proof,
                  const bit_vector &h_startbalance,
                  const bit_vector &h_endbalance,
                  const bit_vector *h_incoming,
                  const bit_vector *h_outgoing
                 )
{
    typedef Fr<ppzksnark_ppT> FieldT;
    const r1cs_primary_input<FieldT> input = l_input_map_multi<FieldT>(h_startbalance, h_endbalance, h_incoming, h_outgoing);

    std::cout << "**** After l_input_map_multi *****" << std::endl;

    return r1cs_ppzksnark_verifier_strong_IC<ppzksnark_ppT>(verification_key, input, proof);

}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#include <boost/simd/arch/common/detail/generic/sinh_kernel.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/constant/maxlog.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/scalar/average.hpp>
#include <boost/simd/function/scalar/bitofsign.hpp>
#include <boost/simd/function/scalar/bitwise_xor.hpp>
#include <boost/simd/function/scalar/exp.hpp>
#include <boost/simd/function/scalar/if_else.hpp>
#include <boost/simd/function/scalar/rec.hpp>
#include <boost/simd/function/scalar/sqr.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( sinhcosh_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_<bd::floating_<A0> >
                          , bd::scalar_<bd::floating_<A0> >
                          , bd::scalar_<bd::floating_<A0> >
                          )
  {
    //////////////////////////////////////////////////////////////////////////////
    // if x = abs(a0) is less than 1 sinh is computed using a polynomial(float)
    // respectively rational(double) approx from cephes.
    // else according x < Threshold e =  exp(x) or exp(x/2) is respectively
    // computed
    // * in the first case sinh is (e-rec(e))/2 and cosh (e+rec(e))/2
    // * in the second     sinh and cosh are (e/2)*e (avoiding undue overflow)
    // Threshold is Maxlog - Log_2
    //////////////////////////////////////////////////////////////////////////////
    BOOST_FORCEINLINE void operator() ( A0 a0,A0 & a1,A0 & a2) const BOOST_NOEXCEPT
    {
      A0 x = bs::abs(a0);
      auto test1 = (x >  Maxlog<A0>()-Log_2<A0>());
      A0 fac = if_else(test1, Half<A0>(), One<A0>());
      A0 tmp = exp(x*fac);
      A0 tmp1 = Half<A0>()*tmp;
      A0 rtmp = rec(tmp);
      A0 r =  test1 ? tmp1*tmp : tmp1-Half<A0>()*rtmp;
      a1 = bitwise_xor(((x < One<A0>())
                  ? detail::sinh_kernel<A0>::compute(x, sqr(x))
                  : r),
                 bitofsign(a0));
      a2 = test1 ? r : bs::average(tmp, rtmp);
    }
  };
} } }

#endif


#include <algorithm>
#include <cmath>
#include <vector>
#include <Eigen/Dense>
#include "bicycle/bicycle.h"
#include "control_design_functions.h"
#include "firmware_generator.h"
#include "robot_bicycle_parameters.h"

int main(int argc, char ** argv)
{
  bicycle::Bicycle rb = bicycle::robot_bicycle();

  design_parameters params;
  params.N = 101;
  params.Ts = 0.005;
  params.lowest_speed = 0.5;
  params.highest_speed = 10.0;
  // LQR design parameters
  constexpr double pi = M_PI;
  constexpr double max_lean = 2.0*pi/180.0;    // rad
  constexpr double max_steer = 5.0*pi/180.0;   // rad
  constexpr double max_lean_frequency = 1*pi;  // rad / s
  constexpr double max_steer_frequency = 20*pi;// rad / s
  constexpr double max_steer_torque = 0.5;     // N * m
  params.Q = Eigen::MatrixXd::Zero(4, 4);
  params.Q(0, 0) = std::pow(max_lean, -2.0);
  params.Q(1, 1) = std::pow(max_steer, -2.0);
  params.Q(2, 2) = std::pow(max_lean_frequency * max_lean, -2.0);
  params.Q(3, 3) = std::pow(max_steer_frequency * max_steer, -2.0);
  params.R.resize(1, 1);
  params.R << std::pow(max_steer_torque, -2.0);
  // Observer pole placement factor
  params.pole_placement_factor = 3.0;
  // Kalman design
  constexpr double lean_acc_std = 1e-3;          // rad / s / s
  constexpr double steer_acc_std = 1e-2;         // rad / s / s
  constexpr double lean_vel_std = .6827 * lean_acc_std;      // rad / s
  constexpr double steer_vel_std = .6827 * steer_acc_std;     // rad / s
  params.W.resize(4, 4);          // Process noise covariance
  params.W << std::pow(lean_vel_std, 2.0), 0.0, 0.0, 0.0,
              0.0, std::pow(steer_vel_std, 2.0), 0.0, 0.0,
              0.0, 0.0, std::pow(lean_acc_std, 2.0), 0.0,
              0.0, 0.0, 0.0, std::pow(steer_acc_std, 2.0);

  params.V.resize(2, 2);          // Measurement noise covariance
  params.V << std::pow(2*pi/20000, 2.0), 0,
              0, std::pow(0.00227631723111, 2.0);

  std::vector<model_data> md = design_controller(params, rb);
  std::sort(md.begin(), md.end());
  firmware_generator(md);
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2008 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Wolfgang Bangerth, Texas A&M University, 2008
 */

// @sect3{Include files}

// 像往常一样，我们从包括一些著名的文件开始。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/function.h>
#include <deal.II/base/utilities.h>

#include <deal.II/lac/block_vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/block_sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_tools.h>
#include <deal.II/grid/grid_refinement.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_renumbering.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_system.h>
#include <deal.II/fe/fe_values.h>

#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>

// 然后我们需要包括稀疏直接求解器UMFPACK的头文件。

#include <deal.II/lac/sparse_direct.h>

// 这包括不完全LU因子化的库，它将被用作3D的预处理程序。

#include <deal.II/lac/sparse_ilu.h>

// 这是C++语言。

#include <iostream>
#include <fstream>
#include <memory>

// 和所有的程序一样，名字空间dealii被包括在内。

namespace Step22
{
  using namespace dealii;
// @sect3{Defining the inner preconditioner type}

// 正如介绍中所解释的，我们将分别对两个和三个空间维度使用不同的预处理程序。我们通过使用空间维度作为模板参数来区分它们。关于模板的细节，请参见 step-4 。我们不打算在这里创建任何预处理对象，我们所做的只是创建一个持有确定预处理类的本地别名的类，这样我们就可以以独立于维度的方式编写我们的程序。

  template <int dim>
  struct InnerPreconditioner;

// 在二维中，我们将使用一个稀疏的直接求解器作为预处理程序。

  template <>
  struct InnerPreconditioner<2>
  {
    using type = SparseDirectUMFPACK;
  };

// 还有三维的ILU预处理，由SparseILU调用。

  template <>
  struct InnerPreconditioner<3>
  {
    using type = SparseILU<double>;
  };
// @sect3{The StokesProblem class template}

// 这是对 step-20 的改编，所以主类和数据类型与那里使用的几乎相同。唯一不同的是，我们有一个额外的成员  preconditioner_matrix  ，用于预处理Schur补码，以及一个相应的稀疏模式  preconditioner_sparsity_pattern  。此外，我们没有依赖LinearOperator，而是实现了我们自己的InverseMatrix类。

// 在这个例子中，我们还使用了自适应网格细化，其处理方式与  step-6  类似。根据介绍中的讨论，我们也将使用AffineConstraints对象来实现Dirichlet边界条件。因此，我们改变名称  hanging_node_constraints into constraints  。

  template <int dim>
  class StokesProblem
  {
  public:
    StokesProblem(const unsigned int degree);
    void run();

  private:
    void setup_dofs();
    void assemble_system();
    void solve();
    void output_results(const unsigned int refinement_cycle) const;
    void refine_mesh();

    const unsigned int degree;

    Triangulation<dim> triangulation;
    FESystem<dim>      fe;
    DoFHandler<dim>    dof_handler;

    AffineConstraints<double> constraints;

    BlockSparsityPattern      sparsity_pattern;
    BlockSparseMatrix<double> system_matrix;

    BlockSparsityPattern      preconditioner_sparsity_pattern;
    BlockSparseMatrix<double> preconditioner_matrix;

    BlockVector<double> solution;
    BlockVector<double> system_rhs;

// 这一条是新的：我们将使用一个所谓的共享指针结构来访问预处理程序。共享指针本质上只是指针的一种方便形式。几个共享指针可以指向同一个对象（就像普通的指针一样），但是当最后一个指向前提器对象的共享指针对象被删除时（例如共享指针对象超出了范围，它所在的类被销毁，或者指针被分配给了不同的前提器对象），那么指向的前提器对象也被销毁。这确保了我们不必手动跟踪有多少地方仍在引用一个前置条件器对象，它永远不会产生内存泄漏，也不会产生一个指向已被销毁对象的悬空指针。

    std::shared_ptr<typename InnerPreconditioner<dim>::type> A_preconditioner;
  };
// @sect3{Boundary values and right hand side}

// 与 step-20 和其他大多数例子程序一样，下一个任务是定义PDE的数据：对于斯托克斯问题，我们将在部分边界上使用自然边界值（即同质诺伊曼型），对于这些边界，我们不必做任何特殊处理（同质性意味着弱形式中的相应项只是零），而在边界的其余部分使用速度的边界条件（迪里希勒型），如介绍中所述。

// 为了强制执行速度上的Dirichlet边界值，我们将像往常一样使用 VectorTools::interpolate_boundary_values 函数，这要求我们写一个具有与有限元一样多分量的函数对象。换句话说，我们必须在 $(u,p)$ -空间上定义函数，但在插值边界值时，我们要过滤掉压力分量。

// 下面的函数对象是介绍中描述的边界值的表示。

  template <int dim>
  class BoundaryValues : public Function<dim>
  {
  public:
    BoundaryValues()
      : Function<dim>(dim + 1)
    {}

    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;

    virtual void vector_value(const Point<dim> &p,
                              Vector<double> &  value) const override;
  };

  template <int dim>
  double BoundaryValues<dim>::value(const Point<dim> & p,
                                    const unsigned int component) const
  {
    Assert(component < this->n_components,
           ExcIndexRange(component, 0, this->n_components));

    if (component == 0)
      return (p[0] < 0 ? -1 : (p[0] > 0 ? 1 : 0));
    return 0;
  }

  template <int dim>
  void BoundaryValues<dim>::vector_value(const Point<dim> &p,
                                         Vector<double> &  values) const
  {
    for (unsigned int c = 0; c < this->n_components; ++c)
      values(c) = BoundaryValues<dim>::value(p, c);
  }

// 我们为右手边实现类似的函数，在目前的例子中，右手边只是零。

  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    RightHandSide()
      : Function<dim>(dim + 1)
    {}

    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;

    virtual void vector_value(const Point<dim> &p,
                              Vector<double> &  value) const override;
  };

  template <int dim>
  double RightHandSide<dim>::value(const Point<dim> & /*p*/,
                                   const unsigned int /*component*/) const
  {
    return 0;
  }

  template <int dim>
  void RightHandSide<dim>::vector_value(const Point<dim> &p,
                                        Vector<double> &  values) const
  {
    for (unsigned int c = 0; c < this->n_components; ++c)
      values(c) = RightHandSide<dim>::value(p, c);
  }
// @sect3{Linear solvers and preconditioners}

// 在介绍中广泛讨论了线性求解器和预处理器。在这里，我们创建将被使用的各自对象。

//  @sect4{The InverseMatrix class template}   InverseMatrix 类表示逆矩阵的数据结构。与 step-20 不同，我们用一个类来实现，而不是用辅助函数inverse_linear_operator()，我们将把这个类应用于不同种类的矩阵，这些矩阵需要不同的预处理程序（在 step-20 中，我们只对质量矩阵使用非同一性预处理程序）。矩阵和预处理器的类型通过模板参数传递给这个类，当创建 InverseMatrix 对象时，这些类型的矩阵和预处理器对象将被传递给构造器。成员函数 vmult 是通过解决一个线性系统得到的。

  template <class MatrixType, class PreconditionerType>
  class InverseMatrix : public Subscriptor
  {
  public:
    InverseMatrix(const MatrixType &        m,
                  const PreconditionerType &preconditioner);

    void vmult(Vector<double> &dst, const Vector<double> &src) const;

  private:
    const SmartPointer<const MatrixType>         matrix;
    const SmartPointer<const PreconditionerType> preconditioner;
  };

  template <class MatrixType, class PreconditionerType>
  InverseMatrix<MatrixType, PreconditionerType>::InverseMatrix(
    const MatrixType &        m,
    const PreconditionerType &preconditioner)
    : matrix(&m)
    , preconditioner(&preconditioner)
  {}

// 这就是 vmult 函数的实现。

// 在这个类中，我们对解算器控制使用了一个相当大的容忍度。这样做的原因是，该函数被频繁使用，因此，任何使CG求解中的残差变小的额外努力都会使求解更加昂贵。请注意，我们不仅将该类作为Schur补码的预处理程序，而且在形成拉普拉斯矩阵的逆时也使用该类；因此，该类直接对解本身的精度负责，所以我们也不能选择太大的公差。

  template <class MatrixType, class PreconditionerType>
  void InverseMatrix<MatrixType, PreconditionerType>::vmult(
    Vector<double> &      dst,
    const Vector<double> &src) const
  {
    SolverControl            solver_control(src.size(), 1e-6 * src.l2_norm());
    SolverCG<Vector<double>> cg(solver_control);

    dst = 0;

    cg.solve(*matrix, dst, src, *preconditioner);
  }
// @sect4{The SchurComplement class template}

// 这个类实现了介绍中讨论的Schur补码。它与  step-20  相类似。 不过，我们现在用一个模板参数 PreconditionerType 来调用它，以便在指定逆矩阵类的各自类型时访问它。作为上述定义的结果，声明  InverseMatrix  现在包含了上述预处理类的第二个模板参数，这也影响到  SmartPointer object m_inverse  。

  template <class PreconditionerType>
  class SchurComplement : public Subscriptor
  {
  public:
    SchurComplement(
      const BlockSparseMatrix<double> &system_matrix,
      const InverseMatrix<SparseMatrix<double>, PreconditionerType> &A_inverse);

    void vmult(Vector<double> &dst, const Vector<double> &src) const;

  private:
    const SmartPointer<const BlockSparseMatrix<double>> system_matrix;
    const SmartPointer<
      const InverseMatrix<SparseMatrix<double>, PreconditionerType>>
      A_inverse;

    mutable Vector<double> tmp1, tmp2;
  };

  template <class PreconditionerType>
  SchurComplement<PreconditionerType>::SchurComplement(
    const BlockSparseMatrix<double> &system_matrix,
    const InverseMatrix<SparseMatrix<double>, PreconditionerType> &A_inverse)
    : system_matrix(&system_matrix)
    , A_inverse(&A_inverse)
    , tmp1(system_matrix.block(0, 0).m())
    , tmp2(system_matrix.block(0, 0).m())
  {}

  template <class PreconditionerType>
  void
  SchurComplement<PreconditionerType>::vmult(Vector<double> &      dst,
                                             const Vector<double> &src) const
  {
    system_matrix->block(0, 1).vmult(tmp1, src);
    A_inverse->vmult(tmp2, tmp1);
    system_matrix->block(1, 0).vmult(dst, tmp2);
  }
// @sect3{StokesProblem class implementation}
// @sect4{StokesProblem::StokesProblem}

// 这个类的构造函数看起来与  step-20  的构造函数非常相似。构造函数初始化了多项式程度、三角形、有限元系统和dof处理器的变量。矢量速度分量的基础多项式函数的阶数为 degree+1 ，压力的阶数为 degree 。 这就得到了LBB稳定元对 $Q_{degree+1}^d\times Q_{degree}$ ，通常被称为泰勒-霍德元。

// 请注意，我们用MeshSmoothing参数初始化三角形，这可以确保单元的细化是以PDE解的近似保持良好的方式进行的（如果网格过于非结构化就会出现问题），详情请参见 Triangulation::MeshSmoothing 的文档。

  template <int dim>
  StokesProblem<dim>::StokesProblem(const unsigned int degree)
    : degree(degree)
    , triangulation(Triangulation<dim>::maximum_smoothing)
    , fe(FE_Q<dim>(degree + 1), dim, FE_Q<dim>(degree), 1)
    , dof_handler(triangulation)
  {}
// @sect4{StokesProblem::setup_dofs}

// 给定一个网格，该函数将自由度与之关联，并创建相应的矩阵和向量。在开始的时候，它还释放了指向预处理对象的指针（如果共享指针在此时指向任何东西的话），因为在这之后肯定不会再需要它了，在组装矩阵后必须重新计算，并将稀疏矩阵从其稀疏模式对象中解开。

// 然后，我们继续分配自由度并重新编号。为了使ILU预处理程序（在3D中）有效地工作，重要的是以这样的方式列举自由度，以减少矩阵的带宽，或者也许更重要的是：以这样的方式使ILU尽可能地接近于真正的LU分解。另一方面，我们需要保留在  step-20  和  step-21  中已经看到的速度和压力的块状结构。这将分两步完成。首先，对所有的道次进行重新编号，以改善ILU，然后我们再一次按组件重新编号。由于 DoFRenumbering::component_wise 没有触及单个块内的重新编号，所以第一步的基本重新编号仍然存在。至于如何对自由度进行重新编号以提高ILU：deal.II有许多算法试图找到排序以提高ILU，或减少矩阵的带宽，或优化其他方面。DoFRenumbering命名空间显示了我们在本教程程序中基于这里讨论的测试案例而获得的几种算法的结果比较。在这里，我们将使用传统的Cuthill-McKee算法，该算法已经在之前的一些教程程序中使用。 在section on improved ILU中我们将更详细地讨论这个问题。
//与以前的教程程序相比，
//还有一个变化。没有理由对 dim 的速度成分进行单独排序。事实上，与其先列举所有 $x$ -velocities，再列举所有 $y$ -velocities，等等，我们希望将所有速度放在一起，只在速度（所有分量）和压力之间分开。默认情况下， DoFRenumbering::component_wise 函数不是这样做的：它把每个矢量分量分开处理；我们要做的是把几个分量分成 "块"，并把这个块结构传递给该函数。因此，我们分配一个矢量 block_component ，有多少个元素就有多少个分量，描述所有的速度分量对应于块0，而压力分量将形成块1。

  template <int dim>
  void StokesProblem<dim>::setup_dofs()
  {
    A_preconditioner.reset();
    system_matrix.clear();
    preconditioner_matrix.clear();

    dof_handler.distribute_dofs(fe);
    DoFRenumbering::Cuthill_McKee(dof_handler);

    std::vector<unsigned int> block_component(dim + 1, 0);
    block_component[dim] = 1;
    DoFRenumbering::component_wise(dof_handler, block_component);

// 现在是对Dirichlet边界条件的实现，在介绍中的讨论之后，这应该是很明显的。所有的变化是，这个函数已经出现在设置函数中，而我们习惯于在一些汇编例程中看到它。在我们设置网格的下面，我们将把施加Dirichlet边界条件的顶部边界与边界指标1联系起来。 我们必须将这个边界指标作为第二个参数传递给下面的插值函数。 不过，还有一件事。 描述Dirichlet条件的函数是为所有分量定义的，包括速度和压力。然而，Dirichlet条件只为速度而设置。 为此，我们使用一个只选择速度分量的ComponentMask。通过指定我们想要的特定分量，从有限元中获得该分量掩码。由于我们使用自适应细化网格，仿生约束对象需要首先填充由DoF处理程序生成的悬挂节点约束。注意这两个函数的顺序；我们首先计算悬挂节点约束，然后将边界值插入约束对象。这确保了我们在有悬挂节点的边界上尊重H1一致性（在三个空间维度上），悬挂节点需要支配Dirichlet边界值。

    {
      constraints.clear();

      FEValuesExtractors::Vector velocities(0);
      DoFTools::make_hanging_node_constraints(dof_handler, constraints);
      VectorTools::interpolate_boundary_values(dof_handler,
                                               1,
                                               BoundaryValues<dim>(),
                                               constraints,
                                               fe.component_mask(velocities));
    }

    constraints.close();

// 与 step-20 相类似，我们计算各个组件中的道夫。我们可以用与那里相同的方式来做，但我们想在我们已经用于重新编号的块结构上进行操作。函数  DoFTools::count_dofs_per_fe_block  的作用与  DoFTools::count_dofs_per_fe_component  相同，但现在通过  block_component  将速度和压力块分组。

    const std::vector<types::global_dof_index> dofs_per_block =
      DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
    const unsigned int n_u = dofs_per_block[0];
    const unsigned int n_p = dofs_per_block[1];

    std::cout << "   Number of active cells: " << triangulation.n_active_cells()
              << std::endl
              << "   Number of degrees of freedom: " << dof_handler.n_dofs()
              << " (" << n_u << '+' << n_p << ')' << std::endl;

// 下一个任务是为我们将创建的系统矩阵分配一个稀疏模式，为预处理矩阵分配一个稀疏模式。我们可以用与 step-20 相同的方式来做这件事，即通过 DoFTools::make_sparsity_pattern. 直接建立一个SparsityPattern类型的对象，但是，有一个重要的理由不这样做。在3D中，函数 DoFTools::max_couplings_between_dofs 对各个道夫之间的耦合产生了一个保守但相当大的数字，因此，最初为创建矩阵的稀疏模式提供的内存太多--实际上，对于中等大小的3D问题，初始稀疏模式甚至无法放入大多数系统的物理内存中，也请参见 step-18  中的讨论。相反，我们首先建立临时对象，使用不同的数据结构，不需要分配更多的内存，但不适合作为SparseMatrix或BlockSparseMatrix对象的基础；在第二步，我们将这些对象复制到BlockSparsityPattern类型的对象中。这完全类似于我们在  step-11  和  step-18  中已经做过的事情。特别是，我们利用了这样一个事实，即我们永远不会写入系统矩阵的 $(1,1)$ 块中，而且这是唯一需要填充的预处理矩阵块。

// 所有这些都是在新范围内完成的，这意味着一旦信息被复制到  sparsity_pattern  ，  dsp  的内存将被释放。

    {
      BlockDynamicSparsityPattern dsp(2, 2);

      dsp.block(0, 0).reinit(n_u, n_u);
      dsp.block(1, 0).reinit(n_p, n_u);
      dsp.block(0, 1).reinit(n_u, n_p);
      dsp.block(1, 1).reinit(n_p, n_p);

      dsp.collect_sizes();

      Table<2, DoFTools::Coupling> coupling(dim + 1, dim + 1);

      for (unsigned int c = 0; c < dim + 1; ++c)
        for (unsigned int d = 0; d < dim + 1; ++d)
          if (!((c == dim) && (d == dim)))
            coupling[c][d] = DoFTools::always;
          else
            coupling[c][d] = DoFTools::none;

      DoFTools::make_sparsity_pattern(
        dof_handler, coupling, dsp, constraints, false);

    }

    {
      BlockDynamicSparsityPattern preconditioner_dsp(2, 2);

      preconditioner_dsp.block(0, 0).reinit(n_u, n_u);
      preconditioner_dsp.block(1, 0).reinit(n_p, n_u);
      preconditioner_dsp.block(0, 1).reinit(n_u, n_p);
      preconditioner_dsp.block(1, 1).reinit(n_p, n_p);

      preconditioner_dsp.collect_sizes();

      Table<2, DoFTools::Coupling> preconditioner_coupling(dim + 1, dim + 1);

      for (unsigned int c = 0; c < dim + 1; ++c)
        for (unsigned int d = 0; d < dim + 1; ++d)
          if (((c == dim) && (d == dim)))
            preconditioner_coupling[c][d] = DoFTools::always;
          else
            preconditioner_coupling[c][d] = DoFTools::none;

      DoFTools::make_sparsity_pattern(dof_handler,
                                      preconditioner_coupling,
                                      preconditioner_dsp,
                                      constraints,
                                      false);

      preconditioner_sparsity_pattern.copy_from(preconditioner_dsp);
    }

// 最后，与  step-20  中的方法类似，从块状结构中创建系统矩阵、前导矩阵、解决方案和右侧向量。

    system_matrix.reinit(sparsity_pattern);
    preconditioner_matrix.reinit(preconditioner_sparsity_pattern);

    solution.reinit(2);
    solution.block(0).reinit(n_u);
    solution.block(1).reinit(n_p);
    solution.collect_sizes();

    system_rhs.reinit(2);
    system_rhs.block(0).reinit(n_u);
    system_rhs.block(1).reinit(n_p);
    system_rhs.collect_sizes();
  }
// @sect4{StokesProblem::assemble_system}

// 汇编过程遵循 step-20 和介绍中的讨论。我们使用众所周知的缩写来表示保存本单元自由度的局部矩阵、右手边和全局编号的数据结构。

  template <int dim>
  void StokesProblem<dim>::assemble_system()
  {
    system_matrix         = 0;
    system_rhs            = 0;
    preconditioner_matrix = 0;

    QGauss<dim> quadrature_formula(degree + 2);

    FEValues<dim> fe_values(fe,
                            quadrature_formula,
                            update_values | update_quadrature_points |
                              update_JxW_values | update_gradients);

    const unsigned int dofs_per_cell = fe.n_dofs_per_cell();

    const unsigned int n_q_points = quadrature_formula.size();

    FullMatrix<double> local_matrix(dofs_per_cell, dofs_per_cell);
    FullMatrix<double> local_preconditioner_matrix(dofs_per_cell,
                                                   dofs_per_cell);
    Vector<double>     local_rhs(dofs_per_cell);

    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

    const RightHandSide<dim>    right_hand_side;
    std::vector<Vector<double>> rhs_values(n_q_points, Vector<double>(dim + 1));

// 接下来，我们需要两个对象，作为FEValues对象的提取器。它们的用途在  @ref  vector_valued 的报告中详细解释。

    const FEValuesExtractors::Vector velocities(0);
    const FEValuesExtractors::Scalar pressure(dim);

// 作为对 step-20 和 step-21 的扩展，我们包括了一些优化，使这个特定问题的装配速度大大加快。这些改进是基于这样的观察：当我们像 step-20 那样做时，我们做了太多次的计算：对称梯度实际上在每个正交点有 dofs_per_cell 个不同的值，但是我们从FEValues对象中提取了 dofs_per_cell*dofs_per_cell 次。

// - 在 i 的循环和 j 的内循环中。在3D中，这意味着评估它 $89^2=7921$ 次而不是 $89$ 次，这是一个不小的差别。

// 所以我们在这里要做的是，在开始对单元上的道夫进行循环之前，在正交点得到一个秩-2张量的向量（类似的还有压力上的发散和基函数值）来避免这种重复计算。首先，我们创建各自的对象来保存这些值。然后，我们开始在所有单元上进行循环，并在正交点上进行循环，在那里我们首先提取这些值。我们在这里还实现了一个优化：本地矩阵（以及全局矩阵）将是对称的，因为所有涉及的操作都是相对于 $i$ 和 $j$ 对称的。这可以通过简单地运行内循环而不是 dofs_per_cell, but only up to i 来实现，即外循环的索引。

    std::vector<SymmetricTensor<2, dim>> symgrad_phi_u(dofs_per_cell);
    std::vector<double>                  div_phi_u(dofs_per_cell);
    std::vector<double>                  phi_p(dofs_per_cell);

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        fe_values.reinit(cell);
        local_matrix                = 0;
        local_preconditioner_matrix = 0;
        local_rhs                   = 0;

        right_hand_side.vector_value_list(fe_values.get_quadrature_points(),
                                          rhs_values);

        for (unsigned int q = 0; q < n_q_points; ++q)
          {
            for (unsigned int k = 0; k < dofs_per_cell; ++k)
              {
                symgrad_phi_u[k] =
                  fe_values[velocities].symmetric_gradient(k, q);
                div_phi_u[k] = fe_values[velocities].divergence(k, q);
                phi_p[k]     = fe_values[pressure].value(k, q);
              }

// 最后是系统矩阵和我们用于预处理程序的矩阵的双线性形式。回顾一下，这两个的公式分别是
  //  @f{align*}{
  //    A_{ij} &= a(\varphi_i,\varphi_j)
  //    \\     &= \underbrace{2(\varepsilon(\varphi_{i,\textbf{u}}),
  //                            \varepsilon(\varphi_{j,\textbf{u}}))_{\Omega}}
  //                         _{(1)}
  //            \;
  //              \underbrace{- (\textrm{div}\; \varphi_{i,\textbf{u}},
  //                             \varphi_{j,p})_{\Omega}}
  //                         _{(2)}
  //            \;
  //              \underbrace{- (\varphi_{i,p},
  //                             \textrm{div}\;
  //                             \varphi_{j,\textbf{u}})_{\Omega}}
  //                         _{(3)}
  //  @f}
  //  和
  //  @f{align*}{
  //    M_{ij} &= \underbrace{(\varphi_{i,p},
  //                           \varphi_{j,p})_{\Omega}}
  //                         _{(4)},
  //  @f} ，
  //  其中 $\varphi_{i,\textbf{u}}$ 和 $\varphi_{i,p}$ 是 $i$ th形状函数的速度和压力成分。然后，上述各种术语在下面的实现中很容易识别。

            for (unsigned int i = 0; i < dofs_per_cell; ++i)
              {
                for (unsigned int j = 0; j <= i; ++j)
                  {
                    local_matrix(i, j) +=
                      (2 * (symgrad_phi_u[i] * symgrad_phi_u[j]) // (1)
                       - div_phi_u[i] * phi_p[j]                 // (2)
                       - phi_p[i] * div_phi_u[j])                // (3)
                      * fe_values.JxW(q);                        // * dx

                    local_preconditioner_matrix(i, j) +=
                      (phi_p[i] * phi_p[j]) // (4)
                      * fe_values.JxW(q);   // * dx
                  }

// 注意在上述（1）的实现中，`operator*`被重载用于对称张量，产生两个张量之间的标量乘积。            对于右手边，我们利用形状函数只在一个分量中不为零的事实（因为我们的元素是原始的）。 我们不是将代表形状函数i的dim+1值的张量与整个右手边的向量相乘，而是只看唯一的非零分量。函数 FiniteElement::system_to_component_index 将返回这个形状函数所处的分量（0=x速度，1=y速度，2=2d中的压力），我们用它来挑选出右手边向量的正确分量来相乘。

                const unsigned int component_i =
                  fe.system_to_component_index(i).first;
                local_rhs(i) += (fe_values.shape_value(i, q)   // (phi_u_i(x_q)
                                 * rhs_values[q](component_i)) // * f(x_q))
                                * fe_values.JxW(q);            // * dx
              }
          }

// 在我们将局部数据写入全局矩阵之前（同时使用AffineConstraints对象来应用Dirichlet边界条件并消除悬挂的节点约束，正如我们在介绍中讨论的那样），我们必须注意一件事。由于对称性，我们只建立了一半的局部矩阵，但我们要保存完整的矩阵，以便使用标准函数进行解算。这是通过翻转指数来实现的，以防我们指向本地矩阵的空部分。

        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          for (unsigned int j = i + 1; j < dofs_per_cell; ++j)
            {
              local_matrix(i, j) = local_matrix(j, i);
              local_preconditioner_matrix(i, j) =
                local_preconditioner_matrix(j, i);
            }

        cell->get_dof_indices(local_dof_indices);
        constraints.distribute_local_to_global(local_matrix,
                                               local_rhs,
                                               local_dof_indices,
                                               system_matrix,
                                               system_rhs);
        constraints.distribute_local_to_global(local_preconditioner_matrix,
                                               local_dof_indices,
                                               preconditioner_matrix);
      }

// 在我们要解决这个线性系统之前，我们为速度-速度矩阵生成一个预处理程序，即系统矩阵中的 block(0,0) 。如上所述，这取决于空间维度。由于 InnerPreconditioner::type 别名所描述的两个类具有相同的接口，因此无论我们想使用稀疏直接求解器还是ILU，都不需要做任何不同的事情。

    std::cout << "   Computing preconditioner..." << std::endl << std::flush;

    A_preconditioner =
      std::make_shared<typename InnerPreconditioner<dim>::type>();
    A_preconditioner->initialize(
      system_matrix.block(0, 0),
      typename InnerPreconditioner<dim>::type::AdditionalData());
  }

//  @sect4{StokesProblem::solve}

// 经过前面介绍中的讨论和各自类的定义， solve 函数的实现是相当直接的，其方式与 step-20 类似。首先，我们需要一个 InverseMatrix 类的对象，代表矩阵A的逆。正如在介绍中所描述的，在  InnerPreconditioner::type  类型的内部预处理器的帮助下，生成了逆。

  template <int dim>
  void StokesProblem<dim>::solve()
  {
    const InverseMatrix<SparseMatrix<double>,
                        typename InnerPreconditioner<dim>::type>
                   A_inverse(system_matrix.block(0, 0), *A_preconditioner);
    Vector<double> tmp(solution.block(0).size());

// 这与  step-20  中的情况一样。我们生成 Schur 补数的右手边  $B A^{-1} F - G$  和一个代表各自线性运算的对象  $B A^{-1} B^T$  ，现在有一个模板参数表示预处理器

// - 按照类的定义。

    {
      Vector<double> schur_rhs(solution.block(1).size());
      A_inverse.vmult(tmp, system_rhs.block(0));
      system_matrix.block(1, 0).vmult(schur_rhs, tmp);
      schur_rhs -= system_rhs.block(1);

      SchurComplement<typename InnerPreconditioner<dim>::type> schur_complement(
        system_matrix, A_inverse);

// 解算器调用的常规控制结构被创建...

      SolverControl            solver_control(solution.block(1).size(),
                                   1e-6 * schur_rhs.l2_norm());
      SolverCG<Vector<double>> cg(solver_control);

// 现在是对舒尔补码的预处理。正如介绍中所解释的，预处理是由压力变量的质量矩阵来完成的。

// 实际上，求解器需要有 $P^{-1}$ 形式的预处理，所以我们需要创建一个逆运算。我们再次使用一个 InverseMatrix 类的对象，它实现了求解器需要的 vmult 操作。 在这种情况下，我们必须对压力质量矩阵进行反转。正如在早期的教程程序中已经证明的那样，质量矩阵的反转是一个相当便宜和简单的操作（与拉普拉斯矩阵等相比）。带有ILU预处理的CG方法在5-10步内收敛，与网格大小无关。 这正是我们在这里所做的。我们选择另一个ILU预处理，并通过相应的模板参数将其带入InverseMatrix对象。 然后在逆矩阵的vmult操作中调用一个CG求解器。

// 另一种方法是选择因子为1.2的SSOR预处理器，这种方法构建成本较低，但之后需要更多的迭代。它需要大约两倍的迭代次数，但其生成的成本几乎可以忽略不计。

      SparseILU<double> preconditioner;
      preconditioner.initialize(preconditioner_matrix.block(1, 1),
                                SparseILU<double>::AdditionalData());

      InverseMatrix<SparseMatrix<double>, SparseILU<double>> m_inverse(
        preconditioner_matrix.block(1, 1), preconditioner);

// 有了舒尔补码和高效的预处理程序，我们可以用通常的方法解决压力的相关方程（即解向量中的0块）。

      cg.solve(schur_complement, solution.block(1), schur_rhs, m_inverse);

// 在这第一个求解步骤之后，必须将悬挂的节点约束分布到求解中，以实现一致的压力场。

      constraints.distribute(solution);

      std::cout << "  " << solver_control.last_step()
                << " outer CG Schur complement iterations for pressure"
                << std::endl;
    }

// 和 step-20 一样，我们最后需要解速度方程，在这里我们插入压力方程的解。这只涉及我们已经知道的对象

// 所以我们只需用 $p$ 乘以 $B^T$ ，减去右边的部分，再乘以 $A$ 的逆数。最后，我们需要分配悬挂节点的约束，以获得一个一致的流场。

    {
      system_matrix.block(0, 1).vmult(tmp, solution.block(1));
      tmp *= -1;
      tmp += system_rhs.block(0);

      A_inverse.vmult(solution.block(0), tmp);

      constraints.distribute(solution);
    }
  }
// @sect4{StokesProblem::output_results}

// 下一个函数生成图形输出。在这个例子中，我们将使用VTK文件格式。 我们给问题中的各个变量附上名字： velocity to the dim 速度的组成部分和 pressure 压力的组成部分。

// 并非所有的可视化程序都有能力将各个矢量分量组合成一个矢量来提供矢量图；特别是对于一些基于VTK的可视化程序来说，这一点是成立的。在这种情况下，在包含数据的文件中应该已经描述了组件的逻辑分组为矢量的情况。换句话说，我们需要做的是为我们的输出编写者提供一种方法，让他们知道有限元的哪些分量在逻辑上形成一个矢量（在 $d$ 空间维度上有 $d$ 分量），而不是让他们假设我们只是有一堆标量场。 这是用 DataComponentInterpretation 命名空间的成员实现的：和文件名一样，我们创建一个矢量，其中第一个 dim 分量指的是速度，并被赋予 DataComponentInterpretation::component_is_part_of_vector; 标签，我们最后推一个标签 DataComponentInterpretation::component_is_scalar 来描述压力变量的分组。

// 然后函数的其余部分与  step-20  中的相同。

  template <int dim>
  void
  StokesProblem<dim>::output_results(const unsigned int refinement_cycle) const
  {
    std::vector<std::string> solution_names(dim, "velocity");
    solution_names.emplace_back("pressure");

    std::vector<DataComponentInterpretation::DataComponentInterpretation>
      data_component_interpretation(
        dim, DataComponentInterpretation::component_is_part_of_vector);
    data_component_interpretation.push_back(
      DataComponentInterpretation::component_is_scalar);

    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution,
                             solution_names,
                             DataOut<dim>::type_dof_data,
                             data_component_interpretation);
    data_out.build_patches();

    std::ofstream output(
      "solution-" + Utilities::int_to_string(refinement_cycle, 2) + ".vtk");
    data_out.write_vtk(output);
  }
// @sect4{StokesProblem::refine_mesh}

// 这是 StokesProblem 类中最后一个有趣的函数。 正如它的名字所示，它获取问题的解决方案，并在需要时细化网格。其过程与 step-6 中的相应步骤相同，不同的是我们只根据压力的变化进行细化，也就是说，我们用ComponentMask类型的掩码对象调用Kelly误差估计器，选择我们感兴趣的压力的单一标量分量（我们通过指定我们想要的分量从有限元类中得到这样一个掩码）。此外，我们没有再次粗化网格。

  template <int dim>
  void StokesProblem<dim>::refine_mesh()
  {
    Vector<float> estimated_error_per_cell(triangulation.n_active_cells());

    FEValuesExtractors::Scalar pressure(dim);
    KellyErrorEstimator<dim>::estimate(
      dof_handler,
      QGauss<dim - 1>(degree + 1),
      std::map<types::boundary_id, const Function<dim> *>(),
      solution,
      estimated_error_per_cell,
      fe.component_mask(pressure));

    GridRefinement::refine_and_coarsen_fixed_number(triangulation,
                                                    estimated_error_per_cell,
                                                    0.3,
                                                    0.0);
    triangulation.execute_coarsening_and_refinement();
  }
// @sect4{StokesProblem::run}

// 在斯托克斯类中的最后一步，像往常一样，是生成初始网格的函数，并按各自的顺序调用其他函数。

// 我们从一个大小为 $4 \times 1$ （2D）或 $4 \times 1 \times 1$ （3D）的矩形开始，在 $R^2/R^3$ 中分别放置为 $(-2,2)\times(-1,0)$ 或 $(-2,2)\times(0,1)\times(-1,0)$  。在每个方向上以相等的网格大小开始是很自然的，所以我们在第一个坐标方向上将初始矩形细分四次。为了将创建网格所涉及的变量的范围限制在我们实际需要的范围内，我们将整个块放在一对大括号之间。

  template <int dim>
  void StokesProblem<dim>::run()
  {
    {
      std::vector<unsigned int> subdivisions(dim, 1);
      subdivisions[0] = 4;

      const Point<dim> bottom_left = (dim == 2 ?                //
                                        Point<dim>(-2, -1) :    // 2d case
                                        Point<dim>(-2, 0, -1)); // 3d case

      const Point<dim> top_right = (dim == 2 ?              //
                                      Point<dim>(2, 0) :    // 2d case
                                      Point<dim>(2, 1, 0)); // 3d case

      GridGenerator::subdivided_hyper_rectangle(triangulation,
                                                subdivisions,
                                                bottom_left,
                                                top_right);
    }

// 边界指标1被设置为所有受Dirichlet边界条件约束的边界，即位于最后一个坐标方向上的0的面。详见上面的例子描述。

    for (const auto &cell : triangulation.active_cell_iterators())
      for (const auto &face : cell->face_iterators())
        if (face->center()[dim - 1] == 0)
          face->set_all_boundary_ids(1);

// 然后，在第一次求解之前，我们应用一个初始细化。在3D中，会有更多的自由度，所以我们在那里细化得更少。

    triangulation.refine_global(4 - dim);

// 正如在 step-6 中第一次看到的那样，我们在不同的细化级别上循环细化（除了第一个循环），设置自由度和矩阵，组装，求解和创建输出。

    for (unsigned int refinement_cycle = 0; refinement_cycle < 6;
         ++refinement_cycle)
      {
        std::cout << "Refinement cycle " << refinement_cycle << std::endl;

        if (refinement_cycle > 0)
          refine_mesh();

        setup_dofs();

        std::cout << "   Assembling..." << std::endl << std::flush;
        assemble_system();

        std::cout << "   Solving..." << std::flush;
        solve();

        output_results(refinement_cycle);

        std::cout << std::endl;
      }
  }
} // namespace Step22
// @sect3{The main function}

// 主函数与  step-20  中的相同。我们将元素度数作为参数传递，并在众所周知的模板槽中选择空间尺寸。

int main()
{
  try
    {
      using namespace Step22;

      StokesProblem<2> flow_problem(1);
      flow_problem.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}