/******************************
      Author: Joel Veness
        Date: 2013
******************************/

#include "skipcts.hpp"
#include "fastmath.hpp"

#include <boost/random.hpp>

// adaptive KT estimator parameters
static const bool UseDiscounting = true;
static const count_t Gamma       = 0.02f;
static const count_t Discount    = 1.0f - Gamma;
static const count_t KT_Alpha = 0.0625f;
static const count_t KT_Alpha2 = KT_Alpha + KT_Alpha;

// initialise weights for switching model
static const double SplitPrior        = 0.925;
static const double LogSplitPrior   = std::log(SplitPrior);
static const double LogStopPrior    = std::log(1.0 - SplitPrior);

static const double LogOneHalf = std::log(0.5);

struct prior_t {
    double stop;
    double split;
    double skip;
};

// prior weights for either splitting, stopping or skipping
static const prior_t SplitSkipPrior[] = {
    { LogStopPrior,    LogSplitPrior,   std::log(0.0)   },
    { std::log(0.075), std::log(0.85),  std::log(0.075) },
    { std::log(0.075), std::log(0.85),  std::log(0.075) },
    { std::log(0.075), std::log(0.85),  std::log(0.075) },
    { std::log(0.075), std::log(0.85),  std::log(0.075) },
    { std::log(0.075), std::log(0.85),  std::log(0.075) },
};

// zobrist key generation for context hashing
typedef boost::mt19937 randsrc_t;
typedef boost::variate_generator<randsrc_t &, boost::uniform_int<uint64_t> > zobrng_t;
static randsrc_t randsrc(666);
static boost::uniform_int<uint64_t> uint64_random_range(0, std::numeric_limits<uint64_t>::max());
static zobrng_t zobrng(randsrc, uint64_random_range);

// precomputed static tables
zobhash_t SkipCTS::s_zobtbl[MaxDepth][2];
weight_t SkipCTS::s_log_tbl[LogTblSize];

/* adds two numbers represented in the logarithmic domain */
inline static double fast_logadd(double log_x, double log_y) {

    if (log_x < log_y) {
        return fast_jacoblog(log_y - log_x) + log_x;
    } else {
        return fast_jacoblog(log_x - log_y) + log_y;
    }
}

/* skip nodes */
SkipNode::SkipNode() :
    m_log_prob_est(LogStopPrior),
    m_log_prob_split(LogSplitPrior),
    m_log_prob_weighted(0.0),
    m_log_skip_lik(NULL),
    m_depth(-1),
    m_buf(0.0)
{
    m_count[0] = 0.0f;
    m_count[1] = 0.0f;
}

SkipNode::~SkipNode() {
    delete [] m_log_skip_lik;
}

/* compute the logarithm of the KT-estimator update multiplier */
inline double SkipNode::logKTMul(bit_t b) const {

    return fast_log(ktMul(b));
}

/* compute the logarithm of the KT-estimator update multiplier */
inline double SkipNode::ktMul(bit_t b) const {

    count_t kt_mul_numer = m_count[b] + KT_Alpha;
    count_t kt_mul_denom = m_count[0] + m_count[1] + KT_Alpha2;

    return kt_mul_numer / kt_mul_denom;
}

/* update the KT estimates */
inline void SkipNode::updateKT(bit_t b, double log_est_mul) {

    if (UseDiscounting) {
        m_count[0] *= Discount;
        m_count[1] *= Discount;
    }
    m_count[b]++;
}

/* precompute the zobrist hash keys used for context hashing. */
void SkipCTS::initZobrist() {

    static bool init = false;

    if (!init) {
        for (int i=0; i < SkipCTS::MaxDepth; i++) {
            s_zobtbl[i][0] = zobrng();
            s_zobtbl[i][1] = zobrng();
        }
        init = true;
    }
}

/* initialise a table of precomputed logarithms */
void SkipCTS::initLogTbl() {

    static bool init = false;

    if (!init) {
        for (int i=0; i < SkipCTS::LogTblSize; i++) {
            s_log_tbl[i] = std::log(static_cast<double>(i));
        }
        init = true;
    }
}

/* initialise the hash deltas to save having to recompompute each
   zobrist hash key from scratch. */
void SkipCTS::initHashDeltas() {

    // compute the hash deltas
    indices_t delta;

    for (int i=m_depth; i >= 0; i--) {

        indices_list_t &il = m_indices[i];

        for (int j=0; j < il.size(); j++) {

            indices_t &idxs = il[j];
            indices_t diff(m_depth);

            indices_t::iterator it = std::set_symmetric_difference(
                delta.begin(), delta.end(), idxs.begin(),
                idxs.end(), diff.begin()
            );
            diff.resize(it - diff.begin());
            delta = idxs;
            idxs = diff;
        }
    }
}

/* initialise auxilary information such as number of remaining skips,
   and the depth of the context. */
void SkipCTS::initAuxInfo() {

    // initialise the auxilary information
    for (int i=0; i <= m_depth; i++) {

        m_auxinfo.push_back(aux_info_list_t());
        indices_list_t &il = m_indices[i];

        for (int j=0; j < il.size(); j++) {

            m_auxinfo.back().push_back(aux_info_t());

            // precompute last index
            m_auxinfo.back().back().last_idx   = -1;
            if (!il[j].empty())
                m_auxinfo.back().back().last_idx = static_cast<int>(il[j].back());

            // precompute number of skips remaining
            m_auxinfo.back().back().skips_left = m_skips;
            for (size_t k=0; k < il[j].size(); k++) {
                if ((k == 0 &&  il[j][k] > 0) || (k > 0 &&  il[j][k] >  il[j][k-1]+1))
                    m_auxinfo.back().back().skips_left--;
            }
        }
    }
}

/* compute the indices for the zobrist hashes, on a per depth basis */
void SkipCTS::initIndices() {

    for (size_t i=0; i <= m_depth; i++)
        m_indices.push_back(indices_list_t());

    skip_contexts_t::const_iterator it = m_skip_contexts.begin();
    for ( ; it != m_skip_contexts.end(); ++it) {

        const std::string &s = *it;
        int depth = static_cast<int>(std::count(s.begin(), s.end(), 'b'));
        m_indices[depth].push_back(indices_t());

        for (size_t j=0; j < s.length(); j++) {
            if (s[j] == 'b') m_indices[depth].back().push_back(j);
        }
    }
}

/* precomputes the context indices */
void SkipCTS::initContexts() {

    // generate contexts
    m_skip_contexts.clear();
    genBoundedSkipContexts("", m_depth, m_skips);

    initIndices();

    m_skip_contexts.clear();
}

/* create the skip context tree */
SkipCTS::SkipCTS(history_t &history, int depth, int max_skips, size_t log2_slots) :
    m_history(history),
    m_depth(depth),
    m_skips(max_skips),
    m_log_skip_preds(MaxDepth),
    m_nodes(new SkipNode[size_t(1) << log2_slots]),
    m_mask((size_t(1) << log2_slots)-1)
{
    initZobrist();
    initContexts();
    initAuxInfo();
    initHashDeltas();
    initLogTbl();
}

SkipCTS::~SkipCTS() {
    delete [] m_nodes;
}

/* the logarithm of the probability of all processed experience */
double SkipCTS::logBlockProbability() const {

    return getNode(0, 0, numSubmodels(-1, m_skips)).logProbWeighted();
}

/* compute the switching rate for a given time t */
double SkipCTS::switchRate(size_t t) const {

    return 1.0 / double(t - m_depth + 3);
}

int SkipCTS::numSubmodels(int position, int skips) const {

    int n = m_depth - position;
    return (skips == 0) ? std::min(n, 2) : n;
}

/* the probability of seeing a particular symbol next */
double SkipCTS::prob(bit_t b) {

    // We proceed as with update(), except that we keep track of the symbol probability at
    // each node instead of actually updating parameters
    getContext();

    zobhash_t hash = 0;
    int skips_left, last_idx;
    double symbolLogProb = LogOneHalf;

    // propagate the symbol probability from leaves to root
    for (int i=m_depth; i >= 0; i--) {

        // update the KT statistics, then the weighted
        // probability for every node on this level
        const indices_list_t &il = m_indices[i];

        for (int j=0; j < il.size(); j++) {

            getContextInfo(hash, il[j]);
            skips_left = m_auxinfo[i][j].skips_left;
            last_idx   = m_auxinfo[i][j].last_idx;

            // update the node
            int n_submodels = numSubmodels(last_idx, skips_left);

            SkipNode &n = getNode(hash, i, n_submodels);

            // handle the stop case
            double log_est_mul = n.logKTMul(b);
            if (n_submodels == 1) {
                n.m_buf = symbolLogProb = log_est_mul;
                continue;
            }

            // Here we rely on the property that log_prob_est and the like are unnormalized
            // posteriors. we accumulate the symbol log probability under 'symbolLogProb'
            symbolLogProb = n.m_log_prob_est + log_est_mul;

            // handle the split case
            zobhash_t delta = s_zobtbl[last_idx+1][m_context[last_idx+1]];
            const SkipNode &nn = getNode(hash ^ delta, i+1, numSubmodels(last_idx+1, skips_left));
            // recall that m_buf contains the symbol probability at the child node
            double log_split_pred = nn.m_buf;
            symbolLogProb = fast_logadd(symbolLogProb, n.m_log_prob_split + log_split_pred);

            // handle the skipping case
            if (n_submodels > 2) {

                // if we did not yet allocate these models, this node must never have been
                // updated. We assume (perhaps incorrectly) that none of this node's children
                // exist and pretend they return a symbol probability of 0.5
                if (n.m_log_skip_lik == NULL) {

                    const prior_t &p = SplitSkipPrior[skips_left];
                    symbolLogProb = fast_logadd(symbolLogProb, p.skip + LogOneHalf);
                }

                // mix in the symbol probability from the skipping models
                else for (int k=last_idx+2; k < m_depth; k++) {

                    zobhash_t h = hash ^ s_zobtbl[k][m_context[k]];
                    SkipNode &sn = getNode(h, i+1, numSubmodels(k, skips_left - 1));

                    double log_skip_pred = sn.m_buf;
                    int z = k - last_idx - 2;
                    symbolLogProb = fast_logadd(symbolLogProb, n.m_log_skip_lik[z] + log_skip_pred);
                }
            }

            // Finally we normalize by the mixture probability at this node
            symbolLogProb -= n.m_log_prob_weighted;
            n.m_buf = symbolLogProb;

            assert(n.m_buf < 0.0);
        }
    }

    // our scheme assumes that the last node processed is the root; the variable 'symbolLogProb'
    // contains its symbol probability
    return fast_exp(symbolLogProb);
}

/* gets the node's index into the hash table */
inline SkipNode &SkipCTS::getNode(zobhash_t hash, int depth, int submodels) const {

    size_t key = static_cast<size_t>(hash & m_mask);

    // do a linear scan till we find either an empty slot, or
    // a populated slot with matching depth
    do {
        if (m_nodes[key].m_depth == -1) break;
        if (m_nodes[key].m_depth == depth && m_nodes[key].m_submodels == submodels) break;
        key = (key + 1) & m_mask;
    } while (true);

    // mark the slot as used
    m_nodes[key].m_depth = depth;
    m_nodes[key].m_submodels = submodels;

    return m_nodes[key];
}

/* performs the update operation to maintain a switching posterior. */
void SkipCTS::posteriorUpdate(
    SkipNode &n, double log_scale, double log_alpha,
    double log_K, double log_mul, double &log_post
) const {

    log_post = log_scale +
        fast_logadd(
            log_alpha + n.m_log_prob_weighted,
            log_K + log_post + log_mul
        );
}

/* lazy allocation of skipping posterior weights. */
void SkipCTS::lazyAllocate(SkipNode &n, int n_submodels, int skips_left) const {

    n.m_log_skip_lik = new weight_t[n_submodels-2];

    const prior_t &p = SplitSkipPrior[skips_left];
    n.m_log_prob_est    = p.stop;
    n.m_log_prob_split  = p.split;
    for (int k=0; k < n_submodels-2; k++) {
        n.m_log_skip_lik[k] = p.skip - s_log_tbl[n_submodels-2];
    }
}

/* compute the information needed to update the current context stats. */
void SkipCTS::getContextInfo(zobhash_t &hash, const indices_t &idxs) const {

    // compute the hash
    for (int k=0; k < idxs.size(); k++) {
        size_t x = idxs[k];
        hash ^= s_zobtbl[x][m_context[x]];
    }
}

/* process a new piece of sensory experience */
void SkipCTS::update(bit_t b) {

    getContext();

    double alpha = switchRate(m_history.size());
    double log_alpha = fast_log(alpha);

    zobhash_t hash = 0;
    int skips_left, last_idx;

    // update nodes from deepest to shallowest
    for (int i=m_depth; i >= 0; i--) {

        // update the KT statistics, then the weighted
        // probability for every node on this level
        const indices_list_t &il = m_indices[i];

        for (int j=0; j < il.size(); j++) {

            m_log_skip_preds.clear();

            getContextInfo(hash, il[j]);
            skips_left = m_auxinfo[i][j].skips_left;
            last_idx   = m_auxinfo[i][j].last_idx;

            // update the node
            int n_submodels = numSubmodels(last_idx, skips_left);

            SkipNode &n = getNode(hash, i, n_submodels);
            n.m_buf = n.m_log_prob_weighted;

            // lazy allocation of skipping prior weights
            if (n_submodels > 2 && n.m_log_skip_lik == NULL)
                lazyAllocate(n, n_submodels, skips_left);

            // handle the stop case
            double log_est_mul = n.logKTMul(b);
            if (n_submodels == 1) {
                n.updateKT(b, log_est_mul);
                n.m_log_prob_weighted += log_est_mul;
                n.m_buf = log_est_mul;
                continue;
            }

            double log_acc = n.m_log_prob_est + log_est_mul;
            n.updateKT(b, log_est_mul);

            // handle the split case
            zobhash_t delta = s_zobtbl[last_idx+1][m_context[last_idx+1]];
            const SkipNode &nn = getNode(hash ^ delta, i+1, numSubmodels(last_idx+1, skips_left));
            double log_split_pred = nn.m_buf;
            log_acc = fast_logadd(log_acc, n.m_log_prob_split + log_split_pred);

            // handle the skipping case
            if (n_submodels > 2) {

                // update the skipping models
                for (int k=last_idx+2; k < m_depth; k++) {

                    zobhash_t h = hash ^ s_zobtbl[k][m_context[k]];
                    SkipNode &sn = getNode(h, i+1, numSubmodels(k, skips_left - 1));

                    double log_skip_pred = sn.m_buf;
                    m_log_skip_preds.push_back(log_skip_pred);
                    int z = k - last_idx - 2;
                    log_acc = fast_logadd(log_acc, n.m_log_skip_lik[z] + log_skip_pred);
                }
            }

            // store the weighted probability
            n.m_log_prob_weighted = log_acc;

            assert(n.m_log_prob_weighted < n.m_buf);
            // Store the *difference* in log probability in m_buf
            n.m_buf = n.m_log_prob_weighted - n.m_buf;

            updatePosteriors(n, n_submodels, alpha, log_alpha, log_est_mul, log_split_pred);
        }
    }

    m_history.push_back(b != 0);
}

/* update the switching posterior weights */
void SkipCTS::updatePosteriors(SkipNode &n, int n_submodels, double alpha,
    double log_alpha, double log_stop_mul, double log_split_mul) {

    // update switching log-posteriors
    double dn        =  static_cast<double>(n_submodels);
    double K         =  (1.0 - alpha) * dn - 1.0;
    double log_K     =  fast_log(K);
    double log_scale = -s_log_tbl[n_submodels-1];

    posteriorUpdate(n, log_scale, log_alpha, log_K, log_stop_mul, n.m_log_prob_est);
    posteriorUpdate(n, log_scale, log_alpha, log_K, log_split_mul, n.m_log_prob_split);
    for (int k=0; k < m_log_skip_preds.size(); k++) {
        posteriorUpdate(n, log_scale, log_alpha, log_K, m_log_skip_preds[k], n.m_log_skip_lik[k]);
    }
}

/* generate possible context strings with bounded # of skips */
void SkipCTS::genBoundedSkipContexts(std::string buf, int depth, int skips) {

    m_skip_contexts.push_back(buf);

    if (depth == 0) return;

    if (skips > 0) {
        std::string skipstr = "*";
        for (int i=0; i < depth-1; i++) {
            genBoundedSkipContexts(buf + skipstr + "b", depth - (i+1) - 1, skips-1);
            skipstr.append("*");
        }
    }

    genBoundedSkipContexts(buf + "b", depth - 1, skips);
}

/* prints a set of skips contexts */
void SkipCTS::printContexts(skip_contexts_t &l) const {

    skip_contexts_t::const_iterator it = l.begin();
    for (; it != l.end(); ++it) {
        std::cout << *it << std::endl;
    }
}

/* compute the current binary context */
inline void SkipCTS::getContext() {

    size_t offset = m_history.size();
    m_context.clear();

    for (size_t i=0; i < m_depth; ++i) {
        m_context.push_back(m_history[offset-i-1]);
    }
}


/*
*  Copyright 2017 Pedro Proenca <p.proenca@surrey.ac.uk> (University of Surrey)
*
*  TODO: Out of Bounds
*/

#include "Dataset.h"
#include "OMGUtilities.hpp"
#include <fstream>
#include <string.h>
#include <stdio.h>
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>

using namespace std;

int main(int argc, char ** argv)
{
	cout<<"Nr Threads: "<<Eigen::nbThreads()<<endl;

	string dataset_name;
	string sequence;
	int MAX_NR_FUSION_FRAMES;
	float consistency_threshold;
	if (argc>2){
		MAX_NR_FUSION_FRAMES = atoi(argv[1]);
		consistency_threshold = atof(argv[2]);
		dataset_name = argv[3];
		sequence = argv[4];
	}else{
		cout<<"Insert dataset name:";
		cin>>dataset_name;
		cout<<"Insert sequence name:";
		cin>>sequence;
		cout<<"Insert maximum number of frames (default 5): ";
		cin>>MAX_NR_FUSION_FRAMES;
		cout<<"Insert consistency criteria: (default 9): ";
		cin>>consistency_threshold;
	}

	stringstream string_buff;
	string data_path = "../../Data/";
	int dataset_type = 0;

	// Get sequence's file structure
	string_buff<<data_path<<dataset_name<<"/"<<sequence;
	Dataset dataset(string_buff.str(),dataset_name);
	dataset.load_and_align_pose_gt(string_buff.str());

	// Get intrinsics
	cv::Mat K_rgb, K_ir, dist_coeffs_rgb, dist_coeffs_ir, R_stereo, t_stereo;
	string_buff<<"/calib_params.xml";
	utils::loadCalibParameters(string_buff.str(), K_rgb, dist_coeffs_rgb, K_ir, dist_coeffs_ir, R_stereo, t_stereo);
	float fx = K_rgb.at<double>(0,0); float fy = K_rgb.at<double>(1,1); float cx = K_rgb.at<double>(0,2);
	float cy = K_rgb.at<double>(1,2);

	// Read frame 1 to allocate and get dimension
	cv::Mat rgb_img = cv::imread(dataset.rgb_files[0], cv::IMREAD_COLOR);
	cv::Mat d_img = cv::imread(dataset.d_files[0], cv::IMREAD_ANYDEPTH);
	int width = rgb_img.cols;
	int height = rgb_img.rows;
	/* -------------------- Allocation & pre-computation -------------------- */
	// Used for backprojection
	cv::Mat_<float> X_pre(height,width);
	cv::Mat_<float> Y_pre(height,width);
	// Incidence angle used to convert depth to range
	cv::Mat_<float> cos_alpha(height,width);
	cv::Mat_<float> cos_alpha_sqr(height,width);
	for (int r=0;r<height; r++){
		for (int c=0;c<width; c++){
			float u = c-cx;
			float v = r-cy;
			float D = sqrt(u*u+v*v);
			X_pre.at<float>(r,c) = u/fx; Y_pre.at<float>(r,c) = v/fy;
			cos_alpha.at<float>(r,c) = cos(atan(D/fx));
		}
	}
	cos_alpha_sqr = cos_alpha.mul(cos_alpha);

	int mxn = height*width;
	float * point_cloud_array = (float*)malloc(3*mxn*sizeof(float));
	float * point_range_array = (float*)malloc(mxn*sizeof(float));
	float * point_var_array = (float*)malloc(mxn*sizeof(float));
	float * point_var_reg_array = (float*)malloc(MAX_NR_FUSION_FRAMES*mxn*sizeof(float));
	Eigen::MatrixXf cloud_reg(MAX_NR_FUSION_FRAMES*mxn,3);
	Eigen::MatrixXf cloud_frame(mxn,3);
	Eigen::ArrayXf range_frame(mxn,1);
	Eigen::ArrayXf range_reg(MAX_NR_FUSION_FRAMES*mxn,1);
	Eigen::ArrayXf range_reg_sqr(MAX_NR_FUSION_FRAMES*mxn,1);
	cv::Mat_<float> d_sensor_model_uncertainty(height,width);
	cv::Mat_<float> d_conv(height,width);
	cv::Mat_<float> d_conv_uncertainty(height,width); // output
	cv::Mat_<float> d_sqr(height,width);
	cv::Mat_<float> normalizer(height,width); // temporary
	cv::Mat_<float> range_map(height,width);
	cv::Mat_<float> X(height,width);
	cv::Mat_<float> Y(height,width);
	cv::Mat_<float> range_cov_map(height,width);;
	cv::Mat_<float> range_OMG_map(height,width);
	cv::Mat_<float> range_OMG_var_map(height,width);
	cv::Mat_<float> depth_OMG_map(height,width);
	cv::Mat_<float> depth_OMG_var_map(height,width);
	cv::Mat_<int> fusion_counter(height,width);
	cv::Mat_<cv::Vec3f> depth_map_jet(height,width);
	cv::Mat_<float> norm_weights(height,width);
	Eigen::ArrayXi U_reg(MAX_NR_FUSION_FRAMES*mxn,1);
	Eigen::ArrayXi V_reg(MAX_NR_FUSION_FRAMES*mxn,1);
	int nr_points_acc = 0;
	int * frame_ptr = (int*)malloc(MAX_NR_FUSION_FRAMES*sizeof(int));
	for(int j=0; j<MAX_NR_FUSION_FRAMES; j++)
		frame_ptr[j] = 0;

	Eigen::Matrix4d T_old;

	/* ---------------------------------------------------------------------- */
	cv::namedWindow("Fused Depth");
	cv::namedWindow("Raw Depth");
	cv::namedWindow("Fused Depth Uncertainty");

	for(int i=0; i<dataset.rgb_tstamps.size(); i++){

		// Read frame i
		rgb_img = cv::imread(dataset.rgb_files[i], cv::IMREAD_COLOR);
		d_img = cv::imread(dataset.d_files[i], cv::IMREAD_ANYDEPTH);
		d_img.convertTo(d_img, CV_32F);
		d_img = d_img/5;

		// Removing depth image margins due to extreme noise as suggested in the paper
		if (dataset_name.compare("ICL_NUIM")==0){
			d_img(cv::Range(0,5),cv::Range(0,width)) = 0.0f;
			d_img(cv::Range(height-5,height),cv::Range(0,width)) = 0.0f;
			d_img(cv::Range(0,height),cv::Range(0,5)) = 0.0f;
			d_img(cv::Range(0,height),cv::Range(width-5,width)) = 0.0f;
		}

		double t1 = cv::getTickCount();
		// Kinect 1 sensor model uncertainty according to Khoshelham and Elberink:
		// sigma_z = 1.425*10^(-6)*Z^2
		cv::pow(d_img.mul(d_img)*0.000001425,2,d_sensor_model_uncertainty);

		/* -------------------- Convolution of Gaussian Mixtures -------------------- */
		cv::Mat kernel = (cv::Mat_<float>(3,3) << 1.0f, 2.0f, 1.0f, 2.0f, 4.0f, 2.0f, 1.0f, 2.0f, 1.0f); // kernel
		// Obtain normalizing factor S
		cv::min(d_img, 1, normalizer);
		cv::filter2D(normalizer,normalizer,-1,kernel,cv::Point(-1,-1),0,cv::BORDER_DEFAULT);
		// Obtain MG mean
		cv::filter2D(d_img,d_conv,-1,kernel,cv::Point(-1,-1),0,cv::BORDER_DEFAULT);
		cv::divide(d_conv,normalizer,d_conv,1);
		// Obtain MG var
		cv::pow(d_img,2,d_sqr);
		cv::filter2D(d_sqr+d_sensor_model_uncertainty,d_conv_uncertainty,-1,kernel,cv::Point(-1,-1),0,cv::BORDER_DEFAULT);
		cv::divide(d_conv_uncertainty,normalizer,d_conv_uncertainty,1);
		d_conv_uncertainty = d_conv_uncertainty - d_conv.mul(d_conv);

		/* --------------------- Convert depth to range --------------------- */
		cv::divide(d_conv,cos_alpha,range_map,1);
		cv::divide(d_conv_uncertainty,cos_alpha_sqr,range_cov_map,1);
		/* ------------------------------------------------------------------ */
		// Backproject to point cloud
		X = X_pre.mul(d_conv); Y = Y_pre.mul(d_conv);
		// Do housecleaning to remove Zeros or NaNs
		int nr_valid_pts = utils::remove_invalid_points(X, Y, d_conv, range_map, range_cov_map, point_cloud_array, point_range_array, point_var_array);
		cloud_frame = Eigen::Map<Eigen::MatrixXf>(point_cloud_array, mxn, 3);
		range_frame = Eigen::Map<Eigen::ArrayXf>(point_range_array, mxn, 1);

		/* -------------------- Remove old measurements -------------------- */
		// Just shifting data
		int offset  = frame_ptr[1];
		nr_points_acc -= offset;
		cloud_reg.block(0,0,nr_points_acc,3) = cloud_reg.block(offset,0,nr_points_acc,3);
		memcpy(point_var_reg_array, &point_var_reg_array[offset], sizeof(float)*nr_points_acc);
		range_reg.block(0,0,nr_points_acc,1) = range_reg.block(offset,0,nr_points_acc,1);
		for(int j=0;j<MAX_NR_FUSION_FRAMES;j++)
			frame_ptr[j]-=offset;

		/* -------------------- Transform point cloud -------------------- */
		// Using gt pose for demonstration purposes
		// Get absolute pose
		double t_new[3] = {1000*dataset.gt_poses[i][0], 1000*dataset.gt_poses[i][1], 1000*dataset.gt_poses[i][2]};
		double q_new[4] = {dataset.gt_poses[i][3], dataset.gt_poses[i][4], dataset.gt_poses[i][5], dataset.gt_poses[i][6]};
		Eigen::Matrix3d R_new;
		utils::convertQuaternion2SO3(q_new,R_new);
		Eigen::Matrix4d T_new = Eigen::Matrix4d::Identity();
		T_new.block(0,0,3,3) = R_new;
		T_new(0,3) = t_new[0]; T_new(1,3) = t_new[1]; T_new(2,3) = t_new[2];

		// Compute local transformation
		Eigen::Matrix4d	dT = (T_old.inverse()*T_new).inverse();
		T_old = T_new;
		Eigen::MatrixXf dR = dT.transpose().cast<float>().block(0,0,3,3);
		float tx = dT(0,3); float ty = dT(1,3); float tz = dT(2,3);
		if(nr_points_acc>0){
			// Rotate
			cloud_reg.block(0,0,nr_points_acc,3) = cloud_reg.block(0,0,nr_points_acc,3)*dR;
			// Translate
			cloud_reg.col(0) = cloud_reg.col(0).array()+tx;
			cloud_reg.col(1) = cloud_reg.col(1).array()+ty;
			cloud_reg.col(2) = cloud_reg.col(2).array()+tz;
		}

		/* --------------------------- FUSION --------------------------------- */
		// TODO: Remove pixels out-of-bounds, Check pixel consistency
		// Initialize images with current frame
		norm_weights = 0.0f;
		range_OMG_map = 0.0f;
		range_OMG_var_map = 0.0f;
		range_OMG_var_map = 0.0f;
		fusion_counter = 1.0f;
		norm_weights += 1/range_cov_map;
		range_OMG_map += range_map.mul(norm_weights);
		range_OMG_var_map += norm_weights.mul((range_cov_map + range_map.mul(range_map)));

		range_reg_sqr = range_reg*range_reg;
		// Projection
		U_reg = (fx*cloud_reg.col(0).array()/cloud_reg.col(2).array() + cx).template cast<int>();
		V_reg = (fy*cloud_reg.col(1).array()/cloud_reg.col(2).array() + cy).template cast<int>();

		for(int pt=nr_points_acc-1; pt>-1; pt--){
			int u_p = U_reg(pt); int v_p = V_reg(pt); float r_p = range_reg(pt);
			float r_var = point_var_reg_array[pt];
			if(r_p>0 && u_p>-1 && v_p>-1 && u_p < 640 && v_p < 480){
				float r_var_inv = 1/r_var;
				float r_curr = range_OMG_map.at<float>(v_p,u_p)/norm_weights.at<float>(v_p,u_p);
				float r_var_curr = range_OMG_var_map.at<float>(v_p,u_p)/norm_weights.at<float>(v_p,u_p) - r_curr*r_curr;
				if (fusion_counter.at<int>(v_p,u_p)<3 || pow(r_p-r_curr,2)<consistency_threshold*r_var_curr){
					norm_weights.at<float>(v_p,u_p) += r_var_inv;
					range_OMG_map.at<float>(v_p,u_p) += r_p*r_var_inv;
					range_OMG_var_map.at<float>(v_p,u_p) += r_var_inv*(r_var + range_reg_sqr(pt));
					fusion_counter.at<int>(v_p,u_p) += 1;
				}
			}
		}

		cv::divide(range_OMG_map,norm_weights,range_OMG_map,1);
		cv::divide(range_OMG_var_map,norm_weights,range_OMG_var_map,1);
		range_OMG_var_map -= range_OMG_map.mul(range_OMG_map);

		// Convert range to depth
		depth_OMG_map = range_OMG_map.mul(cos_alpha);
		depth_OMG_var_map =  range_OMG_var_map.mul(cos_alpha_sqr);

		/* ----------------------- Update point cloud with new measurements ----------------------- */

		cloud_reg.block(nr_points_acc,0,nr_valid_pts,3) = cloud_frame.block(0,0,nr_valid_pts,3);
		memcpy(&point_var_reg_array[nr_points_acc], point_var_array, sizeof(float)*nr_valid_pts);
		range_reg.block(nr_points_acc,0,nr_valid_pts,1) = range_frame.block(0,0,nr_valid_pts,1);

		// Update pointers
		for(int j=0; j<MAX_NR_FUSION_FRAMES-1;j++)
			frame_ptr[j] = frame_ptr[j+1];
		frame_ptr[MAX_NR_FUSION_FRAMES-1] = nr_points_acc;
		nr_points_acc += nr_valid_pts;
		double t2 = cv::getTickCount();
		double time_elapsed = (t2-t1)/(double)cv::getTickFrequency();
		cout<<"Time elapsed: "<<time_elapsed<<" s"<<endl;

		/* ------------------------------------- Show Images -------------------------------------- */
		utils::convertGray2jet(d_img, depth_map_jet,500,4000);
		cv::imshow("Raw Depth",depth_map_jet);
		cv::waitKey(1);

		utils::convertGray2jet(depth_OMG_map, depth_map_jet,500,4000);
		cv::imshow("Fused Depth",depth_map_jet);
		cv::waitKey(1);

		double min_d, max_d;
		cv::minMaxIdx(depth_OMG_var_map, &min_d,&max_d);max_d = 50000;
		cv::Mat adjMap;
		cv::convertScaleAbs(depth_OMG_var_map, adjMap, 255/max_d);
		cv::imshow("Fused Depth Uncertainty",adjMap);
		cv::waitKey(1);

	}
	return 0;
}


#include <boost/math/constants/calculate_constants.hpp>


/*
 * This file is part of bogus, a C++ sparse block matrix library.
 *
 * Copyright 2013 Gilles Daviet <gdaviet@gmail.com>
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/

#ifndef BOGUS_EIGEN_SPARSE_LINEAR_SOLVERS_HPP
#define BOGUS_EIGEN_SPARSE_LINEAR_SOLVERS_HPP

/*  Depending on your version of Eigen, this file may make use of LGPL licensed code.
	See Eigen/src/Sparse/SimplicialCholesky.h for more information
*/

#include "SparseHeader.hpp"

#define BOGUS_WITH_EIGEN_STABLE_SPARSE_API
#ifdef BOGUS_WITH_EIGEN_STABLE_SPARSE_API

#include "EigenLinearSolvers.hpp"
#include "../Utils/LinearSolverBase.hpp"
#include "../Utils/NaiveSharedPtr.hpp"

#include <Eigen/OrderingMethods>

#ifndef EIGEN_MPL2_ONLY
#include <Eigen/SparseCholesky>
#define BOGUS_WITH_EIGEN_SPARSE_LDLT
#endif

#if EIGEN_VERSION_AT_LEAST(3,1,92)
#include <Eigen/SparseLU>
#define BOGUS_WITH_EIGEN_SPARSE_LU
#endif

#ifndef BOGUS_EIGEN_SPARSE_LDLT_IMPL
#define BOGUS_EIGEN_SPARSE_LDLT_IMPL(Derived) Eigen::SimplicialLDLT<Derived>
#endif

#ifndef BOGUS_EIGEN_SPARSE_LU_IMPL
#define BOGUS_EIGEN_SPARSE_LU_IMPL(Derived) Eigen::SparseLU<Derived, Eigen::COLAMDOrdering<int> >
#endif

namespace bogus {

template < typename Derived, typename ImplType >
struct LinearSolverTraits< Factorization< Eigen::SparseMatrixBase< Derived >, ImplType > >
{
	typedef typename Derived::PlainObject MatrixType ;
	typedef ImplType FactType ;

	template < typename RhsT > struct Result {
		typedef typename EigenSolveResult< FactType, RhsT >::Type Type ;
	} ;
	template < typename RhsT >
	struct Result< Eigen::MatrixBase< RhsT > > {
		typedef typename Result< RhsT >::Type Type ;
	} ;
} ;

template < typename Derived, typename FactType >
struct Factorization< Eigen::SparseMatrixBase< Derived >, FactType >
		: public LinearSolverBase< Factorization< Eigen::SparseMatrixBase< Derived >, FactType > >
{
	typedef Eigen::SparseMatrixBase< Derived > MatrixType ;
	typedef LinearSolverTraits< Factorization< MatrixType, FactType > > Traits ;

	Factorization() {}
	template< typename OtherDerived >
	explicit Factorization ( const Eigen::SparseMatrixBase< OtherDerived >& mat )
		: m_fact( new typename Traits::FactType( mat ) )
	{}

	template< typename OtherDerived >
	Factorization& compute ( const Eigen::SparseMatrixBase< OtherDerived >& mat )
	{
		m_fact.reset( new typename Traits::FactType( mat ) ) ;
		return *this ;
	}

	template < typename RhsT, typename ResT >
	void solve( const Eigen::MatrixBase< RhsT >& rhs, ResT& res ) const
	{
		assert( m_fact ) ;
		res = m_fact->solve( rhs ) ;
	}

	template < typename RhsT >
	typename Traits::template Result< Eigen::MatrixBase< RhsT > >::Type
	solve( const Eigen::MatrixBase< RhsT >& rhs ) const
	{
		assert( m_fact ) ;
		return m_fact->solve( rhs ) ;
	}

	const typename Traits::FactType& factorization() const {
		return *m_fact ;
	}

  protected:
	BOGUS_SHARED_PTR( typename Traits::FactType, m_fact ) ;
} ;

template < typename Derived, typename FactType >
struct EigenSparseFactorization : public Factorization<
		Eigen::SparseMatrixBase< Derived >,
		FactType >
{
	EigenSparseFactorization() {}
	explicit EigenSparseFactorization ( const Eigen::SparseMatrixBase< Derived >& mat )
		: Factorization< Eigen::SparseMatrixBase< Derived >, FactType >( mat )
	{}
} ;

// LDLT

#ifdef BOGUS_WITH_EIGEN_SPARSE_LDLT
#if ! EIGEN_VERSION_AT_LEAST(3,2,90)
template < typename MatrixType, typename RhsType >
struct EigenSolveResult< Eigen::SimplicialLDLT< MatrixType >, RhsType >
{
	typedef Eigen::SimplicialLDLT< MatrixType > FactType ;
	typedef Eigen::internal::solve_retval< Eigen::SimplicialCholeskyBase< FactType >, RhsType > Type ;
};
#endif
#endif

template < typename Derived >
struct LinearSolverTraits< LDLT< Eigen::SparseMatrixBase< Derived > > >
		: public LinearSolverTraits< Factorization<Eigen::SparseMatrixBase<Derived>, BOGUS_EIGEN_SPARSE_LDLT_IMPL(Derived) > >
{
} ;
template < typename Derived >
struct LinearSolverTraits< LU< Eigen::SparseMatrixBase< Derived > > >
		: public LinearSolverTraits< Factorization<Eigen::SparseMatrixBase<Derived>, BOGUS_EIGEN_SPARSE_LU_IMPL(Derived) > >
{
} ;

template < typename Derived >
struct LDLT< Eigen::SparseMatrixBase< Derived > >
		: public Factorization< Eigen::SparseMatrixBase< Derived >,
		typename LinearSolverTraits< LDLT< Eigen::SparseMatrixBase< Derived > > >::FactType >
{
	typedef Eigen::SparseMatrixBase< Derived > MatrixType ;
	typedef LinearSolverTraits< LDLT< MatrixType > > Traits ;

	typedef Factorization< MatrixType, typename Traits::FactType > Base ;

	LDLT() : Base() {}
	template< typename OtherDerived >
	explicit LDLT ( const Eigen::SparseMatrixBase< OtherDerived >& mat )
		: Base( mat )
	{}

} ;

template < typename Scalar, int _Options = 0, typename _Index = int >
struct SparseLDLT : public LDLT< Eigen::SparseMatrixBase< Eigen::SparseMatrix< Scalar, _Options, _Index > > >
{
	SparseLDLT() {}
	template< typename OtherDerived >
	explicit SparseLDLT ( const Eigen::SparseMatrixBase< OtherDerived >& mat )
		: LDLT< Eigen::SparseMatrixBase< Eigen::SparseMatrix< Scalar, _Options, _Index > > >( mat )
	{}
} ;

template < typename Derived >
struct LU< Eigen::SparseMatrixBase< Derived > >
		: public Factorization< Eigen::SparseMatrixBase< Derived >,
		typename LinearSolverTraits< LU< Eigen::SparseMatrixBase< Derived > > >::FactType >
{
	typedef Eigen::SparseMatrixBase< Derived > MatrixType ;
	typedef LinearSolverTraits< LU< MatrixType > > Traits ;

	typedef Factorization< MatrixType, typename Traits::FactType > Base ;

	LU() : Base() {}
	template< typename OtherDerived >
	explicit LU ( const Eigen::SparseMatrixBase< OtherDerived >& mat )
		: Base( mat )
	{}

} ;

template < typename Scalar, int _Options = 0, typename _Index = int >
struct SparseLU : public LU< Eigen::SparseMatrixBase< Eigen::SparseMatrix< Scalar, _Options, _Index > > >
{
	SparseLU() {}
	template< typename OtherDerived >
	explicit SparseLU ( const Eigen::SparseMatrixBase< OtherDerived >& mat )
		: LU< Eigen::SparseMatrixBase< Eigen::SparseMatrix< Scalar, _Options, _Index > > >( mat )
	{}
} ;

} //namespace bogus

#endif // EIGEN_STABLE_API

#endif //HPP


/*
 * EigenProxy.hpp
 *
 *  Created on: 12 Apr 2018
 *      Author: Viktor Csomor
 */

#ifndef C_ATTL3_CORE_EIGENPROXY_H_
#define C_ATTL3_CORE_EIGENPROXY_H_

#define EIGEN_USE_THREADS

#include <algorithm>
#include <cassert>
#include <cstddef>
#include <Eigen/Dense>
#include <iostream>
#include <string>
#include <thread>
#include <unsupported/Eigen/CXX11/Tensor>

/**
 * The namespace containing all classes and typedefs of the C-ATTL3 library.
 */
namespace cattle {

/**
 * An alias for a single row matrix of an arbitrary scalar type.
 */
template<typename Scalar>
using RowVector = Eigen::Matrix<Scalar,1,Eigen::Dynamic,Eigen::RowMajor, 1,Eigen::Dynamic>;

/**
 * An alias for a single column matrix of an arbitrary scalar type.
 */
template <typename Scalar>
using ColVector = Eigen::Matrix<Scalar,Eigen::Dynamic,1,Eigen::ColMajor,Eigen::Dynamic,1>;

/**
 * An alias for a dynamically sized matrix of an arbitrary scalar type.
 */
template<typename Scalar>
using Matrix = Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor,
		Eigen::Dynamic,Eigen::Dynamic>;

/**
 * An alias for a class that can be used to map raw pointer data to a dynamically
 * sized Matrix of an arbitrary scalar type.
 */
template<typename Scalar>
using MatrixMap = Eigen::Map<Matrix<Scalar>>;

/**
 * An alias for a tensor of arbitrary rank and scalar type with dynamic dimensionality.
 */
template<typename Scalar, std::size_t Rank>
using Tensor = Eigen::Tensor<Scalar,Rank,Eigen::ColMajor,std::size_t>;

/**
 * An for a class that can be used to map raw pointer data to a tensor of arbitrary
 * rank and scalar type with dynamic dimensionality.
 */
template<typename Scalar, std::size_t Rank>
using TensorMap = Eigen::TensorMap<Tensor<Scalar,Rank>>;

/**
 * An alias for permutation matrices.
 */
using PermMatrix = Eigen::PermutationMatrix<Eigen::Dynamic,Eigen::Dynamic>;

/**
 * An alias for self-adjoint eigen solvers.
 */
template<typename Scalar>
using EigenSolver = Eigen::SelfAdjointEigenSolver<Matrix<Scalar>>;

/**
 * An alias for Eigen's bi-diagonal divide and conquer singular-value decomposition.
 */
template<typename Scalar>
using SVD = Eigen::BDCSVD<Matrix<Scalar>>;

/**
 * An alias for Eigen's singular-value decomposition options.
 */
using SVDOptions = Eigen::DecompositionOptions;

/**
 * @return The number of threads used by Eigen to accelerate operations
 * supporting multithreading.
 */
inline int num_of_eval_threads() {
	return Eigen::nbThreads();
}

/**
 * @param num_of_threads The number of threads Eigen should use to accelerate
 * operations supporting multithreading. The lower bound of the actual value
 * applied is 1 while the upper bound is the maximum of 1 and the level of
 * hardware concurrency detected.
 */
inline void set_num_of_eval_threads(int num_of_threads) {
	int max = std::max(1, (int) std::thread::hardware_concurrency());
	Eigen::setNbThreads(std::max(1, std::min(num_of_threads, max)));
}

/**
 * It serializes the matrix in a format such that the first two numbers denote the
 * matrix's number of rows and columns respectively and the remaining numbers represent
 * the coefficients of the matrix in column-major order.
 *
 * @param matrix The matrix to serialize.
 * @param out_stream The non-binary stream to serialize the matrix to.
 */
template<typename Scalar>
inline void serialize(const Matrix<Scalar>& matrix, std::ostream& out_stream) {
	out_stream << sizeof(Scalar);
	out_stream << " " << matrix.rows();
	out_stream << " " << matrix.cols();
	for (std::size_t i = 0; i < matrix.size(); ++i)
		out_stream << " " << *(matrix.data() + i);
	out_stream << std::flush;
}

/**
 * It serializes the matrix into a file at the specified file path.
 *
 * @param matrix The matrix to serialize.
 * @param file_path The path to the file to which the matrix is to be serialized.
 */
template<typename Scalar>
inline void serialize(const Matrix<Scalar>& matrix, const std::string& file_path) {
	std::ofstream out_stream(file_path);
	assert(out_stream.is_open());
	serialize<Scalar>(matrix, out_stream);
}

/**
 * It serializes the matrix in a format such that the first 2 bytes denote the size of
 * a single coefficient of the matrix in bytes, the second and third 4 bytes denote the
 * matrix's number of rows and columns respectively, and the remaining bytes contain
 * the coefficients of the matrix in column-major order.
 *
 * @param matrix The matrix to serialize.
 * @param out_stream The binary stream to serialize the matrix to.
 */
template<typename Scalar>
inline void serialize_binary(const Matrix<Scalar>& matrix, std::ostream& out_stream) {
	unsigned short scalar_size = static_cast<unsigned short>(sizeof(Scalar));
	out_stream.write(reinterpret_cast<const char*>(&scalar_size),
			std::streamsize(sizeof(unsigned short)));
	unsigned rows = static_cast<unsigned>(matrix.rows());
	unsigned cols = static_cast<unsigned>(matrix.cols());
	out_stream.write(reinterpret_cast<const char*>(&rows), std::streamsize(sizeof(unsigned)));
	out_stream.write(reinterpret_cast<const char*>(&cols), std::streamsize(sizeof(unsigned)));
	out_stream.write(reinterpret_cast<const char*>(matrix.data()),
			std::streamsize(matrix.size() * sizeof(Scalar)));
	out_stream << std::flush;
}

/**
 * It serializes the matrix into a binary file at the specified file path.
 *
 * @param matrix The matrix to serialize.
 * @param file_path The path to the binary file to which the matrix is to be serialized.
 */
template<typename Scalar>
inline void serialize_binary(const Matrix<Scalar>& matrix, const std::string& file_path) {
	std::ofstream out_stream(file_path, std::ios::binary);
	assert(out_stream.is_open());
	serialize_binary<Scalar>(matrix, out_stream);
}

/**
 * It deserializes a matrix assuming the serialized format matches that used by the
 * serialize() method.
 *
 * @param in_stream The stream to the serialized matrix.
 * @return The unserialized matrix.
 */
template<typename Scalar>
inline Matrix<Scalar> deserialize(std::istream& in_stream) {
	unsigned rows, cols;
	in_stream >> rows;
	in_stream >> cols;
	Matrix<Scalar> matrix(rows, cols);
	for (std::size_t i = 0; i < matrix.size(); ++i)
		in_stream >> *(matrix.data() + i);
	return matrix;
}

/**
 * It deserializes a matrix from the file at the provided file path.
 *
 * @param file_path The path to the file containing the serialized matrix.
 * @return The deserialized matrix.
 */
template<typename Scalar>
inline Matrix<Scalar> deserialize(const std::string& file_path) {
	std::ifstream in_stream(file_path);
	assert(in_stream.is_open());
	return deserialize<Scalar>(in_stream);
}

/**
 * It deserializes a matrix assuming the serialized format matches that used by the
 * serialize_binary() method.
 *
 * @param in_stream The binary stream to the serialized matrix.
 * @return The unserialized matrix.
 */
template<typename Scalar>
inline Matrix<Scalar> deserialize_binary(std::istream& in_stream) {
	unsigned short scalar_size;
	in_stream.read(reinterpret_cast<char*>(&scalar_size), std::streamsize(sizeof(unsigned short)));
	assert(scalar_size == sizeof(Scalar));
	unsigned rows, cols;
	in_stream.read(reinterpret_cast<char*>(&rows), std::streamsize(sizeof(unsigned)));
	in_stream.read(reinterpret_cast<char*>(&cols), std::streamsize(sizeof(unsigned)));
	Matrix<Scalar> matrix(rows, cols);
	in_stream.read(reinterpret_cast<char*>(matrix.data()),
			std::streamsize(matrix.size() * sizeof(Scalar)));
	return matrix;
}

/**
 * It deserializes a matrix from the binary file at the provided file path.
 *
 * @param file_path The path to the binary file containing the serialized matrix.
 * @return The deserialized matrix.
 */
template<typename Scalar>
inline Matrix<Scalar> deserialize_binary(const std::string& file_path) {
	std::ifstream in_stream(file_path, std::ios::binary);
	assert(in_stream.is_open());
	return deserialize_binary<Scalar>(in_stream);
}

}

#endif /* C_ATTL3_CORE_EIGENPROXY_H_ */


/*
 * GridMapMath.hpp
 *
 *  Created on: Dec 2, 2013
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, Autonomous Systems Lab
 */

#pragma once

#include "grid_map_core/TypeDefs.hpp"
#include "grid_map_core/BufferRegion.hpp"

#include <Eigen/Core>
#include <vector>
#include <map>

namespace grid_map {

/*!
 * Gets the position of a cell specified by its index in the map frame.
 * @param[out] position the position of the center of the cell in the map frame.
 * @param[in] index of the cell.
 * @param[in] mapLength the lengths in x and y direction.
 * @param[in] mapPosition the position of the map.
 * @param[in] resolution the resolution of the map.
 * @param[in] bufferSize the size of the buffer (optional).
 * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional).
 * @return true if successful, false if index not within range of buffer.
 */
bool getPositionFromIndex(Eigen::Vector2d& position,
                          const Eigen::Array2i& index,
                          const Eigen::Array2d& mapLength,
                          const Eigen::Vector2d& mapPosition,
                          const double& resolution,
                          const Eigen::Array2i& bufferSize,
                          const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Gets the index of the cell which contains a position in the map frame.
 * @param[out] index of the cell.
 * @param[in] position the position in the map frame.
 * @param[in] mapLength the lengths in x and y direction.
 * @param[in] mapPosition the position of the map.
 * @param[in] resolution the resolution of the map.
 * @param[in] bufferSize the size of the buffer (optional).
 * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional).
 * @return true if successful, false if position outside of map.
 */
bool getIndexFromPosition(Eigen::Array2i& index,
                          const Eigen::Vector2d& position,
                          const Eigen::Array2d& mapLength,
                          const Eigen::Vector2d& mapPosition,
                          const double& resolution,
                          const Eigen::Array2i& bufferSize,
                          const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Checks if position is within the map boundaries.
 * @param[in] position the position which is to be checked.
 * @param[in] mapLength the length of the map.
 * @param[in] mapPosition the position of the map.
 * @return true if position is within map, false otherwise.
 */
bool checkIfPositionWithinMap(const Eigen::Vector2d& position,
                              const Eigen::Array2d& mapLength,
                              const Eigen::Vector2d& mapPosition);

/*!
 * Gets the position of the data structure origin.
 * @param[in] position the position of the map.
 * @param[in] mapLength the map length.
 * @param[out] positionOfOrigin the position of the data structure origin.
 */
void getPositionOfDataStructureOrigin(const Eigen::Vector2d& position,
                                      const Eigen::Array2d& mapLength,
                                      Eigen::Vector2d& positionOfOrigin);

/*!
 * Computes how many cells/indeces the map is moved based on a position shift in
 * the grid map frame. Use this function if you are moving the grid map
 * and want to ensure that the cells match before and after.
 * @param[out] indexShift the corresponding shift of the indices.
 * @param[in] positionShift the desired position shift.
 * @param[in] resolution the resolution of the map.
 * @return true if successful.
 */
bool getIndexShiftFromPositionShift(Eigen::Array2i& indexShift,
                                    const Eigen::Vector2d& positionShift,
                                    const double& resolution);

/*!
 * Computes the corresponding position shift from a index shift. Use this function
 * if you are moving the grid map and want to ensure that the cells match
 * before and after.
 * @param[out] positionShift the corresponding shift in position in the grid map frame.
 * @param[in] indexShift the desired shift of the indeces.
 * @param[in] resolution the resolution of the map.
 * @return true if successful.
 */
bool getPositionShiftFromIndexShift(Eigen::Vector2d& positionShift,
                                    const Eigen::Array2i& indexShift,
                                    const double& resolution);

/*!
 * Checks if index is within range of the buffer.
 * @param[in] index to check.
 * @param[in] bufferSize the size of the buffer.
 * @return true if index is within, and false if index is outside of the buffer.
 */
bool checkIfIndexWithinRange(const Eigen::Array2i& index, const Eigen::Array2i& bufferSize);

/*!
 * Maps an index that runs out of the range of the circular buffer back into allowed the region.
 * This is the 2d version of mapIndexWithinRange(int&, const int&).
 * @param[in/out] index the indeces that will be mapped into the valid region of the buffer.
 * @param[in] bufferSize the size of the buffer.
 */
void mapIndexWithinRange(Eigen::Array2i& index,
                         const Eigen::Array2i& bufferSize);

/*!
 * Maps an index that runs out of the range of the circular buffer back into allowed the region.
 * @param[in/out] index the index that will be mapped into the valid region of the buffer.
 * @param[in] bufferSize the size of the buffer.
 */
void mapIndexWithinRange(int& index, const int& bufferSize);

/*!
 * Limits (cuts off) the position to lie inside the map.
 * @param[in/out] position the position to be limited.
 * @param[in] mapLength the lengths in x and y direction.
 * @param[in] mapPosition the position of the map.
 */
void limitPositionToRange(Eigen::Vector2d& position,
                          const Eigen::Array2d& mapLength,
                          const Eigen::Vector2d& mapPosition);

/*!
 * Provides the alignment transformation from the buffer order (outer/inner storage)
 * and the map frame (x/y-coordinate).
 * @return the alignment transformation.
 */
const Eigen::Matrix2i getBufferOrderToMapFrameAlignment();

/*!
 * Given a map and a desired submap (defined by position and size), this function computes
 * various information about the submap. The returned submap might be smaller than the requested
 * size as it respects the boundaries of the map.
 * @param[out] submapTopLeftIndex the top left index of the returned submap.
 * @param[out] submapBufferSize the buffer size of the returned submap.
 * @param[out] submapPosition the position of the submap (center) in the map frame.
 * @param[out] submapLength the length of the submap.
 * @param[out] requestedIndexInSubmap the index in the submap that corresponds to the requested
 *             position of the submap.
 * @param[in] requestedSubmapPosition the requested submap position (center) in the map frame.
 * @param[in] requestedSubmapLength the requested submap length.
 * @param[in] mapLength the lengths in x and y direction.
 * @param[in] mapPosition the position of the map.
 * @param[in] resolution the resolution of the map.
 * @param[in] bufferSize the buffer size of the map.
 * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional).
 * @return true if successful.
 */
bool getSubmapInformation(Eigen::Array2i& submapTopLeftIndex,
                          Eigen::Array2i& submapBufferSize,
                          Eigen::Vector2d& submapPosition,
                          Eigen::Array2d& submapLength,
                          Eigen::Array2i& requestedIndexInSubmap,
                          const Eigen::Vector2d& requestedSubmapPosition,
                          const Eigen::Vector2d& requestedSubmapLength,
                          const Eigen::Array2d& mapLength,
                          const Eigen::Vector2d& mapPosition,
                          const double& resolution,
                          const Eigen::Array2i& bufferSize,
                          const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Computes the regions in the circular buffer that make up the data for
 * a requested submap.
 * @param[out] submapBufferRegions the list of buffer regions that make up the submap.
 * @param[in] submapIndex the index (top-left) for the requested submap.
 * @param[in] submapBufferSize the size of the requested submap.
 * @param[in] bufferSize the buffer size of the map.
 * @param[in] bufferStartIndex the index of the starting point of the circular buffer (optional).
 * @return true if successful, false if requested submap is not fully contained in the map.
 */
bool getBufferRegionsForSubmap(std::vector<BufferRegion>& submapBufferRegions,
                               const Index& submapIndex,
                               const Size& submapBufferSize,
                               const Size& bufferSize,
                               const Index& bufferStartIndex = Index::Zero());

/*!
 * Increases the index by one to iterate through the map.
 * Increments either to the neighboring index to the right or to
 * the start of the lower row. Returns false if end of iterations are reached.
 * @param[in/out] index the index in the map that is incremented (corrected for the circular buffer).
 * @param[in] bufferSize the map buffer size.
 * @param[in] bufferStartIndex the map buffer start index.
 * @return true if successfully incremented indeces, false if end of iteration limits are reached.
 */
bool incrementIndex(Eigen::Array2i& index, const Eigen::Array2i& bufferSize,
                    const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Increases the index by one to iterate through the cells of a submap.
 * Increments either to the neighboring index to the right or to
 * the start of the lower row. Returns false if end of iterations are reached.
 *
 * Note: This function does not check if submap actually fits to the map. This needs
 * to be checked before separately.
 *
 * @param[in/out] submapIndex the index in the submap that is incremented.
 * @param[out] index the index in the map that is incremented (corrected for the circular buffer).
 * @param[in] submapTopLefIndex the top left index of the submap.
 * @param[in] submapBufferSize the submap buffer size.
 * @param[in] bufferSize the map buffer size.
 * @param[in] bufferStartIndex the map buffer start index.
 * @return true if successfully incremented indeces, false if end of iteration limits are reached.
 */
bool incrementIndexForSubmap(Eigen::Array2i& submapIndex, Eigen::Array2i& index,
                             const Eigen::Array2i& submapTopLeftIndex,
                             const Eigen::Array2i& submapBufferSize,
                             const Eigen::Array2i& bufferSize,
                             const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());

/*!
 * Retrieve the index as unwrapped index, i.e., as the corresponding index of a
 * grid map with no circular buffer offset.
 * @param bufferIndex the index in the circular buffer.
 * @param bufferSize the map buffer size.
 * @param bufferStartIndex the map buffer start index.
 * @return the unwrapped index.
 */
Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize,
                              const Index& bufferStartIndex);

/*!
 * Returns the 1d index corresponding to the 2d index for either row- or column-major format.
 * Note: Eigen is defaulting to column-major format.
 * @param[in] index the 2d index.
 * @param[in] bufferSize the map buffer size.
 * @param[in] (optional) rowMajor if the 1d index is generated for row-major format.
 * @return the 1d index.
 */
unsigned int get1dIndexFrom2dIndex(const Index& index, const Size& bufferSize,
                                   const bool rowMajor);

/*!
 * Generates a list of indices for a region in the map.
 * @param regionIndex the region top-left index.
 * @param regionSize the region size.
 * @param indices the list of indices of the region.
 */
void getIndicesForRegion(const Index& regionIndex, const Size& regionSize,
                         std::vector<Index> indices);

/*!
 * Generates a list of indices for multiple regions in the map.
 * This method makes sure every index is only once contained in the list.
 * @param regionIndeces the regions' top-left index.
 * @param regionSizes the regions' sizes.
 * @param indices the list of indices of the regions.
 */
void getIndicesForRegions(const std::vector<Index>& regionIndeces, const Size& regionSizes,
                          std::vector<Index> indices);

} // namespace


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    Rot3.cpp
 * @brief   Rotation, common code between Rotation matrix and Quaternion
 * @author  Alireza Fathi
 * @author  Christian Potthast
 * @author  Frank Dellaert
 * @author  Richard Roberts
 * @author  Varun Agrawal
 */

#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <boost/math/constants/constants.hpp>

#include <cmath>
#include <random>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
void Rot3::print(const std::string& s) const {
  cout << (s.empty() ? "R: " : s + " ");
  gtsam::print(static_cast<Matrix>(matrix()));
}

/* ************************************************************************* */
Rot3 Rot3::Random(std::mt19937& rng) {
  Unit3 axis = Unit3::Random(rng);
  uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
  double angle = randomAngle(rng);
  return AxisAngle(axis, angle);
}

/* ************************************************************************* */
Rot3 Rot3::AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p) {
  // if a_p is already aligned with b_p, return the identity rotation
  if (std::abs(a_p.dot(b_p)) > 0.999999999) {
    return Rot3();
  }

  // Check axis was not degenerate cross product
  const Vector3 z = axis.unitVector();
  if (z.hasNaN())
    throw std::runtime_error("AlignSinglePair: axis has Nans");

  // Now, calculate rotation that takes b_p to a_p
  const Matrix3 P = I_3x3 - z * z.transpose();  // orthogonal projector
  const Vector3 a_po = P * a_p.unitVector();    // point in a orthogonal to axis
  const Vector3 b_po = P * b_p.unitVector();    // point in b orthogonal to axis
  const Vector3 x = a_po.normalized();          // x-axis in axis-orthogonal plane, along a_p vector
  const Vector3 y = z.cross(x);                 // y-axis in axis-orthogonal plane
  const double u = x.dot(b_po);                 // x-coordinate for b_po
  const double v = y.dot(b_po);                 // y-coordinate for b_po
  double angle = std::atan2(v, u);
  return Rot3::AxisAngle(z, -angle);
}

/* ************************************************************************* */
Rot3 Rot3::AlignTwoPairs(const Unit3& a_p, const Unit3& b_p,  //
                         const Unit3& a_q, const Unit3& b_q) {
  // there are three frames in play:
  // a: the first frame in which p and q are measured
  // b: the second frame in which p and q are measured
  // i: intermediate, after aligning first pair

  // First, find rotation around that aligns a_p and b_p
  Rot3 i_R_b = AlignPair(a_p.cross(b_p), a_p, b_p);

  // Rotate points in frame b to the intermediate frame,
  // in which we expect the point p to be aligned now
  Unit3 i_q = i_R_b * b_q;
  assert(assert_equal(a_p, i_R_b * b_p, 1e-6));

  // Now align second pair: we need to align i_q to a_q
  Rot3 a_R_i = AlignPair(a_p, a_q, i_q);
  assert(assert_equal(a_p, a_R_i * a_p, 1e-6));
  assert(assert_equal(a_q, a_R_i * i_q, 1e-6));

  // The desired rotation is the product of both
  Rot3 a_R_b = a_R_i * i_R_b;
  return a_R_b;
}

/* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const {
  return equal_with_abs_tol(matrix(), R.matrix(), tol);
}

/* ************************************************************************* */
Point3 Rot3::operator*(const Point3& p) const {
  return rotate(p);
}

/* ************************************************************************* */
Unit3 Rot3::rotate(const Unit3& p,
    OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
  Matrix32 Dp;
  Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0)));
  if (Hp) *Hp = q.basis().transpose() * matrix() * Dp;
  if (HR) *HR = -q.basis().transpose() * matrix() * p.skew();
  return q;
}

/* ************************************************************************* */
Unit3 Rot3::unrotate(const Unit3& p,
    OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
  Matrix32 Dp;
  Unit3 q = Unit3(unrotate(p.point3(Dp)));
  if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp;
  if (HR) *HR = q.basis().transpose() * q.skew();
  return q;
}

/* ************************************************************************* */
Unit3 Rot3::operator*(const Unit3& p) const {
  return rotate(p);
}

/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
    OptionalJacobian<3,3> H2) const {
  const Matrix3& Rt = transpose();
  Point3 q(Rt * p); // q = Rt*p
  const double wx = q.x(), wy = q.y(), wz = q.z();
  if (H1)
    *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
  if (H2)
    *H2 = Rt;
  return q;
}

/* ************************************************************************* */
Point3 Rot3::column(int index) const{
  if(index == 3)
    return r3();
  else if(index == 2)
    return r2();
  else if(index == 1)
    return r1(); // default returns r1
  else
    throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
}

/* ************************************************************************* */
Vector3 Rot3::xyz(OptionalJacobian<3, 3> H) const {
  Matrix3 I;Vector3 q;
  if (H) {
    Matrix93 mH;
    const auto m = matrix();
#ifdef GTSAM_USE_QUATERNIONS
    SO3{m}.vec(mH);
#else
    rot_.vec(mH);
#endif

    Matrix39 qHm;
    boost::tie(I, q) = RQ(m, qHm);

    // TODO : Explore whether this expression can be optimized as both
    // qHm and mH are super-sparse
    *H = qHm * mH;
  } else
    boost::tie(I, q) = RQ(matrix());
  return q;
}

/* ************************************************************************* */
Vector3 Rot3::ypr(OptionalJacobian<3, 3> H) const {
  Vector3 q = xyz(H);
  if (H) H->row(0).swap(H->row(2));

  return Vector3(q(2),q(1),q(0));
}

/* ************************************************************************* */
Vector3 Rot3::rpy(OptionalJacobian<3, 3> H) const { return xyz(H); }

/* ************************************************************************* */
double Rot3::roll(OptionalJacobian<1, 3> H) const {
  double r;
  if (H) {
    Matrix3 xyzH;
    r = xyz(xyzH)(0);
    *H = xyzH.row(0);
  } else
    r = xyz()(0);
  return r;
}

/* ************************************************************************* */
double Rot3::pitch(OptionalJacobian<1, 3> H) const {
  double p;
  if (H) {
    Matrix3 xyzH;
    p = xyz(xyzH)(1);
    *H = xyzH.row(1);
  } else
    p = xyz()(1);
  return p;
}

/* ************************************************************************* */
double Rot3::yaw(OptionalJacobian<1, 3> H) const {
  double y;
  if (H) {
    Matrix3 xyzH;
    y = xyz(xyzH)(2);
    *H = xyzH.row(2);
  } else
    y = xyz()(2);
  return y;
}

/* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
Vector Rot3::quaternion() const {
  gtsam::Quaternion q = toQuaternion();
  Vector v(4);
  v(0) = q.w();
  v(1) = q.x();
  v(2) = q.y();
  v(3) = q.z();
  return v;
}
#endif

/* ************************************************************************* */
pair<Unit3, double> Rot3::axisAngle() const {
  const Vector3 omega = Rot3::Logmap(*this);
  return std::pair<Unit3, double>(Unit3(omega), omega.norm());
}

/* ************************************************************************* */
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
  return SO3::ExpmapDerivative(x);
}

/* ************************************************************************* */
Matrix3 Rot3::LogmapDerivative(const Vector3& x)    {
  return SO3::LogmapDerivative(x);
}

/* ************************************************************************* */
pair<Matrix3, Vector3> RQ(const Matrix3& A, OptionalJacobian<3, 9> H) {
  const double x = -atan2(-A(2, 1), A(2, 2));
  const auto Qx = Rot3::Rx(-x).matrix();
  const Matrix3 B = A * Qx;

  const double y = -atan2(B(2, 0), B(2, 2));
  const auto Qy = Rot3::Ry(-y).matrix();
  const Matrix3 C = B * Qy;

  const double z = -atan2(-C(1, 0), C(1, 1));
  const auto Qz = Rot3::Rz(-z).matrix();
  const Matrix3 R = C * Qz;

  if (H) {
    if (std::abs(y - M_PI / 2) < 1e-2)
      throw std::runtime_error(
          "Rot3::RQ : Derivative undefined at singularity (gimbal lock)");

    auto atan_d1 = [](double y, double x) { return x / (x * x + y * y); };
    auto atan_d2 = [](double y, double x) { return -y / (x * x + y * y); };

    const auto sx = -Qx(2, 1), cx = Qx(1, 1);
    const auto sy = -Qy(0, 2), cy = Qy(0, 0);

    *H = Matrix39::Zero();
    // First, calculate the derivate of x
    (*H)(0, 5) = atan_d1(A(2, 1), A(2, 2));
    (*H)(0, 8) = atan_d2(A(2, 1), A(2, 2));

    // Next, calculate the derivate of y. We have
    // b20 = a20 and b22 = a21 * sx + a22 * cx
    (*H)(1, 2) = -atan_d1(B(2, 0), B(2, 2));
    const auto yHb22 = -atan_d2(B(2, 0), B(2, 2));
    (*H)(1, 5) = yHb22 * sx;
    (*H)(1, 8) = yHb22 * cx;

    // Next, calculate the derivate of z. We have
    // c10 = a10 * cy + a11 * sx * sy + a12 * cx * sy
    // c11 = a11 * cx - a12 * sx
    const auto c10Hx = (A(1, 1) * cx - A(1, 2) * sx) * sy;
    const auto c10Hy = A(1, 2) * cx * cy + A(1, 1) * cy * sx - A(1, 0) * sy;
    Vector9 c10HA = c10Hx * H->row(0) + c10Hy * H->row(1);
    c10HA[1] = cy;
    c10HA[4] = sx * sy;
    c10HA[7] = cx * sy;

    const auto c11Hx = -A(1, 2) * cx - A(1, 1) * sx;
    Vector9 c11HA = c11Hx * H->row(0);
    c11HA[4] = cx;
    c11HA[7] = -sx;

    H->block<1, 9>(2, 0) =
        atan_d1(C(1, 0), C(1, 1)) * c10HA + atan_d2(C(1, 0), C(1, 1)) * c11HA;
  }

  const auto xyz = Vector3(x, y, z);
  return make_pair(R, xyz);
}

/* ************************************************************************* */
ostream &operator<<(ostream &os, const Rot3& R) {
  os << R.matrix().format(matlabFormat());
  return os;
}

/* ************************************************************************* */
Rot3 Rot3::slerp(double t, const Rot3& other) const {
  return interpolate(*this, other, t);
}

/* ************************************************************************* */

} // namespace gtsam


/**
 *  Moore-Greitzer engine DBA control with the specification-guided engine.
 *
 *  Created by Yinan Li on July 3, 2020.
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <sys/stat.h>

#include <string>
#include <cmath>
#include <boost/numeric/odeint.hpp>

#include "src/DBAparser.h"
#include "src/abstraction.hpp"
#include "src/bsolver.hpp"
#include "src/hdf5io.h"

const double a = 1./3.5;
const double B = 2.0;
const double H = 0.18;
const double W = 0.25;
const double lc = 8.0;
const double cx = 1.0/lc;
const double cy = 1.0/(4*lc*B*B);
const double aH = a+H;
const double H2 = H/(2.0*W*W*W);
const double W2 = 3*W*W;

/* user defined dynamics */
struct mgode2 {

    static const int n = 2;  // system dimension
    static const int nu = 2;  // control dimension

    /* template constructor
     * @param[out] dx
     * @param[in] x
     * @param u
     */
    template<typename S>
    mgode2(S *dx, const S *x, rocs::Rn u) {
	dx[0] = cx * (aH+H2*(x[0]-W)*(W2-(x[0]-W)*(x[0]-W)) - x[1]) + u[0];
	dx[1] = cy * (x[0] - u[1]*sqrt(x[1]));
    }
};

int main(int argc, char *argv[])
{
    /**
     * Input arguments:
     * engine2dAbstII dbafile
     */
    if (argc != 2) {
	std::cout << "Improper number of arguments.\n";
	std::exit(1);
    }

    clock_t tb, te;
    /* set the state space */
    double xlb[]{0.44, 0.6};
    double xub[]{0.54, 0.7};

    /* set the control values */
    // double Lmu = 0.01;
    double Lu = 0.05;
    double ulb[]{-Lu, 0.5};
    double uub[]{Lu, 0.8};
    double mu[]{Lu/5, 0.01};

    /* set the sampling time and disturbance */
    double delta = 0.01;
    /* parameters for computing the flow */
    int kmax = 10;
    double tol = 0.01;
    double alpha = 0.5;
    double beta = 2;
    rocs::params controlparams(kmax, tol, alpha, beta);

    /* define the control system */
    double t= 0.1;
    rocs::CTCntlSys<mgode2> engine("Moore-Greitzer", t, mgode2::n, mgode2::nu,
				  delta, &controlparams);
    engine.init_workspace(xlb, xub);
    engine.init_inputset(mu, ulb, uub);
    engine.allocate_flows();

    /**
     * Abstraction
     */
    rocs::abstraction< rocs::CTCntlSys<mgode2> > abst(&engine);
    const double eta[]{0.00018, 0.00018};
    abst.init_state(eta, xlb, xub);
    std::cout << "The number of abstraction states: " << abst._x._nv << '\n';
    double obs[][2]{{0.497, 0.503}, {0.650, 0.656}};
    auto avoid = [&obs, abst, eta](size_t& id) {
		     std::vector<double> x(abst._x._dim);
    		     abst._x.id_to_val(x, id);
    		     double c1 = eta[0]/2.0; //+1e-10;
    		     double c2 = eta[1]/2.0; //+1e-10;
		     if ((obs[0][0]-c1) <= x[0] && x[0] <= (obs[0][1]+c1) &&
			 (obs[1][0]-c2) <= x[1] && x[1] <= (obs[1][1]+c2))
			 return -1;
		     return 0;
		 };
    abst.assign_labels(avoid);
    abst.assign_label_outofdomain(0);
    std::vector<size_t> obstacles;
    for (size_t i = 0; i < abst._x._nv; ++i) {
    	if (abst._labels[i] < 0)
    	    obstacles.push_back(i);
    }

    std::string transfile = "abstII_0.00018.h5";
    struct stat buffer;
    float tabst;
    if(stat(transfile.c_str(), &buffer) == 0) {
	/* Read from a file */
	std::cout << "Transitions have been computed. Reading transitions...\n";
	rocs::h5FileHandler transRdr(transfile, H5F_ACC_RDONLY);
	tb = clock();
	transRdr.read_transitions(abst._ts);
	te = clock();
	tabst = (float)(te - tb)/CLOCKS_PER_SEC;
	std::cout << "Time of reading abstraction: " << tabst << '\n';
    } else {
	std::cout << "Transitions haven't been computed. Computing transitions...\n";
	/* Robustness margins */
	double e1[] = {0.0, 0.0};
	double e2[] = {0.0, 0.0};
	tb = clock();
	abst.assign_transitions(e1, e2);
	te = clock();
	tabst = (float)(te - tb)/CLOCKS_PER_SEC;
	std::cout << "Time of computing abstraction: " << tabst << '\n';
	/* Write abstraction to file */
	rocs::h5FileHandler transWtr(transfile, H5F_ACC_TRUNC);
	transWtr.write_transitions(abst._ts);
    }
    std::cout << "# of transitions: " << abst._ts._ntrans << '\n';

    /**
     * Read DBA from spec*.txt file
     */
    std::cout << "Reading the specification...\n";
    rocs::UintSmall nAP = 0, nNodes = 0, q0 = 0;
    std::vector<rocs::UintSmall> acc;
    std::vector<std::vector<rocs::UintSmall>> arrayM;
    std::string specfile = std::string(argv[1]);
    if (!rocs::read_spec(specfile, nNodes, nAP, q0, arrayM, acc))
	std::exit(1);

    /*
     * Assign labels to states: has to be consistent with the dba file.
     */
    double e = 0.003;
    double goal[][2]{{0.4519-e, 0.4519+e}, {0.6513-e, 0.6513+e}};

    auto label_target = [&goal, &abst, &eta](size_t i) {
			    std::vector<double> x(abst._x._dim);
			    abst._x.id_to_val(x, i);
			    double c1= eta[0]/2.0; //+1e-10;
			    double c2= eta[1]/2.0; //+1e-10;

			    return (goal[0][0] <= (x[0]-c1) && (x[0]+c1) <= goal[0][1] &&
				    goal[1][0] <= (x[1]-c2) && (x[1]+c2) <= goal[1][1]) ?
				1: abst._labels[i];
			};
    abst.assign_labels(label_target);
    std::cout << "Specification assignment is done.\n";
    std::vector<size_t> targetIDs;
    std::vector<rocs::Rn> targetPts;   //initial invariant set
    rocs::Rn x(abst._x._dim);
    for(size_t i = 0; i < abst._labels.size(); ++i) {
    	if(abst._labels[i] > 0) {
    	    targetIDs.push_back(i);
    	    abst._x.id_to_val(x, i);
    	    targetPts.push_back(x);
    	}
    }

    /**
     * Solve a Buchi game on the product of NTS and DBA.
     */
    std::cout << "Start solving a Buchi game on the product of the abstraction and DBA...\n";
    rocs::BSolver solver; // memories will be allocated for psolver
    solver.construct_dba((int)nAP, (int)nNodes, (int)q0, acc, arrayM);
    tb = clock();
    solver.load_abstraction(abst);
    solver.generate_product(abst);
    solver.solve_buchigame_on_product();
    te = clock();
    float tsyn = (float)(te - tb)/CLOCKS_PER_SEC;
    std::cout << "Time of synthesizing controller: " << tsyn << '\n';

    /**
     * Display and save memoryless controllers.
     */
    std::cout << "Writing the controller...\n";
    // std::string datafile = "controller_abstII.txt";
    // solver.write_controller_to_txt(const_cast<char*>(datafile.c_str()));
    std::string datafile = "controller_abstII_0.00018.h5";
    rocs::h5FileHandler ctlrWtr(datafile, H5F_ACC_TRUNC);
    ctlrWtr.write_problem_setting< rocs::CTCntlSys<mgode2> >(engine);
    ctlrWtr.write_2d_array<double>(targetPts, "G");
    ctlrWtr.write_array<double>(eta, mgode2::n, "eta");
    ctlrWtr.write_2d_array<double>(abst._x._data, "xgrid");
    ctlrWtr.write_discrete_controller(&(solver._sol));
    std::cout << "Controller writing is done.\n";

    std::cout << "Total time of used (abstraction+synthesis): " << tabst+tsyn << '\n';

    engine.release_flows();
    return 0;
}


/*
 * This file is modified from the sources of Gazebo 7.0
 *
 * Copyright (C) 2012-2016 Open Source Robotics Foundation
 *
 * 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.
 *
*/
#ifndef _GAZEBO_MATH_FUNCTIONS_HH_
#define _GAZEBO_MATH_FUNCTIONS_HH_

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/round.hpp>
#include <algorithm>
#include <cmath>
#include <limits>
#include <string>
#include <iostream>
#include <vector>

/// \brief Double maximum value
#define GZ_DBL_MAX std::numeric_limits<double>::max()

/// \brief Double min value
#define GZ_DBL_MIN std::numeric_limits<double>::min()

/// \brief Double positive infinite value
#define GZ_DBL_INF std::numeric_limits<double>::infinity()

/// \brief Float maximum value
#define GZ_FLT_MAX std::numeric_limits<float>::max()

/// \brief Float minimum value
#define GZ_FLT_MIN std::numeric_limits<float>::min()

/// \brief 32bit unsigned integer maximum value
#define GZ_UINT32_MAX std::numeric_limits<uint32_t>::max()

/// \brief 32bit unsigned integer minimum value
#define GZ_UINT32_MIN std::numeric_limits<uint32_t>::min()

/// \brief 32bit integer maximum value
#define GZ_INT32_MAX std::numeric_limits<int32_t>::max()

/// \brief 32bit integer minimum value
#define GZ_INT32_MIN std::numeric_limits<int32_t>::min()

namespace gazebo
{
  namespace math
  {
    /// \addtogroup gazebo_math
    /// \brief A set of classes that encapsulate math related properties and
    ///        functions.
    /// \{

    /// \brief Returns the representation of a quiet not a number (NAN)
    static const double NAN_D = std::numeric_limits<double>::quiet_NaN();

    /// \brief Returns the representation of a quiet not a number (NAN)
    static const int NAN_I = std::numeric_limits<int>::quiet_NaN();

    /// \brief Simple clamping function
    /// \param[in] _v value
    /// \param[in] _min minimum
    /// \param[in] _max maximum
    template<typename T>
    inline T clamp(T _v, T _min, T _max)
    {
      return std::max(std::min(_v, _max), _min);
    }

    /// \brief check if a float is NaN
    /// \param[in] _v the value
    /// \return true if _v is not a number, false otherwise
    inline bool isnan(float _v)
    {
      return (boost::math::isnan)(_v);
    }

    /// \brief check if a double is NaN
    /// \param[in] _v the value
    /// \return true if _v is not a number, false otherwise
    inline bool isnan(double _v)
    {
      return (boost::math::isnan)(_v);
    }

    /// \brief Fix a nan value.
    /// \param[in] _v Value to correct.
    /// \return 0 if _v is NaN, _v otherwise.
    inline float fixnan(float _v)
    {
      return isnan(_v) || std::isinf(_v) ? 0.0f : _v;
    }

    /// \brief Fix a nan value.
    /// \param[in] _v Value to correct.
    /// \return 0 if _v is NaN, _v otherwise.
    inline double fixnan(double _v)
    {
      return isnan(_v) || std::isinf(_v) ? 0.0 : _v;
    }

    /// \brief get mean of vector of values
    /// \param[in] _values the vector of values
    /// \return the mean
    template<typename T>
    inline T mean(const std::vector<T> &_values)
    {
      T sum = 0;
      for (unsigned int i = 0; i < _values.size(); ++i)
        sum += _values[i];
      return sum / _values.size();
    }

    /// \brief get variance of vector of values
    /// \param[in] _values the vector of values
    /// \return the squared deviation
    template<typename T>
    inline T variance(const std::vector<T> &_values)
    {
      T avg = mean<T>(_values);

      T sum = 0;
      for (unsigned int i = 0; i < _values.size(); ++i)
        sum += (_values[i] - avg) * (_values[i] - avg);
      return sum / _values.size();
    }

    /// \brief get the maximum value of vector of values
    /// \param[in] _values the vector of values
    /// \return maximum
    template<typename T>
    inline T max(const std::vector<T> &_values)
    {
      T max = std::numeric_limits<T>::min();
      for (unsigned int i = 0; i < _values.size(); ++i)
        if (_values[i] > max)
          max = _values[i];
      return max;
    }

    /// \brief get the minimum value of vector of values
    /// \param[in] _values the vector of values
    /// \return minimum
    template<typename T>
    inline T min(const std::vector<T> &_values)
    {
      T min = std::numeric_limits<T>::max();
      for (unsigned int i = 0; i < _values.size(); ++i)
        if (_values[i] < min)
          min = _values[i];
      return min;
    }

    /// \brief check if two values are equal, within a tolerance
    /// \param[in] _a the first value
    /// \param[in] _b the second value
    /// \param[in] _epsilon the tolerance
    template<typename T>
    inline bool equal(const T &_a, const T &_b,
                      const T &_epsilon = 1e-6)
    {
      return std::fabs(_a - _b) <= _epsilon;
    }

    /// \brief get value at a specified precision
    /// \param[in] _a the number
    /// \param[in] _precision the precision
    /// \return the value for the specified precision
    template<typename T>
    inline T precision(const T &_a, const unsigned int &_precision)
    {
      if (!std::isinf(_a))
      {
        return boost::math::round(
          _a * pow(10, _precision)) / pow(10, _precision);
      }
      else
      {
        return _a;
      }
    }

    /// \brief is this a power of 2?
    /// \param[in] _x the number
    /// \return true if _x is a power of 2, false otherwise
    inline bool isPowerOfTwo(unsigned int _x)
    {
      return ((_x != 0) && ((_x & (~_x + 1)) == _x));
    }

    /// \brief Get the smallest power of two that is greater or equal to a given
    /// value
    /// \param[in] _x the number
    /// \return the same value if _x is already a power of two. Otherwise,
    /// it returns the smallest power of two that is greater than _x
    inline unsigned int roundUpPowerOfTwo(unsigned int _x)
    {
      if (_x == 0)
        return 1;

      if (isPowerOfTwo(_x))
        return _x;

      while (_x & (_x - 1))
        _x = _x & (_x - 1);

      _x = _x << 1;

      return _x;
    }

    /// \brief parse string into an integer
    /// \param[in] _input the string
    /// \return an integer, 0 or 0 and a message in the error stream
    inline int parseInt(const std::string& _input)
    {
      const char *p = _input.c_str();
      if (!*p || *p == '?')
        return NAN_I;

      int s = 1;
      while (*p == ' ')
        p++;

      if (*p == '-')
      {
        s = -1;
        p++;
      }

      double acc = 0;
      while (*p >= '0' && *p <= '9')
        acc = acc * 10 + *p++ - '0';

      if (*p)
      {
        std::cerr << "Invalid int numeric format[" << _input << "]\n";
        return 0.0;
      }

      return s * acc;
    }

    /// \brief parse string into float
    /// \param _input the string
    /// \return a floating point number (can be NaN) or 0 with a message in the
    /// error stream
    inline double parseFloat(const std::string& _input)
    {
      const char *p = _input.c_str();
      if (!*p || *p == '?')
        return NAN_D;
      int s = 1;
      while (*p == ' ')
        p++;

      if (*p == '-')
      {
        s = -1;
        p++;
      }

      double acc = 0;
      while (*p >= '0' && *p <= '9')
        acc = acc * 10 + *p++ - '0';

      if (*p == '.')
      {
        double k = 0.1;
        p++;
        while (*p >= '0' && *p <= '9')
        {
          acc += (*p++ - '0') * k;
          k *= 0.1;
        }
      }
      if (*p == 'e')
      {
        int es = 1;
        int f = 0;
        p++;
        if (*p == '-')
        {
          es = -1;
          p++;
        }
        else if (*p == '+')
        {
          es = 1;
          p++;
        }
        while (*p >= '0' && *p <= '9')
          f = f * 10 + *p++ - '0';

        acc *= pow(10, f*es);
      }

      if (*p)
      {
        std::cerr << "Invalid double numeric format[" << _input << "]\n";
        return 0.0;
      }
      return s * acc;
    }
    /// \}
  }
}
#endif


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     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 <libint2/initialize.h>
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE sphere_lebedev_rule_test

// Standard includes
#include <fstream>

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/xtp/orbitals.h"
#include "votca/xtp/sphere_lebedev_rule.h"
#include <votca/tools/eigenio_matrixmarket.h>

using namespace votca::xtp;
using namespace votca;

BOOST_AUTO_TEST_SUITE(sphere_lebedev_rule_test)

BOOST_AUTO_TEST_CASE(medium_test) {
  libint2::initialize();
  QMMolecule mol("noname", 0);
  mol.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                   "/sphere_lebedev_rule/CH4.xyz");

  LebedevGrid spheregrid;

  auto grid = spheregrid.CalculateSphericalGrids(mol, "medium");

  auto Hgrid = grid.at("H");
  auto Cgrid = grid.at("C");

  Eigen::VectorXd C_phi_ref = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) +
      "/sphere_lebedev_rule/C_phi_ref_medium.mm");
  C_phi_ref *= votca::tools::conv::Pi / 180.0;

  Eigen::VectorXd C_theta_ref = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) +
      "/sphere_lebedev_rule/C_theta_ref_medium.mm");

  Eigen::VectorXd C_weight_ref = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) +
      "/sphere_lebedev_rule/C_weight_ref_medium.mm");
  BOOST_CHECK_EQUAL(Cgrid.phi.size(), C_phi_ref.size());
  BOOST_CHECK_EQUAL(Cgrid.theta.size(), C_theta_ref.size());
  BOOST_CHECK_EQUAL(Cgrid.weight.size(), C_weight_ref.size());
  BOOST_CHECK_EQUAL(Hgrid.phi.size(), C_phi_ref.size());
  BOOST_CHECK_EQUAL(Hgrid.theta.size(), C_theta_ref.size());
  BOOST_CHECK_EQUAL(Hgrid.weight.size(), C_weight_ref.size());

  bool Cphi = C_phi_ref.isApprox(Cgrid.phi, 0.001);
  bool Ctheta = C_theta_ref.isApprox(Cgrid.theta, 0.001);
  if (!Cphi || !Ctheta) {
    std::cout << "phi_ref : Phi_comp | theta_ref : theta_comp" << std::endl;
    for (Index i = 0; i < C_phi_ref.size(); i++) {
      std::cout << Cgrid.phi[i] << ":" << C_phi_ref[i] << " | "
                << Cgrid.theta[i] << ":" << C_theta_ref[i] << std::endl;
    }
  }
  bool Cweight = C_weight_ref.isApprox(Cgrid.weight, 0.0001);
  BOOST_CHECK_EQUAL(Cphi, true);
  BOOST_CHECK_EQUAL(Ctheta, true);
  BOOST_CHECK_EQUAL(Cweight, true);

  bool Hphi = C_phi_ref.isApprox(Hgrid.phi, 0.001);
  bool Htheta = C_theta_ref.isApprox(Hgrid.theta, 0.001);
  bool Hweight = C_weight_ref.isApprox(Hgrid.weight, 0.0001);
  BOOST_CHECK_EQUAL(Hphi, true);
  BOOST_CHECK_EQUAL(Htheta, true);
  BOOST_CHECK_EQUAL(Hweight, true);
  libint2::finalize();
}

BOOST_AUTO_TEST_CASE(fine_test) {
  libint2::initialize();
  QMMolecule mol("noname", 0);
  mol.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                   "/sphere_lebedev_rule/molecule.xyz");
  LebedevGrid spheregrid;

  auto grid = spheregrid.CalculateSphericalGrids(mol, "fine");

  auto Hgrid = grid.at("H");
  auto Gegrid = grid.at("Ge");

  Eigen::VectorXd Ge_phi_ref = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) +
      "/sphere_lebedev_rule/Ge_phi_ref_fine.mm");

  Eigen::VectorXd Ge_theta_ref = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) +
      "/sphere_lebedev_rule/Ge_theta_ref_fine.mm");

  Eigen::VectorXd Ge_weight_ref =
      votca::tools::EigenIO_MatrixMarket::ReadVector(
          std::string(XTP_TEST_DATA_FOLDER) +
          "/sphere_lebedev_rule/Ge_weight_ref_fine.mm");

  Eigen::VectorXd H_phi_ref = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) +
      "/sphere_lebedev_rule/H_phi_ref_fine.mm");

  Eigen::VectorXd H_theta_ref = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) +
      "/sphere_lebedev_rule/H_theta_ref_fine.mm");

  Eigen::VectorXd H_weight_ref = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) +
      "/sphere_lebedev_rule/H_weight_ref_fine.mm");

  BOOST_CHECK_EQUAL(Gegrid.phi.size(), Ge_phi_ref.size());
  BOOST_CHECK_EQUAL(Gegrid.theta.size(), Ge_theta_ref.size());
  BOOST_CHECK_EQUAL(Gegrid.weight.size(), Ge_weight_ref.size());
  BOOST_CHECK_EQUAL(Hgrid.phi.size(), H_phi_ref.size());
  BOOST_CHECK_EQUAL(Hgrid.theta.size(), H_theta_ref.size());
  BOOST_CHECK_EQUAL(Hgrid.weight.size(), H_weight_ref.size());

  bool Gephi = Ge_phi_ref.isApprox(Gegrid.phi, 0.001);
  bool Getheta = Ge_theta_ref.isApprox(Gegrid.theta, 0.001);
  if (!Gephi || !Getheta) {
    std::cout << "phi_ref : Phi_comp | theta_ref : theta_comp" << std::endl;
    for (Index i = 0; i < Ge_phi_ref.size(); i++) {
      std::cout << Gegrid.phi[i] << ":" << Ge_phi_ref[i] << " | "
                << Gegrid.theta[i] << ":" << Ge_theta_ref[i] << std::endl;
    }
  }

  bool Geweight = Ge_weight_ref.isApprox(Gegrid.weight, 0.0001);
  BOOST_CHECK_EQUAL(Gephi, true);
  BOOST_CHECK_EQUAL(Getheta, true);
  BOOST_CHECK_EQUAL(Geweight, true);

  bool Hphi = H_phi_ref.isApprox(Hgrid.phi, 0.001);
  bool Htheta = H_theta_ref.isApprox(Hgrid.theta, 0.001);
  bool Hweight = H_weight_ref.isApprox(Hgrid.weight, 0.0001);
  BOOST_CHECK_EQUAL(Hphi, true);
  BOOST_CHECK_EQUAL(Htheta, true);
  BOOST_CHECK_EQUAL(Hweight, true);
  libint2::finalize();
}
BOOST_AUTO_TEST_CASE(element_not_implemented) {
  libint2::initialize();
  QMMolecule mol("noname", 0);
  mol.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                   "/sphere_lebedev_rule/hg.xyz");

  LebedevGrid spheregrid;

  BOOST_REQUIRE_THROW(spheregrid.CalculateSphericalGrids(mol, "xfine"),
                      std::runtime_error);
  libint2::finalize();
}

BOOST_AUTO_TEST_CASE(xfine_test) {
  libint2::initialize();
  QMMolecule mol("noname", 0);
  mol.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                   "/sphere_lebedev_rule/molecule.xyz");

  LebedevGrid spheregrid;

  auto grid = spheregrid.CalculateSphericalGrids(mol, "xfine");

  auto Hgrid = grid.at("H");
  auto Gegrid = grid.at("Ge");

  Eigen::VectorXd Ge_phi_ref = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) +
      "/sphere_lebedev_rule/Ge_phi_ref_xfine.mm");

  Eigen::VectorXd Ge_theta_ref = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) +
      "/sphere_lebedev_rule/Ge_theta_ref_xfine.mm");

  Eigen::VectorXd Ge_weight_ref =
      votca::tools::EigenIO_MatrixMarket::ReadVector(
          std::string(XTP_TEST_DATA_FOLDER) +
          "/sphere_lebedev_rule/Ge_weight_ref_xfine.mm");

  Eigen::VectorXd H_phi_ref = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) +
      "/sphere_lebedev_rule/H_phi_ref_xfine.mm");

  Eigen::VectorXd H_theta_ref = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) +
      "/sphere_lebedev_rule/H_theta_ref_xfine.mm");
  Eigen::VectorXd H_weight_ref = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) +
      "/sphere_lebedev_rule/H_weight_ref_xfine.mm");
  BOOST_CHECK_EQUAL(Gegrid.phi.size(), Ge_phi_ref.size());
  BOOST_CHECK_EQUAL(Gegrid.theta.size(), Ge_theta_ref.size());
  BOOST_CHECK_EQUAL(Gegrid.weight.size(), Ge_weight_ref.size());
  BOOST_CHECK_EQUAL(Hgrid.phi.size(), H_phi_ref.size());
  BOOST_CHECK_EQUAL(Hgrid.theta.size(), H_theta_ref.size());
  BOOST_CHECK_EQUAL(Hgrid.weight.size(), H_weight_ref.size());

  bool Gephi = Ge_phi_ref.isApprox(Gegrid.phi, 0.001);
  bool Getheta = Ge_theta_ref.isApprox(Gegrid.theta, 0.001);
  if (!Gephi || !Getheta) {
    std::cout << "phi_ref : Phi_comp | theta_ref : theta_comp" << std::endl;
    for (Index i = 0; i < Ge_phi_ref.size(); i++) {
      std::cout << Gegrid.phi[i] << ":" << Ge_phi_ref[i] << " | "
                << Gegrid.theta[i] << ":" << Ge_theta_ref[i] << std::endl;
    }
  }

  bool Geweight = Ge_weight_ref.isApprox(Gegrid.weight, 0.0001);
  BOOST_CHECK_EQUAL(Gephi, true);
  BOOST_CHECK_EQUAL(Getheta, true);
  BOOST_CHECK_EQUAL(Geweight, true);

  bool Hphi = H_phi_ref.isApprox(Hgrid.phi, 0.001);
  bool Htheta = H_theta_ref.isApprox(Hgrid.theta, 0.001);
  bool Hweight = H_weight_ref.isApprox(Hgrid.weight, 0.0001);
  BOOST_CHECK_EQUAL(Hphi, true);
  BOOST_CHECK_EQUAL(Htheta, true);
  BOOST_CHECK_EQUAL(Hweight, true);

  libint2::finalize();
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include "Common.hh"

#include <Eigen/Sparse>
#include <Eigen/SparseCore>

namespace betri
{

using EigenSpMatT = Eigen::SparseMatrix<Scalar>;
using EigenTripletT = Eigen::Triplet<Scalar>;
using EigenVectorT = Eigen::VectorXd;

/**
 * @brief Computes a Laplace-based iterative parametrization of a given surface
 *
 */
class Parametrization
{
public:

	using Vertices = std::vector<VertexHandle>;

	explicit Parametrization(BezierTMesh &mesh) : m_mesh(mesh) {}

	// directly solve parametrization
	virtual bool solve() = 0;

	// add/remove needed properties for weights, texture coords etc.
	virtual void prepare() = 0;
	virtual void cleanup() = 0;

protected:

	///////////////////////////////////////////////////////////
	// member variables
	///////////////////////////////////////////////////////////

	BezierTMesh &m_mesh;
};

}


#ifndef _HOLE_FILLING_
#define _HOLE_FILLING_

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/Polygon_mesh_processing/triangulate_hole.h>

#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <boost/foreach.hpp>
#include <ros/ros.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Polyhedron_3<K> P;
typedef P::Halfedge_handle H;
typedef P::Facet_handle F;
typedef P::Vertex_handle V;
typedef P::Point_3  Point3;

int fill_hole(std::string mesh_path, double max_perimeter, std::string base_path);

#endif
// HOLE_FILLING


#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

vec addTwo_parallel(vec x, vec y){
    vec z(x.n_elem);
    int i;
#pragma omp parallel for schedule(static) \
private(i) shared(x,y,z)
    for(i=0; i < x.n_elem; i++)
        z(i) = x(i) + 2 * y(i);
    return z;
}

int main()
{

    vec a = ones<vec>(10);
    vec b = ones<vec>(10);
    vec c = addTwo_parallel(a, b);
    c.print();
    return 0;
}


#include<algorithm>
#include<string>
#include<iostream>
#include<list>
#include <boost/algorithm/string.hpp>
#include "ordinary_number.h"

using namespace std;

int ordinary_number(int n, std::string arg) {

    vector<string> strs;
    boost::split(strs, arg, boost::is_space());
    vector<int> nums(n);

    for(int i=0;i<n;i++)
        nums[i] = stoi(strs.at(i));

    int peek = 1;
    int count = 0;

    while(peek < n-1) {
        if(nums.at(peek-1) < nums.at(peek) && nums.at(peek) < nums.at(peek+1)) {
            count++;
        } else if (nums.at(peek+1) < nums.at(peek) && nums.at(peek) < nums.at(peek-1)) {
            count++;
        }
        peek++;
    }
    return count;
}


/**
 * @date Wed Jul 13 16:58:26 2011 +0200
 * @author Andre Anjos <andre.anjos@idiap.ch>
 *
 * @brief Implementation of the DataShuffler.
 *
 * Copyright (C) 2011-2014 Idiap Research Institute, Martigny, Switzerland
 */

#include <stdexcept>
#include <sys/time.h>
#include <boost/format.hpp>

#include <bob.core/assert.h>
#include <bob.core/array_copy.h>

#include <bob.learn.mlp/shuffler.h>

bob::learn::mlp::DataShuffler::DataShuffler
(const std::vector<blitz::Array<double,2> >& data,
 const std::vector<blitz::Array<double,1> >& target):
  m_data(data.size()),
  m_target(target.size()),
  m_range(),
  m_do_stdnorm(false),
  m_mean(),
  m_stddev()
{
  if (data.size() == 0)
    throw std::runtime_error("data vector cannot be empty");
  if (target.size() == 0)
    throw std::runtime_error("target vector cannot be empty");

  bob::core::array::assertSameDimensionLength(data.size(), target.size());

  // checks shapes, minimum number of examples
  for (size_t k=0; k<data.size(); ++k) {
    if (data[k].size() == 0) {
      boost::format m("class %u has no samples");
      m % k;
      throw std::runtime_error(m.str());
    }
    //this may also trigger if I cannot get doubles from the Arrayset
    bob::core::array::assertSameDimensionLength(data[0].extent(1), data[k].extent(1));
    bob::core::array::assertSameShape(target[0], target[k]);
  }

  // set save values for the mean and stddev (even if not used at start)
  m_mean.resize(data[0].extent(1));
  m_mean = 0.;
  m_stddev.resize(data[0].extent(1));
  m_stddev = 1.;

  // copies the target data to my own variable
  for (size_t k=0; k<target.size(); ++k) {
    m_data[k].reference(bob::core::array::ccopy(data[k]));
    m_target[k].reference(bob::core::array::ccopy(target[k]));
  }

  // creates one range tailored for the range of each data object
  for (size_t i=0; i<data.size(); ++i) {
    m_range.push_back(boost::uniform_int<size_t>(0, m_data[i].extent(0)-1));
  }
}

bob::learn::mlp::DataShuffler::DataShuffler(const bob::learn::mlp::DataShuffler& other):
  m_data(other.m_data.size()),
  m_target(other.m_target.size()),
  m_range(other.m_range),
  m_do_stdnorm(other.m_do_stdnorm),
  m_mean(bob::core::array::ccopy(other.m_mean)),
  m_stddev(bob::core::array::ccopy(other.m_stddev))
{
  for (size_t k=0; k<m_target.size(); ++k) {
    m_data[k].reference(bob::core::array::ccopy(other.m_data[k]));
    m_target[k].reference(bob::core::array::ccopy(other.m_target[k]));
  }
}

bob::learn::mlp::DataShuffler::~DataShuffler() { }

bob::learn::mlp::DataShuffler& bob::learn::mlp::DataShuffler::operator=(const bob::learn::mlp::DataShuffler& other) {

  m_data.resize(other.m_data.size());
  m_target.resize(other.m_target.size());

  for (size_t k=0; k<m_target.size(); ++k) {
    m_data[k].reference(bob::core::array::ccopy(other.m_data[k]));
    m_target[k].reference(bob::core::array::ccopy(other.m_target[k]));
  }

  m_range = other.m_range;

  m_mean.reference(bob::core::array::ccopy(other.m_mean));
  m_stddev.reference(bob::core::array::ccopy(other.m_stddev));
  m_do_stdnorm = other.m_do_stdnorm;

  return *this;
}

/**
 * Calculates mean and std.dev. in a single loop.
 * see: http://en.wikipedia.org/wiki/Algorithms_for_calculating_variance
 */
void evaluateStdNormParameters(const std::vector<blitz::Array<double,2> >& data,
    blitz::Array<double,1>& mean, blitz::Array<double,1>& stddev) {

  mean = 0.;
  stddev = 0.; ///< temporarily used to accumulate square sum!
  double samples = 0;

  blitz::Range all = blitz::Range::all();
  for (size_t k=0; k<data.size(); ++k) {
    for (int i=0; i<data[k].extent(0); ++i) {
      mean += data[k](i,all);
      stddev += blitz::pow2(data[k](i,all));
      ++samples;
    }
  }
  stddev -= blitz::pow2(mean) / samples;
  stddev /= (samples-1); ///< note: unbiased sample variance
  stddev = blitz::sqrt(stddev);

  mean /= (samples);
}

/**
 * Applies standard normalization parameters to all data arrays given
 */
void applyStdNormParameters(std::vector<blitz::Array<double,2> >& data,
    const blitz::Array<double,1>& mean, const blitz::Array<double,1>& stddev) {
  blitz::Range all = blitz::Range::all();
  for (size_t k=0; k<data.size(); ++k) {
    for (int i=0; i<data[k].extent(0); ++i) {
      data[k](i,all) = (data[k](i,all) - mean) / stddev;
    }
  }
}

/**
 * Inverts the application of std normalization parameters
 */
void invertApplyStdNormParameters(std::vector<blitz::Array<double,2> >& data,
    const blitz::Array<double,1>& mean, const blitz::Array<double,1>& stddev) {
  blitz::Range all = blitz::Range::all();
  for (size_t k=0; k<data.size(); ++k) {
    for (int i=0; i<data[k].extent(0); ++i) {
      data[k](i,all) = (data[k](i,all) * stddev) + mean;
    }
  }
}

void bob::learn::mlp::DataShuffler::setAutoStdNorm(bool s) {
  if (s && !m_do_stdnorm) {
    evaluateStdNormParameters(m_data, m_mean, m_stddev);
    applyStdNormParameters(m_data, m_mean, m_stddev);
  }
  if (!s && m_do_stdnorm) {
    invertApplyStdNormParameters(m_data, m_mean, m_stddev);
    m_mean = 0.;
    m_stddev = 1.;
  }
  m_do_stdnorm = s;
}

void bob::learn::mlp::DataShuffler::getStdNorm(blitz::Array<double,1>& mean,
    blitz::Array<double,1>& stddev) const {
  bob::core::array::assertSameShape(mean, m_mean);
  bob::core::array::assertSameShape(stddev, m_stddev);
  if (m_do_stdnorm) {
    mean = m_mean;
    stddev = m_stddev;
  }
  else {
    evaluateStdNormParameters(m_data, mean, stddev);
  }
}

void bob::learn::mlp::DataShuffler::operator() (boost::mt19937& rng,
    blitz::Array<double,2>& data, blitz::Array<double,2>& target) {

  bob::core::array::assertSameDimensionLength(data.extent(0), target.extent(0));

  size_t counter = 0;
  size_t max = data.extent(0);
  blitz::Range all = blitz::Range::all();
  while (true) {
    for (size_t i=0; i<m_data.size(); ++i) { //for all classes
      size_t index = m_range[i](rng); //pick a random position within class
      data(counter,all) = m_data[i](index,all);
      target(counter,all) = m_target[i];
      ++counter;
      if (counter >= max) break;
    }
    if (counter >= max) break;
  }

}

void bob::learn::mlp::DataShuffler::operator() (blitz::Array<double,2>& data,
    blitz::Array<double,2>& target) {
  struct timeval tv;
  gettimeofday(&tv, 0);
  boost::mt19937 rng(tv.tv_sec + tv.tv_usec);
  operator()(rng, data, target);
}