// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

// This example consists of a single static target which sits in one
// place and does not move; in effect it has a "GPS" which measures
// its position

#include <Eigen/StdVector>
#include <iostream>
#include <stdint.h>

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/stuff/sampler.h>

#include "targetTypes3D.hpp"

using namespace Eigen;
using namespace std;
using namespace g2o;

int main()
{
  // Set up the optimiser
  SparseOptimizer optimizer;
  optimizer.setVerbose(false);

  // Create the block solver - the dimensions are specified because
  // 3D observations marginalise to a 3D estimate
  typedef BlockSolver<BlockSolverTraits<3, 3> > BlockSolver_3_3;
  OptimizationAlgorithmGaussNewton* solver = new OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolver_3_3>(
      g2o::make_unique<LinearSolverCholmod<BlockSolver_3_3::PoseMatrixType>>()));

  optimizer.setAlgorithm(solver);

  // Sample the actual location of the target
  Vector3d truePoint(sampleUniform(-500, 500),
                     sampleUniform(-500, 500),
                     sampleUniform(-500, 500));

  // Construct vertex which corresponds to the actual point of the target
  VertexPosition3D* position = new VertexPosition3D();
  position->setId(0);
  optimizer.addVertex(position);

  // Now generate some noise corrupted measurements; for simplicity
  // these are uniformly distributed about the true target. These are
  // modelled as a unary edge because they do not like to, say,
  // another node in the map.
  int numMeasurements = 10;
  double noiseLimit = sqrt(12.);
  double noiseSigma = noiseLimit*noiseLimit / 12.0;

  for (int i = 0; i < numMeasurements; i++)
    {
      Vector3d measurement = truePoint +
        Vector3d(sampleUniform(-0.5, 0.5) * noiseLimit,
                 sampleUniform(-0.5, 0.5) * noiseLimit,
                 sampleUniform(-0.5, 0.5) * noiseLimit);
      GPSObservationPosition3DEdge* goe = new GPSObservationPosition3DEdge();
      goe->setVertex(0, position);
      goe->setMeasurement(measurement);
      goe->setInformation(Matrix3d::Identity() / noiseSigma);
      optimizer.addEdge(goe);
    }

  // Configure and set things going
  optimizer.initializeOptimization();
  optimizer.setVerbose(true);
  optimizer.optimize(5);

  cout << "truePoint=\n" << truePoint << endl;

  cerr <<  "computed estimate=\n" << dynamic_cast<VertexPosition3D*>(optimizer.vertices().find(0)->second)->estimate() << endl;

  //position->setMarginalized(true);

  SparseBlockMatrix<MatrixXd> spinv;

  optimizer.computeMarginals(spinv, position);

  //optimizer.solver()->computeMarginals();

  // covariance
  //
  cout << "covariance\n" << spinv << endl;

  cout << spinv.block(0,0) << endl;

}


//
// 	Copyright (c) 2021 Cem Bassoy, cem.bassoy@gmail.com
//
//  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/test/unit_test.hpp>
#include <boost/numeric/ublas/tensor/extents.hpp>

BOOST_AUTO_TEST_SUITE ( test_shape_dynamic_static_rank )

struct fixture
{
  template<std::size_t N>
  using shape_t = boost::numeric::ublas::extents<N>;

//  static inline auto n     = shape_t<0>{};
  static inline auto n1    = shape_t<1>{1};
  static inline auto n2    = shape_t<1>{2};
  static inline auto n11   = shape_t<2>{1,1};
  static inline auto n12   = shape_t<2>{1,2};
  static inline auto n21   = shape_t<2>{2,1};
  static inline auto n22   = shape_t<2>{2,2};
  static inline auto n32   = shape_t<2>{3,2};
  static inline auto n111  = shape_t<3>{1,1,1};
  static inline auto n211  = shape_t<3>{2,1,1};
  static inline auto n121  = shape_t<3>{1,2,1};
  static inline auto n112  = shape_t<3>{1,1,2};
  static inline auto n123  = shape_t<3>{1,2,3};
  static inline auto n321  = shape_t<3>{3,2,1};
  static inline auto n213  = shape_t<3>{2,1,3};
  static inline auto n432  = shape_t<3>{4,3,2};
};

BOOST_FIXTURE_TEST_CASE(test_extents_dynamic_rank_static_empty,
                        fixture,
                        *boost::unit_test::label("dynamic_extents_rank_static") *boost::unit_test::label("empty"))
{
  namespace ublas = boost::numeric::ublas;
//  BOOST_CHECK( ublas::empty(n   ));
  BOOST_CHECK(!ublas::empty(n1  ));
  BOOST_CHECK(!ublas::empty(n2  ));
  BOOST_CHECK(!ublas::empty(n11 ));
  BOOST_CHECK(!ublas::empty(n12 ));
  BOOST_CHECK(!ublas::empty(n21 ));
  BOOST_CHECK(!ublas::empty(n22 ));
  BOOST_CHECK(!ublas::empty(n32 ));
  BOOST_CHECK(!ublas::empty(n111));
  BOOST_CHECK(!ublas::empty(n211));
  BOOST_CHECK(!ublas::empty(n121));
  BOOST_CHECK(!ublas::empty(n112));
  BOOST_CHECK(!ublas::empty(n123));
  BOOST_CHECK(!ublas::empty(n321));
  BOOST_CHECK(!ublas::empty(n213));
  BOOST_CHECK(!ublas::empty(n432));

  BOOST_CHECK_THROW( shape_t<3>({1,1,0}), std::invalid_argument);
  BOOST_CHECK_THROW( shape_t<2>({1,0}), std::invalid_argument);
  BOOST_CHECK_THROW( shape_t<1>({0}  ), std::invalid_argument);
  BOOST_CHECK_THROW( shape_t<2>({0,1}), std::invalid_argument);
}

BOOST_FIXTURE_TEST_CASE(test_extents_dynamic_rank_static_size,
                        fixture,
                        *boost::unit_test::label("dynamic_extents_rank_static") *boost::unit_test::label("size"))
{
  namespace ublas = boost::numeric::ublas;

//  BOOST_CHECK_EQUAL(ublas::size(n   ),0);
  BOOST_CHECK_EQUAL(ublas::size(n1  ),1);
  BOOST_CHECK_EQUAL(ublas::size(n2  ),1);
  BOOST_CHECK_EQUAL(ublas::size(n11 ),2);
  BOOST_CHECK_EQUAL(ublas::size(n12 ),2);
  BOOST_CHECK_EQUAL(ublas::size(n21 ),2);
  BOOST_CHECK_EQUAL(ublas::size(n22 ),2);
  BOOST_CHECK_EQUAL(ublas::size(n32 ),2);
  BOOST_CHECK_EQUAL(ublas::size(n111),3);
  BOOST_CHECK_EQUAL(ublas::size(n211),3);
  BOOST_CHECK_EQUAL(ublas::size(n121),3);
  BOOST_CHECK_EQUAL(ublas::size(n112),3);
  BOOST_CHECK_EQUAL(ublas::size(n123),3);
  BOOST_CHECK_EQUAL(ublas::size(n321),3);
  BOOST_CHECK_EQUAL(ublas::size(n213),3);
  BOOST_CHECK_EQUAL(ublas::size(n432),3);
}

BOOST_FIXTURE_TEST_CASE(test_extents_dynamic_rank_static_at_read,
                        fixture,
                        *boost::unit_test::label("dynamic_extents_rank_static") *boost::unit_test::label("at_read"))
{
  BOOST_CHECK_EQUAL(n1  .at(0),1);
  BOOST_CHECK_EQUAL(n2  .at(0),2);

  BOOST_CHECK_EQUAL(n11 .at(0),1);
  BOOST_CHECK_EQUAL(n11 .at(1),1);

  BOOST_CHECK_EQUAL(n12 .at(0),1);
  BOOST_CHECK_EQUAL(n12 .at(1),2);

  BOOST_CHECK_EQUAL(n21 .at(0),2);
  BOOST_CHECK_EQUAL(n21 .at(1),1);

  BOOST_CHECK_EQUAL(n22 .at(0),2);
  BOOST_CHECK_EQUAL(n22 .at(1),2);

  BOOST_CHECK_EQUAL(n32 .at(0),3);
  BOOST_CHECK_EQUAL(n32 .at(1),2);

  BOOST_CHECK_EQUAL(n432.at(0),4);
  BOOST_CHECK_EQUAL(n432.at(1),3);
  BOOST_CHECK_EQUAL(n432.at(2),2);

//  BOOST_CHECK_THROW( (void)n  .at(0), std::out_of_range);
  BOOST_CHECK_THROW( (void)n32.at(2), std::out_of_range);
  BOOST_CHECK_THROW( (void)n32.at(5), std::out_of_range);
}

BOOST_FIXTURE_TEST_CASE(test_extents_dynamic_rank_static_operator_access_read,
                        fixture,
                        *boost::unit_test::label("dynamic_extents_rank_static") *boost::unit_test::label("operator_access_read"))
{
  BOOST_CHECK_EQUAL(n1  [0],1);
  BOOST_CHECK_EQUAL(n2  [0],2);

  BOOST_CHECK_EQUAL(n11 [0],1);
  BOOST_CHECK_EQUAL(n11 [1],1);

  BOOST_CHECK_EQUAL(n12 [0],1);
  BOOST_CHECK_EQUAL(n12 [1],2);

  BOOST_CHECK_EQUAL(n21 [0],2);
  BOOST_CHECK_EQUAL(n21 [1],1);

  BOOST_CHECK_EQUAL(n22 [0],2);
  BOOST_CHECK_EQUAL(n22 [1],2);

  BOOST_CHECK_EQUAL(n32 [0],3);
  BOOST_CHECK_EQUAL(n32 [1],2);

  BOOST_CHECK_EQUAL(n432[0],4);
  BOOST_CHECK_EQUAL(n432[1],3);
  BOOST_CHECK_EQUAL(n432[2],2);
}

BOOST_AUTO_TEST_SUITE_END()


#include "Include\NeuralNetwork.h"
#include <iostream>
#include <assert.h>
#include <armadillo_bits/constants_old.hpp>

#include "../math.h"
#include "../Drawing.h"
#include "../GenericAlgorithm/Include/GenericAlgorithm.h"

namespace nn
{
	NeuralNetwork::NeuralNetwork(uint16_t inputLayer, uint16_t hiddenLayer, uint16_t outputLayer, float learnRate)
		: learnRate_(learnRate)
		, layersCount_(3)
	{
		layers_		= { inputLayer, hiddenLayer, outputLayer };
		neurons_	= new arma::mat[3];
		bias_		= new arma::mat[3];
		weights_	= new arma::mat[2];

		weights_[0] = (arma::randu(hiddenLayer, inputLayer) * 2.0f - 1.0f);	// input - hidden
		weights_[1] = (arma::randu(outputLayer, hiddenLayer) * 2.0f - 1.0f); // hidden - output

		bias_[0] = (arma::randu(inputLayer,	1) * 2.0f - 1.0f);
		bias_[1] = (arma::randu(hiddenLayer, 1) * 2.0f - 1.0f);
		bias_[2] = (arma::randu(outputLayer, 1) * 2.0f - 1.0f);
	}

	NeuralNetwork::NeuralNetwork(const std::vector<uint16_t>& layers, float learnRate)
		: learnRate_(learnRate)
		, layersCount_(layers.size())
	{
		assert(layers.size() > 2);

		layers_		= layers;
		neurons_	= new arma::mat[layersCount_];
		bias_		= new arma::mat[layersCount_];
		weights_	= new arma::mat[layersCount_ - 1];

		// filling random weights
		for (size_t i = 1; i < layersCount_; i++)
			weights_[i - 1] = (arma::randu(layers[i], layers[i - 1]) * 2.0f - 1.0f);

		// filling random biases
		for (size_t i = 0; i < layersCount_; i++)
			bias_[i] = (arma::randu(layers[i], 1) * 2.0f - 1.0f);
	}

	NeuralNetwork::NeuralNetwork(NeuralNetwork* neural_network)
		: learnRate_(neural_network->learnRate_)
		, layersCount_(neural_network->layersCount_)
	{
		layers_.assign(neural_network->layers_.begin(), neural_network->layers_.end());
		neurons_	= new arma::mat[layersCount_];
		bias_		= new arma::mat[layersCount_];
		weights_	= new arma::mat[layersCount_ - 1];

		// filling random weights
		for (size_t i = 1; i < layersCount_; i++)
			weights_[i - 1] = arma::zeros(layers_[i], layers_[i - 1]);

		// filling random biases
		for (size_t i = 0; i < layersCount_; i++)
			bias_[i] = arma::zeros(layers_[i]);
	}

	NeuralNetwork::~NeuralNetwork()
	{
		layers_.clear();
		delete[] bias_;
		delete[] neurons_;
		delete[] weights_;
	}

	void NeuralNetwork::activationFunction(double& x)
	{
		x = atan(x);
	}

	void NeuralNetwork::train(const arma::mat& inputs, const arma::mat& targets)
	{
		assert(inputs.n_rows == layers_[0] || targets.n_rows == layers_[layersCount_ - 1]);

		query(inputs);

		// error for output layer
		arma::mat error = targets - neurons_[layersCount_ - 1];
		error_coef = abs(arma::accu(error));

		for (size_t i = layersCount_ - 1; i > 0; i--)
		{
			// error for each layer
			if (i != layersCount_ - 1)
				error = weights_[i].t() * error;

			// (error % neurons_[i] % (1 - neurons_[i])) - delta
			weights_[i - 1] += learnRate_ * (error % neurons_[i] % (1 - neurons_[i])) * neurons_[i - 1].t();
			bias_[i] += learnRate_ * (error % neurons_[i] % (1 - neurons_[i]));
		}
	}

	void NeuralNetwork::query(const arma::mat& inputs)
	{
		assert(inputs.n_rows == layers_[0]);

		neurons_[0] = inputs;
		for (size_t i = 1; i < layersCount_; i++)
		{
			neurons_[i] = weights_[i - 1] * neurons_[i - 1] + bias_[i];
			neurons_[i].for_each([](arma::mat::elem_type& val) { activationFunction(val); });
		}
	}

	void NeuralNetwork::backQuery(const arma::mat& inputs)
	{
		assert(inputs.n_rows == layers_[layersCount_ - 1]);

		neurons_[layersCount_ - 1] = inputs;

		for (int32_t i = layersCount_ - 1; i > 0; i--)
		{
			neurons_[i - 1] = weights_[i - 1].t() * neurons_[i];
			neurons_[i - 1].for_each([](arma::mat::elem_type& val) { activationFunction(val); });
		}
	}

	const arma::mat& NeuralNetwork::result()
	{
		return neurons_[layersCount_ - 1];
	}

	const arma::Mat<double>& nn::NeuralNetwork::input()
	{
		return neurons_[0];
	}

	const arma::Mat<double>& nn::NeuralNetwork::getLayer(uint16_t inx)
	{
		return neurons_[inx];
	}

	uint16_t NeuralNetwork::getGeneration()
	{
		return generation_;
	}

	void NeuralNetwork::setGeneration(uint16_t generation)
	{
		generation_ = generation;
	}

	double NeuralNetwork::getFitness()
	{
		return fitness_;
	}

	void NeuralNetwork::setFitness(double fitness)
	{
		fitness_ = fitness;
	}

	void NeuralNetwork::addFitness(double value)
	{
		fitness_ += value;
	}

	void NeuralNetwork::save(const std::string& fileName)
	{

	}

	void NeuralNetwork::load(const std::string& fileName)
	{

	}

	void NeuralNetwork::reproduce(const NeuralNetwork* brain1, const NeuralNetwork* brain2)
	{
		assert(brain1->layersCount_ == brain2->layersCount_);
		assert(layersCount_ == brain1->layersCount_);

		for (int i = 0; i < layersCount_ - 1; ++i)
		{
			ga::GenericAlgorithm::selection(brain1->weights_[i], brain2->weights_[i], weights_[i], true);
		}

		for (int i = 0; i < layersCount_; ++i)
		{
			ga::GenericAlgorithm::selection(brain1->bias_[i], brain2->bias_[i], bias_[i], true);
		}
	}

	void NeuralNetwork::draw(int x, int y, float neuronScale, float spacingX, float spacingY)
	{
		float multX = (neuronScale * 2 + spacingX);
		float multY = (neuronScale * 2 + spacingY);
		//DrawFilledRect(x - m_Layers.size() * multX / 2, y - 250, m_Layers.size() * multX, 500, RGBColor(0.3f * 255, 0.4f * 255, 0.6f * 255));
		//DrawTextQL("Generation: " + std::to_string(m_Generation), 0, 20, 0, RGBColor{ 255, 255, 255 });

		// Draw Dendrites
		for (int i = 1; i < layersCount_; i++)
		{
			int prevLayerNeurCount = i > 0 ? layers_[i - 1] : 0;

			for (int j = 0; j < layers_[i]; j++)
			{
				int x1 = x + (i - layersCount_ / 2.0f + 0.5) * multX;
				int y1 = y + (j - neurons_[i].n_rows / 2.0f + 0.5) * multY;

				float value = neurons_[i][j];

				// Draw Dendrites
				for (int k = 0; k < layers_[i - 1]; k++)
				{
					float weight = weights_[i - 1].at(j, k);

					 int x2 = x + ((i - 1) - layersCount_ / 2.0f + 0.5) * multX;
					 int y2 = y + (k - prevLayerNeurCount / 2.0f + 0.5) * multY;

					 int cX = (x1 + x2) / 2;
					 int cY = (y1 + y2) / 2;
					 float dirX = (x2 - (x1));
					 float dirY = (y2 - (y1));
					 float angle = std::atan2(dirY, dirX);
					 //float angle = atan2(vector2.y, vector2.x) - atan2(vector1.y, vector1.x);;
					 if (angle < 0) angle += 2 * M_PI;
					 angle = angle * 180 / M_PI + 180;

					 DrawLineThinkT(
					 	x1, y1,
					 	x2, y2,
					 	fabs(weight) * 2.0f,
					 	RGBColor(255 * std::fmax(0, -weight), 0, 255 * fmax(0, weight), 0)
					 );
				}
			}
		}

		// Draw Neurons
		for (int i = 0; i < layersCount_; i++)
		{
			for (int j = 0; j < neurons_[i].n_rows; j++)
			{
				int x1 = x + (i - layersCount_ / 2.0f + 0.5) * multX;
				int y1 = y + (j - neurons_[i].n_rows / 2.0f + 0.5) * multY;
				float value = neurons_[i][j];

				// Draw Neurons
				DrawFilledCircle(x1, y1, neuronScale, RGBColor(0, 0, 0));
				DrawFilledCircle(x1, y1, neuronScale - 2, RGBColor(255 * std::abs(value), 255 * value, 255 * value));
			}

		}
	}
} // namespace nn


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

#include <iostream>
#include <boost/lexical_cast.hpp>
#include <rl/math/Constants.h>
#include <rl/math/Quaternion.h>
#include <rl/math/Rotation.h>
#include <rl/math/Vector.h>

int
main(int argc, char** argv)
{
	if (argc < 7)
	{
		std::cout << "Usage: rlEulerAnglesDemo AXIS0 AXIS1 AXIS2 DEG0 DEG1 DEG2" << std::endl;
		return EXIT_FAILURE;
	}

	rl::math::Matrix33 rotation = rl::math::Matrix33::Identity();

	for (std::size_t i = 0; i < 3; ++i)
	{
		rl::math::Real angle = boost::lexical_cast<rl::math::Real>(argv[i + 4]) * rl::math::constants::deg2rad;

		rl::math::Vector3 axis(
			0 == boost::lexical_cast<int>(argv[i + 1]) ? 1 : 0,
			1 == boost::lexical_cast<int>(argv[i + 1]) ? 1 : 0,
			2 == boost::lexical_cast<int>(argv[i + 1]) ? 1 : 0
		);
		std::cout << "angle" << i << ": " << angle << " rad - axis" << i << ": " << axis.transpose() << std::endl;

		rotation = rotation * rl::math::AngleAxis(angle, axis);
	}

	std::cout << std::endl;

	rl::math::Quaternion quaternion(rotation);
	std::cout << "quaternion.w: " << quaternion.w() << " - quaternion.vec: " << quaternion.vec().transpose() << std::endl;

	rl::math::AngleAxis angleAxis(rotation);
	std::cout << "angle: " << angleAxis.angle() << " rad - axis: " << angleAxis.axis().transpose() << std::endl;

	rl::math::Vector3 orientation = rotation.eulerAngles(2, 1, 0).reverse();
	std::cout << "x: " << orientation.x() * rl::math::constants::rad2deg << " deg - y: " << orientation.y() * rl::math::constants::rad2deg << " deg - z: " << orientation.z() * rl::math::constants::rad2deg << " deg" << std::endl;

	return EXIT_SUCCESS;
}


//=====================================================
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//=====================================================
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
//
#ifndef EIGEN2_INTERFACE_HH
#define EIGEN2_INTERFACE_HH
// #include <cblas.h>

#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <Eigen/QR>

#include "btl.hh"

using namespace Eigen;

template<class real, int SIZE=Dynamic>
class eigen2_interface
{

public :

  enum {IsFixedSize = (SIZE!=Dynamic)};

  typedef real real_type;

  typedef std::vector<real> stl_vector;
  typedef std::vector<stl_vector> stl_matrix;

  typedef Eigen::Matrix<real,SIZE,SIZE> gene_matrix;
  typedef Eigen::Matrix<real,SIZE,1> gene_vector;

  static inline std::string name( void )
  {
    #if defined(EIGEN_VECTORIZE_SSE)
    if (SIZE==Dynamic) return "eigen2"; else return "tiny_eigen2";
    #elif defined(EIGEN_VECTORIZE_ALTIVEC) || defined(EIGEN_VECTORIZE_VSX)
    if (SIZE==Dynamic) return "eigen2"; else return "tiny_eigen2";
    #else
    if (SIZE==Dynamic) return "eigen2_novec"; else return "tiny_eigen2_novec";
    #endif
  }

  static void free_matrix(gene_matrix & A, int N) {}

  static void free_vector(gene_vector & B) {}

  static BTL_DONT_INLINE void matrix_from_stl(gene_matrix & A, stl_matrix & A_stl){
    A.resize(A_stl[0].size(), A_stl.size());

    for (int j=0; j<A_stl.size() ; j++){
      for (int i=0; i<A_stl[j].size() ; i++){
        A.coeffRef(i,j) = A_stl[j][i];
      }
    }
  }

  static BTL_DONT_INLINE  void vector_from_stl(gene_vector & B, stl_vector & B_stl){
    B.resize(B_stl.size(),1);

    for (int i=0; i<B_stl.size() ; i++){
      B.coeffRef(i) = B_stl[i];
    }
  }

  static BTL_DONT_INLINE  void vector_to_stl(gene_vector & B, stl_vector & B_stl){
    for (int i=0; i<B_stl.size() ; i++){
      B_stl[i] = B.coeff(i);
    }
  }

  static BTL_DONT_INLINE  void matrix_to_stl(gene_matrix & A, stl_matrix & A_stl){
    int N=A_stl.size();

    for (int j=0;j<N;j++){
      A_stl[j].resize(N);
      for (int i=0;i<N;i++){
        A_stl[j][i] = A.coeff(i,j);
      }
    }
  }

  static inline void matrix_matrix_product(const gene_matrix & A, const gene_matrix & B, gene_matrix & X, int N){
    X = (A*B).lazy();
  }

  static inline void transposed_matrix_matrix_product(const gene_matrix & A, const gene_matrix & B, gene_matrix & X, int N){
    X = (A.transpose()*B.transpose()).lazy();
  }

  static inline void ata_product(const gene_matrix & A, gene_matrix & X, int N){
    X = (A.transpose()*A).lazy();
  }

  static inline void aat_product(const gene_matrix & A, gene_matrix & X, int N){
    X = (A*A.transpose()).lazy();
  }

  static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N){
    X = (A*B)/*.lazy()*/;
  }

  static inline void atv_product(gene_matrix & A, gene_vector & B, gene_vector & X, int N){
    X = (A.transpose()*B)/*.lazy()*/;
  }

  static inline void axpy(real coef, const gene_vector & X, gene_vector & Y, int N){
    Y += coef * X;
  }

  static inline void axpby(real a, const gene_vector & X, real b, gene_vector & Y, int N){
    Y = a*X + b*Y;
  }

  static inline void copy_matrix(const gene_matrix & source, gene_matrix & cible, int N){
    cible = source;
  }

  static inline void copy_vector(const gene_vector & source, gene_vector & cible, int N){
    cible = source;
  }

  static inline void trisolve_lower(const gene_matrix & L, const gene_vector& B, gene_vector& X, int N){
    X = L.template marked<LowerTriangular>().solveTriangular(B);
  }

  static inline void trisolve_lower_matrix(const gene_matrix & L, const gene_matrix& B, gene_matrix& X, int N){
    X = L.template marked<LowerTriangular>().solveTriangular(B);
  }

  static inline void cholesky(const gene_matrix & X, gene_matrix & C, int N){
    C = X.llt().matrixL();
//     C = X;
//     Cholesky<gene_matrix>::computeInPlace(C);
//     Cholesky<gene_matrix>::computeInPlaceBlock(C);
  }

  static inline void lu_decomp(const gene_matrix & X, gene_matrix & C, int N){
    C = X.lu().matrixLU();
//     C = X.inverse();
  }

  static inline void tridiagonalization(const gene_matrix & X, gene_matrix & C, int N){
    C = Tridiagonalization<gene_matrix>(X).packedMatrix();
  }

  static inline void hessenberg(const gene_matrix & X, gene_matrix & C, int N){
    C = HessenbergDecomposition<gene_matrix>(X).packedMatrix();
  }

};

#endif


#ifndef INCLUDE_SWIFT_VIO_TWO_VIEW_GEOMETRY_HPP_
#define INCLUDE_SWIFT_VIO_TWO_VIEW_GEOMETRY_HPP_
#include <okvis/kinematics/Transformation.hpp>
#include <Eigen/Core>

namespace okvis {
class TwoViewGeometry {
 public:
  static float computeErrorEssentialMat(okvis::kinematics::Transformation T_ji,
                                        Eigen::Vector3d bearing_i,
                                        Eigen::Vector3d bearing_j, double fi,
                                        double fj, double sigmai = 1.0,
                                        double sigmaj = 1.0);

  /**
   * @brief deviationFromEpipolarLine compute deviation in pixels from the
   * epipolar line. see
   * https://github.com/opencv/opencv/blob/master/modules/calib3d/src/fundam.cpp
   * @param E_ji t_ji X R_ji. Epipolar constraint is p_j' * t_ji X R_ji * p_i = 0.
   * @param bearing_i [x, y, 1] undistorted image coordinate at z=1 for point in
   * image i
   * @param bearing_j [x, y, 1] undistorted image coordinate at z=1 for point in
   * image j
   * @param focal_length nominal focal length to convert the epipolar line error
   * into error of pixel unit.
   * @return squared distance to epipolar line. Distance has a unit of pixels.
   */
  static float computeErrorEssentialMat(Eigen::Matrix3d E_ji,
                                        Eigen::Vector3d bearing_i,
                                        Eigen::Vector3d bearing_j, double fi,
                                        double fj, double sigmai = 1.0,
                                        double sigmaj = 1.0);
};
}  // namespace okvis

#endif  // INCLUDE_SWIFT_VIO_TWO_VIEW_GEOMETRY_HPP_


//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Unit tests for the transformation of points (with and without
/// covariance)
///
/// \author Kirk MacTavish
//////////////////////////////////////////////////////////////////////////////////////////////

#include <gtest/gtest.h>

#include <math.h>
#include <iomanip>
#include <ios>
#include <iostream>
#include <typeinfo>

#include <Eigen/Dense>
#include <lgmath.hpp>
#include <lgmath/CommonMath.hpp>

/////////////////////////////////////////////////////////////////////////////////////////////
///
/// UNIT TESTS OF POINTS WITH AND WITHOUT COVARIANCE
///
/////////////////////////////////////////////////////////////////////////////////////////////

using namespace lgmath;

/////////////////////////////////////////////////////////////////////////////////////////////
// HELPER CONSTANTS
/////////////////////////////////////////////////////////////////////////////////////////////

static const so3::RotationMatrix C_z180 =
    so3::Rotation(so3::AxisAngle(0., 0., constants::PI)).matrix();
static const so3::RotationMatrix C_z90 =
    so3::Rotation(so3::AxisAngle(0., 0., constants::PI_DIV_TWO)).matrix();

/////////////////////////////////////////////////////////////////////////////////////////////
// MAIN TESTS
/////////////////////////////////////////////////////////////////////////////////////////////

TEST(Points, PointCovarianceTransform) {
  r3::CovarianceMatrix cov_a;
  cov_a.setZero();
  cov_a.diagonal() << 1., 2., 3.;
  r3::HPoint p_a = (r3::Point() << 2., 3., 4.).finished().homogeneous();

  {
    std::cout << "Given a 180 degree transform, "
              << "when we transform the covariance, "
              << "then the covariance should be unchanged." << std::endl;
    se3::Transformation T_ba(C_z180, se3::TranslationVector::Zero());
    auto cov_b =
        r3::transformCovariance<r3::COVARIANCE_NOT_REQUIRED>(T_ba, cov_a);
    std::cout << cov_a << "\n==\n" << cov_b << std::endl;
    EXPECT_TRUE(cov_a.isApprox(cov_b));
  }
  {
    std::cout << "Given a 90 degree transform, "
              << "when we transform the covariance, "
              << "then the covariance should should have x and y swapped."
              << std::endl;
    se3::Transformation T_ba(C_z90, se3::TranslationVector::Zero());
    auto cov_b =
        r3::transformCovariance<r3::COVARIANCE_NOT_REQUIRED>(T_ba, cov_a);
    cov_a.row(0).swap(cov_a.row(1));
    cov_a.col(0).swap(cov_a.col(1));
    std::cout << cov_a << "\n==\n" << cov_b << std::endl;
    EXPECT_TRUE(cov_a.isApprox(cov_b));
  }
  {
    std::cout << "Given uncertain translation, "
              << "when we translate the point and covariance, "
              << "then the covariance should be additive." << std::endl;
    se3::LieAlgebraCovariance S_ba = se3::LieAlgebraCovariance::Zero();
    S_ba.topLeftCorner<3, 3>() = cov_a;
    se3::TransformationWithCovariance T_ba(
        so3::RotationMatrix::Identity(), se3::TranslationVector::Zero(), S_ba);
    auto p_b = T_ba * p_a;
    r3::CovarianceMatrix cov_b;
    EXPECT_NO_THROW(cov_b = r3::transformCovariance(T_ba, cov_a, p_b));
    r3::CovarianceMatrix cov_b_expect = cov_a * 2.;
    std::cout << cov_b << "\n==\n" << cov_b_expect << std::endl;
    EXPECT_TRUE(cov_b.isApprox(cov_b_expect));
  }
  {
    std::cout << "Given uninitialized uncertain transform, "
              << "when we transform the point without ignoring the 'covariance "
                 "set' flag, "
              << "then it should throw." << std::endl;
    se3::TransformationWithCovariance T_ba;
    r3::CovarianceMatrix cov_b;
    auto p_b = T_ba * p_a;
    EXPECT_THROW(cov_b = r3::transformCovariance(T_ba, cov_a, p_b),
                 std::runtime_error);

    std::cout
        << "when we transform the point but ignore the 'covariance set' flag, "
        << "then it shouldn't throw, and the covariance should be unchanged, "
        << std::endl;
    EXPECT_NO_THROW(cov_b =
                        r3::transformCovariance<r3::COVARIANCE_NOT_REQUIRED>(
                            T_ba, cov_a, p_b));
    std::cout << cov_a << "\n==\n" << cov_b << std::endl;
    EXPECT_TRUE(cov_a.isApprox(cov_b));
  }
  {
    std::cout
        << "Given uncertain rotation, "
        << "when we transform the point and covariance, "
        << "then the covariance should be unchanged in Z, and larger in X and Y"
        << std::endl;
    se3::LieAlgebraCovariance S_ba;
    S_ba.setZero();
    S_ba(5, 5) = 1;
    se3::TransformationWithCovariance T_ba(
        se3::TransformationMatrix(se3::TransformationMatrix::Identity()), S_ba);
    auto p_b = T_ba * p_a;
    auto cov_b = r3::transformCovariance(T_ba, cov_a, p_b);
    EXPECT_TRUE(cov_b(0, 0) > cov_a(0, 0) + 1e-3);
    EXPECT_TRUE(cov_b(1, 1) > cov_a(1, 1) + 1e-3);
    EXPECT_TRUE(cov_b(2, 2) == cov_a(2, 2));  // Approximately
  }
}

TEST(Points, PointTransform) {
  std::cout << "Given a point, "
            << "when we transform it, "
            << "then it should be in the right spot." << std::endl;

  r3::HPoint x;
  x << 1., 2., 3., 1.;
  se3::Transformation T(
      (se3::LieAlgebra() << 1., 0, 0, constants::PI, 0, 0).finished());
  r3::HPoint x_tf;
  x_tf << 2., -2., -3., 1.;
  std::cout << T * x << "\n==\n" << x_tf << std::endl;
  EXPECT_TRUE((T * x).isApprox(x_tf));
}

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


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

#include <cmath>
#include <cfloat>
#include <cstdint>
#include <limits>
#include <type_traits>
#include <boost/math/ccmath/fdim.hpp>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/math/ccmath/isinf.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

template <typename T>
constexpr void test()
{
    static_assert(boost::math::ccmath::isnan(boost::math::ccmath::fdim(std::numeric_limits<T>::quiet_NaN(), T(1))), "If x is NaN, NaN is returned");
    static_assert(boost::math::ccmath::isnan(boost::math::ccmath::fdim(T(1), std::numeric_limits<T>::quiet_NaN())), "If y is NaN, NaN is returned");

    static_assert(boost::math::ccmath::fdim(T(4), T(1)) == T(3));
    static_assert(boost::math::ccmath::fdim(T(1), T(4)) == T(0));
    static_assert(boost::math::ccmath::fdim(T(4), T(-1)) == T(5));
    static_assert(boost::math::ccmath::fdim(T(1), T(-4)) == T(5));

    static_assert(boost::math::ccmath::isinf(boost::math::ccmath::fdim(std::numeric_limits<T>::infinity(), T(-1))));
}

#if !defined(BOOST_MATH_NO_CONSTEXPR_DETECTION) && !defined(BOOST_MATH_USING_BUILTIN_CONSTANT_P)
int main()
{
    test<float>();
    test<double>();

    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test<long double>();
    #endif

    #ifdef BOOST_HAS_FLOAT128
    test<boost::multiprecision::float128>();
    #endif

    return 0;
}
#else
int main()
{
    return 0;
}
#endif


#pragma once

#include "RelaxedUT.h"
#include <Eigen/QR>

using namespace RelaxedUnscentedTransformation;

void RelaxedUnscentedTransformation::genQ(int n, const Eigen::VectorXi& inl, const MixedNonlinearityList& mix,
	Eigen::SparseMatrix<double>& Q,
	Eigen::SparseMatrix<double>& Q1) {
	// Construct M matrix from the weights keeping it orthogonal to inl
	typedef Eigen::MatrixXd Matrix;
	Matrix M = Matrix::Zero(mix.size() + inl.size(), n);
	for (int n = 0; n < inl.size(); n++)
		M(n, inl(n)) = 1;
	for (int n = 0; n < mix.size(); n++)
		for (int i0 = 0; i0 < mix[n].i.size(); i0++) {
			bool flag = false;
			for (int i = 0; i < inl.size(); i++)
				if (mix[n].i[i0] == inl(i))
					flag = true;
			if (!flag)
				M(n + inl.size(), mix[n].i[i0]) = mix[n].M(i0);
		}
	// RQ factorization
	Eigen::HouseholderQR<Matrix> solver(M.transpose());
	Eigen::MatrixXd Qdense = solver.householderQ().transpose();
	double rel_treshold = 1e-6;
	// Compute rank
	int rank = n;
	{
		Eigen::MatrixXd to_compute_rank = (M * Qdense.transpose()).cwiseAbs();
		double treshold = to_compute_rank.maxCoeff() * rel_treshold;
		for (int i = 0; i < to_compute_rank.cols(); i++)
			if (to_compute_rank.col(i).maxCoeff() < treshold) {
				rank = i;
				break;
			}
	}
	// make it sparse
	for (int i = 0; i < Qdense.rows(); i++)
		for (int j = 0; j < Qdense.cols(); j++)
			if (abs(Qdense(i, j)) < 1e-8)
				Qdense(i, j) = 0;
	// save it sparse
	Q = Qdense.sparseView();
	Q1 = Q.block(0, 0, rank, n);
}


#ifndef LINEAR_LEAST_SQUARE_MODEL_HPP
#define LINEAR_LEAST_SQUARE_MODEL_HPP

#include <iostream>

#include <math.h>
#include <Eigen/QR>
#include <Eigen/Dense>

#include "basic_types.hpp"
#include "regression2d.hpp"

namespace GRANSAC
{

// model paramter number = 3
template<int t_param_num>
class LinearLeastSquaresModel: public AbstractModel<t_param_num>
{
protected:
	//[1  x0  x0^2] [m_a0] = [y0]
    //[1  x1  x1^2] [m_a1]   [y1]
    //[1  x2  x2^2] [m_a2]   [y2]
    //...                    ...
    //[1  xn  xn^2]          [yn]
    // std::vector<VPFloat> m_a;

	// build a lookup table to calculate point to curve distance
	// e.g. target fitting is points in a 400x400 image
	// then grid of lookup table is 40x40 cell, each cell is 10x10 pixel
	int m_grid_size_x; // cell size of grid, e.g. m_grid_size = 10 -> each cell is 10x10 pixel
	int m_grid_size_y;
	std::vector<std::vector<float> > m_occupied_list;

	virtual float computeDistanceMeasure(std::shared_ptr<AbstractParameter> input_data) override
	{
		auto ext_point2D = std::dynamic_pointer_cast<Point2D>(input_data);
		if (ext_point2D == nullptr)
			throw std::runtime_error("PolynomialModel::ComputeDistanceMeasure() - Passed parameter are not of type Point2D.");

		// build a lookup table for distance calculation
		float min_dist = std::numeric_limits<float>::max();
        for (auto each : m_occupied_list){
            float dist = fabs(ext_point2D->m_point2D[0]/m_grid_size_x-each[0]) + fabs(ext_point2D->m_point2D[1]/m_grid_size_y-each[1]); // p-1 distance, 10 is grid size
            if (min_dist > dist)
                min_dist = dist;
        }
		return min_dist; // distance in grid, 10 times smaller than actual distance
	};

public:
	LinearLeastSquaresModel(const std::vector<std::shared_ptr<AbstractParameter>> &input_data,
	               const std::map<std::string, float>& additional_params)
	{
		initialize(input_data, additional_params);
	};

	virtual void initialize(const std::vector<std::shared_ptr<AbstractParameter>> &input_data,
	                        const std::map<std::string, float>& additional_params) override
	{
		int img_width = (additional_params.count("img_width") == 1) ? additional_params.at("img_width") : 400;
		int img_height = (additional_params.count("img_height") == 1) ? additional_params.at("img_height") : 400;
		int grid_num_x = (additional_params.count("grid_num_x") == 1) ? additional_params.at("grid_num_x") : 40;
		int grid_num_y = (additional_params.count("grid_num_y") == 1) ? additional_params.at("grid_num_y") : 40;

		m_grid_size_x = img_width / grid_num_x; // e.g. 10
		m_grid_size_y = img_height / grid_num_y;

		// alway calculate curve with three points, since y = ax^2+bx+c
		if (input_data.size() < t_param_num)
			throw std::runtime_error("PolynomialModel - Number of input parameters does not match minimum number required for this model.");

		AbstractModel<t_param_num>::m_model_def_parameters = input_data;

		// compute deterministic curve parameters with 3 points
		std::vector<float> x_values, y_values, coeff;
		for (int i=0; i< input_data.size(); i++){
			auto point = std::dynamic_pointer_cast<Point2D>(input_data[i]);
			if (point == nullptr)
				throw std::runtime_error("QuadraticModel - InputParams type mismatch. It is not a Point2D.");

			x_values.push_back(point->m_point2D[0]);
			y_values.push_back(point->m_point2D[1]);
		}
		Regression2D::calculateLLS(x_values, y_values, coeff, t_param_num-1);

		std::vector<float> coeff_f(coeff.begin(), coeff.end());
		AbstractModel<t_param_num>::m_model_coeffs = coeff_f;

		m_occupied_list.reserve(grid_num_x*grid_num_y/2);
		//e.g. img_size=400x400, grid_size=40x40, cell_size=10x10
		for (int i=0; i<grid_num_x; i++){ // e.g. i=0; i<40

			float x_0 = float(i*m_grid_size_x); // e.g. i*10
			float x_1 = float((i+1)*m_grid_size_x);

            float y_0 = 0;
			float y_1 = 0;
            for (int j = 0; j < t_param_num; j++){
                y_0 += coeff[j]*std::pow(x_0, float(j));
                y_1 += coeff[j]*std::pow(x_1, float(j));
            }

			bool condi_1 = y_0 <= img_height && y_0 >=0; // e.g. condi_1 = y_0<=400 && y_0>=0
			bool condi_2 = y_1 <= img_height && y_1 >=0;
			if (condi_1 || condi_2){
				y_0 = (y_0 > img_height) ? img_height : y_0;
				y_0 = (y_0 < 0) ? 0 : y_0;
				y_1 = (y_1 > img_height) ? img_height : y_1;
				y_1 = (y_1 < 0) ? 0 : y_1;

				int y_0_idx = floor(y_0 / m_grid_size_y); // e.g. y_0_idx = floor(y_0 / 10)
				int y_1_idx = floor(y_1 / m_grid_size_y);

				if (y_0_idx < y_1_idx){
					for (int j=y_0_idx; j<y_1_idx; j++){
						std::vector<float> xy_pos{float(i), float(j)};
						m_occupied_list.push_back(xy_pos);
					}
				} else if (y_0_idx > y_1_idx){
					for (int j=y_1_idx; j<y_0_idx; j++){
						std::vector<float> xy_pos{float(i), float(j)};
						m_occupied_list.push_back(xy_pos);
					}
				} else{
					std::vector<float> xy_pos{float(i), float(y_0_idx)};
					m_occupied_list.push_back(xy_pos);
				}
			}
		}
		// //if curve go through cell, fill cell = 1, otherwise = 0

	};

	virtual std::pair<float, std::vector<std::shared_ptr<AbstractParameter> > > evaluate(const std::vector<std::shared_ptr<AbstractParameter>>& evaluate_data, float threshold)
	{
		std::vector<std::shared_ptr<AbstractParameter>> inliers;
		int n_total_data = evaluate_data.size();
		int n_inliers = 0;

		for (auto& each : evaluate_data)
		{
			if (computeDistanceMeasure(each) < threshold)
			{
				inliers.push_back(each);
				n_inliers++;
			}
		}
		float inlier_fraction = float(n_inliers) / float(n_total_data); // This is the inlier fraction
		return std::make_pair(inlier_fraction, inliers);
	};

};

}

#endif /* LINEAR_LEAST_SQUARE_MODEL_HPP */


#pragma once

#include <deal.II/base/tensor.h>
#include <complex>
#include <functional>
#include "aux/tensor_helpers.hpp"

namespace boltzmann {

template <int dim>
class QTrapz;
namespace _ {

template <class T>
struct NumberTraits;

template <int dim, int rank, class NUMBER>
struct NumberTraits<dealii::Tensor<dim, rank, NUMBER> >
{
  typedef NUMBER numeric_t;
};

template <>
struct NumberTraits<double>
{
  typedef double numeric_t;
};

template <>
struct NumberTraits<std::complex<double> >
{
  typedef std::complex<double> numeric_t;
};

}  // end namespace

template <>
class QTrapz<1>
{
 public:
  QTrapz(int npts, double a, double b);

  template <class NUMBER>
  NUMBER compute(const std::function<NUMBER(double)>& f) const;

  const std::vector<double>& get_weights() { return weights_; }

  const std::vector<double>& get_points() { return points_; }

 private:
  const double a;
  const double b;
  const int npts;
  std::vector<double> points_;
  std::vector<double> weights_;
};

/**
 * trapezoidal quadrature rule, works also with tensor valued functions
 *
 *
 * @return
 */
template <class NUMBER>
NUMBER
qtrapz1d(const std::function<NUMBER(double)>& f, double a, double b, int npts)
{
  typedef typename _::NumberTraits<NUMBER>::numeric_t numeric_t;
  const double h = (b - a) / double(npts - 1);

  NUMBER sum = 0;
  for (int i = 0; i < npts - 1; ++i) {
    const double x1 = a + h * (i);
    const double x2 = a + h * (i + 1);
    sum += f(x1) + f(x2);
  }
  sum *= numeric_t(0.5 * h);
  return sum;
}

QTrapz<1>::QTrapz(int npts, double a, double b)
    : a(a)
    , b(b)
    , npts(npts)
    , points_(npts)
    , weights_(npts)
{
  double h = (b - a) / (npts - 1);
  for (int i = 0; i < npts; ++i) {
    if (i == 0 || i == npts - 1)
      weights_[i] = 0.5 * h;
    else
      weights_[i] = h;
    points_[i] = a + i * h;
  }
}

}  // end namespace boltzmann


/*
   bernmm-test.cpp:  test module

   Copyright (C) 2008, 2009, David Harvey

   This file is part of the bernmm package (version 1.1).

   bernmm is released under a BSD-style license. See the README file in
   the source distribution for details.
*/

#include <iostream>
#include <NTL/ZZ.h>
#include <gmp.h>
#include "bern_modp_util.h"
#include "bern_modp.h"
#include "bern_rat.h"

NTL_CLIENT;

using namespace bernmm;
using namespace std;

/*
   Computes B_0, B_1, ..., B_{n-1} using naive algorithm, writes them to res.
*/
void bern_naive(mpq_t* res, long n)
{
   mpq_t t, u;
   mpq_init(t);
   mpq_init(u);

   // compute res[j] = B_j / j! for 0 <= j < n
   if (n > 0)
      mpq_set_si(res[0], 1, 1);

   for (long j = 1; j < n; j++)
   {
      mpq_set_si(res[j], 0, 1);
      mpq_set_ui(t, 1, 1);
      for (long k = 0; k < j; k++)
      {
         mpz_mul_ui(mpq_denref(t), mpq_denref(t), k + 2);
         mpq_mul(u, res[j - 1 - k], t);
         mpq_sub(res[j], res[j], u);
      }
   }

   // multiply through by j! for 0 <= j < n
   mpq_set_ui(t, 1, 1);
   for (long j = 2; j < n; j++)
   {
      mpz_mul_ui(mpq_numref(t), mpq_numref(t), j);
      mpq_mul(res[j], res[j], t);
   }

   mpq_clear(u);
   mpq_clear(t);
}

/*
   Tests _bern_modp_powg() for a given p and k by comparing against the
   rational number B_k (must be supplied in b).

   Returns 1 on success.
*/
int testcase__bern_modp_powg(long p, long k, mpq_t b)
{
   mulmod_t pinv = PrepMulMod(p);

   // compute B_k mod p using _bern_modp_powg()
   long x = _bern_modp_powg(p, pinv, k);
   x = MulMod(x, k, p, pinv);

   // compute B_k mod p from rational B_k
   long y = mpz_fdiv_ui(mpq_numref(b), p);
   long z = mpz_fdiv_ui(mpq_denref(b), p);
   return y == MulMod(z, x, p, pinv);
}

/*
   Tests _bern_modp_powg() by comparing against naive computation of B_k
   (as a rational) for a range of small p and k.

   Returns 1 on success.
*/
int test__bern_modp_powg()
{
   int success = 1;

   const long MAX = 300;
   mpq_t bern[MAX];

   // compute B_k's as rational numbers using naive algorithm
   for (long i = 0; i < MAX; i++)
      mpq_init(bern[i]);
   bern_naive(bern, MAX);

   // try a range of k's
   for (long k = 2; k < MAX && success; k += 2)
   {
      // try a range of small p's
      for (long p = k + 3; p < 2*MAX && success; p += 2)
      {
         if (!ProbPrime(p))
            continue;
         success = success && testcase__bern_modp_powg(p, k, bern[k]);
      }

      // try a single larger p
      success = success && testcase__bern_modp_powg(1000003, k, bern[k]);
   }

   // if we're on a 32-bit machine, try a single example with p right near
   // NTL's boundary (this is infeasible on a 64-bit machine)
   if (NTL_SP_NBITS <= 32)
   {
      long p = NTL_SP_BOUND - 1;
      while (!ProbPrime(p))
         p--;

      long k = (MAX/2)*2 - 2;
      success = success && testcase__bern_modp_powg(p, k, bern[k]);
   }

   for (long i = 0; i < MAX; i++)
      mpq_clear(bern[i]);

   return success;
}

/*
   Tests _bern_modp_pow2() for a given p and k by comparing against result
   from _bern_modp_powg().

   Returns 1 on success.

   If 2^k = 1 mod p, then _bern_modp_pow2() won't work, so it just returns 1.
*/
int testcase__bern_modp_pow2(long p, long k)
{
   mulmod_t pinv = PrepMulMod(p);

   if (PowerMod(2, k, p, pinv) == 1)
      return 1;

   long x = _bern_modp_powg(p, pinv, k);
   long y = _bern_modp_pow2(p, pinv, k);

   return x == y;
}

/*
   Tests _bern_modp_pow2() by comparing against _bern_modp_powg() for
   a range of p and k.

   Returns 1 on success.
*/
int test__bern_modp_pow2()
{
   int success = 1;

   // exhaustive comparison over some small p and k
   for (long p = 5; p < 2000 && success; p += 2)
   {
      if (!ProbPrime(p))
         continue;

      for (long k = 2; k <= p - 3 && success; k += 2)
         success = success && testcase__bern_modp_pow2(p, k);
   }

   // a few larger values of p
   for (long p = 1000000; p < 1030000; p++)
   {
      if (!ProbPrime(p))
         continue;

      long k = 2 * (rand() % ((p-3)/2)) + 2;
      success = success && testcase__bern_modp_pow2(p, k);
   }

   // if we're on a 32-bit machine, try a single example with p right near
   // NTL's boundary (this is infeasible on a 64-bit machine)
   if (NTL_SP_NBITS <= 32)
   {
      long p = NTL_SP_BOUND - 1;
      while (!ProbPrime(p))
         p--;
      success = success & testcase__bern_modp_pow2(p, 10);
   }

   // try a few just below the REDC barrier
   if (ULONG_BITS == 32)
   {
      long boundary = 1L << (ULONG_BITS/2 - 1);
      for (long p = boundary - 1000; p < boundary && success; p++)
      {
         if (ProbPrime(p))
         {
            for (long trial = 0; trial < 1000 && success; trial++)
            {
               long k = 2 * (rand() % ((p-3)/2)) + 2;
               success = success && testcase__bern_modp_pow2(p, k);
            }
         }
      }
   }
   else
   {
      // on a 64-bit machine, only try one, since these are huge!
      long p = 1L << (ULONG_BITS/2 - 1);
      while (!ProbPrime(p))
         p--;
      success = success && testcase__bern_modp_pow2(p, 10);
   }

   return success;
}

/*
   Tests bern_rat() by comparing against the naive algorithm for several small
   k, and testing against bern_modp() for a couple of larger k.

   Returns 1 on success.
*/
int test_bern_rat()
{
   int success = 1;

   const long MAX = 300;
   mpq_t bern[MAX];

   // compute B_k's as rational numbers using naive algorithm
   for (long i = 0; i < MAX; i++)
      mpq_init(bern[i]);
   bern_naive(bern, MAX);

   mpq_t x;
   mpq_init(x);

   // exhaustive test for small k
   for (long k = 0; k < MAX && success; k++)
   {
      bern_rat(x, k, 4);    // try with 4 threads just for fun
      success = success && mpq_equal(x, bern[k]);
   }

   // try a few larger k
   for (long i = 0; i < 50 && success; i++)
   {
      long k = ((random() % 20000) / 2) * 2;
      bern_rat(x, k, 4);

      // compare with modular information
      long p = 1000003;
      long num = mpz_fdiv_ui(mpq_numref(x), p);
      long den = mpz_fdiv_ui(mpq_denref(x), p);
      success = success && (MulMod(bern_modp(p, k), den, p) == num);
   }

   mpq_clear(x);
   for (long i = 0; i < MAX; i++)
      mpq_clear(bern[i]);

   return success;
}

void report(int success)
{
   if (success)
      cout << "ok" << endl;
   else
   {
      cout << "failed!" << endl;
      abort();
   }
}

int main(int argc, char* argv[])
{
   if (argc == 1)
   {
      cout << "bernmm test module" << endl;
      cout << endl;
      cout << "   bernmm-test --test" << endl;
      cout << "        runs test suite" << endl;
      cout << "   bernmm-test --rational <k> <threads>" << endl;
      cout << "        computes B_k with <threads> threads" << endl;
      cout << "   bernmm-test --modular <p> <k>" << endl;
      cout << "        computes B_k mod p" << endl;
      return 0;
   }

   if (!strcmp(argv[1], "--test"))
   {
      cout << "testing _bern_modp_powg()... " << flush;
      report(test__bern_modp_powg());

      cout << "testing _bern_modp_pow2()... " << flush;
      report(test__bern_modp_pow2());

      cout << "testing bern_rat()... " << flush;
      report(test_bern_rat());
   }
   else if (!strcmp(argv[1], "--rational"))
   {
      if (argc <= 3)
      {
         cout << "not enough arguments" << endl;
         return 0;
      }
      long k = atol(argv[2]);
      long threads = atol(argv[3]);
      mpq_t r;
      mpq_init(r);
      bern_rat(r, k, threads);
      gmp_printf("%Zd/%Zd\n", mpq_numref(r), mpq_denref(r));
      mpq_clear(r);
   }
   else if (!strcmp(argv[1], "--modular"))
   {
      if (argc <= 3)
      {
         cout << "not enough arguments" << endl;
         return 0;
      }
      long p = atol(argv[2]);
      long k = atol(argv[3]);
      cout << bern_modp(p, k) << endl;
   }
   else
   {
      cout << "unknown command" << endl;
   }

   return 0;
}

// end of file ================================================================


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/sqrt.hpp
 *
 * \brief Compute the square root of element of a vector or matrix expression.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_SQRT_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_SQRT_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/expression/matrix_unary_functor.hpp>
#include <boost/numeric/ublasx/expression/vector_unary_functor.hpp>
#include <cmath>
//#include <complex>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename VectorExprT>
struct vector_sqrt_functor_traits
{
    typedef VectorExprT input_expression_type;
    typedef typename vector_traits<input_expression_type>::value_type signature_argument_type;
    typedef signature_argument_type signature_result_type;
    typedef vector_unary_functor_traits<
                input_expression_type,
                signature_result_type (signature_argument_type)
            > unary_functor_expression_type;
    typedef typename unary_functor_expression_type::result_type result_type;
    typedef typename unary_functor_expression_type::expression_type expression_type;
};

template <typename MatrixExprT>
struct matrix_sqrt_functor_traits
{
    typedef MatrixExprT input_expression_type;
    typedef typename matrix_traits<input_expression_type>::value_type signature_argument_type;
    typedef signature_argument_type signature_result_type;
    typedef matrix_unary_functor_traits<
                input_expression_type,
                signature_result_type (signature_argument_type)
            > unary_functor_expression_type;
    typedef typename unary_functor_expression_type::result_type result_type;
    typedef typename unary_functor_expression_type::expression_type expression_type;
};

// Note: this wrapper is needed since we have both templated and non-templated
//       overloaded versions of the 'sqrt' function.
//       So whithout this wrapper, the the compiler is not able to infer what
//       overloaded function to use.
template <typename T>
BOOST_UBLAS_INLINE
T sqrt_impl(T const& x)
{
    return ::std::sqrt(x);
}

} // Namespace detail

/**
 * \brief Applies the \c std::sqrt function to a given vector expression.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param ve The input vector expression.
 * \return A vector expression representing the application of \c std::sqrt to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename VectorExprT>
BOOST_UBLAS_INLINE
typename detail::vector_sqrt_functor_traits<VectorExprT>::result_type sqrt(vector_expression<VectorExprT> const& ve)
{
    typedef typename detail::vector_sqrt_functor_traits<VectorExprT>::expression_type expression_type;
    typedef typename detail::vector_sqrt_functor_traits<VectorExprT>::signature_argument_type signature_argument_type;

    return expression_type(ve(), detail::sqrt_impl<signature_argument_type>);
//  return expression_type(ve(), ::std::sqrt<signature_argument_type>);
//  return expression_type(ve(), ::std::sqrt<signature_result_type>);
//  typedef signature_result_type(*fun_ptr_type)(signature_argument_type);
//  fun_ptr_type ptr_sqrt_fun(&::std::sqrt);
//  return expression_type(ve(), ptr_sqrt_fun);
//  return expression_type(ve(), (signature_result_type (*)(signature_argument_type))&::std::sqrt);
}

/**
 * \brief Applies the \c std::sqrt function to a given matrix expression.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \return A matrix expression representing the application of \c std::sqrt to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename detail::matrix_sqrt_functor_traits<MatrixExprT>::result_type sqrt(matrix_expression<MatrixExprT> const& me)
{
    typedef typename detail::matrix_sqrt_functor_traits<MatrixExprT>::expression_type expression_type;
    typedef typename detail::matrix_sqrt_functor_traits<MatrixExprT>::signature_argument_type signature_argument_type;

    return expression_type(me(), detail::sqrt_impl<signature_argument_type>);
//  return expression_type(me(), ::std::sqrt<signature_argument_type>);
//  return expression_type(me(), ::std::sqrt<signature_result_type>);
//  typedef signature_result_type(*fun_ptr_type)(signature_argument_type);
//  fun_ptr_type ptr_sqrt_fun(&::std::sqrt);
//  return expression_type(me(), ptr_sqrt_fun);
//  return expression_type(me(), (signature_result_type (*)(signature_argument_type))&::std::sqrt);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_SQRT_HPP


/* Boost example/newton-raphson.cpp
 * Newton iteration for intervals (partial: 0/0 is missing)
 *
 * Copyright Guillaume Melquiond 2003
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation.
 *
 * None of the above authors make any representation about the
 * suitability of this software for any purpose. It is provided "as
 * is" without express or implied warranty.
 *
 * $Id: newton-raphson.cpp,v 1.2 2003/02/05 17:34:35 gmelquio Exp $
 */

#include <boost/numeric/interval.hpp>
#include <boost/numeric/interval/io.hpp>
#include <vector>
#include <algorithm>
#include <utility>
#include <iostream>
#include <iomanip>

template <class I> I f(const I& x)
{ return x * (x - 1.) * (x - 2.) * (x - 3.) * (x - 4.); }
template <class I> I f_diff(const I& x)
{ return (((5. * x - 40.) * x + 105.) * x - 100.) * x + 24.; }

static const double max_width = 1e-10;
static const double alpha = 0.75;

using namespace boost;
using namespace numeric;
using namespace interval_lib;

// First method: no empty intervals

typedef interval<double> I1_aux;
typedef unprotect<I1_aux>::type I1;

std::vector<I1> newton_raphson(const I1& xs) {
  std::vector<I1> l, res;
  I1 vf, vd, x, x1, x2;
  l.push_back(xs);
  while (!l.empty()) {
    x = l.back();
    l.pop_back();
    bool x2_used;
    double xx = median(x);
    vf = f(xx);
    vd = f_diff(x);
    if (in_zero(vf) && in_zero(vd)) {
      x1 = I1::whole();
      x2_used = false;
    } else {
      x1 = xx - division_part1(vf, vd, x2_used);
      if (x2_used) x2 = xx - division_part2(vf, vd);
    }
    if (overlap(x1, x)) x1 = intersect(x, x1);
    else if (x2_used) { x1 = x2; x2_used = false; }
    else continue;
    if (x2_used)
      if (overlap(x2, x)) x2 = intersect(x, x2);
      else x2_used = false;
    if (x2_used && width(x2) > width(x1)) std::swap(x1, x2);
    if (!in_zero(f(x1)))
      if (x2_used) { x1 = x2; x2_used = false; }
      else continue;
    if (width(x1) < max_width) res.push_back(x1);
    else if (width(x1) > alpha * width(x)) {
      std::pair<I1, I1> p = bisect(x);
      if (in_zero(f(p.first))) l.push_back(p.first);
      x2 = p.second;
      x2_used = true;
    } else l.push_back(x1);
    if (x2_used && in_zero(f(x2)))
      if (width(x2) < max_width) res.push_back(x1);
      else l.push_back(x2);
  }
  return res;
}

// Second method: with empty intervals

typedef change_checking<I1_aux, checking_no_nan<double> >::type I2_aux;
typedef unprotect<I2_aux>::type I2;

std::vector<I2> newton_raphson(const I2& xs) {
  std::vector<I2> l, res;
  I2 vf, vd, x, x1, x2;
  l.push_back(xs);
  while (!l.empty()) {
    x = l.back();
    l.pop_back();
    double xx = median(x);
    vf = f(xx);
    vd = f_diff(x);
    if (in_zero(vf) && in_zero(vd)) {
      x1 = x;
      x2 = I2::empty();
    } else {
      bool x2_used;
      x1 = intersect(x, xx - division_part1(vf, vd, x2_used));
      x2 = x2_used ? intersect(x, xx - division_part2(vf, vd)) : I2::empty();
    }
    if (width(x2) > width(x1)) std::swap(x1, x2);
    if (empty(x1) || !in_zero(f(x1)))
      if (!empty(x2)) { x1 = x2; x2 = I2::empty(); }
      else continue;
    if (width(x1) < max_width) res.push_back(x1);
    else if (width(x1) > alpha * width(x)) {
      std::pair<I2, I2> p = bisect(x);
      if (in_zero(f(p.first))) l.push_back(p.first);
      x2 = p.second;
    } else l.push_back(x1);
    if (!empty(x2) && in_zero(f(x2)))
      if (width(x2) < max_width) res.push_back(x1);
      else l.push_back(x2);
  }
  return res;
}

int main() {
  {
    I1_aux::traits_type::rounding rnd;
    std::vector<I1> res = newton_raphson(I1(-1, 5.1));
    std::cout << "Results: " << std::endl << std::setprecision(12);
    for(std::vector<I1>::const_iterator i = res.begin(); i != res.end(); ++i)
      std::cout << "  " << *i << std::endl;
    std::cout << std::endl;
  }
  {
    I2_aux::traits_type::rounding rnd;
    std::vector<I2> res = newton_raphson(I2(-1, 5.1));
    std::cout << "Results: " << std::endl << std::setprecision(12);
    for(std::vector<I2>::const_iterator i = res.begin(); i != res.end(); ++i)
      std::cout << "  " << *i << std::endl;
    std::cout << std::endl;
  }
}


/**
 * @file
 * @brief NPDE homework ElementMatrixComputation code
 * @author Janik Schüttler, edited by Oliver Rietmann
 * @date 03.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "mylinearloadvector.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>

#include <Eigen/Core>
#include <functional>

namespace ElementMatrixComputation {

namespace {

/* SAM_LISTING_BEGIN_1 */
Eigen::Vector4d computeLoadVector(
    const Eigen::MatrixXd &vertices,
    std::function<double(const Eigen::Vector2d &)> f) {
  // Number of nodes of the element: triangles = 3, rectangles = 4
  const int num_nodes = vertices.cols();
  // Vector for returning element vector
  Eigen::Vector4d elem_vec = Eigen::Vector4d::Zero();

  //====================
  // Your code goes here
  //====================

  return elem_vec;
}
/* SAM_LISTING_END_1 */

}  // namespace

Eigen::Vector4d MyLinearLoadVector::Eval(const lf::mesh::Entity &cell) {
  // Topological type of the cell
  const lf::base::RefEl ref_el{cell.RefEl()};
  const lf::base::size_type num_nodes{ref_el.NumNodes()};

  // Obtain the vertex coordinates of the cell, which completely
  // describe its shape.
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();

  // Matrix storing corner coordinates in its columns
  auto vertices = geo_ptr->Global(ref_el.NodeCoords());

  return computeLoadVector(vertices, f_);
}

}  // namespace ElementMatrixComputation


//
// Arun Venkatraman (arunvenk@cs.cmu.edu)
// December 2016
//

#pragma once

#include <utils/math_utils_temp.hh>

#include <Eigen/Dense>

#include <sstream>
#include <vector>

namespace ilqr
{

template<int _rows, int _cols>
using Matrix = Eigen::Matrix<double, _rows, _cols>;

template<int _rows>
using Vector = Eigen::Matrix<double, _rows, 1>;

// Helper function for debugging a sequence of vectors.
template<int _dim>
std::string time_print(const std::vector<Vector<_dim>> &vectors)
{
    std::ostringstream oss;
    for (size_t t = 0; t < vectors.size(); ++t)
    {
        const Vector<_dim> &vec = vectors[t];
        oss << "t=" << t  << ": " << vec.transpose();
        if (t < vectors.size() -1)
        {
            oss << std::endl;
        }
    }
    return oss.str();
}

template<int _xdim, int _udim, typename DynamicsFunc>
void linearize_dynamics(const DynamicsFunc &dynamics_func,
                        const Vector<_xdim> &x,
                        const Vector<_udim> &u,
                        Matrix<_xdim, _xdim> &A,
                        Matrix<_xdim, _udim> &B
                       )
{
    const auto helper = [&dynamics_func](const Vector<_xdim+_udim> &xu) -> Vector<_xdim>
    {
        Vector<_xdim> x = xu.topRows(_xdim);
        Vector<_udim> u = xu.bottomRows(_udim);
        return Vector<_xdim>(dynamics_func(x,u));
    };

    Vector<_xdim + _udim> xu;
    xu.topRows(_xdim) = x;
    xu.bottomRows(_udim) = u;
    const Matrix<_xdim, _xdim+_udim> J
        = math::jacobian<_xdim+_udim, _xdim, decltype(helper)>(helper, xu);

    A = J.leftCols(_xdim);
    B = J.rightCols(_udim);
}

template<int _xdim, int _udim, typename CostFunc>
void quadratize_cost(const CostFunc &cost_func,
                     const int t,
                     const Eigen::VectorXd &x,
                     const Eigen::VectorXd &u,
                     Matrix<_xdim,_xdim> &Q,
                     Matrix<_udim,_udim> &R,
                     Matrix<_xdim,_udim> &P,
                     Vector<_xdim> &g_x,
                     Vector<_udim> &g_u
                     )
{
    const auto helper = [&cost_func, t](const Vector<_xdim+_udim> &xu) -> double
    {
        Vector<_xdim> x = xu.topRows(_xdim);
        Vector<_udim> u = xu.bottomRows(_udim);
        return double(cost_func(x,u,t));
    };

    Vector<_xdim + _udim> xu;
    xu.topRows(_xdim) = x;
    xu.bottomRows(_udim) = u;

    constexpr double ZERO_THRESH = 1e-7;

    Vector<_xdim+_udim> g
        = math::gradient<_xdim+_udim, decltype(helper)>(helper, xu);
    g = g.array() * (g.array().abs() > ZERO_THRESH).template cast<double>();
    g_x = g.topRows(_xdim);
    g_u = g.bottomRows(_udim);

    // Zero out components that are less than this threshold. We do this since
    // finite differencing has numerical issues.
    Matrix<_xdim+_udim,_xdim+_udim> H
        = math::hessian<_xdim+_udim, decltype(helper)>(helper, xu);
    //Eigen::MatrixXd H = g * g.transpose();
    H = H.array() * (H.array().abs() > ZERO_THRESH).template cast<double>();
    Q = H.topLeftCorner(_xdim, _xdim);
    P = H.topRightCorner(_xdim, _udim);
    R = H.bottomRightCorner(_udim, _udim);

    Q = (Q + Q.transpose())/2.0;
    Q = math::project_to_psd(Q, 1e-11);
    //math::check_psd(Q, 1e-12);

    // Control terms.
    R = math::project_to_psd(R, 1e-8);
    //math::check_psd(R, 1e-9);
}

template<int _xdim, typename CostFunc>
void quadratize_cost(const CostFunc &cost_func,
                     const Eigen::VectorXd &x,
                     Matrix<_xdim,_xdim> &Q,
                     Vector<_xdim> &g
                     )
{
    constexpr double ZERO_THRESH = 1e-7;

    g = math::gradient<_xdim, CostFunc>(cost_func, x);
    g = g.array() * (g.array().abs() > ZERO_THRESH).template cast<double>();

    // Zero out components that are less than this threshold. We do this since
    // finite differencing has numerical issues.
    Matrix<_xdim,_xdim> H
        = math::hessian<_xdim, CostFunc>(cost_func, x);
    //Eigen::MatrixXd H = g * g.transpose();
    Q = H.array() * (H.array().abs() > ZERO_THRESH).template cast<double>();

    Q = (Q + Q.transpose())/2.0;
    Q = math::project_to_psd(Q, 1e-11);
    math::check_psd(Q, 1e-12);
}

} // namespace ilqr


/*!
 * \file fitness_metric.cc
 *
 * \author Ethan Adams
 * \date
 *
 * This file contains the cpp version of FitnessMetric.py
 */

#include <iostream>

#include <Eigen/Dense>
#include <Eigen/Core>

#include <unsupported/Eigen/NonLinearOptimization>

#include "BingoCpp/explicit_regression.h"
#include "BingoCpp/fitness_metric.h"

namespace bingo {

int LMFunctor::operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) {
  agraphIndv.set_constants(x);
  fvec = fit->evaluate_fitness_vector(agraphIndv, *train);
  return 0;
}

int LMFunctor::df(const Eigen::VectorXd &x, Eigen::MatrixXd &fjac) {
  double epsilon;
  epsilon = 1e-5f;

  for (int i = 0; i < x.size(); i++) {
    Eigen::VectorXd xPlus(x);
    xPlus(i) += epsilon;
    Eigen::VectorXd xMinus(x);
    xMinus(i) -= epsilon;
    Eigen::VectorXd fvecPlus(values());
    operator()(xPlus, fvecPlus);
    Eigen::VectorXd fvecMinus(values());
    operator()(xMinus, fvecMinus);
    Eigen::VectorXd fvecDiff(values());
    fvecDiff = (fvecPlus - fvecMinus) / (2.0 * epsilon);
    fjac.block(0, i, values(), 1) = fvecDiff;
  }

  return 0;
}

double FitnessMetric::evaluate_fitness(AcyclicGraph &indv,
                                       TrainingData &train) {
  if (indv.needs_optimization()) {
    optimize_constants(indv, train);
  }

  return ((evaluate_fitness_vector(indv, train)).abs()).mean();
}

void FitnessMetric::optimize_constants(AcyclicGraph &indv,
                                       TrainingData &train) {
  LMFunctor functor;
  functor.train = &train;
  functor.fit = this;
  functor.m = functor.train->Size();
  // indv.input_constants();
  functor.n = indv.count_constants();
  functor.agraphIndv = indv;
  Eigen::VectorXd vec = Eigen::VectorXd::Random(functor.n);
  Eigen::LevenbergMarquardt<LMFunctor, double> lm(functor);
  lm.minimize(vec);
  indv.set_constants(vec);
  indv.needs_opt = false;
}
} // namespace bingo


/*
MIT License

Copyright (c) 2021 Yoshifumi Asakura

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.
*/

#include <iostream>
#include <boost/math/distributions/skew_normal.hpp>

class max_record {
  // no private
public:
  int    num_grid;
  double width_sig;
  double dmax;
  double inv_max;
  int    search_yet;
         max_record();
  double get_inv_max(
    boost::math::skew_normal_distribution<double> snd,
    double mu,
    double sigma
  );
};

max_record::max_record(){
  num_grid   = 400;
  width_sig  = 10.0;
  search_yet = 1;
  dmax       = 0.0;
  inv_max    = 1.0;
}

double max_record::get_inv_max(
    boost::math::skew_normal_distribution<double> snd,
    double mu,
    double sigma
){
  if(sigma <= 0.0){
    std::cout << ">>> error, sigma <= 0" << std::endl;
    return(0.0);
  }
  if(search_yet){
    search_yet  = 0;
    double cur;
    double x;
    double xmin = mu - sigma * width_sig;
    double xmax = mu + sigma * width_sig;
    double dx   = (xmax - xmin) / num_grid;
    for(x = xmin; x <= xmax; x += dx){
      cur = boost::math::pdf(snd, x);
      if(dmax < cur){
        dmax = cur;
      }
    }
    if(dmax == 0.0){
      inv_max = 0.0;
      std::cout << ">>> error, max of the distribusion is 0" << std::endl;
    } else {
      inv_max = 1.0 / dmax;
    }
    std::cout << ">>> " << dmax << " " << inv_max << std::endl;
  }
  return(inv_max);
}

// use this in skew_normal_1d
max_record mr;

double skew_normal_1d(
  double   mu_t,
  double   x,
  double   sigma_inner,
  double   startx,
  double   gauss_max,
  double   skew_shape
){
  // set sigma as the wave length get similar to the normal gaussian
  double sigma = sigma_inner;

  // a constant parameter
  double alpha = skew_shape * sigma;

  double mu    = mu_t + startx;

  // use below in pdf
  boost::math::skew_normal_distribution<double> snd(mu, sigma, alpha);

  double ratio = mr.get_inv_max(snd, mu, sigma);

  double out   = boost::math::pdf(snd, x) * gauss_max * ratio;
  return(out);
}


#include <Eigen/Core>
#include <Eigen/Geometry>

#include <ndt_mcl/3d_ndt_ukf.h>

#include <iostream>
#include <iomanip>

using namespace std;

int main()
{
    {
        UKF3D ukf;

        std::cout << "ukf.getLambda() : " << ukf.getLambda() << std::endl;
        std::cout << "ukf.getXsi() : " << ukf.getXsi() << std::endl;

        for(int i = 0; i < ukf.getNbSigmaPoints(); i++) {
            std::cout << "mean weight [" << i << "]:" << ukf.getWeightMean(i) << std::endl;
        }

        std::cout << "####################################################" << std::endl;

        UKF3D::Params params;
        params.alpha = 0.4;
        params.beta = 2.;
        params.kappa = 3.;
        ukf.setParams(params);

        std::cout << "ukf.getLambda() : " << ukf.getLambda() << std::endl;
        std::cout << "ukf.getXsi() : " << ukf.getXsi() << std::endl;

        for(int i = 0; i < ukf.getNbSigmaPoints(); i++) {
            std::cout << "mean weight [" << i << "]:" << ukf.getWeightMean(i) << std::endl;
        }

        Eigen::VectorXd x(6);
        x << 1, 2, 3, 0.0, 0.0, 0.0;
        std::cout << "x: " << x << std::endl;
        Eigen::Affine3d T = ndt_generic::vectorToAffine3d(x);

        Eigen::MatrixXd cov(6,6);
        cov.setZero();
        cov(0,0) = 0.1; cov(1,1) = 0.1; cov(2,2) = 0.1;
        cov(3,3) = 0.02; cov(4,4) = 0.02; cov(5,5) = 0.02;

        //ukf.initializeFilter(T, cov);
        ukf.assignSigmas(x, cov);

        std::cout << ukf.getDebugString() << std::endl;

        // Eigen::VectorXd mean = ukf.computePoseMean();
        // std::cout << "mean : " <<  ukf.computePoseMean().transpose() << std::endl;

        Eigen::VectorXd mean(6);
        std::vector<Eigen::Affine3d> T_sigmas = ukf.getSigmasAsAffine3d();
        std::vector<double> weights = ukf.getMeanWeights();
        Eigen::Affine3d T2 = ndt_generic::getAffine3dMean(T_sigmas);
        Eigen::Affine3d T3 = ndt_generic::getAffine3dMeanWeights(T_sigmas, weights);
        Eigen::Affine3d T4 = ndt_generic::getAffine3dMeanWeightsUsingQuat(T_sigmas, weights);
        Eigen::Affine3d T5 = ndt_generic::getAffine3dMeanWeightsUsingQuatNaive(T_sigmas, weights);

        std::cout << "T  -> vec : " << ndt_generic::affine3dToStringRPY(T) << std::endl;
        std::cout << "T2 -> vec : " << ndt_generic::affine3dToStringRPY(T2) << std::endl;
        std::cout << "T3 -> vec : " << ndt_generic::affine3dToStringRPY(T3) << std::endl;
        std::cout << "T4 -> vec : " << ndt_generic::affine3dToStringRPY(T4) << std::endl;
        std::cout << "T5 -> vec : " << ndt_generic::affine3dToStringRPY(T5) << std::endl;

        std::cout << " T : " << ndt_generic::affine3dToStringRotMat(T) << std::endl;
        std::cout << " T2: " << ndt_generic::affine3dToStringRotMat(T2) << std::endl;
        std::cout << " T3: " << ndt_generic::affine3dToStringRotMat(T3) << std::endl;
        std::cout << " T4: " << ndt_generic::affine3dToStringRotMat(T4) << std::endl;
        std::cout << " T5: " << ndt_generic::affine3dToStringRotMat(T5) << std::endl;

        std::cout << ukf.getDebugString() << std::endl;

        Eigen::VectorXd incr(6);
        incr << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0;
        Eigen::Affine3d T_incr = ndt_generic::vectorToAffine3d(incr);

        std::cout << "T_incr : " << ndt_generic::affine3dToStringRotMat(T_incr) << std::endl;

        for (int i = 0; i< 5; i++) {
            std::cout << "------------------------------" << std::endl;
            std::cout << ukf.getDebugString();
            ukf.predict(T_incr, cov);
            mean = ukf.computePoseMean();

            std::cout << "mean[" << i << "]: " <<  mean.transpose() << std::endl;
        }
    }

    // {
    //     UKF3D ukf;

    //     ukf.setParams(0.4, 2., 3);

    //     Eigen::VectorXd x(6), x2(6);
    //     x << 1, 2, 3, 0.0, 0.0, 0.0;

    //     Eigen::MatrixXd cov(6,6), cov2(6,6);
    //     cov.setZero();
    //     cov(0,0) = 0.1; cov(1,1) = 0.1; cov(2,2) = 0.1;
    //     cov(3,3) = 0.02; cov(4,4) = 0.02; cov(5,5) = 0.02;

    //     for (int i = 1; i < 10; i++) {
    //         double alpha = i*0.01;
    //         double kappa = i;
    //         ukf.setParams(alpha, 2., kappa);
    //         std::cout << "x: " << x << std::endl;
    //         std::cout << "cov : " << cov << std::endl;
    //         ukf.assignSigmas(x, cov);

    //         x2 = ukf.computePoseMean();
    //         cov2 = ukf.computePoseCov(x2);

    //         std::cout << "----------------------------" << std::endl;
    //         std::cout << "x2: " << x2 << std::endl;
    //         std::cout << "cov2 : " << cov2 << std::endl;
    //     }
    // }
    {
        UKF3D ukf;

        UKF3D::Params params;
        params.alpha = 0.1;
        params.beta = 2.;
        params.kappa = 3.;
        ukf.setParams(params);

        Eigen::VectorXd x(6), x2(6);
        x << 1, 2, 3, 0.0, 0.0, 0.0;

        Eigen::MatrixXd cov(6,6), cov2(6,6);
        cov.setZero();
        cov(0,0) = 10; cov(1,1) = 1; cov(2,2) = 0.1;
        cov(3,3) = 0.02; cov(4,4) = 0.02; cov(5,5) = 0.02;

        std::cout << "x: " << x << std::endl;
        std::cout << "cov : " << cov << std::endl;
        ukf.assignSigmas(x, cov);

        x2 = ukf.computePoseMean();
        cov2 = ukf.computePoseCov(x2);

        std::cout << "----------------------------" << std::endl;
        std::cout << "x2: " << x2 << std::endl;
        std::cout << "cov2 : " << cov2 << std::endl;
    }
}


#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <iostream>
#include <random>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <Eigen/LU>

#include <gtest/gtest.h>
#include "calibration/Calibration.h"

int main(int argc, char **argv)
{
	std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> dis(-1, 1);

	// AX = ZB
	Eigen::Matrix4f Z = Eigen::Matrix4f::Identity();
	Eigen::Vector4f qz_use = Eigen::Vector4f(dis(gen), dis(gen), dis(gen), dis(gen) );
	Eigen::Quaternionf qz( qz_use[0], qz_use[1], qz_use[2], qz_use[3] );
	qz.normalize();
	Eigen::Vector3f tz = Eigen::Vector3f( dis(gen), dis(gen), dis(gen) );
	Z.block<3,3>(0,0) = qz.toRotationMatrix();
	Z.block<3,1>(0,3) = tz;
	//std::cout << "Z: " << std::endl << Z << std::endl;

 	Eigen::Matrix4f X = Eigen::Matrix4f::Identity();
	Eigen::Vector4f qx_use = Eigen::Vector4f(dis(gen), dis(gen), dis(gen), dis(gen) );
	Eigen::Quaternionf qx( qx_use[0], qx_use[1], qx_use[2], qx_use[3] );
	qx.normalize();
	Eigen::Vector3f tx = Eigen::Vector3f(dis(gen),dis(gen), dis(gen));
	X.block<3,3>(0,0) = qx.toRotationMatrix();
	X.block<3,1>(0,3) = tx;
	//std::cout << "X: " << std::endl << X << std::endl;

 	int NPoses = 6;

	std::vector<Matrix4f> A, B;

	Eigen::Matrix4f a = Eigen::Matrix4f::Identity();

	Eigen::Vector4f q_use = Eigen::Vector4f::Random();
	Eigen::Quaternionf q( q_use[0], q_use[1], q_use[2], q_use[3]);
	q.normalize();

	Eigen::Vector3f t = Eigen::Vector3f( dis(gen), dis(gen), dis(gen) );

	a.block<3,3>(0,0) = q.toRotationMatrix();
	a.block<3,1>(0,3) = t;

	for (int i = 0; i < NPoses; i++)

	{

		// test data
		Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
		b = Z.inverse()*a*X;

		Eigen::Matrix4f d = Eigen::Matrix4f::Identity();

		Eigen::Vector3f t = 0.001*Eigen::Vector3f( dis(gen),dis(gen), dis(gen) );

		float roll = dis(gen)*M_PI/512;
		float pitch = dis(gen)*M_PI/512;
		float yaw = dis(gen)*M_PI/512;
		Eigen::Matrix3f R;
		R = AngleAxisf(roll, Vector3f::UnitX())
			*AngleAxisf(pitch, Vector3f::UnitY())
  			*AngleAxisf(yaw, Vector3f::UnitZ());
		d.block<3,3>(0,0) = R;
		d.block<3,1>(0,3) = t;

		a = d*a;

		A.push_back(a);
		B.push_back(b);

	}

	// first, create an object of your class
	calibration::Calibration mycalib;

	mycalib.setInput(A, B);

	mycalib.computeNonLinOpt();

	Eigen::Matrix4f Tx, Tz;
	Tx = Eigen::Matrix4f::Identity();
	Tz = Eigen::Matrix4f::Identity();
	mycalib.getOutput(Tz, Tx);

	// print out the result

	std::cout << "Input Z: " << std::endl << Z << std::endl;
	std::cout << "Estimated Z:" << std::endl << Tz << std::endl;
	std::cout << "Error over Z estimation: " << std::endl << Z-Tz << std::endl << std::endl;

	std::cout << "Input X: " << std::endl << X << std::endl;
	std::cout << "Estimated X:" << std::endl << Tx << std::endl;
	std::cout << "Error over X estimation: " << std::endl << X-Tx << std::endl;

	return 0;
}


// Implementation of the GCPR 2016 Paper "Joint Object Pose Estimation and
// Shape Reconstruction in Urban Street Scenes Using 3D Shape Priors" by Engelmann et al.
// Copyright (C) 2016  Francis Engelmann - Visual Computing Institute RWTH Aachen University
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.

// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

// Eigen includes
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Geometry>
#include <Eigen/SVD>

// C/C++ includes
#include <iostream>
#include <tuple>

// Own includes
#include "geometry.h"

namespace gvl {

// See http://www.cs.virginia.edu/~gfx/Courses/1999/intro.fall99.html/lookat.html
// for implementation details.
Eigen::Matrix4d computeLookAtMatrix(const Eigen::Vector3d& I,
                                    const Eigen::Vector3d& E,
                                    const Eigen::Vector3d& U)
{
  Eigen::Vector3d F = I-E;
  Eigen::Vector3d f = F/F.norm();
  Eigen::Vector3d u = U/U.norm();
  Eigen::Vector3d s = f.cross(u);
  Eigen::Vector3d w = s.cross(f);

  Eigen::Matrix<double,4,4> M = Eigen::Matrix<double,4,4>::Identity();
  M(0,0) = s(0); M(0,1) = s(1); M(0,2) = s(2);
  M(1,0) = w(0); M(1,1) = w(1); M(1,2) = w(2);
  M(2,0) = -f(0); M(2,1) = -f(1); M(2,2) = -f(2);

  Eigen::Matrix<double,4,4> T = Eigen::Matrix<double,4,4>::Identity();
  T(0,3) = -E(0); T(1,3) = -E(1); T(2,3) = -E(2);

  return M*T;
}

Eigen::Matrix3d computeIntrinsicMatrix(const double f_x,
                                       const double f_y,
                                       const double p_x,
                                       const double p_y,
                                       const double s)
{
  Eigen::Matrix<double,3,3> K = Eigen::Matrix<double,3,3>::Identity();
  K(0,0) = f_x;
  K(1,1) = f_y;
  K(0,2) = p_x;
  K(1,2) = p_y;
  K(0,1) = s;
  return K;
}

// See http://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
Eigen::Matrix4d computeTransformationFromPlane(const double a,
                                               const double b,
                                               const double c,
                                               const double d)
{
  Eigen::Vector3d n; n << a,b,c;
  Eigen::Vector3d m; m << 0,-1,0;
  Eigen::Vector3d v; v = n.cross(m);
  double sin = v.norm(); // sine of angle
  double ccos = n.dot(m); // cosine of angle
  Eigen::Matrix3d v_x; v_x << 0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0;
  Eigen::Matrix3d R; R = Eigen::Matrix3d::Identity() + v_x + v_x*v_x*(1-ccos)/(sin*sin);
  Eigen::Vector3d t; t << 0, -d, 0;
  Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
  T.block<3,3>(0,0) = R;
  T.block<3,1>(0,3) = t;
  return T;
}

void computeVerticesFromDisparity(const cv::Mat& disparity,
                                  const Eigen::Matrix3d& K,
                                  const double b,
                                  cv::Mat& vertices)
{
  // Init variables
  const double max_depth = 1000;
  const double min_depth = 1;
  const double& baseline = b;
  const double& f = K(0,0);
  Eigen::Matrix3d Kinv = K.inverse();
  vertices = cv::Mat(disparity.rows, disparity.cols, CV_64FC3, cv::Scalar(0,0,0));

  // Iterate over disparity pixels, compute corresponding vertex
  for (int v=0; v<disparity.rows; v++) {
    for (int u=0; u<disparity.cols; u++) {
      // Compute depth
      double disp = (double)((uint16_t)(disparity.at<unsigned short>(v,u)))/256.0;
      double depth = (baseline*f)/(disp); //256 according to spspstereo
      if (depth < min_depth || depth > max_depth) continue;

      // Compute vertice
      //Eigen::Vector3d v3d = depth*Kinv*Eigen::Vector3d(u,v,1.0);
      vertices.at<cv::Vec3d>(v,u)[0] = depth*(Kinv(0,0)*u + Kinv(0,1)*v + Kinv(0,2));
      vertices.at<cv::Vec3d>(v,u)[1] = depth*(Kinv(1,0)*u + Kinv(1,1)*v + Kinv(1,2));
      vertices.at<cv::Vec3d>(v,u)[2] = depth*(Kinv(2,0)*u + Kinv(2,1)*v + Kinv(2,2));
    }
  }
}

void computeDisparityFromVertices(const cv::Mat& vertices,
                                  const Eigen::Matrix3d& K,
                                  const double b,
                                  cv::Mat& disparity)
{

  // Init variables
  double max_depth = 1000;
  double min_depth = 1;
  const double& baseline = b;
  const double& f = K(0,0);

  // Iterate over disparity pixels, compute corresponding vertex
  for (int v=0; v<vertices.rows; v++) {
    for (int u=0; u<vertices.cols; u++) {
      cv::Vec3d vertex = vertices.at<cv::Vec3d>(v,u);
      const double& depth = vertex[2];
      if (depth < min_depth || depth > max_depth) continue;
      double disp = (baseline*f)/depth;
      //unsigned short disp_old = disparity.at<unsigned short>(v,u);
      unsigned short disp_new = disp*256;
      disparity.at<unsigned short>(v,u) = disp_new;
    }
  }
}

void computeNormalsFromVertices(const cv::Mat& vertices, cv::Mat& normals)
{
  double neighbor_dist_threshold = 0.5;
  normals = cv::Mat(vertices.rows, vertices.cols, CV_64FC3, cv::Scalar(0,0,1) );

  // Iterate over vertices in image
  for (int v=1; v<normals.rows-1; v++) {
    for (int u=1; u<normals.cols-1; u++) {

      // Go over 8-neighborhood to compute #neighbors
      cv::Vec3d b = vertices.at<cv::Vec3d>(v,u); // center
      //unsigned char neighbor_mask = 0;  // binary mask to mark neighbors within distance threshold
      unsigned char neighbor_count = 0; // number of neighbors within distance threshold
      char neighbor_curr = -1;  // index of current neighor that is checked, used for mask
      cv::Vec3d centroid(0,0,0); // Accumulator to compute centroid
      Eigen::MatrixXd data_matrix(9,3);

      // Iterate over 8-neighborhood
      for (int x=u-1; x<=u+1; x++) {
        for (int y=v-1; y<=v+1; y++) {
          neighbor_curr++;
          cv::Vec3d a = vertices.at<cv::Vec3d>(y,x);
          cv::Vec3d d = a-b;
          double squared_dist = (d).dot(d);

          // Only accept points within distance threshold
          if (squared_dist < neighbor_dist_threshold*neighbor_dist_threshold) {
            data_matrix(neighbor_count, 0) = a[0];
            data_matrix(neighbor_count, 1) = a[1];
            data_matrix(neighbor_count, 2) = a[2];
            neighbor_count++;
            //neighbor_mask = neighbor_mask | (1<<neighbor_curr);
            centroid += a;
          }
        }
      }

      // if hte number of neighbors is too small, we need at least 3 vertices
      if (neighbor_count < 3) continue; // default normal (0,0,1) is assigned, see init

      // Compute centroid and resize data_matrix
      centroid /= (double)neighbor_count;
      data_matrix.resize(neighbor_count, 3);

      // Subtract mean from data_matrix
      for (int i=0; i<neighbor_count; i++) {
        data_matrix(i,0) -= centroid[0];
        data_matrix(i,1) -= centroid[1];
        data_matrix(i,2) -= centroid[2];
      }

      // Last eigenvector corresponds to normal
      Eigen::JacobiSVD<Eigen::MatrixXd> svd(data_matrix, Eigen::ComputeThinU | Eigen::ComputeFullV);

      /*double ev0 = svd.singularValues()[0];
      double ev1 = svd.singularValues()[1];
      double ratio = ev0/ev1;
      if (ratio>16) continue; // default normal (0,0,1) is assigned, see init*/

      Eigen::Vector3d n = svd.matrixV().col(2);
      n /= n.norm();

      // Get the correct orientation, assuming all points are visible from origin
      Eigen::Vector3d vector; vector << b[0],b[1],b[2];
      vector/=vector.norm();
      double dot = vector.dot(n);
      if (dot>0) n*=-1;

      cv::Vec3d normal;
      normal[0] = n[0];
      normal[1] = n[1];
      normal[2] = n[2];
      normals.at<cv::Vec3d>(v,u) = normal;

    }
  }
}

void computeNormalsFromVerticesSimple(const cv::Mat& vertices, cv::Mat& normals)
{
  normals = cv::Mat(vertices.rows, vertices.cols, CV_64FC3, cv::Scalar(0,0,0) );
  for (int v=1; v<normals.rows-1; v++) {
    for (int u=1; u<normals.cols-1; u++) {

      cv::Vec3d v1 = vertices.at<cv::Vec3d>(v+1,u);
      cv::Vec3d v2 = vertices.at<cv::Vec3d>(v,u);
      cv::Vec3d v3 = vertices.at<cv::Vec3d>(v,u+1);

      Eigen::Vector3d v1_; v1_ << v1[0],v1[1],v1[2];
      Eigen::Vector3d v2_; v2_ << v2[0],v2[1],v2[2];
      Eigen::Vector3d v3_; v3_ << v3[0],v3[1],v3[2];
      Eigen::Vector3d n = (v1_-v2_).cross(v3_-v2_);
      n /= n.norm();
      cv::Vec3d normal; normal[0] = n[0]; normal[1] = n[1]; normal[2] = n[2];
      normals.at<cv::Vec3d>(v,u) = normal;
    }
  }
}

void computePointcloudFromVerticesAndColor(const cv::Mat &vertices,
                                           const cv::Mat &colors,
                                           gvl::Pointcloud& pointcloud)
{
  // Check that vertices and colors have the same size
  assert(vertices.cols == colors.cols && vertices.rows == colors.rows);

  // Allocate as many points as pixels in the image,
  // this allows to address the 3d-points by 2d-coordinates
  pointcloud.points.resize(vertices.cols*vertices.rows);

  for (int v=0; v<vertices.rows; v++) {
    for (int u=0; u<vertices.cols; u++) {
      gvl::Point point;
      point.x = (double)vertices.at<cv::Vec3d>(v,u)[0];
      point.y = (double)vertices.at<cv::Vec3d>(v,u)[1];
      point.z = (double)vertices.at<cv::Vec3d>(v,u)[2];
      point.r = (unsigned char)colors.at<cv::Vec3b>(v,u)[2];
      point.g = (unsigned char)colors.at<cv::Vec3b>(v,u)[1];
      point.b = (unsigned char)colors.at<cv::Vec3b>(v,u)[0];
      point.u = u;
      point.v = v;
      int index = v*vertices.cols + u;
      pointcloud.points.at(index) = point;
    }
  }
}

void computeBoundingBoxFromPointcloud(const Pointcloud &pointcloud, BoundingBox& bb)
{
  double min_x=900, max_x=-900;
  double min_y=900, max_y=-900;
  double min_z=900, max_z=-900;
  for (auto p : pointcloud.points) {
    if (p.x > max_x) max_x=p.x;
    if (p.y > max_y) max_y=p.y;
    if (p.z > max_z) max_z=p.z;
    if (p.x < min_x) min_x=p.x;
    if (p.y < min_y) min_y=p.y;
    if (p.z < min_z) min_z=p.z;
  }
  bb.height = max_y - min_y;
  bb.width = max_x - min_x;
  bb.length = max_z - min_z;
  bb.x = (max_x + min_x)/2.0;
  bb.y = max_y;
  bb.z = (max_z + min_z)/2.0;
  bb.rotation_y=0;
}

double computeSquaredDistance(const Point& p1, const Point& p2)
{
  double d1 = (p1.x-p2.x);
  double d2 = (p1.y-p2.y);
  double d3 = (p1.z-p2.z);
  return d1*d1+d2*d2+d3*d3;
}

double bilinearInterpolation(const Eigen::Vector4d& values,
                             const Eigen::Vector2d& position) {
  const double& u = position[0];
  const double& v = position[1];
  return (1-u)*(1-v)*values[0] +
         (0+u)*(1-v)*values[1] +
         (1-u)*(0+v)*values[2] +
         (0+u)*(0+v)*values[3];
}

Eigen::Matrix4d computePoseFromRotTransScale(const double rotation_y,
                                             const Eigen::Vector3d& translation,
                                             const double scale)
{
  //Eigen::AngleAxisd aa(rotation_y, Eigen::Vector3d(0,1,0)); aa.matrix();
  Eigen::Matrix3d rotation = Eigen::Matrix3d::Identity();
  rotation(0,0) = std::cos(rotation_y);   rotation(0,2) = std::sin(rotation_y);
  rotation(2,0) = -std::sin(rotation_y);  rotation(2,2) = std::cos(rotation_y);
  Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
  pose.block<3,3>(0, 0) = rotation*scale;
  pose.block<3,1>(0, 3) = translation;
  return pose;
}

void computeBoundingBoxFromAnnotation(const Annotation &annotation,
                                      const Eigen::Vector3f &color,
                                      BoundingBox &bb)
{
  bb.height = 1.7;
  bb.width = 2.0;
  bb.length = 4.5;
  bb.x = annotation.translation[0];
  bb.y = annotation.translation[1];
  bb.z = annotation.translation[2];
  bb.rotation_y = annotation.rotation_y;
  bb.r = color[0];
  bb.g = color[1];
  bb.b = color[2];
  bb.score = 1.0;
}

double computeMedian(std::vector<double>& values) {
  std::sort(values.begin(), values.end());
  std::cout << "Median >> Values: "; for (auto t: values) std::cout << t << " "; std::cout << std::endl;
  double median;
  if (values.size()%2 == 1) {
    std::cout << "Median >> Size: " << values.size() << std::endl;
    median = values.at(values.size()/2);
  } else {
    median = 0.5*values.at(values.size()/2 - 1)+0.5*values.at(values.size()/2);;
  }
  std::cout << "Median: " << median << std::endl;
  return median;
}

double computeVerticalDistanceToPlane(const double a,
                                      const double b,
                                      const double c,
                                      const double d,
                                      const double x,
                                      const double z)
{
  double y = (-d-c*z-a*x)/b;
  return y;
}

// From: http://stackoverflow.com/questions/7685495/transforming-a-3d-plane-by-4x4-matrix
// This does NOT seem to work!!!
void transform_plane(const Eigen::Matrix4d &trafo, Eigen::Vector4d &p)
{
  std::cerr << "Transform_plane does NOT seem to work!!!" << std::endl;
  Eigen::Vector4d O = Eigen::Vector4d(p[0]*p[3], p[1]*p[3], p[2]*p[3], 1);
  Eigen::Vector4d N = Eigen::Vector4d(p[0], p[1], p[2], 0.0);
  O = trafo*O;
  N = (trafo.inverse()).transpose() * N;

  Eigen::Vector3d n = Eigen::Vector3d(N[0],N[1],N[2]);
  Eigen::Vector3d o = Eigen::Vector3d(O[0],O[1],O[2]);
  p[0] = N[0];
  p[1] = N[1];
  p[2] = N[2];
  p[3] = o.dot(n);

  //vector4 O = (xyz * d, 1)
  //vector4 N = (xyz, 0)
  //O = M * O
  //N = transpose(invert(M)) * N
  //xyz = N.xyz
  //d = dot(O.xyz, N.xyz)
}

}


#include <boost/ut.hpp>
#include <units/isq/si/international/length.h>
#include <units/isq/si/international/speed.h>  // IWYU pragma: keep
#include <units/isq/si/length.h>
#include <units/isq/si/speed.h>  // IWYU pragma: keep
#include <units/isq/si/time.h>

namespace embed {
using namespace units::isq;
namespace units_testing {
Speed auto avg_speed(Length auto d, Time auto t)
{
  return d / t;
}
}  // namespace units_testing

// This test is here purely to ensure that units compile and work in
// libembeddedhal
boost::ut::suite units_test = []() {
  using namespace boost::ut;

  using namespace units::isq;
  using namespace units::aliases::isq::si::speed;
  using namespace units::aliases::isq::si::length;
  using namespace units::aliases::isq::si::time;
  using units::aliases::isq::si::international::speed::mi_per_h;

  Speed auto v1 =
    units_testing::avg_speed(km<std::int64_t>(1000), h<std::int64_t>(3));
  Speed auto v2 = units_testing::avg_speed(
    si::length<si::international::mile>(140), si::time<si::hour>(2));

  expect(v1 == km_per_h<std::int64_t>(333));
  expect(v2 == mi_per_h(70));
};
}  // namespace embed


/* Copyright (c) 2017, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
// g++ -Wall -std=c++1z -I /usr/include/eigen3/ main.cpp -o test
#include <random>
#include <cmath>
#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include "vmf.hpp"

int main() {

  std::mt19937 rnd(1);
  vMF<float,3> vmf(Eigen::Vector3f(0,0,1), 100);

  std::ofstream out("vmfSamples_tau100.csv");
  for (size_t i=0; i<10000; ++i) {
    Eigen::Vector3f x = vmf.sample(rnd);
    out << x(0) << " " << x(1) << " " << x(2) << std::endl;
  }
  out.close();

  return 0;
}


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

#include "ceres/autodiff_local_parameterization.h"
#include <Eigen/Core>
#include <ceres/autodiff_cost_function.h>
#include <game/vsr/cga_op.h>
#include <glog/logging.h>

using namespace vsr::cga;

const double kPi = 3.141592653589793238462643383279;

struct VectorCorrespondencesCostFunctor {
  VectorCorrespondencesCostFunctor(const Vec &a, const Vec &b) : a_(a), b_(b) {}

  template <typename T>
  auto operator()(const T *const rotor, T *residual) const -> bool {
    Rotor<T> R(rotor);
    Vector<T> a(a_);
    Vector<T> b(b_);
    Vector<T> c = a.spin(R);

    for (int i = 0; i < 3; ++i) {
      residual[i] = c[i] - b[i];
    }

    return true;
  }

private:
  const Vec a_;
  const Vec b_;
};

struct RotorPlus {
  template <typename T>
  bool operator()(const T *x, const T *delta, T *x_plus_delta) const {
    const T squared_norm_delta =
        delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
    T r_delta[4];
    if (squared_norm_delta > T(0.0)) {
      T norm_delta = sqrt(squared_norm_delta);
      const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
      r_delta[0] = cos(norm_delta);
      r_delta[1] = sin_delta_by_delta * delta[0];
      r_delta[2] = sin_delta_by_delta * delta[1];
      r_delta[3] = sin_delta_by_delta * delta[2];
    } else {
      // We do not just use r_delta = [1,0,0,0] here because that is a
      // constant and when used for automatic differentiation will
      // lead to a zero derivative. Instead we take a first order
      // approximation and evaluate it at zero.
      r_delta[0] = T(1.0);
      r_delta[1] = delta[0];
      r_delta[2] = delta[1];
      r_delta[3] = delta[2];
    }

    Rotor<T> rotor = Rotor<T>{r_delta[0], r_delta[1], r_delta[2], r_delta[3]} *
                     Rotor<T>{x[0], x[1], x[2], x[3]};

    for (int i = 0; i < 4; ++i)
      x_plus_delta[i] = rotor[i];

    return true;
  }
};

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

  google::InitGoogleLogging(argv[0]);

  double theta_half{kPi / 6.0};
  Rot rotor{cos(theta_half), -sin(theta_half), 0.0, 0.0};
  Vec a{1.0, 0.0, 0.0};
  Vec b{0.0, 1.0, 0.0};

  Eigen::Matrix<double, 3, 4, Eigen::RowMajor> global_jacobian;
  Eigen::Matrix<double, 4, 3, Eigen::RowMajor> local_jacobian;
  Eigen::Matrix<double, 3, 3, Eigen::RowMajor> jacobian;
  Eigen::Matrix<double, 1, 3> result;

  const double *parameters[1] = {rotor.begin()};
  double *global_jacobian_array[1] = {global_jacobian.data()};

  ceres::AutoDiffCostFunction<VectorCorrespondencesCostFunctor, 3, 4>(
      new VectorCorrespondencesCostFunctor(a, b))
      .Evaluate(parameters, result.data(), global_jacobian_array);

  ceres::AutoDiffLocalParameterization<RotorPlus, 4, 3>(new RotorPlus())
      .ComputeJacobian(rotor.begin(), local_jacobian.data());

  jacobian = global_jacobian * local_jacobian;

  std::cout << "Jacobian of the function F = R * a * ~R - b where" << std::endl;
  std::cout << "R is a Euclidean rotor with coefficients:" << std::endl;
  std::cout << "R: " << rotor << std::endl;
  std::cout << "and a and b are vectors with coefficients:" << std::endl;
  std::cout << "a: " << a << std::endl;
  std::cout << "b: " << b << std::endl;
  std::cout << "The resulting vector have coefficients:" << std::endl;
  std::cout << result << std::endl;
  std::cout << std::endl;
  std::cout << "The resulting 3x4 global jacobian:" << std::endl;
  std::cout << global_jacobian << std::endl;
  std::cout << "The 3x3 local jacobian:" << std::endl;
  std::cout << local_jacobian << std::endl;
  std::cout << "The final 3x3 jacobian:" << std::endl;
  std::cout << jacobian << std::endl;

  return 0;
}