//
// Libor Novak
// 03/20/2017
//
// Tests object detection of 2D bounding boxes a single scale detector on an image pyramid. It outputs BBTXT.
//
//

#include <caffe/caffe.hpp>
#include "caffe/util/benchmark.hpp"
#include "caffe/util/utils_bb.hpp"

// This code only works with OpenCV!
#ifdef USE_OPENCV

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <algorithm>
#include <iosfwd>
#include <memory>
#include <string>
#include <utility>
#include <vector>

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

#define BASIC_NON_MAXIMA_SUPPRESSION

#ifdef BASIC_NON_MAXIMA_SUPPRESSION
    // Maximum intersection over union of two boxes that will be both kept after NMS
    #define IOU_THRESHOLD 0.5
#else
    // Maximum similariry of two bounding boxes that will be both kept
    #define SIMILARITY_THRESHOLD 0.65
#endif

namespace {

#ifndef BASIC_NON_MAXIMA_SUPPRESSION
    /**
     * @brief Combines 2 bounding boxes into one
     * @param bb1
     * @param bb2
     * @return Merged bounding box
     */
    BB2D merge_bbs (const BB2D &bb1, const BB2D &bb2)
    {
        BB2D bb;
        bb.path_image = bb1.path_image;
        bb.label      = bb1.label;
//        bb.conf       = std::max(bb1.conf, bb2.conf);
        bb.conf       = bb1.conf + bb2.conf;
        bb.xmin       = (bb1.conf*bb1.xmin + bb2.conf*bb2.xmin) / (bb1.conf+bb2.conf);
        bb.ymin       = (bb1.conf*bb1.ymin + bb2.conf*bb2.ymin) / (bb1.conf+bb2.conf);
        bb.xmax       = (bb1.conf*bb1.xmax + bb2.conf*bb2.xmax) / (bb1.conf+bb2.conf);
        bb.ymax       = (bb1.conf*bb1.ymax + bb2.conf*bb2.ymax) / (bb1.conf+bb2.conf);

        return bb;
    }

    /**
     * @brief Similarity measure of 2 bounding boxes
     * @param bb1
     * @param bb2
     * @return Similarity (the more similar bbs, the larger)
     */
    double similarity (const BB2D &bb1, const BB2D &bb2)
    {
        return iou2d(bb1, bb2);
    }
#endif

}

/**
 * @brief Wraps the input layer into a vector of cv::Mat so we could assign data to it more easily
 * @param input_layer Pointer to the net input layer blob
 * @param input_channels Vector of cv::Mat, which will be assigned
 */
void wrapInputLayer (caffe::Blob<float>* input_layer, std::vector<cv::Mat> &out_input_channels)
{
    out_input_channels.clear();

    int height = input_layer->shape(2);
    int width  = input_layer->shape(3);

    float* input_data = input_layer->mutable_cpu_data();

    for (int i = 0; i < input_layer->shape(1); ++i)
    {
        cv::Mat channel(height, width, CV_32FC1, input_data);
        out_input_channels.push_back(channel);
        input_data += width * height;
    }
}

std::vector<BB2D> extract2DBoundingBoxes (caffe::Blob<float> *output, const std::string &path_image,
                                          double scale)
{
    std::vector<BB2D> bounding_boxes;

    float *data_output = output->mutable_cpu_data();

    // 2D bounding box
    cv::Mat acc_prob(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 0));
    cv::Mat acc_xmin(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 1));
    cv::Mat acc_ymin(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 2));
    cv::Mat acc_xmax(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 3));
    cv::Mat acc_ymax(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 4));

    // Extract detected boxes - only extract local maxima from 3x3 neighborhood
    for (int i = 0; i < acc_prob.rows; ++i)
    {
        for (int j = 0; j < acc_prob.cols; ++j)
        {
            float conf = acc_prob.at<float>(i, j);
            if (conf >= 0.1)
            {
                // Check if it is a local maximum
                if (i > 0)
                {
                    if (j > 0 && acc_prob.at<float>(i-1, j-1) > conf) continue;
                    if (acc_prob.at<float>(i-1, j) > conf) continue;
                    if (j < acc_prob.cols-1 && acc_prob.at<float>(i-1, j+1) > conf) continue;
                }
                if (j > 0 && acc_prob.at<float>(i, j-1) > conf) continue;
                if (j < acc_prob.cols-1 && acc_prob.at<float>(i, j+1) > conf) continue;
                if (i < acc_prob.rows-1)
                {
                    if (j > 0 && acc_prob.at<float>(i+1, j-1) > conf) continue;
                    if (acc_prob.at<float>(i+1, j) > conf) continue;
                    if (j < acc_prob.cols-1 && acc_prob.at<float>(i+1, j+1) > conf) continue;
                }

                // Ok, this is a local maximum
                double xmin = acc_xmin.at<float>(i, j) / scale;
                double ymin = acc_ymin.at<float>(i, j) / scale;
                double xmax = acc_xmax.at<float>(i, j) / scale;
                double ymax = acc_ymax.at<float>(i, j) / scale;

                bounding_boxes.emplace_back(path_image, 1, conf, xmin, ymin, xmax, ymax);
            }
        }
    }

    return bounding_boxes;
}

std::vector<BB2D> detectObjects (const std::string &path_image, const std::vector<double> &scales,
                                 const std::shared_ptr<caffe::Net<float>> &net)
{
#ifdef MEASURE_TIME
    caffe::CPUTimer timer;
#endif
    std::vector<BB2D> bounding_boxes;

    caffe::Blob<float>* input_layer  = net->input_blobs()[0];

    std::vector<cv::Mat> input_channels;

#ifdef MEASURE_TIME
    timer.Start();
#endif
    // Read the image
    cv::Mat image = cv::imread(path_image, CV_LOAD_IMAGE_COLOR);
    // Convert to zero mean and unit variance
    cv::Mat imagef; image.convertTo(imagef, CV_32FC3);
    imagef -= cv::Scalar(128.0f, 128.0f, 128.0f);
    imagef *= 1.0f/128.0f;
#ifdef MEASURE_TIME
    timer.Stop(); std::cout << "Time to to read image: " << timer.MilliSeconds() << " ms" << std::endl;
#endif

#ifdef MEASURE_TIME
    timer.Start();
#endif
    // Build the image pyramid and run detection on each scale of the pyramid
    for (double s: scales)
    {
        cv::Mat imagef_scaled;
        cv::resize(imagef, imagef_scaled, cv::Size(), s, s);

        // Reshape the network
        input_layer->Reshape(1, input_layer->shape(1), imagef_scaled.rows, imagef_scaled.cols);
        net->Reshape();

        // Prepare the cv::Mats for input
        wrapInputLayer(input_layer, input_channels);
        // Copy the image to the input layer of the network
        cv::split(imagef_scaled, input_channels);

        net->Forward();

        for (caffe::Blob<float>* output: net->output_blobs())
        {
            std::vector<BB2D> new_bbs = extract2DBoundingBoxes(output, path_image, s);
            bounding_boxes.insert(bounding_boxes.end(), new_bbs.begin(), new_bbs.end());
        }
    }

#ifdef MEASURE_TIME
    timer.Stop(); std::cout << "Time net + bb extraction: " << timer.MilliSeconds() << " ms" << std::endl;
#endif

    return bounding_boxes;
}

std::vector<BB2D> nonMaximaSuppression (std::vector<BB2D> &bbs)
{
#ifdef BASIC_NON_MAXIMA_SUPPRESSION
    // This is standard non-maxima suppression, where we throw away boxes, which have a higher intersection
    // over union then given threshold with some other box with a higher confidence

    // Sort by confidence in the descending order
    std::sort(bbs.begin(), bbs.end(), [](const BB2D &a, const BB2D &b) { return a.conf > b.conf; });

    std::vector<BB2D> bbs_out;
    std::vector<bool> active(bbs.size(), true);

    for (int i = 0; i < bbs.size(); ++i)
    {
        if (active[i])
        {
            active[i] = false;
            bbs_out.push_back(bbs[i]);

            // Check intersection with all remaining bounding boxes
            for (int j = i; j < bbs.size(); ++j)
            {
                if (active[j] && iou2d(bbs_out.back(), bbs[j]) > IOU_THRESHOLD)
                {
//                    bbs_out[bbs_out.size()-1] = merge_bbs(bbs[j], bbs_out[bbs_out.size()-1]);
                    active[j] = false;

                    // Restart the checking - the bb changed
//                    j = i;
                }
            }
        }
    }
#else
    // More sophisticated non-maxima suppression (accordint to the authors of OverFeat), where we merge
    // simimar boxes instead of throwing them away

    // Compute the matrix of similarities
    cv::Mat sims(bbs.size(), bbs.size(), CV_64FC1, cv::Scalar(0,0));
    for (int i = 0; i < bbs.size()-1; ++i)
    {
        for (int j = i+1; j < bbs.size(); ++j)  sims.at<double>(i, j) = similarity(bbs[i], bbs[j]);
    }

    std::vector<bool> active(bbs.size(), true);
    while (true)
    {
        cv::Point mx_loc;
        double mx;
        cv::minMaxLoc(sims, 0, &mx, 0, &mx_loc);

        if (mx < SIMILARITY_THRESHOLD) break;

        // Merge the two boxes with the highest similarity
        bbs[mx_loc.x] = merge_bbs(bbs[mx_loc.x], bbs[mx_loc.y]);
        // Remove the second box from further computation
        active[mx_loc.y] = false;
        for (int j = mx_loc.y; j < bbs.size(); ++j) sims.at<double>(mx_loc.y, j) = 0.0;
        for (int i = 0; i < mx_loc.y; ++i) sims.at<double>(i, mx_loc.y) = 0.0;

        // Recompute the distances for the new box
        for (int j = mx_loc.x+1; j < bbs.size(); ++j)
        {
            if (active[j]) sims.at<double>(mx_loc.x, j) = similarity(bbs[j], bbs[mx_loc.x]);
        }
        for (int i = 0; i < mx_loc.y; ++i)
        {
            if (active[i]) sims.at<double>(i, mx_loc.y) = similarity(bbs[i], bbs[mx_loc.x]);
        }
    }

    // Copy out the active bounding boxes
    std::vector<BB2D> bbs_out;
    for (int i = 0; i < bbs.size(); ++i)
    {
        if (active[i]) bbs_out.push_back(bbs[i]);
    }
#endif

    return bbs_out;
}

void writeBoundingBoxes (const std::vector<BB2D> &bbs, std::ofstream &fout)
{
    for (const BB2D &bb: bbs)
    {
        // The line of a BBTXT file is: filename label confidence xmin ymin xmax ymax
        fout << bb.path_image << " " << bb.label << " " << bb.conf << " " << bb.xmin << " " << bb.ymin
             << " " << bb.xmax << " " << bb.ymax << std::endl;
    }
}

void runPyramidDetection (const std::string &path_prototxt, const std::string &path_caffemodel,
                          const std::string &path_image_list, const std::string &path_out)
{
#ifdef CPU_ONLY
    caffe::Caffe::set_mode(caffe::Caffe::CPU);
#else
    caffe::Caffe::set_mode(caffe::Caffe::GPU);
#endif

#ifdef MEASURE_TIME
    caffe::CPUTimer timer;
#endif

    // Scaling factor is 1.5
    // around that size
//    const std::vector<double> scales = { 1.0, 0.66, 0.44, 0.29, 0.19 };
    const std::vector<double> scales = { 1.0 };

    // Create network and load trained weights from caffemodel file
    auto net = std::make_shared<caffe::Net<float>>(path_prototxt, caffe::TEST);
    net->CopyTrainedLayersFrom(path_caffemodel);

    caffe::Blob<float>* input_layer  = net->input_blobs()[0];
    caffe::Blob<float>* output_layer = net->output_blobs()[0];

    CHECK_EQ(net->num_inputs(), 1) << "Network should have exactly one input.";
    CHECK_EQ(input_layer->shape(1), 3) << "Input layer must have 3 channels.";
    CHECK_EQ(output_layer->shape(1), 5) << "Unsupported network, only 5 channels!";

    std::ifstream infile(path_image_list.c_str());
    CHECK(infile) << "Unable to open image list TXT file '" << path_image_list << "'!";
    std::string line;

    std::ofstream fout; fout.open(path_out);
    CHECK(fout) << "Output file '" << path_out << "' could not have been created!";
    std::ofstream fout_nms; fout_nms.open(path_out.substr(0, path_out.size()-6) + "_nms.bbtxt");

    // -- RUN THE DETECTOR ON EACH IMAGE -- //
    while (std::getline(infile, line))
    {
        LOG(INFO) << line;
        CHECK(boost::filesystem::exists(line)) << "Image '" << line << "' not found!";

        // Detect bbs on the image
        std::vector<BB2D> bbs = detectObjects(line, scales, net);

        // Save the bounding boxes before NMS to a BBTXT file
        writeBoundingBoxes(bbs, fout);

        // Non-maxima suppression
#ifdef MEASURE_TIME
        timer.Start();
#endif
        bbs = nonMaximaSuppression(bbs);
#ifdef MEASURE_TIME
        timer.Stop(); std::cout << "Time to perform NMS: " << timer.MilliSeconds() << " ms" << std::endl;
#endif

        // Save the bounding boxes after NMS to a BBTXT file
        writeBoundingBoxes(bbs, fout_nms);
    }

    fout.close();
    fout_nms.close();
}

// -----------------------------------------------  MAIN  ------------------------------------------------ //

struct ProgramArguments
{
    std::string path_prototxt;
    std::string path_caffemodel;
    std::string path_image_list;
    std::string path_out;
};

/**
 * @brief Parses arguments of the program
 */
void parseArguments (int argc, char** argv, ProgramArguments &pa)
{
    try {
        po::options_description desc("Arguments");
        desc.add_options()
            ("help", "Print help")
            ("prototxt", po::value<std::string>(&pa.path_prototxt)->required(),
             "Model file of the network (*.prototxt)")
            ("caffemodel", po::value<std::string>(&pa.path_caffemodel)->required(),
             "Weight file of the network (*.caffemodel)")
            ("image_list", po::value<std::string>(&pa.path_image_list)->required(),
             "Path to a TXT file with paths to the images to be tested")
            ("path_out", po::value<std::string>(&pa.path_out)->required(),
             "Path to the output BBTXT file")
        ;

        po::positional_options_description positional;
        positional.add("prototxt", 1);
        positional.add("caffemodel", 1);
        positional.add("image_list", 1);
        positional.add("path_out", 1);

        // Parse the input arguments
        po::variables_map vm;
        po::store(po::command_line_parser(argc, argv).options(desc).positional(positional).run(), vm);

        if (vm.count("help")) {
            std::cout << "Usage: ./detect_accumulator path/f.prototxt path/f.caffemodel path/image_list.txt path/out.bbtxt\n";
            std::cout << desc;
            exit(EXIT_SUCCESS);
        }

        po::notify(vm);

        if (!boost::filesystem::exists(pa.path_prototxt))
        {
            std::cerr << "ERROR: File '" << pa.path_prototxt << "' does not exist!" << std::endl;
            exit(EXIT_FAILURE);
        }
        if (!boost::filesystem::exists(pa.path_caffemodel))
        {
            std::cerr << "ERROR: File '" << pa.path_caffemodel << "' does not exist!" << std::endl;
            exit(EXIT_FAILURE);
        }
        if (!boost::filesystem::exists(pa.path_image_list))
        {
            std::cerr << "ERROR: File '" << pa.path_image_list << "' does not exist!" << std::endl;
            exit(EXIT_FAILURE);
        }
        if (boost::filesystem::exists(pa.path_out))
        {
            std::cerr << "ERROR: File '" << pa.path_out << "' already exists!" << std::endl;
            exit(EXIT_FAILURE);
        }
        if (pa.path_out.substr(pa.path_out.size()-6, 6) != ".bbtxt")
        {
            std::cerr << "ERROR: BBTXT file is produced on the output. The given output filename does not "
                      << "match the extension .bbtxt!" << std::endl;
            exit(EXIT_FAILURE);
        }
    }
    catch(std::exception& e)
    {
        std::cerr << e.what() << "\n";
        exit(EXIT_FAILURE);
    }
}

int main (int argc, char** argv)
{
    FLAGS_logtostderr = 1;
    FLAGS_minloglevel = ::google::INFO;
    ::google::InitGoogleLogging(argv[0]);

    ProgramArguments pa;
    parseArguments(argc, argv, pa);

    runPyramidDetection(pa.path_prototxt, pa.path_caffemodel, pa.path_image_list, pa.path_out);

    return EXIT_SUCCESS;
}

#else
int main(int argc, char** argv) {
    LOG(FATAL) << "This example requires OpenCV; compile with USE_OPENCV.";
}
#endif  // USE_OPENCV


/*
   Copyright 2018 Simon Vogl <svogl@voxel.at>
                  Angel Merino-Sastre <amerino@voxel.at>

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/
#include <iostream>

#if OCV_VERSION_MAJOR >= 3
#  include <opencv2/imgproc.hpp>
#  include <opencv2/highgui.hpp>
#else
#  include <opencv2/imgproc/imgproc.hpp>
#  include <opencv2/highgui/highgui.hpp>
#endif

#include <boost/log/trivial.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string/case_conv.hpp>

#include "toffy/base/focus.hpp"

using namespace toffy;
using namespace toffy::filters;
using namespace cv;
using namespace std;
namespace logging = boost::log;
namespace fs = boost::filesystem;

#ifdef CM_DEBUG
const bool dbg=true;
#else
const bool dbg=false;
#endif

const std::string Focus::id_name = "focus";

Focus::Focus(): Filter(Focus::id_name),
		    in_img("ampl"), out_focus("focus")
{
}

void Focus::updateConfig(const boost::property_tree::ptree &pt) {
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ <<  " " << id();

    using namespace boost::property_tree;

    Filter::updateConfig(pt);

    boost::optional<const boost::property_tree::ptree& > ocvo = pt.get_child_optional( "options.cameraMatrix" );

    in_img = pt.get<string>("inputs.img",in_img);
    out_focus = pt.get<string>("outputs.focus",out_focus);
}

boost::property_tree::ptree Focus::getConfig() const {
    boost::property_tree::ptree pt;

    pt = Filter::getConfig();

    pt.put("inputs.img", in_img);
    pt.put("outputs.focus", out_focus);

    return pt;
}

bool Focus::filter(const Frame &in, Frame& out) {
	BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ <<  " " << id();

	boost::shared_ptr<cv::Mat> img;
	try {
		img = boost::any_cast<boost::shared_ptr<cv::Mat> >(in.getData(in_img));

	} catch(const boost::bad_any_cast &) {
		BOOST_LOG_TRIVIAL(warning) <<
			"Could not cast input " << in_img <<
			", filter  " << id() <<" not applied.";
		return false;
	}

	{
	    //
	    // compute focus based on laplacian:
	    // cf. http://stackoverflow.com/questions/28717054/calculating-sharpness-of-an-image
	    //
	    Mat dst;
	    cv::Laplacian(*img, dst, CV_32F);

	    cv::Scalar mu, sigma;
	    cv::meanStdDev(dst, mu, sigma);

	    double focusMeasure = sigma.val[0] * sigma.val[0];

	    out.addData(out_focus, focusMeasure);
	}

	return true;
}


/* Copyright 2021 Kovács Gergely Attila
*
*   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 F1TENTH_SENSOR_FUSION__KFTRACKER_HPP
#define F1TENTH_SENSOR_FUSION__KFTRACKER_HPP

#include <opencv2/video/tracking.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/container/vector.hpp>
#include <pcl/point_types.h>
#include <memory>

namespace f1tenth_sensor_fusion
{
    typedef boost::container::vector<pcl::PointXYZ> PointVector;

#define prune_interval 50

    class KFTracker
    {
    public:
        boost::container::vector<int> track(const PointVector &cCentres);
        void initialize(const PointVector &cCentres);

    private:
        boost::mutex mutex_;
        boost::container::vector<std::unique_ptr<cv::KalmanFilter>> k_filters_;
        size_t kf_prune_ctr_ = 0;

        inline float euclidian_dst(const pcl::PointXYZ &p, const pcl::PointXYZ &q)
        {
            return sqrt((p.x - q.x) * (p.x - q.x) + (p.y - q.y) * (p.y - q.y + (p.z - q.z) * (p.z - q.z)));
        }

        inline void _set_kfilter_state_pre(std::unique_ptr<cv::KalmanFilter> &filter, const pcl::PointXYZ &pt)
        {
            filter->statePre.at<float>(0) = pt.x;
            filter->statePre.at<float>(1) = pt.y;
            filter->statePre.at<float>(2) = 0;
            filter->statePre.at<float>(3) = 0;
        }

        void _init_KFilters(size_t n);
        PointVector generate_predictions();
        boost::container::vector<int> match_objID(const PointVector &pred, const PointVector &cCentres, bool *cluster_used);
        void create_kfilters_for_new_clusters(boost::container::vector<int> &objID, const PointVector &centres, const bool *cluster_used);
        void prune_unused_kfilters(boost::container::vector<int> &objID);
        void correct_kfilter_matrices(const PointVector &cCentres, const boost::container::vector<int> &objID);

        /**
         * Return the indices of the smallest element of a 2D matrix. Each row represents a KF predition, each column a detected cluster centroid, their
         * intersections the distance between the two.
         *
         * @param mat the distance matrix
         * @return the indicies of the minimum element. <-1, -1> if no minimum was found (i.e. all elements equal)
         */
        std::pair<int, int> find_min_IDX(const float *mat, const size_t row, const size_t col);
    };
}

#endif // F1TENTH_SENSOR_FUSION__KFTRACKER_HPP


//=============================================================================
//
// AKAZE.cpp
// Authors: Pablo F. Alcantarilla (1), Jesus Nuevo (2)
// Institutions: Toshiba Research Europe Ltd (1)
//               TrueVision Solutions (2)
// Date: 07/10/2014
// Email: pablofdezalc@gmail.com
//
// AKAZE Features Copyright 2014, Pablo F. Alcantarilla, Jesus Nuevo
// All Rights Reserved
// See LICENSE for the license information
//=============================================================================

/**
 * @file AKAZE.cpp
 * @brief Main class for detecting and describing binary features in an
 * accelerated nonlinear scale space
 * @date Oct 07, 2014
 * @author Pablo F. Alcantarilla, Jesus Nuevo
 */

#include "cimg/CImg.h"
#include "src/AKAZE.h"
#include "src/utils.h"
#include "timer/timer.hpp"

#include <Eigen/Dense>
#include <math.h>
#include <cassert>
#include <ctime>
#include <iostream>

#define _USE_MATH_DEFINESo

using namespace libAKAZE;

/* ************************************************************************* */
AKAZE::AKAZE(const AKAZEOptions& options) : options_(options) {
  Eigen::initParallel();
#ifdef AKAZE_USE_OPENMP
  omp_set_num_threads(options.num_threads);
#define EIGEN_DONT_PARALLELIZE
#endif
  ncycles_ = 0;
  reordering_ = true;

  if (options_.descriptor_size > 0 && options_.descriptor >= MLDB_UPRIGHT) {
    generateDescriptorSubsample(
        descriptorSamples_, descriptorBits_, options_.descriptor_size,
        options_.descriptor_pattern_size, options_.descriptor_channels);
  }
  Allocate_Memory_Evolution();
}

/* ************************************************************************* */
AKAZE::~AKAZE() { evolution_.clear(); }

/* ************************************************************************* */
void AKAZE::Allocate_Memory_Evolution() {

  float rfactor = 0.0;
  int level_height = 0, level_width = 0;

  // Allocate the dimension of the matrices for the evolution
  evolution_.reserve(options_.omax * options_.nsublevels);
  for (int i = 0; i < options_.omax; i++) {
    rfactor = 1.0 / pow(2.f, i);
    level_height = (int)(options_.img_height * rfactor);
    level_width = (int)(options_.img_width * rfactor);

    // Smallest possible octave and allow one scale if the image is small
    if ((level_width < 80 || level_height < 40) && i != 0) {
      options_.omax = i;
      break;
    }

    for (int j = 0; j < options_.nsublevels; j++) {
      TEvolution step;
      step.Lx.resize(level_height, level_width);
      step.Ly.resize(level_height, level_width);
      step.Lxx.resize(level_height, level_width);
      step.Lxy.resize(level_height, level_width);
      step.Lyy.resize(level_height, level_width);
      step.Lt.resize(level_height, level_width);
      step.Ldet.resize(level_height, level_width);
      step.Lflow.resize(level_height, level_width);
      step.Lstep.resize(level_height, level_width);

      step.esigma = options_.soffset *
                    pow(2.f, (float)(j) / (float)(options_.nsublevels) + i);
      step.sigma_size = fRound(step.esigma);
      step.etime = 0.5 * (step.esigma * step.esigma);
      step.octave = i;
      step.sublevel = j;
      evolution_.push_back(step);
    }
  }

  // Allocate memory for the number of cycles and time steps
  for (size_t i = 1; i < evolution_.size(); i++) {
    int naux = 0;
    std::vector<float> tau;
    float ttime = 0.0;
    ttime = evolution_[i].etime - evolution_[i - 1].etime;
    naux = fed_tau_by_process_time(ttime, 1, 0.25, reordering_, tau);
    nsteps_.push_back(naux);
    tsteps_.push_back(tau);
    ncycles_++;
  }
}

/* ************************************************************************* */
int AKAZE::Create_Nonlinear_Scale_Space(const RowMatrixXf& img) {
  if (evolution_.size() == 0) {
    std::cerr << "Error generating the nonlinear scale space!!" << std::endl;
    std::cerr << "Firstly you need to call AKAZE::Allocate_Memory_Evolution()"
              << std::endl;
    return -1;
  }

  timer::Timer timer;

  // Copy the original image to the first level of the evolution
  gaussian_2D_convolution(img, evolution_[0].Lt, 0, 0, options_.soffset);
  evolution_[0].Lsmooth = evolution_[0].Lt;

  // First compute the kcontrast factor
  options_.kcontrast = compute_k_percentile(
      img, options_.kcontrast_percentile, 1.0, options_.kcontrast_nbins, 0, 0);

  timing_.kcontrast = timer.elapsedMs();

  // Now generate the rest of evolution levels
  for (size_t i = 1; i < evolution_.size(); i++) {

    if (evolution_[i].octave > evolution_[i - 1].octave) {
      halfsample_image(evolution_[i - 1].Lt, evolution_[i].Lt);
      options_.kcontrast = options_.kcontrast * 0.75;
    } else {
      evolution_[i].Lt = evolution_[i - 1].Lt;
    }

    gaussian_2D_convolution(evolution_[i].Lt, evolution_[i].Lsmooth, 0, 0, 1.0);

    // Compute the Gaussian derivatives Lx and Ly
    image_derivatives_scharr(evolution_[i].Lsmooth, evolution_[i].Lx, 1, 0);
    image_derivatives_scharr(evolution_[i].Lsmooth, evolution_[i].Ly, 0, 1);

    // Compute the conductivity equation
    switch (options_.diffusivity) {
      case PM_G1:
        pm_g1(evolution_[i].Lx, evolution_[i].Ly, evolution_[i].Lflow,
              options_.kcontrast);
        break;
      case PM_G2:
        pm_g2(evolution_[i].Lx, evolution_[i].Ly, evolution_[i].Lflow,
              options_.kcontrast);
        break;
      case WEICKERT:
        weickert_diffusivity(evolution_[i].Lx, evolution_[i].Ly,
                             evolution_[i].Lflow, options_.kcontrast);
        break;
      case CHARBONNIER:
        charbonnier_diffusivity(evolution_[i].Lx, evolution_[i].Ly,
                                evolution_[i].Lflow, options_.kcontrast);
        break;
      default:
        std::cerr << "Diffusivity: " << options_.diffusivity
                  << " is not supported" << std::endl;
    }

    // Perform FED n inner steps
    for (int j = 0; j < nsteps_[i - 1]; j++)
      nld_step_scalar(evolution_[i].Lt, evolution_[i].Lflow,
                      evolution_[i].Lstep, tsteps_[i - 1][j]);
  }

  timing_.scale = timer.elapsedMs();

  return 0;
}

/* ************************************************************************* */
void AKAZE::Feature_Detection(std::vector<AKAZEKeypoint>& kpts) {
  timer::Timer timer;

  kpts.clear();
  Compute_Determinant_Hessian_Response();
  Find_Scale_Space_Extrema(kpts);
  Do_Subpixel_Refinement(kpts);

  timing_.detector = timer.elapsedMs();
}

/* ************************************************************************* */
void AKAZE::Compute_Multiscale_Derivatives() {
  timer::Timer timer;

#ifdef AKAZE_USE_OPENMP
#pragma omp parallel for
#endif
  for (int i = 0; i < (int)(evolution_.size()); i++) {

    float ratio = pow(2.f, (float)evolution_[i].octave);
    int sigma_size_ =
        fRound(evolution_[i].esigma * options_.derivative_factor / ratio);

    compute_scharr_derivatives(evolution_[i].Lsmooth, evolution_[i].Lx, 1, 0,
                               sigma_size_);
    compute_scharr_derivatives(evolution_[i].Lsmooth, evolution_[i].Ly, 0, 1,
                               sigma_size_);
    compute_scharr_derivatives(evolution_[i].Lx, evolution_[i].Lxx, 1, 0,
                               sigma_size_);
    compute_scharr_derivatives(evolution_[i].Ly, evolution_[i].Lyy, 0, 1,
                               sigma_size_);
    compute_scharr_derivatives(evolution_[i].Lx, evolution_[i].Lxy, 0, 1,
                               sigma_size_);

    evolution_[i].Lx = evolution_[i].Lx * ((sigma_size_));
    evolution_[i].Ly = evolution_[i].Ly * ((sigma_size_));
    evolution_[i].Lxx = evolution_[i].Lxx * ((sigma_size_) * (sigma_size_));
    evolution_[i].Lxy = evolution_[i].Lxy * ((sigma_size_) * (sigma_size_));
    evolution_[i].Lyy = evolution_[i].Lyy * ((sigma_size_) * (sigma_size_));
  }

  timing_.derivatives = timer.elapsedMs();
}

/* ************************************************************************* */
void AKAZE::Compute_Determinant_Hessian_Response() {

  // Firstly compute the multiscale derivatives
  Compute_Multiscale_Derivatives();

  for (size_t i = 0; i < evolution_.size(); i++) {
    if (options_.verbosity == true) {
      std::cout
          << "Computing detector response. Determinant of Hessian. Evolution "
             "time: " << evolution_[i].etime << std::endl;
    }

    evolution_[i].Ldet =
        evolution_[i].Lxx.cwiseProduct(evolution_[i].Lyy).array() -
        evolution_[i].Lxy.array().square();
  }
}

/* ************************************************************************* */
void AKAZE::Find_Scale_Space_Extrema(std::vector<AKAZEKeypoint>& kpts) {
  float value = 0.0;
  float dist = 0.0, ratio = 0.0, smax = 0.0;
  int npoints = 0, id_repeated = 0;
  int sigma_size_ = 0, left_x = 0, right_x = 0, up_y = 0, down_y = 0;
  bool is_extremum = false, is_repeated = false, is_out = false;
  AKAZEKeypoint point;
  std::vector<AKAZEKeypoint> kpts_aux;

  // Set maximum size
  if (options_.descriptor == SURF_UPRIGHT || options_.descriptor == SURF ||
      options_.descriptor == MLDB_UPRIGHT || options_.descriptor == MLDB) {
    smax = 10.0 * sqrtf(2.0);
  } else if (options_.descriptor == MSURF_UPRIGHT ||
             options_.descriptor == MSURF) {
    smax = 12.0 * sqrtf(2.0);
  }

  timer::Timer timer;

  for (size_t i = 0; i < evolution_.size(); i++) {
    for (int ix = 1; ix < evolution_[i].Ldet.rows() - 1; ix++) {
      for (int jx = 1; jx < evolution_[i].Ldet.cols() - 1; jx++) {
        is_extremum = false;
        is_repeated = false;
        is_out = false;
        RowMatrixXf& ldet = evolution_[i].Ldet;
        value = ldet(ix, jx);

        // Filter the points with the detector threshold
        if (value > options_.dthreshold && value >= options_.min_dthreshold &&
            value > ldet(ix, jx - 1) && value > ldet(ix, jx + 1) &&
            value > ldet(ix - 1, jx - 1) && value > ldet(ix - 1, jx) &&
            value > ldet(ix - 1, jx + 1) && value > ldet(ix + 1, jx - 1) &&
            value > ldet(ix + 1, jx) && value > ldet(ix + 1, jx + 1)) {

          is_extremum = true;
          point.response = fabs(value);
          point.size = evolution_[i].esigma * options_.derivative_factor;
          point.octave = evolution_[i].octave;
          point.class_id = i;
          ratio = pow(2.f, point.octave);
          sigma_size_ = fRound(point.size / ratio);
          point.pt.x() = jx;
          point.pt.y() = ix;

          // Compare response with the same and lower scale
          for (size_t ik = 0; ik < kpts_aux.size(); ik++) {

            if ((point.class_id - 1) == kpts_aux[ik].class_id ||
                point.class_id == kpts_aux[ik].class_id) {
              dist = sqrt(pow(point.pt.x() * ratio - kpts_aux[ik].pt.x(), 2) +
                          pow(point.pt.y() * ratio - kpts_aux[ik].pt.y(), 2));
              if (dist <= point.size) {
                if (point.response > kpts_aux[ik].response) {
                  id_repeated = ik;
                  is_repeated = true;
                } else {
                  is_extremum = false;
                }
                break;
              }
            }
          }

          // Check out of bounds
          if (is_extremum == true) {

            // Check that the point is under the image limits for the descriptor
            // computation
            left_x = fRound(point.pt.x() - smax * sigma_size_) - 1;
            right_x = fRound(point.pt.x() + smax * sigma_size_) + 1;
            up_y = fRound(point.pt.y() - smax * sigma_size_) - 1;
            down_y = fRound(point.pt.y() + smax * sigma_size_) + 1;

            if (left_x < 0 || right_x >= evolution_[i].Ldet.cols() ||
                up_y < 0 || down_y >= evolution_[i].Ldet.rows()) {
              is_out = true;
            }

            if (is_out == false) {
              if (is_repeated == false) {
                point.pt.x() *= ratio;
                point.pt.y() *= ratio;
                kpts_aux.push_back(point);
                npoints++;
              } else {
                point.pt.x() *= ratio;
                point.pt.y() *= ratio;
                kpts_aux[id_repeated] = point;
              }
            }  // if is_out
          }    // if is_extremum
        }
      }  // for jx
    }    // for ix
  }      // for i

  // Now filter points with the upper scale level
  kpts.reserve(kpts_aux.size());
  for (size_t i = 0; i < kpts_aux.size(); i++) {

    is_repeated = false;
    const AKAZEKeypoint& point = kpts_aux[i];
    for (size_t j = i + 1; j < kpts_aux.size(); j++) {

      // Compare response with the upper scale
      if ((point.class_id + 1) == kpts_aux[j].class_id) {
        dist = (point.pt - kpts_aux[j].pt).squaredNorm();

        if (dist <= point.size * point.size) {
          if (point.response < kpts_aux[j].response) {
            is_repeated = true;
            break;
          }
        }
      }
    }

    if (is_repeated == false) {
      kpts.push_back(point);
    }
  }

  timing_.extrema = timer.elapsedMs();
}

/* ************************************************************************* */
void AKAZE::Do_Subpixel_Refinement(std::vector<AKAZEKeypoint>& kpts) {
  float Dx = 0.0, Dy = 0.0, ratio = 0.0;
  float Dxx = 0.0, Dyy = 0.0, Dxy = 0.0;
  int x = 0, y = 0;
  Eigen::Matrix2f A;
  A.setZero();
  Eigen::Vector2f b(0.0, 0.0);
  Eigen::Vector2f dst(0.0, 0.0);

  timer::Timer timer;
  for (size_t i = 0; i < kpts.size(); i++) {
    ratio = pow(2.f, kpts[i].octave);
    x = fRound(kpts[i].pt.x() / ratio);
    y = fRound(kpts[i].pt.y() / ratio);

    // Compute the gradient
    Dx = 0.5 * (evolution_[kpts[i].class_id].Ldet(y, x + 1) -
                evolution_[kpts[i].class_id].Ldet(y, x - 1));
    Dy = 0.5 * (evolution_[kpts[i].class_id].Ldet(y + 1, x) -
                evolution_[kpts[i].class_id].Ldet(y - 1, x));

    // Compute the Hessian
    Dxx = evolution_[kpts[i].class_id].Ldet(y, x + 1) +
          evolution_[kpts[i].class_id].Ldet(y, x - 1) -
          2.0 * evolution_[kpts[i].class_id].Ldet(y, x);

    Dyy = evolution_[kpts[i].class_id].Ldet(y + 1, x) +
          evolution_[kpts[i].class_id].Ldet(y - 1, x) -
          2.0 * evolution_[kpts[i].class_id].Ldet(y, x);

    Dxy = 0.25 * (evolution_[kpts[i].class_id].Ldet(y + 1, x + 1) +
                  evolution_[kpts[i].class_id].Ldet(y - 1, x - 1)) -
          0.25 * (evolution_[kpts[i].class_id].Ldet(y - 1, x + 1) +
                  evolution_[kpts[i].class_id].Ldet(y + 1, +x - 1));

    // Solve the linear system
    A(0, 0) = Dxx;
    A(1, 1) = Dyy;
    A(0, 1) = A(1, 0) = Dxy;
    b(0) = -Dx;
    b(1) = -Dy;

    dst = A.fullPivLu().solve(b);

    if (fabs(dst(0)) <= 1.0 && fabs(dst(1)) <= 1.0) {
      kpts[i].pt.x() = x + dst(0);
      kpts[i].pt.y() = y + dst(1);
      int power = powf(2, evolution_[kpts[i].class_id].octave);
      kpts[i].pt.x() *= power;
      kpts[i].pt.y() *= power;
      kpts[i].angle = 0.0;

      // In OpenCV the size of a keypoint its the diameter
      kpts[i].size *= 2.0;
    }
    // Delete the point since its not stable
    else {
      kpts.erase(kpts.begin() + i);
      i--;
    }
  }
  timing_.subpixel = timer.elapsedMs();
}

/* ************************************************************************* */
/**
 * @brief This method computes the set of descriptors through the nonlinear
 * scale space
 * @param kpts Vector of detected keypoints
 * @param desc Matrix to store the descriptors
*/
void AKAZE::Compute_Descriptors(std::vector<AKAZEKeypoint>& kpts,
                                AKAZEDescriptors& desc) {
  // Allocate memory for the matrix with the descriptors
  if (options_.descriptor < MLDB_UPRIGHT) {
    desc.float_descriptor.resize(kpts.size());
  } else {
    desc.binary_descriptor.resize(kpts.size());
    // We use the full length binary descriptor -> 486 bits
    int t;
    if (options_.descriptor_size == 0) {
      t = (6 + 36 + 120) * options_.descriptor_channels;
    } else {
      t = options_.descriptor_size;
    }

    for (int i = 0; i < desc.binary_descriptor.size(); i++) {
      desc.binary_descriptor[i].resize(t);
    }
  }

  timer::Timer timer;

  switch (options_.descriptor) {
    case SURF_UPRIGHT:  // Upright descriptors, not invariant to rotation
    {

#ifdef AKAZE_USE_OPENMP
#pragma omp parallel for
#endif
      for (int i = 0; i < (int)(kpts.size()); i++) {
        Get_SURF_Descriptor_Upright_64(kpts[i], desc.float_descriptor[i]);
      }
    } break;
    case SURF: {
#ifdef AKAZE_USE_OPENMP
#pragma omp parallel for
#endif
      for (int i = 0; i < (int)(kpts.size()); i++) {
        Compute_Main_Orientation(kpts[i]);
        Get_SURF_Descriptor_64(kpts[i], desc.float_descriptor[i]);
      }
    } break;
    case MSURF_UPRIGHT:  // Upright descriptors, not invariant to rotation
    {
#ifdef AKAZE_USE_OPENMP
#pragma omp parallel for
#endif
      for (int i = 0; i < (int)(kpts.size()); i++) {
        Get_MSURF_Upright_Descriptor_64(kpts[i], desc.float_descriptor[i]);
      }
    } break;
    case MSURF: {
#ifdef AKAZE_USE_OPENMP
#pragma omp parallel for
#endif
      for (int i = 0; i < (int)(kpts.size()); i++) {
        Compute_Main_Orientation(kpts[i]);
        Get_MSURF_Descriptor_64(kpts[i], desc.float_descriptor[i]);
      }
    } break;
    case MLDB_UPRIGHT: {
#ifdef AKAZE_USE_OPENMP
#pragma omp parallel for
#endif
      for (int i = 0; i < (int)(kpts.size()); i++) {
        Get_Upright_MLDB_Full_Descriptor(
            kpts[i], (unsigned char*)(desc.binary_descriptor[i].data()));
      }
    } break;
    case MLDB: {
#ifdef AKAZE_USE_OPENMP
#pragma omp parallel for
#endif
      for (int i = 0; i < (int)(kpts.size()); i++) {
        Compute_Main_Orientation(kpts[i]);
        Get_MLDB_Full_Descriptor(
            kpts[i], (unsigned char*)(desc.binary_descriptor[i].data()));
      }
    } break;
  }

  timing_.descriptor = timer.elapsedMs();
}

/* ************************************************************************* */
void AKAZE::Compute_Main_Orientation(AKAZEKeypoint& kpt) const {
  int ix = 0, iy = 0, idx = 0, s = 0, level = 0;
  float xf = 0.0, yf = 0.0, gweight = 0.0, ratio = 0.0;
  float resX[109], resY[109], Ang[109];
  const int id[] = {6, 5, 4, 3, 2, 1, 0, 1, 2, 3, 4, 5, 6};

  // Variables for computing the dominant direction
  float sumX = 0.0, sumY = 0.0, max = 0.0, ang1 = 0.0, ang2 = 0.0;

  // Get the information from the keypoint
  level = kpt.class_id;
  ratio = (float)(1 << evolution_[level].octave);
  s = fRound(0.5 * kpt.size / ratio);
  xf = kpt.pt.x() / ratio;
  yf = kpt.pt.y() / ratio;

  // Calculate derivatives responses for points within radius of 6*scale
  for (int i = -6; i <= 6; ++i) {
    for (int j = -6; j <= 6; ++j) {
      if (i * i + j * j < 36) {
        iy = fRound(yf + j * s);
        ix = fRound(xf + i * s);

        gweight = gauss25[id[i + 6]][id[j + 6]];
        resX[idx] = gweight * evolution_[level].Lx(iy, ix);
        resY[idx] = gweight * evolution_[level].Ly(iy, ix);
        Ang[idx] = atan2(resY[idx], resX[idx]) * (M_PI / 180.0);
        ++idx;
      }
    }
  }

  // Loop slides pi/3 window around feature point
  for (ang1 = 0; ang1 < 2.0 * M_PI; ang1 += 0.15f) {
    ang2 = (ang1 + M_PI / 3.0f > 2.0 * M_PI ? ang1 - 5.0f * M_PI / 3.0f
                                            : ang1 + M_PI / 3.0f);
    sumX = sumY = 0.f;

    for (size_t k = 0; k < 109; ++k) {
      // Get angle from the x-axis of the sample point
      const float& ang = Ang[k];

      // Determine whether the point is within the window
      if (ang1 < ang2 && ang1 < ang && ang < ang2) {
        sumX += resX[k];
        sumY += resY[k];
      } else if (ang2 < ang1 && ((ang > 0 && ang < ang2) ||
                                 (ang > ang1 && ang < 2.0 * M_PI))) {
        sumX += resX[k];
        sumY += resY[k];
      }
    }

    // if the vector produced from this window is longer than all
    // previous vectors then this forms the new dominant direction
    if (sumX * sumX + sumY * sumY > max) {
      // store largest orientation
      max = sumX * sumX + sumY * sumY;
      kpt.angle = atan2(sumY, sumX) * (M_PI / 180.0);
    }
  }
}

/* ************************************************************************* */
void AKAZE::Get_SURF_Descriptor_Upright_64(const AKAZEKeypoint& kpt,
                                           Eigen::VectorXf& desc) const {
  desc.setZero(64);
  float dx = 0.0, dy = 0.0, mdx = 0.0, mdy = 0.0;
  float rx = 0.0, ry = 0.0, len = 0.0, xf = 0.0, yf = 0.0;
  float sample_x = 0.0, sample_y = 0.0;
  float fx = 0.0, fy = 0.0, ratio = 0.0, res1 = 0.0, res2 = 0.0, res3 = 0.0,
        res4 = 0.0;
  int x1 = 0, y1 = 0, x2 = 0, y2 = 0, sample_step = 0, pattern_size = 0,
      dcount = 0;
  int scale = 0, dsize = 0, level = 0;

  // Set the descriptor size and the sample and pattern sizes
  dsize = 64;
  sample_step = 5;
  pattern_size = 10;

  // Get the information from the keypoint
  ratio = (float)(1 << kpt.octave);
  scale = fRound(0.5 * kpt.size / ratio);
  level = kpt.class_id;
  yf = kpt.pt.y() / ratio;
  xf = kpt.pt.x() / ratio;

  // Calculate descriptor for this interest point
  for (int i = -pattern_size; i < pattern_size; i += sample_step) {
    for (int j = -pattern_size; j < pattern_size; j += sample_step) {

      dx = dy = mdx = mdy = 0.0;

      for (int k = i; k < i + sample_step; k++) {
        for (int l = j; l < j + sample_step; l++) {

          // Get the coordinates of the sample point on the rotated axis
          sample_y = yf + l * scale;
          sample_x = xf + k * scale;

          y1 = (int)(sample_y - .5);
          x1 = (int)(sample_x - .5);

          y2 = (int)(sample_y + .5);
          x2 = (int)(sample_x + .5);

          fx = sample_x - x1;
          fy = sample_y - y1;

          res1 = evolution_[level].Lx(y1, x1);
          res2 = evolution_[level].Lx(y1, x2);
          res3 = evolution_[level].Lx(y2, x1);
          res4 = evolution_[level].Lx(y2, x2);
          rx = (1.0 - fx) * (1.0 - fy) * res1 + fx * (1.0 - fy) * res2 +
               (1.0 - fx) * fy * res3 + fx * fy * res4;

          res1 = evolution_[level].Ly(y1, x1);
          res2 = evolution_[level].Ly(y1, x2);
          res3 = evolution_[level].Ly(y2, x1);
          res4 = evolution_[level].Ly(y2, x2);
          ry = (1.0 - fx) * (1.0 - fy) * res1 + fx * (1.0 - fy) * res2 +
               (1.0 - fx) * fy * res3 + fx * fy * res4;

          // Sum the derivatives to the cumulative descriptor
          dx += rx;
          dy += ry;
          mdx += fabs(rx);
          mdy += fabs(ry);
        }
      }

      // Add the values to the descriptor vector
      desc(dcount++) = dx;
      desc(dcount++) = dy;
      desc(dcount++) = mdx;
      desc(dcount++) = mdy;

      // Store the current length^2 of the vector
      len += dx * dx + dy * dy + mdx * mdx + mdy * mdy;
    }
  }

  // convert to unit vector
  len = sqrt(len);

  for (int i = 0; i < dsize; i++) {
    desc[i] /= len;
  }
}

/* ************************************************************************* */
void AKAZE::Get_SURF_Descriptor_64(const AKAZEKeypoint& kpt,
                                   Eigen::VectorXf& desc) const {
  desc.setZero(64);
  float dx = 0.0, dy = 0.0, mdx = 0.0, mdy = 0.0;
  float rx = 0.0, ry = 0.0, rrx = 0.0, rry = 0.0, len = 0.0, xf = 0.0, yf = 0.0;
  float sample_x = 0.0, sample_y = 0.0, co = 0.0, si = 0.0, angle = 0.0;
  float fx = 0.0, fy = 0.0, ratio = 0.0, res1 = 0.0, res2 = 0.0, res3 = 0.0,
        res4 = 0.0;
  int x1 = 0, y1 = 0, x2 = 0, y2 = 0, sample_step = 0, pattern_size = 0,
      dcount = 0;
  int scale = 0, dsize = 0, level = 0;

  // Set the descriptor size and the sample and pattern sizes
  dsize = 64;
  sample_step = 5;
  pattern_size = 10;

  // Get the information from the keypoint
  ratio = (float)(1 << kpt.octave);
  scale = fRound(0.5 * kpt.size / ratio);
  angle = kpt.angle;
  level = kpt.class_id;
  yf = kpt.pt.y() / ratio;
  xf = kpt.pt.x() / ratio;
  co = cos(angle);
  si = sin(angle);

  // Calculate descriptor for this interest point
  for (int i = -pattern_size; i < pattern_size; i += sample_step) {
    for (int j = -pattern_size; j < pattern_size; j += sample_step) {

      dx = dy = mdx = mdy = 0.0;

      for (int k = i; k < i + sample_step; k++) {
        for (int l = j; l < j + sample_step; l++) {

          // Get the coordinates of the sample point on the rotated axis
          sample_y = yf + (l * scale * co + k * scale * si);
          sample_x = xf + (-l * scale * si + k * scale * co);

          y1 = (int)(sample_y - .5);
          x1 = (int)(sample_x - .5);

          y2 = (int)(sample_y + .5);
          x2 = (int)(sample_x + .5);

          fx = sample_x - x1;
          fy = sample_y - y1;

          res1 = evolution_[level].Lx(y1, x1);
          res2 = evolution_[level].Lx(y1, x2);
          res3 = evolution_[level].Lx(y2, x1);
          res4 = evolution_[level].Lx(y2, x2);
          rx = (1.0 - fx) * (1.0 - fy) * res1 + fx * (1.0 - fy) * res2 +
               (1.0 - fx) * fy * res3 + fx * fy * res4;

          res1 = evolution_[level].Ly(y1, x1);
          res2 = evolution_[level].Ly(y1, x2);
          res3 = evolution_[level].Ly(y2, x1);
          res4 = evolution_[level].Ly(y2, x2);
          ry = (1.0 - fx) * (1.0 - fy) * res1 + fx * (1.0 - fy) * res2 +
               (1.0 - fx) * fy * res3 + fx * fy * res4;

          // Get the x and y derivatives on the rotated axis
          rry = rx * co + ry * si;
          rrx = -rx * si + ry * co;

          // Sum the derivatives to the cumulative descriptor
          dx += rrx;
          dy += rry;
          mdx += fabs(rrx);
          mdy += fabs(rry);
        }
      }

      // Add the values to the descriptor vector
      desc(dcount++) = dx;
      desc(dcount++) = dy;
      desc(dcount++) = mdx;
      desc(dcount++) = mdy;

      // Store the current length^2 of the vector
      len += dx * dx + dy * dy + mdx * mdx + mdy * mdy;
    }
  }

  // convert to unit vector
  len = sqrt(len);

  for (int i = 0; i < dsize; i++) desc[i] /= len;
}

/* ************************************************************************* */
void AKAZE::Get_MSURF_Upright_Descriptor_64(const AKAZEKeypoint& kpt,
                                            Eigen::VectorXf& desc) const {
  desc.setZero(64);
  float dx = 0.0, dy = 0.0, mdx = 0.0, mdy = 0.0, gauss_s1 = 0.0,
        gauss_s2 = 0.0;
  float rx = 0.0, ry = 0.0, len = 0.0, xf = 0.0, yf = 0.0, ys = 0.0, xs = 0.0;
  float sample_x = 0.0, sample_y = 0.0;
  int x1 = 0, y1 = 0, sample_step = 0, pattern_size = 0;
  int x2 = 0, y2 = 0, kx = 0, ky = 0, i = 0, j = 0, dcount = 0;
  float fx = 0.0, fy = 0.0, ratio = 0.0, res1 = 0.0, res2 = 0.0, res3 = 0.0,
        res4 = 0.0;
  int scale = 0, dsize = 0, level = 0;

  // Subregion centers for the 4x4 gaussian weighting
  float cx = -0.5, cy = 0.5;

  // Set the descriptor size and the sample and pattern sizes
  dsize = 64;
  sample_step = 5;
  pattern_size = 12;

  // Get the information from the keypoint
  ratio = (float)(1 << kpt.octave);
  scale = fRound(0.5 * kpt.size / ratio);
  level = kpt.class_id;
  yf = kpt.pt.y() / ratio;
  xf = kpt.pt.x() / ratio;

  i = -8;

  // Calculate descriptor for this interest point
  // Area of size 24 s x 24 s
  while (i < pattern_size) {
    j = -8;
    i = i - 4;

    cx += 1.0;
    cy = -0.5;

    while (j < pattern_size) {
      dx = dy = mdx = mdy = 0.0;
      cy += 1.0;
      j = j - 4;

      ky = i + sample_step;
      kx = j + sample_step;

      ys = yf + (ky * scale);
      xs = xf + (kx * scale);

      for (int k = i; k < i + 9; k++) {
        for (int l = j; l < j + 9; l++) {
          sample_y = k * scale + yf;
          sample_x = l * scale + xf;

          // Get the gaussian weighted x and y responses
          gauss_s1 = gaussian(xs - sample_x, ys - sample_y, 2.50 * scale);

          y1 = (int)(sample_y - .5);
          x1 = (int)(sample_x - .5);

          y2 = (int)(sample_y + .5);
          x2 = (int)(sample_x + .5);

          fx = sample_x - x1;
          fy = sample_y - y1;

          res1 = evolution_[level].Lx(y1, x1);
          res2 = evolution_[level].Lx(y1, x2);
          res3 = evolution_[level].Lx(y2, x1);
          res4 = evolution_[level].Lx(y2, x2);
          rx = (1.0 - fx) * (1.0 - fy) * res1 + fx * (1.0 - fy) * res2 +
               (1.0 - fx) * fy * res3 + fx * fy * res4;

          res1 = evolution_[level].Ly(y1, x1);
          res2 = evolution_[level].Ly(y1, x2);
          res3 = evolution_[level].Ly(y2, x1);
          res4 = evolution_[level].Ly(y2, x2);
          ry = (1.0 - fx) * (1.0 - fy) * res1 + fx * (1.0 - fy) * res2 +
               (1.0 - fx) * fy * res3 + fx * fy * res4;

          rx = gauss_s1 * rx;
          ry = gauss_s1 * ry;

          // Sum the derivatives to the cumulative descriptor
          dx += rx;
          dy += ry;
          mdx += fabs(rx);
          mdy += fabs(ry);
        }
      }

      // Add the values to the descriptor vector
      gauss_s2 = gaussian(cx - 2.0f, cy - 2.0f, 1.5f);

      desc(dcount++) = dx * gauss_s2;
      desc(dcount++) = dy * gauss_s2;
      desc(dcount++) = mdx * gauss_s2;
      desc(dcount++) = mdy * gauss_s2;

      len += (dx * dx + dy * dy + mdx * mdx + mdy * mdy) * gauss_s2 * gauss_s2;

      j += 9;
    }

    i += 9;
  }

  // convert to unit vector
  len = sqrt(len);

  for (int i = 0; i < dsize; i++) desc[i] /= len;
}

/* ************************************************************************* */
void AKAZE::Get_MSURF_Descriptor_64(const AKAZEKeypoint& kpt,
                                    Eigen::VectorXf& desc) const {
  desc.setZero(64);
  float dx = 0.0, dy = 0.0, mdx = 0.0, mdy = 0.0, gauss_s1 = 0.0,
        gauss_s2 = 0.0;
  float rx = 0.0, ry = 0.0, rrx = 0.0, rry = 0.0, len = 0.0, xf = 0.0, yf = 0.0,
        ys = 0.0, xs = 0.0;
  float sample_x = 0.0, sample_y = 0.0, co = 0.0, si = 0.0, angle = 0.0;
  float fx = 0.0, fy = 0.0, ratio = 0.0, res1 = 0.0, res2 = 0.0, res3 = 0.0,
        res4 = 0.0;
  int x1 = 0, y1 = 0, x2 = 0, y2 = 0, sample_step = 0, pattern_size = 0;
  int kx = 0, ky = 0, i = 0, j = 0, dcount = 0;
  int scale = 0, dsize = 0, level = 0;

  // Subregion centers for the 4x4 gaussian weighting
  float cx = -0.5, cy = 0.5;

  // Set the descriptor size and the sample and pattern sizes
  dsize = 64;
  sample_step = 5;
  pattern_size = 12;

  // Get the information from the keypoint
  ratio = (float)(1 << kpt.octave);
  scale = fRound(0.5 * kpt.size / ratio);
  angle = kpt.angle;
  level = kpt.class_id;
  yf = kpt.pt.y() / ratio;
  xf = kpt.pt.x() / ratio;
  co = cos(angle);
  si = sin(angle);

  i = -8;

  // Calculate descriptor for this interest point
  // Area of size 24 s x 24 s
  while (i < pattern_size) {
    j = -8;
    i = i - 4;

    cx += 1.0;
    cy = -0.5;

    while (j < pattern_size) {
      dx = dy = mdx = mdy = 0.0;
      cy += 1.0;
      j = j - 4;

      ky = i + sample_step;
      kx = j + sample_step;

      xs = xf + (-kx * scale * si + ky * scale * co);
      ys = yf + (kx * scale * co + ky * scale * si);

      for (int k = i; k < i + 9; ++k) {
        for (int l = j; l < j + 9; ++l) {
          // Get coords of sample point on the rotated axis
          sample_y = yf + (l * scale * co + k * scale * si);
          sample_x = xf + (-l * scale * si + k * scale * co);

          // Get the gaussian weighted x and y responses
          gauss_s1 = gaussian(xs - sample_x, ys - sample_y, 2.5 * scale);

          y1 = fRound(sample_y - .5);
          x1 = fRound(sample_x - .5);

          y2 = fRound(sample_y + .5);
          x2 = fRound(sample_x + .5);

          fx = sample_x - x1;
          fy = sample_y - y1;

          res1 = evolution_[level].Lx(y1, x1);
          res2 = evolution_[level].Lx(y1, x2);
          res3 = evolution_[level].Lx(y2, x1);
          res4 = evolution_[level].Lx(y2, x2);
          rx = (1.0 - fx) * (1.0 - fy) * res1 + fx * (1.0 - fy) * res2 +
               (1.0 - fx) * fy * res3 + fx * fy * res4;

          res1 = evolution_[level].Ly(y1, x1);
          res2 = evolution_[level].Ly(y1, x2);
          res3 = evolution_[level].Ly(y2, x1);
          res4 = evolution_[level].Ly(y2, x2);
          ry = (1.0 - fx) * (1.0 - fy) * res1 + fx * (1.0 - fy) * res2 +
               (1.0 - fx) * fy * res3 + fx * fy * res4;

          // Get the x and y derivatives on the rotated axis
          rry = gauss_s1 * (rx * co + ry * si);
          rrx = gauss_s1 * (-rx * si + ry * co);

          // Sum the derivatives to the cumulative descriptor
          dx += rrx;
          dy += rry;
          mdx += fabs(rrx);
          mdy += fabs(rry);
        }
      }

      // Add the values to the descriptor vector
      gauss_s2 = gaussian(cx - 2.0f, cy - 2.0f, 1.5f);
      desc(dcount++) = dx * gauss_s2;
      desc(dcount++) = dy * gauss_s2;
      desc(dcount++) = mdx * gauss_s2;
      desc(dcount++) = mdy * gauss_s2;

      len += (dx * dx + dy * dy + mdx * mdx + mdy * mdy) * gauss_s2 * gauss_s2;

      j += 9;
    }

    i += 9;
  }

  // convert to unit vector
  len = sqrt(len);

  desc /= len;
}

/* ************************************************************************* */
void AKAZE::Get_MLDB_Full_Descriptor(const AKAZEKeypoint& kpt,
                                     unsigned char* desc) const {

  const int max_channels = 3;
  assert(options_.descriptor_channels <= max_channels);
  float values[16 * max_channels];
  const double size_mult[3] = {1, 2.0 / 3.0, 1.0 / 2.0};

  float ratio = (float)(1 << kpt.octave);
  float scale = (float)fRound(0.5f * kpt.size / ratio);
  float xf = kpt.pt.x() / ratio;
  float yf = kpt.pt.y() / ratio;
  float co = cos(kpt.angle);
  float si = sin(kpt.angle);
  int pattern_size = options_.descriptor_pattern_size;

  int dpos = 0;
  for (int lvl = 0; lvl < 3; lvl++) {
    int val_count = (lvl + 2) * (lvl + 2);
    int sample_step = static_cast<int>(ceil(pattern_size * size_mult[lvl]));
    MLDB_Fill_Values(values, sample_step, kpt.class_id, xf, yf, co, si, scale);
    MLDB_Binary_Comparisons(values, desc, val_count, dpos);
  }
}

/* ************************************************************************* */
void AKAZE::Get_Upright_MLDB_Full_Descriptor(const AKAZEKeypoint& kpt,
                                             unsigned char* desc) const {

  const int max_channels = 3;
  assert(options_.descriptor_channels <= max_channels);
  float values[16 * max_channels];
  const double size_mult[3] = {1, 2.0 / 3.0, 1.0 / 2.0};

  float ratio = (float)(1 << kpt.octave);
  float scale = (float)fRound(0.5f * kpt.size / ratio);
  float xf = kpt.pt.x() / ratio;
  float yf = kpt.pt.y() / ratio;
  int pattern_size = options_.descriptor_pattern_size;

  int dpos = 0;
  for (int lvl = 0; lvl < 3; lvl++) {
    int val_count = (lvl + 2) * (lvl + 2);
    int sample_step = static_cast<int>(ceil(pattern_size * size_mult[lvl]));
    MLDB_Fill_Upright_Values(values, sample_step, kpt.class_id, xf, yf, scale);
    MLDB_Binary_Comparisons(values, desc, val_count, dpos);
  }
}

/* ************************************************************************* */
void AKAZE::MLDB_Fill_Values(float* values, int sample_step, int level,
                             float xf, float yf, float co, float si,
                             float scale) const {

  int pattern_size = options_.descriptor_pattern_size;
  int nr_channels = options_.descriptor_channels;
  int valpos = 0;

  for (int i = -pattern_size; i < pattern_size; i += sample_step) {
    for (int j = -pattern_size; j < pattern_size; j += sample_step) {

      float di = 0.0, dx = 0.0, dy = 0.0;
      int nsamples = 0;

      for (int k = i; k < i + sample_step; k++) {
        for (int l = j; l < j + sample_step; l++) {

          float sample_y = yf + (l * co * scale + k * si * scale);
          float sample_x = xf + (-l * si * scale + k * co * scale);

          int y1 = fRound(sample_y);
          int x1 = fRound(sample_x);

          float ri = evolution_[level].Lt(y1, x1);
          di += ri;

          if (nr_channels > 1) {
            float rx = evolution_[level].Lx(y1, x1);
            float ry = evolution_[level].Ly(y1, x1);
            if (nr_channels == 2) {
              dx += sqrtf(rx * rx + ry * ry);
            } else {
              float rry = rx * co + ry * si;
              float rrx = -rx * si + ry * co;
              dx += rrx;
              dy += rry;
            }
          }
          nsamples++;
        }
      }

      di /= nsamples;
      dx /= nsamples;
      dy /= nsamples;

      values[valpos] = di;

      if (nr_channels > 1) values[valpos + 1] = dx;

      if (nr_channels > 2) values[valpos + 2] = dy;

      valpos += nr_channels;
    }
  }
}

/* ************************************************************************* */
void AKAZE::MLDB_Fill_Upright_Values(float* values, int sample_step, int level,
                                     float xf, float yf, float scale) const {

  int pattern_size = options_.descriptor_pattern_size;
  int nr_channels = options_.descriptor_channels;
  int valpos = 0;

  for (int i = -pattern_size; i < pattern_size; i += sample_step) {
    for (int j = -pattern_size; j < pattern_size; j += sample_step) {

      float di = 0.0, dx = 0.0, dy = 0.0;
      int nsamples = 0;

      for (int k = i; k < i + sample_step; k++) {
        for (int l = j; l < j + sample_step; l++) {

          float sample_y = yf + l * scale;
          float sample_x = xf + k * scale;

          int y1 = fRound(sample_y);
          int x1 = fRound(sample_x);

          float ri = evolution_[level].Lt(y1, x1);
          di += ri;

          if (nr_channels > 1) {
            float rx = evolution_[level].Lx(y1, x1);
            float ry = evolution_[level].Ly(y1, x1);
            if (nr_channels == 2) {
              dx += sqrtf(rx * rx + ry * ry);
            } else {
              dx += rx;
              dy += ry;
            }
          }
          nsamples++;
        }
      }

      di /= nsamples;
      dx /= nsamples;
      dy /= nsamples;

      values[valpos] = di;

      if (nr_channels > 1) values[valpos + 1] = dx;

      if (nr_channels > 2) values[valpos + 2] = dy;

      valpos += nr_channels;
    }
  }
}

/* ************************************************************************* */
void AKAZE::MLDB_Binary_Comparisons(float* values, unsigned char* desc,
                                    int count, int& dpos) const {
#define TOGGLE_FLT(x) ((x) ^ ((int)(x) < 0 ? 0x7fffffff : 0))

  int nr_channels = options_.descriptor_channels;
  int* ivalues = (int*)values;
  for (int i = 0; i < count * nr_channels; i++) {
    ivalues[i] = TOGGLE_FLT(ivalues[i]);
  }
#undef TOGGLE_FLT

  for (int pos = 0; pos < nr_channels; pos++) {
    for (int i = 0; i < count; i++) {
      int ival = ivalues[nr_channels * i + pos];
      for (int j = i + 1; j < count; j++) {
        int res = ival > ivalues[nr_channels * j + pos];
        desc[dpos >> 3] |= (res << (dpos & 7));
        dpos++;
      }
    }
  }
}

/* ************************************************************************* */
void AKAZE::Get_MLDB_Descriptor_Subset(const AKAZEKeypoint& kpt,
                                       unsigned char* desc) {
  float di = 0.f, dx = 0.f, dy = 0.f;
  float rx = 0.f, ry = 0.f;
  float sample_x = 0.f, sample_y = 0.f;
  int x1 = 0, y1 = 0;

  // Get the information from the keypoint
  float ratio = (float)(1 << kpt.octave);
  int scale = fRound(0.5 * kpt.size / ratio);
  float angle = kpt.angle;
  float level = kpt.class_id;
  float yf = kpt.pt.y() / ratio;
  float xf = kpt.pt.x() / ratio;
  float co = cos(angle);
  float si = sin(angle);

  // Allocate memory for the matrix of values
  Eigen::VectorXf values((4 + 9 + 16) * options_.descriptor_channels);

  // Sample everything, but only do the comparisons
  std::vector<int> steps(3);
  steps.at(0) = options_.descriptor_pattern_size;
  steps.at(1) = ceil(2.f * options_.descriptor_pattern_size / 3.f);
  steps.at(2) = options_.descriptor_pattern_size / 2;

  for (int i = 0; i < descriptorSamples_.rows(); i++) {
    const Eigen::RowVectorXi& coords = descriptorSamples_.row(i);
    int sample_step = steps.at(coords(0));
    di = 0.0f;
    dx = 0.0f;
    dy = 0.0f;

    for (int k = coords(1); k < coords(1) + sample_step; k++) {
      for (int l = coords(2); l < coords(2) + sample_step; l++) {

        // Get the coordinates of the sample point
        sample_y = yf + (l * scale * co + k * scale * si);
        sample_x = xf + (-l * scale * si + k * scale * co);

        y1 = fRound(sample_y);
        x1 = fRound(sample_x);

        di += evolution_[level].Lt(y1, x1);

        if (options_.descriptor_channels > 1) {
          rx = evolution_[level].Lx(y1, x1);
          ry = evolution_[level].Ly(y1, x1);

          if (options_.descriptor_channels == 2) {
           dx += sqrtf(rx * rx + ry * ry);
          } else if (options_.descriptor_channels == 3) {
            // Get the x and y derivatives on the rotated axis
            dx += rx * co + ry * si;
            dy += -rx * si + ry * co;
          }
        }
      }
    }

    values(options_.descriptor_channels * i) = di;

    if (options_.descriptor_channels == 2) {
      values(options_.descriptor_channels * i + 1) = dx;
    } else if (options_.descriptor_channels == 3) {
      values(options_.descriptor_channels * i + 1) = dx;
      values(options_.descriptor_channels * i + 2) = dy;
    }
  }

  // Do the comparisons
  const float* vals = values.data();
  const int* comps = descriptorBits_.data();

  for (int i = 0; i < descriptorBits_.rows(); i++) {
    if (vals[comps[2 * i]] > vals[comps[2 * i + 1]]) {
      desc[i / 8] |= (1 << (i % 8));
    }
  }
}

/* ************************************************************************* */
void AKAZE::Get_Upright_MLDB_Descriptor_Subset(const AKAZEKeypoint& kpt,
                                               unsigned char* desc) {

  float di = 0.0f, dx = 0.0f, dy = 0.0f;
  float rx = 0.0f, ry = 0.0f;
  float sample_x = 0.0f, sample_y = 0.0f;
  int x1 = 0, y1 = 0;

  // Get the information from the keypoint
  float ratio = (float)(1 << kpt.octave);
  int scale = fRound(0.5 * kpt.size / ratio);
  float level = kpt.class_id;
  float yf = kpt.pt.y() / ratio;
  float xf = kpt.pt.x() / ratio;

  // Allocate memory for the matrix of values
  Eigen::VectorXf values((4 + 9 + 16) * options_.descriptor_channels);

  std::vector<int> steps(3);
  steps.at(0) = options_.descriptor_pattern_size;
  steps.at(1) = ceil(2.f * options_.descriptor_pattern_size / 3.f);
  steps.at(2) = options_.descriptor_pattern_size / 2;

  for (int i = 0; i < descriptorSamples_.rows(); i++) {
    const Eigen::RowVectorXi coords = descriptorSamples_.row(i);
    int sample_step = steps.at(coords[0]);
    di = 0.0f, dx = 0.0f, dy = 0.0f;

    for (int k = coords[1]; k < coords[1] + sample_step; k++) {
      for (int l = coords[2]; l < coords[2] + sample_step; l++) {

        // Get the coordinates of the sample point
        sample_y = yf + l * scale;
        sample_x = xf + k * scale;

        y1 = fRound(sample_y);
        x1 = fRound(sample_x);
        di += evolution_[level].Lt(y1, x1);

        if (options_.descriptor_channels > 1) {
          rx = evolution_[level].Lx(y1, x1);
          ry = evolution_[level].Ly(y1, x1);

          if (options_.descriptor_channels == 2) {
            dx += sqrtf(rx * rx + ry * ry);
          } else if (options_.descriptor_channels == 3) {
            dx += rx;
            dy += ry;
          }
        }
      }
    }

    values(options_.descriptor_channels * i) = di;

    if (options_.descriptor_channels == 2) {
      values(options_.descriptor_channels * i + 1) = dx;
    } else if (options_.descriptor_channels == 3) {
      values(options_.descriptor_channels * i + 1) = dx;
      values(options_.descriptor_channels * i + 2) = dy;
    }
  }

  // Do the comparisons
  const float* vals = values.data();
  const int* comps = descriptorBits_.data();
  for (int i = 0; i < descriptorBits_.rows(); i++) {
    if (vals[comps[2 * i]] > vals[comps[2 * i + 1]]) {
      desc[i / 8] |= (1 << (i % 8));
    }
  }
}

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

// This method saves the scale space into jpg images
void AKAZE::Save_Scale_Space() {
  std::string outputFile;
  cimg_library::CImg<float> img_aux;
  for (size_t i = 0; i < evolution_.size(); i++) {
    ConvertEigenToCImg(evolution_[i].Lt, img_aux);
    outputFile = "../output/evolution_" + to_formatted_string(i, 2) + ".jpg";
    img_aux.save(outputFile.c_str());
  }
}

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

// This method saves the feature detector responses of the nonlinear scale
// space into jpg images
void AKAZE::Save_Detector_Responses() {
  cimg_library::CImg<float> img_aux;
  std::string outputFile;
  float ttime = 0.0;
  int nimgs = 0;

  for (size_t i = 0; i < evolution_.size(); i++) {
    ttime = evolution_[i + 1].etime - evolution_[i].etime;
    if (ttime > 0) {
      ConvertEigenToCImg(evolution_[i].Ldet, img_aux);
      outputFile =
          "../output/images/detector_" + to_formatted_string(nimgs, 2) + ".jpg";
      img_aux.save(outputFile.c_str());
      nimgs++;
    }
  }
}

/* ************************************************************************* */
void AKAZE::Show_Computation_Times() const {
  std::cout << "(*) Time Scale Space: " << timing_.scale << std::endl;
  std::cout << "(*) Time Detector: " << timing_.detector << std::endl;
  std::cout << "   - Time Derivatives: " << timing_.derivatives << std::endl;
  std::cout << "   - Time Extrema: " << timing_.extrema << std::endl;
  std::cout << "   - Time Subpixel: " << timing_.subpixel << std::endl;
  std::cout << "(*) Time Descriptor: " << timing_.descriptor << std::endl;
  std::cout << std::endl;
}

/* ************************************************************************* */
void libAKAZE::generateDescriptorSubsample(Eigen::MatrixXi& sampleList,
                                           Eigen::MatrixXi& comparisons, int nbits,
                                           int pattern_size, int nchannels) {
  int ssz = 0;
  for (int i = 0; i < 3; i++) {
    int gz = (i + 2) * (i + 2);
    ssz += gz * (gz - 1) / 2;
  }
  ssz *= nchannels;

  assert(nbits <= ssz &&
         "descriptor size can't be bigger than full descriptor");

  // Since the full descriptor is usually under 10k elements, we pick
  // the selection from the full matrix.  We take as many samples per
  // pick as the number of channels. For every pick, we
  // take the two samples involved and put them in the sampling list
  Eigen::MatrixXi fullM(ssz / nchannels, 5);
  for (size_t i = 0, c = 0; i < 3; i++) {
    const int gdiv = i + 2;  // grid divisions, per row
    const int gsz = gdiv * gdiv;
    const int psz = ceil(2. * pattern_size / (float)gdiv);

    for (int j = 0; j < gsz; j++) {
      for (int k = j + 1; k < gsz; k++, c++) {
        fullM(c, 0) = i;
        fullM(c, 1) = psz * (j % gdiv) - pattern_size;
        fullM(c, 2) = psz * (j / gdiv) - pattern_size;
        fullM(c, 3) = psz * (k % gdiv) - pattern_size;
        fullM(c, 4) = psz * (k / gdiv) - pattern_size;
      }
    }
  }

  srand(1024);
  Eigen::MatrixXi comps(
      nchannels * static_cast<int>(ceil(nbits / (float)nchannels)), 2);
  comps.setConstant(1000);

  // Select some samples. A sample includes all channels
  int count = 0;
  size_t npicks = ceil(nbits / (float)nchannels);
  Eigen::MatrixXi samples(29, 3);
  Eigen::MatrixXi fullcopy = fullM;
  samples.setConstant(-1);

  for (size_t i = 0; i < npicks; i++) {
    size_t k = rand() % (fullM.rows() - i);
    if (i < 6) {
      // Force use of the coarser grid values and comparisons
      k = i;
    }

    bool n = true;

    for (int j = 0; j < count; j++) {
      if (samples(j, 0) == fullcopy(k, 0) && samples(j, 1) == fullcopy(k, 1) &&
          samples(j, 2) == fullcopy(k, 2)) {
        n = false;
        comps(i * nchannels, 0) = nchannels * j;
        comps(i * nchannels + 1, 0) = nchannels * j + 1;
        comps(i * nchannels + 2, 0) = nchannels * j + 2;
        break;
      }
    }

    if (n) {
      samples(count, 0) = fullcopy(k, 0);
      samples(count, 1) = fullcopy(k, 1);
      samples(count, 2) = fullcopy(k, 2);
      comps(i * nchannels, 0) = nchannels * count;
      comps(i * nchannels + 1, 0) = nchannels * count + 1;
      comps(i * nchannels + 2, 0) = nchannels * count + 2;
      count++;
    }

    n = true;
    for (int j = 0; j < count; j++) {
      if (samples(j, 0) == fullcopy(k, 0) && samples(j, 1) == fullcopy(k, 3) &&
          samples(j, 2) == fullcopy(k, 4)) {
        n = false;
        comps(i * nchannels, 1) = nchannels * j;
        comps(i * nchannels + 1, 1) = nchannels * j + 1;
        comps(i * nchannels + 2, 1) = nchannels * j + 2;
        break;
      }
    }

    if (n) {
      samples(count, 0) = fullcopy(k, 0);
      samples(count, 1) = fullcopy(k, 3);
      samples(count, 2) = fullcopy(k, 4);
      comps(i * nchannels, 1) = nchannels * count;
      comps(i * nchannels + 1, 1) = nchannels * count + 1;
      comps(i * nchannels + 2, 1) = nchannels * count + 2;
      count++;
    }

    fullcopy.row(k) = fullcopy.row(fullcopy.rows() - i - 1);
  }

  sampleList = samples.topRows(count);
  comparisons = comps.topRows(nbits);
}

/* ************************************************************************* */
void libAKAZE::check_descriptor_limits(int& x, int& y, int width, int height) {

  if (x < 0) x = 0;

  if (y < 0) y = 0;

  if (x > width - 1) x = width - 1;

  if (y > height - 1) y = height - 1;
}


#include <boost/numeric/interval/ext/integer.hpp>


//  boost/chrono/stopwatches/formatters/ accumulator_set_formatter.hpp  ------------------------------------------------------------//
//  Copyright 2011 Vicente J. Botet Escriba
//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt
//  See http://www.boost.org/libs/chrono/stopwatches for documentation.

#ifndef BOOST_CHRONO_STOPWATCHES_FORMATTERS_ACCUMULATOR_SET_HPP
#define BOOST_CHRONO_STOPWATCHES_FORMATTERS_ACCUMULATOR_SET_HPP

#include <boost/chrono/stopwatches/formatters/base_formatter.hpp>
#include <boost/chrono/chrono_io.hpp>
#include <boost/current_function.hpp>
#include <boost/accumulators/framework/accumulator_set.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/format.hpp>
#include <boost/format/group.hpp>
#include <boost/cstdint.hpp>
#include <boost/assert.hpp>
#include <string>
#include <iostream>
#include <cassert>
#include <iomanip>

#define BOOST_CHRONO_STOPWATCHES_ACCUMULATOR_SET_FORMAT_DEFAULT "count=%1%, sum=%2%, min=%3%, max=%4%, mean=%5%\n"

namespace boost
{
  namespace chrono
  {

    template<typename Ratio = milli, typename CharT = char,
        typename Traits = std::char_traits<CharT>,
        class Alloc = std::allocator<CharT> >
    class basic_accumulator_set_formatter: public base_formatter<CharT, Traits> , public basic_format<
        CharT, Traits>
    {

    public:
      typedef base_formatter<CharT, Traits> base_type;
      typedef basic_format<CharT, Traits> format_type;
      typedef std::basic_string<CharT, Traits, Alloc> string_type;
      typedef CharT char_type;
      typedef std::basic_ostream<CharT, Traits> ostream_type;

      basic_accumulator_set_formatter() :
            base_type(),
            format_type(BOOST_CHRONO_STOPWATCHES_ACCUMULATOR_SET_FORMAT_DEFAULT)
      {
      }
      basic_accumulator_set_formatter(ostream_type& os) :
            base_type(os),
            format_type(BOOST_CHRONO_STOPWATCHES_ACCUMULATOR_SET_FORMAT_DEFAULT)
      {
      }
      basic_accumulator_set_formatter(const char* fmt, ostream_type& os =
          std::cout) :
        base_type(os), format_type(fmt)
      {
      }
      basic_accumulator_set_formatter(string_type const& fmt, ostream_type& os =
          std::cout) :
        base_type(os), format_type(fmt)
      {
      }

      //      static string_type format(const char* s)
      //      {
      //        string_type res(s);
      //        res += boost::chrono::detail::adaptive_string(" : ");
      //        res += BOOST_CHRONO_STOPWATCHES_ACCUMULATOR_SET_FORMAT_DEFAULT;
      //        return res;
      //      }

      template<class Stopwatch>
      void operator()(Stopwatch & stopwatch_
          //, system::error_code & ec = BOOST_CHRONO_THROWS
          )
      {

        typedef typename Stopwatch::laps_collector::storage_type
            laps_collector_acc;
        laps_collector_acc const& acc =
            stopwatch_.get_laps_collector().accumulator_set();

        typedef typename Stopwatch::duration duration_t;

        duration_style_io_saver dsios(this->os_);
        this->os_
            << static_cast<format_type&> (*this)
                % boost::accumulators::count(acc)
                % io::group(std::fixed, std::setprecision(this->precision_), duration_fmt(this->duration_style_), (boost::accumulators::count(acc)
                    == 0) ? boost::chrono::duration<double, Ratio>(duration_t::zero()) : boost::chrono::duration<
                    double, Ratio>(duration_t(boost::accumulators::sum(acc))))
                % io::group(std::fixed, std::setprecision(this->precision_), duration_fmt(this->duration_style_), (boost::accumulators::count(acc)
                    == 0) ? boost::chrono::duration<double, Ratio>(duration_t::zero()) : boost::chrono::duration<
                    double, Ratio>(duration_t((boost::accumulators::min)(acc))))
                % io::group(std::fixed, std::setprecision(this->precision_), duration_fmt(this->duration_style_), (boost::accumulators::count(acc)
                    == 0) ? boost::chrono::duration<double, Ratio>(duration_t::zero()) : boost::chrono::duration<
                    double, Ratio>(duration_t((boost::accumulators::max)(acc))))
                % io::group(std::fixed, std::setprecision(this->precision_), duration_fmt(this->duration_style_), ((boost::accumulators::count(acc)
                    > 0) ? boost::chrono::duration<double, Ratio>(duration_t(boost::accumulators::sum(acc)
                    / boost::accumulators::count(acc))) : boost::chrono::duration<
                    double, Ratio>(duration_t::zero())));

      }
    };

    typedef basic_accumulator_set_formatter<milli, char>
        accumulator_set_formatter;
    typedef basic_accumulator_set_formatter<milli, wchar_t>
        waccumulator_set_formatter;

  } // namespace chrono
} // namespace boost

#if 0
#define BOOST_CHRONO_STOPWATCHES_ACCUMULATOR_SET_FORMAT(F) \
  boost::chrono::detail::adaptive_string(F " : " BOOST_CHRONO_STOPWATCHES_ACCUMULATOR_SET_FORMAT_DEFAULT)
#ifdef __GNUC__
#define BOOST_CHRONO_STOPWATCHES_ACCUMULATOR_SET_FUNCTION_FORMAT \
  boost::chrono::elapsed_formatter::format(BOOST_CURRENT_FUNCTION)
#else
#define BOOST_CHRONO_STOPWATCHES_ACCUMULATOR_SET_FUNCTION_FORMAT \
    BOOST_CHRONO_STOPWATCHES_ACCUMULATOR_SET_FORMAT(BOOST_CURRENT_FUNCTION)
#endif
#endif

#endif


#include <iostream>
#include <boost/tokenizer.hpp>
#include <string>
#include <boost/program_options.hpp>
#include <LRWrapper.h>
#include <LegionMatrix.h>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>
#include <sstream>
#include "SlappyParams.h"
#include "Initializer.h"
#include "BetterMapper.h"
#include <MustEpochKernelLauncher.h>
#include <LegionHelper.h>
#include "InGroupSource.h"
#include "Sweep.h"
#include "SweepMapper.h"
//#include "gnuplot_i.h"

#include "CrossGroupSrc.h"
enum Task_IDs
{
	TOP_LEVEL_TASK_ID
};

using namespace Dragon;

int setup_diags(Context ctx, HighLevelRuntime* runtime,
                 LRWrapper& diags, LRWrapper& lines,
				 int& ndiag, int nx_l, int ny_l, int nz_l);

void setup_slgg(Context ctx, HighLevelRuntime* runtime,LRWrapper lw_slgg,int nmat,int nmom,int ng)
{
	double sigS[nmat][ng];
	for(int g=0;g<ng;g++)
		for(int m=0;m<nmat;m++)
		{
			if(g == 0)
			{
				if(m == 0)
					sigS[m][g] = 0.5;
				else
					sigS[m][g] = 1.2;
			}
			else
				sigS[m][g] = sigS[m][g-1]+0.005;
		}

RegionRequirement rr(lw_slgg.lr,READ_WRITE,EXCLUSIVE,lw_slgg.lr);
		rr.add_field(0);

		printf("Mapping slgg\n");
PhysicalRegion pr = runtime->map_region(ctx,rr);

LegionMatrix<double> slgg(lw_slgg,pr,0);

for(int g2 = 0;g2<ng;g2++)
	for(int g1=0;g1<ng;g1++)
		for(int m=0;m<nmat;m++)
		{
			if(m == 0)
			{
				if(g1 == g2)
					slgg(m,0,g1,g2) = 0.2*sigS[m][g1];
				else if(g2 < g1)
					if(g1 > 0)
						slgg(m,0,g1,g2) = 0.1*sigS[m][g1]/((double)g1);
					else
						slgg(m,0,g1,g2) = 0.3*sigS[m][g1];
				else
					if(g1 < ng-1)
						slgg(m,0,g1,g2) = 0.7*sigS[m][g1]/((double)(ng-g1-1));
					else
						slgg(m,0,g1,g2) = 0.9*sigS[m][g1];
			}
			else
			{
				if(g1 == g2)
					slgg(m,0,g1,g2) = 0.5*sigS[m][g1];
				else if(g2 < g1)
					if(g1 > 0)
						slgg(m,0,g1,g2) = 0.1*sigS[m][g1]/((double)g1);
					else
						slgg(m,0,g1,g2) = 0.6*sigS[m][g1];
				else
					if(g1 < ng-1)
						slgg(m,0,g1,g2) = 0.4*sigS[m][g1]/((double)(ng-g1-1));
					else
						slgg(m,0,g1,g2) = 0.9*sigS[m][g1];
			}
		}

if(nmom > 1)
{
	for(int m=0;m<nmat;m++)
	{
		double coef1,coef2;
		if(m==0)
		{coef1=0.1;coef2=0.5;}
		else
		{coef1=0.8;coef2=0.6;}
		for(int g2=0;g2<ng;g2++)
			for(int g1=0;g1<ng;g1++)
			{
				slgg(m,1,g1,g2) = coef1*slgg(m,0,g1,g2);
				for(int n=2;n<nmom;n++)
					slgg(m,n,g1,g2) = coef2*slgg(m,n-1,g1,g2);
			}

	}
}

runtime->unmap_region(ctx,pr);

}

void top_level_task(const Task *task,
                    const std::vector<PhysicalRegion> &regions,
                    Context ctx, HighLevelRuntime *runtime)
{

	runtime->disable_profiling();
	namespace po = boost::program_options;
	using namespace std;

	const InputArgs &command_args = HighLevelRuntime::get_input_args();
	SlappyParams params(command_args);

	int nx=params.nx;
	int ny=params.ny;
	int nz = params.nz;
	int nx_l(params.nx_l),ny_l(params.ny_l),nz_l(params.nz_l);
	int nCx(params.nCx), nCy(params.nCy), nCz(params.nCz);
	int ng(params.ng),nang(params.nang),nmom(params.nmom);
	int noct(params.noct);
	int nmat(params.nmat);
	int cmom(params.cmom);

	int& nDiag = params.nDiag;
	int& nPoints = params.nPoints;

	LRWrapper allArrays;
	allArrays.create(ctx,runtime,"allArrays",{1},0,LRWrapper(),1,LRWrapper(),2,LRWrapper(),3,LRWrapper());
	// Contains the by-chunk decomposition for all chunk dependent arrays
	LRWrapper chunkArrays;
	chunkArrays.create(ctx,runtime,"chunkArrays",{nCx,nCy,nCz},
	                   FIDs::Array::angFluxes,LRWrapper(),
	                   FIDs::Array::scalFluxes,LRWrapper(),
	                   FIDs::Array::savedFlux,LRWrapper(),
	                   FIDs::Array::sigmaT,LRWrapper(),
	                   FIDs::Array::sigmaS,LRWrapper(),
	                   FIDs::Array::mat,LRWrapper(),
	                   FIDs::Array::bci,LRWrapper(),
	                   FIDs::Array::bcj,LRWrapper(),
	                   FIDs::Array::bck,LRWrapper());

	// Setup all of the interior logical regions
	{
	RegionRequirement rr(chunkArrays.lr,WRITE_ONLY,EXCLUSIVE,chunkArrays.lr);
			rr.add_field(FIDs::Array::angFluxes);
			rr.add_field(FIDs::Array::scalFluxes);
			rr.add_field(FIDs::Array::savedFlux);
			rr.add_field(FIDs::Array::sigmaT);
			rr.add_field(FIDs::Array::sigmaS);
			rr.add_field(FIDs::Array::mat);
			rr.add_field(FIDs::Array::bci);
			rr.add_field(FIDs::Array::bcj);
			rr.add_field(FIDs::Array::bck);

	PhysicalRegion pr = runtime->map_region(ctx,rr);

	LegionMatrix<LRWrapper> angFluxes(chunkArrays,pr,FIDs::Array::angFluxes);
	LegionMatrix<LRWrapper> scalFluxes(chunkArrays,pr,FIDs::Array::scalFluxes);
	LegionMatrix<LRWrapper> savedFluxes(chunkArrays,pr,FIDs::Array::savedFlux);
	LegionMatrix<LRWrapper> sigmaT(chunkArrays,pr,FIDs::Array::sigmaT);
	LegionMatrix<LRWrapper> sigmaS(chunkArrays,pr,FIDs::Array::sigmaS);
	LegionMatrix<LRWrapper> mat(chunkArrays,pr,FIDs::Array::mat);
	LegionMatrix<LRWrapper> bci(chunkArrays,pr,FIDs::Array::bci);
	LegionMatrix<LRWrapper> bcj(chunkArrays,pr,FIDs::Array::bcj);
	LegionMatrix<LRWrapper> bck(chunkArrays,pr,FIDs::Array::bck);

	for(int k=0;k<nCz;k++)
		for(int j=0;j<nCy;j++)
			for(int i=0;i<nCx;i++)
			{
				{
				char* cell_id;
				asprintf(&cell_id," %i, %i, %i,",i,j,k);
				auto cn = [&](const char* name)
		{
					std::string res(name);
					return res+cell_id;
		};
				angFluxes(i,j,k) = (createLRWrapper(ctx,runtime,cn("angFluxes").c_str(),{nang,nx_l,ny_l,nz_l,ng,noct},
				                   FIDs::AngFlux::ptr_in,double(),
				                   FIDs::AngFlux::ptr_out,double()));

				scalFluxes(i,j,k) = createLRWrapper(ctx,runtime,cn("scalFluxes").c_str(),{cmom,nx_l,ny_l,nz_l,ng},
								   FIDs::ScalFlux::q2grp,double(),
								   FIDs::ScalFlux::qtot,double(),
				                   FIDs::ScalFlux::flux,double()
				                   );
				savedFluxes(i,j,k) = createLRWrapper(ctx,runtime,cn("savedFluxes").c_str(),{nx_l,ny_l,nz_l,ng},
				                   FIDs::SavedFlux::fluxpo,double(),
				                   FIDs::SavedFlux::fluxpi,double());

				sigmaT(i,j,k) = createLRWrapper(ctx,runtime,cn("sigmaT").c_str(),{nx_l,ny_l,nz_l,ng},
				                   FIDs::SigmaT::t_xs,double(),
				                   FIDs::SigmaT::a_xs,double(),
				                   FIDs::SigmaT::qi,double());

				sigmaS(i,j,k) = createLRWrapper(ctx,runtime,cn("sigmaS").c_str(),{nmom,nx_l,ny_l,nz_l,ng},
				                   FIDs::SigmaS::s_xs,double());

				mat(i,j,k) = createLRWrapper(ctx,runtime,cn("mat").c_str(),{nx_l,ny_l,nz_l},
				                   FIDs::Mat::mat,int());

				bci(i,j,k) = createLRWrapper(ctx,runtime,cn("bci").c_str(),{nang,ny_l,nz_l,ng},
				                   FIDs::BCi::ib_out,double(),1,double());
				bcj(i,j,k) = createLRWrapper(ctx,runtime,cn("bcj").c_str(),{nang,nx_l,nz_l,ng},
				                   FIDs::BCj::jb_out,double(),1,double());
				bck(i,j,k) = createLRWrapper(ctx,runtime,cn("bck").c_str(),{nang,nx_l,ny_l,ng},
				                   FIDs::BCk::kb_out,double(),1,double());
				}

			}

	runtime->unmap_region(ctx,pr);
	runtime->unmap_all_regions(ctx);

	}

	LRWrapper lw_slgg;
	lw_slgg.create(ctx,runtime,"slgg",{nmat,nmom,ng,ng},0,double());
	setup_slgg(ctx,runtime,lw_slgg,nmat,nmom,ng);

	// Setup all of the interior logical regions

	LRWrapper diags;
	LRWrapper lines;

	printf("Setting up Diags\n");
	nPoints = setup_diags(ctx,runtime,diags,lines,nDiag,nx_l,ny_l,nz_l);

	// Initialize stuff
	Initializer setup_stuff(params);

	auto init_op = genMustEpochKernel(setup_stuff,ctx,runtime,chunkArrays,lw_slgg);
//	runtime->unmap_all_regions(ctx);

	init_op.execute(ctx,runtime);
	printf("finished init op\n");

//	{
//
//	RegionRequirement rr(chunkArrays.lr,READ_ONLY,SIMULTANEOUS,chunkArrays.lr);
//			rr.add_field(FIDs::Array::sigmaT);
//			rr.add_field(FIDs::Array::sigmaS);
//
//
//
//
//	PhysicalRegion pr = runtime->map_region(ctx,rr);
//	LegionMatrix<LRWrapper> sigmaT(chunkArrays,pr,FIDs::Array::sigmaT);
//	LegionMatrix<LRWrapper> sigmaS(chunkArrays,pr,FIDs::Array::sigmaS);
//
//	float* x = (float*)malloc(nx*sizeof(float));
//	float* y = (float*)malloc(ny*sizeof(float));
//	float* qi_z = (float*)malloc(nx*ny*sizeof(float));
//
//
//	for(int jC=0;jC<nCy;jC++)
//		for(int iC=0;iC<nCx;iC++)
//		{
//			RegionRequirement rr_sigmaT(sigmaT(iC,jC,0).cast().lr,READ_ONLY,EXCLUSIVE,sigmaT(iC,jC,0).cast().lr);
//			rr_sigmaT.add_field(FIDs::SigmaT::t_xs);
//			rr_sigmaT.add_field(FIDs::SigmaT::qi);
//
//			RegionRequirement rr_sigmaS(sigmaS(iC,jC,0).cast().lr,READ_ONLY,EXCLUSIVE,sigmaS(iC,jC,0).cast().lr);
//			rr_sigmaS.add_field(FIDs::SigmaS::s_xs);
//
//			PhysicalRegion pr_sigmaT = runtime->map_region(ctx,rr_sigmaT);
//			PhysicalRegion pr_sigmaS = runtime->map_region(ctx,rr_sigmaS);
//
//			LegionMatrix<double> t_xs(sigmaT(iC,jC,0).cast(),pr_sigmaT,FIDs::SigmaT::t_xs);
//			LegionMatrix<double> qi(sigmaT(iC,jC,0).cast(),pr_sigmaT,FIDs::SigmaT::qi);
//			LegionMatrix<double> s_xs(sigmaS(iC,jC,0).cast(),pr_sigmaS,FIDs::SigmaS::s_xs);
//
//			for(int j=0;j<ny_l;j++)
//				for(int i=0;i<nx_l;i++)
//				{
//					int ix = i+nx_l*iC;
//					int iy = j+ny_l*jC;
//
//					x[ix] = ix;
//					y[iy] = iy;
////					printf("t_xs(%i, %i, 0) = %e\n",ix,iy,t_xs(i,j,0,0).cast());
//					qi_z[ix+nx*iy] = t_xs(i,j,0,0).cast();
//
//				}
//
//
//
//			runtime->unmap_region(ctx,pr_sigmaT);
//			runtime->unmap_region(ctx,pr_sigmaS);
//
//
//		}
//
//	gnuplot_ctrl* qi_plt = gnuplot_init();
//
//	gnuplot_plot_xyz(qi_plt,x,y,qi_z,nx,ny,"qi");
//	getchar();
//
//	runtime->unmap_region(ctx,pr);
//
//	runtime->unmap_all_regions(ctx);
//
//	}

	{
	RegionRequirement rr(allArrays.lr,READ_WRITE,EXCLUSIVE,allArrays.lr);
	rr.add_field(0);
	rr.add_field(1);
	rr.add_field(2);
	rr.add_field(3);

PhysicalRegion pr = runtime->map_region(ctx,rr);
	{

	LegionMatrix<LRWrapper> _allArrays(allArrays,pr,0);
	LegionMatrix<LRWrapper> _slgg(allArrays,pr,1);
	LegionMatrix<LRWrapper> _diag(allArrays,pr,2);
	LegionMatrix<LRWrapper> _lines(allArrays,pr,3);

	_allArrays(0) = chunkArrays;
	_slgg(0) = lw_slgg;
	_diag(0) = diags;
	_lines(0) = lines;

	}

	runtime->unmap_region(ctx,pr);

	}

	CrossGroupSrc outer(params);

	auto outer_op = genSingleKernel(outer, ctx, runtime, allArrays, chunkArrays, lw_slgg, diags,lines);

	printf("Executing outer op\n");
	runtime->execute_task(ctx,outer_op).get_void_result();
	printf("finished!\n");
}

static void update_mappers(Machine machine, HighLevelRuntime *rt,
						   const std::set<Processor> &local_procs)
{
	printf("Updating mappers\n");
  for (std::set<Processor>::const_iterator it = local_procs.begin();
		it != local_procs.end(); it++)
  {
//	rt->add_mapper(1,new SweepMapper(machine, rt, *it), *it);
    rt->replace_default_mapper(new BetterMapper(machine, rt, *it), *it);
  }
}

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

  char hostname[512];
  gethostname(hostname,512);

  srand(23948723);
//
//	  dup2(fileno(err_file),STDERR_FILENO);
  HighLevelRuntime::set_top_level_task_id(TOP_LEVEL_TASK_ID);
  HighLevelRuntime::register_legion_task<top_level_task>(TOP_LEVEL_TASK_ID,
      Processor::LOC_PROC, true/*single*/, false/*index*/);

//	  fclose(err_file);

//	  std::stringstream buffer;
  setbuf(stdout,NULL);
  setvbuf(stderr,NULL,_IOLBF,1024);

  system("export GASNET_BACKTRACE=1");

//  TaskHelper::register_hybrid_variants<FieldKernelOp<Initializer>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<CrossGroupSrc>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<OuterSrcScat>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<OuterConv>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<SaveFluxes>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<InGroupSource>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<InnerSrcScat>>();

//  RegisterDim3Variants();

  TaskHelper::register_hybrid_variants<LegionHelper::Setter<bool>>();
  TaskHelper::register_hybrid_variants<LegionHelper::Setter<int>>();
  TaskHelper::register_hybrid_variants<LegionHelper::Setter<double>>();
  TaskHelper::register_hybrid_variants<LegionHelper::Setter<LRWrapper>>();
  TaskHelper::register_cpu_variants<LegionHelper::Getter<int>>();
  TaskHelper::register_cpu_variants<LegionHelper::Getter<bool>>();

  CrossGroupSrc::register_cpu();
  InnerSrcScat::register_cpu();

  Initializer::register_task();

  register_sweep_tasks();

//  TaskHelper::register_hybrid_variants<FieldKernelOp<Dim3Sweep<0>>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<Dim3Sweep<1>>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<Dim3Sweep<2>>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<Dim3Sweep<3>>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<Dim3Sweep<4>>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<Dim3Sweep<5>>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<Dim3Sweep<6>>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<Dim3Sweep<7>>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<Dim3Sweep<8>>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<Dim3Sweep<9>>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<Dim3Sweep<10>>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<Dim3Sweep<11>>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<Dim3Sweep<12>>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<Dim3Sweep<13>>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<Dim3Sweep<14>>>();
//  TaskHelper::register_hybrid_variants<FieldKernelOp<Dim3Sweep<15>>>();

	  HighLevelRuntime::set_registration_callback(update_mappers);

  return HighLevelRuntime::start(argc, argv);
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_OPERATOR_FUNCTIONS_DIVIDES_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_DIVIDES_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief divides generic tag

     Represents the divides function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct divides_ : ext::elementwise_<divides_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<divides_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_divides_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::divides_, Site> dispatching_divides_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::divides_, Site>();
   }
   template<class... Args>
   struct impl_divides_;
  }
  /*!
    return the elementwise division of the two parameters
    Infix notation can be used with operator '/'

    @par Semantic:

    For every parameters of types respectively T0, T1:

    @code
    T0 r = divides(a0,a1);
    @endcode

    is similar to:

    @code
    T0 r = a0/a1;
    @endcode

    @par Alias:
    @c div, @c rdiv

    @see  @funcref{fast_divides}, @funcref{rec}, @funcref{fast_rec}, @funcref{divs}, @funcref{divfloor},
    @funcref{divceil}, @funcref{divround}, @funcref{divround2even}, @funcref{divfix}
    @param a0

    @param a1

    @return a value of the same type as the second parameter
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::divides_             , divides         , 2 )
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::divides_             , div             , 2 )
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::divides_             , rdiv            , 2 )
} }

#include <boost/simd/operator/specific/common.hpp>

#endif


#ifndef ASLAM_BACKEND_COMPRESSED_COLUMN_MATRIX_HPP
#define ASLAM_BACKEND_COMPRESSED_COLUMN_MATRIX_HPP

#include <cs.h>
#include "Cholmod.hpp"
#include <vector>
#include <eigen3/Eigen/Core>
//#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
//#include <eigen3/Eigen/Sparse>
#include <sm/assert_macros.hpp>
#include <sm/string_routines.hpp>
#include <boost/cstdint.hpp>
#include "JacobianContainer.hpp"
#include "ErrorTerm.hpp"
#include <iostream>
#include "Matrix.hpp"

namespace aslam {
  namespace backend {

    struct JacobianColumnPointer {
      JacobianColumnPointer(size_t sc = 0, size_t epc = 0) : startValueIndex(sc), elementsPerColumn(epc) {}
      size_t startValueIndex;
      size_t elementsPerColumn;
    };

    template<typename INDEX_T = int>
    class CompressedColumnMatrix : public Matrix {
    public:

      SM_DEFINE_EXCEPTION(Exception, std::runtime_error);

      /// \brief the index type of the matrix
      typedef INDEX_T index_t;

      //typedef Eigen::MappedCompressedColumnMatrix<value_t> eigen_sparse_t;
      CompressedColumnMatrix();

      /// \brief initialize and set the potential number of nonzeros.
      CompressedColumnMatrix(size_t rows, size_t cols, size_t nnz, size_t num_cols);

      virtual ~CompressedColumnMatrix();

      /// \brief Return this matrix as a Cholmod sparse matrix
      void getView(cholmod_sparse* cs);

      virtual void toDenseInto(Eigen::MatrixXd& outM) const;

      /// \brief Clear all values in this matrix
      void clear();

      ///  \brief Initialize the matrix
      void init(size_t rows, size_t cols, size_t nnz, size_t num_cols);

      /// \brief return the number of rows in this matrix
      virtual size_t rows() const;

      /// \brief return the number of columns in this matrix
      virtual size_t cols() const;

      /// \brief get the element at row r and column c
      virtual double operator()(size_t r, size_t c) const;

      /// \brief get the element at row r and column c. r and c are checked for validity
      double value(size_t r, size_t c) const;

      /// \brief Get the number of non zeros
      size_t nnz() const;

      /// \brief Get the underlying values
      const std::vector<double>& values() const;

      /// \brief Get the underlying row indices
      const std::vector<index_t>& row_ind() const;

      /// \brief Get the underlying column pointers
      const std::vector<index_t>& col_ptr() const;

      /**
       * \brief A convenience function that gets the Jacobians from the
       *        error term and calls appendJacobiansSymbolic()
       *
       */
      JacobianColumnPointer appendErrorJacobiansSymbolic(const ErrorTerm& e);

      /**
       * \brief Append \f$\mathbf J^T\f$ to the right of this matrix.
       *
       * This method is amortized O(1) as the block for J^T at the right
       * is contiguous in memory and at the end of the _values array
       *
       * @param Jrows The number of rows in the Jacobian (corresponding to the number of elements in an error term)
       * @param dvs The list of design variables with non-zero Jacobians in this column.
       * @return outColumnPointer A pointer to the value array of the Jacobian matrix.
       */
      JacobianColumnPointer appendJacobiansSymbolic(int Jrows, const std::vector<DesignVariable*>& dvs);

      /// \brief Write the Jacobian values to the matrix using the pointer provided by appendJacobiansSymbolic()
      void writeJacobians(const JacobianContainer& jc, const JacobianColumnPointer& cp);

      /// \brief A convenience function that calls appendJacobiansSymbolic() and then writeJacobians()
      void appendJacobians(const JacobianContainer& jc);

      /// \brief Push a constant diagonal block on to the end of the matrix.
      void pushConstantDiagonalBlock(double constant);

      /// \brief Push a diagonal block on to the end of the matrix.
      ///        The diagonal vector must have the same number of rows as the matrix.
      void pushDiagonalBlock(const Eigen::VectorXd& diagonal);

      /// \brief Pop the diagonal block off of the matrix.
      void popDiagonalBlock();

      /// \brief update the diagonal block
      void updateDiagonalBlock(const Eigen::VectorXd& diagonal);

      /// \brief update the diagonal block with a constant value
      void updateConstantDiagonalBlock(double diagonal);

      /// \brief right multiply the vector y = A x
      void rightMultiply(const Eigen::VectorXd& x, Eigen::VectorXd& outY) const;

      /// \brief left multiply the vector y = A^T x
      void leftMultiply(const Eigen::VectorXd& x, Eigen::VectorXd& outY) const;

      /// \brief Initialize the matrix from a dense matrix
      virtual void fromDense(const Eigen::MatrixXd& M);

      /// \brief Initialize the matrix from a dense matrix.
      ///        Entries with absolute value less than tolerance
      ///        should be considered zeros.
      virtual void fromDenseTolerance(const Eigen::MatrixXd& M, double tolerance);

      /// Writes to standard output
      virtual void write(std::ostream& stream) const;

      /// Writes to standard stream in MATLAB format
      void writeMATLAB(std::ostream& stream) const;

      /// Initializes the matrix from a cholmod_sparse matrix
      void fromCholmodSparse(const cholmod_sparse* cs);

    private:
      void checkMatrixDbg();

      size_t _rows;
      size_t _cols;
      std::vector<double> _values;
      std::vector<index_t> _row_ind;
      std::vector<index_t> _col_ptr;

      bool _hasDiagonalAppended;
      // Keep one of these guys around to make cs views.
      cholmod_sparse _cholmodSparse;

    };

  } // namespace backend
} // namespace aslam

#include "implementation/CompressedColumnMatrix.hpp"

#endif /* ASLAM_BACKEND_COMPRESSED_COLUMN_MATRIX_HPP */