//
// Created by david on 2019-05-27.
//

#include <complex.h>
#undef I

#include <Eigen/QR>
#include <Eigen/SVD>
#include <general/class_tic_toc.h>
#include <math/svd.h>

//
// Created by david on 2021-03-26.
//
/*! \brief Performs SVD on a matrix
 *  This function is defined in cpp to avoid long compilation times when having Eigen::BDCSVD included everywhere in headers.
 *  Performs rigorous checks to ensure stability of DMRG.
 *  In some cases Eigen::BCDSVD/JacobiSVD will fail with segfault. Here we use a patched version of Eigen that throws an error
 *  instead so we get a chance to catch it and use lapack svd instead.
 *   \param mat_ptr Pointer to the matrix. Supported are double * and std::complex<double> *
 *   \param rows Rows of the matrix
 *   \param cols Columns of the matrix
 *   \param rank_max Maximum number of singular values
 *   \return The U, S, and V matrices (with S as a vector) extracted from the Eigen::BCDSVD SVD object.
 */
template<typename Scalar>
std::tuple<svd::solver::MatrixType<Scalar>, svd::solver::VectorType<Scalar>, svd::solver::MatrixType<Scalar>, long>
    svd::solver::do_svd_eigen(const Scalar *mat_ptr, long rows, long cols, std::optional<long> rank_max) {
    if(not rank_max.has_value()) rank_max = std::min(rows, cols);

    svd::log->trace("Starting SVD with Eigen");
    Eigen::Map<const MatrixType<Scalar>> mat(mat_ptr, rows, cols);

    if(rows <= 0) throw std::runtime_error(fmt::format("SVD error: rows = {}", rows));
    if(cols <= 0) throw std::runtime_error(fmt::format("SVD error: cols = {}", cols));

#ifndef NDEBUG
    // These are more expensive debugging operations
    if(not mat.allFinite()) throw std::runtime_error("SVD error: matrix has inf's or nan's");
    if(mat.isZero(0)) throw std::runtime_error("SVD error: matrix is all zeros");
    if(mat.isZero(1e-12)) svd::log->warn("Lapacke SVD Warning\n\t Given matrix elements are all close to zero (prec 1e-12)");
#endif

    Eigen::BDCSVD<MatrixType<Scalar>> SVD;

    // Setup the SVD solver
    SVD.setSwitchSize(static_cast<int>(switchsize));
    SVD.setThreshold(threshold);
    bool use_jacobi = std::min(rows, cols) < static_cast<long>(switchsize);
    svd::log->trace("Running SVD with threshold {:.4e} | switchsize {} | size {}", threshold, switchsize, rank_max.value());
    if(use_jacobi) {
        // We only use Jacobi for precision. So we use all the precision we can get.
        svd::log->debug("Running Eigen::JacobiSVD threshold {:.4e} | switchsize {} | rank_max {}", threshold, switchsize, rank_max.value());
        // Run the svd
        t_jac->tic();
        SVD.compute(mat, Eigen::ComputeFullU | Eigen::ComputeFullV | Eigen::FullPivHouseholderQRPreconditioner);
        t_jac->toc();
    } else {
        svd::log->debug("Running Eigen::BDCSVD threshold {:.4e} | switchsize {} | rank_max {}", threshold, switchsize, rank_max.value());
        // Run the svd
        t_svd->tic();
        SVD.compute(mat, Eigen::ComputeThinU | Eigen::ComputeThinV);
        t_svd->toc();
    }
    if(count) count.value()++;
    long max_size = std::min(SVD.singularValues().size(), rank_max.value());
    long rank     = (SVD.singularValues().head(max_size).array() >= threshold).count();
    svd::log->trace("Truncating singular values");
    if(rank == SVD.singularValues().size()) {
        truncation_error = 0;
    } else {
        truncation_error = SVD.singularValues().tail(SVD.singularValues().size() - rank).norm();
    }

    if(SVD.rank() <= 0 or rank == 0 or not SVD.matrixU().leftCols(rank).allFinite() or not SVD.singularValues().head(rank).allFinite() or
       not SVD.matrixV().leftCols(rank).allFinite()) {
        throw std::runtime_error(fmt::format(FMT_COMPILE("Eigen SVD error \n"
                                                         "  svd_threshold    = {:.4e}\n"
                                                         "  Truncation Error = {:.4e}\n"
                                                         "  Rank             = {}\n"
                                                         "  Dims             = ({}, {})\n"
                                                         "  A all finite     : {}\n"
                                                         "  U all finite     : {}\n"
                                                         "  S all finite     : {}\n"
                                                         "  V all finite     : {}\n"),
                                             threshold, truncation_error, rank, rows, cols, mat.allFinite(), SVD.matrixU().leftCols(rank).allFinite(),
                                             SVD.singularValues().head(rank).allFinite(), SVD.matrixV().leftCols(rank).allFinite()));
    }
    svd::log->trace("SVD with Eigen finished successfully");

    return std::make_tuple(SVD.matrixU().leftCols(rank), SVD.singularValues().head(rank), SVD.matrixV().leftCols(rank).adjoint(), rank);
}

//! \relates svd::class_SVD
//! \brief force instantiation of do_svd for type 'double'
template std::tuple<svd::solver::MatrixType<double>, svd::solver::VectorType<double>, svd::solver::MatrixType<double>, long>
    svd::solver::do_svd_eigen(const double *, long, long, std::optional<long>);

using cplx = std::complex<double>;
//! \relates svd::class_SVD
//! \brief force instantiation of do_svd for type 'std::complex<double>'
template std::tuple<svd::solver::MatrixType<cplx>, svd::solver::VectorType<cplx>, svd::solver::MatrixType<cplx>, long>
    svd::solver::do_svd_eigen(const cplx *, long, long, std::optional<long>);


/**

Copyright (c) 2016, Aumann Florian, Borella Jocelyn, Heller Florian, Meißner Pascal, Schleicher Ralf, Stöckle Patrick, Stroh Daniel, Trautmann Jeremias, Walter Milena, Wittenbeck Valerij
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

4. The use is explicitly not permitted to any application which deliberately try to kill or do harm to any living creature.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#define BOOST_TEST_STATIC_LINK

#include <boost/test/included/unit_test.hpp>
#include "next_best_view/test_cases/BaseTest.h"

using namespace next_best_view;
using namespace boost::unit_test;

class MathTest : public BaseTest {
public:
    MathTest() : BaseTest(false, true) {}

    virtual ~MathTest() {}

    /*!
     * \brief evaluates the correctness of the Sphere To Cartesian and Cartesian To Sphere Methods.
     */
    void evaluateS2CandC2S() {
        ROS_INFO("Running Test for S2C and C2S");

        // tolerance
        double tolerance = 2E-7;

        // Unit Sphere Tests
        int thetaDivisor = 32;
        double thetaStepSize = M_PI / (double) thetaDivisor;
        int phiDivisor = 64;
        double phiStepSize = M_2_PI / (double) phiDivisor;
        for (int thetaFac = 0; thetaFac < thetaDivisor; thetaFac++) {
            double theta = - M_PI_2 + thetaFac * thetaStepSize;
            for (int phiFac = 0; phiFac < phiDivisor; phiFac++) {
                double phi = - M_PI + phiFac * phiStepSize;

                // create coordinates
                SimpleSphereCoordinates scoords(1, theta, phi);
                // convert to cartesian
                SimpleVector3 ccoords = MathHelper::convertS2C(scoords);
                // convert to sphere again
                SimpleSphereCoordinates rescoords = MathHelper::convertC2S(ccoords);
                // convert to cartesian again
                SimpleVector3 reccoords = MathHelper::convertS2C(rescoords);

                double cerror = (ccoords - reccoords).lpNorm<2>();

                BOOST_CHECK_MESSAGE(cerror < tolerance, "error has to be minimal.");
            }
        }
    }

    void iterationTest() {
      evaluateS2CandC2S();
    }
};

test_suite* init_unit_test_suite( int argc, char* argv[] ) {
    test_suite* evaluation = BOOST_TEST_SUITE("Evaluation NBV");

    boost::shared_ptr<MathTest> testPtr(new MathTest());

    evaluation->add(BOOST_CLASS_TEST_CASE(&MathTest::iterationTest, testPtr));

    framework::master_test_suite().add(evaluation);

    return 0;
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ASEC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ASEC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the inverse secant in radian: \f$\arccos(1/x)\f$.

    @par Header <boost/simd/function/asec.hpp>

    @see asecd, asecpi

    @par Example:

      @snippet asec.cpp asec

    @par Possible output:

      @snippet asec.txt asec

  **/
  IEEEValue asec(IEEEValue const & x);
} }
#endif

#include <boost/simd/function/scalar/asec.hpp>
#include <boost/simd/function/simd/asec.hpp>

#endif


#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <chrono>
#include <ctime>
#include <climits>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/types/sba/types_six_dof_expsmap.h>

struct
{
    Eigen::Vector3d pose_world;
    float grayscale;
    Measurement(Eigen::Vector3d p, float g) :
};

Eigen::Vector project2Dto3D(int x , int y, int d, float fx, float fy, float cx, float cy, float scale){
    float zz = float(d) / scale;
    float xx = zz * (x - cx) / fx;
    float yy = zz * (y - cy ) / fy;
    return Eigen::Vector3d(xx, yy, zz);
}

Eigen::Vector2d project3Dto2D(float x, float y, float z, float fx, float fy, float cx, float cy){
    float u = fx*x/z + cx;
    float v = fy*y/z + cy;
    return Eigen::Vector2d(u, v);
}

class EdgeSE3ProjectUVDirect: public: BaseUnaryEdge<1, doublem VertexSE3Expmap>{
    private:
        Eigen::Vector3d x_world_;
        float cx_ = 0, cy_ = 0, fx_ = 0, fy_ = 0;
        Mat* image_ = nullptr;

    protected:
    float getPixelValue(float x, float y){
        uchar* data = & data[int(y) * issmage_->step + int(x)];
        float xx = x - floor(x);
        float yy = y - floor(y);
        return float (
                   ( 1-xx ) * ( 1-yy ) * data[0] +
                   xx* ( 1-yy ) * data[1] +
                   ( 1-xx ) *yy*data[ image_->step ] +
                   xx*yy*data[image_->step+1]);
    }
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeSE3ProjectUVDirect(Eigen::Vector3d point, float fx, float fy, float cx, float cy, Mat *img):
                x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image ) {}

    virtual void computeError(){
        const VertexSE3Expmap *v = static_cast<const VertexSE3Expmap*> (_vertices[0]);
        Eigen::Vector3d x_local = v->estimate().map(x_world_);
        float x = x_local[0]*fx_/x_local[2] + cx_;
        float y = x_local[1] * fy_/x_local[2] + cy_;

        if (x-4 < 0 || x + 4 > image_->cols || y-4 < 0 || y + 4 > image_->rows) {
            _error (0, 0) = 0.0;
            this->setLevel(1);
        }
        else{
            _error(0, 0) = getPixelValue(x, y) - _measurement;
        }
    }

    virtual void linearOplus(){
        if(level() = 1){
            _jacobianOplusXi = Eigen:Matrix<double, 1, 6>::Zero();
            return;
        }

        VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> (_vertices[0]);
        Eigen::Vector3d xyz_trans = vtx->estimate().map(x_world_);

        double x = xyz_trans[0];
        double y = xyz_trans[1];

        double invz = 1.0 / xyz_trans[2];
        double invz_2 = invz*invz;

        float u = x*fx_*invz + cx_;
        float v = y*fy_*invz + cy_;

        // Jacobian from SE3 to uv, the matrix is (2, 6)
        // In g2o, the Lie algebra is translation in first three
        Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;

        jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
        jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
        jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
        jacobian_uv_ksai ( 0,3 ) = invz *fx_;
        jacobian_uv_ksai ( 0,4 ) = 0;
        jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;

        jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
        jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
        jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
        jacobian_uv_ksai ( 1,3 ) = 0;
        jacobian_uv_ksai ( 1,4 ) = invz *fy_;
        jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;

        Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
        // simple image gradient
        jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
        jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;

        _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;

    }
}

bool poseEstimationDirect( const vector<Measurement> & measurements, Mat& gray; Eigen::Matrix3f &K, Eigen::Isometry3d &Tcw){
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;
    DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense<DirectBlock::PoseMatrixType>();
    DirectBlock *solver_ptr = new DirectBlock(linearSolver);

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);
<<<<<<< HEAD

=======

>>>>>>> d3aa7eab7a66633ee6b6716534884a999f4bfb7a
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setEstimate(g2o::SE3Quat(Tcw.rotation(), Tcw.translation()));
    pose->setId(0);
    optimizer.addVertex(pose);

    int id = 1;
    for ( Measurement mm: measurements){
        EdgeSE3ProjectUVDirect* edge = new EdgeSE3ProjectUVDirect(
            mm.pose_world,
            K(0,0), K(1,1), K(0,2), K(1,2), gray
        );
        edge->setVertex(0, pose);
        edge->setMeasurement(mm.grayscale);       //based on our assumption of garyscale consistency
        edge->setInformation(Eigen::Matrix<double, 1,1>::Identity());
        edge->setId(id++);
        optimizer.addEdge(edge);
    }

    cout << "edge number in a graph: " << optimizer.edges().size() << endl;
    optimizer.initializeOptimization();
    optimizer.optimize(30);
    Tcw = pose->estimate();
}

int main(int argc, char** argv){
    if ( argc != 2 )
    {
        cout<<"usage: useLK path_to_dataset"<<endl;
        return 1;
    }
    srand ( ( unsigned int ) time ( 0 ) );
    string path_to_dataset = argv[1];
    string associate_file = path_to_dataset + "/associate.txt";

    ifstream fin ( associate_file );

    string rgb_file, depth_file, time_rgb, time_depth;
    cv::Mat color, depth, gray;
    vector<Measurement> measurements;
    // Camera intrinstic parameters
    float cx = 325.5;
    float cy = 253.5;
    float fx = 518.0;
    float fy = 519.0;
    float depth_scale = 1000.0;
    Eigen::Matrix3f K;
    K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;

    Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();

    cv::Mat prev_color;
    for(int index = 0; index<10;index++){
        fin>> time_rgb >> rgb_file >> time_depth >> depth_file;
        color = cv::imread(path_to_dataset+"/"+rgb_file);
        depth = imread(path_to_dataset + "/" + depth_file, -1);
        if(color.data == nullptr || depth.data == nullptr) continue;

        cvtColor(color, gray, cv::COLOR_BGR2GRAY);

        if(index == 0){
            vector<cv::KeyPoint> keypoints;
            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            detector->detect(color, keypoints);
            for (auto kp: keypoints){
               if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
                    continue;
                ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];
                if ( d==0 )
                    continue;
                Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
                float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
                measurements.push_back ( Measurement ( p3d, grayscale ) );
            }
            prev_color = color.clone();
            continue;
        }
        // 使用直接法计算相机运动
        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        poseEstimationDirect ( measurements, &gray, K, Tcw );
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
        cout<<"direct method costs time: "<<time_used.count() <<" seconds."<<endl;
        cout<<"Tcw="<<Tcw.matrix() <<endl;

        // plot the feature points
        cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );
        prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) );
        color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) );
        for ( Measurement m:measurements )
        {
            if ( rand() > RAND_MAX/5 )
                continue;
            Eigen::Vector3d p = m.pos_world;
            Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );
            Eigen::Vector3d p2 = Tcw*m.pos_world;
            Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );
            if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows )
                continue;

            float b = 255*float ( rand() ) /RAND_MAX;
            float g = 255*float ( rand() ) /RAND_MAX;
            float r = 255*float ( rand() ) /RAND_MAX;
            cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 );
            cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 8, cv::Scalar ( b,g,r ), 2 );
            cv::line ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), cv::Scalar ( b,g,r ), 1 );
        }
        cv::imshow ( "result", img_show );
        cv::waitKey ( 0 );

    }
    return 0;
    }
}


/*
 neighbours.cxx

 Copyright (c) 2018 Guy Skinner

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "neighbour.hxx"
#include "utils.hxx"

Neighbour::Neighbour(unsigned long nbas,
                     ublas::matrix<double> plat,
                     ublas::matrix<double> basis) {
  NumberOfAtoms = nbas;
  LatticeVectors = plat;
  BasisVectors = basis;
}

Neighbour::~Neighbour() {
}

void Neighbour::SetInverseLattice(ublas::matrix<double> iplat,
                                  ublas::matrix<double> xbasis) {
  InverseLatticeVectors = iplat;
  InverseBasisVectors = xbasis;
}

void Neighbour::GetSiteList(double rmax) {

  /* Create Site List */
  ublas::vector<long> rcut(3);
  ublas::vector<long> sext(3);

  for (auto i = 0; i < 3; i++) {
    ublas::matrix_row<ublas::matrix<double>> p(LatticeVectors,i);
    rcut(i) = static_cast<long>(rmax/ublas::norm_2(p)+2);
    sext(i) = 2*rcut(i) + 1;
  }

  SupercellExtension = sext;
  /* Linked-List Block Size */
  Neighbour::LinkedListBlocks(rmax);

  ublas::vector<double> cent(3);
  for (auto i = 0; i < 3; i++) cent(i) = 0.5*(sext(i) - 1);
  Centre = cent;

  //long product = boost::accumulate(sext,1,std::multiplies<long>());
  long smax = NumberOfAtoms*product(sext);
  long blks = boost::accumulate(Blocks,1,std::multiplies<long>());

  ublas::matrix<double> sites(smax,3);
  ublas::vector<long> basptr(smax);
  ublas::vector<long> linkl(smax);
  ublas::vector<long> head(blks);
  ublas::vector<double> sitex(3);

  /* Make Sure Zero Vectors */
  head.clear();
  linkl.clear();

  ublas::matrix_row<ublas::matrix<double>> p0(LatticeVectors,0);
  ublas::matrix_row<ublas::matrix<double>> p1(LatticeVectors,1);
  ublas::matrix_row<ublas::matrix<double>> p2(LatticeVectors,2);

  long icnt = 0;
  for (auto i = 0; i < NumberOfAtoms; i++) {
    ublas::matrix_row<ublas::matrix<double>> b(BasisVectors,i);
    ublas::matrix_row<ublas::matrix<double>> xb(InverseBasisVectors,i);
    for (auto j = -rcut(0); j <= rcut(0); j++) {
      for (auto k = -rcut(1); k <= rcut(1); k++) {
        for (auto l = -rcut(2); l <= rcut(2); l++) {
          for (auto a = 0; a < 3; a++) {
            sites(icnt,a) = b(a) + j*p0(a) + k*p1(a) + l*p2(a);
            sitex(a) = xb(a);
          }
          basptr(icnt) = i;
          sitex(0) += static_cast<double>(j);
          sitex(1) += static_cast<double>(k);
          sitex(2) += static_cast<double>(l);
          unsigned long icell = FindCell(sitex);
          linkl(icnt) = head(icell);
          head(icell) = icnt;
          icnt++;
        }
      }
    }
  }

  Sites = sites;
  BasisPointers = basptr;
  Head = head;
  LinkedList = linkl;

}

void Neighbour::LinkedListBlocks(double rmax) {

  /* Decompose Lattice Vector Matrix into Lattice Vectors */
  ublas::matrix_row<ublas::matrix<double>> p1(LatticeVectors,0);
  ublas::matrix_row<ublas::matrix<double>> p2(LatticeVectors,1);
  ublas::matrix_row<ublas::matrix<double>> p3(LatticeVectors,2);

  /* Cross Product of Lattice Vectors */
  ublas::vector<double> c1 = cross3(p2,p3);
  ublas::vector<double> c2 = cross3(p3,p1);
  ublas::vector<double> c3 = cross3(p1,p2);

  ublas::vector<double> w(3);
  w(0) = ublas::inner_prod(p1,c1)/ublas::norm_2(c1);
  w(1) = ublas::inner_prod(p2,c2)/ublas::norm_2(c2);
  w(2) = ublas::inner_prod(p3,c3)/ublas::norm_2(c3);

  ublas::vector<long> blks(3);
  for (auto i = 0; i < 3; i++) {
    blks(i) = static_cast<long>(SupercellExtension(i)*w(i)/rmax);
    if (blks(i) < 3) blks(i) = 3;
  }

  Blocks = blks;

}

void Neighbour::GetNhbrList(double rmax) {

  /* Calculate Site List */
  Neighbour::GetSiteList(rmax);

  ublas::vector<long> tot(NumberOfAtoms);
  ublas::vector<long> ptrs(650*NumberOfAtoms);

  unsigned long nsum = 0;
  for (auto i = 0; i < NumberOfAtoms; i++) {
    tot(i) = 0;
    ublas::matrix_row<ublas::matrix<double>> b(BasisVectors,i);
    ublas::matrix_row<ublas::matrix<double>> bx(InverseBasisVectors,i);
    unsigned long icell = FindCell(bx);
    for (auto imx = -1; imx <= 1; imx++) {
      for (auto imy = -Blocks(0); imy <= Blocks(0); imy += Blocks(0)) {
        for (auto imz = -Blocks(0)*Blocks(1);
             imz <= Blocks(0)*Blocks(1);
             imz += Blocks(0)*Blocks(1)) {
          long jcell = icell + imx + imy + imz;
          long j = Head(jcell);
          while (j != 0) {
            ublas::matrix_row<ublas::matrix<double>> site(Sites,j);
            ublas::vector<double> ra = site - b;
            double r = ublas::norm_2(ra);
            if (r <= rmax and r > 0.0) {
              ptrs(nsum) = j;
              tot(i)++;
              nsum++;
            }
            j = LinkedList(j);
          }
        }
      }
    }
  }

  Total = tot;
  Pointers = ptrs;

}

unsigned long Neighbour::FindCell(ublas::vector<double> sitex){

  /* Find Block that Atom Belongs In */
  ublas::vector<long> ixs(3);
  ublas::vector<double> scent = sitex + Centre;

  for (auto i = 0; i < 3; i++) {
    ixs(i) = static_cast<long>(Blocks(i)*scent(i)/SupercellExtension(i));
    if (ixs(i) > Blocks(i)) ixs(i) = Blocks(i)-1;
  }

  unsigned long icell = ixs(0) + Blocks(0)*ixs(1) + Blocks(1)*Blocks(0)*ixs(2);
  return icell;

}


#include "m_dvr.h"

#include <quartz>
#include <boost/property_tree/ptree.hpp>

#include "src/util/ptree.h"
#include "src/parse/printer.h"

#include "src/parse/math/math_object.h"
#include "src/parse/math/gaussian.h"

namespace quartz {

namespace ptree = boost::property_tree;

ptree::ptree m_dvr(const ptree::ptree & input) {

  const std::function<math::Gaussian<cx_double>(const ptree::ptree &)>
      parse_initial = [](const ptree::ptree & pt) -> math::Gaussian<cx_double> {
    return parse::gaussian(pt);
  };

  const std::function<MathObject<double>(const ptree::ptree &)>
    parse_potential = [](const ptree::ptree & pt)
      ->MathObject<double> {return parse::math_object(pt);};

  std::vector<math::Gaussian<cx_double>> initial =
      util::get_list(input.get_child("initial"), parse_initial);

  const arma::field<MathObject<double>> potentials =
      util::get_mat_object(input.get_child("potential"), parse_potential);

  const arma::uvec grid =
      arma::uvec(util::get_list<arma::uword>(input.get_child("grid")));

  const arma::mat range =
      util::get_mat<double>(input.get_child("range")).t();

  arma::vec masses = arma::ones(grid.n_elem);

  const auto steps = input.get<arma::uword>("steps");
  const auto dt = input.get<double>("dt");

  if (input.get_child_optional("mass")) {
    masses = arma::vec(util::get_list<double>(input.get_child("mass")));
  }

  method::m_dvr::State initial_state(initial, grid, range, masses);
  method::m_dvr::Operator op(initial_state, potentials);
  auto wrapper =
      math::schrotinger_wrapper<method::m_dvr::Operator,
                                method::m_dvr::State,
                                arma::field<MathObject<double>>>;

  ptree::ptree result;

  auto printer_pair = printer(input, result, initial_state);

  propagate(initial_state, op, wrapper, potentials, printer_pair.first, steps,
            dt, printer_pair.second);

  return result;
}

}


// [[Rcpp::depends(BH)]]

#include <boost/multi_array.hpp>
#include <Rcpp.h>
#include <fstream>
#include <iostream>

using namespace Rcpp;
using namespace boost;

// [[Rcpp::export]]
NumericVector cohort_sim(const List s_fp) {
  const List s_ss = s_fp["ss"];
  const int n_year(s_ss["PROJ_YEARS"]), n_sex(2), n_age(66), n_agr(9), n_cd4(7);
  const int n_ts(10);
  const double dt(0.1);

  // Map single age indices (0=15..65=80) to age groups
  const int a2g[n_age] = {
    0, 0, 1, 1, 1, 2, 2, 2, 2, 2, // 15-16 = 0, 17-19 = 1, 20-24 = 2
    3, 3, 3, 3, 3, 4, 4, 4, 4, 4, // 25-29 = 3, 30-34 = 4
    5, 5, 5, 5, 5, 6, 6, 6, 6, 6, // 35-39 = 5, 40-44 = 6
    7, 7, 7, 7, 7, 8, 8, 8, 8, 8, // 45-49 = 7, 50-54 = 8
    8, 8, 8, 8, 8, 8, 8, 8, 8, 8, // 55-64 = 8
    8, 8, 8, 8, 8, 8, 8, 8, 8, 8, // 65-74 = 8
    8, 8, 8, 8, 8, 8};            // 75-80 = 8

  multi_array_ref<double, 3> surv(REAL(s_fp["Sx"]), extents[n_year][n_sex][n_age]);
  multi_array_ref<double, 3> inci(REAL(s_fp["new_inf"]), extents[n_year][n_sex][n_age]);
  multi_array_ref<double, 3> in_dist(REAL(s_fp["cd4_initdist"]), extents[n_sex][n_agr][n_cd4]);
  multi_array_ref<double, 3> in_prog(REAL(s_fp["cd4_prog"]), extents[n_sex][n_agr][n_cd4-1]);
  multi_array_ref<double, 3> in_mort(REAL(s_fp["cd4_mort"]), extents[n_sex][n_agr][n_cd4]);

  multi_array<double, 3> dist(extents[n_sex][n_age][n_cd4]);
  multi_array<double, 3> prog(extents[n_sex][n_age][n_cd4-1]);
  multi_array<double, 3> mort(extents[n_sex][n_age][n_cd4]);

  double hivpop[n_year * n_sex * n_age * n_cd4];
  multi_array_ref<double, 4> Y(hivpop, extents[n_year][n_sex][n_age][n_cd4]);
  double influx[n_cd4], efflux[n_cd4];

  memset(hivpop, 0, n_year * n_sex * n_age * n_cd4 * sizeof(double));

  for (int si(0); si < n_sex; ++si) {
    for (int ai(0); ai < n_age; ++ai) {
      for (int hi(0); hi < n_cd4; ++hi) dist[si][ai][hi] = in_dist[si][a2g[ai]][hi];
      for (int hi(0); hi < n_cd4; ++hi) mort[si][ai][hi] = in_mort[si][a2g[ai]][hi] * dt; // 1.0 - exp(-in_mort[si][a2g[ai]][hi] * dt);
      for (int hi(0); hi < n_cd4-1; ++hi) prog[si][ai][hi] = in_prog[si][a2g[ai]][hi] * dt; // 1.0 - exp(-in_prog[si][a2g[ai]][hi] * dt);
    }
  }

  // Year 1 simulation
  for (int si(0); si < n_sex; ++si) {
    for (int ai(0); ai < n_age; ++ai) {
      for (int hi(0); hi < n_cd4; ++hi) {
        Y[0][si][ai][hi] = inci[0][si][ai] * dist[si][ai][hi];
      }
    }
  }

  // Subsequent year simulation
  for (int yi(1); yi < n_year; ++yi) {

    // Aging and non-HIV mortality
    for (int si(0); si < n_sex; ++si) {
      for (int hi(0); hi < n_cd4; ++hi) {
        for (int ai(1); ai < n_age; ++ai) Y[yi][si][ai][hi] = Y[yi-1][si][ai-1][hi] * surv[yi][si][ai];
        Y[yi][si][n_age-1][hi] += Y[yi-1][si][n_age-1][hi] * surv[yi][si][n_age-1];
      }
    }

    // HIV disease progression and mortality
    for (int ti(0); ti < n_ts; ++ti) {
      for (int si(0); si < n_sex; ++si) {
        for (int ai(0); ai < n_age; ++ai) {
          efflux[n_cd4-1] = Y[yi][si][ai][n_cd4-1] * mort[si][ai][n_cd4-1];
          for (int hi(0); hi < n_cd4-1; ++hi) efflux[hi] = Y[yi][si][ai][hi] * (prog[si][ai][hi] + mort[si][ai][hi]);
          for (int hi(1); hi < n_cd4; ++hi) influx[hi] = Y[yi][si][ai][hi-1] * prog[si][ai][hi-1];
          for (int hi(0); hi < n_cd4; ++hi) Y[yi][si][ai][hi] += influx[hi] - efflux[hi];
        }
      }
    }

    // Distribute new HIV infections
    for (int si(0); si < n_sex; ++si) {
      for (int ai(0); ai < n_age; ++ai) {
        for (int hi(0); hi < n_cd4; ++hi) Y[yi][si][ai][hi] += inci[yi][si][ai] * dist[si][ai][hi];
      }
    }
  }

  NumericVector rval(n_year * n_sex * n_age * n_cd4);
  for (int i(0); i < rval.length(); ++i) rval[i] = hivpop[i];
  return rval; // in R, convert to array via array(rval, c(n_cd4, n_age, n_sex, n_year))
}


/*
 * ietl.cpp
 *
 *  Created on: Jan 19, 2012
 *      Author: david
 */

#include "../Common.hpp"
#include "../as_range.hpp"
#include <boost/graph/adjacency_list.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <ietl/interface/ublas.h>
#include <ietl/vectorspace.h>
#include <ietl/lanczos.h>
#include <boost/random.hpp>
#include <boost/limits.hpp>
#include <limits>
#include <iostream>
#include <vector>
#include <cmath>

namespace graphseg { namespace detail {

std::vector<EigenComponent> solver_ietl(const SparseMatrix& A, unsigned int num_ev)
{
	typedef boost::numeric::ublas::symmetric_matrix<
		double, boost::numeric::ublas::lower> Matrix;
	typedef boost::numeric::ublas::vector<double> Vector;

	int N = A.dim;
	Matrix mat(N, N);
	for(int i=0;i<N;i++)
		for(int j=0;j<=i;j++)
			mat(i,j) = 0;
	for(auto& e : A.entries)
		mat(e.i,e.j) = e.weight;

	typedef ietl::vectorspace<Vector> Vecspace;
	typedef boost::lagged_fibonacci607 Gen;

	Vecspace vec(N);
	Gen mygen;
	ietl::lanczos<Matrix,Vecspace> lanczos(mat,vec);

	// Creation of an iteration object:
	int max_iter = 10*N;
	double rel_tol = 50*std::numeric_limits<double>::epsilon();
	double abs_tol = 0.00001f;// std::pow(std::numeric_limits<double>::epsilon(),2./3);
	std::cout << "Computation of 2 lowest converged eigenvalues\n\n";
	std::cout << "-----------------------------------\n\n";
	int n_lowest_eigenval = num_ev;
	std::vector<double> eigen;
	std::vector<double> err;
	std::vector<int> multiplicity;
	ietl::lanczos_iteration_nlowest<double> iter(max_iter, n_lowest_eigenval, rel_tol, abs_tol);
	try{
		lanczos.calculate_eigenvalues(iter,mygen);
		//lanczos.more_eigenvalues(iter);
		eigen = lanczos.eigenvalues();
		err = lanczos.errors();
		multiplicity = lanczos.multiplicities();
		std::cout<<"number of iterations: "<<iter.iterations()<<"\n";
	}
	catch (std::runtime_error& e) {
		std::cout << e.what() << "\n";
	}

  // Printing eigenvalues with error & multiplicities:
	std::cout << "#        eigenvalue            error         multiplicity\n";
	std::cout.precision(10);
	for (int i=0;i<eigen.size();++i)
		std::cout << i << "\t" << eigen[i] << "\t" << err[i] << "\t" << multiplicity[i] << "\n";

	// call of eigenvectors function follows:
	std::cout << "\nEigen vectors computations for lowest eigenvalues:\n\n";
	auto ew_begin = eigen.begin();
	while(*ew_begin <= 0.0f) ew_begin ++;
	auto ew_end = ew_begin + num_ev;
	std::vector<Vector> eigenvectors; // for storing the eigen vectors.
	ietl::Info<double> info; // (m1, m2, ma, eigenvalue, residualm, status).

	try {
		lanczos.eigenvectors(ew_begin,ew_end,std::back_inserter(eigenvectors),info,mygen);
	}
	catch (std::runtime_error& e) {
		std::cout << e.what() << "\n";
	}

	// std::cout << "Printing eigenvectors:\n\n";
	// for(std::vector<Vector>::iterator it = eigenvectors.begin();it!=eigenvectors.end();it++){
	// 	std::copy((it)->begin(),(it)->end(),std::ostream_iterator<double>(std::cout,", "));
	// 	std::cout << "\n\n";
	// }
	std::cout << " Information about the eigenvector computations:\n\n";
	for(int i = 0; i < info.size(); i++) {
		std::cout << " m1(" << i+1 << "): " << info.m1(i) << ", m2(" << i+1 << "): "
			<< info.m2(i) << ", ma(" << i+1 << "): " << info.ma(i) << " eigenvalue("
			<< i+1 << "): " << info.eigenvalue(i) << " residual(" << i+1 << "): "
			<< info.residual(i) << " error_info(" << i+1 << "): "
			<< info.error_info(i) << "\n\n";
	}

  	std::vector<EigenComponent> solution(eigenvectors.size());
	for(unsigned int i=0; i<solution.size(); i++) {
		EigenComponent& cmp = solution[i];
		cmp.eigenvalue = *(ew_begin + i);
#ifdef SPECTRAL_VERBOSE
		std::cout << "Eigenvalue " << i << ": " << cmp.eigenvalue << std::endl;
		cmp.eigenvector = Eigen::VectorXf(N);
#endif
		for(unsigned int j=0; j<N; j++) {
			// convert back to generalized eigenvalue problem!
			cmp.eigenvector[j] = eigenvectors[i][j];
		}
	}
#ifdef SPECTRAL_VERBOSE
	std::cout << "Sparse Solver: returning" << std::endl;
#endif
	return solution;
}

}}


#include <boost/test/unit_test.hpp>

#include <iostream>
#include <memory>

#include "common/services/workRp.hpp"

namespace cs = ble::src::common::services;

namespace ble::tests::unit_tests::common::services::work_rp {

void case1_kw()
{
    // arrange
    double expected = 0.0;
    double s = 0.;
    double n = 2.0;

    // act
    double actual = cs::rp::get_kw(s, n);

    // assert
    BOOST_CHECK_CLOSE(expected, actual, 1e-8);
}

void case2_kw()
{
    // arrange
    double expected = 1.0;
    double s = 1.;
    double n = 2.0;

    // act
    double actual = cs::rp::get_kw(s, n);

    // assert
    BOOST_CHECK_CLOSE(expected, actual, 1e-8);
}

void case1_koil()
{
    // arrange
    double expected = 1.0;
    double s = 0.;
    double n = 2.0;

    // act
    double actual = cs::rp::get_koil(s, n);

    // assert
    BOOST_CHECK_CLOSE(expected, actual, 1e-8);
}

void case2_koil()
{
    // arrange
    double expected = 0.0;
    double s = 1.;
    double n = 2.0;

    // act
    double actual = cs::rp::get_koil(s, n);

    // assert
    BOOST_CHECK_CLOSE(expected, actual, 1e-8);
}

void case1_fbl()
{
    // arrange
    double expected = 1.0;
    double s = 1.;
    double kmu = 0.125; // = mw / moil;
    double n = 2.0;

    // act
    double actual = cs::rp::get_fbl(s, n, kmu);

    // assert
    BOOST_CHECK_CLOSE(expected, actual, 1e-8);
}

void case2_fbl()
{
    // arrange
    double expected = 0.0;
    double s = 0.;
    double kmu = 0.125; // = mw / moil;
    double n = 2.0;

    // act
    double actual = cs::rp::get_fbl(s, n, kmu);

    // assert
    BOOST_CHECK_CLOSE(expected, actual, 1e-8);
}

void case3_fbl()
{
    // arrange
    double expected = 0.2;
    double s = 0.2;
    double kmu = 1.0; // = mw / moil;
    double n = 1.0;

    // act
    double actual = cs::rp::get_fbl(s, n, kmu);

    // assert
    BOOST_CHECK_CLOSE(expected, actual, 1e-8);
}

void case1_sigma()
{
    // arrange
    double expected = 1.0;
    double s = 0.2;
    double kmu = 1.0; // = mw / moil;
    double n = 1.0;

    // act
    double actual = cs::rp::get_sigma(s, n, kmu);

    // assert
    BOOST_CHECK_CLOSE(expected, actual, 1e-8);
}

void case2_sigma()
{
    // arrange
    double expected = 0.1;
    double s = 0.0;
    double kmu = 0.1; // = mw / moil;
    double n = 2.0;

    // act
    double actual = cs::rp::get_sigma(s, n, kmu);

    // assert
    BOOST_CHECK_CLOSE(expected, actual, 1e-8);
}

void case1_dfbl()
{
    // arrange
    double expected = 0.0;
    double s = 0.0;
    double kmu = 0.1; // = mw / moil;
    double n = 2.0;

    // act
    double actual = cs::rp::get_dfbl(s, n, kmu);

    // assert
    BOOST_CHECK_CLOSE(expected, actual, 1e-8);
}

void case2_dfbl()
{
    // arrange
    double expected = 2.076124567474048; // from wxmaxima
    double s = 0.6;
    double kmu = 2.0; // = mw / moil;
    double n = 2.0;

    // act
    double actual = cs::rp::get_dfbl(s, n, kmu);

    // assert
    BOOST_CHECK_CLOSE(expected, actual, 1e-8);
}

}


#ifndef _HIGHER_ORDER_ENERGY_HPP_
#define _HIGHER_ORDER_ENERGY_HPP_

/*
 * higher-order-energy.hpp
 *
 * Copyright 2012 Alexander Fix
 * See LICENSE.txt for license information
 *
 * A representation for higher order pseudo-boolean energy functions
 *
 * Built up by calls to AddVar and AddTerm
 * Main operation is the reduction to quadratic form, ToQuadratic()
 * ToQuadratic() impements the reduction described in
 *  A Graph Cut Algorithm for Higher Order Markov Random Fields
 *  Alexander Fix, Artinan Gruber, Endre Boros, Ramin Zabih, ICCV 2011
 *
 * Template parameters:
 *  Energy   The result type of the energy function
 *  D        The maximum degree of any monomial
 *
 * Example usage:
 *
 * To represent the function
 *      f(x_0, ..., x_3) = 7x_0 + 9x_1 - 8x_2 + 13x_3 + 27x_1x_2x_3
 *                       - 31x_1x_3x_4 + 18x_1x_2x_3x_4
 * we can do the following:
 *
 * HigherOrderEnergy<int, 4> f;
 * f.AddVars(4);
 * f.AddUnaryTerm(0, 7);
 * f.AddUnaryTerm(1, 9);
 * f.AddUnaryTerm(2, -8);
 * f.AddUnaryTerm(3, 13);
 *
 * VarId term1[] = {1, 2, 3};
 * VarId term2[] = {1, 3, 4};
 * VarId term3[] = {1, 2, 3, 4};
 * f.AddTerm(27, 3, term1);
 * f.AddTerm(-31, 3, term2);
 * f.AddTerm(18, 4, term3);
 *
 *
 * Then, we can get an equivalent quadratic form (by adding additional
 * variables and performing transformations) by calling ToQuadratic. We can
 * solve this using QPBO, or any other quadratic optimizer that has the same
 * interface.
 *
 * QPBO<int> qr;
 * f.ToQuadratic(qr);
 * qr.Solve();
 *
 *
 */

#include <vector>
#include <list>
#include <boost/foreach.hpp>

template <typename R, int D>
class HigherOrderEnergy {
    public:
        typedef int VarId;

        // Constructs empty HigherOrderEnergy with no variables or terms
        HigherOrderEnergy();

        // Adds variables to the HigherOrderEnergy. Variables must be added
        // before any terms referencing them can be added
        VarId AddVar();
        VarId AddVars(int n);
        VarId NumVars() const { return _varCounter; }

        // Adds a monomial to the HigherOrderEnergy. degree must be <= D
        // vars is an array of length at least degree, with the indices of
        // the corresponding variables or literals in the monomial
        void AddTerm(R coeff, int degree, const VarId vars[]);

        void AddUnaryTerm(VarId v, R coeff);

        // Reduces the HigherOrderEnergy to quadratic form
        // NOTE: THIS IS A DESTRUCTIVE OPERATION, so do not rely on the
        // HigherOrderEnergy being in any useful state after this operation.
        //
        // This is a templated function, so it will work with any class
        // implementing the necessary interface. See quadratic-rep.hpp for
        // the minimal requirements.
        template <typename QR>
        void ToQuadratic(QR &qr);

        void Clear();

    private:
        struct Term
        {
            R coeff;
            int degree;
            VarId vars[D];

            Term(R _coeff, int _degree, const VarId _vars[])
                : coeff(_coeff), degree(_degree)
            {
                for (int i = 0; i < degree; ++i)
                    vars[i] = _vars[i];
            }

            bool operator<(const Term& t) const;
            bool operator==(const Term& t) const;

            std::string ToString() const;

        };
        static int Compare(int d1,
                const VarId vars1[],
                int d2,
                const VarId vars2[]);

        struct VarRecord {
            VarRecord(VarId id)
                : _id(id), _positiveTerms(0), _higherOrderTerms(0),
                _quadraticTerms(0), _sumDegrees(0), _terms(), _coeff(0) { }
            VarId _id;
            int _positiveTerms;
            int _higherOrderTerms;
            int _quadraticTerms;
            int _sumDegrees;
            std::list<Term> _terms;
            R _coeff;

            void PrintTerms() const;
        };

        R _constantTerm;

        void RemoveTerm(Term* tp);
        void _EliminatePositiveTerms();
        template <typename QR>
        void _ReduceNegativeTerms(QR& qr);
        void _ReportMultilinearStats();

        size_t NumTerms() const {
            size_t numTerms = 0;
            BOOST_FOREACH(const VarRecord& vr, _varRecords)
                numTerms += vr._terms.size();
            return numTerms;
        }

        VarId _varCounter;

        typedef std::vector<VarRecord> varRecordVec_t;
        varRecordVec_t _varRecords;
};

template <typename R, int D>
inline HigherOrderEnergy<R, D>::HigherOrderEnergy()
    : _constantTerm(0), _varCounter(0), _varRecords()
{ }

template <typename R, int D>
inline typename HigherOrderEnergy<R, D>::VarId
HigherOrderEnergy<R, D>::AddVar() {
    VarRecord vr(_varCounter);
    _varRecords.push_back(vr);
    return _varCounter++;
}

template <typename R, int D>
inline typename HigherOrderEnergy<R, D>::VarId
HigherOrderEnergy<R, D>::AddVars(int n) {
    VarId firstVar = _varCounter;
    for (int i = 0; i < n; ++i)
        this->AddVar();
    return firstVar;
}

template <typename R, int D>
inline void
HigherOrderEnergy<R, D>::AddTerm(R coeff, int d, const VarId vars[]) {
    if(coeff == 0) {
        return;
    } else if (d == 0) {
        _constantTerm += coeff;
        return;
    } else if (d == 1) {
        _varRecords[vars[0]]._coeff += coeff;
        return;
    } else {
        VarRecord& smallestVarRec = _varRecords[vars[0]];
        typename std::list<Term>::iterator it = smallestVarRec._terms.begin();
        int compareVars = 1;
        while (it != smallestVarRec._terms.end()) {
            compareVars = Compare(d, vars, it->degree, it->vars);
            if (compareVars == 0) {
                break;
            } else if (compareVars < 0) {
                break;
            } else {
                ++it;
            }
        }
        if (compareVars == 0) {
            it->coeff += coeff;
        } else {
            if (d > 2) {
                smallestVarRec._higherOrderTerms++;
                smallestVarRec._sumDegrees += d;
            } else {
                smallestVarRec._quadraticTerms++;
            }
            smallestVarRec._terms.insert(it, Term(coeff, d, vars));
        }
        if (coeff > 0)
            smallestVarRec._positiveTerms++;
        return;
    }
}

template <typename R, int D>
inline void HigherOrderEnergy<R, D>::AddUnaryTerm(VarId var, R coeff) {
    _varRecords[var]._coeff += coeff;
}

template <typename R, int D>
void HigherOrderEnergy<R, D>::_EliminatePositiveTerms() {
    size_t numVars = _varRecords.size();
    for (size_t varIndex = 0; varIndex < numVars; ++varIndex) {
        R positiveSum = 0;
        VarId newPosVar = AddVar();

        VarRecord& vr = _varRecords[varIndex];

        typename std::list<Term>::iterator termIt = vr._terms.begin();
        VarId newVars[D];
        while (termIt != vr._terms.end()) {
            Term& t = *termIt;
            //std::cout << "\t" << t.ToString() << std::endl;
            typename std::list<Term>::iterator currIt = termIt;
            ++termIt;

            if (t.coeff > 0) {
                positiveSum += t.coeff;
                for (int i = 0; i < t.degree - 1; ++i) {
                    newVars[i] = t.vars[i+1];
                    assert(newVars[i] >= vr._id);
                }
                AddTerm(t.coeff, t.degree - 1, newVars);
                newVars[t.degree - 1] = newPosVar;
                AddTerm(-t.coeff, t.degree, newVars);
                if (t.degree == 2) {
                    vr._quadraticTerms--;
                } else {
                    vr._higherOrderTerms--;
                    vr._sumDegrees -= t.degree;
                }
                vr._terms.erase(currIt);
            }
        }
        VarId quadratic[2];
        quadratic[0] = vr._id;
        quadratic[1] = newPosVar;
        AddTerm(positiveSum, 2, quadratic);
    }
}

template <typename R, int D>
template <typename QR>
void HigherOrderEnergy<R, D>::_ReduceNegativeTerms(QR& qr) {
    // Estimate expected size of quadratic problem. Only nodes/edges are
    // created below, so we can count them ahead of time
    int expectedVars = _varCounter;
    int expectedEdges = 0;
    BOOST_FOREACH(const VarRecord& vr, _varRecords) {
        expectedVars += vr._higherOrderTerms;
        expectedEdges += vr._quadraticTerms;
        expectedEdges += vr._sumDegrees;
    }

    //std::cout << "\tExpected Vars: " << expectedVars << "\tExpected Edges: " << expectedEdges << std::endl;

    qr.SetMaxEdgeNum(expectedEdges);
    qr.AddNode(_varCounter);

    // Term-by-term reduction from Friedman & Drineas
    BOOST_FOREACH(VarRecord& vr, _varRecords) {
        BOOST_FOREACH(Term& t, vr._terms) {
            if (t.degree == 2) {
                qr.AddPairwiseTerm(t.vars[0], t.vars[1], 0, 0, 0, t.coeff);
            } else {
                typename QR::NodeId w = qr.AddNode();
                assert(t.coeff <= 0);
                for (int i = 0; i < t.degree; ++i) {
                    qr.AddPairwiseTerm(t.vars[i], w, 0, 0, 0, t.coeff);
                }
                qr.AddUnaryTerm(w, 0, t.coeff*(1-t.degree));
            }
        }
    }
    BOOST_FOREACH(VarRecord& vr, _varRecords) {
        qr.AddUnaryTerm(vr._id, 0, vr._coeff);
    }
}

template <typename R, int D>
template <typename QR>
inline void HigherOrderEnergy<R, D>::ToQuadratic(QR& qr) {
    _EliminatePositiveTerms();
    _ReduceNegativeTerms(qr);
}

template <typename R, int D>
inline int HigherOrderEnergy<R, D>::Compare(int d1, const VarId vars1[], int d2, const VarId vars2[]) {
    if (d1 < d2)
        return -1;
    if (d1 > d2)
        return 1;
    for (int index = 0; index < d1; ++index) {
        if (vars1[index] != vars2[index])
            return (vars1[index] < vars2[index]) ? -1 : 1;
    }
    return 0;
}

template <typename R, int D>
inline void HigherOrderEnergy<R, D>::Clear() {
    _varRecords.clear();
};

#endif


/******************************************************************************
 *
 *  filename: Ray.h
 *  author  : Do Won Cha
 *  content :
 *
 *****************************************************************************/

#pragma once
#ifndef _RAY_RAY_
#define _RAY_RAY_

#include <Eigen/Core>
#include <iostream>

namespace raytracer
{

using namespace Eigen;

class Ray
{
public:
  Ray() { }

  Ray(Vector3f position,
      Vector3f direction) :
  position_(position),
  direction_(direction)
  { }

  ~Ray() {}

  Vector3f evaluate(float t) const { return position_ + direction_ * t; }

  Vector3f position() const { return position_; }
  Vector3f direction() const { return direction_; }

  friend std::ostream& operator << (std::ostream& s, const Ray& ray)
  {
	  return s << "";
  }

private:
  Vector3f position_;
  Vector3f direction_;
};

}     // end of namespace raytracer

#endif // _RAY_RAY_


/*
 * math.cpp
 *
 *  Created on: Jan 11, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   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.
 */

//libraries
#include <Eigen/Eigen>
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <boost/shared_ptr.hpp>

//local
#include "../math/typedefs.hpp"
#include "../math/statistics.hpp"
#include "../math/transformation.hpp"

namespace bp = boost::python;

namespace python_export {

void export_math_types() {
	// =============================================================================================
	// region === MATH TYPES =======================================================================
	// =============================================================================================
	bp::class_<math::Vector2i>("Vector2i", bp::init<int, int>())
			.def(bp::init<int>())
			.def_readwrite("x", &math::Vector2i::x)
			.def_readwrite("y", &math::Vector2i::y)
			.def_readwrite("u", &math::Vector2i::u)
			.def_readwrite("v", &math::Vector2i::v)
			;
	bp::class_<math::Vector3i>("Vector3i", bp::init<int, int, int>())
				.def(bp::init<int>())
				.def_readwrite("x", &math::Vector3i::x)
				.def_readwrite("y", &math::Vector3i::y)
				.def_readwrite("z", &math::Vector3i::z)
				;

	bp::class_<math::Vector2f>("Vector2f", bp::init<float, float>())
			.def(bp::init<float>())
			.def_readwrite("x", &math::Vector2f::x)
			.def_readwrite("y", &math::Vector2f::y)
			.def_readwrite("u", &math::Vector2f::u)
			.def_readwrite("v", &math::Vector2f::v)
			;

}

void export_math_functions(){
	bp::def("mean_vector_length", &math::mean_vector_length,
			"Computes the mean vector length for all vectors in the given field", bp::args("vector_field"));

	bp::def("transformation_vector_to_matrix3d", &math::transformation_vector_to_matrix3d,
			"Convert 6X1 transformation vector to 4X4 matrix in homogeneous coordinates", bp::args("vector"));
}

} // namespace python_export


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Author: Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/graph_utility.hpp>

int main()
{
    using namespace boost;
    enum
    {
        A,
        B,
        C,
        D,
        E,
        F,
        N
    };
    const char* name = "ABCDEF";

    // A directed graph

    typedef adjacency_matrix< directedS > Graph;
    Graph g(N);
    add_edge(B, C, g);
    add_edge(B, F, g);
    add_edge(C, A, g);
    add_edge(C, C, g);
    add_edge(D, E, g);
    add_edge(E, D, g);
    add_edge(F, A, g);

    std::cout << "vertex set: ";
    print_vertices(g, name);
    std::cout << std::endl;

    std::cout << "edge set: ";
    print_edges(g, name);
    std::cout << std::endl;

    std::cout << "out-edges: " << std::endl;
    print_graph(g, name);
    std::cout << std::endl;

    // An undirected graph

    typedef adjacency_matrix< undirectedS > UGraph;
    UGraph ug(N);
    add_edge(B, C, ug);
    add_edge(B, F, ug);
    add_edge(C, A, ug);
    add_edge(D, E, ug);
    add_edge(F, A, ug);

    std::cout << "vertex set: ";
    print_vertices(ug, name);
    std::cout << std::endl;

    std::cout << "edge set: ";
    print_edges(ug, name);
    std::cout << std::endl;

    std::cout << "incident edges: " << std::endl;
    print_graph(ug, name);
    std::cout << std::endl;
    return 0;
}


//! [substract]
#include <boost/simd/function/load.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/store.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/simd/pack.hpp>
#include <cstdlib>
#include <iomanip>
#include <iostream>
#include <numeric>
#include <vector>

template <typename T>
void print(std::string mes, const T& out)
{
  std::cout << mes << std::endl;
  for (size_t i = 0; i < out.size(); ++i) {
    if (i && (i % 8 == 0))
      std::cout << std::endl;
    std::cout << std::setw(5) << out[i] << " ";
  }
  std::cout << std::endl << std::endl;
}

int main()
{
  namespace bs = boost::simd;
  size_t size  = 128;
  std::vector<int32_t> array(size);
  std::vector<int32_t> out(size);
  // Initialize input array
  std::iota(array.begin(), array.end(), 0);
  int32_t scalar = 42;

  using pack_t     = bs::pack<int32_t>;
  size_t pack_card = bs::cardinal_of<pack_t>();

  // Scalar version
  for (size_t i = 0; i < size; ++i) {
    out[i] = array[i] - scalar;
  }
  print("scalar loop output", out);

  {
    pack_t p_out, pkvalue(scalar);
    for (size_t i = 0; i < size; i += pack_card) {
      pack_t p_arr(array.data() + i);
      p_out = p_arr - pkvalue;
      bs::store(p_out, out.data() + i);
    }
    print("SIMD 1 loop output", out);
  }

  {
    pack_t p_out, p_arr, pkvalue{scalar};
    for (size_t i = 0; i < size; i += pack_card) {
      p_arr = bs::load<pack_t>(array.data() + i);
      p_out = p_arr - pkvalue;
      bs::store(p_out, out.data() + i);
    }
    print("SIMD 2 loop output", out);
  }

  {
    // set size to an arbitrary value
    size = 133;
    pack_t p_out, p_arr, pkvalue{scalar};
    array.resize(size);
    out.resize(size);
    std::iota(array.begin(), array.end(), 0);
    size_t i = 0;
    for (; i + pack_card <= size; i += pack_card) {
      p_arr = bs::load<pack_t>(array.data() + i);
      p_out = p_arr - pkvalue;
      bs::store(p_out, out.data() + i);
    }

    for (; i < size; ++i) {
      out[i] = array[i] - scalar;
    }
    print("SIMD 3 loop output", out);
  }
  return 0;
}
// This code can be compiled using (for instance for gcc)
// g++ substract.cpp -msse4.2 -std=c++11 -O3 -DNDEBUG -o substract
// -I/path_to/boost_simd/ -I/path_to/boost/
//! [substract]


#ifdef COMPILATION_INSTRUCTIONS
nvcc -x cu --expt-relaxed-constexpr`#$CXX` -Wno-deprecated-declarations $0 -o $0x -lcudart -lcublas -lboost_unit_test_framework \
`pkg-config --libs blas` -DBOOST_LOG_DYN_LINK -lboost_system&&$0x&&rm $0x;exit
#endif
// © Alfredo A. Correa 2019-2020

#define BOOST_TEST_MODULE "C++ Unit Tests for Multi cuBLAS herk"
#define BOOST_TEST_DYN_LINK
#include<boost/test/unit_test.hpp>

#include "../../../adaptors/cuda.hpp" // multi::cuda ns

#include "../../../adaptors/blas/cuda.hpp"
#include "../../../adaptors/blas/herk.hpp"
#include "../../../adaptors/blas/gemm.hpp"

#include "../../../array.hpp"

#include <boost/log/expressions.hpp>

namespace multi = boost::multi;
namespace cuda = multi::cuda;

BOOST_AUTO_TEST_CASE(multi_blas_cuda_herk_complex){
#if 1
	using complex = std::complex<double>;
	complex const I{0, 1};

	multi::array<complex, 2> const a = {
		{ 1. + 3.*I, 3.- 2.*I, 4.+ 1.*I},
		{ 9. + 1.*I, 7.- 8.*I, 1.- 3.*I}
	};
	{
		multi::array<complex, 2> c({2, 2}, 9999.);
		namespace blas = multi::blas;
		using blas::herk;
		herk(a, c);
		BOOST_REQUIRE( c[1][0] == complex(50., -49.) );
		BOOST_REQUIRE( c[0][1] == complex(50., +49.) );

		multi::array<complex, 2> const c_copy = herk(1., a);
		BOOST_REQUIRE( c == c_copy );

		using blas::gemm;
		using blas::hermitized;
		BOOST_REQUIRE( gemm(a, hermitized(a)) == herk(a) );
	}
	{
		cuda::array<complex, 2> const acu = a; BOOST_REQUIRE(a == acu);
		cuda::array<complex, 2> ccu({2, 2}, 9999.);
		using multi::blas::herk;
		herk(acu, ccu);
		BOOST_REQUIRE( ccu[1][0] == complex(50., -49.) );
		BOOST_REQUIRE( ccu[0][1] == complex(50., +49.) );

		cuda::array<complex, 2> const ccu_copy = herk(1., acu);
		BOOST_REQUIRE( herk(1., acu) == ccu );
	}
	{
		cuda::managed::array<complex, 2> const amcu = a; BOOST_REQUIRE(a == amcu);
		cuda::managed::array<complex, 2> cmcu({2, 2}, 9999.);
		using multi::blas::herk;
		herk(1., amcu, cmcu);
		BOOST_REQUIRE( cmcu[1][0] == complex(50., -49.) );
		BOOST_REQUIRE( cmcu[0][1] == complex(50., +49.) );

		cuda::managed::array<complex, 2> const cmcu_copy = herk(1., amcu);
		BOOST_REQUIRE( cmcu_copy == cmcu );
	}
	{
		multi::array<complex, 2> c({3, 3}, 9999.);
		using multi::blas::herk;
		using multi::blas::hermitized;
		herk(1., hermitized(a), c);
		BOOST_REQUIRE( c[2][1] == complex(41, +2) );
		BOOST_REQUIRE( c[1][2] == complex(41, -2) );

		multi::array<complex, 2> const c_copy = herk(1., hermitized(a));
		BOOST_REQUIRE( c_copy == c );
	}
	{
		cuda::array<complex, 2> const acu = a; BOOST_REQUIRE(a == acu);
		cuda::array<complex, 2> ccu({3, 3}, 9999.);
		using multi::blas::herk;
		using multi::blas::hermitized;
		herk(1., hermitized(acu), ccu);
		BOOST_REQUIRE( ccu[2][1] == complex(41, +2) );
		BOOST_REQUIRE( ccu[1][2] == complex(41, -2) );

		cuda::array<complex, 2> const ccu_copy = herk(1., hermitized(acu));
		BOOST_REQUIRE( ccu_copy == ccu );
	}
	{
		cuda::managed::array<complex, 2> const acu = a; BOOST_REQUIRE(a == acu);
		cuda::managed::array<complex, 2> ccu({3, 3}, 9999.);
		using multi::blas::herk;
		using multi::blas::hermitized;
		herk(1., hermitized(acu), ccu);
		BOOST_REQUIRE( ccu[2][1] == complex(41, +2) );
		BOOST_REQUIRE( ccu[1][2] == complex(41, -2) );

		cuda::managed::array<complex, 2> const ccu_copy = herk(1., hermitized(acu));
		BOOST_REQUIRE( ccu_copy == ccu );
	}
#endif
}

#if 0
BOOST_AUTO_TEST_CASE(multi_blas_cuda_herk_real){
	multi::array<double, 2> const a = {
		{ 1., 3., 4.},
		{ 9., 7., 1.}
	};
	{
		multi::array<double, 2> c({2, 2}, 9999);
		using multi::blas::herk;
		herk(1., a, c);
		BOOST_REQUIRE( c[1][0] == 34 );
		BOOST_REQUIRE( c[0][1] == 34 );

		multi::array<double, 2> const c_copy = herk(1., a);
		BOOST_REQUIRE( c == c_copy );
	}
	{
		cuda::array<double, 2> const acu = a; BOOST_REQUIRE(a == acu);
		cuda::array<double, 2> ccu({2, 2}, 9999.);
		using multi::blas::herk;
		herk(1., acu, ccu);
		BOOST_REQUIRE( ccu[1][0] == 34 );
		BOOST_REQUIRE( ccu[0][1] == 34 );

		cuda::array<double, 2> const ccu_copy = herk(1., acu);
		BOOST_REQUIRE( herk(1., acu) == ccu );
	}
	{
		cuda::array<double, 2> const acu = a; BOOST_REQUIRE(a == acu);
	//	cuda::array<double, 2> ccu({2, 2}, 9999.);
		using multi::blas::herk;
		cuda::array<double, 2> ccu = herk(acu);
		BOOST_REQUIRE( ccu[1][0] == 34 );
		BOOST_REQUIRE( ccu[0][1] == 34 );

		cuda::array<double, 2> const ccu_copy = herk(1., acu);
		BOOST_REQUIRE( herk(1., acu) == ccu );
	}
	{
		cuda::managed::array<double, 2> const amcu = a; BOOST_REQUIRE(a == amcu);
		cuda::managed::array<double, 2> cmcu({2, 2}, 9999.);
		using multi::blas::herk;
		herk(1., amcu, cmcu);
		BOOST_REQUIRE( cmcu[1][0] == 34 );
		BOOST_REQUIRE( cmcu[0][1] == 34 );

		cuda::managed::array<double, 2> const cmcu_copy = herk(1., amcu);
		BOOST_REQUIRE( cmcu_copy == cmcu );
	}
	if(0){
		multi::array<double, 2> c({3, 3}, 9999.);
		using multi::blas::herk;
		using multi::blas::hermitized;
		herk(1., hermitized(a), c);
		BOOST_REQUIRE( c[2][1] == 19 );
		BOOST_REQUIRE( c[1][2] == 19 );

		multi::array<double, 2> const c_copy = herk(1., hermitized(a));
		BOOST_REQUIRE( c_copy == c );
	}
	if(0){
		cuda::array<double, 2> const acu = a; BOOST_REQUIRE(acu == a);
		cuda::array<double, 2> ccu({3, 3}, 9999.);
		using multi::blas::herk;
		using multi::blas::hermitized;
		herk(1., hermitized(acu), ccu);
		BOOST_REQUIRE( ccu[2][1] == 19 );
		BOOST_REQUIRE( ccu[1][2] == 19 );

		cuda::array<double, 2> const c_copy = herk(1., hermitized(a));
		BOOST_REQUIRE( c_copy == ccu );
	}
	if(0){
		cuda::managed::array<double, 2> const amcu = a; BOOST_REQUIRE(amcu == a);
		cuda::managed::array<double, 2> cmcu({3, 3}, 9999.);
		using multi::blas::herk;
		using multi::blas::hermitized;
		herk(1., hermitized(amcu), cmcu);
		BOOST_REQUIRE( cmcu[2][1] == 19 );
		BOOST_REQUIRE( cmcu[1][2] == 19 );

		cuda::managed::array<double, 2> const c_copy = herk(1., hermitized(a));
		BOOST_REQUIRE( c_copy == cmcu );
	}
}
#endif


// SPDX-License-Identifier: Apache-2.0
// Copyright 2021 - 2021, the Anboto author and contributors
#include <Core/Core.h>
#include <ScatterDraw/DataSource.h>
#include <Eigen/Eigen.h>
#include <ScatterDraw/Histogram.h>

namespace Upp {

Histogram::Histogram() {
	Clear();
}

Histogram::Histogram(const Histogram &hist) {
	*this = hist;
}

Histogram &Histogram::operator=(const Histogram &hist) {
	totalVals = hist.totalVals;

	values = hist.values;
	ranges <<= hist.ranges;

	return *this;
}

void Histogram::Clear() {
	ranges.Clear();
	values.resize(0);
}

Histogram &Histogram::Create2D(const Vector<Vector<double> > &_ranges, const Vector<double> &data, double total) {
	Clear();

	int numAxis = 2;
	valuesIdx.SetNumAxis(numAxis);
	for (int ix = 0; ix < numAxis; ++ix)
		valuesIdx.SetAxisDim(ix, _ranges[ix].GetCount());
	values.resize(valuesIdx.size());
	values.setZero();
	ranges.SetCount(numAxis);
	for (int ix = 0; ix < numAxis; ++ix) {
		ranges[ix].SetCount(_ranges[ix].GetCount());
		for (int d = 0; d < _ranges[ix].GetCount(); ++d)
			ranges[ix][d] = _ranges[ix][d];
	}
	totalVals = total;
	for (int i = 0; i < data.GetCount(); ++i)
		values(ptrdiff_t(i)) = data[i];
	return *this;
}

Histogram &Histogram::Create(Upp::Array<HistogramDataAxis> &dataAxis, bool isY) {
	Clear();

	int numAxis = dataAxis.GetCount();
	valuesIdx.SetNumAxis(numAxis);
	for (int ix = 0; ix < numAxis; ++ix)
		valuesIdx.SetAxisDim(ix, dataAxis[ix].numVals);
	values.resize(valuesIdx.size());
	values.setZero();
	ranges.SetCount(numAxis);
	Vector<double> delta;
	delta.SetCount(numAxis);
	for (int ix = 0; ix < numAxis; ++ix) {
		ranges[ix].SetCount(dataAxis[ix].numVals);
		delta[ix] = (dataAxis[ix].max - dataAxis[ix].min + 1)/dataAxis[ix].numVals;
		for (int d = 0; d < dataAxis[ix].numVals; ++d)
			ranges[ix][d] = (d + 1)*delta[ix];
	}

	totalVals = double(dataAxis[0].data.GetCount());
	Vector<int> index;
	index.SetCount(numAxis);
	for (int64 i = 0; i < totalVals; ++i) {
		for (int ix = 0; ix < numAxis; ++ix) {
			double d = isY ? dataAxis[ix].data.y(i) : dataAxis[ix].data.x(i);
			if (!!IsNum(d)) {
				double val = d - dataAxis[ix].min;
				val = val/delta[ix];
				if (val >= dataAxis[ix].numVals)
					val = dataAxis[ix].numVals - 1;
				else if (val < 0)
					val = 0;
				ASSERT(int(val) >= 0 && int(val) < dataAxis[ix].numVals);
				index[ix] = int(val);
			} else
				index[ix] = Null;
		}
		if (!IsNull(index[0]))
			values(valuesIdx.GetIndex(index))++;
	}
	return *this;
}

Histogram &Histogram::Create(DataSource &data, double min, double max, int numVals, bool isY) {
	Clear();

	values.resize(numVals);
	values.setZero();
	ranges.SetCount(1);
	ranges[0].SetCount(numVals);
	double delta = (max - min)/numVals;
	for (int ii = 0; ii < numVals; ++ii)
		ranges[0][ii] = min + (ii + 0.5)*delta;

	int64 total = data.GetCount();
	totalVals = 0;
	for (int64 i = 0; i < total; ++i) {
		double d = isY ? data.y(i) : data.x(i);
		if (!!IsNum(d)) {
			double val = (d - min)/delta;
			if (val >= numVals)
				val = numVals - 1;
			else if (val < 0)
				val = 0;
			values(int(val))++;
			totalVals++;
		}
	}
	return *this;
}

Histogram &Histogram::Normalize(double val) {
	if (totalVals == val)
		return *this;

	if (totalVals > 0)
		values *= val/totalVals;
	totalVals = val;
	return *this;
}

double Histogram::Compare(const Histogram &hist) {
	ASSERT(hist.values.size() == values.size());
	double res = 0;
	ptrdiff_t size = values.size();
	for(ptrdiff_t i = 0; i < size; i++)
		res += min(values[i], hist.values[i]);
	res /= totalVals;
	return res;
}

}


#include "drake/solvers/integer_inequality_solver.h"

#include <algorithm>
#include <functional>
#include <iostream>
#include <vector>

#include <Eigen/Dense>

#include "drake/common/drake_assert.h"

namespace drake {
namespace solvers {
namespace {

using IntegerVectorList = std::vector<std::vector<int>>;
using SolutionList = Eigen::Matrix<int, -1, -1, Eigen::RowMajor>;

bool IsElementwiseNonnegative(const Eigen::MatrixXi& A) {
  return (A.array() >= 0).all();
}

bool IsElementwiseNonpositive(const Eigen::MatrixXi& A) {
  return (A.array() <= 0).all();
}

/* Construct finite-set of admissible values, i.e., an alphabet, for each
 * coordinate from specified upper and lower bounds, e.g., a lower bound and
 * upper bound of -1, 2 translates to the alphabet [-1, 0, 1, 2]. This function
 * exists only to simplify the external interface.
*/
IntegerVectorList BuildAlphabetFromBounds(const Eigen::VectorXi& lower_bound,
                                          const Eigen::VectorXi& upper_bound) {
  DRAKE_ASSERT(lower_bound.size() == upper_bound.size());

  IntegerVectorList alphabet(lower_bound.size());
  int cnt = 0;

  for (auto& col_alphabet : alphabet) {
    DRAKE_ASSERT(lower_bound(cnt) <= upper_bound(cnt));

    for (int i = lower_bound(cnt); i <= upper_bound(cnt); i++) {
      col_alphabet.push_back(i);
    }
    cnt++;
  }

  return alphabet;
}

/* If a column Ai of A is nonnegative (resp. nonpositive), then  {Ai*z : z ∈ Qi}
 * is totally ordered, where Qi is the alphabet for the iᵗʰ component. In other
 * words, the inequalities Ai z1 ≤ Ai z2 ≤ ... ≤ Ai zm hold for zj ∈ Qi
 * sorted in ascending (resp. descending) order. This allows for infeasibility
 * propagation in the recursive enumeration of integer solutions.  This function
 * detects when {Ai z : z ∈ Qi} is totally ordered and then sorts the alphabet
 * using this ordering.
 */
enum class ColumnType { Nonnegative, Nonpositive, Indefinite };
std::vector<ColumnType> ProcessInputs(const Eigen::MatrixXi& A,
                                      IntegerVectorList* alphabet) {
  DRAKE_ASSERT(alphabet);
  int cnt = 0;
  std::vector<ColumnType> column_type(A.cols(), ColumnType::Indefinite);

  for (auto& col_alphabet : *alphabet) {
    if (IsElementwiseNonnegative(A.col(cnt))) {
      std::sort(col_alphabet.begin(), col_alphabet.end());
      column_type[cnt] = ColumnType::Nonnegative;
    } else if (IsElementwiseNonpositive(A.col(cnt))) {
      std::sort(col_alphabet.begin(), col_alphabet.end(), std::greater<int>());
      column_type[cnt] = ColumnType::Nonpositive;
    }
    cnt++;
  }

  return column_type;
}

/* Given an integer z and a list of integer vectors V, constructs
 * the Cartesian product (v, z) for all v ∈ V */
SolutionList CartesianProduct(const SolutionList& V, int z) {
  SolutionList cart_products(V.rows(), V.cols() + 1);
  cart_products << V, SolutionList::Constant(V.rows(), 1, z);

  return cart_products;
}

SolutionList VerticalStack(const SolutionList& A,
                              const SolutionList& B) {
  DRAKE_ASSERT(A.cols() == B.cols());
  if (A.rows() == 0) {
    return B;
  }
  if (B.rows() == 0) {
    return A;
  }
  SolutionList Y(A.rows() + B.rows(), B.cols());
  Y << A, B;
  return Y;
}

/* Find each solution (x(1), x(2), ..., x(n)) to Ax <= b when x(i) can only take
on values in a finite alphabet Qi, e.g., Qi = {1, 2, 3, 8, 9}.   We do this
by recursively enumerating the solutions to
A (x(1), x(2), ..., x(n-1) ) <= (b - A_n x_n) for all values of x(n). If the
column vectors {An*z : z ∈ Qn} are totally ordered, we propagate infeasibility:
if no solutions exist when x(n) = z, then no solution can exist if x(n) takes
on values larger than z (in the ordering).  We assume the function
"ProcessInputs" was previously called to sort the alphabet in ascending order.
*/
// TODO(frankpermenter):  Update to use preallocated memory
SolutionList FeasiblePoints(const Eigen::MatrixXi& A,
                               const Eigen::VectorXi& b,
                               const IntegerVectorList& column_alphabets,
                               const std::vector<ColumnType>& column_type,
                               int last_free_var_pos) {
  DRAKE_ASSERT((last_free_var_pos >= 0) && (last_free_var_pos < A.cols()));
  DRAKE_ASSERT(column_type.size() == static_cast<std::vector<ColumnType>
                                                    ::size_type>(A.cols()));

  SolutionList feasible_points(0, last_free_var_pos + 1);
  for (const auto& value : column_alphabets[last_free_var_pos]) {
    SolutionList new_feasible_points;

    if (last_free_var_pos == 0) {
      if (IsElementwiseNonnegative(b - A.col(0) * value)) {
        new_feasible_points.resize(1, 1);
        new_feasible_points(0, 0) = value;
      }
    } else {
      new_feasible_points = CartesianProduct(
          FeasiblePoints(A,
                        b - A.col(last_free_var_pos) * value,
                        column_alphabets,
                        column_type,
                        last_free_var_pos - 1), value);
    }

    if (new_feasible_points.rows() > 0) {
      feasible_points = VerticalStack(feasible_points, new_feasible_points);
    } else {
      //  Propagate infeasibility: if this test passes, then no feasible
      //  points exist for remaining values in the alphabet.
      if (column_type[last_free_var_pos] != ColumnType::Indefinite) {
        return feasible_points;
      }
    }
  }

  return feasible_points;
}

}  // namespace

SolutionList EnumerateIntegerSolutions(
    const Eigen::Ref<const Eigen::MatrixXi>& A,
    const Eigen::Ref<const Eigen::VectorXi>& b,
    const Eigen::Ref<const Eigen::VectorXi>& lower_bound,
    const Eigen::Ref<const Eigen::VectorXi>& upper_bound) {
  DRAKE_DEMAND(A.rows() == b.rows());
  DRAKE_DEMAND(A.cols() == lower_bound.size());
  DRAKE_DEMAND(A.cols() == upper_bound.size());

  auto variable_alphabets = BuildAlphabetFromBounds(lower_bound, upper_bound);
  // Returns type (nonnegative, nonpositive, or indefinite) of A's
  // columns and sorts the variable alphabet accordingly.
  auto column_type = ProcessInputs(A, &variable_alphabets);

  return FeasiblePoints(A, b, variable_alphabets, column_type, A.cols()-1);
}

}  // namespace solvers
}  // namespace drake


/// \file
/// Maintainer: Felice Serena
///
///

#include "uniform_grid.h"
#include <Eigen/Core>
#include <set>

#include <boost/test/unit_test.hpp>

namespace utf = boost::unit_test;

typedef std::vector<int> CellCoordinate;

using namespace MouseTrack;
using namespace Eigen;

BOOST_AUTO_TEST_CASE(uniform_grid_4d) {
  constexpr int Dim = 4;
  typedef UniformGrid4d UG;
  UG::PointList all(Dim, 5);
  all.col(0) = Vector4d(0.0, 0.0, 0.0, 0.0);
  all.col(1) = Vector4d(2.0, 2.0, 2.0, 2.0);
  all.col(2) = Vector4d(10.0, 0.0, 0.0, 0.0);
  all.col(3) = Vector4d(0.0, -1.0, -1.0, -1.0);
  all.col(4) = Vector4d(10.0, 1.0, 1.0, 1.0);

  std::multiset<PointIndex> expected0;
  expected0.insert(2);
  expected0.insert(4);

  std::multiset<PointIndex> expected1;
  expected1.insert(0);
  expected1.insert(3);

  UG oracle(2, 10.0 / 20);
  oracle.compute(all);

  Matrix<double, 4, 2> query;
  query.col(0) = Vector4d(9.5, 0, 0, 0);
  query.col(1) = Vector4d(0.1, 0.0, 0, 0);
  auto result = oracle.find_in_range(query, 2);

  std::multiset<PointIndex> received0(result[0].begin(), result[0].end());
  std::multiset<PointIndex> received1(result[1].begin(), result[1].end());

  BOOST_CHECK_EQUAL(expected0.size(), received0.size());
  BOOST_CHECK_MESSAGE(
      expected0 == received0,
      "Expected and received set do not contain same elements.");

  BOOST_CHECK_EQUAL(expected1.size(), received1.size());
  BOOST_CHECK_MESSAGE(
      expected1 == received1,
      "Expected and received set do not contain same elements.");
}

BOOST_AUTO_TEST_CASE(uniform_grid_4d_two_close_points) {
  constexpr int Dim = 4;
  typedef UniformGrid4d UG;
  UG::PointList all(Dim, 2);
  all.col(0) = Vector4d(0.0, 0.0, 0.0, 0.0);
  all.col(1) = Vector4d(0.1, 0.1, 0.1, 0);

  UG oracle(2, 10.0 / 20);
  oracle.compute(all);

  std::multiset<PointIndex> expected;

  // first test
  expected.insert(0);
  expected.insert(1);
  auto result = oracle.find_in_range(Vector4d(0.0, 0, 0, 0), 1)[0];
  std::multiset<PointIndex> received(result.begin(), result.end());

  BOOST_CHECK_EQUAL(expected.size(), received.size());
  BOOST_CHECK_MESSAGE(
      expected == received,
      "Expected and received set do not contain same elements.");
}

BOOST_AUTO_TEST_CASE(uniform_grid_Xd_find_closest) {
  constexpr int Dim = 4;
  typedef UniformGrid4d UG;
  UG::PointList all(Dim, 5);
  all.col(0) = Vector4d(0.0, 0.0, 0.0, 0.0);
  all.col(1) = Vector4d(2.0, 2.0, 2.0, 2.0);
  all.col(2) = Vector4d(10.0, 0.0, 0.0, 0.0);
  all.col(3) = Vector4d(0.0, -1.0, -1.0, -1.0);
  all.col(4) = Vector4d(10.0, 1.0, 1.0, 1.0);

  UG oracle(2, 10.0 / 20);
  oracle.compute(all);

  std::vector<size_t> expected0;
  expected0.push_back(4);

  std::vector<size_t> expected1;
  expected1.push_back(0);

  Matrix<double, 4, 2> query;
  query.col(0) = Vector4d(10.1, 0.9, 0.9, 0.9);
  query.col(1) = Vector4d(0.1, 0.1, 0.1, 0.1);

  auto result = oracle.find_closest(query, 1);
  BOOST_CHECK_EQUAL(expected0.size(), result[0].size());
  BOOST_CHECK_EQUAL(expected0[0], result[0][0]);

  BOOST_CHECK_EQUAL(expected1.size(), result[1].size());
  BOOST_CHECK_EQUAL(expected1[0], result[1][0]);
}

BOOST_AUTO_TEST_CASE(uniform_grid_Xd_find_closestK) {
  constexpr int Dim = 4;
  typedef UniformGrid4d UG;
  UG::PointList all(Dim, 5);
  all.col(0) = Vector4d(0.0, 0.0, 0.0, 0.0);
  all.col(1) = Vector4d(2.0, 2.0, 2.0, 2.0);
  all.col(2) = Vector4d(10.0, 0.0, 0.0, 0.0);
  all.col(3) = Vector4d(0.0, -1.0, -1.0, -1.0);
  all.col(4) = Vector4d(10.0, 1.0, 1.0, 1.0);

  std::multiset<PointIndex> expected;
  expected.insert(2);
  expected.insert(4);

  UG oracle(2, 10.0 / 20);
  oracle.compute(all);

  auto result = oracle.find_closest(Vector4d(10.1, 0.9, 0.9, 0.9), 2)[0];

  std::multiset<PointIndex> received(result.begin(), result.end());

  BOOST_CHECK_MESSAGE(
      expected.size() == received.size(),
      "Expected and received sets have different cardinalities.");
  BOOST_CHECK_MESSAGE(
      expected == received,
      "Expected and received set do not contain same elements.");
}


/**
 * @file k_means_clustering_engine.hpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-06-26
 */
#ifndef PAAL_K_MEANS_CLUSTERING_ENGINE_HPP
#define PAAL_K_MEANS_CLUSTERING_ENGINE_HPP

#include "paal/utils/type_functions.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/irange.hpp"

#include <boost/range/combine.hpp>
#include <boost/range/adaptor/indexed.hpp>
#include <boost/range/algorithm_ext/iota.hpp>
#include <boost/range/algorithm/for_each.hpp>

#include <vector>
#include <random>
#include <iostream>

namespace paal {

/**
 * @param lrange
 * @param rrange
 * @tparam RangeLeft
 * @tparam RangeRight
 */
template <class RangeLeft, class RangeRight>
auto distance_square(RangeLeft && lrange, RangeRight && rrange) {
    assert(!boost::empty(lrange));
    assert(boost::distance(lrange) == boost::distance(rrange));

    //TODO change to sum_functors when generic lambdas appears
    decltype(*std::begin(lrange) * *std::begin(rrange)) dist{};
    for (auto point_pair : boost::combine(lrange, rrange)) {
        auto diff = boost::get<0>(point_pair) - boost::get<1>(point_pair);
        dist += diff * diff;
    }
    return dist;
}

/**
 * @param point
 * @param centers
 * @tparam Point
 * @tparam Centers
 */
template <class Point, class Centers>
auto closest_to(Point && point, Centers && centers){
    using coor_t = range_to_elem_t<Point>;
    auto dist = std::numeric_limits<coor_t>::max();
    int new_center = 0;
    for (auto center : centers | boost::adaptors::indexed()){
        auto new_dist = distance_square(center.value(), point);

        if (new_dist < dist) {
            dist = new_dist;
            new_center = center.index();
        }
    }
    return new_center;
}

///k means visitor
struct k_means_visitor {
    /**
    * @param last_center
    * @param new_center
    * @tparam Center
    * @tparam New_center
    */
    template <class Center, class New_center>
    void move_center(Center &last_center, New_center &new_center) {};
    ///new iteration
    void new_iteration() {};
};

/**
 * @param points
 * @param centers
 * @param centroid functor return centroid of set of samples
 * @param closest_to
 * @param result pairs of point and id of cluster
 * (number from 0,1,2 ...,k-1)
 * @param c_equal
 * @param visitor
 * @tparam Points
 * @tparam Centers
 * @tparam Centroid
 * @tparam ClosestTo
 * @tparam OutputIterator
 * @tparam CentroidEqual
 * @tparam Visitor
 */
template <class Points,
          class Centers,
          class Centroid,
          class ClosestTo, class OutputIterator,
          class CentroidEqual = utils::equal_to,
          class Visitor=k_means_visitor >
auto k_means(Points &&points, Centers & centers,
             Centroid centroid, ClosestTo closest_to,
             OutputIterator result,
             CentroidEqual c_equal = CentroidEqual{},
             Visitor visitor=Visitor{}) {
    using point_t = range_to_elem_t<Points>;
    using points_bag = std::vector<point_t>;

    std::vector<points_bag> cluster_points;
    cluster_points.resize(centers.size());
    bool zm;
    do {
        visitor.new_iteration();
        zm = false;
        boost::for_each(cluster_points, std::mem_fn(&points_bag::clear));

        for (auto && point : points) {
            cluster_points[closest_to(point)].push_back(point);
        }

        for (auto point : cluster_points | boost::adaptors::indexed()) {
            if(point.value().empty()) continue;
            auto && old_center = centers[point.index()];
            auto && new_center = centroid(point.value());
            if (!c_equal(new_center, old_center)) {
                visitor.move_center(old_center, new_center);
                old_center = new_center;
                zm = true;
            }
        }
    } while (zm == true);
    for (int cur_cluster : irange(cluster_points.size())) {
        for (auto const & point : cluster_points[cur_cluster]) {
            *result = std::make_pair(point, cur_cluster);
            ++result;
        }
    }
    return centers;
}

/**
 * @param points
 * @param number_of_centers
 * @tparam Points
 */
template <typename Points, typename OutputIterator, typename RNG = std::default_random_engine>
auto get_random_centers(Points &&points, int number_of_centers, OutputIterator out,
                        RNG && rng = std::default_random_engine{}) {

    std::vector<int> centers(points.size());
    boost::iota(centers, 0);
    std::shuffle(centers.begin(),centers.end(), rng);
    centers.resize(number_of_centers);
    for (auto && center : centers) {
        *out=points[center];
        ++out;
    }
}

/**
 * @param points
 * @param number_of_clusters
 * @tparam Points
 */
template <typename Points, typename RNG = std::default_random_engine>
auto get_random_clusters(Points &&points, int number_of_clusters,
                         RNG && rng = std::default_random_engine{}) {
    std::vector<typename std::decay<Points>::type> clusters(number_of_clusters);
    std::uniform_int_distribution<> dis(0, number_of_clusters - 1);

    for (auto o : points) {
        clusters[distribution(rng)].push_back(o);
    }
    return clusters;
}

} //!paal

#endif /* PAAL_K_MEANS_CLUSTERING_ENGINE_HPP */


//=============================================================================
//
//  CLASS NConstraintGmmInterface
//
//=============================================================================

#ifndef COMISO_LINEARCONSTRAINT_HH
#define COMISO_LINEARCONSTRAINT_HH

//== INCLUDES =================================================================

#include <CoMISo/Config/CoMISoDefines.hh>
#include "NConstraintInterface.hh"
#include <Eigen/StdVector>

//== FORWARDDECLARATIONS ======================================================

//== NAMESPACES ===============================================================

namespace COMISO {

//== CLASS DEFINITION =========================================================

/** \class NProblemGmmInterface NProblemGmmInterface.hh <ACG/.../NPRoblemGmmInterface.hh>

    Brief Description.

    A more elaborate description follows.
*/
class COMISODLLEXPORT LinearConstraint : public NConstraintInterface
{
public:

  // sparse vector type
  typedef NConstraintInterface::SVectorNC SVectorNC;

  // different types of constraints
//  enum ConstraintType {NC_EQUAL, NC_LESS_EQUAL, NC_GREATER_EQUAL};

  /// Default constructor
  LinearConstraint(const ConstraintType _type = NC_EQUAL);

  // linear equation of the form -> coeffs_^T *x  + b_=_type= 0
  LinearConstraint(const SVectorNC& _coeffs, const double _b, const ConstraintType _type = NC_EQUAL);

  /// Destructor
  virtual ~LinearConstraint();

  virtual int n_unknowns();

  void  resize(const unsigned int _n);

  const SVectorNC& coeffs() const;
        SVectorNC& coeffs();

  const double&    b() const;
        double&    b();

  virtual double eval_constraint ( const double* _x );

  virtual void eval_gradient( const double* _x, SVectorNC& _g      );

  virtual void eval_hessian    ( const double* _x, SMatrixNC& _h      );

  virtual bool is_linear() { return true;}

  // inherited from base
//  virtual ConstraintType  constraint_type (                                      ) { return type_; }

private:

  // linear equation of the form -> coeffs_^T * x + b_
  SVectorNC coeffs_;
  double    b_;
};

//=============================================================================
} // namespace COMISO
//=============================================================================
// support std vectors
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(COMISO::LinearConstraint);
//=============================================================================
#endif // ACG_LINEARCONSTRAINT_HH defined
//=============================================================================


// Small executable to fill a PCD file
// Author: Max Schwarz <max.schwarz@uni-bonn.de>

#include <depth_filler/depth_filler.h>
#include <depth_filler/domain_transform.h>

#include <boost/program_options.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

namespace po = boost::program_options;

void imagesToPointCloud(const cv::Mat_<float>& depth, const cv::Mat_<cv::Vec3b>& rgb,
		float fx, float fy, float cx, float cy,
		pcl::PointCloud<pcl::PointXYZRGB>& output)
{
	output.resize(rgb.rows*rgb.cols);
	output.width = rgb.cols;
	output.height = rgb.rows;

	unsigned int outputIdx = 0;
	for(unsigned int y = 0; y < output.height; ++y)
	{
		for(unsigned int x = 0; x < output.width; ++x)
		{
			pcl::PointXYZRGB& p = output[outputIdx];

			p.r = rgb(y,x)[2];
			p.g = rgb(y,x)[1];
			p.b = rgb(y,x)[0];

			p.z = depth(y,x);
			p.x = p.z * (x - cx) / fx;
			p.y = p.z * (y - cy) / fy;

			outputIdx++;
		}
	}
}

struct Blob
{
	std::vector<cv::Point2i> pixels;
	cv::Rect rect;

	bool operator<(const Blob& other) const
	{ return pixels.size() > other.pixels.size(); }
};

void FindBlobs(const cv::Mat &binary, std::vector < Blob > &blobs)
{
    blobs.clear();

    // Fill the label_image with the blobs
    // 0  - background
    // 1  - unlabelled foreground
    // 2+ - labelled foreground

    cv::Mat label_image;
    binary.convertTo(label_image, CV_32SC1);

    int label_count = 2; // starts at 2 because 0,1 are used already

    for(int y=0; y < label_image.rows; y++) {
        int *row = (int*)label_image.ptr(y);
        for(int x=0; x < label_image.cols; x++) {
            if(row[x] != 1) {
                continue;
            }

            Blob blob;
            cv::floodFill(label_image, cv::Point(x,y), label_count, &blob.rect, 0, 0, 8);

            for(int i=blob.rect.y; i < (blob.rect.y+blob.rect.height); i++) {
                int *row2 = (int*)label_image.ptr(i);
                for(int j=blob.rect.x; j < (blob.rect.x+blob.rect.width); j++) {
                    if(row2[j] != label_count) {
                        continue;
                    }

                    blob.pixels.push_back(cv::Point2i(j,i));
                }
            }

            blobs.push_back(blob);

            label_count++;
        }
    }
}

int main(int argc, char** argv)
{
	float fx = NAN;
	float fy = NAN;
	float cx = -1;
	float cy = -1;
	unsigned int erode = 0;
	unsigned int subsample = 1;

	po::options_description desc("Options");
	desc.add_options()
		("help", "produce help message")
		("fx", po::value<float>(), "set focal length (x)")
		("fy", po::value<float>(), "set focal length (y)")
		("cx", po::value<float>(), "set optical center (x)")
		("cy", po::value<float>(), "set optical center (y)")
		("benchmark", "Run 20 times for profiling")
		("erode", po::value<unsigned int>(), "erode depth with specified kernel half-size")
		("subsample", po::value<unsigned int>(), "subsample input")
		("dump", po::value<std::string>(), "dump sparse system under prefix")
		("dump-prefill", po::value<std::string>(), "dump prefill cloud")
		("dump-input", po::value<std::string>(), "dump (subsampled) input cloud")
		("method", po::value<std::string>()->default_value("domain"), "method (domain or opt)")
	;

	po::options_description hidden("Hidden");
	hidden.add_options()
		("input-file", po::value<std::string>(), "input file")
		("output-file", po::value<std::string>(), "output file")
	;

	po::options_description cmdline;
	cmdline.add(desc).add(hidden);

	po::positional_options_description p;
	p.add("input-file", 1);
	p.add("output-file", 1);

	po::variables_map vm;
	po::store(po::command_line_parser(argc, argv).options(cmdline).positional(p).run(), vm);
	po::notify(vm);

	if(vm.count("help"))
	{
		std::cout << desc << "\n";
		return 1;
	}

	if(!vm.count("input-file") || !vm.count("output-file"))
	{
		fprintf(stderr, "Require input and output file!\n");
		std::cerr << desc << "\n";
		return 1;
	}

	pcl::PointCloud<pcl::PointXYZRGB> input;
	pcl::io::loadPCDFile(vm["input-file"].as<std::string>(), input);

	if(input.size() == 0)
	{
		fprintf(stderr, "Could not read input file\n");
		return 1;
	}

	if(!input.isOrganized())
	{
		fprintf(stderr, "Error: Input is not organized!\n");
		return 1;
	}

	if(vm.count("fx") && vm.count("fy") && vm.count("cx") && vm.count("cy"))
	{
		fx = vm["fx"].as<float>();
		fy = vm["fy"].as<float>();
		cx = vm["cx"].as<float>();
		cy = vm["cy"].as<float>();
	}
	else
	{
		// Estimate from PCD...
		cx = input.width/2 + 0.5f;
		cy = input.height/2 + 0.5f;
		fx = 0.0f;
		fy = 0.0f;
		unsigned int count = 0;

		unsigned int idx = 0;
		for(int y = 0; y < (int)input.height; ++y)
		{
			for(int x = 0; x < (int)input.width; ++x)
			{
				const auto& point = input[idx++];

				if(!std::isfinite(point.z))
					continue;

				if((x - cx) * (y - cy) * point.z != 0)
				{
					fx += point.z / point.x * (x - cx);
					fy += point.z / point.y * (y - cy);
					count++;
				}
			}
		}

		fx /= count;
		fy /= count;

		printf("Calculated focal lengths: fx=%f, fy=%f\n", fx, fy);
	}

	if(vm.count("subsample"))
	{
		subsample = vm["subsample"].as<unsigned int>();
		fx /= subsample;
		fy /= subsample;
		cx = (cx - 0.5f) / subsample + 0.5f;
		cy = (cy - 0.5f) / subsample + 0.5f;
	}

	cv::Mat_<float> depth(input.height/subsample, input.width/subsample);
	depth = NAN;

	cv::Mat_<cv::Vec3b> rgb(input.height, input.width);

	int inputIdx = 0;
	for(unsigned int y = 0; y < input.height; ++y)
	{
		for(unsigned int x = 0; x < input.width; ++x)
		{
			const auto& point = input[inputIdx];

			auto dx = x / subsample;
			auto dy = y / subsample;

			if(std::isnan(depth(dy,dx)) || depth(dy,dx) > point.z)
				depth(dy,dx) = point.z;

			rgb(y,x) = cv::Vec3b(point.b, point.g, point.r);

			inputIdx++;
		}
	}

	cv::Mat_<cv::Vec3b> rgbOut;
	cv::resize(rgb, rgbOut, cv::Size(rgb.cols / subsample, rgb.rows / subsample), fx, fy, cv::INTER_AREA);

	pcl::PointCloud<pcl::PointXYZRGB> output;

	if(vm.count("dump-input"))
	{
		imagesToPointCloud(depth, rgbOut, fx, fy, cx, cy, output);
		if(pcl::io::savePCDFileBinary(vm["dump-input"].as<std::string>(), output) < 0)
		{
			fprintf(stderr, "Could not write prefill file\n");
		}
	}

	depth_filler::DepthFiller filler;

	// Fill in holes in depth data under very strict conditions
	cv::Mat_<float> tmp = filler.prefill(depth);
	depth = tmp;

	if(vm.count("erode"))
	{
		erode = vm["erode"].as<unsigned int>();
		filler.erodeDepth(depth, depth, erode);
	}

	cv::Mat_<float> filled;

	if(vm["method"].as<std::string>() == "opt")
	{
		if(!vm.count("benchmark"))
			filled = filler.fillDepth(depth, rgbOut);
		else
		{
			for(int i = 0; i < 20; ++i)
				filled = filler.fillDepth(depth, rgbOut);
		}
	}
	else if(vm["method"].as<std::string>() == "domain")
	{
		cv::Mat_<uint8_t> grayU;
		cv::cvtColor(rgbOut, grayU, cv::COLOR_BGR2GRAY);
		cv::Mat_<float> gray;
		grayU.convertTo(gray, CV_32FC1);

		depth_filler::DomainTransformFiller dmFiller;

		if(!vm.count("benchmark"))
			filled = dmFiller.fillDepth(depth, gray);
		else
		{
			for(int i = 0; i < 600; ++i)
				filled = dmFiller.fillDepth(depth, gray);
		}
	}
	else
		throw std::runtime_error("Unknown method specified");

	imagesToPointCloud(filled, rgbOut, fx, fy, cx, cy, output);
	if(pcl::io::savePCDFileBinary(vm["output-file"].as<std::string>(), output) < 0)
	{
		fprintf(stderr, "Could not write output file\n");
	}

	if(vm.count("dump-prefill"))
	{
		imagesToPointCloud(depth, rgbOut, fx, fy, cx, cy, output);
		if(pcl::io::savePCDFileBinary(vm["dump-prefill"].as<std::string>(), output) < 0)
		{
			fprintf(stderr, "Could not write prefill file\n");
		}
	}

	if(vm.count("dump"))
	{
		filler.dumpSystem(vm["dump"].as<std::string>());
	}

	return 0;
}


#include <gtest/gtest.h>

#include "scheme/objective/voxel/FieldCache.hh"

#include <random>
#include <boost/foreach.hpp>

#include <sstream>

namespace scheme { namespace objective { namespace voxel { namespace fctest {

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

struct Ellipse3D : Field3D <double> {
	static size_t ncalls;
	static double sqr(double f) { return f*f; }
	double c1,c2,c3,sd1,sd2,sd3;
	Ellipse3D() : c1(0),c2(0),c3(0),sd1(1),sd2(1),sd3(1) {}
	Ellipse3D(double a,double b,double c,double d,double e,double f) : c1(a),c2(b),c3(c),sd1(d),sd2(e),sd3(f) {}
	double operator()(double f, double g, double h) const {
		++ncalls;
		return 10.0+std::exp( -sqr((f-c1)/sd1) - sqr((g-c2)/sd2) - sqr((h-c3)/sd3) );
	}
	template<class F3> double operator()(F3 const & f3) const { return this->operator()(f3[0],f3[1],f3[2]); }
};
size_t Ellipse3D::ncalls = 0;

template<class Cache,class Field>
double test_cache_vs_field(Cache const & cache, Field field, int nsamp=10000){
	// cout << cache.num_elements()/1000000.0 <<"M" << endl;
	BOOST_STATIC_ASSERT((Cache::DIM==3));
	typedef util::SimpleArray<3,double> F3;
	std::mt19937 rng((unsigned int)time(0));
	std::uniform_real_distribution<> uniform;
	double maxerr = 0.0;
	for(int i = 0; i < nsamp; ++i){
		F3 samp = F3(uniform(rng),uniform(rng),uniform(rng)) * (cache.ub_-cache.lb_) + cache.lb_;
		double cacheval = cache[samp];
		double fieldval = field(samp);
		// std::cout << cacheval << " " << fieldval << std::endl;
		maxerr = std::max( std::abs(fieldval-cacheval), maxerr );
		if( std::abs(fieldval-cacheval) > 10.0 ){
			cout << samp << " " << fieldval << " " << cacheval << endl;
			cout << cache.lb_ << endl;
			cout << cache.ub_ << endl;
		}
	}
	return maxerr;
}

TEST(FieldCache,test_accuracy_random){

	Ellipse3D field(1,2,3,4,5,6);
	ASSERT_FLOAT_EQ( field(1,2,3), 11.0 );

	ASSERT_LE( test_cache_vs_field( FieldCache3D<double>(field,-10,13,1.6), field, 100000 ), 0.30 );
	ASSERT_LE( test_cache_vs_field( FieldCache3D<double>(field,-10,13,0.8), field, 100000 ), 0.15 );
	ASSERT_LE( test_cache_vs_field( FieldCache3D<double>(field,-10,13,0.4), field, 100000 ), 0.08 );
	ASSERT_LE( test_cache_vs_field( FieldCache3D<double>(field,-10,13,0.2), field, 100000 ), 0.04 );

}

TEST(FieldCache,test_file_cache){
	#ifdef CEREAL
		std::string tmpfile = "FieldCache_test_file.bin.gz";
		if(boost::filesystem::exists(tmpfile)) boost::filesystem::remove(tmpfile);

		Ellipse3D field(1,2,3,4,5,6);
		FieldCache3D<double> f1(field,-11,13,1.6,tmpfile);
		size_t ncalls = Ellipse3D::ncalls;

		FieldCache3D<double> f2(field,-11,13,1.6,tmpfile);

		ASSERT_EQ( Ellipse3D::ncalls, ncalls ); // field not called again
		ASSERT_EQ( f1 , f2 );

		cout << "PING 81" << endl;

		f1.resize( util::SimpleArray<3,size_t>(0,0,0) ); // spoil the cache
		io::write_cache(tmpfile,f1);

		std::cout << "NOTE: the bounds-mismatch warning below is expected:" << std::endl;
		FieldCache3D<double> f3(field,-11,13,1.6,tmpfile);
		ASSERT_NE( Ellipse3D::ncalls, ncalls ); // field must be called again
		ASSERT_EQ( f2.shape()[0] , f3.shape()[0] );
		ASSERT_EQ( f2.shape()[1] , f3.shape()[1] );
		ASSERT_EQ( f2.shape()[2] , f3.shape()[2] );
		ASSERT_EQ( f2 , f3 );
	#endif

}

TEST(BoundingFieldCache,test_bounding_ellipse_max){
	typedef util::SimpleArray<3,double> F3;
	Ellipse3D field(1,2,3,4,5,6);
	FieldCache3D<double> f1(field,-11,13,0.824234);
	BoundingFieldCache3D<double,AggMax> bf1(f1,2.873,1.234);

	std::mt19937 rng((unsigned int)time(0));
	std::uniform_real_distribution<> uniform;
	for(int i = 0; i < 10000; ++i){
		F3 idx = F3( uniform(rng), uniform(rng), uniform(rng) ) * (f1.ub_-f1.lb_) + f1.lb_;
		if( f1[idx] > bf1[idx] ) cout << idx << endl;
		ASSERT_LE( f1[idx] , bf1[idx] );
	}
}

struct Delta : Field3D<double> {
	double operator()(double f, double g, double h) const {
		if( fabs(f) < 0.1 && fabs(g) < 0.1 && fabs(h) < 0.1 ) return 1.0;
		return 0;
	}

};

TEST(BoundingFieldCache,test_bounding_delta){
	typedef util::SimpleArray<3,double> F3;
	Delta delta;
	ASSERT_EQ( delta(0.0,0.0,0.0), 1 );
	ASSERT_EQ( delta(0.2,0.0,0.0), 0 );
	FieldCache3D<double> f1(delta,-7.5,7.5,1.0);
	ASSERT_EQ( f1[F3(0,0,0)], 1.0 );
	double sum = 0; for(size_t i = 0; i < f1.num_elements(); ++i) sum += f1.data()[i];
	ASSERT_EQ( sum, 1.0 );

	double spread = 5.4;
	BoundingFieldCache3D<double,AggMax> bf1(f1,spread,1.0);
	std::mt19937 rng((unsigned int)time(0));
	std::uniform_real_distribution<> uniform;
	for(int i = 0; i < 100000; ++i){
		F3 idx = F3( uniform(rng), uniform(rng), uniform(rng) ) * (bf1.ub_-bf1.lb_) + bf1.lb_;
		// if( idx.norm() < spread-sqrt(3.0) && bf1[idx]!=1.0) cout << "FAIL1 " << idx << " " << idx.norm()-spread << endl;
		// if( idx.norm() > spread+sqrt(3.0) && bf1[idx]!=0.0) cout << "FAIL0 " << idx << " " << idx.norm()-spread << endl;
		// not sure if cell diagonal sqrt(3) is tight bound here... seems like it should be sqrt(3)/2...
		if( idx.norm() < spread-sqrt(3.0) ) ASSERT_EQ( bf1[idx], 1.0 );
		if( idx.norm() > spread+sqrt(3.0) ) ASSERT_LE( bf1[idx], 0.00000000001 ); // can be "uninitialized" vals in corners
	}
}

}}}}


/**
 * @file test_IO.cc
 * @author Pedro Henrique S. Perrusi (pedro.perrusi@gmail.com)
 * @brief Unitest module for the bunny_mesh/data_io.h file.
 * @version 1.0
 * @date 2019-02-10
 *
 * @copyright Copyright (c) 2019 Pedro Henrique S. Perrusi
 *
 */
#include "gtest/gtest.h"
#include "bunny_mesh/data_io.h"

#include <Eigen/Dense>
#include <iostream>

using namespace bunny_dataIO;

/**
 * @brief Tests writing and reading integers between numpy array and eigen
 */
TEST(IO, Write_Read_Int)
{
    const std::string filename = "test/data/sequential_int.npy";
    // loads a simple matrix
    IndexMatrixType matEigen(3,3);
    matEigen << 1, 2, 3,
                4, 5, 6,
                7, 8, 9;
    // writes it to file
    saveIntMatrixToNumpyArray(filename, matEigen);

    // reads same matrix from file
    IndexMatrixType matNumpy;
    matNumpy = readIntNumPyArray(filename);

    // Begin testing...
    ASSERT_EQ(matEigen.rows(), matNumpy.rows());
    ASSERT_EQ(matEigen.cols(), matNumpy.cols());
    ASSERT_TRUE(matEigen.isApprox(matNumpy));
}

/**
 * @brief Tests writing and reading doubles between numpy array and eigen
 */
TEST(IO, Write_Read_Double)
{
    const std::string filename = "test/data/sequential_double.npy";
    // loads a simple matrix
    Point3DMatrixType matEigen(3,3);
    matEigen << 1.1, 2.2, 3.3,
                4.4, 5.5, 6.6,
                7.7, 8.8, 9.9;
    // writes it to file
    saveMatrixToNumpyArray(filename, matEigen);

    // reads same matrix from file
    Point3DMatrixType matNumpy;
    matNumpy = readFloatNumPyArray(filename);

    // Begin testing...
    ASSERT_EQ(matEigen.rows(), matNumpy.rows());
    ASSERT_EQ(matEigen.cols(), matNumpy.cols());
    ASSERT_TRUE(matEigen.isApprox(matNumpy));
}