/**
 * Copyright (c) 2020 Neka-Nat
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * IN THE SOFTWARE.
**/
#include "cupoch/visualization/utility/gl_helper.h"

#include <Eigen/Dense>
#include <cmath>

namespace cupoch {
namespace visualization {
namespace gl_helper {

GLMatrix4f LookAt(const Eigen::Vector3f &eye,
                  const Eigen::Vector3f &lookat,
                  const Eigen::Vector3f &up) {
    Eigen::Vector3f front_dir = (eye - lookat).normalized();
    Eigen::Vector3f up_dir = up.normalized();
    Eigen::Vector3f right_dir = up_dir.cross(front_dir).normalized();
    up_dir = front_dir.cross(right_dir).normalized();

    Eigen::Matrix4f mat = Eigen::Matrix4f::Zero();
    mat.block<1, 3>(0, 0) = right_dir.transpose();
    mat.block<1, 3>(1, 0) = up_dir.transpose();
    mat.block<1, 3>(2, 0) = front_dir.transpose();
    mat(0, 3) = -right_dir.dot(eye);
    mat(1, 3) = -up_dir.dot(eye);
    mat(2, 3) = -front_dir.dot(eye);
    mat(3, 3) = 1.0;
    return mat.cast<GLfloat>();
}

GLMatrix4f Perspective(float field_of_view_,
                       float aspect,
                       float z_near,
                       float z_far) {
    Eigen::Matrix4f mat = Eigen::Matrix4f::Zero();
    float fov_rad = field_of_view_ / 180.0 * M_PI;
    float tan_half_fov = std::tan(fov_rad / 2.0);
    mat(0, 0) = 1.0 / aspect / tan_half_fov;
    mat(1, 1) = 1.0 / tan_half_fov;
    mat(2, 2) = -(z_far + z_near) / (z_far - z_near);
    mat(3, 2) = -1.0;
    mat(2, 3) = -2.0 * z_far * z_near / (z_far - z_near);
    return mat.cast<GLfloat>();
}

GLMatrix4f Ortho(float left,
                 float right,
                 float bottom,
                 float top,
                 float z_near,
                 float z_far) {
    Eigen::Matrix4f mat = Eigen::Matrix4f::Zero();
    mat(0, 0) = 2.0 / (right - left);
    mat(1, 1) = 2.0 / (top - bottom);
    mat(2, 2) = -2.0 / (z_far - z_near);
    mat(0, 3) = -(right + left) / (right - left);
    mat(1, 3) = -(top + bottom) / (top - bottom);
    mat(2, 3) = -(z_far + z_near) / (z_far - z_near);
    mat(3, 3) = 1.0;
    return mat.cast<GLfloat>();
}

Eigen::Vector3f Project(const Eigen::Vector3f &point,
                        const GLMatrix4f &mvp_matrix,
                        const int width,
                        const int height) {
    Eigen::Vector4f pos = mvp_matrix.cast<float>() *
                          Eigen::Vector4f(point(0), point(1), point(2), 1.0);
    if (pos(3) == 0.0) {
        return Eigen::Vector3f::Zero();
    }
    pos /= pos(3);
    return Eigen::Vector3f((pos(0) * 0.5 + 0.5) * (float)width,
                           (pos(1) * 0.5 + 0.5) * (float)height,
                           (1.0 + pos(2)) * 0.5);
}

Eigen::Vector3f Unproject(const Eigen::Vector3f &screen_point,
                          const GLMatrix4f &mvp_matrix,
                          const int width,
                          const int height) {
    Eigen::Vector4f point =
            mvp_matrix.cast<float>().inverse() *
            Eigen::Vector4f(screen_point(0) / (float)width * 2.0 - 1.0,
                            screen_point(1) / (float)height * 2.0 - 1.0,
                            screen_point(2) * 2.0 - 1.0, 1.0);
    if (point(3) == 0.0) {
        return Eigen::Vector3f::Zero();
    }
    point /= point(3);
    return point.block<3, 1>(0, 0);
}

int ColorCodeToPickIndex(const Eigen::Vector4i &color) {
    if (color(0) == 255) {
        return -1;
    } else {
        return ((color(0) * 256 + color(1)) * 256 + color(2)) * 256 + color(3);
    }
}

}  // namespace gl_helper
}  // namespace visualization
}  // namespace cupoch


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
cpp_int Combination(int n, int r) {
    cpp_int u = 1, d = 1;
    for (int i = 2; i <= n; i++) {
        u *= i;
        if (i <= n - r) d *= i;
    }
    for (int i = 2; i <= r; i++) d *= i;
    return u / d;
}
int main() {
    int l; cin >> l;
    cpp_int ans = Combination(l - 1, 11);
    cout << ans << endl;
}


#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>

#include <opencv2/opencv.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/transforms.h>

#include "imgProc.hpp"
#include "sfm.hpp"
#include "tapl/pte/ptEngine.hpp"

inline double deg2rad(double deg) { return deg * M_PI / 180.0; }
inline double rad2deg(double rad) { return rad * 180.0 / M_PI; }

/**
 * Estimate four possible R and T from the Essential Matrix
 */
std::vector<Eigen::MatrixXd> computeInitialRTfromE(const Eigen::MatrixXd &E) {

    // SVD decomposition
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(E, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // build 'Z' and 'W'
    Eigen::MatrixXd Z(3,3);
    Z << 0, 1, 0,
        -1, 0, 0,
         0, 0, 0;
    Eigen::MatrixXd W(3,3);
    W << 0, -1, 0,
         1,  0, 0,
         0,  0, 1;

    // E can be re-written as E=MQ; where M=U.Z.U^T and Q=U.W.V^T or Q=U.W^T.V^T
    Eigen::MatrixXd M = svd.matrixU() * Z * svd.matrixU().transpose();
    Eigen::MatrixXd Q1 = svd.matrixU() * W * svd.matrixV().transpose();
    Eigen::MatrixXd Q2 = svd.matrixU() * W.transpose() * svd.matrixV().transpose();

    // R can be computed as R=(det Q)Q
    Eigen::MatrixXd R1 = Q1.determinant() * Q1;
    Eigen::MatrixXd R2 = Q2.determinant() * Q2;

    // E = UΣV^T, T is simply either u3 or −u3, where u3 is the third column vector of U
    Eigen::MatrixXd T1 = svd.matrixU().col(2);
    Eigen::MatrixXd T2 = -svd.matrixU().col(2);

    // compose four possible RT
    std::vector<Eigen::MatrixXd> RT;
    // R1 and T1
    Eigen::MatrixXd R1T1(3,4);
    R1T1.block<3,3>(0,0) = R1;
    R1T1.block<3,1>(0,3) = T1;
    RT.push_back(R1T1);
    // R1 and T2
    Eigen::MatrixXd R1T2(3,4);
    R1T2.block<3,3>(0,0) = R1;
    R1T2.block<3,1>(0,3) = T2;
    RT.push_back(R1T2);
    // R2 and T1
    Eigen::MatrixXd R2T1(3,4);
    R2T1.block<3,3>(0,0) = R2;
    R2T1.block<3,1>(0,3) = T1;
    RT.push_back(R2T1);
    // R2 and T2
    Eigen::MatrixXd R2T2(3,4);
    R2T2.block<3,3>(0,0) = R2;
    R2T2.block<3,1>(0,3) = T2;
    RT.push_back(R2T2);

    // return
    return RT;
}

/**
 * Linear estimate of the 3d point
 */
tapl::Point3d tapl::cve::StructureFromMotion::linearEstimate3dPt(
                    const std::vector<tapl::Point2d> &point2d,
                    const std::vector<Eigen::MatrixXd> &projectionMatrices ) {
    // We can re-write [p x MP = 0] in the form [AP = 0] and solve for P
    // by decomposing A using SVD
    // let's formulate the A matrix
    Eigen::MatrixXd A(point2d.size()*2, 4);
    for (auto i=0; i<point2d.size(); ++i) {
        A.row(i*2) << (point2d.at(i).x * projectionMatrices.at(i).row(2)) -
                                    projectionMatrices.at(i).row(0);
        A.row((i*2)+1) << (point2d.at(i).y * projectionMatrices.at(i).row(2)) -
                                    projectionMatrices.at(i).row(1);
    }
    // SVD decomposition
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
    // P can be obtained as the last column of v or last row of v_transpose
    Eigen::MatrixXd pt3d_homogeneous = svd.matrixV().col(3);
    // homogeneous to cartesian coordinate conversion
    tapl::Point3d pt3d( pt3d_homogeneous(0,0) / pt3d_homogeneous(3,0),
                        pt3d_homogeneous(1,0) / pt3d_homogeneous(3,0),
                        pt3d_homogeneous(2,0) / pt3d_homogeneous(3,0) );
    return pt3d;
}

/**
 * Non-linear estimate of the 3d point
 */
tapl::Point3d tapl::cve::StructureFromMotion::nonLinearEstimate3dPt(
                    const std::vector<tapl::Point2d> &point2d,
                    const std::vector<Eigen::MatrixXd> &projectionMatrices,
                    std::pair<std::vector<float>,std::vector<float>> &reprojectionErrors,
                    const uint16_t nIterations,
                    const float reprErrorThresh ) {
    // compute linear estimate of the 3d point
    tapl::Point3d pt3d_initial = linearEstimate3dPt(point2d, projectionMatrices);
    // perform non-linear least squares optimization
    tapl::Point3d pt3d_optimize;
    reprojectionErrors = \
        gnOptim.optimize(pt3d_initial, point2d, projectionMatrices, nIterations, reprErrorThresh, pt3d_optimize);
    return pt3d_optimize;
}

/**
 * Estimate R, T, and triengulated points
 */
std::pair<Eigen::MatrixXd, std::vector<tapl::Point3d>>
    tapl::cve::StructureFromMotion::computeSFM(
                    const Eigen::MatrixXd &E,
                    const std::vector<std::vector<tapl::Point2d>> &points2d,
                    const Eigen::MatrixXd &projectionMat1,
                    const float &maxReprojectionErr ) {
    // compute four possible RT
    std::vector<Eigen::MatrixXd> RT = computeInitialRTfromE(E);
    // projection matrices
    std::vector<Eigen::MatrixXd> projectionMatrices(2);
    projectionMatrices.at(0) = projectionMat1;
    // set of triangulated 3d points
    std::vector<std::vector<tapl::Point3d>> pointsTriangulatedRef(4, std::vector<tapl::Point3d>(points2d.size()));
    std::vector<std::vector<tapl::Point3d>> pointsTriangulated(4, std::vector<tapl::Point3d>(points2d.size()));
    // reprojection errors
    std::vector<std::vector<std::pair<std::vector<float>,std::vector<float>>>>
        reprojectionErrors(4, std::vector<std::pair<std::vector<float>,std::vector<float>>>(points2d.size()));
    // iterate over 4 possible R & T
    for (auto it1=RT.begin(); it1!= RT.end(); ++it1) {
        auto rtIdx = std::distance(RT.begin(), it1);
        // build projection matrix
        Eigen::MatrixXd projectionMat2 = projectionMat1 * (*it1);
        // TODO: Update the implementation so that we have 1 projection matrix per camera
        projectionMatrices.at(1) = projectionMat2;
        // go through each point and triangulate it through multiple-views
        for (auto it2=points2d.begin(); it2!=points2d.end(); ++it2) {
            auto ptIdx = std::distance(points2d.begin(), it2);
            // triangulate 3d point
            std::pair<std::vector<float>,std::vector<float>> reprojectionError;
            auto pt3d = this->nonLinearEstimate3dPt(*it2, projectionMatrices, reprojectionError);
            reprojectionErrors[rtIdx][ptIdx] = reprojectionError;
            pointsTriangulatedRef[rtIdx][ptIdx] = pt3d;

            // convert to homogeneous coordinate system
            Eigen::MatrixXd point3dHomogeneous(4,1);
            point3dHomogeneous << pt3d.x, pt3d.y, pt3d.z, 1.0;
            auto ptsTransformed = (*it1) * point3dHomogeneous;
            pointsTriangulated[rtIdx][ptIdx] = *(new tapl::Point3d(ptsTransformed(0), ptsTransformed(1), ptsTransformed(2)));
        }
    }

    // for each RT, count number of points that fall in front of all cameras.
    // the one with maximum number of points will correspond to the correct RT
    size_t correctIdx = 0;
    uint16_t maxCount = 0;
    for (auto i=0; i<RT.size(); ++i) {
        uint16_t count = 0;
        for (auto j=0; j<points2d.size(); ++j) {
            if ((pointsTriangulatedRef[i][j].z > 0.0) &&
                (pointsTriangulated[i][j].z > 0.0)) {
                   count ++;
               }
        }
        if (count > maxCount) {
            maxCount = count;
            correctIdx = i;
        }
    }

    Eigen::MatrixXd correctRT = RT[correctIdx];
    std::vector<tapl::Point3d> correctTriangulatedPtsRef = pointsTriangulatedRef[correctIdx];
    std::vector<tapl::Point3d> correctTriangulatedPts = pointsTriangulated[correctIdx];

    // use PnP to get refined pose
    // get pose using ransac
    cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);
    cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);
    const int iterationsCount = 500;       // number of Ransac iterations. default 100
    const float reprojectionError = 1.0;    // maximum allowed distance to consider it an inlier. default 8.0
    const float confidence = 0.99;          // RANSAC successful confidence. default 0.99
    const bool useExtrinsicGuess = true;   // default false
    const int flags = cv::SOLVEPNP_ITERATIVE;
    cv::Mat inliers;
    std::vector<cv::Point2f> kpts2d_;
    std::vector<cv::Point3f> kpts3d_;
    std::vector<uint16_t> idxRemove;
    for (auto i=0; i<points2d.size(); ++i) {
        // filter out the points outside of region-of-interest and the points with high reprojection error
        if ((correctTriangulatedPts[i].z > 0.0) && (correctTriangulatedPtsRef[i].z > 0.0) &&
            (correctTriangulatedPtsRef[i].z > this->minXYZ.at(2)) && (correctTriangulatedPtsRef[i].z < this->maxXYZ.at(2)) &&
            (fabs(correctTriangulatedPtsRef[i].x) > this->minXYZ.at(0)) && (fabs(correctTriangulatedPtsRef[i].x) < this->maxXYZ.at(0)) &&
            (fabs(correctTriangulatedPtsRef[i].y) > this->minXYZ.at(1)) && (fabs(correctTriangulatedPtsRef[i].y) < this->maxXYZ.at(1)) &&
            reprojectionErrors[correctIdx][i].second.at(0) < maxReprojectionErr ) { // post-optimization reprojection error in the first camera frame

            // compute epipolar lines
            Eigen::MatrixXf pt2(3,1);
            pt2 << points2d[i][1].x, points2d[i][1].y, 1.0;
            Eigen::MatrixXf epipolarLine1 = (E.transpose().cast <float> ()) * pt2.cast <float> ();
            epipolarLine1 = epipolarLine1 / epipolarLine1(2,0); // normalize
            // compute distance between epipolar line to the point in the other image
            auto dist2ep = fabs(static_cast<float>(epipolarLine1(0,0)) * points2d[i][0].x +
                                static_cast<float>(epipolarLine1(1,0)) * points2d[i][0].y +
                                static_cast<float>(epipolarLine1(2,0))) /
                            sqrt(pow(static_cast<float>(epipolarLine1(0,0)), 2) +
                                 pow(static_cast<float>(epipolarLine1(1,0)), 2));
            // if (dist2ep < 50.0) {
            // points in the camera plane for which the pose needs to be computed
            kpts2d_.push_back(cv::Point2f(points2d[i][1].x, points2d[i][1].y));
            // 3d points in the coordinate of reference camera frame
            kpts3d_.push_back(cv::Point3f(correctTriangulatedPtsRef[i].x, correctTriangulatedPtsRef[i].y, correctTriangulatedPtsRef[i].z));
            // }
            // else idxRemove.push_back(i);
        }
        else idxRemove.push_back(i);
    }
    // remove outliers
    for (auto it_idx=idxRemove.end()-1; it_idx!=idxRemove.begin()-1; --it_idx) {
        correctTriangulatedPtsRef.erase(correctTriangulatedPtsRef.begin()+(*it_idx));
    }
    if(kpts3d_.size() >= 6) {
        cv::solvePnPRansac(kpts3d_, kpts2d_, this->K, this->dist_coeff, rvec, tvec,
                            useExtrinsicGuess, iterationsCount, reprojectionError, confidence,
                            inliers, flags );
        float inliers_ratio = static_cast<float>(inliers.size().height) / static_cast<float>(kpts3d_.size());
        // TLOG_DEBUG << "Inliers ratio [" << kpts3d_.size() << "/" << inliers.size().height << "] = [" << inliers_ratio << "]";

        // get global pose in world reference frame
        // check for NaNs
        rvec.convertTo(rvec, CV_32F);
        tvec.convertTo(tvec, CV_32F);
        cv::patchNaNs(rvec, 0.0); // replace NaN with 0.0
        cv::patchNaNs(tvec, 0.0); // replace NaN with 0.0
        // rotation matrix
        cv::Mat R;
        cv::Rodrigues(rvec, R);
        // translation matrix
        cv::Mat t = cv::Mat::eye(4,4, CV_32FC1);
        tvec.copyTo(t(cv::Rect(3,0,1,3)));
        // build an RT matrix
        Eigen::MatrixXd R_eigen(3,3);
        cv2eigen(R, R_eigen);
        Eigen::MatrixXd RT_pnp(3,4);
        RT_pnp.block<3,3>(0,0) << R_eigen;
        RT_pnp.block<3,1>(0,3) << tvec.at<float>(0,0), tvec.at<float>(0,1), tvec.at<float>(0,2);

        return std::pair<Eigen::MatrixXd, std::vector<tapl::Point3d>>(RT_pnp, correctTriangulatedPtsRef);
    }
    else {
        TLOG_WARN << "PnP failed";
        return std::pair<Eigen::MatrixXd, std::vector<tapl::Point3d>>(correctRT, correctTriangulatedPtsRef);
    }
}

/**
 * Structure-from-Motion constructor
 */
tapl::cve::StructureFromMotion::StructureFromMotion(
                                     const std::vector<cv::Mat> &images,
                                     const cv::Mat &K,
                                     const std::vector<float> &minXYZ,
                                     const std::vector<float> &maxXYZ,
                                     const bool verbose) {
    // copy inputs to private variable
    this->images = images;
    this->K = K;
    this->minXYZ = minXYZ;
    this->maxXYZ = maxXYZ;
    this->verbose = verbose;
    // make bundles of 'm' images
}

/**
 * Structure-from-Motion implementation
 */
tapl::ResultCode tapl::cve::StructureFromMotion::process(
                                std::vector<tapl::Point3dColor> &points,
                                std::vector<tapl::Pose6dof> &poses,
                                std::vector<tapl::CameraPairs> &framePairs) {

    // global pose
    Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
    // Go through each camera frame
    for (auto it=this->images.begin()+1; it!=this->images.end(); ++it) {
        if (this->verbose) TLOG_INFO << "processing frames [" <<
                                        std::distance(images.begin(), it) <<
                                        "] and [" <<
                                        std::distance(images.begin(), it+1) << "]";

        tapl::CameraPairs camPairs(*(it-1), *it, this->K);

        // compute fundamental matrix
        if (tapl::cve::computeFundamentalMatrix(camPairs) != tapl::SUCCESS) {
            TLOG_ERROR << "could not compute fundamental matrix";
            return tapl::FAILURE;
        }

        // retrieve fundamental matrix
        cv::Mat F;
        if (camPairs.getFundamentalMatrix(F) !=  tapl::SUCCESS) {
            TLOG_ERROR << "could not retrieve fundamental matrix";
            return tapl::FAILURE;
        }

        // compute essential matrix
        cv::Mat E = this->K.t() * F * this->K;

        // get keypoints
        std::vector<cv::KeyPoint> kpts1;
        if (camPairs.first->getKeypoints(kpts1) !=  tapl::SUCCESS) {
            TLOG_ERROR << "could not retrieve keypoints";
            return tapl::FAILURE;
        }
        std::vector<cv::KeyPoint> kpts2;
        if (camPairs.second->getKeypoints(kpts2) !=  tapl::SUCCESS) {
            TLOG_ERROR << "could not retrieve keypoints";
            return tapl::FAILURE;
        }

        // get keypoints matches
        std::vector<cv::DMatch> kptMatches;
        if (camPairs.getKptsMatches(kptMatches) !=  tapl::SUCCESS) {
            TLOG_ERROR << "could not retrieve keypoints matches";
            return tapl::FAILURE;
        }

        // build vector of 'n' points in 'm' camera frames (n x m - Point2d)
        std::vector<std::vector<tapl::Point2d>> kptSet;
        std::vector<tapl::Point2d> trackedKpts;
        for (auto it_match=kptMatches.begin(); it_match!=kptMatches.end(); ++it_match) {
            if (it_match->distance < 100.0) {
                tapl::Point2d pt1 = tapl::Point2d( kpts1.at((*it_match).queryIdx).pt.x,
                                                   kpts1.at((*it_match).queryIdx).pt.y );
                tapl::Point2d pt2 = tapl::Point2d( kpts2.at((*it_match).trainIdx).pt.x,
                                                   kpts2.at((*it_match).trainIdx).pt.y );
                trackedKpts.push_back(pt1);
                // TODO: compute corresponding lines, find and reject outliers

                // this point in all cameras
                std::vector<tapl::Point2d> ptCameras = {pt1, pt2};
                kptSet.push_back(ptCameras);
            }
        }

        // draw the matches
        cv::Mat matchImg;
        cv::drawMatches(*(it-1), kpts1, *(it), kpts2,
                        kptMatches, matchImg,
                        cv::Scalar::all(-1), cv::Scalar::all(-1),
                        std::vector<char>(), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);

        // convert E to eigen format
        Eigen::MatrixXd eigenE(3,3);
        cv2eigen(E, eigenE);
        // convert K to eigen format
        Eigen::MatrixXd eigenK(3,3);
        Eigen::MatrixXd projectionMat1(3,4);
        cv2eigen(this->K, eigenK);
        projectionMat1.block<3,3>(0,0) = eigenK;
        projectionMat1(3,3) = 1.0;
        auto sfm = computeSFM(eigenE, kptSet, projectionMat1);
        Eigen::MatrixXd RT = sfm.first;
        std::vector<tapl::Point3d> triangulatedPts = sfm.second;
        // Get point color
        std::vector<tapl::Point3dColor> triangulatedPtsColor;
        cv::Mat imgProj = cv::Mat::zeros((it-1)->rows, (it-1)->cols, CV_8UC3);
        for ( auto &pt3d : triangulatedPts) {
            Eigen::MatrixXd eigenPt3d(3,1);
            eigenPt3d << pt3d.x, pt3d.y, pt3d.z;
            auto eigenPt2dHomogeneous = eigenK * eigenPt3d;
            float pxX, pxY;
            pxX = eigenPt2dHomogeneous(0,0) / eigenPt2dHomogeneous(2,0);
            pxY = eigenPt2dHomogeneous(1,0) / eigenPt2dHomogeneous(2,0);
            // check if the coordinate is within limits
            cv::Vec3b rgb;
            if ((pxX >= 0) && (pxX < (it-1)->cols) &&
                (pxY >= 0) && (pxY < (it-1)->rows)) {
                rgb = (it-1)->at<cv::Vec3b>(static_cast<int>(pxY),static_cast<int>(pxX));
                imgProj.at<cv::Vec3b>(static_cast<int>(pxY),static_cast<int>(pxX)) = rgb;
            }
            else rgb = cv::Vec3b(0, 0, 0);
            tapl::Point3dColor pt3dColor( pt3d.x, pt3d.y, pt3d.z, static_cast<uint8_t>(rgb[0]),
                                          static_cast<uint8_t>(rgb[1]), static_cast<uint8_t>(rgb[2]));
            triangulatedPtsColor.push_back(pt3dColor);
        }

        // plot
        Eigen::Matrix4f P = Eigen::Matrix4f::Identity();
        P.block<3,4>(0,0) = RT.cast<float>();
        Eigen::Matrix4f Pi = Eigen::Matrix4f::Identity();
        Pi.block<3,3>(0,0) = P.block<3,3>(0,0).transpose();
        Pi.block<3,1>(0,3) = -P.block<3,3>(0,0).transpose() * P.block<3,1>(0,3);
        globalPose = globalPose * Pi;

        // compute pose
        cv::Mat cvPose;
        eigen2cv(globalPose, cvPose);
        tapl::Pose6dof pose(cvPose);
        // add to camera
        camPairs.pushPose(pose);
        camPairs.pushTrackedKpts(trackedKpts);
        camPairs.pushTriangulatedPts(triangulatedPtsColor);
        // push to the output vector
        poses.push_back(pose);
        for (auto &point : triangulatedPtsColor) points.push_back(point);
        // push camera pairs
        framePairs.push_back(camPairs);
    }

    // return success
    return tapl::SUCCESS;
}


/**
 * \file dcs/math/stats/distribution/pareto.hpp
 *
 * \brief The Pareto probability distribution.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_STATS_DISTRIBUTION_PARETO_HPP
#define DCS_MATH_STATS_DISTRIBUTION_PARETO_HPP

#include <dcs/detail/config/boost.hpp>

#if !DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(103500) // 1.35
# 	error "Required Boost library version >= 1.35"
#endif

#include <boost/math/distributions/pareto.hpp>
#include <cmath>
#include <cstddef>
#include <dcs/math/policies/policy.hpp>
#include <dcs/math/random/uniform_01_adaptor.hpp>
#include <iostream>
#include <vector>

namespace dcs { namespace math { namespace stats {

/**
 * \brief The Pareto distribution with shape parameter \f$\alpha\f$ and scale
 *  parameter \f$\beta\f$.
 *
 * \tparam RealT The type used for real numbers.
 * \tparam PolicyT The policy type.
 *
 * The probability density function (pdf):
 * \f[
 *   \Pr(x|\lambda) = \frac{\alpha\,\beta^\alpha}{x^{\alpha+1}}, \quad
 *                    \text{ for }x>\beta
 * \f]
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template < typename RealT=double, typename PolicyT=::dcs::math::policies::policy<> >
class pareto_distribution
{
	public: typedef RealT support_type;
	public: typedef RealT value_type;
	public: typedef PolicyT policy_type;

	public: explicit pareto_distribution(support_type shape=1, support_type scale=1)
		: dist_(shape, scale)
	{
		// empty
	}

	// compiler-generated copy ctor and assignment operator are fine

	/**
	 * \brief Generate a random number distributed according to this
	 * pareto distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \return A random number distributed according to this pareto
	 * distribution.
	 *
	 * A \c pareto random number distribution produces random numbers
	 * \f$x > 0\f$ distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|\alpha) = \frac{\alpha\,k^\alpha}{x^{\alpha+1}}, \quad
	 *                   \text{ for }x>k
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		support_type rand(UniformRandomGeneratorT& rng) const
	{
		::dcs::math::random::uniform_01_adaptor<UniformRandomGeneratorT&, support_type> eng(rng);

		// Use the inversion method:
    	//    x=\frac{k}{(1-p)^{1/\alpha}}
    	// => x=k(1-p)^{-1/\alpha}
    	// => x=ku^{-1/\alpha}, where u is a uniform random number in [0,1)

#if DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(104000)
		return dist_.scale()*::std::pow(value_type(1)-eng(), -value_type(1)/dist_.shape());
//		return dist_.scale()*::std::pow(eng(), -value_type(1)/dist_.shape());
#else
		return dist_.location()*::std::pow(value_type(1)-eng(), -value_type(1)/dist_.shape());
//		return dist_.location()*::std::pow(eng(), -value_type(1)/dist_.shape());
#endif // DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION
	}

	/**
	 * \brief Generate a vector of random numbers distributed according to this
	 * pareto distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \param n The number of random numbers to generate.
	 * \return A vector of random numbers distributed according to this
	 * pareto distribution.
	 *
	 * A \c pareto random number distribution produces random numbers
	 * \f$x > 0\f$ distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|\lambda) = \frac{\alpha\,\beta^\alpha}{x^{\alpha+1}}, \quad
	 *                    \text{ for }x>\beta
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		::std::vector<support_type> rand(UniformRandomGeneratorT& rng, ::std::size_t n)
	{
		::std::vector<support_type> rnds(n);

        for ( ; n > 0; --n)
		{
			rnds.push_back(rand());
		}

		return rnds;
	}

	public: support_type shape() const
	{
		return dist_.shape();
	}

	public: support_type scale() const
	{
#if DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(104000)
		return dist_.scale();
#else
		return dist_.location();
#endif // DCS_DETAIL_CONFIG_BOOST_VERSION
	}

	public: support_type quantile(value_type p) const
	{
		return ::boost::math::quantile(dist_, p);
	}

	public: value_type mean() const
	{
#if DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(104000)
		return dist_.scale()*dist_.shape()/(dist_.shape()-1);
#else
		return dist_.location()*dist_.shape()/(dist_.shape()-1);
#endif // DCS_DETAIL_CONFIG_BOOST_VERSION
	}

	public: value_type variance() const
	{
#if DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(104000)
		return dist_.scale()*dist_.scale()*dist_.shape()/((dist_.shape()-1)*(dist_.shape()-2));
#else
		return dist_.location()*dist_.location()*dist_.shape()/((dist_.shape()-1)*(dist_.shape()-2));
#endif // DCS_DETAIL_CONFIG_BOOST_VERSION
	}

	private: ::boost::math::pareto_distribution<value_type,policy_type> dist_;
};

template <
	typename CharT,
	typename CharTraitsT,
	typename RealT,
	typename PolicyT
>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, pareto_distribution<RealT,PolicyT> const& dist)
{
	return os << "Pareto("
			  << "shape=" <<  dist.shape()
			  << ", scale=" <<  dist.scale()
			  << ")";
}

}}} // Namespace dcs::math::stats

#endif // DCS_MATH_STATS_DISTRIBUTION_PARETO_HPP


/**
 * @file    LossFunction.cpp
 * @brief   Gaussian loss function with mahalanobis distance, and robust loss
 * @author  Jing Dong
 * @date    Oct 14, 2017
 */

#include <minisam/core/LossFunction.h>

#include <Eigen/Dense>

namespace minisam {

/* ************************************************************************** */
std::shared_ptr<LossFunction> GaussianLoss::SqrtInformation(
    const Eigen::MatrixXd& R) {
  assert(R.rows() == R.cols() &&
         "[GaussianLoss::SqrtInformation] non-square sqrt-root matrix");
  return std::shared_ptr<LossFunction>(new GaussianLoss(R));
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> GaussianLoss::Information(
    const Eigen::MatrixXd& I) {
  assert(I.rows() == I.cols() &&
         "[GaussianLoss::Information] non-square information matrix");
  Eigen::LLT<Eigen::MatrixXd> llt(I.selfadjointView<Eigen::Upper>());
  return std::shared_ptr<LossFunction>(new GaussianLoss(llt.matrixU()));
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> GaussianLoss::Covariance(
    const Eigen::MatrixXd& Sigma) {
  assert(Sigma.rows() == Sigma.cols() &&
         "[GaussianLoss::Covariance] non-square covariance matrix");
  return Information(Sigma.inverse());
}

/* ************************************************************************** */
void GaussianLoss::print(std::ostream& out) const {
  out << "Gaussian loss function : R =" << std::endl << sqrt_info_ << std::endl;
}

/* ************************************************************************** */
void GaussianLoss::weightInPlace(Eigen::VectorXd& b) const {
  assert(sqrt_info_.cols() == b.size() &&
         "[GaussianLoss::weightInPlace] error size wrong");
  b = sqrt_info_ * b;
}

/* ************************************************************************** */
void GaussianLoss::weightInPlace(std::vector<Eigen::MatrixXd>& As,
                                 Eigen::VectorXd& b) const {
  assert(sqrt_info_.cols() == b.size() &&
         "[GaussianLoss::weightInPlace] error size wrong");
  b = sqrt_info_ * b;
  for (auto& A : As) {
    assert(sqrt_info_.cols() == A.rows() &&
           "[GaussianLoss::weightInPlace] jacobian size wrong");
    A = sqrt_info_ * A;
  }
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> DiagonalLoss::Precisions(
    const Eigen::VectorXd& I_diag) {
  return std::shared_ptr<LossFunction>(new DiagonalLoss(I_diag.cwiseSqrt()));
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> DiagonalLoss::Sigmas(
    const Eigen::VectorXd& S_diag) {
  return std::shared_ptr<LossFunction>(new DiagonalLoss(S_diag.cwiseInverse()));
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> DiagonalLoss::Variances(
    const Eigen::VectorXd& V_diag) {
  return std::shared_ptr<LossFunction>(
      new DiagonalLoss((V_diag.cwiseInverse()).cwiseSqrt()));
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> DiagonalLoss::Scales(const Eigen::VectorXd& s) {
  return std::shared_ptr<LossFunction>(new DiagonalLoss(s));
}

/* ************************************************************************** */
void DiagonalLoss::print(std::ostream& out) const {
  out << "Diagonal loss function : R_diag = [" << sqrt_info_diag_.transpose()
      << "]'" << std::endl;
}

/* ************************************************************************** */
void DiagonalLoss::weightInPlace(Eigen::VectorXd& b) const {
  assert(sqrt_info_diag_.size() == b.size() &&
         "[DiagonalLoss::weightInPlace] error size wrong");
  b = b.cwiseProduct(sqrt_info_diag_);
}

/* ************************************************************************** */
void DiagonalLoss::weightInPlace(std::vector<Eigen::MatrixXd>& As,
                                 Eigen::VectorXd& b) const {
  assert(sqrt_info_diag_.size() == b.size() &&
         "[DiagonalLoss::weightInPlace] error size wrong");
  b = b.cwiseProduct(sqrt_info_diag_);
  for (auto& A : As) {
    assert(sqrt_info_diag_.size() == A.rows() &&
           "[DiagonalLoss::weightInPlace] jacobian size wrong");
    for (int i = 0; i < A.rows(); i++) {
      A.row(i) *= sqrt_info_diag_(i);
    }
  }
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> ScaleLoss::Precision(double prec) {
  return std::shared_ptr<LossFunction>(new ScaleLoss(std::sqrt(prec)));
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> ScaleLoss::Sigma(double sigma) {
  return std::shared_ptr<LossFunction>(new ScaleLoss(1.0 / sigma));
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> ScaleLoss::Variance(double var) {
  return std::shared_ptr<LossFunction>(new ScaleLoss(1.0 / std::sqrt(var)));
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> ScaleLoss::Scale(double s) {
  return std::shared_ptr<LossFunction>(new ScaleLoss(s));
}

/* ************************************************************************** */
void ScaleLoss::print(std::ostream& out) const {
  out << "Scale loss function : inv_sigma = " << inv_sigma_ << std::endl;
}

/* ************************************************************************** */
void ScaleLoss::weightInPlace(Eigen::VectorXd& b) const { b *= inv_sigma_; }

/* ************************************************************************** */
void ScaleLoss::weightInPlace(std::vector<Eigen::MatrixXd>& As,
                              Eigen::VectorXd& b) const {
  b *= inv_sigma_;
  for (auto& A : As) {
    A *= inv_sigma_;
  }
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> CauchyLoss::Cauchy(double k) {
  return std::shared_ptr<LossFunction>(new CauchyLoss(k));
}

/* ************************************************************************** */
void CauchyLoss::print(std::ostream& out) const {
  out << "Cauchy loss function : k = " << k_ << std::endl;
}

/* ************************************************************************** */
void CauchyLoss::weightInPlace(Eigen::VectorXd& b) const {
  double sqrtw = std::sqrt(weight(b.norm()));
  b *= sqrtw;
}

/* ************************************************************************** */
void CauchyLoss::weightInPlace(std::vector<Eigen::MatrixXd>& As,
                               Eigen::VectorXd& b) const {
  const double sqrtw = std::sqrt(weight(b.norm()));
  b *= sqrtw;
  for (auto& A : As) {
    A *= sqrtw;
  }
}

/* ************************************************************************** */
std::shared_ptr<LossFunction> HuberLoss::Huber(double k) {
  return std::shared_ptr<LossFunction>(new HuberLoss(k));
}

/* ************************************************************************** */
void HuberLoss::print(std::ostream& out) const {
  out << "Huber loss function : k = " << k_ << std::endl;
}

/* ************************************************************************** */
void HuberLoss::weightInPlace(Eigen::VectorXd& b) const {
  double sqrtw = std::sqrt(weight(b.norm()));
  b *= sqrtw;
}

/* ************************************************************************** */
void HuberLoss::weightInPlace(std::vector<Eigen::MatrixXd>& As,
                              Eigen::VectorXd& b) const {
  const double sqrtw = std::sqrt(weight(b.norm()));
  b *= sqrtw;
  for (auto& A : As) {
    A *= sqrtw;
  }
}

/* ************************************************************************** */
void ComposedLoss::print(std::ostream& out) const {
  out << "Composed loss function : " << std::endl << "Loss 1 : ";
  l1_->print(out);
  out << "Loss 2 : ";
  l2_->print(out);
}

/* ************************************************************************** */
void ComposedLoss::weightInPlace(Eigen::VectorXd& b) const {
  l1_->weightInPlace(b);
  l2_->weightInPlace(b);
}

/* ************************************************************************** */
void ComposedLoss::weightInPlace(std::vector<Eigen::MatrixXd>& As,
                                 Eigen::VectorXd& b) const {
  l1_->weightInPlace(As, b);
  l2_->weightInPlace(As, b);
}
}  // namespace minisam


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

     http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/

// Free energy, energy, and specific heat of square lattice Ising model

#pragma once

#include <cmath>
#include <boost/math/constants/constants.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>

namespace tfi {
namespace energy {
namespace chain {

namespace {

template<typename T>
struct functor {
  typedef T real_t;
  functor(real_t J, real_t Gamma) : J_(J), Gamma_(Gamma) {}
  real_t operator()(real_t k) const {
    using std::abs; using std::cos; using std::sin; using std::sqrt; using std::pow;
    return 2 * abs(J_) * sqrt(pow(cos(k) - (Gamma_ / J_), 2) + pow(sin(k), 2));
  }
  real_t J_, Gamma_;
};

template<typename T>
functor<T> func(T J, T Gamma) { return functor<T>(J, Gamma); }

}

template<typename T>
inline T infinite(T J, T Gamma) {
  using std::abs;
  typedef T real_t;
  if (abs(J)> 0) {
    auto pi = boost::math::constants::pi<real_t>();
    boost::math::quadrature::tanh_sinh<real_t> integrator;
    return -integrator.integrate(func(J, Gamma), 0, pi) / (2 * pi);
  } else {
    return -abs(Gamma);
  }
}

} // end namespace chain
} // end namespace energy
} // end namespace tfi


#include "obj.h"
#include "assignment.h"
#include "HungarianAlg.h"

#include <math.h>
#include <Eigen/Dense>

#include <cmath>
#include <ctime>

#include <iostream>
#include <vector>
#include <string>
#include <iostream>

template<typename LeftClass, typename RightClass>
double function_t(LeftClass& lc, RightClass& rc) {
	return std::sqrt((lc.x - rc.x)*(lc.x -	rc.x) + (lc.y - rc.y)*(lc.y -	rc.y));
}

#define RANGE 100.0

int main (int argc, char** argv) {
#ifdef (WIN32 || WIN64)
	srand((unsigned)time(NULL));
#else
	srandom((unsigned)time(NULL));
#endif
	int M = 10;
	int N = 15;
	double dist_thresh = 10.0;
	std::vector<ele::LeftObj> left_obj(M);
	std::vector<ele::RightObj> right_obj(N);

	Eigen::MatrixXd l_value = (Eigen::MatrixXd::Random(M, 2)).array().abs() * 100.0;
	for (int i=0; i<l_value.rows(); i++) {
		left_obj[i].x = l_value(i, 0);
		left_obj[i].x = l_value(i, 1);
	}
	Eigen::MatrixXd r_value = (Eigen::MatrixXd::Random(N, 2)).array().abs() * 100.0;
	for (int i=0; i<r_value.rows(); i++) {
		right_obj[i].x = r_value(i, 0);
		right_obj[i].x = r_value(i, 1);
	}

	std::cout << "create Assingment" << std::endl;
	ele::Assignment<ele::LeftObj, ele::RightObj> assign_engine(&left_obj,
			&right_obj,  function_t<ele::LeftObj, ele::RightObj>, dist_thresh);

	std::cout << "start Solve..." << std::endl;
	assign_engine.Solve();

	std::cout << "Solved and get result" << std::endl;
	std::vector<int> result = assign_engine.Assign();

	for (size_t i=0; i<result.size(); i++) {
		if (result[i] < 0)
			std::cout << "Left: " << i << " --> null" << std::endl;
		else
			std::cout << "Left: " << i << " --> Right: " << result[i] << std::endl;
	}

  return 0;
}


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

int main(int argc, char **argv)
{
  // exploit namespaces to shorten code
  using namespace boost::numeric::ublas;
  using std::cout;
  using std::endl;

  // declare three 3x3 matrices of complex<long double> elements
  matrix<std::complex<long double> > m(11, 11);

  // iterate over 3x3 matrix entries
  // r : row index
  // c : column index
  for (unsigned r = 0; r < m.size1(); r++) {
    for (unsigned c = 0; c < m.size2(); c++) {
      // enumerated matrix entries
        int a = r -5;
        int b = c - 5;
      m(r,c) =  a + b * 1i;

    }
  }

  // print to screen as demonstration
  cout << "m:" << endl;
  cout << m << endl;

    /*
  cout << endl << "n:" << endl;
  cout << n << endl;
  cout << endl << "o:" << endl;
  cout << o << endl;
  cout << endl << "m + n:" << endl;
  cout << m + n << endl;
  cout << endl << "m * n:" << endl;
  cout << prod(m, n) << endl;*/
}


#include "stdafx.h"
#include "Calibration.h"
#include "Configuration.h"
#include "IPCClient.h"

#include <string>
#include <vector>
#include <iostream>

#include <Eigen/Dense>

static IPCClient Driver;
CalibrationContext CalCtx;
CalibrationState LastState = CalibrationState::None;
Eigen::Vector3d ReferenceTranslation;
Eigen::Vector3d ReferenceRotation;

void InitCalibrator()
{
	Driver.Connect();
}

struct Pose
{
	Eigen::Matrix3d rot;
	Eigen::Vector3d trans;

	Pose() { }
	Pose(vr::HmdMatrix34_t hmdMatrix)
	{
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 3; j++) {
				rot(i,j) = hmdMatrix.m[i][j];
			}
		}
		trans = Eigen::Vector3d(hmdMatrix.m[0][3], hmdMatrix.m[1][3], hmdMatrix.m[2][3]);
	}
	Pose(double x, double y, double z) : trans(Eigen::Vector3d(x,y,z)) { }
};

Pose ReferencePose;

struct Sample
{
	Pose ref, target;
	bool valid;
	Sample() : valid(false) { }
	Sample(Pose ref, Pose target) : valid(true), ref(ref), target(target) { }
};

struct DSample
{
	bool valid;
	Eigen::Vector3d ref, target;
};

bool StartsWith(const std::string &str, const std::string &prefix)
{
	if (str.length() < prefix.length())
		return false;

	return str.compare(0, prefix.length(), prefix) == 0;
}

bool EndsWith(const std::string &str, const std::string &suffix)
{
	if (str.length() < suffix.length())
		return false;

	return str.compare(str.length() - suffix.length(), suffix.length(), suffix) == 0;
}

Eigen::Vector3d AxisFromRotationMatrix3(Eigen::Matrix3d rot)
{
	return Eigen::Vector3d(rot(2,1) - rot(1,2), rot(0,2) - rot(2,0), rot(1,0) - rot(0,1));
}

double AngleFromRotationMatrix3(Eigen::Matrix3d rot)
{
	return acos((rot(0,0) + rot(1,1) + rot(2,2) - 1.0) / 2.0);
}

DSample DeltaRotationSamples(Sample s1, Sample s2)
{
	// Difference in rotation between samples.
	auto dref = s1.ref.rot * s2.ref.rot.transpose();
	auto dtarget = s1.target.rot * s2.target.rot.transpose();

	// When stuck together, the two tracked objects rotate as a pair,
	// therefore their axes of rotation must be equal between any given pair of samples.
	DSample ds;
	ds.ref = AxisFromRotationMatrix3(dref);
	ds.target = AxisFromRotationMatrix3(dtarget);

	// Reject samples that were too close to each other.
	auto refA = AngleFromRotationMatrix3(dref);
	auto targetA = AngleFromRotationMatrix3(dtarget);
	ds.valid = refA > 0.4 && targetA > 0.4 && ds.ref.norm() > 0.01 && ds.target.norm() > 0.01;

	ds.ref.normalize();
	ds.target.normalize();
	return ds;
}

Eigen::Vector3d CalibrateRotation(const std::vector<Sample> &samples)
{
	std::vector<DSample> deltas;

	for (size_t i = 0; i < samples.size(); i++)
	{
		for (size_t j = 0; j < i; j++)
		{
			auto delta = DeltaRotationSamples(samples[i], samples[j]);
			if (delta.valid)
				deltas.push_back(delta);
		}
	}
	char buf[256];
	snprintf(buf, sizeof buf, "Got %zd samples with %zd delta samples\n", samples.size(), deltas.size());
	CalCtx.Log(buf);

	// Kabsch algorithm

	Eigen::MatrixXd refPoints(deltas.size(), 3), targetPoints(deltas.size(), 3);
	Eigen::Vector3d refCentroid(0,0,0), targetCentroid(0,0,0);

	for (size_t i = 0; i < deltas.size(); i++)
	{
		refPoints.row(i) = deltas[i].ref;
		refCentroid += deltas[i].ref;

		targetPoints.row(i) = deltas[i].target;
		targetCentroid += deltas[i].target;
	}

	refCentroid /= (double) deltas.size();
	targetCentroid /= (double) deltas.size();

	for (size_t i = 0; i < deltas.size(); i++)
	{
		refPoints.row(i) -= refCentroid;
		targetPoints.row(i) -= targetCentroid;
	}

	auto crossCV = refPoints.transpose() * targetPoints;

	Eigen::BDCSVD<Eigen::MatrixXd> bdcsvd;
	auto svd = bdcsvd.compute(crossCV, Eigen::ComputeThinU | Eigen::ComputeThinV);

	Eigen::Matrix3d i = Eigen::Matrix3d::Identity();
	if ((svd.matrixU() * svd.matrixV().transpose()).determinant() < 0)
	{
		i(2,2) = -1;
	}

	Eigen::Matrix3d rot = svd.matrixV() * i * svd.matrixU().transpose();
	rot.transposeInPlace();

	Eigen::Vector3d euler = rot.eulerAngles(2, 1, 0) * 180.0 / EIGEN_PI;

	snprintf(buf, sizeof buf, "Calibrated rotation: yaw=%.2f pitch=%.2f roll=%.2f\n", euler[1], euler[2], euler[0]);
	CalCtx.Log(buf);
	return euler;
}

Eigen::Vector3d CalibrateTranslation(const std::vector<Sample> &samples)
{
	std::vector<std::pair<Eigen::Vector3d, Eigen::Matrix3d>> deltas;

	for (size_t i = 0; i < samples.size(); i++)
	{
		for (size_t j = 0; j < i; j++)
		{
			auto QAi = samples[i].ref.rot.transpose();
			auto QAj = samples[j].ref.rot.transpose();
			auto dQA = QAj - QAi;
			auto CA = QAj * (samples[j].ref.trans - samples[j].target.trans) - QAi * (samples[i].ref.trans - samples[i].target.trans);
			deltas.push_back(std::make_pair(CA, dQA));

			auto QBi = samples[i].target.rot.transpose();
			auto QBj = samples[j].target.rot.transpose();
			auto dQB = QBj - QBi;
			auto CB = QBj * (samples[j].ref.trans - samples[j].target.trans) - QBi * (samples[i].ref.trans - samples[i].target.trans);
			deltas.push_back(std::make_pair(CB, dQB));
		}
	}

	Eigen::VectorXd constants(deltas.size() * 3);
	Eigen::MatrixXd coefficients(deltas.size() * 3, 3);

	for (size_t i = 0; i < deltas.size(); i++)
	{
		for (int axis = 0; axis < 3; axis++)
		{
			constants(i * 3 + axis) = deltas[i].first(axis);
			coefficients.row(i * 3 + axis) = deltas[i].second.row(axis);
		}
	}

	Eigen::Vector3d trans = coefficients.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(constants);
	auto transcm = trans * 100.0;

	char buf[256];
	snprintf(buf, sizeof buf, "Calibrated translation x=%.2f y=%.2f z=%.2f\n", transcm[0], transcm[1], transcm[2]);
	CalCtx.Log(buf);
	return transcm;
}

Sample CollectSample(const CalibrationContext &ctx)
{
	vr::TrackedDevicePose_t reference, target;
	reference.bPoseIsValid = false;
	target.bPoseIsValid = false;

	reference = ctx.devicePoses[ctx.referenceID];
	target = ctx.devicePoses[ctx.targetID];

	bool ok = true;
	if (!reference.bPoseIsValid)
	{
		CalCtx.Log("Reference device is not tracking\n"); ok = false;
	}
	if (!target.bPoseIsValid)
	{
		CalCtx.Log("Target device is not tracking\n"); ok = false;
	}
	if (!ok)
	{
		CalCtx.Log("Aborting calibration!\n");
		CalCtx.state = CalibrationState::None;
		return Sample();
	}

	return Sample(
		Pose(reference.mDeviceToAbsoluteTracking),
		Pose(target.mDeviceToAbsoluteTracking)
	);
}

vr::HmdQuaternion_t VRRotationQuat(Eigen::Vector3d eulerdeg)
{
	auto euler = eulerdeg * EIGEN_PI / 180.0;

	Eigen::Quaterniond rotQuat =
		Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitZ()) *
		Eigen::AngleAxisd(euler(1), Eigen::Vector3d::UnitY()) *
		Eigen::AngleAxisd(euler(2), Eigen::Vector3d::UnitX());

	vr::HmdQuaternion_t vrRotQuat;
	vrRotQuat.x = rotQuat.coeffs()[0];
	vrRotQuat.y = rotQuat.coeffs()[1];
	vrRotQuat.z = rotQuat.coeffs()[2];
	vrRotQuat.w = rotQuat.coeffs()[3];
	return vrRotQuat;
}

vr::HmdVector3d_t VRTranslationVec(Eigen::Vector3d transcm)
{
	auto trans = transcm * 0.01;
	vr::HmdVector3d_t vrTrans;
	vrTrans.v[0] = trans[0];
	vrTrans.v[1] = trans[1];
	vrTrans.v[2] = trans[2];
	return vrTrans;
}

void ResetAndDisableOffsets(uint32_t id)
{
	vr::HmdVector3d_t zeroV;
	zeroV.v[0] = zeroV.v[1] = zeroV.v[2] = 0;

	vr::HmdQuaternion_t zeroQ;
	zeroQ.x = 0; zeroQ.y = 0; zeroQ.z = 0; zeroQ.w = 1;

	protocol::Request req(protocol::RequestSetDeviceTransform);
	req.setDeviceTransform = { id, false, zeroV, zeroQ };
	Driver.SendBlocking(req);
}

static_assert(vr::k_unTrackedDeviceIndex_Hmd == 0, "HMD index expected to be 0");

void ScanAndApplyProfile(CalibrationContext &ctx)
{
	char buffer[vr::k_unMaxPropertyStringSize];
	ctx.enabled = ctx.validProfile;

	for (uint32_t id = 0; id < vr::k_unMaxTrackedDeviceCount; ++id)
	{
		auto deviceClass = vr::VRSystem()->GetTrackedDeviceClass(id);
		if (deviceClass == vr::TrackedDeviceClass_Invalid)
			continue;

		/*if (deviceClass == vr::TrackedDeviceClass_HMD) // for debugging unexpected universe switches
		{
			vr::ETrackedPropertyError err = vr::TrackedProp_Success;
			auto universeId = vr::VRSystem()->GetUint64TrackedDeviceProperty(id, vr::Prop_CurrentUniverseId_Uint64, &err);
			printf("uid %d err %d\n", universeId, err);
			ResetAndDisableOffsets(id);
			continue;
		}*/

		if (!ctx.enabled)
		{
			ResetAndDisableOffsets(id);
			continue;
		}

		vr::ETrackedPropertyError err = vr::TrackedProp_Success;
		vr::VRSystem()->GetStringTrackedDeviceProperty(id, vr::Prop_TrackingSystemName_String, buffer, vr::k_unMaxPropertyStringSize, &err);

		if (err != vr::TrackedProp_Success)
		{
			ResetAndDisableOffsets(id);
			continue;
		}

		std::string trackingSystem(buffer);

		if (id == vr::k_unTrackedDeviceIndex_Hmd)
		{
			//auto p = ctx.devicePoses[id].mDeviceToAbsoluteTracking.m;
			//printf("HMD %d: %f %f %f\n", id, p[0][3], p[1][3], p[2][3]);

			if (trackingSystem != ctx.referenceTrackingSystem)
			{
				// Currently using an HMD with a different tracking system than the calibration.
				ctx.enabled = false;
			}

			ResetAndDisableOffsets(id);
			continue;
		}

		if (trackingSystem != ctx.targetTrackingSystem)
		{
			ResetAndDisableOffsets(id);
			continue;
		}

		protocol::Request req(protocol::RequestSetDeviceTransform);
		req.setDeviceTransform = {
			id,
			true,
			VRTranslationVec(ctx.calibratedTranslation),
			VRRotationQuat(ctx.calibratedRotation)
		};
		Driver.SendBlocking(req);
	}

	if (ctx.enabled && ctx.chaperone.valid && ctx.chaperone.autoApply)
	{
		uint32_t quadCount = 0;
		vr::VRChaperoneSetup()->GetLiveCollisionBoundsInfo(nullptr, &quadCount);

		// Heuristic: when SteamVR resets to a blank-ish chaperone, it uses empty geometry,
		// but manual adjustments (e.g. via a play space mover) will not touch geometry.
		if (quadCount != ctx.chaperone.geometry.size())
		{
			ApplyChaperoneBounds();
		}
	}
}

void StartCalibration()
{
	CalCtx.state = CalibrationState::Begin;
	CalCtx.wantedUpdateInterval = 0.0;
	CalCtx.messages.clear();
}

void SetReferenceOffset() {
	auto &ctx = CalCtx;
	Pose pose(ctx.devicePoses[ctx.referenceID].mDeviceToAbsoluteTracking);
	ReferencePose = pose;
	ReferenceTranslation = ctx.calibratedTranslation;
	ReferenceRotation = ctx.calibratedRotation;
}

void CalibrationTick(double time)
{
	if (!vr::VRSystem())
		return;

	auto &ctx = CalCtx;
	if ((time - ctx.timeLastTick) < 0.05)
		return;

	ctx.timeLastTick = time;
	vr::VRSystem()->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseRawAndUncalibrated, 0.0f, ctx.devicePoses, vr::k_unMaxTrackedDeviceCount);

	if (ctx.state == CalibrationState::None)
	{
		ctx.wantedUpdateInterval = 1.0;

		if ((time - ctx.timeLastScan) >= 1.0)
		{
			ScanAndApplyProfile(ctx);
			ctx.timeLastScan = time;
		}
		return;
	}

	if (ctx.state == CalibrationState::Editing)
	{
		ctx.wantedUpdateInterval = 0.1;

		if ((time - ctx.timeLastScan) >= 0.1)
		{
			ScanAndApplyProfile(ctx);
			ctx.timeLastScan = time;
		}
		return;
	}

	if (ctx.state == CalibrationState::Referencing)
	{
		Pose pose(ctx.devicePoses[ctx.referenceID].mDeviceToAbsoluteTracking);
		Eigen::Vector3d deltaTrans = pose.trans - ReferencePose.trans;
		ctx.calibratedTranslation = (ReferenceTranslation + (deltaTrans * 100));

		// Attempt # 1, getting teh euler delta and adding it to the original reference rotation - does not work.
		//auto rotation = pose.rot.eulerAngles(2, 1, 0) * 180.0 / EIGEN_PI;
		/*ctx.calibratedRotation[0] = ReferenceRotation(0) + rotation(0);
		ctx.calibratedRotation[1] = ReferenceRotation(1) + rotation(1);
		ctx.calibratedRotation[2] = ReferenceRotation(2) + rotation(2);*/
		//ctx.calibratedRotation[0] = rotation(0);
		//ctx.calibratedRotation[1] = rotation(1);
		//ctx.calibratedRotation[2] = rotation(2);

		// Attempt #2, convert it all to quaternions ?? didnt get far with this one.
		/*Eigen::Quaterniond currentQuat =
			Eigen::AngleAxisd(ctx.calibratedRotation(0), Eigen::Vector3d::UnitZ()) *
			Eigen::AngleAxisd(ctx.calibratedRotation(1), Eigen::Vector3d::UnitY()) *
			Eigen::AngleAxisd(ctx.calibratedRotation(2), Eigen::Vector3d::UnitX());
		Eigen::Matrix3d deltaRot = pose.rot - ReferencePose.rot;
		Eigen::Quaternionf delta(deltaRot);
		vr::HmdQuaternion_t deltaQuat;
		deltaQuat.x = delta.coeffs()[0];
		deltaQuat.y = delta.coeffs()[1];
		deltaQuat.z = delta.coeffs()[2];
		deltaQuat.w = delta.coeffs()[3];*/
		//currentQuat.normalize();
		// Eigen::Matrix3d updatedRot = currentQuat.toRotationMatrix() + deltaRot;

		ctx.wantedUpdateInterval = 0.025;

		if ((time - ctx.timeLastScan) >= 0.025)
		{
			ScanAndApplyProfile(ctx);
			ctx.timeLastScan = time;
		}
		return;
	}
	LastState = ctx.state;

	if (ctx.state == CalibrationState::Begin)
	{
		bool ok = true;

		char referenceSerial[256], targetSerial[256];
		vr::VRSystem()->GetStringTrackedDeviceProperty(ctx.referenceID, vr::Prop_SerialNumber_String, referenceSerial, 256);
		vr::VRSystem()->GetStringTrackedDeviceProperty(ctx.targetID, vr::Prop_SerialNumber_String, targetSerial, 256);

		char buf[256];
		snprintf(buf, sizeof buf, "Reference device ID: %d, serial: %s\n", ctx.referenceID, referenceSerial);
		CalCtx.Log(buf);
		snprintf(buf, sizeof buf, "Target device ID: %d, serial %s\n", ctx.targetID, targetSerial);
		CalCtx.Log(buf);

		if (ctx.referenceID == -1)
		{
			CalCtx.Log("Missing reference device\n"); ok = false;
		}
		else if (!ctx.devicePoses[ctx.referenceID].bPoseIsValid)
		{
			CalCtx.Log("Reference device is not tracking\n"); ok = false;
		}

		if (ctx.targetID == -1)
		{
			CalCtx.Log("Missing target device\n"); ok = false;
		}
		else if (!ctx.devicePoses[ctx.targetID].bPoseIsValid)
		{
			CalCtx.Log("Target device is not tracking\n"); ok = false;
		}

		if (!ok)
		{
			ctx.state = CalibrationState::None;
			CalCtx.Log("Aborting calibration!\n");
			return;
		}

		ResetAndDisableOffsets(ctx.targetID);
		ctx.state = CalibrationState::Rotation;
		ctx.wantedUpdateInterval = 0.0;

		CalCtx.Log("Starting calibration...\n");
		return;
	}

	auto sample = CollectSample(ctx);
	if (!sample.valid)
	{
		return;
	}

	static std::vector<Sample> samples;
	samples.push_back(sample);

	CalCtx.Progress(samples.size(), CalCtx.SampleCount());

	if (samples.size() == CalCtx.SampleCount())
	{
		CalCtx.Log("\n");
		if (ctx.state == CalibrationState::Rotation)
		{
			ctx.calibratedRotation = CalibrateRotation(samples);

			auto vrRotQuat = VRRotationQuat(ctx.calibratedRotation);

			protocol::Request req(protocol::RequestSetDeviceTransform);
			req.setDeviceTransform = { ctx.targetID, true, vrRotQuat };
			Driver.SendBlocking(req);

			ctx.state = CalibrationState::Translation;
		}
		else if (ctx.state == CalibrationState::Translation)
		{
			ctx.calibratedTranslation = CalibrateTranslation(samples);

			auto vrTrans = VRTranslationVec(ctx.calibratedTranslation);

			protocol::Request req(protocol::RequestSetDeviceTransform);
			req.setDeviceTransform = { ctx.targetID, true, vrTrans };
			Driver.SendBlocking(req);

			ctx.validProfile = true;
			SaveProfile(ctx);
			CalCtx.Log("Finished calibration, profile saved\n");

			ctx.state = CalibrationState::None;
		}

		samples.clear();
	}
}

void LoadChaperoneBounds()
{
	vr::VRChaperoneSetup()->RevertWorkingCopy();

	uint32_t quadCount = 0;
	vr::VRChaperoneSetup()->GetLiveCollisionBoundsInfo(nullptr, &quadCount);

	CalCtx.chaperone.geometry.resize(quadCount);
	vr::VRChaperoneSetup()->GetLiveCollisionBoundsInfo(&CalCtx.chaperone.geometry[0], &quadCount);
	vr::VRChaperoneSetup()->GetWorkingStandingZeroPoseToRawTrackingPose(&CalCtx.chaperone.standingCenter);
	vr::VRChaperoneSetup()->GetWorkingPlayAreaSize(&CalCtx.chaperone.playSpaceSize.v[0], &CalCtx.chaperone.playSpaceSize.v[1]);
	CalCtx.chaperone.valid = true;
}

void ApplyChaperoneBounds()
{
	vr::VRChaperoneSetup()->RevertWorkingCopy();
	vr::VRChaperoneSetup()->SetWorkingCollisionBoundsInfo(&CalCtx.chaperone.geometry[0], CalCtx.chaperone.geometry.size());
	vr::VRChaperoneSetup()->SetWorkingStandingZeroPoseToRawTrackingPose(&CalCtx.chaperone.standingCenter);
	vr::VRChaperoneSetup()->SetWorkingPlayAreaSize(CalCtx.chaperone.playSpaceSize.v[0], CalCtx.chaperone.playSpaceSize.v[1]);
	vr::VRChaperoneSetup()->CommitWorkingCopy(vr::EChaperoneConfigFile_Live);
}


#pragma once
#include <functional>
#include <random>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Cholesky>

namespace filter_bay
{
/*!
Simplifies drawing from a multivariant normal distribution.
*/
class NormalSampler
{
  // Drawing from multivariate normal distribution:
  // https://en.wikipedia.org/wiki/Multivariate_normal_distribution#Drawing_values_from_the_distribution
  // Sample by projecting standard normal distributed values with L, where
  // L L^T = covarianc
public:
  // typedef Eigen::Matrix<double, dim, dim> MatrixDim;
  // typedef Eigen::Matrix<double, dim, 1> VectorDim;

  /*!
  Create the sampler using a random_device to create the seed.
  */
  NormalSampler()
  {
    std::random_device random_device;
    uniform_generator = std::mt19937(random_device());
  }

  /*!
  Create the sampler via a seed. For testing it should always be the same seed.
  In real world use the random_device class to generate the seed.
  */
  NormalSampler(unsigned int seed) : uniform_generator(seed) {}

  /*!
  Samples a random vector from a normal distribution using a robust
  decomposition algorithm.
  \param mean the mean of the distribution
  \param covariance the covariance of the distribution
  */
  template <typename scalar, int dim>
  auto sample_robust(const Eigen::Matrix<scalar, dim, 1> &mean,
                     const Eigen::Matrix<scalar, dim, dim> &covariance)
      -> Eigen::Matrix<scalar, dim, 1>
  {
    // Use LDLT decomposition which works with semi definite covariances:
    auto ldlt = covariance.ldlt();
    // L = P^T L sqrt(D), where D is a diagonal matrix.
    // https://stats.stackexchange.com/questions/48749/how-to-sample-from-a-multivariate-normal-given-the-pt-ldlt-p-decomposition-o
    auto P_T = ldlt.transpositionsP().transpose();
    auto L = ldlt.matrixL().toDenseMatrix();
    // Root for diagonal matrix can be calculated element wise
    auto sqrt_D = ldlt.vectorD().cwiseSqrt().asDiagonal();
    auto decomposed = P_T * L * sqrt_D;
    return sample<scalar, dim>(mean, decomposed);
  }

  /*!
  Samples a random vector from a normal distribution using the Cholesky
  decomposition. It might fail, if the covariance is not positive definite.
  \param mean the mean of the distribution
  \param covariance the covariance of the distribution
  */
  template <typename scalar, int dim>
  auto sample_cholesky(const Eigen::Matrix<scalar, dim, 1> &mean,
                       const Eigen::Matrix<scalar, dim, dim> &covariance)
      -> Eigen::Matrix<scalar, dim, 1>
  {
    auto decomposed = covariance.llt().matrixL();
    return sample<scalar, dim>(mean, decomposed);
  }

  /*!
  Draws random values from a normal distribution parametrized by the mean and
  standard deviation (NOT variance as in the covariance matrix version).
  */
  template <typename scalar>
  scalar draw_normal(scalar mean, scalar standard_deviation)
  {
    std::normal_distribution<scalar> normal_dist;
    return normal_dist(uniform_generator) * standard_deviation + mean;
  }

private:
  // Algorithm that provides uniform distributed pseudo-random numbers.
  // Real randomness is imposed via a random seed.
  std::mt19937 uniform_generator;

  /*!
  Samples with the given mean and decomposed covariance matrix.
  */
  template <typename scalar, int dim>
  auto sample(const Eigen::Matrix<scalar, dim, 1> &mean,
              const Eigen::Matrix<scalar, dim, dim> &decomposed)
      -> Eigen::Matrix<scalar, dim, 1>
  {
    auto norm_dist = create_normal_dist_vector<scalar, dim>();
    return mean + decomposed * norm_dist;
  }

  /*!
  Creates a vector with all elements drawn randlomly from a standard normal
  distribution.
  */
  template <typename scalar, int dim>
  Eigen::Matrix<scalar, dim, 1> create_normal_dist_vector()
  {
    std::normal_distribution<scalar> normal_dist;
    Eigen::Matrix<scalar, dim, 1> result;
    for (int i = 0; i < dim; i++)
    {
      result(i) = normal_dist(uniform_generator);
    }
    return result;
  }
};
} // namespace filter_bay


#include <kv/Heine.hpp>
#include <kv/qAiry.hpp>
#include <kv/qBessel.hpp>
#include <cmath>
#include <iostream>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/interval.hpp>

// define rounding operations for double
// if "rdouble.hpp" is not included, result of computation is not "verified"
#include <kv/rdouble.hpp>
typedef kv::interval<double> itv;
typedef kv::complex< kv::interval<double> > cp;
using namespace std;
namespace ub = boost::numeric::ublas;

int main()
{
  cout.precision(17);
  ub::vector< itv > x(30);
  int n=20;
  itv y,nu,q,xx;
  q="0.7";
  nu=1.5;
  y=20.;

  x(0)=y;

  for(int i=1;i<=n;i++){
    xx=mid(x(i-1));
    x(i)=xx-kv::HEratio(itv(xx),itv(nu),itv(q))*(1-q)*x(i-1)
      /(kv::HEratio(itv(x(i-1)),itv(nu),itv(q))-kv::HEratio(itv(q*x(i-1)),itv(nu),itv(q)));
    cout<<x(i)<<endl;
    //cout<<i<<endl;
    cout<<"value of HE inf"<<kv::Hahn_Exton(itv(x(i).lower()),itv(nu),itv(q))<<endl;
    cout<<"value of HE sup"<<kv::Hahn_Exton(itv(x(i).upper()),itv(nu),itv(q))<<endl;
    cout<<"value of HE mid"<<kv::Hahn_Exton(itv(mid(x(i))),itv(nu),itv(q))<<endl;
  }
 }
  // to be updated


#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <iostream>

#include <Eigen/Dense>
#include <algorithm>

#include "aux/eigen2hdf.hpp"
#include "quadrature/qhermite.hpp"
#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"
#include "spectral/h2n_1d.hpp"
#include "spectral/hermite_to_nodal.hpp"
#include "spectral/polar_to_hermite.hpp"

//#include <deal.II/base/exceptions.h>

using namespace std;
using namespace boltzmann;

namespace po = boost::program_options;

int main(int argc, char* argv[])
{
  typedef Eigen::VectorXd vec_t;

  int K = atoi(argv[1]);

  // create basis files
  typedef typename SpectralBasisFactoryHN::basis_type hermite_basis_t;
  hermite_basis_t hermite_basis;
  SpectralBasisFactoryHN::create(hermite_basis, K, 2);
  cout << "Hermite basis size: " << hermite_basis.n_dofs() << endl;

  // hermite to nodal
  typedef Eigen::MatrixXd mat_t;
  Hermite2Nodal<hermite_basis_t> h2n(
      hermite_basis, K, [K](mat_t& m1, mat_t& m2) { H2N_1d<>::create(m1, m2, K); });
  Hermite2Nodal<hermite_basis_t> h2ng(
      hermite_basis, K, [K](mat_t& m1, mat_t& m2) { H2NG_1d::create(m1, m2, K, 1.0); });

  auto& M = h2n.get_h2n();

  auto& M2 = h2ng.get_h2n();

  auto diff = M - M2;
  diff = diff.select(abs(diff) < 1e-15, Eigen::MatrixXd::Zero(diff.rows(), diff.cols()), diff);

  cout << "difference matrix\n";
  cout << diff << endl;
  // cout << "M := H2N-matrix\n";
  // cout << "M.T * M\n";
  // cout << M.transpose()* M  << endl;

  // fname = "coefficients.h5";
  // if(!boost::filesystem::exists(fname)) {
  //   cout << endl << fname << " does not exist. Abort!\n";
  //   return 1;
  // }
  // vec_t cp(N); // polar coefficients
  // hid_t h5_init = H5Fopen(fname.c_str(), H5F_ACC_RDONLY, H5P_DEFAULT);
  // eigen2hdf::load(h5_init, "coeffs", cp);
  // H5Fclose(h5_init);

  // AssertDimension(polar_basis.n_dofs(), cp.size());

  hid_t h5f = H5Fcreate("test.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  eigen2hdf::save(h5f, "M", M);
  eigen2hdf::save(h5f, "Mg", M2);
  // eigen2hdf::save(h5f, "coeffs_nodal", cn);
  // eigen2hdf::save(h5f, "coeffs_hermite", ch);
  // eigen2hdf::save(h5f, "coeffs", cp2);
  H5Fclose(h5f);

  return 0;
}


///
/// \file hankel_gemv.hpp
///
#ifndef MXPFIT_HANKEL_MATRIX_HPP
#define MXPFIT_HANKEL_MATRIX_HPP

#include <Eigen/Core>

#include <fftw3/shared_plan.hpp>

namespace mxpfit
{

///
/// ### HankelMatrix
///
/// \brief Expression of a generalized Hankel matrix.
///
/// \tparam T  Scalar type of matrix elements
///
/// For the given vector \f$ \boldsymbol{h} = [h_0,h_1,...,h_{n+m-1}]^{T},\f$ a
/// \f$ m \times n \f$ Hankel matrix, \f$ A, \f$ is defined as
///
/// \f[
///   A = \left[ \begin{array}{cccccc}
///     h_0    & h_1    & h_2    & h_3   & \cdots & h_{n-1} \\
///     h_1    & h_2    & h_3    & h_4   & \cdots & h_{n}   \\
///     h_2    & h_3    & h_4    & h_5   & \cdots & h_{n+1} \\
///     h_3    & h_4    & h_5    & h_6   & \cdots & h_{n+2} \\
///     \vdots & \vdots & \vdots &\vdots & \ddots & \vdots \\
///     h_{m-1} & h_{m} & h_{m+1} & h_{m+2} & \cdots & h_{m+n-1}
///   \end{array} \right]
/// \f]
///
/// This class represents a Hankel matrix expression from the given number of
/// rows, \f$ m, \f$ the number of columns \f$n,\f$ and a vector expression for
/// the coefficients, \f$ \boldsymbol{h}. \f$ If the given vector expression is
/// l-value, this class wraps the existing vector expression, otherwise storage
/// for coefficients are allocated and stored.
///
/// This class also provides the interface for matrix-vector multiplication
/// compatible to `MatrixFreeGEMV`. This operation can be performed efficiently
/// using the fast Fourier transform (FFT) in \f$O((m+n)\log(m+n).\f$
///
/// For this purpose, the class also allocate internal vectors as working space.
/// The FFT plans are automatically generated whenever matrix size is updated.
/// This operation is done in thread-safe manner, using mutex-lock.
///
template <typename T>
class HankelMatrix
{
public:
    using Scalar              = T;
    using RealScalar          = typename Eigen::NumTraits<T>::Real;
    using ComplexScalar       = std::complex<RealScalar>;
    using StorageIndex        = Eigen::Index;
    using Index               = Eigen::Index;
    using CoeffsVector        = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using CoeffsVectorRef     = Eigen::Ref<const CoeffsVector>;
    using ComplexCoeffsVector = Eigen::Matrix<ComplexScalar, Eigen::Dynamic, 1>;
    using PlainObject = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;

private:
    using FFT  = fftw3::FFT<RealScalar>;
    using IFFT = fftw3::IFFT<RealScalar>;

    typename FFT::PlanPointer m_fft_plan;
    typename IFFT::PlanPointer m_ifft_plan;

    Index m_rows;
    Index m_cols;

    mutable CoeffsVector m_work;
    CoeffsVectorRef m_coeffs;
    ComplexCoeffsVector m_caux;
    mutable ComplexCoeffsVector m_xaux;

public:
    enum
    {
        /// `Scalar` is complex number?
        IsComplex = Eigen::NumTraits<Scalar>::IsComplex
    };

    /// Default constructor: create an empty matrix
    HankelMatrix()
        : m_fft_plan(),
          m_ifft_plan(),
          m_rows(),
          m_cols(),
          m_work(),
          m_coeffs(m_work), // need to initialize Eigen::Ref object
          m_caux(),
          m_xaux()
    {
    }

    /// Copy constructor
    HankelMatrix(const HankelMatrix &other)
        : m_fft_plan(other.m_fft_plan),
          m_ifft_plan(other.m_ifft_plan),
          m_rows(other.m_rows),
          m_cols(other.m_cols),
          m_work(other.m_work),
          m_coeffs(other.m_coeffs),
          m_caux(other.m_caux),
          m_xaux(other.m_xaux)
    {
    }

    ///
    /// Construct a Hankel matrix with memory allocation
    ///
    /// \param[in] nrows the number of rows
    /// \param[in] ncols the number of columns
    /// \param[in] prescribed_fft_size the FFT length (optional)
    ///
    /// \pre  `nrows >= 0 && ncols >= 0` is required
    ///
    HankelMatrix(Index nrows, Index ncols, Index prescribed_fft_size = 0)
        : m_fft_plan(),
          m_ifft_plan(),
          m_rows(nrows),
          m_cols(ncols),
          m_work(get_fft_length(nrows, ncols, prescribed_fft_size)),
          m_coeffs(m_work), // need to initialize Eigen::Ref
          m_caux(IsComplex ? m_work.size() : m_work.size() / 2 + 1),
          m_xaux(m_caux.size())
    {
        assert(nrows >= Index() && "nrows must be a non-negative integer");
        assert(ncols >= Index() && "ncols must be a non-negative integer");
        set_fft_plans();
    }

    /// Default destructor
    ~HankelMatrix()
    {
    }

    /// Assignment operator is deleted as a consequence that Eigen::Ref is
    /// non-assignable
    HankelMatrix &operator=(const HankelMatrix &other) = delete;

    /// \return the number of rows
    Index rows() const
    {
        return m_rows;
    }

    /// \return the number of columns
    Index cols() const
    {
        return m_cols;
    }

    /// \return size of vector that defines current Hankel matrix
    Index size() const
    {
        // Return 0 if the matrix is empty
        return std::max(Index(), rows() + cols() - 1);
    }

    /// \return  A const reference of the coefficient vector
    const CoeffsVectorRef &coeffs() const
    {
        return m_coeffs;
    }

    ///
    /// Resize internal Hankel matrix and update FFT plan for matrix-vector
    /// multiplication if necessary.
    ///
    void resize(Index nrows, Index ncols, Index prescribed_fft_size = 0)
    {
        assert(nrows >= Index() && "nrows must be a non-negative integer");
        assert(ncols >= Index() && "ncols must be a non-negative integer");

        m_rows = nrows;
        m_cols = ncols;

        const Index n1 = m_work.size();
        const Index n2 = get_fft_length(nrows, ncols, prescribed_fft_size);

        if (n1 != n2)
        {
            m_work.resize(n2);
            m_caux.resize(IsComplex ? m_work.size() : m_work.size() / 2 + 1);
            m_xaux.resize(m_caux.size());
            set_fft_plans();

            if (coeffs().size() != size())
            {
                // Eigen::Ref to dummy object
                m_coeffs.~CoeffsVectorRef();
                ::new (&m_coeffs) CoeffsVectorRef(m_work);
            }
        }
    }

    ///
    /// Set a Hankel matrix from the number of rows, columns and vector
    /// expression of matrix coefficients.
    ///
    /// \param[in] coeffs   the vector expression of matrix coefficients
    ///
    /// \pre `coeffs.size() ==  size()` is required
    ///
    template <typename Derived>
    void setCoeffs(const Eigen::EigenBase<Derived> &coeffs)
    {
        assert(coeffs.size() == size() &&
               "Invalid size for the vector of coefficients");
        if (rows() == Index() || cols() == Index())
        {
            // Matrix is empty. Nothing to do.
            return;
        }

        m_coeffs.~CoeffsVectorRef();
        ::new (&m_coeffs) CoeffsVectorRef(coeffs.derived());

        compute_aux_vector();
    }

    void setCoeffs(const CoeffsVectorRef &coeffs)
    {
        assert(coeffs.size() == size() &&
               "Invalid size for the vector of coefficients");
        if (rows() == Index() || cols() == Index())
        {
            // Matrix is empty. Nothing to do.
            return;
        }

        if (&(coeffs.derived()) != &m_coeffs)
        {
            m_coeffs.~CoeffsVectorRef();
            ::new (&m_coeffs) CoeffsVectorRef(coeffs);
            compute_aux_vector();
        }
    }

    /// \return The same Hankel matrix in dense form.
    PlainObject toDenseMatrix() const
    {
        PlainObject ret(rows(), cols());
        for (Index j = 0; j < cols(); ++j)
        {
            for (Index i = 0; i < rows(); ++i)
            {
                ret(i, j) = coeffs()(i + j);
            }
        }
        return ret;
    }

    /// Compute `dst += alpha * A * rhs`
    template <typename Dest, typename RHS>
    void apply(Dest &dst, const Eigen::MatrixBase<RHS> &rhs, Scalar alpha) const
    {
        apply_impl<false>(dst, rhs, alpha);
    }

    /// Compute `dst += alpha * A.conjugate() * rhs`
    template <typename Dest, typename RHS>
    void applyConjugate(Dest &dst, const Eigen::MatrixBase<RHS> &rhs,
                        Scalar alpha) const
    {
        apply_impl<(IsComplex != 0)>(dst, rhs, alpha);
    }

    /// Compute `dst += alpha * A.transpose() * rhs`
    template <typename Dest, typename RHS>
    void applyTranspose(Dest &dst, const Eigen::MatrixBase<RHS> &rhs,
                        Scalar alpha) const
    {
        apply_transpose_impl<false>(dst, rhs, alpha);
    }

    /// Compute `dst += alpha * A.transpose() * rhs`
    template <typename Dest, typename RHS>
    void applyAdjoint(Dest &dst, const Eigen::MatrixBase<RHS> &rhs,
                      Scalar alpha) const
    {
        apply_transpose_impl<(IsComplex != 0)>(dst, rhs, alpha);
    }

private:
    static Index get_fft_length(Index nrows, Index ncols,
                                Index prescribed_fft_size)
    {
        // Enforce fft_size becomes even
        const Index n = (nrows + ncols) / 2;
        return std::max({prescribed_fft_size, 2 * n});
    }

    void set_fft_plans()
    {
        const int n       = static_cast<int>(m_work.size());
        const int howmany = 1;
        m_fft_plan  = FFT::make_plan(n, howmany, m_work.data(), m_xaux.data());
        m_ifft_plan = IFFT::make_plan(n, howmany, m_xaux.data(), m_work.data());
    }

    // Pre-compute auxiliary vector for matrix-vector multiplication operation
    void compute_aux_vector()
    {
        // Set first column of circulant matrix C. Then compute the discrete
        // Fourier transform this vector and store the result into \c caux.
        const auto nhead    = rows();
        const auto ntail    = cols() - 1;
        const auto npadding = m_work.size() - nhead - ntail;

        m_work.head(nhead) = coeffs().tail(nhead);
        if (npadding > Index())
        {
            m_work.segment(nhead, npadding).setZero();
        }
        m_work.tail(ntail) = coeffs().head(ntail);

        // m_caux <-- FFT[m_work]
        FFT::run(m_fft_plan, m_work.data(), m_caux.data());
        m_caux *= RealScalar(1) / m_work.size();
    }

    template <bool ComplexConjugate, typename Dest, typename RHS>
    void apply_impl(Dest &dst, const Eigen::MatrixBase<RHS> &rhs,
                    Scalar alpha) const
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(Dest);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(RHS);

        assert(dst.size() == rows());
        assert(rhs.size() == cols());

        if (alpha == Scalar(/*zero*/))
        {
            return;
        }
        //
        // Form new vector x' = [x(n-1),x(n-2),...,x(0),0....0] of
        // length n + m - 1, and compute FFT.
        //
        if (ComplexConjugate)
        {
            m_work.head(cols()) = rhs.reverse().conjugate();
        }
        else
        {
            m_work.head(cols()) = rhs.reverse();
        }

        m_work.tail(m_work.size() - cols()).setZero();

        // m_xaux <-- FFT[m_work]
        FFT::run(m_fft_plan, m_work.data(), m_xaux.data());
        //
        // y[0:nrows] = IFFT(FFT(c') * FFT(x'))[0:nrows]
        //
        m_xaux = m_xaux.cwiseProduct(m_caux);
        IFFT::run(m_ifft_plan, m_xaux.data(), m_work.data());

        if (ComplexConjugate)
        {
            dst += alpha * m_work.head(rows()).conjugate();
        }
        else
        {
            dst += alpha * m_work.head(rows());
        }
    }

    // Compute `dst += alpha * A^T * rhs`
    template <bool ComplexConjugate, typename Dest, typename RHS>
    void apply_transpose_impl(Dest &dst, const Eigen::MatrixBase<RHS> &rhs,
                              Scalar alpha) const
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(Dest);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(RHS);

        assert(dst.size() == cols());
        assert(rhs.size() == rows());

        if (alpha == Scalar(/*zero*/))
        {
            return;
        }
        //
        // Form new vector x' = [0,0,...,0,x(m-1),x(m-2),...,x(0)] of
        // length n + m - 1, and compute FFT.
        //
        m_work.head(m_work.size() - rows()).setZero();
        if (ComplexConjugate)
        {
            m_work.tail(rows()) = rhs.reverse().conjugate();
        }
        else
        {
            m_work.tail(rows()) = rhs.reverse();
        }

        // m_xaux <-- FFT[m_work]
        FFT::run(m_fft_plan, m_work.data(), m_xaux.data());
        //
        // y[0:ncols-1] = IFFT(FFT(c') * FFT(x'))[nrows:size - 1]
        //
        m_xaux = m_xaux.cwiseProduct(m_caux);
        IFFT::run(m_ifft_plan, m_xaux.data(), m_work.data());

        if (ComplexConjugate)
        {
            dst += alpha * m_work.tail(cols()).conjugate();
        }
        else
        {
            dst += alpha * m_work.tail(cols());
        }
    }
};

} // namespace mxpfit

#endif /* MXPFIT_HANKEL_MATRIX_HPP */


/*============================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center (DKFZ)
All rights reserved.

Use of this source code is governed by a 3-clause BSD license that can be
found in the LICENSE file.

============================================================================*/

#include <boost/math/constants/constants.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <boost/version.hpp>

#include "mitkFormulaParser.h"
#include "mitkFresnel.h"

namespace qi = boost::spirit::qi;
namespace ascii = boost::spirit::ascii;
namespace phx = boost::phoenix;

typedef std::string::const_iterator Iter;
typedef ascii::space_type Skipper;

namespace qi = boost::spirit::qi;

namespace mitk
{
  /*!
   *	@brief			Transforms the given number from degrees to radians and returns it.
   *	@tparam T		The scalar type that represents a value (e.g. double).
   *	@param[in] deg	A scalar value in degrees.
   *	@return			The given value in radians.
   */
  template<typename T>
  inline T deg2rad(const T deg)
  {
    return deg * boost::math::constants::pi<T>() / static_cast<T>(180);
  }

  /*!
   *	@brief			Returns the cosine of the given degree scalar.
   *	@tparam T		The scalar type that represents a value (e.g. double).
   *	@param[in] t	A scalar value in degrees whose cosine should be returned.
   *	@return			The cosine of the given degree scalar.
   */
  template<typename T>
  inline T cosd(const T t)
  {
    return std::cos(deg2rad(t));
  }

  /*!
   *	@brief			Returns the sine of the given degree scalar.
   *	@tparam T		The scalar type that represents a value (e.g. double).
   *	@param[in] t	A scalar value in degrees whose sine should be returned.
   *	@return			The sine of the given degree scalar.
   */
  template<typename T>
  inline T sind(const T t)
  {
    return std::sin(deg2rad(t));
  }

  /*!
   *	@brief			Returns the tangent of the given degree scalar.
   *	@tparam T		The scalar type that represents a value (e.g. double).
   *	@param[in] t	A scalar value in degrees whose tangent should be returned.
   *	@return			The tangent of the given degree scalar.
   */
  template<typename T>
  inline T tand(const T t)
  {
    return std::tan(deg2rad(t));
  }

  /*!
   *	@brief			Returns the fresnel integral sine at the given x-coordinate.
   *	@details		Code for "fresnel_s()" (fresnel.cpp and fresnel.h) taken as-is from the GNU
   *					Scientific Library (http://www.gnu.org/software/gsl/), specifically from
   *					http://www.network-theory.co.uk/download/gslextras/Fresnel/.
   *	@tparam T		The scalar type that represents a value (e.g. double).
   *	@param[in] t	The x-coordinate at which the fresnel integral sine should be returned.
   *	@return			The fresnel integral sine at the given x-coordinate.
   */
  template<typename T>
  T fresnelS(const T t)
  {
    T x = t / boost::math::constants::root_half_pi<T>();
    return static_cast<T>(fresnel_s(x) / boost::math::constants::root_two_div_pi<T>());
  }

  /*!
   *	@brief			Returns the fresnel integral cosine at the given x-coordinate.
   *	@details		Code for "fresnel_c()" (fresnel.cpp and fresnel.h) taken as-is from the GNU
   *					Scientific Library (http://www.gnu.org/software/gsl/), specifically from
   *					http://www.network-theory.co.uk/download/gslextras/Fresnel/.
   *	@tparam T		The scalar type that represents a value (e.g. double).
   *	@param[in] t	The x-coordinate at which the fresnel integral cosine should be returned.
   *	@return			The fresnel integral cosine at the given x-coordinate.
   */
  template<typename T>
  T fresnelC(const T t)
  {
    T x = t / boost::math::constants::root_half_pi<T>();
    return static_cast<T>(fresnel_c(x) / boost::math::constants::root_two_div_pi<T>());
  }

  /*!
   *	@brief		The grammar that defines the language (i.e. what is allowed) for the parser.
   */
  class Grammar : public qi::grammar<Iter, FormulaParser::ValueType(), Skipper>
  {
    /*!
     *	@brief	Helper structure that makes it easier to dynamically call any
     *			one-parameter-function by overloading the @c () operator.
     */
    struct func1_
    {
      // Required for Phoenix 3+
      template<typename Sig>
      struct result;

      /*!
       *	@brief				Helper structure that is needed for compatibility with
       *						@c boost::phoenix.
       *	@tparam Functor		Type of the functor (this struct).
       *	@tparam Function	Type of the function that should be called.
       *	@tparam Arg1		Type of the argument the function should be called with.
       *
       */
      template<typename Functor, typename Function, typename Arg1>
      struct result<Functor(Function, Arg1&)>
      {
        /*! @brief The result structure always needs this typedef */
        typedef Arg1 type;
      };

      /*!
       *	@brief				Calls the function @b f with the argument @b a1 and returns the
       *						result.
       *						The result always has the same type as the argument.
       *	@tparam Function	Type of the function that should be called.
       *	@tparam Arg1			Type of the argument the function should be called with.
       *	@param[in] f		The function that should be called.
       *	@param[in] a1		The argument the function should be called with.
       *	@return				The result of the called function.
       */
      template<typename Function, typename Arg1>
      Arg1 operator()(const Function f, const Arg1 a1) const
      {
        return f(a1);
      }
    };

    /*!
     *	@brief	Helper structure that maps strings to function calls so that parsing e.g.
     *			@c "cos(0)" actually calls the @c std::cos function with parameter @c 1 so it
     *			returns @c 0.
     */
    class unaryFunction_ :
      public qi::symbols<typename std::iterator_traits<Iter>::value_type, FormulaParser::ValueType(*)(FormulaParser::ValueType)>
    {
    public:
      /*!
       *	@brief Constructs the structure, this is where the mapping takes place.
       */
      unaryFunction_()
      {
        this->add
        ("abs", static_cast<FormulaParser::ValueType(*)(FormulaParser::ValueType)>(&std::abs))
          ("exp", static_cast<FormulaParser::ValueType(*)(FormulaParser::ValueType)>(&std::exp)) // @TODO: exp ignores division by zero
          ("sin", static_cast<FormulaParser::ValueType(*)(FormulaParser::ValueType)>(&std::sin))
          ("cos", static_cast<FormulaParser::ValueType(*)(FormulaParser::ValueType)>(&std::cos))
          ("tan", static_cast<FormulaParser::ValueType(*)(FormulaParser::ValueType)>(&std::tan))
          ("sind", &sind)
          ("cosd", &cosd)
          ("tand", &tand)
          ("fresnelS", &fresnelS)
          ("fresnelC", &fresnelC);
      }
    } unaryFunction;

  public:
    /*!
     *	@brief							Constructs the grammar with the given formula parser.
     *	@param[in, out] formulaParser	The formula parser this grammar is for - so it can
     *									access its variable map.
     */
    Grammar(FormulaParser& formulaParser) : Grammar::base_type(start)
    {
      using qi::_val;
      using qi::_1;
      using qi::_2;
      using qi::char_;
      using qi::alpha;
      using qi::alnum;
      using qi::double_;
      using qi::as_string;

      phx::function<func1_> func1;

      start = expression > qi::eoi;

      expression = term[_val = _1]
        >> *(('+' >> term[_val += _1])
          | ('-' >> term[_val -= _1]));

      term = factor[_val = _1]
        >> *(('*' >> factor[_val *= _1])
          | ('/' >> factor[_val /= _1]));

      factor = primary[_val = _1];
      /*!	@TODO:	Repair exponentiation */
      //>> *('^' >> factor[phx::bind<FormulaParser::ValueType, FormulaParser::ValueType, FormulaParser::ValueType>(std::pow, _val, _1)]);

      variable = as_string[alpha >> *(alnum | char_('_'))]
        [_val = phx::bind(&FormulaParser::lookupVariable, &formulaParser, _1)];

      primary = double_[_val = _1]
        | '(' >> expression[_val = _1] >> ')'
        | ('-' >> primary[_val = -_1])
        | ('+' >> primary[_val = _1])
        | (unaryFunction >> '(' >> expression >> ')')[_val = func1(_1, _2)]
        | variable[_val = _1];
    }

    /*! the rules of the grammar. */
    qi::rule<Iter, FormulaParser::ValueType(), Skipper> start;
    qi::rule<Iter, FormulaParser::ValueType(), Skipper> expression;
    qi::rule<Iter, FormulaParser::ValueType(), Skipper> term;
    qi::rule<Iter, FormulaParser::ValueType(), Skipper> factor;
    qi::rule<Iter, FormulaParser::ValueType(), Skipper> variable;
    qi::rule<Iter, FormulaParser::ValueType(), Skipper> primary;
  };

  FormulaParser::FormulaParser(const VariableMapType* variables) : m_Variables(variables)
  {}

  FormulaParser::ValueType FormulaParser::parse(const std::string& input)
  {
    std::string::const_iterator iter = input.begin();
    std::string::const_iterator end = input.end();
    FormulaParser::ValueType result = static_cast<FormulaParser::ValueType>(0);

    try
    {
      if (!qi::phrase_parse(iter, end, Grammar(*this), ascii::space, result))
      {
        mitkThrowException(FormulaParserException) << "Could not parse '" << input <<
          "': Grammar could not be applied to the input " << "at all.";
      }
    }
    catch (qi::expectation_failure<Iter>& e)
    {
      std::string parsed = "";

      for (Iter i = input.begin(); i != e.first; i++)
      {
        parsed += *i;
      }
      mitkThrowException(FormulaParserException) << "Error while parsing '" << input <<
        "': Unexpected character '" << *e.first << "' after '" << parsed << "'";
    }

    return result;
  };

  FormulaParser::ValueType FormulaParser::lookupVariable(const std::string var)
  {
    if (m_Variables == nullptr)
    {
      mitkThrowException(FormulaParserException) << "Map of variables is empty";
    }

    try
    {
      return m_Variables->at(var);
    }
    catch (std::out_of_range&)
    {
      mitkThrowException(FormulaParserException) << "No variable '" << var << "' defined in lookup";
    }
  };

}


/* ROS-CHOMP.
 *
 * Copyright (C) 2015 Jafar Qutteineh. All rights reserved.
 * License (3-Cluase BSD): https://github.com/j3sq/ROS-CHOMP/blob/master/LICENSE
 *
 * This code uses and is based on code from:
 *   Project: trychomp https://github.com/poftwaresatent/trychomp
 *   Copyright (C) 2014 Roland Philippsen. All rights reserved.
 *   License (3-Clause BSD) : https://github.com/poftwaresatent/trychomp
 * **
 * \file chomp.cpp
 *
 * CHOMP for point vehicles (x,y) moving holonomously in the plane. It will
 * plan a trajectory (xi) connecting start point (qs) to end point (qe) while
 * avoiding obstacles (obs)
 */
#include <iostream>
#include <Eigen/Dense>
#include <stdlib.h>
#include <err.h>
#include "chomp.hpp"

typedef Eigen::VectorXd Vector;
typedef Eigen::MatrixXd Matrix;
typedef Eigen::Isometry3d Transform;

using namespace std;

namespace chomp {
//////////////////////////////////////////////////
// trajectory etc

static size_t const nq(20);             // number of q stacked into xi
static size_t const cdim(2);            // dimension of config space
static size_t const xidim(nq *cdim);    // dimension of trajectory, xidim = nq * cdim
static size_t const iteration_limit(10000);
static double const dt(1.0);            // time step
static double const eta(100.0);         // >= 1, regularization factor for gradient descent
static double const lambda(1.0);        // weight of smoothness objective

//////////////////////////////////////////////////
// gradient descent etc

Matrix AA;                      // metric
Vector bb;                      // acceleration bias for start and end config
Matrix Ainv;                    // inverse of AA

static void init_chomp(Vector const &qs, Vector const &qe, Vector  &xi)
{
	if (xi.rows() == xidim) {
		//do nothing. Use existing trajectory
	} else {
		//initalize a new trajectory based on a direct line connecting qs to qe
		xi = Vector::Zero(xidim);
		Vector dxi(cdim);
		dxi << (qe(0) - qs(0)) / (nq - 1), (qe(0) - qs(0)) / (nq - 1);
		for (size_t ii(0); ii < nq; ++ii)
			xi.block(cdim * ii, 0, cdim, 1) = qs + ii * dxi;

		/*
		 * //use this instead if you want to inalize all points to qs
		 * for (size_t ii (0); ii < nq; ++ii) {
		 * xi.block (cdim * ii, 0, cdim, 1) = qs;
		 * }
		 */
	}
	AA = Matrix::Zero(xidim, xidim);
	for (size_t ii(0); ii < nq; ++ii) {
		AA.block(cdim * ii, cdim * ii, cdim, cdim) = 2.0 * Matrix::Identity(cdim, cdim);
		if (ii > 0) {
			AA.block(cdim * (ii - 1), cdim * ii, cdim, cdim) = -1.0 * Matrix::Identity(cdim, cdim);
			AA.block(cdim * ii, cdim * (ii - 1), cdim, cdim) = -1.0 * Matrix::Identity(cdim, cdim);
		}
	}
	AA /= dt * dt * (nq + 1);

	bb = Vector::Zero(xidim);
	bb.block(0, 0, cdim, 1) = qs;
	bb.block(xidim - cdim, 0, cdim, 1) = qe;
	bb /= -dt * dt * (nq + 1);

	// not needed anyhow
	// double cc (double (qs.transpose() * qs) + double (qe.transpose() * qe));
	// cc /= dt * dt * (nq + 1);

	Ainv = AA.inverse();
}

static double chomp_iteration(Vector const &qs, Vector const &qe, Vector  &xi, Matrix const &obs)
{
	//////////////////////////////////////////////////
	// beginning of "the" CHOMP iteration
	Vector nabla_smooth(AA * xi + bb);
	Vector const & xidd(nabla_smooth); // indeed, it is the same in this formulation...

	Vector nabla_obs(Vector::Zero(xidim));

	for (size_t iq(0); iq < nq; ++iq) {
		Vector const qq(xi.block(iq * cdim, 0, cdim, 1));
		Vector qd;
		if (0 == iq) {
			qd = 0.5 * (xi.block((iq + 1) * cdim, 0, cdim, 1) - qs);
		} else if (iq == nq - 1) {
			qd = 0.5 * (qe - xi.block((iq - 1) * cdim, 0, cdim, 1));
		} else {
			qd = 0.5 * (xi.block((iq + 1) * cdim, 0, cdim, 1) - xi.block((iq - 1) * cdim, 0, cdim, 1));;
		}

		// In this case, C and W are the same, Jacobian is identity.  We
		// still write more or less the full-fledged CHOMP expressions
		// (but  we only use one body point) to make subsequent extension
		// easier.
		//
		Vector const & xx(qq);
		Vector const & xd(qd);
		Matrix const JJ(Matrix::Identity(2, 2));        // a little silly here, as noted above.
		double const vel(xd.norm());
		if (vel < 1.0e-3)                               // avoid div by zero further down
			continue;
		Vector const xdn(xd / vel);
		Vector const xdd(JJ * xidd.block(iq * cdim, 0, cdim, 1));
		Matrix const prj(Matrix::Identity(2, 2) - xdn * xdn.transpose()); // hardcoded planar case
		Vector const kappa(prj * xdd / pow(vel, 2.0));

		for (int ii = 0; ii < obs.cols(); ii++) {
			Vector delta(xx - obs.block(0, ii, cdim, 1));
			double const dist(delta.norm());
			if ((dist >= obs(2, ii)) || (dist < 1e-9))
				continue;
			static double const gain(10.0);                                                 // hardcoded param
			double const cost(gain * obs(2, ii) * pow(1.0 - dist / obs(2, ii), 3.0) / 3.0); // hardcoded param
			delta *= -gain *pow(1.0 - dist / obs(2, ii), 2.0) / dist;                       // hardcoded param
			nabla_obs.block(iq * cdim, 0, cdim, 1) += JJ.transpose() * vel * (prj * delta - cost * kappa);
		}
	}

	Vector dxi(Ainv * (nabla_obs + lambda * nabla_smooth));
	xi -= dxi / eta;
	//return the error (in Euclidean sense ). Remeber that the difference is -dxi/eta
	return dxi.norm() / eta;

	// end of "the" CHOMP iteration
	//////////////////////////////////////////////////
}

void generatePath(Vector const &qs, Vector const &qe, Vector &xi, Matrix const &obs)
{
	init_chomp(qs, qe, xi);
	double err;
	for (size_t ii = 0; ii < iteration_limit; ii++) {
		err = chomp_iteration(qs, qe, xi, obs);
		if (err < 0.01)
			break;
	}
}
} //namespace


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/Gaussian.h>
#include <BayesFilters/GaussianFilter.h>
#include <BayesFilters/KFCorrection.h>
#include <BayesFilters/KFPrediction.h>
#include <BayesFilters/SimulatedLinearSensor.h>
#include <BayesFilters/SimulatedStateModel.h>
#include <BayesFilters/WhiteNoiseAcceleration.h>
#include <BayesFilters/utils.h>

#include <Eigen/Dense>

using namespace bfl;
using namespace Eigen;

class KFSimulation : public GaussianFilter
{
public:
    KFSimulation
    (
        Gaussian& initial_state,
        std::unique_ptr<GaussianPrediction> prediction,
        std::unique_ptr<GaussianCorrection> correction,
        std::size_t simulation_steps
    ) noexcept :
        GaussianFilter(initial_state, std::move(prediction), std::move(correction)),
        simulation_steps_(simulation_steps)
    { }

protected:
    bool runCondition() override
    {
        if (getFilteringStep() < simulation_steps_)
            return true;
        else
            return false;
    }

    std::vector<std::string> log_filenames(const std::string& prefix_path, const std::string& prefix_name) override
    {
        return  {prefix_path + "/" + prefix_name + "_pred_mean",
                 prefix_path + "/" + prefix_name + "_cor_mean"};
    }

    void log() override
    {
        logger(predicted_state_.mean().transpose(), corrected_state_.mean().transpose());
    }

private:
    std::size_t simulation_steps_;
};

int main()
{
    std::cout << "Running a KF filter on a simulated target." << std::endl;
    std::cout << "Data is logged in the test folder with prefix testKF." << std::endl;

    /* A set of parameters needed to run a Kalman filter in a simulated environment. */
    Vector4d initial_simulated_state(10.0f, 0.0f, 10.0f, 0.0f);
    std::size_t simulation_time = 100;

    /* Step 1 - Initialization */

    Gaussian initial_state(4);
    Vector4d initial_mean(4.0f, 0.04f, 15.0f, 0.4f);
    Matrix4d initial_covariance;
    initial_covariance << pow(0.05, 2), 0,            0,            0,
                          0,            pow(0.05, 2), 0,            0,
                          0,            0,            pow(0.01, 2), 0,
                          0,            0,            0,            pow(0.01, 2);
    initial_state.mean() = initial_mean;
    initial_state.covariance() = initial_covariance;

    /* Step 2 - Prediction */

    /* Step 2.1 - Define the state model. */

    /* Initialize a white noise acceleration state model. */
    double T = 1.0f;
    double tilde_q = 10.0f;

    std::unique_ptr<LinearStateModel> wna = utils::make_unique<WhiteNoiseAcceleration>(T, tilde_q);

    /* Step 2.2 - Define the prediction step. */

    /* Initialize the Kalman filter prediction step and pass the ownership of the state model. */
    std::unique_ptr<KFPrediction> kf_prediction = utils::make_unique<KFPrediction>(std::move(wna));

    /* Step 3 - Correction */

    /* Step 3.1 - Define where the measurement are originated from (simulated in this case). */

    /* Initialize simulated target model with a white noise acceleration. */
    std::unique_ptr<StateModel> target_model = utils::make_unique<WhiteNoiseAcceleration>(T, tilde_q);
    std::unique_ptr<SimulatedStateModel> simulated_state_model = utils::make_unique<SimulatedStateModel>(std::move(target_model), initial_simulated_state, simulation_time);
    simulated_state_model->enable_log(".", "testKF");

    /* Step 3.2 - Initialize a measurement model (a linear sensor reading x and y coordinates). */
    std::unique_ptr<LinearMeasurementModel> simulated_linear_sensor = utils::make_unique<SimulatedLinearSensor>(std::move(simulated_state_model));
    simulated_linear_sensor->enable_log(".", "testKF");

    /* Step 3.3 - Initialize the Kalman filter correction step and pass the ownership of the measurement model. */
    std::unique_ptr<KFCorrection> kf_correction = utils::make_unique<KFCorrection>(std::move(simulated_linear_sensor));

    /* Step 4 - Assemble the Kalman filter. */
    std::cout << "Constructing Kalman filter..." << std::flush;
    KFSimulation kf(initial_state, std::move(kf_prediction), std::move(kf_correction), simulation_time);
    kf.enable_log(".", "testKF");
    std::cout << "done!" << std::endl;

    /* Step 5 - Boot the filter. */
    std::cout << "Booting Kalman filter..." << std::flush;
    kf.boot();
    std::cout << "completed!" << std::endl;

    /* Step 6 - Run the filter and wait until it is closed. */
    /* Note that since this is a simulation, the filter will end upon simulation termination. */
    std::cout << "Running Kalman filter..." << std::flush;
    kf.run();
    std::cout << "waiting..." << std::flush;

    if (!kf.wait())
        return EXIT_FAILURE;

    std::cout << "completed!" << std::endl;

    return EXIT_SUCCESS;
}