#pragma once

#include <Core/RaCore.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

// We use Eigen::Matrix as base type to represent matrices in OpenMesh.
// This is not standard, and several namespace functions are not available in OpenMesh
// for Eigen::Matrix.
// We need these functions to be defined before including OpenMesh, otherwise some
// compilers (e.g. MSVC) will not be able to compile the code.
namespace OpenMesh {

template <typename Derived>
typename Derived::Scalar dot( Eigen::MatrixBase<Derived> const& v1,
                              Eigen::MatrixBase<Derived> const& v2 ) {
    return v1.dot( v2 );
}

template <typename Derived>
typename Derived::Scalar norm( Eigen::MatrixBase<Derived> const& v1 ) {
    return v1.norm();
}

template <typename Derived>
Derived normalize( Eigen::MatrixBase<Derived>& v1 ) {
    v1.normalize();
    return v1;
}

template <typename Derived>
void vectorize( Eigen::MatrixBase<Derived>& v1, Scalar a ) {
    v1.setConstant( a );
}

template <typename Derived>
typename Derived::Scalar sqrnorm( Eigen::MatrixBase<Derived> const& v1 ) {
    return v1.squaredNorm();
}

template <typename Derived>
typename Derived::template cross_product_return_type<Derived>::type
cross( Eigen::MatrixBase<Derived> const& v1, Eigen::MatrixBase<Derived> const& v2 ) {
    return v1.cross( v2 );
}

} // namespace OpenMesh

#include <Core/Types.hpp>

#include <OpenMesh/Core/Mesh/Traits.hh>
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>

namespace OpenMesh {
template <>
struct vector_traits<Ra::Core::Vector3> {
    using vector_type         = Ra::Core::Vector3;
    using value_type          = Scalar;
    static const size_t size_ = 3;
    static size_t size() { return 3; }
};

} // namespace OpenMesh


/*
 test/eval/static_eval_erc.cpp

 Copyright 2013 Karsten Ahnert

 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 <gpcxx/eval/static_eval_erc.hpp>
#include <gpcxx/tree/basic_tree.hpp>

#include <boost/fusion/include/make_vector.hpp>
#include <boost/math/constants/constants.hpp>

#include <gtest/gtest.h>

#include <array>
#include <sstream>
#include <cmath>
#include <random>

#define TESTNAME basic_eval_erc_tests

using namespace std;
namespace fusion = boost::fusion;

// class basic_eval_erc_test : public ::testing::Test
// {
// protected:
//
//     virtual void SetUp( void )
//     {
//     }
//
//     virtual void TearDown( void )
//     {
//     }
// };

TEST( TESTNAME , TestCase )
{
    typedef double value_type;
    typedef std::array< double , 2 > context_type;
    typedef std::string symbol_type;

    auto eval = gpcxx::make_static_eval_erc< value_type , symbol_type , context_type >(
        fusion::make_vector( 1.0 , std::normal_distribution<>( 0.0 , 1.0 ) ) ,
        fusion::make_vector(
                 fusion::make_vector( "1" , []( context_type const& t ) { return 1.0; } )
               , fusion::make_vector( "2" , []( context_type const& t ) { return 2.0; } )
               , fusion::make_vector( "x" , []( context_type const& t ) { return t[0]; } )
               , fusion::make_vector( "y" , []( context_type const& t ) { return t[1]; } )
                ) ,
        fusion::make_vector(
                 fusion::make_vector( "sin" , []( double v ) -> double { return std::sin( v ); } )
               , fusion::make_vector( "cos" , []( double v ) -> double { return std::cos( v ); } )
                ) ,
        fusion::make_vector(
                 fusion::make_vector( "plus" , std::plus< double >() )
               , fusion::make_vector( "minus" , std::minus< double >() )
                ) );

    typedef decltype( eval ) eval_type;
    typedef eval_type::node_attribute_type node_attribute_type;
    typedef gpcxx::basic_tree< node_attribute_type > tree_type;

    std::mt19937 rng;

    EXPECT_EQ( eval.get_terminal_symbols() , std::vector< std::string >( { "1" , "2" , "x" , "y" } ) );
    EXPECT_EQ( eval.get_unary_symbols() , std::vector< std::string >( { "sin" , "cos" } ) );
    EXPECT_EQ( eval.get_binary_symbols() , std::vector< std::string >( { "plus" , "minus" } ) );

    auto terminal_dist = eval.get_terminal_symbol_distribution();
    auto unary_dist = eval.get_unary_symbol_distribution();
    auto binary_dist = eval.get_binary_symbol_distribution();

    node_attribute_type v1 = terminal_dist( rng );
    node_attribute_type v2 = unary_dist( rng );
    node_attribute_type v3 = binary_dist( rng );

    tree_type tree;
    auto c1 = tree.insert_below( tree.root() , "plus" );
    auto c2 = tree.insert_below( c1 , "sin" );
    /* auto c3 = */ tree.insert_below( c2 , "x" );
    auto c4 = tree.insert_below( c1 , "minus" );
    /* auto c5 = */ tree.insert_below( c4 , 12.2 );
    /* auto c6 = */ tree.insert_below( c4 , "y" );

    context_type context = {{ 0.5 * boost::math::double_constants::pi , 2.0  }};
    value_type val1 = eval( tree , context );
    EXPECT_DOUBLE_EQ( val1 , 11.2 );
}


/*
Copyright 2015 Rogier van Dalen.

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.
*/

#define BOOST_TEST_MODULE test_product_hash
#include "utility/test/boost_unit_test.hpp"

#include "math/product.hpp"

#include <boost/functional/hash.hpp>

#include "range/tuple.hpp"
#include "range/std/container.hpp"

#include "math/arithmetic_magma.hpp"
#include "math/cost.hpp"
#include "math/sequence.hpp"

#include "math/check/check_hash.hpp"

BOOST_AUTO_TEST_SUITE (test_suite_product_hash)

template <class Inverse> void check_hash_homogeneous() {
    typedef math::product <math::over <float, math::cost <double>>, Inverse>
        product;
    std::vector <product> examples;
    examples.push_back (math::zero <product>());
    examples.push_back (product (0, math::cost <double> (0)));
    examples.push_back (product (0, math::cost <double> (4)));
    examples.push_back (product (0, math::zero <math::cost <double>>()));
    examples.push_back (product (1, math::cost <double> (0)));
    examples.push_back (product (1, math::cost <double> (4)));
    examples.push_back (product (1, math::zero <math::cost <double>>()));

    math::check_hash (examples);
}

template <class Direction, class Inverse> void check_hash_heterogeneous() {
    typedef math::product <math::over <
            float, math::sequence <char, Direction>>, Inverse>
        product;
    typedef math::product <math::over <
            float, math::optional_sequence <char, Direction>>, Inverse>
        optional_product;
    typedef math::product <math::over <
            float, math::sequence_annihilator <char, Direction>>, Inverse>
        annihilator_product;

    auto examples = range::make_tuple (
        annihilator_product (0, math::sequence_annihilator <char, Direction>()),
        annihilator_product (4, math::sequence_annihilator <char, Direction>()),
        optional_product (0, math::optional_sequence <char, Direction>()),
        optional_product (1, math::optional_sequence <char, Direction> ('a')),
        product (5, math::sequence <char, Direction> (std::string ("abc"))));

    math::check_hash (examples);
    math::check_cast_hash <product> (examples);
}

BOOST_AUTO_TEST_CASE (test_hash) {
    check_hash_homogeneous <math::with_inverse<>>();
    check_hash_homogeneous <math::with_inverse <math::callable::plus>>();
    check_hash_homogeneous <math::with_inverse <math::callable::times>>();

    check_hash_heterogeneous <math::left, math::with_inverse<>>();
    check_hash_heterogeneous <math::left,
        math::with_inverse <math::callable::plus>>();
    check_hash_heterogeneous <math::left,
        math::with_inverse <math::callable::times>>();

    check_hash_heterogeneous <math::right, math::with_inverse<>>();
    check_hash_heterogeneous <math::right,
        math::with_inverse <math::callable::plus>>();
    check_hash_heterogeneous <math::right,
        math::with_inverse <math::callable::times>>();
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once
#ifndef DSFMT_CALC_JUMP_HPP
#define DSFMT_CALC_JUMP_HPP
/**
 * @file dSFMT-calc-jump.hpp
 *
 * @brief functions for calculating jump polynomial.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <iostream>
#include <iomanip>
#include <sstream>
#include <NTL/GF2X.h>

namespace dsfmt {
/**
 * converts polynomial to string for convenient use in C language.
 * @param x output string
 * @param polynomial input polynomial
 */
    static inline void polytostring(std::string& x, NTL::GF2X& polynomial)
    {
	using namespace NTL;
	using namespace std;

	long degree = deg(polynomial);
	int buff;
	stringstream ss;
	for (int i = 0; i <= degree; i+=4) {
	    buff = 0;
	    for (int j = 0; j < 4; j++) {
		if (IsOne(coeff(polynomial, i + j))) {
		    buff |= 1 << j;
		} else {
		    buff &= (0x0f ^ (1 << j));
		}
	    }
	    ss << hex << buff;
	}
	ss << flush;
	x = ss.str();
    }

/**
 * converts string to polynomial
 * @param str string
 * @param poly output polynomial
 */
    static inline void stringtopoly(NTL::GF2X& poly, std::string& str)
    {
	using namespace NTL;
	using namespace std;

	stringstream ss(str);
	char c;
	long p = 0;
	clear(poly);
	while(ss) {
	    ss >> c;
	    if (!ss) {
		break;
	    }
	    if (c >= 'a') {
		c = c - 'a' + 10;
	    } else {
		c = c - '0';
	    }
	    for (int j = 0; j < 4; j++) {
		if (c & (1 << j)) {
		    SetCoeff(poly, p, 1);
		} else {
		    SetCoeff(poly, p, 0);
		}
		p++;
	    }
	}
    }

/**
 * calculate the jump polynomial.
 * SFMT generates 4 32-bit integers from one internal state.
 * @param jump_str output string which represents jump polynomial.
 * @param step jump step of internal state
 * @param characteristic polynomial
 */
    static inline void calc_jump(std::string& jump_str,
				 NTL::ZZ& step,
				 NTL::GF2X& characteristic)
    {
	using namespace NTL;
	using namespace std;
	GF2X jump;
	PowerXMod(jump, step, characteristic);
	polytostring(jump_str, jump);
    }
}
#endif


#pragma once

#include <Core/RaCore.hpp>
#include <Eigen/Core>

namespace Ra {
namespace Core {
namespace Geometry {
struct Frustum {
  public:
    enum FACES : int {
        FRONT  = 0, // Near plane
        BACK   = 1, // Far plane
        TOP    = 2,
        BOTTOM = 3,
        LEFT   = 4,
        RIGHT  = 5
    };

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    /// Default constructor
    explicit inline Frustum( const Eigen::Matrix<Scalar, 4, 4>& mvp ) {
        // Near clipping plane.
        m_planes[FRONT] = mvp.row( 3 ) + mvp.row( 2 );
        // Far clipping plane.
        m_planes[BACK] = mvp.row( 3 ) - mvp.row( 2 );
        // Top clipping plane.
        m_planes[TOP] = mvp.row( 3 ) - mvp.row( 1 );
        // Bottom clipping plane.
        m_planes[BOTTOM] = mvp.row( 3 ) + mvp.row( 1 );
        // Left clipping plane.
        m_planes[LEFT] = mvp.row( 3 ) + mvp.row( 0 );
        // Right clipping plane.
        m_planes[RIGHT] = mvp.row( 3 ) - mvp.row( 0 );
    }

    inline const Eigen::Matrix<Scalar, 4, 1>& getPlane( uint p ) const { return m_planes[p]; }

  public:
    /// Clipping planes
    std::array<Eigen::Matrix<Scalar, 4, 1>, 6> m_planes;
};
} // namespace Geometry
} // namespace Core
} // namespace Ra


#include "matchers/matcher.h"
#include "opencv2/opencv.hpp"
#include "camera/CameraModel.h"
#include "types/Frame.h"
//#include "extractors/FASTextractor.h"
#include "extractors/ORBextractor.h"
#include "types/Map.h"
#include "types/MapPoint.h"
#include "mapping/LocalBA.h"
#include "Viewer.h"
#include <memory>
#include <Eigen/Geometry>
#include <thread>
#include <sophus/se3.hpp>
#include <iostream>
#include <stdint.h>
#include <unordered_set>
#include "g2o/stuff/sampler.h"
#include<suitesparse/cholmod.h>

using namespace TRACKING_BENCH;

// simple direct
using namespace std;

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

// Camera intrinsics
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// baseline
double baseline = 0.573;
// paths
string left_file = "/home/lyc/share/slambook2-master/slambook2-master/ch8/left.png";
string disparity_file = "/home/lyc/code/slam_bench/trackingBench-SLAM/data/disparity.png";

// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

/// class for accumulator jacobians in parallel
class JacobianAccumulator {
public:
    JacobianAccumulator(
            const cv::Mat &img1_,
            const cv::Mat &img2_,
            const VecVector2d &px_ref_,
            const vector<double> depth_ref_,
            Sophus::SE3d &T21_) :
            img1(img1_), img2(img2_), px_ref(px_ref_), depth_ref(depth_ref_), T21(T21_) {
        projection = VecVector2d(px_ref.size(), Eigen::Vector2d(0, 0));
    }

    /// accumulate jacobians in a range
    void accumulate_jacobian(const cv::Range &range);

    /// get hessian matrix
    Matrix6d hessian() const { return H; }

    /// get bias
    Vector6d bias() const { return b; }

    /// get total cost
    double cost_func() const { return cost; }

    /// get projected points
    VecVector2d projected_points() const { return projection; }

    /// reset h, b, cost to zero
    void reset() {
        H = Matrix6d::Zero();
        b = Vector6d::Zero();
        cost = 0;
    }

private:
    const cv::Mat &img1;
    const cv::Mat &img2;
    const VecVector2d &px_ref;
    const vector<double> depth_ref;
    Sophus::SE3d &T21;
    VecVector2d projection; // projected points

    std::mutex hessian_mutex;
    Matrix6d H = Matrix6d::Zero();
    Vector6d b = Vector6d::Zero();
    double cost = 0;
};

/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationMultiLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3d &T21
);

/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3d &T21
);

// bilinear interpolation
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
    // boundary check
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= img.cols) x = img.cols - 1;
    if (y >= img.rows) y = img.rows - 1;
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
//    std::cout<<"f1: "<<(1 - xx) * (1 - yy)<<" x1: "<<(int)data[0]<<std::endl;
//    std::cout<<"f2: "<<xx * (1 - yy)<<" x2: "<<(int)data[1]<<std::endl;
//    std::cout<<"f3: "<<(1 - xx) * yy<<" x3: "<<(int)data[img.step]<<std::endl;
//    std::cout<<"f4: "<<xx * yy<<" x4: "<<(int)data[img.step + 1]<<std::endl;
    return float(
            (1 - xx) * (1 - yy) * data[0] +
            xx * (1 - yy) * data[1] +
            (1 - xx) * yy * data[img.step] +
            xx * yy * data[img.step + 1]
    );
}

void DirectPoseEstimationSingleLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,//new
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3d &T21) {

    const int iterations = 10;
    double cost = 0, lastCost = 0;
    auto t1 = chrono::steady_clock::now();
    JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);

    for (int iter = 0; iter < iterations; iter++) {
        jaco_accu.reset();
        cv::parallel_for_(cv::Range(0, px_ref.size()),
                          std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, std::placeholders::_1));
        Matrix6d H = jaco_accu.hessian();
        Vector6d b = jaco_accu.bias();

        // solve update and put it into estimation
        Vector6d update = H.ldlt().solve(b);
        T21 = Sophus::SE3d::exp(-update) * T21;
        cost = jaco_accu.cost_func();

        if (std::isnan(update[0])) {
            // sometimes occurred when we have a black or white patch and H is irreversible
            cout << "update is nan" << endl;
            break;
        }
        if (iter > 0 && cost > lastCost) {
            cout << "cost increased: " << cost << ", " << lastCost << endl;
            break;
        }
        if (update.norm() < 1e-3) {
            // converge
            break;
        }

        lastCost = cost;
        cout << "iteration: " << iter << ", cost: " << cost << endl;
    }

    cout << "T21 = \n" << T21.matrix() << endl;
    auto t2 = chrono::steady_clock::now();
    auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "direct method for single layer: " << time_used.count() << endl;

    // plot the projected pixels here
    cv::Mat img2_show;
    cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
    VecVector2d projection = jaco_accu.projected_points();
    for (size_t i = 0; i < px_ref.size(); ++i) {
        auto p_ref = px_ref[i];
        auto p_cur = projection[i];
        if (p_cur[0] > 0 && p_cur[1] > 0) {
            cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
                     cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("current", img2_show);
    cv::waitKey(0);
}

void JacobianAccumulator::accumulate_jacobian(const cv::Range &range) {

    // parameters
    const int half_patch_size = 2;
    int cnt_good = 0;
    Matrix6d hessian = Matrix6d::Zero();
    Vector6d bias = Vector6d::Zero();
    double cost_tmp = 0;

    for (size_t i = range.start; i < range.end; i++) {

        // compute the projection in the second image
        Eigen::Vector3d point_ref =
                depth_ref[i] * Eigen::Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
        Eigen::Vector3d point_cur = T21 * point_ref;
        if (point_cur[2] < 0)   // depth invalid
            continue;

        float u = fx * point_cur[0] / point_cur[2] + cx, v = fy * point_cur[1] / point_cur[2] + cy;
        if (u < half_patch_size || u > img2.cols - half_patch_size || v < half_patch_size ||
            v > img2.rows - half_patch_size)
            continue;

        projection[i] = Eigen::Vector2d(u, v);
        double X = point_ref[0], Y = point_ref[1], Z = point_ref[2],
                Z2 = Z * Z, Z_inv = 1.0 / Z, Z2_inv = Z_inv * Z_inv;
        cnt_good++;

        // and compute error and jacobian
        for (int y = -half_patch_size; y < half_patch_size; y++)
            for (int x = -half_patch_size; x < half_patch_size; x++) {

                double error = GetPixelValue(img2, u + x, v + y) -
                               GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y);

                Matrix26d J_pixel_xi;
                Eigen::Vector2d J_img_pixel;

                J_pixel_xi(0, 0) = fx * Z_inv;
                J_pixel_xi(0, 1) = 0;
                J_pixel_xi(0, 2) = -fx * X * Z2_inv;
                J_pixel_xi(0, 3) = -fx * X * Y * Z2_inv;
                J_pixel_xi(0, 4) = fx + fx * X * X * Z2_inv;
                J_pixel_xi(0, 5) = -fx * Y * Z_inv;

                J_pixel_xi(1, 0) = 0;
                J_pixel_xi(1, 1) = fy * Z_inv;
                J_pixel_xi(1, 2) = -fy * Y * Z2_inv;
                J_pixel_xi(1, 3) = -fy - fy * Y * Y * Z2_inv;
                J_pixel_xi(1, 4) = fy * X * Y * Z2_inv;
                J_pixel_xi(1, 5) = fy * X * Z_inv;

                J_img_pixel = Eigen::Vector2d(
                        0.5 * (GetPixelValue(img1, px_ref[i][0] + 1 + x, px_ref[i][1] + y) - GetPixelValue(img1, px_ref[i][0] - 1 + x, px_ref[i][1] + y)),
                        0.5 * (GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + 1 + y) - GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] - 1 + y))
                );

                // total jacobian
                Vector6d J = -1.0 * (J_img_pixel.transpose() * J_pixel_xi).transpose();

                hessian += J * J.transpose();
                bias += -error * J;
                cost_tmp += error * error;
                std::cout<<" H "<<J * J.transpose()<<std::endl;
                std::cout<<" b "<<-error * J<<std::endl;
            }
        std::cout<<"H: "<<hessian<<std::endl;
        std::cout<<"bias: "<<bias<<std::endl;
    }

    if (cnt_good) {
        // set hessian, bias and cost
        unique_lock<mutex> lck(hessian_mutex);
        H += hessian;
        b += bias;
        cost += cost_tmp / cnt_good;
    }
    //std::cout<<" mean cost: "<<std::endl;
}

void DirectPoseEstimationMultiLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3d &T21)
{
    // parameters
    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = {1.0, 0.5, 0.25, 0.125};

    // create pyramids
    vector<cv::Mat> pyr1, pyr2; // image pyramids
    for (int i = 0; i < pyramids; i++) {
        if (i == 0) {
            pyr1.push_back(img1);
            pyr2.push_back(img2);
        } else {
            cv::Mat img1_pyr, img2_pyr;
            cv::resize(pyr1[i - 1], img1_pyr,
                       cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
            cv::resize(pyr2[i - 1], img2_pyr,
                       cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));
            pyr1.push_back(img1_pyr);
            pyr2.push_back(img2_pyr);
        }
    }

    double fxG = fx, fyG = fy, cxG = cx, cyG = cy;  // backup the old values
    for (int level = pyramids - 1; level >= 0; level--) {
        VecVector2d px_ref_pyr; // set the keypoints in this pyramid level
        for (auto &px: px_ref) {
            px_ref_pyr.push_back(scales[level] * px);
        }

        // scale fx, fy, cx, cy in different pyramid levels
        fx = fxG * scales[level];
        fy = fyG * scales[level];
        cx = cxG * scales[level];
        cy = cyG * scales[level];
        DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
    }

}

void LoadImages(const string &strPathToSequence, vector<string> &vstrImageLeft,
                vector<string> &vstrImageRight, vector<double> &vTimestamps)
{
    ifstream fTimes;
    string strPathTimeFile = strPathToSequence + "/times.txt";
    fTimes.open(strPathTimeFile.c_str());
    while(!fTimes.eof())
    {
        string s;
        getline(fTimes,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            double t;
            ss >> t;
            vTimestamps.push_back(t);
        }
    }

    string strPrefixLeft = strPathToSequence + "/image_0/";
    string strPrefixRight = strPathToSequence + "/image_1/";

    const int nTimes = vTimestamps.size();
    vstrImageLeft.resize(nTimes);
    vstrImageRight.resize(nTimes);

    for(int i=0; i<nTimes; i++)
    {
        stringstream ss;
        ss << setfill('0') << setw(6) << i;
        vstrImageLeft[i] = strPrefixLeft + ss.str() + ".png";
        vstrImageRight[i] = strPrefixRight + ss.str() + ".png";
    }
}

void LoadKittiGroundTruth(const string& file, vector<Eigen::Matrix3d>& R, vector<Eigen::Vector3d>& t)
{
    ifstream fTimes;
    fTimes.open(file.c_str());
    R.reserve(1000);
    t.reserve(1000);

    while(!fTimes.eof())
    {
        Eigen::Vector3d t0;
        Eigen::Matrix3d R0;

        string s;
        for (int i = 0; i < 3; i ++)
        {
            for (int j = 0;j < 3; j ++)
            {
                getline(fTimes,s,' ');
                R0(i, j) =  std::atof(s.c_str());
            }
            if (i == 2)
                getline(fTimes,s);
            else
                getline(fTimes,s,' ');
            t0(i) = std::atof(s.c_str());
        }
        R.emplace_back(R0.transpose());
        t.emplace_back(- R0.transpose() * t0);
        //getline(fTimes,s);
    }
}

void test_projection()
{

    // Load Images Paths
    std::vector<string> imageLeft;
    std::vector<string> imageRight;
    std::vector<double> timeStamp;
    LoadImages( "/media/lyc/ 其他/dataset/sequences/00",
                imageLeft, imageRight, timeStamp);
    string left_file = "/home/lyc/code/slam_bench/trackingBench-SLAM/data/left.png";
    string right_file = "/home/lyc/code/slam_bench/trackingBench-SLAM/data/right.png";
    string disparity_file = "/home/lyc/code/slam_bench/trackingBench-SLAM/data/disparity.png";
    // Load ground truth
    cv::Mat disparity = cv::imread(disparity_file, 0);
    std::vector<Eigen::Matrix3d> gtR;
    std::vector<Eigen::Vector3d> gtT;
    LoadKittiGroundTruth("/home/lyc/share/data_odometry_poses/dataset/poses/00.txt",
                         gtR, gtT);

    //
    // 内参
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 间距
    double d = 0.573;

    auto extractor_ptr = std::make_shared<ORBExtractor>();
    auto local_ba = std::make_shared<LocalBA>();

    std::shared_ptr<Frame> key_frame, last_frame;
    auto matcher_ptr = std::make_shared<Matcher>();
    auto map_ptr = std::make_shared<TRACKING_BENCH::Map>();

    // viewer
    auto viewer = std::make_shared<Viewer>();
    viewer->SetCameraPos(Eigen::Matrix4f::Identity());
    viewer->Run();

    // view gt pos
    std::vector<cv::KeyPoint> keyPoints_ref;
    std::vector<Eigen::Matrix4f> kfs_pos;

    // for images
    for (int i = 0;i < 2;i +=1)
    {
        cv::Mat imLeft, imRight, show;
        std::vector<cv::KeyPoint> keyPoints_r, keyPoints_l;
        cv::Mat descriptors_r, descriptors_l;
        // 0. read image to frame
        imLeft = cv::imread(imageLeft[i], CV_LOAD_IMAGE_UNCHANGED);
        static auto camera_ptr = std::make_shared<PinholeCamera>(imLeft.cols, imLeft.rows,fx, fy, cx, cy);
        cv::Mat out;

        auto cur_frame_ptr = std::make_shared<Frame>(imLeft, 0, 5, 0.6, camera_ptr);
        if(last_frame != nullptr)
        {
            cur_frame_ptr->SetPose(last_frame->GetPose());
        }
        else
        {
            Eigen::Matrix4f pos = Eigen::Matrix4f::Identity();
            pos.block<3, 3>(0, 0) = gtR.at(i).cast<float>();
            pos.block<3, 1>(0 ,3) = gtT.at(i).cast<float>();
            cur_frame_ptr->SetPose(pos);
            viewer->SetCameraPos(cur_frame_ptr->GetPoseInverse());
        }

        // 1. match points
        if (last_frame != nullptr)
        {
            // match with last frame
//            auto matches = matcher_ptr->searchByBow(cur_frame_ptr, cur_frame_ptr, true);
//            auto matches = matcher_ptr->searchByBF(cur_frame_ptr, key_frame, 0, 5, 10, 30);
//            auto matches = matcher_ptr->searchByViolence(cur_frame_ptr, key_frame, 0, 5, 50);
            // matches by optical flow
//            std::vector<cv::Point2f> pts;
//            std::vector<cv::KeyPoint> kps;
//            auto matches = matcher_ptr->searchByOPFlow(cur_frame_ptr, last_frame, pts, true, true);
//            kps.reserve(pts.size());
//            for (auto& pt:pts)
//            {
//                cv::KeyPoint kp;
//                kp.pt = pt;
//                kps.emplace_back(kp);
//            }
//            cur_frame_ptr->SetKeys(kps, cur_frame_ptr);
            // matches by projection frame

//            extractor_ptr->operator()(cur_frame_ptr->GetImagePyramid(),
//                                      cur_frame_ptr->GetScaleFactors(),
//                                      2000,
//                                      80,
//                                      30,
//                                      keyPoints_l,
//                                      descriptors_l);
//
//            cur_frame_ptr->SetKeys(keyPoints_l, cur_frame_ptr, descriptors_l);
//            cur_frame_ptr->AssignFeaturesToGrid();

            Eigen::Matrix4f pos = Eigen::Matrix4f::Identity();
            pos.block<3, 3>(0, 0) = gtR.at(i).cast<float>();
            pos.block<3, 1>(0 ,3) = gtT.at(i).cast<float>();
            pos(2, 3) = -0.85;
            cur_frame_ptr->SetPose(last_frame->GetPose());//pos);//last_frame->GetPose());

//            matcher_ptr->setProjectionParam(30, 50, 30, true, 30);
//            auto matches = matcher_ptr->searchByProjection(cur_frame_ptr, key_frame);

            // matches by projection map
//            matcher_ptr->setProjectionParam(30, 50, 30, true, 20);
//            auto matches = matcher_ptr->searchByProjection(map_ptr, cur_frame_ptr, 0.6);
//
//            // assign map point
//            for (auto &item:matches)
//            {
//                // last frame
////                shared_ptr<MapPoint> mp = last_frame->GetMapPoint(item.trainIdx);
//                // key frame
////                shared_ptr<MapPoint> mp = key_frame->GetMapPoint(item.trainIdx);
//                // map
//                shared_ptr<MapPoint> mp = map_ptr->GetAllMapPoints().at(item.trainIdx);
//                if (mp != nullptr)
//                    cur_frame_ptr->AddMapPoint(mp, item.queryIdx);
//            }
//
//            for (auto& m:matches)
//            {
//                m.trainIdx =  map_ptr->GetAllMapPoints().at(m.trainIdx)->GetReferenceFeature()->idxF;
//            }

            // direct
            matcher_ptr->setDirectParam(4, 0, 20, 20, 0.01);
            auto matches = matcher_ptr->searchByDirect(map_ptr, cur_frame_ptr, last_frame);
            // pnp
            cur_frame_ptr->SetPose(Eigen::Matrix4f::Identity());
            local_ba->PoseOptimization(cur_frame_ptr);
            std::cout<<"current pose Twc: "<<cur_frame_ptr->GetPoseInverse()<<std::endl;
            // feature align
            cv::Mat show2 = cur_frame_ptr->GetImagePyramid()[0].clone();
            cv::cvtColor(show2,show2,CV_GRAY2BGR);
            for (const auto& f:cur_frame_ptr->GetKeys())
            {
                cv::line(show2,
                         cv::Point((int)f->px.x(), (int)f->px.y()),
                         cv::Point((int)f->point->GetReferenceFeature()->px.x(), (int)f->point->GetReferenceFeature()->px.y()),
                         cv::Scalar(255,0,0), 2);
            }
            cv::imshow("image align", show2);
            cv::waitKey(0);

//            cv::drawMatches(imLeft,
//                            keyPoints_l,
//                            key_frame->GetImagePyramid().at(0),
//                            keyPoints_ref, matches, show);
//            cv::imshow("match", show);

            // 2. pose optimization
//            local_ba->PoseOptimization(cur_frame_ptr);

            std::cout<<"current pose Twc: "<<cur_frame_ptr->GetPoseInverse()<<std::endl;
            std::cout<<"ground truth pose Twc: "<<pos.inverse()<<std::endl;
        }

        kfs_pos.emplace_back(cur_frame_ptr->GetPoseInverse());
        viewer->SetKeyFrames(kfs_pos);
        if(i % 10 == 0)
        {// 3. key frame

            extractor_ptr->operator()(cur_frame_ptr->GetImagePyramid(),
                                      cur_frame_ptr->GetScaleFactors(),
                                      2000,
                                      80,
                                      30,
                                      keyPoints_l,
                                      descriptors_l);

            cur_frame_ptr->SetKeys(keyPoints_l, cur_frame_ptr, descriptors_l);
            cur_frame_ptr->AssignFeaturesToGrid();
            imRight = cv::imread(imageRight[i], CV_LOAD_IMAGE_UNCHANGED);
//            cv::imshow("right", imRight);
            static auto right_camera_ptr = std::make_shared<PinholeCamera>(imLeft.cols, imLeft.rows, fx, fy, cx, cy);
//            right_camera_ptr->UndistortImage(imRight, imRight);
            // 4. add map points by stereo
            auto right_frame_ptr = std::make_shared<Frame>(imRight, 0, 5, 0.6, right_camera_ptr);
//            extractor_ptr->operator()(cur_frame_ptr->GetImagePyramid(),
//                                      cur_frame_ptr->GetScaleFactors(),
//                                      2000,
//                                      80,
//                                      30,
//                                      keyPoints_r,
//                                      descriptors_r);
//            right_frame_ptr->SetKeys(keyPoints_r, right_frame_ptr, descriptors_r);

            auto depth = local_ba->AddMapPointsByStereo(cur_frame_ptr, right_frame_ptr, d*fx, fx);
            float sum_err = 0, cnt = 0;
            for (size_t j = 0; j < depth.size(); j ++)
            {
                if(depth[j] > 0)
                {

                    int u = (int)cur_frame_ptr->GetKey(j)->kp.pt.x;
                    int v = (int)cur_frame_ptr->GetKey(j)->kp.pt.y;

                    double disp = disparity.at<uchar>(v, u);
                    disp = (disp)/fx;
                    float err = abs(depth[j]/5 - d/disp);
                    sum_err += err;
                    cnt ++;

                    depth[j] = d/disp;

                    Eigen::Vector3f norm;
                    norm[0] = (u-cx)/fx;
                    norm[1] = (v-cy)/fy;
                    norm[2] = 1;

//                    const auto& pt = cur_frame_ptr->GetKey(j);
//                    norm = camera_ptr->Cam2World(pt->px);

                    const Eigen::Matrix3f& R = cur_frame_ptr->GetRotation();
                    Eigen::Vector3f t = cur_frame_ptr->GetTranslation();
//                    t.x() += 0.5;
                    auto mp = std::make_shared<MapPoint>(R * norm * depth[j] + t, map_ptr, cur_frame_ptr, cur_frame_ptr->GetKey(j), cur_frame_ptr->GetDescriptor(j));

                    cur_frame_ptr->AddMapPoint(mp, j);
                    map_ptr->AddMapPoint(mp);
                    map_ptr->AddKeyFrame(cur_frame_ptr);
                }
            }
            std::cout<<" error: "<<sum_err<<" mean: "<<sum_err/cnt<<" cnt: "<<cnt<<std::endl;
            viewer->SetMapPoints(map_ptr->GetAllMapPoints(), cur_frame_ptr->GetMapPointMatches());
            keyPoints_ref = keyPoints_l;
            key_frame = cur_frame_ptr;
        }
        last_frame = cur_frame_ptr;
        // visualization
        cv::waitKey(2);
    }
    for(int i = 0;i < gtR.size();i += 1)
    {
        Eigen::Matrix4f pos = Eigen::Matrix4f::Identity();
        pos.block<3, 3>(0, 0) = gtR.at(i).cast<float>().transpose();
        pos.block<3, 1>(0 ,3) = -  pos.block<3, 3>(0, 0) * gtT.at(i).cast<float>();
        kfs_pos.emplace_back(pos);
    }
    cv::waitKey(0);
    viewer->RequestFinish();
}

int test_direct()
{
    std::vector<string> imageLeft;
    std::vector<string> imageRight;
    std::vector<double> timeStamp;

    LoadImages( "/media/lyc/ 其他/dataset/sequences/00",
                imageLeft, imageRight, timeStamp);

    cv::Mat left_img = cv::imread(imageLeft[0], 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng;
    int nPoints = 1;
    int boarder = 2;
    VecVector2d pixels_ref;
    vector<double> depth_ref;
    for (int i = 0; i < nPoints; i++)
    {
        int x = 130;//rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = 73;//rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }
    // estimates 01~05.png's pose using this information
    Sophus::SE3d T_cur_ref;

    //for (int i = 1; i < 6; i++) {  // 1~10
    cv::Mat img = cv::imread(imageLeft[1], 0);
    // try single layer by uncomment this line
    // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
    DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
    //}
    return 0;
}

void test_direct2()
{
    std::vector<string> imageLeft;
    std::vector<string> imageRight;
    std::vector<double> timeStamp;

    LoadImages( "/media/lyc/ 其他/dataset/sequences/00",
                imageLeft, imageRight, timeStamp);

    auto matcher_ptr = std::make_shared<Matcher>();

    cv::Mat left_img = cv::imread(imageLeft[0], 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng;
    int nPoints = 2000;
    int boarder = 20;
    VecVector2d pixels_ref;
    vector<double> depth_ref;
    auto map_ptr = std::make_shared<TRACKING_BENCH::Map>();

    auto camera_ptr = std::make_shared<PinholeCamera>(left_img.cols, left_img.rows,fx, fy, cx, cy);
    cv::Mat img = cv::imread(imageLeft[1], 0);

    auto ref_frame_ptr = std::make_shared<Frame>(left_img, 0, 4, 0.5, camera_ptr);
    auto cur_frame_ptr = std::make_shared<Frame>(img, 0, 4, 0.5, camera_ptr);

    std::vector<cv::KeyPoint> kps;
    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++)
    {
        int x = rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
        kps.emplace_back(cv::Point2f((float)x,(float)y),0);
    }
    ref_frame_ptr->SetKeys(kps, ref_frame_ptr);

    for (int i = 0;i < nPoints; i++)
    {
        Eigen::Vector3f norm;
        norm[0] = (pixels_ref.at(i).x() - cx)/fx;
        norm[1] = (pixels_ref.at(i).y() - cy)/fy;
        norm[2] = 1;
        auto mp = std::make_shared<MapPoint>(norm * depth_ref[i], map_ptr, ref_frame_ptr, ref_frame_ptr->GetKey(i), cv::Mat());

        ref_frame_ptr->AddMapPoint(mp, i);
        map_ptr->AddMapPoint(mp);
    }
    ref_frame_ptr->SetPose(Eigen::Matrix4f::Identity());
    Eigen::Matrix4f init_gauss = Eigen::Matrix4f::Identity();
    cur_frame_ptr->SetPose(init_gauss);

    matcher_ptr->setDirectParam(3, 0, 10, 5, 0.0001);
    auto T_cur_ref = matcher_ptr->SparseImageAlign(cur_frame_ptr, ref_frame_ptr);
    std::cout<<"T: "<<T_cur_ref<<std::endl;

}

void test_image_align()
{
    std::vector<string> imageLeft;
    std::vector<string> imageRight;
    std::vector<double> timeStamp;

    LoadImages( "/media/lyc/ 其他/dataset/sequences/00",
                imageLeft, imageRight, timeStamp);

    auto matcher_ptr = std::make_shared<Matcher>();

    cv::Mat left_img = cv::imread(imageLeft[0], 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng;
    int nPoints = 1;
    int boarder = 20;
    VecVector2d pixels_ref;
    vector<double> depth_ref;
    auto map_ptr = std::make_shared<TRACKING_BENCH::Map>();

    auto camera_ptr = std::make_shared<PinholeCamera>(left_img.cols, left_img.rows,fx, fy, cx, cy);
    cv::Mat img = cv::imread(imageLeft[1], 0);

    auto ref_frame_ptr = std::make_shared<Frame>(left_img, 0, 4, 0.5, camera_ptr);
    auto cur_frame_ptr = std::make_shared<Frame>(img, 0, 4, 0.5, camera_ptr);

    std::vector<cv::KeyPoint> kps;
    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++)
    {
        int x = 130;//rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = 73;//rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
        kps.emplace_back(cv::Point2f((float)x,(float)y),0, 0, 0, 1);
    }
    ref_frame_ptr->SetKeys(kps, ref_frame_ptr);

    ref_frame_ptr->SetPose(Eigen::Matrix4f::Identity());
    Eigen::Matrix4f init_gauss = Eigen::Matrix4f::Identity();
    cur_frame_ptr->SetPose(init_gauss);
    for (int i = 0;i < nPoints; i++)
    {
        Eigen::Vector3f norm;
        norm[0] = (pixels_ref.at(i).x() - cx)/fx;
        norm[1] = (pixels_ref.at(i).y() - cy)/fy;
        norm[2] = 1;
        auto mp = std::make_shared<MapPoint>(norm * depth_ref[i], map_ptr, ref_frame_ptr, ref_frame_ptr->GetKey(i), cv::Mat());

        ref_frame_ptr->AddMapPoint(mp, i);
        map_ptr->AddMapPoint(mp);

        Eigen::Vector2f px_cur(130, 73);

        cv::Mat show1 = cur_frame_ptr->GetImagePyramid()[0].clone();
        cv::Mat show2 = ref_frame_ptr->GetImagePyramid()[0].clone();
        cv::cvtColor(show1, show1, CV_GRAY2BGR);
        cv::cvtColor(show2, show2, CV_GRAY2BGR);
        cv::circle(show2, cv::Point((int)px_cur[0], (int)px_cur[1]), 5, cv::Scalar(255, 0, 0), 5);

        std::cout<<"before "<<std::endl<<px_cur<<std::endl;
        matcher_ptr->FindMatchDirect(mp, cur_frame_ptr, px_cur);
        std::cout<<"after "<<std::endl<<px_cur<<std::endl;

        cv::circle(show1, cv::Point((int)px_cur[0], (int)px_cur[1]), 5, cv::Scalar(255, 0, 0), 5);

        cv::imshow("before", show2);
        cv::imshow("after", show1);
        cv::waitKey(0);
    }

    ref_frame_ptr->SetPose(Eigen::Matrix4f::Identity());
    cur_frame_ptr->SetPose(Eigen::Matrix4f::Identity());

//    const cv::Mat& cur_img, uint8_t* ref_patch_with_border, uint8_t* ref_patch, const int n_iter, Eigen::Vector2f& cur_ps_estimate, bool no_simd = false
    int half_patch_size = 4;
    uint8_t patch[half_patch_size*half_patch_size*4] __attribute__((aligned(16)));
    uint8_t patch_with_border[(half_patch_size+1)*(half_patch_size+1)*4] __attribute__((aligned(16)));

    Eigen::Vector2f px_scaled(65, 36.5);
    const int cur_step = ref_frame_ptr->GetImagePyramid()[1].step.p[0];

    for(int y=0; y<(half_patch_size+1)*2; ++y)
    {
        uint8_t *it = (uint8_t *) ref_frame_ptr->GetImagePyramid()[1].data + ((int)px_scaled[1] + y - half_patch_size-1) * cur_step + (int)px_scaled[0] - half_patch_size-1;
        for (int x = 0; x < (half_patch_size+1)*2; ++x, ++it)
        {
            int id = x + y*(half_patch_size+1)*2;
            patch_with_border[id] = *it;
        }
    }

    uint8_t* patch_ptr = patch;
    for(int y = 1; y < half_patch_size*2 + 1; ++y, patch_ptr += half_patch_size*2)
    {
        uint8_t* y_ptr = patch_with_border + y * (half_patch_size*2 + 2) + 1;
        for(int x = 0; x < half_patch_size*2; ++x)
            patch_ptr[x] = y_ptr[x];
    }

    cv::Mat show1 = cur_frame_ptr->GetImagePyramid()[1].clone();
    cv::Mat show2 = ref_frame_ptr->GetImagePyramid()[1].clone();
    cv::cvtColor(show1, show1, CV_GRAY2BGR);
    cv::cvtColor(show2, show2, CV_GRAY2BGR);

    cv::circle(show2, cv::Point((int)px_scaled[0], (int)px_scaled[1]), 5, cv::Scalar(255, 0, 0), 5);

    std::cout<<" before: "<<px_scaled<<std::endl;
    matcher_ptr->Align2D(cur_frame_ptr->GetImagePyramid()[1], patch_with_border, patch, 40, px_scaled);
    std::cout<<" align: "<<px_scaled<<std::endl;
    cv::circle(show1, cv::Point((int)px_scaled[0], (int)px_scaled[1]), 5, cv::Scalar(255, 0, 0), 5);

    cv::imshow("before", show2);
    cv::imshow("after", show1);
    cv::waitKey(0);
}

int main()
{
    //test_direct();
    //test_direct2();
    test_projection();
//    test_image_align();
    return 0;
}


#include "correlation.hpp"
#include "test/test_helpers.hpp"
#include <boost/numeric/ublas/io.hpp>

#include <boost/test/unit_test.hpp>

extern trafo_matrix_t matrix_from_string_vector(const std::vector<std::string>& source);

BOOST_AUTO_TEST_SUITE(correlation_fn_test)

BOOST_AUTO_TEST_CASE( transformed_correlation_identity_test )
{
	for(int dim = 1; dim <= 3; ++dim)
	{
		auto base = makeGaussianCorrelation(0.5);
		trafo_matrix_t identity = boost::numeric::ublas::identity_matrix<double>(dim, dim);
		auto trafoed = makeTransformedCorrelation(base, identity);
		for(int i = 0; i < 100; ++i)
		{
			auto p = gen_vect(dim);
			BOOST_CHECK_EQUAL( base(p), trafoed(p) );
		}
	}
}

BOOST_AUTO_TEST_CASE( matrix_from_string_vec_test )
{
	for(int dim = 1; dim <= 3; ++dim)
	{
		// create the string that is the identity matrix
		std::vector<std::string> src;
		trafo_matrix_t reference(dim, dim);
		for(int j = 0; j < dim; ++j)
		{
			for(int k = 0; k < dim; ++k)
			{
				src.push_back(k == j ? "1" : "0");
				reference(j, k) = k == j ? 1 : 0;
			}
		}

		auto matrix = matrix_from_string_vector(src);
		std::cout << matrix << "\n" << reference << "\n";
		for(int j = 0; j < dim; ++j)
		{
			for(int k = 0; k < dim; ++k)
			{
				BOOST_CHECK_EQUAL( matrix(j, k), reference(j, k));
			}
		}

	}

	/// \todo add test that checks for transposedness
}

BOOST_AUTO_TEST_SUITE_END()


#include <vector>
#include <deque>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
using namespace std;

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

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include "../../../core/format_helper.h"

//#include "../../base/stereo_cvsgbm.h"
#include "../../base/stereo_sgm.h"

void computeSGM();

//cv::StereoSGBM sgbm;
cv::Mat img_left, img_right;
cv::Mat img_disp, mat_disp, img_disp_save, mat_disp_save;

int sad_window_size = 5;                  // 3 - 11
int sad_window_size_max = 11;
int P1 = 10;                              // 70
int P1_max = 100;
int P2 = 50;                             // 800
int P2_max = 600;
//int frame_num = 391;
int frame_num = 102;
int frame_num_max = 1000;
int pre_filter_cap = 40;                  // 70
int pre_filter_cap_max = 200;
int min_disparity = 0;
int min_disparity_max = 20;
//int number_of_disparities = 128;          // or 256 max
int number_of_disparities = 200;          // or 256 max
int number_of_disparities_max = 512;
int uniqueness_ratio = 0;                 // 0 - 15
int uniqueness_ratio_max = 15;
int speckle_window_size = 200;            // 100
int speckle_window_size_max = 1000;
int speckle_range = 32;
int speckle_range_max = 128;
int disp12_max_diff = 1;                 // 1
int disp12_max_diff_max = 20;
int full_dp = 1;
int full_dp_max = 1;

void updateParameters()
{
  if(P2 <= P1) {
    P2 = P1 + 1;
    std::cout << "Warning: P2 too small! Using P2 = " << P2 << "\n";
  }
  int residue = number_of_disparities % 16;
  if(residue != 0) {
    number_of_disparities += (16 - residue);
    std::cout << "Warning: number_of_disparities \% 16 != 0! Using number_of_disparities = " << number_of_disparities << "\n";
  }
  //sgbm.preFilterCap = pre_filter_cap;
  //sgbm.SADWindowSize = sad_window_size;
  //sgbm.P1 = P1;
  //sgbm.P2 = P2;
  //sgbm.minDisparity = min_disparity;
  //sgbm.numberOfDisparities = number_of_disparities;
  //sgbm.uniquenessRatio = uniqueness_ratio;
  //sgbm.speckleWindowSize = speckle_window_size;
  //sgbm.speckleRange = speckle_range;
  //sgbm.disp12MaxDiff = disp12_max_diff;
  //sgbm.fullDP = full_dp;
}

void on_trackbar(int, void*)
{
  updateParameters();
}

void computeSGM()
{
  //recon::StereoSGBMParams params(min_disparity, number_of_disparities, sad_window_size, P1, P2, disp12_max_diff,
  //                               pre_filter_cap, uniqueness_ratio, speckle_window_size, speckle_range, full_dp);

  //cv::Mat buffer;
  //recon::computeDisparitySGBM(img_left, img_right, img_disp, params, buffer);
  //recon::computeSGM(img_left, img_right, params, img_disp);

  //double sigma = 0.7;
  //cv::GaussianBlur(img_left, img_left, cv::Size(3,3), sigma);
  //cv::GaussianBlur(img_right, img_right, cv::Size(3,3), sigma);

  // opencv
  //sgbm(img_left, img_right, img_disp);
  //img_disp.convertTo(img_disp_save, CV_8U, 1.0/16.0);
  //img_disp.convertTo(mat_disp_save, CV_32F, 1.0/16.0);
  //img_disp.convertTo(img_disp, CV_8U, (255.0/sgbm.numberOfDisparities) * (1.0/16.0));

  recon::StereoSGMParams sgm_params;
  sgm_params.disp_range = number_of_disparities;
  sgm_params.window_sz = sad_window_size;
  sgm_params.penalty1 = P1;
  sgm_params.penalty2 = P2;
  recon::StereoSGM sgm(sgm_params);
  sgm.compute(img_left, img_right, mat_disp);
  mat_disp.convertTo(img_disp, CV_8U);

  cv::equalizeHist(img_disp, img_disp);
  cv::imshow("Disparity image", img_disp);

  //cv::imwrite("disp_img.png", img_disp);
}

void fineTuneSGM(const vector<string>& imagelist, const string& source_folder)
{
  // http://www.cvlibs.net/datasets/kitti/eval_stereo_flow_detail.php?benchmark=stereo&error=3&eval=all&result=3ae300a3a3b3ed3e48a63ecb665dffcc127cf8ab
  //sgbm.preFilterCap = 63;
  //sgbm.SADWindowSize = 3;       // 3, 5
  //int cn = 1;                   // image channel size
  ////sgbm.P1 = 8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
  //sgbm.P1 = 4*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;       // 50
  //sgbm.P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;      // 800
  //sgbm.minDisparity = 0;
  //sgbm.numberOfDisparities = 128;     // 128, 256
  //sgbm.uniquenessRatio = 10;          // 10, 0
  //sgbm.speckleWindowSize = 100;       // 100
  //sgbm.speckleRange = 32;             // 32
  //sgbm.disp12MaxDiff = 1;
  //sgbm.fullDP = 1;

  // better for KITTI
  // http://www.cvlibs.net/datasets/kitti/eval_stereo_flow_detail.php?benchmark=stereo&error=3&eval=all&result=0d8c42f3fb90ff87bf89c1f8f7977cf309a0f49d
  //sgbm.preFilterCap = 15;
  //sgbm.SADWindowSize = 5;
  //sgbm.P1 = 50;
  //sgbm.P2 = 800;
  //sgbm.minDisparity = 0;
  //sgbm.numberOfDisparities = 256;
  //sgbm.uniquenessRatio = 0;
  //sgbm.speckleWindowSize = 100;
  //sgbm.speckleRange = 32;
  //sgbm.disp12MaxDiff = 1;
  //sgbm.fullDP = 1;
  updateParameters();

  cv::namedWindow("Disparity image");
  cv::namedWindow("Left image");
  cv::namedWindow("Right image");
  cv::namedWindow("Parameters");
  cv::createTrackbar("Frame number", "Parameters", &frame_num, frame_num_max, on_trackbar);
  cv::createTrackbar("preFilterCap", "Parameters", &pre_filter_cap, pre_filter_cap_max, on_trackbar);
  cv::createTrackbar("SADWindowSize", "Parameters", &sad_window_size, sad_window_size_max, on_trackbar);
  cv::createTrackbar("P1", "Parameters", &P1, P1_max, on_trackbar);
  cv::createTrackbar("P2", "Parameters", &P2, P2_max, on_trackbar);
  cv::createTrackbar("minDisparity", "Parameters", &min_disparity, min_disparity_max, on_trackbar);
  cv::createTrackbar("numberOfDisparities", "Parameters", &number_of_disparities, number_of_disparities_max, on_trackbar);
  cv::createTrackbar("uniquenessRatio", "Parameters", &uniqueness_ratio, uniqueness_ratio_max, on_trackbar);
  cv::createTrackbar("speckleWindowSize", "Parameters", &speckle_window_size, speckle_window_size_max, on_trackbar);
  cv::createTrackbar("speckleRange", "Parameters", &speckle_range, speckle_range_max, on_trackbar);
  cv::createTrackbar("disp12MaxDiff", "Parameters", &disp12_max_diff, disp12_max_diff_max, on_trackbar);
  cv::createTrackbar("fullDP", "Parameters", &full_dp, full_dp_max, on_trackbar);
  cv::imshow("Parameters", cv::Mat::zeros(1, 1400, CV_8U));

  while(true) {
    cout << source_folder + imagelist[frame_num*2] << "\n";
    img_left = cv::imread(source_folder + imagelist[frame_num*2], CV_LOAD_IMAGE_GRAYSCALE);
    img_right = cv::imread(source_folder + imagelist[(frame_num*2)+1], CV_LOAD_IMAGE_GRAYSCALE);
    //img_left = cv::imread("/home/kivan/Projects/datasets/Middlebury/im2.png", CV_LOAD_IMAGE_GRAYSCALE);
    //img_right = cv::imread("/home/kivan/Projects/datasets/Middlebury/im6.png", CV_LOAD_IMAGE_GRAYSCALE);
    cv::imshow("Left image", img_left);
    cv::imshow("Right image", img_right);

    //cv::Mat img_left_eq, img_right_eq;
    //cv::equalizeHist(img_left, img_left_eq);
    //cv::equalizeHist(img_right, img_right_eq);
    //img_left = img_left_eq;
    //img_right = img_right_eq;
    //cv::imshow("left_eq", img_left_eq);
    //cv::imshow("right_eq", img_right_eq);

    computeSGM();
    while (cv::waitKey(0) != 10);
  }
  cv::waitKey(0);
}

int main(int argc, char** argv)
{
  string config_file;
  string imagelistfn;
  string source_folder;

  try {
    po::options_description generic("Generic options");
    generic.add_options()
      ("help", "produce help message")
      ("config,c", po::value<string>(&config_file)->default_value("config.txt"), "config filename")
      ;
    po::options_description config("Config file options");
    config.add_options()
      ("source_folder,s", po::value<string>(&source_folder), "folder with source")
      ("imglist,l", po::value<string>(&imagelistfn), "file with image list")
      ;

    po::options_description cmdline_options;
    cmdline_options.add(generic).add(config);

    po::options_description config_file_options;
    config_file_options.add(config);
    po::variables_map vm;
    //po::store(po::command_line_parser(argc, argv).options(cmdline_options).allow_unregistered().run(), vm);
    po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
    notify(vm);
    if(vm.count("help")) {
      cout << generic;
      cout << config;
      return 0;
    }

    ifstream ifs(config_file.c_str());
    if (!ifs) {
      cout << "can not open config file: " << config_file << "\n";
      cout << generic;
      cout << config;
      return 0;
    }
    else {
      po::store(parse_config_file(ifs, config_file_options, true), vm);
      notify(vm);
    }
  }
  catch(std::exception& e) {
    cout << e.what() << "\n";
    return 1;
  }

  std::vector<std::string> imagelist;
  bool ok = core::FormatHelper::readStringList(imagelistfn, imagelist);
  if(!ok || imagelist.empty())
  {
    cout << "can not open " << imagelistfn << " or the string list is empty" << endl;
    return -1;
  }

  fineTuneSGM(imagelist, source_folder);

  return 0;
}


/*

Copyright (c) 2005-2015, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef _RUNGEKUTTAFEHLBERGIVPODESOLVER_HPP_
#define _RUNGEKUTTAFEHLBERGIVPODESOLVER_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include "AbstractOneStepIvpOdeSolver.hpp"

/**
 * A concrete one step ODE solver class that employs the Runge Kutta
 * Fehlberg adaptive solver (RKF45).
 *
 * This solver is good for problems where you need to be able to
 * guarantee the accuracy of the answer as it is specified via the
 * tolerance parameter.
 *
 * The solver should also be reasonably fast as it increases the
 * timestep when the solutions are changing slowly, whilst maintaining
 * accuracy.
 */
class RungeKuttaFehlbergIvpOdeSolver : public AbstractIvpOdeSolver
{
friend class TestRungeKuttaFehlbergIvpOdeSolver;

private:
    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Archive, never used directly - boost uses this.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        // This calls serialize on the base class - all member variables instantiated on construction or temporary.
        archive & boost::serialization::base_object<AbstractIvpOdeSolver>(*this);
    }

    /*
     * All these are here for more efficient memory allocation, rather than
     * because they need to be member variables.
     */

    double m1932o2197;  /**< Working memory: numerical value for the fraction 1932/2197.  */
    double m7200o2197;  /**< Working memory: numerical value for the fraction 7200/2197.  */
    double m7296o2197;  /**< Working memory: numerical value for the fraction 7296/2197.  */
    double m12o13;      /**< Working memory: numerical value for the fraction 12/13.      */
    double m439o216;    /**< Working memory: numerical value for the fraction 439/216.    */
    double m3680o513;   /**< Working memory: numerical value for the fraction 3680/513.   */
    double m845o4104;   /**< Working memory: numerical value for the fraction 845/4104.   */
    double m8o27;       /**< Working memory: numerical value for the fraction 8/27.       */
    double m3544o2565;  /**< Working memory: numerical value for the fraction 3544/2565.  */
    double m1859o4104;  /**< Working memory: numerical value for the fraction 1859/4104.  */
    double m1o360;      /**< Working memory: numerical value for the fraction 1/360.      */
    double m128o4275;   /**< Working memory: numerical value for the fraction 128/4275.   */
    double m2197o75240; /**< Working memory: numerical value for the fraction 2197/75240. */
    double m2o55;       /**< Working memory: numerical value for the fraction 2/55.       */
    double m25o216;     /**< Working memory: numerical value for the fraction 25/216.     */
    double m1408o2565;  /**< Working memory: numerical value for the fraction 1408/2565.  */
    double m2197o4104;  /**< Working memory: numerical value for the fraction 2197/4104.  */

    std::vector<double> mError; /**< Error expression, used to adjust the timestep in the RKF45 method. */

    std::vector<double> mk1;  /**< Working memory: expression k1 in the RKF45 method.  */
    std::vector<double> mk2;  /**< Working memory: expression k2 in the RKF45 method.  */
    std::vector<double> mk3;  /**< Working memory: expression k3 in the RKF45 method.  */
    std::vector<double> mk4;  /**< Working memory: expression k4 in the RKF45 method.  */
    std::vector<double> mk5;  /**< Working memory: expression k5 in the RKF45 method.  */
    std::vector<double> mk6;  /**< Working memory: expression k6 in the RKF45 method.  */
    std::vector<double> myk2; /**< Working memory: expression yk2 in the RKF45 method. */
    std::vector<double> myk3; /**< Working memory: expression yk3 in the RKF45 method. */
    std::vector<double> myk4; /**< Working memory: expression yk4 in the RKF45 method. */
    std::vector<double> myk5; /**< Working memory: expression yk5 in the RKF45 method. */
    std::vector<double> myk6; /**< Working memory: expression yk6 in the RKF45 method. */

protected:

    /**
     * Method that actually performs the solving on behalf of the public Solve methods.
     *
     * @param rSolution  an ODE solution to input data into if requited
     * @param pAbstractOdeSystem  the ODE system to solve
     * @param rCurrentYValues  the current (initial) state; results will also be returned in here
     * @param rWorkingMemory  working memory; same size as rCurrentYValues
     * @param startTime  initial time
     * @param endTime  time to solve to
     * @param maxTimeStep  the maximum size of timestep allowable
     * @param minTimeStep  the maximum size of timestep allowable (to prevent huge loops)
     * @param tolerance  how accurate the numerical solution must be
     * @param outputSolution whether to output into rSolution (or save time by not doing)
     */
    void InternalSolve(OdeSolution& rSolution,
                       AbstractOdeSystem* pAbstractOdeSystem,
                       std::vector<double>& rCurrentYValues,
                       std::vector<double>& rWorkingMemory,
                       double startTime,
                       double endTime,
                       double maxTimeStep,
                       double minTimeStep,
                       double tolerance,
                       bool outputSolution);

    /**
     * Calculate the solution to the ODE system at the next timestep.
     * Updates the mError vector with current error.
     *
     * @param pAbstractOdeSystem  the ODE system to solve
     * @param timeStep  dt
     * @param time  the current time
     * @param rCurrentYValues  the current (initial) state
     * @param rNextYValues  the state at the next timestep
     */
    void CalculateNextYValue(AbstractOdeSystem* pAbstractOdeSystem,
                             double timeStep,
                             double time,
                             std::vector<double>& rCurrentYValues,
                             std::vector<double>& rNextYValues);

    /**
     * Use the error approximation of the last call to the CalculateNextYValue()
     * method to change the time step appropriately.
     *
     * @param rCurrentStepSize  the current step size being used (returns answer via this reference)
     * @param rError  the error in the approximation at this time step
     * @param rTolerance  the tolerance required
     * @param rMaxTimeStep  the maximum timestep to be used
     * @param rMinTimeStep  the minimum timestep to be used (to prevent huge loops)
     */
    void AdjustStepSize(double& rCurrentStepSize,
                        const double& rError,
                        const double& rTolerance,
                        const double& rMaxTimeStep,
                        const double& rMinTimeStep);

public:

    /**
     * Constructor.
     */
    RungeKuttaFehlbergIvpOdeSolver();

    /**
     * Solves a system of ODEs using a specified one-step ODE solver and returns
     * the solution as an OdeSolution object.
     *
     * @param pAbstractOdeSystem  pointer to the concrete ODE system to be solved
     * @param rYValues  a standard vector specifying the intial condition of each
     *                  solution variable in the system (this can be the initial
     *                  conditions vector stored in the ODE system)
     * @param startTime  the time at which the initial conditions are specified
     * @param endTime  the time to which the system should be solved and the solution
     *                 returned
     * @param timeStep  the time interval to be used by the solver
     * @param ignoredSamplingTime  the interval at which to sample the solution to the ODE system
     *                             (ignored in this class as the timestep is variable)
     *
     * @return OdeSolution is an object containing an integer of the number of
     * equations, a stdAbstractOdeSystem::vector of times and a std::vector of std::vectors where
     * each of those vectors contains the solution for one variable of the ODE
     * system at those times.
     */
    OdeSolution Solve(AbstractOdeSystem* pAbstractOdeSystem,
                      std::vector<double>& rYValues,
                      double startTime,
                      double endTime,
                      double timeStep,
                      double ignoredSamplingTime);

    /**
     * Second version of Solve. Solves a system of ODEs using a specified one-step
     * ODE solver. This method does not return the solution and therefore does not
     * take in a sampling time. Instead, the mStateVariables component in the ODE
     * system object is updated.
     *
     * @param pAbstractOdeSystem  pointer to the concrete ODE system to be solved
     * @param rYValues  a standard vector specifying the intial condition of each
     *                  solution variable in the system (this can be the initial
     *                  conditions vector stored in the ODE system)
     * @param startTime  the time at which the initial conditions are specified
     * @param endTime  the time to which the system should be solved and the solution
     *                 returned
     * @param timeStep  the time interval to be used by the solver
     */
    void Solve(AbstractOdeSystem* pAbstractOdeSystem,
               std::vector<double>& rYValues,
               double startTime,
               double endTime,
               double timeStep);

};

#include "SerializationExportWrapper.hpp"
CHASTE_CLASS_EXPORT(RungeKuttaFehlbergIvpOdeSolver)

#endif //_RUNGEKUTTAFEHLBERGIVPODESOLVER_HPP_


#include "teca_integrated_vapor_transport.h"

#include "teca_cartesian_mesh.h"
#include "teca_array_collection.h"
#include "teca_variant_array.h"
#include "teca_metadata.h"
#include "teca_coordinate_util.h"

#include <algorithm>
#include <iostream>
#include <string>
#include <cmath>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

using std::string;
using std::vector;
using std::cerr;
using std::endl;
using std::cos;

//#define TECA_DEBUG

namespace {
template <typename coord_t, typename num_t>
void cartesian_ivt(unsigned long nx, unsigned long ny,
    unsigned long nz, const coord_t *plev, const num_t *wind,
    const num_t *q, num_t *ivt)
{
    unsigned long nxy = nx*ny;
    unsigned long nxyz = nxy*nz;

    // compute the integrand
    num_t *f = (num_t*)malloc(nxyz*sizeof(num_t));
    for (unsigned long i = 0; i < nxyz; ++i)
        f[i] = wind[i]*q[i];

    // initialize the result
    memset(ivt, 0, nxy*sizeof(num_t));

    // work an x-y slice at  a time
    unsigned long nzm1 = nz - 1;
    for (unsigned long k = 0; k < nzm1; ++k)
    {
        // dp over the slice
        num_t h2 = num_t(0.5) * (plev[k+1] - plev[k]);

        // the current two x-y-planes of data
        unsigned long knxy = k*nxy;
        num_t *f_k0 = f + knxy;
        num_t *f_k1 = f_k0 + nxy;

        // accumulate this plane of data using trapazoid rule
        for (unsigned long q = 0; q < nxy; ++q)
        {
            ivt[q] += h2 * (f_k0[q] + f_k1[q]);
        }
    }

    // free up the integrand
    free(f);

    // check the sign, in this way we can handle both increasing and decreasing
    // pressure coordinates
    num_t s = plev[1] - plev[0] < num_t(0) ? num_t(-1) : num_t(1);

    // scale by -1/g
    num_t m1g = s/num_t(9.80665);
    for (unsigned long i = 0; i < nxy; ++i)
        ivt[i] *= m1g;
}

template <typename coord_t, typename num_t>
void cartesian_ivt(unsigned long nx, unsigned long ny,
    unsigned long nz, const coord_t *plev, const num_t *wind,
    const char *wind_valid, const num_t *q, const char *q_valid,
    num_t *ivt)
{
    unsigned long nxy = nx*ny;
    unsigned long nxyz = nxy*nz;

    // compute the mask
    char *mask = (char*)malloc(nxyz);
    for (unsigned long i = 0; i < nxyz; ++i)
        mask[i] = wind_valid[i] && q_valid[i] ? 1 : 0;

    // compute the integrand
    num_t *f = (num_t*)malloc(nxyz*sizeof(num_t));
    for (unsigned long i = 0; i < nxyz; ++i)
        f[i] = wind[i]*q[i];

    // initialize the result
    memset(ivt, 0, nxy*sizeof(num_t));

    // work an x-y slice at a time
    unsigned long nzm1 = nz - 1;
    for (unsigned long k = 0; k < nzm1; ++k)
    {
        // dp over the slice
        num_t h2 = num_t(0.5) * (plev[k+1] - plev[k]);

        // the current two x-y-planes of data
        unsigned long knxy = k*nxy;
        num_t *f_k0 = f + knxy;
        num_t *f_k1 = f_k0 + nxy;

        char *mask_k0 = mask + knxy;
        char *mask_k1 = mask_k0 + nxy;

        // accumulate this plane of data using trapazoid rule
        for (unsigned long q = 0; q < nxy; ++q)
        {
            ivt[q] += ((mask_k0[q] && mask_k1[q]) ?
               h2 * (f_k0[q] + f_k1[q]) : num_t(0));
        }
    }

    // free up the integrand and mask
    free(mask);
    free(f);

    // check the sign, in this way we can handle both increasing and decreasing
    // pressure coordinates
    num_t s = plev[1] - plev[0] < num_t(0) ? num_t(-1) : num_t(1);

    // scale by -1/g
    num_t m1g = s/num_t(9.80665);
    for (unsigned long i = 0; i < nxy; ++i)
        ivt[i] *= m1g;
}
}

// --------------------------------------------------------------------------
teca_integrated_vapor_transport::teca_integrated_vapor_transport() :
    wind_u_variable("ua"), wind_v_variable("va"),
    specific_humidity_variable("hus"), ivt_u_variable("ivt_u"),
    ivt_v_variable("ivt_v"), fill_value(1.0e20)
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_integrated_vapor_transport::~teca_integrated_vapor_transport()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_integrated_vapor_transport::get_properties_description(
    const string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_integrated_vapor_transport":prefix));

    opts.add_options()
        TECA_POPTS_GET(std::string, prefix, wind_u_variable,
            "name of the variable containg the lon component of the wind vector")
        TECA_POPTS_GET(std::string, prefix, wind_v_variable,
            "name of the variable containg the lat component of the wind vector")
        TECA_POPTS_GET(std::string, prefix, specific_humidity_variable,
            "name of the variable containg the specific humidity")
        TECA_POPTS_GET(double, prefix, fill_value,
            "the value of the NetCDF _FillValue attribute")
        ;

    this->teca_algorithm::get_properties_description(prefix, opts);

    global_opts.add(opts);
}

// --------------------------------------------------------------------------
void teca_integrated_vapor_transport::set_properties(
    const string &prefix, variables_map &opts)
{
    this->teca_algorithm::set_properties(prefix, opts);

    TECA_POPTS_SET(opts, std::string, prefix, wind_u_variable)
    TECA_POPTS_SET(opts, std::string, prefix, wind_v_variable)
    TECA_POPTS_SET(opts, std::string, prefix, specific_humidity_variable)
    TECA_POPTS_SET(opts, double, prefix, fill_value)
}
#endif

// --------------------------------------------------------------------------
teca_metadata teca_integrated_vapor_transport::get_output_metadata(
    unsigned int port,
    const std::vector<teca_metadata> &input_md)
{
#ifdef TECA_DEBUG
    std::cerr << teca_parallel_id()
        << "teca_integrated_vapor_transport::get_output_metadata" << std::endl;
#endif
    (void)port;

    // set things up in the first pass, and don't modify in subsequent passes
    // due to threading concerns

    if (this->get_number_of_derived_variables() == 0)
    {
        // the base class will handle dealing with the transformation of
        // mesh dimensions and reporting the array we produce, but we have
        // to determine the data type and tell the name of the produced array.
        const teca_metadata &md = input_md[0];

        teca_metadata attributes;
        if (md.get("attributes", attributes))
        {
            TECA_FATAL_ERROR("Failed to determine output data type "
                "because attributes are misisng")
            return teca_metadata();
        }

        teca_metadata u_atts;
        if (attributes.get(this->wind_u_variable, u_atts))
        {
            TECA_FATAL_ERROR("Failed to determine output data type "
                "because attributes for \"" << this->wind_u_variable
                << "\" are misisng")
            return teca_metadata();
        }

        int type_code = 0;
        if (u_atts.get("type_code", type_code))
        {
            TECA_FATAL_ERROR("Failed to determine output data type "
                "because attributes for \"" << this->wind_u_variable
                << "\" is misisng a \"type_code\"")
            return teca_metadata();
        }

        teca_array_attributes ivt_u_atts(
            type_code, teca_array_attributes::point_centering,
            0, "kg m^{-1} s^{-1}", "longitudinal integrated vapor transport",
            "the longitudinal component of integrated vapor transport",
            1, this->fill_value);

        teca_array_attributes ivt_v_atts(
            type_code, teca_array_attributes::point_centering,
            0, "kg m^{-1} s^{-1}", "latitudinal integrated vapor transport",
            "the latitudinal component of integrated vapor transport",
            this->fill_value);

        // install name and attributes of the output variables in the base classs
        this->append_derived_variable(this->ivt_u_variable);
        this->append_derived_variable(this->ivt_v_variable);

        this->append_derived_variable_attribute(ivt_u_atts);
        this->append_derived_variable_attribute(ivt_v_atts);

    }

    if (this->get_number_of_dependent_variables() == 0)
    {
        // install the names of the input variables in the base class
        this->append_dependent_variable(this->wind_u_variable);
        this->append_dependent_variable(this->wind_v_variable);
        this->append_dependent_variable(this->specific_humidity_variable);
    }

    // invoke the base class method, which does the work of transforming
    // the mesh and reporting the variables and their attributes.
    return teca_vertical_reduction::get_output_metadata(port, input_md);
}

// --------------------------------------------------------------------------
std::vector<teca_metadata> teca_integrated_vapor_transport::get_upstream_request(
    unsigned int port,
    const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
    // invoke the base class method
    return teca_vertical_reduction::get_upstream_request(port, input_md, request);
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_integrated_vapor_transport::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    std::cerr << teca_parallel_id()
        << "teca_integrated_vapor_transport::execute" << std::endl;
#endif
    (void)port;

    // get the input mesh
    const_p_teca_cartesian_mesh in_mesh
        = std::dynamic_pointer_cast<const teca_cartesian_mesh>(input_data[0]);

    if (!in_mesh)
    {
        TECA_FATAL_ERROR("Failed to compute IVT because a cartesian mesh is required.")
        return nullptr;
    }

    // get the input dimensions
    unsigned long extent[6] = {0};
    if (in_mesh->get_extent(extent))
    {
        TECA_FATAL_ERROR("Failed to compute IVT because mesh extent is missing.")
        return nullptr;
    }

    unsigned long nx = extent[1] - extent[0] + 1;
    unsigned long ny = extent[3] - extent[2] + 1;
    unsigned long nz = extent[5] - extent[4] + 1;

    // get the pressure coordinates
    const_p_teca_variant_array p = in_mesh->get_z_coordinates();
    if (!p)
    {
        TECA_FATAL_ERROR("Failed to compute IVT because pressure coordinates are missing")
        return nullptr;
    }

    if (p->size() < 2)
    {
        TECA_FATAL_ERROR("Failed to compute IVT because z dimensions "
            << p->size() << " < 2 as required by the integration method")
        return nullptr;
    }

    // gather the input arrays
    const_p_teca_variant_array wind_u =
        in_mesh->get_point_arrays()->get(this->wind_u_variable);

    if (!wind_u)
    {
        TECA_FATAL_ERROR("Failed to compute IVT because longitudinal wind \""
            << this->wind_u_variable << "\" is missing")
        return nullptr;
    }

    const_p_teca_variant_array wind_u_valid =
           in_mesh->get_point_arrays()->get(this->wind_u_variable + "_valid");

    const_p_teca_variant_array wind_v =
        in_mesh->get_point_arrays()->get(this->wind_v_variable);

    if (!wind_v)
    {
        TECA_FATAL_ERROR("Failed to compute IVT because latitudinal wind \""
            << this->wind_v_variable << "\" is missing")
        return nullptr;
    }

    const_p_teca_variant_array wind_v_valid =
           in_mesh->get_point_arrays()->get(this->wind_v_variable + "_valid");

    const_p_teca_variant_array q =
        in_mesh->get_point_arrays()->get(this->specific_humidity_variable);

    if (!q)
    {
        TECA_FATAL_ERROR("Failed to compute IVT because specific humidity \""
            << this->specific_humidity_variable << "\" is missing")
        return nullptr;
    }

    const_p_teca_variant_array q_valid =
           in_mesh->get_point_arrays()->get(this->specific_humidity_variable + "_valid");

    // the base class will construct the output mesh
    p_teca_cartesian_mesh out_mesh
        = std::dynamic_pointer_cast<teca_cartesian_mesh>(
            std::const_pointer_cast<teca_dataset>(
                teca_vertical_reduction::execute(port, input_data, request)));

    if (!out_mesh)
    {
        TECA_FATAL_ERROR("Failed to compute IVT because the output mesh was "
            "not constructed")
        return nullptr;
    }

    // allocate the output arrays
    unsigned long nxy = nx*ny;
    p_teca_variant_array ivt_u = wind_u->new_instance(nxy);
    p_teca_variant_array ivt_v = wind_u->new_instance(nxy);

    // store the result
    out_mesh->get_point_arrays()->set(this->ivt_u_variable, ivt_u);
    out_mesh->get_point_arrays()->set(this->ivt_v_variable, ivt_v);

    // calculate IVT
    NESTED_TEMPLATE_DISPATCH_FP(const teca_variant_array_impl,
        p.get(), _COORDS,

        const NT_COORDS *p_p = static_cast<TT_COORDS*>(p.get())->get();

        NESTED_TEMPLATE_DISPATCH_FP(teca_variant_array_impl,
            ivt_u.get(), _DATA,

            NT_DATA *p_ivt_u = static_cast<TT_DATA*>(ivt_u.get())->get();
            NT_DATA *p_ivt_v = static_cast<TT_DATA*>(ivt_v.get())->get();

            const NT_DATA *p_wind_u = static_cast<const TT_DATA*>(wind_u.get())->get();
            const NT_DATA *p_wind_v = static_cast<const TT_DATA*>(wind_v.get())->get();
            const NT_DATA *p_q = static_cast<const TT_DATA*>(q.get())->get();

            const char *p_wind_u_valid = nullptr;
            const char *p_wind_v_valid = nullptr;
            const char *p_q_valid = nullptr;
            if (wind_u_valid)
            {
                using TT_MASK = teca_char_array;

                p_wind_u_valid = dynamic_cast<const TT_MASK*>(wind_u_valid.get())->get();
                p_wind_v_valid = dynamic_cast<const TT_MASK*>(wind_v_valid.get())->get();
                p_q_valid = dynamic_cast<const TT_MASK*>(q_valid.get())->get();

                ::cartesian_ivt(nx, ny, nz, p_p, p_wind_u, p_wind_u_valid, p_q, p_q_valid, p_ivt_u);
                ::cartesian_ivt(nx, ny, nz, p_p, p_wind_v, p_wind_v_valid, p_q, p_q_valid, p_ivt_v);
            }
            else
            {
                ::cartesian_ivt(nx, ny, nz, p_p, p_wind_u, p_q, p_ivt_u);
                ::cartesian_ivt(nx, ny, nz, p_p, p_wind_v, p_q, p_ivt_v);
            }
            )
        )

    // pass 2D arrays through.
    p_teca_array_collection in_arrays =
        std::const_pointer_cast<teca_array_collection>(in_mesh->get_point_arrays());

    p_teca_array_collection out_arrays = out_mesh->get_point_arrays();

    int n_arrays = in_arrays->size();
    for (int i = 0; i < n_arrays; ++i)
    {
        p_teca_variant_array array = in_arrays->get(i);
        if (array->size() == nxy)
        {
            // pass the array.
            out_arrays->append(in_arrays->get_name(i), array);
        }
    }

    return out_mesh;
}


#include <Eigen/Geometry>

// #include <nlohmann/json/json.hpp>

//#include "geometry/bounds.hpp"
#include "geometry/polygon.hpp"

#include "terrain.hpp"

using terrain::geometry::Polygon;

using nlohmann::json;

namespace terrain {

nlohmann::json generate_diamond( const double width,
                                 const double desired_precision)
{
    const double width_2 = width /2;
    return {{"layout",  {{"precision", desired_precision},
                         {"x", width_2},
                         {"y", width_2},
                         {"width", width}}},
            {"allow", {{{  width, width_2},
                        {width_2,   width},
                        {      0, width_2},
                        {width_2,       0}}}}};
}

}; // namespace terrain


#include <Eigen/Dense>
#include <assert.h>
#include <boost/math/special_functions/sign.hpp>
#include <iostream>
#include <math.h>

#include "grid.hpp"
#include "spdlog/spdlog.h"

using namespace Eigen;
using namespace SnowSimulator;

// ============================================================================
// GRIDCELL METHODS
// ============================================================================

GridCell::GridCell() {}

void GridCell::addMaterialPoint(MaterialPoint *materialPoint) {
  m_materialPoints.push_back(materialPoint);
  materialPoint->m_cell = this;
}

void GridCell::clear() { m_materialPoints.clear(); }

// ============================================================================
// GRIDNODE METHODS
// ============================================================================

GridNode::GridNode(Vector3i idx, Grid *grid)
    : m_idx(idx), m_grid(grid), m_mass(0), m_velocity(Vector3f::Zero()),
      m_nextVelocity(Vector3f::Zero()), m_force(Vector3f::Zero()) {}

Vector3f GridNode::position() const {
  return m_idx.cast<float>() * m_grid->m_spacing;
}

void GridNode::zeroForce() { m_force = Vector3f::Zero(); }

void GridNode::addForce(Vector3f force) { m_force += force; }

/**
 * The cubic B-spline formulation of the grid basis function used for
 * determining how particle properties are transferred to the grid. Both
 * this function and the gradient of this function below are called for each
 * pair of particle and grid node. This is referred to as N in the paper.
 */
float GridNode::cubicBSpline(float x) const {
  float absx = fabs(x);

  if (absx >= 2) {
    return 0;
  } else if (absx < 1) {
    return (0.5f * absx - 1.f) * absx * absx + (2.0f / 3);
  } else {
    return (((-1.0f / 6) * absx + 1) * absx - 2) * absx + (4.0f / 3);
  }
}

float GridNode::gradCubicBSpline(float x) const {
  float absx = fabs(x);
  // int sign = copysign(1, x);
  int sign = x > 0 ? 1 : -1;

  if (absx < 1) {
    return sign * (1.5 * absx - 2) * absx;
  } else if (absx < 2) {
    return sign * ((-0.5f * absx + 2) * absx - 2);
  } else {
    return 0;
  }
}

/**
 * Computes the weight necessary for rasterizing the properties of a
 * materialPoint to the grid. This is referred to as w in the paper.
 */
float GridNode::basisFunction(Vector3f particlePos) const {
  Vector3f offset = particlePos / m_grid->m_spacing - m_idx.cast<float>();

  return cubicBSpline(offset.x()) * cubicBSpline(offset.y()) *
         cubicBSpline(offset.z());
}

Vector3f GridNode::gradBasisFunction(Vector3f particlePos) const {
  float invSpacing = 1.0f / m_grid->m_spacing;
  Vector3f offset = invSpacing * particlePos - m_idx.cast<float>();

  float gx = gradCubicBSpline(offset.x()) * cubicBSpline(offset.y()) *
             cubicBSpline(offset.z());
  float gy = cubicBSpline(offset.x()) * gradCubicBSpline(offset.y()) *
             cubicBSpline(offset.z());
  float gz = cubicBSpline(offset.x()) * cubicBSpline(offset.y()) *
             gradCubicBSpline(offset.z());
  return Vector3f(gx, gy, gz);
}

// ============================================================================
// GRID METHODS
// ============================================================================

Grid::Grid(Vector3f origin, Vector3i dimensions, float spacing)
    : m_origin(origin), m_dim(dimensions), m_spacing(spacing) {
  auto logger = spdlog::get("snowsim");

  logger->info("Creating grid of size ({}, {}, {})", m_dim.x(), m_dim.y(),
               m_dim.z());

  int num_nodes = m_dim.prod();
  logger->info("Total node count: {}", num_nodes);

  // Allocate space for each of the gridNodes

  logger->info("Allocating grid nodes and cells...");

  for (int i = 0; i < num_nodes; i++) {
    GridNode *node = new GridNode(idxToVector(i), this);
    m_gridNodes.push_back(node);
    m_gridCells.push_back(new GridCell());
  }

  // Make sure each GridNode has a reference to the 4^3 surrounding GridCells
  // within the support radius of the basis function.
  // Each GridCell also needs to know about the 6^3 surrounding GridNodes.

  logger->info("Linking grid nodes to neighboring cells...");

  for (int i = 0; i < num_nodes; i++) {
    Vector3i idx = idxToVector(i);
    GridNode *current = m_gridNodes[i];

    // GridCell indexing is such that the corresponding GridNode is at the
    // top-left corner of the GridCell.

    Vector3i minIdx = (idx.array() - 2).max(0);
    Vector3i maxIdx = (idx.array() + 1).min(m_dim.array() - 1);

    for (int ox = minIdx.x(); ox < maxIdx.x(); ox++) {
      for (int oy = minIdx.y(); oy < maxIdx.y(); oy++) {
        for (int oz = minIdx.z(); oz < maxIdx.z(); oz++) {
          int cellIdx = vectorToIdx(Vector3i(ox, oy, oz));
          GridCell *neighborCell = m_gridCells[cellIdx];
          current->m_neighborCells.push_back(neighborCell);
        }
      }
    }
  }
}

std::vector<GridNode *> Grid::getAllNodes() { return m_gridNodes; }


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Arr_segment_traits_2.h>
#include <CGAL/Arrangement_2.h>

#include <boost/unordered_map.hpp>
#include <unordered_map>

typedef int                                           Number_type;
typedef CGAL::Simple_cartesian<Number_type>           Kernel;
typedef CGAL::Arr_segment_traits_2<Kernel>            Traits_2;
typedef Traits_2::Point_2                             Point_2;
typedef Traits_2::X_monotone_curve_2                  Segment_2;
typedef CGAL::Arrangement_2<Traits_2>                 Arrangement_2;
typedef Arrangement_2::Vertex_handle                  Vertex_handle;
typedef Arrangement_2::Halfedge_handle                Halfedge_handle;

int main()
{
  Arrangement_2   arr;

  Segment_2       s1(Point_2(1, 3), Point_2(3, 5));
  Segment_2       s2(Point_2(3, 5), Point_2(5, 3));
  Segment_2       s3(Point_2(5, 3), Point_2(3, 1));
  Segment_2       s4(Point_2(3, 1), Point_2(1, 3));
  Segment_2       s5(Point_2(1, 3), Point_2(5, 3));

  Halfedge_handle e1 = arr.insert_in_face_interior(s1, arr.unbounded_face());
  Vertex_handle   v1 = e1->source();

  std::map<Vertex_handle,int> sm;
  sm[v1]= 1;
  std::unordered_map<Vertex_handle,int> sum;
  sum[v1]= 1;
  boost::unordered_map<Vertex_handle,int> bum;
  bum[v1]= 1;
  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// asy_distribution.hpp                                                      //
//                                                                           //
//  Copyright 2010 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_COMMON_ASY_DISTRIBUTION_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_COMMON_ASY_DISTRIBUTION_HPP_ER_2010
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/policies/policy.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace contingency_table{
namespace pearson_chisq_statistic{

    namespace result_of{

        template<typename T1,
            typename Policy = boost::math::policies::policy<> >
        struct asy_distribution{
            typedef boost::math::chi_squared_distribution<T1,Policy> type;
        };

    }// result_of

    template<typename T1,typename AccSet,typename H0,typename Policy>
    typename pearson_chisq_statistic::result_of::asy_distribution<T1,Policy>::type
    asy_distribution(
        const H0& hypothesis,
        const AccSet& acc,
        const Policy& pol
    )
    {
        namespace ns = pearson_chisq_statistic;
        typedef typename ns::result_of::asy_distribution<
            T1,Policy>::type result_;
        T1 df = static_cast<T1>( ns::degrees_of_freedom( hypothesis, acc ) );
        return result_( df );
    }

    template<typename T1,typename AccSet,typename H0>
    typename pearson_chisq_statistic::result_of::asy_distribution<T1>::type
    asy_distribution(
        const H0& hypothesis,
        const AccSet& acc
    ){
        typedef boost::math::policies::policy<> pol_;
        return asy_distribution<T1>( hypothesis, acc, pol_() );
    }

}// pearson_chisq_statistic
}// contingency_table
}// detail
}// statistics
}// boost

#endif


// Copyright 2021 Jan Niklas Hasse <jhasse@bixense.com>
// For conditions of distribution and use, see copyright notice in LICENSE.txt
/// Contains jngl::Mat4 class
/// @file
#pragma once

#if !defined(__has_include) || __has_include("boost/version.hpp")
#include <boost/version.hpp>
#endif
#include <initializer_list>

namespace jngl {

/// 4x4 matrix
///
/// Identical to `boost::qvm::mat<float, 4, 4>`, but stores the elements column-major in memory
class Mat4 {
public:
	/// creates identity matrix
	Mat4() = default;

	/// construct matrix from row-major array with 16 elements
	Mat4(std::initializer_list<float>);

	/// column-major
	float data[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 };
};

} // namespace jngl

#if BOOST_VERSION >= 106200 || __has_include("boost/qvm/mat_traits.hpp")
#include <boost/qvm/mat_traits.hpp>

namespace boost::qvm {
template <> struct mat_traits<jngl::Mat4> {
	static int const rows = 4;
	static int const cols = 4;
	using scalar_type = float;

	template <int R, int C> static scalar_type read_element(const jngl::Mat4& m) {
		return m.data[C * 4 + R];
	}
	template <int R, int C> static scalar_type& write_element(jngl::Mat4& m) {
		return m.data[C * 4 + R];
	}
};
} // namespace boost::qvm
#endif


/*
 *
 * Copyright (c) 2003 Kresimir Fresl, Toon Knapen and Karl Meerbergen
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_TRAITS_UBLAS_SPARSE_MATRIX_H
#define BOOST_NUMERIC_BINDINGS_TRAITS_UBLAS_SPARSE_MATRIX_H

#include <cstddef>
#ifndef BOOST_UBLAS_HAVE_BINDINGS
#  include <boost/numeric/ublas/matrix_sparse.hpp>
#endif
#include <boost/numeric/bindings/traits/sparse_traits.hpp>
#include <boost/numeric/bindings/traits/detail/ublas_ordering.hpp>
#include <algorithm>

namespace boost { namespace numeric { namespace bindings { namespace traits {

  // ublas::compressed_matrix<>
  template <typename T, typename F, std::size_t IB, typename IA, typename TA,
            typename MType
            >
  struct sparse_matrix_detail_traits<
    boost::numeric::ublas::compressed_matrix<T,F,IB,IA,TA>,
    MType
  >
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<
         boost::numeric::ublas::compressed_matrix<T,F,IB,IA,TA>,
         typename boost::remove_const<MType>::type
       >::value)
    );
#endif

    typedef
      boost::numeric::ublas::compressed_matrix<T,F,IB,IA,TA> identifier_type;
    typedef MType matrix_type;

    typedef general_t matrix_structure;
    typedef compressed_t storage_format;
    typedef typename detail::ublas_ordering<
      typename F::orientation_category
    >::type ordering_type;
    typedef F layout_type;

    typedef T value_type;

  private:
    typedef typename detail::generate_const<MType,TA>::type val_array_t;
    typedef typename detail::generate_const<MType,IA>::type idx_array_t;

  public:
    typedef typename vector_traits<val_array_t>::pointer value_pointer;
    typedef typename vector_traits<idx_array_t>::pointer index_pointer;

    BOOST_STATIC_CONSTANT (std::size_t, index_base = IB);

    static index_pointer index1_storage (matrix_type& cm) {
      //assert (cm.filled1() == layout_type::num_rows (cm.size1(), cm.size2()) + 1);
      return vector_traits<idx_array_t>::storage (cm.index1_data());
    }
    static index_pointer index2_storage (matrix_type& cm) {
      return vector_traits<idx_array_t>::storage (cm.index2_data());
    }
    static value_pointer value_storage (matrix_type& cm) {
      return vector_traits<val_array_t>::storage (cm.value_data());
    }

    static std::ptrdiff_t num_rows (matrix_type& cm) { return cm.size1(); }
    static std::ptrdiff_t num_columns (matrix_type& cm) { return cm.size2(); }
    static std::ptrdiff_t num_nonzeros (matrix_type& cm) {
      return cm.nnz();
      // Joerg, this isn't very intuitive :o(
      // return cm.non_zeros();
    }
  };

  // ublas::coordinate_matrix<>
  template <typename T, typename F, std::size_t IB, typename IA, typename TA,
            typename MType
            >
  struct sparse_matrix_detail_traits<
    boost::numeric::ublas::coordinate_matrix<T,F,IB,IA,TA>,
    MType
  >
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<
         boost::numeric::ublas::coordinate_matrix<T,F,IB,IA,TA>,
         typename boost::remove_const<MType>::type
       >::value)
    );
#endif

    typedef
      boost::numeric::ublas::coordinate_matrix<T,F,IB,IA,TA> identifier_type;
    typedef MType matrix_type;

    typedef general_t matrix_structure;
    typedef coordinate_t storage_format;
    typedef typename detail::ublas_ordering<
      typename F::orientation_category
    >::type ordering_type;

    typedef T value_type;

  private:
    typedef typename detail::generate_const<MType,TA>::type val_array_t;
    typedef typename detail::generate_const<MType,IA>::type idx_array_t;

  public:
    typedef typename vector_traits<val_array_t>::pointer value_pointer;
    typedef typename vector_traits<idx_array_t>::pointer index_pointer;

    BOOST_STATIC_CONSTANT (std::size_t, index_base = IB);

    static index_pointer index1_storage (matrix_type& cm) {
      return vector_traits<idx_array_t>::storage (cm.index1_data());
    }
    static index_pointer index2_storage (matrix_type& cm) {
      return vector_traits<idx_array_t>::storage (cm.index2_data());
    }
    static value_pointer value_storage (matrix_type& cm) {
      return vector_traits<val_array_t>::storage (cm.value_data());
    }

    static int num_rows (matrix_type& cm) { return cm.size1(); }
    static int num_columns (matrix_type& cm) { return cm.size2(); }
    static int num_nonzeros (matrix_type& cm) {
      return cm.nnz();
      // Joerg, this isn't very intuitive :o(
      // return cm.non_zeros();
    }
  };

}}}}

#endif // BOOST_NUMERIC_BINDINGS_TRAITS_UBLAS_SPARSE_MATRIX_H


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
#include <boost/test/unit_test.hpp>

#include "math/math_all.hpp"
#include "geometry/nnquadtree.hpp"

BOOST_AUTO_TEST_CASE(geometry_nnquadtree)
{
    const int N = 10000;
    BOOST_TEST_MESSAGE("* Testing NNQuadTree on " << N << " points");

    // generate random points in the unit square
    math::Points2 points;
    srand(0);
    for (int i = 0; i < N; i++)
    {
        points.emplace_back((double) rand() / RAND_MAX,
                            (double) rand() / RAND_MAX);
    }

    // insert the points into a quadtree
    geometry::NNQuadTree<math::Point2, int, 16> qtree({0,0}, {1,1});
    for (int i = 0; i < N; i++)
    {
        qtree.insert(points[i], i);
    }

    // test NN search, compare with brute force algorithm
    for (int i = 0; i < N; i++)
    {
        math::Point2 testme((double) rand() / RAND_MAX,
                            (double) rand() / RAND_MAX);

        int id = -1;
        double minDist = std::numeric_limits<double>::max();
        for (int j = 0; j < N; j++)
        {
            double d = norm_2(points[j] - testme);
            if (d < minDist) {
                minDist = d;
                id = j;
            }
        }

        // test
        BOOST_REQUIRE(id == qtree.nearest(testme));
    }
}