//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "ParserFlatbuffersSerializeFixture.hpp"
#include "../Deserializer.hpp"

#include <armnn/utility/IgnoreUnused.hpp>

#include <string>
#include <iostream>

BOOST_AUTO_TEST_SUITE(Deserializer)

struct AddFixture : public ParserFlatbuffersSerializeFixture
{
    explicit AddFixture(const std::string & inputShape1,
                        const std::string & inputShape2,
                        const std::string & outputShape,
                        const std::string & dataType,
                        const std::string & activation="NONE")
    {
        armnn::IgnoreUnused(activation);
        m_JsonString = R"(
        {
                inputIds: [0, 1],
                outputIds: [3],
                layers: [
                {
                    layer_type: "InputLayer",
                    layer: {
                          base: {
                                layerBindingId: 0,
                                base: {
                                    index: 0,
                                    layerName: "InputLayer1",
                                    layerType: "Input",
                                    inputSlots: [{
                                        index: 0,
                                        connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                                    }],
                                    outputSlots: [ {
                                        index: 0,
                                        tensorInfo: {
                                            dimensions: )" + inputShape1 + R"(,
                                            dataType: )" + dataType + R"(
                                        },
                                    }],
                                 },}},
                },
                {
                layer_type: "InputLayer",
                layer: {
                       base: {
                            layerBindingId: 1,
                            base: {
                                  index:1,
                                  layerName: "InputLayer2",
                                  layerType: "Input",
                                  inputSlots: [{
                                      index: 0,
                                      connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                                  }],
                                  outputSlots: [ {
                                      index: 0,
                                      tensorInfo: {
                                          dimensions: )" + inputShape2 + R"(,
                                          dataType: )" + dataType + R"(
                                      },
                                  }],
                                },}},
                },
                {
                layer_type: "AdditionLayer",
                layer : {
                        base: {
                             index:2,
                             layerName: "AdditionLayer",
                             layerType: "Addition",
                             inputSlots: [
                                            {
                                             index: 0,
                                             connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                                            },
                                            {
                                             index: 1,
                                             connection: {sourceLayerIndex:1, outputSlotIndex:0 },
                                            }
                             ],
                             outputSlots: [ {
                                 index: 0,
                                 tensorInfo: {
                                     dimensions: )" + outputShape + R"(,
                                     dataType: )" + dataType + R"(
                                 },
                             }],
                            }},
                },
                {
                layer_type: "OutputLayer",
                layer: {
                        base:{
                              layerBindingId: 0,
                              base: {
                                    index: 3,
                                    layerName: "OutputLayer",
                                    layerType: "Output",
                                    inputSlots: [{
                                        index: 0,
                                        connection: {sourceLayerIndex:2, outputSlotIndex:0 },
                                    }],
                                    outputSlots: [ {
                                        index: 0,
                                        tensorInfo: {
                                            dimensions: )" + outputShape + R"(,
                                            dataType: )" + dataType + R"(
                                        },
                                }],
                            }}},
                }]
         }
        )";
        Setup();
    }
};

struct SimpleAddFixture : AddFixture
{
    SimpleAddFixture() : AddFixture("[ 2, 2 ]",
                                    "[ 2, 2 ]",
                                    "[ 2, 2 ]",
                                    "QuantisedAsymm8") {}
};

struct SimpleAddFixture2 : AddFixture
{
    SimpleAddFixture2() : AddFixture("[ 2, 2, 1, 1 ]",
                                     "[ 2, 2, 1, 1 ]",
                                     "[ 2, 2, 1, 1 ]",
                                     "Float32") {}
};

BOOST_FIXTURE_TEST_CASE(AddQuantisedAsymm8, SimpleAddFixture)
{
  RunTest<2, armnn::DataType::QAsymmU8>(
      0,
      {{"InputLayer1", { 0, 1, 2, 3 }},
      {"InputLayer2", { 4, 5, 6, 7 }}},
      {{"OutputLayer", { 4, 6, 8, 10 }}});
}

BOOST_FIXTURE_TEST_CASE(AddFloat32, SimpleAddFixture2)
{
    RunTest<4, armnn::DataType::Float32>(
    0,
    {{"InputLayer1", { 111, 85, 226, 3 }},
    {"InputLayer2", {   5,   8,  10, 12 }}},
    {{"OutputLayer", { 116, 93, 236, 15 }}});
}

BOOST_AUTO_TEST_SUITE_END()


#include "gtest/gtest.h"

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

#include "cnn/cnn.h"
#include "util/serialization-util.h"

using namespace cnn;
using namespace neuralccg;

void test_equal(const Tensor &t1, const Tensor &t2) {
  ASSERT_EQ(t1.d, t2.d);
  for (unsigned i = 0; i < t1.d.size(); ++i) {
    EXPECT_EQ(t1.v[i], t2.v[i]);
  }
}

void test_equal(const Parameters &t1, const Parameters &t2) {
  ASSERT_EQ(t1.dim, t2.dim);
  test_equal(t1.values, t2.values);
}

void test_equal(const LookupParameters &t1, const LookupParameters &t2) {
  ASSERT_EQ(t1.dim, t2.dim);
  ASSERT_EQ(t1.values.size(), t2.values.size());
  for (unsigned i = 0; i < t1.values.size(); ++i) {
    test_equal(t1.values[i], t2.values[i]);
  }
}

void test_equal(const Model &m1, const Model &m2) {
  ASSERT_EQ(m1.parameters_list().size(), m2.parameters_list().size());
  ASSERT_EQ(m1.lookup_parameters_list().size(), m2.lookup_parameters_list().size());
  ASSERT_EQ(m1.all_parameters_list().size(), m2.all_parameters_list().size());
  for (unsigned i = 0; i < m1.parameters_list().size(); ++i) {
    test_equal(*m1.parameters_list()[i], *m2.parameters_list()[i]);
  }
  for (unsigned i = 0; i < m1.lookup_parameters_list().size(); ++i) {
    test_equal(*m1.lookup_parameters_list()[i], *m2.lookup_parameters_list()[i]);
  }
  for (unsigned i = 0; i < m1.all_parameters_list().size(); ++i) {
    Parameters *m1_params = dynamic_cast<Parameters *>(m1.all_parameters_list()[i]);
    Parameters *m2_params = dynamic_cast<Parameters *>(m2.all_parameters_list()[i]);
    ASSERT_EQ((m1_params == nullptr), (m2_params == nullptr));
    if (m1_params != nullptr) {
      test_equal(*m1_params, *m2_params);
    } else {
      LookupParameters *m1_lookup_params = dynamic_cast<LookupParameters *>(m1.all_parameters_list()[i]);
      LookupParameters *m2_lookup_params = dynamic_cast<LookupParameters *>(m2.all_parameters_list()[i]);
      ASSERT_NE(m1_lookup_params, nullptr);
      ASSERT_NE(m2_lookup_params, nullptr);
      test_equal(*m1_lookup_params, *m2_lookup_params);
    }
  }
}

void test_dimensions_serialization(const Dim &in_dim) {
  DimensionsProto dim_proto;
  SerializationUtil::AsDimensionsProto(in_dim, &dim_proto);

  Dim out_dim;
  SerializationUtil::FromDimensionsProto(dim_proto, &out_dim);
  ASSERT_EQ(out_dim, in_dim);
}

void test_tensor_serialization(const Tensor &in_tensor) {
  TensorProto tensor_proto;
  SerializationUtil::AsTensorProto(in_tensor, &tensor_proto);

  Dim out_dim;
  SerializationUtil::FromDimensionsProto(tensor_proto.dimensions(), &out_dim);
  float *values = static_cast<float *>(_mm_malloc(out_dim.size() * sizeof(float), 32));
  Tensor out_tensor(out_dim, values);
  SerializationUtil::FromTensorProto(tensor_proto, &out_tensor);
  test_equal(in_tensor, out_tensor);
  _mm_free(values);
}

void test_model_serialization(const Model &in_model, const Dim &params_dim, const Dim &lookup_params_dim, int num_lookups) {
  ModelProto model_proto;
  SerializationUtil::AsModelProto(in_model, &model_proto);

  Model out_model;
  out_model.add_parameters(params_dim);
  out_model.add_lookup_parameters(num_lookups, lookup_params_dim);
  SerializationUtil::FromModelProto(model_proto, &out_model);
  test_equal(in_model, out_model);
}

void do_testing() {
  Dim test_dim({500, 100});
  test_dimensions_serialization(test_dim);
  std::cerr << "Tested (de)serializing dimensions." << std::endl;

  float *values = static_cast<float *>(_mm_malloc(test_dim.size() * sizeof(float), 32));
  ASSERT_NE(values, nullptr);
  values[0] = 0;
  for (unsigned i = 0; i < test_dim.size(); ++i) {
    values[i] = static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
  }
  Tensor test_tensor(test_dim, values);
  test_tensor_serialization(test_tensor);
  std::cerr << "Tested (de)serializing tensor." << std::endl;
  _mm_free(values);

  Dim params_dim({256, 128});
  Dim lookup_params_dim({128, 128});
  int num_lookups = 100;

  Model test_model;
  test_model.add_parameters(params_dim);
  test_model.add_lookup_parameters(num_lookups, lookup_params_dim);
  test_model_serialization(test_model, params_dim, lookup_params_dim, num_lookups);
  std::cerr << "Tested (de)serializing model." << std::endl;
}

void do_benchmarking() {
  Dim params_dim({1024, 512});
  Dim lookup_params_dim({128});
  int lookup_params_size = 2000;

  Model model;
  model.add_parameters(params_dim);
  model.add_lookup_parameters(lookup_params_size, lookup_params_dim);

  std::string boost_file = "model.ser";
  std::string protobuf_file = "model.pb";

  std::chrono::high_resolution_clock::time_point tic, toc;

  tic = std::chrono::high_resolution_clock::now();
  ModelProto output_model_proto;
  SerializationUtil::AsModelProto(model, &output_model_proto);
  std::fstream protobuf_out(protobuf_file, std::ios::out | std::ios::trunc | std::ios::binary);
  ASSERT_TRUE(output_model_proto.SerializeToOstream(&protobuf_out));
  protobuf_out.close();
  toc = std::chrono::high_resolution_clock::now();
  std::cerr << std::chrono::duration_cast<std::chrono::milliseconds>(toc - tic).count()
      << " ms. to save model with protobuffer." << std::endl;

  tic = std::chrono::high_resolution_clock::now();
  ModelProto input_model_proto;
  std::fstream protobuf_in(protobuf_file, std::ios::in | std::ios::binary);
  ASSERT_TRUE(input_model_proto.ParseFromIstream(&protobuf_in));
  protobuf_in.close();
  Model protobuf_model;
  protobuf_model.add_parameters(params_dim);
  protobuf_model.add_lookup_parameters(lookup_params_size, lookup_params_dim);
  SerializationUtil::FromModelProto(input_model_proto, &protobuf_model);
  toc = std::chrono::high_resolution_clock::now();
  std::cerr << std::chrono::duration_cast<std::chrono::milliseconds>(toc - tic).count()
      << " ms. to load model with protobuffer." << std::endl;

  test_equal(model, protobuf_model);

  tic = std::chrono::high_resolution_clock::now();
  std::ofstream boost_out(boost_file);
  boost::archive::text_oarchive boost_oa(boost_out);
  boost_oa << model;
  boost_out.close();
  toc = std::chrono::high_resolution_clock::now();
  std::cerr << std::chrono::duration_cast<std::chrono::milliseconds>(toc - tic).count()
      << " ms. to save model with boost." << std::endl;

  tic = std::chrono::high_resolution_clock::now();
  Model boost_model;
  boost_model.add_parameters(params_dim);
  boost_model.add_lookup_parameters(lookup_params_size, lookup_params_dim);
  std::ifstream boost_in(boost_file);
  boost::archive::text_iarchive boost_ia(boost_in);
  boost_ia & boost_model;
  boost_in.close();
  toc = std::chrono::high_resolution_clock::now();
  std::cerr << std::chrono::duration_cast<std::chrono::milliseconds>(toc - tic).count()
      << " ms. to load model with boost." << std::endl;
  test_equal(model, boost_model);
}

int main(int argc, char **argv) {
  Initialize(argc, argv);
  do_testing();
  do_benchmarking();
}


#include "strands_movebase/subsample_cloud_nodelet.h"

// this should really be in the implementation (.cpp file)
#include <pluginlib/class_list_macros.h>

#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <boost/thread/thread.hpp>
#include "strands_movebase/noise_approximate_voxel_grid.h"

// watch the capitalization carefully
PLUGINLIB_EXPORT_CLASS(strands_movebase::subsample_cloud_nodelet, nodelet::Nodelet)

namespace strands_movebase {

void subsample_cloud_nodelet::onInit()
{
    NODELET_INFO("Initializing subsample_cloud nodelet...");

    ros::NodeHandle nh = getNodeHandle();
    ros::NodeHandle pn = getPrivateNodeHandle();

    // topic of input cloud
    if (!pn.hasParam("input")) {
        ROS_ERROR("Could not find parameter input.");
    }
    std::string input;
    pn.getParam("input", input);
    NODELET_INFO("With input %s", input.c_str());

    // topic of output cloud
    if (!pn.hasParam("output")) {
        ROS_ERROR("Could not find parameter output.");
    }
    std::string output;
    pn.getParam("output", output);
    NODELET_INFO("And output %s", output.c_str());

    pn.param<double>("resolution", resolution, 0.05);
    pn.param<int>("min_points", min_points, 5);
    pn.param<int>("skip_points", skip_points, 20);

    time = ros::Time::now();

    sub = nh.subscribe(input, 1, &subsample_cloud_nodelet::callback, this);
    pub = nh.advertise<sensor_msgs::PointCloud2>(output, 1);
}

void subsample_cloud_nodelet::callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
    // this will give us a rate of ~5hz
    ros::Time new_time = ros::Time::now();
    if ((new_time - time).toSec() < 0.18) {
        return;
    }
    time = new_time;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::fromROSMsg(*msg, *cloud);
    pcl::PointCloud<pcl::PointXYZ> voxel_cloud;

    noise_approximate_voxel_grid sor(min_points, skip_points); // a bit faster than nvg, not as accurate
    sor.setInputCloud(cloud);
    sor.setLeafSize(resolution, resolution, resolution);
    sor.filter(voxel_cloud);

    sensor_msgs::PointCloud2 msg_cloud;
    pcl::toROSMsg(voxel_cloud, msg_cloud);
    msg_cloud.header = msg->header;

    pub.publish(msg_cloud);
}

} // movebase_processing


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 *                                                                            *
 * This program is free software; you can redistribute it and/or modify       *
 * it under the terms of the Lesser GNU General Public License as published by*
 * the Free Software Foundation; either version 3 of the License, or          *
 * (at your option) any later version.                                        *
 *                                                                            *
 * This program 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              *
 * Lesser GNU General Public License for more details.                        *
 *                                                                            *
 * You should have received a copy of the Lesser GNU General Public License   *
 * along with this program. If not, see <http://www.gnu.org/licenses/>.       *
 ******************************************************************************/

#include "aslam/calibration/error-terms/ErrorTermTangency.h"

#include <Eigen/Dense>

using namespace aslam::backend;

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

  ErrorTermTangency::ErrorTermTangency(const aslam::backend::EuclideanExpression& v_r_mr_cross_i,
                                       const Covariance& sigma2_tangency_constraint, ErrorTermGroupReference etgr) :
      ErrorTermGroupMember(etgr),
        _v_r_mr_cross_i(v_r_mr_cross_i),
        _sigma2_tangency_constraint(sigma2_tangency_constraint) {
      setInvR(_sigma2_tangency_constraint.inverse());
      DesignVariable::set_t dv;
      _v_r_mr_cross_i.getDesignVariables(dv);
      setDesignVariablesIterator(dv.begin(), dv.end());
    }

    ErrorTermTangency::~ErrorTermTangency() {
    }

/******************************************************************************/
/* Methods                                                                    */
/******************************************************************************/

    double ErrorTermTangency::evaluateErrorImplementation() {
      error_t error;
      error = _v_r_mr_cross_i.toValue();
      setError(error);
      return evaluateChiSquaredError();
    }

    void ErrorTermTangency::evaluateJacobiansImplementation(JacobianContainer&
        jacobians) {
      _v_r_mr_cross_i.evaluateJacobians(jacobians);
    }

  }
}


/***************************************************************************
                          mddlsph.cc  -  description
                             -------------------
    begin                : Mon Jun 11 2007
    copyright            : (C) 2005 by Knut-Helge Vik
    email                : knuthelv@ifi.uio.no
 ***************************************************************************/
#include "mddlsph.h"
#include "../treealgs/dijkstra_sp.h"
#include <fstream>
#include "../simtime.h"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace std;
using namespace boost;
using namespace TreeAlgorithms;

/* -------------------------------------------------------------------------

	Algorithm(): Minimum Diameter Degree Limited Shortest Path Heuristic
							start function

------------------------------------------------------------------------- */
void
MDDLShortestPathHeuristic::Algorithm(vertex_descriptorN zsource)
{
	init(zsource);	// identify z-nodes and store them in VertexSet ZVertSet
	if(num_zvertices <= 0)
	{
		cout << WRITE_FUNCTION << " Error: No Z-vertices." << endl;	exit(0);
	}
	//cerr << WRITE_FUNCTION << " Start SPH zsource " << zsource << " and " << num_zvertices << " znodes" << endl ;

	// -- Start SPH Algorithm --
	T_mddlsph.insertVertex(zsource, g); 				// add source to the SPH tree
	ZVertSet.erase(zsource);
	RunDijkstraForEveryZ(g);							// find SP for every z-node and store in SPKeeper

	// -- fail safe --
	double failsafe = 0;
	// -- end fail safe --

	while(!ZVertSet.empty())
	{
		degreeBroken.clear();
		int z = -1;
		PathVector newPath; 							// .first contains path to new z-node to be added to T_bddlsph
														// .second is the tree-vertex link -> TODO: probably not needed
		if(!ExtractClosestZ(newPath, z)) 				// extract the z-node closest to T_bddlsph
		{
			if(!TreeAlgorithms::relaxDegree(degreeBroken, degree_bound)) break;
			ASSERTING(!degreeBroken.empty());
		}
		else
		{
			ASSERTING(z > -1);
			AddTreeSPH(newPath); // add the new path to T_bddlsph
			updateDist(z);
		}
	}

	// -- start debug --
	//cerr << WRITE_FUNCTION << "Produced Steiner Tree: " << endl;
	//T_mddlsph.print();
	//T_mddlsph.printVertexState(cerr);
	// -- end debug --
}

/* -------------------------------------------------------------------------

	ExtractClosestZ(): Returns the vertex that is closest to the T_mddlsph

		src: Dijkstra_shortest_path from this node
		p: next/parent pointers
		distances: distances from src to each other node in the graph

	IDEA: iterate through the z-vertices and check how close they are to
	the T_mddlsph return the closest z-vertex and add the path to the T_mddlsph

 --------------------------------------------------------------------------*/
bool
MDDLShortestPathHeuristic::ExtractClosestZ(PathVector &newPath, int &z_closest)
{
	double diameterTree	= MAXIMUM_WEIGHT; //(numeric_limits<double>::max)();
	int z_in = -1;
	PathVector tempPath;

	// zit - z-node not-in-tree iterator, tit_mddlsph - tree node iterator
	// iterate through the z-vertices and check how close they are to T_mddlsph
	for(VertexSet::iterator zit = ZVertSet.begin(), zit_end = ZVertSet.end(); zit != zit_end; ++zit)
	{
		vertex_descriptorN zAddToTree = *zit;

		ASSERTING(!T_mddlsph.V.contains(zAddToTree));
		//cerr << WRITE_FUNCTION  << " Z : " << zAddToTree << endl;

		ShortestPathKeeper &spk = findSPMaps(zAddToTree);

		VertexSet_it tit_mddlsph, tit_mddlsph_end;
		for(tit_mddlsph = T_mddlsph.V.begin(), tit_mddlsph_end = T_mddlsph.V.end(); tit_mddlsph != tit_mddlsph_end; ++tit_mddlsph) // check distance between new znode zit to every tree node tit_mddlsph
		{
			vertex_descriptorN zInTree = *tit_mddlsph;
			double new_diameter = ecc[zInTree] + spk.zdistance[zInTree];

			if(diameterTree > new_diameter && getOutDegree(T_mddlsph.g, zInTree) < degree_bound[zInTree]) //getDegreeConstraint(zInTree))	// is the new z-node closer than the previous -> then update distance etc.
			{
				if(FindPath(tempPath, zInTree, zAddToTree, spk.zparent))
				{
					diameterTree	= new_diameter;
					z_closest		= zAddToTree;
					z_in			= zInTree;
					newPath			= tempPath;
				}
			}
			tempPath = PathVector();

			if(getOutDegree(T_mddlsph.g, zInTree) >= degree_bound[zInTree]) degreeBroken.insert(zInTree);
		}
	}

	if(diameterTree < MAXIMUM_WEIGHT)return true;

	return false;
}

/* -------------------------------------------------------------------------

	FindDistance():	Find distance from z_vert to tree_vert using zparent
			and zdistance from DijkstraSP(z_vert)

------------------------------------------------------------------------- */
bool
MDDLShortestPathHeuristic::FindPath(PathVector &newPath, vertex_descriptorN tree_vert, vertex_descriptorN z_vert, const ParentVector &zparent)
{
	bool intact = true;
	ASSERTING(tree_vert != z_vert);
	vertex_descriptorN traverse_vert = tree_vert;
	do{
		if(getOutDegree(T_mddlsph.g, traverse_vert) >= degree_bound[traverse_vert])
		{
			degreeBroken.insert(traverse_vert);
			return false;
		}

		newPath.first.push_front(traverse_vert);
		traverse_vert = zparent[traverse_vert];
	}while(traverse_vert != z_vert);

	newPath.first.push_front(traverse_vert); 		// add last node
	ASSERTING(newPath.first.size() > 1);
	return intact;
}

/* -------------------------------------------------------------------------

	AddSPHTree(): Returns the vertex that is closest to the source
		if it is not already in the SPH-tree.

	all_vertex_info.first: path (vertex_descriptors) to new z_node
	all_vertex_info.second: node in the T_mddlsph (steiner or z-node) that
		links the new z-node to T_mddlsph
 --------------------------------------------------------------------------*/
void
MDDLShortestPathHeuristic::AddTreeSPH(PathVector &newPath)
{
	ASSERTING(newPath.first.size() > 1);
	for(list<vertex_descriptorN>::reverse_iterator vit = newPath.first.rbegin(), vit_end = newPath.first.rend(); vit != vit_end; )
	{
		// iterate
		vertex_descriptorN u = *vit;
		vit++;
		if(vit == vit_end) break;
		vertex_descriptorN v = *vit;

		pair<edge_descriptorN, bool> ep = edge(u, v, g);
		ASSERTING(ep.second);

		ASSERTING(near_[u] > -1);

		//cerr << "1 (" << u << "," << v << ")" << " near[" << u << "] " << near[u] << " near[" << v << "] " << near[v] << " ecc[" << v << "] " << ecc[v] << endl;

		if(near_[v] < 0)
		{
			near_[v] = u;
			ecc[v] = ecc[u] + g[ep.first].weight;
		}

		//cerr << "2 (" << u << "," << v << ")" << " near[" << u << "] " << near[u] << " near[" << v << "] " << near[v] << " ecc[" << v << "] " << ecc[v] << endl;
	}

	bool done = false;
	list<vertex_descriptorN>::iterator vit, vit_end;
	for(vit = newPath.first.begin(), vit_end = newPath.first.end(); vit != vit_end; )
	{
		// iterate
		vertex_descriptorN u = *vit;
		vit++;
		if(vit == vit_end) break;
		vertex_descriptorN v = *vit;

		// -- debug --
		//cerr << "adding (" << u << "," << v << ")" << endl;
		// -- end debug --

		if(T_mddlsph.V.contains(v)) done = true;

		T_mddlsph.insertEdge(u, v, g);

		ZVertSet.erase(u);
		ZVertSet.erase(v);

		if(done) break;
	}
}

void
MDDLShortestPathHeuristic::updateDist(vertex_descriptorN z)
{
	VertexSet::iterator vit, vit_end, vit_in, vit_in_end;
	ShortestPathKeeper &spk = findSPMaps(z);

	//cerr << WRITE_FUNCTION << " z " << z << " near[z] " << near[z] << " spk.zdistance[near[z]]" << spk.zdistance[near[z]] << endl;
	ASSERTING(near_[z] > -1);
	ASSERTING(ecc[near_[z]] >= 0);

	// set dist(z,u) and ecc(z)
	for(vit = T_mddlsph.V.begin(), vit_end = T_mddlsph.V.end(); vit != vit_end; ++vit)
	{
		if(dist(near_[z],*vit) > 0) dist(z,*vit) = dist(near_[z],*vit) + spk.zdistance[near_[z]];
	}

	dist(z,z) = 0;
	ecc[z] = ecc[near_[z]] + spk.zdistance[near_[z]];

	// update dist(near(z), u) and ecc(near(z))
	dist(near_[z],z) = spk.zdistance[near_[z]];
	if(ecc[near_[z]] <= 0) ecc[near_[z]] = spk.zdistance[near_[z]];

	// update other nodes' values of dist and ecc
	for(vit = T_mddlsph.V.begin(), vit_end = T_mddlsph.V.end(); vit != vit_end; ++vit)
	{
		ASSERTING(dist(*vit, near_[z]) >= 0);
		ASSERTING(dist(*vit, z) >= 0);
		ASSERTING(ecc[*vit] >= 0);

		dist(*vit,z) = dist(*vit, near_[z]) + spk.zdistance[near_[z]];
		ecc[*vit] = std::max(ecc[*vit], dist(*vit,z));
	}

	//cerr << " update the near values for other nodes in G " << endl;
	for(vit = ZVertSet.begin(), vit_end = ZVertSet.end(); vit != vit_end; ++vit)
	{
		ShortestPathKeeper &spk = findSPMaps(*vit);

		double curr_ecc = (std::numeric_limits<double>::max)();

		//cerr << " examine all nodes in T_mddlsph to determine near(" << *vit << ")	" << endl;
		for(vit_in = T_mddlsph.V.begin(), vit_in_end = T_mddlsph.V.end(); vit_in != vit_in_end; ++vit_in)
		{
			double new_ecc = ecc[*vit_in] + spk.zdistance[*vit_in];

			//cerr << " curr ecc " << curr_ecc << " new ecc " << new_ecc << endl;
			ASSERTING(ecc[*vit_in] >= 0);
			ASSERTING(spk.zdistance[*vit_in] >= 0);

			if(new_ecc < curr_ecc && getOutDegree(T_mddlsph.g, *vit_in) < degree_bound[*vit_in] )
			{
				//cerr << " new near [" << *vit << "] = " << *vit_in << endl;
				near_[*vit] = *vit_in;
				curr_ecc = new_ecc;
			}
		}

		if(near_[*vit] > -1)
		{
			if(getOutDegree(T_mddlsph.g, near_[*vit]) >= degree_bound[near_[*vit]])
			{
				degreeBroken.insert(near_[*vit]);
				//cerr << " inserted degree broken " << degreeBroken << endl;
			}
		}
	}
}

void
MDDLShortestPathHeuristic::init(vertex_descriptorN src)
{
	vsVertexMap	vsmap = get(&VertexProp::vertexState, g);

	VertexSet::const_iterator vit, vit_end, vit_in, vit_in_end;
	for(vit = inputT.V.begin(), vit_end = inputT.V.end(); vit != vit_end; ++vit)
	{
		if(vsmap[*vit] == GROUP_MEMBER)
		{
			ZVertSet.insert(*vit);
			num_zvertices++;
		}

		degree_bound[*vit] = getDegreeConstraint(g, *vit);
		ecc[*vit] = 0;
		if(*vit != src)
		{
			pair<edge_descriptorN, bool> ep = edge(*vit, src, g);
			if(ep.second) near_[*vit] = src;
			else near_[*vit] = -1;
		}
		else
		{
			near_[*vit] = *vit;
			ecc[*vit] = 0;
		}
		for(vit_in = inputT.V.begin(), vit_in_end = inputT.V.end(); vit_in != vit_in_end; ++vit_in)
			dist(*vit,*vit_in) = 0;
	}
	ASSERTING(!ZVertSet.empty());
}


#pragma once

#include <Eigen/Core>
using namespace Eigen;

#include <samson/types.hpp>

namespace samson
{
    using namespace samson::types;
}


/*!
 * @file
 * Contains unit tests for the @ref Enumerable typeclass.
 */

#include <boost/mpl11/enumerable.hpp>

#include <boost/mpl11/core.hpp>

#include "test_method_dispatch.hpp"

using namespace boost::mpl11;

///////////////////////////
// Test method dispatching
///////////////////////////
namespace boost { namespace mpl11 {
    template <>
    struct Enumerable<Archetype<>>
        : instantiate<Enumerable>::with<Archetype<>>
    {
        template <typename> using succ_impl = method_tag<succ>;
        template <typename> using pred_impl = method_tag<pred>;
    };
}}

struct tests
    : method<succ, archetype<>>
    , method<pred, archetype<>>
{ };

int main() { }


#ifndef MAIN_HPP
#define MAIN_HPP

#include <cstdlib>
#include <vector>
#include <math.h>
#include <iostream>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <tuple>
#include <utility>
#include <algorithm>
#include <set>
#include <iomanip> //std::setprecision
#include <map>  //Pour avoir des maps

// option parser
#include <boost/program_options.hpp>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
boost::mt19937 generator(static_cast<unsigned int>(time(0)));
boost::uniform_real<> uni_dist(0,1);
boost::variate_generator<boost::mt19937&, boost::uniform_real<> > uni(generator, uni_dist);

#define Ligne "_____________________________________________\n"

#define RESETCOLOR "\033[0m"
#define BLACK   "\033[30m"      		   /* Black */
#define RED     "\033[31m"      		   /* Red */
#define GREEN   "\033[32m"      		   /* Green */
#define YELLOW  "\033[33m"      		   /* Yellow */
#define BLUE    "\033[34m"      		   /* Blue */
#define MAGENTA "\033[35m"      		   /* Magenta */
#define CYAN    "\033[36m"      		   /* Cyan */
#define WHITE   "\033[37m"      		   /* White */
#define BOLDBLACK   "\033[1m\033[30m"      /* Bold Black */
#define BOLDRED     "\033[1m\033[31m"      /* Bold Red */
#define BOLDGREEN   "\033[1m\033[32m"      /* Bold Green */
#define BOLDYELLOW  "\033[1m\033[33m"      /* Bold Yellow */
#define BOLDBLUE    "\033[1m\033[34m"      /* Bold Blue */
#define BOLDMAGENTA "\033[1m\033[35m"      /* Bold Magenta */
#define BOLDCYAN    "\033[1m\033[36m"      /* Bold Cyan */
#define BOLDWHITE   "\033[1m\033[37m"      /* Bold White */

#endif //TYPES_H


/*
 Copyright (C) 2016 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 <boost/assign/list_of.hpp>
using boost::assign::list_of;

#include <ql/cashflows/iborcoupon.hpp>
#include <ql/cashflows/simplecashflow.hpp>

#include <qle/instruments/crossccybasisswap.hpp>

namespace QuantExt {

CrossCcyBasisSwap::CrossCcyBasisSwap(Real payNominal, const Currency& payCurrency, const Schedule& paySchedule,
                                     const boost::shared_ptr<IborIndex>& payIndex, Spread paySpread, Real recNominal,
                                     const Currency& recCurrency, const Schedule& recSchedule,
                                     const boost::shared_ptr<IborIndex>& recIndex, Spread recSpread)
    : CrossCcySwap(2), payNominal_(payNominal), payCurrency_(payCurrency), paySchedule_(paySchedule),
      payIndex_(payIndex), paySpread_(paySpread), recNominal_(recNominal), recCurrency_(recCurrency),
      recSchedule_(recSchedule), recIndex_(recIndex), recSpread_(recSpread) {
    registerWith(payIndex);
    registerWith(recIndex);
    initialize();
}

void CrossCcyBasisSwap::initialize() {
    // Pay leg
    legs_[0] = IborLeg(paySchedule_, payIndex_).withNotionals(payNominal_).withSpreads(paySpread_);
    payer_[0] = -1.0;
    currencies_[0] = payCurrency_;
    // Pay leg notional exchange at start.
    Date initialPayDate = paySchedule_.dates().front();
    boost::shared_ptr<CashFlow> initialPayCF(new SimpleCashFlow(-payNominal_, initialPayDate));
    legs_[0].insert(legs_[0].begin(), initialPayCF);
    // Pay leg notional exchange at end.
    Date finalPayDate = paySchedule_.dates().back();
    boost::shared_ptr<CashFlow> finalPayCF(new SimpleCashFlow(payNominal_, finalPayDate));
    legs_[0].push_back(finalPayCF);

    // Receive leg
    legs_[1] = IborLeg(recSchedule_, recIndex_).withNotionals(recNominal_).withSpreads(recSpread_);
    payer_[1] = +1.0;
    currencies_[1] = recCurrency_;
    // Receive leg notional exchange at start.
    Date initialRecDate = recSchedule_.dates().front();
    boost::shared_ptr<CashFlow> initialRecCF(new SimpleCashFlow(-recNominal_, initialRecDate));
    legs_[1].insert(legs_[1].begin(), initialRecCF);
    // Receive leg notional exchange at end.
    Date finalRecDate = recSchedule_.dates().back();
    boost::shared_ptr<CashFlow> finalRecCF(new SimpleCashFlow(recNominal_, finalRecDate));
    legs_[1].push_back(finalRecCF);

    // Register the instrument with all cashflows on each leg.
    for (Size legNo = 0; legNo < 2; legNo++) {
        Leg::iterator it;
        for (it = legs_[legNo].begin(); it != legs_[legNo].end(); ++it) {
            registerWith(*it);
        }
    }
}

void CrossCcyBasisSwap::setupArguments(PricingEngine::arguments* args) const {

    CrossCcySwap::setupArguments(args);

    CrossCcyBasisSwap::arguments* arguments = dynamic_cast<CrossCcyBasisSwap::arguments*>(args);

    /* Returns here if e.g. args is CrossCcySwap::arguments which
       is the case if PricingEngine is a CrossCcySwap::engine. */
    if (!arguments)
        return;

    arguments->paySpread = paySpread_;
    arguments->recSpread = recSpread_;
}

void CrossCcyBasisSwap::fetchResults(const PricingEngine::results* r) const {

    CrossCcySwap::fetchResults(r);

    const CrossCcyBasisSwap::results* results = dynamic_cast<const CrossCcyBasisSwap::results*>(r);
    if (results) {
        /* If PricingEngine::results are of type
           CrossCcyBasisSwap::results */
        fairPaySpread_ = results->fairPaySpread;
        fairRecSpread_ = results->fairRecSpread;
    } else {
        /* If not, e.g. if the engine is a CrossCcySwap::engine */
        fairPaySpread_ = Null<Spread>();
        fairRecSpread_ = Null<Spread>();
    }

    /* Calculate the fair pay and receive spreads if they are null */
    static Spread basisPoint = 1.0e-4;
    if (fairPaySpread_ == Null<Spread>()) {
        if (legBPS_[0] != Null<Real>())
            fairPaySpread_ = paySpread_ - NPV_ / (legBPS_[0] / basisPoint);
    }
    if (fairRecSpread_ == Null<Spread>()) {
        if (legBPS_[1] != Null<Real>())
            fairRecSpread_ = recSpread_ - NPV_ / (legBPS_[1] / basisPoint);
    }
}

void CrossCcyBasisSwap::setupExpired() const {
    CrossCcySwap::setupExpired();
    fairPaySpread_ = Null<Spread>();
    fairRecSpread_ = Null<Spread>();
}

void CrossCcyBasisSwap::arguments::validate() const {
    CrossCcySwap::arguments::validate();
    QL_REQUIRE(paySpread != Null<Spread>(), "Pay spread cannot be null");
    QL_REQUIRE(recSpread != Null<Spread>(), "Rec spread cannot be null");
}

void CrossCcyBasisSwap::results::reset() {
    CrossCcySwap::results::reset();
    fairPaySpread = Null<Spread>();
    fairRecSpread = Null<Spread>();
}
} // namespace QuantExt


#include <iostream>
#include <fstream>
#include <map>
#include <ctype.h>
#include <climits>

#include <strips_prob.hxx>
#include <ff_to_aptk.hxx>
#include <fluent.hxx>
#include <action.hxx>
#include <cond_eff.hxx>
#include <strips_state.hxx>
#include <fwd_search_prob.hxx>
#include <rp_heuristic.hxx>
#include <aptk/open_list.hxx>

#include <boost/program_options.hpp>
#include <boost/algorithm/string.hpp>
#include <string>
#include <aptk/string_conversions.hxx>
#include <aptk/at_bfs.hxx>
#include <aptk/closed_list.hxx>

#include "utility.hxx"
#include "goal_set_parse.hxx"
#include "plan_recognition.hxx"
#include "lm_plan_recognition.hxx"

namespace po = boost::program_options;

using	aptk::STRIPS_Problem;
using	aptk::Action;
using	aptk::agnostic::Fwd_Search_Problem;
using namespace pr;
using namespace pr_lm;

double euclidean_distance(const std::vector<double> P, const std::vector<double> Q){
  assert(P.size() == Q.size());

  double h_d_2 = 0.0;

  for (unsigned int i = 0; i < P.size(); i++)
  {
    double p_q_i = P[i] - Q[i];
    h_d_2 += p_q_i*p_q_i;
  }

  return -1*sqrt(h_d_2);
}

std::pair<double, std::vector<double>> call_plan_rec(aptk::STRIPS_Problem& prob, aptk::Fluent_Vec init,  std::vector<aptk::Fluent_Vec> goal_set,
  std::vector<unsigned> observations, std::vector<double> priors, unsigned int goal_index,
  float beta, float theta, bool approx, bool landmark, bool negated_as_neutral, bool variable_beta, bool verbose
  ){

  std::vector<double> perfect_dist;
  for (unsigned int i = 0; i < goal_set.size(); i++)
    if (i == goal_index)
      perfect_dist.push_back(1.0);
    else
      perfect_dist.push_back(0.0);

  // Using traditional plan rec not difference plan rec
  std::vector<double> posteriors;

  if(!landmark)
  {
    PlanRecognitionEngine pr(&prob, goal_set, priors, approx, negated_as_neutral,
        variable_beta, verbose);
    posteriors = pr.plan_recognition(observations, init, beta);
  }else{
    PlanRecognitionEngine_LM prlm(&prob, goal_set, theta, verbose, priors);
    posteriors = prlm.plan_recognition(observations);
  }

  std::pair<double,std::vector<double>> reward;
  reward.first = euclidean_distance(perfect_dist, posteriors);
  reward.second = posteriors;

  return reward;

}

std::vector<float> aggregate_plan_recognition(aptk::STRIPS_Problem& prob, aptk::Fluent_Vec init,  std::vector<aptk::Fluent_Vec> goal_set,
  std::vector<unsigned> observations, std::vector<double> priors, unsigned int goal_index,
  float beta, float theta, bool approx, bool landmark, bool negated_as_neutral, bool variable_beta, bool verbose
  ){

  float thresh_1 = -1*0.367423;
  //float thresh_2 = -1*0.244949;
  //float thresh_3 = -1*0.122471;
  float cost_thresh_1 = 0;
  //float cost_thresh_2 = 0;
  //float cost_thresh_3 = 0;
  bool thresh_1_done = false;
  //bool thresh_3_done = false;
  //bool thresh_2_done = false;

  float prior_sum = 0;
  float priors_av;
  for(unsigned i = 0; i < priors.size(); i++){
    if(i != goal_index){
      prior_sum += priors[i];
    }

  }

  priors_av = prior_sum / ((float) priors.size() - 1);

  PlanRecognitionEngine pr(&prob, goal_set, priors,false,false,
      false,false);
  Fwd_Search_Problem search_prob_o(&prob);
  float optimal_cost;
  std::vector<int> plan;
  std::vector<unsigned int> obs;
  pr.heuristic_estimate(search_prob_o,  optimal_cost, plan, obs);

  std::vector<unsigned int> current_obs;
  for(unsigned int i = 0; i < observations.size(); i++){
    current_obs.push_back(observations[i]);

    auto result = call_plan_rec(prob, prob.init(), goal_set, current_obs,  priors, goal_index,
      beta, theta, approx, landmark, negated_as_neutral, variable_beta, verbose);

    float observations_eval = result.first;
    auto posts = result.second;

    //std::cout << print_action_vec(prob, current_obs) << std::endl;
    //print_vector(posts);
    //std::cout  << std::endl;
    //std::cout  << std::endl;
    //std::cout  << std::endl;
    //std::cout  << std::endl;

    //for (auto i : posts){
      //std::cout << i << std::endl;
    //}

    float max_prob = 0;
    for(unsigned i = 0; i < posts.size(); i++){
      if(i != goal_index){
        if (posts[i] > max_prob){
          max_prob = posts[i];
        }
      }

    }

    if (max_prob < posts[goal_index] - (priors_av)){
      //std::cout << "MaxProb: " << max_prob << ", GoalProb: " << posts[goal_index] << ", YES";
        thresh_1_done= true;

    }
    else{
      //std::cout << "MaxProb: " << max_prob << ", GoalProb: " << posts[goal_index] << ", NO";
    }

    cost_thresh_1 += prob.actions()[observations[i]]->cost();

    //if (observations_eval > thresh_2){
      //thresh_2_done = true;
    //} else if (!thresh_2_done){
      //cost_thresh_2 += prob.actions()[observations[i]]->cost();
    //}

    //if (observations_eval > thresh_3){
      //thresh_3_done = true;
    //} else if (!thresh_3_done){
      //cost_thresh_3 += prob.actions()[observations[i]]->cost();
    //}

    if(thresh_1_done ){//&& thresh_2_done && thresh_3_done){
      break;
    }else{
      //std::cout << observations[i] << std::endl;
    }

  }

  std::vector<float> thress;

  std::cout << cost_thresh_1 << std::endl;
  std::cout << optimal_cost << std::endl;

  //cost_thresh_1 = (cost_thresh_1+1)/optimal_cost > 1 ? 1 : cost_thresh_1/optimal_cost;
  //cost_thresh_2 = (cost_thresh_2+1)/optimal_cost > 1 ? 1 : cost_thresh_2/optimal_cost;
  //cost_thresh_3 = (cost_thresh_3+1)/optimal_cost > 1 ? 1 : cost_thresh_3/optimal_cost;
  //thress.push_back(cost_thresh_1);
  //thress.push_back(cost_thresh_2);
  //thress.push_back(cost_thresh_3);
  return thress;

}

void process_command_line_options( int ac, char** av, po::variables_map& vm) {
	po::options_description desc( "Additional Options" );

	desc.add_options()
		( "domain,d", po::value<std::string>(), "Input PDDL domain description" )
		( "problem,p", po::value<std::string>(), "Input PDDL problem description" )
		( "goalset,g", po::value<std::string>(), "Input goal-set description" )
		( "observations,o", po::value<std::string>(), "Input observation description" )

		( "beta,b", po::value<float>()->default_value(1), "Value determining how rational an observer believes the agent to be." )
		( "landmark,l", po::bool_switch()->default_value(false), "Use landmark plan recognition." )
		( "theta,t", po::value<float>()->default_value(0.2), "Threshold for filtering gaols." )

		( "approx,a", po::bool_switch()->default_value(false), "Use approx plan recognition." )
		( "nan,n", po::bool_switch()->default_value(false), "Approximate C(G, ~O) as c(G) in approximate plan recognition." )
		( "static-beta,S", po::bool_switch()->default_value(false), "Keep beta value static in search." )

		( "verbose,v", po::bool_switch()->default_value(false), "Verbose results." )
		( "help,h", "Show help message" )
	;
  po::positional_options_description positionalOptions;
  positionalOptions.add("domain", 1);
  positionalOptions.add("problem", 1);
  positionalOptions.add("goalset", 1);
  positionalOptions.add("observations", 1);

	try {
    po::store(po::command_line_parser(ac, av).options(desc).positional(positionalOptions).run(), vm);
		//po::store( po::parse_command_line( ac, av, desc ), vm);
		po::notify( vm );
	}
	catch ( std::exception& e ) {
		std::cerr << "Error: " << e.what() << std::endl;
		std::exit(1);
	}
	catch ( ... ) {
		std::cerr << "Exception of unknown type!" << std::endl;
		std::exit(1);
	}

	if ( vm.count("help") ) {
    std::cout << "Author: Aleck MacNally\n";
    std::cout << "Purpose: This program is an AI planner for classical planning domains.\n\
It differs from other AI planners in that it's objective is to find the plan which\n\
which is most obviously attempting to achieve it's goal to an observer.\n\n";
    std::cout << "Usage: PFT DOMAIN PROBLEM GOALSET [OPTIONS]" << std::endl << std::endl;
		std::cout << desc << std::endl;
		std::exit(0);
	}

}

int main( int argc, char** argv){
	po::variables_map vm;
	process_command_line_options( argc, argv, vm );

	if ( !vm.count( "domain" ) ) {
		std::cerr << "No PDDL domain was specified!" << std::endl;
		std::exit(1);
	}

	if ( !vm.count( "problem" ) ) {
		std::cerr << "No PDDL problem was specified!" << std::endl;
		std::exit(1);
	}

	if ( !vm.count( "goalset" ) ) {
		std::cerr << "No Goal Set file was specified!" << std::endl;
		std::exit(1);
	}

	if ( !vm.count( "observations" ) ) {
		std::cerr << "No Goal Set file was specified!" << std::endl;
		std::exit(1);
	}

  std::string goal_set_name = vm["goalset"].as<std::string>();
  std::string observations_name = vm["observations"].as<std::string>();
  float beta = vm["beta"].as<float>();
  float theta = vm["theta"].as<float>();
  bool optimal = !vm["approx"].as<bool>();
  bool landmark = vm["landmark"].as<bool>();
  bool verbose = vm["verbose"].as<bool>();
  bool negated_as_neutral = vm["nan"].as<bool>();
  bool variable_beta = !vm["static-beta"].as<bool>();
  bool approx = !optimal;

	aptk::STRIPS_Problem	prob;
	aptk::FF_Parser::get_problem_description( vm["domain"].as<std::string>(), vm["problem"].as<std::string>(), prob );
	std::vector<aptk::Fluent_Vec> goal_set = goal_set_parsing(goal_set_name, prob);
  std::vector<unsigned int> observations = observations_parsing(observations_name, prob);

  std::vector<double> priors;
  for (unsigned int i = 0; i < goal_set.size(); i++){
    priors.push_back(0.5);
  }

  priors = normalize_vector(priors);

  unsigned int goal_index = 0;
  std::vector<unsigned> true_goal_v = prob.goal();
  std::set<unsigned> true_goal(true_goal_v.begin(), true_goal_v.end());
  for (unsigned int i = 0; i < goal_set.size(); i++){
    std::vector<unsigned> cand_goal_v = goal_set[i];
    std::set<unsigned> cand_goal (cand_goal_v.begin(), cand_goal_v.end());
    if (cand_goal == true_goal)
      goal_index = i;

  }
  prob.set_true_init(prob.init());

  std::cout << std::endl;

  auto result = aggregate_plan_recognition(prob, prob.init(), goal_set, observations,  priors, goal_index,
    beta, theta, approx, landmark, negated_as_neutral, variable_beta, verbose);

  //std::cout << result[0] << std::endl;
}


const bool enable_expensive_tests = false;

#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <util/test_macros.hpp>
#include <vector>
#include <string>
#include <functional>

#include <random/random.hpp>

#include <sframe/sframe_iterators.hpp>
#include <unity/toolkits/recsys/models/factorization_models.hpp>
#include <unity/toolkits/util/data_generators.hpp>
#include <sframe/testing_utils.hpp>
#include <util/testing_utils.hpp>

#include <cfenv>

using namespace turi;
using namespace turi::recsys;

void run_exact_test(
    const std::vector<size_t>& n_categorical_values,
    std::map<std::string, flexible_type> opts,
    const std::string& model_type) {

  bool binary_target = false;

  if(model_type == "mf" || model_type == "logistic_mf")
    opts["only_2_factor_terms"] = true;

  std::feraiseexcept(FE_ALL_EXCEPT);

  size_t n_observations = opts.at("n_observations");
  opts.erase("n_observations");

  std::string target_column_name = "target";

  std::vector<std::string> column_names = {"user_id", "item_id"};

  DASSERT_NE(n_categorical_values[0], 0);
  DASSERT_NE(n_categorical_values[1], 0);

  /* for(size_t i = 2; i < n_categorical_values.size(); ++i) { */
  /*   column_names.push_back("C-" + std::to_string(i)); */
  /* } */

  lm_data_generator lmdata(column_names, n_categorical_values, opts);

  sframe train_data = lmdata.generate(n_observations, target_column_name, 0, 0);
  sframe test_data = lmdata.generate(n_observations, target_column_name, 1, 0);

  std::map<std::string, flexible_type> options =
      { {"solver", "auto"},
        {"binary_target", binary_target},
        {"target", target_column_name},
        {"regularization", 0},
        {"sgd_step_size", 0},
        {"max_iterations", binary_target ? 200 : 100},
        {"sgd_convergence_threshold", 1e-10},

        // Add in the ranking regularizer
        {"ranking_regularization", 0.1},
        {"unobserved_rating_value", 0}
      };

  if(model_type == "mf" || model_type == "logistic_mf")
    opts.erase("only_2_factor_terms");

  options.insert(opts.begin(), opts.end());

  typedef std::shared_ptr<recsys::recsys_model_base> model_ptr;
  auto new_model = [&]() -> model_ptr {
    model_ptr ret;
    ret.reset(new recsys_ranking_factorization_model);
    return ret;
  };

  model_ptr model = new_model();
  model->init_options(options);
  model->setup_and_train(train_data);

  auto model2 = new_model();

  {
    // Save it
    dir_archive archive_write;
    archive_write.open_directory_for_write("recsys_get_similar_cxx_tests");

    turi::oarchive oarc(archive_write);

    oarc << *model;

    archive_write.close();

    // Load it
    dir_archive archive_read;
    archive_read.open_directory_for_read("recsys_get_similar_cxx_tests");

    turi::iarchive iarc(archive_read);

    iarc >> (*model2);
    sframe y_hat_sf = model2->predict(model->create_ml_data(test_data));
  }

  logprogress_stream << "Getting similar items" << std::endl;
  auto chosen_items = std::vector<flexible_type>{0};
  auto items_sa = make_testing_sarray(flex_type_enum::INTEGER, chosen_items);
  auto chosen_users = std::vector<flexible_type>{0};
  auto users_sa = make_testing_sarray(flex_type_enum::INTEGER, chosen_users);

  size_t k = 5;
  sframe result_1 = model->get_similar_items(items_sa, k);
  sframe result_2 = model2->get_similar_items(items_sa, k);
  sframe result_3 = model->get_similar_users(users_sa, k);
  sframe result_4 = model2->get_similar_users(users_sa, k);
}

struct get_similar_tests  {
 public:

  ////////////////////////////////////////////////////////////////////////////////

  void test_mf() {
    std::map<std::string, flexible_type> opts = {
      {"n_observations",      100 } };

    run_exact_test({10, 10}, opts, "mf");
  }
};

BOOST_FIXTURE_TEST_SUITE(_get_similar_tests, get_similar_tests)
BOOST_AUTO_TEST_CASE(test_mf) {
  get_similar_tests::test_mf();
}
BOOST_AUTO_TEST_SUITE_END()


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

#include "NetComm.h"
#include "BankServer.h"

#include "BlindSignature.h"
#include "MoneyOrder.h"
#include "MoneyOrderInfo.h"
#include "Random.h"
#include "Rsa.h"
#include "SecretSplitting.h"
#include "Utilities.h"

#include <boost/progress.hpp>

namespace
{
    const unsigned int BASE = 10;
}

void Bank::BankServer::run(tcp::socket sock1)
{
    try
    {
        while (true)
        {
            // Read Command Decide Path Forward
            std::string cmd = ReadAndAcknowledge(sock1);

            if (cmd == BankCommands::DEPOSIT_MONEY_ORDER)
            {
                DepositMoneyOrder(sock1);
            }
            else if (cmd == BankCommands::SIGN_MONEY_ORDER)
            {
                SignMoneyOrder(sock1);
            }
            else if (cmd == BankCommands::GET_PUBLIC_KEY)
            {
                GetPublicKey(sock1);
            }
            else if (cmd == BankCommands::OPEN_ACCOUNT)
            {
                OpenAccount(sock1);
            }
            else if (cmd == BankCommands::CLOSE_CONNECTION)
            {
                break;
            }
            else
            {
                std::cerr << "Unknown command: " << cmd << std::endl;
            }
        }
    }
    catch (std::exception& e)
    {
        std::cerr << "Exception in Bank::BankServer::run(): " << e.what() << "\n";
    }
}

void Bank::BankServer::SignMoneyOrder(tcp::socket& sock1)
{
    try
    {
        //////////////////////////////////////////////////////////////////////////////////////////
        // Read identity string and amount
        std::string expected_ident = ReadAndAcknowledge(sock1);
        unsigned int expected_amount = std::atoi(ReadAndAcknowledge(sock1).c_str());

        //////////////////////////////////////////////////////////////////////////////////////////
        // Check how many money orders to expect
        unsigned int num_money_orders = atoi(ReadAndAcknowledge(sock1).c_str());

        //////////////////////////////////////////////////////////////////////////////////////////
        // Receive Money Orders
        std::vector<mpz_class> money_orders;
        std::cout << "Receiving " << num_money_orders << " money orders ... " << std::endl;
        boost::progress_display show_money_order_progress( num_money_orders );
        for (unsigned int i = 0; i < num_money_orders; ++i)
        {
            std::string money_order = ReadAndAcknowledge(sock1);

            money_orders.push_back((mpz_class(money_order, BASE)));
            ++show_money_order_progress;
        }

        //////////////////////////////////////////////////////////////////////////////////////////
        // Choose random number to not verify
        mpz_class ran = Random::GenerateRandomNumberRange(num_money_orders) - 1;
        unsigned int r = std::atoi(ran.get_str(BASE).c_str());
        WriteAndWaitForAcknowledge(sock1, ran.get_str(BASE));

        //////////////////////////////////////////////////////////////////////////////////////////
        // Receive Money Order Info
        std::vector<MoneyOrderInfo> money_orders_info;
        for (unsigned int i = 0; i < num_money_orders; ++i)
        {
            if (i == r)
            {
                // empty info
                MoneyOrderInfo info;
                money_orders_info.push_back(info);
            }
            else
            {
                MoneyOrderInfo info;
                std::string money_order_info = ReadAndAcknowledge(sock1);
                info.Deserialize(money_order_info);
                money_orders_info.push_back(info);
            }
        }

        bool all_verified = true;
        std::cout << "Verifying " << num_money_orders-1 << " money orders ..." << std::endl;
        boost::progress_display show_verification_progress( num_money_orders-1 );
        for (unsigned int i = 0; i < num_money_orders; ++i)
        {
            // if this is not the money order to sign
            if (i != r)
            {
                mpz_class signed_blinded_text = Rsa::Sign(money_orders[i], priv, pub, false);
                mpz_class unblinded_signed_text = BlindSignature::Unblind(signed_blinded_text, pub, (mpz_class(money_orders_info[i].m_blinding_factor, BASE)), false);
                mpz_class unsigned_text = Rsa::Unsign(unblinded_signed_text, pub, true);

                MoneyOrder mo;
                std::string nts = Utilities::NumberToString(unsigned_text);
                mo.Deserialize(nts);

                unsigned int j = 0;
                bool verified = true;

                verified &= (mo.m_amount == expected_amount);

                for (const auto& cd : money_orders_info[i].m_commit_data)
                {
                    // verify left side
                    verified &= Verify(cd.first, mo.m_identity_strings[j].first.first,
                                                 mo.m_identity_strings[j].first.second);

                    // verify left side
                    verified &= Verify(cd.second, mo.m_identity_strings[j].second.first,
                                                  mo.m_identity_strings[j].second.second);

                    verified &= (expected_ident == SecretSplitting::GetSecret(Utilities::StringToNumber(cd.first.b), Utilities::StringToNumber(cd.second.b)));

                    ++j;
                }

                ++show_verification_progress;

                all_verified &= verified;
            }
        }

        // if they have all been verified then sign the one that is still blinded
        if (all_verified)
        {
            mpz_class signed_blinded_text = Rsa::Sign(money_orders[r], priv, pub, false);

            // send it back to the buyer
            WriteAndWaitForAcknowledge(sock1, signed_blinded_text.get_str(BASE));
        }
    }
    catch (std::exception& e)
    {
        std::cerr << "Exception in Bank::BankServer::SignMoneyOrder: " << e.what() << "\n";
    }
}

void Bank::BankServer::DepositMoneyOrder(tcp::socket& sock1)
{
    std::string identity = ReadAndAcknowledge(sock1);

    std::string BuyersMoneyOrderStr = ReadAndAcknowledge(sock1);
    mpz_class BuyersMoneyOrder(BuyersMoneyOrderStr);
    mpz_class UnsignedBuyersMoneyOrder = Rsa::Unsign( BuyersMoneyOrder, pub, true );

    MoneyOrder moneyOrder;
    moneyOrder.Deserialize( Utilities::NumberToString( UnsignedBuyersMoneyOrder ) );

    std::string selectorStr = ReadAndAcknowledge(sock1);

    std::vector<std::string> commitDataStrVector;
    for(unsigned int i = 0; i < moneyOrder.m_identity_strings.size(); ++i)
    {
        commitDataStrVector.push_back(ReadAndAcknowledge(sock1));
    }

    // check the database to determine whether this money order has already been deposited
    auto it = m_deposits.find(moneyOrder.m_uniqueness);
    if (it == m_deposits.end())
    {
        DepositInformation depositInfo( identity, moneyOrder, selectorStr, commitDataStrVector );
        m_deposits.insert( std::pair<std::string, DepositInformation>( moneyOrder.m_uniqueness, depositInfo ));

        WriteAndWaitForAcknowledge(sock1, "Deposit Successful!");
    }
    else
    {
        std::cout << "Deposit Unsuccesful.  Determining the perpetrator..." << std::endl;

        // if the selector string match, then the merchant cheated
        if (selectorStr == it->second.selectorStr)
        {
            std::cout << "The merchant, " << identity << ", cheated!" << std::endl;
        }
        else
        {
            // if there are different selector strings, then the buyer cheated
            std::cout << "The buyer cheated!  Possible identities ..." << std::endl;

            int i = 0;
            for (const auto& cd : it->second.identity_strings)
            {
                CommitData data1;
                data1.Deserialize(cd);

                CommitData data2;
                data2.Deserialize(commitDataStrVector[i]);

                std::cout << "Identity: " << SecretSplitting::GetSecret(Utilities::StringToNumber(data1.b),
                                                                        Utilities::StringToNumber(data2.b)) << std::endl;

                ++i;
            }
        }

        WriteAndWaitForAcknowledge(sock1, "Deposit Unsuccessful!");
    }
}

void Bank::BankServer::GetPublicKey(tcp::socket& sock1)
{
    try
    {
        std::string pub_str = pub.e.get_str(BASE);
        std::string mod_str = pub.N.get_str(BASE);

        WriteAndWaitForAcknowledge(sock1, pub_str);
        WriteAndWaitForAcknowledge(sock1, mod_str);
    }
    catch (std::exception& e)
    {
        std::cerr << "Exception in Bank::BankServer::GetPublicKey(): " << e.what() << "\n";
    }
}

void Bank::BankServer::OpenAccount(tcp::socket& sock1)
{
    try
    {
        std::string identity = ReadAndAcknowledge( sock1 );
        unsigned int amount  = std::atoi( ReadAndAcknowledge( sock1 ).c_str() );

        if (m_accounts.find( identity ) == m_accounts.end())
        {
            AccountInformation ai( amount );
            m_accounts.insert( std::pair<std::string, AccountInformation>( identity, ai ) );
            std::cout << "New account opened for: " << identity << std::endl;
        }
    }
    catch (std::exception& e)
    {
        std::cerr << "Exception in Bank::BankServer::OpenAccount(): " << e.what() << "\n";
    }
}


#include "CS_Retro_NavigatorGadget.h"

#include <armadillo>
#include <algorithm>
#include <cmath>

#include "hoNDArray_math_util.cpp"

using namespace Gadgetron;

// class constructor
CS_Retro_NavigatorGadget::CS_Retro_NavigatorGadget()
{
}

// class destructor - delete temporal buffer/memory
CS_Retro_NavigatorGadget::~CS_Retro_NavigatorGadget()
{
}

// read flexible data header
int CS_Retro_NavigatorGadget::process_config(ACE_Message_Block *mb)
{
	// get gadget property
#ifdef __GADGETRON_VERSION_HIGHER_3_6__
	min_card_freq_	= MinCardFreq.value();
	max_card_freq_	= MaxCardFreq.value();
	min_resp_freq_	= MinRespFreq.value();
	max_resp_freq_	= MaxRespFreq.value();
	iNavMethod_		= NavigationMethod.value();
#else
	min_card_freq_	= *(get_int_value("MinCardFreq").get());
	max_card_freq_	= *(get_int_value("MaxCardFreq").get());
	min_resp_freq_	= *(get_int_value("MinRespFreq").get());
	max_resp_freq_	= *(get_int_value("MaxRespFreq").get());
	iNavMethod_		= *(get_int_value("NavigationMethod").get());
#endif

	// some basic error checking for parameters
	if (min_card_freq_ < 0 || max_card_freq_ < 0 || min_resp_freq_ < 0 || max_resp_freq_ < 0) {
		GERROR("Given parameters min_card_freq_, max_card_freq_, min_resp_freq_, max_resp_freq_ must not be negative!\n");
		return GADGET_FAIL;
	}

	if (min_card_freq_ >= max_card_freq_) {
		GERROR("max_card_freq_ must be greater than min_card_freq_!\n");
		return GADGET_FAIL;
	}

	if (min_resp_freq_ >= max_resp_freq_) {
		GERROR("max_resp_freq_ must be greater than min_resp_freq_!\n");
		return GADGET_FAIL;
	}

	return GADGET_OK;
}

int CS_Retro_NavigatorGadget::process(GadgetContainerMessage<ISMRMRD::ImageHeader> *m1,GadgetContainerMessage<hoNDArray<std::complex<float> > > *m2, GadgetContainerMessage<hoNDArray<std::complex<float> > > *m3)
{
	// fetch attribute values from header
	iNoChannels_ = m1->getObjectPtr()->channels;
	iNoNav_		 = m1->getObjectPtr()->user_int[5];
	lNoScans_	 = m3->getObjectPtr()->get_size(1);

	field_of_view_[0] = m1->getObjectPtr()->field_of_view[0];
	field_of_view_[1] = m1->getObjectPtr()->field_of_view[1];
	field_of_view_[2] = m1->getObjectPtr()->field_of_view[2];

	// get navigator signal according to selected method
	// 0: classical
	// 1: PCA
	switch (iNavMethod_) {
	case 0:
		try {
			getNav2D(*m2->getObjectPtr());
		} catch ( ... ) {
			GERROR("An exception occurred\n");
			return GADGET_FAIL;
		}

		break;

	case 1:
		try {
			getNav2DPCA(*m2->getObjectPtr());
		} catch ( ... ) {
			GERROR("An exception occurred\n");
			return GADGET_FAIL;
		}

		break;

	default:
		GERROR("Navigation method %d unknown! Please specify one via gadget property.\n", iNavMethod_);
		return GADGET_FAIL;
	}

	// update cardiac gates to 1 if no signal is present
	if (navigator_card_interpolated_.size() == 0) {
		set_number_of_gates(m1->getObjectPtr()->user_int[0], 1, 1);
	}

	// equalize vector lengths (zero-padding)
	while (navigator_card_interpolated_.size() < navigator_resp_interpolated_.size()) {
		navigator_card_interpolated_.push_back(0);
	}
	while (navigator_resp_interpolated_.size() < navigator_card_interpolated_.size()) {
		navigator_resp_interpolated_.push_back(0);
	}

	// achieve message format [respiratory navigator, cardiac navigator]
	GadgetContainerMessage<hoNDArray<float> > *tmp_m2 = new GadgetContainerMessage<hoNDArray<float> >();
	tmp_m2->getObjectPtr()->create(navigator_resp_interpolated_.size(), 2);

	// copy data to array (resp, card, resp, card,... - therefore Indices 2*iI [+1]
	float *fPtr = tmp_m2->getObjectPtr()->get_data_ptr();
	for (size_t iI = 0; iI < navigator_resp_interpolated_.size(); iI++) {
		fPtr[2*iI] = navigator_resp_interpolated_.at(iI);
		fPtr[2*iI+1] = navigator_card_interpolated_.at(iI);
	}

	m1->cont(tmp_m2);
	tmp_m2->cont(m3);

	if (this->next()->putq(m1) < 0) {
		return GADGET_FAIL;
	}

	// free memory
	m2->cont(NULL);
	m2->release();

	return GADGET_OK;
}

// get interpolated navigator signal
void CS_Retro_NavigatorGadget::getNav2D(hoNDArray<std::complex<float> > &aNav)
{
	GDEBUG("\n\n**************************************\n********** get navigator 2D **********\n**************************************\n\n");

	// reconstruct the 1-D projections for all measurements and all channels
	GINFO("domain transformation - k-space to image\n");

	/* MATLAB
	% Reconstruct the 1-D projections for all measurements and all channels
	dImg = fftshift(ifft(ifftshift(dKSpace)));
	dImg = fftshift(ifft(ifftshift(dImg, 3), [], 3), 3);
	dImg = flipdim(dImg, 1); % Invert the RO direction: 1-N -> H-F
	dImg = dImg(iNSamples/4:iNSamples.*3/4 - 1, :, :, :); % RO x t x PE x CH
	*/
	hoNDArray<std::complex<float> > aImg = aNav;
	hoNDFFT_CS<float>::instance()->ifftshift3D(aImg);
	hoNDFFT_CS<float>::instance()->ifft1(aImg);
	hoNDFFT_CS<float>::instance()->fftshift3D(aImg);
	hoNDFFT_CS<float>::instance()->ifft(&aImg, 2, true);
	flip_array(aImg, 0);

	// crop center part due to twofold oversampling
	GINFO("crop center part of two-fold oversampled data..\n");

	std::vector<size_t> vStart, vSize;

	vStart.push_back(static_cast<size_t>(std::floor(0.25*aNav.get_size(0)))-1);
	vStart.push_back(0);
	vStart.push_back(0);
	vStart.push_back(0);

	vSize.push_back(std::floor(0.5*aNav.get_size(0)));
	vSize.push_back(aNav.get_size(1));
	vSize.push_back(aNav.get_size(2));
	vSize.push_back(aNav.get_size(3));

	get_subarray(aImg, vStart, vSize, aImg);

	// get channel power and normalize channel images
	GINFO("calculate channel power and normalize channel images..\n");

	std::vector<float> fPower(iNoChannels_);
	hoNDArray<std::complex<float> > aPower = aImg;
	aPower.fill(std::complex<float>(0,0));

	for (int c = 0; c < iNoChannels_; c++) {
		size_t offset = aImg.get_size(0)*aImg.get_size(1)*aImg.get_size(2)*c;
		hoNDArray<std::complex<float> > SubArray(aImg.get_size(0), aImg.get_size(1), aImg.get_size(2), aImg.get_data_ptr()+offset, false);
		fPower.at(c) = asum(SubArray);

		// fill part of the 3D array
		#pragma omp parallel for
		for (size_t i = 0; i < aImg.get_size(0)*aImg.get_size(1)*aImg.get_size(2); i++) {
			aPower.at(i+offset) = std::complex<float>(fPower.at(c), fPower.at(c));
		}
	}

	divide(aImg, aPower, aImg);

	GINFO("channel images normalized..\n");

	aPower.clear();

	// get x range of respiratory motion & FFT without scrambling
	hoNDArray<std::complex<float> > aFreq = aImg;
	hoNDFFT_CS<float>::instance()->fft(&aFreq, 1, false);
	multiplyConj(aFreq,aFreq,aPower);

	// conversion from complex float to float
	hoNDArray<float> afPower(aPower.get_dimensions());
	for (size_t i = 0; i < afPower.get_number_of_elements(); i++) {
		afPower[i] = aPower[i].real();
	}

	/*
	dIMGres = 1./(double(dNavPeriod)./1000.*double(iNMeasurements)); % The frequency resolution of dIMG in Hz
	dPower = squeeze(sum(dPower(:, round(1./(5.*dIMGres)):round(1./(3.*dIMGres)), :, :), 2)); % RO x PE x CH
	*/
	float fIMGRes = 1.0/((static_cast<float>(GlobalVar::instance()->iNavPeriod_)/1000.0)*static_cast<float>(iNoNav_)); // frequency resolution of aImg in Hz
	hoNDArray<float> aPowerInChan, aPowerAcrossChan;

	vStart.clear();
	vStart.push_back(0);
	vStart.push_back(std::floor(1.0/(5*fIMGRes)-.5));
	vStart.push_back(0);
	vStart.push_back(0);

	vSize.clear();
	vSize.push_back(afPower.get_size(0));
	vSize.push_back(std::ceil(1.0/(3*fIMGRes)-.5)-std::ceil(1.0/(5*fIMGRes)-.5)+1);
	vSize.push_back(afPower.get_size(2));
	vSize.push_back(afPower.get_size(3));

	get_subarray(afPower, vStart, vSize, afPower);

	sum_dim(afPower, 1, afPower); // RO x PE x CH
	sum_dim(afPower, 1, aPowerInChan); // RO x CH
	sum_dim(aPowerInChan, 1, aPowerAcrossChan); // RO x 1

	// Prevent detection of regions in the abdomen
	for (long i = aPowerAcrossChan.get_number_of_elements(); i > std::floor(aPowerAcrossChan.get_number_of_elements()*.75); i--) {
		aPowerAcrossChan.at(i) = 0;
	}

	// get Gaussian filter kernel
	std::vector<float> vGaussian;
	filter1DGaussian(vGaussian, 20);
	arrayConv(aPowerAcrossChan, vGaussian, 0);

	// find index of maximum
	int iMaxIndex = amax(aPowerAcrossChan);

	GINFO("data filtered and maximum determined.. iMaxIndex: %i\n", iMaxIndex);

	if (iMaxIndex < 20) {
		GWARN("iMaxIndex=%d < 20! It is set to 20 to be able to perform further computing. Errors may occur.\n", iMaxIndex);
		iMaxIndex = 20;
	}

	if ((iMaxIndex < 20) || (iMaxIndex > static_cast<int>(aPowerInChan.get_size(0))-20)) {
		GERROR("Error: iMaxIndex out of bounds..\n");

		throw std::range_error("iMaxIndex out of bounds\n");
	}

	//-------------------------------------------------------------------------
	// sort out channels with no relevant information in target area
	GINFO("get channels which contain most information..\n");

	vStart.clear();
	vStart.push_back(iMaxIndex-20);
	vStart.push_back(0);

	vSize.clear();
	vSize.push_back(41);
	vSize.push_back(iNoChannels_);

	get_subarray(aPowerInChan, vStart, vSize,aPowerInChan);

	GINFO("41 elements around maximum cropped..\n");

	std::vector<float> vGoodChannels;

	// loop over channels
	for (int c = 0; c < iNoChannels_; c++) {
		hoNDArray<float> aTmp;
		size_t offset = aPowerInChan.get_size(0)*c;
		aTmp.create(aPowerInChan.get_size(0), aPowerInChan.get_data_ptr()+offset, false);
		int iTmpInd = amax(aTmp);
		vGoodChannels.push_back(aTmp.at(iTmpInd));
	}

	int iIndex = std::max_element(vGoodChannels.begin(), vGoodChannels.end())- vGoodChannels.begin();
	float fMax = vGoodChannels.at(iIndex);
	int iNumGood = 0;

	for (size_t i = 0; i < vGoodChannels.size(); i++) {
		if (vGoodChannels.at(i) > .2*fMax) {
			vGoodChannels.at(i) = 1;
			iNumGood++;
		} else {
			vGoodChannels.at(i) = 0;
		}
	}

	for (size_t i = 0; i < vGoodChannels.size(); i++) {
		GDEBUG("vGoodChannels[%i]: %f\n", i,vGoodChannels.at(i));
	}

	//-------------------------------------------------------------------------
	// get the best PE line
	// get sub array of good channels
	GDEBUG("get best PE line..\n");

	hoNDArray<float> aPowerInPE(41,afPower.get_size(1), iNumGood);

	vStart.clear();
	vStart.push_back(iMaxIndex-20);
	vStart.push_back(0);
	vStart.push_back(0);

	vSize.clear();
	vSize.push_back(41);
	vSize.push_back(afPower.get_size(1));
	vSize.push_back(1);

	GDEBUG("vStart: %i, %i, %i, vSize: %i, %i, %i, afPower size: %i, %i, %i\n", vStart.at(0), vStart.at(1), vStart.at(2), vSize.at(0), vSize.at(1), vSize.at(2), afPower.get_size(0), afPower.get_size(1), afPower.get_size(2));

	size_t o = 0; //helper - only non-zero entries of vGoodChannels are of interest
	for (int c = 0; c < iNoChannels_; c++) {
		if (vGoodChannels.at(c) == 1) {
			hoNDArray<float> aTmp;
			aTmp.create(&vSize);
			vStart.at(2) = c;
			get_subarray(afPower, vStart, vSize, aTmp);

			// fill array
			size_t offset = aPowerInPE.get_size(0)*aPowerInPE.get_size(1)*o;
			for (size_t i = 0; i < aTmp.get_number_of_elements(); i++) {
				aPowerInPE.at(i + offset) = aTmp.at(i);
			}

			o++;
		}
	}

	sum_dim(aPowerInPE, 2, aPowerInPE);
	sum_dim(aPowerInPE, 0, aPowerInPE);

	GINFO("\n aPowerInPE\n");
	aPowerInPE.print(std::cout);

	iMaxIndex = amax(aPowerInPE);
	int iMaxChan = iMaxIndex;

	GINFO("found at %i\n", iMaxIndex);

	//-------------------------------------------------------------------------
	// find best corresponding channels according to best phase encoding position
	GINFO("searching for channels according to best PE line..\n");

	// get sub array
	vStart.clear();
	vStart.push_back(0);
	vStart.push_back(0);
	vStart.push_back(iMaxIndex);
	vStart.push_back(0);

	vSize.clear();
	vSize.push_back(aImg.get_size(0));
	vSize.push_back(aImg.get_size(1));
	vSize.push_back(1);
	vSize.push_back(aImg.get_size(3));

	aFreq.clear();

	get_subarray(aImg, vStart, vSize, aFreq);

	hoNDFFT_CS<float>::instance()->fft(&aFreq, 1, false);
	multiplyConj(aFreq,aFreq,aPower);

	// conversion from complex float to float
	afPower.clear();
	afPower.create(*aPower.get_dimensions());
	for (size_t i = 0; i < afPower.get_number_of_elements(); i++) {
		afPower[i] = aPower[i].real();
	}

	fIMGRes = 1.0/((static_cast<float>(GlobalVar::instance()->iNavPeriod_)/1000.0)*static_cast<float>(iNoNav_)); // frequency resolution of aImg in Hz

	aPowerInChan.clear();
	aPowerAcrossChan.clear();

	vStart.clear();
	vStart.push_back(0);
	vStart.push_back(std::ceil(1.0/(5*fIMGRes)-.5));
	vStart.push_back(0);
	vStart.push_back(0);

	vSize.clear();
	vSize.push_back(afPower.get_size(0));
	vSize.push_back(std::ceil(1.0/(3*fIMGRes)-.5)-std::ceil(1.0/(5*fIMGRes)-.5)+1);
	vSize.push_back(afPower.get_size(2));
	vSize.push_back(afPower.get_size(3));

	GDEBUG("vSize: %i, %i, %i, %i - fIMGRes: %f\n", vSize.at(0), vSize.at(1), vSize.at(2), vSize.at(3), fIMGRes);

	get_subarray(afPower, vStart, vSize, afPower);
	sum_dim(afPower, 1, afPower); // RO x PE x CH
	sum_dim(afPower, 1, aPowerInChan); // RO x CH
	sum_dim(aPowerInChan, 1, aPowerAcrossChan); // RO x 1

	// Prevent detection of regions in the abdomen
	for (long i = aPowerAcrossChan.get_number_of_elements(); i > std::floor(aPowerAcrossChan.get_number_of_elements()*.75); i--) {
		aPowerAcrossChan.at(i) = 0;
	}

	GINFO("filter data with Gaussian kernel..\n");

	// get Gaussian filter kernel
	vGaussian.clear();
	filter1DGaussian(vGaussian, 20);
	arrayConv(aPowerAcrossChan, vGaussian);

	// find index of maximum
	iMaxIndex = amax(aPowerAcrossChan);
	int dX = iMaxIndex;

	GINFO("found at %i\n", iMaxIndex);

	// get good channels
	vStart.clear();
	vStart.push_back(iMaxIndex-20);
	vStart.push_back(0);

	vSize.clear();
	vSize.push_back(41);
	vSize.push_back(aPowerInChan.get_size(1));

	get_subarray(aPowerInChan, vStart, vSize,aPowerInChan);

	vGoodChannels.clear();

	// loop over channels
	for (int c = 0; c < iNoChannels_; c++) {
		size_t offset = aPowerInChan.get_size(0)*c;

		hoNDArray<float> aTmp;
		aTmp.create(aPowerInChan.get_size(0), aPowerInChan.get_data_ptr()+offset, false);

		int iTmpInd = amax(aTmp);
		vGoodChannels.push_back(aTmp.at(iTmpInd));
	}

	iIndex = std::max_element(vGoodChannels.begin(), vGoodChannels.end())-vGoodChannels.begin();
	fMax =vGoodChannels.at(iIndex);
	iNumGood = 0;

	for (size_t i = 0; i < vGoodChannels.size(); i++) {
		if (vGoodChannels.at(i) > .2*fMax) {
			vGoodChannels.at(i) = 1;
			iNumGood++;
		} else {
			vGoodChannels.at(i) = 0;
		}
	}

	for (size_t i = 0; i < vGoodChannels.size(); i++) {
		GDEBUG("vGoodChannels[%i]: %f\n", i,vGoodChannels.at(i));
	}

	// get relevant image - dRelevantImg = squeeze(dImg(:, :, dPos, lGoodChannels));
	vStart.clear();
	vStart.push_back(0);
	vStart.push_back(0);
	vStart.push_back(iMaxChan);
	vStart.push_back(0);

	vSize.clear();
	vSize.push_back(aImg.get_size(0));
	vSize.push_back(aImg.get_size(1));
	vSize.push_back(1);
	vSize.push_back(1);

	hoNDArray<std::complex<float> > acRelevantImg(aImg.get_size(0), aImg.get_size(1), iNumGood);

	o = 0; //helper - only non-zero entries of vGoodChannels are of interest
	hoNDArray<std::complex<float> > afTmp;
	for (int c = 0; c < iNoChannels_; c++) {
		if (vGoodChannels.at(c) == 1) {
			vStart.at(3) = c;
			get_subarray(aImg, vStart, vSize, afTmp);

			// fill array
			size_t offset = acRelevantImg.get_size(0)*acRelevantImg.get_size(1)*o;
			for (size_t i = 0; i < afTmp.get_number_of_elements(); i++) {
				acRelevantImg.at(i + offset) = afTmp.at(i);
			}

			o++;
		}
	}

	//-------------------------------------------------------------------------
	GINFO("get SOSImg\n");

	hoNDArray<float> aRelevantImg(*acRelevantImg.get_dimensions());
	multiplyConj(acRelevantImg,acRelevantImg,acRelevantImg);
	// complex float to float datatype
	for (size_t i = 0; i < aRelevantImg.get_number_of_elements(); i++) {
		aRelevantImg[i] = acRelevantImg[i].real();
	}

	// loop over channels
	size_t tOffset = aRelevantImg.get_size(0)*aRelevantImg.get_size(1);
	hoNDArray<float> hafMax(aRelevantImg.get_dimensions());
	float *fPtr = hafMax.get_data_ptr();
	for (size_t iI = 0; iI < aRelevantImg.get_size(2); iI++) {
		hoNDArray<float> tmp(aRelevantImg.get_size(0), aRelevantImg.get_size(1), aRelevantImg.get_data_ptr()+tOffset*iI);
		iMaxIndex = amax(tmp);
		fMax = tmp.at(iMaxIndex);

		for (size_t iL = 0; iL < tOffset; iL++) {
			fPtr[iL+tOffset*iI] = fMax;
		}
	}

	divide(aRelevantImg, hafMax, aRelevantImg);

	hoNDArray<float> aSOSImg;
	sum_dim(aRelevantImg, 2, aSOSImg);
	sqrt(aSOSImg, aSOSImg);

	// convert array
	hoNDArray<std::complex<float> > cfaSOSImgTest(aSOSImg.get_dimensions());
	cfaSOSImgTest.fill(std::complex<float>(0.0, 0.0));
	std::complex<float> *cfPointer = cfaSOSImgTest.get_data_ptr();
	for (size_t iI = 0; iI < cfaSOSImgTest.get_number_of_elements(); iI++) {
		cfPointer[iI] = std::complex<float>(aSOSImg.at(iI), 0.0);
	}

	//-------------------------------------------------------------------------
	// get navigator
	int iDisplacementMax = 80; // [mm] diaphragm displacement (max. +/- 80mm)
	int iDisplacement = static_cast<int>(std::ceil(static_cast<float>(iDisplacementMax)/(static_cast<float>(field_of_view_[0])/static_cast<float>(aImg.get_size(0))) - 0.5));

	// fill last line in ref image
	hoNDArray<float> aRefImg = aSOSImg;
	aRefImg.fill(0.0);
	for (size_t iI = 0; iI < aSOSImg.get_size(0); iI++) {
		aRefImg.at(iI + aSOSImg.get_size(0)*(aSOSImg.get_size(1)-1)) = aSOSImg.at(iI + aSOSImg.get_size(0)*(aSOSImg.get_size(1)-1));
	}

	// create index vector and navigator vector (filled with zeros)
	std::vector<int> vIdx;
	for (size_t i = 0; i < aRefImg.get_size(1); i++) {
		vIdx.push_back(i);
	}

	std::vector<float> navigator_resp;
	for (size_t i = 0; i < aRefImg.get_size(1); i++) {
		navigator_resp.push_back(0);
	}

	hoNDArray<float> aRMSImg;
	for (int i = aRefImg.get_size(1)-2; i > 1; i--) {// -1 ; i--){
		aRMSImg.create(aRefImg.get_size(0), 2*iDisplacement+1);
		aRMSImg.fill(0.0);

		//MATLAB: tmp = dRefImg(:,idx(i+1:end))
		hoNDArray<float> tmp;

		vStart.clear();
		vStart.push_back(0);
		vStart.push_back(i+1);

		vSize.clear();
		vSize.push_back(aRefImg.get_size(0));
		vSize.push_back(aRefImg.get_size(1)-i-1);

		get_subarray(aRefImg, vStart, vSize, tmp);

		hoNDArray<float> repTmp(tmp.get_dimensions());

		//MATLAB: dSOSImg(:,idx(i)
		hoNDArray<float> aTmp;

		vStart.clear();
		vStart.push_back(0);
		vStart.push_back(vIdx.at(i));

		vSize.clear();
		vSize.push_back(aSOSImg.get_size(0));
		vSize.push_back(1);

		get_subarray(aSOSImg, vStart, vSize, aTmp);

		hoNDArray<float> aTmp2 = aTmp;
		circshift(aTmp2, -iDisplacement-1, 0);

		for (int l = -iDisplacement; l <= iDisplacement; l++) {
			//MATLAB: circshift(dSOSImg(:,idx(i)), iD)
			circshift(aTmp2, 1, 0);

			//MATLAB: (tmp - repmat(circshift(dSOSImg(:,idx(i)), iD),[1 size(tmp,2)]))
			hoNDArray<float> tmp2(tmp.get_dimensions());
			tmp2.fill(0.0); // result of subtraction

			int N = tmp.get_size(0), LE = tmp.get_size(1);
			float *pA = tmp.begin(), *pB = aTmp2.begin(), *pR = tmp2.begin();

			#pragma omp parallel for default(none) schedule(static) shared(N, pA, pB, pR, LE)
			for (int iL = 0; iL < LE; iL++) {
				for (int iE = 0; iE < N; iE++) {
					pR[iE + N*iL] = pA[iE + N*iL] - pB[iE];
				}
			}

			//MATLAB: (tmp - repmat(circshift(dSOSImg(:,idx(i)), iD),[1 size(tmp,2)])).^2
			multiply(tmp2, tmp2, tmp2);

			//MATLAB: sum((tmp - repmat(circshift(dSOSImg(:,idx(i)), iD),[1 size(tmp,2)])).^2,2)
			hoNDArray<float> hTmp2;
			if (tmp2.get_number_of_dimensions() > 1) {
				std::vector<size_t> vD = *tmp2.get_dimensions();
				vD.pop_back();
				hTmp2.create(&vD);
				hTmp2.fill(0.0);

				float *pNewArray = hTmp2.get_data_ptr();
				float *pOldArray = tmp2.get_data_ptr();
				int N = tmp2.get_size(1);
				int L = hTmp2.get_number_of_elements();

				#pragma omp parallel for default(none) schedule(static) shared(N, L, pNewArray, pOldArray)
				for (int sum_dim = 0; sum_dim < N; sum_dim++) {
					for (int i = 0; i < L; i++) {
						pNewArray[i] += pOldArray[i + L*sum_dim];
					}
				}
			}

			//MATLAB: dRMSImg(:,dDisplacement-iD+1) = sum((tmp - repmat(circshift(dSOSImg(:,idx(i)), iD),[1 size(tmp,2)])).^2,2)
			int iOffset = (iDisplacement-l)*hTmp2.get_size(0);
			memcpy(aRMSImg.begin() + iOffset, hTmp2.begin(), sizeof(float)*hTmp2.get_size(0));
		}

		//MATLAB: sum(dRMSImg(dX-round(dDisplacement/2):dX+round(dDisplacement/2),:))
		vStart.clear();
		vStart.push_back(dX-static_cast<int>((static_cast<float>(iDisplacement)/2)+.5));
		vStart.push_back(0);

		vSize.clear();
		vSize.push_back(iDisplacement+1);
		vSize.push_back(aRMSImg.get_size(1));

		get_subarray(aRMSImg, vStart, vSize, aRMSImg);

		hoNDArray<std::complex<float>> cfaRMSImgTest(aRMSImg.get_dimensions());
		cfaRMSImgTest.fill(std::complex<float>(0.0, 0.0));
		std::complex<float> *cfPointer = cfaRMSImgTest.get_data_ptr();
		for (size_t iI = 0; iI < cfaRMSImgTest.get_number_of_elements(); iI++) {
			cfPointer[iI] = std::complex<float>(aRMSImg.at(iI), 0.0);
		}

		sum_dim(aRMSImg, 0, aRMSImg);

		cfaRMSImgTest.clear();
		cfaRMSImgTest.create(aRMSImg.get_dimensions());
		cfaRMSImgTest.fill(std::complex<float>(0.0, 0.0));
		cfPointer = cfaRMSImgTest.get_data_ptr();
		for (size_t iI = 0; iI < cfaRMSImgTest.get_number_of_elements(); iI++) {
			cfPointer[iI] = std::complex<float>(aRMSImg.at(iI), 0.0);
		}

		//MATLAB: min(sum(dRMSImg(dX-round(dDisplacement/2):dX+round(dDisplacement/2),:)))
		// TODO: change back to amin (Gadgetron project seems to introduce the error)
// 		int iMinVal = amin(&aRMSImg);
		int iMinVal = aRMSImg.at(0);
		for (size_t i = 0; i < aRMSImg.get_number_of_elements(); i++) {
			if (aRMSImg.at(i) < iMinVal) {
				iMinVal = aRMSImg.at(i);
			}
		}

		//MATLAB: dDisplacement + 1 - dNav(i)
		navigator_resp.at(i) = iDisplacement - iMinVal;

		//MATLAB: circshift(dSOSImg(:,idx(i)), dNav(i))
		hoNDArray<float> aTmp3 = aTmp;
		circshift(aTmp3, navigator_resp.at(i), 0);

		if (i%20 == 0) {
			GINFO("Getting Navigator - %.1f %%\n", static_cast<float>(aRefImg.get_size(1)-2-i)/static_cast<float>(aRefImg.get_size(1)-2)*100);
		}

		//MATLAB: dRefImg(:,idx(i)) = circshift(dSOSImg(:,idx(i)), dNav(i))
		memcpy(aRefImg.get_data_ptr()+vIdx.at(i)*aRefImg.get_size(0), aTmp3.get_data_ptr(), sizeof(float)*aTmp3.get_size(0));
	}

	for (size_t i = 0; i < navigator_resp.size(); i++) {
		navigator_resp.at(i) *= -1;
	}

	// get Gaussian filter kernel and calculate convolution with navigator data
	vGaussian.clear();
	filter1DGaussian(vGaussian, 5);
	vectorConv(navigator_resp, vGaussian, 0);

	//-------------------------------------------------------------------------
	// interpolate navigator data signal to TR intervals
	GINFO("interpolation of navigator data to TR intervals..\n");

	for (size_t i = 0; i < navigator_resp.size(); i++) {
		navigator_resp.at(i) = -navigator_resp.at(i);
	}

	int iMin = std::min_element(navigator_resp.begin(), navigator_resp.end())-navigator_resp.begin();
	float fMin = navigator_resp.at(iMin);
	for (size_t i = 0; i < navigator_resp.size(); i++) {
		navigator_resp.at(i) -= fMin;
	}

	// build vector with elements 0..lNoScans_ to interpolate navigator_resp_interpolated_ below
	std::vector<float> vNavIndNew;
	for (long i = 0; i < lNoScans_; i++) {
		vNavIndNew.push_back(i);
	}

	GDEBUG("vNavInd size: %i, navigator_resp size: %i, vNavIndNew size: %i\n", GlobalVar::instance()->vNavInd_.size(), navigator_resp.size(), vNavIndNew.size());

	std::vector<float> vNavInd = GlobalVar::instance()->vNavInd_;
	navigator_resp_interpolated_ = interp1<float>(vNavInd, navigator_resp, vNavIndNew);

	return;
}

// get interpolated navigator signal by Principal Component Analysis
void CS_Retro_NavigatorGadget::getNav2DPCA(hoNDArray<std::complex<float> > &aNav)
{
	GDEBUG("\n\n**************************************\n********** get navigator 2D **********\n**************************************\n\n");

	// reconstruct the 1-D projections for all measurements and all channels
	GINFO("domain transformation - k-space to image\n");

	size_t iNSamples		= aNav.get_size(0);
	size_t iNMeasurement	= aNav.get_size(1);
	size_t iNavRes	 		= aNav.get_size(2);
	size_t iNChannels		= aNav.get_size(3);

	/* MATLAB
	% Reconstruct the 1-D projections for all measurements and all channels
	dImg = fftshift(ifft(ifftshift(dKSpace)));
	*/
	hoNDArray<std::complex<float> > aImg = aNav;
	hoNDFFT_CS<float>::instance()->ifftshift3D(aImg);
	hoNDFFT_CS<float>::instance()->ifft1(aImg);
	hoNDFFT_CS<float>::instance()->fftshift3D(aImg);

	// ATTENTION: Work with [s] values
	// dNavPeriod = dNavPeriod/1000; dTR = dTR/1000;
	min_card_freq_ /= 60;
	max_card_freq_ /= 60;
	min_resp_freq_ /= 60;
	max_resp_freq_ /= 60;

	// 1. Step: Restack
	// is already done in C++ code

	// 2. Step: Compute PCA based on KLT principal components are saved in coeff in descending order
	std::vector<size_t> coeff_dims;
	coeff_dims.push_back(iNMeasurement);
	coeff_dims.push_back(iNMeasurement);

	hoNDArray<std::complex<float> > coeff;
	coeff.create(&coeff_dims);
	coeff.delete_data_on_destruct(true);

	// prepare aImg for KLT
	// first: permute
	std::vector<size_t> aImg_new_order;
	aImg_new_order.push_back(0);
	aImg_new_order.push_back(2);
	aImg_new_order.push_back(3);
	aImg_new_order.push_back(1);
	aImg = permute(aImg, aImg_new_order);

	// then reshape
	std::vector<size_t> new_aImg_dims;
	new_aImg_dims.push_back(iNSamples*iNavRes*iNChannels);
	new_aImg_dims.push_back(iNMeasurement);
	aImg.reshape(&new_aImg_dims);

	GINFO("Performing KLT (may take a while)\n");

	hoNDKLT<std::complex<float> > VT;
	VT.prepare(aImg, static_cast<size_t>(1), static_cast<size_t>(0), true);
	VT.eigen_vector(coeff);

	GINFO("Continuing with search for motion\n");

	// 3. Step: search for respiratory motion
	// calculate frequency
	//%dFs = 1/((length(iLC)*dTR/1000)/(length(coeff)));
	double f_s = iNMeasurement/(lNoScans_*(GlobalVar::instance()->fTR_/1000.0)); // Get the sampling frequency (/1000 because fTR_ is in ms, not in s)

	// get next base of 2
	//%dFactorfft = 2.^nextpow2(size(dCoeff,1));
	unsigned int factor_fft = std::pow(2, std::ceil(log(coeff.get_size(0))/log(2)));

	// first zero padd coeff
	hoNDArray<std::complex<float> > coeff_padded;
	std::vector<size_t> coeff_padded_dims;
	coeff_padded_dims.push_back(factor_fft);
	coeff_padded_dims.push_back(coeff.get_size(1));
	coeff_padded.create(&coeff_padded_dims);
	for (size_t col = 0; col < coeff.get_size(1); col++) {
		size_t offset_old = col * coeff.get_size(0);
		size_t offset_new = col * coeff_padded.get_size(0);
		memcpy(coeff_padded.get_data_ptr()+offset_new, coeff.get_data_ptr()+offset_old, coeff.get_size(0)*sizeof(coeff.at(0)));
	}

	// and continue with fft
	// note: ifftshift2D is done before zero padding, thus none here
	hoNDFFT_CS<float>::instance()->fft1(coeff_padded);

	//%dCoeffF = abs(dCoeffF);
	hoNDArray<float> coeff_abs;
	coeff_abs.create(&coeff_padded_dims);
	for (size_t i = 0; i < coeff_padded.get_number_of_elements(); i++) {
		coeff_abs.at(i) = abs(coeff_padded.at(i));
	}

	// filtering
	// calculate lower and upper boundary frequency
	// Note: we only need the floor() value, so we build it directly instead of later on.
	//%Fl = dFactorfft/dFs * dCutOffResp(1);
	const unsigned int resp_f_l = std::floor(factor_fft/f_s * min_resp_freq_);
	//%Fu = dFactorfft/dFs * dCutOffResp(2);
	const unsigned int resp_f_u = std::floor(factor_fft/f_s * max_resp_freq_);

	if (resp_f_u <= resp_f_l) {
		GERROR("resp_f_u (=%d) must be greater than resp_f_l (=%d). Please set parameters correct!\n", resp_f_u, resp_f_l);
		throw runtime_error("Illegal parameters min_resp_freq_ or max_resp_freq_!");
	}

	// now get iPeak (dVal can be omitted). iPeak is the column number where the maximum value occures.
	// we could either implement it the Matlab way (crop hoNDArray, search max value per line and max col position)
	// or we do some intelligent data handling to get the position directly:
	float max_val = std::numeric_limits<float>::min();		// initialize as minimum (it could also be 0 because coeff_abs only contains abs values)
	size_t peak_position = 0;		// note: -1 is insufficient because size_t is unsigned, so pos is max. But we will rewrite max_val either.
	for (size_t f = static_cast<size_t>(resp_f_l); f <= static_cast<size_t>(resp_f_u); f++) {
		for (size_t component_number = search_range_min_-1; component_number < search_range_max_; component_number++) {		// be aware: search_range counts MATLAB like (from 1 to length)
			size_t pos = component_number * coeff_abs.get_size(0) + f;

			if (max_val < coeff_abs.at(pos)) {
				max_val = coeff_abs.at(pos);
				peak_position = component_number;
			}
		}
	}

	// now extract navi signal
	//%dRespNavi = real(dCoeff(:,iPeak)) - imag(dCoeff(:,iPeak));
	std::vector<float> resp_navi;

	for (size_t f = 0; f < coeff.get_size(0); f++) {
		size_t pos = peak_position * coeff.get_size(0) + f;
		resp_navi.push_back(coeff.at(pos).real() - coeff.at(pos).imag());
	}

	// and convolute
	//%dRespNavi = conv(dRespNavi, fGaussianLP(5), 'same');
	// first build gaussian low pass
	std::vector<float> gaussian_lowpass = {
		0.0269,
		0.2334,
		0.4794,
		0.2334,
		0.0269,
	};
	// convolute
	resp_navi = arma::conv_to<std::vector<float> >::from(arma::conv(arma::Col<float>(resp_navi), arma::Col<float>(gaussian_lowpass), "same"));

	// Note: Matlab implementation: %dRespNavi = -dRespNavi;
	// is implicitly done during KLT and can be omitted here

	// build vector with elements 0..lNoScans_ to interpolate navigator_resp_interpolated_ below
	std::vector<float> nav_ind_new;
	for (long i = 0; i < lNoScans_; i++) {
		nav_ind_new.push_back(i);
	}

	// get navigator indices
	std::vector<float> nav_ind = GlobalVar::instance()->vNavInd_;

	GDEBUG("nav_ind size: %i, resp_navi size: %i, nav_ind_new size: %i\n", nav_ind.size(), resp_navi.size(), nav_ind_new.size());

	// interpolate to output vector
	navigator_resp_interpolated_ = interp1<float>(nav_ind, resp_navi, nav_ind_new);

	// 4. Step: search for cardiac motion
	//%Fl = dFactorfft/dFs * dCutOffCard(1);
	const unsigned int card_f_l = std::floor(factor_fft/f_s * min_card_freq_);
	//%Fu = dFactorfft/dFs * dCutOffCard(2);
	const unsigned int card_f_u = std::floor(factor_fft/f_s * max_card_freq_);

	if (card_f_u <= card_f_l) {
		GERROR("card_f_u (=%d) must be greater than card_f_l (=%d). Please set parameters correct!\n", card_f_u, card_f_l);
		throw runtime_error("Illegal parameters min_resp_freq_ or max_resp_freq_!");
	}

	// now get iPeak (dVal can be omitted). iPeak is the column number where the maximum value occures.
	// we could either implement it the Matlab way (crop hoNDArray, search max value per line and max col position)
	// or we do some intelligent data handling to get the position directly:
	max_val = std::numeric_limits<float>::min();		// initialize as minimum (it could also be 0 because coeff_abs only contains abs values)
	peak_position = 0;		// note: -1 is insufficient because size_t is unsigned, so pos is max. But we will rewrite max_val either.
	for (size_t f = static_cast<size_t>(card_f_l); f <= static_cast<size_t>(card_f_u); f++) {
		for (size_t component_number = search_range_min_-1; component_number < search_range_max_; component_number++) {		// be aware: search_range counts MATLAB like (from 1 to length)
			size_t pos = component_number * coeff_abs.get_size(0) + f;

			if (max_val < coeff_abs.at(pos)) {
				max_val = coeff_abs.at(pos);
				peak_position = component_number;
			}
		}
	}

	// now extract navi signal
	//%dCardNavi = real(dCoeff(:,iPeak)) - imag(dCoeff(:,iPeak));
	for (size_t f = 0; f < coeff.get_size(0); f++) {
		size_t pos = peak_position * coeff.get_size(0) + f;
		navigator_card_interpolated_.push_back(coeff.at(pos).real() - coeff.at(pos).imag());
	}

	// interpolate signal
	//%dCardNavi = interp1(0:dNavPeriod:dNavPeriod*(size(dCoeff,1)-1), dCardNavi, 0:dTR:dTR*(length(iLC)-1), 'pchip');
	// first build new interpolation indices
	nav_ind.clear();
	nav_ind_new.clear();
	for (unsigned int i = 0; i < GlobalVar::instance()->iNavPeriod_*(coeff.get_size(0)-1); i += GlobalVar::instance()->iNavPeriod_) {
		nav_ind.push_back(i);
	}
	for (int counter = 0; counter < lNoScans_; counter++) {
		nav_ind_new.push_back(counter*GlobalVar::instance()->fTR_);
	}
	navigator_card_interpolated_ = interp1<float>(nav_ind, navigator_card_interpolated_, nav_ind_new);

	// change f_s
	//%dFs = 1./(dTR/1000); % now interpolated to TR level
	f_s = 1.0f/(GlobalVar::instance()->fTR_/1000);

	// butterworth filtering
	// % smoothing and R-peak detection
	//%[b,a] = fButterself(dCutOffCard(1)/(dFs/2), dCutOffCard(2)/(dFs/2));
	//%dCardNavi = FiltFiltSelf(b,a,double(dCardNavi));
	butterworth_filtering(min_card_freq_/(f_s/2.0f), max_card_freq_/(f_s/2.0f), navigator_card_interpolated_);

	//%dCardNavi = diff(dCardNavi); % search R-peak and differential signal
	navigator_card_interpolated_ = arma::conv_to<std::vector<float> >::from(arma::diff(arma::Col<float>(navigator_card_interpolated_)));

	//%dECGInt_ms = (dCardNavi-min(dCardNavi(floor(length(dCardNavi)/10):floor(length(dCardNavi)*8/10))))/(max(dCardNavi(floor(length(dCardNavi)/10):floor(length(dCardNavi)*8/10)))-min(dCardNavi(floor(length(dCardNavi)/10):floor(length(dCardNavi)*8/10))));
	// first get correction factors
	float factor_subtract	= *std::min_element(std::begin(navigator_card_interpolated_)+(navigator_card_interpolated_.size()/10), std::begin(navigator_card_interpolated_)+(navigator_card_interpolated_.size()*8/10));	// note: integer divisions ensure std::floor()
	float factor_divide		= *std::max_element(std::begin(navigator_card_interpolated_)+(navigator_card_interpolated_.size()/10), std::begin(navigator_card_interpolated_)+(navigator_card_interpolated_.size()*8/10)) - factor_subtract;

	// apply factors
	for (size_t i = 0; i < navigator_card_interpolated_.size(); i++) {
		navigator_card_interpolated_.at(i) = (navigator_card_interpolated_.at(i)-factor_subtract) / factor_divide;
	}
}

void CS_Retro_NavigatorGadget::butterworth_filtering(const double fl, const double fh, std::vector<float> &signal)
{
	// Filter the Signal with a first order butterworth filter

	//===============================================================
	//calculate the numerator and denominator of a first order butterworth filter. (End of calulation is indicated by =======)
	//===============================================================

	//ul = 4*tan(pi*fl/2);
	//uh = 4*tan(pi*fh/2);
	float ul = 4*tan(M_PI*fl/2);
	float uh = 4*tan(M_PI*fh/2);

	//Bandwidth and center frequency
	float Bw = uh - ul;
	float Wn = std::sqrt(ul*uh);

	// Matlab:
	//t1 = [1+(Wn*(-Bw/Wn)/4) Wn/4; -Wn/4 1];
	//t2 = [1-(Wn*(-Bw/Wn)/4) -Wn/4; Wn/4 1];
	//ad = inv(t2)*t1;
	// Note: indexing follows mathematical convention: t1[row number][column number]
	float t1[2][2], t2[2][2];
	std::complex<float> ad[2][2];	// note: only real values in ad, but calculation of e later on must be complex

	t1[0][0] = 1+(Wn*(-Bw/Wn)/4);
	t1[0][1] = Wn/4;
	t1[1][0] = -Wn/4;
	t1[1][1] = 1;

	// also transpose
	t2[0][0]= 1-(Wn*(-Bw/Wn)/4);
	t2[0][1]= -Wn/4;
	t2[1][0]= Wn/4;
	t2[1][1]= 1;

	// matlab: ad = inv(t2)*t1;
	float det_t2 = t2[0][0]*t2[1][1]-t2[0][1]*t2[1][0];
	ad[0][0] = (+t2[1][1]*t1[0][0]-t2[0][1]*t1[1][0])/det_t2;
	ad[0][1] = (+t2[1][1]*t1[0][1]-t2[0][1]*t1[1][1])/det_t2;
	ad[1][0] = (-t2[1][0]*t1[0][0]+t2[0][0]*t1[1][0])/det_t2;
	ad[1][1] = (-t2[1][0]*t1[0][1]+t2[0][0]*t1[1][1])/det_t2;

	//%den = poly(ad);
	//e = eig(ad);
	std::complex<float> e[2];

	// computate eigenvalues
	e[0] = (ad[0][0]+ad[1][1]+std::sqrt(std::pow(ad[0][0]+ad[1][1], 2) - std::complex<float>(4)*(ad[0][0]*ad[1][1]-ad[0][1]*ad[1][0])))/std::complex<float>(2);
	e[1] = (ad[0][0]+ad[1][1]-std::sqrt(std::pow(ad[0][0]+ad[1][1], 2) - std::complex<float>(4)*(ad[0][0]*ad[1][1]-ad[0][1]*ad[1][0])))/std::complex<float>(2);

	std::complex<float> kern[3];

	//den = [1 0 0];
	float den[] = {1, 0, 0};

	// In Matlab:
	//% Expand recursion formula
	//den(2) = den(2) - e(1)*den(1);
	//den(3) = den(3) - e(2)*den(2);
	//den(2) = den(2) - e(2)*den(1);
	//
	// Some thoughts:
	//	- e is either real or conjugate complex (see above)
	//	- in complex case: calculation of den becomes real (because e conjugate complex)
	//	- then: imaginary part can always be ignored and the matlab calculation reduces to:
	den[1] = -e[0].real() - e[1].real();
	den[2] = e[0].real() * e[1].real() + std::pow(e[0].imag(), 2);

	Wn = 2*atan(Wn/4);

	//%  normalize so |H(w)| == 1:
	//%kern = exp(-1i*Wn*(0:2));
	for (size_t k = 0; k < ARRAYSIZE(kern); k++) {
		kern[k] = std::exp(std::complex<float>(0.0, -1.0)*std::complex<float>(Wn)*std::complex<float>(k));
	}

	//f = (kern(1)*den(1)+kern(2)*den(2)+kern(3)*den(3))/(kern(1)-kern(3));
	std::complex<float> f = (kern[0]*den[0]+kern[1]*den[1]+kern[2]*den[2])/(kern[0]-kern[2]);

	float num[3] = {f.real(), 0, -f.real()};

	//===========================================================
	//end of calculating the numerator and denominator of the first order butterworth filter
	//===========================================================

	//filtfilt() equivalent function. b = num and a = den

	//============================================================
	//start of zero phase digital filter function
	//============================================================

	//n    = length(den); always 3 in first order case
	//z(n) = 0;
	//num = num / den(1);
	//den = den / den(1);
	float z[3] = {0};

	//Y    = zeros(size(X));
	//for m = 1:length(Y)
	//  Y(m) = num(1) * X(m) + z(1);
	//   for i = 2:n
	//      z(i - 1) = num(i) * X(m) + z(i) - den(i) * Y(m);
	//   end
	//end
	std::vector<float> Y;
	for (size_t m = 0; m < signal.size(); m++) {
		Y.push_back(num[0] * signal.at(m) + z[0]);

		for (size_t i = 1; i < ARRAYSIZE(den); i++) {
			z[i-1] = num[i] * signal.at(m) + z[i] - den[i] * Y.at(m);
		}
	}

	//clear z
	//z(n) = 0;
	z[0] = 0;
	z[1] = 0;
	z[2] = 0;

	//flip vector
	std::reverse(Y.begin(),Y.end());
	signal.clear();
	signal = Y;

	//Y    = zeros(size(X));
	// second round filtering (backward)
	//for m = 1:length(Y)
	//   Y(m) = b(1) * X(m) + z(1);
	//   for i = 2:n
	//      z(i - 1) = b(i) * X(m) + z(i) - a(i) * Y(m);
	//   end
	//end
	Y.clear();
	for (size_t m = 0; m < signal.size(); m++) {
		Y.push_back(num[0] * signal.at(m) + z[0]);

		for (size_t i = 1; i < ARRAYSIZE(den); i++) {
			z[i-1] = num[i] * signal.at(m) + z[i] - den[i] * Y.at(m);
		}
	}

	//flip again
	std::reverse(Y.begin(),Y.end());
	signal = Y;
	Y.clear();

	//============================================================
	//end of zero phase digital filter function
	//============================================================
}

GADGET_FACTORY_DECLARE(CS_Retro_NavigatorGadget)