#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <ceres/loss_function.h>
#include <ceres/autodiff_cost_function.h>

#include <Eigen/Jacobi>
#include <Eigen/SVD>
#include <Eigen/LU>

#include <iostream>

#include <opencv2/core/utility.hpp>

#include <sphericalsfm/sfm.h>
#include <sphericalsfm/so3.h>

namespace sphericalsfm {

    class ParallelTriangulator : public cv::ParallelLoopBody
    {
    public:
        ParallelTriangulator( SfM &_sfm ) :sfm(_sfm) { }
        virtual void operator()(const cv::Range &range) const CV_OVERRIDE
        {
            for ( int j = range.start; j < range.end; j++ )
            {
                if ( !sfm.points.exists(j) ) continue;

                int firstcam = -1;
                int lastcam = -1;

                int nobs = 0;
                for ( int i = 0; i < sfm.numCameras; i++ )
                {
                    if ( !sfm.cameras.exists(i) ) continue;
                    if ( !( sfm.observations.exists(i,j) ) ) continue;
                    if ( firstcam == -1 ) firstcam = i;
                    lastcam = i;
                    nobs++;
                }

                sfm.SetPoint( j, Eigen::Vector3d::Zero() );
                if ( nobs < 3 ) continue;

                Eigen::MatrixXd A( nobs*2, 4 );

                int n = 0;
                for ( int i = 0; i < sfm.numCameras; i++ )
                {
                    if ( !sfm.cameras.exists(i) ) continue;
                    if ( !( sfm.observations.exists(i,j) ) ) continue;

                    Observation vec = sfm.observations(i,j);

                    Eigen::Vector2d point(vec(0)/sfm.intrinsics.focal,vec(1)/sfm.intrinsics.focal);
                    Eigen::Matrix4d P = sfm.GetPose(i).P;

                    A.row(2*n+0) = P.row(2) * point[0] - P.row(0);
                    A.row(2*n+1) = P.row(2) * point[1] - P.row(1);
                    n++;
                }

                Eigen::JacobiSVD<Eigen::MatrixXd> svdA(A,Eigen::ComputeFullV);
                Eigen::Vector4d Xh = svdA.matrixV().col(3);
                Eigen::Vector3d X = Xh.head(3)/Xh(3);

                sfm.SetPoint( j, X );
            }
        }
        ParallelTriangulator& operator=(const ParallelTriangulator &) {
            return *this;
        }
    private:
        SfM &sfm;
    };

    struct ReprojectionError
    {
        ReprojectionError( double _focal, double _x, double _y )
        : focal(_focal), x(_x), y(_y)
        {

        }

        template <typename T>
        bool operator()(const T* const camera_t,
                        const T* const camera_r,
                        const T* const point,
                        T* residuals) const
        {
            // transform from world to camera
            T p[3];
            ceres::AngleAxisRotatePoint(camera_r, point, p);
            p[0] += camera_t[0]; p[1] += camera_t[1]; p[2] += camera_t[2];

            // projection
            T xp = p[0] / p[2];
            T yp = p[1] / p[2];

            // intrinsics
            T fxp = T(focal) * xp;
            T fyp = T(focal) * yp;

            // residuals
            residuals[0] = fxp - T(x);
            residuals[1] = fyp - T(y);

            return true;
        }

        double focal, x, y;
    };

    SfM::SfM( const Intrinsics &_intrinsics )
    : intrinsics( _intrinsics ),
    numCameras( 0 ),
    numPoints( 0 ),
    nextCamera( -1 ),
    nextPoint( 0 )
    {
    }

    double * SfM::GetCameraPtr( int camera )
    {
        return (double*)&cameras(camera);
    }

    double * SfM::GetPointPtr( int point )
    {
        return (double*)&points(point);
    }

    int SfM::AddCamera( const Pose &initial_pose, const std::string &path )
    {
        nextCamera++;
        numCameras++;

        cameras( nextCamera ).head(3) = initial_pose.t;
        cameras( nextCamera ).tail(3) = initial_pose.r;
        paths( nextCamera ) = path;
        rotationFixed( nextCamera ) = false;
        translationFixed( nextCamera ) = false;

        return nextCamera;
    }

    int SfM::AddPoint( const Point &initial_position, const cv::Mat &descriptor )
    {
        numPoints++;

        points( nextPoint ) = initial_position;
        pointFixed( nextPoint ) = false;

        cv::Mat descriptor_copy;
        descriptor.copyTo( descriptor_copy );
        descriptors( nextPoint ) = descriptor_copy;

        return nextPoint++;
    }

    void SfM::MergePoint( int point1, int point2 )
    {
        for ( int i = 0; i < numCameras; i++ )
        {
            if ( !cameras.exists(i) ) continue;
            if ( !( observations.exists(i,point2) ) ) continue;

            observations(i,point1) = observations(i,point2);
        }

        RemovePoint( point2 );
    }

    void SfM::AddObservation( int camera, int point, const Observation &observation )
    {
        observations(camera,point) = observation;
    }

    void SfM::AddMeasurement( int i, int j, const Pose &measurement )
    {
        measurements(i,j) = measurement;
    }

    bool SfM::GetMeasurement( int i, int j, Pose &measurement )
    {
        if ( !measurements.exists(i,j) ) return false;

        measurement = measurements(i,j);

        return true;
    }

    bool SfM::GetObservation( int camera, int point, Observation &observation )
    {
        if ( !observations.exists(camera,point) ) return false;

        observation = observations(camera,point);

        return true;
    }

    void SfM::Retriangulate()
    {
        cv::parallel_for_(cv::Range(0,numPoints), [&](const cv::Range &range){
        //for ( int j = 0; j < numPoints; j++ )
        for ( int j = range.start; j < range.end; j++ )
        {
            if ( !points.exists(j) ) continue;

            int firstcam = -1;
            int lastcam = -1;

            int nobs = 0;
            for ( int i = 0; i < numCameras; i++ )
            {
                if ( !cameras.exists(i) ) continue;
                if ( !( observations.exists(i,j) ) ) continue;
                if ( firstcam == -1 ) firstcam = i;
                lastcam = i;
                nobs++;
            }

            SetPoint( j, Eigen::Vector3d::Zero() );
            if ( nobs < 3 ) continue;

            Eigen::MatrixXd A( nobs*2, 4 );

            int n = 0;
            for ( int i = 0; i < numCameras; i++ )
            {
                if ( !cameras.exists(i) ) continue;
                if ( !( observations.exists(i,j) ) ) continue;

                Observation vec = observations(i,j);

                Eigen::Vector2d point(vec(0)/intrinsics.focal,vec(1)/intrinsics.focal);
                Eigen::Matrix4d P = GetPose(i).P;

                A.row(2*n+0) = P.row(2) * point[0] - P.row(0);
                A.row(2*n+1) = P.row(2) * point[1] - P.row(1);
                n++;
            }

            Eigen::JacobiSVD<Eigen::MatrixXd> svdA(A,Eigen::ComputeFullV);
            Eigen::Vector4d Xh = svdA.matrixV().col(3);
            Eigen::Vector3d X = Xh.head(3)/Xh(3);

            SetPoint( j, X );
        }
        });
    }

    void SfM::PreOptimize()
    {
        loss_function = new ceres::CauchyLoss( 2.0 );
    }

    void SfM::ConfigureSolverOptions( ceres::Solver::Options &options )
    {
        options.minimizer_type = ceres::TRUST_REGION;
        options.linear_solver_type = ceres::SPARSE_SCHUR;
        options.max_num_iterations = 1000;
        options.max_num_consecutive_invalid_steps = 100;
        options.minimizer_progress_to_stdout = true;
        options.num_threads = 16;
    }

    void SfM::AddResidual( ceres::Problem &problem, int camera, int point )
    {
        Observation vec = observations(camera,point);

        ReprojectionError *reproj_error = new ReprojectionError(intrinsics.focal,vec(0),vec(1));
        ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<ReprojectionError, 2, 3, 3, 3>(reproj_error);
        problem.AddResidualBlock(cost_function, loss_function, GetCameraPtr(camera), GetCameraPtr(camera)+3, GetPointPtr(point) );

        if ( translationFixed( camera ) ) problem.SetParameterBlockConstant( GetCameraPtr(camera) );
        if ( rotationFixed( camera ) ) problem.SetParameterBlockConstant( GetCameraPtr(camera)+3 );
        if ( pointFixed( point ) ) problem.SetParameterBlockConstant( GetPointPtr(point) );
    }

    bool SfM::Optimize()
    {
        if ( numCameras == 0 || numPoints == 0 ) return false;

        ceres::Problem problem;

        PreOptimize();

        bool added_one_camera = false;

        std::cout << "\tBuilding BA problem...\n";

        for ( int j = 0; j < numPoints; j++ )
        {
            if ( !points.exists(j) ) continue;
            if ( points(j).norm() == 0 ) continue;

            int nobs = 0;
            for ( int i = 0; i < numCameras; i++ )
            {
                if ( !cameras.exists(i) ) continue;
                if ( !( observations.exists(i,j) ) ) continue;

                nobs++;
            }

            if ( nobs < 3 ) continue;
            for ( int i = 0; i < numCameras; i++ )
            {
                if ( !cameras.exists(i) ) continue;
                if ( !( observations.exists(i,j) ) ) continue;

                AddResidual( problem, i, j );
                added_one_camera = true;
            }
        }

        if ( !added_one_camera ) {
            std::cout << "didn't add any cameras\n";
            return false;
        }

        std::cout << "Running optimizer...\n";
        std::cout << "\t" << problem.NumResiduals() << " residuals\n";

        ceres::Solver::Options options;
        ConfigureSolverOptions( options );
        ceres::Solver::Summary summary;
        ceres::Solve(options, &problem, &summary);
        std::cout << summary.FullReport() << "\n";
        if ( summary.termination_type == ceres::FAILURE )
        {
            std::cout << "error: ceres failed.\n";
            exit(1);
        }

        PostOptimize();

        return ( summary.termination_type == ceres::CONVERGENCE );
    }

    void SfM::PostOptimize()
    {

    }

    void SfM::Apply( const Pose &pose )
    {
        // x = PX
        // X -> pose*X
        // x = P' * (pose*X)
        // x = (P*poseinv) * (pose*X)
        Pose poseinv = pose.inverse();
        for ( int i = 0; i < numCameras; i++ )
        {
            Pose campose = GetPose(i);
            campose.postMultiply( poseinv );
            SetPose( i, campose );
        }
        for ( int j = 0; j < numPoints; j++ )
        {
            Point X = GetPoint(j);
            X = pose.apply(X);
            SetPoint( j, X );
        }
    }

    void SfM::Apply( double scale )
    {
        for ( int i = 0; i < numCameras; i++ )
        {
            Pose campose = GetPose(i);
            campose.t *= scale;
            campose.P.block<3,1>(0,3) = campose.t;
            SetPose( i, campose );
        }
        for ( int j = 0; j < numPoints; j++ )
        {
            Point X = GetPoint(j);
            X *= scale;
            SetPoint( j, X );
        }
    }

    void SfM::Unapply( const Pose &pose )
    {
        // x = PX
        // X -> poseinv*X
        // x = P' * (poseinv*X)
        // x = (P*pose) * (poseinv*X)
        for ( int i = 0; i < numCameras; i++ )
        {
            Pose campose = GetPose(i);
            campose.postMultiply( pose );
            SetPose( i, campose );
        }
        Pose poseinv = pose.inverse();
        for ( int j = 0; j < numPoints; j++ )
        {
            Point X = GetPoint(j);
            X = poseinv.apply(X);
            SetPoint( j, X );
        }
    }

    Pose SfM::GetPose( int camera )
    {
        if ( !cameras.exists(camera) ) return Pose();

        return Pose( cameras( camera ).head(3), cameras( camera ).tail(3) );
    }

    void SfM::SetPose( int camera, const Pose &pose )
    {
        cameras( camera ).head(3) = pose.t;
        cameras( camera ).tail(3) = pose.r;
    }

    Point SfM::GetPoint( int point )
    {
        if ( !points.exists( point ) ) return Point(0,0,0);
        return points( point );
    }

    void SfM::SetPoint( int point, const Point &position )
    {
        points( point ) = position;
    }

    void SfM::RemovePoint( int point )
    {
        for ( int i = 0; i < numCameras; i++ )
        {
            observations.erase(i,point);
        }
        points.erase( point );
        descriptors.erase( point );
    }

    void SfM::RemoveCamera( int camera )
    {
        cameras.erase(camera);
        observations.erase(camera);

        for ( int j = 0; j < numPoints; j++ )
        {
            int i = 0;
            for ( ; i < numCameras; i++ )
            {
                if ( observations.exists(i,j) ) break;
            }
            if ( i == numCameras ) points.erase(j);
        }
    }

    void SfM::WritePoses( const std::string &path, const std::vector<int> &indices )
    {
        assert(indices.size() == numCameras);
        FILE *f = fopen( path.c_str(), "w" );

        for ( int i = 0; i < numCameras; i++ )
        {
            fprintf(f,"%d ",indices[i]);
            Camera camera = cameras(i);
            for ( int j = 0; j < 6; j++ )
            {
                fprintf(f,"%.15lf ",camera(j));
            }
            fprintf(f,"\n");
        }

        fclose(f);
    }

    void SfM::WritePointsOBJ( const std::string &path )
    {
        FILE *f = fopen( path.c_str(), "w" );

        std::vector<int> nobs(numPoints);
        std::vector<double> distances(numPoints);
        for ( int j = 0; j < numPoints; j++ )
        {
            nobs[j] = 0;
        }

        for ( int i = 0; i < numCameras; i++ )
        {
            Pose pose = GetPose(i);
            Eigen::Vector3d center = pose.getCenter();

            for ( int j = 0; j < numPoints; j++ )
            {
                if ( !points.exists(j) ) continue;
                if ( !observations.exists(i,j) ) continue;

                nobs[j]++;
                distances[j] = (GetPoint(j)-center).norm();
            }
        }

        for ( int i = 0; i < numPoints; i++ )
        {
            if ( !points.exists(i) ) continue;

            if ( distances[i] > 2000. ) continue;
            Point X = GetPoint(i);
            if ( X.norm() == 0 ) continue;
            fprintf(f,"v %0.15lf %0.15lf %0.15lf\n", X(0), X(1), X(2) );
        }

        fclose( f );
    }

    void SfM::WriteCameraCentersOBJ( const std::string &path )
    {
        FILE *f = fopen( path.c_str(), "w" );

        for ( int i = 0; i < numCameras; i++ )
        {
            Pose pose = GetPose(i);
            Eigen::Vector3d center = pose.getCenter();
            fprintf(f,"v %0.15lf %0.15lf %0.15lf\n", center(0), center(1), center(2) );
        }

        fclose( f );
    }
}


/**
 * @file fixed_point.hpp
 * @author Salvatore Cardamone
 * @brief Largely a replica of the Fixed Point Math Library developed by
 *        Peter Schregle. Pared down superfluous functionality and added some
 *        bits and pieces to make the code a little more useful for the types
 *        of calculation we do in tyche++.
 */
#ifndef __TYCHEPLUSPLUS_FIXED_POINT_HPP
#define __TYCHEPLUSPLUS_FIXED_POINT_HPP

#include <cstddef>
#include <boost/operators.hpp>
#include <boost/type_index.hpp>
#include "utilities/type_promotion.hpp"

namespace tycheplusplus {

/**
 * @class FixedPoint
 * @brief Generic fixed point functionality, allowing us to use this class as
 *        we would any other data type, like float or double. All integer
 *        arithmetic, so if there's no dedicated FPU, using FixedPoint as the
 *        real numerical type is probably advantageous. Furthermore, any
 *        high-level synthesis tools will hopefully pick up on the use of
 *        integer arithmetic and generate efficient implementations.
 *
 *        boost/operators.hpp provides a fairly remarkable set of
 *        functionalities, whereby operators can be automatically generated from
 *        a smaller set of operators. Deriving from the appropriate boost
 *        classes within boost/operators.hpp then gives us access to the full
 *        complement of operators.
 *
 *        For instance, deriving from boost::ordered_field_operators allows us
 *        to explicitly define the += operator, and obtain the + operator for
 *        free, without the need for additional boilerplate.
 *
 * @tparam B Data type used to store the fixed point number. It the type is
 *           signed, then the fixed point representation will be signed too.
 * @tparam I Number of integer bits.
 * @tparam F Number of fractional bits. Automatically determined from number of
 *           integer bits and number of available storage bits.
 */
template<typename B,
         unsigned char I,
         unsigned char F = std::numeric_limits<B>::digits - I>
class FixedPoint
    : boost::ordered_field_operators<FixedPoint<B,I,F>,
      boost::unit_steppable<FixedPoint<B,I,F>,
      boost::shiftable<FixedPoint<B,I,F> > > >
{

public:
  /**
   * @brief Class constructor.
   * @param value Single precision value to initialise with.
   */
  FixedPoint(float value)
      : value_(value * two_power_f_ + (value >= 0 ? 0.5 : -0.5)) {}

  /**
   * @brief Class constructor.
   * @param value Double precision value to initialise with.
   */
  FixedPoint(double value)
      : value_(value * two_power_f_ + (value >= 0 ? 0.5 : -0.5)) {
  }

  /**
   * @brief Class constructor.
   * @param value Single precision value to initialise with.
   */
  FixedPoint<B,I,F>& operator +=(FixedPoint<B,I,F> const& rhs) {
    value_ += rhs.value_;
    return *this;
  }

  /**
   * @brief Subtraction assignment operator.
   * @param rhs Value to subtract from lhs.
   * @retval lhs - rhs.
   */
  FixedPoint<B,I,F>& operator -=(FixedPoint<B,I,F> const& rhs) {
    value_ -= rhs.value_;
    return *this;
  }

  /**
   * @brief Multiplication assignment operator.
   * @param rhs Value to multiply lhs by.
   * @retval lhs * rhs.
   */
  FixedPoint<B,I,F>& operator *=(FixedPoint<B,I,F> const& rhs) {
    value_ = (static_cast<typename TypePromotion<B>::type>
	      (value_) * rhs.value_) >> number_fractional_bits_;
    return *this;
  }

  /**
   * @brief Division assignment operator.
   * @param rhs Value to divide lhs by.
   * @retval lhs / rhs.
   */
  FixedPoint<B,I,F>& operator /=(FixedPoint<B,I,F> const& rhs) {
    value_ = (static_cast<typename TypePromotion<B>::type>
	      (value_) << number_fractional_bits_) / rhs.value_;
    return *this;
  }

  /**
   * @brief Convert the internal value of the fixed point object to a float.
   * @retval Fixed point number cast to float.
   */
  float AsFloat() const {
    return (float)value_ / two_power_f_;
  }

  /**
   * @brief Convert the internal value of the fixed point object to a double.
   * @retval Fixed point number cast to double.
   */
  double AsDouble() const {
    return (double)value_ / two_power_f_;
  }

  /**
   * @brief Print some information about the object.
   */
  void Print(std::ostream& stream) const {
    stream << " *** FixedPoint object" << std::endl
	   << "     "
	   << (int)number_fractional_bits_ << " fractional bits and "
	   << (int)number_integer_bits_ << " integer bits." << std::endl
	   << "     Storage type: "
	   << boost::typeindex::type_id<B>().pretty_name() << std::endl
	   << "     Has Sign Bit: "
	   << std::numeric_limits<B>::is_signed << std::endl
	   << "     Stored Value: " << std::hex << value_ << std::dec << std::endl
	   << "     Floating Point: " << AsDouble() << std::endl;
  }

  /**
   * @brief Compute the exponential of a fixed point number.
   *
   *        This is fairly inefficient, utilising (I+F) integer multiplications.
   *        The exponential is split into its integer and fractional parts:
   *
   *                         exp(i.f) = exp(i) * exp(f)
   *
   *        For the fractional part, we move down the fractional bits of the
   *        argument, lookup the associated value for the exponential of the
   *        fractional bit and multiply-accumulate if the bit is high, otherwise
   *        it makes no contribution. So, for instance, e^{0.625} is equal to:
   *
   *                    1*exp(0.5) * 0*exp(0.25) * 1*exp(0.125)
   *
   *        the values of the exponential for which are already tabulated.
   *        We compute the integer part in a similar fashion using the
   *        integer part lookup table. If the argument is negative, we
   *        divide-accumulate rather than multiply-accumulate.
   *
   *        We should implement a specialised Gaussian function, since the
   *        integer part is only relevant over a much smaller dynamic range.
   * @param arg The argument of the exponential function.
   * @retval exp(arg).
   */
  friend FixedPoint<B,I,F> exp(FixedPoint<B,I,F> const& arg) {

    FixedPoint<B,I,F> result(1.0);

    // We start from the MSB in the fractional part and work our way down the
    // number of fractional bits
    for (int i_frac = F-1; i_frac >= 0; --i_frac) {
      if (arg.value_ & 1ULL<<i_frac) {
	result.value_ =
	  (static_cast<typename TypePromotion<B>::type>(result.value_) *
	   (exp_frac_lut[F-i_frac-1] >> (32-F))) >> F;
      }
    }

    // Need to find out whether we're dividing or multiplying for the
    // integer part
    bool is_negative =
      std::numeric_limits<B>::is_signed && ((1ULL << (I+F-1)) & arg.value_);

    // If the number is negative, we need to do some two's complement to get the
    // integer part then work our way up from the LSB
    if (is_negative) {
      B integer_part = ~(arg.value_ >> F) + 1;
      for (int i_int = 0; i_int<I; ++i_int) {
	if (integer_part & 1ULL<<i_int) {
	  result.value_ =
	    (static_cast<typename TypePromotion<B>::type>(result.value_) << F) /
	    (exp_int_lut[i_int] >> (32-F));
	}
      }
    // If the number is positive, we start from the MSB in the integer part and
    // work our way up the number of integer bits
    } else {
      for (int i_int = F; i_int<(I+F); ++i_int) {
	if (arg.value_ & 1ULL<<i_int) {
	  result.value_ =
	    (static_cast<typename TypePromotion<B>::type>(result.value_) *
	     (exp_int_lut[i_int-F]) >> (32-F)) >> F;
	}
      }
    }

    return result;

  }

private:
  // Alias for the sake of simplifying functions
  B value_;
  static constexpr unsigned char number_integer_bits_ = (unsigned char)I;
  static constexpr unsigned char number_fractional_bits_ = (unsigned char)F;
  static constexpr B two_power_f_ = (1ULL << F);

  // exp[0.5], exp[0.25], exp[0.125], etc... in Q32.32
  static constexpr unsigned long exp_frac_lut[32] = {
    0x00000001a61298e2, 0x0000000148b5e3c4, 0x000000012216045b,
    0x000000011082b578, 0x0000000108205601, 0x0000000104080ab5,
    0x0000000102020156, 0x000000010100802b, 0x0000000100802005,
    0x0000000100400801, 0x0000000100200200, 0x0000000100100080,
    0x0000000100080020, 0x0000000100040008, 0x0000000100020002,
    0x0000000100010001, 0x0000000100008000, 0x0000000100004000,
    0x0000000100002000, 0x0000000100001000, 0x0000000100000800,
    0x0000000100000400, 0x0000000100000200, 0x0000000100000100,
    0x0000000100000080, 0x0000000100000040, 0x0000000100000020,
    0x0000000100000010, 0x0000000100000008, 0x0000000100000004,
    0x0000000100000002, 0x0000000100000001
  };
  // exp[1], exp[2], exp[4], etc... in Q32.32
  static constexpr unsigned long exp_int_lut[32] = {
    0x00000002b7e15163, 0x0000000763992e35, 0x0000003699205c4e,
    0x00000ba4f53ea386, 0x0087975e85400100, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff, 0xffffffffffffffff,
    0xffffffffffffffff, 0xffffffffffffffff
  };

};

}

#endif /* #ifndef __TYCHEPLUSPLUS_FIXED_POINT_HPP */


#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <limits>
#include <cassert>
#include <map>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, boost::property<boost::edge_weight_t, int>> Graph;

void testcase()
{
  int n, e, s, source_node, target_node;
  std::cin >> n >> e >> s >> source_node >> target_node;
  assert(n >= 1 && n <= 500 && e >= 1 && s >= 1 && s <= 10);
  assert(source_node >= 0 && source_node < n);
  assert(target_node >= 0 && target_node < n);

  Graph G(n);
  auto shared_weights = boost::get(boost::edge_weight, G);
  int infinite_weight = std::numeric_limits<int>::max();
  std::vector<std::map<Graph::edge_descriptor, int>> weights_by_species(s);
  for (int i = 0; i < e; i++)
  {
    int a, b;
    std::cin >> a >> b;
    Graph::edge_descriptor edge = boost::add_edge(a, b, infinite_weight, G).first;

    for (int j = 0; j < s; j++)
    {
      int w;
      std::cin >> w;
      weights_by_species.at(j).insert(std::make_pair(edge, w));
    }
  }

  for (int i = 0; i < s; i++)
  {
    int hive_location;
    std::cin >> hive_location;
    auto species_weight_map = boost::make_assoc_property_map(weights_by_species.at(i));
    std::vector<Graph::vertex_descriptor> predecessors(n);
    boost::prim_minimum_spanning_tree(G, boost::make_iterator_property_map(predecessors.begin(), boost::get(boost::vertex_index, G)), boost::weight_map(species_weight_map).root_vertex(hive_location));

    for (Graph::vertex_descriptor a = 0; a < Graph::vertex_descriptor(n); a++)
    {
      Graph::vertex_descriptor b = predecessors.at(a);
      if (a == b)
      {
        continue;
      }
      assert(boost::edge(a, b, G).second);
      Graph::edge_descriptor edge = boost::edge(a, b, G).first;
      int &min_weight = shared_weights[edge];
      min_weight = std::min({min_weight, species_weight_map[edge]});
    }
  }

  Graph G_finite(n);
  for (auto edge_iterators = boost::edges(G); edge_iterators.first != edge_iterators.second; edge_iterators.first++)
  {
    auto edge = *edge_iterators.first;
    int w = shared_weights[edge];
    assert(w >= 0);
    if (w != infinite_weight)
    {
      boost::add_edge(edge.m_source, edge.m_target, w, G_finite);
    }
  }

  std::vector<int> distances(n);
  boost::dijkstra_shortest_paths(G_finite, boost::vertex(source_node, G_finite), boost::distance_map(boost::make_iterator_property_map(distances.begin(), boost::get(boost::vertex_index, G_finite))));

  std::cout << distances.at(target_node) << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


/* Copyright (c) 2017, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once

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

#define LOG_2 0.69314718055994529
#define LOG_PI 1.1447298858494002
#define LOG_2PI 1.8378770664093453
#define LOG_4PI 2.5310242469692907

template<typename T>
inline T logBesselI(T nu, T x)
{
  //TODO link against boost
  // for large values of x besselI \approx exp(x)/sqrt(2 PI x)
//  if(x>100.)  return x - 0.5*log(2.*M_PI*x);
//  return log(std::cyl_bessel_i(nu,x));
  return x - 0.5*LOG_2PI -0.5*log(x);
};

template<typename T>
inline T logxOverSinhX(T x) {
  if (fabs(x) < 1e-9)
    return 0.;
  else
    return log(x)-log(sinh(x));
}
template<typename T>
inline T xOverSinhX(T x) {
  if (fabs(x) < 1e-9)
    return 1.;
  else
    return x/sinh(x);
}
template<typename T>
inline T xOverTanPiHalfX(T x) {
  if (fabs(x) < 1e-9)
    return 2./M_PI;
  else
    return x/tan(x*M_PI*0.5);
}

template<typename T>
inline T logSumExp(const Eigen::Matrix<T,Eigen::Dynamic,1>& logX) {
  T logMax = logX.maxCoeff();
  return log((logX.array()-logMax).exp().sum()) + logMax;
}

template <typename T, uint32_t D>
inline T MLEstimateTau(const Eigen::Matrix<T,3,1>& xSum, const
    Eigen::Matrix<T,3,1>& mu, T count) {
  // Need double precision to achive convergence; single is not enough.
  double tau = 1.0;
  double prevTau = 0.;
  double eps = 1e-8;
  double R = xSum.norm()/count;
  while (fabs(tau - prevTau) > eps) {
//    std::cout << "tau " << tau << " R " << R << std::endl;
    double inv_tanh_tau = 1./tanh(tau);
    double inv_tau = 1./tau;
    double f = -inv_tau + inv_tanh_tau - R;
    double df = inv_tau*inv_tau - inv_tanh_tau*inv_tanh_tau + 1.;
    prevTau = tau;
    tau -= f/df;
  }
  return tau;
};

template<typename T, int D>
class vMF
{
public:
  vMF()
    : mu_(0,0,1), tau_(0.), unif_(0.,1.), gauss_(0.,1.)
  {}
  vMF(const Eigen::Matrix<T,D,1>& mu, T tau)
    : mu_(mu), tau_(tau), unif_(0.,1.), gauss_(0.,1.)
  {}
  vMF(const Eigen::Matrix<T,D,1>& tauMu)
    : mu_(tauMu.normalized()), tau_(tauMu.norm()), unif_(0.,1.), gauss_(0.,1.)
  {}
  vMF(const vMF<T,D>& vmf)
    : mu_(vmf.mu_), tau_(vmf.tau_), unif_(0.,1.), gauss_(0.,1.)
  {}

  T logPdf(const Eigen::Matrix<T,D,1>& x) const {
    const T d = static_cast<T>(D);
    if (tau_ < 1e-9) {
      // TODO insert general formula here
      return -LOG_4PI;
    } else {
      return (d/2. -1.)*log(tau_) - (d/2.)*LOG_2PI
        - logBesselI<T>(d/2. -1.,tau_) + tau_*mu_.dot(x);
    }
  }

  /// Use uniform distribution on the sphere as a proposal distribution
  Eigen::Matrix<T,D,1> sample(std::mt19937& rnd) {
//    std::cout << "stating n sampling ------------ " << std::endl;
    // implemented using rejection sampling and proposals from a gaussian
    Eigen::Matrix<T,D,1> x;
    T pdf_g = -LOG_4PI;
    // bound via maximum over vMF at mu
    // TODO: dont know why I need to multiply by 2 here to get the
    // correct samples (as seen in concentration estimates)
    T M = 2.*tau_;
    while(42) {
      // sample from zero mean Gaussian
      for (uint32_t d=0; d<D; d++) x[d] = gauss_(rnd);
      x.normalize();
      // rejection sampling (in log domain)
      T u = log(unif_(rnd));
      T pdf_f = 2.*tau_*x.dot(mu_); //this->logPdf(x);
//      std::cout << pdf_f << " " << pdf_g << " " << M << " " << tau_ << std::endl;
      if(u < pdf_f-(M+pdf_g)) break;
    };
    return x;
  }

  Eigen::Matrix<T,D,1> mu_;
  T tau_;
private:
  std::uniform_real_distribution<T> unif_;
  std::normal_distribution<T> gauss_;
};

template<>
float vMF<float,3>::logPdf(const Eigen::Matrix<float,3,1>& x) const {
  if (tau_ < 1e-9) {
    return -LOG_4PI;
  } else {
    return -LOG_2PI + log(tau_) + tau_*(mu_.dot(x)-1.) - log(1.-exp(-2.*tau_));
//    return 0.5*LOG_PI - 0.5*LOG_2 + tau_*mu_.dot(x) + logxOverSinhX(tau_);
  }
}

template<>
Eigen::Matrix<float,3,1> vMF<float,3>::sample(std::mt19937& rnd) {
  if (tau_ < 1e-10) {
    return Eigen::Vector3f(gauss_(rnd), gauss_(rnd), gauss_(rnd)).normalized();
  }
  // https://www.mitsuba-renderer.org/~wenzel/files/vmf.pdf
  // sample around (0,0,1)
  Eigen::Vector2f v(gauss_(rnd), gauss_(rnd));
  v.normalize();
  const float u = unif_(rnd);
  const float w = 1. + log(u+(1.-u)*exp(-2.*tau_))/tau_;
  const float a = sqrtf(1.-w*w);
  Eigen::Vector3f x(a*v(0), a*v(1), w);

  // rotate to mu
  Eigen::Vector3f axis = Eigen::Vector3f(0,0,1).cross(mu_);
  float angle = acos(mu_[2]);

  if (fabs(angle) <1e-9)
    return x;

  Eigen::Quaternion<float> q(cos(angle*0.5),
      sin(angle*0.5)*axis(0)/axis.norm(),
      sin(angle*0.5)*axis(1)/axis.norm(),
      sin(angle*0.5)*axis(2)/axis.norm());
  return q._transformVector(x);
}


#include <Eigen/Dense>
#include <iostream>

int main(int, char *[]) {
  // AngleAxisf aa = Quaternionf(..);
  Eigen::VectorXd vq(4);
  vq[0] = 1.0;
  vq[1] = 0.0;
  vq(2) = 0.0;
  vq(3) = 0.0;
  Eigen::Matrix<float, 4, 1> coe;
  Eigen::Matrix<float, 3, 3> R;
  Eigen::Quaternionf q(2.0, 0.1, 0.3, 4.0);  // wxyz
  coe = q.coeffs();
  std::cout << coe << std::endl;  // xyzw

  Eigen::Vector3f vec = q.vec();  // xyz
  std::cout << vec << std::endl;

  Eigen::AngleAxisf aa;
  Eigen::Matrix3f mat;
  Eigen::Quaternionf qm(mat);
  Eigen::Quaternionf qa(aa);

  Eigen::Quaternionf qw;
  qw.setIdentity();  // important
  R = qw.toRotationMatrix();
  qw = qw * q;
  qw = qw.normalized();  //规范化
  std::cout << R << std::endl;
  qw = qw.Identity();  // 1 0 0 0 or qw.setIdentity();
  std::cout << qw.w() << "  " << qw.x() << "  " << qw.y() << "  " << qw.z()
            << std::endl;
  qw = qw.inverse();
  std::cout << qw.w() << "  " << qw.x() << "  " << qw.y() << "  " << qw.z()
            << std::endl;

  Eigen::Vector3f w1;
  w1 << 1, 2, 3;
  Eigen::Vector3f w2;
  w2 << 2, 3, 4;
  qw = qw.Identity();
  qw = qw.setFromTwoVectors(w1, w2);  //出来的norm=1--姿态四元数
  std::cout << qw.squaredNorm() << std::endl;  // norm^2  .norm()
  q.setIdentity();
  qw = qw.normalized();
  q = q.normalized();
  q = q.inverse();
  std::cout << q.angularDistance(qw) << std::endl;  //必须先规范化
  std::cout << qw.dot(q) << std::endl;              // dot product 内积
  std::cout << qw.w() << "  " << qw.x() << "  " << qw.y() << "  " << qw.z()
            << std::endl;
  std::cout << q.w() << "  " << q.x() << "  " << q.y() << "  " << q.z()
            << std::endl;

  // std::cout<<q;
  std::cout << std::endl;
  for (int size = 1; size <= 4; ++size) {
    Eigen::MatrixXi m(size, size + 1);    // a (size)x(size+1)-matrix of int's
    for (int j = 0; j < m.cols(); ++j)    // loop over columns
      for (int i = 0; i < m.rows(); ++i)  // loop over rows
        m(i, j) = i + j * m.rows();       // to access matrix coefficients,
    // use operator()(int,int)
    std::cout << m << "\n\n";
  }
  Eigen::VectorXf v(4);  // a vector of 4 float's
  // to access vector coefficients, use either operator () or operator []
  v[0] = 1;
  v[1] = 2;
  v(2) = 3;
  v(3) = 4;
  std::cout << "\nv:\n" << v << std::endl;
}


#include <complex>
#include <fstream>
#include <iostream>
#include <numeric>
#include <vector>
using namespace std;
using namespace std::complex_literals;

#include <boost/math/special_functions/bessel.hpp>
#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <ieompp/algebra/monomial.hpp>
#include <ieompp/algebra/operator.hpp>
#include <ieompp/constants.hpp>
#include <ieompp/lattices/periodic_chain.hpp>
#include <ieompp/models/hubbard_real_space/basis.hpp>
#include <ieompp/models/hubbard_real_space/expectation_value.hpp>
#include <ieompp/openmp.hpp>
namespace hubbard = ieompp::models::hubbard_real_space;

complex<double> minus_i_power(uint64_t power)
{
    switch(power % 4) {
        case 0:
            return 1.;
        case 1:
            return -1.i;
        case 2:
            return -1.;
        default:
            return 1.i;
    }
}

int main()
{
    const uint64_t N = 128;
    ieompp::lattices::PeriodicChain<double> lattice(N, 1.);
    hubbard::Basis1Operator<ieompp::algebra::Monomial<ieompp::algebra::Operator<uint64_t, bool>>>
        basis(lattice);

    const double dt      = 0.01;
    const uint64_t steps = 10000;

    // precompute time dependent prefactors and their complex conjugates
    vector<vector<complex<double>>> h_vals(N), h_vals_conj(N);
#pragma omp parallel for
    for(uint64_t i = 0; i < N; ++i) {
        h_vals[i].resize(steps);
        h_vals_conj[i].resize(steps);
        const auto j                   = lattice.lattice_distance(0, i);
        std::complex<double> prefactor = minus_i_power(j);
        for(uint64_t step = 0; step < steps; ++step) {
            double bess          = boost::math::cyl_bessel_j(j, 2 * step * dt);
            h_vals[i][step]      = bess * prefactor;
            h_vals_conj[i][step] = bess * std::conj(prefactor);
        }
    }

    const hubbard::ExpectationValue1DHalfFilled<double, decltype(lattice)> expectation_value(
        lattice, 1., 0.5);

    vector<vector<complex<double>>> results;
#pragma omp parallel
    {
#pragma omp critical
        {
            results.emplace_back(steps, complex<double>(0.));
        }
    }

#pragma omp parallel for
    for(uint64_t i = 0; i < N; ++i) {
        for(uint64_t j = 0; j < N; ++j) {
            const auto ev = expectation_value(basis[i].front().index1, basis[j].front().index1);
            for(uint64_t step = 0; step < steps; ++step) {
                results[omp_get_thread_num()][step] +=
                    ev * (h_vals[i][step] * h_vals_conj[j][step]);
            }
        }
    }

    ofstream file("theory.txt", ofstream::trunc);
    for(uint64_t step = 0; step < steps; ++step) {
        complex<double> val = 0.;
        for(auto &result : results) {
            val += result[step];
        }
        file << step * dt << '\t' << val.real() << '\t' << val.imag() << '\n';
    }
    file.close();
}


#pragma once

#include <algorithm>
#include <fstream>
#include <iostream>
#include <list>
#include <stdio.h>

#include <Eigen/Dense>

#include "configuration.hpp"
#include "utils/util.hpp"

namespace util {
double SmoothPos(double ini, double end, double moving_duration,
                 double curr_time);
double SmoothVel(double ini, double end, double moving_duration,
                 double curr_time);
double SmoothAcc(double ini, double end, double moving_duration,
                 double curr_time);
void SinusoidTrajectory(double initTime_, const Eigen::VectorXd &midPoint_,
                        const Eigen::VectorXd &amp_,
                        const Eigen::VectorXd &freq_, double evalTime_,
                        Eigen::VectorXd &p_, Eigen::VectorXd &v_,
                        Eigen::VectorXd &a_, double smoothing_dur = 1.0);
double Smooth(double ini, double fin, double rat);
} // namespace util

class HermiteCurve {
public:
  HermiteCurve();
  HermiteCurve(const double &start_pos, const double &start_vel,
               const double &end_pos, const double &end_vel);
  ~HermiteCurve();
  void Initialize(const double &start_pos, const double &start_vel,
                  const double &end_pos, const double &end_vel);
  double Evaluate(const double &s_in);
  double EvaluateFirstDerivative(const double &s_in);
  double EvaluateSecondDerivative(const double &s_in);

private:
  double p1_;
  double v1_;
  double p2_;
  double v2_;

  double s_;
};

class HermiteCurveVec {
public:
  HermiteCurveVec();
  HermiteCurveVec(const Eigen::VectorXd &start_pos,
                  const Eigen::VectorXd &start_vel,
                  const Eigen::VectorXd &end_pos,
                  const Eigen::VectorXd &end_vel);

  void Initialize(const Eigen::VectorXd &start_pos,
                  const Eigen::VectorXd &start_vel,
                  const Eigen::VectorXd &end_pos,
                  const Eigen::VectorXd &end_vel);

  ~HermiteCurveVec();
  Eigen::VectorXd Evaluate(const double &s_in);
  Eigen::VectorXd EvaluateFirstDerivative(const double &s_in);
  Eigen::VectorXd EvaluateSecondDerivative(const double &s_in);

private:
  Eigen::VectorXd p1_;
  Eigen::VectorXd v1_;
  Eigen::VectorXd p2_;
  Eigen::VectorXd v2_;

  std::vector<HermiteCurve> curves_;
  Eigen::VectorXd output_;
};

class HermiteQuaternionCurve {
public:
  HermiteQuaternionCurve();
  HermiteQuaternionCurve(const Eigen::Quaterniond &quat_start,
                         const Eigen::Vector3d &angular_velocity_start,
                         const Eigen::Quaterniond &quat_end,
                         const Eigen::Vector3d &angular_velocity_end);
  ~HermiteQuaternionCurve();

  void Initialize(const Eigen::Quaterniond &quat_start,
                  const Eigen::Vector3d &angular_velocity_start,
                  const Eigen::Quaterniond &quat_end,
                  const Eigen::Vector3d &angular_velocity_end);

  // All values are expressed in "world frame"
  void Evaluate(const double &s_in, Eigen::Quaterniond &quat_out);
  void GetAngularVelocity(const double &s_in, Eigen::Vector3d &ang_vel_out);
  void GetAngularAcceleration(const double &s_in, Eigen::Vector3d &ang_acc_out);

private:
  Eigen::Quaterniond qa;   // Starting quaternion
  Eigen::Vector3d omega_a; // Starting Angular Velocity
  Eigen::Quaterniond qb;   // Ending quaternion
  Eigen::Vector3d omega_b; // Ending Angular velocity

  Eigen::AngleAxisd omega_a_aa; // axis angle representation of omega_a
  Eigen::AngleAxisd omega_b_aa; // axis angle representation of omega_b

  void initialize_data_structures();

  void computeBasis(const double &s_in); // computes the basis functions
  void computeOmegas();

  Eigen::Quaterniond q0; // quat0
  Eigen::Quaterniond q1; // quat1
  Eigen::Quaterniond q2; // quat1
  Eigen::Quaterniond q3; // quat1

  double b1; // basis 1
  double b2; // basis 2
  double b3; // basis 3

  double bdot1; // 1st derivative of basis 1
  double bdot2; // 1st derivative of basis 2
  double bdot3; // 1st derivative of basis 3

  double bddot1; // 2nd derivative of basis 1
  double bddot2; // 2nd derivative of basis 2
  double bddot3; // 2nd derivative of basis 3

  Eigen::Vector3d omega_1;
  Eigen::Vector3d omega_2;
  Eigen::Vector3d omega_3;

  Eigen::AngleAxisd omega_1aa;
  Eigen::AngleAxisd omega_2aa;
  Eigen::AngleAxisd omega_3aa;

  // Allocate memory for quaternion operations
  Eigen::Quaterniond qtmp1;
  Eigen::Quaterniond qtmp2;
  Eigen::Quaterniond qtmp3;

  // progression variable
  double s_;
};

class MinJerkCurve {
public:
  // Constructors
  MinJerkCurve();
  MinJerkCurve(const Eigen::Vector3d &init, const Eigen::Vector3d &end,
               const double &time_start, const double &time_end);

  void SetParams(const Eigen::Vector3d &init, const Eigen::Vector3d &end,
                 const double &time_start, const double &time_end);

  void GetPos(const double &time, double &pos);
  void GetVel(const double &time, double &vel);
  void GetAcc(const double &time, double &acc);

  // Destructor
  ~MinJerkCurve();

private:
  Eigen::MatrixXd C_mat;     // Matrix of Coefficients
  Eigen::MatrixXd C_mat_inv; // Inverse of Matrix of Coefficients
  Eigen::VectorXd
      a_coeffs; // mininum jerk coeffs. a = [a0, a1, a2, a3, a4, a5, a6];
  Eigen::VectorXd bound_cond; // boundary conditions x_b = [ x(to), xdot(to),
                              // xddot(to), x(tf), xdot(tf), xddot(tf)]

  Eigen::Vector3d init_cond; // initial pos, vel, acceleration
  Eigen::Vector3d end_cond;  // final pos, vel, acceleration
  double to;                 // Starting time
  double tf;                 // Ending time

  void Initialization();

  // Compute the coefficients
  void compute_coeffs();
};

class MinJerkCurveVec {
public:
  MinJerkCurveVec();
  MinJerkCurveVec(const Eigen::VectorXd &start_pos,
                  const Eigen::VectorXd &start_vel,
                  const Eigen::VectorXd &start_acc,
                  const Eigen::VectorXd &end_pos,
                  const Eigen::VectorXd &end_vel,
                  const Eigen::VectorXd &end_acc, double duration);
  ~MinJerkCurveVec();
  Eigen::VectorXd Evaluate(const double &t_in);
  Eigen::VectorXd EvaluateFirstDerivative(const double &t_in);
  Eigen::VectorXd EvaluateSecondDerivative(const double &t_in);

private:
  double Ts_;

  Eigen::VectorXd p1_;
  Eigen::VectorXd v1_;
  Eigen::VectorXd a1_;

  Eigen::VectorXd p2_;
  Eigen::VectorXd v2_;
  Eigen::VectorXd a2_;

  std::vector<MinJerkCurve> curves_;
  Eigen::VectorXd output_;
};


#include "inf_pers2cyc.h"
#define _USE_MATH_DEFINES
#include <cmath>
#include <tuple>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#include <boost/graph/graph_utility.hpp>
#include "utils.h"
#include "mesh_writer.h"
#include "tests.h"
#include "flow_graph.hpp"

const size_t MAX_N_INTERVALS = 1000;

struct CompPersPairsByFiltIndex {
    CompPersPairsByFiltIndex(Bitmap_cubical_complex* _complex) : complex(_complex) {}

    bool operator()(const std::pair<int,int>& p1, const std::pair<int,int>& p2) {
        return complex->key(p1.second) - complex->key(p1.first) >
               complex->key(p2.second) - complex->key(p2.first);
    }

    Bitmap_cubical_complex* complex;
};

struct CompPersPairsByFuncVal {
    CompPersPairsByFuncVal(Bitmap_cubical_complex* _complex) : complex(_complex) {}

    bool operator()(const std::pair<int,int>& p1, const std::pair<int,int>& p2) {
        return complex->filtration(p1.second) - complex->filtration(p1.first) >
               complex->filtration(p2.second) - complex->filtration(p2.first);
    }

    Bitmap_cubical_complex* complex;
};

void computePersistenceCube(
    const std::string& perseus_fname,
    const IntervSortType interv_sort_type,
    const ExecOptions& ops,
    std::string* filt_fname) {

    Bitmap_cubical_complex bm_cube_cmplx(perseus_fname.c_str());
    cout << endl << "bitmap complex load done" << endl;

    Persistent_cohomology pcoh(bm_cube_cmplx);
    pcoh.init_coefficients(2);
    pcoh.compute_persistent_cohomology(0.0);
    auto pers_pairs = pcoh.get_persistent_pairs();

    vector<std::pair<int,int>> pers_pairs_dim2;
    for (auto p : pers_pairs) {
        if (bm_cube_cmplx.dimension(get<0>(p)) == 2) {
            pers_pairs_dim2.emplace_back(get<0>(p), get<1>(p));
        }
    }

    if (interv_sort_type == BY_FILT_INDEX) {
        CompPersPairsByFiltIndex comp(&bm_cube_cmplx);
        std::sort(pers_pairs_dim2.begin(), pers_pairs_dim2.end(), comp);
    } else {
        CompPersPairsByFuncVal comp(&bm_cube_cmplx);
        std::sort(pers_pairs_dim2.begin(), pers_pairs_dim2.end(), comp);
    }

    cout << endl << "computing persistence done, total intervals: "
        << pers_pairs_dim2.size() << endl;

    int dim, x_res, y_res, z_res;

    std::ifstream fin(perseus_fname.c_str());
    fin >> dim;
    if (dim != 3) {
        cout << "FATAL: perseus file dim not 3" << endl;
        exit(-1);
    }

    fin >> x_res >> y_res >> z_res;

    std::string purename;
    getFilePurename(perseus_fname, &purename);

    *filt_fname = purename
        + (interv_sort_type == BY_FILT_INDEX ? "_IX" : "_FV")
        + ".filt";

    cout << endl << "writing filt file '"
        << *filt_fname << "'" << endl;

    writeFiltCube(
        &bm_cube_cmplx, x_res, y_res, z_res,
        pers_pairs_dim2, *filt_fname);
}

void writeFiltCube(Bitmap_cubical_complex* bm_cube_cmplx,
    const int x_res, const int y_res, const int z_res,
    vector<std::pair<int,int>> pers_pairs_dim2,
    const std::string& filename) {

    int num_intv = std::min(pers_pairs_dim2.size(), MAX_N_INTERVALS);

    // cell count of every dimension for each pair.
    // indexed the same as 'pers_pairs_dim2'.
    vector<array<int,4>> dim_cell_count_all(num_intv, {-1, -1, -1, -1});
    vector<array<int,2>> pairs_key_index(num_intv);

    for (auto i = 0; i < num_intv; i ++) {
        // i-th pair in 'pers_pairs_dim2'
        auto p = pers_pairs_dim2[i];
        pairs_key_index[i][0] = bm_cube_cmplx->key(p.first);
        pairs_key_index[i][1] = i;
    }

    struct  {
        bool operator()(const array<int,2>& ki1, const array<int,2>& ki2) {
            return ki1[0] < ki2[0];
        }
    } comp;

    std::sort(pairs_key_index.begin(), pairs_key_index.end(), comp);
    // for (auto ki : pairs_key_index) {
    //     cout << ki[0] << " " << ki[1] << endl;
    // }

    // calculate the cell count of every dimension for all pairs
    array<int,4> dim_cell_count {0, 0, 0, 0};
    int cur_ind = 0;

    for (auto key = 0;
        key < bm_cube_cmplx->num_simplices() &&
        cur_ind < num_intv;
        key ++) {

        auto cell_id = bm_cube_cmplx->simplex(key);
        dim_cell_count[bm_cube_cmplx->dimension(cell_id)] ++;

        if (key == pairs_key_index.at(cur_ind).at(0)) {
            auto intv_ind = pairs_key_index.at(cur_ind).at(1);
            dim_cell_count_all.at(intv_ind) = dim_cell_count;
            cur_ind ++;
        }
    }

    if (cur_ind != num_intv) {
        cout << "FATAL: cur_ind != num_intv in writeFiltCube" << endl;
        exit(-1);
    }

    std::ofstream fout(filename);

    fout << num_intv << endl;

    for (auto i = 0; i < num_intv; i ++) {
        auto p = pers_pairs_dim2[i];

        fout << i << " "                              // 0: seq of interval
            << dim_cell_count_all.at(i).at(2)         // 1: count of 2 and 3-cells
            + dim_cell_count_all.at(i).at(3) << " "
            << bm_cube_cmplx->key(p.first) << " "     // 2: start key
            << bm_cube_cmplx->key(p.second) << " "    // 3: end key
            << p.first << " "                         // 4: start bm id
            << bm_cube_cmplx->key(p.second)           // 5: index length
            - bm_cube_cmplx->key(p.first) << " "
            << bm_cube_cmplx->filtration(p.second)    // 6: function value length
            - bm_cube_cmplx->filtration(p.first) << " "
            << dim_cell_count_all.at(i).at(1) << " "  // 7: 1-cell tount
            << dim_cell_count_all.at(i).at(2) << " "  // 8: 2-cell tount
            << dim_cell_count_all.at(i).at(3) << endl;// 9: 3-cell tount
    }

    fout << endl;

    // the given resolution is in terms of 3-d cells (cubes), while
    // the resolution for the filt file as well as for CubeComplex
    // is in terms of 0-d cells (vertices).
    fout << x_res+1 << " " << y_res+1 << " " << z_res+1 << endl << endl;

    vector<int> verts;
    for (auto key = 0; key <= bm_cube_cmplx->num_simplices(); key ++) {
        auto cell_id = bm_cube_cmplx->simplex(key);
        auto dim = bm_cube_cmplx->dimension(cell_id);

        if (dim == 2 || dim == 3) {
            getBitmapComplexCellVerts(cell_id, dim,
                x_res, y_res, z_res, &verts);

            fout << verts[0];
            for (auto i = 1; i < verts.size(); i ++) {
                fout << ' ' << verts[i];
            }
            fout << endl;
        }
    }
}

void infPers2CycsFromFile(
    const std::string& filt_fname,
    const ComplexType cmplx_type,
    const int start_interval,
    const int num_intervals,
    const ExecOptions& ops) {

    int num_all_intervals;

    {
        std::ifstream fin(filt_fname);
        fin >> num_all_intervals;
    }

    cout << endl << "filt file num of intervals: " << num_all_intervals << endl;

    for (int i = start_interval;
        i < num_all_intervals && i < start_interval + num_intervals;
        i ++) {

        cout << endl << endl << "---- " << i << "-th interval ----" << endl;

        // this complex will be deleted in infPers2Cyc
        CellComplex* complex;
        if (cmplx_type == CUBE_CMPLX) {
            complex = new CubeComplex();
        }
        complex->init(1,3);

        vector<int> pos_cell_2d_verts;
        int start_key = -1, start_id = -1;

        // loadToyCubeComplex((CubeComplex*)complex, &pos_cell_2d_verts);
        // std::string purename = "hardcode_toy";

        loadComplex(filt_fname, cmplx_type, complex,
            i, &start_key, &start_id, &pos_cell_2d_verts);

        char buf[50];
        snprintf(buf, 50, "%03d_", i);

        std::string purename;
        getFilePurename(filt_fname, &purename);
        purename = purename + buf + std::to_string(start_key)
            + "_" + std::to_string(start_id);

        if (ops.verbose) {
            cout << endl << "positive cell: " << pos_cell_2d_verts << endl;
        }
        cout << endl << "load complex done" << endl;

        if (ops.write_intem) {
            complex->writeMesh(purename + "_orig.off");
            // ((CubeComplex*)complex)->writeEdges(purename + "_orig_edges.ply");
        }

        infPers2Cyc(cmplx_type, complex, pos_cell_2d_verts, ops, purename);
    }
}

void loadComplex(
    const std::string& filt_fname,
    const ComplexType cmplx_type,
    CellComplex* complex,
    const int interval_id,
    int* interv_start_key,
    int* interv_start_id,
    vector<int>* pos_cell_2d_verts) {

    std::ifstream fin(filt_fname);

    int num_all_intervals, interv_cell_count;
    array<int,4> interv_dim_cell_count;

    fin >> num_all_intervals;

    for (int i = 0; i < num_all_intervals; i ++) {
        int interv_seq, cell_count, start_key, end_key, len_idx, start_id;
        double len_fv;
        array<int,4> dim_cell_count;

        fin >> interv_seq >> cell_count >> start_key >> end_key >> start_id
            >> len_idx >> len_fv
            >> dim_cell_count[1] >> dim_cell_count[2] >> dim_cell_count[3];

        if (i == interval_id) {
            interv_cell_count = cell_count;
            interv_dim_cell_count = dim_cell_count;
            *interv_start_key = start_key;
            *interv_start_id = start_id;
        }
    }

    if (cmplx_type == CUBE_CMPLX) {
        int x_res, y_res, z_res;
        fin >> x_res >> y_res >> z_res;
        ((CubeComplex*)complex)->setResolution(x_res, y_res, z_res);
    }

    complex->reserveCellSize(1, interv_dim_cell_count[1]);
    complex->reserveCellSize(2, interv_dim_cell_count[2]);
    complex->reserveCellSize(3, interv_dim_cell_count[3]);

    std::string line, s;
    vector<int> verts;

    for (int k = 0; k < interv_cell_count;) {
        if (!std::getline(fin, line)) {
            cout << "FATAL: reach end of file but not finish reading in loadComplex" << endl;
            exit(-1);
        }

        if (line.compare("") == 0) {
            continue;
        }

        std::istringstream iss(line);
        verts.clear();
        while (getline(iss, s, ' ')) {
            verts.push_back(std::stoi(s));
        }

        // cout << verts << endl;

        if (verts.size() == complex->getVertCnt(2)) {
            complex->toCanonVerts(2, &verts);
            complex->addCellNoConvCanon(2, verts);
        } else if (verts.size() == complex->getVertCnt(3)) {
            complex->toCanonVerts(3, &verts);
            complex->addCellNoConvCanon(3, verts);
        } else {
            cout << "FATAL: filt file has cells other than 2d or 3d" << endl;
        }

        k ++;
    }

    if (verts.size() != complex->getVertCnt(2)) {
        cout << "FATAL: positive cell vert count not right in loadComplex" << endl;
        exit(-1);
    }

    *pos_cell_2d_verts = verts;
}

void infPers2Cyc(
    const ComplexType cmplx_type, CellComplex* orig_complex,
    const vector<int>& pos_cell_2d_verts, const ExecOptions& ops,
    const std::string& file_prefix) {

    /* pruning */

    pruneComplex(orig_complex);

    // orig_complex->print();
    cout << endl << "prune done" << endl;

    if (ops.verbose) {
        cout << endl << "orig_complex map load:" << endl;
        orig_complex->printLoadStat();
    }

    if (ops.write_intem) {
        orig_complex->writeMesh(file_prefix + "_pruned.off");
    }

    /* get 2-connected component */

    auto start_cell_2d_id = orig_complex->getCellIdNoConvCanon(2, pos_cell_2d_verts);
    if (start_cell_2d_id < 0) {
        cout << "FATAL: start_cell_2d_id < 0 in infPers2Cyc" << endl;
        exit(-1);
    }

    auto orig_complex_cell_1d_cnt = orig_complex->getCellCount(1);
    auto orig_complex_cell_2d_cnt = orig_complex->getCellCount(2);
    auto orig_complex_cell_3d_cnt = orig_complex->getCellCount(3);

    orig_complex->deleteDimCofaces(2);
    orig_complex->deleteDimVertQueryMap(2);

    CellComplex* conn_complex;

    if (cmplx_type == CUBE_CMPLX) {
        conn_complex = new CubeComplex();
        ((CubeComplex*)conn_complex)->setResolution(
            ((CubeComplex*)orig_complex)->getXRes(),
            ((CubeComplex*)orig_complex)->getYRes(),
            ((CubeComplex*)orig_complex)->getZRes());
    }

    conn_complex->init(1,3);
    conn_complex->setCofaceCountHint(2, 2);

    // just some hints
    conn_complex->reserveCellSize(1, orig_complex_cell_1d_cnt/4+1);
    conn_complex->reserveCellSize(2, orig_complex_cell_2d_cnt/4+1);
    conn_complex->reserveCellSize(3, orig_complex_cell_3d_cnt/4+1);

    cellConnectedComponent(orig_complex, start_cell_2d_id, conn_complex);

    int conn_compnt_cell_2d_cnt = conn_complex->getCellCount(2);

    if (ops.write_intem) {
        conn_complex->writeMesh(file_prefix + "_conn.off");
    }

    /* add necessary 3-cells */

    vector<int> cell_3d_verts;
    vector<int> f_2d_verts;

    for (auto cell_3d_id = 0;
        cell_3d_id < orig_complex->getCellSize(3);
        cell_3d_id ++) {

        if (orig_complex->isCellValid(3, cell_3d_id)) {
            orig_complex->getCellVerts(3, cell_3d_id, &cell_3d_verts);
            orig_complex->getFaceVerts(3, cell_3d_verts, 0, &f_2d_verts);

            if (conn_complex->getCellId(2, f_2d_verts) >= 0) {
                conn_complex->addCellNoConvCanon(3, cell_3d_verts);
            }
        }
    }

    if (conn_compnt_cell_2d_cnt != conn_complex->getCellCount(2)) {
        cout << "FATAL: 2-cell count not equal after 3-cells adding" << endl;
        exit(-1);
    }

    cout << endl << "2-conn component done" << endl;

    if (ops.verbose) {
        cout << endl <<  "conn_complex map load:" << endl;
        conn_complex->printLoadStat();
    }

    delete orig_complex;
    orig_complex = NULL;

    /* reconstruct void boundaries */

    vector<array<int,2>>* cell_2d_void_map =
        new vector<array<int,2>>(conn_complex->getCellSize(2), {-1, -1});
    int void_cnt;

    reconVoidBound(cmplx_type, conn_complex, cell_2d_void_map, ops, file_prefix, &void_cnt);

    cout << endl << "void boundary reconstruction done" << endl;

    // free some memory space

    int src, sink;
    {
        auto pos_cell_2d_id = conn_complex->
            getCellIdNoConvCanon(2, pos_cell_2d_verts);

        vector<int> void_ids;
        getCell2dVoidIds(pos_cell_2d_id, conn_complex, cell_2d_void_map, &void_ids);
        src = void_ids.at(0);
        sink = void_ids.at(1);

        // auto cofaces = conn_complex->getCellCofaces(2, pos_cell_2d_id);
        // src = cofaces->at(0);
        // sink = cofaces->at(1);
    }

    if (src == sink) {
        cout << "FATAL: src sink the same in infPers2Cyc" << endl;
        exit(-1);
    }

    conn_complex->deleteDimCellData(1);
    conn_complex->deleteDimCellData(3);
    conn_complex->deleteDimVertQueryMap(2);

    /* record the corresponding set of 2-cells for each graph edge */

    GEdgeCellMap* gedge_cell_map = new GEdgeCellMap(conn_complex->getCellCount(2)/4+1);

    std::pair<int,int> gedge;
    const vector<int> empty_vector;
    for (auto cell_2d_id = 0;
        cell_2d_id < conn_complex->getCellSize(2);
        cell_2d_id ++) {

        if (conn_complex->isCellValid(2, cell_2d_id)) {
            vector<int> void_ids;
            getCell2dVoidIds(cell_2d_id, conn_complex, cell_2d_void_map, &void_ids);

            if (void_ids.at(0) != void_ids.at(1)) {
                gedge.first = void_ids.at(0);
                gedge.second = void_ids.at(1);
                toCanonEdge(&gedge);

                auto iter = gedge_cell_map->find(gedge);
                if (iter == gedge_cell_map->end()) {
                    std::tie(iter, std::ignore) =
                        gedge_cell_map->insert({gedge, empty_vector});
                    iter->second.reserve(1);
                    iter->second.push_back(cell_2d_id);
                } else {
                    iter->second.push_back(cell_2d_id);
                }
            }
        }
    }

    if (ops.verbose) {
        cout << endl << "gedge_cell_map: ";
        printHashMapLoad(*gedge_cell_map);
    }
    // cout << endl;
    // printGEdgeCellMap(gedge_cell_map, conn_complex);

    /* compute the min cyc by flow network */

    delete cell_2d_void_map;
    conn_complex->deleteDimCofaces(2);

    if (cmplx_type == CUBE_CMPLX) {
        long max_w = 0;

        for (auto iter = gedge_cell_map->begin();
            iter != gedge_cell_map->end(); iter ++) {

            if (iter->second.size() > max_w) {
                max_w = iter->second.size();
            }
        }

        if (ops.verbose) {
            cout << endl << "max_w: " << max_w << endl;
        }

        // TODO: use cpp limits instead
        if (max_w <= 127/2) {
            if (ops.verbose) {
                cout << "use 'char' as weight type" << endl;
            }

            computeMinCyc<char>(
                conn_complex, gedge_cell_map, void_cnt,
                src, sink, ops, file_prefix);
        } else if (max_w <= 32767/2) {
            if (ops.verbose) {
                cout << "use 'short' as weight type" << endl;
            }

            computeMinCyc<short>(
                conn_complex, gedge_cell_map, void_cnt,
                src, sink, ops, file_prefix);
        } else {
            if (ops.verbose) {
                cout << "use 'int' as weight type" << endl;
            }

            computeMinCyc<int>(
                conn_complex, gedge_cell_map, void_cnt,
                src, sink, ops, file_prefix);
        }
    }

    cout << endl << "compute min cycle done" << endl;

    delete gedge_cell_map;
    delete conn_complex;
}

void cellConnectedComponent(
    CellComplex* in_complex, const int start_cell_2d_id, CellComplex* out_complex) {

    vector<bool> cell_2d_visited(in_complex->getCellSize(2), false);

    cell_2d_visited[start_cell_2d_id] = true;
    vector<int> cell_stack { start_cell_2d_id };

    // vertices container for the 2-cell being deleted
    vector<int> cell_2d_verts;
    // vertices container for 1-faces of the 2-cell being deleted
    vector<int> f_verts;

    while (!cell_stack.empty()) {
        auto cell_2d_id = cell_stack[cell_stack.size() - 1];
        cell_stack.pop_back();
        in_complex->getCellVerts(2, cell_2d_id, &cell_2d_verts);

#ifdef DEBUG_OPT
        if (out_complex->getCellIdNoConvCanon(2, cell_2d_verts) >= 0) {
            cout << "FATAL: cell in out_complex" << endl;
            exit(-1);
        }
#endif

        out_complex->addCellNoConvCanon(2, cell_2d_verts);

        for (auto i = 0; i < in_complex->getFaceCnt(2); i ++) {
            in_complex->getFaceVerts(2, cell_2d_verts, i, &f_verts);
            auto face_id = in_complex->getCellId(1, f_verts);
            const vector<int>* cofaces = in_complex->getCellCofaces(1, face_id);

            for (auto j = 0; j < cofaces->size(); j ++) {
#ifdef DEBUG_OPT
                if ( !in_complex->isCellValid( 2, cofaces->at(j) ) ) {
                    cout << "FATAL: coface invalid in cellConnectedComponent" << endl;
                    exit(-1);
                }
#endif

                if (!cell_2d_visited[ cofaces->at(j) ]) {
                    cell_stack.push_back( cofaces->at(j) );
                    cell_2d_visited[ cofaces->at(j) ] = true;
                }
            }
        }
    }
}

void pruneComplex(CellComplex* complex) {

    std::unordered_set<int> one_cells_w1cof;
    for (auto cell_1d_id = 0; cell_1d_id < complex->getCellSize(1); cell_1d_id ++) {
        if (complex->isCellValid(1, cell_1d_id)) {
            if (complex->getCellCofaces(1, cell_1d_id)->size() == 1) {
                one_cells_w1cof.insert(cell_1d_id);
            }
        }
    }

    // vertices container for the 2-cell being deleted
    vector<int> cell_2d_verts;
    // vertices container for 1-faces of the 2-cell being deleted
    vector<int> f_verts;

    while (!one_cells_w1cof.empty()) {
        auto cell_1d_id = *(one_cells_w1cof.begin());
        // the 2-cell being deleted
        auto cell_2d_id = complex->getCellCofaces(1, cell_1d_id)->at(0);

#ifdef DEBUG_OPT
        if (complex->getCellCofaces(1, cell_1d_id)->size() != 1) {
            cout << "FATAL: 1d cell has cofaces not 1" << endl;
            exit(-1);
        }
        if (complex->getCellCofaces(2, cell_2d_id)->size() != 0) {
            cout << "FATAL: 2d cell has coface in prune" << endl;
            exit(-1);
        }
#endif

        complex->getCellVerts(2, cell_2d_id, &cell_2d_verts);
        complex->deleteCell(2, cell_2d_id);

        // enumerate all faces of the 2-cell being deleted
        for (auto i = 0; i < complex->getFaceCnt(2); i ++) {
            complex->getFaceVerts(2, cell_2d_verts, i, &f_verts);
            auto face_id = complex->getCellId(1, f_verts);

            complex->deleteCoface(1, face_id, cell_2d_id);

            if (complex->getCellCofaces(1, face_id)->size() == 0) {
                one_cells_w1cof.erase(face_id);
            } else if (complex->getCellCofaces(1, face_id)->size() == 1) {
                one_cells_w1cof.insert(face_id);
            }
        }
    }
}

void get2DCellPtCubic(
    CellComplex* complex, const int cell_2d_id,
    const vector<int>& cell_1d_verts, Vector3d* pt) {

    vector<int> cell_2d_verts;
    complex->getCellVerts(2, cell_2d_id, &cell_2d_verts);

    int verts[2];
    int j = 0;
    for (auto v : cell_2d_verts) {
        if (v != cell_1d_verts[0] && v != cell_1d_verts[1]) {
            verts[j] = v;
            j ++;
        }
    }

#ifdef DEBUG_OPT
    if (j != 2) {
        cout << "FATAL: j!=2 in get2DCellPtCubic" << endl;
        exit(-1);
    }
#endif

    Vector3d pt1, pt2;
    complex->getVertPos(verts[0], &pt1);
    complex->getVertPos(verts[1], &pt2);
    *pt = (pt1+pt2) / 2;
}

// get the canonical oriented 2d cell vertex sequence where the orientation is
// from the from_vert to the to_vert. the canonical vertex sequence then starts
// from the smallest vertex following the orientation.
// the input 'verts' is assumed to be in canon order.
void getCanonOrien2DCellVertsCubic(const vector<int>& verts,
    const int& from_vert, const int& to_vert, vector<int>* canon_verts) {

    canon_verts->clear();

    int inc;
    for (auto i = 0; i < verts.size(); i ++) {
        if (verts[i] == from_vert) {
            if (verts[ mod(i+1, verts.size()) ] == to_vert) {
                inc = 1;
            } else {
                inc = -1;
            }

            break;
        }
    }

    // int min_ind = 0;
    // int min_vert = verts[0];

    // for (auto i = 1; i < verts.size(); i ++) {
    //     if (verts[i] < min_vert) {
    //         min_ind = i;
    //         min_vert = verts[i];
    //     }
    // }

    for (auto i = 0; i < verts.size(); i ++) {
        // canon_verts->push_back( verts[ mod(min_ind+i*inc, verts.size()) ] );
        canon_verts->push_back( verts[ mod(i*inc, verts.size()) ] );
    }
}

void addOrien2DCellPairs(
    const ComplexType cmplx_type,
    CellComplex* complex,
    const int cell_1d_id,
    const vector<int>& cell_1d_verts,
    VertArrIdMap* orien_cell_2d_id_map,
    Graph<int>* orien_cell_2d_graph) {

    // if (cell_1d->cofaces.size() == 2) {
    //     int dosomething = 0;
    // }

    const vector<int>* cell_1d_cofaces = complex->getCellCofaces(1, cell_1d_id);

    Vector3d cell_1d_pt0, cell_1d_pt1;
    complex->getVertPos(cell_1d_verts[0], &cell_1d_pt0);
    complex->getVertPos(cell_1d_verts[1], &cell_1d_pt1);

    // cout << endl << cell_1d_verts << endl;
    // cout << cell_1d_pt0 << endl;
    // cout << cell_1d_pt1 << endl;

    Vector3d cell_1d_mid_pt = (cell_1d_pt0+cell_1d_pt1) / 2;
    // cout << cell_1d_mid_pt << endl;

    /* get the plane equation of the first coface */

    auto first_cell_2d_id = cell_1d_cofaces->at(0);
    vector<int> first_cell_2d_verts;
    complex->getCellVerts(2, first_cell_2d_id, &first_cell_2d_verts);

    Vector3d first_cell_2d_pt0, first_cell_2d_pt1, first_cell_2d_pt2;
    complex->getVertPos(first_cell_2d_verts[0], &first_cell_2d_pt0);
    complex->getVertPos(first_cell_2d_verts[1], &first_cell_2d_pt1);
    complex->getVertPos(first_cell_2d_verts[2], &first_cell_2d_pt2);

    Vector4d first_cell_plane_eq;
    getPlaneEquation(first_cell_2d_pt0, first_cell_2d_pt1, first_cell_2d_pt2, &first_cell_plane_eq);
    // first_cell_plane_eq = -first_cell_plane_eq;
    // cout << first_cell_plane_eq << endl;
    // cout << evalPlaneEquation(first_cell_plane_eq, first_cell_2d_pt0) << endl;
    // cout << evalPlaneEquation(first_cell_plane_eq, first_cell_2d_pt1) << endl;
    // cout << evalPlaneEquation(first_cell_plane_eq, first_cell_2d_pt2) << endl;

    /* get the angles of other cofaces w.r.t the first coface */

    // TODO: this vector seems useless
    vector<Vector3d> cell_2d_pts(cell_1d_cofaces->size());
    vector<std::pair<Decimal,int>> cell_2d_order(cell_1d_cofaces->size());

    Vector3d first_cell_2d_vec;
    for (auto i = 0; i < cell_1d_cofaces->size(); i ++) {
        if (cmplx_type == CUBE_CMPLX) {
            get2DCellPtCubic(complex, cell_1d_cofaces->at(i), cell_1d_verts, &cell_2d_pts[i]);
        } else if (cmplx_type == SIMP_CMPLX) {
            int dosomething = 0;
        }

        if (i == 0) {
            cell_2d_order[i].first = 0;
            first_cell_2d_vec = cell_2d_pts[i] - cell_1d_mid_pt;
            first_cell_2d_vec.normalize();
        } else {
            Vector3d cell_2d_vec = cell_2d_pts[i] - cell_1d_mid_pt;
            cell_2d_vec.normalize();

            cell_2d_order[i].first = acos(first_cell_2d_vec.dot(cell_2d_vec));
            if (evalPlaneEquation(first_cell_plane_eq, cell_2d_pts[i]) < 0) {
                cell_2d_order[i].first = 2*M_PI - cell_2d_order[i].first;
            }
        }

        cell_2d_order[i].second = i;
    }

    struct {
        bool operator()(const std::pair<Decimal,int> &a,
            const std::pair<Decimal,int> &b) const {
            return a.first < b.first;
        }
    } comp;

    std::sort(cell_2d_order.begin(), cell_2d_order.end(), comp);

#ifdef DEBUG_OPT
    if (cell_2d_order[0].second != 0) {
        cout << "FATAL: cell_2d_order[0].second != 0 in addOrien2DCellPairs" << endl;
        exit(-1);
    }
#endif

    // for (auto pair : cell_2d_order) {
    //     cout << radian2Degree(pair.first) << ' ';
    // }
    // cout << endl;

    Vector3d cell_1d_vec = cell_1d_pt1 - cell_1d_pt0;
    cell_1d_vec.normalize();
    Vector3d cell_1d_ortho_pt = cell_1d_mid_pt + cell_1d_vec.cross(first_cell_2d_vec);

    bool cell_1d_0_to_1 = true;
    if (evalPlaneEquation(first_cell_plane_eq, cell_1d_ortho_pt) < 0) {
        cell_1d_0_to_1 = false;
    }

    // cout << "cell_1d_0_to_1: " << cell_1d_0_to_1 << endl;

    vector<int> cell_2d_verts1, cell_2d_verts2, canon_verts1, canon_verts2;
    canon_verts1.reserve(complex->getVertCnt(2));
    canon_verts2.reserve(complex->getVertCnt(2));

    for (auto i = 0; i < cell_2d_order.size(); i ++) {
        if (cmplx_type == CUBE_CMPLX) {
            auto cell_2d_order1 = cell_2d_order[i].second;
            auto cell_2d_order2 = cell_2d_order[ mod(i+1, cell_2d_order.size()) ].second;

            auto cell_2d_id1 = cell_1d_cofaces->at(cell_2d_order1);
            auto cell_2d_id2 = cell_1d_cofaces->at(cell_2d_order2);

            auto cell_2d_cofaces1 = complex->getCellCofaces(2, cell_2d_id1);
            auto cell_2d_cofaces2 = complex->getCellCofaces(2, cell_2d_id2);

            // check whether the two oriented 2-cells enclose a 3-cell
            if (vecIntersect(*cell_2d_cofaces1, *cell_2d_cofaces2)) {
                if (cell_1d_cofaces->size() == 2) {
                    auto degree1 = cell_2d_order[i].first;
                    auto degree2 = cell_2d_order[ mod(i+1, cell_2d_order.size()) ].first;

                    // cout << "deg:" << radian2Degree(degree1) << " " << radian2Degree(degree2)
                        // << " " << radian2Degree(radianSub(degree2, degree1)) << endl;

                    if (radianSub(degree2, degree1) <= M_PI) {
                        continue;
                    }
                } else {
                    continue;
                }
            }

            complex->getCellVerts(2, cell_2d_id1, &cell_2d_verts1);
            complex->getCellVerts(2, cell_2d_id2, &cell_2d_verts2);

            if (cell_1d_0_to_1) {
                // CAUTION: should guarantee the input vertices in canon order
                getCanonOrien2DCellVertsCubic(
                    cell_2d_verts1, cell_1d_verts[0], cell_1d_verts[1], &canon_verts1);
                getCanonOrien2DCellVertsCubic(
                    cell_2d_verts2, cell_1d_verts[1], cell_1d_verts[0], &canon_verts2);
            } else {
                getCanonOrien2DCellVertsCubic(
                    cell_2d_verts1, cell_1d_verts[1], cell_1d_verts[0], &canon_verts1);
                getCanonOrien2DCellVertsCubic(
                    cell_2d_verts2, cell_1d_verts[0], cell_1d_verts[1], &canon_verts2);
            }

            // cout << cell_2d_verts1 << endl;
            // cout << "paired 2-cells: " << canon_verts1 << ' ' << canon_verts2 << endl;

            auto id1 = orien_cell_2d_id_map->getId(canon_verts1);
            if (id1 < 0) {
                id1 = orien_cell_2d_id_map->addArrayLabel(canon_verts1);
            }

            auto id2 = orien_cell_2d_id_map->getId(canon_verts2);
            if (id2 < 0) {
                id2 = orien_cell_2d_id_map->addArrayLabel(canon_verts2);
            }

            orien_cell_2d_graph->addEdge(id1, id2);
        }
    }
}

void reconVoidBound(
    const ComplexType cmplx_type,
    CellComplex* complex,
    vector<array<int,2>>* cell_2d_void_map,
    const ExecOptions& ops,
    const std::string& file_prefix,
    int* void_cnt) {

    VertArrIdMap orien_cell_2d_id_map;
    orien_cell_2d_id_map.init(complex->getVertCnt(2));
    // cout << "complex->getVertCnt(2): " << complex->getVertCnt(2) << endl;

    Graph<int> orien_cell_2d_graph;

    /* get the number of boundary oriented 2-cells
       and reserve the size for the graph and the id map */

    int orien_cell_2d_cnt = 0;
    for (auto cell_2d_id = 0; cell_2d_id < complex->getCellSize(2); cell_2d_id ++) {
        if (complex->isCellValid(2, cell_2d_id)) {
            orien_cell_2d_cnt += 2 - complex->getCellCofaces(2, cell_2d_id)->size();
        }
    }

    orien_cell_2d_id_map.reserve(orien_cell_2d_cnt);
    orien_cell_2d_graph.reserve(orien_cell_2d_cnt);

    /* traverse all boundary 2-cells and their 1-faces
       to add all pairs of orented 2-cells */
    // TODO: traverse all 1-cells instead

    std::unordered_set<int>* visited_cells_1d = new std::unordered_set<int>;
    vector<int> cell_1d_verts, cell_2d_verts;

    for (auto cell_2d_id = 0; cell_2d_id < complex->getCellSize(2); cell_2d_id ++) {
        if (complex->isCellValid(2, cell_2d_id)) {
            if (complex->getCellCofaces(2, cell_2d_id)->size() < 2) {
                complex->getCellVerts(2, cell_2d_id, &cell_2d_verts);

                for (auto i = 0; i < complex->getFaceCnt(2); i ++) {
                    complex->getFaceVerts(2, cell_2d_verts, i, &cell_1d_verts);
                    complex->toCanonVerts(1, &cell_1d_verts);
                    // cout << "  " << cell_1d_verts << endl;
                    auto cell_1d_id = complex->getCellIdNoConvCanon(1, cell_1d_verts);

                    if (visited_cells_1d->find(cell_1d_id) == visited_cells_1d->end()) {
                        visited_cells_1d->insert(cell_1d_id);
                        addOrien2DCellPairs(cmplx_type, complex, cell_1d_id, cell_1d_verts,
                            &orien_cell_2d_id_map, &orien_cell_2d_graph);
                    }
                }
            }
        }
    }

    delete visited_cells_1d;
    visited_cells_1d = NULL;

    if (ops.verbose) {
        cout << endl << "orien_cell_2d_id_map size stat:" << endl;
        orien_cell_2d_id_map.printLoadStat();
        cout << endl << "orien_cell_2d_graph: capacity="
            << orien_cell_2d_graph.adj_nodes_.capacity()
            << ",size=" << orien_cell_2d_graph.adj_nodes_.size() << endl;
    }

    // TODO: maybe can add a degree check for the graph: 4 for cubical complex?
    // orien_cell_2d_graph.print();
    // cout << endl;

    /* get connected components of the graph of oriented 2-cells by DFS */

    int void_id = 0;
    vector<bool> orien_cell_2d_visited;
    orien_cell_2d_visited.resize(orien_cell_2d_graph.size(), false);
    vector<int> node_stack;
    vector<int> orien_cell_2d;

    for (auto i = 0; i < orien_cell_2d_graph.size(); i ++) {
        if (orien_cell_2d_visited[i] == false) {
            node_stack = { i };
            orien_cell_2d_visited[i] = true;

            MeshWriter* mesh_writer;
            if (ops.write_intem) {
                mesh_writer = new MeshWriter(complex, complex->getVertCnt(2));
            }

            // cout << endl << "node_stack.capacity(): " << node_stack.capacity() << endl << endl;
            // cout << "void " << void_id << ": (";
            // cout << "void " << void_id << ":" << endl;

            while (!node_stack.empty()) {
                auto node = node_stack[node_stack.size() - 1];
                node_stack.pop_back();

                // cout << node << ",";
                orien_cell_2d_id_map.getArrayLabel(node, &orien_cell_2d);
                // cout << orien_cell_2d << endl;

                if (ops.write_intem) {
                    mesh_writer->addFace(orien_cell_2d);
                }

                complex->toCanonVerts(2, &orien_cell_2d);
                auto cell_2d_id = complex->getCellIdNoConvCanon(2, orien_cell_2d);

                if (cell_2d_void_map->at(cell_2d_id).at(0) < 0) {
                    cell_2d_void_map->at(cell_2d_id).at(0) = void_id + complex->getCellSize(3);
                } else if (cell_2d_void_map->at(cell_2d_id).at(1) < 0) {
                    cell_2d_void_map->at(cell_2d_id).at(1) = void_id + complex->getCellSize(3);
                } else {
                    cout << "FATAL cell_2d_void_map[cell_2d_id] both >= 0 in reconVoidBound" << endl;
                    exit(-1);
                }

                // auto cofaces = const_cast<vector<int>*>(complex->getCellCofaces(2, cell_2d_id));
                // cofaces->push_back(void_id + complex->getCellSize(3));

                for (auto j = 0; j < orien_cell_2d_graph.adj_nodes_[node].size(); j ++) {
                    auto adj_node = orien_cell_2d_graph.adj_nodes_[node][j];

                    if (orien_cell_2d_visited[adj_node] == false) {
                        node_stack.push_back(adj_node);
                        orien_cell_2d_visited[adj_node] = true;
                    }
                }
            }

            if (ops.write_intem) {
                mesh_writer->write(file_prefix + "_void" + std::to_string(void_id) + ".off");
                delete mesh_writer;
            }

            void_id ++;
            // cout << ")" << endl;
            // cout << endl;
        }
    }

    *void_cnt = complex->getCellSize(3) + void_id;

    if (ops.verbose) {
        cout << endl << "void count: " << void_id << endl;
    }
}

template<typename WeightType>
void computeMinCyc(
    CellComplex* complex,
    const GEdgeCellMap* gedge_cell_map,
    const int void_cnt,
    const int src,
    const int sink,
    const ExecOptions& ops,
    const std::string& file_prefix) {

    // if (ops.verbose) {
    //     cout << "vertex size:" << void_cnt + gedge_cell_map->size() << endl;
    // }

    // FlowGraph<int,WeightType> _graph(void_cnt + gedge_cell_map->size());

    // int edge_id = 0;
    // for (auto iter = gedge_cell_map->begin();
    //     iter != gedge_cell_map->end(); iter ++) {

    //     const std::pair<int,int>& edge = iter->first;

    //     WeightType w = 0;
    //     for (auto cell_2d_id : iter->second) {
    //         w += (WeightType)(complex->getWeight(2, cell_2d_id));
    //     }

    //     _graph.addEdge(edge.first, edge.second, w);
    //     _graph.addEdge(edge.second, void_cnt + edge_id, w);
    //     _graph.addEdge(void_cnt + edge_id, edge.first, w);

    //     edge_id ++;
    // }

    // return;

    /* compute the maximal flow */

    typedef boost::adjacency_list_traits < boost::vecS, boost::vecS, boost::directedS > Traits;

    typedef boost::adjacency_list < boost::vecS, boost::vecS, boost::directedS,
    // boost::property < boost::vertex_name_t, std::string,
    boost::property < boost::vertex_index_t, int, // originally it's 'long'
    boost::property < boost::vertex_color_t, boost::default_color_type,
    boost::property < boost::vertex_distance_t, int, // originally it's 'long'
    boost::property < boost::vertex_predecessor_t, Traits::edge_descriptor > > > >,

    boost::property < boost::edge_capacity_t, WeightType,
    boost::property < boost::edge_residual_capacity_t, WeightType,
    boost::property < boost::edge_reverse_t, Traits::edge_descriptor > > > > FlowGraphBoost;

    if (ops.verbose) {
        cout << endl << "graph elem sizes: "
            << sizeof(Traits::vertex_descriptor) << " "
            << sizeof(Traits::edge_descriptor) << endl;
    }

    FlowGraphBoost* graph = new FlowGraphBoost;
    typename boost::property_map<FlowGraphBoost, boost::edge_capacity_t>::type
        capacity = get(boost::edge_capacity, *graph);
    typename boost::property_map<FlowGraphBoost, boost::edge_residual_capacity_t>::type
        residual_capacity = get(boost::edge_residual_capacity, *graph);
    typename boost::property_map<FlowGraphBoost, boost::edge_reverse_t>::type
        reverse_edge = get(boost::edge_reverse, *graph);

    for (auto iter = gedge_cell_map->begin();
        iter != gedge_cell_map->end(); iter ++) {

        const std::pair<int,int>& edge = iter->first;
        Traits::edge_descriptor e1, e2;

        boost::tie(e1, boost::tuples::ignore)
            = add_edge(edge.first, edge.second, *graph);
        boost::tie(e2, boost::tuples::ignore)
            = add_edge(edge.second, edge.first, *graph);

        WeightType w = 0;
        for (auto cell_2d_id : iter->second) {
            w += (WeightType)(complex->getWeight(2, cell_2d_id));
        }

        // cout << edge << " w=" << w << endl;

        capacity[e1] = w;
        capacity[e2] = w;
        reverse_edge[e1] = e2;
        reverse_edge[e2] = e1;
    }

    cout << endl << "build flow network done" << endl;

    auto max_flow = boykov_kolmogorov_max_flow(*graph ,src, sink);
    if (ops.verbose) {
        cout << endl << "max_flow: " << (double)max_flow << endl;
    }

    /* do a DFS to get the min-cut */

    if (ops.verbose) {
        cout << "graph num_vertices: " << num_vertices(*graph) << endl;
    }

    vector<bool> visited;

    {
        visited.resize(num_vertices(*graph), false);
        visited[src] = true;
        vector<int> vert_stack = { src };

        while (!vert_stack.empty()) {
            auto v = vert_stack.back();
            vert_stack.pop_back();

            typename boost::graph_traits<FlowGraphBoost>::out_edge_iterator e_iter, e_end;
            boost::tie(e_iter, e_end) = out_edges(v, *graph);
            for (; e_iter != e_end; e_iter ++) {
                if (residual_capacity[*e_iter] > 0) {
                    int adj_v = target(*e_iter, *graph);
                    if (!visited[adj_v]) {
                        vert_stack.push_back(adj_v);
                        visited[adj_v] = true;
                    }
                }
            }
        }
    }

    delete graph;

    /* collect edges across the min-cut */

    MeshWriter mesh_writer(complex, complex->getVertCnt(2));
    vector<int> cell_2d_verts;

    for (auto iter = gedge_cell_map->begin();
        iter != gedge_cell_map->end(); iter ++) {

        const std::pair<int,int>& edge = iter->first;
        if (visited[edge.first] != visited[edge.second]) {
            for (auto cell_2d_id : iter->second) {
                complex->getCellVerts(2, cell_2d_id, &cell_2d_verts);
                mesh_writer.addFace(cell_2d_verts);
            }
        }
    }

    mesh_writer.write(file_prefix + "_mincyc.off");
}

void getBitmapComplexCellVerts(const int cell_handle, const int cell_dim,
    const int x_res, const int y_res, const int z_res, vector<int>* verts) {

    const int x_cnt = 2*x_res + 1;
    const int y_cnt = 2*y_res + 1;

    const int z_coord = cell_handle / (x_cnt*y_cnt);
    const int left = cell_handle % (x_cnt*y_cnt);
    const int y_coord = left / x_cnt;
    const int x_coord = left % x_cnt;

    int vari_cnt = 0;

    int x_vari = 1;
    if (x_coord % 2 == 1) {
        x_vari = 2;
        vari_cnt ++;
    }

    int y_vari = 1;
    if (y_coord % 2 == 1) {
        y_vari = 2;
        vari_cnt ++;
    }

    int z_vari = 1;
    if (z_coord % 2 == 1) {
        z_vari = 2;
        vari_cnt ++;
    }

#ifdef DEBUG_OPT
    if (vari_cnt != cell_dim) {
        cout << "FATAL: getBitmapCC2dCellVerts dim not equal" << endl;
        exit(-1);
    }
#endif

    vector<int> temp_verts;
    for (auto x = x_coord/2; x < x_coord/2 + x_vari; x ++) {
        for (auto y = y_coord/2; y < y_coord/2 + y_vari; y ++) {
            for (auto z = z_coord/2; z < z_coord/2 + z_vari; z ++) {
                temp_verts.push_back(x + y*(x_res+1) + z*(x_res+1)*(y_res+1));
            }
        }
    }

    if (cell_dim == 2) {
        *verts = {temp_verts[0], temp_verts[1], temp_verts[3], temp_verts[2]};
    } else if (cell_dim == 3) {
        *verts = {temp_verts[0], temp_verts[1], temp_verts[3], temp_verts[2],
            temp_verts[4], temp_verts[5], temp_verts[7], temp_verts[6]};
    }
}

void printGEdgeCellMap(const GEdgeCellMap* gedge_cell_map, CellComplex* complex) {
    cout << "3-cells [" << complex->getCellCount(3) << "]:" << endl;
    for (int cell_3d_id = 0; cell_3d_id < complex->getCellSize(3); cell_3d_id ++) {
        if (complex->isCellValid(3, cell_3d_id)) {
            vector<int> verts;
            complex->getCellVerts(3, cell_3d_id, &verts);
            cout << cell_3d_id << ": " << verts << endl;
        }
    }
    cout << endl;

    for (auto iter = gedge_cell_map->begin();
        iter != gedge_cell_map->end(); iter ++) {

        cout << iter->first << ":" << endl;
        for (auto cell_2d_id : iter->second) {
            vector<int> verts;
            complex->getCellVerts(2, cell_2d_id, &verts);
            cout << "  " << verts << endl;
        }
    }
}

void getCell2dVoidIds(
    const int cell_2d_id, CellComplex* complex,
    vector<array<int,2>>* cell_2d_void_map, vector<int>* void_ids) {

    void_ids->clear();

    auto cofaces = complex->getCellCofaces(2, cell_2d_id);
    for (auto cell_3d_id : *cofaces) {
        void_ids->push_back(cell_3d_id);
    }

    for (auto void_id : cell_2d_void_map->at(cell_2d_id)) {
        if (void_id >= 0) {
            void_ids->push_back(void_id);
        }
    }

    if (void_ids->size() != 2) {
        cout << "FATAL: void_ids.size() != 2 in getCell2dVoidIds" << endl;
        exit(-1);
    }
}


#ifndef _LDAPLUSPLUS_OPTIMIZATION_SECOND_ORDER_LOGISTIC_REGRESSION_APPROXIMATION_HPP_
#define _LDAPLUSPLUS_OPTIMIZATION_SECOND_ORDER_LOGISTIC_REGRESSION_APPROXIMATION_HPP_

#include <cmath>
#include <vector>

#include <Eigen/Core>

namespace ldaplusplus {
namespace optimization {

/**
 * SecondOrderLogisticRegressionApproximation is a second order taylor
 * approximation to the expectation of the logistic loss function of a random
 * variable.
 *
 * We use this class to approximate the following equation in the lower bound
 * of the likelihood of an LDA model. \f$q\f$ is the variational distribution
 * used and \f$\bar{z}\f$ is a random variable (the mean of the topic
 * assignments). The equation below is for a single document.
 *
 * \f[
 *     \mathbb{E}_q\left[
 *         \eta_{y_n}^T \bar{z} -
 *         \log\left( \sum_{\hat{y}=1}^Y \exp(\eta_{\hat{y}}^T \bar{z}) \right)
 *         \right] \approx
 *         \eta_{y_n}^T \mathbb{E}_q[\bar{z}] -
 *         \log \sum_{\hat{y}=1}^Y \exp(\eta_{\hat{y}}^T \mathbb{E}_q[\bar{z})\left(
 *         1 + \frac{1}{2} \eta_{\hat{y}}^T \mathbb{V}_q[\bar{z}] \eta_{\hat{y}}
 *         \right)
 * \f]
 */
template <typename Scalar>
class SecondOrderLogisticRegressionApproximation
{
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;

    public:
        /**
         * @param X     The documents defining the minimization problem (\f$X
         *              \in \mathbb{R}^{D \times N}\f$)
         * @param X_var A vector containing the variance matrix for each
         *              document (\f$X_{\text{var}} \in \mathbb{R}^{N \times D
         *              \times D}\f$)
         * @param y     The class indexes for each document (\f$y \in
         *              \mathbb{N}^N\f$)
         * @param Cy    A different weight for each class in the optimization
         *              problem
         * @param L     The L2 regularization penalty for the weights
         */
        SecondOrderLogisticRegressionApproximation(
            const MatrixX &X,
            const std::vector<MatrixX> &X_var,
            const Eigen::VectorXi &y,
            VectorX Cy,
            Scalar L
        );

        /**
         * @param X     The documents defining the minimization problem (\f$X
         *              \in \mathbb{R}^{D \times N}\f$)
         * @param X_var A vector containing the variance matrix for each
         *              document (\f$X_{\text{var}} \in \mathbb{R}^{N \times D
         *              \times D}\f$)
         * @param y     The class indexes for each document (\f$y \in
         *              \mathbb{N}^N\f$)
         * @param L     The L2 regularization penalty for the weights
         */
        SecondOrderLogisticRegressionApproximation(
            const MatrixX &X,
            const std::vector<MatrixX> &X_var,
            const Eigen::VectorXi &y,
            Scalar L
        );

        /**
         * The value of the objective function to be minimized.
         *
         * \f$N\f$ is the number of documents (different vectors), \f$X_n \in
         * \mathbb{R}^D\f$ is the nth document, \f$\eta_y \in \mathbb{R}^D\f$
         * is the weights vector for the class \f$y\f$ defining the hyperplane
         * that separates class \f$y\f$ from all the other, \f$y_n\f$
         * is the class of the nth document and finally \f$X_n^{\text{var}} \in
         * \mathbb{R}^{D \times D}\f$ is the variance of the nth document (see
         * the class description).
         *
         * \f[
         *     J = - \sum_{n=1}^N C_{y_n} \left(
         *         \eta_{y_n}^T X_n -
         *         \log \sum_{\hat{y}=1}^Y \exp(\eta_{\hat{y}}^T X_n) \left(
         *         1 +
         *         \frac{1}{2} \eta_{\hat{y}}^T X_n^{\text{var}} \eta_{\hat{y}}
         *         \right)
         *         \right) +
         *         \frac{L}{2} \left\| \eta \right\|_F^2
         * \f]
         *
         * @param eta The weights of the linear model (\f$\eta \in
         *            \mathbb{R}^{D \times Y}\f$)
         */
        Scalar value(const MatrixX &eta) const;

        /**
         * The gradient of the objective function implemented in value().
         *
         * We use \f$I(y) \in \mathbb{R}^Y\f$ as the indicator vector of
         * \f$y\f$ (a vector with all the values 0 except at the yth position).
         *
         * \f[
         *     \nabla_{\eta} J = - \sum_{n=1}^N C_{y_n} \left(
         *         X_n I(y_n)^T -
         *         \frac{
         *              \sum_{\hat{y}=1}^Y \left(\left(
         *              X_n \exp(\eta_{\hat{y}}^T X_n) \left(
         *              1 +
         *              \frac{1}{2} \eta_{\hat{y}}^T X_n^{\text{var}} \eta_{\hat{y}}
         *              \right)\right) + \left(
         *              \frac{1}{2}
         *              \exp(\eta_{\hat{y}}^T X_n) \eta_{\hat{y}}^T \left(
         *              X_n^{\text{var}} + \left(X_n^{\text{var}}\right)^T
         *              \right)\right)
         *              \right) I(\hat{y})^T
         *              }
         *              {
         *              \sum_{\hat{y}=1}^Y \exp(\eta_{\hat{y}}^T X_n) \left(
         *              1 +
         *              \frac{1}{2} \eta_{\hat{y}}^T X_n^{\text{var}} \eta_{\hat{y}}
         *              \right)
         *              }
         *         \right) +
         *         L \eta
         * \f]
         *
         * @param eta  The weights of the linear model (\f$\eta \in
         *             \mathbb{R}^{D \times Y}\f$)
         * @param grad A matrix of dimensions equal to \f$\eta\f$ that will
         *             hold the result
         */
        void gradient(const MatrixX &eta, Eigen::Ref<MatrixX> grad) const;

    private:
        const MatrixX &X_;
        const std::vector<MatrixX> &X_var_;
        const Eigen::VectorXi &y_;
        Scalar L_;
        VectorX Cy_;
};

}  // namespace optimization
}  // namespace ldaplusplus

#endif // _LDAPLUSPLUS_OPTIMIZATION_SECOND_ORDER_LOGISTIC_REGRESSION_APPROXIMATION_HPP_


#include <iostream>
#include <Eigen/Dense>
#include <cassert>
#include <vector>
#include "Option.h"
#include <cmath>
#include "Regression.h"
#include "FileManagement.h"
#include <chrono>
#include "nonuniform_grid.h"
#include <random>
#include "Simulation.h"
#include <tuple>
#include "HestonOption.h"
#include "OptimalExecution.h"

int main() {

  // Generation of option prices
  // auto o = Option("Call");
  // o.set_tn(1);
  // o.set_s_max(100);
  // o.set_numdiff_t(10);
  // o.set_numdiff_s(10);
  // o.set_volatility(.1);
  // o.set_interest_rate(.15);
  // o.set_strike(50);
  // o.set_stock_boundary_condition("Dirichlet");
  // o.fixed_difference_step();
  //
  // o.compute_solution_grid("Crank-Nicholson");
  // o.print_solution_grid();

  // Heston option Monte-Carlo Simulation
  // auto h = HestonOption("Call", 1, 10, 100, 50);
  // h.set_params_stock_process(45, .1);
  // h.set_prams_variance_process(.2, 1, 1, .25, .5);
  // auto pr = h.compute_price();
  //
  // std::cout << pr << "\n";

  // Generation of optimal execution paths
   // double a_coeff_, double b_coeff_, double sigma_coeff_, double k_coeff_, double phi_coeff_
   // auto o = OptimalExecution(.6,.1, 0.1,.1,.6);
   // // num_diff_t_, int num_diff_q_
   // o.set_num_steps(100,100);
   // // true_impact, trading_speed, impact_nonlinearity
   // o.compute_liquidation("Linear", 1);
   // Eigen::VectorXd h = o.get_value_process();
   // std::cout << "Value Process\n" << h << "\n";
   // Eigen::VectorXd j = o.get_optimal_speed_process();
   // std::cout << "Speed Process\n" << j << "\n";
   // Eigen::VectorXd s = o.get_stock_process();
   // std::cout << "Stock Process\n" << s << "\n";
   // Eigen::VectorXd c = o.get_cash_process();
   // std::cout << "Cash Process\n" << c << "\n";
   // Eigen::VectorXd i = o.get_inventory_process();
   // std::cout << "Inventory Process\n" << i << "\n";
   //
   // std::cout << o.get_value_matrix();

    // Generate 500 optimal execution scenarios and save it in the csv files
    // Eigen::MatrixXd nonlinear_speed_csv_value,nonlinear_speed_csv_optimal_speed, nonlinear_speed_csv_stock, nonlinear_speed_csv_cash, nonlinear_speed_csv_inventory;
    // nonlinear_speed_csv_value.resize(101,100); nonlinear_speed_csv_optimal_speed.resize(101,100); nonlinear_speed_csv_stock.resize(101,100); nonlinear_speed_csv_cash.resize(101,100); nonlinear_speed_csv_inventory.resize(101,100);
    // Eigen::MatrixXd linear_speed_csv_value,linear_speed_csv_optimal_speed, linear_speed_csv_stock, linear_speed_csv_cash, linear_speed_csv_inventory;
    // linear_speed_csv_value.resize(101,100); linear_speed_csv_optimal_speed.resize(101,100); linear_speed_csv_stock.resize(101,100); linear_speed_csv_cash.resize(101,100); linear_speed_csv_inventory.resize(101,100);
    // for (int sim = 0; sim < 100; ++sim) {
        // Trading speed: Nonlinear, true impact: Nonlinear
        // auto o = OptimalExecution(.08,.06, .1,.08,.06);
        // o.set_num_steps(100,100);
        // o.compute_liquidation("Nonlinear", "Nonlinear", .6);
        // Eigen::VectorXd value_process = o.get_value_process();
        // nonlinear_speed_csv_value.col(sim) = value_process;
        // Eigen::VectorXd optimal_speed_process = o.get_optimal_speed_process();
        // nonlinear_speed_csv_optimal_speed.col(sim) = optimal_speed_process;
        // Eigen::VectorXd stock_process = o.get_stock_process();
        // nonlinear_speed_csv_stock.col(sim) = stock_process;
        // Eigen::VectorXd cash_process = o.get_cash_process();
        // nonlinear_speed_csv_cash.col(sim) = cash_process;
        // Eigen::VectorXd inventory_process = o.get_inventory_process();
        // nonlinear_speed_csv_inventory.col(sim) = inventory_process;

        // Trading speed: Linear, true impact: Nonlinear
        // auto ol = OptimalExecution(.6,.1, 0.1,.1,.6);
        // ol.set_num_steps(100,100);
        // ol.compute_liquidation("Linear", 1);
        // Eigen::VectorXd value_process_lin = ol.get_value_process();
        // linear_speed_csv_value.col(sim) = value_process_lin;
        // Eigen::VectorXd optimal_speed_process_lin = ol.get_optimal_speed_process();
        // linear_speed_csv_optimal_speed.col(sim) = optimal_speed_process_lin;
        // Eigen::VectorXd stock_process_lin = ol.get_stock_process();
        // linear_speed_csv_stock.col(sim) = stock_process_lin;
        // Eigen::VectorXd cash_process_lin = ol.get_cash_process();
        // linear_speed_csv_cash.col(sim) = cash_process_lin;
        // Eigen::VectorXd inventory_process_lin = ol.get_inventory_process();
        // linear_speed_csv_inventory.col(sim) = inventory_process_lin;
    // }
    // saveData("data/nonlinear_speed_csv_value.csv", nonlinear_speed_csv_value);
    // saveData("data/nonlinear_speed_csv_optimal_speed.csv", nonlinear_speed_csv_optimal_speed);
    // saveData("data/nonlinear_speed_csv_stock.csv", nonlinear_speed_csv_stock);
    // saveData("data/nonlinear_speed_csv_cash.csv", nonlinear_speed_csv_cash);
    // saveData("data/nonlinear_speed_csv_inventory.csv", nonlinear_speed_csv_inventory);

    // saveData("data/linear_speed_csv_value.csv", linear_speed_csv_value);
    // saveData("data/linear_speed_csv_optimal_speed.csv", linear_speed_csv_optimal_speed);
    // saveData("data/linear_speed_csv_stock.csv", linear_speed_csv_stock);
    // saveData("data/linear_speed_csv_cash.csv", linear_speed_csv_cash);
    // saveData("data/linear_speed_csv_inventory.csv", linear_speed_csv_inventory);

    return 0;
}


#include <Rcpp.h>
#include <RcppEigen.h>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
using namespace Rcpp;
using namespace Eigen;

// [[Rcpp::depends(RcppEigen)]]
//

typedef Eigen::MappedSparseMatrix<double> MSpMat;
typedef Eigen::SparseMatrix<double> SpMat;
Eigen::SimplicialLLT <Eigen::SparseMatrix<double>, Eigen::Lower, Eigen::NaturalOrdering<int>> cholesky;

// [[Rcpp::export]]
double bym_scale(const SEXP &Q_) {

    //Map SparseMatrix
    const SpMat Q(Rcpp::as<MSpMat>(Q_));

    MatrixXd L = cholesky.compute(Q).matrixL();
    MatrixXd Sigma;
    Sigma.setZero(Q.rows(),Q.cols());
    Sigma.diagonal() = 1 / pow(L.diagonal().array(), 2);
    int n = Sigma.rows();
    for(int i = (n-2); i >= 0; --i){
        for(int j = (n-1); j >= i; --j){
            Sigma(i,j) -= 1.0 / L(i,i) * Sigma.col(j).tail(n-i-1).dot(L.col(i).tail(n-i-1));
            Sigma(j,i) = Sigma(i,j);
        }
    }

    MatrixXd A = Eigen::MatrixXd::Constant(1,n,1);
    MatrixXd W = Sigma * A.transpose();
    Sigma = Sigma - W * (A*W).inverse() * W.transpose();

    return(exp(Sigma.diagonal().array().log().mean()));
}