#include "kalman_filter.hpp"
#include "kalman_filter_cache.hpp"
#include "samplers.hpp"
#include "sampling/simple.hpp"

#include <boost/foreach.hpp>
#include <boost/random.hpp>
//#include <boost/random/exponential_distribution.hpp>
#include <boost/shared_ptr.hpp>
#include <cmath>
#include <ctime>
#include <Eigen/Cholesky>
#include <Eigen/Dense>
#include <limits>
#include <cmath>

extern "C" {
#include "third-party/gen_beta.h"
}

#include "detail/random.hpp"

namespace biggles
{

void sample_birth_rate_given_partition_orig(const partition& part_sample, model::parameters& params) {
    off_t n_total_born(part_sample.tracks().size());
    model::mean_new_tracks_per_frame(params) = sampling::sample_gamma(1.f + n_total_born, 1.f) /
        static_cast<float>(part_sample.duration());
    if (not std::isfinite(model::mean_new_tracks_per_frame(params))) {
        std::cerr << "duration = " << part_sample.duration() << std::endl;
        std::cerr << "number of tracks = " << n_total_born << std::endl;
        std::cerr << "sampled birth rate = " << model::mean_new_tracks_per_frame(params) << std::endl;
        BOOST_ASSERT(std::isfinite(model::mean_new_tracks_per_frame(params)));
    }
    //BOOST_ASSERT(model::mean_new_tracks_per_frame(params)>0.f);
    if (model::mean_new_tracks_per_frame(params) == 0.f)
        model::mean_new_tracks_per_frame(params) = std::numeric_limits<float>::min();
}

void sample_birth_rate_given_partition_alt(const partition& part_sample, model::parameters& params) {
    off_t n_total_born(part_sample.tracks().size());
    const size_t min_surv(1);
    model::mean_new_tracks_per_frame(params) = sampling::sample_gamma(1.f + n_total_born, 1.f) /
        static_cast<float>(part_sample.duration() - min_surv); // no birth in the last frames
    BOOST_ASSERT(std::isfinite(model::mean_new_tracks_per_frame(params)));
    BOOST_ASSERT(model::mean_new_tracks_per_frame(params)>0.f);
}

void sample_birth_rate_given_partition(const partition& part_sample, model::parameters& params) {
    sample_birth_rate_given_partition_orig(part_sample, params);
}

void sample_clutter_rate_given_partition(const partition& part_sample, model::parameters& params) {
    off_t n_total_false(part_sample.clutter().size());
    model::mean_false_observations_per_frame(params) = sampling::sample_gamma(1 + n_total_false, 1.f) /
        static_cast<float>(part_sample.duration());
    if (not std::isfinite(model::mean_false_observations_per_frame(params))) {
        std::cerr << "number of clutter = " << n_total_false << std::endl;
        std::cerr << "partition duration = " << part_sample.duration() << std::endl;
        std::cerr << "sampled clutter rate = " << model::mean_false_observations_per_frame(params) << std::endl;
        throw std::logic_error("clutter rate sampling returned a not finite value");
    }
    if (model::mean_false_observations_per_frame(params) == 0.f)
        model::mean_false_observations_per_frame(params) = std::numeric_limits<float>::min();
}

void sample_observation_probability_given_partition_orig(const partition& part_sample, model::parameters& params) {

    if (part_sample.tracks().empty()) {
        model::generate_observation_probability(params) = sampling::uniform_real(0.f, 1.f);
        return;
    }
    off_t n_total_generated(0);
    off_t sum_of_durations(0);
    // calculate sum_of_durations and n_total_generated
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks())
    {
        sum_of_durations += t_ptr->duration();
        // the track's size is the number of observations within it
        n_total_generated += t_ptr->size();
    }
    //BOOST_ASSERT(n_total_generated > 0); // there would be no tracks otherwise
    // the 1.0001s below should be 1 but the sampling functions for beta distributions hit an infinite loop if the sizes
    // are zero.
    // beta(1,1) is uniform distribution FIXME
    model::generate_observation_probability(params) =
        sampling::sample_beta(1.0001f + n_total_generated, 1.0001f + (sum_of_durations - n_total_generated));

}

void sample_observation_probability_given_partition_alt(const partition& part_sample, model::parameters& params) {

    if (part_sample.tracks().empty()) {
        model::generate_observation_probability(params) = sampling::uniform_real(0.f, 1.f);
        return;
    }
    off_t n_total_generated(0);
    off_t sum_of_durations(0);
    const size_t min_obs(2);
    // calculate sum_of_durations and n_total_generated
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks())
    {
        sum_of_durations += t_ptr->duration() - min_obs;
        n_total_generated += t_ptr->size() - min_obs;
    }
    BOOST_ASSERT(n_total_generated >= 0);
    BOOST_ASSERT(sum_of_durations >= n_total_generated);
    model::generate_observation_probability(params) =
        sampling::sample_beta(1.0001f + n_total_generated, 1.0001f + sum_of_durations - n_total_generated);
}

void sample_observation_probability_given_partition(const partition& part_sample, model::parameters& params) {
    sample_observation_probability_given_partition_orig(part_sample, params);
}

void sample_survival_probability_given_partition_orig(const partition& part_sample, model::parameters& params) {
    if(part_sample.tracks().empty()) {
        model::frame_to_frame_survival_probability(params) = sampling::uniform_real(0.f, 1.f);
        return;
    }

    off_t n_total_died(part_sample.tracks().size()); // everything that has a beginning has an end, Neo. Whatever.
    off_t n_total_survived(0);

    // calculate n_total_survived
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks())
    {
        // a track 'survives' for one fewer ticks than it's duration
        if(t_ptr->duration() >= 1)
            n_total_survived += t_ptr->duration() - 1;
    }
    //BOOST_ASSERT(n_total_survived > 0); // there would be no tracks otherwise
    //BOOST_ASSERT(n_total_died > 0); // there would be no tracks otherwise
    do model::frame_to_frame_survival_probability(params) = sampling::sample_beta(
            1.0001f + n_total_survived, 1.0001f + n_total_died);
    while (model::frame_to_frame_survival_probability(params) == 1.f);
}

void sample_survival_probability_given_partition_alt(const partition& part_sample, model::parameters& params) {
    if(part_sample.tracks().empty()) {
        model::frame_to_frame_survival_probability(params) = sampling::uniform_real(0.f, 1.f);
        return;
    }

    off_t n_total_died(0);
    off_t n_total_survived(0);

    const size_t min_surv(1);

    // calculate n_total_survived
    // a track 'survives' for one fewer ticks than it's duration and one survival is guaranteed
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks()) {
        n_total_survived += t_ptr->duration() - 1 - min_surv ;
        n_total_died += off_t(t_ptr->last_time_stamp() < part_sample.last_time_stamp());
    }
    BOOST_ASSERT(n_total_survived >= 0);
    BOOST_ASSERT(n_total_died >= 0);
    model::frame_to_frame_survival_probability(params) = sampling::sample_beta(1.0001f + n_total_survived, 1.0001f + n_total_died);
}

void sample_survival_probability_given_partition(const partition& part_sample, model::parameters& params) {
    sample_survival_probability_given_partition_orig(part_sample, params);
}

void sample_tracking_control_parameters_given_partition(const partition& partition, model::parameters& params) {
    sample_birth_rate_given_partition(partition, params);
    sample_clutter_rate_given_partition(partition, params);
    sample_observation_probability_given_partition(partition, params);
    sample_survival_probability_given_partition(partition, params);
}

void sample_observation_error_given_partition(const partition& partition, model::parameters& params) {
    // special case: no tracks. sample from a fairly all-encompassing distribution
    if (not is_symmetric(model::observation_error_covariance(params))) {
        std::cout << measure_asymmetry(model::observation_error_covariance(params)) << std::endl;
        throw std::runtime_error("R is not symmetric at the start");
    }

    if(partition.tracks().empty())
    {
        model::observation_error_covariance(params) = sampling::sample_inverse_wishart(Eigen::Matrix2f::Identity() * 2.f, 5);
        //model::observation_error_covariance(params) = Eigen::Matrix2f::Identity() * 0.09f; // FIXME PRIOR calculation
        return;
    }

    if (not is_symmetric(model::observation_error_covariance(params))) {
        std::cout << measure_asymmetry(model::observation_error_covariance(params)) << std::endl;
        throw std::runtime_error("R is not symmetric after track empty.");
    }

    // calculate Wishart Phi parameters for sampling R

    // this is the Wishart prior
    Eigen::Matrix2f Phi = Eigen::Matrix2f::Identity() * 2.f;
    size_t s = 5;

    if (not is_symmetric(Phi)) {
        std::cout << measure_asymmetry(Phi) << std::endl;
        throw std::runtime_error("Phi is not symmetric after init");
    }

    // update parameters for each track
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, partition.tracks())
    {
        Phi += wishart_phi_parameter_for_track(t_ptr, params);
        s += t_ptr->size(); // number of observations in track
    }

    if (not is_symmetric(Phi)) {
        std::cout << measure_asymmetry(Phi) << std::endl;
        throw std::runtime_error("Phi is not symmetric after update");
    }

    model::observation_error_covariance(params) = sampling::sample_inverse_wishart(Phi, s);
    //model::observation_error_covariance(params) = Eigen::Matrix2f::Identity() * 0.09f; // FIXME PRIOR test
    if (not is_symmetric(model::observation_error_covariance(params))) {
        std::cout << measure_asymmetry(model::observation_error_covariance(params)) << std::endl;
        throw std::runtime_error("R is not symmetric after sampling.");
    }

}

void sample_model_parameters_given_partition(const partition& partition, model::parameters& params) {
    sample_tracking_control_parameters_given_partition(partition, params);
    /*
    model::mean_new_tracks_per_frame(params) = 0.2f; // FIXME PRIOR calculation
    model::mean_false_observations_per_frame(params) = 0.01f; // FIXME PRIOR calculation
    model::generate_observation_probability(params) = .9f; // FIXME PRIOR calculation
    model::frame_to_frame_survival_probability(params) = .9f; // FIXME PRIOR calculation
    */

    // sample constraint radius [removed]

    sample_observation_error_given_partition(partition, params);

}

Eigen::Matrix2f wishart_phi_parameter_for_track(const boost::shared_ptr<const track>& track_p, model::parameters& params)
{
    const track& track(*track_p);
    Eigen::Matrix2f Phi = Eigen::Matrix2f::Zero();
    BOOST_ASSERT(is_symmetric(Phi));

    // create a Kalman filter for the track
    static kalman_filter_cache kf_cache;
    const matrix2f& R(model::observation_error_covariance(params));
    const matrix4f& Q(model::process_noise_covariance(params));
    const kalman_filter& kf(kf_cache.get(track_p, R, Q));

    // ... and a list of smoothed states and covariances
    kalman_filter::states_and_cov_deque smoothed_states_and_covariances;

    // use a _front_ inserter since we generate results in reverse order
    rts_smooth(kf, std::front_inserter(smoothed_states_and_covariances));

    // the observation matrix
    Eigen::Matrix<float, 2, 4> B;
    B << 1, 0, 0, 0,
         0, 0, 1, 0;

    // iterate over state and covariances for the track. keep a track of which time stamp this is
    time_stamp time_stamp(track.first_time_stamp());
    track::const_iterator track_obs_it(track.begin()); // also start iterating over track observations
    BOOST_FOREACH(const kalman_filter::state_covariance_pair& state_and_cov, smoothed_states_and_covariances)
    {
        BOOST_ASSERT(time_stamp < track.last_time_stamp());

        // extract the state and covariance
        const Eigen::Vector4f& state(state_and_cov.first);
        const Eigen::Matrix4f& cov(state_and_cov.second);

        // do we have an associated observation?
        if((track_obs_it != track.end()) && (t(*track_obs_it) == time_stamp))
        {
            // extract observation
            const observation& obs(*track_obs_it);

            // sample state
            Eigen::Vector4f sampled_state(sampling::sample_multivariate_gaussian(state, cov));

            // sampled observation
            Eigen::Vector2f sampled_obs = B * sampled_state;

            // calculate delta
            Eigen::Vector2f obs_vector, delta;
            obs_vector << x(obs), y(obs);
            delta = obs_vector - sampled_obs;

            // update Wishart parameter
            Phi += delta * delta.transpose();
            if (not is_symmetric(Phi)) {
                OK1(cov);
                OK(time_stamp - track.first_time_stamp());
                OK1(Phi);
                OK1(obs_vector);
                OK1(sampled_obs);
                BOOST_ASSERT(is_symmetric(Phi));
            }

            ++track_obs_it;
        }

        ++time_stamp;
    }
    if (not is_symmetric(Phi)) {
        std::cout << "Phi = " << std::endl;
        std::cout << Phi << std::endl;
        throw std::runtime_error("Phi is not symmetric");
    }
    return Phi;
}

} // namespace biggles


#include <iostream>
#include <armadillo>

int main() {
    arma::vec v0(5, arma::fill::randn);
    arma::vec v = 1.0 / (1 + arma::exp(-v0));
    std::cout << v << std::endl;
}


/***********************************************************************************
 * Copyright (c) 2018, UT-Battelle
 * All rights reserved.
 *
 * 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 xacc 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 <COPYRIGHT HOLDER> 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.
 *
 * Contributors:
 *   Initial implementation - H. Charles Zhao
 *
 **********************************************************************************/

#include <boost/math/constants/constants.hpp>
#include "ProjectQBaseListener.h"
#include "exprtk.hpp"
#include "XACC.hpp"
#include "IRProvider.hpp"
#include "IR.hpp"
#include "ProjectQToXACCListener.hpp"

using namespace projectq;

using symbol_table_t = exprtk::symbol_table<double>;
using expression_t = exprtk::expression<double>;
using parser_t = exprtk::parser<double>;

namespace xacc {
    namespace quantum {
        constexpr static double pi = boost::math::constants::pi<double>();

        ProjectQToXACCListener::ProjectQToXACCListener(std::shared_ptr<xacc::IR> ir) : ir(ir) {
            gateRegistry = xacc::getService<IRProvider>("gate");
        }

        double evalMathExpression(const std::string &expression) {
            symbol_table_t symbol_table;
            symbol_table.add_constant("pi", pi);
            expression_t expr;
            expr.register_symbol_table(symbol_table);
            parser_t parser;
            parser.compile(expression, expr);
            return expr.value();
        }

        InstructionParameter strToParam(const std::string &str) {
            double num = evalMathExpression(str);
            if (std::isnan(num)) {
                return InstructionParameter(str);
            } else {
                return InstructionParameter(num);
            }
        }

        void ProjectQToXACCListener::enterXacckernel(projectq::ProjectQParser::XacckernelContext *ctx) {
            std::vector<InstructionParameter> params;
            for (int i = 0; i < ctx->typedparam().size(); i++) {
                params.push_back(InstructionParameter(ctx->typedparam(static_cast<size_t>(i))->IDENTIFIER()->getText()));
            }
            curFunc = gateRegistry->createFunction(ctx->kernelname->getText(), {}, params);
            functions.insert({curFunc->name(), curFunc});
        }

        void ProjectQToXACCListener::exitXacckernel(projectq::ProjectQParser::XacckernelContext *ctx) {
            ir->addKernel(curFunc);
        }

        void ProjectQToXACCListener::exitKernelcall(projectq::ProjectQParser::KernelcallContext *ctx) {
            std::string gateName = ctx->kernelname->getText();
            if (functions.count(gateName)) {
                curFunc->addInstruction(functions[gateName]);
            } else {
                xacc::error("Tried calling an undefined kernel.");
            }
        }

        void ProjectQToXACCListener::exitGate(projectq::ProjectQParser::GateContext *ctx) {
            std::string gateName = ctx->gatename()->getText();
            if (gateName == "CX") gateName = "CNOT";

            std::vector<std::shared_ptr<xacc::Instruction>> instructions;
            std::shared_ptr<xacc::Instruction> instruction;

            // Check for qubit range
            if (ctx->qbitarglist()->qbit().size() == 1 && ctx->qbitarglist()->qbit(0)->INT().size() == 2) {
                int startQbit = std::stoi(ctx->qbitarglist()->qbit(0)->INT(0)->getText());
                int endQbit = std::stoi(ctx->qbitarglist()->qbit(0)->INT(1)->getText());
                if (endQbit <= startQbit)
                    xacc::error("Invalid qubit range.");
                for (int i = startQbit; i <= endQbit; i++) {
                    instruction = gateRegistry->createInstruction(gateName, { i });
                    instructions.push_back(instruction);
                }
            } else {
                std::vector<int> qubits;
                for (int i = 0; i < ctx->qbitarglist()->qbit().size(); i++) {
                    qubits.push_back(std::stoi(ctx->qbitarglist()->qbit(static_cast<size_t>(i))->INT(0)->getText()));
                }
                instruction = gateRegistry->createInstruction(gateName, qubits);
                instructions.push_back(instruction);
            }

            if (ctx->paramlist() != nullptr) {
                InstructionParameter param;
                for (int i = 0; i < ctx->paramlist()->param().size(); i++) {
                    param = strToParam(ctx->paramlist()->param(static_cast<size_t>(i))->getText());
                    for (int j = 0; j < instructions.size(); j++) {
                        instructions[j]->setParameter(i, param);
                    }
                }
            }

            for (int i = 0; i < instructions.size(); i++) {
                curFunc->addInstruction(instructions[i]);
            }
        }

        void ProjectQToXACCListener::exitMeasure(projectq::ProjectQParser::MeasureContext *ctx) {
            std::shared_ptr<xacc::Instruction> instruction;
            InstructionParameter param;

            // Check for qubit range

            if (ctx->qbit()->INT().size() == 2) {
                int startQbit = std::stoi(ctx->qbit()->INT(0)->getText());
                int endQbit = std::stoi(ctx->qbit()->INT(1)->getText());
                if (endQbit <= startQbit)
                    xacc::error("Invalid qubit range.");
                for (int i = startQbit; i <= endQbit; i++) {
                    instruction = gateRegistry->createInstruction("Measure", { i });
                    param = i;
                    instruction->setParameter(0, param);
                    curFunc->addInstruction(instruction);
                }
            } else {
                std::vector<int> qubits;
                qubits.push_back(std::stoi(ctx->qbit()->INT(0)->getText()));
                instruction = gateRegistry->createInstruction("Measure", qubits);
                param = 0;
                instruction->setParameter(0, param);
                curFunc->addInstruction(instruction);
            }
        }
    }
}


#include "pose_regression.h"
#include <Eigen/Dense>
#include <Eigen/LU>
#include <cmath>

void PoseRegression::InitializePose(const HybridPredictionContainer& predictions,
                                    const PoseRegressionPara& para,
                                    AffineXform3d* rigid_pose) {

  const vector<Keypoint>* keypoints = predictions.GetKeypoints();
  const vector<EdgeVector>* edges = predictions.GetEdgeVectors();
  const vector<SymmetryCorres>* symcorres = predictions.GetSymmetryCorres();

  // initial weights of different representations
  vector<double> weight_keypts (keypoints->size()), weight_edges (edges->size()), weight_symcorres (symcorres->size());
  for (unsigned id = 0; id < keypoints->size(); ++id)
    weight_keypts[id] = 1;
  for (unsigned id = 0; id < edges->size(); ++id)
    weight_edges[id] = 1;
  for (unsigned id = 0; id < symcorres->size(); ++id)
    weight_symcorres[id] = 1;

  // Generate 12x12 Data matrix from different representations
  Matrix<double, 12, 12> DataMatrix;
  GenerateDataMatrix(predictions, weight_keypts, weight_edges, weight_symcorres,
    para, &DataMatrix);

  // Calculate Pose initial
  vector<Vector12d> eigenVectors;
  const unsigned numEigs = 4;
  //LeadingEigenSpace(DataMatrix, numEigs, predictions, &eigenVectors);
  LeadingEigenSpace(DataMatrix, numEigs, predictions, &eigenVectors, rigid_pose);
}
/*
Compute the 12x12 matrix
*/
void PoseRegression::GenerateDataMatrix(const HybridPredictionContainer& predictions,
                                        const vector<double>& weight_keypts,
                                        const vector<double>& weight_edges,
                                        const vector<double>& weight_symcorres,
                                        const PoseRegressionPara& para,
                                        Matrix12d* data_matrix) {
  const vector<Keypoint>* keypoints = predictions.GetKeypoints();
  const vector<EdgeVector>* edges = predictions.GetEdgeVectors();
  const vector<SymmetryCorres>* symcorres = predictions.GetSymmetryCorres();

  // Step1: add contribution of keypoint to data matrix
  unsigned numKpts = keypoints->size();
  Matrix<double, 12, Dynamic> J_kpts;
  Matrix<double, Dynamic, Dynamic> W_kpts;
  J_kpts.resize(12, numKpts*3);
  W_kpts.resize(numKpts*3, numKpts*3);
  Matrix<double, 12, 12> M_kpts;

  J_kpts.fill(0.0);
  W_kpts.fill(0.0);
  M_kpts.fill(0.0);

  for (unsigned ptId = 0; ptId < numKpts; ++ptId) {
    const Keypoint& kp = (*keypoints)[ptId];
    //kp.point3D_gt
    //kp.point2D_pred
    double x_hc, y_hc, z_hc;
    x_hc = kp.point2D_pred[0];
    y_hc = kp.point2D_pred[1];
    z_hc = 1.0;

    double x_3d, y_3d, z_3d;
    x_3d = kp.point3D_gt[0];
    y_3d = kp.point3D_gt[1];
    z_3d = kp.point3D_gt[2];

    J_kpts(0, ptId * 3) = 0 * x_3d;     J_kpts(0, ptId * 3 + 1) = -z_hc * x_3d; J_kpts(0, ptId * 3 + 2) = y_hc * x_3d;
    J_kpts(1, ptId * 3) = z_hc * x_3d;  J_kpts(1, ptId * 3 + 1) = 0 * x_3d;     J_kpts(1, ptId * 3 + 2) = -x_hc * x_3d;
    J_kpts(2, ptId * 3) = -y_hc * x_3d; J_kpts(2, ptId * 3 + 1) = x_hc * x_3d;  J_kpts(2, ptId * 3 + 2) = 0 * x_3d;

    J_kpts(3, ptId * 3) = 0 * y_3d;     J_kpts(3, ptId * 3 + 1) = -z_hc * y_3d; J_kpts(3, ptId * 3 + 2) = y_hc * y_3d;
    J_kpts(4, ptId * 3) = z_hc * y_3d;  J_kpts(4, ptId * 3 + 1) = 0 * y_3d;     J_kpts(4, ptId * 3 + 2) = -x_hc * y_3d;
    J_kpts(5, ptId * 3) = -y_hc * y_3d; J_kpts(5, ptId * 3 + 1) = x_hc * y_3d;  J_kpts(5, ptId * 3 + 2) = 0 * y_3d;

    J_kpts(6, ptId * 3) = 0 * z_3d;     J_kpts(6, ptId * 3 + 1) = -z_hc * z_3d; J_kpts(6, ptId * 3 + 2) = y_hc * z_3d;
    J_kpts(7, ptId * 3) = z_hc * z_3d;  J_kpts(7, ptId * 3 + 1) = 0 * z_3d;     J_kpts(7, ptId * 3 + 2) = -x_hc * z_3d;
    J_kpts(8, ptId * 3) = -y_hc * z_3d; J_kpts(8, ptId * 3 + 1) = x_hc * z_3d;  J_kpts(8, ptId * 3 + 2) = 0 * z_3d;

    J_kpts(9, ptId * 3) = 0 * 1.0;      J_kpts(9, ptId * 3 + 1) = -z_hc * 1.0; J_kpts(9, ptId * 3 + 2) = y_hc * 1.0;
    J_kpts(10, ptId * 3) = z_hc * 1.0;  J_kpts(10, ptId * 3 + 1) = 0 * 1.0;    J_kpts(10, ptId * 3 + 2) = -x_hc * 1.0;
    J_kpts(11, ptId * 3) = -y_hc * 1.0; J_kpts(11, ptId * 3 + 1) = x_hc * 1.0; J_kpts(11, ptId * 3 + 2) = 0 * 1.0;

    // calculate weight for keypoints
    //kp.inv_half_var
    EigenSolver<Matrix2d> es_kp(kp.inv_half_var);
    W_kpts(ptId * 3, ptId * 3) = es_kp.eigenvalues().norm() * 0.02;
    W_kpts(ptId * 3 + 1, ptId * 3 + 1) = es_kp.eigenvalues().norm() * 0.02;
    W_kpts(ptId * 3 + 2, ptId * 3 + 2) = es_kp.eigenvalues().norm() * 2;
  }
  // add J_kpts into Datamatrix
  M_kpts = J_kpts * W_kpts * J_kpts.transpose();
  for (unsigned row_id = 0; row_id < 12; ++row_id) {
    for (unsigned com_id = 0; com_id < 12; ++com_id) {
      (*data_matrix)(row_id, com_id) = M_kpts(row_id, com_id) * para.gamma_kpts;
    }
  }
  //  Step2: add contribution of edge for data matrix
  unsigned numedges = edges->size();
  Matrix<double, 12, Dynamic> J_edge(12, numedges*3);
  Matrix<double, Dynamic, Dynamic> W_edge(numedges*3, numedges*3);
  Matrix<double, 12, 12> M_edge;
  J_edge.fill(0.0);
  W_edge.fill(0.0);
  M_edge.fill(0.0);

  for (unsigned edgeId = 0; edgeId < numedges; ++edgeId) {
    const EdgeVector& ev = (*edges)[edgeId];
    const Keypoint& kp_start = (*keypoints)[ev.start_id]; // edge = s - e
    const Keypoint& kp_end = (*keypoints)[ev.end_id];
    // ev.vec_pred
    // pts2d_s
    // pts3d_t
    // edge3d
    // Part1 of step2
    double x_hc, y_hc, z_hc; // denotes pts2d_s
    x_hc = kp_end.point2D_pred[0];
    y_hc = kp_end.point2D_pred[1];
    z_hc = 1.0;

    double x_3d, y_3d, z_3d; // denotes edge3d
    x_3d = kp_start.point3D_gt[0] - kp_end.point3D_gt[0];
    y_3d = kp_start.point3D_gt[1] - kp_end.point3D_gt[1];
    z_3d = kp_start.point3D_gt[2] - kp_end.point3D_gt[2];

    J_edge(0, edgeId * 3) = 0 * x_3d;     J_edge(0, edgeId * 3 + 1) = -z_hc * x_3d; J_edge(0, edgeId * 3 + 2) = y_hc * x_3d;
    J_edge(1, edgeId * 3) = z_hc * x_3d;  J_edge(1, edgeId * 3 + 1) = 0 * x_3d;     J_edge(1, edgeId * 3 + 2) = -x_hc * x_3d;
    J_edge(2, edgeId * 3) = -y_hc * x_3d; J_edge(2, edgeId * 3 + 1) = x_hc * x_3d;  J_edge(2, edgeId * 3 + 2) = 0 * x_3d;

    J_edge(3, edgeId * 3) = 0 * y_3d;     J_edge(3, edgeId * 3 + 1) = -z_hc * y_3d; J_edge(3, edgeId * 3 + 2) = y_hc * y_3d;
    J_edge(4, edgeId * 3) = z_hc * y_3d;  J_edge(4, edgeId * 3 + 1) = 0 * y_3d;     J_edge(4, edgeId * 3 + 2) = -x_hc * y_3d;
    J_edge(5, edgeId * 3) = -y_hc * y_3d; J_edge(5, edgeId * 3 + 1) = x_hc * y_3d;  J_edge(5, edgeId * 3 + 2) = 0 * y_3d;

    J_edge(6, edgeId * 3) = 0 * z_3d;     J_edge(6, edgeId * 3 + 1) = -z_hc * z_3d; J_edge(6, edgeId * 3 + 2) = y_hc * z_3d;
    J_edge(7, edgeId * 3) = z_hc * z_3d;  J_edge(7, edgeId * 3 + 1) = 0 * z_3d;     J_edge(7, edgeId * 3 + 2) = -x_hc * z_3d;
    J_edge(8, edgeId * 3) = -y_hc * z_3d; J_edge(8, edgeId * 3 + 1) = x_hc * z_3d;  J_edge(8, edgeId * 3 + 2) = 0 * z_3d;

    //Part2 of step2
    x_hc = ev.vec_pred[0];   // denote edge_pred_2d
    y_hc = ev.vec_pred[1];
    z_hc = 0.0;

    x_3d = kp_start.point3D_gt[0];// denotes pts3d_t
    y_3d = kp_start.point3D_gt[1];
    z_3d = kp_start.point3D_gt[2];

    J_edge(0, edgeId * 3) += 0 * x_3d;     J_edge(0, edgeId * 3 + 1) += -z_hc * x_3d; J_edge(0, edgeId * 3 + 2) += y_hc * x_3d;
    J_edge(1, edgeId * 3) += z_hc * x_3d;  J_edge(1, edgeId * 3 + 1) += 0 * x_3d;     J_edge(1, edgeId * 3 + 2) += -x_hc * x_3d;
    J_edge(2, edgeId * 3) += -y_hc * x_3d; J_edge(2, edgeId * 3 + 1) += x_hc * x_3d;  J_edge(2, edgeId * 3 + 2) += 0 * x_3d;

    J_edge(3, edgeId * 3) += 0 * y_3d;     J_edge(3, edgeId * 3 + 1) += -z_hc * y_3d; J_edge(3, edgeId * 3 + 2) += y_hc * y_3d;
    J_edge(4, edgeId * 3) += z_hc * y_3d;  J_edge(4, edgeId * 3 + 1) += 0 * y_3d;     J_edge(4, edgeId * 3 + 2) += -x_hc * y_3d;
    J_edge(5, edgeId * 3) += -y_hc * y_3d; J_edge(5, edgeId * 3 + 1) += x_hc * y_3d;  J_edge(5, edgeId * 3 + 2) += 0 * y_3d;

    J_edge(6, edgeId * 3) += 0 * z_3d;     J_edge(6, edgeId * 3 + 1) += -z_hc * z_3d; J_edge(6, edgeId * 3 + 2) += y_hc * z_3d;
    J_edge(7, edgeId * 3) += z_hc * z_3d;  J_edge(7, edgeId * 3 + 1) += 0 * z_3d;     J_edge(7, edgeId * 3 + 2) += -x_hc * z_3d;
    J_edge(8, edgeId * 3) += -y_hc * z_3d; J_edge(8, edgeId * 3 + 1) += x_hc * z_3d;  J_edge(8, edgeId * 3 + 2) += 0 * z_3d;

    J_edge(9, edgeId * 3) = 0 * 1.0;      J_edge(9, edgeId * 3 + 1) = -z_hc * 1.0;  J_edge(9, edgeId * 3 + 2) = y_hc * 1.0;
    J_edge(10, edgeId * 3) = z_hc * 1.0;  J_edge(10, edgeId * 3 + 1) = 0 * 1.0;     J_edge(10, edgeId * 3 + 2) = -x_hc * 1.0;
    J_edge(11, edgeId * 3) = -y_hc * 1.0; J_edge(11, edgeId * 3 + 1) = x_hc * 1.0;  J_edge(11, edgeId * 3 + 2) = 0 * 1.0;

    W_edge(edgeId * 3, edgeId * 3) = weight_edges[edgeId];
    W_edge(edgeId * 3 + 1, edgeId * 3 + 1) = weight_edges[edgeId];
    W_edge(edgeId * 3 + 2, edgeId * 3 + 2) = weight_edges[edgeId];
  }
  // add J_edge into Datamatrix
  M_edge = J_edge * W_edge * J_edge.transpose();
  for (unsigned row_id = 0; row_id < 12; ++row_id) {
    for (unsigned com_id = 0; com_id < 12; ++com_id) {
      (*data_matrix)(row_id, com_id) += M_edge(row_id, com_id) * para.gamma_edge;
    }
  }

  // Step3: add contribution of symmetry for data matrix
  unsigned numSym = symcorres->size();
  Matrix<double, 9, Dynamic> J_sym(9, numSym);
  Matrix<double, Dynamic, Dynamic> W_sym(numSym, numSym);
  Matrix<double, 9, 9> M_sym;

  J_sym.fill(0.0);
  W_sym.fill(0.0);
  M_sym.fill(0.0);

  const Vector3d &normal_gt = predictions.GetReflectionPlaneNormal();
  for (unsigned symId = 0; symId < numSym; ++symId) {
    const SymmetryCorres& sc = (*symcorres)[symId];

    double x_hc, y_hc, z_hc; // denotes sym_pred_2d
    x_hc = sc.qs1_cross_qs2[0];
    y_hc = sc.qs1_cross_qs2[1];
    z_hc = sc.qs1_cross_qs2[2];

    J_sym(0, symId) = x_hc * normal_gt[0]; J_sym(1, symId) = y_hc * normal_gt[0]; J_sym(2, symId) = z_hc * normal_gt[0];
    J_sym(3, symId) = x_hc * normal_gt[1]; J_sym(4, symId) = y_hc * normal_gt[1]; J_sym(5, symId) = z_hc * normal_gt[1];
    J_sym(6, symId) = x_hc * normal_gt[2]; J_sym(7, symId) = y_hc * normal_gt[2]; J_sym(8, symId) = z_hc * normal_gt[2];
    W_sym(symId, symId) = weight_symcorres[symId];
  }  // add J_kpts into Datamatrix
  M_sym = J_sym * W_sym * J_sym.transpose();
  for (unsigned row_id = 0; row_id < 9; ++row_id) {
    for (unsigned com_id = 0; com_id < 9; ++com_id) {
      (*data_matrix)(row_id, com_id) += M_sym(row_id, com_id) * para.gamma_sym;
    }
  }
}

// Leading eigen-space computation
void PoseRegression::LeadingEigenSpace(Matrix12d& data_matrix, const unsigned& numEigs,
                                       const HybridPredictionContainer& predictions, vector<Vector12d>* eigenVectors,
                                       AffineXform3d* rigid_pose) {
  Matrix<double, 12, 4> eigenvector;
  // SVD of data_matrix svd.singularValues()[0]
  JacobiSVD<Matrix12d> svd(data_matrix, ComputeFullV | ComputeFullU);
  for (unsigned com_id = 0; com_id < numEigs; ++com_id) {
    for (unsigned row_id = 0; row_id < 12; ++row_id)
      eigenvector(row_id, com_id) = svd.matrixV().col(11 - com_id)[row_id];
  }
  // Calculate initial coefficient(beta) of different eigenvectors
  Map<MatrixXd> A1(eigenvector.col(0).head(9).data(), 3,3);
  Map<MatrixXd> A2(eigenvector.col(1).head(9).data(), 3,3);
  Map<MatrixXd> A3(eigenvector.col(2).head(9).data(), 3,3);
  Map<MatrixXd> A4(eigenvector.col(3).head(9).data(), 3,3);

  Matrix3d B1 = A1.transpose() * A1;
  Matrix3d B2 = A1.transpose() * A2 + A2.transpose() * A1;
  Matrix3d B3 = A1.transpose() * A3 + A3.transpose() * A1;
  Matrix3d B4 = A2.transpose() * A2;
  Matrix3d B5 = A2.transpose() * A3 + A3.transpose() * A2;
  Matrix3d B6 = A3.transpose() * A3;

  // form linear system Cx = y to solve gamma(x) which contains coefficient
  Matrix6d C;
  Vector6d y(1.0, 0.0, 0.0, 1.0, 0.0, 1.0);
  Vector6d gamma;

  C(0, 0) = B1(0, 0); C(0, 1) = B2(0, 0); C(0, 2) = B3(0, 0); C(0, 3) = B4(0, 0); C(0, 4) = B5(0, 0); C(0, 5) = B6(0, 0);
  C(1, 0) = B1(0, 1); C(1, 1) = B2(0, 1); C(1, 2) = B3(0, 1); C(1, 3) = B4(0, 1); C(1, 4) = B5(0, 1); C(1, 5) = B6(0, 1);
  C(2, 0) = B1(0, 2); C(2, 1) = B2(0, 2); C(2, 2) = B3(0, 2); C(2, 3) = B4(0, 2); C(2, 4) = B5(0, 2); C(2, 5) = B6(0, 2);
  C(3, 0) = B1(1, 1); C(3, 1) = B2(1, 1); C(3, 2) = B3(1, 1); C(3, 3) = B4(1, 1); C(3, 4) = B5(1, 1); C(3, 5) = B6(1, 1);
  C(4, 0) = B1(1, 2); C(4, 1) = B2(1, 2); C(4, 2) = B3(1, 2); C(4, 3) = B4(1, 2); C(4, 4) = B5(1, 2); C(4, 5) = B6(1, 2);
  C(5, 0) = B1(2, 2); C(5, 1) = B2(2, 2); C(5, 2) = B3(2, 2); C(5, 3) = B4(2, 2); C(5, 4) = B5(2, 2); C(5, 5) = B6(2, 2);

  gamma = C.lu().solve(y);
  //project gamma in to valid coefficient space
  MatrixXd Gamma = MatrixXd::Random(3, 3);
  Gamma(0, 0) = gamma[0];  Gamma(0, 1) = gamma[1];  Gamma(0, 2) = gamma[2];
  Gamma(1, 0) = gamma[1];  Gamma(1, 1) = gamma[3];  Gamma(1, 2) = gamma[4];
  Gamma(2, 0) = gamma[2];  Gamma(2, 1) = gamma[4];  Gamma(2, 2) = gamma[5];
  // calculated sorted eigen vector and eigen value in increasing order
  SelfAdjointEigenSolver<MatrixXd> es(Gamma);
  MatrixXcd D = es.eigenvalues().asDiagonal();
  MatrixXcd V = es.eigenvectors();
  D(0, 0) = 0;
  D(1, 1) = 0;
  Gamma = (V * D * V.inverse()).real();
  // assign new gamma
  gamma[0] = Gamma(0, 0); gamma[1] = Gamma(0, 1); gamma[2] = Gamma(0, 2);
  gamma[3] = Gamma(1, 1); gamma[4] = Gamma(1, 2); gamma[5] = Gamma(2, 2);
  if (gamma[0] < 0)
    gamma *= -1.0;
  // recover initial coefficient beta from gamma with following equations:
  // gamma[0] = beta[0]^2; gamma[1] = beta[0] * beta[1]; gamma[2] = beta[0]*beta[2]
  // gamma[3] = beta[1]^2; gamma[4] = beta[1] * beta[2]; gamma[5] = beta[2]^2;
  Vector4d beta;
  double temp;
  beta[3] = 0;
  beta[0] = sqrt(gamma[0]);
  beta[1] = gamma[1] / beta[0];
  temp = sqrt(gamma[3]);
  if (gamma[1] < 0)
    temp = temp * -1.0;
  beta[1] = 0.5 * (temp + beta[1]);
  beta[2] = gamma[4] / beta[1];
  temp = gamma[2] / beta[0];
  beta[2] = 0.5 * (beta[2] + temp);

  // check the validation of betas
  Matrix3d R_init;
  Vector3d t_init;
  Matrix<double, 12, 9> At1;
  Matrix<double, 12, 3> At2;
  At1 = data_matrix.leftCols(9);
  At2 = data_matrix.rightCols(3);
  R_init = beta[0] * A1 + beta[1] * A2 + beta[2] * A3 + beta[3] * A4;

  // check1 det(R) > 0
  if (R_init.determinant() < 0) {
    beta = beta * -1.0;
    R_init = beta[0] * A1 + beta[1] * A2 + beta[2] * A3 + beta[3] * A4;
  }

  Map<VectorXd> R_vec(R_init.data(), 9, 1);
  t_init = -1.0 * (At2.transpose() * At2).lu().solve(At2.transpose() * At1 * R_vec);

 // check2 depth > 0
  const vector<Keypoint>* keypoints = predictions.GetKeypoints();
  double z = 0;
  unsigned numKpts = keypoints->size();

  for (unsigned ptId = 0; ptId < numKpts; ++ptId) {
    const Keypoint& kp = (*keypoints)[ptId];
    z = z + (R_init * kp.point3D_gt + t_init)[2];
  }

  if (z < 0){
    t_init = t_init * -1.0;
    R_vec = -1.0 * (At1.transpose() * At1).lu().solve(At1.transpose() * At2 * t_init);
    Map<Matrix3d> R_temp(R_vec.data(), 3,3);
    // map R_init into SO(3)
    JacobiSVD<Matrix3d> svd(R_temp, ComputeFullV | ComputeFullU);
    R_init = svd.matrixU() * svd.matrixV().transpose();
    if (R_init.determinant() < 0){
      Vector3d dia(1.0,1.0,-1.0);
      R_init = svd.matrixU() * dia.asDiagonal() * svd.matrixV().transpose();
    }
    t_init = -1.0 * (At2.transpose() * At2).lu().solve(At2.transpose() * At1 * R_vec);
  }

  // optimize beta and R_init simutaneously
  Matrix3d A = R_init;
  Matrix3d R;
  for(unsigned iter = 0; iter < 10; ++iter) {
    // optimize R_init
    JacobiSVD<Matrix3d> svd(A, ComputeFullV | ComputeFullU);
    if (A.determinant() > 0) {
      R = svd.matrixU() * svd.matrixV().transpose();
    }else {
      Vector3d dia(1.0, 1.0, -1.0);
      R = svd.matrixU() * dia.asDiagonal() * svd.matrixV().transpose();
    }
    // optimize betas by forming a linear system for beta
    Matrix<double, 9, 4> D;
    unsigned row_id;
    for(row_id = 0; row_id < 3; ++row_id) {
      D(row_id, 0) = A1(0, row_id); D(row_id, 1) = A2(0, row_id);
      D(row_id, 2) = A3(0, row_id); D(row_id, 3) = A4(0, row_id);
    }
    for(row_id = 3; row_id < 6; ++row_id) {
      D(row_id, 0) = A1(1, row_id - 3); D(row_id, 1) = A2(1, row_id - 3);
      D(row_id, 2) = A3(1, row_id - 3); D(row_id, 3) = A4(1, row_id - 3);
    }
    for(row_id = 6; row_id < 9; ++row_id) {
      D(row_id, 0) = A1(2, row_id - 6); D(row_id, 1) = A2(2, row_id - 6);
      D(row_id, 2) = A3(2, row_id - 6); D(row_id, 3) = A4(2, row_id - 6);
    }
    Matrix3d R_temp = R.transpose();
    Map<VectorXd> dy(R_temp.data(), 9, 1);
    beta = D.bdcSvd(ComputeFullV | ComputeFullU).solve(dy);
    A = beta[0] * A1 + beta[1] * A2 + beta[2] * A3 + beta[3] * A4;
  }

  R_init = R;
  Map<Matrix<double, 9, 1>> R_vec1(R.data(), 9, 1);
  t_init = -1.0 * (At2.transpose() * At2).lu().solve(At2.transpose() * At1 * R_vec1);
  // check R_init and t_init again
  z = 0;
  for (unsigned ptId = 0; ptId < numKpts; ++ptId) {
    const Keypoint& kp = (*keypoints)[ptId];
    z = z + (R_init * kp.point3D_gt + t_init)[2];
  }
  if (z < 0) {
    t_init = t_init * -1.0;
    R_vec = -1.0 * (At1.transpose() * At1).lu().solve(At1.transpose() * At2 * t_init);
    Map<Matrix3d> R_temp1(R_vec.data(), 3, 3);

    // map R_init into SO(3)
    JacobiSVD<Matrix3d> svd(R_temp1, ComputeFullV | ComputeFullU);
    R_init = svd.matrixU() * svd.matrixV().transpose();
    if (R_init.determinant() < 0) {
      Vector3d dia1(1.0, 1.0, -1.0);
      R_init = svd.matrixU() * dia1.asDiagonal() * svd.matrixV().transpose();
    }
    t_init = -1.0 * (At2.transpose() * At2).lu().solve(At2.transpose() * At1 * R_vec);
  }

 // Assign pose initial to rigid_pose
  for (unsigned row_id = 0; row_id < 3; ++row_id) {
    (*rigid_pose)[0][row_id] = t_init[row_id];
    for (unsigned com_id = 0; com_id < 3; ++com_id)
      (*rigid_pose)[com_id + 1][row_id] = R_init(row_id, com_id);
  }
}


// Filename: complete_iteration.cpp (part of MTL4)

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace mtl;

template <typename Matrix>
void f(Matrix& A)
{
    using traits::range_generator;
    A= 7.0;    // Set values in diagonal

    // Types of outer and inner cursor
    typedef typename range_generator<tag::major, Matrix>::type c_type;
    typedef typename range_generator<tag::nz, c_type>::type    ic_type;

    // Define the property maps
    typename traits::row<Matrix>::type               row(A);
    typename traits::col<Matrix>::type               col(A);
    typename traits::const_value<Matrix>::type       value(A);

    // Now iterate over the matrix
    for (c_type cursor= begin<tag::major>(A), cend= end<tag::major>(A); cursor != cend; ++cursor)
       for (ic_type icursor= begin<tag::nz>(cursor), icend= end<tag::nz>(cursor); icursor != icend; ++icursor)
	   std::cout << "A[" << row(*icursor) << ", " << col(*icursor) << "] = " << value(*icursor) << '\n';
}

int main(int, char**)
{
    // Define a row-major sparse and a column-major dense matrix
    compressed2D<double>                             A(3, 3);
    dense2D<double, mat::parameters<col_major> >  B(3, 3);

    f(A);
    f(B);

    return 0;
}


#include "laplacian_smoothing.h"
#include <vector>
#include <set>
// #include <unordered_set>
#include <iostream>

// #include <boost/container/flat_set.hpp>

// typedef std::vector<boost::container::flat_set<size_t>> Adjacency;
typedef std::vector<std::set<size_t> > Adjacency;
//typedef std::vector<std::unordered_set<size_t> > Adjacency;
/**
 * maps each vertex to all its neighbours
 */

template<typename T>
void swap(T& a, T&b)
{
	T tmp = a;
	a = b;
	b = tmp;
}

/**
 * generates the adjacency list for the vertices in the mesh
 * returns the adjacency list
 */
Adjacency adjacencyList(Mesh& mesh)
{
	Adjacency adjacency(mesh.vertexCount);

	size_t faceCount = mesh.faceCount;
	size_t* faces = mesh.faces;

	//#pragma omp parallel for
	for (int i = 0; i < faceCount * 3; i += 3)
	{
		//#pragma omp critical
		{
			size_t a, b, c;
			a = mesh.faces[i];
			b = faces[i + 1];
			c = faces[i + 2];
			//std::cout<<"i"<<i<<" ("<<a<<","<<b<<","<<c<<")\n";

			adjacency[a].insert(b);
			adjacency[a].insert(c);
			adjacency[b].insert(a);
			adjacency[b].insert(c);
			adjacency[c].insert(a);
			adjacency[c].insert(b);
		}
	}
	//std::cout<<"adjacency done\n";
	return adjacency;
}

void smooth(Mesh& mesh, unsigned int rounds)
{
	auto adjacency = adjacencyList(mesh);

	Point* vertices = mesh.vertices;
	Point* normals = mesh.normals;
	size_t vertexCount = mesh.vertexCount;

	Point* new_verts = new Point[vertexCount];
	Point* new_norms = new Point[vertexCount];

	for (unsigned int i = 0; i < rounds; ++i)
	{
		#pragma omp parallel for
		for (int vert = 0; vert < vertexCount; ++vert)
		{
			for (unsigned int off = 0; off < 3; ++off)
			{
				float new_vert = vertices[vert][off];
				float new_norm = normals[vert][off];
				auto& neis = adjacency[vert];
				for (auto nei : neis)
				{
					new_vert += vertices[nei][off];
					new_norm += normals[nei][off];
				}
				new_verts[vert][off] = new_vert / (neis.size() + 1);
				new_norms[vert][off] = new_norm / (neis.size() + 1);
			}

		}
		swap(normals, new_norms);
		swap(vertices, new_verts);
	}
	swap(normals, new_norms);
	swap(vertices, new_verts);
	delete[] new_norms;
	delete[] new_verts;
	mesh.normals = normals;
	mesh.vertices = vertices;
}


#include <base/eigen2hdf.hpp>
#include <base/init.hpp>
#include <base/timer.hpp>
#include <boost/program_options.hpp>
#include <ridgelet/construction/ft.hpp>
#include <ridgelet/ridgelet_frame.hpp>
#include "ridgelet/basis.hpp"

#include <fstream>
#include <iostream>
#include <sstream>

using namespace std;

void dump(const RidgeletFrame& RF)
{
  const char* fname = "assemble.h5";
  hid_t file = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  auto& lambdas = RF.lambdas();

  for (auto& lam : lambdas) {
    if (lam.t == rt_type::S) {
      auto& M = RF.get_dense(lam);
      stringstream ss;
      ss << lam;
      string dset = ss.str();
      //      cout << "creating dset: " << dset << "\n";
      eigen2hdf::save(file, dset, M);
    } else {
      auto& M = RF.get_sparse(lam);
      stringstream ss;
      ss << lam;
      string dset = ss.str();
      //      cout << "creating dset:" << dset << "\n";
      eigen2hdf::save_sparse(file, dset, M);
    }
  }
  H5Fclose(file);
  cout << "wrote ridgelet coeffs to " << fname << "\n";
}

void listb(const RidgeletFrame& RF, unsigned int rho_x, unsigned int rho_y)
{
  for (auto& lam : RF.lambdas()) {
    unsigned int tx = translation_size(lam.t, lam.j, rho_x, 0);
    unsigned int ty = translation_size(lam.t, lam.j, rho_y, 1);

    if (lam.t == rt_type::S) {
      auto& M = RF.get_dense(lam);
      stringstream ss;
      ss << lam;
      string sl = ss.str();
      stringstream dim;
      dim << M.rows() << " x " << M.cols();
      cout << setw(15) << sl << ", rt_size: " << setw(15) << dim.str() << ", T: " << setw(5) << tx
           << " x " << setw(5) << ty << "\n";
    } else {
      auto& M = RF.get_sparse(lam);
      stringstream ss;
      ss << lam;
      string sl = ss.str();
      stringstream dim;
      dim << M.rows() << " x " << M.cols();
      cout << setw(15) << sl << ", rt_size: " << setw(15) << dim.str() << ", T: " << setw(5) << tx
           << " x " << setw(5) << ty << "\n";
    }
  }
  cout << "T: translation set dimensions, rt_size: ridgelet coefficient array dimensions"
       << "\n";
}

int main(int argc, char* argv[])
{
  SOURCE_INFO();
  namespace po = boost::program_options;

  unsigned int Jx, Jy, rho_x, rho_y;

  po::options_description options("options");
  options.add_options()("help", "produce help message")
      ("Jx,i", po::value<unsigned int>(&Jx)->default_value(3), "Jx")
      ("Jy,j", po::value<unsigned int>(&Jy)->default_value(3), "Jy")
      ("rx,x", po::value<unsigned int>(&rho_x)->default_value(1), "rho_x")
      ("ry,y", po::value<unsigned int>(&rho_y)->default_value(1), "rho_x")
      ("dump", "dump coefficients to hdf5")
      ("list", "list basis and sizes");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << options << "\n";
    return 0;
  }
  cout << setw(20) << "Jx"
       << ": " << Jx << "\n"
       << setw(20) << "Jy"
       << ": " << Jy << "\n"
       << setw(20) << "rho_x"
       << ": " << rho_x << "\n"
       << setw(20) << "rho_y"
       << ": " << rho_y << "\n";

  RDTSCTimer timer;
  timer.start();
  RidgeletFrame frame(Jx, Jy, rho_x, rho_y);
  auto tlap = timer.stop();
  cout << "RidgeletFrame::RidgeletFrame(): " << tlap / 1e9 << " [Gcycles]"
       << "\n";

  cout << "frame.size: " << frame.size() << "\n";
  cout << "frame.Nx: " << frame.Nx() << "\n";
  cout << "frame.Ny: " << frame.Ny() << "\n";

  {
    ofstream fout("assemble_rt.txt");
    for (auto& lam : frame.lambdas()) {
      fout << lam << endl;
    }
    fout.close();
  }

  if (vm.count("dump")) dump(frame);

  if (vm.count("list")) listb(frame, rho_x, rho_y);

  return 0;
}


#include <iostream>                  // for std::cout
#include <utility>                   // for std::pair
#include <algorithm>                 // for std::for_each
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

using namespace boost;

int main(int, char *[])
{
    std::cout << sizeof(long unsigned int) << std::endl;
    // create a typedef for the Graph type
    typedef adjacency_list<vecS, vecS, bidirectionalS> Graph;

    // Make convenient labels for the vertices
    enum
    {
        A, B, C, D, E, N
    };
    const int num_vertices = N;
    const char *name = "ABCDE";

    // writing out the edges in the graph
    typedef std::pair<int, int> Edge;
    Edge edge_array[] =
            {Edge(A, B), Edge(A, D), Edge(C, A), Edge(D, C),
             Edge(C, E), Edge(B, D), Edge(D, E)};
    const int num_edges = sizeof(edge_array) / sizeof(edge_array[0]);

    // declare a graph object
    Graph g(num_vertices);

    // add the edges to the graph object
    for (int i = 0; i < num_edges; ++i)
        add_edge(edge_array[i].first, edge_array[i].second, g);

    Graph g1(edge_array, edge_array + sizeof(edge_array) / sizeof(Edge), num_vertices);
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 polynomials toolbox - plevl/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of polynomials components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 06/03/2011
///
#include <nt2/polynomials/include/functions/plevl.hpp>
#include <boost/simd/sdk/simd/native.hpp>

#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/include/functions/splat.hpp>

#include <boost/array.hpp>

NT2_TEST_CASE_TPL ( plevl_real__2_0,  NT2_SIMD_REAL_TYPES)
{
  using nt2::plevl;
  using nt2::tag::plevl_;
  using boost::simd::native;

  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>             vT;

  static const boost::array<T, 3 > A = {{T(2), T(3), T(4) }};

  NT2_TEST_EQUAL(plevl( nt2::splat<vT>(1), A)[0], T(10)); //1*1^3 + 2*1^2 + 3*1 +4
  NT2_TEST_EQUAL(plevl( nt2::splat<vT>(2), A)[0], T(26)); //1*2^3 + 2*2^2 + 3*2 +4
  NT2_TEST_EQUAL(plevl( nt2::splat<vT>(3), A)[0], T(58)); //1*3^3 + 2*3^2 + 3*3 +4

} // end of test for floating_


/*
 [auto_generated]
 libs/numeric/odeint/test_external/eigen/runge_kutta4.cpp

 [begin_description]
 tba.
 [end_description]

 Copyright 2013 Karsten Ahnert
 Copyright 2013 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE odeint_eigen_runge_kutta4

#include <boost/test/unit_test.hpp>

#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/external/eigen/eigen_resize.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

struct sys
{
    template< class State , class Deriv >
    void operator()( const State &x , Deriv &dxdt , double t ) const
    {
        dxdt[0] = 1.0;
    }
};

BOOST_AUTO_TEST_SUITE( eigen_runge_kutta4 )

BOOST_AUTO_TEST_CASE( compile_time_matrix )
{
    typedef Eigen::Matrix< double , 1 , 1 > state_type;
    state_type x;
    x[0] = 10.0;
    runge_kutta4< state_type , double , state_type , double , vector_space_algebra > rk4;
    rk4.do_step( sys() , x , 0.0 , 0.1 );
    BOOST_CHECK_CLOSE( x[0] , 10.1 , 1.0e-13 );

}

BOOST_AUTO_TEST_CASE( runtime_matrix )
{
    typedef Eigen::Matrix< double , Eigen::Dynamic , 1 > state_type;
    state_type x( 1 );
    x[0] = 10.0;
    runge_kutta4< state_type , double , state_type , double , vector_space_algebra > rk4;
    rk4.do_step( sys() , x , 0.0 , 0.1 );
    BOOST_CHECK_CLOSE( x[0] , 10.1 , 1.0e-13 );
}

BOOST_AUTO_TEST_CASE( compile_time_array )
{
    typedef Eigen::Array< double , 1 , 1 > state_type;
    state_type x;
    x[0] = 10.0;
    runge_kutta4< state_type , double , state_type , double , vector_space_algebra > rk4;
    rk4.do_step( sys() , x , 0.0 , 0.1 );
    BOOST_CHECK_CLOSE( x[0] , 10.1 , 1.0e-13 );

}

BOOST_AUTO_TEST_CASE( runtime_array )
{
    typedef Eigen::Array< double , Eigen::Dynamic , 1 > state_type;
    state_type x( 1 );
    x[0] = 10.0;
    runge_kutta4< state_type , double , state_type , double , vector_space_algebra > rk4;
    rk4.do_step( sys() , x , 0.0 , 0.1 );
    BOOST_CHECK_CLOSE( x[0] , 10.1 , 1.0e-13 );
}

BOOST_AUTO_TEST_SUITE_END()


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>

#include <boost/numeric/mtl/matrix/inserter.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/recursion/predefined_masks.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>

#include <boost/numeric/mtl/operation/print_matrix.hpp>
#include <boost/numeric/mtl/operation/frobenius_norm.hpp>

using namespace std;

template <typename MatrixA>
void test(MatrixA& a, const char* name)
{
    a= 0.0;
    {
	mtl::mat::inserter<MatrixA>  ins(a);
	                  ins(0, 1) << 1.0; ins(0, 2) << 4.0;
	ins(1, 0) << 1.0; ins(1, 1) << 3.0; ins(1, 2) << 4.0; ins(1, 3) << 4.0;
	                  ins(2, 1) << 9.0; ins(2, 2) << 4.0; ins(2, 3) << 2.0;
	                                    ins(3, 2) << 4.0;
    }

    std::cout << "\n" << name << " a = \n" << a << "\n";
    std::cout.precision (6);
    std::cout << "frobenius_norm(a) = " << frobenius_norm(a) << "\n"; std::cout.flush();

    MTL_THROW_IF(frobenius_norm(a) < 13.266 || frobenius_norm(a) > 13.267, mtl::runtime_error("wrong frobenius_norm"));
}

int main(int, char**)
{
    using namespace mtl;
    unsigned size= 4;

    dense2D<double>                                      dr(size, size);
    dense2D<double, mat::parameters<col_major> >      dc(size, size);
    morton_dense<double, recursion::morton_z_mask>       mzd(size, size);
    morton_dense<double, recursion::doppled_2_row_mask>  d2r(size, size);
    compressed2D<double>                                 cr(size, size);
    compressed2D<double, mat::parameters<col_major> > cc(size, size);

    dense2D<complex<double> >                            drc(size, size);
    compressed2D<complex<double> >                       crc(size, size);

    test(dr, "Dense row major");
    test(dc, "Dense column major");
    test(mzd, "Morton Z-order");
    test(d2r, "Hybrid 2 row-major");
    test(cr, "Compressed row major");
    test(cc, "Compressed column major");
    test(drc, "Dense row major complex");
    test(crc, "Compressed row major complex");

    return 0;
}


/*
 * test_DIFF.cpp
 *
 *  Created on: 2013-4-18
 *      Author: fasiondog
 */

#ifdef TEST_ALL_IN_ONE
    #include <boost/test/unit_test.hpp>
#else
    #define BOOST_TEST_MODULE test_hikyuu_indicator_suite
    #include <boost/test/unit_test.hpp>
#endif

#include <fstream>
#include <hikyuu/StockManager.h>
#include <hikyuu/indicator/crt/KDATA.h>
#include <hikyuu/indicator/crt/DIFF.h>
#include <hikyuu/indicator/crt/PRICELIST.h>

using namespace hku;

/**
 * @defgroup test_indicator_DIFF test_indicator_DIFF
 * @ingroup test_hikyuu_indicator_suite
 * @{
 */

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_DIFF ) {
    /** @arg 正常测试 */
    PriceList d;
    for (size_t i = 0; i < 10; ++i) {
        d.push_back(i);
    }

    Indicator ind = PRICELIST(d);
    Indicator diff = DIFF(ind);
    BOOST_CHECK(diff.size() == 10);
    BOOST_CHECK(diff.discard() == 1);
    BOOST_CHECK(std::isnan(diff[0]));
    for (size_t i = 1; i < 10; ++i) {
        BOOST_CHECK(diff[i] == d[i] - d[i-1]);
    }

    /** @arg operator */
    diff = DIFF();
    Indicator expect = DIFF(ind);
    Indicator result = diff(ind);
    BOOST_CHECK(expect.size() == result.size());
    for (size_t i = 0; i < result.discard(); i++) {
        BOOST_CHECK(std::isnan(result[i]));
    }
    for (size_t i = result.discard(); i < expect.size(); ++i) {
        BOOST_CHECK(result[i] == expect[i]);
    }
}

//-----------------------------------------------------------------------------
// test export
//-----------------------------------------------------------------------------
#if HKU_SUPPORT_SERIALIZATION

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_DIFF_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/DIFF.xml";

    Stock stock = sm.getStock("sh000001");
    KData kdata = stock.getKData(KQuery(-20));
    Indicator ma1 = DIFF(CLOSE(kdata));
    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(ma1);
    }

    Indicator ma2;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(ma2);
    }

    BOOST_CHECK(ma1.size() == ma2.size());
    BOOST_CHECK(ma1.discard() == ma2.discard());
    BOOST_CHECK(ma1.getResultNumber() == ma2.getResultNumber());
    for (size_t i = ma1.discard(); i < ma1.size(); ++i) {
        BOOST_CHECK_CLOSE(ma1[i], ma2[i], 0.00001);
    }
}
#endif /* #if HKU_SUPPORT_SERIALIZATION */

/** @} */


// Copyright 2015-2018 Hans Dembinski
//
// 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)

//[ guide_axis_with_uoflow_off

#include <boost/histogram.hpp>
#include <string>

int main() {
  using namespace boost::histogram;

  // create a 1d-histogram over integer values from 1 to 6
  auto h1 = make_histogram(axis::integer<int>(1, 7));
  // axis has size 6...
  assert(h1.axis().size() == 6);
  // ... but histogram has size 8, because of overflow and underflow bins
  assert(h1.size() == 8);

  // create a 1d-histogram for throws of a six-sided die without extra bins,
  // since the values cannot be smaller than 1 or larger than 6
  auto h2 = make_histogram(axis::integer<int, use_default, axis::option::none>(1, 7));
  // now size of axis and histogram is equal
  assert(h2.axis().size() == 6);
  assert(h2.size() == 6);
}

//]


/*
    tests/eigen.cpp -- automatic conversion of Eigen types

    Copyright (c) 2016 Wenzel Jakob <wenzel.jakob@epfl.ch>

    All rights reserved. Use of this source code is governed by a
    BSD-style license that can be found in the LICENSE file.
*/

#include "pybind11_tests.h"
#include "constructor_stats.h"
#include <pybind11/eigen.h>
#include <pybind11/stl.h>

#if defined(_MSC_VER)
#  pragma warning(disable: 4996) // C4996: std::unary_negation is deprecated
#endif

#include <Eigen/Cholesky>

using MatrixXdR = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

// Sets/resets a testing reference matrix to have values of 10*r + c, where r and c are the
// (1-based) row/column number.
template <typename M> void reset_ref(M &x) {
    for (int i = 0; i < x.rows(); i++) for (int j = 0; j < x.cols(); j++)
        x(i, j) = 11 + 10*i + j;
}

// Returns a static, column-major matrix
Eigen::MatrixXd &get_cm() {
    static Eigen::MatrixXd *x;
    if (!x) {
        x = new Eigen::MatrixXd(3, 3);
        reset_ref(*x);
    }
    return *x;
}
// Likewise, but row-major
MatrixXdR &get_rm() {
    static MatrixXdR *x;
    if (!x) {
        x = new MatrixXdR(3, 3);
        reset_ref(*x);
    }
    return *x;
}
// Resets the values of the static matrices returned by get_cm()/get_rm()
void reset_refs() {
    reset_ref(get_cm());
    reset_ref(get_rm());
}

// Returns element 2,1 from a matrix (used to test copy/nocopy)
double get_elem(Eigen::Ref<const Eigen::MatrixXd> m) { return m(2, 1); };

// Returns a matrix with 10*r + 100*c added to each matrix element (to help test that the matrix
// reference is referencing rows/columns correctly).
template <typename MatrixArgType> Eigen::MatrixXd adjust_matrix(MatrixArgType m) {
    Eigen::MatrixXd ret(m);
    for (int c = 0; c < m.cols(); c++) for (int r = 0; r < m.rows(); r++)
        ret(r, c) += 10*r + 100*c;
    return ret;
}

struct CustomOperatorNew {
    CustomOperatorNew() = default;

    Eigen::Matrix4d a = Eigen::Matrix4d::Zero();
    Eigen::Matrix4d b = Eigen::Matrix4d::Identity();

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};

TEST_SUBMODULE(eigen, m) {
    using FixedMatrixR = Eigen::Matrix<float, 5, 6, Eigen::RowMajor>;
    using FixedMatrixC = Eigen::Matrix<float, 5, 6>;
    using DenseMatrixR = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
    using DenseMatrixC = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>;
    using FourRowMatrixC = Eigen::Matrix<float, 4, Eigen::Dynamic>;
    using FourColMatrixC = Eigen::Matrix<float, Eigen::Dynamic, 4>;
    using FourRowMatrixR = Eigen::Matrix<float, 4, Eigen::Dynamic>;
    using FourColMatrixR = Eigen::Matrix<float, Eigen::Dynamic, 4>;
    using SparseMatrixR = Eigen::SparseMatrix<float, Eigen::RowMajor>;
    using SparseMatrixC = Eigen::SparseMatrix<float>;

    m.attr("have_eigen") = true;

    // various tests
    m.def("double_col", [](const Eigen::VectorXf &x) -> Eigen::VectorXf { return 2.0f * x; });
    m.def("double_row", [](const Eigen::RowVectorXf &x) -> Eigen::RowVectorXf { return 2.0f * x; });
    m.def("double_complex", [](const Eigen::VectorXcf &x) -> Eigen::VectorXcf { return 2.0f * x; });
    m.def("double_threec", [](py::EigenDRef<Eigen::Vector3f> x) { x *= 2; });
    m.def("double_threer", [](py::EigenDRef<Eigen::RowVector3f> x) { x *= 2; });
    m.def("double_mat_cm", [](Eigen::MatrixXf x) -> Eigen::MatrixXf { return 2.0f * x; });
    m.def("double_mat_rm", [](DenseMatrixR x) -> DenseMatrixR { return 2.0f * x; });

    // test_eigen_ref_to_python
    // Different ways of passing via Eigen::Ref; the first and second are the Eigen-recommended
    m.def("cholesky1", [](Eigen::Ref<MatrixXdR> x) -> Eigen::MatrixXd { return x.llt().matrixL(); });
    m.def("cholesky2", [](const Eigen::Ref<const MatrixXdR> &x) -> Eigen::MatrixXd { return x.llt().matrixL(); });
    m.def("cholesky3", [](const Eigen::Ref<MatrixXdR> &x) -> Eigen::MatrixXd { return x.llt().matrixL(); });
    m.def("cholesky4", [](Eigen::Ref<const MatrixXdR> x) -> Eigen::MatrixXd { return x.llt().matrixL(); });

    // test_eigen_ref_mutators
    // Mutators: these add some value to the given element using Eigen, but Eigen should be mapping into
    // the numpy array data and so the result should show up there.  There are three versions: one that
    // works on a contiguous-row matrix (numpy's default), one for a contiguous-column matrix, and one
    // for any matrix.
    auto add_rm = [](Eigen::Ref<MatrixXdR> x, int r, int c, double v) { x(r,c) += v; };
    auto add_cm = [](Eigen::Ref<Eigen::MatrixXd> x, int r, int c, double v) { x(r,c) += v; };

    // Mutators (Eigen maps into numpy variables):
    m.def("add_rm", add_rm); // Only takes row-contiguous
    m.def("add_cm", add_cm); // Only takes column-contiguous
    // Overloaded versions that will accept either row or column contiguous:
    m.def("add1", add_rm);
    m.def("add1", add_cm);
    m.def("add2", add_cm);
    m.def("add2", add_rm);
    // This one accepts a matrix of any stride:
    m.def("add_any", [](py::EigenDRef<Eigen::MatrixXd> x, int r, int c, double v) { x(r,c) += v; });

    // Return mutable references (numpy maps into eigen variables)
    m.def("get_cm_ref", []() { return Eigen::Ref<Eigen::MatrixXd>(get_cm()); });
    m.def("get_rm_ref", []() { return Eigen::Ref<MatrixXdR>(get_rm()); });
    // The same references, but non-mutable (numpy maps into eigen variables, but is !writeable)
    m.def("get_cm_const_ref", []() { return Eigen::Ref<const Eigen::MatrixXd>(get_cm()); });
    m.def("get_rm_const_ref", []() { return Eigen::Ref<const MatrixXdR>(get_rm()); });

    m.def("reset_refs", reset_refs); // Restores get_{cm,rm}_ref to original values

    // Increments and returns ref to (same) matrix
    m.def("incr_matrix", [](Eigen::Ref<Eigen::MatrixXd> m, double v) {
        m += Eigen::MatrixXd::Constant(m.rows(), m.cols(), v);
        return m;
    }, py::return_value_policy::reference);

    // Same, but accepts a matrix of any strides
    m.def("incr_matrix_any", [](py::EigenDRef<Eigen::MatrixXd> m, double v) {
        m += Eigen::MatrixXd::Constant(m.rows(), m.cols(), v);
        return m;
    }, py::return_value_policy::reference);

    // Returns an eigen slice of even rows
    m.def("even_rows", [](py::EigenDRef<Eigen::MatrixXd> m) {
        return py::EigenDMap<Eigen::MatrixXd>(
                m.data(), (m.rows() + 1) / 2, m.cols(),
                py::EigenDStride(m.outerStride(), 2 * m.innerStride()));
    }, py::return_value_policy::reference);

    // Returns an eigen slice of even columns
    m.def("even_cols", [](py::EigenDRef<Eigen::MatrixXd> m) {
        return py::EigenDMap<Eigen::MatrixXd>(
                m.data(), m.rows(), (m.cols() + 1) / 2,
                py::EigenDStride(2 * m.outerStride(), m.innerStride()));
    }, py::return_value_policy::reference);

    // Returns diagonals: a vector-like object with an inner stride != 1
    m.def("diagonal", [](const Eigen::Ref<const Eigen::MatrixXd> &x) { return x.diagonal(); });
    m.def("diagonal_1", [](const Eigen::Ref<const Eigen::MatrixXd> &x) { return x.diagonal<1>(); });
    m.def("diagonal_n", [](const Eigen::Ref<const Eigen::MatrixXd> &x, int index) { return x.diagonal(index); });

    // Return a block of a matrix (gives non-standard strides)
    m.def("block", [](const Eigen::Ref<const Eigen::MatrixXd> &x, int start_row, int start_col, int block_rows, int block_cols) {
        return x.block(start_row, start_col, block_rows, block_cols);
    });

    // test_eigen_return_references, test_eigen_keepalive
    // return value referencing/copying tests:
    class ReturnTester {
        Eigen::MatrixXd mat = create();
    public:
        ReturnTester() { print_created(this); }
        ~ReturnTester() { print_destroyed(this); }
        static Eigen::MatrixXd create() { return Eigen::MatrixXd::Ones(10, 10); }
        static const Eigen::MatrixXd createConst() { return Eigen::MatrixXd::Ones(10, 10); }
        Eigen::MatrixXd &get() { return mat; }
        Eigen::MatrixXd *getPtr() { return &mat; }
        const Eigen::MatrixXd &view() { return mat; }
        const Eigen::MatrixXd *viewPtr() { return &mat; }
        Eigen::Ref<Eigen::MatrixXd> ref() { return mat; }
        Eigen::Ref<const Eigen::MatrixXd> refConst() { return mat; }
        Eigen::Block<Eigen::MatrixXd> block(int r, int c, int nrow, int ncol) { return mat.block(r, c, nrow, ncol); }
        Eigen::Block<const Eigen::MatrixXd> blockConst(int r, int c, int nrow, int ncol) const { return mat.block(r, c, nrow, ncol); }
        py::EigenDMap<Eigen::Matrix2d> corners() { return py::EigenDMap<Eigen::Matrix2d>(mat.data(),
                    py::EigenDStride(mat.outerStride() * (mat.outerSize()-1), mat.innerStride() * (mat.innerSize()-1))); }
        py::EigenDMap<const Eigen::Matrix2d> cornersConst() const { return py::EigenDMap<const Eigen::Matrix2d>(mat.data(),
                    py::EigenDStride(mat.outerStride() * (mat.outerSize()-1), mat.innerStride() * (mat.innerSize()-1))); }
    };
    using rvp = py::return_value_policy;
    py::class_<ReturnTester>(m, "ReturnTester")
        .def(py::init<>())
        .def_static("create", &ReturnTester::create)
        .def_static("create_const", &ReturnTester::createConst)
        .def("get", &ReturnTester::get, rvp::reference_internal)
        .def("get_ptr", &ReturnTester::getPtr, rvp::reference_internal)
        .def("view", &ReturnTester::view, rvp::reference_internal)
        .def("view_ptr", &ReturnTester::view, rvp::reference_internal)
        .def("copy_get", &ReturnTester::get)   // Default rvp: copy
        .def("copy_view", &ReturnTester::view) //         "
        .def("ref", &ReturnTester::ref) // Default for Ref is to reference
        .def("ref_const", &ReturnTester::refConst) // Likewise, but const
        .def("ref_safe", &ReturnTester::ref, rvp::reference_internal)
        .def("ref_const_safe", &ReturnTester::refConst, rvp::reference_internal)
        .def("copy_ref", &ReturnTester::ref, rvp::copy)
        .def("copy_ref_const", &ReturnTester::refConst, rvp::copy)
        .def("block", &ReturnTester::block)
        .def("block_safe", &ReturnTester::block, rvp::reference_internal)
        .def("block_const", &ReturnTester::blockConst, rvp::reference_internal)
        .def("copy_block", &ReturnTester::block, rvp::copy)
        .def("corners", &ReturnTester::corners, rvp::reference_internal)
        .def("corners_const", &ReturnTester::cornersConst, rvp::reference_internal)
        ;

    // test_special_matrix_objects
    // Returns a DiagonalMatrix with diagonal (1,2,3,...)
    m.def("incr_diag", [](int k) {
        Eigen::DiagonalMatrix<int, Eigen::Dynamic> m(k);
        for (int i = 0; i < k; i++) m.diagonal()[i] = i+1;
        return m;
    });

    // Returns a SelfAdjointView referencing the lower triangle of m
    m.def("symmetric_lower", [](const Eigen::MatrixXi &m) {
            return m.selfadjointView<Eigen::Lower>();
    });
    // Returns a SelfAdjointView referencing the lower triangle of m
    m.def("symmetric_upper", [](const Eigen::MatrixXi &m) {
            return m.selfadjointView<Eigen::Upper>();
    });

    // Test matrix for various functions below.
    Eigen::MatrixXf mat(5, 6);
    mat << 0,  3,  0,  0,  0, 11,
           22, 0,  0,  0, 17, 11,
           7,  5,  0,  1,  0, 11,
           0,  0,  0,  0,  0, 11,
           0,  0, 14,  0,  8, 11;

    // test_fixed, and various other tests
    m.def("fixed_r", [mat]() -> FixedMatrixR { return FixedMatrixR(mat); });
    m.def("fixed_r_const", [mat]() -> const FixedMatrixR { return FixedMatrixR(mat); });
    m.def("fixed_c", [mat]() -> FixedMatrixC { return FixedMatrixC(mat); });
    m.def("fixed_copy_r", [](const FixedMatrixR &m) -> FixedMatrixR { return m; });
    m.def("fixed_copy_c", [](const FixedMatrixC &m) -> FixedMatrixC { return m; });
    // test_mutator_descriptors
    m.def("fixed_mutator_r", [](Eigen::Ref<FixedMatrixR>) {});
    m.def("fixed_mutator_c", [](Eigen::Ref<FixedMatrixC>) {});
    m.def("fixed_mutator_a", [](py::EigenDRef<FixedMatrixC>) {});
    // test_dense
    m.def("dense_r", [mat]() -> DenseMatrixR { return DenseMatrixR(mat); });
    m.def("dense_c", [mat]() -> DenseMatrixC { return DenseMatrixC(mat); });
    m.def("dense_copy_r", [](const DenseMatrixR &m) -> DenseMatrixR { return m; });
    m.def("dense_copy_c", [](const DenseMatrixC &m) -> DenseMatrixC { return m; });
    // test_sparse, test_sparse_signature
    m.def("sparse_r", [mat]() -> SparseMatrixR { return Eigen::SparseView<Eigen::MatrixXf>(mat); });
    m.def("sparse_c", [mat]() -> SparseMatrixC { return Eigen::SparseView<Eigen::MatrixXf>(mat); });
    m.def("sparse_copy_r", [](const SparseMatrixR &m) -> SparseMatrixR { return m; });
    m.def("sparse_copy_c", [](const SparseMatrixC &m) -> SparseMatrixC { return m; });
    // test_partially_fixed
    m.def("partial_copy_four_rm_r", [](const FourRowMatrixR &m) -> FourRowMatrixR { return m; });
    m.def("partial_copy_four_rm_c", [](const FourColMatrixR &m) -> FourColMatrixR { return m; });
    m.def("partial_copy_four_cm_r", [](const FourRowMatrixC &m) -> FourRowMatrixC { return m; });
    m.def("partial_copy_four_cm_c", [](const FourColMatrixC &m) -> FourColMatrixC { return m; });

    // test_cpp_casting
    // Test that we can cast a numpy object to a Eigen::MatrixXd explicitly
    m.def("cpp_copy", [](py::handle m) { return m.cast<Eigen::MatrixXd>()(1, 0); });
    m.def("cpp_ref_c", [](py::handle m) { return m.cast<Eigen::Ref<Eigen::MatrixXd>>()(1, 0); });
    m.def("cpp_ref_r", [](py::handle m) { return m.cast<Eigen::Ref<MatrixXdR>>()(1, 0); });
    m.def("cpp_ref_any", [](py::handle m) { return m.cast<py::EigenDRef<Eigen::MatrixXd>>()(1, 0); });

    // test_nocopy_wrapper
    // Test that we can prevent copying into an argument that would normally copy: First a version
    // that would allow copying (if types or strides don't match) for comparison:
    m.def("get_elem", &get_elem);
    // Now this alternative that calls the tells pybind to fail rather than copy:
    m.def("get_elem_nocopy", [](Eigen::Ref<const Eigen::MatrixXd> m) -> double { return get_elem(m); },
            py::arg().noconvert());
    // Also test a row-major-only no-copy const ref:
    m.def("get_elem_rm_nocopy", [](Eigen::Ref<const Eigen::Matrix<long, -1, -1, Eigen::RowMajor>> &m) -> long { return m(2, 1); },
            py::arg().noconvert());

    // test_issue738
    // Issue #738: 1xN or Nx1 2D matrices were neither accepted nor properly copied with an
    // incompatible stride value on the length-1 dimension--but that should be allowed (without
    // requiring a copy!) because the stride value can be safely ignored on a size-1 dimension.
    m.def("iss738_f1", &adjust_matrix<const Eigen::Ref<const Eigen::MatrixXd> &>, py::arg().noconvert());
    m.def("iss738_f2", &adjust_matrix<const Eigen::Ref<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>> &>, py::arg().noconvert());

    // test_issue1105
    // Issue #1105: when converting from a numpy two-dimensional (Nx1) or (1xN) value into a dense
    // eigen Vector or RowVector, the argument would fail to load because the numpy copy would fail:
    // numpy won't broadcast a Nx1 into a 1-dimensional vector.
    m.def("iss1105_col", [](Eigen::VectorXd) { return true; });
    m.def("iss1105_row", [](Eigen::RowVectorXd) { return true; });

    // test_named_arguments
    // Make sure named arguments are working properly:
    m.def("matrix_multiply", [](const py::EigenDRef<const Eigen::MatrixXd> A, const py::EigenDRef<const Eigen::MatrixXd> B)
            -> Eigen::MatrixXd {
        if (A.cols() != B.rows()) throw std::domain_error("Nonconformable matrices!");
        return A * B;
    }, py::arg("A"), py::arg("B"));

    // test_custom_operator_new
    py::class_<CustomOperatorNew>(m, "CustomOperatorNew")
        .def(py::init<>())
        .def_readonly("a", &CustomOperatorNew::a)
        .def_readonly("b", &CustomOperatorNew::b);

    // test_eigen_ref_life_support
    // In case of a failure (the caster's temp array does not live long enough), creating
    // a new array (np.ones(10)) increases the chances that the temp array will be garbage
    // collected and/or that its memory will be overridden with different values.
    m.def("get_elem_direct", [](Eigen::Ref<const Eigen::VectorXd> v) {
        py::module::import("numpy").attr("ones")(10);
        return v(5);
    });
    m.def("get_elem_indirect", [](std::vector<Eigen::Ref<const Eigen::VectorXd>> v) {
        py::module::import("numpy").attr("ones")(10);
        return v[0](5);
    });
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestMerge
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/types/pair.hpp>
#include <boost/compute/algorithm/copy_n.hpp>
#include <boost/compute/algorithm/merge.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/lambda.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(simple_merge_int)
{
    int data1[] = { 1, 3, 5, 7 };
    int data2[] = { 2, 4, 6, 8 };

    boost::compute::vector<int> v1(4, context);
    boost::compute::vector<int> v2(4, context);
    boost::compute::vector<int> v3(8, context);

    boost::compute::copy_n(data1, 4, v1.begin(), queue);
    boost::compute::copy_n(data2, 4, v2.begin(), queue);
    boost::compute::fill(v3.begin(), v3.end(), 0, queue);

    // merge v1 with v2 into v3
    boost::compute::merge(
        v1.begin(), v1.end(),
        v2.begin(), v2.end(),
        v3.begin(),
        queue
    );
    CHECK_RANGE_EQUAL(int, 8, v3, (1, 2, 3, 4, 5, 6, 7, 8));

    // merge v2 with v1 into v3
    boost::compute::merge(
        v2.begin(), v2.end(),
        v1.begin(), v1.end(),
        v3.begin(),
        queue
    );
    CHECK_RANGE_EQUAL(int, 8, v3, (1, 2, 3, 4, 5, 6, 7, 8));

    // merge v1 with v1 into v3
    boost::compute::merge(
        v1.begin(), v1.end(),
        v1.begin(), v1.end(),
        v3.begin(),
        queue
    );
    CHECK_RANGE_EQUAL(int, 8, v3, (1, 1, 3, 3, 5, 5, 7, 7));

    // merge v2 with v2 into v3
    boost::compute::merge(
        v2.begin(), v2.end(),
        v2.begin(), v2.end(),
        v3.begin(),
        queue
    );
    CHECK_RANGE_EQUAL(int, 8, v3, (2, 2, 4, 4, 6, 6, 8, 8));

    // merge v1 with empty range into v3
    boost::compute::merge(
        v1.begin(), v1.end(),
        v1.begin(), v1.begin(),
        v3.begin(),
        queue
    );
    CHECK_RANGE_EQUAL(int, 4, v3, (1, 3, 5, 7));

    // merge v2 with empty range into v3
    boost::compute::merge(
        v1.begin(), v1.begin(),
        v2.begin(), v2.end(),
        v3.begin(),
        queue
    );
    CHECK_RANGE_EQUAL(int, 4, v3, (2, 4, 6, 8));
}

BOOST_AUTO_TEST_CASE(merge_pairs)
{
    std::vector<std::pair<int, float> > data1;
    std::vector<std::pair<int, float> > data2;

    data1.push_back(std::make_pair(0, 0.1f));
    data1.push_back(std::make_pair(2, 2.1f));
    data1.push_back(std::make_pair(4, 4.1f));
    data1.push_back(std::make_pair(6, 6.1f));
    data2.push_back(std::make_pair(1, 1.1f));
    data2.push_back(std::make_pair(3, 3.1f));
    data2.push_back(std::make_pair(5, 5.1f));
    data2.push_back(std::make_pair(7, 7.1f));

    std::vector<std::pair<int, float> > data3(data1.size() + data2.size());
    std::fill(data3.begin(), data3.end(), std::make_pair(-1, -1.f));

    boost::compute::vector<std::pair<int, float> > v1(data1.size(), context);
    boost::compute::vector<std::pair<int, float> > v2(data2.size(), context);
    boost::compute::vector<std::pair<int, float> > v3(data3.size(), context);

    boost::compute::copy(data1.begin(), data1.end(), v1.begin(), queue);
    boost::compute::copy(data2.begin(), data2.end(), v2.begin(), queue);

    using ::boost::compute::lambda::_1;
    using ::boost::compute::lambda::_2;
    using ::boost::compute::lambda::get;

    boost::compute::merge(
        v1.begin(), v1.end(),
        v2.begin(), v2.end(),
        v3.begin(),
        get<0>(_1) < get<0>(_2),
        queue
    );

    boost::compute::copy(v3.begin(), v3.end(), data3.begin(), queue);

    BOOST_CHECK(v3[0] == std::make_pair(0, 0.1f));
    BOOST_CHECK(v3[1] == std::make_pair(1, 1.1f));
    BOOST_CHECK(v3[2] == std::make_pair(2, 2.1f));
    BOOST_CHECK(v3[3] == std::make_pair(3, 3.1f));
    BOOST_CHECK(v3[4] == std::make_pair(4, 4.1f));
    BOOST_CHECK(v3[5] == std::make_pair(5, 5.1f));
    BOOST_CHECK(v3[6] == std::make_pair(6, 6.1f));
    BOOST_CHECK(v3[7] == std::make_pair(7, 7.1f));
}

BOOST_AUTO_TEST_CASE(merge_floats)
{
    float data1[] = { 1.1f, 2.2f, 3.3f, 4.4f,
                      5.5f, 6.6f, 7.7f, 8.8f };
    float data2[] = { 1.0f, 2.0f, 3.9f, 4.9f,
                      6.8f, 6.9f, 7.0f, 7.1f };

    boost::compute::vector<float> v1(8, context);
    boost::compute::vector<float> v2(8, context);
    boost::compute::vector<float> v3(v1.size() + v2.size(), context);

    boost::compute::copy_n(data1, 8, v1.begin(), queue);
    boost::compute::copy_n(data2, 8, v2.begin(), queue);
    boost::compute::fill(v3.begin(), v3.end(), 0.f, queue);

    boost::compute::merge(
        v1.begin(), v1.end(),
        v2.begin(), v2.end(),
        v3.begin(),
        queue
    );
    CHECK_RANGE_EQUAL(float, 16, v3,
      (1.0f, 1.1f, 2.0f, 2.2f, 3.3f, 3.9f, 4.4f, 4.9f,
       5.5f, 6.6f, 6.8f, 6.9f, 7.0f, 7.1f, 7.7f, 8.8f)
    );
}

BOOST_AUTO_TEST_SUITE_END()


//  (C) Copyright Eric Niebler 2005.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/test/unit_test.hpp>
#include <boost/time_series/characteristic_series.hpp>
#include <boost/time_series/constant_series.hpp>
#include <boost/time_series/delta_series.hpp>
#include <boost/time_series/dense_series.hpp>
#include <boost/time_series/heaviside_series.hpp>
#include <boost/time_series/inverse_heaviside_series.hpp>
#include <boost/time_series/piecewise_constant_series.hpp>
#include <boost/time_series/clipped_series.hpp>
#include <boost/time_series/sparse_series.hpp>
#include <boost/time_series/ordered_inserter.hpp>
#include <boost/time_series/numeric/clip.hpp>

using namespace boost;
using namespace unit_test;

namespace seq = boost::sequence;
namespace rrs = boost::range_run_storage;

///////////////////////////////////////////////////////////////////////////////
// test_shift
//
template<typename Series>
void test_clip()
{
    Series base;

    time_series::make_ordered_inserter(base)
        (1, -1, 2)
        (2, 3, 5)
        (3, 6, 9)
    .commit();

    // Sanity check
    time_series::piecewise_constant_series<int> base_result;
    time_series::make_ordered_inserter(base_result)
        (1, -1, 2)
        (2, 3, 5)
        (3, 6, 9)
    .commit();
    BOOST_CHECK_EQUAL(base, base_result);

    // Test1: clip right 1
    Series test1 = time_series::clip(base, -1, 8);
    time_series::piecewise_constant_series<int> test1_result;
    time_series::make_ordered_inserter(test1_result)
        (1, -1, 2)
        (2, 3, 5)
        (3, 6, 8)
    .commit();
    BOOST_CHECK_EQUAL(test1.discretization(), 1);
    BOOST_CHECK_EQUAL(test1, test1_result);

    // Test2: clip left 1
    Series test2 = time_series::clip(base, 0, 9);
    time_series::piecewise_constant_series<int> test2_result;
    time_series::make_ordered_inserter(test2_result)
        (1, 0, 2)
        (2, 3, 5)
        (3, 6, 9)
    .commit();
    BOOST_CHECK_EQUAL(test2.discretization(), 1);
    BOOST_CHECK_EQUAL(test2, test2_result);

    // Test3: clip right 5
    Series test3 = time_series::clip(base, -1, 4);
    time_series::piecewise_constant_series<int> test3_result;
    time_series::make_ordered_inserter(test3_result)
        (1, -1, 2)
        (2, 3, 4)
    .commit();
    BOOST_CHECK_EQUAL(test3.discretization(), 1);
    BOOST_CHECK_EQUAL(test3, test3_result);

    // Test4: clip left 5
    Series test4 = time_series::clip(base, 4, 9);
    time_series::piecewise_constant_series<int> test4_result;
    time_series::make_ordered_inserter(test4_result)
        (2, 4, 5)
        (3, 6, 9)
    .commit();
    BOOST_CHECK_EQUAL(test4.discretization(), 1);
    BOOST_CHECK_EQUAL(test4, test4_result);

    // Test5: clip right 4, self-assign
    test3 = time_series::clip(test3, -1, 0);
    time_series::piecewise_constant_series<int> test5_result;
    time_series::make_ordered_inserter(test5_result)
        (1, -1, 0)
    .commit();
    BOOST_CHECK_EQUAL(test3.discretization(), 1);
    BOOST_CHECK_EQUAL(test3, test5_result);

    // Test6: clip left 4, self-assign
    test4 = time_series::clip(test4, 8, 9);
    time_series::piecewise_constant_series<int> test6_result;
    time_series::make_ordered_inserter(test6_result)
        (3, 8, 9)
    .commit();
    BOOST_CHECK_EQUAL(test4.discretization(), 1);
    BOOST_CHECK_EQUAL(test4, test6_result);
}

void test_clip2()
{
    using namespace boost;
    using namespace time_series;

    dense_series<int> d(start = -2, stop = 8, value = 5);
    clipped_series<dense_series<int> > cd = clip(d, 3, 8);

    piecewise_constant_series<int> test1_result;
    make_ordered_inserter(test1_result)
        (5, 3, 8)
    .commit();
    BOOST_CHECK_EQUAL(cd, test1_result);

    for(int i = -10; i < 20; ++i)
    {
        BOOST_CHECK_EQUAL(cd[i], test1_result[i]);
    }

    for(int i = -10; i < 20; ++i)
    {
        BOOST_CHECK_EQUAL(range_run_storage::get_at(cd, i), range_run_storage::get_at(test1_result, i));
    }

    constant_series<int> c(5);
    clipped_series<constant_series<int> > cc = clip(c, 3, 8);

    BOOST_CHECK_EQUAL(0, std::distance(seq::begin(cc), seq::end(cc)));
    BOOST_CHECK_EQUAL(3, rrs::offset(rrs::pre_run(cc)));
    BOOST_CHECK_EQUAL(5u, rrs::length(rrs::pre_run(cc)));
    BOOST_CHECK_EQUAL(5, rrs::pre_value(cc));

    clipped_series<constant_series<int> > cc2 = clip(c, 3, inf);

    BOOST_CHECK_EQUAL(0, std::distance(seq::begin(cc2), seq::end(cc2)));
    BOOST_CHECK_EQUAL(5, rrs::pre_value(cc2));
    BOOST_CHECK_EQUAL(3, rrs::offset(rrs::pre_run(cc2)));
    BOOST_CHECK(inf == rrs::end_offset(rrs::pre_run(cc2)));
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("clip test");

    void (*pfn_dense)() = &test_clip<time_series::dense_series<int> >;
    test->add(BOOST_TEST_CASE(pfn_dense));

    void (*pfn_sparse)() = &test_clip<time_series::sparse_series<int> >;
    test->add(BOOST_TEST_CASE(pfn_sparse));

    void (*pfn_piecewise_constant)() = &test_clip<time_series::piecewise_constant_series<int> >;
    test->add(BOOST_TEST_CASE(pfn_piecewise_constant));

    test->add(BOOST_TEST_CASE(&test_clip2));

    return test;
}


#include "ICPReg.h"
#include <Eigen/src/Core/IO.h>

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

bool next_iteration = false;

void
keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event,
	void* nothing)
{
	if (event.getKeySym() == "space" && event.keyDown())
		next_iteration = true;
}

ICPReg::ICPReg(QString filename_model, QString filename_data, int iterations)
	: filename_model(filename_model),filename_data(filename_data),iterations(iterations)
{

	// The point clouds we will be using
	cloud_in.reset(new PointCloudT);  // Original point cloud
	cloud_tr.reset(new PointCloudT);  // Transformed point cloud
	cloud_icp.reset(new PointCloudT);  // ICP output point cloud
	viewer.reset(new pcl::visualization::PCLVisualizer("viewer", false));
}

ICPReg::~ICPReg()
{
}

void
ICPReg::print4x4Matrix(const Eigen::Matrix4d & matrix)
{
	printf("Rotation matrix :\n");
	printf("    | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
	printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
	printf("    | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
	printf("Translation vector :\n");
	printf("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));
}

void
ICPReg::proceed(QString filename_model, QString filename_data, int iterations)
{
	pcl::console::TicToc time;
	time.tic();
	std::string file_name = filename_model.toStdString();

	emit infoRec("Loading model...");
	emit progressBarUpdate(10);

	pcl::io::loadPLYFile(file_name, *cloud_in);

	emit infoRec("Loading model finished.");
	emit progressBarUpdate(20);

						 //Downsampling
	pcl::console::print_highlight("Downsampling...\n");
	pcl::VoxelGrid<pcl::PointXYZ> grid;
	const float leaf = 0.005f;
	grid.setLeafSize(leaf, leaf, leaf);
	grid.setInputCloud(cloud_in);
	grid.filter(*cloud_in);

	emit infoRec("Downsampling finished.");
	emit progressBarUpdate(30);

	// Defining a rotation matrix and translation vector
	Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();

	// A rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
	double theta = M_PI / 8;  // The angle of rotation in radians
	transformation_matrix(0, 0) = cos(theta);
	transformation_matrix(0, 1) = -sin(theta);
	transformation_matrix(1, 0) = sin(theta);
	transformation_matrix(1, 1) = cos(theta);

	// A translation on Z axis (0.4 meters)
	transformation_matrix(2, 3) = 0.4;

	// Display in terminal the transformation matrix
	std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
	print4x4Matrix(transformation_matrix);

	// Executing the transformation
	pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);
	*cloud_tr = *cloud_icp;  // We backup cloud_icp into cloud_tr for later use

	emit infoRec("Transform finished.");
	emit progressBarUpdate(40);
	QString matrix;
	Eigen::IOFormat OctaveFmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
	std::stringstream trans;
	trans << transformation_matrix.format(OctaveFmt);
	emit infoRec(QString::fromStdString(trans.str()));

							 // The Iterative Closest Point algorithm
	time.tic();
	pcl::IterativeClosestPoint<PointT, PointT> icp;
	icp.setMaximumIterations(iterations);
	icp.setInputSource(cloud_icp);
	icp.setInputTarget(cloud_in);
	icp.align(*cloud_icp);
	icp.setMaximumIterations(1);  // We set this variable to 1 for the next time we will call .align () function
	std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc() << " ms" << std::endl;

	if (icp.hasConverged())
	{
		std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
		std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
		transformation_matrix = icp.getFinalTransformation().cast<double>();
		print4x4Matrix(transformation_matrix);

		trans.str("");
		trans << transformation_matrix.format(OctaveFmt);
		emit infoRec(QString::fromStdString(trans.str()));
	}
	else
	{
		PCL_ERROR("\nICP has not converged.\n");
		return;
	}

	viewer.reset(new pcl::visualization::PCLVisualizer("icp_demo", true));
	// Create two verticaly separated viewports
	int v1(0);
	int v2(1);
	viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);

	// The color we will be using
	float bckgr_gray_level = 0.0;  // Black
	float txt_gray_lvl = 1.0 - bckgr_gray_level;

	// Original point cloud is white
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(cloud_in, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl,
		(int)255 * txt_gray_lvl);
	viewer->addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
	viewer->addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v2", v2);

	// Transformed point cloud is green
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h(cloud_tr, 20, 180, 20);
	viewer->addPointCloud(cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);

	// ICP aligned point cloud is red
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h(cloud_icp, 180, 20, 20);
	viewer->addPointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);

	// Adding text descriptions in each viewport
	viewer->addText("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);
	viewer->addText("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);

	std::stringstream ss;
	ss << iterations;
	std::string iterations_cnt = "ICP iterations = " + ss.str();
	viewer->addText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);

	// Set background color
	viewer->setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
	viewer->setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);

	// Set camera position and orientation
	viewer->setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
	viewer->setSize(640, 512);  // Visualiser window size

								  // Register keyboard callback :
	viewer->registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);

	// Display the visualiser
	while (!viewer->wasStopped())
	{
		viewer->spinOnce();

		// The user pressed "space" :
		if (next_iteration)
		{
			// The Iterative Closest Point algorithm
			time.tic();
			icp.align(*cloud_icp);
			std::cout << "Applied 1 ICP iteration in " << time.toc() << " ms" << std::endl;

			std::stringstream info_ss;
			info_ss << "Applied 1 ICP iteration in " << time.toc() << " ms";
			emit infoRec(QString::fromStdString(info_ss.str()));
			emit progressBarUpdate(50);

			if (icp.hasConverged())
			{
				printf("\033[11A");  // Go up 11 lines in terminal output.
				printf("\nICP has converged, score is %+.0e\n", icp.getFitnessScore());
				std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl;
				transformation_matrix *= icp.getFinalTransformation().cast<double>();  // WARNING /!\ This is not accurate! For "educational" purpose only!
				print4x4Matrix(transformation_matrix);  // Print the transformation between original pose and current pose

				trans.str("");
				trans << transformation_matrix.format(OctaveFmt);
				emit infoRec(QString::fromStdString(trans.str()));

				info_ss.str("");
				info_ss << "ICP has converged, score is " << icp.getFitnessScore();
				emit infoRec(QString::fromStdString(info_ss.str()));
				emit progressBarUpdate(100);

				ss.str("");
				ss << iterations;
				std::string iterations_cnt = "ICP iterations = " + ss.str();
				viewer->updateText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");
				viewer->updatePointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2");
			}
			else
			{
				PCL_ERROR("\nICP has not converged.\n");
				return;
			}
		}
		next_iteration = false;

		//emit progressBarUpdate(100);
	}
	qDebug("123");
}

void
ICPReg::OnStarted()
{
	this->proceed(filename_model, filename_data, iterations);
	emit finished();
}

//void
//ICPReg::OnFinished()
//{
//
//}


// Copyright 2015 Conrad Sanderson (http://conradsanderson.id.au)
// Copyright 2015 National ICT Australia (NICTA)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------------

#include <armadillo>
#include "catch.hpp"

using namespace arma;

TEST_CASE("fn_find_unique_1")
  {
  mat A =
    {
    {  1,  3,  5,  6,  7 },
    {  2,  4,  5,  7,  8 },
    {  3,  5,  5,  6,  9 },
    };

  uvec indices = find_unique(A);

  uvec indices2 = { 0, 1, 2, 4, 5, 9, 10, 13, 14 };

  REQUIRE( indices.n_elem == indices2.n_elem );

  bool same = true;

  for(uword i=0; i < indices.n_elem; ++i)
    {
    if(indices(i) != indices2(i))  { same = false; break; }
    }

  REQUIRE( same == true );

  vec unique_elem = { 1, 2, 3, 4, 5, 6, 7, 8, 9 };

  REQUIRE( accu(abs( A.elem(indices) - unique_elem )) == Approx(0.0) );

  // REQUIRE_THROWS(  );
  }

TEST_CASE("fn_find_unique_2")
  {
  cx_mat A =
    {
    { cx_double(1,-1), cx_double(3, 2), cx_double(5, 2), cx_double(6, 1), cx_double(7,-1) },
    { cx_double(2, 1), cx_double(4, 4), cx_double(5, 2), cx_double(7,-1), cx_double(8, 1) },
    { cx_double(3, 2), cx_double(5, 1), cx_double(5, 3), cx_double(6, 1), cx_double(9,-9) }
    };

  uvec indices = find_unique(A);

  uvec indices2 = { 0, 1, 2, 4, 5, 6, 8, 9, 10, 13, 14 };

  REQUIRE( indices.n_elem == indices2.n_elem );

  bool same = true;

  for(uword i=0; i < indices.n_elem; ++i)
    {
    if(indices(i) != indices2(i))  { same = false; break; }
    }

  REQUIRE( same == true );

  cx_vec unique_elem =
    {
    cx_double(1,-1),
    cx_double(2, 1),
    cx_double(3, 2),
    cx_double(4, 4),
    cx_double(5, 1),
    cx_double(5, 2),
    cx_double(5, 3),
    cx_double(6, 1),
    cx_double(7,-1),
    cx_double(8, 1),
    cx_double(9,-9)
    };

  REQUIRE( accu(abs( A.elem(indices) - unique_elem )) == Approx(0.0) );

  // REQUIRE_THROWS(  );
  }


// Erwann Rogard, wrote in July 2009:
//
// This iterator is by nbecker and was found in the boost's vault.
// Changes that I made are shown by ER_2007_07
//
// arch-tag: d007cbba-7c3d-48e0-9567-af7299fe5708
#ifndef cycle_iterator2_ext_H
#define cycle_iterator2_ext_H

#include <boost/iterator/iterator_adaptor.hpp>
#include <iterator>

// See http://www.nabble.com/cycle-iterators-td25215321.html

namespace boost {

  //! This is a cycle iterator that does NOT keep track of wraparound.
  template<typename BaseIterator, typename offset_t>
  class cycle_iterator2_ext : public boost::iterator_adaptor<cycle_iterator2_ext<BaseIterator, offset_t>,
							 BaseIterator
						       >
  {
  public:
    typedef typename boost::iterator_adaptor<cycle_iterator2_ext<BaseIterator, offset_t>,
					     BaseIterator
					    > super_t;

    typedef typename super_t::difference_type difference_type;
    typedef typename super_t::reference reference;

    explicit cycle_iterator2_ext()
    :size(0),position(0) // ER_2009_07
      {}

    explicit cycle_iterator2_ext (BaseIterator const& _b, BaseIterator const& _e, offset_t offset=0) :
      // base(_b),  // ER_2009_07
      super_t(_b),
      size (std::distance (_b, _e)) {
      SetPos (offset);
    }

    template <typename OtherBase, typename OtherOffset>
    cycle_iterator2_ext (cycle_iterator2_ext<OtherBase,OtherOffset> const& other,
		     typename enable_if_convertible<OtherBase, BaseIterator>::type* = 0) :
      // base (other.base), //ER_2009_07
      super_t(other),
      size (other.size),
      position (other.position)
    {}

  private:
    friend class boost::iterator_core_access;

    void increment () {
      ++position;
      if (position >= size) {
	position -= size;
      }
    }

    void decrement () {
      --position;
      if (position < 0) {
	position += size;
      }
    }

    void SetPos (offset_t newpos) {
      position = newpos % size;
      if (position < 0)
	position += size;
    }

    void advance (difference_type n) {
      offset_t newpos = position + n;
      SetPos (newpos);
    }

    template<typename OtherBase, typename OtherOffset>
    difference_type
    distance_to (cycle_iterator2_ext<OtherBase, OtherOffset> const& y) const {
      if (size == 0)
	return 0;

      else {
	offset_t pos1 = realposition();
	offset_t pos2 = y.realposition();
	//	return -(pos1 - pos2);
	offset_t diff = pos1 - pos2;
	if (diff < 0)
	  diff += size;
	return -diff;
      }
    }

    template<typename OtherBase, typename OtherOffset>
    bool equal (cycle_iterator2_ext<OtherBase, OtherOffset> const& y) const {
      return distance_to (y) == 0;
    }

    reference dereference() const {
        //return *(base + position); //ER_2009_07
        return  *(this->base_reference() + position);
    }

    offset_t PositiveMod (offset_t x) const {
      offset_t y = x % size;
      if (y < 0)
	y += size;
      return y;
    }

  public:

    reference operator[] (difference_type n) const {
//        return *(base + PositiveMod (position + n)); //ER_2009_07
        return *(this->base_reference() + PositiveMod (position + n));
    }

    offset_t offset() const { return position; }

    offset_t realposition () const {
      return position;
    }

    //  private:

    //BaseIterator base; //ER_2009_07
    offset_t size;
    offset_t position;
  };

  template<typename offset_t, typename BaseIterator>
  cycle_iterator2_ext<BaseIterator, offset_t> make_cycle_iterator2_ext(BaseIterator b, BaseIterator e, offset_t offset=0) {
    return cycle_iterator2_ext<BaseIterator, offset_t> (b, e, offset);
  }

  template<typename BaseIterator>
  cycle_iterator2_ext<BaseIterator, int> make_cycle_iterator2_ext(BaseIterator b, BaseIterator e, int offset=0) {
    return cycle_iterator2_ext<BaseIterator, int> (b, e, offset);
  }

} //namespace boost

#endif