/*
 * This file is part of Kintinuous.
 *
 * Copyright (C) 2015 The National University of Ireland Maynooth and
 * Massachusetts Institute of Technology
 *
 * The use of the code within this file and all code within files that
 * make up the software that is Kintinuous is permitted for
 * non-commercial purposes only.  The full terms and conditions that
 * apply to the code within this file are detailed within the LICENSE.txt
 * file and at <http://www.cs.nuim.ie/research/vision/data/kintinuous/code.php>
 * unless explicitly stated.  By downloading this file you agree to
 * comply with these terms.
 *
 * If you wish to use any of this code for commercial purposes then
 * please email commercialisation@nuim.ie.
 *
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2011, Willow Garage, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. 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 <fstream>
#include <iostream>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <opencv2/opencv.hpp>
#include <boost/filesystem.hpp>

#include "../utils/ConfigArgs.h"
#include "KintinuousTracker.h"
#include "Volume.h"

KintinuousTracker::KintinuousTracker(cv::Mat * depthIntrinsics)
 : tsdfAvailable(false),
   imageAvailable(false),
   cycledMutex(false),
   global_time_(0),
   place_recognition_movement(0.15),
   cloud_device_ (Resolution::get().numPixels() * 3),
   liveTsdf(0),
   liveImage(0),
   cycled(false),
   overlap(0),
   parked(false),
   icp(0),
   rgbd(0),
   groundTruth(0)
{
    intr.fx = depthIntrinsics->at<double>(0, 0);
    intr.fy = depthIntrinsics->at<double>(1, 1);
    intr.cx = depthIntrinsics->at<double>(0, 2);
    intr.cy = depthIntrinsics->at<double>(1, 2);

    const Eigen::Vector3f volume_size = Eigen::Vector3f::Constant(Volume::get().getVolumeSize());
    const Eigen::Vector3i volume_resolution(VOLUME_X, VOLUME_Y, VOLUME_Z);

    tsdf_volume_ = new TsdfVolume(volume_resolution);

    tsdf_volume_->setSize(volume_size);

    color_volume_ = new ColorVolume(*tsdf_volume_);

    initialRotation = Eigen::Matrix3f::Identity();

    if(ConfigArgs::get().staticMode || ConfigArgs::get().dynamicCube)
    {
        volumeBasis = volume_size * 0.5f - Eigen::Vector3f(0, 0, (volume_size(2) * 0.5) + (ConfigArgs::get().staticMode ? 0.45 : 0));
    }
    else
    {
        volumeBasis = volume_size * 0.5f;
    }

    const float default_tranc_dist = std::max(0.01f, volume_size(0) / 100.0f);
    tsdf_volume_->setTsdfTruncDist (default_tranc_dist);

    allocateBuffers();

    rmats_.reserve(30000);
    tvecs_.reserve(30000);

    voxelWrap.x = 0;
    voxelWrap.y = 0;
    voxelWrap.z = 0;

    init_utime.assignValue(std::numeric_limits<unsigned long long>::max());
    firstDepthData.assignValue(0);
    firstRgbImage.assignValue(0);

    if(ConfigArgs::get().trajectoryFile.size())
    {
        groundTruth = new GroundTruthOdometry(tvecs_,
                                              rmats_,
                                              camera_trajectory,
                                              current_utime);

        odometryProvider = dynamic_cast<OdometryProvider *>(groundTruth);

        lastOdometry = CloudSlice::GROUNDTRUTH;

        std::cout << "Using ground truth odometry" << std::endl;
    }
    else if(ConfigArgs::get().useRGBD || ConfigArgs::get().useRGBDICP)
    {
        rgbd = new RGBDOdometry(tvecs_,
                                rmats_,
                                vmaps_g_prev_,
                                nmaps_g_prev_,
                                vmaps_curr_,
                                nmaps_curr_,
                                intr);

        odometryProvider = dynamic_cast<OdometryProvider *>(rgbd);

        lastOdometry = CloudSlice::RGBD;

        if(ConfigArgs::get().useRGBDICP)
        {
            std::cout << "Using ICP+RGB-D odometry" << std::endl;
        }
        else
        {
            std::cout << "Using RGB-D odometry" << std::endl;
        }
    }
    else
    {
        icp = new ICPOdometry(tvecs_,
                              rmats_,
                              vmaps_g_prev_,
                              nmaps_g_prev_,
                              vmaps_curr_,
                              nmaps_curr_,
                              intr);

        odometryProvider = dynamic_cast<OdometryProvider *>(icp);

        lastOdometry = CloudSlice::ICP;

        std::cout << "Using ICP odometry" << std::endl;
    }

    reset();
}

KintinuousTracker::~KintinuousTracker()
{
    if(icp)
        delete icp;

    if(rgbd)
        delete rgbd;

    if(groundTruth)
        delete groundTruth;

    delete color_volume_;
    delete tsdf_volume_;
}

void KintinuousTracker::outputPose(uint64_t & timestamp, Eigen::Matrix<float, 3, 3, Eigen::RowMajor> & Rcurr)
{
    std::string filename = ConfigArgs::get().saveFile;
    filename.append(".poses");

    std::ofstream file;
    file.open(filename.c_str(), std::fstream::app);

    std::stringstream strs;

    strs << std::setprecision(6) << std::fixed << (double)timestamp / 1000000.0 << " ";

    file << strs.str() << currentGlobalCamera(0) << " " << currentGlobalCamera(1) << " " << currentGlobalCamera(2) << " ";

    Eigen::Quaternionf currentCameraRotation(Rcurr);

    file << currentCameraRotation.x() << " " << currentCameraRotation.y() << " " << currentCameraRotation.z() << " " << currentCameraRotation.w() << "\n";

    file.close();
}

void KintinuousTracker::loadTrajectory(const std::string & filename)
{
    std::ifstream file;
    std::string line;
    file.open(filename.c_str());

    double trajSum = 0.0;
    bool first = true;
    Eigen::Vector3f lastT;

    while (!file.eof())
    {
        long long unsigned int utime;
        float x, y, z, qx, qy, qz, qw;
        std::getline(file, line);
        int n = sscanf(line.c_str(), "%llu,%f,%f,%f,%f,%f,%f,%f", &utime, &x, &y, &z, &qx, &qy, &qz, &qw);

        if(file.eof())
            break;

        assert(n == 8);

        Eigen::Quaternionf q(qw, qx, qy, qz);
        Eigen::Vector3f t(x, y, z);

        if(!first)
        {
            trajSum += (t - lastT).norm();
        }

        first = false;
        lastT = t;

        Eigen::Isometry3f T;
        T.setIdentity();
        T.pretranslate(t).rotate(q);
        camera_trajectory[utime] = T;
    }
    std::cout << "Done loading ground truth, length: " << trajSum << std::endl;
    current_utime = 0;
}

void KintinuousTracker::reset()
{
    modelColor.create(Resolution::get().rows(), Resolution::get().cols());
    modelSurface.create(Resolution::get().rows(), Resolution::get().cols());
    modelDepth.create(Resolution::get().rows(), Resolution::get().cols());
    global_time_ = 0;
    rmats_.clear ();
    tvecs_.clear ();

    cycled = false;

    rmats_.push_back(initialRotation);
    tvecs_.push_back(volumeBasis);

    Eigen::Vector3f initialTrans = volumeBasis;
    initialTrans(0) -= Volume::get().getVolumeSize() * 0.5;
    initialTrans(1) -= Volume::get().getVolumeSize() * 0.5;
    initialTrans(2) -= Volume::get().getVolumeSize() * 0.5;

    const float3 & voxelSizeMeters = Volume::get().getVoxelSizeMeters();

    Eigen::Vector3f currentCameraTranslation = initialTrans;
    currentCameraTranslation(0) += voxelWrap.x * voxelSizeMeters.x;
    currentCameraTranslation(1) += voxelWrap.y * voxelSizeMeters.y;
    currentCameraTranslation(2) += voxelWrap.z * voxelSizeMeters.z;

    currentGlobalCamera = currentCameraTranslation;

    lastPlaceRecognitionTrans = currentGlobalCamera;
    lastPlaceRecognitionRot = initialRotation;

    for(int i = 0; i < PR_BUFFER_SIZE; i++)
    {
        placeRecognitionBuffer[i].dump();
    }

    placeRecognitionId.assignValue(0);

    densePoseGraph.clear();
    latestDensePoseId.assignValue(0);

    voxelWrap.x = 0;
    voxelWrap.y = 0;
    voxelWrap.z = 0;

    odometryProvider->reset();

    init_utime.assignValue(std::numeric_limits<unsigned long long>::max());

    unsigned short * firstDepth = firstDepthData.getValue();

    if(firstDepth)
    {
        delete [] firstDepth;
    }

    unsigned char * firstImg = firstRgbImage.getValue();

    if(firstImg)
    {
        delete [] firstImg;
    }

    firstDepthData.assignValue(0);
    firstRgbImage.assignValue(0);

    if(ConfigArgs::get().staticMode)
    {
        parked = true;
    }
    else
    {
        parked = false;
    }

    for(unsigned int i = 0; i < sharedCloudSlices.size(); i++)
    {
        delete sharedCloudSlices.at(i);
    }
    sharedCloudSlices.clear();

    tsdf_volume_->reset();
    color_volume_->reset();

    if(!ConfigArgs::get().onlineDeformation)
    {
        std::string filename = ConfigArgs::get().saveFile;
        filename.append(".poses");
        std::ofstream file;
        file.open(filename.c_str());
        file.close();
    }
}

void KintinuousTracker::allocateBuffers()
{
    depths_curr_.resize (ICPOdometry::LEVELS);

    vmaps_g_prev_.resize (ICPOdometry::LEVELS);
    nmaps_g_prev_.resize (ICPOdometry::LEVELS);

    vmaps_curr_.resize (ICPOdometry::LEVELS);
    nmaps_curr_.resize (ICPOdometry::LEVELS);

    for (int i = 0; i < ICPOdometry::LEVELS; ++i)
    {
        int pyr_rows = Resolution::get().rows() >> i;
        int pyr_cols = Resolution::get().cols() >> i;

        depths_curr_[i].create (pyr_rows, pyr_cols);

        vmaps_g_prev_[i].create (pyr_rows*3, pyr_cols);
        nmaps_g_prev_[i].create (pyr_rows*3, pyr_cols);

        vmaps_curr_[i].create (pyr_rows*3, pyr_cols);
        nmaps_curr_[i].create (pyr_rows*3, pyr_cols);
    }

    vmap_curr_color.create(Resolution::get().rows(), Resolution::get().cols());
    depthRawScaled_.create(Resolution::get().rows(), Resolution::get().cols());
}

void KintinuousTracker::repositionCube(Eigen::Matrix<float, 3, 3, Eigen::RowMajor> & currentRotation)
{
    Eigen::Vector3f rotations = rodrigues2(currentRotation);

    float yRot = rotations(1);

    const float PI = 3.14159265359f;

    float radius = Volume::get().getVolumeSize() * 0.5;

    Eigen::Vector3f newPosition = volumeBasis;

    newPosition(0) = radius * (cos(yRot + (PI / 2)) + 1.0f);
    newPosition(2) = radius * (sin(yRot - (PI / 2)) + 1.0f);

    Eigen::Vector3f currentTranslation = (tvecs_.at(tvecs_.size() - 1) - newPosition);

    Eigen::Vector3f voxelSize = tsdf_volume_->getVoxelSize();

    const int thresh = parked ? ConfigArgs::get().staticMode ? VOLUME_X * 3 : VOLUME_X : ConfigArgs::get().voxelShift;

    int xVoxelTrans = 0;
    int yVoxelTrans = 0;
    int zVoxelTrans = 0;

    if((int)std::floor(currentTranslation(0) / voxelSize(0)) < 0)
    {
        xVoxelTrans = std::max(-thresh, (int)std::floor(currentTranslation(0) / voxelSize(0)));
    }
    else
    {
        xVoxelTrans = std::min(thresh, (int)std::floor(currentTranslation(0) / voxelSize(0)));
    }

    if((int)std::floor(currentTranslation(1) / voxelSize(1)) < 0)
    {
        yVoxelTrans = std::max(-thresh, (int)std::floor(currentTranslation(1) / voxelSize(1)));
    }
    else
    {
        yVoxelTrans = std::min(thresh, (int)std::floor(currentTranslation(1) / voxelSize(1)));
    }

    if((int)std::floor(currentTranslation(2) / voxelSize(2)) < 0)
    {
        zVoxelTrans = std::max(-thresh, (int)std::floor(currentTranslation(2) / voxelSize(2)));
    }
    else
    {
        zVoxelTrans = std::min(thresh, (int)std::floor(currentTranslation(2) / voxelSize(2)));
    }

    if(xVoxelTrans >= thresh || xVoxelTrans <= -thresh ||
       yVoxelTrans >= thresh || yVoxelTrans <= -thresh ||
       zVoxelTrans >= thresh || zVoxelTrans <= -thresh)
    {
        volumeBasis = newPosition;
    }
}

void KintinuousTracker::processFrame(const DeviceArray2D<unsigned short>& depth_raw,
                                     const DeviceArray2D<PixelRGB> & colors,
                                     unsigned char * rgbImage,
                                     unsigned short * depthData,
                                     uint64_t timestamp,
                                     bool compression,
                                     uint8_t * lastCompressedDepth,
                                     int depthSize,
                                     uint8_t * lastCompressedImage,
                                     int imageSize)
{
    lagTime = Stopwatch::getCurrentSystemTime();

    lastDepthData = depthData;
    lastRgbImage = rgbImage;

    if(groundTruth &&!groundTruth->preRun(rgbImage, depthData, timestamp))
    {
        return;
    }

    if(icp || ConfigArgs::get().useRGBDICP || !ConfigArgs::get().disableColorAngleWeight)
    {
        bilateralFilter(depth_raw, depths_curr_[0]);

        for (int i = 1; i < ICPOdometry::LEVELS; ++i)
        {
            pyrDown(depths_curr_[i-1], depths_curr_[i]);
        }

        for (int i = 0; i < ICPOdometry::LEVELS; ++i)
        {
            createVMap(intr(i), depths_curr_[i], vmaps_curr_[i]);
            createNMap(vmaps_curr_[i], nmaps_curr_[i]);
        }
    }

    if(global_time_ == 0)
    {
        Eigen::Matrix<float, 3, 3, Eigen::RowMajor> init_Rcam = rmats_[rmats_.size() - 1];
        Eigen::Vector3f   init_tcam = tvecs_[tvecs_.size() - 1];

        Mat33&  device_Rcam = device_cast<Mat33> (init_Rcam);
        float3& device_tcam = device_cast<float3>(init_tcam);

        Eigen::Matrix<float, 3, 3, Eigen::RowMajor> init_Rcam_inv = init_Rcam.inverse ();
        Mat33&   device_Rcam_inv = device_cast<Mat33> (init_Rcam_inv);
        float3 device_volume_size = device_cast<const float3>(tsdf_volume_->getSize());

        int3 emptyVoxel;

        emptyVoxel.x = 0;
        emptyVoxel.y = 0;
        emptyVoxel.z = 0;

        if(rgbd)
        {
            rgbd->firstRun(depth_raw, colors);
        }

        integrateTsdfVolume(depth_raw,
                            intr,
                            device_volume_size,
                            device_Rcam_inv,
                            device_tcam,
                            tsdf_volume_->getTsdfTruncDist(),
                            tsdf_volume_->data(),
                            depthRawScaled_,
                            emptyVoxel,
                            color_volume_->data(),
                            colors,
                            nmaps_curr_[0],
                            !ConfigArgs::get().disableColorAngleWeight);

        for (int i = 0; i < ICPOdometry::LEVELS; ++i)
        {
            tranformMaps(vmaps_curr_[i], nmaps_curr_[i], device_Rcam, device_tcam, vmaps_g_prev_[i], nmaps_g_prev_[i]);
        }

        ++global_time_;

        init_utime.assignValue(timestamp);

        last_utime = timestamp;
        current_utime = timestamp;

        Eigen::Matrix4f curr = Eigen::Matrix4f::Identity();
        curr.topLeftCorner(3,3) = init_Rcam;
        curr.topRightCorner(3,1) = currentGlobalCamera;

        densePoseGraph.push_back(DensePose(current_utime, curr, true));

        latestDensePoseId++;

        unsigned short * firstDepth = new unsigned short[Resolution::get().numPixels()];
        memcpy(firstDepth, lastDepthData, Resolution::get().numPixels() * 2);

        unsigned char * firstImg = new unsigned char[Resolution::get().numPixels() * 3];
        memcpy(firstImg, lastRgbImage, Resolution::get().numPixels() * 3);

        firstDepthData.assignValue(firstDepth);
        firstRgbImage.assignValue(firstImg);

        if(ConfigArgs::get().vocabFile.size())
        {
            addToPlaceRecognition(depthSize, imageSize, compression, (unsigned short *)lastCompressedDepth, (unsigned char *)lastCompressedImage);
        }

        boost::mutex::scoped_lock lock(cloudMutex);

        cloudSignal.notify_all();

        return;
    }

    Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rprev = rmats_[rmats_.size() - 1];
    Eigen::Vector3f tprev = tvecs_[tvecs_.size() - 1];
    Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcurr = Rprev;
    Eigen::Vector3f tcurr = tprev;

    TICK("Odometry");
    lastOdometry = odometryProvider->getIncrementalTransformation(tcurr,
                                                                  Rcurr,
                                                                  depth_raw,
                                                                  colors,
                                                                  timestamp,
                                                                  rgbImage,
                                                                  depthData);
    TOCK("Odometry");

    last_utime = current_utime;
    current_utime = timestamp;

    //save tranform
    rmats_.push_back(Rcurr);
    tvecs_.push_back(tcurr);

    Eigen::Vector3f initialTrans = volumeBasis;
    initialTrans(0) -= Volume::get().getVolumeSize() * 0.5;
    initialTrans(1) -= Volume::get().getVolumeSize() * 0.5;
    initialTrans(2) -= Volume::get().getVolumeSize() * 0.5;

    const float3 & voxelSizeMeters = Volume::get().getVoxelSizeMeters();

    currentGlobalCamera = initialTrans;
    currentGlobalCamera(0) += voxelWrap.x * voxelSizeMeters.x;
    currentGlobalCamera(1) += voxelWrap.y * voxelSizeMeters.y;
    currentGlobalCamera(2) += voxelWrap.z * voxelSizeMeters.z;

    currentGlobalCamera(0) += tcurr(0) - volumeBasis(0);
    currentGlobalCamera(1) += tcurr(1) - volumeBasis(1);
    currentGlobalCamera(2) += tcurr(2) - volumeBasis(2);

    if(ConfigArgs::get().dynamicCube)
    {
        repositionCube(Rcurr);
    }

    bool shiftSend = false;
    bool isLoopPose = false;

    if(ConfigArgs::get().vocabFile.size())
    {
        float rnorm = rodrigues2(Rcurr.inverse() * lastPlaceRecognitionRot).norm();
        float tnorm = (currentGlobalCamera - lastPlaceRecognitionTrans).norm();
        const float alpha = 1.f;

        if((rnorm + alpha * tnorm)/2 >= place_recognition_movement)
        {
            lastPlaceRecognitionRot = Rcurr;
            lastPlaceRecognitionTrans = currentGlobalCamera;

            addToPlaceRecognition(depthSize, imageSize, compression, (unsigned short *)lastCompressedDepth, (unsigned char *)lastCompressedImage);

            isLoopPose = true;
        }
        else
        {
            shiftSend = true;
        }
    }

    float3 device_volume_size = device_cast<const float3> (tsdf_volume_->getSize());
    Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcurr_inv = Rcurr.inverse ();
    Mat33& device_Rcurr = device_cast<Mat33> (Rcurr);
    Mat33&  device_Rcurr_inv = device_cast<Mat33> (Rcurr_inv);
    float3& device_tcurr = device_cast<float3> (tcurr);

    Eigen::Vector3f currentTranslation = (tvecs_.at(tvecs_.size() - 1) - volumeBasis);

    Eigen::Vector3f voxelSize = tsdf_volume_->getVoxelSize();

    const int thresh = parked ? ConfigArgs::get().staticMode ? std::numeric_limits<int>::max() : std::numeric_limits<int>::max() : ConfigArgs::get().voxelShift;

    int xVoxelTrans = 0;
    int yVoxelTrans = 0;
    int zVoxelTrans = 0;

    if((int)std::floor(currentTranslation(0) / voxelSize(0)) < 0)
    {
        xVoxelTrans = std::max(-thresh, (int)std::floor(currentTranslation(0) / voxelSize(0)));
    }
    else
    {
        xVoxelTrans = std::min(thresh, (int)std::floor(currentTranslation(0) / voxelSize(0)));
    }

    if((int)std::floor(currentTranslation(1) / voxelSize(1)) < 0)
    {
        yVoxelTrans = std::max(-thresh, (int)std::floor(currentTranslation(1) / voxelSize(1)));
    }
    else
    {
        yVoxelTrans = std::min(thresh, (int)std::floor(currentTranslation(1) / voxelSize(1)));
    }

    if((int)std::floor(currentTranslation(2) / voxelSize(2)) < 0)
    {
        zVoxelTrans = std::max(-thresh, (int)std::floor(currentTranslation(2) / voxelSize(2)));
    }
    else
    {
        zVoxelTrans = std::min(thresh, (int)std::floor(currentTranslation(2) / voxelSize(2)));
    }

    vWrapCopyUpdate();

    cycled = false;

    PlaceRecognitionInput * nextPlaceRecognitionFrame = 0;

    if(xVoxelTrans >= thresh)
    {
        cloud_buffer_ = tsdf_volume_->fetchCloud(cloud_device_,
                                                 vWrapCopy,
                                                 color_volume_->data(),
                                                 0, xVoxelTrans + 1 + overlap,
                                                 0, VOLUME_Y,
                                                 0, VOLUME_Z,
                                                 voxelWrap);

        clearVolumeX(tsdf_volume_->data(), voxelWrap.x, voxelWrap.x + xVoxelTrans);
        clearVolumeXc(color_volume_->data(), voxelWrap.x, voxelWrap.x + xVoxelTrans);

        cycled = true;
    }
    else if(xVoxelTrans <= -thresh)
    {
        cloud_buffer_ = tsdf_volume_->fetchCloud(cloud_device_,
                                                 vWrapCopy,
                                                 color_volume_->data(),
                                                 VOLUME_X + (xVoxelTrans - overlap), VOLUME_X,
                                                 0, VOLUME_Y,
                                                 0, VOLUME_Z,
                                                 voxelWrap);

        clearVolumeXBack(tsdf_volume_->data(), voxelWrap.x, voxelWrap.x + xVoxelTrans);
        clearVolumeXBackc(color_volume_->data(), voxelWrap.x, voxelWrap.x + xVoxelTrans);

        cycled = true;
    }

    if(cycled)
    {
        if(shiftSend)
        {
            lastPlaceRecognitionRot = Rcurr;
            lastPlaceRecognitionTrans = currentGlobalCamera;

            nextPlaceRecognitionFrame = addToPlaceRecognition(depthSize, imageSize, compression, (unsigned short *)lastCompressedDepth, (unsigned char *)lastCompressedImage);

            isLoopPose = true;

            shiftSend = false;
        }

        int3 trans = {xVoxelTrans, 0, 0};
        mutexOutCloudBuffer(device_tcurr, trans, nextPlaceRecognitionFrame);
        nextPlaceRecognitionFrame = 0;
    }

    vWrapCopyUpdate();

    cycled = false;

    if(yVoxelTrans >= thresh)
    {
        cloud_buffer_ = tsdf_volume_->fetchCloud(cloud_device_,
                                                 vWrapCopy,
                                                 color_volume_->data(),
                                                 0, VOLUME_X,
                                                 0, yVoxelTrans + 1 + overlap,
                                                 0, VOLUME_Z,
                                                 voxelWrap);

        clearVolumeY(tsdf_volume_->data(), voxelWrap.y, voxelWrap.y + yVoxelTrans);
        clearVolumeYc(color_volume_->data(), voxelWrap.y, voxelWrap.y + yVoxelTrans);

        cycled = true;
    }
    else if(yVoxelTrans <= -thresh)
    {
        cloud_buffer_ = tsdf_volume_->fetchCloud(cloud_device_,
                                                 vWrapCopy,
                                                 color_volume_->data(),
                                                 0, VOLUME_X,
                                                 VOLUME_Y + (yVoxelTrans - overlap), VOLUME_Y,
                                                 0, VOLUME_Z,
                                                 voxelWrap);

        clearVolumeYBack(tsdf_volume_->data(), voxelWrap.y, voxelWrap.y + yVoxelTrans);
        clearVolumeYBackc(color_volume_->data(), voxelWrap.y, voxelWrap.y + yVoxelTrans);

        cycled = true;
    }

    if(cycled)
    {
        if(shiftSend)
        {
            lastPlaceRecognitionRot = Rcurr;
            lastPlaceRecognitionTrans = currentGlobalCamera;

            nextPlaceRecognitionFrame = addToPlaceRecognition(depthSize, imageSize, compression, (unsigned short *)lastCompressedDepth, (unsigned char *)lastCompressedImage);

            isLoopPose = true;

            shiftSend = false;
        }

        int3 trans = {0, yVoxelTrans, 0};
        mutexOutCloudBuffer(device_tcurr, trans, nextPlaceRecognitionFrame);
        nextPlaceRecognitionFrame = 0;
    }

    vWrapCopyUpdate();

    cycled = false;

    if(zVoxelTrans >= thresh)
    {
        cloud_buffer_ = tsdf_volume_->fetchCloud(cloud_device_,
                                                 vWrapCopy,
                                                 color_volume_->data(),
                                                 0, VOLUME_X,
                                                 0, VOLUME_Y,
                                                 0, zVoxelTrans + 1 + overlap,
                                                 voxelWrap);

        clearVolumeZ(tsdf_volume_->data(), voxelWrap.z, voxelWrap.z + zVoxelTrans);
        clearVolumeZc(color_volume_->data(), voxelWrap.z, voxelWrap.z + zVoxelTrans);

        cycled = true;
    }
    else if(zVoxelTrans <= -thresh)
    {
        cloud_buffer_ = tsdf_volume_->fetchCloud(cloud_device_,
                                                 vWrapCopy,
                                                 color_volume_->data(),
                                                 0, VOLUME_X,
                                                 0, VOLUME_Y,
                                                 VOLUME_Z + (zVoxelTrans - overlap) - 1, VOLUME_Z - 1,
                                                 voxelWrap);

        clearVolumeZBack(tsdf_volume_->data(), voxelWrap.z, voxelWrap.z + zVoxelTrans);
        clearVolumeZBackc(color_volume_->data(), voxelWrap.z, voxelWrap.z + zVoxelTrans);

        cycled = true;
    }

    if(cycled)
    {
        if(shiftSend)
        {
            lastPlaceRecognitionRot = Rcurr;
            lastPlaceRecognitionTrans = currentGlobalCamera;

            nextPlaceRecognitionFrame = addToPlaceRecognition(depthSize, imageSize, compression, (unsigned short *)lastCompressedDepth, (unsigned char *)lastCompressedImage);

            isLoopPose = true;

            shiftSend = false;
        }

        int3 trans = {0, 0, zVoxelTrans};
        mutexOutCloudBuffer(device_tcurr, trans, nextPlaceRecognitionFrame);
        nextPlaceRecognitionFrame = 0;
    }

    vWrapCopyUpdate();

    if(tsdfRequest.getValue())
    {
        bool tsdfNeeded = false;

        boost::mutex::scoped_lock tsdfLock(tsdfMutex);

        tsdfNeeded = !tsdfAvailable;

        tsdfLock.unlock();

        if(tsdfNeeded)
        {
            mutexOutLiveTsdf();
        }
    }

    bool imageNeeded = false;

    boost::mutex::scoped_lock imageLock(imageMutex);

    imageNeeded = !imageAvailable;

    imageLock.unlock();

    if(imageNeeded)
    {
        mutexOutLiveImage();
    }
    TICK("integrateTsdfVolume");
    integrateTsdfVolume (depth_raw,
                         intr,
                         device_volume_size,
                         device_Rcurr_inv,
                         device_tcurr,
                         tsdf_volume_->getTsdfTruncDist(),
                         tsdf_volume_->data(),
                         depthRawScaled_,
                         vWrapCopy,
                         color_volume_->data(),
                         colors,
                         nmaps_curr_[0],
                         !ConfigArgs::get().disableColorAngleWeight);
    TOCK("integrateTsdfVolume");
    vWrapCopyUpdate();

    raycast(intr,
            device_Rcurr,
            device_tcurr,
            tsdf_volume_->getTsdfTruncDist(),
            device_volume_size,
            tsdf_volume_->data(),
            vmaps_g_prev_[0],
            nmaps_g_prev_[0],
            vWrapCopy,
            vmap_curr_color,
            color_volume_->data());

    if(icp || ConfigArgs::get().useRGBDICP)
    {
        for (int i = 1; i < ICPOdometry::LEVELS; ++i)
        {
            resizeVMap(vmaps_g_prev_[i-1], vmaps_g_prev_[i]);
            resizeNMap(nmaps_g_prev_[i-1], nmaps_g_prev_[i]);
        }
    }

    ++global_time_;

    Eigen::Matrix4f curr = Eigen::Matrix4f::Identity();
    curr.topLeftCorner(3,3) = Rcurr;
    curr.topRightCorner(3,1) = currentGlobalCamera;

    densePoseGraph.push_back(DensePose(current_utime, curr, isLoopPose));

    latestDensePoseId++;

    if(!ConfigArgs::get().onlineDeformation)
    {
        outputPose(timestamp, Rcurr);
    }
}

PlaceRecognitionInput * KintinuousTracker::addToPlaceRecognition(int depthSize,
                                                                 int imageSize,
                                                                 bool compression,
                                                                 unsigned short * lastCompressedDepth,
                                                                 unsigned char * lastCompressedImage)
{
    int nextSlot = placeRecognitionId.getValue();

    assert(nextSlot < PR_BUFFER_SIZE);

    int depthDataSize = compression ? depthSize : Resolution::get().numPixels() * 2;
    int rgbDataSize = compression ? imageSize : Resolution::get().numPixels() * 3;

    unsigned char * depthPr = new unsigned char[depthDataSize];
    unsigned char * imgPr = new unsigned char[rgbDataSize];

    if(!compression)
    {
        memcpy(depthPr, lastDepthData, depthDataSize);
        memcpy(imgPr, lastRgbImage, rgbDataSize);
    }
    else
    {
        memcpy(depthPr, lastCompressedDepth, depthDataSize);
        memcpy(imgPr, lastCompressedImage, rgbDataSize);
    }

    PlaceRecognitionInput * toReturn = &placeRecognitionBuffer[nextSlot];

    placeRecognitionBuffer[nextSlot].rgbImage = imgPr;
    placeRecognitionBuffer[nextSlot].imageSize = rgbDataSize;
    placeRecognitionBuffer[nextSlot].depthMap = (unsigned short *)depthPr;
    placeRecognitionBuffer[nextSlot].depthSize = depthDataSize;
    placeRecognitionBuffer[nextSlot].isCompressed = compression;
    placeRecognitionBuffer[nextSlot].utime = current_utime;
    placeRecognitionBuffer[nextSlot].lagTime = lagTime;
    placeRecognitionBuffer[nextSlot].trans = lastPlaceRecognitionTrans;
    placeRecognitionBuffer[nextSlot].rotation = lastPlaceRecognitionRot;
    placeRecognitionId++;

    return toReturn;
}

void KintinuousTracker::getImage()
{
    LightSource light;
    light.number = 1;
    light.pos[0].x = tsdf_volume_->getSize()(0) * (-3.f);
    light.pos[0].y = tsdf_volume_->getSize()(1) * (-3.f);
    light.pos[0].z = tsdf_volume_->getSize()(2) * (-3.f);

    generateImage(vmaps_g_prev_[0], nmaps_g_prev_[0], vmap_curr_color, light, modelSurface, modelColor);
}

void KintinuousTracker::getModelDepth()
{
    Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcurr_inv = rmats_.back().inverse();
    Mat33  device_Rcurr_inv = device_cast<Mat33> (Rcurr_inv);
    float3 device_tcurr = device_cast<float3>(tvecs_.back());

    generateDepth(device_Rcurr_inv, device_tcurr, vmaps_g_prev_[0], nmaps_g_prev_[0], modelDepth, Volume::get().getVolumeSize());

    int cols;
    modelDepth.download(modelDepthHost, cols);
}

Eigen::Vector3f KintinuousTracker::getVolumeOffset() const
{
    return volumeBasis;
}

void KintinuousTracker::setParked(const bool park)
{
    parked = park;
}

Eigen::Vector3f KintinuousTracker::getLastTranslation() const
{
    return tvecs_.at(tvecs_.size() - 1) - volumeBasis;
}

Eigen::Vector3f KintinuousTracker::getVoxelSize() const
{
    return tsdf_volume_->getVoxelSize();
}

void KintinuousTracker::finalise()
{
    vWrapCopyUpdate();

    pcl::PointCloud<pcl::PointXYZRGB> * newCloud = 0;

    cloud_buffer_ = tsdf_volume_->fetchCloud(cloud_device_,
                                             vWrapCopy,
                                             color_volume_->data(),
                                             0, VOLUME_X,
                                             0, VOLUME_Y,
                                             0, VOLUME_Z,
                                             voxelWrap);

    newCloud = new pcl::PointCloud<pcl::PointXYZRGB>();

    cloud_buffer_.download(newCloud->points);

    boost::mutex::scoped_lock lock(cloudMutex);

    cycledMutex = true;

    sharedCloudSlices.push_back(new CloudSlice(newCloud,
                                               CloudSlice::FINAL,
                                               lastOdometry,
                                               currentGlobalCamera,
                                               rmats_.at(rmats_.size() - 1),
                                               current_utime,
                                               Stopwatch::getCurrentSystemTime(),
                                               lastRgbImage,
                                               0,
                                               0,
                                               lastDepthData));

    if(ConfigArgs::get().vocabFile.size())
    {
        lastPlaceRecognitionRot = rmats_.at(rmats_.size() - 1);
        lastPlaceRecognitionTrans = currentGlobalCamera;

        addToPlaceRecognition(Resolution::get().numPixels() * 2, Resolution::get().numPixels() * 3, false, 0, 0);

        sharedCloudSlices.back()->placeRecognitionFrame = &placeRecognitionBuffer[placeRecognitionId.getValue() - 1];
    }

    cloudSignal.notify_all();
}

Eigen::Matrix<float, 3, 3, Eigen::RowMajor> KintinuousTracker::getLastRotation() const
{
    return rmats_.at(rmats_.size() - 1);
}

std::vector<CloudSlice *> & KintinuousTracker::getCloudSlices()
{
    return sharedCloudSlices;
}

void KintinuousTracker::setOverlap(int overlap)
{
    this->overlap = overlap;
}

CloudSlice * KintinuousTracker::getLiveTsdf()
{
    return liveTsdf;
}

CloudSlice * KintinuousTracker::getLiveImage()
{
    return liveImage;
}

void KintinuousTracker::vWrapCopyUpdate()
{
	vWrapCopy = voxelWrap;

	if(vWrapCopy.x < 0)
		vWrapCopy.x = VOLUME_X - ((-vWrapCopy.x) % VOLUME_X);
	if(vWrapCopy.y < 0)
		vWrapCopy.y = VOLUME_Y - ((-vWrapCopy.y) % VOLUME_Y);
	if(vWrapCopy.z < 0)
		vWrapCopy.z = VOLUME_Z - ((-vWrapCopy.z) % VOLUME_Z);
}

void KintinuousTracker::mutexOutLiveTsdf()
{
    cloud_buffer_ = tsdf_volume_->fetchCloud(cloud_device_,
                                             vWrapCopy,
                                             color_volume_->data(),
                                             0, VOLUME_X,
                                             0, VOLUME_Y,
                                             0, VOLUME_Z,
                                             voxelWrap,
                                             1);

	pcl::PointCloud<pcl::PointXYZRGB> * newCloud = new pcl::PointCloud<pcl::PointXYZRGB>();

    cloud_buffer_.download(newCloud->points);

    boost::mutex::scoped_lock tsdfLock(tsdfMutex);

    tsdfAvailable = true;

	if(liveTsdf)
		delete liveTsdf;

	liveTsdf = new CloudSlice(newCloud,
							  CloudSlice::TSDF,
							  lastOdometry,
							  currentGlobalCamera,
							  rmats_.at(rmats_.size() - 1),
							  current_utime,
							  Stopwatch::getCurrentSystemTime(),
							  0);

    tsdfLock.unlock();
}

void KintinuousTracker::mutexOutLiveImage()
{
    int cols;
    getImage();

    modelColor.download(modelHost, cols);
    unsigned char * tsdfImageColor = new unsigned char[Resolution::get().numPixels() * 3];
    memcpy(tsdfImageColor, &modelHost[0], Resolution::get().numPixels() * 3);

    modelSurface.download(modelHost, cols);
    unsigned char * tsdfImage = new unsigned char[Resolution::get().numPixels() * 3];
    memcpy(tsdfImage, &modelHost[0], Resolution::get().numPixels() * 3);

    boost::mutex::scoped_lock imageLock(imageMutex);

    imageAvailable = true;

    if(liveImage)
        delete liveImage;

    liveImage = new CloudSlice(0,
                               CloudSlice::TSDF,
                               lastOdometry,
                               currentGlobalCamera,
                               rmats_.at(rmats_.size() - 1),
                               current_utime,
                               Stopwatch::getCurrentSystemTime(),
                               lastRgbImage,
                               tsdfImageColor,
                               tsdfImage,
                               lastDepthData);

    imageLock.unlock();
}

void KintinuousTracker::mutexOutCloudBuffer(float3 & device_tcurr,
                                            int3 voxelTrans,
                                            PlaceRecognitionInput * placeRecognitionFrame)
{
	Eigen::Vector3f voxelSize = tsdf_volume_->getVoxelSize();

	pcl::PointCloud<pcl::PointXYZRGB> * newCloud = 0;

    newCloud = new pcl::PointCloud<pcl::PointXYZRGB>();

    cloud_buffer_.download(newCloud->points);

	boost::mutex::scoped_lock lock(cloudMutex);

	Eigen::Vector3f voxelTransSize;
	voxelTransSize(0) = voxelSize(0) * voxelTrans.x;
	voxelTransSize(1) = voxelSize(1) * voxelTrans.y;
	voxelTransSize(2) = voxelSize(2) * voxelTrans.z;

	tvecs_.at(tvecs_.size() - 1) -= voxelTransSize;

    cycledMutex = true;

    CloudSlice::Dimension direction = voxelTrans.x > 0 ? CloudSlice::XPlus  :
                                      voxelTrans.x < 0 ? CloudSlice::XMinus :
                                      voxelTrans.y > 0 ? CloudSlice::YPlus  :
                                      voxelTrans.y < 0 ? CloudSlice::YMinus :
                                      voxelTrans.z > 0 ? CloudSlice::ZPlus  :
                                                         CloudSlice::ZMinus;

    sharedCloudSlices.push_back(new CloudSlice(newCloud,
                                               direction,
                                               lastOdometry,
                                               currentGlobalCamera,
                                               rmats_.at(rmats_.size() - 1),
                                               current_utime,
                                               lagTime,
                                               0,
                                               0,
                                               0,
                                               0,
                                               placeRecognitionFrame));

	voxelWrap.x += voxelTrans.x;
	voxelWrap.y += voxelTrans.y;
	voxelWrap.z += voxelTrans.z;

	cloudSignal.notify_all();

	device_tcurr.x -= voxelTransSize(0);
	device_tcurr.y -= voxelTransSize(1);
	device_tcurr.z -= voxelTransSize(2);
}

Eigen::Vector3f KintinuousTracker::rodrigues2(const Eigen::Matrix3f& matrix)
{
    Eigen::JacobiSVD<Eigen::Matrix3f> svd(matrix, Eigen::ComputeFullV | Eigen::ComputeFullU);
    Eigen::Matrix3f R = svd.matrixU() * svd.matrixV().transpose();

    double rx = R(2, 1) - R(1, 2);
    double ry = R(0, 2) - R(2, 0);
    double rz = R(1, 0) - R(0, 1);

    double s = sqrt((rx*rx + ry*ry + rz*rz)*0.25);
    double c = (R.trace() - 1) * 0.5;
    c = c > 1. ? 1. : c < -1. ? -1. : c;

    double theta = acos(c);

    if( s < 1e-5 )
    {
        double t;

        if( c > 0 )
            rx = ry = rz = 0;
        else
        {
            t = (R(0, 0) + 1)*0.5;
            rx = sqrt( std::max(t, 0.0) );
            t = (R(1, 1) + 1)*0.5;
            ry = sqrt( std::max(t, 0.0) ) * (R(0, 1) < 0 ? -1.0 : 1.0);
            t = (R(2, 2) + 1)*0.5;
            rz = sqrt( std::max(t, 0.0) ) * (R(0, 2) < 0 ? -1.0 : 1.0);

            if( fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R(1, 2) > 0) != (ry*rz > 0) )
                rz = -rz;
            theta /= sqrt(rx*rx + ry*ry + rz*rz);
            rx *= theta;
            ry *= theta;
            rz *= theta;
        }
    }
    else
    {
        double vth = 1/(2*s);
        vth *= theta;
        rx *= vth; ry *= vth; rz *= vth;
    }
    return Eigen::Vector3d(rx, ry, rz).cast<float>();
}


#include "FdrAnalysisEngine.h"
#include "../PeptideSpectralMatch.h"
#include "../CommonParameters.h"
#include "../MetaMorpheusEngineResults.h"
#include "FdrAnalysisResults.h"
#include "../GlobalVariables.h"

#include <boost/math/special_functions/gamma.hpp>

#include <numeric>
#include <math.h>
#include "Group.h"

namespace EngineLayer
{
    namespace FdrAnalysis
    {

        FdrAnalysisEngine::FdrAnalysisEngine(std::vector<PeptideSpectralMatch*> &psms,
                                             int massDiffAcceptorNumNotches,
                                             CommonParameters *commonParameters,
                                             std::vector<std::string> nestedIds,
                                             int verbosityLevel ) :
            MetaMorpheusEngine(commonParameters, nestedIds, verbosityLevel),
            MassDiffAcceptorNumNotches(massDiffAcceptorNumNotches),
            UseDeltaScore(commonParameters->getUseDeltaScore()),
            CalculateEValue(commonParameters->getCalculateEValue()),
            ScoreCutoff(commonParameters->getScoreCutoff()),
            AllPsms(psms)
        {
        }

        MetaMorpheusEngineResults *FdrAnalysisEngine::RunSpecific()
        {
            FdrAnalysisResults *myAnalysisResults = new FdrAnalysisResults(this);

            Status("Running FDR analysis...");
            DoFalseDiscoveryRateAnalysis(myAnalysisResults);

            std::vector<PeptideSpectralMatch *> tmp;
            for ( auto b: AllPsms ) {
                if (b->getFdrInfo()->getQValue() < 0.01 ) {
                    tmp.push_back(b);
                }
            }

            myAnalysisResults->setPsmsWithin1PercentFdr(tmp.size() );

            return myAnalysisResults;
        }

        void FdrAnalysisEngine::DoFalseDiscoveryRateAnalysis(FdrAnalysisResults *myAnalysisResults)
        {
            // Stop if canceled
            if (GlobalVariables::getStopLoops())
            {
                return;
            }

            // calculate FDR on a per-protease basis (targets and decoys for a specific protease)
#ifdef ORIG
            auto psmsGroupedByProtease = AllPsms.GroupBy([&] (std::any p){
                    p::DigestionParams::Protease;
                });
#endif
            std::function<bool(PeptideSpectralMatch*,PeptideSpectralMatch*)> f1 = [&]
                (PeptideSpectralMatch *l, PeptideSpectralMatch *r) {
                return l->digestionParams->getProtease() < r->digestionParams->getProtease(); } ;
            std::function<bool(PeptideSpectralMatch*,PeptideSpectralMatch*)> f2 = [&]
                (PeptideSpectralMatch *l, PeptideSpectralMatch *r) {
                return l->digestionParams->getProtease() != r->digestionParams->getProtease(); } ;
            std::vector<std::vector<PeptideSpectralMatch *>> psmsGroupedByProtease = Group::GroupBy( AllPsms, f1, f2 );

            for (auto psms : psmsGroupedByProtease)
            {

                // generate the null distribution for e-value calculations
                double globalMeanScore = 0;
                int globalMeanCount = 0;

#ifdef ORIG
                //if (CalculateEValue && psms.Any())
#endif
                if (CalculateEValue && !psms.empty())
                {
                    std::vector<double> combinedScores;
                    for (auto psm : psms)
                    {
                        auto allscores = psm->getAllScores();
                        std::sort(allscores.begin(), allscores.end());
                        combinedScores.insert(combinedScores.end(), allscores.begin(), allscores.end());

                        //remove top scoring peptide
#ifdef ORIG
                        //if (combinedScores.Any())
#endif
                        if ( !combinedScores.empty() )
                        {
                            combinedScores.pop_back();
                        }
                    }

#ifdef ORIG
                    //if (combinedScores.Any())
#endif
                    if ( !combinedScores.empty() )
                    {
                        //globalMeanScore = combinedScores.Average();
                        globalMeanScore = std::accumulate(combinedScores.begin(),combinedScores.end(), 0.0)/combinedScores.size();
                        globalMeanCount = static_cast<int>(static_cast<double>(combinedScores.size()) / psms.size());
                    }
                    else
                    {
                        // should be a very rare case... if there are PSMs but each PSM only has one hit
                        globalMeanScore = 0;
                        globalMeanCount = 0;
                    }
                }

                //Calculate delta scores for the psms (regardless of if we are using them)
                for (auto psm : psms)
                {
                    if (psm != nullptr)
                    {
                        psm->CalculateDeltaScore(ScoreCutoff);
                    }
                }

                //determine if Score or DeltaScore performs better
                if (UseDeltaScore)
                {
                    constexpr double qValueCutoff = 0.01; //optimize to get the most PSMs at a 1% FDR

#ifdef ORIG
                    std::vector<PeptideSpectralMatch*> scoreSorted = psms.OrderByDescending([&] (std::any b)   {
                            b::Score;
                        }).ThenBy([&] (std::any b)   {
                                b::PeptideMonisotopicMass.HasValue ? std::abs(b::ScanPrecursorMass-b::PeptideMonisotopicMass->Value)
                                :  std::numeric_limits<double>::max();
                            }).GroupBy([&] (std::any b) {
                                    return std::tuple <std::string, int, std::optional<double>>(b::FullFilePath, b::ScanNumber, b::PeptideMonisotopicMass)}
                                )->Select([&] (std::any b) {
                                        b::First();
                                    }).ToList();

#endif
                    //OrderByDescending and ThenBy
                    std::stable_sort (psms.begin(), psms.end(), [&] (PeptideSpectralMatch *r, PeptideSpectralMatch *l) {
                            if ( r->getScore() > l->getScore() ) return true;
                            if ( r->getScore() < l->getScore() ) return false;

                            double lval= std::numeric_limits<double>::max(), rval=std::numeric_limits<double>::max();
                            if ( l->getPeptideMonisotopicMass().has_value() ) {
                                lval = std::abs(l->getScanPrecursorMass() - l->getPeptideMonisotopicMass().value());
                            }
                            if ( r->getPeptideMonisotopicMass().has_value() ) {
                                rval = std::abs(r->getScanPrecursorMass() - r->getPeptideMonisotopicMass().value());
                            }
                            return rval < lval;
                        });
                    //GroupBy tuple
                    std::function<bool(PeptideSpectralMatch*,PeptideSpectralMatch*)> f3 = [&]
                        (PeptideSpectralMatch *l, PeptideSpectralMatch *r) {
                        return l->getFullFilePath() != r->getFullFilePath() &&
                        l->getScanNumber() != r->getScanNumber() &&
                        l->getPeptideMonisotopicMass() != r->getPeptideMonisotopicMass(); } ;
                    std::vector<std::vector<PeptideSpectralMatch *>>tvec;
                    //Edgar: Note: cannot use the Group::GroupBy functionality here because of the custom sorting.
                    for ( auto p : psms ) {
                        bool found = false;
                        for ( auto q: tvec ) {
                            if ( f3 ( p, q[0] ) ) {
                                found = true;
                                q.push_back(p);
                                break;
                            }
                        }
                        if ( !found ) {
                            auto vec = new std::vector<PeptideSpectralMatch *>();
                            vec->push_back(p);
                            tvec.push_back(*vec);
                        }
                    }

                    // Select first
                    std::vector<PeptideSpectralMatch*> scoreSorted;
                    for ( auto t: tvec ) {
                        scoreSorted.push_back(t[0]);
                    }

                    int ScorePSMs = GetNumPSMsAtqValueCutoff(scoreSorted, qValueCutoff);
#ifdef ORIG
                    scoreSorted = psms.OrderByDescending([&] (std::any b)	{
                            b::DeltaScore;
                        }).ThenBy([&] (std::any b){
                                b::PeptideMonisotopicMass.HasValue ? std::abs(b::ScanPrecursorMass - b::PeptideMonisotopicMass->Value) :
                                std::numeric_limits<double>::max();
                            }).GroupBy([&] (std::any b)		{
                                    return std::tuple < std::string>;
                                }, int, std::optional<double>>(b::FullFilePath, b::ScanNumber, b::PeptideMonisotopicMass))->Select([&] (std::any b)  {
                                        b::First();
                                    }).ToList();
#endif
                    //OrderByDescending and ThenBy
                    std::stable_sort (psms.begin(), psms.end(), [&] (PeptideSpectralMatch *r, PeptideSpectralMatch *l) {
                            if ( r->getDeltaScore() > l->getDeltaScore() ) return true;
                            if ( r->getDeltaScore() < l->getDeltaScore() ) return false;

                            double lval= std::numeric_limits<double>::max(), rval=std::numeric_limits<double>::max();
                            if ( l->getPeptideMonisotopicMass().has_value() ) {
                                lval = std::abs(l->getScanPrecursorMass() - l->getPeptideMonisotopicMass().value());
                            }
                            if ( r->getPeptideMonisotopicMass().has_value() ) {
                                rval = std::abs(r->getScanPrecursorMass() - r->getPeptideMonisotopicMass().value());
                            }
                            return rval < lval;
                        });
                    //GroupBy tuple
                    tvec.clear();
                    for ( auto p : psms ) {
                        bool found = false;
                        for ( auto q: tvec ) {
                            if ( f3 ( p, q[0] ) ) {
                                found = true;
                                q.push_back(p);
                                break;
                            }
                        }
                        if ( !found ) {
                            auto vec = new std::vector<PeptideSpectralMatch *>();
                            vec->push_back(p);
                            tvec.push_back(*vec);
                        }
                    }
                    // Select first
                    scoreSorted.clear();
                    for ( auto t: tvec ) {
                        scoreSorted.push_back(t[0]);
                    }
                    int DeltaScorePSMs = GetNumPSMsAtqValueCutoff(scoreSorted, qValueCutoff);

                    //sort by best method
                    myAnalysisResults->setDeltaScoreImprovement(DeltaScorePSMs > ScorePSMs);
#ifdef ORIG
                    psms = myAnalysisResults->getDeltaScoreImprovement() ? psms.OrderByDescending([&] (std::any b)     {
                            b::DeltaScore;
                        }).ThenBy([&] (std::any b)  {
                                b::PeptideMonisotopicMass.HasValue ? std::abs(b::ScanPrecursorMass - b::PeptideMonisotopicMass->Value) : std::numeric_limits<double>::max();
                            }).ToList() :
                        psms.OrderByDescending([&] (std::any b) {
                                b::Score;
                            }).ThenBy([&] (std::any b){
                                    b::PeptideMonisotopicMass.HasValue ? std::abs(b::ScanPrecursorMass - b::PeptideMonisotopicMass->Value) : std::numeric_limits<double>::max();
                                }).ToList();
#endif
                    if ( myAnalysisResults->getDeltaScoreImprovement() ) {
                        //OrderByDescending and ThenBy
                        std::stable_sort (psms.begin(), psms.end(), [&] (PeptideSpectralMatch *r, PeptideSpectralMatch *l) {
                                if ( r->getDeltaScore() > l->getDeltaScore() ) return true;
                                if ( r->getDeltaScore() < l->getDeltaScore() ) return false;

                                double lval= std::numeric_limits<double>::max(), rval=std::numeric_limits<double>::max();
                                if ( l->getPeptideMonisotopicMass().has_value() ) {
                                    lval = std::abs(l->getScanPrecursorMass() - l->getPeptideMonisotopicMass().value());
                                }
                                if ( r->getPeptideMonisotopicMass().has_value() ) {
                                    rval = std::abs(r->getScanPrecursorMass() - r->getPeptideMonisotopicMass().value());
                                }
                                return rval < lval;
                            });
                    }
                    else {
                    //OrderByDescending and ThenBy
                    std::stable_sort (psms.begin(), psms.end(), [&] (PeptideSpectralMatch *r, PeptideSpectralMatch *l) {
                            if ( r->getScore() > l->getScore() ) return true;
                            if ( r->getScore() < l->getScore() ) return false;

                            double lval= std::numeric_limits<double>::max(), rval=std::numeric_limits<double>::max();
                            if ( l->getPeptideMonisotopicMass().has_value() ) {
                                lval = std::abs(l->getScanPrecursorMass() - l->getPeptideMonisotopicMass().value());
                            }
                            if ( r->getPeptideMonisotopicMass().has_value() ) {
                                rval = std::abs(r->getScanPrecursorMass() - r->getPeptideMonisotopicMass().value());
                            }
                            return rval < lval;
                        });
                    }

                }
                else //sort by score
                {
#ifdef ORIG
                    psms = psms.OrderByDescending([&] (std::any b) {
                            b::Score;
                        }).ThenBy([&] (std::any b)   {
                                b::PeptideMonisotopicMass.HasValue ? std::abs(b::ScanPrecursorMass - b::PeptideMonisotopicMass->Value) :
                                std::numeric_limits<double>::max();
                            }).ToList();
#endif
                    //OrderByDescending and ThenBy
                    std::stable_sort (psms.begin(), psms.end(), [&] (PeptideSpectralMatch *r, PeptideSpectralMatch *l) {
                            if ( r->getScore() > l->getScore() ) return true;
                            if ( r->getScore() < l->getScore() ) return false;

                            double lval= std::numeric_limits<double>::max(), rval=std::numeric_limits<double>::max();
                            if ( l->getPeptideMonisotopicMass().has_value() ) {
                                lval = std::abs(l->getScanPrecursorMass() - l->getPeptideMonisotopicMass().value());
                            }
                            if ( r->getPeptideMonisotopicMass().has_value() ) {
                                rval = std::abs(r->getScanPrecursorMass() - r->getPeptideMonisotopicMass().value());
                            }
                            return rval < lval;
                        });
                }

                double cumulativeTarget = 0;
                double cumulativeDecoy = 0;

                //set up arrays for local FDRs
                std::vector<double> cumulativeTargetPerNotch(MassDiffAcceptorNumNotches + 1);
                std::vector<double> cumulativeDecoyPerNotch(MassDiffAcceptorNumNotches + 1);

                //Assign FDR values to PSMs
                for (int i = 0; i < (int)psms.size(); i++)
                {
                    // Stop if canceled
                    if (GlobalVariables::getStopLoops())
                    {
                        break;
                    }

                    PeptideSpectralMatch *psm = psms[i];
                    std::optional<int> tempVar = psm->getNotch();
                    int notch = tempVar.has_value() ? tempVar.value() : MassDiffAcceptorNumNotches;
                    if (psm->getIsDecoy())
                    {
                        // the PSM can be ambiguous between a target and a decoy sequence
                        // in that case, count it as the fraction of decoy hits
                        // e.g. if the PSM matched to 1 target and 2 decoys, it counts as 2/3 decoy
                        double decoyHits = 0;
                        double totalHits = 0;
#ifdef ORIG
                        auto hits = psm->BestMatchingPeptides.GroupBy([&] (std::any p)       {
                                p::Peptide::FullSequence;
                            });
#endif
                        std::vector<std::vector<std::tuple<int, PeptideWithSetModifications *>>>hits;
                        int current =0;
                        std::string currFullSequence, prevFullSequence;
                        auto tmpsms = psm->getBestMatchingPeptides();
                        for ( auto p = tmpsms.begin(); p != tmpsms.end(); p++ ) {
                            if ( p == tmpsms.begin() ) {
                                std::vector<std::tuple<int, PeptideWithSetModifications *>> *v = new std::vector<std::tuple<int, PeptideWithSetModifications *>>;
                                hits.push_back(*v);
                                prevFullSequence = std::get<1>(*p)->getFullSequence();
                            }
                            else {
                                auto q = p - 1;
                                currFullSequence = std::get<1>(*p)->getFullSequence();
                                if ( currFullSequence != prevFullSequence ) {
                                    std::vector<std::tuple<int, PeptideWithSetModifications *>> *v = new std::vector<std::tuple<int, PeptideWithSetModifications *>>;
                                    hits.push_back(*v);
                                    current++;
                                    prevFullSequence = currFullSequence;
                                }
                            }
                            hits[current].push_back(*p);
                        }
                        for (auto hit : hits)
                        {
#ifdef ORIG
                            if (hit->First().Peptide.Protein.IsDecoy)
#endif
                                if (std::get<1>(hit.front())->getProtein()->getIsDecoy())
                            {
                                decoyHits++;
                            }
                            totalHits++;
                        }

                        cumulativeDecoy += decoyHits / totalHits;
                        cumulativeDecoyPerNotch[notch] += decoyHits / totalHits;
                    }
                    else
                    {
                        cumulativeTarget++;
                        cumulativeTargetPerNotch[notch]++;
                    }

                    double qValue = std::min(1.0, cumulativeDecoy / cumulativeTarget);
                    double qValueNotch = std::min(1.0, cumulativeDecoyPerNotch[notch] / cumulativeTargetPerNotch[notch]);

                    double maximumLikelihood = 0;
                    double eValue = 0;
                    double eScore = 0;
                    if (CalculateEValue)
                    {
                        eValue = GetEValue(psm, globalMeanCount, globalMeanScore, maximumLikelihood);
                        //eScore = -Math::Log(eValue, 10);
                        eScore = -log10(eValue);
                    }

                    psm->SetFdrValues(cumulativeTarget, cumulativeDecoy, qValue, cumulativeTargetPerNotch[notch],
                                      cumulativeDecoyPerNotch[notch], qValueNotch, maximumLikelihood, eValue, eScore, CalculateEValue);
                }

                // set q-value thresholds such that a lower scoring PSM can't have
                // a higher confidence than a higher scoring PSM
                //Populate min qValues
                double qValueThreshold = 1.0;
                std::vector<double> qValueNotchThreshold(MassDiffAcceptorNumNotches + 1);
                for (int i = 0; i < (int)qValueNotchThreshold.size(); i++)
                {
                    qValueNotchThreshold[i] = 1.0;
                }

                for (int i = (int)psms.size() - 1; i >= 0; i--)
                {
                    PeptideSpectralMatch *psm = psms[i];

                    // threshold q-values
                    if (psm->getFdrInfo()->getQValue() > qValueThreshold)
                    {
                        psm->getFdrInfo()->setQValue(qValueThreshold);
                    }
                    else if (psm->getFdrInfo()->getQValue() < qValueThreshold)
                    {
                        qValueThreshold = psm->getFdrInfo()->getQValue();
                    }

                    // threshold notch q-values
                    std::optional<int> tempVar2 = psm->getNotch();
                    int notch = tempVar2.has_value() ? tempVar2.value() : MassDiffAcceptorNumNotches;
                    if (psm->getFdrInfo()->getQValueNotch() > qValueNotchThreshold[notch])
                    {
                        psm->getFdrInfo()->setQValueNotch(qValueNotchThreshold[notch]);
                    }
                    else if (psm->getFdrInfo()->getQValueNotch() < qValueNotchThreshold[notch])
                    {
                        qValueNotchThreshold[notch] = psm->getFdrInfo()->getQValueNotch();
                    }
                }
            }
        }

        double FdrAnalysisEngine::GetEValue(PeptideSpectralMatch *psm, int globalMeanCount, double globalMeanScore, double &maximumLikelihood)
        {
            // get all of the PSM's scores for all hits, sort them, then remove the last value (the best score)
            std::vector<double> scoresWithoutBestHit;
            scoresWithoutBestHit.insert(scoresWithoutBestHit.end(), psm->getAllScores().begin(), psm->getAllScores().end());
            std::sort(scoresWithoutBestHit.begin(), scoresWithoutBestHit.end());

#ifdef ORIG
            if (scoresWithoutBestHit.Any())
#endif
            if (!scoresWithoutBestHit.empty())

            {
                scoresWithoutBestHit.pop_back();
            }

            // this is the "default" case for when there are no scores except the best hit
            // it uses a global mean score (all scores across all PSMs) to generate the null Poisson distribution
            // this will be overriden by the next few lines if there are enough scores in this PSM to estimate a null distribution
            //double preValue = SpecialFunctions::GammaLowerRegularized(globalMeanScore, psm->getScore());
            //Edgar Note: the boost gamma lower regularized function gamma_p does not accept the first argument to be 0.
            //            I verified with the C# version that the result returned by SpecialFunctions::GammaLowerRegularized
            //            is in that case 1, independent of the second argument. Mimicking the same here.
            double preValue = 1;
            if ( globalMeanScore > 0 ) {
                preValue = boost::math::gamma_p (globalMeanScore, psm->getScore());
            }
            maximumLikelihood = globalMeanScore;

            // calculate single-spectrum evalue if there are enough hits besides the best scoring peptide
            if (psm->getScore() == 0)
            {
                preValue = 1;
                maximumLikelihood = 0;
            }
#ifdef ORIG
            else if (scoresWithoutBestHit.Any())
#endif
            else if (!scoresWithoutBestHit.empty())
            {
                //maximumLikelihood = scoresWithoutBestHit.Average();
                maximumLikelihood = std::accumulate(scoresWithoutBestHit.begin(), scoresWithoutBestHit.end(), 0.0)/scoresWithoutBestHit.size();

                // this is the cumulative distribution for the poisson at each score up to but not including the score of the winner.
                // This is the probability that the winner has of getting that score at random by matching against a SINGLE spectrum
                if (maximumLikelihood > 0)
                {
                    //preValue = SpecialFunctions::GammaLowerRegularized(maximumLikelihood, psm->getScore());
                    //preValue = Math::GammaLowerRegularized(maximumLikelihood, psm->getScore());
                    preValue = boost::math::gamma_p ( maximumLikelihood, psm->getScore());
                }
            }

            // Now the probability of getting the winner's score goes up for each spectrum match.
            // We multiply the preValue by the number of theoretical spectrum within the tolerance to get this new probability.
            int count = scoresWithoutBestHit.size();
            if (count == 0)
            {
                count = globalMeanCount;
            }

            double probabilityOfScore = 1 - std::pow(preValue, count);
            return count * probabilityOfScore;
        }

        int FdrAnalysisEngine::GetNumPSMsAtqValueCutoff(std::vector<PeptideSpectralMatch*> &psms, double qValueCutoff)
        {
            int cumulative_target = 0;
            int cumulative_decoy = 0;
            for (auto psm : psms)
            {
                if (psm->getIsDecoy())
                {
                    cumulative_decoy++;
                    if (static_cast<double>(cumulative_decoy) / cumulative_target >= qValueCutoff)
                    {
                        return cumulative_target;
                    }
                }
                else
                {
                    cumulative_target++;
                }
            }
            return cumulative_target;
        }
    }
}


#ifndef data_hpp
#define data_hpp

#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <fstream>
#include <set>
#include <bitset>
#include <iomanip>
#include <Eigen/Eigen>
#include <Eigen/Sparse>
#include <boost/format.hpp>
#include "gadgets.hpp"
#include "common.h"

using namespace std;
using namespace Eigen;

class SnpInfo {
public:
    const string ID;
    const string a1; // the referece allele
    const string a2; // the coded allele
    const int chrom;
    const float genPos;
    const int physPos;

    int index;
    int window;
    int windStart;  // for window surrounding the SNP
    int windSize;   // for window surrounding the SNP
    float af;       // allele frequency
    bool included;  // flag for inclusion in panel
    bool isQTL;     // for simulation

    SnpInfo(const int idx, const string &id, const string &allele1, const string &allele2,
            const int chr, const float gpos, const int ppos)
    : ID(id), index(idx), a1(allele1), a2(allele2), chrom(chr), genPos(gpos), physPos(ppos) {
        window = 0;
        windStart = -1;
        windSize  = 0;
        af = -1;
        included = true;
        isQTL = false;
    };

    void resetWindow(void) {windStart = -1; windSize = 0;};
};

class IndInfo {
public:
    const string famID;
    const string indID;
    const string catID;    // catenated family and individual ID
    const string fatherID;
    const string motherID;
    const int famFileOrder; // original fam file order
    const int sex;  // 1: male, 2: female

    int index;
    bool kept;

    float phenotype;

    VectorXf covariates;  // covariates for fixed effects

    IndInfo(const int idx, const string &fid, const string &pid, const string &dad, const string &mom, const int sex)
    : famID(fid), indID(pid), catID(fid+":"+pid), fatherID(dad), motherID(mom), index(idx), famFileOrder(idx), sex(sex) {
        phenotype = -9;
        kept = true;
    }
};

using PpBedIndex = std::vector<IndexEntry>;

class Data {
public:
    Data();

    // mmap related data
    int ppBedFd;
    double *ppBedMap;
    Map<MatrixXd> mappedZ;
    PpBedIndex ppbedIndex;

    // Original data
    MatrixXf X;              // coefficient matrix for fixed effects
    MatrixXf Z;              // coefficient matrix for SNP effects
    VectorXf D;              // 2pqn
    VectorXf y;              // phenotypes
    vector<int> G; 			 // groups
    VectorXd fail;			 // Failure indicator

    // Vectors for the sparse format solution
    std::vector<std::vector<int>> Zones; // Vector for each SNP: per SNP all the indices with 1 allele are written down
    std::vector<std::vector<int>> Ztwos; // Vector for each SNP: per SNP all the indices with 2 alleles are written down
    VectorXd means; //Mean for each SNP
    VectorXd sds;
    VectorXd mean_sd_ratio;

    MatrixXf XPX;            // X'X the MME lhs
    MatrixXf ZPX;            // Z'X the covariance matrix of SNPs and fixed effects
    VectorXf XPXdiag;        // X'X diagonal
    VectorXf ZPZdiag;        // Z'Z diagonal
    VectorXf XPy;            // X'y the MME rhs for fixed effects
    VectorXf ZPy;            // Z'y the MME rhs for snp effects

    VectorXf snp2pq;         // 2pq of SNPs
    VectorXf se;             // se from GWAS summary data
    VectorXf tss;            // total ss (ypy) for every SNP
    VectorXf b;              // beta from GWAS summary data
    VectorXf n;              // sample size for each SNP in GWAS

    vector<SnpInfo*> snpInfoVec;
    vector<IndInfo*> indInfoVec;

    map<string, SnpInfo*> snpInfoMap;
    map<string, IndInfo*> indInfoMap;

    vector<SnpInfo*> incdSnpInfoVec;
    vector<IndInfo*> keptIndInfoVec;

    vector<string> fixedEffectNames;
    vector<string> snpEffectNames;

    vector<bool> fullSnpFlag;
    vector<vector<SnpInfo*> > mldmVec;

    unsigned numFixedEffects = 0;
    unsigned numSnps;
    unsigned numInds;

    unsigned numGroups = 1; // number of groups

    void preprocessCSVFile(const string &csvFile, const string &preprocessedCSVFile, const string &preprovessedCSVIndexFile, bool compress);
    void mapPreprocessBedFile(const string &preprocessedBedFile);
    void unmapPreprocessedBedFile();

    void mapCompressedPreprocessBedFile(const string &preprocessedBedFile, const string &indexFile);
    void unmapCompressedPreprocessedBedFile();

    void readCSV(const string &filename, int cols);

    void readFamFile(const string &famFile);
    void readBimFile(const string &bimFile);
    void readBedFile_noMPI(const string &bedFile);
    void readBedFile_noMPI_unstandardised(const string &bedFile);

    void readPhenotypeFile(const string &phenFile);
    void readGroupFile(const string &groupFile);
    void readCSVFile(const string &csvFile);
    void readCSVPhenFile( const string &csvFile);
    void readmSFile(const string& mSfile);
    //BayesW variables
    void readFailureFile(const string &failureFile);
};

#endif /* data_hpp */


#include <chrono>
#include <iostream>
#include <boost/format.hpp>
#include <boost/filesystem.hpp>
#include <boost/circular_buffer.hpp>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_color_handlers.h>

#include <fast_gicp/gicp/fast_gicp.hpp>
#include <fast_gicp/gicp/fast_vgicp.hpp>

#ifdef USE_VGICP_CUDA
#include <fast_gicp/gicp/fast_vgicp_cuda.hpp>
#endif

class KittiLoader {
public:
  KittiLoader(const std::string& dataset_path)
  : dataset_path(dataset_path)
  {
    for(num_frames = 0;; num_frames++) {
      std::string filename = (boost::format("%s/%06d.bin") % dataset_path % num_frames).str();
      if(!boost::filesystem::exists(filename)) {
        break;
      }
    }

    if(num_frames == 0) {
      std::cerr << "error: no files in " << dataset_path << std::endl;
    }
  }
  ~KittiLoader() {}

  size_t size() const { return num_frames; }

  pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud(size_t i) const {
    std::string filename = (boost::format("%s/%06d.bin") % dataset_path % i).str();
    FILE* file = fopen(filename.c_str(), "rb");
    if(!file) {
      std::cerr << "error: failed to load " << filename << std::endl;
      return nullptr;
    }

    std::vector<float> buffer(1000000);
    size_t num_points = fread(reinterpret_cast<char*>(buffer.data()), sizeof(float), buffer.size(), file) / 4;
    fclose(file);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    cloud->resize(num_points);

    for(int i = 0; i < num_points; i++) {
      auto& pt = cloud->at(i);
      pt.x = buffer[i * 4];
      pt.y = buffer[i * 4 + 1];
      pt.z = buffer[i * 4 + 2];
      // pt.intensity = buffer[i * 4 + 3];
    }

    return cloud;
  }

private:
  int num_frames;
  std::string dataset_path;
};

int main(int argc, char** argv) {
  if(argc < 2) {
    std::cout << "usage: gicp_kitti /your/kitti/path/sequences/00/velodyne" << std::endl;
    return 0;
  }

  KittiLoader kitti(argv[1]);

  // use downsample_resolution=1.0 for fast registration
  double downsample_resolution = 0.25;
  pcl::ApproximateVoxelGrid<pcl::PointXYZ> voxelgrid;
  voxelgrid.setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);

  // registration method
  // you should fine-tune hyper-parameters (e.g., voxel resolution, max correspondence distance) for the best result
  // fast_gicp::FastGICP<pcl::PointXYZ, pcl::PointXYZ> gicp;
  fast_gicp::FastVGICP<pcl::PointXYZ, pcl::PointXYZ> gicp;
  // fast_gicp::FastVGICPCuda<pcl::PointXYZ, pcl::PointXYZ> gicp;
  gicp.setMaxCorrespondenceDistance(1.0);

  // set initial frame as target
  voxelgrid.setInputCloud(kitti.cloud(0));
  auto target = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
  voxelgrid.filter(*target);
  gicp.setInputTarget(target);

  // sensor pose sequence
  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses(kitti.size());
  poses[0].setIdentity();

  // trajectory for visualization
  auto trajectory = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
  trajectory->push_back(pcl::PointXYZ(0.0f, 0.0f, 0.0f));

  pcl::visualization::PCLVisualizer vis;
  vis.addPointCloud<pcl::PointXYZ>(trajectory, "trajectory");

  // for calculating FPS
  boost::circular_buffer<std::chrono::high_resolution_clock::time_point> stamps(30);
  stamps.push_back(std::chrono::high_resolution_clock::now());

  for(int i=1; i<kitti.size(); i++) {
    // set the current frame as source
    voxelgrid.setInputCloud(kitti.cloud(i));
    auto source = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    voxelgrid.filter(*source);
    gicp.setInputSource(source);

    // align and swap source and target cloud for next registration
    auto aligned = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    gicp.align(*aligned);
    gicp.swapSourceAndTarget();

    // accumulate pose
    poses[i] = poses[i - 1] * gicp.getFinalTransformation().cast<double>();

    // FPS display
    stamps.push_back(std::chrono::high_resolution_clock::now());
    std::cout << stamps.size() / (std::chrono::duration_cast<std::chrono::nanoseconds>(stamps.back() - stamps.front()).count() / 1e9) << "fps" << std::endl;

    // visualization
    trajectory->push_back(pcl::PointXYZ(poses[i](0, 3), poses[i](1, 3), poses[i](2, 3)));
    vis.updatePointCloud<pcl::PointXYZ>(trajectory, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(trajectory, 255.0, 0.0, 0.0), "trajectory");
    vis.spinOnce();
  }

  // save the estimated poses
  std::ofstream ofs("/tmp/traj.txt");
  for(const auto& pose: poses) {
    for(int i=0; i<3; i++) {
      for(int j=0; j<4; j++) {
        if(i || j) {
          ofs << " ";
        }

        ofs << pose(i, j);
      }
    }
    ofs << std::endl;
  }

  return 0;
}


#ifndef AIKIDO_ROBOT_IK_GRADIENTDESCENTIK_HPP_
#define AIKIDO_ROBOT_IK_GRADIENTDESCENTIK_HPP_

#include <vector>

#include <Eigen/Dense>
#include <dart/dart.hpp>

#include "aikido/constraint/Sampleable.hpp"
#include "aikido/robot/InverseKinematics.hpp"
#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"

namespace aikido {
namespace robot {

/// Optimization-based IK solver that uses gradient descent from random seed
// configurations.
class GradientDescentIk : public InverseKinematics
{
public:
  GradientDescentIk(
      ::dart::dynamics::MetaSkeletonPtr arm,
      ::dart::dynamics::BodyNodePtr endEffector,
      statespace::dart::ConstMetaSkeletonStateSpacePtr armSpace,
      common::RNG* rng);

  // Documentation inherited.
  std::vector<Eigen::VectorXd> getSolutions(
      const Eigen::Isometry3d& targetPose,
      const size_t maxSolutions) const override;

private:
  ::dart::dynamics::MetaSkeletonPtr mArm;
  ::dart::dynamics::BodyNodePtr mEndEffector;
  statespace::dart::ConstMetaSkeletonStateSpacePtr mArmSpace;

  std::shared_ptr<aikido::constraint::Sampleable> mSeedConfigSampleable;
  ::dart::dynamics::InverseKinematicsPtr mDartIkSolver;
};

} // namespace robot
} // namespace aikido

#endif // AIKIDO_ROBOT_IK_GRADIENTDESCENTIK_HPP_


/*
 Copyright (C) 2018 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>

#include <ql/errors.hpp>
#include <qle/instruments/commodityforward.hpp>

#include <ored/portfolio/builders/commodityforward.hpp>
#include <ored/portfolio/commodityforward.hpp>
#include <ored/portfolio/enginefactory.hpp>
#include <ored/utilities/to_string.hpp>

using QuantExt::CommodityIndex;
using QuantExt::PriceTermStructure;
using namespace QuantLib;
using namespace std;

namespace ore {
namespace data {

CommodityForward::CommodityForward() : Trade("CommodityForward"), quantity_(0.0), strike_(0.0) {}

CommodityForward::CommodityForward(const Envelope& envelope, const string& position, const string& commodityName,
                                   const string& currency, Real quantity, const string& maturityDate, Real strike)
    : Trade("CommodityForward", envelope), position_(position), commodityName_(commodityName), currency_(currency),
      quantity_(quantity), maturityDate_(maturityDate), strike_(strike) {}

CommodityForward::CommodityForward(const Envelope& envelope, const string& position, const string& commodityName,
                                   const string& currency, Real quantity, const string& maturityDate, Real strike,
                                   const Date& futureExpiryDate, const boost::optional<bool>& physicallySettled,
                                   const Date& paymentDate)
    : Trade("CommodityForward", envelope), position_(position), commodityName_(commodityName), currency_(currency),
      quantity_(quantity), maturityDate_(maturityDate), strike_(strike), isFuturePrice_(true),
      futureExpiryDate_(futureExpiryDate), physicallySettled_(physicallySettled), paymentDate_(paymentDate) {}

CommodityForward::CommodityForward(const Envelope& envelope, const string& position, const string& commodityName,
                                   const string& currency, Real quantity, const string& maturityDate, Real strike,
                                   const Period& futureExpiryOffset, const Calendar& offsetCalendar,
                                   const boost::optional<bool>& physicallySettled,
                                   const Date& paymentDate)
    : Trade("CommodityForward", envelope), position_(position), commodityName_(commodityName), currency_(currency),
      quantity_(quantity), maturityDate_(maturityDate), strike_(strike), isFuturePrice_(true),
      futureExpiryOffset_(futureExpiryOffset), offsetCalendar_(offsetCalendar), physicallySettled_(physicallySettled),
      paymentDate_(paymentDate) {}

void CommodityForward::build(const boost::shared_ptr<EngineFactory>& engineFactory) {

    // Create the underlying commodity index for the forward
    const boost::shared_ptr<Market>& market = engineFactory->market();
    auto index = *market->commodityIndex(commodityName_, engineFactory->configuration(MarketContext::pricing));
    maturity_ = parseDate(maturityDate_);
    if (isFuturePrice_ && *isFuturePrice_) {

        // Get the commodity index from the market.
        index = *market->commodityIndex(commodityName_, engineFactory->configuration(MarketContext::pricing));

        // May have been given an explicit future expiry date or an offset and calendar or neither.
        Date expiryDate = maturity_;
        if (futureExpiryDate_ != Date()) {
            expiryDate = futureExpiryDate_;
        } else if (futureExpiryOffset_ != Period()) {
            Calendar cal = offsetCalendar_.empty() ? NullCalendar() : offsetCalendar_;
            expiryDate = cal.advance(maturity_, futureExpiryOffset_);
        }

        // Clone the index with the relevant expiry date.
        index = index->clone(expiryDate);

    }

    Date paymentDate = paymentDate_;
    bool physicallySettled = true;
    if (physicallySettled_ && !(*physicallySettled_)) {
        // If cash settled and given a payment date that is not greater than the maturity date, set it equal to the
        // maturity date and log a warning to continue processing.
        physicallySettled = false;
        if (paymentDate_ != Date() && paymentDate_ < maturity_) {
            WLOG("Commodity forward " << id() << " has payment date (" << io::iso_date(paymentDate_) <<
                ") before the maturity date (" << io::iso_date(maturity_) << "). Setting payment date" <<
                " equal to the maturity date.");
            paymentDate = maturity_;
        }
    } else {
        // If physically settled and given a payment date, log a warning that it is ignored.
        if (paymentDate_ != Date()) {
            WLOG("Commodity forward " << id() << " supplies a payment date (" << io::iso_date(paymentDate_) <<
                ") but is physically settled. The payment date is ignored.");
            paymentDate = Date();
        }
    }

    // Create the commodity forward instrument
    Currency currency = parseCurrency(currency_);
    Position::Type position = parsePositionType(position_);
    boost::shared_ptr<Instrument> commodityForward = boost::make_shared<QuantExt::CommodityForward>(
        index, currency, position, quantity_, maturity_, strike_, physicallySettled, paymentDate);

    // Pricing engine
    boost::shared_ptr<EngineBuilder> builder = engineFactory->builder(tradeType_);
    QL_REQUIRE(builder, "No builder found for " << tradeType_);
    boost::shared_ptr<CommodityForwardEngineBuilder> commodityForwardEngineBuilder =
        boost::dynamic_pointer_cast<CommodityForwardEngineBuilder>(builder);
    commodityForward->setPricingEngine(commodityForwardEngineBuilder->engine(currency));

    // set up other Trade details
    instrument_ = boost::make_shared<VanillaInstrument>(commodityForward);
    npvCurrency_ = currency_;

    // notional_ = strike_ * quantity_;
    notional_ = Null<Real>(); // is handled by override of notional()
    notionalCurrency_ = currency_;

    additionalData_["quantity"] = quantity_;
    additionalData_["strike"] = strike_;
    additionalData_["strikeCurrency"] = currency_;
}

Real CommodityForward::notional() const {
    // try to get the notional from the additional results of the instrument
    try {
        return instrument_->qlInstrument()->result<Real>("currentNotional");
    } catch (const std::exception& e) {
        if (strcmp(e.what(), "currentNotional not provided"))
            ALOG("error when retrieving notional: " << e.what());
    }
    // if not provided, return null
    return Null<Real>();
}

std::map<AssetClass, std::set<std::string>>
CommodityForward::underlyingIndices(const boost::shared_ptr<ReferenceDataManager>& referenceDataManager) const {
    return {{AssetClass::COM, std::set<std::string>({commodityName_})}};
}

void CommodityForward::fromXML(XMLNode* node) {

    Trade::fromXML(node);
    XMLNode* commodityDataNode = XMLUtils::getChildNode(node, "CommodityForwardData");

    position_ = XMLUtils::getChildValue(commodityDataNode, "Position", true);
    commodityName_ = XMLUtils::getChildValue(commodityDataNode, "Name", true);
    currency_ = XMLUtils::getChildValue(commodityDataNode, "Currency", true);
    quantity_ = XMLUtils::getChildValueAsDouble(commodityDataNode, "Quantity", true);
    maturityDate_ = XMLUtils::getChildValue(commodityDataNode, "Maturity", true);
    strike_ = XMLUtils::getChildValueAsDouble(commodityDataNode, "Strike", true);

    isFuturePrice_ = boost::none;
    if (XMLNode* n = XMLUtils::getChildNode(commodityDataNode, "IsFuturePrice"))
        isFuturePrice_ = parseBool(XMLUtils::getNodeValue(n));

    futureExpiryDate_ = Date();
    if (XMLNode* n = XMLUtils::getChildNode(commodityDataNode, "FutureExpiryDate"))
        futureExpiryDate_ = parseDate(XMLUtils::getNodeValue(n));

    // If not given an explicit future expiry date, check for offset and calendar.
    if (futureExpiryDate_ == Date()) {
        futureExpiryOffset_ = Period();
        if (XMLNode* n = XMLUtils::getChildNode(commodityDataNode, "FutureExpiryOffset"))
            futureExpiryOffset_ = parsePeriod(XMLUtils::getNodeValue(n));

        offsetCalendar_ = Calendar();
        if (XMLNode* n = XMLUtils::getChildNode(commodityDataNode, "FutureExpiryOffsetCalendar"))
            offsetCalendar_ = parseCalendar(XMLUtils::getNodeValue(n));
    }

    physicallySettled_ = boost::none;
    if (XMLNode* n = XMLUtils::getChildNode(commodityDataNode, "PhysicallySettled"))
        physicallySettled_ = parseBool(XMLUtils::getNodeValue(n));

    paymentDate_ = Date();
    if (XMLNode* n = XMLUtils::getChildNode(commodityDataNode, "PaymentDate"))
        paymentDate_ = parseDate(XMLUtils::getNodeValue(n));
}

XMLNode* CommodityForward::toXML(XMLDocument& doc) {

    XMLNode* node = Trade::toXML(doc);
    XMLNode* commodityDataNode = doc.allocNode("CommodityForwardData");
    XMLUtils::appendNode(node, commodityDataNode);

    XMLUtils::addChild(doc, commodityDataNode, "Position", position_);
    XMLUtils::addChild(doc, commodityDataNode, "Maturity", maturityDate_);
    XMLUtils::addChild(doc, commodityDataNode, "Name", commodityName_);
    XMLUtils::addChild(doc, commodityDataNode, "Currency", currency_);
    XMLUtils::addChild(doc, commodityDataNode, "Strike", strike_);
    XMLUtils::addChild(doc, commodityDataNode, "Quantity", quantity_);

    if (isFuturePrice_)
        XMLUtils::addChild(doc, commodityDataNode, "IsFuturePrice", *isFuturePrice_);

    if (futureExpiryDate_ != Date())
        XMLUtils::addChild(doc, commodityDataNode, "FutureExpiryDate", to_string(futureExpiryDate_));

    if (futureExpiryOffset_ != Period())
        XMLUtils::addChild(doc, commodityDataNode, "FutureExpiryOffset", to_string(futureExpiryOffset_));

    if (offsetCalendar_ != Calendar())
        XMLUtils::addChild(doc, commodityDataNode, "FutureExpiryOffsetCalendar", to_string(offsetCalendar_));

    if (physicallySettled_)
        XMLUtils::addChild(doc, commodityDataNode, "PhysicallySettled", *physicallySettled_);

    if (paymentDate_ != Date())
        XMLUtils::addChild(doc, commodityDataNode, "PaymentDate", to_string(paymentDate_));

    return node;
}

} // namespace data
} // namespace ore