// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_TRANSFORM_MAP_TRANSFORMER_HPP
#define BOOST_GEOMETRY_STRATEGIES_TRANSFORM_MAP_TRANSFORMER_HPP

#include <cstddef>

#include <boost/geometry/strategies/transform/matrix_transformers.hpp>

namespace boost { namespace geometry
{

// Silence warning C4127: conditional expression is constant
#if defined(_MSC_VER)
#pragma warning(push)
#pragma warning(disable : 4127)
#endif

namespace strategy { namespace transform
{

/*!
\brief Transformation strategy to map from one to another Cartesian coordinate system
\ingroup strategies
\tparam Mirror if true map is mirrored upside-down (in most cases pixels
    are from top to bottom, while map is from bottom to top)
 */
template
<
    typename CalculationType,
    std::size_t Dimension1,
    std::size_t Dimension2,
    bool Mirror = false,
    bool SameScale = true
>
class map_transformer
    : public ublas_transformer<CalculationType, Dimension1, Dimension2>
{
    typedef boost::numeric::ublas::matrix<CalculationType> M;

public :
    template <typename B, typename D>
    explicit inline map_transformer(B const& box, D const& width, D const& height)
    {
        set_transformation(
                get<min_corner, 0>(box), get<min_corner, 1>(box),
                get<max_corner, 0>(box), get<max_corner, 1>(box),
                width, height);
    }

    template <typename W, typename D>
    explicit inline map_transformer(W const& wx1, W const& wy1, W const& wx2, W const& wy2,
                        D const& width, D const& height)
    {
        set_transformation(wx1, wy1, wx2, wy2, width, height);
    }

private :
    template <typename W, typename P, typename S>
    inline void set_transformation_point(W const& wx, W const& wy,
        P const& px, P const& py,
        S const& scalex, S const& scaley)
    {

        // Translate to a coordinate system centered on world coordinates (-wx, -wy)
        M t1(3,3);
        t1(0,0) = 1;   t1(0,1) = 0;   t1(0,2) = -wx;
        t1(1,0) = 0;   t1(1,1) = 1;   t1(1,2) = -wy;
        t1(2,0) = 0;   t1(2,1) = 0;   t1(2,2) = 1;

        // Scale the map
        M s(3,3);
        s(0,0) = scalex;   s(0,1) = 0;   s(0,2) = 0;
        s(1,0) = 0;    s(1,1) = scaley;  s(1,2) = 0;
        s(2,0) = 0;    s(2,1) = 0;      s(2,2) = 1;

        // Translate to a coordinate system centered on the specified pixels (+px, +py)
        M t2(3, 3);
        t2(0,0) = 1;   t2(0,1) = 0;   t2(0,2) = px;
        t2(1,0) = 0;   t2(1,1) = 1;   t2(1,2) = py;
        t2(2,0) = 0;   t2(2,1) = 0;   t2(2,2) = 1;

        // Calculate combination matrix in two steps
        this->m_matrix = boost::numeric::ublas::prod(s, t1);
        this->m_matrix = boost::numeric::ublas::prod(t2, this->m_matrix);
    }

    template <typename W, typename D>
    void set_transformation(W const& wx1, W const& wy1, W const& wx2, W const& wy2,
                    D const& width, D const& height)
    {
        D px1 = 0;
        D py1 = 0;
        D px2 = width;
        D py2 = height;

        // Get the same type, but at least a double
        typedef typename select_most_precise<D, double>::type type;

        // Calculate appropriate scale, take min because whole box must fit
        // Scale is in PIXELS/MAPUNITS (meters)
        W wdx = wx2 - wx1;
        W wdy = wy2 - wy1;
        type sx = (px2 - px1) / boost::numeric_cast<type>(wdx);
        type sy = (py2 - py1) / boost::numeric_cast<type>(wdy);

        if (SameScale)
        {
            type scale = (std::min)(sx, sy);
            sx = scale;
            sy = scale;
        }

        // Calculate centerpoints
        W wtx = wx1 + wx2;
        W wty = wy1 + wy2;
        W two = 2;
        W wmx = wtx / two;
        W wmy = wty / two;
        type pmx = (px1 + px2) / 2.0;
        type pmy = (py1 + py2) / 2.0;

        set_transformation_point(wmx, wmy, pmx, pmy, sx, sy);

        if (Mirror)
        {
            // Mirror in y-direction
            M m(3,3);
            m(0,0) = 1;   m(0,1) = 0;   m(0,2) = 0;
            m(1,0) = 0;   m(1,1) = -1;  m(1,2) = 0;
            m(2,0) = 0;   m(2,1) = 0;   m(2,2) = 1;

            // Translate in y-direction such that it fits again
            M y(3, 3);
            y(0,0) = 1;   y(0,1) = 0;   y(0,2) = 0;
            y(1,0) = 0;   y(1,1) = 1;   y(1,2) = height;
            y(2,0) = 0;   y(2,1) = 0;   y(2,2) = 1;

            // Calculate combination matrix in two steps
            this->m_matrix = boost::numeric::ublas::prod(m, this->m_matrix);
            this->m_matrix = boost::numeric::ublas::prod(y, this->m_matrix);
        }
    }
};

}} // namespace strategy::transform

#if defined(_MSC_VER)
#pragma warning(pop)
#endif

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_TRANSFORM_MAP_TRANSFORMER_HPP


#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>);


#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


#include <unordered_map>
#include <memory>
#include <iterator>
#include <Eigen/Core>
#include <Eigen/LU>
#include "./parser.hpp"
#include "./lattice.hpp"
#include "./site.hpp"
#include "./search_equiv_sites.hpp"
#include "./myhash.hpp"
#include "./myindex.hpp"
#include "./input.hpp"
#include "./metroconf.hpp"

using allclusters = std::vector<std::vector<std::vector<std::vector<int>>>>;

void dispInput(std::shared_ptr<Input> in){
	//  [INPUT]
	std::cout << "MAXBODY     : " << in->getDataByString("MAXBODY") << std::endl;
	std::cout << "TRUNCATION  : " << in->getDataByString("TRUNCATION") << std::endl;
	std::cout << "SPINCE      : " << in->getDataByString("SPINCE") << std::endl;
	std::cout << "SPINPOSCAR  : " << in->getDataByString("SPINPOSCAR") << std::endl;
	//  [OPTION]
	std::cout << "PAIRTRUNC   : " << in->getDataByString("PAIRTRUNC")  << std::endl;
	std::cout << "TRITRUNC    : " << in->getDataByString("TRITRUNC") << std::endl;
	std::cout << "QUADTRUNC   : " << in->getDataByString("QUADTRUNC") << std::endl;
	std::cout << "CLUSTEROUT  : " << in->getDataByString("CLUSTEROUT") << std::endl;
	std::cout << "CORRDUMP    : " << in->getDataByString("CORRDUMP") << std::endl;
}

void showResult(int is_corrdump, const std::shared_ptr<Input>& in){

	const ParseClusterOut parse_cluster_out("./cluster.out");

	if( is_corrdump>0 ){
		Conf2corr PoscarSpin("./poscar.spin", in, parse_cluster_out.getLabel(), parse_cluster_out.getCluster());

		for(int i=0; i<PoscarSpin.getSpins().size(); ++i ){
			bool isin = PoscarSpin.isInNthNearestNeighborPair(i);
			if( isin ) {std::cout << std::endl; return;}
		}

		for(const auto& corrs : PoscarSpin.getCorrelationFunctions() ) {
			for(const auto& corr : corrs){
				std::cout << corr << " ";
			}
			std::cout << std::endl;
		}
	} else {
		const ParseEcicar ecicar("./ecicar");
		std::shared_ptr<allclusters> pall_clusters_dummy(new allclusters());
		for(int i=0; i<parse_cluster_out.getCluster()->size(); ++i){
			for(const auto eci : ecicar.getEci() ){
				if( (i+1) == eci.first ) {
					pall_clusters_dummy->push_back((*(parse_cluster_out.getCluster()))[i]);
					break;
				}
			}
		}

		Metroconf PoscarSpin("./poscar.spin", in, parse_cluster_out.getLabel(), pall_clusters_dummy, ecicar.getEci(), nullptr, nullptr);
		PoscarSpin.setTotalEnergy();
		auto compositions = PoscarSpin.getCompositions();

		for(int i=0; i<PoscarSpin.getSpins().size(); ++i ){
			bool isin = PoscarSpin.isInNthNearestNeighborPair(i);
			if( isin ) {std::cout << std::endl; return;}
		}

		for(const auto c : compositions) std::cout << c << " ";
		std::cout << PoscarSpin.getTotalEnergy() << std::endl;
	}

}

int main(int argc, char* argv[]){
	int is_corrdump = -1;
	int is_read     = -1;

	int num_max_body = 0;
	double truncation = 0;
	double precision  = 0.00001;
	double pair_truncation = 0;
	double triplet_truncation = 0;
	double quadruplet_truncation = 0;
	std::shared_ptr<Input> in(new Input("getconf.ini"));

	in->setData("MAXBODY",      num_max_body, true);
	in->setData("TRUNCATION",   truncation, true);

	in->setData("PRECISION", precision);
	in->setData("PAIRTRUNC", pair_truncation);
	in->setData("TRITRUNC",  triplet_truncation);
	in->setData("QUADTRUNC", quadruplet_truncation);

	in->setData("CLUSTEROUT",  is_read);
	in->setData("CORRDUMP",    is_corrdump);

	if( pair_truncation == 0 ) pair_truncation = truncation;
	if( triplet_truncation == 0 ) triplet_truncation = truncation;
	if( quadruplet_truncation == 0 ) quadruplet_truncation = truncation;

	if( pair_truncation < triplet_truncation or triplet_truncation < quadruplet_truncation ) {
		std::cerr << "ERROR : TRITRUNC <= PAIRTRUNC(orTRUNATION) and QUADTRUNC <= TRITRUNC(orTRUNATION)" << std::endl;
		exit(1);
	}

	if( is_read>0 ){
		showResult(is_corrdump, in);
		return 1;
	}

	std::string filename_poscar_in = "poscar.spin";

	ParsePoscar poscar(filename_poscar_in.c_str());
	const auto atoms_unit = poscar.getAtoms();

	Eigen::Vector3d unit_x, unit_y, unit_z;
	unit_x << 1, 0, 0;
	unit_y << 0, 1, 0;
	unit_z << 0, 0, 1;

	double unit_length_x = (poscar.getLatticeBasis() * unit_x).norm();
	double unit_length_y = (poscar.getLatticeBasis() * unit_y).norm();
	double unit_length_z = (poscar.getLatticeBasis() * unit_z).norm();

	int N_unit = poscar.getAtoms().size();
	/* if use ceil, integer distance cannot be correctly handled. */
	int expand_x = std::ceil((pair_truncation*3.)/unit_length_x);
	int expand_y = std::ceil((pair_truncation*3.)/unit_length_y);
	int expand_z = std::ceil((pair_truncation*3.)/unit_length_z);

	poscar.expandPoscar(expand_x, expand_y, expand_z);

	const auto atoms = poscar.getAtoms();
	const int N = atoms.size();

	/*  setting lattice and site */
	std::vector<std::shared_ptr<Lattice>> vec_lattices;
	std::vector<std::shared_ptr<Site>> vec_sites;
	setSiteAndLattice(vec_lattices, vec_sites, poscar);

	std::ofstream cluster_info("cluster.info", std::ios::out );
	std::ofstream cluster_out( "cluster.out", std::ios::out );

	for( int i=0; i<atoms_unit.size(); ++i){
		cluster_out << atoms_unit[i].first << " " <<  atoms_unit[i].second.transpose() << std::endl;
	}
	cluster_out << "--" << std::endl;

	/*  set nbody = 1 */
	cluster_info << " -- point cluster" << std::endl;
	int num_index = 1;
	for( const auto& lattice : vec_lattices ){
		cluster_info << num_index << " : "  << "[" << lattice->getLatticeNum() << "] : 0 : 1 : 0 0 0 " << std::endl;
		cluster_out << num_index << " ";
		cluster_out << "1" << " ";
		cluster_out << N_unit << " ";
		cluster_out << N_unit << " ";
		for( int i=0; i<N_unit; ++i){
			cluster_out << vec_sites[i]->getSiteNum() << " ";
		}
		cluster_out << std::endl;
		++num_index;
	}

	/*  set nbody = 2 */
	cluster_info << " -- 2 body cluster" << std::endl;
	std::unordered_map<PairIndex , Eigen::Vector3d, hash_pairindex> index_pair_clusters;
	if( pair_truncation == 0 ) pair_truncation = truncation;
	setSiteReferencesAndPairIndex(vec_lattices, vec_sites, index_pair_clusters, pair_truncation, precision);

	std::vector<PairIndex> sorted_vec_pairindex;
	for(const auto& index_pair : index_pair_clusters){
		sorted_vec_pairindex.push_back(index_pair.first);
	}
	sort(sorted_vec_pairindex.begin(), sorted_vec_pairindex.end());

	cluster_info << " # [index] : distance : multiplicity : coordination " << std::endl;
	for( const auto& pairindex : sorted_vec_pairindex ){
		cluster_info << num_index << " : ";
		cluster_info << "[" << pairindex.lattice_index[0] << "," << pairindex.lattice_index[1]  << "] : ";
		const auto pair_itr = vec_sites[0]->getLinkedSiteIterator(pairindex.distance);
		cluster_info << pairindex.distance << " : " << std::distance(pair_itr.first, pair_itr.second) << " : ";
		cluster_info << index_pair_clusters[pairindex].transpose() << std::endl;

		for( const auto& site : vec_sites ) {
			const auto test_pair_itr = site->getLinkedSiteIterator(pairindex.distance);
			assert( std::distance(pair_itr.first, pair_itr.second) == std::distance(test_pair_itr.first, test_pair_itr.second) );
		}

		cluster_out << num_index << " ";
		cluster_out << "2" << " ";
		cluster_out << N_unit << " ";
		cluster_out << N_unit * std::distance(pair_itr.first, pair_itr.second) << " ";
		for(int i=0; i<N_unit; ++i){
			auto pair_itr = vec_sites[i]->getLinkedSiteIterator(pairindex.distance);
			while( pair_itr.first != pair_itr.second ){
				cluster_out << vec_sites[i]->getSiteNum() << " " << (*(pair_itr.first)).site->getSiteNum() << " ";
				++pair_itr.first;
			}
		}
		cluster_out << std::endl;
		++num_index;
	}
	if( num_max_body == 2 ){
		cluster_out.close();
		cluster_info.close();
		showResult(is_corrdump, in);
		return 1;
	}

	/*  set nbody = 3 */
	cluster_info << " -- 3 body cluster" << std::endl;
	std::vector<TripletIndex> keys_index_triplet_cluster;
	std::unordered_map<TripletIndex, std::vector<std::vector<Eigen::Vector3d>>, hash_tripletindex> index_triplet_cluster;
	setTripletIndex(vec_lattices, vec_sites, sorted_vec_pairindex, keys_index_triplet_cluster, index_triplet_cluster, triplet_truncation, precision);

	for(int i=2; i>=0; --i){
		stable_sort(keys_index_triplet_cluster.begin(), keys_index_triplet_cluster.end(), [i](const TripletIndex& lhs, const TripletIndex& rhs){
				return lhs.vec_pairindex[i] < rhs.vec_pairindex[i];
		});
	}

	for(const auto& tripletindex : keys_index_triplet_cluster){

		cluster_info << num_index << " : ";
		cluster_info << "[] : ";
		cluster_info << " [ ";
		cluster_info << tripletindex.vec_pairindex[0].distance << ",";
		cluster_info << tripletindex.vec_pairindex[1].distance << ",";
		cluster_info << tripletindex.vec_pairindex[2].distance;
		cluster_info << " ] : ";
		cluster_info << index_triplet_cluster[tripletindex].size() <<  " : ";

		cluster_info << index_triplet_cluster[tripletindex][0][0].transpose() << ",";
		cluster_info << index_triplet_cluster[tripletindex][0][1].transpose() << std::endl;

		cluster_out << num_index << " ";
		cluster_out << "3" << " ";
		cluster_out << N_unit << " ";
		cluster_out << N_unit*index_triplet_cluster[tripletindex].size() << " ";
		for(int i=0; i<N_unit; ++i){
			for(int j=0; j<index_triplet_cluster[tripletindex].size(); ++j){
				cluster_out  << vec_sites[i]->getSiteNum() << " ";
				cluster_out  << vec_sites[i]->getLinkedSite(index_triplet_cluster[tripletindex][j][0])->getSiteNum() << " ";
				cluster_out  << vec_sites[i]->getLinkedSite(index_triplet_cluster[tripletindex][j][1])->getSiteNum() << " ";
			}
		}
		cluster_out << std::endl;
		++num_index;
	}

	if( num_max_body == 3 ){
		cluster_out.close();
		cluster_info.close();
		showResult(is_corrdump, in);
		return 1;
	}

	// /*  set nbody = 4 */
	cluster_info << " -- 4 body cluster" << std::endl;
	std::vector<QuadrupletIndex> keys_index_quadruplet_cluster;
	std::unordered_map<QuadrupletIndex, std::vector<std::vector<Eigen::Vector3d>>, hash_quadrupletindex> index_quadruplet_cluster;
	setQuadrupletIndex(vec_lattices, vec_sites, sorted_vec_pairindex,  index_triplet_cluster, keys_index_quadruplet_cluster, index_quadruplet_cluster, quadruplet_truncation, precision);

	for(int i=3; i>=0; --i){
		for(int j=2; j>=0; --j){
		stable_sort(keys_index_quadruplet_cluster.begin(), keys_index_quadruplet_cluster.end(),
			[i,j](const QuadrupletIndex& lhs, const QuadrupletIndex& rhs){
				return lhs.vec_tripletindex[i].vec_pairindex[j] < rhs.vec_tripletindex[i].vec_pairindex[j];
			});
		}
	}

	for(const auto& key_index_quadruplet_cluster : keys_index_quadruplet_cluster){

		cluster_info << num_index << " : ";
		cluster_info << "[] : ";
		cluster_info << " [ ";
		for( const auto distance : key_index_quadruplet_cluster.getVecDistance() ){
			cluster_info << distance << ",";
		}
		cluster_info << " ] : ";
		cluster_info << index_quadruplet_cluster[key_index_quadruplet_cluster].size() <<  " : ";

		cluster_info << index_quadruplet_cluster[key_index_quadruplet_cluster][0][0].transpose() << ",";
		cluster_info << index_quadruplet_cluster[key_index_quadruplet_cluster][0][1].transpose() << ",";
		cluster_info << index_quadruplet_cluster[key_index_quadruplet_cluster][0][2].transpose() << std::endl;

		cluster_out << num_index << " ";
		cluster_out << "4" << " ";
		cluster_out << N_unit << " ";
		cluster_out << N_unit*index_quadruplet_cluster[key_index_quadruplet_cluster].size() << " ";
		for(int i=0; i<N_unit; ++i){
			for(int j=0; j<index_quadruplet_cluster[key_index_quadruplet_cluster].size(); ++j){
				cluster_out  << vec_sites[i]->getSiteNum() << " ";
				cluster_out  << vec_sites[i]->getLinkedSite(index_quadruplet_cluster[key_index_quadruplet_cluster][j][0])->getSiteNum() << " ";
				cluster_out  << vec_sites[i]->getLinkedSite(index_quadruplet_cluster[key_index_quadruplet_cluster][j][1])->getSiteNum() << " ";
				cluster_out  << vec_sites[i]->getLinkedSite(index_quadruplet_cluster[key_index_quadruplet_cluster][j][2])->getSiteNum() << " ";
			}
		}
		cluster_out << std::endl;
		++num_index;
	}
	cluster_out.close();
	cluster_info.close();

	showResult(is_corrdump, in);
	return 1;
}


///////////////////////////////////////////////////////////////
//  Copyright 2020 Madhur Chauhan.
//  Copyright 2020 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#ifndef BOOST_MP_ADD_UNSIGNED_ADDC_32_HPP
#define BOOST_MP_ADD_UNSIGNED_ADDC_32_HPP

#include <boost/multiprecision/cpp_int/intel_intrinsics.hpp>
#include <boost/multiprecision/detail/assert.hpp>

namespace boost { namespace multiprecision { namespace backends {

template <class CppInt1, class CppInt2, class CppInt3>
inline BOOST_MP_CXX14_CONSTEXPR void add_unsigned_constexpr(CppInt1& result, const CppInt2& a, const CppInt3& b) noexcept(is_non_throwing_cpp_int<CppInt1>::value)
{
   using ::boost::multiprecision::std_constexpr::swap;
   //
   // This is the generic, C++ only version of addition.
   // It's also used for all constexpr branches, hence the name.
   // Nothing fancy, just let uintmax_t take the strain:
   //
   double_limb_type carry = 0;
   std::size_t         m(0), x(0);
   std::size_t         as = a.size();
   std::size_t         bs = b.size();
   minmax(as, bs, m, x);
   if (x == 1)
   {
      bool s = a.sign();
      result = static_cast<double_limb_type>(*a.limbs()) + static_cast<double_limb_type>(*b.limbs());
      result.sign(s);
      return;
   }
   result.resize(x, x);
   typename CppInt2::const_limb_pointer pa     = a.limbs();
   typename CppInt3::const_limb_pointer pb     = b.limbs();
   typename CppInt1::limb_pointer       pr     = result.limbs();
   typename CppInt1::limb_pointer       pr_end = pr + m;

   if (as < bs)
      swap(pa, pb);

   // First where a and b overlap:
   while (pr != pr_end)
   {
      carry += static_cast<double_limb_type>(*pa) + static_cast<double_limb_type>(*pb);
#ifdef __MSVC_RUNTIME_CHECKS
      *pr = static_cast<limb_type>(carry & ~static_cast<limb_type>(0));
#else
      *pr = static_cast<limb_type>(carry);
#endif
      carry >>= CppInt1::limb_bits;
      ++pr, ++pa, ++pb;
   }
   pr_end += x - m;
   // Now where only a has digits:
   while (pr != pr_end)
   {
      if (!carry)
      {
         if (pa != pr)
            std_constexpr::copy(pa, pa + (pr_end - pr), pr);
         break;
      }
      carry += static_cast<double_limb_type>(*pa);
#ifdef __MSVC_RUNTIME_CHECKS
      *pr = static_cast<limb_type>(carry & ~static_cast<limb_type>(0));
#else
      *pr = static_cast<limb_type>(carry);
#endif
      carry >>= CppInt1::limb_bits;
      ++pr, ++pa;
   }
   if (carry)
   {
      // We overflowed, need to add one more limb:
      result.resize(x + 1, x + 1);
      if (result.size() > x)
         result.limbs()[x] = static_cast<limb_type>(1u);
   }
   result.normalize();
   result.sign(a.sign());
}
//
// Core subtraction routine for all non-trivial cpp_int's:
//
template <class CppInt1, class CppInt2, class CppInt3>
inline BOOST_MP_CXX14_CONSTEXPR void subtract_unsigned_constexpr(CppInt1& result, const CppInt2& a, const CppInt3& b) noexcept(is_non_throwing_cpp_int<CppInt1>::value)
{
   using ::boost::multiprecision::std_constexpr::swap;
   //
   // This is the generic, C++ only version of subtraction.
   // It's also used for all constexpr branches, hence the name.
   // Nothing fancy, just let uintmax_t take the strain:
   //
   double_limb_type borrow = 0;
   std::size_t         m(0), x(0);
   minmax(a.size(), b.size(), m, x);
   //
   // special cases for small limb counts:
   //
   if (x == 1)
   {
      bool      s  = a.sign();
      limb_type al = *a.limbs();
      limb_type bl = *b.limbs();
      if (bl > al)
      {
         ::boost::multiprecision::std_constexpr::swap(al, bl);
         s = !s;
      }
      result = al - bl;
      result.sign(s);
      return;
   }
   // This isn't used till later, but comparison has to occur before we resize the result,
   // as that may also resize a or b if this is an inplace operation:
   int c = a.compare_unsigned(b);
   // Set up the result vector:
   result.resize(x, x);
   // Now that a, b, and result are stable, get pointers to their limbs:
   typename CppInt2::const_limb_pointer pa      = a.limbs();
   typename CppInt3::const_limb_pointer pb      = b.limbs();
   typename CppInt1::limb_pointer       pr      = result.limbs();
   bool                                 swapped = false;
   if (c < 0)
   {
      swap(pa, pb);
      swapped = true;
   }
   else if (c == 0)
   {
      result = static_cast<limb_type>(0);
      return;
   }

   std::size_t i = 0;
   // First where a and b overlap:
   while (i < m)
   {
      borrow = static_cast<double_limb_type>(pa[i]) - static_cast<double_limb_type>(pb[i]) - borrow;
      pr[i]  = static_cast<limb_type>(borrow);
      borrow = (borrow >> CppInt1::limb_bits) & 1u;
      ++i;
   }
   // Now where only a has digits, only as long as we've borrowed:
   while (borrow && (i < x))
   {
      borrow = static_cast<double_limb_type>(pa[i]) - borrow;
      pr[i]  = static_cast<limb_type>(borrow);
      borrow = (borrow >> CppInt1::limb_bits) & 1u;
      ++i;
   }
   // Any remaining digits are the same as those in pa:
   if ((x != i) && (pa != pr))
      std_constexpr::copy(pa + i, pa + x, pr + i);
   BOOST_MP_ASSERT(0 == borrow);

   //
   // We may have lost digits, if so update limb usage count:
   //
   result.normalize();
   result.sign(a.sign());
   if (swapped)
      result.negate();
}

#ifdef BOOST_MP_HAS_IMMINTRIN_H
//
// This is the key addition routine where all the argument types are non-trivial cpp_int's:
//
//
// This optimization is limited to: GCC, LLVM, ICC (Intel), MSVC for x86_64 and i386.
// If your architecture and compiler supports ADC intrinsic, please file a bug
//
// As of May, 2020 major compilers don't recognize carry chain though adc
// intrinsics are used to hint compilers to use ADC and still compilers don't
// unroll the loop efficiently (except LLVM) so manual unrolling is done.
//
// Also note that these intrinsics were only introduced by Intel as part of the
// ADX processor extensions, even though the addc instruction has been available
// for basically all x86 processors.  That means gcc-9, clang-9, msvc-14.2 and up
// are required to support these intrinsics.
//
template <class CppInt1, class CppInt2, class CppInt3>
inline BOOST_MP_CXX14_CONSTEXPR void add_unsigned(CppInt1& result, const CppInt2& a, const CppInt3& b) noexcept(is_non_throwing_cpp_int<CppInt1>::value)
{
#ifndef BOOST_MP_NO_CONSTEXPR_DETECTION
   if (BOOST_MP_IS_CONST_EVALUATED(a.size()))
   {
      add_unsigned_constexpr(result, a, b);
   }
   else
#endif
   {
      using std::swap;

      // Nothing fancy, just let uintmax_t take the strain:
      std::size_t m(0), x(0);
      std::size_t as = a.size();
      std::size_t bs = b.size();
      minmax(as, bs, m, x);
      if (x == 1)
      {
         bool s = a.sign();
         result = static_cast<double_limb_type>(*a.limbs()) + static_cast<double_limb_type>(*b.limbs());
         result.sign(s);
         return;
      }
      result.resize(x, x);
      typename CppInt2::const_limb_pointer pa = a.limbs();
      typename CppInt3::const_limb_pointer pb = b.limbs();
      typename CppInt1::limb_pointer       pr = result.limbs();

      if (as < bs)
         swap(pa, pb);
      // First where a and b overlap:
      std::size_t      i = 0;
      unsigned char carry = 0;
#if defined(BOOST_MSVC) && !defined(BOOST_HAS_INT128) && defined(_M_X64)
      //
      // Special case for 32-bit limbs on 64-bit architecture - we can process
      // 2 limbs with each instruction.
      //
      for (; i + 8 <= m; i += 8)
      {
         carry = _addcarry_u64(carry, *(unsigned long long*)(pa + i + 0), *(unsigned long long*)(pb + i + 0), (unsigned long long*)(pr + i));
         carry = _addcarry_u64(carry, *(unsigned long long*)(pa + i + 2), *(unsigned long long*)(pb + i + 2), (unsigned long long*)(pr + i + 2));
         carry = _addcarry_u64(carry, *(unsigned long long*)(pa + i + 4), *(unsigned long long*)(pb + i + 4), (unsigned long long*)(pr + i + 4));
         carry = _addcarry_u64(carry, *(unsigned long long*)(pa + i + 6), *(unsigned long long*)(pb + i + 6), (unsigned long long*)(pr + i + 6));
      }
#else
      for (; i + 4 <= m; i += 4)
      {
         carry = ::boost::multiprecision::detail::addcarry_limb(carry, pa[i + 0], pb[i + 0], pr + i);
         carry = ::boost::multiprecision::detail::addcarry_limb(carry, pa[i + 1], pb[i + 1], pr + i + 1);
         carry = ::boost::multiprecision::detail::addcarry_limb(carry, pa[i + 2], pb[i + 2], pr + i + 2);
         carry = ::boost::multiprecision::detail::addcarry_limb(carry, pa[i + 3], pb[i + 3], pr + i + 3);
      }
#endif
      for (; i < m; ++i)
         carry = ::boost::multiprecision::detail::addcarry_limb(carry, pa[i], pb[i], pr + i);
      for (; i < x && carry; ++i)
         // We know carry is 1, so we just need to increment pa[i] (ie add a literal 1) and capture the carry:
         carry = ::boost::multiprecision::detail::addcarry_limb(0, pa[i], 1, pr + i);
      if (i == x && carry)
      {
         // We overflowed, need to add one more limb:
         result.resize(x + 1, x + 1);
         if (result.size() > x)
            result.limbs()[x] = static_cast<limb_type>(1u);
      }
      else if ((x != i) && (pa != pr))
         // Copy remaining digits only if we need to:
         std_constexpr::copy(pa + i, pa + x, pr + i);
      result.normalize();
      result.sign(a.sign());
   }
}

template <class CppInt1, class CppInt2, class CppInt3>
inline BOOST_MP_CXX14_CONSTEXPR void subtract_unsigned(CppInt1& result, const CppInt2& a, const CppInt3& b) noexcept(is_non_throwing_cpp_int<CppInt1>::value)
{
#ifndef BOOST_MP_NO_CONSTEXPR_DETECTION
   if (BOOST_MP_IS_CONST_EVALUATED(a.size()))
   {
      subtract_unsigned_constexpr(result, a, b);
   }
   else
#endif
   {
      using std::swap;

      // Nothing fancy, just let uintmax_t take the strain:
      std::size_t         m(0), x(0);
      minmax(a.size(), b.size(), m, x);
      //
      // special cases for small limb counts:
      //
      if (x == 1)
      {
         bool      s = a.sign();
         limb_type al = *a.limbs();
         limb_type bl = *b.limbs();
         if (bl > al)
         {
            ::boost::multiprecision::std_constexpr::swap(al, bl);
            s = !s;
         }
         result = al - bl;
         result.sign(s);
         return;
      }
      // This isn't used till later, but comparison has to occur before we resize the result,
      // as that may also resize a or b if this is an inplace operation:
      int c = a.compare_unsigned(b);
      // Set up the result vector:
      result.resize(x, x);
      // Now that a, b, and result are stable, get pointers to their limbs:
      typename CppInt2::const_limb_pointer pa = a.limbs();
      typename CppInt3::const_limb_pointer pb = b.limbs();
      typename CppInt1::limb_pointer       pr = result.limbs();
      bool                                 swapped = false;
      if (c < 0)
      {
         swap(pa, pb);
         swapped = true;
      }
      else if (c == 0)
      {
         result = static_cast<limb_type>(0);
         return;
      }

      std::size_t i = 0;
      unsigned char borrow = 0;
      // First where a and b overlap:
#if defined(BOOST_MSVC) && !defined(BOOST_HAS_INT128) && defined(_M_X64)
      //
      // Special case for 32-bit limbs on 64-bit architecture - we can process
      // 2 limbs with each instruction.
      //
      for (; i + 8 <= m; i += 8)
      {
         borrow = _subborrow_u64(borrow, *reinterpret_cast<const unsigned long long*>(pa + i), *reinterpret_cast<const unsigned long long*>(pb + i), reinterpret_cast<unsigned long long*>(pr + i));
         borrow = _subborrow_u64(borrow, *reinterpret_cast<const unsigned long long*>(pa + i + 2), *reinterpret_cast<const unsigned long long*>(pb + i + 2), reinterpret_cast<unsigned long long*>(pr + i + 2));
         borrow = _subborrow_u64(borrow, *reinterpret_cast<const unsigned long long*>(pa + i + 4), *reinterpret_cast<const unsigned long long*>(pb + i + 4), reinterpret_cast<unsigned long long*>(pr + i + 4));
         borrow = _subborrow_u64(borrow, *reinterpret_cast<const unsigned long long*>(pa + i + 6), *reinterpret_cast<const unsigned long long*>(pb + i + 6), reinterpret_cast<unsigned long long*>(pr + i + 6));
      }
#else
      for(; i + 4 <= m; i += 4)
      {
         borrow = boost::multiprecision::detail::subborrow_limb(borrow, pa[i], pb[i], pr + i);
         borrow = boost::multiprecision::detail::subborrow_limb(borrow, pa[i + 1], pb[i + 1], pr + i + 1);
         borrow = boost::multiprecision::detail::subborrow_limb(borrow, pa[i + 2], pb[i + 2], pr + i + 2);
         borrow = boost::multiprecision::detail::subborrow_limb(borrow, pa[i + 3], pb[i + 3], pr + i + 3);
      }
#endif
      for (; i < m; ++i)
         borrow = boost::multiprecision::detail::subborrow_limb(borrow, pa[i], pb[i], pr + i);

      // Now where only a has digits, only as long as we've borrowed:
      while (borrow && (i < x))
      {
         borrow = boost::multiprecision::detail::subborrow_limb(borrow, pa[i], 0, pr + i);
         ++i;
      }
      // Any remaining digits are the same as those in pa:
      if ((x != i) && (pa != pr))
         std_constexpr::copy(pa + i, pa + x, pr + i);
      BOOST_MP_ASSERT(0 == borrow);

      //
      // We may have lost digits, if so update limb usage count:
      //
      result.normalize();
      result.sign(a.sign());
      if (swapped)
         result.negate();
   }  // constepxr.
}

#else

template <class CppInt1, class CppInt2, class CppInt3>
inline BOOST_MP_CXX14_CONSTEXPR void add_unsigned(CppInt1& result, const CppInt2& a, const CppInt3& b) noexcept(is_non_throwing_cpp_int<CppInt1>::value)
{
   add_unsigned_constexpr(result, a, b);
}

template <class CppInt1, class CppInt2, class CppInt3>
inline BOOST_MP_CXX14_CONSTEXPR void subtract_unsigned(CppInt1& result, const CppInt2& a, const CppInt3& b) noexcept(is_non_throwing_cpp_int<CppInt1>::value)
{
   subtract_unsigned_constexpr(result, a, b);
}

#endif

} } }  // namespaces

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_PERIMETER_HPP
#define BOOST_GEOMETRY_ALGORITHMS_PERIMETER_HPP

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/geometries/concepts/check.hpp>
#include <boost/geometry/strategies/default_length_result.hpp>
#include <boost/geometry/algorithms/length.hpp>
#include <boost/geometry/algorithms/detail/calculate_null.hpp>
#include <boost/geometry/algorithms/detail/calculate_sum.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// Default perimeter is 0.0, specializations implement calculated values
template <typename Tag, typename Geometry, typename Strategy>
struct perimeter : detail::calculate_null
    <
        typename default_length_result<Geometry>::type,
        Geometry,
        Strategy
    >
{};

template <typename Geometry, typename Strategy>
struct perimeter<ring_tag, Geometry, Strategy>
    : detail::length::range_length
        <
            Geometry,
            Strategy,
            closure<Geometry>::value
        >
{};

template <typename Polygon, typename Strategy>
struct perimeter<polygon_tag, Polygon, Strategy>
    : detail::calculate_polygon_sum
        <
            typename default_length_result<Polygon>::type,
            Polygon,
            Strategy,
            detail::length::range_length
                <
                    typename ring_type<Polygon>::type,
                    Strategy,
                    closure<Polygon>::value
                >
        >
{};

// box,n-sphere: to be implemented

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

/*!
\brief \brief_calc{perimeter}
\ingroup perimeter
\details The function perimeter returns the perimeter of a geometry,
    using the default distance-calculation-strategy
\tparam Geometry \tparam_geometry
\param geometry \param_geometry
\return \return_calc{perimeter}

\qbk{[include reference/algorithms/perimeter.qbk]}
 */
template<typename Geometry>
inline typename default_length_result<Geometry>::type perimeter(
        Geometry const& geometry)
{
    concept::check<Geometry const>();

    typedef typename point_type<Geometry>::type point_type;
    typedef typename strategy::distance::services::default_strategy
        <
            point_tag, point_type
        >::type strategy_type;

    return dispatch::perimeter
        <
            typename tag<Geometry>::type,
            Geometry,
            strategy_type
        >::apply(geometry, strategy_type());
}

/*!
\brief \brief_calc{perimeter} \brief_strategy
\ingroup perimeter
\details The function perimeter returns the perimeter of a geometry,
    using specified strategy
\tparam Geometry \tparam_geometry
\tparam Strategy \tparam_strategy{distance}
\param geometry \param_geometry
\param strategy strategy to be used for distance calculations.
\return \return_calc{perimeter}

\qbk{distinguish,with strategy}
\qbk{[include reference/algorithms/perimeter.qbk]}
 */
template<typename Geometry, typename Strategy>
inline typename default_length_result<Geometry>::type perimeter(
        Geometry const& geometry, Strategy const& strategy)
{
    concept::check<Geometry const>();

    return dispatch::perimeter
        <
            typename tag<Geometry>::type,
            Geometry,
            Strategy
        >::apply(geometry, strategy);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_PERIMETER_HPP