/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

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

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

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

#include <vector>
#include <limits>
#include <boost/algorithm/minmax_element.hpp>
#include <Core/Math/Histogram.h>
#include <Core/Math/MiscMath.h>

using namespace SCIRun::Core::Math;

namespace
{
  double Nan()
  {
    return std::numeric_limits<double>::quiet_NaN();
  }
}

Histogram::Histogram()
{
  this->min_ = Nan();
  this->max_ = Nan();
  this->bin_start_ = Nan();
  this->bin_size_ = Nan();
  this->histogram_.resize( 0 );
}

Histogram::Histogram( const double* data, size_t size )
{
  this->compute( data, size );
}

// For char and short data we do a single pass over the data to speed up the computation. The
// first pass is used to generate the histogram. In the next step we use this data to compute
// min and max
bool Histogram::compute( const double* data, size_t size )
{
  this->min_ = Nan();
  this->max_ = Nan();
  this->bin_start_ = Nan();
  this->bin_size_ = Nan();
  this->histogram_.resize( 0 );
  if ( size == 0 )
    return false;

  try
  {
    this->min_ = std::numeric_limits<double>::max();
    this->max_ = std::numeric_limits<double>::min();

    for ( size_t j = 0 ; j < size ; j++ )
    {
      double val = data[ j ];
      if ( ! IsFinite( val ) ) continue;
      if ( val < this->min_ ) this->min_ = val;
      if ( val > this->max_ ) this->max_ = val;
    }

    if ( this->min_ > this->max_ )
    {
      // Most likely all the data is NaN
      this->min_ = Nan();
      this->max_ = Nan();
      this->bin_start_ = Nan();
      this->bin_size_ = Nan();
      this->histogram_.resize( 0 );
      return false;
    }

    if ( this->min_ == this->max_ )
    {
      this->bin_size_  = 1.0;
      this->bin_start_ = this->min_ - ( this->bin_size_ * 0.5 );
      this->histogram_.resize( 1, 0 );
    }
    else
    {
      size_t hist_size = 0x100;
      this->histogram_.resize( hist_size, 0 );
      this->bin_size_ = ( this->max_ - this->min_ ) / static_cast<double>( hist_size - 1 );
      this->bin_start_ = this->min_ - ( this->bin_size_ * 0.5 );
    }

    double inv_bin_size = 1.0 / bin_size_;

    for ( size_t j = 1 ; j < size ; j++ )
    {
      double val = data[ j ];
      if ( IsFinite( val ) )
      {
        size_t idx = static_cast<size_t>( ( val - this->min_ ) * inv_bin_size );
        this->histogram_[ idx ]++;
      }
    }

    auto min_max = boost::minmax_element( this->histogram_.begin(), this->histogram_.end() );
    this->min_bin_ = (*min_max.first);
    this->max_bin_ = (*min_max.second);
  }
  catch( ... )
  {
    this->min_ = Nan();
    this->max_ = Nan();
    this->bin_start_ = Nan();
    this->bin_size_ = Nan();
    this->histogram_.resize( 0 );
    return false;
  }

  return true;
}

double Histogram::get_min() const
{
  return this->min_;
}

double Histogram::get_max() const
{
  return this->max_;
}

double Histogram::get_cum_value( double fraction ) const
{
  size_t tot_hist = 0;
  for ( size_t j = 0; j < this->histogram_.size(); j++ )
  {
    tot_hist += this->histogram_[ j ];
  }

  double multiplier = 1.0 / static_cast<double>( tot_hist );
  double jj = 0.0;
  size_t cur_hist = 0;

  for ( size_t j = 0; j < this->histogram_.size(); j++ )
  {
    double frac_start = static_cast<double>( cur_hist ) * multiplier;
    cur_hist += this->histogram_[ j ];
    double frac_end = static_cast<double>( cur_hist ) * multiplier;
    if ( fraction > frac_start && fraction <= frac_end )
    {
      jj = static_cast<double>( j ) + ( frac_end - fraction )/( frac_end - frac_start );
      break;
    }
  }

  return this->bin_start_ + ( jj * this->bin_size_ );
}

size_t Histogram::get_max_bin() const
{
  return this->max_bin_;
}

size_t Histogram::get_min_bin() const
{
  return this->min_bin_;
}

double Histogram::get_bin_size() const
{
  return this->bin_size_;
}

double Histogram::get_bin_start( size_t idx ) const
{
  return this->bin_start_ + idx * this->bin_size_;
}

double Histogram::get_bin_end( size_t idx ) const
{
  return this->bin_start_ + ( idx + 1 ) * this->bin_size_;
}

const std::vector<size_t>& Histogram::get_bins() const
{
  return this->histogram_;
}

size_t Histogram::get_size() const
{
  return this->histogram_.size();
}

bool Histogram::is_valid() const
{
  return !( IsNan( this->min_ ) || IsNan( this->max_ ) );
}


#include "nbsimParticle.h"
#include <Eigen/Dense>

namespace nbsim{
    Particle::Particle(Eigen::Vector3d init_position, Eigen::Vector3d init_velocity){
        current_position=init_position;
        current_velocity=init_velocity;
    }
    Particle::~Particle(){};
    Eigen::Vector3d Particle::getPosition(){
        return current_position;
    };
    Eigen::Vector3d Particle::getVelocity(){
        return current_velocity;
    };
    void Particle::integrateTimestep(Eigen::Vector3d acceleration, double timestep){
        current_position+=current_velocity*timestep;
        current_velocity+=acceleration*timestep;
    }
}


#include "catch.hpp"
#include <boost/icl/interval.hpp>
#include <boost/icl/interval_set.hpp>
#include <boost/icl/separate_interval_set.hpp>
#include <boost/optional.hpp>

TEST_CASE("interval tests", "[boost]") {
  using namespace boost::icl;

  SECTION("closed intervals") {
    auto ival = interval<int>::closed(1, 3);
    REQUIRE(ival.lower() == 1);
    REQUIRE(ival.upper() == 3);
  }

  SECTION("open intervals") {
    auto ival = interval<int>::open(1, 3);
    REQUIRE(ival.lower() == 1);
    REQUIRE(ival.upper() == 3);
  }

  SECTION("interval sets") {
    //interval sets contain exactly one interval at any time.
    //interval bounds expand based on addition of new intervals.
    auto iSet = interval_set<int>{};
    iSet += interval<int>::closed(1, 3);
    iSet += interval<int>::closed(2, 5); //upper bound expands to 5
    iSet += interval<int>::closed(0, 4); //lower bound expands to 0
    REQUIRE(contains(iSet, 0));
    REQUIRE(contains(iSet, 3));
    REQUIRE(contains(iSet, 5));
    REQUIRE_FALSE(contains(iSet, 7));
  }

  SECTION("separate interval sets") {
    auto siSet = separate_interval_set<int>{};
    siSet += interval<int>::closed(1, 3); //is now {(1,3)}
    siSet += interval<int>::closed(9, 10); //is now {(1,3), (9,10)}
    siSet += interval<int>::closed(2, 5); //is now {(1,5), (9,10)}

    REQUIRE(contains(siSet, 4));
    REQUIRE_FALSE(contains(siSet, 7));
    REQUIRE_FALSE(contains(siSet, 8));
    REQUIRE(contains(siSet, 9));

    REQUIRE(intersects(siSet, interval<int>::closed(2, 7)));
    REQUIRE_FALSE(intersects(siSet, interval<int>::closed(11, 12)));
    REQUIRE(intersects(siSet, interval<int>::closed(5, 6)));
    REQUIRE(intersects(siSet, interval<int>::closed(5, 9)));
    REQUIRE_FALSE(intersects(siSet, interval<int>::closed(7, 8)));
    REQUIRE(intersects(siSet, interval<int>::closed(7, 9)));
  }
}

TEST_CASE("optional test", "[boost]") {
  SECTION("optional construction") {
    auto optInt = boost::optional<int>{};
    REQUIRE_FALSE(optInt.is_initialized());
    optInt = 1;
    REQUIRE(optInt.is_initialized());
    REQUIRE(optInt.get() == 1);
  }

  SECTION("optional returning function") {
    auto positivePart = [](int num)->boost::optional<int> {
      return num > 0 ? num : boost::optional<int>{};
    };

    auto optVal = positivePart(5);
    REQUIRE(optVal.is_initialized());
    REQUIRE(optVal.get() == 5);
    REQUIRE_FALSE(positivePart(-5).is_initialized());
  }
}


/*
Copyright (c) 2012-2018, Visillect Service LLC. All rights reserved.
Developed for Kharkevich Institute for Information Transmission Problems of the
              Russian Academy of Sciences (IITP RAS).

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.

THIS SOFTWARE IS PROVIDED BY COPYRIGHT HOLDERS "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 COPYRIGHT HOLDERS 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.

The views and conclusions contained in the software and documentation are those
of the authors and should not be interpreted as representing official policies,
either expressed or implied, of copyright holders.
*/

#include <cassert>
#include <cmath>
#include <algorithm>

#include <colorseg/color_vertex.h>

THIRDPARTY_INCLUDES_BEGIN
#include <Eigen/Dense>
THIRDPARTY_INCLUDES_END

namespace vi { namespace colorseg {

inline Eigen::Vector3d homographyInv(Eigen::Vector3d const & src, double a, double k)
{
  double EPS = 1.e-5;
  assert(k > -EPS);
  assert(a > -EPS);
  assert(a < 1 + EPS);

  Eigen::Vector4d src_vec(src[0] / 255., src[1] / 255., src[2] / 255., 1);

  Eigen::Matrix<double, 4, 4> H = Eigen::MatrixXd::Identity(4, 4);
  for (int i = 0; i < 3; ++i)
    H(3, i) = k;

  Eigen::Matrix<double, 4, 4> S = Eigen::MatrixXd::Identity(4, 4);
  for (int i = 0; i < 3; ++i)
    S(i, i) = k + 1;

  Eigen::Matrix<double, 4, 4> A = Eigen::MatrixXd::Identity(4, 4);
  for (int i = 0; i < 3; ++i)
    for (int j = 0; j < 3; ++j)
        if (i != j)
            A(i, j) = a;

  Eigen::Matrix<double, 4, 4> P = A * S * H;
  Eigen::Vector4d dst_vec = P.inverse() * src_vec;

  if (std::abs(dst_vec[3]) >= EPS)
  {
    for (int i = 0; i < 3; i++)
      dst_vec[i] = (dst_vec[i] / dst_vec[3]) * 255;
  }

  return Eigen::Vector3d(dst_vec[0], dst_vec[1], dst_vec[2]);
}

inline void homography(double * dst, uint8_t const * src, double a, double k)
{
  double EPS = 1.e-5;
  assert(k > -EPS);
  assert(a > -EPS);
  assert(a < 1 + EPS);

  Eigen::Vector4d src_vec((double)src[0] / 255., (double)src[1] / 255., (double)src[2] / 255., 1);

  Eigen::Matrix<double, 4, 4> H = Eigen::MatrixXd::Identity(4, 4);
  for (int i = 0; i < 3; ++i)
    H(3, i) = k;

  Eigen::Matrix<double, 4, 4> S = Eigen::MatrixXd::Identity(4, 4);
  for (int i = 0; i < 3; ++i)
    S(i, i) = k + 1;

  Eigen::Matrix<double, 4, 4> A = Eigen::MatrixXd::Identity(4, 4);
  for (int i = 0; i < 3; ++i)
    for (int j = 0; j < 3; ++j)
        if (i != j)
            A(i, j) = a;

  Eigen::Matrix<double, 4, 4> P = A * S * H;
  Eigen::Vector4d dst_vec = P * src_vec;

  if (std::abs(dst_vec[3]) >= EPS)
  {
    for (int i = 0; i < 3; i++)
      dst_vec[i] = (dst_vec[i] / dst_vec[3]) * 255;
  }

  dst[0] = dst_vec[0];
  dst[1] = dst_vec[1];
  dst[2] = dst_vec[2];
}

}} // ns vi::colorseg


 #ifndef REDDM_H
#define REDDM_H
#include"basis.hpp"
#include <Eigen/Dense>
#include"diag.h"
namespace Many_Body{
  template<typename T>
  using MatrixD =Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
  template<typename T>
  using VectorD =Eigen::Matrix<T,  Eigen::Dynamic, 1>;
  template<typename T>
  inline T conj(T a)
  {return a;}

    template<typename T>
  inline std::complex< T > conj(std::complex< T > a)
  { std::complex< T > b(real(a), -imag(a));
    return b;}

  template<typename TotalBasis, typename Matrix, typename Vector>
  std::vector<Eigen::VectorXd> makeThermalRDMTP(Matrix& H, Vector& ev, TotalBasis& TP, double T, bool isDiag=false, int site=0)
{
  double beta=1./T;
  if(!isDiag)
    {
  diagMat(H, ev);
    }
  std::cout<< "GS "<< ev[0]<<std::endl;
  Eigen::MatrixXd rho=Eigen::MatrixXd::Zero(H.rows(), H.rows());
  double Z{0};
  //  std::cout<< ev << std::endl;
  for(int i=0; i<rho.rows(); i++)
    {
      rho(i, i)=std::exp(-beta*ev(i));
      Z+=std::exp(-beta*ev(i));
    }
  std::cout<< "Z "<<std::setprecision(9)<< Z << std::endl;
  rho/=Z;
  //  std::cout<< rho<< std::endl;

  rho=rho*H.adjoint();
  rho=H*rho;
  std::vector<Eigen::VectorXd> evs;
  auto RDM= makeRedDMTP(TP, TP.rbasis, site, rho);
  Eigen::MatrixXd S=Eigen::MatrixXd::Zero(RDM[0].rows(), RDM[0].rows());
  for(auto& M : RDM){

  Eigen::VectorXd evRDM=Eigen::VectorXd::Zero(M.rows());
  std::cout<< "matri x"<< std::endl;
  std::cout<< M<< std::endl;
  S+=M;
  std::cout<< std::endl;
  diagMat(M, evRDM);
  evs.push_back(evRDM);

  }
    std::cout<< "SUM OF all"<< std::endl;
  std::cout<< S<< std::endl;
    std::cout<< ""<< std::endl;

  return evs;
}
template<typename Basis, typename T>
void makeRedDM(Basis& basis, int site, MatrixD<T>& rho )
{
  size_t dim=basis.maxParticles+1;
  MatrixD<T> DM=MatrixD<T>::Zero(dim, dim);
  for(auto it= basis.begin(); it!=basis.end(); ++it)
    {
      //      std::cout<< " loop 1 "<<std::endl;
      auto currentID1 =Id(*it);
      auto p1=basis.particlesAt(currentID1, site);
      DM(p1, p1)+=rho(Position(*it), Position(*it));
      auto itx=it;
      itx++;
      for(auto it2= itx; it2!=basis.end(); ++it2)
    {
      //            std::cout<< " loop 2 "<<std::endl;

      auto currentID2 =Id(*it2);
       auto state1=GetLattice(*it).makeStateVec();
       auto state2=GetLattice(*it2).makeStateVec();
       auto p2=basis.particlesAt(currentID2, site);
       state1.erase(state1.begin()+site);
       state2.erase(state2.begin()+site);
       if(state1==state2)
	 {
      DM(p1, p2)+=rho(Position(*it), Position(*it2));
      DM(p2, p1)+=rho(Position(*it2), Position(*it));
      // std::cout << currentID1 << " part a 1 " << p1 << std::endl;
      //       std::cout << currentID2 << " part a 2 " << p2 << std::endl;
	 }
    }
    }

  std::cout<< DM << std::endl;
}
  template<typename Basis, typename T>
  void makeRedDM(Basis& basis, int site, VectorD<T>& psi)
{
  size_t dim=basis.maxParticles+1;

  MatrixD<T> DM=MatrixD<T>::Zero(dim, dim);
  for(auto it= basis.begin(); it!=basis.end(); ++it)
    {
      //      std::cout<< " loop 1 "<<std::endl;
      auto currentID1 =Id(*it);
      auto p1=basis.particlesAt(currentID1, site);
      DM(p1, p1)+=Many_Body::conj(static_cast<T>((psi(Position(*it)))))*(psi(Position(*it)));
	      //rho(Position(*it), Position(*it));
      auto itx=it;
      itx++;
      for(auto it2= itx; it2!=basis.end(); ++it2)
    {
      //            std::cout<< " loop 2 "<<std::endl;

      auto currentID2 =Id(*it2);
       auto state1=GetLattice(*it).makeStateVec();
       auto state2=GetLattice(*it2).makeStateVec();
       auto p2=basis.particlesAt(currentID2, site);
       state1.erase(state1.begin()+site);
       state2.erase(state2.begin()+site);
       if(state1==state2)
	 {
	   DM(p1, p2)+=Many_Body::conj(static_cast<T>((psi(Position(*it2)))))*(psi(Position(*it)));
	//rho(Position(*it), Position(*it2));

	   DM(p2, p1)+=Many_Body::conj(static_cast<T>((psi(Position(*it)))))*(psi(Position(*it2)));
	//rho(Position(*it2), Position(*it));
      // std::cout << currentID1 << " part a 1 " << p1 << std::endl;
      //       std::cout << currentID2 << " part a 2 " << p2 << std::endl;
	 }
    }
    }

  std::cout<< DM << std::endl;
}

template<typename TotalBasis, class SubBasis, typename T>
std::vector<MatrixD<T>> makeRedDMTP(const TotalBasis& totalBasis, const SubBasis& subBasis, int site, MatrixD<T>& rho )
{
  // making reduced DM for each degree of freedom in subasis L

       std::vector<MatrixD<T>> mats(totalBasis.lbasis.maxParticles+1);
       size_t dim=(totalBasis.rbasis.maxParticles+1);

       std::fill(mats.begin(), mats.end(), MatrixD<T>::Zero(dim, dim));

       //  std::cout<< dim << std::endl;

  for(auto it= totalBasis.begin(); it!=totalBasis.end(); ++it)
    {
      auto itL=totalBasis.lbasis.find(LeftId(*it));
      auto itR=totalBasis.rbasis.find(RightId(*it));

      //      std::cout<< " loop 1 "<<std::endl;
       auto currentID1L =Id(*itL);
       auto currentID1R =Id(*itR);
       auto p1L=totalBasis.lbasis.particlesAt(currentID1L, site);
       auto p1R=totalBasis.rbasis.particlesAt(currentID1R, site);
       // unsafe way to determine the position in the matrix
       // upper first fix electron then iterate boson
       size_t newPos= p1R;

       //       if(std::abs(rho(Position(*it), Position(*it)))>0.001){std::cout<< " her "<< p1L<< "  " << p1R << std::endl;}
       auto itx=it;
       // itx++;
       for(auto it2= itx; it2!=totalBasis.end(); ++it2)
     {
         auto it2L=totalBasis.lbasis.find(LeftId(*it2));
      auto it2R=totalBasis.rbasis.find(RightId(*it2));

      //      std::cout<< " loop 1 "<<std::endl; // re think this one
       auto currentID2L =Id(*it2L);
       auto currentID2R =Id(*it2R);
       auto p2L=totalBasis.lbasis.particlesAt(currentID2L, site);
       auto p2R=totalBasis.rbasis.particlesAt(currentID2R, site);
       size_t newPos2=p2R;
       // making the delta function
       auto state1L=GetLattice(*itL).makeStateVec();
       auto state1R=GetLattice(*itR).makeStateVec();
       auto state2L=GetLattice(*it2L).makeStateVec();
       auto state2R=GetLattice(*it2R).makeStateVec();
       state1L.erase(state1L.begin()+site);
       state2L.erase(state2L.begin()+site);
       state1R.erase(state1R.begin()+site);
       state2R.erase(state2R.begin()+site);
       if(state1L==state2L and state1R==state2R)
	 {
	   //if(p2L!=p1L){std::cout << "here "<< std::endl;}
	   if(newPos!=newPos2){
       mats[p2L](newPos, newPos2)+=rho(Position(*it), Position(*it2));
       mats[p2L](newPos2, newPos)+=rho(Position(*it2), Position(*it));
	   }
	   else{
	     mats[p1L](newPos, newPos)+=rho(Position(*it), Position(*it2));
	   }
	 }
         }
}
  return mats;
}

  template<typename TotalBasis, class SubBasis, typename T>
std::vector<MatrixD<T>> makeRedDMTP(const TotalBasis& totalBasis, const SubBasis& subBasis, int site, VectorD<T>& psi )
{
  // making reduced DM for each degree of freedom in subasis L

       std::vector<MatrixD<T>> mats(totalBasis.lbasis.maxParticles+1);
       size_t dim=(totalBasis.rbasis.maxParticles+1);

       std::fill(mats.begin(), mats.end(), MatrixD<T>::Zero(dim, dim));

       //  std::cout<< dim << std::endl;

  for(auto it= totalBasis.begin(); it!=totalBasis.end(); ++it)
    {
      auto itL=totalBasis.lbasis.find(LeftId(*it));
      auto itR=totalBasis.rbasis.find(RightId(*it));

      //      std::cout<< " loop 1 "<<std::endl;
       auto currentID1L =Id(*itL);
       auto currentID1R =Id(*itR);
       auto p1L=totalBasis.lbasis.particlesAt(currentID1L, site);
       auto p1R=totalBasis.rbasis.particlesAt(currentID1R, site);
       // unsafe way to determine the position in the matrix
       // upper first fix electron then iterate boson
       size_t newPos= p1R;
       mats[p1L](newPos, newPos)+=Many_Body::conj(static_cast<T>((psi(Position(*it)))))*(psi(Position(*it)));

       //       if(std::abs(rho(Position(*it), Position(*it)))>0.001){std::cout<< " her "<< p1L<< "  " << p1R << std::endl;}
       auto itx=it;
       itx++;
       for(auto it2= itx; it2!=totalBasis.end(); ++it2)
     {
         auto it2L=totalBasis.lbasis.find(LeftId(*it2));
      auto it2R=totalBasis.rbasis.find(RightId(*it2));

      //      std::cout<< " loop 1 "<<std::endl;
       auto currentID2L =Id(*it2L);
       auto currentID2R =Id(*it2R);
       auto p2L=totalBasis.lbasis.particlesAt(currentID2L, site);
       auto p2R=totalBasis.rbasis.particlesAt(currentID2R, site);
       size_t newPos2=p2R;
       // making the delta function
       auto state1L=GetLattice(*itL).makeStateVec();
       auto state1R=GetLattice(*itR).makeStateVec();
       auto state2L=GetLattice(*it2L).makeStateVec();
       auto state2R=GetLattice(*it2R).makeStateVec();
       state1L.erase(state1L.begin()+site);
       state2L.erase(state2L.begin()+site);
       state1R.erase(state1R.begin()+site);
       state2R.erase(state2R.begin()+site);
       if(state1L==state2L and state1R==state2R)
	 {
	   mats[p2L](newPos, newPos2)+=Many_Body::conj(static_cast<T>((psi(Position(*it2)))))*(psi(Position(*it)));

	 //rho(Position(*it), Position(*it2));
	   mats[p2L](newPos2, newPos)+=Many_Body::conj(static_cast<T>((psi(Position(*it)))))*(psi(Position(*it2)));
	 //rho(Position(*it2), Position(*it));

	 }
         }
}
  return mats;
}

}

#endif /* REDDM_H */


/* ---------------------------------------------------------------------
 *
 * 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: Daniel Arndt, Matthias Maier, 2015
 *
 * Based on step-22 by Wolfgang Bangerth and Martin Kronbichler
 */

// 这个例子程序是对 step-22 的轻微修改，使用Trilinos并行运行，以演示交易.II中周期性边界条件的使用。因此我们不讨论大部分的源代码，只对处理周期性约束的部分进行评论。其余的请看 step-22 和底部的完整源代码。

// 为了实现周期性边界条件，只有两个函数需要修改。

// -  StokesProblem<dim>::setup_dofs()  : 用周期性约束来填充AffineConstraints对象

// -  StokesProblem<dim>::create_mesh()  : 为分布式三角形提供周期性信息。

// 程序的其余部分与 step-22 相同，所以让我们跳过这一部分，只在下面显示这两个函数。完整的程序可以在下面的 "普通程序 "部分找到）。

//  @cond  跳过

#include <deal.II/base/conditional_ostream.h>

#include <deal.II/distributed/grid_refinement.h>

#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/affine_constraints.h>

#include <deal.II/lac/trilinos_solver.h>
#include <deal.II/lac/trilinos_precondition.h>
#include <deal.II/lac/trilinos_block_sparse_matrix.h>
#include <deal.II/lac/trilinos_parallel_block_vector.h>
#include <deal.II/lac/block_sparsity_pattern.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_tools.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/mapping_q.h>

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

namespace Step45
{
  using namespace dealii;

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

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

    const unsigned int degree;

    MPI_Comm mpi_communicator;

    parallel::distributed::Triangulation<dim> triangulation;
    FESystem<dim>                             fe;
    DoFHandler<dim>                           dof_handler;

    AffineConstraints<double> constraints;
    std::vector<IndexSet>     owned_partitioning;
    std::vector<IndexSet>     relevant_partitioning;

    TrilinosWrappers::BlockSparseMatrix system_matrix;

    TrilinosWrappers::BlockSparseMatrix preconditioner_matrix;

    TrilinosWrappers::MPI::BlockVector solution;
    TrilinosWrappers::MPI::BlockVector system_rhs;

    ConditionalOStream pcout;

    MappingQ<dim> mapping;
  };

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

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

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

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

    return 0;
  }

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

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

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

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

  template <int dim>
  double RightHandSide<dim>::value(const Point<dim> & p,
                                   const unsigned int component) const
  {
    const Point<dim> center(0.75, 0.1);
    const double     r = (p - center).norm();

    if (component == 0)
      return std::exp(-100. * r * r);
    return 0;
  }

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

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

    void vmult(TrilinosWrappers::MPI::Vector &      dst,
               const TrilinosWrappers::MPI::Vector &src) const;

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

    const MPI_Comm *                      mpi_communicator;
    mutable TrilinosWrappers::MPI::Vector tmp;
  };

  template <class MatrixType, class PreconditionerType>
  InverseMatrix<MatrixType, PreconditionerType>::InverseMatrix(
    const MatrixType &        m,
    const PreconditionerType &preconditioner,
    const IndexSet &          locally_owned,
    const MPI_Comm &          mpi_communicator)
    : matrix(&m)
    , preconditioner(&preconditioner)
    , mpi_communicator(&mpi_communicator)
    , tmp(locally_owned, mpi_communicator)
  {}

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

    tmp = 0.;
    cg.solve(*matrix, tmp, src, *preconditioner);
    dst = tmp;
  }

  template <class PreconditionerType>
  class SchurComplement : public TrilinosWrappers::SparseMatrix
  {
  public:
    SchurComplement(const TrilinosWrappers::BlockSparseMatrix &system_matrix,
                    const InverseMatrix<TrilinosWrappers::SparseMatrix,
                                        PreconditionerType> &  A_inverse,
                    const IndexSet &                           owned_pres,
                    const MPI_Comm &mpi_communicator);

    void vmult(TrilinosWrappers::MPI::Vector &      dst,
               const TrilinosWrappers::MPI::Vector &src) const;

  private:
    const SmartPointer<const TrilinosWrappers::BlockSparseMatrix> system_matrix;
    const SmartPointer<
      const InverseMatrix<TrilinosWrappers::SparseMatrix, PreconditionerType>>
                                          A_inverse;
    mutable TrilinosWrappers::MPI::Vector tmp1, tmp2;
  };

  template <class PreconditionerType>
  SchurComplement<PreconditionerType>::SchurComplement(
    const TrilinosWrappers::BlockSparseMatrix &system_matrix,
    const InverseMatrix<TrilinosWrappers::SparseMatrix, PreconditionerType>
      &             A_inverse,
    const IndexSet &owned_vel,
    const MPI_Comm &mpi_communicator)
    : system_matrix(&system_matrix)
    , A_inverse(&A_inverse)
    , tmp1(owned_vel, mpi_communicator)
    , tmp2(tmp1)
  {}

  template <class PreconditionerType>
  void SchurComplement<PreconditionerType>::vmult(
    TrilinosWrappers::MPI::Vector &      dst,
    const TrilinosWrappers::MPI::Vector &src) const
  {
    system_matrix->block(0, 1).vmult(tmp1, src);
    A_inverse->vmult(tmp2, tmp1);
    system_matrix->block(1, 0).vmult(dst, tmp2);
  }

  template <int dim>
  StokesProblem<dim>::StokesProblem(const unsigned int degree)
    : degree(degree)
    , mpi_communicator(MPI_COMM_WORLD)
    , triangulation(mpi_communicator)
    , fe(FE_Q<dim>(degree + 1), dim, FE_Q<dim>(degree), 1)
    , dof_handler(triangulation)
    , pcout(std::cout, Utilities::MPI::this_mpi_process(mpi_communicator) == 0)
    , mapping(degree + 1)
  {}
// @endcond
// @sect3{Setting up periodicity constraints on distributed triangulations}
  template <int dim>
  void StokesProblem<dim>::create_mesh()
  {
    Point<dim>   center;
    const double inner_radius = .5;
    const double outer_radius = 1.;

    GridGenerator::quarter_hyper_shell(
      triangulation, center, inner_radius, outer_radius, 0, true);

// 在我们可以规定周期性约束之前，我们需要确保位于域的对面但由周期性面连接的单元是幽灵层的一部分，如果其中一个单元存储在本地处理器上。在这一点上，我们需要考虑我们要如何规定周期性。左边边界上的面的顶点 $\text{vertices}_2$ 应该与下面边界上的面的顶点 $\text{vertices}_1$ 相匹配，由 $\text{vertices}_2=R\cdot \text{vertices}_1+b$ 给出，其中旋转矩阵 $R$ 和偏移量 $b$ 由
//  @f{align*}
//  R=\begin{pmatrix}
//  0&1\\-1&0
//  \end{pmatrix},
//  \quad
//  b=\begin{pmatrix}0&0\end{pmatrix}.
//  @f}
//  给出。 我们将所得信息保存到这里的数据结构是基于三角结构的。

    std::vector<GridTools::PeriodicFacePair<
      typename parallel::distributed::Triangulation<dim>::cell_iterator>>
      periodicity_vector;

    FullMatrix<double> rotation_matrix(dim);
    rotation_matrix[0][1] = 1.;
    rotation_matrix[1][0] = -1.;

    GridTools::collect_periodic_faces(triangulation,
                                      2,
                                      3,
                                      1,
                                      periodicity_vector,
                                      Tensor<1, dim>(),
                                      rotation_matrix);

// 现在，只要调用 parallel::distributed::Triangulation::add_periodicity. 就可以告诉三角函数所需的周期性，特别容易。
    triangulation.add_periodicity(periodicity_vector);

    triangulation.refine_global(4 - dim);
  }

  template <int dim>
  void StokesProblem<dim>::setup_dofs()
  {
    dof_handler.distribute_dofs(fe);

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

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

    {
      owned_partitioning.clear();
      IndexSet locally_owned_dofs = dof_handler.locally_owned_dofs();
      owned_partitioning.push_back(locally_owned_dofs.get_view(0, n_u));
      owned_partitioning.push_back(locally_owned_dofs.get_view(n_u, n_u + n_p));

      relevant_partitioning.clear();
      IndexSet locally_relevant_dofs;
      DoFTools::extract_locally_relevant_dofs(dof_handler,
                                              locally_relevant_dofs);
      relevant_partitioning.push_back(locally_relevant_dofs.get_view(0, n_u));
      relevant_partitioning.push_back(
        locally_relevant_dofs.get_view(n_u, n_u + n_p));

      constraints.clear();
      constraints.reinit(locally_relevant_dofs);

      FEValuesExtractors::Vector velocities(0);

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

// 在我们为网格提供了周期性约束的必要信息后，我们现在可以实际创建它们。对于描述匹配，我们使用与之前相同的方法，也就是说，左边边界上的一个面的 $\text{vertices}_2$ 应该与下面边界上的一个面的顶点 $\text{vertices}_1$ 匹配，由 $\text{vertices}_2=R\cdot \text{vertices}_1+b$  ]，其中旋转矩阵 $R$ 和偏移量 $b$ 由
// @f{align*}
//  R=\begin{pmatrix}
//  0&1\\-1&0
//  \end{pmatrix},
//  \quad
//  b=\begin{pmatrix}0&0\end{pmatrix}.
//  @f}
//  给出。 这两个对象不仅描述了应该如何匹配面，而且还描述了解决方案应该从 $\text{face}_2$ 转换到 $\text{face}_1$ 的意义。

      FullMatrix<double> rotation_matrix(dim);
      rotation_matrix[0][1] = 1.;
      rotation_matrix[1][0] = -1.;

      Tensor<1, dim> offset;

// 为了设置约束，我们首先将周期性信息存储在一个类型为 std::vector@<GridTools::PeriodicFacePair<typename 的辅助对象中。
// DoFHandler@<dim@>::%cell_iterator@>  。周期性边界的边界指标为2（x=0）和3（y=0）。所有其他的参数我们之前已经设置好了。在这种情况下，方向并不重要。由于 $\text{vertices}_2=R\cdot \text{vertices}_1+b$ 这正是我们想要的。

      std::vector<
        GridTools::PeriodicFacePair<typename DoFHandler<dim>::cell_iterator>>
        periodicity_vector;

      const unsigned int direction = 1;

      GridTools::collect_periodic_faces(dof_handler,
                                        2,
                                        3,
                                        direction,
                                        periodicity_vector,
                                        offset,
                                        rotation_matrix);

// 接下来，我们需要提供关于解决方案中哪些矢量值分量应该被旋转的信息。由于我们在这里选择只约束速度，并且从解决方案矢量的第一个分量开始，我们只需插入一个0。

      std::vector<unsigned int> first_vector_components;
      first_vector_components.push_back(0);

// 在设置了周期性_vector中的所有信息之后，我们要做的就是告诉make_periodicity_constraints来创建所需的约束。

      DoFTools::make_periodicity_constraints<dim, dim>(periodicity_vector,
                                                       constraints,
                                                       fe.component_mask(
                                                         velocities),
                                                       first_vector_components);

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

    constraints.close();

    {
      TrilinosWrappers::BlockSparsityPattern bsp(owned_partitioning,
                                                 owned_partitioning,
                                                 relevant_partitioning,
                                                 mpi_communicator);

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

      DoFTools::make_sparsity_pattern(dof_handler,
                                      coupling,
                                      bsp,
                                      constraints,
                                      false,
                                      Utilities::MPI::this_mpi_process(
                                        mpi_communicator));

      bsp.compress();

      system_matrix.reinit(bsp);
    }

    {
      TrilinosWrappers::BlockSparsityPattern preconditioner_bsp(
        owned_partitioning,
        owned_partitioning,
        relevant_partitioning,
        mpi_communicator);

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

      DoFTools::make_sparsity_pattern(dof_handler,
                                      preconditioner_coupling,
                                      preconditioner_bsp,
                                      constraints,
                                      false,
                                      Utilities::MPI::this_mpi_process(
                                        mpi_communicator));

      preconditioner_bsp.compress();

      preconditioner_matrix.reinit(preconditioner_bsp);
    }

    system_rhs.reinit(owned_partitioning, mpi_communicator);
    solution.reinit(owned_partitioning,
                    relevant_partitioning,
                    mpi_communicator);
  }

// 然后程序的其余部分又与  step-22  相同。我们现在省略它，但和以前一样，你可以在下面的 "普通程序 "部分找到这些部分。

//  @cond  SKIP

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

    QGauss<dim> quadrature_formula(degree + 2);

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

    const unsigned int dofs_per_cell = fe.n_dofs_per_cell();

    const unsigned int n_q_points = quadrature_formula.size();

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

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

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

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

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

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

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

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

              for (unsigned int i = 0; i < dofs_per_cell; ++i)
                {
                  for (unsigned int j = 0; j <= i; ++j)
                    {
                      local_matrix(i, j) +=
                        (symgrad_phi_u[i] * symgrad_phi_u[j] // diffusion
                         - div_phi_u[i] * phi_p[j]           // pressure force
                         - phi_p[i] * div_phi_u[j])          // divergence
                        * fe_values.JxW(q);

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

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

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

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

    system_matrix.compress(VectorOperation::add);
    system_rhs.compress(VectorOperation::add);

    pcout << "   Computing preconditioner..." << std::endl << std::flush;
  }

  template <int dim>
  void StokesProblem<dim>::solve()
  {
    TrilinosWrappers::PreconditionJacobi A_preconditioner;
    A_preconditioner.initialize(system_matrix.block(0, 0));

    const InverseMatrix<TrilinosWrappers::SparseMatrix,
                        TrilinosWrappers::PreconditionJacobi>
      A_inverse(system_matrix.block(0, 0),
                A_preconditioner,
                owned_partitioning[0],
                mpi_communicator);

    TrilinosWrappers::MPI::BlockVector tmp(owned_partitioning,
                                           mpi_communicator);

      TrilinosWrappers::MPI::Vector schur_rhs(owned_partitioning[1],
                                              mpi_communicator);
      A_inverse.vmult(tmp.block(0), system_rhs.block(0));
      system_matrix.block(1, 0).vmult(schur_rhs, tmp.block(0));
      schur_rhs -= system_rhs.block(1);

      SchurComplement<TrilinosWrappers::PreconditionJacobi> schur_complement(
        system_matrix, A_inverse, owned_partitioning[0], mpi_communicator);

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

      TrilinosWrappers::PreconditionAMG preconditioner;
      preconditioner.initialize(preconditioner_matrix.block(1, 1));

      InverseMatrix<TrilinosWrappers::SparseMatrix,
                    TrilinosWrappers::PreconditionAMG>
        m_inverse(preconditioner_matrix.block(1, 1),
                  preconditioner,
                  owned_partitioning[1],
                  mpi_communicator);

      cg.solve(schur_complement, tmp.block(1), schur_rhs, preconditioner);

      constraints.distribute(tmp);
      solution.block(1) = tmp.block(1);
    }

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

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

      constraints.distribute(tmp);
      solution.block(0) = tmp.block(0);
    }
  }

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

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

    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution,
                             solution_names,
                             DataOut<dim>::type_dof_data,
                             data_component_interpretation);
    Vector<float> subdomain(triangulation.n_active_cells());
    for (unsigned int i = 0; i < subdomain.size(); ++i)
      subdomain(i) = triangulation.locally_owned_subdomain();
    data_out.add_data_vector(subdomain, "subdomain");
    data_out.build_patches(mapping, degree + 1);

    data_out.write_vtu_with_pvtu_record(
      "./", "solution", refinement_cycle, MPI_COMM_WORLD, 2);
  }

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

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

    parallel::distributed::GridRefinement::refine_and_coarsen_fixed_number(
      triangulation, estimated_error_per_cell, 0.3, 0.0);
    triangulation.execute_coarsening_and_refinement();
  }

  template <int dim>
  void StokesProblem<dim>::run()
  {
    create_mesh();

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

        if (refinement_cycle > 0)
          refine_mesh();

        setup_dofs();

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

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

        output_results(refinement_cycle);

        pcout << std::endl;
      }
  }
} // namespace Step45

int main(int argc, char *argv[])
{
  try
    {
      using namespace dealii;
      using namespace Step45;

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

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

  return 0;
}
// @endcond


// CANONICALSTRUCTURE class for General DMPs
//   Let x spread in time
//   D.E Argiropoulos - General DMP - June 2020

#include <iostream>
#include <string>
#include <armadillo>

using namespace std;
using namespace arma;

class CanonicalStructure
{
public:
    double ax , x0, Rx0;
    vec x, dx;
    vec Fx, fx;
    vec Rdx, Rx;
    double t0, tf, T;
    double taf , dtaf ;

    CanonicalStructure(int len);
    ~CanonicalStructure();
    void generateFx(std::string kind, vec timed);
    double getFx(double t);
    void generateRfx(string kind, vec timed);

};

CanonicalStructure::CanonicalStructure(int len)
{

  x.set_size(len);
  dx.set_size(len);
  Fx.set_size(len);
  fx.set_size(len);
  Rx.set_size(len);
  Rdx.set_size(len);

  std::cout << "/* Constructor of CanonicalStructure */" << '\n';
}
CanonicalStructure::~CanonicalStructure()
{
}

void CanonicalStructure::generateFx(string kind, vec timed)
{
  int len = timed.n_rows;
  x0 = 0.01;
  t0 = 0;
  ax = 1/2;
  T = timed[len-1]; // starts from 0sec - (len-1)*ts
  tf = T + t0;
  taf = 1;
  dtaf = 0;
  if ( kind == "discrete")
  {
    //CS for discrete
    for (int i = 0; i<len; i++)
    {
      Fx[i] =  ax*timed[i];
      fx[i] = ax;
      x[i] = x0 + Fx[i]/taf ;
      dx[i] = fx[i]/taf;
    }
  }
}

double CanonicalStructure::getFx(double t)
{
  return ax*t;
}

void CanonicalStructure::generateRfx(string kind, vec timed)
{
  int len = timed.n_rows;
  Rx0 = x[len-1] ;
  if ( kind == "discrete")
  {
    for (int i = 0;i<len;i++)
    {
      Rx[i] = Rx0 - Fx[i]/taf ;
      Rdx[i] = -fx[i]/taf;
    }

  }
}


#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <Eigen/Dense>
#include "tinyply.h"
#include "pca.h"
#include <regex>
#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;

// list of paths of all files under the directory 'dir' when the extension matches the regex
// file_list<true> searches recursively into sub-directories;
// file_list<false> searches only the specified directory
template <bool RECURSIVE> std::vector<fs::path> file_list(fs::path dir, std::regex ext_pattern)
{
	std::vector<fs::path> result;
	using iterator = std::conditional<RECURSIVE, fs::recursive_directory_iterator, fs::directory_iterator>::type;
	const iterator end;
	for (iterator iter{ dir }; iter != end; ++iter)
	{
		const std::string extension = iter->path().extension().string();
		if (fs::is_regular_file(*iter) && std::regex_match(extension, ext_pattern)) result.push_back(*iter);
	}
	return result;
}

int main(int argc, char* argv[])
{

	std::cout
		<< std::fixed << std::endl
		<< "Usage            : ./<app.exe> <dir>" << std::endl
		<< "Default          : ./pca_mesh.exe ../../data/" << std::endl
		<< std::endl;

	//
	// Initial parameters
	//
	const fs::path input_dir = (argc > 1) ? argv[1] : "../../data/";
	const std::string output_filename = "output_pca.ply";
	const std::vector<fs::path>& input_files = file_list<false>(input_dir, std::regex("\\.(?:ply)"));

	//
	// Compose output filename
	//
	std::stringstream output_abs_filename;
	output_abs_filename << input_dir << "/Output/";
	fs::create_directory(output_abs_filename.str());
	output_abs_filename << output_filename;

	//
	// Verify the first file and check the number of vertices
	//
	std::ifstream ss(input_files.at(0).string(), std::ios::binary);
	tinyply::PlyFile file(ss);
	std::vector<float> verts_first_file;
	file.request_properties_from_element("vertex", { "x", "y", "z" }, verts_first_file);

	//
	// Create matrix for PCA
	//
	Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> pca_input_matrix(verts_first_file.size(), input_files.size());

	std::cout << "Matrix size: " << pca_input_matrix.rows() << ' ' << pca_input_matrix.cols() << std::endl << std::endl;

	std::vector<float> verts;
	std::vector<float> norms;
	std::vector<uint8_t> colors;
	std::vector<uint32_t> faces;
	std::vector<float> uvCoords;
	uint32_t file_count = 0;

	for (const auto& filename : input_files)
	{
		try
		{
			std::cout << "Reading file <" << filename.string() << "> ... ";
			//
			// Read source ply file
			//
			std::ifstream ss(filename.string(), std::ios::binary);
			tinyply::PlyFile file(ss);

			uint32_t vertexCount, normalCount, colorCount, faceCount, faceTexcoordCount, faceColorCount;
			vertexCount = normalCount = colorCount = faceCount = faceTexcoordCount = faceColorCount = 0;

			vertexCount = file.request_properties_from_element("vertex", { "x", "y", "z" }, verts);

			if (verts.size() != verts_first_file.size())
				throw("[FAIL] The number of vertices does not match");

			normalCount = file.request_properties_from_element("vertex", { "nx", "ny", "nz" }, norms);
			colorCount = file.request_properties_from_element("vertex", { "red", "green", "blue", "alpha" }, colors);

			faceCount = file.request_properties_from_element("face", { "vertex_indices" }, faces, 3);
			faceTexcoordCount = file.request_properties_from_element("face", { "texcoord" }, uvCoords, 6);

			file.read(ss);

			for (int i = 0; i < verts.size(); ++i)
			{
				pca_input_matrix(i, file_count) = verts[i];
			}

			++file_count;

			std::cout << "verts: " << vertexCount << " [OK]" << std::endl;
		}
		catch (const std::exception & e)
		{
			std::cerr << "Caught exception: " << e.what() << std::endl;
		}
	}

	pca_t<float> pca;
	pca.set_input(pca_input_matrix);
	pca.compute();

	std::cout
		<< "Values: \n" << pca.get_eigen_values() << std::endl << std::endl
		<< "Vectors: \n" << pca.get_eigen_vectors() << std::endl << std::endl;

	const auto& result = pca.reprojection();

	for (int i = 0; i < verts.size(); ++i)
	{
		verts[i] = result(i, 0);
	}

	try
	{

		//
		// Write ply file
		//
		std::filebuf fb;
		fb.open(output_abs_filename.str(), std::ios::out | std::ios::binary);
		std::ostream outputStream(&fb);

		tinyply::PlyFile ply_out_file;

		if (!verts.empty())
			ply_out_file.add_properties_to_element("vertex", { "x", "y", "z" }, verts);
		if (!norms.empty())
			ply_out_file.add_properties_to_element("vertex", { "nx", "ny", "nz" }, norms);
		if (!colors.empty())
			ply_out_file.add_properties_to_element("vertex", { "red", "green", "blue", "alpha" }, colors);
		if (!faces.empty())
			ply_out_file.add_properties_to_element("face", { "vertex_indices" }, faces, 3, tinyply::PlyProperty::Type::UINT8);
		if (!uvCoords.empty())
			ply_out_file.add_properties_to_element("face", { "texcoord" }, uvCoords, 6, tinyply::PlyProperty::Type::UINT8);

		ply_out_file.write(outputStream, true);

		fb.close();
	}
	catch (const std::exception & e)
	{
		std::cerr << "Caught exception: " << e.what() << std::endl;
	}

}


/**
 * \ file OneMinusFilter.cpp
 */

#include <ATK/Tools/OneMinusFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>
#include <boost/scoped_array.hpp>

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE( OneMinusFilter_sinus_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::OneMinusFilter<float> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(data[i] + outdata[i], 1., 0.0001);
  }
}


#ifndef ALM_CONVEX_HULL_H
#define ALM_CONVEX_HULL_H

#include <vector>

#include <Eigen/Dense>

namespace math_utils{

typedef double coord_t;   // coordinate type
typedef double coord2_t;  // must be big enough to hold 2*max(|coordinate|)^2

// struct Point {
class Point{
public:
  coord_t x, y;

  Point(){}
  Point(const coord_t x_in, const coord_t y_in){
  	x = x_in;
  	y = y_in;
  }

  bool operator<(const Point &p) const {
    return x < p.x || (x == p.x && y < p.y);
  }
};

std::vector<Point> eigenToPoints(const Eigen::Ref<const Eigen::Matrix<double, 2, Eigen::Dynamic>>& P);

std::vector<Point> convexHull(std::vector<Point> P);
bool inConvexHull(
    const Eigen::Ref<const Eigen::Matrix<double, 2, Eigen::Dynamic>> &P,
    const Eigen::Ref<const Eigen::Vector2d> &q, double tolerance = 1e-16);

// Returns the perpendicular distance from point q to the convex hull of pts.
// Specifically, if pts form a polytope defined by a_i'x <= b_i where each a_i
// is a unit vector, then this returns:
//
// d* = min [b_i - a_i'q]
//       i
//
// If q is inside the convex hull of pts, then d* will be positive, else it will
// be negative.
double signedDistanceInsideConvexHull(
    const Eigen::Ref<const Eigen::Matrix<double, 2, Eigen::Dynamic>> &pts,
    const Eigen::Ref<const Eigen::Vector2d> &q);

}

#endif


/*
 * testScheduler.cpp
 * @date March 25, 2011
 * @author Frank Dellaert
 */

//#define ENABLE_TIMING
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/timing.h>
#include <gtsam_unstable/discrete/Scheduler.h>

#include <boost/assign/std/map.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/optional.hpp>

using namespace boost::assign;
using namespace std;
using namespace gtsam;

/* ************************************************************************* */
// Create the expected graph of constraints
DiscreteFactorGraph createExpected() {
  // Start building
  size_t nrFaculty = 4, nrTimeSlots = 3;

  // variables assigning a time to a student:
  // Akansel and Jake
  DiscreteKey A(6, nrTimeSlots), J(7, nrTimeSlots);

  // variables assigning a faculty member to a student area
  // Akansel:AI,ME,PC and Jake:HR,CT,AI
  DiscreteKey A1(0, nrFaculty), J1(3, nrFaculty);
  DiscreteKey A2(1, nrFaculty), J2(4, nrFaculty);
  DiscreteKey A3(2, nrFaculty), J3(5, nrFaculty);

  CSP expected;

  // Area constraints
  string faculty_in_A = "1 0 0 1";
  string faculty_in_C = "0 0 1 0";
  string faculty_in_H = "0 0 0 1";
  string faculty_in_M = "0 1 0 0";
  string faculty_in_P = "1 0 1 0";
  string available = "1 1 1 0   1 1 1 1   0 1 1 1";

  // Akansel
  expected.add(A1, faculty_in_A);  // Area 1
  expected.add(A1, "1 1 1 0");     // Advisor
  expected.add(A & A1, available);
  expected.add(A2, faculty_in_M);  // Area 2
  expected.add(A2, "1 1 1 0");     // Advisor
  expected.add(A & A2, available);
  expected.add(A3, faculty_in_P);  // Area 3
  expected.add(A3, "1 1 1 0");     // Advisor
  expected.add(A & A3, available);
  // Mutual exclusion for faculty
  expected.addAllDiff(A1 & A2 & A3);

  // Jake
  expected.add(J1, faculty_in_H);  // Area 1
  expected.add(J1, "1 0 1 1");     // Advisor
  expected.add(J & J1, available);
  expected.add(J2, faculty_in_C);  // Area 2
  expected.add(J2, "1 0 1 1");     // Advisor
  expected.add(J & J2, available);
  expected.add(J3, faculty_in_A);  // Area 3
  expected.add(J3, "1 0 1 1");     // Advisor
  expected.add(J & J3, available);
  // Mutual exclusion for faculty
  expected.addAllDiff(J1 & J2 & J3);

  // Mutual exclusion for students
  expected.addAllDiff(A, J);

  return std::move(expected);
}

/* ************************************************************************* */
TEST(schedulingExample, test) {
  Scheduler s(2);

  // add faculty
  s.addFaculty("Frank");
  s.addFaculty("Harvey");
  s.addFaculty("Magnus");
  s.addFaculty("Andrea");

  // add time slots
  s.addSlot("Mon");
  s.addSlot("Wed");
  s.addSlot("Fri");

  // add areas
  s.addArea("Frank", "AI");
  s.addArea("Frank", "PC");
  s.addArea("Harvey", "ME");
  s.addArea("Magnus", "CT");
  s.addArea("Magnus", "PC");
  s.addArea("Andrea", "AI");
  s.addArea("Andrea", "HR");

  // add availability, nrTimeSlots * nrFaculty
  string available = "1 1 1 0  1 1 1 1  0 1 1 1";
  s.setAvailability(available);

  // add students
  s.addStudent("Akansel", "AI", "ME", "PC", "Andrea");
  s.addStudent("Jake", "HR", "CT", "AI", "Harvey");

  // BUILD THE GRAPH !
  s.buildGraph();
  //  s.print();

  // Check graph
  DiscreteFactorGraph expected = createExpected();
  EXPECT(assert_equal(expected, (DiscreteFactorGraph)s));

  // Do brute force product and output that to file
  DecisionTreeFactor product = s.product();
  // product.dot("scheduling", false);

  // Do exact inference
  gttic(small);
  auto MPE = s.optimalAssignment();
  gttoc(small);

  // print MPE, commented out as unit tests don't print
  //  s.printAssignment(MPE);

  // Commented out as does not work yet
  // s.runArcConsistency(8,10,true);

  // find the assignment of students to slots with most possible committees
  // Commented out as not implemented yet
  //  auto bestSchedule = s.bestSchedule();
  //  GTSAM_PRINT(bestSchedule);

  //  find the corresponding most desirable committee assignment
  // Commented out as not implemented yet
  //  auto bestAssignment = s.bestAssignment(bestSchedule);
  //  GTSAM_PRINT(bestAssignment);
}

/* ************************************************************************* */
TEST(schedulingExample, smallFromFile) {
  string path(TOPSRCDIR "/gtsam_unstable/discrete/examples/");
  Scheduler s(2, path + "small.csv");

  // add areas
  s.addArea("Frank", "AI");
  s.addArea("Frank", "PC");
  s.addArea("Harvey", "ME");
  s.addArea("Magnus", "CT");
  s.addArea("Magnus", "PC");
  s.addArea("Andrea", "AI");
  s.addArea("Andrea", "HR");

  //   add students
  s.addStudent("Akansel", "AI", "ME", "PC", "Andrea");
  s.addStudent("Jake", "HR", "CT", "AI", "Harvey");
  //  s.print();

  // BUILD THE GRAPH !
  s.buildGraph();

  // Check graph
  DiscreteFactorGraph expected = createExpected();
  EXPECT(assert_equal(expected, (DiscreteFactorGraph)s));
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2015 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution  and  use  in  source  and  binary  forms,  with  or  without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
//
// * 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.
//
// * Neither the name of Google Inc. 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  OWNER 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.
//
// Author: keir@google.com (Keir Mierle)
//
// A  simple implementation  of N-dimensional  dual numbers,  for automatically
// computing exact derivatives of functions.
//
// While    a   complete    treatment   of    the   mechanics    of   automatic
// differentation    is    beyond   the    scope    of    this   header    (see
// http://en.wikipedia.org/wiki/Automatic_differentiation  for   details),  the
// basic idea is to extend normal  arithmetic with an extra element, "e," often
// denoted with the  greek symbol epsilon, such that  e != 0 but e^2  = 0. Dual
// numbers are  extensions of  the real numbers  analogous to  complex numbers:
// whereas complex numbers augment the reals by introducing an imaginary unit i
// such that  i^2 = -1, dual  numbers introduce an "infinitesimal"  unit e such
// that e^2 = 0. Dual numbers have two components: the "real" component and the
// "infinitesimal" component, generally written as  x + y*e. Surprisingly, this
// leads to a convenient method for computing exact derivatives without needing
// to manipulate complicated symbolic expressions.
//
// For example, consider the function
//
//   f(x) = x^2 ,
//
// evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20.
// Next, augument 10 with an infinitesimal to get:
//
//   f(10 + e) = (10 + e)^2
//             = 100 + 2 * 10 * e + e^2
//             = 100 + 20 * e       -+-
//                     --            |
//                     |             +--- This is zero, since e^2 = 0
//                     |
//                     +----------------- This is df/dx!
//
// Note that the derivative of f with  respect to x is simply the infinitesimal
// component of the value  of f(x + e). So, in order to  take the derivative of
// any function, it  is only necessary to replace the  numeric "object" used in
// the function with  one extended with infinitesimals. The  class Jet, defined
// in this header, is one such example of this, where substitution is done with
// templates.
//
// To  handle derivatives  of  functions taking  multiple arguments,  different
// infinitesimals are  used, one for each  variable to take the  derivative of.
// For example, consider a scalar function of two scalar parameters x and y:
//
//   f(x, y) = x^2 + x * y
//
// Following the  technique above, to  compute the derivatives df/dx  and df/dy
// for f(1, 3) involves doing two evaluations  of f, the first time replacing x
// with x + e, the second time replacing y with y + e.
//
// For df/dx:
//
//   f(1 + e, y) = (1 + e)^2 + (1 + e) * 3
//               = 1 + 2 * e + 3 + 3 * e
//               = 4 + 5 * e
//
//               --> df/dx = 5
//
// For df/dy:
//
//   f(1, 3 + e) = 1^2 + 1 * (3 + e)
//               = 1 + 3 + e
//               = 4 + e
//
//               --> df/dy = 1
//
// To take the  gradient of f with the implementation  of dual numbers ("jets")
// in  this file,  it  is necessary  to  create  a single  jet  type which  has
// components for the  derivative in x and  y, and passing them  to a templated
// version of f:
//
//   template<typename T>
//   T f(const T &x, const T &y) {
//     return x * x + x * y;
//   }
//
//   // The "2" means there should be 2 dual number components.
//   Jet<double, 2> x(1, 0);  // Pick the 0th dual number for x.
//   Jet<double, 2> y(3, 1);  // Pick the 1st dual number for y.
//   Jet<double, 2> z = f(x, y);
//
//   std::cout << z << std::endl;
//
// For  the  more mathematically  inclined,  this  file implements  first-order
// "jets". A 1st order jet is an element of the ring
//
//   T[N] = T[t_1, ..., t_N] / (t_1, ..., t_N)^2
//
// which essentially means that each jet  consists of a "scalar" value 'a' from
// T and a 1st order perturbation vector 'v' of length N:
//
//   x = a + \sum_i v[i] t_i
//
// A shorthand is to write an element as x = a + u, where u is the pertubation.
// Then, the  main point about  the arithmetic of jets  is that the  product of
// perturbations is zero:
//
//   (a + u) * (b + v) = ab + av + bu + uv
//                     = ab + (av + bu) + 0
//
// which is what operator* implements below. Addition is simpler:
//
//   (a + u) + (b + v) = (a + b) + (u + v).
//
// The only remaining  question is how to  evaluate the function of  a jet, for
// which we use the chain rule:
//
//   f(a + u) = f(a) + f'(a) u
//
// where f'(a) is the (scalar) derivative of f at a.

#ifndef PUBLIC_JET_HPP_
#define PUBLIC_JET_HPP_

#include <Eigen/Core>
#include <cmath>

template <typename Tp, int N>
struct Jet {
  // Default-construct "a"  because otherwise  this can lead  to false
  // errors  about uninitialized  uses when  other classes  relying on
  // default constructed Tp  (where Tp is a Jet<Tp,  N>). This usually
  // only happens  in opt  mode. Note that  the C++  standard mandates
  // that e.g. default constructed doubles are initialized to 0.0; see
  // sections 8.5 of the C++03 standard.
  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Jet() : a() { v.setZero(); }

  // Constructor from scalar: a + [0, ...].
  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit
    Jet(const Tp& value) { a = value; v.setZero(); }

  // Constructor from scalar plus variable: a + [0, ..., 1, ... 0].
  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
  Jet(const Tp& value, int k) { a = value; v.setZero(); v[k] = Tp(1.0); }

  // Constructor from scalar and vector part
  // ---------------------------------------
  // The use of Eigen::DenseBase allows Eigen expressions to be passed
  // in  without being  fully evaluated until  they are assigned  to v
  template<typename Derived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
  Jet(const Tp& a, const Eigen::DenseBase<Derived> &v) : a(a), v(v) {}

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
  Jet<Tp, N>& operator+=(const Jet<Tp, N> &y)
    { *this = *this + y; return *this; }

  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
  Jet<Tp, N>& operator-=(const Jet<Tp, N> &y)
    { *this = *this - y; return *this; }

  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
  Jet<Tp, N>& operator*=(const Jet<Tp, N> &y)
    { *this = *this * y; return *this; }

  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
  Jet<Tp, N>& operator/=(const Jet<Tp, N> &y)
    { *this = *this / y; return *this; }

#ifdef CEREAL_SERIALIZE_FUNCTION_NAME

  friend class cereal::access;

  template<class Archive>
  void serialize(Archive& archive) {
    archive(cereal::make_nvp("[real]", a));
    archive(cereal::make_nvp("[dual]", v));
  }

#endif  // CEREAL_SERIALIZE_FUNCTION_NAME

  // Jet dimensionality.
  enum { kDimensions = N };

  // The scalar part.
  Tp a;

  // The infinitesimal part.
  Eigen::Matrix<Tp, N, 1, Eigen::AutoAlign> v;
};

// Unary +
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> const& operator+(const Jet<Tp, N>& f) { return f; }

// Unary -
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator-(const Jet<Tp, N>&f) { return Jet<Tp, N>(-f.a, -f.v); }

// Binary +
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator+(const Jet<Tp, N>& f, const Jet<Tp, N>& g)
 { return Jet<Tp, N>(f.a + g.a, f.v + g.v); }

// Binary + with a scalar: x + s
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator+(const Jet<Tp, N>& f, Tp s)
  { return Jet<Tp, N>(f.a + s, f.v); }

// Binary + with a scalar: s + x
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator+(Tp s, const Jet<Tp, N>& f)
  { return Jet<Tp, N>(f.a + s, f.v); }

// Binary -
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator-(const Jet<Tp, N>& f, const Jet<Tp, N>& g)
  { return Jet<Tp, N>(f.a - g.a, f.v - g.v); }

// Binary - with a scalar: x - s
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator-(const Jet<Tp, N>& f, Tp s)
  { return Jet<Tp, N>(f.a - s, f.v); }

// Binary - with a scalar: s - x
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator-(Tp s, const Jet<Tp, N>& f)
  { return Jet<Tp, N>(s - f.a, -f.v); }

// Binary *
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator*(const Jet<Tp, N>& f, const Jet<Tp, N>& g)
 { return Jet<Tp, N>(f.a * g.a, f.a * g.v + f.v * g.a); }

// Binary * with a scalar: x * s
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator*(const Jet<Tp, N>& f, Tp s)
  { return Jet<Tp, N>(f.a * s, f.v * s); }

// Binary * with a scalar: s * x
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator*(Tp s, const Jet<Tp, N>& f)
  { return Jet<Tp, N>(f.a * s, f.v * s); }

// Binary /
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator/(const Jet<Tp, N>& f, const Jet<Tp, N>& g) {
  //   a + u   (a + u)(b - v)   (a + u)(b - v)
  //   ----- = -------------- = --------------
  //   b + v   (b + v)(b - v)        b^2
  return Jet<Tp, N>(f.a / g.a, (f.v - f.a / g.a * g.v) / g.a);
}

// Binary / with a scalar: s / x
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator/(Tp s, const Jet<Tp, N>& g)
  { return Jet<Tp, N>(s / g.a, -s * g.v / (g.a * g.a)); }

// Binary / with a scalar: x / s
template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> operator/(const Jet<Tp, N>& f, Tp s)
  { return Jet<Tp, N>(f.a / s, f.v / s); }

// Binary comparison operators for both scalars and jets.
#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op)                                    \
  template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE                \
  bool operator op(const Tp& s, const Jet<Tp, N>& g) { return s op g.a; }           \
                                                                                \
  template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE                \
  bool operator op(const Jet<Tp, N>& f, const Tp& s) { return f.a op s; }           \
                                                                                \
  template<typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE                \
  bool operator op(const Jet<Tp, N>& f, const Jet<Tp, N>& g) { return f.a op g.a; } \

CERES_DEFINE_JET_COMPARISON_OPERATOR(< )
CERES_DEFINE_JET_COMPARISON_OPERATOR(<=)
CERES_DEFINE_JET_COMPARISON_OPERATOR(> )
CERES_DEFINE_JET_COMPARISON_OPERATOR(>=)
CERES_DEFINE_JET_COMPARISON_OPERATOR(==)
CERES_DEFINE_JET_COMPARISON_OPERATOR(!=)

#undef CERES_DEFINE_JET_COMPARISON_OPERATOR

// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule.

using std::abs;  // abs(x + h) ~= x + h or -(x + h)
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> abs(const Jet<Tp, N>& f) { return f.a < Tp(0.0) ? -f : f; }

using std::log;  // log(a + h) ~= log(a) + h / a
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> log(const Jet<Tp, N>& f) { return Jet<Tp, N>(log(f.a), f.v / f.a); }

using std::exp;  // exp(a + h) ~= exp(a) + exp(a) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> exp(const Jet<Tp, N>& f) { return Jet<Tp, N>(exp(f.a), exp(f.a) * f.v); }

using std::sqrt;  // sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a))
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> sqrt(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(sqrt(f.a), f.v / (Tp(2.0) * sqrt(f.a))); }

using std::cos;  // cos(a + h) ~= cos(a) - sin(a) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> cos(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(cos(f.a), - sin(f.a) * f.v); }

using std::acos;  // acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> acos(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(acos(f.a), f.v / sqrt(Tp(1.0) - f.a * f.a)); }

using std::sin;  // sin(a + h) ~= sin(a) + cos(a) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> sin(const Jet<Tp, N>& f) { return Jet<Tp, N>(sin(f.a), cos(f.a) * f.v); }

using std::asin;  // asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> asin(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(asin(f.a), f.v / sqrt(Tp(1.0) - f.a * f.a)); }

using std::tan;  // tan(a + h) ~= tan(a) + (1 + tan(a)^2) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> tan(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(tan(f.a), Tp(1.0) + tan(f.a) * tan(f.a) * f.v); }

using std::atan;  // atan(a + h) ~= atan(a) + 1 / (1 + a^2) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> atan(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(atan(f.a), f.v / (Tp(1.0) + f.a * f.a)); }

using std::sinh;  // sinh(a + h) ~= sinh(a) + cosh(a) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> sinh(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(sinh(f.a), cosh(f.a) * f.v); }

using std::cosh;  // cosh(a + h) ~= cosh(a) + sinh(a) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> cosh(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(cosh(f.a), sinh(f.a) * f.v); }

using std::tanh;  // tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> tanh(const Jet<Tp, N>& f)
  { return Jet<Tp, N>(tan(f.a), (Tp(1.0) - tan(f.a) * tan(f.a)) * f.v); }

using std::atan2;

// atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2)
//
// In words: the rate of change of theta is 1/r times the rate of
// change of (x, y) in the positive angular direction.
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> atan2(const Jet<Tp, N>& g, const Jet<Tp, N>& f) {
  // Note order of arguments:
  //
  //   f = a + da
  //   g = b + db

  Tp const tmp = Tp(1.0) / (f.a * f.a + g.a * g.a);
  return Jet<Tp, N>(atan2(g.a, f.a), tmp * (- g.a * f.v + f.a * g.v));
}

using std::pow;

// pow -- base is a differentiable function, exponent is a constant.
// (a+da)^p ~= a^p + p*a^(p-1) da
template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> pow(const Jet<Tp, N>& f, double g) {
  Tp const tmp = g * pow(f.a, g - Tp(1.0));
  return Jet<Tp, N>(pow(f.a, g), tmp * f.v);
}

// pow -- base is a constant, exponent is a differentiable function.
// We have various special cases, see the comment for pow(Jet, Jet) for
// analysis:
//
// 1. For f > 0 we have: (f)^(g + dg) ~= f^g + f^g log(f) dg
//
// 2. For f == 0 and g > 0 we have: (f)^(g + dg) ~= f^g
//
// 3. For f < 0 and integer g we have: (f)^(g + dg) ~= f^g but if dg
// != 0, the derivatives are not defined and we return NaN.

template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> pow(double f, const Jet<Tp, N>& g) {
  if (f == 0 && g.a > 0) {
    // Handle case 2.
    return Jet<Tp, N>(Tp(0.0));
  }
  if (f < 0 && g.a == floor(g.a)) {
    // Handle case 3.
    Jet<Tp, N> ret(pow(f, g.a));
    for (int i = 0; i < N; i++) {
      if (g.v[i] != Tp(0.0)) {
        // Return a NaN when g.v != 0.
        ret.v[i] = std::numeric_limits<Tp>::quiet_NaN();
      }
    }
    return ret;
  }
  // Handle case 1.
  Tp const tmp = pow(f, g.a);
  return Jet<Tp, N>(tmp, log(f) * tmp * g.v);
}

// pow -- both base and exponent are differentiable functions. This has a
// variety of special cases that require careful handling.
//
// 1. For f > 0:
//    (f + df)^(g + dg) ~= f^g + f^(g - 1) * (g * df + f * log(f) * dg)
//    The numerical evaluation of f * log(f) for f > 0 is well behaved, even for
//    extremely small values (e.g. 1e-99).
//
// 2. For f == 0 and g > 1: (f + df)^(g + dg) ~= 0
//    This cases is needed because log(0) can not be evaluated in the f > 0
//    expression. However the function f*log(f) is well behaved around f == 0
//    and its limit as f-->0 is zero.
//
// 3. For f == 0 and g == 1: (f + df)^(g + dg) ~= 0 + df
//
// 4. For f == 0 and 0 < g < 1: The value is finite but the derivatives are not.
//
// 5. For f == 0 and g < 0: The value and derivatives of f^g are not finite.
//
// 6. For f == 0 and g == 0: The C standard incorrectly defines 0^0 to be 1
//    "because there are applications that can exploit this definition". We
//    (arbitrarily) decree that derivatives here will be nonfinite, since that
//    is consistent with the behavior for f == 0, g < 0 and 0 < g < 1.
//    Practically any definition could have been justified because mathematical
//    consistency has been lost at this point.
//
// 7. For f < 0, g integer, dg == 0: (f + df)^(g + dg) ~= f^g + g * f^(g - 1) df
//    This is equivalent to the case where f is a differentiable function and g
//    is a constant (to first order).
//
// 8. For f < 0, g integer, dg != 0: The value is finite but the derivatives are
//    not, because any change in the value of g moves us away from the point
//    with a real-valued answer into the region with complex-valued answers.
//
// 9. For f < 0, g noninteger: The value and derivatives of f^g are not finite.

template <typename Tp, int N> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Jet<Tp, N> pow(const Jet<Tp, N>& f, const Jet<Tp, N>& g) {
  if (f.a == 0 && g.a >= 1) {
    // Handle cases 2 and 3.
    if (g.a > 1) {
      return Jet<Tp, N>(Tp(0.0));
    }
    return f;
  }
  if (f.a < 0 && g.a == floor(g.a)) {
    // Handle cases 7 and 8.
    Tp const tmp = g.a * pow(f.a, g.a - Tp(1.0));
    Jet<Tp, N> ret(pow(f.a, g.a), tmp * f.v);
    for (int i = 0; i < N; i++) {
      if (g.v[i] != Tp(0.0)) {
        // Return a NaN when g.v != 0.
        ret.v[i] = std::numeric_limits<Tp>::quiet_NaN();
      }
    }
    return ret;
  }
  // Handle the remaining cases. For cases 4,5,6,9 we allow the log() function
  // to generate -HUGE_VAL or NaN, since those cases result in a nonfinite
  // derivative.
  Tp const tmp1 = pow(f.a, g.a);
  Tp const tmp2 = g.a * pow(f.a, g.a - Tp(1.0));
  Tp const tmp3 = tmp1 * log(f.a);
  return Jet<Tp, N>(tmp1, tmp2 * f.v + tmp3 * g.v);
}

template <typename Tp, int N>
std::ostream &operator<<(std::ostream &s, const Jet<Tp, N>& z)
  { return s << "[" << z.a << "; " << z.v.transpose() << "]"; }

namespace Eigen {

// Creating  a  specialization of  NumTraits  enables
// placing Jet  objects inside Eigen  arrays, getting
// all the goodness of Eigen combined with autodiff.
template<typename Tp, int N>
struct NumTraits<Jet<Tp, N> > {
  typedef Jet<Tp, N> Real;
  typedef Jet<Tp, N> NonInteger;
  typedef Jet<Tp, N> Nested;
  typedef Jet<Tp, N> Literal;

  // For Jet types, multiplication is more expensive than addition.
  enum { IsComplex = 0, IsInteger = 0, IsSigned, ReadCost = 1, AddCost = 1,
         MulCost = 3, HasFloatingPoint = 1, RequireInitialization = 1 };

  // Assuming that for Jets, division is as expensive as multiplication.
  template<bool Vectorized> struct Div { enum { Cost = 3 }; };

  static Jet<Tp, N> dummy_precision()
    { return Jet<Tp, N>(1e-12); }

  static inline Real epsilon()
    { return Real(std::numeric_limits<Tp>::epsilon()); }

  static inline int digits10() { return 0; }
};

}  // namespace Eigen

#endif  // PUBLIC_JET_HPP_