//  Boost pow_test.cpp test file
//  Tests the pow function

//  (C) Copyright Bruno Lalande 2008.
//  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)

#include <cmath>
#include <string>
#include <iostream>

#include <boost/math/concepts/real_concept.hpp>
#include <boost/test/test_exec_monitor.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include <boost/typeof/typeof.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/static_assert.hpp>

#include <boost/math/special_functions/pow.hpp>

#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()
BOOST_TYPEOF_REGISTER_TYPE(boost::math::concepts::real_concept)

using namespace boost;
using namespace boost::math;

template <int N, class T>
void test_pow(T base)
{
    typedef typename tools::promote_args<T>::type result_type;

    BOOST_MATH_STD_USING

    if ((base == 0) && N < 0)
    {
       BOOST_CHECK_THROW(math::pow<N>(base), std::overflow_error);
    }
    else
    {
       BOOST_CHECK_CLOSE(math::pow<N>(base),
              pow(static_cast<result_type>(base), static_cast<result_type>(N)),
              boost::math::tools::epsilon<result_type>() * 100 * 400); // 400 eps as a %
    }
}

template <int N, class T>
void test_with_big_bases()
{
    for (T base = T(); base < T(1000); ++base)
        test_pow<N>(base);
}

template <int N, class T>
void test_with_small_bases()
{
    T base = 0.9f;
    for (int i = 0; i < 10; ++i)
    {
        base += base/50;
        test_pow<N>(base);
    }
}

template <class T, int Factor>
void test_with_small_exponents()
{
    test_with_big_bases<0, T>();
    test_with_big_bases<Factor*1, T>();
    test_with_big_bases<Factor*2, T>();
    test_with_big_bases<Factor*3, T>();
    test_with_big_bases<Factor*5, T>();
    test_with_big_bases<Factor*6, T>();
    test_with_big_bases<Factor*7, T>();
    test_with_big_bases<Factor*8, T>();
    test_with_big_bases<Factor*9, T>();
    test_with_big_bases<Factor*10, T>();
    test_with_big_bases<Factor*11, T>();
    test_with_big_bases<Factor*12, T>();
}

template <class T, int Factor>
void test_with_big_exponents()
{
    test_with_small_bases<Factor*50, T>();
    test_with_small_bases<Factor*100, T>();
    test_with_small_bases<Factor*150, T>();
    test_with_small_bases<Factor*200, T>();
    test_with_small_bases<Factor*250, T>();
    test_with_small_bases<Factor*300, T>();
    test_with_small_bases<Factor*350, T>();
    test_with_small_bases<Factor*400, T>();
    test_with_small_bases<Factor*450, T>();
    test_with_small_bases<Factor*500, T>();
}

void test_return_types()
{
    BOOST_STATIC_ASSERT((is_same<BOOST_TYPEOF(pow<2>('\1')), double>::value));
    BOOST_STATIC_ASSERT((is_same<BOOST_TYPEOF(pow<2>(L'\2')), double>::value));
    BOOST_STATIC_ASSERT((is_same<BOOST_TYPEOF(pow<2>(3)), double>::value));
    BOOST_STATIC_ASSERT((is_same<BOOST_TYPEOF(pow<2>(4u)), double>::value));
    BOOST_STATIC_ASSERT((is_same<BOOST_TYPEOF(pow<2>(5ul)), double>::value));
    BOOST_STATIC_ASSERT((is_same<BOOST_TYPEOF(pow<2>(6.0f)), float>::value));
    BOOST_STATIC_ASSERT((is_same<BOOST_TYPEOF(pow<2>(7.0)), double>::value));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    BOOST_STATIC_ASSERT((is_same<BOOST_TYPEOF(pow<2>(7.0l)), long double>::value));
#endif
}

namespace boost { namespace math { namespace policies {
template <class T>
T user_overflow_error(const char*, const char*, const T&)
{ return T(123.45); }
}}}

namespace boost { namespace math { namespace policies {
template <class T>
T user_indeterminate_result_error(const char*, const char*, const T&)
{ return T(456.78); }
}}}

void test_error_policy()
{
    using namespace policies;

    BOOST_CHECK(pow<-2>(
                    0.0,
                    policy< ::boost::math::policies::overflow_error<user_error> >()
                )
                == 123.45);

    BOOST_CHECK(pow<0>(
                    0.0,
                    policy< ::boost::math::policies::indeterminate_result_error<user_error> >()
                )
                == 456.78);
}

int test_main(int, char* [])
{
    using namespace std;

    cout << "Testing with integral bases and positive small exponents" << endl;
    test_with_small_exponents<int, 1>();
    cout << "Testing with integral bases and negative small exponents" << endl;
    test_with_small_exponents<int, -1>();

    cout << "Testing with float precision bases and positive small exponents" << endl;
    test_with_small_exponents<float, 1>();
    cout << "Testing with float precision bases and negative small exponents" << endl;
    test_with_small_exponents<float, -1>();

    cout << "Testing with float precision bases and positive big exponents" << endl;
    test_with_big_exponents<float, 1>();
    cout << "Testing with float precision bases and negative big exponents" << endl;
    test_with_big_exponents<float, -1>();

     cout << "Testing with double precision bases and positive small exponents" << endl;
    test_with_small_exponents<double, 1>();
    cout << "Testing with double precision bases and negative small exponents" << endl;
    test_with_small_exponents<double, -1>();

    cout << "Testing with double precision bases and positive big exponents" << endl;
    test_with_big_exponents<double, 1>();
    cout << "Testing with double precision bases and negative big exponents" << endl;
    test_with_big_exponents<double, -1>();

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    cout << "Testing with long double precision bases and positive small exponents" << endl;
    test_with_small_exponents<long double, 1>();
    cout << "Testing with long double precision bases and negative small exponents" << endl;
    test_with_small_exponents<long double, -1>();

    cout << "Testing with long double precision bases and positive big exponents" << endl;
    test_with_big_exponents<long double, 1>();
    cout << "Testing with long double precision bases and negative big exponents" << endl;
    test_with_big_exponents<long double, -1>();

    cout << "Testing with concepts::real_concept precision bases and positive small exponents" << endl;
    test_with_small_exponents<boost::math::concepts::real_concept, 1>();
    cout << "Testing with concepts::real_concept precision bases and negative small exponents" << endl;
    test_with_small_exponents<boost::math::concepts::real_concept, -1>();

    cout << "Testing with concepts::real_concept precision bases and positive big exponents" << endl;
    test_with_big_exponents<boost::math::concepts::real_concept, 1>();
    cout << "Testing with concepts::real_concept precision bases and negative big exponents" << endl;
    test_with_big_exponents<boost::math::concepts::real_concept, -1>();
#endif

    test_return_types();

    test_error_policy();

    return 0;
}

/*

  Running 1 test case...
  Testing with integral bases and positive small exponents
  Testing with integral bases and negative small exponents
  Testing with float precision bases and positive small exponents
  Testing with float precision bases and negative small exponents
  Testing with float precision bases and positive big exponents
  Testing with float precision bases and negative big exponents
  Testing with double precision bases and positive small exponents
  Testing with double precision bases and negative small exponents
  Testing with double precision bases and positive big exponents
  Testing with double precision bases and negative big exponents
  Testing with long double precision bases and positive small exponents
  Testing with long double precision bases and negative small exponents
  Testing with long double precision bases and positive big exponents
  Testing with long double precision bases and negative big exponents
  Testing with concepts::real_concept precision bases and positive small exponents
  Testing with concepts::real_concept precision bases and negative small exponents
  Testing with concepts::real_concept precision bases and positive big exponents
  Testing with concepts::real_concept precision bases and negative big exponents

  *** No errors detected

  */


#include <fstream>
#include <iostream>
#include <memory>
#include <pcl/common/common_headers.h>
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <boost/tokenizer.hpp>

namespace {
using ::pcl::PointCloud;
using ::pcl::PointXYZ;

// Command usage help
void printUsage (const char* progName)
{
  std::cout << "\n\nUsage: "<<progName<<" [options] <data.txt> <outfile.pcd>\n\n"
            << "Options:\n"
            << "-------------------------------------------\n"
            << "-h           this help text\n"
            << "\n\n";
}

std::unique_ptr<PointCloud<PointXYZ>> makeCloud() {
	auto cloud = std::make_unique<PointCloud<PointXYZ>>();

	// dimensions of the tof image
	cloud->width = 180;
	cloud->height = 240;
	// We may have NaN values in data set
	cloud->is_dense = false;
	cloud->points.resize(cloud->width * cloud->height);

	// Set sensor location to world coordinate center
	cloud->sensor_origin_.setZero();
	cloud->sensor_orientation_.w() = 0.0f;
	cloud->sensor_orientation_.x() = 1.0f;
	cloud->sensor_orientation_.y() = 0.0f;
	cloud->sensor_orientation_.z() = 0.0f;
	return cloud;
}

// Use camera intrinsic parameters to transform points from u,v,depth
// into real-world xyz coordinates. (Depth sensor is assumed at (0,0,0))
pcl::PointXYZ pointXYZFromDepth(float u, float v, float depth) {

	const float bad_point = std::numeric_limits<float>::quiet_NaN ();

	// Camera intrinsic parameters
	const float alph_x = 492.68967;		// focal width / sensor width
	const float alph_y = 492.6062;		// focal height / sensor height
	const float u0 = 323.59485;			// image center
	const float v0 = 234.65974;			// image center

	pcl::PointXYZ point;

	// depth value of zero is invalid
	if (depth == 0) {
		point.x = point.y = point.z = bad_point;
		return point;
	}

	point.x = depth * ((u - u0) / alph_x);
	point.y = depth * ((v - v0) / alph_y);
	point.z = depth;
	return point;
}

int writePcd(const std::string& filename, const std::string& outfile) {

	// Open the raw file for reading
	std::ifstream infile(filename);
	if (!infile) {
		throw std::runtime_error("File not found.");
	}

	std::unique_ptr<PointCloud<PointXYZ>> cloud = makeCloud();
	int i = 0;
	// File format: u,v,depth,confidence
	std::vector<float> data(4);

	// Read file and insert data into point cloud
	for (std::string line; std::getline(infile, line) && i < cloud->points.size();) {
     	boost::tokenizer<boost::char_separator<char>> tok(line, boost::char_separator<char>(","));
     	data.clear();
     	for (const auto& t: tok) {
     		data.push_back(std::stof(t));
     	}
     	if (data.size() < 3) {
     		throw std::runtime_error("Incorrect file format on line " + std::to_string(i));
     	}

     	// Transform from u,v,d into x,y,z world coordinates
     	cloud->points[i] = pointXYZFromDepth(data[0], data[1], data[2]);
		i++;
	}

	// Write point cloud to file
	pcl::io::savePCDFileASCII (outfile, *cloud);
	std::cerr << "Saved " << cloud->size() << " data points to " << outfile << std::endl;

	return(0);

}
} // namespace

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

	// Parse command-line arguments
  	if (pcl::console::find_argument (argc, argv, "-h") >= 0)
  	{
    	printUsage(argv[0]);
    	return 0;
  	}
  	if (argc != 3) {
  		std::cout << "Incorrect number of arguments.";
  		printUsage(argv[0]);
  		return 0;
  	}
  	std::string filename = std::string(argv[1]);
  	std::string outfile = std::string(argv[2]);

  	return writePcd(filename, outfile);
}


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

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include "gtest/gtest.h"
#include "theia/math/util.h"
#include "theia/sfm/transformation/align_point_clouds.h"

namespace theia {
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::RowMajor;
using Eigen::Vector3d;

namespace {
double kEpsilon = 1e-6;

void UmeyamaSimpleTest() {
  std::vector<Vector3d> left = {
      Vector3d(0.4, -3.105, 2.147),
      Vector3d(1.293, 7.1982, -.068),
      Vector3d(-5.34, 0.708, -3.69),
      Vector3d(-.345, 1.987, 0.936),
      Vector3d(0.93, 1.45, 1.079),
      Vector3d(-3.15, -4.73, 2.49),
      Vector3d(2.401, -2.03, -1.87),
      Vector3d(3.192, -.573, 0.1),
      Vector3d(-2.53, 3.07, -5.19)};

  const Matrix3d rotation_mat =
      Eigen::AngleAxisd(DegToRad(15.0), Vector3d(1.0, -2.7, 1.9).normalized())
          .toRotationMatrix();
  const Vector3d translation_vec(0, 2, 2);
  const double expected_scale = 1.5;

  // Transform the points.
  std::vector<Vector3d> right;
  for (int i = 0; i < left.size(); i++) {
    Vector3d transformed_point =
        expected_scale * rotation_mat * left[i] + translation_vec;
    right.emplace_back(transformed_point);
  }

  // Compute the similarity transformation.
  Matrix3d rotation;
  Vector3d translation;
  double scale;
  AlignPointCloudsUmeyama(left, right, &rotation, &translation, &scale);

  // Ensure the calculated transformation is the same as the one we set.
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      ASSERT_LT(std::abs(rotation(i, j) - rotation_mat(i, j)), kEpsilon);
    }
    ASSERT_LT(std::abs(translation(i) - translation_vec(i)), kEpsilon);
  }
  ASSERT_LT(fabs(expected_scale - scale), kEpsilon);
}

}  // namespace

TEST(AlignPointCloudsUmeyama, SimpleTest) {
  UmeyamaSimpleTest();
}

}  // namespace theia


/**
 * @file upwindquadrature_test.cc
 * @brief NPDE homework UpwindQuadrature code
 * @author  Philippe Peter
 * @date June 2020
 * @copyright Developed at ETH Zurich
 */

#include "../upwindquadrature.h"

#include <gtest/gtest.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>

#include <Eigen/Core>
#include <memory>

namespace UpwindQuadrature::test {

TEST(UpwindQuadrature, opposite_velocity_direction_1) {
  Eigen::MatrixXd coords(2, 3);
  coords << 0, 1, 0, 0, 0, 1;

  Eigen::MatrixXd velocities(2, 3);
  velocities << 1, 1, 1, 1, 1, 1;

  auto res =
      opposite_velocity_directions(lf::geometry::TriaO1(coords), velocities);
  EXPECT_EQ(res[0], Direction::OUTWARDS);
  EXPECT_EQ(res[1], Direction::OUTWARDS);
  EXPECT_EQ(res[2], Direction::OUTWARDS);
}

TEST(UpwindQuadrature, opposite_velocity_direction_2) {
  Eigen::MatrixXd coords(2, 3);
  coords << -1, 2, 0, -1, -1, 2;

  Eigen::MatrixXd velocities(2, 3);
  velocities << -1, 1, 0, -1, -1, 1;

  auto res =
      opposite_velocity_directions(lf::geometry::TriaO1(coords), velocities);
  EXPECT_EQ(res[0], Direction::INWARDS);
  EXPECT_EQ(res[1], Direction::INWARDS);
  EXPECT_EQ(res[2], Direction::INWARDS);
}

TEST(UpwindQuadrature, opposite_velocity_direction_3) {
  Eigen::MatrixXd coords(2, 3);
  coords << 1, 2, 1, 1, 1, 2;

  Eigen::MatrixXd velocities(2, 3);
  velocities << 0, 1, 0, -1, 0, 1;

  auto res =
      opposite_velocity_directions(lf::geometry::TriaO1(coords), velocities);
  EXPECT_EQ(res[0], Direction::ALONG_EDGE);
  EXPECT_EQ(res[1], Direction::ALONG_EDGE);
  EXPECT_EQ(res[2], Direction::ALONG_EDGE);
}

TEST(UpwindQuadrature, initialize_masses) {
  // construct a triangular tensor product mesh on the unit square
  std::unique_ptr<lf::mesh::MeshFactory> mesh_factory_ptr =
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::mesh::utils::TPTriagMeshBuilder builder(std::move(mesh_factory_ptr));
  builder.setBottomLeftCorner(Eigen::Vector2d{0.0, 0.0})
      .setTopRightCorner(Eigen::Vector2d{1.0, 1.0})
      .setNumXCells(2)
      .setNumYCells(2);
  std::shared_ptr<lf::mesh::Mesh> mesh_p = builder.Build();

  // a third of the area of each triangle.
  double reference_mass = 1.0 / 24.0;

  auto computed_masses = UpwindQuadrature::initializeMasses(mesh_p);
  EXPECT_DOUBLE_EQ(computed_masses(*(mesh_p->EntityByIndex(2, 0))),
                   2.0 * reference_mass);
  EXPECT_DOUBLE_EQ(computed_masses(*(mesh_p->EntityByIndex(2, 1))),
                   3.0 * reference_mass);
  EXPECT_DOUBLE_EQ(computed_masses(*(mesh_p->EntityByIndex(2, 2))),
                   1.0 * reference_mass);
  EXPECT_DOUBLE_EQ(computed_masses(*(mesh_p->EntityByIndex(2, 3))),
                   3.0 * reference_mass);
  EXPECT_DOUBLE_EQ(computed_masses(*(mesh_p->EntityByIndex(2, 4))),
                   6.0 * reference_mass);
  EXPECT_DOUBLE_EQ(computed_masses(*(mesh_p->EntityByIndex(2, 5))),
                   3.0 * reference_mass);
  EXPECT_DOUBLE_EQ(computed_masses(*(mesh_p->EntityByIndex(2, 6))),
                   1.0 * reference_mass);
  EXPECT_DOUBLE_EQ(computed_masses(*(mesh_p->EntityByIndex(2, 7))),
                   3.0 * reference_mass);
  EXPECT_DOUBLE_EQ(computed_masses(*(mesh_p->EntityByIndex(2, 8))),
                   2.0 * reference_mass);
}

TEST(UpwindQuadrature, upwind_convection_element_matrix_provider_1) {
  // construct a triangular tensor product mesh on the unit square
  std::unique_ptr<lf::mesh::MeshFactory> mesh_factory_ptr =
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::mesh::utils::TPTriagMeshBuilder builder(std::move(mesh_factory_ptr));
  builder.setBottomLeftCorner(Eigen::Vector2d{0.0, 0.0})
      .setTopRightCorner(Eigen::Vector2d{1.0, 1.0})
      .setNumXCells(2)
      .setNumYCells(2);
  std::shared_ptr<lf::mesh::Mesh> mesh_p = builder.Build();

  // initialize masses and velocity field
  auto masses = UpwindQuadrature::initializeMasses(mesh_p);
  const auto v = [](const Eigen::Vector2d & /*x*/) {
    return (Eigen::Vector2d() << 1, 2).finished();
  };

  // initialize already implemented reference provider and
  // the upwind provider
  ConvectionElementMatrixProvider reference(v);
  UpwindConvectionElementMatrixProvider upwind(v, masses);

  const lf::mesh::Entity &element_0 = *(mesh_p->EntityByIndex(0, 0));
  const lf::mesh::Entity &element_1 = *(mesh_p->EntityByIndex(0, 1));

  // element 0 is at nonoe of the corners the upwind triangle.
  EXPECT_NEAR(upwind.Eval(element_0).norm(), 0.0, 1E-15);

  // element 1 is the upwind triangle at corner 2.
  Eigen::MatrixXd reference_eval = reference.Eval(element_1);
  Eigen::MatrixXd upwind_eval = upwind.Eval(element_1);

  EXPECT_NEAR(upwind_eval.row(0).norm(), 0.0, 1E-14);
  EXPECT_NEAR(upwind_eval.row(1).norm(), 0.0, 1E-14);
  EXPECT_NEAR((upwind_eval.row(2) - 6.0 * reference_eval.row(2)).norm(), 0.0,
              1E-14);
}

TEST(UpwindQuadrature, upwind_convection_element_matrix_provider_2) {
  // construct a triangular tensor product mesh on the unit square
  std::unique_ptr<lf::mesh::MeshFactory> mesh_factory_ptr =
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::mesh::utils::TPTriagMeshBuilder builder(std::move(mesh_factory_ptr));
  builder.setBottomLeftCorner(Eigen::Vector2d{0.0, 0.0})
      .setTopRightCorner(Eigen::Vector2d{1.0, 1.0})
      .setNumXCells(2)
      .setNumYCells(2);
  std::shared_ptr<lf::mesh::Mesh> mesh_p = builder.Build();

  // initialize masses and velocity field
  auto masses = UpwindQuadrature::initializeMasses(mesh_p);
  const auto v = [](const Eigen::Vector2d & /*x*/) {
    return (Eigen::Vector2d() << -2, -1).finished();
  };

  // initialize already implemented reference provider and
  // the upwind provider
  ConvectionElementMatrixProvider reference(v);
  UpwindConvectionElementMatrixProvider upwind(v, masses);

  const lf::mesh::Entity &element_1 = *(mesh_p->EntityByIndex(0, 1));

  // element 1 is the upwind triangle at corner 0.
  Eigen::MatrixXd reference_eval = reference.Eval(element_1);
  Eigen::MatrixXd upwind_eval = upwind.Eval(element_1);

  EXPECT_NEAR((upwind_eval.row(0) - 2.0 * reference_eval.row(0)).norm(), 0.0,
              1E-14);
  EXPECT_NEAR(upwind_eval.row(1).norm(), 0.0, 1E-14);
  EXPECT_NEAR(upwind_eval.row(2).norm(), 0.0, 1E-14);
}

TEST(UpwindQuadrature, upwind_convection_element_matrix_provider_3) {
  // construct a triangular tensor product mesh on the unit square
  std::unique_ptr<lf::mesh::MeshFactory> mesh_factory_ptr =
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::mesh::utils::TPTriagMeshBuilder builder(std::move(mesh_factory_ptr));
  builder.setBottomLeftCorner(Eigen::Vector2d{0.0, 0.0})
      .setTopRightCorner(Eigen::Vector2d{1.0, 1.0})
      .setNumXCells(2)
      .setNumYCells(2);
  std::shared_ptr<lf::mesh::Mesh> mesh_p = builder.Build();

  // initialize masses and velocity field
  auto masses = UpwindQuadrature::initializeMasses(mesh_p);
  const auto v = [](const Eigen::Vector2d & /*x*/) {
    return (Eigen::Vector2d() << 0, -1).finished();
  };

  // initialize already implemented reference provider and
  // the upwind provider
  ConvectionElementMatrixProvider reference(v);
  UpwindConvectionElementMatrixProvider upwind(v, masses);

  const lf::mesh::Entity &element_1 = *(mesh_p->EntityByIndex(0, 1));

  // at corner 1 of element 1, -v(a^1) points along the edge
  //--> contribution split between triangle sharing that edge.
  Eigen::MatrixXd reference_eval = reference.Eval(element_1);
  Eigen::MatrixXd upwind_eval = upwind.Eval(element_1);

  EXPECT_NEAR(upwind_eval.row(0).norm(), 0.0, 1E-14);
  EXPECT_NEAR((upwind_eval.row(1) - 1.5 * reference_eval.row(1)).norm(), 0.0,
              1E-14);
  EXPECT_NEAR(upwind_eval.row(2).norm(), 0.0, 1E-14);
}

}  // namespace UpwindQuadrature::test


#include <stdio.h>
#include <opencv2/opencv.hpp>
#include <iostream>
#include "planner.h"
#include <boost/heap/binomial_heap.hpp>
#include "node2d.h"

using namespace cv;
using namespace std;
nav_msgs::OccupancyGridPtr grid;
struct CompareNodes1
{
    /// Sorting 3D nodes by increasing C value - the total estimated cost
    bool operator()(const Node3D *lhs, const Node3D *rhs) const
    {
        return lhs->getG() > rhs->getG();
    }
    /// Sorting 2D nodes by increasing C value - the total estimated cost
    bool operator()(const Node2D *lhs, const Node2D *rhs) const
    {
        return lhs->getG() > rhs->getG();
    }
};
struct CompareNodes
{
    /// Sorting 3D nodes by increasing C value - the total estimated cost
    bool operator()(const Node3D *lhs, const Node3D *rhs) const
    {
        return lhs->getG() > rhs->getG();
    }
    /// Sorting 2D nodes by increasing C value - the total estimated cost
    bool operator()(const Node2D *lhs, const Node2D *rhs) const
    {
        return lhs->getG() > rhs->getG();
    }
};
float a1Star(Node2D &start,
             Node2D &goal,
             Node2D *nodes2D,
             int width,
             int height,
             CollisionDetection &configurationSpace,
             Visualize &visualization);
float jjStar(Node2D &start,
             Node2D &goal,
             Node2D *nodes2D,
             int width,
             int height,
             CollisionDetection &configurationSpace,
             Visualize &visualization);

void updateMap(const nav_msgs::OccupancyGrid::Ptr map)
{
    grid = map;
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "jj_star");
    ros::NodeHandle n;
    ros::Subscriber subMap;
    subMap = n.subscribe("/map", 1, updateMap);
    ros::Duration(0.5).sleep();
    ros::spinOnce();
    ros::spinOnce();
    ros::spinOnce();
    int width = grid->info.width;
    int height = grid->info.height;

    Node2D *nodes2D = new Node2D[width * height]();
    //Node2D start2d(61, 124, 0, 0, nullptr);
    // create a 2d goal node
    //Node2D goal2d(61, 77, 0, 0, nullptr);
    Node2D start2d(1, 1, 0, 0, nullptr);
    // create a 2d goal node
    Node2D goal2d(100,105, 0, 0, nullptr);
    goal2d.setIdx(width);
    CollisionDetection configurationSpace;
    configurationSpace.updateGrid(grid);
    Visualize visualization;
    ros::WallTime start_, end_;
    visualization.clear();

    std::cout << "height : " << height << std::endl;
    std::cout << "width : " << width << std::endl;
    start_ = ros::WallTime::now();
    float astart_cost = a1Star(start2d, goal2d, nodes2D, width, height, configurationSpace, visualization);
    end_ = ros::WallTime::now();
    double execution_time = (end_ - start_).toNSec() * 1e-6;
    ROS_INFO_STREAM("Astar time (ms): " << execution_time);

    start_ = ros::WallTime::now();
    jjStar(start2d, goal2d, nodes2D, width, height, configurationSpace, visualization);
    end_ = ros::WallTime::now();
    execution_time = (end_ - start_).toNSec() * 1e-6;
    ROS_INFO_STREAM("jjStar time (ms): " << execution_time);

    float jjstar_cost = nodes2D[goal2d.getIdx()].getG();
    std::cout << "astar cost : " << astart_cost << std::endl;
    std::cout << "jjstar cost : " << jjstar_cost << std::endl;
    Mat image;
    image = imread("/home/ljj/code/APA/pathplannerOnGit/src/path_planner/maps/jj_map_parking.png", -1);
    if (!image.data)
    {
        printf("No image data \n");
        return -1;
    }

    float Max_C = 0;
    for(int i = 0; i < width * height ; i++){
        if(nodes2D[i].getG() > Max_C){
            Max_C = nodes2D[i].getG();
        }
    }
    std::cout << "Max cost : " << Max_C << std::endl;

    int nodecount = 0;
    for (int i = 0; i < width * height ; i++){
        if(nodes2D[i].isClosed()){
            int r = i / width;

            int c = i % width;
            //std::cout <<"row : "<<r<<", col : "<<c<<std::endl;
            Vec3b &intensity = image.at<Vec3b>(124 - r, c);

            intensity.val[0] = saturate_cast< uchar >(255 * nodes2D[i].getG() / Max_C);
            intensity.val[1] = 1;
            intensity.val[2] = saturate_cast< uchar >(255 * nodes2D[i].getG() / Max_C);
            //intensity.val[0] = 1;
            //intensity.val[1] = 2;
            //intensity.val[2] = 3;
            nodecount++;
        }
    }
    //std::cout << "total node number : " << nodecount << std::endl;
    namedWindow("Display Image", WINDOW_AUTOSIZE);
    imshow("Display Image", image);
    imwrite("/home/ljj/code/APA/pathplannerOnGit/src/path_planner/maps/jj_map_parking1.png", image);
    waitKey(0);

    delete[] nodes2D;
    return 0;
}
//###################################################
float jjStar(Node2D &start,
             Node2D &goal,
             Node2D *nodes2D,
             int width,
             int height,
             CollisionDetection &configurationSpace,
             Visualize &visualization)
{
    // PREDECESSOR AND SUCCESSOR INDEX
    int iPred, iSucc;
    float newG;

    // reset the open and closed list
    for (int i = 0; i < width * height; ++i)
    {
        nodes2D[i].open();
    }
    boost::heap::binomial_heap<Node2D *,
                               boost::heap::compare<CompareNodes1>>
        O;
    // update h value
    start.updateH(goal);
    // mark start as open
    start.open();
    // push on priority queue
    O.push(&start);
    iPred = start.setIdx(width);
    nodes2D[iPred] = start;
    int tt = nodes2D[iPred].isOpen() ? 1 : 0;
    std::cout << "node2D[iPred].isOpen : "<< tt <<std::endl;
    // NODE POINTER
    Node2D *nPred;
    Node2D *nSucc;
    int node_count = 0;
    while (!O.empty())
    {
        // pop node with lowest cost from priority queue
        //std::cout << "in while()"<<std::endl;
        nPred = O.top();
        O.pop();
        // set index
        iPred = nPred->setIdx(width);

        for (int i = 0; i < Node2D::dir; i++)
        {
            // create possible successor
            nSucc = nPred->createSuccessor(i);
            // set index of the successor
            iSucc = nSucc->setIdx(width);

            int isongrid = nSucc->isOnGrid(width, height)? 1:0;
            int isTrav = configurationSpace.isTraversable(nSucc)?1:0;
            //std::cout << "succ X : "<< nSucc->getX()
             //         << "succ Y : "<< nSucc->getY() <<std::endl;
            //std::cout << "isOnGrid(width, height) : "<< isongrid <<std::endl;
            //std::cout << "isTraversable(nSucc) : "<< isTrav <<std::endl;
            if (nSucc->isOnGrid(width, height) && configurationSpace.isTraversable(nSucc))
            {
                // calculate new G value
                nSucc->updateG();
                newG = nSucc->getG();
                //int isopen = nodes2D[iSucc].isOpen()?1:0;
                //int lower = newG < nodes2D[iSucc].getG() ? 1:0;
                //std::cout << "nodes2D[iSucc].isOpen() : "<< isopen <<std::endl;
                //std::cout << "newG < nodes2D[iSucc].getG() : "<< lower <<std::endl;
                if (nodes2D[iSucc].isOpen() || newG < nodes2D[iSucc].getG())
                {
                    nodes2D[iSucc] = *nSucc;
                    nodes2D[iSucc].close();
                    nodes2D[iSucc].setG(newG);
                    O.push(&nodes2D[iSucc]);
                    node_count ++ ;
                    //std::cout << "total node number : "<<node_count<<std::endl;

                }
                else
                {
                    delete nSucc;
                }
            }
            else
            {
                delete nSucc;
            }
        }

    }
}
//###################################################
float a1Star(Node2D &start,
             Node2D &goal,
             Node2D *nodes2D,
             int width,
             int height,
             CollisionDetection &configurationSpace,
             Visualize &visualization)
{

    // PREDECESSOR AND SUCCESSOR INDEX
    int iPred, iSucc;
    float newG;

    // reset the open and closed list
    for (int i = 0; i < width * height; ++i)
    {
        nodes2D[i].reset();
    }

    // VISUALIZATION DELAY
    ros::Duration d(0.001);

    boost::heap::binomial_heap<Node2D *,
                               boost::heap::compare<CompareNodes>>
        O;
    // update h value
    start.updateH(goal);
    // mark start as open
    start.open();
    // push on priority queue
    O.push(&start);
    iPred = start.setIdx(width);
    nodes2D[iPred] = start;

    // NODE POINTER
    Node2D *nPred;
    Node2D *nSucc;

    // continue until O empty
    while (!O.empty())
    {
        // pop node with lowest cost from priority queue
        nPred = O.top();
        // set index
        iPred = nPred->setIdx(width);

        // _____________________________
        // LAZY DELETION of rewired node
        // if there exists a pointer this node has already been expanded
        if (nodes2D[iPred].isClosed())
        {
            // pop node from the open list and start with a fresh node
            O.pop();
            continue;
        }
        // _________________
        // EXPANSION OF NODE
        else if (nodes2D[iPred].isOpen())
        {
            // add node to closed list
            nodes2D[iPred].close();
            nodes2D[iPred].discover();

            // RViz visualization
            if (Constants::visualization2D)
            {
                visualization.publishNode2DPoses(*nPred);
                visualization.publishNode2DPose(*nPred);
                //        d.sleep();
            }

            // remove node from open list
            O.pop();

            // _________
            // GOAL TEST
            if (*nPred == goal)
            {
                return nPred->getG();
            }
            // ____________________
            // CONTINUE WITH SEARCH
            else
            {
                // _______________________________
                // CREATE POSSIBLE SUCCESSOR NODES
                for (int i = 0; i < Node2D::dir; i++)
                {
                    // create possible successor
                    nSucc = nPred->createSuccessor(i);
                    // set index of the successor
                    iSucc = nSucc->setIdx(width);

                    // ensure successor is on grid ROW MAJOR
                    // ensure successor is not blocked by obstacle
                    // ensure successor is not on closed list
                    if (nSucc->isOnGrid(width, height) && configurationSpace.isTraversable(nSucc) && !nodes2D[iSucc].isClosed())
                    {
                        // calculate new G value
                        nSucc->updateG();
                        newG = nSucc->getG();

                        // if successor not on open list or g value lower than before put it on open list
                        if (!nodes2D[iSucc].isOpen() || newG < nodes2D[iSucc].getG())
                        {
                            // calculate the H value
                            nSucc->updateH(goal);
                            // put successor on open list
                            nSucc->open();
                            nodes2D[iSucc] = *nSucc;
                            O.push(&nodes2D[iSucc]);
                            delete nSucc;
                        }
                        else
                        {
                            delete nSucc;
                        }
                    }
                    else
                    {
                        delete nSucc;
                    }
                }
            }
        }
    }

    // return large number to guide search away
    return 1000;
}


#include "base/image.h"
#include "base/pose.h"
#include "base/reconstruction.h"
#include "util/logging.h"
#include "util/misc.h"
#include "util/option_manager.h"

#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>

#include <Eigen/Dense>

using namespace colmap;

// Takes as input a reconstruction, extracts the global down vectors (+y) in the
// local image coordinates for every image, and then saves these vectors as .txt
// files in the images directory. This is useful for generating an artificial
// ground truth dataset for testing gravity assisted SfM.

// Note that the images themselves don't have to exist in the given output
// directory, you can pass any path as long as the subdirectory structure from
// the images directory exists.

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

  std::string reconstruction_path;
  std::string images_path;

  OptionManager options;
  options.AddRequiredOption("reconstruction_path", &reconstruction_path);
  options.AddRequiredOption("images_path", &images_path);
  options.Parse(argc, argv);

  Reconstruction reconstruction;
  reconstruction.Read(reconstruction_path);

  bool first_file = true;

  for (const auto& ipair : reconstruction.Images()) {
    const Image& image = ipair.second;
    std::string root, ext;
    SplitFileExtension(image.Name(), &root, &ext);
    // might break if images_path is empty... ¯\_(ツ)_/¯
    const std::string gravity_file_name =
        EnsureTrailingSlash(images_path) + root + ".txt";

    if (first_file && ExistsFile(gravity_file_name)) {
      // If gravity files exist but not for the first file then what are you
      // even doing?

      std::cout
          << "Warning: this will overwrite existing .txt files in the images "
             "path. Confirm with 'y'.\n";
      char confirm_char;
      std::cin >> confirm_char;

      if (confirm_char != 'y') {
        return EXIT_FAILURE;
      }
    }
    first_file = false;

    const Eigen::Vector3d gravity =
        QuaternionToEigenQuaternion(image.Qvec()) * Eigen::Vector3d(0, 1, 0);

    std::ofstream of(gravity_file_name);
    of << std::setprecision(9) << gravity.transpose() << '\n';
  }

  return EXIT_SUCCESS;
}


/*
*   Copyright (c) 2010-2016, MIT Probabilistic Computing Project
*
*   Licensed 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 CONDITIONS OF ANY KIND, either express or implied.
*   See the License for the specific language governing permissions and
*   limitations under the License.
*/

// Program to check the modified Bessel functions of the first (I_0)
// and second (I_1) kinds against boost at points sampled randomly
// from N(0, 1), N(15, 1), and U[-709, +709].  The edge cases are
// zero, since I_0 is even and I_1 is odd; 15, where we switch from
// one rational approximation to another; and +/- 709, which are about
// where the values overflow the representable range of IEEE 754
// double-precision floating-point numbers.
//
// Usage:
//
//      c++ -I../include/CrossCat -o bessamp bessamp.cpp ../src/RandomNumberGenerator.cpp ../src/numerics.cpp ../src/weakprng.cpp
//      ./bessamp | head -n <nsamples>
//
// The output is
//
//      nu x boost local relerr
//
// where nu \in {0, 1}, x \in [-709, +709], boost is I_nu(x) evaluated
// using boost, local is I_nu(x) evaluated using the local numerics
// code, and relerr is the relative error of local from boost, or the
// absolute magnitude of local if boost = 0.
//
// The invocation
//
//      ./bessamp | awk '$5 > 1e-14'
//
// will show points at which we disagree with boost by more than one
// digit.  So far I have not seen it show any.

#include <boost/math/special_functions/bessel.hpp>
#include <cmath>
#include <cstdio>
#include <limits>

#include "RandomNumberGenerator.h"
#include "numerics.h"

static double relerr(double expected, double actual) {
    return fabs(expected == 0 ? actual : (actual - expected)/actual);
}

static double check(unsigned nu, double x) {
    double expected, actual, error;

    expected = boost::math::cyl_bessel_i(nu, x);
    actual = nu == 0 ? numerics::i_0(x) : numerics::i_1(x);
    error = relerr(expected, actual);
    if (printf("%u %.17e %.17e %.17e %.17e\n", nu, x, expected, actual, error)
        < 0)
        abort();
}

int main(int argc, char **argv) {
    const double epsilon = std::numeric_limits<double>::epsilon();
    RandomNumberGenerator rng;
    unsigned nu;

    for (;;) {
        for (nu = 0; nu < 2; nu++) {
            check(nu, rng.stdnormal());
            check(nu, 15 + rng.stdnormal());
            check(nu, rng.nexti(+709 - -709) + -709);
        }
    }
}


//  Copyright John Maddock 2017.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Basic sanity check that header <boost/math/tools/config.hpp>
// #includes all the files that it needs to.
//
#include <boost/math/common_factor_ct.hpp>

template <boost::math::static_gcd_type a, boost::math::static_gcd_type b>
struct static_value_check;

template <boost::math::static_gcd_type a>
struct static_value_check<a, a>
{
   static const boost::math::static_gcd_type value = a;
};

void compile_and_link_test()
{
   typedef static_value_check<boost::math::static_gcd<42, 30>::value, 6> checked_type;
   boost::math::static_gcd_type result = checked_type::value;
   (void)result;
   typedef static_value_check<boost::math::static_lcm<18, 30>::value, 90> checked_type_2;
   boost::math::static_gcd_type result_2 = checked_type_2::value;
   (void)result_2;
}


// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007, 2009
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/math/concepts/real_concept.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/results_collector.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/math/tools/stats.hpp>
#include <boost/math/tools/test.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/array.hpp>
#include "functor.hpp"

#include "handle_test_result.hpp"
#include "table_type.hpp"

#ifndef SC_
#define SC_(x) static_cast<typename table_type<T>::type>(BOOST_JOIN(x, L))
#endif

#define BOOST_CHECK_CLOSE_EX(a, b, prec, i) \
   {\
      unsigned int failures = boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed;\
      BOOST_CHECK_CLOSE(a, b, prec); \
      if(failures != boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed)\
      {\
         std::cerr << "Failure was at row " << i << std::endl;\
         std::cerr << std::setprecision(35); \
         std::cerr << "{ " << data[i][0] << " , " << data[i][1] << " , " << data[i][2];\
         std::cerr << " , " << data[i][3] << " , " << data[i][4] << " , " << data[i][5] << " } " << std::endl;\
      }\
   }

template <class Real, class T>
void do_test_gamma_2(const T& data, const char* type_name, const char* test_name)
{
   //
   // test gamma_p_inv(T, T) against data:
   //
   using namespace std;
   typedef Real                   value_type;

   std::cout << test_name << " with type " << type_name << std::endl;

   //
   // These sanity checks test for a round trip accuracy of one half
   // of the bits in T, unless T is type float, in which case we check
   // for just one decimal digit.  The problem here is the sensitivity
   // of the functions, not their accuracy.  This test data was generated
   // for the forward functions, which means that when it is used as
   // the input to the inverses then it is necessarily inexact.  This rounding
   // of the input is what makes the data unsuitable for use as an accuracy check,
   // and also demonstrates that you can't in general round-trip these functions.
   // It is however a useful sanity check.
   //
   value_type precision = static_cast<value_type>(ldexp(1.0, 1-boost::math::policies::digits<value_type, boost::math::policies::policy<> >()/2)) * 100;
   if(boost::math::policies::digits<value_type, boost::math::policies::policy<> >() < 50)
      precision = 1;   // 1% or two decimal digits, all we can hope for when the input is truncated to float

   for(unsigned i = 0; i < data.size(); ++i)
   {
      //
      // These inverse tests are thrown off if the output of the
      // incomplete gamma is too close to 1: basically there is insuffient
      // information left in the value we're using as input to the inverse
      // to be able to get back to the original value.
      //
      if(Real(data[i][5]) == 0)
         BOOST_CHECK_EQUAL(boost::math::gamma_p_inv(Real(data[i][0]), Real(data[i][5])), value_type(0));
      else if((1 - Real(data[i][5]) > 0.001)
         && (fabs(Real(data[i][5])) > 2 * boost::math::tools::min_value<value_type>())
         && (fabs(Real(data[i][5])) > 2 * boost::math::tools::min_value<double>()))
      {
         value_type inv = boost::math::gamma_p_inv(Real(data[i][0]), Real(data[i][5]));
         BOOST_CHECK_CLOSE_EX(Real(data[i][1]), inv, precision, i);
      }
      else if(1 == Real(data[i][5]))
         BOOST_CHECK_EQUAL(boost::math::gamma_p_inv(Real(data[i][0]), Real(data[i][5])), std::numeric_limits<value_type>::has_infinity ? std::numeric_limits<value_type>::infinity() : boost::math::tools::max_value<value_type>());
      else
      {
         // not enough bits in our input to get back to x, but we should be in
         // the same ball park:
         value_type inv = boost::math::gamma_p_inv(Real(data[i][0]), Real(data[i][5]));
         BOOST_CHECK_CLOSE_EX(Real(data[i][1]), inv, 100000, i);
      }

      if(Real(data[i][3]) == 0)
         BOOST_CHECK_EQUAL(boost::math::gamma_q_inv(Real(data[i][0]), Real(data[i][3])), std::numeric_limits<value_type>::has_infinity ? std::numeric_limits<value_type>::infinity() : boost::math::tools::max_value<value_type>());
      else if((1 - Real(data[i][3]) > 0.001) && (fabs(Real(data[i][3])) > 2 * boost::math::tools::min_value<value_type>()))
      {
         value_type inv = boost::math::gamma_q_inv(Real(data[i][0]), Real(data[i][3]));
         BOOST_CHECK_CLOSE_EX(Real(data[i][1]), inv, precision, i);
      }
      else if(1 == Real(data[i][3]))
         BOOST_CHECK_EQUAL(boost::math::gamma_q_inv(Real(data[i][0]), Real(data[i][3])), value_type(0));
      else if(fabs(Real(data[i][3])) > 2 * boost::math::tools::min_value<value_type>())
      {
         // not enough bits in our input to get back to x, but we should be in
         // the same ball park:
         value_type inv = boost::math::gamma_q_inv(Real(data[i][0]), Real(data[i][3]));
         BOOST_CHECK_CLOSE_EX(Real(data[i][1]), inv, 100, i);
      }
   }
   std::cout << std::endl;
}

template <class Real, class T>
void do_test_gamma_inv(const T& data, const char* type_name, const char* test_name)
{
#if !(defined(ERROR_REPORTING_MODE) && !defined(GAMMAP_INV_FUNCTION_TO_TEST))
   typedef Real                   value_type;

   typedef value_type (*pg)(value_type, value_type);
#ifdef GAMMAP_INV_FUNCTION_TO_TEST
   pg funcp = GAMMAP_INV_FUNCTION_TO_TEST;
#elif defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   pg funcp = boost::math::gamma_p_inv<value_type, value_type>;
#else
   pg funcp = boost::math::gamma_p_inv;
#endif

   boost::math::tools::test_result<value_type> result;

   std::cout << "Testing " << test_name << " with type " << type_name
      << "\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";

   //
   // test gamma_p_inv(T, T) against data:
   //
   result = boost::math::tools::test_hetero<Real>(
      data,
      bind_func<Real>(funcp, 0, 1),
      extract_result<Real>(2));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "gamma_p_inv", test_name);
   //
   // test gamma_q_inv(T, T) against data:
   //
#ifdef GAMMAQ_INV_FUNCTION_TO_TEST
   funcp = GAMMAQ_INV_FUNCTION_TO_TEST;
#elif defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   funcp = boost::math::gamma_q_inv<value_type, value_type>;
#else
   funcp = boost::math::gamma_q_inv;
#endif
   result = boost::math::tools::test_hetero<Real>(
      data,
      bind_func<Real>(funcp, 0, 1),
      extract_result<Real>(3));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "gamma_q_inv", test_name);
#endif
}

template <class T>
void test_gamma(T, const char* name)
{
#if !defined(TEST_UDT) && !defined(ERROR_REPORTING_MODE)
   //
   // The actual test data is rather verbose, so it's in a separate file
   //
   // First the data for the incomplete gamma function, each
   // row has the following 6 entries:
   // Parameter a, parameter z,
   // Expected tgamma(a, z), Expected gamma_q(a, z)
   // Expected tgamma_lower(a, z), Expected gamma_p(a, z)
   //
#  include "igamma_med_data.ipp"

   do_test_gamma_2<T>(igamma_med_data, name, "Running round trip sanity checks on incomplete gamma medium sized values");

#  include "igamma_small_data.ipp"

   do_test_gamma_2<T>(igamma_small_data, name, "Running round trip sanity checks on incomplete gamma small values");

#  include "igamma_big_data.ipp"

   do_test_gamma_2<T>(igamma_big_data, name, "Running round trip sanity checks on incomplete gamma large values");

#endif

#  include "gamma_inv_data.ipp"

   do_test_gamma_inv<T>(gamma_inv_data, name, "incomplete gamma inverse(a, z) medium values");

#  include "gamma_inv_big_data.ipp"

   do_test_gamma_inv<T>(gamma_inv_big_data, name, "incomplete gamma inverse(a, z) large values");

#  include "gamma_inv_small_data.ipp"

   do_test_gamma_inv<T>(gamma_inv_small_data, name, "incomplete gamma inverse(a, z) small values");
}

template <class T>
void test_spots(T, const char* type_name)
{
   std::cout << "Running spot checks for type " << type_name << std::endl;
   //
   // basic sanity checks, tolerance is 150 epsilon expressed as a percentage:
   //
   T tolerance = boost::math::tools::epsilon<T>() * 15000;
   if(tolerance < 1e-25f)
      tolerance = 1e-25f;  // limit of test data?
   BOOST_CHECK_CLOSE(::boost::math::gamma_q_inv(static_cast<T>(1)/100, static_cast<T>(1.0/128)), static_cast<T>(0.35767144525455121503672919307647515332256996883787L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_q_inv(static_cast<T>(1)/100, static_cast<T>(0.5)), static_cast<T>(4.4655350189103486773248562646452806745879516124613e-31L), tolerance*10);
   //
   // We can't test in this region against Mathworld's data as the results produced
   // by functions.wolfram.com appear to be in error, and do *not* round trip with
   // their own version of gamma_q.  Using our output from the inverse as input to
   // their version of gamma_q *does* round trip however.  It should be pointed out
   // that the functions in this area are very sensitive with nearly infinite
   // first derivatives, it's also questionable how useful these functions are
   // in this part of the domain.
   //
   //BOOST_CHECK_CLOSE(::boost::math::gamma_q_inv(static_cast<T>(1e-2), static_cast<T>(1.0-1.0/128)), static_cast<T>(3.8106736649978161389878528903698068142257930575497e-181L), tolerance);
   //
   BOOST_CHECK_CLOSE(::boost::math::gamma_q_inv(static_cast<T>(0.5), static_cast<T>(1.0/128)), static_cast<T>(3.5379794687984498627918583429482809311448951189097L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_q_inv(static_cast<T>(0.5), static_cast<T>(1.0/2)), static_cast<T>(0.22746821155978637597125832348982469815821055329511L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_q_inv(static_cast<T>(0.5), static_cast<T>(1.0-1.0/128)), static_cast<T>(0.000047938431649305382237483273209405461203600840052182L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_q_inv(static_cast<T>(10), static_cast<T>(1.0/128)), static_cast<T>(19.221865946801723949866005318845155649972164294057L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_q_inv(static_cast<T>(10), static_cast<T>(1.0/2)), static_cast<T>(9.6687146147141311517500637401166726067778162022664L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_q_inv(static_cast<T>(10), static_cast<T>(1.0-1.0/128)), static_cast<T>(3.9754602513640844712089002210120603689809432130520L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_q_inv(static_cast<T>(10000), static_cast<T>(1.0/128)), static_cast<T>(10243.369973939134157953734588122880006091919872879L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_q_inv(static_cast<T>(10000), static_cast<T>(1.0/2)), static_cast<T>(9999.6666686420474237369661574633153551436435884101L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_q_inv(static_cast<T>(10000), static_cast<T>(1.0-1.0/128)), static_cast<T>(9759.8597223369324083191194574874497413261589080204L), tolerance);
}


//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

int main () {
    using namespace boost::numeric::ublas;
    vector<std::complex<double> > v (3);
    for (unsigned i = 0; i < v.size (); ++ i)
        v (i) = std::complex<double> (i, i);

    std::cout << - v << std::endl;
    std::cout << conj (v) << std::endl;
    std::cout << real (v) << std::endl;
    std::cout << imag (v) << std::endl;
    std::cout << trans (v) << std::endl;
    std::cout << herm (v) << std::endl;
}


#include <boost/math/special_functions/beta.hpp>
#include <deque>
#include <random>
#include <unordered_map>
#include "relles.h"

#include<iostream>
static uint64_t beta_bsearch(std::unordered_map<uint64_t, long double>& memo, long double value, uint64_t n) {
  uint64_t low = 0;
  uint64_t high = n;

  std::random_device rd;
  std::mt19937_64 gen(rd());
  std::uniform_real_distribution<> unif_dis(0.0, 1.0);

  while (low < high - 1) {
    uint64_t idx = low + (high - low) / 2;
    if (memo.find(idx) == memo.end()) {
      long double beta = boost::math::ibeta_inv(idx - low + 1, high - idx, unif_dis(gen), boost::math::policies::policy<boost::math::policies::digits2<25>>());
      memo[idx] = memo[low] + (memo[high] - memo[low]) * beta;
    }

    if (memo[idx] < value) {
      low = idx;
    } else {
      high = idx;
    }
  }

  assert(low == high - 1);

  return low;
}

static uint64_t beta_isearch(std::unordered_map<uint64_t, long double>& memo, std::deque<std::pair<uint64_t, long double>>& prev_points, long double value, uint64_t n) {
  uint64_t low = 0;
  while (prev_points.back().second < value) {
    low = prev_points.back().first;
    prev_points.pop_back();
  }
  long double low_val = memo[low];
  uint64_t high = prev_points.back().first;
  long double high_val = memo[high];

  std::random_device rd;
  std::mt19937_64 gen(rd());
  std::uniform_real_distribution<> unif_dis(0.0, 1.0);

  while (low < high - 2) {
    uint64_t idx = round((value - low_val) / (high_val - low_val) * (high - low - 2)) + low + 1;
    assert(idx >= 0);
    if (memo.find(idx) == memo.end()) {
      long double beta = boost::math::ibeta_inv(idx - low, high - idx + 1, unif_dis(gen), boost::math::policies::policy<boost::math::policies::digits2<25>>());
      memo[idx] = memo[low] + (memo[high] - memo[low]) * beta;
      prev_points.emplace_back(idx, memo[idx]);
    }

    if (memo[idx] < value) {
      low = idx;
      low_val = memo[idx];
    } else {
      high = idx;
      high_val = memo[idx];
    }
  }

  return low + 1;
}

/*
 * The O(k log n) algorithm for multinomial sampling.
 */
std::vector<uint64_t> relles(uint64_t n, const std::vector<long double>& dist) {
  std::unordered_map<uint64_t, long double> memo;
  std::vector<uint64_t> output(dist.size());

  memo[0] = 0;
  memo[n] = 1;

  long double cum = 0;
  uint64_t last = 0;

  for (int i = 0; i < dist.size(); i ++) {
    cum += dist[i];
    uint64_t loc = beta_bsearch(memo, cum, n);
    output[i] = loc - last - 1;
    last = loc;
  }

  return output;
}

/*
 * The O(k log log n) algorithm for multinomial sampling.
 */
std::vector<uint64_t> relles_enhanced(uint64_t n, const std::vector<long double>& dist) {
  std::unordered_map<uint64_t, long double> memo;
  std::deque<std::pair<uint64_t, long double>> prev_points = { { n, 1 } };
  std::vector<uint64_t> output(dist.size());

  memo[0] = 0;
  memo[n] = 1;

  long double cum = 0;
  uint64_t last = 0;

  for (int i = 0; i < dist.size(); i ++) {
    cum += dist[i];
    if (cum >= 1) {
      output[i] = n - last - 1;
      break;
    }
    uint64_t loc = beta_isearch(memo, prev_points, cum, n);
    output[i] = loc - last - 1;
    last = loc;
  }

  return output;
}


#include "teca_latitude_damper.h"

#include "teca_variant_array.h"
#include "teca_metadata.h"
#include "teca_cartesian_mesh.h"
#include "teca_string_util.h"

#include <iostream>
#include <set>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

#include <complex.h>

using std::cerr;
using std::endl;

//#define TECA_DEBUG
namespace {

// get the filter ready to be applied in the next steps
template <typename coord_t>
void get_lat_filter(
    coord_t *filter, const coord_t *lat, size_t n_lat_vals,
    coord_t mu, coord_t sigma)
{
    coord_t two_sigma_sqr = 2.0*sigma*sigma;
    for (size_t i = 0; i < n_lat_vals; ++i)
    {
        coord_t x_min_mu = lat[i] - mu;
        coord_t neg_x_min_mu_sqr = -x_min_mu*x_min_mu;
        filter[i] = coord_t(1) - exp(neg_x_min_mu_sqr/two_sigma_sqr);
    }
}

// damp the input array using inverted gaussian
template <typename num_t, typename coord_t>
void apply_lat_filter(
    num_t *output, const num_t *input, const coord_t *filter,
    size_t n_lat_vals, size_t n_lon_vals)
{
    for (size_t j = 0; j < n_lat_vals; ++j)
    {
        size_t jj = j * n_lon_vals;
        for (size_t i = 0; i < n_lon_vals; ++i)
        {
            output[jj + i] = filter[j] * input[jj + i];
        }
    }
}

};

// --------------------------------------------------------------------------
teca_latitude_damper::teca_latitude_damper() :
    center(std::numeric_limits<double>::quiet_NaN()),
    half_width_at_half_max(std::numeric_limits<double>::quiet_NaN()),
    variable_post_fix("")
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_latitude_damper::~teca_latitude_damper()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_latitude_damper::get_properties_description(
    const std::string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_latitude_damper":prefix));

    opts.add_options()
        TECA_POPTS_GET(double, prefix, center,
            "set the center (mu) for the gaussian filter")
        TECA_POPTS_GET(double, prefix, half_width_at_half_max,
            "set the value of the half width at half maximum (HWHM) "
            "to calculate sigma from: sigma = HWHM/std::sqrt(2.0*std::log(2.0))")
        TECA_POPTS_MULTI_GET(std::vector<std::string>, prefix, damped_variables,
            "set the variables that will be damped by the inverted "
            "gaussian filter")
        TECA_POPTS_GET(std::string, prefix, variable_post_fix,
            "set the post-fix that will be attached to the variables "
            "that will be saved in the output")
        ;

    this->teca_algorithm::get_properties_description(prefix, opts);

    global_opts.add(opts);
}
// --------------------------------------------------------------------------
void teca_latitude_damper::set_properties(const std::string &prefix,
    variables_map &opts)
{
    this->teca_algorithm::set_properties(prefix, opts);

    TECA_POPTS_SET(opts, double, prefix, center)
    TECA_POPTS_SET(opts, double, prefix, half_width_at_half_max)
    TECA_POPTS_SET(opts, std::vector<std::string>, prefix, damped_variables)
    TECA_POPTS_SET(opts, std::string, prefix, variable_post_fix)
}
#endif

// --------------------------------------------------------------------------
int teca_latitude_damper::get_sigma(const teca_metadata &request, double &sigma)
{
    double hwhm = 0.0;
    if (std::isnan(this->half_width_at_half_max))
    {
        if (request.has("half_width_at_half_max"))
            request.get("half_width_at_half_max", hwhm);
        else
            return -1;
    }
    else
    {
        hwhm = this->half_width_at_half_max;
    }

    sigma = hwhm/std::sqrt(2.0*std::log(2.0));

    return 0;
}

// --------------------------------------------------------------------------
int teca_latitude_damper::get_mu(const teca_metadata &request, double &mu)
{
    if (std::isnan(this->center))
    {
        if (request.has("center"))
            request.get("center", mu);
        else
            return -1;
    }
    else
    {
        mu = this->center;
    }

    return 0;
}

// --------------------------------------------------------------------------
int teca_latitude_damper::get_damped_variables(std::vector<std::string> &vars)
{
    if (this->damped_variables.empty())
        return -1;
    else
        vars = this->damped_variables;

    return 0;
}

// --------------------------------------------------------------------------
teca_metadata teca_latitude_damper::get_output_metadata(
    unsigned int port,
    const std::vector<teca_metadata> &input_md)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_latitude_damper::get_output_metadata" << endl;
#endif
    (void)port;

    // add in the array we will generate
    teca_metadata out_md(input_md[0]);

    const std::string &var_post_fix = this->variable_post_fix;
    if (!var_post_fix.empty())
    {
        std::vector<std::string> &damped_vars = this->damped_variables;

        size_t n_arrays = damped_vars.size();
        for (size_t i = 0; i < n_arrays; ++i)
        {
            out_md.append("variables", damped_vars[i] + var_post_fix);
        }
    }

    return out_md;
}

// --------------------------------------------------------------------------
std::vector<teca_metadata> teca_latitude_damper::get_upstream_request(
    unsigned int port,
    const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_latitude_damper::get_upstream_request" << endl;
#endif
    (void) port;
    (void) input_md;

    std::vector<teca_metadata> up_reqs;
    teca_metadata req(request);

    // get the name of the array to request
    std::vector<std::string> damped_vars;
    if (this->get_damped_variables(damped_vars))
    {
        TECA_FATAL_ERROR("No variables to damp specified")
        return up_reqs;
    }

    // pass the incoming request upstream, and
    // add in what we need
    std::set<std::string> arrays;
    if (req.has("arrays"))
        req.get("arrays", arrays);

    arrays.insert(damped_vars.begin(), damped_vars.end());

    // Cleaning off the postfix for arrays passed in the pipeline.
    // For ex a down stream could request "foo_damped" then we'd
    // need to request "foo". also remove "foo_damped" from the
    // request.
    const std::string &var_post_fix = this->variable_post_fix;
    if (!var_post_fix.empty())
    {
        teca_string_util::remove_post_fix(arrays, var_post_fix);
    }

    req.set("arrays", arrays);

    // send up
    up_reqs.push_back(req);
    return up_reqs;
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_latitude_damper::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id() << "teca_latitude_damper::execute" << endl;
#endif

    (void)port;

    // get the input
    const_p_teca_cartesian_mesh in_mesh =
        std::dynamic_pointer_cast<const teca_cartesian_mesh>(input_data[0]);

    if (!in_mesh)
    {
        TECA_FATAL_ERROR("empty input, or not a mesh")
        return nullptr;
    }

    // create output and copy metadata, coordinates, etc
    p_teca_cartesian_mesh out_mesh =
        std::dynamic_pointer_cast<teca_cartesian_mesh>(in_mesh->new_instance());

    out_mesh->shallow_copy(
        std::const_pointer_cast<teca_cartesian_mesh>(in_mesh));

    // get the input array names
    std::vector<std::string> damped_vars;
    if (this->get_damped_variables(damped_vars))
    {
        TECA_FATAL_ERROR("No variable specified to damp")
        return nullptr;
    }

    // get Gaussian paramters. if none were provided, these are the defaults
    // that will be used.
    double mu = 0.0;
    double sigma = 45.0;

    this->get_mu(request, mu);
    this->get_sigma(request, sigma);

    // get the coordinate axes
    const_p_teca_variant_array lat = in_mesh->get_y_coordinates();
    const_p_teca_variant_array lon = in_mesh->get_x_coordinates();

    size_t n_lat = lat->size();
    size_t n_lon = lon->size();

    p_teca_variant_array filter_array = lat->new_instance(n_lat);

    // Get the gaussian filter
    NESTED_TEMPLATE_DISPATCH_FP(teca_variant_array_impl,
        filter_array.get(),
        _COORD,

        const NT_COORD *p_lat = static_cast<const TT_COORD*>(lat.get())->get();

        NT_COORD *filter = (NT_COORD*)malloc(n_lat*sizeof(NT_COORD));
        ::get_lat_filter<NT_COORD>(filter, p_lat, n_lat, mu, sigma);

        size_t n_arrays = damped_vars.size();
        for (size_t i = 0; i < n_arrays; ++i)
        {
            // get the input array
            const_p_teca_variant_array input_array
                = out_mesh->get_point_arrays()->get(damped_vars[i]);
            if (!input_array)
            {
                TECA_FATAL_ERROR("damper variable \"" << damped_vars[i]
                    << "\" is not in the input")
                return nullptr;
            }

            // apply the gaussian damper
            size_t n_elem = input_array->size();
            p_teca_variant_array damped_array = input_array->new_instance(n_elem);

            NESTED_TEMPLATE_DISPATCH(teca_variant_array_impl,
                damped_array.get(),
                _DATA,

                const NT_DATA *p_in = static_cast<const TT_DATA*>(input_array.get())->get();
                NT_DATA *p_damped_array = static_cast<TT_DATA*>(damped_array.get())->get();

                ::apply_lat_filter(p_damped_array, p_in, filter, n_lat, n_lon);
            )

            // set the damped array in the output
            std::string out_var_name = damped_vars[i] + this->variable_post_fix;
            out_mesh->get_point_arrays()->set(out_var_name, damped_array);
        }

        free(filter);
    )

    teca_metadata &omd = out_mesh->get_metadata();
    omd.set("gaussian_filter_hwhm", sigma);
    omd.set("gaussian_filter_center_lat", mu);

    return out_mesh;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SQR_ABS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SQR_ABS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing sqr_abs capabilities

    Computes the square of the absolute value of its parameter. For real entries it is the same as @ref sqr.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = sqr_abs(x);
    @endcode

    is equivalent to:

    @code
    T r = sqr(abs(x));
    @endcode

  **/
  Value sqr_abs(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/sqr_abs.hpp>
#include <boost/simd/function/simd/sqr_abs.hpp>

#endif


///////////////////////////////////////////////////////////////////////////////
// variance.hpp
//
//  Copyright 2005 Daniel Egloff, Eric Niebler. 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)

#ifndef BOOST_ACCUMULATORS_STATISTICS_VARIANCE_HPP_EAN_28_10_2005
#define BOOST_ACCUMULATORS_STATISTICS_VARIANCE_HPP_EAN_28_10_2005

#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/moment.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    //! Lazy calculation of variance.
    /*!
        Default sample variance implementation based on the second moment \f$ M_n^{(2)} \f$ moment<2>, mean and count.
        \f[
            \sigma_n^2 = M_n^{(2)} - \mu_n^2.
        \f]
        where
        \f[
            \mu_n = \frac{1}{n} \sum_{i = 1}^n x_i.
        \f]
        is the estimate of the sample mean and \f$n\f$ is the number of samples.
    */
    template<typename Sample, typename MeanFeature>
    struct lazy_variance_impl
      : accumulator_base
    {
        // for boost::result_of
        typedef typename numeric::functional::fdiv<Sample, std::size_t>::result_type result_type;

        lazy_variance_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            extractor<MeanFeature> mean;
            result_type tmp = mean(args);
            return accumulators::moment<2>(args) - tmp * tmp;
        }
    };

    //! Iterative calculation of variance.
    /*!
        Iterative calculation of sample variance \f$\sigma_n^2\f$ according to the formula
        \f[
            \sigma_n^2 = \frac{1}{n} \sum_{i = 1}^n (x_i - \mu_n)^2 = \frac{n-1}{n} \sigma_{n-1}^2 + \frac{1}{n-1}(x_n - \mu_n)^2.
        \f]
        where
        \f[
            \mu_n = \frac{1}{n} \sum_{i = 1}^n x_i.
        \f]
        is the estimate of the sample mean and \f$n\f$ is the number of samples.

        Note that the sample variance is not defined for \f$n <= 1\f$.

        A simplification can be obtained by the approximate recursion
        \f[
            \sigma_n^2 \approx \frac{n-1}{n} \sigma_{n-1}^2 + \frac{1}{n}(x_n - \mu_n)^2.
        \f]
        because the difference
        \f[
            \left(\frac{1}{n-1} - \frac{1}{n}\right)(x_n - \mu_n)^2 = \frac{1}{n(n-1)}(x_n - \mu_n)^2.
        \f]
        converges to zero as \f$n \rightarrow \infty\f$. However, for small \f$ n \f$ the difference
        can be non-negligible.
    */
    template<typename Sample, typename MeanFeature, typename Tag>
    struct variance_impl
      : accumulator_base
    {
        // for boost::result_of
        typedef typename numeric::functional::fdiv<Sample, std::size_t>::result_type result_type;

        template<typename Args>
        variance_impl(Args const &args)
          : variance(numeric::fdiv(args[sample | Sample()], numeric::one<std::size_t>::value))
        {
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            std::size_t cnt = count(args);

            if(cnt > 1)
            {
                extractor<MeanFeature> mean;
                result_type tmp = args[parameter::keyword<Tag>::get()] - mean(args);
                this->variance =
                    numeric::fdiv(this->variance * (cnt - 1), cnt)
                  + numeric::fdiv(tmp * tmp, cnt - 1);
            }
        }

        result_type result(dont_care) const
        {
            return this->variance;
        }

    private:
        result_type variance;
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::variance
// tag::immediate_variance
//
namespace tag
{
    struct lazy_variance
      : depends_on<moment<2>, mean>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::lazy_variance_impl<mpl::_1, mean> impl;
    };

    struct variance
      : depends_on<count, immediate_mean>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::variance_impl<mpl::_1, mean, sample> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::lazy_variance
// extract::variance
//
namespace extract
{
    extractor<tag::lazy_variance> const lazy_variance = {};
    extractor<tag::variance> const variance = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(lazy_variance)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(variance)
}

using extract::lazy_variance;
using extract::variance;

// variance(lazy) -> lazy_variance
template<>
struct as_feature<tag::variance(lazy)>
{
    typedef tag::lazy_variance type;
};

// variance(immediate) -> variance
template<>
struct as_feature<tag::variance(immediate)>
{
    typedef tag::variance type;
};

// for the purposes of feature-based dependency resolution,
// immediate_variance provides the same feature as variance
template<>
struct feature_of<tag::lazy_variance>
  : feature_of<tag::variance>
{
};

// So that variance can be automatically substituted with
// weighted_variance when the weight parameter is non-void.
template<>
struct as_weighted_feature<tag::variance>
{
    typedef tag::weighted_variance type;
};

// for the purposes of feature-based dependency resolution,
// weighted_variance provides the same feature as variance
template<>
struct feature_of<tag::weighted_variance>
  : feature_of<tag::variance>
{
};

// So that immediate_variance can be automatically substituted with
// immediate_weighted_variance when the weight parameter is non-void.
template<>
struct as_weighted_feature<tag::lazy_variance>
{
    typedef tag::lazy_weighted_variance type;
};

// for the purposes of feature-based dependency resolution,
// immediate_weighted_variance provides the same feature as immediate_variance
template<>
struct feature_of<tag::lazy_weighted_variance>
  : feature_of<tag::lazy_variance>
{
};

////////////////////////////////////////////////////////////////////////////
//// droppable_accumulator<variance_impl>
////  need to specialize droppable lazy variance to cache the result at the
////  point the accumulator is dropped.
///// INTERNAL ONLY
/////
//template<typename Sample, typename MeanFeature>
//struct droppable_accumulator<impl::variance_impl<Sample, MeanFeature> >
//  : droppable_accumulator_base<
//        with_cached_result<impl::variance_impl<Sample, MeanFeature> >
//    >
//{
//    template<typename Args>
//    droppable_accumulator(Args const &args)
//      : droppable_accumulator::base(args)
//    {
//    }
//};

}} // namespace boost::accumulators

#endif


/* Copyright (C) 5/23/18 Julian Stobbe - All Rights Reserved
 * You may use, distribute and modify this code under the
 * terms of the MIT license.
 *
 * You should have received a copy of the MIT license with
 * this file.
 */

#ifndef STAT_ACC_HPP_
#define STAT_ACC_HPP_

#include <deque>

#include <boost/accumulators/numeric/functional/vector.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/density.hpp>
#include <boost/accumulators/statistics/kurtosis.hpp>
#include <boost/accumulators/statistics/skewness.hpp>
#include <Eigen/Dense>

#include <algorithm>
#include <vector>

namespace MCUtil
{

    /*!
     * @brief Statistic types to be collected
     */
enum class StatType : unsigned char {
    MEAN = 0,
    VARIANCE  = 1
    //,SKEWNESS = 2,
    //KURTOSIS = 3
};

//use cache with sfinae to only define caching vector if needed

    /*!
     * @brief               This class provides the low level accumulation functionality for statistical quantities of observables
     * @tparam T            Type of quantity
     * @tparam CACHE_SIZE   Cache for the last CACHE_SIZE samples. This can be used for quantities that depend on multiple samples
     */
template<typename T, unsigned long CACHE_SIZE = 0>
class StatAcc
{

    using AccT = boost::accumulators::accumulator_set<T,
            boost::accumulators::features<
                    boost::accumulators::tag::mean,
                    boost::accumulators::tag::variance
                    //,boost::accumulators::tag::skewness
                    //,boost::accumulators::tag::kurtosis
            > >;

private:
    AccT acc;
    //@TODO: switch to boost circular buffer to store full time series?
    std::deque<T> cache;
    unsigned long cache_used;

public:
    /*StatAcc(): cache_used{0}
    {
        if(CACHE_SIZE)
            cache.resize(CACHE_SIZE);
    }
     */

    StatAcc &operator=(const StatAcc &) = delete;
    //StatAcc(const StatAcc&) = delete;

    /*!
     * @brief
     * @tparam ArgTypes     Argument types for the constructor of the quantity type to be accumulated
     * @param args          Arguments for the constructor of the quantity type to be accumulated
     */
    template<typename... ArgTypes>
    StatAcc(ArgTypes&&... args):
            acc(T(std::forward<ArgTypes>(args)...)), cache_used{0} {
        if(CACHE_SIZE)
            cache.resize(CACHE_SIZE);
    }

    friend std::ostream &operator<<(std::ostream &stream, const StatAcc &sa) {
        stream << "This is the temporary output for StatAcc";
        return stream;
    }

    /*!
     * @brief       Adds another sample to the accumulator
     * @param val   Value of next sample
     */
    void operator()(T val) {
        if(CACHE_SIZE) {
            if(cache_used < CACHE_SIZE) {
                cache.push_back(val);
                cache_used += 1;
            } else {
                cache.pop_front();
                cache.push_back(val);
            }
        }
        acc(val);
    }

    /*!
     * @brief       extracts statistic of accumulated quantity
     * @param st    Stat Type
     * @return      Value of statistic for quantity
     */
    T extract(const StatType st) {
        T res;
        switch(st) {
            case StatType::MEAN:
                res = boost::accumulators::mean(acc);
                break;
            case StatType::VARIANCE:
                res = boost::accumulators::variance(acc);
                break;
                /* @TODO: template disable
                 * case StatType::SKEWNESS:
                    res = boost::accumulators::variance(acc);
                    break;
                case StatType::KURTOSIS:
                    res = boost::accumulators::kurtosis(acc);
                    break;
                    */
        }
        return res;
    }
};

// ===== Eigen::MatrixXd specialization

    // TODO: do this as template specialization
    template<typename T, unsigned long CACHE_SIZE = 0>
    class StatAccEigen
    {

        using AccT = boost::accumulators::accumulator_set<std::vector<T>,
                boost::accumulators::features<
                        boost::accumulators::tag::mean,
                        boost::accumulators::tag::variance
                        //,boost::accumulators::tag::skewness
                        //,boost::accumulators::tag::kurtosis
                > >;

    private:
        AccT acc;
        std::vector<T> mapped;
        std::deque<std::vector<T>> cache;
        int r;
        int c;
        unsigned long cache_used;

    public:

        StatAccEigen &operator=(const StatAccEigen &) = delete;

        /*!
         * @brief
         * @param rows     Rows of matrices to be accumulated
         * @param cols     Cols of matrices to be accumulated
         */
        StatAccEigen(int rows, int cols):
                acc(std::vector<T>(rows*cols)), cache_used{0}, r(rows), c(cols)
        {
                mapped.resize(cols*rows);
                if(CACHE_SIZE)
                cache.resize(CACHE_SIZE);
        }
        /*!
         * @brief       Adds another sample to the accumulator
         * @param val   Value of next sample
         */
        void new_sample(const Eigen::Ref<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>>& val) {
            Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Map(&mapped[0], val.rows(), val.cols()) = val;
            if(CACHE_SIZE) {
                if(cache_used < CACHE_SIZE) {
                    cache.push_back(mapped);
                    cache_used += 1;
                } else {
                    cache.pop_front();
                    cache.push_back(mapped);
                }
            }
            acc(mapped);
        }

        /*!
         * @brief       Adds another sample to the accumulator
         * @param val   Value of next sample
         */
        void operator()(const Eigen::Ref<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>>& val) {
            Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Map(&mapped[0], val.rows(), val.cols()) = val;
            if(CACHE_SIZE) {
                if(cache_used < CACHE_SIZE) {
                    cache.push_back(mapped);
                    cache_used += 1;
                } else {
                    cache.pop_front();
                    cache.push_back(mapped);
                }
            }
            acc(mapped);
        }

        /*!
         * @brief       extracts statistic of accumulated quantity
         * @param st    Stat Type
         * @return      Value of statistic for quantity
         */
        Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> extract(const StatType st) {
            std::vector<T> res;
            switch(st) {
                case StatType::MEAN:
                    res = boost::accumulators::mean(acc);
                    break;
                case StatType::VARIANCE:
                    res = boost::accumulators::variance(acc);
                    break;
                    /* @TODO: template disable
                     * case StatType::SKEWNESS:
                        res = boost::accumulators::variance(acc);
                        break;
                    case StatType::KURTOSIS:
                        res = boost::accumulators::kurtosis(acc);
                        break;
                        */
            }
            Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> out = \
                Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>>(&res[0], r, c);
            return out;
        }
    };

} // end namespace

#endif


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm algebra elementary add
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/algorithm/algebra/elementary/add.h"

namespace fa = fern::algorithm;

template<
    class Value1,
    class Value2,
    class Result>
void verify_value(
    Value1 const& value1,
    Value2 const& value2,
    Result const& result_we_want)
{
    fa::SequentialExecutionPolicy sequential;

    Result result_we_get;
    fa::algebra::add(sequential, value1, value2, result_we_get);
    BOOST_CHECK_EQUAL(result_we_get, result_we_want);
}

BOOST_AUTO_TEST_CASE(d0_array_d0_array)
{
    verify_value<int8_t, int8_t, int8_t>(-5, 6, 1);
    verify_value<int8_t, int8_t, int8_t>(-5, -5, -10);
    verify_value<double, uint8_t, double>(-5.5, 5, -0.5);
}


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#define BOOST_TEST_MODULE icl::test_doc_code unit test
#include <libs/icl/test/disable_test_warnings.hpp>

#include <limits>
#include <complex>

#include <string>
#include <vector>
#include <boost/mpl/list.hpp>
#include "../unit_test_unwarned.hpp"

// interval instance types
#include "../test_type_lists.hpp"
#include "../test_value_maker.hpp"

#include <boost/type_traits/is_same.hpp>

#include <boost/icl/rational.hpp>

#include <boost/icl/detail/interval_morphism.hpp>
#include <boost/icl/interval_map.hpp>
#include "../test_laws.hpp"

using namespace std;
using namespace boost;
using namespace unit_test;
using namespace boost::icl;

BOOST_AUTO_TEST_CASE(intro_sample_telecast)
{
    // Switch on my favorite telecasts using an interval_set
    interval<int>::type news(2000, 2015);
    interval<int>::type talk_show(2245, 2330);
    interval_set<int> myTvProgram;
    myTvProgram.add(news).add(talk_show);

    // Iterating over elements (seconds) would be silly ...
    for(interval_set<int>::iterator telecast = myTvProgram.begin();
        telecast != myTvProgram.end(); ++telecast)
        //...so this iterates over intervals
        //TV.switch_on(*telecast);
        cout << *telecast;

    cout << endl;
}

BOOST_AUTO_TEST_CASE(interface_sample_identifiers)
{
    typedef interval_set<std::string, less, continuous_interval<std::string> > IdentifiersT;
    IdentifiersT identifiers, excluded;

    // special identifiers shall be excluded
    identifiers += continuous_interval<std::string>::right_open("a", "c");
    identifiers -= std::string("boost");
    cout << "identifiers: " << identifiers << endl;

    excluded = IdentifiersT(icl::hull(identifiers)) - identifiers;
    cout << "excluded   : " << excluded << endl;

    if(icl::contains(identifiers, std::string("boost")))
        cout << "error, identifiers.contains('boost')\n";
}

BOOST_AUTO_TEST_CASE(function_reference_element_iteration)
{
    // begin of doc code -------------------------------------------------------
    interval_set<int> inter_set;
    inter_set.add(interval<int>::right_open(0,3))
             .add(interval<int>::right_open(7,9));

    for(interval_set<int>::element_const_iterator creeper = elements_begin(inter_set);
        creeper != elements_end(inter_set); ++creeper)
        cout << *creeper << " ";
    cout << endl;
    //Program output: 0 1 2 7 8

    for(interval_set<int>::element_reverse_iterator repeerc = elements_rbegin(inter_set);
        repeerc != elements_rend(inter_set); ++repeerc)
        cout << *repeerc << " ";
    cout << endl;
    //Program output: 8 7 2 1 0
    // end of doc code ---------------------------------------------------------

    // Testcode
    std::stringstream result;
    for(interval_set<int>::element_iterator creeper2 = elements_begin(inter_set);
        creeper2 != elements_end(inter_set); ++creeper2)
        result << *creeper2 << " ";

    BOOST_CHECK_EQUAL(result.str(), std::string("0 1 2 7 8 "));

    std::stringstream tluser;
    for(interval_set<int>::element_const_reverse_iterator repeerc2
            = elements_rbegin(const_cast<const interval_set<int>&>(inter_set));
        repeerc2 != elements_rend(const_cast<const interval_set<int>&>(inter_set)); ++repeerc2)
        tluser << *repeerc2 << " ";

    BOOST_CHECK_EQUAL(tluser.str(), std::string("8 7 2 1 0 "));
}