#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;
}


// Copyright (C) 2018-2020 Chris Richardson (chris@bpi.cam.ac.uk)
// SPDX-License-Identifier:    MIT

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <chrono>
#include <iostream>
#include <memory>
#include <mpi.h>

#include "CreateA.h"
#include <spmv/L2GMap.h>
#include <spmv/read_petsc.h>

void restrict_main()
{
  int mpi_rank;
  MPI_Comm_rank(MPI_COMM_WORLD, &mpi_rank);
  int mpi_size;
  MPI_Comm_size(MPI_COMM_WORLD, &mpi_size);

  // Keep list of timings
  std::map<std::string, std::chrono::duration<double>> timings;

  auto timer_start = std::chrono::system_clock::now();
  // Read in a PETSc binary format matrix
  auto R = spmv::read_petsc_binary_matrix(MPI_COMM_WORLD, "R4.dat");
  auto q = spmv::read_petsc_binary_vector(MPI_COMM_WORLD, "b4.dat");

  // Get local and global sizes
  std::int64_t M = R.rows();
  auto l2g = R.col_map();
  std::int64_t N = l2g->global_size();

  std::cout << "Vector = " << q.size() << " " << M << "\n";

  auto timer_end = std::chrono::system_clock::now();
  timings["0.PetscRead"] += (timer_end - timer_start);

  timer_start = std::chrono::system_clock::now();

  if (mpi_rank == 0)
    std::cout << "Creating vector of size " << N << "\n";

  // Vector in "column space" with extra space for ghosts at end
  Eigen::VectorXd psp(l2g->local_size() + l2g->num_ghosts());

  timer_end = std::chrono::system_clock::now();
  timings["1.VecCreate"] += (timer_end - timer_start);

  // Apply matrix
  if (mpi_rank == 0)
    std::cout << "Applying matrix\n";

  double pnorm_sum, qnorm_sum;
  for (int i = 0; i < 10; ++i) {
    // Restrict
    timer_start = std::chrono::system_clock::now();
    psp = R.transpmult(q);

    timer_end = std::chrono::system_clock::now();
    timings["3.SpMV"] += (timer_end - timer_start);

    timer_start = std::chrono::system_clock::now();
    l2g->reverse_update(psp.data());
    timer_end = std::chrono::system_clock::now();
    timings["2.SparseUpdate"] += (timer_end - timer_start);

    Eigen::Map<Eigen::VectorXd> p(psp.data(), l2g->local_size());
    double pnorm = p.squaredNorm();
    MPI_Allreduce(&pnorm, &pnorm_sum, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);

    // Prolongate
    timer_start = std::chrono::system_clock::now();
    l2g->update(psp.data());
    timer_end = std::chrono::system_clock::now();
    timings["2.SparseUpdate"] += (timer_end - timer_start);

    timer_start = std::chrono::system_clock::now();
    q = R.mult(psp);

    timer_end = std::chrono::system_clock::now();
    timings["3.SpMV"] += (timer_end - timer_start);

    double qnorm = q.squaredNorm();
    MPI_Allreduce(&qnorm, &qnorm_sum, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
  }

  if (mpi_rank == 0)
    std::cout << "\nTimings (" << mpi_size
              << ")\n----------------------------\n";

  std::chrono::duration<double> total_time
      = std::chrono::duration<double>::zero();
  for (auto q : timings)
    total_time += q.second;
  timings["Total"] = total_time;

  for (auto q : timings) {
    double q_local = q.second.count(), q_max, q_min;
    MPI_Reduce(&q_local, &q_max, 1, MPI_DOUBLE, MPI_MAX, 0, MPI_COMM_WORLD);
    MPI_Reduce(&q_local, &q_min, 1, MPI_DOUBLE, MPI_MIN, 0, MPI_COMM_WORLD);

    if (mpi_rank == 0) {
      std::string pad(16 - q.first.size(), ' ');
      std::cout << "[" << q.first << "]" << pad << q_min << '\t' << q_max
                << "\n";
    }
  }

  if (mpi_rank == 0) {
    std::cout << "----------------------------\n";
    std::cout << "norm q = " << qnorm_sum << "\n";
    std::cout << "norm p = " << pnorm_sum << "\n";
  }
}
//-----------------------------------------------------------------------------
int main(int argc, char** argv)
{
#ifdef _OPENMP
  int provided;
  MPI_Init_thread(&argc, &argv, MPI_THREAD_FUNNELED, &provided);
  if (provided < MPI_THREAD_FUNNELED) {
    std::cout << "The threading support level is lesser than required"
              << std::endl;
    MPI_Abort(MPI_COMM_WORLD, EXIT_FAILURE);
  }
#else
  MPI_Init(&argc, &argv);
#endif

  restrict_main();

  MPI_Finalize();
  return 0;
}


/**
 * @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 "asymmetric_normal.hpp"
#include "normal.hpp"

#include <gtest/gtest.h>

#include <boost/random/mersenne_twister.hpp>

using namespace MultidimensionalArray;
using namespace ProbabilityDistributions;

TEST(AsymmetricNormalTest, Likelihood) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  AsymmetricNormal<double> dist1(0.25, 0, 1);
  Normal<double> dist2(0, 1);
  Array<double> samples;
  dist1.sample(samples, n_samples, rng);

  auto indexes = Distribution<double>::sort_data(samples);

  double likelihood11 = dist1.log_likelihood(samples);
  double likelihood12 = dist2.log_likelihood(samples);

  EXPECT_GE(likelihood11, likelihood12);

  dist1.MLE(samples, indexes);
  dist2.MLE(samples, indexes);

  double likelihood21 = dist1.log_likelihood(samples);
  double likelihood22 = dist2.log_likelihood(samples);

  EXPECT_GE(likelihood22, likelihood12);
  EXPECT_GE(likelihood21, likelihood11);
  EXPECT_GT(likelihood21, likelihood22);

  EXPECT_LT(0, dist1.get_sigma());
  EXPECT_LT(0, dist1.get_p());
  EXPECT_GT(1, dist1.get_p());
}

TEST(AsymmetricNormalTest, LikelihoodConsistency) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  AsymmetricNormal<double> dist1(0.5, 0, 1);
  dist1.fix_p(true);
  Normal<double> dist2(0, 1);
  Array<double> samples;
  dist1.sample(samples, n_samples, rng);

  auto indexes = Distribution<double>::sort_data(samples);

  double likelihood11 = dist1.log_likelihood(samples);
  double likelihood12 = dist2.log_likelihood(samples);

  EXPECT_DOUBLE_EQ(likelihood12, likelihood11);

  dist1.MLE(samples, indexes);
  dist2.MLE(samples, indexes);

  double likelihood21 = dist1.log_likelihood(samples);
  double likelihood22 = dist2.log_likelihood(samples);

  EXPECT_DOUBLE_EQ(likelihood22, likelihood21);

  EXPECT_NEAR(dist1.get_mu(), dist2.get_mu(), 1e-8);
  EXPECT_DOUBLE_EQ(dist1.get_sigma(), dist2.get_sigma());
}

TEST(AsymmetricNormalTest, MLE) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  AsymmetricNormal<double> dist(0.5, 0, 1);
  Array<double> samples;
  dist.sample(samples, n_samples, rng);
  auto indexes = Distribution<double>::sort_data(samples);
  dist.MLE(samples, indexes);

  double p = dist.get_p(), mu = dist.get_mu(), sigma = dist.get_sigma();
  double eps = 1e-4;
  double ll = dist.log_likelihood(samples);

  dist.set_p(p + eps);
  EXPECT_GE(ll, dist.log_likelihood(samples));
  dist.set_p(p - eps);
  EXPECT_GE(ll, dist.log_likelihood(samples));
  dist.set_p(p);

  dist.set_mu(mu + eps);
  EXPECT_GE(ll, dist.log_likelihood(samples));
  dist.set_mu(mu - eps);
  EXPECT_GE(ll, dist.log_likelihood(samples));
  dist.set_mu(mu);

  dist.set_sigma(sigma + eps);
  EXPECT_GE(ll, dist.log_likelihood(samples));
  dist.set_sigma(sigma - eps);
  EXPECT_GE(ll, dist.log_likelihood(samples));
  dist.set_sigma(sigma);
}

TEST(AsymmetricNormalTest, Samples) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  AsymmetricNormal<double> dist(0.5, 0, 1);
  Array<double> samples;
  dist.sample(samples, n_samples, rng);
  for (size_t i = 0; i < n_samples; i++) {
    EXPECT_LT(-5, samples(i,0));
    EXPECT_GT(5, samples(i,0));
  }
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    Rot3Q.cpp
 * @brief   Rotation (internal: quaternion representation*)
 * @author  Richard Roberts
 */

#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro

#ifdef GTSAM_USE_QUATERNIONS

#include <boost/math/constants/constants.hpp>
#include <gtsam/geometry/Rot3.h>

using namespace std;

namespace gtsam {

  static const Matrix I3 = eye(3);

  /* ************************************************************************* */
  Rot3::Rot3() : quaternion_(Quaternion::Identity()) {}

  /* ************************************************************************* */
  Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) :
      quaternion_((Eigen::Matrix3d() <<
          col1.x(), col2.x(), col3.x(),
          col1.y(), col2.y(), col3.y(),
          col1.z(), col2.z(), col3.z()).finished()) {}

  /* ************************************************************************* */
  Rot3::Rot3(double R11, double R12, double R13,
      double R21, double R22, double R23,
      double R31, double R32, double R33) :
        quaternion_((Eigen::Matrix3d() <<
            R11, R12, R13,
            R21, R22, R23,
            R31, R32, R33).finished()) {}

  /* ************************************************************************* */
  Rot3::Rot3(const Matrix3& R) :
      quaternion_(R) {}

  /* ************************************************************************* */
  Rot3::Rot3(const Matrix& R) :
      quaternion_(Matrix3(R)) {}

//  /* ************************************************************************* */
//   Rot3::Rot3(const Matrix3& R) :
//       quaternion_(R) {}

  /* ************************************************************************* */
  Rot3::Rot3(const Quaternion& q) : quaternion_(q) {}

  /* ************************************************************************* */
  Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); }

  /* ************************************************************************* */
  Rot3 Rot3::Ry(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); }

  /* ************************************************************************* */
  Rot3 Rot3::Rz(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); }

  /* ************************************************************************* */
  Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3(
      Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
      Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
      Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
  }

  /* ************************************************************************* */
  Rot3 Rot3::rodriguez(const Vector& w, double theta) {
    return Quaternion(Eigen::AngleAxisd(theta, w)); }

  /* ************************************************************************* */
  Rot3 Rot3::compose(const Rot3& R2,
  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
    if (H1) *H1 = R2.transpose();
    if (H2) *H2 = I3;
    return Rot3(quaternion_ * R2.quaternion_);
  }

  /* ************************************************************************* */
  Rot3 Rot3::operator*(const Rot3& R2) const {
    return Rot3(quaternion_ * R2.quaternion_);
  }

  /* ************************************************************************* */
  Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
    if (H1) *H1 = -matrix();
    return Rot3(quaternion_.inverse());
  }

  /* ************************************************************************* */
  Rot3 Rot3::between(const Rot3& R2,
  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
    if (H1) *H1 = -(R2.transpose()*matrix());
    if (H2) *H2 = I3;
    return between_default(*this, R2);
  }

  /* ************************************************************************* */
  Point3 Rot3::rotate(const Point3& p,
        boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2) const {
    Matrix R = matrix();
    if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
    if (H2) *H2 = R;
    Eigen::Vector3d r = R * p.vector();
    return Point3(r.x(), r.y(), r.z());
  }

  /* ************************************************************************* */
  // Log map at identity - return the canonical coordinates of this rotation
  Vector3 Rot3::Logmap(const Rot3& R) {
    Eigen::AngleAxisd angleAxis(R.quaternion_);
    if(angleAxis.angle() > M_PI)      // Important:  use the smallest possible
      angleAxis.angle() -= 2.0*M_PI;  // angle, e.g. no more than PI, to keep
    if(angleAxis.angle() < -M_PI)     // error continuous.
      angleAxis.angle() += 2.0*M_PI;
    return angleAxis.axis() * angleAxis.angle();
  }

  /* ************************************************************************* */
  Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
    return compose(Expmap(omega));
  }

  /* ************************************************************************* */
  Vector3 Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const {
    return Logmap(between(t2));
  }

  /* ************************************************************************* */
  Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();}

  /* ************************************************************************* */
  Matrix3 Rot3::transpose() const {return quaternion_.toRotationMatrix().transpose();}

  /* ************************************************************************* */
  Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); }

  /* ************************************************************************* */
  Point3 Rot3::r2() const { return Point3(quaternion_.toRotationMatrix().col(1)); }

  /* ************************************************************************* */
  Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); }

  /* ************************************************************************* */
  Quaternion Rot3::toQuaternion() const { return quaternion_; }

 /* ************************************************************************* */

} // namespace gtsam

#endif


#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
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[simplify_inserter
//` Simplify a linestring using an output iterator

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> P;
    typedef boost::geometry::model::linestring<P> L;

    L line;
    boost::geometry::read_wkt("linestring(1.1 1.1, 2.5 2.1, 3.1 3.1, 4.9 1.1, 3.1 1.9)", line);

    typedef boost::geometry::strategy::distance::projected_point<P, P> DS;
    typedef boost::geometry::strategy::simplify::douglas_peucker<P, DS> simplification;

    L simplified;
    boost::geometry::simplify_inserter(line, std::back_inserter(simplified), 0.5, simplification()); //std::ostream_iterator<P>(std::cout, "\n"), 0.5);//);
    //std::cout << simplified[0];
    //boost::geometry::simplify_inserter(line, std::ostream_iterator<P>(std::cout, "\n"), 0.5);//, simplification());

    std::ostream_iterator<P> out(std::cout, "\n");
    std::copy(simplified.begin(), simplified.end(), out);

    std::cout
        << "  original: " << boost::geometry::dsv(line) << std::endl
        << "simplified: " << boost::geometry::dsv(simplified) << std::endl;

    return 0;
}

//]

//[simplify_inserter_output
/*`
Output:
[pre
simplify_inserter: 16
simplify_inserter: 0.339837
]
*/
//]
/*
OUTPUT
POINT(1.1 1.1)  original: ((1.1, 1.1), (2.5, 2.1), (3.1, 3.1), (4.9, 1.1), (3.1, 1.9))
simplified: ((1.1, 1.1), (3.1, 3.1), (4.9, 1.1), (3.1, 1.9))
*/
/*
OUTPUT
POINT(1.1 1.1)  original: ((1.1, 1.1), (2.5, 2.1), (3.1, 3.1), (4.9, 1.1), (3.1, 1.9))
simplified: ((1.1, 1.1), (3.1, 3.1), (4.9, 1.1), (3.1, 1.9))
*/
/*
OUTPUT
POINT(1.1 1.1)  original: ((1.1, 1.1), (2.5, 2.1), (3.1, 3.1), (4.9, 1.1), (3.1, 1.9))
simplified: ((1.1, 1.1), (3.1, 3.1), (4.9, 1.1), (3.1, 1.9))
*/


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


#define CATCH_CONFIG_MAIN
#include "catch2/catch.hpp"
#include <Eigen/Dense>
#include <fstream>
#include <iostream>
#include <cassert>
#include "Network.h"

using Eigen::MatrixXd;

bool test_vs(){
	MatrixXd expected_result (5,1) ;
	expected_result << 24, -8, 20, -4, 1;
	std::ifstream net_file ("./test_net.txt");
    assert(net_file.is_open());
    Network net = Network();
    net.preprocess_netlist(net_file);
    net.parse_netlist(net_file);
    net.calculate();
    net_file.close();
    return expected_result.isApprox(net.get_result_matrix());
}

bool test_cs(){
	MatrixXd expected_result (3,1) ;
	expected_result << 8, -24, -11;
	std::ifstream net_file2 ("./test_net2.txt");
    assert(net_file2.is_open());
    Network net = Network();
    net.preprocess_netlist(net_file2);
    net.parse_netlist(net_file2);
    net.calculate();
    net_file2.close();
    return expected_result.isApprox(net.get_result_matrix());
}

TEST_CASE( "Circuit test" ) {
	REQUIRE(test_vs() == true);
	REQUIRE(test_cs() == true);
}


#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;
}


#include "linalg.h"
#include <cstring>

// this files requires Eigen.
// Linux:
//   sudo apt install libeigen3-dev
// Windows:
//   download it from http://eigen.tuxfamily.org/
//   unzip it in ./gmsh-api/eigen
//   be sure to load the environment
//   (the eigen folder should be added to INCLUDE)

#include <Eigen/Dense>

// print a vector<double> as "[v1, v2, v3, ...]"

std::ostream &operator<<(std::ostream &s, std::vector<double> const &v)
{
    s << '[';
    for (int i = 0; i < v.size(); ++i)
    {
        if (std::abs(v[i]) < 1e-10)
            s << "0";
        else
            s << v[i];
        if (i != v.size() - 1)
            s << ", ";
    }
    s << ']';
    return s;
}

// create and return a random matrix of dimension "dim"

std::vector<double> randomMatrix(int dim)
{
    // create a dim x dim mrandom matrix using Eigen
    Eigen::MatrixXd mat = Eigen::MatrixXd::Random(dim, dim);
    // copy its values into a std::vector
    std::vector<double> v(dim * dim);
    std::memcpy(&(v[0]), mat.data(), dim * dim * sizeof(double));
    return v;
}

// compute the inverse of a square matrix of size (dim,dim) given as a std::vector<double>
// return the inverse as a std::vector<double>

std::vector<double> inverse(std::vector<double> &M, int dim)
{
    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> mapA(&(M[0]), dim, dim);
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> B = mapA.inverse();
    std::vector<double> invM(dim * dim);
    std::memcpy(&(invM[0]), B.data(), dim * dim * sizeof(double));
    return invM;
}

// multiply 2 matrices given as std::vector<double>
// return the result as a std::vector<double>

std::vector<double> matmult(std::vector<double> &A, std::vector<double> &B, int dim)
{
    assert(A.size() == dim * dim);
    assert(B.size() == dim * dim);

    std::vector<double> C(dim * dim);
    for (int i = 0; i < dim; ++i)
        for (int j = 0; j < dim; ++j)
        {
            double v = 0;
            for (int k = 0; k < dim; ++k)
                v += A[i * dim + k] * B[k * dim + j];
            C[i * dim + j] = v;
        }
    return C;
}


#ifndef NOBODY_CAMERA_H_
#define NOBODY_CAMERA_H_

#include <Eigen/Dense>
#include <cmath>
#include <nobody/orthonormal_frame.hpp>

namespace nobody {

class Camera {
  using Vector3f = Eigen::Vector3f;

 public:
  Camera() = default;
  Camera(int width, int height, float fov)
      : screen_width_(width), screen_height_(height), field_of_view_(fov) {}

  const Vector3f& position() const { return frame_.origin(); }
  Vector3f direction() const { return -frame_.back(); }
  const Orthonormal_frame& frame() const { return frame_; }
  float field_of_view() const { return field_of_view_; }
  float vertical_field_of_view() const;
  float horizontal_field_of_view() const;
  float opengl_field_of_view() const;
  int screen_width() const { return screen_width_; }
  int screen_height() const { return screen_height_; }
  float pixel_size() const { return pixel_size_; }
  float aspect_ratio() const { return aspect_ratio_; }

  void look_at(const Vector3f& eye, const Vector3f& center, const Vector3f& up);
  void set_screen_resolution(int width, int height);
  void set_field_of_view(float fov);
  void set_vertical_field_of_view(float fov) { set_field_of_view(fov); }
  void set_horizontal_field_of_view(float fov);

 private:
  void compute_pixel_size();
  void compute_aspect_ratio();

 private:
  Orthonormal_frame frame_;

  float field_of_view_;
  int screen_height_;
  int screen_width_;
  float pixel_size_;
  float aspect_ratio_;
};

}  // namespace nobody

#endif  // NOBODY_CAMERA_H_