#ifndef HELPERSDNA
#define HELPERSDNA

#include <boost/operators.hpp>
#include <iostream>
#include <ostream>
#include <boost/bimap.hpp>
#include <string>
#include <map>
#include<vector>

using namespace std;

template<class vectt>
unsigned hamming_distance(vectt& v1,vectt& v2){
	assert(v1.size()=v2.size());
	unsigned hd = 0;
	for(unsigned i=0;i<v1.size();++i)
		if(v1[i] != v2[i]) hd++;
	return hd;
}

template<class vectt>
void flipvecdir(vectt& v){
	vectt tmp = v;
	for(unsigned i=0;i<v.size();++i){
		v[i] = tmp[v.size()-1-i];
	}
}

// convert decimal number to a number with another base
template<class base_t>
vector<base_t> tobase(unsigned nu, unsigned base, unsigned size){

	vector<base_t> nn(size,(base_t) 0 );
	if(nu == 0) return nn;

	unsigned div = nu;
	unsigned i = 0;
	while(div != 0){
		nn[i] = base_t(div % base); // rest
		div = div/base;
		i++;
	}
	return nn;
};

// convert vector of numbers in some basis to
template<class base_t>
unsigned frombase(const vector<base_t>& nn,unsigned base){
	int nu = 0;
	//base_t adf =  nn[0];
	//int adfi =  (int) adf;
	unsigned basepow = 1;
	for(unsigned i=0;i<nn.size();++i) {

		base_t adf = nn[i];
		nu += ((unsigned)adf) * basepow;//((unsigned) (adf)) * ((int) pow((double) base,(int) i));
		basepow *= base;
	}
	return nu;
};

// converts each 2 characters in data_char to 3 elements of GF(47), which are appended at
template<class pfe>
void char2pfe(string& data_char, vector<pfe>& data_b47){
    if((data_char.size() % 2) != 0 )data_char.push_back('\n');

    data_b47.resize(0);
	// convert vector of char to vector with base 47;
    // every 2 char's are coded to three pfe's
    for(unsigned i=0;i<data_char.size()/2; ++i){
        // the unsigned char type below is important, otherwise errors occur when converting..
		unsigned indec = frombase(vector<unsigned char>(data_char.begin()+i*2, data_char.begin()+i*2+2),256);
        vector<char> adf(data_char.begin()+i*2, data_char.begin()+i*2+2);
        //for(unsigned j=0;j<adf.size();++j) cout << adf[j];
        //cout << endl;

        vector<pfe> tmpvec = tobase<pfe>(indec,47,3);
        data_b47.insert(data_b47.begin()+i*3,tmpvec.begin(),tmpvec.end());
    	//cout << i*3 << "  " << data_b47.size() << endl;
	}
};

template<class pfe>
void pfe2char(string& data_char, vector<pfe>& data_b47){

    assert(data_b47.size() % 3  == 0);

    data_char.resize(2*(data_char.size()/3) );
    // convert vector of char to vector with base 47;
    // every 2 char's are coded to three pfe's
    for(unsigned i=0;i<data_b47.size()/3; ++i){
        unsigned indec = frombase(vector<pfe>(data_b47.begin()+i*3, data_b47.begin()+i*3+3),47);
        //vector<char> adf(data_b47.begin()+i*3, data_b47.begin()+i*3+3);
        //for(unsigned j=0;j<adf.size();++j) cout << adf[j];
        //cout << endl;

        vector<char> tmpvec = tobase<char>(indec,256,2);
        data_char.insert(data_char.begin()+i*2,tmpvec.begin(),tmpvec.end());
    }

};

/*
maps each element of GF(47) to a string with letters {A,C,G,T} where
*/

template<class PFE>
class DNAmap{
private:
	//typedef boost::bimap< PFE , std::string > mapdna_type;
	//mapdna_type mapdna;

	map<PFE,string> pfetostr;
	map<string,PFE> strtopfe;

public:
	DNAmap(){
		//initialize map
		unsigned prime = 47;

		char nucl[] = "ACGT"; // nucleotides

		vector<string> allpos(4*4*4);
		for(unsigned i=0;i<allpos.size();++i){
			char cur[] = "AAA";
			cur[0] = nucl[i % 4];
			cur[1] = nucl[(i/4) % 4];
			cur[2] = nucl[(i/16) % 4];
			allpos[i] = string(cur);
		}

		unsigned j = 0;
		for(unsigned i=0;i<prime;++i){
			while( allpos[j][1] == allpos[j][2]) j++;
			//mapdna.insert( typename mapdna_type::value_type(PFE(i), allpos[j] ));
			pfetostr[PFE(i)] = allpos[j];
			strtopfe[allpos[j]] = PFE(i);

			j++;
		}
		//print_map(mapdna.right, " ", cout);
	}

	/*
	void printmap(){
		for(auto it = pfetostr.cbegin(); it != pfetostr.cend(); ++it){
    		std::cout << it->first << " " << it->second << std::endl;
		}
	}
	*/

	// codeword to DNA fragment
	void cw2frag(const vector<PFE>& cw, string& frag ){
		frag.resize(0);
		for(unsigned i=0;i<cw.size();++i){
			frag.append(pfetostr[cw[i]]); //mapdna.left.at(cw[i]);
		}
	}

	// DNA fragment to codeword
	void frag2cw(vector<PFE>& cw, const string& frag ){
		assert( (frag.size() % 3) == 0   );
		cw.resize(frag.size()/3);
		for(unsigned i=0;i<cw.size();++i){

			//char charartmp[] = "AAA"; // nucleotides
			//charartmp[0] = frag[i*3];
			//charartmp[1] = frag[i*3+1];
			//charartmp[2] = frag[i*3+2];
			//string tmp(charartmp);
			string tmp = string(frag.begin()+i*3, frag.begin()+i*3+3  );

			//print_map(mapdna.right, " ", cout);

			//cout <<  mapdna.right.at(tmp )  << endl;
			//cw[i] = PFE( mapdna.right.at(tmp ) );
			cw[i] = strtopfe[tmp];
			//cout << cw[i] << endl;
		}
	}

};

#endif


#include <boost/mp11/mpl.hpp>
#include <type_traits>
#include <string>

template <int I>
using int_ = std::integral_constant<int, I>;

using namespace boost::mp11;

template <typename Sequence, typename Value>
struct index_of_impl
{
	using index = mp_find<Sequence, Value>;
	using size = mp_size<Sequence>;
	using index_smaller_than_size = mp_less<index, size>;
	using type = mp_if<index_smaller_than_size, index, int_<-1>>;
};

template <typename Sequence, typename Value>
using index_of = typename index_of_impl<Sequence, Value>::type;

int main()
{
	using l = mp_list<std::string, int, bool>;

	constexpr int r1 = index_of<l, bool>::value;
	static_assert(r1 == 2);

	constexpr int r2 = index_of<l, double>::value;
	static_assert(r2 == -1);
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <sophus/se3.hpp>
#include <chrono>

using namespace std;
using namespace cv;

// void find_feature_matches(
//   const Mat &img_1, const Mat &img_2,
//   std::vector<KeyPoint> &keypoints_1,
//   std::vector<KeyPoint> &keypoints_2,
//   std::vector<DMatch> &matches);

Point2d pixel2cam(const Point2d &p, const Mat &K) {
  return Point2d
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  vector<DMatch> match;
  // BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  double min_dist = 10000, max_dist = 0;

  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }

  printf("-- Max dist : %f \n", max_dist);
  printf("-- Min dist : %f \n", min_dist);

  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

struct ProjectionError
{
  ProjectionError(const Eigen::Vector2d& measurement, const Eigen::Vector3d& point,
                  const Eigen::Matrix3d& K) : _x(measurement), _X(point), _K(K)
    {}

  template <class T>
  bool operator() (const T* const params, T* residuals) const
  {
    T X[3] = {T(_X.x()), T(_X.y()), T(_X.z())};
    T rot_X[3];
    ceres::UnitQuaternionRotatePoint(params, X, rot_X);
    rot_X[0] += params[4];
    rot_X[1] += params[5];
    rot_X[2] += params[6];
    double fx = _K(0, 0);
    double fy = _K(1, 1);
    double cx = _K(0, 2);
    double cy = _K(1, 2);

    // T fx = T(_K(0, 0));
    // T fy = T(_K(1, 1));
    // T cx = T(_K(0, 2));
    // T cy = T(_K(1, 2));

    residuals[0] = _x[0] - ((rot_X[0] / rot_X[2]) * fx + cx); // parentheses have importance but do not known why
    residuals[1] = _x[1] - ((rot_X[1] / rot_X[2]) * fy + cy);

    // T rot[9];
    // ceres::QuaternionToRotation(params, rot);
    // Eigen::Matrix<T, 3, 3> R;
    // R << rot[0], rot[1], rot[2],
    //      rot[3], rot[4], rot[5],
    //      rot[6], rot[7], rot[8];

    // Eigen::Matrix<T, 3, 1> p(T(_X.x()), T(_X.y()), T(_X.z()));
    // Eigen::Matrix<T, 3, 1> t(params[4], params[5], params[6]);

    // Eigen::Matrix<T, 3, 3> K;
    // K << T(_K(0, 0)),  T(_K(0, 1)),  T(_K(0, 2)),
    //      T(_K(1, 0)),  T(_K(1, 1)),  T(_K(1, 2)),
    //      T(_K(2, 0)),  T(_K(2, 1)),  T(_K(2, 2));
    // Eigen::Matrix<T, 3, 1> p2 = K * (R * p + t);

    // residuals[0] = _x[0] - p2.x()/p2.z();
    // residuals[1] = _x[1] - p2.y()/p2.z();

    return true;
  }

  private:
    Eigen::Vector2d _x;
    Eigen::Vector3d _X;
    Eigen::Matrix3d _K;
};

void pose_refinement_ceres(const VecVector3d& points_3d, const VecVector2d& points_2d, const Eigen::Matrix3d& K, Sophus::SE3d& pose)
{
  Eigen::Vector3d trans = pose.translation();
  Eigen::Quaterniond quat = pose.so3().unit_quaternion();

  double pose_params[7] = {
    quat.w(), quat.x(), quat.y(), quat.z(), trans.x(), trans.y(), trans.z()
  };

  ceres::Problem problem;
  for (int i = 0; i < points_3d.size(); ++i)
  {
    problem.AddResidualBlock(
      new ceres::AutoDiffCostFunction<ProjectionError, 2, 7>(
      // new ceres::NumericDiffCostFunction<ProjectionError, ceres::CENTRAL, 2, 7>(
        new ProjectionError(points_2d[i], points_3d[i], K)
      ),
      nullptr,
      pose_params
    );
  }
  auto *quat_t_parameterization = new ceres::ProductParameterization(new ceres::QuaternionParameterization(),
                                                                     new ceres::IdentityParameterization(3));
  problem.SetParameterization(pose_params, quat_t_parameterization);

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
  options.minimizer_progress_to_stdout = false;

  ceres::Solver::Summary summary;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  ceres::Solve(options, &problem, &summary);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();

  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double >>(t2 - t1);
  cout << "optimization with ceres costs time: " << time_used.count() << " seconds." << endl;
  std::cout << summary.BriefReport() << std::endl;

  Eigen::Quaterniond q(pose_params[0], pose_params[1], pose_params[2], pose_params[3]);
  Eigen::Vector3d t(pose_params[4], pose_params[5], pose_params[6]);

  pose = Sophus::SE3d(q.toRotationMatrix(), t);
}

int main(int argc, char **argv) {
  // if (argc != 5) {
  //   cout << "usage: pose_estimation_3d2d img1 img2 depth1 depth2" << endl;
  //   return 1;
  // }
  string f1 = "../1.png"; //argv[1];
  string f2 = "../2.png"; //argv[2];
  string f3 = "../1_depth.png"; //argv[3];
  Mat img_1 = imread(f1, CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread(f2, CV_LOAD_IMAGE_COLOR);
  assert(img_1.data && img_2.data && "Can not load images!");

  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
  cout << "一共找到了" << matches.size() << "组匹配点" << endl;

  Mat d1 = imread(f3, CV_LOAD_IMAGE_UNCHANGED);
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  vector<Point3f> pts_3d;
  vector<Point2f> pts_2d;
  for (DMatch m:matches) {
    ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
    if (d == 0)   // bad depth
      continue;
    float dd = d / 5000.0;
    Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
    pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
    pts_2d.push_back(keypoints_2[m.trainIdx].pt);
  }

  cout << "3d-2d pairs: " << pts_3d.size() << endl;

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  Mat r, t;
  solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false);
  Mat R;
  cv::Rodrigues(r, R);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;

  cout << "R=" << endl << R << endl;
  cout << "t=" << endl << t << endl;

  VecVector3d pts_3d_eigen;
  VecVector2d pts_2d_eigen;
  for (size_t i = 0; i < pts_3d.size(); ++i) {
    pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
    pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
  }
  Eigen::Matrix3d K_eigen;
  K_eigen << 520.9, 0, 325.1,
             0, 521.0, 249.7,
             0, 0, 1;

  // Ceres
  cout << "Custom Ceres" << endl;
  Sophus::SE3d pose_ceres;
  pose_refinement_ceres(pts_3d_eigen, pts_2d_eigen, K_eigen, pose_ceres);
  Eigen::Vector3d t_ceres = pose_ceres.translation();
  Eigen::Matrix3d R_ceres = pose_ceres.so3().unit_quaternion().toRotationMatrix();
  cout << "R_ceres = " << endl << R_ceres << endl;
  cout << "t_ceres = " << endl << t_ceres << endl;

  return 0;
}


#include <alglib/ap.h>
#include <alglib/interpolation.h>

#include "spline-1d.hpp"

#define This Spline1d

namespace perceive
{
// ----------------------------------------------------------- inclusive-between

// inclusive_between<int>(5, x, 8);
template<typename T>
inline bool
inclusive_between(const T& low_bound, const T& value, const T& high_bound)
{
   return value <= high_bound && value >= low_bound;
}

// ----------------------------------------------------------------------- Pimpl

class This::Pimpl
{
 public:
   Pimpl()
       : is_init(false)
       , n_control_points(0)
       , approx_length(dNAN)
   {}

   bool is_init;
   uint n_control_points;
   double approx_length;
   alglib::spline1dinterpolant spline_x;
   alglib::spline1dfitreport rep_x;

   double min_x{0.0};
   double max_x{1.0};
   double range_inv{1.0}; // 1.0 / (max_x - min_x)

   void update_range_inv() { range_inv = 1.0 / (max_x - min_x); }
};

// ---------------------------------------------------------------- Construction

This::Spline1d()
    : pimpl(make_unique<Pimpl>())
{}

This::Spline1d(const Spline1d& rhs)
    : Spline1d()
{
   *this = rhs;
}

This::Spline1d(Spline1d&& rhs)
    : pimpl{nullptr}
{
   *this = std::move(rhs);
}

This::Spline1d(const std::vector<double>& ps, double rho, int M_factor)
    : Spline1d()
{
   init(ps, rho, M_factor);
}
This::Spline1d(const std::vector<float>& ps, double rho, int M_factor)
    : Spline1d()
{
   init(ps, rho, M_factor);
}
This::Spline1d(const std::vector<Vector2>& ps, double rho, int M_factor)
    : Spline1d()
{
   init(ps, rho, M_factor);
}
This::Spline1d(const std::deque<double>& ps, double rho, int M_factor)
    : Spline1d()
{
   init(ps, rho, M_factor);
}
This::Spline1d(const std::deque<float>& ps, double rho, int M_factor)
    : Spline1d()
{
   init(ps, rho, M_factor);
}
This::Spline1d(const std::deque<Vector2>& ps, double rho, int M_factor)
    : Spline1d()
{
   init(ps, rho, M_factor);
}

This::Spline1d(const std::vector<array<double, 6>>& coefficients)
    : Spline1d()
{
   init(coefficients);
}

This::~Spline1d() = default;

Spline1d& This::operator=(const Spline1d& rhs)
{
   if(this != &rhs) *pimpl = *rhs.pimpl;
   return *this;
}

Spline1d& This::operator=(Spline1d&& rhs) = default;

// ------------------------------------------------------------------- Accessors

bool This::is_init() const { return pimpl->is_init; }
uint This::n_control_points() const { return pimpl->n_control_points; }
double This::approx_length() const { return pimpl->approx_length; }

double This::rms_error() const { return pimpl->rep_x.rmserror; }
double This::avg_error() const { return pimpl->rep_x.avgerror; }
double This::avg_rel_error() const { return pimpl->rep_x.avgrelerror; }
double This::max_error() const { return pimpl->rep_x.maxerror; }

real This::min_x() const { return pimpl->min_x; }
real This::max_x() const { return pimpl->max_x; }
void This::set_min_x(real x)
{
   pimpl->min_x = x;
   pimpl->update_range_inv();
}
void This::set_max_x(real x)
{
   pimpl->max_x = x;
   pimpl->update_range_inv();
}

// -------------------------------------------------------------------- p-spline

/*************************************************************************
Fitting by penalized cubic spline.

Equidistant grid with M nodes on [min(x,xc),max(x,xc)] is  used  to  build
basis functions. Basis functions are cubic splines with  natural  boundary
conditions. Problem is regularized by  adding non-linearity penalty to the
usual least squares penalty function:

    S(x) = arg min { LS + P }, where
    LS   = SUM { w[i]^2*(y[i] - S(x[i]))^2 } - least squares penalty
    P    = C*10^rho*integral{ S''(x)^2*dx } - non-linearity penalty
    rho  - tunable constant given by user
    C    - automatically determined scale parameter,
           makes penalty invariant with respect to scaling of X, Y, W.

INPUT PARAMETERS:
    X   -   points, array[0..N-1].
    Y   -   function values, array[0..N-1].
    N   -   number of points (optional):
            * N>0
            * if given, only first N elements of X/Y are processed
            * if not given, automatically determined from X/Y sizes
    M   -   number of basis functions ( = number_of_nodes), M>=4.
    Rho -   regularization  constant  passed   by   user.   It   penalizes
            nonlinearity in the regression spline. It  is  logarithmically
            scaled,  i.e.  actual  value  of  regularization  constant  is
            calculated as 10^Rho. It is automatically scaled so that:
            * Rho=2.0 corresponds to moderate amount of nonlinearity
            * generally, it should be somewhere in the [-8.0,+8.0]
            If you do not want to penalize nonlineary,
            pass small Rho. Values as low as -15 should work.

OUTPUT PARAMETERS:
    Info-   same format as in LSFitLinearWC() subroutine.
            * Info>0    task is solved
            * Info<=0   an error occured:
                        -4 means inconvergence of internal SVD or
                           Cholesky decomposition; problem may be
                           too ill-conditioned (very rare)
    S   -   spline interpolant.
    Rep -   Following fields are set:
            * RMSError      rms error on the (X,Y).
            * AvgError      average error on the (X,Y).
            * AvgRelError   average relative error on the non-zero Y
            * MaxError      maximum error
                            NON-WEIGHTED ERRORS ARE CALCULATED

IMPORTANT:
    this subroitine doesn't calculate task's condition number for K<>0.

NOTE 1: additional nodes are added to the spline outside  of  the  fitting
interval to force linearity when x<min(x,xc) or x>max(x,xc).  It  is  done
for consistency - we penalize non-linearity  at [min(x,xc),max(x,xc)],  so
it is natural to force linearity outside of this interval.

NOTE 2: function automatically sorts points,  so  caller may pass unsorted
array.

  -- ALGLIB PROJECT --
     Copyright 18.08.2009 by Bochkanov Sergey
*************************************************************************/

template<typename U>
bool init_pspline(Spline1d& curve,
                  const U& container,
                  double rho,
                  unsigned M_factor)
{
   curve.pimpl->n_control_points = 0;
   curve.pimpl->is_init          = false;
   curve.pimpl->approx_length    = dNAN;

   const uint32_t len = uint32_t(container.size());
   if(len < 5) return false;

   alglib::real_1d_array T;
   alglib::real_1d_array X;
   T.setlength(len);
   X.setlength(len);

   alglib::ae_int_t counter = 0;
   double t                 = 0.0;
   double dt                = 1.0 / double(len - 1);
   for(const auto& p : container) {
      T[counter] = t;
      X[counter] = real(p);
      t += dt;
      counter++;
   }

   alglib::ae_int_t info_x;
   alglib::ae_int_t M = std::max<int>(4, int(M_factor));

   alglib::spline1dfitpenalized(
       T, X, M, rho, info_x, curve.pimpl->spline_x, curve.pimpl->rep_x);

   curve.pimpl->n_control_points = unsigned(M);

   if(info_x > 0) {
      curve.pimpl->is_init       = true;
      curve.pimpl->approx_length = 0.0;
      double dt                  = 0.1 / double(M - 1);
      real p                     = curve.evaluate_t(0.0);
      for(double t = dt; t <= 1.0; t += dt) {
         real q = curve.evaluate_t(t);
         curve.pimpl->approx_length += fabs(p - q);
         p = q;
      }
   }

   return curve.pimpl->is_init;
}

template<typename U>
bool init_pspline_xy(Spline1d& curve,
                     const U& container,
                     double rho,
                     unsigned M_factor)
{
   curve.pimpl->n_control_points = 0;
   curve.pimpl->is_init          = false;
   curve.pimpl->approx_length    = dNAN;

   const unsigned len = unsigned(container.size());
   if(len < 5) return false;

   alglib::real_1d_array X;
   alglib::real_1d_array Y;
   X.setlength(len);
   Y.setlength(len);

   double min_x = std::numeric_limits<double>::max();
   double max_x = std::numeric_limits<double>::lowest();

   uint counter = 0;
   for(const auto& p : container) {
      X[counter] = p.x;
      Y[counter] = p.y;
      if(p.x < min_x) min_x = p.x;
      if(p.x > max_x) max_x = p.x;
      counter++;
   }

   double range_inv = 1.0 / (max_x - min_x);

   // Now scale the 'X' values
   for(unsigned i = 0; i < len; ++i) X[i] = (X[i] - min_x) * range_inv;

   alglib::ae_int_t info_x;
   alglib::ae_int_t M = std::max<int>(4, int(M_factor));

   alglib::spline1dfitpenalized(
       X, Y, M, rho, info_x, curve.pimpl->spline_x, curve.pimpl->rep_x);

   curve.pimpl->n_control_points = unsigned(M);

   if(info_x > 0) {
      curve.pimpl->is_init       = true;
      curve.pimpl->approx_length = 0.0;
      curve.set_min_x(min_x);
      curve.set_max_x(max_x);
   }

   return curve.pimpl->is_init;
}

// ------------------------------------------------------------------------ Init

bool This::init(const std::vector<double>& ps, double rho, int M_factor)
{
   return init_pspline(*this, ps, rho, unsigned(M_factor));
}

bool This::init(const std::deque<double>& ps, double rho, int M_factor)
{
   return init_pspline(*this, ps, rho, unsigned(M_factor));
}

bool This::init(const std::vector<float>& ps, double rho, int M_factor)
{
   return init_pspline(*this, ps, rho, unsigned(M_factor));
}

bool This::init(const std::deque<float>& ps, double rho, int M_factor)
{
   return init_pspline(*this, ps, rho, unsigned(M_factor));
}

bool This::init(const std::vector<Vector2>& ps, double rho, int M_factor)
{
   return init_pspline_xy(*this, ps, rho, unsigned(M_factor));
}

bool This::init(const std::deque<Vector2>& ps, double rho, int M_factor)
{
   return init_pspline_xy(*this, ps, rho, unsigned(M_factor));
}

// -------------------------------------------------------------------- Evaluate

real This::evaluate_t(double t) const
{
   // assert(t >= 0.0 && t <= 1.0);
   return is_init() ? real(alglib::spline1dcalc(pimpl->spline_x, t))
                    : real(NAN);
}

real This::evaluate(double x) const
{
   // assert(min-x >= 0.0 && max-x <= 1.0);
   // cout << format("t = {}", (x - pimpl->min_x) * pimpl->range_inv) << endl;
   return evaluate_t((x - pimpl->min_x) * pimpl->range_inv);
}

// ---------------------------------------------------------------------- Unpack

std::vector<array<double, 6>> This::unpack() const
{
   std::vector<array<double, 6>> out;
   unpack(out);
   return out;
}

void This::unpack(std::vector<array<double, 6>>& out) const
{
   alglib::ae_int_t nx{0};
   alglib::real_2d_array tbl_x;
   spline1dunpack(pimpl->spline_x, nx, tbl_x);

   size_t N = size_t(tbl_x.rows());

   out.resize(N + 1);
   for(size_t ind = 0; ind < N; ++ind) {
      auto& coeff = out[size_t(ind)];
      auto ptr    = &coeff[0];
      for(unsigned i = 0; i < 6; ++i) *ptr++ = tbl_x(alglib::ae_int_t(ind), i);
   }

   std::fill(out[N].begin(), out[N].end(), 0.0);
   out[N][0] = pimpl->min_x;
   out[N][1] = pimpl->max_x;
}

void This::init(const std::vector<array<double, 6>>& coefficients)
{
   using alglib::spline1dinterpolant;
   auto make_interpolant = [&](spline1dinterpolant& interpolant) {
      const unsigned n = unsigned(coefficients.size());
      alglib::real_1d_array x, y, d; // (x, f(x), f'(x))
      x.setlength(n);
      y.setlength(n);
      d.setlength(n);
      unsigned pos = 0; // write position

      auto write = [&](double t,
                       double X0,
                       double X1,
                       double C0,
                       double C1,
                       double C2,
                       double C3) {
         x(pos) = X0 + t * (X1 - X0);
         y(pos) = C0 + C1 * t + C2 * t * t + C3 * t * t * t;
         d(pos) = C1 + 2.0 * C2 * t + 3.0 * C3 * t * t;
         pos++;
      };

      for(unsigned ind = 0; ind < coefficients.size() - 1; ++ind) {
         const auto& CC = coefficients[ind];
         auto X0        = CC[0];
         auto X1        = CC[1];
         auto C0        = CC[2];
         auto C1        = CC[3];
         auto C2        = CC[4];
         auto C3        = CC[5];
         write(0.0, X0, X1, C0, C1, C2, C3);

         // Write that final coefficient
         if(ind == coefficients.size() - 2) write(1.0, X0, X1, C0, C1, C2, C3);
      }

      if(pos != n) { FATAL(format("pos = {} != {} = n", pos, n)); }

      alglib::spline1dbuildhermite(x, y, d, interpolant);
   };

   make_interpolant(pimpl->spline_x);

   pimpl->is_init           = true;
   pimpl->n_control_points  = unsigned(coefficients.size());
   pimpl->rep_x.rmserror    = 0.0;
   pimpl->rep_x.avgerror    = 0.0;
   pimpl->rep_x.maxerror    = 0.0;
   pimpl->rep_x.avgrelerror = 0.0;
   pimpl->min_x             = coefficients.back()[0];
   set_max_x(coefficients.back()[1]);
}

// ----------------------------------------------------------------- Interpolate

void This::interpolate_t(unsigned n,
                         std::vector<real>& out,
                         double min_t,
                         double max_t)
{
   assert(min_t >= 0.0 && min_t <= 1.0);
   assert(max_t >= 0.0 && max_t <= 1.0);
   assert(min_t <= max_t);

   out.reserve(n);

   if(n == 0)
      return;
   else if(n == 1)
      out.push_back(evaluate_t(max_t));
   else {
      double t  = min_t;
      double dt = (max_t - min_t) / double(n - 1);
      for(unsigned i = 0; i < n - 1; ++i) {
         out.push_back(evaluate_t(clamp(t, min_t, max_t)));
         t += dt;
      }
      out.push_back(evaluate_t(max_t));
   }

   assert(out.size() == n);
}

// ---------------------------------------------------------------------- Smooth

bool This::smooth(std::vector<real>& path, double rho, int M_factor)
{
   if(path.size() < 5) return false;
   Spline1d spline;
   if(!spline.init(path, rho, M_factor)) return false;
   std::vector<real> t;
   spline.interpolate_t(unsigned(path.size()), t);
   if(!(t.size() == path.size()))
      FATAL(format("Container size mismatch: expected {}, but got {}",
                   path.size(),
                   t.size()));
   auto src = t.begin();
   auto dst = path.begin();
   while(src != t.end()) *src++ = *dst++;
   return true;
}

} // namespace perceive


/*!
 * Copyright (C) tkornuta, IBM Corporation 2015-2019
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
/*!
 * @file mnist_convnet_features_visualization_test.cpp
 * @brief Program for visualization of features of convolutional neural net trained on MNIST digits.
 * @author tkornuta
 * @date:   03-04-2017
 *
 * Copyright (c) 2017, Tomasz Kornuta, IBM Corporation. All rights reserved.
 *
 */

#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>

#include <importers/MNISTMatrixImporter.hpp>

#include <logger/Log.hpp>
#include <logger/ConsoleOutput.hpp>
using namespace mic::logger;

#include <application/ApplicationState.hpp>

#include <configuration/ParameterServer.hpp>

#include <opengl/visualization/WindowManager.hpp>
#include <opengl/visualization/WindowGrayscaleBatch.hpp>
#include <opengl/visualization/WindowCollectorChart.hpp>
using namespace mic::opengl::visualization;

// Neural net.
#include <mlnn/BackpropagationNeuralNetwork.hpp>
using namespace mic::mlnn;

// Encoders.
#include <encoders/MatrixXfMatrixXfEncoder.hpp>
#include <encoders/UIntMatrixXfEncoder.hpp>

/// Windows for displaying activations.
WindowGrayscaleBatch<float> *w_conv10, *w_conv11, *w_conv12, *w_conv13, *w_conv14, *w_conv15, *w_conv16;
WindowGrayscaleBatch<float> *w_conv20, *w_conv21, *w_conv22, *w_conv23, *w_conv24, *w_conv25;
WindowGrayscaleBatch<float> *w_conv30, *w_conv31, *w_conv32, *w_conv33, *w_conv34, *w_conv35;
/// Window for displaying chart with statistics.
WindowCollectorChart<float>* w_chart;
/// Data collector.
mic::utils::DataCollectorPtr<std::string, float> collector_ptr;

/// MNIST importer.
mic::importers::MNISTMatrixImporter<float>* importer;
/// Multi-layer neural network.
BackpropagationNeuralNetwork<float> neural_net;

/// MNIST matrix encoder.
mic::encoders::MatrixXfMatrixXfEncoder* mnist_encoder;
/// Label 2 matrix encoder (1 hot).
mic::encoders::UIntMatrixXfEncoder* label_encoder;

const size_t batch_size = 1;
const char* convent_filename = "nn_convent.txt";
const char* convnet_log = "nn_convent_log.csv";

/*!
 * \brief Function for batch sampling.
 * \author tkornuta
 */
void batch_function (void) {

/*	if (neural_net.load(convent_filename)) {
		LOG(LINFO) << "Loaded neural network from a file";
	} else {*/
		{
			neural_net.pushLayer(new mic::mlnn::convolution::Cropping<float>(28, 28, 1, 2));
			neural_net.pushLayer(new mic::mlnn::convolution::Convolution<float>(24, 24, 1, 9, 5, 1));
			neural_net.pushLayer(new ELU<float>(20, 20, 9));
			neural_net.pushLayer(new mic::mlnn::convolution::MaxPooling<float>(20, 20, 9, 2));

			neural_net.pushLayer(new mic::mlnn::convolution::Convolution<float>(10, 10, 9, 16, 7, 1));
			neural_net.pushLayer(new ELU<float>(4, 4, 16));
			neural_net.pushLayer(new mic::mlnn::convolution::MaxPooling<float>(4, 4, 16, 2));

			neural_net.pushLayer(new Linear<float>(2, 2, 16, 10, 1, 1));
			neural_net.pushLayer(new Softmax<float>(10));

			if (!neural_net.verify())
				exit(-1);

			neural_net.setLoss<  mic::neural_nets::loss::SquaredErrorLoss<float> >();
			neural_net.setOptimization<  mic::neural_nets::optimization::Adam<float> >();

		LOG(LINFO) << "Generated new neural network";
	}//: else

	// Import data from datasets.
	if (!importer->importData())
		exit(-1);

	size_t iteration = 0;

	// Retrieve the next minibatch.
	//mic::types::MNISTBatch bt = importer->getNextBatch();
	//importer->setNextSampleIndex(5);

	// Main application loop.
	while (!APP_STATE->Quit()) {

		// If not paused.
		if (!APP_STATE->isPaused()) {

			// If single step mode - pause after the step.
			if (APP_STATE->isSingleStepModeOn())
				APP_STATE->pressPause();

			{ // Enter critical section - with the use of scoped lock from AppState!
				APP_DATA_SYNCHRONIZATION_SCOPED_LOCK();

				// Retrieve the next minibatch.
				mic::types::MNISTBatch<float> bt = importer->getRandomBatch();

				// Encode data.
				mic::types::MatrixXfPtr encoded_batch = mnist_encoder->encodeBatch(bt.data());
				mic::types::MatrixXfPtr encoded_labels = label_encoder->encodeBatch(bt.labels());

/*				mic::types::MatrixPtr<float> encoded_batch = MAKE_MATRIX_PTR(float, patch_size*patch_size, 1);
				for (size_t i=0; i<patch_size*patch_size; i++)
					(*encoded_batch)[i]= 1.0 -(float)i/(patch_size*patch_size);*/
				/*mic::types::MatrixPtr<float> encoded_labels = MAKE_MATRIX_PTR(float, output_size, 1);
				encoded_labels->setZero();
				(*encoded_labels)[0]= 1.0;*/
				/*(*encoded_labels)[6]= 1.0;
				(*encoded_labels)[9]= 1.0;
				(*encoded_labels)[15]= 1.0;*/

				// Train the autoencoder.
				float loss = neural_net.train (encoded_batch, encoded_labels, 0.001, 0.0001);

				// Get reconstruction.
				/*mic::types::MatrixXfPtr encoded_reconstruction = neural_net.getPredictions();
				std::vector<mic::types::MatrixXfPtr> decoded_reconstruction = mnist_encoder->decodeBatch(encoded_reconstruction);
				w_reconstruction->setBatchUnsynchronized(decoded_reconstruction);*/

				if (iteration%10 == 0) {
					// Visualize the weights.
					//std::shared_ptr<Layer<float> > layer1 = neural_net.getLayer(3);

					std::shared_ptr<mic::mlnn::convolution::Convolution<float> > conv1 =
							neural_net.getLayer<mic::mlnn::convolution::Convolution<float> >(1);
					w_conv10->setBatchUnsynchronized(conv1->getInputActivations());
					w_conv11->setBatchUnsynchronized(conv1->getInputGradientActivations());
					w_conv12->setBatchUnsynchronized(conv1->getWeightActivations());
					w_conv13->setBatchUnsynchronized(conv1->getWeightGradientActivations());
					w_conv14->setBatchUnsynchronized(conv1->getOutputActivations());
                    w_conv15->setBatchUnsynchronized(conv1->getOutputGradientActivations());

					// Similarity.
					mic::types::MatrixPtr<float> similarity = conv1->getFilterSimilarityMatrix();
					w_conv16->setSampleUnsynchronized(similarity);

					float max_similarity = 0;
					float mean_similarity = 0;
					for (size_t i=0; i<9; i++)
						for (size_t j=0; j<i; j++) {
							std::string label = "Similarity " + std::to_string(i) + "-" +std::to_string(j);
							collector_ptr->addDataToContainer(label, (*similarity)(i,j));
							mean_similarity += (*similarity)(i,j);
							max_similarity = ((*similarity)(i,j) > max_similarity) ? (*similarity)(i,j) : max_similarity;
						}//: for

					collector_ptr->addDataToContainer("Similarity max", max_similarity);
					mean_similarity /= (1+2+3+4+5+6+7+8);
					collector_ptr->addDataToContainer("Similarity mean", mean_similarity);

					std::shared_ptr<mic::mlnn::convolution::Convolution<float> > conv2 =
							neural_net.getLayer<mic::mlnn::convolution::Convolution<float> >(4);
					w_conv20->setBatchUnsynchronized(conv2->getInputActivations());
					w_conv21->setBatchUnsynchronized(conv2->getInputGradientActivations());
					w_conv22->setBatchUnsynchronized(conv2->getWeightActivations());
					w_conv23->setBatchUnsynchronized(conv2->getWeightGradientActivations());
					w_conv24->setBatchUnsynchronized(conv2->getOutputActivations());
					w_conv25->setBatchUnsynchronized(conv2->getOutputGradientActivations());

					std::shared_ptr<mic::mlnn::fully_connected::Linear<float> > lin1 =
							neural_net.getLayer<mic::mlnn::fully_connected::Linear<float> >(7);
					w_conv30->setBatchUnsynchronized(lin1->getInputActivations());
					w_conv31->setBatchUnsynchronized(lin1->getInputGradientActivations());
					w_conv32->setBatchUnsynchronized(lin1->getWeightActivations());
					w_conv33->setBatchUnsynchronized(lin1->getWeightGradientActivations());

					std::shared_ptr<Layer<float> > sm1 = neural_net.getLayer(7);
					w_conv34->setBatchUnsynchronized(sm1->getOutputActivations());
					w_conv35->setBatchUnsynchronized(sm1->getOutputGradientActivations());

					// Add data to chart window.
					collector_ptr->addDataToContainer("Loss", loss);
					//float reconstruction_error = neural_net.getLayer<mic::mlnn::fully_connected::Linear<float> >(1)->calculateMeanReconstructionError();
					//collector_ptr->addDataToContainer("Reconstruction Error", reconstruction_error);

					// Export to file.
					collector_ptr->exportDataToCsv(convnet_log);
				}//: if

				iteration++;
				//float reconstruction_error = neural_net.getLayer<mic::mlnn::fully_connected::Linear<float> >(1)->calculateMeanReconstructionError();

				LOG(LINFO) << "Iteration: " << iteration << " loss =" << loss;// << " reconstruction error =" << reconstruction_error;
			}//: end of critical section

		}//: if

		// Sleep.
		APP_SLEEP();
	}//: while

}//: image_encoder_and_visualization_test

/*!
 * \brief Main program function. Runs two threads: main (for GLUT) and another one (for data processing).
 * \author tkornuta
 * @param[in] argc Number of parameters (passed to glManaged).
 * @param[in] argv List of parameters (passed to glManaged).
 * @return (not used)
 */
int main(int argc, char* argv[]) {
	// Set console output to logger.
	LOGGER->addOutput(new ConsoleOutput());
	LOG(LINFO) << "Logger initialized. Starting application";

	// Parse parameters.
	PARAM_SERVER->parseApplicationParameters(argc, argv);

	// Initilize application state ("touch it") ;)
	APP_STATE;

	// Load dataset.
	importer = new mic::importers::MNISTMatrixImporter<float>();
	importer->setBatchSize(batch_size);

	// Initialize the encoders.
	mnist_encoder = new mic::encoders::MatrixXfMatrixXfEncoder(28, 28);
	label_encoder = new mic::encoders::UIntMatrixXfEncoder(10);

	// Set parameters of all property-tree derived objects - USER independent part.
	PARAM_SERVER->loadPropertiesFromConfiguration();

	// Initialize property-dependent variables of all registered property-tree objects - USER dependent part.
	PARAM_SERVER->initializePropertyDependentVariables();

	// Initialize GLUT! :]
	VGL_MANAGER->initializeGLUT(argc, argv);

	// Create batch visualization window.
	w_conv10 = new WindowGrayscaleBatch<float>("Conv1 x", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 50, 50, 256, 256);
	w_conv11 = new WindowGrayscaleBatch<float>("Conv1 dx", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 316, 50, 256, 256);
	w_conv12 = new WindowGrayscaleBatch<float>("Conv1 W", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 562, 50, 256, 256);
	w_conv13 = new WindowGrayscaleBatch<float>("Conv1 dW", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 818, 50, 256, 256);
	w_conv14 = new WindowGrayscaleBatch<float>("Conv1 y", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 1074, 50, 256, 256);
	w_conv15 = new WindowGrayscaleBatch<float>("Conv1 dy", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 1330, 50, 256, 256);
	w_conv16 = new WindowGrayscaleBatch<float>("Conv1 similarity", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 1586, 50, 256, 256);

	w_conv20 = new WindowGrayscaleBatch<float>("Conv2 x", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 50, 336, 256, 256);
	w_conv21 = new WindowGrayscaleBatch<float>("Conv2 dx", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 316, 336, 256, 256);
	w_conv22 = new WindowGrayscaleBatch<float>("Conv2 W", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 562, 336, 256, 256);
	w_conv23 = new WindowGrayscaleBatch<float>("Conv2 dW", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 818, 336, 256, 256);
	w_conv24 = new WindowGrayscaleBatch<float>("Conv2 y", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 1074, 336, 256, 256);
	w_conv25 = new WindowGrayscaleBatch<float>("Conv2 dy", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 1330, 336, 256, 256);

	w_conv30 = new WindowGrayscaleBatch<float>("L1 x", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 50, 622, 256, 256);
	w_conv31 = new WindowGrayscaleBatch<float>("L1 dx", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 316, 622, 256, 256);
	w_conv32 = new WindowGrayscaleBatch<float>("L1 W", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 562, 622, 256, 256);
	w_conv33 = new WindowGrayscaleBatch<float>("L1 dW", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 818, 622, 256, 256);
	w_conv34 = new WindowGrayscaleBatch<float>("SM y", WindowGrayscaleBatch<float>::Norm_None, WindowGrayscaleBatch<float>::Grid_Both, 1074, 622, 256, 256);
	w_conv35 = new WindowGrayscaleBatch<float>("SM dy", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 1330, 622, 256, 256);

	// Chart.
	w_chart = new WindowCollectorChart<float>("Statistics", 60, 878, 512, 256);
	collector_ptr= std::make_shared < mic::utils::DataCollector<std::string, float> >( );
	w_chart->setDataCollectorPtr(collector_ptr);

	// Create data containers.
	collector_ptr->createContainer("Loss", mic::types::color_rgba(0, 100, 0, 180));
	//collector_ptr->createContainer("Reconstruction Error", mic::types::color_rgba(255, 255, 255, 180));
	collector_ptr->createContainer("Similarity max", mic::types::color_rgba(255, 0, 0, 180));
	collector_ptr->createContainer("Similarity mean", mic::types::color_rgba(0, 0, 255, 180));
	for (size_t i=0; i<9; i++)
		for (size_t j=0; j<i; j++) {
			std::string label = "Similarity " + std::to_string(i) + "-" +std::to_string(j);
			collector_ptr->createContainer(label, mic::types::color_rgba(255*(9*i+j)/81, 255*(9*i+j)/81, 255*(9*i+j)/81, 180));
		}

	boost::thread batch_thread(boost::bind(&batch_function));

	// Start visualization thread.
	VGL_MANAGER->startVisualizationLoop();

	LOG(LINFO) << "Waiting for threads to join...";
	// End test thread.
	batch_thread.join();
	LOG(LINFO) << "Threads joined - ending application";
}//: main


/**
 *  @file    Parameters.hpp
 *  @brief   Definition of the diffusion problem.
 *  @author  Francois Roy
 *  @date    12/04/2019
 */
#ifndef PARAMETERS_H
#define PARAMETERS_H

#include <vector>
#include <Eigen/Core>
#include "spdlog/spdlog.h"

namespace numerical
{
namespace fdm
{

/**
* Defines the parameters for the scalar finite difference problem.
*/
template<typename T>
struct Parameters
{
    T alpha, theta;
    std::vector<std::vector<T>> lengths;
    T t0, tend;
    int nt;
    std::vector<int> n;
    Parameters()
    {
    	// default parameters
    	alpha = 1.0;
        theta = 0.5;
        // unit cube
    	lengths = {{0.0, 1.0}, {0.0, 1.0}, {0.0, 1.0}};
        // 10 divisions per dimensions
    	n = {10, 10, 10};
        // initial time
    	t0 = 0.0;
        // final time
    	tend = 1.0;
        // number of uniform time step
    	nt = 10;
    }
};

} // end namespace fdm
}  // end namesapce numerical

#endif  // PARAMETERS_H


#include <iostream>
#include <string>
#include <fstream>
#include <iomanip>

using namespace std;

// abs(i), labs(l)                     Absolute value
// acos(d), acosl(ld)                  Inverse cosine
// acosh(d)                            Inverse hyperbolic cosine
// asin(d), asinl(ld)                  Inverse sine
// asinh(d)                            Inverse hyperbolic sine
// atan(d), atanl(ld)                  Inverse tangent
// atan2(d,d), atan2l(ld,ld)           Inverse tangent
// atanh(d)                            Inverse hyperbolic tangent
// _class(d)                           Classification of floating-point values
// cbrt(x)                             Cube root
// ceil(d), ceill(ld)                  Smallest f-int not less than x
// cos(d), cosl(ld)                    Cosine
// cosh(d), coshl(ld)                  Hyperbolic cosine
// copysign(d,d)                       Return 1st arg with same sign as 2nd
// drem(x,x)                           IEEE remainder
// exp(d), expl(ld)                    Exponential
// expm1(d)                            Exp(x)-1
// erf(d), erfl(ld)                    Error function
// erfc(d), erfcl(ld)                  Complementary error function
// fabs(d), fabsl(ld)                  Floating point absolute value
// int finite(d)                       Nonzero if finite
// floor(d), floor(ld)                 Largest f-int not greater than x
// fmod(d,d), fmodl(ld,ld)             Floating point remainder
// frexp(d, int* e)                    Break into mantissa/exponent  (*)
// frexpl(ld, int* e)                  Break into mantissa/exponent  (*)
// gammaFunc(d)                        Gamma function (** needs special
//                                     implementation using lgamma)
// hypot(d,d)                          Hypotenuse: sqrt(x*x+y*y)
// int ilogb(d)                        Integer unbiased exponent
// int isnan(d)                        Nonzero if NaNS or NaNQ
// int itrunc(d)                       Truncate and convert to integer
// j0(d)                               Bessel function first kind, order 0
// j1(d)                               Bessel function first kind, order 1
// jn(int, double)                     Bessel function first kind, order i
// ldexp(d,i), ldexpl(ld,i)            Compute d * 2^i
// lgamma(d), lgammal(ld)              Log absolute gamma
// log(d), logl(ld)                    Natural logarithm
// logb(d)                             Unbiased exponent (IEEE)
// log1p(d)                            Compute log(1 + x)
// log10(d), log10l(ld)                Logarithm base 10
// modf(d, int* i), modfl(ld, int* i)  Break into integral/fractional part
// double nearest(double)              Nearest floating point integer
// nextafter(d, d)                     Next representable neighbor of 1st
//                                     in direction of 2nd
// pow(d,d), powl(ld,ld)               Computes x ^ y
// d remainder(d,d)                    IEEE remainder
// d rint(d)                           Round to f-integer (depends on mode)
// d rsqrt(d)                          Reciprocal square root
// d scalb(d,d)                        Return x * (2^y)
// sin(d), sinl(ld)                    Sine
// sinh(d), sinhl(ld)                  Hyperbolic sine
// sqr(x)                              Return x * x
// sqrt(d), sqrtl(ld)                  Square root
// tan(d), tanl(ld)                    Tangent
// tanh(d), tanhl(ld)                  Hyperbolic tangent
// trunc(d)                            Nearest f-int in the direction of 0
// unsigned uitrunc(d)                 Truncate and convert to unsigned
// int unordered(d,d)                  Nonzero if comparison is unordered
// y0(d)                               Bessel function 2nd kind, order 0
// y1(d)                               Bessel function 2nd kind, order 1
// yn(i,d)                             Bessel function 2nd kind, order d

ofstream ofs;

const int ldflag = 1;
const int cflag = 2;
const int ieeeflag = 3;
const int bsdflag = 4;
const int cflag1 = 5;
const int cflag2 = 6;
const int nofuncflag = 7;

void one(const char* applicName, const char* specialization, const char* funcName,
    const char* returnType, const char* comment, int flag=0, int noCastFlag=0)
{
    if (specialization != 0 && !strlen(specialization))
        specialization = 0;
    if (returnType != 0 && !strlen(returnType))
        returnType = 0;
    if (comment != 0 && !strlen(comment))
        comment = 0;

    ofs << "// " << applicName << "(";
    if (specialization)
        ofs << specialization;
    else
        ofs << "P_numtype1";
    ofs << ")";
    if (comment)
        ofs << "    " << comment;
    ofs << std::endl;

    if (flag == cflag)
        ofs << "#ifdef BZ_HAVE_COMPLEX_FCNS" << std::endl;
    else if (flag == cflag1)
        ofs << "#ifdef BZ_HAVE_COMPLEX_MATH1" << std::endl;
    else if (flag == cflag2)
        ofs << "#ifdef BZ_HAVE_COMPLEX_MATH2" << std::endl;
    else if (flag == ieeeflag)
        ofs << "#ifdef BZ_HAVE_IEEE_MATH" << std::endl;
    else if (flag == bsdflag)
        ofs << "#ifdef BZ_HAVE_SYSTEM_V_MATH" << std::endl;
//    else if (flag == ldflag)
//        ofs << "#ifdef BZ_LONGDOUBLE128" << std::endl;

    if (!specialization)
    {
        ofs << "template<typename P_numtype1>" << std::endl;
    }
    else {
        ofs << "template<>" << std::endl;
    }
    ofs << "class _bz_" << applicName;
    if (specialization)
        ofs << "<" << specialization << ">";

    ofs << " : public OneOperandApplicativeTemplatesBase {" << std::endl;

    ofs << "public:" << std::endl;
    ofs << "    typedef ";
    if (specialization)
        ofs << specialization;
    else
        ofs << "P_numtype1";
    ofs << " T_numtype1;" << std::endl;

    ofs << "    typedef ";
    if (returnType)
        ofs << returnType;
    else if (specialization)
        ofs << specialization;
    else
        ofs << "P_numtype1";
    ofs << " T_numtype;" << std::endl;

    if (strcmp(applicName,"blitz_isnan") == 0) // Special case nan
    {
        ofs << std::endl << "    static inline T_numtype apply(T_numtype1 x)"
            << std::endl << "    {" << std::endl;
        ofs << "#ifdef isnan" << std::endl;
        ofs << "        "
            << "// Some platforms define isnan as a macro, which causes the"
            << std::endl << "        "
            << "// BZ_IEEEMATHFN_SCOPE macro to break." << std::endl;
        ofs << "        return isnan(x);" << std::endl;
        ofs << "#else" << std::endl;
        ofs << "        return BZ_IEEEMATHFN_SCOPE(isnan)(x);" << std::endl;
        ofs << "#endif" << std::endl << "    }" << std::endl;
    }
    else
    {
        ofs << std::endl << "    static inline T_numtype apply(T_numtype1 x)"
            << std::endl << "    { return ";

        if (noCastFlag == nofuncflag)
        {
            ofs << funcName;
        }
        else {
        if ((flag == cflag) || (flag == cflag1) || (flag == cflag2))
            ofs << "BZ_CMATHFN_SCOPE(";
        else if ((flag == ieeeflag) || (flag == bsdflag))
            ofs << "BZ_IEEEMATHFN_SCOPE(";
        else
            ofs << "BZ_MATHFN_SCOPE(";

        ofs << funcName << ")(";
        if (specialization != 0)
            ofs << "(" << specialization << ")";
        else if ((returnType)&&(!noCastFlag))
            ofs << "(" << returnType << ")";

        ofs << "x)";
        }
        ofs << "; }" << std::endl;
    }

    ofs << std::endl << "    template<typename T1>" << std::endl
        << "    static void prettyPrint(BZ_STD_SCOPE(string) &str, prettyPrintFormat& format,"
        << std::endl
        << "        const T1& a)" << std::endl
        << "    {" << std::endl
        << "        str += \"" << funcName;
      ofs  << "(\";" << std::endl
        << "        a.prettyPrint(str,format);" << std::endl
        << "        str += \")\";" << std::endl
        << "    }" << std::endl
        << "};" << std::endl;

   if ((flag != ldflag) && (flag != 0))
        ofs << "#endif" << std::endl;

    ofs << std::endl;
}

void two(const char* applicName, const char* specialization, const char* funcName,
    const char* returnType, const char* comment, int flag=0, int noCastFlag=0)
{
    if (specialization != 0 && !strlen(specialization))
        specialization = 0;
    if (returnType != 0 && !strlen(returnType))
        returnType = 0;
    if (comment != 0 && !strlen(comment))
        comment = 0;

    ofs << "// " << applicName << "(";
    if (specialization)
        ofs << specialization << ", " << specialization;
    else
        ofs << "P_numtype1, P_numtype2";
    ofs << ")";
    if (comment)
        ofs << "    " << comment;
    ofs << std::endl;

    if (flag == cflag)
        ofs << "#ifdef BZ_HAVE_COMPLEX_FCNS" << std::endl;
    else if (flag == cflag1)
        ofs << "#ifdef BZ_HAVE_COMPLEX_MATH1" << std::endl;
    else if (flag == cflag2)
        ofs << "#ifdef BZ_HAVE_COMPLEX_MATH2" << std::endl;
    else if (flag == ieeeflag)
        ofs << "#ifdef BZ_HAVE_IEEE_MATH" << std::endl;
    else if (flag == bsdflag)
        ofs << "#ifdef BZ_HAVE_SYSTEM_V_MATH" << std::endl;
//    else if (flag == ldflag)
//        ofs << "#ifdef BZ_LONGDOUBLE128" << std::endl;

    if (!specialization)
    {
        ofs << "template<typename P_numtype1, typename P_numtype2>" << std::endl;
    }
    else {
        ofs << "template<>" << std::endl;
    }
    ofs << "class _bz_" << applicName;
    if (specialization)
        ofs << "<" << specialization  << ", " << specialization << " >";
    ofs << " : public TwoOperandApplicativeTemplatesBase {" << std::endl;

    ofs << "public:" << std::endl;
    ofs << "    typedef ";
    if (specialization)
        ofs << specialization;
    else
        ofs << "P_numtype1";
    ofs << " T_numtype1;" << std::endl;

    ofs << "    typedef ";
    if (specialization)
        ofs << specialization;
    else
        ofs << "P_numtype2";
    ofs << " T_numtype2;" << std::endl;

    ofs << "    typedef ";
    if (returnType)
        ofs << returnType;
    else if (specialization)
        ofs << specialization;
    else
        ofs << "BZ_PROMOTE(T_numtype1, T_numtype2)";
    ofs << " T_numtype;" << std::endl;

    ofs << std::endl << "    static inline T_numtype apply(T_numtype1 x, T_numtype2 y)"
        << std::endl << "    { return ";

    if ((flag == cflag) || (flag == cflag1) || (flag == cflag2))
        ofs << "BZ_CMATHFN_SCOPE(";
    else if ((flag == ieeeflag) || (flag == bsdflag))
        ofs << "BZ_IEEEMATHFN_SCOPE(";
    else
        ofs << "BZ_MATHFN_SCOPE(";

    ofs << funcName << ")(";

    if (specialization != 0)
        ofs << "(" << specialization << ")";
    else if ((returnType) && (!noCastFlag))
        ofs << "(" << returnType << ")";

    ofs << "x,";
    if (specialization != 0)
        ofs << "(" << specialization << ")";
    else if ((returnType) && (!noCastFlag))
        ofs << "(" << returnType << ")";
    ofs << "y); }" << std::endl;

    ofs << std::endl << "    template<typename T1, typename T2>" << std::endl
        << "    static void prettyPrint(BZ_STD_SCOPE(string) &str, prettyPrintFormat& format,"
        << std::endl
        << "        const T1& a, const T2& b)" << std::endl
        << "    {" << std::endl
        << "        str += \"" << funcName;
      ofs  << "(\";" << std::endl
        << "        a.prettyPrint(str,format);" << std::endl
        << "        str += \",\";" << std::endl
        << "        b.prettyPrint(str,format);" << std::endl
        << "        str += \")\";" << std::endl
        << "    }" << std::endl;

    ofs << "};" << std::endl;

    if ((flag != ldflag) && (flag != 0))
        ofs << "#endif" << std::endl;

    ofs << std::endl;
}

int main()
{
    std::cout << "Generating <mathfunc.h>" << std::endl;

    ofs.open("../mathfunc.h");

    ofs <<
"// Generated: " << __FILE__ << " " << __DATE__ << " " << __TIME__
                 << std::endl << std::endl <<
"#ifndef BZ_MATHFUNC_H\n"
"#define BZ_MATHFUNC_H\n"
"\n"
"#ifndef BZ_APPLICS_H\n"
" #error <blitz/mathfunc.h> should be included via <blitz/applics.h>\n"
"#endif\n\n"
"\n"
"#ifndef BZ_PRETTYPRINT_H\n"
" #include <blitz/prettyprint.h>\n"
"#endif\n\n"
"BZ_NAMESPACE(blitz)\n\n";

    one("abs", 0, "abs", 0, "Absolute value");
    one("abs","long","labs","long", 0);
    one("abs","float"       ,"fabs",    "float",       0);

one("abs"    ,"double"      ,"fabs"    ,"double"       ,"");
one("abs"    ,"long double" ,"fabs"   ,"long double"  ,"", ldflag);
one("abs"    ,"complex<float> ", "abs","float", "", cflag);
one("abs"    ,"complex<double> ", "abs", "double", "", cflag);
one("abs"    ,"complex<long double> ", "abs", "long double", "", cflag);
one("acos"   ,""            ,"acos"    ,"double"       ,"Inverse cosine");
one("acos"   ,"float"       ,"acos"  ,"float"         ,"");
one("acos"   ,"long double" ,"acos"   ,"long double"  ,"", ldflag);
one("acos"   ,"complex<float> ", "acos", "complex<float>", "", cflag2);
one("acos"   ,"complex<double> ", "acos", "complex<double>", "", cflag2);
one("acos", "complex<long double> ", "acos", "complex<long double>", "", cflag2);
one("acosh"  ,""            ,"acosh"   ,"double"       ,"Inverse hyperbolic cosine", ieeeflag);
one("asin"   ,""            ,"asin"    ,"double"       ,"Inverse sine");
one("asin",   "float",       "asin",    "float", "");
one("asin"   ,"long double" ,"asin"   ,"long double"  ,"", ldflag);
one("asin"   ,"complex<float> ", "asin", "complex<float>", "", cflag2);
one("asin"   ,"complex<double> ", "asin", "complex<double>", "", cflag2);
one("asin", "complex<long double> ", "asin", "complex<long double>", "", cflag2);
one("asinh"  ,""            ,"asinh"   ,"double"       ,"Inverse hyperbolic sine", ieeeflag);
one("arg",   ""            ,"0"    ,0             ,"", cflag, nofuncflag);
one("arg",   "complex<float> ", "arg", "float", "", cflag, 0);
one("arg",   "complex<double> ", "arg", "double", "", cflag, 0);
one("arg",   "complex<long double> ", "arg", "long double", "", cflag, 0);
one("atan"   ,""            ,"atan"    ,"double"       ,"Inverse tangent");
one("atan",   "float",       "atan",    "float",        "");
one("atan"   ,"long double" ,"atan"   ,"long double"  ,"", ldflag);
one("atan"   ,"complex<float> ", "atan", "complex<float>", "", cflag2);
one("atan"   ,"complex<double> ", "atan", "complex<double>", "", cflag2);
one("atan", "complex<long double> ", "atan", "complex<long double>", "", cflag2);
one("atanh"  ,""            ,"atanh"   ,"double"       ,"Inverse hyperbolic tangent", ieeeflag);
two("atan2"  ,""            ,"atan2"   ,"double"       ,"Inverse tangent");
two("atan2"  ,"float"       ,"atan2"   ,"float"        ,"");
two("atan2"  ,"long double" ,"atan2"   ,"long double"  ,"");
one("_class" ,""            ,"_class"  ,"int"          ,"Classification of float-point value (FP_xxx)", bsdflag,1);
one("cbrt"   ,""            ,"cbrt"    ,"double"       ,"Cube root", ieeeflag);
one("ceil"   ,""            ,"ceil"    ,"double"       ,"Ceiling");
one("ceil",   "float",       "ceil",    "float",       "");
one("ceil"   ,"long double" ,"ceil"   ,"long double"  ,"", ldflag);
one("conj",   ""            ,"conj"    ,0             ,"", cflag);
one("cos"    ,""            ,"cos"     ,"double"       ,"Cosine");
one("cos",    "float",       "cos",     "float",       "");
one("cos"    ,"long double" ,"cos"    ,"long double"  ,"", ldflag);
one("cos"   ,"complex<float> ", "cos", "complex<float>", "", cflag1);
one("cos"   ,"complex<double> ", "cos", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("cos", "complex<long double> ", "cos", "complex<long double>", "", cflag1);
 ofs << "#endif\n";
two("copysign", ""          ,"copysign","double"       ,"", bsdflag);
one("cosh"   ,""            ,"cosh"    ,"double"       ,"Hyperbolic cosine");
one("cosh",   "float",       "cosh",    "float", "");
one("cosh"   ,"long double" ,"cosh"   ,"long double"  ,"", ldflag);
one("cosh"   ,"complex<float> ", "cosh", "complex<float>", "", cflag1);
one("cosh"   ,"complex<double> ", "cosh", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("cosh", "complex<long double> ", "cosh", "complex<long double>", "", cflag1);
 ofs << "#endif\n";
two("drem"   ,""            ,"drem"    ,"double"       ,"Remainder", bsdflag);
one("exp"    ,""            ,"exp"     ,"double"       ,"Exponential");
one("exp",    "float",       "exp",     "float",       "");
one("exp"    ,"long double" ,"exp"    ,"long double"  ,"", ldflag      );
one("exp"   ,"complex<float> ", "exp", "complex<float>", "", cflag1);
one("exp"   ,"complex<double> ", "exp", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("exp", "complex<long double> ", "exp", "complex<long double>", "", cflag1);
 ofs << "#endif\n";
one("expm1"  ,""            ,"expm1"   ,"double"       ,"Exp(x)-1", ieeeflag);
one("erf"    ,""            ,"erf"     ,"double"       ,"Error function", ieeeflag);
one("erfc"   ,""            ,"erfc"    ,"double"       ,"Complementary error function", ieeeflag);

// blitz-bugs/archive/0189.html
// one("finite" ,""            ,"finite"  ,"int"          ,"Nonzero if finite", ieeeflag,1);

one("floor"  ,""            ,"floor"   ,"double"       ,"Floor function");
one("floor",  "float",       "floor",   "float",        "");
one("floor"  ,"long double" ,"floor"   ,"long double"  ,"");
two("fmod"   ,""            ,"fmod"    ,"double"       ,"Modulo remainder");
two("hypot"  ,""            ,"hypot"   ,"double"       ,"sqrt(x*x+y*y)",bsdflag);
one("ilogb"  ,""            ,"ilogb"   ,"int"          ,"Integer unbiased exponent", ieeeflag,1);
one("blitz_isnan"  ,""            ,"blitz_isnan"   ,"int"          ,"Nonzero if NaNS or NaNQ", ieeeflag,nofuncflag);
one("itrunc" ,""            ,"itrunc"  ,"int"          ,"Truncate and convert to integer", bsdflag,1);
one("j0"     ,""            ,"j0"      ,"double"       ,"Bessel function first kind, order 0", ieeeflag);
one("j1"     ,""            ,"j1"      ,"double"       ,"Bessel function first kind, order 1", ieeeflag);
one("lgamma" ,""            ,"lgamma"  ,"double"       ,"Log absolute gamma", ieeeflag);
one("log"    ,""            ,"log"     ,"double"       ,"Natural logarithm");
one("log",    "float",       "log",     "float",        "");
one("log"    ,"long double" ,"log"     ,"long double"  ,"", ldflag);
one("log"   ,"complex<float> ", "log", "complex<float>", "", cflag1);
one("log"   ,"complex<double> ", "log", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("log", "complex<long double> ", "log", "complex<long double>", "", cflag1);
 ofs << "#endif\n";
one("logb"   ,""            ,"logb"    ,"double"       ,"Unbiased exponent (IEEE)", ieeeflag);
one("log1p"  ,""            ,"log1p"   ,"double"       ,"Compute log(1 + x)", ieeeflag);
one("log10"  ,""            ,"log10"   ,"double"       ,"Logarithm base 10");
one("log10",  "float",       "log10",   "float",        "");
one("log10"  ,"long double" ,"log10"  ,"long double"  ,"", ldflag);
one("log10"   ,"complex<float> ", "log10", "complex<float>", "", cflag2);
one("log10"   ,"complex<double> ", "log10", "complex<double>", "", cflag2);
one("log10", "complex<long double> ", "log10", "complex<long double>", "", cflag2);
one("nearest", ""           ,"nearest" ,"double"       ,"Nearest floating point integer", bsdflag);
two("nextafter", "",         "nextafter", "double",     "Next representable number after x towards y", bsdflag);

ofs <<
"template<typename P_numtype>\n"
"class _bz_negate : public OneOperandApplicativeTemplatesBase {\n"
"public:\n"
"    typedef BZ_SIGNEDTYPE(P_numtype) T_numtype;\n\n"
"    static inline T_numtype apply(T_numtype x)\n"
"    { return -x; }\n\n"
"        template<typename T1>\n"
"        "
"static void prettyPrint(BZ_STD_SCOPE(string) &str, prettyPrintFormat& format, const T1& a)\n"
"        {\n"
"                str += \"-(\";\n"
"                       a.prettyPrint(str,format);\n"
"                       str += \")\";\n"
"        }\n"
"};\n\n"
;

one("norm",   ""            ,"norm"    ,0             ,"", cflag);

two("polar"  ,""            ,"polar"   ,"complex<T_numtype1>", "", cflag, 1);
two("pow"    ,""            ,"pow"     ,"double"       ,"Power");
 ofs << "#ifndef __PGI\n";
two("pow"    ,"float"       ,"pow"     ,"float"        ,"");
 ofs << "#endif\n";
two("pow"    ,"long double" ,"pow"     ,"long double"  ,"");
two("pow"    ,"complex<float>","pow"   ,"complex<float>" ,"",cflag1);
two("pow"    ,"complex<double>","pow"  ,"complex<double>","",cflag1);
 ofs << "#ifndef __PGI\n";
two("pow"    ,"complex<long double>","pow","complex<long double>","",cflag1);
 ofs << "#endif\n";
two("remainder", "",         "remainder", "double",     "Remainder", bsdflag);

one("rint"   ,""            ,"rint"    ,"double"       ,"Round to floating point integer", ieeeflag);
one("rsqrt"  ,""            ,"rsqrt"   ,"double"       ,"Reciprocal square root", bsdflag);
two("scalb"  ,""            ,"scalb"   ,"double"       ,"x * (2**y)", bsdflag);
one("sin"    ,""            ,"sin"     ,"double"       ,"Sine");
one("sin",    "float",       "sin",     "float",       "");
one("sin"    ,"long double" ,"sin"    ,"long double"  ,"", ldflag);
one("sin"   ,"complex<float> ", "sin", "complex<float>", "", cflag1);
one("sin"   ,"complex<double> ", "sin", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("sin", "complex<long double> ", "sin", "complex<long double>", "", cflag1);
 ofs << "#endif\n";
one("sinh"   ,""            ,"sinh"    ,"double"       ,"Hyperbolic sine");
one("sinh",   "float",       "sinh",    "float",        "");
one("sinh"   ,"long double" ,"sinh"   ,"long double"  ,"", ldflag);
one("sinh"   ,"complex<float> ", "sinh", "complex<float>", "", cflag1);
one("sinh"   ,"complex<double> ", "sinh", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("sinh", "complex<long double> ", "sinh", "complex<long double>", "", cflag1);
 ofs << "#endif\n";

ofs <<
"template<typename P_numtype>\n"
"class _bz_sqr : public OneOperandApplicativeTemplatesBase {\n"
"public:\n"
"    typedef P_numtype T_numtype;\n\n"
"    static inline T_numtype apply(T_numtype x)\n"
"    { return x*x; }\n"
"    template<typename T1>\n"
"    static void prettyPrint(BZ_STD_SCOPE(string) &str, prettyPrintFormat& format,\n"
"        const T1& a)\n"
"    {\n"
"        str += \"sqr(\";\n"
"        a.prettyPrint(str,format);\n"
"        str += \")\";\n"
"    }\n"
"};\n\n"
"#ifdef BZ_HAVE_COMPLEX\n"
"// Specialization of _bz_sqr for complex<T>\n"
"template<typename T>\n"
"class _bz_sqr<complex<T> > : public OneOperandApplicativeTemplatesBase {\n"
"public:\n"
"    typedef complex<T> T_numtype;\n\n"
"    static inline T_numtype apply(T_numtype x)\n"
"    {\n"
"        T r = x.real();  T i = x.imag();\n"
"        return T_numtype(r*r-i*i, 2*r*i);\n"
"    }\n"
"    template<typename T1>\n"
"    static void prettyPrint(BZ_STD_SCOPE(string) &str, prettyPrintFormat& format,\n"
"        const T1& a)\n"
"    {\n"
"        str += \"sqr(\";\n"
"        a.prettyPrint(str,format);\n"
"        str += \")\";\n"
"    }\n"
"};\n"
"#endif\n\n"
;

one("sqrt"   ,""            ,"sqrt"    ,"double"       ,"Square root");
one("sqrt",   "float",       "sqrt",    "float",        "");
one("sqrt"   ,"long double" ,"sqrt"   ,"long double"  ,"", ldflag);
one("sqrt"   ,"complex<float> ", "sqrt", "complex<float>", "", cflag1);
one("sqrt"   ,"complex<double> ", "sqrt", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("sqrt", "complex<long double> ", "sqrt", "complex<long double>", "", cflag1);
 ofs << "#endif\n";
one("tan"    ,""            ,"tan"     ,"double"       ,"Tangent");
one("tan",    "float",       "tan",    "float",         "");
one("tan"    ,"long double" ,"tan"    ,"long double"  ,"");
one("tan"   ,"complex<float> ", "tan", "complex<float>", "", cflag1);
one("tan"   ,"complex<double> ", "tan", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("tan", "complex<long double> ", "tan", "complex<long double>", "", cflag1);
 ofs << "#endif\n";
one("tanh"   ,""            ,"tanh"    ,"double"       ,"Hyperbolic tangent");
one("tanh",   "float",       "tanh",    "float",        "");
one("tanh"   ,"long double" ,"tanh"   ,"long double"  ,"", ldflag);
one("tanh"   ,"complex<float> ", "tanh", "complex<float>", "", cflag1);
one("tanh"   ,"complex<double> ", "tanh", "complex<double>", "", cflag1);
 ofs << "#ifndef __PGI\n";
one("tanh", "complex<long double> ", "tanh", "complex<long double>", "", cflag1);
 ofs << "#endif\n";

// blitz-bugs/archive/0189.html
// one("trunc"  ,""            ,"trunc"   ,"double"       ,"Nearest floating integer in the direction of zero", ieeeflag);

one("uitrunc", ""           ,"uitrunc" ,"unsigned"     ,"Truncate and convert to unsigned", bsdflag);
two("unordered", "",         "unordered", "int",       "True if a comparison of x and y would be unordered", bsdflag,1);
one("y0"     ,""            ,"y0"      ,"double"       ,"Bessel function of the second kind, order zero", ieeeflag);
one("y1"     ,""            ,"y1"      ,"double"       ,"Bessel function of the second kind, order one", ieeeflag);

    ofs << std::endl << std::endl <<
"BZ_NAMESPACE_END\n\n"
"#endif // BZ_MATHFUNC_H\n";

    return 0;
}


#include <sbrandom.h>

#include <boost/random.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>

namespace Scenebuilder{;

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

class SamplerImpl{
public:
	boost::random::mt19937                     rng;
	boost::random::uniform_int_distribution<>  uniInt;
	boost::random::uniform_real_distribution<> uniReal;
	boost::random::normal_distribution<>       normal;

public:
	void   Seed          (int    _seed);
	int    SampleInt     (int    _min, int    _max);
	real_t SampleReal    (real_t _min, real_t _max);
	real_t SampleNormal  ();

};

void SamplerImpl::Seed(int _seed){
	rng.seed(_seed);
}

int SamplerImpl::SampleInt (int _min, int _max){
	if(_min >= _max)
		return _min;

	uniInt.param(boost::random::uniform_int_distribution<>::param_type(_min, _max));
	return uniInt(rng);
}

real_t SamplerImpl::SampleReal(real_t _min, real_t _max){
	if(_min >= _max)
		return _min;

	uniReal.param(boost::random::uniform_real_distribution<>::param_type(_min, _max));
	return uniReal(rng);
}

real_t SamplerImpl::SampleNormal(){
	return normal(rng);
}

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

Sampler::Sampler(){
	impl = new SamplerImpl();
}

Sampler::~Sampler(){
	delete impl;
}

void Sampler::Seed(int _seed){
	impl->Seed(_seed);
}

bool Sampler::SampleBool(){
	return (bool)SampleInt(0, 1);
}

int Sampler::SampleInt(int _min, int _max){
	return impl->SampleInt(_min, _max);
}

real_t Sampler::SampleReal(real_t _min, real_t _max){
	return impl->SampleReal(_min, _max);
}

real_t Sampler::SampleNormal(){
	return impl->SampleNormal();
}

}


/*
   confidence_intervals.cc
   Copyright (c) 2011 mldb.ai inc.  All rights reserved.
   This file is part of MLDB. Copyright 2015 mldb.ai inc. All rights reserved.
*/

#include "mldb/utils/confidence_intervals.h"
#include <boost/math/distributions/binomial.hpp>
#include <boost/math/distributions/normal.hpp> // for normal_distribution
#include <math.h>
#include "mldb/arch/exception.h"
#include "mldb/utils/string_functions.h"
#include <random>

using namespace std;
using namespace boost;
using boost::math::normal; // typedef provides default type is double.

namespace MLDB
{

ConfidenceIntervals::
ConfidenceIntervals(float alpha, std::string m) : alpha_(alpha) {
    init(m);
}

void ConfidenceIntervals::init(std::string m) {
    if(m=="wilson") {
        method = WILSON;

        // Construct a standard normal distribution s
        normal s; // (default mean = zero, and standard deviation = unity)
        const double b = quantile(s, 1-alpha_);
        const double d = pow(b, 2);
        const double a = d / 2;
        const double c = d / 4;

        wilsonFnct = [=] (double sumNum, double sumDenom, double sign)
            {
                return (a/sumDenom+sumNum/sumDenom+b*
                        sign*sqrt(c/(sumDenom*sumDenom)+(sumNum*
                        (1-sumNum/sumDenom))/(sumDenom*sumDenom)))/(1+d/sumDenom);
            };
    }
    else if (m=="clopper_pearson") {
        method = CLOPPER_PEARSON;
    }
    else {
        throw MLDB::Exception(MLDB::format("Unknown confidence interval method '%s'", m.c_str()));
    }
}

unsigned
ConfidenceIntervals::getMethod()
{
    return method;
}

double
ConfidenceIntervals::
wilsonBinomialUpperLowerBound(int trials,
        int successes, WilsonBoundDirection dir) const
{
    double sumNum = (double)successes;
    double sumDenom = (double)trials;
    double sign = (double)dir;
    return wilsonFnct(sumNum, sumDenom, sign);
}

double
ConfidenceIntervals::
binomialUpperBound(int trials, int successes) const
{
    switch(method){
    case WILSON:          return wilsonBinomialUpperLowerBound(trials, successes, UPPER);
    case CLOPPER_PEARSON: return math::binomial_distribution<>::find_upper_bound_on_p(trials, successes, alpha_);
    }
    throw MLDB::Exception(MLDB::format("Unknown method '%s'", method));
}

double
ConfidenceIntervals::
binomialLowerBound(int trials, int successes) const
{
    switch(method){
    case WILSON:          return wilsonBinomialUpperLowerBound(trials, successes, LOWER);
    case CLOPPER_PEARSON: return math::binomial_distribution<>::find_lower_bound_on_p(trials, successes, alpha_);
    }
    throw MLDB::Exception(MLDB::format("Unknown method '%s'", method));
}

pair<double,double>
ConfidenceIntervals::
binomialTwoSidedBound(int trials, int successes) const
{
    assertClopperPearson();
    return make_pair(
            math::binomial_distribution<>::find_lower_bound_on_p(trials, successes, alpha_/2.0),
            math::binomial_distribution<>::find_upper_bound_on_p(trials, successes, alpha_/2.0)
        );
}

vector<double>
ConfidenceIntervals::
createBootstrapSamples(const vector<double>& sample, int replications,
        int resampleSize) const
{
    assertClopperPearson();
    int sampleSize = sample.size();

    if(sampleSize ==0)
    {
        throw MLDB::Exception("Can't compute bootstrap mean from empty sample");
    }

    std::mt19937 rng;

    vector<double> resampleMeans;
    for(int i=0; i<replications;i++)
    {
        double accumulator = 0;
        for(int j=0;j<resampleSize;j++)
        {
            accumulator += sample[rng() % sampleSize];
        }
        resampleMeans.push_back(accumulator/resampleSize);
    }
    sort(resampleMeans.begin(), resampleMeans.end());
    return resampleMeans;
}

double
ConfidenceIntervals::
bootstrapMeanUpperBound(const vector<double>& sample, int replications,
        int resampleSize) const
{
    assertClopperPearson();
    vector<double> resampleMeans = createBootstrapSamples(sample, replications, resampleSize);
    return resampleMeans[replications*(1-alpha_)];
}

double
ConfidenceIntervals::
bootstrapMeanLowerBound(const vector<double>& sample, int replications,
        int resampleSize) const
{
    assertClopperPearson();
    vector<double> resampleMeans = createBootstrapSamples(sample, replications, resampleSize);
    return resampleMeans[replications*alpha_];
}

pair<double,double>
ConfidenceIntervals::
bootstrapMeanTwoSidedBound(const vector<double>& sample, int replications,
        int resampleSize) const
{
    assertClopperPearson();
    vector<double> resampleMeans = createBootstrapSamples(sample, replications, resampleSize);
    return make_pair(
            resampleMeans[replications*(alpha_/2.0)],resampleMeans[replications*(1-alpha_/2.0)]
        );
}

void ConfidenceIntervals::assertClopperPearson() const
{
    if (method != CLOPPER_PEARSON)
        throw MLDB::Exception("Can only use this method with Clopper-Peason method!");
}

std::string ConfidenceIntervals::
print(Method m) const
{
    switch(m) {
    case WILSON:          return "wilson";
    case CLOPPER_PEARSON: return "clopper_pearson";
    default:
        throw MLDB::Exception("Unknown method");
    }
}

#if 0
void ConfidenceIntervals::
serialize(MLDB::DB::Store_Writer & store) const
{
    int version = 1;
    store << version << print(method) << alpha_;
}

void ConfidenceIntervals::
reconstitute(MLDB::DB::Store_Reader & store)
{
    int version;
    int REQUIRED_V = 1;
    store >> version;
    if(version!=REQUIRED_V) {
        throw MLDB::Exception(MLDB::format(
                    "invalid ConfidenceInterval version! exptected %d, got %d",
                    REQUIRED_V, version));
    }

    string method;
    store >> method >> alpha_;
    init(method);
}
#endif

}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    cpp_int x, k, d; cin >> x >> k >> d;
    if (x < 0) x *= -1;
    cpp_int a = min(x / d, k);
    k -= a, x -= a * d;
    if (k % 2 == 0) cout << x << endl;
    else cout << abs(x - d) << endl;
}


#include <iostream>
#include <fstream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>
#include <boost/format.hpp>  // for formating strings
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>

int main( int argc, char** argv )
{
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    // 问题：什么是aligned_allocator?
    // STL compatible allocator to use with types requiring a non standrad alignment.
    // TO-DO: 对齐方式可以动态选择，主要原因是防止大小不同导致的内存错误
    vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses; // 相机位姿

    // 读取txt文件
    ifstream fin("./pose.txt");
    // 也可以写成if(!fin.is_open())
    if (!fin)
    {
        cerr<<"请在有pose.txt的目录下运行此程序"<<endl;
        // 有error时候return 1或者-1
        return 1;
    }

    // 分别处理5张图片
    for ( int i=0; i<5; i++ )
    {
        // 这是定义了读图片的格式，allows formatted i/o
        // 这个读rgb,depth图的方法很独特，本质上和scanf差不多，学习下
        boost::format fmt( "./%s/%d.%s" ); //图像文件格式

        // TO-DO: emplace_back据说功能一样，但是效率更高，有待考察。。
        colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
        // VIP: imread分为不同的flags，其中-1表示原图输入，0表示灰度图，1表示RGB三通道
        // -1 returns the loaded image as is (with alpha channel, otherwise it gets cropped)
        // ppm(Portable PixMap) can be one byte per pixel (up to 2 bytes), which stores RGB
        // pgm(Portable GreyMap) stores grayscale info, one value per pixel - up to 2 bytes
        // 这里可以用-1或者2，都是一个效果，2也可以把他们转换成2 bytes
        depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像

        // 正式读取txt文件的内容，以数组的形式读取
        // 静态分配--数组初始化方法，默认都是0
        double data[7] = {0};

        // for(auto x:v) where, v is data, x is defined variable
        // auto+&表示使用引用，因为后续要修改data!
        for ( auto& d:data ){
            fin>>d;
        }

        // 定义了旋转矩阵的四元数
        // pose文件的构成：前三个是XYZ，后四个表征四元数(3个虚部+1个实部)
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
        // cout<<"quaternion = \n"<<q.coeffs() <<endl;
        Eigen::Isometry3d T(q);

        // pretranslate函数用于inital position setting
        // Applies on the right the translation matrix represented by the vector
        T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));

        // 通过push_back保存各个图像的初始位姿！
        poses.push_back( T );
    }

    // 计算点云并拼接
    // 相机内参
    double cx = 325.5; // 这个值挺有意思，并不是640/2
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    // 说明一开始depth是以mm计的，后面要转换成meter
    double depthScale = 1000.0;

    cout<<"正在将图像转换为点云..."<<endl;

    // 定义点云使用的格式：这里用的是XYZRGB
    typedef pcl::PointXYZRGB PointT;
    typedef pcl::PointCloud<PointT> PointCloud;

    // 新建一个点云，用到了new
    // 这个构造函数的输入是一个指针，最后返回一个指针
    // PointCloud (PointCloud< PointT > &pc)
    PointCloud::Ptr pointCloud( new PointCloud );
    // 处理每对RGBD数据，并添加至点云中
    for ( int i=0; i<5; i++ )
    {
        cout<<"转换图像中: "<<i+1<<endl;
        // 获取原始数据
        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        // 获取pose数据
        Eigen::Isometry3d T = poses[i];

        // 按照行列的方式对于每个像素进行操作
        for ( int v=0; v<color.rows; v++ )
            for ( int u=0; u<color.cols; u++ )
            {
                // depth是以mm为单位，unsigned int范围是0~2^32-1
                // 先用depth.ptr<>(v)取到指针，然后[u]取到值
                unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值

                if ( d==0 ) continue; // 为0表示没有测量到

                // 以下是将图像坐标系转换成相机坐标系
                Eigen::Vector3d point;
                point[2] = double(d)/depthScale; // 由mm转换成m
                point[0] = (u-cx)*point[2]/fx;
                point[1] = (v-cy)*point[2]/fy;

                // 将相机坐标系转换成世界坐标系
                Eigen::Vector3d pointWorld = T*point;

                // 在点云中定义每一个像素点对应的三维空间点
                PointT p ;
                p.x = pointWorld[0];
                p.y = pointWorld[1];
                p.z = pointWorld[2];

                // data表征cv::Mat里Pointer to the user data
                // step表征cv::Mat里Number of bytes each matrix row occupies
                // RGB每个都是1byte，用color图对应像素点的值赋值罢了
                // VIP: opencv顺序是BGR！
                p.b = color.data[ v*color.step+u*color.channels() ];
                p.g = color.data[ v*color.step+u*color.channels()+1 ];
                p.r = color.data[ v*color.step+u*color.channels()+2 ];

                // ->和*ptr.的效用是一样的
                // points是PointCloud的parameter，是PCL最基本的数据类型
                // 之后也可以通过pointCloud->points[i]取到第i个数据
                pointCloud->points.push_back( p );
            }
    }

    pointCloud->is_dense = false;
    cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
    // 最后调用save函数将建好的点云存到本地
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

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

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

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

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_distance_geographic_pointlike_pointlike
#endif

#include <boost/test/included/unit_test.hpp>

#include "test_distance_geo_common.hpp"
#include "test_empty_geometry.hpp"

//===========================================================================

template <typename Point, typename Strategy>
void test_distance_point_point(Strategy const& strategy)
{

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "point/point distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<Point, Point> tester;

    tester::apply("p-p-01",
                  "POINT(1 1)",
                  "POINT(0 0)",
                  strategy.apply(Point(1,1), Point(0,0)),
                  strategy, true, false, false);
}

//===========================================================================

template <typename Point, typename Strategy>
void test_distance_multipoint_point(Strategy const& strategy)
{

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multipoint/point distance tests" << std::endl;
#endif
    typedef bg::model::multi_point<Point> multi_point_type;

    typedef test_distance_of_geometries<multi_point_type, Point> tester;

    tester::apply("mp-p-01",
                  "MULTIPOINT(1 1,1 2,2 3)",
                  "POINT(0 0)",
                  pp_distance<Point>("POINT(0 0)","POINT(1 1)",strategy),
                  strategy, true, false, false);

    tester::apply("mp-p-01",
                  "MULTIPOINT(0 0,0 2,2 0,2 2)",
                  "POINT(1.1 1.1)",
                  pp_distance<Point>("POINT(1.1 1.1)","POINT(2 2)",strategy),
                  strategy, true, false, false);
}

//===========================================================================

template <typename Point, typename Strategy>
void test_distance_multipoint_multipoint(Strategy const& strategy)
{

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multipoint/multipoint distance tests" << std::endl;
#endif
    typedef bg::model::multi_point<Point> multi_point_type;

    typedef test_distance_of_geometries<multi_point_type, multi_point_type> tester;

    tester::apply("mp-mp-01",
                  "MULTIPOINT(1 1,1 2,2 3)",
                  "MULTIPOINT(0 0, 0 -1)",
                  pp_distance<Point>("POINT(0 0)","POINT(1 1)",strategy),
                  strategy, true, false, false);
}

//===========================================================================
//===========================================================================
//===========================================================================

template <typename Point, typename Strategy>
void test_all_pl_pl(Strategy pp_strategy)
{
    test_distance_point_point<Point>(pp_strategy);
    test_distance_multipoint_point<Point>(pp_strategy);
    test_distance_multipoint_multipoint<Point>(pp_strategy);

    test_more_empty_input_pointlike_pointlike<Point>(pp_strategy);
}

BOOST_AUTO_TEST_CASE( test_all_pointlike_pointlike )
{
    typedef bg::model::point
            <
                double, 2,
                bg::cs::spherical_equatorial<bg::degree>
            > sph_point;

    test_all_pl_pl<sph_point>(spherical_pp());

    typedef bg::model::point
            <
                double, 2,
                bg::cs::geographic<bg::degree>
            > geo_point;

    test_all_pl_pl<geo_point>(vincenty_pp());
    test_all_pl_pl<geo_point>(thomas_pp());
    test_all_pl_pl<geo_point>(andoyer_pp());
}


#include <boost/test/unit_test.hpp>

#include <sway/math.h>

using namespace sway;

BOOST_AUTO_TEST_SUITE(TVector4TestSuite)

/*!
 * \brief
 *    Убеждаемся, что конструктор по умолчанию приводит все компоненты к нулю.
 */
BOOST_AUTO_TEST_CASE(TVector4TestCase_DefaultConstructor) {
	const math::vec4i_t vec4;

	BOOST_CHECK_EQUAL(vec4.getX(), 0);
	BOOST_CHECK_EQUAL(vec4.getY(), 0);
	BOOST_CHECK_EQUAL(vec4.getZ(), 0);
	BOOST_CHECK_EQUAL(vec4.getW(), 0);
}

/*!
 * \brief
 *    Убеждаемся, что конструктор устанавливает все значения компонентов в те,
 *    которые были заданы.
 */
BOOST_AUTO_TEST_CASE(TVector4TestCase_ComponentConstructor) {
	const s32_t x = 1, y = 2, z = 3, w = 4;
	const math::vec4i_t vec4(x, y, z, w);

	BOOST_CHECK_EQUAL(vec4.getX(), x);
	BOOST_CHECK_EQUAL(vec4.getY(), y);
	BOOST_CHECK_EQUAL(vec4.getZ(), z);
	BOOST_CHECK_EQUAL(vec4.getW(), w);
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/test/unit_test.hpp>
#include "algorithms/math/bitwise_operators.hpp"

BOOST_AUTO_TEST_SUITE(BitwiseOperations)

BOOST_AUTO_TEST_CASE(add)
{
    BOOST_CHECK(0 == BitwiseOperators::BitwiseAdd(0, 0));
    BOOST_CHECK(0 == BitwiseOperators::BitwiseAddRecursive(0, 0));

    BOOST_CHECK(1 == BitwiseOperators::BitwiseAdd(1, 0));
    BOOST_CHECK(1 == BitwiseOperators::BitwiseAddRecursive(1, 0));

    BOOST_CHECK(1 == BitwiseOperators::BitwiseAdd(0, 1));
    BOOST_CHECK(1 == BitwiseOperators::BitwiseAddRecursive(0, 1));

    BOOST_CHECK(2 == BitwiseOperators::BitwiseAdd(1, 1));
    BOOST_CHECK(2 == BitwiseOperators::BitwiseAddRecursive(1, 1));

    BOOST_CHECK(5 == BitwiseOperators::BitwiseAdd(2, 3));
    BOOST_CHECK(5 == BitwiseOperators::BitwiseAddRecursive(2, 3));
}

BOOST_AUTO_TEST_CASE(abs) {
    BOOST_CHECK(0 == BitwiseOperators::BitwiseAbs(0));
    BOOST_CHECK(1 == BitwiseOperators::BitwiseAbs(1));
    BOOST_CHECK(1 == BitwiseOperators::BitwiseAbs(-1));
    BOOST_CHECK(16894 == BitwiseOperators::BitwiseAbs(16894));
    BOOST_CHECK(78953 == BitwiseOperators::BitwiseAbs(-78953));
}

BOOST_AUTO_TEST_SUITE_END()


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// Use, modification and distribution is subject to 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 <geometry_test_common.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/extensions/nsphere/nsphere.hpp>

template <typename P, typename T>
void test_comparable_margin_circle()
{
    bg::model::nsphere<P, T> c;

    bg::set<0>(c.center(), 0);
    bg::set<1>(c.center(), 0);
    c.radius(2);

    double d = bg::index::detail::comparable_margin(c);
    BOOST_CHECK_CLOSE(d, 2, 0.001);
}

template <typename P, typename T>
void test_comparable_margin_sphere()
{
    bg::model::nsphere<P, T> s;

    bg::set<0>(s, 0);
    bg::set<1>(s, 0);
    bg::set<2>(s, 0);
    bg::set_radius<0>(s, 2);

    double d = bg::index::detail::comparable_margin(s);
    BOOST_CHECK_CLOSE(d, 4, 0.001);
}

int test_main(int, char* [])
{
    test_comparable_margin_circle<bg::model::point<double, 2, bg::cs::cartesian>, double>();
    test_comparable_margin_sphere<bg::model::point<double, 3, bg::cs::cartesian>, double>();
    return 0;
}