// 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 <typeinfo>
#include <iostream>
#include <cassert>
#include <boost/numeric/mtl/mtl.hpp>

struct poisson2D_dirichlet
{
    poisson2D_dirichlet(int m, int n) : m(m), n(n) {}

    template <typename Vector>
    Vector operator*(const Vector& v) const
    {
	assert(int(size(v)) == m * n);
	Vector w(m * n);

	for (int i= 0; i < m; i++)
	    for (int j= 0; j < n; j++) {
		int k= i * n + j; // offset
		w[k]= 4 * v[k];
		if (i > 0) w[k]-= v[k-n];   // upper neighbor
		if (i < m-1) w[k]-= v[k+n]; // lower neighbor
		if (j > 0) w[k]-= v[k-1];   // left neighbor
		if (j < n-1) w[k]-= v[k+1]; // right neighbor
	    }
	return w;
    }
    int m, n;
};

namespace mtl { namespace ashape {
    template <> struct ashape_aux<poisson2D_dirichlet>
    {	typedef nonscal type;    };
}}

int main(int, char**)
{
    using namespace std;

    mtl::compressed2D<double> A0;
    laplacian_setup(A0, 4, 5);
    cout << "A0 is\n" << A0 << endl;

    mtl::dense_vector<double> v(20);
    iota(v);
    cout << "v is " << v << endl;

    mtl::dense_vector<double> w1(A0 * v);
    cout << "A0 * v is " << w1 << endl;

    poisson2D_dirichlet A(4, 5);
    mtl::dense_vector<double> w2(A * v);
    cout << "A * v is " << w2 << endl;

    w1-= w2;
    if (one_norm(w1) > 0.001) throw "Wrong result";

    return 0;
}


//////////////////////////////////
//
// Compile: g++ -std=c++14 -O3 ex5.cxx -o ex5 -lboost_timer
//
//////////////////////////////////

#include <iostream>		// std::cout
#include <fstream>		// std::ifsteam
#include <utility>		// std::pair
#include <vector>		// std::vector
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/timer/timer.hpp>

int main(int argc, char*argv[]){

  if (argc != 2)
  {
    std::cerr << "No file!!!" << std::endl;
    return -1;
  }

  std::ifstream file(argv[1]); // std::ifstream::in ??
  std::string str;

  boost::timer::cpu_timer timer;

  /* start get number of edges */
  getline(file, str);

  using namespace boost::spirit;
  using qi::int_;
  using qi::double_;
  using qi::parse;

  int n;

  auto it = str.begin();
  bool r = parse(it, str.end(),
		 int_[([&n](int i){n = i;})] >> int_);
  /* end get number of edges */

  /* start get list of edges and weights */
  typedef std::pair<int, int> Edge;
  std::vector<Edge> Edges; // vector to store std::pair of edges.
  std::vector<int> Weights; // vector to weights as integers.

  while (getline(file,str)){ // get graph line-by-line.
    int Vert1;
    int Vert2;
    double Weight; // are all weights integer-valued ??

    auto it = str.begin(); // initialize iterator for qi::parse.

    bool r = parse(it, str.end(), // parse graph.
		 int_[([&Vert1](int i){Vert1 = i;})] >> qi::space >> int_[([&Vert2](int i){Vert2 = i;})] >> qi::space >> double_[([&Weight](double i){Weight = i;})]);

    Edge edge = std::make_pair(Vert1, Vert2); // make edge-pair out of vertices.
    Edges.push_back(edge);
    Weights.push_back(Weight);
  }
  /* end get list of edges and weights */

  typedef boost::property<boost::edge_weight_t, double> EdgeWeightProperty; // initialize type to store weights on edges.
  // adjacency_list<out-edges, vertex_set, directedness, vertex properties, edge properties>
  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, EdgeWeightProperty> Graph; // create graph.

  Graph g(Edges.begin(),Edges.end(), Weights.begin(), n); // populate graph.

  // here, the graph should be built... Find shortest path.
  typedef boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  vertex_descriptor source = boost::vertex(1, g); // define source vertex as vertex with index == 1.
  std::vector<vertex_descriptor> parents(boost::num_vertices(g)); // initialize vectors for predecessor and distances.
  std::vector<int> distances(boost::num_vertices(g));

  // find shortest distances.
  boost::dijkstra_shortest_paths(g, source, boost::predecessor_map(&parents[0]).distance_map(&distances[0]));

  /* start find longest-shortest path */
  int maxDistance = 0;
  int maxVertex = 0;

  // create iterator over vertices.
  // vertexPair.first is the iterated element, and .second is the end-index of all vertices.
  typedef boost::graph_traits <Graph>::vertex_iterator vertex_iter;
  std::pair<vertex_iter, vertex_iter> vertexPair;

  for (vertexPair = boost::vertices(g); vertexPair.first != vertexPair.second; ++vertexPair.first) // vertexPair = boost::vertices loops over all vertices in g.
  {
    // replace maxDistance if a greater distance is found, and maxDistance must be less than "infinity" (of 32-bit signed integer).
    if ((distances[*vertexPair.first] > maxDistance) && (distances[*vertexPair.first] < 2147483647)){
      maxDistance = distances[*vertexPair.first];
      maxVertex = *vertexPair.first;
    }
    // if distance == maxDistance, check if vertex index is smaller.
    if ((distances[*vertexPair.first] == maxDistance) && (*vertexPair.first < maxVertex)){
      maxVertex = *vertexPair.first;
    }
  }

  boost::timer::cpu_times times = timer.elapsed();

  // output vertex and distance of the longest-shortest path.
  std::cout << "RESULT VERTEX " << maxVertex << std::endl;
  std::cout << "RESULT DIST " << maxDistance <<  std::endl;
  /* end find longest-shortest path */

  // print CPU- and Wall-Time.
  // boost::timer::cpu_times returns tuple of wall, system and user times in nanoseconds.
  std::cout << std::endl;
  std::cout << "WALL-CLOCK " << times.wall / 1e9 << "s" << std::endl;
  std::cout << "USER TIME " << times.user / 1e9 << "s" << std::endl;

  file.close();
  return 0;
}


/**
 * \file SinusGeneratorFilter.cpp
 */

#include "SinusGeneratorFilter.h"

#include <cmath>

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

namespace ATK
{
  template<typename DataType_>
  SinusGeneratorFilter<DataType_>::SinusGeneratorFilter()
  :Parent(0, 2), volume(1), offset(0), frequency(0), frequ_cos(1), frequ_sin(0), cos(1), sin(0)
  {
  }

  template<typename DataType_>
  SinusGeneratorFilter<DataType_>::~SinusGeneratorFilter()
  {
  }

  template<typename DataType_>
  void SinusGeneratorFilter<DataType_>::set_frequency(DataType_ frequency)
  {
    this->frequency = frequency;
    this->frequ_cos = std::cos(2 * boost::math::constants::pi<DataType_>() * frequency / output_sampling_rate);
    this->frequ_sin = std::sin(2 * boost::math::constants::pi<DataType_>() * frequency / output_sampling_rate);
    Parent::setup();
  }

  template<typename DataType_>
  DataType_ SinusGeneratorFilter<DataType_>::get_frequency() const
  {
    return frequency;
  }

  template<typename DataType_>
  void SinusGeneratorFilter<DataType_>::set_volume(DataType_ volume)
  {
    this->volume = volume;
  }

  template<typename DataType_>
  DataType_ SinusGeneratorFilter<DataType_>::get_volume() const
  {
    return volume;
  }

  template<typename DataType_>
  void SinusGeneratorFilter<DataType_>::set_offset(DataType_ offset)
  {
    this->offset = offset;
  }

  template<typename DataType_>
  DataType_ SinusGeneratorFilter<DataType_>::get_offset() const
  {
    return offset;
  }

  template<typename DataType_>
  void SinusGeneratorFilter<DataType_>::full_setup()
  {
    Parent::full_setup();
    cos = 1;
    sin = 0;
  }

  template<typename DataType_>
  void SinusGeneratorFilter<DataType_>::process_impl(int64_t size) const
  {
    DataType* ATK_RESTRICT output_cos = outputs[0];
    DataType* ATK_RESTRICT output_sin = outputs[1];

    for(int64_t i = 0; i < size; ++i)
    {
      auto new_cos = cos * frequ_cos - sin * frequ_sin;
      auto new_sin = cos * frequ_sin + sin * frequ_cos;
      auto norm = (new_cos * new_cos + new_sin * new_sin);

      cos = new_cos / norm;
      sin = new_sin / norm;

      *(output_cos++) = volume * cos + offset;
      *(output_sin++) = volume * sin + offset;
    }
  }

  template class SinusGeneratorFilter<float>;
  template class SinusGeneratorFilter<double>;
}


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

// Set the parameters |i|, |j|, and |k| to be the indices of the grid cell
// containing the point |p| for a grid with lower corner |lc| and grid cell
// width (spacing) |dx|.
inline void floor(const Eigen::Vector3d& p, const Eigen::Vector3d& lc,
                  double dx, int& i, int& j, int& k) {
  // Compute |p|'s location relative to |lc|.
  // Dividing by |dx| yields a 3D vector indicating the number of grid
  // cells (including fractions of grid cells, as the vector elements are
  // floating-point values) away from |lc| that |p| is located.
  Eigen::Vector3d p_lc_over_dx = (p - lc) / dx;

  // Set the indices to the floor of |p_lc_over_dx|'s elements.
  i = static_cast<int>(p_lc_over_dx[0]);
  j = static_cast<int>(p_lc_over_dx[1]);
  k = static_cast<int>(p_lc_over_dx[2]);
}

int main(int argc, char** argv) {
  // Make a grid with spacing of 2 with lower corner (1, 2, 3).
  double dx = 2.0;
  Eigen::Vector3d lc(1, 2, 3);

  // (i, j, k) should be (floor((6-1)/2), floor((4-2)/2), floor((3-3)/2)),
  // which is (floor(2.5), floor(1), floor(0)) = (2, 1, 0).
  Eigen::Vector3d p1(6, 4, 3);
  int i, j, k;
  floor(p1, lc, dx, i, j, k);
  std::cout << "i = " << i << ", j = " << j << ", k = " << k << std::endl;

  // (i, j, k) should be (3, 8, 9).
  Eigen::Vector3d p2(7, 18, 22);
  floor(p2, lc, dx, i, j, k);
  std::cout << "i = " << i << ", j = " << j << ", k = " << k << std::endl;

  // (i, j, k) should be (-1, 0, 0)--wait, negative indices aren't
  // allowed, right?!
  Eigen::Vector3d p3(-1, 2, 3);
  floor(p3, lc, dx, i, j, k);
  std::cout << "i = " << i << ", j = " << j << ", k = " << k << std::endl;

  return 0;
}


#include <cstdlib>
#include <iostream>
#include <fstream>
#include <iterator>
#include <vector>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Construct_theta_graph_2.h>
#include <CGAL/property_map.h>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

// select the kernel type
typedef CGAL::Exact_predicates_inexact_constructions_kernel   Kernel;
typedef Kernel::Point_2                   Point_2;
typedef Kernel::Direction_2               Direction_2;

/* define the struct for edge property */
struct Edge_property {
  /* record the Euclidean length of the edge */
  double euclidean_length;
};

// define the Graph (e.g., to be undirected,
// and to use Edge_property as the edge property, etc.)
typedef boost::adjacency_list<boost::listS,
                              boost::vecS,
                              boost::undirectedS,
                              Point_2,
                              Edge_property>  Graph;

int main(int argc, char ** argv)
{
  const std::string fname = argc!=3 ? "data/n20.cin" : argv[2];
  unsigned int k = argc!=3 ? 6 : atoi(argv[1]);

  if (argc != 3) {
    std::cout << "Usage: " << argv[0] << " <no. of cones> <input filename>" << std::endl;
    std::cout << "Using default values " << k << " " << fname << "\n";
  }

  if (k<2) {
    std::cout << "The number of cones should be larger than 1!" << std::endl;
    return 1;
  }
  // open the file containing the vertex list
  std::ifstream inf(fname);
  if (!inf) {
    std::cout << "Cannot open file " << argv[1] << "!" << std::endl;
    return 1;
  }

  // iterators for reading the vertex list file
  std::istream_iterator< Point_2 > input_begin( inf );
  std::istream_iterator< Point_2 > input_end;

  // initialize the functor
  // If the initial direction is omitted, the x-axis will be used
  CGAL::Construct_theta_graph_2<Kernel, Graph> theta(k);
  // create an adjacency_list object
  Graph g;
  // construct the theta graph on the vertex list
  theta(input_begin, input_end, g);

  // select a source vertex for dijkstra's algorithm
  boost::graph_traits<Graph>::vertex_descriptor v0;
  v0 = vertex(0, g);
  std::cout << "The source vertex is: " << g[v0] << std::endl;

  std::cout << "The index of source vertex is: " << v0 << std::endl;

  // calculating edge length in Euclidean distance and store them in the edge property
  boost::graph_traits<Graph>::edge_iterator ei, ei_end;
  for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
    boost::graph_traits<Graph>::edge_descriptor e = *ei;
    boost::graph_traits<Graph>::vertex_descriptor  u = source(e, g);
    boost::graph_traits<Graph>::vertex_descriptor  v = target(e, g);
    const Point_2& pu = g[u];
    const Point_2& pv = g[v];

    double dist = CGAL::sqrt( CGAL::to_double(CGAL::squared_distance(pu,pv)) );
    g[e].euclidean_length = dist;
    std::cout << "Edge (" << g[u] << ", " << g[v] << "): " << dist << std::endl;
  }

  // calculating the distances from v0 to other vertices
  boost::graph_traits<Graph>::vertices_size_type n = num_vertices(g);
  // vector for storing the results
  std::vector<double> distances(n);
  // Calling the Dijkstra's algorithm implementation from boost.
  boost::dijkstra_shortest_paths(g,
                                 v0,
                                 boost::weight_map(get(&Edge_property::euclidean_length, g)).
                                 distance_map(CGAL::make_property_map(distances)) );

  std::cout << "distances are:" << std::endl;
  for (unsigned int i=0; i < n; ++i) {
    std::cout << "distances[" << i << "] = " << distances[i] << ", (x,y)=" << g[vertex(i, g)];
    std::cout << " at Vertex " << i << std::endl;
  }

  return 0;
}


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_POW_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_POW_HPP_INCLUDED

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

 /*!

    @ingroup group-exponential
    Function object implementing pow capabilities

    Computes \f$x^y\f$

    @par Semantic:

    For every parameters of floating types respectively T, U:

    @code
    T r = pow(x,y);
    @endcode

    is similar to:

    @code
    T r = exp(y*log(x));
    @endcode

    The pow function is conformant to std:pow considering the limits behaviours
    defined by the standard:

         - pow(+0, y), where y is a negative odd integer, returns +Inf
         - pow(-0, y), where y is a negative odd integer, returns -Inf
         - pow(+/-0, y), where y is negative, finite, and is an even integer or a non-integer, returns +Inf
         - pow(+/-0, -Inf) returns +inf
         - pow(+0, y), where y is a positive odd integer, returns +0
         - pow(-0, y), where y is a positive odd integer, returns -0
         - pow(+/-0, y), where y is positive non-integer or a positive even integer, returns +0
         - pow(-1, +/-Inf) returns 1
         - pow(+1, y) returns 1 for any y, even when y is Nan
         - pow(x, +/-0) returns 1 for any x, even when x is Nan
         - pow(x, y) returns Nan if x is finite and negative and y is finite and non-integer.
         - pow(x, -Inf) returns +Inf for any |x|<1
         - pow(x, -Inf) returns +0 for any |x|>1
         - pow(x, +Inf) returns +0 for any |x|<1
         - pow(x, +Inf) returns +Inf for any |x|>1
         - pow(-Inf, y) returns -0 if y is a negative odd integer
         - pow(-Inf, y) returns +0 if y is a negative non-integer or even integer
         - pow(-Inf, y) returns -Inf if y is a positive odd integer
         - pow(-Inf, y) returns +Inf if y is a positive non-integer or even integer
         - pow(+Inf, y) returns +0 for any negative y
         - pow(+Inf, y) returns +Inf for any positive y

      But return a value of the same type as the first parameter, which is necessary for good SIMD behaviour.

      @par Decorators

        -std_ decorator provides access to std:pow

        -fast_ decorator almost nearly uses the naive formula and so doesnot really care for limits and leads to lower

  **/
  Value pow(Value const & v0, Value const& y);
} }
#endif

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

#endif


// IIR_Butterworth.cpp : This file contains the 'main' function. Program execution begins and ends there.
//Calculate the coefficients of the butterworth filter. It follows the steps as described in Matlab.
//The same symbols as in Matlab are used to facilitate the writing of the code
#define _CRTDBG_MAP_ALLOC
#include <stdio.h>
#include <stdlib.h>
#include <crtdbg.h>
#include <iostream>
#include <math.h>
#include <string.h>
#include <vector>
#include <complex.h>
#include "IIR_Butterworth.h"

#define ARMA_DONT_USE_CXX11
#include <armadillo>

#ifdef _DEBUG
#define DBG_NEW new ( _NORMAL_BLOCK , __FILE__ , __LINE__ )
// Replace _NORMAL_BLOCK with _CLIENT_BLOCK if you want the
// allocations to be of _CLIENT_BLOCK type
#else
#define DBG_NEW new
#endif

#define PI 3.141592653589793

int main()
{

    double f1 = 100;  //High Pass
    double f2 = 300; //Low Pass
    double sf = 2048;    //Sampling frequency
    int order_filt = 2; //Order
    double Nyquist_F = sf / 2;

    std::vector<std::vector<double> > coeff_final(2);

    int type_filt = 0;
    IIR_B_F::IIR_Butterworth ir_b;

    bool check_stability_flag;

    //_CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);
    //_CrtSetBreakAlloc(154);
    //_CrtSetBreakAlloc(155);
    //_CrtSetBreakAlloc(156);

    int coeff_numb = 0;
    switch (type_filt)
    {
    case 0:

        coeff_numb = 2*(size_t)order_filt + 1;

        for (int i = 0; i < 2; i++)
        {

            coeff_final[i].resize(coeff_numb);

        }

        _CrtDumpMemoryLeaks();

        coeff_final = ir_b.lp2bp(f1 / Nyquist_F, f2 / Nyquist_F, order_filt);

       check_stability_flag =  ir_b.check_stability_iir(coeff_final);

       if (check_stability_flag)
       {

           std::cout << "The filter is stable" << std::endl;

       }

       else
       {

           std::cout << "The filter is unstable" << std::endl;

       }

        for (int kk = 0; kk < 2; kk++)
        {
            if (kk == 0)
            {

                std::cout << "Numerator: " << std::ends;

            }

            else
            {

                std::cout << "Denumerator: " << std::ends;

            }

            for (int ll = 0; ll < coeff_numb; ll++)

            {
                std::cout << coeff_final[kk][ll] << "\t" << std::ends;

            }

            std::cout << std::endl;

        }

        break;

    case 1:

        coeff_numb = 2 * order_filt + 1;

        for (int i = 0; i < 2; i++)
        {

            coeff_final[i].resize(coeff_numb);

        }

        _CrtDumpMemoryLeaks();
        coeff_final = ir_b.lp2bs(f1 / Nyquist_F, f2 / Nyquist_F, order_filt);

        check_stability_flag = ir_b.check_stability_iir(coeff_final);

        if (check_stability_flag)
        {

            std::cout << "The filter is stable" << std::endl;

        }

        else
        {

            std::cout << "The filter is unstable" << std::endl;

        }

        for (int kk = 0; kk < 2; kk++)
        {

            if (kk == 0)
            {

                std::cout << "Numerator: " << std::ends;

            }

            else
            {

                std::cout << "Denumerator: " << std::ends;

            }

            for (int ll = 0; ll < coeff_numb; ll++)

            {

                std::cout << coeff_final[kk][ll] << "\t" << std::ends;

            }

            std::cout << std::endl;

        }

        break;

    case 2:

        coeff_numb = order_filt + 1;

        for (int i = 0; i < 2; i++)
        {

            coeff_final[i].resize(coeff_numb);

        }

        _CrtDumpMemoryLeaks();
        coeff_final = ir_b.lp2hp(f1 / Nyquist_F, order_filt);

        check_stability_flag = ir_b.check_stability_iir(coeff_final);

        if (check_stability_flag)
        {

            std::cout << "The filter is stable" << std::endl;

        }

        else
        {

            std::cout << "The filter is unstable" << std::endl;

        }

        for (int kk = 0; kk < 2; kk++)
        {

            if (kk == 0)
            {

                std::cout << "Numerator: " << std::ends;

            }

            else
            {

                std::cout << "Denumerator: " << std::ends;

            }

            for (int ll = 0; ll < coeff_numb; ll++)

            {

                std::cout << coeff_final[kk][ll] << "\t" << std::ends;

            }

            std::cout << std::endl;
        }

        break;

    case 3:
        coeff_numb = order_filt + 1;

        for (int i = 0; i < 2; i++)
        {

            coeff_final[i].resize(coeff_numb);

        }

        _CrtDumpMemoryLeaks();
        coeff_final = ir_b.lp2lp(f2 / Nyquist_F, order_filt);

        check_stability_flag = ir_b.check_stability_iir(coeff_final);

        if (check_stability_flag)
        {

            std::cout << "The filter is stable" << std::endl;

        }

        else
        {

            std::cout << "The filter is unstable" << std::endl;

        }

        for (int kk = 0; kk < 2; kk++)
        {

            if (kk == 0)
            {

                std::cout << "Numerator: " << std::ends;

            }

            else
            {

                std::cout << "Denumerator: " << std::ends;

            }

            for (int ll = 0; ll < coeff_numb; ll++)

            {

                std::cout << coeff_final[kk][ll] << "\t" << std::ends;

            }

            std::cout << std::endl;

        }

        break;

    }

     //Create a complex sine wave
    std::vector<double> test_sin(sf,0.0);
    double sf_1 = 20;
    double sf_2 = 60;
    for (double kk = 0; kk < sf; kk++)
    {

            test_sin[kk] = sin(2*PI*kk*sf_1/sf) + sin(2*PI*kk*sf_2/sf);

    }

    std::vector<double> filt_sign = ir_b.Filter_Data(coeff_final,test_sin);

}


/* ----------------------------------------------------------------------------
 * Copyright 2022, Jeferson Lima
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 *  @file   examples/rotation_ex.cpp
 *  @author Jeferson Lima
 *  @brief  Rotation Matrix Example
 *  @date   Mar 7, 2022
 **/

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

void rot2dVec(const Eigen::Matrix<float, 4, 1>& v,
              Eigen::Matrix<float, 4, 1>& r,
	      float theta)
{

  Eigen::Matrix<float, 4, 4> rotation;
  rotation << std::cos(theta), -std::sin(theta), 0.0, 0.0,
              std::sin(theta), std::cos(theta),  0.0, 0.0,
	      0.0,             0.0,              1.0, 0.0,
	      0.0,             0.0,              0.0, 1.0;

  std::cout << "v values: \n" << v << std::endl;
  std::cout << "rotation values: \n" << rotation << std::endl;

  r = rotation * v;

}

int main(int argc, char* argv[])
{

  Eigen::Matrix<float, 4, 1> v;
  Eigen::Matrix<float, 4, 1> r;
  float theta;

  // init matrix
  v << 0.5, 0.5, 0.0, 1.0;
  theta = 0.785398; // 45 degree -> radian

  rot2dVec(v, r, theta);

  std::cout << "r values:\n" << r << std::endl;

  return 0;
}


#ifndef TRIUMF_NMR_UTILITIES_HPP
#define TRIUMF_NMR_UTILITIES_HPP

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

#include <triumf/constants/codata_2018.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// nuclear magnetic resonance
namespace nmr {

// miscellaneous helper functions
namespace utilities {

// calculate the gyromagnetic ratio (MHz / T) of an NMR probe nucleus from its
// magnetic dipole moment (nm) and spin quantum number
template <typename T>
constexpr T calculate_gamma(T magnetic_dipole_moment, T spin_quantum_number) {
  return 1e6 * boost::math::constants::two_pi<T>() *
         triumf::constants::codata_2018::nuclear_magneton_in_MHz_T<T>::value() *
         magnetic_dipole_moment / spin_quantum_number;
}

} // namespace utilities

} // namespace nmr

} // namespace triumf

#endif // TRIUMF_NMR_UTILITIES_HPP


#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <sophus/se3.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <vector>
#include <cmath>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>
using namespace std;
using namespace Eigen;

string TrajectoryFile = "../compare.txt";
void ICP(vector<Vector3d> pts_est, vector<Vector3d> pts_gt, Matrix3d &R, Vector3d &t);
void DrawTrajectory(vector<Sophus::SE3d> poses1, vector<Sophus::SE3d> poses2, string winName="Trajectory Viewer");

int main(){
    ifstream fin(TrajectoryFile);
    vector<Vector3d> pts_est,pts_gt;
    vector<Sophus::SE3d> poses_gt, poses_est;
    while(!fin.eof()){
        double time, tx,ty,tz,qx,qy,qz,qw;
        fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
        Quaterniond q(qw,qx,qy,qz);
        Vector3d t_est(tx,ty,tz);
        Sophus::SE3d SE3_est(q,t_est);
        pts_est.push_back(t_est);
        poses_est.push_back(SE3_est);

        fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
        Quaterniond q2(qw,qx,qy,qz);
        Vector3d t_gt(tx,ty,tz);
        Sophus::SE3d SE3_gt(q2,t_gt);
        poses_gt.push_back(SE3_gt);
        pts_gt.push_back(t_gt);
    }
    Matrix3d R;
    Vector3d t;
    ICP(pts_est, pts_gt, R, t);
    Sophus::SE3d SE3_E2G(R, t);

    int N = poses_gt.size();
    vector<Sophus::SE3d> poses_after(N);
    for(int i=0;i<N;i++){
        poses_after[i] = SE3_E2G*poses_est[i];
    }
    DrawTrajectory(poses_gt, poses_est, "before");
    DrawTrajectory(poses_gt, poses_after, "after");

    double error=0;
    for (int i=0;i<N;i++){
        Sophus::SE3d SE3_differ = poses_gt[i].inverse()*poses_after[i];
        typedef Eigen::Matrix<double, 6, 1> Vector6d;
        Vector6d se3_differ = SE3_differ.log();
        error += se3_differ.dot(se3_differ);
    }
    double RMSE = sqrt(error/N);
    cout<<"RMSE: "<<RMSE<<endl;

    return 0;
}

void ICP(vector<Vector3d> pts_est, vector<Vector3d> pts_gt, Matrix3d &R, Vector3d &t){
    Vector3d pe(0,0,0), pg(0,0,0);
    int N = pts_est.size();
    for(int i=0;i<N;i++){
        pe += pts_est[i];
        pg += pts_gt[i];
    }
    pe /= N;
    pg /= N;
    vector<Vector3d> q_est(N), q_gt(N);
    for (int i=0;i<N;i++){
        q_est[i] = pts_est[i]-pe;
        q_gt[i] = pts_gt[i]-pg;
    }
    Matrix3d W = Matrix3d::Zero();
    for (int i=0;i<N;++i){
        W += q_gt[i]*q_est[i].transpose();
    }
    cout<<"W="<<W<<endl;
    JacobiSVD<Matrix3d> svd(W, ComputeFullU|ComputeFullV);
    Matrix3d U = svd.matrixU();
    Matrix3d V = svd.matrixV();
    cout<<"U="<<U<<endl;
    cout<<"V="<<V<<endl;
    Matrix3d R_ = U*(V.transpose());
    if(R_.determinant()<0){
        R_ = -R_;
    }

    Vector3d t_ = pg-R_*pe;
    R = R_;
    t = t_;
}

void DrawTrajectory(vector<Sophus::SE3d> poses1, vector<Sophus::SE3d> poses2, string winName) {

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind(winName, 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glLineWidth(2);
        for (size_t i = 0; i < poses1.size() - 1; i++) {
            glColor3f(1, 0.0f, 0);
            glBegin(GL_LINES);
            auto p1 = poses1[i], p2 = poses1[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        for (size_t i = 0; i < poses2.size() - 1; i++) {
            glColor3f(0, 0.0f, 1);
            glBegin(GL_LINES);
            auto p1 = poses2[i], p2 = poses2[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }

        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }

}


#include <iostream>
#include <array>
#include <cmath>
#include <tuple>
#include <cassert>
#include <NTL/ZZ.h>
#include <NTL/vector.h>
#include <random>
#include <chrono>

using namespace std;
using namespace NTL;

//ex. compile:
//g++ -o <exe> remainder_alg_first_pass.cpp -std=c++11 -march=native -O2 -lntl -lgmp -lgmpxx -pthread

// to compile and run on linux use
// g++ -o <executable file name> remainder_alg_first_pass.cpp
// ./<executable file name>

//note that everywhere it says 'int' we will eventually want to use GMP or whatnot
//ints are too small...

//translates indices in trees to indices in arrays
//if you want left child of (i,j), do (i+1, 2*j)
//right child is (i+1, 2*j+1), and parent is (i-1, j//2)
int flatten_index(int i, int j)
{
	assert (j <= pow(2,i) - 1);
	return pow(2,i) +j-1;
}

//gives tree indices from array index
tuple<int, int> lift_index(int k)
{
	int i = log2(k+1);
	int j = k + 1 - pow(2,i);

	return make_tuple(i,j);
}

void print_tree(Vec<ZZ> & X)
{
	cout << endl;
	for (int i = 0; i < X.length(); i++)
	{
		cout << X[i] << " , ";
	}
	cout << endl;
}

ZZ mtree_val(int i, int j, const Vec<ZZ> &m)
{
	int N = m.length();
	int depth = log2(N);
	assert ((i <= depth) and (j < pow(2, i)));

	if (i == depth)
	{
		return m[j];
	}

	else
	{
		return mtree_val(i+1, 2*j, m)*mtree_val(i+1, 2*j+1, m);
	}
}

ZZ Atree_val(int i, int j, const Vec<ZZ> &A)
{
	int N = A.length();
	int depth = log2(N);
	assert ((i <= depth) and (j < pow(2,i)));

	if ((i == depth) or (j == 0))
	{
		return A[j];
	}

	else
	{
		return Atree_val(i+1, 2*j-1, A)*Atree_val(i+1, 2*j, A);
	}
}

//k will be approx log log N (it's the space-time tradeoff)
//when k = 0 we have normal remainder tree
Vec<ZZ> RemTree(const Vec<ZZ> &A, const Vec<ZZ> &m, const ZZ &V = ZZ(1))
{
	int N = A.length();
	assert (N == m.length());
	int depth = log2(N);

	//assume the input is of size a power of 2
	Vec<ZZ> mtree;
	mtree.SetLength(2*N-1);

	//initialize leaves
	for (int j = 0; j < pow(2, depth); j++)
	{
		int leaf = flatten_index(depth,j);
		mtree[leaf] = m[j];
	}

	//build up the product tree recursively
	for (int i = depth-1; i >= 0; i--)
	{
		for (int j = 0; j < pow(2,i); j++)
		{
			int parent = flatten_index(i,j);
			int left = 2*parent + 1;
			int right = left + 1;

			//padding is not necessary if on this step we catch IndexError with 1
			mtree[parent] = mtree[left]*mtree[right];
		}
	}

	Vec<ZZ> Atree;
	Atree.SetLength(2*N-1);

	//initialize leaves
	for (int j = 0; j < pow(2, depth); j++)
	{
		int leaf = flatten_index(depth, j);
		Atree[leaf] = A[j];
	}

	//build up the 'product' tree recursively
	for (int i = depth-1; i >= 0; i--)
	{
		int leftmost = flatten_index(i,0);
		int leftmost_child = 2*leftmost + 1;
		Atree[leftmost] = A[leftmost_child];

		for (int j = 1; j < pow(2, i); j++)
		{
			int parent = flatten_index(i,j);
			int superleft = 2*parent;
			int left = superleft + 1;

			Atree[parent] = Atree[superleft]*Atree[left];
		}
	}

	Vec<ZZ> remtree;
	remtree.SetLength(2*N-1);

	remtree[0] = (V*A[0])%mtree[0];

	for (int i = 0; i < depth; i++)
	{
		//notice it goes every two!
		for (int j = 0; j < pow(2, i); j++)
		{
			int parent = flatten_index(i,j);
			int left = 2*parent + 1;
			int right = left + 1;

			remtree[left] = remtree[parent]%mtree[left];
			remtree[right] = (remtree[parent]*Atree[right])%mtree[right];
		}
	}

	Vec<ZZ> remainders;
	remainders.SetLength(N);

	int leftmost_leaf = flatten_index(depth, 0);
	for (int j = 0; j < N; j++)
	{
		remainders[j] = remtree[leftmost_leaf+j];
	}
	return remainders;
}

//searches up to N for Wilson primes
Vec<ZZ> WilsonRemainders(long N, long interval)
{
	Vec<ZZ> A;
	Vec<ZZ> m;

	A.SetLength(interval);
	m.SetLength(interval);

	Vec<ZZ> remainders;
	remainders.SetLength(N);

	ZZ V = ZZ(1);

	for (int b = 0; b*interval < N; b++)
	{
		for (int i = 0; i < interval; i++)
		{
			ZZ val = ZZ(b*interval+(i+1));

			A[i] = val-1;
			if (val == 1)
			{
				A[i] += 1;
			}

			if (ProbPrime(val))
			{
				m[i] = val*val;
			}

			else
			{
				m[i] = 1;
			}
		}

		Vec<ZZ> interval_remainders = RemTree(A, m, V);
		for (int i = 0; i < interval; i++)
		{
			remainders[b*interval+i] = interval_remainders[i];
			V *= A[i];
		}
	}

	return remainders;
}

int main()
{
	long N = pow(2,1);

	Vec<ZZ> A;
	Vec<ZZ> m;

	A.SetLength(N);
	m.SetLength(N);

	random_device rd;
	mt19937 mt(rd());
	uniform_int_distribution<int> dist(1, N);

	for(int i = 0; i < N; i++){
		int bitsize = log2(i+1)+2;

		A[i] = dist(mt);
		m[i] = dist(mt);

	}

	//Vec<ZZ> ztree = zero_vector(2*N);

	//print_tree(A);
	//print_tree(m);

	auto start = chrono::high_resolution_clock::now();
	Vec<ZZ> remainders = RemTree(A,m);
	Vec<ZZ> WREM = WilsonRemainders(pow(2,18), pow(2,18));
	auto finish = chrono::high_resolution_clock::now();

	chrono::duration<double> runtime = finish-start;

	//print_tree(WREM);
	cout << remainders[0] << endl;
	cout << WREM[562] << " , " << 563*563 << endl;
	cout << runtime.count() << endl;

}


// Compression methods for vectors
// Author: Max Schwarz <max.schwarz@uni-bonn.de>

#include <vector_compression/vector_compression.h>

#include <Eigen/Core>

#include "contrib/angles.h"

#include <iostream>

#define DEBUG 0

namespace vector_compression
{

// The "hemi-oct" compression scheme is taken from
//  Cigolle, Zina H., Sam Donow, and Daniel Evangelakos.
//  "A survey of efficient representations for independent unit vectors."
//  Journal of Computer Graphics Techniques Vol 3.2 (2014).

// Assume normalized input on +Z hemisphere.
// Output is [-1, 1]
static Eigen::Vector2f float32x3_to_hemioct(const Eigen::Vector3f& v)
{
	// Project the hemisphere onto the hemi-octahedron,
	// and then project into the xy plane
	Eigen::Vector2f p = v.head<2>() * (1.0f / (fabsf(v.x()) + fabsf(v.y()) + v.z()));

	// Rotate and scale the center diamond to the unit square
	return Eigen::Vector2f(p.x() + p.y(), p.x() - p.y());
}

static Eigen::Vector3f hemioct_to_float32x3(const Eigen::Vector2f& e)
{
	// Rotate and scale the unit square back to the center diamond
	Eigen::Vector2f temp = 0.5f * Eigen::Vector2f(e.x() + e.y(), e.x() - e.y());
	Eigen::Vector3f v;
	v << temp, 1.0 - fabsf(temp.x()) - fabsf(temp.y());

	return v.normalized();
}

int32_t float_to_snormb(float v, int b)
{
	return std::round(std::min(1.0f, std::max(-1.0f, v)) * ((1 << (b-1)) - 1));
}

float snormb_to_float(int32_t v, int b)
{
	// Sign extend
	if(v & (1 << (b-1)))
		v |= 0xFFFFFFFF & ~((1 << b) - 1);

	return std::min(1.0f, std::max(-1.0f, ((float)v) / ((1 << (b-1)) - 1)));
}

void encodeQuaternion(const Eigen::Quaternionf& quat, uint8_t* dest)
{
	// Recover rotation angle & axis
	float angle = 2.0f * acosf(quat.w());
	angle = angles::normalize_angle(angle);

	float norm = sqrtf(1.0f - quat.w()*quat.w());

	Eigen::Vector3f axis;

	if(norm != 0)
	{
		axis << quat.x() / norm, quat.y() / norm, quat.z() / norm;
	}
	else
		axis << 0.0f, 0.0f, 0.0f;

	// Map to the upper hemisphere
	if(axis.z() < 0)
	{
		axis *= -1.0f;
		angle = -angle;
	}

	Eigen::Vector2f hemi = float32x3_to_hemioct(axis);

	int32_t hemi_a = float_to_snormb(hemi.x(), 12);
	int32_t hemi_b = float_to_snormb(hemi.y(), 12);

	int32_t angle_sn = float_to_snormb(angle / M_PI, 16);

#if DEBUG
	printf("encode: %f, %f, %f, angle_sn: %u (0x%X)\n", hemi.x(), hemi.y(), angle, angle_sn, angle_sn);
	printf("angle scale: %d\n", ((1 << (16-1)) - 1));
#endif

	dest[0] = hemi_a;
	dest[1] = ((hemi_a >> 8) & 0xF) | ((hemi_b & 0xF) << 4);
	dest[2] = hemi_b >> 4;
	dest[3] = angle_sn;
	dest[4] = angle_sn >> 8;
}

Eigen::Quaternionf decodeQuaternion(const uint8_t* src)
{
	int32_t hemi_a = src[0] | (((uint32_t)src[1] & 0xF) << 8);
	int32_t hemi_b = (src[1] >> 4) | (((uint32_t)src[2]) << 4);
	int32_t angle_sn = src[3] | (((uint32_t)src[4]) << 8);

	Eigen::Vector2f hemi(
		snormb_to_float(hemi_a, 12), snormb_to_float(hemi_b, 12)
	);

	Eigen::Vector3f axis = hemioct_to_float32x3(hemi);

	float angle = snormb_to_float(angle_sn, 16) * M_PI;

#if DEBUG
	printf("decode: %f, %f, %f, angle_sn: %d\n", hemi.x(), hemi.y(), angle, angle_sn);
#endif

	return Eigen::Quaternionf(Eigen::AngleAxisf(angle, axis));
}

Eigen::Vector3f latticeToVector(const Eigen::Vector3i& lattice, float r)
{
	return r * Eigen::Vector3f(
		lattice.y() + lattice.z(),
		lattice.x() + lattice.z(),
		lattice.x() + lattice.y()
	);
}

Eigen::Vector3f alignedLatticeToVector(const Eigen::Vector3i& lattice, float r)
{
	Eigen::Vector3f m = lattice.cast<float>();

	Eigen::Vector3f x = m + m.y() * Eigen::Vector3f::UnitY() + ((lattice.x() + lattice.z()) % 2) * Eigen::Vector3f::UnitY();

	x *= r;

	return x;
}

static inline int sgn(float x)
{
	if(x >= 0)
		return 1;
	else
		return -1;
}

static inline float roundFloat(float x)
{
	return std::round(x);
}

Eigen::Vector3i vectorToAlignedLattice(const Eigen::Vector3f& p, float r)
{
	Eigen::Vector3f m = p / r;

	Eigen::Vector3f cartesian_lattice = m.unaryExpr(std::ptr_fun(roundFloat));
	Eigen::Vector3i lattice_int = cartesian_lattice.cast<int>();

	if(lattice_int.sum() % 2 != 0)
	{
		Eigen::Vector3f local = m - cartesian_lattice;
		int c;
		local.array().abs().maxCoeff(&c);

		lattice_int[c] += sgn(local[c]);
	}

	lattice_int.y() = (lattice_int.y() - ((lattice_int.x() + lattice_int.z()) % 2)) / 2;

	return lattice_int;
}

Eigen::Vector3i vectorToLattice(const Eigen::Vector3f& p, float r)
{
	Eigen::Vector3f normalized = p / r;

	Eigen::Vector3f cartesian_lattice = normalized.unaryExpr(std::ptr_fun(roundFloat));
	Eigen::Vector3i lattice_int = cartesian_lattice.cast<int>();

	if(lattice_int.sum() % 2 != 0)
	{
		Eigen::Vector3f local = normalized - cartesian_lattice;
		int c;
		local.array().abs().maxCoeff(&c);

		lattice_int[c] += sgn(local[c]);
	}

	Eigen::Matrix3i inv;
	inv <<
		-1, 1, 1,
		1, -1, 1,
		1, 1, -1
	;

	return (inv * lattice_int) / 2;
}

void encode3DVector6(const Eigen::Vector3f& vec, float extent, uint8_t* dest)
{
	// We are using 16 bit resolution per lattice index.
	float lattice_r = extent / (2.0f * ((1 << 15)-1));

	Eigen::Vector3i lattice = vectorToLattice(vec, lattice_r);

	for(int i = 0; i < 3; ++i)
	{
		lattice[i] = std::min(32767, std::max(-32768, lattice[i]));
		dest[2*i] = lattice[i];
		dest[2*i+1] = lattice[i] >> 8;
	}
}

Eigen::Vector3f decode3DVector6(float extent, const uint8_t* src)
{
	// We are using 16 bit resolution per lattice index.
	float lattice_r = extent / (2.0f * ((1 << 15)-1));

	Eigen::Vector3i lattice;
	for(int i = 0; i < 3; ++i)
	{
		int16_t idx = src[2*i] | (((uint16_t)src[2*i+1]) << 8);
		lattice[i] = idx;
	}

	return latticeToVector(lattice, lattice_r);
}

VectorCompression::VectorCompression(unsigned int bits)
 : m_bits(bits)
{
	setExtent(Eigen::Vector3f(1.0, 1.0, 1.0));
}

void VectorCompression::setExtent(const Eigen::Vector3f& extent)
{
	m_extent = extent;

	m_r = powf(sqrtf(2.0) * extent.x() * extent.y() * extent.z() / (1LL << m_bits), 1.0f / 3.0f) * sqrtf(2.0);

	m_latticeExtent.x() = std::floor(extent.x() / m_r);
	m_latticeExtent.y() = std::floor(extent.y() / m_r / 2.0f);
	m_latticeExtent.z() = std::floor(extent.z() / m_r);

	uint64_t high = 8 * m_latticeExtent.x() * m_latticeExtent.y() * m_latticeExtent.z();
	if(high > (1ULL << m_bits))
	{
		std::cerr << "lattice overflow: " << high << " > " << (1LL << 24) << "\n";
		std::cerr << "calculated radius: " << m_r;
		throw std::runtime_error("lattice overflow");
	}
}

uint64_t VectorCompression::compress(const Eigen::Vector3f& vec) const
{
	Eigen::Vector3i lattice = vectorToAlignedLattice(vec, m_r);

	// Shift to [0, 2*m_latticeExtent]
	lattice += m_latticeExtent;

	// Constrain
	lattice.x() = std::max(0, std::min(2*m_latticeExtent.x(), lattice.x()));
	lattice.y() = std::max(0, std::min(2*m_latticeExtent.y(), lattice.y()));
	lattice.z() = std::max(0, std::min(2*m_latticeExtent.z(), lattice.z()));

	uint64_t index = lattice.x() + lattice.y() * 2 * m_latticeExtent.x() + lattice.z() * 4 * m_latticeExtent.x() * m_latticeExtent.y();

	// Sanity check
	if(index >= (1ULL << m_bits))
	{
		std::cerr << "lattice overflow while compressing " << vec.transpose() << "\n";
		std::cerr << "radius: " << m_r << "\n";
		std::cerr << "lattice extents: " << m_latticeExtent.transpose() << "\n";
		std::cerr << "lattice coords: " << lattice.transpose() << "\n";
		std::cerr << "=> index: " << index << "\n";
		index = (1 << m_bits) - 1;
	}

	return index;
}

Eigen::Vector3f VectorCompression::uncompress(uint64_t compressed) const
{
	Eigen::Vector3i lattice;

	lattice.z() = compressed / (4 * m_latticeExtent.x() * m_latticeExtent.y());

	uint64_t remainder = compressed % (4 * m_latticeExtent.x() * m_latticeExtent.y());

	lattice.y() = remainder / (2*m_latticeExtent.x());
	lattice.x() = remainder % (2*m_latticeExtent.x());

	// Shift to [-extent, extent]
	lattice -= m_latticeExtent;

	return alignedLatticeToVector(lattice, m_r);
}

float VectorCompression::guaranteedPrecision() const
{
	return sqrt(2.0f) * m_r;
}

}


// MIT License

// Copyright (c) 2018 Benjamin Bercovici

// 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.

#ifndef STATE_HEADER
#define STATE_HEADER

#include <armadillo>

namespace OC{

	class State{

	public:

		State(arma::vec state,double mu);

		/**
		Computes true anomaly from eccentric anomaly
		@param ecc eccentric anomaly
		@param e eccentricity (0 =< e < 1)
		@return true anomaly
		*/
		static double f_from_ecc(const double & ecc,const double & e);

		/**
		Computes eccentric anomaly eccentric from true anomaly
		@param f true anomaly
		@param e eccentricity (0 =< e < 1)
		@return eccentric anomaly
		*/
		static double ecc_from_f(const double & f,const double & e);

		/**
		Computes hyperbolic anomaly from true anomaly
		@param f true anomaly
		@param e eccentricity (1 < e)
		@return hyperbolic anomaly
		*/
		static double H_from_f(const double & f,const double & e);

		/**
		Computes true anomaly from hyperbolic anomaly
		@param H hyperbolic anomaly
		@param e eccentricity (1 < e)
		@return true anomaly
		*/
		static double f_from_H(const  double & H,const  double & e);

		/**
		Computes true anomaly from mean anomaly
		@param M mean anomaly
		@param e eccentricity (0 =< e < 1)
		@return true anomaly
		*/
		static double f_from_M(const double & M,const double & e);

		/**
		Computes eccentric anomaly eccentric from mean anomaly
		@param M mean anomaly
		@param e eccentricity (0 =< e < 1)
		@param pedantic if true, will print out convergence details
		@return eccentric anomaly
		*/
		static double ecc_from_M(const double & M,const double & e,const bool & pedantic = false);

		/**
		Computes hyperbolic anomaly eccentric from mean anomaly
		@param M mean anomaly
		@param e eccentricity (1 < e)
		@param pedantic if true, will print out convergence details
		@return hyperbolic anomaly
		*/
		static double H_from_M(const  double & M,const  double & e,const bool & pedantic = false);

		/**
		Computes mean anomaly from eccentric anomaly
		@param ecc eccentric anomaly
		@param e eccentricity (0 =< e < 1)
		@return mean anomaly
		*/
		static double M_from_ecc(const double & ecc,const double & e);

		/**
		Computes mean anomaly from hyperbolic anomaly
		@param H hyperbolic anomaly
		@param e eccentricity (1 < e)
		@return mean anomaly
		*/
		static double M_from_H(const double & H,const double & e);

		/**
		Computes mean anomaly from true anomaly
		@param f true anomaly
		@param e eccentricity (0 =< e < 1)
		@return mean anomaly
		*/
		static double M_from_f(const double & f,const double & e);

		virtual double get_momentum() const = 0;
		virtual double get_energy() const = 0;
		virtual double get_a() const = 0;
		virtual double get_eccentricity() const = 0;

		/**
		Get the state vector
		@return 6x1 state vector (either cartesian state or keplerian state)
		*/
		arma::vec get_state() const;

		/**
		Get mean motion
		@return mean motion (rad/s)
		*/
		double get_n() const;

		/**
		Get ellipse parameter
		@return ellipse parameter (m)
		*/
		double get_parameter() const;

		/**
		Get standard gravitational parameter
		@return standard gravitational parameter (kg^3/s^2)
		*/
		double get_mu() const;

		/**
		Set standard gravitational parameter
		@param mu standard gravitational parameter (kg^3/s^2)
		*/
		void set_mu(double mu);

		/**
		Sets the state to the prescribed value
		@param state 6x1 state
		*/
		void set_state(arma::vec state) ;

	protected:
		arma::vec state;
		double mu;

	};

}

#endif


// lu_decomposition.cpp example program comparing float vs posit equation solver
//
// Copyright (C) 2017-2019 Stillwater Supercomputing, Inc.
//
// This file is part of the universal numbers project, which is released under an MIT Open Source license.

#include "common.hpp"

#include <iostream>
#include <typeinfo>

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

using namespace std;
using namespace sw::unum;

// Turn it off for now
#define USE_POSIT

int main(int argc, char** argv)
try {
	const size_t nbits = 16;
	const size_t es = 1;
	const size_t vecSize = 32;

#ifdef USE_POSIT
	using Matrix = mtl::dense2D< posit<nbits, es> >;
	using Vector = mtl::dense_vector< posit<nbits, es> >;
#else
	using Matrix = mtl::dense2D<float>;
	using Vector = mtl::dense_vector<float>;
#endif

	Matrix  A(4, 4), L(4, 4), U(4, 4), AA(4, 4);
	Vector	v(4);
	double 	c = 1.0;

	for (unsigned i = 0; i < 4; i++)
		for (unsigned j = 0; j < 4; j++) {
			U[i][j] = i <= j ? c * (i + j + 2) : (0);
			L[i][j] = i > j ? c * (i + j + 1) : (i == j ? (1) : (0));
		}

	std::cout << "L is:\n" << L << "U is:\n" << U;
	A = L * U;
	std::cout << "A is:\n" << A;
	AA = adjoint(A);

	for (unsigned i = 0; i < 4; i++)
		v[i] = double(i);

	Vector b(A*v), b2(adjoint(A)*v);

	Matrix LU(A);
	lu(LU);
	std::cout << "LU decomposition of A is:\n" << LU;

	Matrix B(lu_f(A));
	std::cout << "LU decomposition of A (as function result) is:\n" << B;

	Vector v1(lu_solve_straight(A, b));
	std::cout << "v1 is " << v1 << "\n";

	Vector v2(lu_solve(A, b));
	std::cout << "v2 is " << v2 << "\n";

	mtl::dense_vector<unsigned> P;
	lu(A, P);
	std::cout << "LU with pivoting is \n" << with_format(A, 5, 2) << "Permutation is " << P << "\n";
	Vector v3(lu_apply(A, P, b));
	std::cout << "v3 is " << v3 << "\n";

	Vector v4(lu_adjoint_apply(A, P, b2));
	std::cout << "v4 is " << v4 << "\n";

	Vector v5(lu_adjoint_solve(AA, b));
	std::cout << "v5 is " << v5 << "\n";

	int nrOfFailedTestCases = 0;
	return nrOfFailedTestCases > 0 ? EXIT_FAILURE : EXIT_SUCCESS;
}
catch (char const* msg) {
	cerr << msg << endl;
	return EXIT_FAILURE;
}
catch (const posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	cerr << "caught unknown exception" << endl;
	return EXIT_FAILURE;
}


// Group A - Exact Pricing Methods
//
// by Scott Sidoli
//
// 6-9-19
//
// NormalDistribution.hpp
//
// Function pertaining to the normal distribution including the CDF and PDF.
// Used in option pricing.

#ifndef NormalDistribution_hpp
#define NormalDistribution_hpp

#include <iostream>

using namespace std;
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions.hpp>
using namespace boost::math;

double N(double x)
{

	normal_distribution<> myNormal(0.0, 1.0);

	return cdf(myNormal, x);

}

double n(double x)
{

	normal_distribution<> myNormal(0.0, 1.0);

	return pdf(myNormal, x);

}

#endif


#ifndef TRIUMF_SUPERCONDUCTIVITY_PHENOMENOLOGY_HPP
#define TRIUMF_SUPERCONDUCTIVITY_PHENOMENOLOGY_HPP

#include <cmath>

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

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

//
namespace superconductivity {

// purely phenomenological relations in superconductivity
namespace phenomenology {

// temperature dependence of the (reduced) penetration depth
template <typename T = double>
T reduced_penetration_depth(T reduced_temperature, T exponent) {
  if (reduced_temperature >= 1.0) {
    return std::numeric_limits<T>::infinity();
  } else if (reduced_temperature <= 0.0) {
    return 1.0;
  } else {
    return 1.0 / std::sqrt(1.0 - std::pow(reduced_temperature, exponent));
  }
}

// temperature dependence of the (reduced) penetration depth
template <typename T = double>
T reduced_penetration_depth(T temperature, T critical_temperature, T exponent) {
  return reduced_penetration_depth<T>(temperature / critical_temperature,
                                      exponent);
}

// temperature dependence of the penetration depth
template <typename T = double>
T penetration_depth(T temperature, T critical_temperature, T exponent,
                    T lambda_0) {
  return lambda_0 * reduced_penetration_depth<T>(
                        temperature, critical_temperature, exponent);
}

// reduced gap
// after Halbritter ca. 1970
template <typename T = double> T reduced_gap(T reduced_temperature) {
  if (reduced_temperature >= 1.0) {
    return 0.0;
  } else if (reduced_temperature <= 0.0) {
    return 1.0;
  } else {
    return std::cos(boost::math::constants::half_pi<T>() *
                    std::pow(reduced_temperature, 2));
  }
}

/// The superconducting transition temperature T_c (K) as a function of applied
/// magnetic field. The calculation assumes a "parabolic" relationship by
/// default (i.e., exponent = 0.5).
template <typename T = double>
T critical_temperature(T applied_field, T critical_temperature_0,
                       T critical_field, T exponent = 0.5) {
  if (applied_field < 0.0) {
    // don't consider negative fields
    return critical_temperature_0;
  } else if (applied_field > critical_field) {
    // no superconductivity above the critical field
    return 0.0;
  } else {
    // the phenomenological field dependence
    return critical_temperature_0 *
           std::pow(1.0 - (applied_field / critical_field), exponent);
  }
}

/// The superconducting transition temperature T_c (K) as a function of applied
/// magnetic field. The calculation assumes a relationship obtained from
/// inverting: Hc2(T) / Hc2(0) = [1 - (T / Tc)^2] / [1 + (T / Tc)^2]. See e.g.,:
/// M. Tinkham, Phys. Rev. 129, 2413 (1963).
/// https://doi.org/10.1103/PhysRev.129.2413
template <typename T = double>
T critical_temperature_II(T applied_field, T critical_temperature_0,
                          T upper_critical_field) {
  if (applied_field < 0.0) {
    // don't consider negative fields
    return critical_temperature_0;
  } else if (applied_field > upper_critical_field) {
    // no superconductivity above the critical field
    return 0.0;
  } else {
    // reduced field
    T reduced_field = applied_field / upper_critical_field;
    // the phenomenological field dependence
    return critical_temperature_0 *
           std::sqrt((1.0 - reduced_field * reduced_field) /
                     (1.0 + reduced_field * reduced_field));
  }
}

} // namespace phenomenology

} // namespace superconductivity

} // namespace triumf

#endif // TRIUMF_SUPERCONDUCTIVITY_PHENOMENOLOGY_HPP


//
// Created by tomlucas on 26.06.20.
//

#include "types/SO3.h"
#include <vector>
#include "GaussianNoiseVector.h"
#include "adekf_viz.h"
#include <Eigen/Eigenvalues>

#define max_iterations 100
#define weighted_mean_epsilon 1e-12
using namespace adekf;

/**
 * Calculate the weighted mean of quaternions
 * @param quats  list of quaternions
 * @param probs the corresponding probabilities
 * @return The weighted mean of quats
 */
SO3d weightedMean(std::vector<SO3d> &quats, std::vector<double> &probs)
{
    SO3d sum = quats[0];
    SO3d old_sum = sum;
    decltype(sum - old_sum) diff_sum = diff_sum.Zero();
    decltype(sum - old_sum) selector_weights = selector_weights.Zero();

    int iterations = 0;
    do
    {
        iterations++;
        old_sum = sum;
        diff_sum = diff_sum.Zero();
        for (int i = 0; i < quats.size(); i++)
        {
            diff_sum = diff_sum + probs[i] * (quats[i] - sum); //.cwiseProduct(selectors[filter_bank[i].second]);
        }
        sum = sum + diff_sum; //.cwiseQuotient(selector_weights);
    } while (iterations <= max_iterations && (sum - old_sum).norm() > weighted_mean_epsilon);
    if (iterations > max_iterations)
        printf("Warning: stopped due to excess of iterations");
    return sum;
}

/**
 * Calculate the weighted covariance of quaternion distributions
 * @param quats The list of mean points of the distributions
 * @param covs The covariances of the distributions
 * @param probs The weights of the distributions
 * @param target The weighted mean of the means
 * @return The weighted covariance sum
 */
Eigen::Matrix3d weightedCovarianceSum(std::vector<SO3d> &quats, std::vector<Eigen::Matrix3d> &covs, std::vector<double> &probs, const SO3d &target)
{
    Eigen::Matrix3d sum = Eigen::Matrix3d::Zero();
    Eigen::Matrix3d weights = Eigen::Matrix3d::Zero();
    for (size_t i = 0; i < quats.size(); i++)
    {
        if (probs[i] > 0.)
        {
            auto diff = (quats[i] - target).eval();
            auto plus_diff = eval((quats[i] + getDerivator<3>()) - target);
            Eigen::Matrix<double, 3, 3> D(3, 3);
            //Initialise the Jacobian
            for (size_t j = 0; j < 3; ++j)
                D.col(j) = plus_diff[j].v; //write to cols since col major (transposes matrix )
            //Covariance select = selectors[filter_bank[i].second] * selectors[filter_bank[i].second].transpose() * probabilities(i);
            sum += probs[i] * (D.transpose() * (covs[i]) * D + diff * diff.transpose());
            //weights += select;
        }
    }
    assert(sum.determinant() > 0.);
    return sum;
}

/**
 * Calculates the weighted mean of quaternions with naive/improper mixing (in parameter space)
 * @param quats The quaternions to mix
 * @param probs The weights
 * @return Weighted mean of quats
 */
SO3d badWeightedMean(std::vector<SO3d> &quats, std::vector<double> &probs)
{
    Eigen::Vector4d sum = sum.Zero();
    for (int i = 0; i < quats.size(); i++)
    {
        sum += quats[i].coeffs() * probs[i];
    }
    sum = sum.normalized();
    return SO3d(sum.data());
}
/**
 * Calculates a numeric gold standard Variance of quats.
 *
 * Uses a list of uniformly sampled quaternions from the distributions and combines them with the proper mixing
 *
 * @param quats The list of uniformly sampled quaternions
 * @param probs The probabilities of the quaternioms
 * @param target The weighted mean of means of the distributions
 * @return The weighted covariance sum
 */
Eigen::Matrix3d goldWeightedCovarianceSum(std::vector<SO3d> &quats, std::vector<double> &probs, const SO3d &target)
{

    Eigen::Matrix3d sum = Eigen::Matrix3d::Zero();
    for (size_t i = 0; i < quats.size(); i++)
    {
        if (probs[i] > 0.)
        {
            auto diff = (quats[i] - target).eval();
            sum += probs[i] * (diff * diff.transpose());
        }
    }
    return sum;
}

/**
 * Improper way of the weighted sum of covariances.
 *
 * Uses the naive approach from the original IMM
 * @param quats The list of quaternion means
 * @param covs The covariances
 * @param probs The weights of the quaternions
 * @param target The weighted mean of means
 * @return The naive weighted covariance sum
 */
Eigen::Matrix3d badWeightedCovarianceSum(std::vector<SO3d> &quats, std::vector<Eigen::Matrix3d> &covs, std::vector<double> &probs, const SO3d &target)
{

    Eigen::Matrix3d sum = Eigen::Matrix3d::Zero();
    for (size_t i = 0; i < quats.size(); i++)
    {
        if (probs[i] > 0.)
        {
            auto diff = (quats[i] - target).eval();
            sum += probs[i] * (covs[i] + diff * diff.transpose());
        }
    }
    return sum;
}

/**
 * Calculates the difference between the bp-weighted mean and the naive weighted mean
 * @param quats List of quaternions
 * @param probs weights
 * @return BPMean-NaiveMean
 */
double diff(std::vector<SO3d> &quats, std::vector<double> &probs)
{
    return (weightedMean(quats, probs) - badWeightedMean(quats, probs)).norm();
}

/**
 * Plots the naive mixing vs the boxplus mixing
 * @param argc command line argument counter
 * @param argv command line arguments
 */
void plot_bad_vs_bp()
{
    std::vector<SO3d> quats;
    std::vector<std::vector<double>> probs;
    std::vector<Eigen::Matrix3d> covs;
    covs.push_back(Eigen::Matrix3d::Identity() * 0.01);
    covs.push_back(Eigen::Matrix3d::Identity() * 0.02);

    quats.push_back(SO3d(Eigen::Vector3d(0, 0, 0)));
    quats.push_back(SO3d(Eigen::Vector3d(0, 0, 0.0)));
    std::vector<double> trial{0.7, 0.3};
    std::cout << weightedMean(quats, trial) << std::endl;
    //return 0;
    //list of probability pairs
    for (double prob = 0.5; prob <= 1.; prob += 0.05)
    {
        probs.push_back(std::vector<double>(2));
        probs.back()[0] = prob;
        probs.back()[1] = 1. - prob;
    }
    //compute for different sigmas
    for (double sigma = 0.0; sigma < 3.; sigma += 0.01)
    {
        quats[1] = SO3d{Eigen::Vector3d(0, 0, sigma)};
        Eigen::Matrix<double, -1, 1> diffs = diffs.Zero(probs.size());
        //compute mean diff for different probabilities
        for (int i = 0; i < probs.size(); i++)
        {
            diffs(i) = diff(quats, probs[i]);
        }
        std::cout << "Sigma: " << sigma << " Max diff: " << diffs.maxCoeff() << " Diffs: " << diffs.transpose() << std::endl;
        adekf::viz::plotVector(diffs, "Mean diff", 300, "abcdefghiklmn");
        //compute covariance diff for different probabilities
        for (int i = 0; i < probs.size(); i++)
        {
            diffs(i) = (weightedCovarianceSum(quats, covs, probs[i], weightedMean(quats, probs[i])) - badWeightedCovarianceSum(quats, covs, probs[i], badWeightedMean(quats, probs[i]))).norm();
        }
        std::cout << "Sigma: " << sigma << " Max diff: " << diffs.maxCoeff() << " Diffs: " << diffs.transpose() << std::endl;
        adekf::viz::plotVector(diffs, "Sigma diff", 300, "abcdefghiklmn");
    }
}

/**
 * Calculates the likelihood of a vector given the covariance of the distribution
 * @param delta the vector to check the likelihood of
 * @param sigma The covariance
 * @return N(delta; 0, sigma)
 */
double likelihood(const Eigen::Vector3d &delta, const Eigen::Matrix3d &sigma)
{
    return exp(-0.5 * (delta).transpose() * sigma.inverse() * delta) / sqrt(sigma.determinant() * pow((2 * M_PI), sigma.rows()));
}

/**
 * Plots a numeric gold standard mixing against the naive mixing and the bp-mixing
 * @param argc command line argument counter
 * @param argv command line arguments
 */
void plot_real_gold_vs_()
{
    std::vector<SO3d> quats(2);
    std::vector<double> probs{0.7, 0.3};

    std::vector<Eigen::Matrix3d> covs;
    covs.push_back(Eigen::Matrix3d::Identity() * 0.01);
    covs.push_back(Eigen::Matrix3d::Identity() * 0.02);
    //Compute for different sigmas
    for (double sigma = 0.0; sigma < 3.; sigma += 0.05)
    {
        quats[0] = (SO3d(Eigen::Vector3d(0, 0, 0)));
        quats[1] = (SO3d(Eigen::Vector3d(0, 0, sigma)));
        //return 0;

        const double max = M_PI / 2;
        const double step = 0.02;
        const int steps = max / step;
        std::vector<SO3d> gold_quats(pow(steps, 3));
        std::vector<double> gold_probs(pow(steps, 3));
        //uniform sample of quaternions from distributions
        for (int l = 0; l < probs.size(); ++l)
        {

            for (int i = -steps; i < steps; i++)
            {
                for (int j = -steps; j < steps; j++)
                {
                    for (int k = -steps; k < steps; ++k)
                    {
                        Eigen::Vector3d delta = step * Eigen::Vector3d(i, j, k);
                        gold_quats.push_back(quats[l] + delta);
                        gold_probs.push_back(likelihood(delta, covs[l]) * probs[l]);
                    }
                }
            }
        }
        double sum = 0;
        //normalise probabilities of sampled quats
        for (const auto &prob : gold_probs)
        {
            sum += prob;
        }
        for (auto &prob : gold_probs)
        {
            prob /= sum;
        }
        sum = 0;
        //compute means
        auto mean = weightedMean(quats, probs);
        auto badMean = badWeightedMean(quats, probs);
        auto goldMean = weightedMean(gold_quats, gold_probs);
        Eigen::Matrix<double, 2, 1> diffs = diffs.Zero();
        diffs(0) = (mean - goldMean).norm();
        diffs(1) = (badMean - goldMean).norm();
        adekf::viz::plotVector(diffs, "Gold Mean diff", 305, "mb");
        auto goldCov = goldWeightedCovarianceSum(gold_quats, gold_probs, goldMean);
        diffs(0) = (weightedCovarianceSum(quats, covs, probs, mean) - goldCov).norm();
        diffs(1) = (badWeightedCovarianceSum(quats, covs, probs, badMean) - goldCov).norm();
        adekf::viz::plotVector(diffs, "Gold Sigma diff", 305, "mb");
    }
}

Eigen::Vector3d weightedRefMean(aligned_vector<SO3d> &quats, std::vector<double> &probs, SO3d ref)
{
    Eigen::Vector3d sum = sum.Zero();

    for (int i = 0; i < quats.size(); i++)
    {
        sum = sum + probs[i] * (quats[i] - ref);
    }
    return sum;
}

/**
 * Calculates a numeric gold standard Variance of quats.
 *
 * Uses a list of uniformly sampled quaternions from the distributions and combines them with the proper mixing
 *
 * @param quats The list of uniformly sampled quaternions
 * @param probs The probabilities of the quaternioms
 * @param target The weighted mean of means of the distributions
 * @return The weighted covariance sum
 */
Eigen::Matrix3d goldWeightedCovarianceSumRef(aligned_vector<SO3d> &quats, std::vector<double> &probs, const SO3d &ref)
{

    Eigen::Matrix3d sum = Eigen::Matrix3d::Zero();
    Eigen::Vector3d mu_ref = weightedRefMean(quats, probs, ref);
    for (size_t i = 0; i < quats.size(); i++)
    {
        if (probs[i] > 0.)
        {
            auto diff = ((quats[i] - ref) - mu_ref).eval();
            sum += probs[i] * (diff * diff.transpose());
        }
    }
    return sum;
}

/**
 * Plots a numeric gold standard mixing against the naive mixing and the bp-mixing
 * @param argc command line argument counter
 * @param argv command line arguments
 */
void plot_covariance_centered()
{
    SO3d r1, r2; //r1 r2

    Eigen::Matrix3d cov = cov.Identity() * 0.1;
    //Compute for different sigmas
    for (double sigma = 0.0; sigma < 3.; sigma += 0.05)
    {
        r1 = (SO3d(Eigen::Vector3d(0, 0, 0)));
        r2 = (SO3d(Eigen::Vector3d(0, 0, sigma)));
        Eigen::Vector3d diff = r2 - r1;
        const double max = M_PI / 2;
        const double step = 0.02;
        const int steps = max / step;
        aligned_vector<SO3d> gold_quats(pow(steps, 3));
        std::vector<double> gold_probs(pow(steps, 3));
        //uniform sample of quaternions from distribution

        for (int i = -steps; i < steps; i++)
        {
            for (int j = -steps; j < steps; j++)
            {
                for (int k = -steps; k < steps; ++k)
                {
                    Eigen::Vector3d delta = step * Eigen::Vector3d(i, j, k);
                    gold_quats.push_back(r1 + (diff + delta));
                    gold_probs.push_back(likelihood(delta, cov));
                }
            }
        }
        double sum = 0;
        //normalise probabilities of sampled quats
        for (const auto &prob : gold_probs)
        {
            sum += prob;
        }
        for (auto &prob : gold_probs)
        {
            prob /= sum;
        }
        sum = 0;
        //compute means

        Eigen::Matrix<double, 2, 1> diffs = diffs.Zero();
        auto goldCov = goldWeightedCovarianceSumRef(gold_quats, gold_probs, r2);
        auto transform = transformReferenceJacobian(r1, r2, diff);
        auto linearCov = transform * cov * transform.transpose();
        diffs(0)=(linearCov-goldCov).norm();
        diffs(1)=(cov-goldCov).norm();
        //diffs(0) = goldCov.determinant();
        adekf::viz::plotVector(diffs, "Covariance transform centered", 61, "mb");
    }
}

void plot_covariance_displaced()
{
    SO3d r1, r2; //r1 r2

    Eigen::Matrix3d cov = cov.Identity() * 0.1;
    //Compute for different sigmas
    for (double sigma = 0.0; sigma < 3.; sigma += 0.05)
    {
        r1 = (SO3d(Eigen::Vector3d(0, 0, 0)));
        r2 = (SO3d(Eigen::Vector3d(0, 0, sigma)));
        Eigen::Vector3d diff = r2 - r1;
        const double max = M_PI / 2;
        const double step = 0.02;
        const int steps = max / step;
        aligned_vector<SO3d> gold_quats(pow(steps, 3));
        std::vector<double> gold_probs(pow(steps, 3));
        //uniform sample of quaternions from distribution

        for (int i = -steps; i < steps; i++)
        {
            for (int j = -steps; j < steps; j++)
            {
                for (int k = -steps; k < steps; ++k)
                {
                    Eigen::Vector3d delta = step * Eigen::Vector3d(i, j, k);
                    gold_quats.push_back(r1 + (delta));
                    gold_probs.push_back(likelihood(delta, cov));
                }
            }
        }
        double sum = 0;
        //normalise probabilities of sampled quats
        for (const auto &prob : gold_probs)
        {
            sum += prob;
        }
        for (auto &prob : gold_probs)
        {
            prob /= sum;
        }
        sum = 0;
        //compute means

        Eigen::Matrix<double, 2, 1> diffs = diffs.Zero();
        auto goldCov = goldWeightedCovarianceSumRef(gold_quats, gold_probs, r2);
        auto transform = transformReferenceJacobian(r1, r2);
        auto linearCov = transform * cov * transform.transpose();
        diffs(0)=(linearCov-goldCov).norm();
        diffs(1)=(cov-goldCov).norm();
        //diffs(0) = goldCov.determinant();
        adekf::viz::plotVector(diffs, "Covariance transform displaced", 61, "mb");
    }
}

void plot_covariance_gold()
{
    SO3d r1, r2; //r1 r2

    Eigen::Matrix3d cov = cov.Identity() * 0.1;
    //Compute for different sigmas
    for (double sigma = 0.0; sigma < 3.; sigma += 0.05)
    {
        r1 = (SO3d(Eigen::Vector3d(0, 0, 0)));
        r2 = (SO3d(Eigen::Vector3d(0, 0, sigma)));
        Eigen::Vector3d diff = r2 - r1;
        const double max = M_PI / 2;
        const double step = 0.02;
        const int steps = max / step;
        aligned_vector<SO3d> gold_quats_centered(pow(steps, 3));

        aligned_vector<SO3d> gold_quats_displaced(pow(steps, 3));

        std::vector<double> gold_probs(pow(steps, 3));

        //uniform sample of quaternions from distribution

        for (int i = -steps; i < steps; i++)
        {
            for (int j = -steps; j < steps; j++)
            {
                for (int k = -steps; k < steps; ++k)
                {
                    Eigen::Vector3d delta = step * Eigen::Vector3d(i, j, k);
                    gold_quats_displaced.push_back(r1 + (delta));
                    gold_quats_centered.push_back(r1+(diff+delta));
                    gold_probs.push_back(likelihood(delta, cov));
                }
            }
        }
        double sum = 0;
        //normalise probabilities of sampled quats
        for (const auto &prob : gold_probs)
        {
            sum += prob;
        }
        for (auto &prob : gold_probs)
        {
            prob /= sum;
        }
        sum = 0;
        //compute means

        Eigen::Matrix<double, 3, 1> diffs = diffs.Zero();
        auto goldCovDisplaced = goldWeightedCovarianceSumRef(gold_quats_displaced, gold_probs, r2);
        auto goldCovCentered=goldWeightedCovarianceSumRef(gold_quats_centered,gold_probs,r2);
        diffs(0)=goldCovDisplaced.determinant()/cov.determinant();
        diffs(1)=goldCovCentered.determinant()/cov.determinant();
        diffs(2)=diffs(0)*diffs(1);

        //diffs(0) = goldCov.determinant();
        adekf::viz::plotVector(diffs, "Covariance transform gold", 61, "DCM");
        auto transform = transformReferenceJacobian(r1, r2);
        auto linearCov = (transform * cov * transform.transpose()).eval();
        diffs(0)=linearCov.determinant()/cov.determinant();
        transform=transformReferenceJacobian(r1,r2,diff);
        linearCov = transform * cov * transform.transpose();
        diffs(1)=linearCov.determinant()/cov.determinant();
        diffs(2)=diffs(0)*diffs(1);
        adekf::viz::plotVector(diffs, "Covariance transform linear", 61, "DCM");
    }
}

std::vector<std::thread> threads;
auto parallel=[](auto function){ threads.emplace_back(function);};
int main(int argc, char *argv[])
{
    viz::initGuis(argc, argv);

    //Choose some to plot
    //parallel(plot_bad_vs_bp);
    //parallel(plot_real_gold_vs_);//takes really long ~10 min
    //parallel(plot_covariance_displaced);
    //parallel(plot_covariance_centered);
    parallel(plot_covariance_gold);
    viz::runGuis();
    for(auto & thread: threads) thread.join();
    return 0;
}


/*
 * euclidean.hpp
 *
 *  Created on: Aug 16, 2009
 *      Author: stewie
 */

#ifndef EUCLIDEAN_HPP_
#define EUCLIDEAN_HPP_

#include <cmath>
#include <functional> // for std::plus
#include <numeric> // for std::inner_product

#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>

/**
 * @ingroup lab
 *
 * @file euclidean.hpp
 *
 * This file contains routines for computing various kinds of distances between
 * attractors and other objects.
 */

namespace bn {

namespace detail {

/**
 * @private
 * Helper class to compute a squared difference.
 */
struct SquareDifference {
	/**
	 * Result type of this function object.
	 */
	typedef double result_type;

	/**
	 * Computes a squared difference.
	 * @param x an object
	 * @param y another object
	 * @return the squared difference
	 */
	template<class T> result_type operator()(const T& x, const T& y) const {
		return std::pow(x - y, 2);
	}
};

}

/**
 * @ingroup lab
 *
 * Computes the euclidean distance between two ranges.
 *
 * The second range shall have at least as many elements as the first.
 * @param first1 input iterator to the initial position of the first range
 * @param last1 input iterator to the final position of the first range
 * @param first2 input iterator to the initial position of the second range
 * @return the euclidean distance
 */
template<class InputIterator> double euclidean_distance(
		const InputIterator first1, const InputIterator last1,
		const InputIterator first2) {
	return std::sqrt(std::inner_product(first1, last1, first2, 0.0, std::plus<
			double>(), detail::SquareDifference()));
}

/**
 * @ingroup lab
 *
 * Computes the euclidean distance between two ranges.
 *
 * The second range shall have at least as many elements as the first. This
 * overload uses concepts from
 * The Boost Range Library. Practically it is syntactic sugar which replaces:
 * @code
 * vector<double> v1 = ...
 * vector<double> v2 = ...
 * euclidean_distance(v1.begin(), v1.end(), v2.begin());
 * @endcode
 * with the tighter:
 * @code
 * vector<double> v1 = ...
 * vector<double> v2 = ...
 * euclidean_distance(v1, v2);
 * @endcode
 * @param x first input range
 * @param y second input range
 * @return the euclidean distance
 */
template<class SinglePassRange> double euclidean_distance(
		const SinglePassRange& x, const SinglePassRange& y) {
	return euclidean_distance(boost::begin(x), boost::end(x), boost::begin(y));
}

/**
 * @ingroup lab
 *
 * A function object to compute the euclidean distance between objects.
 *
 * This is particularly useful in STL algorithms since it is cumbersome (or
 * simply impossible) to pass template function pointers as arguments.
 *
 * For example:
 * @code
 * vector<AttractorExpression> expr = ...
 * util::Matrix<double> m = make_distance_matrix<double>(expr.begin(),
 * 	expr.end(), &euclidean_distance);
 * @endcode
 * is wrong because the compiler cannot determine which overload to choose; also
 * that function is a template so you have to explicitly instantiate its
 * parameters to take a pointer. On the contrary, this is simpler and correct:
 * @code
 * vector<AttractorExpression> expr = ...
 * util::Matrix<double> m = make_distance_matrix<double>(expr.begin(),
 * 	expr.end(), Euclidean());
 * @endcode
 */
struct Euclidean {
	/**
	 * Result type of this function object.
	 */
	typedef double result_type;

	/**
	 * Computes the euclidean distance between two vector of the same length.
	 * @param x the first vector
	 * @param y the second vector
	 * @return the euclidean distance
	 */
	template<class T> result_type operator()(const T& x, const T& y) const {
		return euclidean_distance(x, y);
	}
};

}

#endif /* EUCLIDEAN_HPP_ */


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/* EncryptedArray.cpp - Data-movement operations on arrays of slots
 */
#include <NTL/ZZ.h>
NTL_CLIENT
#include "EncryptedArray.h"
#include "polyEval.h"

static void buildDigitPolynomial(ZZX& result, long p, long e);

// extractDigits assumes that the slots of *this contains integers mod p^r
// i.e., that only the free terms are nonzero. (If that assumptions does
// not hold then the result will not be a valid ciphertext anymore.)
//
// It returns in the slots of digits[j] the j'th-lowest gigits from the
// integers in the slots of the input. Namely, the i'th slot of digits[j]
// contains the j'th digit in the p-base expansion of the integer in the
// i'th slot of the *this.
//
// If the shortCut flag is set then digits[j] contains the j'th digits wrt
// mod-p plaintext space and the highest possible level (for all j). Otherwise
// digits[j] still contains the j'th digit in the base-p expansion, but wrt
// mod-p^{r-j} plaintext space, and all the ciphertexts are at the same level.
void extractDigits(vector<Ctxt>& digits, const Ctxt& c, long r, bool shortCut)
{
  FHEcontext& context = (FHEcontext&) c.getContext();
  long rr = c.effectiveR();
  if (r<=0 || r>rr) r = rr; // how many digits to extract

  long p = context.zMStar.getP();
  ZZX x2p;
  if (p>3) {
    buildDigitPolynomial(x2p, p, r);
  }

  Ctxt tmp(c.getPubKey(), c.getPtxtSpace());
  digits.resize(r, tmp);      // allocate space
  vector<Ctxt> w(r, tmp);
  for (long i=0; i<r; i++) {
    tmp = c;
    for (long j=0; j<i; j++) {
      FHE_NTIMER_START(square);
      if (p==2) w[j].square();
      else if (p==3) w[j].cube();
      else polyEval(w[j], x2p, w[j]); // "in spirit" w[j] = w[j]^p
      FHE_NTIMER_STOP(square);
      tmp -= w[j];
      tmp.divideByP();
    }
    w[i] = tmp; // needed in the next round
    if (shortCut) {
      digits[i] = tmp; // digits[i]=i'th lowest digit
      digits[i].reducePtxtSpace(p);
    }
  }
  // If not shortCut, copy w into digits
  if (!shortCut) for (long i=0; i<r; i++) digits[i] = w[i];
}

// Compute a degree-p polynomial poly(x) s.t. for any t<e and integr z of the
// form z = z0 + p^t*z1 (with 0<=z0<p), we have poly(z) = z0 (mod p^{t+1}).
//
// We get poly(x) by interpolating a degree-(p-1) polynomial poly'(x)
// s.t. poly'(z0)=z0 - z0^p (mod p^e) for all 0<=z0<p, and then setting
// poly(x) = x^p + poly'(x).
static void buildDigitPolynomial(ZZX& result, long p, long e)
{
  if (p<2 || e<=1) return; // nothing to do
  FHE_TIMER_START;
  long p2e = power_long(p,e); // the integer p^e

  // Compute x - x^p (mod p^e), for x=0,1,...,p-1
  vec_long x(INIT_SIZE, p);
  vec_long y(INIT_SIZE, p);
  long bottom = -(p/2);
  for (long j=0; j<p; j++) {
    long z = bottom+j;
    x[j] = z;
    y[j] = z-PowerMod((z < 0 ? z + p2e : z), p, p2e);  // x - x^p (mod p^e)

    while (y[j] > p2e/2)         y[j] -= p2e;
    while (y[j] < -(p2e/2))      y[j] += p2e;
  }
  interpolateMod(result, x, y, p, e);
  assert(deg(result)<p); // interpolating p points, should get deg<=p-1
  SetCoeff(result, p);   // return result = x^p + poly'(x)
  //  cerr << "# digitExt mod "<<p<<"^"<<e<<"="<<result<<endl;
  FHE_TIMER_STOP;
}


/**
 * \file Chebyshev2Filter.hxx
 */

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

#include <ATK/EQ/Chebyshev2Filter.h>
#include <ATK/EQ/helpers.h>

namespace Chebyshev2Utilities
{
  template<typename DataType>
  void create_chebyshev2_analog_coefficients(int order, DataType ripple, EQUtilities::ZPK<DataType>& zpk)
  {
    zpk.z.clear(); // no zeros for this filter type
    zpk.p.clear();

    DataType de = static_cast<DataType>(1.0 / std::sqrt(std::pow(10, (0.1 * ripple)) - 1));
    DataType mu = static_cast<DataType>(boost::math::asinh(1.0 / de) / order);

    for(gsl::index i = -order+1; i < order; i += 2)
    {
      std::complex<DataType> p1 = -std::complex<DataType>(std::cos(i * boost::math::constants::pi<DataType>() / (2 * order)), std::sin(i * boost::math::constants::pi<DataType>() / (2 * order)));
      std::complex<DataType> p2 = std::complex<DataType>(std::sinh(mu) * p1.real(), std::cosh(mu) * p1.imag());

      zpk.p.push_back(std::complex<DataType>(1, 0) / p2);

      if(i == 0)
      {
        continue;
      }

      zpk.z.push_back(std::complex<DataType>(0, 1 / std::sin(i * boost::math::constants::pi<DataType>() / (2 * order))));
    }

    std::complex<DataType> f = 1;
    for(gsl::index i = 0; i < zpk.p.size(); ++i)
    {
      f *= - zpk.p[i];
    }
    for(gsl::index i = 0; i < zpk.z.size(); ++i)
    {
      f /= - zpk.z[i];
    }
    zpk.k = f.real();
  }

  template<typename DataType, typename Container>
  void create_default_chebyshev2_coeffs(size_t order, DataType ripple, DataType Wn, Container& coefficients_in, Container& coefficients_out)
  {
    EQUtilities::ZPK<DataType> zpk;

    int fs = 2;
    create_chebyshev2_analog_coefficients(static_cast<int>(order), ripple, zpk);
    EQUtilities::populate_lp_coeffs(Wn, fs, order, zpk, coefficients_in, coefficients_out);
  }

  template<typename DataType, typename Container>
  void create_bp_chebyshev2_coeffs(size_t order, DataType ripple, DataType wc1, DataType wc2, Container& coefficients_in, Container& coefficients_out)
  {
    EQUtilities::ZPK<DataType> zpk;

    int fs = 2;
    create_chebyshev2_analog_coefficients(static_cast<int>(order/2), ripple, zpk);
    EQUtilities::populate_bp_coeffs(wc1, wc2, fs, order, zpk, coefficients_in, coefficients_out);
  }

  template<typename DataType, typename Container>
  void create_bs_chebyshev2_coeffs(size_t order, DataType ripple, DataType wc1, DataType wc2, Container& coefficients_in, Container& coefficients_out)
  {
    EQUtilities::ZPK<DataType> zpk;

    int fs = 2;
    create_chebyshev2_analog_coefficients(static_cast<int>(order/2), ripple, zpk);
    EQUtilities::populate_bs_coeffs(wc1, wc2, fs, order, zpk, coefficients_in, coefficients_out);
  }
}

namespace ATK
{
  template <typename DataType>
  Chebyshev2LowPassCoefficients<DataType>::Chebyshev2LowPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void Chebyshev2LowPassCoefficients<DataType_>::set_ripple(CoeffDataType ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev2LowPassCoefficients<DataType_>::CoeffDataType Chebyshev2LowPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType_>
  void Chebyshev2LowPassCoefficients<DataType_>::set_cut_frequency(CoeffDataType cut_frequency)
  {
    if(cut_frequency <= 0)
    {
      throw std::out_of_range("Frequency can't be negative");
    }
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev2LowPassCoefficients<DataType_>::CoeffDataType Chebyshev2LowPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType>
  void Chebyshev2LowPassCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  unsigned int Chebyshev2LowPassCoefficients<DataType>::get_order() const
  {
    return in_order;
  }

  template <typename DataType>
  void Chebyshev2LowPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    Chebyshev2Utilities::create_default_chebyshev2_coeffs(in_order, ripple, 2 * cut_frequency / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  Chebyshev2HighPassCoefficients<DataType>::Chebyshev2HighPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void Chebyshev2HighPassCoefficients<DataType_>::set_cut_frequency(CoeffDataType cut_frequency)
  {
    if(cut_frequency <= 0)
    {
      throw std::out_of_range("Frequency can't be negative");
    }
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev2HighPassCoefficients<DataType_>::CoeffDataType Chebyshev2HighPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType_>
  void Chebyshev2HighPassCoefficients<DataType_>::set_ripple(CoeffDataType ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev2HighPassCoefficients<DataType_>::CoeffDataType Chebyshev2HighPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev2HighPassCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  unsigned int Chebyshev2HighPassCoefficients<DataType>::get_order() const
  {
    return in_order;
  }

  template <typename DataType>
  void Chebyshev2HighPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    Chebyshev2Utilities::create_default_chebyshev2_coeffs(in_order, ripple, (input_sampling_rate - 2 * cut_frequency) / input_sampling_rate, coefficients_in, coefficients_out);
    for(gsl::index i = in_order - 1; i >= 0; i -= 2)
    {
      coefficients_in[i] = - coefficients_in[i];
      coefficients_out[i] = - coefficients_out[i];
    }
  }

  template <typename DataType>
  Chebyshev2BandPassCoefficients<DataType>::Chebyshev2BandPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void Chebyshev2BandPassCoefficients<DataType_>::set_cut_frequencies(std::pair<CoeffDataType, CoeffDataType> cut_frequencies)
  {
    if(cut_frequencies.first <= 0 || cut_frequencies.second <= 0)
    {
      throw std::out_of_range("Frequencies can't be negative");
    }
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void Chebyshev2BandPassCoefficients<DataType_>::set_cut_frequencies(CoeffDataType f0, CoeffDataType f1)
  {
    set_cut_frequencies(std::make_pair(f0, f1));
  }

  template <typename DataType_>
  std::pair<typename Chebyshev2BandPassCoefficients<DataType_>::CoeffDataType, typename Chebyshev2BandPassCoefficients<DataType_>::CoeffDataType> Chebyshev2BandPassCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType_>
  void Chebyshev2BandPassCoefficients<DataType_>::set_ripple(CoeffDataType ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev2BandPassCoefficients<DataType_>::CoeffDataType Chebyshev2BandPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev2BandPassCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  unsigned int Chebyshev2BandPassCoefficients<DataType>::get_order() const
  {
    return in_order / 2;
  }

  template <typename DataType>
  void Chebyshev2BandPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    Chebyshev2Utilities::create_bp_chebyshev2_coeffs(in_order, ripple, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  Chebyshev2BandStopCoefficients<DataType>::Chebyshev2BandStopCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void Chebyshev2BandStopCoefficients<DataType_>::set_cut_frequencies(std::pair<CoeffDataType, CoeffDataType> cut_frequencies)
  {
    if(cut_frequencies.first <= 0 || cut_frequencies.second <= 0)
    {
      throw std::out_of_range("Frequencies can't be negative");
    }
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void Chebyshev2BandStopCoefficients<DataType_>::set_cut_frequencies(CoeffDataType f0, CoeffDataType f1)
  {
    set_cut_frequencies(std::make_pair(f0, f1));
  }

  template <typename DataType_>
  std::pair<typename Chebyshev2BandStopCoefficients<DataType_>::CoeffDataType, typename Chebyshev2BandStopCoefficients<DataType_>::CoeffDataType> Chebyshev2BandStopCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType_>
  void Chebyshev2BandStopCoefficients<DataType_>::set_ripple(CoeffDataType ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev2BandStopCoefficients<DataType_>::CoeffDataType Chebyshev2BandStopCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev2BandStopCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  unsigned int Chebyshev2BandStopCoefficients<DataType>::get_order() const
  {
    return in_order / 2;
  }

  template <typename DataType>
  void Chebyshev2BandStopCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    Chebyshev2Utilities::create_bs_chebyshev2_coeffs(in_order, ripple, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }
}


#ifndef THIN_PLATE_SPLINE_HPP
#define THIN_PLATE_SPLINE_HPP

#include <Eigen/Dense>
#include <Eigen/QR>
#include "arc_utilities/arc_exceptions.hpp"
#include "arc_utilities/eigen_helpers.hpp"

namespace arc_utilities {
// Note that DIMENSIONS must be 2 or 3, not something like `Eigen::Dynamic`
template <int DIMENSIONS = 3>
class ThinPlateSpline {
 public:
  typedef Eigen::Matrix<double, DIMENSIONS, Eigen::Dynamic> PointSet;
  typedef Eigen::Matrix<double, DIMENSIONS + 1, Eigen::Dynamic> HomogeneousPointSet;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  static_assert(DIMENSIONS > 1, "Using TPS only makes sense for 2 or more dimensions");
  static_assert(DIMENSIONS <= 3,
                "The choice of kernel function for DIMENSIONS >= 4 is not clear."
                " See note after equation (29) in "
                "https://www.geometrictools.com/Documentation/ThinPlateSplines.pdf");

  ThinPlateSpline() : solved_(false) {}

  /**
   * @brief ThinPlateSpline::ThinPlateSpline An arbitrary dimensional thin
   *  plate spline for interpolation between points.
   * @param template_points A (DIMINESION x NUMPOINTS) matrix which define
   *  the starting points that control the warping. I.e. the template is
   *  warped to the target
   * @param target_points A (DIMINESION x NUMPOINTS) matrix which define
   *  the target points that control the warping. I.e. the template is
   *  warped to the target
   */
  template <typename DerivedA, typename DerivedB>
  ThinPlateSpline(const Eigen::MatrixBase<DerivedA>& template_points, const Eigen::MatrixBase<DerivedB>& target_points)
      : solved_(false) {
    setTemplatePoints(template_points);
    setTargetPoints(target_points);
    solveForCoeffs();
  }

  /**
   * @note Checking for the vailidty of the input data is put off until
   *  the warping function is solved
   */
  template <typename Derived>
  void setTemplatePoints(const Eigen::MatrixBase<Derived>& template_points) {
    using namespace Eigen;
    static_assert(
        MatrixBase<Derived>::RowsAtCompileTime == DIMENSIONS || MatrixBase<Derived>::RowsAtCompileTime == Dynamic,
        "INVALID DATA DIMENSIONS FOR template_points");

    homogeneous_template_points_.resize(DIMENSIONS + 1, template_points.cols());
    homogeneous_template_points_ << template_points, RowVectorXd::Ones(template_points.cols());
    solved_ = false;
  }

  /**
   * @note Checking for the vailidty of the input data is put off until
   *  the warping function is solved
   */
  template <typename Derived>
  void setTargetPoints(const Eigen::MatrixBase<Derived>& target_points) {
    using namespace Eigen;
    static_assert(
        MatrixBase<Derived>::RowsAtCompileTime == DIMENSIONS || MatrixBase<Derived>::RowsAtCompileTime == Dynamic,
        "INVALID DATA DIMENSIONS FOR target_points");

    target_points_ = target_points;
    solved_ = false;
  }

  void solveForCoeffs() {
    using namespace Eigen;
    if (homogeneous_template_points_.cols() < 1 || target_points_.cols() < 1) {
      throw_arc_exception(std::invalid_argument, "template and target points must have valid data before solving");
    }
    if (homogeneous_template_points_.cols() != target_points_.cols()) {
      throw_arc_exception(std::invalid_argument, "template and target points must have the same number of points");
    }

    const auto NUMPOINTS = homogeneous_template_points_.cols();

    // Calculate the kernel matrix for the template (control) points
    const MatrixXd Rsquared = EigenHelpers::CalculateSquaredDistanceMatrix(homogeneous_template_points_);
    const MatrixXd kernel = Rsquared.unaryExpr(&ThinPlateSpline::KernelFunction);

    // Form the system:   A * params = B
    //       A = [K,       template'
    //            template,    0]
    //       B = [target'
    //              0    ]
    MatrixXd A(NUMPOINTS + DIMENSIONS + 1, NUMPOINTS + DIMENSIONS + 1);
    A << kernel, homogeneous_template_points_.transpose(), homogeneous_template_points_,
        MatrixXd::Zero(DIMENSIONS + 1, DIMENSIONS + 1);
    MatrixXd B(NUMPOINTS + DIMENSIONS + 1, DIMENSIONS);
    B << target_points_.transpose(), MatrixXd::Zero(DIMENSIONS + 1, DIMENSIONS);

    // params = A \ B
    const Matrix<double, Dynamic, DIMENSIONS> params = A.colPivHouseholderQr().solve(B).eval();
    warping_coeffs_ = params.topRows(NUMPOINTS).transpose();
    affine_coeffs_ = params.bottomRows(DIMENSIONS + 1).transpose();
    solved_ = true;
  }

  template <typename Derived>
  Eigen::Matrix<double, DIMENSIONS, Eigen::Dynamic> interpolate(
      const Eigen::MatrixBase<Derived>& interpolation_points) {
    using namespace Eigen;
    static_assert(
        MatrixBase<Derived>::RowsAtCompileTime == DIMENSIONS || MatrixBase<Derived>::RowsAtCompileTime == Dynamic,
        "INVALID DATA DIMENSIONS FOR interpolation_points");

    if (!solved_) {
      solveForCoeffs();
    }

    const auto num_points = interpolation_points.cols();
    HomogeneousPointSet homogeneous_interpolation_points(DIMENSIONS + 1, num_points);
    homogeneous_interpolation_points << interpolation_points, RowVectorXd::Ones(num_points);

    // Calculate the kenerl matrix comparing the new points to the template (control) points
    const MatrixXd Rsquared =
        EigenHelpers::SquaredDistancesBetweenPointSets(homogeneous_template_points_, homogeneous_interpolation_points);
    const MatrixXd kernel = Rsquared.unaryExpr(&ThinPlateSpline::KernelFunction);

    return affine_coeffs_ * homogeneous_interpolation_points + warping_coeffs_ * kernel;
  }

 protected:
  static double KernelFunction(const double r_sq) {
    // Equations taken from
    // https://www.geometrictools.com/Documentation/ThinPlateSplines.pdf
    // with coefficient (alpha) dropped as it is absorbed by warping_coeffs_
    switch (DIMENSIONS) {
      case 2:
        // This should be r^2 * log(r). We do not take the square root
        // because the factor of 2 gets absorbed by warping_coeffs_
        return r_sq == 0.0 ? 0.0 : r_sq * std::log(r_sq);
      case 3:
        // The negative is kept here for consistency with
        // https://www.geometrictools.com/GTEngine/Include/Mathematics/GteIntpThinPlateSpline3.h
        // and other TPS implementations found online
        return r_sq == 0.0 ? 0.0 : -std::sqrt(r_sq);
      default:
        static_assert(DIMENSIONS == 2 || DIMENSIONS == 3, "Only 2 and 3 dimensional kernels are implemented");
    }
  }

  HomogeneousPointSet homogeneous_template_points_;
  PointSet target_points_;
  Eigen::Matrix<double, DIMENSIONS, DIMENSIONS + 1> affine_coeffs_;
  PointSet warping_coeffs_;
  bool solved_;
};
}  // namespace arc_utilities

#endif  // THIN_PLATE_SPLINE_HPP