#include<iostream>
#include<fstream>
#include<string>
#include<algorithm>
#include<list>
#include<stdexcept>
#include<functional>

#include <boost/program_options.hpp>
#include <boost/tokenizer.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/lexical_cast.hpp>

#include <Interp.hpp>

#include "Integrate.hpp"

using namespace std;
namespace po = boost::program_options;

void print_version()
{
  cout<<"integrate-cli - linked against libIntegrate version "<< libIntegrate_VERSION_FULL << endl;
}

void print_usage(char prog_name[])
{

  cout<<"usage: "<<prog_name<<" [OPTIONS]"<<"\n";

}

void print_documentation( )
{
  cout<<"Reads function from a file an integrates it."
      <<"\n"
      <<"The data is contained in a gnuplot-style text file, with each x-y pair on a new line, separated by white space."
      <<"\n";
}

std::function<double(std::vector<double>&,std::vector<double>&)> create(std::string type)
{
  if( boost::starts_with("riemann",type) )
    return _1D::RiemannRule<double>();

  if( boost::starts_with("trapezoid",type) )
    return _1D::TrapezoidRule<double>();

  if( boost::starts_with("simpson",type) )
    return [](std::vector<double>& x, std::vector<double>& y){
      // create an interpolator to pass to the integrator
      _1D::CubicSplineInterpolator<double> interp(x,y);

      double min = *std::min_element( x.begin(), x.end() );
      double max = *std::max_element( x.begin(), x.end() );

      _1D::SimpsonRule<double> integ;

      return integ( interp, min, max, x.size() );
    };

  if( boost::starts_with("gauss-legendre",type) )
    return [](std::vector<double>& x, std::vector<double>& y){
      // create an interpolator to pass to the integrator
      _1D::CubicSplineInterpolator<double> interp(x,y);

      double min = *std::min_element( x.begin(), x.end() );
      double max = *std::max_element( x.begin(), x.end() );

      // we have quadratures of order
      // 8, 16, 32, and 64.
      if( x.size() <= 8 )
      {
        _1D::GQ::GaussLegendreQuadrature<double,8> integ;
        return integ( interp, min, max );
      }
      if( x.size() <= 16 )
      {
        _1D::GQ::GaussLegendreQuadrature<double,16> integ;
        return integ( interp, min, max );
      }
      if( x.size() <= 32 )
      {
        _1D::GQ::GaussLegendreQuadrature<double,32> integ;
        return integ( interp, min, max );
      }

      _1D::GQ::GaussLegendreQuadrature<double,64> integ;
      return integ( interp, min, max );
    };

  return nullptr;
}

void ReadFunction(std::istream &_in, double *&_x, double *&_y, int &_n, int _dimensions = 1, int _multiplicity = 1);

int main( int argc, char* argv[])
{
    po::options_description options("Allowed options");
    options.add_options()
      ("help,h"      , "print help message")
      ("batch,b"     , "output in 'batch' mode")
      ("method,m"    , po::value<string>()->default_value("riemann"),     "integration method.")
      ("list,l"      ,                                                   "list available integration methods.")
      ("integrate-data" ,                                                "file containing data to be integrated.")
      ;

    po::positional_options_description args;
    args.add("integrate-data", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(options).positional(args).run(), vm);
    po::notify(vm);

    if (argc == 1 || vm.count("help"))
    {
      print_version();
      print_usage( argv[0] );
      cout<<"\n";
      cout << options<< "\n";
      cout<<"\n";
      print_documentation();
      cout<<"\n";
      return 1;
    }

    if (vm.count("list"))
    {
      print_version();
      cout<<"\t'riemann' : simple riemann sum\n";
      cout<<"\t'trapezoid' : trapezoid rule\n";
      cout<<"\t'simpson' : simposon's rule (uses interpolation)\n";
      cout<<"\t'gauss-legendre' : Gauss-Legendre interpolation (uses interpolation)\n";
      return 1;
    }

    ifstream in;
    double *x, *y, sum;
    int n;

    // load data
    in.open( vm["integrate-data"].as<string>().c_str() );
    if( !in.is_open() )
    {
      cerr << "ERROR: could not open file: "<<vm["integrate-data"].as<string>()<< endl;
      return 0;
    }

    ReadFunction( in, x, y, n );
    in.close();

    std::function<double(std::vector<double>&,std::vector<double>&)> interp;
    interp = create(vm["method"].as<string>());
    if( !interp )
    {
      delete[] x;
      delete[] y;
      cerr << "ERROR: Unrecognized interpolation method (" << vm["method"].as<string>()<< ")." << endl;
      return 0;
    }

    std::vector<double> X(x,x+n),Y(y,y+n);

    delete[] x;
    delete[] y;

    sum = interp(X,Y);

    std::cout << sum << std::endl;

    return 0;
}

void ReadFunction(std::istream &_in, double  *&_x, double *&_y, int *&_n, int _dimensions, int _multiplicity)
{

  double *xbuffer1, *xbuffer2;
  double *ybuffer1, *ybuffer2;

  //////////////////////////////
  //  READ IN DATA FROM FILE  //
  //////////////////////////////

  const int chunck = 1000;
  int buffsize,buffsize_old;
  int i,j;
  int n;

  std::string line;

  buffsize = chunck;

  xbuffer1 = new double[buffsize*_dimensions  ];
  ybuffer1 = new double[buffsize*_multiplicity];

  i = 0;
  while( getline( _in, line ) )
  {
    // if line is blank, skip it
    if( line == "" )
      continue;
    std::vector<std::string> columns;
    boost::char_separator<char> sep(" \t");
    boost::tokenizer<boost::char_separator<char>> toks(line,sep);
    for( auto it = toks.begin(); it != toks.end(); ++it )
      columns.push_back( *it );

    // if first column starts with a #, this line is a comment
    // and we should skip it.
    if( columns.size() > 0 && columns[0].find('#') == 0 )
      continue;

    if(i == buffsize - 1)  // this "grows" the buffer
    {
      buffsize_old = buffsize;
      buffsize = buffsize + chunck;

      xbuffer2 = new double[buffsize*_dimensions  ];
      std::copy( xbuffer1, xbuffer1 + buffsize_old*_dimensions, xbuffer2);
      delete[] xbuffer1;

      ybuffer2 = new double[buffsize*_multiplicity];
      std::copy( ybuffer1, ybuffer1 + buffsize_old*_multiplicity, ybuffer2);
      delete[] ybuffer1;

      xbuffer1 = xbuffer2;
      ybuffer1 = ybuffer2;
    }

    for(j = 0; j < _dimensions; j++)
      xbuffer1[i*_dimensions + j]   = boost::lexical_cast<double>(columns[j]);
    for(j = 0; j < _multiplicity; j++)
      ybuffer1[i*_multiplicity + j] = boost::lexical_cast<double>(columns[j+_dimensions]);

    i++;
  }

  n = i;

  /** \todo add actual multi-coordinate support */

  _n = new int[_dimensions];
  _x = new double[n * _dimensions  ];
  _y = new double[n * _multiplicity];

  for(j = 0; j < n * _dimensions  ; j++)
    _x[j] = xbuffer1[j];
  for(j = 0; j < n * _multiplicity; j++)
    _y[j] = ybuffer1[j];
  for(j = 0; j < _dimensions; j++)
    _n[j] = n;

  delete[] xbuffer1;
  delete[] ybuffer1;

}

void ReadFunction(std::istream &_in, double *&_x, double *&_y, int &_n, int _dimensions, int _multiplicity)
{
  int *n;
  ReadFunction(_in, _x, _y, n, _dimensions, _multiplicity);
  _n = n[0];
  delete[] n;
}


//
// Created by yongqi on 17-12-4.
//

#ifndef VFORCE_MATHUTILS_HPP
#define VFORCE_MATHUTILS_HPP

#include <cmath>
#include <Eigen/Eigen>

namespace VForce {
namespace Utils {
/**
 * make angle between [-pi, pi)
 * @param x input angle in radius
 * @return normalized angle
 */
inline double normalize_angle(double x) {
  x = fmod(x + M_PI, 2 * M_PI);
  return x < 0 ? x += 2 * M_PI : x - M_PI;
}

inline float normalize_anlge(float x) {
  x = fmod(x + M_PI, 2 * M_PI);
  return x < 0 ? x += 2 * M_PI : x - M_PI;
}

/**
 * radian to degree
 * @tparam T
 * @param x
 * @return
 */
template<typename T>
inline T rad2deg(T x) {
  return x / M_PI * 180.;
}

/**
 * degree to radian
 * @tparam T
 * @param x
 * @return
 */
template<typename T>
inline T deg2rad(T x) {
  return x * M_PI / 180.;
}

/**
 * Convert pose(in Rz(yaw)Ry(pitch)Rx(roll) order) into transformation matrix
 * @tparam T
 * @param x
 * @param y
 * @param z
 * @param roll
 * @param pitch
 * @param yaw
 * @param tf
 */
template<typename T>
void pose2matrix(const T &x, const T &y, const T &z,
                 const T &roll, const T &pitch, const T &yaw,
                 Eigen::Matrix<T, 4, 4> &t) {
  T A = cos(yaw),  B = sin(yaw),  C = cos(pitch), D = sin (pitch),
      E = cos(roll), F = sin(roll), DE = D*E,       DF = D*F;

  t(0, 0) = A*C;  t(0, 1) = A*DF - B*E;  t(0, 2) = B*F + A*DE;  t(0, 3) = x;
  t(1, 0) = B*C;  t(1, 1) = A*E + B*DF;  t(1, 2) = B*DE - A*F;  t(1, 3) = y;
  t(2, 0) = -D;   t(2, 1) = C*F;         t(2, 2) = C*E;         t(2, 3) = z;
  t(3, 0) = 0;    t(3, 1) = 0;           t(3, 2) = 0;           t(3, 3) = 1;
}

/**
 * Convert transformation matrix into pose
 * @tparam T
 * @param matrix
 * @param x
 * @param y
 * @param z
 * @param roll
 * @param pitch
 * @param yaw
 */
template<typename T>
void matrix2pose(const Eigen::Matrix<T, 4, 4> &t,
                 T &x, T &y, T &z,
                 T &roll, T &pitch, T &yaw) {
  x = t(0, 3);
  y = t(1, 3);
  z = t(2, 3);
  roll = atan2(t(2, 1), t(2, 2));
  pitch = atan2(-t(2, 0), sqrt(t(2, 1) * t(2, 1) + t(2, 2) * t(2, 2)));
  yaw = atan2(t(1, 0), t(0, 0));
}

}
}

#endif //VFORCE_MATHUTILS_HPP


/**
 * gaussian.hpp v1.0.0
 *
 * Contains methods related to Gaussian distributions and smoothing.
 * Methods for curve fitting are in gaussianfit.cpp.
 */

#pragma once

#define SQRT2PI 2.506628275

#include <cmath>

#define  ARMA_DONT_USE_WRAPPER
#define  ARMA_USE_LAPACK
#define  ARMA_DONT_PRINT_ERRORS
#include <armadillo>

namespace bib {

  namespace gaussian {

    const double M_SQRT_2OVERPI = std::sqrt(2.0 / M_PI);
    const double M_SQRT_2OVERPI_CUBED = std::pow(M_SQRT_2OVERPI, 3.0);

    /**
     * Represents a Gaussian distribution. Comparison functions only
     * compare amplitude (used for sorting from most to least important
     * when curve fitting).
     *
     * amplitude: suqare root of the amplitude of the distribution.
     * mean: center point of the distribution.
     * stdev: standard deviation of the distribution.
     */
    struct Parameters {

      double amplitude;
      double mean;
      double stdev;

      bool operator >(const Parameters& a) const {
	return amplitude * amplitude / stdev > a.amplitude * a.amplitude / a.stdev;
      }

      bool operator <(const Parameters& a) const {
	return amplitude * amplitude / stdev < a.amplitude * a.amplitude / a.stdev;
      }

    };

    /**
     * Represents a Gaussian distribution with shape (adds skewness).
     * Comparison functions only compare amplitude (used for sorting
     * from most to least important when curve fitting).
     *
     * shape: shape parameter (alpha) capturing skewness.
     * amplitude: suqare root of the amplitude of the distribution.
     * mean: center point of the distribution.
     * stdev: standard deviation of the distribution.
     */
    struct SkewParameters {

      double amplitude;
      double mean;
      double stdev;
      double shape;

      bool operator >(const SkewParameters& a) const {
	return amplitude * amplitude / stdev > a.amplitude * a.amplitude / a.stdev;
      }

      bool operator <(const SkewParameters& a) const {
	return amplitude * amplitude / stdev < a.amplitude * a.amplitude / a.stdev;
      }

    };

    /**
     * Returns the value of a Gaussian distribution at a given point x.
     *
     * amplitude: suqare root of the amplitude of the distribution.
     * mean: center point of the distribution.
     * stdev: standard deviation of the distribution.
     * x: the point at which to sample the distribution.
     */
    inline double curveValue(double amplitude, double mean, double stdev, double x) {
      return amplitude * amplitude / stdev * std::exp( -(x - mean) * (x - mean) / stdev / stdev / 2.0 );
    }

    /**
     * Returns the value of a Gaussian distribution with shape at a given point x.
     *
     * amplitude: suqare root of the amplitude of the distribution.
     * mean: center point of the distribution.
     * stdev: standard deviation of the distribution.
     * shape: shape parameter (alpha) capturing skewness.
     * x: the point at which to sample the distribution.
     */
    inline double curveValue(double amplitude, double mean, double stdev, double shape, double x) {
      return amplitude * amplitude / stdev * std::exp( -(x - mean) * (x - mean) / stdev / stdev / 2.0 )
	* (1.0 + std::erf(shape * (x - mean) / (stdev * M_SQRT2)));
    }

    /**
     * Returns the value of a Gaussian distribution with shape at a given point x.
     *
     * distribution: the distribution to sample.
     * x: the point at which to sample the distribution.
     */
    inline double curveValue(Parameters distribution, double x) {
      return curveValue(distribution.amplitude, distribution.mean, distribution.stdev, x);
    }

    /**
     * Returns the value of a Gaussian distribution with shape at a given point x.
     *
     * distribution: the distribution to sample.
     * x: the point at which to sample the distribution.
     */
    inline double curveValue(SkewParameters distribution, double x) {
      return curveValue(distribution.amplitude, distribution.mean, distribution.stdev,
			distribution.shape, x);
    }

    /**
     * Smooths the input vector using a Gaussian distribution of the given width.
     *
     * input: the input vector to smooth.
     * kernelWidth: standard deviation of the distribution to use for smoothing.
     */
    arma::Col<double> smooth(const arma::Col<double>& input, double kernelWidth);

    /**
     * Smooths the input vector using the second derivative of a Gaussian distribution
     * of the given width.
     *
     * input: the input vector to smooth.
     * kernelWidth: standard deviation of the distribution to use for smoothing.
     */
    arma::Col<double> scaleSpaceSmooth(const arma::Col<double>& input, double kernelWidth);

    std::vector<double> getDistribution(double A, double x, double u, size_t length);

    inline int sgn(double val) {
      return (0.0 < val) - (val < 0.0);
    }

    inline double skewMode(SkewParameters gaussian) {
      double delta = gaussian.shape / std::sqrt(1.0 + gaussian.shape * gaussian.shape);
      double uz = M_SQRT_2OVERPI * delta;
      double oz = std::sqrt(1.0 - uz * uz);
      double skewness = ((4.0 - M_PI) / 2.0)
	* ((M_SQRT_2OVERPI_CUBED * delta * delta * delta) / std::pow(1.0 - 2.0 * delta * delta / M_PI, 1.5));
      return (uz - skewness * oz / 2.0 - sgn(gaussian.shape) / 2.0 * std::exp(-2.0 * M_PI / std::abs(gaussian.shape)))
	* gaussian.stdev + gaussian.mean;
    }

  }

} // namespace bib::gaussian


#include "Gaussian.h"
#include <iostream>
#include <stdexcept>
#include <cmath>
#include "../Utils.h"
//#include <boost/math/special_functions/erf.hpp>

namespace DNest4
{

Gaussian::Gaussian(double center, double width)
:center(center)
,width(width)
{
    if(width <= 0.0)
        throw std::domain_error("Gaussian distribution must have positive width.");
}

double Gaussian::cdf(double x) const
{
    return normal_cdf((x-center)/width);
}

double Gaussian::cdf_inverse(double x) const
{
    if(x < 0.0 || x > 1.0)
        throw std::domain_error("Input to cdf_inverse must be in [0, 1].");
    return center + width*normal_inverse_cdf(x);
    //return center + width * sqrt(2) * boost::math::erf_inv(2*x - 1);
}

double Gaussian::log_pdf(double x) const
{
	double r = (x - center)/width;
    return -0.5*r*r - _norm_pdf_logC;
}

TruncatedGaussian::TruncatedGaussian(double center, double width, double lower, double upper)
:center(center)
,width(width)
,lower(lower)
,upper(upper)
{
    if(width <= 0.0)
        throw std::domain_error("TruncatedGaussian distribution must have positive width.");
    if(lower >= upper)
        throw std::domain_error("TruncatedGaussian: lower bound should be less than upper bound.");
    // the original, untruncated, Gaussian distribution
    unG = Gaussian(center, width);
    c = unG.cdf(upper) - unG.cdf(lower);
}

double TruncatedGaussian::cdf(double x) const
{
    double up = std::max(std::min(x,upper), lower);
    return (unG.cdf(up) - unG.cdf(lower)) / c;
}

double TruncatedGaussian::cdf_inverse(double x) const
{
    if(x < 0.0 || x > 1.0)
        throw std::domain_error("Input to cdf_inverse must be in [0, 1].");
    double xx = unG.cdf(lower) + x * c;
    return unG.cdf_inverse(xx);
}

double TruncatedGaussian::log_pdf(double x) const
{
    if(x<lower or x>upper) return -std::numeric_limits<double>::infinity();
    return unG.log_pdf(x) - log(c);
}

} // namespace DNest4


/*
MIT License

Copyright (c) 2020 kyawakyawa

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include <iostream>
#include <utility>

#include "macros.h"

IGNORE_STRICT_WARNING_PUSH

#include <Eigen/Core>

IGNORE_STRICT_WARNING_POP

// reference http://takashiijiri.com/study/miscs/QRfactorization.htm

template <typename Real, int N>
Eigen::Matrix<Real, N, N> HouseholderTransformation(
    const Eigen::Matrix<Real, N, 1>& x, const Eigen::Matrix<Real, N, 1>& y) {
  static_assert(N >= 1);
  const auto aux = x - y;
  const Eigen::Matrix<Real, N, 1> u = aux.normalized();

  return Eigen::Matrix<Real, N, N>::Identity() - 2 * u * u.transpose();
}

template <typename Real>
Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic> HouseholderTransformation(
    const Eigen::Matrix<Real, Eigen::Dynamic, 1>& x,
    const Eigen::Matrix<Real, Eigen::Dynamic, 1>& y) {
  assert(x.rows() >= 1 && x.rows() == y.rows());
  const auto aux = x - y;
  const Eigen::Matrix<Real, Eigen::Dynamic, 1> u = aux.normalized();

  return Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic>::Identity(
             x.rows(), x.rows()) -
         2 * u * u.transpose();
}

template <typename Real, int N, int M>
std::pair<Eigen::Matrix<Real, N, N>, Eigen::Matrix<Real, N, M>>
QRDecompositionWithHouseholderTransformation(
    const Eigen::Matrix<Real, N, M>& input) {
  static_assert(M >= 1 && N >= M);
  Eigen::Matrix<Real, N, M> R = input;
  Eigen::Matrix<Real, N, N> Q = Eigen::Matrix<Real, N, N>::Identity();

  for (int k = 0; k < M - 1; ++k) {
    const Real sign = (R(k, k) >= Real(0) ? Real(1) : Real(-1));
    const int Mmk = M - k;

    const Eigen::Matrix<Real, Eigen::Dynamic, 1> x = R.block(k, k, Mmk, 1);
    // グラフィック用途(ローテーションとスケールの分離)の場合、
    // y(0,0)の符号は+で固定したほうがいい(この値がRの対角要素になる)
    Eigen::Matrix<Real, Eigen::Dynamic, 1> y =
        Eigen::Matrix<Real, Eigen::Dynamic, 1>::Zero(Mmk, 1);
    const Real norm = x.norm();
    // uが0 vectorになる場合
    // この場合はそもそも計算する必要がない
    if (std::abs(norm - std::abs(x[0])) <
        std::numeric_limits<Real>::epsilon()) {
      continue;
    }
    y[0] = -sign * x.norm();

    const Eigen::Matrix<Real, Eigen::Dynamic, 1> u = (x - y).normalized();

    R.bottomRightCorner(Mmk, Mmk) -=
        Real(2) * u * (u.transpose() * R.bottomRightCorner(Mmk, Mmk));
    //////////////////// Rのみ欲しい場合はここまでの計算で良い
    ///////////////////////

    if (k > 0) {
      Q.topRightCorner(k, Mmk) -=
          Real(2) * (Q.topRightCorner(k, Mmk) * u) * u.transpose();
    }
    Q.bottomRightCorner(Mmk, Mmk) -=
        Real(2) * (Q.bottomRightCorner(Mmk, Mmk) * u) * u.transpose();
  }

  return std::make_pair(Q, R);
}

#ifdef USE_STACK_TRACE_LOGGER
#include <glog/logging.h>
#endif

int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) {
#ifdef USE_STACK_TRACE_LOGGER
  (void)argc;
  google::InitGoogleLogging(argv[0]);
  google::InstallFailureSignalHandler();
#endif

  const Eigen::Vector3d x = {1, 2, 3};
  const Eigen::Vector3d y = {3, 2, 1};

  std::cout << "----------HouseholderTransformation----------" << std::endl;

  std::cout << " x = (" << x.transpose() << ")^T\n y = (" << y.transpose()
            << ")^T\n"
            << std::endl;

  const Eigen::Matrix3d H = HouseholderTransformation(x, y);
  std::cout << " H = \n" << H << "\n" << std::endl;

  const Eigen::Vector3d Hx = H * x;
  const Eigen::Vector3d Hy = H * y;

  std::cout << " (Hx)^T = (" << Hx.transpose() << ")\n" << std::endl;
  std::cout << " (Hy)^T = (" << Hy.transpose() << ")\n" << std::endl;

  std::cout << " H*H^T = \n" << H * H.transpose() << "\n" << std::endl;

  std::cout << "---------- ----------" << std::endl;

  std::cout
      << "----------QRDecompositionWithHouseholderTransformation----------"
      << std::endl;
  Eigen::Matrix3d A;

  A(0, 0) = 1.;
  A(0, 1) = 4.;
  A(0, 2) = 9.;

  A(1, 0) = 16.;
  A(1, 1) = 25.;
  A(1, 2) = 36.;

  A(2, 0) = 49.;
  A(2, 1) = 64.;
  A(2, 2) = 81.;

  const auto result = QRDecompositionWithHouseholderTransformation(A);

  std::cout << " Q = \n" << result.first << "\n" << std::endl;
  std::cout << " R = \n" << result.second << "\n" << std::endl;

  std::cout << " QR = \n" << result.first * result.second << "\n" << std::endl;

  std::cout << " QQ^T = \n"
            << result.first * result.first.transpose() << "\n"
            << std::endl;

  std::cout << "---------- ----------" << std::endl;
  return 0;
}


/**
 * @file Interpolation.hpp
 * @author bwu
 * @brief Interpolation method implementation
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_MATH_INTERPOLATION_HPP
#define GENERIC_MATH_INTERPOLATION_HPP
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include "generic/common/Traits.hpp"
#include "MathUtility.hpp"
#include <vector>
namespace generic {
namespace math    {
using generic::common::float_type;
template <typename num_type>
class Interpolation
{
    using float_t = float_type<num_type>;
    using vector_t = boost::numeric::ublas::vector<float_t>;
    using band_matrix_t = boost::numeric::ublas::banded_matrix<float_t>;
public:
    enum class Method { Linear = 1, Cubic = 2};
    enum class BCType { FirstDeriv = 1, SecondDeriv = 2, NotAKnot = 3 };

    ///@brief constructs an interpolation object with method
    Interpolation(Method method = Method::Linear);
    /**
     * @brief constructs an interpolation object with parameters
     * @param x input samples of x
     * @param y input samples of y
     * @param method interpolation method
     * @note if Cubic method failed with input x, y samples, it will decay to Linear method
     * @param monotonic set whether curve monotonic when interpolation
     * @param left left boundary condition type
     * @param lValue left boundary condition initial value
     * @param right right boundary condition type
     * @param rValue right boundary condition inital value
     */
    Interpolation(const std::vector<num_type> & x, const std::vector<num_type> & y,
                    Method method, bool monotonic = false,
                    BCType left  = BCType::SecondDeriv, float_t lValue  = 0,
                    BCType right = BCType::SecondDeriv, float_t rValue = 0);

    ///@brief sets boundary conditions and inital values
    void SetBoundary(BCType left, BCType right, float_t lVal, float_t rVal);
    ///@brief sets input samples of x and y
    void SetSamples(const std::vector<num_type> & x, const std::vector<num_type> & y);

    ///@brief gets interpolated value of input x
    num_type operator() (num_type x) const { return Interpolate(x); }

private:
    void UpdateCoefficients();
    bool makeMonotonic();
    size_t FindClosest(num_type x) const;
    num_type Interpolate(num_type x) const;
    num_type Interpolate(num_type x, const num_integer_tag) const;
    float_t Interpolate(num_type x, const num_floating_tag) const;

private:
    Method m_method;
    bool m_monotonic;
    BCType m_left, m_right;
    float_t m_lVal, m_rVal;
    std::vector<num_type> m_x, m_y;
    float_t m_coef0 = 0;
    std::vector<float_t> m_coef1, m_coef2, m_coef3;
};

template <typename num_type>
inline Interpolation<num_type>::Interpolation(Method method)
 : m_method(method), m_monotonic(false), m_left(BCType::SecondDeriv), m_right(BCType::SecondDeriv), m_lVal(0), m_rVal(0)
{
}

template <typename num_type>
inline Interpolation<num_type>::Interpolation(const std::vector<num_type> & x, const std::vector<num_type> & y,
                                                Method method, bool monotonic,
                                                BCType left, float_t lValue, BCType right, float_t rValue)
 : m_method(method), m_monotonic(monotonic), m_left(left), m_right(right), m_lVal(lValue), m_rVal(rValue)
{
    SetSamples(x, y);
}

template <typename num_type>
inline void Interpolation<num_type>::SetBoundary(BCType left, BCType right, float_t lVal, float_t rVal)
{
    assert(m_x.size() == 0 && "should set boundary type before setting samples");
    m_left = left; m_right = right;
    m_lVal = lVal; m_rVal = rVal;
}

template <typename num_type>
inline void Interpolation<num_type>::SetSamples(const std::vector<num_type> & x, const std::vector<num_type> & y)
{
    const size_t size = x.size();
    assert(size > 0 && size == y.size());
    m_x = x; m_y = y;
    m_coef1.assign(size, 0); m_coef2.assign(size, 0), m_coef3.assign(size, 0);
    if(size < 2) return;
    if(size < 3) m_method = Method::Linear;
    if(Method::Linear == m_method){
        for(size_t i = 0, j = 1; i < size - 1; ++i, ++j)
            m_coef1[i] = float_t(m_y[j] - m_y[i]) / float_t(m_x[j] - m_x[i]);
        m_coef1[size - 1] = m_coef1[size - 2];
    }
    else if(Method::Cubic == m_method){
        size_t lower = (m_right == BCType::NotAKnot) ? 2 : 1;
        size_t upper = (m_left  == BCType::NotAKnot) ? 2 : 1;
        band_matrix_t m(size, size, lower, upper);
        vector_t rhs(size);
        for(size_t i = 0, j = 1, k = 2; k < size; ++i, ++j, ++k){
            m(j, i) = 1.0 / 3.0 * (m_x[j] - m_x[i]);
            m(j, j) = 2.0 / 3.0 * (m_x[k] - m_x[i]);
            m(j, k) = 1.0 / 3.0 * (m_x[k] - m_x[j]);
            rhs[j] = (m_y[k] - m_y[j]) / (m_x[k] - m_x[j]) - (m_y[j] - m_y[i]) / (m_x[j] - m_x[i]);
        }
        //BC
        if(m_left == BCType::FirstDeriv){
            m(0, 0) = 2.0 * (m_x[1] - m_x[0]);
            m(0, 1) = 1.0 * (m_x[1] - m_x[0]);
            rhs[0] = 3.0 * ((m_y[1] - m_y[0]) / (m_x[1] - m_x[0]) - m_lVal);
        }
        else if(m_left == BCType::SecondDeriv){
            m(0, 0) = 2.0;
            m(0, 1) = 0.0;
            rhs[0] = m_lVal;
        }
        else if(m_left == BCType::NotAKnot){
            m(0, 0) = - m_x[2] + m_x[1];
            m(0, 1) =   m_x[2] - m_x[0];
            m(0, 2) = - m_x[1] + m_x[0];
            rhs[0] = 0.0;
        }
        else { assert(false); }

        if(m_right == BCType::FirstDeriv){
            m(size - 1, size - 1) = 2.0 * (m_x[size - 1] - m_x[size - 2]);
            m(size - 1, size - 2) = 1.0 * (m_x[size - 1] - m_x[size - 2]);
            rhs[size - 1] = 3.0 * (m_rVal - (m_y[size - 1] - m_y[size - 2]) / (m_x[size - 1] - m_x[size - 2]));
        }
        else if(m_right == BCType::SecondDeriv){
            m(size - 1, size - 1) = 2.0;
            m(size - 1, size - 2) = 0.0;
            rhs[size - 1] = m_rVal;
        }
        else if(m_right == BCType::NotAKnot){
            m(size - 1, size - 3) = - m_x[size - 1] + m_x[size - 2];
            m(size - 1, size - 2) =   m_x[size - 1] - m_x[size - 3];
            m(size - 1, size - 1) = - m_x[size - 2] + m_x[size - 3];
        }
        else { assert(false); }

        boost::numeric::ublas::permutation_matrix<size_t> pm(size);

        //decay to linear method if fail at LU factorization
        try {
            lu_factorize(m, pm);
            lu_substitute(m, pm, rhs);
        }
        catch (...) {
            m_method = Method::Linear;
            SetSamples(x, y);
            return;
        }

        for(size_t i = 0; i < size; ++i)
            m_coef2[i] = rhs[i];

        for(size_t i = 0, j = 1; j < size; ++i, ++j) {
            m_coef1[i] = (m_y[j] - m_y[i]) / (m_x[j] - m_x[i]) - 1.0 / 3.0 * (2.0 * m_coef2[i] + m_coef2[j]) * (m_x[j] - m_x[i]);
            m_coef3[i] = 1.0 / 3.0 * (m_coef2[j] - m_coef2[i]) / (m_x[j] - m_x[i]);
        }

        float_t h = x[size - 1] - x[size - 2];
        m_coef3[size - 1] = 0.0;
        m_coef1[size - 1] = 3.0 * m_coef3[size - 2] * h * h + 2.0 * m_coef2[size - 2] * h + m_coef1[size - 2];
        if(m_right == BCType::FirstDeriv) m_coef2[size - 1] = 0.0;
    }
    m_coef0 = (m_left == BCType::FirstDeriv) ? 0 : m_coef3[0];

    if(!m_monotonic && size > 2) makeMonotonic();
}

template <typename num_type>
inline void Interpolation<num_type>::UpdateCoefficients()
{
    size_t size = m_coef1.size();
    for(size_t i = 0; i < size - 1; ++i){
        const float_t h = m_x[i + 1] - m_x[i];
        m_coef2[i] = (3.0 * (m_y[i + 1] - m_y[i]) / h - (2.0 * m_coef1[i] + m_coef1[i + 1])) / h;
        m_coef3[i] = ((m_coef1[i + 1] - m_coef1[i]) / (3.0 * h) - 2.0 / 3.0 * m_coef2[i]) / h;
    }
    m_coef0 = (m_left == BCType::FirstDeriv) ? 0 : m_coef3[0];
}

template <typename num_type>
inline bool Interpolation<num_type>::makeMonotonic()
{
    assert(m_x.size() > 2);
    bool modified = false;
    for(size_t i = 0; i < m_x.size(); ++i){
        size_t im1 = std::max(i - 1, size_t(0));
        size_t ip1 = std::min(i + 1, m_x.size() - 1);
        if((m_y[im1] <= m_y[i] && m_y[i] <= m_y[ip1] && m_coef1[i] < 0) ||
           (m_y[im1] >= m_y[i] && m_y[i] >= m_y[ip1] && m_coef1[i] > 0)) {
            modified = true;
            m_coef1[i] = 0;
        }
    }

    for(size_t i = 0; i < m_x.size() - 1; ++i){
        float_t avg = float_t(m_y[i + 1] - m_y[i]) / float_t(m_x[i + 1] - m_x[i]);
        if(EQ(avg, 0.0) && (!EQ(m_coef1[i], 0.0) || !EQ(m_coef1[i], 0.0))){
            modified = true;
            m_coef1[i] = 0;
            m_coef1[i + 1] = 0;
        }
        else if((GE(m_coef1[i], 0.0) && GE(m_coef1[i + 1], 0.0) && GE(avg, 0.0)) ||
                (LE(m_coef1[i], 0.0) && LE(m_coef1[i + 1], 0.0) && LE(avg, 0.0))) {
            float_t r = std::sqrt(m_coef1[i] * m_coef1[i] + m_coef1[i + 1] * m_coef1[i + 1]) / std::fabs(avg);
            if(r > 3.0){
                modified = true;
                m_coef1[i] *= (3.0 / r);
                m_coef1[i + 1] *= (3.0 / r);
            }
        }
    }

    if(modified == true){
        UpdateCoefficients();
        m_monotonic = true;
    }
    return modified;
}

template <typename num_type>
inline size_t Interpolation<num_type>::FindClosest(num_type x) const
{
    typename std::vector<num_type>::const_iterator iter;
    iter = std::upper_bound(m_x.begin(), m_x.end(), x);
    size_t index = std::max(int(iter - m_x.begin() - 1), 0);
    return index;
}

template <typename num_type>
inline num_type Interpolation<num_type>::Interpolate(num_type x) const
{
    return Interpolate(x, typename num_traits_float_or_int<num_type>::tag());
}

template <typename num_type>
inline num_type Interpolation<num_type>::Interpolate(num_type x, const num_integer_tag) const
{
    return num_type(std::round(Interpolate(x, num_floating_tag())));
}

template <typename num_type>
inline float_type<num_type> Interpolation<num_type>::Interpolate(num_type x, const num_floating_tag) const
{
    size_t n = m_x.size();
    size_t idx = FindClosest(x);
    float_t h = x - m_x[idx];
    if(x < m_x[0]) return (m_coef0 * h + m_coef1[0]) * h + m_y[0];
    else if(x > m_x[n - 1]) return (m_coef2[n-1] * h + m_coef1[n - 1]) * h + m_y[n - 1];
    else return ((m_coef3[idx] * h + m_coef2[idx]) * h + m_coef1[idx]) * h + m_y[idx];
}

}//namespace math
}//namespace generic
#endif//GENERIC_MATH_INTERPOLATION_HPP


#include "Tetrahedron.h"
#include <Ziran/CS/Util/Debug.h>
#include <Eigen/Dense>

namespace ZIRAN {

template <class T>
void barycentricWeights(
    const Vector<T, 3>& p,
    const Vector<T, 3>& a,
    const Vector<T, 3>& b,
    const Vector<T, 3>& c,
    const Vector<T, 3>& d,
    Vector<T, 4>& weights)
{
    Matrix<T, 3, 3> Dm;
    Dm.col(0) = a - d;
    Dm.col(1) = b - d;
    Dm.col(2) = c - d;

    Vector<T, 3> weights_for_abc = Dm.partialPivLu().solve(p - d);
    weights.template head<3>() = weights_for_abc;
    weights(3) = (T)1 - weights_for_abc.array().sum();
    ZIRAN_ASSERT((weights.array() >= (T)0).all(), weights.transpose());
    ZIRAN_ASSERT((weights.array() <= (T)1).all(), weights.transpose());
    ZIRAN_ASSERT(std::abs(weights.array().sum() - (T)1) < 1e-7, weights.transpose());

    Vector<T, 3> test_p = Vector<T, 3>::Constant((T)0);
    test_p += weights(0) * a;
    test_p += weights(1) * b;
    test_p += weights(2) * c;
    test_p += weights(3) * d;
    ZIRAN_ASSERT((p - test_p).squaredNorm() < 1e-7);
}

template void barycentricWeights<double>(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1>&);
template void barycentricWeights<float>(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&, Eigen::Matrix<float, 3, 1, 0, 3, 1> const&, Eigen::Matrix<float, 3, 1, 0, 3, 1> const&, Eigen::Matrix<float, 3, 1, 0, 3, 1> const&, Eigen::Matrix<float, 3, 1, 0, 3, 1> const&, Eigen::Matrix<float, 4, 1, 0, 4, 1>&);
} // namespace ZIRAN


//          Copyright Christopher Kormanyos 2021.
// 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)

#include <boost/math/bindings/e_float.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/multiprecision/number.hpp>

// cd C:\Users\User\Documents\Ks\PC_Software\NumericalPrograms\ExtendedNumberTypes\e_float\libs\e_float\build

// g++ -Wall -Wextra -m64 -O3 -std=gnu++11 -DE_FLOAT_TYPE_EFX -I../../../libs/e_float/src -IC:/boost/modular_boost/boost/libs/multiprecision/include -IC:/boost/modular_boost/boost/libs/math/include -IC:/boost/boost_1_75_0 ../src/e_float/efx/e_float_efx.cpp ../src/e_float/e_float.cpp ../src/e_float/e_float_base.cpp ../src/functions/constants/constants.cpp ../src/functions/elementary/elementary_complex.cpp ../src/functions/elementary/elementary_hyper_g.cpp ../src/functions/elementary/elementary_math.cpp ../src/functions/elementary/elementary_trans.cpp ../src/functions/elementary/elementary_trig.cpp ../test_boost/test_boost.cpp -o test_boost.exe

namespace local
{
  using big_float_type = boost::multiprecision::number<boost::math::ef::e_float, boost::multiprecision::et_off>;

  template<typename FloatingPointType>
  FloatingPointType hypergeometric_2f1(const FloatingPointType& a,
                                       const FloatingPointType& b,
                                       const FloatingPointType& c,
                                       const FloatingPointType& x)
  {
    // Compute the series representation of hypergeometric_2f1
    // taken from Abramowitz and Stegun 15.1.1.
    // There are no checks on input range or parameter boundaries.

    FloatingPointType x_pow_n_div_n_fact(x);
    FloatingPointType pochham_a         (a);
    FloatingPointType pochham_b         (b);
    FloatingPointType pochham_c         (c);
    FloatingPointType ap                (a);
    FloatingPointType bp                (b);
    FloatingPointType cp                (c);

    FloatingPointType h2f1 = 1 + (((pochham_a * pochham_b) / pochham_c) * x_pow_n_div_n_fact);

    const FloatingPointType tol = std::numeric_limits<FloatingPointType>::epsilon() * x;

    // Series expansion of hyperg_2f1(a, b; c; x).
    for(std::int32_t n = INT32_C(2); n < INT32_C(100000); ++n)
    {
      x_pow_n_div_n_fact *= x;
      x_pow_n_div_n_fact /= n;

      pochham_a *= ++ap;
      pochham_b *= ++bp;
      pochham_c *= ++cp;

      const FloatingPointType term = ((pochham_a * pochham_b) / pochham_c) * x_pow_n_div_n_fact;

      using std::fabs;

      if((n > 11) && (fabs(term) < tol))
      {
        break;
      }

      h2f1 += term;
    }

    return h2f1;
  }

  template<typename FloatingPointType>
  FloatingPointType hypergeometric_2f1_regularized(const FloatingPointType& a,
                                                   const FloatingPointType& b,
                                                   const FloatingPointType& c,
                                                   const FloatingPointType& x)
  {
    return hypergeometric_2f1(a, b, c, x) / boost::math::tgamma(c);
  }

  template<typename FloatingPointType>
  FloatingPointType pochhammer(const FloatingPointType& x,
                               const FloatingPointType& a)
  {
    return boost::math::tgamma(x + a) / boost::math::tgamma(x);
  }

  template<typename FloatingPointType>
  FloatingPointType legendre_pvu(const FloatingPointType& v,
                                 const FloatingPointType& u,
                                 const FloatingPointType& x)
  {
    using std::pow;

    // See also the third series representation provided in:
    // https://functions.wolfram.com/HypergeometricFunctions/LegendreP2General/06/01/04/

    const FloatingPointType u_half       = u / 2U;
    const FloatingPointType one_minus_x  = 1U - x;
    const FloatingPointType one_minus_mu = 1U - u;

    const FloatingPointType tgamma_term    = boost::math::tgamma(one_minus_mu);
    const FloatingPointType h2f1_reg_term  = hypergeometric_2f1_regularized(FloatingPointType(-v),
                                                                            FloatingPointType(1U + v),
                                                                            one_minus_mu,
                                                                            FloatingPointType(one_minus_x / 2U));

    return (pow(1U + x, u_half) * h2f1_reg_term) / pow(one_minus_x, u_half);
  }

  template<typename FloatingPointType>
  FloatingPointType legendre_qvu(const FloatingPointType& v,
                                 const FloatingPointType& u,
                                 const FloatingPointType& x)
  {
    using std::cos;
    using std::pow;
    using std::sin;

    // See also the third series representation provided in:
    // https://functions.wolfram.com/HypergeometricFunctions/LegendreQ2General/06/01/02/

    const FloatingPointType u_pi     = u * boost::math::constants::pi<FloatingPointType>();
    const FloatingPointType sin_u_pi = sin(u_pi);
    const FloatingPointType cos_u_pi = cos(u_pi);

    const FloatingPointType one_minus_x          = 1U - x;
    const FloatingPointType one_plus_x           = 1U + x;
    const FloatingPointType u_half               = u / 2U;
    const FloatingPointType one_minus_x_over_two = one_minus_x / 2U;

    const FloatingPointType one_plus_x_over_one_minus_x_pow_u_half = pow(one_plus_x / one_minus_x, u_half);

    const FloatingPointType v_plus_one =  v + 1U;

    const FloatingPointType h2f1_1 = hypergeometric_2f1_regularized(FloatingPointType(-v), v_plus_one, FloatingPointType(1U - u), one_minus_x_over_two);
    const FloatingPointType h2f1_2 = hypergeometric_2f1_regularized(FloatingPointType(-v), v_plus_one, FloatingPointType(1U + u), one_minus_x_over_two);

    const FloatingPointType term1 = (h2f1_1 * one_plus_x_over_one_minus_x_pow_u_half) * cos_u_pi;
    const FloatingPointType term2 = (h2f1_2 / one_plus_x_over_one_minus_x_pow_u_half) * pochhammer(FloatingPointType(v_plus_one - u), FloatingPointType(u * 2U));

    return (boost::math::constants::half_pi<FloatingPointType>() * (term1 - term2)) / sin_u_pi;
  }
}

bool test___tgamma()
{
  const local::big_float_type x(0.5F);

  const local::big_float_type g       = boost::math::tgamma(x);
  const local::big_float_type sqrt_pi = boost::math::constants::root_pi<local::big_float_type>();

  std::cout << std::setprecision(std::numeric_limits<local::big_float_type>::digits10) << g       << std::endl;
  std::cout << std::setprecision(std::numeric_limits<local::big_float_type>::digits10) << sqrt_pi << std::endl;

  const local::big_float_type ratio = g / sqrt_pi;
  const local::big_float_type delta = fabs(1 - ratio);

  const bool result_is_ok = delta < std::numeric_limits<local::big_float_type>::epsilon() * 10U;

  return result_is_ok;
}

bool test_legendre()
{
  const local::big_float_type x = local::big_float_type(UINT32_C(789)) / 1000U;

  // Compute some values of the Legendre function of the second kind
  // on the real axis within the unit circle.

  const local::big_float_type lpvu = local::legendre_pvu(local::big_float_type(local::big_float_type(1U) / 3),
                                                         local::big_float_type(local::big_float_type(1U) / 7),
                                                         x);

  const local::big_float_type lqvu = local::legendre_qvu(local::big_float_type(local::big_float_type(1U) / 3),
                                                         local::big_float_type(local::big_float_type(1U) / 7),
                                                         x);

  // N[LegendreP[1/3, 1/7, 2, 789/1000], 104]
  const local::big_float_type control_lpvu
  {
    "0.99315918549340645725680897933376572969241094126736434178747245976770375217673830111149222182128969080027"
  };

  // N[LegendreQ[1/3, 1/7, 2, 789/1000], 104]
  const local::big_float_type control_lqvu
  {
    "0.18027013586354735033576549475861160812128148962186378344662781978695122523958952227406954299821460356031"
  };

  std::cout << std::setprecision(std::numeric_limits<local::big_float_type>::digits10) << lpvu         << std::endl;
  std::cout << std::setprecision(std::numeric_limits<local::big_float_type>::digits10) << control_lpvu << std::endl;
  std::cout << std::setprecision(std::numeric_limits<local::big_float_type>::digits10) << lqvu         << std::endl;
  std::cout << std::setprecision(std::numeric_limits<local::big_float_type>::digits10) << control_lqvu << std::endl;

  using std::fabs;

  const local::big_float_type closeness_lpvu = fabs(1 - (lpvu / control_lpvu));
  const local::big_float_type closeness_lqvu = fabs(1 - (lqvu / control_lqvu));

  const bool result_lpvu_is_ok = (closeness_lpvu < (std::numeric_limits<local::big_float_type>::epsilon() * UINT32_C(10000000)));
  const bool result_lqvu_is_ok = (closeness_lqvu < (std::numeric_limits<local::big_float_type>::epsilon() * UINT32_C(10000000)));

  const bool result_is_ok = (result_lpvu_is_ok && result_lqvu_is_ok);

  return result_is_ok;
}

bool test_boost_sf()
{
  const bool test_tgamma___is_ok = test___tgamma();
  const bool test_legendre_is_ok = test_legendre();

  std::cout << "test_tgamma___is_ok: " << std::boolalpha << test_tgamma___is_ok << std::endl;
  std::cout << "test_legendre_is_ok: " << std::boolalpha << test_legendre_is_ok << std::endl;

  const bool result_is_ok = (test_tgamma___is_ok && test_legendre_is_ok);

  return result_is_ok;
}


/*
 * FileName: main.cpp
 * Description: Main program.
 * Copyright (C) 2014  K M Masum Habib <masum.habib@gmail.com>
 * Created: 02 Nov 2014.
 */

#include "main.h"
#include "config.h"

#include <iostream>
#include <string>
#include <stdexcept>

#include <armadillo>

using namespace std;
using namespace arma;

/*
 * The main function
 */
int main(int argc, char** argv) {

    cout << "Armadillo Minimum Project v" << ARMAMIN_VERSION << endl;

    mat A = randu<mat>(3,3); // construct a random 3x3 matrix.
    mat B;
    B << 1 << 2 << 3 << endr // B = | 1    2    3 |
      << 4 << 5 << 6 << endr //     | 4    5    6 |
      << 7 << 8 << 9 << endr;//     | 7    8    8 |

    // print A and B
    cout << "A = " << endl << A << endl;
    cout << "B = " << endl << B << endl;

    // compute A*B
    mat C = A*B;
    // show A*B
    cout << "A*B = " << endl << C << endl;

    // compute A+B
    C = A+B;
    // show A+B
    cout << "A+B = " << endl << C << endl;

    // compute inverse
    C = solve(A, B);
    cout << "A\\B = " << endl << C << endl;

    // Eigenvalue solver
    cx_vec E = eig_gen(A);
    cout << "Eigen values of (A): " << endl << E << endl;

    return MAIN_SUCCESS;
}


//
// Created by moriya on 02/10/17.
//

#ifndef LIBSCAPI_MATRIX_H
#define LIBSCAPI_MATRIX_H

#include <iostream>
#include <NTL/GF2E.h>
#include <NTL/GF2X.h>
#include <NTL/ZZ_p.h>
#include <NTL/GF2XFactoring.h>
#include <iostream>
#include <vector>
#include <array>
#include <../../include/primitives/Mersenne.hpp>

using namespace NTL;

/**
 * A hyper-invertible matrix is a matrix of which every (non-trivial) square sub-matrix is invertible. Given
 * a hyper-invertible matrix M and vectors x and y satisfying y = M x, then given any |x| components of x
 * and y (any mixture is fine!), one can compute all other components of x and y as a linear function from
 * the given components.
 * Such matrices provide very good diversion and concentration properties: Given a vector x with random-ness in some components;
 * then this very same randomness can be observed in any components of y.
 * Similarly, given a vector x with up to k non-zero elements, then either y will have a non-zero element in
 * each subset of k components, or x is the zero-vector.
 * We present a construction of hyper-invertible matrices and a bunch of applications.
 */

using namespace std;
using namespace NTL;

template <typename FieldType>
class HIM {
private:
    int m_n,m_m;
    FieldType** m_matrix;
    TemplateField<FieldType> *field;
public:

    /**
     * This method allocate m-by-n matrix.
     * m rows, n columns.
     */
    HIM(int m, int n, TemplateField<FieldType> *field);

    HIM();

    /**
     * This method is a construction of a hyper-invertible m-by-n matrix M over a finite field F with |F| ≥ 2n.
     * Let α1,...,αn , β1,...,βm denote fixed distinct elements in F according the vectors alpha and beta,
     * and consider the function f:Fn → Fm,
     * mapping (x1,...,xn) to (y1,...,ym) such that the points (β1,y1),...,(βm,ym) lie on the polynomial g(·)
     * of degree n−1 defined by the points (α1,x1),...,(αn,xn).
     * Due to the linearity of Lagrange interpolation, f is linear and can be expressed as a matrix:
     * M = {λi,j} j=1,...n i=1,...,m
     * where λ i,j = {multiplication}k=1,..n (βi−αk)/(αj−αk)
     */
    FieldType** InitHIMByVectors(vector<FieldType> &alpha, vector<FieldType> &beta);

    FieldType** InitHIMVectorAndsizes(vector<FieldType> &alpha, int n, int m);

    /**
     * This method create vectors alpha and beta,
     * and init the matrix by the method InitHIMByVectors(alpha, beta).
     */
    FieldType** InitHIM();

    /**
     * This method print the matrix
     */
    void Print();

    /**
     * matrix/vector multiplication.
     * The result is the answer vector.
     */
    void MatrixMult(std::vector<FieldType> &vector, std::vector<FieldType> &answer);

    void allocate(int m, int n, TemplateField<FieldType> *field);

    virtual ~HIM();
};

template <typename FieldType>
HIM<FieldType>::HIM(){}

template <typename FieldType>
HIM<FieldType>::HIM(int m, int n, TemplateField<FieldType> *field) {
    // m rows, n columns
    this->m_m = m;
    this->m_n = n;
    this->field = field;
    this->m_matrix = new FieldType*[m_m];

    for (int i = 0; i < m_m; i++)
    {
        m_matrix[i] = new FieldType[m_n];
    }
}

template <typename FieldType>
FieldType** HIM<FieldType>::InitHIMByVectors(vector<FieldType> &alpha, vector<FieldType> &beta)
{
    FieldType lambda;

    int m = beta.size();
    int n = alpha.size();
    for (int i = 0; i < m; i++)
    {
        for (int j = 0; j < n; j++)
        {
            // lambda = 1
            lambda = *(field->GetOne());

            // compute value for matrix[i,j]
            for (int k = 0; k < n; k++)
            {
                if (k == j)
                {
                    continue;
                }

                lambda *= ((beta[i]) - (alpha[k])) / ((alpha[j]) - (alpha[k]));
            }

            // set the matrix
            (m_matrix[i][j]) = lambda;
        }
    }
    return m_matrix;
}

template <typename FieldType>
FieldType** HIM<FieldType>::InitHIMVectorAndsizes(vector<FieldType> &alpha, int n, int m)
{
    FieldType lambda;

    for (int i = 0; i < m; i++)
    {
        for (int j = 0; j < n; j++)
        {
            // lambda = 1
            lambda = *(field->GetOne());

            // compute value for matrix[i,j]
            for (int k = 0; k < n; k++)
            {
                if (k == j)
                {
                    continue;
                }

                lambda *= ((alpha[n+i]) - (alpha[k])) / ((alpha[j]) - (alpha[k]));
            }

            // set the matrix
            (m_matrix[i][j]) = lambda;
        }
    }
    return m_matrix;
}

template <typename FieldType>
void HIM<FieldType>::allocate(int m, int n, TemplateField<FieldType> *field)
{
    // m rows, n columns
    this->m_m = m;
    this->m_n = n;
    this->field = field;
    this->m_matrix = new FieldType*[m_m];
    for (int i = 0; i < m_m; i++)
    {
        m_matrix[i] = new FieldType[m_n];
    }
}

template <typename FieldType>
FieldType** HIM<FieldType>::InitHIM()
{
    int i;
    vector<FieldType> alpha(m_n);
    vector<FieldType> beta(m_m);

    // check if valid
    if (256 <= m_m+m_n)
    {
        cout << "error";
    }

    // Let alpha_j and beta_i be arbitrary field elements
    for (i = 0; i < m_n; i++)
    {
        alpha[i] = field->GetElement(i);
    }

    for (i = 0; i < m_m; i++)
    {
        beta[i] = field->GetElement(m_n+i);
    }

    return(InitHIMByVectors(alpha,beta));
}

template <typename FieldType>
void HIM<FieldType>::Print()
{
    for (int i = 0; i < m_m; i++) {
        for (int j = 0; j < m_n; j++) {
            cout << (m_matrix[i][j]) << " ";
        }

        cout << " " << '\n';
    }

}

template <typename FieldType>
void HIM<FieldType>::MatrixMult(std::vector<FieldType> &vector, std::vector<FieldType> &answer)
{
    FieldType temp1;
    for(int i = 0; i < m_m; i++)
    {
        // answer[i] = 0
        answer[i] = *(field->GetZero());

        for(int j=0; j < m_n; j++)
        {
            temp1 = m_matrix[i][j] * vector[j];
            //answer[i] = answer[i] + temp1;
            answer[i] += temp1;
        }
    }
}

template <typename FieldType>
HIM<FieldType>::~HIM() {
    for (int i = 0; i < m_m; i++)
    {
        delete[] m_matrix[i];
    }
    delete[] m_matrix;
}

template<typename FieldType>
class VDM {
private:
    int m_n,m_m;
    FieldType** m_matrix;
    TemplateField<FieldType> *field;
public:
    VDM(int n, int m, TemplateField<FieldType> *field);
    VDM() {};
    ~VDM();
    void InitVDM();
    void Print();
    void MatrixMult(std::vector<FieldType> &vector, std::vector<FieldType> &answer, int length);

    void allocate(int n, int m, TemplateField<FieldType> *field);
};

template<typename FieldType>
VDM<FieldType>::VDM(int n, int m, TemplateField<FieldType> *field) {
    this->m_m = m;
    this->m_n = n;
    this->field = field;
    this->m_matrix = new FieldType*[m_n];
    for (int i = 0; i < m_n; i++)
    {
        m_matrix[i] = new FieldType[m_m];
    }
}

template<typename FieldType>
void VDM<FieldType>::allocate(int n, int m, TemplateField<FieldType> *field) {

    this->m_m = m;
    this->m_n = n;
    this->field = field;
    this->m_matrix = new FieldType*[m_n];
    for (int i = 0; i < m_n; i++)
    {
        m_matrix[i] = new FieldType[m_m];
    }
}

template<typename FieldType>
void VDM<FieldType>::InitVDM() {
    vector<FieldType> alpha(m_n);
    for (int i = 0; i < m_n; i++) {
        alpha[i] = field->GetElement(i + 1);
    }

    for (int i = 0; i < m_n; i++) {
        m_matrix[i][0] = *(field->GetOne());
        for (int k = 1; k < m_m; k++) {
            m_matrix[i][k] = m_matrix[i][k - 1] * (alpha[i]);
        }
    }
}

/**
 * the function print the matrix
 */
template<typename FieldType>
void VDM<FieldType>::Print()
{
    for (int i = 0; i < m_n; i++)
    {
        for(int j = 0; j < m_m; j++)
        {
            cout << (m_matrix[i][j]) << " ";

        }
        cout << " " << '\n';
    }

}

template<typename FieldType>
void VDM<FieldType>::MatrixMult(std::vector<FieldType> &vector, std::vector<FieldType> &answer, int length)
{
    for(int i = 0; i < m_n; i++)
    {
        // answer[i] = 0
        answer[i] = *(field->GetZero());

        for(int j=0; j < length; j++)
        {
            answer[i] += (m_matrix[i][j] * vector[j]);
        }
    }

}
//
template<typename FieldType>
VDM<FieldType>::~VDM() {
    for (int i = 0; i < m_n; i++) {
        delete[] m_matrix[i];
    }
    delete[] m_matrix;
}

template<typename FieldType>
class VDMTranspose {
private:
    int m_n,m_m;
    FieldType** m_matrix;
    TemplateField<FieldType> *field;
public:
    VDMTranspose(int n, int m, TemplateField<FieldType> *field);
    VDMTranspose() {};
    ~VDMTranspose();
    void InitVDMTranspose();
    void Print();
    void MatrixMult(std::vector<FieldType> &vector, std::vector<FieldType> &answer, int length);

    void allocate(int n, int m, TemplateField<FieldType> *field);
};

template<typename FieldType>
VDMTranspose<FieldType>::VDMTranspose(int n, int m, TemplateField<FieldType> *field) {
    this->m_m = m;
    this->m_n = n;
    this->field = field;
    this->m_matrix = new FieldType*[m_n];
    for (int i = 0; i < m_n; i++)
    {
        m_matrix[i] = new FieldType[m_m];
    }
}

template<typename FieldType>
void VDMTranspose<FieldType>::allocate(int n, int m, TemplateField<FieldType> *field) {

    this->m_m = m;
    this->m_n = n;
    this->field = field;
    this->m_matrix = new FieldType*[m_n];
    for (int i = 0; i < m_n; i++)
    {
        m_matrix[i] = new FieldType[m_m];
    }
}

template<typename FieldType>
void VDMTranspose<FieldType>::InitVDMTranspose() {
    vector<FieldType> alpha(m_m);
    for (int i = 0; i < m_m; i++) {
        alpha[i] = field->GetElement(i + 1);
    }

    for (int i = 0; i < m_m; i++) {
        m_matrix[0][i] = *(field->GetOne());
        for (int k = 1; k < m_n; k++) {
            m_matrix[k][i] = m_matrix[k-1][i] * (alpha[k]);
        }
    }
}

/**
 * the function print the matrix
 */
template<typename FieldType>
void VDMTranspose<FieldType>::Print()
{
    for (int i = 0; i < m_m; i++)
    {
        for(int j = 0; j < m_n; j++)
        {
            cout << (m_matrix[i][j]) << " ";

        }
        cout << " " << '\n';
    }

}

template<typename FieldType>
void VDMTranspose<FieldType>::MatrixMult(std::vector<FieldType> &vector, std::vector<FieldType> &answer, int length)
{
    for(int i = 0; i < length; i++)
    {
        // answer[i] = 0
        answer[i] = *(field->GetZero());

        for(int j=0; j < m_m; j++)
        {
            answer[i] += (m_matrix[i][j] * vector[j]);
        }
    }

}

//
template<typename FieldType>
VDMTranspose<FieldType>::~VDMTranspose() {
    for (int i = 0; i < m_n; i++) {
        delete[] m_matrix[i];
    }
    delete[] m_matrix;
}

#endif //LIBSCAPI_MATRIX_H


#include <cassert>
#include <cmath>
#include <vector>

#include <boost/multiprecision/cpp_dec_float.hpp>

// https://stackoverflow.com/a/24519913
template <typename T>
std::vector<T> full_convolve(const std::vector<T> &f, const std::vector<T> &g) {
    const int nf = f.size();
    const int ng = g.size();
    const int n = nf + ng - 1;
    std::vector<T> ret(n, T());
    for (int i = 0; i < n; ++i) {
        const int jmin = (i >= ng - 1)? i - (ng - 1) : 0;
        const int jmax = (i <  nf - 1)? i            : nf - 1;
        for(int j = jmin; j <= jmax; ++j) {
            ret[i] += (f[j] * g[i - j]);
        }
    }
    return ret;
}

template <typename T>
class Lagrange {
public:
    Lagrange(const std::vector<T> &x, const std::vector<T> &w) :
        coefficients()
    {
        assert(x.size() == w.size());
        std::vector<T> p;
        p.push_back(0.0);
        for (std::size_t j = 0; j < x.size(); ++j) {
            std::vector<T> pt;
            pt.push_back(w[j]);
            for (std::size_t k = 0; k < x.size(); ++k) {
                if (k == j) {
                    continue;
                }
                T fac = x[j] - x[k];
                std::vector<T> poly;
                poly.push_back(1.0 / fac);
                poly.push_back(-x[k] / fac);
                pt = full_convolve(pt, poly);
            }
            p = add(p, pt);
        }
        coefficients = p;
    }

    T operator ()(const T &x) const {
        assert(coefficients.size() > 0);
        T result = 0.0;
        std::size_t d = coefficients.size() - 1;
        for (std::size_t i = 0; i < d; ++i) {
            // result += std::pow(x, d) * coefficients[i];
            result += coefficients[i];
            result *= x;
        }
        result += coefficients[d];
        return result;
    }

    std::vector<T> add(const std::vector<T> &a, const std::vector<T> &b) const {
        int diff = a.size() - b.size();
        if ( diff == 0 ) {
            std::vector<T> result;
            for (std::size_t i = 0; i < a.size(); ++i) {
                result.push_back(a[i] + b[i]);
            }
            return result;
        } else if ( diff > 0 ) {
            std::vector<T> new_b(diff, 0);
            new_b.reserve(diff + b.size());
            new_b.insert(new_b.end(), b.begin(), b.end());
            return add(a, new_b);
        } else {
            std::vector<T> new_a(-diff, 0);
            new_a.reserve(-diff + a.size());
            new_a.insert(new_a.end(), a.begin(), a.end());
            return add(new_a, b);
        }
    }

    std::vector<T> coefficients;

    static T fast(T x, const std::vector<T> &xs, const std::vector<T> &ws) {
        assert(xs.size() == ws.size());
        T result = 0.0;
        for (std::size_t j = 0; j < xs.size(); ++j) {
            T n = 1.0, d = 1.0;
            for (std::size_t k = 0; k < xs.size(); ++k) {
                if ( j == k ) {
                    continue;
                }
                n *= x     - xs[k];
                d *= xs[j] - xs[k];
            }
            result += ws[j] * (n / d);
        }
        return result;
    }

};

template <typename T>
class DeltaLagrange {
public:
    DeltaLagrange(const std::vector<T> &x, const std::vector<T> &w) :
        x0(x[0]),
        w0(w[0]),
        coefficients()
    {
        assert(x.size() == w.size());
        std::vector<T> p;
        p.push_back(0.0);
        for (std::size_t j = 0; j < x.size(); ++j) {
            std::vector<T> pt;
            pt.push_back(w[j] - w0);
            auto xj = x[j] - x0;
            for (std::size_t k = 0; k < x.size(); ++k) {
                if (k == j) {
                    continue;
                }
                auto xk = x[k] - x0;
                T fac = xj - xk;
                std::vector<T> poly;
                poly.push_back(1.0 / fac);
                poly.push_back(-xk / fac);
                pt = full_convolve(pt, poly);
            }
            p = add(p, pt);
        }
        coefficients = p;
    }

    T operator ()(const T &x) const {
        assert(coefficients.size() > 0);
        T result = 0.0;
        auto xd = x - x0;
        std::size_t d = coefficients.size() - 1;
        for (std::size_t i = 0; i < d; ++i) {
            // result += std::pow(x, d) * coefficients[i];
            result += coefficients[i];
            result *= xd;
        }
        result += coefficients[d];
        return result + w0;
    }

    std::vector<T> add(const std::vector<T> &a, const std::vector<T> &b) const {
        int diff = a.size() - b.size();
        if ( diff == 0 ) {
            std::vector<T> result;
            for (std::size_t i = 0; i < a.size(); ++i) {
                result.push_back(a[i] + b[i]);
            }
            return result;
        } else if ( diff > 0 ) {
            std::vector<T> new_b(diff, 0);
            new_b.reserve(diff + b.size());
            new_b.insert(new_b.end(), b.begin(), b.end());
            return add(a, new_b);
        } else {
            std::vector<T> new_a(-diff, 0);
            new_a.reserve(-diff + a.size());
            new_a.insert(new_a.end(), a.begin(), a.end());
            return add(new_a, b);
        }
    }

    T x0;
    T w0;
    std::vector<T> coefficients;

};


///2
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Triangulation_face_base_2.h>
#include <vector>
#include <iostream>
#include <queue>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/bipartite.hpp>

// Epic kernel is enough, no constructions needed, provided the squared distance
// fits into a double (!)
typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
// we want to store an index with each vertex
typedef std::size_t                                            Index;
typedef CGAL::Triangulation_vertex_base_with_info_2<Index,K>   Vb;
typedef CGAL::Triangulation_face_base_2<K>                     Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb>            Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds>                  Delaunay;

// BGL Graph definitions
// =====================
// Graph Type, OutEdgeList Type, VertexList Type, (un)directedS
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS    // Use vecS for the VertexList! Choosing setS for the OutEdgeList disallows parallel edges.
    >          graph;
typedef boost::graph_traits<graph>::vertex_descriptor    vertex_desc;    // Vertex Descriptor: with vecS vertex list, this is really just an int in the range [0, num_vertices(G)).
typedef boost::graph_traits<graph>::edge_iterator    edge_it;    // to iterate over all edges
typedef  boost::graph_traits<graph>::out_edge_iterator      out_edge_it;
// Coloring definitions
typedef std::vector< boost::default_color_type > partition_t;

void testcase() {
  Index n, m;
  double r;
  std::cin >> n >> m >> r;

  // read points: first, we read all points and store them into a vector,
  // together with their indices
  typedef std::pair<K::Point_2,Index> IPoint;
  std::vector<IPoint> points;
  points.reserve(n);
  for (Index i = 0; i < n; ++i) {
    int x, y;
    std::cin >> x >> y;
    points.emplace_back(K::Point_2(x, y), i);
  }

  Delaunay t;
  t.insert(points.begin(), points.end());

  graph G(n);
  for (auto e = t.finite_edges_begin(); e != t.finite_edges_end(); ++e) {
    Index i1 = e->first->vertex((e->second+1)%3)->info();
    Index i2 = e->first->vertex((e->second+2)%3)->info();
    if(t.segment(e).squared_length() <= r * r) {
      boost::add_edge(int(i1), int(i2), G);
    }
  }

  // check for bipartiteness of the graph with *only* the delaunay edges
  partition_t partition(n);
  bool without_interference = boost::is_bipartite(G, boost::get(boost::vertex_index, G),
        boost::make_iterator_property_map(partition.begin(), boost::get(boost::vertex_index, G)));
  if(without_interference) {
    // if bipartite only with delaunay edges, we can use the coloring found by BGL
    // to create one triangulation for each color. Since same colored points should
    // not be closer than r to each other, this new triangulation should not include small edges
    std::vector<K::Point_2> points_1;
    std::vector<K::Point_2> points_2;
    for(auto v = t.finite_vertices_begin(); v != t.finite_vertices_end(); ++v) {
        Index i = v->info();
        if(partition[i] == boost::color_traits<boost::default_color_type>::white()) {
            points_1.push_back(v->point());
        } else {
            points_2.push_back(v->point());
        }
    }

    for(int i = 0; i < 2; i++) {
        if(!without_interference) break;
        Delaunay ti;
        if(i == 0) ti.insert(points_1.begin(), points_1.end());
        else ti.insert(points_2.begin(), points_2.end());
        // loop through edges
        for (auto e = ti.finite_edges_begin(); e != ti.finite_edges_end(); ++e) {
            if(t.segment(e).squared_length() <= r * r) {
                without_interference = false;
                break;
            }
        }
    }
  }

  std::vector<int> component_map(n);	// We MUST use such a vector as an Exterior Property Map: Vertex -> Component
  boost::connected_components(G,
        boost::make_iterator_property_map(component_map.begin(), boost::get(boost::vertex_index, G)));

  for(Index i = 0; i < m; i++) {
    int a0, a1, b0, b1;
    std::cin >> a0 >> a1 >> b0 >> b1;
    if(!without_interference) {
        std::cout << "n";
        continue;
    }
    K::Point_2 a = K::Point_2(a0, a1);
    K::Point_2 b = K::Point_2(b0, b1);

    if(CGAL::squared_distance(a, b) <= r * r) {
      std::cout << "y";
      continue;
    }
    auto vertex_a = t.nearest_vertex(a);
    auto vertex_b = t.nearest_vertex(b);

    if(CGAL::squared_distance(a, vertex_a->point()) > r * r || CGAL::squared_distance(b, vertex_b->point()) > r * r) {
        std::cout << "n";
        continue;
    }

    int va = vertex_a->info();
    int vb = vertex_b->info();
    if(component_map[va] == component_map[vb]) {
        std::cout << "y";
        continue;
    } else {
        std::cout << "n";
        continue;
    }
  }

  std::cout << "\n";

}

int main()
{
  std::ios_base::sync_with_stdio(false);
  std::size_t t;
  for (std::cin >> t; t > 0; --t) testcase();
  return 0;
}