#include <iostream>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <map>
std::vector<std::pair<int, int> > conv0, conv1, conv2, conv3, conv4;
std::vector<std::pair<int, int> > pool01, pool12, pool23, pool34;
std::vector<std::pair<std::pair<int, int>, double> > deconv01, deconv12, deconv23, deconv34;
std::vector<float> positions;

void InitializeFullIndicesCPP(const char* filename) {
	FILE* fp = fopen(filename, "rb");
    int num = 4;
    fread(&num, sizeof(int), 1, fp);
    fread(&num, sizeof(int), 1, fp);
    printf("%d\n", num);
    conv0.resize(num);
    fread(conv0.data(), sizeof(std::pair<int, int>), conv0.size(), fp);
    fread(&num, sizeof(int), 1, fp);
    printf("%d\n", num);
    conv1.resize(num);
    fread(conv1.data(), sizeof(std::pair<int, int>), conv1.size(), fp);
    fread(&num, sizeof(int), 1, fp);
    printf("%d\n", num);
    conv2.resize(num);
    fread(conv2.data(), sizeof(std::pair<int, int>), conv2.size(), fp);
    fread(&num, sizeof(int), 1, fp);
    printf("%d\n", num);
    conv3.resize(num);
    fread(conv3.data(), sizeof(std::pair<int, int>), conv3.size(), fp);
    fread(&num, sizeof(int), 1, fp);
    printf("%d\n", num);
    conv4.resize(num);
    fread(conv4.data(), sizeof(std::pair<int, int>), conv4.size(), fp);
    fread(&num, sizeof(int), 1, fp);
    pool01.resize(num);
    fread(pool01.data(), sizeof(std::pair<int, int>), pool01.size(), fp);
    fread(&num, sizeof(int), 1, fp);
    pool12.resize(num);
    fread(pool12.data(), sizeof(std::pair<int, int>), pool12.size(), fp);
    fread(&num, sizeof(int), 1, fp);
    pool23.resize(num);
    fread(pool23.data(), sizeof(std::pair<int, int>), pool23.size(), fp);
    fread(&num, sizeof(int), 1, fp);
    pool34.resize(num);
    fread(pool34.data(), sizeof(std::pair<int, int>), pool34.size(), fp);
    fread(&num, sizeof(int), 1, fp);
    deconv01.resize(num);
    fread(deconv01.data(), sizeof(std::pair<std::pair<int, int>, double>), deconv01.size(), fp);
    fread(&num, sizeof(int), 1, fp);
    deconv12.resize(num);
    fread(deconv12.data(), sizeof(std::pair<std::pair<int, int>, double>), deconv12.size(), fp);
    fread(&num, sizeof(int), 1, fp);
    deconv23.resize(num);
    fread(deconv23.data(), sizeof(std::pair<std::pair<int, int>, double>), deconv23.size(), fp);
    fread(&num, sizeof(int), 1, fp);
    deconv34.resize(num);
    fread(deconv34.data(), sizeof(std::pair<std::pair<int, int>, double>), deconv34.size(), fp);
    fclose(fp);

    for (int i = 0; i < 4; ++i) {
    	auto& conv = (i == 0) ? deconv01 : ((i == 1) ? deconv12 : (i == 2 ? deconv23 : deconv34));
    	for (int j = 0; j < (int)conv.size(); j += 4) {
    		int min_id = -1;
    		for (int k = 0; k < 4; ++k) {
    			if (conv[j + k].first.first != -1 && (min_id == -1 || conv[j + k].second < conv[min_id].second)) {
    				min_id = j + k;
    			}
    		}
    		if (min_id != -1) {
    			for (int k = 0; k < 4; ++k) {
    				if (conv[j + k].first.first == -1) {
    					conv[j + k] = conv[min_id];
    				}
    			}
    		}
    	}
    }
}

int GetNumConvIndicesCPP(int level) {
	auto& conv = (level == 0) ? conv0 : ((level == 1) ? conv1 : ((level == 2) ? conv2 : (level == 3 ? conv3 : conv4)));
	return (int)conv.size();
}

int GetNumPoolIndicesCPP(int level) {
	level += 1;
	auto& conv = ((level == 1) ? conv1 : ((level == 2) ? conv2 : ((level == 3) ? conv3 : conv4)));
	return (int)conv.size() / 9;
}

int GetNumDeconvIndicesCPP(int level) {
	auto& conv = (level == 0) ? deconv01 : ((level == 1) ? deconv12 : (level == 2 ? deconv23 : deconv34));
	return (int)conv.size();
}

void GetConvIndicesCPP(void* indatav, int level, int rosy) {
	int* indices = (int*)indatav;
	auto& conv = (level == 0) ? conv0 : ((level == 1) ? conv1 : ((level == 2) ? conv2 : (level == 3 ? conv3 : conv4)));
	if (rosy == 1) {
		for (int i = 0; i < (int)conv.size(); ++i) {
			indices[i] = conv[i].first;
		}
	} else {
		for (int i = 0; i < (int)conv.size(); ++i) {
			for (int dir = 0; dir < 4; ++dir) {
				indices[i * 4 + dir] = conv[i].first * 4 + (dir + conv[i].second) % 4;
			}
		}
	}
}

void GetPoolIndicesCPP(void* indatav, int level, int rosy) {
	int num_pool = GetNumPoolIndicesCPP(level);
	int* indices = (int*)indatav;
	auto& pool = (level == 0) ? pool01 : ((level == 1) ? pool12 : (level == 2 ? pool23 : pool34));
	if (rosy == 1) {
		for (int i = 0; i < (int)pool.size(); ++i) {
			int index = pool[i].first;
			indices[i] = index;
		}
	} else {
		for (int i = 0; i < (int)pool.size(); ++i) {
			int offset = i * 4;
			for (int j = 0; j < 4; ++j) {
				int index = pool[i].first * 4 + (j + pool[i].second) % 4;
				indices[i * 4 + j] = index;
			}
		}
	}
}

void GetPoolIndicesReverseCPP(void* indatav, int level, int rosy, int stride) {
	int num_pool = GetNumPoolIndicesCPP(level);
	std::vector<int> count(num_pool, 0);
	int* indices = (int*)indatav;

	auto& pool = (level == 0) ? pool01 : ((level == 1) ? pool12 : pool23);
	if (rosy == 1) {
		for (int i = 0; i < (int)pool.size(); ++i) {
			int index = pool[i].first;
			int& offset = count[pool[i].first];
			if (offset < stride) {
				indices[index * stride + offset] = i;
				offset += 1;
			}
		}
		for (int i = 0; i < num_pool; ++i) {
			int offset = count[i];
			int t = (offset == 0) ? (int)pool.size() : indices[i * stride + offset - 1];
			for (int j = offset; j < stride; ++j) {
				indices[i * stride + j] = t;
			}
		}
	} else {
		for (int i = 0; i < (int)pool.size(); ++i) {
			int& offset = count[pool[i].first];
			for (int j = 0; j < 4; ++j) {
				int index = pool[i].first * 4 + (j + pool[i].second) % 4;
				if (offset < stride) {
					indices[index * stride + offset] = i * 4 + j;
				}
			}
			offset += 1;
		}
		for (int i = 0; i < num_pool * 4; ++i) {
			int offset = count[i / 4];
			int t = (offset == 0) ? (int)pool.size() * 4 : indices[i * stride + offset - 1];
			for (int j = offset; j < stride; ++j) {
				indices[i * stride + j] = t;
			}
		}
	}
}

void GetDeconvIndicesCPP(void* indatav, int level, int rosy) {
	int* indices = (int*)indatav;

	auto& conv = (level == 0) ? deconv01 : ((level == 1) ? deconv12 : (level == 2 ? deconv23 : deconv34));
	int num_pool = GetNumPoolIndicesCPP(level);
	if (rosy == 1) {
		for (int i = 0; i < (int)conv.size(); ++i) {
			int index = conv[i].first.first;
			indices[i] = index;
		}
	} else {
		for (int i = 0; i < (int)conv.size(); ++i) {
			for (int j = 0; j < 4; ++j) {
				int index = conv[i].first.first * 4 + (j + conv[i].first.second) % 4;
				indices[i] = index;
			}
		}
	}
}

extern "C" {

void InitializeTangentSpace(void* vertices, void* faces, void* nf, int num_v, int num_f, void* tangent_space) {
	auto V = Eigen::Map<Eigen::MatrixXf>((float*)vertices, 3, num_v);
	auto F = Eigen::Map<Eigen::MatrixXi>((int*)faces, 3, num_f);
	auto FN = Eigen::Map<Eigen::MatrixXf>((float*)nf, 3, num_f);
	for (int i = 0; i < num_f; ++i) {
		Eigen::Matrix3d p, q;
		Eigen::Vector3f v1 = V.col(F(1, i)) - V.col(F(0, i));
		Eigen::Vector3f v2 = V.col(F(2, i)) - V.col(F(0, i));
		Eigen::Vector3f v3 = FN.col(i);
		p.col(0) = Eigen::Vector3d(v1[0], v1[1], v1[2]);
		p.col(1) = Eigen::Vector3d(v2[0], v2[1], v2[2]);
		p.col(2) = Eigen::Vector3d(v3[0], v3[1], v3[2]);
		q = p.inverse();
		double* triangle_space_f = ((double*)tangent_space) + 6 * i;
		for (int j = 0; j < 2; ++j) {
			for (int k = 0; k < 3; ++k) {
				triangle_space_f[k * 2 + j] = q(j, k);
			}
		}
	}

}

void ReadBaryCentry(void* bary_coords, void* bary_indices, const char* filename) {
	FILE* fp = fopen(filename, "rb");
	int num_C, num_I;
	fread(&num_C, sizeof(int), 1, fp);
	fread(&num_I, sizeof(int), 1, fp);
	fread(bary_coords, sizeof(float), 3 * num_C, fp);
	fread(bary_indices, sizeof(int), num_I, fp);
	fclose(fp);
}

void ComputeFacesInfo(void* fv, void* fn, void* v, void* f, int num_v, int num_f) {
	auto V = Eigen::Map<Eigen::MatrixXf>((float*)v, 3, num_v);
	auto F = Eigen::Map<Eigen::MatrixXi>((int*)f, 3, num_f);
#pragma omp parallel for
	for (int i = 0; i < num_f; ++i) {
		float* fv_f = ((float*)fv) + 3 * i;
		float* fn_f = ((float*)fn) + 3 * i;
		Eigen::Vector3f FV = (V.col(F(0, i)) + V.col(F(1, i)) + V.col(F(2, i))) / 3.0f;
		Eigen::Vector3f FN = (Eigen::Vector3f(V.col(F(1, i)) - V.col(F(0, i))).cross(Eigen::Vector3f(V.col(F(2, i)) - V.col(F(0, i))))).normalized();
		fv_f[0] = FV[0];
		fv_f[1] = FV[1];
		fv_f[2] = FV[2];
		fn_f[0] = FN[0];
		fn_f[1] = FN[1];
		fn_f[2] = FN[2];
	}
}

void InitializeE2E(void* vertices, void* faces, int num_v, int num_f, void* pE2E) {
	int* iE2E = (int*)pE2E;
	auto V = Eigen::Map<Eigen::MatrixXf>((float*)vertices, 3, num_v);
	auto F = Eigen::Map<Eigen::MatrixXi>((int*)faces, 3, num_f);
	//E2E = Eigen::VectorXi(num_f * 3);
	std::map<std::pair<int, int>, int > dedgeid;
	for (int i = 0; i < num_f; ++i) {
		for (int j = 0; j < 3; ++j) {
			iE2E[i * 3 + j] = -1;
			int x = F(j, i);
			int y = F((j + 1) % 3, i);
			auto key = std::make_pair(x, y);
			auto reverse_key = std::make_pair(y, x);
			if (dedgeid.count(reverse_key)) {
				int deid = dedgeid[reverse_key];
				iE2E[i * 3 + j] = deid;
				iE2E[deid] = i * 3 + j;
				dedgeid.erase(reverse_key);
			} else {
				dedgeid[key] = i * 3 + j;
			}
		}
	}
}

void IndexPool(const void * indatav, int inlen, void * outdatav, int outlen, int invalid_idx) {
	int* input = (int*)indatav;
	int* output = (int*)outdatav;
	for (int i = 0; i < inlen; ++i) {
		int next_index = input[i] * 4;
		while (output[next_index] != -1)
			next_index += 1;
		output[next_index] = i;
	}

	for (int i = 0; i < outlen; i += 4) {
		if (output[i] == -1)
			output[i] = invalid_idx;
		for (int j = 0; j < 3; ++j) {
			if (output[i + j + 1] == -1)
				output[i + j + 1] = output[i + j];
		}
	}
}

void IndexLabel(const void* indatav, int inlen, void* outdatav, int outlen, int invalid_idx) {
	int* input = (int*)indatav;
	int* output = (int*)outdatav;
	int top = 0;
	for (int i = 0; i < inlen; ++i) {
		if (input[i] != invalid_idx) {
			output[top++] = i;
		}
	}
}

void ReadDialateIndices(void* indatav, const char* filename, int size) {
	int t;
	FILE* fp = fopen(filename, "rb");
	fread(&t, sizeof(int), 1, fp);
	fread(indatav, sizeof(int), size, fp);
	fclose(fp);
}

void ReadArray(void* indatav, const char* filename, int size, int stride) {
	FILE* fp = fopen(filename, "rb");
	fread(indatav, stride, size, fp);
	fclose(fp);
}

int ReadOBJVertices(const char* filename) {
	positions.clear();
	positions.reserve(65536);
	std::ifstream is(filename);
	char c;
	while (is >> c) {
		if (c != 'v')
			break;
		float x, y, z;
		is >> x >> y >> z;
		positions.push_back(x);
		positions.push_back(y);
		positions.push_back(z);
	}
	return positions.size() / 3;
}

void CopyOBJVertices(void* pvertices) {
	memcpy(pvertices, positions.data(), sizeof(float) * positions.size());
}

void InitializeFullIndices(const char* filename) {
	InitializeFullIndicesCPP(filename);
}

int GetLevels() {
	return 4;
}

int GetNumConvIndices(int level) {
	return GetNumConvIndicesCPP(level);
}

int GetNumPoolIndices(int level) {
	return GetNumPoolIndicesCPP(level);
}

int GetNumDeconvIndices(int level) {
	return GetNumDeconvIndicesCPP(level);
}

void GetConvIndices(void* indatav, int level, int rosy) {
	GetConvIndicesCPP(indatav, level, rosy);
}

void GetPoolIndices(void* indatav, int level, int rosy) {
	GetPoolIndicesCPP(indatav, level, rosy);
}

void GetDeconvIndices(void* indatav, int level, int rosy) {
	GetDeconvIndicesCPP(indatav, level, rosy);
}

}


/**
 * Copyright (c) 2018, University Osnabrück
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the University Osnabrück nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL University Osnabrück BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * GraphSLAM.cpp
 *
 *  @date July 22, 2019
 *  @author Malte Hillmann
 */
#include "lvr2/registration/GraphSLAM.hpp"

#include <Eigen/SparseCholesky>

#include <math.h>

using namespace std;
using namespace Eigen;

namespace lvr2
{

/**
 * @brief Lists all numbers of scans near to the scan
 * @param scans reference to a vector containing the SlamScanPtr
 * @param scan number of the current scan
 * @param options SlamOptions struct with all params
 * @param output Returns vector of the scan-numbers which ar defined as "close"
 * */
bool findCloseScans(const vector<SLAMScanPtr>& scans, size_t scan, const SLAMOptions& options, vector<size_t>& output)
{
    if (scan < options.loopSize)
    {
        return false;
    }

    const SLAMScanPtr& cur = scans[scan];

    // closeLoopPairs not specified => use closeLoopDistance
    if (options.closeLoopPairs < 0)
    {
        double maxDist = std::pow(options.closeLoopDistance, 2);
        Vector3d pos = cur->getPosition();
        for (size_t other = 0; other < scan - options.loopSize; other++)
        {
            if ((scans[other]->getPosition() - pos).squaredNorm() < maxDist)
            {
                output.push_back(other);
            }
        }
    }
    else
    {
        // convert current Scan to KDTree for Pair search
        auto tree = KDTree::create(cur, options.maxLeafSize);

        size_t maxLen = 0;
        for (size_t other = 0; other < scan - options.loopSize; other++)
        {
            maxLen = max(maxLen, scans[other]->numPoints());
        }
        KDTree::Neighbor* neighbors = new KDTree::Neighbor[maxLen];

        for (size_t other = 0; other < scan - options.loopSize; other++)
        {
            size_t count = KDTree::nearestNeighbors(tree, scans[other], neighbors, options.slamMaxDistance);
            if (count >= options.closeLoopPairs)
            {
                output.push_back(other);
            }
        }

        delete[] neighbors;
    }

    return !output.empty();
}

/**
 * Conversion from Pose to Matrix representation in GraphSLAMs internally consistent Coordinate System
 *
 * @brief Conversion from Pose to Matrix representation
 * */
void EulerToMatrix4(const Vector3d& pos, const Vector3d& theta, Matrix4d& mat);

/**
 * Conversion from Matrix to Pose representation in GraphSLAMs internally consistent Coordinate System
 *
 * @brief Conversion from Matrix to Pose representation
 * */
void Matrix4ToEuler(const Matrix4d mat, Vector3d& rPosTheta, Vector3d& rPos);

GraphSLAM::GraphSLAM(const SLAMOptions* options)
    : m_options(options)
{
}

void GraphSLAM::doGraphSLAM(const vector<SLAMScanPtr>& scans, size_t last, const std::vector<bool>& new_scans) const
{
    // ignore first scan, keep last scan => n = last - 1 + 1
    size_t n = last;

    Graph graph;
    graph.reserve(n * n / 2);

    GraphMatrix A(6 * n, 6 * n);
    GraphVector B(6 * n);
    GraphVector X(6 * n);

    for (size_t iteration = 0;
            iteration < m_options->slamIterations;
            iteration++)
    {
        cout << "GraphSLAM Iteration " << iteration << " of " << m_options->slamIterations << endl;

        createGraph(scans, last, graph);

        // Construct the linear equation system A * X = B..
        fillEquation(scans, graph, A, B);

        graph.clear();

        X = SimplicialCholesky<GraphMatrix>().compute(A).solve(B);

        double sum_position_diff = 0.0;

        // Start with second Scan
        #pragma omp parallel for reduction(+:sum_position_diff) schedule(static)
        for (size_t i = 1; i <= last; i++)
        {
            if (new_scans.empty() || new_scans.at(i))
            {
                const SLAMScanPtr& scan = scans[i];

                // Now update the Poses
                Matrix6d Ha = Matrix6d::Identity();

                Matrix4d initialPose = scan->pose();
                Vector3d pos, theta;
                Matrix4ToEuler(initialPose, theta, pos);
                if (m_options->verbose)
                {
                    cout << "Start of " << i << ": " << pos.transpose() << ", " << theta.transpose() << endl;
                }

                double ctx, stx, cty, sty;

#ifndef __APPLE__
                sincos(theta.x(), &stx, &ctx);
                sincos(theta.y(), &sty, &cty);
#else
                __sincos(theta.x(), &stx, &ctx);
                __sincos(theta.y(), &sty, &cty);
#endif

                // Fill Ha
                Ha(0, 4) = -pos.z() * ctx + pos.y() * stx;
                Ha(0, 5) = pos.y() * cty * ctx + pos.z() * stx * cty;

                Ha(1, 3) = pos.z();
                Ha(1, 4) = -pos.x() * stx;
                Ha(1, 5) = -pos.x() * ctx * cty + pos.z() * sty;

                Ha(2, 3) = -pos.y();
                Ha(2, 4) = pos.x() * ctx;
                Ha(2, 5) = -pos.x() * cty * stx - pos.y() * sty;

                Ha(3, 5) = sty;

                Ha(4, 4) = stx;
                Ha(4, 5) = ctx * cty;

                Ha(5, 4) = ctx;
                Ha(5, 5) = -stx * cty;

                // Correct pose estimate
                Vector6d result = Ha.inverse() * X.block<6, 1>((i - 1) * 6, 0);

                // Update the Pose
                pos -= result.block<3, 1>(0, 0);
                theta -= result.block<3, 1>(3, 0);
                Matrix4d transform;
                EulerToMatrix4(pos, theta, transform);

                if (m_options->verbose)
                {
                    cout << "End: pos: " << pos.transpose() << "," << endl << "theta: " << theta.transpose() << endl;
                }

                transform = transform * initialPose.inverse();

                scan->transform(transform, m_options->createFrames, FrameUse::GRAPHSLAM);

                sum_position_diff += result.block<3, 1>(0, 0).norm();
            }
        }

        if (m_options->createFrames)
        {
            // add Frames to unused Scans
            scans[0]->addFrame(FrameUse::GRAPHSLAM);
            for (size_t i = last + 1; i < scans.size(); i++)
            {
                scans[i]->addFrame(FrameUse::INVALID);
            }
        }

        cout << "Sum of Position differences = " << sum_position_diff << endl;

        double avg = sum_position_diff / n;
        if (avg < m_options->slamEpsilon)
        {
            break;
        }
    }
}

void GraphSLAM::createGraph(const vector<SLAMScanPtr>& scans, size_t last, Graph& graph) const
{
    graph.clear();

    for (size_t i = 1; i <= last; i++)
    {
        graph.push_back(make_pair(i - 1, i));
    }

    vector<size_t> others;
    for (size_t i = m_options->loopSize; i <= last; i++)
    {
        findCloseScans(scans, i, *m_options, others);

        for (size_t other : others)
        {
            graph.push_back(make_pair(other, i));
        }

        others.clear();
    }
}

void GraphSLAM::fillEquation(const vector<SLAMScanPtr>& scans, const Graph& graph, GraphMatrix& mat, GraphVector& vec) const
{
    // Cache all KDTrees
    map<size_t, KDTreePtr> trees;
    for (size_t i = 0; i < graph.size(); i++)
    {
        size_t a = graph[i].first;
        if (trees.find(a) == trees.end())
        {
            auto tree = KDTree::create(scans[a], m_options->maxLeafSize);
            trees.insert(make_pair(a, tree));
        }
    }

    vector<pair<Matrix6d, Vector6d>> coeff(graph.size());

    #pragma omp parallel for schedule(dynamic)
    for (size_t i = 0; i < graph.size(); i++)
    {
        int a, b;
        std::tie(a, b) = graph[i];

        KDTreePtr tree  = trees[a];
        SLAMScanPtr scan = scans[b];

        Matrix6d coeffMat;
        Vector6d coeffVec;
        eulerCovariance(tree, scan, coeffMat, coeffVec);

        coeff[i] = make_pair(coeffMat, coeffVec);
    }

    trees.clear();

    map<pair<int, int>, Matrix6d> result;

    mat.setZero();
    vec.setZero();

    for (size_t i = 0; i < graph.size(); i++)
    {
        int a, b;
        std::tie(a, b) = graph[i];

        Matrix6d coeffMat;
        Vector6d coeffVec;
        std::tie(coeffMat, coeffVec) = coeff[i];

        // first scan is not part of Matrix => ignore any a or b of 0

        int offsetA = (a - 1) * 6;
        int offsetB = (b - 1) * 6;

        if (offsetA >= 0)
        {
            vec.block<6, 1>(offsetA, 0) += coeffVec;
            auto key = make_pair(offsetA, offsetA);
            auto found = result.find(key);
            if (found == result.end())
            {
                result.insert(make_pair(key, coeffMat));
            }
            else
            {
                found->second += coeffMat;
            }
        }
        if (offsetB >= 0)
        {
            vec.block<6, 1>(offsetB, 0) -= coeffVec;
            auto key = make_pair(offsetB, offsetB);
            auto found = result.find(key);
            if (found == result.end())
            {
                result.insert(make_pair(key, coeffMat));
            }
            else
            {
                found->second += coeffMat;
            }
        }
        if (offsetA >= 0 && offsetB >= 0)
        {
            auto key = make_pair(offsetA, offsetB);
            auto found = result.find(key);
            if (found == result.end())
            {
                result.insert(make_pair(key, -coeffMat));
            }
            else
            {
                found->second -= coeffMat;
            }

            key = make_pair(offsetB, offsetA);
            found = result.find(key);
            if (found == result.end())
            {
                result.insert(make_pair(key, -coeffMat));
            }
            else
            {
                found->second -= coeffMat;
            }
        }
    }

    vector<Triplet<double>> triplets;
    triplets.reserve(result.size() * 6 * 6);

    int x, y;
    for (auto& e : result)
    {
        tie(x, y) = e.first;
        Matrix6d& m = e.second;
        for (int dx = 0; dx < 6; dx++)
        {
            for (int dy = 0; dy < 6; dy++)
            {
                triplets.push_back(Triplet<double>(x + dx, y + dy, m(dx, dy)));
            }
        }
    }
    mat.setFromTriplets(triplets.begin(), triplets.end());
}

void GraphSLAM::eulerCovariance(KDTreePtr tree, SLAMScanPtr scan, Matrix6d& outMat, Vector6d& outVec) const
{
    size_t n = scan->numPoints();

    KDTree::Neighbor* results = new KDTree::Neighbor[n];

    size_t pairs = KDTree::nearestNeighbors(tree, scan, results, m_options->slamMaxDistance);

    Vector6d mz = Vector6d::Zero();
    Vector3d sum = Vector3d::Zero();
    double xy, yz, xz, ypz, xpz, xpy;
    xy = yz = xz = ypz = xpz = xpy = 0.0;

    for (size_t i = 0; i < n; i++)
    {
        if (results[i] == nullptr)
        {
            continue;
        }

        Vector3d p = scan->point(i).cast<double>();
        Vector3d r = results[i]->cast<double>();

        Vector3d mid = (p + r) / 2.0;
        Vector3d d = r - p;

        double x = mid.x(), y = mid.y(), z = mid.z();

        sum += mid;

        xpy += x * x + y * y;
        xpz += x * x + z * z;
        ypz += y * y + z * z;

        xy += x * y;
        xz += x * z;
        yz += y * z;

        mz.block<3, 1>(0, 0) += d;

        mz(3) += -z * d.y() + y * d.z();
        mz(4) += -y * d.x() + x * d.y();
        mz(5) += z * d.x() - x * d.z();
    }

    Matrix6d mm = Matrix6d::Zero();
    mm(0, 0) = mm(1, 1) = mm(2, 2) = pairs;
    mm(3, 3) = ypz;
    mm(4, 4) = xpy;
    mm(5, 5) = xpz;

    mm(0, 4) = mm(4, 0) = -sum.y();
    mm(0, 5) = mm(5, 0) = sum.z();
    mm(1, 3) = mm(3, 1) = -sum.z();
    mm(1, 4) = mm(4, 1) = sum.x();
    mm(2, 3) = mm(3, 2) = sum.y();
    mm(2, 5) = mm(5, 2) = -sum.x();

    mm(3, 4) = mm(4, 3) = -xz;
    mm(3, 5) = mm(5, 3) = -xy;
    mm(4, 5) = mm(5, 4) = -yz;

    Vector6d d = mm.inverse() * mz;

    double ss = 0.0;

    for (size_t i = 0; i < n; i++)
    {
        if (results[i] == nullptr)
        {
            continue;
        }

        Vector3d p = scan->point(i).cast<double>();
        Vector3d r = results[i]->cast<double>();

        Vector3d mid = (p + r) / 2.0;
        Vector3d delta = r - p;

        ss += pow(delta.x() + (d(0) - mid.y() * d(4) + mid.z() * d(5)), 2.0)
              + pow(delta.y() + (d(1) - mid.z() * d(3) + mid.x() * d(4)), 2.0)
              + pow(delta.z() + (d(2) + mid.y() * d(3) - mid.x() * d(5)), 2.0);
    }

    delete[] results;

    ss = ss / (2.0 * pairs - 3.0);

    ss = 1.0 / ss;

    outMat = mm * ss;
    outVec = mz * ss;
}

void EulerToMatrix4(const Vector3d& pos,
                    const Vector3d& theta,
                    Matrix4d& mat)
{
    double sx = sin(theta[0]);
    double cx = cos(theta[0]);
    double sy = sin(theta[1]);
    double cy = cos(theta[1]);
    double sz = sin(theta[2]);
    double cz = cos(theta[2]);

    mat << cy* cz,
        sx* sy* cz + cx* sz,
        -cx* sy* cz + sx* sz,
        0.0,
        -cy* sz,
        -sx* sy* sz + cx* cz,
        cx* sy* sz + sx* cz,
        0.0,
        sy,
        -sx* cy,
        cx* cy,

        0.0,

        pos[0],
        pos[1],
        pos[2],
        1;

    mat.transposeInPlace();
}

void Matrix4ToEuler(const Matrix4d inputMat,
                    Vector3d& rPosTheta,
                    Vector3d& rPos)
{
    Matrix4d mat = inputMat.transpose();

    double _trX, _trY;

    // Calculate Y-axis angle
    rPosTheta[1] = asin(max(-1.0, min(1.0, mat(2, 0)))); // asin returns nan for any number outside [-1, 1]
    if (mat(0, 0) <= 0.0)
    {
        rPosTheta[1] = M_PI - rPosTheta[1];
    }

    double C = cos(rPosTheta[1]);
    if (fabs( C ) > 0.005)                    // Gimbal lock?
    {
        _trX      =  mat(2, 2) / C;             // No, so get X-axis angle
        _trY      =  -mat(2, 1) / C;
        rPosTheta[0]  = atan2( _trY, _trX );
        _trX      =  mat(0, 0) / C;              // Get Z-axis angle
        _trY      = -mat(1, 0) / C;
        rPosTheta[2]  = atan2( _trY, _trX );
    }
    else                                        // Gimbal lock has occurred
    {
        rPosTheta[0] = 0.0;                       // Set X-axis angle to zero
        _trX      =  mat(1, 1);  //1                // And calculate Z-axis angle
        _trY      =  mat(0, 1);  //2
        rPosTheta[2]  = atan2( _trY, _trX );
    }

    rPos = inputMat.block<3, 1>(0, 3);
}

} /* namespace lvr2 */


#include "stdafx.h"
#include <iostream>
#include <boost/format.hpp>

#include "api.h"
#include "api_helpers.hpp"
#include "hpgl_core.h"
#include "sugarbox_grid.h"
#include "ok_params.h"
#include "sk_params.h"
#include "sgs_params.h"
#include "ik_params.h"
#include "median_ik.h"
#include "property_writer.h"
#include "progress_reporter.h"
#include "hpgl_exception.h"
#include "output.h"

static void init_cov_param(hpgl::covariance_param_t * cp, hpgl_cov_params_t * params)
{
	for (int i = 0; i < 3; ++i)
	{
		cp->m_ranges[i] = params->m_ranges[i];
		cp->m_angles[i] = params->m_angles[i];
	}
	cp->m_sill = params->m_sill;
	cp->m_nugget = params->m_nugget;
	cp->m_covariance_type = (hpgl::covariance_type_t) params->m_covariance_type;
}

static void
handle_exception(const std::exception & ex)
{
	hpgl::set_last_exception_message(ex.what());
}

extern "C" {

HPGL_API char * hpgl_get_last_exception_message()
{
	return (char *) hpgl::get_last_exception_message().c_str();
}

HPGL_API void hpgl_set_thread_num(int n_threads)
{
	hpgl::set_thread_num(n_threads);
}

HPGL_API int hpgl_get_thread_num()
{
	return hpgl::get_thread_num();
}

HPGL_API void hpgl_read_inc_file_float(
		char * filename,
		float undefined_value,
		int size,
		float * data,
		unsigned char * mask)
{
	hpgl::read_inc_file_float(
			filename,
			undefined_value,
			size,
			data,
			mask);
}

HPGL_API void hpgl_read_inc_file_byte(
		char * filename,
		int undefined_value,
		int size,
		unsigned char * data,
		unsigned char * mask,
		unsigned char * values,
		int values_count)
{
	hpgl::read_inc_file_byte(
			filename,
			undefined_value,
			size,
			data,
			mask);

	for (int i = 0; i < size; ++i)
	{
		if (mask[i] != 0)
		{
			for (int j = 0; j < values_count; ++j)
			{
				if (data[i] == values[j])
				{
					data[i] = j;
					break;
				}
			}
		}
	}
}

HPGL_API void hpgl_write_inc_file_float(
		char * filename,
		hpgl_cont_masked_array_t * arr,
		float undefined_value,
		char * name)
{
	using namespace hpgl;
	property_writer_t writer;
	writer.init(filename, name);
	cont_property_array_t prop(
			arr->m_data,
			arr->m_mask,
			get_shape_volume(&(arr->m_shape)));
	writer.write_double(prop, undefined_value);
}

void init_remap_table(unsigned char * values, int values_count, int indicator_count, std::vector<unsigned char> & remap_table)
{
	using namespace boost;
	if (values_count == indicator_count)
	{
		remap_table.assign(values, values+values_count);
	}
	else if (values_count > indicator_count)
	{
		LOGWARNING(format("Given %1% values for %2% indicators. Ignoring extra values.\n") % values_count % indicator_count);
		remap_table.assign(values, values + indicator_count);
	}
	else
	{
		if (values_count > 0)
			LOGWARNING(format("Warning: Given %1% values for %2% indicators. Ignoring values. Using [0, 1, 2 ..]\n") % values_count % indicator_count);
		for (int i = 0; i < indicator_count; ++i)
			remap_table.push_back(i);
	}
}

HPGL_API void hpgl_write_inc_file_byte(
		char * filename,
		hpgl_ind_masked_array_t * arr,
		int undefined_value,
		char * name,
		unsigned char * values,
		int values_count)
{
	using namespace hpgl;
	std::vector<unsigned char> remap_table;
	init_remap_table(values, values_count, arr->m_indicator_count, remap_table);

	property_writer_t writer;
	writer.init(filename, name);
	indicator_property_array_t prop(
			arr->m_data,
			arr->m_mask,
			get_shape_volume(&(arr->m_shape)),
			arr->m_indicator_count);
	writer.write_byte(prop, undefined_value, remap_table);
}

HPGL_API void
hpgl_write_gslib_cont_property(
		hpgl_cont_masked_array_t * data,
		const char * filename,
		const char * name,
		double undefined_value)
{
	using namespace hpgl;
	int size = get_shape_volume(&data->m_shape);
	sp_double_property_array_t prop(new cont_property_array_t(data->m_data, data->m_mask, size));
	hpgl::property_writer_t writer;
	writer.init(filename, name);
	writer.write_gslib_double(prop, undefined_value);
}

HPGL_API void
hpgl_write_gslib_byte_property(
		hpgl_ind_masked_array_t * data,
		const char * filename,
		const char * name,
		double undefined_value,
		unsigned char * values,
		int values_count)
{
	using namespace hpgl;
	int size = get_shape_volume(&data->m_shape);
	sp_byte_property_array_t prop( new indicator_property_array_t(data->m_data, data->m_mask, size, data->m_indicator_count));
	std::vector<unsigned char> remap_table;
	init_remap_table(values, values_count, data->m_indicator_count, remap_table);

	property_writer_t writer;
	writer.init(filename, name);
	writer.write_gslib_byte(prop, (unsigned char)undefined_value, remap_table);
}

HPGL_API void hpgl_ordinary_kriging(
    hpgl_cont_masked_array_t * input_data,
    hpgl_ok_params_t * params,
    hpgl_cont_masked_array_t * output_data)
{
	using namespace hpgl;
	int in_size = get_shape_volume(&input_data->m_shape);
	int out_size = get_shape_volume(&output_data->m_shape);

	cont_property_array_t in_prop(input_data->m_data, input_data->m_mask, in_size);
	cont_property_array_t out_prop(output_data->m_data, output_data->m_mask, out_size);

	sugarbox_grid_t grid;
	init_grid(grid, &input_data->m_shape);

	ok_params_t ok_p;
	ok_p.m_covariance_type = (covariance_type_t) params->m_covariance_type;
	ok_p.set_ranges(
			params->m_ranges[0],
			params->m_ranges[1],
			params->m_ranges[2]);
	ok_p.set_angles(
			params->m_angles[0],
			params->m_angles[1],
			params->m_angles[2]);
	ok_p.m_sill = params->m_sill;
	ok_p.m_nugget = params->m_nugget;

	ok_p.set_radiuses(
			params->m_radiuses[0],
			params->m_radiuses[1],
			params->m_radiuses[2]);

	ok_p.m_max_neighbours = params->m_max_neighbours;

	hpgl::ordinary_kriging(in_prop, grid, ok_p, out_prop, true);
}

static void init_sk_params(hpgl_sk_params_t * params, hpgl::sk_params_t & sk_p)
{
	using namespace hpgl;
	sk_p.m_covariance_type = (covariance_type_t) params->m_covariance_type;
	sk_p.set_ranges(
			params->m_ranges[0],
			params->m_ranges[1],
			params->m_ranges[2]);
	sk_p.set_angles(
			params->m_angles[0],
			params->m_angles[1],
			params->m_angles[2]);
	sk_p.m_sill = params->m_sill;
	sk_p.m_nugget = params->m_nugget;

	sk_p.set_radiuses(
			params->m_radiuses[0],
			params->m_radiuses[1],
			params->m_radiuses[2]);

	sk_p.m_max_neighbours = params->m_max_neighbours;

	if (!params->m_automatic_mean)
	{
		sk_p.set_mean(params->m_mean);
	}
}

HPGL_API void hpgl_simple_kriging(
    float * input_data,
    unsigned char * input_mask,
    hpgl_shape_t * input_data_shape,
    hpgl_sk_params_t * params,
    float * output_data,
    unsigned char * output_mask,
    hpgl_shape_t * output_data_shape)
{
	using namespace hpgl;
	int in_size = input_data_shape->m_data[0] * input_data_shape->m_data[1] * input_data_shape->m_data[2];
	int out_size = output_data_shape->m_data[0] * output_data_shape->m_data[1] * output_data_shape->m_data[2];

	cont_property_array_t in_prop(input_data, input_mask, in_size);
	cont_property_array_t out_prop(output_data, output_mask, out_size);

	sugarbox_grid_t grid;
	grid.init(
			input_data_shape->m_data[0],
			input_data_shape->m_data[1],
			input_data_shape->m_data[2]
			  );

	sk_params_t sk_p;
	init_sk_params(params, sk_p);

	hpgl::simple_kriging(in_prop, grid, sk_p, out_prop);
	}

}

HPGL_API int
hpgl_simple_kriging_weights(
		float * center_coords,
		float * neighbours_x,
		float * neighbours_y,
		float * neighbours_z,
		int neighbours_count,
		hpgl_cov_params_t * params,
		float * weights)
{
	using namespace hpgl;
	real_location_t center(center_coords[0], center_coords[1], center_coords[2]);

	std::vector<real_location_t> neighbour_coords(neighbours_count);
	for (int i = 0; i < neighbours_count; ++i)
	{
		neighbour_coords[i][0] = neighbours_x[i];
		neighbour_coords[i][1] = neighbours_y[i];
		neighbour_coords[i][2] = neighbours_z[i];
	}

	std::vector<kriging_weight_t> weights2;
	double variance;

	sk_params_t sk_p;

	sk_p.m_covariance_type = (covariance_type_t) params->m_covariance_type;
	sk_p.set_ranges(
			params->m_ranges[0],
			params->m_ranges[1],
			params->m_ranges[2]);
	sk_p.set_angles(
			params->m_angles[0],
			params->m_angles[1],
			params->m_angles[2]);
	sk_p.m_sill = params->m_sill;
	sk_p.m_nugget = params->m_nugget;

	simple_kriging_weights(
			&sk_p,
			center,
			neighbour_coords,
			weights2,
			variance);

	for (int i = 0; i < neighbours_count; ++i)
	{
		weights[i] = weights2.at(i);
	}
	return 0;
}

HPGL_API void hpgl_lvm_kriging(
    float * input_data,
    unsigned char * input_mask,
    hpgl_shape_t * input_data_shape,
    float * mean_data,
    hpgl_shape_t * mean_data_shape,
    hpgl_ok_params_t * params,
    float * output_data,
    unsigned char * output_mask,
    hpgl_shape_t * output_data_shape)
{
	using namespace hpgl;
	int size = get_shape_volume(input_data_shape);
	cont_property_array_t input_prop(input_data, input_mask, size);
	sugarbox_grid_t grid;
	init_grid(grid, input_data_shape);

	ok_params_t ok_p;
	ok_p.m_covariance_type = (covariance_type_t) params->m_covariance_type;
	ok_p.set_ranges(
			params->m_ranges[0],
			params->m_ranges[1],
			params->m_ranges[2]);
	ok_p.set_angles(
			params->m_angles[0],
			params->m_angles[1],
			params->m_angles[2]);
	ok_p.m_sill = params->m_sill;
	ok_p.m_nugget = params->m_nugget;

	ok_p.set_radiuses(
			params->m_radiuses[0],
			params->m_radiuses[1],
			params->m_radiuses[2]);

	ok_p.m_max_neighbours = params->m_max_neighbours;

	cont_property_array_t out_prop(output_data, output_mask, size);
	lvm_kriging(input_prop, mean_data, grid, ok_p, out_prop);
}

HPGL_API void
hpgl_indicator_kriging(
		hpgl_ind_masked_array_t * in_data,
		hpgl_ind_masked_array_t * out_data,
		hpgl_ik_params_t * params,
		int indicator_count)
{
	using namespace hpgl;
	int size = get_shape_volume(&in_data->m_shape);
	int size2 = get_shape_volume(&out_data->m_shape);
	assert (size == size2);
	indicator_property_array_t in_prop(in_data->m_data, in_data->m_mask, size, in_data->m_indicator_count);
	indicator_property_array_t out_prop(out_data->m_data, out_data->m_mask, size2, out_data->m_indicator_count);

	ik_params_t ikp;
	init_sis_params(params, indicator_count, &ikp);

	sugarbox_grid_t grid;
	init_grid(grid, &(in_data->m_shape));

	indicator_kriging(in_prop, grid, ikp, out_prop);
}

HPGL_API void
hpgl_sgs_simulation(
		hpgl_cont_masked_array_t * data,
		hpgl_sgs_params_t * params,
		hpgl_non_parametric_cdf_t * cdf,
		double * mean,
		hpgl_ubyte_array_t * simulation_mask)

{
	using namespace hpgl;
	int size = get_shape_volume(&(data->m_shape));
	cont_property_array_t prop(data->m_data, data->m_mask, size);

	sugarbox_grid_t grid;
	init_grid(grid, &(data->m_shape));

	sgs_params_t sgs_p;
	init_sgs_params(params, &sgs_p);

	if (mean != 0)
		sgs_p.set_mean(*mean);

	sgs_p.m_lvm = 0;
	sgs_p.m_mean_kind = mean != 0 ? e_mean_stationary : e_mean_stationary_auto;

	hpgl::sequential_gaussian_simulation(
			grid,
			sgs_p,
			prop,
			cdf,
			simulation_mask != 0 ? simulation_mask->m_data : 0);
}

HPGL_API void hpgl_sgs_lvm_simulation(
		hpgl_cont_masked_array_t * data,
		hpgl_sgs_params_t * params,
		hpgl_non_parametric_cdf_t * cdf,
		hpgl_float_array_t * means,
		hpgl_ubyte_array_t * simulation_mask)
{
	using namespace hpgl;
	int size = get_shape_volume(&(data->m_shape));
	cont_property_array_t prop(data->m_data, data->m_mask, size);

	sugarbox_grid_t grid;
	init_grid(grid, &(data->m_shape));

	sgs_params_t sgs_p;
	init_sgs_params(params, &sgs_p);

	sgs_p.m_lvm = means->m_data;
	sgs_p.m_mean_kind = e_mean_varying;

	hpgl::sequential_gaussian_simulation_lvm(
			grid,
			sgs_p,
			means->m_data,
			prop,
			cdf,
			simulation_mask != 0 ? simulation_mask->m_data : 0);
}

HPGL_API void hpgl_median_ik(
		hpgl_ind_masked_array_t * in_data,
		hpgl_median_ik_params_t * params,
		hpgl_ind_masked_array_t * out_data)
{
	using namespace hpgl;

	int size = get_shape_volume(&(in_data->m_shape));

	sugarbox_grid_t grid;
	init_grid(grid, &(in_data->m_shape));

	median_ik_params mik_p;
	mik_p.m_covariance_type = (covariance_type_t) params->m_covariance_type;
	mik_p.set_ranges(
			params->m_ranges[0],
			params->m_ranges[1],
			params->m_ranges[2]);
	mik_p.set_angles(
			params->m_angles[0],
			params->m_angles[1],
			params->m_angles[2]);
	mik_p.m_sill = params->m_sill;
	mik_p.m_nugget = params->m_nugget;

	mik_p.set_radiuses(
			params->m_radiuses[0],
			params->m_radiuses[1],
			params->m_radiuses[2]);

	mik_p.m_max_neighbours = params->m_max_neighbours;
	mik_p.m_marginal_probs[0] = params->m_marginal_probs[0];
	mik_p.m_marginal_probs[1] = params->m_marginal_probs[1];

	indicator_property_array_t in_prop(
			in_data->m_data,
			in_data->m_mask,
			size,
			2);
	indicator_property_array_t out_prop(
			out_data->m_data,
			out_data->m_mask,
			size,
			2);
	median_ik_for_two_indicators(mik_p, grid, in_prop, out_prop);

}

HPGL_API void
hpgl_sis_simulation(
		hpgl_ind_masked_array_t * data,
		hpgl_ik_params_t * params,
		int indicator_count,
		int seed,
		hpgl_ubyte_array_t * simulation_mask)
{
	using namespace hpgl;
	int size = get_shape_volume(&data->m_shape);
	indicator_property_array_t prop(data->m_data, data->m_mask, size, indicator_count);

	sugarbox_grid_t grid;
	init_grid(grid, &data->m_shape);

	ik_params_t ikp;
	init_sis_params(params, indicator_count, &ikp);

	progress_reporter_t rep(size);

	sequential_indicator_simulation(
			prop,
			grid,
			ikp,
			seed,
			rep,
			false,
			simulation_mask !=0 ? simulation_mask->m_data : 0);
}

HPGL_API void
hpgl_sis_simulation_lvm(
		hpgl_ind_masked_array_t * data,
		hpgl_ik_params_t * params,
		hpgl_float_array_t * mean_data,
		int indicator_count,
		int seed,
		hpgl_ubyte_array_t * simulation_mask,
		int use_correlograms)
{
	using namespace hpgl;
	int size = get_shape_volume(&data->m_shape);
	indicator_property_array_t prop(data->m_data, data->m_mask, size, indicator_count);

	sugarbox_grid_t grid;
	init_grid(grid, &data->m_shape);

	ik_params_t ikp;
	init_sis_params(params, indicator_count, &ikp);

	progress_reporter_t rep(size);

	std::vector<const mean_t *> means;
	for (int i = 0; i < indicator_count; ++i)
	{
		means.push_back(mean_data[i].m_data);
	}

	sequential_indicator_simulation_lvm(
			prop,
			grid,
			ikp,
			seed,
			&means[0],
			rep,
			use_correlograms != 0,
			simulation_mask != 0 ? simulation_mask->m_data : 0);

}

HPGL_API void
hpgl_simple_cokriging_mark1(
		hpgl_cont_masked_array_t * input_data,
		hpgl_cont_masked_array_t * secondary_data,
		hpgl_cokriging_m1_params_t * params,
		hpgl_cont_masked_array_t * output_data)
{
	using namespace boost;
	using namespace hpgl;
	int size = get_shape_volume(&input_data->m_shape);
	int size2 = get_shape_volume(&secondary_data->m_shape);
	int size3 = get_shape_volume(&output_data->m_shape);

	if (size != size2)
	{
		throw hpgl_exception(
				"hpgl_simple_cokriging_mark1",
				format("Size of secondary data (%1%) is different from size of primary data (%2%)") % size2 % size);
	}

	if (size != size3)
	{
		throw hpgl_exception(
				"hpgl_simple_cokriging_mark1",
				format("Size of output data (%1%) is different from size of primary data (%2%)") % size3 % size);
	}

	cont_property_array_t primary_prop(
			input_data->m_data, input_data->m_mask, size);
	cont_property_array_t secondary_prop(
			secondary_data->m_data, secondary_data->m_mask, size2);
	cont_property_array_t output_prop(
			output_data->m_data, output_data->m_mask, size3);

	neighbourhood_param_t np;
	np.m_max_neighbours = params->m_max_neighbours;

	covariance_param_t cp;
	cp.m_nugget = params->m_nugget;
	cp.m_sill = params->m_sill;
	cp.m_covariance_type = (covariance_type_t) params->m_covariance_type;

	for (int i = 0; i < 3; ++i)
	{
		np.m_radiuses[i] = params->m_radiuses[i];
		cp.m_ranges[i] = params->m_ranges[i];
		cp.m_angles[i] = params->m_angles[i];
	}

	sugarbox_grid_t grid;
	init_grid(grid, &input_data->m_shape);

	simple_cokriging_markI(
			grid,
			primary_prop,
			secondary_prop,
			params->m_primary_mean,
			params->m_secondary_mean,
			params->m_secondary_variance,
			params->m_correlation_coef,
			np,
			cp,
			output_prop);

}

HPGL_API void
hpgl_simple_cokriging_mark2(
		hpgl_cont_masked_array_t * primary_data,
		hpgl_cont_masked_array_t * secondary_data,
		hpgl_cokriging_m2_params_t * params,
		hpgl_cont_masked_array_t * output_data)
{
	using namespace boost;
	using namespace hpgl;
	int size = get_shape_volume(&primary_data->m_shape);
	int size2 = get_shape_volume(&secondary_data->m_shape);
	int size3 = get_shape_volume(&output_data->m_shape);

	if (size != size2)
	{
		throw hpgl_exception(
				"hpgl_simple_cokriging_mark1",
				format("Size of secondary data (%1%) is different from size of primary data (%2%)") % size2 % size);
	}

	if (size != size3)
	{
		throw hpgl_exception(
				"hpgl_simple_cokriging_mark1",
				format("Size of output data (%1%) is different from size of primary data (%2%)") % size3 % size);
	}

	cont_property_array_t primary_prop(
			primary_data->m_data, primary_data->m_mask, size);
	cont_property_array_t secondary_prop(
			secondary_data->m_data, secondary_data->m_mask, size2);
	cont_property_array_t out_prop(
			output_data->m_data, output_data->m_mask, size3);

	sugarbox_grid_t grid;
	init_grid(grid, &primary_data->m_shape);

	covariance_param_t primary_cp, secondary_cp;

	init_cov_param(&primary_cp, &params->m_primary_cov_params);
	init_cov_param(&secondary_cp, &params->m_secondary_cov_params);

	neighbourhood_param_t np;
	np.m_max_neighbours = params->m_max_neighbours;
	for (int i = 0; i < 3; ++i)
	{
		np.m_radiuses[i] = params->m_radiuses[i];
	}

	simple_cokriging_markII(
			grid, primary_prop,
			secondary_prop,
			params->m_primary_mean,
			params->m_secondary_mean,
			params->m_correlation_coef,
			np,
			primary_cp,
			secondary_cp,
			out_prop);
}


#include "simulationexecutor.h"

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include "scriptloader.h"
#include "simulation.h"

#include "nonlinearhamiltonian.h"

using namespace boost;
using namespace property_tree;

SimulationExecutor::SimulationExecutor(const std::string& filename) {
    ptree tree;
    json_parser::read_json(filename, tree);

    for (auto it = tree.begin(); it != tree.end(); ++it) {
        if (it->first == "Simulation") {
            const ptree& child = it->second;
            SimulationParameter params(child.get<double>("dx"),
                                       child.get<double>("dt"),
                                       child.get<double>("mass"),
                                       child.get<int>("iterations"),
                                       child.get<int>("atoms"));
            simulations.push_back(std::pair<SimulationParameter, std::string>(params, child.get<std::string>("script")));
        }
    }

    run();
}

SimulationExecutor::SimulationExecutor(const SimulationParameter& params, const std::string& scriptFile) {
    simulations.push_back(std::pair<SimulationParameter, std::string>(params, scriptFile));

    run();
}

void SimulationExecutor::run() {
    for (std::pair<SimulationParameter, std::string> simul : simulations) {
        Simulation sim(simul.first, nullptr);
        ScriptExecutor(sim, simul.second);
    }
}


#include <boost/program_options.hpp>
//#include <experimental/filesystem>
#include <boost/filesystem.hpp>
#include <iostream>
#include <string>
#include <algorithm>
#include <regex>
#include <cstdlib>
#include <cmath>
#include <armadillo>

#include "atomsciflow/parser/cif.h"
#include "atomsciflow/parser/xyz.h"
#include "atomsciflow/base/crystal.h"
#include "atomsciflow/utils.h"

#include "atomsciflow/cube_handle/cube_handle.h"
// needs: libboost-dev, libboost-program-options-dev

namespace po = boost::program_options;

//namespace filesys = std::experimental::filesystem;  // --std=c++17 -lstdc++fs
namespace filesys = boost::filesystem;     // --std=c++11 -lboost_filesystem -lboost_system

// used to allow negative number parsed to boost cmd option
std::vector<po::option> ignore_numbers(std::vector<std::string>& args)
{
    // this function can help to alow negative number args but it probhibits positional args
    // however we do not need positional args. so it is ok.
    std::vector<po::option> result;
    int pos = 0;
    while(!args.empty()) {
        const auto& arg = args[0];
        double num;
        if(boost::conversion::try_lexical_convert(arg, num)) {
            result.push_back(po::option());
            po::option& opt = result.back();

            opt.position_key = pos++;
            opt.value.push_back(arg);
            opt.original_tokens.push_back(arg);

            args.erase(args.begin());
        } else {
            break;
        }
    }

    return result;
}

int main(int argc, char const* argv[]) {
    //

    po::options_description global("Global options");
    global.add_options()
        ("command", po::value<std::string>(), "command to execute")
        ("subargs", po::value<std::vector<std::string> >(), "Arguments for command")
        ("help, h", "print out help information");

    po::positional_options_description pos;
    pos.add("command", 1).add("subargs", -1);

    po::variables_map vm;

    po::parsed_options parsed = po::command_line_parser(argc, argv).
        options(global).
        positional(pos).
        allow_unregistered().
        run();

    po::store(parsed, vm);

    std::cout << "**********************************************************************" << std::endl;
    std::cout << "*                       skit-cube.x utils runnig                     *" << std::endl;
    std::cout << "**********************************************************************" << std::endl;

    if (0 == vm.count("command")) { // or by vm.empty()
        std::cout << "You didn't specify any subcommand!\n";
        std::exit(1);
    }
    std::string cmd = vm["command"].as<std::string>();

    if (cmd == "along") {
        //
        std::cout << "----------------------------------------------------------------------" << std::endl;
        std::cout << "sub commands -> along                                                 " << std::endl;
        std::cout << "Run info:" << std::endl;

        // along command has the following options:
        po::options_description opt_along("along options");
        opt_along.add_options()
            ("input, i", po::value<std::string>(), "input cube file")
            ("output, o", po::value<std::string>(), "output structure file")
            ("abscissa", po::value<std::vector<std::string> >()->multitoken(), "choose the direction to do the dimension reduction");
        //opts.add_options()
        //    ("input, i", po::value<std::string>(&input), "input structure file")
        //    ("output, o", po::value<std::string>(&output), "output structure file");
        // collect all the unrecognized options from the first pass. this will include the
        // (positional) command name so we need to erase that
        std::vector<std::string> opts = po::collect_unrecognized(parsed.options, po::include_positional);
        opts.erase(opts.begin());
        //parse again...
        po::store(po::command_line_parser(opts).options(opt_along).style(po::command_line_style::unix_style | po::command_line_style::allow_long_disguise).extra_style_parser(&ignore_numbers).run(), vm);
        std::cout << "parse arguments again" << "\n";

        if (vm.count("help")) {
            std::cout << opt_along << std::endl;
            std::exit(1);
        }
        po::notify(vm);

        std::cout << "Preparing to deal with input and output\n";

        if (vm.count("input") && vm.count("output")) {

            std::string input_file = vm["input"].as<std::string>();
            std::string output_file = vm["output"].as<std::string>();

            filesys::path in_path(input_file);
            filesys::path out_path(output_file);

            std::vector<std::string> abscissa = vm["abscissa"].as<std::vector<std::string>>();

            std::cout << "input: " << input_file << std::endl;
            std::cout << "output: " << output_file << std::endl;

            // read structure file
            std::cout << "in_path(extension)->" << in_path.extension().string() << std::endl;

            atomsciflow::CubeElectronDensity cube;
            cube.read_cube(input_file);

            double bohr_to_angstrom = 0.529177249;
            // data dimension reduction
            // the unit of value is actually not physical now!
            // cell_volume are in unit of Angstrom^3
            arma::mat latcell(3, 3);
            latcell.row(0) = arma::conv_to<arma::rowvec>::from(cube.crystal.cell[0]);
            latcell.row(1) = arma::conv_to<arma::rowvec>::from(cube.crystal.cell[1]);
            latcell.row(2) = arma::conv_to<arma::rowvec>::from(cube.crystal.cell[2]);

            std::cout << "latcell:\n";
            std::cout << latcell << std::endl;

            // std::cout << "test value:\n";
            // std::cout << arma::dot(arma::cross(latcell.row(0), latcell.row(1)), latcell.row(2)) << std::endl;

            double cell_volume = arma::dot(arma::cross(latcell.row(0), latcell.row(1)), latcell.row(2));

            double cell_volume_per_unit = cell_volume / (cube.ngridx * cube.ngridy * cube.ngridz);

            //std::cout << "cell volume per unit: " << cell_volume_per_unit << std::endl;

            int ngridx = cube.ngridx;
            int ngridy = cube.ngridy;
            int ngridz = cube.ngridz;
            // value in cube file are \rho(r)_of_electrons in unit of e/Bohr^3
            // namely number of electrons each Borh^3
            // so we have to convert it to e/Angstrom^3, through divide it by borh_to_angstrom**3

            double total_electrons = arma::accu(cube.data)  * cell_volume_per_unit / pow(bohr_to_angstrom, 3);

            std::cout << "======================================================\n";
            std::cout << "           Information collected\n";
            std::cout << "------------------------------------------------------\n";
            std::cout << "cell volume: " << cell_volume << " (A^3)\n";
            std::cout << "total electrons: " << total_electrons << "\n";

            // cube.data is in unit of e/Bohr^3
            // we will build data_red_? to be in unit of e/Anstrom, namely number of electrons per Angstrom
            // to do this we have to time the volume density with bohr_to_angstrom^-3
            std::vector<double> data_red_a;
            std::vector<double> data_red_b;
            std::vector<double> data_red_c;
            //arma::vec data_red_a;
            //arma::vec data_red_b;
            //arma::vec data_red_c;

            double a = arma::norm(latcell.row(0), 2);
            double b = arma::norm(latcell.row(1), 2);
            double c = arma::norm(latcell.row(2), 2);

            if (std::find(abscissa.begin(), abscissa.end(), "c") != abscissa.end()) {
                double len_ci = c / ngridz;
                auto size = arma::size(cube.data);
                double tmp;
                double rho_line;
                for (int ci = 0; ci < size[2]; ci++) {
                    tmp  = 0;
                    for (int bi = 0; bi < size[1]; bi++) {
                        for (int ai = 0; ai < size[0]; ai++) {
                            tmp += cube.data.at(ai, bi, ci);
                        }
                    }
                    rho_line = tmp * cell_volume_per_unit / pow(bohr_to_angstrom, 3) / len_ci;
                    data_red_c.push_back(rho_line);
                }
            }

            if (std::find(abscissa.begin(), abscissa.end(), "b") != abscissa.end())  {
                double len_bi = b / ngridy;
                auto size = arma::size(cube.data);
                double tmp;
                double rho_line;
                for (int bi  = 0; bi < size[1]; bi++) {
                    tmp = 0;
                    for (int ai = 0; ai < size[0]; ai++) {
                        for (int ci = 0; ci < size[2]; ci++) {
                            tmp += cube.data.at(ai, bi, ci);
                        }
                    }
                    rho_line = tmp * cell_volume_per_unit / pow(bohr_to_angstrom, 3) / len_bi;
                    data_red_b.push_back(rho_line);
                }
            }

            if (std::find(abscissa.begin(), abscissa.end(), "a") != abscissa.end())  {
                double len_ai = a / ngridx;
                auto size = arma::size(cube.data);
                double tmp;
                double rho_line;
                for (int ai = 0; ai < size[0]; ai++) {
                    tmp = 0;
                    for (int ci = 0; ci < size[2]; ci++) {
                        for (int bi = 0; bi < size[1]; bi++) {
                            tmp += cube.data.at(ai, bi, ci);
                        }
                    }
                    rho_line = tmp * cell_volume_per_unit / pow(bohr_to_angstrom, 3) / len_ai;
                    data_red_a.push_back(rho_line);
                }
            }

            std::cout << "Preparing to output the data\n";
            // output the data and make the plot
            if (std::find(abscissa.begin(), abscissa.end(), "c") != abscissa.end()) {
                std::ofstream fout;
                fout.open(output_file+".1d.c.data");
                fout << "#c(angstrom) rho(e) (number of electron per Angstrom)\n";
                arma::vec c_coord = arma::linspace(0, c, data_red_c.size());
                for (int i = 0; i < data_red_c.size(); i++) {
                    fout << c_coord.at(i) << " " << data_red_c.at(i) << "\n";
                }
                fout.close();
            }

            if (std::find(abscissa.begin(), abscissa.end(), "b") != abscissa.end()) {
                std::ofstream fout;
                fout.open(output_file+".1d.b.data");
                fout << "#b(angstrom) rho(e) (number of electron per Angstrom)\n";
                arma::vec b_coord = arma::linspace(0, b, data_red_b.size());
                for (int i = 0; i < data_red_b.size(); i++) {
                    fout << b_coord.at(i) << " " << data_red_b.at(i) << "\n";
                }
                fout.close();
            }

            if (std::find(abscissa.begin(), abscissa.end(), "a") != abscissa.end())  {
                std::ofstream fout;
                fout.open(output_file+".1d.a.data");
                fout << "#a(angstrom) rho(e) (number of electron per Angstrom)\n";
                arma::vec a_coord = arma::linspace(0, a, data_red_a.size());
                for (int i = 0; i < data_red_a.size(); i++) {
                    fout << a_coord.at(i) << " " << data_red_a.at(i) << "\n";
                }
                fout.close();
            }

            //std::ofstream fxx;
            //fxx.open("xxx.data");
            //fxx << cube.data << std::endl;
            //fxx.close();

        }

        std::cout << "----------------------------------------------------------------------" << std::endl;
        std::cout << "sub command: along finished!!!                                      " << std::endl;
        std::cout << "----------------------------------------------------------------------" << std::endl;

    } else if (cmd == "diff") {
        //
        std::cout << "----------------------------------------------------------------------" << std::endl;
        std::cout << "sub commands -> diff                                                 " << std::endl;
        std::cout << "Run info:" << std::endl;

        // along command has the following options:
        po::options_description opt_diff("diff options");
        opt_diff.add_options()
            ("input, i", po::value<std::vector<std::string> >()->multitoken(), "input three cube file: TOTAL PART1 PART2")
            ("output, o", po::value<std::string>(), "output structure file")
            ("abscissa", po::value<std::vector<std::string> >()->multitoken(), "choose the direction to do the dimension reduction");
        //opts.add_options()
        //    ("input, i", po::value<std::string>(&input), "input structure file")
        //    ("output, o", po::value<std::string>(&output), "output structure file");
        // collect all the unrecognized options from the first pass. this will include the
        // (positional) command name so we need to erase that
        std::vector<std::string> opts = po::collect_unrecognized(parsed.options, po::include_positional);
        opts.erase(opts.begin());
        //parse again...
        po::store(po::command_line_parser(opts).options(opt_diff).style(po::command_line_style::unix_style | po::command_line_style::allow_long_disguise).extra_style_parser(&ignore_numbers).run(), vm);
        std::cout << "parse arguments again" << "\n";

        if (vm.count("help")) {
            std::cout << opt_diff << std::endl;
            std::exit(1);
        }
        po::notify(vm);

        std::cout << "Preparing to deal with input and output\n";

        if (vm.count("input") && vm.count("output")) {

            // std::string input_file = vm["input"].as<std::string>();
            std::vector<std::string> input_files = vm["input"].as<std::vector<std::string>>();
            std::string output_file = vm["output"].as<std::string>();

            // filesys::path in_path(input_file);
            filesys::path out_path(output_file);

            std::vector<std::string> abscissa = vm["abscissa"].as<std::vector<std::string>>();

            std::cout << "input: \n"
                << input_files[0] << "\n"
                << input_files[1] << "\n"
                << input_files[2] << "\n" << std::endl;

            std::cout << "output: " << output_file << std::endl;

            atomsciflow::cube_diff_1d(input_files, output_file, abscissa);

        }

        std::cout << "----------------------------------------------------------------------" << std::endl;
        std::cout << "sub command: diff finished!!!                                      " << std::endl;
        std::cout << "----------------------------------------------------------------------" << std::endl;

    } else {
        std::cout << "The specified subcommand is not defined!\n";
    }

    //
    return 0;
}