//
// Copyright (c) 2020 INRIA
//

#include "pinocchio/multibody/liegroup/liegroup.hpp"
#include "pinocchio/multibody/liegroup/liegroup-collection.hpp"
#include "pinocchio/multibody/liegroup/liegroup-generic.hpp"
#include "pinocchio/multibody/liegroup/cartesian-product-variant.hpp"
#include "pinocchio/multibody/liegroup/cartesian-product.hpp"

#include "pinocchio/multibody/joint/joint-generic.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include <boost/algorithm/string.hpp>

using namespace pinocchio;

namespace pinocchio {
template<typename Derived>
std::ostream& operator<< (std::ostream& os, const LieGroupBase<Derived>& lg)
{
  return os << lg.name();
}
template<typename LieGroupCollection>
std::ostream& operator<< (std::ostream& os, const LieGroupGenericTpl<LieGroupCollection>& lg)
{
  return os << lg.name();
}
} // namespace pinocchio

template<typename Scalar, int Options, template<typename S, int O> class LieGroupCollectionTpl>
struct TestCartesianProduct
{

  typedef LieGroupCollectionTpl<Scalar,Options> LieGroupCollection;

  typedef LieGroupGenericTpl<LieGroupCollection> LieGroupGeneric;
  typedef typename LieGroupGeneric::ConfigVector_t ConfigVector_t;
  typedef typename LieGroupGeneric::TangentVector_t TangentVector_t;

  typedef CartesianProductOperationVariantTpl<Scalar, Options, LieGroupCollectionTpl > CartesianProduct;

  template<typename Derived>
  void operator() (const LieGroupBase<Derived> & lg) const
  {
    LieGroupGenericTpl<LieGroupCollection> lg_generic(lg.derived());
    CartesianProduct cp(lg_generic);
    test(lg,cp);

    CartesianProduct cp2;
    cp2.append(lg);
    BOOST_CHECK(cp == cp2);
  }

  template<typename LieGroup>
  static void test(const LieGroupBase<LieGroup> & lg,
                   const CartesianProduct & cp)
  {
    BOOST_CHECK(lg.nq() == cp.nq());
    BOOST_CHECK(lg.nv() == cp.nv());

    std::cout << "name: " << cp.name() << std::endl;

    BOOST_CHECK(lg.neutral() == cp.neutral());

    typedef typename LieGroup::ConfigVector_t ConfigVector;
    typedef typename LieGroup::TangentVector_t TangentVector;
    typedef typename LieGroup::JacobianMatrix_t JacobianMatrix;

    ConfigVector q0 = lg.random();
    ConfigVector q1 = lg.random();
    TangentVector v = TangentVector_t::Random(lg.nv());
    ConfigVector qout_ref(lg.nq()), qout(lg.nq());
    lg.integrate(q0, v, qout_ref);
    cp.integrate(q0, v, qout);

    BOOST_CHECK(qout.isApprox(qout_ref));

    TangentVector v_diff_ref(lg.nv()), v_diff(lg.nv());
    lg.difference(q0,q1,v_diff_ref);
    cp.difference(q0,q1,v_diff);

    BOOST_CHECK(v_diff_ref.isApprox(v_diff));
    BOOST_CHECK_EQUAL(lg.squaredDistance(q0, q1), cp.squaredDistance(q0, q1));
    BOOST_CHECK_EQUAL(lg.distance(q0, q1), cp.distance(q0, q1));

    JacobianMatrix
    J_ref(JacobianMatrix::Zero(lg.nv(),lg.nv())),
    J(JacobianMatrix::Zero(lg.nv(),lg.nv()));

    lg.dDifference(q0, q1, J_ref, ARG0);
    cp.dDifference(q0, q1, J, ARG0);

    BOOST_CHECK(J.isApprox(J_ref));

    lg.dDifference(q0, q1, J_ref, ARG1);
    cp.dDifference(q0, q1, J, ARG1);

    BOOST_CHECK(J.isApprox(J_ref));

    lg.dIntegrate(q0, v, J_ref, ARG0);
    cp.dIntegrate(q0, v, J, ARG0);

    BOOST_CHECK(J.isApprox(J_ref));

    lg.dIntegrate(q0, v, J_ref, ARG1);
    cp.dIntegrate(q0, v, J, ARG1);

    BOOST_CHECK(J.isApprox(J_ref));

    BOOST_CHECK(cp.isSameConfiguration(q0,q0));
    ConfigVector q_rand;
    cp.random(q_rand);
    ConfigVector q_rand_copy = q_rand;

    lg.normalize(q_rand_copy);
    cp.normalize(q_rand);
    BOOST_CHECK(q_rand.isApprox(q_rand_copy));

    const ConfigVector lb(-ConfigVector::Ones(lg.nq()));
    const ConfigVector ub( ConfigVector::Ones(lg.nq()));

    cp.randomConfiguration(lb, ub, q_rand);
  }
};

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_cartesian_product_with_liegroup_variant)
{
  boost::mpl::for_each<LieGroupCollectionDefault::LieGroupVariant::types>(TestCartesianProduct<double,0,LieGroupCollectionDefaultTpl>());
}

BOOST_AUTO_TEST_CASE(test_cartesian_product_vs_cartesian_product_variant)
{
  typedef SpecialEuclideanOperationTpl<3,double,0> SE3;
  typedef VectorSpaceOperationTpl<3,double,0> Rn;

  typedef CartesianProductOperation<SE3, Rn> CPRef;
  typedef CartesianProductOperationVariantTpl<double, 0, LieGroupCollectionDefaultTpl > CP;

  SE3 lg1; Rn lg2;
  typedef LieGroupGenericTpl<CP::LieGroupCollection> LieGroupGeneric;
  LieGroupGeneric lg1_variant(lg1);
  LieGroupGeneric lg2_variant(lg2);

  CP cartesian_product(lg1_variant,lg2_variant);
  CP cartesian_product2(lg1_variant); cartesian_product2.append(lg2_variant);
  std::cout << "cartesian_product: " << cartesian_product << std::endl;

  BOOST_CHECK(cartesian_product == cartesian_product2);
  CPRef cartesian_product_ref;

  TestCartesianProduct<double,0,LieGroupCollectionDefaultTpl>::test(cartesian_product_ref,
                                                                    cartesian_product);
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/numeric/mtl/mtl.hpp>

int main()
try {
    using namespace mtl;

    dense2D<int> A(2, 2), B(2, 2), C(4, 4);

    for (size_t r= 0; r < 2; ++r)
        for (size_t c= 0; c < 2; ++c) {
            A[r][c]= (r+1) * 10 + c+1;
            B[r][c]= (r+1) * 1000 + (c+1) * 100;
        }

    C= kron(A, B);
    std::cout << "kron(A, B) is\n" << C;

    MTL_THROW_IF(C[0][0] != 12100, mtl::runtime_error("Wrong value in C[0][0]"));
    MTL_THROW_IF(C[3][3] != 48400, mtl::runtime_error("Wrong value in C[3][3]"));

    return EXIT_SUCCESS;
}
catch (const mtl::runtime_error& e) {
    std::cerr << "Caught an MTL runtime error: " << e.what() << std::endl;
    return EXIT_FAILURE;
}


//cl /EHsc /I D:\Project\boost_1_53_0\boost_1_53_0\ transform_iterator.cpp
#include <iostream>
#include <functional>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/permutation_iterator.hpp>
#include <boost/functional.hpp>

template< typename T >
struct square
{
    T operator() (const T& x)  const { return x * x; }
    typedef T result_type;
};

int main()
{

	int x[] = { 1, 2, 3, 4, 5, 6, 7, 8 };
	int y[] = { 1, 2, 3, 4, 5, 6, 7, 8 };
	const int N = sizeof(x)/sizeof(int);

	typedef square <int>  Function;

    //Example 1///////////////////////////////
	typedef boost::counting_iterator<int>                       count_iterator;
	typedef boost::transform_iterator<Function, count_iterator> square_transform_count_iterator;
	count_iterator count_i_begin(0), count_i_end(N);
	square_transform_count_iterator square_transform_begin(count_i_begin, Function());
	square_transform_count_iterator square_transform_end  (count_i_end,   Function());

	std::cout << "\n\nSquaring with counting iterator:" << std::endl;
	while (square_transform_begin != square_transform_end)
	  std::cout << *square_transform_begin++ << " ";
	std::cout << std::endl;

	//Example 2///////////////////////////////
	typedef boost::transform_iterator<Function, int *> square_transform_raw_iterator;
	{
		square_transform_raw_iterator square_raw_begin(x, Function());
		square_transform_raw_iterator square_raw_end(x+N, Function());
		std::cout << "\n\nSquaring with raw input buffer:" << std::endl;
		while (square_raw_begin != square_raw_end)
		  std::cout << *square_raw_begin++ << " " ;
		std::cout << std::endl;

		std::cout << "Checking whether input buffer is modified.\n Input Buffer x is:" << std::endl;
		for(int index=0; index<N; index++)
		  std::cout << x[index] << " " ;
		std::cout << std::endl;
	}

	//Example 3///////////////////////////////
	typedef boost::transform_iterator<Function, square_transform_raw_iterator> square_transform_transform_iterator;
	{
		square_transform_raw_iterator square_raw_begin(x, Function());
		square_transform_raw_iterator square_raw_end(x+N, Function());
		square_transform_transform_iterator square_transform_begin(square_raw_begin, Function());
		square_transform_transform_iterator square_transform_end  (square_raw_end,   Function());

		std::cout << "\nExample 3\nSquare and Square of the raw input buffer:" << std::endl;
		while (square_transform_begin != square_transform_end)
		  std::cout << *square_transform_begin++ << " " ;
		std::cout << std::endl;

		std::cout << "Checking whether input buffer is modified.\n Input Buffer x is:" << std::endl;
		for(int index=0; index<N; index++)
		  std::cout << x[index] << " " ;
		std::cout << std::endl;
	}

	typedef boost::permutation_iterator<square_transform_raw_iterator, count_iterator> permutation_iterator;
	//Example 4///////////////////////////////
    {
		square_transform_raw_iterator square_raw_begin(x, Function());
		square_transform_raw_iterator square_raw_end(x+N, Function());

		permutation_iterator perm_begin(square_raw_begin, count_i_begin);
		permutation_iterator perm_end  (square_raw_end, count_i_end);

		std::cout << "\nUsing Permutation Iterator with transform and counting Iterator:" << std::endl;
		while (perm_begin != perm_end)
		  std::cout << *perm_begin++ << " ";
		std::cout << std::endl;
	}

    //Example 5///////////////////////////////
    {
		typedef boost::permutation_iterator<int *, count_iterator> permute_iterator;
		permute_iterator perm_begin(x, count_i_begin);
		permute_iterator perm_end  (x+N, count_i_end);
        //First modify
		*perm_begin = 10;

		std::cout << "\nUsing Permutation Iterator with Simple buffer:" << std::endl;
		while (perm_begin != perm_end)
		  std::cout << *perm_begin++ << " ";
		std::cout << std::endl;
	}

    //Example 5///////////////////////////////
    {
		typedef boost::permutation_iterator<int *, count_iterator> permute_iterator;
		typedef boost::transform_iterator<Function, permute_iterator> permute_transform_iterator;
		permute_iterator perm_begin(x, count_i_begin);
		permute_iterator perm_end  (x+N, count_i_end);

		permute_transform_iterator perm_transform_begin(perm_begin, Function());
		permute_transform_iterator perm_transform_end  (perm_end, Function());
		//First modify
	}

	return 0;
}


// This file is part of MLDB. Copyright 2015 mldb.ai inc. All rights reserved.

/*
   confidence_interval_test.cc
   Copyright (c) 2010 mldb.ai inc.  All rights reserved.
*/

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#include "mldb/utils/confidence_intervals.h"
#include <boost/test/unit_test.hpp>
#include <iostream>

using namespace std;
using namespace MLDB;

BOOST_AUTO_TEST_CASE( conf_intervals )
{
    ConfidenceIntervals ci(0.5);
    vector<double> sample = {1.0,1.0,2.0,3.0,3.0};
    BOOST_CHECK_GE(2, ci.bootstrapMeanLowerBound(sample, 10000, 4));
    BOOST_CHECK_LE(2, ci.bootstrapMeanUpperBound(sample, 10000, 4));
    auto b = ci.bootstrapMeanTwoSidedBound(sample, 10000, 4);
    BOOST_CHECK_GE(2, b.first);
    BOOST_CHECK_LE(2, b.second);
}

BOOST_AUTO_TEST_CASE( conf_intervals_wilson )
{
    // from boost doc:
    // In order to obtain a two sided bound on the success fraction, you call both find_lower_bound_on_p and find_upper_bound_on_p each with the same arguments. If the desired risk level that the true success fraction lies outside the bounds is α, then you pass α/2 to these functions. So for example a two sided 95% confidence interval would be obtained by passing α = 0.025 to each of the functions.
    float confidence = 0.8;
    float alpha = (1 - confidence) / 2;

    // comparing against http://epitools.ausvet.com.au/content.php?page=CIProportion&SampleSize=200&Positive=35&Conf=0.8&Digits=3
    // and https://gist.github.com/paulgb/6627336

    cout << "Wilson" << endl;
    ConfidenceIntervals cI(alpha, "wilson");
    BOOST_CHECK_CLOSE( cI.binomialLowerBound(200, 35), 0.1432, 0.1);
    BOOST_CHECK_CLOSE( cI.binomialUpperBound(200, 35), 0.212, 0.1);

    cout << "CP" << endl;
    ConfidenceIntervals cI2(alpha, "clopper_pearson");
    BOOST_CHECK_CLOSE( cI2.binomialLowerBound(200, 35), 0.14065, 0.1);
    BOOST_CHECK_CLOSE( cI2.binomialUpperBound(200, 35), 0.2144, 0.1);
}


//when you need a function, just find that function and include a header file
//also need to get eigen in your cmake file
//getting th, v values straight from IMU
#include <iostream>
#include <Eigen/Dense>
#include <math.h>

//Global Variables
using Eigen::Vector3f;
Vector3f g(0,0,-9.81); //gravity

//Cross Product Equivalent
using Eigen::Matrix3f;
using Eigen::Vector3f;
Matrix3f crossProductEquivalent(Vector3f v)
{
  Matrix3f c;
  c << 0, -v(2), v(1),
       v(2), 0, -v(0),
       -v(1), v(0), 0;
  //std::cout << c << std::endl;
  return c;
}

//Quaternion Multiplication
using Eigen::Quaternionf;
using Eigen::Vector3f;
Quaternionf qMultiply(Quaternionf q1, Quaternionf q2)
{
  float w1 = q1.w();
  float w2 = q2.w();
  Vector3f v1 = q1.vec();
  Vector3f v2 = q2.vec();
  float wReturn = w1*w2 - v1(0)*v2(0) - v1(1)*v2(1) - v1(2)*v2(2);
  Vector3f vReturn;
  vReturn(0) = w1*v2(0) + v1(0)*w2 + v1(1)*v2(2) - v1(2)*v2(1);
  vReturn(1) = w1*v2(1) - v1(0)*v2(2) + v1(1)*w2 + v1(2)*v2(0);
  vReturn(2) = w1*v2(2) + v1(0)*v2(1) - v1(1)*v2(0) + v1(2)*w2;
  Quaternionf qReturn;
  qReturn.w() = wReturn;
  qReturn.vec() = vReturn;
  //std::cout << qReturn.w() << std::endl << qReturn.vec() << std::endl;
  return qReturn;
}

//Quaternion Exponential
//implements simple 0th-order integration
//ref: quaternion kinematics, section 4.6.1
using Eigen::Quaternionf;
using Eigen::Vector3f;
Quaternionf qExponential(float dt, Vector3f w)
{
  float wn = w.norm();
  Vector3f wN = w.normalized();
  Quaternionf qReturn;
  qReturn.w() = cos(wn * dt / 2);
  qReturn.vec() = wN * sin(wn * dt / 2);
  return qReturn;
}

int main()
{

//Propagation

//Execute each time the IMU is sampled
Vector3f am; //accelerometer measurement (get from IMU)
Vector3f wm; //gyroscope measurement (get from IMU)
float dt; //(get from ROS)

//Build Omega(w)
wmx = crossProductEquivalent(wm);
Matrix 4f Omega;
Omega << -wmx(0,0), -wmx(0,1), -wmx(0,2), wm(0),
	 -wmx(1,0), -wmx(1,1), -wmx(1,2), wm(1),
         -wmx(2,0), -wmx(2,1), -wmx(2,2), wm(2),
         -wm(0), -wm(1), -wm(2), 0;

//Measurements at time l-1
Vector3f amOld; //accelerometer measurement from last time
Vector3f wmOld; //gyroscope measurement from last time

//Propagate state estimate

// Constants
using Eigen::Quaternionf;
using Eigen::Vector3f;
using Eigen::Matrix3f
Matrix3f I3 = Matrix3f::Identity(3,3);
Matrix3f O3 = Matrix3f::Zero();
float g = 9.81;

//Propagate quaternion
using Eigen::Quaternionf;
using Eigen::Vector3f;
using Eigen::Matrix3f
Quaternionf qHat;
Matrix3f RHat = qHat.toRotationMatrix();
Matrix3f RHatProp = (I - dt * wmx) * RHat;
Quaternionf qHatExp = qExponential(wmOld, dt);
Quaternionf qHatProp = qMultiply(qHatExp, qHatExp);

//Propagate p, the position
Vector3f pHatProp = pHat + vHat*dt + RHat*RHatProp*(amOld - baHat)*dt^2 + 0.5*g*(dt^2)

//Propagate v, the velocity
Vector3f vHatProp = vHat + RHat*RHatProp*(amOld - baHat)*dt + g*dt;

//Propagate bg and ba, the gyroscope and accelerometer biases
Vector3f bgHatProp = bgHat;
Vector3f baHatProp = baHat;

//Calculate IMU error state transition matrix
using Eigen::MatrixXf;
phipq = -crossProductEquivalent(pHatProp - pHat - vHat*dt - 0.5*g*dt^2);
phivq = -crossProductEquivalent(vHatProp - vHat - g*dt);
phigbg = RHat.transpose() * RHatProp * dt;
phipbg = crossProductEquivalent(vHat - g*dt) * RHat.transpose() * RHatProp * dt;
phipa = RHat.transpose() * RHatProp * dt^2;
phivbg = crossProductEquivalent(vHat - g*dt) * RHat.transpose() * RHatProp * dt;
phiva = RHat.transpose() * RHatProp * dt;
MatrixXf PhiProp(15, 15);
PhiProp << I3,    O3,      O3, phiqbg,    O3,
	         phipq, I3, (dt*I3), phipbg, phipa,
           phivq, O3,      I3, phivbg, phiva,
	         O3,    O3,      O3,     I3,    O3,
           O3,    O3,      O3,     O3,    I3;
}


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <vector>

typedef tiny::MathTypes<float> MT;
typedef MT::vector3_type       V;
typedef MT::quaternion_type    Q;
typedef MT::real_type          T;

class ContactInfo
{
public:

  V m_point;
  V m_normal;
  T m_distance;

};

class MyCallback
  : public geometry::ContactsCallback<V>
{
public:

  std::vector<ContactInfo> m_contacts;

public:

  void operator()(
                  V const & point
                  , V const & normal
                  , typename V::real_type const & distance
                  )
  {
    ContactInfo info;

    info.m_point = point;
    info.m_normal = normal;
    info.m_distance = distance;

    m_contacts.push_back(info);
  }

};

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(contacts_obb_sphere_test)
{
  // Touching rigth side
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    V const centerB = V::make(2.0, 0.0, 0.0);
    T const radiusB = 1.0;

    geometry::OBB<MT>   const obb    = geometry::make_obb<MT>(centerA, qA, half_extA);
    geometry::Sphere<V> const sphere = geometry::make_sphere(centerB, radiusB);

    MyCallback callback;

    geometry::contacts_obb_sphere(obb, sphere, 0.0, callback, false);

    BOOST_CHECK(callback.m_contacts.size() == 1u);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[0], 1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[1], 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[2], 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[0], 1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[1], 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[2], 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_distance, 0.0, 0.01);

  }
  // Separating rigth side
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    V const centerB = V::make(2.1, 0.0, 0.0);
    T const radiusB = 1.0;

    geometry::OBB<MT>   const obb    = geometry::make_obb<MT>(centerA, qA, half_extA);
    geometry::Sphere<V> const sphere = geometry::make_sphere(centerB, radiusB);

    MyCallback callback;

    geometry::contacts_obb_sphere(obb, sphere, 0.0, callback, false);

    BOOST_CHECK(callback.m_contacts.size() == 0u);
  }
  // Penetration rigth side
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    V const centerB = V::make(1.5, 0.0, 0.0);
    T const radiusB = 1.0;

    geometry::OBB<MT>   const obb    = geometry::make_obb<MT>(centerA, qA, half_extA);
    geometry::Sphere<V> const sphere = geometry::make_sphere(centerB, radiusB);

    MyCallback callback;

    geometry::contacts_obb_sphere(obb, sphere, 0.0, callback, false);

    BOOST_CHECK(callback.m_contacts.size() == 1u);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[0], 1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[1], 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[2], 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[0], 1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[1], 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[2], 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_distance, -0.5, 0.01);

  }
  // Penetration rigth side flipped-case
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    V const centerB = V::make(1.5, 0.0, 0.0);
    T const radiusB = 1.0;

    geometry::OBB<MT>   const obb    = geometry::make_obb<MT>(centerA, qA, half_extA);
    geometry::Sphere<V> const sphere = geometry::make_sphere(centerB, radiusB);

    MyCallback callback;

    geometry::contacts_obb_sphere(obb, sphere, 0.0, callback, true);

    BOOST_CHECK(callback.m_contacts.size() == 1u);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[0], 1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[1], 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[2], 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[0],-1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[1], 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[2], 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_distance, -0.5, 0.01);

  }

  // Touching left side
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    V const centerB = V::make(-2.0, 0.0, 0.0);
    T const radiusB = 1.0;

    geometry::OBB<MT>   const obb    = geometry::make_obb<MT>(centerA, qA, half_extA);
    geometry::Sphere<V> const sphere = geometry::make_sphere(centerB, radiusB);

    MyCallback callback;

    geometry::contacts_obb_sphere(obb, sphere, 0.0, callback, false);

    BOOST_CHECK(callback.m_contacts.size() == 1u);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[0],-1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[1], 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[2], 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[0],-1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[1], 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[2], 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_distance, 0.0, 0.01);

  }
  // Separating left side
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    V const centerB = V::make(-2.1, 0.0, 0.0);
    T const radiusB = 1.0;

    geometry::OBB<MT>   const obb    = geometry::make_obb<MT>(centerA, qA, half_extA);
    geometry::Sphere<V> const sphere = geometry::make_sphere(centerB, radiusB);

    MyCallback callback;

    geometry::contacts_obb_sphere(obb, sphere, 0.0, callback, false);

    BOOST_CHECK(callback.m_contacts.size() == 0u);
  }
  // Penetration left side
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    V const centerB = V::make(-1.5, 0.0, 0.0);
    T const radiusB = 1.0;

    geometry::OBB<MT>   const obb    = geometry::make_obb<MT>(centerA, qA, half_extA);
    geometry::Sphere<V> const sphere = geometry::make_sphere(centerB, radiusB);

    MyCallback callback;

    geometry::contacts_obb_sphere(obb, sphere, 0.0, callback, false);

    BOOST_CHECK(callback.m_contacts.size() == 1u);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[0], -1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[1], 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[2], 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[0], -1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[1], 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[2], 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_distance, -0.5, 0.01);

  }
  // Penetration left side flipped-case
  {
    V const centerA   = V::make(0.0, 0.0, 0.0);
    V const half_extA = V::make(1.0, 1.0, 1.0);
    Q const qA        = Q::identity();

    V const centerB = V::make(-1.5, 0.0, 0.0);
    T const radiusB = 1.0;

    geometry::OBB<MT>   const obb    = geometry::make_obb<MT>(centerA, qA, half_extA);
    geometry::Sphere<V> const sphere = geometry::make_sphere(centerB, radiusB);

    MyCallback callback;

    geometry::contacts_obb_sphere(obb, sphere, 0.0, callback, true);

    BOOST_CHECK(callback.m_contacts.size() == 1u);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[0], -1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[1], 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_point[2], 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[0], 1.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[1], 0.0, 0.01);
    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_normal[2], 0.0, 0.01);

    BOOST_CHECK_CLOSE(callback.m_contacts[0].m_distance, -0.5, 0.01);

  }

}

BOOST_AUTO_TEST_SUITE_END();


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>
#include <typeinfo>

template <typename At, typename Ut>
void dense_ic_0(const At& As, const Ut& Us)
{
    mtl::dense2D<double> U(upper(As));

    const std::size_t n= num_rows(U);

    for (std::size_t k= 0; k < n; k++) {
	double dia= U[k][k]= sqrt(U[k][k]);
	for (std::size_t i = k + 1; i < n; i++) {
	    double d= U[k][i] /= dia;
	    for (std::size_t j = k + 1; j <= i; j++)
		if (U[j][i] != 0.0)
		    U[j][i] -= d * U[k][j];
	}
    }

    std::cout << "Factorizing A = \n" << As << "-> U = \n" << with_format(U, 6, 2)
	      << "trans(U) * U = \n" << with_format(mtl::dense2D<double>(trans(U) * U), 6, 2);

    if (std::abs(U[2][3] - Us[2][3]) > 0.001) throw "Wrong value in L for sparse IC(0) factorization";

    if (std::abs(U[3][3] - 1. / Us[3][3]) > 0.001) throw "Wrong value in U for sparse IC(0) factorization";
}

template <typename Solver>
void test(const Solver&)
{
    typedef typename mtl::ashape::ashape<Solver>::type shape;
    std::cout << "type is " << typeid(Solver).name() << '\n';
    std::cout << "ashape is " << typeid(shape).name() << '\n';
    std::cout << "type is " << (mtl::traits::is_scalar<Solver>::value ? "" : "not ") << "scalar\n";
    std::cout << "type is " << (mtl::traits::backward_index_evaluatable<Solver>::value ? "" : "not ") << "back-eval\n";
}

int main()
{
    // For a more realistic example set sz to 1000 or larger
    const int size = 3, N = size * size;

    typedef mtl::compressed2D<double>  matrix_type;
    mtl::compressed2D<double>          A(N, N), dia(N, N);
    laplacian_setup(A, size, size);
    // dia= 1.0; A+= dia;

    itl::pc::ic_0<matrix_type, float>  P(A);
    mtl::dense_vector<double>          x(N, 1.0), b(N);

    if(size > 1 && size < 4)
	dense_ic_0(A, P.get_U());

    b = A * x;
    x= 0;

    itl::cyclic_iteration<double> iter(b, N, 1.e-6, 0.0, 1);
    cg(A, x, b, P, iter);

    // test(mtl::lazy(b)= solve(P, x));

    return 0;
}


#include <Eigen/Dense>
#include "Geometry.hh"
#include "PeriodicTable.hh"
#include "XYZMatrix.hh"
#include "ZMatrix.hh"
#include "io/manipulators.hh"
#include "exceptions.hh"

double Geometry::nuclearRepulsion() const
{
	double sum = 0;

	for (int j = 1; j < size(); ++j)
	{
		sum += (charges().head(j).transpose().array()
			/ (positions().block(0, 0, 3, j).colwise() - position(j))
				.colwise().norm().array())
			.sum();
	}

	return sum;
}

void Geometry::setAtom(int idx, const std::string& symbol,
	double x, double y, double z)
{
	checkIndex(idx);

	const Element& elem = PeriodicTable::singleton().findBySymbol(symbol);

	_positions.col(idx) << x, y, z;
	_masses(idx) = elem.mass();
	_charges(idx) = elem.number();
	_symbols[idx] = symbol;
}

std::ostream& Geometry::print(std::ostream& os) const
{
	os << "Geometry (\n" << indent;
	for (int i = 0; i < size(); i++)
		os << _charges(i) << "\t" << _masses(i) << "\t"
			<< symbol(i) << "\t"
			<< position(i).transpose() << "\n";
	os << dedent << ")";
	return os;
}

JobIStream& Geometry::scan(JobIStream& is)
{
	is >> getline;
	if (is.eof())
		throw UnexpectedEOF();

	std::string elem;
	double x, y, z;
	is >> element(elem, false) >> x >> y >> z;
	if (!is.fail())
	{
		is.ungetLastLine();

		XYZMatrix mat;
		is >> mat;
		mat.fillGeometry(this);
	}
	else
	{
		is.ungetLastLine();

		ZMatrix mat;
		is >> mat;
		mat.fillGeometry(this);
	}

	return is;
}

void Geometry::toPrincipalAxes()
{
	// Move center of mass to origin
	Eigen::Vector3d cm = (positions() * masses()) / masses().sum();
	_positions.colwise() -= cm;

	// Compute principal axes
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(inertia());
	const Eigen::Matrix3d& axes = solver.eigenvectors();
	Eigen::Vector3d c = axes.col(0), b = axes.col(1), a = axes.col(0);
	if (a.cross(b).dot(c) < 0)
		// axis system is left-handed, flip one
		a = -a;

	// First two angles put the z axis in place
	double theta = std::acos(c.z());
	double psi = theta > 2 * std::numeric_limits<double>::epsilon()
		? std::atan2(c.y(), c.x()) : 0;

	// Rotate the a axis into the xy-plane
	Eigen::Matrix3d R = (Eigen::AngleAxisd(-theta, Eigen::Vector3d::UnitY())
		* Eigen::AngleAxisd(-psi, Eigen::Vector3d::UnitZ()))
		.toRotationMatrix();
	a = R * a;

	// Last angle aligns a axis with x (and b with y)
	double phi = std::atan2(a.y(), a.x());
	R = Eigen::AngleAxisd(-phi, Eigen::Vector3d::UnitZ()) * R;
	_positions = R * positions();
}

Eigen::Matrix3d Geometry::inertia() const
{
	Eigen::Matrix3d I;
        for (int j = 0; j < 3; j++)
        {
                for (int i = 0; i <= j; i++)
                {
			I(j,i) = I(i,j) =
				-_positions.row(i).cwiseProduct(_positions.row(j))
					.dot(masses());
		}
        }

        double trace = I.trace();
        for (int i = 0; i < 3; i++)
		I(i,i) += trace;

	return I;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// Written by Cornelius Steinhardt

#include <cmath>
#include <string>

// #include <boost/test/minimal.hpp>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

template <typename Matrix>
void test1(Matrix& m, double tau)
{
  mtl::mat::inserter<Matrix> ins(m);
  size_t nrows=num_rows(m);
  double val;
  for (size_t r=0;r<nrows;++r)
  {
    for (size_t c=0;c<nrows;++c)
    {
      if(r==c)
        ins(r,c) << 1.;
      else
      {
        val=2.*(static_cast<double>(rand())/RAND_MAX - 0.5);
        if (val<tau)
          ins(r,c) << val;
      }
    }
  }
}

template <typename Matrix, typename Vector, typename Left, typename Right>
void test(char const* name, char const* comment, Matrix const& A, Vector& x, Vector const& b, Left const& L, Right const& R,
	  unsigned restart, bool check_convergence= true)
{
    const int Niter = 100;

    std::cout << name << comment << "\n";
    x= 2.0, 3., 4., 8;

    itl::cyclic_iteration<double> iter(b, Niter, 1.e-8, 0.0, 10);
    gmres(A, x, b, L, R, iter, restart);
    std::cout << "x= " << x << " \n" ;
    Vector r(b - A*x);
    if (false && check_convergence && two_norm(r) > 0.00001)
	throw std::string(name) + std::string(" doesn't converge!");
}

int main(int, char**)
{
    const int N = 2;
    typedef mtl::compressed2D<double> matrix_type;
    matrix_type                   A(N*N, N*N);
    laplacian_setup(A, N, N);

    mtl::dense_vector<double> b(N*N, 1), x(N*N,1), r(N*N);

    itl::pc::identity<matrix_type>         Ident(A);
    itl::pc::ic_0<matrix_type>             ic(A);
    itl::pc::ilu_0<matrix_type>            ilu(A);
    itl::pc::diagonal<matrix_type>         diag(A);

    std::cout << "A has " << A.nnz() << " non-zero entries" << std::endl;
    std::cout << "A =\n" << A << " \n";

    test("Non-preconditioned GMRES(1)", "\nWon't convergence (for large examples,without restarts)!",
	 A, x, b, Ident, Ident, 1, false);
    test("Non-preconditioned GMRES(4)", "", A, x, b, Ident, Ident, 4);
    test("Left ILU(0) GMRES(4)", "", A, x, b, ilu, Ident, 4);
    test("Left IC(0) GMRES(4)", "", A, x, b, ic, Ident, 4);
    test("Left diag GMRES(4)", "", A, x, b, diag, Ident, 4);

    test("Right ILU(0) GMRES(4)", "", A, x, b, Ident, ilu, 4);
    test("Right IC(0) GMRES(4)", "", A, x, b, Ident, ic, 4);
    test("Right diag GMRES(4)", "", A, x, b, Ident, diag, 4);

    test("Left ILU(0) Right ILU(0) GMRES(4)", "", A, x, b, ilu, ilu, 4);
    test("Left ILU(0) Right IC(0) GMRES(4)", "", A, x, b, ilu, ic, 4);
    test("Left ILU(0) Right diag GMRES(4)", "", A, x, b, ilu, diag, 4);

    return 0;
}


// inverting symmetric/hermitian positive definite
// factor (potrf()) and invert (potri())

// #define BOOST_UBLAS_STRICT_HERMITIAN
// .. doesn't work (yet?)

//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/lapack/computational/potri.hpp>
#include <boost/numeric/bindings/lapack/computational/potrf.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/symmetric.hpp>
#include <boost/numeric/bindings/ublas/hermitian.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace blas = boost::numeric::bindings::blas;
namespace bindings = boost::numeric::bindings;
namespace lapack = boost::numeric::bindings::lapack;

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

typedef double real_t;

typedef std::complex<real_t> cmplx_t;

#ifndef F_ROW_MAJOR
typedef ublas::matrix<real_t, ublas::column_major> m_t;
typedef ublas::matrix<cmplx_t, ublas::column_major> cm_t;
#else
typedef ublas::matrix<real_t, ublas::row_major> m_t;
typedef ublas::matrix<cmplx_t, ublas::row_major> cm_t;
#endif

#ifndef F_UPPER
typedef ublas::symmetric_adaptor<m_t, ublas::lower> symm_t;
typedef ublas::hermitian_adaptor<cm_t, ublas::lower> herm_t;
#else
typedef ublas::symmetric_adaptor<m_t, ublas::upper> symm_t;
typedef ublas::hermitian_adaptor<cm_t, ublas::upper> herm_t;
#endif

int main() {

  cout << endl;

  cout << "real symmetric\n" << endl;

  size_t n = 3;
  m_t a (n, n);    // matrix (storage)
  symm_t sa (a);   // symmetric adaptor

#ifdef F_UPPER
  init_symm (sa, 'u');
#else
  init_symm (sa, 'l');
#endif
  // ifdef F_UPPER
  //        [5 4 3 2 1]
  //        [0 5 4 3 2]
  //    a = [0 0 5 4 3]
  //        [0 0 0 5 4]
  //        [0 0 0 0 n]
  // else
  //        [5 0 0 0 0]
  //        [4 5 0 0 0]
  //    a = [3 4 5 0 0]
  //        [2 3 4 5 0]
  //        [1 2 3 4 5]
  print_m (sa, "A");
  cout << endl;

  m_t a2 (sa);   // full symmetric copy of sa:
                 // .. sa is `lost' after potrf();
                 // .. only one parameter of symm() is symmetric matrix

  int ierr = lapack::potrf (sa);
  if (!ierr) {
    lapack::potri (sa);
    // ri should be (almost) identity matrix:
    m_t ri (n, n);
    blas::symm ( bindings::tag::right(), 1.0, sa, a2, 0.0, ri);
    print_m (ri, "I = A * A^(-1)");
    cout << endl;
    blas::symm ( bindings::tag::left(), 1.0, sa, a2, 0.0, ri);
    print_m (ri, "I = A^(-1) * A");
    cout << endl;
  }

  cout << "\n===========================\n" << endl;
  cout << "complex hermitian (almost ;o)\n" << endl;

  // hermitian
  cm_t ca (3, 3);
  herm_t ha (ca);

#ifndef F_UPPER
  ha (0, 0) = cmplx_t (3, 0);
  ha (1, 0) = cmplx_t (2, 0);
  ha (1, 1) = cmplx_t (3, 0);
  ha (2, 0) = cmplx_t (1, 0);
  ha (2, 1) = cmplx_t (2, 0);
  ha (2, 2) = cmplx_t (3, 0);
#else
  ha (0, 0) = cmplx_t (3, 0);
  ha (0, 1) = cmplx_t (2, 0);
  ha (0, 2) = cmplx_t (1, 0);
  ha (1, 1) = cmplx_t (3, 0);
  ha (1, 2) = cmplx_t (2, 0);
  ha (2, 2) = cmplx_t (3, 0);
#endif

  print_m (ha, "A");
  cout << endl;

  cm_t ca2 (ha);  // full hermitian

  ierr = lapack::potri (ha);   // potrf()
  if (ierr == 0) {
    lapack::potri (ha);        // potri()
    cm_t ic (3, 3);
    blas::hemm ( bindings::tag::right(), 1.0, ha, ca2, 0.0, ic);
    print_m (ic, "I = A * A^(-1)");
    cout << endl;
  }
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl;

  cout << "\n===========================\n" << endl;
  cout << "complex hermitian\n" << endl;

#ifndef F_UPPER
  ha (0, 0) = cmplx_t (25, 0);
  ha (1, 0) = cmplx_t (-5, 5);
  ha (1, 1) = cmplx_t (51, 0);
  ha (2, 0) = cmplx_t (10, -5);
  ha (2, 1) = cmplx_t (4, 6);
  ha (2, 2) = cmplx_t (71, 0);
#else
  ha (0, 0) = cmplx_t (25, 0);
  ha (0, 1) = cmplx_t (-5, -5);
  ha (0, 2) = cmplx_t (10, 5);
  ha (1, 1) = cmplx_t (51, 0);
  ha (1, 2) = cmplx_t (4, -6);
  ha (2, 2) = cmplx_t (71, 0);
#endif
  print_m (ha, "A");
  cout << endl;

  ca2 = ha;

  ierr = lapack::potrf (ha);
  if (ierr == 0) {
    lapack::potri (ha);
    cm_t ic (3, 3);
    blas::hemm ( bindings::tag::right(), 1.0, ha, ca2, 0.0, ic);
    print_m (ic, "I = A * A^(-1)");
    cout << endl;
    blas::hemm ( bindings::tag::left(), 1.0, ha, ca2, 0.0, ic);
    print_m (ic, "I = A^(-1) * A");
    cout << endl;
  }
  else
    cout << "matrix is not positive definite: ierr = "
         << ierr << endl;

  cout << endl;

}


#pragma once
#include "math_util.hpp"
#include <boost/assert.hpp>
#include <boost/operators.hpp>
#include <cmath>
#include <limits>
#include <numeric>

namespace dmc
{
	template <class Scalar, int Dimension>
	class vector;

	template <class Derived, class Scalar, int Dimension>
	class vector_base
		: boost::addable<
			  Derived,
			  boost::subtractable<
				  Derived,
				  boost::multipliable<
					  Derived,
					  Scalar,
					  boost::dividable<
						  Derived,
						  Scalar,
						  boost::equality_comparable<
							  Derived>>>>>
	{
	public:
		typedef Scalar scalar_type;
		static const int dimension = Dimension;

		scalar_type* data()
		{
			return &values_[0];
		}

		const scalar_type* data() const
		{
			return &values_[0];
		}

		scalar_type& operator[](int index)
		{
			BOOST_ASSERT(0 <= index && index < dimension);
			return values_[index];
		}

		scalar_type operator[](int index) const
		{
			BOOST_ASSERT(0 <= index && index < dimension);
			return values_[index];
		}

		Derived operator-() const
		{
			return map([](auto x) { return -x; });
		}

		const Derived& operator+() const
		{
			return derived();
		}

		Derived& operator+=(const Derived& rhs)
		{
			for (int i = 0; i < dimension; ++i)
				(*this)[i] += rhs[i];
			return derived();
		}

		Derived& operator-=(const Derived& rhs)
		{
			for (int i = 0; i < dimension; ++i)
				(*this)[i] -= rhs[i];
			return derived();
		}

		Derived& operator*=(scalar_type rhs)
		{
			for (int i = 0; i < dimension; ++i)
				(*this)[i] *= rhs;
			return derived();
		}

		Derived& operator/=(scalar_type rhs)
		{
			for (int i = 0; i < dimension; ++i)
				(*this)[i] /= rhs;
			return derived();
		}

		template <class F>
		vector<typename std::result_of<F(scalar_type)>::type, dimension> map(F f) const
		{
			vector<typename std::result_of<F(scalar_type)>::type, dimension> result;
			for (int i = 0; i < dimension; ++i)
				result[i] = f((*this)[i]);
			return result;
		}

		template <class T>
		auto cast() const
		{
			return map([](auto x) { return static_cast<T>(x); });
		}

		auto sign() const
		{
			return map([](auto x) { return dmc::sign(x); });
		}

		template <class T, class F>
		auto reduce(T t, F f) const
		{
			for (int i = 0; i < dimension; ++i)
				t = f(t, (*this)[i]);
			return t;
		}

		template <class F>
		auto reduce(F f) const
		{
			auto t = (*this)[0];
			for (int i = 1; i < dimension; ++i)
				t = f(t, (*this)[i]);
			return t;
		}

		auto sum() const
		{
			return reduce([](auto x, auto y) {
				return x + y;
			});
		}

		auto product() const
		{
			return reduce([](auto x, auto y) {
				return x * y;
			});
		}

		auto abs() const
		{
			return map([](auto x) { using std::abs; return abs(x); });
		}

		auto norm_l1() const
		{
			return abs().sum();
		}

		auto squared() const
		{
			return map([](auto x) { return squared(x); });
		}

		auto norm_l2_sq() const
		{
			return squared().sum();
		}

		auto norm_l2() const
		{
			using std::sqrt;
			return sqrt(norm_l2_sq());
		}

		auto max() const
		{
			return reduce([](auto x, auto y) { using std::max; return max(x, y); });
		}

		auto min() const
		{
			return reduce([](auto x, auto y) { using std::min; return min(x, y); });
		}

		bool try_normalize()
		{
			auto n = norm_l2();
			if (n < std::numeric_limits<scalar_type>::epsilon())
				return false;

			*this /= n;
			return true;
		}

		Derived clamp(const Derived& minimum, const Derived& maximum)
		{
			Derived result;
			for (int i = 0; i < dimension; ++i)
				result[i] = std::max(minimum[i], std::min(maximum[i], (*this)[i]));
			return result;
		}

		static Derived all(scalar_type s)
		{
			Derived result;
			for (int i = 0; i < dimension; ++i)
				result[i] = s;
			return result;
		}

		friend bool operator==(const Derived& lhs, const Derived& rhs)
		{
			for (int i = 0; i < dimension; ++i)
				if (lhs[i] != rhs[i])
					return false;

			return true;
		}

	private:
		Derived& derived()
		{
			return static_cast<Derived&>(*this);
		}

		const Derived& derived() const
		{
			return static_cast<const Derived&>(*this);
		}

		scalar_type values_[dimension] = {};
	};
}


#pragma once

#include <algorithm>
#include <boost/range.hpp>
#include <math.h>
#include <math/Vec.hpp>
#include <utility>

template <int rows_, int cols_, class T = float>
struct Matrix {
	static const int rows = rows_;
	static const int cols = cols_;
	static const int size = rows * cols;

	typedef T value_type;

	typedef T & reference;
	typedef T const & const_reference;

	typedef T * pointer;
	typedef T const * const_pointer;

	typedef T row_type[cols];
	typedef row_type storage_type[rows];

	typedef pointer iterator;
	typedef const_pointer const_iterator;

	typedef std::pair<iterator, iterator> range_type;
	typedef std::pair<const_iterator, const_iterator> const_range_type;

	Matrix() {
		iterator end = this->end();
		for (iterator it = this->begin(); it != end; *it++ = 0) { ; }
	}

	template <class InIt>
	Matrix(InIt it, InIt end) {
		assign(it, end);
	}

	template <class U>
	Matrix(U (&array)[rows * cols]) {
		assign(boost::begin(array), boost::end(array));
	}

	template <class R>
	Matrix(R (&array)[rows]) {
		iterator out = begin();
		for (int i = 0; i < rows; ++i) {
			out = std::copy(boost::begin(array[i]), boost::end(array[i]), out);
		}
	}

	row_type & operator [] (int i) { return data_[i]; }
	row_type const & operator [] (int i) const { return data_[i]; }

	reference operator () (int i, int j) { return data_[i][j]; }
	const_reference operator () (int i, int j) const { return data_[i][j]; }

	range_type row_range(int i) { return range_type(data_[i], data_[i] + cols); }
	const_range_type row_range(int i) const { return const_range_type(data_[i], data_[i] + cols); }

	range_type column_range(int j) { return range_type(data_[0] + j, data_[rows] + j); }
	const_range_type column_range(int j) const { return const_range_type(data_[0] + j, data_[rows] + j); }

	iterator begin() { return data_[0]; }
	const_iterator begin() const { return data_[0]; }

	iterator end() { return data_[rows]; }
	const_iterator end() const { return data_[rows]; }

	template <class It>
	void assign(It it, It end) {
		iterator out_end = this->end();
		for (iterator out = begin(); out != out_end && it != end; *out++ = *it++) { ; }
	}

	template <class U>
	Vec<rows, T> operator * (Vec<cols, U> const & v) const {
		Vec<rows, T> out;
		for (int i = 0; i < rows; ++i) {
			out[i] = dot(row_range(i), 1, std::make_pair(v.begin(), v.end()), 1);
		}

		return out;
	}

	template <class U>
	friend Vec<cols, T> operator * (Vec<rows, U> const & v, Matrix const & M) {
		Vec<cols, T> out;
		for (int i = 0; i < cols; ++i) {
			out[i] = M.dot(std::make_pair(v.begin(), v.end()), 1, M.column_range(i), cols);
		}

		return out;
	}

	template <int c, class U>
	Matrix<rows, c, T> operator * (Matrix<cols, c, U> const & right) const {
		Matrix<rows, c> out;
		for (int i = 0; i < rows; ++i) {
			for (int j = 0; j < right.cols; ++j) {
				out.data_[i][j] = dot(row_range(i), 1, right.column_range(j), right.cols);
			}
		}
		return out;
	}

	template <class U>
	Matrix & operator *= (Matrix<cols, cols, U> const & right) {
		*this = (*this) * right;
		return *this;
	}

	template <class stream>
	friend stream & operator << (stream & out, Matrix const & m) {
		out << "{ ";
		for (int i = 0; i < m.rows; ++i) {
			if (i) { out << ", "; }

			out << "{ ";
			for (int j = 0; j < m.cols; ++j) {
				if (j) { out << ", "; }
				out << m.data_[i][j];
			}
			out << " }";
		}
		out << " }";
		return out;
	}

	void set_row(int i, Vec<cols, T> const & row) {
		range_type r = row_range(i);
		for (int j = 0; j < cols; ++j) { r.first[j] = row[j]; }
	}

	void set_column(int j, Vec<rows, T> const & col) {
		range_type r = column_range(j);
		for (int i = 0; i < rows; ++i) { r.first[i] = col[i]; }
	}

	private:
		template <class L, class R>
		static inline T dot(L left, int stride_left, R right, int stride_right) {
			T out = 0;

			while (left.first < left.second && right.first < right.second) {
				out += *left.first * *right.first;
				left.first += stride_left;
				right.first += stride_right;
			}

			return out;
		}

		storage_type data_;
};

namespace detail {
	namespace MatrixInverter {
		inline int abs(int x) { return ::abs(x); }
		inline float abs(float x) { return fabsf(x); }
		inline double abs(double x) { return fabs(x); }
		inline long double abs(long double x) { return fabsl(x); }

		template <class T, int N>
		inline void swap(T (&left)[N], T (&right)[N]) {
			float *l = left, *r = right;
			for (int i = 0; i < N; ++i) { std::swap(*l++, *r++); }
		}

		template <class T>
		inline void div(T * begin, T * end, T d) {
			for (; begin != end; ++begin) { *begin /= d; }
		}

		template <class T>
		inline T max_abs(T * begin, T * end) {
			T m = 0;
			for (T * it = begin; it < end; ++it) {
				T v = abs(*it);
				if (it != begin && v <= m) { continue; }
				m = v;
			}

			return m;
		}

		template <class T>
		inline int find_pivot_row(T * begin, T * end, int stride) {
			T m = 0; int p = 0, i = 0;
			for (T * it = begin; it < end; ++i, it += stride) {
				T v = abs(*it);
				if (i && v <= m) { continue; }
				m = v; p = i;
			}

			return p;
		}

		template <class T>
		inline void normalize_front(T * begin, T * end) {
			float d = *begin; *begin = 1; ++begin;
			while (begin < end) { *begin++ /= d; }
		}

		template <class T>
		inline void scale_and_subtract(T * begin, T * end, T * out) {
			T s = -*out / *begin++; *out++ = 0;
			while (begin < end) { *out++ += s * *begin++; }
		}

		template <class T, int N>
		void invert_matrix(Matrix<N, N, T> * M) {
			T S[N][2 * N];

			for (int i = 0; i < N; ++i) {
				for (int j = 0; j < N; ++j) {
					S[i][j] = (*M)[i][j];
					S[i][j + N] = (i == j);
				}
			}

			for (int i = 0; i < N; ++i) { div(S[i], S[i] + 2 * N, max_abs(S[i], S[i] + N)); }

			for (int i = 0; i < N; ++i) {
				int pivot = find_pivot_row(S[i] + i, S[N] + i, 2 * N);
				if (pivot) { swap(S[i], S[i + pivot]); }

				normalize_front(S[i] + i, S[i] + 2 * N);
				for (int j = i + 1; j < N; ++j) { scale_and_subtract(S[i] + i, S[i] + 2 * N, S[j] + i); }
			}

			for (int i = N - 1; i > 0; --i) {
				for (int j = i - 1; j >= 0; --j) { scale_and_subtract(S[i] + i, S[i] + 2 * N, S[j] + i); }
			}

			for (int i = 0; i < N; ++i) {
				for (int j = 0; j < N; ++j) { (*M)[i][j] = S[i][j + N]; }
			}
		}
	}
}

using detail::MatrixInverter::invert_matrix;


/*
 * Copyright (c) 2018, Sunanda Bose (Neel Basu) (neel.basu.z@gmail.com)
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 * DAMAGE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "mathematica++ Unit Test (Module::Integer)"
#include <boost/test/unit_test.hpp>
#include <string>
#include <cmath>
#include <complex>
#include <map>
#include <vector>
#include <iostream>
#include <boost/format.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_io.hpp>
#include <boost/tuple/tuple_comparison.hpp>
#include "boost/tuple/tuple.hpp"
#include "mathematica++/connector.h"
#include "mathematica++/m.h"
#include "mathematica++/io.h"
#include "mathematica++/declares.h"
#include "mathematica++/operators.h"
#include "mathematica++/rules.h"

using namespace mathematica;

MATHEMATICA_DECLARE(Table)
MATHEMATICA_DECLARE(Det)
MATHEMATICA_DECLARE(Dot)
MATHEMATICA_DECLARE(Mod)

BOOST_AUTO_TEST_SUITE(integer)

BOOST_AUTO_TEST_CASE(integer_size){
    connector shell;
    BOOST_CHECK(shell.connected());
    {
        value result;
        shell << Power(2, 68);
        shell >> result;

        std::string res_str = *result;
        res_str.erase(0, 1);

        // std::cout << *result << " " << res_str << std::endl;

        BOOST_CHECK(res_str == "295147905179352825856");
    }
    {
        value res_intermediate;
        value res_final;

        shell << List(Power(2, 61), Power(2, 62), Power(2, 63), Power(2, 64), Power(2, 65));
        shell >> res_intermediate;
        shell << Total(res_intermediate);
        shell >> res_final;

        std::string res_nstr = *res_final;
        res_nstr.erase(0, 1);

        // std::cout << res_intermediate << std::endl;
        // std::cout << res_final << std::endl;

        BOOST_CHECK(res_nstr == "71481133285624512512");
    }
    {
        mathematica::symbol i("i");
        mathematica::symbol j("j");

        value res_mata;
        value res_matb;
        value res_matc;
        value res_det;

        shell << Table(Mod(i+j, 2), List(i, 1, 2), List(j, 1, 2));
        shell >> res_mata;
        shell << Table(Mod(i+j, 3), List(i, 1, 2), List(j, 1, 2));
        shell >> res_matb;
        shell << Dot(res_mata, res_matb);
        shell >> res_matc;
        shell << Det(res_matc);
        shell >> res_det;

        BOOST_CHECK(*res_det == -2);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include "../metropolis_hastings.hpp"
#include "../utils.hpp"
#include "gtest/gtest.h"

#include <boost/log/trivial.hpp>
#include <boost/math/distributions/beta.hpp>
#include <chrono>
#include <fstream>
#include <iostream>
#include <random>
#include <string>

namespace FilterModel {
TEST(sample, wtv) {
    boost::math::beta_distribution<double> dist(2, 2);
    std::default_random_engine generator;
    generator.seed(std::chrono::system_clock::now().time_since_epoch().count());

    std::vector<double> values;

    double x = MetropolisHastingsSampler::sample<double, std::default_random_engine>(
        100000,
        [dist](double x) {
            double p = pdf(dist, x);
            double log_p = std::log(p);
            return log_p;
        },
        [](std::default_random_engine gen) {
            std::uniform_real_distribution<double> uniform(0, 1);
            double x = uniform(gen);
            return x;
        },
        [](double center, std::default_random_engine gen) {
            std::normal_distribution<double> conditional(center, 0.25);
            double x = sample_probability(conditional, gen);
            return x;
        },
        generator, &values);

    std::string out_path =
        "/home/skinnersboxy/perkins-input-filter/MulticlassFilterModel/output/values.csv";
    std::ofstream output_file(out_path);

    if (output_file.is_open()) {
        output_file << vector_to_string(values);
        output_file.close();
    } else {
        BOOST_LOG_TRIVIAL(fatal) << "File " << out_path << " cannot be written to.";
        assert(false);
    }
}

}  // namespace FilterModel


#include "../src/params.h"
#include "../src/sigmaplus_prover.h"
#include "../src/sigmaplus_verifier.h"

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(sigma_protocol_tests)

BOOST_AUTO_TEST_CASE(one_out_of_n)
{
    auto params = sigma::Params::get_default();
    int N = 16384;
    int n = params->get_n();
    int m = params->get_m();
    int index = 102;

    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_gens;
    h_gens.resize(n * m);
    for(int i = 0; i < n * m; ++i ){
        h_gens[i].randomize();
    }
    secp_primitives::Scalar r;
    r.randomize();
    sigma::SigmaPlusProver<secp_primitives::Scalar,secp_primitives::GroupElement> prover(g,h_gens, n, m);

    std::vector<secp_primitives::GroupElement> commits;
    for(int i = 0; i < N; ++i){
        if(i == index){
            secp_primitives::GroupElement c;
            secp_primitives::Scalar zero(unsigned(0));
            c = sigma::SigmaPrimitives<secp_primitives::Scalar,secp_primitives::GroupElement>::commit(g, zero, h_gens[0], r);
            commits.push_back(c);

        }
        else{
            commits.push_back(secp_primitives::GroupElement());
            commits[i].randomize();
        }
    }
    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proof(n,m);

    prover.proof(commits, index, r, true, proof);

    sigma::SigmaPlusVerifier<secp_primitives::Scalar,secp_primitives::GroupElement> verifier(g, h_gens, n, m);

    BOOST_CHECK(verifier.verify(commits, proof, true));
}

BOOST_AUTO_TEST_CASE(one_out_of_n_one)
{
    auto params = sigma::Params::get_default();
    int N = 16384;
    int n = params->get_n();
    int m = params->get_m();
    int index = 1023;

    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_gens;
    h_gens.resize(n * m);
    for(int i = 0; i < n * m; ++i ){
        h_gens[i].randomize();
    }
    secp_primitives::Scalar r;
    r.randomize();
    sigma::SigmaPlusProver<secp_primitives::Scalar,secp_primitives::GroupElement> prover(g,h_gens, n, m);

    std::vector<secp_primitives::GroupElement> commits;
    for(int i = 0; i < N; ++i){
        if(i == index){
            secp_primitives::GroupElement c;
            secp_primitives::Scalar one(unsigned(1));
            c = sigma::SigmaPrimitives<secp_primitives::Scalar,secp_primitives::GroupElement>::commit(g, one, h_gens[0], r);
            commits.push_back(c);

        }
        else{
            commits.push_back(secp_primitives::GroupElement());
            commits[i].randomize();
        }
    }
    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proof(n,m);

    prover.proof(commits, index, r, true, proof, unsigned(1));

    sigma::SigmaPlusVerifier<secp_primitives::Scalar,secp_primitives::GroupElement> verifier(g, h_gens, n, m);

    BOOST_CHECK(verifier.verify(commits, proof, true, unsigned(1)));
}

BOOST_AUTO_TEST_CASE(one_out_of_n_padding)
{
    auto params = sigma::Params::get_default();
    int N = 10000;
    int n = params->get_n();
    int m = params->get_m();
    int index = 9999;

    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_gens;
    h_gens.resize(n * m);
    for(int i = 0; i < n * m; ++i ){
        h_gens[i].randomize();
    }
    secp_primitives::Scalar r;
    r.randomize();
    sigma::SigmaPlusProver<secp_primitives::Scalar,secp_primitives::GroupElement> prover(g,h_gens, n, m);

    std::vector<secp_primitives::GroupElement> commits;
    for(int i = 0; i < N; ++i){
        if(i == index){
            secp_primitives::GroupElement c;
            secp_primitives::Scalar zero(unsigned(0));
            c = sigma::SigmaPrimitives<secp_primitives::Scalar,secp_primitives::GroupElement>::commit(g, zero, h_gens[0], r);
            commits.push_back(c);

        }
        else{
            commits.push_back(secp_primitives::GroupElement());
            commits[i].randomize();
        }
    }
    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proof(n, m);

    prover.proof(commits, index, r, true, proof);

    sigma::SigmaPlusVerifier<secp_primitives::Scalar,secp_primitives::GroupElement> verifier(g, h_gens, n, m);

    BOOST_CHECK(verifier.verify(commits, proof, true));

    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proofNew(n, m);
    prover.proof(commits, 11111, r, true, proofNew);
    BOOST_CHECK(verifier.verify(commits, proofNew, true));
}

BOOST_AUTO_TEST_CASE(prove_and_verify_in_different_set)
{
    auto params = sigma::Params::get_default();
    int N = 16384;
    int n = params->get_n();
    int m = params->get_m();
    int index = 0;

    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_gens;
    h_gens.resize(n * m);
    for(int i = 0; i < n * m; ++i ){
        h_gens[i].randomize();
    }
    secp_primitives::Scalar r;
    r.randomize();
    sigma::SigmaPlusProver<secp_primitives::Scalar,secp_primitives::GroupElement> prover(g,h_gens, n, m);

    std::vector<secp_primitives::GroupElement> commits;
    for(int i = 0; i < N; ++i){
        if(i == index){
            secp_primitives::GroupElement c;
            secp_primitives::Scalar zero(unsigned(0));
            c = sigma::SigmaPrimitives<secp_primitives::Scalar,secp_primitives::GroupElement>::commit(g, zero, h_gens[0], r);
            commits.push_back(c);

        }
        else{
            commits.push_back(secp_primitives::GroupElement());
            commits[i].randomize();
        }
    }

    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proof(n,m);

    prover.proof(commits, index, r, true, proof);

    sigma::SigmaPlusVerifier<secp_primitives::Scalar,secp_primitives::GroupElement> verifier(g, h_gens, n, m);

    // Add more commit
    secp_primitives::GroupElement c;
    secp_primitives::Scalar zero(unsigned(0));
    c = sigma::SigmaPrimitives<secp_primitives::Scalar,secp_primitives::GroupElement>::commit(g, zero, h_gens[0], r);
    commits.push_back(c);

    BOOST_CHECK(!verifier.verify(commits, proof, true));
}

BOOST_AUTO_TEST_CASE(prove_coin_out_of_index)
{
    auto params = sigma::Params::get_default();
    int N = 16384;
    int n = params->get_n();
    int m = params->get_m();

    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_gens;
    h_gens.resize(n * m);
    for(int i = 0; i < n * m; ++i ){
        h_gens[i].randomize();
    }
    secp_primitives::Scalar r;
    r.randomize();
    sigma::SigmaPlusProver<secp_primitives::Scalar,secp_primitives::GroupElement> prover(g,h_gens, n, m);

    std::vector<secp_primitives::GroupElement> commits;
    for(int i = 0; i < N; ++i){
        commits.push_back(secp_primitives::GroupElement());
        commits[i].randomize();
    }

    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proof(n,m);

    prover.proof(commits, commits.size(), r, true, proof);

    sigma::SigmaPlusVerifier<secp_primitives::Scalar,secp_primitives::GroupElement> verifier(g, h_gens, n, m);
    BOOST_CHECK(!verifier.verify(commits,proof, true));
}

BOOST_AUTO_TEST_CASE(prove_coin_not_in_set)
{
    auto params = sigma::Params::get_default();
    int N = 16384;
    int n = params->get_n();
    int m = params->get_m();
    int index = 0;
    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_gens;
    h_gens.resize(n * m);
    for(int i = 0; i < n * m; ++i ){
        h_gens[i].randomize();
    }
    secp_primitives::Scalar r;
    r.randomize();
    sigma::SigmaPlusProver<secp_primitives::Scalar,secp_primitives::GroupElement> prover(g,h_gens, n, m);

    std::vector<secp_primitives::GroupElement> commits;
    for(int i = 0; i < N; ++i){
        commits.push_back(secp_primitives::GroupElement());
        commits[i].randomize();
    }

    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proof(n,m);

    prover.proof(commits, index, r, true, proof);

    sigma::SigmaPlusVerifier<secp_primitives::Scalar,secp_primitives::GroupElement> verifier(g, h_gens, n, m);
    BOOST_CHECK(!verifier.verify(commits,proof, true));
}

BOOST_AUTO_TEST_SUITE_END()


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

#include "Eigen/Eigen"  // AFTER GRIDMAP!
#include <Eigen/Core>
#include <unsupported/Eigen/Splines>

class SplineFunction {

  public:

      // The spline is used to interpolate antenna gain values, as we only have the graphs
      SplineFunction()
      {}

      SplineFunction(Eigen::VectorXd const &x_vec, Eigen::VectorXd const &y_vec)
        : x_min(x_vec.minCoeff()),
          x_max(x_vec.maxCoeff()),
          y_min(y_vec.minCoeff()),
          y_max(y_vec.maxCoeff()),
          // Spline fitting here. X values are scaled down to [0, 1] for this.
          spline_(Eigen::SplineFitting<Eigen::Spline<double, 1>>::Interpolate(y_vec.transpose(), std::min<int>(x_vec.rows() - 1, 6), scaled_values(x_vec)))  // No more than cubic spline, but accept short vectors.
      {}

      // x values need to be scaled down in extraction as well.
      double interpDeg(double x) const {
        double y;
        y = spline_(scaled_value(x))(0);

        // interpolation may produce values bigger and lower than our limits ...
        y = std::max(std::min(y, y_max), y_min );
        return y;
      }

      double interpRad(double x) const {
          return interpDeg(x*180.0/M_PI);
      }

      // Helpers to scale X values down to [0, 1]
      double scaled_value(double x) const {
        return (x - x_min) / (x_max - x_min);
      }

    private:
      Eigen::RowVectorXd scaled_values(Eigen::VectorXd const &x_vec) const {
        return x_vec.unaryExpr([this](double x) { return scaled_value(x); }).transpose();
      }

      double x_min;
      double x_max;
      double y_min;
      double y_max;

      // Spline of one-dimensional "points."
      Eigen::Spline<double, 1> spline_;
};

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

// quick build:
// g++ -I /usr/include/eigen3/ play_with_eigen.cpp -o play_with_eigen -std=c++11

using namespace std::placeholders;
using Eigen::MatrixXd;

const double C = 299792458.0;
const double SENSITIVITY = -115; // dB

const double TAG_LOSSES = -4.8;

const double LOSS_CONSTANT = 147.55;
const double freq= 865e6;
const double lambda =  C/freq;
const double ANTENNA_LOSSES_LIST [25] = {  -22.6, -25.2, -25, -20.2, -17.6, -15.6, -14, -11.2, -7.8, -5.2, -2.4, -0.6, 0, -0.6, -2.4, -5.2, -8.8, -12.2, -16.4, -19.2, -20.8, -24.4, -28.2, -24, -22.6};
const double ANTENNA_ANGLES_LIST [25] = {-180.0, -165.0, -150.0, -135.0, -120.0, -105.0, -90.0, -75.0, -60.0, -45.0, -30.0, -15.0, 0.0, 15.0, 30.0, 45.0, 60.0, 75.0, 90.0, 105.0, 120.0, 135.0, 150.0, 165.0, 180.0};

//! Generates a mesh, just like Matlab's meshgrid
//  Template specialization for column vectors (Eigen::VectorXd)
//  in : x, y column vectors
//       X, Y matrices, used to save the mesh
template <typename Scalar>
void meshgrid(const Eigen::Matrix<Scalar, -1, 1>& x,
              const Eigen::Matrix<Scalar, -1, 1>& y,
              Eigen::Matrix<Scalar, -1, -1>& X,
              Eigen::Matrix<Scalar, -1, -1>& Y) {
  const long nx = x.size(), ny = y.size();
  X.resize(ny, nx);
  Y.resize(ny, nx);
  for (long i = 0; i < ny; ++i) {
    X.row(i) = x.transpose();
  }

  // for (long j = 0; j < nx; ++j) {
  //   Y.col(j) = y;
  // }
  for (long j = 0; j < nx; ++j) {
    Y.col(j) = y.reverse();
  }
}

//! Generates a mesh, just like Matlab's meshgrid
//  Template specialization for row vectors (Eigen::RowVectorXd)
//  in : x, y row vectors
//       X, Y matrices, used to save the mesh
template <typename Scalar>
void meshgrid(const Eigen::Matrix<Scalar, 1, -1>& x,
              const Eigen::Matrix<Scalar, 1, -1>& y,
              Eigen::Matrix<Scalar, -1, -1>& X,
              Eigen::Matrix<Scalar, -1, -1>& Y) {
  Eigen::Matrix<Scalar, -1, 1> xt = x.transpose(),
                               yt = y.transpose();
  meshgrid(xt, yt, X, Y);
}

typedef Eigen::VectorXd Vec;
typedef Eigen::MatrixXd Mat;

int main(int argc, char **argv)
{
    int Nx, Ny, x_min, x_max, y_min, y_max;
    double x_m, y_m;
    Mat X, Y, R, A, propL, antL,totalLoss, rxPower;
    Vec x,y;

    double txPower = 0; //dB

    /////////////////////////        // build spline to interpolate antenna gains;
    std::vector<double> xVec(ANTENNA_ANGLES_LIST, ANTENNA_ANGLES_LIST + 25);
    Eigen::VectorXd xvals = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(xVec.data(), xVec.size());
    std::vector<double> yVec(ANTENNA_LOSSES_LIST, ANTENNA_LOSSES_LIST + 25);
    Eigen::VectorXd yvals= Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(yVec.data(), yVec.size());
    SplineFunction _antenna_gains= SplineFunction(xvals, yvals);
    ////////////////////////

    Nx = 5;
    Ny = 8;
    x_min = -1;
    x_max = 1;
    y_min = -2;
    y_max = 2;
    x_m = 0.5;
    y_m = 1.2;

    x = Vec::LinSpaced(Nx, x_min, x_max);
    y = Vec::LinSpaced(Ny, y_min, y_max);

    std::cout << "x" << std::endl;
    std::cout << x << std::endl;
    std::cout << "y" << std::endl;
    std::cout << y << std::endl;

    // create X,Y meshgrids
    meshgrid(x, y, X, Y);

    // distance to point m
    X = X.array() - x_m;
    Y = Y.array() - y_m;

    std::cout << "X" << std::endl;
    std::cout << X << std::endl;
    std::cout << "Y" << std::endl;
    std::cout << Y << std::endl;

    // create R,Ang matrixes
    R = (X.array().square() + Y.array().square()).array().sqrt();
    A = Y.binaryExpr(X, std::ptr_fun(atan2));

    std::cout << "R" << std::endl;
    std::cout << R << std::endl;

    std::cout << "A" << std::endl;
    std::cout << (A*180.0/3.141592) << std::endl;

    // Create a propagation matrix without taking obstacles
    auto funtor = std::bind(&SplineFunction::interpRad, _antenna_gains, _1) ;
    antL =  TAG_LOSSES + A.unaryExpr( funtor ).array();
    std::cout << "antL" << std::endl;
    std::cout << antL << std::endl;

    propL = LOSS_CONSTANT - (20.0 * (R * freq).unaryExpr(std::ptr_fun(log10))).array() ;
    std::cout << "propL" << std::endl;
    std::cout << propL << std::endl;

    // signal goes from antenna to tag and comes back again, so we double the losses
    totalLoss =  2*antL + 2*propL;
    std::cout << "totalLoss" << std::endl;
    std::cout << totalLoss << std::endl;

    rxPower = txPower + totalLoss.array();

    // this should remove points where friis is not applicable
    rxPower = (R.array()>2*lambda).select(rxPower,SENSITIVITY);
    // this should remove points where received power is too low
    rxPower = (rxPower.array()>SENSITIVITY).select(rxPower,SENSITIVITY);

    std::cout << "rxPower" << std::endl;
    std::cout << rxPower << std::endl;

}


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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
    int n = 10000;
  VectorXd x(n), b(n);
  SparseMatrix<double> A(n,n);
  /* ... fill A and b ... */
  BiCGSTAB<SparseMatrix<double> > solver;
  solver.compute(A);
  x = solver.solve(b);
  std::cout << "#iterations:     " << solver.iterations() << std::endl;
  std::cout << "estimated error: " << solver.error()      << std::endl;
  /* ... update b ... */
  x = solver.solve(b); // solve again
  return 0;
}


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

#include <cmath>

#include <Eigen/Core>

#include "Tudat/Mathematics/BasicMathematics/sphericalHarmonics.h"
#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h"

namespace tudat
{
namespace basic_mathematics
{

//! Update maximum degree and order of cache
void SphericalHarmonicsCache::resetMaximumDegreeAndOrder( const int maximumDegree, const int maximumOrder )
{
    maximumDegree_ = maximumDegree;
    maximumOrder_ = maximumOrder;

    legendreCache_->resetMaximumDegreeAndOrder( maximumDegree_, maximumOrder_ );

    sinesOfLongitude_.resize( maximumOrder_ + 1 );
    cosinesOfLongitude_.resize( maximumOrder_ + 1 );
    referenceRadiusRatioPowers_.resize( maximumDegree_ + 2 );
}

//! Compute the gradient of a single term of a spherical harmonics potential field.
Eigen::Vector3d computePotentialGradient(
        const double distance,
        const double radiusPowerTerm,
        const double cosineOfOrderLongitude,
        const double sineOfOrderLongitude,
        const double cosineOfLatitude,
        const double preMultiplier,
        const int degree,
        const int order,
        const double cosineHarmonicCoefficient,
        const double sineHarmonicCoefficient,
        const double legendrePolynomial,
        const double legendrePolynomialDerivative )
{
    // Return result.
    return ( Eigen::Vector3d( ) <<
             - preMultiplier / distance
             * radiusPowerTerm
             * ( static_cast< double >( degree ) + 1.0 ) * legendrePolynomial
             * ( cosineHarmonicCoefficient * cosineOfOrderLongitude
                 + sineHarmonicCoefficient * sineOfOrderLongitude ),
             preMultiplier * radiusPowerTerm
             * legendrePolynomialDerivative * cosineOfLatitude * (
                 cosineHarmonicCoefficient * cosineOfOrderLongitude
                 + sineHarmonicCoefficient * sineOfOrderLongitude ),
             preMultiplier * radiusPowerTerm
             * static_cast< double >( order ) * legendrePolynomial
             * ( sineHarmonicCoefficient * cosineOfOrderLongitude
                 - cosineHarmonicCoefficient * sineOfOrderLongitude ) ).finished( );
}

//! Compute the gradient of a single term of a spherical harmonics potential field.
Eigen::Vector3d computePotentialGradient(
        const Eigen::Vector3d& sphericalPosition,
        const double referenceRadius,
        const double preMultiplier,
        const int degree,
        const int order,
        const double cosineHarmonicCoefficient,
        const double sineHarmonicCoefficient,
        const double legendrePolynomial,
        const double legendrePolynomialDerivative )
{
    return computePotentialGradient(
                sphericalPosition( radiusIndex ),
                basic_mathematics::raiseToIntegerPower
                ( referenceRadius / sphericalPosition( radiusIndex ), static_cast< double >( degree ) + 1.0 ),
                std::cos( static_cast< double >( order ) * sphericalPosition( longitudeIndex ) ),
                std::sin( static_cast< double >( order ) * sphericalPosition( longitudeIndex ) ),
                std::cos( sphericalPosition( latitudeIndex ) ), preMultiplier, degree, order,
                cosineHarmonicCoefficient, sineHarmonicCoefficient, legendrePolynomial,legendrePolynomialDerivative );
}

//! Compute the gradient of a single term of a spherical harmonics potential field.
Eigen::Vector3d computePotentialGradient( const Eigen::Vector3d& sphericalPosition,
                                          const double preMultiplier,
                                          const int degree,
                                          const int order,
                                          const double cosineHarmonicCoefficient,
                                          const double sineHarmonicCoefficient,
                                          const double legendrePolynomial,
                                          const double legendrePolynomialDerivative,
                                          const boost::shared_ptr< SphericalHarmonicsCache > sphericalHarmonicsCache )
{
    return computePotentialGradient(
                sphericalPosition( radiusIndex ),
                sphericalHarmonicsCache->getReferenceRadiusRatioPowers( degree + 1 ),
                sphericalHarmonicsCache->getCosineOfMultipleLongitude( order ),
                sphericalHarmonicsCache->getSineOfMultipleLongitude( order ),
                sphericalHarmonicsCache->getLegendreCache( )->getCurrentPolynomialParameterComplement( ),
                preMultiplier, degree, order,
                cosineHarmonicCoefficient, sineHarmonicCoefficient, legendrePolynomial,legendrePolynomialDerivative );
}

} // namespace basic_mathematics
} // namespace tudat