// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#include "rttbBoostMaskVoxelizationThread.h"

#include "rttbInvalidParameterException.h"

#include <boost/geometry.hpp>
#include <boost/make_shared.hpp>

namespace rttb
{
	namespace masks
	{
		namespace boost
		{
			BoostMaskVoxelizationThread::BoostMaskVoxelizationThread(const BoostPolygonMap& APolygonMap,
                const VoxelIndexVector& aGlobalBoundingBox, BoostArrayMapPointer anArrayMap, ::boost::shared_ptr<std::mutex> aMutex, bool strict) : _geometryCoordinateBoostPolygonMap(APolygonMap),
                _globalBoundingBox(aGlobalBoundingBox), _resultVoxelization(anArrayMap), _mutex(aMutex), _strict(strict)
			{
			}

			void BoostMaskVoxelizationThread::operator()()
			{
				rttb::VoxelGridIndex3D minIndex = _globalBoundingBox.at(0);
				rttb::VoxelGridIndex3D maxIndex = _globalBoundingBox.at(1);
				const unsigned int globalBoundingBoxSize0 = maxIndex[0] - minIndex[0] + 1;
				const unsigned int globalBoundingBoxSize1 = maxIndex[1] - minIndex[1] + 1;

                std::map<double, ::boost::shared_ptr<BoostArray2D> > voxelizationMapInThread;

                 for (auto & it : _geometryCoordinateBoostPolygonMap)
				{
                    BoostArray2D maskArray(::boost::extents[globalBoundingBoxSize0][globalBoundingBoxSize1]);

					BoostPolygonVector boostPolygonVec = it.second;

					for (unsigned int x = 0; x < globalBoundingBoxSize0; ++x)
					{
						for (unsigned int y = 0; y < globalBoundingBoxSize1; ++y)
						{
							rttb::VoxelGridIndex3D currentIndex;
							currentIndex[0] = x + minIndex[0];
							currentIndex[1] = y + minIndex[1];
							currentIndex[2] = 0;

							//Get intersection polygons of the dose voxel and the structure
							BoostPolygonDeque polygons = getIntersections(currentIndex, boostPolygonVec);

							//Calc areas of all intersection polygons
							double volumeFraction = calcArea(polygons);
                            volumeFraction = correctForErrorAndStrictness(volumeFraction, _strict);
                            if (volumeFraction < 0 || volumeFraction > 1 )
                            {
                                throw rttb::core::InvalidParameterException("Mask calculation failed! The volume fraction should >= 0 and <= 1!");
                            }

							maskArray[x][y] = volumeFraction;
						}
					}

                    voxelizationMapInThread.insert(std::pair<double, BoostArray2DPointer>(it.first, ::boost::make_shared<BoostArray2D>(maskArray)));
				}
                //insert gathered values into voxelization map
                std::unique_lock<std::mutex> lock(*_mutex);
                _resultVoxelization->insert(voxelizationMapInThread.begin(), voxelizationMapInThread.end());

			}

			/*Get intersection polygons of the contour and a voxel polygon*/
			BoostMaskVoxelizationThread::BoostPolygonDeque BoostMaskVoxelizationThread::getIntersections(
			    const rttb::VoxelGridIndex3D&
			    aVoxelIndex3D, const BoostPolygonVector& intersectionSlicePolygons)
			{
				BoostMaskVoxelizationThread::BoostPolygonDeque polygonDeque;

				BoostRing2D voxelPolygon = get2DContour(aVoxelIndex3D);
				::boost::geometry::correct(voxelPolygon);

				BoostPolygonVector::const_iterator it;

				for (it = intersectionSlicePolygons.begin(); it != intersectionSlicePolygons.end(); ++it)
				{
					BoostPolygon2D contour = *it;
					::boost::geometry::correct(contour);

					BoostPolygonDeque intersection;
					::boost::geometry::intersection(voxelPolygon, contour, intersection);
					polygonDeque.insert(polygonDeque.end(), intersection.begin(), intersection.end());
				}

				return polygonDeque;
			}

			BoostMaskVoxelizationThread::BoostRing2D BoostMaskVoxelizationThread::get2DContour(
			    const rttb::VoxelGridIndex3D& aVoxelGrid3D)
			{
				BoostRing2D polygon;

				BoostPoint2D point1(aVoxelGrid3D[0] - 0.5, aVoxelGrid3D[1] - 0.5);
				::boost::geometry::append(polygon, point1);

				BoostPoint2D point2(aVoxelGrid3D[0] + 0.5, aVoxelGrid3D[1] - 0.5);
				::boost::geometry::append(polygon, point2);

				BoostPoint2D point3(aVoxelGrid3D[0] + 0.5, aVoxelGrid3D[1] + 0.5);
				::boost::geometry::append(polygon, point3);

				BoostPoint2D point4(aVoxelGrid3D[0] - 0.5, aVoxelGrid3D[1] + 0.5);
				::boost::geometry::append(polygon, point4);

				::boost::geometry::append(polygon, point1);

				return polygon;

			}

			/*Calculate the intersection area*/
			double BoostMaskVoxelizationThread::calcArea(const BoostPolygonDeque& aPolygonDeque)
			{
				double area = 0;

				BoostPolygonDeque::const_iterator it;

				for (it = aPolygonDeque.begin(); it != aPolygonDeque.end(); ++it)
				{
					area += ::boost::geometry::area(*it);
				}

				return area;
			}

            double BoostMaskVoxelizationThread::correctForErrorAndStrictness(double volumeFraction, bool strict) const
            {
                if (strict){
                    if (volumeFraction > 1 && (volumeFraction - 1) <= errorConstant)
                    {
                        volumeFraction = 1;
                    }
                }
                else {
                    if (volumeFraction > 1){
                        volumeFraction = 1;
                    }
                    else if (volumeFraction < 0){
                        volumeFraction = 0;
                    }
                }
                return volumeFraction;
            }

        }
	}
}


#include <Eigen/Dense>
#include <opencv2/opencv.hpp>

#define CVUI_IMPLEMENTATION

#include "cvui.h"

#define WINDOW_NAME "CVUI Hello World!"

typedef Eigen::Array<bool,Eigen::Dynamic,1> ArrayXb;

int main(void)
{
    cv::Mat lena = cv::imread("../data/sss.png");
    cv::Mat frame = lena.clone();
    int low_threshold = 50, high_threshold = 150;

    cv::namedWindow(WINDOW_NAME);
    cvui::init(WINDOW_NAME);

    ArrayXb modes = ArrayXb::Constant(3,false);

    bool& ANNOTATE = modes(0);
    bool& DELETE = modes(1);
    bool& MERGE = modes(2);

    ANNOTATE = true;

    std::vector<cv::Point> points;

    while (true)
    {

        bool a_mode = ANNOTATE;
        bool d_mode = DELETE;
        bool m_mode = MERGE;

        bool undo_flag = false, redo_flag = false;

        cvui::window(frame, 10, 50, 180, 180, "Mode");
        cvui::checkbox(frame, 15, 80, "Annotate", &a_mode);
        cvui::checkbox(frame, 15, 120, "Delete", &d_mode);
        cvui::checkbox(frame, 15, 160, "Merge", &m_mode);

        cvui::window(frame, 10, 200, 180, 180, "Revert");
        cvui::checkbox(frame, 15, 240, "Undo", &undo_flag);
        cvui::checkbox(frame, 80, 240, "Redo", &redo_flag);

        cvui::printf(frame, 15, 280, 0.4, 0xff0000, "Button click count: %d", 3);

        cvui::update();
        cv::imshow(WINDOW_NAME, frame);

        if (cv::waitKey(30) == 27) {
            break;
        }

        if (undo_flag)
        {
            printf("UNDO!\n");
            continue;
        } else if (redo_flag)
        {
            printf("REDO!\n");
            continue;
        } else if (a_mode && !ANNOTATE)
        {
            modes = ArrayXb::Constant(3,false);
            ANNOTATE = true;
        }
        else if (d_mode && !DELETE)
        {
            modes = ArrayXb::Constant(3,false);
            DELETE = true;
        }
        else if (m_mode && !MERGE)
        {
            modes = ArrayXb::Constant(3,false);
            MERGE = true;
        }

        if (ANNOTATE)
        {
            if (cvui::mouse(cvui::DOWN)) {
                // Position the rectangle at the mouse pointer.
                cv::Point pt;
                pt.x = cvui::mouse().x;
                pt.y = cvui::mouse().y;
                cv::circle( frame, pt, 3, cv::Scalar(0,0,255), 3, 8, 0 );
            }
        } else if (DELETE) {

        } else if (MERGE) {

        }
        // cvui::trackbar(frame, 15, 110, 165, &low_threshold, 5, 150);
        // cvui::trackbar(frame, 15, 180, 165, &high_threshold, 80, 300);

    }
    return 0;
}


#include "stdio.h"
#include "stdlib.h"
#include "string.h"
#include <iostream>
#include <torch/torch.h>
#include <boost/circular_buffer.hpp>
#include <vector>

#include "agent.h"

using namespace std;
using namespace torch;
using Experience = std::tuple<torch::Tensor, torch::Tensor, torch::Tensor, torch::Tensor, torch::Tensor>;

/*learning hyper-params*/
const int64_t actor_state = 5;//input size of actor
const int64_t actor_layer_1 = 256;
const int64_t actor_layer_2 = 128;
const int64_t actor_layer_3 = 64;
const int64_t actor_layer_4 = 32;
const int64_t actor_out = 1;//output size of actor

// input to critic = actor_state , actor_out as critic is for action-value
const int64_t critic_layer_1 = 200;
const int64_t critic_layer_2 = 100;
const int64_t critic_layer_3 = 25;
const int64_t critic_out = 1;// output_size of critic
// note: architecture and activation f's of networks can be changed in "model.hpp"

const float actor_lr = 3e-4;      //learning rate for actor = 0.0001
const float critic_lr = 1e-3;     //learning rate of critic= 0.001
const float tau = 0.01;            //rate at which target nets are updated from local nets
const int batch_size = 256;        //IT IIZ WHAT IT IIZ!
float action_scale = 1.0;     // we are going to scale the output of action by this quantity
const float gamma = 0.99;         //forget/discout rate

Agent::Agent():
a_local(Actor(actor_state, actor_layer_1, actor_layer_2, actor_layer_3, actor_layer_4, actor_out)),
a_target(Actor(actor_state, actor_layer_1, actor_layer_2, actor_layer_3, actor_layer_4, actor_out)),
c_local(Critic(actor_state, actor_out, critic_layer_1, critic_layer_2, critic_layer_3, critic_out)),
c_target(Critic(actor_state, actor_out, critic_layer_1, critic_layer_2, critic_layer_3, critic_out)),
actor_optimizer(a_local->parameters(), actor_lr),
critic_optimizer(c_local->parameters(), critic_lr),
Buffer(ReplayBuffer()),
noise(OUNoise(actor_out)),
device(torch::kCPU)
{
    Agent::id = id;
    if (torch::cuda::is_available())
    {
        std::cout << "CUDA is available! Training on GPU." << std::endl;
        device = torch::Device(torch::kCUDA);
    }
    a_local->to(device);
    a_target->to(device);
    c_local->to(device);
    c_target->to(device);

    Agent::hard_copy();
}

float Agent::get_reward(float &reward, float (&reward_type)[5], vector<float> &new_state, vector<float> &old_state)
{
    //target is to reach the top cos(pi) = -1
    float new_distance = abs((-1) - new_state[1]);
    float old_distance = abs((-1) - old_state[1]);
    reward = 10*((0.99*old_distance)-new_distance);
    reward_type[0] += reward; // task reward

    reward -= abs(new_state[3]);
    reward_type[1] -= abs(new_state[3]);// negative reward for more torque

    reward -= 0.1*new_state[4];
    reward_type[2] -= 0.1*new_state[4];// negative reward for more time taken

    if(new_distance<0.01 && abs(new_state[2])<0.3)// if we are near the top and our speed is less
    {
        reward += 1000;
        reward_type[3] += 1000;// task successful
        return 1.0;
    }
    else if(abs(new_state[1])>10)//else if our speed is too much
    {
        reward -=1000;
        reward_type[4] -= 1000;// safety breached shut down
        return 1.0;
    }
    else
    {
        return 0.0;
    }

}

void Agent::step(Env &env, float &done, float &reward, float (&reward_type)[5], float &total_reward)
{
    a_local->eval();
    NoGradGuard gaurd;

    vector<float> sensor_values_vector;
    env.get_state(sensor_values_vector);
    Tensor sensor_values_tensor = torch::tensor(sensor_values_vector,device=Agent::device);

    auto action_tensor_cpu = a_local->forward(sensor_values_tensor , action_scale).to(torch::kCPU);//global action scale
    vector<double> action_vector(action_tensor_cpu.data<float>(), action_tensor_cpu.data<float>() + action_tensor_cpu.numel());
    action_vector = Agent::noise.sample(action_vector);   //adding noise to the action vector

    env.take_action(action_vector);
    auto action_tensor = torch::tensor(action_vector, device = Agent::device);    //getting action back on gpu for saving

    vector<float> sensor_values_vector_new;
    env.get_state(sensor_values_vector_new);
    Tensor sensor_values_tensor_new = torch::tensor(sensor_values_vector_new,device=Agent::device);

    done = Agent::get_reward(reward, reward_type, sensor_values_vector_new, sensor_values_vector);
    total_reward += reward;

    Tensor reward_tensor = torch::tensor(reward, device = Agent::device);
    reward_tensor = torch::unsqueeze(reward_tensor, 0);

    Tensor done_tensor = torch::tensor(done, device = Agent::device);
    done_tensor = torch::unsqueeze(done_tensor, 0);

    Agent::Buffer.addExperienceState(sensor_values_tensor, action_tensor, reward_tensor, sensor_values_tensor_new, done_tensor);
}

void Agent::learn(double &total_actor_loss, double &total_critic_loss)
{
    if(Agent::Buffer.getLength() > size_t(batch_size))//batchsixe global
    {
        a_local->train();
        auto& [state, action, reward_batch, next_state, donezo] = Buffer.sample(batch_size);//global batch_size
        auto actions_next = a_target->forward(next_state,action_scale);//global actions acale
        auto Q_targets_next = c_target->forward(next_state, actions_next);
        auto Q_targets = reward_batch + (gamma * Q_targets_next * (1 - donezo));//global gamma
        auto Q_expected = c_local->forward(state, action);

        torch::Tensor critic_loss = torch::mse_loss(Q_expected, Q_targets.detach());
        critic_optimizer.zero_grad();
        critic_loss.backward();
        critic_optimizer.step();

        auto actions_pred = a_local->forward(state,action_scale);////global actions acale

        auto actor_loss = -(c_local->forward(state, actions_pred).mean());
        actor_optimizer.zero_grad();
        actor_loss.backward();
        actor_optimizer.step();

        Agent::soft_copy();

        total_actor_loss += actor_loss.item<double>();
        total_critic_loss += critic_loss.item<double>();
    }
}

void Agent::hard_copy()
{
    torch::NoGradGuard no_grad;
    for(size_t i=0; i < a_target->parameters().size(); i++)
        a_target->parameters()[i].copy_(a_local->parameters()[i]);
    for(size_t i=0; i < c_target->parameters().size(); i++)
        c_target->parameters()[i].copy_(c_local->parameters()[i]);
}

void Agent::soft_copy()
{
    torch::NoGradGuard no_grad;//       disables calulation of gradients
    for(size_t i=0; i < a_target->parameters().size(); i++)
        a_target->parameters()[i].copy_(tau * a_local->parameters()[i] + (1.0 - tau) * a_target->parameters()[i]);//global tau
    for(size_t i=0; i < c_target->parameters().size(); i++)
        c_target->parameters()[i].copy_(tau * c_local->parameters()[i] + (1.0 - tau) * c_target->parameters()[i]);//global tau
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_LOCSCALE_CONSTRAIN_HPP
#define STAN_MATH_PRIM_SCAL_FUN_LOCSCALE_CONSTRAIN_HPP

#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/scal/fun/identity_constrain.hpp>
#include <stan/math/prim/scal/fun/abs.hpp>
#include <stan/math/prim/scal/meta/size_of.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <cmath>
#include <limits>

namespace stan {
namespace math {

/**
 * Return the linearly transformed value for the specified unconstrained input
 * and specified location and scale.
 *
 *
The transform applied is
 *
 *
\f$f(x) = mu + sigma * x\f$
 *
 *
where mu is the location and sigma is the scale.
 *
 *
If the location is zero and the scale is one this
 * reduces to identity_constrain(x).
 *
 * @tparam T type of scalar
 * @tparam M type of mean
 * @tparam S type of scale
 * @param[in] x Unconstrained scalar input
 * @param[in] mu location of constrained output
 * @param[in] sigma scale of constrained output
 * @return linear transformed value correspdonding to inputs
 * @throw std::domain_error if sigma <= 0
 * @throw std::domain_error if mu is not finite
 */
template <typename T, typename M, typename S>
inline typename boost::math::tools::promote_args<T, M, S>::type
locscale_constrain(const T& x, const M& mu, const S& sigma) {
  check_finite("locscale_constrain", "location", mu);
  if (sigma == 1) {
    if (mu == 0)
      return identity_constrain(x);
    return mu + x;
  }
  check_positive_finite("locscale_constrain", "scale", sigma);
  return mu + sigma * x;
}

/**
 * Return the linearly transformed value for the specified unconstrained input
 * and specified location and scale, incrementing the specified
 * reference with the log absolute Jacobian determinant of the
 * transform.
 *
 *
The transform applied is
 *
 *
\f$f(x) = mu + sigma * x\f$
 *
 *
where mu is the location and sigma is the scale.
 *
 * If the location is zero and scale is one, this function
 * reduces to identity_constraint(x, lp).
 *
 * @tparam T type of scalar
 * @tparam M type of mean
 * @tparam S type of scale
 * @param[in] x Unconstrained scalar input
 * @param[in] mu location of constrained output
 * @param[in] sigma scale of constrained output
 * @param[in,out] lp Reference to log probability to increment.
 * @return linear transformed value corresponding to inputs
 * @throw std::domain_error if sigma <= 0
 * @throw std::domain_error if mu is not finite
 */
template <typename T, typename M, typename S>
inline typename boost::math::tools::promote_args<T, M, S>::type
locscale_constrain(const T& x, const M& mu, const S& sigma, T& lp) {
  using std::log;
  check_finite("locscale_constrain", "location", mu);
  if (sigma == 1) {
    if (mu == 0)
      return identity_constrain(x);
    return mu + x;
  }
  check_positive_finite("locscale_constrain", "scale", sigma);
  lp += size_of(x) * log(sigma);
  return mu + sigma * x;
}

}  // namespace math

}  // namespace stan

#endif


#include <Eigen/Core>
#include <Eigen/Dense>
#include "parametrizer.hpp"
using namespace Eigen;

void subdivide(MatrixXi &F, MatrixXd &V, VectorXd& rho, VectorXi &V2E, VectorXi &E2E, VectorXi &boundary,
               VectorXi &nonmanifold, double maxLength);

void subdivide_edgeDiff(MatrixXi &F, MatrixXd &V, MatrixXd &N, MatrixXd &Q, MatrixXd &O, MatrixXd* S,
                    VectorXi &V2E, VectorXi &E2E, VectorXi &boundary, VectorXi &nonmanifold,
                    std::vector<Vector2i> &edge_diff, std::vector<DEdge> &edge_values,
                    std::vector<Vector3i> &face_edgeOrients, std::vector<Vector3i> &face_edgeIds,
                    std::vector<int>& sharp_edges, std::map<int, int> &singularities, int max_len);


#include <vector>

#include <boost/shared_ptr.hpp>
#include <gflags/gflags.h>
#include <glog/logging.h>

#include <cmath>

#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/layers/photometric_augmentation_layer.hpp"
#include "caffe/util/math_functions.hpp"

namespace caffe {

template <typename Dtype>
void PhotometricAugmentationLayer<Dtype>::LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {
	//allocate noise blob
	for(int i=0;i<bottom.size();i++){
		noise.push_back(new Blob<Dtype>);
	}
}

template <typename Dtype>
PhotometricAugmentationLayer<Dtype>::~PhotometricAugmentationLayer(){
	for(int i=0;i<noise.size();i++){
		delete noise[i];
	}
}

template <typename Dtype>
void PhotometricAugmentationLayer<Dtype>::Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {
	//check bottoms are same size
	for(int i=1;i<bottom.size();i++){
		CHECK_EQ(bottom[i]->shape(0), bottom[0]->shape(0)) << "All bottoms must have the same dimensions";
		CHECK_EQ(bottom[i]->shape(1), bottom[0]->shape(1)) << "All bottoms must have the same dimensions";
		CHECK_EQ(bottom[i]->shape(2), bottom[0]->shape(2)) << "All bottoms must have the same dimensions";
		CHECK_EQ(bottom[i]->shape(3), bottom[0]->shape(3)) << "All bottoms must have the same dimensions";
	}

	//reshape tops
	for(int i=0;i<bottom.size();i++){
		top[i]->ReshapeLike(*bottom[0]);
	}
	int batchSize = bottom[0]->shape(0);
	int channels = bottom[0]->shape(1);

	//check mean subtraction params are valid
	int num_mean_params = this->layer_param_.photometric_augmentation_param().mean_subtraction_size();
	if (num_mean_params > 0){
		//check if means == channels

		CHECK_EQ(channels, num_mean_params) << "There must be a mean_subtraction param for every input channel";

		hasMeans = true;
		vector<int> shape_mean(1);
		shape_mean[0] = num_mean_params;
		shape_mean[1] = 1;
		shape_mean[2] = 1;
		shape_mean[3] = 1;
		means.Reshape(shape_mean);

		//copy data to means
		Dtype* meansData = means.mutable_cpu_data();
		for(int i=0;i<num_mean_params;i++){
			meansData[i] = (Dtype)this->layer_param_.photometric_augmentation_param().mean_subtraction(i);
		}
	}
	else{
		hasMeans = false;
	}

	//reshape helper blobs
	vector<int> shape_t_params(4);
	shape_t_params[0] = batchSize;
	shape_t_params[1] = 1;
	shape_t_params[2] = 1;
	shape_t_params[3] = 1;

	vector<int> shape_t_params2(4);
	shape_t_params2[0] = batchSize;
	shape_t_params2[1] = 1;
	shape_t_params2[2] = 1;
	shape_t_params2[3] = 3;

	for(int i=0;i<bottom.size();i++){
		noise[i]->ReshapeLike(*bottom[0]);
	}
	contrast.Reshape(shape_t_params);
	brightness.Reshape(shape_t_params);
	colour.Reshape(shape_t_params2);
	gamma.Reshape(shape_t_params);
}

template <typename Dtype>
void PhotometricAugmentationLayer<Dtype>::Forward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {
	//get parameters
	Dtype contrast_min = this->layer_param_.photometric_augmentation_param().contrast_min();
	Dtype contrast_max = this->layer_param_.photometric_augmentation_param().contrast_max();
	Dtype brightness_sigma = this->layer_param_.photometric_augmentation_param().brightness_sigma();
	Dtype colour_min = this->layer_param_.photometric_augmentation_param().colour_min();
	Dtype colour_max = this->layer_param_.photometric_augmentation_param().colour_max();
	Dtype gamma_min = this->layer_param_.photometric_augmentation_param().gamma_min();
	Dtype gamma_max = this->layer_param_.photometric_augmentation_param().gamma_max();
	Dtype noise_sigma = this->layer_param_.photometric_augmentation_param().noise_sigma();

	//Dtype mean_subtraction = this->layer_param_.photometric_augmentation_param().mean_subtraction();

	//helpers
	Dtype* contrastData = contrast.mutable_cpu_data();
	Dtype* brightnessData = brightness.mutable_cpu_data();
	Dtype* colourData = colour.mutable_cpu_data();
	Dtype* gammaData = gamma.mutable_cpu_data();
	const Dtype* meanData = means.cpu_data();

	int N = top[0]->shape(0);

	//generate paramaters
	if (noise_sigma>0){
		for(int i=0;i<bottom.size();i++){
			caffe_rng_gaussian(noise[i]->count(), (Dtype)0, noise_sigma, noise[i]->mutable_cpu_data());
		}
	}
	else{
		for(int i=0;i<bottom.size();i++){
			caffe_set(noise[i]->count(), (Dtype)0, noise[i]->mutable_cpu_data());
		}
	}
	caffe_rng_uniform(N,contrast_min,contrast_max,contrastData);
	if(brightness_sigma > 0){
		caffe_rng_gaussian(N, (Dtype)0, brightness_sigma, brightnessData);
	}
	else{
		caffe_set(N, (Dtype)0, brightnessData);
	}
	caffe_rng_uniform(N*3,colour_min,colour_max,colourData);
	caffe_rng_uniform(N,gamma_min,gamma_max,gammaData);

	//transform
	for(int b=0;b<bottom.size();b++){
		Dtype* noiseData = noise[b]->mutable_cpu_data();
		const Dtype* bottom_data = bottom[b]->cpu_data();
		Dtype* topData = top[b]->mutable_cpu_data();

		for(int n=0;n<N;n++){
			const Dtype* curBottom = &bottom_data[bottom[b]->offset(n,0,0,0)];
			Dtype* curTop = &topData[top[b]->offset(n,0,0,0)];
			Dtype* curNoise =  &noiseData[noise[b]->offset(n,0,0,0)];
			int channelPixels = bottom[b]->shape(2) * bottom[b]->shape(3);
			int pixels = bottom[b]->shape(1) * channelPixels;

			//a*I + b
			caffe_axpy(pixels,contrastData[n] + 1,curBottom,curTop);
			caffe_add_scalar(pixels, brightnessData[n], curTop);

			//color
			caffe_scal(channelPixels,colourData[3*n],curTop);
			caffe_scal(channelPixels,colourData[3*n+1],&curTop[channelPixels]);
			caffe_scal(channelPixels,colourData[3*n+2],&curTop[channelPixels*2]);

			//clamp values
			for(int i=0;i<pixels;i++){
				if(curTop[i] < 0){
					curTop[i]  = 0;
				}
				else if(curTop[i] > 1){
					curTop[i] = 1;
				}
			}

			//gamma
			Dtype gammaInv = 1/gammaData[n];
			caffe_powx(pixels,curTop,gammaInv,curTop);

			//combine noise
			caffe_axpy(pixels, (Dtype)1, curNoise, curTop);

			//subtract mean
			caffe_add_scalar(channelPixels,-meanData[0],curTop);
			caffe_add_scalar(channelPixels,-meanData[1],&curTop[channelPixels]);
			caffe_add_scalar(channelPixels,-meanData[2],&curTop[channelPixels*2]);
		}
	}
}

template <typename Dtype>
void PhotometricAugmentationLayer<Dtype>::Backward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& top) {
	//do nothing
}

#ifdef CPU_ONLY
STUB_GPU(PhotometricAugmentationLayer);
#endif

INSTANTIATE_CLASS(PhotometricAugmentationLayer);
REGISTER_LAYER_CLASS(PhotometricAugmentation);

}  // namespace caffe


#include <pcl/range_image/range_image.h>
#include <pcl/range_image/range_image_planar.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/console/print.h>
#include <pcl/surface/organized_fast_mesh.h>
#include <pcl/console/time.h>
#include <Eigen/StdVector>
#include <Eigen/Geometry>
#include <iostream>
#include <pcl/surface/impl/organized_fast_mesh.hpp>
#include <boost/thread/thread.hpp>

#include <pcl/common/common_headers.h>

#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
using namespace pcl::console;
int main (int argc, char** argv) {

	// Generate the data
	if (argc<2)
	{

		print_error ("Syntax is: %s input.pcd -w 640 -h 480 -cx 320 -cy 240 -fx 525 -fy 525 -type 0 -size 2\n", argv[0]);
		print_info ("  where options are:\n");
		print_info ("                     -w X = width of detph iamge ");

		return -1;
	}
	std::string filename = argv[1];

	int width=640,height=480,size=2,type=0;
	float fx=525,fy=525,cx=320,cy=240;

	parse_argument (argc, argv, "-w", width);
	parse_argument (argc, argv, "-h", height);
	parse_argument (argc, argv, "-cx", cx);
	parse_argument (argc, argv, "-cy", cy);
	parse_argument (argc, argv, "-fx", fx);
	parse_argument (argc, argv, "-fy", fy);
	parse_argument (argc, argv, "-type", type);
	parse_argument (argc, argv, "-size", size);
	//convert unorignized point cloud to orginized point cloud begin
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::io::loadPCDFile (filename, *cloud);
	print_info ("Read pcd file successfully\n");
	Eigen::Affine3f sensorPose;
	sensorPose.setIdentity();
	pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
	float noiseLevel=0.00;
	float minRange = 0.0f;

	pcl::RangeImagePlanar::Ptr rangeImage(new pcl::RangeImagePlanar);
	rangeImage->createFromPointCloudWithFixedSize(*cloud,width,height,cx,cy,fx,fy,sensorPose,coordinate_frame);
	std::cout << rangeImage << "\n";
	//convert unorignized point cloud to orginized point cloud end

	//viusalization of range image
	pcl::visualization::RangeImageVisualizer range_image_widget ("Range Image");
	range_image_widget.showRangeImage (*rangeImage);
	range_image_widget.setWindowTitle("Range Image");
	//triangulation based on range image
	pcl::OrganizedFastMesh<pcl::PointWithRange>::Ptr tri(new pcl::OrganizedFastMesh<pcl::PointWithRange>);
	pcl::search::KdTree<pcl::PointWithRange>::Ptr tree (new pcl::search::KdTree<pcl::PointWithRange>);
	tree->setInputCloud(rangeImage);
	pcl::PolygonMesh triangles;
	tri->setTrianglePixelSize(size);
	tri->setInputCloud(rangeImage);
	tri->setSearchMethod(tree);
	tri->setTriangulationType((pcl::OrganizedFastMesh<pcl::PointWithRange>::TriangulationType)type);
	tri->reconstruct(triangles);
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("Reconstruction"));
	viewer->setBackgroundColor(0.5,0.5,0.5);
	viewer->addPolygonMesh(triangles,"tin");
	viewer->addCoordinateSystem();
	while (!range_image_widget.wasStopped ()&&!viewer->wasStopped())
	{
		range_image_widget.spinOnce ();

		pcl_sleep (0.01);
		viewer->spinOnce ();

	}
}


#ifndef KDL_CHAINIKSOLVERPOS_GN_HPP
#define KDL_CHAINIKSOLVERPOS_GN_HPP
/**
 \file   chainiksolverpos_lma.hpp
 \brief  computing inverse position kinematics using Levenberg-Marquardt.
*/

/**************************************************************************
    begin                : May 2012
    copyright            : (C) 2012 Erwin Aertbelien
    email                : firstname.lastname@mech.kuleuven.ac.be

 History (only major changes)( AUTHOR-Description ) :

 ***************************************************************************
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Lesser General Public            *
 *   License as published by the Free Software Foundation; either          *
 *   version 2.1 of the License, or (at your option) any later version.    *
 *                                                                         *
 *   This library 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     *
 *   Lesser General Public License for more details.                       *
 *                                                                         *
 *   You should have received a copy of the GNU Lesser General Public      *
 *   License along with this library; if not, write to the Free Software   *
 *   Foundation, Inc., 59 Temple Place,                                    *
 *   Suite 330, Boston, MA  02111-1307  USA                                *
 *                                                                         *
 ***************************************************************************/

#include "chainiksolver.hpp"
#include "chain.hpp"
#include <Eigen/Dense>

namespace KDL
{

/**
 * \brief Solver for the inverse position kinematics that uses Levenberg-Marquardt.
 *
 * The robustness and speed of this solver is improved in several ways:
 *   - by using a Levenberg-Marquardt method that automatically adapts the damping when
 *     computing the inverse damped least squares inverse velocity kinematics.
 *   - by using an internal implementation of forward position kinematics and the
 *     Jacobian kinematics.  This implementation is more numerically robust,
 *     is able to cache previous computations, and implements an \f$ \mathcal{O}(N) \f$
 *     algorithm for the computation of the Jacobian (with \f$N\f$, the number of joints, and for
 *     a fixed size task space).
 *   - by providing a way to specify the weights in task space, you can weigh rotations wrt translations.
 *     This is important e.g. to specify that rotations do not matter for the problem at hand, or to
 *     specify how important you judge rotations w.r.t. translations, typically in S.I.-units, ([m],[rad]),
 *     the rotations are over-specified, this can be avoided using the weight matrix. Weights also
 *     make the solver more robust .
 *   - only the constructors call memory allocation.
 *
 * De general principles behind the optimisation is inspired on:
 *   Jorge Nocedal, Stephen J. Wright, Numerical Optimization,Springer-Verlag New York, 1999.

 * \ingroup KinematicFamily
 */
class ChainIkSolverPos_LMA : public KDL::ChainIkSolverPos
{
private:
	typedef double ScalarType;
    typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,Eigen::Dynamic> MatrixXq;
    typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,1> VectorXq;
public:

    static const int E_GRADIENT_JOINTS_TOO_SMALL = -100;
    static const int E_INCREMENT_JOINTS_TOO_SMALL = -101;

    /**
	 * \brief constructs an ChainIkSolverPos_LMA solver.
	 *
	 * The default parameters are choosen to be applicable to industrial-size robots
	 * (e.g. 0.5 to 3 meters range in task space), with an accuracy that is more then
	 * sufficient for typical industrial applications.
	 *
	 * Weights are applied in task space, i.e. the kinematic solver minimizes:
	 * \f$ E = \Delta \mathbf{x}^T \mathbf{L} \mathbf{L}^T \Delta \mathbf{x} \f$, with \f$\mathbf{L}\f$ a diagonal matrix.
	 *
	 * \param _chain specifies the kinematic chain.
	 * \param _L specifies the "square root" of the weight (diagonal) matrix in task space. This diagonal matrix is specified as a vector.
	 * \param _eps specifies the desired accuracy in task space; after weighing with
	 *        the weight matrix, it is applied on \f$E\f$.
	 * \param _maxiter specifies the maximum number of iterations.
	 * \param _eps_joints specifies that the algorithm has to stop when the computed joint angle increments are
	 *        smaller then _eps_joints.  This is to avoid unnecessary computations up to _maxiter when the joint angle
	 *        increments are so small that they effectively (in floating point) do not change the joint angles any more.  The default
	 *        is a few digits above numerical accuracy.
     */
    ChainIkSolverPos_LMA(
    		const KDL::Chain& _chain,
    		const Eigen::Matrix<double,6,1>& _L,
    		double _eps=1E-5,
    		int _maxiter=500,
    		double _eps_joints=1E-15
    );

    /**
     * \brief identical the full constructor for ChainIkSolverPos_LMA, but provides for a default weight matrix.
     *
     *  \f$\mathbf{L} = \mathrm{diag}\left( \begin{bmatrix} 1 & 1 & 1 & 0.01 & 0.01 & 0.01 \end{bmatrix} \right) \f$.
     */
    ChainIkSolverPos_LMA(
    		const KDL::Chain& _chain,
    		double _eps=1E-5,
    		int _maxiter=500,
    		double _eps_joints=1E-15
    );

    /**
     * \brief computes the inverse position kinematics.
     *
     * \param q_init initial joint position.
     * \param T_base_goal goal position expressed with respect to the robot base.
     * \param q_out  joint position that achieves the specified goal position (if successful).
     * \return E_NOERROR if successful,
     *         E_GRADIENT_JOINTS_TOO_SMALL the gradient of \f$ E \f$ towards the joints is to small,
     *         E_INCREMENT_JOINTS_TOO_SMALL if joint position increments are to small,
     *         E_MAX_ITER_EXCEEDED if number of iterations is exceeded.
     */
    virtual int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out);

    /**
     * \brief destructor.
     */
    virtual ~ChainIkSolverPos_LMA();

    /**
     * \brief for internal use only.
     *
     * Only exposed for test and diagnostic purposes.
     */
    void compute_fwdpos(const VectorXq& q);

    /**
     * \brief for internal use only.
     * Only exposed for test and diagnostic purposes.
     * compute_fwdpos(q) should always have been called before.
     */
    void compute_jacobian(const VectorXq& q);

    /**
     * \brief for internal use only.
     * Only exposed for test and diagnostic purposes.
     */
    void display_jac(const KDL::JntArray& jval);

    /// @copydoc KDL::SolverI::strError()
    virtual const char* strError(const int error) const;

private:
    const KDL::Chain& chain;
    unsigned int nj;
    unsigned int ns;

public:

    /**
     * \brief contains the last number of  iterations for an execution of CartToJnt.
     */
    int lastNrOfIter;

    /**
     * \brief contains the last value for \f$ E \f$ after an execution of CartToJnt.
     */
    double lastDifference;

    /**
     * \brief contains the last value for the (unweighted) translational difference after an execution of CartToJnt.
     */
    double lastTransDiff;

    /**
     * \brief contains the last value for the (unweighted) rotational difference after an execution of CartToJnt.
     */
    double lastRotDiff;

    /**
     * \brief contains the last values for the singular values of the weighted Jacobian after an execution of CartToJnt.
     */
    VectorXq lastSV;

    /**
     * \brief for internal use only.
     *
     * contains the last value for the Jacobian after an execution of compute_jacobian.
     */
    MatrixXq jac;

    /**
     * \brief for internal use only.
     *
     * contains the gradient of the error criterion after an execution of CartToJnt.
     */
    VectorXq grad;
    /**
     * \brief for internal use only.
     *
     * contains the last value for the position of the tip of the robot (head) with respect to the base, after an execution of compute_jacobian.
     */
    KDL::Frame T_base_head;

    /**
     * \brief display information on each iteration step to the console.
     */
    bool display_information;
private:
    // additional specification of the inverse position kinematics problem:
    unsigned int maxiter;
    double eps;
    double eps_joints;
    Eigen::Matrix<ScalarType,6,1> L;

    // state of compute_fwdpos and compute_jacobian:
    std::vector<KDL::Frame> T_base_jointroot;
    std::vector<KDL::Frame> T_base_jointtip;
					// need 2 vectors because of the somewhat strange definition of segment.hpp
					// you could also recompute jointtip out of jointroot,
    				// but then you'll need more expensive cos/sin functions.

    // the following are state of CartToJnt that is pre-allocated:

    VectorXq q;
    MatrixXq A;
    VectorXq tmp;
    Eigen::LDLT<MatrixXq> ldlt;
    Eigen::JacobiSVD<MatrixXq> svd;
    VectorXq diffq;
    VectorXq q_new;
    VectorXq original_Aii;
};

}; // namespace KDL

#endif


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory for Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */
#include <Utils/Geometry.h>
#include <Utils/Geometry/AtomCollection.h>
#include <Utils/IO/ChemicalFileFormats/XyzStreamHandler.h>
#include <Utils/Math/QuaternionFit.h>
#include <gmock/gmock.h>
#include <Eigen/Geometry>

using namespace testing;
namespace Scine {
namespace Utils {
namespace Tests {

/**
 * @class QuaternionFitTest QuaternionFitTest.cpp
 * @brief Comprises tests for the class Scine::Utils::QuaternionFit.
 * @test
 */
class QuaternionFitTest : public Test {
 public:
  const int numberParticles = 22;
  Eigen::MatrixXd randomPos1, randomPos2;

 protected:
  void SetUp() override {
    randomPos1 = Eigen::MatrixXd::Random(numberParticles, 3) * 22;
    randomPos2 = Eigen::MatrixXd::Random(numberParticles, 3) * 10.6;
  }
};

TEST_F(QuaternionFitTest, ReturnsZeroTranslationForIdenticalMatrix) {
  QuaternionFit fit(randomPos1, randomPos1);
  Eigen::Vector3d translationVector = fit.getTransVector();
  ASSERT_TRUE(translationVector.isZero());
}

TEST_F(QuaternionFitTest, TwoDimers) {
  PositionCollection positions1(2, 3);
  PositionCollection positions2(2, 3);
  positions1 << -2, 0, 0, 2, 0, 0;
  positions2 << -1, 0, 0, 1, 0, 0;
  QuaternionFit fit(positions1, positions2);
  ASSERT_NEAR(fit.getRMSD(), 1.0, 1E-6);
}

TEST_F(QuaternionFitTest, Chirality) {
  std::stringstream stream("5\n\n"
                           "C     -0.0220968652    0.0032150545    0.0165197402\n"
                           "H     -0.6690087809    0.8893598645   -0.1009085033\n"
                           "H     -0.3777879439   -0.8577518853   -0.5882960277\n"
                           "H      0.0964209243   -0.3151252987    1.0637808670\n"
                           "H      0.9724726657    0.2803022650   -0.3910960761");

  auto atoms1 = XyzStreamHandler::read(stream);
  auto atoms2 = atoms1;

  // Swap positions
  atoms2.setPosition(1, atoms1.getPosition(2));
  atoms2.setPosition(2, atoms1.getPosition(1));

  // left hand vs right hand do not allow inversion (default), expect no inversion and bad fit
  QuaternionFit fit1(atoms1.getPositions(), atoms2.getPositions());
  ASSERT_NEAR(fit1.getRMSD(), 2.098277, 1E-6);

  Eigen::MatrixXd refMat = atoms1.getPositions();
  Eigen::MatrixXd fitMat = atoms2.getPositions();
  Eigen::VectorXd weights = Eigen::VectorXd::Ones(atoms1.size());
  bool improperRotationsAllowed = true;

  // left hand vs right hand allow inversion, expect inversion
  QuaternionFit fit2(refMat, fitMat, weights, improperRotationsAllowed);
  ASSERT_NEAR(fit2.getRMSD(), 0.046643, 1E-6);

  // left hand vs left hand allow inverison, expect none
  refMat = atoms1.getPositions();
  fitMat = atoms2.getPositions();
  QuaternionFit fit3(refMat, fitMat, weights, improperRotationsAllowed);
  ASSERT_NEAR(fit3.getRMSD(), 0.046643, 1E-6);
}

TEST_F(QuaternionFitTest, Weights) {
  std::stringstream stream("3\n\n"
                           "O    -0.00392892   0.38587356  0.00000000\n"
                           "H    -0.76036657  -0.19836384  0.00000000\n"
                           "H     0.75900101  -0.18964299  0.00000000");

  auto atoms1 = XyzStreamHandler::read(stream);
  auto atoms2 = atoms1;

  // Move one position
  auto pos = atoms1.getPosition(1);
  atoms2.setPosition(2, pos + Position{0, 0, 1});

  Eigen::MatrixXd refMat = atoms1.getPositions();
  Eigen::MatrixXd fitMat = atoms2.getPositions();
  Eigen::VectorXd fitWeights = Eigen::VectorXd::Ones(atoms1.size());
  fitWeights[2] = 0;
  bool improperRotationsAllowed = false;
  QuaternionFit fit(refMat, fitMat, fitWeights, improperRotationsAllowed);

  // Standard RMSD
  ASSERT_NEAR(fit.getRMSD(), 1.755372, 1E-6);

  // Weighted RMSD
  ASSERT_NEAR(fit.getWeightedRMSD(fitWeights), 0, 1E-6);
}

TEST_F(QuaternionFitTest, ReturnsZeroRotationForIdenticalMatrix) {
  QuaternionFit fit(randomPos1, randomPos1);
  Eigen::Matrix3d rotationMatrix = fit.getRotationMatrix();
  ASSERT_TRUE(rotationMatrix.isApprox(Eigen::Matrix3d::Identity()));
}

TEST_F(QuaternionFitTest, IsCorrectForTranslatedPositions) {
  Eigen::Vector3d translation(-9.654, 5.85588, 0.11);

  Eigen::MatrixXd translatedPositions = randomPos1;
  translatedPositions.rowwise() += translation.transpose();
  QuaternionFit fit(randomPos1, translatedPositions);

  Eigen::Vector3d translationVector = fit.getTransVector();
  Eigen::Matrix3d rotationMatrix = fit.getRotationMatrix();

  ASSERT_TRUE(translationVector.isApprox(translation));
  ASSERT_TRUE(rotationMatrix.isApprox(Eigen::Matrix3d::Identity()));
}

TEST_F(QuaternionFitTest, IsCorrectForRotatedPositions) {
  // First, necessary to center the reference positions:
  Eigen::Vector3d mean = randomPos1.colwise().mean();
  randomPos1.rowwise() -= mean.transpose();

  Eigen::AngleAxisd rotation(4.001, Eigen::Vector3d::Random().normalized());

  Eigen::MatrixXd rotatedPositions(numberParticles, 3);
  for (int i = 0; i < numberParticles; ++i) {
    rotatedPositions.row(i) = rotation * randomPos1.row(i).transpose();
  }
  QuaternionFit fit(randomPos1, rotatedPositions);

  Eigen::Vector3d translationVector = fit.getTransVector();
  Eigen::Matrix3d rotationMatrix = fit.getRotationMatrix();

  ASSERT_TRUE(translationVector.isZero());
  ASSERT_TRUE(rotationMatrix.isApprox(rotation.toRotationMatrix()));
}

} // namespace Tests
} /* namespace Utils */
} /* namespace Scine */


#include <stan/math/rev.hpp>
#include <boost/mp11.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/rev/functor/test_fixture_ode.hpp>
#include <test/unit/math/rev/functor/test_fixture_ode_lorenz.hpp>
#include <test/unit/math/rev/functor/ode_test_functors.hpp>

/**
 *
 * Use same solver functor type for both w & w/o tolerance control
 */
template <typename solve_type, typename... Ts>
using ode_test_tuple = std::tuple<solve_type, solve_type, Ts...>;

/**
 * Outer product of test types
 */
using lorenz_test_types = boost::mp11::mp_product<
    ode_test_tuple,
    ::testing::Types<ode_adams_functor, ode_bdf_functor, ode_ckrk_functor,
                     ode_rk45_functor, ode_adjoint_functor>>;

TYPED_TEST_SUITE_P(lorenz_test);
TYPED_TEST_P(lorenz_test, param_and_data_finite_diff) {
  if (std::is_same<TypeParam,
                   std::tuple<ode_rk45_functor, ode_rk45_functor>>::value) {
    this->test_fd_vd(1.e-6, 3e-2);
    this->test_fd_dv(1.e-6, 3e-2);
    this->test_fd_vv(1.e-6, 3e-2);
  } else if (std::is_same<TypeParam, std::tuple<ode_ckrk_functor,
                                                ode_ckrk_functor>>::value) {
    this->test_fd_vd(1.e-6, 5e-2);
    this->test_fd_dv(1.e-6, 5e-2);
    this->test_fd_vv(1.e-6, 5e-2);
  } else {
    this->test_fd_vd(1.e-6, 1e-2);
    this->test_fd_dv(1.e-6, 1e-2);
    this->test_fd_vv(1.e-6, 1e-2);
  }
}
REGISTER_TYPED_TEST_SUITE_P(lorenz_test, param_and_data_finite_diff);
INSTANTIATE_TYPED_TEST_SUITE_P(StanOde, lorenz_test, lorenz_test_types);


#include <iostream>
#include <cassert>
#include <cstdlib>
#include <ctime>
#include <cmath>
#include <random>
#include <armadillo>
#include "port_authority.hpp"
#include "mpi.h"

using namespace std;
using namespace pauth;
using namespace arma;

int main() {

  int taskid, numtasks;
  MPI_Init(nullptr, nullptr);
  MPI_Comm_size(MPI_COMM_WORLD, &numtasks);
  MPI_Comm_rank(MPI_COMM_WORLD, &taskid);

  srand(time(NULL));
  const auto delta_max = static_cast<double>( (rand() % 200 + 50) / 100.0 );
  const auto spring_const = static_cast<double>( (rand() % 1990 + 10) / 500.0 );
  const auto T = static_cast<double>( (rand() % 200000 + 10) / 1000.0 );
  const auto p0x = static_cast<double>( (rand() % 100 - 200) / 50.0 );
  const auto p0y = static_cast<double>( (rand() % 100 - 200) / 50.0 );
  const auto p0z = static_cast<double>( (rand() % 100 - 200) / 50.0 );
  const auto E0x = static_cast<double>( (rand() % 100 - 200) / 20.0 );
  const auto E0y = static_cast<double>( (rand() % 100 - 200) / 20.0 );
  const auto E0z = static_cast<double>( (rand() % 100 - 200) / 20.0 );
  const unsigned nsteps = 20000000;

  if (taskid == 0) {
    cout << "3D linear dipole test started.\n";
    cout << '\n';
    cout << "delta_max = " << delta_max << '\n';
    cout << "k         = " << spring_const << '\n';
    cout << "T         = " << T << '\n';
    cout << "p0x       = " << p0x << '\n';
    cout << "p0y       = " << p0y << '\n';
    cout << "p0z       = " << p0z << '\n';
    cout << "E0x       = " << E0x << '\n';
    cout << "E0y       = " << E0y << '\n';
    cout << "E0z       = " << E0z << '\n';
    cout << '\n';
  }

  const double dx = 0.1;
  const double kB = 1.0;
  const molecular_id id = molecular_id::Test1;
  const size_t N = 1;
  const size_t D = 3;
  const double L = 1.0;
  const metric m = euclidean;
  const bc boundary = no_bc;
  const mat inv_chi = arma::eye(3, 3);
  const vec E0({E0x, E0y, E0z});
  const vec pequil = inv_chi.i() * E0;
  dipole_strain_linear_3d_potential strain_pot(inv_chi);
  dipole_electric_potential electric_pot({E0x, E0y, E0z}, {0, 1, 2});

  metropolis sim(id, N, D, L, continuous_trial_move(delta_max),
                 {&strain_pot, &electric_pot}, T, kB, m, boundary,
                 metropolis_acc, hardware_entropy_seed_gen, true);
  sim.set_positions(pequil, 0);
  const double u0 = accessors::U(sim);
  sim.set_positions({p0x, p0y, p0z}, 0);

  metropolis_suite msuite(sim, 0, 1, info_lvl_flag::QUIET);

  msuite.add_variable_to_average("px", [](const metropolis &sim) {
    return sim.positions()(0, 0);
  });
  msuite.add_variable_to_average("py", [](const metropolis &sim) {
    return sim.positions()(1, 0);
  });
  msuite.add_variable_to_average("pz", [](const metropolis &sim) {
    return sim.positions()(2, 0);
  });
  msuite.add_variable_to_average("px^2", [](const metropolis &sim) {
    auto x = sim.positions()(0, 0);
    return x*x;
  });
  msuite.add_variable_to_average("py^2", [](const metropolis &sim) {
    auto y = sim.positions()(1, 0);
    return y*y;
  });
  msuite.add_variable_to_average("pz^2", [](const metropolis &sim) {
    auto z = sim.positions()(2, 0);
    return z*z;
  });
  msuite.add_variable_to_average("U", accessors::U);
  msuite.add_variable_to_average("delta(x - x0)", [=](const metropolis &sim) {
    for (unsigned i = 0; i < 3; ++i) {
      const double pi = sim.positions()(i, 0);
      if (pi < pequil(i) - dx || pi > pequil(i) + dx) return 0;
    }
    return 1;
  });

  msuite.simulate(nsteps);

  if(taskid == 0) {

    auto averages = msuite.averages();

    const double exp_px = averages["px"];
    const double exp_py = averages["py"];
    const double exp_pz = averages["pz"];

    const double exp_pxsq = averages["px^2"];
    const double exp_pysq = averages["py^2"];
    const double exp_pzsq = averages["pz^2"];

    const double exp_E = averages["U"];

    vec eigval;
    mat eigvec;
    eig_sym(eigval, eigvec, inv_chi);

    vec Ev = eigvec * E0;
    double Z_an = 1.0;
    for(unsigned i = 0; i < 3; ++i)
      Z_an *= sqrt(2 * M_PI * kB * T / eigval(i)) *
              exp(Ev(i)*Ev(i) / (2 * eigval(i) * kB * T));

    const auto exp_pxsq_an = (kB * T / eigval(0));
    const auto exp_pysq_an = (kB * T / eigval(1));
    const auto exp_pzsq_an = (kB * T / eigval(2));

    const auto exp_E_an = 3 * kB * T / 2.0;

    cout << "exp_px    =   " << exp_px << " ?= " << pequil(0) << '\n';
    assert(abs((exp_px - pequil(0)) / pequil(0)) < 5e-2);
    cout << "exp_py    =   " << exp_py << " ?= " << pequil(1) << '\n';
    assert(abs((exp_py - pequil(1)) / pequil(1)) < 5e-2);
    cout << "exp_pz    =   " << exp_pz << " ?= " << pequil(2) << '\n';
    assert(abs((exp_pz - pequil(2)) / pequil(2)) < 5e-2);

    cout << "exp_px^2  =   " << exp_pxsq << " ?= " << exp_pxsq_an << '\n';
    assert(abs((exp_pxsq - exp_pxsq_an) / exp_pxsq_an) < 1e-2);
    cout << "exp_py^2  =   " << exp_pysq << " ?= " << exp_pysq_an << '\n';
    assert(abs((exp_pysq - exp_pysq_an) / exp_pysq_an) < 1e-2);
    cout << "exp_pz^2  =   " << exp_pzsq << " ?= " << exp_pzsq_an << '\n';
    assert(abs((exp_pzsq - exp_pzsq_an) / exp_pzsq_an) < 1e-2);

    cout << "exp_E    =   " << exp_E << " ?= " << exp_E_an << '\n';
    assert(abs((exp_E - exp_E_an) / exp_E_an) < 1e-2);
    cout << "Z        =   "
         << (2.0 * dx * exp(-u0 / (kB * T)) / averages["delta(x - x0)"])
         << " ?= " << Z_an << '\n';
    assert(abs( ((pow(2.0 * dx, 3) * exp(-u0 / (kB * T)) / averages["delta(x - x0)"]) -
                Z_an) / Z_an ) < 1e-2);
    cout << "---------------------------------------------\n";

  }

  if(taskid == 0) cout << "3D linear dipole test passed.\n";

  MPI_Finalize();

  return 0;

}


//==============================================================================
//         Copyright 2003 - 2012 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012        MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

#include <nt2/table.hpp>
#include <nt2/include/functions/width.hpp>
#include <nt2/include/functions/rand.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/linalg/details/utility/f77_wrapper.hpp>
#include "../../flops/solve.hpp"
#include "../../flops/lu.hpp"

#include "details.hpp"
#include <magma.h>
#include <boost/fusion/include/at.hpp>

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/metric/absolute_time.hpp>
#include <nt2/sdk/bench/metric/gflops.hpp>
#include <nt2/sdk/bench/protocol/max_duration.hpp>
#include <nt2/sdk/bench/setup/geometric.hpp>
#include <nt2/sdk/bench/setup/constant.hpp>
#include <nt2/sdk/bench/setup/combination.hpp>
#include <nt2/sdk/bench/stats/median.hpp>

using namespace nt2::bench;
using namespace nt2;

template<typename T> struct sgesv_nt2
{
  template<typename Setup>
  sgesv_nt2(Setup const& s)
              :  w(boost::fusion::at_c<1>(s))
              ,  h(boost::fusion::at_c<0>(s))
  {
    a  = nt2::rand(h,w, nt2::meta::as_<T>());
    b  = nt2::rand(h,1, nt2::meta::as_<T>());
    }

  void operator()()
  {
    h1 = h;
    nhrs = w;
    nhrs = nt2::width(b);
    jpvt.resize(nt2::of_size(h1, 1) );

    a1 = a;
    b1 = b;

   magma_sgesv( h1, nhrs, a1.data(), h1, jpvt.data(), b1.data(),h1,&i);
    }

  friend std::ostream& operator<<(std::ostream& os, sgesv_nt2<T> const& p)
  {
    return os << "(" << p.size() << ")";
  }

  std::size_t size() const { return h*w; }
  std::size_t flops() const {
    return ((FLOPS_GETRF(h,w)+ FLOPS_DGETRS(h,1)));
  }

private:
  std::size_t w,h;
  nt2_la_int i,h1,w1,nhrs;
  nt2::table<T> a,a1,b,b1;
  nt2::table<nt2_la_int> jpvt;
};

NT2_REGISTER_BENCHMARK_TPL( sgesv_nt2, (float) )
{
  std::size_t size_min = args("size_min", 16);
  std::size_t size_max = args("size_max", 4096);
  std::size_t size_step = args("size_step", 10);

  run_during_with< sgesv_nt2<T> > ( 1.
                                , and_( geometric(size_min,size_max,size_step)
                                      , geometric(size_min,size_max,size_step)
                                      )
                                , gflops<stats::median_>()
                                );
}

template<typename T> struct dgesv_nt2
{
  template<typename Setup>
  dgesv_nt2(Setup const& s)
              :  w(boost::fusion::at_c<1>(s))
              ,  h(boost::fusion::at_c<0>(s))
  {
    a  = nt2::rand(h,w, nt2::meta::as_<T>());
    b  = nt2::rand(h,1, nt2::meta::as_<T>());
    }

  void operator()()
  {
    h1 = h;
    nhrs = w;
    nhrs = nt2::width(b);
    jpvt.resize(nt2::of_size(h1, 1) );

    a1 = a;
    b1 = b;

    magma_dgesv( h1, nhrs, a1.data(), h1, jpvt.data(), b1.data(),h1,&i);
    }

  friend std::ostream& operator<<(std::ostream& os, dgesv_nt2<T> const& p)
  {
    return os << "(" << p.size() << ")";
  }

  std::size_t size() const { return h*w; }
  std::size_t flops() const {
    return ((FLOPS_GETRF(h,w)+ FLOPS_DGETRS(h,1)));
  }

private:
  std::size_t w,h;
  nt2_la_int i,h1,w1,nhrs;
  nt2::table<T> a,a1,b,b1;
  nt2::table<nt2_la_int> jpvt;
};

NT2_REGISTER_BENCHMARK_TPL( dgesv_nt2, (double) )
{
  std::size_t size_min = args("size_min", 16);
  std::size_t size_max = args("size_max", 4096);
  std::size_t size_step = args("size_step", 10);

  run_during_with< dgesv_nt2<T> > ( 1.
                                , and_( geometric(size_min,size_max,size_step)
                                      , geometric(size_min,size_max,size_step)
                                      )
                                , gflops<stats::median_>()
                                );
}


#ifndef decision_heuristic_hh
#define decision_heuristic_hh

#include <random>
#include <vector>
#include <map>
#include <algorithm>
//#include <dlib/svm.h>
#include "qcdcl.hh"
#include "solver_types.hh"
#include "constraint.hh"

using std::random_device;
using std::bernoulli_distribution;
using std::map;
using std::fill;
using std::min;
using std::max;

namespace Qute {

class DecisionHeuristic {

//typedef map<unsigned long,double> sample_type;
//typedef dlib::sparse_linear_kernel<sample_type> kernel_type;
// typedef dlib::sparse_radial_basis_kernel<sample_type> kernel_type;
//typedef dlib::svm_pegasos<kernel_type> trainer_type;

public:
  DecisionHeuristic(QCDCL_solver& solver);
  virtual ~DecisionHeuristic() {}
  virtual void addVariable(bool auxiliary) = 0;
  virtual void notifyStart() = 0;
  virtual void notifyAssigned(Literal l) = 0;
  virtual void notifyUnassigned(Literal l) = 0;
  virtual void notifyEligible(Variable v) = 0;
  virtual void notifyLearned(Constraint& c, ConstraintType constraint_type, vector<Literal>& conflict_side_literals) = 0;
  virtual void notifyBacktrack(uint32_t decision_level_before) = 0;
  virtual void notifyRestart();
  virtual Literal getDecisionLiteral() = 0;
  virtual void notifyConflict(ConstraintType constraint_type);

  enum class PhaseHeuristicOption: int8_t {INVJW, QTYPE, WATCHER, RANDOM, PHFALSE, PHTRUE};

  void setPhaseHeuristic(PhaseHeuristicOption heuristic);
  bool phaseHeuristic(Variable v);

protected:
  //bool svmPhase(Variable v);
  bool randomPhase();
  bool invJeroslowWang(Variable v);
  bool watcherPhaseHeuristic(Variable v);
  double invJeroslowWangScore(Literal l, ConstraintType constraint_type);
  bool qtypeDecHeur(Variable v);
  int nrLiteralOccurrences(Literal l, ConstraintType constraint_type);

  QCDCL_solver& solver;
  random_device generator;
  bernoulli_distribution distribution;
  PhaseHeuristicOption phase_heuristic;
  vector<lbool> saved_phase;
  /*vector<map<Variable,bool>> assignment_cache;
  vector<bool> label_cache;
  map<Variable,trainer_type> trainer_map;
  map<Variable,uint32_t> last_update;*/
  uint32_t conflict_counter;

};

inline void DecisionHeuristic::setPhaseHeuristic(PhaseHeuristicOption heuristic) {
  phase_heuristic = heuristic;
}

inline bool DecisionHeuristic::randomPhase() {
  return distribution(generator);
}

}

#endif


//==================================================================================================
/*!
  @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_ARCH_COMMON_SIMD_FUNCTION_PREDECESSOR_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_PREDECESSOR_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/function/simd/bitfloating.hpp>
#include <boost/simd/function/simd/bitinteger.hpp>
#include <boost/simd/function/simd/if_allbits_else.hpp>
#include <boost/simd/function/simd/is_gez.hpp>
#include <boost/simd/function/simd/is_less.hpp>
#include <boost/simd/function/simd/is_nan.hpp>
#include <boost/simd/function/simd/is_not_equal.hpp>
#include <boost/simd/function/simd/minusone.hpp>
#include <boost/simd/function/simd/plus.hpp>
#include <boost/simd/function/simd/seladd.hpp>
#include <boost/simd/function/simd/selsub.hpp>
#include <boost/simd/function/simd/subs.hpp>
#include <boost/assert.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(predecessor_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return seladd(neq(a0, Valmin<A0>()), a0, Mone<A0>());
      }
   };

   BOOST_DISPATCH_OVERLOAD(predecessor_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return if_nan_else(is_nan(a0), bitfloating(minusone(bitinteger(a0))));
      }
   };

   BOOST_DISPATCH_OVERLOAD(predecessor_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::integer_<A0>, X>
                          , bs::pack_<bd::integer_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        BOOST_ASSERT_MSG(assert_all(is_gez(a1)), "predecessor rank must be non negative");
        return selsub( is_less_equal(Valmin<A0>()+a1, a0), a0, a1);
      }
   };

   BOOST_DISPATCH_OVERLOAD(predecessor_
                          , (typename A0, typename A1, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::integer_<A1>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const  A1&  a1) const BOOST_NOEXCEPT
      {
        BOOST_ASSERT_MSG(assert_all(is_gez(a1)), "predecessor rank must be non negative");
        return if_allbits_else(is_nan(a0), bitfloating(subs(bitinteger(a0), a1)));
      }
   };

} } }

#endif


/**
 * Validate some passports
 */
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <vector>
#include <sstream>

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/regex.hpp>

class Passport
{
public:
    Passport() :
        byr(0),
        iyr(0),
        eyr(0),
        hgt(0),
        hcl(""),
        ecl(""),
        pid(""),
        cid("")
    {}

    static Passport parse(const std::string &rec) {
        Passport p;

        std::vector<std::string> tokens;
        boost::split(tokens, rec, boost::is_any_of(" "));

        for(std::vector<std::string>::const_iterator it=tokens.begin(); it!=tokens.end(); ++it)
        {
            std::vector<std::string> rectokens;
            boost::split(rectokens, *it, boost::is_any_of(":"));

            if(rectokens.size() != 2)
            {
                continue;
            }

            if(boost::contains(*it, "byr"))
            {
                p.byr = boost::lexical_cast<int>(rectokens[1]);
            }

            else if(boost::contains(*it, "iyr"))
            {
                p.iyr = boost::lexical_cast<int>(rectokens[1]);
            }

            else if(boost::contains(*it, "eyr"))
            {
                p.eyr = boost::lexical_cast<int>(rectokens[1]);
            }

            else if(boost::contains(*it, "hgt"))
            {
                if(boost::contains(rectokens[1], "cm"))
                {
                    std::string hgt = rectokens[1];
                    boost::replace_all(hgt, "cm", "");
                    p.hgt = boost::lexical_cast<int>(hgt);
                }
                else if(boost::contains(rectokens[1], "in"))
                {
                    std::string hgt = rectokens[1];
                    boost::replace_all(hgt, "in", "");
                    int hgt_in = boost::lexical_cast<int>(hgt);
                    p.hgt = hgt_in * 2.54; //  convert inches to cm
                }
            }

            else if(boost::contains(*it, "hcl"))
            {
                p.hcl = rectokens[1];
            }

            else if(boost::contains(*it, "ecl"))
            {
                p.ecl = rectokens[1];
            }

            else if(boost::contains(*it, "pid"))
            {
                p.pid = rectokens[1];
            }

            else if(boost::contains(*it, "cid"))
            {
                p.cid = rectokens[1];
            }
        }

        return p;
    }

    bool isValid() const {
        static boost::regex hcl_regex("#[0-9a-f]{6}");
        static boost::regex ecl_regex("amb|blu|brn|gry|grn|hzl|oth");
        static boost::regex pid_regex("[0-9]{9}");

        return byr >= 1920 && byr <= 2002 &&
               iyr >= 2010 && iyr <= 2020 &&
               eyr >= 2020 && eyr <= 2030 &&
               hgt >= 150 && hgt <= 193 &&
               boost::regex_match(hcl, hcl_regex) &&
               boost::regex_match(ecl, ecl_regex) &&
               boost::regex_match(pid, pid_regex);
    }

    int byr;
    int iyr;
    int eyr;
    int hgt;
    std::string hcl;
    std::string ecl;
    std::string pid;
    std::string cid;

    int birthYear;
};

void usage()
{
    std::cout << "day4 <input.txt>" << std::endl;
}

int main(int argc, char *argv[])
{
    if(argc != 2)
    {
        usage();
        return EXIT_FAILURE;
    }

    std::vector<int> numbers;

    std::ifstream ifs(argv[1], std::ifstream::in);
    std::string line;
    std::string rec;
    std::vector<Passport> passports;
    int validPassports = 0;
    while(std::getline(ifs, line))
    {
        rec.append(line);
        rec.append(" ");

        //  Treat an empty like as a record separator
        if(line.length() == 0)
        {
            //  We've accumulated a full passport record, parse it
            std::cout << "record: " << rec << std::endl;
            Passport p = Passport::parse(rec);
            passports.push_back(p);
            rec.clear();

            if(p.isValid())
            {
                std::cout << "valid" << std::endl << std::endl;
                validPassports++;
            }
            else
            {
                std::cout << "invalid" << std::endl << std::endl;
            }
        }
    }

    if(ifs.eof())
    {
        Passport p = Passport::parse(rec);
        passports.push_back(p);

        if(p.isValid())
        {
            std::cout << "valid" << std::endl;
            validPassports++;
        }
        else
        {
            std::cout << "invalid" << std::endl;
        }
    }

    std::cout << "read " << passports.size() << " passports" << std::endl;

    std::cout << validPassports << " valid" << std::endl;

    ifs.close();
}


#include <iostream>
#include <fstream>
#include <stack>
#include <vector>
#include <typeinfo>
#include <queue>

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

#include <ta-lib/ta_libc.h>

#include "../util/util_func.h"
#include "macd.h"

int main(){

    std::ifstream f;
    f.open("close.csv", std::ios::in);
    if(!f.is_open()){
        std::cout << "open failed" << std::endl;
        return -1;
    }

    std::string line;
    int count = 0;

    std::vector<std::string> v;
    std::queue<double> close_q;

    while(std::getline(f, line)){
        if(count == 0) {
            count = count + 1;
            continue;
        }
        boost::algorithm::split(v, line, boost::is_any_of(","));
        std::cout << v[0] << "  " << v[1] << "  " << v[2] << std::endl;
        v.clear();
        close_q.push(boost::lexical_cast<double>(v[2]));
    }
    int SIZE = close_q.size();
    std::vector<double> close;
    for(int i = 0; i < SIZE ; i++){
        close.push_back(close_q.front());
        close_q.pop();
    }

    for( int i = 0; i < SIZE; i++){
        int a = macd_calulate(close, 0, i);
        std::cout << "i:  " << i << "  "<< a << std::endl;
    }
    std::vector<double> a;
    load_history(a);
    std::cout << a[0] <<"   " <<  a[1] << std::endl;

}


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

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

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

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

#include <qle/models/cpicapfloorhelper.hpp>

#include <ql/quotes/simplequote.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {

CpiCapFloorHelper::CpiCapFloorHelper(Option::Type type, Real baseCPI, const Date& maturity, const Calendar& fixCalendar,
                                     BusinessDayConvention fixConvention, const Calendar& payCalendar,
                                     BusinessDayConvention payConvention, Real strike,
                                     const Handle<ZeroInflationIndex>& infIndex, const Period& observationLag,
                                     Real marketPremium, CPI::InterpolationType observationInterpolation,
                                     BlackCalibrationHelper::CalibrationErrorType errorType)
    : BlackCalibrationHelper(Handle<Quote>(boost::make_shared<SimpleQuote>(0.0)), Handle<YieldTermStructure>(),
                             errorType),
      // start date does not really matter ?
      instrument_(boost::shared_ptr<CPICapFloor>(new CPICapFloor(
          type, 1.0, Settings::instance().evaluationDate(), baseCPI, maturity, fixCalendar, fixConvention, payCalendar,
          payConvention, strike, infIndex, observationLag, observationInterpolation))) {
    QL_REQUIRE(errorType == BlackCalibrationHelper::PriceError ||
                   errorType == BlackCalibrationHelper::RelativePriceError,
               "CpiCapFloorHelper supports only PriceError and "
               "RelativePriceError error types");
    QL_REQUIRE(marketPremium > 0.0 && !close_enough(marketPremium, 0.0),
               "can not calibrate to market premium " << marketPremium);
    marketValue_ = marketPremium;
}

Real CpiCapFloorHelper::modelValue() const {
    calculate();
    instrument_->setPricingEngine(engine_);
    return instrument_->NPV();
}

Real CpiCapFloorHelper::blackPrice(Volatility) const {
    calculate();
    return marketValue_;
}

} // namespace QuantExt


#ifndef CLASSES_BINNEDTYPEDMATRIX_HH_
#define CLASSES_BINNEDTYPEDMATRIX_HH_

#include "structs.hh"

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

#include <vector>
#include <fstream>

namespace EMC {

extern std::map<std::string, matrixType> matrixTypeMap;

struct BinnedTypedMatrix {
	inline BinnedTypedMatrix(std::vector<double> rowIndex,
			std::vector<double> columnIndex, matrixType matType) :
			matType(matType),
			rowCount(matType == mtDensity ?
				rowIndex.size() - 1 : rowIndex.size()),
			columnCount(matType == mtDensity ?
				columnIndex.size() - 1 : columnIndex.size()),
			rowIndex(rowIndex), columnIndex(columnIndex), m(rowCount, columnCount) {
		std::cout << "creating BinnedTypedMatrix with " << rowCount << "x" << columnCount << std::endl;
	}

	matrixType matType;
	unsigned int rowCount;
	unsigned int columnCount;
	std::vector<double> rowIndex; // in case of density these are borders
	std::vector<double> columnIndex; // in case of density these are borders
	boost::numeric::ublas::matrix<ValueError> m;

	static BinnedTypedMatrix readFromFile(std::ifstream& inFile);
	BinnedTypedMatrix makeProbability(int countRate);
	void print();

	void writeToFile(std::ofstream& outFile);
};

} /* namespace EMC */

#endif /* CLASSES_BINNEDTYPEDMATRIX_HH_ */


#include "ShortestEdgeCollapse.h"
#include <wmtk/TriMesh.h>
#include <wmtk/utils/VectorUtils.h>
#include <wmtk/ExecutionScheduler.hpp>
#include <wmtk/utils/TupleUtils.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace wmtk;
using namespace sec;

bool sec::ShortestEdgeCollapse::invariants(const std::vector<Tuple>& new_tris)
{
    if (m_has_envelope) {
        for (auto& t : new_tris) {
            std::array<Eigen::Vector3d, 3> tris;
            auto vs = t.oriented_tri_vertices(*this);
            for (auto j = 0; j < 3; j++) tris[j] = vertex_attrs[vs[j].vid(*this)].pos;
            bool outside = m_envelope.is_outside(tris);
            if (outside) return false;
        }
    }
    return true;
}

bool sec::ShortestEdgeCollapse::write_triangle_mesh(std::string path)
{
    Eigen::MatrixXd V = Eigen::MatrixXd::Zero(vert_capacity(), 3);
    for (auto& t : get_vertices()) {
        auto i = t.vid(*this);
        V.row(i) = vertex_attrs[i].pos;
    }

    Eigen::MatrixXi F = Eigen::MatrixXi::Constant(tri_capacity(), 3, -1);
    for (auto& t : get_faces()) {
        auto i = t.fid(*this);
        auto vs = oriented_tri_vertices(t);
        for (int j = 0; j < 3; j++) {
            F(i, j) = vs[j].vid(*this);
        }
    }

    return igl::write_triangle_mesh(path, V, F);
}

bool sec::ShortestEdgeCollapse::collapse_edge_before(const Tuple& t)
{
    if (!ConcurrentTriMesh::collapse_edge_before(t)) return false;
    if (vertex_attrs[t.vid(*this)].freeze || vertex_attrs[t.switch_vertex(*this).vid(*this)].freeze)
        return false;
    position_cache.local().v1p = vertex_attrs[t.vid(*this)].pos;
    position_cache.local().v2p = vertex_attrs[t.switch_vertex(*this).vid(*this)].pos;
    return true;
}

bool sec::ShortestEdgeCollapse::collapse_edge_after(const TriMesh::Tuple& t)
{
    const Eigen::Vector3d p = (position_cache.local().v1p + position_cache.local().v2p) / 2.0;
    auto vid = t.vid(*this);
    vertex_attrs[vid].pos = p;

    return true;
}

std::vector<TriMesh::Tuple> sec::ShortestEdgeCollapse::new_edges_after(
    const std::vector<TriMesh::Tuple>& tris) const
{
    std::vector<TriMesh::Tuple> new_edges;
    std::vector<size_t> one_ring_fid;

    for (auto t : tris) {
        for (auto j = 0; j < 3; j++) {
            new_edges.push_back(tuple_from_edge(t.fid(*this), j));
        }
    }
    wmtk::unique_edge_tuples(*this, new_edges);
    return new_edges;
}

bool sec::ShortestEdgeCollapse::collapse_shortest(int target_vert_number)
{
    size_t initial_size = get_vertices().size();
    auto collect_all_ops = std::vector<std::pair<std::string, Tuple>>();
    int starting_num = get_vertices().size();
    for (auto& loc : get_edges()) collect_all_ops.emplace_back("edge_collapse", loc);

    auto renew = [](auto& m, auto op, auto& tris) {
        auto edges = m.new_edges_after(tris);
        auto optup = std::vector<std::pair<std::string, Tuple>>();
        for (auto& e : edges) optup.emplace_back("edge_collapse", e);
        return optup;
    };
    auto measure_len2 = [](auto& m, auto op, const Tuple& new_e) {
        auto len2 =
            (m.vertex_attrs[new_e.vid(m)].pos - m.vertex_attrs[new_e.switch_vertex(m).vid(m)].pos)
                .squaredNorm();
        return -len2;
    };
    auto setup_and_execute = [&](auto executor) {
        executor.num_threads = NUM_THREADS;
        executor.renew_neighbor_tuples = renew;
        executor.priority = measure_len2;
        executor.stopping_criterion_checking_frequency =
            target_vert_number > 0 ? (initial_size - target_vert_number - 1)
                                   : std::numeric_limits<int>::max();
        executor.stopping_criterion = [](auto& m) { return true; };
        executor(*this, collect_all_ops);
    };

    if (NUM_THREADS > 0) {
        auto executor = wmtk::ExecutePass<ShortestEdgeCollapse, ExecutionPolicy::kPartition>();
        executor.lock_vertices = [](auto& m, const auto& e, int task_id) {
            return m.try_set_edge_mutex_two_ring(e, task_id);
        };
        setup_and_execute(executor);
    } else {
        auto executor = wmtk::ExecutePass<ShortestEdgeCollapse, ExecutionPolicy::kSeq>();
        setup_and_execute(executor);
    }
    return true;
}


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <Eigen/Core>

#include <iostream>

double summation(const Eigen::MatrixXd& X, const Eigen::MatrixXd& Y) {
#ifndef _OPENMP
    std::cerr << "Warning: OpenMP is disabled." << std::endl;
#endif
    double s = 0;
    size_t i, j;
#ifdef _OPENMP
#pragma omp parallel for private(j) reduction(+:s)
#endif
    for (i = 0; i < X.rows(); ++i) {
        for (j = 0; j < Y.rows(); ++j) {
            if (0 < X(i) * Y(j)) {
                s += std::log10((X(i) + Y(j)) * (X(i) + Y(j))) + std::sqrt(X(i) * Y(j));
            }
        }
    }
    return s;
}

PYBIND11_MODULE(openmp_cpp, m) {
    m.def("summation", &summation);
}


/// \file nuslam.hpp
/// \brief A package for Extended Kalman Filter Slam implementation.

#ifndef NUSLAM_INCLUDE_GUARD_HPP
#define NUSLAM_INCLUDE_GUARD_HPP

#include "rigid2d/diff_drive.hpp"
#include "rigid2d/rigid2d.hpp"

#include <armadillo>
#include <cmath>
#include <iosfwd>
#include <map>
#include <utility>
#include <vector>

namespace nuslam
{
  using rigid2d::Config2D;
  using rigid2d::Twist2D;

  /// \brief A Mesurement vector Vector
  struct Measurement
  {
    /// \param r - the distance to landmark
    double r;
    /// \param phi - the relative bearing of landmark
    double phi;
    /// \param id - the measured landmark id
    int id;

    /// \brief create zero-measurement
    Measurement();

    /// \brief create a measurement with r,phi inputs
    /// \param x_ - x input of the measurement
    /// \param y_ - y input of the measurement
    /// \param id_ - the measured landmark id
    explicit Measurement(double x_, double y_, int id_);

    /// \brief create a measurement vector
    /// \return the measurement vector
    arma::mat compute_z();
  };

  /// \brief initialize the guess of the robot's state and covariance matrix.
  /// Start with a guess for the robot state (0, 0, 0) and zero covariance matrix.
  class EKF
  {
  private:
    arma::mat q_t = arma::mat(3, 1);
    arma::mat m_t;

    // arma::mat xi = arma::mat(3, 1);
    // arma::mat cov = arma::mat(3, 3);

    arma::mat Q_mat = arma::mat(3, 3);
    arma::mat R_mat = arma::mat(2, 2);

    arma::mat xi_predict = arma::mat(3, 1);
    arma::mat cov_predict = arma::mat(3, 3);

    std::map<int, int> id2landmark;

  public:
    /// \brief initialize the combined state vector.
    /// Start with a guess for the robot state (0, 0, 0) and zero e map state.
    EKF();

    /// \brief updates the landmarks matrix and landmark covariance.
    /// \param meas - the measured landmark.
    void add_new_measurement(const Measurement &meas);

    /// \brief check if the measured landmark exists in the landmark dictionary.
    /// \param landmark_id - the id of the measured landmark.
    /// \return bool - is the measured exists?
    bool check_landmarks(const int landmark_id);

    /// \brief updates the landmarks matrix and landmark covariance.
    /// \param meas - the measured landmark.
    void update_landmark(const Measurement &meas);

    /// \brief gets the state q_t.
    /// \param twist - the twist of the robot.
    /// \return the updated state q_t
    arma::mat get_new_state(const Twist2D &twist);

    /// \brief gets derivative of g with respect to the state ξ.
    /// \param twist - the twist of the robot.
    arma::mat get_transition(const Twist2D &twist);

    /// \brief predicts the next step - finds the estimated state and covariance.
    /// \param twist - the twist of the robot.
    void predict(const Twist2D &twist);

    /// \brief compute the measurement h for range and bearing to landmark.
    /// \param index - the index of the measured landmark in the landmark matrix.
    /// \return the h matrix.
    arma::mat get_h(int index);

    /// \brief compute the derivative of h with respect to the state.
    /// \param index - the index of the measured landmark in the landmark matrix.
    /// \return the H matrix.
    arma::mat get_H(int index);

    /// \brief updates the next step.
    /// \param meas - the measured landmark.
    void update(std::vector<Measurement> meas);

    /// \brief add a new zero row and column to a matrix (used when adding new
    /// landmark). \param mat - the current matrix. \return the updated matrix.
    arma::mat update_matrix_size(arma::mat mat);

    /// \brief run the Extended Kalman Filter algorithm.
    /// \param twist - the twist of the robot.
    /// \param meas -  the current measurement.
    void run_ekf(const Twist2D &twist, const std::vector<Measurement> &meas);

    /// \brief output the robot state
    /// \return the robot state
    arma::mat output_state();

    /// \brief output the map state
    /// \return the map state
    arma::mat output_map_state();
  };
} // namespace nuslam

#endif


//
//  matrix_boost.hpp
//  jerome
//
//  Created by Anton Leuski on 5/13/16.
//  Copyright © 2016 Anton Leuski & ICT/USC. All rights reserved.
//
//  This file is part of Jerome.
//
//  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 __jerome_type_matrix_boost_hpp__
#define __jerome_type_matrix_boost_hpp__

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wshorten-64-to-32"

#define BOOST_UBLAS_MOVE_SEMANTICS

#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/range/algorithm/max_element.hpp>

#pragma clang diagnostic pop

namespace boost {
  namespace numeric {
    namespace ublas {

      // (op v) [i] = op( v [i] )
      template<class OP, class E>
      BOOST_UBLAS_INLINE
      typename boost::numeric::ublas::vector_unary_traits<E, OP>::result_type
      apply_to_all (const boost::numeric::ublas::vector_expression<E> &e, const OP& op = OP() ) {
        typedef typename boost::numeric::ublas::vector_unary_traits<E, OP>::expression_type expression_type;
        return expression_type (e ());
      }

      template<class T>
      struct scalar_log:
      public scalar_unary_functor<T> {
        typedef typename scalar_unary_functor<T>::argument_type argument_type;
        typedef typename scalar_unary_functor<T>::result_type result_type;

        static BOOST_UBLAS_INLINE
        result_type apply (argument_type t) {
          return std::log(t);
        }
      };

      template<class T>
      struct scalar_log_plus1:
      public scalar_unary_functor<T> {
        typedef typename scalar_unary_functor<T>::argument_type argument_type;
        typedef typename scalar_unary_functor<T>::result_type result_type;

        static BOOST_UBLAS_INLINE
        result_type apply (argument_type t) {
          return std::log(t+1.0);
        }
      };

      template<class T>
      struct scalar_exp:
      public scalar_unary_functor<T> {
        typedef typename scalar_unary_functor<T>::argument_type argument_type;
        typedef typename scalar_unary_functor<T>::result_type result_type;

        static BOOST_UBLAS_INLINE
        result_type apply (argument_type t) {
          return std::exp(t);
        }
      };

      template<class T1, class T2>
      struct scalar_pow:
      public scalar_binary_functor<T1, T2> {
        typedef typename scalar_binary_functor<T1, T2>::argument1_type argument1_type;
        typedef typename scalar_binary_functor<T1, T2>::argument2_type argument2_type;
        typedef typename scalar_binary_functor<T1, T2>::result_type result_type;

        static BOOST_UBLAS_INLINE
        result_type apply (argument1_type t1, argument2_type t2) {
          return std::pow((double)t1,(double)t2);
        }
      };

      // (log v) [i] = log (v [i])
      template<class E>
      BOOST_UBLAS_INLINE
      typename vector_unary_traits<E, scalar_log<typename E::value_type> >::result_type
      log (const vector_expression<E> &e) {
        typedef typename vector_unary_traits<E, scalar_log<typename E::value_type> >::expression_type expression_type;
        return expression_type (e ());
      }

      // (log v) [i] = log (v [i])
      template<class E>
      BOOST_UBLAS_INLINE
      typename vector_unary_traits<E, scalar_log_plus1<typename E::value_type> >::result_type
      log_plus1 (const vector_expression<E> &e) {
        typedef typename vector_unary_traits<E, scalar_log_plus1<typename E::value_type> >::expression_type expression_type;
        return expression_type (e ());
      }

      // (exp v) [i] = exp (v [i])
      template<class E>
      BOOST_UBLAS_INLINE
      typename vector_unary_traits<E, scalar_exp<typename E::value_type> >::result_type
      exp (const vector_expression<E> &e) {
        typedef typename vector_unary_traits<E, scalar_exp<typename E::value_type> >::expression_type expression_type;
        return expression_type (e ());
      }

      // (log m) [i] [j] = log (m [i] [j])
      template<class E>
      BOOST_UBLAS_INLINE
      typename matrix_unary1_traits<E, scalar_log<typename E::value_type> >::result_type
      log (const matrix_expression<E> &e) {
        typedef typename matrix_unary1_traits<E, scalar_log<typename E::value_type> >::expression_type expression_type;
        return expression_type (e ());
      }

      // (exp m) [i] [j] = exp (m [i] [j])
      template<class E>
      BOOST_UBLAS_INLINE
      typename matrix_unary1_traits<E, scalar_exp<typename E::value_type> >::result_type
      exp (const matrix_expression<E> &e) {
        typedef typename matrix_unary1_traits<E, scalar_exp<typename E::value_type> >::expression_type expression_type;
        return expression_type (e ());
      }

      // (pow(v, t) ) [i] = pow(v [i], t)
      template<class E1, class T2>
      BOOST_UBLAS_INLINE
      typename enable_if< is_convertible<T2, typename E1::value_type >,
      typename vector_binary_scalar2_traits<E1, const T2, scalar_pow<typename E1::value_type, T2> >::result_type
      >::type
      element_pow (const vector_expression<E1> &e1,
                   const T2 &e2) {
        typedef typename vector_binary_scalar2_traits<E1, const T2, scalar_pow<typename E1::value_type, T2> >::expression_type expression_type;
        return expression_type (e1 (), e2);
      }

      // (pow(m, t)) [i] [j] = pow(m [i] [j], t)
      template<class E1, class T2>
      BOOST_UBLAS_INLINE
      typename enable_if< is_convertible<T2, typename E1::value_type>,
      typename matrix_binary_scalar2_traits<E1, const T2, scalar_pow<typename E1::value_type, T2> >::result_type
      >::type
      element_pow (const matrix_expression<E1> &e1,
                   const T2 &e2) {
        typedef typename matrix_binary_scalar2_traits<E1, const T2, scalar_pow<typename E1::value_type, T2> >::expression_type expression_type;
        return expression_type (e1 (), e2);
      }

      template<class M>
      BOOST_UBLAS_INLINE
      matrix_vector_slice<M> diag(M &data) {
        const typename M::size_type s = std::min(data.size1(),data.size2());
        return matrix_vector_slice<M>(data, slice(0,1,s), slice(0,1,s));
      };

      template<class M>
      BOOST_UBLAS_INLINE
      const matrix_vector_slice<const M> diag(const M &data) {
        const typename M::size_type s = std::min(data.size1(),data.size2());
        return matrix_vector_slice<const M>(data, slice(0,1,s), slice(0,1,s));
      };

    }
  }
}

namespace jerome {

  namespace ublas = boost::numeric::ublas;

  template<class T> using Vector = ublas::vector<T>;
  // sparse array
  template<class T> using SparseVector = ublas::compressed_vector<T>;

  typedef ublas::matrix<WeightValue, ublas::column_major>     WeightMatrix;
  typedef ublas::matrix_row<WeightMatrix>                     WeightMatrixRow;
  typedef ublas::matrix_row<const WeightMatrix>               WeightMatrixConstRow;
  typedef ublas::matrix_column<WeightMatrix>                  WeightMatrixColumn;
  typedef ublas::matrix_column<const WeightMatrix>            WeightMatrixConstColumn;
  typedef ublas::vector<WeightMatrix::value_type>             WeightVector;
  typedef ublas::compressed_vector<WeightMatrix::value_type>	SparseWeightVector;

  typedef ublas::symmetric_matrix<WeightMatrix::value_type, ublas::upper>			SymmetricWeightMatrix;
  typedef ublas::matrix_range<SymmetricWeightMatrix>					SymmetricWeightMatrixRange;

  template <class M>
  struct traits {
    typedef typename M::size_type size_type;
    typedef typename M::value_type value_type;
  };

  struct MatrixSize {
    traits<WeightMatrix>::size_type rowCount;
    traits<WeightMatrix>::size_type columnCount;

    MatrixSize() = default;

    MatrixSize(const WeightMatrix& m)
    : rowCount(m.size1()), columnCount(m.size2())
    {}
  };

  typedef ublas::scalar_matrix<WeightMatrix::value_type>
  WeightMatrixScalar;

  typedef ublas::scalar_vector<WeightMatrix::value_type>
  WeightVectorScalar;

  inline WeightMatrixScalar WeightMatrixZero(const MatrixSize& size)
  {
    return WeightMatrixScalar(size.rowCount, size.columnCount, 0);
  }

  inline WeightMatrixScalar WeightMatrixOnes(const MatrixSize& size)
  {
    return WeightMatrixScalar(size.rowCount, size.columnCount, 1);
  }

  inline WeightVectorScalar WeightVectorZero(traits<WeightVector>::size_type size)
  {
    return WeightVectorScalar(size, 0);
  }

  inline WeightVectorScalar WeightVectorOnes(traits<WeightVector>::size_type size)
  {
    return WeightVectorScalar(size, 1);
  }

  template <typename M, typename C>
  inline auto column(M&& m, C c)
  -> decltype(ublas::matrix_column<typename std::remove_reference<M>::type>(std::forward<M>(m), c))
  {
    return ublas::matrix_column<typename std::remove_reference<M>::type>(std::forward<M>(m), c);
  }

  template <typename M, typename C>
  inline auto row(M&& m, C c)
  -> decltype(ublas::matrix_row<typename std::remove_reference<M>::type>(std::forward<M>(m), c))
  {
    return ublas::matrix_row<typename std::remove_reference<M>::type>(std::forward<M>(m), c);
  }

  template <typename M>
  inline auto sum(const M& m)
  -> decltype(ublas::sum(m))
  {
    return ublas::sum(m);
  }

  template <typename A, typename B>
  inline auto prod(const A& a, const B& b)
  -> decltype(ublas::prod(a, b))
  {
    return ublas::prod(a, b);
  }

  template <typename A, typename B>
  inline auto outer_prod(const A& a, const B& b)
  -> decltype(ublas::outer_prod(a, b))
  {
    return ublas::outer_prod(a, b);
  }

  template <typename A, typename B>
  inline auto element_pow(const A& a, B b)
  -> decltype(ublas::element_pow(a, b))
  {
    return ublas::element_pow(a, b);
  }

  template <typename A, typename B>
  inline auto element_prod(const A& a, const B& b)
  -> decltype(ublas::element_prod(a, b))
  {
    return ublas::element_prod(a, b);
  }

  template <typename M>
  inline SparseWeightVector sparse_log_plus1(const M& m)
  {
    SparseWeightVector result(m.size());
    for (auto a_it = m.begin(), a_end = m.end(); a_it != a_end; ++a_it) {
      result(a_it.index()) = std::log(1.0+ (*a_it));
    }

    return result;
  }

  template <typename M>
  inline auto max_element(const M& m)
  -> decltype(*boost::max_element(m))
  {
    return *boost::max_element(m);
  }

  template <typename M, typename S>
  inline void resize(M&& m, const S& s)
  {
    m.resize(s);
  }

  template <typename V>
  inline void append_vector_to_vector(const V& src, V&& dst, const typename traits<V>::size_type& off) {
    for(auto i = src.begin(), e = src.end(); i != e; ++i) {
      dst[i.index()+off] = *i;
    }
  }

  template <typename V>
  inline void append_sparse_vector_to_sparse_vector(const V& src, V&& dst,
                                                    const typename traits<V>::size_type& off) {
    for(auto i = src.begin(), e = src.end(); i != e; ++i) {
      dst[i.index()+off] = *i;
    }
  }

  template <typename X, typename I, typename V>
  inline auto set_value_at_index_in_vector(const X& x, const I& i, V&& v)
  -> decltype(v[i])
  {
    return v[i] = x;
  }

  template <typename I, typename V>
  inline auto increment_value_at_index_in_vector(const I& i, V&& v)
  -> decltype(v[i])
  {
    return v[i] += 1;
  }

  template <typename T, typename M>
  inline auto matrix_cast(M&& m)
  -> decltype(m)
  {
    return m;
  }

  template <typename V, typename OP>
  inline void for_each(const V& v, OP&& op) {
    for (auto E = v.begin(), __end = v.end(); E != __end; ++E) {
      op(E.index(), *E);
    }
  }

  // dense vectors only!!!!
  template <typename V, typename Q>
  inline auto element_div(const V& v, const Q& q)
  -> decltype(ublas::element_div(v, q))
  {
    // ubleas::element_div causes divide by zero. I guess, it tries to compute
    // 1/|Dl| for each values v even for 0 ones....
    // return element_div(term.tfs() * lambda().value(),
    // field.documentLengths());
    return ublas::element_div(v, q);
  }

  template <typename A, typename B, typename C>
  inline void sparse_outer_prod_add_to(const A& a, const B& b, C&& c) {
    for (auto a_it = a.begin(), a_end = a.end(); a_it != a_end; ++a_it) {
      const auto a_index = a_it.index();
      const auto a_value = *a_it;
      for (auto b_it = b.begin(), b_end = b.end(); b_it != b_end; ++b_it) {
        c(a_index, b_it.index()) += a_value * (*b_it);
      }
    }
  }

  template <typename A, typename B, typename C>
  inline void sparse_scale_add_to(const A& a, const B& b, C&& c) {
    for (auto a_it = a.begin(), a_end = a.end(); a_it != a_end; ++a_it) {
      c(a_it.index()) += (*a_it) * b;
    }
  }

  // boost UBLAS is horrible at taking advantage of sparse vectors.
  // Every element-wise operation you apply to sparse vectors is applied
  // to every vector element including zero ones. So we have to implement
  // them ourselves.
  template <typename A, typename B, typename C>
  inline SparseWeightVector sparse_scale_div_by(const A& a, const B& b, const C& c) {
    SparseWeightVector result(a.size());
    for (auto a_it = a.begin(), a_end = a.end(); a_it != a_end; ++a_it) {
      result(a_it.index()) = (*a_it) * b / c(a_it.index());
    }
    return result;
  }

}

namespace boost { namespace numeric { namespace ublas {

  std::ostream& operator << (std::ostream& outs, const jerome::WeightMatrix& obj);
  std::ostream& operator << (std::ostream& outs, const jerome::SymmetricWeightMatrix& obj);
  std::ostream& operator << (std::ostream& outs, const jerome::SymmetricWeightMatrixRange& obj);
  std::ostream& operator << (std::ostream& outs, const jerome::WeightVector& obj);

  // fascinating shit: when looking for an overloaded operator, compiler looks into the
  // namespace of the object parameter. Aparently, if you have a typedef, the compiler
  // ignores the namespace of the typedef and goes directly for the namespace of the class used
  // in the typedef. in this case it's boost:numeric::ublas.

  // if you do not put the operator in the namespace, the compiler will not find it.
}}}

#endif // __jerome_type_matrix_boost_hpp__


#include <catch2/catch.hpp>
#include <Eigen/Dense>
#include <utility> //move

#include <ssme/parameters.h>

//class Param_Pack_Fixture
//{
//public:
//
//    Eigen::Matrix<double,NP,1> m_trans_params;
//    Eigen::Matrix<double,NP,1> m_un_trans_params;
//
//    Param_Pack_Fixture()
//    {
//        m_trans_params(0) = 1.0; m_un_trans_params(0) = 1.0;
//        m_trans_params(1) = -1.3; m_un_trans_params(1) = std::exp(-1.3); //log
//        m_trans_params(2) = 9.5; m_un_trans_params(2) = std::exp(9.5) / (1.0 + std::exp(9.5)); // logit
//        m_trans_params(3) = .89; m_un_trans_params(3) = (1.0 - std::exp(.89)) / ( -1.0 - std::exp(.89) ); // twice fisher
//    }
//};

//TEST_CASE("test transform container", "[transform_container]"){
//
//    // default ctor
//    param::transform_container<double, 2> ts;
//    REQUIRE( ts.size() == 0);
//    REQUIRE( ts.capacity() == 2);
//
//    // add a transform and test to make sure it works
//    ts.add_transform(param::trans_type::TT_logit);
//    REQUIRE( ts.size() == 1);
//    REQUIRE( ts.capacity() == 2);
//    REQUIRE( std::abs(ts.get_transforms()[0]->trans(.5)) < .0001 ); // logit(.5) == 0
//
//    // copy ctor
//    param::transform_container<double, 2> ts2 = ts;
//    REQUIRE( ts2.size() == 1);
//    REQUIRE( ts2.capacity() == 2);
//    REQUIRE( std::abs(ts2.get_transforms()[0]->trans(.5)) < .0001 ); // logit(.5) == 0
//
//    // assignment operator
//    ts2.add_transform(param::trans_type::TT_log);
//    ts = ts2;
//    REQUIRE( ts.size() == 2);
//    REQUIRE( ts2.size() == 2);
//    REQUIRE( ts.capacity() == 2);
//    REQUIRE( ts2.capacity() == 2);
//    REQUIRE( std::abs(ts.get_transforms()[1]->trans(1.0)) < .001 ); // log(1) == 0
//
//}

TEST_CASE("test constructors", "[pack]"){

    // 1
    // default ctor
    // size() should be 0, and everything should be a nullptr
    param::pack<double,4> p1;
//    REQUIRE( p1.size() == 0);
//    REQUIRE( p1.capacity() == 4);
//    REQUIRE_THROWS( p1.get_trans_params() );
//    REQUIRE_THROWS( p1.get_untrans_params());
//    REQUIRE_THROWS( p1.get_log_jacobian() );
//
//    // 2
//    // another constructor that fills everything up
//    Eigen::Matrix<double,4,1> trans_params;
//    trans_params << 1.0, -1.3, 9.5, .89;
//    std::vector<std::string> ts {"null", "log", "logit", "twice_fisher"};
//    param::pack<double,4> p2(trans_params, ts, true);
//    REQUIRE( p2.size() == 4);
//    REQUIRE( p2.capacity() == 4);
////    REQUIRE_THROWS( p1.get_trans_params() );
////    REQUIRE_THROWS( p1.get_untrans_params());
////    REQUIRE_THROWS( p1.get_log_jacobian() );
//
//    // 3 copy ctor
//    param::pack<double,4> p3(p2);
//    param::pack<double,4> p4(p1);
//    REQUIRE( p3.size() == 4);
//    REQUIRE( p4.size() == 0);
//    REQUIRE( p4.capacity() == p3.capacity());

}

TEST_CASE("test assignment", "[pack]"){

    std::vector<std::string> ts {"null", "log", "logit", "twice_fisher"};
    Eigen::Matrix<double,4,1> trans_params;
    trans_params << 1.0, -1.3, 9.5, .89;
    param::pack<double,4> pp1(trans_params, ts);

    // test assignment operator
    Eigen::Matrix<double,4,1> new_trans_params;
    new_trans_params << 1.0, 1.0, 1.0, 1.0;
    param::pack<double,4> pp2(new_trans_params, ts);
    pp1 = pp2;
    for(size_t i = 0; i < 4; ++i)
        REQUIRE( std::abs(1.0 - pp1.get_trans_params()(i)) < .00001);

}

TEST_CASE("test transformations", "[pack]"){

    Eigen::Matrix<double,4,1> ideal_un_trans_params = {1.0, 0.2725318, 0.9999252, 0.4177803};
    Eigen::Matrix<double,4,1> trans_params;
    trans_params << 1.0, -1.3, 9.5, .89;
    std::vector<std::string> ts {"null", "log", "logit", "twice_fisher"};
    param::pack<double,4> pp(trans_params, ts);
    for(size_t i = 0; i < 4; ++i)
        REQUIRE( std::abs(ideal_un_trans_params(i) - pp.get_untrans_params()(i)) < .0001);
}

TEST_CASE("test LogJacobians", "[pack]"){

    Eigen::Matrix<double,4,1> trans_params;
    trans_params << 1.0, -1.3, 9.5, .89;
    std::vector<std::string> ts {"null", "log", "logit", "twice_fisher"};
    param::pack<double,4> pp(trans_params, ts);
    REQUIRE( std::abs(-11.6851 - pp.get_log_jacobian()) < .0001);
    // TODO make this 11.68 calculation more apparent
}

TEST_CASE("test subsetting", "[pack]"){

    std::vector<std::string> ts {"null", "log", "logit", "twice_fisher"};
    Eigen::Matrix<double,4,1> trans_params;
    trans_params << 1.0, -1.3, 9.5, .89;
    param::pack<double,4> pp(trans_params, ts);
    Eigen::Matrix<double,4,1> ideal_un_trans_params = {1.0, 0.2725318, 0.9999252, 0.4177803};

    for(size_t i = 0; i < 4; ++i){
//        std::cout << "trans_params(i): " << trans_params(i)
//                  << "  pp.get_trans_params(i,i)(0): " << pp.get_trans_params(i,i)(0) << "\n";
        REQUIRE( std::abs(trans_params(i) - pp.get_trans_params(i,i)(0)) < .0001);
    }
    for(size_t i = 0; i < 4; ++i)
        REQUIRE( std::abs(ideal_un_trans_params(i) - pp.get_untrans_params(i,i)(0)) < .0001);
}