#include <iostream>
#include <Eigen/Core>
#include <sys/time.h>

#include "celerite.h"

using namespace Eigen;

double get_timestamp () {
  struct timeval now;
  gettimeofday (&now, NULL);
  return double(now.tv_usec) * 1.0e-6 + double(now.tv_sec);
}

template <typename T, int J_comp>
void run_benchmark (int J, int N) {
  const auto Options = J_comp == 1 ? ColMajor : RowMajor;
  typedef Matrix<T, Dynamic, J_comp, Options> matrix;
  typedef Matrix<T, Dynamic, 1> vector;

  // Random matrices
  srand(1234);
  vector A(N), Y = vector::Random(N), Z, bA(N), bD(N), bY(N), bZ(N);
  matrix U = matrix::Random(N, J), bU(N, J),
         V0 = matrix::Random(N, J), V(N, J), bV(N, J), bW(N, J),
         P = matrix::Random(N-1, J), bP(N-1, J);
  Matrix<T, J_comp, J_comp, Options> S(J, J), bS(J, J);
  Matrix<T, J_comp, 1> F(J), G(J), bF(J), bG(J);
  S.setZero();

  // Likelihood time
  double strt, end, count = 0.0;

  strt = get_timestamp();
  do {
    V << V0;
    A.setConstant(10*J);
    int flag = celerite::factor(U, P, A, V, S);
    T ll = log(A.array()).sum();

    Z = Y;
    celerite::solve(U, P, A, V, Z, F, G);
    ll += Y.transpose() * Z;

    end = get_timestamp();
    count += 1.0;
  } while ((end - strt < 0.7) || (count < 3.0));
  std::cout << sizeof(T) << "," << J_comp << "," << J << "," << N << ",";
  std::cout << ((end - strt) / count) << ",";

  // Grad time
  strt = get_timestamp();
  count = 0.0;
  do {
    bZ = Y;
    bY = Z;
    bD.array() = 1.0 / A.array();

    bF.setZero();
    bG.setZero();

    bU.setZero();
    bP.setZero();

    celerite::solve_grad(U, P, A, V, Z, F, G, bZ, bF, bG, bU, bP, bD, bW, bY);

    bS.setZero();
    celerite::factor_grad(U, P, A, V, S, bS, bU, bP, bD, bW);

    end = get_timestamp();
    count += 1.0;
  } while ((end - strt < 0.7) || (count < 3.0));

  std::cout << ((end - strt) / count);
  std::cout << std::endl;
}

#define RUN_BENCHMARK(J, N)      \
  run_benchmark<double, J>(J, N);      \
  run_benchmark<double, Dynamic>(J, N);

#define RUN_BENCHMARKS(J)        \
RUN_BENCHMARK(J, 64    )         \
RUN_BENCHMARK(J, 128   )         \
RUN_BENCHMARK(J, 256   )         \
RUN_BENCHMARK(J, 512   )         \
RUN_BENCHMARK(J, 1024  )         \
RUN_BENCHMARK(J, 2048  )         \
RUN_BENCHMARK(J, 4096  )         \
RUN_BENCHMARK(J, 8192  )         \
RUN_BENCHMARK(J, 16384 )         \
RUN_BENCHMARK(J, 32768 )         \
RUN_BENCHMARK(J, 65536 )         \
RUN_BENCHMARK(J, 131072)         \
RUN_BENCHMARK(J, 262144)

int main ()
{
  std::cout << "size,J_comp,J,N,time,grad_time\n";
  //RUN_BENCHMARKS(1);
  RUN_BENCHMARKS(2);
  RUN_BENCHMARKS(4);
  RUN_BENCHMARKS(8);
  RUN_BENCHMARKS(16);
  RUN_BENCHMARKS(32);
  RUN_BENCHMARKS(64);
  RUN_BENCHMARKS(128);

  return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_ISQRT_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_ISQRT_HPP_INCLUDED

#include <boost/simd/toolbox/arithmetic/functions/isqrt.hpp>
#include <boost/simd/include/functions/simd/sqrt.hpp>
#include <boost/simd/include/functions/simd/toint.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::isqrt_, tag::cpu_,
                       (A0)(X),
                       ((simd_< floating_<A0>, X >))
                      )
  {
    typedef typename dispatch::meta::as_integer<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      return boost::simd::itrunc(boost::simd::sqrt(a0));
    }
  };
} } }

#endif


// Copyright (c) 2017, Lawrence Livermore National Security, LLC and
// UT-Battelle, LLC.
// Produced at the Lawrence Livermore National Laboratory and the Oak Ridge
// National Laboratory.
// LLNL-CODE-743438
// All rights reserved.
// This file is part of MGmol. For details, see https://github.com/llnl/mgmol.
// Please also read this link https://github.com/llnl/mgmol/LICENSE

#include "random.h"

#include <algorithm>
#include <cstdlib>
#include <iostream>
#include <vector>

#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

/* Generate random numbers between (a, b] */
double generate_rand(const int a, const int b)
{
    double val = (b - a) * (double)rand() / (double)RAND_MAX + a;
    return val;
}

/* Generate random numbers between (0, 1] */
double generate_rand_num()
{
    double val = (double)rand() / (double)RAND_MAX;
    return val;
}

/* Generate a vector of random numbers between (a, b] */
std::vector<double> generate_rand(const int n, const int a, const int b)
{
    std::vector<double> vec(n);
    for (int i = 0; i < n; i++)
        vec[i] = generate_rand(a, b);

    return vec;
}
std::vector<double> generate_rand(const int n)
{
    std::vector<double> vec(n);
    std::generate(vec.begin(), vec.end(), &generate_rand_num);

    return vec;
}

template <typename DataType>
void generateRandomData(
    std::vector<DataType>& data, const DataType minv, const DataType maxv)
{
    typedef boost::minstd_rand rng_type;
    typedef boost::uniform_real<> distribution_type;

    int seed = 113;
    rng_type rng(seed);
    distribution_type nd(minv, maxv);
    boost::variate_generator<rng_type, distribution_type> gen(rng, nd);

    for (auto& d : data)
        d = gen();
}

template void generateRandomData(
    std::vector<double>& data, const double minv, const double maxv);
template void generateRandomData(
    std::vector<float>& data, const float minv, const float maxv);


#include "jacobiantest.hpp"
#include <kinfam_io.hpp>
#include <Eigen/Core>

CPPUNIT_TEST_SUITE_REGISTRATION(JacobianTest);

using namespace KDL;

void JacobianTest::setUp(){}
void JacobianTest::tearDown(){}

void JacobianTest::TestChangeRefPoint(){
    //Create a random jacobian
    Jacobian j1(5);
    j1.data.setRandom();
    //Create a random Vector
    Vector p;
    random(p);

    Jacobian j2(5);
    CPPUNIT_ASSERT(changeRefPoint(j1,p,j2));
    CPPUNIT_ASSERT(j1!=j2);
    Jacobian j3(4);
    CPPUNIT_ASSERT(!changeRefPoint(j1,p,j3));
    j3.resize(5);
    CPPUNIT_ASSERT(changeRefPoint(j2,-p,j3));
    CPPUNIT_ASSERT_EQUAL(j1,j3);

}

void JacobianTest::TestChangeRefFrame(){
    //Create a random jacobian
    Jacobian j1(5);
    j1.data.setRandom();
    //Create a random frame
    Frame f;
    random(f);

    Jacobian j2(5);
    CPPUNIT_ASSERT(changeRefFrame(j1,f,j2));
    CPPUNIT_ASSERT(j1!=j2);
    Jacobian j3(4);
    CPPUNIT_ASSERT(!changeRefFrame(j1,f,j3));
    j3.resize(5);
    CPPUNIT_ASSERT(changeRefFrame(j2,f.Inverse(),j3));
    CPPUNIT_ASSERT_EQUAL(j1,j3);
}

void JacobianTest::TestChangeBase(){
    //Create a random jacobian
    Jacobian j1(5);
    j1.data.setRandom();
    //Create a random rotation
    Rotation r;
    random(r);

    Jacobian j2(5);
    CPPUNIT_ASSERT(changeBase(j1,r,j2));
    CPPUNIT_ASSERT(j1!=j2);
    Jacobian j3(4);
    CPPUNIT_ASSERT(!changeBase(j1,r,j3));
    j3.resize(5);
    CPPUNIT_ASSERT(changeBase(j2,r.Inverse(),j3));
    CPPUNIT_ASSERT_EQUAL(j1,j3);
}

void JacobianTest::TestConstructor(){
    //Create an empty Jacobian
    Jacobian j1(2);
    //Get size
    CPPUNIT_ASSERT_EQUAL(j1.rows(),(unsigned int)6);
    CPPUNIT_ASSERT_EQUAL(j1.columns(),(unsigned int)2);
    //Create a second Jacobian from empty
    Jacobian j2(j1);
    //Get size
    CPPUNIT_ASSERT_EQUAL(j2.rows(),(unsigned int)6);
    CPPUNIT_ASSERT_EQUAL(j2.columns(),(unsigned int)2);
    Jacobian j3=j1;
    //Get size
    CPPUNIT_ASSERT_EQUAL(j3.rows(),(unsigned int)6);
    CPPUNIT_ASSERT_EQUAL(j3.columns(),(unsigned int)2);

    //Test resize
    j1.resize(5);
    //Get size
    CPPUNIT_ASSERT_EQUAL(j1.rows(),(unsigned int)6);
    CPPUNIT_ASSERT_EQUAL(j1.columns(),(unsigned int)5);

    j2=j1;
    //Get size
    CPPUNIT_ASSERT_EQUAL(j2.rows(),(unsigned int)6);
    CPPUNIT_ASSERT_EQUAL(j2.columns(),(unsigned int)5);
}

void JacobianTest::TestGetSetColumn(){}


#include <iostream>
#include <algorithm>
#include <cmath>        // abs() for float, and fabs()
#include <math.h>       // pow()
#include <random>
#include <climits>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp> // row(), column()

using namespace boost::numeric::ublas;

#define print(var) \
  std::cout<<#var" = "<<(var)<<std::endl;
#define printstr(str) \
  std::cout<<str<<std::endl;
#define printLine() \
  std::cout<<"============================"<<std::endl;


#include "testsuite.h"
#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main() {
  Array<int, 2> A(4);
  Array<int, 1> B(4), C(4);

  firstIndex i;
  secondIndex j;

  A = i+j;

  B = max(A,j);

  C = 3, 4, 5, 6;
  BZTEST(count(B == C) == 4);

  return 0;
}


#pragma once
//standard include
#include <math.h>
#include <iostream>

//opencv include
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/core/core.hpp"
#include <boost/circular_buffer.hpp>

#include "track3d.hpp"

struct DepthInfo
{
	float depth;
	bool  error;
};

//this contains all the info we need to decide between goals once we are certain if it is a goal
struct GoalInfo
{
	cv::Point3f pos;
	float confidence;
	float distance;
	float angle;
	cv::Rect rect;
	size_t contour_index;
	bool depth_error;
	cv::Point com;
	cv::Rect br;
	cv::RotatedRect rtRect;
	cv::Point2f lineStart;
	cv::Point2f lineEnd;
};

//This contains all the necessary info for a goal
struct GoalFound
{
	cv::Point3f pos;
	cv::Point3f left_pos;
	cv::Point3f right_pos;
	float distance;
	float angle;
	float confidence;
	size_t left_contour_index;
	size_t right_contour_index;
	cv::Rect left_rect;
	cv::Rect right_rect;
	cv::RotatedRect left_rotated_rect;
	cv::RotatedRect right_rotated_rect;
};

class GoalDetector
{
	public:
		GoalDetector(const cv::Point2f &fov_size, const cv::Size &frame_size, bool gui = false);

		std::vector< GoalFound > return_found(void) const;

		void drawOnFrame(cv::Mat &image,const std::vector< std::vector< cv::Point>> &contours) const;

		//These are the three functions to call to run GoalDetector
		//they fill in _contours, _infos, _depth_mins, etc
		void clear(void);

		//If your objectypes have the same width it's safe to run
		//getContours and computeConfidences with different types
		void findBoilers(const cv::Mat& image, const cv::Mat& depth);
		const std::vector< std::vector< cv::Point > > getContours(const cv::Mat& image);

		bool Valid(void) const;
		void setCameraAngle(double camera_angle);
		void setBlueScale(double blue_scale);
		void setRedScale(double red_scale);
		void setOtsuThreshold(int otsu_threshold);
		void setMinConfidence(double min_valid_confidence);

	private:

		cv::Point2f _fov_size;
		cv::Size    _frame_size;

		// Save detection info
		bool        _isValid;
		std::vector< GoalFound > _return_found;
		float       _min_valid_confidence;

		int         _otsu_threshold;
		int         _blue_scale;
		int         _red_scale;

		int         _camera_angle;

		float createConfidence(float expectedVal, float expectedStddev, float actualVal);
		float distanceUsingFOV(ObjectType _goal_shape, const cv::Rect &rect) const;
		float distanceUsingFixedHeight(const cv::Rect &rect,const cv::Point &center, float expected_delta_height) const;
		bool generateThresholdAddSubtract(const cv::Mat& imageIn, cv::Mat& imageOut);
		void isValid();
		const std::vector<DepthInfo> getDepths(const cv::Mat &depth, const std::vector< std::vector< cv::Point > > &contours, const ObjectNum &objtype, float expected_height);
		const std::vector< GoalInfo > getInfo(const std::vector< std::vector< cv::Point > > &contours, const std::vector<DepthInfo> &depth_maxs, ObjectNum objtype);
};


/**
 * @file tests/mean_shift_test.cpp
 * @author Shangtong Zhang
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */

#include <mlpack/core.hpp>

#include <mlpack/methods/mean_shift/mean_shift.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::meanshift;
using namespace mlpack::distribution;

BOOST_AUTO_TEST_SUITE(MeanShiftTest);

// Generate dataset; written transposed because it's easier to read.
arma::mat meanShiftData("  0.0   0.0;" // Class 1.
                     "  0.3   0.4;"
                     "  0.1   0.0;"
                     "  0.1   0.3;"
                     " -0.2  -0.2;"
                     " -0.1   0.3;"
                     " -0.4   0.1;"
                     "  0.2  -0.1;"
                     "  0.3   0.0;"
                     " -0.3  -0.3;"
                     "  0.1  -0.1;"
                     "  0.2  -0.3;"
                     " -0.3   0.2;"
                     " 10.0  10.0;" // Class 2.
                     " 10.1   9.9;"
                     "  9.9  10.0;"
                     " 10.2   9.7;"
                     " 10.2   9.8;"
                     "  9.7  10.3;"
                     "  9.9  10.1;"
                     "-10.0   5.0;" // Class 3.
                     " -9.8   5.1;"
                     " -9.9   4.9;"
                     "-10.0   4.9;"
                     "-10.2   5.2;"
                     "-10.1   5.1;"
                     "-10.3   5.3;"
                     "-10.0   4.8;"
                     " -9.6   5.0;"
                     " -9.8   5.1;");

/**
 * 30-point 3-class test case for Mean Shift.
 */
BOOST_AUTO_TEST_CASE(MeanShiftSimpleTest)
{
  MeanShift<> meanShift;

  arma::Row<size_t> assignments;
  arma::mat centroids;
  meanShift.Cluster((arma::mat) trans(meanShiftData), assignments, centroids);

  // Now make sure we got it all right.  There is no restriction on how the
  // clusters are ordered, so we have to be careful about that.
  size_t firstClass = assignments(0);

  for (size_t i = 1; i < 13; ++i)
    BOOST_REQUIRE_EQUAL(assignments(i), firstClass);

  size_t secondClass = assignments(13);

  // To ensure that class 1 != class 2.
  BOOST_REQUIRE_NE(firstClass, secondClass);

  for (size_t i = 13; i < 20; ++i)
    BOOST_REQUIRE_EQUAL(assignments(i), secondClass);

  size_t thirdClass = assignments(20);

  // To ensure that this is the third class which we haven't seen yet.
  BOOST_REQUIRE_NE(firstClass, thirdClass);
  BOOST_REQUIRE_NE(secondClass, thirdClass);

  for (size_t i = 20; i < 30; ++i)
    BOOST_REQUIRE_EQUAL(assignments(i), thirdClass);
}

// Generate samples from four Gaussians, and make sure mean shift nearly
// recovers those four centers.
BOOST_AUTO_TEST_CASE(GaussianClustering)
{
  GaussianDistribution g1("0.0 0.0 0.0", arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2("5.0 5.0 5.0", 2 * arma::eye<arma::mat>(3, 3));
  GaussianDistribution g3("-3.0 3.0 -1.0", arma::eye<arma::mat>(3, 3));
  GaussianDistribution g4("6.0 -2.0 -2.0", 3 * arma::eye<arma::mat>(3, 3));

  // We may need to run this multiple times, because sometimes it may converge
  // to the wrong number of clusters.
  bool success = false;
  for (size_t trial = 0; trial < 4; ++trial)
  {
    arma::mat dataset(3, 4000);
    for (size_t i = 0; i < 1000; ++i)
      dataset.col(i) = g1.Random();
    for (size_t i = 1000; i < 2000; ++i)
      dataset.col(i) = g2.Random();
    for (size_t i = 2000; i < 3000; ++i)
      dataset.col(i) = g3.Random();
    for (size_t i = 3000; i < 4000; ++i)
      dataset.col(i) = g4.Random();

    // Now that the dataset is generated, run mean shift.  Pre-set radius.
    MeanShift<> meanShift(2.9);

    arma::Row<size_t> assignments;
    arma::mat centroids;
    meanShift.Cluster(dataset, assignments, centroids);

    success = (centroids.n_cols == 4);
    if (!success)
      continue;
    success = (centroids.n_rows == 3);
    if (!success)
      continue;

    // Check that each centroid is close to only one mean.
    arma::vec centroidDistances(4);
    arma::uvec minIndices(4);
    for (size_t i = 0; i < 4; ++i)
    {
      centroidDistances(0) = metric::EuclideanDistance::Evaluate(g1.Mean(),
          centroids.col(i));
      centroidDistances(1) = metric::EuclideanDistance::Evaluate(g2.Mean(),
          centroids.col(i));
      centroidDistances(2) = metric::EuclideanDistance::Evaluate(g3.Mean(),
          centroids.col(i));
      centroidDistances(3) = metric::EuclideanDistance::Evaluate(g4.Mean(),
          centroids.col(i));

      // Are we near a centroid of a Gaussian?
      const double minVal = centroidDistances.min(minIndices[i]);
      success = (std::abs(minVal) <= 0.65);
      if (!success)
        break;
    }

    // Ensure each centroid corresponds to a different Gaussian.
    bool innerSuccess = true;
    for (size_t i = 0; i < 4; ++i)
      for (size_t j = i + 1; j < 4; ++j)
        innerSuccess &= (minIndices[i] != minIndices[j]);

    if (innerSuccess)
      success = true;

    if (success)
      break;
  }

  BOOST_REQUIRE_EQUAL(success, true);
}

BOOST_AUTO_TEST_SUITE_END();


//Author: Dr. Shantanu Shahane
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <float.h>
#include <string.h>
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <unsupported/Eigen/SparseExtra>
#include <Eigen/SparseLU>
#include <Eigen/OrderingMethods>
#include <Eigen/Core>
#include "_hypre_utilities.h"
#include "HYPRE_krylov.h"
#include "HYPRE.h"
#include "HYPRE_parcsr_ls.h"
#include "general_functions.hpp"
#include "class.hpp"

SEMI_IMPLICIT_SPLIT_SOLVER::SEMI_IMPLICIT_SPLIT_SOLVER(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, vector<bool> &u_dirichlet_flag1, vector<bool> &v_dirichlet_flag1, vector<bool> &p_dirichlet_flag1, int n_outer_iter1, double iterative_tolerance1, int precond_freq_it1)
{
    u_dirichlet_flag = u_dirichlet_flag1, v_dirichlet_flag = v_dirichlet_flag1, p_dirichlet_flag = p_dirichlet_flag1;
    n_outer_iter = n_outer_iter1, iterative_tolerance = iterative_tolerance1, precond_freq_it = precond_freq_it1;
    check_bc(points, parameters);
    clock_t clock_t1 = clock(), clock_t2 = clock();
    solver_p.init(points, cloud, parameters, p_dirichlet_flag, 0.0, 0.0, 1.0, true); //BC for p_prime is identical to BC of p_new
    parameters.factoring_timer = ((double)(clock() - clock_t1)) / CLOCKS_PER_SEC;

    zero_vector = Eigen::VectorXd::Zero(points.nv);
    zero_vector_1 = Eigen::VectorXd::Zero(points.nv + 1);
    p_bc_full_neumann = true;
    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv] && p_dirichlet_flag[iv])
        { //boundary point found with dirichlet BC
            p_bc_full_neumann = false;
            break;
        }

    if (p_bc_full_neumann)
        p_source = zero_vector_1, p_prime = zero_vector_1; //this is full Neumann for pressure
    else
        p_source = zero_vector, p_prime = zero_vector;
    u_source = zero_vector, v_source = zero_vector;
    u_old_old = zero_vector, v_old_old = zero_vector;
    u_prime = zero_vector, v_prime = zero_vector;
    u_iter_old = zero_vector, v_iter_old = zero_vector;
    normal_mom_x = zero_vector, normal_mom_y = zero_vector;
}

void SEMI_IMPLICIT_SPLIT_SOLVER::check_bc(POINTS &points, PARAMETERS &parameters)
{
    int u_dirichlet_flag_sum = accumulate(u_dirichlet_flag.begin(), u_dirichlet_flag.end(), 0);
    if (u_dirichlet_flag_sum == 0)
    {
        printf("\n\nERROR from SEMI_IMPLICIT_SPLIT_SOLVER::check_bc Setting u_dirichlet_flag to full Neumann BC is not permitted; sum of u_dirichlet_flag: %i\n\n", u_dirichlet_flag_sum);
        throw bad_exception();
    }
    int v_dirichlet_flag_sum = accumulate(v_dirichlet_flag.begin(), v_dirichlet_flag.end(), 0);
    if (v_dirichlet_flag_sum == 0)
    {
        printf("\n\nERROR from FRACTIONASEMI_IMPLICIT_SPLIT_SOLVERL_STEP_1::check_bc Setting v_dirichlet_flag to full Neumann BC is not permitted; sum of v_dirichlet_flag: %i\n\n", v_dirichlet_flag_sum);
        throw bad_exception();
    }
    if (parameters.rho < 0 || parameters.mu < 0)
    {
        printf("\n\nERROR from SEMI_IMPLICIT_SPLIT_SOLVER::check_bc Some parameters are not set; parameters.rho: %g, parameters.mu: %g\n\n", parameters.rho, parameters.mu);
        throw bad_exception();
    }
}

void SEMI_IMPLICIT_SPLIT_SOLVER::calc_vel(POINTS &points, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old, Eigen::VectorXd &body_force_x, Eigen::VectorXd &body_force_y)
{
    if (it == 0)
    { //BDF1: implicit Euler
        u_source = ((parameters.rho / parameters.dt) * u_old) - (points.grad_x_matrix_EIGEN_internal * p_new.head(points.nv)) + body_force_x;
        v_source = ((parameters.rho / parameters.dt) * v_old) - (points.grad_y_matrix_EIGEN_internal * p_new.head(points.nv)) + body_force_y;
    }
    else
    { //BDF2
        u_source = -((bdf2_alpha_2 * parameters.rho / parameters.dt) * u_old) - ((bdf2_alpha_3 * parameters.rho / parameters.dt) * u_old_old) - (points.grad_x_matrix_EIGEN_internal * p_new.head(points.nv)) + body_force_x;
        v_source = -((bdf2_alpha_2 * parameters.rho / parameters.dt) * v_old) - ((bdf2_alpha_3 * parameters.rho / parameters.dt) * v_old_old) - (points.grad_y_matrix_EIGEN_internal * p_new.head(points.nv)) + body_force_y;
    }

    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv])
        { //retain boundary condition from "_old"
            if (u_dirichlet_flag[iv])
                u_source[iv] = u_old[iv];
            else
                u_source[iv] = 0.0;
            if (v_dirichlet_flag[iv])
                v_source[iv] = v_old[iv];
            else
                v_source[iv] = 0.0;
        }
    u_new = solver_eigen_u.solveWithGuess(u_source, u_new);
    v_new = solver_eigen_v.solveWithGuess(v_source, v_new);
}

void SEMI_IMPLICIT_SPLIT_SOLVER::calc_vel(POINTS &points, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old)
{
    if (it == 0)
    { //BDF1: implicit Euler
        u_source = ((parameters.rho / parameters.dt) * u_old) - (points.grad_x_matrix_EIGEN_internal * p_new.head(points.nv));
        v_source = ((parameters.rho / parameters.dt) * v_old) - (points.grad_y_matrix_EIGEN_internal * p_new.head(points.nv));
    }
    else
    { //BDF2
        u_source = -((bdf2_alpha_2 * parameters.rho / parameters.dt) * u_old) - ((bdf2_alpha_3 * parameters.rho / parameters.dt) * u_old_old) - (points.grad_x_matrix_EIGEN_internal * p_new.head(points.nv));
        v_source = -((bdf2_alpha_2 * parameters.rho / parameters.dt) * v_old) - ((bdf2_alpha_3 * parameters.rho / parameters.dt) * v_old_old) - (points.grad_y_matrix_EIGEN_internal * p_new.head(points.nv));
    }

    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv])
        { //retain boundary condition from "_old"
            if (u_dirichlet_flag[iv])
                u_source[iv] = u_old[iv];
            else
                u_source[iv] = 0.0;
            if (v_dirichlet_flag[iv])
                v_source[iv] = v_old[iv];
            else
                v_source[iv] = 0.0;
        }
    u_new = solver_eigen_u.solveWithGuess(u_source, u_new);
    v_new = solver_eigen_v.solveWithGuess(v_source, v_new);
}

void SEMI_IMPLICIT_SPLIT_SOLVER::calc_pressure(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old)
{
    if (p_bc_full_neumann)
        p_source = zero_vector_1; //this is full Neumann for pressure
    else
        p_source = zero_vector;
    if (it == 0) //BDF1: implicit Euler
        p_source.head(points.nv) = ((points.grad_x_matrix_EIGEN_internal * u_new) + (points.grad_y_matrix_EIGEN_internal * v_new)) * (parameters.rho / parameters.dt);
    else
        p_source.head(points.nv) = ((points.grad_x_matrix_EIGEN_internal * u_new) + (points.grad_y_matrix_EIGEN_internal * v_new)) * (bdf2_alpha_1 * parameters.rho / parameters.dt);
    //BC for p_prime is identical to BC of p_new
    //p_source of p_prime is zero for both dirichlet and neumann for p_new
    if (p_bc_full_neumann) //this is full Neumann for pressure
        solver_p.general_solve(points, parameters, p_prime, zero_vector_1, p_source);
    else
        solver_p.general_solve(points, parameters, p_prime, zero_vector, p_source);
    p_new = p_new + p_prime;

    normal_mom_x = zero_vector, normal_mom_y = zero_vector;
    if (it == 0)
    { //BDF1: implicit Euler
        normal_mom_x = -parameters.rho * (u_new.cwiseProduct(points.grad_x_matrix_EIGEN_boundary * u_new) + v_new.cwiseProduct(points.grad_y_matrix_EIGEN_boundary * u_new)) + parameters.mu * (points.laplacian_matrix_EIGEN_boundary * u_new) - (parameters.rho / parameters.dt) * (u_new - u_old);
        normal_mom_y = -parameters.rho * (u_new.cwiseProduct(points.grad_x_matrix_EIGEN_boundary * v_new) + v_new.cwiseProduct(points.grad_y_matrix_EIGEN_boundary * v_new)) + parameters.mu * (points.laplacian_matrix_EIGEN_boundary * v_new) - (parameters.rho / parameters.dt) * (v_new - v_old);
    }
    else
    {
        normal_mom_x = -parameters.rho * (u_new.cwiseProduct(points.grad_x_matrix_EIGEN_boundary * u_new) + v_new.cwiseProduct(points.grad_y_matrix_EIGEN_boundary * u_new)) + parameters.mu * (points.laplacian_matrix_EIGEN_boundary * u_new) - (parameters.rho / parameters.dt) * ((bdf2_alpha_1 * u_new) + (bdf2_alpha_2 * u_old) + (bdf2_alpha_3 * u_old_old));
        normal_mom_y = -parameters.rho * (u_new.cwiseProduct(points.grad_x_matrix_EIGEN_boundary * v_new) + v_new.cwiseProduct(points.grad_y_matrix_EIGEN_boundary * v_new)) + parameters.mu * (points.laplacian_matrix_EIGEN_boundary * v_new) - (parameters.rho / parameters.dt) * ((bdf2_alpha_1 * v_new) + (bdf2_alpha_2 * v_old) + (bdf2_alpha_3 * v_old_old));
    }

    double rhs, diag_coeff, off_diag_coeff;
    int dim = parameters.dimension, ivnb;

    for (int iv = 0; iv < points.nv; iv++)
    { //set boundary condition
        if (points.boundary_flag[iv])
        {
            if (!p_dirichlet_flag[iv]) //p_new for dirichlet BC is retained when p_prime is added (p_prime is zero at dirichlet BC)
            {                          //p_new for neumann BC has to be set explicitly
                rhs = (normal_mom_x[iv] * points.normal[dim * iv]) + (normal_mom_y[iv] * points.normal[dim * iv + 1]);
                for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
                {
                    ivnb = cloud.nb_points_col[i1];
                    if (ivnb == iv)
                        diag_coeff = (cloud.grad_x_coeff[i1] * points.normal[dim * iv]) + (cloud.grad_y_coeff[i1] * points.normal[dim * iv + 1]);
                    else
                    {
                        off_diag_coeff = (cloud.grad_x_coeff[i1] * points.normal[dim * iv]) + (cloud.grad_y_coeff[i1] * points.normal[dim * iv + 1]);
                        rhs = rhs - (off_diag_coeff * p_new[ivnb]);
                    }
                }
                p_new[iv] = rhs / diag_coeff;
            }
        }
    }
}

void SEMI_IMPLICIT_SPLIT_SOLVER::calc_vel_corr(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new)
{
    if (it == 0)
    { //BDF1: implicit Euler
        u_prime = ((points.grad_x_matrix_EIGEN_internal * p_prime.head(points.nv)) * (parameters.dt / parameters.rho));
        v_prime = ((points.grad_y_matrix_EIGEN_internal * p_prime.head(points.nv)) * (parameters.dt / parameters.rho));
    }
    else
    {
        u_prime = ((points.grad_x_matrix_EIGEN_internal * p_prime.head(points.nv)) * (parameters.dt / (bdf2_alpha_1 * parameters.rho)));
        v_prime = ((points.grad_y_matrix_EIGEN_internal * p_prime.head(points.nv)) * (parameters.dt / (bdf2_alpha_1 * parameters.rho)));
    }

    u_new = u_new - u_prime;
    v_new = v_new - v_prime;
    //internal points are set above; boundary values are obtained implicitly in the solver (for both dirichlet and neumann); u_prime and v_prime are zero at all boundaries
}

void SEMI_IMPLICIT_SPLIT_SOLVER::extras(POINTS &points, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old)
{
    // double total_steady_err, max_err, l1_err;
    // calc_max_l1_error(u_old, u_new, max_err, l1_err);
    // total_steady_err = l1_err / parameters.dt;
    // calc_max_l1_error(v_old, v_new, max_err, l1_err);
    // total_steady_err += l1_err / parameters.dt;
    // parameters.steady_error_log.push_back(total_steady_err);
    u_old_old = u_old, v_old_old = v_old;
    // return total_steady_err;
}

void SEMI_IMPLICIT_SPLIT_SOLVER::modify_vel_matrix(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new)
{
    int ivnb, dim = parameters.dimension, index;
    double value, unsteady_factor;
    if (it == 0)
        unsteady_factor = 1.0; //BDF1: implicit Euler
    else
        unsteady_factor = bdf2_alpha_1;
    for (int iv = 0; iv < points.nv; iv++)
        if (!points.boundary_flag[iv])
        { //coefficients of boundary points never updated for velocities
            for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
            {
                ivnb = cloud.nb_points_col[i1];
                value = -parameters.mu * cloud.laplacian_coeff[i1];                    //diffusion
                value = value + (u_new[iv] * parameters.rho * cloud.grad_x_coeff[i1]); //convection
                value = value + (v_new[iv] * parameters.rho * cloud.grad_y_coeff[i1]); //convection
                if (ivnb == iv)
                    value = value + (unsteady_factor * parameters.rho / parameters.dt); //diagonal term
                index = nb_points_col_matrix_u[i1];
                matrix_u.valuePtr()[index] = value;
                index = nb_points_col_matrix_v[i1];
                matrix_v.valuePtr()[index] = value;
            }
        }
}

void SEMI_IMPLICIT_SPLIT_SOLVER::set_vel_matrix(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new)
{
    matrix_u.resize(0, 0), matrix_v.resize(0, 0);
    vector<Eigen::Triplet<double>> triplet;
    int ivnb, dim = parameters.dimension;
    double value, unsteady_factor;
    if (it == 0)
        unsteady_factor = 1.0; //BDF1: implicit Euler
    else
        unsteady_factor = bdf2_alpha_1;
    for (int iv = 0; iv < points.nv; iv++)
    {
        if (points.boundary_flag[iv])
        {
            if (u_dirichlet_flag[iv])
                triplet.push_back(Eigen::Triplet<double>(iv, iv, 1.0));
            else
            {
                for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
                {
                    ivnb = cloud.nb_points_col[i1];
                    value = points.normal[dim * iv] * cloud.grad_x_coeff[i1] + points.normal[dim * iv + 1] * cloud.grad_y_coeff[i1];
                    triplet.push_back(Eigen::Triplet<double>(iv, ivnb, value));
                }
            }
        }
        else
        {
            for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
            {
                ivnb = cloud.nb_points_col[i1];
                value = -parameters.mu * cloud.laplacian_coeff[i1];                    //diffusion
                value = value + (u_new[iv] * parameters.rho * cloud.grad_x_coeff[i1]); //convection
                value = value + (v_new[iv] * parameters.rho * cloud.grad_y_coeff[i1]); //convection
                if (ivnb == iv)
                    value = value + (unsteady_factor * parameters.rho / parameters.dt); //diagonal term
                triplet.push_back(Eigen::Triplet<double>(iv, ivnb, value));
            }
        }
    }
    matrix_u.resize(points.nv, points.nv);
    matrix_u.setFromTriplets(triplet.begin(), triplet.end());
    matrix_u.makeCompressed();
    triplet.clear();

    for (int iv = 0; iv < points.nv; iv++)
    {
        if (points.boundary_flag[iv])
        {
            if (v_dirichlet_flag[iv])
                triplet.push_back(Eigen::Triplet<double>(iv, iv, 1.0));
            else
            {
                for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
                {
                    ivnb = cloud.nb_points_col[i1];
                    value = points.normal[dim * iv] * cloud.grad_x_coeff[i1] + points.normal[dim * iv + 1] * cloud.grad_y_coeff[i1];
                    triplet.push_back(Eigen::Triplet<double>(iv, ivnb, value));
                }
            }
        }
        else
        {
            for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
            {
                ivnb = cloud.nb_points_col[i1];
                value = -parameters.mu * cloud.laplacian_coeff[i1];                    //diffusion
                value = value + (u_new[iv] * parameters.rho * cloud.grad_x_coeff[i1]); //convection
                value = value + (v_new[iv] * parameters.rho * cloud.grad_y_coeff[i1]); //convection
                if (ivnb == iv)
                    value = value + (parameters.rho / parameters.dt); //diagonal term
                triplet.push_back(Eigen::Triplet<double>(iv, ivnb, value));
            }
        }
    }
    matrix_v.resize(points.nv, points.nv);
    matrix_v.setFromTriplets(triplet.begin(), triplet.end());
    matrix_v.makeCompressed();
    triplet.clear();
}

void SEMI_IMPLICIT_SPLIT_SOLVER::calc_nb_points_col_matrix(POINTS &points, CLOUD &cloud, PARAMETERS &parameters)
{
    int ivnb, index;
    nb_points_col_matrix_u.clear(), nb_points_col_matrix_v.clear();
    for (int i1 = 0; i1 < cloud.nb_points_col.size(); i1++) //initialize to -1
        nb_points_col_matrix_u.push_back(-1), nb_points_col_matrix_v.push_back(-1);
    for (int iv = 0; iv < points.nv; iv++)
        if (!points.boundary_flag[iv])
        { //coefficients of boundary points never updated for velocities; nb_points_col_matrix ahas value of [-1] at bounday points
            for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
            {
                index = -1;
                ivnb = cloud.nb_points_col[i1];
                for (int i2 = matrix_u.outerIndexPtr()[iv]; i2 < matrix_u.outerIndexPtr()[iv + 1]; i2++)
                    if (matrix_u.innerIndexPtr()[i2] == ivnb)
                    {
                        index = i2;
                        break;
                    }
                if (index < 0)
                {
                    cout << "\n\nError from SEMI_IMPLICIT_SPLIT_SOLVER::calc_nb_points_col_matrix in matrix_u, unable to find ivnb: " << ivnb << " for iv: " << iv << ", points.boundary_flag[iv]: " << points.boundary_flag[iv] << "\n\n";
                    throw bad_exception();
                }
                nb_points_col_matrix_u[i1] = index;
            }
        }
    for (int iv = 0; iv < points.nv; iv++)
        if (!points.boundary_flag[iv])
        { //coefficients of boundary points never updated for velocities; nb_points_col_matrix ahas value of [-1] at bounday points
            for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
            {
                index = -1;
                ivnb = cloud.nb_points_col[i1];
                for (int i2 = matrix_v.outerIndexPtr()[iv]; i2 < matrix_v.outerIndexPtr()[iv + 1]; i2++)
                    if (matrix_v.innerIndexPtr()[i2] == ivnb)
                    {
                        index = i2;
                        break;
                    }
                if (index < 0)
                {
                    cout << "\n\nError from SEMI_IMPLICIT_SPLIT_SOLVER::calc_nb_points_col_matrix in matrix_v, unable to find ivnb: " << ivnb << " for iv: " << iv << ", points.boundary_flag[iv]: " << points.boundary_flag[iv] << "\n\n";
                    throw bad_exception();
                }
                nb_points_col_matrix_v[i1] = index;
            }
        }
}

void SEMI_IMPLICIT_SPLIT_SOLVER::single_timestep_2d(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old, int it1, vector<int> &n_outer_iter_log, vector<double> &iterative_l1_err_log, vector<double> &iterative_max_err_log)
{
    it = it1;
    if (p_source.size() != p_new.size())
        p_new = Eigen::VectorXd::Zero(p_source.size());
    if (p_source.size() != p_old.size())
        p_old = Eigen::VectorXd::Zero(p_source.size());
    double total_steady_err;
    u_new = u_old, v_new = v_old, p_new = p_old; //initialize
    if (it % precond_freq_it == 0 || it == 0 || it == 1)
    {
        set_vel_matrix(points, cloud, parameters, u_new, v_new);
        solver_eigen_u.setTolerance(parameters.solver_tolerance); //default is machine precision (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#ac160a444af8998f93da9aa30e858470d)
        solver_eigen_u.setMaxIterations(parameters.n_iter);       //default is twice number of columns (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#af83de7a7d31d9d4bd1fef6222b07335b)
        solver_eigen_u.preconditioner().setDroptol(parameters.precond_droptol);
        solver_eigen_u.compute(matrix_u);
        solver_eigen_v.setTolerance(parameters.solver_tolerance); //default is machine precision (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#ac160a444af8998f93da9aa30e858470d)
        solver_eigen_v.setMaxIterations(parameters.n_iter);       //default is twice number of columns (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#af83de7a7d31d9d4bd1fef6222b07335b)
        solver_eigen_v.preconditioner().setDroptol(parameters.precond_droptol);
        solver_eigen_v.compute(matrix_v);
        calc_nb_points_col_matrix(points, cloud, parameters);
    }
    for (outer_iter = 0; outer_iter < n_outer_iter; outer_iter++)
    {
        u_iter_old = u_new, v_iter_old = v_new;
        modify_vel_matrix(points, cloud, parameters, u_new, v_new);
        calc_vel(points, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
        calc_pressure(points, cloud, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
        calc_vel_corr(points, cloud, parameters, u_new, v_new);
        iterative_l1_err = (u_new - u_iter_old).lpNorm<1>() / (parameters.dimension * u_new.size());
        iterative_l1_err += ((v_new - v_iter_old).lpNorm<1>() / (parameters.dimension * v_new.size()));
        iterative_max_err = (u_new - u_iter_old).lpNorm<Eigen::Infinity>() / parameters.dimension;
        iterative_max_err += ((v_new - v_iter_old).lpNorm<Eigen::Infinity>() / parameters.dimension);
        if ((outer_iter == n_outer_iter - 1) || (iterative_l1_err <= iterative_tolerance))
        {
            iterative_max_err_log.push_back(iterative_max_err);
            iterative_l1_err_log.push_back(iterative_l1_err);
            n_outer_iter_log.push_back(outer_iter + 1);
            // printf("    SEMI_IMPLICIT_SPLIT_SOLVER::single_timestep_2d outer_iter: %i, iterative l1_err: %g, max_err: %g, tolerance: %g\n", outer_iter, iterative_l1_err, iterative_max_err, iterative_tolerance);
        }
        if (iterative_l1_err <= iterative_tolerance)
            break;
    }
    extras(points, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
    // return total_steady_err;
}

void SEMI_IMPLICIT_SPLIT_SOLVER::single_timestep_2d(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old, Eigen::VectorXd &body_force_x, Eigen::VectorXd &body_force_y, int it1, vector<int> &n_outer_iter_log, vector<double> &iterative_l1_err_log, vector<double> &iterative_max_err_log)
{
    it = it1;
    if (p_source.size() != p_new.size())
        p_new = Eigen::VectorXd::Zero(p_source.size());
    if (p_source.size() != p_old.size())
        p_old = Eigen::VectorXd::Zero(p_source.size());
    double total_steady_err;
    u_new = u_old, v_new = v_old, p_new = p_old; //initialize
    if (it % precond_freq_it == 0 || it == 0 || it == 1)
    {
        set_vel_matrix(points, cloud, parameters, u_new, v_new);
        solver_eigen_u.setTolerance(parameters.solver_tolerance); //default is machine precision (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#ac160a444af8998f93da9aa30e858470d)
        solver_eigen_u.setMaxIterations(parameters.n_iter);       //default is twice number of columns (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#af83de7a7d31d9d4bd1fef6222b07335b)
        solver_eigen_u.preconditioner().setDroptol(parameters.precond_droptol);
        solver_eigen_u.compute(matrix_u);
        solver_eigen_v.setTolerance(parameters.solver_tolerance); //default is machine precision (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#ac160a444af8998f93da9aa30e858470d)
        solver_eigen_v.setMaxIterations(parameters.n_iter);       //default is twice number of columns (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#af83de7a7d31d9d4bd1fef6222b07335b)
        solver_eigen_v.preconditioner().setDroptol(parameters.precond_droptol);
        solver_eigen_v.compute(matrix_v);
        calc_nb_points_col_matrix(points, cloud, parameters);
    }
    for (outer_iter = 0; outer_iter < n_outer_iter; outer_iter++)
    {
        u_iter_old = u_new, v_iter_old = v_new;
        modify_vel_matrix(points, cloud, parameters, u_new, v_new);
        calc_vel(points, parameters, u_new, v_new, p_new, u_old, v_old, p_old, body_force_x, body_force_y);
        calc_pressure(points, cloud, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
        calc_vel_corr(points, cloud, parameters, u_new, v_new);
        iterative_l1_err = (u_new - u_iter_old).lpNorm<1>() / (parameters.dimension * u_new.size());
        iterative_l1_err += ((v_new - v_iter_old).lpNorm<1>() / (parameters.dimension * v_new.size()));
        iterative_max_err = (u_new - u_iter_old).lpNorm<Eigen::Infinity>() / parameters.dimension;
        iterative_max_err += ((v_new - v_iter_old).lpNorm<Eigen::Infinity>() / parameters.dimension);
        if ((outer_iter == n_outer_iter - 1) || (iterative_l1_err <= iterative_tolerance))
        {
            iterative_max_err_log.push_back(iterative_max_err);
            iterative_l1_err_log.push_back(iterative_l1_err);
            n_outer_iter_log.push_back(outer_iter + 1);
            // printf("    SEMI_IMPLICIT_SPLIT_SOLVER::single_timestep_2d outer_iter: %i, iterative l1_err: %g, max_err: %g, tolerance: %g\n", outer_iter, iterative_l1_err, iterative_max_err, iterative_tolerance);
        }
        if (iterative_l1_err <= iterative_tolerance)
            break;
    }
    extras(points, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
    // return total_steady_err;
}


#include "gate_matrix.hpp"
// Turn off Eigen warnings
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsuggest-override"
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#pragma GCC diagnostic ignored "-Wunused-function"
#include <Eigen/Dense>
#pragma GCC diagnostic pop
#include <cassert>
#include <iostream>
#include <unordered_map>

namespace {
using namespace qcor::utils;
using namespace std::complex_literals;
Eigen::Matrix2cd getGateMat(const qop_t &in_op) {
  static const Eigen::Matrix2cd X_mat = []() {
    Eigen::Matrix2cd result = Eigen::MatrixXcd::Zero(2, 2);
    result << 0.0, 1.0, 1.0, 0.0;
    return result;
  }();
  static const Eigen::Matrix2cd Y_mat = []() {
    Eigen::Matrix2cd result = Eigen::MatrixXcd::Zero(2, 2);
    result << 0.0, -1i, 1i, 0.0;
    return result;
  }();
  static const Eigen::Matrix2cd Z_mat = []() {
    Eigen::Matrix2cd result = Eigen::MatrixXcd::Zero(2, 2);
    result << 1.0, 0.0, 0.0, -1.0;
    return result;
  }();
  static const Eigen::Matrix2cd H_mat = []() {
    Eigen::Matrix2cd result = Eigen::MatrixXcd::Zero(2, 2);
    result << M_SQRT1_2, M_SQRT1_2, M_SQRT1_2, -M_SQRT1_2;
    return result;
  }();

  static const auto rx_mat = [](const std::vector<double> &in_params) {
    assert(in_params.size() == 1);
    auto theta = in_params[0];
    Eigen::Matrix2cd result = Eigen::MatrixXcd::Zero(2, 2);
    result << std::cos(0.5 * theta),
        std::complex<double>(0, -1) * std::sin(0.5 * theta),
        std::complex<double>(0, -1) * std::sin(0.5 * theta),
        std::cos(0.5 * theta);
    return result;
  };

  static const auto ry_mat = [](const std::vector<double> &in_params) {
    assert(in_params.size() == 1);
    auto theta = in_params[0];
    Eigen::Matrix2cd result = Eigen::MatrixXcd::Zero(2, 2);
    result << std::cos(0.5 * theta), -std::sin(0.5 * theta),
        std::sin(0.5 * theta), std::cos(0.5 * theta);
    return result;
  };

  static const auto rz_mat = [](const std::vector<double> &in_params) {
    assert(in_params.size() == 1);
    auto theta = in_params[0];
    Eigen::Matrix2cd result = Eigen::MatrixXcd::Zero(2, 2);
    result << std::exp(std::complex<double>(0, -0.5 * theta)), 0.0, 0.0,
        std::exp(std::complex<double>(0, 0.5 * theta));
    return result;
  };

  static const auto p_mat = [](const std::vector<double> &in_params) {
    assert(in_params.size() == 1);
    auto theta = in_params[0];
    Eigen::Matrix2cd result = Eigen::MatrixXcd::Zero(2, 2);
    result << 1.0, 0.0, 0.0, std::exp(std::complex<double>(0, theta));
    return result;
  };

  static const std::unordered_map<std::string, Eigen::Matrix2cd>
      GateMatrixCache = {{"x", X_mat},           {"y", Y_mat},
                         {"z", Z_mat},           {"h", H_mat},
                         {"t", p_mat({M_PI_4})}, {"tdg", p_mat({-M_PI_4})},
                         {"s", p_mat({M_PI_2})}, {"sdg", p_mat({-M_PI_2})}};
  const auto &gateName = in_op.first;
  const auto &gateParams = in_op.second;
  const auto it = GateMatrixCache.find(gateName);
  if (it != GateMatrixCache.end()) {
    return it->second;
  }
  if (gateName == "rx") {
    return rx_mat(gateParams);
  }
  if (gateName == "ry") {
    return ry_mat(gateParams);
  }
  if (gateName == "rz") {
    return rz_mat(gateParams);
  }
  throw std::runtime_error("Unknown single qubit gate: " + gateName);
  return Eigen::MatrixXcd::Zero(2, 2);
}

// If the matrix is finite: no NaN elements
template <typename Derived>
inline bool isFinite(const Eigen::MatrixBase<Derived> &x) {
  return ((x - x).array() == (x - x).array()).all();
}

// Default tolerace for validation
constexpr double TOLERANCE = 1e-6;

template <typename Derived>
bool allClose(const Eigen::MatrixBase<Derived> &in_mat1,
              const Eigen::MatrixBase<Derived> &in_mat2,
              double in_tol = TOLERANCE) {
  if (!isFinite(in_mat1) || !isFinite(in_mat2)) {
    return false;
  }

  if (in_mat1.rows() == in_mat2.rows() && in_mat1.cols() == in_mat2.cols()) {
    for (int i = 0; i < in_mat1.rows(); ++i) {
      for (int j = 0; j < in_mat1.cols(); ++j) {
        if (std::abs(in_mat1(i, j) - in_mat2(i, j)) > in_tol) {
          return false;
        }
      }
    }

    return true;
  }
  return false;
}

// Use Z-Y decomposition of Nielsen and Chuang (Theorem 4.1).
// An arbitrary one qubit gate matrix can be writen as
// U = [ exp(j*(a-b/2-d/2))*cos(c/2), -exp(j*(a-b/2+d/2))*sin(c/2)
//       exp(j*(a+b/2-d/2))*sin(c/2), exp(j*(a+b/2+d/2))*cos(c/2)]
// where a,b,c,d are real numbers.
// Then U = exp(j*a) Rz(b) Ry(c) Rz(d).
std::tuple<double, double, double, double>
singleQubitGateDecompose(const Eigen::Matrix2cd &matrix) {
  static const Eigen::Matrix2cd ID_MAT = Eigen::Matrix2cd::Identity();
  if (allClose(matrix, ID_MAT)) {
    return std::make_tuple(0.0, 0.0, 0.0, 0.0);
  }
  const auto checkParams = [&matrix](double a, double bHalf, double cHalf,
                                     double dHalf) {
    Eigen::Matrix2cd U;
    U << std::exp(1i * (a - bHalf - dHalf)) * std::cos(cHalf),
        -std::exp(1i * (a - bHalf + dHalf)) * std::sin(cHalf),
        std::exp(1i * (a + bHalf - dHalf)) * std::sin(cHalf),
        std::exp(1i * (a + bHalf + dHalf)) * std::cos(cHalf);

    return allClose(U, matrix);
  };

  double a, bHalf, cHalf, dHalf;
  const double TOLERANCE = 1e-9;
  if (std::abs(matrix(0, 1)) < TOLERANCE) {
    auto two_a = fmod(std::arg(matrix(0, 0) * matrix(1, 1)), 2 * M_PI);
    a = (std::abs(two_a) < TOLERANCE || std::abs(two_a) > 2 * M_PI - TOLERANCE)
            ? 0
            : two_a / 2.0;
    auto dHalf = 0.0;
    auto b = std::arg(matrix(1, 1)) - std::arg(matrix(0, 0));
    std::vector<double> possibleBhalf{fmod(b / 2.0, 2 * M_PI),
                                      fmod(b / 2.0 + M_PI, 2.0 * M_PI)};
    std::vector<double> possibleChalf{0.0, M_PI};
    bool found = false;
    for (size_t i = 0; i < possibleBhalf.size(); ++i) {
      for (size_t j = 0; j < possibleChalf.size(); ++j) {
        bHalf = possibleBhalf[i];
        cHalf = possibleChalf[j];
        if (checkParams(a, bHalf, cHalf, dHalf)) {
          found = true;
          break;
        }
      }
      if (found) {
        break;
      }
    }
    assert(found);
  } else if (std::abs(matrix(0, 0)) < TOLERANCE) {
    auto two_a = fmod(std::arg(-matrix(0, 1) * matrix(1, 0)), 2 * M_PI);
    a = (std::abs(two_a) < TOLERANCE || std::abs(two_a) > 2 * M_PI - TOLERANCE)
            ? 0
            : two_a / 2.0;
    dHalf = 0;
    auto b = std::arg(matrix(1, 0)) - std::arg(matrix(0, 1)) + M_PI;
    std::vector<double> possibleBhalf{fmod(b / 2., 2 * M_PI),
                                      fmod(b / 2. + M_PI, 2 * M_PI)};
    std::vector<double> possibleChalf{M_PI / 2., 3. / 2. * M_PI};
    bool found = false;
    for (size_t i = 0; i < possibleBhalf.size(); ++i) {
      for (size_t j = 0; j < possibleChalf.size(); ++j) {
        bHalf = possibleBhalf[i];
        cHalf = possibleChalf[j];
        if (checkParams(a, bHalf, cHalf, dHalf)) {
          found = true;
          break;
        }
      }
      if (found) {
        break;
      }
    }
    assert(found);
  } else {
    auto two_a = fmod(std::arg(matrix(0, 0) * matrix(1, 1)), 2 * M_PI);
    a = (std::abs(two_a) < TOLERANCE || std::abs(two_a) > 2 * M_PI - TOLERANCE)
            ? 0
            : two_a / 2.0;
    auto two_d = 2. * std::arg(matrix(0, 1)) - 2. * std::arg(matrix(0, 0));
    std::vector<double> possibleDhalf{
        fmod(two_d / 4., 2 * M_PI), fmod(two_d / 4. + M_PI / 2., 2 * M_PI),
        fmod(two_d / 4. + M_PI, 2 * M_PI),
        fmod(two_d / 4. + 3. / 2. * M_PI, 2 * M_PI)};
    auto two_b = 2. * std::arg(matrix(1, 0)) - 2. * std::arg(matrix(0, 0));
    std::vector<double> possibleBhalf{
        fmod(two_b / 4., 2 * M_PI), fmod(two_b / 4. + M_PI / 2., 2 * M_PI),
        fmod(two_b / 4. + M_PI, 2 * M_PI),
        fmod(two_b / 4. + 3. / 2. * M_PI, 2 * M_PI)};
    auto tmp = std::acos(std::abs(matrix(1, 1)));
    std::vector<double> possibleChalf{
        fmod(tmp, 2 * M_PI), fmod(tmp + M_PI, 2 * M_PI),
        fmod(-1. * tmp, 2 * M_PI), fmod(-1. * tmp + M_PI, 2 * M_PI)};
    bool found = false;
    for (size_t i = 0; i < possibleBhalf.size(); ++i) {
      for (size_t j = 0; j < possibleChalf.size(); ++j) {
        for (size_t k = 0; k < possibleDhalf.size(); ++k) {
          bHalf = possibleBhalf[i];
          cHalf = possibleChalf[j];
          dHalf = possibleDhalf[k];
          if (checkParams(a, bHalf, cHalf, dHalf)) {
            found = true;
            break;
          }
        }
        if (found) {
          break;
        }
      }
      if (found) {
        break;
      }
    }
    assert(found);
  }

  // Final check:
  assert(checkParams(a, bHalf, cHalf, dHalf));
  return std::make_tuple(a, bHalf, cHalf, dHalf);
};

std::vector<pauli_decomp_t>
simplifySingleQubitSeq(double zAngleBefore, double yAngle, double zAngleAfter) {
  auto zExpBefore = zAngleBefore / M_PI - 0.5;
  auto middleExp = yAngle / M_PI;
  std::string middlePauli = "rx";
  auto zExpAfter = zAngleAfter / M_PI + 0.5;

  // Helper functions:
  const auto isNearZeroMod = [](double a, double period) -> bool {
    const auto halfPeriod = period / 2;
    const double TOL = 1e-8;
    return std::abs(fmod(a + halfPeriod, period) - halfPeriod) < TOL;
  };

  const auto toQuarterTurns = [](double in_exp) -> int {
    return static_cast<int>(round(2 * in_exp)) % 4;
  };

  const auto isCliffordRotation = [&](double in_exp) -> bool {
    return isNearZeroMod(in_exp, 0.5);
  };

  const auto isQuarterTurn = [&](double in_exp) -> bool {
    return (isCliffordRotation(in_exp) && toQuarterTurns(in_exp) % 2 == 1);
  };

  const auto isHalfTurn = [&](double in_exp) -> bool {
    return (isCliffordRotation(in_exp) && toQuarterTurns(in_exp) == 2);
  };

  const auto isNoTurn = [&](double in_exp) -> bool {
    return (isCliffordRotation(in_exp) && toQuarterTurns(in_exp) == 0);
  };

  // Clean up angles
  if (isCliffordRotation(zExpBefore)) {
    if ((isQuarterTurn(zExpBefore) || isQuarterTurn(zExpAfter)) !=
        (isHalfTurn(middleExp) && isNoTurn(zExpBefore - zExpAfter))) {
      zExpBefore += 0.5;
      zExpAfter -= 0.5;
      middlePauli = "ry";
    }
    if (isHalfTurn(zExpBefore) || isHalfTurn(zExpAfter)) {
      zExpBefore -= 1;
      zExpAfter += 1;
      middleExp = -middleExp;
    }
  }
  if (isNoTurn(middleExp)) {
    zExpBefore += zExpAfter;
    zExpAfter = 0;
  } else if (isHalfTurn(middleExp)) {
    zExpAfter -= zExpBefore;
    zExpBefore = 0;
  }

  std::vector<pauli_decomp_t> composite;
  if (!isNoTurn(zExpBefore)) {
    composite.emplace_back(std::make_pair("rz", zExpBefore * M_PI));
  }
  if (!isNoTurn(middleExp)) {
    composite.emplace_back(std::make_pair(middlePauli, middleExp * M_PI));
  }
  if (!isNoTurn(zExpAfter)) {
    composite.emplace_back(std::make_pair("rz", zExpAfter * M_PI));
  }
  return composite;
}

} // namespace
namespace qcor {
namespace utils {
std::vector<pauli_decomp_t>
decompose_gate_sequence(const std::vector<qop_t> &op_list) {
  Eigen::Matrix2cd totalU = Eigen::MatrixXcd::Identity(2, 2);
  for (const auto &op : op_list) {
    // std::cout << "Gate: " << op.first << ": " << op.second.size() << "\n";
    totalU = getGateMat(op) * totalU;
  }

  // std::cout << "Total U = " << totalU << "\n";
  auto [a, bHalf, cHalf, dHalf] = singleQubitGateDecompose(totalU);

  // Validate U = exp(j*a) Rz(b) Ry(c) Rz(d).
  const auto validate = [](const Eigen::Matrix2cd &in_mat, double a, double b,
                           double c, double d) {
    Eigen::Matrix2cd Rz_b, Ry_c, Rz_d;
    Rz_b << std::exp(-1i * b / 2.0), 0, 0, std::exp(1i * b / 2.0);
    Rz_d << std::exp(-1i * d / 2.0), 0, 0, std::exp(1i * d / 2.0);
    Ry_c << std::cos(c / 2), -std::sin(c / 2), std::sin(c / 2), std::cos(c / 2);
    Eigen::Matrix2cd mat = std::exp(1i * a) * Rz_b * Ry_c * Rz_d;
    return allClose(in_mat, mat);
  };
  // Validate the *raw* decomposition
  assert(validate(totalU, a, 2 * bHalf, 2 * cHalf, 2 * dHalf));
  return simplifySingleQubitSeq(2 * dHalf, 2 * cHalf, 2 * bHalf);
}
} // namespace utils
} // namespace qcor


#include <boost/numeric/odeint/stepper/runge_kutta4_classic.hpp>


#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <mimkl/data_structures.hpp>
#include <mimkl/definitions.hpp>
#include <mimkl/kernels.hpp>
#include <mimkl/linear_algebra.hpp>
#include <stdexcept>

using mimkl::data_structures::DataFrame;
using mimkl::data_structures::indexing_from_vector;
using mimkl::data_structures::range;
using mimkl::definitions::Indexing;

int main(int argc, char **argv)
{
    try
    {
        Eigen::Matrix<double, 2, 3> X;
        X << 1., 2., 3., 4., 5., 6.;

        Eigen::MatrixXd Y(4, 3);
        Y << 1., 2., 3., 4., 5., 6., 7., 8., 9., 10., 11., 12.;

        // check range()
        std::vector<int> vec = range(X.cols()); //
        std::cout << "a range:\n";
        for (std::vector<int>::const_iterator i = vec.begin(); i != vec.end();
             ++i)
            std::cout << *i << " ";
        std::cout << std::endl;
        // check indexing_from_vector()
        Indexing an_indexing = indexing_from_vector(vec);
        std::cout << "first element of an Indexing\n"
                  << an_indexing.begin()->second << std::endl;

        // test constructors
        DataFrame df;       // default
        DataFrame df1 = df; // trivial
        DataFrame dfY = Y;  // assignment
        df = Y;
        DataFrame dfX = X;

        DataFrame df_copy(X);              // copy
        std::cout << df_copy << std::endl; // operator<<

        DataFrame df_copy_withindex(X, indexing_from_vector(range(X.rows())),
                                    indexing_from_vector(range(X.cols()))); // copy

        // construct from given vector of strings
        std::vector<std::string> labels;
        labels.push_back("patient_1");
        labels.push_back("patient_2");
        labels.push_back("patient_3");
        labels.push_back("patient_3");
        Indexing another_indexing;
        Index i = 0;
        for (const auto &element : labels)
        {
            another_indexing.emplace(element, i++);
        }
        std::cout << "first key of an Indexing\n"
                  << another_indexing.begin()->first << std::endl;
        DataFrame df_ylabels(Y, another_indexing, an_indexing); // copy
        std::cout << "duplicate row labels df\n" << df_ylabels << std::endl;

        //// slicing
        // no such index found
        try
        {
            df_copy["no_way_josé"];
        }
        catch (...)
        {
            std::cout << "it's ok josé" << std::endl;
        }

        DataFrame col_slice = df_copy["1"]; // 2nd col
        std::cout << "col slice\n" << col_slice << std::endl;
        DataFrame row_slice = df_copy.loc("1"); // 2nd row

        Eigen::MatrixXd row_mat(1, 3);
        row_mat << 4., 5., 6.;
        DataFrame df_row_mat(row_mat);
        row_slice = row_slice - df_row_mat; // some df operation
        std::cout << "row slice with substraction\n" << row_slice << std::endl;
        std::cout << ".norm()\n" << row_slice.norm() << std::endl;

        std::cout << df_ylabels << std::endl;
        DataFrame labeled_row = df_ylabels.loc("patient_2"); // 2nd row
        std::cout << "a labeled row:\n" << labeled_row << std::endl;
        // duplicate label
        DataFrame duplicate_row = df_ylabels.loc("patient_3"); // 2nd row
        std::cout << "two rows with same label:\n"
                  << duplicate_row << std::endl;

        // chained
        DataFrame an_element = (df_ylabels.loc("patient_2"))["0"]; // row and col
        std::cout << "chained, an element:\n" << an_element << std::endl;

        std::vector<std::string> col_vec(2);
        col_vec.push_back("0");
        col_vec.push_back("2");
        DataFrame two_element =
        df_ylabels.loc("patient_2")[col_vec]; // row and col
        std::cout << "chained, a row two elements:\n"
                  << two_element << std::endl;

        Eigen::MatrixXd row_ref(1, 2);
        row_ref << 4., 6.;
        std::cout << "matrix row :\n" << row_ref << std::endl;

        DataFrame ref = row_ref;
        std::cout << "df row :\n" << ref << std::endl;

        Eigen::MatrixXd m;
        m = ref.matrix(); // protected constructor
        std::cout << "get plain matrix back :\n"
                  << m << std::endl; // no indexing
        assert((ref.norm() == row_ref.norm()) && " norm on df ");

        assert(((two_element - row_ref).norm() == 0.0) &&
               "matrix operation on DF and matrix");
        assert(((two_element - ref).norm() == 0.0) &&
               "matrix operation on DFs");

        // multiplication, lin. kernel
        Eigen::Matrix<double, 2, 2> K_lin_ref;
        K_lin_ref << 1., 4., 4., 16.;
        Eigen::SparseMatrix<double> L(3, 3);
        L.insert(0, 0) = 1.;
        assert(
        (((dfX * L.selfadjointView<Eigen::Lower>() * dfX.adjoint()) - K_lin_ref)
         .norm() == 0.0) &&
        "df multiplication / linear kernel directly");

        return EXIT_SUCCESS;
    }
    catch (const std::exception &e)
    {
        std::cerr << e.what();
        return EXIT_FAILURE;
    }
}


/* Copyright (c) 2016, the Cap authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#define BOOST_TEST_MODULE DistributedEnergyStorage

#include "main.cc"

#include <cap/energy_storage_device.h>
#include <cap/mp_values.h>
#include <deal.II/base/types.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/dofs/dof_handler.h>
#include <boost/test/unit_test.hpp>
#include <boost/format.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/info_parser.hpp>
#include <cmath>
#include <iostream>
#include <fstream>
#include <numeric>

namespace cap
{

void distributed_problem(std::shared_ptr<cap::EnergyStorageDevice> dev)
{
  double const charge_current = 5e-3;
  // This is the values computed using one processor
  double const exact_voltage = 0.24307431815;
  double const time_step = 1e-2;
  double const percent_tolerance = 1e-2;
  double computed_voltage;
  double computed_current;
  for (unsigned int i = 0; i < 3; ++i)
    dev->evolve_one_time_step_constant_current(time_step, charge_current);
  dev->get_current(computed_current);
  dev->get_voltage(computed_voltage);

  BOOST_CHECK_CLOSE(computed_voltage, exact_voltage, percent_tolerance);
  BOOST_CHECK_CLOSE(computed_current, charge_current, percent_tolerance);
}
}

BOOST_AUTO_TEST_CASE(test_distributed_energy_storage)
{
  // Parse input file
  boost::property_tree::ptree device_database;
  boost::property_tree::info_parser::read_info("super_capacitor.info",
                                               device_database);
  boost::property_tree::ptree geometry_database;
  boost::property_tree::info_parser::read_info("generate_mesh.info",
                                               geometry_database);
  device_database.put_child("geometry", geometry_database);

  std::shared_ptr<cap::EnergyStorageDevice> device =
      cap::EnergyStorageDevice::build(device_database,
                                      boost::mpi::communicator());

  cap::distributed_problem(device);
}


/*
 * Copyright 2020 Adobe. All rights reserved.
 * This file is licensed to you under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License. You may obtain a copy
 * of the License at http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software distributed under
 * the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR REPRESENTATIONS
 * OF ANY KIND, either express or implied. See the License for the specific language
 * governing permissions and limitations under the License.
 */
#include <lagrange/testing/common.h>

#include <lagrange/utils/geometry2d.h>
#include <Eigen/Core>

TEST_CASE("utils-geometry2d")
{
    using namespace lagrange;
    using Vec2 = Eigen::Vector2d;

    REQUIRE(sqr_minimum_distance(Vec2(0, 0), Vec2(0, 1), Vec2(0, 0)) == 0.0);
    REQUIRE(sqr_minimum_distance(Vec2(0, 0), Vec2(0, 1), Vec2(0, 1)) == 0.0);
    REQUIRE(sqr_minimum_distance(Vec2(0, 0), Vec2(0, 1), Vec2(0, .5)) == 0.0);

    REQUIRE(sqr_minimum_distance(Vec2(0, 0), Vec2(0, 1), Vec2(1, 0)) == (1 * 1));
    REQUIRE(sqr_minimum_distance(Vec2(0, 0), Vec2(0, 1), Vec2(1, .2)) == (1 * 1));

    REQUIRE(sqr_minimum_distance(Vec2(0, 0), Vec2(0, 1), Vec2(0, -1)) == (1 * 1));
    REQUIRE(sqr_minimum_distance(Vec2(0, 0), Vec2(0, 1), Vec2(0, 2)) == (1 * 1));
    REQUIRE(sqr_minimum_distance(Vec2(0, 0), Vec2(0, 1), Vec2(0, 3)) == (2 * 2));

    REQUIRE(sqr_minimum_distance(Vec2(0, 0), Vec2(1, 1), Vec2(0, 1)) == .5);
}


#include<iostream>
#include <armadillo>

using namespace std;
using namespace arma;

void testing_armadillo(void);

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

	testing_armadillo();

	return 0;
}

void testing_armadillo(void)
{
	cout<<endl<<"Testing armadillo::mat"<<endl<<endl;

	mat A(4, 5, fill::randu);
	mat B(4, 5, fill::randn);

	cout<<"A = "<<A<<endl;
	cout<<"B = "<<B<<endl;

	cout<<"A*B.t() = "<<A*B.t()<<endl;

}


#include <walrus_stair_detector/walrus_stair_detector.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <gtest/gtest.h>
#include "yaml-cpp/yaml.h"
#include <boost/filesystem.hpp>

std::string pcd_file;
std::string config_file;

namespace YAML {
template<>
struct convert<Eigen::Vector3f> {
  static bool decode(const YAML::Node& node, Eigen::Vector3f& rhs) {
    if(!node.IsSequence() || node.size() != 3) {
      return false;
    }

    rhs[0] = node[0].as<double>();
    rhs[1] = node[1].as<double>();
    rhs[2] = node[2].as<double>();
    return true;
  }
};
}

#define EXPECT_VECTOR_ANGLE_LE(val1, val2, max_angle)			\
  do {									\
    Eigen::Vector3f val1_norm = val1.normalized();			\
    Eigen::Vector3f val2_norm = val2.normalized();			\
    if(val1_norm != val2_norm) {					\
      double angle = acos(val1_norm.dot(val2_norm));			\
      EXPECT_LE(fabs(angle), max_angle)					\
	<< "Expected angle between "					\
	<< "[" << val1[0] << ", " << val1[1] << ", " << val1[2] << "]"	\
	<< " and [" << val2[0] << ", " << val2[1] << ", " << val2[2] << "]" \
	<< " <= " << max_angle << ", but was " << angle;		\
    }									\
  } while(0)

#define EXPECT_VECTOR_NEAR(val1, val2, abs_error)			\
  do {									\
    double error = (val1 - val2).norm();				\
    EXPECT_LE(error, abs_error)						\
      << "Expected error between "					\
      << "[" << val1[0] << ", " << val1[1] << ", " << val1[2] << "]"	\
      << " and [" << val2[0] << ", " << val2[1] << ", " << val2[2] << "]" \
      << " <= " << abs_error << ", but was " << error;			\
  } while(0)

struct StairDetectorTestData {
  StairDetectorTestData(std::string pcd_file, std::string config_file) : pcd_file(pcd_file), config_file(config_file) {}
  std::string pcd_file;
  std::string config_file;
};
void PrintTo(const StairDetectorTestData& data, ::std::ostream* os) {
  *os << "PCD File: " << data.pcd_file << ", Config File: " << data.config_file;
}

class StairDetectorTest : public ::testing::TestWithParam<StairDetectorTestData> {};

TEST_P(StairDetectorTest, detectStairs) {
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::io::loadPCDFile(GetParam().pcd_file, *cloud);

  YAML::Node config = YAML::LoadFile(GetParam().config_file);
  YAML::Node sensor_config = config["sensor"];
  YAML::Node stairs_config = config["stairs"];

  Eigen::Vector3f vertical_estimate = sensor_config["vertical"].as<Eigen::Vector3f>();

  walrus_stair_detector::WalrusStairDetector detector;
  std::vector<walrus_stair_detector::StairModel> stairs;
  detector.detect(cloud, vertical_estimate, &stairs);

  ASSERT_EQ(stairs_config.size(), stairs.size());

  for(int i = 0; i < stairs.size(); ++i) {
    YAML::Node stair_config = stairs_config[i];
    walrus_stair_detector::StairModel& stair = stairs[i];
    EXPECT_NEAR(stair_config["rise"].as<double>(), stair.rise, 0.012);
    EXPECT_NEAR(stair_config["run"].as<double>(), stair.run, 0.008);

    EXPECT_VECTOR_ANGLE_LE(stair_config["direction"].as<Eigen::Vector3f>(), stair.direction, 0.05);

    Eigen::Vector3f expected_origin = stair_config["origin"].as<Eigen::Vector3f>();

    double expected_x = expected_origin.dot(stair.horizontal);
    double expected_y = expected_origin.dot(stair.vertical);
    double expected_z = expected_origin.dot(stair.direction);

    double actual_x = stair.origin.dot(stair.horizontal);
    double actual_y = stair.origin.dot(stair.vertical);
    double actual_z = stair.origin.dot(stair.direction);

    // Expect the horizontal component to be less accurate
    EXPECT_NEAR(expected_x, actual_x, 0.14);

    double max_origin_center_error = 0.045;
    Eigen::Vector3f expected_origin_center(0, expected_y, expected_z);
    Eigen::Vector3f origin_center(0, actual_y, actual_z);
    if(stair_config["can_miss_first_stair"].IsDefined() && stair_config["can_miss_first_stair"].as<bool>()){
      Eigen::Vector3f upper_origin_center(0, actual_y - stair.rise, actual_z - stair.run);
      if((expected_origin_center - upper_origin_center).norm() <= max_origin_center_error)
	EXPECT_VECTOR_NEAR(expected_origin_center, upper_origin_center, max_origin_center_error);
      else
	EXPECT_VECTOR_NEAR(expected_origin_center, origin_center, max_origin_center_error);
    }
    else {
      EXPECT_VECTOR_NEAR(expected_origin_center, origin_center, max_origin_center_error);
    }

    EXPECT_GE(stair_config["width"].as<double>(), stair.width);
    EXPECT_LE(stair_config["width"].as<double>()-0.17, stair.width);

    if(stair_config["num_stairs"].IsScalar()) {
      EXPECT_EQ(stair_config["num_stairs"].as<int>(), stair.num_stairs);
    }
    else {
      EXPECT_LE(stair_config["num_stairs"][0].as<int>(), stair.num_stairs);
      EXPECT_GE(stair_config["num_stairs"][1].as<int>(), stair.num_stairs);
    }
  }
}

std::string find_package(const std::string& package, const std::string& path) {
  std::stringstream command;
  command << "catkin_find " << package << " " << path;

  char buf[1000];
  FILE *fp = popen(command.str().c_str(), "r");
  if (fp == NULL) {
    return "";
  }

  std::string result;
  while (fgets(buf, sizeof(buf), fp) != NULL) {
    result = buf;
    break;
  }

  if(result.size() > 0 && result[result.size()-1] == '\n')
    result = result.substr(0, result.size()-1);

  pclose(fp);
  return result;
}

std::vector<StairDetectorTestData> GenerateStairDetectorParameterList() {
  std::vector<StairDetectorTestData> data;
  std::string testdata_root_string = find_package("walrus_testdata", "kinectv2/stairs");

  if(testdata_root_string.size() > 0) {
    boost::filesystem::path testdata_root (testdata_root_string);
    std::cout << "Searching for test data in: " << testdata_root << std::endl;
    if(!boost::filesystem::is_directory(testdata_root))
      throw new std::runtime_error("Test data path is not a directory");

    boost::filesystem::directory_iterator end;
    for(boost::filesystem::directory_iterator itr(testdata_root); itr != end; ++itr) {
      boost::filesystem::path test_collection = itr->path();
      std::cout << "\tFound test collection: " << test_collection.filename() << std::endl;

      boost::filesystem::path test_collection_description = test_collection;
      test_collection_description/="description.yaml";
      if(boost::filesystem::exists(test_collection_description)){
	for(boost::filesystem::directory_iterator itr2(test_collection); itr2 != end; ++itr2) {
	  if(itr2->path().extension() == ".pcd") {
	    std::cout << "\t\tFound test: " << itr2->path().filename() << std::endl;
	    data.push_back(StairDetectorTestData(itr2->path().native(), test_collection_description.native()));
	  }
	}
      }
      else
	std::cout << "Did not find test description: " << test_collection_description << std::endl;
    }

  }
  else
    throw new std::runtime_error("Could not find test data folder in walrus_testdata");
  return data;
}
INSTANTIATE_TEST_CASE_P(
			StairTestData,
			StairDetectorTest,
			testing::ValuesIn(GenerateStairDetectorParameterList()));

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);

  return RUN_ALL_TESTS();
}


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *    Hameduddin, I. Rotate vector(s) about axis, rodrigues_rot.m, available at
 *        http://www.mathworks.com/matlabcentral/fileexchange/34426-rotate-vectors-about-axis,
 *        2012, last accessed: 11th January, 2014.
 *    Murray, G. Rotation Matrices and Formulas java script, RotationMatrix.java available at
 *        https://sites.google.com/site/glennmurray/Home/rotation-matrices-and-formulas, 2011.
 *        last accessed: 20th January, 2014.
 *
 */

#define BOOST_TEST_MAIN

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

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Mathematics/BasicMathematics/rotationAboutArbitraryAxis.h"

namespace tudat
{
namespace unit_tests
{

//! Test suite for rotations about about arbitrary axes.
BOOST_AUTO_TEST_SUITE( test_RotationAboutArbitraryAxis )

BOOST_AUTO_TEST_CASE( test_RotationAboutArbitraryAxis_PointRotationWithCommonOrigin )
{

    //Benchmark data is obtained using Matlab Script (Hameduddin, 2012).

    //Set expected rotated vector.
    const Eigen::Vector3d expectedRotatedPosition = Eigen::Vector3d( 0.0, 1.414213562373095, 1.0 );

    //Set origin of rotation.
    const Eigen::Vector3d originOfRotation = Eigen::Vector3d( 0.0, 0.0, 0.0 );

    //Set angle of rotation [rad].
    const double angleOfRotation = mathematical_constants::PI / 4.0;

    //Set axis of rotation.
    const Eigen::Vector3d axisOfRotation = Eigen::Vector3d( 0.0, 0.0, 1.0 );

    //Set initial position of point.
    const Eigen::Vector3d initialPositionOfPoint = Eigen::Vector3d( 1.0, 1.0, 1.0 );

    //Compute rotated position.
    const Eigen::Vector3d computedRotatedPosition = basic_mathematics::
        computeRotationOfPointAboutArbitraryAxis( originOfRotation, angleOfRotation,
                                                  axisOfRotation, initialPositionOfPoint );

    // Compare computed and expected vectors.
    BOOST_CHECK_SMALL( computedRotatedPosition.x( ), std::numeric_limits< double >::epsilon( ) );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedRotatedPosition.segment( 1, 2 ),
                                       expectedRotatedPosition.segment( 1, 2),
                                       std::numeric_limits< double >::epsilon( ) );

}

BOOST_AUTO_TEST_CASE( test_RotationAboutArbitraryAxis_PointRotationWithDifferentOrigins )
{

    //Benchmark data is obtained using java script (Murray, 2013).

    //Set expected rotated vector.
    const Eigen::Vector3d expectedRotatedPosition = Eigen::Vector3d( 3.156561876696307,
                                                                     -5.97145870839039,
                                                                     -4.418680390057799 );
    //Set origin of rotation.
    const Eigen::Vector3d originOfRotation = Eigen::Vector3d( 4.0, 1.0, -1.0 );

    //Set angle of rotation [rad]
    const double angleOfRotation = 7.0;

    //Set axis of rotation.
    const Eigen::Vector3d axisOfRotation = Eigen::Vector3d( 2.0, -2.0, 3.0 );

    //Set initial position of point.
    const Eigen::Vector3d initialPositionOfPoint = Eigen::Vector3d( -1.0, -5.0, -1.0 );

    //Compute rotated position.
    const Eigen::Vector3d computedRotatedPosition = basic_mathematics::
        computeRotationOfPointAboutArbitraryAxis( originOfRotation,  angleOfRotation,
                                                  axisOfRotation, initialPositionOfPoint );

    // Compare computed and expected radiation pressure acceleration vectors.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedRotatedPosition,
                                       expectedRotatedPosition,
                                       std::numeric_limits<double>::epsilon( ) );

}

BOOST_AUTO_TEST_CASE( test_RotationAboutArbitraryAxis_PointRotationWithDifferentOrigins2 )
{

    //Benchmark data is obtained using java script (Murray, 2013).

    //Set expected rotated vector.
    const Eigen::Vector3d expectedRotatedPosition = Eigen::Vector3d( 15.71267522236938,
                                                                     -0.9723168350942417,
                                                                      9.449538267504582 );
    //Set origin of rotation.
    const Eigen::Vector3d originOfRotation = Eigen::Vector3d( 1.5, 3.8, 12.0 );

    //Set angle of rotation [rad]
    const double angleOfRotation = 3.0;

    //Set axis of rotation.
    const Eigen::Vector3d axisOfRotation = Eigen::Vector3d( -4.6, 6.75, 7.7 );

    //Set initial position of point.
    const Eigen::Vector3d initialPositionOfPoint = Eigen::Vector3d( -4.3, -5.2, 1.2 );

    //Compute rotated position.
    const Eigen::Vector3d computedRotatedPosition = basic_mathematics::
        computeRotationOfPointAboutArbitraryAxis( originOfRotation, angleOfRotation,
                                                  axisOfRotation, initialPositionOfPoint );

    // Compare computed and expected radiation pressure acceleration vectors.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedRotatedPosition, expectedRotatedPosition, 1.0e-14 );

}

BOOST_AUTO_TEST_CASE( test_RotationAboutArbitraryAxis_VectorRotationWithCommonOrigin )
{

    //Benchmark data is obtained using Matlab Script (Hameduddin, 2012).

    //Set expected rotated vector.
    const Eigen::Vector3d expectedRotatedVector = Eigen::Vector3d( -5.480598288892924,
                                                                    0.532794739754852,
                                                                    6.138336269794405 );
    //Set origin of rotation.
    const Eigen::Vector3d originOfRotation = Eigen::Vector3d( 0.0, 0.0, 0.0 );

    //Set angle of rotation [rad].
    const double angleOfRotation = 12.0;

    //Set axis of rotation.
    const Eigen::Vector3d axisOfRotation = Eigen::Vector3d( -1.0, -2.0, -3.0 );

    //Set initial position of vector tail.
    const Eigen::Vector3d initialPositionOfVectorTail = Eigen::Vector3d( 1.0, 3.0, 5.0 );

    //Set initial vector.
    const Eigen::Vector3d initialVector = Eigen::Vector3d( -6.0, 4.0, 4.0 );

    //Compute rotated position.
    const Eigen::Vector3d computedRotatedVector = basic_mathematics::
        computeRotationOfVectorAboutArbitraryAxis( originOfRotation, angleOfRotation,
                                                   axisOfRotation, initialPositionOfVectorTail,
                                                   initialVector );

    // Compare computed and expected radiation pressure acceleration vectors
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedRotatedVector, expectedRotatedVector, 1.0e-14 );

}

BOOST_AUTO_TEST_CASE( test_RotationAboutArbitraryAxis_VectorRotationWithDifferentOrigins )
{

    //Benchmark data is obtained using java script (Murray, 2013).

    //Set expected rotated vector.
    const Eigen::Vector3d expectedRotatedVector = Eigen::Vector3d( -7.15233485964669,
                                                                   -2.4444137866784175,
                                                                    8.308967883857736 );
    //Set origin of rotation.
    const Eigen::Vector3d originOfRotation = Eigen::Vector3d( 1.5, 3.8, 12.0 );

    //Set angle of rotation [rad].
    const double angleOfRotation = 3.0;

    //Set axis of rotation.
    const Eigen::Vector3d axisOfRotation = Eigen::Vector3d( -4.6, 6.75, 7.7 );

    //Set initial position of vector tail.
    const Eigen::Vector3d initialPositionOfVectorTail = Eigen::Vector3d( -4.3, -5.2, 1.2 );

    //Set initial vector.
    const Eigen::Vector3d initialVector = Eigen::Vector3d( 0.3, 11.2, 0.8 );

    //Compute rotated position.
    const Eigen::Vector3d computedRotatedVector = basic_mathematics::
        computeRotationOfVectorAboutArbitraryAxis( originOfRotation, angleOfRotation,
                                                   axisOfRotation, initialPositionOfVectorTail,
                                                   initialVector );

  // Compare computed and expected rotated vectors.
  TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedRotatedVector, expectedRotatedVector, 1.0e-14 );

}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat