#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include "sophus/so3.hpp"
#include "sophus/se3.hpp"
#include <chrono>

using namespace std;
using namespace cv;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

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

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

void bundleAdjustment (
    const vector<Point3f> points_3d,
    const vector<Point2f> points_2d,
    const Mat& K,
    Mat& R, Mat& t
);

void myBundleAdjustment (
    const vector<Point3f> points_3d,
    const vector<Point2f> points_2d[],
    const Mat& K,
    Mat& R, Mat& t
);

int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 读取图像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

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

    // 建立3D点
    Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d[2];
    for ( DMatch m:matches )
    {
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        if ( d == 0 )   // bad depth
            continue;
        float dd = d/5000.0;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
        pts_2d[0].push_back ( keypoints_1[m.queryIdx].pt );
        pts_2d[1].push_back ( keypoints_2[m.trainIdx].pt );
    }

    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;

    Mat r, t;
    solvePnP ( pts_3d, pts_2d[1], K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    Mat R;
    cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵

    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;

    cout<<"calling bundle adjustment"<<endl;

    ::myBundleAdjustment ( pts_3d, pts_2d, K, R, t );
}

void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

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

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, match );

    //-- 第四步:匹配点对筛选
    double min_dist=10000, max_dist=0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = match[i].distance;
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_dist ) max_dist = dist;
    }

    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

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

Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
           (
               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
}

class VertexCamera : public g2o::BaseVertex<6, Sophus::SE3d>
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() // 重置
    {
        _estimate = Sophus::SE3d();
    }

    virtual void oplusImpl( const double* update ) // 更新
    {
        Eigen::Map<const Vector6d> update_vec(update);
        setEstimate(Sophus::SE3d::exp(update_vec) * _estimate);
    }
    // 存盘和读盘：留空
    virtual bool read( istream& in ) {
        return false;
    }
    virtual bool write( ostream& out ) const {
        return false;
    }
};

class VertexPointXYZ : public g2o::BaseVertex<3, Eigen::Vector3d>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual bool read( istream& in ) {
        return false;
    }
    virtual bool write( ostream& out ) const {
        return false;
    }

    virtual void setToOriginImpl() {
      _estimate.fill(0);
    }

    virtual void oplusImpl(const double* update)
    {
      Eigen::Map<const Eigen::Vector3d> v(update);
      _estimate += v;
    }
};

class EdgeProject : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, VertexPointXYZ, VertexCamera>{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeProject() {
        _cam = nullptr;
        resizeParameters(1);
        installParameter(_cam, 0);
    }

    virtual bool read( istream& in ) {
        return false;
    }
    virtual bool write( ostream& out ) const {
        return false;
    }

    void computeError()  {
      const VertexCamera* v1 = static_cast<const VertexCamera*>(_vertices[1]);
      const VertexPointXYZ* v2 = static_cast<const VertexPointXYZ*>(_vertices[0]);
      const g2o::CameraParameters * cam
        = static_cast<const g2o::CameraParameters *>(_cam);
      _error = _measurement - cam->cam_map(v1->estimate() * (v2->estimate()));

    }

    g2o::CameraParameters * _cam;
};

// reference: T=
//     0.99778  -0.0522171   0.0413239   -0.129224
//    0.051047    0.998279   0.0288811 -0.00970178
//  -0.0427608  -0.0267076    0.998728   0.0601133
//           0           0           0           1

void myBundleAdjustment (
    const vector< Point3f > points_3d,
    const vector< Point2f > points_2d[],
    const Mat& K,
    Mat& R, Mat& t )
{
// 初始化g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    std::unique_ptr<Block::LinearSolverType> linearSolver = g2o::make_unique<g2o::LinearSolverCSparse<Block::PoseMatrixType>>(); // 线性方程求解器
    std::unique_ptr<Block> solver_ptr = g2o::make_unique<Block> ( move(linearSolver) );     // 矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( move(solver_ptr) );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

    // vertex
    VertexCamera* pose[2]; // camera pose
    pose[0] = new VertexCamera();
    pose[0]->setId(0);
    pose[0]->setEstimate(Sophus::SE3d());
    optimizer.addVertex(pose[0]);

    Eigen::Matrix3d R_mat;
    R_mat <<
          R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
               R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
               R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
    pose[1] = new VertexCamera();
    pose[1]->setId ( 1 );
    pose[1]->setEstimate ( Sophus::SE3d (
                            R_mat,
                            Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
                        ) );
    optimizer.addVertex ( pose[1] );

    int index = 2;
    for ( const Point3f p:points_3d )   // landmarks
    {
        VertexPointXYZ* point = new VertexPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
        optimizer.addVertex ( point );
    }

    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters (
        K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
    );
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // edges
    index = 2;
    assert(points_2d[0].size() == points_2d[1].size());
    for (size_t i = 0; i < points_2d[0].size(); i++)
    {
        for (size_t j = 0; j < 2; j++) {
            EdgeProject* edge = new EdgeProject();
            edge->setId ( index );
            // cout << index << endl;
            edge->setVertex ( 0, dynamic_cast<VertexPointXYZ*> ( optimizer.vertex ( index / 2 + 1 ) ) );
            edge->setVertex ( 1, pose[j] );
            edge->setMeasurement ( Eigen::Vector2d ( points_2d[j][i].x, points_2d[j][i].y ) );
            edge->setParameterId ( 0,0 );
            edge->setInformation ( Eigen::Matrix2d::Identity() );
            optimizer.addEdge ( edge );
            index++;
        }
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose ( true );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
    cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;

    cout<<endl<<"after optimization:"<<endl;
    cout<<"T1="<<endl<<pose[0]->estimate().matrix() <<endl;
    cout<<"T2="<<endl<<pose[1]->estimate().matrix() <<endl;
    cout<<"relative T="<<endl<<(pose[0]->estimate().inverse() * pose[1]->estimate()).matrix() <<endl;
}

void bundleAdjustment (
    const vector< Point3f > points_3d,
    const vector< Point2f > points_2d,
    const Mat& K,
    Mat& R, Mat& t )
{
    // 初始化g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    std::unique_ptr<Block::LinearSolverType> linearSolver = g2o::make_unique<g2o::LinearSolverCSparse<Block::PoseMatrixType>>(); // 线性方程求解器
    std::unique_ptr<Block> solver_ptr = g2o::make_unique<Block> ( move(linearSolver) );     // 矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( move(solver_ptr) );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    Eigen::Matrix3d R_mat;
    R_mat <<
          R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
               R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
               R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
                            R_mat,
                            Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
                        ) );
    optimizer.addVertex ( pose );

    int index = 1;
    for ( const Point3f p:points_3d )   // landmarks
    {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
        optimizer.addVertex ( point );
    }

    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters (
        K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
    );
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // edges
    index = 1;
    for ( const Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId ( index );
        edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
        edge->setVertex ( 1, pose );
        edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
        edge->setParameterId ( 0,0 );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        index++;
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose ( true );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
    cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;

    cout<<endl<<"after optimization:"<<endl;
    cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <iterator>
#include <vector>
#include <list>
// Use boost::queue instead of std::queue because std::queue doesn't
// model Buffer; it has to top() function. -Jeremy
#include <boost/pending/queue.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_utility.hpp>

using namespace std;
using namespace boost;
/*
  This example does a best-first-search (using dijkstra's) and
  simultaneously makes a copy of the graph (assuming the graph is
  connected).

  Example Graph: (p. 90 "Data Structures and Network Algorithms", Tarjan)

              g
            3+ +2
            / 1 \
           e+----f
           |+0 5++
           | \ / |
         10|  d  |12
           |8++\7|
           +/ | +|
           b 4|  c
            \ | +
            6+|/3
              a

  Sample Output:
a --> c d
b --> a d
c --> f
d --> c e f
e --> b g
f --> e g
g -->
Starting graph:
a(32767); c d
c(32767); f
d(32767); c e f
f(32767); e g
e(32767); b g
g(32767);
b(32767); a d
Result:
a(0); d c
d(4); f e c
c(3); f
f(9); g e
e(4); g b
g(7);
b(14); d a

*/

typedef property< vertex_color_t, default_color_type,
    property< vertex_distance_t, int > >
    VProperty;
typedef int weight_t;
typedef property< edge_weight_t, weight_t > EProperty;

typedef adjacency_list< vecS, vecS, directedS, VProperty, EProperty > Graph;

template < class Tag >
struct endl_printer : public boost::base_visitor< endl_printer< Tag > >
{
    typedef Tag event_filter;
    endl_printer(std::ostream& os) : m_os(os) {}
    template < class T, class Graph > void operator()(T, Graph&)
    {
        m_os << std::endl;
    }
    std::ostream& m_os;
};
template < class Tag > endl_printer< Tag > print_endl(std::ostream& os, Tag)
{
    return endl_printer< Tag >(os);
}

template < class PA, class Tag >
struct edge_printer : public boost::base_visitor< edge_printer< PA, Tag > >
{
    typedef Tag event_filter;

    edge_printer(PA pa, std::ostream& os) : m_pa(pa), m_os(os) {}

    template < class T, class Graph > void operator()(T x, Graph& g)
    {
        m_os << "(" << get(m_pa, source(x, g)) << "," << get(m_pa, target(x, g))
             << ") ";
    }
    PA m_pa;
    std::ostream& m_os;
};
template < class PA, class Tag >
edge_printer< PA, Tag > print_edge(PA pa, std::ostream& os, Tag)
{
    return edge_printer< PA, Tag >(pa, os);
}

template < class NewGraph, class Tag >
struct graph_copier
: public boost::base_visitor< graph_copier< NewGraph, Tag > >
{
    typedef Tag event_filter;

    graph_copier(NewGraph& graph) : new_g(graph) {}

    template < class Edge, class Graph > void operator()(Edge e, Graph& g)
    {
        add_edge(source(e, g), target(e, g), new_g);
    }

private:
    NewGraph& new_g;
};
template < class NewGraph, class Tag >
inline graph_copier< NewGraph, Tag > copy_graph(NewGraph& g, Tag)
{
    return graph_copier< NewGraph, Tag >(g);
}

template < class Graph, class Name > void print(Graph& G, Name name)
{
    typename boost::graph_traits< Graph >::vertex_iterator ui, uiend;
    for (boost::tie(ui, uiend) = vertices(G); ui != uiend; ++ui)
    {
        cout << name[*ui] << " --> ";
        typename boost::graph_traits< Graph >::adjacency_iterator vi, viend;
        for (boost::tie(vi, viend) = adjacent_vertices(*ui, G); vi != viend;
             ++vi)
            cout << name[*vi] << " ";
        cout << endl;
    }
}

int main(int, char*[])
{
    // Name and ID numbers for the vertices
    char name[] = "abcdefg";
    enum
    {
        a,
        b,
        c,
        d,
        e,
        f,
        g,
        N
    };

    Graph G(N);
    boost::property_map< Graph, vertex_index_t >::type vertex_id
        = get(vertex_index, G);

    std::vector< weight_t > distance(N, (numeric_limits< weight_t >::max)());
    typedef boost::graph_traits< Graph >::vertex_descriptor Vertex;
    std::vector< Vertex > parent(N);

    typedef std::pair< int, int > E;

    E edges[] = { E(a, c), E(a, d), E(b, a), E(b, d), E(c, f), E(d, c), E(d, e),
        E(d, f), E(e, b), E(e, g), E(f, e), E(f, g) };

    int weight[] = { 3, 4, 6, 8, 12, 7, 0, 5, 10, 3, 1, 2 };

    for (int i = 0; i < 12; ++i)
        add_edge(edges[i].first, edges[i].second, weight[i], G);

    print(G, name);

    adjacency_list< listS, vecS, directedS,
        property< vertex_color_t, default_color_type > >
        G_copy(N);

    cout << "Starting graph:" << endl;

    std::ostream_iterator< int > cout_int(std::cout, " ");
    std::ostream_iterator< char > cout_char(std::cout, " ");

    boost::queue< Vertex > Q;
    boost::breadth_first_search(G, vertex(a, G), Q,
        make_bfs_visitor(boost::make_list(
            write_property(make_iterator_property_map(name, vertex_id, name[0]),
                cout_char, on_examine_vertex()),
            write_property(make_iterator_property_map(
                               distance.begin(), vertex_id, distance[0]),
                cout_int, on_examine_vertex()),
            print_edge(make_iterator_property_map(name, vertex_id, name[0]),
                std::cout, on_examine_edge()),
            print_endl(std::cout, on_finish_vertex()))),
        get(vertex_color, G));

    std::cout << "about to call dijkstra's" << std::endl;

    parent[vertex(a, G)] = vertex(a, G);
    boost::dijkstra_shortest_paths(G, vertex(a, G),
        distance_map(make_iterator_property_map(
                         distance.begin(), vertex_id, distance[0]))
            .predecessor_map(make_iterator_property_map(
                parent.begin(), vertex_id, parent[0]))
            .visitor(
                make_dijkstra_visitor(copy_graph(G_copy, on_examine_edge()))));

    cout << endl;
    cout << "Result:" << endl;
    boost::breadth_first_search(G, vertex(a, G),
        visitor(make_bfs_visitor(boost::make_list(
            write_property(make_iterator_property_map(name, vertex_id, name[0]),
                cout_char, on_examine_vertex()),
            write_property(make_iterator_property_map(
                               distance.begin(), vertex_id, distance[0]),
                cout_int, on_examine_vertex()),
            print_edge(make_iterator_property_map(name, vertex_id, name[0]),
                std::cout, on_examine_edge()),
            print_endl(std::cout, on_finish_vertex())))));

    return 0;
}


#ifndef STAN_MATH_FWD_MAT_FUN_TRACE_QUAD_FORM_HPP
#define STAN_MATH_FWD_MAT_FUN_TRACE_QUAD_FORM_HPP

#include <boost/type_traits.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <stan/math/fwd/mat/fun/multiply.hpp>
#include <stan/math/prim/mat/fun/multiply.hpp>
#include <stan/math/prim/mat/fun/transpose.hpp>
#include <stan/math/prim/mat/fun/trace.hpp>
#include <stan/math/fwd/core.hpp>

namespace stan {
  namespace math {

    template<int RA, int CA, int RB, int CB, typename T>
    inline fvar<T>
    trace_quad_form(const Eigen::Matrix<fvar<T>, RA, CA> &A,
                    const Eigen::Matrix<fvar<T>, RB, CB> &B) {
      check_square("trace_quad_form", "A", A);
      check_multiplicable("trace_quad_form",
                          "A", A,
                          "B", B);
      return trace(multiply(transpose(B),
                            multiply(A, B)));
    }

    template<int RA, int CA, int RB, int CB, typename T>
    inline fvar<T>
    trace_quad_form(const Eigen::Matrix<fvar<T>, RA, CA> &A,
                    const Eigen::Matrix<double, RB, CB> &B) {
      check_square("trace_quad_form", "A", A);
      check_multiplicable("trace_quad_form",
                          "A", A,
                          "B", B);
      return trace(multiply(transpose(B),
                            multiply(A, B)));
    }

    template<int RA, int CA, int RB, int CB, typename T>
    inline fvar<T>
    trace_quad_form(const Eigen::Matrix<double, RA, CA> &A,
                    const Eigen::Matrix<fvar<T>, RB, CB> &B) {
      check_square("trace_quad_form", "A", A);
      check_multiplicable("trace_quad_form",
                          "A", A,
                          "B", B);
      return trace(multiply(transpose(B),
                            multiply(A, B)));
    }
  }
}

#endif


#include <stdio.h>
#include <iostream>
#include <iomanip>

#include <Eigen/Core>

#include "../libfovis/initial_homography_estimation.hpp"
#include <bot_core/bot_core.h>
#include <bot_lcmgl_client/lcmgl.h>
#include <GL/gl.h>

using namespace std;

#define dump(var) (cerr<<" "#var<<" =[\n"<< setprecision (3)<<var<<"];"<<endl)

static Eigen::ArrayXf flattenMatrix(Eigen::MatrixXf &m)
{
  return Eigen::Map<Eigen::ArrayXf>(m.data(), m.rows() * m.cols());
}

static void warpImage(const uint8_t * image, int width, int height, int rowstride, const Eigen::Matrix3f &H,
    uint8_t * warped_image)
{
  //setup the utility matrices
  Eigen::MatrixXf x = Eigen::VectorXf::LinSpaced(width, 0, width - 1).transpose().replicate(height, 1);
  Eigen::MatrixXf y = Eigen::VectorXf::LinSpaced(height, 0, height - 1).replicate(1, width);
  Eigen::MatrixXf xx = flattenMatrix(x);
  Eigen::MatrixXf yy = flattenMatrix(y);

  Eigen::MatrixXf imageHomogeneousPoints(3, xx.rows());
  imageHomogeneousPoints.row(0) = xx.transpose();
  imageHomogeneousPoints.row(1) = yy.transpose();
  imageHomogeneousPoints.row(2).setOnes();

  Eigen::MatrixXf warpedHomogeneousPoints;
  warpedHomogeneousPoints = H * imageHomogeneousPoints;

  Eigen::MatrixXf warped = Eigen::MatrixXf(height, width);

  const double defaultValue = 128;
  //Bilinear interpolation
  for (int i = 0; i < warpedHomogeneousPoints.cols(); i++) {
    double val;
    Eigen::Vector2f pt = warpedHomogeneousPoints.col(i).head(2) / warpedHomogeneousPoints(2, i);
    Eigen::Vector2i fipt(floor(pt(0)), floor(pt(1)));
    Eigen::Vector2i cipt(ceil(pt(0)), ceil(pt(1)));
    if (0 <= pt(0) && pt(0) < width - 1 && 0 <= pt(1) && pt(1) < height - 1) {
      double x1 = pt(0) - fipt(0);
      double y1 = pt(1) - fipt(1);
      double x2 = 1 - x1;
      double y2 = 1 - y1;
      val = x2 * y2 * image[fipt(1) * rowstride + fipt(0)] + x1 * y2 * image[fipt(1) * rowstride + fipt(0) + 1] + x2
          * y1 * image[(fipt(1) + 1) * rowstride + fipt(0)] + x1 * y1 * image[(fipt(1) + 1) * rowstride + fipt(0) + 1];

    }
    else if (0 <= fipt(0) && fipt(0) < width && 0 <= fipt(1) && fipt(1) < height) {
      val = image[fipt(1) * rowstride + fipt(0)];
    }
    else if (0 <= cipt(0) && cipt(0) < width && 0 <= cipt(1) && cipt(1) < height) {
      val = image[cipt(1) * rowstride + cipt(0)];
    }
    else
      val = defaultValue; //templateImage(i / width, i % width); //default to the same as template, so error is 0

    warped_image[(i % height) * rowstride + (i / height)] = val; //Eigen is Column-major
  }
}

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

  uint8_t* src_im = (uint8_t*) malloc(640 * 480 * sizeof(uint8_t));

  int width, height, rowstride;
  bot_pgm_read_fname("test_im.pgm", &src_im, &width, &height, &rowstride);

  Eigen::MatrixXf ref_image = Eigen::MatrixXf::Zero(60, 80);
  ref_image.block(15, 25, 30, 30).setConstant(255.0);

  Eigen::Matrix3f H_warp;
  H_warp.setZero();

  double u = 2;
  double v = 1;
  double theta = 1 * M_PI / 180.0;
  int downsample = 0;
  if (argc >= 2)
    u = atof(argv[1]);

  if (argc >= 3)
    v = atof(argv[2]);

  if (argc >= 4)
    theta = atof(argv[3]) * M_PI / 180.0;

  if (argc >= 5)
    downsample = atoi(argv[4]);

  H_warp(0, 0) = cos(theta);
  H_warp(0, 1) = sin(theta);
  H_warp(1, 0) = -sin(theta);
  H_warp(1, 1) = cos(theta);

  H_warp(0, 2) = u;
  H_warp(1, 2) = v;
  H_warp(2, 2) = 1;

  uint8_t* warped_im = (uint8_t*) malloc(width * height * sizeof(uint8_t));
  warpImage(src_im, width, height, rowstride, H_warp, warped_im);

  fovis::InitialHomographyEstimator rotation_estimator;
  rotation_estimator.setTemplateImage(warped_im, width, height, rowstride, downsample);

  rotation_estimator.setTestImage(src_im, width, height, rowstride, downsample);
  double finalRMS = 0;
  Eigen::Matrix3f H_est = rotation_estimator.track(Eigen::Matrix3f::Identity(), 10, &finalRMS);

  double scale_factor = 1 << downsample;
  Eigen::Matrix3f S = Eigen::Matrix3f::Identity() * scale_factor;
  S(2,2)=1;

  //scale homography up to the full size image
  H_est = S * H_est * S.inverse();

  dump(H_warp);
  dump(H_est);
  fprintf(stderr, "finalRMS=%f\n", finalRMS);

  //draw results
  lcm_t * lcm = lcm_create(NULL);
  bot_lcmgl_t* lcmgl = bot_lcmgl_init(lcm, "Init_Homography_Test\n");

  //convert to pixel coords
  bot_lcmgl_push_matrix(lcmgl);
  bot_lcmgl_rotated(lcmgl, -90, 0, 0, 1);
  bot_lcmgl_scalef(lcmgl, 10.0 / width, -10.0 / width, 1);

  // template image
  bot_lcmgl_color3f(lcmgl, 1, 1, 1);
  int template_texid = bot_lcmgl_texture2d(lcmgl, warped_im, width, height, rowstride, BOT_LCMGL_LUMINANCE,
      BOT_LCMGL_COMPRESS_NONE);

  bot_lcmgl_push_matrix(lcmgl);
  bot_lcmgl_translated(lcmgl, 0, height + 10, 0);
  bot_lcmgl_texture_draw_quad(lcmgl, template_texid, 0, 0, 0, 0, height, 0, width, height, 0, width, 0, 0);
  bot_lcmgl_pop_matrix(lcmgl);

  // test image
  bot_lcmgl_color3f(lcmgl, 1, 1, 1);
  int gray_texid = bot_lcmgl_texture2d(lcmgl, src_im, width, height, rowstride, BOT_LCMGL_LUMINANCE,
      BOT_LCMGL_COMPRESS_NONE);
  bot_lcmgl_texture_draw_quad(lcmgl, gray_texid, 0, 0, 0, 0, height, 0, width, height, 0, width, 0, 0);

  //draw the ESM homography estimate
  bot_lcmgl_line_width(lcmgl, 2.0);
  bot_lcmgl_color3f(lcmgl, 1, 1, 0);
  bot_lcmgl_begin(lcmgl, GL_LINE_STRIP);
  Eigen::MatrixXf vertices(5, 3);
  vertices << 0, 0, 1, width, 0, 1, width, height, 1, 0, height, 1, 0, 0, 1;

  Eigen::MatrixXf warpedPoints = H_est * vertices.transpose();
  warpedPoints.row(0) = warpedPoints.row(0).array() / warpedPoints.row(2).array();
  warpedPoints.row(1) = warpedPoints.row(1).array() / warpedPoints.row(2).array();
  for (int i = 0; i < warpedPoints.cols(); i++) {
    bot_lcmgl_vertex2f(lcmgl, warpedPoints(0, i), warpedPoints(1, i));
  }
  bot_lcmgl_end(lcmgl);

  bot_lcmgl_pop_matrix(lcmgl);

  bot_lcmgl_switch_buffer(lcmgl);

  return 0;
}


//   Copyright 2018 Dhruvesh Nikhilkumar Patel
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
//
#include <boost/numeric/ublas/matrix.hpp>
#include "../include/Polynomial.ipp"
#include <gmpxx.h>
#include <iostream>
int main()
{
   using namespace boost::numeric::ublas;
   using coefT = mpz_class;
   using entryT= Polynomial<coefT>;
   entryT p1 {1,2,3};
   entryT p2 {4,5,6};
   entryT p3 {1};
   entryT p4 {7,8,9,10};
   matrix<entryT> m (2,2);
   m(0,0)=p1;
   m(0,1)=p2;
   m(1,0)=p3;
   m(1,1)=p4;
   std::cout<<"m(0,0)= "<<m(0,0)<<std::endl;
   std::cout<<"m(0,1)= "<<m(0,1)<<std::endl;
   std::cout<<"m(1,0)= "<<m(1,0)<<std::endl;
   std::cout<<"m(1,1)= "<<m(1,1)<<std::endl;

   matrix<entryT> m2 (2,2);
   m2 = prod(m,m);
   std::cout<<"m2=m*m"<<std::endl;
   std::cout<<"m2(0,0)= "<<m2(0,0)<<std::endl;
   std::cout<<"m2(0,1)= "<<m2(0,1)<<std::endl;
   std::cout<<"m2(1,0)= "<<m2(1,0)<<std::endl;
   std::cout<<"m2(1,1)= "<<m2(1,1)<<std::endl;

   return 0;
}


/**
 * @file   unitary.cc
 * @date   12/2018
 * @author Imran Ashraf
 * @author Anneriet Krol
 * @brief  unitary matrix (decomposition) implementation
 */

#include <unitary.h>

#ifndef WITHOUT_UNITARY_DECOMPOSITION
#include <Eigen/MatrixFunctions>
#include <src/misc/lapacke.h>
#endif

typedef unsigned int uint;

#include <chrono>

namespace ql
{

unitary::unitary() : name(""), is_decomposed(false) {}

unitary::unitary(std::string name, std::vector<std::complex<double>> array) :
name(name), array(array), is_decomposed(false) {}

double unitary::size() {
    // JvS: Note that the original unitary::size() used
    // Eigen::Matrix::size() if the array is empty. However, if the array
    // is empty, _matrix is never initialized beyond its default ctor,
    // which "allocates" a 0x0 matrix, and is thus size 0, exactly what
    // array.size() would return.
    // Don't get me started about why this returns a double.
    return (double) array.size();
}

#ifdef WITHOUT_UNITARY_DECOMPOSITION

void unitary::decompose() {
    throw std::runtime_error("unitary decomposition was explicitly disabled in this build!");
}

#else

// JvS: this was originally the class "unitary" itself, but compile times of
// Eigen are so excessive that I moved it into its own compile unit and
// provided a wrapper instead. It doesn't actually NEED to be wrapped like
// this, because the Eigen::Matrix<...> member is actually only used within
// the scope of a single method (calling other methods), but I'm not touching
// this code.
class UnitaryDecomposer
{
private:
    Eigen::Matrix<std::complex<double>, Eigen::Dynamic, Eigen::Dynamic> _matrix;

public:
    std::string name;
    std::vector<std::complex<double>> array;
    std::vector<std::complex<double>> SU;
    double delta;
    double alpha;
    double beta;
    double gamma;
    bool is_decomposed;
    std::vector<double> instructionlist;

    typedef Eigen::Matrix<std::complex<double>, Eigen::Dynamic, Eigen::Dynamic> complex_matrix ;

    UnitaryDecomposer() : name(""), is_decomposed(false) {}

    UnitaryDecomposer(std::string name, std::vector<std::complex<double>> array) :
            name(name), array(array), is_decomposed(false)
    {
        DOUT("constructing unitary: " << name
                  << ", containing: " << array.size() << " elements");
    }

    double size()
    {
        if(!array.empty())
            return (double) array.size();
        else
            return (double) _matrix.size();
    }

    complex_matrix getMatrix()
    {
        if (!array.empty())
        {
            int matrix_size = (int)std::pow(array.size(), 0.5);

            Eigen::Map<complex_matrix> matrix(array.data(), matrix_size, matrix_size);
            _matrix = matrix.transpose();
        }
        return _matrix;
    }

    void decompose()
    {
        DOUT("decomposing Unitary: " << name);

        getMatrix();
        int matrix_size = _matrix.rows();

        // compute the number of qubits: length of array is collumns*rows, so log2(sqrt(array.size))
        int numberofbits = uint64_log2(matrix_size);

        Eigen::MatrixXcd identity = Eigen::MatrixXcd::Identity(matrix_size, matrix_size);
        Eigen::MatrixXcd matmatadjoint = (_matrix.adjoint()*_matrix);
        // very little accuracy because of tests using printed-from-matlab code that does not have many digits after the comma
        if( !matmatadjoint.isApprox(identity, 0.001))
        {
            //Throw an error
            EOUT("Unitary " << name <<" is not a unitary matrix!");

            throw ql::exception("Error: Unitary '"+ name+"' is not a unitary matrix. Cannot be decomposed!" + to_string(matmatadjoint), false);
        }
        // initialize the general M^k lookuptable
        genMk();

        decomp_function(_matrix, numberofbits); //needed because the matrix is read in columnmajor

        DOUT("Done decomposing");
        is_decomposed = true;
    }

    std::string to_string(complex_matrix m, std::string vector_prefix = "",
                            std::string elem_sep = ", ")
    {
        std::ostringstream ss;
        ss << m << "\n";
        return ss.str();
    }

    // std::chrono::duration<double> CSD_time;
    // std::chrono::duration<double> CSD_time2;
    // std::chrono::duration<double> CSD_time3;
    // std::chrono::duration<double> zyz_time;
    // std::chrono::duration<double> multiplexing_time;
    // std::chrono::duration<double> demultiplexing_time;

    void decomp_function(const Eigen::Ref<const complex_matrix>& matrix, int numberofbits)
    {
        DOUT("decomp_function: \n" << to_string(matrix));
        if(numberofbits == 1)
        {
            zyz_decomp(matrix);
        }
        else
        {
            int n = matrix.rows()/2;

            complex_matrix V(n,n);
            complex_matrix W(n,n);
            Eigen::VectorXcd D(n);
            // if q2 is zero, the whole thing is a demultiplexing problem instead of full CSD
            if(matrix.bottomLeftCorner(n,n).isZero(10e-14) && matrix.topRightCorner(n,n).isZero(10e-14))
            {
                DOUT("Optimization: q2 is zero, only demultiplexing will be performed.");
                instructionlist.push_back(200.0);
                if(matrix.topLeftCorner(n, n).isApprox(matrix.bottomRightCorner(n,n),10e-4))
                {
                    DOUT("Optimization: Unitaries are equal, skip one step in the recursion for unitaries of size: " << n << " They are both: " << matrix.topLeftCorner(n, n));
                    instructionlist.push_back(300.0);
                    decomp_function(matrix.topLeftCorner(n, n), numberofbits-1);
                }
                else
                {
                    demultiplexing(matrix.topLeftCorner(n, n), matrix.bottomRightCorner(n,n), V, D, W, numberofbits-1);

                    decomp_function(W, numberofbits-1);
                    multicontrolledZ(D, D.rows());
                    decomp_function(V, numberofbits-1);
                }
            }
            // Check to see if it the kronecker product of a bigger matrix and the identity matrix.
            // By checking if the first row is equal to the second row one over, and if thelast two rows are equal
            // Which means the last qubit is not affected by this gate
            else if (matrix(Eigen::seqN(0, n, 2), Eigen::seqN(1, n, 2)).isZero()  && matrix(Eigen::seqN(1, n, 2), Eigen::seqN(0, n, 2)).isZero()  && matrix.block(0,0,1,2*n-1) == matrix.block(1,1,1,2*n-1) &&  matrix.block(2*n-2,0,1,2*n-1) ==  matrix.block(2*n-1,1,1,2*n-1))
            {
                DOUT("Optimization: last qubit is not affected, skip one step in the recursion.");
                // Code for last qubit not affected
                instructionlist.push_back(100.0);
                decomp_function(matrix(Eigen::seqN(0, n, 2), Eigen::seqN(0, n, 2)), numberofbits-1);

            }
            else
            {
            complex_matrix ss(n,n);
            complex_matrix L0(n,n);
            complex_matrix L1(n,n);
            complex_matrix R0(n,n);
            complex_matrix R1(n,n);
            // auto start = std::chrono::steady_clock::now();
            CSD(matrix, L0, L1, R0, R1, ss);
            // CSD_time += (std::chrono::steady_clock::now() - start);
            demultiplexing(R0, R1, V, D, W, numberofbits-1);
            decomp_function(W, numberofbits-1);
            multicontrolledZ(D, D.rows());
            decomp_function(V, numberofbits-1);

            multicontrolledY(ss.diagonal(), n);

            demultiplexing(L0, L1, V, D, W, numberofbits-1);
            decomp_function(W, numberofbits-1);
            multicontrolledZ(D, D.rows());
            decomp_function(V, numberofbits-1);
            }
        }
    }

    void CSD(const Eigen::Ref<const complex_matrix>& U, Eigen::Ref<complex_matrix> u1, Eigen::Ref<complex_matrix> u2, Eigen::Ref<complex_matrix> v1, Eigen::Ref<complex_matrix> v2, Eigen::Ref<complex_matrix> s)
    {
        // auto start = std::chrono::steady_clock::now();
        //Cosine sine decomposition
        // U = [q1, U01] = [u1    ][c  s][v1  ]
        //     [q2, U11] = [    u2][-s c][   v2]
        int n = U.rows();
        // complex_matrix c(n,n); // c matrix is not needed for the higher level
        // complex_matrix q1 = U.topLeftCorner(n/2,m/2);

        Eigen::BDCSVD<complex_matrix> svd(n/2,n/2);
        svd.compute(U.topLeftCorner(n/2,n/2), Eigen::ComputeThinU | Eigen::ComputeThinV); // possible because it's square anyway

        // thinCSD: q1 = u1*c*v1.adjoint()
        //          q2 = u2*s*v1.adjoint()
        int p = n/2;
        // complex_matrix z = Eigen::MatrixXd::Identity(p, p).colwise().reverse();
        complex_matrix c(svd.singularValues().reverse().asDiagonal());
        u1.noalias() = svd.matrixU().rowwise().reverse();
        v1.noalias() = svd.matrixV().rowwise().reverse(); // Same v as in matlab: u*s*v.adjoint() = q1

        complex_matrix q2 = U.bottomLeftCorner(p,p)*v1;

        int k = 0;
        for(int j = 1; j < p; j++)
        {
            if(c(j,j).real() <= 0.70710678119)
            {
                k = j;
            }
        }
        //complex_matrix b = q2.block( 0,0, p, k+1);

        Eigen::HouseholderQR<complex_matrix> qr(p,k+1);
        qr.compute(q2.block( 0,0, p, k+1));
        u2 = qr.householderQ();
        s.noalias() = u2.adjoint()*q2;
        if(k < p-1)
        {
            DOUT("k is smaller than size of q1 = "<< p << ", adjustments will be made, k = " << k);
            k = k+1;
            Eigen::BDCSVD<complex_matrix> svd2(p-k, p-k);
            svd2.compute(s.block(k, k, p-k, p-k), Eigen::ComputeThinU | Eigen::ComputeThinV);
            s.block(k, k, p-k, p-k) = svd2.singularValues().asDiagonal();
            c.block(0,k, p,p-k) = c.block(0,k, p,p-k)*svd2.matrixV();
            u2.block(0,k, p,p-k) = u2.block(0,k, p,p-k)*svd2.matrixU();
            v1.block(0,k, p,p-k) = v1.block(0,k, p,p-k)*svd2.matrixV();

            Eigen::HouseholderQR<complex_matrix> qr2(p-k, p-k);

            qr2.compute(c.block(k,k, p-k,p-k));
            c.block(k,k,p-k,p-k) = qr2.matrixQR().triangularView<Eigen::Upper>();
            u1.block(0,k, p,p-k) = u1.block(0,k, p,p-k)*qr2.householderQ();
        }
        // CSD_time2 += (std::chrono::steady_clock::now() - start);

        // auto start2 = std::chrono::steady_clock::now();

        std::vector<int> c_ind;
        std::vector<int> s_ind;
        for(int j = 0; j < p; j++)
        {
            if(c(j,j).real() < 0)
            {
                c_ind.push_back(j);
            }
            if(s(j,j).real() < 0)
            {
                s_ind.push_back(j);
            }
        }

        c(c_ind,c_ind) = -c(c_ind,c_ind);
        u1(Eigen::all, c_ind) = -u1(Eigen::all, c_ind);

        //s.diagonal()(s_ind) = -s.diagonal()(s_ind);
        s(s_ind,s_ind) = -s(s_ind,s_ind);
        u2(Eigen::all, s_ind) = -u2(Eigen::all, s_ind);

        if(!U.topLeftCorner(p,p).isApprox(u1*c*v1.adjoint(), 10e-8) || !U.bottomLeftCorner(p,p).isApprox(u2*s*v1.adjoint(), 10e-8))
        {
            if(U.topLeftCorner(p,p).isApprox(u1*c*v1.adjoint(), 10e-8))
            {
                DOUT("q1 is correct");
            }
            else
            {
                DOUT("q1 is not correct! (is not usually an issue");
                DOUT("q1: \n" << U.topLeftCorner(p,p));
                DOUT("reconstructed q1: \n" << u1*c*v1.adjoint());

            }
            if(U.bottomLeftCorner(p,p).isApprox(u2*s*v1.adjoint(), 10e-8))
            {
                DOUT("q2 is correct");
            }
            else
            {
                DOUT("q2 is not correct! (is not usually an issue)");
                DOUT("q2: " << U.bottomLeftCorner(p,p));
                DOUT("reconstructed q2: " << u2*s*v1.adjoint());
            }
        }
        v1.adjointInPlace(); // Use this instead of = v1.adjoint (to avoid aliasing issues)
        s = -s;

        complex_matrix tmp_s = u1.adjoint()*U.topRightCorner(p,p);
        complex_matrix tmp_c = u2.adjoint()*U.bottomRightCorner(p,p);

        // std::vector<int> c_ind_row;
        // std::vector<int> s_ind_row;
        for(int i = 0; i < p; i++)
        {
            if(std::abs(s(i,i)) > std::abs(c(i,i)))
            {
                // std::vector<int> s_ind_row;
                v2.row(i).noalias() = tmp_s.row(i)/s(i,i);
            }
            else
            {
                // c_ind_row.push_back(i);
                v2.row(i).noalias() = tmp_c.row(i)/c(i,i);
            }
        }

        // v2(s_ind_row, Eigen::all) = tmp_s(s_ind_row, Eigen::all).rowwise() / s(s_ind_row, s_ind_row).array();
        // v2(c_ind_row, Eigen::all) = tmp_c(c_ind_row, Eigen::all).rowwise() / c(c_ind_row, c_ind_row).array();
        // U = [q1, U01] = [u1    ][c  s][v1  ]
        //     [q2, U11] = [    u2][-s c][   v2]

        complex_matrix tmp(n,n);
        tmp.topLeftCorner(p,p) = u1*c*v1;
        tmp.bottomLeftCorner(p,p) = -u2*s*v1;
        tmp.topRightCorner(p,p) = u1*s*v2;
        tmp.bottomRightCorner(p,p) = u2*c*v2;
        // Just to see if it kinda matches
        if(!tmp.isApprox(U, 10e-2))
        {
            throw ql::exception("CSD of unitary '"+ name+"' is wrong! Failed at matrix: \n"+to_string(tmp) + "\nwhich should be: \n" + to_string(U), false);
        }
            // CSD_time3 += (std::chrono::steady_clock::now() - start2);

    }

    void zyz_decomp(const Eigen::Ref<const complex_matrix>& matrix)
    {
        // auto start = std::chrono::steady_clock::now();

        ql::complex_t det = matrix.determinant();// matrix(0,0)*matrix(1,1)-matrix(1,0)*matrix(0,1);

        double delta = atan2(det.imag(), det.real())/matrix.rows();
        std::complex<double> A = exp(std::complex<double>(0,-1)*delta)*matrix(0,0);
        std::complex<double> B = exp(std::complex<double>(0,-1)*delta)*matrix(0,1); //to comply with the other y-gate definition

        double sw = sqrt(pow((double) B.imag(),2) + pow((double) B.real(),2) + pow((double) A.imag(),2));
        double wx = 0;
        double wy = 0;
        double wz = 0;

        if(sw > 0)
        {
        wx = B.imag()/sw;
        wy = B.real()/sw;
        wz = A.imag()/sw;
        }

        double t1 = atan2(A.imag(),A.real());
        double t2 = atan2(B.imag(), B.real());
        alpha = t1+t2;
        gamma = t1-t2;
        beta = 2*atan2(sw*sqrt(pow((double) wx,2)+pow((double) wy,2)),sqrt(pow((double) A.real(),2)+pow((wz*sw),2)));
        instructionlist.push_back(-gamma);
        instructionlist.push_back(-beta);
        instructionlist.push_back(-alpha);
        // zyz_time += (std::chrono::steady_clock::now() - start);
    }

    void demultiplexing(const Eigen::Ref<const complex_matrix> &U1, const Eigen::Ref<const complex_matrix> &U2,  Eigen::Ref<complex_matrix> V,  Eigen::Ref<Eigen::VectorXcd> D,  Eigen::Ref<complex_matrix> W, int numberofcontrolbits)
    {
        // [U1 0 ]  = [V 0][D 0 ][W 0]
        // [0  U2]    [0 V][0 D*][0 W]
        // auto start = std::chrono::steady_clock::now();
        complex_matrix check = U1*U2.adjoint();
        // complex_matrix D;
        // complex_matrix V;
        // complex_matrix W;
        if(check == check.adjoint())
        {
            IOUT("Demultiplexing matrix is self-adjoint()");
            Eigen::SelfAdjointEigenSolver<Eigen::MatrixXcd> eigslv(check);
            D.noalias() = ((complex_matrix) eigslv.eigenvalues()).cwiseSqrt();
            V.noalias() = eigslv.eigenvectors();
            W.noalias() = D.asDiagonal()*V.adjoint()*U2;
        }
        else
        {
            if (numberofcontrolbits < 5) //schur is faster for small matrices
            {
                Eigen::ComplexSchur<complex_matrix> decomposition(check);
                D.noalias() = decomposition.matrixT().diagonal().cwiseSqrt();
                V.noalias() = decomposition.matrixU();
                W.noalias() = D.asDiagonal() * V.adjoint() * U2;
            }
            else
            {
                Eigen::ComplexEigenSolver<complex_matrix> decomposition(check);
                D.noalias() = decomposition.eigenvalues().cwiseSqrt();
                V.noalias() = decomposition.eigenvectors();
                W.noalias() = D.asDiagonal() * V.adjoint() * U2;
            }
        }

        // demultiplexing_time += (std::chrono::steady_clock::now() - start);
        if(!(V*V.adjoint()).isApprox(Eigen::MatrixXd::Identity(V.rows(), V.rows()), 10e-3))
        {
            DOUT("Eigenvalue decomposition incorrect: V is not unitary, adjustments will be made");
            Eigen::BDCSVD<complex_matrix> svd3(V.block(0,0,V.rows(),2), Eigen::ComputeFullU);
            V.block(0,0,V.rows(),2) = svd3.matrixU();
            svd3.compute(V(Eigen::all,Eigen::seq(Eigen::last-1,Eigen::last)), Eigen::ComputeFullU);
            V(Eigen::all,Eigen::seq(Eigen::last-1,Eigen::last)) = svd3.matrixU();

        }

        complex_matrix Dtemp = D.asDiagonal();
        if(!U1.isApprox(V*Dtemp*W, 10e-2) || !U2.isApprox(V*Dtemp.adjoint()*W, 10e-2))
        {
            EOUT("Demultiplexing not correct!");
            throw ql::exception("Demultiplexing of unitary '"+ name+"' not correct! Failed at matrix U1: \n"+to_string(U1)+ "and matrix U2: \n" +to_string(U2) + "\nwhile they are: \n" + to_string(V*D.asDiagonal()*W) + "\nand \n" + to_string(V*D.conjugate().asDiagonal()*W), false);
        }

    }

    std::vector<Eigen::MatrixXd> genMk_lookuptable;

    // returns M^k = (-1)^(b_(i-1)*g_(i-1)), where * is bitwise inner product, g = binary gray code, b = binary code.
    void genMk()
    {
        int numberqubits = uint64_log2(_matrix.rows());
        for(int n = 1; n <= numberqubits; n++)
        {
            int size=1<<n;
            Eigen::MatrixXd Mk(size,size);
            for(int i = 0; i < size; i++)
            {
                for(int j = 0; j < size ;j++)
                {
                    Mk(i,j) =std::pow(-1, bitParity(i&(j^(j>>1))));
                }
            }
        genMk_lookuptable.push_back(Mk);
        }

        // return genMk_lookuptable[numberqubits-1];
    }

    // source: https://stackoverflow.com/questions/994593/how-to-do-an-integer-log2-in-c user Todd Lehman
    int uint64_log2(uint64_t n)
    {
    #define S(k) if (n >= (UINT64_C(1) << k)) { i += k; n >>= k; }

    int i = -(n == 0); S(32); S(16); S(8); S(4); S(2); S(1); return i;

    #undef S
    }

    int bitParity(int i)
    {
        if (i < 2 << 16)
        {
            i = (i >> 16) ^ i;
            i = (i >> 8) ^ i;
            i = (i >> 4) ^ i;
            i = (i >> 2) ^ i;
            i = (i >> 1) ^ i;
            return i % 2;
        }
        else
        {
            throw ql::exception("Bit parity number too big!", false);
        }
    }

    void multicontrolledY(const Eigen::Ref<const Eigen::VectorXcd> &ss, int halfthesizeofthematrix)
    {
        // auto start = std::chrono::steady_clock::now();
        Eigen::VectorXd temp =  2*Eigen::asin(ss.array()).real();
        Eigen::CompleteOrthogonalDecomposition<Eigen::MatrixXd> dec(genMk_lookuptable[uint64_log2(halfthesizeofthematrix)-1]);
        Eigen::VectorXd tr = dec.solve(temp);
        // Check is very approximate to account for low-precision input matrices
        if(!temp.isApprox(genMk_lookuptable[uint64_log2(halfthesizeofthematrix)-1]*tr, 10e-2))
        {
                EOUT("Multicontrolled Y not correct!");
                throw ql::exception("Demultiplexing of unitary '"+ name+"' not correct! Failed at demultiplexing of matrix ss: \n"  + to_string(ss), false);
        }

        instructionlist.insert(instructionlist.end(), &tr[0], &tr[halfthesizeofthematrix]);
        // multiplexing_time += std::chrono::steady_clock::now() - start;
    }

    void multicontrolledZ(const Eigen::Ref<const Eigen::VectorXcd> &D, int halfthesizeofthematrix)
    {
        // auto start = std::chrono::steady_clock::now();

        Eigen::VectorXd temp =  (std::complex<double>(0,-2)*Eigen::log(D.array())).real();
        Eigen::CompleteOrthogonalDecomposition<Eigen::MatrixXd> dec(genMk_lookuptable[uint64_log2(halfthesizeofthematrix)-1]);
        Eigen::VectorXd tr = dec.solve(temp);
        // Check is very approximate to account for low-precision input matrices
        if(!temp.isApprox(genMk_lookuptable[uint64_log2(halfthesizeofthematrix)-1]*tr, 10e-2))
        {
                EOUT("Multicontrolled Z not correct!");
                throw ql::exception("Demultiplexing of unitary '"+ name+"' not correct! Failed at demultiplexing of matrix D: \n"+ to_string(D), false);
        }

        instructionlist.insert(instructionlist.end(), &tr[0], &tr[halfthesizeofthematrix]);
        // multiplexing_time += std::chrono::steady_clock::now() - start;

    }
    ~UnitaryDecomposer()
    {
        // destroy unitary
        DOUT("destructing unitary: " << name);
    }
};

void unitary::decompose() {
    UnitaryDecomposer decomposer(name, array);
    decomposer.decompose();
    SU = decomposer.SU;
    alpha = decomposer.alpha;
    beta = decomposer.beta;
    gamma = decomposer.gamma;
    is_decomposed = decomposer.is_decomposed;
    instructionlist = decomposer.instructionlist;
}

#endif

}


#include <iostream>
#include <fstream>
#include <algorithm>
#include <complex>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/algorithm/minmax.hpp>

#include "../include/TriMesh.h"
#include "../include/utils.h"
#include "../include/lagrange.h"
#include "../include/geometry.h"
#include "../include/ConstructCurveMesh.h"
#include "../include/GetQuadraturePointsWeight2D.h"
#include "../include/solver.h"
#include "../include/euler.h"
#include "../include/Param.h"
#include "../include/Collective.h"
#include "../include/InvertMatrix.h"

using namespace std;

int main(int argc, char *argv[])
{
    Param param;
    // Set up the param struct
    param = ReadParamIn(string(argv[1]));
    TriMesh mesh(param.mesh_file);
    // Testing Calculate Residaul
    int p = param.order;
    int q = param.order_geo;
    int Np = int((p + 1) * (p + 2) / 2);
    TriMesh curved_mesh = mesh;
    string boundary_name="bottom";
    ConstructCurveMesh(mesh, curved_mesh, geometry::BumpFunction, boundary_name, q);

    ResData resdata_postproc;
    if (p == 0)
    {
        solver::CalcResData(curved_mesh, 1, resdata_postproc);
    }else
    {
        solver::CalcResData(curved_mesh, p, resdata_postproc);
    }

    int Np_solution;
    if (p == 0)
        Np_solution = 3;
    else
        Np_solution = Np;
    ublas::vector<ublas::matrix<double> > Nodes(curved_mesh.E.size(), ublas::matrix<double>(Np_solution, 2, 0.0));
    ublas::vector<ublas::matrix<double> > State_on_Nodes(curved_mesh.E.size(), ublas::matrix<double>(Np_solution, 4, 0.0));

    ifstream file_nodes, file_state;
    file_nodes.open("nodes.dat");
    file_state.open("states.dat");
    for (int ielem = 0; ielem < curved_mesh.E.size(); ielem++)
    {
        for (int ip = 0; ip < Np_solution; ip++)
        {
            double x, y, s0, s1, s2, s3;
            char temp;
            file_nodes  >> x  >> y;
            file_state  >> s0 >> s1 >> s2 >> s3;
            Nodes(ielem)(ip, 0) = x; Nodes(ielem)(ip, 1) = y;
            State_on_Nodes(ielem)(ip, 0) = s0; State_on_Nodes(ielem)(ip, 1) = s1;
            State_on_Nodes(ielem)(ip, 2) = s2; State_on_Nodes(ielem)(ip, 3) = s3;
        }
    }
    file_nodes.close();
    file_state.close();
    param.h = 0.0625;
    double err_entropy, coeff_lift, coeff_drag;
    std::vector<std::vector<double> > p_coeff_dist;
    solver::CalcScalarOutputs(curved_mesh, resdata_postproc, State_on_Nodes, Nodes, param,
                                    err_entropy, coeff_lift, coeff_drag, p_coeff_dist);
    cout.setf(ios::scientific, ios::floatfield);
    std::cout << setprecision(8) << err_entropy << ' ' << coeff_lift << ' ' << coeff_drag << endl;
    ofstream file_pdist;
    file_pdist.open("pressure_distribution.dat");
    for (int i = 0; i < p_coeff_dist.size(); i++)
    {
        cout.setf(ios::scientific, ios::floatfield);
        file_pdist  << setprecision(10) << p_coeff_dist[i][0] << ' ' << p_coeff_dist[i][1] << std::endl;
    }
    file_pdist.close();
    return 0;
}


#include <mex.h>
#include <Eigen/Core>
#include "splineGeneration.h"
#include "drakeMexUtil.h"
#include <iostream>
#include <limits>

using namespace std;
using namespace Eigen;

const int GRID_STEPS = 10;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  string usage = "[coefs, ts, objective_value] = nWaypointCubicSplineFreeKnotTimesmex.cpp(t0, tf, xs, xd0, xdf)";
  if (nrhs != 5)
    mexErrMsgIdAndTxt("Drake:nWaypointCubicSplineFreeKnotTimesmex.cpp:WrongNumberOfInputs", usage.c_str());
  if (nlhs < 2 || nlhs > 3)
    mexErrMsgIdAndTxt("Drake:nWaypointCubicSplineFreeKnotTimesmex.cpp:WrongNumberOfOutputs", usage.c_str());

  double t0 = mxGetPrSafe(prhs[0])[0];
  double tf = mxGetPrSafe(prhs[1])[0];
  MatrixXd xs = matlabToEigen<Dynamic, Dynamic>(prhs[2]);
  auto xd0 = matlabToEigen<Dynamic, 1>(prhs[3]);
  auto xdf = matlabToEigen<Dynamic, 1>(prhs[4]);

  mwSize ndof = static_cast<mwSize>(xs.rows());
  mwSize num_segments = static_cast<mwSize>(xs.cols()) - 1;
  mwSize num_knots = num_segments - 1;
  if (num_knots >= 3)
    mexWarnMsgTxt("More knots than two is likely to be super slow in a grid search!\n");
  if (num_knots <= 0)
    mexErrMsgIdAndTxt("Drake:nWaypointCubicSplineFreeKnotTimesmex.cpp:NotEnoughKnotsToJustifyThisFunction", usage.c_str());
  mwSize num_coeffs_per_segment = 4;
  mwSize dims[] = { ndof, num_segments, num_coeffs_per_segment };
  plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);

  std::vector<double> segment_times;
  segment_times.resize(static_cast<size_t>(num_segments) + 1);
  segment_times[0] = t0;
  segment_times[static_cast<size_t>(num_segments)] = tf;
  std::vector<double> best_segment_times = segment_times;
  double t_step = (tf - t0) / GRID_STEPS;
  double min_objective_value = numeric_limits<double>::infinity();

  // assemble the knot point locations for input to nWaypointCubicSpline
  MatrixXd xi = xs.block(0, 1, ndof, num_knots);

  if (GRID_STEPS <= num_knots) {
    // If we have have too few grid steps, then by pigeonhole it's
    // impossible to give each a unique time in our grid search.
    mexErrMsgIdAndTxt("Drake:nWaypointCubicSplineFreeKnotTimesmex.cpp:TooManyKnotsForNumGridSteps", usage.c_str());
  }
  std::vector<int> t_indices;
  t_indices.reserve(num_knots);
  for (int i = 0; i < num_knots; i++) {
    t_indices.push_back(i + 1); // assume knot point won't be the same time as the initial state, or previous knot point
  }

  while (t_indices[0] < GRID_STEPS - num_knots + 1) {
    for (int i = 0; i < num_knots; i++)
      segment_times[i + 1] = t0 + t_indices[i] * t_step;

    bool valid_solution = true;
    double objective_value = 0.0;
    for (int dof = 0; dof < ndof && valid_solution; dof++) {
      try {
        PiecewisePolynomial<double> spline = nWaypointCubicSpline(segment_times, xs(dof, 0), xd0[dof], xs(dof, num_segments), xdf[dof], xi.row(dof).transpose());
        PiecewisePolynomial<double> acceleration_squared = spline.derivative(2);
        acceleration_squared *= acceleration_squared;
        PiecewisePolynomial<double> acceleration_squared_integral = acceleration_squared.integral();
        objective_value += acceleration_squared_integral.scalarValue(spline.getEndTime()) - acceleration_squared_integral.scalarValue(spline.getStartTime());
      } catch (ConstraintMatrixSingularError&) {
        valid_solution = false;
      }
    }

    if (valid_solution && objective_value < min_objective_value) {
      best_segment_times = segment_times;
      min_objective_value = objective_value;
    }

    // Advance grid search counter or terminate, counting from
    // the latest t_index, and on overflow carrying to the
    // next lowest t_index and resetting to the new value of that
    // next lowest t_index. (since times must always be in order!)
    t_indices[num_knots - 1]++;
    // carry, except for the lowest place, which we
    // use to detect doneness.
    for (size_t i = num_knots - 1; i > 0; i--) {
      if ((i == num_knots - 1 && t_indices[i] >= GRID_STEPS) || (i < num_knots - 1 && t_indices[i] >= t_indices[i + 1])) {
        t_indices[i - 1]++;
        t_indices[i] = t_indices[i - 1] + 1;
      }
    }
  }

  for (mwSize dof = 0; dof < ndof; dof++) {
    PiecewisePolynomial<double> spline = nWaypointCubicSpline(best_segment_times, xs(dof, 0), xd0[dof], xs(dof, num_segments), xdf[dof], xi.row(dof).transpose());
    for (mwSize segment_index = 0; segment_index < spline.getNumberOfSegments(); segment_index++) {
      for (mwSize coefficient_index = 0; coefficient_index < num_coeffs_per_segment; coefficient_index++) {
        mwSize sub[] = { dof, segment_index, num_coeffs_per_segment - coefficient_index - 1 }; // Matlab's reverse coefficient indexing...
        *(mxGetPr(plhs[0]) + sub2ind(3, dims, sub)) = spline.getPolynomial(static_cast<int>(segment_index)).getCoefficients()[coefficient_index];
      }
    }
  }
  plhs[1] = stdVectorToMatlab(best_segment_times);

  if (nlhs > 2)
    plhs[2] = mxCreateDoubleScalar(min_objective_value);
}


#ifndef MP_PAGERANK_HPP
#define MP_PAGERANK_HPP

#include "defs.hpp"
#include "tools/iter.hpp"

#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_concepts.hpp>

namespace mp {

namespace detail {

/*
 * Params:
 *  g            - The bidirectional Graph (read-only).
 *  from_rank    - The page-rank of the last step (read-only).
 *  to_rank      - The page-rank of the next iteration (write-only).
 *  damping      - The damping factor.
 *  sinks        - A Collection of sinks (vertices without outgoing edges; read-only).
 *  weight       - A mapping from edge-descriptor to edge-weight (read-only).
 *  total_weight - A mapping from vertex-descriptor to the sum of all edge-weights beginning at that vertex (read-only).
 */
template<typename Graph, typename RankMap1, typename RankMap2, typename SinkCollection, typename EdgeWeightMap, typename TotalWeightMap>
void page_rank_step(const Graph &g,
                    RankMap1 from_rank, RankMap2 to_rank, typename boost::property_traits<RankMap1>::value_type damping,
                    const SinkCollection &sinks, EdgeWeightMap weight, TotalWeightMap total_weight)
{
    using rank_type = typename boost::property_traits<RankMap1>::value_type;

    // Iterate over all vertices and their incoming edges.
    for (auto vertex : make_iter_range(vertices(g))) {
        rank_type rank(0);

        // Consider all incoming edges.
        // Sources of these edges give the current vertex a part of their
        // rank, according to the edge's weight.
        for(auto edge : make_iter_range(in_edges(vertex, g)))  {
            auto vertex_source = source(edge, g);

            // Using this coefficient instead of 1/out_degree(vertex_source)
            // to support weighted edges.
            rank_type coeff = rank_type(get(weight, edge)) / rank_type(get(total_weight, vertex_source));
            rank += get(from_rank, vertex_source) * coeff;
        }

        // Treat all "sinks" as incoming edges, too.
        for (auto sink : sinks) {
            // Sinks distribute their rank evenly.
            rank += get(from_rank, sink) / num_vertices(g);
        }

        rank = ((rank_type(1) - damping) / num_vertices(g)) + damping * rank;
        put(to_rank, vertex, rank);
    }
}

} // namespace datail

/**
  * Computes PageRank for the given Graph.
  *
  * The algorithm will run until the StopPredicate returns true.
  *
  * \param g
  *      The Graph
  * \param[out] rank
  *      A map from vertex to rank. Will store the output once the function returns.
  * \param damping
  *     The damping factor. Between 0 and 1.
  * \param weight
  *     A map from edge descriptor to edge weight. Values will be normalized by this function.
  * \param stop
  *     The predicate.
  */
// Vertex index: http://www.boost.org/doc/libs/1_56_0/libs/graph/doc/faq.html (item 5)
template<typename Graph, typename RankMap, typename EdgeWeightMap, typename StopPredicate>
void page_rank(const Graph &g,
               RankMap rank, typename boost::property_traits<RankMap>::value_type damping,
               EdgeWeightMap weight,
               StopPredicate &&stop)
{
    using graph_traits       = boost::graph_traits<Graph>;
    using edge_descriptor    = typename graph_traits::edge_descriptor;
    using vertex_descriptor  = typename graph_traits::vertex_descriptor;
    using rank_type          = typename boost::property_traits<RankMap>::value_type;
    using weight_type        = typename boost::property_traits<EdgeWeightMap>::value_type;

    BOOST_CONCEPT_ASSERT(( boost::BidirectionalGraphConcept<Graph> ));
    BOOST_CONCEPT_ASSERT(( boost::ReadWritePropertyMapConcept<RankMap, vertex_descriptor> ));
    BOOST_CONCEPT_ASSERT(( boost::ReadablePropertyMapConcept<EdgeWeightMap, edge_descriptor> ));

    // The start rank of every vertex is 1/n
    // where n is the number of vertices.
    {
        rank_type start(rank_type(1) / num_vertices(g));

        for (auto vertex : make_iter_range(vertices(g))) {
            put(rank, vertex, start);
        }
    }

    auto indices = get(boost::vertex_index, g);

    // A second rank_map to store the ranks of the previous or next iteration.
    std::vector<rank_type> aux_rank_vector(num_vertices(g));
    auto aux_rank = boost::make_iterator_property_map(
                aux_rank_vector.begin(), indices);

    // For every vertex, compute the total of its outgoing edges' weights.
    std::vector<weight_type> total_weight_vector(num_vertices(g));
    auto total_weight = boost::make_iterator_property_map(
                total_weight_vector.begin(), indices);
    {
        for (auto vertex : make_iter_range(vertices(g))) {
            weight_type total(0);

            for(auto edge : make_iter_range(out_edges(vertex, g))) {
                total += get(weight, edge);
            }

            put(total_weight, vertex, total);
        }
    }

    // Collection of "sinks" - vertices that have no outgoing edges.
    std::vector<vertex_descriptor> sinks;
    {
        for (auto vertex : make_iter_range(vertices(g))) {
            if (out_degree(vertex, g) == 0) {
                sinks.push_back(vertex);
            }
        }
    }

    bool toggle = true;
    while (1) {
        if (toggle) {
            if (stop(rank, g)) {
                break;
            }

            detail::page_rank_step(g, rank, aux_rank, damping, sinks, weight, total_weight);
        } else {
            if (stop(aux_rank, g)) {
                break;
            }

            detail::page_rank_step(g, aux_rank, rank, damping, sinks, weight, total_weight);
        }

        toggle = !toggle;
    }

    if (!toggle) {
        // Copy results from "aux_rank" to "rank".
        for (auto vertex : make_iter_range(vertices(g))) {
            put(rank, vertex, get(aux_rank, vertex));
        }
    }
}

} // namespace mp

#endif // MP_PAGERANK_HPP


#include <cstdlib>
#include <iostream>
#include <complex>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <Eigen/Core>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/eigen/vector.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

typedef double real;
typedef std::complex<real> complex;

int main(int argc, char *argv[]) {
  {
    typedef ublas::vector<real> vector;
    typedef ublas::matrix<real> matrix;
    typedef vector::size_type size_type;
    rand_normal<real>::reset();
    size_type n=8;
    vector v1(n), v2(n);
    for (size_type i=0; i<n; ++i)
      v1(i)=rand_normal<real>::get();
    for (size_type i=0; i<n; ++i)
      v2(i)=rand_normal<real>::get();
    matrix M1(n, n), M2(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i) {
    	M1(i, j)=0;
    	M2(i, j)=0;
      }
    ublas::matrix_column<matrix> mc(M1, 2);
    ublas::matrix_row<matrix> mr(M2, 3);
    mc=v1;
    mr=v2;
    std::cout << "ublas using vectors    : dot(v1, v2) = " << ublas::inner_prod(v1, v2) << '\n'
	      << "blas using vectors     : dot(v1, v2) = " << blas::dot(v1, v2) << '\n'
	      << "blas using cols & rows : dot(v1, v2) = " << blas::dot(mc, mr) << '\n'
	      << "blas using vectors     : dotu(v1, v2) = " << blas::dotu(v1, v2) << '\n'
	      << "blas using cols & rows : dotu(v1, v2) = " << blas::dotu(mc, mr) << '\n';
  }
  {
    typedef ublas::vector<complex> vector;
    typedef ublas::matrix<complex> matrix;
    typedef vector::size_type size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    vector v1(n), v2(n);
    for (size_type i=0; i<n; ++i)
      v1(i)=rand_normal<complex>::get();
    for (size_type i=0; i<n; ++i)
      v2(i)=rand_normal<complex>::get();
    matrix M1(n, n), M2(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i) {
    	M1(i, j)=0;
    	M2(i, j)=0;
      }
    ublas::matrix_column<matrix> mc(M1, 2);
    ublas::matrix_row<matrix> mr(M2, 3);
    mc=v1;
    mr=v2;
    std::cout << "ublas using vectors    : dot(v1, v2) = " << ublas::inner_prod(v1, v2) << '\n'
	      << "blas using vectors     : dot(v1, v2) = " << blas::dot(v1, v2) << '\n'
	      << "blas using cols & rows : dot(v1, v2) = " << blas::dot(mc, mr) << '\n'
	      << "blas using vectors     : dotu(v1, v2) = " << blas::dotu(v1, v2) << '\n'
	      << "blas using cols & rows : dotu(v1, v2) = " << blas::dotu(mc, mr) << '\n';
  }
  {
    typedef Eigen::Matrix<real, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<real>::reset();
    size_type n=8;
    vector v1(n), v2(n);
    for (size_type i=0; i<n; ++i)
      v1(i)=rand_normal<real>::get();
    for (size_type i=0; i<n; ++i)
      v2(i)=rand_normal<real>::get();
    matrix M1(n, n), M2(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i) {
    	M1(i, j)=0;
    	M2(i, j)=0;
      }
    auto mc=M1.col(2);
    auto mr=M2.row(3);
    mc=v1;
    mr=v2;
    std::cout << "eigen using vectors    : dot(v1, v2) = " << v1.transpose()*v2 << '\n'
	      << "blas using vectors     : dot(v1, v2) = " << blas::dot(v1, v2) << '\n'
	      << "blas using cols & rows : dot(v1, v2) = " << blas::dot(mc, mr) << '\n'
	      << "blas using vectors     : dotu(v1, v2) = " << blas::dotu(v1, v2) << '\n'
	      << "blas using cols & rows : dotu(v1, v2) = " << blas::dotu(mc, mr) << '\n';
  }
  {
    typedef Eigen::Matrix<complex, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<complex, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    vector v1(n), v2(n);
    for (size_type i=0; i<n; ++i)
      v1(i)=rand_normal<complex>::get();
    for (size_type i=0; i<n; ++i)
      v2(i)=rand_normal<complex>::get();
    matrix M1(n, n), M2(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i) {
    	M1(i, j)=0;
    	M2(i, j)=0;
      }
    auto mc=M1.col(2);
    auto mr=M2.row(3);
    mc=v1;
    mr=v2;
    std::cout << "eigen using vectors    : dot(v1, v2) = " << v1.transpose()*v2 << '\n'
	      << "blas using vectors     : dot(v1, v2) = " << blas::dot(v1, v2) << '\n'
	      << "blas using cols & rows : dot(v1, v2) = " << blas::dot(mc, mr) << '\n'
	      << "blas using vectors     : dotu(v1, v2) = " << blas::dotu(v1, v2) << '\n'
	      << "blas using cols & rows : dotu(v1, v2) = " << blas::dotu(mc, mr) << '\n';
  }
  return EXIT_SUCCESS;
}