//=======================================================================
// Copyright 2015 Tsinghua University.
// Authors: Fuan Pu (Pu.Fuan@gmail.com)
//
// Dung's abstract argumentation framework
//=======================================================================

#ifndef DUNG_PREFERRED_REASONER_HPP
#define DUNG_PREFERRED_REASONER_HPP

#include <iostream>
#include <string>
#include <utility>
#include <algorithm>

//boost
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>

#include "config/config.hpp"
#include "ArgumentProperty.hpp"
#include "AttackProperty.hpp"
#include "bitmatrix/bitvector.hpp"
#include "bitmatrix/bitmatrix.hpp"
#include "DungAF.hpp"
#include "Reasoner.hpp"

namespace argumatrix{

using namespace std;

/**
* This reasoner for Dung theories performs inference on the preferred extension.
* Computes the (unique) grounded extension, i.e., the least fixpoint of
* the characteristic function.
*/
class PreferredReasoner : public Reasoner {
public:
	PreferredReasoner(const DungAF& daf, streambuf* osbuff = std::cout.rdbuf())
		: Reasoner(daf, osbuff) {
		// m_argNum = m_daf.getNumberOfArguments();
		// m_BmAtkMtx = m_daf.getAttackMatrix();
		m_attackedBy = m_BmAtkMtx.transpose();
	}

	/**
	 * Compute all extensions given a specific semantics. Each extension is a set
	 * of arguments. Here, we use a bitvector to represent an extension. The results
	 * are stored in m_extensions. When a new extension bv is computed, it can be added
	 * to m_extensions by m_extensions.push_back(bv).
	 * @return no return. The results are stored in m_extensions.
	 */
	void computeExtensions();

	/**
	 * Get attackers of arguments in _bv, R^-(S)
	 * R^-(S) = {x|x attacks some argument in S}.
	 * R^-(S_bv) = D^T*S_bv
	 * @param _bv the bitvector of a set of arguments S.
	 * @return a set of arguments in bitvector form.
	 */
	bitvector getAttackers(const bitvector& _bv);

private:
	enum SELECT_TYPE { TYPE_A, TYPE_B };
	enum LABELS { BLANK = 0, _IN_, _OUT_, MUST_OUT, UNDEC, Label_Num };

	/**
	 * Before computing the preferred extension, we will preprocess all arguments
	 * by assigning initial labels to some arguments for reducing search space.
	 * The preprocessing operations contain (1) setting all self-attacking arguments
	 * with OUT label; (2) setting all unattacked arguments with IN label;
	 * (3) setting all arguments attacked by IN-label arguments with OUT label;
	 * Lastly, all arguments labeled IN or OUT will be removed from BLANK.
	 */
	void preprocessing(bitvector& _blank, bitvector& _in, bitvector& _out, bitvector& _undec);

	void findPreferredExtensions(bitvector& _blank, bitvector& _in,
			bitvector& _out, bitvector& _undec, bitvector& _must_out);

	bool lookAhead(bitvector& _blank_new, bitvector& _tmp_must_out);

	pair<size_type, SELECT_TYPE> selectArgument(const bitvector& _blank, const bitvector& _in,
		const bitvector& _out, const bitvector& _undec, const bitvector& _must_out);

private:
	/**
	 * The transpose of the attack matrix, it provides an effective way to
	 * access all attacked arguments of a given argument. The attacked arguments
	 * of the argument with index i is m_attackedBy[i].
	 */
	bitmatrix m_attackedBy;

};  // class GroundedReasoner

void PreferredReasoner::computeExtensions()
{
	m_extensions.clear();

	// We using five bitvector to encode the labellings of all arguments.
	// Each argument is merely labeled with one of the following five labellings.
	bitvector _blank = bitvector::UniversalSet(m_argNum);  // At beginning, all arguments are labeled BLANK
	bitvector _out = bitvector::EmptySet(m_argNum);   // Empty set
	bitvector _in = bitvector::EmptySet(m_argNum);
	bitvector _must_out = bitvector::EmptySet(m_argNum);
	bitvector _undec = bitvector::EmptySet(m_argNum);

	// Preprocessing
	preprocessing(_blank, _in, _out, _undec);

	findPreferredExtensions(_blank, _in, _out, _undec, _must_out);
}

void PreferredReasoner::preprocessing(bitvector& _blank, bitvector& _in, bitvector& _out, bitvector& _undec)
{
	// setting all self-attacking arguments with OUT label
	// _out |= m_attackMatrix.diag();
	_undec |= getSelfAttackingArguments();

	// getting all unattacked arguments by characteristic function
	_in |= characteristic(bitvector::EmptySet(m_argNum));

	// setting all arguments, attacked by IN-label arguments, with OUT label;
	_out |= getAttacked(_in);

	// Removing IN and OUT from BLANK
	// _blank ^= (_in|_out);
	_blank -= _in;
	_blank -= _out;
	_blank -= _undec;
}

void PreferredReasoner::findPreferredExtensions(bitvector& _blank,
		bitvector& _in, bitvector& _out, bitvector& _undec, bitvector& _must_out)
{
	size_type i;
	SELECT_TYPE _s_type;

	boost::tie(i, _s_type) = selectArgument(_blank, _in, _out, _undec, _must_out);

	while (i != bitvector::npos)
	{
		bitvector _blank_new(_blank);
		bitvector _out_new(_out);
		bitvector _in_new(_in);
		bitvector _must_out_new(_must_out);
		bitvector _undec_new(_undec);

		_in_new[i] = true; _blank_new[i] = false;  // label i with IN

		_out_new |= m_attackedBy[i];
		_blank_new -= m_attackedBy[i];
		_undec_new -= m_attackedBy[i];
		_must_out_new -= m_attackedBy[i];

		// get argument i's attackers which are labeled by BLANK or UNDEC
		bitvector _tmp_must_out( m_BmAtkMtx[i]&(_blank_new|_undec_new) );
		_must_out_new |= _tmp_must_out;
		_blank_new -= _tmp_must_out;
		_undec_new -= _tmp_must_out;

		//bool forwardCheck = lookAhead(_blank_new, _tmp_must_out);
		bool forwardCheck = lookAhead(_blank_new, _must_out_new);
		if ( forwardCheck )
		{
			findPreferredExtensions(_blank_new, _in_new, _out_new, _undec_new, _must_out_new);
		}

		if (_s_type == TYPE_B)
		{
			_undec[i] = true;  _blank[i] = false;
			if ( !lookAhead(_blank, _must_out) )
				return;
		}
		else   // TYPE_A
		{
			if ( !forwardCheck )
				return;

			_blank = _blank_new;
			_in = _in_new;
			_out = _out_new;
			_must_out = _must_out_new;
			_undec = _undec_new;
		}

		boost::tie(i, _s_type) = selectArgument(_blank, _in, _out, _undec, _must_out);
	}

	// if there are no argument labeled MUST_OUT, then inserts the argument
	// set labeled IN into extensions
	if ( _must_out.is_emptyset() )
	{
		std::set<bitvector>::iterator sa_itr;

		for (sa_itr = m_extensions.begin(); sa_itr != m_extensions.end(); sa_itr++)
		{
			if (_in.is_subset_of( *sa_itr ))
			{
				//cout << "duplicate: " << _in << "is subset of" << *sa_itr << endl;
				return;
			}
		}

		m_extensions.insert( _in );
	}
}

bool PreferredReasoner::lookAhead(bitvector& _blank_new, bitvector& _tmp_must_out)
{
	for (size_type i = _tmp_must_out.find_first(); i != bitvector::npos; i = _tmp_must_out.find_next(i))
	{
		if ( !m_BmAtkMtx[i].intersects(_blank_new) )
			return false;
	}

	return true;
}

pair<size_type, PreferredReasoner::SELECT_TYPE> PreferredReasoner::selectArgument(
	const bitvector& _blank, const bitvector& _in, const bitvector& _out,
	const bitvector& _undec, const bitvector& _must_out)
{
	size_type i = bitvector::npos;
	for (size_type j = _blank.find_first(); j != bitvector::npos; j = _blank.find_next(j))
	{
		//bool _m_in = true;
		if( m_BmAtkMtx[j].is_subset_of(_out|_must_out) )
		//if ( !m_attackMatrix[j].intersects(_blank|_undec) )
		{
			return make_pair(j, TYPE_A);
		}

		if ( i == bitvector::npos) 	{
			i = j;
		} else if (m_attackedBy[j].count()+m_BmAtkMtx[j].count() >
			m_attackedBy[i].count()+m_BmAtkMtx[i].count()) {
			i = j;
		}
	}

	return make_pair(i, TYPE_B);
}

bitvector PreferredReasoner::getAttackers(const bitvector& _bv)
{
	assert( _bv.size() == m_argNum );

	return m_attackedBy * _bv;
}

} // namespace argumatrix

#endif  //DUNG_PREFERRED_REASONER_HPP


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

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

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing oneplus capabilities

    Returns the entry plus one, saturated in the entry type.
    If @c x is not @ref Nan, @c oneplus(x) is greater or equal to x.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = oneplus(x);
    @endcode

    The code is similar to:

    @code
    T r = (x == Valmax) ? x : inc(x)
    @endcode

    @see inc, plus, adds

    @par Alias

    incs

  **/
  const boost::dispatch::functor<tag::oneplus_> oneplus = {};
} }
#endif

#include <boost/simd/function/scalar/oneplus.hpp>
#include <boost/simd/function/simd/oneplus.hpp>

#endif


#include <opencv2/video/tracking.hpp>
#include <iostream>

using namespace std;

// g2o
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>

// Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <boost/format.hpp>

#include "core/Tracker.h"
#include "core/Feature.h"
#include <core/Config.h>
#include <utils/SLAMTools.h>
#include "g2o_types/types_direct.h"

namespace zed_slam
{

typedef Eigen::Matrix<double, 6, 1, Eigen::ColMajor> Vector6d;

Tracker::Tracker()
{
    klt_max_iter_ = Config::get<int> ( "klt_max_iter" );
    klt_eps_ = Config::get<int> ( "klt_eps" );
    klt_window_size_ = Config::get<int> ( "klt_window_size" );

    epsilon_direct_ = Config::get<double> ( "epsilon_direct" );
    confidence_threshold_ = Config::get<double> ( "confidence_threshold" );
}

void Tracker::init()
{

}

Eigen::Isometry3d Tracker::trackImage ( Frame::Ptr frame )
{
    return Eigen::Isometry3d::Identity();
}

int Tracker::trackKLT ( Frame::Ptr first, Frame::Ptr second )
{
    vector<uchar> status;
    vector<float> error;
    vector<float> min_eig_vec;
    cv::TermCriteria termcrit ( cv::TermCriteria::COUNT+cv::TermCriteria::EPS , klt_max_iter_, klt_eps_ );
    vector<cv::Point2f> pixel_keypoint_second = first->getKeypointPixelPosition();
    // use opencv to calculate the optical flow and get the keypoints' position in the second frame
    // API C++: void calcOpticalFlowPyrLK(InputArray prevImg, InputArray nextImg, InputArray prevPts, InputOutputArray nextPts, OutputArray status, OutputArray err, Size winSize=Size(21,21), int maxLevel=3, TermCriteria criteria=TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01), int flags=0, double minEigThreshold=1e-4 )

    cv::calcOpticalFlowPyrLK (
        first->color_, second->color_,
        first->getKeypointPixelPosition(), pixel_keypoint_second,
        status, error, cv::Size ( klt_window_size_, klt_window_size_ ),
        4, termcrit, cv::OPTFLOW_USE_INITIAL_FLOW );

    // check the good
    auto iter_first=first->features_.begin();
    int good = 0;
    for ( size_t i=0; iter_first!=first->features_.end(); ++i )
    {
        if ( status[i]==false )
        {
            // throw the failed feature
            // TODO throw away? or keep them?
            iter_first = first->features_.erase ( iter_first );
            continue;
        }
        // klt is good, add this feature into second frame
        cv::KeyPoint kp = ( *iter_first ).second->keypoint_;
        kp.pt = pixel_keypoint_second[i];
        shared_ptr<Feature> f (new Feature ( second.get(), kp, ( *iter_first ).second->depth_, ( *iter_first ).second->confidence_ ));
        f->position_ = ( *iter_first ).second->position_;
        f->quality_ = ( *iter_first ).second->quality_+1;
        f->quality_ = f->quality_ > 100? 100: f->quality_;
        second->features_.insert ( {f->id_,f} );
        iter_first++;
        good++;
    }

    cout<<"there are "<<good<<" good feature captured by klt"<<endl;
    return 0;
}

int Tracker::poseEstimationTwoFrames ( Frame::Ptr first, Frame::Ptr second )
{
    // we minimize the stereo reprojection error
    // init g2o
    cout<<"setting up g2o"<<endl;
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType> ();
    g2o::BlockSolver_6_3*   solver_ptr = new g2o::BlockSolver_6_3 ( linearSolver );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    optimizer.setAlgorithm ( solver );
    optimizer.setVerbose ( false );

    // pose of the second frame
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setEstimate ( SLAMTools::toSE3Quat ( first->T_f_w_ ) );
    pose->setId ( 0 );
    optimizer.addVertex ( pose );

    // points and edges
    vector<g2o::EdgeSE3ProjectXYZOnlyPose*> edges;
    for ( auto iter=second->features_.begin(); iter!=second->features_.end(); iter++ )
    {
        if ( ( *iter ).second->quality_ == 0 )
        {
            // from this to end are all new features which do not occur in frame 1
            break;
        }
        // 3d point
        g2o::EdgeSE3ProjectXYZOnlyPose* edge = new g2o::EdgeSE3ProjectXYZOnlyPose();
        edge->setVertex ( 0, pose );
        edge->Xw = ( *iter ).second->position_;
        edge->fx = second->camera_->fx();
        edge->fy = second->camera_->fy();
        edge->cx = second->camera_->cx();
        edge->cy = second->camera_->cy();
        edge->setMeasurement ( Eigen::Vector2d (
                                   ( *iter ).second->keypoint_.pt.x, ( *iter ).second->keypoint_.pt.y ) );
        // cout<<edge->Xw<<endl;
        // TODO how to set the information matrix? how to use confidence?
        //edge->setInformation ( ( *iter )->confidence_ * Eigen::Matrix2d::Identity() );
        //cout<<(*iter)->confidence_<<endl;
        edge->setInformation ( ( *iter ).second->confidence_ * Eigen::Matrix2d::Identity() );
        edge->setRobustKernel ( new g2o::RobustKernelHuber() );
        optimizer.addEdge ( edge );
        edges.push_back ( edge );
    }

    // in optical flow we usually have missing features but hardly have outlier features, but the depth is not good as rgbd sensors, bad features can also occur.
    cout<<"optimizing..."<<endl;
    optimizer.initializeOptimization();
    optimizer.optimize ( 10 );

    int nbad = 0;
    std::for_each ( edges.begin(), edges.end(), [&] ( g2o::EdgeSE3ProjectXYZOnlyPose* edge )
    {
        edge->computeError();
        //cout<<edge->chi2()<<endl;
        if ( edge->chi2() > 5.991 )
        {
            edge->setLevel ( 1 );
            nbad++;
        }
        else
        {
            edge->setRobustKernel ( nullptr );
        }
    } );

    //cout<<"bad features="<<nbad<<endl;
    optimizer.initializeOptimization();
    optimizer.optimize ( 10 );

    // get the pose of camera
    g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*> ( optimizer.vertex ( 0 ) );
    Eigen::Isometry3d T = g2o::SE3Quat ( v->estimate() );
    second->T_f_w_ = T;

    // project the features into the camera frame
    // remove the outlier
    //cv::Mat features_img;
    //cv::cvtColor( second->color_, features_img, CV_GRAY2RGB );
    int i=0;
    /*
    for ( auto feature:second->features_ )
    {
        cout<<feature->quality_<<endl;
    }*/
    //cout<<first->features_.size()<<endl;
    //cout<<second->features_.size()<<endl;
    for ( auto iter = second->features_.begin(); iter!=second->features_.end(); )
    {
        shared_ptr<Feature> feature = (*iter).second;
        //cv::circle( features_img, feature->keypoint_.pt, 4*(feature->keypoint_.octave+1), cv::Scalar( 0, 0, 255), 1);
        if ( feature->quality_ > 0 )
        {
            edges[i]->computeError();
            // this feature is tracked from the first frame, we adjust the 2d pixel position
            if ( edges[i]->chi2() > 5.991 )
            {
                // tracked but a outlier, reduce its quality
                if ( feature->quality_ < 40 )
                {
                    iter = second->features_.erase ( iter );
                    i++;
                    continue;
                }
                else
                {
                    // this looks like a long tracked feature, keep it
                    Eigen::Vector2d pixel = second->world2pixel ( feature->position_ );
                    feature->keypoint_.pt.x = pixel[0];
                    feature->keypoint_.pt.y = pixel[1];
                    iter++;
                    i++;
                }
            }
            else
            {
                // update the pixel position
                Eigen::Vector2d pixel = second->world2pixel ( feature->position_ );
                feature->keypoint_.pt.x = pixel[0];
                feature->keypoint_.pt.y = pixel[1];
                iter++;
                i++;
            }
        }
        else
        {
            // this is a new feature, we compute the 3d position according to the pose of second frame
            //cout<<feature->keypoint_.pt.x<<','<<feature->keypoint_.pt.y<<endl;
            feature->position_ = second->pixel2world (
                                     Eigen::Vector2d ( feature->keypoint_.pt.x, feature->keypoint_.pt.y )
                                 );
            iter++;
            i++;
        }
        //cv::circle( features_img, feature->keypoint_.pt, 4*(feature->keypoint_.octave+1), cv::Scalar( 0, 255, 0), 1);
    }
    //cv::imshow("feature_optimized", features_img );

    //SLAMTools::drawFeatures( first );
    //cv::waitKey(0);
    return 0;
}

// direct method to estimate the pose
// because we have confidence map, we can choose points that have high confidence (which indicates their texture is good in stereo matching), instead of computing the derivatives of pixels
int Tracker::poseEstimationTwoFramesDirect ( Frame::Ptr first, Frame::Ptr second )
{
    // use the high confiden points to estimate the pose
    // confidence is 8UC4 but an int number
    double residual=1e10, new_residual = 0;

    float step = 1;
    cout<<"calling direct pose estimation"<<endl;
    vector<pair<Eigen::Vector3d, float>> points_first; // the 3d points in world frame with high confidence value, they need only to be computed once

    int high_confidence_points = 0;
    for ( int y=50; y<first->color_.rows-50; y+=2 )
    {
        for ( int x=50; x<first->color_.cols-50; x+=2 )
        {
            int confidence = first->confidence_.data[ y*first->confidence_.step + x*4 ];
            if ( confidence > confidence_threshold_ )
            {
                high_confidence_points ++ ;
                BiLinearWeights weights;
                pair<Eigen::Vector3d, float> p;
                float pixel_first = first->getBiLinearPixelValue ( Eigen::Vector2d ( x,y ),weights );
                p.first = first->pixel2world ( Eigen::Vector2d ( x,y ) );
                p.second = pixel_first;
                points_first.push_back ( p );
            }
        }
    }
    cout<<"high confidence points="<<high_confidence_points<<endl;

    for ( int iterate = 0; iterate<20; iterate ++ )
    {
        // residual and Jacobians
        Eigen::Matrix<double, 6, 6> H;
        H.setZero();
        Eigen::Matrix<double, 6, 1> b;
        b.setZero();

        new_residual = 0;
        int good = 0;
        // we don't use the pixels that are near the boarder
        for ( auto p:points_first )
        {
            // compute the reprojection of this point
            Eigen::Vector2d pixel_position_second = second->world2pixel ( p.first );
            if ( second->inFrame ( pixel_position_second, 3 ) ==false )
            {
                continue;
            }

            // compute the reprojection error
            BiLinearWeights weights;
            float pixel_second = second->getBiLinearPixelValue ( pixel_position_second, weights );
            Eigen::Matrix<double, 2, 6> jacobian_uv_ksai = second->getJacobianUV2SE3 ( p.first );
            Eigen::Matrix<double,1,2> jacobian_pixel_uv = second->getJacobianPixel2UV ( pixel_position_second );

            Eigen::Matrix<double, 1, 6> jacobian_pixel_ksai = -jacobian_pixel_uv * jacobian_uv_ksai;

            float error = p.second - pixel_second;
            new_residual += error*error;
            good++;
            //cout<<"j_uv_ksai="<<jacobian_pixel_ksai<<endl;
            H.noalias() += jacobian_pixel_ksai.transpose() * jacobian_pixel_ksai;
            b.noalias() += -jacobian_pixel_ksai*error;
        }

        if ( good == 0 )
            break;
        new_residual /= good;
        Eigen::Matrix<double,6,1> x = H.ldlt().solve ( b );
        if ( bool ( std::isnan ( double ( x[0] ) ) ) )
        {
            cerr<<"solving gauss-newton failed."<<endl;
            return 0;
        }

        Vector6d dx;
        dx ( 0 ) = x ( 3 );
        dx ( 1 ) = x ( 4 );
        dx ( 2 ) = x ( 5 );
        dx ( 3 ) = x ( 0 );
        dx ( 4 ) = x ( 1 );
        dx ( 5 ) = x ( 2 );
        dx = step * dx;
        Eigen::Isometry3d dT = ( Eigen::Isometry3d ) g2o::SE3Quat::exp ( dx );
        //cout<<"dT="<<dT.matrix()<<endl;
        second->T_f_w_ = dT*second->T_f_w_;

        cout<<"new residual="<<new_residual<<", old="<<residual<<endl;
        if ( iterate>3 && ( new_residual>residual || ( residual-new_residual ) <epsilon_direct_ ) )
        {
            cout<<"iteration="<<iterate<<endl;
            break;
        }
        residual = new_residual;
    }
    return 0;
}

// TODO: (1) use patch instead of single pixel value (2) use image pyramid level information (3) robustify the error norm and use weights in each term
int Tracker::poseEstimationTwoFramesSemiDirect ( Frame::Ptr first, Frame::Ptr second )
{
    // residual of photometric error
    double residual=1e10, new_residual = 0;

    //SLAMTools::drawFeatures( first );

    double step = 1;

    boost::format fmt ( "iterator: %d" );
    for ( int iterate = 0; iterate<20; iterate ++ )
    {
        Eigen::Matrix<double, 6, 6> H;
        H.setZero();
        Eigen::Matrix<double, 6, 1> b;
        b.setZero();

        new_residual = 0;
        int good = 0;
        // residual and Jacobians
        for ( auto iter_second = second->features_.begin(), iter_first = first->features_.begin(); iter_second != second->features_.end(); )
        {
            // jacobian: from pixel value to se(3)
            Eigen::Matrix<double, 1, 6> jacobian_pixel_ksai;
            // jacobian: from pixel value to pixel coordinates;
            Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
            jacobian_pixel_uv.setZero();

            shared_ptr<Feature> feature_second = (*iter_second).second;
            shared_ptr<Feature> feature_first = (*iter_first).second ;
            //if ( feature_second->quality_ > 0 )
            // this is a feature tracked from first frame
            // compute the bilinear interpolated pixel value
            BiLinearWeights bilinear_weights;
            float pixel_value_first = feature_first->getBilinearPixelValue ( bilinear_weights, jacobian_pixel_uv, false );
            // in second frame we should reproject the 3d position into second frame
            float pixel_value_second_bilinear = feature_second->getBilinearPixelValue ( bilinear_weights, jacobian_pixel_uv, true );

            // we use zero order interpolation here
            //uchar pixel_value_first = feature_first->getPixelValue( false );
            //uchar pixel_value_second = feature_second->getPixelValue( true );

            //cout<<"pixel:"<< ( int ) pixel_value_first<<","<< ( int ) pixel_value_second<<","<<(int)pixel_value_second_bilinear<<endl;
            if ( pixel_value_first < 0 || pixel_value_second_bilinear<0 )
            {
                // remove this feature
                iter_first = first->features_.erase ( iter_first );
                iter_second = second->features_.erase ( iter_second );
                continue;
            }
            else
            {
                iter_first++;
                iter_second++;
            }

            //cout<<"pixel:"<< pixel_value_first<<","<<pixel_value_second_bilinear<<endl;
            double dv = double ( pixel_value_first - pixel_value_second_bilinear );
            //cout<<"j_pixel_uv="<<jacobian_pixel_uv.matrix()<<endl;
            new_residual += dv*dv;
            good++;

            // compute the jacobian_pixel_ksai
            Eigen::Matrix<double,2,6> jacobian_uv_ksai = feature_second->getJacobPixel2se3();
            //jacobian_pixel_uv = feature_second->getZeroOrderJacobian().transpose();
            //cout<<"j_pixel_uv="<<jacobian_pixel_uv.matrix()<<endl;
            //cout<<"Ji="<<jacobian_uv_ksai.matrix() <<endl;
            jacobian_pixel_ksai = -jacobian_pixel_uv * jacobian_uv_ksai;
            H.noalias() += jacobian_pixel_ksai.transpose() *jacobian_pixel_ksai;
            b.noalias() += - jacobian_pixel_ksai * dv;

        } // end of feature loop
        if ( good == 0 )
            break;

        new_residual/=good;
        //cout<<"new residual = "<<new_residual<<", old = "<<residual<<", good points= "<<good<<endl;
        //cout<<H.matrix() <<endl;
        //cout<<b.matrix() <<endl;

        // solve H dx = b and iterate it!
        Eigen::Matrix<double,6,1> x = H.ldlt().solve ( b );
        if ( bool ( std::isnan ( double ( x[0] ) ) ) )
        {
            cerr<<"solving gauss-newton failed."<<endl;
            return 0;
        }

        //cout<<"delta ksai = "<<x<<endl;
        // The se3 used in g2o is [\omega, \upsilon] where the first one is so(3) and second one is translation, but in our formulation the first three numbers are translation and the second are so(3).
        Vector6d dx;
        dx ( 0 ) = x ( 3 );
        dx ( 1 ) = x ( 4 );
        dx ( 2 ) = x ( 5 );
        dx ( 3 ) = x ( 0 );
        dx ( 4 ) = x ( 1 );
        dx ( 5 ) = x ( 2 );
        dx = step * dx;
        Eigen::Isometry3d dT = ( Eigen::Isometry3d ) g2o::SE3Quat::exp ( dx );
        //cout<<"dT="<<endl<<dT.matrix()<<endl;
        second->T_f_w_ = dT*second->T_f_w_;
        //second->T_f_w_ = second->T_f_w_ * dT;

        if ( iterate>5 && ( new_residual>residual || ( residual-new_residual ) <epsilon_direct_ ) )
        {
            //cout<<"iterate="<<iterate<<endl;
            break;
        }
        residual = new_residual;

        /*
        if (iterate%10==0)
        {
            SLAMTools::drawFeaturesReprojection( second, (fmt%iterate).str() );
            cv::waitKey(0);
        }
        cv::waitKey(0);
        */
    }

    //cout<<"first="<<first->T_f_w_.matrix() <<endl;
    //cout<<"second="<<second->T_f_w_.matrix() <<endl;
    // update the features in second frame
    for ( auto feature:second->features_ )
    {
        Eigen::Vector2d pixel = second->world2pixel ( feature.second->position_ );
        //cout<<"pixel="<<pixel.matrix()<<endl;
        //cout<<"keypoint: "<<feature->keypoint_.pt<<endl;
        feature.second->keypoint_.pt.x = pixel[0];
        feature.second->keypoint_.pt.y = pixel[1];
    }

    return 1;
}

int Tracker::poseEstimationTwoFramesSemiDirectUsingG2O ( Frame::Ptr first, Frame::Ptr second )
{
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;
    //typedef g2o::BlockSolverX DirectBlock;
    cout<<"calling pose estimation semi-direct using g2o"<<endl;

    // copy the features into second
    for ( auto f:first->features_ )
    {
        shared_ptr<Feature> f2 (new Feature ( *f.second ));
        f2->frame_ = second.get();
        second->features_.insert ( {f.second->id_, f2} );
    }

    g2o::SparseOptimizer optimizer;
    DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
    //DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<DirectBlock::PoseMatrixType> ();
    DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    //g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton ( solver_ptr );
    optimizer.setAlgorithm ( solver );
    optimizer.setVerbose ( true );

    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setEstimate ( SLAMTools::toSE3Quat ( second->T_f_w_ ) );
    pose->setId ( 0 );
    optimizer.addVertex ( pose );

    int id = 1;
    vector< g2o::EdgeSE3ProjectDirect*> edges;
    //Feature* feature = *second->features_.begin();
    for ( auto f: second->features_ )
    {
        auto feature=f.second;
        //if ( id>5 )
        //    break;
        //cout<<"feature position="<<feature->position_<<endl;
        //cout<<"feature pixel in frame 1="<<feature->keypoint_.pt<<endl;

        //float grayscale_value = first->getBiLinearPixelValue ( feature->getPixelPosition ( false ) ) ;
        //cout<<"measurement="<<grayscale_value<<endl;
        // check whether inside the image
        //if ( feature->isInsideFrameImage ( 10 ) ==false )
        //    continue;
        g2o::EdgeSE3ProjectDirect* edge = new g2o::EdgeSE3ProjectDirect (
            feature->position_,
            second->camera_->fx(),
            second->camera_->fy(),
            second->camera_->cx(),
            second->camera_->cy(),
            &second->color_,
            (feature->keypoint_.octave+1)*2
        );
        edge->setVertex ( 0, pose );
        //edge->setMeasurement ( first->getBiLinearPixelValue ( feature->getPixelPosition ( false ) )-second->getBiLinearPixelValue ( feature->getPixelPosition ( true ) ) );
        edge->setMeasurement ( first->getBiLinearPixelValue ( feature->getPixelPosition ( false ) ) );
        edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );
        //edge->setRobustKernel( new g2o::RobustKernelHuber() );
        edge->setId ( id );
        optimizer.addEdge ( edge );
        edges.push_back ( edge );
        id++;

        //edge->computeError();
        //cout<<"before error="<<edge->error() <<endl;
    }
    cout<<"add total "<<id<<" edges."<<endl;

    cout<<"calling optimize()"<<endl;
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    cout<<"pose="<<endl<<pose->estimate()<<endl;

    // optimizer.optimize ( 10 );
    /*
    cout<<"doing gauss-newton optimization"<<endl;
    for ( int i=0; i<10; i++ )
    {
        Eigen::Matrix<double,6,6> H;
        Eigen::Matrix<double,6,1> b;
        H.setZero();
        b.setZero();
        double chi2 = 0;
        for ( g2o::EdgeSE3ProjectDirect* edge :edges )
        {
            edge->computeError();
            edge->linearizeOplus();

            double error = edge->error() ( 0,0 );

            Eigen::Matrix<double, 1,6> J = edge->jacobian;
            H.noalias() += J.transpose() *J;
            b.noalias() -= J.transpose() * error;
            chi2 += error*error;
        }
        Eigen::Matrix<double,6,1> x = H.ldlt().solve ( b );
        cout<<"x*="<<x<<endl;
        cout<<"chi2="<<chi2<<endl;

        //Eigen::Isometry3d T = pose->estimate();
        //T = Eigen::Isometry3d(g2o::SE3Quat::exp(x))*T;
        //pose->setEstimate( SLAMTools::toSE3Quat(T) );
        //pose->setEstimate( g2o::SE3Quat.exp(x.data())* );
        pose->oplusImpl( x.data() );
    }

    for ( g2o::EdgeSE3ProjectDirect* e:edges )
    {
        e->computeError();
        // cout<<"measurement="<<e->measurement() <<endl;
        cout<<"error="<<e->error() <<endl;
        //cout<<"information="<<e->information() <<endl;
        //cout<<"chi2="<<e->chi2() <<endl;
    }

    */
    cout<<"optimize stops"<<endl;
    cout<<"pose="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;

    second->T_f_w_ = pose->estimate();
    return 0;
}

// direct method to estimate the pose
// because we have confidence map, we can choose points that have high confidence (which indicates their texture is good in stereo matching), instead of computing the derivatives of pixels
int Tracker::poseEstimationTwoFramesDirectUsingG2o ( Frame::Ptr first, Frame::Ptr second )
{
    // use the high confiden points to estimate the pose
    // confidence is 8UC4 but an int number
    float step = 1;
    cout<<"calling direct pose estimation"<<endl;
    vector<pair<Eigen::Vector3d, float>> points_first; // the 3d points in world frame with high confidence value, they need only to be computed once

    int high_confidence_points = 0;
    for ( int y=50; y<first->color_.rows-50; y+=2 )
    {
        for ( int x=50; x<first->color_.cols-50; x+=2 )
        {
            int confidence = first->confidence_.data[ y*first->confidence_.step + x*4 ];
            if ( confidence > confidence_threshold_ )
            {
                high_confidence_points ++ ;
                pair<Eigen::Vector3d, float> p;
                float pixel_first = first->getBiLinearPixelValue ( Eigen::Vector2d ( x,y ) );
                p.first = first->pixel2world ( Eigen::Vector2d ( x,y ) );
                p.second = pixel_first;
                points_first.push_back ( p );
            }
        }
    }

    cout<<"high confidence points="<<high_confidence_points<<endl;

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;
    g2o::SparseOptimizer optimizer;
    DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
    //DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<DirectBlock::PoseMatrixType> ();
    DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    //g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton ( solver_ptr );
    optimizer.setAlgorithm ( solver );
    optimizer.setVerbose ( false );

    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setEstimate ( SLAMTools::toSE3Quat ( second->T_f_w_ ) );
    pose->setId ( 0 );
    optimizer.addVertex ( pose );
    int id=1;
    for ( auto p:points_first )
    {
        g2o::EdgeSE3ProjectDirect* edge = new g2o::EdgeSE3ProjectDirect(
            p.first,
            second->camera_->fx(),
            second->camera_->fy(),
            second->camera_->cx(),
            second->camera_->cy(),
            &second->color_,2
        );
        edge->setVertex(0, pose);
        edge->setMeasurement( p.second );
        edge->setId(id);
        edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );
        edge->setRobustKernel( new g2o::RobustKernelHuber() );

        optimizer.addEdge(edge);
        id++;
    }
    cout<<"add total "<<id<<" edges."<<endl;

    optimizer.initializeOptimization();
    optimizer.optimize(10);

    cout<<"pose="<<pose->estimate()<<endl;
    second->T_f_w_ = pose->estimate();
    return 0;

}

} //namespace zed_slam


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Gentry, A., Smyth, D., and Oliver, W. . The Mark IV Supersonic-Hypersonic Arbitrary Body
 *        Program, Volume II - Program Formulation, Douglas Aircraft Aircraft Company, 1973.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/array.hpp>
#include <boost/make_shared.hpp>
#include <memory>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Astrodynamics/Aerodynamics/hypersonicLocalInclinationAnalysis.h"
#include "Tudat/Astrodynamics/Aerodynamics/customAerodynamicCoefficientInterface.h"
#include "Tudat/Basics/basicTypedefs.h"
#include "Tudat/Mathematics/GeometricShapes/capsule.h"
#include "Tudat/Mathematics/GeometricShapes/sphereSegment.h"

#include "Tudat/Astrodynamics/Aerodynamics/UnitTests/testApolloCapsuleCoefficients.h"

namespace tudat
{
namespace unit_tests
{

using Eigen::Vector6d;
using mathematical_constants::PI;

using namespace aerodynamics;

BOOST_AUTO_TEST_SUITE( test_aerodynamic_coefficient_generator )

//! Test coefficient generator for hypersonic local inclination.
BOOST_AUTO_TEST_CASE( testAerodynamicCoefficientGeneratorHypersonicLocalInclination )
{
    // Set units of coefficients
    const double expectedValueOfForceCoefficient = 1.0;

    // Tolerance in absolute units.
    const double toleranceForceCoefficient = 1.0e-2;
    const double toleranceAerodynamicCoefficients3 = 1.0e-4;
    const double toleranceAerodynamicCoefficients4 = 1.0e-2;
    const double toleranceAerodynamicCoefficients5 = 1.0e-4;

    // Create test sphere.
    std::shared_ptr< geometric_shapes::SphereSegment > sphere
            = std::make_shared< geometric_shapes::SphereSegment >( 1.0 );

    // Set vehicle in analysis with 10,000 panels.
    std::vector< int > numberOfLines( 1, 31 );
    std::vector< int > numberOfPoints( 1, 31 );
    std::vector< bool > invertOrder( 1, false );

    // Create analysis object.
    std::vector< std::vector< double > > independentVariableDataPoints;
    independentVariableDataPoints.resize( 3 );

    independentVariableDataPoints[ 0 ] =
            getDefaultHypersonicLocalInclinationMachPoints( "Full" );
    independentVariableDataPoints[ 1 ] =
            getDefaultHypersonicLocalInclinationAngleOfAttackPoints( );
    independentVariableDataPoints[ 2 ] =
            getDefaultHypersonicLocalInclinationAngleOfSideslipPoints( );

    // Set methods to use for aerodynamic analysis.
    std::vector< std::vector< int > > analysisMethod( 2, std::vector< int >( 1, 0 ));
    analysisMethod[ 1 ][ 0 ] = 1;

    // Generate sphere database of aerodynamic coefficients.
    std::shared_ptr< HypersonicLocalInclinationAnalysis > coefficientInterface =
            std::make_shared< HypersonicLocalInclinationAnalysis >(
                independentVariableDataPoints, sphere,
                numberOfLines, numberOfPoints,
                invertOrder, analysisMethod, PI, 1.0,
                Eigen::Vector3d::Zero( ) );

    // Test basic properties of coefficient generator
    BOOST_CHECK_EQUAL(
                coefficientInterface->getIndependentVariableNames( ).size( ), 3 );
    BOOST_CHECK_EQUAL( coefficientInterface->getIndependentVariableName( 0 ),
                       mach_number_dependent );
    BOOST_CHECK_EQUAL( coefficientInterface->getIndependentVariableName( 1 ),
                       angle_of_attack_dependent );
    BOOST_CHECK_EQUAL( coefficientInterface->getIndependentVariableName( 2 ),
                       angle_of_sideslip_dependent );

    bool isVariableIndexTooHigh = 0;
    try
    {
        coefficientInterface->getIndependentVariableName( 3 );
    }
    catch ( std::runtime_error )
    {
        isVariableIndexTooHigh = 1;
    }
    BOOST_CHECK_EQUAL( isVariableIndexTooHigh, 1 );

    // Allocate memory for independent variables to pass to analysis for retrieval.
    boost::array< int, 3 > independentVariables;
    independentVariables[ 0 ] = 0;
    independentVariables[ 1 ] = 0;
    independentVariables[ 2 ] = 0;
    std::vector< double > independentVariablesVector( 3 );
    std::vector< double > interpolatingIndependentVariablesVector( 3 );

    // Declare local test variables.
    Vector6d aerodynamicCoefficients_ = Vector6d::Zero( );
    double forceCoefficient_;

    // Iterate over all angles of attack to verify sphere coefficients. Total force coefficient
    // should be one; all moment coefficients should be zero.
    // The functionality is tested directly from the generator, as well as from the
    // coefficient interface, both interpolated at the nodes, and halfway between the nodes.
    for ( int i = 0; i < coefficientInterface->getNumberOfValuesOfIndependentVariable( 0 ); i++ )
    {
        independentVariables[ 0 ] = i;
        independentVariablesVector[ 0 ] = coefficientInterface->getIndependentVariablePoint( 0, i );
        if( i < coefficientInterface->getNumberOfValuesOfIndependentVariable( 0 ) - 1 )
        {
            interpolatingIndependentVariablesVector[ 0 ] =
                    coefficientInterface->getIndependentVariablePoint( 0, i ) + 0.5 * (
                        coefficientInterface->getIndependentVariablePoint( 0, i + 1 ) -
                        coefficientInterface->getIndependentVariablePoint( 0, i ) );
        }

        for ( int j = 0; j <
              coefficientInterface->getNumberOfValuesOfIndependentVariable( 1 ); j++ )
        {
            independentVariables[ 1 ] = j;
            independentVariablesVector[ 1 ] =
                    coefficientInterface->getIndependentVariablePoint( 1, j );
            if( j < coefficientInterface->getNumberOfValuesOfIndependentVariable( 1 ) - 1 )
            {
                interpolatingIndependentVariablesVector[ 1 ] =
                        coefficientInterface->getIndependentVariablePoint( 1, j ) + 0.5 * (
                            coefficientInterface->getIndependentVariablePoint( 1, j + 1 ) -
                            coefficientInterface->getIndependentVariablePoint( 1, j ) );
            }

            for ( int k = 0; k <
                  coefficientInterface->getNumberOfValuesOfIndependentVariable( 2 ); k++ )
            {
                independentVariables[ 2 ] = k;
                independentVariablesVector[ 2 ] =
                        coefficientInterface->getIndependentVariablePoint( 2, k );
                if( k < coefficientInterface->getNumberOfValuesOfIndependentVariable( 2 ) - 1 )
                {
                    interpolatingIndependentVariablesVector[ 2 ] =
                            coefficientInterface->getIndependentVariablePoint( 2, k ) + 0.5 * (
                                coefficientInterface->getIndependentVariablePoint( 2, k + 1 ) -
                                coefficientInterface->getIndependentVariablePoint( 2, k ) );
                }

                // Retrieve aerodynamic coefficients.
                aerodynamicCoefficients_ =
                        coefficientInterface->getAerodynamicCoefficientsDataPoint(
                            independentVariables );
                forceCoefficient_ = ( aerodynamicCoefficients_.head( 3 ) ).norm( );

                // Test if the computed force coefficient corresponds to the expected value
                // within the specified tolerance.
                BOOST_CHECK_CLOSE_FRACTION( forceCoefficient_,
                                            expectedValueOfForceCoefficient,
                                            toleranceForceCoefficient );

                // Test if the computed moment coefficients correspond to the expected value (0.0)
                // within the specified tolerance.
                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 3 ),
                                   toleranceAerodynamicCoefficients3 );

                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 4 ),
                                   toleranceAerodynamicCoefficients4 );

                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 5 ),
                                   toleranceAerodynamicCoefficients5 );

                // Retrieve aerodynamic coefficients from coefficient interface.
                coefficientInterface->updateCurrentCoefficients( independentVariablesVector );

                aerodynamicCoefficients_ =
                        coefficientInterface->getCurrentAerodynamicCoefficients( );
                forceCoefficient_ = ( aerodynamicCoefficients_.head( 3 ) ).norm( );

                // Test if the computed force coefficient corresponds to the expected value
                // within the specified tolerance.
                BOOST_CHECK_CLOSE_FRACTION( forceCoefficient_,
                                            expectedValueOfForceCoefficient,
                                            toleranceForceCoefficient );

                // Test if the computed moment coefficients correspond to the expected value (0.0)
                // within the specified tolerance.
                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 3 ),
                                   toleranceAerodynamicCoefficients3 );

                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 4 ),
                                   toleranceAerodynamicCoefficients4 );

                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 5 ),
                                   toleranceAerodynamicCoefficients5 );

                // Retrieve aerodynamic coefficients from coefficient interface.
                coefficientInterface->updateCurrentCoefficients(
                            interpolatingIndependentVariablesVector );

                aerodynamicCoefficients_ =
                        coefficientInterface->getCurrentAerodynamicCoefficients( );
                forceCoefficient_ = ( aerodynamicCoefficients_.head( 3 ) ).norm( );

                // Test if the computed force coefficient corresponds to the expected value
                // within the specified tolerance.
                BOOST_CHECK_CLOSE_FRACTION( forceCoefficient_,
                                            expectedValueOfForceCoefficient,
                                            toleranceForceCoefficient );

                // Test if the computed moment coefficients correspond to the expected value (0.0)
                // within the specified tolerance.
                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 3 ),
                                   toleranceAerodynamicCoefficients3 );

                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 4 ),
                                   toleranceAerodynamicCoefficients4 );

                BOOST_CHECK_SMALL( aerodynamicCoefficients_( 5 ),
                                   toleranceAerodynamicCoefficients5 );
            }
        }
    }
}

std::shared_ptr< HypersonicLocalInclinationAnalysis > getApolloCoefficientInterface( )
{

    // Create test capsule.
    std::shared_ptr< geometric_shapes::Capsule > capsule
            = std::make_shared< geometric_shapes::Capsule >(
                4.694, 1.956, 2.662, -1.0 * 33.0 * PI / 180.0, 0.196 );

    std::vector< int > numberOfLines( 4 );
    std::vector< int > numberOfPoints( 4 );
    std::vector< bool > invertOrders( 4 );

    // Set number of analysis points.
    numberOfLines[ 0 ] = 31;
    numberOfPoints[ 0 ] = 31;
    numberOfLines[ 1 ] = 31;
    numberOfPoints[ 1 ] = 31;
    numberOfLines[ 2 ] = 31;
    numberOfPoints[ 2 ] = 10;
    numberOfLines[ 3 ] = 11;
    numberOfPoints[ 3 ] = 11;
    invertOrders[ 0 ] = 0;
    invertOrders[ 1 ] = 0;
    invertOrders[ 2 ] = 0;
    invertOrders[ 3 ] = 0;

    Eigen::Vector3d momentReference;
    momentReference( 0 ) = -0.6624;
    momentReference( 1 ) = 0.0;
    momentReference( 2 ) = -0.1369;

    std::vector< std::vector< double > > independentVariableDataPoints( 3 );
    independentVariableDataPoints[ 0 ] = getDefaultHypersonicLocalInclinationMachPoints( "Full" );
    std::vector< double > angleOfAttackPoints( 7 );

    for ( int i = 0; i < 7; i++ )
    {
        angleOfAttackPoints[ i ] = static_cast< double >( i - 6 ) * 5.0 * PI / 180.0;
    }

    independentVariableDataPoints[ 1 ] = angleOfAttackPoints;
    independentVariableDataPoints[ 2 ] =
            getDefaultHypersonicLocalInclinationAngleOfSideslipPoints( );
    std::vector< std::vector< int > > selectedMethods( 2, std::vector< int >( 4 ));

    selectedMethods[ 0 ][ 0 ] = 1;
    selectedMethods[ 0 ][ 1 ] = 5;
    selectedMethods[ 0 ][ 2 ] = 5;
    selectedMethods[ 0 ][ 3 ] = 1;
    selectedMethods[ 1 ][ 0 ] = 6;
    selectedMethods[ 1 ][ 1 ] = 3;
    selectedMethods[ 1 ][ 2 ] = 3;
    selectedMethods[ 1 ][ 3 ] = 3;

    // Create analysis object and capsule database.
    return std::make_shared< HypersonicLocalInclinationAnalysis >(
                independentVariableDataPoints, capsule, numberOfLines, numberOfPoints,
                invertOrders, selectedMethods, PI * pow( capsule->getMiddleRadius( ), 2.0 ),
                3.9116, momentReference );
}

//! Apollo capsule test case.
BOOST_AUTO_TEST_CASE( testApolloCapsule )
{
    // Set units of coefficients.
    const double expectedValueOfAerodynamicCoefficients0 = -1.51;
    const double expectedValueOfAerodynamicCoefficients4 = -0.052;

    // Tolerance in absolute units.
    const double toleranceAerodynamicCoefficients0 = 0.1;
    const double toleranceAerodynamicCoefficients1 = std::numeric_limits< double >::epsilon( );
    const double toleranceAerodynamicCoefficients2 = std::numeric_limits< double >::epsilon( );
    const double toleranceAerodynamicCoefficients3 = std::numeric_limits< double >::epsilon( );
    const double toleranceAerodynamicCoefficients4 = 0.05;
    const double toleranceAerodynamicCoefficients5 = std::numeric_limits< double >::epsilon( );

    // Create aerodynamic coefficients.
    std::shared_ptr< HypersonicLocalInclinationAnalysis > coefficientInterface = getApolloCoefficientInterface( );

    // Retrieve coefficients at zero angle of attack for comparison.
    boost::array< int, 3 > independentVariables;

    independentVariables[ 0 ] =
            coefficientInterface->getNumberOfValuesOfIndependentVariable( 0 ) - 1;
    independentVariables[ 1 ] = 6;
    independentVariables[ 2 ] = 0;

    // Declare local test variables.
    Eigen::VectorXd aerodynamicCoefficients_;
    aerodynamicCoefficients_ = coefficientInterface->getAerodynamicCoefficientsDataPoint(
                independentVariables );

    // Compare values to database values.
    BOOST_CHECK_SMALL(
                aerodynamicCoefficients_( 0 ) - expectedValueOfAerodynamicCoefficients0,
                toleranceAerodynamicCoefficients0 );

    BOOST_CHECK_SMALL( aerodynamicCoefficients_( 1 ),
                       toleranceAerodynamicCoefficients1 );

    BOOST_CHECK_SMALL( aerodynamicCoefficients_( 2 ),
                       toleranceAerodynamicCoefficients2 );

    BOOST_CHECK_SMALL( aerodynamicCoefficients_( 3 ),
                       toleranceAerodynamicCoefficients3 );

    BOOST_CHECK_SMALL(
                aerodynamicCoefficients_( 4 ) - expectedValueOfAerodynamicCoefficients4,
                toleranceAerodynamicCoefficients4 );

    BOOST_CHECK_SMALL( aerodynamicCoefficients_( 5 ),
                       toleranceAerodynamicCoefficients5 );

    std::vector< double > independentVariablesVector( 3 );
    independentVariablesVector[ 0 ] = coefficientInterface->getIndependentVariablePoint(
                0, coefficientInterface->getNumberOfValuesOfIndependentVariable( 0 ) - 1 );
    independentVariablesVector[ 1 ] = coefficientInterface->getIndependentVariablePoint(
                1, 6 );
    independentVariablesVector[ 2 ] = coefficientInterface->getIndependentVariablePoint(
                2, 0 );

    coefficientInterface->updateCurrentCoefficients( independentVariablesVector );
    aerodynamicCoefficients_ = coefficientInterface->getCurrentAerodynamicCoefficients( );

    // Compare values to database values.
    BOOST_CHECK_SMALL(
                aerodynamicCoefficients_( 0 ) - expectedValueOfAerodynamicCoefficients0,
                toleranceAerodynamicCoefficients0 );

    BOOST_CHECK_SMALL( aerodynamicCoefficients_( 1 ),
                       toleranceAerodynamicCoefficients1 );

    BOOST_CHECK_SMALL( aerodynamicCoefficients_( 2 ),
                       toleranceAerodynamicCoefficients2 );

    BOOST_CHECK_SMALL( aerodynamicCoefficients_( 3 ),
                       toleranceAerodynamicCoefficients3 );

    BOOST_CHECK_SMALL(
                aerodynamicCoefficients_( 4 ) - expectedValueOfAerodynamicCoefficients4,
                toleranceAerodynamicCoefficients4 );

    BOOST_CHECK_SMALL( aerodynamicCoefficients_( 5 ),
                       toleranceAerodynamicCoefficients5 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/*
 * Luca Anzalone
 */

#include <jni.h>

#include <string>
#include <vector>
#include <mutex>

#include <android/log.h>

#include <opencv2/opencv.hpp>
#include <opencv2/features2d.hpp>  // FAST
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/tracking.hpp>

#include <dlib/image_io.h>
#include <dlib/image_processing.h>
#include <dlib/image_processing/generic_image.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/opencv/cv_image.h>
#include <dlib/dnn.h>
#define LOG_TAG "native-lib"
#define LOGD(...) \
  ((void)__android_log_print(ANDROID_LOG_DEBUG, LOG_TAG, __VA_ARGS__))

#define JNI_METHOD(NAME) \
    Java_com_dev_anzalone_luca_facelandmarks_Native_##NAME

#define KERNEL_SIZE 5 // 3, 5, 7, 9

#define NV21 17
#define YV12 842094169
#define YUV_420_888 35
#define PYRAMIDS 3
#define MAX_FRAME_COUNT 5

using namespace std;
using namespace dlib;
// ----------------------------------------------------------------------------------------

// The next bit of code defines a ResNet network.  It's basically copied
// and pasted from the dnn_imagenet_ex.cpp example, except we replaced the loss
// layer with loss_metric and made the network somewhat smaller.  Go read the introductory
// dlib DNN examples to learn what all this stuff means.
//
// Also, the dnn_metric_learning_on_images_ex.cpp example shows how to train this network.
// The dlib_face_recognition_resnet_model_v1 model used by this example was trained using
// essentially the code shown in dnn_metric_learning_on_images_ex.cpp except the
// mini-batches were made larger (35x15 instead of 5x5), the iterations without progress
// was set to 10000, and the training dataset consisted of about 3 million images instead of
// 55.  Also, the input layer was locked to images of size 150.
template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual = add_prev1<block<N,BN,1,tag1<SUBNET>>>;

template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual_down = add_prev2<avg_pool<2,2,2,2,skip1<tag2<block<N,BN,2,tag1<SUBNET>>>>>>;

template <int N, template <typename> class BN, int stride, typename SUBNET>
using block  = BN<con<N,3,3,1,1,relu<BN<con<N,3,3,stride,stride,SUBNET>>>>>;

template <int N, typename SUBNET> using ares      = relu<residual<block,N,affine,SUBNET>>;
template <int N, typename SUBNET> using ares_down = relu<residual_down<block,N,affine,SUBNET>>;

template <typename SUBNET> using alevel0 = ares_down<256,SUBNET>;
template <typename SUBNET> using alevel1 = ares<256,ares<256,ares_down<256,SUBNET>>>;
template <typename SUBNET> using alevel2 = ares<128,ares<128,ares_down<128,SUBNET>>>;
template <typename SUBNET> using alevel3 = ares<64,ares<64,ares<64,ares_down<64,SUBNET>>>>;
template <typename SUBNET> using alevel4 = ares<32,ares<32,ares<32,SUBNET>>>;

using anet_type = loss_metric<fc_no_bias<128,avg_pool_everything<
                            alevel0<
                            alevel1<
                            alevel2<
                            alevel3<
                            alevel4<
                            max_pool<3,3,2,2,relu<affine<con<32,7,7,2,2,
                            input_rgb_image_sized<150>
                            >>>>>>>>>>>>;

// ----------------------------------------------------------------------------------------

dlib::vector<matrix<rgb_pixel>> jitter_image(
    const matrix<rgb_pixel>& img
);

// ----------------------------------------------------------------------------------------

// global variables:
dlib::shape_predictor  predictor;
dlib::matrix<float,0,1> descriptor;

std::vector<dlib::matrix<float,0,1>> face_descriptors;
anet_type face_net;
std::mutex _mutex;
int imageFormat = NV21;

// -------------------------------------------------------------------------------------------------
// -- Lucas-Kanade Optical Flow Tracker
// -------------------------------------------------------------------------------------------------
namespace LK {
    // variables
    int frameCount = 0;
    bool isTracking = false;
    cv::Mat prev_img;
    std::vector<cv::Point2f> prev_pts;
    std::vector<cv::Point2f> next_pts;
    cv::TermCriteria criteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 25, 0.01);
    cv::Size ROI(20, 20);

    /** Initialize tracking with the current frame and detected landmarks */
    void start(cv::Mat &mat, dlib::full_object_detection &pts) {
        // release stuff..
        prev_img.release();
        prev_img = mat;
        prev_pts.clear();
        next_pts.clear();

        // consider the new points
        for (unsigned long i = 0; i < pts.num_parts(); i++) {
            auto pt = pts.part(i);
            prev_pts.push_back(cv::Point2f(pt.x(), pt.y()));
        }

        // reset count
        frameCount = 0;
        isTracking = true;
    }

    /** tracking points in the next captured frame */
    std::vector<cv::Point2f> track(cv::Mat &frame) {
        std::vector<uchar> status;
        std::vector<float> err;
        std::vector<cv::Point2f> tracked;

        // get the new points from the old one
        calcOpticalFlowPyrLK(prev_img, frame, prev_pts, next_pts, status, err,
                             ROI, PYRAMIDS, criteria);

        for (int i = 0; i < status.size(); ++i) {
            if (status[i] == 0) {
                // flow not found: take the old point
                tracked.push_back(prev_pts[i]);
            } else {
                // flow found: take the new point
                tracked.push_back(next_pts[i]);
            }
        }

        // switch the previous points and image with the current
        swap(prev_img, frame);
        swap(prev_pts, tracked);
        next_pts.clear();

        // increase tracking frame count
        if (frameCount++ > MAX_FRAME_COUNT) {
            isTracking = false;
        }

        return prev_pts;
    }
}
// -------------------------------------------------------------------------------------------------

extern "C"
JNIEXPORT void JNICALL
JNI_METHOD(loadModel)(JNIEnv* env, jclass, jstring detectorPath, jstring netPath) {
    try {
        const char *path = env->GetStringUTFChars(detectorPath, JNI_FALSE);
        const char *net = env->GetStringUTFChars(netPath, JNI_FALSE);
        _mutex.lock();
            // cause the later initialization of the tracking
            LK::isTracking = false;

            // load the shape predictor
            dlib::deserialize(path) >> predictor;
            dlib::deserialize(net) >> face_net;
        _mutex.unlock();

        env->ReleaseStringUTFChars(detectorPath, path); //free mem
        env->ReleaseStringUTFChars(netPath, net); //free mem
        LOGD("JNI: model loaded");

    } catch (dlib::serialization_error &e) {
        LOGD("JNI: failed to model -> %s", e.what());
    }
}
//--------------------------------------------------------------------------------------------------

void rotateMat(cv::Mat &mat, int rotation) {
    if (rotation == 90) { // portrait
        LOGD("JNI: rotation 90");
        cv::transpose(mat, mat);
        cv::flip(mat, mat, -1);
    } else if (rotation == 0) { // landscape-left
        LOGD("JNI: rotation 0");
        cv::flip(mat, mat, 1);
    } else if (rotation == 180) { // landscape-right
        LOGD("JNI: rotation 180");
        cv::flip(mat, mat, 0);
    }
}

extern "C"
JNIEXPORT void JNICALL
JNI_METHOD(setImageFormat)(JNIEnv* env, jclass, jint format) {
    imageFormat = format;
}

extern "C"
JNIEXPORT void JNICALL
JNI_METHOD(loadFace) (JNIEnv * env, jobject obj, jobjectArray myArray)
{
  int len1 = 16;
  jfloatArray dim =  (jfloatArray)env->GetObjectArrayElement(myArray, 0);
  int len2 = 128;

  for(int i=0; i<len1; ++i){
     jfloatArray oneDim = (jfloatArray)env->GetObjectArrayElement(myArray, i);
     jfloat *element = env->GetFloatArrayElements(oneDim, 0);
     dlib::matrix<float,0,1> fdata;
     fdata.set_size(128, 1);

     //allocate localArray[i] using len2
     for(int j=0; j<len2; ++j) {
        LOGD("____________ %f", element[j]);
        fdata(j,0) =  element[j];
     }
     face_descriptors.push_back(fdata);

     env->ReleaseFloatArrayElements( oneDim , (jfloat *)element, 0);
  }
  //TODO play with localArray, don't forget to release memory ;)
}

//--------------------------------------------------------------------------------------------------
//-- LANDMARK DETECTION
//--------------------------------------------------------------------------------------------------
extern "C"
JNIEXPORT jlongArray JNICALL
JNI_METHOD(detectLandmarks)(JNIEnv* env, jclass, jbyteArray yuvFrame, jint rotation, jint width, jint height, jint left, jint top, jint right, jint bottom) {
    LOGD("JNI: detectLandmarks");

    // copy content of frame into image
    jbyte *data = env->GetByteArrayElements(yuvFrame, 0);

    // convert yuv-frame to cv::Mat
    cv::Mat yuvMat(height + height / 2, width, CV_8UC1, (unsigned char *) data);
    cv::Mat grayMat(height, width, CV_8UC1);

    // to grayscale
    if (imageFormat == NV21)
        cv::cvtColor(yuvMat, grayMat, cv::COLOR_YUV2GRAY_NV21);  // was CV_YUV2GRAY_NV21
    else if (imageFormat == YV12)
        cv::cvtColor(yuvMat, grayMat, cv::COLOR_YUV2GRAY_YV12);  // was CV_YUV2GRAY_YV12

    // adjust rotation according to phone orientation
    rotateMat(grayMat, rotation);

    // crop face for enhancements
    cv::Rect faceROI(left, top, right - left, bottom - top);
    cv::Mat face = grayMat(faceROI);

    // apply filters
    cv::medianBlur(face, face, KERNEL_SIZE);  // remove noise
    cv::equalizeHist(face, face);  // improve contrast

    if (!LK::isTracking) {
        // -- DETECT LANDMARKS -- //

        // cv::mat to dlib::image
        dlib::cv_image<unsigned char> image(grayMat);

        // detect landmark points
        _mutex.lock();
        dlib::rectangle region(left, top, right, bottom);
        dlib::full_object_detection points = predictor(image, region);
        std::vector<dlib::matrix<rgb_pixel>> faces;
        dlib::matrix<rgb_pixel> face_chip;
        dlib::extract_image_chip(image, get_face_chip_details(points,150,0.25), face_chip);
        faces.push_back(move(face_chip));
        std::vector<dlib::matrix<float,0,1>> descriptors = face_net(faces);

        if (dlib::length(face_descriptors[5] - descriptors[0]) < 0.5){
            LOGD("____________ FACE RECOGNITION");
        }
        _mutex.unlock();

        // result
        auto num_points = points.num_parts();
        jsize len = (jsize) (num_points * sizeof(short)); // num_points * 2

        jlong buffer[len];
        jlongArray result = env->NewLongArray(len);

        // copy points in the buffer
        auto k = 0;
        for (unsigned long i = 0l; i < num_points; ++i) {
            dlib::point p = points.part(i);
            buffer[k++] = p.x();
            buffer[k++] = p.y();
        }

        // set the content of buffer into result array
        env->SetLongArrayRegion(result, 0, len, buffer);

        // free mem
        env->ReleaseByteArrayElements(yuvFrame, data, 0);

        // uncomment to enable tracking for the next frames
//        LK::start(grayMat, points);

        return result;

    } else {
        // -- COMPUTE LK-OPTICAL FLOW --
        auto trackedPts = LK::track(grayMat);

        // result
        auto num_points = trackedPts.size();
        jsize len = (jsize) (num_points * sizeof(short)); // num_points * 2

        jlong buffer[len];
        jlongArray result = env->NewLongArray(len);

        // copy tracked points in the buffer
        auto k = 0;
        for (unsigned long i = 0l; i < num_points; ++i) {
            auto p = trackedPts[i];
            buffer[k++] = static_cast<jlong>(p.x);
            buffer[k++] = static_cast<jlong>(p.y);
        }

        // set the content of buffer into result array
        env->SetLongArrayRegion(result, 0, len, buffer);

        // free mem
        env->ReleaseByteArrayElements(yuvFrame, data, 0);

        return result;
    }
}

//--------------------------------------------------------------------------------------------------


/**
 * @file gan_impl.hpp
 * @author Kris Singh
 * @author Shikhar Jaiswal
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license. You should have received a copy of the
 * 3-clause BSD license along with mlpack. If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_ANN_GAN_GAN_IMPL_HPP
#define MLPACK_METHODS_ANN_GAN_GAN_IMPL_HPP

#include "gan.hpp"

#include <mlpack/core.hpp>

#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/init_rules/network_init.hpp>
#include <mlpack/methods/ann/visitor/output_parameter_visitor.hpp>
#include <mlpack/methods/ann/activation_functions/softplus_function.hpp>
#include <boost/serialization/variant.hpp>

namespace mlpack {
namespace ann /** Artifical Neural Network.  */ {
template<
  typename Model,
  typename InitializationRuleType,
  typename Noise,
  typename PolicyType
>
GAN<Model, InitializationRuleType, Noise, PolicyType>::GAN(
    arma::mat& predictors,
    Model generator,
    Model discriminator,
    InitializationRuleType& initializeRule,
    Noise& noiseFunction,
    const size_t noiseDim,
    const size_t batchSize,
    const size_t generatorUpdateStep,
    const size_t preTrainSize,
    const double multiplier,
    const double clippingParameter,
    const double lambda):
    generator(std::move(generator)),
    discriminator(std::move(discriminator)),
    initializeRule(initializeRule),
    noiseFunction(noiseFunction),
    noiseDim(noiseDim),
    batchSize(batchSize),
    generatorUpdateStep(generatorUpdateStep),
    preTrainSize(preTrainSize),
    multiplier(multiplier),
    clippingParameter(clippingParameter),
    lambda(lambda),
    reset(false)
{
  // Insert IdentityLayer for joining the Generator and Discriminator.
  this->discriminator.network.insert(
      this->discriminator.network.begin(),
      new IdentityLayer<>());

  counter = 0;
  currentBatch = 0;

  this->discriminator.deterministic = this->generator.deterministic = true;

  this->predictors.set_size(predictors.n_rows, predictors.n_cols + batchSize);
  this->predictors.cols(0, predictors.n_cols - 1) = predictors;
  this->discriminator.predictors = arma::mat(this->predictors.memptr(),
      this->predictors.n_rows, this->predictors.n_cols, false, false);

  responses.ones(1, predictors.n_cols + batchSize);
  responses.cols(predictors.n_cols,
      predictors.n_cols + batchSize - 1) = arma::zeros(1, batchSize);
  this->discriminator.responses = arma::mat(this->responses.memptr(),
      this->responses.n_rows, this->responses.n_cols, false, false);

  numFunctions = predictors.n_cols;

  noise.set_size(noiseDim, batchSize);

  this->generator.predictors.set_size(noiseDim, batchSize);
  this->generator.responses.set_size(predictors.n_rows, batchSize);
}

template<
  typename Model,
  typename InitializationRuleType,
  typename Noise,
  typename PolicyType
>
GAN<Model, InitializationRuleType, Noise, PolicyType>::GAN(
    const GAN& network):
    predictors(network.predictors),
    responses(network.responses),
    generator(network.generator),
    discriminator(network.discriminator),
    initializeRule(network.initializeRule),
    noiseFunction(network.noiseFunction),
    noiseDim(network.noiseDim),
    batchSize(network.batchSize),
    generatorUpdateStep(network.generatorUpdateStep),
    preTrainSize(network.preTrainSize),
    multiplier(network.multiplier),
    clippingParameter(network.clippingParameter),
    lambda(network.lambda),
    reset(network.reset),
    counter(network.counter),
    currentBatch(network.currentBatch),
    parameter(network.parameter),
    numFunctions(network.numFunctions),
    noise(network.noise)
{
  /* Nothing to do here */
}

template<
  typename Model,
  typename InitializationRuleType,
  typename Noise,
  typename PolicyType
>
GAN<Model, InitializationRuleType, Noise, PolicyType>::GAN(
    GAN&& network):
    predictors(std::move(network.predictors)),
    responses(std::move(network.responses)),
    generator(std::move(network.generator)),
    discriminator(std::move(network.discriminator)),
    initializeRule(std::move(network.initializeRule)),
    noiseFunction(std::move(network.noiseFunction)),
    noiseDim(network.noiseDim),
    batchSize(network.batchSize),
    generatorUpdateStep(network.generatorUpdateStep),
    preTrainSize(network.preTrainSize),
    multiplier(network.multiplier),
    clippingParameter(network.clippingParameter),
    lambda(network.lambda),
    reset(network.reset),
    counter(network.counter),
    currentBatch(network.currentBatch),
    parameter(std::move(network.parameter)),
    numFunctions(network.numFunctions),
    noise(std::move(network.noise))
{
  /* Nothing to do here */
}

template<
  typename Model,
  typename InitializationRuleType,
  typename Noise,
  typename PolicyType
>
void GAN<Model, InitializationRuleType, Noise, PolicyType>::Reset()
{
  size_t genWeights = 0;
  size_t discWeights = 0;

  NetworkInitialization<InitializationRuleType> networkInit(initializeRule);

  for (size_t i = 0; i < generator.network.size(); ++i)
  {
    genWeights += boost::apply_visitor(weightSizeVisitor, generator.network[i]);
  }

  for (size_t i = 0; i < discriminator.network.size(); ++i)
  {
    discWeights += boost::apply_visitor(weightSizeVisitor,
        discriminator.network[i]);
  }

  parameter.set_size(genWeights + discWeights, 1);
  generator.Parameters() = arma::mat(parameter.memptr(), genWeights, 1, false,
      false);
  discriminator.Parameters() = arma::mat(parameter.memptr() + genWeights,
      discWeights, 1, false, false);

  // Initialize the parameters generator
  networkInit.Initialize(generator.network, parameter);
  // Initialize the parameters discriminator
  networkInit.Initialize(discriminator.network, parameter, genWeights);

  reset = true;
}

template<
  typename Model,
  typename InitializationRuleType,
  typename Noise,
  typename PolicyType
>
template<typename OptimizerType>
double GAN<Model, InitializationRuleType, Noise, PolicyType>::Train(
    OptimizerType& Optimizer)
{
  if (!reset)
    Reset();
  return Optimizer.Optimize(*this, parameter);
}

template<
  typename Model,
  typename InitializationRuleType,
  typename Noise,
  typename PolicyType
>
template<typename Policy>
typename std::enable_if<std::is_same<Policy, StandardGAN>::value ||
                        std::is_same<Policy, DCGAN>::value, double>::type
GAN<Model, InitializationRuleType, Noise, PolicyType>::Evaluate(
    const arma::mat& /* parameters */,
    const size_t i,
    const size_t /* batchSize */)
{
  if (!reset)
    Reset();

  currentInput = arma::mat(predictors.memptr() + (i * predictors.n_rows),
      predictors.n_rows, batchSize, false, false);
  currentTarget = arma::mat(responses.memptr() + i, 1, batchSize, false,
      false);

  discriminator.Forward(std::move(currentInput));
  double res = discriminator.outputLayer.Forward(
      std::move(boost::apply_visitor(
      outputParameterVisitor,
      discriminator.network.back())), std::move(currentTarget));

  noise.imbue( [&]() { return noiseFunction();} );
  generator.Forward(std::move(noise));

  predictors.cols(numFunctions, numFunctions + batchSize - 1) =
      boost::apply_visitor(outputParameterVisitor, generator.network.back());
  discriminator.Forward(std::move(predictors.cols(numFunctions,
      numFunctions + batchSize - 1)));
  responses.cols(numFunctions, numFunctions + batchSize - 1) =
      arma::zeros(1, batchSize);

  currentTarget = arma::mat(responses.memptr() + numFunctions,
      1, batchSize, false, false);
  res += discriminator.outputLayer.Forward(
      std::move(boost::apply_visitor(
      outputParameterVisitor,
      discriminator.network.back())), std::move(currentTarget));

  return res;
}

template<
  typename Model,
  typename InitializationRuleType,
  typename Noise,
  typename PolicyType
>
template<typename GradType, typename Policy>
typename std::enable_if<std::is_same<Policy, StandardGAN>::value ||
                        std::is_same<Policy, DCGAN>::value, double>::type
GAN<Model, InitializationRuleType, Noise, PolicyType>::
EvaluateWithGradient(const arma::mat& /* parameters */,
                     const size_t i,
                     GradType& gradient,
                     const size_t /* batchSize */)
{
  if (!reset)
    Reset();

  if (gradient.is_empty())
  {
    if (parameter.is_empty())
      Reset();
    gradient = arma::zeros<arma::mat>(parameter.n_elem, 1);
  }
  else
    gradient.zeros();

  if (noiseGradientDiscriminator.is_empty())
  {
    noiseGradientDiscriminator = arma::zeros<arma::mat>(
        gradientDiscriminator.n_elem, 1);
  }
  else
  {
    noiseGradientDiscriminator.zeros();
  }

  gradientGenerator = arma::mat(gradient.memptr(),
      generator.Parameters().n_elem, 1, false, false);

  gradientDiscriminator = arma::mat(gradient.memptr() +
      gradientGenerator.n_elem,
      discriminator.Parameters().n_elem, 1, false, false);

  // Get the gradients of the Discriminator.
  double res = discriminator.EvaluateWithGradient(discriminator.parameter,
      i, gradientDiscriminator, batchSize);

  noise.imbue( [&]() { return noiseFunction();} );
  generator.Forward(std::move(noise));
  predictors.cols(numFunctions, numFunctions + batchSize - 1) =
      boost::apply_visitor(outputParameterVisitor, generator.network.back());
  responses.cols(numFunctions, numFunctions + batchSize - 1) =
      arma::zeros(1, batchSize);

  // Get the gradients of the Generator.
  res += discriminator.EvaluateWithGradient(discriminator.parameter,
      numFunctions, noiseGradientDiscriminator, batchSize);
  gradientDiscriminator += noiseGradientDiscriminator;

  if (currentBatch % generatorUpdateStep == 0 && preTrainSize == 0)
  {
    // Minimize -log(D(G(noise))).
    // Pass the error from Discriminator to Generator.
    responses.cols(numFunctions, numFunctions + batchSize - 1) =
        arma::ones(1, batchSize);
    discriminator.Gradient(discriminator.parameter, numFunctions,
        noiseGradientDiscriminator, batchSize);
    generator.error = boost::apply_visitor(deltaVisitor,
        discriminator.network[1]);

    generator.Predictors() = noise;
    generator.ResetGradients(gradientGenerator);
    generator.Gradient(generator.parameter, 0, gradientGenerator, batchSize);

    gradientGenerator *= multiplier;
  }

  counter++;
  currentBatch++;

  // Revert the counter to zero, if the total dataset get's covered.
  if (counter * batchSize >= numFunctions)
  {
    counter = 0;
  }

  if (preTrainSize > 0)
  {
    preTrainSize--;
  }

  return res;
}

template<
  typename Model,
  typename InitializationRuleType,
  typename Noise,
  typename PolicyType
>
template<typename Policy>
typename std::enable_if<std::is_same<Policy, StandardGAN>::value ||
                        std::is_same<Policy, DCGAN>::value, void>::type
GAN<Model, InitializationRuleType, Noise, PolicyType>::
Gradient(const arma::mat& parameters,
         const size_t i,
         arma::mat& gradient,
         const size_t batchSize)
{
  this->EvaluateWithGradient(parameters, i, gradient, batchSize);
}

template<
  typename Model,
  typename InitializationRuleType,
  typename Noise,
  typename PolicyType
>
void GAN<Model, InitializationRuleType, Noise, PolicyType>::Shuffle()
{
  const arma::uvec ordering = arma::shuffle(arma::linspace<arma::uvec>(0,
      numFunctions - 1, numFunctions));
  predictors.cols(0, numFunctions - 1) = predictors.cols(ordering);
}

template<
  typename Model,
  typename InitializationRuleType,
  typename Noise,
  typename PolicyType
>
void GAN<Model, InitializationRuleType, Noise, PolicyType>::Forward(
    arma::mat&& input)
{
  if (!reset)
    Reset();

  generator.Forward(std::move(input));
  ganOutput = boost::apply_visitor(
      outputParameterVisitor,
      generator.network.back());

  discriminator.Forward(std::move(ganOutput));
}

template<
  typename Model,
  typename InitializationRuleType,
  typename Noise,
  typename PolicyType
>
void GAN<Model, InitializationRuleType, Noise, PolicyType>::
Predict(arma::mat&& input, arma::mat& output)
{
  if (!reset)
    Reset();

  Forward(std::move(input));

  output = boost::apply_visitor(outputParameterVisitor,
      discriminator.network.back());
}

template<
  typename Model,
  typename InitializationRuleType,
  typename Noise,
  typename PolicyType
>
template<typename Archive>
void GAN<Model, InitializationRuleType, Noise, PolicyType>::
serialize(Archive& ar, const unsigned int /* version */)
{
  ar & BOOST_SERIALIZATION_NVP(parameter);
  ar & BOOST_SERIALIZATION_NVP(generator);
  ar & BOOST_SERIALIZATION_NVP(discriminator);
  ar & BOOST_SERIALIZATION_NVP(noiseFunction);
}

} // namespace ann
} // namespace mlpack
# endif


/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkBoostDividedEdgeBundling.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/*-------------------------------------------------------------------------
  Copyright 2008 Sandia Corporation.
  Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
  the U.S. Government retains certain rights in this software.
-------------------------------------------------------------------------*/
#include "vtkBoostDividedEdgeBundling.h"

#include "vtkBoostGraphAdapter.h"
#include "vtkDataSetAttributes.h"
#include "vtkDirectedGraph.h"
#include "vtkFloatArray.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkMath.h"
#include "vtkNew.h"
#include "vtkObjectFactory.h"
#include "vtkPoints.h"
#include "vtkVectorOperators.h"

#include <boost/property_map/property_map.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <algorithm>

vtkStandardNewMacro(vtkBoostDividedEdgeBundling);

vtkBoostDividedEdgeBundling::vtkBoostDividedEdgeBundling()
{
}

class vtkBundlingMetadata
{
public:
  vtkBundlingMetadata(vtkBoostDividedEdgeBundling *alg, vtkDirectedGraph *g)
    : Outer(alg), Graph(g)
  {
    this->Nodes = reinterpret_cast<vtkVector3f*>(
      vtkArrayDownCast<vtkFloatArray>(g->GetPoints()->GetData())->GetPointer(0));
    this->Edges.resize(g->GetNumberOfEdges());
    for (vtkIdType e = 0; e < g->GetNumberOfEdges(); ++e)
    {
      this->Edges[e] = std::make_pair(g->GetSourceVertex(e), g->GetTargetVertex(e));
    }
    this->VelocityDamping = 0.1f;
    this->EdgeCoulombConstant = 0.5f;
    //this->EdgeCoulombConstant = 50.0f;
    this->EdgeCoulombDecay = 35.0f;
    this->EdgeSpringConstant = 0.1f;
    //this->EdgeSpringConstant = 0.0005f;
    this->EdgeLaneWidth = 25.0f;
    this->UseNewForce = true;
  }

  void ProjectOnto(vtkIdType e1, vtkIdType e2, vtkVector3f& s, vtkVector3f& t);
  void NormalizeNodePositions();
  void DenormalizeNodePositions();
  void CalculateNodeDistances();
  float AngleCompatibility(vtkIdType e1, vtkIdType e2);
  float ScaleCompatibility(vtkIdType e1, vtkIdType e2);
  float PositionCompatibility(vtkIdType e1, vtkIdType e2);
  float VisibilityCompatibility(vtkIdType e1, vtkIdType e2);
  float ConnectivityCompatibility(vtkIdType e1, vtkIdType e2);
  void CalculateEdgeLengths();
  void CalculateEdgeCompatibilities();
  void InitializeEdgeMesh();
  void DoubleEdgeMeshResolution();
  void SimulateEdgeStep();
  void LayoutEdgePoints();
  void SmoothEdges();

  float SimulationStep;
  int CycleIterations;
  int MeshCount;
  float VelocityDamping;
  float EdgeCoulombConstant;
  float EdgeCoulombDecay;
  float EdgeSpringConstant;
  float EdgeLaneWidth;
  bool UseNewForce;
  vtkBoostDividedEdgeBundling *Outer;
  vtkDirectedGraph *Graph;
  vtkVector3f *Nodes;
  std::vector<std::pair<vtkIdType, vtkIdType> > Edges;
  std::vector<std::vector<float> > NodeDistances;
  std::vector<float> EdgeLengths;
  std::vector<std::vector<float> > EdgeCompatibilities;
  std::vector<std::vector<float> > EdgeDots;
  std::vector<std::vector<vtkVector3f> > EdgeMesh;
  std::vector<std::vector<vtkVector3f> > EdgeMeshVelocities;
  std::vector<std::vector<vtkVector3f> > EdgeMeshAccelerations;
  //std::vector<std::vector<float> > EdgeMeshGroupCounts;
  vtkVector2f XRange;
  vtkVector2f YRange;
  vtkVector2f ZRange;
  float Scale;
};

void vtkBundlingMetadata::NormalizeNodePositions()
{
  this->XRange = vtkVector2f(VTK_FLOAT_MAX, VTK_FLOAT_MIN);
  this->YRange = vtkVector2f(VTK_FLOAT_MAX, VTK_FLOAT_MIN);
  this->ZRange = vtkVector2f(VTK_FLOAT_MAX, VTK_FLOAT_MIN);
  for (vtkIdType i = 0; i < this->Graph->GetNumberOfVertices(); ++i)
  {
    vtkVector3f p = this->Nodes[i];
    this->XRange[0] = std::min(this->XRange[0], p[0]);
    this->XRange[1] = std::max(this->XRange[1], p[0]);
    this->YRange[0] = std::min(this->YRange[0], p[1]);
    this->YRange[1] = std::max(this->YRange[1], p[1]);
    this->ZRange[0] = std::min(this->ZRange[0], p[2]);
    this->ZRange[1] = std::max(this->ZRange[1], p[2]);
  }
  float dx = this->XRange[1] - this->XRange[0];
  float dy = this->YRange[1] - this->YRange[0];
  float dz = this->ZRange[1] - this->ZRange[0];
  this->Scale = std::max(dx, std::max(dy, dz));
  for (vtkIdType i = 0; i < this->Graph->GetNumberOfVertices(); ++i)
  {
    vtkVector3f p = this->Nodes[i];
    this->Nodes[i] = vtkVector3f(
      (p[0] - this->XRange[0])/this->Scale * 1000.0f,
      (p[1] - this->YRange[0])/this->Scale * 1000.0f,
      (p[2] - this->ZRange[0])/this->Scale * 1000.0f);
  }
}

void vtkBundlingMetadata::DenormalizeNodePositions()
{
  for (vtkIdType i = 0; i < this->Graph->GetNumberOfVertices(); ++i)
  {
    vtkVector3f p = this->Nodes[i];
    this->Nodes[i] = vtkVector3f(
      p[0] / 1000.0f * this->Scale + this->XRange[0],
      p[1] / 1000.0f * this->Scale + this->YRange[0],
      p[2] / 1000.0f * this->Scale + this->ZRange[0]);
  }
  for (vtkIdType i = 0; i < (int)this->EdgeMesh.size(); ++i)
  {
    for (vtkIdType j = 0; j < (int)this->EdgeMesh[i].size(); ++j)
    {
      vtkVector3f p = this->EdgeMesh[i][j];
      this->EdgeMesh[i][j] = vtkVector3f(
        p[0] / 1000.0f * this->Scale + this->XRange[0],
        p[1] / 1000.0f * this->Scale + this->YRange[0],
        p[2] / 1000.0f * this->Scale + this->ZRange[0]);
    }
  }
}

void vtkBundlingMetadata::CalculateNodeDistances()
{
  vtkIdType numVerts = this->Graph->GetNumberOfVertices();
  vtkIdType numEdges = this->Graph->GetNumberOfEdges();
  this->NodeDistances.resize(numVerts, std::vector<float>(numVerts, VTK_FLOAT_MAX));
  std::vector<float> weights(numVerts, 1.0f);
  vtkNew<vtkFloatArray> weightMap;
  weightMap->SetNumberOfTuples(numEdges);
  for (vtkIdType e = 0; e < numEdges; ++e)
  {
    weightMap->SetValue(e, 1.0f);
  }
  boost::vtkGraphEdgePropertyMapHelper<vtkFloatArray*> weightProp(weightMap);
  boost::johnson_all_pairs_shortest_paths(
    this->Graph, this->NodeDistances,
    boost::weight_map(weightProp));
}

float vtkBundlingMetadata::AngleCompatibility(vtkIdType e1, vtkIdType e2)
{
  if (this->EdgeLengths[e1] == 0.0f || this->EdgeLengths[e2] == 0.0f)
  {
    return 0.0f;
  }
  vtkVector3f s1 = this->Nodes[this->Edges[e1].first];
  vtkVector3f t1 = this->Nodes[this->Edges[e1].second];
  vtkVector3f s2 = this->Nodes[this->Edges[e2].first];
  vtkVector3f t2 = this->Nodes[this->Edges[e2].second];
  vtkVector3f p1 = s1 - t1;
  vtkVector3f p2 = s2 - t2;
  float compatibility = p1.Dot(p2) / (this->EdgeLengths[e1]*this->EdgeLengths[e2]);
  return fabs(compatibility);
}

float vtkBundlingMetadata::ScaleCompatibility(vtkIdType e1, vtkIdType e2)
{
  float len1 = this->EdgeLengths[e1];
  float len2 = this->EdgeLengths[e2];
  float average = (len1 + len2) / 2.0f;
  if (average == 0.0f)
  {
    return 0.0f;
  }
  return 2.0f / (average / std::min(len1, len2) + std::max(len1, len2) / average);
}

float vtkBundlingMetadata::PositionCompatibility(vtkIdType e1, vtkIdType e2)
{
  float len1 = this->EdgeLengths[e1];
  float len2 = this->EdgeLengths[e2];
  float average = (len1 + len2) / 2.0f;
  if (average == 0.0f)
  {
    return 0.0f;
  }
  vtkVector3f s1 = this->Nodes[this->Edges[e1].first];
  vtkVector3f t1 = this->Nodes[this->Edges[e1].second];
  vtkVector3f s2 = this->Nodes[this->Edges[e2].first];
  vtkVector3f t2 = this->Nodes[this->Edges[e2].second];
  vtkVector3f mid1 = 0.5*(s1 + t1);
  vtkVector3f mid2 = 0.5*(s2 + t2);
  return average / (average + (mid1 - mid2).Norm());
}

void vtkBundlingMetadata::ProjectOnto(vtkIdType e1, vtkIdType e2, vtkVector3f& s, vtkVector3f& t)
{
  vtkVector3f s1 = this->Nodes[this->Edges[e1].first];
  vtkVector3f t1 = this->Nodes[this->Edges[e1].second];
  vtkVector3f s2 = this->Nodes[this->Edges[e2].first];
  vtkVector3f t2 = this->Nodes[this->Edges[e2].second];
  vtkVector3f norm = t2 - s2;
  norm.Normalize();
  vtkVector3f toHead = s1 - s2;
  vtkVector3f toTail = t1 - s2;
  vtkVector3f headOnOther = norm * norm.Dot(toHead);
  vtkVector3f tailOnOther = norm * norm.Dot(toTail);
  s = s2 + headOnOther;
  t = s2 + tailOnOther;
}

float vtkBundlingMetadata::VisibilityCompatibility(vtkIdType e1, vtkIdType e2)
{
  vtkVector3f is;
  vtkVector3f it;
  vtkVector3f js;
  vtkVector3f jt;
  this->ProjectOnto(e1, e2, is, it);
  this->ProjectOnto(e2, e1, js, jt);
  float ilen = (is - it).Norm();
  float jlen = (js - jt).Norm();
  if (ilen == 0.0f || jlen == 0.0f)
  {
    return 0.0f;
  }
  vtkVector3f s1 = this->Nodes[this->Edges[e1].first];
  vtkVector3f t1 = this->Nodes[this->Edges[e1].second];
  vtkVector3f s2 = this->Nodes[this->Edges[e2].first];
  vtkVector3f t2 = this->Nodes[this->Edges[e2].second];
  vtkVector3f mid1 = 0.5*(s1 + t1);
  vtkVector3f mid2 = 0.5*(s2 + t2);
  vtkVector3f imid = 0.5*(is + it);
  vtkVector3f jmid = 0.5*(js + jt);
  float midQI = (mid2 - imid).Norm();
  float vpq = std::max(0.0f, 1.0f - (2.0f * midQI) / ilen);
  float midPJ = (mid1 - jmid).Norm();
  float vqp = std::max(0.0f, 1.0f - (2.0f * midPJ) / jlen);

  return std::min(vpq, vqp);
}

float vtkBundlingMetadata::ConnectivityCompatibility(vtkIdType e1, vtkIdType e2)
{
  vtkIdType s1 = this->Edges[e1].first;
  vtkIdType t1 = this->Edges[e1].second;
  vtkIdType s2 = this->Edges[e2].first;
  vtkIdType t2 = this->Edges[e2].second;
  if (s1 == s2 || s1 == t2 || t1 == s2 || t1 == t2)
  {
    return 1.0f;
  }
  float minPath = std::min(this->NodeDistances[s1][s2], std::min(this->NodeDistances[s1][t2],
    std::min(this->NodeDistances[t1][s2], this->NodeDistances[t1][t2])));
  return 1.0f / (minPath + 1.0f);
}

void vtkBundlingMetadata::CalculateEdgeLengths()
{
  vtkIdType numEdges = this->Graph->GetNumberOfEdges();
  this->EdgeLengths.resize(numEdges);
  for (vtkIdType e = 0; e < numEdges; ++e)
  {
    vtkVector3f s = this->Nodes[this->Edges[e].first];
    vtkVector3f t = this->Nodes[this->Edges[e].second];
    this->EdgeLengths[e] = (s - t).Norm();
  }
}

void vtkBundlingMetadata::CalculateEdgeCompatibilities()
{
  vtkIdType numEdges = this->Graph->GetNumberOfEdges();
  this->EdgeCompatibilities.resize(numEdges, std::vector<float>(numEdges, 1.0f));
  this->EdgeDots.resize(numEdges, std::vector<float>(numEdges, 1.0f));
  for (vtkIdType e1 = 0; e1 < numEdges; ++e1)
  {
    vtkVector3f s1 = this->Nodes[this->Edges[e1].first];
    vtkVector3f t1 = this->Nodes[this->Edges[e1].second];
    vtkVector3f r1 = s1 - t1;
    r1.Normalize();
    for (vtkIdType e2 = e1 + 1; e2 < numEdges; ++e2)
    {
      float compatibility = 1.0f;
      compatibility *= this->AngleCompatibility(e1, e2);
      compatibility *= this->ScaleCompatibility(e1, e2);
      compatibility *= this->PositionCompatibility(e1, e2);
      compatibility *= this->VisibilityCompatibility(e1, e2);
      compatibility *= this->ConnectivityCompatibility(e1, e2);
      this->EdgeCompatibilities[e1][e2] = compatibility;
      this->EdgeCompatibilities[e2][e1] = compatibility;

      vtkVector3f s2 = this->Nodes[this->Edges[e2].first];
      vtkVector3f t2 = this->Nodes[this->Edges[e2].second];
      vtkVector3f r2 = s2 - t2;
      r2.Normalize();
      float dot = r1.Dot(r2);
      this->EdgeDots[e1][e2] = dot;
      this->EdgeDots[e2][e1] = dot;
    }
  }
}

void vtkBundlingMetadata::InitializeEdgeMesh()
{
  this->MeshCount = 2;
  vtkIdType numEdges = this->Graph->GetNumberOfEdges();
  this->EdgeMesh.resize(numEdges, std::vector<vtkVector3f>(2));
  this->EdgeMeshVelocities.resize(numEdges, std::vector<vtkVector3f>(2));
  this->EdgeMeshAccelerations.resize(numEdges, std::vector<vtkVector3f>(2));
  //this->EdgeMeshGroupCounts.resize(numEdges, std::vector<float>(2, 1.0f));
  for (vtkIdType e = 0; e < numEdges; ++e)
  {
    this->EdgeMesh[e][0] = this->Nodes[this->Edges[e].first];
    this->EdgeMesh[e][1] = this->Nodes[this->Edges[e].second];
  }
}

void vtkBundlingMetadata::DoubleEdgeMeshResolution()
{
  int newMeshCount = (this->MeshCount - 1)*2 + 1;
  vtkIdType numEdges = this->Graph->GetNumberOfEdges();
  std::vector<std::vector<vtkVector3f> > newEdgeMesh(
      numEdges, std::vector<vtkVector3f>(newMeshCount));
  std::vector<std::vector<vtkVector3f> > newEdgeMeshVelocities(
      numEdges, std::vector<vtkVector3f>(newMeshCount, vtkVector3f(0.0f, 0.0f, 0.0f)));
  std::vector<std::vector<vtkVector3f> > newEdgeMeshAccelerations(
      numEdges, std::vector<vtkVector3f>(newMeshCount, vtkVector3f(0.0f, 0.0f, 0.0f)));
  //std::vector<std::vector<float> > newEdgeMeshGroupCounts(
  //    numEdges, std::vector<float>(newMeshCount, 1.0f));
  for (vtkIdType e = 0; e < numEdges; ++e)
  {
    for (int m = 0; m < newMeshCount; ++m)
    {
      float indexFloat = (this->MeshCount - 1.0f)*m/(newMeshCount - 1.0f);
      int index = static_cast<int>(indexFloat);
      float alpha = indexFloat - index;
      vtkVector3f before = this->EdgeMesh[e][index];
      if (alpha > 0)
      {
        vtkVector3f after = this->EdgeMesh[e][index+1];
        newEdgeMesh[e][m] = before + alpha*(after - before);
      }
      else
      {
        newEdgeMesh[e][m] = before;
      }
    }
  }
  this->MeshCount = newMeshCount;
  this->EdgeMesh = newEdgeMesh;
  this->EdgeMeshVelocities = newEdgeMeshVelocities;
  this->EdgeMeshAccelerations = newEdgeMeshAccelerations;
  //this->EdgeMeshGroupCounts = newEdgeMeshGroupCounts;
}

void vtkBundlingMetadata::SimulateEdgeStep()
{
  vtkIdType numEdges = this->Graph->GetNumberOfEdges();

  for (vtkIdType e1 = 0; e1 < numEdges; ++e1)
  {
    float weight1 = 1.0f;
    for (int m1 = 0; m1 < this->MeshCount; ++m1)
    {
      // Immovable
      if (m1 <= 0 || m1 >= this->MeshCount - 1)
      {
        continue;
      }

      // Move the point according to dynamics
      vtkVector3f position = this->EdgeMesh[e1][m1];
      vtkVector3f velocity = this->EdgeMeshVelocities[e1][m1];
      vtkVector3f acceleration = this->EdgeMeshAccelerations[e1][m1];
      velocity = velocity + acceleration * this->SimulationStep * 0.5f;
      velocity = velocity * this->VelocityDamping;
      position = position + velocity * this->SimulationStep;
      this->EdgeMesh[e1][m1] = position;

      acceleration = vtkVector3f(0.0f, 0.0f, 0.0f);

      // Spring force
      vtkVector3f prevPosition = this->EdgeMesh[e1][m1-1];
      vtkVector3f prevDirection = prevPosition - position;
      float prevDist = prevDirection.Norm();
      float prevForce = this->EdgeSpringConstant / 1000.0f * (this->MeshCount - 1) * prevDist * weight1;
      prevDirection.Normalize();
      acceleration = acceleration + prevForce * prevDirection;

      vtkVector3f nextPosition = this->EdgeMesh[e1][m1+1];
      vtkVector3f nextDirection = nextPosition - position;
      float nextDist = nextDirection.Norm();
      float nextForce = this->EdgeSpringConstant / 1000.0f * (this->MeshCount - 1) * nextDist * weight1;
      nextDirection.Normalize();
      acceleration = acceleration + nextForce * nextDirection;

      // Coulomb force
      float normalizedEdgeCoulombConstant = this->EdgeCoulombConstant / sqrt(static_cast<float>(numEdges));

      for (vtkIdType e2 = 0; e2 < numEdges; ++e2)
      {
        if (e1 == e2)
        {
          continue;
        }

        float compatibility = this->EdgeCompatibilities[e1][e2];
        if (compatibility <= 0.05)
        {
          continue;
        }

        float dot = this->EdgeDots[e1][e2];
        float weight2 = 1.0f;

        int m2;
        if (dot >= 0.0f)
        {
          m2 = m1;
        }
        else
        {
          m2 = this->MeshCount - 1 - m1;
        }

        vtkVector3f position2;
        // If we're going the same direction is edge1, then the potential minimum is at the point.
        if (dot >= 0.0f)
        {
          position2 = this->EdgeMesh[e2][m2];
        }
        // If we're going the opposite direction, the potential minimum is edgeLaneWidth to the "right."
        else
        {
          vtkVector3f tangent = this->EdgeMesh[e2][m2+1] - this->EdgeMesh[e2][m2-1];
          tangent.Normalize();
          // This assumes 2D
          vtkVector3f normal(-tangent[1], tangent[0], 0.0f);
          position2 = this->EdgeMesh[e2][m2] + normal*this->EdgeLaneWidth;
        }

        vtkVector3f direction = position2 - position;
        float distance = direction.Norm();

        // Inverse force.
        float force;
        if (!this->UseNewForce)
        {
          force = normalizedEdgeCoulombConstant * 30.0f / (this->MeshCount - 1) / (distance + 0.01f);
        }
        // New force.
        else
        {
          force = 4.0f * 10000.0f / (this->MeshCount - 1) * this->EdgeCoulombDecay * normalizedEdgeCoulombConstant * distance / (3.1415926f * pow(this->EdgeCoulombDecay * this->EdgeCoulombDecay + distance * distance, 2));
        }
        force *= weight2;
        force *= compatibility;

        if (distance > 0.0f)
        {
          direction.Normalize();
          acceleration = acceleration + force * direction;
        }
      }

      velocity = velocity + acceleration * this->SimulationStep * 0.5f;
      this->EdgeMeshVelocities[e1][m1] = velocity;
      this->EdgeMeshAccelerations[e1][m1] = acceleration;
    }
  }
}

void vtkBundlingMetadata::SmoothEdges()
{
  // From Mathematica Total[GaussianMatrix[{3, 3}]]
  int kernelSize = 3;
  // Has to sum to 1.0 to be correct.
  float gaussianKernel[] = {0.10468, 0.139936, 0.166874, 0.177019, 0.166874, 0.139936, 0.10468};
  vtkIdType numEdges = this->Graph->GetNumberOfEdges();
  std::vector<std::vector<vtkVector3f> > smoothedEdgeMesh(
      numEdges, std::vector<vtkVector3f>(this->MeshCount));
  for (vtkIdType e = 0; e < numEdges; ++e)
  {
    for (int m = 1; m < this->MeshCount - 1; ++m)
    {
      vtkVector3f smoothed(0.0f, 0.0f, 0.0f);
      for (int kernelIndex = 0; kernelIndex < kernelSize * 2 + 1; kernelIndex++)
      {
        int m2 = m + kernelIndex - kernelSize;
        m2 = std::max(0, std::min(this->MeshCount - 1, m2));

        vtkVector3f pt = this->EdgeMesh[e][m2];
        smoothed = smoothed + gaussianKernel[kernelIndex] * pt;
      }
      smoothedEdgeMesh[e][m] = smoothed;
    }
  }
  this->EdgeMesh = smoothedEdgeMesh;
}

void vtkBundlingMetadata::LayoutEdgePoints()
{
  this->InitializeEdgeMesh();
  this->SimulationStep = 40.0f;
  this->CycleIterations = 30;
  for (int i = 0; i < 5; ++i)
  {
    vtkDebugWithObjectMacro(this->Outer, "vtkBoostDividedEdgeBundling cycle " << i);
    this->CycleIterations = this->CycleIterations * 2 / 3;
    this->SimulationStep = 0.85f*this->SimulationStep;
    this->DoubleEdgeMeshResolution();
    for (int j = 0; j < this->CycleIterations; ++j)
    {
      vtkDebugWithObjectMacro(this->Outer, "vtkBoostDividedEdgeBundling iteration " << j);
      this->SimulateEdgeStep();
    }
  }
  this->SmoothEdges();
}

int vtkBoostDividedEdgeBundling::RequestData(
  vtkInformation *vtkNotUsed(request),
  vtkInformationVector **inputVector,
  vtkInformationVector *outputVector)
{
  // get the info objects
  vtkInformation *graphInfo = inputVector[0]->GetInformationObject(0);
  vtkInformation *outInfo = outputVector->GetInformationObject(0);

  // get the input and output
  vtkDirectedGraph *g = vtkDirectedGraph::SafeDownCast(
    graphInfo->Get(vtkDataObject::DATA_OBJECT()));
  vtkDirectedGraph *output = vtkDirectedGraph::SafeDownCast(
    outInfo->Get(vtkDataObject::DATA_OBJECT()));

  vtkBundlingMetadata *meta = new vtkBundlingMetadata(this, g);

  meta->NormalizeNodePositions();
  meta->CalculateEdgeLengths();
  meta->CalculateNodeDistances();
  meta->CalculateEdgeCompatibilities();
  meta->LayoutEdgePoints();
  meta->DenormalizeNodePositions();

  output->ShallowCopy(g);

  for (vtkIdType e = 0; e < g->GetNumberOfEdges(); ++e)
  {
    output->ClearEdgePoints(e);
    for (int m = 1; m < meta->MeshCount-1; ++m)
    {
      vtkVector3f edgePoint = meta->EdgeMesh[e][m];
      output->AddEdgePoint(e, edgePoint[0], edgePoint[1], edgePoint[2]);
    }
  }

  delete meta;

  return 1;
}

void vtkBoostDividedEdgeBundling::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os,indent);
}


// Andrew Naplavkov

#ifndef BARK_GEOMETRY_GEOMETRY_HPP
#define BARK_GEOMETRY_GEOMETRY_HPP

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/geometry/multi/geometries/multi_linestring.hpp>
#include <boost/geometry/multi/geometries/multi_point.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>
#include <boost/variant.hpp>
#include <vector>

namespace bark::geometry {

using point = boost::geometry::model::d2::point_xy<double>;
using linestring = boost::geometry::model::linestring<point>;
using polygon = boost::geometry::model::polygon<point, false, true>;
using multi_point = boost::geometry::model::multi_point<point>;
using multi_linestring = boost::geometry::model::multi_linestring<linestring>;
using multi_polygon = boost::geometry::model::multi_polygon<polygon>;
struct geometry_collection;

using geometry = boost::variant<point,
                                linestring,
                                polygon,
                                multi_point,
                                multi_linestring,
                                multi_polygon,
                                boost::recursive_wrapper<geometry_collection>>;

struct geometry_collection : std::vector<geometry> {
};

using box = boost::geometry::model::box<point>;

using multi_box = std::vector<box>;

using box_rtree =
    boost::geometry::index::rtree<box, boost::geometry::index::quadratic<16>>;

}  // namespace bark::geometry

#endif  // BARK_GEOMETRY_GEOMETRY_HPP