/**
 * @file upwindquadrature_main.cc
 * @brief Convergence study for 3 methods to solve CD BVP
 * @author Philippe Peter
 * @date July 2020
 * @copyright Developed at SAM, ETH Zurich
 */
#include <lf/fe/fe.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>
#include <lf/refinement/refinement.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <cmath>
#include <fstream>
#include <iostream>
#include <memory>
#include <string>

#include "cd_tools.h"
#include "standard_fem.h"
#include "supg.h"
#include "upwind.h"

int main() {
  // parameter functions:
  // velocity field
  const Eigen::Vector2d v(2.0, 3.0);
  const auto velocity = [&v](const Eigen::Vector2d& x) { return v; };
  // diffusion coefficient
  const double eps = 1.0;
  const auto epsilon = [&eps](const Eigen::Vector2d& x) { return eps; };
  // exact solution:
  const auto u_exact = [&eps](const Eigen::Vector2d& x) {
    return x(0) * x(1) * x(1) - x(1) * x(1) * std::exp(2 * (x(0) - 1) / eps) -
           x(0) * std::exp(3 * (x(1) - 1) / eps) +
           std::exp(2 * (x(0) - 1) / eps + 3 * (x(1) - 1) / eps);
  };
  // gradient of the exact solution
  const auto u_grad_exact = [&eps](const Eigen::Vector2d& x) {
    Eigen::Vector2d res;
    res(0) = x(1) * x(1) -
             x(1) * x(1) * 2.0 / eps * std::exp(2 * (x(0) - 1) / eps) -
             std::exp(3 * (x(1) - 1) / eps) +
             2.0 / eps * std::exp(2 * (x(0) - 1) / eps + 3 * (x(1) - 1) / eps);
    res(1) = 2 * x(0) * x(1) - 2 * x(1) * std::exp(2 * (x(0) - 1) / eps) -
             x(0) * 3.0 / eps * std::exp(3 * (x(1) - 1) / eps) +
             3.0 / eps * std::exp(2 * (x(0) - 1) / eps + 3 * (x(1) - 1) / eps);
    return res;
  };
  // Laplacian of the exact solution
  const auto u_laplace_exact = [&eps](const Eigen::Vector2d& x) {
    double uxx =
        -4.0 / (eps * eps) * x(1) * x(1) * std::exp(2 * (x(0) - 1) / eps) +
        4.0 / (eps * eps) *
            std::exp(2 * (x(0) - 1) / eps + 3 * (x(1) - 1) / eps);
    double uyy = 2.0 * x(0) - 2.0 * std::exp(2 * (x(0) - 1) / eps) -
                 x(0) * 9.0 / (eps * eps) * std::exp(3 * (x(1) - 1) / eps) +
                 9.0 / (eps * eps) *
                     std::exp(2 * (x(0) - 1) / eps + 3 * (x(1) - 1) / eps);
    return uxx + uyy;
  };
  // source function
  const auto f = [&eps, &v, &u_grad_exact,
                  &u_laplace_exact](const Eigen::Vector2d& x) {
    return -eps * u_laplace_exact(x) + v.transpose() * u_grad_exact(x);
  };

  // boundary conditions
  const auto g = [&u_exact](const Eigen::Vector2d& x) { return u_exact(x); };

  // Construct mesh hierarchy:
  std::unique_ptr<lf::mesh::MeshFactory> top_mesh_factory_ptr =
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::mesh::utils::TPTriagMeshBuilder builder(std::move(top_mesh_factory_ptr));
  builder.setBottomLeftCorner(Eigen::Vector2d{0.0, 0.0})
      .setTopRightCorner(Eigen::Vector2d{1.0, 1.0})
      .setNumXCells(2)
      .setNumYCells(2);
  std::shared_ptr<lf::mesh::Mesh> top_mesh = builder.Build();

  std::shared_ptr<lf::refinement::MeshHierarchy> multi_mesh_p =
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(top_mesh, 6);
  lf::refinement::MeshHierarchy& multi_mesh{*multi_mesh_p};
  multi_mesh.PrintInfo(std::cout);

  // get number of levels:
  unsigned L = multi_mesh.NumLevels();

  // Output file
  std::string file_name = "results_errors.txt";
  std::ofstream file;
  file.open(file_name);
  file << "h, $L^2$-Error u (FEM), $L^2$-Error u (Upwind), $L^2$-Error u "
          "(SUPG) \n";

  // Perform computations on all levels and compute  L2-errors
  for (unsigned l = 0; l < L; ++l) {
    // extract mesh and construct FE space.
    std::shared_ptr<const lf::mesh::Mesh> mesh_p{multi_mesh.getMesh(l)};
    auto fe_space =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

    // compute solutions using Standard FE, Upwind, SUPG method
    Eigen::VectorXd sol_standard = ConvectionDiffusion::SolveCDBVPStandardFem(
        fe_space, epsilon, velocity, f, g);
    lf::fe::MeshFunctionFE sol_standard_mf(fe_space, sol_standard);

    Eigen::VectorXd sol_stable = ConvectionDiffusion::SolveCDBVPUpwind(
        fe_space, epsilon, velocity, f, g);
    lf::fe::MeshFunctionFE sol_upwind_mf(fe_space, sol_stable);

    Eigen::VectorXd sol_supg =
        ConvectionDiffusion::SolveCDBVPSupg(fe_space, epsilon, velocity, f, g);
    lf::fe::MeshFunctionFE sol_supg_mf(fe_space, sol_supg);

    // Wrap exact solution into mesh function for error computations
    auto mf_solution = lf::mesh::utils::MeshFunctionGlobal(u_exact);

    // Calculate L2 errors:
    double L2err_standard = std::sqrt(lf::fe::IntegrateMeshFunction(
        *mesh_p, lf::mesh::utils::squaredNorm(sol_standard_mf - mf_solution),
        10));
    double L2err_upwind = std::sqrt(lf::fe::IntegrateMeshFunction(
        *mesh_p, lf::mesh::utils::squaredNorm(sol_upwind_mf - mf_solution),
        10));
    double L2err_supg = std::sqrt(lf::fe::IntegrateMeshFunction(
        *mesh_p, lf::mesh::utils::squaredNorm(sol_supg_mf - mf_solution), 10));

    // output
    std::cout << "Level " << l
              << "(h= " << ConvectionDiffusion::MeshWidth(mesh_p)
              << "): " << L2err_standard << ", " << L2err_upwind << ", "
              << L2err_supg << std::endl;
    file << ConvectionDiffusion::MeshWidth(mesh_p) << ", " << L2err_standard
         << ", " << L2err_upwind << ", " << L2err_supg << std::endl;
  }
  file.close();

  // Plot
  std::system("python3 " CURRENT_SOURCE_DIR
              "/plot_convergence.py " CURRENT_BINARY_DIR);

  return 0;
}


#include "Srobotconfig.h"
#include <Eigen/Dense>
#include <iostream>
#include <vector>

using namespace std;

namespace SRobot
{
    SCARA Scara(250,250);
	//初始化TransMatrix
	double mTransMatrix[16]={0};
    double mangle[4]={0};
    double mpose[6]={0};

	//只使用一种姿态
	bool mConfig = 1;

    Matrix3d skew(Vector3d w)
    {
        Matrix3d so3;
        so3 << 0, -w(2), w(1),
            w(2), 0, -w(0),
            -w(1), w(0), 0;
        return so3;
    }

    Matrix4d R2SE3(Vector3d v, Vector3d w, int i, double theta)
    {
        Matrix4d SE3 = Matrix4d::Identity();
        if (i == 2)
        {
            SE3(2,3) = -theta;
        }
        else
        {
            theta = PI * theta / 180;
            Matrix3d so3 = skew(w);
            Matrix3d SO3 = Matrix3d::Identity() + so3 * sin(theta) + so3 * so3 * (1 - cos(theta));  //Rodrigues Formula
            Vector3d p = (Matrix3d::Identity() - SO3) * skew(w) * v + w * w.transpose() * v * theta;
            SE3.block<3,3>(0,0) = SO3;
            SE3.block<3,1>(0,3) = p;
        }
        return SE3;
    }

    Matrix4d GetInverse(Matrix4d T)
    {
        Matrix4d T_inv = Matrix4d::Identity();
        T_inv.block<3,3>(0,0) = T.block<3,3>(0,0).transpose();
        T_inv.block<3,1>(0,3) = -T.block<3,3>(0,0).transpose() * T.block<3,1>(0,3);
        return T_inv;
    }

    void Subproblem1 (Vector3d r, Vector3d p, Vector3d q, Vector3d w, double &theta)
    {
        Vector3d u,v,u_dot,v_dot;
        u = p - r;
        v = q - r;

        u_dot = u - w * w.transpose() * u;
        v_dot = v - w * w.transpose() * v;

        theta = atan2(w.transpose()*(u_dot.cross(v_dot)),u_dot.transpose()*v_dot) * 180 / PI;
    }

//     //SCRAR没有用 写一下当练习
    void Subproblem2 (Vector3d r, Vector3d p, Vector3d q, Vector3d w1, Vector3d w2, vector<double> &theta1, vector<double> &theta2 )
    {
        Vector3d u,v;
        u = p - r;
        v = q - r;

        double w12 = w1.transpose()*w2;
        double w2u = w2.transpose()*u;
        double w1v = w1.transpose()*v;
        double alpha = (w12*w2u - w1v) / (w12*w12 -1);
        double beta = (w12*w1v-w2u) / (w12*w12 -1);

        double u_norm_2 = u.transpose()*u;
        Vector3d w1_cross_w2 = w1.cross(w2);
        double Gamma_2 = (u_norm_2 - alpha*alpha -beta*beta -2*alpha*beta*w12) / (w1_cross_w2).dot(w1_cross_w2);

        if(Gamma_2>=0){
            double Gamma = sqrt(Gamma_2);
            Vector3d z1 = alpha * w1 + beta * w2 + Gamma * w1_cross_w2;
            Vector3d z2 = alpha * w1 + beta * w2 - Gamma * w1_cross_w2;
            Vector3d c1 = z1 + r;
            Vector3d c2 = z2 + r;

            double theta1_temp, theta2_temp;
            Subproblem1(r, q, c1, w1, theta1_temp);
            theta1.push_back(-theta1_temp);
            Subproblem1(r, q, c2, w1, theta1_temp);
            theta1.push_back(-theta1_temp);

            Subproblem1(r, p, c1, w2, theta2_temp);
            theta2.push_back(theta2_temp);
            Subproblem1(r, p, c2, w2, theta2_temp);
            theta2.push_back(theta2_temp);
        }
    }
   void Subproblem3 (Vector3d r, Vector3d p, Vector3d q, Vector3d w, double delta, vector<double> &theta)
    {
        Vector3d u,v,u_dot,v_dot;
        u = p - r;
        v = q - r;
        u_dot = u - w * w.transpose() * u;
        v_dot = v - w * w.transpose() * v;
        double delta_dot_2 = delta*delta - (w.dot(p-q))*(w.dot(p-q));   //maybe bug

        double theta0 = atan2(w.dot(u_dot.cross(v_dot)),u_dot.dot(v_dot));

        double Phi = (u_dot.dot(u_dot) + v_dot.dot(v_dot) -delta_dot_2)/sqrt(4*u_dot.dot(u_dot)*v_dot.dot(v_dot));
        if (abs(Phi) <= 1){
            theta.push_back((theta0+acos(Phi))/PI*180);
            theta.push_back((theta0-acos(Phi))/PI*180);
        }
    }

	void SetRobotEndPos(double x, double y, double z, double yaw, double pitch, double roll)
	{
        mpose[0] = x;
        mpose[1] = y;
        mpose[2] = z;
        mpose[3] = yaw/180*PI;
        mpose[4] = pitch/180*PI;
        mpose[5] = roll/180*PI;

        Eigen::AngleAxisd yawAngle(AngleAxisd(mpose[3],Vector3d::UnitZ()));
        Eigen::AngleAxisd pitchAngle(AngleAxisd(mpose[4],Vector3d::UnitY()));
        Eigen::AngleAxisd rollAngle(AngleAxisd(mpose[5],Vector3d::UnitZ()));
        Matrix3d rotate;
        rotate = yawAngle*pitchAngle*rollAngle;
        Matrix4d T = Matrix4d::Identity();
        T.block<3,3>(0,0) = rotate;
        T(0,3) = x;
        T(1,3) = y;
        T(2,3) = z;

        for (int i=0; i<4; i++){
            for(int j=0; j<4; j++){
                mTransMatrix[4*i+j] = T(i,j);
            }
        }
	}

	void GetJointAngles(double &angle1, double &angle2, double &angle3, double &angle4)
	{
        robotBackward(mTransMatrix, mConfig, mangle);
        angle1 = mangle[0];
        angle2 = mangle[1];
        angle3 = mangle[2];
        angle4 = mangle[3];
	}

	void SetRobotJoint(double angle1, double angle2, double angle3, double angle4)
	{
        mangle[0] = angle1;
        mangle[1] = angle2;
        mangle[2] = angle3;
        mangle[3] = angle4;
	}

	void GetJointEndPos(double &x, double &y, double &z, double &yaw, double &pitch, double &roll)
	{
        Matrix4d T;
        Matrix3d R;
        Vector3d Euler;
        robotForward(mangle, mTransMatrix, mConfig);
        for (int i=0; i<4; i++){
            for(int j=0; j<4; j++){
                T(i,j) = mTransMatrix[4*i+j];
            }
        }
        x = T(0,3);
        y = T(1,3);
        z = T(2,3);
        R = T.block<3,3>(0,0);
        Euler = R.eulerAngles(2,1,2);
        yaw = Euler(0)/PI*180;
        pitch = Euler(1)/PI*180;
        roll = Euler(2)/PI*180;
	}

	/********************************************************************
	ABSTRACT:	机器人逆运动学

	INPUTS:		T[16]:	位姿矩阵，其中长度距离为米

				config：姿态，六轴机器人对应有8种姿态（即对应的逆运动学8个解），
				Scara机器人有2个解，Delta机器人有一个解，
				为了安全，实验室中我们只计算一种即可。config用来作为选解的标志数。

	OUTPUTS:    theta[6] 6个关节角, 单位为弧度

	RETURN:		<none>
	***********************************************************************/
	void robotBackward(const double* TransVector, bool mconfig, double* theta)
	{
        //get angle3
		theta[2] = -TransVector[11];

        Matrix4d G, T;
        for (int i=0; i<4; i++){
            for(int j=0; j<4; j++){
                T(i,j) = TransVector[4*i+j];
            }
        }
        G = T*GetInverse(Scara.G0);
        //Subproblem3 to get angle 2
        Matrix4d T3 = Matrix4d::Identity();
        T3(2,3) = theta[2];
        Vector4d q3(500,0,0,1);
        Vector4d q1(0,0,0,1);

        double delta = (G*q3).norm();
        Vector4d p = T3*q3;
        Vector4d q2(250,0,0,1);
        vector<double> angle2;
        Subproblem3(q2.head(3),p.head(3),q1.head(3),Scara.w.col(1),delta,angle2);
        theta[1] = angle2[mconfig];
        //Subproblem1 to get angle 1
        Matrix4d T2 = R2SE3(Scara.v.col(1),Scara.w.col(1), 1, theta[1]);
        Vector4d q = G*q3;
        p =T2*T3*q3;
        Subproblem1(q1.head(3),p.head(3),q.head(3),Scara.w.col(0),theta[0]);
        //Subproblem1 to get angle 4
        Matrix4d T1 = R2SE3(Scara.v.col(0),Scara.w.col(0), 0, theta[0]);
        q = GetInverse(T3)*GetInverse(T2)*GetInverse(T1)*G*q1;
        p = q1;
        Subproblem1(q3.head(3),p.head(3),q.head(3),Scara.w.col(3),theta[3]);
	}

	/********************************************************************
	ABSTRACT:	机器人正运动学

	INPUTS:		q[6]: 6个关节角, 单位为弧度

	OUTPUTS:	config用来作为选解的标志数。

				TransVector[16] : 刚体变换矩阵，也就是末端的位姿描述，其中长度距离为米

	RETURN:		<none>
	***********************************************************************/
	void robotForward(const double* q, double* TransVector, bool mconfig)
	{
        Matrix4d T=Matrix4d::Identity();
        cout<<endl;
		for(int i=0;i<4;i++){
            T = T * R2SE3(Scara.v.col(i),Scara.w.col(i),i,q[i]);
        }
        T = T*Scara.G0;
        for (int i=0; i<4; i++){
            for(int j=0; j<4; j++){
                TransVector[4*i+j] = T(i,j);
            }
        }
	}
}


#include <mex.h>
#include "armaMex.hpp"
#include <armadillo>
#include <boost/math/special_functions/expint.hpp>
#include <boost/math/special_functions/trigamma.hpp>
#define POSITIVE_EPS 0.0001
#define epsilon1 1e-30
#define epsilon2 1e-7

double Lentz_Algorithm(double const x)
{
    double f_prev = epsilon1, C_prev = epsilon1, D_prev = 0, delta = 2+epsilon2, D_curr, C_curr, f_curr;
    double j = 1.0, tmp1, tmp2;
    while (delta-1>=epsilon2 || 1-delta >= epsilon2)
    {
        j++;
        tmp1 = x+2*j-1;
        tmp2 = pow(j-1,2);
        D_curr = 1/(tmp1-tmp2*D_prev);
        C_curr = tmp1-tmp2/C_prev;
        delta = C_curr*D_curr;
        f_curr = f_prev*delta;
        f_prev = f_curr;
        C_prev = C_curr;
        D_prev = D_curr;
    }
    return 1/(x+1+f_curr);
}

void mexFunction(int nlhs,mxArray *plhs[],int nrhs, const mxArray *prhs[])
{
    /* Input Interface*/
    mat XDat = armaGetPr(prhs[0]);
    uword maxIter, s, p = XDat.n_cols,n = XDat.n_rows;
    double tol, eta, r;

    if (nrhs>1)
    {
        eta = armaGetDouble(prhs[1]);
        eta = eta/(p+eta);
    }
    else eta = 300.0/(p+300.0);//0.05; //4*sqrt(double(p)/(p^2-1));

    if (nrhs>2) maxIter = (uword)armaGetDouble(prhs[2]);
    else maxIter = 10000;
    if (nrhs>3) tol = armaGetDouble(prhs[3]);
    else tol = 0.01;

    if (nrhs>4) r = armaGetDouble(prhs[4]);
    else
    {
        r = 0.5; //(1-400.0/double(p))/(1+400.0/double(p));
        //if(r<0.05) r = 0.05;
    }
    if (nrhs>5) s = (uword)armaGetDouble(prhs[5]);
    else s = round((double)p/(1e-3*(p-1)+1)); //(pow((double)p,1.5)/(5e-2*(p-1)+sqrt(double(p))));

	mat nS = XDat.t()*XDat;

    /* Initialization */
    auto t_start = std::chrono::high_resolution_clock::now();
	vec dnS = nS.diag();
    uword pe = p*(p-1)/2;

    uvec idl(pe);
    uvec idu(pe);
    uvec idr(pe);
    uvec idc(pe);
    uword k = 0, i, j, jp, kappa;
    for (j = 0,jp = 0;j<p;j++,jp+=p)
    {
        for (i=j+1;i<p;i++)
        {
            idl(k) = jp+i;
            idu(k) = i*p+j;
            idr(k) = i;
            idc(k) = j;
            k++;
        }

    }

    double psr = (double)p/s;
    uvec idp(p), id0(s), idd;
    vec nd2pp = (double)n/2+linspace<vec>(p,1,p);

    mat ML(p,p,fill::eye);
    mat ML2(p,p,fill::eye);
    mat ML2pVL(p,p,fill::eye);
    mat VL(p,p,fill::zeros);
    mat LAMBDA(p,p,fill::zeros);

    vec h(pe,fill::zeros),zeta(pe),mL(pe,fill::zeros);
    zeta.fill(10.0);

    vec alpha(p,fill::ones);
    vec beta(p,fill::ones);

    double a = (double)pe/2, b = a/50;

    vec mLold = h/zeta;
    vec lambdaold(pe,fill::ones);

    mat VL2(p,p,fill::zeros);
    for (i=1;i<p;i++)
    {
        VL2(span(i,p-1),i) += i;
        VL2(i,span(i,p-1)) += i;
    }
	VL2 *= 0.01;

	vec d(pe,1);
	d.fill(0.5);

    vec mD, mD2, mD2pvD, vD, vL, lambda(pe), gmL, gvL, gmD, gvD, c5(p), c6, mLnew, lambdanew, alphatmp, betatmp, dtmp;//mL,
    mat c1, c2, c3, c4, LDL, c2pc3, K_tmp1, K_tmp2;
    double omega, difmL, diflambda, difmax;
    vec d1h(pe,fill::zeros), d1zeta(pe,fill::zeros), d1alpha(p,fill::zeros), d1beta(p,fill::zeros), d1d(pe,fill::zeros);
    double d2h = 0, d2zeta = 0, d2alpha = 0, d2beta = 0, d2d = 0, d1b = 0, d2b = 0;
    double tau = 6e2; //, tauzeta = tauh, taualpha = tauh, taubeta = tauh, taub = tauh, taud = tauh;
    double rho, gb, btmp;//1/eta;rho_ub, c7=(5*eta>0.25)?0.25:5*eta,
    vec gh, gzeta, galpha, gbeta, gd;
    arma_rng::set_seed(0);

    /* KL proximal variational inference */
    mexPrintf("Start Running BISN ...\n");
    for (kappa=1;kappa<=maxIter;kappa++)
    {
        mD = alpha/beta;
        mD2 = square(mD);
        vD = mD/beta;
        mD2pvD = mD2+vD;

        vL = 1/zeta;
        VL.elem(idl) = vL;
        mL =  h%vL;
        ML.elem(idl) = mL;
        ML2.elem(idl) = square(mL);
        ML2pVL.elem(idl) = ML2.elem(idl)+vL;

        omega = a/b;
        lambda = d;
        lambda.transform([](double val){return (val > 10) ? Lentz_Algorithm(val) : (- boost::math::expint(-val) * exp(val));});
        lambda = 1/(d%lambda)-1;
        LAMBDA.elem(idl) = omega*lambda;
        LAMBDA.elem(idu) = LAMBDA.elem(idl);

        if (kappa==1)
        {
            c1 = nS;
            c2 = -VL;
            c2.each_row() += sum(VL);
            c2 *= LAMBDA(1);
            //c2 = LAMBDA(1)*(repmat(sum(VL),p,1)-VL);
            c3 = LAMBDA;
            c4 = (VL+VL2)*mD2pvD(0);
			VL2.clear();
        }
        else
        {
            K_tmp1 = ML.rows(id0);
            K_tmp1.each_row() %= mD.t();
            LDL = K_tmp1*ML.t();
            c1 *= r;
            c1.rows(id0) += psr*(nS.rows(id0)+LDL%LAMBDA.rows(id0))*ML;

            c2 *= r;
            c2.rows(id0) += psr*LAMBDA.rows(id0)*VL;

            c3 *= r;
            c3.rows(id0) += psr*LAMBDA.rows(id0)*ML2;

            K_tmp1 = ML2pVL.rows(id0);
            K_tmp1.each_row() %= mD2pvD.t();
            K_tmp2 = ML2.rows(id0);
            K_tmp2.each_row() %= mD2.t();
            c4 *=r;
            c4.rows(id0) += psr*(K_tmp1*ML2pVL.t()-K_tmp2*ML2.t()+square(LDL));
        }

        c2pc3 = c2+c3;
        gmL = -c1.elem(idl)%mD.elem(idc)-ML.elem(idl)%(mD2pvD.elem(idc)%c2.elem(idl)+vD.elem(idc)%c3.elem(idl));
        gvL = dnS.elem(idr)%mD.elem(idc)+c2pc3.elem(idl)%mD2pvD.elem(idc);
        gh = gmL+mL%gvL - h;
        gvL.elem(find(gvL<0)).zeros();
        gzeta = gvL - zeta;
        d1h = (1-1/tau)*d1h+gh/tau;
        d2h = (1-1/tau)*d2h+mean(square(gh))/tau;
        d1zeta = (1-1/tau)*d1zeta + gzeta/tau;
        d2zeta = (1-1/tau)*d2zeta + mean(square(gzeta))/tau;

        gmD = (VL.t()*dnS+trans(sum(ML%c1))+trans(sum(VL%(c2pc3+c3)))%mD)/2;
        gvD = trans(sum(ML2pVL%c2pc3))/4;;
        c5 = alpha;
        c5.transform([](double val) { return boost::math::trigamma(val); });
        c6 = mD/(alpha%c5-1);
        alphatmp = nd2pp+c6/beta%gvD;
        alphatmp.elem(find(alphatmp<0)).zeros();
        betatmp = gmD+(1/beta+c5%c6)%gvD;
        betatmp.elem(find(betatmp<0)).zeros();
        galpha = alphatmp - alpha;
        gbeta = betatmp - beta;
        d1alpha = (1-1/tau)*d1alpha + galpha/tau;
        d2alpha = (1-1/tau)*d2alpha + mean(square(galpha))/tau;
        d1beta = (1-1/tau)*d1beta + gbeta/tau;
        d2beta = (1-1/tau)*d2beta + mean(square(gbeta))/tau;

        dtmp = omega/2*c4.elem(idl);
        dtmp.elem(find(dtmp<0)).zeros();
        gd = dtmp - d;
        d1d = (1-1/tau)*d1d + gd/tau;
        d2d = (1-1/tau)*d2d + mean(square(gd))/tau;

        btmp = sum(lambda%c4.elem(idl))/2;
        if (btmp<0) btmp = 0;
        gb = btmp - b;
        d1b = (1-1/tau)*d1b + gb/tau;
        d2b = (1-1/tau)*d2b + pow(gb,2)/tau;

        rho = (mean(square(d1h))+mean(square(d1zeta))+p/pe*(mean(square(d1alpha))+mean(square(d1beta)))+mean(square(d1d))+pow(d1b,2)/pe)/(d2h+d2zeta+p/pe*(d2alpha+d2beta)+d2d+d2b/pe);
        if (rho>eta) rho = eta;

        tau = (1-rho)*tau + 1;
        h += rho*gh;
        zeta += rho*gzeta;
        alpha += rho*galpha;
        beta += rho*gbeta;
        d += rho*gd;
        b += rho*gb;

        if (kappa%100 == 0)
        {
            mLnew = h/zeta;
            lambdanew = lambda;
            difmL = sqrt(mean(square(mLnew-mLold))/mean(square(mLold)));
            diflambda = max(abs(lambdanew-lambdaold));
            difmax = max(abs(mLnew-mLold));
            mexPrintf("#no. of iterations = %d, difmL = %f, difmax = %f, diflambda = %f\n",kappa,difmL,difmax,diflambda);
            mexEvalString("drawnow;");
            if (difmL<tol)
                break;
            else
            {
                mLold = mLnew;
                lambdaold = lambdanew;
            }
        }

        id0 = randperm(p, s);

        K_tmp1 = ML.rows(id0);
        K_tmp1.each_row() %= mD.t();
        LDL = K_tmp1*ML.t();
        c1.rows(id0) -= psr*(nS.rows(id0)+LDL%LAMBDA.rows(id0))*ML;
        c2.rows(id0) -= psr*LAMBDA.rows(id0)*VL;
        c3.rows(id0) -= psr*LAMBDA.rows(id0)*ML2;

        K_tmp1 = ML2pVL.rows(id0);
        K_tmp1.each_row() %= mD2pvD.t();
        K_tmp2 = ML2.rows(id0);
        K_tmp2.each_row() %= mD2.t();
        c4.rows(id0) -= psr*(K_tmp1*ML2pVL.t()-K_tmp2*ML2.t()+square(LDL));
    }

    ML.elem(idl) = h/zeta;
    VL.elem(idl) = 1/zeta;
    mD = alpha/beta;
    vD = mD/beta;
    auto t_end = std::chrono::high_resolution_clock::now();
    double ElapsedTime = std::chrono::duration<double, std::milli>(t_end-t_start).count() / 1e3;
    if (kappa < maxIter && difmL<tol) mexPrintf("BISN converges, elapsed time is %e seconds.\n",ElapsedTime);
    else mexPrintf("BISN reaches the maximum number of iterations, elapsed time is %e seconds.\n",ElapsedTime);

    /* Output Interface */
    if (nlhs>3)
    {
        plhs[0] = armaCreateMxMatrix(p,p,mxDOUBLE_CLASS,mxREAL);
        armaSetPr(plhs[0],ML);
        plhs[1] = armaCreateMxMatrix(p,p,mxDOUBLE_CLASS,mxREAL);
        armaSetPr(plhs[1],VL);
        plhs[2] = armaCreateMxMatrix(p,1,mxDOUBLE_CLASS,mxREAL);
        armaSetPr(plhs[2],mD);
        plhs[3] = armaCreateMxMatrix(p,1,mxDOUBLE_CLASS,mxREAL);
        armaSetPr(plhs[3],vD);
        if (nlhs>4)
        {
            omega = a/b;
            plhs[4] = mxCreateDoubleScalar(omega);
        }
        if (nlhs>5)
        {
            lambda = d;
            lambda.transform([](double val){return (val > 10) ? Lentz_Algorithm(val) : (- boost::math::expint(-val) * exp(val));});
            lambda = 1/(d%lambda)-1;
            plhs[5] = armaCreateMxMatrix(pe,1,mxDOUBLE_CLASS,mxREAL);
            armaSetPr(plhs[5],lambda);
        }
        if (nlhs>6) plhs[6] = mxCreateDoubleScalar(ElapsedTime);
    }
    else
    {
        mexErrMsgIdAndTxt("BINS:output","Expected at least four output arguments");
    }

}


// Copyright (c) 2015-2019 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "genotype.hpp"

#include <boost/math/special_functions/binomial.hpp>
#include <boost/numeric/conversion/cast.hpp>

#include "utils/maths.hpp"

namespace octopus {

// non-member methods

Genotype<Haplotype> remap(const Genotype<Haplotype>& genotype, const GenomicRegion& region)
{
    Genotype<Haplotype> result {genotype.ploidy()};
    for (const auto& haplotype : genotype) result.emplace(remap(haplotype, region));
    return result;
}

std::size_t num_genotypes(const unsigned num_elements, const unsigned ploidy)
{
    return boost::math::binomial_coefficient<double>(num_elements + ploidy - 1, num_elements - 1);
}

boost::optional<std::size_t> num_genotypes_noexcept(const unsigned num_elements, const unsigned ploidy) noexcept
{
    boost::optional<std::size_t> result {};
    try {
        result = num_genotypes(num_elements, ploidy);
    } catch (...) {}
    return result;
}

std::size_t max_num_elements(const std::size_t num_genotypes, const unsigned ploidy)
{
    if (num_genotypes == 0 || ploidy == 0) return 0;
    auto y = maths::factorial<std::size_t>(ploidy);
    if (y >= num_genotypes) return 1;
    const auto t = num_genotypes * y;
    unsigned j {1};
    for (; j < num_genotypes; ++j) {
        y /= j;
        y *= j + ploidy;
        if (y >= t) break;
    }
    return j + 1;
}

std::size_t element_cardinality_in_genotypes(const unsigned num_elements, const unsigned ploidy)
{
    return ploidy * (num_genotypes(num_elements, ploidy) / num_elements);
}

std::size_t num_max_zygosity_genotypes(const unsigned num_elements, const unsigned ploidy)
{
    namespace bmp = boost::math::policies;
    using policy = bmp::policy<bmp::overflow_error<bmp::throw_on_error>>;
    try {
        return boost::numeric_cast<std::size_t>(boost::math::binomial_coefficient<double>(num_elements, ploidy, policy {}));
    } catch (const boost::numeric::positive_overflow& e) {
        throw std::overflow_error {e.what()};
    }
}

boost::optional<std::size_t> num_max_zygosity_genotypes_noexcept(unsigned num_elements, unsigned ploidy) noexcept
{
    assert(num_elements >= ploidy);
    boost::optional<std::size_t> result {};
    try {
        result = num_max_zygosity_genotypes(num_elements, ploidy);
    } catch (...) {}
    return result;
}

} // namespace octopus


#include "NelderMead.hpp"

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

class Example : public NelderMead<Eigen::VectorXd, double> {
    double f(const Eigen::VectorXd& ys){
        double res = 0.0;
        double m = 1.0;
        for(int i=0; i<ys.size(); i++){
            res += m*std::abs(ys[i]);
            m*=10000.0;
        }
        return res;
    }
};

int main(){
    std::vector<Eigen::VectorXd> xs;

    xs.push_back(Eigen::Vector2d(1, 101));
    xs.push_back(Eigen::Vector2d(2, 100));
    xs.push_back(Eigen::Vector2d(3, 101));

    Example q;
    q.setSimplex(xs);
    for(int i=0; i<200; i++){
        q.iterate();
        std::cout<<q.current().first<<"\n";
    }
}


#include <armadillo>
#include <cstring>
#include <ctime>
#include "gyro_rocket6g.hh"
#include "matrix_tool.hh"
#include "stochastic.hh"

GyroRocket6G::GyroRocket6G()
{
    snprintf(name, sizeof(name), "Rocket6G Gyro Sensor Model");
    srand(static_cast<unsigned int>(time(NULL)));
}

void GyroRocket6G::algorithm(LaunchVehicle *VehicleIn)
{
    arma::vec3 WBIB = VehicleIn->DM->WBIB;

    //-------------------------------------------------------------------------
    // ARW RRW
    double sig(1.0);
    double RRW(0.0130848811);  // 0.4422689813  7.6072577e-3
    double ARW(0.2828427125);  // 0.07071067812  7.90569415e-3
    double Freq(200.0);

    for (int i = 0; i < 3; i++) {
        VehicleIn->Sensor->ITA2_G(i) =
            gauss(0, 1.0) * RRW * RAD;  // distribution(generator) * RRW * RAD;
        VehicleIn->Sensor->BETA_G(i) = 0.9999 * VehicleIn->Sensor->BETA_G(i) +
                                       VehicleIn->Sensor->ITA2_G(i) * VehicleIn->dt;
        VehicleIn->Sensor->ITA1_G(i) = gauss(0, 1.0) *
                                       (ARW * sqrt(Freq) / 60. * (1 / sig)) *
                                       RAD;  // distribution(generator) * (ARW *
                                             // sqrt(Freq) / 60 * (1 / sig)) * RAD;
    }

    // combining all uncertainties
    VehicleIn->Sensor->EWBIB = VehicleIn->Sensor->ITA1_G +
                               VehicleIn->Sensor->BETA_G;  // EMSBG + EUG + EWG;

    VehicleIn->Sensor->WBICB = WBIB + VehicleIn->Sensor->EWBIB;

    return;
}


/**
 * @author      Mahdi Maghrebi <mahdi.maghrebi@nih.gov>
 * October 2019
 * This is the Implementation of K-NN Algorithm in Distributed Systems as developed
 * in "PANDA: Extreme Scale Parallel K-Nearest Neighbor on Distributed Architectures", Patwary et a., 2016
 */

#include <iostream>
#include <string>
#include <fstream>
#include <mpi.h>
#include <math.h>
#include <vector>
#include <stack>
#include <boost/iostreams/device/mapped_file.hpp>
#include <boost/iostreams/stream.hpp>
#include <set>
#include <omp.h>
#include <iomanip>

using namespace std;
/**
 * Read the output of linux command execution
 * @param  cmd  is the inux command to be executed
 * @return the output from the execution of the linux command
 */
std::string exec(const char* cmd) {
	std::array<char, 128> buffer;
	std::string result;
	std::unique_ptr<FILE, decltype(&pclose)> pipe(popen(cmd, "r"), pclose);
	if (!pipe) {
		throw std::runtime_error("popen() failed!");
	}
	while (fgets(buffer.data(), buffer.size(), pipe.get()) != nullptr) {
		result += buffer.data();
	}
	return result;
}
/**
 * Defining the criteria for Sorting the data in a pair container from the biggest value to the smallest
 */
bool sortinrev(const pair<double,int> &a,const pair<double,int> &b) {
	return (a.first > b.first);
}
/**
 * Compute the variance of a sampled data over data dimensions and Sort dimensions according to their variability
 * @param   DataCounts  Number of total data from which we take the samples
 * @param   nodeData0 Dataset containing the data available for sampling
 * @param   featureCounts Number of features in dataset (equal to number of columns in the input csv file)
 * @param   world_size  Total number of MPI processors
 * @param	globalKdTreeSamples Number of Samples from dataset for computation here
 * @return  VectorGlobalSqrtSum A sorted pair containig the index of the dimensions with the highest variability
 */
auto findMaxVarDims(int DataCounts,double **nodeData0, int featureCounts, int world_size, int globalKdTreeSamples) {
	double samplingData[globalKdTreeSamples][featureCounts];
	double localSum[featureCounts], globalSum[featureCounts];
	double localSqrtSum[featureCounts], globalSqrtSum[featureCounts];

	for (int j=0; j<featureCounts; ++j){
		localSum[j]=0;
		localSqrtSum[j]=0;
	}

	for (int i=0; i< globalKdTreeSamples; ++i){
		int randomIndex=rand()%DataCounts;
		for (int j=0; j<featureCounts; ++j){
			samplingData[i][j]=nodeData0[randomIndex][j];
			localSum[j]+=samplingData[i][j];
		}
	}

	MPI_Allreduce(localSum,globalSum,featureCounts,MPI_DOUBLE,MPI_SUM,MPI_COMM_WORLD);

	for (int i=0; i< globalKdTreeSamples; ++i){
		for (int j=0; j<featureCounts; ++j){
			localSqrtSum[j]+=pow((samplingData[i][j]-(globalSum[j]/world_size/globalKdTreeSamples)),2) ;
		}
	}

	MPI_Allreduce(localSqrtSum,globalSqrtSum,featureCounts,MPI_DOUBLE,MPI_SUM,MPI_COMM_WORLD);

	vector<pair<double,int>> VectorGlobalSqrtSum;

	for (int j=0; j<featureCounts; ++j){
		VectorGlobalSqrtSum.push_back(make_pair(globalSqrtSum[j],j));
	}
	sort(VectorGlobalSqrtSum.begin(), VectorGlobalSqrtSum.end(),sortinrev);
	return VectorGlobalSqrtSum;
}
/**
 * Compute the distance between 2 data points within the same bucket
 * @param  index Index of the first data point
 * @param  index2 Index of the second data point
 * @param  mappedData2 2D array containing dataset	owned by each processor
 * @param  featureCounts Number of features in dataset (equal to number of columns in the input csv file)
 * @return sqrt(dist) The distance between 2 data points within the same bucket
 */
double computeDistance (int index,int index2, double** mappedData2, int featureCounts){
	double dist=0;
	for (int i=0; i<featureCounts; ++i){
		double differences=mappedData2[index][i]-mappedData2[index2][i];
		dist+=differences*differences;
	}
	return sqrt(dist);
}
/**
 * Compute the distance between 2 data points during querying
 * @param   index Index of the first data point
 * @param   i Index of the processor that has sent query
 * @param   jj Beginning index of the desired data point in the received data from the querying processor
 * @param   mappedData2 2D array containing dataset	owned by the current processor
 * @param   receivingPointCoordinates 2D array containing data received from the querying processors
 * @param   featureCounts Number of features in dataset (equal to number of columns in the input csv file)
 * @return sqrt(dist) The distance between 2 data points
 */
double computeDistance2 (int index, int i, int jj, double** mappedData2, double** receivingPointCoordinates,int featureCounts ){
	double dist=0;
	for (int k=0; k<featureCounts; ++k){
		double differences= mappedData2[index][k]-receivingPointCoordinates[i][jj+k];
		dist+=differences*differences;
	}
	return sqrt(dist);
}
/**
 * Compute the median of data at a dividing node of the global Kd Tree
 * @param  maxVarDimension Index of the chosen dimension for computing median
 * @param  nodeDataIndex0 Vector containing the indices of data available at the dividing node
 * @param   globalKdTreeSamplesMedian Number of data sampled by each processor to collaboratively compute the median at the dividing node of the global Kd tree
 * @param   Epsilon The acceptable buffer in estimating the median
 * @param   world_size Total number of MPI processors
 * @param   world_rank Rank of each MPI processor
 * @param	data 2D array containing the datapoint coordinates owned by each processor
 * @return  MedianCandidate The estimated value of median at the dividing node
 */
double globalFindMedian (int maxVarDimension, vector<int> nodeDataIndex0, int globalKdTreeSamplesMedian, double Epsilon, int world_size, int world_rank, double** data) {
	int randomIndex;
	vector <double> sampledDataValues, leftSampledDataValues, rightSampledDataValues;
	sampledDataValues.reserve(globalKdTreeSamplesMedian);
	leftSampledDataValues.reserve(globalKdTreeSamplesMedian);
	rightSampledDataValues.reserve(globalKdTreeSamplesMedian);

	for (int i=0; i< globalKdTreeSamplesMedian; ++i){
		randomIndex=rand()%nodeDataIndex0.size();
		int index=nodeDataIndex0[randomIndex];
		sampledDataValues.push_back(data[index][maxVarDimension]);
	}

	int randomRank;
	double MedianCandidate;
	int totalCountsData=world_size*globalKdTreeSamplesMedian;
	int accumulatedLeftCounts=0;
	bool whileFlag=true;
	int whileCount=0;

	while(whileFlag){
		if (world_rank==0) {randomRank=rand()%world_size;}
		MPI_Bcast(&randomRank,1,MPI_INT,0,MPI_COMM_WORLD);

		if (world_rank==randomRank) {
			randomIndex=rand()%sampledDataValues.size();
			MedianCandidate=sampledDataValues[randomIndex];
		}
		MPI_Bcast(&MedianCandidate,1,MPI_DOUBLE,randomRank,MPI_COMM_WORLD);

		int leftCounts=0; int rightCounts=0;int globalleftCounts=0;
		leftSampledDataValues.clear();
		rightSampledDataValues.clear();

		for (int i=0; i<sampledDataValues.size() ; ++i){
			if (sampledDataValues[i] < MedianCandidate) {
				leftSampledDataValues.push_back(sampledDataValues[i]);
				++leftCounts;
			}
			else{
				rightSampledDataValues.push_back(sampledDataValues[i]);
				++rightCounts;
			}
		}

		MPI_Allreduce(&leftCounts,&globalleftCounts,1,MPI_INT,MPI_SUM,MPI_COMM_WORLD);
		globalleftCounts+=accumulatedLeftCounts;
		double ratio= double(globalleftCounts)/totalCountsData;

		if ( ratio < 0.5+Epsilon && ratio > 0.5-Epsilon ) {
			whileFlag=false;
			return MedianCandidate ;}
		else if (ratio < 0.5-Epsilon){
			accumulatedLeftCounts=globalleftCounts;
			sampledDataValues.clear();
			sampledDataValues=rightSampledDataValues;
		}

		++whileCount;
		// For diagnosis, the following error hints at the difficulty of finding the median
		MPI_File logfile;
		char line[1024];
		if (whileCount % 10000 == 0) {
			printf("Too Many Trials for Global KD Tree Median, Processor = %d \n",world_rank);
			sprintf(line,"Too Many Trials for Global KD Tree Median, Processor = %d \n",world_rank);
			MPI_File_write(logfile, line, strlen(line), MPI_CHAR, MPI_STATUS_IGNORE);
		}
	}
}
/**
 * Compute the median of data at a dividing node of the local Kd Tree
 * @param  localKdTreeSamplesMedian Number of samples used to compute the median
 * @paramn  sampledDataValues The coordinates of the sampled data
 * @param   Epsilon The acceptable buffer in estimating the median
 * @param   world_rank Rank of each MPI processor
 * @return  MedianCandidate The estimated value of median at the dividing node
 */
double localFindMedian (int localKdTreeSamplesMedian,vector<double> sampledDataValues, double Epsilon, int world_rank) {
	vector <double> leftSampledDataValues, rightSampledDataValues;
	leftSampledDataValues.reserve(localKdTreeSamplesMedian);
	rightSampledDataValues.reserve(localKdTreeSamplesMedian);

	int accumulatedLeftCounts=0;
	bool whileFlag=true;
	int whileCount=0;

	while(whileFlag){
		int randomIndex=rand()%sampledDataValues.size();
		double MedianCandidate=sampledDataValues[randomIndex];
		int leftCounts=0;
		int rightCounts=0;
		leftSampledDataValues.clear();
		rightSampledDataValues.clear();

		for (int i=0; i<sampledDataValues.size() ; ++i){
			if (sampledDataValues[i] < MedianCandidate) {
				leftSampledDataValues.push_back(sampledDataValues[i]);
				++leftCounts;
			}
			else{
				rightSampledDataValues.push_back(sampledDataValues[i]);
				++rightCounts;
			}
		}
		leftCounts+=accumulatedLeftCounts;
		double ratio= double(leftCounts)/localKdTreeSamplesMedian;

		if ( ratio < 0.5+Epsilon && ratio > 0.5-Epsilon ) {
			whileFlag=false;
			return MedianCandidate;
		}
		else if (ratio < 0.5-Epsilon){
			accumulatedLeftCounts=leftCounts;
			sampledDataValues.clear();
			sampledDataValues=rightSampledDataValues;
			whileFlag=true;
		}
		else if (ratio > 0.5+Epsilon){
			whileFlag=true;
		}
		++whileCount;

		if (whileCount % 10000 == 0) {
			if (Epsilon<0.25) Epsilon*=2;
			else return MedianCandidate;
		}
	}
}
/**
 * Sort the max-heap data structure for a new data inserted at its index i
 * @param  ID The ID of the point data
 * @paramn  i Index of the inserted data in the Heap
 * @param   KNNDistanceinBuckets The values of distances for selected K-NNs
 * @param   KNNIDsinBuckets The IDs of the selected K-NNs
 * @param   KNNCounts  Desired count of K-NNs to be computed in this program
 */
void Max_Heapify(int ID, int i, double ** KNNDistanceinBuckets, int ** KNNIDsinBuckets,int KNNCounts) {
	int largest = 0;
	int l = 2*i + 1;
	int r = 2*i + 2;

	if ((l < KNNCounts) && (KNNDistanceinBuckets[ID][l] > KNNDistanceinBuckets[ID][i])) {
		largest = l;
	}
	else {
		largest = i;
	}

	if ((r < KNNCounts) && (KNNDistanceinBuckets[ID][r] > KNNDistanceinBuckets[ID][largest])) {
		largest = r;
	}

	if (largest != i) {
		std::swap(KNNDistanceinBuckets[ID][i], KNNDistanceinBuckets[ID][largest]);
		std::swap(KNNIDsinBuckets[ID][i], KNNIDsinBuckets[ID][largest]);
		Max_Heapify(ID, largest, KNNDistanceinBuckets, KNNIDsinBuckets,KNNCounts);
	}
}
/**
 * Build Max-Heap datat structure for the first time
 * @param  ID The ID of the point data
 * @param   KNNCounts  Desired count of K-NNs to be computed in this program
 * @param   KNNDistanceinBuckets The values of distances for selected K-NNs
 * @param   KNNIDsinBuckets The IDs of the selected K-NNs
 */
void Build_Max_Heap(int ID,int KNNCounts, double** KNNDistanceinBuckets, int** KNNIDsinBuckets) {
	for (int i = floor((KNNCounts - 1) / 2); i >= 0; i--) {
		Max_Heapify(ID, i,KNNDistanceinBuckets, KNNIDsinBuckets,KNNCounts);
	}
}
/**
 * Sort the max-heap data structure for a newly inserted data
 * @param   k The index of the inserted point data
 * @param   receivingHeapArrayDistances2DCopy The values of distances for selected K-NNs
 * @param   receivingHeapArray2DCopy The IDs of the selected K-NNs
 * @param   KNNCounts  Desired count of K-NNs to be computed in this program
 */
void Max_Heapify2 (int k, double * receivingHeapArrayDistances2DCopy, int * receivingHeapArray2DCopy,int KNNCounts) {
	int largest = 0;
	int l = 2*k + 1;
	int r = 2*k + 2;

	if ((l < KNNCounts) && (receivingHeapArrayDistances2DCopy[l] > receivingHeapArrayDistances2DCopy[k])) {
		largest = l;
	}
	else {
		largest = k;
	}

	if ((r < KNNCounts) && (receivingHeapArrayDistances2DCopy[r] > receivingHeapArrayDistances2DCopy[largest])) {
		largest = r;
	}

	if (largest != k) {
		std::swap(receivingHeapArrayDistances2DCopy[k], receivingHeapArrayDistances2DCopy[largest]);
		std::swap(receivingHeapArray2DCopy[k], receivingHeapArray2DCopy[largest]);
		Max_Heapify2(largest,receivingHeapArrayDistances2DCopy,receivingHeapArray2DCopy,KNNCounts);
	}
}
/**
 * Build Max-Heap datat structure for the first time
 * @param   KNNCounts  Desired count of K-NNs to be computed in this program
 * @param   receivingHeapArrayDistances2DCopy The values of distances for selected K-NNs
 * @param   receivingHeapArray2DCopy The IDs of the selected K-NNs
 */
void Build_Max_Heap2(int KNNCounts, double* receivingHeapArrayDistances2DCopy, int* receivingHeapArray2DCopy) {
	for (int ii = floor((KNNCounts - 1) / 2); ii >= 0; ii--) {
		Max_Heapify2(ii,receivingHeapArrayDistances2DCopy,receivingHeapArray2DCopy,KNNCounts);
	}
}

/**
 * Main Function of the Code
 */
int main(int argc, char * const argv[]) {
	/**
	 * MPI Parallel Logfile
	 */
	MPI_File logfile;
	char line[1024];
	/**
	 * Beginning MPI communications
	 */
	MPI_Init(NULL, NULL);
	/**
	 * world_size is defined here as total number of MPI processors
	 */
	int world_size;
	MPI_Comm_size(MPI_COMM_WORLD, &world_size);
	/**
	 * world_rank is defined here as the rank of MPI processors
	 */
	int world_rank;
	MPI_Comm_rank(MPI_COMM_WORLD, &world_rank);
	/**
	 * The errors and informational messages are outputted to the log file
	 */
	MPI_File_open(MPI_COMM_WORLD, "Setting.txt", MPI_MODE_WRONLY | MPI_MODE_CREATE,MPI_INFO_NULL, &logfile);
	/**
	 * The following arguments are passed to the code (in order) from the command line:
	 * fileName is the full path to the input csv dataset
	 * KNNCounts is the desired number of K-NNs for each data point to be computed in this code
	 * featureCounts is the number of columns in the input csv datastet (number of data dimensions)
	 */
	string fileName = argv[1];
	const int KNNCounts = atoi(argv[2]);

	int featureCounts, colIndex1, colIndex2;
	if (argc == 3) {
		string cmd0="head -n 1 "+ fileName + " |tr '\\,' '\\n' |wc -l ";
		featureCounts = stoi(exec(cmd0.c_str()));
	} else if (argc == 5) {
		string cmd0="head -n 1 "+ fileName + " |tr '\\,' '\\n' |wc -l ";
		featureCounts = stoi(exec(cmd0.c_str()));
		colIndex1 = atoi(argv[3]);
		colIndex2 = atoi(argv[4]);
	} else 	{
		printf("Wrong Input Arguments\n");
		sprintf(line,"Wrong Input Arguments\n");
		MPI_File_write(logfile, line, strlen(line), MPI_CHAR, MPI_STATUS_IGNORE);
		return -1;
	}
	/**
	 * The following important parameters are used in the design of algorithm. Their values are
	 * initialized according to the suggested values in the referencing paper.
	 * globalKdTreeSamples is the number of data sampled by each processor to collaboratively compute dimensions with the highest variability.
	 * globalKdTreeSamplesMedian is the number of data sampled by each processor to collaboratively compute the median of the chosen dimension for each splitting node within the global Kd Tree.
	 * localKdTreeSamplesMedian is the number of data sampled by each processor separately to compute the median of the chosen dimension for each splitting node within the local Kd Tree.
	 * Epsilon is a buffer in accepting the Median value
	 * Parallel_IO is a flag that defines if the input csv file can be read in parallel by all the processors
	 * bucketSize is the size of a bucket (or a leaf) in the local Kd Tree
	 * estimatedExtraLayers: To limit the growing size of the local Kd Trees, the growth of the tree is limited by a number of layers defined here from the initial guess of the required buckets
	 */
	const int globalKdTreeSamples=256;
	const int globalKdTreeSamplesMedian=256;
	int localKdTreeSamplesMedian=1024;
	double Epsilon=0.01;
	const int Parallel_IO = 1;
	const int bucketSize=32;
	const int estimatedExtraLayers=1;
	/**
	 * Seed for random number generation
	 */
	srand(17);
	/**
	 * total number of MPI processors should be a power of 2 due to algorithm design for global Kd Tree.
	 * Otherwise, output an error and exit the program
	 */
	bool powerOfTwo = !(world_size == 0) && !(world_size & (world_size - 1));
	if (powerOfTwo!=true) {
		if (world_rank==0) {
			printf("Number of Processors should be a power of 2\n");
			sprintf(line,"Number of Processors should be a power of 2\n");
			MPI_File_write(logfile, line, strlen(line), MPI_CHAR, MPI_STATUS_IGNORE);
		}
		MPI_Finalize();
		return 0;
	}
	int numericWidth=floor(log10(world_size) + 1);
	/**
	 * The master processor splits the input csv file as each processor could have its own non-overlapping set of input data
	 */
	if (world_rank==0) {
		string cmd=string("split -n l/")+to_string(world_size)+" "+ fileName+" -a "+to_string(numericWidth)+" -d tmpFile --additional-suffix=.csv";
		int returnValue=system(cmd.c_str());
	}
	/**
	 * All procesors neeed to stop here until master processor returns
	 */
	MPI_Barrier(MPI_COMM_WORLD);
	/**
	 * Each processor reads its own set of data from a unique csv file (localFileName)
	 */
	int worldRankWidth=floor(log10(world_rank) + 1);
	std::stringstream ss;
	ss << std::setw(numericWidth-worldRankWidth) << std::setfill('0') << world_rank;
	std::string s = ss.str();
	string localFileName="tmpFile"+s+".csv";

	ifstream infile;
	infile.open(localFileName);
	/**
	 * Output error in case the localFileName was not opened for reading
	 */
	if(infile.fail()) {
		printf("error in opening the input file\n");
		sprintf(line,"error in opening the input file\n");
		MPI_File_write(logfile, line, strlen(line), MPI_CHAR, MPI_STATUS_IGNORE);
		return 1;
	}
	/**
	 * Each processor finds out about the number of records in its localFileName
	 */
	string cmd3="wc -l "+localFileName;
	string outputCmd3 = exec(cmd3.c_str());
	int tmpFileLineCounts=stoi(outputCmd3.substr(0, outputCmd3.find(" ")));
	/**
	 * The master node needs to subtract 1 record which is for header information
	 */
	if (world_rank==0) {
		string dummyLine;
		getline(infile, dummyLine);
		--tmpFileLineCounts;
	}
	/**
	 * MPI communication between the processors as they all need to know how many data the other processors have
	 */
	int tmpFileLineCountsArray[world_size], tmpFileLineCountsArrayCum[world_size] ;
	int sendBuffer0[0];
	sendBuffer0[0]=tmpFileLineCounts;
	MPI_Allgather(sendBuffer0,1,MPI_INT,tmpFileLineCountsArray,1,MPI_INT,MPI_COMM_WORLD);
	/**
	 * All Processors make an array tmpFileLineCountsArrayCum that cummulatively stores the number of data in the other processors
	 */
	for (int i=0; i<world_size; ++i) {
		tmpFileLineCountsArrayCum[i]=0;
	}

	for (int i=0; i<world_size; ++i) {
		for (int j=0; j<i+1; ++j) {
			tmpFileLineCountsArrayCum[i]+=tmpFileLineCountsArray[j];
		}
	}
	if (world_rank==0) {
		printf("The input csv file contains %d rows of raw data (w/o header) with %d columns\n",tmpFileLineCountsArrayCum[world_size-1],featureCounts);
		sprintf(line,"The input csv file contains %d rows of raw data (w/o header) with %d columns\n",tmpFileLineCountsArrayCum[world_size-1],featureCounts);
		MPI_File_write(logfile, line, strlen(line), MPI_CHAR, MPI_STATUS_IGNORE);
	}
	/**
	 * Parse data from csv file and store them in a 2D array
	 */
	double ** inputdata= new double*[tmpFileLineCounts];;
	for (int i=0; i<tmpFileLineCounts; ++i) { inputdata[i] = new double[featureCounts]; }

	if (argc==3){
		for (int i=0; i<tmpFileLineCounts; ++i) {
			string temp, temp2;
			getline(infile, temp);
			for (int j=0; j<featureCounts; ++j){
				temp2 =temp.substr(0, temp.find(","));
				inputdata[i][j]=atof(temp2.c_str());
				temp.erase(0, temp.find(",") + 1);
			}
		}
	} else {
		for (int i=0; i<tmpFileLineCounts; ++i) {
			string temp, temp2;
			getline(infile, temp);
			for (int j=0; j<featureCounts; ++j){
				temp2 =temp.substr(0, temp.find(","));
				if (j >= colIndex1-1 && j < colIndex2) inputdata[i][j] = atof(temp2.c_str());
				temp.erase(0, temp.find(",") + 1);
			}
		}
	}
	if (argc == 5) featureCounts=colIndex2-colIndex1+1;
	/**
	 * Remove the local input files as their data has been already parsed and read
	 */
	infile.close();
	string cmd2= string("rm ")+localFileName;
	int returnValue=system(cmd2.c_str());
	/**
	 * Query about the number of available OpenMP processors and set it for OpenMP
	 */
	int nProcessors = omp_get_num_procs();
    omp_set_num_threads(nProcessors-1);
	cout <<"Total Number of OpenMP Processes in the Parallel Region = "<< nProcessors-1 <<endl;

	/**
	 * Compute dimensions with the highest variance
	 */
	vector<pair<double,int>> VectorGlobalSqrtSum;
	vector <int> nodeDataIndex[world_size];
	nodeDataIndex[0].reserve(tmpFileLineCounts);
	for (int i=0; i<tmpFileLineCounts; ++i){nodeDataIndex[0].push_back(i);}
	VectorGlobalSqrtSum=findMaxVarDims(nodeDataIndex[0].size(), inputdata, featureCounts, world_size, globalKdTreeSamples);
	/**
	 * Constructing the global Kd Tree collaboratively by all the processors
	 */
	vector<double> globalMedianValuesforNodes;
	vector <int> nextLayerNodeDataIndex[world_size];
	int nodeCounts=1, nodesLayer=0;
	double medianNodeData;

	while (nodeCounts!= world_size){
		if (world_rank ==0) {
			printf("Constructing Global Kd Tree: Layer = %d \n",nodesLayer);
			sprintf(line,"Constructing Global Kd Tree: Layer = %d \n",nodesLayer);
			MPI_File_write(logfile, line, strlen(line), MPI_CHAR, MPI_STATUS_IGNORE);
		}
		int indexMaxVarDim=VectorGlobalSqrtSum[nodesLayer].second;

		for (int i=0; i<nodeCounts; ++i){
			int countLeft=0, countRight=0;
			medianNodeData=globalFindMedian(indexMaxVarDim,nodeDataIndex[i], globalKdTreeSamplesMedian,Epsilon, world_size,world_rank, inputdata);
			globalMedianValuesforNodes.push_back(medianNodeData);

			for (int j=0; j< nodeDataIndex[i].size(); ++j){
				int index=nodeDataIndex[i][j];
				if (inputdata[index][indexMaxVarDim] < medianNodeData){
					nextLayerNodeDataIndex[i*2].push_back(index);
					++countLeft;
				}
				else{
					nextLayerNodeDataIndex[i*2+1].push_back(index);
					++countRight;
				}
			}
		}
		nodeCounts*=2;
		++nodesLayer;

		for (int i=0; i<nodeCounts; ++i){
			nodeDataIndex[i].clear();
			nodeDataIndex[i]=nextLayerNodeDataIndex[i];
			nextLayerNodeDataIndex[i].clear();
		}
	}

	int indexMaxVarDim=VectorGlobalSqrtSum[nodesLayer].second;
	for (int i=0; i<nodeCounts; ++i){
		int countLeft=0, countRight=0;
		medianNodeData=globalFindMedian(indexMaxVarDim,nodeDataIndex[i], globalKdTreeSamplesMedian,Epsilon, world_size,world_rank, inputdata);
		globalMedianValuesforNodes.push_back(medianNodeData);
	}

	/**
	 * Once the number of dividing nodes in the global Kd Tree became equal to the number of MPI processors
	 * each processor will be responsible for the data of one dividing node
	 * Index of data for each processor is stored at ProcessorLocalDataIndex
	 */
	int *ProcessorLocalDataIndex;
	int cnts;

	for (int i=0; i<nodeCounts; ++i){
		int rcount[world_size];
		int send_buffer[0];
		int displs[nodeCounts];
		displs[0]=0;
		int myDATA[nodeDataIndex[i].size()];

		for (int j=0; j< nodeDataIndex[i].size(); ++j){
			if (world_rank>0) myDATA[j]= nodeDataIndex[i][j]+tmpFileLineCountsArrayCum[world_rank-1];
			else if(world_rank==0) myDATA[j]= nodeDataIndex[i][j];
		}

		int Totalcounts=(int)nodeDataIndex[i].size();
		send_buffer[0]=nodeDataIndex[i].size();
		MPI_Gather(send_buffer,1, MPI_INT,rcount,1, MPI_INT,i,MPI_COMM_WORLD);

		if (world_rank==i){
			cnts=0;
			for (int k=0; k<nodeCounts; ++k){cnts+=rcount[k];}
			for (int k=1; k<nodeCounts; ++k){displs[k]=displs[k-1]+rcount[k-1];}
			ProcessorLocalDataIndex = new int[cnts];
		}
		MPI_Gatherv(myDATA,Totalcounts,MPI_INT,ProcessorLocalDataIndex,rcount,displs,MPI_INT,i,MPI_COMM_WORLD);
	}
	/**
	 * Now, each processor only reads its own data from the input csv file according to the indices of ProcessorLocalDataIndex
	 * If parallel I/O is not available (Parallel_IO=0), each processor reads the file at a time
	 * the main output of this section is mappedData which is a 2D array storing dataset
	 */
	int rankOfProcess=0;
	double mappedData[cnts][featureCounts];
	int indexLookupArray[tmpFileLineCountsArrayCum[world_size-1]];

	if (Parallel_IO){
		using boost::iostreams::mapped_file_source;
		using boost::iostreams::stream;
		mapped_file_source mmap(fileName);
		stream<mapped_file_source> is(mmap, std::ios::binary);
		string tempString,tempString2;
		int m_numLines = 0;
		string dummyLine;
		getline(is, dummyLine);

		for (int i=0; i<cnts; ++i){
			int lineIndex=ProcessorLocalDataIndex[i];
			bool flag=true;

			while (flag==true){
				if (m_numLines==lineIndex) {
					indexLookupArray[lineIndex]=i;
					getline(is, tempString);
					for (int j=0;j<featureCounts;++j){
						tempString2 =tempString.substr(0, tempString.find(","));
						mappedData[i][j]=atof(tempString2.c_str());
						tempString.erase(0, tempString.find(",") + 1);
					}
					m_numLines++;
					flag=false;
				}
				else {
					getline(is, dummyLine);
					m_numLines++;
					flag=true;
					if(!is) {flag=false; break;}
				}
			}
		}
		mmap.close();
	}
	else{
		while (rankOfProcess < world_size){
			if (world_rank==rankOfProcess){
				using boost::iostreams::mapped_file_source;
				using boost::iostreams::stream;
				mapped_file_source mmap(fileName);
				stream<mapped_file_source> is(mmap, std::ios::binary);
				string tempString,tempString2;
				int m_numLines = 0;
				string dummyLine;
				getline(is, dummyLine);

				for (int i=0; i<cnts; ++i){
					int lineIndex=ProcessorLocalDataIndex[i];
					bool flag=true;

					while (flag==true){
						if (m_numLines==lineIndex) {
							indexLookupArray[lineIndex]=i;
							getline(is, tempString);
							for (int j=0;j<featureCounts;++j){
								tempString2 =tempString.substr(0, tempString.find(","));
								mappedData[i][j]=atof(tempString2.c_str());
								tempString.erase(0, tempString.find(",") + 1);
							}
							m_numLines++;
							flag=false;
						}
						else {
							getline(is, dummyLine);
							m_numLines++;
							flag=true;
							if(!is) {flag=false; break;}
						}
					}
				}
				mmap.close();
			}
			++rankOfProcess;
			MPI_Barrier(MPI_COMM_WORLD);
		}
	}
	/**
	 * Now, it is the time to construct the local Kd Tree by each processor separately
	 * Tree construction continues until all data is stored in the buckets of size bucketSize
	 * or maxAllowedLayers is reached
	 */
	if (world_rank==0) {
		printf("Constructing the Local Kd Tree\n");
		sprintf(line,"Constructing the Local Kd Tree\n");
		MPI_File_write(logfile, line, strlen(line), MPI_CHAR, MPI_STATUS_IGNORE);
	}

	int layerNodeCounts=1;
	int localNodesLayer=0;
	vector < vector<int> > localNodeDataIndex;
	vector <int> tmpvector;
	vector <double> localMedianNodeData;
	vector <int> isBucket;
	bool localFlag=true;
	int numberofNodeSofar;
	int nodeIndexofaPoint[cnts];
	tmpvector.reserve(cnts);

	for (int i=0; i<cnts; ++i){
		tmpvector.push_back(ProcessorLocalDataIndex[i]);
		nodeIndexofaPoint[i]=-1;
	}
	localNodeDataIndex.push_back(tmpvector);

	isBucket.reserve(localNodeDataIndex[0].size()/bucketSize);
	localMedianNodeData.reserve(localNodeDataIndex[0].size()/bucketSize);
	/**
	 * Ideally we need estimatedLayers number of layers in the local Kd tree
	 */
	int estimatedLayers=int(log2(localNodeDataIndex[0].size()/bucketSize))+1;
	int maxAllowedLayers=estimatedLayers+estimatedExtraLayers;
	if (maxAllowedLayers+nodesLayer > featureCounts){
		printf("Error in Exceeding Dimensions, increase BucketSize\n");
		sprintf(line,"Error in Exceeding Dimensions, increase BucketSize\n");
		MPI_File_write(logfile, line, strlen(line), MPI_CHAR, MPI_STATUS_IGNORE);
	}

	if (localNodeDataIndex[0].size() <= bucketSize+1) {isBucket.push_back(1); localFlag=false;}
	else {isBucket.push_back(0);}

	while (localFlag){
		int indexMaxVarDim=VectorGlobalSqrtSum[localNodesLayer+nodesLayer].second;
		if (localNodesLayer==0) {numberofNodeSofar=0;}
		else {numberofNodeSofar=pow(2,localNodesLayer)-1;}

		for (int i=0; i<layerNodeCounts; ++i){
			int globalID=numberofNodeSofar +i;
			int countLeft=0, countRight=0;
			int leftNodeGlobalIndex=numberofNodeSofar+layerNodeCounts+(i*2);
			int rightNodeGlobalIndex=numberofNodeSofar+layerNodeCounts+(i*2)+1;
			localNodeDataIndex.push_back(std::vector<int>());
			localNodeDataIndex.push_back(std::vector<int>());

			if (isBucket[globalID]==1) {isBucket.push_back(0); isBucket.push_back(0); localMedianNodeData.push_back(0); continue;}
			if (localNodeDataIndex[globalID].size()==0) {isBucket.push_back(0); isBucket.push_back(0); localMedianNodeData.push_back(0); continue;}
			if (localKdTreeSamplesMedian > localNodeDataIndex[globalID].size()/2) localKdTreeSamplesMedian=localNodeDataIndex[globalID].size()/2;
			vector <double> sampledDataValues;
			for (int i=0; i< localKdTreeSamplesMedian; ++i){
				int randomIndex=rand()%localNodeDataIndex[globalID].size();
				int index=localNodeDataIndex[globalID][randomIndex];
				int index1=indexLookupArray[index];
				sampledDataValues.push_back(mappedData[index1][indexMaxVarDim]);
			}

			double temp=localFindMedian(localKdTreeSamplesMedian,sampledDataValues,Epsilon,world_rank);
			localMedianNodeData.push_back(temp);

			for (int j=0; j< localNodeDataIndex[globalID].size(); ++j){
				int index0=localNodeDataIndex[globalID][j];
				int index=indexLookupArray[index0];
				if (mappedData[index][indexMaxVarDim] < localMedianNodeData[globalID]){
					localNodeDataIndex[leftNodeGlobalIndex].push_back(index0);
					++countLeft;
				}
				else{
					localNodeDataIndex[rightNodeGlobalIndex].push_back(index0);
					++countRight;
				}
			}

			if (countLeft ==1) {
				localNodeDataIndex[rightNodeGlobalIndex].push_back(localNodeDataIndex[leftNodeGlobalIndex][0]);
				localNodeDataIndex[leftNodeGlobalIndex].pop_back();
				--countLeft;
			}

			if (countRight ==1) {
				localNodeDataIndex[leftNodeGlobalIndex].push_back(localNodeDataIndex[rightNodeGlobalIndex][0]);
				localNodeDataIndex[rightNodeGlobalIndex].pop_back();
				--countRight;
			}

			if ((countLeft <= bucketSize+1 && countLeft >0)|| ((localNodesLayer == maxAllowedLayers-1) && countLeft >0) ) {
				isBucket.push_back(1);

				for (int j=0; j< localNodeDataIndex[leftNodeGlobalIndex].size(); ++j){
					int index0=localNodeDataIndex[leftNodeGlobalIndex][j];
					int index=indexLookupArray[index0];
					nodeIndexofaPoint[index]=leftNodeGlobalIndex;
				}
			}
			else {isBucket.push_back(0);}

			if ((countRight <= bucketSize+1 && countRight >0) || ((localNodesLayer == maxAllowedLayers-1) && countRight >0) ) {
				isBucket.push_back(1);

				for (int j=0; j< localNodeDataIndex[rightNodeGlobalIndex].size(); ++j){
					int index0=localNodeDataIndex[rightNodeGlobalIndex][j];
					int index=indexLookupArray[index0];
					nodeIndexofaPoint[index]=rightNodeGlobalIndex;
				}
			}
			else {isBucket.push_back(0);}
		}

		localFlag=false;
		for (int i=0; i<layerNodeCounts; ++i){
			int globalID=numberofNodeSofar + i;
			if (isBucket[globalID]==0 && localNodeDataIndex[globalID].size()>0 ) {localFlag=true; break;
			}
		}
		layerNodeCounts*=2;
		++localNodesLayer;
	}
	/**
	 * For performance, it is better to refer to local Kd tree later
	 * from the ID of the first dividing node which has been converted to a bucket
	 */
	int FirstBucket;
	for (int i=0; i< localNodeDataIndex.size(); ++i){
		if (isBucket[i] == 1) {FirstBucket=i;break;}
	}
	/**
	 * Now, it is the time to start computing K-NNs from the data points within each bucket in the local Kd Tree
	 * and store them in KNNIDsinBuckets and KNNDistanceinBuckets
	 * To improve the performance, the data locality was considered for main arrays of localNodeDataIndex2 and mappedData2
	 * and the data within the same bucket arranged close to each other in the new arrays
	 */
	if (world_rank==0) {
		printf("Computing K-NNs for the points within the Same Bucket\n");
		sprintf(line,"Computing K-NNs for the points within the Same Bucket\n");
		MPI_File_write(logfile, line, strlen(line), MPI_CHAR, MPI_STATUS_IGNORE);
	}
	int KNNIDsinBucketsFilledCounts[cnts];
	int localIndexConvertor[cnts];
	int counter=0;
	vector<vector<int>> localNodeDataIndex2;

	int **KNNIDsinBuckets = new int*[cnts];
	for (int i=0; i<cnts; ++i) { KNNIDsinBuckets[i] = new int[KNNCounts]; }

	double ** KNNDistanceinBuckets = new double*[cnts];
	for (int i=0; i<cnts; ++i) { KNNDistanceinBuckets[i] = new double[KNNCounts]; }

	for (int i=0; i< localNodeDataIndex.size(); ++i){
		localNodeDataIndex2.push_back(std::vector<int>());
		if (isBucket[i] == 0) {continue;}

		for (int j=0; j< localNodeDataIndex[i].size(); ++j){
			localIndexConvertor[counter]=localNodeDataIndex[i][j];
			localNodeDataIndex2[i].push_back(counter);
			++counter;
		}
	}

	double** mappedData2=new double*[cnts];
	for (int i=0; i<cnts; ++i) { mappedData2[i] = new double[featureCounts]; }

	int nodeIndexofaPoint2[cnts];

	for (int i=0; i< cnts; ++i){
		int pointID=localIndexConvertor[i];
		int index=indexLookupArray[pointID];
		nodeIndexofaPoint2[i]=nodeIndexofaPoint[index];
		for (int j=0; j<featureCounts; ++j){
			mappedData2[i][j]=mappedData[index][j];
		}
	}

	for (int i=0; i< cnts; ++i){
		for (int j=0; j< KNNCounts; ++j){
			KNNIDsinBuckets[i][j]=-1;
		}
	}

	for (int i=0; i< cnts; ++i){
		KNNIDsinBucketsFilledCounts[i]=0;
	}

	for (int i=FirstBucket; i< localNodeDataIndex2.size(); ++i){
		if (isBucket[i] == 0) {continue;}
		for (int j=0; j< localNodeDataIndex2[i].size()-1; ++j){
			int index=localNodeDataIndex2[i][j];
			for (int k=j+1; k<localNodeDataIndex2[i].size(); ++k){
				int index2=localNodeDataIndex2[i][k];
				int emptyIndex = KNNIDsinBucketsFilledCounts[index];
				double dist=computeDistance(index,index2,mappedData2,featureCounts);

				if  (emptyIndex < KNNCounts) {
					KNNIDsinBuckets[index][emptyIndex]=localIndexConvertor[index2];
					++KNNIDsinBucketsFilledCounts[index];
					KNNDistanceinBuckets[index][emptyIndex]=dist;
					if (emptyIndex==(KNNCounts-1)) Build_Max_Heap(index,KNNCounts,KNNDistanceinBuckets,KNNIDsinBuckets);
				}
				else {
					if (dist < KNNDistanceinBuckets[index][0]) {
						KNNIDsinBuckets[index][0]=localIndexConvertor[index2];
						KNNDistanceinBuckets[index][0]=dist;
						Max_Heapify(index, 0, KNNDistanceinBuckets, KNNIDsinBuckets,KNNCounts);
					}
				}

				int emptyIndex2 = KNNIDsinBucketsFilledCounts[index2];
				if  (emptyIndex2 < KNNCounts) {
					KNNIDsinBuckets[index2][emptyIndex2]=localIndexConvertor[index];
					++KNNIDsinBucketsFilledCounts[index2];
					KNNDistanceinBuckets[index2][emptyIndex2]=dist;
					if (emptyIndex2==(KNNCounts-1)) Build_Max_Heap(index2,KNNCounts,KNNDistanceinBuckets,KNNIDsinBuckets);
				}
				else {
					if (dist < KNNDistanceinBuckets[index2][0]) {
						KNNIDsinBuckets[index2][0]=localIndexConvertor[index];
						KNNDistanceinBuckets[index2][0]=dist;
						Max_Heapify(index2, 0, KNNDistanceinBuckets, KNNIDsinBuckets, KNNCounts);}
				}
			}
		}
	}
	/**
	 * Now, it is the time to find the IDs of processors that contain the neighboring sub-spaces
	 * A neighboring processor is selected if its distance from the given point is less than
	 * the maximum distance in the heap of that point (first entry of heap)
	 */
	if (world_rank==0) {
		printf("Finding the Spatial Neighboring Processors\n");
		sprintf(line,"Finding the Spatial Neighboring Processors\n");
		MPI_File_write(logfile, line, strlen(line), MPI_CHAR, MPI_STATUS_IGNORE);
	}

	vector<int> ScatterVlocalNodeDataIndex[world_size];
	vector<int> ScatterVKNNIDsinBucketsFilledCounts[world_size];

	int globalLayerID = int(log2(world_size));
	int lowestNodeID=pow(2,globalLayerID)-1;
	int highestNodeID=lowestNodeID+world_size-1;
	int NeighboringNodes[cnts][world_size-1];

	if (world_size != 1){
		for (int i=0; i<cnts; ++i){
			for (int j=0; j<world_size-1; ++j){
				NeighboringNodes[i][j]=-1;
			}
		}

		for (int i=FirstBucket; i< localNodeDataIndex2.size(); ++i){
			if (isBucket[i] == 0) {continue;}
			for (int j=0; j< localNodeDataIndex2[i].size(); ++j){
				int index1= localNodeDataIndex2[i][j];

				double rPrime=KNNDistanceinBuckets[index1][0];
				stack<pair<int,double>> globalStack;
				globalStack.push(make_pair(0,0));
				/**
				 * C1NodeID is the closer child, and C2NodeID is the other child
				 */
				int C1NodeID,C2NodeID;
				int jcounts=0;

				while (!globalStack.empty()){
					pair<int,double> topPairinStack=globalStack.top();
					int nodeID=topPairinStack.first;
					double dValue=topPairinStack.second;
					globalStack.pop();
					int nodesLayer0=int(log2(nodeID+1));
					int indexMaxVarDim=VectorGlobalSqrtSum[nodesLayer0].second;

					if (dValue < rPrime){
						double dPrime= mappedData2[index1][indexMaxVarDim] - globalMedianValuesforNodes[nodeID];
						if (dPrime < 0) {
							C1NodeID=2*nodeID+1;
							C2NodeID=2*nodeID+2;
						}
						else{
							C1NodeID=2*nodeID+2;
							C2NodeID=2*nodeID+1;
						}

						dPrime=sqrt(dValue*dValue+dPrime*dPrime);
						if (dPrime<rPrime) {
							if (C2NodeID <= highestNodeID) {
								globalStack.push(make_pair(C2NodeID,dPrime));
								if (C2NodeID >= lowestNodeID && (C2NodeID-lowestNodeID)!=world_rank) {
									NeighboringNodes[index1][jcounts]=C2NodeID-lowestNodeID;
									ScatterVlocalNodeDataIndex[C2NodeID-lowestNodeID].push_back(index1);
									ScatterVKNNIDsinBucketsFilledCounts[C2NodeID-lowestNodeID].push_back(KNNIDsinBucketsFilledCounts[index1]);
									++jcounts;
								}
							}
						}

						if (C1NodeID <= highestNodeID) {
							globalStack.push(make_pair(C1NodeID,dValue));
							if (C1NodeID >= lowestNodeID && (C1NodeID-lowestNodeID)!=world_rank) {
								NeighboringNodes[index1][jcounts]=C1NodeID-lowestNodeID;
								ScatterVlocalNodeDataIndex[C1NodeID-lowestNodeID].push_back(index1);
								ScatterVKNNIDsinBucketsFilledCounts[C1NodeID-lowestNodeID].push_back(KNNIDsinBucketsFilledCounts[index1]);
								++jcounts;
							}
						}
					}
				}
			}
		}
	}
	/**
	 * Now, send the data of the given point to the neighboring processors identified above
	 * for further computation of possible K-NNs in those processors
	 */
	int displ[world_size],displ2[world_size],displ3[world_size];
	int bufferCounts[world_size],bufferCounts2[world_size],bufferCounts3[world_size];
	bufferCounts[world_rank]=0;
	bufferCounts2[world_rank]=0;
	bufferCounts3[world_rank]=0;

	if (world_size != 1){
		for (int i=0; i<cnts; ++i){
			ScatterVlocalNodeDataIndex[world_rank].push_back(i);
			ScatterVKNNIDsinBucketsFilledCounts[world_rank].push_back(KNNIDsinBucketsFilledCounts[i]);
		}

		for (int i=0; i<world_size; ++i){
			bufferCounts[i]=ScatterVlocalNodeDataIndex[i].size();
			bufferCounts2[i]=ScatterVlocalNodeDataIndex[i].size()*KNNCounts;
			bufferCounts3[i]=ScatterVlocalNodeDataIndex[i].size()*featureCounts;
		}

		displ[0]=0;
		displ2[0]=0;
		displ3[0]=0;
		for (int i=1; i<world_size; ++i){
			displ[i]= displ[i-1]+bufferCounts[i-1];
			displ2[i]= displ2[i-1]+bufferCounts2[i-1];
			displ3[i]= displ3[i-1]+bufferCounts3[i-1];
		}
	}

	const int ArraySizeScatterV=displ[world_size-1]+bufferCounts[world_size-1];
	int sendbuffer[ArraySizeScatterV];
	int sendbuffer2[ArraySizeScatterV];
	int sendbuffer4[ArraySizeScatterV*KNNCounts];
	double sendbuffer5[ArraySizeScatterV*featureCounts];
	double sendbuffer6[ArraySizeScatterV*KNNCounts];

	if (world_size != 1){
		for (int i=0; i<world_size; ++i){
			int KIndex=displ[i];
			for (int j=0; j<ScatterVlocalNodeDataIndex[i].size(); ++j){
				sendbuffer[KIndex+j]=ScatterVlocalNodeDataIndex[i][j];
				sendbuffer2[KIndex+j]=ScatterVKNNIDsinBucketsFilledCounts[i][j];
				for (int kk=0; kk<KNNCounts; ++kk){
					sendbuffer4[(KIndex+j)*KNNCounts+kk]= KNNIDsinBuckets[ScatterVlocalNodeDataIndex[i][j]][kk];
					sendbuffer6[(KIndex+j)*KNNCounts+kk]= KNNDistanceinBuckets[ScatterVlocalNodeDataIndex[i][j]][kk];
				}
				for (int ll=0; ll<featureCounts; ++ll){
					sendbuffer5[(KIndex+j)*featureCounts+ll]= mappedData2[ScatterVlocalNodeDataIndex[i][j]][ll];
				}
			}
		}
	}

	int receivingCountsMatrix[world_size];
	int receiveCounts,TotalReceiveCounts=0;
	int *receivingIndices[world_size];
	int *receivingHeapSize[world_size];
	int *receivingHeapArray[world_size];
	double *receivingHeapArrayDistances[world_size];
	double *receivingPointCoordinates[world_size];

	if (world_size != 1){
		for (int i=0; i<world_size; ++i){
			MPI_Scatter (bufferCounts,1,MPI_INT,&receiveCounts,1 ,MPI_INT,i,MPI_COMM_WORLD);
			receivingIndices[i]=new int[receiveCounts];
			receivingHeapSize[i]=new int[receiveCounts];
			receivingHeapArray[i]=new int[receiveCounts*KNNCounts];
			receivingHeapArrayDistances[i]=new double[receiveCounts*KNNCounts];
			receivingPointCoordinates[i]=new double[receiveCounts*featureCounts];
			receivingCountsMatrix[i]=receiveCounts;
			TotalReceiveCounts+=receiveCounts;

			MPI_Scatterv (&sendbuffer ,bufferCounts, displ, MPI_INT,&receivingIndices[i][0],receiveCounts,MPI_INT,i,MPI_COMM_WORLD);
			MPI_Scatterv (&sendbuffer2,bufferCounts, displ, MPI_INT,&receivingHeapSize[i][0],receiveCounts,MPI_INT,i,MPI_COMM_WORLD);
			MPI_Scatterv (&sendbuffer4,bufferCounts2,displ2,MPI_INT,&receivingHeapArray[i][0],receiveCounts*KNNCounts,MPI_INT,i,MPI_COMM_WORLD);
			MPI_Scatterv (&sendbuffer5,bufferCounts3,displ3,MPI_DOUBLE,&receivingPointCoordinates[i][0],receiveCounts*featureCounts,MPI_DOUBLE,i,MPI_COMM_WORLD);
			MPI_Scatterv (&sendbuffer6,bufferCounts2,displ2,MPI_DOUBLE,&receivingHeapArrayDistances[i][0],receiveCounts*KNNCounts,MPI_DOUBLE,i,MPI_COMM_WORLD);
		}
	}
	else{
		receivingIndices[0]=new int[cnts];
		receivingHeapArray[0]=new int[cnts*KNNCounts];
		receivingHeapArrayDistances[0]=new double[cnts*KNNCounts];
		receivingPointCoordinates[0]=new double[cnts*featureCounts];
		receivingCountsMatrix[0]=cnts;
		receivingHeapSize[0]=new int[cnts];

		for (int i=0; i<cnts; ++i){
			receivingIndices[0][i]=i;
			receivingHeapSize[0][i]=KNNIDsinBucketsFilledCounts[i];

			for (int j=0; j<KNNCounts; ++j){
				receivingHeapArrayDistances[0][i*KNNCounts+j]=KNNDistanceinBuckets[i][j];
				receivingHeapArray[0][i*KNNCounts+j]=KNNIDsinBuckets[i][j];
			}

			for (int j=0; j<featureCounts; ++j){
				receivingPointCoordinates[0][i*featureCounts+j]=mappedData2[i][j];
			}
		}
	}
	delete[] ProcessorLocalDataIndex;
	/**
	 * Now, follow querying to compute possible K-NNs for each given point
	 * For each point, querying is performed on the local Kd Tree of its hosting processor as well as
	 * the local Kd Tree of the neighboring processors identified above
	 * This section is the implementation of Algorithm 1 in the referencing paper and is computationally the most expensive part of the code
	 */
	if (world_rank==0) {
		printf("Computing K-NNs for Queries\n");
		sprintf(line,"Computing K-NNs for Queries\n");
		MPI_File_write(logfile, line, strlen(line), MPI_CHAR, MPI_STATUS_IGNORE);
	}
	/**
	 * C1NodeID is the closer child, and C2NodeID is the other child
	 */
	int C1NodeID,C2NodeID;

	for (int i=0; i<world_size; ++i){
		/**
		 * To improve the performance, multi-threading using OpenMP is implemented here
		 */
        #pragma omp parallel for private(C1NodeID,C2NodeID)
		for (int j=0; j<receivingCountsMatrix[i]; ++j){

			int tmpReceivingHeapSize=receivingHeapSize[i][j];
			double rPrimeValue=receivingHeapArrayDistances[i][j*KNNCounts];

			double * receivingHeapArrayDistances2DCopy=new double[KNNCounts];
			int * receivingHeapArray2DCopy=new int[KNNCounts];
			/**
			 * To improve the performance, 1D arrays receivingHeapArray2DCopy and receivingHeapArrayDistances2DCopy are used here
			 */
			for (int k=0; k<KNNCounts; ++k){
				receivingHeapArray2DCopy[k]=receivingHeapArray[i][j*KNNCounts+k];
				receivingHeapArrayDistances2DCopy[k]=receivingHeapArrayDistances[i][j*KNNCounts+k];
			}

			int tmpIsHeapChanged=0;
			stack<pair<int,double>> globalStack;
			globalStack.push(make_pair(0,0));

			while (!globalStack.empty()){
				pair<int,double> topPairinStack=globalStack.top();
				int nodeID=topPairinStack.first;
				double dValue=topPairinStack.second;
				globalStack.pop();
				int nodesLayer0=int(log2(nodeID+1));
				int indexMaxVarDim=VectorGlobalSqrtSum[nodesLayer0+nodesLayer].second;

				if (isBucket[nodeID] == 1) {
					for (int kk=0; kk<localNodeDataIndex2[nodeID].size(); ++kk){

						if (i==world_rank) {
							int index=receivingIndices[i][j];
							if (nodeIndexofaPoint2[index]==nodeID) break;
						}

						int index= localNodeDataIndex2[nodeID][kk];
						double distance=computeDistance2(index,i,j*featureCounts,mappedData2,receivingPointCoordinates,featureCounts);

						if (distance < rPrimeValue){
							if (tmpReceivingHeapSize < KNNCounts){
								receivingHeapArray2DCopy[tmpReceivingHeapSize]=localIndexConvertor[index];
								receivingHeapArrayDistances2DCopy[tmpReceivingHeapSize]=distance;
								++tmpReceivingHeapSize;
								tmpIsHeapChanged=1;
								if(tmpReceivingHeapSize==KNNCounts) {
									Build_Max_Heap2(KNNCounts,receivingHeapArrayDistances2DCopy,receivingHeapArray2DCopy);
									rPrimeValue=receivingHeapArrayDistances2DCopy[0];
								}
							}
							else if (distance < receivingHeapArrayDistances2DCopy[0]){
								receivingHeapArrayDistances2DCopy[0]=distance;
								receivingHeapArray2DCopy[0]=localIndexConvertor[index];
								Max_Heapify2(0,receivingHeapArrayDistances2DCopy,receivingHeapArray2DCopy,KNNCounts);
								tmpIsHeapChanged=1;
								rPrimeValue=receivingHeapArrayDistances2DCopy[0];
							}
						}
					}
				}
				else {
					if (dValue < rPrimeValue){
						double dPrime= receivingPointCoordinates[i][j*featureCounts+indexMaxVarDim] - localMedianNodeData[nodeID];
						if (dPrime < 0) {
							C1NodeID=2*nodeID+1;
							C2NodeID=2*nodeID+2;
						}
						else{
							C1NodeID=2*nodeID+2;
							C2NodeID=2*nodeID+1;
						}

						dPrime=sqrt(dValue*dValue+dPrime*dPrime);
						if (dPrime < rPrimeValue) {
							if (C2NodeID <= localNodeDataIndex2.size()) {
								globalStack.push(make_pair(C2NodeID,dPrime));
							}
						}

						if (C1NodeID <= localNodeDataIndex2.size()) {
							globalStack.push(make_pair(C1NodeID,dValue));
						}
					}
				}
			}

			if (tmpIsHeapChanged==1) {
				for (int k=0; k<KNNCounts; ++k){
					receivingHeapArray[i][j*KNNCounts+k]=receivingHeapArray2DCopy[k];
					receivingHeapArrayDistances[i][j*KNNCounts+k]=receivingHeapArrayDistances2DCopy[k];
				}
			}
		}
	}
	/**
	 * Now, Send the newly computed K-NNs from the above (Algorithm 1) to the original processor contained it
	 */
	if (world_rank==0) {
		printf("Sending the Outputs of Query Computations Back to the Original Node\n");
		sprintf(line,"Sending the Outputs of Query Computations Back to the Original Node\n");
		MPI_File_write(logfile, line, strlen(line), MPI_CHAR, MPI_STATUS_IGNORE);
	}
	int sendbuffer4return[TotalReceiveCounts*KNNCounts];
	double sendbuffer6return[TotalReceiveCounts*KNNCounts];

	if (world_size != 1){
		int indexreturn=0;
		for (int i=0; i<world_size; ++i){
			for (int j=0; j<receivingCountsMatrix[i]; ++j){
				for (int k=0; k<KNNCounts; ++k){
					sendbuffer4return[indexreturn]=receivingHeapArray[i][j*KNNCounts+k];
					sendbuffer6return[indexreturn]=receivingHeapArrayDistances[i][j*KNNCounts+k];
					++indexreturn;
				}
			}
		}
	}

	int displreturn[world_size];
	displreturn[0]=0;
	for (int i=1; i<world_size; ++i){displreturn[i]= displreturn[i-1]+receivingCountsMatrix[i-1]*KNNCounts;}
	int sendCounts[world_size];
	for (int i=0; i<world_size; ++i){sendCounts[i]= receivingCountsMatrix[i]*KNNCounts;}
	int *originalNodereceivingHeapArray[world_size];
	double *originalNodereceivingHeapArrayDistances[world_size];

	if (world_size != 1){
		for (int i=0; i<world_size; ++i){
			originalNodereceivingHeapArray[i]=new int[bufferCounts2[i]];
			originalNodereceivingHeapArrayDistances[i]=new double[bufferCounts2[i]];
			MPI_Scatterv (&sendbuffer4return,sendCounts,displreturn,MPI_INT,&originalNodereceivingHeapArray[i][0],bufferCounts2[i],MPI_INT,i,MPI_COMM_WORLD);
			MPI_Scatterv (&sendbuffer6return,sendCounts,displreturn,MPI_DOUBLE,&originalNodereceivingHeapArrayDistances[i][0],bufferCounts2[i],MPI_DOUBLE,i,MPI_COMM_WORLD);
		}
	}
	else {
		originalNodereceivingHeapArray[0]=new int[cnts*KNNCounts];
		originalNodereceivingHeapArrayDistances[0]=new double[cnts*KNNCounts];

		for (int i=0; i<cnts*KNNCounts; ++i){
			originalNodereceivingHeapArray[0][i]=receivingHeapArray[0][i];
			originalNodereceivingHeapArrayDistances[0][i]=receivingHeapArrayDistances[0][i];
		}
	}
	/**
	 * Now, organize and sort the K-NNs for each given point either
	 * received from the neighboring processors or was initially computed from the points within the same bucket
	 * after sorting, choose only the desired number (KNNCounts) of K-NNs with the shortest distance
	 */
	if (world_rank==0) {
		printf("Preparing the Final Outputs\n");
		sprintf(line,"Preparing the Final Outputs\n");
		MPI_File_write(logfile, line, strlen(line), MPI_CHAR, MPI_STATUS_IGNORE);
	}

	ofstream outputFileIndex,outputFileDistance;
	string filename1= "KNN_Indices_"+to_string(world_rank)+".csv";
	outputFileIndex.open(filename1);
	string filename2= "KNN_Distances_"+to_string(world_rank)+".csv";
	outputFileDistance.open(filename2);

	for (int i=0; i<cnts; ++i){
		/**
		 * Set container removes the duplicates and sort data accroding to their distance
		 */
		set <pair<double,int>> setContainer;
		int pointID=localIndexConvertor[i];
		/**
		 * Insert into Set container the K-NNs initially computed from the points within the same bucket
		 */
		for (int j=0; j<KNNCounts; ++j){
			if (KNNIDsinBuckets[i][j] != -1) setContainer.insert(make_pair(KNNDistanceinBuckets[i][j],KNNIDsinBuckets[i][j]));
		}
		/**
		 * Insert into Set container the K-NNs computed from the querying in the same processor
		 */
		for (int k=0; k<KNNCounts; ++k){
			setContainer.insert(make_pair(originalNodereceivingHeapArrayDistances[world_rank][i*KNNCounts+k],originalNodereceivingHeapArray[world_rank][i*KNNCounts+k]));
		}
		/**
		 * Insert into Set container the K-NNs computed from the querying of the other neighboring processors
		 */
		for (int j=0; j<world_size-1; ++j){
			int neighborID=NeighboringNodes[i][j];
			if (neighborID == -1) continue;

			vector<int>::iterator it = std::find(ScatterVlocalNodeDataIndex[neighborID].begin(), ScatterVlocalNodeDataIndex[neighborID].end(), i);
			int index = std::distance(ScatterVlocalNodeDataIndex[neighborID].begin(), it);

			for (int k=0; k<KNNCounts; ++k){
				setContainer.insert(make_pair(originalNodereceivingHeapArrayDistances[neighborID][index*KNNCounts+k],originalNodereceivingHeapArray[neighborID][index*KNNCounts+k]));
			}
		}
		/**
		 * Output the results as sorted in the Set container
		 */
		set <pair<double,int>>::iterator pairIt;
		pairIt=setContainer.begin();
		int outputCounter=0;
		for (int ii=0; ii<KNNCounts*2; ++ii){
			if (outputCounter==KNNCounts) break;

			if ((*pairIt).second==-1) {pairIt++; continue;}
			if (outputCounter == 0) outputFileIndex<<pointID<<",";

			if (outputCounter != KNNCounts-1) { outputFileIndex<<(*pairIt).second<<",";
			} else {outputFileIndex<<(*pairIt).second<<endl;}

			if (outputCounter == 0) outputFileDistance<<pointID<<",";

			if (outputCounter != KNNCounts-1) { outputFileDistance<<(*pairIt).first<<",";
			} else {outputFileDistance<<(*pairIt).first<<endl;}
			pairIt++;
			outputCounter++;
		}
	} //loop cnts

	delete[] receivingHeapArray[world_size];
	delete[] receivingHeapArrayDistances[world_size];

	outputFileIndex.close();
	outputFileDistance.close();
	/**
	 * All procesors neeed to stop here until master processor returns
	 */
	MPI_Barrier(MPI_COMM_WORLD);
	/**
	 * Concatenate the Outputs from various processors into a single file and remove the extra output files
	 */

	if (world_rank==0) {
		string cmd4= string("cat KNN_Indices_*.csv > KNN_Indices.csv");
		int returnValue=system(cmd4.c_str());
		string cmd5= string("cat KNN_Distances_*.csv > KNN_Distances.csv");
		returnValue=system(cmd5.c_str());

		string cmd6= string("rm KNN_Indices_*");
		returnValue=system(cmd6.c_str());
		string cmd7= string("rm KNN_Distances_*");
		returnValue=system(cmd7.c_str());
	}

	MPI_File_close(&logfile);
	MPI_Finalize();
	return 0;
}


#include <opencv2/opencv.hpp>
#include <string>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace cv;

// this program shows how to use optical flow

string file_1 = "../1.png";  // first image
string file_2 = "../2.png";  // second image

// TODO implement this funciton
/**
 * single level optical flow
 * @param [in] img1 the first image
 * @param [in] img2 the second image
 * @param [in] kp1 keypoints in img1
 * @param [in|out] kp2 keypoints in img2, if empty, use initial guess in kp1
 * @param [out] success true if a keypoint is tracked successfully
 * @param [in] inverse use inverse formulation?
 */
void OpticalFlowSingleLevel(
        const Mat &img1,
        const Mat &img2,
        const vector<KeyPoint> &kp1,
        vector<KeyPoint> &kp2,
        vector<bool> &success,
        bool inverse = false
);

// TODO implement this funciton
/**
 * multi level optical flow, scale of pyramid is set to 2 by default
 * the image pyramid will be create inside the function
 * @param [in] img1 the first pyramid
 * @param [in] img2 the second pyramid
 * @param [in] kp1 keypoints in img1
 * @param [out] kp2 keypoints in img2
 * @param [out] success true if a keypoint is tracked successfully
 * @param [in] inverse set true to enable inverse formulation
 */
void OpticalFlowMultiLevel(
        const Mat &img1,
        const Mat &img2,
        const vector<KeyPoint> &kp1,
        vector<KeyPoint> &kp2,
        vector<bool> &success,
        bool inverse = false
);

/**
 * get a gray scale value from reference image (bi-linear interpolated)
 * @param img
 * @param x
 * @param y
 * @return
 */
inline float GetPixelValue(const cv::Mat &img, float x, float y)
{
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    return float(
            (1 - xx) * (1 - yy) * data[0] +
            xx * (1 - yy) * data[1] +
            (1 - xx) * yy * data[img.step] +
            xx * yy * data[img.step + 1]
    );
}

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

    // images, note they are CV_8UC1, not CV_8UC3
    Mat img1 = imread(file_1, 0);
    if(img1.data == NULL )
    {
        perror("please check your path");
        return 0;
    }
    Mat img2 = imread(file_2, 0);
    if(img2.data == NULL )
    {
        perror("please check your path");
        return 0;
    }

    // key points, using GFTT here.
    vector<KeyPoint> kp1;
    Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20); // maximum 500 keypoints
    detector->detect(img1, kp1); //匹配点

    // now lets track these key points in the second image
    // first use single level LK in the validation picture
    vector<KeyPoint> kp2_single;
    vector<bool> success_single;
    OpticalFlowSingleLevel(img1, img2, kp1, kp2_single, success_single);

    // then test multi-level LK
    vector<KeyPoint> kp2_multi;
    vector<bool> success_multi;
    OpticalFlowMultiLevel(img1, img2, kp1, kp2_multi, success_multi);

    // use opencv's flow for validation
    vector<Point2f> pt1, pt2;
    for (auto &kp: kp1) pt1.push_back(kp.pt);
    vector<uchar> status;
    vector<float> error;
    cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error, cv::Size(8, 8));

    // plot the differences of those functions
    Mat img2_single;
    cv::cvtColor(img2, img2_single, CV_GRAY2BGR);
    for (int i = 0; i < kp2_single.size(); i++)
    {
        if (success_single[i])
        {
            cv::circle(img2_single, kp2_single[i].pt, 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_single, kp1[i].pt, kp2_single[i].pt, cv::Scalar(0, 250, 0));
        }
    }

    Mat img2_multi;
    cv::cvtColor(img2, img2_multi, CV_GRAY2BGR);
    for (int i = 0; i < kp2_multi.size(); i++)
    {
        if (success_multi[i])
        {
            cv::circle(img2_multi, kp2_multi[i].pt, 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_multi, kp1[i].pt, kp2_multi[i].pt, cv::Scalar(0, 250, 0));
        }
    }

    Mat img2_CV;
    cv::cvtColor(img2, img2_CV, CV_GRAY2BGR);
    for (int i = 0; i < pt2.size(); i++)
    {
        if (status[i])
        {
            cv::circle(img2_CV, pt2[i], 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_CV, pt1[i], pt2[i], cv::Scalar(0, 250, 0));
        }
    }

    cv::imshow("tracked single level", img2_single);
    cv::imshow("tracked multi level", img2_multi);
    cv::imshow("tracked by opencv", img2_CV);
    cv::waitKey(0);

    return 0;
}

void OpticalFlowSingleLevel(
        const Mat &img1,
        const Mat &img2,
        const vector<KeyPoint> &kp1, //特征点
        vector<KeyPoint> &kp2,
        vector<bool> &success,
        bool inverse)
{

    // parameters
    int half_patch_size = 4;
    int iterations = 10;
    bool have_initial = !kp2.empty();

    for (size_t i = 0; i < kp1.size(); i++) //取特征点
    {
        auto kp = kp1[i];
        double dx = 0, dy = 0; // dx,dy need to be estimated
        if (have_initial)
        {
            dx = kp2[i].pt.x - kp.pt.x;
            dy = kp2[i].pt.y - kp.pt.y;
        }

        double cost = 0, lastCost = 0;
        bool succ = true; // indicate if this point succeeded

        // Gauss-Newton iterations
        for (int iter = 0; iter < iterations; iter++)
        {
            Eigen::Matrix2d H = Eigen::Matrix2d::Zero();
            Eigen::Vector2d b = Eigen::Vector2d::Zero();
            cost = 0;

            if (kp.pt.x + dx <= half_patch_size || kp.pt.x + dx >= img1.cols - half_patch_size ||
                kp.pt.y + dy <= half_patch_size || kp.pt.y + dy >= img1.rows - half_patch_size)
            {   // go outside
                succ = false;
                break;
            }

            // compute cost and jacobian
            for (int x = -half_patch_size; x < half_patch_size; x++)// 遍历图像块
            {
                for (int y = -half_patch_size; y < half_patch_size; y++)// 遍历图像块
                {

                    // TODO START YOUR CODE HERE (~8 lines)
                    double X = kp.pt.x +x;
                    double Y = kp.pt.y +y;
                    bool have_computed_J = false;
                    error =   -(GetPixelValue(img1,X,Y) - GetPixelValue(img2,X+dx,Y+dy)) ;
//                    error = GetPixelValue(img1,X,Y) - GetPixelValue(img2,X+dx,Y+dy);
                    Eigen::Vector2d J;  // Jacobian
                    if (inverse == false)
                    {
                        // Forward Jacobian
                        J[0] = ( GetPixelValue(img2,X+dx+1,Y+dy) - GetPixelValue(img2,X+dx-1,Y+dy) )/2;
                        J[1] = ( GetPixelValue(img2,X+dx,Y+dy+1) - GetPixelValue(img2,X+dx,Y+dy-1) )/2;

                    }
                    else
                    {
                        // Inverse Jacobian
                        // NOTE this J does not change when dx, dy is updated, so we can store it and only compute error

                        if(have_computed_J == false)
                        {
                            J[0] = (GetPixelValue(img1, X + 1, Y) - GetPixelValue(img1, X - 1, Y)) / 2;
                            J[1] = (GetPixelValue(img1, X, Y + 1) - GetPixelValue(img1, X, Y - 1)) / 2;
                            have_computed_J = true;
                        }

                    // compute H, b and set cost;
                    H;

                    b;

                    cost;

                    // TODO END YOUR CODE HERE

                }

            }

            // compute update
            // TODO START YOUR CODE HERE (~1 lines)
            Eigen::Vector2d update;

            // TODO END YOUR CODE HERE

            if (isnan(update[0]))
            {
                // sometimes occurred when we have a black or white patch and H is irreversible
                cout << "update is nan" << endl;
                succ = false;
                break;
            }
            if (iter > 0 && cost > lastCost)
            {
                cout << "cost increased: " << cost << ", " << lastCost << endl;
                break;
            }

            // update dx, dy
            dx += update[0];
            dy += update[1];
            lastCost = cost;
            succ = true;
        }

        success.push_back(succ);

        // set kp2
        if (have_initial)
        {
            kp2[i].pt = kp.pt + Point2f(dx, dy);
        } else
        {
            KeyPoint tracked = kp;
            tracked.pt += cv::Point2f(dx, dy);
            kp2.push_back(tracked);
        6
    }
}

void OpticalFlowMultiLevel(
        const Mat &img1,
        const Mat &img2,
        const vector<KeyPoint> &kp1,
        vector<KeyPoint> &kp2,
        vector<bool> &success,
        bool inverse)
{

    // parameters
    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = {1.0, 0.5, 0.25, 0.125};

    // create pyramids
    vector<Mat> pyr1, pyr2; // image pyramids
    // TODO START YOUR CODE HERE (~8 lines)
    for (int i = 0; i < pyramids; i++)
    {

    }
    // TODO END YOUR CODE HERE

    // coarse-to-fine LK tracking in pyramids
    // TODO START YOUR CODE HERE

    // TODO END YOUR CODE HERE
    // don't forget to set the results into kp2
}


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#include <ctime>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

#include "rttbBioModel.h"
#include "rttbBioModelScatterPlots.h"
#include "rttbInvalidParameterException.h"

namespace rttb
{
	namespace models
	{

		/* Initiate Random Number generator with current time */
		boost::random::mt19937 rng(static_cast<unsigned>(time(nullptr)));
		/* Generate random number between 0 and 1 */
		boost::random::uniform_01<boost::mt19937> uniDist(rng);

		ScatterPlotType getScatterPlotVary1Parameter(BioModel& aModel, int aParamId,
		        BioModelParamType aMean, BioModelParamType aVariance, DoseTypeGy aNormalisationDose,
		        int numberOfPoints,
		        DoseTypeGy aMinDose, DoseTypeGy aMaxDose)
		{
			ScatterPlotType scatterPlotData;

			if (aVariance == 0)
			{
				//set to small positive value to avoid negative infinity!
				aVariance = 1e-30;
			}

			if (aMaxDose <= aMinDose)
			{
				throw core::InvalidParameterException("Parameter invalid: aMaxDose must be > aMinDose!");
			}

			if (aNormalisationDose <= 0)
			{
				throw core::InvalidParameterException("Parameter invalid: aNormalisationDose must be > 0!");
			}

			/* Choose Normal Distribution */
			boost::random::normal_distribution<double> gaussian_dist(0, aVariance);

			/* Create a Gaussian Random Number generator
			*  by binding with previously defined
			*  normal distribution object
			*/
			boost::random::variate_generator<boost::mt19937&, boost::normal_distribution<double> > generator(
			    rng, gaussian_dist);

			int i = 0;

			while (i < numberOfPoints)
			{
				double paramValue, probability;
				double randomValue = generator();
				paramValue = randomValue + aMean;
				probability = normal_pdf(randomValue, aVariance);

				if (probability > 1)
				{
					probability = 1;
				}

				//randomly select a dose between aMinDose and aMaxDose
				double dose = uniDist() * (aMaxDose - aMinDose) + aMinDose;

				if (probability > 0)
				{
					try
					{
						aModel.setParameterByID(aParamId, paramValue);
						aModel.init(dose / aNormalisationDose);
						double value = aModel.getValue();
						std::pair<double, double> modelProbPair = std::make_pair(value, probability);
						scatterPlotData.insert(std::pair<double, std::pair<double, double> >(dose, modelProbPair));
						i++;
					}
					catch (core::InvalidParameterException& /*e*/)
					{
						//repeat evaluation to guarantee the correct number of scatter values
						continue;
					}
				}
			}

			return scatterPlotData;
		}

		double normal_pdf(double aValue, double aVariance)
		{
			static const double inv_sqrt_2pi = 0.3989422804014327;
			double a = (aValue) / aVariance;

			return inv_sqrt_2pi / aVariance * std::exp(-0.5f * a * a);
		}

		ScatterPlotType getScatterPlotVaryParameters(BioModel& aModel,
		        std::vector<int> aParamIdVec, BioModel::ParamVectorType aMeanVec,
		        BioModel::ParamVectorType aVarianceVec,
		        DoseTypeGy aNormalisationDose, int numberOfPoints, DoseTypeGy aMinDose, DoseTypeGy aMaxDose)
		{

			ScatterPlotType scatterPlotData;

			if (aMaxDose <= aMinDose)
			{
				throw core::InvalidParameterException("Parameter invalid: aMaxDose must be > aMinDose!");
			}

			if (aNormalisationDose <= 0)
			{
				throw core::InvalidParameterException("Parameter invalid: aNormalisationDose must be > 0!");
			}

			//all input vectors need to have the same size
			if (((aVarianceVec.size() != aMeanVec.size()) || (aVarianceVec.size() != aParamIdVec.size())))
			{
				throw core::InvalidParameterException("Parameter vectors have different sizes!");
			}

			for (double & v : aVarianceVec)
			{
				//set to small positive value to avoid negative infinity!
				if (v == 0)
				{
					v = 1e-30;
				}
			}

			double paramValue;

			// vary all parameters for each scattered point
			int i = 0;

			while (i < numberOfPoints)
			{
				double probability = 1;

				for (GridIndexType j = 0; j < aParamIdVec.size(); j++)
				{
					/* Choose Normal Distribution */
					boost::random::normal_distribution<double> gaussian_dist(0, aVarianceVec.at(j));

					/* Create a Gaussian Random Number generator
					*  by binding with previously defined
					*  normal distribution object
					*/
					boost::random::variate_generator<boost::mt19937&, boost::normal_distribution<double> > generator(
					    rng, gaussian_dist);

					double randomValue = generator();
					paramValue = randomValue + aMeanVec.at(j);

					if (aVarianceVec.at(j) != 0)
					{

						/* calculate combined probability */
						probability = probability * normal_pdf(randomValue, aVarianceVec.at(j));

					}
					else
					{
						throw core::InvalidParameterException("Parameter invalid: Variance should not be 0!");
					}

					aModel.setParameterByID(aParamIdVec.at(j), paramValue);
				}

				//randomly select a dose between aMinDose and aMaxDose
				double dose = uniDist() * (aMaxDose - aMinDose) + aMinDose;

				if (probability > 0)
				{
					try
					{
						aModel.init(dose / aNormalisationDose);
						double value = aModel.getValue();
						std::pair<double, double> modelProbPair = std::make_pair(value, probability);
						scatterPlotData.insert(std::pair<double, std::pair<double, double> >(dose, modelProbPair));
						i++;
					}
					catch (core::InvalidParameterException& /*e*/)
					{
						//repeat evaluation to guarantee the correct number of scatter values
						continue;
					}
				}
			}

			return scatterPlotData;
		}

	}//end namespace models
}//end namespace rttb


#include <blitz/array.h>
#include <fstream>

using namespace blitz;

void makeLogo();

int main()
{
    makeLogo();
    return 0;
}

void setInitialConditions(Array<float,2>& c, Array<float,2>& P1,
    Array<float,2>& P2, Array<float,2>& P3, int N, int M);

void snapshot(const Array<float,2>& P, const Array<float,2>& c);

void makeLogo()
{
    const int N = 300, M = 900;
    int niters = 3000;

    Array<float,2> P1, P2, P3, c;
    allocateArrays(shape(N,M), P1, P2, P3, c);
    Range I(1,N-2), J(1,M-2);

    setInitialConditions(c, P1, P2, P3, N, M);

    for (int iter=0; iter < niters; ++iter)
    {
        P3(I,J) = (2-4*c(I,J)) * P2(I,J)
          + c(I,J)*(P2(I-1,J) + P2(I+1,J) + P2(I,J-1) + P2(I,J+1))
          - P1(I,J);

        cycleArrays(P1,P2,P3);

        snapshot(P2, c);
    }

}

void setInitialConditions(Array<float,2>& c, Array<float,2>& P1,
    Array<float,2>& P2, Array<float,2>& P3, int N, int M)
{
    // Set the velocity field
    c = 0.3;

    ifstream ifs("blitz3.pgm");
    char tmpBuf[128];
    int pixel;
    ifs.getline(tmpBuf, 128);
    ifs.getline(tmpBuf, 128);
    ifs.getline(tmpBuf, 128);

    for (int pi=0; pi < 199; ++pi)
    {
        for (int pj=0; pj < 798; ++pj)
        {
            ifs >> pixel;
            if (pixel)
                c(pi+50,pj+56) = 0.02;
        }
    }

    // Initial pressure distribution: gaussian pulse
    using namespace blitz::tensor;
    int cr = N/6-1;
//    int cc = 7.0*M/8.0-1;
    float s2 = 64.0 * 9.0 / pow2(N/2.0);
    P1 = 0.0;
//    P2 = exp(-(pow2(i-cr)+pow2(j-cc)) * s2);
    P2 = exp(-(pow2(i-cr)) * s2);

    P3 = 0.0;
}

void snapshot(const Array<float,2>& P, const Array<float,2>& c)
{
    static int count = 0, snapshotNum = 0;
    if (++count < 50)
        return;

    count = 0;
    ++snapshotNum;
    char filename[128];
    sprintf(filename, "logo%03d.m", snapshotNum);

    ofstream ofs(filename);
    int N = P.length(firstDim);
    int M = P.length(secondDim);

    float Pmin = -0.6;
    float PScale = 1.0/1.2;
    float VScale = 1.0;

    ofs << "P" << snapshotNum << " = [ ";
    for (int i=0; i < N; ++i)
    {
        for (int j=0; j < M; ++j)
        {
            float value1 = (P(i,j)-Pmin)*PScale;
            float value2 = c(i,j)*VScale;
            int r1 = value1 * 4096;
            int r2 = value2 * 4096;
            ofs << r1 << " " << r2 << "    ";
        }
        if (i < N-1)
            ofs << ";" << endl;
    }
    ofs << "];" << endl;
}