// MIT License

// Copyright (c) 2018 Benjamin Bercovici

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef STATE_HEADER
#define STATE_HEADER

#include <armadillo>

namespace OC{

	class State{

	public:

		State(arma::vec state,double mu);

		/**
		Computes true anomaly from eccentric anomaly
		@param ecc eccentric anomaly
		@param e eccentricity (0 =< e < 1)
		@return true anomaly
		*/
		static double f_from_ecc(const double & ecc,const double & e);

		/**
		Computes eccentric anomaly eccentric from true anomaly
		@param f true anomaly
		@param e eccentricity (0 =< e < 1)
		@return eccentric anomaly
		*/
		static double ecc_from_f(const double & f,const double & e);

		/**
		Computes hyperbolic anomaly from true anomaly
		@param f true anomaly
		@param e eccentricity (1 < e)
		@return hyperbolic anomaly
		*/
		static double H_from_f(const double & f,const double & e);

		/**
		Computes true anomaly from hyperbolic anomaly
		@param H hyperbolic anomaly
		@param e eccentricity (1 < e)
		@return true anomaly
		*/
		static double f_from_H(const  double & H,const  double & e);

		/**
		Computes true anomaly from mean anomaly
		@param M mean anomaly
		@param e eccentricity (0 =< e < 1)
		@return true anomaly
		*/
		static double f_from_M(const double & M,const double & e);

		/**
		Computes eccentric anomaly eccentric from mean anomaly
		@param M mean anomaly
		@param e eccentricity (0 =< e < 1)
		@param pedantic if true, will print out convergence details
		@return eccentric anomaly
		*/
		static double ecc_from_M(const double & M,const double & e,const bool & pedantic = false);

		/**
		Computes hyperbolic anomaly eccentric from mean anomaly
		@param M mean anomaly
		@param e eccentricity (1 < e)
		@param pedantic if true, will print out convergence details
		@return hyperbolic anomaly
		*/
		static double H_from_M(const  double & M,const  double & e,const bool & pedantic = false);

		/**
		Computes mean anomaly from eccentric anomaly
		@param ecc eccentric anomaly
		@param e eccentricity (0 =< e < 1)
		@return mean anomaly
		*/
		static double M_from_ecc(const double & ecc,const double & e);

		/**
		Computes mean anomaly from hyperbolic anomaly
		@param H hyperbolic anomaly
		@param e eccentricity (1 < e)
		@return mean anomaly
		*/
		static double M_from_H(const double & H,const double & e);

		/**
		Computes mean anomaly from true anomaly
		@param f true anomaly
		@param e eccentricity (0 =< e < 1)
		@return mean anomaly
		*/
		static double M_from_f(const double & f,const double & e);

		virtual double get_momentum() const = 0;
		virtual double get_energy() const = 0;
		virtual double get_a() const = 0;
		virtual double get_eccentricity() const = 0;

		/**
		Get the state vector
		@return 6x1 state vector (either cartesian state or keplerian state)
		*/
		arma::vec get_state() const;

		/**
		Get mean motion
		@return mean motion (rad/s)
		*/
		double get_n() const;

		/**
		Get ellipse parameter
		@return ellipse parameter (m)
		*/
		double get_parameter() const;

		/**
		Get standard gravitational parameter
		@return standard gravitational parameter (kg^3/s^2)
		*/
		double get_mu() const;

		/**
		Set standard gravitational parameter
		@param mu standard gravitational parameter (kg^3/s^2)
		*/
		void set_mu(double mu);

		/**
		Sets the state to the prescribed value
		@param state 6x1 state
		*/
		void set_state(arma::vec state) ;

	protected:
		arma::vec state;
		double mu;

	};

}

#endif


#include "Geometry3D.hpp"

#include <Eigen/Dense>
#include <numeric>

namespace pointcloudhandler {

		Eigen::Vector3d Geometry3D::ComputeMinBound(const std::vector<Eigen::Vector3d>& points) const
		{
			if (points.empty())
			{
				return Eigen::Vector3d(0.0, 0.0, 0.0);
			}
			return std::accumulate(points.begin(), points.end(), points[0],
				[](const Eigen::Vector3d& a, const Eigen::Vector3d& b)
			{
				return a.array().min(b.array()).matrix();
			});
		}

		Eigen::Vector3d Geometry3D::ComputeMaxBound(const std::vector<Eigen::Vector3d>& points) const
		{
			if (points.empty()) {
				return Eigen::Vector3d(0.0, 0.0, 0.0);
			}
			return std::accumulate(points.begin(), points.end(), points[0],
				[](const Eigen::Vector3d& a, const Eigen::Vector3d& b)
			{
				return a.array().max(b.array()).matrix();
			});
		}

		Eigen::Vector3d Geometry3D::ComputeCenter(const std::vector<Eigen::Vector3d>& points) const
		{
			Eigen::Vector3d center(0, 0, 0);
			if (points.empty())
			{
				return center;
			}
			center = std::accumulate(points.begin(), points.end(), center);
			center /= double(points.size());
			return center;
		}

		void Geometry3D::TransformPoints(const Eigen::Matrix4d& transformation, std::vector<Eigen::Vector3d>& points) const
		{
			for (auto& point : points)
			{
				Eigen::Vector4d newPoint = transformation * Eigen::Vector4d(point(0), point(1), point(2), 1.0);
				point = newPoint.head<3>() / newPoint(3);
			}
		}

		void Geometry3D::TranslatePoints(const Eigen::Vector3d& translation, std::vector<Eigen::Vector3d>& points, bool relative) const
		{
			Eigen::Vector3d transform = translation;
			if (!relative)
			{
				transform -= ComputeCenter(points);
			}
			for (auto& point : points)
			{
				point += transform;
			}
		}

		void Geometry3D::ScalePoints(const double scale, std::vector<Eigen::Vector3d>& points, bool center) const
		{
			Eigen::Vector3d pointsCenter(0, 0, 0);
			if (center && !points.empty())
			{
				pointsCenter = ComputeCenter(points);
			}
			for (auto& point : points)
			{
				point = (point - pointsCenter) * scale + pointsCenter;
			}
		}

		void Geometry3D::RotatePoints(const Eigen::Matrix3d& R, std::vector<Eigen::Vector3d>& points, bool center) const
		{
			Eigen::Vector3d pointsCenter(0, 0, 0);
			if (center && !points.empty())
			{
				pointsCenter = ComputeCenter(points);
			}
			for (auto& point : points)
			{
				point = R * (point - pointsCenter) + pointsCenter;
			}
		}

}  // namespace pointcloudfilter


//  (C) Copyright Gennadiy Rozental 2011-2015.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE example
#include <boost/test/included/unit_test.hpp>

#include <cmath>

BOOST_AUTO_TEST_CASE( test )
{
  // sin 45 radians is actually ~ 0.85, sin 45 degrees is ~0.707
  double res = std::sin( 45. );

  BOOST_WARN_MESSAGE( res < 0.71,
                      "sin(45){" << res << "} is > 0.71. Arg is not in radian?" );
}
//]


// lu_decomposition.cpp example program comparing float vs posit equation solver
//
// Copyright (C) 2017-2019 Stillwater Supercomputing, Inc.
//
// This file is part of the universal numbers project, which is released under an MIT Open Source license.

#include "common.hpp"

#include <iostream>
#include <typeinfo>

#include <boost/numeric/mtl/mtl.hpp>
#include <universal/posit/posit>

using namespace std;
using namespace sw::unum;

// Turn it off for now
#define USE_POSIT

int main(int argc, char** argv)
try {
	const size_t nbits = 16;
	const size_t es = 1;
	const size_t vecSize = 32;

#ifdef USE_POSIT
	using Matrix = mtl::dense2D< posit<nbits, es> >;
	using Vector = mtl::dense_vector< posit<nbits, es> >;
#else
	using Matrix = mtl::dense2D<float>;
	using Vector = mtl::dense_vector<float>;
#endif

	Matrix  A(4, 4), L(4, 4), U(4, 4), AA(4, 4);
	Vector	v(4);
	double 	c = 1.0;

	for (unsigned i = 0; i < 4; i++)
		for (unsigned j = 0; j < 4; j++) {
			U[i][j] = i <= j ? c * (i + j + 2) : (0);
			L[i][j] = i > j ? c * (i + j + 1) : (i == j ? (1) : (0));
		}

	std::cout << "L is:\n" << L << "U is:\n" << U;
	A = L * U;
	std::cout << "A is:\n" << A;
	AA = adjoint(A);

	for (unsigned i = 0; i < 4; i++)
		v[i] = double(i);

	Vector b(A*v), b2(adjoint(A)*v);

	Matrix LU(A);
	lu(LU);
	std::cout << "LU decomposition of A is:\n" << LU;

	Matrix B(lu_f(A));
	std::cout << "LU decomposition of A (as function result) is:\n" << B;

	Vector v1(lu_solve_straight(A, b));
	std::cout << "v1 is " << v1 << "\n";

	Vector v2(lu_solve(A, b));
	std::cout << "v2 is " << v2 << "\n";

	mtl::dense_vector<unsigned> P;
	lu(A, P);
	std::cout << "LU with pivoting is \n" << with_format(A, 5, 2) << "Permutation is " << P << "\n";
	Vector v3(lu_apply(A, P, b));
	std::cout << "v3 is " << v3 << "\n";

	Vector v4(lu_adjoint_apply(A, P, b2));
	std::cout << "v4 is " << v4 << "\n";

	Vector v5(lu_adjoint_solve(AA, b));
	std::cout << "v5 is " << v5 << "\n";

	int nrOfFailedTestCases = 0;
	return nrOfFailedTestCases > 0 ? EXIT_FAILURE : EXIT_SUCCESS;
}
catch (char const* msg) {
	cerr << msg << endl;
	return EXIT_FAILURE;
}
catch (const posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	cerr << "caught unknown exception" << endl;
	return EXIT_FAILURE;
}


//
// Copyright (c) 2019 INRIA
//

#include <pinocchio/math/rpy.hpp>

#include <boost/variant.hpp> // to avoid C99 warnings

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_rpyToMatrix)
{
  double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
  double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
  double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;

  Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(r, p, y);

  Eigen::Matrix3d R_Eig = (Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ())
                           * Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
                           * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX())
                          ).toRotationMatrix();

  BOOST_CHECK(R.isApprox(R_Eig));

  Eigen::Vector3d v;
  v << r, p, y;

  Eigen::Matrix3d Rv = pinocchio::rpy::rpyToMatrix(v);

  BOOST_CHECK(Rv.isApprox(R_Eig));
}

BOOST_AUTO_TEST_CASE(test_matrixToRpy)
{
  double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
  double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
  double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;

  Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(r, p, y);

  Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);

  BOOST_CHECK_CLOSE(r,v[0],1e-12);
  BOOST_CHECK_CLOSE(p,v[1],1e-12);
  BOOST_CHECK_CLOSE(y,v[2],1e-12);
}

BOOST_AUTO_TEST_SUITE_END()


// Group A - Exact Pricing Methods
//
// by Scott Sidoli
//
// 6-9-19
//
// NormalDistribution.hpp
//
// Function pertaining to the normal distribution including the CDF and PDF.
// Used in option pricing.

#ifndef NormalDistribution_hpp
#define NormalDistribution_hpp

#include <iostream>

using namespace std;
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions.hpp>
using namespace boost::math;

double N(double x)
{

	normal_distribution<> myNormal(0.0, 1.0);

	return cdf(myNormal, x);

}

double n(double x)
{

	normal_distribution<> myNormal(0.0, 1.0);

	return pdf(myNormal, x);

}

#endif


#ifndef TRIUMF_SUPERCONDUCTIVITY_PHENOMENOLOGY_HPP
#define TRIUMF_SUPERCONDUCTIVITY_PHENOMENOLOGY_HPP

#include <cmath>

#include <boost/math/constants/constants.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

//
namespace superconductivity {

// purely phenomenological relations in superconductivity
namespace phenomenology {

// temperature dependence of the (reduced) penetration depth
template <typename T = double>
T reduced_penetration_depth(T reduced_temperature, T exponent) {
  if (reduced_temperature >= 1.0) {
    return std::numeric_limits<T>::infinity();
  } else if (reduced_temperature <= 0.0) {
    return 1.0;
  } else {
    return 1.0 / std::sqrt(1.0 - std::pow(reduced_temperature, exponent));
  }
}

// temperature dependence of the (reduced) penetration depth
template <typename T = double>
T reduced_penetration_depth(T temperature, T critical_temperature, T exponent) {
  return reduced_penetration_depth<T>(temperature / critical_temperature,
                                      exponent);
}

// temperature dependence of the penetration depth
template <typename T = double>
T penetration_depth(T temperature, T critical_temperature, T exponent,
                    T lambda_0) {
  return lambda_0 * reduced_penetration_depth<T>(
                        temperature, critical_temperature, exponent);
}

// reduced gap
// after Halbritter ca. 1970
template <typename T = double> T reduced_gap(T reduced_temperature) {
  if (reduced_temperature >= 1.0) {
    return 0.0;
  } else if (reduced_temperature <= 0.0) {
    return 1.0;
  } else {
    return std::cos(boost::math::constants::half_pi<T>() *
                    std::pow(reduced_temperature, 2));
  }
}

/// The superconducting transition temperature T_c (K) as a function of applied
/// magnetic field. The calculation assumes a "parabolic" relationship by
/// default (i.e., exponent = 0.5).
template <typename T = double>
T critical_temperature(T applied_field, T critical_temperature_0,
                       T critical_field, T exponent = 0.5) {
  if (applied_field < 0.0) {
    // don't consider negative fields
    return critical_temperature_0;
  } else if (applied_field > critical_field) {
    // no superconductivity above the critical field
    return 0.0;
  } else {
    // the phenomenological field dependence
    return critical_temperature_0 *
           std::pow(1.0 - (applied_field / critical_field), exponent);
  }
}

/// The superconducting transition temperature T_c (K) as a function of applied
/// magnetic field. The calculation assumes a relationship obtained from
/// inverting: Hc2(T) / Hc2(0) = [1 - (T / Tc)^2] / [1 + (T / Tc)^2]. See e.g.,:
/// M. Tinkham, Phys. Rev. 129, 2413 (1963).
/// https://doi.org/10.1103/PhysRev.129.2413
template <typename T = double>
T critical_temperature_II(T applied_field, T critical_temperature_0,
                          T upper_critical_field) {
  if (applied_field < 0.0) {
    // don't consider negative fields
    return critical_temperature_0;
  } else if (applied_field > upper_critical_field) {
    // no superconductivity above the critical field
    return 0.0;
  } else {
    // reduced field
    T reduced_field = applied_field / upper_critical_field;
    // the phenomenological field dependence
    return critical_temperature_0 *
           std::sqrt((1.0 - reduced_field * reduced_field) /
                     (1.0 + reduced_field * reduced_field));
  }
}

} // namespace phenomenology

} // namespace superconductivity

} // namespace triumf

#endif // TRIUMF_SUPERCONDUCTIVITY_PHENOMENOLOGY_HPP


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

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

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

//[union
//` Shows how to get a united geometry of two polygons

#include <iostream>
#include <vector>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/domains/gis/io/wkt/wkt.hpp>

#include <boost/foreach.hpp>
/*<-*/ #include "create_svg_overlay.hpp" /*->*/

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

    polygon green, blue;

    boost::geometry::read_wkt(
        "POLYGON((2 1.3,2.4 1.7,2.8 1.8,3.4 1.2,3.7 1.6,3.4 2,4.1 3,5.3 2.6,5.4 1.2,4.9 0.8,2.9 0.7,2 1.3)"
            "(4.0 2.0, 4.2 1.4, 4.8 1.9, 4.4 2.2, 4.0 2.0))", green);

    boost::geometry::read_wkt(
        "POLYGON((4.0 -0.5 , 3.5 1.0 , 2.0 1.5 , 3.5 2.0 , 4.0 3.5 , 4.5 2.0 , 6.0 1.5 , 4.5 1.0 , 4.0 -0.5))", blue);

    std::vector<polygon> output;
    boost::geometry::union_(green, blue, output);

    int i = 0;
    std::cout << "green || blue:" << std::endl;
    BOOST_FOREACH(polygon const& p, output)
    {
        std::cout << i++ << ": " << boost::geometry::area(p) << std::endl;
    }

    /*<-*/ create_svg("union.svg", green, blue, output); /*->*/
    return 0;
}

//]

//[union_output
/*`
Output:
[pre
green || blue:
0: 5.64795

[$img/algorithms/union.png]

]
*/
//]


//
// Created by tomlucas on 26.06.20.
//

#include "types/SO3.h"
#include <vector>
#include "GaussianNoiseVector.h"
#include "adekf_viz.h"
#include <Eigen/Eigenvalues>

#define max_iterations 100
#define weighted_mean_epsilon 1e-12
using namespace adekf;

/**
 * Calculate the weighted mean of quaternions
 * @param quats  list of quaternions
 * @param probs the corresponding probabilities
 * @return The weighted mean of quats
 */
SO3d weightedMean(std::vector<SO3d> &quats, std::vector<double> &probs)
{
    SO3d sum = quats[0];
    SO3d old_sum = sum;
    decltype(sum - old_sum) diff_sum = diff_sum.Zero();
    decltype(sum - old_sum) selector_weights = selector_weights.Zero();

    int iterations = 0;
    do
    {
        iterations++;
        old_sum = sum;
        diff_sum = diff_sum.Zero();
        for (int i = 0; i < quats.size(); i++)
        {
            diff_sum = diff_sum + probs[i] * (quats[i] - sum); //.cwiseProduct(selectors[filter_bank[i].second]);
        }
        sum = sum + diff_sum; //.cwiseQuotient(selector_weights);
    } while (iterations <= max_iterations && (sum - old_sum).norm() > weighted_mean_epsilon);
    if (iterations > max_iterations)
        printf("Warning: stopped due to excess of iterations");
    return sum;
}

/**
 * Calculate the weighted covariance of quaternion distributions
 * @param quats The list of mean points of the distributions
 * @param covs The covariances of the distributions
 * @param probs The weights of the distributions
 * @param target The weighted mean of the means
 * @return The weighted covariance sum
 */
Eigen::Matrix3d weightedCovarianceSum(std::vector<SO3d> &quats, std::vector<Eigen::Matrix3d> &covs, std::vector<double> &probs, const SO3d &target)
{
    Eigen::Matrix3d sum = Eigen::Matrix3d::Zero();
    Eigen::Matrix3d weights = Eigen::Matrix3d::Zero();
    for (size_t i = 0; i < quats.size(); i++)
    {
        if (probs[i] > 0.)
        {
            auto diff = (quats[i] - target).eval();
            auto plus_diff = eval((quats[i] + getDerivator<3>()) - target);
            Eigen::Matrix<double, 3, 3> D(3, 3);
            //Initialise the Jacobian
            for (size_t j = 0; j < 3; ++j)
                D.col(j) = plus_diff[j].v; //write to cols since col major (transposes matrix )
            //Covariance select = selectors[filter_bank[i].second] * selectors[filter_bank[i].second].transpose() * probabilities(i);
            sum += probs[i] * (D.transpose() * (covs[i]) * D + diff * diff.transpose());
            //weights += select;
        }
    }
    assert(sum.determinant() > 0.);
    return sum;
}

/**
 * Calculates the weighted mean of quaternions with naive/improper mixing (in parameter space)
 * @param quats The quaternions to mix
 * @param probs The weights
 * @return Weighted mean of quats
 */
SO3d badWeightedMean(std::vector<SO3d> &quats, std::vector<double> &probs)
{
    Eigen::Vector4d sum = sum.Zero();
    for (int i = 0; i < quats.size(); i++)
    {
        sum += quats[i].coeffs() * probs[i];
    }
    sum = sum.normalized();
    return SO3d(sum.data());
}
/**
 * Calculates a numeric gold standard Variance of quats.
 *
 * Uses a list of uniformly sampled quaternions from the distributions and combines them with the proper mixing
 *
 * @param quats The list of uniformly sampled quaternions
 * @param probs The probabilities of the quaternioms
 * @param target The weighted mean of means of the distributions
 * @return The weighted covariance sum
 */
Eigen::Matrix3d goldWeightedCovarianceSum(std::vector<SO3d> &quats, std::vector<double> &probs, const SO3d &target)
{

    Eigen::Matrix3d sum = Eigen::Matrix3d::Zero();
    for (size_t i = 0; i < quats.size(); i++)
    {
        if (probs[i] > 0.)
        {
            auto diff = (quats[i] - target).eval();
            sum += probs[i] * (diff * diff.transpose());
        }
    }
    return sum;
}

/**
 * Improper way of the weighted sum of covariances.
 *
 * Uses the naive approach from the original IMM
 * @param quats The list of quaternion means
 * @param covs The covariances
 * @param probs The weights of the quaternions
 * @param target The weighted mean of means
 * @return The naive weighted covariance sum
 */
Eigen::Matrix3d badWeightedCovarianceSum(std::vector<SO3d> &quats, std::vector<Eigen::Matrix3d> &covs, std::vector<double> &probs, const SO3d &target)
{

    Eigen::Matrix3d sum = Eigen::Matrix3d::Zero();
    for (size_t i = 0; i < quats.size(); i++)
    {
        if (probs[i] > 0.)
        {
            auto diff = (quats[i] - target).eval();
            sum += probs[i] * (covs[i] + diff * diff.transpose());
        }
    }
    return sum;
}

/**
 * Calculates the difference between the bp-weighted mean and the naive weighted mean
 * @param quats List of quaternions
 * @param probs weights
 * @return BPMean-NaiveMean
 */
double diff(std::vector<SO3d> &quats, std::vector<double> &probs)
{
    return (weightedMean(quats, probs) - badWeightedMean(quats, probs)).norm();
}

/**
 * Plots the naive mixing vs the boxplus mixing
 * @param argc command line argument counter
 * @param argv command line arguments
 */
void plot_bad_vs_bp()
{
    std::vector<SO3d> quats;
    std::vector<std::vector<double>> probs;
    std::vector<Eigen::Matrix3d> covs;
    covs.push_back(Eigen::Matrix3d::Identity() * 0.01);
    covs.push_back(Eigen::Matrix3d::Identity() * 0.02);

    quats.push_back(SO3d(Eigen::Vector3d(0, 0, 0)));
    quats.push_back(SO3d(Eigen::Vector3d(0, 0, 0.0)));
    std::vector<double> trial{0.7, 0.3};
    std::cout << weightedMean(quats, trial) << std::endl;
    //return 0;
    //list of probability pairs
    for (double prob = 0.5; prob <= 1.; prob += 0.05)
    {
        probs.push_back(std::vector<double>(2));
        probs.back()[0] = prob;
        probs.back()[1] = 1. - prob;
    }
    //compute for different sigmas
    for (double sigma = 0.0; sigma < 3.; sigma += 0.01)
    {
        quats[1] = SO3d{Eigen::Vector3d(0, 0, sigma)};
        Eigen::Matrix<double, -1, 1> diffs = diffs.Zero(probs.size());
        //compute mean diff for different probabilities
        for (int i = 0; i < probs.size(); i++)
        {
            diffs(i) = diff(quats, probs[i]);
        }
        std::cout << "Sigma: " << sigma << " Max diff: " << diffs.maxCoeff() << " Diffs: " << diffs.transpose() << std::endl;
        adekf::viz::plotVector(diffs, "Mean diff", 300, "abcdefghiklmn");
        //compute covariance diff for different probabilities
        for (int i = 0; i < probs.size(); i++)
        {
            diffs(i) = (weightedCovarianceSum(quats, covs, probs[i], weightedMean(quats, probs[i])) - badWeightedCovarianceSum(quats, covs, probs[i], badWeightedMean(quats, probs[i]))).norm();
        }
        std::cout << "Sigma: " << sigma << " Max diff: " << diffs.maxCoeff() << " Diffs: " << diffs.transpose() << std::endl;
        adekf::viz::plotVector(diffs, "Sigma diff", 300, "abcdefghiklmn");
    }
}

/**
 * Calculates the likelihood of a vector given the covariance of the distribution
 * @param delta the vector to check the likelihood of
 * @param sigma The covariance
 * @return N(delta; 0, sigma)
 */
double likelihood(const Eigen::Vector3d &delta, const Eigen::Matrix3d &sigma)
{
    return exp(-0.5 * (delta).transpose() * sigma.inverse() * delta) / sqrt(sigma.determinant() * pow((2 * M_PI), sigma.rows()));
}

/**
 * Plots a numeric gold standard mixing against the naive mixing and the bp-mixing
 * @param argc command line argument counter
 * @param argv command line arguments
 */
void plot_real_gold_vs_()
{
    std::vector<SO3d> quats(2);
    std::vector<double> probs{0.7, 0.3};

    std::vector<Eigen::Matrix3d> covs;
    covs.push_back(Eigen::Matrix3d::Identity() * 0.01);
    covs.push_back(Eigen::Matrix3d::Identity() * 0.02);
    //Compute for different sigmas
    for (double sigma = 0.0; sigma < 3.; sigma += 0.05)
    {
        quats[0] = (SO3d(Eigen::Vector3d(0, 0, 0)));
        quats[1] = (SO3d(Eigen::Vector3d(0, 0, sigma)));
        //return 0;

        const double max = M_PI / 2;
        const double step = 0.02;
        const int steps = max / step;
        std::vector<SO3d> gold_quats(pow(steps, 3));
        std::vector<double> gold_probs(pow(steps, 3));
        //uniform sample of quaternions from distributions
        for (int l = 0; l < probs.size(); ++l)
        {

            for (int i = -steps; i < steps; i++)
            {
                for (int j = -steps; j < steps; j++)
                {
                    for (int k = -steps; k < steps; ++k)
                    {
                        Eigen::Vector3d delta = step * Eigen::Vector3d(i, j, k);
                        gold_quats.push_back(quats[l] + delta);
                        gold_probs.push_back(likelihood(delta, covs[l]) * probs[l]);
                    }
                }
            }
        }
        double sum = 0;
        //normalise probabilities of sampled quats
        for (const auto &prob : gold_probs)
        {
            sum += prob;
        }
        for (auto &prob : gold_probs)
        {
            prob /= sum;
        }
        sum = 0;
        //compute means
        auto mean = weightedMean(quats, probs);
        auto badMean = badWeightedMean(quats, probs);
        auto goldMean = weightedMean(gold_quats, gold_probs);
        Eigen::Matrix<double, 2, 1> diffs = diffs.Zero();
        diffs(0) = (mean - goldMean).norm();
        diffs(1) = (badMean - goldMean).norm();
        adekf::viz::plotVector(diffs, "Gold Mean diff", 305, "mb");
        auto goldCov = goldWeightedCovarianceSum(gold_quats, gold_probs, goldMean);
        diffs(0) = (weightedCovarianceSum(quats, covs, probs, mean) - goldCov).norm();
        diffs(1) = (badWeightedCovarianceSum(quats, covs, probs, badMean) - goldCov).norm();
        adekf::viz::plotVector(diffs, "Gold Sigma diff", 305, "mb");
    }
}

Eigen::Vector3d weightedRefMean(aligned_vector<SO3d> &quats, std::vector<double> &probs, SO3d ref)
{
    Eigen::Vector3d sum = sum.Zero();

    for (int i = 0; i < quats.size(); i++)
    {
        sum = sum + probs[i] * (quats[i] - ref);
    }
    return sum;
}

/**
 * Calculates a numeric gold standard Variance of quats.
 *
 * Uses a list of uniformly sampled quaternions from the distributions and combines them with the proper mixing
 *
 * @param quats The list of uniformly sampled quaternions
 * @param probs The probabilities of the quaternioms
 * @param target The weighted mean of means of the distributions
 * @return The weighted covariance sum
 */
Eigen::Matrix3d goldWeightedCovarianceSumRef(aligned_vector<SO3d> &quats, std::vector<double> &probs, const SO3d &ref)
{

    Eigen::Matrix3d sum = Eigen::Matrix3d::Zero();
    Eigen::Vector3d mu_ref = weightedRefMean(quats, probs, ref);
    for (size_t i = 0; i < quats.size(); i++)
    {
        if (probs[i] > 0.)
        {
            auto diff = ((quats[i] - ref) - mu_ref).eval();
            sum += probs[i] * (diff * diff.transpose());
        }
    }
    return sum;
}

/**
 * Plots a numeric gold standard mixing against the naive mixing and the bp-mixing
 * @param argc command line argument counter
 * @param argv command line arguments
 */
void plot_covariance_centered()
{
    SO3d r1, r2; //r1 r2

    Eigen::Matrix3d cov = cov.Identity() * 0.1;
    //Compute for different sigmas
    for (double sigma = 0.0; sigma < 3.; sigma += 0.05)
    {
        r1 = (SO3d(Eigen::Vector3d(0, 0, 0)));
        r2 = (SO3d(Eigen::Vector3d(0, 0, sigma)));
        Eigen::Vector3d diff = r2 - r1;
        const double max = M_PI / 2;
        const double step = 0.02;
        const int steps = max / step;
        aligned_vector<SO3d> gold_quats(pow(steps, 3));
        std::vector<double> gold_probs(pow(steps, 3));
        //uniform sample of quaternions from distribution

        for (int i = -steps; i < steps; i++)
        {
            for (int j = -steps; j < steps; j++)
            {
                for (int k = -steps; k < steps; ++k)
                {
                    Eigen::Vector3d delta = step * Eigen::Vector3d(i, j, k);
                    gold_quats.push_back(r1 + (diff + delta));
                    gold_probs.push_back(likelihood(delta, cov));
                }
            }
        }
        double sum = 0;
        //normalise probabilities of sampled quats
        for (const auto &prob : gold_probs)
        {
            sum += prob;
        }
        for (auto &prob : gold_probs)
        {
            prob /= sum;
        }
        sum = 0;
        //compute means

        Eigen::Matrix<double, 2, 1> diffs = diffs.Zero();
        auto goldCov = goldWeightedCovarianceSumRef(gold_quats, gold_probs, r2);
        auto transform = transformReferenceJacobian(r1, r2, diff);
        auto linearCov = transform * cov * transform.transpose();
        diffs(0)=(linearCov-goldCov).norm();
        diffs(1)=(cov-goldCov).norm();
        //diffs(0) = goldCov.determinant();
        adekf::viz::plotVector(diffs, "Covariance transform centered", 61, "mb");
    }
}

void plot_covariance_displaced()
{
    SO3d r1, r2; //r1 r2

    Eigen::Matrix3d cov = cov.Identity() * 0.1;
    //Compute for different sigmas
    for (double sigma = 0.0; sigma < 3.; sigma += 0.05)
    {
        r1 = (SO3d(Eigen::Vector3d(0, 0, 0)));
        r2 = (SO3d(Eigen::Vector3d(0, 0, sigma)));
        Eigen::Vector3d diff = r2 - r1;
        const double max = M_PI / 2;
        const double step = 0.02;
        const int steps = max / step;
        aligned_vector<SO3d> gold_quats(pow(steps, 3));
        std::vector<double> gold_probs(pow(steps, 3));
        //uniform sample of quaternions from distribution

        for (int i = -steps; i < steps; i++)
        {
            for (int j = -steps; j < steps; j++)
            {
                for (int k = -steps; k < steps; ++k)
                {
                    Eigen::Vector3d delta = step * Eigen::Vector3d(i, j, k);
                    gold_quats.push_back(r1 + (delta));
                    gold_probs.push_back(likelihood(delta, cov));
                }
            }
        }
        double sum = 0;
        //normalise probabilities of sampled quats
        for (const auto &prob : gold_probs)
        {
            sum += prob;
        }
        for (auto &prob : gold_probs)
        {
            prob /= sum;
        }
        sum = 0;
        //compute means

        Eigen::Matrix<double, 2, 1> diffs = diffs.Zero();
        auto goldCov = goldWeightedCovarianceSumRef(gold_quats, gold_probs, r2);
        auto transform = transformReferenceJacobian(r1, r2);
        auto linearCov = transform * cov * transform.transpose();
        diffs(0)=(linearCov-goldCov).norm();
        diffs(1)=(cov-goldCov).norm();
        //diffs(0) = goldCov.determinant();
        adekf::viz::plotVector(diffs, "Covariance transform displaced", 61, "mb");
    }
}

void plot_covariance_gold()
{
    SO3d r1, r2; //r1 r2

    Eigen::Matrix3d cov = cov.Identity() * 0.1;
    //Compute for different sigmas
    for (double sigma = 0.0; sigma < 3.; sigma += 0.05)
    {
        r1 = (SO3d(Eigen::Vector3d(0, 0, 0)));
        r2 = (SO3d(Eigen::Vector3d(0, 0, sigma)));
        Eigen::Vector3d diff = r2 - r1;
        const double max = M_PI / 2;
        const double step = 0.02;
        const int steps = max / step;
        aligned_vector<SO3d> gold_quats_centered(pow(steps, 3));

        aligned_vector<SO3d> gold_quats_displaced(pow(steps, 3));

        std::vector<double> gold_probs(pow(steps, 3));

        //uniform sample of quaternions from distribution

        for (int i = -steps; i < steps; i++)
        {
            for (int j = -steps; j < steps; j++)
            {
                for (int k = -steps; k < steps; ++k)
                {
                    Eigen::Vector3d delta = step * Eigen::Vector3d(i, j, k);
                    gold_quats_displaced.push_back(r1 + (delta));
                    gold_quats_centered.push_back(r1+(diff+delta));
                    gold_probs.push_back(likelihood(delta, cov));
                }
            }
        }
        double sum = 0;
        //normalise probabilities of sampled quats
        for (const auto &prob : gold_probs)
        {
            sum += prob;
        }
        for (auto &prob : gold_probs)
        {
            prob /= sum;
        }
        sum = 0;
        //compute means

        Eigen::Matrix<double, 3, 1> diffs = diffs.Zero();
        auto goldCovDisplaced = goldWeightedCovarianceSumRef(gold_quats_displaced, gold_probs, r2);
        auto goldCovCentered=goldWeightedCovarianceSumRef(gold_quats_centered,gold_probs,r2);
        diffs(0)=goldCovDisplaced.determinant()/cov.determinant();
        diffs(1)=goldCovCentered.determinant()/cov.determinant();
        diffs(2)=diffs(0)*diffs(1);

        //diffs(0) = goldCov.determinant();
        adekf::viz::plotVector(diffs, "Covariance transform gold", 61, "DCM");
        auto transform = transformReferenceJacobian(r1, r2);
        auto linearCov = (transform * cov * transform.transpose()).eval();
        diffs(0)=linearCov.determinant()/cov.determinant();
        transform=transformReferenceJacobian(r1,r2,diff);
        linearCov = transform * cov * transform.transpose();
        diffs(1)=linearCov.determinant()/cov.determinant();
        diffs(2)=diffs(0)*diffs(1);
        adekf::viz::plotVector(diffs, "Covariance transform linear", 61, "DCM");
    }
}

std::vector<std::thread> threads;
auto parallel=[](auto function){ threads.emplace_back(function);};
int main(int argc, char *argv[])
{
    viz::initGuis(argc, argv);

    //Choose some to plot
    //parallel(plot_bad_vs_bp);
    //parallel(plot_real_gold_vs_);//takes really long ~10 min
    //parallel(plot_covariance_displaced);
    //parallel(plot_covariance_centered);
    parallel(plot_covariance_gold);
    viz::runGuis();
    for(auto & thread: threads) thread.join();
    return 0;
}


/*
 * euclidean.hpp
 *
 *  Created on: Aug 16, 2009
 *      Author: stewie
 */

#ifndef EUCLIDEAN_HPP_
#define EUCLIDEAN_HPP_

#include <cmath>
#include <functional> // for std::plus
#include <numeric> // for std::inner_product

#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>

/**
 * @ingroup lab
 *
 * @file euclidean.hpp
 *
 * This file contains routines for computing various kinds of distances between
 * attractors and other objects.
 */

namespace bn {

namespace detail {

/**
 * @private
 * Helper class to compute a squared difference.
 */
struct SquareDifference {
	/**
	 * Result type of this function object.
	 */
	typedef double result_type;

	/**
	 * Computes a squared difference.
	 * @param x an object
	 * @param y another object
	 * @return the squared difference
	 */
	template<class T> result_type operator()(const T& x, const T& y) const {
		return std::pow(x - y, 2);
	}
};

}

/**
 * @ingroup lab
 *
 * Computes the euclidean distance between two ranges.
 *
 * The second range shall have at least as many elements as the first.
 * @param first1 input iterator to the initial position of the first range
 * @param last1 input iterator to the final position of the first range
 * @param first2 input iterator to the initial position of the second range
 * @return the euclidean distance
 */
template<class InputIterator> double euclidean_distance(
		const InputIterator first1, const InputIterator last1,
		const InputIterator first2) {
	return std::sqrt(std::inner_product(first1, last1, first2, 0.0, std::plus<
			double>(), detail::SquareDifference()));
}

/**
 * @ingroup lab
 *
 * Computes the euclidean distance between two ranges.
 *
 * The second range shall have at least as many elements as the first. This
 * overload uses concepts from
 * The Boost Range Library. Practically it is syntactic sugar which replaces:
 * @code
 * vector<double> v1 = ...
 * vector<double> v2 = ...
 * euclidean_distance(v1.begin(), v1.end(), v2.begin());
 * @endcode
 * with the tighter:
 * @code
 * vector<double> v1 = ...
 * vector<double> v2 = ...
 * euclidean_distance(v1, v2);
 * @endcode
 * @param x first input range
 * @param y second input range
 * @return the euclidean distance
 */
template<class SinglePassRange> double euclidean_distance(
		const SinglePassRange& x, const SinglePassRange& y) {
	return euclidean_distance(boost::begin(x), boost::end(x), boost::begin(y));
}

/**
 * @ingroup lab
 *
 * A function object to compute the euclidean distance between objects.
 *
 * This is particularly useful in STL algorithms since it is cumbersome (or
 * simply impossible) to pass template function pointers as arguments.
 *
 * For example:
 * @code
 * vector<AttractorExpression> expr = ...
 * util::Matrix<double> m = make_distance_matrix<double>(expr.begin(),
 * 	expr.end(), &euclidean_distance);
 * @endcode
 * is wrong because the compiler cannot determine which overload to choose; also
 * that function is a template so you have to explicitly instantiate its
 * parameters to take a pointer. On the contrary, this is simpler and correct:
 * @code
 * vector<AttractorExpression> expr = ...
 * util::Matrix<double> m = make_distance_matrix<double>(expr.begin(),
 * 	expr.end(), Euclidean());
 * @endcode
 */
struct Euclidean {
	/**
	 * Result type of this function object.
	 */
	typedef double result_type;

	/**
	 * Computes the euclidean distance between two vector of the same length.
	 * @param x the first vector
	 * @param y the second vector
	 * @return the euclidean distance
	 */
	template<class T> result_type operator()(const T& x, const T& y) const {
		return euclidean_distance(x, y);
	}
};

}

#endif /* EUCLIDEAN_HPP_ */


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */
/* EncryptedArray.cpp - Data-movement operations on arrays of slots
 */
#include <NTL/ZZ.h>
NTL_CLIENT
#include "EncryptedArray.h"
#include "polyEval.h"

static void buildDigitPolynomial(ZZX& result, long p, long e);

// extractDigits assumes that the slots of *this contains integers mod p^r
// i.e., that only the free terms are nonzero. (If that assumptions does
// not hold then the result will not be a valid ciphertext anymore.)
//
// It returns in the slots of digits[j] the j'th-lowest gigits from the
// integers in the slots of the input. Namely, the i'th slot of digits[j]
// contains the j'th digit in the p-base expansion of the integer in the
// i'th slot of the *this.
//
// If the shortCut flag is set then digits[j] contains the j'th digits wrt
// mod-p plaintext space and the highest possible level (for all j). Otherwise
// digits[j] still contains the j'th digit in the base-p expansion, but wrt
// mod-p^{r-j} plaintext space, and all the ciphertexts are at the same level.
void extractDigits(vector<Ctxt>& digits, const Ctxt& c, long r, bool shortCut)
{
  FHEcontext& context = (FHEcontext&) c.getContext();
  long rr = c.effectiveR();
  if (r<=0 || r>rr) r = rr; // how many digits to extract

  long p = context.zMStar.getP();
  ZZX x2p;
  if (p>3) {
    buildDigitPolynomial(x2p, p, r);
  }

  Ctxt tmp(c.getPubKey(), c.getPtxtSpace());
  digits.resize(r, tmp);      // allocate space
  vector<Ctxt> w(r, tmp);
  for (long i=0; i<r; i++) {
    tmp = c;
    for (long j=0; j<i; j++) {
      FHE_NTIMER_START(square);
      if (p==2) w[j].square();
      else if (p==3) w[j].cube();
      else polyEval(w[j], x2p, w[j]); // "in spirit" w[j] = w[j]^p
      FHE_NTIMER_STOP(square);
      tmp -= w[j];
      tmp.divideByP();
    }
    w[i] = tmp; // needed in the next round
    if (shortCut) {
      digits[i] = tmp; // digits[i]=i'th lowest digit
      digits[i].reducePtxtSpace(p);
    }
  }
  // If not shortCut, copy w into digits
  if (!shortCut) for (long i=0; i<r; i++) digits[i] = w[i];
}

// Compute a degree-p polynomial poly(x) s.t. for any t<e and integr z of the
// form z = z0 + p^t*z1 (with 0<=z0<p), we have poly(z) = z0 (mod p^{t+1}).
//
// We get poly(x) by interpolating a degree-(p-1) polynomial poly'(x)
// s.t. poly'(z0)=z0 - z0^p (mod p^e) for all 0<=z0<p, and then setting
// poly(x) = x^p + poly'(x).
static void buildDigitPolynomial(ZZX& result, long p, long e)
{
  if (p<2 || e<=1) return; // nothing to do
  FHE_TIMER_START;
  long p2e = power_long(p,e); // the integer p^e

  // Compute x - x^p (mod p^e), for x=0,1,...,p-1
  vec_long x(INIT_SIZE, p);
  vec_long y(INIT_SIZE, p);
  long bottom = -(p/2);
  for (long j=0; j<p; j++) {
    long z = bottom+j;
    x[j] = z;
    y[j] = z-PowerMod((z < 0 ? z + p2e : z), p, p2e);  // x - x^p (mod p^e)

    while (y[j] > p2e/2)         y[j] -= p2e;
    while (y[j] < -(p2e/2))      y[j] += p2e;
  }
  interpolateMod(result, x, y, p, e);
  assert(deg(result)<p); // interpolating p points, should get deg<=p-1
  SetCoeff(result, p);   // return result = x^p + poly'(x)
  //  cerr << "# digitExt mod "<<p<<"^"<<e<<"="<<result<<endl;
  FHE_TIMER_STOP;
}


#include <Eigen/Core>
#include <iostream>
using namespace Eigen;
using namespace std;

template<typename Derived>
Eigen::Reshaped<Derived, 4, 2>
reshape_helper(MatrixBase<Derived>& m)
{
  return Eigen::Reshaped<Derived, 4, 2>(m.derived());
}

int main(int, char**)
{
  MatrixXd m(2, 4);
  m << 1, 2, 3, 4,
       5, 6, 7, 8;
  MatrixXd n = reshape_helper(m);
  cout << "matrix m is:" << endl << m << endl;
  cout << "matrix n is:" << endl << n << endl;
  return 0;
}


/**
 * \file Chebyshev2Filter.hxx
 */

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/asinh.hpp>

#include <ATK/EQ/Chebyshev2Filter.h>
#include <ATK/EQ/helpers.h>

namespace Chebyshev2Utilities
{
  template<typename DataType>
  void create_chebyshev2_analog_coefficients(int order, DataType ripple, EQUtilities::ZPK<DataType>& zpk)
  {
    zpk.z.clear(); // no zeros for this filter type
    zpk.p.clear();

    DataType de = static_cast<DataType>(1.0 / std::sqrt(std::pow(10, (0.1 * ripple)) - 1));
    DataType mu = static_cast<DataType>(boost::math::asinh(1.0 / de) / order);

    for(gsl::index i = -order+1; i < order; i += 2)
    {
      std::complex<DataType> p1 = -std::complex<DataType>(std::cos(i * boost::math::constants::pi<DataType>() / (2 * order)), std::sin(i * boost::math::constants::pi<DataType>() / (2 * order)));
      std::complex<DataType> p2 = std::complex<DataType>(std::sinh(mu) * p1.real(), std::cosh(mu) * p1.imag());

      zpk.p.push_back(std::complex<DataType>(1, 0) / p2);

      if(i == 0)
      {
        continue;
      }

      zpk.z.push_back(std::complex<DataType>(0, 1 / std::sin(i * boost::math::constants::pi<DataType>() / (2 * order))));
    }

    std::complex<DataType> f = 1;
    for(gsl::index i = 0; i < zpk.p.size(); ++i)
    {
      f *= - zpk.p[i];
    }
    for(gsl::index i = 0; i < zpk.z.size(); ++i)
    {
      f /= - zpk.z[i];
    }
    zpk.k = f.real();
  }

  template<typename DataType, typename Container>
  void create_default_chebyshev2_coeffs(size_t order, DataType ripple, DataType Wn, Container& coefficients_in, Container& coefficients_out)
  {
    EQUtilities::ZPK<DataType> zpk;

    int fs = 2;
    create_chebyshev2_analog_coefficients(static_cast<int>(order), ripple, zpk);
    EQUtilities::populate_lp_coeffs(Wn, fs, order, zpk, coefficients_in, coefficients_out);
  }

  template<typename DataType, typename Container>
  void create_bp_chebyshev2_coeffs(size_t order, DataType ripple, DataType wc1, DataType wc2, Container& coefficients_in, Container& coefficients_out)
  {
    EQUtilities::ZPK<DataType> zpk;

    int fs = 2;
    create_chebyshev2_analog_coefficients(static_cast<int>(order/2), ripple, zpk);
    EQUtilities::populate_bp_coeffs(wc1, wc2, fs, order, zpk, coefficients_in, coefficients_out);
  }

  template<typename DataType, typename Container>
  void create_bs_chebyshev2_coeffs(size_t order, DataType ripple, DataType wc1, DataType wc2, Container& coefficients_in, Container& coefficients_out)
  {
    EQUtilities::ZPK<DataType> zpk;

    int fs = 2;
    create_chebyshev2_analog_coefficients(static_cast<int>(order/2), ripple, zpk);
    EQUtilities::populate_bs_coeffs(wc1, wc2, fs, order, zpk, coefficients_in, coefficients_out);
  }
}

namespace ATK
{
  template <typename DataType>
  Chebyshev2LowPassCoefficients<DataType>::Chebyshev2LowPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void Chebyshev2LowPassCoefficients<DataType_>::set_ripple(CoeffDataType ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev2LowPassCoefficients<DataType_>::CoeffDataType Chebyshev2LowPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType_>
  void Chebyshev2LowPassCoefficients<DataType_>::set_cut_frequency(CoeffDataType cut_frequency)
  {
    if(cut_frequency <= 0)
    {
      throw std::out_of_range("Frequency can't be negative");
    }
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev2LowPassCoefficients<DataType_>::CoeffDataType Chebyshev2LowPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType>
  void Chebyshev2LowPassCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  unsigned int Chebyshev2LowPassCoefficients<DataType>::get_order() const
  {
    return in_order;
  }

  template <typename DataType>
  void Chebyshev2LowPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    Chebyshev2Utilities::create_default_chebyshev2_coeffs(in_order, ripple, 2 * cut_frequency / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  Chebyshev2HighPassCoefficients<DataType>::Chebyshev2HighPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void Chebyshev2HighPassCoefficients<DataType_>::set_cut_frequency(CoeffDataType cut_frequency)
  {
    if(cut_frequency <= 0)
    {
      throw std::out_of_range("Frequency can't be negative");
    }
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev2HighPassCoefficients<DataType_>::CoeffDataType Chebyshev2HighPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType_>
  void Chebyshev2HighPassCoefficients<DataType_>::set_ripple(CoeffDataType ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev2HighPassCoefficients<DataType_>::CoeffDataType Chebyshev2HighPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev2HighPassCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  unsigned int Chebyshev2HighPassCoefficients<DataType>::get_order() const
  {
    return in_order;
  }

  template <typename DataType>
  void Chebyshev2HighPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    Chebyshev2Utilities::create_default_chebyshev2_coeffs(in_order, ripple, (input_sampling_rate - 2 * cut_frequency) / input_sampling_rate, coefficients_in, coefficients_out);
    for(gsl::index i = in_order - 1; i >= 0; i -= 2)
    {
      coefficients_in[i] = - coefficients_in[i];
      coefficients_out[i] = - coefficients_out[i];
    }
  }

  template <typename DataType>
  Chebyshev2BandPassCoefficients<DataType>::Chebyshev2BandPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void Chebyshev2BandPassCoefficients<DataType_>::set_cut_frequencies(std::pair<CoeffDataType, CoeffDataType> cut_frequencies)
  {
    if(cut_frequencies.first <= 0 || cut_frequencies.second <= 0)
    {
      throw std::out_of_range("Frequencies can't be negative");
    }
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void Chebyshev2BandPassCoefficients<DataType_>::set_cut_frequencies(CoeffDataType f0, CoeffDataType f1)
  {
    set_cut_frequencies(std::make_pair(f0, f1));
  }

  template <typename DataType_>
  std::pair<typename Chebyshev2BandPassCoefficients<DataType_>::CoeffDataType, typename Chebyshev2BandPassCoefficients<DataType_>::CoeffDataType> Chebyshev2BandPassCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType_>
  void Chebyshev2BandPassCoefficients<DataType_>::set_ripple(CoeffDataType ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev2BandPassCoefficients<DataType_>::CoeffDataType Chebyshev2BandPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev2BandPassCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  unsigned int Chebyshev2BandPassCoefficients<DataType>::get_order() const
  {
    return in_order / 2;
  }

  template <typename DataType>
  void Chebyshev2BandPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    Chebyshev2Utilities::create_bp_chebyshev2_coeffs(in_order, ripple, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  Chebyshev2BandStopCoefficients<DataType>::Chebyshev2BandStopCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void Chebyshev2BandStopCoefficients<DataType_>::set_cut_frequencies(std::pair<CoeffDataType, CoeffDataType> cut_frequencies)
  {
    if(cut_frequencies.first <= 0 || cut_frequencies.second <= 0)
    {
      throw std::out_of_range("Frequencies can't be negative");
    }
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void Chebyshev2BandStopCoefficients<DataType_>::set_cut_frequencies(CoeffDataType f0, CoeffDataType f1)
  {
    set_cut_frequencies(std::make_pair(f0, f1));
  }

  template <typename DataType_>
  std::pair<typename Chebyshev2BandStopCoefficients<DataType_>::CoeffDataType, typename Chebyshev2BandStopCoefficients<DataType_>::CoeffDataType> Chebyshev2BandStopCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType_>
  void Chebyshev2BandStopCoefficients<DataType_>::set_ripple(CoeffDataType ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev2BandStopCoefficients<DataType_>::CoeffDataType Chebyshev2BandStopCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev2BandStopCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  unsigned int Chebyshev2BandStopCoefficients<DataType>::get_order() const
  {
    return in_order / 2;
  }

  template <typename DataType>
  void Chebyshev2BandStopCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    Chebyshev2Utilities::create_bs_chebyshev2_coeffs(in_order, ripple, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }
}


#ifndef THIN_PLATE_SPLINE_HPP
#define THIN_PLATE_SPLINE_HPP

#include <Eigen/Dense>
#include <Eigen/QR>
#include "arc_utilities/arc_exceptions.hpp"
#include "arc_utilities/eigen_helpers.hpp"

namespace arc_utilities {
// Note that DIMENSIONS must be 2 or 3, not something like `Eigen::Dynamic`
template <int DIMENSIONS = 3>
class ThinPlateSpline {
 public:
  typedef Eigen::Matrix<double, DIMENSIONS, Eigen::Dynamic> PointSet;
  typedef Eigen::Matrix<double, DIMENSIONS + 1, Eigen::Dynamic> HomogeneousPointSet;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  static_assert(DIMENSIONS > 1, "Using TPS only makes sense for 2 or more dimensions");
  static_assert(DIMENSIONS <= 3,
                "The choice of kernel function for DIMENSIONS >= 4 is not clear."
                " See note after equation (29) in "
                "https://www.geometrictools.com/Documentation/ThinPlateSplines.pdf");

  ThinPlateSpline() : solved_(false) {}

  /**
   * @brief ThinPlateSpline::ThinPlateSpline An arbitrary dimensional thin
   *  plate spline for interpolation between points.
   * @param template_points A (DIMINESION x NUMPOINTS) matrix which define
   *  the starting points that control the warping. I.e. the template is
   *  warped to the target
   * @param target_points A (DIMINESION x NUMPOINTS) matrix which define
   *  the target points that control the warping. I.e. the template is
   *  warped to the target
   */
  template <typename DerivedA, typename DerivedB>
  ThinPlateSpline(const Eigen::MatrixBase<DerivedA>& template_points, const Eigen::MatrixBase<DerivedB>& target_points)
      : solved_(false) {
    setTemplatePoints(template_points);
    setTargetPoints(target_points);
    solveForCoeffs();
  }

  /**
   * @note Checking for the vailidty of the input data is put off until
   *  the warping function is solved
   */
  template <typename Derived>
  void setTemplatePoints(const Eigen::MatrixBase<Derived>& template_points) {
    using namespace Eigen;
    static_assert(
        MatrixBase<Derived>::RowsAtCompileTime == DIMENSIONS || MatrixBase<Derived>::RowsAtCompileTime == Dynamic,
        "INVALID DATA DIMENSIONS FOR template_points");

    homogeneous_template_points_.resize(DIMENSIONS + 1, template_points.cols());
    homogeneous_template_points_ << template_points, RowVectorXd::Ones(template_points.cols());
    solved_ = false;
  }

  /**
   * @note Checking for the vailidty of the input data is put off until
   *  the warping function is solved
   */
  template <typename Derived>
  void setTargetPoints(const Eigen::MatrixBase<Derived>& target_points) {
    using namespace Eigen;
    static_assert(
        MatrixBase<Derived>::RowsAtCompileTime == DIMENSIONS || MatrixBase<Derived>::RowsAtCompileTime == Dynamic,
        "INVALID DATA DIMENSIONS FOR target_points");

    target_points_ = target_points;
    solved_ = false;
  }

  void solveForCoeffs() {
    using namespace Eigen;
    if (homogeneous_template_points_.cols() < 1 || target_points_.cols() < 1) {
      throw_arc_exception(std::invalid_argument, "template and target points must have valid data before solving");
    }
    if (homogeneous_template_points_.cols() != target_points_.cols()) {
      throw_arc_exception(std::invalid_argument, "template and target points must have the same number of points");
    }

    const auto NUMPOINTS = homogeneous_template_points_.cols();

    // Calculate the kernel matrix for the template (control) points
    const MatrixXd Rsquared = EigenHelpers::CalculateSquaredDistanceMatrix(homogeneous_template_points_);
    const MatrixXd kernel = Rsquared.unaryExpr(&ThinPlateSpline::KernelFunction);

    // Form the system:   A * params = B
    //       A = [K,       template'
    //            template,    0]
    //       B = [target'
    //              0    ]
    MatrixXd A(NUMPOINTS + DIMENSIONS + 1, NUMPOINTS + DIMENSIONS + 1);
    A << kernel, homogeneous_template_points_.transpose(), homogeneous_template_points_,
        MatrixXd::Zero(DIMENSIONS + 1, DIMENSIONS + 1);
    MatrixXd B(NUMPOINTS + DIMENSIONS + 1, DIMENSIONS);
    B << target_points_.transpose(), MatrixXd::Zero(DIMENSIONS + 1, DIMENSIONS);

    // params = A \ B
    const Matrix<double, Dynamic, DIMENSIONS> params = A.colPivHouseholderQr().solve(B).eval();
    warping_coeffs_ = params.topRows(NUMPOINTS).transpose();
    affine_coeffs_ = params.bottomRows(DIMENSIONS + 1).transpose();
    solved_ = true;
  }

  template <typename Derived>
  Eigen::Matrix<double, DIMENSIONS, Eigen::Dynamic> interpolate(
      const Eigen::MatrixBase<Derived>& interpolation_points) {
    using namespace Eigen;
    static_assert(
        MatrixBase<Derived>::RowsAtCompileTime == DIMENSIONS || MatrixBase<Derived>::RowsAtCompileTime == Dynamic,
        "INVALID DATA DIMENSIONS FOR interpolation_points");

    if (!solved_) {
      solveForCoeffs();
    }

    const auto num_points = interpolation_points.cols();
    HomogeneousPointSet homogeneous_interpolation_points(DIMENSIONS + 1, num_points);
    homogeneous_interpolation_points << interpolation_points, RowVectorXd::Ones(num_points);

    // Calculate the kenerl matrix comparing the new points to the template (control) points
    const MatrixXd Rsquared =
        EigenHelpers::SquaredDistancesBetweenPointSets(homogeneous_template_points_, homogeneous_interpolation_points);
    const MatrixXd kernel = Rsquared.unaryExpr(&ThinPlateSpline::KernelFunction);

    return affine_coeffs_ * homogeneous_interpolation_points + warping_coeffs_ * kernel;
  }

 protected:
  static double KernelFunction(const double r_sq) {
    // Equations taken from
    // https://www.geometrictools.com/Documentation/ThinPlateSplines.pdf
    // with coefficient (alpha) dropped as it is absorbed by warping_coeffs_
    switch (DIMENSIONS) {
      case 2:
        // This should be r^2 * log(r). We do not take the square root
        // because the factor of 2 gets absorbed by warping_coeffs_
        return r_sq == 0.0 ? 0.0 : r_sq * std::log(r_sq);
      case 3:
        // The negative is kept here for consistency with
        // https://www.geometrictools.com/GTEngine/Include/Mathematics/GteIntpThinPlateSpline3.h
        // and other TPS implementations found online
        return r_sq == 0.0 ? 0.0 : -std::sqrt(r_sq);
      default:
        static_assert(DIMENSIONS == 2 || DIMENSIONS == 3, "Only 2 and 3 dimensional kernels are implemented");
    }
  }

  HomogeneousPointSet homogeneous_template_points_;
  PointSet target_points_;
  Eigen::Matrix<double, DIMENSIONS, DIMENSIONS + 1> affine_coeffs_;
  PointSet warping_coeffs_;
  bool solved_;
};
}  // namespace arc_utilities

#endif  // THIN_PLATE_SPLINE_HPP


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
* \file
* \author   Collin Johnson
*
* Definition of binomial_proportion_z_test.
*/

#include <math/z_test.h>
#include <boost/math/distributions/normal.hpp>

namespace vulcan
{
namespace math
{

z_test_results_t binomial_proportion_z_test(const z_test_sample_t& sampleA,
                                            const z_test_sample_t& sampleB,
                                            double confidence)
{
    // Code closely related to Boost.Math example at:
    // http://www.boost.org/doc/libs/1_65_1/libs/math/doc/html/math_toolkit/stat_tut/weg/st_eg/two_sample_students_t.html

    using namespace boost::math;

    assert(sampleA.numSamples > 1);
    assert(sampleB.numSamples > 1);
    assert(sampleA.numTrue >= 0);
    assert(sampleB.numTrue >= 0);

    double successA = static_cast<double>(sampleA.numTrue) / sampleA.numSamples;
    double successB = static_cast<double>(sampleB.numTrue) / sampleB.numSamples;

    double totalSuccess = static_cast<double>(sampleA.numTrue + sampleB.numTrue)
        / (sampleA.numSamples + sampleB.numSamples);

    double zDenom = std::sqrt(totalSuccess * (1.0 - totalSuccess)
        * ((1.0 / sampleA.numSamples) + (1.0 / sampleB.numSamples)));

    z_test_results_t results;
    results.zValue     = (successA - successB) / zDenom;
    results.confidence = confidence;

    normal dist;

    results.pValueDifferent = cdf(complement(dist, std::abs(results.zValue)));
    results.areDifferent    = results.pValueDifferent < (confidence / 2.0);

    results.pValueGreater = cdf(complement(dist, results.zValue));
    results.isGreater     = results.pValueGreater < confidence;

    results.pValueLess = cdf(dist, results.zValue);
    results.isLess     = results.pValueLess < confidence;

    return results;
}

} // namespace math
} // namespace vulcan


/**
 * MIT License
 *
 * Copyright (c) 2018 Prabhsimran Singh
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
#pragma once

#include <algorithm>
#include <cmath>
#include <iostream>

#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
using namespace Eigen;

namespace F {

inline float glorot_uniform(const int &n_in, const int &n_out) {
    return 2.0 / (n_in + n_out);
}

inline MatrixXf sigmoid(const MatrixXf &x) {
    return (1.0 + (-x).array().exp()).inverse().matrix();
}

inline MatrixXf sigmoid_derivative(const MatrixXf &x) {
    return x * (1 - x.array()).matrix();
}

inline MatrixXf relu(const MatrixXf &x) {
    return x.cwiseMax(0.0);
}

MatrixXf tanh(const MatrixXf &x) {
    return sigmoid(x);
}

MatrixXf log_softmax(const MatrixXf &x) {
    auto e = x.array().exp();
    return (e / e.sum()).log().matrix();
}
} // namespace F


#include "SimplePly.h"
#include "rply.h"
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/SVD>

/**Calculates the distance the point is from the plane
   @param point a single point's position.
   @param planeEq plane Equation.
   @return distance between point and plane.
 */
float distToPlane(Eigen::Vector3f point, Eigen::Vector4f planeEq){
  float distance = planeEq(0)*point(0) + planeEq(1)*point(1) + planeEq(2)*point(2) + planeEq(3);
  distance /= std::sqrt(std::pow(planeEq(0),2) + std::pow(planeEq(1),2) + std::pow(planeEq(2),2));
  distance = std::abs(distance);
  return distance;
}

/**builds a plane out of 3 points
   @param point list of point positions.
   @return the equation of the plane.
 */
Eigen::Vector4f calcPlane(std::vector<Eigen::Vector3f> points){
  Eigen::Vector3f vectAB = points[1] - points[0], vectAC = points[2] - points[0];
  Eigen::Vector3f planeNorm = vectAB.cross(vectAC);
  float planeConst = -points[0].dot(planeNorm);
  Eigen::Vector4f planeEquation(planeNorm(0),planeNorm(1),planeNorm(2),planeConst);
  return planeEquation;
}

/**finds the distance that a point (with the greatest distance from the plane) has
   @param points list of points positions.
   @param planeEq the equation for the plane.
   @return the max distance between any point and the plane.
 */
float maxDistance(std::vector<Eigen::Vector3f> points, Eigen::Vector4f planeEq){
  float maxDist = 0, dist = 0;
  for(int i = 0; i < points.size(); i++){
    if((dist = distToPlane(points[i], planeEq)) > maxDist){
      maxDist = dist;
    }
  }
  return maxDist;
}

/**calculates the centroid of the points given.
   @param points list of points positions.
   @return the centroid of the points positions.
 */
Eigen::Vector3f calcCentroid(std::vector<Eigen::Vector3f> points){
  float aveX = 0, aveY = 0, aveZ = 0;
  Eigen::Vector3f centroid;
  for(int i = 0; i < points.size(); i++){
    aveX += points[i](0);
    aveY += points[i](1);
    aveZ += points[i](2);
  }
  //averaging the values.
  aveX/=points.size();
  aveY/=points.size();
  aveZ/=points.size();
  centroid(0) = aveX;
  centroid(1) = aveY;
  centroid(2) = aveZ;
  return centroid;
}

/** Preforms orthogonal least squares on the points given to find the best plane. Then the
    method projects the points onto the plane.
    @param points list of points location.
    @param plyPoints list of points in ply format.
*/
void leastSquares(std::vector<Eigen::Vector3f> points, std::vector<PlyPoint *> &plyPoints){
  Eigen::MatrixXf pointsMat(points.size(),3), pointsV;
  Eigen::Vector3f centroid = calcCentroid(points), planeNorm, adjustedPoint;
  Eigen::Vector4f planeEq;
  float planeNormNorm = 0, dist = 0;
  //builds matrix to use SVD on.
  for(int i = 0; i < points.size(); i++){
    for(int j = 0; j < 3; j++){
      pointsMat(i,j) = points[i](j) - centroid(j);
    }
  }
  //preforms SVD
  Eigen::JacobiSVD<Eigen::MatrixXf> svd(pointsMat, Eigen::ComputeFullV);
  pointsV = svd.matrixV();
  //builds plane based off of the SVD eigen vector and the centroid.
  for(int i = 0; i < 3; i++){
    planeEq(i) = pointsV(i,0);
    planeEq(3) -= planeEq(i)*centroid(i);
  }
  //finds normal of the plane normal.
  for(int i = 0; i < 3; i++){
    planeNormNorm += std::pow(planeEq(i),2);
  }
  planeNormNorm = std::sqrt(planeNormNorm);
  //builds a normalised plane normal vector.
  for(int i = 0; i < 3; i++){
    planeNorm(i) = planeEq(i)/planeNormNorm;
  }
  //projects points positions onto the calcuated Plane.
  for(int i = 0; i < plyPoints.size(); i++){
    dist = distToPlane(points[i],planeEq);

    adjustedPoint(0)= points[i](0) - (dist*planeNorm(0));
    adjustedPoint(1)= points[i](1) - (dist*planeNorm(1));
    adjustedPoint(2)= points[i](2) - (dist*planeNorm(2));
    if(dist < distToPlane(adjustedPoint,planeEq)){
      adjustedPoint(0)= points[i](0) + (2*dist*planeNorm(0));
      adjustedPoint(1)= points[i](1) + (2*dist*planeNorm(1));
      adjustedPoint(2)= points[i](2) + (2*dist*planeNorm(2));
    }

    (*plyPoints[i]).location(0) = adjustedPoint(0);
    (*plyPoints[i]).location(1) = adjustedPoint(1);
    (*plyPoints[i]).location(2) = adjustedPoint(2);
  }
}

/** Based on a percentage of points in the biggest plane, it calculates the threshold for the plane.
 * @param points the list of points positions.
 * @param planeEq the equation for the plane.
 * @param probThreshold the percentage of points required to determine threshold.
 * @return the calculated threshold.
 */
float calcThreshold(std::vector<Eigen::Vector3f> points, Eigen::Vector4f planeEq, float probThreshold){
  std::vector<float> buckets;
  float maxDist = maxDistance(points, planeEq);
  int numJumps = 10000, pointCount = 0, placeHold = 0, reqPoints = std::ceil(points.size()*probThreshold);
  float jumpSize = maxDist/(numJumps-1), dist, threshold = 0;

  for(int i = 0; i < numJumps; i++){
    buckets.push_back(0);
  }
  //Counts the number of Points in each region bucket.
  for(int i = 0; i < points.size(); i++){
    dist = distToPlane(points[i],planeEq);
    buckets[std::floor(dist/jumpSize)]++;
  }
  //sums point count in buckets until it goes over the users estimated plane count.
  while(pointCount < reqPoints){
    pointCount += buckets[placeHold];
    placeHold++;
  }
  //sets that point as threshold.
  threshold = (placeHold+1)*jumpSize;

  return threshold;

}

/**Determines the number of trials needed to have the accuracy that is requested.
 * @param inlierProb the probability of a point being an inlier.
 * @param acceptedFailProb the accepted rate of failure.
 *@return the number of trials required.
 */
float numChecks(float inlierProb, float acceptedFailProb){
  float numChecks = 0;
  if(inlierProb < 1){
    numChecks = ((std::log(acceptedFailProb))/(std::log(1 - (std::pow(inlierProb,3)))));
  }

  return numChecks;
}

/**Determines if the plane is a null plane.
   @param plane the plane equation.x
   @return a statement of if the plane is a null plane.
 */
bool planeNull(Eigen::Vector4f plane){
  return (abs(plane(0)) == 0 && plane(1) == 0 && plane(2) == 0 && plane(3) == 0);
}

/**Proforms analysis on the inputed point cloud, to find planes in the point cloud, colouring
   them accordingly. The program requires the user to input some information about the data,
   such as a points limit to when to stop looking for planes, an estimate of the percentage
   of points in the largest plane, and the probability of success.
*/
int main (int argc, char *argv[]) {
  int pointIn, count, bestCount, point1In, point2In, point3In, reqPlaneCalcs = 0;
  float posThresh = 0, threshProb = 0.4, threshold = 0, inlierProb = 0.001;
  Eigen::Vector4f bestPlane(0,0,0,0), curPlane;
  Eigen::Vector3f holdPoint;
  Eigen::Vector3i baseColour;
  std::vector<Eigen::Vector3f> points, pointsList, planePoints;
  int trialCount = 0,planeCount = 0;
  bool stillPlanes = true, threshFound = false;
  // Check the commandline arguments.
  if (argc != 6) {
    std::cout << "Usage: planeFinder <input file> <output file> <min num of points in plane> <percentage of points in largest plane> <accepted failure rate>" << std::endl;
    return -1;
  }
  int planeSizeThreshold = atoi(argv[3]);
  float pointPercentBiggestPlane = atof(argv[4]);
  float acceptedFailProb = atof(argv[5]);
  std::vector<PlyPoint *> pointsList2, planePoints2;

  reqPlaneCalcs = numChecks(pointPercentBiggestPlane, acceptedFailProb);
  threshold = std::numeric_limits<float>::max();
  std::cout << "Searching for planes until the next best plane size is less than " << planeSizeThreshold << std::endl;
  std::cout << "The estimated percentage of points in the biggest plane is " << pointPercentBiggestPlane << std::endl;
  std::cout << "Applying RANSAC with an acceptedFailRate of " << acceptedFailProb << std::endl;

  // Storage for the point cloud.
  SimplePly ply;
  // Read in the data from a PLY file
  std::cout << "Reading PLY data from " << argv[1] << std::endl;
  if (!ply.read(argv[1])) {
    std::cout << "Could not read PLY data from file " << argv[1] << std::endl;
    return -1;
  }

  // Stores copy of points read in.
  for(int i = 0; i < ply.size(); ++i){
    pointsList2.push_back(&ply[i]);
  }

  std::cout << "Read " << ply.size() << " points" << std::endl;

  // Recolour points - here we are just doing colour based on index
  std::cout << "Recolouring points" << std::endl;

  //Sets colours
  std::vector<Eigen::Vector3i> colours;
  colours.push_back(Eigen::Vector3i(255,0,0));
  colours.push_back(Eigen::Vector3i(0,255,0));
  colours.push_back(Eigen::Vector3i(0,0,255));
  colours.push_back(Eigen::Vector3i(255,255,0));
  colours.push_back(Eigen::Vector3i(0,255,255));
  colours.push_back(Eigen::Vector3i(255,0,255));
  colours.push_back(Eigen::Vector3i(255,255,255));
  colours.push_back(Eigen::Vector3i(0,0,0));

  holdPoint = Eigen::Vector3f(ply[0].location(0),ply[0].location(1),ply[0].location(2));

  //builds a vector containing vectors of the points positions.
  for(int i = 0; i < ply.size(); i++){
    holdPoint = Eigen::Vector3f(ply[i].location(0),ply[i].location(1),ply[i].location(2));
    pointsList.push_back(holdPoint);
  }

  //starts looking for planes.
  while(stillPlanes && pointsList.size() > 2){
    //zeros bestPlane.
    for(int j = 0; j < 4; j++){
      bestPlane(j) = 0;
    }

    //increase plane Count.

    planeCount++;
    //Resets variables.
    bestCount = 0;
    inlierProb = 0.001;
    trialCount = 0;

    //finds the threshold based off of users estimate.
    if(!threshFound){
      for(int z = 0; z < 110; z++){
	point1In = std::rand()%pointsList.size();
	point2In = std::rand()%pointsList.size();
	point3In = std::rand()%pointsList.size();
	points.clear();
	points.push_back(pointsList[point1In]);
	points.push_back(pointsList[point2In]);
	points.push_back(pointsList[point3In]);
	curPlane = calcPlane(points);
	if(!planeNull(curPlane)){
	  posThresh = calcThreshold(pointsList, curPlane, pointPercentBiggestPlane);
	  if(posThresh < threshold){
	    threshold = posThresh;
	    bestPlane = curPlane;
	  }
	}
      }
      //makes sure only one threshold is found.
      threshFound = true;
    }

    //starts looking for planes with the calculated threshold.
    while(trialCount <  numChecks(inlierProb, acceptedFailProb)){
      trialCount++;
      std::cout << numChecks(inlierProb, acceptedFailProb) << std::endl;
      //finds three random points and builds a plane.
      point1In = std::rand()%pointsList.size();
      point2In = std::rand()%pointsList.size();
      point3In = std::rand()%pointsList.size();
      points.clear();
      points.push_back(pointsList[point1In]);
      points.push_back(pointsList[point2In]);
      points.push_back(pointsList[point3In]);
      curPlane = calcPlane(points);
      count = 0;
      //makes sure it isn't a null plane.
      if(!planeNull(curPlane)){
	for(int k = 0; k < pointsList.size(); k++){
	  //counts number of points within threshold.
	  if(distToPlane(pointsList[k], curPlane) <= threshold){
	    count++;
	  }
	}
	//checks if this is best plane then updates if it is.
	if(count > bestCount){
	  bestCount = count;
	  bestPlane = curPlane;
	  inlierProb = ((float)bestCount)/pointsList.size();

	}
      }
    }
    //Cuts out if the plane is too small.
    if(bestCount < planeSizeThreshold){
      stillPlanes = false;
    }
    //Makes sure the unlikely chance of a null best plane occuring doesn't get through.
    else if(!planeNull(bestPlane)){
      //recolours and removes points.
      for(int j = pointsList2.size() - 1; j > -1; j--){
	holdPoint(0) = (*pointsList2[j]).location(0);
	holdPoint(1) =  (*pointsList2[j]).location(1);
	holdPoint(2) =  (*pointsList2[j]).location(2);

	if(distToPlane(holdPoint, bestPlane) <= threshold){

	  (*pointsList2[j]).colour = colours[planeCount%colours.size()];
	  //planePoints.push_back(pointsList[j]);
	  pointsList[j] = pointsList[pointsList.size() - 1];
	  pointsList.pop_back();
	  //planePoints2.push_back(pointsList2[j]);
	  pointsList2[j] = pointsList2[pointsList2.size() - 1];
	  pointsList2.pop_back();

	}
      }
      //would do least squares if it worked properly. No time to work out the error.
      //leastSquares(planePoints, planePoints2);
      //planePoints.clear();
      //planePoints2.clear();
    }
  }

  // Write the resulting (re-coloured) point cloud to a PLY file.
  std::cout << "Writing PLY data to " << argv[2] << std::endl;
  if (!ply.write(argv[2])) {
    std::cout << "Could not write PLY data to file " << argv[2] << std::endl;
    return -2;
  }
  return 0;
}


/* Boost example/transc.cpp
 * how to use an external library (GMP/MPFR in this case) in order to
 * get correct transcendental functions on intervals
 *
 * Copyright 2003 Guillaume Melquiond
 *
 * 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 <boost/numeric/interval.hpp>
//extern "C" {
#include <gmp.h>
#include <mpfr.h>
//}
#include <iostream>

struct full_rounding:
  boost::numeric::interval_lib::rounded_arith_opp<double>
{
private:
  typedef int mpfr_func(mpfr_t, const __mpfr_struct*, mp_rnd_t);
  double invoke_mpfr(double x, mpfr_func f, mp_rnd_t r) {
    mpfr_t xx;
    mpfr_init_set_d(xx, x, GMP_RNDN);
    f(xx, xx, r);
    double res = mpfr_get_d(xx, r);
    mpfr_clear(xx);
    return res;
  }
public:
# define GENR_FUNC(name) \
  double name##_down(double x) { return invoke_mpfr(x, mpfr_##name, GMP_RNDD); } \
  double name##_up  (double x) { return invoke_mpfr(x, mpfr_##name, GMP_RNDU); }
  GENR_FUNC(exp)
  GENR_FUNC(log)
  GENR_FUNC(sin)
  GENR_FUNC(cos)
  GENR_FUNC(tan)
  GENR_FUNC(asin)
  GENR_FUNC(acos)
  GENR_FUNC(atan)
  GENR_FUNC(sinh)
  GENR_FUNC(cosh)
  GENR_FUNC(tanh)
  GENR_FUNC(asinh)
  GENR_FUNC(acosh)
  GENR_FUNC(atanh)
};

namespace dummy {
  using namespace boost;
  using namespace numeric;
  using namespace interval_lib;
  typedef save_state<full_rounding> R;
  typedef checking_strict<double> P;
  typedef interval<double, policies<R, P> > I;
};

typedef dummy::I I;

template<class os_t>
os_t& operator<<(os_t &os, const I &a) {
  os << '[' << a.lower() << ',' << a.upper() << ']';
  return os;
}

int main() {
  I x(0.5, 2.5);
  std::cout << "x = " << x << std::endl;
  std::cout.precision(16);
# define GENR_TEST(name) \
  std::cout << #name "(x) = " << name(x) << std::endl
  GENR_TEST(exp);
  GENR_TEST(log);
  GENR_TEST(sin);
  GENR_TEST(cos);
  GENR_TEST(tan);
  GENR_TEST(asin);
  GENR_TEST(acos);
  GENR_TEST(atan);
  GENR_TEST(sinh);
  GENR_TEST(cosh);
  GENR_TEST(tanh);
  GENR_TEST(asinh);
  GENR_TEST(acosh);
  GENR_TEST(atanh);
  return 0;
}


// find_mean_and_sd_normal.cpp

// Copyright Paul A. Bristow 2007, 2010.

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

// Example of finding mean or sd for normal distribution.

// Note that this file contains Quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[normal_std
/*`
First we need some includes to access the normal distribution,
the algorithms to find location and scale
(and some std output of course).
*/

#include <boost/math/distributions/normal.hpp> // for normal_distribution
  using boost::math::normal; // typedef provides default type is double.
#include <boost/math/distributions/cauchy.hpp> // for cauchy_distribution
  using boost::math::cauchy; // typedef provides default type is double.
#include <boost/math/distributions/find_location.hpp>
  using boost::math::find_location;
#include <boost/math/distributions/find_scale.hpp>
  using boost::math::find_scale;
  using boost::math::complement;
  using boost::math::policies::policy;

#include <iostream>
  using std::cout; using std::endl; using std::left; using std::showpoint; using std::noshowpoint;
#include <iomanip>
  using std::setw; using std::setprecision;
#include <limits>
  using std::numeric_limits;
#include <stdexcept>
  using std::exception;
//] [/normal_std Quickbook]

int main()
{
  cout << "Find_location (mean) and find_scale (standard deviation) examples." << endl;
  try
  {

//[normal_find_location_and_scale_eg

/*`
[h4 Using find_location and find_scale to meet dispensing and measurement specifications]

Consider an example from K Krishnamoorthy,
Handbook of Statistical Distributions with Applications,
ISBN 1-58488-635-8, (2006) p 126, example 10.3.7.

"A machine is set to pack 3 kg of ground beef per pack.
Over a long period of time it is found that the average packed was 3 kg
with a standard deviation of 0.1 kg.
Assume the packing is normally distributed."

We start by constructing a normal distribution with the given parameters:
*/

double mean = 3.; // kg
double standard_deviation = 0.1; // kg
normal packs(mean, standard_deviation);
/*`We can then find the fraction (or %) of packages that weigh more than 3.1 kg.
*/

double max_weight = 3.1; // kg
cout << "Percentage of packs > " << max_weight << " is "
<< cdf(complement(packs, max_weight)) * 100. << endl; // P(X > 3.1)

/*`We might want to ensure that 95% of packs are over a minimum weight specification,
then we want the value of the mean such that P(X < 2.9) = 0.05.

Using the mean of 3 kg, we can estimate
the fraction of packs that fail to meet the specification of 2.9 kg.
*/

double minimum_weight = 2.9;
cout <<"Fraction of packs <= " << minimum_weight << " with a mean of " << mean
  << " is " << cdf(complement(packs, minimum_weight)) << endl;
// fraction of packs <= 2.9 with a mean of 3 is 0.841345

/*`This is 0.84 - more than the target fraction of 0.95.
If we want 95% to be over the minimum weight,
what should we set the mean weight to be?

Using the KK StatCalc program supplied with the book and the method given on page 126 gives 3.06449.

We can confirm this by constructing a new distribution which we call 'xpacks'
with a safety margin mean of 3.06449 thus:
*/
double over_mean = 3.06449;
normal xpacks(over_mean, standard_deviation);
cout << "Fraction of packs >= " << minimum_weight
<< " with a mean of " << xpacks.mean()
  << " is " << cdf(complement(xpacks, minimum_weight)) << endl;
// fraction of packs >= 2.9 with a mean of 3.06449 is 0.950005

/*`Using this Math Toolkit, we can calculate the required mean directly thus:
*/
double under_fraction = 0.05;  // so 95% are above the minimum weight mean - sd = 2.9
double low_limit = standard_deviation;
double offset = mean - low_limit - quantile(packs, under_fraction);
double nominal_mean = mean + offset;
// mean + (mean - low_limit - quantile(packs, under_fraction));

normal nominal_packs(nominal_mean, standard_deviation);
cout << "Setting the packer to " << nominal_mean << " will mean that "
  << "fraction of packs >= " << minimum_weight
  << " is " << cdf(complement(nominal_packs, minimum_weight)) << endl;
// Setting the packer to 3.06449 will mean that fraction of packs >= 2.9 is 0.95

/*`
This calculation is generalized as the free function called
[link math_toolkit.dist.dist_ref.dist_algorithms find_location].

To use this we will need to
*/

#include <boost/math/distributions/find_location.hpp>
  using boost::math::find_location;
/*`and then use find_location function to find safe_mean,
& construct a new normal distribution called 'goodpacks'.
*/
double safe_mean = find_location<normal>(minimum_weight, under_fraction, standard_deviation);
normal good_packs(safe_mean, standard_deviation);
/*`with the same confirmation as before:
*/
cout << "Setting the packer to " << nominal_mean << " will mean that "
  << "fraction of packs >= " << minimum_weight
  << " is " << cdf(complement(good_packs, minimum_weight)) << endl;
// Setting the packer to 3.06449 will mean that fraction of packs >= 2.9 is 0.95

/*`
[h4 Using Cauchy-Lorentz instead of normal distribution]

After examining the weight distribution of a large number of packs, we might decide that,
after all, the assumption of a normal distribution is not really justified.
We might find that the fit is better to a __cauchy_distrib.
This distribution has wider 'wings', so that whereas most of the values
are closer to the mean than the normal, there are also more values than 'normal'
that lie further from the mean than the normal.

This might happen because a larger than normal lump of meat is either included or excluded.

We first create a __cauchy_distrib with the original mean and standard deviation,
and estimate the fraction that lie below our minimum weight specification.
*/

cauchy cpacks(mean, standard_deviation);
cout << "Cauchy Setting the packer to " << mean << " will mean that "
  << "fraction of packs >= " << minimum_weight
  << " is " << cdf(complement(cpacks, minimum_weight)) << endl;
// Cauchy Setting the packer to 3 will mean that fraction of packs >= 2.9 is 0.75

/*`Note that far fewer of the packs meet the specification, only 75% instead of 95%.
Now we can repeat the find_location, using the cauchy distribution as template parameter,
in place of the normal used above.
*/

double lc = find_location<cauchy>(minimum_weight, under_fraction, standard_deviation);
cout << "find_location<cauchy>(minimum_weight, over fraction, standard_deviation); " << lc << endl;
// find_location<cauchy>(minimum_weight, over fraction, packs.standard_deviation()); 3.53138
/*`Note that the safe_mean setting needs to be much higher, 3.53138 instead of 3.06449,
so we will make rather less profit.

And again confirm that the fraction meeting specification is as expected.
*/
cauchy goodcpacks(lc, standard_deviation);
cout << "Cauchy Setting the packer to " << lc << " will mean that "
  << "fraction of packs >= " << minimum_weight
  << " is " << cdf(complement(goodcpacks, minimum_weight)) << endl;
// Cauchy Setting the packer to 3.53138 will mean that fraction of packs >= 2.9 is 0.95

/*`Finally we could estimate the effect of a much tighter specification,
that 99% of packs met the specification.
*/

cout << "Cauchy Setting the packer to "
  << find_location<cauchy>(minimum_weight, 0.99, standard_deviation)
  << " will mean that "
  << "fraction of packs >= " << minimum_weight
  << " is " << cdf(complement(goodcpacks, minimum_weight)) << endl;

/*`Setting the packer to 3.13263 will mean that fraction of packs >= 2.9 is 0.99,
but will more than double the mean loss from 0.0644 to 0.133 kg per pack.

Of course, this calculation is not limited to packs of meat, it applies to dispensing anything,
and it also applies to a 'virtual' material like any measurement.

The only caveat is that the calculation assumes that the standard deviation (scale) is known with
a reasonably low uncertainty, something that is not so easy to ensure in practice.
And that the distribution is well defined, __normal_distrib or __cauchy_distrib, or some other.

If one is simply dispensing a very large number of packs,
then it may be feasible to measure the weight of hundreds or thousands of packs.
With a healthy 'degrees of freedom', the confidence intervals for the standard deviation
are not too wide, typically about + and - 10% for hundreds of observations.

For other applications, where it is more difficult or expensive to make many observations,
the confidence intervals are depressingly wide.

See [link math_toolkit.dist.stat_tut.weg.cs_eg.chi_sq_intervals Confidence Intervals on the standard deviation]
for a worked example
[@../../../example/chi_square_std_dev_test.cpp chi_square_std_dev_test.cpp]
of estimating these intervals.

[h4 Changing the scale or standard deviation]

Alternatively, we could invest in a better (more precise) packer
(or measuring device) with a lower standard deviation, or scale.

This might cost more, but would reduce the amount we have to 'give away'
in order to meet the specification.

To estimate how much better (how much smaller standard deviation) it would have to be,
we need to get the 5% quantile to be located at the under_weight limit, 2.9
*/
double p = 0.05; // wanted p th quantile.
cout << "Quantile of " << p << " = " << quantile(packs, p)
  << ", mean = " << packs.mean() << ", sd = " << packs.standard_deviation() << endl;
/*`
Quantile of 0.05 = 2.83551, mean = 3, sd = 0.1

With the current packer (mean = 3, sd = 0.1), the 5% quantile is at 2.8551 kg,
a little below our target of 2.9 kg.
So we know that the standard deviation is going to have to be smaller.

Let's start by guessing that it (now 0.1) needs to be halved, to a standard deviation of 0.05 kg.
*/
normal pack05(mean, 0.05);
cout << "Quantile of " << p << " = " << quantile(pack05, p)
  << ", mean = " << pack05.mean() << ", sd = " << pack05.standard_deviation() << endl;
// Quantile of 0.05 = 2.91776, mean = 3, sd = 0.05

cout <<"Fraction of packs >= " << minimum_weight << " with a mean of " << mean
  << " and standard deviation of " << pack05.standard_deviation()
  << " is " << cdf(complement(pack05, minimum_weight)) << endl;
// Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.05 is 0.97725
/*`
So 0.05 was quite a good guess, but we are a little over the 2.9 target,
so the standard deviation could be a tiny bit more. So we could do some
more guessing to get closer, say by increasing standard deviation to 0.06 kg,
constructing another new distribution called pack06.
*/
normal pack06(mean, 0.06);
cout << "Quantile of " << p << " = " << quantile(pack06, p)
  << ", mean = " << pack06.mean() << ", sd = " << pack06.standard_deviation() << endl;
// Quantile of 0.05 = 2.90131, mean = 3, sd = 0.06

cout <<"Fraction of packs >= " << minimum_weight << " with a mean of " << mean
  << " and standard deviation of " << pack06.standard_deviation()
  << " is " << cdf(complement(pack06, minimum_weight)) << endl;
// Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.06 is 0.95221
/*`
Now we are getting really close, but to do the job properly,
we might need to use root finding method, for example the tools provided,
and used elsewhere, in the Math Toolkit, see
[link math_toolkit.toolkit.internals1.roots2  Root Finding Without Derivatives].

But in this (normal) distribution case, we can and should be even smarter
and make a direct calculation.
*/

/*`Our required limit is minimum_weight = 2.9 kg, often called the random variate z.
For a standard normal distribution, then probability p = N((minimum_weight - mean) / sd).

We want to find the standard deviation that would be required to meet this limit,
so that the p th quantile is located at z (minimum_weight).
In this case, the 0.05 (5%) quantile is at 2.9 kg pack weight, when the mean is 3 kg,
ensuring that 0.95 (95%) of packs are above the minimum weight.

Rearranging, we can directly calculate the required standard deviation:
*/
normal N01; // standard normal distribution with meamn zero and unit standard deviation.
p = 0.05;
double qp = quantile(N01, p);
double sd95 = (minimum_weight - mean) / qp;

cout << "For the "<< p << "th quantile to be located at "
  << minimum_weight << ", would need a standard deviation of " << sd95 << endl;
// For the 0.05th quantile to be located at 2.9, would need a standard deviation of 0.0607957

/*`We can now construct a new (normal) distribution pack95 for the 'better' packer,
and check that our distribution will meet the specification.
*/

normal pack95(mean, sd95);
cout <<"Fraction of packs >= " << minimum_weight << " with a mean of " << mean
  << " and standard deviation of " << pack95.standard_deviation()
  << " is " << cdf(complement(pack95, minimum_weight)) << endl;
// Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.0607957 is 0.95

/*`This calculation is generalized in the free function find_scale,
as shown below, giving the same standard deviation.
*/
double ss = find_scale<normal>(minimum_weight, under_fraction, packs.mean());
cout << "find_scale<normal>(minimum_weight, under_fraction, packs.mean()); " << ss << endl;
// find_scale<normal>(minimum_weight, under_fraction, packs.mean()); 0.0607957

/*`If we had defined an over_fraction, or percentage that must pass specification
*/
double over_fraction = 0.95;
/*`And (wrongly) written

 double sso = find_scale<normal>(minimum_weight, over_fraction, packs.mean());

With the default policy, we would get a message like

[pre
Message from thrown exception was:
   Error in function boost::math::find_scale<Dist, Policy>(double, double, double, Policy):
   Computed scale (-0.060795683191176959) is <= 0! Was the complement intended?
]

But this would return a *negative* standard deviation - obviously impossible.
The probability should be 1 - over_fraction, not over_fraction, thus:
*/

double ss1o = find_scale<normal>(minimum_weight, 1 - over_fraction, packs.mean());
cout << "find_scale<normal>(minimum_weight, under_fraction, packs.mean()); " << ss1o << endl;
// find_scale<normal>(minimum_weight, under_fraction, packs.mean()); 0.0607957

/*`But notice that using '1 - over_fraction' - will lead to a
[link why_complements loss of accuracy, especially if over_fraction was close to unity.]
In this (very common) case, we should instead use the __complements,
giving the most accurate result.
*/

double ssc = find_scale<normal>(complement(minimum_weight, over_fraction, packs.mean()));
cout << "find_scale<normal>(complement(minimum_weight, over_fraction, packs.mean())); " << ssc << endl;
// find_scale<normal>(complement(minimum_weight, over_fraction, packs.mean())); 0.0607957

/*`Note that our guess of 0.06 was close to the accurate value of 0.060795683191176959.

We can again confirm our prediction thus:
*/

normal pack95c(mean, ssc);
cout <<"Fraction of packs >= " << minimum_weight << " with a mean of " << mean
  << " and standard deviation of " << pack95c.standard_deviation()
  << " is " << cdf(complement(pack95c, minimum_weight)) << endl;
// Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.0607957 is 0.95

/*`Notice that these two deceptively simple questions:

* Do we over-fill to make sure we meet a minimum specification (or under-fill to avoid an overdose)?

and/or

* Do we measure better?

are actually extremely common.

The weight of beef might be replaced by a measurement of more or less anything,
from drug tablet content, Apollo landing rocket firing, X-ray treatment doses...

The scale can be variation in dispensing or uncertainty in measurement.
*/
//] [/normal_find_location_and_scale_eg Quickbook end]

  }
  catch(const exception& e)
  { // Always useful to include try & catch blocks because default policies
    // are to throw exceptions on arguments that cause errors like underflow, overflow.
    // Lacking try & catch blocks, the program will abort without a message below,
    // which may give some helpful clues as to the cause of the exception.
    cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << endl;
  }
  return 0;
}  // int main()

/*

Output is:

//[normal_find_location_and_scale_output

Find_location (mean) and find_scale (standard deviation) examples.
Percentage of packs > 3.1 is 15.8655
Fraction of packs <= 2.9 with a mean of 3 is 0.841345
Fraction of packs >= 2.9 with a mean of 3.06449 is 0.950005
Setting the packer to 3.06449 will mean that fraction of packs >= 2.9 is 0.95
Setting the packer to 3.06449 will mean that fraction of packs >= 2.9 is 0.95
Cauchy Setting the packer to 3 will mean that fraction of packs >= 2.9 is 0.75
find_location<cauchy>(minimum_weight, over fraction, standard_deviation); 3.53138
Cauchy Setting the packer to 3.53138 will mean that fraction of packs >= 2.9 is 0.95
Cauchy Setting the packer to -0.282052 will mean that fraction of packs >= 2.9 is 0.95
Quantile of 0.05 = 2.83551, mean = 3, sd = 0.1
Quantile of 0.05 = 2.91776, mean = 3, sd = 0.05
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.05 is 0.97725
Quantile of 0.05 = 2.90131, mean = 3, sd = 0.06
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.06 is 0.95221
For the 0.05th quantile to be located at 2.9, would need a standard deviation of 0.0607957
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.0607957 is 0.95
find_scale<normal>(minimum_weight, under_fraction, packs.mean()); 0.0607957
find_scale<normal>(minimum_weight, under_fraction, packs.mean()); 0.0607957
find_scale<normal>(complement(minimum_weight, over_fraction, packs.mean())); 0.0607957
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.0607957 is 0.95

//] [/normal_find_location_and_scale_eg_output]

*/


#ifndef RSVD_ERROR_ESTIMATORS_HPP_
#define RSVD_ERROR_ESTIMATORS_HPP_

#include <Eigen/Dense>

namespace Rsvd {

/// \brief Compute the relative Frobenius error of a matrix approximation.
///
/// \long The formula for the relative error is
/// \f[ \varepsilon = \frac{ \| \tilde{A} - A \|_{\mathrm{F}} }{ \| A \|_{\mathrm{F}} }. \f]
///
/// Both matrices can be real or complex.
///
/// \tparam MatrixType Eigen matrix type.
///
/// \param reference Reference matrix \f$ A \f$. It is assumed that this matrix has positive
/// Frobenius norm.
///
/// \param approximation Approximation \f$ \tilde{A} \f$ of the reference matrix.
///
/// \return Relative Frobenius error \f$ \varepsilon \f$ of the approximation (non-negative real).
template <typename MatrixType>
typename Eigen::NumTraits<typename MatrixType::Scalar>::Real
relativeFrobeniusNormError(const MatrixType &reference, const MatrixType &approximation) {
  const auto differenceNorm = (approximation - reference).norm();
  const auto referenceNorm = reference.norm();

  assert(referenceNorm > 0);

  return differenceNorm / referenceNorm;
}

} // namespace Rsvd

#endif


#include "optimizers.h"
#include <Eigen/Dense>

using namespace probreg;

GaussNewtonResult probreg::gaussNewton(const Vector& x, const gn_func_type& fn, Float eps, Integer num_max_iteration) {
    const Float eps2 = eps * eps;
    Vector xn = x;
    Matrix rx;
    for (Integer n = 0; n < num_max_iteration; ++n) {
        ResidualAmatBvec amat_bvec = fn(xn);
        const Vector dx = std::get<1>(amat_bvec).ldlt().solve(std::get<2>(amat_bvec));
        xn -= dx;
        rx = std::get<0>(amat_bvec);
        if (dx.squaredNorm() < eps2) {
            break;
        }
    }
    return std::make_pair(xn, rx);
}