#include "mex.h"
#include <Eigen/Dense>
#include "../radialpose.h"

void print_usage() {
	mexPrintf("[R,t,f,params] = radialpose_mex(x,X,solver,[upgrade_only]);\n");
	mexPrintf(" Solvers:\n");
	mexPrintf("  1 - D(1,0) - 5p -- Larsson et al.  ICCV 2019\n");
	mexPrintf("  2 - D(2,0) - 5p -- Larsson et al.  ICCV 2019\n");
	mexPrintf("  3 - D(3,0) - 5p -- Larsson et al.  ICCV 2019  (Minimal)\n");
	mexPrintf("  4 - D(3,3) - 8p -- Larsson et al.  ICCV 2019\n");
	mexPrintf("  5 - U(1,0) - 5p -- Larsson et al.  ICCV 2019\n");
	mexPrintf("  6 - U(0,1) - 4p -- Larsson et al.  ICCV 2017  (Minimal, Non-planar)\n");
	mexPrintf("  7 - U(0,1) - 4p -- Bujnak et al.   ACCV 2010  (Minimal, Non-planar)\n");
	mexPrintf("  8 - U(0,1) - 5p -- Kukelova et al. ICCV 2013\n");
	mexPrintf("  9 - U(0,2) - 5p -- Kukelova et al. ICCV 2013\n");
	mexPrintf(" 10 - U(0,3) - 5p -- Kukelova et al. ICCV 2013  (Minimal)\n");
	mexPrintf(" 11 - U(0,1) - 4p -- Oskarsson       arxiv 2018 (Minimal, Planar)\n");
	mexPrintf(" 12 - N/A    - 5p -- Kukelova et al. ICCV 2013  (Minimal, 1D Radial)\n\n");
	mexPrintf("If upgrade_only=true we don't solve for the 1D radial camera and only try to upgrade.\n This is only for the two-step solvers.\n");
}

void save_poses(int nlhs, mxArray *plhs[], std::vector<radialpose::Camera> &poses) {
	int n_sols = poses.size();
	int n_params = 0;
	if (n_sols > 0)
		n_params = poses[0].dist_params.size();

	if (nlhs >= 1) {
		plhs[0] = mxCreateDoubleMatrix(9, n_sols, mxREAL);
		double *p = mxGetPr(plhs[0]);
		for (int i = 0; i < n_sols; ++i)
			for (int j = 0; j < 9; ++j)
				p[9 * i + j] = poses[i].R(j);
	}
	if (nlhs >= 2) {
		plhs[1] = mxCreateDoubleMatrix(3, n_sols, mxREAL);
		double *p = mxGetPr(plhs[1]);
		for (int i = 0; i < n_sols; ++i)
			for (int j = 0; j < 3; ++j)
				p[3 * i + j] = poses[i].t(j);
	}
	if (nlhs >= 3) {
		plhs[2] = mxCreateDoubleMatrix(1, n_sols, mxREAL);
		double *p = mxGetPr(plhs[2]);
		for (int i = 0; i < n_sols; ++i)
			p[i] = poses[i].focal;

	}
	if (nlhs >= 4) {
		plhs[3] = mxCreateDoubleMatrix(n_params, n_sols, mxREAL);
		double *p = mxGetPr(plhs[3]);
		for (int i = 0; i < n_sols; ++i)
			for (int j = 0; j < n_params; ++j)
				p[n_params * i + j] = poses[i].dist_params[j];
	}
}

void mexFunction(int nlhs,mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

	if (nrhs < 3 || nrhs > 5) {
		print_usage();
		mexErrMsgTxt("Incorrect number of input arguments.");
	}
	if (nlhs > 4) {
		print_usage();
		mexErrMsgTxt("Wrong number of output arguments.");
	}

	if (mxGetM(prhs[0]) != 2) {
		print_usage();
		mexErrMsgTxt("First input must be 2 x N matrix.");
	}
	if (mxGetM(prhs[1]) != 3) {
		print_usage();
		mexErrMsgTxt("Second input must be 3 x N matrix.");
	}
	if (mxGetN(prhs[0]) != mxGetN(prhs[1])) {
		print_usage();
		mexErrMsgTxt("Not the same number of 2D points and 3D points.");
	}

	bool use_radial_solver = true;
	if (nrhs >= 4 && mxGetScalar(prhs[3]) != 0.0) {
		use_radial_solver = false;
	}

	double damp_factor = 0.0;
	if (nrhs >= 5) {
		damp_factor = mxGetScalar(prhs[4]);
	}

	// If we don't call the radial solver we don't normalize world coord system
	bool center_world_coordinates = use_radial_solver;

	Eigen::Matrix<double, 2, Eigen::Dynamic> x = Eigen::Map<Eigen::Matrix<double, 2, Eigen::Dynamic>>(mxGetPr(prhs[0]), 2, mxGetN(prhs[0]));
	Eigen::Matrix<double, 3, Eigen::Dynamic> X = Eigen::Map<Eigen::Matrix<double, 3, Eigen::Dynamic>>(mxGetPr(prhs[1]), 3, mxGetN(prhs[1]));

	if(x.cols() > 8) {
		// This is to avoid crash when solver is called with more than 8 points
		x = x.block(0,0,2,8).eval();
		X = X.block(0,0,3,8).eval();
	}

	int solver_idx = static_cast<int>(mxGetScalar(prhs[2]));

	std::vector<radialpose::Camera> poses;

	if (solver_idx == 1) { // D(1,0)
		radialpose::larsson_iccv19::Solver<1, 0, true> estimator;
		estimator.use_radial_solver = use_radial_solver;
		estimator.center_world_coord = center_world_coordinates;
		estimator.damp_factor = damp_factor;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 2) { // D(2,0)
		radialpose::larsson_iccv19::Solver<2, 0, true> estimator;
		estimator.use_radial_solver = use_radial_solver;
		estimator.center_world_coord = center_world_coordinates;
		estimator.damp_factor = damp_factor;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 3) { // D(3,0)
		radialpose::larsson_iccv19::Solver<3, 0, true> estimator;
		estimator.use_radial_solver = use_radial_solver;
		estimator.center_world_coord = center_world_coordinates;
		estimator.damp_factor = damp_factor;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 4) { // D(3,3)
		radialpose::larsson_iccv19::Solver<3, 3, true> estimator;
		estimator.use_radial_solver = use_radial_solver;
		estimator.center_world_coord = center_world_coordinates;
		estimator.damp_factor = damp_factor;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 5) { // U(1,0)
		radialpose::larsson_iccv19::Solver<1, 0, false> estimator;
		estimator.use_radial_solver = use_radial_solver;
		estimator.center_world_coord = center_world_coordinates;
		estimator.damp_factor = damp_factor;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 6) { // U(0,1)
		radialpose::larsson_iccv17::NonPlanarSolver estimator;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 7) {  // U(0,1)
		radialpose::bujnak_accv10::NonPlanarSolver estimator;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 8) {  // U(0,1)
		radialpose::kukelova_iccv13::Solver estimator(1);
		estimator.use_radial_solver = use_radial_solver;
		estimator.center_world_coord = center_world_coordinates;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 9) {  // U(0,2)
		radialpose::kukelova_iccv13::Solver estimator(2);
		estimator.use_radial_solver = use_radial_solver;
		estimator.center_world_coord = center_world_coordinates;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 10) { // U(0,3)
		radialpose::kukelova_iccv13::Solver estimator(3);
		estimator.use_radial_solver = use_radial_solver;
		estimator.center_world_coord = center_world_coordinates;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 11) {  // U(0,1)
		radialpose::oskarsson_arxiv18::PlanarSolver estimator;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 12) {  // N/A
		radialpose::kukelova_iccv13::Radial1DSolver estimator;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else {
		print_usage();
		mexErrMsgTxt("Solver NYI.\n");
	}
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "polar_dec.h"
#include "polar_svd.h"
#ifdef _WIN32
#else
#  include <fenv.h>
#endif
#include <cmath>
#include <Eigen/Eigenvalues>
#include <iostream>
#include <cfenv>

// From Olga's CGAL mentee's ARAP code
template <
  typename DerivedA,
  typename DerivedR,
  typename DerivedT,
  typename DerivedU,
  typename DerivedS,
  typename DerivedV>
IGL_INLINE void igl::polar_dec(
  const Eigen::PlainObjectBase<DerivedA> & A,
  Eigen::PlainObjectBase<DerivedR> & R,
  Eigen::PlainObjectBase<DerivedT> & T,
  Eigen::PlainObjectBase<DerivedU> & U,
  Eigen::PlainObjectBase<DerivedS> & S,
  Eigen::PlainObjectBase<DerivedV> & V)
{
  using namespace std;
  using namespace Eigen;
  typedef typename DerivedA::Scalar Scalar;

  const Scalar th = std::sqrt(Eigen::NumTraits<Scalar>::dummy_precision());

  Eigen::SelfAdjointEigenSolver<DerivedA> eig;
  feclearexcept(FE_UNDERFLOW);
  eig.computeDirect(A.transpose()*A);
  if(fetestexcept(FE_UNDERFLOW) || eig.eigenvalues()(0)/eig.eigenvalues()(2)<th)
  {
    cout<<"resorting to svd 1..."<<endl;
    return polar_svd(A,R,T,U,S,V);
  }

  S = eig.eigenvalues().cwiseSqrt();

  V = eig.eigenvectors();
  U = A * V;
  R = U * S.asDiagonal().inverse() * V.transpose();
  T = V * S.asDiagonal() * V.transpose();

  S = S.reverse().eval();
  V = V.rowwise().reverse().eval();
  U = U.rowwise().reverse().eval() * S.asDiagonal().inverse();

  if(R.determinant() < 0)
  {
    // Annoyingly the .eval() is necessary
    auto W = V.eval();
    const auto & SVT = S.asDiagonal() * V.adjoint();
    W.col(V.cols()-1) *= -1.;
    R = U*W.transpose();
    T = W*SVT;
  }

  if(std::fabs(R.squaredNorm()-3.) > th)
  {
    cout<<"resorting to svd 2..."<<endl;
    return polar_svd(A,R,T,U,S,V);
  }
}

template <
  typename DerivedA,
  typename DerivedR,
  typename DerivedT>
IGL_INLINE void igl::polar_dec(
  const Eigen::PlainObjectBase<DerivedA> & A,
  Eigen::PlainObjectBase<DerivedR> & R,
  Eigen::PlainObjectBase<DerivedT> & T)
{
  DerivedA U;
  DerivedA V;
  Eigen::Matrix<typename DerivedA::Scalar,DerivedA::RowsAtCompileTime,1> S;
  return igl::polar_dec(A,R,T,U,S,V);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template  void igl::polar_dec<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::polar_dec<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
#endif


// Copyright (c)  Mingcheng Zuo, Dietmar Wolz.
//
// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory.

// Eigen based implementation of differential evolution (GCL-DE) derived from
// "A case learning-based differential evolution algorithm for global optimization of interplanetary trajectory design,
//  Mingcheng Zuo, Guangming Dai, Lei Peng, Maocai Wang, Zhengquan Liu", https://doi.org/10.1016/j.asoc.2020.106451

#include <Eigen/Core>
#include <iostream>
#include <float.h>
#include <ctime>
#include <random>
#include "pcg_random.hpp"

using namespace std;

typedef Eigen::Matrix<double, Eigen::Dynamic, 1> vec;
typedef Eigen::Matrix<int, Eigen::Dynamic, 1> ivec;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> mat;

typedef void (*callback_parallel)(int, int, double[], double[]);

namespace gcl_differential_evolution {

static uniform_real_distribution<> distr_01 = std::uniform_real_distribution<>(
        0, 1);

static normal_distribution<> gauss_01 = std::normal_distribution<>(0, 1);

static double normreal(pcg64 *rs, double mu, double sdev) {
    return gauss_01(*rs) * sdev + mu;
}

static vec zeros(int n) {
    return Eigen::MatrixXd::Zero(n, 1);
}

static vec constant(int n, double val) {
    return Eigen::MatrixXd::Constant(n, 1, val);
}

static Eigen::MatrixXd uniformVec(int dim, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dim, 1, [&]() {
        return distr_01(rs);
    });
}

static Eigen::MatrixXd normalVec(int dim, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dim, 1, [&]() {
        return gauss_01(rs);
    });
}

static Eigen::MatrixXd uniform(int dx, int dy, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dx, dy, [&]() {
        return distr_01(rs);
    });
}

struct IndexVal {
    int index;
    double val;
};

static bool compareIndexVal(IndexVal i1, IndexVal i2) {
    return (i1.val < i2.val);
}

static ivec sort_index(const vec &x) {
    int size = x.size();
    IndexVal ivals[size];
    for (int i = 0; i < size; i++) {
        ivals[i].index = i;
        ivals[i].val = x[i];
    }
    std::sort(ivals, ivals + size, compareIndexVal);
    return Eigen::MatrixXi::NullaryExpr(size, 1, [&ivals](int i) {
        return ivals[i].index;
    });
}

// wrapper around the fitness function, scales according to boundaries

class Fitness {

public:

    Fitness(callback_parallel func_par_, int dim_, const vec &lower_limit,
            const vec &upper_limit) {
        func_par = func_par_;
        dim = dim_;
        lower = lower_limit;
        upper = upper_limit;
        evaluationCounter = 0;
        if (lower.size() > 0) // bounds defined
            scale = (upper - lower);
    }

    vec getClosestFeasible(const vec &X) const {
        if (lower.size() > 0) {
            return X.cwiseMin(upper).cwiseMax(lower);
        }
        return X;
    }

    void values(const mat &popX, vec &ys) {
        int popsize = popX.cols();
        int n = popX.rows();
        double pargs[popsize * n];
        double res[popsize];
        for (int p = 0; p < popX.cols(); p++) {
            for (int i = 0; i < n; i++)
                pargs[p * n + i] = popX(i, p);
        }
        func_par(popsize, n, pargs, res);
        for (int p = 0; p < popX.cols(); p++)
            ys[p] = res[p];
        evaluationCounter += popsize;
    }

    bool feasible(int i, double x) {
        return lower.size() == 0 || (x >= lower[i] && x <= upper[i]);
    }

    vec sample(pcg64 &rs) {
        if (lower.size() > 0) {
            vec rv = uniformVec(dim, rs);
            return (rv.array() * scale.array()).matrix() + lower;
        } else
            return normalVec(dim, rs);
    }

    double sample_i(int i, pcg64 &rs) {
        if (lower.size() > 0)
            return lower[i] + scale[i] * distr_01(rs);
        else
            return gauss_01(rs);
    }

    int getEvaluations() {
        return evaluationCounter;
    }

private:
    callback_parallel func_par;
    int dim;
    vec lower;
    vec upper;
    long evaluationCounter;
    vec scale;
};

class GclDeOptimizer {

public:

    GclDeOptimizer(long runid_, Fitness *fitfun_, int dim_, int seed_,
            int popsize_, int maxEvaluations_, double pbest_,
            double stopfitness_, double F0_, double CR0_) {
        // runid used to identify a specific run
        runid = runid_;
        // fitness function to minimize
        fitfun = fitfun_;
        // Number of objective variables/problem dimension
        dim = dim_;
        // Population size
        popsize = popsize_ > 0 ? popsize_ : int(dim * 8.5 + 150);
        // maximal number of evaluations allowed.
        maxEvaluations = maxEvaluations_;
        // use low value 0 < pbest <= 1 to narrow search.
        pbest = pbest_;
        // Limit for fitness value.
        stopfitness = stopfitness_;
        F0 = F0_;
        CR0 = CR0_;
        // stop criteria
        stop = 0;
        rs = new pcg64(seed_);
        init();
    }

    ~GclDeOptimizer() {
        delete rs;
    }

    double rnd01() {
        return distr_01(*rs);
    }

    int rndInt(int max) {
        return (int) (max * distr_01(*rs));
    }

    void doOptimize() {
        int gen_stuck = 0;
        vector<vec> sp;

        int maxIter = maxEvaluations / popsize + 1;
        double previous_best = DBL_MAX;
        double CR, F;

        // -------------------- Generation Loop --------------------------------

        for (iterations = 1;; iterations++) {
            // sort population
            ivec sindex = sort_index(nextY);
            popY = nextY(sindex, Eigen::all);
            popX = nextX(Eigen::all, sindex);

            bestX = popX.col(0);
            bestY = popY[0];

            if (isfinite(stopfitness) && bestY < stopfitness) {
                stop = 1;
                return;
            }
            if (bestY == previous_best)
                gen_stuck++;
            else
                gen_stuck = 0;
            previous_best = bestY;

            if (fitfun->getEvaluations() >= maxEvaluations)
                return;
            for (int p = 0; p < popsize; p++) {
                int r1, r2, r3;
                do {
                    r1 = rndInt(popsize);
                } while (r1 == p);
                do {
                    r2 = rndInt(int(popsize * pbest));
                } while (r2 == p || r2 == r1);
                do {
                    r3 = rndInt(popsize + sp.size());
                } while (r3 == p || r3 == r2 || r3 == r1);
                int jr = rndInt(dim);
                //Produce the CR and F
                double mu = 1
                        - sqrt(float(iterations / maxIter))
                                * exp(float(-gen_stuck / iterations));
                if (iterations % 2 == 1) {
                    CR = normreal(rs, 0.95, 0.01);
                    F = normreal(rs, mu, 1);
                    if (F < 0 || F > 1)
                        F = rnd01();
                } else {
                    CR = abs(normreal(rs, CR0, 0.01));
                    F = F0;
                }
                vec ui = popX.col(p);
                for (int j = 0; j < dim; j++) {
                    if (j == jr || rnd01() < CR) {
                        if (r3 < popsize)
                            ui[j] = popX(j, r1)
                                    + F * (popX(j, r2) - popX(j, r3));
                        else
                            ui[j] = popX(j, r1)
                                    + F * ((popX)(j, r2) - sp[r3 - popsize][j]);
                        if (!fitfun->feasible(j, ui[j]))
                            ui[j] = fitfun->sample_i(j, *rs);
                    }
                }
                nextX.col(p) = ui;
            }
            fitfun->values(nextX, nextY);
            for (int p = 0; p < popsize; p++) {
                if (nextY[p] < popY[p]) {
                    if (sp.size() < popsize)
                        sp.push_back(popX.col(p));
                    else
                        sp[rndInt(popsize)] = popX.col(p);
                } else {        // no improvement, copy from parent
                    nextX.col(p) = popX.col(p);
                    nextY[p] = popY[p];
                }
            }
        }
    }

    void init() {
        popCR = zeros(popsize);
        popF = zeros(popsize);
        nextX = mat(dim, popsize);
        for (int p = 0; p < popsize; p++)
            nextX.col(p) = fitfun->sample(*rs);
        nextY = vec(popsize);
        fitfun->values(nextX, nextY);
    }

    vec getBestX() {
        return bestX;
    }

    double getBestValue() {
        return bestY;
    }

    double getIterations() {
        return iterations;
    }

    double getStop() {
        return stop;
    }

private:
    long runid;
    Fitness *fitfun;
    int popsize; // population size
    int dim;
    int maxEvaluations;
    double pbest;
    double stopfitness;
    int iterations;
    double bestY;
    vec bestX;
    int stop;
    double F0;
    double CR0;
    pcg64 *rs;
    mat popX;
    vec popY;
    mat nextX;
    vec nextY;
    vec popCR;
    vec popF;
};
}

using namespace gcl_differential_evolution;

extern "C" {
double* optimizeGCLDE_C(long runid, callback_parallel func_par, int dim,
        int seed, double *lower, double *upper, int maxEvals, double pbest,
        double stopfitness, int popsize, double F0, double CR0) {
    int n = dim;
    double *res = new double[n + 4];
    vec lower_limit(n), upper_limit(n);
    bool useLimit = false;
    for (int i = 0; i < n; i++) {
        lower_limit[i] = lower[i];
        upper_limit[i] = upper[i];
        useLimit |= (lower[i] != 0);
        useLimit |= (upper[i] != 0);
    }
    if (useLimit == false) {
        lower_limit.resize(0);
        upper_limit.resize(0);
    }
    Fitness fitfun(func_par, n, lower_limit, upper_limit);
    GclDeOptimizer opt(runid, &fitfun, dim, seed, popsize, maxEvals, pbest,
            stopfitness, F0, CR0);
    try {
        opt.doOptimize();
        vec bestX = opt.getBestX();
        double bestY = opt.getBestValue();
        for (int i = 0; i < n; i++)
            res[i] = bestX[i];
        res[n] = bestY;
        res[n + 1] = fitfun.getEvaluations();
        res[n + 2] = opt.getIterations();
        res[n + 3] = opt.getStop();
        return res;
    } catch (std::exception &e) {
        cout << e.what() << endl;
        return res;
    }
}
}


// This file is part of:
// gnss-sim: A GNSS Signal Simulator
//
// Copyright (c) 2020 Damien Dusha
// SPDX-License-Identifier: MIT
//
// Derived from https://github.com/osqzss/gps-sdr-sim (MIT Licence):
// Copyright (c) 2015-2020 Takuji Ebinuma

#include "gpssim.h"
#include "gps_channel.h"
#include "gps_time.h"
#include "gps_ephem.h"
#include "gps_math.h"
#include "gps_subframe.h"
#include "geodesy.h"
#include "ionosphere.h"
#include "noise_generator.h"
#include "rinex2_reader.h"
#include "sin_table.h"
#include "sample_writer.h"
#include "satellite_gain.h"

#include <Eigen/Core>

#include <unistd.h>
#include <iostream>

#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cmath>
#include <ctime>

namespace {

static constexpr double kWavelengthGpsL1ca_m = 0.190293672798365;

}   // namespace

/*! \brief Compute range between a satellite and the receiver
 *  \param[out] rho The computed range
 *  \param[in] eph Ephemeris data of the satellite
 *  \param[in] g GPS time at time of receiving the signal
 *  \param[in] e_pos_e_a is the position of the receiver.
 */
range_t ComputeRange(const ephem_t &eph, const ionoutc_t &ionoutc,
                  gpstime_t g, const Eigen::Vector3d &e_pos_e_a)
{
	double pos[3],vel[3],clk[2];
	double los[3];
	double xrot,yrot;

	double neu[3];
	double tmat[3][3];

	// SV position at time of the pseudorange observation.
	satpos(eph, g, pos, vel, clk);

	// Receiver to satellite vector and light-time.
	subVect(los, pos, e_pos_e_a);
	const double tau = normVect(los)/SPEED_OF_LIGHT;

	// Extrapolate the satellite position backwards to the transmission time.
	pos[0] -= vel[0]*tau;
	pos[1] -= vel[1]*tau;
	pos[2] -= vel[2]*tau;

	// Earth rotation correction. The change in velocity can be neglected.
	xrot = pos[0] + pos[1]*OMEGA_EARTH*tau;
	yrot = pos[1] - pos[0]*OMEGA_EARTH*tau;
	pos[0] = xrot;
	pos[1] = yrot;

    range_t rho;

	// New observer to satellite vector and satellite range.
	subVect(los, pos, e_pos_e_a);
	const double range = normVect(los);
	rho.d = range;

	// Pseudorange.
	rho.range = range - SPEED_OF_LIGHT*clk[0];

	// Relative velocity of SV and receiver.
	const double rate = dotProd(vel, los)/range;

	// Pseudorange rate.
	rho.rate = rate; // - SPEED_OF_LIGHT*clk[1];

	// Time of application.
	rho.g = g;

	// Azimuth and elevation angles.
	const GeodeticPosition llh = xyz2llh(e_pos_e_a);
	ltcmat(llh, tmat);
	ecef2neu(los, tmat, neu);
	rho.azel = neu2azel(neu);

	// Add ionospheric delay
	rho.iono_delay = ionosphericDelay(ionoutc, g, llh, rho.azel);
	rho.range += rho.iono_delay;

	return rho;
}

AzimuthElevation ComputeSatelliteAzel(const ephem_t &eph, gpstime_t g,
                        const Eigen::Vector3d &e_pos_e_a)
{
	double neu[3];
	double pos[3],vel[3],clk[3],los[3];
	double tmat[3][3];

	const GeodeticPosition llh = xyz2llh(e_pos_e_a);
	ltcmat(llh, tmat);

	satpos(eph, g, pos, vel, clk);
	subVect(los, pos, e_pos_e_a);
	ecef2neu(los, tmat, neu);
	return neu2azel(neu);
}

int allocateChannel(GpsChannel *chan, ephem_t *eph, int* allocatedSat,
                    ionoutc_t ionoutc, gpstime_t current_simulation_time,
                    const Eigen::Vector3d &e_pos_e_a, double elevation_mask_deg)
{
    int num_visible_sats = 0;

	for (int sv = 0; sv < MAX_SAT; sv++)
	{
        AzimuthElevation azel;
        bool is_visible = false;
        if (eph[sv].valid) {
            azel = ComputeSatelliteAzel(eph[sv], current_simulation_time, e_pos_e_a);
            is_visible = azel.elevation_deg() > elevation_mask_deg;
        }

		if (is_visible)
		{
			num_visible_sats++;
			if (allocatedSat[sv] == -1) // Visible but not allocated
			{
				// Allocated new satellite
                int i;
				for (i = 0; i < MAX_CHAN; i++)
				{
					if (!chan[i].IsEnabled())
					{
						// Initialize channel
						chan[i] = GpsChannel(sv+1);
						chan[i].azel = azel;

						// Generate subframe
						chan[i].SetEphemeris(eph[sv], ionoutc);

						// Generate navigation message
						chan[i].GenerateNavMsg(current_simulation_time, 1);

						// Initialize pseudorange
                        range_t rho = ComputeRange(eph[sv], ionoutc,
                                     current_simulation_time, e_pos_e_a);
						chan[i].rho0 = rho;

						// Initialize carrier phase
						const double r_xyz = rho.range;

						rho = ComputeRange(eph[sv], ionoutc,
                                     current_simulation_time,
                                     Eigen::Vector3d::Zero());
						const double r_ref = rho.range;

						const double phase_ini =
                                (2.0*r_ref - r_xyz) * (1.0 / kWavelengthGpsL1ca_m);
						chan[i].carrier_phase_cycles = phase_ini - floor(phase_ini);

						// Done.
						break;
					}
				}

				// Set satellite allocation channel
				if (i < MAX_CHAN)
					allocatedSat[sv] = i;
			}
		}
		else if (allocatedSat[sv] >= 0) // Not visible but allocated
		{
			// Clear channel
			chan[allocatedSat[sv]].prn = 0;

			// Clear satellite allocation flag
			allocatedSat[sv] = -1;
		}
	}

	return num_visible_sats;
}

void usage(void)
{
	fprintf(stderr, "Usage: gps-sdr-sim [options]\n"
		"Options:\n"
		"  -e <gps_nav>     RINEX navigation file for GPS ephemerides (required)\n"
		"  -c <location>    ECEF X,Y,Z in meters (static mode) e.g. 3967283.154,1022538.181,4872414.484\n"
		"  -l <location>    Lat,Lon,Hgt (static mode) e.g. 35.681298,139.766247,10.0\n"
		"  -t <date,time>   Scenario start time YYYY/MM/DD,hh:mm:ss\n"
		"  -d <duration>    Duration [sec] (static mode max: %d)\n"
		"  -o <output>      I/Q sampling data file (default: gpssim.bin)\n"
		"  -s <frequency>   Sampling frequency [Hz] (default: 2600000)\n"
		"  -i               Disable ionospheric delay for spacecraft scenario\n"
		"  -v               Show details about simulated channels\n",
		STATIC_MAX_DURATION);

	return;
}

/// \brief Checks whether the sample frequency has an integer number of ns ticks.
bool SampleFrequencyIsValid(int sample_frequency) {
    constexpr int ticks_per_ns = 1000000000;
    int period_ns = ticks_per_ns / sample_frequency;
    return period_ns * sample_frequency == ticks_per_ns;
}

int main(int argc, char *argv[])
{
	clock_t tstart,tend;

	ephem_t eph[EPHEM_ARRAY_SIZE][MAX_SAT];

	gpstime_t simulation_start_gps_time;
    gpstime_t current_simulation_time;

    // Default static location; Tokyo
	GeodeticPosition llh = GeodeticPosition::FromDegrees(35.681298, 139.766247, 10.0);

	GpsChannel chan[MAX_CHAN];
	double elevation_mask_deg = 0.0;

    // User position in ECEF coordinates.
    Eigen::Vector3d e_pos_e_a;

    std::string rinex2_nav_file;
    std::string output_sample_filename = {"gpssim.bin"};

	int result;

	datetime_t t0,tmin,tmax;
	gpstime_t gmin,gmax;

	bool verbose = false;

	ionoutc_t ionoutc;

    int allocatedSat[MAX_SAT];

    NoiseGenerator noise_generator(1.0, 0.20, 0.5);

	////////////////////////////////////////////////////////////
	// Read options
	////////////////////////////////////////////////////////////

	// Default options
	double raw_samp_freq = 2.6e6;
	simulation_start_gps_time.week = -1; // Invalid start time
	double duration = 300;     // 5 minutes.
	ionoutc.enable = true;

	if (argc<3)
	{
		usage();
		exit(1);
	}

	while ((result=getopt(argc,argv,"e:u:g:c:l:o:s:t:d:iv"))!=-1)
	{
		switch (result)
		{
		case 'e':
            rinex2_nav_file.assign(optarg);
			break;
		case 'c':
        {
			// Static ECEF coordinates input mode
            double xyz[3];
			sscanf(optarg,"%lf,%lf,%lf", &xyz[0], &xyz[1], &xyz[2]);
            e_pos_e_a = {xyz[0], xyz[1], xyz[2]};
			break;
        }
		case 'l':
        {
			// Static geodetic coordinates input mode.
            double raw_lat, raw_lon, raw_height;
			sscanf(optarg,"%lf,%lf,%lf",&raw_lat, &raw_lon, &raw_height);
            llh = GeodeticPosition::FromDegrees(raw_lat, raw_lon, raw_height);
			e_pos_e_a = llh2xyz(llh);
			break;
        }
		case 'o':
			output_sample_filename.assign(optarg);
			break;
		case 's':
			raw_samp_freq = atof(optarg);
			if (raw_samp_freq < 1.0e6)
			{
				fprintf(stderr, "ERROR: Invalid sampling frequency.\n");
				exit(1);
			}
			break;
		case 't':
			sscanf(optarg, "%d/%d/%d,%d:%d:%lf", &t0.y, &t0.m, &t0.d, &t0.hh, &t0.mm, &t0.sec);
			if (t0.y<=1980 || t0.m<1 || t0.m>12 || t0.d<1 || t0.d>31 ||
				t0.hh<0 || t0.hh>23 || t0.mm<0 || t0.mm>59 || t0.sec<0.0 || t0.sec>=60.0)
			{
				fprintf(stderr, "ERROR: Invalid date and time.\n");
				exit(1);
			}
			t0.sec = floor(t0.sec);
			simulation_start_gps_time = date2gps(t0);
			break;
		case 'd':
			duration = atof(optarg);
			break;
		case 'i':
			ionoutc.enable = false; // Disable ionospheric correction
			break;
		case 'v':
			verbose = true;
			break;
		case ':':
		case '?':
			usage();
			exit(1);
		default:
			break;
		}
	}

	if (rinex2_nav_file.empty())
	{
		fprintf(stderr, "ERROR: GPS ephemeris file is not specified.\n");
		exit(1);
	}

	if (duration < 0.0 || duration > STATIC_MAX_DURATION)
	{
		fprintf(stderr, "ERROR: Invalid duration.\n");
		exit(1);
	}

    SampleWriter sample_writer;
    if (sample_writer.OpenFile(output_sample_filename))
    {
        fprintf(stderr, "ERROR: Failed to open output sample file \"%s\".\n", output_sample_filename.c_str());
    }

    const int sample_freq_hz = std::lround(raw_samp_freq);
    if (!SampleFrequencyIsValid(sample_freq_hz)) {
        fprintf(stderr, "ERROR: Sample frequency %f not an integer divisor of 1GHz\n",
                raw_samp_freq);
        exit(1);
    }

    const double sample_period_s = 1.0 / raw_samp_freq;
    const int sample_period_ns = 1000000000 / sample_freq_hz;

    // Currently, samples are processed in batches of 1ms
    const double batch_period_s = 0.001;
    const int num_sample_batches = std::lround(duration / batch_period_s);
    const int samples_per_batch = std::lround(
            (1e9 * batch_period_s) / sample_period_ns);

	////////////////////////////////////////////////////////////
	// Read ephemeris
	////////////////////////////////////////////////////////////

	const int neph = readRinexNavAll(eph, &ionoutc, rinex2_nav_file);

	if (neph==0)
	{
		fprintf(stderr, "ERROR: No ephemeris available.\n");
		exit(1);
	}

	if (verbose && ionoutc.valid)
	{
		fprintf(stderr, "  %12.3e %12.3e %12.3e %12.3e\n",
			ionoutc.alpha0, ionoutc.alpha1, ionoutc.alpha2, ionoutc.alpha3);
		fprintf(stderr, "  %12.3e %12.3e %12.3e %12.3e\n",
			ionoutc.beta0, ionoutc.beta1, ionoutc.beta2, ionoutc.beta3);
		fprintf(stderr, "   %19.11e %19.11e  %9d %9d\n",
			ionoutc.A0, ionoutc.A1, ionoutc.tot, ionoutc.wnt);
		fprintf(stderr, "%6d\n", ionoutc.dtls);
	}

	for (int sv = 0; sv < MAX_SAT; sv++)
	{
		if (eph[0][sv].valid)
		{
			gmin = eph[0][sv].toc;
			tmin = eph[0][sv].t;
			break;
		}
	}

	gmax.sec = 0;
	gmax.week = 0;
	tmax.sec = 0;
	tmax.mm = 0;
	tmax.hh = 0;
	tmax.d = 0;
	tmax.m = 0;
	tmax.y = 0;
	for (int sv = 0; sv < MAX_SAT; sv++)
	{
		if (eph[neph-1][sv].valid)
		{
			gmax = eph[neph-1][sv].toc;
			tmax = eph[neph-1][sv].t;
			break;
		}
	}

	// Scenario start time has been set.
	if (simulation_start_gps_time.week >= 0)
	{
		if (subGpsTime(simulation_start_gps_time, gmin) < 0.0 ||
            subGpsTime(gmax, simulation_start_gps_time) < 0.0)
		{
			fprintf(stderr, "ERROR: Invalid start time.\n");
			fprintf(stderr, "tmin = %4d/%02d/%02d,%02d:%02d:%02.0f (%d:%.0f)\n",
				tmin.y, tmin.m, tmin.d, tmin.hh, tmin.mm, tmin.sec,
				gmin.week, gmin.sec);
			fprintf(stderr, "tmax = %4d/%02d/%02d,%02d:%02d:%02.0f (%d:%.0f)\n",
				tmax.y, tmax.m, tmax.d, tmax.hh, tmax.mm, tmax.sec,
				gmax.week, gmax.sec);
			exit(1);
		}
	}
	else
	{
		simulation_start_gps_time = gmin;
		t0 = tmin;
	}

	fprintf(stderr, "Start time = %4d/%02d/%02d,%02d:%02d:%02.0f (%d:%.0f)\n",
		t0.y, t0.m, t0.d, t0.hh, t0.mm, t0.sec, simulation_start_gps_time.week,
         simulation_start_gps_time.sec);
	fprintf(stderr, "Duration = %.1f [sec]\n", duration);

	// Select the current set of ephemerides
	int ieph = -1;

	for (int i = 0; i < neph; i++)
	{
		for (int sv = 0; sv < MAX_SAT; sv++)
		{
			if (eph[i][sv].valid)
			{
				const double dt = subGpsTime(simulation_start_gps_time, eph[i][sv].toc);
				if (dt>=-SECONDS_IN_HOUR && dt<SECONDS_IN_HOUR)
				{
					ieph = i;
					break;
				}
			}
		}

		if (ieph>=0) // ieph has been set
			break;
	}

	if (ieph == -1)
	{
		fprintf(stderr, "ERROR: No current set of ephemerides has been found.\n");
		exit(1);
	}

	////////////////////////////////////////////////////////////
	// Initialize channels
	////////////////////////////////////////////////////////////

	// Clear satellite allocation flag
	for (int sv = 0; sv < MAX_SAT; sv++) {
		allocatedSat[sv] = -1;
    }

	// Initial reception time
	current_simulation_time = incGpsTime(simulation_start_gps_time, 0.0);

	// Allocate visible satellites
	allocateChannel(chan, eph[ieph], allocatedSat, ionoutc, current_simulation_time,
                    e_pos_e_a, elevation_mask_deg);

	for(int i = 0; i < MAX_CHAN; i++)
	{
		if (chan[i].IsEnabled()) {
			fprintf(stderr, "%02d %6.1f %5.1f %11.1f %5.1f\n", chan[i].prn,
				chan[i].azel.azimuth_deg(), chan[i].azel.elevation_deg(), chan[i].
				rho0.d, chan[i].rho0.iono_delay);
        }
	}

	////////////////////////////////////////////////////////////
	// Generate baseband signals
	////////////////////////////////////////////////////////////

	tstart = clock();

	// Update receiver time
	current_simulation_time = incGpsTime(current_simulation_time, batch_period_s);

    PeriodicFunctionTable<double, 1024> sin_table(
        [](double t) -> double {return std::sin(t);}, 1.0);
    PeriodicFunctionTable<double, 1024> cos_table(
        [](double t) -> double {return std::cos(t);}, 1.0);

    // Default all channels to a gain of 1.
    ConstellationGain constellation_gain;
    for (int prn = 0; prn <= 32; ++prn) {
        constellation_gain.SetSatelliteToConstantGain(prn, 1.0);
    }

    // Successively decrease the gain as the PRN increases.
    constellation_gain.SetSatelliteToConstantGain( 1, 1.0);
    constellation_gain.SetSatelliteToConstantGain( 2, 0.9);
    constellation_gain.SetSatelliteToConstantGain( 3, 0.7);
    constellation_gain.SetSatelliteToConstantGain( 6, 0.6);
    constellation_gain.SetSatelliteToConstantGain( 9, 0.5);
    constellation_gain.SetSatelliteToConstantGain(10, 0.4);
    constellation_gain.SetSatelliteToConstantGain(11, 0.3);
    constellation_gain.SetSatelliteToConstantGain(12, 0.2);
    constellation_gain.SetSatelliteToConstantGain(17, 0.17);
    constellation_gain.SetSatelliteToConstantGain(20, 0.15);
    constellation_gain.SetSatelliteToConstantGain(23, 0.13);
    constellation_gain.SetSatelliteToConstantGain(28, 0.10);
    constellation_gain.SetSatelliteToConstantGain(32, 0.07);

    std::cout << "Num batches = " << num_sample_batches << std::endl;
    std::cout << "samples_per_batch = " << samples_per_batch << std::endl;

	for (int batch = 1; batch < num_sample_batches; batch++)
	{
        // Per-channel gain, maximum value of 1.
        std::array<double, MAX_CHAN> gain;

		for (int i = 0; i < MAX_CHAN; i++)
		{
			if (chan[i].IsEnabled())
			{
				// Refresh code phase and data bit counters
				const int sv_index = chan[i].prn-1;

				// Current pseudorange
				const range_t rho = ComputeRange(eph[ieph][sv_index], ionoutc,
                             current_simulation_time, e_pos_e_a);

				chan[i].azel = rho.azel;

				// Update code phase and data bit counters
				chan[i].ComputeCodePhase(rho, batch_period_s);

				// Signal gain.
				gain[i] = constellation_gain.ComputeGain(
                    chan[i].prn, rho.d, rho.azel);
			}
		}

		// There is no intrinsic need to loop here, but we do so to avoid computing
		// the pseudorange too frequently.
		for (int sample = 0; sample < samples_per_batch; sample++)
		{
			double i_acc = 0;
			double q_acc = 0;

			for (int i = 0; i < MAX_CHAN; i++)
			{
				if (chan[i].IsEnabled())
				{
                    const double coeff =
                            chan[i].current_code_data_symbol() * gain[i];
                    const double ip = coeff *
                            cos_table.LookupValue(chan[i].carrier_phase_cycles);
                    const double qp = coeff *
                            sin_table.LookupValue(chan[i].carrier_phase_cycles);

					// Accumulate for all visible satellites.
					i_acc += ip;
					q_acc += qp;

					// Update code phase, including the code chip and data bit.
					chan[i].UpdateCodePhase(sample_period_s);

					// Update carrier phase
					chan[i].UpdateCarrierPhase(sample_period_s);
				}
			}

            // At this point, the min and max values for the I- and Q- samples
            // are -16 and +16 based on the number of satellites processed.
            //
            // We scale them to +/- 0.5 for the sample writer.
            constexpr double kSampleScale = 1.0 / (2.0 * 16.0);
            sample_writer.WriteSample(
                    noise_generator.ScaleAndAddNoise(i_acc * kSampleScale),
                    noise_generator.ScaleAndAddNoise(q_acc * kSampleScale));
        }

        //
        // Update navigation message and channel allocation on 30s boundaries.
        //

        if (current_simulation_time.On30sBoundary())
		{
			// Update navigation message
			for (int i = 0; i < MAX_CHAN; i++)
			{
                if (chan[i].prn>0) {
                    chan[i].GenerateNavMsg(current_simulation_time, 0);
                }
			}

			// Refresh ephemeris and subframes
			// Quick and dirty fix. Need more elegant way.
			for (int sv = 0; sv < MAX_SAT; sv++)
			{
				if (eph[ieph+1][sv].valid)
				{
					const double dt = subGpsTime(eph[ieph+1][sv].toc,
                                                 current_simulation_time);
					if (dt < SECONDS_IN_HOUR)
					{
						ieph++;
						for (int i = 0; i < MAX_CHAN; i++)
						{
							// Generate new subframes if allocated
							if (chan[i].IsEnabled()) {
								chan[i].SetEphemeris(eph[ieph][chan[i].prn-1], ionoutc);
                            }
						}
					}

					break;
				}
			}

			// Update channel allocation
			allocateChannel(chan, eph[ieph], allocatedSat, ionoutc,
                            current_simulation_time, e_pos_e_a, elevation_mask_deg);

			// Show details about simulated channels
			if (verbose)
			{
				fprintf(stderr, "\n");
				for (int i = 0; i < MAX_CHAN; i++)
				{
					if (chan[i].prn>0)
						fprintf(stderr, "%02d %6.1f %5.1f %11.1f %5.1f\n", chan[i].prn,
							chan[i].azel.azimuth_deg(), chan[i].azel.elevation_deg(),
                            chan[i].rho0.d, chan[i].rho0.iono_delay);
				}
			}
		}

		// Update receiver time
		current_simulation_time = incGpsTime(current_simulation_time, batch_period_s);

		// Update time counter
		fprintf(stderr, "\rTime into run = %4.2f",
                subGpsTime(current_simulation_time, simulation_start_gps_time));
		fflush(stdout);
	}

	tend = clock();

	fprintf(stderr, "\nDone!\n");

	// Process time
	fprintf(stderr, "Process time = %.1f [sec]\n", (double)(tend-tstart)/CLOCKS_PER_SEC);

	return(0);
}


#include <iostream>
#include <vector>
#include <boost/polygon/voronoi.hpp>
#include <boost/polygon/polygon.hpp>

typedef boost::polygon::point_data<int> Point;
typedef boost::polygon::segment_data<int> Segment;

int main(int argc, char** argv)
{
    std::vector<Point> points;
    std::vector<Segment> segments;
    points.push_back(Point(0, 0));
    points.push_back(Point(1, 6));
    segments.push_back(Segment(Point(-4, 5), Point(5, -1)));
    segments.push_back(Segment(Point(3, -11), Point(13, -1)));
    boost::polygon::voronoi_diagram<double> vd;
    boost::polygon::construct_voronoi(
        points.begin(),
        points.end(),
        segments.begin(),
        segments.end(),
        &vd);
    return 0;
}


#include <Eigen/Sparse>
// system includes ------------------------------------------------------------
#include <algorithm>
#include <boost/program_options.hpp>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>
// own includes ---------------------------------------------------------------
#include "spectral/basis/spectral_basis_dimension_accessor.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/spectral_elem_accessor.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"

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

const int dim = 2;

int main(int argc, char *argv[])
{
  int K;
  double beta;
  bool sorted;
  po::options_description options("options");
  options.add_options()
      ("help", "produce help message")("K", po::value<int>(&K)->default_value(10))
      ("sorted", po::value<bool>(&sorted)->default_value(true))
      ("beta", po::value<double>(&beta)->default_value(2));
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << options << "\n";
    return 1;
  }

  // ------------------------------------------------------------
  typedef SpectralBasisFactoryKS::basis_type basis_type;
  basis_type trial_basis;
  SpectralBasisFactoryKS::create(trial_basis, K, K, beta, sorted);
  typedef typename basis_type::elem_t elem_t;

  // write to disk
  SpectralBasisFactoryKS::write_basis_descriptor(trial_basis, "spectral_basis.desc");
  basis_type test_basis;
  SpectralBasisFactoryKS::create_test(test_basis, K, K, beta, sorted);
  SpectralBasisFactoryKS::write_basis_descriptor(test_basis, "spectral_basis_test.desc");

#if __cplusplus >= 201402
  auto range = spectral::filter_freq(test_basis.begin(), test_basis.end(), 1);

  for (auto it = std::get<0>(range); it != std::get<1>(range); it++) {
    cout << it->id().to_string() << std::endl;
  }
#endif

  return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Mageswaran.D <mageswaran1989@gmail.com>
//
// Book Refered: OpenCL Programming Guide
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

//---------------------------------------------------------------------------//
// About Sobel Filter:
// * Edge Filter - distinguishes the differrent color region
// * Finds the gradient in x and y-axes
// * Three step process
//   -> Find x-axis gradient with kernel/matrix
//           Gx = [-1 0 +1]
//                [-2 0 +2]
//                [-1 0 +1]
//   -> Find y-axis gradient with kernel/matrix
//           Gy = [-1 -2 -1]
//                [ 0  0  0]
//                [+1 +2 +1]
// * Gradient magnitude G = sqrt(Gx^2 + Gy^2)
//---------------------------------------------------------------------------//

#include <iostream>
#include <string>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/interop/opencv/core.hpp>
#include <boost/compute/interop/opencv/highgui.hpp>
#include <boost/compute/utility/source.hpp>

#include <boost/program_options.hpp>

namespace compute = boost::compute;
namespace po = boost::program_options;

// Create sobel filter program
const char source[] = BOOST_COMPUTE_STRINGIZE_SOURCE (
    //For out of boundary pixels, edge pixel
    // value is returned
    const sampler_t sampler = CLK_ADDRESS_CLAMP_TO_EDGE |
                              CLK_FILTER_NEAREST;
    kernel void sobel_rgb(read_only image2d_t src, write_only image2d_t dst)
    {
        int x = (int)get_global_id(0);
        int y = (int)get_global_id(1);

        if (x >= get_image_width(src) || y >= get_image_height(src))
                return;

        //  [(x-1, y+1), (x, y+1), (x+1, y+1)]
        //  [(x-1, y  ), (x, y  ), (x+1, y  )]
        //  [(x-1, y-1), (x, y-1), (x+1, y-1)]

        //  [p02, p12,   p22]
        //  [p01, pixel, p21]
        //  [p00, p10,   p20]

        //Basically finding influence of neighbour pixels on current pixel
        float4 p00 = read_imagef(src, sampler, (int2)(x - 1, y - 1));
        float4 p10 = read_imagef(src, sampler, (int2)(x,     y - 1));
        float4 p20 = read_imagef(src, sampler, (int2)(x + 1, y - 1));

        float4 p01 = read_imagef(src, sampler, (int2)(x - 1, y));
        //pixel that we are working on
        float4 p21 = read_imagef(src, sampler, (int2)(x + 1, y));

        float4 p02 = read_imagef(src, sampler, (int2)(x - 1, y + 1));
        float4 p12 = read_imagef(src, sampler, (int2)(x,     y + 1));
        float4 p22 = read_imagef(src, sampler, (int2)(x + 1, y + 1));

        //Find Gx = kernel + 3x3 around current pixel
        //           Gx = [-1 0 +1]     [p02, p12,   p22]
        //                [-2 0 +2]  +  [p01, pixel, p21]
        //                [-1 0 +1]     [p00, p10,   p20]
        float3 gx = -p00.xyz + p20.xyz +
                    2.0f * (p21.xyz - p01.xyz)
                    -p02.xyz + p22.xyz;

        //Find Gy = kernel + 3x3 around current pixel
        //           Gy = [-1 -2 -1]     [p02, p12,   p22]
        //                [ 0  0  0]  +  [p01, pixel, p21]
        //                [+1 +2 +1]     [p00, p10,   p20]
        float3 gy = p00.xyz + p20.xyz +
                    2.0f * (- p12.xyz + p10.xyz) -
                    p02.xyz - p22.xyz;
        //Find G
        float3 g = native_sqrt(gx * gx + gy * gy);

        // we could also approximate this as g = fabs(gx) + fabs(gy)
        write_imagef(dst, (int2)(x, y), (float4)(g.x, g.y, g.z, 1.0f));
    }
);

// This example shows how to apply sobel filter on images or on camera frames
// with OpenCV, transfer the frames to the GPU, and apply a sobel filter
// written in OpenCL
int main(int argc, char *argv[])
{
    ///////////////////////////////////////////////////////////////////////////

    // setup the command line arguments
    po::options_description desc;
    desc.add_options()
            ("help",  "show available options")
            ("camera", po::value<int>()->default_value(-1),
                                 "if not default camera, specify a camera id")
            ("image", po::value<std::string>(), "path to image file");

    // Parse the command lines
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    //check the command line arguments
    if(vm.count("help"))
    {
        std::cout << desc << std::endl;
        return 0;
    }

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

    //OpenCV variables
    cv::Mat cv_mat;
    cv::VideoCapture cap; //OpenCV camera handle.

    //OpenCL variables
    // Get default device and setup context
    compute::device gpu = compute::system::default_device();
    compute::context context(gpu);
    compute::command_queue queue(context, gpu);
    compute::program filter_program =
            compute::program::create_with_source(source, context);

    try
    {
        filter_program.build();
    }
    catch(compute::opencl_error e)
    {
        std::cout<<"Build Error: "<<std::endl
                 <<filter_program.build_log();
    }

    // create fliter kernel and set arguments
    compute::kernel filter_kernel(filter_program, "sobel_rgb");

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

    //check for image paths
    if(vm.count("image"))
    {
        // Read image with OpenCV
        cv_mat = cv::imread(vm["image"].as<std::string>(),
                                       CV_LOAD_IMAGE_COLOR);
        if(!cv_mat.data){
            std::cerr << "Failed to load image" << std::endl;
            return -1;
        }
    }
    else //by default use camera
    {
        //open camera
        cap.open(vm["camera"].as<int>());
        // read first frame
        cap >> cv_mat;
        if(!cv_mat.data){
            std::cerr << "failed to capture frame" << std::endl;
            return -1;
        }
    }

    // Convert image to BGRA (OpenCL requires 16-byte aligned data)
    cv::cvtColor(cv_mat, cv_mat, CV_BGR2BGRA);

    // Transfer image/frame data to gpu
    compute::image2d dev_input_image =
            compute::opencv_create_image2d_with_mat(
                cv_mat, compute::image2d::read_write, queue
                );

    // Create output image
    // Be sure what will be your ouput image/frame size
    compute::image2d dev_output_image(
                context,
                dev_input_image.width(),
                dev_input_image.height(),
                dev_input_image.format(),
                compute::image2d::write_only
                );

    filter_kernel.set_arg(0, dev_input_image);
    filter_kernel.set_arg(1, dev_output_image);

    // run flip kernel
    size_t origin[2] = { 0, 0 };
    size_t region[2] = { dev_input_image.width(),
                         dev_input_image.height() };

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

    queue.enqueue_nd_range_kernel(filter_kernel, 2, origin, region, 0);

    //check for image paths
    if(vm.count("image"))
    {
        // show host image
        cv::imshow("Original Image", cv_mat);

        // show gpu image
        compute::opencv_imshow("Filtered Image", dev_output_image, queue);

        // wait and return
        cv::waitKey(0);
    }
    else
    {
        char key = '\0';
        while(key != 27) //check for escape key
        {
            cap >> cv_mat;

            // Convert image to BGRA (OpenCL requires 16-byte aligned data)
            cv::cvtColor(cv_mat, cv_mat, CV_BGR2BGRA);

            // Update the device image memory with current frame data
            compute::opencv_copy_mat_to_image(cv_mat,
                                              dev_input_image,queue);

            // Run the kernel on the device
            queue.enqueue_nd_range_kernel(filter_kernel, 2, origin, region, 0);

            // Show host image
            cv::imshow("Camera Frame", cv_mat);

            // Show GPU image
            compute::opencv_imshow("Filtered RGB Frame", dev_output_image, queue);

            // wait
            key = cv::waitKey(10);
        }
    }
    return 0;
}


/*
 * @file
 * @author University of Warwick
 * @version 1.0
 *
 * @section LICENSE
 *
 * @section DESCRIPTION
 *
 */

#define BOOST_TEST_MODULE EuclideanVector
#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>
#include <stdexcept>

#include "EuclideanVector.h"
#include "EuclideanVector3D.h"
#include "Error.h"
#include "Broadcast.h"
#include <iostream>

namespace utf = boost::unit_test;

using namespace cupcfd::geometry::euclidean;

// Setup
BOOST_AUTO_TEST_CASE(setup)
{
    int argc = boost::unit_test::framework::master_test_suite().argc;
    char ** argv = boost::unit_test::framework::master_test_suite().argv;

    MPI_Init(&argc, &argv);
}

// === Constructor1 ===
// Test 1: Check all values set to zero - 2D
BOOST_AUTO_TEST_CASE(constructor1_test1)
{
	EuclideanVector<double, 2> vec;

	BOOST_CHECK_EQUAL(vec.cmp[0], 0.0);
	BOOST_CHECK_EQUAL(vec.cmp[1], 0.0);
	BOOST_CHECK_EQUAL(vec.isRegistered(), false);
}

// Test 2: Check all values set to zero - 3D
BOOST_AUTO_TEST_CASE(constructor1_test2)
{
	EuclideanVector<double, 3> vec;

	BOOST_CHECK_EQUAL(vec.cmp[0], 0.0);
	BOOST_CHECK_EQUAL(vec.cmp[1], 0.0);
	BOOST_CHECK_EQUAL(vec.cmp[2], 0.0);
	BOOST_CHECK_EQUAL(vec.isRegistered(), false);
}

// === Constructor2 ===
// Test 1: Check values set to match parameters - 2D
BOOST_AUTO_TEST_CASE(constructor2_test1)
{
	EuclideanVector<double, 2> vec(2.0, 3.0);

	BOOST_CHECK_EQUAL(vec.cmp[0], 2.0);
	BOOST_CHECK_EQUAL(vec.cmp[1], 3.0);
	BOOST_CHECK_EQUAL(vec.isRegistered(), false);
}

// Test 2: Check values set to match parameters - 3D
BOOST_AUTO_TEST_CASE(constructor2_test2)
{
	EuclideanVector<double, 3> vec(2.0, 3.0, 4.0);

	BOOST_CHECK_EQUAL(vec.cmp[0], 2.0);
	BOOST_CHECK_EQUAL(vec.cmp[1], 3.0);
	BOOST_CHECK_EQUAL(vec.cmp[2], 4.0);
	BOOST_CHECK_EQUAL(vec.isRegistered(), false);
}

// === Operator= Vector ===
// Test 1: Check values are appropriately copied form src to dst - 2D
BOOST_AUTO_TEST_CASE(operator_assign_vector_test1)
{
	EuclideanVector<double, 2> srcVec(2.0, 3.0);
	EuclideanVector<double, 2> dstVec;

	dstVec = srcVec;

	srcVec.cmp[1] = -15.0;

	BOOST_CHECK_EQUAL(dstVec.cmp[0], 2.0);
	BOOST_CHECK_EQUAL(dstVec.cmp[1], 3.0);
}

// Test 2: Check values are appropriately copied form src to dst - 3D
BOOST_AUTO_TEST_CASE(operator_assign_vector_test2)
{
	EuclideanVector<double, 3> srcVec(2.0, 3.0, 4.0);
	EuclideanVector<double, 3> dstVec;

	dstVec = srcVec;

	srcVec.cmp[1] = -15.0;

	BOOST_CHECK_EQUAL(dstVec.cmp[0], 2.0);
	BOOST_CHECK_EQUAL(dstVec.cmp[1], 3.0);
	BOOST_CHECK_EQUAL(dstVec.cmp[2], 4.0);
}

// === Operator= Scalar ===
// Test 1: Check values are all set to same as scalar - 2D
BOOST_AUTO_TEST_CASE(operator_assign_scalar_test1)
{
	EuclideanVector<double, 2> vec;

	vec = 5.0;

	BOOST_CHECK_EQUAL(vec.cmp[0], 5.0);
	BOOST_CHECK_EQUAL(vec.cmp[1], 5.0);
}

// Test 2: Check values are all set to same as scalar - 3D
BOOST_AUTO_TEST_CASE(operator_assign_scalar_test2)
{
	EuclideanVector<double, 3> vec;

	vec = 5.0;

	BOOST_CHECK_EQUAL(vec.cmp[0], 5.0);
	BOOST_CHECK_EQUAL(vec.cmp[1], 5.0);
	BOOST_CHECK_EQUAL(vec.cmp[2], 5.0);
}

// === operator== ===
// Test 1: Test that vecs match
BOOST_AUTO_TEST_CASE(operator_equivalence_test1)
{
	EuclideanVector<double, 3> vec(1.0, 2.0, 3.0);
	EuclideanVector<double, 3> vec2(1.0, 2.0, 3.0);

	BOOST_CHECK_EQUAL(vec == vec2, true);
}

// Test 2: Test that vecs don't match
BOOST_AUTO_TEST_CASE(operator_equivalence_test2)
{
	EuclideanVector<double, 3> vec(1.0, 2.0, 3.0);
	EuclideanVector<double, 3> vec2(2.0, 2.0, 3.0);
	EuclideanVector<double, 3> vec3(1.0, 3.0, 3.0);
	EuclideanVector<double, 3> vec4(1.0, 2.0, 4.0);

	BOOST_CHECK_EQUAL(vec == vec2, false);
	BOOST_CHECK_EQUAL(vec == vec3, false);
	BOOST_CHECK_EQUAL(vec == vec4, false);
}

// === Operator+ Vector ===
// Test 1: Check values are appropriately summed - 2D
BOOST_AUTO_TEST_CASE(operator_add_vector_test1)
{
	EuclideanVector<double, 2> vec1(5.0, 8.0);
	EuclideanVector<double, 2> vec2(1.2, 4.5);

	EuclideanVector<double, 2> vec3 = vec1 + vec2;

	BOOST_CHECK_EQUAL(vec3.cmp[0], 6.2);
	BOOST_CHECK_EQUAL(vec3.cmp[1], 12.5);
}

// Test 2: Check values are appropriately summed - 3D
BOOST_AUTO_TEST_CASE(operator_add_vector_test2)
{
	EuclideanVector<double, 3> vec1(5.0, 8.0, 9.5);
	EuclideanVector<double, 3> vec2(1.2, 4.5, 19.3);

	EuclideanVector<double, 3> vec3 = vec1 + vec2;

	BOOST_CHECK_EQUAL(vec3.cmp[0], 6.2);
	BOOST_CHECK_EQUAL(vec3.cmp[1], 12.5);
	BOOST_CHECK_EQUAL(vec3.cmp[2], 28.8);
}

// === Operator+ Scalar ===
// Test 1: Check scalars are appropriately added when vector on left - 2D
BOOST_AUTO_TEST_CASE(operator_add_scalar_test1)
{
	EuclideanVector<double, 2> vec1(5.0, 8.0);
	double scalar = 5.0;

	EuclideanVector<double, 2> vec3 = vec1 + scalar;

	BOOST_CHECK_EQUAL(vec3.cmp[0], 10.0);
	BOOST_CHECK_EQUAL(vec3.cmp[1], 13.0);
}

// Test 2: Check scalars are appropriately added when vector on right - 2D
BOOST_AUTO_TEST_CASE(operator_add_scalar_test2)
{
	EuclideanVector<double, 2> vec1(5.0, 8.0);
	double scalar = 5.0;

	EuclideanVector<double, 2> vec3 = scalar + vec1;

	BOOST_CHECK_EQUAL(vec3.cmp[0], 10.0);
	BOOST_CHECK_EQUAL(vec3.cmp[1], 13.0);
}

// Test 3: Check scalars are appropriately added when vector on left - 3D
BOOST_AUTO_TEST_CASE(operator_add_scalar_test3)
{
	EuclideanVector<double, 3> vec1(5.0, 8.0, 9.5);
	double scalar = 5.0;

	EuclideanVector<double, 3> vec3 = vec1 + scalar;

	BOOST_CHECK_EQUAL(vec3.cmp[0], 10.0);
	BOOST_CHECK_EQUAL(vec3.cmp[1], 13.0);
	BOOST_CHECK_EQUAL(vec3.cmp[2], 14.5);
}

// Test 4: Check scalars are appropriately added when vector on right - 3D
BOOST_AUTO_TEST_CASE(operator_add_scalar_test4)
{
	EuclideanVector<double, 3> vec1(5.0, 8.0, 9.5);
	double scalar = 5.0;

	EuclideanVector<double, 3> vec3 = scalar + vec1;

	BOOST_CHECK_EQUAL(vec3.cmp[0], 10.0);
	BOOST_CHECK_EQUAL(vec3.cmp[1], 13.0);
	BOOST_CHECK_EQUAL(vec3.cmp[2], 14.5);
}

// === Operator- Vector ===
// Test 1: Check values are appropriately subtracted - 2D
BOOST_AUTO_TEST_CASE(operator_subtract_vector_test1)
{
	EuclideanVector<double,2> vec1(5.0, 8.0);
	EuclideanVector<double,2> vec2(1.2, 4.5);

	EuclideanVector<double,2> vec3 = vec1 - vec2;

	BOOST_CHECK_EQUAL(vec3.cmp[0], 3.8);
	BOOST_CHECK_EQUAL(vec3.cmp[1], 3.5);
}

// Test 2: Check values are appropriately subtracted - 3D
BOOST_AUTO_TEST_CASE(operator_subtract_vector_test2)
{
	EuclideanVector<double,3> vec1(5.0, 8.0, 9.5);
	EuclideanVector<double,3> vec2(1.2, 4.5, 19.3);

	EuclideanVector<double,3> vec3 = vec1 - vec2;

	BOOST_CHECK_EQUAL(vec3.cmp[0], 3.8);
	BOOST_CHECK_EQUAL(vec3.cmp[1], 3.5);
	BOOST_CHECK_EQUAL(vec3.cmp[2], -9.8);
}

// === Operator- Scalar ===
// Test 1: Check scalar values are subtracted when vector on left - 2D
BOOST_AUTO_TEST_CASE(operator_subtract_scalar_minus_test1,  * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,2> vec1(5.0, 8.0);
	double scalar = 5.0;

	EuclideanVector<double,2> vec3 = vec1 - scalar;

	BOOST_TEST(vec3.cmp[0] == 0.0);
	BOOST_TEST(vec3.cmp[1] == 3.0);
}

// Test 2: Check scalar values are subtracted when vector on right - 2D
BOOST_AUTO_TEST_CASE(operator_subtract_scalar_minus_test2,  * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,2> vec1(5.0, 8.0);
	double scalar = 5.0;

	EuclideanVector<double,2> vec3 = scalar - vec1;

	BOOST_TEST(vec3.cmp[0] == 0.0);
	BOOST_TEST(vec3.cmp[1] == -3.0);
}

// Test 3: Check scalar values are subtracted when vector on left - 3D
BOOST_AUTO_TEST_CASE(operator_subtract_scalar_minus_test3,  * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,3> vec1(5.0, 8.0, 9.5);
	double scalar = 5.0;

	EuclideanVector<double,3> vec3 = vec1 - scalar;

	BOOST_TEST(vec3.cmp[0] == 0.0);
	BOOST_TEST(vec3.cmp[1] == 3.0);
	BOOST_TEST(vec3.cmp[2] == 4.5);
}

// Test 4: Check scalar values are subtracted when vector on right - 3D
BOOST_AUTO_TEST_CASE(operator_subtract_scalar_minus_test4,  * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,3> vec1(5.0, 8.0, 9.5);
	double scalar = 5.0;

	EuclideanVector<double,3> vec3 = scalar - vec1;

	BOOST_TEST(vec3.cmp[0] == 0.0);
	BOOST_TEST(vec3.cmp[1] == -3.0);
	BOOST_TEST(vec3.cmp[2] == -4.5);
}

// === Operator* Scalar ===
// Test 1: Check scalar values are multiplied when vector on left - 2D
BOOST_AUTO_TEST_CASE(operator_subtract_scalar_multiply_test1,  * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,2> vec1(5.0, 8.0);
	double scalar = 5.0;

	EuclideanVector<double,2> vec3 = vec1 * scalar;

	BOOST_TEST(vec3.cmp[0] == 25.0);
	BOOST_TEST(vec3.cmp[1] == 40.0);
}

// Test 2: Check scalar values are multiplied when vector on right - 2D
BOOST_AUTO_TEST_CASE(operator_subtract_scalar_multiply_test2,  * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,2> vec1(5.0, 8.0);
	double scalar = 5.0;

	EuclideanVector<double,2> vec3 = scalar * vec1;

	BOOST_TEST(vec3.cmp[0] == 25.0);
	BOOST_TEST(vec3.cmp[1] == 40.0);
}

// Test 3: Check scalar values are multiplied when vector on left - 3D
BOOST_AUTO_TEST_CASE(operator_subtract_scalar_multiply_test3,  * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,3> vec1(5.0, 8.0, 9.5);
	double scalar = 5.0;

	EuclideanVector<double,3> vec3 = vec1 * scalar;

	BOOST_TEST(vec3.cmp[0] == 25.0);
	BOOST_TEST(vec3.cmp[1] == 40.0);
	BOOST_TEST(vec3.cmp[2] == 47.5);
}

// Test 4: Check scalar values are multiplied when vector on right - 3D
BOOST_AUTO_TEST_CASE(operator_subtract_scalar_multiply_test4,  * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,3> vec1(5.0, 8.0, 9.5);
	double scalar = 5.0;

	EuclideanVector<double,3> vec3 = scalar * vec1;

	BOOST_TEST(vec3.cmp[0] == 25.0);
	BOOST_TEST(vec3.cmp[1] == 40.0);
	BOOST_TEST(vec3.cmp[2] == 47.5);
}

// === Operator/ Scalar ===
// Test 1: Check scalar values are multiplied when vector on left - 2D
BOOST_AUTO_TEST_CASE(operator_subtract_scalar_divide_test1,  * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,2> vec1(5.0, 10.0);
	double scalar = 5.0;

	EuclideanVector<double,2> vec3 = vec1 / scalar;

	BOOST_TEST(vec3.cmp[0] == 1.0);
	BOOST_TEST(vec3.cmp[1] == 2.0);
}

// Test 2: Check scalar values are multiplied when vector on right - 2D
BOOST_AUTO_TEST_CASE(operator_subtract_scalar_divide_test2,  * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,2> vec1(5.0, 10.0);
	double scalar = 5.0;

	EuclideanVector<double,2> vec3 = scalar / vec1;

	BOOST_TEST(vec3.cmp[0] == 1.0);
	BOOST_TEST(vec3.cmp[1] == 0.5);
}

// Test 3: Check scalar values are multiplied when vector on left - 3D
BOOST_AUTO_TEST_CASE(operator_subtract_scalar_divide_test3,  * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,3> vec1(5.0, 10.0, 9.5);
	double scalar = 5.0;

	EuclideanVector<double,3> vec3 = vec1 / scalar;

	BOOST_TEST(vec3.cmp[0] == 1.0);
	BOOST_TEST(vec3.cmp[1] == 2.0);
	BOOST_TEST(vec3.cmp[2] == 1.9);
}

// Test 4: Check scalar values are multiplied when vector on left - 3D
BOOST_AUTO_TEST_CASE(operator_subtract_scalar_divide_test4,  * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,3> vec1(5.0, 10.0, 9.5);
	double scalar = 5.0;

	EuclideanVector<double,3> vec3 = scalar / vec1;

	BOOST_TEST(vec3.cmp[0] == 1.0);
	BOOST_TEST(vec3.cmp[1] == 0.5);
	BOOST_TEST(vec3.cmp[2] == 0.526315789473684);
}

// === CrossProduct ===

// Test 1: Check Cross-Product is computed correctly - 3D
BOOST_AUTO_TEST_CASE(operator_crossproduct_test1, * utf::tolerance(0.0000000001))
{
	EuclideanVector3D<double> vec1(2.0, 3.0, 4.0);
	EuclideanVector3D<double> vec2(3.5, 6.3, 19.7);
	EuclideanVector3D<double> result;

	result = vec1.crossProduct(vec2);

	BOOST_TEST(result.cmp[0] == 33.9);
	BOOST_TEST(result.cmp[1] == -25.4);
	BOOST_TEST(result.cmp[2] == 2.1);
}

// === rotateXAxisRadian ===
// Test 1: Rotate a vector 1 radian
BOOST_AUTO_TEST_CASE(rotateXAxisRadian_test1, * utf::tolerance(0.00001))
{
	EuclideanVector3D<double> vec(1.4, -3.2, 5.6);

	vec.rotateXAxisRadian(1.0);

	BOOST_TEST(vec.cmp[0] == 1.4);
	BOOST_TEST(vec.cmp[1] == -6.4412);
	BOOST_TEST(vec.cmp[2] == 0.332986);
}

// Test 2: Rotate a vector 90 degrees/1.5708 radians
BOOST_AUTO_TEST_CASE(rotateXAxisRadian_test2, * utf::tolerance(0.00001))
{
	EuclideanVector3D<double> vec(0.0,1.0,0.0);

	vec.rotateXAxisRadian(1.5708);

	BOOST_TEST(vec.cmp[0] == 0.0);
	BOOST_TEST(vec.cmp[1] == 0.0);
	BOOST_TEST(vec.cmp[2] == 1.0);
}

// Test 3: Rotate a vector -90 degrees/-1.5708 radians
BOOST_AUTO_TEST_CASE(rotateXAxisRadian_test3, * utf::tolerance(0.00001))
{
	EuclideanVector3D<double> vec(0.0,1.0,0.0);

	vec.rotateXAxisRadian(-1.5708);

	BOOST_TEST(vec.cmp[0] == 0.0);
	BOOST_TEST(vec.cmp[1] == 0.0);
	BOOST_TEST(vec.cmp[2] == -1.0);
}

// === rotateYAxisRadian ===
// Test 1:
BOOST_AUTO_TEST_CASE(rotateYAxisRadian_test1, * utf::tolerance(0.00001))
{
	EuclideanVector3D<double> vec(1.4, -3.2, 5.6);

	vec.rotateYAxisRadian(1.0);

	BOOST_TEST(vec.cmp[0] == 5.46866);
	BOOST_TEST(vec.cmp[1] == -3.2);
	BOOST_TEST(vec.cmp[2] == 1.84763);
}

// Test 2: Rotate a vector 90 degrees/1.5708 radians
BOOST_AUTO_TEST_CASE(rotateYAxisRadian_test2, * utf::tolerance(0.00001))
{
	EuclideanVector3D<double> vec(1.0,0.0,0.0);

	vec.rotateYAxisRadian(1.5708);

	BOOST_TEST(vec.cmp[0] == 0.0);
	BOOST_TEST(vec.cmp[1] == 0.0);
	BOOST_TEST(vec.cmp[2] == -1.0);
}

// Test 3: Rotate a vector -90 degrees/1.5708 radians
BOOST_AUTO_TEST_CASE(rotateYAxisRadian_test3, * utf::tolerance(0.00001))
{
	EuclideanVector3D<double> vec(1.0,0.0,0.0);

	vec.rotateYAxisRadian(-1.5708);

	BOOST_TEST(vec.cmp[0] == 0.0);
	BOOST_TEST(vec.cmp[1] == 0.0);
	BOOST_TEST(vec.cmp[2] == 1.0);
}

// === rotateZAxisRadian ===
// Test 1:
BOOST_AUTO_TEST_CASE(rotateZAxisRadian_test1, * utf::tolerance(0.00001))
{
	EuclideanVector3D<double> vec(1.4, 3.2, 5.6);

	vec.rotateZAxisRadian(1.0);

	BOOST_TEST(vec.cmp[0] == -1.93629);
	BOOST_TEST(vec.cmp[1] == 2.90703);
	BOOST_TEST(vec.cmp[2] == 5.6);
}

// Test 2: Rotate a unit vector
BOOST_AUTO_TEST_CASE(rotateZAxisRadian_test2, * utf::tolerance(0.00001))
{
	EuclideanVector3D<double> vec(1.0,0.0,0.0);

	vec.rotateZAxisRadian(1.5708);

	// Vector was pointing along X axis, should now be pointing up along Y-axis
	BOOST_TEST(vec.cmp[0] == 0.0);
	BOOST_TEST(vec.cmp[1] == 1.0);
	BOOST_TEST(vec.cmp[2] == 0.0);
}

// Test 3: Rotate a unit vector
BOOST_AUTO_TEST_CASE(rotateZAxisRadian_test3, * utf::tolerance(0.00001))
{
	EuclideanVector3D<double> vec(1.0,0.0,0.0);

	vec.rotateZAxisRadian(-1.5708);

	// Vector was pointing along X axis, should now be pointing up along Y-axis
	BOOST_TEST(vec.cmp[0] == 0.0);
	BOOST_TEST(vec.cmp[1] == -1.0);
	BOOST_TEST(vec.cmp[2] == 0.0);
}

// === isParallel ===
// Test 1: Test that parallel vectors detected - 2D
BOOST_AUTO_TEST_CASE(isParallel_test1, * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,2> vec1(2.0, 3.0);
	EuclideanVector<double,2> vec2(4.0, 6.0);

	bool isParallel = vec1.isParallel(vec2);
	BOOST_CHECK_EQUAL(isParallel, true);
}

// Test 2: Test that parallel vectors detected even with inverted sign - 2D
BOOST_AUTO_TEST_CASE(isParallel_test2, * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,2> vec1(2.0, 3.0);
	EuclideanVector<double,2> vec2(-4.0, -6.0);

	bool isParallel = vec1.isParallel(vec2);
	BOOST_CHECK_EQUAL(isParallel, true);
}

// Test 3: Test that parallel vectors detected - 3D
BOOST_AUTO_TEST_CASE(isParallel_test3, * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,3> vec1(2.0, 3.0, 4.0);
	EuclideanVector<double,3> vec2(4.0, 6.0, 8.0);

	bool isParallel = vec1.isParallel(vec2);
	BOOST_CHECK_EQUAL(isParallel, true);
}

// Test 4: Test that non-parallel vectors identified correctly - 2D
BOOST_AUTO_TEST_CASE(isParallel_test4, * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,2> vec1(2.0, 3.0);
	EuclideanVector<double,2> vec2(2.0, 3.5);

	bool isParallel = vec1.isParallel(vec2);
	BOOST_CHECK_EQUAL(isParallel, false);
}

// Test 5: Test that non-parallel vectors identified correctly - 3D
BOOST_AUTO_TEST_CASE(isParallel_test5, * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,3> vec1(2.0, 3.0, 7.6);
	EuclideanVector<double,3> vec2(4.0, 6.0, 8.9);

	bool isParallel = vec1.isParallel(vec2);
	BOOST_CHECK_EQUAL(isParallel, false);
}

// Test 6: Test that parallel vectors detected even if first component is zero - 3D
BOOST_AUTO_TEST_CASE(isParallel_test6, * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,3> vec1(0.0, 3.0, 4.0);
	EuclideanVector<double,3> vec2(0.0, 6.0, 8.0);

	bool isParallel = vec1.isParallel(vec2);
	BOOST_CHECK_EQUAL(isParallel, true);
}

// Test 7: Test that non-parallel vectors detected even if first component is zero - 3D
BOOST_AUTO_TEST_CASE(isParallel_test7, * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,3> vec1(0.0, 3.0, 7.6);
	EuclideanVector<double,3> vec2(0.0, 6.0, 8.9);

	bool isParallel = vec1.isParallel(vec2);
	BOOST_CHECK_EQUAL(isParallel, false);
}

// Test 8: Test that parallel vectors detected even if non-first component is zero - 3D
BOOST_AUTO_TEST_CASE(isParallel_test8, * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,3> vec1(2.0, 0.0, 4.0);
	EuclideanVector<double,3> vec2(4.0, 0.0, 8.0);

	bool isParallel = vec1.isParallel(vec2);
	BOOST_CHECK_EQUAL(isParallel, true);
}

// Test 9: Test that non-parallel vectors detected even if non-first component is zero - 3D
BOOST_AUTO_TEST_CASE(isParallel_test9, * utf::tolerance(0.0000000001))
{
	EuclideanVector<double,3> vec1(2.0, 0.0, 7.6);
	EuclideanVector<double,3> vec2(4.0, 0.0, 8.9);

	bool isParallel = vec1.isParallel(vec2);
	BOOST_CHECK_EQUAL(isParallel, false);
}

// === isPointOnLine ===
// Test 1: Test that point is detected on the line
BOOST_AUTO_TEST_CASE(isPointOnLine_test1, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(2.0, 3.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 6.0, 10.0);
	EuclideanPoint<double,3> point(3.0, 4.5, 7.5);

	bool isPointOnLine = cupcfd::geometry::euclidean::isPointOnLine(x1, x2, point);
	BOOST_CHECK_EQUAL(isPointOnLine, true);
}

// Test 2: Test that x1 is detected as on the line
BOOST_AUTO_TEST_CASE(isPointOnLine_test2, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(2.0, 3.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 6.0, 10.0);

	bool isPointOnLine = cupcfd::geometry::euclidean::isPointOnLine(x1, x2, x1);
	BOOST_CHECK_EQUAL(isPointOnLine, true);
}

// Test 3: Test that x2 is detected as on the line
BOOST_AUTO_TEST_CASE(isPointOnLine_test3, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(2.0, 3.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 6.0, 10.0);

	bool isPointOnLine = cupcfd::geometry::euclidean::isPointOnLine(x1, x2, x2);
	BOOST_CHECK_EQUAL(isPointOnLine, true);
}

// Test 4: Test that a point not on the line is detected as such
BOOST_AUTO_TEST_CASE(isPointOnLine_test4, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(2.0, 3.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 6.0, 10.0);
	EuclideanPoint<double,3> point(2.0, 3.0, 8.12);

	bool isPointOnLine = cupcfd::geometry::euclidean::isPointOnLine(x1, x2, point);
	BOOST_CHECK_EQUAL(isPointOnLine, false);
}

// Test 1: Test that point is detected on the line when there is a zero component to the vector (i.e. on a axis plane)
BOOST_AUTO_TEST_CASE(isPointOnLine_test5, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(2.0, 3.0, 5.0);
	EuclideanPoint<double,3> x2(2.0, 6.0, 10.0);
	EuclideanPoint<double,3> point(2.0, 4.5, 7.5);

	bool isPointOnLine = cupcfd::geometry::euclidean::isPointOnLine(x1, x2, point);
	BOOST_CHECK_EQUAL(isPointOnLine, true);
}

// === isVectorRangeIntersection (3D) ===
// Test 1: Test that two vectors are detected as intersecting - 3D
BOOST_AUTO_TEST_CASE(isVectorRangeIntersection_test1, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(7.0, 8.0, 6.0);
	EuclideanPoint<double,3> x3(7.0, 4.0, 5.0);
	EuclideanPoint<double,3> x4(3.0, 8.0, 6.0);

	// Should Intersect at 5.0, 6.0, 5.5
	bool intersect = cupcfd::geometry::euclidean::isVectorRangeIntersection(x1, x2, x3, x4);
	BOOST_CHECK_EQUAL(intersect, true);
}

// Test 2: Test two vectors that are not coplanar so don't intersect - 3D
BOOST_AUTO_TEST_CASE(isVectorRangeIntersection_test2, * utf::tolerance(0.0000000001))
{

	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(7.0, 8.0, 6.0);
	EuclideanPoint<double,3> x3(7.0, 4.0, 1.3);
	EuclideanPoint<double,3> x4(3.0, 8.0, 3.4);

	bool intersect = cupcfd::geometry::euclidean::isVectorRangeIntersection(x1, x2, x3, x4);

	BOOST_CHECK_EQUAL(intersect, false);
}

// Test 3: Test that two vectors would intersect, but don't because the range of vector 1 is too short - 3D
BOOST_AUTO_TEST_CASE(isVectorRangeIntersection_test3, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 5.0, 5.25);
	EuclideanPoint<double,3> x3(7.0, 4.0, 5.0);
	EuclideanPoint<double,3> x4(3.0, 8.0, 6.0);

	bool intersect = cupcfd::geometry::euclidean::isVectorRangeIntersection(x1, x2, x3, x4);
	BOOST_CHECK_EQUAL(intersect, false);
}

// Test 4: Test that two vectors would intersect, but don't because the range of vector 2 is too short - 3D
BOOST_AUTO_TEST_CASE(isVectorRangeIntersection_test4, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(7.0, 8.0, 6.0);
	EuclideanPoint<double,3> x3(4.0, 7.0, 5.75);
	EuclideanPoint<double,3> x4(3.0, 8.0, 6.0);

	bool intersect = cupcfd::geometry::euclidean::isVectorRangeIntersection(x1, x2, x3, x4);
	BOOST_CHECK_EQUAL(intersect, false);
}

// Test 5: Test two vectors that are coplanar but the intersect point is outside both ranges
BOOST_AUTO_TEST_CASE(isVectorRangeIntersection_test5, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 5.0, 5.25);
	EuclideanPoint<double,3> x3(4.0, 7.0, 5.75);
	EuclideanPoint<double,3> x4(3.0, 8.0, 6.0);

	bool intersect = cupcfd::geometry::euclidean::isVectorRangeIntersection(x1, x2, x3, x4);
	BOOST_CHECK_EQUAL(intersect, false);
}

// Test 6: Test that two lines that are colinear but don't overlap so don't intersect - 3D
BOOST_AUTO_TEST_CASE(isVectorRangeIntersection_test6, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 5.0, 6.0);

	EuclideanPoint<double,3> x3(9.0, 10.0, 11.0);
	EuclideanPoint<double,3> x4(15.0, 16.0, 17.0);

	bool intersect = cupcfd::geometry::euclidean::isVectorRangeIntersection(x1, x2, x3, x4);
	BOOST_CHECK_EQUAL(intersect, false);
}

// Test 7: Test that two lines that are colinear and do overlap are considered as intersecting - 3D
BOOST_AUTO_TEST_CASE(isVectorRangeIntersection_test7, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 5.0, 6.0);

	EuclideanPoint<double,3> x3(3.5, 4.5, 5.5);
	EuclideanPoint<double,3> x4(15.0, 16.0, 17.0);

	bool intersect = cupcfd::geometry::euclidean::isVectorRangeIntersection(x1, x2, x3, x4);
	BOOST_CHECK_EQUAL(intersect, true);
}

// Test 8: Test two vectors that are parallel but never overlap
BOOST_AUTO_TEST_CASE(isVectorRangeIntersection_test8, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 5.0, 6.0);

	EuclideanPoint<double,3> x3(6.0, 7.8, 2.3);
	EuclideanPoint<double,3> x4(7.0, 8.8, 3.3);

	bool intersect = cupcfd::geometry::euclidean::isVectorRangeIntersection(x1, x2, x3, x4);
	BOOST_CHECK_EQUAL(intersect, false);
}

// Test 9: Test that two lines that are colinear but don't overlap so don't intersect (one is in opposite direction) - 3D
// ToDo: This test case fails: Detects as colinear but thinks they overlap?
BOOST_AUTO_TEST_CASE(isVectorRangeIntersection_test9, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 5.0, 6.0);

	EuclideanPoint<double,3> x3(15.0, 16.0, 17.0);
	EuclideanPoint<double,3> x4(9.0, 10.0, 11.0);

	bool intersect = cupcfd::geometry::euclidean::isVectorRangeIntersection(x1, x2, x3, x4);
	BOOST_CHECK_EQUAL(intersect, false);
}

// Test 10: Test that two lines that are colinear and do overlap are considered as intersecting (one is in opposite direction) - 3D
BOOST_AUTO_TEST_CASE(isVectorRangeIntersection_test10, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 5.0, 6.0);

	EuclideanPoint<double,3> x3(15.0, 16.0, 17.0);
	EuclideanPoint<double,3> x4(3.5, 4.5, 5.5);

	bool intersect = cupcfd::geometry::euclidean::isVectorRangeIntersection(x1, x2, x3, x4);
	BOOST_CHECK_EQUAL(intersect, true);
}

// Test 11: Both ranges are just the same point
BOOST_AUTO_TEST_CASE(isVectorRangeIntersection_test11, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(3.0, 4.0, 5.0);

	EuclideanPoint<double,3> x3(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x4(3.0, 4.0, 5.0);

	bool intersect = cupcfd::geometry::euclidean::isVectorRangeIntersection(x1, x2, x3, x4);
	BOOST_CHECK_EQUAL(intersect, true);
}

// Test 12: Both ranges are just points, but not the same point
BOOST_AUTO_TEST_CASE(isVectorRangeIntersection_test12, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(3.0, 4.0, 5.0);

	EuclideanPoint<double,3> x3(4.0, 5.0, 7.0);
	EuclideanPoint<double,3> x4(4.0, 5.0, 7.0);

	bool intersect = cupcfd::geometry::euclidean::isVectorRangeIntersection(x1, x2, x3, x4);
	BOOST_CHECK_EQUAL(intersect, false);
}

// Test 13: Range 1 is a point, Range 2 is a line, the point does not lie on the line
BOOST_AUTO_TEST_CASE(isVectorRangeIntersection_test13, * utf::tolerance(0.0000000001))
{

}

// Test 14: Range 1 is a point, Range 2 is a line, the point does lie on the line
BOOST_AUTO_TEST_CASE(isVectorRangeIntersection_test14, * utf::tolerance(0.0000000001))
{

}

// Test 15: Range 2 is a point, Range 1 is a line, the point does not lie on the line
BOOST_AUTO_TEST_CASE(isVectorRangeIntersection_test15, * utf::tolerance(0.0000000001))
{

}

// Test 16: Range 2 is a point, Range 1 is a line, the point does lie on the line
BOOST_AUTO_TEST_CASE(isVectorRangeIntersection_test16, * utf::tolerance(0.0000000001))
{

}

// === computeVectorRangeIntersection ===
// Test 1: Test that two vectors are detected as intersecting - 3D
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test1, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(7.0, 8.0, 6.0);
	EuclideanPoint<double,3> x3(7.0, 4.0, 5.0);
	EuclideanPoint<double,3> x4(3.0, 8.0, 6.0);
	EuclideanPoint<double,3> intersectPoint;

	// Should Intersect at 5.0, 6.0, 5.5
	cupcfd::error::eCodes status = cupcfd::geometry::euclidean::computeVectorRangeIntersection(x1, x2, x3, x4, intersectPoint);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	BOOST_TEST(intersectPoint.cmp[0] == 5.0);
	BOOST_TEST(intersectPoint.cmp[1] == 6.0);
	BOOST_TEST(intersectPoint.cmp[2] == 5.5);
}

// Test 2: Test two vectors that are not coplanar so don't intersect - 3D
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test2, * utf::tolerance(0.0000000001))
{

	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(7.0, 8.0, 6.0);
	EuclideanPoint<double,3> x3(7.0, 4.0, 1.3);
	EuclideanPoint<double,3> x4(3.0, 8.0, 3.4);
	EuclideanPoint<double,3> intersectPoint;

	cupcfd::error::eCodes status = cupcfd::geometry::euclidean::computeVectorRangeIntersection(x1, x2, x3, x4, intersectPoint);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_GEOMETRY_NO_INTERSECT);
}

// Test 3: Test that two vectors would intersect, but don't because the range of vector 1 is too short - 3D
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test3, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 5.0, 5.25);
	EuclideanPoint<double,3> x3(7.0, 4.0, 5.0);
	EuclideanPoint<double,3> x4(3.0, 8.0, 6.0);
	EuclideanPoint<double,3> intersectPoint;

	cupcfd::error::eCodes status = cupcfd::geometry::euclidean::computeVectorRangeIntersection(x1, x2, x3, x4, intersectPoint);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_GEOMETRY_NO_INTERSECT);
}

// Test 4: Test that two vectors would intersect, but don't because the range of vector 2 is too short - 3D
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test4, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(7.0, 8.0, 6.0);
	EuclideanPoint<double,3> x3(4.0, 7.0, 5.75);
	EuclideanPoint<double,3> x4(3.0, 8.0, 6.0);
	EuclideanPoint<double,3> intersectPoint;

	cupcfd::error::eCodes status = cupcfd::geometry::euclidean::computeVectorRangeIntersection(x1, x2, x3, x4, intersectPoint);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_GEOMETRY_NO_INTERSECT);
}

// Test 5: Test two vectors that are coplanar but the intersect point is outside both ranges
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test5, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 5.0, 5.25);
	EuclideanPoint<double,3> x3(4.0, 7.0, 5.75);
	EuclideanPoint<double,3> x4(3.0, 8.0, 6.0);
	EuclideanPoint<double,3> intersectPoint;

	cupcfd::error::eCodes status = cupcfd::geometry::euclidean::computeVectorRangeIntersection(x1, x2, x3, x4, intersectPoint);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_GEOMETRY_NO_INTERSECT);
}

// Test 6: Test that two lines that are colinear but don't overlap so don't intersect - 3D
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test6, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 5.0, 6.0);
	EuclideanPoint<double,3> x3(9.0, 10.0, 11.0);
	EuclideanPoint<double,3> x4(15.0, 16.0, 17.0);
	EuclideanPoint<double,3> intersectPoint;

	cupcfd::error::eCodes status = cupcfd::geometry::euclidean::computeVectorRangeIntersection(x1, x2, x3, x4, intersectPoint);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_GEOMETRY_NO_INTERSECT);
}

// Test 7: Test that two lines that are colinear and do overlap are considered as intersecting - 3D
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test7, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 5.0, 6.0);
	EuclideanPoint<double,3> x3(3.5, 4.5, 5.5);
	EuclideanPoint<double,3> x4(15.0, 16.0, 17.0);
	EuclideanPoint<double,3> intersectPoint;

	cupcfd::error::eCodes status = cupcfd::geometry::euclidean::computeVectorRangeIntersection(x1, x2, x3, x4, intersectPoint);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	BOOST_TEST(intersectPoint.cmp[0] == 3.5);
	BOOST_TEST(intersectPoint.cmp[1] == 4.5);
	BOOST_TEST(intersectPoint.cmp[2] == 5.5);
}

// Test 8: Test two vectors that are parallel but never overlap
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test8, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 5.0, 6.0);
	EuclideanPoint<double,3> x3(6.0, 7.8, 2.3);
	EuclideanPoint<double,3> x4(7.0, 8.8, 3.3);
	EuclideanPoint<double,3> intersectPoint;

	cupcfd::error::eCodes status = cupcfd::geometry::euclidean::computeVectorRangeIntersection(x1, x2, x3, x4, intersectPoint);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_GEOMETRY_NO_INTERSECT);
}

// Test 9: Test that two lines that are colinear but don't overlap so don't intersect (one is in opposite direction) - 3D
// ToDo: This test case fails: Detects as colinear but thinks they overlap?
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test9, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 5.0, 6.0);
	EuclideanPoint<double,3> x3(15.0, 16.0, 17.0);
	EuclideanPoint<double,3> x4(9.0, 10.0, 11.0);
	EuclideanPoint<double,3> intersectPoint;

	cupcfd::error::eCodes status = cupcfd::geometry::euclidean::computeVectorRangeIntersection(x1, x2, x3, x4, intersectPoint);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_GEOMETRY_NO_INTERSECT);
}

// Test 10: Test that two lines that are colinear and do overlap are considered as intersecting (one is in opposite direction) - 3D
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test10, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(4.0, 5.0, 6.0);
	EuclideanPoint<double,3> x3(15.0, 16.0, 17.0);
	EuclideanPoint<double,3> x4(3.5, 4.5, 5.5);
	EuclideanPoint<double,3> intersectPoint;

	cupcfd::error::eCodes status = cupcfd::geometry::euclidean::computeVectorRangeIntersection(x1, x2, x3, x4, intersectPoint);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	BOOST_TEST(intersectPoint.cmp[0] == 3.5);
	BOOST_TEST(intersectPoint.cmp[1] == 4.5);
	BOOST_TEST(intersectPoint.cmp[2] == 5.5);
}

// Test 11: Both ranges are just the same point
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test11, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x3(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x4(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> intersectPoint;

	cupcfd::error::eCodes status = cupcfd::geometry::euclidean::computeVectorRangeIntersection(x1, x2, x3, x4, intersectPoint);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	BOOST_TEST(intersectPoint.cmp[0] == 3.0);
	BOOST_TEST(intersectPoint.cmp[1] == 4.0);
	BOOST_TEST(intersectPoint.cmp[2] == 5.0);
}

// Test 12: Both ranges are just points, but not the same point
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test12, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x2(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x3(4.0, 5.0, 7.0);
	EuclideanPoint<double,3> x4(4.0, 5.0, 7.0);
	EuclideanPoint<double,3> intersectPoint;

	cupcfd::error::eCodes status = cupcfd::geometry::euclidean::computeVectorRangeIntersection(x1, x2, x3, x4, intersectPoint);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_GEOMETRY_NO_INTERSECT);
}

// Test 13: Range 1 is a point, Range 2 is a line, the point does not lie on the line
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test13, * utf::tolerance(0.0000000001))
{

}

// Test 14: Range 1 is a point, Range 2 is a line, the point does lie on the line
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test14, * utf::tolerance(0.0000000001))
{

}

// Test 15: Range 2 is a point, Range 1 is a line, the point does not lie on the line
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test15, * utf::tolerance(0.0000000001))
{

}

// Test 16: Range 2 is a point, Range 1 is a line, the point does lie on the line
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test16, * utf::tolerance(0.0000000001))
{

}

// Test 17: Correct intersection when higher range is listed first in overlapping colinear setup of oposite directions
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test17, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(15.0, 16.0, 17.0);
	EuclideanPoint<double,3> x2(3.5, 4.5, 5.5);
	EuclideanPoint<double,3> x3(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x4(4.0, 5.0, 6.0);
	EuclideanPoint<double,3> intersectPoint;

	cupcfd::error::eCodes status = cupcfd::geometry::euclidean::computeVectorRangeIntersection(x1, x2, x3, x4, intersectPoint);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	BOOST_TEST(intersectPoint.cmp[0] == 3.5);
	BOOST_TEST(intersectPoint.cmp[1] == 4.5);
	BOOST_TEST(intersectPoint.cmp[2] == 5.5);
}

// Test 18: Correct intersection when higher range is listed first in overlapping colinear setup
BOOST_AUTO_TEST_CASE(computeVectorRangeIntersection_test18, * utf::tolerance(0.0000000001))
{
	EuclideanPoint<double,3> x1(3.5, 4.5, 5.5);
	EuclideanPoint<double,3> x2(15.0, 16.0, 17.0);
	EuclideanPoint<double,3> x3(3.0, 4.0, 5.0);
	EuclideanPoint<double,3> x4(4.0, 5.0, 6.0);
	EuclideanPoint<double,3> intersectPoint;

	cupcfd::error::eCodes status = cupcfd::geometry::euclidean::computeVectorRangeIntersection(x1, x2, x3, x4, intersectPoint);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	BOOST_TEST(intersectPoint.cmp[0] == 4.0);
	BOOST_TEST(intersectPoint.cmp[1] == 5.0);
	BOOST_TEST(intersectPoint.cmp[2] == 6.0);
}

// === DotProduct ===

// Test 1: Test correct dot product - 2D
BOOST_AUTO_TEST_CASE(dotproduct_test1, * utf::tolerance(0.01))
{
	EuclideanVector<double,2> vec1(2.0, 3.0);
	EuclideanVector<double,2> vec2(3.5, 6.3);
	double result = vec1.dotProduct(vec2);

	BOOST_TEST(result == 25.9);
}

// Test 2: Test correct dot product - 3D
BOOST_AUTO_TEST_CASE(dotproduct_test2, * utf::tolerance(0.01))
{
	EuclideanVector<double,3> vec1(2.0, 3.0, 4.0);
	EuclideanVector<double,3> vec2(3.5, 6.3, 19.7);
	double result = vec1.dotProduct(vec2);

	BOOST_TEST(result == 104.7);
}

// === length ===
// Test 1: Test correct length compute, double - 2D
BOOST_AUTO_TEST_CASE(length_test1, * utf::tolerance(0.00001))
{
	EuclideanVector<double,2> vec1(3.5, 6.3);
	double length = vec1.length();
	BOOST_TEST(length == 7.20694);
}

// Test 2: Test correct length compute, double - 3D
BOOST_AUTO_TEST_CASE(length_test2, * utf::tolerance(0.0001))
{
	EuclideanVector<double,3> vec1(3.5, 6.3, 19.7);
	double length = vec1.length();
	BOOST_TEST(length == 20.9769);
}

// === normalise ===
// Test 1: Test in-place normalisation - 2D
BOOST_AUTO_TEST_CASE(normalise_test1, * utf::tolerance(0.000001))
{
	// Setup
	EuclideanVector<double,2> vec(3.5, 6.3);
	vec.normalise();
	BOOST_TEST(vec.cmp[0] == 0.485643);
	BOOST_TEST(vec.cmp[1] == 0.874157);
}

// Test 2: Test in-place normalisation - 3D
BOOST_AUTO_TEST_CASE(normalise_test2, * utf::tolerance(0.00001))
{
	// Setup
	EuclideanVector<double,3> vec(3.5, 6.3, 19.7);
	vec.normalise();
	BOOST_TEST(vec.cmp[0] == 0.16685);
	BOOST_TEST(vec.cmp[1] == 0.30033);
	BOOST_TEST(vec.cmp[2] == 0.939129);
}

// === computeOrthagonalVector ===
// Test 1: Compute Orthagonal Vector
BOOST_AUTO_TEST_CASE(computeOrthagonalVector_test1, * utf::tolerance(0.00001))
{
	EuclideanVector<double,3> vec1(3.5, 6.3, 19.7);

	EuclideanVector<double,3> vec2 = vec1.computeOrthagonalVector();

	// Since the solution could technically be one of many things, test using dot product
	double dotProd = vec1.dotProduct(vec2);
	BOOST_TEST(dotProd == 0.0);
}

// Test 2: Compute Orthagonal Vector when first component of vector is Zero
// (I.e. should use another position as the unknown coefficient)
BOOST_AUTO_TEST_CASE(computeOrthagonalVector_test2, * utf::tolerance(0.00001))
{
	EuclideanVector<double,3> vec1(0.0, 6.3, 19.7);

	EuclideanVector<double,3> vec2 = vec1.computeOrthagonalVector();

	// Since the solution could technically be one of many things, test using dot product
	double dotProd = vec1.dotProduct(vec2);
	BOOST_TEST(dotProd == 0.0);
}

// === isRegistered ===
// Not sure quite where to/when to place this test....
// Should use a setter/getter to set/get registered?

// === register and deregisterMPIType ===
// Test 1: Test that Type is registered without error, and can be deregistered without error
BOOST_AUTO_TEST_CASE(registerMPIType_test1, * utf::tolerance(0.00001))
{
	cupcfd::error::eCodes status;
	EuclideanVector<double,3> vec;

	status = vec.registerMPIType();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	status = vec.deregisterMPIType();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
}

// Test 2: Test that if it is already registered, it does not register again
BOOST_AUTO_TEST_CASE(registerMPIType_test2, * utf::tolerance(0.00001))
{
	cupcfd::error::eCodes status;
	EuclideanVector<double,3> vec;

	status = vec.registerMPIType();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	status = vec.registerMPIType();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_MPI_DATATYPE_REGISTERED);

	// Cleanup for future tests since we use a static variable!
	status = vec.deregisterMPIType();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
}

// Test 3: Test that if unregistered, we cannot deregister again
BOOST_AUTO_TEST_CASE(registerMPIType_test3, * utf::tolerance(0.00001))
{
	cupcfd::error::eCodes status;
	EuclideanVector<double,3> vec;

	BOOST_CHECK_EQUAL(vec.isRegistered(), false);
	status = vec.deregisterMPIType();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_MPI_DATATYPE_UNREGISTERED);
}

// === getMPIType ===
// Test 1: Test that corrected ID is returned for the registered type with error code
BOOST_AUTO_TEST_CASE(getMPIType_test1, * utf::tolerance(0.00001))
{
	cupcfd::error::eCodes status;
	EuclideanVector<double,3> vec;
	MPI_Datatype dType;

	status = vec.registerMPIType();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	status = vec.getMPIType(&dType);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	// Cleanup for future tests since we use a static variable!
	status = vec.deregisterMPIType();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
}

// Test 2: Test error case detected when type unregistered
BOOST_AUTO_TEST_CASE(getMPIType_test2, * utf::tolerance(0.00001))
{
	cupcfd::error::eCodes status;
	EuclideanVector<double,3> vec;
	MPI_Datatype dType;

	BOOST_CHECK_EQUAL(vec.isRegistered(), false);
	status = vec.getMPIType(&dType);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_MPI_DATATYPE_UNREGISTERED);
}

// Test 3: Test corrected ID is returned for method without error code
BOOST_AUTO_TEST_CASE(getMPIType_test3, * utf::tolerance(0.00001))
{

}

// === MPI: Vector Broadcast ===
// Test 1: Testing the the MPI Type was setup correctly, and able to broadcast two vectors
// This should establish that the custom MPI type has the right sizes
BOOST_AUTO_TEST_CASE(MPIVectorBroadcast)
{
	cupcfd::error::eCodes status;
	cupcfd::comm::Communicator comm(MPI_COMM_WORLD);

	// Create Two Vectors on Rank 0

	// Register the type (only once per all tests in file since static?)
	EuclideanVector<double,3> vec;

	// Should not yet be registered
	BOOST_CHECK_EQUAL(vec.isRegistered(), false);

	// Register
	status = vec.registerMPIType();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	// Should now be registered
	BOOST_CHECK_EQUAL(vec.isRegistered(), true);

	if(comm.rank == 0)
	{
		EuclideanVector<double,3> vecs[2] = {EuclideanVector<double,3>(3.5, 6.3, 19.7),
											 EuclideanVector<double,3>(3.0, 4.5, 6.7)};

		status = cupcfd::comm::Broadcast(vecs, 2, 0, comm);
		BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	}
	else
	{
		EuclideanVector<double,3> recvVecs[2];

		status = cupcfd::comm::Broadcast(recvVecs, 2, 0, comm);
		BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

		BOOST_CHECK_EQUAL(recvVecs[0].cmp[0], 3.5);
		BOOST_CHECK_EQUAL(recvVecs[0].cmp[1], 6.3);
		BOOST_CHECK_EQUAL(recvVecs[0].cmp[2], 19.7);

		BOOST_CHECK_EQUAL(recvVecs[1].cmp[0], 3.0);
		BOOST_CHECK_EQUAL(recvVecs[1].cmp[1], 4.5);
		BOOST_CHECK_EQUAL(recvVecs[1].cmp[2], 6.7);
	}

	// Cleanup for future tests since we use a static variable!
	status = vec.deregisterMPIType();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
}

BOOST_AUTO_TEST_CASE(cleanup)
{
    MPI_Finalize();
}