#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <sophus/se3.hpp>

#include <pangolin/pangolin.h>
#include <pangolin/compat/function.h>
#include <pangolin/glcuda.h>
#include <pangolin/glsl.h>
#include <npp.h>

#include <SceneGraph/SceneGraph.h>
#include <SceneGraph/GLVbo.h>

#include <kangaroo/kangaroo.h>
#include <kangaroo/extra/GLCameraHistory.h>
#include <kangaroo/extra/RpgCameraOpen.h>
#include <kangaroo/extra/DisplayUtils.h>
#include <kangaroo/extra/ScanlineRectify.h>
#include <kangaroo/extra/ImageSelect.h>
#include <kangaroo/extra/BaseDisplayCuda.h>
#include <kangaroo/extra/HeightmapFusion.h>
#include <kangaroo/extra/CameraModelPyramid.h>
#include <kangaroo/extra/LoadPosesFromFile.h>
#include <kangaroo/extra/SavePPM.h>

//#define HM_FUSION
//#define PLANE_FIT
//#define COSTVOL_TIME

const int MAXD = 128;

using namespace std;
using namespace pangolin;
using namespace roo;

int main( int argc, char* argv[] )
{
    // Initialise window
    View& container = SetupPangoGLWithCuda(1024, 768);
    size_t cu_mem_start, cu_mem_end, cu_mem_total;
    cudaMemGetInfo( &cu_mem_start, &cu_mem_total );
    glClearColor(1,1,1,0);

    // Open video device
    CameraDevice video = OpenRpgCamera(argc,argv,2,true);

    // Capture first Image
    std::vector<rpg::ImageWrapper> Images;
    video.Capture(Images);

    // native width and height (from camera)
    const unsigned int nw = Images[0].width();
    const unsigned int nh = Images[0].height();

    // Downsample this Image to process less pixels
    const int max_levels = 6;
    const int level = GetLevelFromMaxPixels( nw, nh, 2*640*480 );
//    const int level = 4;
    assert(level <= max_levels);

    // Find centered Image crop which aligns to 16 pixels at given level
    const NppiRect roi = GetCenteredAlignedRegion(nw,nh,16 << level,16 << level);

    // Load Camera intrinsics from file
    CameraModelPyramid cam[] = {
        video.GetProperty("DataSourceDir") + "/lcmod.xml",
        video.GetProperty("DataSourceDir") + "/rcmod.xml"
    };

    for(int i=0; i<2; ++i ) {
        // Adjust to match camera Image dimensions
        CamModelScaleToDimensions(cam[i], nw, nh );

        // Adjust to match cropped aligned Image
        CamModelCropToRegionOfInterest(cam[i], roi);

        cam[i].PopulatePyramid(max_levels);
    }

    const unsigned int w = roi.width;
    const unsigned int h = roi.height;
    const unsigned int lw = w >> level;
    const unsigned int lh = h >> level;

    const Eigen::Matrix3d& K0 = cam[0].K();
    const Eigen::Matrix3d& Kl = cam[0].K(level);

    cout << "Video stream dimensions: " << nw << "x" << nh << endl;
    cout << "Chosen Level: " << level << endl;
    cout << "Processing dimensions: " << lw << "x" << lh << endl;
    cout << "Offset: " << roi.x << "x" << roi.y << endl;

    Eigen::Matrix3d RDFvision;RDFvision<< 1,0,0,  0,1,0,  0,0,1;
    Eigen::Matrix3d RDFrobot; RDFrobot << 0,1,0,  0,0, 1,  1,0,0;
    Eigen::Matrix4d T_vis_ro = Eigen::Matrix4d::Identity();
    T_vis_ro.block<3,3>(0,0) = RDFvision.transpose() * RDFrobot;
    Eigen::Matrix4d T_ro_vis = Eigen::Matrix4d::Identity();
    T_ro_vis.block<3,3>(0,0) = RDFrobot.transpose() * RDFvision;

    const Sophus::SE3d T_rl_orig = T_rlFromCamModelRDF(cam[0], cam[1], RDFvision);
    double k1 = 0;
    double k2 = 0;

    if(cam[0].Type() == MVL_CAMERA_WARPED)
    {
        k1 = cam[0].GetModel()->warped.kappa1;
        k2 = cam[0].GetModel()->warped.kappa2;
    }

    const bool rectify = (k1!=0 || k2!=0); // || camModel[0].GetPose().block<3,3>(0,0)
    if(!rectify) {
        cout << "Using pre-rectified Images" << endl;
    }

    // Check we received at least two Images
    if(Images.size() < 2) {
        std::cerr << "Failed to capture first stereo pair from camera" << std::endl;
        return -1;
    }

    // Load history
    Sophus::SE3d T_wc;
    vector<Sophus::SE3d> gtPoseT_wh;
    LoadPosesFromFile(gtPoseT_wh, video.GetProperty("DataSourceDir") + "/pose.txt", video.GetProperty("StartFrame",0), T_vis_ro, T_ro_vis);

#ifdef PLANE_FIT
    // Plane Parameters
    // These coordinates need to be below the horizon. This could cause trouble!
    Eigen::Matrix3d U; U << w, 0, w,  h/2, h, h,  1, 1, 1;
    Eigen::Matrix3d Q = -(cam[0].Kinv() * U).transpose();
    Eigen::Matrix3d Qinv = Q.inverse();
    Eigen::Vector3d z; z << 1/5.0, 1/5.0, 1/5.0;
    Eigen::Vector3d n_c = Qinv*z;
    Eigen::Vector3d n_w = project((Eigen::Vector4d)(T_wc.inverse().matrix().transpose() * unproject(n_c)));
#endif // PLANE_FIT

    // Define Camera Render Object (for view / scene browsing)
    pangolin::OpenGlRenderState s_cam(
        ProjectionMatrixRDF_TopLeft(w,h,K0(0,0),K0(1,1),K0(0,2),K0(1,2),0.1,10000),
        IdentityMatrix(GlModelViewStack)
    );
    if(!gtPoseT_wh.empty()) {
        s_cam.SetModelViewMatrix(gtPoseT_wh[0].inverse().matrix());
    }

    GlBufferCudaPtr vbo(GlArrayBuffer, lw*lh,GL_FLOAT, 4, cudaGraphicsMapFlagsWriteDiscard, GL_STREAM_DRAW );
    GlBufferCudaPtr cbo(GlArrayBuffer, lw*lh,GL_UNSIGNED_BYTE, 4, cudaGraphicsMapFlagsWriteDiscard, GL_STREAM_DRAW );
    GlBuffer ibo = pangolin::MakeTriangleStripIboForVbo(lw,lh);

    // Allocate Camera Images on device for processing
    roo::Image<unsigned char, TargetHost, DontManage> hCamImg[] = {{0,nw,nh},{0,nw,nh}};
    roo::Image<float2, TargetDevice, Manage> dLookup[] = {{w,h},{w,h}};

    roo::Image<unsigned char, TargetDevice, Manage> upload(w,h);
    Pyramid<unsigned char, max_levels, TargetDevice, Manage> img_pyr[] = {{w,h},{w,h}};

    roo::Image<float, TargetDevice, Manage> img[] = {{lw,lh},{lw,lh}};
    Volume<float, TargetDevice, Manage> vol[] = {{lw,lh,MAXD},{lw,lh,MAXD},{lw,lh,MAXD}};
    roo::Image<float, TargetDevice, Manage>  disp[] = {{lw,lh},{lw,lh}};
    roo::Image<float, TargetDevice, Manage> meanI(lw,lh);
    roo::Image<float, TargetDevice, Manage> varI(lw,lh);
    roo::Image<float, TargetDevice, Manage> temp[] = {{lw,lh},{lw,lh},{lw,lh},{lw,lh},{lw,lh}};

    roo::Image<float4, TargetDevice, Manage>  d3d(lw,lh);
    roo::Image<float, TargetDevice, Manage>  dCrossSection(lw,MAXD);
    roo::Image<unsigned char, TargetDevice,Manage> Scratch(lw*sizeof(LeastSquaresSystem<float,6>),lh);
    roo::Image<float, TargetDevice, Manage>  Err(lw,lh);

    typedef ulong4 census_t;
    roo::Image<census_t, TargetDevice, Manage> census[] = {{lw,lh},{lw,lh}};

    roo::Image<unsigned char, TargetHost, Manage> hImg[] = {{lw,lh},{lw,lh}};
    roo::Image<float, TargetHost, Manage> hDisp(lw,lh);

#ifdef COSTVOL_TIME
    Sophus::SE3d T_wv;
    Volume<CostVolElem, TargetDevice, Manage>  dCostVol(lw,lh,MAXD);
    roo::Image<unsigned char, TargetDevice, Manage> dImgv(lw,lh);
#endif

#ifdef HM_FUSION
//    HeightmapFusion hm(800,800,2);
//    HeightmapFusion hm(200,200,10);
    HeightmapFusion hm(100,100,10);
    const bool center_y = false;

    GlBufferCudaPtr vbo_hm(GlArrayBuffer, hm.WidthPixels(), hm.HeightPixels(), GL_FLOAT, 4, cudaGraphicsMapFlagsWriteDiscard, GL_STREAM_DRAW );
    GlBufferCudaPtr cbo_hm(GlArrayBuffer, hm.WidthPixels(), hm.HeightPixels(), GL_UNSIGNED_BYTE, 4, cudaGraphicsMapFlagsWriteDiscard, GL_STREAM_DRAW );
    GlBufferCudaPtr ibo_hm(GlElementArrayBuffer, hm.WidthPixels(), hm.HeightPixels(), GL_UNSIGNED_INT, 2 );

    //generate index buffer for heightmap
    {
        CudaScopedMappedPtr var(ibo_hm);
        roo::Image<uint2> dIbo((uint2*)*var,hm.WidthPixels(),hm.HeightPixels());
        GenerateTriangleStripIndexBuffer(dIbo);
    }
#endif // HM_FUSION

    // Stereo transformation (post-rectification)
    Sophus::SE3d T_rl = T_rl_orig;

    // Build camera distortion lookup tables
    if(rectify) {
        T_rl = CreateScanlineRectifiedLookupAndT_rl(
                    dLookup[0], dLookup[1], T_rl_orig,
                    cam[0].K(), cam[0].GetModel()->warped.kappa1, cam[0].GetModel()->warped.kappa2,
                    cam[1].K(), cam[1].GetModel()->warped.kappa1, cam[1].GetModel()->warped.kappa2
                    );
    }

    const double baseline = T_rl.translation().norm();

    cudaMemGetInfo( &cu_mem_end, &cu_mem_total );
    cout << "CuTotal: " << cu_mem_total/(1024*1024) << ", Available: " << cu_mem_end/(1024*1024) << ", Used: " << (cu_mem_start-cu_mem_end)/(1024*1024) << endl;

    Var<bool> step("ui.step", false, false);
    Var<bool> run("ui.run", false, true);
    Var<bool> save_dm("ui.save_dm", false);
    Var<bool> record("ui.record_dm", false,false);
    Var<bool> lockToCam("ui.Lock to cam", false, true);
    Var<int> show_level("ui.show level",0, 0, max_levels-1);
    Var<int> show_slice("ui.show slice",MAXD/2, 0, MAXD-1);

    Var<int> maxdisp("ui.maxdisp",MAXD, 0, MAXD);
    Var<bool> subpix("ui.subpix", true, true);

    Var<bool> use_census("ui.use census", false, true);

    Var<float> alpha("ui.alpha", 0.9, 0,1);
    Var<float> r1("ui.r1", 0.0028, 0,0.01);
    Var<float> r2("ui.r2", 0.008, 0,0.01);

    Var<bool> filter("ui.filter", true, true);
    Var<float> eps("ui.eps",0.01*0.01, 0, 0.01);
    Var<int> rad("ui.radius",9, 1, 20);

    Var<bool> applyBilateralFilter("ui.Apply Bilateral Filter", false, true);
    Var<int> bilateralWinSize("ui.size",18, 1, 20);
    Var<float> gs("ui.gs",10, 1E-3, 10);
    Var<float> gr("ui.gr",6, 1E-3, 10);
    Var<float> gc("ui.gc",0.01, 1E-4, 0.1);

    Var<bool> do_sgm_h("ui.SGM horiz", false, true);
    Var<bool> do_sgm_v("ui.SGM vert", false, true);
    Var<bool> do_sgm_reverse("ui.SGM reverse", false, true);
    Var<float> sgm_p1("ui.sgm p1",0.01, 0, 0.1);
    Var<float> sgm_p2("ui.sgm p2",0.02, 0, 1, false);

    Var<bool> leftrightcheck("ui.left-right check", true, true);
    Var<float> maxdispdiff("ui.maxdispdiff",1, 0, 5);

    Var<int> domedits("ui.median its",1, 1, 10);
    Var<bool> domed9x9("ui.median 9x9", false, true);
    Var<bool> domed7x7("ui.median 7x7", false, true);
    Var<bool> domed5x5("ui.median 5x5", false, true);
    Var<int> medi("ui.medi",12, 0, 24);

    Var<float> filtgradthresh("ui.filt grad thresh", 0, 0, 20);

    Var<int> avg_rad("ui.avg_rad",5, 1, 100);

#ifdef PLANE_FIT
    Var<bool> resetPlane("ui.resetplane", true, false);
    Var<bool> plane_do("ui.Compute Ground Plane", false, true);
    Var<float> plane_within("ui.Plane Within",20, 0.1, 100);
    Var<float> plane_c("ui.Plane c", 0.5, 0.0001, 1);
#endif // PLANE_FIT

#ifdef HM_FUSION
    Var<bool> fuse("ui.fuse", false, true);
    Var<bool> save_hm("ui.save heightmap", false, false);
#endif // HM_FUSION

//    Var<bool> draw_frustrum("ui.show frustrum", false, true);
//    Var<bool> show_mesh("ui.show mesh", true, true);
//    Var<bool> show_color("ui.show color", true, true);
    Var<bool> show_history("ui.show history", true, true);
    Var<bool> show_depthmap("ui.show depthmap", true, true);

#ifdef HM_FUSION
    Var<bool> show_heightmap("ui.show heightmap", false, true);
#endif // HM_FUSION

#ifdef COSTVOL_TIME
    Var<bool> cross_section("ui.Cross Section", true, true);
    Var<bool> costvol_reset("ui.Costvol Reset", true, false);
    Var<bool> costvol_reset_stereo("ui.Costvol Reset Stereo", false, false);
    Var<bool> costvol_add("ui.Add to Costvol", false, false);
#endif

    int jump_frames = 0;

    pangolin::RegisterKeyPressCallback(' ', [&run](){run = !run;} );
    pangolin::RegisterKeyPressCallback('l', [&lockToCam](){lockToCam = !lockToCam;} );
    pangolin::RegisterKeyPressCallback(PANGO_SPECIAL + GLUT_KEY_RIGHT, [&step](){step=true;} );
    pangolin::RegisterKeyPressCallback(']', [&jump_frames](){jump_frames=100;} );
    pangolin::RegisterKeyPressCallback('}', [&jump_frames](){jump_frames=1000;} );
    pangolin::RegisterKeyPressCallback('~', [&container](){static bool showpanel=true; showpanel = !showpanel; if(showpanel) { container.SetBounds(0,1,Attach::Pix(180), 1); }else{ container.SetBounds(0,1,0, 1); } Display("ui").Show(showpanel); } );
    pangolin::RegisterKeyPressCallback('1', [&container](){container[0].ToggleShow();} );
    pangolin::RegisterKeyPressCallback('2', [&container](){container[1].ToggleShow();} );
    pangolin::RegisterKeyPressCallback('3', [&container](){container[2].ToggleShow();} );
    pangolin::RegisterKeyPressCallback('4', [&container](){container[3].ToggleShow();} );
    pangolin::RegisterKeyPressCallback('$', [&container](){container[3].SaveRenderNow("screenshot",4);} );

    Handler2dImageSelect handler2d(lw,lh,level);
//    ActivateDrawPyramid<unsigned char,max_levels> adleft(img_pyr[0],GL_LUMINANCE8, false, true);
//    ActivateDrawPyramid<unsigned char,max_levels> adright(img_pyr[1],GL_LUMINANCE8, false, true);
    ActivateDrawImage<float> adleft(img[0],GL_LUMINANCE32F_ARB, false, true);
    ActivateDrawImage<float> adright(img[1],GL_LUMINANCE32F_ARB, false, true);
    ActivateDrawImage<float> adisp(disp[0],GL_LUMINANCE32F_ARB, false, true);
//    ActivateDrawImage<float> adCrossSection(dCrossSection,GL_RGBA_FLOAT32_APPLE, false, true);
    ActivateDrawImage<float> adVol(vol[0].ImageXY(show_slice),GL_LUMINANCE32F_ARB, false, true);

    SceneGraph::GLSceneGraph graph;
    SceneGraph::GLVbo glvbo(&vbo,&ibo,&cbo);

    SceneGraph::GLCameraHistory history;
    history.LoadFromAbsoluteCartesianFile(video.GetProperty("DataSourceDir") + "/pose.txt", video.GetProperty("StartFrame",0), 10000, T_vis_ro, T_ro_vis);
    graph.AddChild(&glvbo);
    graph.AddChild(&history);

#ifdef PLANE_FIT
    SceneGraph::GLGrid glGroundPlane;
    glvbo.AddChild(&glGroundPlane);
#endif

#ifdef HM_FUSION
    SceneGraph::GLVbo glhmvbo(&vbo_hm,&ibo_hm,&cbo_hm);
    graph.AddChild(&glhmvbo);
#endif

    SetupContainer(container, 5, (float)w/h);
    container[0].SetDrawFunction(boostd::ref(adleft)).SetHandler(&handler2d);
    container[1].SetDrawFunction(boostd::ref(adright)).SetHandler(&handler2d);
    container[2].SetDrawFunction(boostd::ref(adisp)).SetHandler(&handler2d);
    container[3].SetDrawFunction(boostd::ref(adVol)).SetHandler(&handler2d);
    container[4].SetDrawFunction(SceneGraph::ActivateDrawFunctor(graph, s_cam))
                .SetHandler( new Handler3D(s_cam, AxisNone) );

    for(int frame=0; !pangolin::ShouldQuit();)
    {
        const bool go = frame==0 || jump_frames > 0 || run || Pushed(step);

        for(; jump_frames > 0; jump_frames--) {
            video.Capture(Images);
        }

        if(go) {
            if(frame>0) video.Capture(Images);

            if(frame < gtPoseT_wh.size()) {
                T_wc = gtPoseT_wh[frame];
            }

            frame++;

            /////////////////////////////////////////////////////////////
            // Upload Images to device (Warp / Decimate if necessery)
            for(int i=0; i<2; ++i ) {
                hCamImg[i].ptr = Images[i].Image.data;

                if(rectify) {
                    upload.CopyFrom(hCamImg[i].SubImage(roi));
                    Warp(img_pyr[i][0], upload, dLookup[i]);
                }else{
                    img_pyr[i][0].CopyFrom(hCamImg[i].SubImage(roi));
                }
            }
        }

        if(go || GuiVarHasChanged() )
        {
            for(int i=0; i<2; ++i ) {
                BoxReduce<unsigned char, max_levels, unsigned int>(img_pyr[i]);
                ElementwiseScaleBias<float,unsigned char,float>(img[i], img_pyr[i][level],1.0f/255.0f);
//                BoxFilter<float,float,float>(temp[0],img[i],Scratch,avg_rad);
//                ElementwiseAdd<float,float,float,float>(img[i], img[i], temp[0], 1, -1, 0.5);

                Census(census[i], img[i]);
            }

            if(use_census) {
                CensusStereoVolume<float, census_t>(vol[0], census[0], census[1], maxdisp, -1);
                if(leftrightcheck) CensusStereoVolume<float, census_t>(vol[1], census[1], census[0], maxdisp, +1);
            }else{
                CostVolumeFromStereoTruncatedAbsAndGrad(vol[0], img[0], img[1], -1, alpha, r1, r2);
                if(leftrightcheck) CostVolumeFromStereoTruncatedAbsAndGrad(vol[1], img[1], img[0], +1, alpha, r1, r2);
            }

            if(filter) {
                // Filter Cost volume
                for(int v=0; v<(leftrightcheck?2:1); ++v)
                {
                    roo::Image<float, TargetDevice, Manage>& I = img[v];
                    ComputeMeanVarience<float,float,float>(varI, temp[0], meanI, I, Scratch, rad);

                    for(int d=0; d<maxdisp; ++d)
                    {
                        roo::Image<float> P = vol[v].ImageXY(d);
                        ComputeCovariance(temp[0],temp[2],temp[1],P,meanI,I,Scratch,rad);
                        GuidedFilter(P,temp[0],varI,temp[1],meanI,I,Scratch,temp[2],temp[3],temp[4],rad,eps);
                    }
                }
            }

            if(applyBilateralFilter) {
                // Filter Cost volume
                for(int v=0; v<(leftrightcheck?2:1); ++v)
                {
                    roo::Image<float, TargetDevice, Manage>& I = img[v];

                    for(int d=0; d<maxdisp; ++d)
                    {
                        roo::Image<float> P = vol[v].ImageXY(d);
                        temp[0].CopyFrom(P);
                        BilateralFilter<float,float,float>(P,temp[0],I,gs,gr,gc,bilateralWinSize);
                    }
                }
            }

            if(do_sgm_h || do_sgm_v) {
                for(int i=0; i<1; ++i) {
                    SemiGlobalMatching<float,float,float>(vol[2],vol[i],img[i], maxdisp, sgm_p1, sgm_p2, do_sgm_h, do_sgm_v, do_sgm_reverse);
                    vol[i].CopyFrom(vol[2]);
                }
            }

            if(subpix) {
                CostVolMinimumSubpix(disp[0],vol[0], maxdisp,-1);
                if(leftrightcheck) CostVolMinimumSubpix(disp[1],vol[1], maxdisp,+1);
            }else{
                CostVolMinimum<float,float>(disp[0],vol[0], maxdisp);
                if(leftrightcheck) CostVolMinimum<float,float>(disp[1],vol[1], maxdisp);
            }

            for(int di=0; di<(leftrightcheck?2:1); ++di) {
                for(int i=0; i < domedits; ++i ) {
                    if(domed9x9) MedianFilterRejectNegative9x9(disp[di],disp[di], medi);
                    if(domed7x7) MedianFilterRejectNegative7x7(disp[di],disp[di], medi);
                    if(domed5x5) MedianFilterRejectNegative5x5(disp[di],disp[di], medi);
                }
            }

//            if(applyBilateralFilter) {
//                temp[0].CopyFrom(disp[0]);
//                BilateralFilter<float,float,float>(disp[0],temp[0],img[0],gs,gr,gc,bilateralWinSize);
//            }

            if(leftrightcheck ) {
                LeftRightCheck(disp[1], disp[0], +1, maxdispdiff);
                LeftRightCheck(disp[0], disp[1], -1, maxdispdiff);
            }

            if(filtgradthresh > 0) {
                FilterDispGrad(disp[0], disp[0], filtgradthresh);
            }

#ifdef COSTVOL_TIME
            if(Pushed(costvol_reset)) {
                T_wv = T_wc;
                dImgv.CopyFrom(img_pyr[0][level]);
                CostVolumeZero(dCostVol);
            }

            if(Pushed(costvol_reset_stereo)) {
                T_wv = T_wc;
                dImgv.CopyFrom(img_pyr[0][level]);
                CostVolumeFromStereo(dCostVol,img_pyr[0][level], img_pyr[1][level]);
            }

            if(Pushed(costvol_add)) {
                const Eigen::Matrix<double,3,4> KT_lv = Kl * (T_wc.inverse() * T_wv).matrix3x4();
                CostVolumeAdd(dCostVol,dImgv, img_pyr[0][level], KT_lv, Kl(0,0), Kl(1,1), Kl(0,2), Kl(1,2), baseline, 0);
            }

            // Extract Minima of cost volume
//            CostVolMinimum(disp[0], dCostVol);
#endif // COSTVOL_TIME

            if(go && save_dm) {
                // file info
                char Index[10];
                sprintf( Index, "%05d", frame );
                string sFileName = (std::string)"disp/depth-" + Index + ".pdm";

                // Save Disparity Image
                hDisp.CopyFrom(disp[0]);
                SavePXM<float>(sFileName, hDisp, "P7", maxdisp);

                if(rectify)
                {
                    // Save rectified Images
                    hImg[0].CopyFrom(img_pyr[0][0]);
                    SavePXM<unsigned char>((std::string)"disp/left-" + Index + ".pgm", hImg[0], "P5");
                    hImg[1].CopyFrom(img_pyr[1][0]);
                    SavePXM<unsigned char>((std::string)"disp/right-" + Index + ".pgm", hImg[1], "P5");
                }
            }

            // Generate point cloud from disparity Image
            DisparityImageToVbo(d3d, disp[0], baseline, Kl(0,0), Kl(1,1), Kl(0,2), Kl(1,2) );

#ifdef PLANE_FIT
            if(plane_do || resetPlane) {
                // Fit plane
                for(int i=0; i<(resetPlane*100+5); ++i )
                {
                    roo::LeastSquaresSystem<float,3> lss = PlaneFitGN(d3d, Qinv, z, Scratch, Err, plane_within, plane_c);
                    Eigen::FullPivLU<Eigen::Matrix3d> lu_JTJ( (Eigen::Matrix3d)lss.JTJ );
                    Eigen::Vector3d x = -1.0 * lu_JTJ.solve( (Eigen::Vector3d)lss.JTy );
                    if( x.norm() > 1 ) x = x / x.norm();
                    for(int i=0; i<3; ++i ) {
                        z(i) *= exp(x(i));
                    }
                    n_c = Qinv * z;
                    n_w = project((Eigen::Vector4d)(T_wc.inverse().matrix().transpose() * unproject(n_c)));
                }
            }
#endif // PLANE_FIT

#ifdef HM_FUSION
            if(Pushed(resetPlane) ) {
                Eigen::Matrix4d T_nw = (PlaneBasis_wp(n_c).inverse() * T_wc.inverse()).matrix();
//                T_nw.block<2,1>(0,3) += Eigen::Vector2d(hm.WidthMeters()/2, hm.HeightMeters() /*/2*/);
                T_nw.block<2,1>(0,3) += Eigen::Vector2d(hm.WidthMeters()/2, hm.HeightMeters() / (center_y ? 2 : 1) );
                hm.Init(T_nw);
            }

            //calcualte the camera to heightmap transform
            if(fuse) {
                hm.Fuse(d3d, img_pyr[0][level], T_wc);
                hm.GenerateVbo(vbo_hm);
                hm.GenerateCbo(cbo_hm);
            }

            if(Pushed(save_hm)) {
                hm.SaveModel("test");
            }
#elif PLANE_FIT
            resetPlane = false;
#endif

//            if(container[3].IsShown())
            {
                // Copy point cloud into VBO
                {
                    CudaScopedMappedPtr var(vbo);
                    roo::Image<float4> dVbo((float4*)*var,lw,lh);
                    dVbo.CopyFrom(d3d);
                }

                // Generate CBO
                {
                    CudaScopedMappedPtr var(cbo);
                    roo::Image<uchar4> dCbo((uchar4*)*var,lw,lh);
#ifdef COSTVOL_TIME
                    ConvertImage<uchar4,unsigned char>(dCbo, dImgv);
#else
                    ConvertImage<uchar4,unsigned char>(dCbo, img_pyr[0][level]);
#endif
                }
            }

            // Update texture views
            adisp.SetImageScale(1.0f/maxdisp);
//            adleft.SetLevel(show_level);
//            adright.SetLevel(show_level);
            adVol.SetImage(vol[0].ImageXY(show_slice));
        }

#ifdef COSTVOL_TIME
        if(cross_section) {
            CostVolumeCrossSection(dCrossSection, dCostVol, handler2d.GetSelectedPoint(true)[1] + 0.5);
        }
#endif // COSTVOL_TIME

        if(Pushed(record)) {
            // Start recording next frame
            frame = 0;
            video.InitDriver("FileReader");
            video.Capture(Images);
            save_dm = true;
        }

        /////////////////////////////////////////////////////////////
        // Setup Drawing

        s_cam.Follow(T_wc.matrix(), lockToCam);

#ifdef COSTVOL_TIME
        glvbo.SetPose(T_wv.matrix());
        glvbo.SetVisible(show_depthmap);
#else
        glvbo.SetPose(T_wc.matrix());
#endif

#ifdef PLANE_FIT
        glGroundPlane.SetPose(PlaneBasis_wp(n_c).matrix());
        glGroundPlane.SetVisible(plane_do);
#endif // PLANE_FIT

#ifdef HM_FUSION
        glhmvbo.SetPose((Eigen::Matrix4d)hm.T_hw().inverse());
        glhmvbo.SetVisible(show_heightmap);
#endif // HM_FUSION

        history.SetNumberToShow(frame);
        history.SetVisible(show_history);

        /////////////////////////////////////////////////////////////
        // Draw

        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        glColor3f(1,1,1);

        pangolin::FinishFrame();
    }
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Evolution/DiscontinuousGalerkin/Limiters/WenoHelpers.hpp"

#include <boost/functional/hash.hpp>  // IWYU pragma: keep
#include <cstddef>
#include <unordered_map>
#include <utility>

#include "DataStructures/DataVector.hpp"
#include "DataStructures/Variables.hpp"    // IWYU pragma: keep
#include "Domain/Structure/Direction.hpp"  // IWYU pragma: keep
#include "Domain/Structure/ElementId.hpp"  // IWYU pragma: keep
#include "Evolution/DiscontinuousGalerkin/Limiters/WenoOscillationIndicator.hpp"
#include "NumericalAlgorithms/LinearOperators/MeanValue.hpp"
#include "Utilities/ConstantExpressions.hpp"
#include "Utilities/EqualWithinRoundoff.hpp"
#include "Utilities/ErrorHandling/Assert.hpp"
#include "Utilities/GenerateInstantiations.hpp"
#include "Utilities/Gsl.hpp"

namespace {

// Compute the unnormalized nonlinear WENO weights. This is a fairly standard
// choice of weights; see e.g., Eq. 3.9 of Zhong2013 or Eq. 3.6 of Zhu2016.
inline double unnormalized_nonlinear_weight(
    const double linear_weight, const double oscillation_indicator) {
  return linear_weight / square(1.e-6 + oscillation_indicator);
}

}  // namespace

namespace Limiters::Weno_detail {

template <size_t VolumeDim>
void reconstruct_from_weighted_sum(
    const gsl::not_null<DataVector*> local_polynomial,
    const double neighbor_linear_weight,
    const DerivativeWeight derivative_weight, const Mesh<VolumeDim>& mesh,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, DataVector,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_polynomials) {
#ifdef SPECTRE_DEBUG
  ASSERT(local_polynomial->size() > 0,
         "The local_polynomial values are missing - was the input correctly\n"
         "initialized before calling reconstruct_from_weighted_sum?");
  // Check inputs match requirements
  const double local_mean = mean_value(*local_polynomial, mesh);
  for (const auto& kv : neighbor_polynomials) {
    const auto& neighbor_polynomial = kv.second;
    const double neighbor_mean = mean_value(neighbor_polynomial, mesh);
    ASSERT(equal_within_roundoff(local_mean, neighbor_mean),
           "Invalid inputs to Weno_detail::reconstruct_from_weighted_sum:\n"
           "The neighbor polynomials should have the same mean as the local\n"
           "polynomial.");
  }
#endif  // ifdef SPECTRE_DEBUG

  // Store linear weights in `local_weights` and `neighbor_weights`
  // These weights will have to be generalized for multiple neighbors per
  // face for use with h-refinement and AMR.
  double local_weight = 1.;
  std::unordered_map<
      std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, double,
      boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
      neighbor_weights;
  for (const auto& kv : neighbor_polynomials) {
    local_weight -= neighbor_linear_weight;
    neighbor_weights[kv.first] = neighbor_linear_weight;
  }

  // Update `local_weights` and `neighbor_weights` to hold the unnormalized
  // nonlinear weights.
  local_weight = unnormalized_nonlinear_weight(
      local_weight,
      oscillation_indicator(derivative_weight, *local_polynomial, mesh));
  for (const auto& kv : neighbor_polynomials) {
    const auto& key = kv.first;
    const auto& neighbor_polynomial = kv.second;
    neighbor_weights[key] = unnormalized_nonlinear_weight(
        neighbor_weights[key],
        oscillation_indicator(derivative_weight, neighbor_polynomial, mesh));
  }

  // Update `local_weights` and `neighbor_weights` to hold the normalized
  // weights; these are the final weights of the WENO reconstruction.
  double normalization = local_weight;
  for (const auto& kv : neighbor_weights) {
    normalization += kv.second;
  }
  local_weight /= normalization;
  for (auto& kv : neighbor_weights) {
    kv.second /= normalization;
  }

  // Perform reconstruction by combining the local and neighbor polynomials.
  *local_polynomial *= local_weight;
  for (const auto& kv : neighbor_polynomials) {
    const auto& key = kv.first;
    const auto& neighbor_polynomial = kv.second;
    *local_polynomial += neighbor_weights.at(key) * neighbor_polynomial;
  }
}

// Explicit instantiations
#define DIM(data) BOOST_PP_TUPLE_ELEM(0, data)

#define INSTANTIATE(_, data)                                                  \
  template void reconstruct_from_weighted_sum(                                \
      const gsl::not_null<DataVector*>, const double, const DerivativeWeight, \
      const Mesh<DIM(data)>&,                                                 \
      const std::unordered_map<                                               \
          std::pair<Direction<DIM(data)>, ElementId<DIM(data)>>, DataVector,  \
          boost::hash<                                                        \
              std::pair<Direction<DIM(data)>, ElementId<DIM(data)>>>>&);

GENERATE_INSTANTIATIONS(INSTANTIATE, (1, 2, 3))

#undef DIM
#undef INSTANTIATE

}  // namespace Limiters::Weno_detail


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <boost/python/class.hpp>
#include <boost/python/return_internal_reference.hpp>

#include <scitbx/sparse/matrix.h>
#include <scitbx/sparse/lu_factorization.h>

namespace scitbx { namespace sparse { namespace boost_python {

template<typename FloatType>
struct gilbert_peierls_lu_factorization_wrapper
{
  typedef gilbert_peierls_lu_factorization<matrix<FloatType> > wt;

  static void wrap(char const *name) {
    using namespace boost::python;
    return_internal_reference<> rir;
    class_<wt>(name, no_init)
      .def(init<typename wt::matrix_type const&>(arg("matrix")))
      .def("factored", &wt::factored, rir)
      .def("l", &wt::l, rir)
      .def("u", &wt::u, rir)
      .def("rows_permutation", &wt::rows_permutation)
      ;
  }
};

void wrap_lu_factorization() {
  gilbert_peierls_lu_factorization_wrapper<double>::wrap(
    "gilbert_peierls_lu_factorization");
}

}}} // scitbx::sparse::boost_python


/*
 * IRIS Localization and Mapping (LaMa)
 *
 * Copyright (c) 2019-today, Eurico Pedrosa, University of Aveiro - Portugal
 * All rights reserved.
 * License: New BSD
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the University of Aveiro nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <cmath>
#include <Eigen/Cholesky>

#include "lama/nlls/levenberg_marquardt.h"

lama::LevenbergMarquard::Options::Options()
{
    eps1 = 1e-4;
    eps2 = 1e-4;
    tau  = 1e-4;
}

lama::LevenbergMarquard::LevenbergMarquard(const Options& options)
    : opt_(options)
{}

void lama::LevenbergMarquard::reset()
{
    mu_ = -1;
    v_  = 2.0;
    stop_ = false;
}

Eigen::VectorXd lama::LevenbergMarquard::step(const VectorXd& residuals, const MatrixXd& J)
{
    chi2_ = residuals.squaredNorm();
    g_    = J.transpose() * residuals;

    double max_abs_g = g_.lpNorm<Eigen::Infinity>();
    if (max_abs_g < opt_.eps1){
        stop_ = true;
        return VectorXd::Zero(J.cols());
    }

    MatrixXd A = J.transpose() * J;
    // mu_ is allways positive because the diagonal of A is also allways
    // positive. Therefore, a negative mu_ requires initialization.
    if ( mu_ < 0 )
        mu_ = opt_.tau * A.diagonal().maxCoeff();

    A.diagonal().array() += (mu_ );
    // Solve the system
    h_ = A.selfadjointView<Eigen::Upper>().llt().solve(-g_);

    double max_abs_h = h_.lpNorm<Eigen::Infinity>();
    if (max_abs_h < opt_.eps2)
        stop_ = true;

    return h_;
}

bool lama::LevenbergMarquard::valid(const VectorXd& residuals)
{
    if (stop_) return true;

    double dF = (chi2_ - residuals.squaredNorm());
    double dL = 0.5*h_.transpose().dot(mu_*h_ - g_);

    if ( dL > 0.0 and dF > 0.0 ){
        // update mu
        mu_ = mu_ * std::max( 1.0 / 3.0, 1 - std::pow(2*(dF/dL)-1,3) );
        v_  = 2.0;

        return true;
    }

    mu_ = mu_ * v_; v_ = 2*v_;
    return false;
}

bool lama::LevenbergMarquard::stop()
{
    return stop_;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2014 Anton Bikineev
//  Copyright 2014 Christopher Kormanyos
//  Copyright 2014 John Maddock
//  Copyright 2014 Paul Bristow
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef BOOST_MATH_HYPERGEOMETRIC_1F1_BESSEL_HPP
#define BOOST_MATH_HYPERGEOMETRIC_1F1_BESSEL_HPP

#include <boost/math/tools/series.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/laguerre.hpp>
#include <boost/math/special_functions/detail/hypergeometric_pFq_checked_series.hpp>
#include <boost/math/special_functions/bessel_iterators.hpp>

  namespace boost { namespace math { namespace detail {

     template <class T, class Policy>
     T hypergeometric_1F1_divergent_fallback(const T& a, const T& b, const T& z, const Policy& pol, int& log_scaling);

     template <class T>
     bool hypergeometric_1F1_is_tricomi_viable_positive_b(const T& a, const T& b, const T& z)
     {
        BOOST_MATH_STD_USING
           if ((z < b) && (a > -50))
              return false;  // might as well fall through to recursion
        if (b <= 100)
           return true;
        // Even though we're in a reasonable domain for Tricomi's approximation,
        // the arguments to the Bessel functions may be so large that we can't
        // actually evaluate them:
        T x = sqrt(fabs(2 * z * b - 4 * a * z));
        T v = b - 1;
        return log(boost::math::constants::e<T>() * x / (2 * v)) * v > tools::log_min_value<T>();
     }

     //
     // Returns an arbitrarily small value compared to "target" for use as a seed
     // value for Bessel recurrences.  Note that we'd better not make it too small
     // or underflow may occur resulting in either one of the terms in the
     // recurrence being zero, or else the result being zero.  Using 1/epsilon
     // as a safety factor ensures that if we do underflow to zero, all of the digits
     // will have been cancelled out anyway:
     //
     template <class T>
     T arbitrary_small_value(const T& target)
     {
        using std::fabs;
        return (tools::min_value<T>() / tools::epsilon<T>()) * (fabs(target) > 1 ? target : 1);
     }

     template <class T, class Policy>
     struct hypergeometric_1F1_AS_13_3_7_tricomi_series
     {
        typedef T result_type;

        enum { cache_size = 64 };

        hypergeometric_1F1_AS_13_3_7_tricomi_series(const T& a, const T& b, const T& z, const Policy& pol_)
           : A_minus_2(1), A_minus_1(0), A(b / 2), mult(z / 2), term(1), b_minus_1_plus_n(b - 1),
            bessel_arg((b / 2 - a) * z),
           two_a_minus_b(2 * a - b), pol(pol_), n(2)
        {
           BOOST_MATH_STD_USING
           term /= pow(fabs(bessel_arg), b_minus_1_plus_n / 2);
           mult /= sqrt(fabs(bessel_arg));
           bessel_cache[cache_size - 1] = bessel_arg > 0 ? boost::math::cyl_bessel_j(b_minus_1_plus_n - 1, 2 * sqrt(bessel_arg), pol) : boost::math::cyl_bessel_i(b_minus_1_plus_n - 1, 2 * sqrt(-bessel_arg), pol);
           if (fabs(bessel_cache[cache_size - 1]) < tools::min_value<T>() / tools::epsilon<T>())
           {
              // We get very limited precision due to rapid denormalisation/underflow of the Bessel values, raise an exception and try something else:
              policies::raise_evaluation_error("hypergeometric_1F1_AS_13_3_7_tricomi_series<%1%>", "Underflow in Bessel functions", bessel_cache[cache_size - 1], pol);
           }
           if ((term * bessel_cache[cache_size - 1] < tools::min_value<T>() / (tools::epsilon<T>() * tools::epsilon<T>())) || !(boost::math::isfinite)(term) || (!std::numeric_limits<T>::has_infinity && (fabs(term) > tools::max_value<T>())))
           {
              term = -log(fabs(bessel_arg)) * b_minus_1_plus_n / 2;
              log_scale = itrunc(term);
              term -= log_scale;
              term = exp(term);
           }
           else
              log_scale = 0;
#ifndef BOOST_NO_CXX17_IF_CONSTEXPR
           if constexpr (std::numeric_limits<T>::has_infinity)
           {
              if (!(boost::math::isfinite)(bessel_cache[cache_size - 1]))
                 policies::raise_evaluation_error("hypergeometric_1F1_AS_13_3_7_tricomi_series<%1%>", "Expected finite Bessel function result but got %1%", bessel_cache[cache_size - 1], pol);
           }
           else
              if ((boost::math::isnan)(bessel_cache[cache_size - 1]) || (fabs(bessel_cache[cache_size - 1]) >= tools::max_value<T>()))
                 policies::raise_evaluation_error("hypergeometric_1F1_AS_13_3_7_tricomi_series<%1%>", "Expected finite Bessel function result but got %1%", bessel_cache[cache_size - 1], pol);
#else
           if ((std::numeric_limits<T>::has_infinity && !(boost::math::isfinite)(bessel_cache[cache_size - 1]))
              || (!std::numeric_limits<T>::has_infinity && ((boost::math::isnan)(bessel_cache[cache_size - 1]) || (fabs(bessel_cache[cache_size - 1]) >= tools::max_value<T>()))))
              policies::raise_evaluation_error("hypergeometric_1F1_AS_13_3_7_tricomi_series<%1%>", "Expected finite Bessel function result but got %1%", bessel_cache[cache_size - 1], pol);
#endif
           cache_offset = -cache_size;
           refill_cache();
        }
        T operator()()
        {
           //
           // We return the n-2 term, and do 2 terms at once as every other term can be
           // very small (or zero) when b == 2a:
           //
           BOOST_MATH_STD_USING
           if(n - 2 - cache_offset >= cache_size)
              refill_cache();
           T result = A_minus_2 * term * bessel_cache[n - 2 - cache_offset];
           term *= mult;
           ++n;
           T A_next = ((b_minus_1_plus_n + 2) * A_minus_1 + two_a_minus_b * A_minus_2) / n;
           b_minus_1_plus_n += 1;
           A_minus_2 = A_minus_1;
           A_minus_1 = A;
           A = A_next;

           if (A_minus_2 != 0)
           {
              if (n - 2 - cache_offset >= cache_size)
                 refill_cache();
              result += A_minus_2 * term * bessel_cache[n - 2 - cache_offset];
           }
           term *= mult;
           ++n;
           A_next = ((b_minus_1_plus_n + 2) * A_minus_1 + two_a_minus_b * A_minus_2) / n;
           b_minus_1_plus_n += 1;
           A_minus_2 = A_minus_1;
           A_minus_1 = A;
           A = A_next;

           return result;
        }

        int scale()const
        {
           return log_scale;
        }

     private:
        T A_minus_2, A_minus_1, A, mult, term, b_minus_1_plus_n, bessel_arg, two_a_minus_b;
        std::array<T, cache_size> bessel_cache;
        const Policy& pol;
        int n, log_scale, cache_offset;

        hypergeometric_1F1_AS_13_3_7_tricomi_series operator=(const hypergeometric_1F1_AS_13_3_7_tricomi_series&);

        void refill_cache()
        {
           BOOST_MATH_STD_USING
           //
           // We don't calculate a new bessel I/J value: instead start our iterator off
           // with an arbitrary small value, then when we get back to the last value in the previous cache
           // calculate the ratio and use it to renormalise all the new values.  This is more efficient, but
           // also avoids problems with J_v(x) or I_v(x) underflowing to zero.
           //
           cache_offset += cache_size;
           T last_value = bessel_cache.back();
           T ratio;
           if (bessel_arg > 0)
           {
              //
              // We will be calculating Bessel J.
              // We need a different recurrence strategy for positive and negative orders:
              //
              if (b_minus_1_plus_n > 0)
              {
                 bessel_j_backwards_iterator<T> i(b_minus_1_plus_n + (int)cache_size - 1, 2 * sqrt(bessel_arg), arbitrary_small_value(last_value));

                 for (int j = cache_size - 1; j >= 0; --j, ++i)
                 {
                    bessel_cache[j] = *i;
                    //
                    // Depending on the value of bessel_arg, the values stored in the cache can grow so
                    // large as to overflow, if that looks likely then we need to rescale all the
                    // existing terms (most of which will then underflow to zero).  In this situation
                    // it's likely that our series will only need 1 or 2 terms of the series but we
                    // can't be sure of that:
                    //
                    if ((j < cache_size - 2) && (tools::max_value<T>() / fabs(64 * bessel_cache[j] / bessel_cache[j + 1]) < fabs(bessel_cache[j])))
                    {
                       T rescale = pow(fabs(bessel_cache[j] / bessel_cache[j + 1]), j + 1) * 2;
                       if (!((boost::math::isfinite)(rescale)))
                          rescale = tools::max_value<T>();
                       for (int k = j; k < cache_size; ++k)
                          bessel_cache[k] /= rescale;
                       bessel_j_backwards_iterator<T> ti(b_minus_1_plus_n + j, 2 * sqrt(bessel_arg), bessel_cache[j + 1], bessel_cache[j]);
                       i = ti;
                    }
                 }
                 ratio = last_value / *i;
              }
              else
              {
                 //
                 // Negative order is difficult: the J_v(x) recurrence relations are unstable
                 // *in both directions* for v < 0, except as v -> -INF when we have
                 // J_-v(x)  ~= -sin(pi.v)Y_v(x).
                 // For small v what we can do is compute every other Bessel function and
                 // then fill in the gaps using the recurrence relation.  This *is* stable
                 // provided that v is not so negative that the above approximation holds.
                 //
                 bessel_cache[1] = cyl_bessel_j(b_minus_1_plus_n + 1, 2 * sqrt(bessel_arg), pol);
                 bessel_cache[0] = (last_value + bessel_cache[1]) / (b_minus_1_plus_n / sqrt(bessel_arg));
                 int pos = 2;
                 while ((pos < cache_size - 1) && (b_minus_1_plus_n + pos < 0))
                 {
                    bessel_cache[pos + 1] = cyl_bessel_j(b_minus_1_plus_n + pos + 1, 2 * sqrt(bessel_arg), pol);
                    bessel_cache[pos] = (bessel_cache[pos-1] + bessel_cache[pos+1]) / ((b_minus_1_plus_n + pos) / sqrt(bessel_arg));
                    pos += 2;
                 }
                 if (pos < cache_size)
                 {
                    //
                    // We have crossed over into the region where backward recursion is the stable direction
                    // start from arbitrary value and recurse down to "pos" and normalise:
                    //
                    bessel_j_backwards_iterator<T> i2(b_minus_1_plus_n + (int)cache_size - 1, 2 * sqrt(bessel_arg), arbitrary_small_value(bessel_cache[pos-1]));
                    for (int loc = cache_size - 1; loc >= pos; --loc)
                       bessel_cache[loc] = *i2++;
                    ratio = bessel_cache[pos - 1] / *i2;
                    //
                    // Sanity check, if we normalised to an unusually small value then it was likely
                    // to be near a root and the calculated ratio is garbage, if so perform one
                    // more J_v(x) evaluation at position and normalise again:
                    //
                    if (fabs(bessel_cache[pos] * ratio / bessel_cache[pos - 1]) > 5)
                       ratio = cyl_bessel_j(b_minus_1_plus_n + pos, 2 * sqrt(bessel_arg), pol) / bessel_cache[pos];
                    while (pos < cache_size)
                       bessel_cache[pos++] *= ratio;
                 }
                 ratio = 1;
              }
           }
           else
           {
              //
              // Bessel I.
              // We need a different recurrence strategy for positive and negative orders:
              //
              if (b_minus_1_plus_n > 0)
              {
                 bessel_i_backwards_iterator<T> i(b_minus_1_plus_n + (int)cache_size - 1, 2 * sqrt(-bessel_arg), arbitrary_small_value(last_value));

                 for (int j = cache_size - 1; j >= 0; --j, ++i)
                 {
                    bessel_cache[j] = *i;
                    //
                    // Depending on the value of bessel_arg, the values stored in the cache can grow so
                    // large as to overflow, if that looks likely then we need to rescale all the
                    // existing terms (most of which will then underflow to zero).  In this situation
                    // it's likely that our series will only need 1 or 2 terms of the series but we
                    // can't be sure of that:
                    //
                    if ((j < cache_size - 2) && (tools::max_value<T>() / fabs(64 * bessel_cache[j] / bessel_cache[j + 1]) < fabs(bessel_cache[j])))
                    {
                       T rescale = pow(fabs(bessel_cache[j] / bessel_cache[j + 1]), j + 1) * 2;
                       if (!((boost::math::isfinite)(rescale)))
                          rescale = tools::max_value<T>();
                       for (int k = j; k < cache_size; ++k)
                          bessel_cache[k] /= rescale;
                       i = bessel_i_backwards_iterator<T>(b_minus_1_plus_n + j, 2 * sqrt(-bessel_arg), bessel_cache[j + 1], bessel_cache[j]);
                    }
                 }
                 ratio = last_value / *i;
              }
              else
              {
                 //
                 // Forwards iteration is stable:
                 //
                 bessel_i_forwards_iterator<T> i(b_minus_1_plus_n, 2 * sqrt(-bessel_arg));
                 int pos = 0;
                 while ((pos < cache_size) && (b_minus_1_plus_n + pos < 0.5))
                 {
                    bessel_cache[pos++] = *i++;
                 }
                 if (pos < cache_size)
                 {
                    //
                    // We have crossed over into the region where backward recursion is the stable direction
                    // start from arbitrary value and recurse down to "pos" and normalise:
                    //
                    bessel_i_backwards_iterator<T> i2(b_minus_1_plus_n + (int)cache_size - 1, 2 * sqrt(-bessel_arg), arbitrary_small_value(last_value));
                    for (int loc = cache_size - 1; loc >= pos; --loc)
                       bessel_cache[loc] = *i2++;
                    ratio = bessel_cache[pos - 1] / *i2;
                    while (pos < cache_size)
                       bessel_cache[pos++] *= ratio;
                 }
                 ratio = 1;
              }
           }
           if(ratio != 1)
              for (auto j = bessel_cache.begin(); j != bessel_cache.end(); ++j)
                 *j *= ratio;
           //
           // Very occationally our normalisation fails because the normalisztion value
           // is sitting right on top of a root (or very close to it).  When that happens
           // best to calculate a fresh Bessel evaluation and normalise again.
           //
           if (fabs(bessel_cache[0] / last_value) > 5)
           {
              ratio = (bessel_arg < 0 ? cyl_bessel_i(b_minus_1_plus_n, 2 * sqrt(-bessel_arg), pol) : cyl_bessel_j(b_minus_1_plus_n, 2 * sqrt(bessel_arg), pol)) / bessel_cache[0];
              if (ratio != 1)
                 for (auto j = bessel_cache.begin(); j != bessel_cache.end(); ++j)
                    *j *= ratio;
           }
        }
     };

     template <class T, class Policy>
     T hypergeometric_1F1_AS_13_3_7_tricomi(const T& a, const T& b, const T& z, const Policy& pol, int& log_scale)
     {
        BOOST_MATH_STD_USING
        //
        // Works for a < 0, b < 0, z > 0.
        //
        // For convergence we require A * term to be converging otherwise we get
        // a divergent alternating series.  It's actually really hard to analyse this
        // and the best purely heuristic policy we've found is
        // z < fabs((2 * a - b) / (sqrt(fabs(a)))) ; b > 0  or:
        // z < fabs((2 * a - b) / (sqrt(fabs(ab)))) ; b < 0
        //
        T prefix(0);
        int prefix_sgn(0);
        bool use_logs = false;
        int scale = 0;
        //
        // We can actually support the b == 2a case within here, but we haven't
        // as we appear never to get here in practice.  Which means this get out
        // clause is a bit of defensive programming....
        //
        if(b == 2 * a)
           return hypergeometric_1F1_divergent_fallback(a, b, z, pol, log_scale);

        try
        {
           prefix = boost::math::tgamma(b, pol);
           prefix *= exp(z / 2);
        }
        catch (const std::runtime_error&)
        {
           use_logs = true;
        }
        if (use_logs || (prefix == 0) || !(boost::math::isfinite)(prefix) || (!std::numeric_limits<T>::has_infinity && (fabs(prefix) >= tools::max_value<T>())))
        {
           use_logs = true;
           prefix = boost::math::lgamma(b, &prefix_sgn, pol) + z / 2;
           scale = itrunc(prefix);
           log_scale += scale;
           prefix -= scale;
        }
        T result(0);
        boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
        bool retry = false;
        int series_scale = 0;
        try
        {
           hypergeometric_1F1_AS_13_3_7_tricomi_series<T, Policy> s(a, b, z, pol);
           series_scale = s.scale();
           log_scale += s.scale();
           try
           {
              T norm = 0;
              result = 0;
              if((a < 0) && (b < 0))
                 result = boost::math::tools::checked_sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter, result, norm);
              else
                 result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter, result);
              if (!(boost::math::isfinite)(result) || (result == 0) || (!std::numeric_limits<T>::has_infinity && (fabs(result) >= tools::max_value<T>())))
                 retry = true;
              if (norm / fabs(result) > 1 / boost::math::tools::root_epsilon<T>())
                 retry = true;  // fatal cancellation
           }
           catch (const std::overflow_error&)
           {
              retry = true;
           }
           catch (const boost::math::evaluation_error&)
           {
              retry = true;
           }
        }
        catch (const std::overflow_error&)
        {
           log_scale -= scale;
           return hypergeometric_1F1_divergent_fallback(a, b, z, pol, log_scale);
        }
        catch (const boost::math::evaluation_error&)
        {
           log_scale -= scale;
           return hypergeometric_1F1_divergent_fallback(a, b, z, pol, log_scale);
        }
        if (retry)
        {
           log_scale -= scale;
           log_scale -= series_scale;
           return hypergeometric_1F1_divergent_fallback(a, b, z, pol, log_scale);
        }
        boost::math::policies::check_series_iterations<T>("boost::math::hypergeometric_1F1_AS_13_3_7<%1%>(%1%,%1%,%1%)", max_iter, pol);
        if (use_logs)
        {
           int sgn = boost::math::sign(result);
           prefix += log(fabs(result));
           result = sgn * prefix_sgn * exp(prefix);
        }
        else
        {
           if ((fabs(result) > 1) && (fabs(prefix) > 1) && (tools::max_value<T>() / fabs(result) < fabs(prefix)))
           {
              // Overflow:
              scale = itrunc(tools::log_max_value<T>()) - 10;
              log_scale += scale;
              result /= exp(T(scale));
           }
           result *= prefix;
        }
        return result;
     }

     template <class T>
     struct cyl_bessel_i_large_x_sum
     {
        typedef T result_type;

        cyl_bessel_i_large_x_sum(const T& v, const T& x) : v(v), z(x), term(1), k(0) {}

        T operator()()
        {
           T result = term;
           ++k;
           term *= -(4 * v * v - (2 * k - 1) * (2 * k - 1)) / (8 * k * z);
           return result;
        }
        T v, z, term;
        int k;
     };

     template <class T, class Policy>
     T cyl_bessel_i_large_x_scaled(const T& v, const T& x, int& log_scaling, const Policy& pol)
     {
        BOOST_MATH_STD_USING
           cyl_bessel_i_large_x_sum<T> s(v, x);
        boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
        T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
        boost::math::policies::check_series_iterations<T>("boost::math::cyl_bessel_i_large_x<%1%>(%1%,%1%)", max_iter, pol);
        int scale = boost::math::itrunc(x);
        log_scaling += scale;
        return result * exp(x - scale) / sqrt(boost::math::constants::two_pi<T>() * x);
     }

     template <class T, class Policy>
     struct hypergeometric_1F1_AS_13_3_6_series
     {
        typedef T result_type;

        enum { cache_size = 64 };
        //
        // This series is only convergent/useful for a and b approximately equal
        // (ideally |a-b| < 1).  The series can also go divergent after a while
        // when b < 0, which limits precision to around that of double.  In that
        // situation we return 0 to terminate the series as otherwise the divergent
        // terms will destroy all the bits in our result before they do eventually
        // converge again.  One important use case for this series is for z < 0
        // and |a| << |b| so that either b-a == b or at least most of the digits in a
        // are lost in the subtraction.  Note that while you can easily convince yourself
        // that the result should be unity when b-a == b, in fact this is not (quite)
        // the case for large z.
        //
        hypergeometric_1F1_AS_13_3_6_series(const T& a, const T& b, const T& z, const T& b_minus_a, const Policy& pol_)
           : b_minus_a(b_minus_a), half_z(z / 2), poch_1(2 * b_minus_a - 1), poch_2(b_minus_a - a), b_poch(b), term(1), last_result(1), sign(1), n(0), cache_offset(-cache_size), scale(0), pol(pol_)
        {
           bessel_i_cache[cache_size - 1] = half_z > tools::log_max_value<T>() ?
              cyl_bessel_i_large_x_scaled(T(b_minus_a - 1.5f), half_z, scale, pol) : boost::math::cyl_bessel_i(b_minus_a - 1.5f, half_z, pol);
           refill_cache();
        }
        T operator()()
        {
           BOOST_MATH_STD_USING
           if(n - cache_offset >= cache_size)
              refill_cache();

           T result = term * (b_minus_a - 0.5f + n) * sign * bessel_i_cache[n - cache_offset];
           ++n;
           term *= poch_1;
           poch_1 = (n == 1) ? T(2 * b_minus_a) : T(poch_1 + 1);
           term *= poch_2;
           poch_2 += 1;
           term /= n;
           term /= b_poch;
           b_poch += 1;
           sign = -sign;

           if ((fabs(result) > fabs(last_result)) && (n > 100))
              return 0;  // series has gone divergent!

           last_result = result;

           return result;
        }

        int scaling()const
        {
           return scale;
        }

     private:
        T b_minus_a, half_z, poch_1, poch_2, b_poch, term, last_result;
        int sign;
        int n, cache_offset, scale;
        const Policy& pol;
        std::array<T, cache_size> bessel_i_cache;

        void refill_cache()
        {
           BOOST_MATH_STD_USING
           //
           // We don't calculate a new bessel I value: instead start our iterator off
           // with an arbitrary small value, then when we get back to the last value in the previous cache
           // calculate the ratio and use it to renormalise all the values.  This is more efficient, but
           // also avoids problems with I_v(x) underflowing to zero.
           //
           cache_offset += cache_size;
           T last_value = bessel_i_cache.back();
           bessel_i_backwards_iterator<T> i(b_minus_a + cache_offset + (int)cache_size - 1.5f, half_z, tools::min_value<T>() * (fabs(last_value) > 1 ? last_value : 1) / tools::epsilon<T>());

           for (int j = cache_size - 1; j >= 0; --j, ++i)
           {
              bessel_i_cache[j] = *i;
              //
              // Depending on the value of half_z, the values stored in the cache can grow so
              // large as to overflow, if that looks likely then we need to rescale all the
              // existing terms (most of which will then underflow to zero).  In this situation
              // it's likely that our series will only need 1 or 2 terms of the series but we
              // can't be sure of that:
              //
              if((j < cache_size - 2) && (bessel_i_cache[j + 1] != 0) && (tools::max_value<T>() / fabs(64 * bessel_i_cache[j] / bessel_i_cache[j + 1]) < fabs(bessel_i_cache[j])))
              {
                 T rescale = pow(fabs(bessel_i_cache[j] / bessel_i_cache[j + 1]), j + 1) * 2;
                 if (rescale > tools::max_value<T>())
                    rescale = tools::max_value<T>();
                 for (int k = j; k < cache_size; ++k)
                    bessel_i_cache[k] /= rescale;
                 i = bessel_i_backwards_iterator<T>(b_minus_a -0.5f + cache_offset + j, half_z, bessel_i_cache[j + 1], bessel_i_cache[j]);
              }
           }
           T ratio = last_value / *i;
           for (auto j = bessel_i_cache.begin(); j != bessel_i_cache.end(); ++j)
              *j *= ratio;
        }

        hypergeometric_1F1_AS_13_3_6_series();
        hypergeometric_1F1_AS_13_3_6_series(const hypergeometric_1F1_AS_13_3_6_series&);
        hypergeometric_1F1_AS_13_3_6_series& operator=(const hypergeometric_1F1_AS_13_3_6_series&);
     };

     template <class T, class Policy>
     T hypergeometric_1F1_AS_13_3_6(const T& a, const T& b, const T& z, const T& b_minus_a, const Policy& pol, int& log_scaling)
     {
        BOOST_MATH_STD_USING
        if(b_minus_a == 0)
        {
           // special case: M(a,a,z) == exp(z)
           int scale = itrunc(z, pol);
           log_scaling += scale;
           return exp(z - scale);
        }
        hypergeometric_1F1_AS_13_3_6_series<T, Policy> s(a, b, z, b_minus_a, pol);
        boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
        T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
        boost::math::policies::check_series_iterations<T>("boost::math::hypergeometric_1F1_AS_13_3_6<%1%>(%1%,%1%,%1%)", max_iter, pol);
        result *= boost::math::tgamma(b_minus_a - 0.5f) * pow(z / 4, -b_minus_a + 0.5f);
        int scale = itrunc(z / 2);
        log_scaling += scale;
        log_scaling += s.scaling();
        result *= exp(z / 2 - scale);
        return result;
     }

     /****************************************************************************************************************/
     //
     // The following are not used at present and are commented out for that reason:
     //
     /****************************************************************************************************************/

#if 0

     template <class T, class Policy>
     struct hypergeometric_1F1_AS_13_3_8_series
     {
        //
        // TODO: store and cache Bessel function evaluations via backwards recurrence.
        //
        // The C term grows by at least an order of magnitude with each iteration, and
        // rate of growth is largely independent of the arguments.  Free parameter h
        // seems to give accurate results for small values (almost zero) or h=1.
        // Convergence and accuracy, only when -a/z > 100, this appears to have no
        // or little benefit over 13.3.7 as it generally requires more iterations?
        //
        hypergeometric_1F1_AS_13_3_8_series(const T& a, const T& b, const T& z, const T& h, const Policy& pol_)
           : C_minus_2(1), C_minus_1(-b * h), C(b * (b + 1) * h * h / 2 - (2 * h - 1) * a / 2),
           bessel_arg(2 * sqrt(-a * z)), bessel_order(b - 1), power_term(std::pow(-a * z, (1 - b) / 2)), mult(z / std::sqrt(-a * z)),
           a_(a), b_(b), z_(z), h_(h), n(2), pol(pol_)
        {
        }
        T operator()()
        {
           // we actually return the n-2 term:
           T result = C_minus_2 * power_term * boost::math::cyl_bessel_j(bessel_order, bessel_arg, pol);
           bessel_order += 1;
           power_term *= mult;
           ++n;
           T C_next = ((1 - 2 * h_) * (n - 1) - b_ * h_) * C
              + ((1 - 2 * h_) * a_ - h_ * (h_ - 1) *(b_ + n - 2)) * C_minus_1
              - h_ * (h_ - 1) * a_ * C_minus_2;
           C_next /= n;
           C_minus_2 = C_minus_1;
           C_minus_1 = C;
           C = C_next;
           return result;
        }
        T C, C_minus_1, C_minus_2, bessel_arg, bessel_order, power_term, mult, a_, b_, z_, h_;
        const Policy& pol;
        int n;

        typedef T result_type;
     };

     template <class T, class Policy>
     T hypergeometric_1F1_AS_13_3_8(const T& a, const T& b, const T& z, const T& h, const Policy& pol)
     {
        BOOST_MATH_STD_USING
        T prefix = exp(h * z) * boost::math::tgamma(b);
        hypergeometric_1F1_AS_13_3_8_series<T, Policy> s(a, b, z, h, pol);
        boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
        T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
        boost::math::policies::check_series_iterations<T>("boost::math::hypergeometric_1F1_AS_13_3_8<%1%>(%1%,%1%,%1%)", max_iter, pol);
        result *= prefix;
        return result;
     }

     //
     // This is the series from https://dlmf.nist.gov/13.11
     // It appears to be unusable for a,z < 0, and for
     // b < 0 appears to never be better than the defining series
     // for 1F1.
     //
     template <class T, class Policy>
     struct hypergeometric_1f1_13_11_1_series
     {
        typedef T result_type;

        hypergeometric_1f1_13_11_1_series(const T& a, const T& b, const T& z, const Policy& pol_)
           : term(1), two_a_minus_1_plus_s(2 * a - 1), two_a_minus_b_plus_s(2 * a - b), b_plus_s(b), a_minus_half_plus_s(a - 0.5f), half_z(z / 2), s(0), pol(pol_)
        {
        }
        T operator()()
        {
           T result = term * a_minus_half_plus_s * boost::math::cyl_bessel_i(a_minus_half_plus_s, half_z, pol);

           term *= two_a_minus_1_plus_s * two_a_minus_b_plus_s / (b_plus_s * ++s);
           two_a_minus_1_plus_s += 1;
           a_minus_half_plus_s += 1;
           two_a_minus_b_plus_s += 1;
           b_plus_s += 1;

           return result;
        }
        T term, two_a_minus_1_plus_s, two_a_minus_b_plus_s, b_plus_s, a_minus_half_plus_s, half_z;
        int s;
        const Policy& pol;
     };

     template <class T, class Policy>
     T hypergeometric_1f1_13_11_1(const T& a, const T& b, const T& z, const Policy& pol, int& log_scaling)
     {
        BOOST_MATH_STD_USING
           bool use_logs = false;
        T prefix;
        int prefix_sgn = 1;
        if (true/*(a < boost::math::max_factorial<T>::value) && (a > 0)*/)
           prefix = boost::math::tgamma(a - 0.5f, pol);
        else
        {
           prefix = boost::math::lgamma(a - 0.5f, &prefix_sgn, pol);
           use_logs = true;
        }
        if (use_logs)
        {
           prefix += z / 2;
           prefix += log(z / 4) * (0.5f - a);
        }
        else if (z > 0)
        {
           prefix *= pow(z / 4, 0.5f - a);
           prefix *= exp(z / 2);
        }
        else
        {
           prefix *= exp(z / 2);
           prefix *= pow(z / 4, 0.5f - a);
        }

        hypergeometric_1f1_13_11_1_series<T, Policy> s(a, b, z, pol);
        boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
        T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
        boost::math::policies::check_series_iterations<T>("boost::math::hypergeometric_1f1_13_11_1<%1%>(%1%,%1%,%1%)", max_iter, pol);
        if (use_logs)
        {
           int scaling = itrunc(prefix);
           log_scaling += scaling;
           prefix -= scaling;
           result *= exp(prefix) * prefix_sgn;
        }
        else
           result *= prefix;

        return result;
     }

#endif

  } } } // namespaces

#endif // BOOST_MATH_HYPERGEOMETRIC_1F1_BESSEL_HPP


//###
#if 0

#include <iostream>

#include <rafl/base/ProbabilityMassFunction.h>
using namespace rafl;

int main()
{
  Histogram<int> hist;
  for(int i=0; i<5; ++i) hist.add(23);
  for(int i=0; i<5; ++i) hist.add(9);
  for(int i=0; i<5; ++i) hist.add(84);
  for(int i=0; i<500; ++i) hist.add(24);
  ProbabilityMassFunction<int> pmf(hist);
  const std::map<int,float>& masses = pmf.get_masses();
  /*for(std::map<int,float>::const_iterator it = masses.begin(), iend = masses.end(); it != iend; ++it)
  {
    std::cout << it->first << ' ' << it->second << '\n';
  }*/
  //std::cout << pmf << '\n';
  std::cout << pmf << '\n';
  std::cout << "Entropy=" << pmf.calculate_entropy() << '\n';
  return 0;
}

#endif

//###
#if 1

#include <Eigen/Dense>

#include <rafl/core/RandomForest.h>
#include <rafl/decisionfunctions/FeatureThresholdingDecisionFunctionGenerator.h>
using namespace rafl;

typedef int Label;
typedef boost::shared_ptr<const Example<Label> > Example_CPtr;
typedef DecisionTree<Label> DT;
typedef RandomForest<Label> RF;
typedef boost::shared_ptr<DT::Settings> Settings_Ptr;

Descriptor_CPtr make_descriptor(float *arr)
{
  Descriptor_Ptr d(new Descriptor(10));
  for(int i = 0; i < 10; ++i)
  {
    (*d)[i] = arr[i];
  }
  return d;
}

int main(int argc, char *argv[])
{
  if(argc != 3)
  {
    std::cout << "Silly boy - enter the path of the training set, followed by the path of the test set!\n";
  }
  else
  {
    std::string trainingSetPath = argv[1];
    std::string testingSetPath = argv[2];

    std::cout << "Training set: " << trainingSetPath << "\n";
    std::cout << "Testing set: " << testingSetPath << "\n";

    // Register the relevant decision function generators with the factory.
    DecisionFunctionGeneratorFactory<Label>::instance().register_rafl_makers();

    // Construct the random forest.
    Settings_Ptr settings;
    try
    {
      settings.reset(new DT::Settings("settings.txt"));
    }
    catch(std::exception&)
    {
      // If the settings file can't be found or is invalid, use default settings.
      unsigned int seed = 12345;
      tvgutil::RandomNumberGenerator_Ptr randomNumberGenerator(new tvgutil::RandomNumberGenerator(seed));
      DT::DecisionFunctionGenerator_CPtr decisionFunctionGenerator(new FeatureThresholdingDecisionFunctionGenerator<Label>);

      settings.reset(new DT::Settings);
      settings->candidateCount = 256;
      settings->decisionFunctionGenerator = decisionFunctionGenerator;
      settings->gainThreshold = 0.0f;
      settings->maxClassSize = 10000;
      settings->maxTreeHeight = 15;
      settings->randomNumberGenerator = randomNumberGenerator;
      settings->seenExamplesThreshold = 30;
      settings->splittabilityThreshold = 0.5f;
    }

    const size_t treeCount = 8;
    RF rf(treeCount, *settings);

    // Train the random forest.
    std::cout << "Training the random forest..\n";
    std::vector<Example_CPtr> trainingExamples = ExampleUtil::load_examples<Label>(trainingSetPath);
    std::set<Label> classLabels;
    for(size_t i = 0, iend = trainingExamples.size(); i < iend; ++i)
    {
      classLabels.insert( trainingExamples.at(i)->get_label() );
    }

    rf.add_examples(trainingExamples);
    const size_t splitBudget = 32768;
    rf.train(splitBudget);
    rf.output(std::cout);

    // Test the random forest and output the results.
    std::cout << "Testing the random forest..\n";
    std::vector<Example_CPtr> testingExamples = ExampleUtil::load_examples<Label>(testingSetPath);
    std::vector<Label> expectedLabels, predictedLabels;

    float totalTests = static_cast<float>(testingExamples.size());
    size_t correctTests = 0, wrongTests = 0;
    for(std::vector<Example_CPtr>::const_iterator it = testingExamples.begin(), iend = testingExamples.end(); it != iend; ++it)
    {
      const Descriptor_CPtr& descriptor = (*it)->get_descriptor();
      const Label& expectedLabel = (*it)->get_label();
      Label predictedLabel = rf.predict(descriptor);
      expectedLabels.push_back(expectedLabel);
      predictedLabels.push_back(predictedLabel);
      if(predictedLabel == expectedLabel) ++correctTests;
      else ++wrongTests;
    }

    std::cout << "Correct %: " << correctTests / totalTests << '\n';
    std::cout << "Wrong %: " << wrongTests / totalTests << '\n';
  }
  return 0;
}

#endif


#ifndef INCLUDED_scheme_nest_maps_SphereDodec_HH
#define INCLUDED_scheme_nest_maps_SphereDodec_HH

#include <stdint.h>
#include <Eigen/Dense>
#include <boost/static_assert.hpp>
#include <iostream>
#include <vector>

namespace scheme {
namespace nest {
namespace pmap {

	using std::cout;
	using std::endl;

	using namespace Eigen;

	namespace dodec_data {
		double * get_dodec_cell_centers(){
			static double p = (1.0+sqrt(5.0)) / 2.0;
			static double n = sqrt(1.0+p*p);
			static double dodec_cell_centers[12*3] = {
				+1/n,+p/n, 0/n,
				+1/n,-p/n, 0/n,
				-1/n,+p/n, 0/n,
				-1/n,-p/n, 0/n,
				+p/n, 0/n,+1/n,
				-p/n, 0/n,+1/n,
				+p/n, 0/n,-1/n,
				-p/n, 0/n,-1/n,
				 0/n,+1/n,+p/n,
				 0/n,+1/n,-p/n,
				 0/n,-1/n,+p/n,
				 0/n,-1/n,-p/n,
			};
			return dodec_cell_centers;
		}
	}

	template<
		int DIM,
		class Value=Eigen::Matrix<double,3,1>,
		class Index=uint64_t,
		class Float=double
	>
	struct SphereDodec {
		BOOST_STATIC_ASSERT_MSG(DIM==2,"SphereDodec DIM must be == 2");

		static int const DIMENSION = DIM;
		typedef Value ValueType ;
		typedef Float FloatType ;
		typedef Index IndexType ;
		typedef Eigen::Array<Index,DIM,1> Indices;
		typedef Eigen::Array<Float,DIM,1> Params;

		///@brief sets value to parameters without change
		///@return false iff invalid parameters
		bool params_to_value(
			Params const & params,
			Index cell_index,
			Index resl,
			Value & value
		) const {
			Map< Matrix<double,3,12> > const cell_centers( dodec_data::get_dodec_cell_centers() );
			std::cout << cell_centers << std::endl;
			for(int i = 0; i < 12; ++i){
				int minj = -1;
				double mind = 9e9;
				for(int j = 0; j < 12; ++j){
					if(i==j)continue;
					double d = (cell_centers.col(i)-cell_centers.col(j)).norm();
					if( d < mind ) {
						mind = d;
						minj = j;
					}

				}
				Matrix<double,3,3> m;
				m.col(0) = cell_centers.col(i);
				m.col(1) = cell_centers.col(minj);
				m.col(2) = m.col(1).cross(m.col(2));
				HouseholderQR<Matrix3d> qr(m);
				m = qr.householderQ();
				// cout << m << endl << endl;
				// cout << (m * Vector3d(1,0,0)).transpose() << " " << cell_centers.col(i).transpose() << endl;
				// cout << (m * Vector3d(0,1,0)).transpose() << " " << cell_centers.col(minj).transpose() << endl;
				// cout << endl;
				// cout << i << " " << minj << " " << mind << " " << endl;
			}
			return false;
		}

		///@brief sets params/cell_index from value
		///@note necessary for value lookup and neighbor lookup
		bool value_to_params(
			Value const & value,
			Index resl,
			Params & params,
			Index & cell_index
		) const ;

		///@brief get parameter space repr of Value for particular cell
		///@note necessary only for neighbor lookup
		void value_to_params_for_cell(
			Value const & value,
			Index resl,
			Params & params
		) const ;

		///@brief return the cell_index of neighboring cells within radius of value
		///@note delta parameter is in "Parameter Space"
		template<class OutIter>
		void get_neighboring_cells(Value const & value, Float radius, OutIter out) const;

		///@brief aka covering radius max distance from bin center to any value within bin
		Float bin_circumradius(Index resl) const ;

		///@brief maximum distance from the bin center which must be within the bin
		Float bin_inradius(Index resl) const ;

		///@brief cell size
		Index num_cells() const ;
	};

}
}
}

#endif


// Copyright (c) Facebook, Inc. and its affiliates.

// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory of this source tree.
#include <fstream>
#include <iostream>
#include <string>

#include "dtt.h"
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <torch/script.h>
#include <torch/torch.h>

#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/parsers/urdf.hpp"

#include "polymetis/torchscript_operators/rotations.hpp"

torch::Tensor validTensor(torch::Tensor x) {
  if (x.dim() < 2) {
    x = x.unsqueeze(1);
  }
  return x.to(torch::kDouble);
}

Eigen::VectorXd matrixToVector(Eigen::MatrixXd A) {
  return Eigen::VectorXd(
      Eigen::Map<Eigen::VectorXd>(A.data(), A.cols() * A.rows()));
}

struct RobotModelPinocchio : torch::CustomClassHolder {
  pinocchio::Model model_;
  pinocchio::Data model_data_;
  pinocchio::FrameIndex ee_frame_idx_;
  Eigen::VectorXd ik_sol_p_;
  Eigen::VectorXd ik_sol_v_;
  pinocchio::Data::Matrix6x ik_sol_J_;

  std::string xml_buffer_;
  std::string ee_link_name_;

  RobotModelPinocchio(std::string urdf_filename, std::string ee_link_name) {
    ee_link_name_ = ee_link_name;

    std::ifstream stream(urdf_filename);
    xml_buffer_ = std::string((std::istreambuf_iterator<char>(stream)),
                              std::istreambuf_iterator<char>());

    initialize();
  }

  RobotModelPinocchio(std::vector<std::string> serialized_state) {
    ee_link_name_ = serialized_state[0];
    xml_buffer_ = serialized_state[1];
    initialize();
  }

  void initialize() {
    pinocchio::urdf::buildModelFromXML(xml_buffer_, model_);
    model_data_ = pinocchio::Data(model_);
    ee_frame_idx_ = model_.getBodyId(ee_link_name_);

    ik_sol_p_ = Eigen::VectorXd(model_.nq);
    ik_sol_v_ = Eigen::VectorXd(model_.nv);
    ik_sol_J_ = pinocchio::Data::Matrix6x(6, model_.nv);
  }

  c10::List<torch::Tensor> get_joint_angle_limits(void) {
    c10::List<torch::Tensor> result;
    torch::Tensor l_result = torch::zeros(model_.nq, torch::kFloat32);
    torch::Tensor u_result = torch::zeros(model_.nq, torch::kFloat32);

    for (int i = 0; i < model_.nq; i++) {
      l_result[i] = model_.lowerPositionLimit[i];
      u_result[i] = model_.upperPositionLimit[i];
    }
    result.push_back(l_result);
    result.push_back(u_result);

    return result;
  }

  torch::Tensor get_joint_velocity_limits(void) {
    torch::Tensor result = torch::zeros(model_.nq, torch::kFloat32);

    for (int i = 0; i < model_.nq; i++) {
      result[i] = model_.velocityLimit[i];
    }

    return result;
  }

  c10::List<torch::Tensor> forward_kinematics(torch::Tensor joint_positions) {
    c10::List<torch::Tensor> result;
    torch::Tensor pos_result = torch::zeros(3, torch::kFloat32);
    torch::Tensor quat_result = torch::zeros(4, torch::kFloat32);

    joint_positions = validTensor(joint_positions);
    pinocchio::forwardKinematics(
        model_, model_data_,
        matrixToVector(dtt::libtorch2eigen<double>(joint_positions)));
    pinocchio::updateFramePlacement(model_, model_data_, ee_frame_idx_);

    auto pos_data = model_data_.oMf[ee_frame_idx_].translation().transpose();
    auto quat_data =
        Eigen::Quaterniond(model_data_.oMf[ee_frame_idx_].rotation());

    for (int i = 0; i < 3; i++) {
      pos_result[i] = pos_data[i];
    }
    quat_result[0] = quat_data.x();
    quat_result[1] = quat_data.y();
    quat_result[2] = quat_data.z();
    quat_result[3] = quat_data.w();

    result.push_back(pos_result);
    result.push_back(quat_result);

    return result;
  }

  torch::Tensor compute_jacobian(torch::Tensor joint_positions) {
    joint_positions = validTensor(joint_positions);

    torch::Tensor result = torch::zeros({6, model_.nq}, torch::kFloat64);
    Eigen::Map<dtt::MatrixXrm<double>> J(result.data_ptr<double>(),
                                         result.size(0), result.size(1));
    pinocchio::computeFrameJacobian(
        model_, model_data_,
        matrixToVector(dtt::libtorch2eigen<double>(joint_positions)),
        ee_frame_idx_, pinocchio::LOCAL_WORLD_ALIGNED, J);

    return result;
  }

  torch::Tensor inverse_dynamics(torch::Tensor joint_positions,
                                 torch::Tensor joint_velocities,
                                 torch::Tensor joint_accelerations) {
    joint_positions = validTensor(joint_positions);
    joint_velocities = validTensor(joint_velocities);
    joint_accelerations = validTensor(joint_accelerations);
    auto q = matrixToVector(dtt::libtorch2eigen<double>(joint_positions));
    auto v = matrixToVector(dtt::libtorch2eigen<double>(joint_velocities));
    auto a = matrixToVector(dtt::libtorch2eigen<double>(joint_accelerations));

    Eigen::Matrix<double, Eigen::Dynamic, 1> tau =
        pinocchio::rnea(model_, model_data_, q, v, a);
    std::vector<int64_t> dims = {tau.rows()};
    return torch::from_blob(tau.data(), dims, torch::kFloat64).clone();
  }

  torch::Tensor inverse_kinematics(torch::Tensor ee_pos, torch::Tensor ee_quat,
                                   torch::Tensor rest_pose, double eps = 1e-4,
                                   int64_t max_iters = 1000, double dt = 0.1,
                                   double damping = 1e-12) {
    ee_pos = validTensor(ee_pos);
    Eigen::Vector3d ee_pos_(
        Eigen::Map<Eigen::Vector3d>(ee_pos.data_ptr<double>(), 3));

    auto quat = tensor4ToQuat(ee_quat);
    auto ee_quat_ = Eigen::Quaterniond(quat).cast<double>();
    auto ee_orient_ = ee_quat_.toRotationMatrix();

    // Initialize IK variables
    const pinocchio::SE3 desired_ee(ee_orient_, ee_pos_);
    rest_pose = validTensor(rest_pose);
    ik_sol_p_ = matrixToVector(dtt::libtorch2eigen<double>(rest_pose));

    ik_sol_J_.setZero();

    Eigen::Matrix<double, 6, 1> err;
    ik_sol_v_ = Eigen::VectorXd(model_.nv);
    ik_sol_v_.setZero();

    // Reset robot pose
    pinocchio::forwardKinematics(model_, model_data_, ik_sol_p_);
    pinocchio::updateFramePlacement(model_, model_data_, ee_frame_idx_);

    // Solve IK iteratively
    for (int i = 0; i < max_iters; i++) {
      // Compute forward kinematics error
      pinocchio::forwardKinematics(model_, model_data_, ik_sol_p_);
      pinocchio::updateFramePlacement(model_, model_data_, ee_frame_idx_);
      const pinocchio::SE3 dMf =
          desired_ee.actInv(model_data_.oMf[ee_frame_idx_]);
      err = pinocchio::log6(dMf).toVector();

      // Check termination
      if (err.norm() < eps) {
        std::cout << "Ending IK at " << i + 1 << "/" << max_iters
                  << " iteration." << std::endl;
        break;
      }

      // Descent solution
      pinocchio::computeFrameJacobian(model_, model_data_, ik_sol_p_,
                                      ee_frame_idx_, pinocchio::LOCAL,
                                      ik_sol_J_);

      pinocchio::Data::Matrix6 JJt;
      JJt.noalias() = ik_sol_J_ * ik_sol_J_.transpose();
      JJt.diagonal().array() += damping;
      ik_sol_v_.noalias() = -ik_sol_J_.transpose() * JJt.ldlt().solve(err);
      ik_sol_p_ = pinocchio::integrate(model_, ik_sol_p_, ik_sol_v_ * dt);
    }

    if (err.norm() >= eps) {
      std::cerr << "WARNING: IK did not converge!" << std::endl;
    }

    std::vector<int64_t> dims = {ik_sol_p_.rows()};
    return torch::from_blob(ik_sol_p_.data(), dims, torch::kFloat64).clone();
  }
};

TORCH_LIBRARY(torchscript_pinocchio, m) {
  m.class_<RobotModelPinocchio>("RobotModelPinocchio")
      .def(torch::init<std::string, std::string>())
      .def("get_joint_angle_limits",
           &RobotModelPinocchio::get_joint_angle_limits)
      .def("get_joint_velocity_limits",
           &RobotModelPinocchio::get_joint_velocity_limits)
      .def("forward_kinematics", &RobotModelPinocchio::forward_kinematics)
      .def("compute_jacobian", &RobotModelPinocchio::compute_jacobian)
      .def("inverse_dynamics", &RobotModelPinocchio::inverse_dynamics)
      .def("inverse_kinematics", &RobotModelPinocchio::inverse_kinematics)
      .def_pickle(
          // __getstate__
          [](const c10::intrusive_ptr<RobotModelPinocchio> &self)
              -> std::vector<std::string> {
            return std::vector<std::string>{self->ee_link_name_,
                                            self->xml_buffer_};
          },
          // __setstate__
          [](std::vector<std::string> state)
              -> c10::intrusive_ptr<RobotModelPinocchio> {
            return c10::make_intrusive<RobotModelPinocchio>(std::move(state));
          });
}


#pragma once

#include <polyfem/Common.hpp>
#include <polyfem/ElementAssemblyValues.hpp>
#include <polyfem/ElementBases.hpp>
#include <polyfem/AutodiffTypes.hpp>
#include <polyfem/Types.hpp>

#include <Eigen/Dense>
#include <tinyexpr.h>
#include <vector>
#include <array>
#include <functional>

namespace polyfem
{
	constexpr int SMALL_N = POLYFEM_SMALL_N;
	constexpr int BIG_N = POLYFEM_BIG_N;

	Eigen::VectorXd gradient_from_energy(const int size, const int n_bases, const ElementAssemblyValues &vals, const Eigen::MatrixXd &displacement, const QuadratureVector &da,
										 const std::function<DScalar1<double, Eigen::Matrix<double, 6, 1>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &fun6,
										 const std::function<DScalar1<double, Eigen::Matrix<double, 8, 1>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &fun8,
										 const std::function<DScalar1<double, Eigen::Matrix<double, 12, 1>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &fun12,
										 const std::function<DScalar1<double, Eigen::Matrix<double, 18, 1>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &fun18,
										 const std::function<DScalar1<double, Eigen::Matrix<double, 24, 1>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &fun24,
										 const std::function<DScalar1<double, Eigen::Matrix<double, 30, 1>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &fun30,
										 const std::function<DScalar1<double, Eigen::Matrix<double, 60, 1>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &fun60,
										 const std::function<DScalar1<double, Eigen::Matrix<double, 81, 1>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &fun81,
										 const std::function<DScalar1<double, Eigen::Matrix<double, Eigen::Dynamic, 1, 0, SMALL_N, 1>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &funN,
										 const std::function<DScalar1<double, Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 1000, 1>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &funBigN,
										 const std::function<DScalar1<double, Eigen::VectorXd>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &funn);

	Eigen::MatrixXd hessian_from_energy(const int size, const int n_bases, const ElementAssemblyValues &vals, const Eigen::MatrixXd &displacement, const QuadratureVector &da,
										const std::function<DScalar2<double, Eigen::Matrix<double, 6, 1>, Eigen::Matrix<double, 6, 6>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &fun6,
										const std::function<DScalar2<double, Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &fun8,
										const std::function<DScalar2<double, Eigen::Matrix<double, 12, 1>, Eigen::Matrix<double, 12, 12>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &fun12,
										const std::function<DScalar2<double, Eigen::Matrix<double, 18, 1>, Eigen::Matrix<double, 18, 18>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &fun18,
										const std::function<DScalar2<double, Eigen::Matrix<double, 24, 1>, Eigen::Matrix<double, 24, 24>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &fun24,
										const std::function<DScalar2<double, Eigen::Matrix<double, 30, 1>, Eigen::Matrix<double, 30, 30>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &fun30,
										const std::function<DScalar2<double, Eigen::Matrix<double, 60, 1>, Eigen::Matrix<double, 60, 60>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &fun60,
										const std::function<DScalar2<double, Eigen::Matrix<double, 81, 1>, Eigen::Matrix<double, 81, 81>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &fun81,
										const std::function<DScalar2<double, Eigen::Matrix<double, Eigen::Dynamic, 1, 0, SMALL_N, 1>, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, 0, SMALL_N, SMALL_N>>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &funN,
										const std::function<DScalar2<double, Eigen::VectorXd, Eigen::MatrixXd>(const ElementAssemblyValues &, const Eigen::MatrixXd &, const QuadratureVector &)> &funn);

	double von_mises_stress_for_stress_tensor(const Eigen::MatrixXd &stress);
	void compute_diplacement_grad(const int size, const ElementBases &bs, const ElementAssemblyValues &vals, const Eigen::MatrixXd &local_pts, const int p, const Eigen::MatrixXd &displacement, Eigen::MatrixXd &displacement_grad);

	double convert_to_lambda(const bool is_volume, const double E, const double nu);
	double convert_to_mu(const double E, const double nu);

	class ElasticityTensor
	{
	public:
		void resize(const int size);

		double operator()(int i, int j) const;
		double &operator()(int i, int j);

		void set_from_entries(const std::vector<double> &entries);
		void set_from_lambda_mu(const double lambda, const double mu);
		void set_from_young_poisson(const double young, const double poisson);

		void set_orthotropic(
			double Ex, double Ey, double Ez,
			double nuYX, double nuZX, double nuZY,
			double muYZ, double muZX, double muXY);
		void set_orthotropic(double Ex, double Ey, double nuYX, double muXY);

		template <int DIM>
		double compute_stress(const std::array<double, DIM> &strain, const int j) const;

	private:
		Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, 0, 6, 6> stifness_tensor_;
		int size_;
	};

	class LameParameters
	{
	public:
		LameParameters();
		~LameParameters();

		void init(const json &params);
		void init_multimaterial(const bool is_volume, const Eigen::MatrixXd &Es, const Eigen::MatrixXd &nus);

		void lambda_mu(double x, double y, double z, int el_id, double &lambda, double &mu) const;

	private:
		struct Internal
		{
			double x, y, z;
		};
		void set_e_nu(const json &E, const json &nu);

		int size_;
		double lambda_ = 1, mu_ = 1;
		Eigen::MatrixXd lambda_mat_, mu_mat_;

		te_expr *lambda_expr_, *mu_expr_;
		Internal *vals_;
		bool is_lambda_mu_;
		bool initialized_;
	};

	class Density
	{
	public:
		Density();
		~Density();

		void init(const json &params);
		void init_multimaterial(const Eigen::MatrixXd &rho);

		double operator()(double x, double y, double z, int el_id) const;

	private:
		void set_rho(const json &rho);

		struct Internal
		{
			double x, y, z;
		};

		double rho_ = 1;
		Eigen::MatrixXd rho_mat_;

		te_expr *rho_expr_;
		Internal *vals_;
		bool initialized_;
	};
} // namespace polyfem


#include <boost/math/octonion.hpp>