/* fastreg includes */
#include <fastreg.hpp>

/* boost includes */
#include <boost/program_options.hpp>

/* itk includes */
#include <itkImage.h>
#include <itkImageFileReader.h>
#include <itkImageFileWriter.h>
#include <itkNiftiImageIO.h>

/* sys headers */
#include <chrono>
#include <cstdlib>
#include <iostream>
#include <thread>

typedef itk::Image<short, 3> ImageTypeShort;
typedef itk::Image<float, 3> ImageTypeFloat;

typedef itk::ImageFileReader<ImageTypeShort> ImageTypeShortReader;
typedef itk::ImageFileReader<ImageTypeFloat> ImageTypeFloatReader;

/*
 * parse file name from a path
 */

std::string get_file_name(const std::string &s) {
    char sep = '/';

#ifdef _WIN32
    sep = '\\';
#endif

    size_t i = s.rfind(sep, s.length());
    if (i != std::string::npos) {
        return (s.substr(i + 1, s.length() - i));
    }

    return ("");
}

/*
 * app
 */

struct FastRegApp {
    FastRegApp(std::string params_path, std::string target_path, std::string target_labels_path,
               std::string source_path, std::string source_labels_path, std::string out_path, bool verbose,
               bool vverbose)
        : exit_(false),
          params_path_(params_path),
          image_name_1(target_path),
          labels_name_1(target_labels_path),
          image_name_2(source_path),
          labels_name_2(source_labels_path),
          out_path_(out_path),
          verbose_(verbose),
          vverbose_(vverbose) {
        /*
         * initialise parameters
         */

        if (verbose_) {
            params.verbosity = 1;
        } else if (vverbose_) {
            params.verbosity = 2;
        }

        /*
         * declare parameters to read from .ini
         */

        boost::program_options::options_description desc("parameters");
        declare_parameters(desc, params);

        /*
         * read parameters from .ini
         */

        boost::program_options::variables_map vm;
        load_parameters(desc, vm);

        /*
         * initialise sobolevfusion
         */

        fastreg = std::make_shared<FastReg>(params);
    }

    /*
     * declare fastreg parameters
     */

    void declare_parameters(boost::program_options::options_description &desc, Params &params) {
        /*
         * image
         */

        desc.add_options()("VOL_DIMS_X", boost::program_options::value<int>(&params.volume_dims[0]),
                           "no. of voxels along x axis");
        desc.add_options()("VOL_DIMS_Y", boost::program_options::value<int>(&params.volume_dims[1]),
                           "no. of voxels along y axis");
        desc.add_options()("VOL_DIMS_Z", boost::program_options::value<int>(&params.volume_dims[2]),
                           "no. of voxels along z axis");

        desc.add_options()("VOL_SIZE_X", boost::program_options::value<float>(&params.volume_size[0]),
                           "vol. size along x axis (metres)");
        desc.add_options()("VOL_SIZE_Y", boost::program_options::value<float>(&params.volume_size[1]),
                           "vol. size along y axis (metres)");
        desc.add_options()("VOL_SIZE_Z", boost::program_options::value<float>(&params.volume_size[2]),
                           "vol. size along z axis (metres)");

        desc.add_options()("NO_LABELS", boost::program_options::value<int>(&params.no_labels), "no. of segmentations");

        /*
         * solver
         */
        desc.add_options()("MAX_ITER", boost::program_options::value<int>(&params.max_iter),
                           "max. no. of iterations of the solver");
        desc.add_options()("MAX_UPDATE_NORM", boost::program_options::value<float>(&params.max_update_norm),
                           "max. update norm when running the solver");

        /* FASTFUSION */
        desc.add_options()("RHO_0", boost::program_options::value<float>(&params.rho_0), "initial density");

        desc.add_options()("RESET_ITER", boost::program_options::value<int>(&params.reset_iter),
                           "iteration when to reset t in order to prevent underdamping");
        desc.add_options()("TAU", boost::program_options::value<float>(&params.tau), "velocity diffusion coefficient");

        desc.add_options()("ALPHA", boost::program_options::value<float>(&params.alpha), "gradient descent step size");
        desc.add_options()("W_REG", boost::program_options::value<float>(&params.w_reg), "regularisation weight");
        desc.add_options()("p", boost::program_options::value<float>(&params.p),
                           "scaling factor for momentum and potential energy");
        desc.add_options()("C", boost::program_options::value<float>(&params.C),
                           "scaling factor for momentum and potential energy");
    }

    /*
     * read parameters from params.ini
     */

    void load_parameters(boost::program_options::options_description &desc, boost::program_options::variables_map &vm) {
        std::ifstream settings_file(params_path_);

        boost::program_options::store(boost::program_options::parse_config_file(settings_file, desc), vm);
        boost::program_options::notify(vm);
    }

    /*
     * load images
     */

    void load_images() {
        /* initialise file names */
        std::string image_file_name_1 = image_name_1;
        std::string image_file_name_2 = image_name_2;

        /* load images */
        ImageTypeFloatReader::Pointer reader_image_1 = ImageTypeFloatReader::New();
        reader_image_1->SetFileName(image_file_name_1.c_str());
        reader_image_1->Update();

        ImageTypeFloatReader::Pointer reader_image_2 = ImageTypeFloatReader::New();
        reader_image_2->SetFileName(image_file_name_2.c_str());
        reader_image_2->Update();

        /* get the images */
        image_1 = reader_image_1->GetOutput();
        image_2 = reader_image_2->GetOutput();
    }

    /*
     * load labels
     */

    void load_labels() {
        /* init file names */
        std::string labels_file_name_1 = labels_name_1;
        std::string labels_file_name_2 = labels_name_2;

        /* load labels */
        ImageTypeShortReader::Pointer reader_labels_1 = ImageTypeShortReader::New();
        reader_labels_1->SetFileName(labels_file_name_1.c_str());
        reader_labels_1->Update();

        ImageTypeShortReader::Pointer reader_labels_2 = ImageTypeShortReader::New();
        reader_labels_2->SetFileName(labels_file_name_2.c_str());
        reader_labels_2->Update();

        /* get the labels */
        image_labels_1 = reader_labels_1->GetOutput();
        image_labels_2 = reader_labels_2->GetOutput();
    }

    /*
     * save warped images
     */

    void save_warped_images() {
        /*
         * images
         */

        std::string input_1 = image_name_1;

        itk::ImageIOBase::Pointer imageIO =
            itk::ImageIOFactory::CreateImageIO(input_1.c_str(), itk::ImageIOFactory::ReadMode);

        imageIO->SetFileName(input_1);
        imageIO->ReadImageInformation();

        itk::NiftiImageIO::Pointer nifti_io = itk::NiftiImageIO::New();
        nifti_io->SetPixelType(imageIO->GetPixelType());

        itk::ImageFileWriter<ImageTypeFloat>::Pointer writer = itk::ImageFileWriter<ImageTypeFloat>::New();

        /* re-sampled image 1 */
        writer->SetFileName(out_path_ + "/target_psi_inv_" + get_file_name(image_name_2) + ".nii");
        writer->SetInput(image_1);
        writer->SetImageIO(nifti_io);
        writer->Update();
        /* re-sampled image 2 */
        writer->SetFileName(out_path_ + "/source_psi_" + get_file_name(image_name_2) + ".nii");
        writer->SetInput(image_2);
        writer->Update();

        /*
         * labels
         */

        std::string input_2 = labels_name_1;

        itk::ImageIOBase::Pointer imageIO_labels =
            itk::ImageIOFactory::CreateImageIO(input_2.c_str(), itk::ImageIOFactory::ReadMode);

        imageIO_labels->SetFileName(input_2);
        imageIO_labels->ReadImageInformation();

        nifti_io->SetPixelType(imageIO_labels->GetPixelType());
        itk::ImageFileWriter<ImageTypeShort>::Pointer writer_labels = itk::ImageFileWriter<ImageTypeShort>::New();

        /* labels 1 */
        writer_labels->SetFileName(out_path_ + "/target_psi_inv_labels_" + get_file_name(image_name_2) + ".nii");
        writer_labels->SetInput(image_labels_1);
        writer_labels->SetImageIO(nifti_io);
        writer_labels->Update();

        /* labels 2 */
        writer_labels->SetFileName(out_path_ + "/source_psi_labels_" + get_file_name(image_name_2) + ".nii");
        writer_labels->SetInput(image_labels_2);
        writer_labels->SetImageIO(nifti_io);
        writer_labels->Update();

        std::cout << "--- SAVED OUTPUT" << std::endl;
    }

    /*
     * execute
     */

    bool execute() {
        /* fastreg app */
        fastreg = fastreg;

        /* declare device pointers for images */
        float *image_data_1, *image_data_2;
        cudaSafeCall(cudaMalloc((void **) &image_data_1, params.no_voxels() * sizeof(float)));
        cudaSafeCall(cudaMalloc((void **) &image_data_2, params.no_voxels() * sizeof(float)));

        /* declare device pointers for labels */
        short *labels_data_1, *labels_data_2;
        cudaSafeCall(cudaMalloc((void **) &labels_data_1, params.no_voxels() * sizeof(short)));
        cudaSafeCall(cudaMalloc((void **) &labels_data_2, params.no_voxels() * sizeof(short)));

        std::cout << "-------" << std::endl;
        std::cout << "TARGET: " << image_name_1 << ", " << labels_name_1 << std::endl;
        std::cout << "SOURCE: " << image_name_2 << ", " << labels_name_2 << std::endl;

        /*
         * NON-RIGID REGISTRATION
         */

        std::cout << "-------" << std::endl;
        std::cout << "RUNNING NON-RIGID REGISTRATION" << std::endl;

        double time_ms = 0;
        bool has_image = false;

        /* load images and labels */
        load_images();
        load_labels();

        cudaSafeCall(cudaMemcpy(image_data_1, image_1->GetBufferPointer(), params.no_voxels() * sizeof(float),
                                cudaMemcpyHostToDevice));
        cudaSafeCall(cudaMemcpy(labels_data_1, image_labels_1->GetBufferPointer(), params.no_voxels() * sizeof(short),
                                cudaMemcpyHostToDevice));

        cudaSafeCall(cudaMemcpy(image_data_2, image_2->GetBufferPointer(), params.no_voxels() * sizeof(float),
                                cudaMemcpyHostToDevice));
        cudaSafeCall(cudaMemcpy(labels_data_2, image_labels_2->GetBufferPointer(), params.no_voxels() * sizeof(short),
                                cudaMemcpyHostToDevice));

        /* run registartion */
        std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); /* measure time */
        has_image = (*(fastreg))(image_data_1, labels_data_1, image_data_2, labels_data_2);
        std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
        std::cout << "--- REGISTRATION TIME: "
                  << std::chrono::duration_cast<std::chrono::milliseconds>(end - begin).count() << "ms" << std::endl;

        /* copy warped data */
        cudaSafeCall(cudaMemcpy(image_1->GetBufferPointer(), image_data_1, params.no_voxels() * sizeof(float),
                                cudaMemcpyDeviceToHost));
        cudaSafeCall(cudaMemcpy(image_2->GetBufferPointer(), image_data_2, params.no_voxels() * sizeof(float),
                                cudaMemcpyDeviceToHost));

        cudaSafeCall(cudaMemcpy(image_labels_1->GetBufferPointer(), labels_data_1, params.no_voxels() * sizeof(short),
                                cudaMemcpyDeviceToHost));
        cudaSafeCall(cudaMemcpy(image_labels_2->GetBufferPointer(), labels_data_2, params.no_voxels() * sizeof(short),
                                cudaMemcpyDeviceToHost));

        save_warped_images();

        return true;
    }

    std::string image_name_1, labels_name_1, image_name_2, labels_name_2; /* file names */

    /* images and labels */
    ImageTypeFloat::Pointer image_1, image_2;
    ImageTypeShort::Pointer image_labels_1, image_labels_2;

    /* pipeline */
    Params params;
    std::shared_ptr<FastReg> fastreg;

    /* app parameters */
    bool exit_, logger_, verbose_, vverbose_;
    std::string params_path_, out_path_;
};

/*
 * parse the input flags and determine the file paths
 */

void parse_flags(std::vector<std::string> args, std::string *params_path, std::string *target_path,
                 std::string *target_labels_path, std::string *source_path, std::string *source_labels_path,
                 std::string *out_path, bool *verbose, bool *vverbose) {
    std::vector<std::string> flags = {"-h", "--help", "--verbose", "--vverbose"};

    int idx = 0;
    for (auto arg : args) {
        if (std::find(std::begin(flags), std::end(flags), arg) != std::end(flags)) {
            if (arg == "-h" || arg == "--help") {
                std::cout << "USAGE: fastreg [OPTIONS] <ini path> <target path> <target labels path> <source path> "
                             "<source labels path> <out path>"
                          << std::endl;
                std::cout << "\t--help -h:    display help" << std::endl;
                std::cout << "\t--verbose: low verbosity" << std::endl;
                std::cout << "\t--vverbose: high verbosity" << std::endl;
                std::exit(EXIT_SUCCESS);
            }

            if (arg == "--verbose") {
                *verbose = true;
            }
            if (arg == "--vverbose") {
                *vverbose = true;
            }
        } else if (idx == 0) {
            *params_path = arg;
            idx++;
        } else if (idx == 1) {
            *target_path = arg;
            idx++;
        } else if (idx == 2) {
            *target_labels_path = arg;
            idx++;
        } else if (idx == 3) {
            *source_path = arg;
            idx++;
        } else if (idx == 4) {
            *source_labels_path = arg;
            idx++;
        } else if (idx == 5) {
            *out_path = arg;
            idx++;
        }
    }
}

/*
 * main
 */

int main(int argc, char *argv[]) {
    int device = 0;

    /* print info. about the gpu */
    fastreg::cuda::setDevice(device);
    fastreg::cuda::printShortCudaDeviceInfo(device);

    /* program requires at least 5 arguments */
    if (argc < 6) {
        return std::cerr << "error: incorrect number of arguments; please supply path to source and target data and "
                            ".ini file; "
                            "exiting..."
                         << std::endl,
               -1;
    }

    std::vector<std::string> args(argv + 1, argv + argc);
    std::string params_path, target_path, target_labels_path, source_path, source_labels_path, out_path;

    bool verbose  = false;
    bool vverbose = false;

    parse_flags(args, &params_path, &target_path, &target_labels_path, &source_path, &source_labels_path, &out_path,
                &verbose, &vverbose);

    /* execute */
    FastRegApp app(params_path, target_path, target_labels_path, source_path, source_labels_path, out_path, verbose,
                   vverbose);
    app.execute();

    return 0;
}


#pragma once

#include <Eigen/Core>
#include <array>
#include <vector>

#include "Face2d.hpp"
// #include "Node2d.hpp"
#include "Variable.hpp"

class Node2d;
class CellQuad4
{
public:
    CellQuad4();
    CellQuad4(unsigned int id, Node2d* pNode1, Node2d* pNode2, Node2d* pNode3,
              Node2d* pNode4);
    ~CellQuad4();

    unsigned int GetID() const;
    Node2d* GetNode(unsigned int index) const;
    Face2d* Face(unsigned int index);
    void SetNeighborPtr(unsigned int index, CellQuad4* pCellQuad);
    CellQuad4* GetNeighborPtr(unsigned int index) const;
    Eigen::Vector2d GetCentroid() const;
    void SetVectorToNeighbor(unsigned int index);
    Eigen::Vector2d GetVectorToNeighbor(unsigned int index) const;
    Eigen::Vector2d GetNormalVectorToBoundary(unsigned int index) const;
    void SetMag_N1Vectors(unsigned int index);
    double GetMag_N1Vectors(unsigned int index) const;
    Eigen::Vector2d GetvectorToFace(unsigned int index) const;
    Eigen::Vector2d GetVectorToNode(unsigned int index) const;
    double GetVolume() const;

    Variable cellvar;

private:
    unsigned int id_;
    std::array<Node2d*, 4> pNodes_;
    std::array<Face2d, 4> faces_;
    std::array<CellQuad4*, 4> pNeighbors_;
    std::array<Eigen::Vector2d, 4> vectorToNeighbors_;
    std::array<Eigen::Vector2d, 4> normalvectorToBoundaries_;
    std::array<double, 4> mag_n1Vectors_;
    std::array<Eigen::Vector2d, 4> vectorToFaces_;
    std::array<Eigen::Vector2d, 4> vectorToNodes_;

    void CheckFaceNormals();
    void CalcCentroid();
    void CalcVolume();
    void CalcVectorToFaces();
    void CalcVectorToNodes();

    Eigen::Vector2d centroid_;
    double volume_;
};


//  Copyright (c) 2012 Thomas Heller
//
//  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 <hpx/hpx_fwd.hpp>
#include <hpx/async.hpp>
#include <hpx/lcos/local/packaged_continuation.hpp>

#include "stencil_iterator.hpp"

#include <boost/make_shared.hpp>

namespace jacobi
{
    namespace server
    {
        void stencil_iterator::step()
        {

            jacobi::row top = top_future[src].get();
            jacobi::row bottom = bottom_future[src].get();

            HPX_ASSERT(top.id);
            HPX_ASSERT(bottom.id);
            HPX_ASSERT(top.id != bottom.id);
            HPX_ASSERT(this->get_gid() != top.id);
            HPX_ASSERT(this->get_gid() != bottom.id);

            std::vector<hpx::lcos::future<void> > fs;
            for(std::size_t x = 1; x < nx-1; x += line_block)
            {
                std::size_t x_end = (std::min)(nx-1, x + line_block);
                fs.push_back(
                    hpx::async(
                        &stencil_iterator::update
                      , this
                      , rows[dst].get(x, x_end)
                      , rows[src].get(x-1, x_end+1)
                      , top.get(x, x_end)
                      , bottom.get(x, x_end)
                    )
                );
            }
            hpx::wait_all(fs);
            std::swap(src, dst);
        }

        void stencil_iterator::update(
            hpx::lcos::future<row_range> dst
          , hpx::lcos::future<row_range> src
          , hpx::lcos::future<row_range> top
          , hpx::lcos::future<row_range> bottom
        )
        {
            row_range d = dst.get();

            std::vector<double>::iterator dst_ptr = d.begin();
            row_range s = src.get();
            std::vector<double>::iterator src_ptr = s.begin();
            row_range t = top.get();
            std::vector<double>::iterator top_ptr = t.begin();
            row_range b = bottom.get();
            std::vector<double>::iterator bottom_ptr = b.begin();

            HPX_ASSERT(
                d.end() - d.begin() + 2 == s.end() - s.begin()
            );
            HPX_ASSERT(
                d.end() - d.begin() == t.end() - t.begin()
            );
            HPX_ASSERT(
                d.end() - d.begin() == b.end() - b.begin()
            );

            ++src_ptr;
            while(dst_ptr < d.end())
            {
                HPX_ASSERT(dst_ptr < d.end());
                HPX_ASSERT(src_ptr < s.end());
                HPX_ASSERT(top_ptr < t.end());
                HPX_ASSERT(bottom_ptr < b.end());
                *dst_ptr
                    =(
                        *(src_ptr - 1) + *(src_ptr + 1)
                      + *top_ptr + *bottom_ptr
                    ) * 0.25;
                ++dst_ptr;
                ++src_ptr;
                ++top_ptr;
                ++bottom_ptr;
            }
        }

        jacobi::row stencil_iterator::get(std::size_t idx)
        {
            HPX_ASSERT(rows[idx].id);
            return rows[idx];
        }
    }
}

typedef hpx::components::managed_component<
    jacobi::server::stencil_iterator
> stencil_iterator_type;

HPX_REGISTER_COMPONENT(stencil_iterator_type, stencil_iterator);

HPX_REGISTER_ACTION(
    jacobi::server::stencil_iterator::init_action
  , jacobi_server_stencil_iterator_init_action
)

HPX_REGISTER_ACTION(
    jacobi::server::stencil_iterator::setup_boundary_action
  , jacobi_server_stencil_iterator_setup_boundary_action
)

HPX_REGISTER_ACTION(
    jacobi::server::stencil_iterator::step_action
  , jacobi_server_stencil_iterator_step_action
)

HPX_REGISTER_ACTION(
    jacobi::server::stencil_iterator::get_action
  , jacobi_server_stencil_iterator_get_action
)


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<float, 4, Eigen::Dynamic> test_float_4_D(const Eigen::Matrix<float, 4, Eigen::Dynamic> & M)
{
	return M;
}
void export_float_4_D()
{
	boost::python::def("test_float_4_D",test_float_4_D);
}


/**
 * @file space.hpp
 * @author Marcus Edel
 *
 * Defenition of miscellaneous space routines.
 */

#ifndef GYM_SPACE_HPP
#define GYM_SPACE_HPP

#include <iostream>
#include <armadillo>

#include "client.hpp"

namespace gym {

class Parser;

/**
 * Definition of the space class.
 */
class Space
{
 public:
  /**
   * Instantiate the Environment object.
   */
  Space();

  /**
   * Destroy the space object.
   */
  ~Space();

  /**
   * Set the client which is connected with the server.
   *
   * @param c The client object which is connected with the server.
   */
  void client(Client& c);

  /*
   * Return a sample action.
   */
  const arma::mat& sample();

  //! Space type defenition.
  enum SpaceType
  {
    DISCRETE,
    BOX,
    MULTIDISCRETE,
  } type;

  //! Observation and action space information.
  std::vector<int> boxShape;
  std::vector<float> boxHigh;
  std::vector<float> boxLow;
  int n;

private:
  //! Locally-stored parser pointer.
  Parser* parser;

  //! Locally-stored client pointer.
  Client* clientPtr;

  //! Locally-stored action sample.
  arma::mat actionSample;
};

} // namespace gym

// Include implementation.
#include "space_impl.hpp"

#endif


#include <iostream>
#include <algorithm>
#include <vector>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <pyublas/numpy.hpp>
#include <boost/foreach.hpp>
#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>

using namespace boost::python;
using namespace pyublas;
namespace ublas = boost::numeric::ublas;

int size_or_neg_1(invalid_ok<numpy_vector<double> > x)
{
  if (x->is_valid())
    return x->size();
  else
    return -1;
}

template <class T>
T doublify(T x)
{
  return 2*x;
}

template <class T>
void doublify_inplace(T x)
{
  x *= 2;
}

template <class T>
numpy_vector<T> doublify_strided(numpy_strided_vector<T> x)
{
  return 2*x;
}

template <class T>
numpy_strided_vector<T> doublify_strided_returned(numpy_strided_vector<T> x)
{
  return 2*x;
}

/* The following two versions preserve shape on output: */
template <class T>
numpy_vector<T> doublify_keep_shape_1(numpy_vector<T> x)
{
  numpy_vector<T> result(x.ndim(), x.dims());
  result.assign(2*x.as_strided());
  return result;
}

template <class T>
numpy_vector<T> doublify_keep_shape_2(numpy_vector<T> x)
{
  numpy_vector<T> result(2*x.as_strided());
  result.reshape(x.ndim(), x.dims());
  return result;
}

template <class T>
void doublify_numpy_vector_inplace(numpy_vector<T> x)
{
  x *= 2;
}

template <class T>
void doublify_numpy_strided_vector_inplace(numpy_strided_vector<T> x)
{
  x *= 2;
}

numpy_vector<double> make_resized_vector(unsigned n)
{
  numpy_vector<double> result;
  result.resize(n);
  return result;
}

#include <boost/range.hpp>
//#include <iostream>
template<typename vec_t>
void D(vec_t &v)
{
  typename boost::range_iterator<vec_t>::type i = boost::begin(v);
  typename boost::range_iterator<vec_t>::type e = boost::end(v);
  for (; i != e; ++i)
    *i *= 2;
}

template<typename vec_t>
void test_speed()
{
  vec_t x (ublas::scalar_vector<double>(100000, 1));
  //  std::cout << &(*boost::begin (x)) << ' ' << &(*boost::end (x)) << '\n';
  for (unsigned i = 0; i < 1000; ++i)
    D(x);
}

void test_ublas_speed()
{
  ublas::vector<double> x(ublas::scalar_vector<double>(100000, 1));

  for (unsigned i = 0; i < 1000; ++i)
    x *= 2;
}

void test_unstrided_speed()
{
  numpy_vector<double> x(ublas::scalar_vector<double>(100000, 1));

  for (unsigned i = 0; i < 1000; ++i)
    x *= 2;
}

void test_strided_speed()
{
  numpy_strided_vector<double> x(ublas::scalar_vector<double>(100000, 1));

  for (unsigned i = 0; i < 1000; ++i)
    x *= 2;
}

BOOST_PYTHON_MODULE(testhelp_ext)
{
  def("size_or_neg_1", size_or_neg_1);

  def("dbl_int", doublify<int>);
  def("dbl_float", doublify<double>);

  def("dbl_numpy_mat", doublify<numpy_matrix<double> >);
  def("dbl_numpy_mat_cm",
      doublify<numpy_matrix<double, ublas::column_major> >);

  def("dbl_numpy_mat_inplace", doublify_inplace<numpy_matrix<double> >);
  def("dbl_numpy_mat_cm_inplace",
      doublify_inplace<numpy_matrix<double, ublas::column_major> >);

  def("dbl_numpy_vec",
      doublify<numpy_vector<double> >);
  def("dbl_numpy_vec_keep_shape_1",
      doublify_keep_shape_1<double>);
  def("dbl_numpy_vec_keep_shape_2",
      doublify_keep_shape_2<double>);
  def("dbl_numpy_strided_vec",
      doublify_strided<double>);
  def("dbl_numpy_strided_vec_ret",
      doublify_strided_returned<double>);
  def("dbl_numpy_vec_inplace",
      doublify_numpy_vector_inplace<double>);
  def("dbl_numpy_strided_vec_inplace",
      doublify_numpy_strided_vector_inplace<double>);
  def("dbl_numpy_strided_vec_inplace",
      doublify_numpy_strided_vector_inplace<double>);
  def("dbl_ublas_vec",
      doublify<ublas::vector<double> >);
  def("dbl_ublas_mat",
      doublify<ublas::matrix<double> >);

  def("make_resized_vector", make_resized_vector);

  def("test_ublas_speed", test_ublas_speed);
  def("test_unstrided_speed", test_unstrided_speed);
  def("test_strided_speed", test_strided_speed);

  def("test_ublas_speed2", test_speed<ublas::vector<double> >);
  def("test_unstrided_speed2", test_speed<numpy_vector<double> >);
  def("test_strided_speed2", test_speed<numpy_strided_vector<double> >);
}


//==================================================================================================
/**
  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)
**/
//==================================================================================================
#include <boost/simd/function/aligned_load.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/align/aligned_alloc.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

namespace bs = boost::simd;
namespace ba = boost::alignment;

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  using p_t  = bs::pack<T,N>;
  using pm_t = bs::pack<bs::logical<T>,N>;

  T* data = static_cast<T*>(ba::aligned_alloc(p_t::alignment, (sizeof(T)) * N));

  std::array<T,N> ref, refv;
  std::array<bs::logical<T>,N> msk;

  for(std::size_t i = 0;i < N; ++i) data[i] = T(i);
  for(std::size_t i = 0;i < msk.size() ; ++i) msk[i]  = i%3 ? true : false;
  for(std::size_t i = 0;i < ref.size() ; ++i) ref[i]  = msk[i] ? data[i] : T(0);
  for(std::size_t i = 0;i < refv.size() ; ++i) refv[i]  = msk[i] ? data[i] : T(7);

  auto pm = bs::aligned_load<pm_t>(&msk[0]);

  STF_ALL_EQUAL( bs::aligned_load<p_t>( bs::mask(&data[0], pm    ) ), ref   );
  STF_ALL_EQUAL( bs::aligned_load<p_t>( bs::mask(&data[0], pm, 7 ) ), refv  );

  STF_ALL_EQUAL( bs::aligned_load<p_t>( bs::mask(&data[0], true    ) ), bs::aligned_load<p_t>(&data[0]) );
  STF_ALL_EQUAL( bs::aligned_load<p_t>( bs::mask(&data[0], true, 7 ) ), bs::aligned_load<p_t>(&data[0]) );

  STF_ALL_EQUAL( bs::aligned_load<p_t>( bs::mask(&data[0], false   ) ), bs::Zero<p_t>() );
  STF_ALL_EQUAL( bs::aligned_load<p_t>( bs::mask(&data[0], false, 7) ), p_t(7)          );

  // This should not segfault as masking prevent memory access
  STF_ALL_EQUAL( bs::aligned_load<p_t>( bs::mask((T*)(0), false   ) ), bs::Zero<p_t>() );
  STF_ALL_EQUAL( bs::aligned_load<p_t>( bs::mask((T*)(0), false, 7) ), p_t(7)          );
}

STF_CASE_TPL( "Check masked aligned_load behavior", STF_NUMERIC_TYPES )
{
  namespace bs = boost::simd;
  static const std::size_t N = bs::pack<T>::static_size;

  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}


#include <vector>
#include <cassert>

#include <boost/range/algorithm.hpp>
#include <boost/range/numeric.hpp>

#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>

extern "C" {
#include <metis.h>
}

namespace py = pybind11;

//---------------------------------------------------------------------------
void pointwise_graph(
        int n, int block_size,
        const std::vector<int> &ptr,
        const std::vector<int> &col,
        std::vector<int> &pptr,
        std::vector<int> &pcol
        )
{
    int np = n / block_size;

    assert(np * block_size == n);

    // Create pointwise matrix
    std::vector<int> ptr1(np + 1, 0);
    std::vector<int> marker(np, -1);
    for(int ip = 0, i = 0; ip < np; ++ip) {
        for(int k = 0; k < block_size; ++k, ++i) {
            for(int j = ptr[i]; j < ptr[i+1]; ++j) {
                int cp = col[j] / block_size;
                if (marker[cp] != ip) {
                    marker[cp] = ip;
                    ++ptr1[ip+1];
                }
            }
        }
    }

    boost::partial_sum(ptr1, ptr1.begin());
    boost::fill(marker, -1);

    std::vector<int> col1(ptr1.back());

    for(int ip = 0, i = 0; ip < np; ++ip) {
        int row_beg = ptr1[ip];
        int row_end = row_beg;

        for(int k = 0; k < block_size; ++k, ++i) {
            for(int j = ptr[i]; j < ptr[i+1]; ++j) {
                int cp = col[j] / block_size;

                if (marker[cp] < row_beg) {
                    marker[cp] = row_end;
                    col1[row_end++] = cp;
                }
            }
        }
    }

    // Transpose pointwise matrix
    int nnz = ptr1.back();

    std::vector<int> ptr2(np + 1, 0);
    std::vector<int> col2(nnz);

    for(int i = 0; i < nnz; ++i)
        ++( ptr2[ col1[i] + 1 ] );

    boost::partial_sum(ptr2, ptr2.begin());

    for(int i = 0; i < np; ++i)
        for(int j = ptr1[i]; j < ptr1[i+1]; ++j)
            col2[ptr2[col1[j]]++] = i;

    std::rotate(ptr2.begin(), ptr2.end() - 1, ptr2.end());
    ptr2.front() = 0;

    // Merge both matrices.
    boost::fill(marker, -1);
    pptr.resize(np + 1, 0);

    for(int i = 0; i < np; ++i) {
        for(int j = ptr1[i]; j < ptr1[i+1]; ++j) {
            int c = col1[j];
            if (marker[c] != i) {
                marker[c] = i;
                ++pptr[i + 1];
            }
        }

        for(int j = ptr2[i]; j < ptr2[i+1]; ++j) {
            int c = col2[j];
            if (marker[c] != i) {
                marker[c] = i;
                ++pptr[i + 1];
            }
        }
    }

    boost::partial_sum(pptr, pptr.begin());
    boost::fill(marker, -1);

    pcol.resize(pptr.back());

    for(int i = 0; i < np; ++i) {
        int row_beg = pptr[i];
        int row_end = row_beg;

        for(int j = ptr1[i]; j < ptr1[i+1]; ++j) {
            int c = col1[j];

            if (marker[c] < row_beg) {
                marker[c] = row_end;
                pcol[row_end++] = c;
            }
        }

        for(int j = ptr2[i]; j < ptr2[i+1]; ++j) {
            int c = col2[j];

            if (marker[c] < row_beg) {
                marker[c] = row_end;
                pcol[row_end++] = c;
            }
        }
    }
}

//---------------------------------------------------------------------------
std::vector<int> pointwise_partition(
        int npart,
        const std::vector<int> &ptr,
        const std::vector<int> &col
        )
{
    int nrows = ptr.size() - 1;

    std::vector<int> part(nrows);

    if (npart == 1) {
        boost::fill(part, 0);
    } else {
        int wgtflag = 0;
        int numflag = 0;
        int options = 0;
        int edgecut;

#if defined(METIS_VER_MAJOR) && (METIS_VER_MAJOR >= 5)
        int nconstraints = 1;
        METIS_PartGraphKway(
                &nrows, //nvtxs
                &nconstraints, //ncon -- new
                const_cast<int*>(ptr.data()), //xadj
                const_cast<int*>(col.data()), //adjncy
                NULL, //vwgt
                NULL, //vsize -- new
                NULL, //adjwgt
                &npart,
                NULL,//real t *tpwgts,
                NULL,// real t ubvec
                NULL,
                &edgecut,
                part.data()
                );
#else
        METIS_PartGraphKway(
                &nrows,
                const_cast<int*>(ptr.data()),
                const_cast<int*>(col.data()),
                NULL,
                NULL,
                &wgtflag,
                &numflag,
                &npart,
                &options,
                &edgecut,
                part.data()
                );
#endif
    }

    return part;
}

//---------------------------------------------------------------------------
py::array_t<int> partition(
        int nparts, int block_size,
        py::array_t<int> &_ptr,
        py::array_t<int> &_col
        )
{
    py::buffer_info pinfo = _ptr.request();
    py::buffer_info cinfo = _col.request();

    std::vector<int> ptr(
            static_cast<const int*>(pinfo.ptr),
            static_cast<const int*>(pinfo.ptr) + pinfo.shape[0]
            );

    std::vector<int> col(
            static_cast<const int*>(cinfo.ptr),
            static_cast<const int*>(cinfo.ptr) + cinfo.shape[0]
            );

    int n = ptr.size() - 1;

    // Pointwise graph
    std::vector<int> pptr, pcol;
    pointwise_graph(n, block_size, ptr, col, pptr, pcol);

    // Pointwise partition
    std::vector<int> ppart = pointwise_partition(nparts, pptr, pcol);

    std::vector<int> part(n);
    for(int i = 0; i < n; ++i)
        part[i] = ppart[i / block_size];

    return py::array(part.size(), part.data());
}

//---------------------------------------------------------------------------
PYBIND11_PLUGIN(pymetis)
{
    py::module m("pymetis", "Python interface to metis");
    m.def("partition", partition);
    return m.ptr();
}


/*
 * Polyfit_test.cpp
 *
 *  Created on: 23 авг. 2017 г.
 *      Author: sergejkrivonos
 */
#define BOOST_TEST_MODULE FANN test
#include <boost/test/unit_test.hpp>
#include <fann.h>

BOOST_AUTO_TEST_CASE(FANN_test_simple)
{
    const unsigned int num_input = 2;
    const unsigned int num_output = 1;
    const unsigned int num_layers = 3;
    const unsigned int num_neurons_hidden = 3;
    const float desired_error = (const float) 0.001;
    const unsigned int max_epochs = 500000;
    const unsigned int epochs_between_reports = 1000;

    struct fann *ann = fann_create_standard(num_layers, num_input, num_neurons_hidden, num_output);

    fann_set_activation_function_hidden(ann, FANN_SIGMOID_SYMMETRIC);
    fann_set_activation_function_output(ann, FANN_SIGMOID_SYMMETRIC);

    fann_train_on_file(ann, TEST_SRC_DIR "/xor.data", max_epochs, epochs_between_reports, desired_error);

    fann_save(ann, "xor_float.net");

    fann_destroy(ann);
}

BOOST_AUTO_TEST_CASE(FANN_test)
{
    const unsigned int num_input = 2;
    const unsigned int num_output = 1;
    const unsigned int num_layers = 3;
    const unsigned int num_neurons_hidden = 3;
    const float error = (const float) 0.001;
    const unsigned int max_epochs = 500000;
    const unsigned int epochs_between_reports = 1000;

    struct fann *ann = fann_create_standard(num_layers, num_input, num_neurons_hidden, num_output);

    fann_set_activation_function_hidden(ann, FANN_LINEAR);
    fann_set_activation_function_output(ann, FANN_LINEAR);

    fann_train_on_file(ann, TEST_SRC_DIR "/xor.data", max_epochs, epochs_between_reports, error);

    fann_save(ann, "xor_linear.net");

    fann_destroy(ann);
}


#pragma once
#include <limits>
#include <Eigen/Dense>

class Ray
{
private:
    Eigen::Vector3f origin;

    Eigen::Vector3f direction;

    float tMax;

public:
    Ray(Eigen::Vector3f origin, Eigen::Vector3f direction) : Ray(origin, direction, std::numeric_limits<float>::infinity()){};

    Ray(Eigen::Vector3f origin, Eigen::Vector3f direction, float max) : origin(origin), direction(direction), tMax(max){};

    Eigen::Vector3f operator()(float t) const { return origin + direction * t; };

    const Eigen::Vector3f &get_origin() const { return origin; };

    const Eigen::Vector3f &get_direction() const { return direction; };

    bool is_valid() const { return tMax == std::numeric_limits<float>::infinity() || tMax > 0; };

    bool is_valid(float t) const { return tMax == std::numeric_limits<float>::infinity() || tMax - t > std::numeric_limits<float>::epsilon(); };

    void set_t_max(float t_max) { this->tMax = t_max; };
};


//  io_ex2.cpp  ----------------------------------------------------------//

//  Copyright 2010 Howard Hinnant
//  Copyright 2010 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

/*
This code was adapted by Vicente J. Botet Escriba from Hinnant's html documentation.
Many thanks to Howard for making his code available under the Boost license.

*/

#include <boost/chrono/chrono_io.hpp>
#include <sstream>
#include <boost/assert.hpp>

int main()
{
    using namespace boost::chrono;

    std::istringstream in("5000 milliseconds 4000 ms 3001 ms");
    seconds d(0);
    in >> d;
    BOOST_ASSERT(in.good());
    BOOST_ASSERT(d == seconds(5));
    in >> d;
    BOOST_ASSERT(in.good());
    BOOST_ASSERT(d == seconds(4));
    in >> d;
    BOOST_ASSERT(in.fail());
    BOOST_ASSERT(d == seconds(4));
    return 0;
}


// Copyright 2018 Google LLC
//
// 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.

#include "cpp/sensor_fusion/online_sensor_fusion.h"

#include <memory>
#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace vr180 {

namespace {

static const double kEpsilon = 1e-7;
static const double kGravity = 9.81;  // m/s^2
static const Eigen::Vector3d kDown(0, 1, 0);

Eigen::Vector3d GetEulerAngles(const Eigen::Matrix3d& coeff) {
  Eigen::Vector3d result;
  result[0] = std::atan2(coeff(0, 2), coeff(2, 2));
  const double c2 = Eigen::Vector2d(coeff(1, 1), coeff(1, 0)).norm();
  result[1] = std::atan2(-coeff(1, 2), c2);
  const double s1 = std::sin(result[0]);
  const double c1 = std::cos(result[0]);
  result[2] = std::atan2(s1 * coeff(2, 1) - c1 * coeff(0, 1),
                         c1 * coeff(0, 0) - s1 * coeff(2, 0));
  return result;
}

Eigen::Matrix3d GetRotationMatrix(const Eigen::Vector3f& orientation) {
  Eigen::Vector3d aa = orientation.cast<double>();
  return Eigen::Quaterniond(Eigen::AngleAxisd(aa.norm(), aa.normalized()))
      .toRotationMatrix();
}

OnlineSensorFusion Init(const Eigen::Vector3d& gravity) {
  OnlineSensorFusion filter = OnlineSensorFusion(OnlineSensorFusion::Options());
  for (int i = 0; i < 2; i++) {
    filter.AddAccelMeasurement(gravity, i);
    filter.AddGyroMeasurement(Eigen::Vector3d::Zero(), i);
  }
  return filter;
}
}  // namespace

TEST(OnlineSensorFusionTest, GetStaticOrientation) {
  OnlineSensorFusion filter = Init(Eigen::Vector3d(0, 0, -kGravity));
  const Eigen::Matrix3d orientation =
      GetRotationMatrix(filter.GetOrientation());
  // Orientation tranform should cause z-axis to point down.
  const Eigen::Vector3d down = orientation * Eigen::Vector3d::UnitZ();
  EXPECT_TRUE(down.isApprox(kDown, kEpsilon));
}

TEST(OnlineSensorFusionTest, GetStaticPortraitOrientation) {
  const OnlineSensorFusion filter = Init(Eigen::Vector3d(0, -kGravity, 0));
  const Eigen::Matrix3d orientation =
      GetRotationMatrix(filter.GetOrientation());
  // Orientation tranform should cause y-axis to point down.
  const Eigen::Vector3d down = orientation * Eigen::Vector3d::UnitY();
  EXPECT_TRUE(down.isApprox(kDown, kEpsilon));
}

TEST(OnlineSensorFusionTest, GetStaticLandscapeOrientation) {
  const OnlineSensorFusion filter = Init(Eigen::Vector3d(-kGravity, 0, 0));
  const Eigen::Matrix3d orientation =
      GetRotationMatrix(filter.GetOrientation());
  // Orientation tranform should cause x-axis to point down.
  const Eigen::Vector3d down = orientation * Eigen::Vector3d::UnitX();
  EXPECT_TRUE(down.isApprox(kDown, kEpsilon));
}

TEST(OnlineSensorFusionTest, Recenter) {
  OnlineSensorFusion filter = Init(Eigen::Vector3d(0, -kGravity, 0));
  // Rotate ~M_PI/4 about each axis.
  for (int i = 200; i > 0; i--) {
    filter.AddAccelMeasurement(Eigen::Vector3d(0, -kGravity, 0), 1 + 1.0 / i);
    filter.AddGyroMeasurement(Eigen::Vector3d(0, M_PI / 4, 0), 1 + 1.0 / i);
  }
  for (int i = 200; i > 0; i--) {
    filter.AddAccelMeasurement(Eigen::Vector3d(0, -kGravity / 2, -kGravity / 2),
                               2 + 1.0 / i);
    filter.AddGyroMeasurement(Eigen::Vector3d(M_PI / 4, 0, 0), 2 + 1.0 / i);
  }
  for (int i = 200; i > 0; i--) {
    filter.AddAccelMeasurement(
        Eigen::Vector3d(-kGravity / 3, -kGravity / 3, -kGravity / 3),
        3 + 1.0 / i);
    filter.AddGyroMeasurement(Eigen::Vector3d(0, 0, M_PI / 4), 3 + 1.0 / i);
  }
  const Eigen::Vector3d angles =
      GetEulerAngles(GetRotationMatrix(filter.GetOrientation()));
  // Verify there is non-zero rotation about all axes.
  for (int i = 0; i < 3; i++) {
    EXPECT_GT(fabs(angles[i]), kEpsilon);
  }
  filter.Recenter();
  const Eigen::Vector3d recentered_angles =
      GetEulerAngles(GetRotationMatrix(filter.GetOrientation()));
  // Expect the recentered orientation to have no rotation about the y-axis.
  EXPECT_NEAR(recentered_angles[0], 0, kEpsilon);
  // Expect the recentered orientation to have the same rotations about the
  // x- z-axes as the non-recentered orientation.
  EXPECT_NEAR(recentered_angles[1], angles[1], kEpsilon);
  EXPECT_NEAR(recentered_angles[2], angles[2], kEpsilon);
}

}  // namespace vr180


#ifndef COMMON_HEADER_HPP
#define COMMON_HEADER_HPP

#include <type_traits>
#include <cassert>
#include <iterator>

// metafunction
#include <boost/mpl/apply.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/lambda.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/identity.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/not_equal_to.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/deref.hpp>

// container
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/at.hpp>

// iterator
#include <boost/mpl/iterator_range.hpp>

// arithmetic
#include <boost/mpl/int.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/multiplies.hpp>
#include <boost/mpl/divides.hpp>
#include <boost/mpl/greater.hpp>
#include <boost/mpl/less.hpp>

// algorithm
#include <boost/mpl/copy.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/find.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/pop_back.hpp>
#include <boost/mpl/pop_front.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/prior.hpp>
#include <boost/mpl/advance.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/erase.hpp>
#include <boost/mpl/reverse.hpp>

namespace mpl = boost::mpl;
using namespace mpl::placeholders;
using mpl::true_;
using mpl::false_;

#endif


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

#include <costmap_converter/costmap_to_lines_ransac.h>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToLinesDBSRANSAC, costmap_converter::BaseCostmapToPolygons)

namespace costmap_converter
{

CostmapToLinesDBSRANSAC::CostmapToLinesDBSRANSAC() : CostmapToPolygonsDBSMCCH()
{
  dynamic_recfg_ = NULL;
}

CostmapToLinesDBSRANSAC::~CostmapToLinesDBSRANSAC()
{
  if (dynamic_recfg_ != NULL)
    delete dynamic_recfg_;
}

void CostmapToLinesDBSRANSAC::initialize(ros::NodeHandle nh)
{
    // DB SCAN
    nh.param("cluster_max_distance", parameter_.max_distance_, 0.4);
    nh.param("cluster_min_pts", parameter_.min_pts_, 2);
    nh.param("cluster_max_pts", parameter_.max_pts_, 30);
    // convex hull (only necessary if outlier filtering is enabled)
    nh.param("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, 0.1);
    parameter_buffered_ = parameter_;

    // ransac
    nh.param("ransac_inlier_distance", ransac_inlier_distance_, 0.2);
    nh.param("ransac_min_inliers", ransac_min_inliers_, 10);
    nh.param("ransac_no_iterations", ransac_no_iterations_, 2000);
    nh.param("ransac_remainig_outliers", ransac_remainig_outliers_, 3);
    nh.param("ransac_convert_outlier_pts", ransac_convert_outlier_pts_, true);
    nh.param("ransac_filter_remaining_outlier_pts", ransac_filter_remaining_outlier_pts_, false);

    // setup dynamic reconfigure
    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>(nh);
    dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>::CallbackType cb = boost::bind(&CostmapToLinesDBSRANSAC::reconfigureCB, this, _1, _2);
    dynamic_recfg_->setCallback(cb);
}

void CostmapToLinesDBSRANSAC::compute()
{
    std::vector< std::vector<KeyPoint> > clusters;
    dbScan(clusters);

    // Create new polygon container
    PolygonContainerPtr polygons(new std::vector<geometry_msgs::Polygon>());

    // fit lines using ransac for each cluster
    for (int i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
    {

      while (clusters[i].size() > ransac_remainig_outliers_)
      {
//         std::vector<KeyPoint> inliers;
        std::vector<KeyPoint> outliers;
        std::pair<KeyPoint,KeyPoint> model;
        if (!lineRansac(clusters[i], ransac_inlier_distance_, ransac_no_iterations_, ransac_min_inliers_, model, NULL, &outliers ) )
          break;

        // add to polygon container
        geometry_msgs::Polygon line;
        line.points.resize(2);
        model.first.toPointMsg(line.points.front());
        model.second.toPointMsg(line.points.back());
        polygons->push_back(line);

        clusters[i] = outliers;
      }
      // create point polygons for remaining outliers
      if (ransac_convert_outlier_pts_)
      {
        if (ransac_filter_remaining_outlier_pts_)
        {
          // take edge points of a convex polygon
          // these points define a cluster and since all lines are extracted,
          // we remove points from the interior...
          geometry_msgs::Polygon polygon;
          convexHull2(clusters[i], polygon);
          for (int j=0; j < (int)polygon.points.size(); ++j)
          {
            polygons->push_back(geometry_msgs::Polygon());
            convertPointToPolygon(polygon.points[j], polygons->back());
          }
        }
        else
        {
          for (int j = 0; j < (int)clusters[i].size(); ++j)
          {
            polygons->push_back(geometry_msgs::Polygon());
            convertPointToPolygon(clusters[i][j], polygons->back());
          }
        }

      }

    }

    // add our non-cluster points to the polygon container (as single points)
    if (!clusters.empty())
    {
      for (int i=0; i < clusters.front().size(); ++i)
      {
        polygons->push_back( geometry_msgs::Polygon() );
        convertPointToPolygon(clusters.front()[i], polygons->back());
      }
    }

    // replace shared polygon container
    updatePolygonContainer(polygons);
}

bool CostmapToLinesDBSRANSAC::lineRansac(const std::vector<KeyPoint>& data, double inlier_distance, int no_iterations,
                                         int min_inliers, std::pair<KeyPoint, KeyPoint>& best_model,
                                         std::vector<KeyPoint>* inliers, std::vector<KeyPoint>* outliers)
{
  if (data.size() < 2 ||  data.size() < min_inliers)
  {
    return false;
  }

  boost::random::uniform_int_distribution<> distribution(0, data.size()-1);

  std::pair<int, int> best_model_idx;
  int best_no_inliers = -1;

  for (int i=0; i < no_iterations; ++i)
  {
    // choose random points to define a line candidate
    int start_idx = distribution(rnd_generator_);
    int end_idx = start_idx;
    while (end_idx == start_idx)
      end_idx = distribution(rnd_generator_);

    // compute inliers
    int no_inliers = 0;
    for (int j=0; j<(int)data.size(); ++j)
    {
      if ( isInlier(data[j], data[start_idx], data[end_idx], inlier_distance) )
        ++no_inliers;
    }

    if (no_inliers > best_no_inliers)
    {
      best_no_inliers = no_inliers;
      best_model_idx.first = start_idx;
      best_model_idx.second = end_idx;
    }
  }

  best_model.first = data[best_model_idx.first];
  best_model.second = data[best_model_idx.second];

  if (best_no_inliers < min_inliers)
    return false;

  // Now repeat the calculation for the best model in order to obtain the inliers and outliers set
  // This might be faster if no_iterations is large, but TEST
  if (inliers || outliers)
  {
    if (inliers)
      inliers->clear();
    if (outliers)
      outliers->clear();

    int no_inliers = 0;
    for (int i=0; i<(int)data.size(); ++i)
    {
        if ( isInlier( data[i], best_model.first, best_model.second, inlier_distance ) )
        {
          if (inliers)
            inliers->push_back( data[i] );
        }
        else
        {
          if (outliers)
            outliers->push_back( data[i] );
        }
    }
  }

  return true;
}

bool CostmapToLinesDBSRANSAC::linearRegression(const std::vector<KeyPoint>& data, double& slope, double& intercept,
                                               double* mean_x_out, double* mean_y_out)
{
  if (data.size() < 2)
  {
    ROS_ERROR("CostmapToLinesDBSRANSAC: at least 2 data points required for linear regression");
    return false;
  }

  double mean_x = 0;
  double mean_y = 0;
  for (int i=0; i<(int)data.size(); ++i)
  {
    mean_x += data[i].x;
    mean_y += data[i].y;
  }
  mean_x /= double(data.size());
  mean_y /= double(data.size());

  if (mean_x_out)
    *mean_x_out = mean_x;

  if (mean_y_out)
    *mean_y_out = mean_y;

  double numerator = 0.0;
  double denominator = 0.0;

  for(int i=0; i<(int)data.size(); ++i)
  {
      double dx = data[i].x - mean_x;
      numerator += dx * (data[i].y - mean_y);
      denominator += dx*dx;
  }

  if (denominator == 0)
  {
    ROS_ERROR("CostmapToLinesDBSRANSAC: linear regression failed, denominator 0");
    return false;
  }
  else
    slope = numerator / denominator;

  intercept = mean_y - slope * mean_x;
  return true;
}

void CostmapToLinesDBSRANSAC::reconfigureCB(CostmapToLinesDBSRANSACConfig& config, uint32_t level)
{
    boost::mutex::scoped_lock lock(parameter_mutex_);
    parameter_buffered_.max_distance_ = config.cluster_max_distance;
    parameter_buffered_.min_pts_ = config.cluster_min_pts;
    parameter_buffered_.max_pts_ = config.cluster_max_pts;
    parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts;
    ransac_inlier_distance_ = config.ransac_inlier_distance;
    ransac_min_inliers_ = config.ransac_min_inliers;
    ransac_no_iterations_ = config.ransac_no_iterations;
    ransac_remainig_outliers_ = config.ransac_remainig_outliers;
    ransac_convert_outlier_pts_ = config.ransac_convert_outlier_pts;
    ransac_filter_remaining_outlier_pts_ = config.ransac_filter_remaining_outlier_pts;
}

/*
void CostmapToLinesDBSRANSAC::adjustLineLength(const std::vector<KeyPoint>& data, const KeyPoint& linept1, const KeyPoint& linept2,
                                               KeyPoint& line_start, KeyPoint& line_end)
{
  line_start = linept1;
  line_end = linept2;

  // infinite line is defined by linept1 and linept2
  double dir_x = line_end.x - line_start.x;
  double dir_y = line_end.y - line_start.y;
  double norm = std::sqrt(dir_x*dir_x + dir_y*dir_y);
  dir_x /= norm;
  dir_y /= norm;

  // project data onto the line and check if the distance is increased in both directions
  for (int i=0; i < (int) data.size(); ++i)
  {
    double dx = data[i].x - line_start.x;
    double dy = data[i].y - line_start.y;
    // check scalar product at start
    double extension = dx*dir_x + dy*dir_y;
    if (extension<0)
    {
      line_start.x -=  dir_x*extension;
      line_start.y -=  dir_y*extension;
    }
    else
    {
      dx = data[i].x - line_end.x;
      dy = data[i].y - line_end.y;
      // check scalar product at end
      double extension = dx*dir_x + dy*dir_y;
      if (extension>0)
      {
        line_end.x +=  dir_x*extension;
        line_end.y +=  dir_y*extension;
      }
    }
  }

}*/

}//end namespace costmap_converter


#include <complex>
#include <exception>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <boost/circular_buffer.hpp>
#include <range/v3/all.hpp>
#include <cstdint>
#include <initializer_list>
#include <iterator>
#include <numeric>
#include <algorithm>
namespace views = ranges::views;
using gint = std::int64_t;
class Generator : public ranges::view_facade<Generator>
{
  int i_ = 0;
  gint start_;
  int factor_ = 16807;
  friend ranges::range_access;
  int read() const
  {
    return start_ & 65535;
  }
  void next()
  {
    start_ *= factor_;
    start_ %= static_cast<gint>(2147483647ul);
  }
  bool equal(ranges::default_sentinel_t) const { return false; }

public:
  Generator(gint start, int factor) : start_(start), factor_(factor)
  {
    start_ *= factor_;
    start_ &= static_cast<gint>(std::numeric_limits<int>::max());
  }
  Generator() {}
};
int main()
{
  Generator a(873, 16807);
  auto left = views::filter(a, [](auto i) { return (i & 0b11) == 0; }) | views::take(5000000) | views::common;

  Generator b(583, 48271);
  auto right = views::filter(b, [](auto i) { return (i & 0b111) == 0; }) | views::take(5000000) | views::common;
  int n = std::transform_reduce(left.begin(), left.end(), right.begin(), 0, std::plus<>{}, std::equal_to<>{});
  std::cout << n << std::endl;
}


#include "mex.h"
#include <iostream>
#include "drakeMexUtil.h"
#include "RigidBodyManipulator.h"
#include <Eigen/Dense>
#include "math.h"

using namespace Eigen;
using namespace std;

/*
 * Mex interface for computing friction cone surface tangent basis vectors given contact normals.
 *
 * MATLAB signature:
 * d =
 *     surfaceTangents(mex_model_ptr, normals)
 */

inline mxArray* getTangentsArray(RigidBodyManipulator* const model, Map<Matrix3Xd> const & normals)
{
  const size_t numContactPairs = normals.cols();
  const mwSize cellDims[] = {1, BASIS_VECTOR_HALF_COUNT};
  mxArray* tangentCells = mxCreateCellArray(2, cellDims);

  vector< Map<Matrix3Xd> > tangents;
  for (int k = 0 ; k < BASIS_VECTOR_HALF_COUNT ; k++)
  {
    mxArray *cell = mxCreateDoubleMatrix(3, numContactPairs, mxREAL );
    tangents.push_back(Map<Matrix3Xd>(mxGetPrSafe(cell), 3, numContactPairs));
    mxSetCell(tangentCells, k, cell);
  }

  model->surfaceTangents(normals, tangents);
  return tangentCells;
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  if (nrhs != 2) {
    mexErrMsgIdAndTxt("Drake:surfaceTangentsmex:NotEnoughInputs","Usage: [d] = surfaceTangentsmex(mex_model_ptr, normals)");
  }

  RigidBodyManipulator *model= (RigidBodyManipulator*) getDrakeMexPointer(prhs[0]);
  const size_t numNormals = mxGetN(prhs[1]);  //number of normal vectors
  const size_t dimNormals = mxGetM(prhs[1]);  //dimension of each normal vector

  if (dimNormals != 3) {
  	mexErrMsgIdAndTxt("Drake:surfaceTangentsmex:InvalidNormalDimensions","Normals must be 3-Dimensional");
  }

  //Mapping Eigen Matrix to existing memory to avoid copy overhead
  Map<Matrix3Xd> normals(mxGetPrSafe(prhs[1]), 3, numNormals);

  if (nlhs > 0) {
    plhs[0] = getTangentsArray(model, normals);
  }
}


/*
 * pca_gp_model.cpp
 *
 *  Created on: Feb 10, 2010
 *      Author: sturm
 */

#include "articulation_models/models/pca_gp_model.h"
#include "articulation_models/utils.h"

using namespace std;
using namespace articulation_msgs;

#include "Eigen/Core"
#include <Eigen/SVD>

using namespace Eigen;

#include <iomanip>
#define VEC(a) setprecision(5)<<fixed<<a.x()<<" "<<a.y()<<" "<<a.z()<<" "<<a.w()<<" l="<<a.length()
#define VEC2(a) "t=["<<VEC(a.getOrigin())<<"] r=[]"<<VEC(a.getRotation())<<"]"
#define PRINT(a) cout << #a <<"=" << VEC(a)<<endl;
#define PRINT2(a) cout << #a <<"=" << VEC2(a)<<endl;

#include <boost/format.hpp>
using namespace boost;

namespace articulation_models {

PCAGPModel::PCAGPModel() {
	complexity = 0;

	downsample = 20;

	//todo:add parameters here
	CovFuncND initialCovFunc;
	double initialSigmaNoise;
	vector<double> params = vector<double>(2);	// DOF + 1
	params[0] = -0.5;	// length scale, dof 1
	params[1] = 0.0;	// sigma
	initialCovFunc.setHyperparameter(params);
	initialSigmaNoise = -5;

	for(size_t i=0;i<7;i++) {
		gp.push_back( new gaussian_process::SingleGP(initialCovFunc,initialSigmaNoise) );
	}

	initialized = false;
	outlier_ratio = 0.0;

}

PCAGPModel::~PCAGPModel() {
	for(size_t i=0;i<gp.size();i++) {
		delete gp[i];
	}
	gp.clear();
}

void PCAGPModel::readParamsFromModel() {
	GenericModel::readParamsFromModel();
	getParam("downsample",downsample);
	getParam("rigid_position",rigid_position);
	getParam("prismatic_dir",prismatic_dir);
	double training_samples_float=0.00;
	getParam("training_samples",training_samples_float);
	training_samples = (int)training_samples_float;
	complexity = 3 + 6*training_samples;

	checkInitialized();//gp needs to build covariance matrix
}

void PCAGPModel::writeParamsToModel() {
	GenericModel::writeParamsToModel();
	setParam("downsample",downsample,ParamMsg::PARAM);
	setParam("rigid_position",rigid_position,ParamMsg::PARAM);
	setParam("prismatic_dir",prismatic_dir,ParamMsg::PARAM);
	setParam("training_samples",training_samples,ParamMsg::PARAM);
}

tf::Transform PCAGPModel::pose(size_t index) {
	tf::Transform pose;
	getParam(str(format("pose[%d]")%index),pose);
	return pose;
}

void PCAGPModel::storeData(bool inliersOnly) {
	// copy data into params
	bool resample = false;
	size_t n = model.track.pose.size();

	vector<double> inliers_cum(n,0);
	double sum = 0;
	for(size_t i=0;i<n;i++) {
		if(inliersOnly)
			sum += 1 - model.track.channels[channelOutlier].values[i];
		else
			sum += 1;
		inliers_cum[i] = sum;
	}
//	cout <<"inlier sum is "<<sum<<endl;

	training_samples = (int)sum;
	if(downsample>0 && n>downsample) {
		training_samples  = downsample;
		resample = true;
	}
	complexity = 1 + getDOFs() + 6*training_samples;

	for(size_t i=0;i<training_samples;i++) {
		double choice = i / (double)training_samples;
		size_t j=0;
		for(j=0;j<n;j++) {
			if(inliers_cum[j]/sum>choice) break;
		}
//		cout <<"choosing j="<<j<<endl;
		setParam(str(format("pose[%d]")%i),poseToTransform(model.track.pose[j]),ParamMsg::PARAM);
	}

}

bool PCAGPModel::fitModel() {
	if(getSamples()<3) return true;

	// outlier experimental
	storeData(false);	// use all points
	outlier_ratio = 0.0;
	buildGPs();
	getLogLikelihood(true);
////	double old_outlier_ratio;
////	int iter=0;
////	do {
////		storeData(true);	// use only inliers
////		buildGPs();
////		old_outlier_ratio = outlier_ratio;
////		getLogLikelihood(true);
////		iter ++;
////		cout <<"GP EM iteration="<<iter<<" outlier_ratio="<<outlier_ratio<<endl;
////	} while(iter < 10 && fabs(outlier_ratio - old_outlier_ratio) > 0.01);
//
//	cout << "outlier ratio="<<this->outlier_ratio<<endl;

	return true;
}

void PCAGPModel::buildGPs() {
	if(training_samples<3) return;
	// fit PCA
	// compute center
	tf::Vector3 sum_p(0,0,0);
	for (size_t j = 0; j < training_samples; j++) {
		sum_p = sum_p + pose(j).getOrigin();
	}
	rigid_position = sum_p / training_samples;

	// find line direction
	MatrixXf X_trans(training_samples, 3);
	for (size_t j = 0; j < training_samples; j++) {
		tf::Vector3 diff = pose(j).getOrigin() - rigid_position;
		X_trans(j, 0) = diff.x();
		X_trans(j, 1) = diff.y();
		X_trans(j, 2) = diff.z();
	}

	VectorXf b(training_samples);
	VectorXf x(3);
	b = VectorXf::Zero(training_samples);
	JacobiSVD<MatrixXf> svdOfA(X_trans);

	const Eigen::MatrixXf U = svdOfA.matrixU();
	const Eigen::MatrixXf V = svdOfA.matrixV();
	const Eigen::VectorXf S = svdOfA.singularValues();

//	Eigen::MatrixXf Y_trans = U * S.asDiagonal();
//	cout << endl;
//	cout <<"X=["<<endl;
//	for(int i=0;i<X_trans.rows();i++) {
//		for(int j=0;j<X_trans.cols();j++) {
//			if(j!=0)
//				cout <<", ";
//			cout <<X_trans(i,j);
//		}
//		cout <<";"<<endl;
//	}
//	cout << "]"<<endl;
//	cout << X_trans<<endl;
//	cout << endl;
//	cout << Y_trans<<endl;
//	cout << endl;
//	cout << V;
//	cout << endl;
	double v0=V(0,0),v1=V(1,0),v2=V(2,0);
	prismatic_dir= tf::Vector3(v0,v1,v2);
//	PRINT(prismatic_dir);

	if(!check_values(rigid_position)) {
		cerr <<"rigid_position has NaNs"<<endl;
		return ;
	}
	if(!check_values(prismatic_dir))  {
		cerr <<"prismatic_dir has NaNs"<<endl;
		return ;
	}
//	cout << X_trans << endl;
//	PRINT(rigid_position);
//	PRINT(prismatic_dir);
//	writeParamsToModel();
//	prismatic_dir = tf::Vector3(1,-1,0);
//	prismatic_dir = tf::Vector3(0.48308,-0.87558,0.00000);
//	prismatic_dir = tf::Vector3(0.88215,0.46638,-0.06560);
//	prismatic_dir = tf::Vector3(prismatic_dir.x(),prismatic_dir.y(),prismatic_dir.z());
//	prismatic_dir.normalize();
//	cout <<"fitting:";
//	PRINT(prismatic_dir);

	TVector<TDoubleVector> input(training_samples);
	TVector<double> output[7];
	for(size_t j=0;j<7;j++) {
		output[j] = TDoubleVector(training_samples);
	}

	for(size_t i=0;i<training_samples;i++) {
		input[i] = TDoubleVector(1);

		geometry_msgs::Pose p = transformToPose(pose(i));
		input[i][0] = predictConfiguration( p )[0];
		output[0][i] = p.position.x;
		output[1][i] = p.position.y;
		output[2][i] = p.position.z;
		output[3][i] = p.orientation.w;
		output[4][i] = p.orientation.x;
		output[5][i] = p.orientation.y;
		output[6][i] = p.orientation.z;
	}

	for(size_t j=0;j<7;j++) {
//		cout << "setting data, j="<<j<<" n="<<input.size()<<endl;
		gp[j]->SetData(input,output[j]);
//		gp[j]->OptimizeGP();
	}

 //	cout << "fitting model, end.."<<endl;
}

V_Configuration PCAGPModel::predictConfiguration(geometry_msgs::Pose pose) {
	tf::Vector3 diff = (positionToVector(pose.position) - rigid_position);
//	PRINT(diff);
//	PRINT(prismatic_dir);

	V_Configuration q( 1 );
	q(0) = diff.dot(prismatic_dir);

	return q;
}

geometry_msgs::Pose PCAGPModel::predictPose(V_Configuration q) {
	geometry_msgs::Pose result;
	double mean_x=0,var_x=0;
	double mean_y=0,var_y=0;
	double mean_z=0,var_z=0;
	double mean_rw=0,var_rw=0;
	double mean_rx=0,var_rx=0;
	double mean_ry=0,var_ry=0;
	double mean_rz=0,var_rz=0;
	TDoubleVector inp(1);
	inp(0) = q[0];
	gp[0]->Evaluate( inp, mean_x, var_x );
	gp[1]->Evaluate( inp, mean_y, var_y );
	gp[2]->Evaluate( inp, mean_z, var_z );
	gp[3]->Evaluate( inp, mean_rw, var_rw );
	gp[4]->Evaluate( inp, mean_rx, var_rx );
	gp[5]->Evaluate( inp, mean_ry, var_ry );
	gp[6]->Evaluate( inp, mean_rz, var_rz );

	tf::Quaternion pred_rot(mean_rx,mean_ry,mean_rz,mean_rw);
	tf::Vector3 pred_pos(mean_x,mean_y,mean_z);
	pred_rot.normalize();
	tf::Transform pred_pose(pred_rot,pred_pos);
	result = transformToPose(pred_pose);

//	tf::Transform t( rigid_orientation, rigid_position + prismatic_dir*q[0]);
//	result = transformToPose(t);

	return result;
}

void PCAGPModel::checkInitialized() {
	if(!initialized) {
		buildGPs();
		initialized = true;
	}
}

void PCAGPModel::projectPoseToConfiguration() {
	checkInitialized();
	GenericModel::projectPoseToConfiguration();
}

void PCAGPModel::projectConfigurationToPose() {
	checkInitialized();
	GenericModel::projectConfigurationToPose();
}

void PCAGPModel::projectConfigurationToJacobian() {
	checkInitialized();
	GenericModel::projectConfigurationToJacobian();
}

bool PCAGPModel::evaluateModel() {
	checkInitialized();
	return GenericModel::evaluateModel();
}

bool PCAGPModel::normalizeParameters() {
	if(model.track.pose.size()>2) {
		rigid_position = rigid_position + predictConfiguration(model.track.pose.front())[0] * prismatic_dir;
		if(predictConfiguration(model.track.pose.back())[0]<0)
			prismatic_dir *= -1;
	}
	return true;
}
}


// Copyright 2013-2020 Lawrence Livermore National Security, LLC and other
// HavoqGT Project Developers. See the top-level LICENSE file for details.
//
// SPDX-License-Identifier: MIT

#ifndef HAVOQGT_MPI_PAGE_RANK_HPP_INCLUDED
#define HAVOQGT_MPI_PAGE_RANK_HPP_INCLUDED

#include <havoqgt/visitor_queue.hpp>
#include <boost/container/deque.hpp>
#include <vector>

namespace havoqgt {

template <typename Visitor>
class pr_queue
{

protected:
  std::vector< Visitor > m_data;
public:
  pr_queue() { }

  bool push(Visitor const & task)
  {
    m_data.push_back(task);
    return true;
  }

  void pop()
  {
    m_data.pop_back();
  }

  Visitor const & top() //const
  {
    return m_data.back();
  }

  size_t size() const
  {
    return m_data.size();;
  }

  bool empty() const
  {
    return m_data.empty();
  }

  void clear()
  {
    m_data.clear();
  }
};

template<typename Graph>
class pr_visitor {
public:
  typedef typename Graph::vertex_locator                 vertex_locator;
  pr_visitor(): rank(std::numeric_limits<double>::min())  { }

  pr_visitor(vertex_locator _vertex, double _rank)
    : vertex(_vertex)
    , rank(_rank) { }

  pr_visitor(vertex_locator _vertex)
    : vertex(_vertex)
    , rank(std::numeric_limits<double>::min()) { }

  template<typename AlgData>
  bool pre_visit(AlgData& alg_data) const {
    if(rank == std::numeric_limits<double>::min()) {
      HAVOQGT_ERROR_MSG("This is a damn logic error!");
      return true;
    }
    std::get<1>(alg_data)[vertex] += rank; //change to next_rank
    return false;
  }

  template<typename VisitorQueueHandle, typename AlgData>
  bool init_visit(Graph& g, VisitorQueueHandle vis_queue, AlgData& alg_data) const {
    return visit(g, vis_queue, alg_data);
  }

  template<typename VisitorQueueHandle, typename AlgData>
  bool visit(Graph& g, VisitorQueueHandle vis_queue, AlgData& alg_data) const {
    //change to cur_rank
    double old_rank = std::get<0>(alg_data)[vertex];
    uint64_t degree = g.degree(vertex);
    double send_rank = old_rank / double(degree);

    typedef typename Graph::edge_iterator eitr_type;
    for(eitr_type eitr = g.edges_begin(vertex); eitr != g.edges_end(vertex); ++eitr) {
      vertex_locator neighbor = eitr.target();
      pr_visitor new_visitor( neighbor, send_rank);
      vis_queue->queue_visitor(new_visitor);
    }
    return true;
  }

  friend inline bool operator>(const pr_visitor& v1, const pr_visitor& v2) {
    return false;
  }

  friend inline bool operator<(const pr_visitor& v1, const pr_visitor& v2) {
    return false;
  }

  vertex_locator   vertex;
  double           rank;
};

template <typename TGraph, typename PRData>
void page_rank(TGraph& g, PRData& cur_rank, PRData& next_rank, bool initial) {
  typedef  pr_visitor<TGraph>    visitor_type;
  auto alg_data = std::forward_as_tuple(cur_rank, next_rank);

  if(initial) {
    cur_rank.reset(double(1)/double(g.max_global_vertex_id()));
  }

  auto vq = create_visitor_queue<visitor_type, detail::visitor_priority_queue>(&g, alg_data);
  vq.init_visitor_traversal();
  next_rank.all_reduce();
}

} //end namespace havoqgt

#endif //HAVOQGT_MPI_PAGE_RANK_HPP_INCLUDED