/**
 This file is part of Poisson Image Editing.

 Copyright Christoph Heindl 2015

 Poisson Image Editing is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 Poisson Image Editing is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with Poisson Image Editing.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <blend/clone.h>
#include <blend/poisson_solver.h>
#include <opencv2/opencv.hpp>
#pragma warning (push)
#pragma warning (disable: 4244)
#include <Eigen/Sparse>
#include <Eigen/Dense>
#pragma warning (pop)

namespace blend {

    namespace detail {
        bool findOverlap(cv::InputArray background,
                         cv::InputArray foreground,
                         int offsetX, int offsetY,
                         cv::Rect &rBackground,
                         cv::Rect &rForeground)
        {
            cv::Mat bg = background.getMat();
            cv::Mat fg = foreground.getMat();

            rBackground = cv::Rect(0, 0, bg.cols, bg.rows) &
                          cv::Rect(offsetX, offsetY, fg.cols, fg.rows);

            // Compensate for negative offsets. If offset < 0, offset in foreground is positive.
            rForeground = cv::Rect(std::max<int>(-offsetX, 0),
                                   std::max<int>(-offsetY, 0),
                                   rBackground.width,
                                   rBackground.height);

            return rForeground.area() > 0;

        }

        void computeMixedGradientVectorField(cv::InputArray background,
                                             cv::InputArray foreground,
                                             cv::OutputArray vx_,
                                             cv::OutputArray vy_)
        {
            cv::Mat bg = background.getMat();
            cv::Mat fg = foreground.getMat();

            const int channels = bg.channels();

            vx_.create(bg.size(), CV_MAKETYPE(CV_32F, channels));
            vy_.create(bg.size(), CV_MAKETYPE(CV_32F, channels));

            cv::Mat vx = vx_.getMat();
            cv::Mat vy = vy_.getMat();

            cv::Mat kernelx = (cv::Mat_<float>(1, 3) << -0.5, 0, 0.5);
            cv::Mat kernely = (cv::Mat_<float>(3, 1) << -0.5, 0, 0.5);

            cv::Mat vxf, vyf, vxb, vyb;
            cv::filter2D(fg, vxf, CV_32F, kernelx, cv::Point(-1,-1), 0, cv::BORDER_REPLICATE);
            cv::filter2D(fg, vyf, CV_32F, kernely, cv::Point(-1,-1), 0, cv::BORDER_REPLICATE);
            cv::filter2D(bg, vxb, CV_32F, kernelx, cv::Point(-1,-1), 0, cv::BORDER_REPLICATE);
            cv::filter2D(bg, vyb, CV_32F, kernely, cv::Point(-1,-1), 0, cv::BORDER_REPLICATE);

            for(int id = 0; id <= (vx.rows * vx.cols * channels - channels); ++id)
            {
                const cv::Vec2f g[2] = {
                    cv::Vec2f(vxf.ptr<float>()[id], vyf.ptr<float>()[id]),
                    cv::Vec2f(vxb.ptr<float>()[id], vyb.ptr<float>()[id])
                };

                int which = (g[0].dot(g[0]) > g[1].dot(g[1])) ? 0 : 1;

                vx.ptr<float>()[id] = g[which][0];
                vy.ptr<float>()[id] = g[which][1];
            }
        }

        void computeWeightedGradientVectorField(cv::InputArray background,
                                                cv::InputArray foreground,
                                                cv::OutputArray vx,
                                                cv::OutputArray vy,
                                                float weightForeground)
        {

            cv::Mat bg = background.getMat();
            cv::Mat fg = foreground.getMat();

            cv::Mat kernelx = (cv::Mat_<float>(1, 3) << -0.5, 0, 0.5);
            cv::Mat kernely = (cv::Mat_<float>(3, 1) << -0.5, 0, 0.5);

            cv::Mat vxf, vyf, vxb, vyb;
            cv::filter2D(fg, vxf, CV_32F, kernelx, cv::Point(-1,-1), 0, cv::BORDER_REPLICATE);
            cv::filter2D(fg, vyf, CV_32F, kernely, cv::Point(-1,-1), 0, cv::BORDER_REPLICATE);
            cv::filter2D(bg, vxb, CV_32F, kernelx, cv::Point(-1,-1), 0, cv::BORDER_REPLICATE);
            cv::filter2D(bg, vyb, CV_32F, kernely, cv::Point(-1,-1), 0, cv::BORDER_REPLICATE);

            cv::addWeighted(vxf, weightForeground, vxb, 1.f - weightForeground, 0, vx);
            cv::addWeighted(vyf, weightForeground, vyb, 1.f - weightForeground, 0, vy);
        }
    }

    void seamlessClone(cv::InputArray background,
                       cv::InputArray foreground,
                       cv::InputArray foregroundMask,
                       int offsetX,
                       int offsetY,
                       cv::OutputArray destination,
                       CloneType type)
    {

        // Copy original background as we only solve for the overlapping area of the translated foreground mask.
        background.getMat().copyTo(destination);

        // Find overlapping region. We will only perform on this region
        cv::Rect rbg, rfg;
        if (!detail::findOverlap(background, foreground, offsetX, offsetY, rbg, rfg))
            return;

        // Compute the guidance vector field
        cv::Mat vx, vy;
        switch (type) {
            case CLONE_FOREGROUND_GRADIENTS:
                detail::computeWeightedGradientVectorField(background.getMat()(rbg),
                                                           foreground.getMat()(rfg),
                                                           vx, vy,
                                                           1.f);
                break;

            case CLONE_AVERAGED_GRADIENTS:
                detail::computeWeightedGradientVectorField(background.getMat()(rbg),
                                                           foreground.getMat()(rfg),
                                                           vx, vy,
                                                           0.5f);
                break;

            case CLONE_MIXED_GRADIENTS:
                detail::computeMixedGradientVectorField(background.getMat()(rbg),
                                                        foreground.getMat()(rfg),
                                                        vx, vy);
                break;

            default:
                break;
        }

        // For the Poisson equation the divergence of the guidance field is necessary.
        cv::Mat vxx, vyy;
        cv::Mat kernelx = (cv::Mat_<float>(1, 3) << -0.5, 0, 0.5);
        cv::Mat kernely = (cv::Mat_<float>(3, 1) << -0.5, 0, 0.5);
        cv::filter2D(vx, vxx, CV_32F, kernelx);
        cv::filter2D(vy, vyy, CV_32F, kernely);

        cv::Mat f = vxx + vyy;

        cv::Mat boundaryMask(rfg.size(), CV_8UC1);
        cv::threshold(foregroundMask.getMat()(rfg), boundaryMask, constants::UNKNOWN, constants::DIRICHLET_BD, cv::THRESH_BINARY_INV);
        cv::rectangle(boundaryMask, cv::Rect(0, 0, boundaryMask.cols, boundaryMask.rows), constants::DIRICHLET_BD, 1);

        cv::Mat boundaryValues(rfg.size(), CV_MAKETYPE(CV_32F, background.channels()));
        background.getMat()(rbg).convertTo(boundaryValues, CV_32F);

        // Solve Poisson equation
        cv::Mat result;
        solvePoissonEquations(f,
                              boundaryMask,
                              boundaryValues,
                              result);

        // Copy result to destination image.
        result.convertTo(destination.getMat()(rbg), CV_8U);

    }

}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the deep learning tools from the
    dlib C++ Library.  In it, we will train the venerable LeNet convolutional
    neural network to recognize hand written digits.  The network will take as
    input a small image and classify it as one of the 10 numeric digits between
    0 and 9.

    The specific network we will run is from the paper
        LeCun, Yann, et al. "Gradient-based learning applied to document recognition."
        Proceedings of the IEEE 86.11 (1998): 2278-2324.
    except that we replace the sigmoid non-linearities with rectified linear units.

    These tools will use CUDA and cuDNN to drastically accelerate network
    training and testing.  CMake should automatically find them if they are
    installed and configure things appropriately.  If not, the program will
    still run but will be much slower to execute.
*/

#include <dlib/dnn.h>
#include <iostream>
#include <dlib/data_io.h>

using namespace std;
using namespace dlib;

#if defined(BUILD_MONOLITHIC)
#define main(cnt, arr)      dlib_dnn_introduction_ex_main(cnt, arr)
#endif

int main(int argc, const char** argv)
try
{
    // This example is going to run on the MNIST dataset.
    if (argc != 2)
    {
        cout << "This example needs the MNIST dataset to run!" << endl;
        cout << "You can get MNIST from http://yann.lecun.com/exdb/mnist/" << endl;
        cout << "Download the 4 files that comprise the dataset, decompress them, and" << endl;
        cout << "put them in a folder.  Then give that folder as input to this program." << endl;
        return 1;
    }

    // MNIST is broken into two parts, a training set of 60000 images and a test set of
    // 10000 images.  Each image is labeled so that we know what hand written digit is
    // depicted.  These next statements load the dataset into memory.
    std::vector<matrix<unsigned char>> training_images;
    std::vector<unsigned long>         training_labels;
    std::vector<matrix<unsigned char>> testing_images;
    std::vector<unsigned long>         testing_labels;
    load_mnist_dataset(argv[1], training_images, training_labels, testing_images, testing_labels);

    // Now let's define the LeNet.  Broadly speaking, there are 3 parts to a network
    // definition.  The loss layer, a bunch of computational layers, and then an input
    // layer.  You can see these components in the network definition below.
    //
    // The input layer here says the network expects to be given matrix<unsigned char>
    // objects as input.  In general, you can use any dlib image or matrix type here, or
    // even define your own types by creating custom input layers.
    //
    // Then the middle layers define the computation the network will do to transform the
    // input into whatever we want.  Here we run the image through multiple convolutions,
    // ReLU units, max pooling operations, and then finally a fully connected layer that
    // converts the whole thing into just 10 numbers.
    //
    // Finally, the loss layer defines the relationship between the network outputs, our 10
    // numbers, and the labels in our dataset.  Since we selected loss_multiclass_log it
    // means we want to do multiclass classification with our network.   Moreover, the
    // number of network outputs (i.e. 10) is the number of possible labels.  Whichever
    // network output is largest is the predicted label.  So for example, if the first
    // network output is largest then the predicted digit is 0, if the last network output
    // is largest then the predicted digit is 9.
    using net_type = loss_multiclass_log<
                                fc<10,
                                relu<fc<84,
                                relu<fc<120,
                                max_pool<2,2,2,2,relu<con<16,5,5,1,1,
                                max_pool<2,2,2,2,relu<con<6,5,5,1,1,
                                input<matrix<unsigned char>>
                                >>>>>>>>>>>>;
    // This net_type defines the entire network architecture.  For example, the block
    // relu<fc<84,SUBNET>> means we take the output from the subnetwork, pass it through a
    // fully connected layer with 84 outputs, then apply ReLU.  Similarly, a block of
    // max_pool<2,2,2,2,relu<con<16,5,5,1,1,SUBNET>>> means we apply 16 convolutions with a
    // 5x5 filter size and 1x1 stride to the output of a subnetwork, then apply ReLU, then
    // perform max pooling with a 2x2 window and 2x2 stride.

    // So with that out of the way, we can make a network instance.
    net_type net;
    // And then train it using the MNIST data.  The code below uses mini-batch stochastic
    // gradient descent with an initial learning rate of 0.01 to accomplish this.
    dnn_trainer<net_type> trainer(net);
    trainer.set_learning_rate(0.01);
    trainer.set_min_learning_rate(0.00001);
    trainer.set_mini_batch_size(128);
    trainer.be_verbose();
    // Since DNN training can take a long time, we can ask the trainer to save its state to
    // a file named "mnist_sync" every 20 seconds.  This way, if we kill this program and
    // start it again it will begin where it left off rather than restarting the training
    // from scratch.  This is because, when the program restarts, this call to
    // set_synchronization_file() will automatically reload the settings from mnist_sync if
    // the file exists.
    trainer.set_synchronization_file("mnist_sync", std::chrono::seconds(20));
    // Finally, this line begins training.  By default, it runs SGD with our specified
    // learning rate until the loss stops decreasing.  Then it reduces the learning rate by
    // a factor of 10 and continues running until the loss stops decreasing again.  It will
    // keep doing this until the learning rate has dropped below the min learning rate
    // defined above or the maximum number of epochs as been executed (defaulted to 10000).
    trainer.train(training_images, training_labels);

    // At this point our net object should have learned how to classify MNIST images.  But
    // before we try it out let's save it to disk.  Note that, since the trainer has been
    // running images through the network, net will have a bunch of state in it related to
    // the last batch of images it processed (e.g. outputs from each layer).  Since we
    // don't care about saving that kind of stuff to disk we can tell the network to forget
    // about that kind of transient data so that our file will be smaller.  We do this by
    // "cleaning" the network before saving it.
    net.clean();
    serialize("mnist_network.dat") << net;
    // Now if we later wanted to recall the network from disk we can simply say:
    // deserialize("mnist_network.dat") >> net;

    // Now let's run the training images through the network.  This statement runs all the
    // images through it and asks the loss layer to convert the network's raw output into
    // labels.  In our case, these labels are the numbers between 0 and 9.
    std::vector<unsigned long> predicted_labels = net(training_images);
    int num_right = 0;
    int num_wrong = 0;
    // And then let's see if it classified them correctly.
    for (size_t i = 0; i < training_images.size(); ++i)
    {
        if (predicted_labels[i] == training_labels[i])
            ++num_right;
        else
            ++num_wrong;

    }
    cout << "training num_right: " << num_right << endl;
    cout << "training num_wrong: " << num_wrong << endl;
    cout << "training accuracy:  " << num_right / static_cast<double>(num_right + num_wrong) << endl;

    // Let's also see if the network can correctly classify the testing images.  Since
    // MNIST is an easy dataset, we should see at least 99% accuracy.
    predicted_labels = net(testing_images);
    num_right = 0;
    num_wrong = 0;
    for (size_t i = 0; i < testing_images.size(); ++i)
    {
        if (predicted_labels[i] == testing_labels[i])
            ++num_right;
        else
            ++num_wrong;

    }
    cout << "testing num_right: " << num_right << endl;
    cout << "testing num_wrong: " << num_wrong << endl;
    cout << "testing accuracy:  " << num_right / static_cast<double>(num_right + num_wrong) << endl;

    // Finally, you can also save network parameters to XML files if you want to do
    // something with the network in another tool.  For example, you could use dlib's
    // tools/convert_dlib_nets_to_caffe to convert the network to a caffe model.
    net_to_xml(net, "lenet.xml");
}
catch(std::exception& e)
{
    cout << e.what() << endl;
}


//==================================================================================================
/*!
  @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_SQR_ABS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SQR_ABS_HPP_INCLUDED

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

  /*!
    @ingroup group-arithmetic
    This function computes the square of the absolute value of its parameter.
    For real entries it coincides with @ref sqr.

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

    @par Decorators

    - saturated_ for integer entries ensure that sqr_abs(x) is never strictly less than x

    @see sqrt, sqr

    @par Example:

      @snippet sqr_abs.cpp sqr_abs

    @par Possible output:

      @snippet sqr_abs.txt sqr_abs

  **/
  Value sqr_abs(Value const& x);
} }
#endif

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

#endif


//
// $Id: Interpolator.hpp 9294 2016-01-15 20:00:16Z chambm $
//
// 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.
//
// The Original Code is the Quameter software.
//
// The Initial Developer of the Original Code is Matt Chambers.
//
// Copyright 2011 Vanderbilt University
//
// Contributor(s):
//

#include "spline.hpp"
#include <vector>
#include <boost/shared_array.hpp>

namespace freicore {

using std::vector;

struct Interpolator
{
    Interpolator(const vector<double>& x, const vector<double>& y)
    {
        _size = x.size();

        if (_size < 4)
            return;

        BOOST_ASSERT(x.size() == y.size());

        //_ypp.reset(spline_cubic_set(_size, const_cast<double*>(&x[0]), const_cast<double*>(&y[0]), 1, 0, 1, 0));

        _ypp.reset(new double[_size]);
        spline_pchip_set(_size, const_cast<double*>(&x[0]), const_cast<double*>(&y[0]), _ypp.get());
    }

    // uses interpolation on piecewise cubic splines to make an f(x) function evenly spaced on the x axis
    void resample(vector<double>& x, vector<double>& y) const
    {
        BOOST_ASSERT(_size == x.size());
        BOOST_ASSERT(_size == y.size());

        if (x.size() < 4)
            return;

        //double minSampleSize = x[1] - x[0];
        //for (int i=2; i < _size; ++i)
            //minSampleSize = min(minSampleSize, x[i] - x[i-1]);

        vector<double> sampleDeltas(_size, 0);
        for (int i=1; i < _size; ++i)
            sampleDeltas[i] = x[i] - x[i-1];
        sort(sampleDeltas.begin(), sampleDeltas.end());

        double minSampleSize = sampleDeltas[min(size_t(_size - 1), size_t(_size*0.01 + 1))];

        size_t newSize = (x.back() - x.front()) / minSampleSize;

        //double ypval, yppval;
        vector<double> newX, newY;
        newX.reserve(newSize);
        newY.reserve(newSize);
        newX.push_back(x[0]);
        newY.push_back(y[0]);
        for (size_t i=1; newX.back() < x.back(); ++i)
        {
            newX.push_back(newX.back() + minSampleSize);
            newY.push_back(0);
            int left = i-1;
            //spline_cubic_val2(_size, &x[0], newX.back(), &left, &y[0], _ypp.get(), &newY.back(), &ypval, &yppval);
            spline_pchip_val(_size, &x[0], &y[0], _ypp.get(), 1, &newX.back(), &newY.back());
        }
        swap(x, newX);
        swap(y, newY);
    }

    double interpolate(const vector<double>& xs, const vector<double>& ys, double x) const
    {
        if (x < xs.front() || x > xs.back() || xs.size() < 4)
            return 0;

        //double ypval, yppval;
        //return spline_cubic_val(_size, const_cast<double*>(&xs[0]), const_cast<double*>(&ys[0]), _ypp.get(), x, &ypval, &yppval);

        double y;
        spline_pchip_val(_size, const_cast<double*>(&xs[0]), const_cast<double*>(&ys[0]), _ypp.get(), 1, &x, &y);
        return y;
    }

    private:
    boost::shared_array<double> _ypp;
    int _size;
};

} // namespace freicore


#include <common_robotics_utilities/ros_conversions.hpp>

#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/math.hpp>

namespace common_robotics_utilities
{
namespace ros_conversions
{
Eigen::Vector3d GeometryPointToEigenVector3d(
    const GeometryPoint& point)
{
  Eigen::Vector3d eigen_point(point.x, point.y, point.z);
  return eigen_point;
}

GeometryPoint EigenVector3dToGeometryPoint(
    const Eigen::Vector3d& point)
{
  GeometryPoint geom_point;
  geom_point.x = point.x();
  geom_point.y = point.y();
  geom_point.z = point.z();
  return geom_point;
}

Eigen::Vector4d GeometryPointToEigenVector4d(
    const GeometryPoint& point)
{
  Eigen::Vector4d eigen_point(point.x, point.y, point.z, 1.0);
  return eigen_point;
}

GeometryPoint EigenVector4dToGeometryPoint(
    const Eigen::Vector4d& point)
{
  GeometryPoint geom_point;
  geom_point.x = point(0);
  geom_point.y = point(1);
  geom_point.z = point(2);
  return geom_point;
}

GeometryPointStamped EigenVector3dToGeometryPointStamped(
    const Eigen::Vector3d& point, const std::string& frame_id)
{
  GeometryPointStamped point_stamped;
  point_stamped.header.frame_id = frame_id;
  point_stamped.point = EigenVector3dToGeometryPoint(point);
  return point_stamped;
}

Eigen::Vector3d GeometryVector3ToEigenVector3d(
    const GeometryVector3& vector)
{
  Eigen::Vector3d eigen_vector(vector.x, vector.y, vector.z);
  return eigen_vector;
}

GeometryVector3 EigenVector3dToGeometryVector3(
    const Eigen::Vector3d& vector)
{
  GeometryVector3 geom_vector;
  geom_vector.x = vector.x();
  geom_vector.y = vector.y();
  geom_vector.z = vector.z();
  return geom_vector;
}

Eigen::Vector4d GeometryVector3ToEigenVector4d(
    const GeometryVector3& vector)
{
  Eigen::Vector4d eigen_vector(vector.x, vector.y, vector.z, 0.0);
  return eigen_vector;
}

GeometryVector3 EigenVector4dToGeometryVector3(
    const Eigen::Vector4d& vector)
{
  GeometryVector3 geom_vector;
  geom_vector.x = vector(0);
  geom_vector.y = vector(1);
  geom_vector.z = vector(2);
  return geom_vector;
}

Eigen::Quaterniond GeometryQuaternionToEigenQuaterniond(
    const GeometryQuaternion& quat)
{
  Eigen::Quaterniond eigen_quaternion(quat.w, quat.x, quat.y, quat.z);
  return eigen_quaternion;
}

GeometryQuaternion EigenQuaterniondToGeometryQuaternion(
    const Eigen::Quaterniond& quat)
{
  GeometryQuaternion geom_quaternion;
  geom_quaternion.w = quat.w();
  geom_quaternion.x = quat.x();
  geom_quaternion.y = quat.y();
  geom_quaternion.z = quat.z();
  return geom_quaternion;
}

Eigen::Isometry3d GeometryPoseToEigenIsometry3d(
    const GeometryPose& pose)
{
  const Eigen::Translation3d trans(pose.position.x,
                                   pose.position.y,
                                   pose.position.z);
  const Eigen::Quaterniond quat(pose.orientation.w,
                                pose.orientation.x,
                                pose.orientation.y,
                                pose.orientation.z);
  const Eigen::Isometry3d eigen_pose = trans * quat;
  return eigen_pose;
}

GeometryPose EigenIsometry3dToGeometryPose(
    const Eigen::Isometry3d& transform)
{
  const Eigen::Vector3d trans = transform.translation();
  const Eigen::Quaterniond quat(transform.rotation());
  GeometryPose geom_pose;
  geom_pose.position.x = trans.x();
  geom_pose.position.y = trans.y();
  geom_pose.position.z = trans.z();
  geom_pose.orientation.w = quat.w();
  geom_pose.orientation.x = quat.x();
  geom_pose.orientation.y = quat.y();
  geom_pose.orientation.z = quat.z();
  return geom_pose;
}

GeometryPoseStamped EigenIsometry3dToGeometryPoseStamped(
    const Eigen::Isometry3d& transform, const std::string& frame_id)
{
  GeometryPoseStamped pose_stamped;
  pose_stamped.header.frame_id = frame_id;
  pose_stamped.pose = EigenIsometry3dToGeometryPose(transform);
  return pose_stamped;
}

Eigen::Isometry3d GeometryTransformToEigenIsometry3d(
    const GeometryTransform& transform)
{
  const Eigen::Translation3d trans(transform.translation.x,
                                   transform.translation.y,
                                   transform.translation.z);
  const Eigen::Quaterniond quat(transform.rotation.w,
                                transform.rotation.x,
                                transform.rotation.y,
                                transform.rotation.z);
  const Eigen::Isometry3d eigen_transform = trans * quat;
  return eigen_transform;
}

GeometryTransform EigenIsometry3dToGeometryTransform(
    const Eigen::Isometry3d& transform)
{
  const Eigen::Vector3d trans = transform.translation();
  const Eigen::Quaterniond quat(transform.rotation());
  GeometryTransform geom_transform;
  geom_transform.translation.x = trans.x();
  geom_transform.translation.y = trans.y();
  geom_transform.translation.z = trans.z();
  geom_transform.rotation.w = quat.w();
  geom_transform.rotation.x = quat.x();
  geom_transform.rotation.y = quat.y();
  geom_transform.rotation.z = quat.z();
  return geom_transform;
}

GeometryTransformStamped EigenIsometry3dToGeometryTransformStamped(
    const Eigen::Isometry3d& transform, const std::string& frame_id,
    const std::string& child_frame_id)
{
  GeometryTransformStamped transform_stamped;
  transform_stamped.header.frame_id = frame_id;
  transform_stamped.child_frame_id = child_frame_id;
  transform_stamped.transform = EigenIsometry3dToGeometryTransform(transform);
  return transform_stamped;
}

Eigen::Matrix3Xd VectorGeometryPointToEigenMatrix3Xd(
    const std::vector<GeometryPoint>& vector_geom)
{
  Eigen::Matrix3Xd eigen_matrix = Eigen::MatrixXd(3, vector_geom.size());
  for (size_t idx = 0; idx < vector_geom.size(); idx++)
  {
    eigen_matrix.block<3,1>(0, static_cast<ssize_t>(idx))
        = GeometryPointToEigenVector3d(vector_geom.at(idx));
  }
  return eigen_matrix;
}

std::vector<GeometryPoint> EigenMatrix3XdToVectorGeometryPoint(
    const Eigen::Matrix3Xd& eigen_matrix)
{
  std::vector<GeometryPoint> vector_geom(
      static_cast<size_t>(eigen_matrix.cols()));
  for (size_t idx = 0; idx < vector_geom.size(); idx++)
  {
    vector_geom.at(idx)
        = EigenVector3dToGeometryPoint(
            eigen_matrix.block<3,1>(0, static_cast<ssize_t>(idx)));
  }
  return vector_geom;
}

std::vector<GeometryPoint>
VectorEigenVector3dToVectorGeometryPoint(
    const common_robotics_utilities::math::VectorVector3d& vector_eigen)
{
  std::vector<GeometryPoint> vector_geom(vector_eigen.size());
  for (size_t idx = 0; idx < vector_eigen.size(); idx++)
  {
    vector_geom.at(idx) = EigenVector3dToGeometryPoint(vector_eigen.at(idx));
  }
  return vector_geom;
}

common_robotics_utilities::math::VectorVector3d
VectorGeometryPointToVectorEigenVector3d(
    const std::vector<GeometryPoint>& vector_geom)
{
  common_robotics_utilities::math::VectorVector3d vector_eigen(
      vector_geom.size());
  for (size_t idx = 0; idx < vector_geom.size(); idx++)
  {
    vector_eigen.at(idx) = GeometryPointToEigenVector3d(vector_geom.at(idx));
  }
  return vector_eigen;
}

common_robotics_utilities::math::VectorVector3d
VectorGeometryVector3ToEigenVector3d(
    const std::vector<GeometryVector3>& vector_geom)
{
  common_robotics_utilities::math::VectorVector3d vector_eigen(
      vector_geom.size());
  for (size_t idx = 0; idx < vector_geom.size(); idx++)
  {
    vector_eigen.at(idx) = GeometryVector3ToEigenVector3d(vector_geom.at(idx));
  }
  return vector_eigen;
}

common_robotics_utilities::math::VectorIsometry3d
VectorGeometryPoseToVectorIsometry3d(
    const std::vector<GeometryPose>& vector_geom)
{
  common_robotics_utilities::math::VectorIsometry3d vector_eigen(
      vector_geom.size());
  for (size_t idx = 0; idx < vector_geom.size(); idx++)
  {
    vector_eigen.at(idx) = GeometryPoseToEigenIsometry3d(vector_geom.at(idx));
  }
  return vector_eigen;
}

common_robotics_utilities::math::VectorIsometry3d
VectorGeometryPoseToVectorIsometry3d(
    const std::vector<GeometryTransform>& vector_geom)
{
  common_robotics_utilities::math::VectorIsometry3d vector_eigen(
      vector_geom.size());
  for (size_t idx = 0; idx < vector_geom.size(); idx++)
  {
    vector_eigen.at(idx) =
        GeometryTransformToEigenIsometry3d(vector_geom.at(idx));
  }
  return vector_eigen;
}

std::vector<GeometryPose> VectorIsometry3dToVectorGeometryPose(
    const common_robotics_utilities::math::VectorIsometry3d& vector_eigen)
{
  std::vector<GeometryPose> vector_geom(vector_eigen.size());
  for (size_t idx = 0; idx < vector_eigen.size(); idx++)
  {
    vector_geom.at(idx) = EigenIsometry3dToGeometryPose(vector_eigen.at(idx));
  }
  return vector_geom;
}

std::vector<GeometryTransform> VectorIsometry3dToVectorGeometryTransform(
    const common_robotics_utilities::math::VectorIsometry3d& vector_eigen)
{
  std::vector<GeometryTransform> vector_geom(vector_eigen.size());
  for (size_t idx = 0; idx < vector_eigen.size(); idx++)
  {
    vector_geom.at(idx) =
        EigenIsometry3dToGeometryTransform(vector_eigen.at(idx));
  }
  return vector_geom;
}
}  // namespace ros_conversions
}  // namespace common_robotics_utilities


#include "gtest/gtest.h"

#include <cmath>
#include <iostream>
#include <numeric>
#include <cassert>
#include <omp.h>

#include <Eigen/Dense>

#include "vptree.h"
#include "dataset.h"

using namespace clustering;

namespace {
static const std::string CURRENT_TDIR( CURRENT_TEST_DIR );
static const float MIN_T = 0.001;

inline double dist( const Eigen::VectorXf& p1, const Eigen::VectorXf& p2 )
{
    return ( p1 - p2 ).norm();
}

typedef VPTREE< Eigen::VectorXf, dist > TTree;
}

TEST( VPTree, DistSearch )
{
    Dataset::Ptr dset = Dataset::create();
    ASSERT_TRUE( dset->load_csv( CURRENT_TDIR + "/csv/vptree01.csv" ) );

    TTree tree;
    tree.create( dset );

    const Dataset::DataContainer& d = dset->data();

    for ( size_t i = 0; i < d.size(); ++i ) {

        TTree::TNeighborsList nlist;

        LOG( INFO ) << "Searching for " << i << " [" << d[i] << "]";

        tree.search_by_dist( d[i], MIN_T, nlist );

        EXPECT_EQ( nlist.size(), 5u );

        for ( size_t j = 0; j < nlist.size(); ++j ) {
            LOG( INFO ) << "Found neighbor id = " << nlist[j].first << " dist= " << nlist[j].second << " [" << d[nlist[j].first] << "]";
            EXPECT_TRUE( dist( d[i], d[nlist[j].first] ) < MIN_T );
        }
    }
}

TEST( VPTree, KNSearch )
{
    Dataset::Ptr dset = Dataset::create();
    ASSERT_TRUE( dset->load_csv( CURRENT_TDIR + "/csv/vptree01.csv" ) );

    TTree tree;
    tree.create( dset );

    const Dataset::DataContainer& d = dset->data();

    for ( size_t i = 0; i < d.size(); ++i ) {

        TTree::TNeighborsList nlist;

        LOG( INFO ) << "Searching for " << i << " [" << d[i] << "]";

        tree.search_by_k( d[i], 5u, nlist );

        EXPECT_EQ( nlist.size(), 5u );

        for ( size_t j = 0; j < nlist.size(); ++j ) {
            LOG( INFO ) << "Found neighbor id = " << nlist[j].first << " dist= " << nlist[j].second << " [" << d[nlist[j].first] << "]";
            EXPECT_TRUE( dist( d[i], d[nlist[j].first] ) < MIN_T );
        }
    }
}

TEST( VPTree, KNSearchNoSimilar )
{
    Dataset::Ptr dset = Dataset::create();
    ASSERT_TRUE( dset->load_csv( CURRENT_TDIR + "/csv/vptree01.csv" ) );

    TTree tree;
    tree.create( dset );

    const Dataset::DataContainer& d = dset->data();

    for ( size_t i = 0; i < d.size(); ++i ) {

        TTree::TNeighborsList nlist;

        LOG( INFO ) << "Searching for " << i << " [" << d[i] << "]";

        tree.search_by_k( d[i], 5u, nlist, true );

        EXPECT_EQ( nlist.size(), 5u );

        for ( size_t j = 0; j < nlist.size(); ++j ) {
            LOG( INFO ) << "Found neighbor id = "
                        << nlist[j].first << " dist= " << nlist[j].second << " [" << d[nlist[j].first] << "]";
            EXPECT_TRUE( dist( d[i], d[nlist[j].first] ) > 0 );
        }
    }
}


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

#include "poisson.h"

/**
 * \mainpage Adaptive FEM for Poisson problem
 *
 * This is the starting code for laboratory number 7 of the course "Theory and
 * Practice of Finite Element methods".
 *
 * We solve the Poisson problem
 *
 * \[
 * \begin{split}
 * -\Delta u &= f \quad \text{ in } \Omega\\
 * n\cdot \nabla u &= g_N \quad \text{ on } \partial\Omega_N\\
 *  u &= g_D \quad \text{ on } \partial\Omega_D
 * \end{split}
 * \]
 *
 * on convex and Lipschitz domains $\Omega$, using the Finite Element method,
 * and the deal.II library (www.dealii.org).
 */

int
main(int argc, char **argv)
{
  Utilities::MPI::MPI_InitFinalize init(argc, argv);
  std::string                      par_name = "";
  if (argc > 1)
    par_name = argv[1];

  deallog.depth_console(2);
  Poisson<2> laplace_problem;
  laplace_problem.initialize(par_name);
  laplace_problem.run();
  return 0;
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_RAYLEIGH_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_RAYLEIGH_RNG_HPP

#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/scal/fun/log1m.hpp>
#include <stan/math/prim/scal/fun/square.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
  namespace math {

    template <class RNG>
    inline double
    rayleigh_rng(double sigma,
                 RNG& rng) {
      using boost::variate_generator;
      using boost::random::uniform_real_distribution;

      static const char* function("rayleigh_rng");

      check_positive(function, "Scale parameter", sigma);

      variate_generator<RNG&, uniform_real_distribution<> >
        uniform_rng(rng, uniform_real_distribution<>(0.0, 1.0));
      return sigma * std::sqrt(-2.0 * std::log(uniform_rng()));
    }

  }
}
#endif


//==================================================================================================
/*!
  @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_CONSTANT_PIO_6_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_PIO_6_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Constant \f$\frac\pi{6}\f$.

    @par Semantic:

    For type T:

    @code
    T r = Pio_6<T>();
    @endcode

    is similar to:

    @code
    T r = Pi<T>()/Six<T>();
    @endcode

    @return a value of type T

**/
  template<typename T> T Pio_6();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Constant \f$\frac\pi{6}\f$.

      Generate the  constant pio_6.

      @return The Pio_6 constant for the proper type
    **/
    const boost::dispatch::functor<tag::pio_6_> pio_6 = {};
  }
} }
#endif

#include <boost/simd/constant/definition/pio_6.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


#ifndef YASMIC_IFSTREAM_AS_MATRIX
#define YASMIC_IFSTREAM_AS_MATRIX

#include <fstream>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_io.hpp>
#include <yasmic/smatrix_traits.hpp>
#include <iterator>

#include <yasmic/generic_matrix_operations.hpp>

namespace yasmic
{
	template <>
    struct smatrix_traits<std::ifstream>
    {
    	typedef unsigned int size_type;
    	typedef int index_type;
		typedef double value_type;

		typedef boost::tuple<index_type, index_type, value_type> nonzero_descriptor;

		typedef std::istream_iterator<nonzero_descriptor> nonzero_iterator;

		typedef void row_iterator;

		typedef void row_nonzero_descriptor;
		typedef void row_nonzero_iterator;

		typedef void column_iterator;
    };

    inline std::pair<smatrix_traits<std::ifstream>::size_type,
                      smatrix_traits<std::ifstream>::size_type>
    dimensions(std::ifstream& f)
    {
    	smatrix_traits<std::ifstream>::size_type nrows,ncols;

    	f.clear();
    	f.seekg(0, std::ios_base::beg);

        f >> nrows >> ncols;

        return (std::make_pair(nrows, ncols));
    }

	smatrix_traits<std::ifstream>::size_type nnz(std::ifstream& f)
	{
		smatrix_traits<std::ifstream>::size_type d1,d2,nnz;

		// clear any error bits
		f.clear();
        f.seekg(0, std::ios_base::beg);

        f >> d1 >> d2 >> nnz;

        return (nnz);
	}

	inline std::pair<smatrix_traits<std::ifstream>::nonzero_iterator,
                      smatrix_traits<std::ifstream>::nonzero_iterator>
    nonzeros(std::ifstream& f)
    {
    	smatrix_traits<std::ifstream>::size_type d1,d2,d3;

    	f.clear();
    	f.seekg(0, std::ios_base::beg);

        f >> d1  >> d2 >> d3;

        f >> boost::tuples::set_open(' ') >> boost::tuples::set_close(' ');

        typedef smatrix_traits<std::ifstream>::nonzero_iterator nz_iter;

        return (std::make_pair(nz_iter(f), nz_iter()));
    }
}

#endif //YASMIC_IFSTREAM_AS_MATRIX


#include <boost/graph/use_mpi.hpp>
#include <boost/throw_exception.hpp>
#include <boost/graph/distributed/page_rank.hpp>
#include <boost/test/minimal.hpp>
#include <boost/graph/distributed/adjacency_list.hpp>
#include <boost/graph/distributed/mpi_process_group.hpp>
#include <boost/test/minimal.hpp>
#include <vector>
#include <iostream>
#include <stdlib.h>
#include <cstdlib>
#include <iomanip>

#include <boost/config.hpp>
#include <boost/graph/distributed/compressed_sparse_row_graph.hpp>
#include <boost/property_map/parallel/distributed_property_map.hpp>
#include <boost/graph/distributed/concepts.hpp>
#include <boost/graph/rmat_graph_generator.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/property_map/vector_property_map.hpp>

#ifdef BOOST_NO_EXCEPTIONS
void
boost::throw_exception(std::exception const& ex)
{
    std::cout << ex.what() << std::endl;
    abort();
}
#endif

using namespace boost;
using boost::graph::distributed::mpi_process_group;

typedef double time_type;

inline time_type get_time()
{
      return MPI_Wtime();
}

std::string print_time(time_type t)
{
      std::ostringstream out;
      out << std::setiosflags(std::ios::fixed) << std::setprecision(2) << t;
      return out.str();
}

bool close_to(double x, double y)
{
  double diff = x - y;
  if (diff < 0) diff = -diff;
  double base = (y == 0? x : y);
  if (base != 0) return diff / base < 0.01;
  else return true;
}

// Make convenient labels for the vertices
//enum vertex_id_t { A, B, C, D, N };

void test_distributed_page_rank(char* graphfilename ,int num_ofVertex, int num_ofEdges, int iterations)
{
  //using namespace boost::graph;

  int N = num_ofVertex;
  mpi_process_group pg;
  std::size_t id = process_id(pg);
  typedef parallel::variant_distribution<mpi_process_group> Distribution;
  Distribution distrib = parallel::block(pg, N);

  // create a typedef for the Graph type
  typedef adjacency_list<vecS,
                         distributedS<mpi_process_group, vecS>,
                         bidirectionalS
                         > Graph;
  typedef graph_traits<Graph>::vertex_descriptor vertex_descriptor;

  // writing out the edges in the graph
  typedef std::pair<int, int> Edge;
  //init Graph
  //std::ifstream graphfile("./amazon_gen_15.txt");
  std::ifstream graphfile(graphfilename);
  std::string line;
  //Edge edge_array[] = { Edge(A,B), Edge(A,C), Edge(B,C), Edge(C,A), Edge(D,C) };
  Edge edge_array[num_ofEdges];
  memset(edge_array,0,sizeof(int));
  //

  //Graph g;
  //std::map<int,int> edge_array;
  int i=0;

  //std::cout<<"!!!!!"<<std::endl;
  while(getline(graphfile,line)!=NULL){

      std::string::size_type index = line.find_first_of("\t",0);
      std::string::size_type end = line.find_first_of("\n",0);
      std::string inedge_s = line.substr(0,index);
      int inedge_num = std::atoi(inedge_s.c_str());
      std::string outedge_s = line.substr(index+1,end);
      int outedge_num = std::atoi(outedge_s.c_str());

      //add_edge(inedge_num, outedge_num, g);
      //edge_array.insert(inedge_num,outedge_num);

      edge_array[i].first=inedge_num;
      edge_array[i].second=outedge_num;
      //std::cout << edge_array[i].first <<" "<<edge_array[i].second<< std::endl;
      i++;
  }
  const int num_edges = sizeof(edge_array)/sizeof(edge_array[0]);

  // declare a graph object
  Graph g(edge_array, edge_array + num_edges, N);

  //std::vector<double> ranks(num_vertices(g));

  //page_rank(g,
  //          make_iterator_property_map(ranks.begin(),
   //                                    get(boost::vertex_index, g)),
   //         n_iterations(iterations), 0.85, N);

  synchronize(g);
  if (id == 0) {
      printf("INFO: Starting PageRank.\n");
      printf("INFO: Params: InputGraphfile=%s, num_ofVertex=%d, num_ofEdges=%d, iterations=%d\n", graphfilename , num_ofVertex, num_ofEdges, iterations );
  }
  std::vector<double> ranks(num_vertices(g));
  time_type start = get_time();
  page_rank(g, make_iterator_property_map(ranks.begin(), get(boost::vertex_index, g)),
          graph::n_iterations(iterations), 0.85, N);
  time_type end = get_time();

  if (process_id(g.process_group()) == 0) {
      std::cout << "INFO: PageRank Complete. time = " <<
          print_time(end - start) << "s." << std::endl;
      //printf("RESULT: %d %d %d %f %f %f %f %f\n",
        //      num_processes(pg), n, m, a, b, c, d, end - start);
  }

  double local_sum = 0.0;
  for(unsigned int i = 0; i < num_vertices(g); ++i) {
    std::cout << g.distribution().global(i)<<" = "<< ranks[i]<< "\n";
    local_sum += ranks[i];
  }

  double sum=0.;
  boost::mpi::reduce(communicator(g.process_group()),
                     local_sum, sum, std::plus<double>(), 0);
  if (process_id(g.process_group()) == 0) {

      std::cout << "Sum = " << sum << "\n\n";
      //BOOST_CHECK(close_to(sum, 5000 )); // 1 when alpha=0
  }

/*
  //   double expected_ranks0[N] = {0.400009, 0.199993, 0.399998, 0.0};
  double expected_ranks[N];// = {1.49011, 0.783296, 1.5766, 0.15};
  std::memset(expected_ranks, 1/N, N*sizeof(double));
  for (int i = 0; i < N; ++i) {
    vertex_descriptor v = vertex(i, g);
    if (v != Graph::null_vertex()
        && owner(v) == process_id(g.process_group())) {
      BOOST_CHECK(close_to(ranks[local(v)], expected_ranks[i]));

    }
  }
  */
}

int test_main(int argc, char* argv[])
{
  mpi::environment env(argc, argv);

  /*
  char* inputGraph = argv[1];
  int num_ofEdges = 131655;
  int num_ofVertex = 32768;
  int iterations = 50;
  if (argc > 1) {
    iterations = atoi(argv[1]);
  }
  */

  if (argc < 5){
      //printf("INFO: Params: InputGraphfile=%s, num_ofVertex=%d, num_ofEdges=%d, iterations=%d\n", "amazon_gen_15.txt",32768 , 131655, 50);
      test_distributed_page_rank("amazon_gen_15.txt",32768, 131655, 50);
  }
  else{
     // printf("INFO: Params: InputGraphfile=%s, num_ofVertex=%d, num_ofEdges=%d, iterations=%d\n", argv[1], atoi(argv[2]), atoi(argv[3]), atoi(argv[4]) );
      test_distributed_page_rank(argv[1], atoi(argv[2]), atoi(argv[3]), atoi(argv[4]) );
  }

  //test_distributed_page_rank(num_ofVertex,num_ofEdges,iterations);

  return 0;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_ULP_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ULP_HPP_INCLUDED

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

 /*!

    @ingroup group-ieee
    Function object implementing ulp capabilities

    Returns the ulp distance to the nearest (distinct) element of the same type.

    @par Semantic:

    @code
    T r = ulp(x);
    @endcode

    is similar to:

    @code
    T r = min(x-prev(x), next(x)-x)/Eps<T>();
    @endcode

    @par Note
    ulp stands for Unit in the Last Place.

    @see ulpdist, eps, Eps

  **/
  const boost::dispatch::functor<tag::ulp_> ulp = {};
} }
#endif

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

#endif


#include "optimization_utils.hpp"
#include <boost/algorithm/clamp.hpp>
#include <random>

namespace optimization
{
std::vector<std::vector<double>> get_initial_positions(double left_bound, double right_bound, int dimension, int number_of_agents)
{
    std::vector<std::vector<double>> positions(number_of_agents, std::vector<double>(dimension, 0.));
    for (auto &agent : positions)
        for (auto &coord : agent)
            coord = get_random(left_bound, right_bound);

    return positions;
}

double get_random(double left_bound, double right_bound)
{
    static std::random_device rd;
    static std::mt19937 gen(rd());
    std::uniform_real_distribution<double> dist(left_bound, right_bound);

    return dist(gen);
}

std::vector<std::vector<double>> &clip_positions(std::vector<std::vector<double>> &positions, double left_bound, double right_bound)
{
    for (auto &agent : positions)
        for (auto &coord : agent)
            coord = boost::algorithm::clamp(coord, left_bound, right_bound);

    return positions;
}
}


/*

Copyright (c) 2005-2015, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford 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 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.

*/

#ifndef NAGAIHONDAFORCE_HPP_
#define NAGAIHONDAFORCE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>
#include "Exception.hpp"

#include "AbstractForce.hpp"
#include "VertexBasedCellPopulation.hpp"

#include <iostream>

/**
 * A force class for use in vertex-based simulations, based on a mechanical
 * model proposed by T. Nagai and H. Honda ("A dynamic cell model for the formation
 * of epithelial tissues", Philosophical Magazine Part B 81:699-719). In contrast to the force proposed
 * by Nagai and Honda this force has an additional force term implemented that scales with the perimeter
 * of a cell to simulate the surface membrane energy. This particular perimeter force term in turn differs from the one
 * proposed by Farhadifar et al (2007) in the sense that it employs a target perimeter.
 *
 * Each of the model parameter member variables are rescaled such that mDampingConstantNormal
 * takes the default value 1, whereas Nagai and Honda (who denote the parameter by
 * nu) take the value 0.01.
 */
template<unsigned DIM>
class NagaiHondaForce  : public AbstractForce<DIM>
{
friend class TestForces;

private:

    friend class boost::serialization::access;
    /**
     * Boost Serialization method for archiving/checkpointing.
     * Archives the object and its member variables.
     *
     * @param archive  The boost archive.
     * @param version  The current version of this class.
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractForce<DIM> >(*this);
        archive & mNagaiHondaDeformationEnergyParameter;
        archive & mNagaiHondaMembraneSurfaceEnergyParameter;
        archive & mNagaiHondaCellCellAdhesionEnergyParameter;
        archive & mNagaiHondaCellBoundaryAdhesionEnergyParameter;
    }

protected:

    /**
     * Cell deformation energy parameter. Has units of kg s^-2 (cell size at equilibrium rest length)^-1.
     */
    double mNagaiHondaDeformationEnergyParameter;

    /**
     * Cell membrane energy parameter. Has units of kg (cell size at equilibrium rest length) s^-2.
     */
    double mNagaiHondaMembraneSurfaceEnergyParameter;

    /**
     * Cell-cell adhesion energy parameter. Has has units of kg (cell size at equilibrium rest length)^2 s^-2.
     * This parameter corresponds to 1/2 of the Lambda parameter in forces proposed by Farhadifar et al (2007).
     * This slight difference comes from the fact that when we apply the forces to a particular node, each
     * edge is visited twice - and hence the force originating from that edge is applied twice.
     */
    double mNagaiHondaCellCellAdhesionEnergyParameter;

    /**
     * Cell-boundary adhesion energy parameter. Has units of kg (cell size at equilibrium rest length)^2 s^-2.
     */
    double mNagaiHondaCellBoundaryAdhesionEnergyParameter;

public:

    /**
     * Constructor.
     */
    NagaiHondaForce();

    /**
     * Destructor.
     */
    virtual ~NagaiHondaForce();

    /**
     * Overridden AddForceContribution() method.
     *
     * Calculates the force on each node in the vertex-based cell population based on the
     * Nagai Honda model.
     *
     * @param rCellPopulation reference to the cell population
     */
    virtual void AddForceContribution(AbstractCellPopulation<DIM>& rCellPopulation);

    /**
     * Get the adhesion parameter for the edge between two given nodes.
     *
     * @param pNodeA one node
     * @param pNodeB the other node
     * @param rVertexCellPopulation reference to the cell population
     *
     * @return the adhesion parameter for this edge.
     */
    virtual double GetAdhesionParameter(Node<DIM>* pNodeA, Node<DIM>* pNodeB, VertexBasedCellPopulation<DIM>& rVertexCellPopulation);

    /**
     * @return mNagaiHondaDeformationEnergyParameter
     */
    double GetNagaiHondaDeformationEnergyParameter();

    /**
     * @return mNagaiHondaMembraneSurfaceEnergyParameter
     */
    double GetNagaiHondaMembraneSurfaceEnergyParameter();

    /**
     * @return mCellCellAdhesionEnergyParameter
     */
    double GetNagaiHondaCellCellAdhesionEnergyParameter();

    /**
     * @return mNagaiHondaCellBoundaryAdhesionEnergyParameter
     */
    double GetNagaiHondaCellBoundaryAdhesionEnergyParameter();

    /**
     * Set mNagaiHondaDeformationEnergyParameter.
     *
     * @param nagaiHondaDeformationEnergyParameter the new value of mNagaiHondaDeformationEnergyParameter
     */
    void SetNagaiHondaDeformationEnergyParameter(double nagaiHondaDeformationEnergyParameter);

    /**
     * Set mNagaiHondaMembraneSurfaceEnergyParameter.
     *
     * @param nagaiHondaMembraneSurfaceEnergyParameter the new value of mNagaiHondaMembraneSurfaceEnergyParameter
     */
    void SetNagaiHondaMembraneSurfaceEnergyParameter(double nagaiHondaMembraneSurfaceEnergyParameter);

    /**
     * Set mNagaiHondaCellCellAdhesionEnergyParameter. This parameter corresponds to 1/2 of the Lambda parameter in the forces by
     * Farhadifar et al (2007).
     *
     * @param nagaiHondaCellCellAdhesionEnergyEnergyParameter the new value of mNagaiHondaCellCellAdhesionEnergyParameter
     */
    void SetNagaiHondaCellCellAdhesionEnergyParameter(double nagaiHondaCellCellAdhesionEnergyEnergyParameter);

    /**
     * Set mNagaiHondaCellBoundaryAdhesionEnergyParameter.
     *
     * @param nagaiHondaCellBoundaryAdhesionEnergyParameter the new value of mNagaiHondaCellBoundaryAdhesionEnergyParameter
     */
    void SetNagaiHondaCellBoundaryAdhesionEnergyParameter(double nagaiHondaCellBoundaryAdhesionEnergyParameter);

    /**
     * Overridden OutputForceParameters() method.
     *
     * @param rParamsFile the file stream to which the parameters are output
     */
    void OutputForceParameters(out_stream& rParamsFile);
};

#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_SAME_DIMS(NagaiHondaForce)

#endif /*NAGAIHONDAFORCE_HPP_*/


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

#ifndef DCS_MATH_STATS_DISTRIBUTION_GAMMA_HPP
#define DCS_MATH_STATS_DISTRIBUTION_GAMMA_HPP

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

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

#include <boost/math/distributions/gamma.hpp>
//#include <boost/random/gamma_distribution.hpp>
//#include <boost/random/variate_generator.hpp>
#include <cmath>
#include <cstddef>
#include <dcs/math/policies/policy.hpp>
#include <dcs/math/random/uniform_01_adaptor.hpp>
//#include <dcs/math/stats/distribution/detail/rgamma.cpp>
//#include <dcs/math/constants.hpp>
#include <iostream>
#include <vector>

namespace dcs { namespace math { namespace stats {

namespace detail {

} // Namespace detail

/**
 * \brief The Gamma distribution with shape parameter \f$k\f$ and scale
 *  parameter \f$\theta\f$.
 *
 * \tparam RealT The type used for real numbers.
 * \tparam PolicyT The policy type.
 *
 * The probability density function (pdf):
 * \f[
 *   \Pr(x|k,\theta) = x^{k-1} \frac{\exp{\left(-x/\theta\right)}}{\Gamma(k)\,\theta^k}
 * \f]
 *
 * \author Cosimo Anglano (cosimo.anglano@mfn.unipmn.it)
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template < typename RealT=double, typename PolicyT=::dcs::math::policies::policy<> >
class gamma_distribution
{
	public: typedef RealT support_type;
	public: typedef RealT value_type;
	public: typedef PolicyT policy_type;

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

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

	/**
	 * \brief Generate a random number distributed according to this
	 * gamma distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \return A random number distributed according to this gamma
	 * distribution.
	 *
	 * A \c gamma random number distribution produces random numbers
	 * \f$x \ge 0\f$ distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|k,\theta) = x^{k-1} \frac{\exp{\left(-x/\theta\right)}}{\Gamma(k)\,\theta^k}
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		support_type rand(UniformRandomGeneratorT& rng) const
	{
/* Don't work: Actually (2010-01-21), Boost uses Gamma with only the parameter
 * shape. In principle this wouldn't be a problem since we may use the scaling
 * property gamma_rand(a,b)==b*gamma_rand(a); but for shape==1, it uses
 * Exp(rng()) which is wrong when b!=1 since Gamma(1,b) ~ Exp(1/b)
 *
		typedef ::boost::gamma_distribution<support_type> rdist_type;
		typedef ::boost::variate_generator<UniformRandomGeneratorT&, rdist_type> variate_type;

		return dist_.scale() * variate_type(rng, rdist_type(dist_.shape()))();
*/
		::dcs::math::random::uniform_01_adaptor<UniformRandomGeneratorT&, support_type> eng(rng);
		value_type r(0);
		//value_type ishape = static_cast<unsigned long>(dist_.shape()); // Not safe
		value_type ishape = ::std::floor(dist_.shape());

		if (dist_.shape() >= value_type(1))
		{
			value_type acc(0);
			for (::std::size_t i = 0; i < ishape; ++i)
			{
				acc += ::std::log(eng());
			}
			r += -acc;
		}
		value_type diff = dist_.shape()-ishape;
		if (diff > 0)
		{
/*
			// Acceptance-rejection method (see: http://en.wikipedia.org/wiki/Gamma_distribution#Generating_gamma-distributed_random_variables)

			value_type v0 = ::dcs::math::constants::e<value_type>::value/(::dcs::math::constants::e<value_type>::value + diff);

			value_type csi;
			value_type eta;

			do
			{
				value_type u1 = eng();
				value_type u2 = eng();
				value_type u3 = eng();

				if (u1 <= v0)
				{
					csi = ::std::pow(u2, value_type(1)/diff);
					eta = u3*::std::pow(csi, diff-value_type(1));
				}
				else
				{
					csi = value_type(1)-::std::log(u2);
					eta = u3*::std::exp(-csi);
				}

			}
			while (eta > (::std::pow(eta, diff-value_type(1))*::std::exp(-csi)));

			r = (csi - r);
*/

			// this is the original code taken from rnd.cc (Anglano, 2005).
			value_type x;
			value_type y;

			do {
				value_type u1 = eng();
				value_type u2 = eng();
				x = ::std::pow(u1, value_type(1)/diff);
				y = ::std::pow(u2, value_type(1)/dist_.scale());
			} while ((x+y)>1);

			x = (x/(x+y));
			y = -::std::log(eng());

			r += x*y;
		}
		return dist_.scale()*r;

/*
		// This uses a function coming from R
		::dcs::math::random::uniform_01_adaptor<UniformRandomGeneratorT&, support_type> eng(rng);
		return detail::rgamma(dist_.shape(), dist_.scale(), eng);
*/
	}

	/**
	 * \brief Generate a vector of random numbers distributed according to this
	 * gamma distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \param n The number of random numbers to generate.
	 * \return A vector of random numbers distributed according to this
	 * gamma distribution.
	 *
	 * A \c gamma random number distribution produces random numbers
	 * \f$x \ge 0\f$ distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|k,\theta) = x^{k-1} \frac{\exp{\left(-x/\theta\right)}}{\Gamma(k)\,\theta^k}
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		::std::vector<support_type> rand(UniformRandomGeneratorT& rng, ::std::size_t n)
	{
		::std::vector<support_type> rnds(n);

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

		return rnds;
	}

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

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

	public: support_type location() const
	{
		return support_type(0);
	}

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

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

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

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

#endif // DCS_MATH_STATS_DISTRIBUTION_GAMMA_HPP


#ifndef MWTRANS_H_
#define MWTRANS_H_

#include <Eigen/Dense>
#include <functional>

class MWtransInt {
public:
  MWtransInt(double lb, double v, double abserr, double referr, int max_depth);

  double perform(std::function<double(double)> functor);

private:
  const int nzero_ = 100;
  const int niter_ = 100;
  const unsigned max_depth_;

  Eigen::ArrayXd zeros_;
  double lb_, ub_, v_;
  const double abserr_;
  const double referr_;
};

#endif


/* This file is part of the Tomographer project, which is distributed under the
 * terms of the MIT license.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 ETH Zurich, Institute for Theoretical Physics, Philippe Faist
 * Copyright (c) 2017 Caltech, Institute for Quantum Information and Matter, Philippe Faist
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <cmath>

#include <string>
#include <sstream>
#include <random>

#include <boost/math/constants/constants.hpp>

// include before <Eigen/*> !
#include "test_tomographer.h"

#include <tomographer/densedm/indepmeasllh.h>
#include <tomographer/tools/eigenutil.h>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

// -----------------------------------------------------------------------------
// fixture(s)

// -----------------------------------------------------------------------------
// test suites

BOOST_AUTO_TEST_SUITE(test_densedm_indeapmeasllh)

BOOST_AUTO_TEST_CASE(basic)
{
  typedef Tomographer::DenseDM::DMTypes<2> DMTypes;
  DMTypes dmt;

  typedef Tomographer::DenseDM::IndepMeasLLH<DMTypes> IndepMeasLLH;
  IndepMeasLLH dat(dmt);

  IndepMeasLLH::VectorParamListType Exn(6, dmt.dim2());
  Exn <<
    0.5, 0.5,  1./std::sqrt(2.0),  0,
    0.5, 0.5, -1./std::sqrt(2.0),  0,
    0.5, 0.5,  0,         1./std::sqrt(2.0),
    0.5, 0.5,  0,        -1./std::sqrt(2.0),
    1,   0,    0,         0,
    0,   1,    0,         0
    ;
  IndepMeasLLH::FreqListType Nx(6);
  Nx << 1500, 800, 300, 300, 10, 30;

  dat.setMeas(Exn, Nx);

  DMTypes::VectorParamType x(dmt.initVectorParamType());
  x << 0.5, 0.5, 0, 0; // maximally mixed state

  DMTypes::RealScalar value = -2*dat.logLikelihoodX(x);

  BOOST_CHECK_CLOSE(value, 4075.70542169248, 1e-4);
  //std::cout << "llh @ mixed state = " << std::setprecision(15) << value << "\n";
}

BOOST_AUTO_TEST_CASE(basic_dyn)
{
  typedef Tomographer::DenseDM::DMTypes<Eigen::Dynamic> DMTypes;
  DMTypes dmt(2);

  typedef Tomographer::DenseDM::IndepMeasLLH<DMTypes> IndepMeasLLH;
  IndepMeasLLH dat(dmt);

  IndepMeasLLH::VectorParamListType Exn(6, dmt.dim2());
  Exn <<
    0.5, 0.5,  1./std::sqrt(2.0),  0,
    0.5, 0.5, -1./std::sqrt(2.0),  0,
    0.5, 0.5,  0,         1./std::sqrt(2.0),
    0.5, 0.5,  0,        -1./std::sqrt(2.0),
    1,   0,    0,         0,
    0,   1,    0,         0
    ;
  IndepMeasLLH::FreqListType Nx(6);
  Nx << 1500, 800, 300, 300, 10, 30;

  dat.setMeas(Exn, Nx);

  DMTypes::VectorParamType x(dmt.initVectorParamType());
  x << 0.5, 0.5, 0, 0; // maximally mixed state

  const DMTypes::VectorParamType xconst(x); // make sure logLikelihoodX() accepts const argument
  DMTypes::RealScalar value = -2*dat.logLikelihoodX(xconst);

  BOOST_CHECK_CLOSE(value, 4075.70542169248, 1e-4);
  //std::cout << "llh @ mixed state = " << std::setprecision(15) << value << "\n";
}

BOOST_AUTO_TEST_CASE(reset_meas)
{
  typedef Tomographer::DenseDM::DMTypes<2> DMTypes;
  DMTypes dmt(2);

  typedef Tomographer::DenseDM::IndepMeasLLH<DMTypes> IndepMeasLLH;
  IndepMeasLLH dat(dmt);

  IndepMeasLLH::VectorParamListType Exn(2,4);
  Exn <<
    1, 0, 0, 0,
    0, 1, 0, 0
    ;
  IndepMeasLLH::FreqListType Nx(2);
  Nx << 50, 50;
  dat.setMeas(Exn, Nx);

  BOOST_CHECK_EQUAL(dat.numEffects(), 2);
  BOOST_CHECK_EQUAL(dat.Exn().rows(), 2);
  { DMTypes::VectorParamType x; x << 0.0,1,0,0; MY_BOOST_CHECK_EIGEN_EQUAL(dat.Exn(1), x, tol); }
  BOOST_CHECK_EQUAL(dat.Nx(0), 50);
  BOOST_CHECK_EQUAL(dat.Nx(1), 50);

  // test resetMeas()
  dat.resetMeas();

  BOOST_CHECK_EQUAL(dat.numEffects(), 0);
  BOOST_CHECK_EQUAL(dat.Exn().rows(), 0);
  BOOST_CHECK_EQUAL(dat.Nx().rows(), 0);
}

BOOST_AUTO_TEST_CASE(add_meas)
{
  typedef Tomographer::DenseDM::DMTypes<2> DMTypes;
  DMTypes dmt(2);
  typedef DMTypes::VectorParamType VectorParamType;
  typedef DMTypes::MatrixType MatrixType;

  typedef Tomographer::DenseDM::IndepMeasLLH<DMTypes> IndepMeasLLH;
  IndepMeasLLH dat(dmt);

  IndepMeasLLH::VectorParamListType Exn(2,4);
  Exn <<
    1, 0, 0, 0,
    0, 1, 0, 0
    ;
  IndepMeasLLH::FreqListType Nx(2);
  Nx << 50, 50;
  dat.setMeas(Exn, Nx);

  // test addMeasEffect(MatrixType)
  dat.addMeasEffect(0.5*MatrixType::Identity(), 75);

  BOOST_CHECK_EQUAL(dat.numEffects(), 3);
  { VectorParamType x; x<<0.5,0.5,0,0; MY_BOOST_CHECK_EIGEN_EQUAL(dat.Exn().row(2), x.transpose(), tol); }
  BOOST_CHECK_EQUAL(dat.Nx().rows(), 3);
  BOOST_CHECK_EQUAL(dat.Nx(2), 75);
  BOOST_CHECK_EQUAL(dat.Nx()(2), 75);

  // test addMeasEffect(VectorParamType)
  dat.addMeasEffect((VectorParamType()<<0.5,0.5,-1./std::sqrt(2),0).finished(), 1175);

  BOOST_CHECK_EQUAL(dat.numEffects(), 4);
  { VectorParamType x; x <<0.5,0.5,-1./std::sqrt(2),0; MY_BOOST_CHECK_EIGEN_EQUAL(dat.Exn().row(3), x.transpose(), tol); }
  BOOST_CHECK_EQUAL(dat.Nx().rows(), 4);
  BOOST_CHECK_EQUAL(dat.Nx(3), 1175);
  BOOST_CHECK_EQUAL(dat.Nx()(3), 1175);

  // and check that the second addMeasEffect() didn't affect the previous data
  { VectorParamType x; x<<0.5,0.5,0,0;  MY_BOOST_CHECK_EIGEN_EQUAL(dat.Exn().row(2), x.transpose(), tol); }
  BOOST_CHECK_EQUAL(dat.Nx(2), 75);
  BOOST_CHECK_EQUAL(dat.Nx()(2), 75);
}

BOOST_AUTO_TEST_CASE(add_meas_checkmeas)
{
  typedef Tomographer::DenseDM::DMTypes<2> DMTypes;
  DMTypes dmt(2);
  //typedef DMTypes::VectorParamType VectorParamType;
  typedef DMTypes::MatrixType MatrixType;

  typedef Tomographer::DenseDM::IndepMeasLLH<DMTypes> IndepMeasLLH;
  IndepMeasLLH dat(dmt);

  IndepMeasLLH::VectorParamListType Exn(2,4);
  Exn <<
    1, 0, 0, 0,
    0, 1, 0, 0
    ;
  IndepMeasLLH::FreqListType Nx(2);
  Nx << 50, 50;
  dat.setMeas(Exn, Nx);

  // test addMeasEffect(MatrixType) --> must raise exception because matrix is not Hermitian
  {
    MatrixType Ebad(dmt.initMatrixType());
    Ebad << 1, 0.5,
            0, 1;
    auto call = [&Ebad,&dat]() { dat.addMeasEffect(Ebad, 500); };
    BOOST_CHECK_THROW(call(), Tomographer::DenseDM::InvalidMeasData);
  }

  // test addMeasEffect(MatrixType) --> must raise exception because matrix is not pos semidef
  {
    MatrixType Ebad(dmt.initMatrixType());
    Ebad << 0, 1e-2,
      1e-2, 1;
    auto call = [&Ebad,&dat]() { dat.addMeasEffect(Ebad, 500); };
    BOOST_CHECK_THROW(call(), Tomographer::DenseDM::InvalidMeasData);
  }

  // test addMeasEffect(MatrixType) --> must raise exception because matrix is zero
  {
    MatrixType Ebad(dmt.initMatrixType());
    Ebad << 0, 1e-12,
      1e-12, 1e-15;
    auto call = [&Ebad,&dat]() { dat.addMeasEffect(Ebad, 500); };
    BOOST_CHECK_THROW(call(), Tomographer::DenseDM::InvalidMeasData);
  }

}

BOOST_AUTO_TEST_SUITE_END()


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license.  See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>

#include <boost/random/uniform_01.hpp>

#include <dpMM/dirMM.hpp>
#include <dpMM/iwTangent.hpp>
#include <dpMM/karcherMean.hpp>
#include <mmf/optimizationSO3_approx.hpp>
#include <mmf/mf.hpp>

/*
 * Prior distribution for a Manhattan Frame. Assumes a uniform prior
 * over the rotation R of the MF
 */
template<typename T>
class MfPrior
{
public:
//  MfPrior(const Dir<Cat<T>, T>& alpha, const
//    std::vector<shared_ptr<IwTangent<T> > >& thetas, uint32_t nIter);
  MfPrior(const DirMM<T>& dirMM, uint32_t nIter);
  MfPrior(const MfPrior<T>& mf);
  ~MfPrior();

//  MfPrior<T>* copy();
  T logPdf(const MF<T>& mf) const;

  MF<T> posteriorSample(const Matrix<T,Dynamic,Dynamic>& x, const
      VectorXu& z, uint32_t k);
//  MF<T> posteriorSample(const vector<Matrix<T,Dynamic,Dynamic> >&x, const
//      VectorXu& z, uint32_t k);
//  // assumes vector [N, sum(x), flatten(sum(outer(x,x)))]
//  MfPrior<T> posteriorFromSS(const Matrix<T,Dynamic,1>& x);
  MF<T> posteriorFromSSsample(const vector<Matrix<T,Dynamic,1> >&x,
      const VectorXu& z, uint32_t k);

//  MfPrior<T> posterior() const;
  MF<T> sample() const;

  Matrix<T,3,3> R() const {return optSO3_.R();};

  Matrix<T,Dynamic,Dynamic> M() const {return optSO3_.M();};

  const DirMM<T>& dirMM() const {return dirMM_;};
  IwTangent<T>* theta(uint32_t k)
  {return reinterpret_cast<IwTangent<T>* >(dirMM_.getTheta(k).get());};
  const IwTangent<T>* theta(uint32_t k) const
  {
//  { cout<<k<<" "<<dirMM_.getK()<<endl;
//    cout<<(dirMM_.getTheta(k).get())<<endl;
    return reinterpret_cast<IwTangent<T>* >(dirMM_.getTheta(k).get());};
//  vector<shared_ptr<IwTangent<T> > > thetasDeepCopy() const
//  {
//    vector<shared_ptr<IwTangent<T> > > thetas;
//    for(uint32_t k=0; k<6; ++k)
//      thetas.push_back(shared_ptr<IwTangent<T> >(
//            thetas_[k]->copyNative()));
//    return thetas;
//  };

  // how many iterations to sample internally for the MF posterior
  uint32_t T_;
  Matrix<T,3,3> R_;
private:
//  // data for this MF needed since we iterate over this data for
//  // posterior inference
//  Matrix<T,Dynamic,Dynamic> x_;

//  std::vector<shared_ptr<IwTangent<T> > > thetas_;
  // prior over the mixture over the six axes
  DirMM<T> dirMM_;

  OptSO3ApproxCpu<T> optSO3_;

  void sample_(const Matrix<T,Dynamic,Dynamic>& x, uint32_t nIter);
  void sample_(const Matrix<T,Dynamic,Dynamic>& x,
    vector<Matrix<T,Dynamic,1> >& SS, uint32_t nIter);
};

// ----------------------------------------
//template<typename T>
//MfPrior<T>::MfPrior(const Dir<Cat<T>, T>& alpha, const
//    std::vector<shared_ptr<IwTangent<T> > >& thetas, uint32_t nIter)
//  : T_(nIter),
////    thetas_(thetas),
//    dirMM_(alpha,thetas_[0],6), optSO3_(5.0,0.05)
//{
//    R_ = Matrix<T,3,3>::Identity();
//};

template<typename T>
MfPrior<T>::MfPrior(const DirMM<T>& dirMM, uint32_t nIter)
  : T_(nIter),
    dirMM_(dirMM), optSO3_(5.0,0.05)
{
    R_ = Matrix<T,3,3>::Identity();
};

template<typename T>
MfPrior<T>::MfPrior(const MfPrior<T>& mfP)
  : T_(mfP.T_),
  R_(mfP.R_),
//  thetas_(mfP.thetasDeepCopy()),
  dirMM_(mfP.dirMM()),
  optSO3_(5.0,0.05)
{};

template<typename T>
MfPrior<T>::~MfPrior()
{};

template<typename T>
void MfPrior<T>::sample_(const Matrix<T,Dynamic,Dynamic>& x,
    uint32_t nIter)
{
  // get the pointers to the thetas in the DirMM so we can set the
  // means
  for(uint32_t k=0; k<6; ++k)
  {
//    if(thetas_[k].get() != dirMM_.getTheta(k).get())
//    {
//      thetas_[k].reset(reinterpret_cast<IwTangent<T>* >(
//            dirMM_.getTheta(k).get()));
//    }
    this->theta(k)->setMean(optSO3_.M().col(k));
  }
  for(uint32_t t=0; t<nIter; ++t)
  {
//    cout<<" -------------------- "<<endl;
    // sample labels - assignments to MF axes
    dirMM_.sampleLabels();
    // compute karcher means
    Matrix<T,Dynamic,Dynamic> xTpS(x.rows(),x.cols());
    Matrix<T,Dynamic, Dynamic> qKarch = karcherMeanMultiple<T>(
        optSO3_.M(),x,xTpS,dirMM_.labels(),6,30);
//    cout<<"qKarch: "<<endl<<qKarch<<endl;
    // sample new MF rotation
    optSO3_.conjugateGradient(R_, qKarch, dirMM_.getCounts(), 100);
    R_ = optSO3_.R();
////    cout<<"R: "<<endl<<R_<<endl;
//    cout<<"M: "<<endl<<optSO3_.M()<<endl;
    // set tangent points of the iwTangent
    for(uint32_t k=0; k<6; ++k)
    {
      this->theta(k)->setMean(optSO3_.M().col(k));
//      dirMM_.getTheta(k)->print();
    }
    // sample covariances in the tangent spaces
    dirMM_.sampleParameters();
    // some output
//    cout<<"@t "<<t<<": logJoint = "<<dirMM_.logJoint()
//      <<" #s "<<dirMM_.getCounts().transpose()
//      <<endl;
  }
}

template<typename T>
void MfPrior<T>::sample_(const Matrix<T,Dynamic,Dynamic>& x,
    vector<Matrix<T,Dynamic,1> >& SS,
    uint32_t nIter)
{
  // TODO: the dirMM sampler ignores the SS (i.e. the number of data as
  // well as the scatter of the data in their respective tangent
  // space!)

  // get the pointers to the thetas in the DirMM so we can set the
  // means
  for(uint32_t k=0; k<6; ++k)
  {
    this->theta(k)->setMean(optSO3_.M().col(k));
  }

  Matrix<T,Dynamic,1> w(x.cols());
  for(uint32_t i=0; i<SS.size(); ++i)
    w(i) = SS[i](0);

  for(uint32_t t=0; t<nIter; ++t)
  {
//    cout<<" -------------------- "<<endl;
    // sample labels - assignments to MF axes
    dirMM_.sampleLabels();
    // compute karcher means
    Matrix<T,Dynamic,Dynamic> xTpS(x.rows(),x.cols());
    Matrix<T,Dynamic, Dynamic> qKarch = karcherMeanMultipleWeighted<T>(
        optSO3_.M(),x,xTpS,w,dirMM_.labels(),6,30);
//    cout<<"qKarch: "<<endl<<qKarch<<endl;
//    cout<<"Mbefore: "<<endl<<optSO3_.M()<<endl;
//    cout<<"R: "<<endl<<R_<<endl;
    // compute counts for each of the 6 directions from SS
    Matrix<T,Dynamic,1> Ws = Matrix<T,Dynamic,1>::Zero(6);
    for(uint32_t i=0; i<SS.size(); ++i)
      Ws(dirMM_.labels()(i)) += w(i);
//    cout<<"Ws: "<<Ws.transpose()<<endl;
//    cout<<"#s: "<<dirMM_.getCounts().transpose()<<endl;
    // sample new MF rotation
    optSO3_.conjugateGradient(R_, qKarch, Ws, 100);
    R_ = optSO3_.R();
////    cout<<"R: "<<endl<<R_<<endl;
//    cout<<"M: "<<endl<<optSO3_.M()<<endl;
    // set tangent points of the iwTangent
    for(uint32_t k=0; k<6; ++k)
    {
      this->theta(k)->setMean(optSO3_.M().col(k));
//      dirMM_.getTheta(k)->print();
    }
    // sample covariances in the tangent spaces
    dirMM_.sampleParameters();
    // some output
//    cout<<"@t "<<t<<": logJoint = "<<dirMM_.logJoint()
//      <<" #s "<<dirMM_.getCounts().transpose()
//      <<endl;
  }
}

template<typename T>
MF<T> MfPrior<T>::posteriorSample(const Matrix<T,Dynamic,Dynamic>& x,
    const VectorXu& z, uint32_t k)
{
  // count to know how big to make the data matrix
  uint32_t Nk = 0;
//#pragma omp parallel for
  for (int i=0; i<z.size(); ++i)
    if(z[i] == k) ++Nk;
  if(Nk > 0)
  {
    // fill data matrix
    Matrix<T,Dynamic,Dynamic> x_(x.rows(),Nk);
    uint32_t j=0;
    for (int i=0; i<z.size(); ++i)
      if(z[i] == k)
      {
        x_.col(j) = x.col(i);
        ++j;
      }
    dirMM_.initialize(x_);
    sample_(x_,T_);
  }else{
    // sample from prior
    dirMM_.reset();
    dirMM_.sampleParameters();
//    dirMM_.sampleFromPrior();
  }
  std::vector<NormalSphere<T> > TGs;
  for(uint32_t k=0; k<6; ++k)
  {
    TGs.push_back(this->theta(k)->normalS_);
  }
  return MF<T>(optSO3_.R(),dirMM_.Pi(),TGs);
}

template<typename T>
MF<T> MfPrior<T>::posteriorFromSSsample(const
    vector<Matrix<T,Dynamic,1> >&x, const VectorXu& z, uint32_t k)
{
  // count to know how big to make the data matrix
  uint32_t Nk = 0;
//#pragma omp parallel for
  for (int i=0; i<z.size(); ++i)
    if(z[i] == k) ++Nk;
  if(Nk > 0)
  {
    // fill data matrix
    vector<Matrix<T,Dynamic,1> > SS(Nk); // SS for this cluster
    Matrix<T,Dynamic,Dynamic> x_(3,Nk);  // karcher means of this cl
    uint32_t j=0;
    for (int i=0; i<z.size(); ++i)
      if(z[i] == k)
      {
        SS[j] = x[i];
        x_.col(j) = x[i].middleRows(1,3);
        ++j;
      }
    dirMM_.initialize(x_);
    sample_(x_,SS,T_);
  }else{
    // sample from prior
    dirMM_.reset();
    dirMM_.sampleParameters();
//    dirMM_.sampleFromPrior();
  }
  std::vector<NormalSphere<T> > TGs;
  for(uint32_t k=0; k<6; ++k)
  {
    TGs.push_back(this->theta(k)->normalS_);
  }
  return MF<T>(optSO3_.R(),dirMM_.Pi(),TGs);
}

template<typename T>
T MfPrior<T>::logPdf(const MF<T>& mf) const
{
  //TODO hmmm this assumes R_ = mf.R() have to think more about what
  //happens if the rotations are different
  T logPdf = 0.;
  for(uint32_t k =0; k<6; ++k)
  {
    logPdf += this->theta(k)->iw0_.logPdf(mf.Sigma(k));
  }
  return logPdf;
};

template<typename T>
MF<T> MfPrior<T>::sample() const
{
  //TODO sample from all SO3
  Matrix<T,3,3> R = Matrix<T,3,3>::Identity();

  boost::uniform_01<T> unif;
//  cout<<this->theta(0)<<endl;
//  cout<<this->theta(0)->iw0_.pRndGen_<<endl;
  double theta = unif(*(this->theta(0)->iw0_.pRndGen_));
  theta *= 2.*M_PI;
  R << cos(theta), -sin(theta), 0.0,
    sin(theta), cos(theta), 0.0,
    0.0,0.0,1.0;

  Cat<T> pi = const_cast<Dir<Cat<T>,T>* >(&(dirMM_.Alpha()))->sample();
  std::vector<NormalSphere<T> > TGs;
  IwTangent<T> iwT(*this->theta(0));
  for(uint32_t k =0; k<6; ++k)
  {
    //TODO fishy
    TGs.push_back(NormalSphere<T>(
          OptSO3ApproxCpu<T>::Rot2M(R).col(k),
          iwT.iw0_.sample(),
          iwT.iw0_.pRndGen_
          ));
  }
  return MF<T>(R_,pi,TGs);
};

//template<typename T>
//MfPrior<T>::
//
//template<typename T>
//MfPrior<T>::
//


/**
 * @file network_util_test.cpp
 * @author Marcus edel
 *
 * Simple tests for things in the network_util file.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/network_util.hpp>
#include <mlpack/methods/ann/layer/linear_layer.hpp>
#include <mlpack/methods/ann/layer/base_layer.hpp>

#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(NetworkUtilTest);

/**
 * Test the network size auxiliary function.
 */
BOOST_AUTO_TEST_CASE(NetworkSizeTest)
{
  // // Create a two layer network without weights.
  // BaseLayer<> baseLayer1;
  // BaseLayer<> baseLayer2;
  // auto noneWeightNetwork = std::tie(baseLayer1, baseLayer2);

  // BOOST_REQUIRE_EQUAL(NetworkSize(noneWeightNetwork), 0);

  // // Create a two layer network.
  // LinearLayer<> linearLayer1(10, 10);
  // LinearLayer<> linearLayer2(10, 100);

  // // Reuse the layer form the first network.
  // auto weightNetwork = std::tie(linearLayer1, baseLayer1, linearLayer2,
  //     baseLayer2);

  // BOOST_REQUIRE_EQUAL(NetworkSize(weightNetwork), 1100);
}

/**
 * Test the layer size auxiliary function.
 */
BOOST_AUTO_TEST_CASE(LayerSizeTest)
{
  // // Create layer without weights.
  // BaseLayer<> baseLayer;
  // BOOST_REQUIRE_EQUAL(LayerSize(baseLayer, baseLayer.OutputParameter()), 0);

  // LinearLayer<> linearLayer(10, 10);
  // BOOST_REQUIRE_EQUAL(LayerSize(linearLayer,
  //     linearLayer.OutputParameter()), 100);

}

BOOST_AUTO_TEST_SUITE_END();


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>

#include <dpMM/basemeasure.hpp>
#include <dpMM/dir.hpp>

/*
 * NIW base measure; samples Normal distribution parameters
 * templated on a discrete distribution (Cat<T> or Mult<T>)
 */
template<class Disc, typename T>
class DirSampled : public BaseMeasure<T>
{
public:
  Dir<Disc,T> dir0_;
  Disc disc_; // discrete distribution - Cat<T> or Mult<T>

  T count_; // number of datapoints associated with this

  DirSampled(const Dir<Disc,T>& dir);
  ~DirSampled();

  virtual baseMeasureType getBaseMeasureType() const {return(DIR_SAMPLED); }

  virtual BaseMeasure<T>* copy();
  virtual DirSampled<Disc,T>* copyNative();

  T logLikelihood(const Matrix<T,Dynamic,1>& x) const;
  T logLikelihood(const Matrix<T,Dynamic,Dynamic>& x, uint32_t i) const
    {return logLikelihood(x.col(i));};
  // assumes vector [counts]
  T logLikelihoodFromSS(const Matrix<T,Dynamic,1>& x) const;
  void posterior(const Matrix<T,Dynamic,Dynamic>& x, const VectorXu& z,
    uint32_t k);
  // assumes vectors [counts]
  virtual void posteriorFromSS(const vector<Matrix<T,Dynamic,1> >&x, const
      VectorXu& z, uint32_t k);
  virtual void posteriorFromSS(const Matrix<T,Dynamic,1> &x);

  void sample();

  T logPdfUnderPrior() const;
  T logPdfUnderPriorMarginalized() const;
  T logPdfUnderPriorMarginalizedMerged(const boost::shared_ptr<DirSampled<Disc,T> >& other) const;

  virtual DirSampled<Disc,T>* merge(const DirSampled<Disc,T>& other);
  void fromMerge(const DirSampled<Disc,T>& dirA, const DirSampled<Disc,T>& dirB);

  void print() const;
  virtual uint32_t getDim() const {return(uint32_t(dir0_.alpha_.size()));};

//  const Matrix<T,Dynamic,Dynamic>& scatter() const {return dir0_.scatter();};
//  const Matrix<T,Dynamic,1>& mean() const {return dir0_.mean();};
  T count() const {return count_;};
  const Matrix<T,Dynamic,1>& counts() const {return dir0_.counts();};
  const Matrix<T,Dynamic,1>& pdf() const {return disc_.pdf();};
//  T& count() {return dir0_.count_;};
//  const Matrix<T,Dynamic,1>& getMean() const {return disc_.mu_;};
//
//  const Matrix<T,Dynamic,Dynamic>& Sigma() const {return disc_.Sigma();};
private:

};

typedef DirSampled<Cat<double>, double> DirCatSampledd;
typedef DirSampled<Cat<float>, float> DirCatSampledf;
typedef DirSampled<Mult<double>, double> DirMultSampledd;
typedef DirSampled<Mult<float>, float> DirMultSampledf;

// ---------------------------------------------------------------------------

template<class Disc, typename T>
DirSampled<Disc,T>::DirSampled(const Dir<Disc,T>& dir)
  : dir0_(dir), disc_(dir0_.sample()), count_(0)
{};

template<class Disc, typename T>
DirSampled<Disc,T>::~DirSampled()
{};

template<class Disc, typename T>
BaseMeasure<T>* DirSampled<Disc,T>::copy()
{
  DirSampled<Disc,T>* dirSampled = new DirSampled<Disc,T>(dir0_);
  dirSampled->disc_ = disc_;
  return dirSampled;
};

template<class Disc, typename T>
DirSampled<Disc,T>* DirSampled<Disc,T>::copyNative()
{
  DirSampled<Disc,T>* dirSampled = new DirSampled<Disc,T>(dir0_);
  dirSampled->disc_ = disc_;
  return dirSampled;
};

template<class Disc, typename T>
T DirSampled<Disc,T>::logLikelihood(const Matrix<T,Dynamic,1>& x) const
{
//  disc_.print();
  T logLike = disc_.logPdf(x);
//  cout<<x.transpose()<<" -> " <<logLike<<endl;
//  cout<<x.transpose()<<" -> " <<disc_.logPdfSlower(x)<<endl;
  return logLike;
};

template<class Disc, typename T>
T DirSampled<Disc,T>::logLikelihoodFromSS(const Matrix<T,Dynamic,1>& x) const
{
//  disc_.print();
  T logLike = disc_.logPdfOfSS(x);
//  cout<<x.transpose()<<" -> " <<logLike<<endl;
//  cout<<x.transpose()<<" -> " <<disc_.logPdfSlower(x)<<endl;
  return logLike;
};

template<class Disc, typename T>
void DirSampled<Disc,T>::posterior(const Matrix<T,Dynamic,Dynamic>& x,
    const VectorXu& z, uint32_t k)
{
  count_ = 0;
  for(uint32_t i=0; i<z.size(); ++i) if(z(i)==k) ++count_;
  disc_ = dir0_.posterior(x,z,k).sample();
};

template<class Disc, typename T>
void DirSampled<Disc,T>::posteriorFromSS(const vector<Matrix<T,Dynamic,1> > & x, const VectorXu& z, uint32_t k)
{
  disc_ = dir0_.posteriorFromCounts(x,z,k).sample();
};

template<class Disc, typename T>
void DirSampled<Disc,T>::posteriorFromSS(const Matrix<T,Dynamic,1> &x)
{
	disc_ = dir0_.posteriorFromCounts(x).sample();
}

template<class Disc, typename T>
T DirSampled<Disc,T>::logPdfUnderPrior() const
{
  return dir0_.logPdf(disc_);
};

template<class Disc, typename T>
T DirSampled<Disc,T>::logPdfUnderPriorMarginalized() const
{
  // evaluates log pdf of sufficient statistics stored within dir0_
//  dir0_.print();
  return dir0_.logPdfMarginalized();
};

template<class Disc, typename T>
T DirSampled<Disc,T>::logPdfUnderPriorMarginalizedMerged(
    const boost::shared_ptr<DirSampled<Disc,T> >& other) const
{
  return dir0_.logPdfUnderPriorMarginalizedMerged(other->dir0_);
};

template<class Disc, typename T>
void DirSampled<Disc,T>::fromMerge(const DirSampled<Disc,T>& dirA,
  const DirSampled<Disc,T>& dirB)
{
  count_ = dirA.count() + dirB.count();
  dir0_.fromMerge(dirA.dir0_,dirB.dir0_);
  disc_ = dir0_.posterior().sample();
};

template<class Disc, typename T>
DirSampled<Disc,T>* DirSampled<Disc,T>::merge(const DirSampled<Disc,T>& other)
{
  DirSampled<Disc,T>* newNiw = this->copyNative();
  newNiw->dir0_.fromMerge(dir0_,other.dir0_);
  newNiw->sample();
  return newNiw;
};

template<class Disc, typename T>
void DirSampled<Disc,T>::sample()
{
  disc_ = dir0_.posterior().sample();
};

template<class Disc, typename T>
void DirSampled<Disc,T>::print() const
{
  dir0_.posterior().print();
  disc_.print();
};


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#define BOOST_TEST_MODULE icl::interval_map_mixed unit test
#include <libs/icl/test/disable_test_warnings.hpp>
#include <string>
#include <boost/mpl/list.hpp>
#include "../unit_test_unwarned.hpp"

// interval instance types
#include "../test_type_lists.hpp"
#include "../test_value_maker.hpp"

#include <boost/icl/interval_set.hpp>
#include <boost/icl/separate_interval_set.hpp>
#include <boost/icl/split_interval_set.hpp>
#include <boost/icl/interval_map.hpp>
#include <boost/icl/split_interval_map.hpp>

using namespace std;
using namespace boost;
using namespace unit_test;
using namespace boost::icl;

#include "../test_interval_map_mixed.hpp"

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_add_4_bicremental_types)
{            interval_map_mixed_add_4_bicremental_types<bicremental_type_1, int>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_add2_4_bicremental_types)
{            interval_map_mixed_add2_4_bicremental_types<bicremental_type_2, int>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_subtract_4_bicremental_types)
{            interval_map_mixed_subtract_4_bicremental_types<bicremental_type_3, int>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_erase_4_bicremental_types)
{            interval_map_mixed_erase_4_bicremental_types<bicremental_type_4, int>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_erase2_4_bicremental_types)
{            interval_map_mixed_erase2_4_bicremental_types<bicremental_type_5, int>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_insert_erase_4_bicremental_types)
{            interval_map_mixed_insert_erase_4_bicremental_types<bicremental_type_6, int>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_insert_erase2_4_bicremental_types)
{            interval_map_mixed_insert_erase2_4_bicremental_types<bicremental_type_7, int>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_basic_intersect_4_bicremental_types)
{            interval_map_mixed_basic_intersect_4_bicremental_types<bicremental_type_8, int>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_basic_intersect2_4_bicremental_types)
{            interval_map_mixed_basic_intersect2_4_bicremental_types<bicremental_type_1, int>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_intersect_4_bicremental_types)
{            interval_map_mixed_intersect_4_bicremental_types<bicremental_type_2, int>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_intersect2_4_bicremental_types)
{            interval_map_mixed_intersect2_4_bicremental_types<bicremental_type_3, int>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_disjoint_4_bicremental_types)
{            interval_map_mixed_disjoint_4_bicremental_types<bicremental_type_4, int>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_erase_if_4_integral_types)
{            interval_map_mixed_erase_if_4_integral_types<int, int>(); }


/* test_uniform_on_sphere_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/uniform_on_sphere.hpp>
#include <boost/assign/list_of.hpp>

#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::uniform_on_sphere<>
#define BOOST_RANDOM_ARG1 dim
#define BOOST_RANDOM_ARG1_DEFAULT 2
#define BOOST_RANDOM_ARG1_VALUE 3

std::vector<double> min0 = boost::assign::list_of(-1.0)(0.0);
std::vector<double> max0 = boost::assign::list_of(1.0)(0.0);
std::vector<double> min1 = boost::assign::list_of(-1.0)(0.0)(0.0);
std::vector<double> max1 = boost::assign::list_of(1.0)(0.0)(0.0);

#define BOOST_RANDOM_DIST0_MIN min0
#define BOOST_RANDOM_DIST0_MAX max0
#define BOOST_RANDOM_DIST1_MIN min1
#define BOOST_RANDOM_DIST1_MAX max1

#define BOOST_RANDOM_TEST1_PARAMS (0)
#define BOOST_RANDOM_TEST1_MIN std::vector<double>()
#define BOOST_RANDOM_TEST1_MAX std::vector<double>()
#define BOOST_RANDOM_TEST2_PARAMS
#define BOOST_RANDOM_TEST2_MIN min0
#define BOOST_RANDOM_TEST2_MAX max0

#include <boost/test/test_tools.hpp>

BOOST_TEST_DONT_PRINT_LOG_VALUE( std::vector<double> )

#include "test_distribution.ipp"

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

struct generate_zeros {
public:
    generate_zeros() : i(0) {}
    typedef unsigned result_type;
    static unsigned (min)() { return 0u; }
    static unsigned (max)() { return boost::random::minstd_rand0::max(); }
    unsigned operator()() {
        static unsigned data[] = { 0, 0, 0, 0, 0, 0 };
        if(i < 6) {
            return data[i++];
        } else {
            return gen();
        }
    }
private:
    int i;
    boost::random::minstd_rand0 gen;
};

BOOST_AUTO_TEST_CASE(test_zeros) {
    generate_zeros gen;
    boost::random::uniform_on_sphere<> dist(2);
    std::vector<double> val = dist(gen);
    BOOST_CHECK(!(boost::math::isnan)(val[0]));
}


/*!
@file
Defines `boost::hana::detail::std::integer_sequence`.

@copyright Louis Dionne 2015
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_HANA_DETAIL_STD_INTEGER_SEQUENCE_HPP
#define BOOST_HANA_DETAIL_STD_INTEGER_SEQUENCE_HPP

#include <boost/hana/detail/std/size_t.hpp>

namespace boost { namespace hana { namespace detail { namespace std {
    template <typename T, T ...n>
    struct integer_sequence { };

    template <size_t ...n>
    using index_sequence = integer_sequence<size_t, n...>;

    namespace int_seq_detail {
        template <typename L, typename R>
        struct concat;

        template <typename T, T ...m, T ...n>
        struct concat<integer_sequence<T, m...>, integer_sequence<T, n...>> {
            using type = integer_sequence<T, m..., (sizeof...(m) + n)...>;
        };

        template <size_t n>
        struct make_index_sequence {
            using type = typename concat<
                typename make_index_sequence<n / 2>::type,
                typename make_index_sequence<n - n / 2>::type
            >::type;
        };

        template <>
        struct make_index_sequence<0> {
            using type = index_sequence<>;
        };

        template <>
        struct make_index_sequence<1> {
            using type = index_sequence<0>;
        };

        template <typename T, typename U>
        struct cast_to;

        template <typename T, typename U, U ...u>
        struct cast_to<T, integer_sequence<U, u...>> {
            using type = integer_sequence<T, static_cast<T>(u)...>;
        };
    } // end namespace int_seq_detail

    template <typename T, T n>
    using make_integer_sequence = typename int_seq_detail::cast_to<T,
        typename int_seq_detail::make_index_sequence<
            static_cast<size_t>(n)
        >::type
    >::type;

    template <size_t n>
    using make_index_sequence =
        typename int_seq_detail::make_index_sequence<n>::type;

    template <typename ...T>
    using index_sequence_for = make_index_sequence<sizeof...(T)>;
}}}} // end namespace boost::hana::detail::std

#endif // !BOOST_HANA_DETAIL_STD_INTEGER_SEQUENCE_HPP


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     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.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE symmetric_matrix_test

// Standard includes
#include <iostream>

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/xtp/symmetric_matrix.h"

using namespace votca::xtp;
using namespace votca;

BOOST_AUTO_TEST_SUITE(symmetric_matrix_test)

BOOST_AUTO_TEST_CASE(Constructor_test) {
  Index dim = 3;
  Eigen::MatrixXd test = Eigen::MatrixXd::Random(dim, dim);
  Eigen::MatrixXd trans = test.transpose();
  test += trans;
  Symmetric_Matrix sym = Symmetric_Matrix(test);

  Eigen::MatrixXd back = Eigen::MatrixXd::Zero(dim, dim);

  sym.AddtoEigenMatrix(back);

  bool check_matrices = back.isApprox(test, 0.000001);
  if (!check_matrices) {
    std::cout << sym << std::endl;
    std::cout << test << std::endl;
    std::cout << back << std::endl;
  }
  BOOST_CHECK_EQUAL(check_matrices, 1);
}

BOOST_AUTO_TEST_CASE(Add_test) {

  Index dim = 3;
  Eigen::MatrixXd test = Eigen::MatrixXd::Random(dim, dim);
  Eigen::MatrixXd trans = test.transpose();
  test += trans;
  Symmetric_Matrix sym = Symmetric_Matrix(test);

  Eigen::MatrixXd rand = Eigen::MatrixXd::Random(dim, dim);

  Eigen::MatrixXd result = rand + 2 * test;

  sym.AddtoEigenMatrix(rand, 2.0);

  bool check_matrices2 = rand.isApprox(result, 0.000001);
  if (!check_matrices2) {
    std::cout << test << std::endl;
    std::cout << sym << std::endl;
    std::cout << rand << std::endl;
    std::cout << result << std::endl;
  }
  BOOST_CHECK_EQUAL(check_matrices2, 1);
}

BOOST_AUTO_TEST_CASE(AddUpper_test) {

  Index dim = 3;
  Eigen::MatrixXd test = Eigen::MatrixXd::Random(dim, dim);
  Eigen::MatrixXd trans = test.transpose();
  test += trans;
  Symmetric_Matrix sym = Symmetric_Matrix(test);

  Eigen::MatrixXd rand = Eigen::MatrixXd::Random(dim, dim);

  Eigen::MatrixXd result = rand + 2 * test;
  Eigen::SelfAdjointView<Eigen::MatrixXd, Eigen::Upper> m =
      rand.selfadjointView<Eigen::Upper>();
  sym.AddtoEigenUpperMatrix(m, 2.0);

  result.triangularView<Eigen::StrictlyLower>() =
      Eigen::MatrixXd::Zero(dim, dim);
  rand.triangularView<Eigen::StrictlyLower>() = Eigen::MatrixXd::Zero(dim, dim);

  bool check_matrices2 = rand.isApprox(result, 0.000001);
  if (!check_matrices2) {
    std::cout << "ref" << std::endl;
    std::cout << rand << std::endl;
    std::cout << "sym matrix full" << std::endl;
    std::cout << result << std::endl;
  }
  BOOST_CHECK_EQUAL(check_matrices2, 1);
}

BOOST_AUTO_TEST_CASE(FullMatrix_test) {

  Index dim = 3;
  Eigen::MatrixXd test = Eigen::MatrixXd::Random(dim, dim);
  Eigen::MatrixXd trans = test.transpose();
  test += trans;
  Symmetric_Matrix sym = Symmetric_Matrix(test);
  Eigen::MatrixXd result = sym.FullMatrix();
  bool check_matrices = test.isApprox(result, 0.000001);

  BOOST_CHECK_EQUAL(check_matrices, 1);
}

BOOST_AUTO_TEST_CASE(UpperMatrix_test) {

  Index dim = 3;
  Eigen::MatrixXd test = Eigen::MatrixXd::Random(dim, dim);
  Eigen::MatrixXd trans = test.transpose();
  test += trans;

  Symmetric_Matrix sym = Symmetric_Matrix(test);
  Eigen::MatrixXd result = sym.UpperMatrix();

  result.triangularView<Eigen::StrictlyLower>() =
      Eigen::MatrixXd::Zero(dim, dim);
  test.triangularView<Eigen::StrictlyLower>() = Eigen::MatrixXd::Zero(dim, dim);

  bool check_matrices = test.isApprox(result, 0.000001);

  BOOST_CHECK_EQUAL(check_matrices, 1);
}

BOOST_AUTO_TEST_CASE(TraceofProd_test) {

  Index dim = 3;
  Eigen::MatrixXd test = Eigen::MatrixXd::Random(dim, dim);
  Eigen::MatrixXd trans = test.transpose();
  test += trans;
  Symmetric_Matrix sym1 = Symmetric_Matrix(test);

  Eigen::MatrixXd test2 = Eigen::MatrixXd::Random(dim, dim);
  Eigen::MatrixXd trans2 = test2.transpose();
  test2 += trans2;
  Symmetric_Matrix sym2 = Symmetric_Matrix(test2);

  double ref = test2.cwiseProduct(test).sum();
  double result = sym1.TraceofProd(sym2);

  bool check = std::abs(ref - result) < 1e-7;
  if (!check) {
    std::cout << "Ref: " << ref << " Sym: " << result << std::endl;
  }

  BOOST_CHECK_EQUAL(check, 1);
}

BOOST_AUTO_TEST_SUITE_END()