#include <stdio.h>
#include <cfloat>
#include <list>
#include <vector>
#include <math.h>
#include <iostream>
#include <algorithm>
#include <numeric>
#include <boost/algorithm/clamp.hpp>
#include <random>
#include "third_party/eigen3/unsupported/Eigen/CXX11/Tensor"
#include "unsupported/Eigen/CXX11/src/Tensor/TensorDeviceCuda.h"
//#include "unsupported/Eigen/CXX11/Tensor"
#include "tensorflow/core/framework/op.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor_shape.h"
#include "tensorflow/core/framework/common_shape_fns.h"
#include "tensorflow/core/util/work_sharder.h"
#include "bboxes.h"
#include "wtoolkit.h"
#include "wtoolkit_cuda.h"
#include <future>

using namespace tensorflow;
using namespace std;

typedef Eigen::ThreadPoolDevice CPUDevice;
typedef Eigen::GpuDevice GPUDevice;
/*
 * num_classes: 类别数，不含背景
 * gaussian_iou: 一般为0.7
 * gbboxes: groundtruth bbox, [B,N,4] 相对坐标
 * glabels: 标签[B,N], 背景为0
 * glength: 有效的groundtruth bbox数量
 * output_size: 输出图的大小[2]=(OH,OW)
 *
 * output:
 * output_heatmaps_c: center heatmaps [B,OH,OW,num_classes]
 * output_hw_offset: [B,OH,OW,4], (h,w,yoffset,xoffset)
 * output_mask: [B,OH,OW,2] (hw_mask,offset_mask)
 */
REGISTER_OP("Center2BoxesEncode")
    .Attr("T: {float,double,int32,int64}")
	.Attr("num_classes:int")
	.Attr("gaussian_iou:float=0.7")
    .Input("gbboxes: T")
    .Input("glabels: int32")
    .Input("glength: int32")
    .Input("output_size: int32")
	.Output("output_heatmaps_c:T")
	.Output("output_hw_offset:T")
	.Output("output_hw_offset_mask:T")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
            const auto input_shape0 = c->input(0);
            const auto batch_size = c->Dim(input_shape0,0);
            int num_classes;
            c->GetAttr("num_classes",&num_classes);
            auto shape0 = c->MakeShape({batch_size,-1,-1,num_classes});
            auto shape1 = c->MakeShape({batch_size,-1,-1,4});
            auto shape2 = c->MakeShape({batch_size,-1,-1,2});

			c->set_output(0, shape0);
			c->set_output(1, shape1);
			c->set_output(2, shape2);

			return Status::OK();
			});

template <typename Device,typename T>
class Center2BoxesEncodeOp: public OpKernel {
	public:
		explicit Center2BoxesEncodeOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("num_classes", &num_classes_));
			OP_REQUIRES_OK(context, context->GetAttr("gaussian_iou", &gaussian_iou_));
		}

		void Compute(OpKernelContext* context) override
        {
            TIME_THISV1("Center2BoxesEncode");
            const Tensor &_gbboxes    = context->input(0);
            const Tensor &_glabels    = context->input(1);
            const Tensor &_gsize      = context->input(2);
            auto          gbboxes     = _gbboxes.template tensor<T,3>();
            auto          glabels     = _glabels.template tensor<int,2>();
            auto          gsize       = _gsize.template tensor<int,1>();
            auto          output_size = context->input(3).template flat<int>().data();
            const auto    batch_size  = _gbboxes.dim_size(0);

            OP_REQUIRES(context, _gbboxes.dims() == 3, errors::InvalidArgument("box data must be 3-dimension"));
            OP_REQUIRES(context, _glabels.dims() == 2, errors::InvalidArgument("labels data must be 2-dimension"));
            OP_REQUIRES(context, _gsize.dims() == 1, errors::InvalidArgument("gsize data must be 1-dimension"));

            int           dims_4d0[4]            = {int(batch_size),output_size[0],output_size[1],num_classes_};
            int           dims_4d1[4]            = {int(batch_size),output_size[0],output_size[1],4};
            int           dims_4d2[4]            = {int(batch_size),output_size[0],output_size[1],2};
            TensorShape  outshape0;
            TensorShape  outshape1;
            TensorShape  outshape2;
            Tensor      *output_heatmaps_c  = NULL;
            Tensor      *output_hw_offset      = NULL;
            Tensor      *output_mask = NULL;

            TensorShapeUtils::MakeShape(dims_4d0, 4, &outshape0);
            TensorShapeUtils::MakeShape(dims_4d1, 4, &outshape1);
            TensorShapeUtils::MakeShape(dims_4d2, 4, &outshape2);

            OP_REQUIRES_OK(context, context->allocate_output(0, outshape0, &output_heatmaps_c));
            OP_REQUIRES_OK(context, context->allocate_output(1, outshape1, &output_hw_offset));
            OP_REQUIRES_OK(context, context->allocate_output(2, outshape2, &output_mask));

            auto heatmaps_c  = output_heatmaps_c->template tensor<T,4>();
            auto hw_offsets     = output_hw_offset->template tensor<T,4>();
            auto o_mask = output_mask->template tensor<T,4>();
            Eigen::Tensor<float,2,Eigen::RowMajor> max_probs(output_size[0],output_size[1]);

            heatmaps_c.setZero();
            hw_offsets.setZero();
            o_mask.setZero();

            for(auto i=0; i<batch_size; ++i) {
                max_probs.setZero();
                for(auto j=0; j<gsize(i); ++j) {
                    const auto fytl = gbboxes(i,j,0)*(output_size[0]-1);
                    const auto fxtl = gbboxes(i,j,1)*(output_size[1]-1);
                    const auto fybr = gbboxes(i,j,2)*(output_size[0]-1);
                    const auto fxbr = gbboxes(i,j,3)*(output_size[1]-1);
                    const auto fyc = (fytl+fybr)/2;
                    const auto fxc = (fxtl+fxbr)/2;
                    const auto yc = int(fyc+0.5);
                    const auto xc = int(fxc+0.5);
                    const auto r0 = get_gaussian_radius(fybr-fytl,fxbr-fxtl,gaussian_iou_);
                    const auto label = glabels(i,j);
                    const auto h = fybr-fytl;
                    const auto w = fxbr-fxtl;

                    if(yc<0||xc<0||yc>=output_size[0]||xc>=output_size[1]) {
                        cout<<"ERROR bboxes data: "<<gbboxes(i,j,0)<<","<<gbboxes(i,j,1)<<","<<gbboxes(i,j,2)<<","<<gbboxes(i,j,3)<<endl;
                        continue;
                    }

                    draw_gaussian(heatmaps_c,xc,yc,r0,i,label,5);
                    draw_gaussianv2(max_probs,hw_offsets,xc,yc,r0,h,w,i,5);

                    hw_offsets(i,yc,xc,2) = fyc-yc;
                    hw_offsets(i,yc,xc,3) = fxc-xc;
                    o_mask(i,yc,xc,1) = 1.0;
                }
                o_mask.chip(i,0).chip(0,2) = max_probs;
            }
        }
        template<typename DT>
        static void draw_gaussian(DT& data,int cx,int cy,float radius,int batch_index,int class_index,float delta=6,float k=1.0)
        {
            const auto width   = data.dimension(2);
            const auto height  = data.dimension(1);
            const auto xtl     = max(0,int(cx-radius));
            const auto ytl     = max(0,int(cy-radius));
            const auto xbr     = min<int>(width,int(cx+radius+1));
            const auto ybr     = min<int>(height,int(cy+radius+1));
            const auto sigma   = (2*radius+1)/delta;
            const auto c_index = class_index-1;

            for(auto x=xtl; x<xbr; ++x) {
                for(auto y=ytl; y<ybr; ++y) {
                    auto dx = x-cx;
                    auto dy = y-cy;
                    auto v = exp(-(dx*dx+dy*dy)/(2*sigma*sigma))*k;
                    data(batch_index,y,x,c_index) = max(data(batch_index,y,x,c_index),v);
                }
            }
        }
        template<typename DT0,typename DT1>
        static void draw_gaussianv2(DT0& data0,DT1& data1,int cx,int cy,float radius,float h,float w,int batch_index,int class_index,float delta=6,float k=1.0)
        {
            const auto width   = data1.dimension(2);
            const auto height  = data1.dimension(1);
            const auto xtl     = max(0,int(cx-radius));
            const auto ytl     = max(0,int(cy-radius));
            const auto xbr     = min<int>(width,int(cx+radius+1));
            const auto ybr     = min<int>(height,int(cy+radius+1));
            const auto sigma   = (2 *radius+1)/delta;
            const auto c_index = class_index-1;

            for(auto x=xtl; x<xbr; ++x) {
                for(auto y=ytl; y<ybr; ++y) {
                    auto dx = x-cx;
                    auto dy = y-cy;
                    auto v = exp(-(dx*dx+dy*dy)/(2*sigma*sigma))*k;
                    if(data0(y,x)<v) {
                        data0(y,x) = v;
                        data1(batch_index,y,x,0) = h;
                        data1(batch_index,y,x,1) = w;
                    }
                }
            }
        }
	private:
        int   num_classes_  = 80;
        float gaussian_iou_ = 0.7f;
};
REGISTER_KERNEL_BUILDER(Name("Center2BoxesEncode").Device(DEVICE_CPU).TypeConstraint<float>("T"), Center2BoxesEncodeOp<CPUDevice, float>);

REGISTER_OP("Center2BoxesDecode")
    .Attr("T: {float,double,int32,int64}")
    .Attr("k:int")
    .Attr("threshold:float")
    .Input("heatmaps: T")
    .Input("offset: T")
    .Input("hw: T")
	.Output("output_bboxes:T")
	.Output("output_labels:int32")
	.Output("output_probs:T")
	.Output("output_index:int32")
	.Output("output_lens:int32")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
            int k;
            c->GetAttr("k",&k);
            const auto input_shape0 = c->input(0);
            const auto batch_size = c->Dim(input_shape0,0);
            auto shape0 = c->MakeShape({batch_size,k,4});
            auto shape1 = c->MakeShape({batch_size,k});
            auto shape2 = c->MakeShape({batch_size});

			c->set_output(0, shape0);
			c->set_output(1, shape1);
			c->set_output(2, shape1);
			c->set_output(3, shape1);
			c->set_output(4, shape2);
			return Status::OK();
			});

template <typename Device,typename T>
class Center2BoxesDecodeOp: public OpKernel {
    private:
        struct InterData{
            InterData(int y,int x,int z,float s):y(y),x(x),z(z),score(s){}
            float score;
            int y,x,z;
            bool operator<(const InterData& rhv)const{
                return score<rhv.score;
            }
        };
        struct Box
        {
            float ymin;
            float xmin;
            float ymax;
            float xmax;
            float prob;
            int classes;
            int index;
        };
    public:
        explicit Center2BoxesDecodeOp(OpKernelConstruction* context) : OpKernel(context) {
            OP_REQUIRES_OK(context, context->GetAttr("k", &k_));
            OP_REQUIRES_OK(context, context->GetAttr("threshold", &threshold_));
        }
        void Compute(OpKernelContext* context) override
        {
            TIME_THISV1("Center2BoxesDecode");
            const Tensor &_heatmaps_c = context->input(0);
            const Tensor &_offset_c   = context->input(1);
            const Tensor &_hw         = context->input(2);

            OP_REQUIRES(context, _heatmaps_c.dims() == 4, errors::InvalidArgument("heatmap data must be 4-dimension"));
            OP_REQUIRES(context, _offset_c.dims() == 4, errors::InvalidArgument("offset data must be 4-dimension"));
            OP_REQUIRES(context, _hw.dims() == 4, errors::InvalidArgument("hw data must be 4-dimension"));

            auto          heatmaps_c_r  = _heatmaps_c.template tensor<T,4>();
            auto          offset_c    = _offset_c.template tensor<T,4>();
            auto          hw          = _hw.template tensor<T,4>();

            const auto batch_size = _heatmaps_c.dim_size(0);
            vector<vector<Box>> res_boxes;

            auto heatmaps_c = batch_sim_max_pool(heatmaps_c_r);

            for(auto i=0; i<batch_size; ++i) {
                auto c = get_top_k(heatmaps_c, i,k_);
                auto tboxes = get_boxes(c,i,hw,offset_c);
                res_boxes.push_back(std::move(tboxes));
            }

            auto         box_nr        = k_;
            int          dims_3d[3]    = {int(batch_size),box_nr,4};
            int          dims_2d[2]    = {int(batch_size),box_nr};
            int          dims_1d[1]    = {int(batch_size)};
            TensorShape  outshape0;
            TensorShape  outshape1;
            TensorShape  outshape2;
            Tensor      *output_boxes  = NULL;
            Tensor      *output_labels = NULL;
            Tensor      *output_probs  = NULL;
            Tensor      *output_indexs = NULL;
            Tensor      *output_lens   = NULL;

            TensorShapeUtils::MakeShape(dims_3d, 3, &outshape0);
            TensorShapeUtils::MakeShape(dims_2d, 2, &outshape1);
            TensorShapeUtils::MakeShape(dims_1d, 1, &outshape2);

            OP_REQUIRES_OK(context, context->allocate_output(0, outshape0, &output_boxes));
            OP_REQUIRES_OK(context, context->allocate_output(1, outshape1, &output_labels));
            OP_REQUIRES_OK(context, context->allocate_output(2, outshape1, &output_probs));
            OP_REQUIRES_OK(context, context->allocate_output(3, outshape1, &output_indexs));
            OP_REQUIRES_OK(context, context->allocate_output(4, outshape2, &output_lens));

            auto o_boxes = output_boxes->template tensor<T,3>();
            auto o_labels = output_labels->template tensor<int,2>();
            auto o_probs = output_probs->template tensor<T,2>();
            auto o_indexs = output_indexs->template tensor<int,2>();
            auto o_lens = output_lens->template tensor<int,1>();

            o_boxes.setZero();
            o_labels.setZero();
            o_probs.setZero();
            o_lens.setZero();
            o_indexs.setZero();

            for(auto i=0; i<batch_size; ++i) {
                auto b_it = next(res_boxes.begin(),i);
                for(auto j=0; j<b_it->size(); ++j) {
                    auto& box = (*b_it)[j];
                    o_boxes(i,j,0) = box.ymin;
                    o_boxes(i,j,1) = box.xmin;
                    o_boxes(i,j,2) = box.ymax;
                    o_boxes(i,j,3) = box.xmax;
                    o_labels(i,j) = box.classes;
                    o_probs(i,j) = box.prob;
                    o_indexs(i,j) = box.index;
                }
                o_lens(i) = b_it->size();
            }
        }

        template<typename DT>
        Eigen::Tensor<T,4,Eigen::RowMajor> batch_sim_max_pool(DT& data,int k=3,float neg_value = 0.0f) {
            Eigen::Tensor<T,4,Eigen::RowMajor> res_data(Eigen::array<Eigen::Index,4>(data.dimensions()));
            for(auto i=0; i<data.dimension(0); ++i) {
                for(auto j=0; j<data.dimension(3); ++j) {
                    Eigen::Tensor<T,2,Eigen::RowMajor> ldata = data.chip(i,0).chip(j,2);
                    sim_max_pool(ldata,k,neg_value);
                    res_data.chip(i,0).chip(j,2) = ldata;
                }
            }
            return res_data;
        }
        template<typename DT>
        void sim_max_pool(DT& data,int k=3,float neg_value = 0.0f) {

            float buffer[k *k];
            int   nr;
            auto  H            = data.dimension(0);
            auto  W            = data.dimension(1);
            auto  max_i        = -1;
            auto  max_j        = -1;
            auto  max_v        = neg_value;

            for(auto i=0; i<H; ++i) {
                for(auto j=0; j<W; ++j) {
                    auto i_min = max<int>(0,i-k);
                    auto j_min = max<int>(0,j-k);
                    auto i_max = min<int>(H-1,i+k+1);
                    auto j_max = min<int>(W-1,j+k+1);

                    max_i = -1;
                    max_v = neg_value;
                    for(auto ii=i_min; ii<i_max; ++ii) {
                        for(auto jj=j_min; jj<j_max; ++jj) {
                            if(data(ii,jj)>max_v) {
                                max_i = ii;
                                max_j = jj;
                                max_v = data(ii,jj);
                            }
                        }
                    }//end ii
                    if((max_i != i) || (max_j != j)) {
                        data(i,j) = neg_value;
                    }
                }
            }
        }

        template<typename DT>
            vector<InterData> get_top_k(const DT& heatmaps,int batch_index,int k) {
                const auto H = heatmaps.dimension(1);
                const auto W = heatmaps.dimension(2);
                const auto C = heatmaps.dimension(3);
                vector<InterData> res;
                res.reserve(H*W/4);
                for(auto y=0; y<H; ++y) {
                    for(auto x=0; x<W; ++x) {
                        for(auto z=0; z<C; ++z) {
                            auto score = heatmaps(batch_index,y,x,z);
                            if(score>threshold_)
                                res.emplace_back(y,x,z,heatmaps(batch_index,y,x,z));
                        }
                    }
                }
                auto mid = res.begin()+min<int>(k,res.size());
                partial_sort(res.begin(),mid,res.end(),[this](auto lhv,auto rhv){ return lhv.score>rhv.score;});
                res.erase(mid,res.end());
                return res;
            }

        template<typename DT0,typename DT1>
        vector<Box> get_boxes(const vector<InterData>& c,int batch_index,const DT0& HW,const DT1& offset) {
            vector<Box> boxes;
            const auto H = HW.dimension(1);
            const auto W = HW.dimension(2);

            for(auto& id:c) {
                auto cx = id.x+offset(batch_index,id.y,id.x,1);
                auto cy = id.y+offset(batch_index,id.y,id.x,0);
                auto hh = HW(batch_index,id.y,id.x,0)/2;
                auto hw = HW(batch_index,id.y,id.x,1)/2;
                Box box;
                box.xmin = (cx-hw)/(W-1);
                box.ymin = (cy-hh)/(H-1);
                box.xmax = (cx+hw)/(W-1);
                box.ymax = (cy+hh)/(H-1);
                box.prob = id.score;
                box.index = id.x+id.y*W;
                box.classes = id.z+1;
                boxes.push_back(box);
            }
            return boxes;
        }
        static inline pair<int,int> index_to_yx(int index,int H,int W) {
            return make_pair(index/W,index%W);
        }
	private:
        int k_ = 0;
        float threshold_ = 1e-3;
};
REGISTER_KERNEL_BUILDER(Name("Center2BoxesDecode").Device(DEVICE_CPU).TypeConstraint<float>("T"), Center2BoxesDecodeOp<CPUDevice, float>);


/** periodic_utils.cc
    Jeremy Barnes, 4 April 2012
    Copyright (c) 2012 Datacratic.  All rights reserved.

*/

#include "periodic_utils.h"
#include "jml/utils/parse_context.h"
#include <boost/tuple/tuple.hpp>
#include "jml/utils/exc_assert.h"

using namespace std;

namespace Datacratic {

std::pair<TimeGranularity, double>
parsePeriod(const std::string & pattern)
{
    TimeGranularity granularity;
    double number;

    ML::Parse_Context context(pattern,
                              pattern.c_str(),
                              pattern.c_str() + pattern.length());

    number = context.expect_double();

    //if (number <= 0)
    //    context.exception("invalid time number: must be > 0");

    if (context.match_literal('x') || context.match_literal("ms"))
	granularity = MILLISECONDS;
    else if (context.match_literal('s'))
        granularity = SECONDS;
    else if (context.match_literal('m'))
        granularity = MINUTES;
    else if (context.match_literal('h'))
        granularity = HOURS;
    else if (context.match_literal('d'))
        granularity = DAYS;
    else if (context.match_literal('w'))
        granularity = WEEKS;
    else if (context.match_literal('M'))
        granularity = MONTHS;
    else if (context.match_literal('y'))
        granularity = YEARS;
    else context.exception("excepted h, m, s, d, w, M, y or x");

    context.expect_eof();

    return make_pair(granularity, number);
}

std::pair<Date, double>
findPeriod(Date current, const std::string & period)
{
    TimeGranularity p;
    double n;
    boost::tie(p, n) = parsePeriod(period);
    return findPeriod(current, p, n);
}

std::pair<Date, double>
findPeriod(Date current, TimeGranularity granularity, double number_)
{
    if (number_  == 0)
        return make_pair(current, 0);

    int64_t number = number_;

    // Make sure it's an integer number of seconds
    ExcAssertEqual(number, number_);

    // Find where the current period starts

    tm t = current.toTm();

    // Remove fractional seconds
    Date result = current;
    int64_t seconds = result.secondsSinceEpoch();
    result = Date::fromSecondsSinceEpoch(seconds);

    //Date unadjusted = result;

    double interval;

    switch (granularity) {
    case DAYS:
        current.quantize(3600 * 24);
        interval = 3600 * 24 * number;
        break;

        if (number != 1)
            throw ML::Exception("only 1d is supported for days");
        // Go to the start of the day
        result.addSeconds(-(3600 * t.tm_hour + 60 * t.tm_min + t.tm_sec));

        // Go to day
        interval = 3600 * 24 * number;

        break;
    case HOURS:
        // Go to the start of the hour
        result.addSeconds(-(60 * t.tm_min + t.tm_sec));

        // Go to hour number n of the day
        result.addSeconds(-3600 * (t.tm_hour % number));

        interval = 3600 * number;

        break;

    case MINUTES:
        // Go to the start of the minute
        result.addSeconds(-t.tm_sec);

        // Go to the next multiple of minutes
        result.addSeconds(-60 * (t.tm_min % number));

        interval = 60 * number;

        break;

    case SECONDS:
        //cerr << "seconds: t.tm_sec = " << t.tm_sec
        //     << " number = " << number << " before = "
        //     << result;
        result.addSeconds(-(t.tm_sec % number));
        //cerr <<" after = " << result << " current = "
        //     << current << endl;

        interval = number;
        break;

    case MILLISECONDS:
	interval = number / 1000.0;

	result = current.quantized(interval);
	if (result > current)
	    result.addSeconds(-interval);

	break;

    default:
        throw ML::Exception("that granualrity is not supported");
    }

    return make_pair(result, interval);
}

std::string
filenameFor(const Date & date, const std::string & pattern)
{
    return date.print(pattern);
}

/*****************************************************************************/
/* TIME PERIOD                                                               */
/*****************************************************************************/

TimePeriod::
TimePeriod(const std::string & periodName)
{
    parse(periodName);
}

TimePeriod::
TimePeriod(const char * periodName)
{
    parse(periodName);
}

TimePeriod::
TimePeriod(TimeGranularity granularity, double number)
    : granularity(granularity), number(number),
      interval(findPeriod(Date(), granularity, number).second)
{
}

Date
TimePeriod::
current(Date now) const
{
    return findPeriod(now, granularity, number).first;
}

Date
TimePeriod::
next(Date now) const
{
    return current(now).plusSeconds(interval);
}

std::string
TimePeriod::
toString() const
{
    string result = boost::lexical_cast<string>(number);//ML::format("%f", number);
    switch (granularity) {
    case MILLISECONDS:  result += "ms";  return result;
    case SECONDS:       result += 's';   return result;
    case MINUTES:       result += 'm';   return result;
    case HOURS:         result += 'h';   return result;
    case DAYS:          result += 'd';   return result;
    case WEEKS:         result += 'w';   return result;
    case MONTHS:        result += 'M';   return result;
    case YEARS:         result += 'y';   return result;
    default:
        throw ML::Exception("unknown time period");
    }
}

void
TimePeriod::
parse(const std::string & str)
{
    std::tie(granularity, number) = parsePeriod(str);
    interval = findPeriod(Date(), granularity, number).second;
}

} // namespace Datacratic


// -*- mode: c++; tab-width: 2; indent-tabs-mode: nil; -*-
// test domain specificity
// Written by Ulrich Germann

#include <boost/program_options.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/format.hpp>
#include <boost/math/distributions/binomial.hpp>

#include "mm/ug_bitext.h"
#include "mm/tpt_typedefs.h"
#include "mm/ug_prime_sampling1.h"
#include "mm/ug_bitext_sampler.h"
#include "mm/ug_phrasepair.h"
#include "mm/ug_lru_cache.h"
#include "generic/sorting/VectorIndexSorter.h"
#include "generic/sorting/NBestList.h"
#include <string>
#include <boost/unordered_map.hpp>
#include "moses/thread_safe_container.h"
#include "mm/ug_prep_phrases.h"

using namespace std;
using namespace Moses;
using namespace Moses::bitext;
namespace po=boost::program_options;
using namespace boost::algorithm;
typedef L2R_Token<SimpleWordId> Token;
typedef mmBitext<Token> mmbitext;
typedef Bitext<Token>::tsa tsa;
typedef Bitext<Token>::iter iter;
typedef imTtrack<Token> imttrack;
typedef imTSA<Token> imtsa;
typedef vector<PhrasePair<Token> > pplist_t;

string bname, bname1, bname2, ifile, L1, L2, Q1, Q2;
size_t maxhits;
size_t cache_size;
void interpret_args(int ac, char* av[]);

typedef PhrasePair<Token>::SortDescendingByJointCount sorter_t;
sorter_t sorter;

void
show(Bitext<Token> const& B, iter const& m, pstats& stats)
{
  pplist_t pplist;
  expand(m, B, stats, pplist, NULL);
  if (pplist.empty()) return;
  cout << "\n" << m.str(B.V1.get()) << " [" << m.ca() << "]" << endl;
  VectorIndexSorter<PhrasePair<Token>, sorter_t> viso(pplist, sorter);
  sptr<vector<size_t> > ranked = viso.GetOrder();
  size_t ctr=0;
  size_t cumul=0;
  BOOST_FOREACH(size_t const i, *ranked)
    {
      typedef map<uint32_t, uint32_t>::value_type entry_t;

      PhrasePair<Token> const& pp = pplist[i];
      if (pp.joint < pp.good1 * .01) break;
      size_t remarkable = 0;
      float p = float(pp.joint)/pp.good1;
      BOOST_FOREACH(entry_t const& e, pp.indoc)
        {
          boost::math::binomial binomi(stats.indoc[e.first], p);
          float x = boost::math::cdf(binomi, e.second);
          float y = boost::math::cdf(boost::math::complement(binomi, e.second-1));
          if ((x > .01 && y > .01) || e.second < 5) continue;
          remarkable += e.second;
          // cout <<  p * stats.indoc[e.first]
          //      << "/" << e.second << "/" << stats.indoc[e.first]
          //      << " " << boost::math::cdf(binomi, e.second)
          //      << " " << boost::math::cdf(boost::math::complement(binomi, e.second-1))
          //      << " " << toString(*B.V2, pp.start2, pp.len2)
          //      << endl;
        }
      if (remarkable*20 > pp.good1)
        {
          cout << boost::format("   %6d | ") % pp.joint
               << toString(*B.V2, pp.start2, pp.len2)
               << boost::format(" (%d: %.2f)") % cumul % (float(cumul)/pp.good1)
               << endl;
          BOOST_FOREACH(entry_t const& e, pp.indoc)
            {
              boost::math::binomial binomi(stats.indoc[e.first], p);
              float x = boost::math::cdf(binomi, e.second);
              float y = boost::math::cdf(boost::math::complement(binomi, e.second-1));
              if ((x > .001 && y > .001) || e.second < 20) continue;
              cout <<  p * stats.indoc[e.first]
                   << "/" << e.second << "/" << stats.indoc[e.first]
                   << " " << boost::math::cdf(binomi, e.second)
                   << " " << boost::math::cdf(boost::math::complement
                                              (binomi, e.second-1))
                   << " " << toString(*B.V2, pp.start2, pp.len2)
                   << endl;
            }
        }
    }
}

void
process(SPTR<Bitext<Token> const> const& bitext, TSA<Token>::tree_iterator& m)
{
  static boost::shared_ptr<SamplingBias> nil(new SamplingBiasAlways(bitext->sid2did()));
  static Moses::bitext::sampling_method random = Moses::bitext::random_sampling;
  // if (m.down())
  if (m.extend((*bitext->V1)["job"]))
    {
      do
        {
          if (m.ca() >= 5000)
            {
              // cout << m.str(bitext->V1.get()) << " [" << m.ca() << "]" << endl;
              Moses::bitext::BitextSampler<Token> s(bitext, m, nil, 10000, random);
              s();
              show(*bitext, m, *s.stats());
              process(bitext, m);
            }
        }
      while (m.over());
      m.up();
    }
}

int main(int argc, char* argv[])
{
  interpret_args(argc, argv);
  SPTR<mmbitext> B(new mmbitext);
  B->open(bname, L1, L2);
  TSA<Token>::tree_iterator m(B->I1.get());
  // m.extend((*B.V1)["job"]);
  process(B.get(), m);
}

void
interpret_args(int ac, char* av[])
{
  po::variables_map vm;
  po::options_description o("Options");
  o.add_options()
    ("help,h",  "print this message")
    ;

  po::options_description h("Hidden Options");
  h.add_options()
    ("bname", po::value<string>(&bname), "base name of corpus")
    ("L1", po::value<string>(&L1), "L1 tag")
    ("L2", po::value<string>(&L2), "L2 tag")
    ;

  h.add(o);
  po::positional_options_description a;
  a.add("bname",1);
  a.add("L1",1);
  a.add("L2",1);

  po::store(po::command_line_parser(ac,av)
            .options(h)
            .positional(a)
            .run(),vm);
  po::notify(vm);
  if (vm.count("help"))
    {
      cout << "\nusage:\n\t" << av[0]
           << " <bname> <L1> <L2>" << endl;
      cout << o << endl;
      exit(0);
    }
}


/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & onward 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 NT2_SDK_TIMING_IMPL_QUERYPERFORMANCECOUNTER_HPP_INCLUDED
#define NT2_SDK_TIMING_IMPL_QUERYPERFORMANCECOUNTER_HPP_INCLUDED

#if !defined(NT2_TIMING_DETECTED)

#if defined(_WIN32) || defined(__WIN32__) || defined(WIN32)

#define NT2_TIMING_DETECTED
#if !defined(VC_EXTRALEAN)
#define VC_EXTRALEAN
#endif

#if !defined(WIN32_LEAN_AND_MEAN)
#define WIN32_LEAN_AND_MEAN
#endif

#include <windows.h>
#include <boost/cstdint.hpp>

namespace nt2
{
  namespace details
  {
    inline double now()
    {
      union pli { LARGE_INTEGER li; double d; };
      pli freq,t;
      QueryPerformanceFrequency( &freq.li );
      QueryPerformanceCounter( &t.li );
      return (((t.d*1000000000)/freq.d)/1000000000);
    }
  }
}
#endif
#endif

#endif


#pragma once
//! c/c++ headers
//! dependency headers
#include <armadillo>
//! project headers
#include "correspondences/common/base.hpp"
#include "correspondences/common/types.hpp"
#include "correspondences/graph/graph.hpp"

namespace correspondences {
namespace mc {

/** @struct Config : CorrespondencesConfigBase
 * @var Config::algo
 * max clique algorithm implementation to call
 */
struct Config : CorrespondencesConfigBase {
  Config()
    : CorrespondencesConfigBase() {
      set_defaults();
    }

  explicit Config(nlohmann::json & config) :
    CorrespondencesConfigBase(config) {
      set_defaults();
      json_utils::check_for_param(config, "algorithm", algo);
    }

  void set_defaults() noexcept final {
    algo = graph::max_clique_algo_e::bnb_basic;
  }

  graph::max_clique_algo_e algo;
};
}  // namespace mc

/** @class MC : public CorrespondencesBase
 * @brief wrapper class definition for correspondence calculation by solving
 * a quadratic assignment problem (MC)
 */
class MC : public CorrespondencesBase {
 public:
   /** MC::MC(source_pts, target_pts, config)
    * @brief constructor for optimization wrapper class
    *
    * @param[in] source_pts distribution of (columnar) source points
    * @param[in] target_pts distribution of (columnar) target points
    * @param[in] config `Config` instance with optimization parameters; see `Config` definition
    * @return
    */
   explicit MC(arma::mat const & source_pts,
       arma::mat const & target_pts, mc::Config config)
     : config_(config)
     , graph_(source_pts, target_pts,
         config_.epsilon, config_.pairwise_dist_threshold)
     , n_(target_pts.n_cols) { }

   /** MC::~MC()
    * @brief destructor for optimization wrapper class
    *
    * @param[in]
    * @return
    */
   ~MC();

   /** MC::calc_correspondences()
    * @brief identify pairwise correspondences between source and target set given optimum found
    * during optimization
    *
    * @param[in]
    * @return solution status
    * @see status_e definition in types.hpp
    */
   status_e calc_correspondences(correspondences_t & corr) noexcept final;

   /** MC::num_consistent_pairs()
    * @brief get identified correspondences
    *
    * @param[in]
    * @return number of pairwise consistencies identified
    */
   size_t num_consistent_pairs() const noexcept { return graph_.get_edges().size(); }

 private:
   mc::Config config_;  //! initialization data struct
   graph::UndirectedGraph graph_;  // NOLINT [linelength] pairwise correspondences consistency graph
   size_t n_;  // NOLINT [linelength] no. of target points (this is for extracting correspondence pairs from vertices)
};
}  // namespace correspondences


//   GAMBIT: Global and Modular BSM Inference Tool
//   *********************************************
///  \file
///
///  Rollcall header for NeutrinoBit.
///
///  Compile-time registration of available
///  observables and likelihoods for neutrino observables.
///
///  Don't put typedefs or other type definitions
///  in this file; see
///  Core/include/types_rollcall.hpp for further
///  instructions on how to add new types.
///
///  *********************************************
///
///  Authors (add name and date if you modify):
///
///  \author Suraj Krishnamurthy
///          (S.Krishnamurthy@uva.nl)
///  \date 2017 Feb
///
///  \author Tomas Gonzalo
///          (t.e.gonzalo@fys.uio.no)
///  \date 2017 July
///  \date 2018
///  \date 2019
///
///  \author Julia Harz
///          (jharz@lpthe.jussieu.fr)
///  \date 2018 April
///  *********************************************

#ifndef __NeutrinoBit_rollcall_hpp__
#define __NeutrinoBit_rollcall_hpp__

#include <Eigen/Sparse>
#include <Eigen/Dense>

#define MODULE NeutrinoBit
START_MODULE

  #define CAPABILITY ordering
  START_CAPABILITY
    #define FUNCTION ordering
    START_FUNCTION(bool)
    ALLOW_MODEL(StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  // Neutrino mass matrix
  #define CAPABILITY m_nu
  START_CAPABILITY
    // Neutrino masss matrix
    #define FUNCTION M_nu
    START_FUNCTION(Eigen::Matrix3cd)
    DEPENDENCY(ordering, bool)
    ALLOW_MODELS(StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY md21
  START_CAPABILITY
    #define FUNCTION md21
    START_FUNCTION(double)
    DEPENDENCY(m_nu, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY md31
  START_CAPABILITY
    #define FUNCTION md31
    START_FUNCTION(double)
    DEPENDENCY(m_nu, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY md32
  START_CAPABILITY
    #define FUNCTION md32
    START_FUNCTION(double)
    DEPENDENCY(m_nu, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY min_mass
  START_CAPABILITY
    #define FUNCTION min_mass
    START_FUNCTION(double)
    DEPENDENCY(m_nu, Eigen::Matrix3cd)
    DEPENDENCY(ordering, bool)
    #undef FUNCTION
  #undef CAPABILITY

  // Construct the neutrino PMNS matrix
  #define CAPABILITY UPMNS
  START_CAPABILITY
    // Neutrino PMNS matrix in a parametrization where the charged Yukawas are diagonal
    #define FUNCTION UPMNS
    START_FUNCTION(Eigen::Matrix3cd)
    ALLOW_MODELS(StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  // Construct the neutrino Seesaw I type Theta matrix
  #define CAPABILITY SeesawI_Theta
  START_CAPABILITY
    // Theta in the Casas-Ibarra parametrization
    #define FUNCTION CI_Theta
    START_FUNCTION(Eigen::Matrix3cd)
    DEPENDENCY(m_nu, Eigen::Matrix3cd)
    DEPENDENCY(UPMNS, Eigen::Matrix3cd)
    DEPENDENCY(SMINPUTS, SMInputs)
    ALLOW_JOINT_MODEL(StandardModel_Higgs, RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY SeesawI_Vnu
  START_CAPABILITY
    #define FUNCTION Vnu
    START_FUNCTION(Eigen::Matrix3cd)
    DEPENDENCY(UPMNS, Eigen::Matrix3cd)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_MODELS(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  // Unitarity checks for the mixing matrix
  #define CAPABILITY Unitarity
  START_CAPABILITY
    // Unitarity of the PMNS matrix
    #define FUNCTION Unitarity_UPMNS
    START_FUNCTION(bool)
    DEPENDENCY(UPMNS, Eigen::Matrix3cd)
    DEPENDENCY(m_nu, Eigen::Matrix3cd)
    #undef FUNCTION

    // Unitarity of the mixing matrix in seesaw I
    #define FUNCTION Unitarity_SeesawI
    START_FUNCTION(bool)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    DEPENDENCY(SeesawI_Vnu, Eigen::Matrix3cd)
    DEPENDENCY(m_nu, Eigen::Matrix3cd)
    ALLOW_MODEL(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Ue1
  START_CAPABILITY
    #define FUNCTION Ue1
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Ue1_phase
  START_CAPABILITY
    #define FUNCTION Ue1_phase
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Um1
  START_CAPABILITY
    #define FUNCTION Um1
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Um1_phase
  START_CAPABILITY
    #define FUNCTION Um1_phase
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Ut1
  START_CAPABILITY
    #define FUNCTION Ut1
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Ut1_phase
  START_CAPABILITY
    #define FUNCTION Ut1_phase
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Ue2
  START_CAPABILITY
    #define FUNCTION Ue2
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Ue2_phase
  START_CAPABILITY
    #define FUNCTION Ue2_phase
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Um2
  START_CAPABILITY
    #define FUNCTION Um2
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Um2_phase
  START_CAPABILITY
    #define FUNCTION Um2_phase
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Ut2
  START_CAPABILITY
    #define FUNCTION Ut2
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Ut2_phase
  START_CAPABILITY
    #define FUNCTION Ut2_phase
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Ue3
  START_CAPABILITY
    #define FUNCTION Ue3
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Ue3_phase
  START_CAPABILITY
    #define FUNCTION Ue3_phase
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Um3
  START_CAPABILITY
    #define FUNCTION Um3
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Um3_phase
  START_CAPABILITY
    #define FUNCTION Um3_phase
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Ut3
  START_CAPABILITY
    #define FUNCTION Ut3
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Ut3_phase
  START_CAPABILITY
    #define FUNCTION Ut3_phase
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2pi0nu
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2pi0nu
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_MODELS(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2piplusl
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2piplusl
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_JOINT_MODEL(RightHandedNeutrinos, StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2Kplusl
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2Kplusl
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_JOINT_MODEL(RightHandedNeutrinos, StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2Dplusl
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2Dplusl
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_JOINT_MODEL(RightHandedNeutrinos, StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2Dsl
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2Dsl
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_JOINT_MODEL(RightHandedNeutrinos, StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2Bplusl
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2Bplusl
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_JOINT_MODEL(RightHandedNeutrinos, StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2Bsl
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2Bsl
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_JOINT_MODEL(RightHandedNeutrinos, StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2Bcl
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2Bcl
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_JOINT_MODEL(RightHandedNeutrinos, StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2etanu
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2etanu
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_MODEL(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2etaprimenu
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2etaprimenu
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_MODEL(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2rhoplusl
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2rhoplusl
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_JOINT_MODEL(RightHandedNeutrinos, StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2rho0nu
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2rho0nu
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_MODEL(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN23nu
  START_CAPABILITY
    #define FUNCTION Gamma_RHN23nu
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_MODEL(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2llnu
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2llnu
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_JOINT_MODEL(RightHandedNeutrinos, StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2null
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2null
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_JOINT_MODEL(RightHandedNeutrinos, StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2nuuubar
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2nuuubar
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_JOINT_MODEL(RightHandedNeutrinos, StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2nuddbar
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2nuddbar
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_JOINT_MODEL(RightHandedNeutrinos, StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_RHN2ludbar
  START_CAPABILITY
    #define FUNCTION Gamma_RHN2ludbar
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_JOINT_MODEL(RightHandedNeutrinos, StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Gamma_BBN
  START_CAPABILITY
    #define FUNCTION Gamma_BBN
    START_FUNCTION(std::vector<double>)
    DEPENDENCY(Gamma_RHN2pi0nu, std::vector<double>)
    DEPENDENCY(Gamma_RHN2piplusl, std::vector<double>)
    DEPENDENCY(Gamma_RHN2Kplusl, std::vector<double>)
    DEPENDENCY(Gamma_RHN2Dplusl, std::vector<double>)
    DEPENDENCY(Gamma_RHN2Dsl, std::vector<double>)
    DEPENDENCY(Gamma_RHN2Bplusl, std::vector<double>)
    DEPENDENCY(Gamma_RHN2Bsl, std::vector<double>)
    DEPENDENCY(Gamma_RHN2Bcl, std::vector<double>)
    DEPENDENCY(Gamma_RHN2etanu, std::vector<double>)
    DEPENDENCY(Gamma_RHN2etaprimenu, std::vector<double>)
    DEPENDENCY(Gamma_RHN2rhoplusl, std::vector<double>)
    DEPENDENCY(Gamma_RHN2rho0nu, std::vector<double>)
    DEPENDENCY(Gamma_RHN23nu, std::vector<double>)
    DEPENDENCY(Gamma_RHN2llnu, std::vector<double>)
    DEPENDENCY(Gamma_RHN2null, std::vector<double>)
    DEPENDENCY(Gamma_RHN2nuuubar, std::vector<double>)
    DEPENDENCY(Gamma_RHN2nuddbar, std::vector<double>)
    DEPENDENCY(Gamma_RHN2ludbar, std::vector<double>)
    ALLOW_MODELS(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnL_bbn
  START_CAPABILITY
    #define FUNCTION lnL_bbn
    START_FUNCTION(double)
    DEPENDENCY(Gamma_BBN, std::vector<double>)
    ALLOW_MODELS(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY R_pi
  START_CAPABILITY
    #define FUNCTION RHN_R_pi
    START_FUNCTION(double)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    DEPENDENCY(SeesawI_Vnu, Eigen::Matrix3cd)
    ALLOW_MODEL(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY R_K
  START_CAPABILITY
    #define FUNCTION RHN_R_K
    START_FUNCTION(double)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    DEPENDENCY(SeesawI_Vnu, Eigen::Matrix3cd)
    ALLOW_MODEL(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY R_tau
  START_CAPABILITY
    #define FUNCTION RHN_R_tau
    START_FUNCTION(double)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_MODEL(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY R_W
  START_CAPABILITY
    #define FUNCTION RHN_R_W
    START_FUNCTION(std::vector<double>)
    //DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    //DEPENDENCY(mw, triplet<double>)
    DEPENDENCY(W_to_l_decays, std::vector<double>)
    ALLOW_MODEL(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnL_R_K
  START_CAPABILITY
    #define FUNCTION lnL_R_K
    START_FUNCTION(double)
    DEPENDENCY(R_K, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnL_R_pi
  START_CAPABILITY
    #define FUNCTION lnL_R_pi
    START_FUNCTION(double)
    DEPENDENCY(R_pi, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnL_R_tau
  START_CAPABILITY
    #define FUNCTION lnL_R_tau
    START_FUNCTION(double)
    DEPENDENCY(R_tau, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnL_R_W
  START_CAPABILITY
    #define FUNCTION lnL_R_W
    START_FUNCTION(double)
    DEPENDENCY(R_W, std::vector<double>)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Thalf_0nubb_Xe
  START_CAPABILITY
    #define FUNCTION RHN_Thalf_0nubb_Xe
    START_FUNCTION(double)
    DEPENDENCY(m_nu, Eigen::Matrix3cd)
    DEPENDENCY(UPMNS, Eigen::Matrix3cd)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_MODEL(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnL_0nubb_KamLAND_Zen
  START_CAPABILITY
    #define FUNCTION lnL_0nubb_KamLAND_Zen
    START_FUNCTION(double)
    DEPENDENCY(Thalf_0nubb_Xe, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY Thalf_0nubb_Ge
  START_CAPABILITY
    #define FUNCTION RHN_Thalf_0nubb_Ge
    START_FUNCTION(double)
    DEPENDENCY(m_nu, Eigen::Matrix3cd)
    DEPENDENCY(UPMNS, Eigen::Matrix3cd)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_MODEL(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnL_0nubb_GERDA
  START_CAPABILITY
    #define FUNCTION lnL_0nubb_GERDA
    START_FUNCTION(double)
    DEPENDENCY(Thalf_0nubb_Ge, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnL_0nubb
  START_CAPABILITY
    #define FUNCTION lnL_0nubb
    START_FUNCTION(double)
    DEPENDENCY(lnL_0nubb_KamLAND_Zen, double)
    DEPENDENCY(lnL_0nubb_GERDA, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY mbb_0nubb_Xe
  START_CAPABILITY
    #define FUNCTION RHN_mbb_0nubb_Xe
    START_FUNCTION(double)
    DEPENDENCY(m_nu, Eigen::Matrix3cd)
    DEPENDENCY(UPMNS, Eigen::Matrix3cd)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_MODEL(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnL_mbb_0nubb_KamLAND_Zen
  START_CAPABILITY
    #define FUNCTION lnL_mbb_0nubb_KamLAND_Zen
    START_FUNCTION(double)
    DEPENDENCY(mbb_0nubb_Xe, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY mbb_0nubb_Ge
  START_CAPABILITY
    #define FUNCTION RHN_mbb_0nubb_Ge
    START_FUNCTION(double)
    DEPENDENCY(m_nu, Eigen::Matrix3cd)
    DEPENDENCY(UPMNS, Eigen::Matrix3cd)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_MODEL(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnL_mbb_0nubb_GERDA
  START_CAPABILITY
    #define FUNCTION lnL_mbb_0nubb_GERDA
    START_FUNCTION(double)
    DEPENDENCY(mbb_0nubb_Ge, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnL_mbb_0nubb
  START_CAPABILITY
    #define FUNCTION lnL_mbb_0nubb
    START_FUNCTION(double)
    DEPENDENCY(lnL_mbb_0nubb_KamLAND_Zen, double)
    DEPENDENCY(lnL_mbb_0nubb_GERDA, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY calc_Vus
  START_CAPABILITY
    #define FUNCTION calc_Vus
    START_FUNCTION(double)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_MODEL(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnLckm_Vusmin
  START_CAPABILITY
    #define FUNCTION lnL_ckm_Vusmin
    START_FUNCTION(double)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(calc_Vus, double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_MODELS(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnLckm_Vus
  START_CAPABILITY
    #define FUNCTION lnL_ckm_Vus
    START_FUNCTION(double)
    DEPENDENCY(SMINPUTS, SMInputs)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    ALLOW_JOINT_MODEL(RightHandedNeutrinos, StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnLpienu
  START_CAPABILITY
    #define FUNCTION lnL_pienu
    START_FUNCTION(double)
    ALLOW_MODEL(RightHandedNeutrinos)
    DEPENDENCY(Ue1, double)
    DEPENDENCY(Ue2, double)
    DEPENDENCY(Ue3, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnLps191e
  START_CAPABILITY
    #define FUNCTION lnL_ps191_e
    START_FUNCTION(double)
    ALLOW_MODEL(RightHandedNeutrinos)
    DEPENDENCY(Ue1, double)
    DEPENDENCY(Ue2, double)
    DEPENDENCY(Ue3, double)
    DEPENDENCY(Um1, double)
    DEPENDENCY(Um2, double)
    DEPENDENCY(Um3, double)
    DEPENDENCY(Ut1, double)
    DEPENDENCY(Ut2, double)
    DEPENDENCY(Ut3, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnLps191mu
  START_CAPABILITY
    #define FUNCTION lnL_ps191_mu
    START_FUNCTION(double)
    ALLOW_MODEL(RightHandedNeutrinos)
    DEPENDENCY(Ue1, double)
    DEPENDENCY(Ue2, double)
    DEPENDENCY(Ue3, double)
    DEPENDENCY(Um1, double)
    DEPENDENCY(Um2, double)
    DEPENDENCY(Um3, double)
    DEPENDENCY(Ut1, double)
    DEPENDENCY(Ut2, double)
    DEPENDENCY(Ut3, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnLcharme
  START_CAPABILITY
    #define FUNCTION lnL_charm_e
    START_FUNCTION(double)
    ALLOW_MODEL(RightHandedNeutrinos)
    DEPENDENCY(Ue1, double)
    DEPENDENCY(Ue2, double)
    DEPENDENCY(Ue3, double)
    DEPENDENCY(Um1, double)
    DEPENDENCY(Um2, double)
    DEPENDENCY(Um3, double)
    DEPENDENCY(Ut1, double)
    DEPENDENCY(Ut2, double)
    DEPENDENCY(Ut3, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnLcharmmu
  START_CAPABILITY
    #define FUNCTION lnL_charm_mu
    START_FUNCTION(double)
    ALLOW_MODEL(RightHandedNeutrinos)
    DEPENDENCY(Ue1, double)
    DEPENDENCY(Ue2, double)
    DEPENDENCY(Ue3, double)
    DEPENDENCY(Um1, double)
    DEPENDENCY(Um2, double)
    DEPENDENCY(Um3, double)
    DEPENDENCY(Ut1, double)
    DEPENDENCY(Ut2, double)
    DEPENDENCY(Ut3, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnLdelphi_shortlived
  START_CAPABILITY
    #define FUNCTION lnL_delphi_short_lived
    START_FUNCTION(double)
    ALLOW_MODEL(RightHandedNeutrinos)
    DEPENDENCY(Ue1, double)
    DEPENDENCY(Ue2, double)
    DEPENDENCY(Ue3, double)
    DEPENDENCY(Um1, double)
    DEPENDENCY(Um2, double)
    DEPENDENCY(Um3, double)
    DEPENDENCY(Ut1, double)
    DEPENDENCY(Ut2, double)
    DEPENDENCY(Ut3, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnLdelphi_longlived
  START_CAPABILITY
    #define FUNCTION lnL_delphi_long_lived
    START_FUNCTION(double)
    ALLOW_MODEL(RightHandedNeutrinos)
    DEPENDENCY(Ue1, double)
    DEPENDENCY(Ue2, double)
    DEPENDENCY(Ue3, double)
    DEPENDENCY(Um1, double)
    DEPENDENCY(Um2, double)
    DEPENDENCY(Um3, double)
    DEPENDENCY(Ut1, double)
    DEPENDENCY(Ut2, double)
    DEPENDENCY(Ut3, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnLatlase
  START_CAPABILITY
    #define FUNCTION lnL_atlas_e
    START_FUNCTION(double)
    ALLOW_MODEL(RightHandedNeutrinos)
    DEPENDENCY(Ue1, double)
    DEPENDENCY(Ue2, double)
    DEPENDENCY(Ue3, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnLatlasmu
  START_CAPABILITY
    #define FUNCTION lnL_atlas_mu
    START_FUNCTION(double)
    ALLOW_MODEL(RightHandedNeutrinos)
    DEPENDENCY(Um1, double)
    DEPENDENCY(Um2, double)
    DEPENDENCY(Um3, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnLe949
  START_CAPABILITY
    #define FUNCTION lnL_e949
    START_FUNCTION(double)
    ALLOW_MODEL(RightHandedNeutrinos)
    DEPENDENCY(Um1, double)
    DEPENDENCY(Um2, double)
    DEPENDENCY(Um3, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnLnutev
  START_CAPABILITY
    #define FUNCTION lnL_nutev
    START_FUNCTION(double)
    ALLOW_MODEL(RightHandedNeutrinos)
    DEPENDENCY(Um1, double)
    DEPENDENCY(Um2, double)
    DEPENDENCY(Um3, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnLcharmtau
  START_CAPABILITY
    #define FUNCTION lnL_charm_tau
    START_FUNCTION(double)
    ALLOW_MODEL(RightHandedNeutrinos)
    DEPENDENCY(Ut1, double)
    DEPENDENCY(Ut2, double)
    DEPENDENCY(Ut3, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnLlhce
  START_CAPABILITY
    #define FUNCTION lnL_lhc_e
    START_FUNCTION(double)
    ALLOW_MODEL(RightHandedNeutrinos)
    DEPENDENCY(Ue1, double)
    DEPENDENCY(Ue2, double)
    DEPENDENCY(Ue3, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY lnLlhcmu
  START_CAPABILITY
    #define FUNCTION lnL_lhc_mu
    START_FUNCTION(double)
    ALLOW_MODEL(RightHandedNeutrinos)
    DEPENDENCY(Um1, double)
    DEPENDENCY(Um2, double)
    DEPENDENCY(Um3, double)
    #undef FUNCTION
  #undef CAPABILITY

  // Perturbativity of the Yukawa couplings (from arXiv:1509.02678)
  #define CAPABILITY perturbativity_lnL
  START_CAPABILITY
    #define FUNCTION perturbativity_likelihood
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    DEPENDENCY(SMINPUTS, SMInputs)
    ALLOW_MODEL(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

  // Active neutrino likelihoods
  #define CAPABILITY theta12
  START_CAPABILITY
    #define FUNCTION theta12
    START_FUNCTION(double)
    ALLOW_MODEL(StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY theta12_lnL
  START_CAPABILITY
    #define FUNCTION theta12_lnL
    START_FUNCTION(double)
    DEPENDENCY(ordering, bool)
    DEPENDENCY(theta12, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY theta23
  START_CAPABILITY
    #define FUNCTION theta23
    START_FUNCTION(double)
    ALLOW_MODEL(StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY theta23_lnL
  START_CAPABILITY
    #define FUNCTION theta23_lnL
    START_FUNCTION(double)
    DEPENDENCY(ordering, bool)
    DEPENDENCY(theta23, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY theta13
  START_CAPABILITY
    #define FUNCTION theta13
    START_FUNCTION(double)
    ALLOW_MODEL(StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY theta13_lnL
  START_CAPABILITY
    #define FUNCTION theta13_lnL
    START_FUNCTION(double)
    DEPENDENCY(ordering, bool)
    DEPENDENCY(theta13, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY deltaCP
  START_CAPABILITY
    #define FUNCTION deltaCP
    START_FUNCTION(double)
    ALLOW_MODEL(StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY deltaCP_lnL
  START_CAPABILITY
    #define FUNCTION deltaCP_lnL
    START_FUNCTION(double)
    DEPENDENCY(ordering, bool)
    DEPENDENCY(deltaCP, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY md21_lnL
  START_CAPABILITY
    #define FUNCTION md21_lnL
    START_FUNCTION(double)
    DEPENDENCY(ordering, bool)
    DEPENDENCY(md21, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY md3l_lnL
  START_CAPABILITY
    #define FUNCTION md3l_lnL
    START_FUNCTION(double)
    DEPENDENCY(ordering, bool)
    DEPENDENCY(md31, double)
    DEPENDENCY(md32, double)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY sum_mnu_lnL
  START_CAPABILITY
    #define FUNCTION sum_mnu_lnL
    START_FUNCTION(double)
    ALLOW_MODEL(StandardModel_SLHA2)
    #undef FUNCTION
  #undef CAPABILITY

  #define CAPABILITY RHN_coupling_slide
  START_CAPABILITY
    #define FUNCTION coupling_slide
    START_FUNCTION(double)
    DEPENDENCY(SeesawI_Theta, Eigen::Matrix3cd)
    DEPENDENCY(Ut1, double)
    DEPENDENCY(Ut2, double)
    DEPENDENCY(Ut3, double)
    ALLOW_MODELS(RightHandedNeutrinos)
    #undef FUNCTION
  #undef CAPABILITY

#undef MODULE

#endif /* defined(__NeutrinoBit_rollcall_hpp__) */


#include <boost/test/unit_test.hpp>
#include <iostream>
#include <test/setup_common.h>
#include <throttle.h>

BOOST_FIXTURE_TEST_SUITE(throttle_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(test_quota_usage)
{
    const double units_per_sec = 1000;
    const unsigned int wait_ms = 100;
    uint32_t expected_quota = (units_per_sec * wait_ms / 1000);

    Throttle throttle(units_per_sec);

    // Start with zero quota
    auto quota = throttle.GetQuota();
    BOOST_CHECK(quota == 0);

    // Accumulate quota over time
    std::this_thread::sleep_for(std::chrono::milliseconds(wait_ms));

    // Get the accumulated quota
    quota = throttle.GetQuota();
    BOOST_CHECK(quota == expected_quota);

    // Use the full quota
    BOOST_CHECK(throttle.UseQuota(quota));
    quota = throttle.GetQuota();
    BOOST_CHECK(quota == 0);

    // Accumulate quota once again
    std::this_thread::sleep_for(std::chrono::milliseconds(wait_ms));
    quota = throttle.GetQuota();
    BOOST_CHECK(quota == expected_quota);

    // Use only half of the quota
    BOOST_CHECK(throttle.UseQuota(quota / 2));

    // Check that the other half remains
    expected_quota = quota / 2;
    quota = throttle.GetQuota();
    BOOST_CHECK(quota == expected_quota);
}

BOOST_AUTO_TEST_CASE(test_quota_capping)
{
    const double units_per_sec = 20000;
    const double max_quota = 100;
    const unsigned int wait_ms = 10;

    Throttle throttle(units_per_sec);
    throttle.SetMaxQuota(max_quota);

    std::this_thread::sleep_for(std::chrono::milliseconds(wait_ms));
    uint32_t uncapped_quota = (units_per_sec * wait_ms / 1000);
    BOOST_CHECK(!throttle.HasQuota(uncapped_quota));
    BOOST_CHECK(throttle.HasQuota(max_quota));
}

BOOST_AUTO_TEST_CASE(test_quota_wait_estimate)
{
    const double units_per_sec = 100;
    const uint32_t target_wait_ms = 10;
    const uint32_t expected_quota = (units_per_sec * target_wait_ms / 1000);

    Throttle throttle(units_per_sec);

    for (int i = 0; i < 10; i++) {
        // Predict the wait to accumulate the expected quota
        const uint32_t wait_ms = throttle.EstimateWait(expected_quota);
        BOOST_CHECK((wait_ms == target_wait_ms) ||
                    (wait_ms == (target_wait_ms - 1)));
        // Given that the quota accumulates continuously as a fractional number
        // and only integer quotas can be consumed, eventually the wait may be
        // one millisecond shorter.

        // Wait and accumulate
        std::this_thread::sleep_for(std::chrono::milliseconds(wait_ms));

        // Now there should be no need to wait any longer
        BOOST_CHECK(throttle.EstimateWait(expected_quota) == 0);
        BOOST_CHECK(throttle.UseQuota(expected_quota));
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include "main.h"
#include "AffineKalmanLP.h"
#include "cvErrorRedirector.h"
#include <vector>
#include <cmath>
#include <boost/tuple/tuple.hpp>

#include "gnuplot-iostream.h"

#define movement_detect         1
//#define INPUTFILE               "GOPR4290_SC.MP4"
//#define INPUTFILE               "GOPR4291_SC.MP4"
//#define INPUTFILE               "GOPR4288.MP4"
//#define INPUTFILE               "GOPR4292_SC.MP4"
//#define INPUTFILE               "GOPR4294_SC.MP4"
//#define INPUTFILE               "IMG_2730.mp4"
#define INPUTFILE               "IMG_2731_SC_2.mp4"
#define OUTPUTFILE              "stabilized_output.avi"

const int HORIZONTAL_BORDER_CROP = 30;

int main(int argc, char **argv)
{
    cvErrorRedirector redir;

    Gnuplot gp;
    // For debugging or manual editing of commands:
    //Gnuplot gp(std::fopen("plot.gnu"));
    // or
    //Gnuplot gp("tee plot.gnu | gnuplot -persist");

    std::vector<std::pair<double, double> > xy_pts_A;
    gp << "set autoscale xfix\nset autoscale yfix\n";
    //gp << "plot '-' with lines title 'translation'\n";
    gp << "plot" << gp.file1d(xy_pts_A) << "with lines title 'cubic'\n";

    //Create a object of stabilization class
    AKLP_VideoStab *stab = new AKLP_VideoStab();

    //Initialize the VideoCapture object
    cv::VideoCapture cap(INPUTFILE);

    cv::Mat frame_2, frame2;
    cv::Mat frame_1, frame1;

    cap >> frame_1;
    cv::cvtColor(frame_1, frame1, cv::COLOR_BGR2GRAY);

    cv::Mat smoothedMat(2,3,CV_64F);

    #ifdef OUTPUTFILE
        cv::VideoWriter outputVideo;
        bool isVideoWriterInit = false;
    #endif

        static int tryes = 10;
    while(true)
    {
        static int num_of_frames = 0;
        try {
            std::cout << "Start: " << ++num_of_frames << std::endl;

            if(num_of_frames == 17){
                num_of_frames = 17;
            }

            std::cout << "CP: 1" << std::endl;
            cap >> frame_2;

            std::cout << "CP: 2" << std::endl;
            if(frame_2.data == NULL)
            {
                //delete stab;
                // It's a good idea to reset path parameters when there's an exception.
                // Unless you want to put something graceful
                //stab = new AKLP_VideoStab();
                cap >> frame_1;
                cv::cvtColor(frame_1, frame1, cv::COLOR_BGR2GRAY);

                if(tryes == 0)
                    break;
                else {
                    tryes--;
                    continue;
                }
            }

            std::cout << "CP: 3" << std::endl;
            cv::cvtColor(frame_2, frame2, cv::COLOR_BGR2GRAY);

            std::cout << "CP: 4" << std::endl;
            cv::Mat smoothedFrame;

            std::cout << "CP: 5" << std::endl;
            smoothedFrame = stab->stabilize(frame_1 , frame_2);

            std::cout << "CP: 6" << std::endl;

            std::cout << "CP: 7" << std::endl;
            cv::Mat resized_smoothedFrame;

            std::cout << "CP: 8" << std::endl;
            cv::resize(smoothedFrame, resized_smoothedFrame,  cv::Size(1440, 1024));

            static float p_diff_x_acc = 0;
            static float p_diff_y_acc = 0;
            p_diff_x_acc += stab->p_diff_x;
            p_diff_y_acc += stab->p_diff_y;
            xy_pts_A.push_back(std::make_pair(-p_diff_x_acc, p_diff_y_acc));

            gp.send1d(xy_pts_A);
            gp << "plot" << gp.file1d(xy_pts_A) << "with lines title 'cubic'\n";
            //gp << "reread\n";

            std::ostringstream stringStream;
            stringStream << "transx: " + std::to_string(p_diff_x_acc) + ", transy: " + std::to_string(p_diff_y_acc);
            std::string copyOfStr = stringStream.str();
            //std::string("transx: " + std::to_string(stab->transX) + ", transy: " + std::to_string(stab->transY)),
            if(movement_detect) {
            cv::putText(resized_smoothedFrame,
                copyOfStr,
                cv::Point(5, 40), // Coordinates
                cv::FONT_HERSHEY_COMPLEX_SMALL, // Font
                1.0, // Scale. 2.0 = 2x bigger
                cv::Scalar(255,255,255), // BGR Color
                1, // Line Thickness (Optional)
                8,
                false); // Anti-alias (Optional)
            }

            std::cout << "CP: 9" << std::endl;
            cv::imshow("Stabilized Video" , resized_smoothedFrame);

            #ifdef OUTPUTFILE
                if(isVideoWriterInit == false) {
                    isVideoWriterInit = true;
                    outputVideo.open(OUTPUTFILE , CV_FOURCC('X' , 'V' , 'I' , 'D'), 30 , frame_1.size());
                }
                outputVideo.write(resized_smoothedFrame);
            #endif

            std::cout << "CP: 10" << std::endl;
            cv::waitKey(10);

            std::cout << "CP: 11" << std::endl;
            frame_1 = frame_2.clone();

            std::cout << "CP: 12" << std::endl;
            frame2.copyTo(frame1);

            std::cout << "End: " << num_of_frames << std::endl;
        } catch (cv::Exception& e) {
            delete stab;
            // It's a good idea to reset path parameters when there's an exception.
            // Unless you want to put something graceful
            stab = new AKLP_VideoStab();
            cap >> frame_1;
            cv::cvtColor(frame_1, frame1, cv::COLOR_BGR2GRAY);
        }

    }

    return 0;
}


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

#ifndef __MPC_HPP__
#define __MPC_HPP__

#include <iostream>
#include <vector>
#include <Eigen/Core>
#include <Eigen/QR>
#include <Eigen/Dense>

#include "bfgs.hpp"

using Eigen::MatrixXd;
using Eigen::VectorXd;

#define PI 3.1415926

struct MPC_Setting
{
	double Lf; // COG to front wheel
	double Lr; // COG to rear wheel

	double dt; // sample time
	size_t N;  // horizon

	size_t n; // dimension of state
	size_t m; // dimension of input

	MPC_Setting()
	{
		Lf = 1.17;
		Lr = 1.77;

		dt = 1 / 100.;
		N = 25;

		n = 6;
		m = 2;
	}
};

struct MPC_Argument
{
	double x, y, psi, v, cte, epsi; // initial state
	double ref_v;
	double latency;
	double coeffs[3];

	MPC_Argument()
	{
		x = y = psi = v = cte = epsi = 0.;
		ref_v = 40 / 3.6;
		latency = 1 / 1000. * 10;
	}
};

double getFitness(const MPC_Setting &setting, const MPC_Argument &arg, const VectorXd &input);
void getPrediction(const MPC_Setting &setting, const MPC_Argument &arg, const VectorXd &input, std::vector<float> &result);
void getGradient(VectorXd &grad, const MPC_Setting &setting, const MPC_Argument &arg, VectorXd &input, const double fit);
void getHess(MatrixXd &hess, const MPC_Setting &setting, const MPC_Argument &arg, VectorXd &input, const double fit);

inline double polyval(const double *coeffs, double x)
{
	return coeffs[0] + coeffs[1] * x + coeffs[2] * x * x;
}

inline double pow2(double x)
{
	return x * x;
}

struct Motion_Model : public NonConstraintObj
{
	MPC_Argument m_arg;
	MPC_Setting m_setting;

	Motion_Model() {}

	double cost(VectorXd const &x)
	{
		return getFitness(m_setting, m_arg, x);
	}
	void grad(VectorXd &diff, VectorXd &x)
	{
		double fit = cost(x);
		getGradient(diff, m_setting, m_arg, x, fit);
	}
};

#endif


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef QCTOOL_SNP_SUMMARY_COMPONENT_HWE_COMPUTATION_HPP
#define QCTOOL_SNP_SUMMARY_COMPONENT_HWE_COMPUTATION_HPP

#include <string>
#include <vector>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <boost/math/distributions/chi_squared.hpp>
#include "genfile/Error.hpp"
#include "metro/likelihood/Multinomial.hpp"
#include "components/SNPSummaryComponent/SNPHWE.hpp"
#include "components/SNPSummaryComponent/SNPSummaryComputation.hpp"

namespace snp_summary_component {
	struct HWEComputation: public SNPSummaryComputation
	{
		HWEComputation() ;

		void operator()( VariantIdentifyingData const& snp, Genotypes const& genotypes, Ploidy const& ploidy, genfile::VariantDataReader&, ResultCallback callback ) ;
		std::string get_summary( std::string const& prefix = "", std::size_t column_width = 20 ) const ;

	private:
		void autosomal_test( VariantIdentifyingData const& snp, Genotypes const& genotypes, ResultCallback callback ) ;
		void autosomal_exact_test( VariantIdentifyingData const& snp, Eigen::VectorXd const& genotype_counts, ResultCallback callback ) ;
		void autosomal_multinomial_test( VariantIdentifyingData const& snp, Eigen::VectorXd const& genotype_counts, ResultCallback callback ) ;
		void X_chromosome_test( VariantIdentifyingData const& snp, Genotypes const& genotypes, Ploidy const& ploidy, ResultCallback callback ) ;

	private:
		double const m_threshhold ;
		boost::math::chi_squared_distribution< double > m_chi_squared_1df ;
		boost::math::chi_squared_distribution< double > m_chi_squared_2df ;
	} ;
}

#endif


#ifndef TVTML_FUNCTIONAL2D_HPP
#define TVTML_FUNCTIONAL2D_HPP

// Eigen includes
#include <Eigen/Core>
#include <Eigen/Sparse>

// video++ includes
#include <vpp/vpp.hh>

// system includes
#include <cmath>
#include <iostream>
#include <fstream>
#include <vector>

namespace tvmtl{

template <enum FUNCTIONAL_DISC disc, typename MANIFOLD, class DATA>
class Functional<FIRSTORDER, disc, MANIFOLD, DATA>{

    public:
	// Manifold typedefs and constants
	static const MANIFOLD_TYPE mf_type = MANIFOLD::MyType;
	static const int value_dim = MANIFOLD::value_dim;
	static const int manifold_dim = MANIFOLD::manifold_dim;
	typedef typename MANIFOLD::scalar_type scalar_type;
	typedef typename MANIFOLD::value_type value_type;
	typedef typename MANIFOLD::ref_type ref_type;
	typedef typename MANIFOLD::cref_type cref_type;
	typedef typename MANIFOLD::deriv1_type deriv1_type;
	typedef typename MANIFOLD::deriv2_type deriv2_type;
	typedef typename MANIFOLD::restricted_deriv2_type restricted_deriv2_type;
	typedef typename MANIFOLD::tm_base_type tm_base_type;

	// Data typedef and constants
	static const int img_dim = DATA::img_dim;
	typedef typename DATA::storage_type img_type;
	typedef typename DATA::weights_type weights_type;
	typedef typename DATA::weights_mat weights_mat;
	typedef typename DATA::inp_mat inp_mat;

	typedef vpp::box_nbh2d<value_type,3,3> nbh_type;

	// Functional parameters and return types
	static const FUNCTIONAL_DISC disc_type;
	typedef double param_type;
	typedef double result_type;

	// Tangent space transformation matrix types
	typedef vpp::imageNd<tm_base_type, img_dim> tm_base_mat_type;

	// Gradient and Hessian types
	typedef Eigen::Matrix<scalar_type, Eigen::Dynamic, 1> gradient_type;
	typedef vpp::imageNd<deriv2_type, img_dim> hessian_type;
	typedef Eigen::SparseMatrix<scalar_type> sparse_hessian_type;

	//Constructor
	Functional(param_type lambda, DATA& dat):
	    lambda_(lambda),
	    data_(dat)
	{
	    eps2_=1e-10;
	   static_assert(img_dim == 2, "Dimension of data and functional must match!");
	}

	void updateWeights();

	void updateTMBase();

	// Evaluation functions
	result_type evaluateJ();
	void  evaluateDJ();
	void  evaluateHJ();

	template <class IMG>
	void output_img(const IMG& img, const char* filename) const;
	template <class IMG>
	void output_matval_img(const IMG& img, const char* filename) const;

	// Getter and Setter
	inline param_type getlambda() const { return lambda_; }
	inline void setlambda(param_type lam) { lambda_=lam; }
	inline param_type geteps2() const { return eps2_; }
	inline void seteps2(param_type eps) { eps2_=eps; }

	inline const weights_mat& getweightsX() const { return weightsX_; }
	inline const weights_mat& getweightsY() const { return weightsY_; }

	inline const gradient_type& getDJ() const { return DJ_; }
	inline const sparse_hessian_type& getHJ() const { return HJ_; }
	inline const tm_base_mat_type& getT() const { return T_; }

    private:
	DATA& data_;

	param_type lambda_, eps2_;
	weights_mat weightsX_, weightsY_;

	tm_base_mat_type T_;
	gradient_type DJ_;
	sparse_hessian_type HJ_;
};

//--------Implementation FIRSTORDER-----/

template <enum FUNCTIONAL_DISC disc, typename MANIFOLD, class DATA >
const FUNCTIONAL_DISC Functional<FIRSTORDER, disc, MANIFOLD, DATA >::disc_type = disc;

// Update the Weights 2D
template <enum FUNCTIONAL_DISC disc, typename MANIFOLD, class DATA >
void Functional<FIRSTORDER, disc, MANIFOLD, DATA >::updateWeights(){

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t Update Weights..." << std::endl;
    #endif

    int nr = data_.img_.nrows();
    int nc = data_.img_.ncols();

    // Subimage boxes
    vpp::box2d without_last_col(vpp::vint2(0,0), vpp::vint2(nr-1, nc-2)); // subdomain without last column
    vpp::box2d without_first_col(vpp::vint2(0,1), vpp::vint2(nr-1, nc-1)); // subdomain without first column
    vpp::box2d without_last_row(vpp::vint2(0,0), vpp::vint2(nr-2, nc-1)); // subdomain without last row
    vpp::box2d without_first_row(vpp::vint2(1,0), vpp::vint2(nr-1, nc-1)); // subdomain without first row

    weightsX_ = weights_mat(data_.img_.domain());
    weightsY_ = weights_mat(data_.img_.domain());

    auto calc_dist = [&] (weights_type& w, const value_type i, const value_type n) {
	w = MANIFOLD::dist_squared(i, n);
    };

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...Horizontal neighbours " << std::endl;
    #endif

    // Horizontal Neighbours
    vpp::fill(weightsX_, 0.0);
    vpp::pixel_wise(weightsX_ | without_last_col, data_.img_ | without_last_col, data_.img_ | without_first_col )(/*vpp::_no_threads*/) | calc_dist;

    #ifdef TV_FUNC_DEBUG
	data_.output_weights(weightsX_,"XWeights.csv");
    #endif
    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...Vertical neighbours" << std::endl;
    #endif

    // Vertical Neighbours
    vpp::fill(weightsY_, 0.0);
    vpp::pixel_wise(weightsY_ | without_last_row, data_.img_ | without_last_row, data_.img_ | without_first_row )(/*vpp::_no_threads*/) | calc_dist;

    #ifdef TV_FUNC_DEBUG
	data_.output_weights(weightsY_,"YWeights.csv");
    #endif

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...Reweighting" << std::endl;
    #endif

    if(disc==ISO){
	auto g =  [&] (const weights_type& ew, weights_type& x, weights_type& y) { x = ew / std::sqrt(x+y+eps2_); y = x; };
	vpp::pixel_wise(data_.edge_weights_, weightsX_, weightsY_) | g ;
    }
    else{
	auto g =  [&] (const weights_type& ew, weights_type& w) { w = ew / std::sqrt(w+eps2_); };
	vpp::pixel_wise(data_.edge_weights_, weightsX_) | g ;
	vpp::pixel_wise(data_.edge_weights_, weightsY_) | g ;
    }
}

// Update the Tangent space ONB
template <enum FUNCTIONAL_DISC disc, typename MANIFOLD, class DATA >
void Functional<FIRSTORDER, disc, MANIFOLD, DATA >::updateTMBase(){

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\tUpdate tangent space basis..." << std::endl;
    #endif

   tm_base_mat_type T(data_.img_.domain());
   vpp::pixel_wise(T, data_.img_)(/*vpp::_no_threads*/) | [&] (tm_base_type& t, const value_type& i) { MANIFOLD::tangent_plane_base(i,t); };
   T_=T;

    #ifdef TV_FUNC_DEBUG
        output_img(T_,"T.csv");
    #endif
}

// Evaluation of J
template <enum FUNCTIONAL_DISC disc, typename MANIFOLD, class DATA >
typename Functional<FIRSTORDER, disc, MANIFOLD, DATA >::result_type Functional<FIRSTORDER, disc, MANIFOLD, DATA >::evaluateJ(){

    // sum d^2(img, img_noise)
    result_type J1, J2;
    J1 = J2 = 0.0;

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\tFunctional evaluation..." << std::endl;
	std::cout << "\t\t...Fidelity part" << std::endl;
    #endif

    if(data_.doInpaint()){
	auto f = [] (const value_type& i, const value_type& n, const bool inp ) { return MANIFOLD::dist_squared(i,n)*(1-inp); };
	J1 = vpp::sum(vpp::pixel_wise(data_.img_, data_.noise_img_, data_.inp_) | f);
    }
    else{
	auto f = [] (const value_type& i, const value_type& n) { return MANIFOLD::dist_squared(i,n); };
	J1 = vpp::sum(vpp::pixel_wise(data_.img_, data_.noise_img_)(/*vpp::_no_threads*/)| f);
    }

	updateWeights();

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...TV part." << std::endl;
    #endif

    if(disc==ISO)
	J2 = vpp::sum( vpp::pixel_wise(weightsX_) | [&] (const weights_type& w) {return 1.0/w;} );
    else
	J2 = vpp::sum( vpp::pixel_wise(weightsX_, weightsY_) | [&] (const weights_type& wx, const weights_type& wy) {return 1.0/wx +1.0/wy;} );
    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "J1: " << J1 << std::endl;
	std::cout << "J2: " << J2 << std::endl;
    #endif

    return 0.5 * J1 + lambda_* J2;
}

// Evaluation of J'
template <enum FUNCTIONAL_DISC disc, typename MANIFOLD, class DATA >
void Functional<FIRSTORDER, disc, MANIFOLD, DATA >::evaluateDJ(){

    #ifdef TV_FUNC_WONES_DEBUG
	output_img(data_.img_,"img.csv");
	vpp::fill(weightsX_, 1.0); // Reset for Debugging
	vpp::fill(weightsY_, 1.0); // Reset for Debugging
    #endif

    img_type grad = img_type(data_.img_.domain());
    int nr = data_.img_.nrows();
    int nc = data_.img_.ncols();

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\tGradient evaluation..." << std::endl;
	std::cout << "\t\t...Fidelity part" << std::endl;
    #endif
    //GRADIENT OF FIDELITY TERM
    if(data_.doInpaint()){
	auto f = [] (value_type& g, const value_type& i, const value_type& n, const bool inp ) { MANIFOLD::deriv1x_dist_squared(i,n,g); g*=(1-inp); };
	vpp::pixel_wise(grad, data_.img_, data_.noise_img_, data_.inp_) | f;
    }
    else{
	auto f = [] (value_type& g, const value_type& i, const value_type& n) { MANIFOLD::deriv1x_dist_squared(i,n,g); };
	vpp::pixel_wise(grad, data_.img_, data_.noise_img_)(/*vpp::_no_threads*/) | f;
    }

    //GRADIENT OF TV TERM

    // Subimage boxes
    vpp::box2d without_last_col(vpp::vint2(0,0), vpp::vint2(nr-1, nc-2)); // subdomain without last column
    vpp::box2d without_first_col(vpp::vint2(0,1), vpp::vint2(nr-1, nc-1)); // subdomain without first column
    vpp::box2d without_last_row(vpp::vint2(0,0), vpp::vint2(nr-2, nc-1)); // subdomain without last row
    vpp::box2d without_first_row(vpp::vint2(1,0), vpp::vint2(nr-1, nc-1)); // subdomain without first row

    auto calc_first_arg_deriv = [&] (value_type& x, const weights_type& w, const value_type& i, const value_type& n) { MANIFOLD::deriv1x_dist_squared(i, n, x); x *= w; };
    auto calc_second_arg_deriv = [&] (value_type& y, const weights_type& w, const value_type& i, const value_type& n) { MANIFOLD::deriv1y_dist_squared(i, n, y); y *= w; };
    auto add_to_gradient = [&] (value_type& g, const value_type& d) { g+=d*lambda_; };

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\tGradient evaluation..." << std::endl;
	std::cout << "\t\t...TV part" << std::endl;
	std::cout << "\t\t...-> XD1" << std::endl;
    #endif
    // Horizontal derivatives and weighting
    // ... w.r.t. to first argument
    { // Temporary image XD1 is deallocated after this scope
	img_type XD1 = img_type(without_last_col);
	vpp::pixel_wise(XD1, weightsX_ | without_last_col, data_.img_ | without_last_col, data_.img_ | without_first_col) | calc_first_arg_deriv;
	#ifdef TV_FUNC_DEBUG
	    output_img(XD1,"XD1.csv");
	#endif
	auto grad_subX1  = grad | without_last_col;
	vpp::pixel_wise(grad_subX1, XD1) | add_to_gradient;
    }
    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...-> XD2" << std::endl;
    #endif
    // ... w.r.t. second argument
    {
	img_type XD2 = img_type(without_last_col);
	vpp::pixel_wise(XD2, weightsX_ | without_last_col, data_.img_ | without_last_col, data_.img_ | without_first_col) | calc_second_arg_deriv;
	#ifdef TV_FUNC_DEBUG
	    output_img(XD2,"XD2.csv");
	#endif
	auto grad_subX2  = grad | without_first_col;
	vpp::pixel_wise(grad_subX2, XD2) | add_to_gradient;
    }

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...-> YD1" << std::endl;
    #endif
    // Vertical derivatives and weighting
    // ... w.r.t. first argument
    {
	img_type YD1 = img_type(without_last_row);
	vpp::pixel_wise(YD1, weightsY_ | without_last_row, data_.img_ | without_last_row, data_.img_ | without_first_row) | calc_first_arg_deriv;
	#ifdef TV_FUNC_DEBUG
	    output_img(YD1,"YD1.csv");
	#endif
	auto grad_subY1  = grad | without_last_row;
	vpp::pixel_wise(grad_subY1, YD1) | add_to_gradient;
    }

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...-> YD2" << std::endl;
    #endif
    // ... w.r.t second argument
    {
	img_type YD2 = img_type(without_last_row);
	vpp::pixel_wise(YD2, weightsY_ | without_last_row, data_.img_ | without_last_row, data_.img_ | without_first_row) | calc_second_arg_deriv;
	#ifdef TV_FUNC_DEBUG
	    output_img(YD2,"YD2.csv");
	#endif
        auto grad_subY2  = grad | without_first_row;
        vpp::pixel_wise(grad_subY2, YD2) | add_to_gradient;
    }

    #ifdef TV_FUNC_DEBUG
	output_img(grad,"grad.csv");
    #endif

    DJ_ = gradient_type::Zero(nr*nc*manifold_dim);

    // flatten rowwise
    //vpp::pixel_wise(grad, grad.domain()) | [&] (value_type& p, vpp::vint2 coord) { DJ_.segment(3*(nc*coord[0]+coord[1]), value_dim) = p; };

    // Apply tangent space restriction and flatten colwise (as in Matlab code)
    updateTMBase();

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...Local to global insert" << std::endl;
    #endif
    auto insert2grad = [&] (const tm_base_type& t, const value_type& p, const vpp::vint2 coord) {
	DJ_.segment(manifold_dim * (coord[0] + nr * coord[1]), manifold_dim) = t.transpose() * Eigen::Map<const Eigen::VectorXd>(p.data(), p.size());
	//DJ_.segment(manifold_dim * (coord[0] + nr * coord[1]), manifold_dim) = t.transpose()*p; //remove: does not work for matrix valued pixel
    };

    vpp::pixel_wise(T_, grad, grad.domain()) | insert2grad;

    #ifdef TV_FUNC_DEBUG
	std::fstream f;
	f.open("gradJ.csv",std::fstream::out);
	f << DJ_;
	f.close();
    #endif
}

// Evaluation of Hessian J
template <enum FUNCTIONAL_DISC disc, typename MANIFOLD, class DATA >
void Functional<FIRSTORDER, disc, MANIFOLD, DATA >::evaluateHJ(){
    #ifdef TV_FUNC_WONES_DEBUG
	vpp::fill(weightsX_, 1.0); // Reset for Debugging
	vpp::fill(weightsY_, 1.0); // Reset for Debugging
    #endif

    hessian_type hessian(data_.img_.domain());
    int nr = data_.img_.nrows();
    int nc = data_.img_.ncols();
    int sparsedim = nr*nc*manifold_dim;

    //HESSIAN OF FIDELITY TERM
     #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\tHessian evaluation..." << std::endl;
	std::cout << "\t\t...Fidelity part" << std::endl;
    #endif

    sparse_hessian_type HF(sparsedim,sparsedim);

    //HF.reserve(Eigen::VectorXi::Constant(nc,manifold_dim));
    typedef Eigen::Triplet<double> Trip;
    std::vector<Trip> triplist;
    triplist.reserve(sparsedim*manifold_dim);

    if(data_.doInpaint()){
	auto f = [] (deriv2_type& h, const value_type& i, const value_type& n, const bool inp ) { MANIFOLD::deriv2xx_dist_squared(i,n,h); h*=(1-inp); };
	vpp::pixel_wise(hessian, data_.img_, data_.noise_img_, data_.inp_) | f;
    }
    else{
	auto f = [] (deriv2_type& h, const value_type& i, const value_type& n) { MANIFOLD::deriv2xx_dist_squared(i,n,h); };
	vpp::pixel_wise(hessian, data_.img_, data_.noise_img_) | f;
    }

    #ifdef TV_FUNC_DEBUG
	output_matval_img(hessian,"HF0.csv");
    #endif

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...Local to global insert" << std::endl;
    #endif
   //TODO: Check whether all 2nd-derivative matrices are symmetric s.t. only half the matrix need to be traversed. e.g. local_col=local_row instead of 0
    auto local2globalInsert = [&](const tm_base_type& t, const deriv2_type& h, const vpp::vint2 coord) {
	int pos = manifold_dim * (coord[0] + nr * coord[1]); // columnwise flattening
	restricted_deriv2_type ht=t.transpose()*h*t;
	for(int local_row = 0; local_row<ht.rows(); local_row++)
	    for(int local_col = local_row; local_col < ht.cols(); local_col++){
		scalar_type e = ht(local_row, local_col);
		if(e!=0){
		    int global_row = pos + local_row;
		    int global_col = pos + local_col;
		    triplist.push_back(Trip(global_row,global_col,e));
		    if(global_row != global_col)
			triplist.push_back(Trip(global_col,global_row,e));
		    //HF.insert(global_row, global_col) = e;
		    //if(global_row != global_col)
		    //	HF.insert(global_col, global_row) = e;
		    }
	    }
    };

    vpp::pixel_wise(T_, hessian, hessian.domain())(vpp::_no_threads) | local2globalInsert;
     #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...Triplet list created" << std::endl;
    #endif
    HF.setFromTriplets(triplist.begin(),triplist.end());
    HF.makeCompressed();

    //HESSIAN OF TV TERM
    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\tHessian evaluation..." << std::endl;
	std::cout << "\t\t...TV part" << std::endl;
    #endif

    sparse_hessian_type HTV(sparsedim,sparsedim);

    //HTV.reserve(Eigen::VectorXi::Constant(nc,5*manifold_dim));
    triplist.clear();
    triplist.reserve(3*sparsedim*manifold_dim);

    // Subimage boxes
    vpp::box2d without_last_col(vpp::vint2(0,0), vpp::vint2(nr-1, nc-2)); // subdomain without last column
    vpp::box2d without_first_col(vpp::vint2(0,1), vpp::vint2(nr-1, nc-1)); // subdomain without first column
    vpp::box2d without_last_row(vpp::vint2(0,0), vpp::vint2(nr-2, nc-1)); // subdomain without last row
    vpp::box2d without_first_row(vpp::vint2(1,0), vpp::vint2(nr-1, nc-1)); // subdomain without first row

    auto calc_xx_der = [&] (deriv2_type& x, const weights_type& w, const value_type& i, const value_type& n) { MANIFOLD::deriv2xx_dist_squared(i, n, x); x*=w; };
    auto calc_xy_der = [&] (deriv2_type& xy, const weights_type& w, const value_type& i, const value_type& n) { MANIFOLD::deriv2xy_dist_squared(i, n, xy); xy*=w; };
    auto calc_yy_der = [&] (deriv2_type& y, const weights_type& w, const value_type& i, const value_type& n) { MANIFOLD::deriv2yy_dist_squared(i, n, y); y*=w; };
    auto add_to_hessian =  [&] (deriv2_type& h, const deriv2_type& d) { h += d; };

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...->XD11" << std::endl;
    #endif
    // Horizontal Second Derivatives and weighting
    // ... w.r.t. first arguments
    { // Temporary image XD11 is deallocated after this scope
	hessian_type XD11(without_last_col);
        vpp::pixel_wise(XD11, weightsX_ | without_last_col, data_.img_ | without_last_col, data_.img_ | without_first_col) | calc_xx_der;
        #ifdef TV_FUNC_DEBUG
		output_matval_img(XD11,"XD11.csv");
        #endif
	auto hess_subX11  = hessian | without_last_col;
	vpp::pixel_wise(hess_subX11, XD11) | [&] (deriv2_type& h, const deriv2_type& d) { h=d; };
    }
	#pragma omp parallel for
	for(int r=0; r< nr; r++)
	    hessian(r,nc-1)=deriv2_type::Zero(); // set last column to zero

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...->XD22" << std::endl;
    #endif
    //... w.r.t. second arguments
    {
	hessian_type XD22(without_last_col);
        vpp::pixel_wise(XD22, weightsX_ | without_last_col, data_.img_ | without_last_col, data_.img_ | without_first_col) | calc_yy_der;
	#ifdef TV_FUNC_DEBUG
	    output_matval_img(XD22,"XD22.csv");
	#endif
	auto hess_subX22  = hessian | without_first_col;
	vpp::pixel_wise(hess_subX22, XD22) | add_to_hessian;
    }

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...->YD11" << std::endl;
    #endif
    // Vertical Second Derivatives weighting
    //... w.r.t. first arguments
    {
	hessian_type YD11(without_last_row);
        vpp::pixel_wise(YD11, weightsY_ | without_last_row, data_.img_ | without_last_row, data_.img_ | without_first_row) | calc_xx_der;
	#ifdef TV_FUNC_DEBUG
	    output_matval_img(YD11,"YD11.csv");
	#endif
	auto hess_subY11  = hessian | without_last_row;
	vpp::pixel_wise(hess_subY11, YD11) | add_to_hessian;
    }

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...->YD22" << std::endl;
    #endif
    //... w.r.t. second arguments
    {
	hessian_type YD22(without_last_row);
        vpp::pixel_wise(YD22, weightsY_ | without_last_row, data_.img_ | without_last_row, data_.img_ | without_first_row) | calc_yy_der;
	#ifdef TV_FUNC_DEBUG
	    output_matval_img(YD22,"YD22.csv");
	#endif
	auto hess_subY22  = hessian | without_first_row;
	vpp::pixel_wise(hess_subY22, YD22) | [&] (deriv2_type& h, const deriv2_type& d) { h+=d; };
    }

    #ifdef TV_FUNC_DEBUG
        output_matval_img(T_, "T.csv");
	output_matval_img(hessian, "NonMixedHessian.csv");
    #endif
    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...Local to global insert" << std::endl;
    #endif
    // Insert elementwise into sparse Hessian
    // NOTE: Eventually make single version for both cases, including an offset
    // --> additional parameters sparse_mat, offset
    int row_offset=0;
    int col_offset=0;
    auto local2globalInsertHTV = [&](const tm_base_type& t1, const tm_base_type& t2, const deriv2_type& h, const vpp::vint2 coord) {
	int pos = manifold_dim*(coord[0]+nr*coord[1]); // columnwise flattening
	restricted_deriv2_type ht = t1.transpose()*h*t2;
	     #ifdef TV_FUNC_DEBUG_VERBOSE2
		std::cout << "\n\nPos: " << pos << " coord[0]=" << coord[0] << " coord[1]=" << coord[1] << std::endl;
		std::cout << "\t\tT_left:" << std::endl;
		std::cout << t1 << std::endl;
		std::cout << "\t\tHbig:" << std::endl;
		std::cout << h << std::endl;
		std::cout << "\t\tT_right:" << std::endl;
		std::cout << t2 << std::endl;
		std::cout << "\t\tHsmall:" << std::endl;
		std::cout << ht << std::endl;
	    #endif
	for(int local_row=0; local_row<ht.rows(); local_row++)
	    for(int local_col=0; local_col<ht.cols(); local_col++){
		scalar_type e = ht(local_row, local_col);
		if(e!=0){
		    int global_row = pos + row_offset + local_row;
		    int global_col = pos + col_offset + local_col;
		    triplist.push_back(Trip(global_row,global_col,e));
		    if(row_offset > 0 || col_offset > 0)
			triplist.push_back(Trip(global_col,global_row,e));
		    //HTV.insert(global_row, global_col) = e;
		    //if(global_col != global_row)
		    //	HTV.insert(global_col, global_row)  = e;
		}
	    }
    };
    vpp::pixel_wise(T_, T_, hessian, hessian.domain())(vpp::_no_threads) | local2globalInsertHTV;

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...->XD12" << std::endl;
    #endif
    // Horizontal Second Derivatives and weighting
    // ... w.r.t. first and second arguments
    {
	hessian_type XD12(without_last_col);
	vpp::pixel_wise(XD12, weightsX_ | without_last_col, data_.img_ | without_last_col, data_.img_ | without_first_col ) | calc_xy_der;
	#ifdef TV_FUNC_DEBUG
	    output_matval_img(XD12,"XD12.csv");
	#endif
	#ifdef TV_FUNC_DEBUG_VERBOSE
		std::cout << "\t\t...Local to global insert:" << std::endl;
	#endif
	// Offsets for upper nyth subdiagonal
	row_offset=0;
	col_offset=manifold_dim*nr;
	vpp::pixel_wise(T_ | without_last_col, T_ | without_first_col, XD12, XD12.domain())(vpp::_no_threads) | local2globalInsertHTV;
    }

    #ifdef TV_FUNC_DEBUG_VERBOSE
	std::cout << "\t\t...->YD12" << std::endl;
    #endif
    // Vertical Second Derivatives and weighting
    //... w.r.t. second arguments
    {
	hessian_type YD12(data_.img_.domain());
	vpp::pixel_wise(YD12 | without_last_row, weightsY_ | without_last_row, data_.img_ | without_last_row, data_.img_ | without_first_row) | calc_xy_der;
	#ifdef TV_FUNC_DEBUG
	    output_matval_img(YD12,"YD12.csv");
        #endif

	//Set last row to zero
	deriv2_type *lastrow = &YD12(nr-1,0);
	for(int c=0; c< nc; c++)
	    lastrow[c]=deriv2_type::Zero();

	#ifdef TV_FUNC_DEBUG_VERBOSE
		std::cout << "\t\t...Local to global insert:" << std::endl;
	#endif
	// Offsets for first upper subdiagonal
	row_offset=0;
	col_offset=manifold_dim;
	vpp::pixel_wise(T_ | without_last_row, T_ | without_first_row, YD12 | without_last_row, without_last_row)(vpp::_no_threads) | local2globalInsertHTV;

	//Manually insert last row
	for(int c=0; c<nc-1; c++)
	    local2globalInsertHTV(T_(nr-1,c), T_(nr-1,c + 1), YD12(nr-1,c), vpp::vint2(nr-1, c));
	    //local2globalInsertHTV(firstrow[c], lastrow[c], vpp::vint2(nr-1,c));

	HTV.setFromTriplets(triplist.begin(),triplist.end());
	HTV.makeCompressed();
	triplist.clear();
    }

	#ifdef TV_FUNC_DEBUG_VERBOSE
		std::cout << "\t\t...Combine Fidelity and TV parts:" << std::endl;
	#endif

	HJ_= HF + lambda_*HTV;

	#ifdef TV_FUNC_DEBUG_VERBOSE
		std::cout << "\t\t...Output Hessian (stats):" << std::endl;
	#endif
    #ifdef TV_FUNC_DEBUG
	if (sparsedim<200){
	    if(sparsedim<70){
		std::cout << "\nFidelity\n" << HF << std::endl;
		std::cout << "\nTV\n" << HTV << std::endl;
		std::cout << "\nHessian\n" << HJ_ << std::endl;
	    }

	    std::fstream f;
	    f.open("H.csv",std::fstream::out);
	    Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "");
	    //f << HJ_.format(CommaInitFmt).;
	    f << HJ_;
	    f.close();

	}
	else{
	    std::cout << "\nFidelity Non-Zeros: " << HJ_.nonZeros() << std::endl;
	    std::cout << "\nTV Non-Zeros: " << HJ_.nonZeros() << std::endl;
	    std::cout << "\nHessian Non-Zeros: " << HJ_.nonZeros() << std::endl;
	}
    // Test Solver:
/*	#ifdef TV_FUNC_DEBUG_VERBOSE
		std::cout << "\t\t...Test Solve" << std::endl;
	#endif
	gradient_type x;

	Eigen::SparseLU<sparse_hessian_type> solver;
	solver.analyzePattern(HJ_);
	solver.factorize(HJ_);
	x = solver.solve(DJ_);

	std::fstream f;
	f.open("Sol.csv",std::fstream::out);
	f << x;
	f.close();*/
    #endif

}

template <enum FUNCTIONAL_DISC disc, typename MANIFOLD, class DATA >
template < class IMG >
void Functional<FIRSTORDER, disc, MANIFOLD, DATA >::output_img(const IMG& img, const char* filename) const{
    int nr = img.nrows();
    int nc = img.ncols();

    std::fstream f;
    f.open(filename, std::fstream::out);
    Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "");
    for (int r=0; r<nr; r++){
	const auto* cur = &img(r,0);
	for (int c=0; c<nc; c++){
	    f << cur[c].format(CommaInitFmt);
	    if(c != nc-1) f << ",";
	}
	f <<  std::endl;
    }
    f.close();
}

template <enum FUNCTIONAL_DISC disc, typename MANIFOLD, class DATA >
template < class IMG >
void Functional<FIRSTORDER, disc, MANIFOLD, DATA >::output_matval_img(const IMG& img, const char* filename) const{
    int nr = img.nrows();
    int nc = img.ncols();

    std::fstream f;
    f.open(filename, std::fstream::out);
    Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "\n");
    for (int r=0; r<nr; r++){
	const auto* cur = &img(r,0);
	for (int c=0; c<nc; c++)
	    f << cur[c].format(CommaInitFmt);
    }
    f.close();
}

}// end namespace tvtml

#endif