/**
 * @file MPCExample.cpp
 * @author Giulio Romualdi
 * @copyright Released under the terms of the BSD 3-Clause License
 * @date 2018
 */

// osqp-eigen
#include "OsqpEigen/OsqpEigen.h"

// eigen
#include <Eigen/Dense>

#include <iostream>

void setDynamicsMatrices(Eigen::Matrix<double, 2, 2> &a, Eigen::Matrix<double, 2, 1> &b)
{
    a << 1.,      0.020,
        0.,      0.9661;

    b << 0.,
        0.0315;
}

void setInequalityConstraints(Eigen::Matrix<double, 2, 1> &xMax, Eigen::Matrix<double, 2, 1> &xMin,
                              Eigen::Matrix<double, 1, 1> &uMax, Eigen::Matrix<double, 1, 1> &uMin)
{
    double u0 = 0.0;

    // input inequality constraints
    uMin << -6.0 - u0;

    uMax << 10.0 - u0;

    // state inequality constraints
    // TODO : change to present pos +/- ranges
    xMin << -100, -6.0;

    xMax << 100, 10.0;
}

void setWeightMatrices(Eigen::DiagonalMatrix<double, 2> &Q, Eigen::DiagonalMatrix<double, 1> &R)
{
    Q.diagonal() << 2, 0;
    R.diagonal() << 0.2;
}

void castMPCToQPHessian(const Eigen::DiagonalMatrix<double, 2> &Q, const Eigen::DiagonalMatrix<double, 1> &R, int mpcWindow,
                        Eigen::SparseMatrix<double> &hessianMatrix)
{

    hessianMatrix.resize(2*(mpcWindow+1) + 1 * mpcWindow, 2*(mpcWindow+1) + 1 * mpcWindow);

    //populate hessian matrix
    for(int i = 0; i<2*(mpcWindow+1) + 1 * mpcWindow; i++){
        if(i < 2*(mpcWindow+1)){
            int posQ=i%2;
            float value = Q.diagonal()[posQ];
            if(value != 0)
                hessianMatrix.insert(i,i) = value;
        }
        else{
            int posR=i%1;
            float value = R.diagonal()[posR];
            if(value != 0)
                hessianMatrix.insert(i,i) = value;
        }
    }
}

void castMPCToQPGradient(const Eigen::DiagonalMatrix<double, 2> &Q, const Eigen::Matrix<double, 2, 1> &xRef, int mpcWindow,
                         Eigen::VectorXd &gradient)
{

    Eigen::Matrix<double,2,1> Qx_ref;
    Qx_ref = Q * (-xRef);

    // populate the gradient vector
    gradient = Eigen::VectorXd::Zero(2*(mpcWindow+1) +  1*mpcWindow, 1);
    for(int i = 0; i<2*(mpcWindow+1); i++){
        int posQ=i%2;
        float value = Qx_ref(posQ,0);
        gradient(i,0) = value;
    }
}

void castMPCToQPConstraintMatrix(const Eigen::Matrix<double, 2, 2> &dynamicMatrix, const Eigen::Matrix<double, 2, 1> &controlMatrix,
                                 int mpcWindow, Eigen::SparseMatrix<double> &constraintMatrix)
{
    constraintMatrix.resize(2*(mpcWindow+1)  + 2*(mpcWindow+1) + 1 * mpcWindow, 2*(mpcWindow+1) + 1 * mpcWindow);

    // populate linear constraint matrix
    for(int i = 0; i<2*(mpcWindow+1); i++){
        constraintMatrix.insert(i,i) = -1;
    }

    for(int i = 0; i < mpcWindow; i++)
        for(int j = 0; j<2; j++)
            for(int k = 0; k<2; k++){
                float value = dynamicMatrix(j,k);
                if(value != 0){
                    constraintMatrix.insert(2 * (i+1) + j, 2 * i + k) = value;
                }
            }

    for(int i = 0; i < mpcWindow; i++)
        for(int j = 0; j < 2; j++)
            for(int k = 0; k < 1; k++){
                float value = controlMatrix(j,k);
                if(value != 0){
                    constraintMatrix.insert(2*(i+1)+j, 1*i+k+2*(mpcWindow + 1)) = value;
                }
            }

    for(int i = 0; i<2*(mpcWindow+1) + 1*mpcWindow; i++){
        constraintMatrix.insert(i+(mpcWindow+1)*2,i) = 1;
    }
}

void castMPCToQPConstraintVectors(const Eigen::Matrix<double, 2, 1> &xMax, const Eigen::Matrix<double, 2, 1> &xMin,
                                   const Eigen::Matrix<double, 1, 1> &uMax, const Eigen::Matrix<double, 1, 1> &uMin,
                                   const Eigen::Matrix<double, 2, 1> &x0,
                                   int mpcWindow, Eigen::VectorXd &lowerBound, Eigen::VectorXd &upperBound)
{
    // evaluate the lower and the upper inequality vectors
    Eigen::VectorXd lowerInequality = Eigen::MatrixXd::Zero(2*(mpcWindow+1) +  1 * mpcWindow, 1);
    Eigen::VectorXd upperInequality = Eigen::MatrixXd::Zero(2*(mpcWindow+1) +  1 * mpcWindow, 1);
    for(int i=0; i<mpcWindow+1; i++){
        lowerInequality.block(2*i,0,2,1) = xMin;
        upperInequality.block(2*i,0,2,1) = xMax;
    }
    for(int i=0; i<mpcWindow; i++){
        lowerInequality.block(1 * i + 2 * (mpcWindow + 1), 0, 1, 1) = uMin;
        upperInequality.block(1 * i + 2 * (mpcWindow + 1), 0, 1, 1) = uMax;
    }

    // evaluate the lower and the upper equality vectors
    Eigen::VectorXd lowerEquality = Eigen::MatrixXd::Zero(2*(mpcWindow+1),1 );
    Eigen::VectorXd upperEquality;
    lowerEquality.block(0,0,2,1) = -x0;
    upperEquality = lowerEquality;
    lowerEquality = lowerEquality;

    // merge inequality and equality vectors
    lowerBound = Eigen::MatrixXd::Zero(2*2*(mpcWindow+1) +  1*mpcWindow,1 );
    lowerBound << lowerEquality,
        lowerInequality;

    upperBound = Eigen::MatrixXd::Zero(2*2*(mpcWindow+1) +  1*mpcWindow,1 );
    upperBound << upperEquality,
        upperInequality;
}

void updateConstraintVectors(const Eigen::Matrix<double, 2, 1> &x0,
                             Eigen::VectorXd &lowerBound, Eigen::VectorXd &upperBound)
{
    lowerBound.block(0,0,2,1) = -x0;
    upperBound.block(0,0,2,1) = -x0;
}

double getErrorNorm(const Eigen::Matrix<double, 2, 1> &x,
                    const Eigen::Matrix<double, 2, 1> &xRef)
{
    // evaluate the error
    Eigen::Matrix<double, 2, 1> error = x - xRef;

    // return the norm
    return error.norm();
}

int main()
{
    // set the preview window
    int mpcWindow = 200;

    // allocate the dynamics matrices
    Eigen::Matrix<double, 2, 2> a;
    Eigen::Matrix<double, 2, 1> b;

    // allocate the constraints vector
    Eigen::Matrix<double, 2, 1> xMax;
    Eigen::Matrix<double, 2, 1> xMin;
    Eigen::Matrix<double, 1, 1> uMax;
    Eigen::Matrix<double, 1, 1> uMin;

    // allocate the weight matrices
    Eigen::DiagonalMatrix<double, 2> Q;
    Eigen::DiagonalMatrix<double, 1> R;

    // allocate the initial and the reference state space
    Eigen::Matrix<double, 2, 1> x0;
    Eigen::Matrix<double, 2, 1> xRef;

    // allocate QP problem matrices and vectores
    Eigen::SparseMatrix<double> hessian;
    Eigen::VectorXd gradient;
    Eigen::SparseMatrix<double> linearMatrix;
    Eigen::VectorXd lowerBound;
    Eigen::VectorXd upperBound;

    // set the initial and the desired states
    x0 << 0, 0 ;
    xRef <<  1, 0;

    // set MPC problem quantities
    setDynamicsMatrices(a, b);
    setInequalityConstraints(xMax, xMin, uMax, uMin);
    setWeightMatrices(Q, R);

    // cast the MPC problem as QP problem
    castMPCToQPHessian(Q, R, mpcWindow, hessian);
    castMPCToQPGradient(Q, xRef, mpcWindow, gradient);
    castMPCToQPConstraintMatrix(a, b, mpcWindow, linearMatrix);
    castMPCToQPConstraintVectors(xMax, xMin, uMax, uMin, x0, mpcWindow, lowerBound, upperBound);

    // instantiate the solver
    OsqpEigen::Solver solver;

    // settings
    //solver.settings()->setVerbosity(false);
    solver.settings()->setWarmStart(true);

    // set the initial data of the QP solver
    solver.data()->setNumberOfVariables(2 * (mpcWindow + 1) + 1 * mpcWindow);
    solver.data()->setNumberOfConstraints(2 * 2 * (mpcWindow + 1) + 1 * mpcWindow);
    if(!solver.data()->setHessianMatrix(hessian)) return 1;
    if(!solver.data()->setGradient(gradient)) return 1;
    if(!solver.data()->setLinearConstraintsMatrix(linearMatrix)) return 1;
    if(!solver.data()->setLowerBound(lowerBound)) return 1;
    if(!solver.data()->setUpperBound(upperBound)) return 1;

    // instantiate the solver
    if(!solver.initSolver()) return 1;

    // controller input and QPSolution vector
    Eigen::VectorXd ctr;
    Eigen::VectorXd QPSolution;

    // number of iteration steps
    int numberOfSteps = 200;

    for (int i = 0; i < numberOfSteps; i++){

        // solve the QP problem
        if(solver.solveProblem() != OsqpEigen::ErrorExitFlag::NoError) return 1;

        // get the controller input
        QPSolution = solver.getSolution();
        ctr = QPSolution.block(2 * (mpcWindow + 1), 0, 1, 1);

        // save data into file
        auto x0Data = x0.data();

        // propagate the model
        x0 = a * x0 + b * ctr;

        // update the constraint bound
        updateConstraintVectors(x0, lowerBound, upperBound);
        if(!solver.updateBounds(lowerBound, upperBound)) return 1;
      }
    return 0;
}


// blas.cpp : This file contains the 'main' function. Program execution begins and ends there.
//

#include <iostream>

/*
#ifdef _WIN32
#undef __STRICT_ANSI__
#endif
*/

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

const boost::numeric::ublas::matrix<double> CreateMatrix(
    const std::size_t n_rows,
    const std::size_t n_cols,
    const std::vector<double>& v)
{
    assert(n_rows * n_cols == v.size());
    boost::numeric::ublas::matrix<double> m(n_rows, n_cols);
    for (std::size_t row = 0; row != n_rows; ++row)
    {
        for (std::size_t col = 0; col != n_cols; ++col)
        {
            m(row, col) = v[(col * n_rows) + row];
        }
    }
    return m;
}

//Chop returns a std::vector of sub-matrices
//[ A at [0]   B at [1] ]
//[ C at [2]   D at [4] ]
const std::vector<boost::numeric::ublas::matrix<double> > Chop(
    const boost::numeric::ublas::matrix<double>& m)
{
    //using boost::numeric::ublas::range;
    //using boost::numeric::ublas::matrix;
    //using boost::numeric::ublas::matrix_range;
    std::vector<boost::numeric::ublas::matrix<double> > v;
    v.reserve(4);
    const int midy = m.size1() / 2;
    const int midx = m.size2() / 2;
    const boost::numeric::ublas::matrix_range<const boost::numeric::ublas::matrix<double> > top_left(m, boost::numeric::ublas::range(0, midy), boost::numeric::ublas::range(0, midx));
    const boost::numeric::ublas::matrix_range<const boost::numeric::ublas::matrix<double> > bottom_left(m, boost::numeric::ublas::range(midy, m.size1()), boost::numeric::ublas::range(0, midx));
    const boost::numeric::ublas::matrix_range<const boost::numeric::ublas::matrix<double> > top_right(m, boost::numeric::ublas::range(0, midy), boost::numeric::ublas::range(midx, m.size2()));
    const boost::numeric::ublas::matrix_range<const boost::numeric::ublas::matrix<double> > bottom_right(m, boost::numeric::ublas::range(midy, m.size1()), boost::numeric::ublas::range(midx, m.size2()));
    v.push_back(boost::numeric::ublas::matrix<double>(top_left));
    v.push_back(boost::numeric::ublas::matrix<double>(top_right));
    v.push_back(boost::numeric::ublas::matrix<double>(bottom_left));
    v.push_back(boost::numeric::ublas::matrix<double>(bottom_right));
    return v;
}

bool IsAboutEqual(const double x, const double y) { return std::abs(x - y) < 0.00001; }

int main()
{
    //using boost::numeric::ublas::matrix;
    //using boost::numeric::ublas::prod;
    //using boost::numeric::ublas::vector;
    {
        //                     [ 1.0 ] | [ 2.0   3.0 ]
        // [ 1.0 2.0 3.0 ]     --------+--------------
        // [ 4.0 5.0 6.0 ]     [ 4.0 ] | [ 5.0   6.0 ]
        // [ 7.0 8.0 9.0 ] ->  [ 7.0 ] | [ 8.0   9.0 ]
        const boost::numeric::ublas::matrix<double> m = CreateMatrix(3, 3, { 1.0,4.0,7.0,2.0,5.0,8.0,3.0,6.0,9.0 });
        assert(m(0, 0) == 1.0); assert(m(0, 1) == 2.0); assert(m(0, 2) == 3.0);
        assert(m(1, 0) == 4.0); assert(m(1, 1) == 5.0); assert(m(1, 2) == 6.0);
        assert(m(2, 0) == 7.0); assert(m(2, 1) == 8.0); assert(m(2, 2) == 9.0);
        const std::vector<boost::numeric::ublas::matrix<double> > n = Chop(m);
        assert(n.size() == 4);
        std::wclog
            << L"m   : " << m << L'\n'
            << L"n[0]: " << n[0] << L'\n'
            << L"n[1]: " << n[1] << L'\n'
            << L"n[2]: " << n[2] << L'\n'
            << L"n[3]: " << n[3] << L'\n';
        assert(n[0].size1() == 1);
        assert(n[0].size2() == 1);
        assert(n[1].size1() == 1);
        assert(n[1].size2() == 2);
        assert(n[2].size1() == 2);
        assert(n[2].size2() == 1);
        assert(n[3].size1() == 2);
        assert(n[3].size2() == 2);
        assert(n[0].size1() + n[2].size1() == m.size1());
        assert(n[1].size1() + n[3].size1() == m.size1());
        assert(n[0].size2() + n[1].size2() == m.size2());
        assert(n[2].size2() + n[3].size2() == m.size2());
    }
    {
        const boost::numeric::ublas::matrix<double> m = CreateMatrix(5, 5,
            {
              1.0, 6.0,11.0,16.0,21.0,
              2.0, 7.0,12.0,17.0,22.0,
              3.0, 8.0,13.0,18.0,23.0,
              4.0, 9.0,14.0,19.0,24.0,
              5.0,10.0,15.0,20.0,25.0
            }
        );
        assert(m(0, 0) == 1.0); assert(m(0, 1) == 2.0); assert(m(0, 2) == 3.0); assert(m(0, 3) == 4.0); assert(m(0, 4) == 5.0);
        assert(m(1, 0) == 6.0); assert(m(1, 1) == 7.0); assert(m(1, 2) == 8.0); assert(m(1, 3) == 9.0); assert(m(1, 4) == 10.0);
        assert(m(2, 0) == 11.0); assert(m(2, 1) == 12.0); assert(m(2, 2) == 13.0); assert(m(2, 3) == 14.0); assert(m(2, 4) == 15.0);
        assert(m(3, 0) == 16.0); assert(m(3, 1) == 17.0); assert(m(3, 2) == 18.0); assert(m(3, 3) == 19.0); assert(m(3, 4) == 20.0);
        assert(m(4, 0) == 21.0); assert(m(4, 1) == 22.0); assert(m(4, 2) == 23.0); assert(m(4, 3) == 24.0); assert(m(4, 4) == 25.0);
        const std::vector<boost::numeric::ublas::matrix<double> > n = Chop(m);
        assert(n.size() == 4);
        std::wclog
            << L"m   : " << m << L'\n'
            << L"n[0]: " << n[0] << L'\n'
            << L"n[1]: " << n[1] << L'\n'
            << L"n[2]: " << n[2] << L'\n'
            << L"n[3]: " << n[3] << L'\n';
        assert(n[0].size1() == 2);
        assert(n[0].size2() == 2);
        assert(n[1].size1() == 2);
        assert(n[1].size2() == 3);
        assert(n[2].size1() == 3);
        assert(n[2].size2() == 2);
        assert(n[3].size1() == 3);
        assert(n[3].size2() == 3);
        assert(n[0].size1() + n[2].size1() == m.size1());
        assert(n[1].size1() + n[3].size1() == m.size1());
        assert(n[0].size2() + n[1].size2() == m.size2());
        assert(n[2].size2() + n[3].size2() == m.size2());
    }
}


/*
*  Copyright August 2015
*  Author: Olalekan P. Ogunmolu
*
* 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.
*
*/

// Include Files
#include "savgol.h"

#include <Eigen/Core>

#include <iostream>

using namespace Eigen;
using namespace std;

void help()
{
  std::cout << "================================================== \n"
            << "USAGE:                                             \n"
            << "\n"
            << "./savgol [<frame_size> [<polynomial_order> [<x_low> <x_high>] ] ] \n"
            << "\n"
            << "       <frame_size>: odd int and ideally greater than\n"
            << "       <polynomial_order>:  an integer             \n"
            << "\n"
            << "       <x_low>, <x_high>: min and max limits of    \n"
            << "       linspaced vector to be filtered.            \n"
            << "===================================================\n"
            << "       Example: ./savgol 9 5   \n\n";;
}

int main (int argc, char** argv)
{
  int F;      //Frame Size
  int k;      //Example Polynomial Order
  double Fd ;
  float x_min, x_max;

  if(argc>1)
  {
    if(argv[1] == "-h" || "-help")
    {
      help();
      return EXIT_SUCCESS;
    }
    else
    {
      for(auto i = 1; i < argc; ++i )
      {
        F = atoi(argv[1]);
        k = atoi(argv[2]);
        x_min = atoi(argv[3]);
        x_max = atoi(argv[4]);
      }
    }
  }
  else  //use default values
  {
    F = 5; k = 3;
    x_min = 900.0; x_max = 980.0;
  }

  auto s = vander(F);        //Compute vandermonde matrix

  cout << "Frame size: " << F << "; \tPolynomial order: " << k << endl;
  cout << "\n Vandermonde Matrix: \n" << s  << endl;

  k = atoi(argv[2]) or 3;

  auto B = sgdiff(k, F, Fd);

  //To express as a real filtering operation, we shift x around the nth time instant
  auto x = VectorXf::LinSpaced(F, x_min, x_max);

  auto Filter = savgolfilt(x, k, F);

  cout <<"\n\nFiltered values in the range \n" << x.transpose().eval() <<"\n are: \n" << Filter << endl;

  return 0;
}

/* Compile:
cd ../; rm build -rf; mkdir build; cd build; cmake ../; make; ./savgol
*/


/*
//@HEADER
// ************************************************************************
//
// tutorial1.cc
//                     		  Pressio
//                             Copyright 2019
//    National Technology & Engineering Solutions of Sandia, LLC (NTESS)
//
// Under the terms of Contract DE-NA0003525 with NTESS, the
// U.S. Government retains certain rights in this software.
//
// Pressio is licensed under BSD-3-Clause terms of use:
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
// IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Questions? Contact Francesco Rizzi (fnrizzi@sandia.gov)
//
// ************************************************************************
//@HEADER
*/

#include <array>
#include <Eigen/Core>

#include "pressio/type_traits.hpp"
#include "pressio/ops.hpp"

struct MyCustomVector
{
  MyCustomVector(std::size_t ext) : d_(ext){}

  double & operator()(int i){ return d_[i]; }
  const double & operator()(int i)const { return d_[i]; }

  std::size_t extent(int k)const { return (k==0) ? d_.size() : 0; }

  void fill(double value){
    std::for_each(d_.begin(), d_.end(), [](double & v){ v= 0.; });
  }

private:
  std::vector<double> d_ = {};
};

struct MyCustomMatrix
{
  MyCustomMatrix(std::size_t nr, std::size_t nc)
    : num_rows_(nr), num_cols_(nc), d_(nr*nc){}

  std::size_t extent(int k)const { return (k==0) ? num_rows_ : num_cols_; }

  double & operator()(int i, int j){ return d_[num_cols_*i+j]; }
  const double & operator()(int i, int j) const { return d_[num_cols_*i+j]; }

  void fill(double value){
    std::for_each(d_.begin(), d_.end(), [=](double & v){ v=value; });
  }

private:
  std::size_t num_rows_ = {};
  std::size_t num_cols_ = {};
  std::vector<double> d_ = {};
};

struct MyRosenbrockSystem
{
  using scalar_type   = double;
  using state_type    = Eigen::VectorXd;
  using residual_type = MyCustomVector;
  using jacobian_type = MyCustomMatrix;

  residual_type createResidual() const{ return residual_type(6);   }
  jacobian_type createJacobian() const{ return jacobian_type(6, 4);}

  void residual(const state_type& x, residual_type & res) const
  {
    const auto & x1 = x(0);
    const auto & x2 = x(1);
    const auto & x3 = x(2);
    const auto & x4 = x(3);
    res(0) = 10.*(x4 - x3*x3);
    res(1) = 10.*(x3 - x2*x2);
    res(2) = 10.*(x2 - x1*x1);
    res(3) = (1.-x1);
    res(4) = (1.-x2);
    res(5) = (1.-x3);
  }

  void jacobian(const state_type & x, jacobian_type & JJ) const
  {
    const auto & x1 = x(0);
    const auto & x2 = x(1);
    const auto & x3 = x(2);
    JJ.fill(0.);

    JJ(0,2) = -20.*x3;
    JJ(0,3) = 10.;
    JJ(1,1) = -20.*x2;
    JJ(1,2) = 10.;
    JJ(2,0) = -20.*x1;
    JJ(2,1) = 10.;
    JJ(3,0) = -1.;
    JJ(4,1) = -1.;
    JJ(5,2) = -1.;
  }
};

namespace pressio{
template<> struct Traits<MyCustomVector>{
  using scalar_type = double;
};

template<> struct Traits<MyCustomMatrix>{
  using scalar_type = double;
};

namespace ops{
MyCustomVector clone(const MyCustomVector & src){ return src; }
MyCustomMatrix clone(const MyCustomMatrix & src){ return src; }

void set_zero(MyCustomVector & o){ o.fill(0); }
void set_zero(MyCustomMatrix & o){ o.fill(0); }

double norm2(const MyCustomVector & v){
  double norm{0};
  for (std::size_t i=0; i<v.extent(0); ++i){
    norm += v(i)*v(i);
  }
  return std::sqrt(norm);
}

template<class HessianType>
void product(pressio::transpose, pressio::nontranspose,
	     const double alpha,
	     const MyCustomMatrix & A,
	     const double beta,
	     HessianType & H)
{
  for (std::size_t i=0; i<A.extent(1); ++i){
    for (std::size_t j=0; j<A.extent(1); ++j)
    {
      H(i,j) *= beta;
      for (std::size_t k=0; k<A.extent(0); ++k){
	H(i,j) += alpha * A(k,i) * A(k,j);
      }
    }
  }
}

template<class GradientType>
void product(pressio::transpose,
	     const double alpha,
	     const MyCustomMatrix & A,
	     const MyCustomVector & b,
	     const double beta,
	     GradientType & g)
{
  for (int i=0; i<g.rows(); ++i){
    g(i) *= beta;
    for (std::size_t k=0; k<A.extent(0); ++k){
      g(i) += alpha * A(k,i) * b(k);
    }
  }
}

template<class HessianType>
HessianType product(pressio::transpose, pressio::nontranspose,
	     double alpha,
	     const MyCustomMatrix & A)
{
  HessianType H(A.extent(1), A.extent(1));
  product(pressio::transpose(), pressio::nontranspose(), alpha, A, 0, H);
  return H;
}

void update(MyCustomVector & v, double a, const MyCustomVector & v1, double b)
{
  for (std::size_t i=0; i<v.extent(0); ++i){
    v(i) = v(i)*a + b*v1(i);
  }
}

void scale(MyCustomVector & v, double factor){
  for (std::size_t i=0; i<v.extent(0); ++i){
    v(i) = v(i)*factor;
  }
}
}}//end namespace pressio::ops

#include "pressio/solvers_linear.hpp"
#include "pressio/solvers_nonlinear.hpp"

int main()
{
  namespace plog   = pressio::log;
  namespace pls    = pressio::linearsolvers;
  namespace pnonls = pressio::nonlinearsolvers;
  plog::initialize(pressio::logto::terminal);
  plog::setVerbosity({plog::level::info});

  using problem_t = MyRosenbrockSystem;
  problem_t problem;

  using state_t   = Eigen::VectorXd;
  state_t x(4);
  x[0] = -0.05; x[1] = 1.1; x[2] = 1.2; x[3] = 1.5;

  using hessian_t    = Eigen::MatrixXd;
  using lin_tag      = pls::direct::HouseholderQR;
  using lin_solver_t = pls::Solver<lin_tag, hessian_t>;
  lin_solver_t linSolver;

  auto gnSolver = pnonls::create_gauss_newton(problem, x, linSolver);
  gnSolver.setTolerance(1e-5);
  gnSolver.solve(problem, x);
  std::cout << std::setprecision(14) << x << std::endl;
  // check solution
  std::cout << "Computed solution: \n "
            << "[" << x(0) << " " << x(1) << " " << x(2) << " " << x(3) << " " << "] \n"
            << "Expected solution: \n "
            << "[1.0000000156741, 0.99999999912477, 0.99999999651993, 0.99999998889888]"
            << std::endl;

  std::vector<double> gold = {
    1.00000001567414e+00,
    9.99999999124769e-01,
    9.99999996519930e-01,
    9.99999988898883e-01};

  std::string sentinel = "PASSED";
  const auto e1 = std::abs(x(0) - gold[0]);
  const auto e2 = std::abs(x(1) - gold[1]);
  const auto e3 = std::abs(x(2) - gold[2]);
  const auto e4 = std::abs(x(3) - gold[3]);
  if (e1>1e-6 or e2>1e-6  or e3>1e-6 or e4>1e-6){
    sentinel = "FAILED";
  }
  std::cout << sentinel << std::endl;

  plog::finalize();
  return 0;
}


#pragma once
#ifndef ENHANCED_MADGWICK_HPP
#define ENHANCED_MADGWICK_HPP

#include <Eigen/Geometry>

/** This enhanced version of the Madgwick filter is a combination of ideas taken from Madgwick et al [2]
 *	and Admiraal et al [1]. Admiraal improves upon the original Madgwick filter by deriving a steepest
 *	(as apposed to gradient) descent formulation for calculating the quaternion estimate update
 *	direction. This eliminates some of the numerical errors in Madgwick's version while also improving speed
 *	and precision.
 *
 *	The notation used for documentation is as follows: [x](y)
 *	Where 'x' is the source and 'y' is the equation number
 *
 *	[1] 	M. Admiraal, S. Wilson and R. Vaidyanathan, "Improved Formulation of the IMU and MARG
 *			Orientation Gradient Descent Algorithm for Motion Tracking in Human-Machine Interfaces," in
 *			International Conference on Multisensor Fusion and Integration for Intelligent Systems, Daegu,
 *			2017.
 *
 *	[2] 	S. O. Madgwick, A. J. Harrison and R. Vaidyanathan, "Estimation of IMU and MARG Orientation
 *			Using A Gradient Descent Algorithm," in International Conference on Rehabilitation Robotics,
 *			Zurich, 2011.
 *
 **/
template<typename T>
class EnhancedMadgwick
{
public:

	EnhancedMadgwick()
	{
		qAccel.normalize();
		qMag.normalize();
	}
	~EnhancedMadgwick() = default;

private:
	Eigen::Quaternion<T> qAccel;
	Eigen::Quaternion<T> qMag;

	/** Calculates the gradient of the error function when referenced to gravity [1](35). This is then
	 *	used in the gradient descent algorithm to take a single step. This calculation assumes the
	 *	acceleration reference vector is [0, 0, -1] because, ya know, gravity.
	 *
	 *	@param[in]	estAccelQuaternion	The current quaternion estimate of orientation from accelerometer data
	 *	@param[in]	measuredAccelVector	A column matrix [x,y,z]' with measured accelerometer data
	 *	@return A quaternion representing the estimated orientation gradient
	 **/
	Eigen::Quaternion<T> accelGradient(Eigen::Quaternion<T>& estAccelQuaternion, Eigen::Matrix<T, 3, 1> measuredAccelVector)
	{
		Eigen::Matrix<T, 4, 1> result;
		Eigen::Matrix<T, 4, 3> leftProduct;
		Eigen::Matrix<T, 3, 1> rightProduct;

		T qw = estAccelQuaternion.w();
		T qx = estAccelQuaternion.x();
		T qy = estAccelQuaternion.y();
		T qz = estAccelQuaternion.z();
		T vmx = measuredAccelVector[0];
		T vmy = measuredAccelVector[1];
		T vmz = measuredAccelVector[2];

		leftProduct << 2 * (
			 qy, -qx, -qw,
		    -qz, -qw,  qx,
			 qw, -qz,  qy,
			-qx, -qy, -qz
			);

		rightProduct <<
			 2 * qw*qy, -2 * qx*qz, -vmx,
			-2 * qw*qx - 2 * qy*qz - vmy,
			-qw * qw + qx * qx + qy * qy - qz * qz - vmx;

		result = leftProduct * rightProduct;
		return Eigen::Quaternion<T>(result[0], result[1], result[2], result[3]);
	}

	/** Calculates the gradient of the error function when referenced to the earth's magnetic field. This
	 *	is under the assumption that the field is perfectly planar in local space and can be represented
	 *	by the vector [Vrx, 0, Vrz]. [1](36)
	 *
	 **/
	Eigen::Quaternion<T> magGradient(Eigen::Quaternion<T>& estMagQuaternion, Eigen::Matrix<T, 3, 1> measuredMagVector, Eigen::Matrix<T, 3, 1> refMagVector)
	{
		Eigen::Matrix<T, 4, 1> result;
		Eigen::Matrix<T, 4, 3> leftProduct;
		Eigen::Matrix<T, 3, 1> rightProduct;

		T qw = estMagQuaternion.w();
		T qx = estMagQuaternion.x();
		T qy = estMagQuaternion.y();
		T qz = estMagQuaternion.z();
		T vmx = measuredMagVector[0];
		T vmy = measuredMagVector[1];
		T vmz = measuredMagVector[2];
		T vrx = refMagVector[0];
		T vry = 0;
		T vrz = refMagVector[2];

		leftProduct << 2 * (
			( vrx*qw - vrz*qy), (-vrx*qz + vrz*qx), (vrx*qy + vrz*qw),
			( vrx*qx + vrz*qz), ( vrx*qy + vrz*qw), (vrx*qz - vrz*qx),
			(-vrx*qy - vrz*qw), ( vrx*qx + vrz*qz), (vrx*qw - vrz*qy),
			(-vrx*qz + vrz*qx), (-vrx*qw + vrz*qy), (vrx*qx + vrz*qz)
			);

		rightProduct <<
			vrx * (qw*qw + qx*qx - qy*qy - qz*qz) + vrz * (-2*qw*qy + 2*qx*qz)            - vmx,
			vrx * (-2*qw*qz + 2*qx*qy)			  + vrz * (2*qw*qx + 2*qy*qz)			  - vmy,
			vrx * (2*qw*qy + 2*qx*qz)			  + vrz * (qw*qw - qx*qx - qy*qy + qz*qz) - vmz;

		result = leftProduct * rightProduct;
		return Eigen::Quaternion<T>(result[0], result[1], result[2], result[3]);
	}

};

#endif /* !ENHANCED_MADGWICK_HPP */


/*=================================================================================
 *	                    Copyleft! 2018 William Yu
 *          Some rights reserved：CC(creativecommons.org)BY-NC-SA
 *                      Copyleft! 2018 William Yu
 *      版权部分所有，遵循CC(creativecommons.org)BY-NC-SA协议授权方式使用
 *
 * Filename                :
 * Description             :
 * Reference               :
 * Programmer(s)           : William Yu, windmillyucong@163.com
 * Company                 : HUST, DMET国家重点实验室FOCUS团队
 * Modification History	   : ver1.0, 2019.01.08, William Yu
                             ver1.1, 2019.01.13, William Yu, add notes
=================================================================================*/

/*-----------------------------[Note]---------------------------
 *
--------------------------------------------------------------*/

/// Include Files
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

using namespace std;
using namespace cv;

/// Global Variables

/// Function Declaration

void find_feature_matches (
    const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches
);

// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );

void pose_estimation_3d3d (
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    Mat& R, Mat& t
);

/// Function definitions

/**
 * @function main
 * @brief
 */
int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d3d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 内参矩阵
    //--调参：相机内参--
    //        | fx,  0, cx, | cx主点偏移
    //   K =  |  0, fy, cy, |
    //        |  0,  0,  1  |
    Mat K = ( Mat_<double> ( 3,3 ) << 518.0,     0, 325.5,
                                          0, 519.0, 253.5,
                                          0,     0,     1 );
    double depthScale = 1000.0;

    //-- 读取图像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    // 建立3D点
    Mat depth1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat depth2 = imread ( argv[4], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像

    vector<Point3f> pts1, pts2;
    for ( DMatch m:matches )
    {
        ushort d1 = depth1.ptr<unsigned short> ( int ( keypoints_1[m.queryIdx].pt.y ) ) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        ushort d2 = depth2.ptr<unsigned short> ( int ( keypoints_2[m.trainIdx].pt.y ) ) [ int ( keypoints_2[m.trainIdx].pt.x ) ];
        if ( d1==0 || d2==0 )   // bad depth //深度值为0，无法使用
            continue;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );//是测试图像的特征点描述子（descriptor）的下标，同时也是描述符对应特征点（keypoint)的下标。
        Point2d p2 = pixel2cam ( keypoints_2[m.trainIdx].pt, K );//是样本图像的特征点描述子的下标，同样也是相应的特征点的下标。
        float dd1 = float ( d1 ) /depthScale;
        float dd2 = float ( d2 ) /depthScale;
        pts1.push_back ( Point3f ( p1.x*dd1, p1.y*dd1, dd1 ) );
        pts2.push_back ( Point3f ( p2.x*dd2, p2.y*dd2, dd2 ) );
    }

    cout<<"3d-3d 点对: "<<pts1.size() <<endl;
    Mat R, t;
    pose_estimation_3d3d ( pts1, pts2, R, t );
    cout<<"---------------------------------------------"<<endl;
    cout<<"ICP via SVD results: "<<endl;
    cout<<"R_12 = "<<R<<endl;
    cout<<"t_12 = "<<t<<endl;
    /*-----------------------------[Note]---------------------------
    * pts1 = R*pts2 + t
    * 则有pts2 = R_inv*pts1 - R_inv*t
    * 对应pts2 = R`*pts1 + t`
    * 即逆运动R` = R_inv
    *        t` = - R_inv*t
    * Notice:旋转矩阵R为正交矩阵，
    * 所有又有 R^(-1) = R^T，逆等于转置
    --------------------------------------------------------------*/
    cout<<"逆运动R：R` = R_inv = "<<R.t() <<endl;   //Mat.t()为矩阵求转置
    //cout<<"逆运动R：R` = R_inv = "<<R.inv() <<endl;  //Mat.inv()为矩阵求逆
    cout<<"逆运动t：t` = - R_inv*t = "<<-R.t() *t <<endl;

    cout<<"---------------------------------------------"<<endl;
    //--校验5组 p1 = R*p2 + t  和  p2 = R_inv*p1 - R_inv*t
    for ( int i=0; i<5; i++ )
    {
        cout<<"p1 = "<<pts1[i]<<endl;
        cout<<"p2 = "<<pts2[i]<<endl;
        cout<<"(R*p2+t) = "<<
            R * (Mat_<double>(3,1)<<pts2[i].x, pts2[i].y, pts2[i].z) + t << endl;
        cout<<"(R`*p1+t`) = "<<
            R.inv() * (Mat_<double>(3,1)<<pts1[i].x, pts1[i].y, pts1[i].z) -R.inv() *t << endl<< endl;
    }
}

/**
 * @function find_feature_matches
 * @brief 特征点匹配与筛选
 * @param  const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& good_matches
 * @retval None
 */

void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& good_matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create("BruteForce-Hamming");

    //--[1]:检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //--[2]:根据角点位置计算 BRIEF 描述子
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );

    //绘制特征点
    Mat outimg1, outimg2;
    drawKeypoints( img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT );
    namedWindow("ORB_img1", WINDOW_NORMAL);
    imshow("ORB_img1",outimg1);
    drawKeypoints( img_2, keypoints_2, outimg2, Scalar::all(-1), DrawMatchesFlags::DEFAULT );
    namedWindow("ORB_img2", WINDOW_NORMAL);
    imshow("ORB_img2",outimg2);
    cout<<"Img1特征点数"<<keypoints_1.size() <<endl;//500个特征点
    cout<<"Img2特征点数"<<keypoints_2.size() <<endl;

    //--[3]:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> matches;
    //BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, matches );
    cout<<"初步匹配"<<matches.size() <<"对匹配点"<<endl; //500对匹配

    //--[4]:匹配点对筛选
    //--筛选要求：1.确保都是正确匹配，删去错误匹配
    //          2.确保正确匹配的前提下，保证点对数足够多

    //--调参：筛选阈值--
    double min_dist=10000, max_dist=0;
    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = matches[i].distance;
        if ( dist < min_dist ) min_dist = dist;//最短距离，最相似
        if ( dist > max_dist ) max_dist = dist;//最长距离，最不相似
    }
    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

    //--调参：筛选阈值--
    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.
    //但有时候最小距离会非常小,设置一个经验值30作为下限.
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( matches[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            good_matches.push_back ( matches[i] );
        }
    }

    //--TODO：调参：逐点检查--

    //--[5]:绘制匹配结果
    Mat img_match;
    Mat img_goodmatch;
    drawMatches ( img_1, keypoints_1, img_2, keypoints_2, matches, img_match );
    drawMatches ( img_1, keypoints_1, img_2, keypoints_2, good_matches, img_goodmatch );
    namedWindow("img_match", WINDOW_NORMAL);
    namedWindow("img_goodmatch", WINDOW_NORMAL);
    imshow ( "img_match", img_match );
    imshow ( "img_goodmatch", img_goodmatch );
    waitKey(0);
}

/**
 * @function pixel2cam
 * @brief 像素坐标转相机归一化坐标
 * @param  const Point2d& p, const Mat& K
 * @retval Point2d
 */
Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
           (
               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
}

/**
 * @function pose_estimation_3d3d
 * @brief   求解R、t
 * @param   const vector<Point3f>& pts1,
            const vector<Point3f>& pts2,
            Mat& R, Mat& t
 * @retval None
 */
void pose_estimation_3d3d (
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    Mat& R, Mat& t
)
{
    Point3f p1, p2;     // center of mass
    int N = pts1.size();
    for ( int i=0; i<N; i++ )
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = Point3f( Vec3f(p1) /  N);
    p2 = Point3f( Vec3f(p2) / N);
    vector<Point3f>     q1 ( N ), q2 ( N ); // remove the center
    for ( int i=0; i<N; i++ )
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for ( int i=0; i<N; i++ )
    {
        W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
    }
    cout<<"W="<<W<<endl;

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    if (U.determinant() * V.determinant() < 0)
	{
        for (int x = 0; x < 3; ++x)
        {
            U(x, 2) *= -1;
        }
	}

    cout<<"U="<<U<<endl;
    cout<<"V="<<V<<endl;

    Eigen::Matrix3d R_ = U* ( V.transpose() );
    Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z );

    // convert to cv::Mat
    R = ( Mat_<double> ( 3,3 ) <<
          R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ),
          R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ),
          R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 )
        );
    t = ( Mat_<double> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) );
}


#include "sbs/physics/collision/intersections.h"

#include <Eigen/Geometry>

namespace sbs {
namespace physics {
namespace collision {

line_segment_t::line_segment_t(point_t const& p, point_t const& q) : p(p), q(q) {}

triangle_t::triangle_t(point_t const& a, point_t const& b, point_t const& c) : a(a), b(b), c(c) {}

normal_t triangle_t::normal() const
{
    Eigen::Vector3d const ab = b - a;
    Eigen::Vector3d const ac = c - a;
    return ab.cross(ac).normalized();
}

ray_t::ray_t(point_t const& p, direction_t const& v, double t) : p(p), v(v), t(t) {}

std::optional<point_t> intersect(line_segment_t const& segment, triangle_t const& triangle)
{
    Eigen::Vector3d const ab = triangle.b - triangle.a;
    Eigen::Vector3d const ac = triangle.c - triangle.a;
    Eigen::Vector3d const qp = segment.p - segment.q;

    Eigen::Vector3d const n = ab.cross(ac);

    double const d = qp.dot(n);
    if (d <= 0.)
        return {};

    Eigen::Vector3d const ap = segment.p - triangle.a;
    double const t           = ap.dot(n);
    if (t < 0.)
        return {};
    if (t > d)
        return {};

    Eigen::Vector3d const e = qp.cross(ap);
    double v                = ac.dot(e);
    if (v < 0. || v > d)
        return {};

    double w = -ab.dot(e);
    if (w < 0. || (v + w) > d)
        return {};

    double const ood = 1. / d;
    v *= ood;
    w *= ood;
    double const u             = 1. - v - w;
    point_t const intersection = u * triangle.a + v * triangle.b + w * triangle.c;
    return intersection;
}

std::optional<point_t> intersect_twoway(line_segment_t const& segment, triangle_t const& triangle)
{
    auto const intersection = intersect(segment, triangle);
    if (intersection.has_value())
        return intersection;

    line_segment_t const flipped_segment{segment.q, segment.p};
    return intersect(flipped_segment, triangle);
}

} // namespace collision
} // namespace physics
} // namespace sbs


#include <Eigen/Core>
#include <Eigen/LU>
#include <iostream>
#include <iomanip>
#include <vector>
#include <cmath>
#include <string>
#include <sstream>
#include <random>
#include "ProbDistributions.h"
#include "DataSet.h"
#include "Cluster.h"
using namespace std;

int dim = 0;

int pickCandidateCluster(vector<int> *bincount,double d = 0.0)
{
	vector<double> prob_list;
	const double alpha = 0.8;
	int n = accumulate(bincount->begin(),bincount->end(),0.0);
	int n_k = 0;
	int i = 0;
	for(auto bin : *bincount){
		n_k += bin;
		prob_list.push_back(((double)n_k - d*(i+1))/(n + alpha));
	}

	double r = pd.uniformRand(0.0,1.0);
	int candidate_cluster = 0;
	for(auto p : prob_list){
		if(r < p)
			break;
		candidate_cluster++;
	}

	return candidate_cluster;
}

double densityMultiNormal(Data &d, Cluster &c)
{
	Eigen::MatrixXd info = c.cov.inverse();
	Eigen::VectorXd mu = c.mean;//[0],c.mean[1]);
	Eigen::VectorXd diff = d.normalized_data - mu;

	double det = c.cov.determinant();
	double a = 1.0/ (pow(2 * 3.151592,c.dimension*0.5) *  sqrt(det));
	double exp_part = -0.5 * diff.transpose() * info * diff;

	return a*exp(exp_part);
}

bool resampling(DataSet *ds, Clusters *cs, Data *d, vector<int> &bin)
{
	int org_c_num = (int)cs->c.size();
	bin[d->cluster_id]--;
	int candidate_cluster = pickCandidateCluster(&bin);
	bin[d->cluster_id]++;

	if(candidate_cluster == d->cluster_id){
		return false;
	}

	Cluster *old_cluster = &(cs->c[d->cluster_id]);
	double eval_new = 0.0;
	double eval_old = densityMultiNormal(*d,*old_cluster);
	Cluster c(dim);
	if(candidate_cluster == org_c_num){//新しいクラスタ
		eval_new = densityMultiNormal(*d,c);
	}else{//既存のクラスタ
		eval_new = densityMultiNormal(*d,cs->c[candidate_cluster]);
	}

	double acceptance = eval_new/eval_old;
	if(pd.uniformRand(0.0,1.0) >= acceptance)
		return false;

	bin[d->cluster_id]--;
	d->cluster_id = candidate_cluster;
	if(candidate_cluster == org_c_num){
		cs->c.push_back(c);
		bin.push_back(1);
	}else
		bin[candidate_cluster]++;
	return true;
}

void sweep(DataSet *ds,Clusters *cs)
{
	vector<int> bincount(cs->c.size(),0);
	for(auto x : ds->x){
		bincount[x.cluster_id]++;
	}

	int chance = 3;
	for(auto &target : ds->x){
		for(int i=0;i<chance;i++){
			resampling(ds,cs,&target,bincount);
		}
	}
	//どのクラスタに標本が幾つかる数える
	for(auto &c : cs->c){
		c.clear();
	}
	for(auto &d : ds->x){
		cs->c[d.cluster_id].regData(&d);
	}
	cs->calcParams();
	cerr << "----" << endl;
}

int main(int argc, char const* argv[])
{
	Clusters cs;
	DataSet ds;
	dim = ds.read();
	if(dim <= 0)
		exit(1);

	int sweep_num = 50;

	//最初のクラスタを作る。平均値は1軸ごとにガウス分布からサンプリング
	cs.c.push_back(Cluster(dim));
	for(auto &d : ds.x){
		cs.c[d.cluster_id].regData(&d);
	}
	cs.calcParams();
	cerr << "----" << endl;

	for(int k=0;k<sweep_num;k++){
		cerr << "sweep " << k << endl;
		sweep(&ds,&cs);
	}
	ds.print();

	exit(0);
}


#include <math.h>
#include <vector>
#include <Eigen/Dense>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned,K> Vb;
typedef CGAL::Triangulation_data_structure_2<Vb> Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds> Delaunay;
typedef Delaunay::Face_circulator Face_circulator;
typedef Delaunay::Face_handle Face_handle;
typedef Delaunay::Point Point;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> MapM3Xd;
typedef Eigen::Ref<Matrix3Xd> RefM3Xd;

// Pass the number of points in point cloud and address of the 0th point coords
static double_t pointCloudVolume( std::size_t N, double_t* p ){

    // We will extract 3*N doubles representing particle positions
    MapM3Xd Xt( p, 3, N );
    Matrix3Xd X0(3,N);

    // Project the points to a unit sphere
    X0 = Xt.colwise().normalized();

    // Rotate all points of the shell so that the 0th point is along z-axis
    Vector3d c = X0.col(0);
    double_t cos_t = c(2);
    double_t sin_t = std::sin( std::acos( cos_t ) );
    Vector3d axis;
    axis << c(1), -c(0), 0.;
    axis.normalize();
    Matrix3d rotMat, axis_cross, outer;
    axis_cross << 0. , -axis(2), axis(1),
               axis(2), 0., -axis(0),
               -axis(1), axis(0), 0.;
    outer.noalias() = axis*axis.transpose();
    rotMat = cos_t*Matrix3d::Identity() + sin_t*axis_cross + (1 - cos_t)*outer;
    Matrix3Xd rPts(3,N);
    rPts = rotMat*X0;

    // Calculate the stereographic projections
    Vector3d p0;
    p0 << 0,0,-1.0; // Point on the plane of projection
    c = rPts.col(0); // The point from which we are projecting

    MapM3Xd l0( &(rPts(0,1)), 3, N-1 );
    Matrix3Xd l(3,N-1), proj(3,N-1);
    l = (l0.colwise() - c).colwise().normalized(); // dirns of projections
    for( std::size_t j=0; j < N-1; ++j ){
        proj.col(j) = ((p0(2) - l0(2,j))/l(2,j))*l.col(j) + l0.col(j);
    }

    // Insert the projected points in a CGAL vertex_with_info vector
    std::vector< std::pair< Point, unsigned> > verts;
    for( std::size_t j=0; j < N-1; ++j ){
        verts.push_back(std::make_pair(Point(proj(0,j),proj(1,j)),j+1));
    }

    // Triangulate
    Delaunay dt( verts.begin(), verts.end() );

    // Iterate over the triangles to calculate volume
    double_t volume = 0.0;
    for( auto ffi = dt.finite_faces_begin(); ffi != dt.finite_faces_end();
            ++ffi){
        auto i = ffi->vertex(0)->info();
        auto j = ffi->vertex(2)->info();
        auto k = ffi->vertex(1)->info();
        volume += 0.166666667*(Xt.col(i).dot(Xt.col(j).cross(Xt.col(k))));
    }

    // Iterate over infinite faces
    Face_circulator fc = dt.incident_faces(dt.infinite_vertex()), done(fc);
    if (fc != 0) {
        do{
            auto i = dt.is_infinite(fc->vertex(0))?0:fc->vertex(0)->info();
            auto j = dt.is_infinite(fc->vertex(2))?0:fc->vertex(2)->info();
            auto k = dt.is_infinite(fc->vertex(1))?0:fc->vertex(1)->info();
            volume += 0.166666667*
                (Xt.col(i).dot(Xt.col(j).cross(Xt.col(k))));
        }while(++fc != done);
    }

    return volume;
}


#include <cmath>
#include <string>
#include <stdexcept>
#include <boost/math/special_functions/bessel.hpp>
#include "spida/grid/besselR.h"
#include <iostream>

namespace spida{

BesselRootGridR::BesselRootGridR(int nr,double maxr) : GridR(nr,maxr),
    m_r(nr), m_sr(nr)
{
    //OutputIterator cyl_bessel_j_zero(
    //                 T v,                       // Floating-point value for Jv.
    //                 int start_index,           // 1-based index of first zero.
    //                 unsigned number_of_roots,  // How many roots to generate.
    //                 OutputIterator out_it);

    // Want J0 -> v = 0, starting with first root -> start_index=1
    boost::math::cyl_bessel_j_zero<double>(0.0,1,nr,std::back_inserter(m_roots));
    // 1-based index of zero (use nr+1 for m_jN rather than nr)
    m_jN = boost::math::cyl_bessel_j_zero<double>(0.0,nr+1);
    // Set physical grid
    for(auto i = 0; i < nr; i++)
        m_r[i] = m_roots[i]*GridR::getMaxR()/m_jN;
    // Set spectral grid
    for(auto i = 0; i < nr; i++)
        m_sr[i] = m_roots[i]*getMaxSR()/m_jN;
}

}


#pragma once
#include "kissfft.hh"
#include <boost/math/constants/constants.hpp>
#include <boost/optional.hpp>
#include <cstddef>

namespace vv
{

	template <class T, class U>
	auto lerp(T x0, T x1, U ratio)
	{
		return x0 + (x1 - x0) * ratio;
	}

	template <class T>
	auto invlerp(T x0, T x1, T x)
	{
		return (x - x0) / (x1 - x0);
	}

	template <class T>
	auto squared(T x)
	{
		return x * x;
	}

	class processor
	{
	public:

		static const std::size_t buffer_size = 4096;
		static const std::size_t nsdf_size = buffer_size / 2;

		explicit processor(double sampleRate)
			: sampleRate_(sampleRate)
			, v1_(buffer_size)
			, v2_(buffer_size + nsdf_size)
			, v3_(buffer_size + nsdf_size)
			, v4_(buffer_size + nsdf_size)
			, v5_(buffer_size + nsdf_size)
			, v6_(buffer_size)
			, v7_(buffer_size / 2)
		{
		}

		void operator ()(const float* input, float* output, double pitch_shift, double formant_shift)
		{
			for (std::size_t i = 0; i < buffer_size; ++i)
				v1_[i] = std::complex<float>(input[i], 0.0f);

			for (std::size_t i = 0; i < buffer_size; ++i)
			{
				auto r = static_cast<double>(i) / static_cast<double>(buffer_size);
				auto w = 0.5 - 0.5 * std::cos(boost::math::constants::two_pi<double>() * r);
				v2_[i] = v1_[i] * static_cast<float>(w);
			}

			fft_.transform(v2_.data(), v3_.data());

			auto cutoff_hz = 800.0;
			auto cutoff_index = static_cast<std::size_t>(std::round(cutoff_hz * static_cast<double>(buffer_size) / sampleRate_));

			for (std::size_t i = 0; i < cutoff_index; ++i)
			{
				v4_[i + 1] = std::norm(v3_[i + 1]);
				v4_[buffer_size - i - 1] = std::norm(v3_[buffer_size - i - 1]);
			}

			ifft_.transform(v4_.data(), v5_.data());

			for (std::size_t i = 0; i < buffer_size + nsdf_size; ++i)
				v5_[i] /= static_cast<float>(buffer_size + nsdf_size);

			for (std::size_t i = 1; i < buffer_size; ++i)
			{
				auto j = buffer_size - i - 1;
				v6_[j] = v6_[j + 1] + squared(v2_[i].real()) + squared(v2_[j].real());
			}

			for (std::size_t i = 0; i < buffer_size / 2; ++i)
			{
				if (v6_[i] < std::numeric_limits<double>::min())
					v7_[i] = 0.0f;
				else
					v7_[i] = 2.0f * v5_[i].real() / v6_[i];
			}

			auto minimum_hz = 50.0;
			auto maximum_hz = 300.0;

			auto minimum_index = static_cast<std::size_t>(std::round(sampleRate_ / maximum_hz));
			auto maximum_index = static_cast<std::size_t>(std::round(sampleRate_ / minimum_hz));

			minimum_index = std::max<std::size_t>(minimum_index, 1);
			minimum_index = std::min<std::size_t>(minimum_index, buffer_size / 2 - 2);

			maximum_index = std::max<std::size_t>(maximum_index, 1);
			maximum_index = std::min<std::size_t>(maximum_index, buffer_size / 2 - 2);

			double maximum_value = 0.0;

			for (std::size_t i = minimum_index; i < maximum_index; ++i)
			{
				auto p1 = v7_[i - 1];
				auto p2 = v7_[i];
				auto p3 = v7_[i + 1];

				if (p1 < p2 && p2 > p3 && p2 > maximum_value)
					maximum_value = p2;
			}

			boost::optional<std::size_t> peak_index;
			double peak_value = 0.0;

			for (std::size_t i = minimum_index; i < maximum_index; ++i)
			{
				auto p1 = v7_[i - 1];
				auto p2 = v7_[i];
				auto p3 = v7_[i + 1];

				if (p1 < p2 && p2 > p3 && p2 > maximum_value * 0.9)
				{
					peak_index = i;
					peak_value = p2;
					break;
				}
			}

			if (!peak_index && last_peak_index_)
				peak_index = last_peak_index_;

			last_peak_index_ = peak_index;

			bool enable = false;

			if (peak_index)
			{
				std::size_t last_dst = 0;
				std::size_t last_src1 = 0;
				std::size_t last_src2 = 0;
				double last_src_ratio = 0.0;

				auto easing = [&](double x)
				{
					return (1.0 - std::cos(boost::math::constants::pi<double>() * x)) / 2.0;
				};

				auto interpolate = [&](double x1, double x2, double ratio)
				{
					return lerp(x1, x2, easing(ratio));
				};

				auto get_value = [&](double indexf) -> double
				{
					if (indexf < 0.0)
						return input[0];

					auto index = static_cast<std::size_t>(std::floor(indexf));
					if (index >= buffer_size - 1)
						return input[buffer_size - 1];

					auto ratio = indexf - std::floor(indexf);

					return interpolate(input[index], input[index + 1], ratio);
				};

				auto overlap = [&](std::size_t dst, std::size_t src1, std::size_t src2, double src_ratio)
				{
					for (std::size_t i = last_dst; i < dst; ++i)
					{
						auto ratio = static_cast<double>(i - last_dst) / static_cast<double>(dst - last_dst);

						auto p1_1 = get_value(static_cast<double>(last_src1) + static_cast<double>(i - last_dst) * formant_shift);
						auto p1_2 = get_value(static_cast<double>(last_src2) + static_cast<double>(i - last_dst) * formant_shift);
						auto p1 = interpolate(p1_1, p1_2, last_src_ratio);

						auto p2_1 = get_value(static_cast<double>(src1) - static_cast<double>(dst - i) * formant_shift);
						auto p2_2 = get_value(static_cast<double>(src2) - static_cast<double>(dst - i) * formant_shift);
						auto p2 = interpolate(p2_1, p2_2, src_ratio);

						output[i] = static_cast<float>(interpolate(p1, p2, ratio));
					}

					last_dst = dst;
					last_src1 = src1;
					last_src2 = src2;
					last_src_ratio = src_ratio;
				};

				auto q = buffer_size / *peak_index;
				auto r = buffer_size % *peak_index;

				auto nf = (static_cast<double>(buffer_size) * pitch_shift - static_cast<double>(r)) / static_cast<double>(*peak_index);
				auto n = static_cast<std::size_t>(std::max(0.0, std::round(nf)));

				if (q != 0 && n != 0)
				{
					auto actual_pitch_shift = static_cast<double>(n * *peak_index + r) / static_cast<double>(buffer_size);

					for (std::size_t i = 1; i <= n; ++i)
					{
						double frame_indexf = 1.0;

						if (n != 1)
							frame_indexf = static_cast<double>((i - 1) * (q - 1)) / static_cast<double>(n - 1) + 1;

						auto frame_index = static_cast<std::size_t>(std::floor(frame_indexf));

						auto dst = static_cast<std::size_t>(std::floor(static_cast<double>(i * *peak_index) / actual_pitch_shift));
						auto src = frame_index * *peak_index;

						if (frame_index == q)
						{
							overlap(dst, src, src, 0.0);
						}
						else
						{
							auto src_ratio = frame_indexf - std::floor(frame_indexf);
							overlap(dst, src, src + *peak_index, src_ratio);
						}
					}

					overlap(buffer_size, buffer_size, buffer_size, 0.0);

					enable = true;
				}
			}

			if (!enable)
				std::copy(input, input + buffer_size, output);
		}

	private:

		double sampleRate_;

		kissfft<float> fft_{ buffer_size + nsdf_size, false };
		kissfft<float> ifft_{ buffer_size + nsdf_size, true };

		std::vector<std::complex<float>> v1_;
		std::vector<std::complex<float>> v2_;
		std::vector<std::complex<float>> v3_;
		std::vector<std::complex<float>> v4_;
		std::vector<std::complex<float>> v5_;
		std::vector<float> v6_;
		std::vector<float> v7_;

		boost::optional<std::size_t> last_peak_index_;

	};

}


#pragma once

#include <Eigen/Dense>
#include <random>

template <int NumArguments, typename StepRNG = std::uniform_real_distribution<>,
          class Generator = std::mt19937>
class MetropolisAlgorithm {
 public:
  using Argument = Eigen::Array<double, NumArguments, 1>;
  using FunctionType = std::function<double(const Argument&)>;
  using GeneratorResType = Generator::result_type;

 private:
  FunctionType pdf;
  StepRNG step;
  std::uniform_real_distribution<> uniform =
      std::uniform_real_distribution<>(0, 1);
  std::mt19937 gen;

  // saving the old probability minimizes the function calls
  double p_old;
  std::size_t argument_index = 0;

 public:
  Argument argument;

  MetropolisAlgorithm(FunctionType pdf, Argument& argstart, StepRNG step,
                      GeneratorResType seed,
                      std::size_t warm_up = 100 * NumArguments)
      : pdf(pdf), argument(argstart), step(step) {
    gen = Generator(seed);  // Standard mersenne_twister_engine seeded with seed

    p_old = pdf(argument);

    for (std::size_t i = 0; i < warm_up; i++) do_step();
  };

  MetropolisAlgorithm(FunctionType pdf, Argument& argstart, StepRNG step,
                      std::size_t warm_up = 100 * NumArguments)
      : pdf(pdf), step(step), argument(argstart) {
    std::random_device rd;
    gen = Generator(rd());  // Standard mersenne_twister_engine seeded with seed

    p_old = pdf(argument);

    for (std::size_t i = 0; i < warm_up; i++) do_step();
  };

  void do_step() {
    double x_old = argument[argument_index];
    double x_new = x_old + step(gen);

    argument[argument_index] = x_new;
    double p_new = pdf(argument);
    double p = p_new / p_old;

    if (p < 1) {
      double s = uniform(gen);

      // reject new values
      if (s > p) {
        x_new = x_old;
        p_new = p_old;
      }
    }

    argument[argument_index] = x_new;
    p_old = p_new;

    argument_index++;
    if (argument_index >= NumArguments) {
      argument_index = 0;
    }
  }

  Argument& next() {
    do_step();
    return argument;
  }

  /**
   * @brief Calculates the expectation value and the standard deviation of the
   * given funtion (under the pdf)
   *
   * Using the Welfords online algorithm to calculate the mean and standard
   * deviation.
   *
   * @param function function to average
   * @param samples number of samples to collect
   * @return std::tuple<double, double, double> Mean, std of mean, variance
   */
  std::tuple<double, double, double> average(
      std::function<double(const Argument&)> function, std::size_t samples) {
    std::size_t n;
    double mean = 0;
    // double mean2 = 0;
    double S = 0;

    for (n = 1; n <= samples; n++) {
      do_step();
      double x = function(argument);
      double old_mean = mean;
      mean += (x - mean) / double(n);
      // mean2 += (x * x - mean2) / double(n);
      S += (x - mean) * (x - old_mean);
    }

    // return {mean, std::sqrt(mean2 - mean * mean)};
    return {mean, std::sqrt(S / (samples - 1) / samples), S / (samples - 1)};
  }

  Eigen::Array<double, Eigen::Dynamic, NumArguments> get_sample(
      Eigen::Index N) {
    using namespace Eigen;
    typedef Array<double, Eigen::Dynamic, NumArguments> ReturnType;

    ReturnType returner(N, NumArguments);

    for (Index i = 0; i < N; i++) {
      do_step();
      returner.row(i) = argument;
    }

    return returner;
  }
};


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_SIGNEDSVD_HPP
#define MCL_SIGNEDSVD_HPP 1

#include <Eigen/Dense>
#include <Eigen/SVD>

namespace mcl
{

template <typename T, int DIM>
static inline void signed_svd(
	const Eigen::Matrix<T,DIM,DIM> &F,
	Eigen::Matrix<T,DIM,1> &S,
	Eigen::Matrix<T,DIM,DIM> &U,
	Eigen::Matrix<T,DIM,DIM> &V)
{
	using namespace Eigen;
	int dim = DIM == Eigen::Dynamic ? F.rows() : DIM;
	typedef Matrix<T,DIM,DIM> MatX;

	JacobiSVD<MatX> svd(F, ComputeFullU | ComputeFullV);
	S = svd.singularValues();
	U = svd.matrixU();
	V = svd.matrixV();

	MatX J = MatX::Identity(dim,dim);
	J(dim-1,dim-1) = -1.0;

	if (U.determinant() < 0.0)
	{
		U = U * J;
		S[dim-1] *= -1.0;
	}

	if (V.determinant() < 0.0)
	{
		V = (J * V.transpose()).transpose();
		S[dim-1] *= -1.0;
	}

	// Degenerate case
	if (!S.allFinite())
	{
		S.setZero();
		U.setIdentity();
		V.setIdentity();
	}

} // end signed svd

} // end mcl

#endif


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2008 - 2020 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: Liang Zhao and Timo Heister, Clemson University, 2016
 */

// @sect3{Include files}

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

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/utilities.h>
#include <deal.II/base/tensor.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/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/solver_gmres.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_refinement.h>
#include <deal.II/grid/grid_tools.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>

// 为了在网格之间传输解决方案，包括这个文件。

#include <deal.II/numerics/solution_transfer.h>

// 这个文件包括UMFPACK：直接求解器。

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

// 还有一个ILU预处理程序。

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

#include <fstream>
#include <iostream>

namespace Step57
{
  using namespace dealii;
// @sect3{The NavierStokesProblem class template}

// 该类管理介绍中描述的矩阵和向量：特别是，我们为当前的解决方案、当前的牛顿更新和直线搜索更新存储了一个BlockVector。 我们还存储了两个AffineConstraints对象：一个是强制执行Dirichlet边界条件的对象，另一个是将所有边界值设为0的对象。第一个约束解向量，第二个约束更新（也就是说，我们从不更新边界值，所以我们强制相关的更新向量值为零）。

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

  private:
    void setup_dofs();

    void initialize_system();

    void assemble(const bool initial_step, const bool assemble_matrix);

    void assemble_system(const bool initial_step);

    void assemble_rhs(const bool initial_step);

    void solve(const bool initial_step);

    void refine_mesh();

    void process_solution(unsigned int refinement);

    void output_results(const unsigned int refinement_cycle) const;

    void newton_iteration(const double       tolerance,
                          const unsigned int max_n_line_searches,
                          const unsigned int max_n_refinements,
                          const bool         is_initial_step,
                          const bool         output_result);

    void compute_initial_guess(double step_size);

    double                               viscosity;
    double                               gamma;
    const unsigned int                   degree;
    std::vector<types::global_dof_index> dofs_per_block;

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

    AffineConstraints<double> zero_constraints;
    AffineConstraints<double> nonzero_constraints;

    BlockSparsityPattern      sparsity_pattern;
    BlockSparseMatrix<double> system_matrix;
    SparseMatrix<double>      pressure_mass_matrix;

    BlockVector<double> present_solution;
    BlockVector<double> newton_update;
    BlockVector<double> system_rhs;
    BlockVector<double> evaluation_point;
  };
// @sect3{Boundary values and right hand side}

// 在这个问题中，我们设定沿空腔上表面的速度为1，其他三面墙的速度为0。右边的函数为零，所以我们在本教程中不需要设置右边的函数。边界函数的分量数为  dim+1  。我们最终将使用 VectorTools::interpolate_boundary_values 来设置边界值，这就要求边界值函数的分量数与解相同，即使没有全部使用。换个说法：为了让这个函数高兴，我们为压力定义了边界值，尽管我们实际上永远不会用到它们。

  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) 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 && std::abs(p[dim - 1] - 1.0) < 1e-10)
      return 1.0;

    return 0;
  }
// @sect3{BlockSchurPreconditioner for Navier Stokes equations}

// 正如介绍中所讨论的，Krylov迭代方法中的预处理器是作为一个矩阵-向量乘积算子实现的。在实践中，舒尔补码预处理器被分解为三个矩阵的乘积（如第一节所述）。第一个因素中的 $\tilde{A}^{-1}$ 涉及到对线性系统 $\tilde{A}x=b$ 的求解。在这里，为了简单起见，我们通过一个直接求解器来解决这个系统。第二个因素中涉及的计算是一个简单的矩阵-向量乘法。舒尔补码 $\tilde{S}$ 可以被压力质量矩阵很好地近似，其逆值可以通过不精确求解器得到。因为压力质量矩阵是对称和正定的，我们可以用CG来解决相应的线性系统。

  template <class PreconditionerMp>
  class BlockSchurPreconditioner : public Subscriptor
  {
  public:
    BlockSchurPreconditioner(double                           gamma,
                             double                           viscosity,
                             const BlockSparseMatrix<double> &S,
                             const SparseMatrix<double> &     P,
                             const PreconditionerMp &         Mppreconditioner);

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

  private:
    const double                     gamma;
    const double                     viscosity;
    const BlockSparseMatrix<double> &stokes_matrix;
    const SparseMatrix<double> &     pressure_mass_matrix;
    const PreconditionerMp &         mp_preconditioner;
    SparseDirectUMFPACK              A_inverse;
  };

// 我们可以注意到，左上角的矩阵逆的初始化是在构造函数中完成的。如果是这样，那么预处理程序的每一次应用就不再需要计算矩阵因子了。

  template <class PreconditionerMp>
  BlockSchurPreconditioner<PreconditionerMp>::BlockSchurPreconditioner(
    double                           gamma,
    double                           viscosity,
    const BlockSparseMatrix<double> &S,
    const SparseMatrix<double> &     P,
    const PreconditionerMp &         Mppreconditioner)
    : gamma(gamma)
    , viscosity(viscosity)
    , stokes_matrix(S)
    , pressure_mass_matrix(P)
    , mp_preconditioner(Mppreconditioner)
  {
    A_inverse.initialize(stokes_matrix.block(0, 0));
  }

  template <class PreconditionerMp>
  void BlockSchurPreconditioner<PreconditionerMp>::vmult(
    BlockVector<double> &      dst,
    const BlockVector<double> &src) const
  {
    Vector<double> utmp(src.block(0));

    {
      SolverControl solver_control(1000, 1e-6 * src.block(1).l2_norm());
      SolverCG<Vector<double>> cg(solver_control);

      dst.block(1) = 0.0;
      cg.solve(pressure_mass_matrix,
               dst.block(1),
               src.block(1),
               mp_preconditioner);
      dst.block(1) *= -(viscosity + gamma);
    }

    {
      stokes_matrix.block(0, 1).vmult(utmp, dst.block(1));
      utmp *= -1.0;
      utmp += src.block(0);
    }

    A_inverse.vmult(dst.block(0), utmp);
  }
// @sect3{StationaryNavierStokes class implementation}
// @sect4{StationaryNavierStokes::StationaryNavierStokes}

// 该类的构造函数看起来与  step-22  中的构造函数非常相似。唯一的区别是粘度和增强的拉格朗日系数  gamma  。

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

// 这个函数初始化DoFHandler，列举当前网格上的自由度和约束。

  template <int dim>
  void StationaryNavierStokes<dim>::setup_dofs()
  {
    system_matrix.clear();
    pressure_mass_matrix.clear();

// 第一步是将DoFs与给定的网格联系起来。

    dof_handler.distribute_dofs(fe);

// 我们对组件重新编号，使所有的速度DoF在压力DoF之前，以便能够将解向量分成两个块，在块预处理程序中分别访问。

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

    dofs_per_block =
      DoFTools::count_dofs_per_fe_block(dof_handler, block_component);
    unsigned int dof_u = dofs_per_block[0];
    unsigned int dof_p = dofs_per_block[1];

// 在牛顿方案中，我们首先将边界条件应用于从初始步骤得到的解。为了确保边界条件在牛顿迭代过程中保持满足，在更新时使用零边界条件  $\delta u^k$  。因此我们设置了两个不同的约束对象。

    FEValuesExtractors::Vector velocities(0);
    {
      nonzero_constraints.clear();

      DoFTools::make_hanging_node_constraints(dof_handler, nonzero_constraints);
      VectorTools::interpolate_boundary_values(dof_handler,
                                               0,
                                               BoundaryValues<dim>(),
                                               nonzero_constraints,
                                               fe.component_mask(velocities));
    }
    nonzero_constraints.close();

    {
      zero_constraints.clear();

      DoFTools::make_hanging_node_constraints(dof_handler, zero_constraints);
      VectorTools::interpolate_boundary_values(dof_handler,
                                               0,
                                               Functions::ZeroFunction<dim>(
                                                 dim + 1),
                                               zero_constraints,
                                               fe.component_mask(velocities));
    }
    zero_constraints.close();

    std::cout << "Number of active cells: " << triangulation.n_active_cells()
              << std::endl
              << "Number of degrees of freedom: " << dof_handler.n_dofs()
              << " (" << dof_u << " + " << dof_p << ')' << std::endl;
  }
// @sect4{StationaryNavierStokes::initialize_system}

// 在每个网格上，SparsityPattern和线性系统的大小是不同的。这个函数在网格细化后初始化它们。

  template <int dim>
  void StationaryNavierStokes<dim>::initialize_system()
  {
    {
      BlockDynamicSparsityPattern dsp(dofs_per_block, dofs_per_block);
      DoFTools::make_sparsity_pattern(dof_handler, dsp, nonzero_constraints);
      sparsity_pattern.copy_from(dsp);
    }

    system_matrix.reinit(sparsity_pattern);

    present_solution.reinit(dofs_per_block);
    newton_update.reinit(dofs_per_block);
    system_rhs.reinit(dofs_per_block);
  }
// @sect4{StationaryNavierStokes::assemble}

// 这个函数建立了我们目前工作的系统矩阵和右手边。 @p initial_step 参数用于确定我们应用哪一组约束（初始步骤为非零，其他为零）。 @p assemble_matrix 参数分别决定了是组装整个系统还是只组装右手边的向量。

  template <int dim>
  void StationaryNavierStokes<dim>::assemble(const bool initial_step,
                                             const bool assemble_matrix)
  {
    if (assemble_matrix)
      system_matrix = 0;

    system_rhs = 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();

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

    FullMatrix<double> local_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);

// 对于线性化系统，我们为当前速度和梯度以及当前压力创建临时存储。在实践中，它们都是通过正交点的形状函数获得的。

    std::vector<Tensor<1, dim>> present_velocity_values(n_q_points);
    std::vector<Tensor<2, dim>> present_velocity_gradients(n_q_points);
    std::vector<double>         present_pressure_values(n_q_points);

    std::vector<double>         div_phi_u(dofs_per_cell);
    std::vector<Tensor<1, dim>> phi_u(dofs_per_cell);
    std::vector<Tensor<2, dim>> grad_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_rhs    = 0;

        fe_values[velocities].get_function_values(evaluation_point,
                                                  present_velocity_values);

        fe_values[velocities].get_function_gradients(
          evaluation_point, present_velocity_gradients);

        fe_values[pressure].get_function_values(evaluation_point,
                                                present_pressure_values);

//装配类似于  step-22  。一个以gamma为系数的附加项是增强拉格朗日（AL），它是通过grad-div稳定化组装的。 正如我们在介绍中所讨论的，系统矩阵的右下块应该为零。由于压力质量矩阵是在创建预处理程序时使用的，所以我们在这里组装它，然后在最后把它移到一个单独的SparseMatrix中（与 step-22 相同）。

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

            for (unsigned int i = 0; i < dofs_per_cell; ++i)
              {
                if (assemble_matrix)
                  {
                    for (unsigned int j = 0; j < dofs_per_cell; ++j)
                      {
                        local_matrix(i, j) +=
                          (viscosity *
                             scalar_product(grad_phi_u[j], grad_phi_u[i]) +
                           present_velocity_gradients[q] * phi_u[j] * phi_u[i] +
                           grad_phi_u[j] * present_velocity_values[q] *
                             phi_u[i] -
                           div_phi_u[i] * phi_p[j] - phi_p[i] * div_phi_u[j] +
                           gamma * div_phi_u[j] * div_phi_u[i] +
                           phi_p[i] * phi_p[j]) *
                          fe_values.JxW(q);
                      }
                  }

                double present_velocity_divergence =
                  trace(present_velocity_gradients[q]);
                local_rhs(i) +=
                  (-viscosity * scalar_product(present_velocity_gradients[q],
                                               grad_phi_u[i]) -
                   present_velocity_gradients[q] * present_velocity_values[q] *
                     phi_u[i] +
                   present_pressure_values[q] * div_phi_u[i] +
                   present_velocity_divergence * phi_p[i] -
                   gamma * present_velocity_divergence * div_phi_u[i]) *
                  fe_values.JxW(q);
              }
          }

        cell->get_dof_indices(local_dof_indices);

        const AffineConstraints<double> &constraints_used =
          initial_step ? nonzero_constraints : zero_constraints;

        if (assemble_matrix)
          {
            constraints_used.distribute_local_to_global(local_matrix,
                                                        local_rhs,
                                                        local_dof_indices,
                                                        system_matrix,
                                                        system_rhs);
          }
        else
          {
            constraints_used.distribute_local_to_global(local_rhs,
                                                        local_dof_indices,
                                                        system_rhs);
          }
      }

    if (assemble_matrix)
      {

// 最后我们把压力质量矩阵移到一个单独的矩阵中。

        pressure_mass_matrix.reinit(sparsity_pattern.block(1, 1));
        pressure_mass_matrix.copy_from(system_matrix.block(1, 1));

// 注意，将这个压力块设置为零并不等同于不在这个块中装配任何东西，因为这里的操作将（错误地）删除从压力作用力的悬挂节点约束中进来的对角线条目。这意味着，我们的整个系统矩阵将有完全为零的行。幸运的是，FGMRES处理这些行没有任何问题。

        system_matrix.block(1, 1) = 0;
      }
  }

  template <int dim>
  void StationaryNavierStokes<dim>::assemble_system(const bool initial_step)
  {
    assemble(initial_step, true);
  }

  template <int dim>
  void StationaryNavierStokes<dim>::assemble_rhs(const bool initial_step)
  {
    assemble(initial_step, false);
  }
// @sect4{StationaryNavierStokes::solve}

// 在这个函数中，我们使用FGMRES和程序开始时定义的块状预处理程序来解决线性系统。我们在这一步得到的是解向量。如果这是初始步骤，解向量为我们提供了纳维尔-斯托克斯方程的初始猜测。对于初始步骤，非零约束被应用，以确保边界条件得到满足。在下面的步骤中，我们将求解牛顿更新，所以使用零约束。

  template <int dim>
  void StationaryNavierStokes<dim>::solve(const bool initial_step)
  {
    const AffineConstraints<double> &constraints_used =
      initial_step ? nonzero_constraints : zero_constraints;

    SolverControl solver_control(system_matrix.m(),
                                 1e-4 * system_rhs.l2_norm(),
                                 true);

    SolverFGMRES<BlockVector<double>> gmres(solver_control);
    SparseILU<double>                 pmass_preconditioner;
    pmass_preconditioner.initialize(pressure_mass_matrix,
                                    SparseILU<double>::AdditionalData());

    const BlockSchurPreconditioner<SparseILU<double>> preconditioner(
      gamma,
      viscosity,
      system_matrix,
      pressure_mass_matrix,
      pmass_preconditioner);

    gmres.solve(system_matrix, newton_update, system_rhs, preconditioner);
    std::cout << "FGMRES steps: " << solver_control.last_step() << std::endl;

    constraints_used.distribute(newton_update);
  }
// @sect4{StationaryNavierStokes::refine_mesh}

// 在粗略的网格上找到一个好的初始猜测后，我们希望通过细化网格来减少误差。这里我们做了类似于 step-15 的自适应细化，只是我们只使用了速度上的Kelly估计器。我们还需要使用SolutionTransfer类将当前的解转移到下一个网格。

  template <int dim>
  void StationaryNavierStokes<dim>::refine_mesh()
  {
    Vector<float> estimated_error_per_cell(triangulation.n_active_cells());
    FEValuesExtractors::Vector velocity(0);
    KellyErrorEstimator<dim>::estimate(
      dof_handler,
      QGauss<dim - 1>(degree + 1),
      std::map<types::boundary_id, const Function<dim> *>(),
      present_solution,
      estimated_error_per_cell,
      fe.component_mask(velocity));

    GridRefinement::refine_and_coarsen_fixed_number(triangulation,
                                                    estimated_error_per_cell,
                                                    0.3,
                                                    0.0);

    triangulation.prepare_coarsening_and_refinement();
    SolutionTransfer<dim, BlockVector<double>> solution_transfer(dof_handler);
    solution_transfer.prepare_for_coarsening_and_refinement(present_solution);
    triangulation.execute_coarsening_and_refinement();

// 首先，DoFHandler被设置，约束被生成。然后我们创建一个临时的BlockVector  tmp  ，其大小与新网格上的解决方案一致。

    setup_dofs();

    BlockVector<double> tmp(dofs_per_block);

// 将解决方案从粗网格转移到细网格，并对新转移的解决方案应用边界值约束。注意，present_solution仍然是对应于旧网格的一个向量。

    solution_transfer.interpolate(present_solution, tmp);
    nonzero_constraints.distribute(tmp);

// 最后设置矩阵和向量，并将present_solution设置为插值后的数据。

    initialize_system();
    present_solution = tmp;
  }
// @sect4{StationaryNavierStokes<dim>::newton_iteration}

// 这个函数实现了牛顿迭代，给定了公差、最大迭代次数和要做的网格细化次数。

// 参数 is_initial_step 告诉我们是否需要 setup_system ，以及应该装配哪一部分，系统矩阵或右手边的矢量。如果我们做直线搜索，在最后一次迭代中检查残差准则时，右手边已经被组装起来了。因此，我们只需要在当前迭代中装配系统矩阵。最后一个参数 output_result 决定了是否应该产生图形输出。

  template <int dim>
  void StationaryNavierStokes<dim>::newton_iteration(
    const double       tolerance,
    const unsigned int max_n_line_searches,
    const unsigned int max_n_refinements,
    const bool         is_initial_step,
    const bool         output_result)
  {
    bool first_step = is_initial_step;

    for (unsigned int refinement_n = 0; refinement_n < max_n_refinements + 1;
         ++refinement_n)
      {
        unsigned int line_search_n = 0;
        double       last_res      = 1.0;
        double       current_res   = 1.0;
        std::cout << "grid refinements: " << refinement_n << std::endl
                  << "viscosity: " << viscosity << std::endl;

        while ((first_step || (current_res > tolerance)) &&
               line_search_n < max_n_line_searches)
          {
            if (first_step)
              {
                setup_dofs();
                initialize_system();
                evaluation_point = present_solution;
                assemble_system(first_step);
                solve(first_step);
                present_solution = newton_update;
                nonzero_constraints.distribute(present_solution);
                first_step       = false;
                evaluation_point = present_solution;
                assemble_rhs(first_step);
                current_res = system_rhs.l2_norm();

                evaluation_point = present_solution;
                assemble_system(first_step);
                solve(first_step);

// 为了确保我们的解决方案越来越接近精确的解决方案，我们让解决方案用权重 alpha 更新，使新的残差小于上一步的残差，这是在下面的循环中完成。这与  step-15  中使用的线搜索算法相同。

                for (double alpha = 1.0; alpha > 1e-5; alpha *= 0.5)
                  {
                    evaluation_point = present_solution;
                    evaluation_point.add(alpha, newton_update);
                    nonzero_constraints.distribute(evaluation_point);
                    assemble_rhs(first_step);
                    current_res = system_rhs.l2_norm();
                    std::cout << "  alpha: " << std::setw(10) << alpha
                              << std::setw(0) << "  residual: " << current_res
                              << std::endl;
                    if (current_res < last_res)
                      break;
                  }
                {
                  present_solution = evaluation_point;
                  std::cout << "  number of line searches: " << line_search_n
                            << "  residual: " << current_res << std::endl;
                  last_res = current_res;
                }
                ++line_search_n;
              }

            if (output_result)
              {
                output_results(max_n_line_searches * refinement_n +
                               line_search_n);

                if (current_res <= tolerance)
                  process_solution(refinement_n);
              }
          }

        if (refinement_n < max_n_refinements)
          {
            refine_mesh();
          }
      }
  }
// @sect4{StationaryNavierStokes::compute_initial_guess}

// 这个函数将通过使用延续法为我们提供一个初始猜测，正如我们在介绍中讨论的那样。雷诺数被逐级增加 step- ，直到我们达到目标值。通过实验，斯托克斯的解足以成为雷诺数为1000的NSE的初始猜测，所以我们从这里开始。 为了确保前一个问题的解决方案与下一个问题足够接近，步长必须足够小。

  template <int dim>
  void StationaryNavierStokes<dim>::compute_initial_guess(double step_size)
  {
    const double target_Re = 1.0 / viscosity;

    bool is_initial_step = true;

    for (double Re = 1000.0; Re < target_Re;
         Re        = std::min(Re + step_size, target_Re))
      {
        viscosity = 1.0 / Re;
        std::cout << "Searching for initial guess with Re = " << Re
                  << std::endl;
        newton_iteration(1e-12, 50, 0, is_initial_step, false);
        is_initial_step = false;
      }
  }
// @sect4{StationaryNavierStokes::output_results}

// 这个函数与 step-22 中的函数相同，只是我们为输出文件选择了一个同时包含雷诺数（即当前环境下的粘度的倒数）的名称。

  template <int dim>
  void StationaryNavierStokes<dim>::output_results(
    const unsigned int output_index) 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(present_solution,
                             solution_names,
                             DataOut<dim>::type_dof_data,
                             data_component_interpretation);
    data_out.build_patches();

    std::ofstream output(std::to_string(1.0 / viscosity) + "-solution-" +
                         Utilities::int_to_string(output_index, 4) + ".vtk");
    data_out.write_vtk(output);
  }
// @sect4{StationaryNavierStokes::process_solution}

// 在我们的测试案例中，我们不知道分析解。该函数输出沿 $x=0.5$ 和 $0 \leq y \leq 1$ 的速度分量，以便与文献中的数据进行比较。

  template <int dim>
  void StationaryNavierStokes<dim>::process_solution(unsigned int refinement)
  {
    std::ofstream f(std::to_string(1.0 / viscosity) + "-line-" +
                    std::to_string(refinement) + ".txt");
    f << "# y u_x u_y" << std::endl;

    Point<dim> p;
    p(0) = 0.5;
    p(1) = 0.5;

    f << std::scientific;

    for (unsigned int i = 0; i <= 100; ++i)
      {
        p(dim - 1) = i / 100.0;

        Vector<double> tmp_vector(dim + 1);
        VectorTools::point_value(dof_handler, present_solution, p, tmp_vector);
        f << p(dim - 1);

        for (int j = 0; j < dim; j++)
          f << " " << tmp_vector(j);
        f << std::endl;
      }
  }
// @sect4{StationaryNavierStokes::run}

// 这是本程序的最后一步。在这一部分，我们分别生成网格和运行其他函数。最大细化度可以通过参数来设置。

  template <int dim>
  void StationaryNavierStokes<dim>::run(const unsigned int refinement)
  {
    GridGenerator::hyper_cube(triangulation);
    triangulation.refine_global(5);

    const double Re = 1.0 / viscosity;

// 如果粘度小于 $1/1000$ ，我们必须首先通过延续法搜索初始猜测。我们应该注意的是，搜索总是在初始网格上进行的，也就是这个程序中的 $8 \times 8$ 网格。之后，我们只需做与粘度大于 $1/1000$ 时相同的工作：运行牛顿迭代，细化网格，转移解决方案，并重复。

    if (Re > 1000.0)
      {
        std::cout << "Searching for initial guess ..." << std::endl;
        const double step_size = 2000.0;
        compute_initial_guess(step_size);
        std::cout << "Found initial guess." << std::endl;
        std::cout << "Computing solution with target Re = " << Re << std::endl;
        viscosity = 1.0 / Re;
        newton_iteration(1e-12, 50, refinement, false, true);
      }
    else
      {

// 当粘度大于1/1000时，斯托克斯方程的解作为初始猜测已经足够好。如果是这样，我们就不需要用延续法来搜索初始猜测了。牛顿迭代可以直接开始。

        newton_iteration(1e-12, 50, refinement, true, true);
      }
  }
} // namespace Step57

int main()
{
  try
    {
      using namespace Step57;

      StationaryNavierStokes<2> flow(/* degree = */

1);
      flow.run(4);
    }
  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
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  return 0;
}


/**
 * @file    sfm.hpp
 * @brief   This provides the implementation of structure-from-motion algorithm
 * @author  Shubham Shrivastava
 */

#ifndef SFM_H_
#define SFM_H_

#include <Eigen/Dense>
#include "tapl/common/common.hpp"
#include "tapl/optim/gaussNewton.hpp"
#include "tapl/viz/visualization.hpp"

namespace tapl {
    namespace cve {

        /**< Structure-from-Motion Pipeline */
        class StructureFromMotion {
        private:
            // vector of images
            std::vector<cv::Mat> images;
            // camera intrinsic matrix
            cv::Mat K = cv::Mat::eye(3, 3, CV_32FC1);
            // camera distortion matrix
            cv::Mat dist_coeff = cv::Mat::zeros(4, 1, CV_32FC1);
            // min and max XYZ
            std::vector<float> minXYZ;
            std::vector<float> maxXYZ;
            // verbose
            bool verbose;
            // triangulated 3D points in each image local coordinate systems
            std::vector<std::vector<tapl::Point3d>> points3d_local;
            // triangulated 3D points in the origin's (first image) coordinate systems
            std::vector<tapl::Point3d> points3d_global;
            // bundle size
            uint16_t bundle_size=2;
            // Gauss-Newton Optimizer
            tapl::optim::GaussNewtonOptimizer gnOptim;

            /**
             * @brief Linear estimate of the 3d point
             *
             * @param[in] point2d projection of the same 3d point in 'n' cameras
             * @param[in] projectionMatrices projection matrices of 'n' cameras
             *
             * @return estimated 3d point
             */
            tapl::Point3d linearEstimate3dPt( const std::vector<tapl::Point2d> &point2d,
                                              const std::vector<Eigen::MatrixXd> &projectionMatrices );

            /**
             * @brief Non-linear estimate of the 3d point
             *
             * @param[in] point2d projection of the same 3d point in 'n' cameras
             * @param[in] projectionMatrices projection matrices of 'n' cameras
             * @param[out] reprojectionErrors pair of pre-optimization and post-optimization
             *                                  reprojection errors
             * @param[in] nIterations maximum number of iterations for non-linear optimization
             * @param[in] reprErrorThresh reprojection error threshold for non-linear optimization
             *
             * @return estimated 3d point
             */
            tapl::Point3d nonLinearEstimate3dPt( const std::vector<tapl::Point2d> &point2d,
                                                 const std::vector<Eigen::MatrixXd> &projectionMatrices,
                                                 std::pair<std::vector<float>,std::vector<float>> &reprojectionErrors,
                                                 const uint16_t nIterations=1000,
                                                 const float reprErrorThresh=2.0 );

            /**
             * @brief Estimate R, T, and triengulated points
             *
             * @param[in] E essential matrix relating the first and the second camera
             * @param[in] points2d projection of the same 3d point in 'n' cameras
             * @param[in] projectionMat1 projection matrices of the first camera
             * @param[in] maxXYZ maximum values of x, y, and z to be considered
             * @param[in] maxReprojectionErr maximum absolute reprojection error in pixel
             *
             * @return pair of RT and triangulated points
             */
             std::pair<Eigen::MatrixXd, std::vector<tapl::Point3d>> computeSFM(
                                        const Eigen::MatrixXd &E,
                                        const std::vector<std::vector<tapl::Point2d>> &points2d,
                                        const Eigen::MatrixXd &projectionMat1,
                                        const float &maxReprojectionErr=50.0 ) ;

        public:
            /**
            * @brief This function initializes the structure-from-motion module
            *
            * @param[in] imgs images from which structure-from-motion is to be computed
            * @param[in] K camera intrinsic matrix
            */
            StructureFromMotion( const std::vector<cv::Mat> &images,
                                 const cv::Mat &K,
                                 const std::vector<float> &minXYZ={0.0, 0.0, 0.0},
                                 const std::vector<float> &maxXYZ={30.0, 30.0, 30.0},
                                 const bool verbose=true );

            /**
            * @brief This function performs structure-from-motion given a set of camera frames
            *
            * @param[out] points point-cloud corresponding to keypoints in the first camera's coordinate frame
            * @param[out] poses poses of each camera frame
            * @param[out] framePairs camera pairs with associated info such as triangulated points
            *
            * @return tapl::SUCCESS if success
            * @return tapl::FAILURE if failure
            */
            tapl::ResultCode process( std::vector<tapl::Point3dColor> &points,
                                      std::vector<tapl::Pose6dof> &poses,
                                      std::vector<tapl::CameraPairs> &framePairs);

        };
    }
}

#endif /* SFM_H_ */


#include <Eigen/Dense>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkPolyData.h>
#include <vtkDelaunay2D.h>
#include <vtkDoubleArray.h>
#include <vtkSmartPointer.h>
#include <vtkIdFilter.h>
#include <vtkPointData.h>

typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> Map3Xd;

int main(){
    clock_t t1;
    t1 = clock();
    for(auto i=0; i < 10000; ++i){
        vtkNew<vtkPolyDataReader> reader;
        reader->SetFileName("T7.vtk");
        reader->Update();
        auto poly = reader->GetOutput();
        auto N = poly->GetNumberOfPoints();
        auto pts = (double*) poly->GetPoints()->GetData()->GetVoidPointer(0);
        Map3Xd points(pts,3,N);

        // Project points to unit sphere
        points.colwise().normalize();

        // Reset the center of the sphere to origin by translating
        Vector3d center = points.rowwise().mean();
        points = points.colwise() - center;

        // Rotate all points so that the point in 0th column is along z-axis
        Vector3d c = points.col(0);
        double_t cos_t = c(2);
        double_t sin_t = std::sqrt( 1 - cos_t*cos_t );
        Vector3d axis;
        axis << c(1), -c(0), 0.;
        Matrix3d rotMat, axis_cross, outer;
        axis_cross << 0. , -axis(2), axis(1),
                        axis(2), 0., -axis(0),
                        -axis(1), axis(0), 0.;

        outer.noalias() = axis*axis.transpose();

        rotMat = cos_t*Matrix3d::Identity() + sin_t*axis_cross + (1-cos_t)*outer;
        Matrix3Xd rPts(3,N);
        rPts = rotMat*points; // The points on a sphere rotated

        // Calculate the stereographic projections
        Vector3d p0;
        Map3Xd l0( &(rPts(0,1)), 3, N-1 );
        Matrix3Xd l(3,N-1), proj(3,N-1);
        p0 << 0,0,-1;
        c = rPts.col(0);
        l = (l0.colwise() - c).colwise().normalized();
        for( auto j=0; j < N-1; ++j ){
            proj.col(j) = ((p0(2) - l0(2,j))/l(2,j))*l.col(j) + l0.col(j);
            proj(j,2) = 0.0;
        }
        // Calculate the 2d delaunay triangulations
        vtkNew<vtkDoubleArray> pts2dArr;
        pts2dArr->SetVoidArray((void*)proj.data(), 3*(N-1), 1);
        pts2dArr->SetNumberOfComponents(3);
        vtkNew<vtkPoints> pts2d;
        pts2d->SetData(pts2dArr);
        vtkNew<vtkPolyData> poly2d;
        poly2d->SetPoints(pts2d);
        vtkNew<vtkIdFilter> idf;
        idf->PointIdsOn();
        idf->SetIdsArrayName("OrigIds");
        idf->SetInputData(poly2d);
        vtkNew<vtkDelaunay2D> d2d;
        d2d->SetInputConnection(idf->GetOutputPort());
        d2d->Update();

        // Write the triangulation to file
        vtkNew<vtkCellArray> final;
        auto stereoTris = d2d->GetOutput()->GetPolys();
        auto idArr = d2d->GetOutput()->GetPointData()->GetArray("OrigIds");
        vtkNew<vtkIdList> idL;
        stereoTris->InitTraversal();
        while( stereoTris->GetNextCell(idL) ){
            final->InsertNextCell(3);
            for(auto j=0; j < idL->GetNumberOfIds(); ++j)
                final->InsertCellPoint( int(
                                        idArr->GetTuple1(idL->GetId(j))) );
        }

    }
    float diff((float)clock() - (float)t1);
    std::cout << "Time elapsed : " << diff / CLOCKS_PER_SEC
              << " seconds" << std::endl;
    return 0;
}