/*This program uses Rcpp attributes to import pure C++ functions into R. The main
function takes in an anrmadillo matrix(which has already had the anova
filter applied to it) and parses through its rows. NA values are skipped
and non-NA values are stored in a vector of vectors called groups, depending
on their group membership. All the non-NA values are concatenated into a vector which
is sorted in ascending order. Each non-NA value is assigned a rank, all the ranks
are stored in another vector called rankvec. The ranks are used in the
calculation of the Kruskall Wallis h statistic to then calculate the p-value of
that specific row.*/

/*The fuction group_size takes one input, a vector of strings called group. This
vector is an ordered character vector indicating what "group" factor level a
specific data element belongs to. This function makes a copy of group, called temp.
Next a unique function is applied to temp, exposing the levels of the factor.
Next we iterate through temp and count how many elements belong to each level,
these counts are stored in gsize.*/

/*The function calculate_kwh takes two inputs, a vector of ranks and a vector
of the number of non-NA values per group.This function implements the
mathematical formula for Kruskal Wallis test statistic. It returns a numeric
value(p-value).*/

/*The function compute_pvalue, simply takes in the Kruskall Wallis test statistic
and computes a p-value.*/

/*The main function is the kwh function, it takes two inputs, an armadillo
matrix and a vector of strings called "group". The vector "group" is fed to
the group_size function which will return a vector of group sizes. One row
of the armadillo matrix is stored in a vector named "cp". We iterate over
"cp" and collect all non-NA values and store them in a vector of vectors
called "groups". We iterate over "groups" and take the size of each group
and store it in a vector called "nonmiss".Next we concatenate all the
vectors from "groups" into one large vector called "cpy." Next we sort
"cpy" by its indices and store the indices in the vector "yvec." Then
we allocate a vector "rankvec", the same size as "yvec". We assing a rank
to each of the elements of "cpy" and keep them in the same order as "yvec".
We then take "rankvec" and and un-concatenate the vector into three smaller
vectors which are stored in "ranks" (the ranks of each group). Finally "ranks"
is input into calculate_kwh and a p-value is returned for that specific row
of the armadillo matrix. This process is repeated untill the last row of the
matrix is reached, all the p-values are stored in order in a list named
"final."  */

#include <RcppArmadillo.h>
#include <boost/math/distributions/chi_squared.hpp>
// [[Rcpp::plugins("cpp11")]]
// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::depends(BH)]]
using namespace Rcpp;

std::vector<int> gp_size(std::vector<std::string> group)
{
  std::vector<std::string> temp;
  temp = group;

  //std::sort(temp.begin(),temp.end());

  temp.erase( std::unique( temp.begin(), temp.end() ), temp.end() );
  int tempsize = 0;

  tempsize = temp.size();

  std::vector<int> gsize(tempsize);

  for(unsigned int i = 0;i<group.size();i++)
  {
    for(unsigned int k=0;k<temp.size();k++)
    {
      if(group[i]==temp[k])
        gsize[k]++;
    }
  }

  return gsize;
}

double calculate_kwh(std::vector <std::vector <double> > ranks, std::vector <double> nonmiss_sizes)
{
  std::vector <double> rsums;
  double tempsum = 0,temporary = 0;
  double n = 0,summation = 0, result = 0;

  for(unsigned int i = 0;i<nonmiss_sizes.size();i++)
  {
    n = n + nonmiss_sizes[i];
  }

  for(unsigned int i = 0,j=0;i<ranks.size();i++)
  {
    for(int k = 0;k<nonmiss_sizes[j];k++)
    {
      tempsum = tempsum + ranks[i][k];
    }

    temporary = pow(tempsum,2)/ nonmiss_sizes[i];
    rsums.push_back(temporary);
    tempsum = 0;
    temporary = 0;
    j++;
  }

  for(unsigned int i = 0;i<rsums.size();i++)
  {
    summation = summation + rsums[i];
  }

  result = ((12/(n*(n+1)))*summation)- 3*(n+1);

  return result;
}

double compute_pvalue(double h,std::vector<double> nonmiss)
{
  double p = 0;
  int df = 0;
  df = nonmiss.size()-1;

  boost::math::chi_squared mydist(df);
  p = boost::math::cdf(mydist, h);

  return 1 - p;
}

// [[Rcpp::export]]
std::list<double> kw_rcpp(arma::mat mtr,std::vector<std::string> group)
{
  std::vector<std::string> unique_groups = group;
  std::sort(unique_groups.begin(),unique_groups.end());
  unique_groups.erase( std::unique( unique_groups.begin(), unique_groups.end() ), unique_groups.end() );

  std::list<double> final;
  std:: vector<double> cp(mtr.n_cols);
  std::vector<std::vector<double> >groups;

  double tempnonmis = 0;

  std::vector<double> nonmiss;
  std::vector<double> tmp;

  for(unsigned int i = 0; i <mtr.n_rows;i++)
  {
        cp = arma::conv_to< std::vector<double> >::from(mtr.row(i));

        for (unsigned int i = 0; i < unique_groups.size(); i++)
        {
          for (unsigned int k = 0; k < cp.size(); k++)
          {
            if (ISNAN(cp[k])){
              continue;
            }
            else if(unique_groups[i] == group[k]) {
              tmp.push_back(cp[k]);
            }
          }

          groups.push_back(tmp);
          tmp.clear();

       }

            int all_na_count = 0;

            for (unsigned int i = 0; i < groups.size(); i++)
            {
              if(groups[i].size()==0)
              {
                all_na_count++;
              }

             else
             {
               tempnonmis = groups[i].size();
               nonmiss.push_back(tempnonmis);
             }
           }

            if((groups.size()-all_na_count) < 2)
            {
              final.push_back(NA_REAL);
              nonmiss.clear();
              groups.clear();
            }

          else
            {
              std::vector<double> cpy;

              for(unsigned int i = 0; i <groups.size();i++)
              {
                if(groups[i].size() == 0)
                {
                  continue;
                }

                else
                {
                  cpy.insert(cpy.end(), groups[i].begin(), groups[i].end());
                }
              }

              std::vector <double> rankvec (cpy.size());
              std::vector<double> yvec(cpy.size());
              std::size_t n(0);

              std::generate(yvec.begin(), yvec.end(), [&]{ return n++; });
              std::sort(yvec.begin(), yvec.end(), [&](int i1, int i2) {return cpy[i1] < cpy[i2]; } );

              for(unsigned int i = 0,j = 1;i<yvec.size();i++)
              {
                rankvec[yvec[i]]= j;
                j++;
              }

              double tempr = 0;
              std::vector<double>tempvec;
              std::vector<std::vector<double> > ranks;

                for (unsigned int i = 0, j = 0; i <nonmiss.size(); i++)
                {
                    for (unsigned int i = 0; i < nonmiss[j]; i++)
                    {
                      tempr = rankvec[i];
                      tempvec.push_back(tempr);
                    }

                  ranks.push_back(tempvec);
                  rankvec.erase(rankvec.begin(),rankvec.begin()+nonmiss[j]);
                  tempvec.clear();

                  j++;
                }

                double h = 0, p = 0;

                h = calculate_kwh(ranks,nonmiss);
                p = compute_pvalue(h,nonmiss);

                final.push_back(p);

                nonmiss.clear();
                groups.clear();
                ranks.clear();
                rankvec.clear();
                yvec.clear();
                cpy.clear();
            }
      }

  return final;
}


#pragma once

#include <Eigen/Dense>

using Eigen::MatrixXf;
using Eigen::VectorXf;

namespace hyped {
namespace utils {
namespace math {

/**
 * @brief    This class is for filtering the data from sensors to smoothen it.
 */
class KalmanMultivariate {
 public:
  /**
   * @brief    Construct a new Kalman object with respective dimensions (with control)
   *
   * @param[in] n                       state dimensionality
   * @param[in] m                       measurement dimensionality
   * @param[in] k                       control dimensionality (default 0)
   */
  KalmanMultivariate(unsigned int n, unsigned int m, unsigned int k = 0);

  /**
   * @brief    Set dynamics model matrices (without control)
   *
   * @param[in] A                       state transition matrix
   * @param[in] Q                       process noise covariance
   */
  void setDynamicsModel(MatrixXf &A, MatrixXf &Q);

  /**
   * @brief    Set dynamics model matrices (with control)
   *
   * @param[in] A                       state transition matrix
   * @param[in] B                       control matrix
   * @param[in] Q                       process noise covariance
   */
  void setDynamicsModel(MatrixXf &A, MatrixXf &B, MatrixXf &Q);

  /**
   * @brief    Set measurement model matrices
   *
   * @param[in] H                       measurement matrix
   * @param[in] R                       measurement noise covariance
   */
  void setMeasurementModel(MatrixXf &H, MatrixXf &R);

  /**
   * @brief    Set model matrices (without control)
   *
   * @param[in] A                       state transition matrix
   * @param[in] Q                       process noise covariance
   * @param[in] H                       measurement matrix
   * @param[in] R                       measurement noise covariance
   */
  void setModels(MatrixXf &A, MatrixXf &Q, MatrixXf &H, MatrixXf &R);

  /**
   * @brief    Set model matrices (with control)
   *
   * @param[in] A                       state transition matrix
   * @param[in] B                       control matrix
   * @param[in] Q                       process noise covariance
   * @param[in] H                       measurement matrix
   * @param[in] R                       measurement noise covariance
   */
  void setModels(MatrixXf &A, MatrixXf &B, MatrixXf &Q, MatrixXf &H, MatrixXf &R);

  /**
   * @brief    Update state transition matrix
   *
   * @param[in] A                       state transition matrix
   */
  void updateA(MatrixXf &A);

  /**
   * @brief    Update measurement covariance matrix
   *
   * @param[in] R                       measurement covariance matrix
   */
  void updateR(MatrixXf &R);

  /**
   * @brief    Set initial beliefs
   *
   * @param[in] x0                      initial state belief
   * @param[in] P0                      initial state covariance (uncertainty)
   */
  void setInitial(VectorXf &x0, MatrixXf &P0);

  /**
   * @brief    Filter measurement and update state belief with covariance (without control)
   *
   * @param[in] z                       measurement vector
   */
  void filter(VectorXf &z);

  /**
   * @brief    Filter measurement and update state belief with covariance (with control)
   *
   * @param[in] u                       control vector
   * @param[in] z                       measurement vector
   */
  void filter(VectorXf &u, VectorXf &z);

  /**
   * @brief     Get the state estimate
   *
   * @return    Returns the current state estimate
   */
  VectorXf &getStateEstimate();

  /**
   * @brief     Get the state uncertainty
   *
   * @return    Returns the current state covariance
   */
  MatrixXf &getStateCovariance();

 private:
  /* problem dimensions */
  unsigned int n_;  // state dimension
  unsigned int m_;  // measurement dimension
  unsigned int k_;  // control dimension (0 if not set)

  /* dynamics model matrices */
  MatrixXf A_;  // state transition matrix: n x n
  MatrixXf B_;  // control matrix: n x k
  MatrixXf Q_;  // process noise covariance: n x n

  /* measurement model matrices */
  MatrixXf H_;  // measurement matrix: m x n
  MatrixXf R_;  // measurement noise covariance: m x m

  /* state estimates */
  VectorXf x_;  // state vector: n x 1
  MatrixXf P_;  // state covariance: n x n
  MatrixXf I_;  // identity matrix: n x n

  /**
   * @brief    Predict state belief with covariance based on dynamics (without control)
   */
  void predict();

  /**
   * @brief    Predict state belief with covariance based on dynamics (with control)
   *
   * @param[in] u                       control vector
   */
  void predict(VectorXf &u);

  /**
   * @brief    Correct state belief with covariance based on measurement
   *
   * @param[in] z                       measurement vector
   */
  void correct(VectorXf &z);
};
}  // namespace math
}  // namespace utils
}  // namespace hyped


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2020 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Authors: Wolfgang Bangerth, 1999,
 *          Guido Kanschat, 2011
 *          Luca Heltai, 2021
 */
#include "stokes.h"

#include <deal.II/lac/linear_operator.h>
#include <deal.II/lac/linear_operator_tools.h>
#include <deal.II/lac/solver_gmres.h>

#include <deal.II/numerics/error_estimator.h>

using namespace dealii;

namespace
{
  std::vector<std::string>
  get_component_names(int dim)
  {
    std::vector<std::string> names(dim + 1, "u");
    names[dim] = "p";
    return names;
  }
} // namespace

template <int dim>
Stokes<dim>::Stokes()
  : BaseBlockProblem<dim>(get_component_names(dim),
                          "Stokes<" + std::to_string(dim) + ">")
  , velocity(0)
  , pressure(dim)
{
  // Output the vector result.
  this->add_data_vector.connect([&](auto &data_out) {
    std::vector<DataComponentInterpretation::DataComponentInterpretation>
      interpretation(dim + 1,
                     DataComponentInterpretation::component_is_part_of_vector);

    interpretation[dim] = DataComponentInterpretation::component_is_scalar;

    data_out.add_data_vector(this->locally_relevant_block_solution,
                             this->component_names,
                             DataOut<dim>::type_dof_data,
                             interpretation);
  });
}

template <int dim>
void
Stokes<dim>::assemble_system_one_cell(
  const typename DoFHandler<dim>::active_cell_iterator &cell,
  ScratchData &                                         scratch,
  CopyData &                                            copy)
{
  auto &cell_matrix = copy.matrices[0];
  auto &cell_rhs    = copy.vectors[0];

  cell->get_dof_indices(copy.local_dof_indices[0]);

  const auto &fe_values = scratch.reinit(cell);
  cell_matrix           = 0;
  cell_rhs              = 0;

  for (const unsigned int q_index : fe_values.quadrature_point_indices())
    {
      for (const unsigned int i : fe_values.dof_indices())
        {
          const auto eps_v = fe_values[velocity].symmetric_gradient(
            i, q_index); // SymmetricTensor<2,dim>
          const auto div_v =
            fe_values[velocity].divergence(i, q_index);         // double
          const auto q = fe_values[pressure].value(i, q_index); // double

          for (const unsigned int j : fe_values.dof_indices())
            {
              const auto eps_u = fe_values[velocity].symmetric_gradient(
                j, q_index); // SymmetricTensor<2,dim>
              const auto div_u =
                fe_values[velocity].divergence(j, q_index);         // double
              const auto p = fe_values[pressure].value(j, q_index); // double

              cell_matrix(i, j) +=
                (scalar_product(eps_v, eps_u) - p * div_v - q * div_u) *
                fe_values.JxW(q_index); // dx
            }
          for (const unsigned int i : fe_values.dof_indices())
            {
              const auto comp_i = this->fe->system_to_component_index(i).first;
              cell_rhs(i) +=
                (fe_values.shape_value(i, q_index) * // phi_i(x_q)
                 this->forcing_term.value(fe_values.quadrature_point(q_index),
                                          comp_i) * // f(x_q)
                 fe_values.JxW(q_index));           // dx
            }
        }
    }

  if (cell->at_boundary())
    //  for(const auto face: cell->face_indices())
    for (unsigned int f = 0; f < GeometryInfo<dim>::faces_per_cell; ++f)
      if (this->neumann_ids.find(cell->face(f)->boundary_id()) !=
          this->neumann_ids.end())
        {
          auto &fe_face_values = scratch.reinit(cell, f);
          for (const unsigned int q_index :
               fe_face_values.quadrature_point_indices())
            for (const unsigned int i : fe_face_values.dof_indices())
              {
                const auto comp_i =
                  this->fe->system_to_component_index(i).first;
                cell_rhs(i) +=
                  fe_face_values.shape_value(i, q_index) *
                  this->neumann_boundary_condition.value(
                    fe_face_values.quadrature_point(q_index), comp_i) *
                  fe_face_values.JxW(q_index);
              }
        }
}

template <int dim>
void
Stokes<dim>::solve()
{
  TimerOutput::Scope                timer_section(this->timer, "solve");
  SolverGMRES<LA::MPI::BlockVector> solver(this->solver_control);

  LA::MPI::PreconditionAMG amg;
  amg.initialize(this->system_block_matrix.block(0, 0));

  solver.solve(this->system_block_matrix,
               this->block_solution,
               this->system_block_rhs,
               PreconditionIdentity());
  this->constraints.distribute(this->block_solution);
  this->locally_relevant_block_solution = this->block_solution;
}

template <int dim>
void
Stokes<dim>::estimate()
{
  TimerOutput::Scope timer_section(this->timer, "estimate");
  if (this->estimator_type == "kelly")
    {
      std::map<types::boundary_id, const Function<dim> *> neumann;
      for (const auto id : this->neumann_ids)
        neumann[id] = &this->neumann_boundary_condition;

      QGauss<dim - 1> face_quad(this->fe->degree + 1);
      KellyErrorEstimator<dim>::estimate(*this->mapping,
                                         this->dof_handler,
                                         face_quad,
                                         neumann,
                                         this->locally_relevant_block_solution,
                                         this->error_per_cell,
                                         this->fe->component_mask(velocity));
    }
  else
    {
      AssertThrow(false, ExcNotImplemented());
    }
  auto global_estimator = this->error_per_cell.l2_norm();
  this->error_table.add_extra_column("estimator", [global_estimator]() {
    return global_estimator;
  });
  this->error_table.error_from_exact(*this->mapping,
                                     this->dof_handler,
                                     this->locally_relevant_block_solution,
                                     this->exact_solution);
}

template class Stokes<1>;
template class Stokes<2>;
template class Stokes<3>;


/*(utf8)
 * This is a slight tweek of the
 * 1€ Filter, template-compliant version
 * by Jonathan Aceituno <join@oin.name>
 * omiting timestamps, alowing for interger input,
 * and giving independant access to the lowpass filter.
 *
 * For details, see http://www.lifl.fr/~casiez/1euro
 * * quote * "Note that parameters fcmin and beta have clear
 * conceptual relationships: if high speed lag is a problem,
 * increase beta; if slow speed jitter is a problem, decrease
 * fcmin."
*/

#include "filter_base.hpp"

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

#define INIT_FREQ 120.
#define INIT_CUTOFF 1.
#define INIT_MIN 1.
#define INIT_BETA 1.

using namespace boost::math::constants;

namespace value_filters
{
template <typename T = double>
class low_pass_filter
{
public:
  low_pass_filter(double _freq = INIT_FREQ, double _dcutoff = INIT_CUTOFF)
      : hatxprev {0}, xprev {0}, alpha {0}, hadprev {false}
  {
    set_alpha(_freq, _dcutoff);
  }

  T operator()(T x)
  {
    T hatx {0};

    if (hadprev)
    {
      hatx = alpha * x + (1 - alpha) * hatxprev;
    }
    else
    {
      hatx = x;
      hadprev = true;
    }

    hatxprev = hatx;
    xprev = x;
    return hatx;
  }

  T xprev {};
  bool hadprev {};
  double dcutoff {}, freq {};

  void set_alpha(double cutoff, double _freq)
  {
    dcutoff = cutoff;
    freq = _freq;
    compute_alpha();
  }

  void set_amount(double amt)
  {
    set_alpha(pow(1 / (1 + amt), 2), freq);
  }

  void update()
  {
    compute_alpha();
  }

private:
  T hatxprev{}, alpha{};

  void compute_alpha()
  {
    T tau = one_div_two_pi<double>() * (1. / dcutoff);
    double te = 1. / freq;

    alpha = 1. / (1. + tau / te);
  }
};

template <typename T = double>
struct one_euro_filter
{
  one_euro_filter(
      double _freq = INIT_FREQ, double _mincutoff = INIT_MIN,
      double _beta = INIT_BETA, double _dcutoff = INIT_CUTOFF)
      : freq {_freq}, beta {_beta}, dcutoff {_dcutoff}, mincutoff {_mincutoff}
  {
  }

  T operator()(T x)
  {
    T dx {0};

    if (xfilt_.hadprev)
      dx = (x - xfilt_.xprev) * freq;

    dxfilt_.set_alpha(dcutoff, freq);
    T edx = dxfilt_(dx);
    T cutoff = mincutoff + beta * std::abs(static_cast<double>(edx));

    xfilt_.set_alpha(cutoff, freq);
    return xfilt_(x);
  }

  double freq {}, beta {}, dcutoff {};

  void set_amount(double amt)
  {
    if (amt <= 0.) amt = 0.0001;
    // mincutoff is basicly the inverse of the amount of filtering
    mincutoff = SCALED_AMOUNT - amt;
  }

  void update()
  {
  }
private:
  double mincutoff {};

  low_pass_filter<T> xfilt_ {}, dxfilt_ {};
};

}


#include <vector>
#include <Eigen/Dense>

using std::vector; using std::string;

typedef Eigen::MatrixXd matrix;

int atom(int ao_index, int orbitals_per_atom)
{
    return ao_index / orbitals_per_atom;
}

int orb_index(int ao_index, int orbitals_per_atom)
{
    return ao_index % orbitals_per_atom;
}

float chi_on_atom(int o1, int o2, int o3, double dipole)
{
    if (o1 == o2 && o3 == 0)
        return 1.0;
    else if (o1 == o3 && (o3 > 0 && o3 <= 3) && o2 == 0)
        return dipole;
    else if (o2 == o3 && (o3 > 0 && o3 <= 3) && o1 == 0)
        return dipole;
    return 0.0;
}

matrix calculate_fock_matrix(matrix hamiltonian_matrix, matrix interaction_matrix, matrix density_matrix, int orbitals_per_atom, double dipole){
    int ndof = hamiltonian_matrix.row(0).size();
    matrix fock_matrix = hamiltonian_matrix;

    for (int p = 0; p < ndof; p++)
    {
        int at_p = atom(p, orbitals_per_atom);
        int orb_p = orb_index(p, orbitals_per_atom);
        for (int orb_q = 0; orb_q < orbitals_per_atom; orb_q++)
        {
            int q = orb_q + at_p * orbitals_per_atom;
            for (int orb_t = 0; orb_t < orbitals_per_atom; orb_t++)
            {
                int t = orb_t + at_p * orbitals_per_atom;
                float chi_pqt = chi_on_atom(orb_p, orb_q, orb_t, dipole);
                for (int r = 0; r < ndof; r++)
                {
                    int at_r = atom(r, orbitals_per_atom);
                    int orb_r = orb_index(r, orbitals_per_atom);
                    for (int orb_s = 0; orb_s < orbitals_per_atom; orb_s++)
                    {
                        int s = orb_s + at_r * orbitals_per_atom;
                        for (int orb_u = 0; orb_u < orbitals_per_atom; orb_u++)
                        {
                            int u = orb_u + at_r * orbitals_per_atom;
                            float chi_rsu = chi_on_atom(orb_r, orb_s, orb_u, dipole);
                            fock_matrix(p,q) += 2.0 * chi_pqt * chi_rsu * interaction_matrix(t, u) * density_matrix(r, s);
                        }
                    }
                }
            }
        }
    }
    for (int p = 0; p < ndof; p++)
    {
        int at_p = atom(p, orbitals_per_atom);
        int orb_p = orb_index(p, orbitals_per_atom);
        for (int orb_s = 0; orb_s < orbitals_per_atom; orb_s++)
        {
            int s = orb_s + at_p * orbitals_per_atom;
            for (int orb_u = 0; orb_u < orbitals_per_atom; orb_u++)
            {
                int u = orb_u + at_p * orbitals_per_atom;
                float chi_psu = chi_on_atom(orb_p, orb_s, orb_u, dipole);
                for (int q = 0; q < ndof; q++)
                {
                    int at_q = atom(q, orbitals_per_atom);
                    int orb_q = orb_index(q, orbitals_per_atom);
                    for (int orb_r = 0; orb_r < orbitals_per_atom; orb_r++)
                    {
                        int r = orb_r + at_q * orbitals_per_atom;
                        for (int orb_t = 0; orb_t < orbitals_per_atom; orb_t++)
                        {
                            int t = orb_t + at_q * orbitals_per_atom;
                            float chi_rqt = chi_on_atom(orb_r, orb_q, orb_t, dipole);
                            fock_matrix(p,q) -= chi_rqt * chi_psu * interaction_matrix(t, u) * density_matrix(r, s);
                        }
                    }
                }
            }
        }
    }
    return fock_matrix;
}


/**
 * @file spherical_kernel.hpp
 * @author Neil Slagle
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_CORE_KERNELS_SPHERICAL_KERNEL_HPP
#define MLPACK_CORE_KERNELS_SPHERICAL_KERNEL_HPP

#include <boost/math/special_functions/gamma.hpp>
#include <mlpack/prereqs.hpp>

namespace mlpack {
namespace kernel {

/**
 * The spherical kernel, which is 1 when the distance between the two argument
 * points is less than or equal to the bandwidth, or 0 otherwise.
 */
class SphericalKernel
{
 public:
  /**
   * Construct the SphericalKernel with the given bandwidth.
   */
  SphericalKernel(const double bandwidth = 1.0) :
    bandwidth(bandwidth),
    bandwidthSquared(std::pow(bandwidth, 2.0))
  { /* Nothing to do. */ }

  /**
   * Evaluate the spherical kernel with the given two vectors.
   *
   * @tparam VecTypeA Type of first vector.
   * @tparam VecTypeB Type of second vector.
   * @param a First vector.
   * @param b Second vector.
   * @return The kernel evaluation between the two vectors.
   */
  template<typename VecTypeA, typename VecTypeB>
  double Evaluate(const VecTypeA& a, const VecTypeB& b) const
  {
    return
        (metric::SquaredEuclideanDistance::Evaluate(a, b) <= bandwidthSquared) ?
        1.0 : 0.0;
  }
  /**
   * Obtains the convolution integral [integral K(||x-a||)K(||b-x||)dx]
   * for the two vectors.
   *
   * @tparam VecTypeA Type of first vector (arma::vec, arma::sp_vec should be
   *       expected).
   * @tparam VecTypeB Type of second vector.
   * @param a First vector.
   * @param b Second vector.
   * @return the convolution integral value.
   */
  template<typename VecTypeA, typename VecTypeB>
  double ConvolutionIntegral(const VecTypeA& a, const VecTypeB& b) const
  {
    double distance = sqrt(metric::SquaredEuclideanDistance::Evaluate(a, b));
    if (distance >= 2.0 * bandwidth)
    {
      return 0.0;
    }
    double volumeSquared = pow(Normalizer(a.n_rows), 2.0);

    switch (a.n_rows)
    {
      case 1:
        return 1.0 / volumeSquared * (2.0 * bandwidth - distance);
        break;
      case 2:
        return 1.0 / volumeSquared *
          (2.0 * bandwidth * bandwidth * acos(distance/(2.0 * bandwidth)) -
          distance / 4.0 * sqrt(4.0*bandwidth*bandwidth-distance*distance));
        break;
      default:
        Log::Fatal << "The spherical kernel does not support convolution\
          integrals above dimension two, yet..." << std::endl;
        return -1.0;
        break;
    }
  }
  double Normalizer(size_t dimension) const
  {
    return pow(bandwidth, (double) dimension) * pow(M_PI, dimension / 2.0) /
        std::tgamma(dimension / 2.0 + 1.0);
  }

  /**
   * Evaluate the kernel when only a distance is given, not two points.
   *
   * @param t Argument to kernel.
   */
  double Evaluate(const double t) const
  {
    return (t <= bandwidth) ? 1.0 : 0.0;
  }
  double Gradient(double t)
  {
    return t == bandwidth ? arma::datum::nan : 0.0;
  }

  //! Serialize the object.
  template<typename Archive>
  void Serialize(Archive& ar, const unsigned int /* version */)
  {
    ar & data::CreateNVP(bandwidth, "bandwidth");
    ar & data::CreateNVP(bandwidthSquared, "bandwidthSquared");
  }

 private:
  double bandwidth;
  double bandwidthSquared;
};

//! Kernel traits for the spherical kernel.
template<>
class KernelTraits<SphericalKernel>
{
 public:
  //! The spherical kernel is normalized: K(x, x) = 1 for all x.
  static const bool IsNormalized = true;
  //! The spherical kernel doesn't include a squared distance.
  static const bool UsesSquaredDistance = false;
};

} // namespace kernel
} // namespace mlpack

#endif


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGY_GEOGRAPHIC_AREA_BOX_HPP
#define BOOST_GEOMETRY_STRATEGY_GEOGRAPHIC_AREA_BOX_HPP

#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/srs/spheroid.hpp>
#include <boost/geometry/strategies/spherical/get_radius.hpp>
#include <boost/geometry/strategy/area.hpp>
#include <boost/geometry/util/normalize_spheroidal_box_coordinates.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace area
{

// Based on the approach for spherical coordinate system:
// https://math.stackexchange.com/questions/131735/surface-element-in-spherical-coordinates
// http://www.cs.cmu.edu/afs/cs/academic/class/16823-s16/www/pdfs/appearance-modeling-3.pdf
// https://www.astronomyclub.xyz/celestial-sphere-2/solid-angle-on-the-celestial-sphere.html
// https://mathworld.wolfram.com/SolidAngle.html
// https://en.wikipedia.org/wiki/Spherical_coordinate_system
// and equations for spheroid:
// https://en.wikipedia.org/wiki/Geographic_coordinate_conversion
// https://en.wikipedia.org/wiki/Meridian_arc
// Note that the equations use geodetic latitudes so we do not have to convert them.
// assume(y_max > y_min);
// assume(x_max > x_min);
// M: a*(1-e^2) / (1-e^2*sin(y)^2)^(3/2);
// N: a / sqrt(1-e^2*sin(y)^2);
// O: N*cos(y)*M;
// tellsimp(log(abs(e*sin(y_min)+1)), p_min);
// tellsimp(log(abs(e*sin(y_min)-1)), m_min);
// tellsimp(log(abs(e*sin(y_max)+1)), p_max);
// tellsimp(log(abs(e*sin(y_max)-1)), m_max);
// S: integrate(integrate(O, y, y_min, y_max), x, x_min, x_max);
// combine(S);
//
// An alternative solution to the above formula was suggested by Charles Karney
// https://github.com/boostorg/geometry/pull/832
// The following are formulas for area of a box defined by the equator and some latitude,
// not arbitrary box.
// For e^2 > 0
// dlambda*b^2*sin(phi)/2*(1/(1-e^2*sin(phi)^2) + atanh(e*sin(phi))/(e*sin(phi)))
// For e^2 < 0
// dlambda*b^2*sin(phi)/2*(1/(1-e^2*sin(phi)^2) + atan(ea*sin(phi))/(ea*sin(phi)))
// where ea = sqrt(-e^2)
template
<
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic_box
{
public:
    template <typename Box>
    struct result_type
        : strategy::area::detail::result_type
            <
                Box,
                CalculationType
            >
    {};

    geographic_box() = default;

    explicit geographic_box(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

    template <typename Box>
    inline auto apply(Box const& box) const
    {
        typedef typename result_type<Box>::type return_type;

        return_type const c0 = 0;

        return_type x_min = get_as_radian<min_corner, 0>(box); // lon
        return_type y_min = get_as_radian<min_corner, 1>(box); // lat
        return_type x_max = get_as_radian<max_corner, 0>(box);
        return_type y_max = get_as_radian<max_corner, 1>(box);

        math::normalize_spheroidal_box_coordinates<radian>(x_min, y_min, x_max, y_max);

        if (x_min == x_max || y_max == y_min)
        {
            return c0;
        }

        return_type const e2 = formula::eccentricity_sqr<return_type>(m_spheroid);

        return_type const x_diff = x_max - x_min;
        return_type const sin_y_min = sin(y_min);
        return_type const sin_y_max = sin(y_max);

        if (math::equals(e2, c0))
        {
            // spherical formula
            return_type const a = get_radius<0>(m_spheroid);
            return x_diff * (sin_y_max - sin_y_min) * a * a;
        }

        return_type const c1 = 1;
        return_type const c2 = 2;
        return_type const b = get_radius<2>(m_spheroid);

        /*
        return_type const c4 = 4;
        return_type const e = math::sqrt(e2);

        return_type const p_min = log(math::abs(e * sin_y_min + c1));
        return_type const p_max = log(math::abs(e * sin_y_max + c1));
        return_type const m_min = log(math::abs(e * sin_y_min - c1));
        return_type const m_max = log(math::abs(e * sin_y_max - c1));
        return_type const n_min = e * sin_y_min * sin_y_min;
        return_type const n_max = e * sin_y_max * sin_y_max;
        return_type const d_min = e * n_min - c1;
        return_type const d_max = e * n_max - c1;

        // NOTE: For equal latitudes the original formula generated by maxima may give negative
        //   result. It's caused by the order of operations, so here they're rearranged for
        //   symmetry.
        return_type const comp0 = (p_min - m_min) / (c4 * e * d_min);
        return_type const comp1 = sin_y_min / (c2 * d_min);
        return_type const comp2 = n_min * (m_min - p_min) / (c4 * d_min);
        return_type const comp3 = (p_max - m_max) / (c4 * e * d_max);
        return_type const comp4 = sin_y_max / (c2 * d_max);
        return_type const comp5 = n_max * (m_max - p_max) / (c4 * d_max);
        return_type const comp02 = comp0 + comp1 + comp2;
        return_type const comp35 = comp3 + comp4 + comp5;

        return b * b * x_diff * (comp02 - comp35);
        */

        return_type const comp0_min = c1 / (c1 - e2 * sin_y_min * sin_y_min);
        return_type const comp0_max = c1 / (c1 - e2 * sin_y_max * sin_y_max);

        // NOTE: For latitudes equal to 0 the original formula returns NAN
        return_type comp1_min = 0, comp1_max = 0;
        if (e2 > c0)
        {
            return_type const e = math::sqrt(e2);
            return_type const e_sin_y_min = e * sin_y_min;
            return_type const e_sin_y_max = e * sin_y_max;

            comp1_min = e_sin_y_min == c0 ? c1 : atanh(e_sin_y_min) / e_sin_y_min;
            comp1_max = e_sin_y_max == c0 ? c1 : atanh(e_sin_y_max) / e_sin_y_max;
        }
        else
        {
            return_type const ea = math::sqrt(-e2);
            return_type const ea_sin_y_min = ea * sin_y_min;
            return_type const ea_sin_y_max = ea * sin_y_max;

            comp1_min = ea_sin_y_min == c0 ? c1 : atan(ea_sin_y_min) / ea_sin_y_min;
            comp1_max = ea_sin_y_max == c0 ? c1 : atan(ea_sin_y_max) / ea_sin_y_max;
        }

        return_type const comp01_min = sin_y_min * (comp0_min + comp1_min);
        return_type const comp01_max = sin_y_max * (comp0_max + comp1_max);

        return b * b * x_diff * (comp01_max - comp01_min) / c2;
    }

    Spheroid model() const
    {
        return m_spheroid;
    }

private:
    Spheroid m_spheroid;
};

}} // namespace strategy::area

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_GEOGRAPHIC_AREA_BOX_HPP


#include <math.h>
#include <iostream>
#include <vector>
#include <Eigen/Dense>

#define M_SQRT5 2.2360679774997896964091736687313
#define M_1_16 0.0625

#define P(a, b) velocity(a, b)
#define O(a, b) velocity(b, a)

using Eigen::MatrixXd;

/*

var s = `Eigen::MatrixXf m(${pointsX.length}, 2);\nm << `; for (var i = 0; i < pointsX.length; i++) s += pointsX[i] + ", " + pointsY[i] + (i == pointsX.length-1 ? ";" : ",\n")

http://weitz.de/hobby/

*/

float velocity(float a, float b)
{
    float cosa = cosf(a);
    float cosb = cosf(b);

    float sina = sinf(a);
    float sinb = sinf(b);

    return (4. + 2. * M_SQRT2 * (sina - M_1_16 * sinb) * (sinb - M_1_16 * sina) * (cosa - cosb)) /
           (2. + (M_SQRT5 - 1) * cosa + (3. - M_SQRT5) * cosb);
}

Eigen::MatrixXf calcVectors(Eigen::MatrixXf m)
{
    Eigen::MatrixXf v = Eigen::MatrixXf::Zero(m.rows() - 1, 2);
    for (int i = 1; i < m.rows(); i++)
    {
        float dx = m(i, 0) - m(i - 1, 0);
        float dy = m(i, 1) - m(i - 1, 1);

        v(i - 1, 0) = dx;
        v(i - 1, 1) = dy;
    }

    return v;
}

Eigen::VectorXf calcDistances(Eigen::MatrixXf m)
{
    Eigen::VectorXf d = Eigen::VectorXf::Zero(m.rows());

    for (int i = 0; i < m.rows(); i++)
    {
        d[i] = sqrtf(m(i, 0) * m(i, 0) + m(i, 1) * m(i, 1));
    }

    return d;
}

Eigen::VectorXf calcAngles(Eigen::MatrixXf m, Eigen::VectorXf d)
{
    assert(m.rows() == d.rows());

    Eigen::VectorXf a = Eigen::VectorXf::Zero(m.rows() + 1);

    for (int i = 1; i < m.rows(); i++)
    {
        // credtig goes to: https://de.mathworks.com/matlabcentral/answers/180131-how-can-i-find-the-angle-between-two-vectors-including-directional-information
        float x1 = m(i - 1, 0);
        float y1 = m(i - 1, 1);

        float x2 = m(i, 0);
        float y2 = m(i, 1);

        a[i] = atan2f(x1 * y2 - y1 * x2, x1 * x2 + y1 * y2);
    }

    // last angle is zero

    return a;
}

// Vectors a, b, c and d are const. They will not be modified
// by the function. Vector f (the solution vector) is non-const
// and thus will be calculated and updated by the function.
void thomas_algorithm(const Eigen::VectorXf &a,
                      const Eigen::VectorXf &b,
                      const Eigen::VectorXf &c,
                      const Eigen::VectorXf &d,
                      Eigen::VectorXf &x)
{
    Eigen::Index N = c.rows();

    // Create the temporary vectors
    // Note that this is inefficient as it is possible to call
    // this function many times. A better implementation would
    // pass these temporary matrices by non-const reference to
    // save excess allocation and deallocation
    std::vector<float> c_star(N, 0.0);
    std::vector<float> d_star(N, 0.0);

    // This updates the coefficients in the first row
    // Note that we should be checking for division by zero here
    c_star[0] = c[0] / b[0];
    d_star[0] = d[0] / b[0];

    // Create the c_star and d_star coefficients in the forward sweep
    for (int i = 1; i < N; i++)
    {

        float m = (b[i] - a[i] * c_star[i - 1]);
        c_star[i] = c[i] / m;
        d_star[i] = (d[i] - a[i] * d_star[i - 1]) / m;
    }

    // This is the reverse sweep, used to update the solution vector f
    x[N - 1] = d_star[N - 1];
    for (int i = N - 2; i >= 0; i--)
    {
        x[i] = d_star[i] - c_star[i] * x[i + 1];
    }
}

void solveAlpha(const Eigen::VectorXf &d, const Eigen::VectorXf &g, Eigen::VectorXf &alpha)
{
    assert(d.rows() + 1 == g.rows());

    Eigen::Index N = g.rows();

    Eigen::VectorXf a = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf b = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf c = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf dd = Eigen::VectorXf::Zero(N);

    for (int i = 1; i < (N - 1); i++)
    {
        a[i] = 1 / d[i - 1];
        b[i] = (2 * d[i - 1] + 2 * d[i]) / (d[i - 1] * d[i]);
        c[i] = 1 / d[i];
        dd[i] = -(2 * g[i] * d[i] + g[i + 1] * d[i - 1]) / (d[i - 1] * d[i]);
    }

    float omega = 1.;
    b[0] = 2. + omega;
    c[0] = 2. * omega + 1.;
    dd[0] = -c[0] * g[1];
    a[N - 1] = 2. * omega + 1.;
    b[N - 1] = 2. + omega;
    dd[N - 1] = 0.;

    thomas_algorithm(a, b, c, dd, alpha);
}

int main()
{
    Eigen::MatrixXf m(9, 2);
    m << 61.21672821044922, 309.6958312988281,
        263.11785888671875, 680.9885864257812,
        509.5057067871094, 653.6121826171875,
        807.2243041992188, 415.77947998046875,
        300.7604675292969, 424.3345947265625,
        141.63497924804688, 723.7642822265625,
        206.65399169921875, 290.87451171875,
        182.69961547851562, 545.8175048828125,
        839.7338256835938, 602.2813720703125;

    Eigen::Index N = m.rows();

    // test velocity function
    float test = P(1.2, 0.5) - O(0.5, 1.2);
    printf("test [should be 0]: %f\n", test);

    // calculate vectors
    Eigen::MatrixXf v = calcVectors(m);
    std::cout << "Vectors:" << std::endl;
    std::cout << v << std::endl;

    // calculate distances
    Eigen::VectorXf d = calcDistances(v);
    std::cout << "Distances:" << std::endl;
    std::cout << d << std::endl;

    // calculate angles
    Eigen::VectorXf g = calcAngles(v, d);
    std::cout << "Gamma Angles:" << std::endl;
    std::cout << g << std::endl;

    // prepare thomas algorithm
    Eigen::VectorXf alpha = Eigen::VectorXf::Zero(N);
    solveAlpha(d, g, alpha);
    std::cout << "Alpha Angles:" << std::endl;
    std::cout << alpha << std::endl;

    // calculate beta, a, b, sx, sy
    Eigen::VectorXf beta = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf a = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf b = Eigen::VectorXf::Zero(N);

    Eigen::VectorXf sx1 = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf sy1 = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf sx2 = Eigen::VectorXf::Zero(N);
    Eigen::VectorXf sy2 = Eigen::VectorXf::Zero(N);

    for (int i = 0; i < N - 1; i++)
    {
        if (i < N - 2)
        {
            beta[i] = -alpha[i + 1] - g[i + 1];
        }
        else
        {
            beta[i] = -alpha[N - 1];
        }

        a[i] = P(alpha[i], beta[i]) * d[i] / 3.;
        b[i] = O(alpha[i], beta[i]) * d[i] / 3.;

        Eigen::Vector2f t(v(i, 0), v(i, 1));

        Eigen::Vector2f v1(m(i, 0), m(i, 1));
        Eigen::Vector2f v2(m(i + 1, 0), m(i + 1, 1));

        Eigen::Rotation2D<float> rotAlpha(alpha[i]);
        Eigen::Rotation2D<float> rotBeta(-beta[i]);

        Eigen::Vector2f t1 = v1 + (rotAlpha * t).normalized() * a[i];
        Eigen::Vector2f t2 = v2 - (rotBeta * t).normalized() * b[i];

        sx1[i] = t1(0);
        sy1[i] = t1(1);
        sx2[i] = t2(0);
        sy2[i] = t2(1);
    }

    std::cout << "Beta Angles:" << std::endl;
    std::cout << beta << std::endl;

    std::cout << "a:" << std::endl;
    std::cout << a << std::endl;

    std::cout << "b:" << std::endl;
    std::cout << b << std::endl;

    // print html stuff for chrome

    std::cout << "var canvas = document.createElement('canvas');\n"
              << "\n"
              << "document.body = document.createElement('body');\n"
              << "document.body.append(canvas);\n"
              << "\n"
              << "canvas.width = document.body.clientWidth;\n"
              << "canvas.height = document.body.clientHeight;\n"
              << "\n"
              << "var ctx = canvas.getContext('2d');\n"
              << "\n"
              << "ctx.beginPath();\n"
              << "\n";

    std::ostringstream bezierString;
    std::ostringstream pointString;

    bezierString << "ctx.moveTo(" << m(0, 0) << "," << m(0, 1) << ");\n\n";
    pointString << "ctx.beginPath(); ctx.arc("
                << m(0, 0) << "," << m(0, 1)
                << ", 5, 0, 2 * Math.PI, false); ctx.fill();\n";

    for (int i = 0; i < N - 1; i++)
    {
        bezierString << "ctx.bezierCurveTo("
                     << sx1(i) << "," << sy1(i) << ","
                     << sx2(i) << "," << sy2(i) << ","
                     << m(i + 1, 0) << "," << m(i + 1, 1) << ");\n";

        pointString << "ctx.beginPath(); ctx.arc("
                    << m(i + 1, 0) << "," << m(i + 1, 1)
                    << ", 5, 0, 2 * Math.PI, false); ctx.fill();\n";
    }

    bezierString << "\nctx.stroke();\n\n";

    std::cout << bezierString.str() << pointString.str();

    return 0;
}


// Original source code from:
// https://github.com/stegua/MyBlogEntries/tree/master/Dijkstra

/// My typedefs
#include <boost/cstdint.hpp>
#include <boost/integer_traits.hpp>
#include <inttypes.h>

typedef int32_t node_t;
typedef int32_t edge_t;
typedef int64_t cost_t;

/// From STL library
#include <fstream>

#include <vector>
using std::vector;

#include <string>

using std::make_pair;
using std::pair;

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_traits.hpp>
using namespace boost;

typedef adjacency_list<vecS, vecS, directedS, no_property,
                       property<edge_weight_t, cost_t>>
    Digraph;
typedef graph_traits<Digraph>::vertex_descriptor Node;
typedef graph_traits<Digraph>::edge_descriptor Arc;

/// Boost Timer
#include <boost/progress.hpp>
using boost::timer;

using namespace boost;

/// Read input data, build graph, and run Dijkstra
cost_t runDijkstra(char *argv[]) {
  /// Read instance from the OR-lib
  std::ifstream infile(argv[1]);
  if (!infile)
    exit(EXIT_FAILURE);

  int n; /// Number of variables
  int m; /// Number of constraints

  // reads file of the form
  // #nodes #edges
  // e_1 = v_i v_j cost[e_m]
  // ..
  // e_m = v_i v_j cost[e_m]

  /// Read the first line
  infile >> n >> m;
  fprintf(stdout, "n %d, m %d\n", n, m);
  /// Build the graph
  Digraph G(n);

  int v, w;
  cost_t c;
  for (int i = 0; i < m; i++) {
    infile >> v >> w >> c;
    add_edge(v - 1, w - 1, c, G);
  }

  vector<Node> P(n);
  vector<cost_t> D(n, std::numeric_limits<cost_t>::max());
  cost_t T_dist;

  timer TIMER;
  for (int i = 0; i < 50; ++i) {
    double t0 = TIMER.elapsed();
    node_t S = i;
    node_t T = n - 1 - i;
    dijkstra_shortest_paths(G, S, predecessor_map(&P[0]).distance_map(&D[0]));
    T_dist = D[T];
    fprintf(stdout, "Time %.4f Cost %" PRId64 "\n", TIMER.elapsed() - t0,
            T_dist);
  }
  fprintf(stdout, "Tot %.4f\n", TIMER.elapsed());

  return T_dist;
}

///------------------------------------------------------------------------------------------
/// Main function
int main(int argc, char **argv) {
  if (argc != 2) {
    fprintf(stdout, "usage: ./dijkstra <filename>\n");
    exit(EXIT_FAILURE);
  }
  /// Measure overall time
  timer TIMER;
  /// Invoke the different Dijkstra algorithm implementations
  cost_t T_dist = runDijkstra(argv);
  /// Print basic figures
  fprintf(stdout, "Cost %" PRId64 " - Time %.3f\n", T_dist, TIMER.elapsed());

  return 0;
}


/*=============================================================================

  NifTK: A software platform for medical image computing.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "niftkPointUtils.h"
#include <mitkCommon.h>
#include <cmath>
#include <boost/math/special_functions/fpclassify.hpp>

namespace niftk
{

//-----------------------------------------------------------------------------
double CalculateStepSize(double *spacing)
{
  double stepSize = 0;
  double smallestDimension = std::numeric_limits<double>::max();

  for (int i = 0; i< 3; i++)
  {
    if (spacing[i] < smallestDimension)
    {
      smallestDimension = spacing[i];
    }
  }
  stepSize = smallestDimension / 3.0;
  return stepSize;
}

//-----------------------------------------------------------------------------
bool AreDifferent(const mitk::Point3D& a, const mitk::Point3D& b)
{
  bool areDifferent = false;

  for (int i = 0; i < 3; i++)
  {
    if (fabs(a[i] - b[i]) > 0.01)
    {
      areDifferent = true;
      break;
    }
  }

  return areDifferent;
}

//-----------------------------------------------------------------------------
double GetSquaredDistanceBetweenPoints(const mitk::Point3D& a, const mitk::Point3D& b)
{
    double distance = 0;

    for (int i = 0; i < 3; i++)
    {
      distance += (a[i] - b[i])*(a[i] - b[i]);
    }

    return distance;
}

//-----------------------------------------------------------------------------
double GetRMSErrorBetweenPoints(
  const mitk::PointSet& fixedPoints,
  const mitk::PointSet& movingPoints,
  const CoordinateAxesData * const transform)
{
  mitk::PointSet::DataType* itkPointSet = movingPoints.GetPointSet();
  mitk::PointSet::PointsContainer* points = itkPointSet->GetPoints();
  mitk::PointSet::PointsIterator pIt;
  mitk::PointSet::PointIdentifier pointID;
  mitk::PointSet::PointType fixedPoint;
  mitk::PointSet::PointType movingPoint;
  mitk::PointSet::PointType transformedMovingPoint;

  double rmsError = 0;
  unsigned long int numberOfPointsUsed = 0;

  for (pIt = points->Begin(); pIt != points->End(); ++pIt)
  {
    pointID = pIt->Index();
    movingPoint = movingPoints.GetPoint(pointID);

    if (fixedPoints.GetPointIfExists(pointID, &fixedPoint))
    {
      if (transform != NULL)
      {
        transformedMovingPoint = transform->MultiplyPoint(movingPoint);
        rmsError += GetSquaredDistanceBetweenPoints(fixedPoint, transformedMovingPoint);
      }
      else
      {
        rmsError += GetSquaredDistanceBetweenPoints(fixedPoint, movingPoint);
      }
      numberOfPointsUsed++;
    }
  }
  if (numberOfPointsUsed > 0)
  {
    rmsError /= static_cast<double>(numberOfPointsUsed);
    rmsError = sqrt(rmsError);
  }
  else
  {
    rmsError = 0;
  }
  return rmsError;
}

//-----------------------------------------------------------------------------
void GetDifference(const mitk::Point3D& a, const mitk::Point3D& b, mitk::Point3D& output)
{
  for (int i = 0; i < 3; i++)
  {
    output[i] = a[i] - b[i];
  }
}

//-----------------------------------------------------------------------------
double Length(mitk::Point3D& vector)
{
  double length = 0;
  for (int i = 0; i < 3; i++)
  {
    length += vector[i]*vector[i];
  }
  if (length > 0)
  {
    length = sqrt(length);
  }
  return length;
}

//-----------------------------------------------------------------------------
void Normalise(mitk::Point3D& vector)
{
  double length = Length(vector);
  if (length > 0)
  {
    for (int i = 0; i < 3; i++)
    {
      vector[i] /= length;
    }
  }
}

//-----------------------------------------------------------------------------
double FindLargestDistanceBetweenTwoPoints(const mitk::PointSet& input)
{
  double maxSquaredDistance = 0;

  mitk::PointSet::PointsContainer* inputContainer = input.GetPointSet()->GetPoints();
  mitk::PointSet::PointsConstIterator outerIt = inputContainer->Begin();
  mitk::PointSet::PointsConstIterator innerIt = inputContainer->Begin();
  mitk::PointSet::PointsConstIterator iterEnd = inputContainer->End();

  for ( ; outerIt != iterEnd; ++outerIt)
  {
    for ( ; innerIt != iterEnd; ++innerIt)
    {
      double squaredDistance = GetSquaredDistanceBetweenPoints(input.GetPoint(outerIt->Index()), input.GetPoint(innerIt->Index()));
      if (squaredDistance > maxSquaredDistance)
      {
        maxSquaredDistance = squaredDistance;
      }
    }
  }
  return sqrt(maxSquaredDistance);
}

//-----------------------------------------------------------------------------
int CopyPointSets(const mitk::PointSet& input, mitk::PointSet& output)
{
  output.Clear();

  mitk::PointSet::PointsContainer* inputContainer = input.GetPointSet()->GetPoints();
  mitk::PointSet::PointsConstIterator inputIt = inputContainer->Begin();
  mitk::PointSet::PointsConstIterator inputEnd = inputContainer->End();
  for ( ; inputIt != inputEnd; ++inputIt)
  {
    output.InsertPoint(inputIt->Index(), input.GetPoint(inputIt->Index()));
  }
  return output.GetSize();
}

//-----------------------------------------------------------------------------
void ScalePointSets(const mitk::PointSet& input, mitk::PointSet& output, double scaleFactor)
{
  output.Clear();

  mitk::PointSet::PointsContainer* inputContainer = input.GetPointSet()->GetPoints();
  mitk::PointSet::PointsConstIterator inputIt = inputContainer->Begin();
  mitk::PointSet::PointsConstIterator inputEnd = inputContainer->End();
  mitk::PointSet::PointType point;

  for ( ; inputIt != inputEnd; ++inputIt)
  {
    point = input.GetPoint(inputIt->Index());
    point[0] *= scaleFactor;
    point[1] *= scaleFactor;
    point[2] *= scaleFactor;

    output.InsertPoint(inputIt->Index(), point);
  }
}

//-----------------------------------------------------------------------------
void CopyValues(const mitk::Point3D& a, mitk::Point3D& b)
{
  for (int i = 0; i < 3; i++)
  {
    b[i] = a[i];
  }
}

//-----------------------------------------------------------------------------
void CrossProduct(const mitk::Point3D& a, const mitk::Point3D& b, mitk::Point3D& c)
{
  mitk::Point3D aCopy;
  mitk::Point3D bCopy;
  CopyValues(a, aCopy);
  CopyValues(b, bCopy);
  Normalise(aCopy);
  Normalise(bCopy);

  c[0] = aCopy[1]*bCopy[2] - bCopy[1]*aCopy[2];
  c[1] = -1 * (aCopy[0]*bCopy[2] - bCopy[0]*aCopy[2]);
  c[2] = aCopy[0]*bCopy[1] - bCopy[0]*aCopy[1];
}

//-----------------------------------------------------------------------------
void ComputeNormalFromPoints(const mitk::Point3D& a, const mitk::Point3D& b, const mitk::Point3D& c, mitk::Point3D& output)
{
  mitk::Point3D aMinusB;
  mitk::Point3D cMinusB;
  GetDifference(a, b, aMinusB);
  GetDifference(c, b, cMinusB);
  CrossProduct(aMinusB, cMinusB, output);
  Normalise(output);
}

//-----------------------------------------------------------------------------
void TransformPointByVtkMatrix(
    const vtkMatrix4x4* matrix,
    const bool& isNormal,
    mitk::Point3D& point
    )
{
  double transformedPoint[4] = {0, 0, 0, 1};
  vtkMatrix4x4* nonConstMatrix = const_cast<vtkMatrix4x4*>(matrix);

  if(nonConstMatrix != NULL)
  {
    transformedPoint[0] = point[0];
    transformedPoint[1] = point[1];
    transformedPoint[2] = point[2];
    transformedPoint[3] = 1;

    nonConstMatrix->MultiplyPoint(transformedPoint, transformedPoint);

    point[0] = transformedPoint[0];
    point[1] = transformedPoint[1];
    point[2] = transformedPoint[2];

    if (isNormal)
    {
      double transformedOrigin[4] = {0, 0, 0, 1};
      nonConstMatrix->MultiplyPoint(transformedOrigin, transformedOrigin);

      point[0] = point[0] - transformedOrigin[0];
      point[1] = point[1] - transformedOrigin[1];
      point[2] = point[2] - transformedOrigin[2];
    }
  }
}

//-----------------------------------------------------------------------------
void TransformPointsByVtkMatrix(
    const mitk::PointSet& input,
    const vtkMatrix4x4& matrix,
    mitk::PointSet& output
    )
{
  mitk::PointSet::DataType* itkPointSet = input.GetPointSet();
  mitk::PointSet::PointsContainer* points = itkPointSet->GetPoints();
  mitk::PointSet::PointsIterator pIt;
  mitk::PointSet::PointIdentifier pointID;
  mitk::PointSet::PointType point;

  output.Clear();

  for (pIt = points->Begin(); pIt != points->End(); ++pIt)
  {
    pointID = pIt->Index();
    point = input.GetPoint(pointID);
    TransformPointByVtkMatrix(&matrix, false, point);
    output.InsertPoint(pointID, point);
  }
}

//-----------------------------------------------------------------------------
int FilterMatchingPoints(
    const mitk::PointSet& fixedPointsIn,
    const mitk::PointSet& movingPointsIn,
    mitk::PointSet& fixedPointsOut,
    mitk::PointSet& movingPointsOut
    )
{
  int matchedPoints = 0;
  fixedPointsOut.Clear();
  movingPointsOut.Clear();

  mitk::PointSet::DataType* fixedPointSet = fixedPointsIn.GetPointSet(0);
  mitk::PointSet::PointsContainer* fixedPoints = fixedPointSet->GetPoints();
  mitk::PointSet::DataType* movingPointSet = movingPointsIn.GetPointSet(0);
  mitk::PointSet::PointsContainer* movingPoints = movingPointSet->GetPoints();

  mitk::PointSet::PointsIterator fixedPointsIt;
  mitk::PointSet::PointsIterator movingPointsIt;

  mitk::PointSet::PointIdentifier pointID;
  mitk::PointSet::PointType fixedPoint;
  mitk::PointSet::PointType movingPoint;

  for (fixedPointsIt = fixedPoints->Begin(); fixedPointsIt != fixedPoints->End(); ++fixedPointsIt)
  {
    pointID = fixedPointsIt->Index();
    fixedPoint = fixedPointsIn.GetPoint(pointID);

    for (movingPointsIt = movingPoints->Begin(); movingPointsIt != movingPoints->End(); ++movingPointsIt)
    {
      if (movingPointsIt->Index() == pointID)
      {
        movingPoint = movingPointsIn.GetPoint(pointID);

        fixedPointsOut.InsertPoint(pointID, fixedPoint);
        movingPointsOut.InsertPoint(pointID, movingPoint);
        matchedPoints++;
      }
    }
  }

  return matchedPoints;
}

//-----------------------------------------------------------------------------
int RemoveNaNPoints(
    const mitk::PointSet& pointsIn,
    mitk::PointSet& pointsOut
    )
{
  int removedPoints = 0;
  pointsOut.Clear();

  mitk::PointSet::DataType* pointSet = pointsIn.GetPointSet(0);
  mitk::PointSet::PointsContainer* points = pointSet->GetPoints();

  mitk::PointSet::PointsIterator pointsIt;

  mitk::PointSet::PointIdentifier pointID;
  mitk::PointSet::PointType point;

  for (pointsIt = points->Begin(); pointsIt != points->End(); ++pointsIt)
  {
    pointID = pointsIt->Index();
    point = pointsIn.GetPoint(pointID);

    if ( CheckForNaNPoint(point) )
    {
      removedPoints++;
    }
    else
    {
      pointsOut.InsertPoint(pointID, point);
    }
  }
  return removedPoints;
}

//-----------------------------------------------------------------------------
bool CheckForNaNPoint( const mitk::PointSet::PointType& point )
{
  if ( boost::math::isnan( point[0] ) || boost::math::isnan( point[1] ) || boost::math::isnan( point[2] ))
  {
    return true;
  }
  return false;
}

//-----------------------------------------------------------------------------
mitk::Point3D ComputeCentroid(const mitk::PointSet& input)
{
  mitk::Point3D average;
  average.Fill(0);

  if (input.GetSize() > 0)
  {
    mitk::PointSet::DataType* pointSet = input.GetPointSet(0);
    mitk::PointSet::PointsContainer* points = pointSet->GetPoints();
    mitk::PointSet::PointsIterator pointsIt;
    mitk::PointSet::PointType point;

    for (pointsIt = points->Begin(); pointsIt != points->End(); ++pointsIt)
    {
      point = input.GetPoint(pointsIt->Index());
      average[0] += point[0];
      average[1] += point[1];
      average[2] += point[2];
    }

    double numberOfPoints = static_cast<double>(input.GetSize());

    average[0] /= numberOfPoints;
    average[1] /= numberOfPoints;
    average[2] /= numberOfPoints;
  }

  return average;
}

}


#include "RBGL.hpp"
#include <boost/graph/graph_utility.hpp>

using namespace boost;

typedef adjacency_list<vecS, vecS, undirectedS,
	// vertex properties
        property<vertex_index_t, int,
    	property<vertex_centrality_t, double> >,
    	// edge properties
    	property<edge_weight_t, double,
    	property<edge_centrality_t, double> > >
    	BCGraph;

// Explicit instantiation of template function max_element circumvents
// compiler error in llvm8 libc++
typedef graph_traits<BCGraph>::edge_descriptor Edge;
typedef graph_traits<BCGraph>::edge_iterator EdgeIterator;
typedef property_map < BCGraph, edge_centrality_t >::type EdgeCentralityMap;
typedef typename property_traits<EdgeCentralityMap>::value_type centrality_type;
typedef indirect_cmp<EdgeCentralityMap, std::less<centrality_type> > EdgeCentralityCompare;

EdgeIterator
max_element(EdgeIterator __first, EdgeIterator __last, EdgeCentralityCompare __comp)
{
    if (__first != __last)
    {
        EdgeIterator __i = __first;
        while (++__i != __last)
            if (__comp(*__first, *__i))
                __first = __i;
    }
    return __first;
}

#include <boost/graph/bc_clustering.hpp>
#include <boost/graph/betweenness_centrality.hpp>

extern "C"
{

	SEXP BGL_brandes_betweenness_centrality(SEXP num_verts_in,
		SEXP num_edges_in, SEXP R_edges_in, SEXP R_weights_in)
        {
    		BCGraph g;

		int NV = Rf_asInteger(num_verts_in);
		int NE = Rf_asInteger(num_edges_in);
		int* edges_in = INTEGER(R_edges_in);
		double* weights_in = REAL(R_weights_in);

		for (int i = 0; i < NE ; i++, edges_in += 2, weights_in++)
		    boost::add_edge(*edges_in, *(edges_in+1), *weights_in, g);

		SEXP anslst, bcvlst, enlst, bcelst, rbcvlst, dom;
		PROTECT(anslst = Rf_allocVector(VECSXP,5));
		PROTECT(bcvlst = Rf_allocMatrix(REALSXP, 1, NV));
		PROTECT(enlst = Rf_allocMatrix(INTSXP, 2, NE));
		PROTECT(bcelst = Rf_allocMatrix(REALSXP, 1, NE));
		PROTECT(rbcvlst = Rf_allocMatrix(REALSXP, 1, NV));
		PROTECT(dom = Rf_allocVector(REALSXP, 1));

		brandes_betweenness_centrality(g,
                        centrality_map(get(vertex_centrality, g)).
			edge_centrality_map(get(edge_centrality, g)).
			weight_map(get(edge_weight, g)));

                property_map<BCGraph, vertex_centrality_t>::type
			v_map = get(vertex_centrality, g);
                property_map<BCGraph, edge_centrality_t>::type
			e_map = get(edge_centrality, g);

                graph_traits < BCGraph>::vertex_iterator vi, v_end;
                graph_traits < BCGraph>::edge_iterator ei, e_end;

                int v = 0, e = 0;

		for ( tie(vi, v_end) = vertices(g); vi != v_end; vi++ )
                    REAL(bcvlst)[v++] = v_map[*vi];
		for ( v = 0, tie(ei, e_end) = edges(g); ei != e_end ; ei++ )
		{
		    INTEGER(enlst)[v++] = source(*ei, g);
		    INTEGER(enlst)[v++] = target(*ei, g);
                    REAL(bcelst)[e++] = e_map[*ei];
                }

		relative_betweenness_centrality(g, get(vertex_centrality, g));
                v_map = get(vertex_centrality, g);

		for ( v = 0, tie(vi, v_end) = vertices(g); vi != v_end; vi++ )
                    REAL(rbcvlst)[v++] = v_map[*vi];

		double dominance = central_point_dominance(g,
		                  get(vertex_centrality, g));

		REAL(dom)[0] = dominance;

		SET_VECTOR_ELT(anslst,0,bcvlst);
		SET_VECTOR_ELT(anslst,1,bcelst);
		SET_VECTOR_ELT(anslst,2,rbcvlst);
		SET_VECTOR_ELT(anslst,3,dom);
		SET_VECTOR_ELT(anslst,4,enlst);
		UNPROTECT(6);
		return(anslst);
	}

	class clustering_threshold : public bc_clustering_threshold<double>
	{
		typedef bc_clustering_threshold<double> inherited;

	public:
	        clustering_threshold(double threshold, const BCGraph& g, bool normalize)
		: inherited(threshold, g, normalize), iter(1) { }

	        bool operator()(double max_centrality, Edge e, const BCGraph& g)
	        {
#if DEBUG
                  std::cout << "Iter: " << iter << " Max Centrality: "
                       << (max_centrality / dividend) << std::endl;
#endif
		  ++iter;
		  return inherited::operator()(max_centrality, e, g);
	        }

	private:
		 unsigned int iter;
	};

	SEXP BGL_betweenness_centrality_clustering (SEXP num_verts_in,
		SEXP num_edges_in, SEXP R_edges_in, SEXP R_weights_in,
		SEXP R_threshold,  SEXP R_normalize)
        {
    		BCGraph g;

		int NE = Rf_asInteger(num_edges_in);
		int* edges_in = INTEGER(R_edges_in);
		double* weights_in = REAL(R_weights_in);

		for (int i = 0; i < NE ; i++, edges_in += 2, weights_in++)
		    boost::add_edge(*edges_in, *(edges_in+1), *weights_in, g);

		double threshold = REAL(R_threshold)[0];
		bool normalize = LOGICAL(R_normalize)[0];

		betweenness_centrality_clustering(g,
			clustering_threshold(threshold, g, normalize),
			get(edge_centrality, g));

		// betweenness_centrality_clustering(g,
		// 	clustering_threshold(threshold, g, normalize));

		SEXP anslst, cnt, bcvlst, bcelst;
		PROTECT(anslst = Rf_allocVector(VECSXP,3));
		PROTECT(cnt = Rf_allocVector(INTSXP, 1));
		PROTECT(bcvlst = Rf_allocMatrix(INTSXP, 2, num_edges(g)));
		PROTECT(bcelst = Rf_allocMatrix(REALSXP, 1, num_edges(g)));

		INTEGER(cnt)[0] = num_edges(g);

		property_map < BCGraph, edge_centrality_t >::type
		         ec = get(edge_centrality, g);

		typedef graph_traits<BCGraph>::edge_iterator   edge_iterator;
		edge_iterator ei, e_end;

#if DEBUG
                std::cout << " edge centralities: ";
		for ( tie(ei, e_end) = edges(g); ei != e_end; ++ei )
                        std::cout << " " << ec[*ei];
                std::cout << std::endl;
#endif

		int i = 0, j = 0;
		for ( tie(ei, e_end) = edges(g); ei != e_end; ++ei )
		{
			INTEGER(bcvlst)[i++] = source(*ei, g);
			INTEGER(bcvlst)[i++] = target(*ei, g);
			REAL(bcelst)[j++] = ec[*ei];
		}

		SET_VECTOR_ELT(anslst,0,cnt);
		SET_VECTOR_ELT(anslst,1,bcvlst);
		SET_VECTOR_ELT(anslst,2,bcelst);
		UNPROTECT(4);
		return(anslst);
	}
}


#include "software/new_geom/polynomial1d.h"

#include <Eigen/Dense>
#include <Eigen/QR>
#include <list>
#include <stdexcept>

#include "software/new_geom/geom_constants.h"

Polynomial1d::Polynomial1d() {}

Polynomial1d::Polynomial1d(const std::vector<double> &coeffs) : coeffs(coeffs) {}

Polynomial1d::Polynomial1d(const std::initializer_list<double> &coeffs)
    : coeffs(std::vector<double>(coeffs))
{
}

Polynomial1d::Polynomial1d(const std::vector<Polynomial1d::Constraint> constraints)
{
    // Check that we have at least two constraints
    if (constraints.size() < 2)
    {
        throw std::invalid_argument(
            "Less then two constraints given, so no unique polynomial solution.");
    }

    // Check that all inputs are unique
    for (size_t i = 0; i < constraints.size(); i++)
    {
        for (size_t j = i + 1; j < constraints.size(); j++)
        {
            if (constraints[i].input == constraints[j].input)
            {
                throw std::invalid_argument(
                    "At least two inputs were equal, does not define a valid set of constraints");
            }
        }
    }

    // Solve for the coefficients
    Eigen::MatrixXd A(constraints.size(), constraints.size());
    Eigen::VectorXd b(constraints.size());

    for (size_t row_index = 0; row_index < constraints.size(); row_index++)
    {
        for (size_t col_index = 0; col_index < constraints.size(); col_index++)
        {
            A(row_index, col_index) =
                std::pow(constraints[row_index].input, static_cast<double>(col_index));
            b(row_index) = constraints[row_index].output;
        }
    }

    const Eigen::VectorXd coeff_vector = A.fullPivLu().solve(b);

    for (size_t i = 0; i < constraints.size(); i++)
    {
        coeffs.emplace_back(coeff_vector(i));
    }
}

double Polynomial1d::getCoeff(unsigned int order) const
{
    if (order >= coeffs.size())
    {
        return 0;
    }
    else
    {
        return coeffs[order];
    }
}

void Polynomial1d::setCoeff(unsigned int order, double coeff)
{
    if (order >= coeffs.size())
    {
        coeffs.resize(order + 1, 0);
    }
    coeffs[order] = coeff;
}

unsigned int Polynomial1d::getOrder() const
{
    if (coeffs.size() != 0)
    {
        for (unsigned int i = coeffs.size(); i > 0; i--)
        {
            if (std::abs(coeffs[i - 1]) >= GeomConstants::FIXED_EPSILON)
            {
                return i - 1;
            }
        }
    }
    // Zero polynomial treated as an order zero polynomial
    return 0;
}

double Polynomial1d::valueAt(double val) const
{
    // Horner's Method:
    // https://www.geeksforgeeks.org/horners-method-polynomial-evaluation/
    unsigned int order = getOrder();
    double retval      = getCoeff(order);
    for (unsigned int i = 1; i <= order; i++)
    {
        retval = retval * val + getCoeff(order - i);
    }
    return retval;
}

Polynomial1d operator+(const Polynomial1d &p1, const Polynomial1d &p2)
{
    Polynomial1d sum;
    unsigned int max_order = std::max(p1.getOrder(), p2.getOrder());
    for (unsigned int i = 0; i <= max_order; i++)
    {
        sum.setCoeff(i, p1.getCoeff(i) + p2.getCoeff(i));
    }
    return sum;
}

Polynomial1d operator-(const Polynomial1d &p1, const Polynomial1d &p2)
{
    Polynomial1d difference;
    unsigned int max_order = std::max(p1.getOrder(), p2.getOrder());
    for (unsigned int i = 0; i <= max_order; i++)
    {
        difference.setCoeff(i, p1.getCoeff(i) - p2.getCoeff(i));
    }
    return difference;
}

Polynomial1d operator*(const Polynomial1d &p1, const Polynomial1d &p2)
{
    Polynomial1d product;
    unsigned int p1_order = p1.getOrder();
    unsigned int p2_order = p2.getOrder();
    for (unsigned int i = 0; i <= p1_order; i++)
    {
        for (unsigned int j = 0; j <= p2_order; j++)
        {
            product.setCoeff(i + j,
                             product.getCoeff(i + j) + (p1.getCoeff(i) * p2.getCoeff(j)));
        }
    }
    return product;
}

Polynomial1d &operator+=(Polynomial1d &p1, const Polynomial1d &p2)
{
    return p1 = p1 + p2;
}

Polynomial1d &operator-=(Polynomial1d &p1, const Polynomial1d &p2)
{
    return p1 = p1 - p2;
}

Polynomial1d &operator*=(Polynomial1d &p1, const Polynomial1d &p2)
{
    return p1 = p1 * p2;
}

bool operator==(const Polynomial1d &p1, const Polynomial1d &p2)
{
    unsigned int p1_order = p1.getOrder();
    unsigned int p2_order = p2.getOrder();
    if (p1_order != p2_order)
    {
        return false;
    }
    for (unsigned int i = 0; i < p1_order; i++)
    {
        if (std::abs(p1.getCoeff(i) - p2.getCoeff(i)) >= GeomConstants::FIXED_EPSILON)
        {
            return false;
        }
    }
    return true;
}


//  Boost common_factor_ct.hpp header file  ----------------------------------//

//  (C) Copyright Daryle Walker and Stephen Cleary 2001-2002.
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_MATH_COMMON_FACTOR_CT_HPP
#define BOOST_MATH_COMMON_FACTOR_CT_HPP

#include <boost/math_fwd.hpp>  // self include
#include <boost/config.hpp>  // for BOOST_STATIC_CONSTANT, etc.
#include <boost/mpl/integral_c.hpp>

namespace boost
{
namespace math
{

//  Implementation details  --------------------------------------------------//

namespace detail
{
#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    // Build GCD with Euclid's recursive algorithm
    template < static_gcd_type Value1, static_gcd_type Value2 >
    struct static_gcd_helper_t
    {
    private:
        BOOST_STATIC_CONSTANT( static_gcd_type, new_value1 = Value2 );
        BOOST_STATIC_CONSTANT( static_gcd_type, new_value2 = Value1 % Value2 );

        #ifndef __BORLANDC__
        #define BOOST_DETAIL_GCD_HELPER_VAL(Value) static_cast<static_gcd_type>(Value)
        #else
        typedef static_gcd_helper_t  self_type;
        #define BOOST_DETAIL_GCD_HELPER_VAL(Value)  (self_type:: Value )
        #endif

        typedef static_gcd_helper_t< BOOST_DETAIL_GCD_HELPER_VAL(new_value1),
         BOOST_DETAIL_GCD_HELPER_VAL(new_value2) >  next_step_type;

        #undef BOOST_DETAIL_GCD_HELPER_VAL

    public:
        BOOST_STATIC_CONSTANT( static_gcd_type, value = next_step_type::value );
    };

    // Non-recursive case
    template < static_gcd_type Value1 >
    struct static_gcd_helper_t< Value1, 0UL >
    {
        BOOST_STATIC_CONSTANT( static_gcd_type, value = Value1 );
    };
#else
    // Use inner class template workaround from Peter Dimov
    template < static_gcd_type Value1 >
    struct static_gcd_helper2_t
    {
        template < static_gcd_type Value2 >
        struct helper
        {
            BOOST_STATIC_CONSTANT( static_gcd_type, value
             = static_gcd_helper2_t<Value2>::BOOST_NESTED_TEMPLATE
             helper<Value1 % Value2>::value );
        };

        template <  >
        struct helper< 0UL >
        {
            BOOST_STATIC_CONSTANT( static_gcd_type, value = Value1 );
        };
    };

    // Special case
    template <  >
    struct static_gcd_helper2_t< 0UL >
    {
        template < static_gcd_type Value2 >
        struct helper
        {
            BOOST_STATIC_CONSTANT( static_gcd_type, value = Value2 );
        };
    };

    // Build the GCD from the above template(s)
    template < static_gcd_type Value1, static_gcd_type Value2 >
    struct static_gcd_helper_t
    {
        BOOST_STATIC_CONSTANT( static_gcd_type, value
         = static_gcd_helper2_t<Value1>::BOOST_NESTED_TEMPLATE
         helper<Value2>::value );
    };
#endif

#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
    // Build the LCM from the GCD
    template < static_gcd_type Value1, static_gcd_type Value2 >
    struct static_lcm_helper_t
    {
        typedef static_gcd_helper_t<Value1, Value2>  gcd_type;

        BOOST_STATIC_CONSTANT( static_gcd_type, value = Value1 / gcd_type::value
         * Value2 );
    };

    // Special case for zero-GCD values
    template < >
    struct static_lcm_helper_t< 0UL, 0UL >
    {
        BOOST_STATIC_CONSTANT( static_gcd_type, value = 0UL );
    };
#else
    // Adapt GCD's inner class template workaround for LCM
    template < static_gcd_type Value1 >
    struct static_lcm_helper2_t
    {
        template < static_gcd_type Value2 >
        struct helper
        {
            typedef static_gcd_helper_t<Value1, Value2>  gcd_type;

            BOOST_STATIC_CONSTANT( static_gcd_type, value = Value1
             / gcd_type::value * Value2 );
        };

        template <  >
        struct helper< 0UL >
        {
            BOOST_STATIC_CONSTANT( static_gcd_type, value = 0UL );
        };
    };

    // Special case
    template <  >
    struct static_lcm_helper2_t< 0UL >
    {
        template < static_gcd_type Value2 >
        struct helper
        {
            BOOST_STATIC_CONSTANT( static_gcd_type, value = 0UL );
        };
    };

    // Build the LCM from the above template(s)
    template < static_gcd_type Value1, static_gcd_type Value2 >
    struct static_lcm_helper_t
    {
        BOOST_STATIC_CONSTANT( static_gcd_type, value
         = static_lcm_helper2_t<Value1>::BOOST_NESTED_TEMPLATE
         helper<Value2>::value );
    };
#endif

}  // namespace detail

//  Compile-time greatest common divisor evaluator class declaration  --------//

template < static_gcd_type Value1, static_gcd_type Value2 >
struct static_gcd : public mpl::integral_c<static_gcd_type, (detail::static_gcd_helper_t<Value1, Value2>::value) >
{
};  // boost::math::static_gcd

//  Compile-time least common multiple evaluator class declaration  ----------//

template < static_gcd_type Value1, static_gcd_type Value2 >
struct static_lcm : public mpl::integral_c<static_gcd_type, (detail::static_lcm_helper_t<Value1, Value2>::value) >
{
};  // boost::math::static_lcm

}  // namespace math
}  // namespace boost

#endif  // BOOST_MATH_COMMON_FACTOR_CT_HPP


/*
Copyright (c) 2016 Bastien Durix

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

/**
 *  \file DistanceField.cpp
 *  \brief Defines functions related to distance field
 *  \author Bastien Durix
 */

#include "DistanceField.h"
#include <boost/math/distributions/normal.hpp>
#include <set>
#include <nlopt.hpp>
#include <iostream>

unsigned int algorithm::skeletonization::closestInd(const boundary::DiscreteBoundary<2>::Ptr disbnd, const Eigen::Vector2d &C)
{
	double distmin = 0.0;
	unsigned int ind = 0;
	for(unsigned int i = 0; i < disbnd->getNbVertices(); i++)
	{
		Eigen::Vector2d vec = disbnd->getCoordinates(i) - C;
		double dist = vec.norm();

		if(dist < distmin || i == 0)
		{
			distmin = dist;
			ind = i;
		}
	}

	return ind;
}

double algorithm::skeletonization::fieldValue(const boundary::DiscreteBoundary<2>::Ptr disbnd, const Eigen::Vector2d &C, double noise)
{
	boost::math::normal norm(0.0,noise);

	std::vector<double> dists(disbnd->getNbVertices());

	double distmin = 0.0;
	for(unsigned int i = 0; i < dists.size(); i++)
	{
		Eigen::Vector2d vec = disbnd->getCoordinates(i) - C;
		double dist = vec.norm();

		if(dist < distmin || i == 0) distmin = dist;

		dists[i] = dist;
	}

	double denom = 0.0;
	double numer = 0.0;

	for(unsigned int i = 0; i < dists.size(); i++)
	{
		double diff = dists[i] - distmin;
		double proba = 2.0*(1.0-boost::math::cdf(norm,diff));

		numer += proba * dists[i];

		denom += proba;
	}

	double val = numer/denom;

	if(val - distmin > 2.0)
	std::cout << val - distmin << std::endl;

	return val;
}

double algorithm::skeletonization::fieldValue(const boundary::DiscreteBoundary<2>::Ptr disbnd, const Eigen::Vector2d &C, Eigen::Vector2d &grad, double noise)
{
	boost::math::normal norm(0.0,noise);

	std::vector<double> dists(disbnd->getNbVertices());

	double distmin = 0.0;
	for(unsigned int i = 0; i < dists.size(); i++)
	{
		Eigen::Vector2d vec = disbnd->getCoordinates(i) - C;
		double dist = vec.norm();

		if(dist < distmin || i == 0) distmin = dist;

		dists[i] = dist;
	}

	double denom = 0.0;
	double numer = 0.0;
	Eigen::Vector2d numvec(0.0,0.0);

	for(unsigned int i = 0; i < dists.size(); i++)
	{
		Eigen::Vector2d vec = disbnd->getCoordinates(i) - C;
		double diff = dists[i] - distmin;
		double proba = 2.0*(1.0-boost::math::cdf(norm,diff));

		numer += proba * dists[i];

		numvec += proba*vec.normalized();

		denom += proba;
	}

	double val = numer/denom;

	if(val - distmin > 2.0)
	std::cout << val - distmin << std::endl;

	grad = numvec * (1.0/denom);

	return val;
}

void algorithm::skeletonization::tangencyBoundary(const boundary::DiscreteBoundary<2>::Ptr disbnd,
												  const Eigen::Vector2d &C,
												  const double &rad,
												  double noise,
												  std::vector<std::list<unsigned int> > &v_ind,
												  double fac)
{
	std::vector<bool> used(disbnd->getNbVertices(),false);
	std::vector<double> val(disbnd->getNbVertices());
	std::vector<bool> isin(disbnd->getNbVertices(),false);
	for(unsigned int i = 0; i < disbnd->getNbVertices(); i++)
	{
		val[i] = (disbnd->getVertex(i) - C).norm() - rad;
		if(val[i] < noise) isin[i] = true;
	}

	bool nochange = false;

	do
	{
		nochange = true;
		for(unsigned int i = 0; i < disbnd->getNbVertices(); i++)
		{
			if(!isin[i])
			{
				unsigned int prev = disbnd->getPrev(i);
				unsigned int next = disbnd->getNext(i);
				if((isin[prev] || isin[next]) && val[i] < fac)
				{
					isin[i] = true;
					nochange = false;
				}
			}
		}
	}while(!nochange);

	std::list<std::list<unsigned int> > l_ind;
	for(unsigned int i = 0; i < disbnd->getNbVertices(); i++)
	{
		if(isin[i] && !used[i])
		{
			std::list<unsigned int> cind;

			used[i] = true;
			unsigned int indn = i;
			do
			{
				cind.push_back(indn);
				used[indn] = true;
				indn = disbnd->getNext(indn);
			}while(isin[indn] && indn != *(cind.begin()));

			unsigned int indp = i;
			cind.pop_front();
			do
			{
				cind.push_front(indp);
				used[indp] = true;
				indp = disbnd->getPrev(indp);
			}while(isin[indp] && indp != *(cind.rbegin()));
			if(indp == *(cind.rbegin()))
				cind.push_front(indp);

			while(val[*(cind.begin())] > noise)
				cind.pop_front();
			while(val[*(cind.rbegin())] > noise)
				cind.pop_back();
			if(cind.size() > 1)
				l_ind.push_back(cind);
		}
	}

	l_ind.sort(
		[disbnd,C](const std::list<unsigned int> &c1, const std::list<unsigned int> &c2)
		{
			Eigen::Vector2d p1 = disbnd->getVertex(*(c1.begin())) - C;
			Eigen::Vector2d p2 = disbnd->getVertex(*(c2.begin())) - C;
			double ang1 = atan2(p1.y(),p1.x());
			double ang2 = atan2(p2.y(),p2.x());
			return ang1 < ang2;
		});
	v_ind = std::vector<std::list<unsigned int> >(l_ind.begin(),l_ind.end());
}

struct DataClosestCenter2d
{
	const boundary::DiscreteBoundary<2>::Ptr &disbnd;
	double noise;

	DataClosestCenter2d(
		const boundary::DiscreteBoundary<2>::Ptr &disbnd_,
		double noise_) :
		disbnd(disbnd_), noise(noise_) {};
};

double minClosestCenter2d(const std::vector<double> &vt, std::vector<double> &, void *dataFun)
{
	DataClosestCenter2d *data = (DataClosestCenter2d*) dataFun;

	Eigen::Vector2d C(vt[0],vt[1]);

	Eigen::Vector2d grad;
	algorithm::skeletonization::fieldValue(data->disbnd, C, grad, data->noise);
	//return -algorithm::skeletonization::recFieldValue(data->vdisbnd, data->vdisshp, data->vcam, C, data->noise);
	return grad.squaredNorm();
}

bool algorithm::skeletonization::closestCenter(const boundary::DiscreteBoundary<2>::Ptr disbnd, Eigen::Vector2d &C, double noise)
{
	unsigned int nbiter = 10000;

	bool fini = false;
	do
	{
		Eigen::Vector2d grad;
		fieldValue(disbnd,C,grad,noise);
		fini = grad.norm() < 0.01*noise;
		if(!fini) C -= grad * noise * 0.1;
		nbiter--;
	}while(nbiter != 0 && !fini);

	std::vector<double> lb(2);
	std::vector<double> ub(2);
	std::vector<double> C_init(2);

	Eigen::Vector2d grad;
	double rad = fieldValue(disbnd,C,grad,noise);

	C_init[0] = C.x();
	C_init[1] = C.y();

	lb[0] = C.x() - rad*0.2;
	lb[1] = C.y() - rad*0.2;

	ub[0] = C.x() + rad*0.2;
	ub[1] = C.y() + rad*0.2;

	DataClosestCenter2d data(disbnd,noise);

	nlopt::opt opt(nlopt::LN_COBYLA, 2);
	opt.set_lower_bounds(lb);
	opt.set_upper_bounds(ub);

	opt.set_min_objective(minClosestCenter2d, &data);

	opt.set_xtol_rel(1e-2);

	double res;
	opt.optimize(C_init, res);
	C.x() = C_init[0];
	C.y() = C_init[1];

	return true;
}

void algorithm::skeletonization::closestCenterOnArc(const boundary::DiscreteBoundary<2>::Ptr disbnd,
													const Eigen::Vector2d &C,
													const std::pair<double,double> &pang,
													double dist,
													Eigen::Vector2d &Cmov,
													double noise)
{
	double step = noise / (dist*dist);

	double ang1 = pang.first;
	double ang2 = pang.second;

	Eigen::Vector2d P1 = C + dist*Eigen::Vector2d(cos(ang1),sin(ang1));
	Eigen::Vector2d P2 = C + dist*Eigen::Vector2d(cos(ang2),sin(ang2));

	while(ang2 - ang1 > step)
	{
		double angmid = (ang1 + ang2) / 2.0;

		Eigen::Vector2d grad;
		Eigen::Vector2d vec(cos(angmid),sin(angmid));
		Cmov = C + dist * vec;
		fieldValue(disbnd,Cmov,grad,noise);

		//double der = dist*(grad.y() * vec.x() - grad.x() * vec.y());
		//if(der < 0.0) ang1 = angmid;
		//if(der > 0.0) ang2 = angmid;

		Eigen::Vector2d vec1 = (P1 - Cmov).normalized();
		Eigen::Vector2d vec2 = (P2 - Cmov).normalized();
		double sim1 = grad.dot(vec1);
		double sim2 = grad.dot(vec2);
		if(sim1 > sim2) ang1 = angmid;
		if(sim2 > sim1) ang2 = angmid;
	}

	double angmid = (ang1 + ang2) / 2.0;

	Eigen::Vector2d vec(cos(angmid),sin(angmid));
	Cmov = C + dist * vec;
}

void algorithm::skeletonization::discontinuitiesOnArc(const boundary::DiscreteBoundary<2>::Ptr disbnd,
													  const Eigen::Vector2d &C,
													  const std::pair<double,double> &pang,
													  double dist,
													  std::vector<Eigen::Vector2d> &vecC,
													  std::vector<std::pair<unsigned int,unsigned int> > &vecInd,
													  double noise)
{
	std::list<Eigen::Vector2d> lisC;
	std::list<std::pair<unsigned int,unsigned int> > lisInd;
	double step = noise / (dist*dist);

	double ang2 = pang.second;
	Eigen::Vector2d P2 = C + dist*Eigen::Vector2d(cos(ang2),sin(ang2));
	unsigned int ind2 = closestInd(disbnd,P2);

	double ang1 = pang.first;
	Eigen::Vector2d P1 = C + dist*Eigen::Vector2d(cos(ang1),sin(ang1));
	unsigned int ind1 = closestInd(disbnd,P1);

	//std::cout << "disc " << ind1 << " ";

	while(ind1 != ind2)
	{
		while(ang2 - ang1 > step)
		{
			double angmid = (ang1 + ang2) / 2.0;
			Eigen::Vector2d vec(cos(angmid),sin(angmid));
			Eigen::Vector2d Cmov = C + dist * vec;

			unsigned int indmid = closestInd(disbnd,Cmov);

			if(indmid == ind1)
			{
				ang1 = angmid;
			}
			else
			{
				ang2 = angmid;
			}
		}

		Eigen::Vector2d P1 = C + dist*Eigen::Vector2d(cos(ang1),sin(ang1));
		unsigned int ind1b = closestInd(disbnd,P1);
		Eigen::Vector2d P2 = C + dist*Eigen::Vector2d(cos(ang2),sin(ang2));
		unsigned int ind2b = closestInd(disbnd,P2);

		double angmid = (ang1 + ang2) / 2.0;
		Eigen::Vector2d vec(cos(angmid),sin(angmid));
		Eigen::Vector2d Cmov = C + dist * vec;

		lisC.push_back(Cmov);
		lisInd.push_back(std::make_pair(ind1b,ind2b));

		ind1 = ind2b;
		ang1 = ang2;
		ang2 = pang.second;
		//std::cout << ind1 << " ";
	}
	//std::cout << std::endl;

	vecC = std::vector<Eigen::Vector2d>(lisC.begin(),lisC.end());
	vecInd = std::vector<std::pair<unsigned int,unsigned int> >(lisInd.begin(),lisInd.end());
}


// minphase.cc
//
// [y,ssp,iter] = minphase(h)
// C++ implementation of minphase.m with the Eigen C++ template library:
//   m-file for extracting the minimum phase factor from the
//   linear-phase filter h. Input: h = (h(0) h(1)...h(N)] (row vector)
//   where the h vector is the right half of a linear-phase FIR filter.
//   It is presumed that any unit-circle zeros of h are of even multiplicity.
//   Copyright (c) January 2002  by  H. J. Orchard and A. N. Willson, Jr.

// Copyright (C) 2017 Robert G. Jenssen
//
// This program is free software; you can redistribute it and/or
// modify it underthe terms of the GNU General Public License as
// published by the Free Software Foundation; either version 3 of
// the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
//
// See the GNU General Public License for more details.
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include <cfloat>
#include <Eigen/Eigen>
#include <octave/oct.h>
#include <octave/parse.h>

typedef Eigen::Matrix<long double,Eigen::Dynamic,1> EigenVectorNx1ld;
typedef Eigen::Matrix<long double,Eigen::Dynamic,Eigen::Dynamic>
  EigenMatrixNxNld;

DEFUN_DLD(minphase, args, nargout,"[y,ssp,iter] = minphase(h)")
{

  // Sanity checks
  octave_idx_type nargin=args.length();
  if ((nargin>1) || (nargout>3))
    {
      print_usage();
    }

  // Input arguments
  ColumnVector h = args(0).column_vector_value();
  octave_idx_type N=h.numel();

  // Output arguments
  RowVector y(N);
  long double ssp = LDBL_MAX;
  octave_idx_type iter = 0;

  // Initialise ss
  long double ss=ssp/2;

  // Initialise column vector hh
  EigenVectorNx1ld hh(N);
  for(auto r=0;r<N;r++)
    {
      hh(r)=h(r);
    }

  // Initialise column vectors yy, b and d
  EigenVectorNx1ld yy(N);
  EigenVectorNx1ld d(N);
  EigenVectorNx1ld b(N);
  for (auto r=0;r<N;r++)
    {
      yy(r)=0;
      d(r)=0;
    }
  yy(0)=1;

  // Allocate A, Al and Ar
  EigenMatrixNxNld A(N,N);
  EigenMatrixNxNld Al(N,N);
  EigenMatrixNxNld Ar(N,N);

  for (auto r=0;r<N;r++)
    {
      for (auto c=0;c<N;c++)
        {
          A(r,c)=0;
          Al(r,c)=0;
          Ar(r,c)=0;
        }
    }

  // Newton-Raphson iteration
  while (ss < ssp)
    {
      yy=yy+d;
      ssp=ss;
      iter=iter+1;

      for(auto r=0;r<N;r++)
        {
          for (auto c=r;c<N;c++)
            {
              Al(r,c-r)=yy(c);
            }
          for (auto c=0;c<(N-r);c++)
            {
              Ar(r,c+r)=yy(c);
            }
        }
      A=Al+Ar;
      b=hh-(Al*yy);
      d=A.colPivHouseholderQr().solve(b);
      ss=d.norm();
    }

  // Done
  for(auto r=0;r<N;r++)
    {
      y(r)=(double)(yy(r));
    }
  octave_value_list retval(nargout);
  if (nargout >= 1)
    {
      retval(0)=y;
    }
  if (nargout >= 2)
    {
      retval(1)=(double)ssp;
    }
  if (nargout == 3)
    {
      retval(2)=iter;
    }
  return retval;
}


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

#include "../include/KF.h"
#include "../include/dsho.h"
#include "../include/matern32.h"
#include "../include/ndsho.h"

using namespace Eigen;
using namespace std;

// function to read a space separated file

std::vector<double> load_csv (const std::string & path) {
    std::ifstream indata;
    indata.open(path);
    std::string line;
    std::vector<double> values;
    uint rows = 0;
    while (std::getline(indata, line)) {
        std::stringstream lineStream(line);
        std::string cell;
        while (std::getline(lineStream, cell, ' ')) {
            //cout << std::stod(cell) << endl;
            values.push_back(std::stod(cell));
        }
        ++rows;
    }
    return values;
}

int main()
{

std::random_device rd;
std::mt19937 rng(rd());

// read file with two component damped simple harmonic oscillator simulated data
//std::vector<double> values = load_csv("two_comp_dsho.txt");
//cout << values.size() << endl;

// map data to VectorXds
//VectorXd times = Map<VectorXd, 0, InnerStride<2> > (values.data(), 10000);
//VectorXd yi = Map<VectorXd, 0, InnerStride<2> > (values.data()+1, 10000);
int vecsize = 10000;
VectorXd yi(vecsize);
VectorXd times = Eigen::VectorXd::Random(vecsize);
times.array() += 1.0;
times.array() *= (100*0.5);
std::sort(times.data(), times.data() + times.size());

// this is the observational error vector
VectorXd yerr = VectorXd::Ones(yi.size());

double obs_err = 0.05;
yerr.array() *= obs_err;

std::uniform_real_distribution<double> uniform(0, 5);

// Now we run a bunch of tests

// create a Matern 3/2 solver and get likelihood of data
gpstate::matern32::Matern32Solver m32(times, yi, yerr,std::sqrt(10), 1.0);
double log_likelihood = m32.KF_log_likelihood();
cout << "logL: " << log_likelihood << endl;

// simulate a Matern 3/2 on the times given by VectorXd times
VectorXd y_sim;
m32.simulate_Matern32(y_sim, rng);

// set parameters for a 2 component DSHO model
Eigen::Vector2d omegas,Qpars,varfs;
omegas << 1, 3*0.15915494309;
Qpars << 10, 10;
varfs << 1, 1;

// set parameters for a single DSHO model
double omega0=12;
double Q=1;
double varf=1;

// simulate a single DSHO with parameters omega0, Q, varf
// we simulate for various values of Q, and measure the simulated variance
// objective is to verify that the variance specified by 'varf'
// corresponds indeed to the actual simulated variance
// (in other words, we are verifying the normalizations of our GPs)
gpstate::dsho::DSHOSolver dsho(times,yi,yerr,omega0, Q, varf);
dsho.simulate_DSHO(y_sim);

double mean = y_sim.mean();
Eigen::VectorXd tmp = y_sim.array()-mean;
double variance  = tmp.dot(tmp) / y_sim.rows();
std::cout << "var1:" << variance << std::endl;

Q = 10;
dsho.set_pars(omega0,Q,varf);
dsho.simulate_DSHO(y_sim);
mean = y_sim.mean();
tmp = y_sim.array()-mean;
variance  = tmp.dot(tmp) / y_sim.rows();
std::cout << "var2:" << variance << std::endl;

Q = 0.1;
dsho.set_pars(omega0,Q,varf);
dsho.simulate_DSHO(y_sim);
mean = y_sim.mean();
tmp = y_sim.array()-mean;
variance  = tmp.dot(tmp) / y_sim.rows();
std::cout << "var3:" << variance << std::endl;

Q=10;
omega0=6;
dsho.set_pars(omega0,Q,varf);
dsho.simulate_DSHO(y_sim);
mean = y_sim.mean();
tmp = y_sim.array()-mean;
variance  = tmp.dot(tmp) / y_sim.rows();
std::cout << "var4:" << variance << std::endl;

omega0=120;
dsho.set_pars(omega0,Q,varf);
dsho.simulate_DSHO(y_sim);
mean = y_sim.mean();
tmp = y_sim.array()-mean;
variance  = tmp.dot(tmp) / y_sim.rows();
std::cout << "var5:" << variance << std::endl;
log_likelihood = dsho.KF_log_likelihood();
cout << "logL: " << log_likelihood << endl;

//std::exit(0);

Eigen::Matrix<double,1,1> omega_one, Qpar_one, varf_one;
omega_one << 120;
Qpar_one << 10;
varf_one << 1;

// Verify that the ndsho implementation is consistent with the dsho one
gpstate::n_dsho::N_DSHOSolver ndsho(times,yi,yerr,omega_one, Qpar_one, varf_one);
ndsho.simulate_N_DSHO(y_sim, rng);
mean = y_sim.mean();
tmp = y_sim.array()-mean;
variance  = tmp.dot(tmp) / y_sim.rows();
std::cout << "var6:" << variance << std::endl;
log_likelihood = ndsho.KF_log_likelihood();
cout << "logL: " << log_likelihood << endl;

// write simulated vector to file GPtest.txt
std::ofstream file("GPtest.txt");
if (file.is_open())
  {
    file << "#time y_sim" << endl;
    for(int i=0; i<times.rows(); i++)
    {
        file <<  times(i) <<" "<< y_sim(i) << endl;
    }
  }

}