// this must define the same symbol as the main module file (numpy requirement)
#define PY_ARRAY_UNIQUE_SYMBOL andres_graph_PyArray_API
#define NO_IMPORT_ARRAY

#include <Python.h>
#include <boost/python.hpp>
#include <vigra/numpy_array.hxx>
#include <vigra/numpy_array_converters.hxx>
#include <andres/graph/hdf5/graph.hxx>
#include <andres/graph/hdf5/grid-graph.hxx>
#include <vector>
#include <string>

#include "andres/graph/grid-graph.hxx"
#include "andres/functional.hxx"
#include "andres/graph/graph.hxx"
#include "andres/graph/threadpool.hxx"
#include "andres/graph/neighborhood.hxx"
#include "andres/graph/multicut-lifted/lifted_mc_model.hxx"

#include "image.hxx"

namespace bp = boost::python;
namespace agraph = andres::graph;

template<class MODEL>
void addLongRangeEdges(
    MODEL & model,
    vigra::NumpyArray<2, float> edgePmap, // in 0-1
    const float beta = 0.5f,
    const int minRadius = 2,
    const int maxRadius = 7
){
    GRAPH_CHECK_OP(model.originalGraph().shape(0), == ,edgePmap.shape(0),"");
    GRAPH_CHECK_OP(model.originalGraph().shape(1), == ,edgePmap.shape(1),"");

    typedef vigra::TinyVector<int,   2> Coord;
    const auto shape = edgePmap.shape();

    auto clipToImg = [&](const Coord & coord){
        auto c = coord;
        for(auto i=0; i<2; ++i){
            c[i] = std::max(0, c[i]);
            c[i] = std::min(int(shape[i]),c[i]);
        }
        return c;
    };

    // move a coordinate to local min
    auto moveToMin = [&](const Coord & coord){
        Coord coords[5] = {
                coord,
                clipToImg(coord+Coord(0,1)),
                clipToImg(coord+Coord(1,0)),
                clipToImg(coord+Coord(-1,-1)),
                clipToImg(coord+Coord(-1, 0))
        };
        auto minVal = std::numeric_limits<float>::infinity();
        auto minCoord = Coord();
        for(size_t i=0; i<5; ++i){
            const auto val = edgePmap[coord];
            if(val<minVal){
                minVal = val;
                minCoord = coord;
            }
        }
        return minCoord;
    };

    auto & originalGraph = model.originalGraph();
    auto & liftedGraph = model.liftedGraph();

    int rad = 5;

    auto pOpt = agraph::ParallelOptions();
    const auto nThreads = pOpt.numThreads(-1).getActualNumThreads();

    typedef vigra::TinyVector<float, 2> FCoord;
    auto node = [&](const Coord & coord){
        return size_t(coord[0] + coord[1]*shape[0]);
    };

    std::mutex graphMutex;
    std::mutex mapMutex;

    std::set<size_t > processed;

    struct ToAdd{
        size_t u,v;
        float w;
    };

    size_t bufferSize = 1000000;
    std::vector<std::vector<ToAdd> > buffers(nThreads);
    for(auto & vec : buffers){
        vec.reserve(bufferSize+1);
    }

    auto addToBuffer = [&](std::vector<ToAdd> & buffer,const size_t u_, const size_t v_, const float w_){

        ToAdd ta;
        ta.u=u_;
        ta.v=v_;
        ta.w=w_;
        buffer.push_back(ta);
        //std::cout<<"buffer size"<<buffer.size()<<"\n";
        if(buffer.size()>=bufferSize){

            std::unique_lock<std::mutex> lock(graphMutex);
            //std::cout<<"clear buffer\n";
            for(const auto & ta  : buffer){
                const auto fe = liftedGraph.findEdge(ta.u,ta.v);
                // not yet  in lifted graph
                // therefore cannot be processed
                if(!fe.first){
                    //std::cout<<"a\n";
                    const auto e = model.setCost(ta.u,ta.v,ta.w,false);
                    processed.insert(e);
                }
                // edge is in lifted graph
                else{
                    //std::cout<<"b\n";
                    auto fm = processed.find(fe.second);
                    // not yet processed
                    if(fm == processed.end()){
                        std::cout<<"b1\n";
                        const auto e = model.setCost(ta.u,ta.v,ta.w,false);
                        processed.insert(e);
                    }
                    else{
                        std::cout<<"b2\n";
                    }
                }
            }
            buffer.resize(0);
        }
    };

    std::cout<<"lifted graph edge num "<<liftedGraph.numberOfEdges()<<"\n";
    agraph::parallel_foreach(
        nThreads,
        shape[1],
        [&](const int threadId, const int y){
            auto & buffer = buffers[threadId];
            for(int x=0; x<shape[0]; ++x){
                const auto p = Coord(x,y);
                const auto u = node(p);
                GRAPH_CHECK_OP(u,<,originalGraph.numberOfVertices(),"");
                GRAPH_CHECK_OP(u,<,liftedGraph.numberOfVertices(),"");
                const auto start = clipToImg(p-maxRadius);
                const auto end = clipToImg(p+maxRadius+1);
                auto q = Coord();
                for(q[0]=start[0]; q[0]<end[0]; ++q[0])
                for(q[1]=start[1]; q[1]<end[1]; ++q[1]){

                    GRAPH_CHECK_OP(q[0],>=,0,"");
                    GRAPH_CHECK_OP(q[1],>=,0,"");
                    GRAPH_CHECK_OP(q[0],<,shape[0],"");
                    GRAPH_CHECK_OP(q[1],<,shape[1],"");
                    const auto v = node(q);

                    size_t e;
                    if( norm(p-q) < float(minRadius))
                        continue;
                    if(p==q || v>u){
                        continue;
                    }

                    GRAPH_CHECK_OP(v,<,originalGraph.numberOfVertices(),"");
                    GRAPH_CHECK_OP(v,<,liftedGraph.numberOfVertices(),"");
                    const auto qf = FCoord(q);
                    const auto pq = q-p;
                    const auto dist = vigra::norm(pq);
                    const auto step =  pq*(1.0f / (dist * 1.3f + 0.5f));
                    auto pOnLine = FCoord(p);
                    auto noMax = true;
                    auto maxVal = -1.0f*std::numeric_limits<float>::infinity();
                    while(Coord(pOnLine)!=q){
                        //std::cout<<"pol "<<pOnLine<<"\n";
                        auto iCord = Coord(pOnLine);
                        if(iCord != p){
                            noMax = false;
                            maxVal = std::max(edgePmap[iCord], maxVal);
                        }
                        pOnLine += step;
                    }
                    const double p1 = std::max(std::min(maxVal,0.999f),0.001f);
                    const double p0 = 1.0 - p1;
                    auto w = std::log(p0/p1) + beta;

                    addToBuffer(buffer, u,v,w);
                }
            }
        }
    );

    // clear whats left in buffers
    for(const auto & buffer : buffers){
        for(const auto & ta : buffer){
            const auto fe = liftedGraph.findEdge(ta.u,ta.v);
            // not yet  in lifted graph
            // therefore cannot be processed
            if(!fe.first){
                const auto e = model.setCost(ta.u,ta.v,ta.w,false);
                processed.insert(e);
            }
            // edge is in lifted graph
            else{
                auto fm = processed.find(fe.second);
                // not yet processed
                if(fm == processed.end()){
                    const auto e = model.setCost(ta.u,ta.v,ta.w,false);
                    processed.insert(e);
                }
            }
        }
    }
    std::cout<<"lifted graph edge num "<<liftedGraph.numberOfEdges()<<"\n";
}

template<class MODEL>
void addLongRangeNH(
    MODEL & model,
    const int radius = 7
){
    for(auto u=0; u<model.originalGraph().numberOfVertices(); ++u){
        const auto otherNodes = agraph::verticesInGraphNeigborhood(model.originalGraph(), u, radius);
        for(const auto v : otherNodes){
            if(u!=v){
                model.setCost(u,v,0.0);
            }
        }
    }
}

template<class LiftedMcModel>
double evalCut(
    LiftedMcModel & liftedMcModel,
    vigra::NumpyArray<1, uint8_t> cut
){
    auto s = 0.0;
    for(auto e=0; e<liftedMcModel.liftedGraph().numberOfEdges(); ++e){
        if(cut[e]){
            s += liftedMcModel.edgeCosts()[e];
        }
    }
    return s;
}

template<class LiftedMcModel>
void setCosts(
    LiftedMcModel & liftedMcModel,
    vigra::NumpyArray<1, vigra::TinyVector<uint64_t, 2> > uv,
    vigra::NumpyArray<1, float> costs,
    const bool overwrite
){
    GRAPH_CHECK_OP(uv.size(), == , costs.size(), "shape mismatch: uv and costs have different size");

    const auto u = uv.bindElementChannel(0);
    const auto v = uv.bindElementChannel(1);
    const auto c = vigra::MultiArrayView<1, float>(costs);
    liftedMcModel.setCosts(u.begin(), u.end(), v.begin(), c.begin(), overwrite);
}

template<class LiftedMcModel>
vigra::NumpyAnyArray edgeLabelsToNodeLabels(
    const LiftedMcModel & model,
    vigra::NumpyArray<1, uint8_t> edgeLabels,
    vigra::NumpyArray<1, uint64_t> nodeLabels
){
    vigra::TinyVector<int,1> shape(model.originalGraph().numberOfVertices());
    nodeLabels.reshapeIfEmpty(shape);
    model.getNodeLabels(edgeLabels, nodeLabels);
    return nodeLabels;
}

template<class LiftedMcModel>
vigra::NumpyAnyArray nodeLabelsToEdgeLabels(
    const LiftedMcModel & model,
    vigra::NumpyArray<1, uint64_t> nodeLabels,
    vigra::NumpyArray<1, uint8_t> edgeLabels
){
    vigra::TinyVector<int,1> shape(model.liftedGraph().numberOfEdges());
    edgeLabels.reshapeIfEmpty(shape);
    model.getEdgeLabels(nodeLabels, edgeLabels);
    return edgeLabels;
}

template<class LiftedMcModel>
void fuseGtObjective(
    LiftedMcModel & model,
    vigra::NumpyArray<2, uint64_t> nodeLabels,
    const size_t rr,
    const double beta,
    const bool verbose
){
    if(verbose)
        std::cout<<"nodeLabel.shape "<<nodeLabels.shape()<<"\n";

    const auto & originalGraph = model.originalGraph();
    const auto & liftedGraph = model.liftedGraph();
    auto nV = originalGraph.numberOfVertices();
    auto nGt = nodeLabels.shape(1);

    std::vector<std::set<size_t> > extendedNh(nV);
    std::vector<std::set<size_t> > extendedNh2(nV);

    for(auto n=0; n<originalGraph.numberOfVertices(); ++n){
        for(auto iter = originalGraph.verticesFromVertexBegin(n); iter!=originalGraph.verticesFromVertexEnd(n); ++iter){
            extendedNh[n].insert(*iter);
            extendedNh2[n].insert(*iter);
        }
    }

    for(auto r=0;r<2;++r){

        for(auto n=0; n<originalGraph.numberOfVertices(); ++n){
            auto & thisNodeNhSet = extendedNh2[n];
            for(auto iter = originalGraph.verticesFromVertexBegin(n); iter!=originalGraph.verticesFromVertexEnd(n); ++iter){
                auto otherNode = *iter;
                const auto & otherNodeNhSet = extendedNh[otherNode];
                thisNodeNhSet.insert(otherNodeNhSet.begin(), otherNodeNhSet.end());
                thisNodeNhSet.erase(n);
            }
        }
        extendedNh = extendedNh2;
    }

    for(auto u=0; u<originalGraph.numberOfVertices(); ++u){
        const auto & enh = extendedNh[u];
        //std::cout<<"node "<<u<<" |enh| = "<<enh.size()<<"\n";

        const auto uGt = nodeLabels.bindInner(u);
        for(const auto v : enh){
            const auto vGt = nodeLabels.bindInner(v);
            auto p1 = 0.0;
            for(size_t gtc=0; gtc<nGt; ++gtc){
                p1 += uGt[gtc] != vGt[gtc] ?  1 : 0;
            }
            p1 /= nGt;
            //if (p1>0.1)
            //    std::cout<<"   p1 "<<p1<<"\n";

            p1 = std::max(std::min(0.999, p1),0.001);
            auto p0 = 1.0 - p1;
            auto w = std::log(p0/p1) + std::log((1.0-beta)/beta);
            model.setCost(u,v,w);
        }
    }
}

template<class LiftedMcModel>
void fuseGtObjectiveGrid(
    LiftedMcModel & model,
    vigra::NumpyArray<3, uint64_t> nodeLabels,
    vigra::NumpyArray<1, double>    pExpert,
    vigra::NumpyArray<2, float>     cutRegularizer,
    const size_t rr,
    const double beta,
    //const double cLocal,
    const bool verbose
){
    if(verbose)
        std::cout<<"nodeLabel.shape "<<nodeLabels.shape()<<"\n";

    const auto & originalGraph = model.originalGraph();
    const auto & liftedGraph = model.liftedGraph();
    auto nV = originalGraph.numberOfVertices();
    typedef vigra::TinyVector<int,2> Coord;
    Coord shape(originalGraph.shape(0), originalGraph.shape(1));

    auto nGt = nodeLabels.shape(2);

    std::vector<std::set<size_t> > extendedNh(nV);
    std::vector<std::set<size_t> > extendedNh2(nV);

    auto clipToImg = [&](const Coord & coord){
        auto c = coord;
        for(auto i=0; i<2; ++i){
            c[i] = std::max(0, c[i]);
            c[i] = std::min(int(shape[i]),c[i]);
        }
        return c;
    };
    auto node = [&](const Coord & coord){
        return size_t(coord[0] + coord[1]*shape[0]);
    };

    Coord p;

    for(p[1]=0; p[1]<shape[1]; ++p[1])
    for(p[0]=0; p[0]<shape[0]; ++p[0]){

        const auto u = node(p);
        const auto start = clipToImg(p-int(rr));
        const auto end = clipToImg(p+int(rr)+1);

        auto lp = nodeLabels.bindInner(p[0]).bindInner(p[1]);
        auto q = Coord();

        for(q[1]=start[1]; q[1]<end[1]; ++q[1])
        for(q[0]=start[0]; q[0]<end[0]; ++q[0]){
            const auto v = node(q);
            if(p!=q && u < v){
                auto d = vigra::norm(p-q);
                if(d<=float(rr)){
                    if(d<=1.5)
                        d*=0.5;
                    auto lq = nodeLabels.bindInner(q[0]).bindInner(q[1]);
                    auto p0 = 0.0;
                    auto p1 = 0.0;
                    for(size_t gtc=0; gtc<nGt; ++gtc){
                        p0 += (lp[gtc] == lq[gtc]) ?  pExpert(gtc) : 0.0;
                        p1 += (lp[gtc] != lq[gtc]) ?  pExpert(gtc) : 0.0;
                    }
                    auto Z = p0+p1;
                    p0/=Z;
                    p1/=Z;
                    //if (p1>0.1)
                    //    std::cout<<"   p1 "<<p1<<"\n";

                    p1 = std::max(std::min(0.99999, p1),0.00001);
                    p0 = 1.0 - p1;
                    auto w = std::log(p0/p1) + std::log((1.0-beta)/beta);
                    if(d<=1.01){
                        auto c = (cutRegularizer[p] + cutRegularizer[q])/2.0;
                        w += c;
                    }
                    model.setCost(u,v,w*d);
                }
            }
        }
    }
}

template<class LiftedMcModel>
void thinObjectSeededSeg(
    LiftedMcModel & model,
    vigra::NumpyArray<2, uint64_t>  linkConstraints,
    vigra::NumpyArray<2, float>     cutCosts,
    const double constraintCost,
    const bool verbose
){

    auto shape = linkConstraints.shape();
    auto tshape = cutCosts.shape();
    GRAPH_CHECK_OP(shape[0],==,model.originalGraph().shape(0),"");
    GRAPH_CHECK_OP(shape[1],==,model.originalGraph().shape(1),"");

    GRAPH_CHECK_OP(shape[0]*2-1,==,tshape[0],"");
    GRAPH_CHECK_OP(shape[1]*2-1,==,tshape[1],"");

    typedef vigra::TinyVector<int,2> Coord;
    ///  link constraints
    std::map<uint64_t, std::vector<size_t> >   constraintSet;

    auto node = [&](const Coord & c){
        return c[0] + c[1]*shape[0];
    };
    std::cout<<"local terms\n";
    for(auto y=0; y<shape[1]; ++y)
    for(auto x=0; x<shape[0]; ++x){
        const Coord c(x,y);
        auto u = node(c);
        if(linkConstraints(x,y)!=255){
            constraintSet[linkConstraints(x,y)].push_back(u);
        }
        if(x+1<shape[0]){
            const Coord c2(x+1,y);
            const auto v = node(c2);
            auto val = cutCosts[c+c2];
            model.setCost(u,v,val);
        }
        if(y+1<shape[1]){
            const Coord c2(x,y+1);
            const auto v = node(c2);
            auto val = cutCosts[c+c2];
            model.setCost(u,v,val);
        }
    }
    std::cout<<"to vecvec\n";
    std::vector< std::vector<size_t> >   cVecVec;
    for(auto iter = constraintSet.begin(); iter!=constraintSet.end(); ++iter){
        cVecVec.push_back(iter->second);
    }
    std::cout<<"must link terms\n";
    // must link constraints
    for(auto & inodes : cVecVec){
        GRAPH_CHECK_OP(inodes.size(),>=,1,"");
        std::cout<<"inodes "<<inodes.size()<<"\n";
        if(inodes.size() >1){
            for(auto i=0; i<inodes.size()-1; ++i)
            for(auto j=i+1; j<inodes.size(); ++j){
                model.setCost(inodes[i],inodes[j], constraintCost);
            }
        }
    }
    std::cout<<"cannot link terms\n";
    // cannot link constraints
    for(auto i=0; i<cVecVec.size()-1; ++i){
        const auto & nodesI = cVecVec[i];
        for(auto j=i+1; j<cVecVec.size(); ++j){
            const auto & nodesJ = cVecVec[j];

            for(auto in : nodesI)
            for(auto jn : nodesJ){
                model.setCost(in,jn,-1.0*constraintCost);
            }
        }
    }

}

template<class LiftedMcModel>
void lmSuperpixel(
    LiftedMcModel & model,
    vigra::NumpyArray<2, float>     cutCosts,
    const size_t seedRadius = 10,
    const size_t stepSize = 2,
    const double sigma = 3.0,
    const double seedRepulsion = 1.0
){

    const int shape[2] = {
        int(model.originalGraph().shape(0)),
        int(model.originalGraph().shape(1))
    };
    auto tshape = cutCosts.shape();

    typedef vigra::TinyVector<int,2> Coord;

    auto node = [&](const Coord & c){
        return c[0] + c[1]*shape[0];
    };
    std::cout<<"generate seeds\n";

    std::vector<Coord> seeds;

    for(auto y=seedRadius+1; y<shape[1]-seedRadius-1; y += seedRadius)
    for(auto x=seedRadius+1; x<shape[0]-seedRadius-1; x += seedRadius){
        const Coord c(x,y);
        seeds.push_back(c);
    }

    std::cout<<"add repulsion number of seeds "<<seeds.size()<<"\n";
    GRAPH_CHECK_OP(seeds.size(),>=,2,"seedRadius is to large");
    // seed repulsion
    auto c=0;
    auto r=0;
    for(auto si=0; si<seeds.size()-1; ++si)
    for(auto sj=si+1; sj<seeds.size(); ++sj){
        const auto & ci = seeds[si];
        const auto & cj = seeds[sj];
        if(vigra::norm(ci-cj)<5*seedRadius){
            model.setCost(node(ci),node(cj),-1.0*seedRepulsion*seedRadius*100.0);
            ++c;
        }
        else{
            ++r;
        }
    }
    std::cout<<"repulsive edges "<<c<<" rejected "<<r<<"\n";

    std::cout<<"add seed attractiveness \n";

    auto nd = [&](const double dist){
        return (1.0/(sigma*std::sqrt(2.0*3.1415926))) *std::exp(-0.5*std::pow(dist/sigma,2));
    };
    for(auto & uCoord  : seeds){
        Coord vCoord;
        for(vCoord[0] = uCoord[0]-seedRadius; vCoord[0]<uCoord[0]+seedRadius+1;++vCoord[0])
        for(vCoord[1] = uCoord[1]-seedRadius; vCoord[1]<uCoord[1]+seedRadius+1;++vCoord[1]){
            auto  d = vigra::norm(uCoord-vCoord);
            auto  w = nd(d);
            model.setCost(node(uCoord), node(vCoord), w*20.0);
        }
    }

    std::cout<<"add grid graph cost attractiveness \n";
    std::cout<<"local terms\n";
    for(auto y=0; y<shape[1]; ++y)
    for(auto x=0; x<shape[0]; ++x){
        const Coord c(x,y);
        auto u = node(c);
        if(x+1<shape[0]){
            const Coord c2(x+1,y);
            const auto v = node(c2);
            auto val = cutCosts[c+c2];
            model.setCost(u,v,val);
        }
        if(y+1<shape[1]){
            const Coord c2(x,y+1);
            const auto v = node(c2);
            auto val = cutCosts[c+c2];
            model.setCost(u,v,val);
        }
    }
    std::cout<<"build model done\n";
}

template<class LiftedMcModel>
void andresImageSegModel(
    LiftedMcModel & model,
    vigra::NumpyArray<2, float> pmapImage,
    const float bias = 0.5
){
    const auto & shape = pmapImage.shape();
    // convert to andres image
    Image image(shape.begin(),shape.begin());
    for(auto y=0; y<shape[1]; ++y)
    for(auto x=0; x<shape[0]; ++x){
        image(x,y) = pmapImage(x,y);
    }

    const auto & graph = model.originalGraph();

    // boundary-probability-to-multicut-problem-image.cxx
    std::vector<float> edgeCutProbabilities;
    PixelCutProbabilityFromEdgesWTA<float, size_t> pixelCutProbabilityWTA(image);
    constructGraphAndEdgeProbabilities(pixelCutProbabilityWTA, graph, edgeCutProbabilities, bias);

}

template<class LiftedMcModel>
void loadBsdModel(LiftedMcModel & model, const std::string & filename){
    auto fileHandle = agraph::hdf5::openFile(filename);
    agraph::hdf5::load(fileHandle,"graph",model._originalGraph());
    agraph::hdf5::load(fileHandle,"graph-lifted",model._liftedGraph());
    std::vector<size_t> shape;
    auto & ec = model._edgeCosts();
    //ec.resize(model.liftedGraph().numberOfEdges());
    std::vector<double > ecd;
    agraph::hdf5::load(fileHandle, "edge-cut-probabilities", shape, ecd);

    transform(
       ecd.begin(),
       ecd.end(),
       ecd.begin(),
       andres::NegativeLogProbabilityRatio<double,double>()
    );

    ec.resize(ecd.size());
    std::copy(ecd.begin(), ecd.end(),ec.begin());
    agraph::hdf5::closeFile(fileHandle);

    transform(
       ec.begin(),
       ec.end(),
       ec.begin(),
       andres::NegativeLogProbabilityRatio<double,double>()
    );

}

template<class OG, class F>
void exportLiftedMcModelT(const std::string & clsName, F && f){
    typedef OG originalGraph;
    typedef agraph::multicut_lifted::LiftedMcModel<originalGraph, float> LiftedMcModel;

    auto cls = bp::class_<LiftedMcModel>
    (
        clsName.c_str(),
        bp::init<
            const originalGraph&
        >(
            bp::arg("originalGraph")
        )[bp::with_custodian_and_ward<1 /*custodian == self*/, 2 /*ward == const originalGraph & */>()]
    )
        .def("originalGraph",&LiftedMcModel::originalGraph , bp::return_internal_reference<>())
        .def("liftedGraph",&LiftedMcModel::liftedGraph , bp::return_internal_reference<>())
        .def("_setCosts",vigra::registerConverters(&setCosts<LiftedMcModel>))
        .def("_setCost",&LiftedMcModel::setCost)
        .def("evalCut",vigra::registerConverters(&evalCut<LiftedMcModel>))
        .def("edgeLabelsToNodeLabels",vigra::registerConverters(&edgeLabelsToNodeLabels<LiftedMcModel>),
            (
                bp::arg("edgeLabels"),
                bp::arg("out") = bp::object()
            )
        )
        .def("loadBsdModel", &loadBsdModel<LiftedMcModel>)
        .def("nodeLabelsToEdgeLabels",vigra::registerConverters(&nodeLabelsToEdgeLabels<LiftedMcModel>),
            (
                bp::arg("nodeLabels"),
                bp::arg("out") = bp::object()
            )
        );

    f(cls);

    bp::def("fuseGtObjective",vigra::registerConverters(&fuseGtObjective<LiftedMcModel>),
        (
            bp::arg("model"),
            bp::arg("nodeLabels"),
            bp::arg("rr") = 2,
            bp::arg("beta") = 0.5,
            bp::arg("verbose") = true
        )
    );
}

template<class MODEL>
vigra::NumpyAnyArray flattenLabels(
    const MODEL & model,
    vigra::NumpyArray<2, uint64_t> labels2d,
    vigra::NumpyArray<1, uint64_t> out
){
    vigra::TinyVector<int, 1> shape(labels2d.size());
    out.reshapeIfEmpty(shape);
    auto c=0;
    for(auto y=0; y<labels2d.shape(1); ++y)
    for(auto x=0; x<labels2d.shape(0); ++x){
        out[c] = labels2d(x,y);
        ++c;
    }
    return out;
}

void exportLiftedMcModel(){
    typedef agraph::GridGraph<2> GridGraph2D;
    typedef agraph::GridGraph<3> GridGraph3D;
    typedef agraph::Graph<> Graph;
    typedef agraph::multicut_lifted::LiftedMcModel<GridGraph2D, float> LiftedMcModelGridGraph2D;
    typedef agraph::multicut_lifted::LiftedMcModel<Graph, float> LiftedMcModelGraph;
    {
        typedef agraph::multicut_lifted::LiftedMcModel<GridGraph2D, float> LiftedMcModelGridGraph2D;
        exportLiftedMcModelT<GridGraph2D>("LiftedMcModelGridGraph2D",
            [&](
                bp::class_< LiftedMcModelGridGraph2D > & cls
            ){
                cls
                    .def("flattenLabels",
                        vigra::registerConverters(&flattenLabels< LiftedMcModelGridGraph2D >),
                        (
                            bp::arg("labels"),
                            bp::arg("out") =  bp::object()
                        )
                    )
                ;
            }
        );
    }
    exportLiftedMcModelT<GridGraph3D>("LiftedMcModelGridGraph3D",
        [&](
            bp::class_<agraph::multicut_lifted::LiftedMcModel<GridGraph3D, float> > & cls
        ){

        }
    );
    exportLiftedMcModelT<Graph>("LiftedMcModelGraph",
        [&](
            bp::class_<agraph::multicut_lifted::LiftedMcModel<Graph, float> > & cls
        ){

        }
    );

    bp::def("addLongRangeEdges",vigra::registerConverters(&addLongRangeEdges<LiftedMcModelGridGraph2D>))
    ;

    bp::def("addLongRangeNH",vigra::registerConverters(&addLongRangeNH<LiftedMcModelGridGraph2D>))
    ;
    bp::def("addLongRangeNH",vigra::registerConverters(&addLongRangeNH<LiftedMcModelGraph>))
    ;

    bp::def("fuseGtObjectiveGrid",vigra::registerConverters(&fuseGtObjectiveGrid<LiftedMcModelGridGraph2D>),
        (
            bp::arg("model"),
            bp::arg("nodeLabels"),
            bp::arg("pExpert"),
            bp::arg("cutRegularizer"),
            bp::arg("rr") = 2,
            bp::arg("beta") = 0.5,
            //bp::arg("cLocal") = 100,
            bp::arg("verbose") = true
        )
    );

    bp::def("thinObjectSeededSeg",vigra::registerConverters(&thinObjectSeededSeg<LiftedMcModelGridGraph2D>),
        (
            bp::arg("model"),
            bp::arg("linkConstraints"),
            bp::arg("cutCosts"),
            bp::arg("c") = 1000.0,
            bp::arg("verbose") = true
        )
    );

    bp::def("lmSuperpixel",vigra::registerConverters(&lmSuperpixel<LiftedMcModelGridGraph2D>),
        (
            bp::arg("model"),
            bp::arg("cutCosts"),
            bp::arg("seedRadius") = 10,
            bp::arg("stepSize") = 2,
            bp::arg("sigma") = 3.0,
            bp::arg("seedRepulsion") =1.0
        )
    );

    bp::def("andresImageSegModel",vigra::registerConverters(&andresImageSegModel<LiftedMcModelGridGraph2D>),
        (
            bp::arg("model"),
            bp::arg("pmap"),
            bp::arg("bias") = 0.5
        )
    );
}


#include <cstdio>
#include <cmath>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <cfloat>
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>
#include <boost/filesystem.hpp>
#include <json/json.h>
#include <H5Cpp.h>

// OpenMP
#include <omp.h>

#include "box_triangle/aabb_triangle_overlap.h"
#include "box_ray/vector3.h"
#include "box_ray/ray.h"
#include "box_ray/box.h"
#include "triangle_point/vec.h"
#include "triangle_point/makelevelset3.h"
#include "triangle_ray/raytri.h"

/** \brief Simple struct representing arbitrary boxes. */
struct Box {
  /** \brief Minimum coordinates. */
  Eigen::Vector3f min;
  /** \brief Maximum coordinates. */
  Eigen::Vector3f max;

  /** \brief Constructor.
   */
  Box() {
    this->min = Eigen::Vector3f::Zero();
    this->max = Eigen::Vector3f::Zero();
  }

  /** \brief Constructor; construct a box from minimum and maximum coordinates.
   * \param[in] min minimum coordinates
   * \param[in] max maximum coordinates
   */
  Box(const Eigen::Vector3f &min, const Eigen::Vector3f &max) {
    this->min = min;
    this->max = max;
  }

  /** \brief Check if a point is inside the box.
   * \param[in] point point to check
   * \return contained
   */
  bool contains(const Eigen::Vector3f &point) {
    bool contains = true;

    for (int d = 0; d < 3; d++) {
      if (point(d) < this->min(d) || point(d) > this->max(d)) {
        contains = false;
      }
    }

    return contains;
  }

  /** \brief Scale the box.
   * \param[in] scale scale per axis
   */
  void scale(const Eigen::Vector3f &scale) {
    for (int d = 0; d < 3; d++) {
      min(d) *= scale(d);
      max(d) *= scale(d);
    }
  }

  /** \brief Scale the box.
   * \param[in] scale scale over all axes
   */
  void scale(float scale) {
    this->scale(Eigen::Vector3f(scale, scale, scale));
  }

  /** \brief Scale the box.
   * \param[in] translation translation per axis
   */
  void translate(const Eigen::Vector3f &translation) {
    for (int d = 0; d < 3; d++) {
      min(d) += translation(d);
      max(d) += translation(d);
    }
  }
};

/** \brief Test box ray intersection.
 * \param[in] box box
 * \param[in] ray ray to intersect with
 * \return intersects
 */
bool box_ray_intersection(const Box &box, const Eigen::Vector3f &ray) {
  for (int d = 0; d < 3; d++) {
    assert(box.min(d) < box.max(d));
  }

  // convert to used data structure
  bri::Vector3 _o(0, 0, 0);
  bri::Vector3 _d(ray(0), ray(1), ray(2));
  bri::Ray _ray(_o, _d);
  bri::Vector3 _min(box.min(0), box.min(1), box.min(2));
  bri::Vector3 _max(box.max(0), box.max(1), box.max(2));
  bri::Box _box(_min, _max);

  // interval for valid its is [0,1] as the ray will
  // be the full-length vector to an observed point
  return _box.intersect(_ray, 0, 0.99);
}

/** \brief Test box ray intersection.
 * \param[in] box box
 * \param[in] origin origin point
 * \param[in] dest destination point
 * \return intersects
 */
bool box_ray_intersection(const Box &box, const Eigen::Vector3f &origin, const Eigen::Vector3f &dest) {
  for (int d = 0; d < 3; d++) {
    assert(box.min(d) < box.max(d));
  }

  // convert to used data structure
  bri::Vector3 _o(origin(0), origin(1), origin(2));
  bri::Vector3 _d(dest(0), dest(1), dest(2));
  bri::Ray _ray(_o, _d);
  bri::Vector3 _min(box.min(0), box.min(1), box.min(2));
  bri::Vector3 _max(box.max(0), box.max(1), box.max(2));
  bri::Box _box(_min, _max);

  // interval for valid its is [0,1] as the ray will
  // be the full-length vector to an observed point
  return _box.intersect(_ray, 0, 0.99);
}

/** \brief Compute triangle box intersection.
 * \param[in] voxel voxel to test intersection for
 * \param[in] v1 first vertex
 * \param[in] v2 second vertex
 * \param[in] v3 third vertex
 * \return intersects
 */
bool triangle_box_intersection(const Box &voxel, const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const Eigen::Vector3f &v3) {
  float half_size[3] = {
    (voxel.max(0) - voxel.min(0))/2.,
    (voxel.max(1) - voxel.min(1))/2.,
    (voxel.max(2) - voxel.min(2))/2.
  };

  float center[3] = {
    voxel.max(0) - half_size[0],
    voxel.max(1) - half_size[1],
    voxel.max(2) - half_size[2]
  };

  float vertices[3][3] = {{v1(0), v1(1), v1(2)}, {v2(0), v2(1), v2(2)}, {v3(0), v3(1), v3(2)}};
  return triBoxOverlap(center, half_size, vertices);
}

/** \brief Compute triangle point distance and corresponding closest point.
 * \param[in] point point
 * \param[in] v1 first vertex
 * \param[in] v2 second vertex
 * \param[in] v3 third vertex
 * \param[out] ray corresponding closest point
 * \return distance
 */
float triangle_point_distance(const Eigen::Vector3f &point, const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const Eigen::Vector3f &v3,
    Eigen::Vector3f &closest_point) {

  Vec3f x0(point.data());
  Vec3f x1(v1.data());
  Vec3f x2(v2.data());
  Vec3f x3(v3.data());

  Vec3f r(0);
  float distance = point_triangle_distance_field(x0, x1, x2, x3, r);

  for (int d = 0; d < 3; d++) {
    closest_point(d) = r[d];
  }

  return distance;
}

/** \brief Test triangle ray intersection.
 * \param[in] origin origin of ray
 * \param[in] dest destination of ray
 * \param[in] v1 first vertex
 * \param[in] v2 second vertex
 * \param[in] v3 third vertex
 * \return intersects
 */
bool triangle_ray_intersection(const Eigen::Vector3f &origin, const Eigen::Vector3f &dest,
    const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const Eigen::Vector3f &v3, float &t) {

  double _origin[3] = {origin(0), origin(1), origin(2)};
  double _dir[3] = {dest(0) - origin(0), dest(1) - origin(1), dest(2) - origin(2)};
  double _v1[3] = {v1(0), v1(1), v1(2)};
  double _v2[3] = {v2(0), v2(1), v2(2)};
  double _v3[3] = {v3(0), v3(1), v3(2)};

  // t is the distance, u and v are barycentric coordinates
  // http://fileadmin.cs.lth.se/cs/personal/tomas_akenine-moller/code/raytri_tam.pdf
  double _t, u, v;
  int success = intersect_triangle(_origin, _dir, _v1, _v2, _v3, &_t, &u, &v);
  t = _t;

  if (success) {
    return true;
  }

  return false;
}

/** \brief Simple struct encapsulating a 3D bounding box. */
struct BoundingBox {
  /** \brief Size of the bounding box. */
  Eigen::Vector3f size;
  /** \brief Translation (i.e. center) of bounding box. */
  Eigen::Vector3f translation;
  /** \brief Rotation of bounding box (in radians per axis). */
  Eigen::Vector3f rotation;
  /** \brief Meta information for writing. */
  std::string meta;

  /** \brief Constructor.
   */
  BoundingBox() {
    this->size = Eigen::Vector3f::Zero();
    this->translation = Eigen::Vector3f::Zero();
    this->rotation = Eigen::Vector3f::Zero();
    this->meta = "";
  }

  /** \brief Assignment operator.
   * \param[in] bounding_box bounding box to assign
   * \return this
   */
  BoundingBox& operator=(const BoundingBox &bounding_box) {
    this->size = bounding_box.size;
    this->translation = bounding_box.translation;
    this->rotation = bounding_box.rotation;
    this->meta = bounding_box.meta;
    return *this;
  }

  /** \brief Convert to regular box.
   * \param[out] box box to convert to
   */
  void to_box(Box &box) const {
    for (int d = 0 ; d < 3; d++) {
      box.min(d) = this->translation(d) - this->size(d)/2;
      box.max(d) = this->translation(d) + this->size(d)/2;
    }
  }

  /** \brief Given the angle in radians, construct a rotation matrix around the y-axis.
   * \param[in] radians angle in radians
   * \param[out] rotation rotation matrix
   */
  static void rotation_matrix_y(const float radians, Eigen::Matrix3f &rotation) {
    rotation = Eigen::Matrix3f::Zero();

    rotation(0, 0) = std::cos(radians); rotation(0, 2) = std::sin(radians);
    rotation(1, 1) = 1;
    rotation(2, 0) = -std::sin(radians); rotation(2, 2) = std::cos(radians);
  }

  /** \brief Check if a point lies inside the bounding box.
   * \param[in] point point to check
   * \return contained in bounding box
   */
  bool contains(const Eigen::Vector3f &point) const {
    Eigen::Vector3f transformed = point - this->translation;
    Eigen::Matrix3f rotation;
    BoundingBox::rotation_matrix_y(this->rotation(1), rotation);
    transformed = rotation*transformed;

    bool contained = true;
    for (int d = 0; d < 3; d++) {
      if (transformed(d) > this->size(d)/2 || transformed(d) < -this->size(d)/2) {
        contained = false;
      }
    }

    return contained;
  }

  /** \brief Get largest scale of bounding box.
   * \return scale
   */
  float scale() const {
    float scale = 0;

    for (int d = 0; d < 3; d++) {
      if (this->size(d) > scale) {
        scale = this->size(d);
      }
    }

    assert(scale > 0);
    return scale;
  }
};

/** \brief Just encapsulating vertices and faces. */
class Mesh {
public:
  /** \brief Empty constructor. */
  Mesh() {

  }

  /** \brief Add a vertex.
   * \param[in] vertex vertex to add
   */
  void add_vertex(Eigen::Vector3f& vertex) {
    this->vertices.push_back(vertex);
  }

  /** \brief Get the number of vertices.
   * \return number of vertices
   */
  int num_vertices() {
    return static_cast<int>(this->vertices.size());
  }

  /** \brief Add a face.
   * \param[in] face face to add
   */
  void add_face(Eigen::Vector3i& face) {
    this->faces.push_back(face);
  }

  /** \brief Get the number of faces.
   * \return number of faces
   */
  int num_faces() {
    return static_cast<int>(this->faces.size());
  }

  /** \brief Translate the mesh.
   * \param[in] translation translation vector
   */
  void translate(const Eigen::Vector3f& translation) {
    for (int v = 0; v < this->num_vertices(); ++v) {
      for (int i = 0; i < 3; ++i) {
        this->vertices[v](i) += translation(i);
      }
    }
  }

  /** \brief Scale the mesh.
   * \param[in] scale scale vector
   */
  void scale(const Eigen::Vector3f& scale) {
    for (int v = 0; v < this->num_vertices(); ++v) {
      for (int i = 0; i < 3; ++i) {
        this->vertices[v](i) *= scale(i);
      }
    }
  }

  /** \brief Voxelize the given mesh into a dense volume.
   * \param[in] mesh mesh to voxelize
   * \param[in] n batch index in dense
   * \param[in] dense dense pre-initialized volume
   */
  void voxelize_UNSCALED(int n, Eigen::Tensor<float, 4, Eigen::RowMajor>& sdf) {

    int height = sdf.dimension(1);
    int width = sdf.dimension(2);
    int depth = sdf.dimension(3);

    for (int h = 0; h < height; ++h) {
      for (int w = 0; w < width; ++w) {
        for (int d = 0; d < depth; ++d) {
          sdf(n, h, w, d) = FLT_MAX;

          // [Data] 99% percentile dimensions:
          // [Data]   4.750000 1.870000 1.970000
          // [Data]   2.540102 1        1.053475
          // [Data]   56       22       24

          float padding_factor = 1.2;
          float width_factor = (padding_factor*2.545454545)/2.545454545; // = padding_factor
          float height_factor = padding_factor/2.545454545;
          float depth_factor = (padding_factor*1.090909091)/2.545454545;

          // the box corresponding to this voxel
          Eigen::Vector3f min(width_factor*static_cast<float>(w)/width, height_factor*static_cast<float>(h)/height,
            depth_factor*static_cast<float>(d)/depth);
          Eigen::Vector3f max(width_factor*static_cast<float>(w + 1)/width, height_factor*static_cast<float>(h + 1)/height,
            depth_factor*static_cast<float>(d + 1)/depth);

          Box voxel_box(min, max);
          voxel_box.translate(Eigen::Vector3f(-width_factor/2, -height_factor/2, -depth_factor/2));
          Eigen::Vector3f center((voxel_box.max(0) + voxel_box.min(0))/2., (voxel_box.max(1) + voxel_box.min(1))/2., (voxel_box.max(2) + voxel_box.min(2))/2.);

          // count number of intersections.
          int num_intersect = 0;
          for (unsigned int f = 0; f < this->faces.size(); ++f) {

            Eigen::Vector3f v1 = this->vertices[this->faces[f](0)];
            Eigen::Vector3f v2 = this->vertices[this->faces[f](1)];
            Eigen::Vector3f v3 = this->vertices[this->faces[f](2)];

            Eigen::Vector3f closest_point;
            triangle_point_distance(center, v1, v2, v3, closest_point);
            float distance = (center - closest_point).norm();

            if (distance < sdf(n, h, w, d)) {
              sdf(n, h, w, d) = distance;
            }

            bool intersect = triangle_ray_intersection(center, Eigen::Vector3f(0, 0, 0), v1, v2, v3, distance);

            if (intersect && distance >= 0) {
              num_intersect++;
            }
          }

          if (num_intersect%2 == 1) {
            sdf(n, h, w, d) *= -1;
          }
        }
      }
    }
  }

  /** \brief Voxelize the given mesh into a dense volume.
   * \param[in] mesh mesh to voxelize
   * \param[in] n batch index in dense
   * \param[in] dense dense pre-initialized volume
   */
  void voxelize_SCALED(int n, Eigen::Tensor<float, 4, Eigen::RowMajor>& sdf) {

    int height = sdf.dimension(1);
    int width = sdf.dimension(2);
    int depth = sdf.dimension(3);

    #pragma omp parallel
    {
      #pragma omp for
      for (int i = 0; i < height*width*depth; i++) {
        int d = i%depth;
        int w = (i/depth)%width;
        int h = (i/depth)/width;
        sdf(n, h, w, d) = FLT_MAX;

        // the box corresponding to this voxel
        Eigen::Vector3f min(static_cast<float>(w), static_cast<float>(h), static_cast<float>(d));
        Eigen::Vector3f max(static_cast<float>(w + 1), static_cast<float>(h + 1), static_cast<float>(d + 1));

        Box voxel_box(min, max);
        //voxel_box.translate(Eigen::Vector3f(-static_cast<float>(width)/2, -static_cast<float>(height)/2, -static_cast<float>(depth)/2));
        Eigen::Vector3f center((voxel_box.max(0) + voxel_box.min(0))/2., (voxel_box.max(1) + voxel_box.min(1))/2., (voxel_box.max(2) + voxel_box.min(2))/2.);

        // count number of intersections.
        int num_intersect = 0;
        for (unsigned int f = 0; f < this->faces.size(); ++f) {

          Eigen::Vector3f v1 = this->vertices[this->faces[f](0)];
          Eigen::Vector3f v2 = this->vertices[this->faces[f](1)];
          Eigen::Vector3f v3 = this->vertices[this->faces[f](2)];

          Eigen::Vector3f closest_point;
          triangle_point_distance(center, v1, v2, v3, closest_point);
          float distance = (center - closest_point).norm();

          if (distance < sdf(n, h, w, d)) {
            sdf(n, h, w, d) = distance;
          }

          bool intersect = triangle_ray_intersection(center, Eigen::Vector3f(0, 0, 0), v1, v2, v3, distance);

          if (intersect && distance >= 0) {
            num_intersect++;
          }
        }

        if (num_intersect%2 == 1) {
          sdf(n, h, w, d) *= -1;
        }
      }
    }
  }

  /** \brief Sample points from the mesh
   * \param[in] mesh mesh to sample from
   * \param[in] n batch index in points
   * \param[in] points pre-initialized tensor holding points
   */
  void sample(int n, Eigen::Tensor<float, 3, Eigen::RowMajor>& points) {

    // The number of points to sample.
    const int N = points.dimension(1);

    // Stores the areas of faces.
    std::vector<float> areas(this->faces.size());
    float sum = 0;

    // Build a probability distribution over faces.
    for (int f = 0; f < this->faces.size(); f++) {
      Eigen::Vector3f a = this->vertices[this->faces[f][0]];
      Eigen::Vector3f b = this->vertices[this->faces[f][1]];
      Eigen::Vector3f c = this->vertices[this->faces[f][2]];

      // Angle between a->b and a->c.
      Eigen::Vector3f ab = b - a;
      Eigen::Vector3f ac = c - a;
      float cos_angle = ab.dot(ac)/(ab.norm()*ac.norm());
      float angle = std::acos(cos_angle);

      // Compute triangle area.
      float area = std::max(0., 0.5*ab.norm()*ac.norm()*std::sin(angle));
      //std::cout << area << " " << std::pow(area, 1./4.) << " " << angle << " " << ab.norm() << " " << ac.norm() << " " << std::sin(angle) << std::endl;

      // Accumulate.
      //area = std::sqrt(area);
      areas[f] = area;
      sum += area;
      //areas.push_back(1);
      //sum += 1;
    }

    //std::cout << sum << std::endl;
    assert(sum > 1e-8);

    for (int f = 0; f < this->faces.size(); f++) {
      //std::cout << areas[f] << " ";
      areas[f] /= sum;
      //std::cout << areas[f] << std::endl;
    }

    std::vector<float> cum_areas(areas.size());
    cum_areas[0] = areas[0];

    for (int f = 1; f < this->faces.size(); f++) {
      cum_areas[f] = areas[f] + cum_areas[f - 1];
    }

    for (int i = 0; i < N; i++) {
      float r = static_cast<float>(std::rand())/static_cast<float>(RAND_MAX);
      int face = 0;

      while (r > cum_areas[face + 1] && face < this->faces.size() - 1) {
        face++;
      }

      assert(face >= 0 && face < this->faces.size());
      //int face = std::rand()%this->faces.size();

      float r1 = 0;
      float r2 = 0;
      do {
        r1 = static_cast<float>(std::rand())/static_cast<float>(RAND_MAX);
        r2 = static_cast<float>(std::rand())/static_cast<float>(RAND_MAX);
      }
      while (r1 + r2 > 1.f);

      int s = std::rand()%3;
      //std::cout << face << " " << areas[face] << std::endl;

      Eigen::Vector3f a = this->vertices[this->faces[face](s)];
      Eigen::Vector3f b = this->vertices[this->faces[face]((s + 1)%3)];
      Eigen::Vector3f c = this->vertices[this->faces[face]((s + 2)%3)];

      Eigen::Vector3f ab = b - a;
      Eigen::Vector3f ac = c - a;

      Eigen::Vector3f point = a + r1*ab + r2*ac;
      points(n, i, 0) = point(0);
      points(n, i, 1) = point(1);
      points(n, i, 2) = point(2);
    }
  }

  /** \brief Write mesh to OFF file.
   * \param[in] filepath path to OFF file to write
   * \return success
   */
  bool to_off(const std::string filepath) {
    std::ofstream* out = new std::ofstream(filepath, std::ofstream::out);
    if (!static_cast<bool>(out)) {
      return false;
    }

    (*out) << "OFF" << std::endl;
    (*out) << this->vertices.size() << " " << this->faces.size() << " 0" << std::endl;

    for (unsigned int v = 0; v < this->vertices.size(); v++) {
      (*out) << this->vertices[v](0) << " " << this->vertices[v](1) << " " << this->vertices[v](2) << std::endl;
    }

    for (unsigned int f = 0; f < this->faces.size(); f++) {
      (*out) << "3 " << this->faces[f](0) << " " << this->faces[f](1) << " " << this->faces[f](2) << std::endl;
    }

    out->close();
    delete out;

    return true;
  }

private:

  /** \brief Vertices as (x,y,z)-vectors. */
  std::vector<Eigen::Vector3f> vertices;

  /** \brief Faces as list of vertex indices. */
  std::vector<Eigen::Vector3i> faces;
};

/** \brief Reading an off file and returning the vertices x, y, z coordinates and the
 * face indices.
 * \param[in] filepath path to the OFF file
 * \param[out] mesh read mesh with vertices and faces
 * \return success
 */
bool read_off(const std::string filepath, Mesh& mesh) {

  std::ifstream* file = new std::ifstream(filepath.c_str());
  std::string line;
  std::stringstream ss;
  int line_nb = 0;

  std::getline(*file, line);
  ++line_nb;

  if (line != "off" && line != "OFF") {
    std::cout << "[Error] Invalid header: \"" << line << "\", " << filepath << std::endl;
    return false;
  }

  size_t n_edges;
  std::getline(*file, line);
  ++line_nb;

  int n_vertices;
  int n_faces;
  ss << line;
  ss >> n_vertices;
  ss >> n_faces;
  ss >> n_edges;

  for (size_t v = 0; v < n_vertices; ++v) {
    std::getline(*file, line);
    ++line_nb;

    ss.clear();
    ss.str("");

    Eigen::Vector3f vertex;
    ss << line;
    ss >> vertex(0);
    ss >> vertex(1);
    ss >> vertex(2);

    mesh.add_vertex(vertex);
  }

  size_t n;
  for (size_t f = 0; f < n_faces; ++f) {
    std::getline(*file, line);
    ++line_nb;

    ss.clear();
    ss.str("");

    size_t n;
    ss << line;
    ss >> n;

    if(n != 3) {
      std::cout << "[Error] Not a triangle (" << n << " points) at " << (line_nb - 1) << std::endl;
      return false;
    }

    Eigen::Vector3i face;
    ss >> face(0);
    ss >> face(1);
    ss >> face(2);

    mesh.add_face(face);
  }

  if (n_vertices != mesh.num_vertices()) {
    std::cout << "[Error] Number of vertices in header differs from actual number of vertices." << std::endl;
    return false;
  }

  if (n_faces != mesh.num_faces()) {
    std::cout << "[Error] Number of faces in header differs from actual number of faces." << std::endl;
    return false;
  }

  file->close();
  delete file;

  return true;
}

/** \brief Write the given set of volumes to h5 file.
 * \param[in] filepath h5 file to write
 * \param[in] n number of volumes
 * \param[in] height height of volumes
 * \param[in] width width of volumes
 * \param[in] depth depth of volumes
 * \param[in] dense volume data
 */
bool write_hdf5(const std::string filepath, Eigen::Tensor<float, 4, Eigen::RowMajor>& dense) {

  try {

    /*
     * Turn off the auto-printing when failure occurs so that we can
     * handle the errors appropriately
     */
    H5::Exception::dontPrint();

    /*
     * Create a new file using H5F_ACC_TRUNC access,
     * default file creation properties, and default file
     * access properties.
     */
    H5::H5File file(filepath, H5F_ACC_TRUNC);

    /*
     * Define the size of the array and create the data space for fixed
     * size dataset.
     */
    hsize_t rank = 4;
    hsize_t dimsf[rank];
    dimsf[0] = dense.dimension(0);
    dimsf[1] = dense.dimension(1);
    dimsf[2] = dense.dimension(2);
    dimsf[3] = dense.dimension(3);
    H5::DataSpace dataspace(rank, dimsf);

    /*
     * Define datatype for the data in the file.
     * We will store little endian INT numbers.
     */
    H5::IntType datatype(H5::PredType::NATIVE_FLOAT);
    datatype.setOrder(H5T_ORDER_LE);

    /*
     * Create a new dataset within the file using defined dataspace and
     * datatype and default dataset creation properties.
     */
    H5::DataSet dataset = file.createDataSet("tensor", datatype, dataspace);

    /*
     * Write the data to the dataset using default memory space, file
     * space, and transfer properties.
     */
    float* data = static_cast<float*>(dense.data());
    dataset.write(data, H5::PredType::NATIVE_FLOAT);
  }  // end of try block

  // catch failure caused by the H5File operations
  catch(H5::FileIException error) {
    error.printError();
    return false;
  }

  // catch failure caused by the DataSet operations
  catch(H5::DataSetIException error) {
    error.printError();
    return false;
  }

  // catch failure caused by the DataSpace operations
  catch(H5::DataSpaceIException error) {
    error.printError();
    return false;
  }

  // catch failure caused by the DataSpace operations
  catch(H5::DataTypeIException error) {
    error.printError();
    return false;
  }

  return true;
}

/** \brief Write the given set of volumes to h5 file.
 * \param[in] filepath h5 file to write
 * \param[in] n number of volumes
 * \param[in] height height of volumes
 * \param[in] width width of volumes
 * \param[in] depth depth of volumes
 * \param[in] dense volume data
 */
bool write_hdf5(const std::string filepath, Eigen::Tensor<float, 3, Eigen::RowMajor>& dense) {

  try {

    /*
     * Turn off the auto-printing when failure occurs so that we can
     * handle the errors appropriately
     */
    H5::Exception::dontPrint();

    /*
     * Create a new file using H5F_ACC_TRUNC access,
     * default file creation properties, and default file
     * access properties.
     */
    H5::H5File file(filepath, H5F_ACC_TRUNC);

    /*
     * Define the size of the array and create the data space for fixed
     * size dataset.
     */
    hsize_t rank = 3;
    hsize_t dimsf[rank];
    dimsf[0] = dense.dimension(0);
    dimsf[1] = dense.dimension(1);
    dimsf[2] = dense.dimension(2);
    H5::DataSpace dataspace(rank, dimsf);

    /*
     * Define datatype for the data in the file.
     * We will store little endian INT numbers.
     */
    H5::IntType datatype(H5::PredType::NATIVE_FLOAT);
    datatype.setOrder(H5T_ORDER_LE);

    /*
     * Create a new dataset within the file using defined dataspace and
     * datatype and default dataset creation properties.
     */
    H5::DataSet dataset = file.createDataSet("tensor", datatype, dataspace);

    /*
     * Write the data to the dataset using default memory space, file
     * space, and transfer properties.
     */
    float* data = static_cast<float*>(dense.data());
    dataset.write(data, H5::PredType::NATIVE_FLOAT);
  }  // end of try block

  // catch failure caused by the H5File operations
  catch(H5::FileIException error) {
    error.printError();
    return false;
  }

  // catch failure caused by the DataSet operations
  catch(H5::DataSetIException error) {
    error.printError();
    return false;
  }

  // catch failure caused by the DataSpace operations
  catch(H5::DataSpaceIException error) {
    error.printError();
    return false;
  }

  // catch failure caused by the DataSpace operations
  catch(H5::DataTypeIException error) {
    error.printError();
    return false;
  }

  return true;
}

/** \brief Write the given set of volumes to h5 file.
 * \param[in] filepath h5 file to write
 * \param[in] n number of volumes
 * \param[in] height height of volumes
 * \param[in] width width of volumes
 * \param[in] depth depth of volumes
 * \param[in] dense volume data
 */
bool write_hdf5(const std::string filepath, Eigen::Tensor<int, 4, Eigen::RowMajor>& dense) {

  try {

    /*
     * Turn off the auto-printing when failure occurs so that we can
     * handle the errors appropriately
     */
    H5::Exception::dontPrint();

    /*
     * Create a new file using H5F_ACC_TRUNC access,
     * default file creation properties, and default file
     * access properties.
     */
    H5::H5File file(filepath, H5F_ACC_TRUNC);

    /*
     * Define the size of the array and create the data space for fixed
     * size dataset.
     */
    hsize_t rank = 4;
    hsize_t dimsf[rank];
    dimsf[0] = dense.dimension(0);
    dimsf[1] = dense.dimension(1);
    dimsf[2] = dense.dimension(2);
    dimsf[3] = dense.dimension(3);
    H5::DataSpace dataspace(rank, dimsf);

    /*
     * Define datatype for the data in the file.
     * We will store little endian INT numbers.
     */
    H5::IntType datatype(H5::PredType::NATIVE_INT);
    datatype.setOrder(H5T_ORDER_LE);

    /*
     * Create a new dataset within the file using defined dataspace and
     * datatype and default dataset creation properties.
     */
    H5::DataSet dataset = file.createDataSet("tensor", datatype, dataspace);

    /*
     * Write the data to the dataset using default memory space, file
     * space, and transfer properties.
     */
    int* data = static_cast<int*>(dense.data());
    dataset.write(data, H5::PredType::NATIVE_INT);
  }  // end of try block

  // catch failure caused by the H5File operations
  catch(H5::FileIException error) {
    error.printError();
    return false;
  }

  // catch failure caused by the DataSet operations
  catch(H5::DataSetIException error) {
    error.printError();
    return false;
  }

  // catch failure caused by the DataSpace operations
  catch(H5::DataSpaceIException error) {
    error.printError();
    return false;
  }

  // catch failure caused by the DataSpace operations
  catch(H5::DataTypeIException error) {
    error.printError();
    return false;
  }

  return true;
}

/** \brief Read all files in a directory matching the given extension.
 * \param[in] directory path to directory
 * \param[out] files read file paths
 * \param[in] extension extension to filter for
 */
void read_directory(const std::string directory, std::map<int, std::string>& files, const std::string extension = ".off") {

  boost::filesystem::path dir(directory);
  boost::filesystem::directory_iterator end;

  files.clear();
  for (boost::filesystem::directory_iterator it(dir); it != end; ++it) {
    if (it->path().extension().string() == extension) {
      int number = std::stoi(it->path().filename().string());
      files.insert(std::pair<int, std::string>(number, it->path().string()));
    }
  }
}

/** \brief Small helper for safely retrieving values from Json root element. */
class JSON {
public:

  /** \brief Constructor.
   * \param[in] root JSON root element
   */
  JSON(Json::Value root) {
    this->root = root;
  }

  /** \brief Get the value of the given key as int.
   * \param[in] key key to retrieve
   * \return value
   */
  int get_int(std::string key) {
    if (!this->root.isMember(key)) {
      std::cout << "[Error] key " << key << " not found" << std::endl;
      exit(1);
    }

    return this->root[key].asInt();
  }

  /** \brief Get the value of the given key as float.
   * \param[in] key key to retrieve
   * \return value
   */
  float get_float(std::string key) {
    if (!this->root.isMember(key)) {
      std::cout << "[Error] key " << key << " not found" << std::endl;
      exit(1);
    }

    return this->root[key].asFloat();
  }

  /** \brief Get the value of the given key as string.
   * \param[in] key key to retrieve
   * \return value
   */
  std::string get_string(std::string key) {
    if (!this->root.isMember(key)) {
      std::cout << "[Error] key " << key << " not found" << std::endl;
      exit(1);
    }

    return this->root[key].asString();
  }

private:

  /** \brief Root element. */
  Json::Value root;
};

int main(int argc, char** argv) {
  if (argc != 2) {
    std::cout << "[Error] Usage: voxelize_cuboids config.json" << std::endl;
    exit(1);
  }

  Json::Value root;
  Json::Reader reader;

  std::string config_file = argv[1];

  if (!boost::filesystem::is_regular_file(boost::filesystem::path(config_file))) {
    std::cout << "[Error] Config file not found: " << config_file << std::endl;
    exit(1);
  }

  std::ifstream json_file(config_file, std::ifstream::in | std::ifstream::binary);
  reader.parse(json_file, root, false);
  JSON json(root);

  // Unsafe!
  int height = json.get_int("height");
  int width = json.get_int("width");
  int depth = json.get_int("depth");

  int image_height = json.get_int("image_height");
  int image_width = json.get_int("image_width");

  std::string suffix = json.get_string("suffix");
  int multiplier = json.get_int("multiplier");

  std::string sdf_file = json.get_string("sdf_file");
  if (sdf_file.empty()) {
    std::cout << "[Error] Read invalid output file" << std::endl;
    exit(1);
  }

  sdf_file = sdf_file + "_" + std::to_string(multiplier) + "_"
      + std::to_string(image_height) + "x" + std::to_string(image_width) + "_"
      + std::to_string(height) + "x" + std::to_string(width) + "x" + std::to_string(depth)
      + suffix + ".h5";

  std::string output_file = json.get_string("output_file");
  if (output_file.empty()) {
    std::cout << "[Error] Read invalid output file" << std::endl;
    exit(1);
  }

  output_file = output_file + "_" + std::to_string(multiplier) + "_"
      + std::to_string(image_height) + "x" + std::to_string(image_width) + "_"
      + std::to_string(height) + "x" + std::to_string(width) + "x" + std::to_string(depth)
      + suffix + ".h5";

  std::string off_directory = json.get_string("off_dir");
  if (off_directory.empty()) {
    std::cout << "[Error] Read invalid off directory" << std::endl;
    exit(1);
  }

  off_directory = off_directory + "_" + std::to_string(multiplier) + "_"
      + std::to_string(image_height) + "x" + std::to_string(image_width) + "_"
      + std::to_string(height) + "x" + std::to_string(width) + "x" + std::to_string(depth) + suffix;

  std::string off_gt_directory = json.get_string("off_gt_dir");
  if (off_gt_directory.empty()) {
    std::cout << "[Error] Read invalid off gt directory" << std::endl;
    exit(1);
  }

  off_gt_directory = off_gt_directory + "_" + std::to_string(multiplier) + "_"
      + std::to_string(image_height) + "x" + std::to_string(image_width) + "_"
      + std::to_string(height) + "x" + std::to_string(width) + "x" + std::to_string(depth) + suffix;

  if (!boost::filesystem::is_directory(boost::filesystem::path(off_gt_directory))) {
    boost::filesystem::create_directories(boost::filesystem::path(off_gt_directory));
  }

  std::string point_file = json.get_string("point_file");
  if (point_file.empty()) {
    std::cout << "[Error] Read invalid point file" << std::endl;
    exit(1);
  }

  point_file = point_file + "_" + std::to_string(multiplier) + "_"
      + std::to_string(image_height) + "x" + std::to_string(image_width) + "_"
      + std::to_string(height) + "x" + std::to_string(width) + "x" + std::to_string(depth)
      + suffix + ".h5";

  if (!boost::filesystem::is_directory(boost::filesystem::path(off_directory))) {
    std::cout << "[Error] OFF directory " << off_directory << " not found" << std::endl;
    exit(1);
  }

  std::map<int, std::string> files;
  read_directory(off_directory, files);

  int N = files.size();
  Eigen::Tensor<float, 4, Eigen::RowMajor> sdf(N, height, width, depth);
  sdf.setZero();

  int N_points = json.get_int("n_points");
  Eigen::Tensor<float, 3, Eigen::RowMajor> points(N, N_points, 3);
  points.setZero();

  std::vector<int> indices;
  for (std::map<int, std::string>::iterator it = files.begin(); it != files.end(); it++) {
    indices.push_back(it->first);
  }

  int n = 0;
  #pragma omp parallel
  {
    #pragma omp for
    for (unsigned int i = 0; i< indices.size(); i++) {
      int n = indices[i];

      Mesh mesh;
      bool success = read_off(files[n], mesh);

      if (!success) {
        std::cout << "[Error] could not read " << files[n] << std::endl;
        exit(1);
      }

      float padding_factor = 1 + json.get_float("padding");
      float scale_factor = static_cast<float>(width)/padding_factor;
      mesh.scale(Eigen::Vector3f(scale_factor, scale_factor, scale_factor));
      mesh.translate(Eigen::Vector3f(static_cast<float>(width)/2, static_cast<float>(height)/2, static_cast<float>(depth)/2));

      std::cout << "[Data] sampling mesh " << n << "/" << files.size() << " " << files[n] << std::endl;
      mesh.sample(i, points);

      std::string off_file = off_gt_directory + "/" + std::to_string(i) + ".off";
      std::cout << "[Data] writing " << off_file << std::endl;
      mesh.to_off(off_file);

      if (!success) {
        std::cout << "[Error] error reading " << files[n] << std::endl;
        exit(1);
      }

      std::cout << "[Data] voxelizing mesh " << n << "/" << files.size() << " " << files[n] << std::endl;
      mesh.voxelize_SCALED(i, sdf);

      n++;
    }
  }

  bool success = write_hdf5(sdf_file, sdf);

  if (success) {
    std::cout <<"[Data] wrote " << sdf_file << std::endl;
  }
  else {
    std::cout << "[Error] error writing " << sdf_file << std::endl;
  }

  success = write_hdf5(point_file, points);

  if (success) {
    std::cout <<"[Data] wrote " << point_file << std::endl;
  }
  else {
    std::cout << "[Error] error writing " << point_file << std::endl;
  }

  Eigen::Tensor<int, 4, Eigen::RowMajor> occ(N, height, width, depth);
  occ.setZero();

  for (int n = 0; n < N; n++) {
    for (int h = 0; h < height; h++) {
      for (int w = 0; w < width; w++) {
        for (int d = 0; d < depth; d++) {
          if (sdf(n, h, w, d) <= 0) {
            occ(n, h, w, d) = 1;
          }
        }
      }
    }
  }

  success = write_hdf5(output_file, occ);

  if (success) {
    std::cout <<"[Data] wrote " << output_file << std::endl;
  }
  else {
    std::cout << "[Error] error writing " << output_file << std::endl;
  }

  exit(0);
}


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Thu 10 May 2018 15:01:38

#ifndef NUHMSSM_susy_parameters_H
#define NUHMSSM_susy_parameters_H

#include "betafunction.hpp"
#include "NUHMSSM_input_parameters.hpp"

#include <iosfwd>
#include <string>
#include <vector>
#include <Eigen/Core>

namespace flexiblesusy {

#ifdef TRACE_STRUCT_TYPE
   #undef TRACE_STRUCT_TYPE
#endif
#define TRACE_STRUCT_TYPE Susy_traces

class NUHMSSM_susy_parameters : public Beta_function {
public:
   explicit NUHMSSM_susy_parameters(const NUHMSSM_input_parameters& input_ = NUHMSSM_input_parameters());
   NUHMSSM_susy_parameters(double scale_, int loops_, int thresholds_, const NUHMSSM_input_parameters& input_, const Eigen::Matrix<double,3,3>& Yd_, const Eigen::Matrix<double,3,3>& Ye_
   , const Eigen::Matrix<double,3,3>& Yu_, double Mu_, double g1_, double g2_,
   double g3_, double vd_, double vu_
);
   NUHMSSM_susy_parameters(const NUHMSSM_susy_parameters&) = default;
   NUHMSSM_susy_parameters(NUHMSSM_susy_parameters&&) = default;
   virtual ~NUHMSSM_susy_parameters() = default;
   NUHMSSM_susy_parameters& operator=(const NUHMSSM_susy_parameters&) = default;
   NUHMSSM_susy_parameters& operator=(NUHMSSM_susy_parameters&&) = default;

   virtual Eigen::ArrayXd beta() const override;
   virtual Eigen::ArrayXd get() const override;
   virtual void print(std::ostream&) const;
   virtual void set(const Eigen::ArrayXd&) override;
   const NUHMSSM_input_parameters& get_input() const;
   NUHMSSM_input_parameters& get_input();
   void set_input_parameters(const NUHMSSM_input_parameters&);

   NUHMSSM_susy_parameters calc_beta() const;
   NUHMSSM_susy_parameters calc_beta(int) const;
   virtual void clear();

   void set_Yd(const Eigen::Matrix<double,3,3>& Yd_) { Yd = Yd_; }
   void set_Yd(int i, int k, const double& value) { Yd(i,k) = value; }
   void set_Ye(const Eigen::Matrix<double,3,3>& Ye_) { Ye = Ye_; }
   void set_Ye(int i, int k, const double& value) { Ye(i,k) = value; }
   void set_Yu(const Eigen::Matrix<double,3,3>& Yu_) { Yu = Yu_; }
   void set_Yu(int i, int k, const double& value) { Yu(i,k) = value; }
   void set_Mu(double Mu_) { Mu = Mu_; }
   void set_g1(double g1_) { g1 = g1_; }
   void set_g2(double g2_) { g2 = g2_; }
   void set_g3(double g3_) { g3 = g3_; }
   void set_vd(double vd_) { vd = vd_; }
   void set_vu(double vu_) { vu = vu_; }

   const Eigen::Matrix<double,3,3>& get_Yd() const { return Yd; }
   double get_Yd(int i, int k) const { return Yd(i,k); }
   const Eigen::Matrix<double,3,3>& get_Ye() const { return Ye; }
   double get_Ye(int i, int k) const { return Ye(i,k); }
   const Eigen::Matrix<double,3,3>& get_Yu() const { return Yu; }
   double get_Yu(int i, int k) const { return Yu(i,k); }
   double get_Mu() const { return Mu; }
   double get_g1() const { return g1; }
   double get_g2() const { return g2; }
   double get_g3() const { return g3; }
   double get_vd() const { return vd; }
   double get_vu() const { return vu; }

   Eigen::Matrix<double,3,3> get_SqSq() const;
   Eigen::Matrix<double,3,3> get_SlSl() const;
   double get_SHdSHd() const;
   double get_SHuSHu() const;
   Eigen::Matrix<double,3,3> get_SdRSdR() const;
   Eigen::Matrix<double,3,3> get_SuRSuR() const;
   Eigen::Matrix<double,3,3> get_SeRSeR() const;

protected:
   Eigen::Matrix<double,3,3> Yd{Eigen::Matrix<double,3,3>::Zero()};
   Eigen::Matrix<double,3,3> Ye{Eigen::Matrix<double,3,3>::Zero()};
   Eigen::Matrix<double,3,3> Yu{Eigen::Matrix<double,3,3>::Zero()};
   double Mu{};
   double g1{};
   double g2{};
   double g3{};
   double vd{};
   double vu{};

   NUHMSSM_input_parameters input{};

private:
   static const int numberOfParameters = 33;

   struct Susy_traces {
      double traceYdAdjYd{};
      double traceYeAdjYe{};
      double traceYdAdjYdYdAdjYd{};
      double traceYdAdjYuYuAdjYd{};
      double traceYeAdjYeYeAdjYe{};
      double traceYuAdjYu{};
      double traceAdjYdYd{};
      double traceAdjYeYe{};
      double traceAdjYuYu{};
      double traceAdjYdYdAdjYdYd{};
      double traceAdjYeYeAdjYeYe{};
      double traceAdjYuYuAdjYdYd{};
      double traceAdjYuYuAdjYuYu{};
      double traceAdjYdYdAdjYdYdAdjYdYd{};
      double traceAdjYeYeAdjYeYeAdjYeYe{};
      double traceAdjYuYuAdjYuYuAdjYdYd{};
      double traceYuAdjYuYuAdjYu{};
      double traceAdjYdYdAdjYuYuAdjYdYd{};
      double traceAdjYuYuAdjYuYuAdjYuYu{};

   };
   Susy_traces calc_susy_traces(int) const;

   Eigen::Matrix<double,3,3> calc_beta_Yd_1_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Yd_2_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Yd_3_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Ye_1_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Ye_2_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Ye_3_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Yu_1_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Yu_2_loop(const TRACE_STRUCT_TYPE&) const;
   Eigen::Matrix<double,3,3> calc_beta_Yu_3_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_Mu_1_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_Mu_2_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_Mu_3_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g1_1_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g1_2_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g1_3_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g2_1_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g2_2_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g2_3_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g3_1_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g3_2_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_g3_3_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_vd_1_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_vd_2_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_vd_3_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_vu_1_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_vu_2_loop(const TRACE_STRUCT_TYPE&) const;
   double calc_beta_vu_3_loop(const TRACE_STRUCT_TYPE&) const;

};

std::ostream& operator<<(std::ostream&, const NUHMSSM_susy_parameters&);

#undef TRACE_STRUCT_TYPE

} // namespace flexiblesusy

#endif


/**
 * @file ffn_impl.hpp
 * @author Marcus Edel
 *
 * Definition of the FFN class, which implements feed forward neural networks.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_ANN_FFN_IMPL_HPP
#define MLPACK_METHODS_ANN_FFN_IMPL_HPP

// In case it hasn't been included yet.
#include "ffn.hpp"

#include "visitor/forward_visitor.hpp"
#include "visitor/backward_visitor.hpp"
#include "visitor/deterministic_set_visitor.hpp"
#include "visitor/gradient_set_visitor.hpp"
#include "visitor/gradient_visitor.hpp"
#include "visitor/set_input_height_visitor.hpp"
#include "visitor/set_input_width_visitor.hpp"

#include <boost/serialization/variant.hpp>

namespace mlpack {
namespace ann /** Artificial Neural Network. */ {

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::FFN(
    OutputLayerType outputLayer, InitializationRuleType initializeRule) :
    outputLayer(std::move(outputLayer)),
    initializeRule(std::move(initializeRule)),
    width(0),
    height(0),
    reset(false),
    numFunctions(0),
    deterministic(true)
{
  /* Nothing to do here */
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::~FFN()
{
  std::for_each(network.begin(), network.end(),
      boost::apply_visitor(deleteVisitor));
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::ResetData(
    arma::mat predictors, arma::mat responses)
{
  numFunctions = responses.n_cols;
  this->predictors = std::move(predictors);
  this->responses = std::move(responses);
  this->deterministic = true;
  ResetDeterministic();

  if (!reset)
    ResetParameters();
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename OptimizerType, typename... CallbackTypes>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Train(
      arma::mat predictors,
      arma::mat responses,
      OptimizerType& optimizer,
      CallbackTypes&&... callbacks)
{
  ResetData(std::move(predictors), std::move(responses));

  // Train the model.
  Timer::Start("ffn_optimization");
  const double out = optimizer.Optimize(*this, parameter, callbacks...);
  Timer::Stop("ffn_optimization");

  Log::Info << "FFN::FFN(): final objective of trained model is " << out
      << "." << std::endl;
  return out;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename OptimizerType, typename... CallbackTypes>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Train(
    arma::mat predictors,
    arma::mat responses,
    CallbackTypes&&... callbacks)
{
  ResetData(std::move(predictors), std::move(responses));

  OptimizerType optimizer;

  // Train the model.
  Timer::Start("ffn_optimization");
  const double out = optimizer.Optimize(*this, parameter, callbacks...);
  Timer::Stop("ffn_optimization");

  Log::Info << "FFN::FFN(): final objective of trained model is " << out
      << "." << std::endl;
  return out;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Forward(
    arma::mat inputs, arma::mat& results)
{
  if (parameter.is_empty())
    ResetParameters();

  if (!deterministic)
  {
    deterministic = true;
    ResetDeterministic();
  }

  currentInput = std::move(inputs);
  Forward(std::move(currentInput));
  results = boost::apply_visitor(outputParameterVisitor, network.back());
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Forward(
    arma::mat inputs, arma::mat& results, const size_t begin, const size_t end)
{
  boost::apply_visitor(ForwardVisitor(std::move(inputs), std::move(
      boost::apply_visitor(outputParameterVisitor, network[begin]))),
      network[begin]);

  for (size_t i = 1; i < end - begin + 1; ++i)
  {
    boost::apply_visitor(ForwardVisitor(std::move(boost::apply_visitor(
        outputParameterVisitor, network[begin + i - 1])), std::move(
        boost::apply_visitor(outputParameterVisitor, network[begin + i]))),
        network[begin + i]);
  }

  results = boost::apply_visitor(outputParameterVisitor, network[end]);
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Backward(
    arma::mat targets, arma::mat& gradients)
{
  double res = outputLayer.Forward(std::move(boost::apply_visitor(
      outputParameterVisitor, network.back())), std::move(targets));

  for (size_t i = 0; i < network.size(); ++i)
  {
    res += boost::apply_visitor(lossVisitor, network[i]);
  }

  outputLayer.Backward(std::move(boost::apply_visitor(outputParameterVisitor,
      network.back())), std::move(targets), std::move(error));

  gradients = arma::zeros<arma::mat>(parameter.n_rows, parameter.n_cols);

  Backward();
  ResetGradients(gradients);
  Gradient(std::move(currentInput));

  return res;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Predict(
    arma::mat predictors, arma::mat& results)
{
  if (parameter.is_empty())
    ResetParameters();

  if (!deterministic)
  {
    deterministic = true;
    ResetDeterministic();
  }

  arma::mat resultsTemp;
  Forward(std::move(arma::mat(predictors.colptr(0),
      predictors.n_rows, 1, false, true)));
  resultsTemp = boost::apply_visitor(outputParameterVisitor,
      network.back()).col(0);

  results = arma::mat(resultsTemp.n_elem, predictors.n_cols);
  results.col(0) = resultsTemp.col(0);

  for (size_t i = 1; i < predictors.n_cols; i++)
  {
    Forward(std::move(arma::mat(predictors.colptr(i),
        predictors.n_rows, 1, false, true)));

    resultsTemp = boost::apply_visitor(outputParameterVisitor,
        network.back());
    results.col(i) = resultsTemp.col(0);
  }
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Evaluate(
    arma::mat predictors, arma::mat responses)
{
  if (parameter.is_empty())
    ResetParameters();

  if (!deterministic)
  {
    deterministic = true;
    ResetDeterministic();
  }

  Forward(std::move(predictors));

  double res = outputLayer.Forward(std::move(boost::apply_visitor(
      outputParameterVisitor, network.back())), std::move(responses));

  for (size_t i = 0; i < network.size(); ++i)
  {
    res += boost::apply_visitor(lossVisitor, network[i]);
  }

  return res;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Evaluate(
    const arma::mat& parameters)
{
  double res = 0;
  for (size_t i = 0; i < predictors.n_cols; ++i)
    res += Evaluate(parameters, i, 1, true);

  return res;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Evaluate(
    const arma::mat& /* parameters */,
    const size_t begin,
    const size_t batchSize,
    const bool deterministic)
{
  if (parameter.is_empty())
    ResetParameters();

  if (deterministic != this->deterministic)
  {
    this->deterministic = deterministic;
    ResetDeterministic();
  }

  Forward(std::move(predictors.cols(begin, begin + batchSize - 1)));
  double res = outputLayer.Forward(
      std::move(boost::apply_visitor(outputParameterVisitor, network.back())),
      std::move(responses.cols(begin, begin + batchSize - 1)));

  for (size_t i = 0; i < network.size(); ++i)
  {
    res += boost::apply_visitor(lossVisitor, network[i]);
  }

  return res;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Evaluate(
    const arma::mat& parameters, const size_t begin, const size_t batchSize)
{
  return Evaluate(parameters, begin, batchSize, true);
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename GradType>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::
EvaluateWithGradient(const arma::mat& parameters, GradType& gradient)
{
  double res = 0;
  for (size_t i = 0; i < predictors.n_cols; ++i)
    res += EvaluateWithGradient(parameters, i, gradient, 1);

  return res;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename GradType>
double FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::
EvaluateWithGradient(const arma::mat& /* parameters */,
                     const size_t begin,
                     GradType& gradient,
                     const size_t batchSize)
{
  if (gradient.is_empty())
  {
    if (parameter.is_empty())
      ResetParameters();

    gradient = arma::zeros<arma::mat>(parameter.n_rows, parameter.n_cols);
  }
  else
  {
    gradient.zeros();
  }

  if (this->deterministic)
  {
    this->deterministic = false;
    ResetDeterministic();
  }

  Forward(std::move(predictors.cols(begin, begin + batchSize - 1)));
  double res = outputLayer.Forward(
      std::move(boost::apply_visitor(outputParameterVisitor, network.back())),
      std::move(responses.cols(begin, begin + batchSize - 1)));

  for (size_t i = 0; i < network.size(); ++i)
  {
    res += boost::apply_visitor(lossVisitor, network[i]);
  }

  outputLayer.Backward(
      std::move(boost::apply_visitor(outputParameterVisitor, network.back())),
      std::move(responses.cols(begin, begin + batchSize - 1)),
      std::move(error));

  Backward();
  ResetGradients(gradient);
  Gradient(std::move(predictors.cols(begin, begin + batchSize - 1)));

  return res;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Gradient(
    const arma::mat& parameters,
    const size_t begin,
    arma::mat& gradient,
    const size_t batchSize)
{
  this->EvaluateWithGradient(parameters, begin, gradient, batchSize);
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Shuffle()
{
  math::ShuffleData(predictors, responses, predictors, responses);
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::ResetParameters()
{
  ResetDeterministic();

  // Reset the network parameter with the given initialization rule.
  NetworkInitialization<InitializationRuleType,
                        CustomLayers...> networkInit(initializeRule);
  networkInit.Initialize(network, parameter);
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::ResetDeterministic()
{
  DeterministicSetVisitor deterministicSetVisitor(deterministic);
  std::for_each(network.begin(), network.end(),
      boost::apply_visitor(deterministicSetVisitor));
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::ResetGradients(arma::mat& gradient)
{
  size_t offset = 0;
  for (size_t i = 0; i < network.size(); ++i)
  {
    offset += boost::apply_visitor(GradientSetVisitor(std::move(gradient),
        offset), network[i]);
  }
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::Forward(arma::mat&& input)
{
  boost::apply_visitor(ForwardVisitor(std::move(input), std::move(
      boost::apply_visitor(outputParameterVisitor, network.front()))),
      network.front());

  if (!reset)
  {
    if (boost::apply_visitor(outputWidthVisitor, network.front()) != 0)
    {
      width = boost::apply_visitor(outputWidthVisitor, network.front());
    }

    if (boost::apply_visitor(outputHeightVisitor, network.front()) != 0)
    {
      height = boost::apply_visitor(outputHeightVisitor, network.front());
    }
  }

  for (size_t i = 1; i < network.size(); ++i)
  {
    if (!reset)
    {
      // Set the input width.
      boost::apply_visitor(SetInputWidthVisitor(width), network[i]);

      // Set the input height.
      boost::apply_visitor(SetInputHeightVisitor(height), network[i]);
    }

    boost::apply_visitor(ForwardVisitor(std::move(boost::apply_visitor(
        outputParameterVisitor, network[i - 1])), std::move(
        boost::apply_visitor(outputParameterVisitor, network[i]))), network[i]);

    if (!reset)
    {
      // Get the output width.
      if (boost::apply_visitor(outputWidthVisitor, network[i]) != 0)
      {
        width = boost::apply_visitor(outputWidthVisitor, network[i]);
      }

      // Get the output height.
      if (boost::apply_visitor(outputHeightVisitor, network[i]) != 0)
      {
        height = boost::apply_visitor(outputHeightVisitor, network[i]);
      }
    }
  }

  if (!reset)
    reset = true;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::Backward()
{
  boost::apply_visitor(BackwardVisitor(std::move(boost::apply_visitor(
      outputParameterVisitor, network.back())), std::move(error), std::move(
      boost::apply_visitor(deltaVisitor, network.back()))), network.back());

  for (size_t i = 2; i < network.size(); ++i)
  {
    boost::apply_visitor(BackwardVisitor(std::move(boost::apply_visitor(
        outputParameterVisitor, network[network.size() - i])), std::move(
        boost::apply_visitor(deltaVisitor, network[network.size() - i + 1])),
        std::move(boost::apply_visitor(deltaVisitor,
        network[network.size() - i]))), network[network.size() - i]);
  }
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::Gradient(arma::mat&& input)
{
  boost::apply_visitor(GradientVisitor(std::move(input), std::move(
      boost::apply_visitor(deltaVisitor, network[1]))), network.front());

  for (size_t i = 1; i < network.size() - 1; ++i)
  {
    boost::apply_visitor(GradientVisitor(std::move(boost::apply_visitor(
        outputParameterVisitor, network[i - 1])), std::move(
        boost::apply_visitor(deltaVisitor, network[i + 1]))), network[i]);
  }

  boost::apply_visitor(GradientVisitor(std::move(boost::apply_visitor(
      outputParameterVisitor, network[network.size() - 2])), std::move(error)),
      network[network.size() - 1]);
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename Archive>
void FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::serialize(
    Archive& ar, const unsigned int version)
{
  ar & BOOST_SERIALIZATION_NVP(parameter);
  ar & BOOST_SERIALIZATION_NVP(width);
  ar & BOOST_SERIALIZATION_NVP(height);
  ar & BOOST_SERIALIZATION_NVP(currentInput);

  // Earlier versions of the FFN code did not serialize whether or not the model
  // was reset.
  if (version > 0)
  {
    ar & BOOST_SERIALIZATION_NVP(reset);
  }

  // Be sure to clear other layers before loading.
  if (Archive::is_loading::value)
  {
    std::for_each(network.begin(), network.end(),
        boost::apply_visitor(deleteVisitor));
    network.clear();
  }

  ar & BOOST_SERIALIZATION_NVP(network);

  // If we are loading, we need to initialize the weights.
  if (Archive::is_loading::value)
  {
    // The behavior in earlier versions was to always assume the weights needed
    // to be reset.
    if (version == 0)
      reset = false;

    size_t offset = 0;
    for (size_t i = 0; i < network.size(); ++i)
    {
      offset += boost::apply_visitor(WeightSetVisitor(std::move(parameter),
          offset), network[i]);

      boost::apply_visitor(resetVisitor, network[i]);
    }

    deterministic = true;
    ResetDeterministic();
  }
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void FFN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::Swap(FFN& network)
{
  std::swap(outputLayer, network.outputLayer);
  std::swap(initializeRule, network.initializeRule);
  std::swap(width, network.width);
  std::swap(height, network.height);
  std::swap(reset, network.reset);
  std::swap(this->network, network.network);
  std::swap(predictors, network.predictors);
  std::swap(responses, network.responses);
  std::swap(parameter, network.parameter);
  std::swap(numFunctions, network.numFunctions);
  std::swap(error, network.error);
  std::swap(currentInput, network.currentInput);
  std::swap(deterministic, network.deterministic);
  std::swap(delta, network.delta);
  std::swap(inputParameter, network.inputParameter);
  std::swap(outputParameter, network.outputParameter);
  std::swap(gradient, network.gradient);
};

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::FFN(
    const FFN& network):
    outputLayer(network.outputLayer),
    initializeRule(network.initializeRule),
    width(network.width),
    height(network.height),
    reset(network.reset),
    predictors(network.predictors),
    responses(network.responses),
    parameter(network.parameter),
    numFunctions(network.numFunctions),
    error(network.error),
    currentInput(network.currentInput),
    deterministic(network.deterministic),
    delta(network.delta),
    inputParameter(network.inputParameter),
    outputParameter(network.outputParameter),
    gradient(network.gradient)
{
  // Build new layers according to source network
  for (size_t i = 0; i < network.network.size(); ++i)
  {
    this->network.push_back(boost::apply_visitor(copyVisitor,
        network.network[i]));
  }
};

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
FFN<OutputLayerType, InitializationRuleType, CustomLayers...>::FFN(
    FFN&& network):
    outputLayer(std::move(network.outputLayer)),
    initializeRule(std::move(network.initializeRule)),
    width(network.width),
    height(network.height),
    reset(network.reset),
    predictors(std::move(network.predictors)),
    responses(std::move(network.responses)),
    parameter(std::move(network.parameter)),
    numFunctions(network.numFunctions),
    error(std::move(network.error)),
    currentInput(std::move(network.currentInput)),
    deterministic(network.deterministic),
    delta(std::move(network.delta)),
    inputParameter(std::move(network.inputParameter)),
    outputParameter(std::move(network.outputParameter)),
    gradient(std::move(network.gradient))
{
  this->network = std::move(network.network);
};

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
FFN<OutputLayerType, InitializationRuleType, CustomLayers...>&
FFN<OutputLayerType, InitializationRuleType,
    CustomLayers...>::operator = (FFN network)
{
  Swap(network);
  return *this;
};

} // namespace ann
} // namespace mlpack

#endif