#include <stan/math/prim/mat.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>

using Eigen::Dynamic;
using Eigen::Matrix;
using Eigen::VectorXd;

TEST(ProbDistributions,Dirichlet) {
  Matrix<double,Dynamic,1> theta(3,1);
  theta << 0.2, 0.3, 0.5;
  Matrix<double,Dynamic,1> alpha(3,1);
  alpha << 1.0, 1.0, 1.0;
  EXPECT_FLOAT_EQ(0.6931472, stan::math::dirichlet_log(theta,alpha));

  Matrix<double,Dynamic,1> theta2(4,1);
  theta2 << 0.01, 0.01, 0.8, 0.18;
  Matrix<double,Dynamic,1> alpha2(4,1);
  alpha2 << 10.5, 11.5, 19.3, 5.1;
  EXPECT_FLOAT_EQ(-43.40045, stan::math::dirichlet_log(theta2,alpha2));
}

TEST(ProbDistributions,DirichletPropto) {
  Matrix<double,Dynamic,1> theta(3,1);
  theta << 0.2, 0.3, 0.5;
  Matrix<double,Dynamic,1> alpha(3,1);
  alpha << 1.0, 1.0, 1.0;
  EXPECT_FLOAT_EQ(0.0, stan::math::dirichlet_log<true>(theta,alpha));

  Matrix<double,Dynamic,1> theta2(4,1);
  theta2 << 0.01, 0.01, 0.8, 0.18;
  Matrix<double,Dynamic,1> alpha2(4,1);
  alpha2 << 10.5, 11.5, 19.3, 5.1;
  EXPECT_FLOAT_EQ(0.0, stan::math::dirichlet_log<true>(theta2,alpha2));
}

TEST(ProbDistributions,DirichletBounds) {
  Matrix<double,Dynamic,1> good_alpha(2,1), bad_alpha(2,1);
  Matrix<double,Dynamic,1> good_theta(2,1), bad_theta(2,1);

  good_theta << 0.25, 0.75;
  good_alpha << 2, 3;
  EXPECT_NO_THROW(stan::math::dirichlet_log(good_theta,good_alpha));

  good_theta << 1.0, 0.0;
  good_alpha << 2, 3;
  EXPECT_NO_THROW(stan::math::dirichlet_log(good_theta,good_alpha))
    << "elements of theta can be 0";

  bad_theta << 0.25, 0.25;
  EXPECT_THROW(stan::math::dirichlet_log(bad_theta,good_alpha),
               std::domain_error)
    << "sum of theta is not 1";

  bad_theta << -0.25, 1.25;
  EXPECT_THROW(stan::math::dirichlet_log(bad_theta,good_alpha),
               std::domain_error)
    << "theta has element less than 0";

  bad_theta << -0.25, 1.25;
  EXPECT_THROW(stan::math::dirichlet_log(bad_theta,good_alpha),
               std::domain_error)
    << "theta has element less than 0";

  bad_alpha << 0.0, 1.0;
  EXPECT_THROW(stan::math::dirichlet_log(good_theta,bad_alpha),
               std::domain_error)
    << "alpha has element equal to 0";

  bad_alpha << -0.5, 1.0;
  EXPECT_THROW(stan::math::dirichlet_log(good_theta,bad_alpha),
               std::domain_error)
    << "alpha has element less than 0";

  bad_alpha = Matrix<double,Dynamic,1>(4,1);
  bad_alpha << 1, 2, 3, 4;
  EXPECT_THROW(stan::math::dirichlet_log(good_theta,bad_alpha),
               std::invalid_argument)
    << "size mismatch: theta is a 2-vector, alpha is a 4-vector";
}

double chi_square(std::vector<int> bin, std::vector<double> expect) {
  double chi = 0;
  for (size_t j = 0; j < bin.size(); j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);
  return chi;
}

void test_dirichlet3_1(VectorXd alpha) {
  boost::random::mt19937 rng;
  int N = 10000;
  int K = boost::math::round(2 * std::pow(N, 0.4));

  // bins 0 vs. 1 + 2
  boost::math::beta_distribution<> dist(alpha(0), alpha(1) + alpha(2));
  boost::math::chi_squared mydist(K - 1);

  std::vector<double> loc(K - 1);
  for (int i = 1; i < K; i++)
    loc[i - 1] = quantile(dist, i / static_cast<double>(K));

  std::vector<int> bin(K, 0);
  std::vector<double> expect(K, N / static_cast<double>(K));

  for (int count = 0; count < N; ++count) {
    Eigen::VectorXd theta = stan::math::dirichlet_rng(alpha,rng);
    int i;
    for (i = 0; i < K-1 && theta(0) > loc[i]; ++i) ;
    ++bin[i];
  }
  EXPECT_TRUE(chi_square(bin,expect) < quantile(complement(mydist, 1e-6)));
}

void test_dirichlet3_2(VectorXd alpha) {
  boost::random::mt19937 rng;
  int N = 10000;
  int K = boost::math::round(2 * std::pow(N, 0.4));
  boost::math::beta_distribution<> dist(alpha(1), alpha(0) + alpha(2));
  boost::math::chi_squared mydist(K - 1);

  std::vector<double> loc(K - 1);
  for(size_t i = 0; i < loc.size(); i++)
    loc[i] = quantile(dist, (i + 1.0) / K);

  std::vector<int> bin(K, 0);
  std::vector<double> expect(K);
  for (int i = 0 ; i < K; i++)
    expect[i] = N / K;

  for (int count = 0; count < N; ++count) {
    VectorXd a = stan::math::dirichlet_rng(alpha,rng);
    int i = 0;
    while (i < K-1 && a(1) > loc[i])
      ++i;
    ++bin[i];
   }

  EXPECT_TRUE(chi_square(bin, expect) < quantile(complement(mydist, 1e-6)));
}

TEST(ProbDistributionsDirichlet, rngTest) {
  VectorXd alpha(3);
  alpha << 2.0, 3.0, 11.0;
  test_dirichlet3_1(alpha);
  test_dirichlet3_2(alpha);

  VectorXd beta(3);
  beta << 0.1, 0.01, 0.2;
  test_dirichlet3_1(beta);
  test_dirichlet3_2(beta);
}

TEST(ProbDistributionsDirichlet, random) {
  boost::random::mt19937 rng;
  VectorXd alpha(3);
  alpha << 2.0, 3.0, 11.0;
  EXPECT_NO_THROW(stan::math::dirichlet_rng(alpha, rng));

  VectorXd beta(3);
  beta << 0.001, 0.0001, 1e-10;
  EXPECT_NO_THROW(stan::math::dirichlet_rng(beta, rng));
}


#include <boost/graph/adjacency_matrix.hpp>
#include <array>
#include <utility>

int main()
{
  enum { topLeft, topRight, bottomRight, bottomLeft };

  std::array<std::pair<int, int>, 4> edges{{
    std::make_pair(topLeft, topRight),
    std::make_pair(topRight, bottomRight),
    std::make_pair(bottomRight, bottomLeft),
    std::make_pair(bottomLeft, topLeft)
  }};

  typedef boost::adjacency_matrix<boost::undirectedS> graph;
  graph g{edges.begin(), edges.end(), 4};
}


/// @file raster_example1.cc
/// @brief raster example
/// @author Jeff Perry <jeffsp@gmail.com>
/// @version 1.0
/// @date 2013-01-14

#include "jack_rabbit/jack_rabbit.h"
#include <algorithm>
#include <boost/lambda/lambda.hpp>
#include <cmath>
#include <complex>
#include <fstream>
#include <iostream>

using namespace boost::lambda;
using namespace jack_rabbit;
using namespace std;

// Affine transformation helper
class Affine
{
    public:
    Affine ()
        : t_ (0), s_ (1)
    { }
    // Translate by 't' and scale by 's'
    Affine (float t, float s)
        : t_ (t), s_ (s)
    { }
    float To (size_t x) const
    { return t_ + x * s_; }
    size_t From (float x) const
    { return static_cast<int> ((x - t_) / s_); }
    private:
    float t_, s_;
};

// Mandelbrot function object:
// Map the Mandelbrot set onto a raster
template<typename T>
class Mandelbrot
{
    public:
    Mandelbrot (size_t /*r*/, size_t /*c*/) { }
    // Define coordinate mapping
    void CoordMap (const Affine &ax, const Affine &ay)
    { ax_ = ax; ay_ = ay; }
    // Remap coordinate (r, c), and then determine if it's
    // in the set or not.
    T operator() (size_t r, size_t c) const
    {
        // Remap x and y
        complex<float> z (ax_.To (c), ay_.To (r));
        complex<float> k (z);
        size_t count = 0;
        // Iterate until we are kindof sure that the point
        // is in the set, or until the point is sure to
        // shoot off to infinity...
        while (++count != ITER)
        {
            z = z * z + k;
            if (norm (z) > 4.0)
                // ... definitely not in the set
                return OUT;
        }
        // ... probably in the set.
        return IN;
    }
    private:
    static const size_t ITER = 500;
    static const T IN = 0;
    static const T OUT = 255;
    Affine ax_, ay_;
};

// PGM file writer helper
template<class T>
void WritePGM (const T &m, ofstream &ofs)
{
    // Write a pgm header
    ofs << "P5\n"
        << "# Raster Example\n"
        << m.cols () << ' ' << m.rows () << '\n'
        << "255\n";

    // Transform to a vector of chars
    vector<char> mm (m.begin (), m.end ());

    // Write the pgm pixels
    const std::streamsize sz =
        static_cast<std::streamsize> (mm.size ());
    ofs.write (&mm[0], sz);
}

int main ()
{
    try
    {
        const size_t M = 256;
        const size_t N = 256;

        clog << "Generating a " << M << "X" << N
            << " image..." << endl;

        typedef raster<int> Image;

        // Some matrices that will contain our images
        Image m0 (M, N);
        Image m1 (M / 3, N / 3);

        // Our Mandelbrot functions
        subscript_generator<Image::value_type,Mandelbrot>
        mandel0 (m0.rows (), m0.cols ());
        Affine ax0 (-2.2f, 3.0f / m0.rows ());
        Affine ay0 (-1.7f, 3.0f / m0.rows ());
        mandel0.CoordMap (ax0, ay0);

        subscript_generator<Image::value_type,Mandelbrot>
        mandel1 (m1.rows (), m1.cols ());
        Affine ax1 (-0.28f, 0.30f / m1.rows ());
        Affine ay1 (-0.92f, 0.30f / m1.rows ());
        mandel1.CoordMap (ax1, ay1);

        // Create the main image
        generate (m0.begin (), m0.end (), mandel0);

        // Lower contrast of the region to zoom
        size_t r1 = ay0.From (ay1.To (0));
        size_t c1 = ax0.From (ax1.To (0));
        size_t r2 = ay0.From (ay1.To (m1.rows ()));
        size_t c2 = ax0.From (ax1.To (m1.cols ()));
        subregion s0 = m0.sub (
            static_cast<int> (r1), static_cast<int> (c1),
            r2 - r1, c2 - c1);
        for_each (m0.begin (s0), m0.end (s0),
            _1 = (_1 - 127) / 2 + 127);

        // Create the zoomed image
        generate (m1.begin (), m1.end (), mandel1);
        // Copy the zoomed image to the main image...
        subregion s1 = { m0.rows () / 9, m0.rows () / 9,
            m1.rows (), m1.cols () };
        copy (m1.begin (), m1.end (), m0.begin (s1));

        // Lower contrast of the zoomed image
        transform (m0.begin (s1), m0.end (s1), m0.begin (s1),
            (_1 - 127) / 2 + 127);

        // Save the image to a file
        const string fn ("raster_example1.pgm");
        clog << "Writing image to " << fn << "..." << endl;

        ofstream ofs (fn.c_str ());
        if (!ofs)
            throw std::runtime_error ("Could not open file for writing");

        WritePGM (m0, ofs);

        return 0;
    }
    catch (const exception &e)
    {
        cerr << e.what () << endl;
        return -1;
    }
}


#include <test/unit/math/prim/prob/hmm_util.hpp>
#include <stan/math/prim/prob/hmm_hidden_state_prob.hpp>
#include <boost/math/distributions.hpp>
#include <boost/random.hpp>
#include <test/unit/math/test_ad.hpp>
#include <test/unit/util.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <vector>

TEST_F(hmm_test, hidden_state_single_outcome) {
  using stan::math::hmm_hidden_state_prob;

  int n_states = 2;
  Eigen::MatrixXd Gamma(n_states, n_states);
  Gamma << 1, 0, 1, 0;
  Eigen::VectorXd rho(n_states);
  rho << 1, 0;

  Eigen::MatrixXd prob = hmm_hidden_state_prob(log_omegas_, Gamma, rho);

  for (int i = 0; i < n_transitions_; i++) {
    EXPECT_EQ(prob(0, i), 1);
    EXPECT_EQ(prob(1, i), 0);
  }
}

TEST_F(hmm_test, hidden_state_identity_transition) {
  // With an identity transition matrix, all latent probabilities
  // are equal. Setting the log density to 1 for all states makes
  // the initial prob drive the subsequent probabilities.
  using stan::math::hmm_hidden_state_prob;
  int n_states = 2;
  Eigen::MatrixXd Gamma = Eigen::MatrixXd::Identity(n_states, n_states);
  Eigen::MatrixXd log_omegas
      = Eigen::MatrixXd::Ones(n_states, n_transitions_ + 1);

  Eigen::MatrixXd prob = hmm_hidden_state_prob(log_omegas, Gamma, rho_);

  for (int i = 0; i < n_transitions_; i++) {
    EXPECT_FLOAT_EQ(prob(0, i), rho_(0));
    EXPECT_FLOAT_EQ(prob(1, i), rho_(1));
  }
}

TEST(hmm_test_nonstandard, hidden_state_symmetry) {
  // In this two states situation, the latent states are
  // symmetric, based on the observational log density,
  // and transition matrix.
  // The initial conditions introduces an asymmetry in the first
  // state. The other hidden states all have probability 0.5.
  using stan::math::hmm_hidden_state_prob;
  int n_states = 2;
  int n_transitions = 2;
  Eigen::MatrixXd Gamma(n_states, n_states);
  Gamma << 0.5, 0.5, 0.5, 0.5;
  Eigen::VectorXd rho(n_states);
  rho << 0.3, 0.7;
  Eigen::MatrixXd log_omegas
      = Eigen::MatrixXd::Ones(n_states, n_transitions + 1);

  Eigen::MatrixXd prob = hmm_hidden_state_prob(log_omegas, Gamma, rho);

  EXPECT_FLOAT_EQ(prob(0, 0), 0.3);
  EXPECT_FLOAT_EQ(prob(1, 0), 0.7);

  for (int i = 1; i < n_transitions; i++) {
    EXPECT_FLOAT_EQ(prob(0, i), 0.5);
    EXPECT_FLOAT_EQ(prob(1, i), 0.5);
  }
}


/*
 * Tools.cpp
 *
 *  Created on: Apr 22, 2014
 *      Author: wilfeli
 */

#include <vector>
#include <math.h>
#include <algorithm>
#include <iterator>
#include <Eigen/Dense>
#include <iostream>
#include "Tools.h"

using Eigen::MatrixXd;
using std::vector;

namespace Tools{

void
mgrid(MatrixXd grid, MatrixXd* meshgrid){
	//creates meshgrid from vector of points
	//define number of steps for each input
	long N_rows = grid.rows();
	int n_steps_i;
	double n_steps_i_int;
	int j_val;

	double val;
	vector<int> n_steps;
	vector<int> ac_n_steps;
	vector<vector<double>> values;
	vector<double> values_temp;
	ac_n_steps.push_back(1);

    //create list of values
	for (int i=0; i < N_rows; i++){
		//n_steps
		n_steps_i_int = (grid(i,1) - grid(i,0))/grid(i,2);

		n_steps_i = static_cast<int>(floor(n_steps_i_int)) + 1;
		n_steps.push_back(n_steps_i);
		ac_n_steps.push_back(ac_n_steps.back() * n_steps_i);

		for (int j=0; j<n_steps_i; j++){
			val = grid(i,0) + grid(i,2)*j;
			values_temp.push_back(val);
		};

		values.push_back(values_temp);
		values_temp.clear();
	};

	//create matrix and fill it with values
	(*meshgrid) = MatrixXd::Zero(N_rows, ac_n_steps.back());

	int step_length = ac_n_steps.back();
	for (int i = 0; i < N_rows; i++){
		step_length = step_length / n_steps[i];
		for (int j = 0; j < ac_n_steps.back(); j++){
			j_val = ((int)(j/step_length))%n_steps[i];
			(*meshgrid)(i,j) = values[i][j_val];

		};
	};

//	std::cout << (*meshgrid).transpose() << "\n";

};

void
mgrid_test(MatrixXd grid, MatrixXd* meshgrid){
    //creates meshgrid from vector of points
    //reverse to correspond to python
    long N_rows = grid.rows();
    int n_steps_i;
    double n_steps_i_int;
    int j_val;

    double val;
    vector<int> n_steps;
    vector<int> ac_n_steps;
    vector<vector<double>> values;
    vector<double> values_temp;
    ac_n_steps.push_back(1);

    for (int i=0; i < N_rows; i++){
        //n_steps
        n_steps_i_int = (grid(i,1) - grid(i,0))/grid(i,2);

        n_steps_i = static_cast<int>(floor(n_steps_i_int)) + 1;
        n_steps.push_back(n_steps_i);
        ac_n_steps.push_back(ac_n_steps.back() * n_steps_i);

        for (int j=0; j<n_steps_i; j++){
            val = grid(i,0) + grid(i,2)*j;
            values_temp.push_back(val);
        };

        values.push_back(values_temp);
        values_temp.clear();
    };

    //create matrix and fill it with values
    (*meshgrid) = MatrixXd::Zero(N_rows, ac_n_steps.back());

    int step_length = 1;
    for (int i = 0; i < N_rows; i++){

        for (int j = 0; j < ac_n_steps.back(); j++){
            j_val = ((int)(j/step_length))%n_steps[i];
            (*meshgrid)(i,j) = values[i][j_val];

        };
        step_length = step_length * n_steps[i];
    };

//    std::cout << (*meshgrid).transpose() << "\n";

};

void
print_vector(std::vector<double> vec){
	//print all q
	for (auto iter:vec){
		std::cout << iter << " ";
	};

    std::cout << std::endl;
};

void
print_vector(std::vector<std::vector<double>> vec){
    //print all q
    for (auto iter:vec){
        print_vector(iter);
    };

};

MyRNG::MyRNG(double seed_):state(seed_){
    m_w = seed_;
};

// Produce a uniform random sample from the open interval (0, 1).
// The method will not return either end point.
double
MyRNG::GetUniform(){
    // 0 <= u < 2^32
	uint64_t u = GetUint();
    // The magic number below is 1/(2^32 + 2).
    // The result is strictly between 0 and 1.
    return (u + 1.0) * 2.328306435454494e-10;
};

// This is the heart of the generator.
// It uses George Marsaglia's MWC algorithm to produce an unsigned integer.
// See http://www.bobwheeler.com/statistics/Password/MarsagliaPost.txt

uint64_t
MyRNG::GetUint(){
    m_z = 36969 * (m_z & 65535) + (m_z >> 16);
    m_w = 18000 * (m_w & 65535) + (m_w >> 16);
    return (m_z << 16) + m_w;
};

double
get_normal(double mean, double sigma, MyRNG& rng){
    if(rng.hasSpare)
	{
		rng.hasSpare = false;
		return sigma * sqrt(rng.rn1) * sin(rng.rn2) + mean;
	}

	rng.hasSpare = true;

	rng.rn1 = rng.GetUniform();
	if(rng.rn1 < 1e-100) {rng.rn1 = 1e-100;};
	rng.rn1 = -2 * log(rng.rn1);
	rng.rn2 = rng.GetUniform() * M_PI * 2;

	return sigma*sqrt(rng.rn1) * cos(rng.rn2) + mean;

};

int
get_int(int min, int max, MyRNG& rng){
	uint64_t x_raw = rng.GetUint();
    //clean state of rng to simplify code
    rng.hasSpare = false;

    int x = min + (x_raw % (int)(max - min + 1));

    return x;

};

};


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/Assign2D.hpp"
#include "../util/DistanceFuncs.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../util/Munkres.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>

namespace fluid {
namespace algorithm {

class Grid
{
public:
  using MatrixXd = Eigen::MatrixXd;
  using VectorXd = Eigen::VectorXd;
  using DataSet = FluidDataSet<std::string, double, 1>;

  DataSet process(DataSet& in, index overSample = 1, index extent = 0,
                  index axis = 0)
  {
    using namespace Eigen;
    using namespace _impl;
    using namespace std;

    assert(in.dims() == 2);
    index    N = in.size();
    index    M = N * overSample;
    ArrayXXd data = asEigen<Array>(in.getData());
    double   xMin = data.col(0).minCoeff();
    double   xMax = data.col(0).maxCoeff();
    double   yMin = data.col(1).minCoeff();
    double   yMax = data.col(1).maxCoeff();
    double   area = (xMax - xMin) * (yMax - yMin);
    double   size = static_cast<double>(N);
    double   step = sqrt(area / M);
    index    numCols, numRows;

    if (area <= 0) return DataSet();

    if (extent > 0)
    {
      numCols = (axis == 0) ? extent : lrint(ceil(size / extent));
      numRows = (axis == 1) ? extent : lrint(ceil(size / extent));
    }
    else
    {
      numCols = lrint(ceil((xMax - xMin) / step));
      numRows = lrint(ceil((yMax - yMin) / step));
    }

    ArrayXd colPos, rowPos;
    if (extent > 0 && axis == 1)
    {
      rowPos = ArrayXidx::LinSpaced(M, 0, M - 1)
                   .unaryExpr([&](const index x) { return x % numRows; })
                   .cast<double>();
      colPos = (ArrayXidx::LinSpaced(M, 0, M - 1) / numRows).cast<double>();
    }
    else
    {
      colPos = ArrayXidx::LinSpaced(M, 0, M - 1)
                   .unaryExpr([&](const index x) { return x % numCols; })
                   .cast<double>();
      rowPos = (ArrayXidx::LinSpaced(M, 0, M - 1) / numCols).cast<double>();
    }

    ArrayXd  xPos = xMin + (colPos / (numCols - 1)) * (xMax - xMin);
    ArrayXd  yPos = yMin + (rowPos / (numRows - 1)) * (yMax - yMin);
    ArrayXXd grid(M, 2);
    grid << xPos, yPos;
    ArrayXXd cost = algorithm::DistanceMatrix<ArrayXXd>(data, grid, 1);
    ArrayXidx  assignment(N);
    bool     outcome = assign2D.process(cost, assignment);
    if (!outcome) return DataSet();

    DataSet    result(2);
    auto       ids = in.getIds();
    RealVector asignedPos(2);
    for (index i = 0; i < N; i++)
    {
      asignedPos(0) = colPos(assignment(i));
      asignedPos(1) = rowPos(assignment(i));
      auto id = ids(i);
      result.add(ids(i), asignedPos);
    }
    return result;
  }

private:
  Assign2D assign2D;
};
}// namespace algorithm
}// namespace fluid


/******************************************************************************

  This source file is part of the Avogadro project.

  Copyright (C) 2010 Eric C. Brown

  This source code is released under the New BSD License, (the "License").

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.

******************************************************************************/

#include "qtaimmathutilities.h"

#include <cmath>
#include <Eigen/Eigenvalues>
#include <Eigen/QR>

namespace Avogadro {
namespace QtPlugins {
namespace QTAIMMathUtilities {

  Matrix<qreal,3,1> eigenvaluesOfASymmetricThreeByThreeMatrix(const Matrix<qreal,3,3> &A)
  {
    SelfAdjointEigenSolver<Matrix<qreal, 3, 3> > eigensolver(A);
    return eigensolver.eigenvalues();
  }

  Matrix<qreal,3,3> eigenvectorsOfASymmetricThreeByThreeMatrix(const Matrix<qreal,3,3> &A)
  {
    SelfAdjointEigenSolver<Matrix<qreal, 3, 3> > eigensolver(A);
    return eigensolver.eigenvectors();
  }

  Matrix<qreal,4,1> eigenvaluesOfASymmetricFourByFourMatrix(const Matrix<qreal,4,4> &A)
  {
    SelfAdjointEigenSolver<Matrix<qreal, 4, 4> > eigensolver(A);
    return eigensolver.eigenvalues();
  }

  Matrix<qreal,4,4> eigenvectorsOfASymmetricFourByFourMatrix(const Matrix<qreal,4,4> &A)
  {
    SelfAdjointEigenSolver<Matrix<qreal, 4, 4> > eigensolver(A);
    return eigensolver.eigenvectors();
  }

  qint64 signOfARealNumber(qreal x)
  {
    if (x > 0.)
      return  1;
    else if (x == 0.)
      return  0;
    else
      return -1;
  }

  qint64 signatureOfASymmetricThreeByThreeMatrix(const Matrix<qreal,3,3> &A)
  {
    SelfAdjointEigenSolver<Matrix<qreal, 3, 3> > eigensolver(A);
    Matrix<qreal,3,1> eigenvalues=eigensolver.eigenvalues();

    return signOfARealNumber(eigenvalues(0)) +
        signOfARealNumber(eigenvalues(1)) +
        signOfARealNumber(eigenvalues(2));
  }

  qreal ellipticityOfASymmetricThreeByThreeMatrix(const Matrix<qreal,3,3> &A)
  {
    SelfAdjointEigenSolver<Matrix<qreal, 3, 3> > eigensolver(A);
    Matrix<qreal, 3, 1> eigenvalues=eigensolver.eigenvalues();

    return (eigenvalues(0) / eigenvalues(1)) - 1.0 ;
  }

  qreal distance(const Matrix<qreal, 3, 1> &a, const Matrix<qreal, 3, 1> &b)
  {
    return sqrt(pow(a(0) - b(0), 2) +
                pow(a(1) - b(1), 2) +
                pow(a(2) - b(2), 2));
  }

  Matrix<qreal,3,1> sphericalToCartesian(const Matrix<qreal,3,1> &rtp,
                                         const Matrix<qreal,3,1> &x0y0z0)
  {
    qreal r=rtp(0);
    qreal theta=rtp(1);
    qreal phi=rtp(2);

    qreal x0=x0y0z0(0);
    qreal y0=x0y0z0(1);
    qreal z0=x0y0z0(2);

    qreal costheta = cos(theta);
    qreal cosphi   = cos(phi);
    qreal sintheta = sin(theta);
    qreal sinphi   = sin(phi);

    Matrix<qreal, 3, 1> xyz(r * cosphi * sintheta + x0,
                            r * sintheta * sinphi + y0,
                            r * costheta          + z0);

    return xyz;
  }

  Matrix<qreal, 3, 1> sphericalToCartesian(const Matrix<qreal, 3, 1> &rtp)
  {
    Matrix<qreal, 3, 1> x0y0z0(0., 0., 0.);

    return  sphericalToCartesian(rtp, x0y0z0);
  }

  Matrix<qreal,3,1> cartesianToSpherical(const Matrix<qreal, 3, 1> &xyz,
                                         const Matrix<qreal, 3, 1> &x0y0z0 )
  {
    qreal x=xyz(0);
    qreal y=xyz(1);
    qreal z=xyz(2);

    qreal x0=x0y0z0(0);
    qreal y0=x0y0z0(1);
    qreal z0=x0y0z0(2);

    qreal xshift = x - x0;
    qreal yshift = y - y0;
    qreal zshift = z - z0;

    qreal length = sqrt(pow(xshift, 2) + pow(yshift, 2) + pow(zshift, 2));

    Matrix<qreal, 3, 1> rtp;

    if (length == 0.)
      rtp << x0, y0, z0 ;
    else if (xshift == 0. && yshift == 0.)
      rtp << length, acos(zshift / length), 0.;
    else
      rtp << length, acos(zshift / length), atan2(xshift, yshift);

    return rtp;
  }

  Matrix<qreal, 3, 1> cartesianToSpherical(const Matrix<qreal, 3, 1> &xyz)
  {
    Matrix<qreal, 3, 1> x0y0z0(0., 0., 0.);

    return  cartesianToSpherical(xyz, x0y0z0);
  }

  // Cerjan-Miller-Baker-Popelier Methods
  //
  // Based on:
  // Popelier, P.L.A. Comput. Phys. Comm. 1996, 93, 212.

  Matrix<qreal, 3, 1> minusThreeSignatureLocatorGradient(const Matrix<qreal, 3, 1> &g,
                                                         const Matrix<qreal, 3, 3> &H)
  {
    Matrix<qreal, 3, 1> value;

    Matrix<qreal, 3, 1> b = eigenvaluesOfASymmetricThreeByThreeMatrix(H);
    Matrix<qreal, 3, 3> U = eigenvectorsOfASymmetricThreeByThreeMatrix(H);

    Matrix<qreal, 3, 1> F = U.transpose() * g;

    Matrix<qreal, 4, 4> A;
    A <<  b(0), 0.  , 0.  , F(0),
    0.  , b(1), 0.  , F(1),
    0.  , 0.  , b(2), F(2),
    F(0), F(1), F(2), 0.   ;

    Matrix<qreal, 4, 1> eval = eigenvaluesOfASymmetricFourByFourMatrix(A);

    Matrix<qreal, 3, 1> lambda;
    lambda << eval(3), eval(3), eval(3);

    Matrix<qreal, 3, 1> denom;
    denom = b - lambda;

    for (qint64 i=0; i < 3; ++i)
      if( denom(i) < SMALL )
        denom(i)=denom(i)+SMALL;

    Matrix<qreal, 3, 1> h;
    h << 0., 0., 0.;

    for (qint64 j = 0; j < 3; ++j)
      for (qint64 i = 0; i < 3; ++i)
        h(j) = h(j) + ( -F(i) * U(j, i) ) / denom(i);

    value = h;

    return value;
  }

  Matrix<qreal, 3, 1> minusOneSignatureLocatorGradient(const Matrix<qreal, 3, 1> &g,
                                                       const Matrix<qreal,3,3> &H)
  {
    Matrix<qreal, 3, 1> value;

    Matrix<qreal, 3, 1> b = eigenvaluesOfASymmetricThreeByThreeMatrix(H);
    Matrix<qreal, 3, 3> U = eigenvectorsOfASymmetricThreeByThreeMatrix(H);

    Matrix<qreal, 3, 1> F = U.transpose() * g;

    Matrix<qreal, 3, 3> A;
    A <<  b(0), 0.  ,  F(0),
    0.  , b(1),  F(1),
    F(0), F(1),  0.   ;

    Matrix<qreal, 3, 1> eval = eigenvaluesOfASymmetricThreeByThreeMatrix(A);

    Matrix<qreal, 3, 1> lambda;
    lambda << eval(2), eval(2), (0.5) * (b(2) - sqrt(pow(b(2), 2) + 4.0 * pow(F(2), 2)));

    Matrix<qreal, 3, 1> denom;
    denom = b - lambda;

    for (qint64 i = 0; i < 3; ++i)
      if (denom(i) < SMALL)
        denom(i) = denom(i) + SMALL;

    Matrix<qreal, 3, 1> h;
    h << 0., 0., 0.;

    for (qint64 j = 0; j < 3; ++j)
      for (qint64 i = 0; i < 3; ++i)
        h(j) = h(j) + (-F(i) * U(j,i)) / denom(i);

    value = h;

    return value;
  }

  Matrix<qreal, 3, 1> plusOneSignatureLocatorGradient(const Matrix<qreal, 3, 1> &g,
                                                      const Matrix<qreal,3,3> &H)
  {
    Matrix<qreal, 3, 1> value;

    Matrix<qreal, 3, 1> b = eigenvaluesOfASymmetricThreeByThreeMatrix(H);
    Matrix<qreal, 3, 3> U = eigenvectorsOfASymmetricThreeByThreeMatrix(H);

    Matrix<qreal, 3, 1> F = U * g;

    Matrix<qreal, 3, 3> A;
    A <<  b(1), 0.  ,  F(1),
    0.  , b(2),  F(2),
    F(1), F(2),  0.;

    Matrix<qreal, 3, 1> eval = eigenvaluesOfASymmetricThreeByThreeMatrix(A);

    Matrix<qreal, 3, 1> lambda;
    lambda << eval(2), eval(2), (0.5) * (b(0) + sqrt(pow(b(0), 2) + 4.0 * pow(F(0), 2)));

    Matrix<qreal, 3, 1> denom;
    denom = b - lambda;

    for (qint64 i = 0; i < 3; ++i)
      if (denom(i) < SMALL)
        denom(i) = denom(i) + SMALL;

    Matrix<qreal, 3, 1> h;
    h << 0., 0., 0.;

    for (qint64 j = 0; j < 3; ++j)
      for (qint64 i = 0; i < 3; ++i)
        h(j) = h(j) + (-F(i) * U(i, j)) / denom(i);

    value = h;

    return value;
  }

  Matrix<qreal,3,1> plusThreeSignatureLocatorGradient(const Matrix<qreal, 3, 1> &g,
                                                      const Matrix<qreal, 3, 3> &H)
  {
    Matrix<qreal, 3, 1> value;

    Matrix<qreal, 3, 1> b = eigenvaluesOfASymmetricThreeByThreeMatrix(H);
    Matrix<qreal, 3, 3> U = eigenvectorsOfASymmetricThreeByThreeMatrix(H);

    Matrix<qreal, 3, 1> F = U * g;

    Matrix<qreal, 4, 4> A;
    A <<  b(0), 0.  , 0.  , F(0),
    0.  , b(1), 0.  , F(1),
    0.  , 0.  , b(2), F(2),
    F(0), F(1), F(2), 0.;

    Matrix<qreal, 4, 1> eval = eigenvaluesOfASymmetricFourByFourMatrix(A);

    Matrix<qreal, 3, 1> lambda;
    lambda << eval(0), eval(0), eval(0);

    Matrix<qreal, 3, 1> denom;
    denom = b - lambda;

    for (qint64 i = 0; i < 3; ++i)
      if (denom(i) < SMALL)
        denom(i) = denom(i) + SMALL;

    Matrix<qreal, 3, 1> h;
    h << 0., 0., 0.;

    for (qint64 j = 0; j < 3; ++j)
      for (qint64 i = 0; i < 3; ++i)
        h(j) = h(j) + (-F(i) * U(i, j) ) / denom(i);

    value = h;

    return value;
  }

} // namespace QTAIMMathUtilities
} // namespace QtPlugins
} // namespace Avogadro


/*
 * Modulo_test.cpp
 *
 *  Created on: Sep 29, 2018
 *      Author: sergejkrivonos
 */
#define BOOST_TEST_MODULE Modulo test
#include <boost/test/unit_test.hpp>
#include <omnn/math/Modulo.h>
#include <omnn/math/Variable.h>

using namespace omnn::math;
using namespace boost::unit_test;

BOOST_AUTO_TEST_CASE(Modulo_test)
{
    Variable va;
    auto _ = (va + 5) / (va + 1);
    auto p = (va + 5) % (va + 1);
    BOOST_TEST(p == (4_v % (va + 1)));
    p.Eval(va, 2);
    p.optimize();
    BOOST_TEST(7_v % 3 == p);
}


#include <cassert>
#include <cmath>
#include <complex>
#include <vector>
#include <Eigen/Core>

#include "Const.h"
#include "Tract.h"

using namespace std;
using namespace Eigen;

Tract::Tract(double c1, double dl, const double S[], int length) :
	a	(130 * Const::pi),
	b	(std::pow(30 * Const::pi, 2)),
	om0	(pow(406 * Const::pi, 2)),
	c1	(c1),
	dl	(dl),
	S	(length)
{
	for (int i = 0; i < length; i++)
		this->S[i] = S[i];
}
Tract::Tract(double c1, double dl, vector<double> &S) :
	a	(130 * Const::pi),
	b	(std::pow(30 * Const::pi, 2)),
	om0	(pow(406 * Const::pi, 2)),
	c1	(c1),
	dl	(dl),
	S	(S)
{
}

Tract::~Tract(void)
{
}

complex<double> Tract::GetBeta(double f)
{
	double omg = 2.0 * Const::pi * f;
	complex<double> s(0, omg);
	return s * om0 / ((s + a) * s + b) + sqrt(s * c1);
}

vector<double> Tract::GetArea(void)
{
	return S;
}

double Tract::GetStartArea(void)
{
	return S.front();
}

double Tract::GetEndArea(void)
{
	return S.back();
}

double Tract::GetElemLength(void)
{
	return dl;
}

Matrix2cd Tract::ChainMatrix(double freq, int from)
{
	return ChainMatrix(freq, from, S.size() - 1);
}
Matrix2cd Tract::ChainMatrix(double freq, int from, int to)
{
	double n = to - from + 1;

	double omg = 2.0 * Const::pi * freq;
	complex<double> s(0, omg);

	complex<double> loss = s * om0 / ((s + a) * s + b) + sqrt(s * c1);
	complex<double> gamma = sqrt((a + s) / (loss + s));
	complex<double> sigma = gamma * (loss + s) / Const::c;

	Matrix2cd r = Matrix2cd::Identity();
	complex<double> sh = sinh(sigma * dl);
	Matrix2cd K;
    K(0, 0) = K(1, 1) = cosh(sigma * dl);

    for (int i = 0; i < n; i++)
    {
        complex<double> coeff = -Const::rho * Const::c / S[from + i] * gamma;

		K(0, 1) = coeff * sh;
        K(1, 0) = 1.0 / coeff * sh;

        r = K * r;
    }

    return r;
}


// solving A * X = B
// using driver function gesv()
// with c_vector<> & c_matrix<>

#include <cstddef>
#include <iostream>
#include <boost/numeric/bindings/lapack/driver/gesv.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace ublas = boost::numeric::ublas;
namespace bindings = boost::numeric::bindings;
namespace lapack = boost::numeric::bindings::lapack;

using std::size_t;
using std::cout;
using std::endl;

typedef ublas::c_matrix<double, 4, 4> m4x4_t;
typedef ublas::c_matrix<double, 3, 3> m3x3_t;
typedef ublas::c_matrix<double, 2, 3> mrhs_t;
typedef ublas::c_vector<double, 5> v5_t;

int main() {

  cout << endl;
  size_t n = 3;

  m4x4_t a (n, n);   // system matrix
  m4x4_t a_copy (n, n);   // system matrix

//       [,1] [,2] [,3]
//  [1,]    1    1    1
//  [2,]    2    3    1
//  [3,]    1   -1   -1

  a(0,0) = 1.; a(0,1) = 1.; a(0,2) = 1.;
  a(1,0) = 2.; a(1,1) = 3.; a(1,2) = 1.;
  a(2,0) = 1.; a(2,1) = -1.; a(2,2) = -1.;
  a_copy = a;

  mrhs_t b (2, n);  // right-hand side matrix

//          [,1] [,2]
//     [1,]    4   10
//     [2,]    9   11
//     [3,]    2   12

  b(0,0) =  4.;  b(1,0) = 10.;
  b(0,1) =  9.;  b(1,1) = 11.;
  b(0,2) = -2.;  b(1,2) = 12.;

  m3x3_t a2; // for part 2
  a2 = project (a, ublas::range (0,3), ublas::range (0,3));
  v5_t b2 (n);
  b2 = row (b, 0);

  // part 1:
  cout << "A: " << a << endl;
  cout << "B: " << ublas::trans(b) << endl;

  std::vector< int > pivota( 100 );
  lapack::gesv (a, pivota, bindings::trans(b));

  cout << "X: " << ublas::trans(b) << endl;
  std::cout << "---" << std::endl;
  cout << "A: " << a << endl;

  cout << "AX/B: " << ublas::prod( a_copy, ublas::trans(b) ) << std::endl;

  cout << endl;

  // part 2:
  cout << "A: " << a2 << endl;
  cout << "B: " << b2 << endl;

  std::vector< int > pivota2( 100 );
 lapack::gesv (a2, pivota2, b2);
  cout << "X: " << b2 << endl;

  cout << endl;
}


#pragma once

// Code adapted from https://github.com/propanoid/DBSCAN

#include <vector>
#include <algorithm>
//#include <omp.h>

// Any basic vector/matrix library should also work
#include <Eigen/Core>

namespace clustering
{
	template<typename Vector, typename Matrix>
	class DBSCAN
	{
	public:
		typedef Vector FeaturesWeights;
		typedef Matrix ClusterData;
		typedef Matrix DistanceMatrix;
		typedef std::vector<unsigned int> Neighbors;
		typedef std::vector<int> Labels;

	private:
		double m_eps;
		size_t m_min_elems;
		double m_dmin;
		double m_dmax;

		Labels m_labels;

	public:

		// 'eps' is the search space for neighbors in the range [0,1], where 0.0 is exactly self and 1.0 is entire dataset
		DBSCAN(double eps, size_t min_elems)
			: m_eps( eps )
			, m_min_elems( min_elems )
			, m_dmin(0.0)
			, m_dmax(0.0)
		{
			reset();
		}

		// Call this to perform clustering, get results by calling 'get_labels()'
		void fit( const ClusterData & C )
		{
			const FeaturesWeights W = std_weights( C.cols() );
			wfit( C, W );
		}

		const Labels & get_labels() const
		{
			return m_labels;
		}

		void reset()
		{
			m_labels.clear();
		}

		void init(double eps, size_t min_elems)
		{
			m_eps = eps;
			m_min_elems = min_elems;
		}

		// Useful for testing
		static ClusterData gen_cluster_data( size_t features_num, size_t elements_num )
		{
			ClusterData cl_d( elements_num, features_num );
			for (size_t i = 0; i < elements_num; ++i)
				for (size_t j = 0; j < features_num; ++j)
					cl_d(i, j) = (-1.0 + rand() * (2.0) / RAND_MAX);
			return cl_d;
		}

		FeaturesWeights std_weights( size_t s )
		{
			// num cols
			FeaturesWeights ws( s );

			for (size_t i = 0; i < s; ++i)
				ws(i) = 1.0;

			return ws;
		}

		void fit_precomputed( const DistanceMatrix & D )
		{
			prepare_labels( D.rows() );
			dbscan( D );
		}

		void wfit( const ClusterData & C, const FeaturesWeights & W )
		{
			prepare_labels( C.rows() );
			const DistanceMatrix D = calc_dist_matrix( C, W );
			dbscan( D );
		}

	private:
		void prepare_labels( size_t s )
		{
			m_labels.resize(s, -1);
		}

		Neighbors find_neighbors(const DistanceMatrix & D, unsigned int pid)
		{
			Neighbors ne;

			for (unsigned int j = 0; j < D.rows(); ++j)
			{
				if 	( D(pid, j) <= m_eps )
				{
					ne.push_back(j);
				}
			}
			return ne;
		}

		const DistanceMatrix calc_dist_matrix( const ClusterData & C, const FeaturesWeights & W )
		{
			ClusterData cl_d = C;

//#pragma omp parallel for
			for (int i = 0; i < (int)cl_d.cols(); ++i)
			{
				auto col = cl_d.col(i);

				const auto r = std::minmax_element( col.data(), col.data() + col.size() );

				double data_min = *r.first;
				double data_range = *r.second - *r.first;

				if (data_range == 0.0) { data_range = 1.0; }

				const double scale = 1/data_range;
				const double min = -1.0*data_min*scale;

				col *= scale;
				col += Vector::Constant(col.size(), min);

				cl_d.col(i) = col;
			}

			// rows x rows
			DistanceMatrix d_m( cl_d.rows(), cl_d.rows() );
			Vector d_max( cl_d.rows() );
			Vector d_min( cl_d.rows() );

			for (int i = 0; i < (int)cl_d.rows(); ++i)
			{
//#pragma omp parallel for
				for (int j = i; j < (int)cl_d.rows(); ++j)
				{
					d_m(i, j) = 0.0;

					if (i != j)
					{
						Vector U = cl_d.row(i);
						Vector V = cl_d.row(j);

						Vector diff = ( U-V );

						for(int k = 0; k < (int)diff.size(); k++)
						{
							auto e = diff[k];
							d_m(i, j) += fabs(e)*W[k];
						}

						d_m(j, i) = d_m(i, j);
					}
				}

				const auto cur_row = d_m.row(i);
				const auto mm = std::minmax_element( cur_row.data(), cur_row.data() + cur_row.size() );

				d_max(i) = *mm.second;
				d_min(i) = *mm.first;
			}

			m_dmin = *(std::min_element( d_min.data(), d_min.data() + d_min.size() ));
			m_dmax = *(std::max_element( d_max.data(), d_max.data() + d_max.size() ));

			m_eps = (m_dmax - m_dmin) * m_eps + m_dmin;

			return d_m;
		}

		void dbscan( const DistanceMatrix & dm )
		{
			std::vector<unsigned int> visited( dm.rows() );

			unsigned int cluster_id = 0;

			for (unsigned int pid = 0; pid < dm.rows(); ++pid)
			{
				if ( !visited[pid] )
				{
					visited[pid] = 1;

					Neighbors ne = find_neighbors(dm, pid );

					if (ne.size() >= m_min_elems)
					{
						m_labels[pid] = cluster_id;

						for (unsigned int i = 0; i < ne.size(); ++i)
						{
							unsigned int nPid = ne[i];

							if ( !visited[nPid] )
							{
								visited[nPid] = 1;

								Neighbors ne1 = find_neighbors(dm, nPid);

								if ( ne1.size() >= m_min_elems )
								{
									for (const auto & n1 : ne1)
									{
										ne.push_back(n1);
									}
								}
							}

							if ( m_labels[nPid] == -1 )
							{
								m_labels[nPid] = cluster_id;
							}
						}

						++cluster_id;
					}
				}
			}
		}
	};
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

#include <iostream>
#include <Eigen/Eigen>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
MatrixXcf a = MatrixXcf::Random(2,2);
cout << "Here is the matrix a\n" << a << endl;

cout << "Here is the matrix a^T\n" << a.transpose() << endl;

cout << "Here is the conjugate of a\n" << a.conjugate() << endl;

cout << "Here is the matrix a^*\n" << a.adjoint() << endl;

}
  return 0;
}


// https://www.codechef.com/problems/FCTRL2

#include <bits/stdc++.h>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;

int main(){
	int t;
	cin>>t;
	while(t--){
		int n;
		cin>>n;
		cpp_int fact = 1;
		for(int i=1; i<=n; i++){
			fact *= i;
		}
		cout<<fact<<endl;
	}
}


#include <Eigen/Eigen>
#include <iostream>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  typedef Matrix<double,4,Dynamic> Matrix4Xd;
Matrix4Xd M = Matrix4Xd::Random(4,5);
Projective3d P(Matrix4d::Random());
cout << "The matrix M is:" << endl << M << endl << endl;
cout << "M.colwise().hnormalized():" << endl << M.colwise().hnormalized() << endl << endl;
cout << "P*M:" << endl << P*M << endl << endl;
cout << "(P*M).colwise().hnormalized():" << endl << (P*M).colwise().hnormalized() << endl << endl;
  return 0;
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

#include <iostream>
#include <Eigen/Eigen>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix3d m = Matrix3d::Zero();
m.triangularView<Eigen::Upper>().setOnes();
cout << "Here is the matrix m:\n" << m << endl;
Matrix3d n = Matrix3d::Ones();
n.triangularView<Eigen::Lower>() *= 2;
cout << "Here is the matrix n:\n" << n << endl;
cout << "And now here is m.inverse()*n, taking advantage of the fact that"
        " m is upper-triangular:\n"
     << m.triangularView<Eigen::Upper>().solve(n) << endl;
cout << "And this is n*m.inverse():\n"
     << m.triangularView<Eigen::Upper>().solve<Eigen::OnTheRight>(n);

  return 0;
}


#include <iostream>
#include <vector>
#include <fstream>

using namespace std;

#include <boost/timer.hpp>

// for sophus
#include <sophus/se3.hpp>

using Sophus::SE3d;

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

using namespace Eigen;

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

#include "plot.h"

using namespace cv;

/**********************************************
* 本程序演示了单目相机在已知轨迹下的稠密深度估计
* 使用极线搜索 + NCC 匹配的方式，与书本的 12.2 节对应
* 请注意本程序并不完美，你完全可以改进它——我其实在故意暴露一些问题(这是借口)。
***********************************************/
std::ofstream debug("debug.txt");
// ------------------------------------------------------------------
// parameters
const int boarder = 20;         // 边缘宽度
const int width = 640;          // 图像宽度
const int height = 480;         // 图像高度
const double fx = 481.2f;       // 相机内参
const double fy = -480.0f;
const double cx = 319.5f;
const double cy = 239.5f;
const int ncc_window_size = 3;    // NCC 取的窗口半宽度
const int ncc_area = (2 * ncc_window_size + 1) * (2 * ncc_window_size + 1); // NCC窗口面积
const double min_cov = 0.1;     // 收敛判定：最小方差
const double max_cov = 10;      // 发散判定：最大方差

// ------------------------------------------------------------------
// 重要的函数
/// 从 REMODE 数据集读取数据
bool readDatasetFiles(
    const string &path,
    vector<string> &color_image_files,
    vector<SE3d> &poses,
    cv::Mat &ref_depth
);

/**
 * 根据新的图像更新深度估计
 * @param ref           参考图像
 * @param curr          当前图像
 * @param T_C_R         参考图像到当前图像的位姿
 * @param depth         深度
 * @param depth_cov     深度方差
 * @return              是否成功
 */
void update(
    const Mat &ref,
    const Mat &curr,
    const SE3d &T_C_R,
    Mat &depth,
    Mat &depth_cov2
);

/**
 * 极线搜索
 * @param ref           参考图像
 * @param curr          当前图像
 * @param T_C_R         位姿
 * @param pt_ref        参考图像中点的位置
 * @param depth_mu      深度均值
 * @param depth_cov     深度方差
 * @param pt_curr       当前点
 * @param epipolar_direction  极线方向
 * @return              是否成功
 */
bool epipolarSearch(
    const Mat &ref,
    const Mat &curr,
    const SE3d &T_C_R,
    const Vector2d &pt_ref,
    const double &depth_mu,
    const double &depth_cov,
    Vector2d &pt_curr,
    Vector2d &epipolar_direction
);

/**
 * 更新深度滤波器
 * @param pt_ref    参考图像点
 * @param pt_curr   当前图像点
 * @param T_C_R     位姿
 * @param epipolar_direction 极线方向
 * @param depth     深度均值
 * @param depth_cov2    深度方向
 * @return          是否成功
 */
bool updateDepthFilter(
    const Vector2d &pt_ref,
    const Vector2d &pt_curr,
    const SE3d &T_C_R,
    const Vector2d &epipolar_direction,
    Mat &depth,
    Mat &depth_cov2
);

/**
 * 计算 NCC 评分
 * @param ref       参考图像
 * @param curr      当前图像
 * @param pt_ref    参考点
 * @param pt_curr   当前点
 * @return          NCC评分
 */
double NCC(const Mat &ref, const Mat &curr, const Vector2d &pt_ref, const Vector2d &pt_curr);

// 双线性灰度插值
inline double getBilinearInterpolatedValue(const Mat &img, const Vector2d &pt) {
    uchar *d = &img.data[int(pt(1, 0)) * img.step + int(pt(0, 0))];
    double xx = pt(0, 0) - floor(pt(0, 0));
    double yy = pt(1, 0) - floor(pt(1, 0));
    return ((1 - xx) * (1 - yy) * double(d[0]) +
            xx * (1 - yy) * double(d[1]) +
            (1 - xx) * yy * double(d[img.step]) +
            xx * yy * double(d[img.step + 1])) / 255.0;
}

// ------------------------------------------------------------------
// 一些小工具
// 显示估计的深度图
// void plotDepth(const Mat &depth_truth, const Mat &depth_estimate);

// 像素到相机坐标系
inline Vector3d px2cam(const Vector2d px) {
    return Vector3d(
        (px(0, 0) - cx) / fx,
        (px(1, 0) - cy) / fy,
        1
    );
}

// 相机坐标系到像素
inline Vector2d cam2px(const Vector3d p_cam) {
    return Vector2d(
        p_cam(0, 0) * fx / p_cam(2, 0) + cx,
        p_cam(1, 0) * fy / p_cam(2, 0) + cy
    );
}

// 检测一个点是否在图像边框内
inline bool inside(const Vector2d &pt) {
    return pt(0, 0) >= boarder && pt(1, 0) >= boarder
           && pt(0, 0) + boarder < width && pt(1, 0) + boarder <= height;
}

// // 显示极线匹配
// void showEpipolarMatch(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_curr);

// // 显示极线
// void showEpipolarLine(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_min_curr,
//                       const Vector2d &px_max_curr);

/// 评测深度估计
void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate);
// ------------------------------------------------------------------

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: dense_mapping path_to_test_dataset" << endl;
        return -1;
    }

    // 从数据集读取数据
    vector<string> color_image_files;
    vector<SE3d> poses_TWC;
    Mat ref_depth;
    bool ret = readDatasetFiles(argv[1], color_image_files, poses_TWC, ref_depth);
    if (ret == false) {
        cout << "Reading image files failed!" << endl;
        return -1;
    }
    cout << "read total " << color_image_files.size() << " files." << endl;

    // 第一张图
    Mat ref = imread(color_image_files[0], 0);                // gray-scale image
    SE3d pose_ref_TWC = poses_TWC[0];
    double init_depth = 3.0;    // 深度初始值
    double init_cov2 = 3.0;     // 方差初始值
    Mat depth(height, width, CV_64F, init_depth);             // 深度图
    Mat depth_cov2(height, width, CV_64F, init_cov2);         // 深度图方差

    for (int index = 1; index < color_image_files.size(); index++) {
        cout << "*** loop " << index << " ***" << endl;
        Mat curr = imread(color_image_files[index], 0);
        if (curr.data == nullptr) continue;
        SE3d pose_curr_TWC = poses_TWC[index];
        SE3d pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC;   // 坐标转换关系： T_C_W * T_W_R = T_C_R
        update(ref, curr, pose_T_C_R, depth, depth_cov2);
        evaludateDepth(ref_depth, depth);
        std::cout << "after eval depth " << std::endl;
        plotDepth(ref_depth, depth);
        // imshow("image", curr);
        // waitKey(1);
        plotCur(curr);
    }

    cout << "estimation returns, saving depth map ..." << endl;
    imwrite("depth.png", depth);
    cout << "done." << endl;

    debug.close();
    return 0;
}

bool readDatasetFiles(
    const string &path,
    vector<string> &color_image_files,
    std::vector<SE3d> &poses,
    cv::Mat &ref_depth) {
    ifstream fin(path + "/first_200_frames_traj_over_table_input_sequence.txt");
    if (!fin) return false;

    while (!fin.eof()) {
        // 数据格式：图像文件名 tx, ty, tz, qx, qy, qz, qw ，注意是 TWC 而非 TCW
        string image;
        fin >> image;
        double data[7];
        for (double &d:data) fin >> d;

        color_image_files.push_back(path + string("/images/") + image);
        poses.push_back(
            SE3d(Quaterniond(data[6], data[3], data[4], data[5]),
                 Vector3d(data[0], data[1], data[2]))
        );
        if (!fin.good()) break;
    }
    fin.close();

    // load reference depth
    fin.open(path + "/depthmaps/scene_000.depth");
    ref_depth = cv::Mat(height, width, CV_64F);
    if (!fin) return false;
    for (int y = 0; y < height; y++)
        for (int x = 0; x < width; x++) {
            double depth = 0;
            fin >> depth;
            ref_depth.ptr<double>(y)[x] = depth / 100.0;
        }

    return true;
}

// 对整个深度图进行更新
void update(const Mat &ref, const Mat &curr, const SE3d &T_C_R, Mat &depth, Mat &depth_cov2) {
    std::cout << curr.size() << "\n";
    for (int x = boarder; x < width - boarder; x++)
    {
        for (int y = boarder; y < height - boarder; y++) {
            // 遍历每个像素
            // std::cout << depth.size() << " " << depth_cov2.size() << "\n";
            if (depth_cov2.ptr<double>(y)[x] < min_cov || depth_cov2.ptr<double>(y)[x] > max_cov) // 深度已收敛或发散
                continue;
            // 在极线上搜索 (x,y) 的匹配
            Vector2d pt_curr;
            Vector2d epipolar_direction;
            bool ret = epipolarSearch(
                ref,
                curr,
                T_C_R,
                Vector2d(x, y),
                depth.ptr<double>(y)[x],
                sqrt(depth_cov2.ptr<double>(y)[x]),
                pt_curr,
                epipolar_direction
            );

            if (ret == false) // 匹配失败
                continue;

            // debug << epipolar_direction.transpose() << "\n";

            // 取消该注释以显示匹配
            // showEpipolarMatch(ref, curr, Vector2d(x, y), pt_curr);

            // 匹配成功，更新深度图
            updateDepthFilter(Vector2d(x, y), pt_curr, T_C_R, epipolar_direction, depth, depth_cov2);
        }
    }
}

// 极线搜索
// 方法见书 12.2 12.3 两节
bool epipolarSearch(
    const Mat &ref, const Mat &curr,
    const SE3d &T_C_R, const Vector2d &pt_ref,
    const double &depth_mu, const double &depth_cov,
    Vector2d &pt_curr, Vector2d &epipolar_direction) {
    Vector3d f_ref = px2cam(pt_ref);
    f_ref.normalize();
    Vector3d P_ref = f_ref * depth_mu;    // 参考帧的 P 向量

    Vector2d px_mean_curr = cam2px(T_C_R * P_ref); // 按深度均值投影的像素
    double d_min = depth_mu - 3 * depth_cov, d_max = depth_mu + 3 * depth_cov;
    if (d_min < 0.1) d_min = 0.1;
    Vector2d px_min_curr = cam2px(T_C_R * (f_ref * d_min));    // 按最小深度投影的像素
    Vector2d px_max_curr = cam2px(T_C_R * (f_ref * d_max));    // 按最大深度投影的像素

    Vector2d epipolar_line = px_max_curr - px_min_curr;    // 极线（线段形式）
    epipolar_direction = epipolar_line;        // 极线方向
    epipolar_direction.normalize();
    double half_length = 0.5 * epipolar_line.norm();    // 极线线段的半长度
    if (half_length > 100) half_length = 100;   // 我们不希望搜索太多东西

    // 取消此句注释以显示极线（线段）
    // showEpipolarLine( ref, curr, pt_ref, px_min_curr, px_max_curr );

    // 在极线上搜索，以深度均值点为中心，左右各取半长度
    double best_ncc = -1.0;
    Vector2d best_px_curr;
    for (double l = -half_length; l <= half_length; l += 0.7) { // l+=sqrt(2)
        Vector2d px_curr = px_mean_curr + l * epipolar_direction;  // 待匹配点
        if (!inside(px_curr))
            continue;
        // 计算待匹配点与参考帧的 NCC
        double ncc = NCC(ref, curr, pt_ref, px_curr);
        if (ncc > best_ncc) {
            best_ncc = ncc;
            best_px_curr = px_curr;
        }
    }
    if (best_ncc < 0.85f)      // 只相信 NCC 很高的匹配
        return false;
    pt_curr = best_px_curr;
    return true;
}

double NCC(
    const Mat &ref, const Mat &curr,
    const Vector2d &pt_ref, const Vector2d &pt_curr) {
    // 零均值-归一化互相关
    // 先算均值
    double mean_ref = 0, mean_curr = 0;
    vector<double> values_ref, values_curr; // 参考帧和当前帧的均值
    for (int x = -ncc_window_size; x <= ncc_window_size; x++)
        for (int y = -ncc_window_size; y <= ncc_window_size; y++) {
            double value_ref = double(ref.ptr<uchar>(int(y + pt_ref(1, 0)))[int(x + pt_ref(0, 0))]) / 255.0;
            mean_ref += value_ref;

            double value_curr = getBilinearInterpolatedValue(curr, pt_curr + Vector2d(x, y));
            mean_curr += value_curr;

            values_ref.push_back(value_ref);
            values_curr.push_back(value_curr);
        }

    mean_ref /= ncc_area;
    mean_curr /= ncc_area;

    // 计算 Zero mean NCC
    double numerator = 0, demoniator1 = 0, demoniator2 = 0;
    for (int i = 0; i < values_ref.size(); i++) {
        double n = (values_ref[i] - mean_ref) * (values_curr[i] - mean_curr);
        numerator += n;
        demoniator1 += (values_ref[i] - mean_ref) * (values_ref[i] - mean_ref);
        demoniator2 += (values_curr[i] - mean_curr) * (values_curr[i] - mean_curr);
    }
    return numerator / sqrt(demoniator1 * demoniator2 + 1e-10);   // 防止分母出现零
}

bool updateDepthFilter(
    const Vector2d &pt_ref,
    const Vector2d &pt_curr,
    const SE3d &T_C_R,
    const Vector2d &epipolar_direction,
    Mat &depth,
    Mat &depth_cov2) {
    // 不知道这段还有没有人看
    // 用三角化计算深度
    SE3d T_R_C = T_C_R.inverse();
    Vector3d f_ref = px2cam(pt_ref);
    f_ref.normalize();
    Vector3d f_curr = px2cam(pt_curr);
    f_curr.normalize();

    // 方程
    // d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC
    // f2 = R_RC * f_cur
    // 转化成下面这个矩阵方程组
    // => [ f_ref^T f_ref, -f_ref^T f2 ] [d_ref]   [f_ref^T t]
    //    [ f_2^T f_ref, -f2^T f2      ] [d_cur] = [f2^T t   ]
    Vector3d t = T_R_C.translation();
    Vector3d f2 = T_R_C.so3() * f_curr;
    Vector2d b = Vector2d(t.dot(f_ref), t.dot(f2));
    Matrix2d A;
    A(0, 0) = f_ref.dot(f_ref);
    A(0, 1) = -f_ref.dot(f2);
    A(1, 0) = -A(0, 1);
    A(1, 1) = -f2.dot(f2);
    Vector2d ans = A.inverse() * b;
    Vector3d xm = ans[0] * f_ref;           // ref 侧的结果
    Vector3d xn = t + ans[1] * f2;          // cur 结果
    Vector3d p_esti = (xm + xn) / 2.0;      // P的位置，取两者的平均
    double depth_estimation = p_esti.norm();   // 深度值
    // debug << depth_estimation << "\n";

    // 计算不确定性（以一个像素为误差）
    Vector3d p = f_ref * depth_estimation;
    Vector3d a = p - t;
    double t_norm = t.norm();
    double a_norm = a.norm();
    double alpha = acos(f_ref.dot(t) / t_norm);
    double beta = acos(-a.dot(t) / (a_norm * t_norm));
    // debug << alpha << " " << beta << "\n";
    Vector3d f_curr_prime = px2cam(pt_curr + epipolar_direction);
    f_curr_prime.normalize();
    double beta_prime = acos(f_curr_prime.dot(-t) / t_norm);
    // debug << beta_prime << "\n";
    double gamma = M_PI - alpha - beta_prime;
    double p_prime = t_norm * sin(beta_prime) / sin(gamma);
    double d_cov = p_prime - depth_estimation;
    double d_cov2 = d_cov * d_cov;
    // debug << d_cov2  <<"\n";

    // 高斯融合
    double mu = depth.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))];
    double sigma2 = depth_cov2.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))];

    double mu_fuse = (d_cov2 * mu + sigma2 * depth_estimation) / (sigma2 + d_cov2);
    double sigma_fuse2 = (sigma2 * d_cov2) / (sigma2 + d_cov2);

    // debug << mu_fuse << "\n";
    depth.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = mu_fuse;
    depth_cov2.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = sigma_fuse2;

    return true;
}

// // 后面这些太简单我就不注释了（其实是因为懒）
// void plotDepth(const Mat &depth_truth, const Mat &depth_estimate) {
//     imshow("depth_truth", depth_truth * 0.4);
//     imshow("depth_estimate", depth_estimate * 0.4);
//     imshow("depth_error", depth_truth - depth_estimate);
//     waitKey(1);
// }

void evaludateDepth(const Mat &depth_truth, const Mat &depth_estimate) {
    double ave_depth_error = 0;     // 平均误差
    double ave_depth_error_sq = 0;      // 平方误差
    int cnt_depth_data = 0;
    for (int y = boarder; y < depth_truth.rows - boarder; y++)
    {
        for (int x = boarder; x < depth_truth.cols - boarder; x++) {
            double error = depth_truth.at<double>(y, x) - depth_estimate.at<double>(y, x);
            ave_depth_error += error;
            ave_depth_error_sq += error * error;
            cnt_depth_data++;
        }
    }
    ave_depth_error /= cnt_depth_data;
    ave_depth_error_sq /= cnt_depth_data;

    cout << "Average squared error = " << ave_depth_error_sq << ", average error: " << ave_depth_error << endl;
}

// void showEpipolarMatch(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_curr) {
//     Mat ref_show, curr_show;
//     cv::cvtColor(ref, ref_show, CV_GRAY2BGR);
//     cv::cvtColor(curr, curr_show, CV_GRAY2BGR);

//     cv::circle(ref_show, cv::Point2f(px_ref(0, 0), px_ref(1, 0)), 5, cv::Scalar(0, 0, 250), 2);
//     cv::circle(curr_show, cv::Point2f(px_curr(0, 0), px_curr(1, 0)), 5, cv::Scalar(0, 0, 250), 2);

//     imshow("ref", ref_show);
//     imshow("curr", curr_show);
//     waitKey(1);
// }

// void showEpipolarLine(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_min_curr,
//                       const Vector2d &px_max_curr) {

//     Mat ref_show, curr_show;
//     cv::cvtColor(ref, ref_show, CV_GRAY2BGR);
//     cv::cvtColor(curr, curr_show, CV_GRAY2BGR);

//     cv::circle(ref_show, cv::Point2f(px_ref(0, 0), px_ref(1, 0)), 5, cv::Scalar(0, 255, 0), 2);
//     cv::circle(curr_show, cv::Point2f(px_min_curr(0, 0), px_min_curr(1, 0)), 5, cv::Scalar(0, 255, 0), 2);
//     cv::circle(curr_show, cv::Point2f(px_max_curr(0, 0), px_max_curr(1, 0)), 5, cv::Scalar(0, 255, 0), 2);
//     cv::line(curr_show, Point2f(px_min_curr(0, 0), px_min_curr(1, 0)), Point2f(px_max_curr(0, 0), px_max_curr(1, 0)),
//              Scalar(0, 255, 0), 1);

//     imshow("ref", ref_show);
//     imshow("curr", curr_show);
//     waitKey(1);
// }


#include <iostream>
#include "greeter/greeter.hpp"

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

#include <loguru.hpp>

int main(int argc, char* argv[]) {
    using namespace boost::numeric::ublas;

    greeter("Now running our demo program!");

    loguru::init(argc, argv);
    loguru::add_file("tools-cpp1.log", loguru::Truncate, loguru::Verbosity_MAX);

    LOG_SCOPE_F(INFO, "Program starts.");

    std::cout << "Testing Boost Matrix example!" << std::endl;
    matrix<double> m (3, 3);
    for (unsigned i = 0; i < m.size1 (); ++ i)
        for (unsigned j = 0; j < m.size2 (); ++ j) {
            m(i, j) = 3 * i + j;
            LOG_SCOPE_F(INFO, "Indices %d, %d.", i, j);
        }
    std::cout << m << std::endl;
    LOG_SCOPE_F(INFO, "Program ends.");

    greeter("Demo program ends!");

    return 0;
}


#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
#include <fstream>
#include <iostream>
#include <sophus/se3.h>

using namespace Eigen;
using namespace std;

typedef vector<Vector3d,Eigen::aligned_allocator<Vector3d> > VecVector3d;
typedef vector<Vector2d,Eigen::aligned_allocator<Vector2d> > VecVector2d;

typedef Matrix<double ,6,1> Vector6d;
string p3d_file="../p3d.txt";
string p2d_file="../p2d.txt";

int main(int argc ,char** argv)
{
    VecVector2d p2d;
    VecVector3d p3d;
    Matrix3d k; //内参
    double fx=520.9,fy=521.0,cx=325.1,cy=249.7;
    k<<fx,0,cx,0,fy,cy,0,0,1;

    //load points into p3d and p2d
    //START YOUR CODE HERE
    ifstream file1(p3d_file);
    if(!file1.is_open())
    {
        perror("p3d open failed");
    }
    ifstream file2(p2d_file);
    if(!file2.is_open())
    {
        perror("p2d open failed");
    }
    while (!file1.eof())
    {
        double p3[3] = {0};
        for (auto &p:p3)
        {
            file1 >> p;
        }
        p3d.push_back(Vector3d(p3[0], p3[1], p3[2]));
    }
    while (!file2.eof())
    {
        double p2[2]={0};
        for (auto& p:p2)
        {
            file2>>p;
        }
        Vector2d v(p2[0],p2[1]);
        p2d.push_back(v);
        //cout<<"p2d "<<v<<" p3d "<<Vector3d(p3[0],p3[1],p3[2])<<endl;
    }
    //END YOUR CODE HERE
    assert( p3d.size() == p2d.size() ); //如果二者行数相等，则而已继续执行，否则程序停止运行

    int iterations=100;
    double cost=0,lastcost=0;
    int nPoints=p3d.size();
    cout<<"points: "<<nPoints<<endl;

    Matrix3d I = Matrix3d::Identity(); //声明单位矩阵
    Vector3d t ;
    t.setZero();
    //cout<< "I:\n"<<I<<endl;
    //cout<< "t:\n"<<t<<endl;
    Sophus::SE3 T_esti(I,t);//变换矩阵
    //Sophus::SE3 T_esti;
    cout<<"T_esti:\n"<<T_esti.matrix()<<endl;

   for (int iter = 0; iter <iterations ; ++iter) //迭代100次
    {
        Matrix<double,6,6> H = Matrix<double,6,6>::Zero();
        Vector6d b = Vector6d::Zero();

        cost=0;
        //compute cost
        for (int i = 0; i <p3d.size() ; ++i) //遍历数组
        {
            //compute cost for p3d[i] and p2d[i]
            //START YOUR CODE HERE
            Vector2d ui=p2d[i]; //2*1 p2p点
            //Vector3d pi=p3d[i];
            //（7.34）4*4 的变换矩阵 × 4*1的3d点

            Vector4d pii = T_esti.matrix()*Vector4d(p3d[i][0],p3d[i][1],p3d[i][2],1);

           // cout<< "pii:\n"<< pii <<endl;
           // cout<< "k:\n" << k << endl;
            // 3*1的s*u =3*3 的内参 × 3*1的(SE3*P)
            Vector3d pi=k*Vector3d(pii[0],pii[1],pii[2]);//s*u

            cout<< "pi:\n"<< pi<<endl;

            //求误差
            Vector2d e( ui[0]-pi[0]/pi[2] , ui[1]-pi[1]/pi[2] ); //除以pi[2]是归一化过程，误差为2*1
            cost += e(0,0)*e(0,0)+e(1,0)*e(1,0);//e.transpose()*e;
            //END YOUR CODE  HERE

            //compute jacobian
            Matrix<double,2,6> J; //定义2*6 雅克比

            //START YOUR CODE HERE
            //7.45
            J(0,0)=-fx/pii[2];
            J(0,1)=0;
            J(0,2)=fx*pii[0]/(pii[2]*pii[2]);
            J(0,3)=fx*pii[0]*pii[1]/(pii[2]*pii[2]);
            J(0,4)=-fx-fx*pii[0]*pii[0]/(pii[2]*pii[2]);
            J(0,5)=fx*pii[1]/pii[2];

            J(1,0)=0;
            J(1,1)=-fy/pii[2];
            J(1,2)=fy*pii[1]/(pii[2]*pii[2]);
            J(1,3)=fy+fy*pii[2]*pii[2]/(pii[0]*pii[0]);
            J(1,4)=-fy*pii[0]*pii[1]/(pii[2]*pii[2]);
            J(1,5)=-fy*pii[0]/pii[2];
            //END YOUR CODE HERE

            H+=J.transpose()*J;
            b+=-J.transpose()*e;
        }

        //solve dx
        Vector6d dx;

        //START YOUR CODE HERE
        dx=H.ldlt().solve(b);
        //END YOUR CODE HERE

        cout<<"iteration "<<iter<<" cost="<<cout.precision(12)<<cost<<endl;
        if(isnan(dx[0]))
        {
            cout<<"result is nan!"<<endl;
            break;
        }

        if(iter>0&&cost>=lastcost)
        {
            //cost increase,update is not good
            cout<<"cost: "<<cout.precision(12)<<cost<<", last cost: "<<cout.precision(12)<<lastcost<<endl;
            break;
        }

        //update your estimation
        //START YOUR CODE HERE
        T_esti=Sophus::SE3::exp(dx) * T_esti;

        //END YOUR CODE HERE
        lastcost=cost;
        cout<<"iteration "<<iter<<" cost="<<cout.precision(12)<<cost<<endl;
    }

    cout<<"estimated pose: \n"<<T_esti.matrix()<<endl;

    return 0;
}


/*
 * geometry.cpp
 * Copyright (C) 2018 exbot <exbot@ubuntu>
 *
 * Distributed under terms of the MIT license.
 */

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

using namespace std;
using namespace Eigen;

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

		Isometry3d T_cw1 = Isometry3d::Identity();
		Isometry3d T_cw2 = Isometry3d::Identity();

		Quaterniond q_cw1(0.55, 0.3, 0.2, 0.2);
		Quaterniond q_cw2(-0.1, 0.3, -0.7, 0.2);

		Vector3d t_cw2(-0.1,0.4,0.8);
		Vector3d t_cw1(0.7,1.1,0.2);
		Vector3d p_c1(0.5,-0.1,0.2);

		T_cw1.rotate(q_cw1.normalized().toRotationMatrix());
		T_cw1.pretranslate(t_cw1);
		T_cw2.rotate(q_cw2.normalized().toRotationMatrix());
		T_cw2.pretranslate(t_cw2);

		Vector3d p_c2 = T_cw2 * T_cw1.inverse() * p_c1;
		cout << "P in camera2: "<< endl << p_c2 << endl;
return 1;
}


#include <armadillo>
#include "mcpdft.h"
#include "functional.h"

namespace mcpdft {
   /*================================================================*/
   /*                      Exchange Functionals                      */
   /*================================================================*/
   Functional::Functional()  {}
   Functional::~Functional() {}

   double Functional::EX_LSDA(const MCPDFT *mc,
                              const arma::vec &rho_a,
                              const arma::vec &rho_b) {
       const double alpha = (2.0/3.0);      // Slater value: a constant
       const double Cx = (9.0/8.0) * alpha * pow(3.0/M_PI,1.0/3.0);

       size_t npts = mc->get_npts();
       arma::vec W(mc->get_w());

       double exc = 0.0;
       for (size_t p = 0; p < npts; p++) {
           double exa = pow(2.0,1.0/3.0) * Cx * pow( rho_a(p), 4.0/3.0) ;
           double exb = pow(2.0,1.0/3.0) * Cx * pow( rho_b(p), 4.0/3.0) ;
           double ex_LSDA = exa + exb;
           exc += - ex_LSDA * W(p);
       }
       return exc;
   }

   double Functional::EX_PBE(const MCPDFT *mc,
		             const arma::vec &rho_a,
		             const arma::vec &rho_b,
		             const arma::vec &sigma_aa,
		             const arma::vec &sigma_bb) {

       const double delta = 0.06672455060314922;
       const double MU = (1.0/3.0) * delta * M_PI * M_PI;
       const double KAPPA = 0.804;
       double tol = 1.0e-20;

       auto kF = [](double RHO) -> double {
                 double dum = pow(3.0 * M_PI * M_PI * RHO ,1.0/3.0);
                 return dum;
       };

       auto eX = [=](double RHO) -> double {
                 double temp = -(3.0 * kF(RHO)) / (4.0 * M_PI);
                 return temp;
       };

       auto FX = [=](double SIGMA) -> double {
                 double temp = 1.0 + KAPPA - KAPPA * pow( (1.0 + (MU * pow(SIGMA,2.0)) / KAPPA ), -1.0 );
                 return temp;
       };

       auto S = [=](double RHO, double SIGMA) -> double {
                double temp = sqrt(SIGMA) / (2.0 * kF(RHO) * RHO);
                return temp;
       };

       size_t npts = mc->get_npts();
       arma::vec W(mc->get_w());

       double exc = 0.0;
       for (int p = 0; p < npts; p++) {
           double rhoa = rho_a(p);
           double rhob = rho_b(p);
           double rho = rhoa + rhob;
           double sigmaaa = sigma_aa(p);
           double sigmabb = sigma_bb(p);
           double sigma = 0.0;
           if ( rho > tol ) {
              if ( rhoa < tol ){
                 rho = rhob;
                 sigmabb = std::max(0.0,sigmabb);
                 sigma = sigmabb;

                 double zk = eX(2.0 * rho) * FX(S(2.0 * rho, 4.0 * sigma));
                 exc += rho * zk * W(p);
              }else if ( rhob < tol ){
                       rho = rhoa;
                       sigmaaa = std::max(0.0,sigmaaa);
                       sigma = sigmaaa;
                       double zk = eX(2.0 * rho) * FX(S(2.0 * rho, 4.0 * sigma));
                       exc += rho * zk * W(p);
              }else {
                    double zka = rhoa * eX(2.0 * rhoa) * FX(S(2.0 * rhoa, 4.0 * sigmaaa));
                    double zkb = rhob * eX(2.0 * rhob) * FX(S(2.0 * rhob, 4.0 * sigmabb));
                    double zk = zka + zkb;
                    exc += zk * W(p);
              }
           }else{
                   exc += 0.0;
                }
       }
       return exc;
   }
   /*================================================================*/
   /*                    Correlation Functionals                     */
   /*================================================================*/
    double Functional::EC_VWN3(const MCPDFT *mc,
                               const arma::vec &rho_a,
                               const arma::vec &rho_b) {
       double tol = 1.0e-20;

       const double ecp1 = 0.03109070000;
       const double ecp2 = -0.409286;
       const double ecp3 = 13.0720;
       const double ecp4 = 42.7198;
       const double ecf1 = 0.01554535000;
       const double ecf2 = -0.743294;
       const double ecf3 = 20.1231;
       const double ecf4 = 101.578;
       const double d2Fz = 1.7099209341613656173;

       size_t npts = mc->get_npts();
       arma::vec W(mc->get_w());

       auto x = [](double RHO) -> double {
                double rs = pow( 3.0 / ( 4.0 * M_PI * RHO ) , 1.0/3.0 );
                double dum = sqrt(rs);
                return dum;
       };
       auto Fz = [](double ZETA) -> double {
                 double dum = (pow((1.0 + ZETA) ,4.0/3.0) + pow((1.0 - ZETA) ,4.0/3.0) - 2.0) / (2.0 * pow(2.0,1.0/3.0) - 2.0);
                 return dum;
       };
       auto X = [](double i, double c, double d) -> double {
                double temp = pow(i,2.0) + c * i + d;
                return temp;
       };
       auto Q = [](double c, double d) -> double {
                double temp1 = sqrt( 4 * d - pow(c,2.0) );
                return temp1;
       };
       auto q = [=](double RHO, double A, double p, double c, double d) -> double {
                double dum1 = A * ( log( pow(x(RHO),2.0) / X(x(RHO),c,d) ) + 2.0 * c * atan( Q(c,d)/(2.0*x(RHO) + c) ) * pow(Q(c,d),-1.0)
                            - c * p * ( log( pow(x(RHO)-p,2.0) / X(x(RHO),c,d) ) + 2.0 * (c + 2.0 * p) * atan( Q(c,d)/(2.0*x(RHO) + c) )
                            * pow(Q(c,d),-1.0) ) * pow(X(p,c,d),-1.0) );
                return dum1;
       };
       auto EcP = [=](double RHO) -> double {
                  double dumm = q(RHO,ecp1,ecp2,ecp3,ecp4);
                  return dumm;
       };
       auto EcF = [=](double RHO) -> double {
                  double dum = q(RHO,ecf1,ecf2,ecf3,ecf4);
                  return dum;
       };

       double exc = 0.0;
       for (int p = 0; p < npts; p++) {
           double rhoa = rho_a(p);
           double rhob = rho_b(p);
           double rho = rhoa + rhob;
           double zeta = 0.0;
           if ( rho > tol ) {
              if ( rhoa < tol ){
                 rho = rhob;
                 zeta = 1.0;
              }else if ( rhob < tol ){
                       rho = rhoa;
                       zeta = 1.0;
              }else {/* if (!(rhoa < tol) && !(rhob < tol) ) */
                    zeta = (rhoa - rhob) / rho;
              }
              double zk = EcP(rho) + Fz(zeta) * (EcF(rho) - EcP(rho));
              exc += rho * zk * W(p);
           }else{
                   double zk = 0.0;
                   exc += 0.0;
                }
       }
       return exc;
   }

   double Functional::EC_PBE(const MCPDFT *mc,
		             const arma::vec &rho_a,
		             const arma::vec &rho_b,
                             const arma::vec &sigma_aa,
                             const arma::vec &sigma_ab,
			     const arma::vec &sigma_bb) {
      double tol = 1.0e-20;
      size_t npts = mc->get_npts();
      arma::vec W(mc->get_w());
      const double pa = 1.0;
      const double Aa = 0.0168869;
      const double a1a = 0.11125;
      const double b1a = 10.357;
      const double b2a = 3.6231;
      const double b3a = 0.88026;
      const double b4a = 0.49671;
      const double pe = 1.0;
      const double c0p = 0.0310907;
      const double a1p = 0.21370;
      const double b1p = 7.5957;
      const double b2p = 3.5876;
      const double b3p = 1.6382;
      const double b4p = 0.49294;
      const double c0f = 0.01554535;
      const double a1f = 0.20548;
      const double b1f = 14.1189;
      const double b2f = 6.1977;
      const double b3f = 3.3662;
      const double b4f = 0.62517;
      const double d2Fz = 1.7099209341613656173;
      const double BETA = 0.06672455060314922;
      const double GAMMA = 0.0310906908696549;

      auto Fi = [=](double ZETA) -> double {
                double dumm = 0.5 * (pow((1.0 + ZETA) ,2.0/3.0 ) + pow((1.0 - ZETA) ,2.0/3.0));
                return dumm;
      };

      auto kF = [](double RHO) -> double {
                double dum = pow(3.0 * M_PI * M_PI * RHO ,1.0/3.0);
                return dum;
      };

      auto ks = [=](double RHO) -> double {
                double temp = sqrt(4.0 * kF(RHO) / M_PI);
                return temp;
      };

      auto Fz = [](double ZETA) -> double {
                double dum = (pow((1.0 + ZETA) ,4.0/3.0) + pow((1.0 - ZETA) ,4.0/3.0) - 2.0) / (2.0 * pow(2.0,1.0/3.0) - 2.0);
                return dum;
      };

      auto t = [=](double RHO, double SIGMA, double ZETA) -> double {
               double temp = sqrt(SIGMA) / (2.0 * ks(RHO) * Fi(ZETA) * RHO);
               return temp;
      };

      auto G = [](double r, double T, double a1, double b1, double b2, double b3, double b4, double p) -> double {
               double dum = -2.0 * T * (1.0 + a1 * r) * log(1.0 + 0.5 * pow(T * (b1 * sqrt(r) + b2 * r + b3 * pow(r,3.0/2.0) + b4 * pow(r, p+1.0)) ,-1.0));
               return dum;

      };

      auto Ac = [=](double r) -> double {
                double temp = -G(r,Aa,a1a,b1a,b2a,b3a,b4a,pa);
                return temp;
      };

      auto EcP = [=](double r) -> double {
                 double dum = G(r,c0p,a1p,b1p,b2p,b3p,b4p,pe);
                 return dum;
      };

      auto EcF = [=](double r) -> double {
                 double dumm = G(r,c0f,a1f,b1f,b2f,b3f,b4f,pe);
                 return dumm;
      };

      auto Ec = [=](double r, double ZETA) -> double {
                double dum = EcP(r) + ( Ac(r) * Fz(ZETA) * (1.0 - pow(ZETA ,4.0)) ) / d2Fz + ( EcF(r) - EcP(r) ) * Fz(ZETA) * pow(ZETA ,4.0);
                return dum;
      };

      auto A = [=](double r, double ZETA) -> double {
               double dum = (BETA/GAMMA) * pow( exp(-Ec(r,ZETA) / (pow(Fi(ZETA),3.0) * GAMMA)) - 1.0, -1.0);
               return dum;
      };

      auto H = [=](double RHO, double SIGMA, double r, double ZETA) -> double {
               double temp = pow(Fi(ZETA),3.0) * GAMMA * log(1.0 + (BETA/GAMMA) * pow(t(RHO,SIGMA,ZETA) ,2.0) * (1.0 + A(r,ZETA) * pow(t(RHO,SIGMA,ZETA),2.0))
                           / (1.0 + A(r,ZETA) * pow(t(RHO,SIGMA,ZETA),2.0) + pow(A(r,ZETA),2.0) * pow(t(RHO,SIGMA,ZETA),4.0)));
               return temp;
      };

      double exc = 0.0;
      for (int p = 0; p < npts; p++) {
          double rhoa = rho_a(p);
          double rhob = rho_b(p);
          double rho = rhoa + rhob;
          double zeta = (rhoa - rhob) / rho;
          double rs =  pow( 3.0 / ( 4.0 * M_PI * rho) , 1.0/3.0 );
          double sigmaaa = sigma_aa(p);
          double sigmaab = sigma_ab(p);
          double sigmabb = sigma_bb(p);
          double sigma = sigmaaa + sigmabb + 2.0 * sigmaab;
          if ( rho > tol ) {
             if ( rhoa < tol ){
                rho = rhob;
                sigmabb = std::max(0.0,sigmabb);
                sigma = sigmabb;
                zeta = 1.0;
             }else if ( rhob < tol ){
                      rho = rhoa;
                      sigmaaa = std::max(0.0,sigmaaa);
                      sigma = sigmaaa;
                      zeta = 1.0;
             }else/* if (!(rhoa < tol) && !(rhob < tol) ) */{
                      sigmaaa = std::max(0.0,sigmaaa);
                      sigmabb = std::max(0.0,sigmabb);
                      sigma = sigmaaa + sigmabb + 2.0 * sigmaab;
             }
             double zk = H(rho,sigma,rs,zeta) + Ec(rs,zeta);
             exc += rho * zk * W(p);
          }else{
                  double zk = 0.0;
                  exc += rho * zk * W(p);
               }
      }
      return exc;
   }
}


//
// Created by krab1k on 02/01/19.
//

#include <functional>
#include <string>
#include <cmath>
#include <Eigen/LU>

#include "qeq.h"
#include "../structures/atom.h"
#include "../geometry.h"
#include "../parameters.h"

CHARGEFW2_METHOD(QEq)

double QEq::overlap_term(const Atom &atom_i, const Atom &atom_j, const std::string &type) const {
    auto Ji = parameters_->atom()->parameter(atom::hardness)(atom_i);
    auto Jj = parameters_->atom()->parameter(atom::hardness)(atom_j);
    auto Rij = distance(atom_i, atom_j);
    if (type == "Nishimoto-Mataga") {
        return 1 / (Rij + 2 / (Ji + Jj));
    } else if (type == "Nishimoto-Mataga-Weiss") {
        const double f = 1.2;
        return f / (Rij + (2 * f) / (Ji + Jj));
    } else if (type == "Ohno") {
        return 1 / std::sqrt(Rij * Rij + std::pow(2 / (Ji + Jj), 2));
    } else if (type == "Ohno-Klopman") {
        return 1 / std::sqrt(Rij * Rij + std::pow(1 / (2 * Ji) + 1 / (2 * Jj), 2));
    } else if (type == "DasGupta-Huzinaga") {
        const double k = 0.4;
        return 1 / (Rij + 1 / (Ji / 2 * exp(k * Rij) + Jj / 2 * exp(k * Rij)));
    } else /* (type == "Louwen-Vogt") */ {
        const double gamma = (Ji + Jj) / 2;
        return 1 / std::cbrt(1 / std::pow(gamma, 3) + std::pow(Rij, 3));
    }
}

Eigen::VectorXd QEq::EE_system(const std::vector<const Atom *> &atoms, double total_charge) const {

    size_t n = atoms.size();

    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n + 1, n + 1);
    Eigen::VectorXd b = Eigen::VectorXd::Zero(n + 1);

    const auto type = get_option_value<std::string>("overlap_term");

    for (size_t i = 0; i < n; i++) {
        const auto &atom_i = *atoms[i];
        A(i, i) = parameters_->atom()->parameter(atom::hardness)(atom_i);
        b(i) = - parameters_->atom()->parameter(atom::electronegativity)(atom_i);
        for (size_t j = i + 1; j < n; j++) {
            const auto &atom_j = *atoms[j];
            auto x = overlap_term(atom_i, atom_j, type);
            A(i, j) = x;
            A(j, i) = x;
        }
    }

    A.row(n) = Eigen::VectorXd::Constant(n + 1, 1);
    A.col(n) = Eigen::VectorXd::Constant(n + 1, 1);
    A(n, n) = 0;
    b(n) = total_charge;

    return A.partialPivLu().solve(b).head(n);
}

std::vector<double> QEq::calculate_charges(const Molecule &molecule) const {
    auto f = [this](const std::vector<const Atom *> &atoms, double total_charge) -> Eigen::VectorXd {
        return EE_system(atoms, total_charge);
    };

    Eigen::VectorXd q = solve_EE(molecule, f);
    return std::vector<double>(q.data(), q.data() + q.size());
}


/*********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2020 Northwestern University
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither the name of the copyright holder 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) 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.
 *********************************************************************/
/**
 * @file numerics.hpp
 * @author Boston Cleek
 * @date 30 Oct 2020
 * @brief Useful numerical utilities
 */
#ifndef NUMERICS_HPP
#define NUMERICS_HPP

#include <cmath>
#include <algorithm>
// #include <numbers>

#include <armadillo>

#include <nav_msgs/Path.h>
#include <tf2/LinearMath/Quaternion.h>

#include <ergodic_exploration/collision.hpp>

namespace ergodic_exploration
{
using arma::mat;
using arma::vec;

// TODO: why gcc cant find numbers
constexpr double PI = 3.14159265358979323846;

/**
 * @brief approximately compare two floating-point numbers
 * @param d1 - a number to compare
 * @param d2 - a second number to compare
 * @param epsilon - absolute threshold required for equality
 * @return true if abs(d1 - d2) < epsilon
 */
inline bool almost_equal(double d1, double d2, double epsilon = 1.0e-12)
{
  return std::fabs(d1 - d2) < epsilon ? true : false;
}

/**
 * @brief Wraps angle between -pi and pi
 * @param rad - angle in radians
 * @return wrapped angle in radians
 */
inline double normalize_angle_PI(double rad)
{
  // floating point remainder essentially this is fmod
  const auto q = std::floor((rad + PI) / (2.0 * PI));
  rad = (rad + PI) - q * 2.0 * PI;

  if (rad < 0.0)
  {
    rad += 2.0 * PI;
  }

  return (rad - PI);
}

/**
 * @brief Wraps angle between 0 and 2pi or 0 to -2pi
 * @param rad - angle in radians
 * @return wrapped angle in radians
 */
inline double normalize_angle_2PI(double rad)
{
  // floating point remainder essentially this is fmod
  const auto q = std::floor(rad / (2.0 * PI));
  rad = (rad)-q * 2.0 * PI;

  if (rad < 0.0)
  {
    rad += 2.0 * PI;
  }

  return rad;
}

/**
 * @brief Get yaw from quaternion
 * @param qx - x-axis rotation component
 * @param qy - y-axis rotation component
 * @param qz - z-axis rotation component
 * @param qw - rotation magnitude
 * @return yaw (rad)
 */
inline double getYaw(double qx, double qy, double qz, double qw)
{
  // TODO: add unit test
  // Source: https://github.com/ros/geometry2/blob/noetic-devel/tf2/include/tf2/impl/utils.h#L122
  const auto sqx = qx * qx;
  const auto sqy = qy * qy;
  const auto sqz = qz * qz;
  const auto sqw = qw * qw;

  // Normalization added from urdfom_headers
  const auto sarg = -2.0 * (qx * qz - qw * qy) / (sqx + sqy + sqz + sqw);

  // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
  if (sarg < -0.99999 || almost_equal(sarg, -0.99999))
  {
    return -2.0 * std::atan2(qy, qx);
  }

  else if (sarg > 0.99999 || almost_equal(sarg, 0.99999))
  {
    return 2.0 * std::atan2(qy, qx);
  }

  return std::atan2(2.0 * (qx * qy + qw * qz), sqw + sqx - sqy - sqz);
}

/**
 * @brief Euclidean distance between two points
 * @param x0 - x-position point 0
 * @param y0 - y-position point 0
 * @param x1 - x-position point 1
 * @param y1 - y-position point 1
 * @return euclidean distance
 */
inline double distance(double x0, double y0, double x1, double y1)
{
  const auto dx = x1 - x0;
  const auto dy = y1 - y0;
  return std::sqrt(dx * dx + dy * dy);
}

/**
 * @brief Entropy of a single grid cell
 * @param p - probability grid cell is occupied represented as a decimal
 * @return entropy
 */
inline double entropy(double p)
{
  // Assign zero information gain
  if (almost_equal(0.0, p) || almost_equal(1.0, p) /*|| p < 0.0*/)
  {
    return 1e-3;
  }

  // unknowm: p = -1 => entropy(0.5) = 0.7
  else if (p < 0.0)
  {
    return 0.7;
  }

  return -p * std::log(p) - (1.0 - p) * std::log(1.0 - p);
}

/**
 * @brief Convert polar to cartesian coordinates
 * @param angle - angle in radians
 * @param range - range measurement
 */
inline vec polar2Cartesian(double angle, double range)
{
  const auto x = range * std::cos(angle);
  const auto y = range * std::sin(angle);
  return { x, y };
}

/**
 * @brief Convert polar to cartesian homogenous coordinates
 * @param angle - angle in radians
 * @param range - range measurement
 */
inline vec polar2CartesianHomo(double angle, double range)
{
  const auto x = range * std::cos(angle);
  const auto y = range * std::sin(angle);
  return { x, y, 1.0 };
}

/**
 * @brief Construct 2D transformation matrix
 * @param x - x position
 * @param y - y position
 * @param angle - yaw in radians
 * @details 2D transformation
 */
inline mat transform2d(double x, double y, double angle)
{
  const mat trans2d = { { std::cos(angle), -std::sin(angle), x },
                        { std::sin(angle), std::cos(angle), y },
                        { 0.0, 0.0, 1.0 } };

  return trans2d;
}

/**
 * @brief Construct 2D transformation matrix
 * @param x - x position
 * @param y - y position
 * @details 2D transformation
 */
inline mat transform2d(double x, double y)
{
  const mat trans2d = { { 1.0, 0.0, x }, { 0.0, 1.0, y }, { 0.0, 0.0, 1.0 } };

  return trans2d;
}

/**
 * @brief Construct 2D transformation
 * @param angle - yaw in radians
 * @details 2D transformation
 */
inline mat transform2d(double angle)
{
  const mat trans2d = { { std::cos(angle), -std::sin(angle), 0.0 },
                        { std::sin(angle), std::cos(angle), 0.0 },
                        { 0.0, 0.0, 1.0 } };
  return trans2d;
}

/**
 * @brief Construct 2D transformation inverse
 * @param trans2d - 2D transformation
 * @details 2D transformation inverse
 */
inline mat transform2dInv(const mat& trans2d)
{
  // R^T flip sign in sin
  const auto stheta = -trans2d(1, 0);
  const auto ctheta = trans2d(0, 0);
  const auto theta = std::atan2(stheta, ctheta);

  // p' = -R^T * p
  const auto x = -(ctheta * trans2d(0, 2) - stheta * trans2d(1, 2));
  const auto y = -(stheta * trans2d(0, 2) + ctheta * trans2d(1, 2));

  return transform2d(theta, x, y);
}

/**
 * @brief Integrate a constant twist
 * @param x - current state [x, y, theta]
 * @param vb - current twist [vx, vy, w]
 * @param dt - time step
 * @return new pose
 */
inline vec integrate_twist(const vec& x, const vec& u, double dt)
{
  // Eqn. 13.35 and 13.36 pg 471 Modern Robotics
  // displacement b to b' (dx, dy, dth)
  vec dqb(3);

  // no rotation
  if (almost_equal(u(2), 0.0))
  {
    dqb(0) = u(0) * dt;
    dqb(1) = u(1) * dt;
    dqb(2) = 0.0;
  }

  else
  {
    const vec vb = u * dt;
    dqb(0) = (vb(0) * std::sin(vb(2)) + vb(1) * (std::cos(vb(2)) - 1.0)) / vb(2);

    dqb(1) = (vb(1) * std::sin(vb(2)) + vb(0) * (1.0 - std::cos(vb(2)))) / vb(2);

    dqb(2) = vb(2);
  }

  return x + transform2d(x(2)) * dqb;
}

/**
 * @brief Determine if control will cause a collision
 * @param collision - collision detector
 * @param grid - grid map
 * @param x0 - initial state
 * @param u - twist [vx, vy, w]
 * @param dt - time step in integration
 * @param horizon - length of integration
 * @return true if the control is collision free
 * @details The control is assumed to be constant and a twist is
 * integrated for a fixed amout of time
 */
inline bool validate_control(const Collision& collision, const GridMap& grid,
                             const vec& x0, const vec& u, double dt, double horizon)
{
  vec x = x0;
  const auto steps = static_cast<unsigned int>(std::abs(horizon / dt));

  for (unsigned int i = 0; i < steps; i++)
  {
    x = integrate_twist(x, u, dt);
    x(2) = normalize_angle_PI(x(2));

    if (collision.collisionCheck(grid, x))
    {
      return false;
    }
  }

  return true;
}

/**
 * @brief Visualize path from following a constant twist
 * @param x0 - current state
 * @param u - twist [vx, vy, w]
 * @param dt - time step
 * @param horizon - control horizon
 * @return trajectory
 */
inline nav_msgs::Path constTwistPath(const std::string& map_frame_id, const vec& x0,
                                     const vec& u, double dt, double horizon)
{
  nav_msgs::Path path;
  path.header.frame_id = map_frame_id;

  const auto steps = static_cast<unsigned int>(std::abs(horizon / dt));
  path.poses.resize(steps);

  vec x = x0;
  for (unsigned int i = 0; i < steps; i++)
  {
    x = integrate_twist(x, u, dt);

    path.poses.at(i).pose.position.x = x(0);
    path.poses.at(i).pose.position.y = x(1);

    tf2::Quaternion quat;
    quat.setRPY(0.0, 0.0, normalize_angle_PI(x(2)));

    path.poses.at(i).pose.orientation.x = quat.x();
    path.poses.at(i).pose.orientation.y = quat.y();
    path.poses.at(i).pose.orientation.z = quat.z();
    path.poses.at(i).pose.orientation.w = quat.w();
  }

  return path;
}

}  // namespace ergodic_exploration
#endif


#include <armadillo>

using namespace arma;

arma::mat COMMON_TEST_WEIGHTS = {
    {1, 1, 2},
    {0, 0, 0},
    {1, 0, 10},
    {5000, 1, 10},
    {-10, -3, -7},
    {-10, 3, -7},
};

arma::mat SIGNALS = {
    {1, 0, 0, 0},
    {0, 1, 0, 1},
    {1, 1, 0, 0},
};

mat create_signals()
{
    mat signals = {{1, 0, 0, 0},
                   {0, 1, 0, 1},
                   {1, 1, 0, 0}};

    return signals;
}

mat sum_signal(mat weights, mat signals)
{
    return weights * signals;
}

bool is_equal(mat x, mat y, float threshold = 0.001)
{
    return approx_equal(x, y, "absdiff", threshold);
}

template <typename T>
class SolverTester
{
public:
    T m_solver;
    arma::mat m_signals;

    SolverTester(T solver)
    {
        m_solver = solver;
        set_up();
    }

    void set_up()
    {
        m_signals = SIGNALS;
        m_solver.set_library(m_signals);
    }

    mat model(mat weights, mat signals)
    {
        return weights * signals;
    }

    void test_solve(arma::mat weights, float tolerance = 0.001)
    {
        mat signal = model(weights, m_signals);
        mat result = m_solver.solve(signal);
        BOOST_TEST_MESSAGE(boost::unit_test::framework::current_test_case().p_name);
        BOOST_TEST_MESSAGE("Expected: " << weights);
        BOOST_TEST_MESSAGE("Actual: " << result);
        BOOST_CHECK(is_equal(weights, result, tolerance));
    }

    void test_multiple_solve(arma::mat weights, float tolerance = 0.001)
    {
        for (int row = 0; row < weights.n_rows; row++)
        {
            arma::mat test_weights = weights.row(row);
            set_up();
            test_solve(test_weights, tolerance);
        }
    }

    void test_state(arma::mat weights, float tolerance = 0.001)
    {
        // Test that solver produces same result without initialization of solver between
        // State of the solver should not affect to result
        set_up();
        test_solve(weights, tolerance);
        test_solve(weights, tolerance);
    }

    void test_common(float tolerance = 0.001)
    {
        arma::mat weights = {1, 1, 2};
        test_state(weights, tolerance);
        test_multiple_solve(COMMON_TEST_WEIGHTS, tolerance);
    }
};


#ifndef VECTOR3D_HEADER_DEFINED
#define VECTOR3D_HEADER_DEFINED

//#include <boost/functional/hash.hpp>

const double EPS_DBL = 1e-12;

struct vector3d{
    double x;
    double y;
    double z;

    vector3d(double x_ = 0.0, double y_ = 0.0, double z_ = 0.0) : x(x_), y(y_), z(z_) {}

    double operator *(const vector3d& vec) const{
        return x * vec.x + y * vec.y + z * vec.z;
    }
    vector3d operator / (double d) const {
        return vector3d(x / d, y / d, z / d);
    }
    vector3d norm() const{
        return *this / sqrt(*this * *this);
    }
    double length() const{
        return sqrt(*this * *this);
    }
    double angle(const vector3d& vec) const{
        return acos((*this * vec)/((*this).length() * vec.length()));
    }
    double distxy(const vector3d& vec) const{
        return sqrt((x-vec.x)*(x-vec.x)+(y-vec.y)*(y-vec.y));
    }
    vector3d operator ^ (const vector3d& vec) const{
        return vector3d(y*vec.z-z*vec.y, z*vec.x-x*vec.z,x*vec.y-y*vec.x);
    }
    vector3d operator + (const vector3d& vec) const{
        return vector3d(x+vec.x,y+vec.y,z+vec.z);
    }
    vector3d operator - (const vector3d& vec) const{
        return vector3d(x-vec.x,y-vec.y,z-vec.z);
    }

    bool operator < (const vector3d& vec) const{
        if(x<vec.x){
            return true;
        }
        else if(((x-vec.x)<EPS_DBL) && (y<vec.y)){
            return true;
        }
        else{
            return false;
        }
    }
    bool operator == (const vector3d& vec) const{
        return (std::abs(x-vec.x)<EPS_DBL) && (std::abs(y-vec.y)<EPS_DBL);
    }
};

inline std::ostream& operator << (std::ostream& os, const vector3d& vec){
    os << "<" << vec.x << ", " << vec.y << ", " << vec.z << ">";
    return os;
}

struct hash {
    size_t operator() (const vector3d& vec) const {
 //       size_t seed = 0;
 //       boost::hash_combine(seed, vec.x);
 //       boost::hash_combine(seed, vec.y);
 //       return seed;
    return std::hash<int>() ((int)(vec.y/25.0))+(((int)(vec.x/25.0)) << 16);
        //        return (size_t)vec.y+((size_t)vec.x << 32);
    }
};

#endif // VECTOR3D_HEADER_DEFINED


///////////////////////////////////////////////////////////////////////////////
// weighted_median.hpp
//
//  Copyright 2006 Eric Niebler, Olivier Gygi. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_MEDIAN_HPP_EAN_28_10_2005
#define BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_MEDIAN_HPP_EAN_28_10_2005

#include <boost/mpl/placeholders.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/weighted_p_square_quantile.hpp>
#include <boost/accumulators/statistics/weighted_density.hpp>
#include <boost/accumulators/statistics/weighted_p_square_cumulative_distribution.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // weighted_median_impl
    //
    /**
        @brief Median estimation for weighted samples based on the \f$P^2\f$ quantile estimator

        The \f$P^2\f$ algorithm for weighted samples is invoked with a quantile probability of 0.5.
    */
    template<typename Sample>
    struct weighted_median_impl
      : accumulator_base
    {
        // for boost::result_of
        typedef typename numeric::functional::average<Sample, std::size_t>::result_type result_type;

        weighted_median_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            return weighted_p_square_quantile_for_median(args);
        }
    };

    ///////////////////////////////////////////////////////////////////////////////
    // with_density_weighted_median_impl
    //
    /**
        @brief Median estimation for weighted samples based on the density estimator

        The algorithm determines the bin in which the \f$0.5*cnt\f$-th sample lies, \f$cnt\f$ being
        the total number of samples. It returns the approximate horizontal position of this sample,
        based on a linear interpolation inside the bin.
    */
    template<typename Sample>
    struct with_density_weighted_median_impl
      : accumulator_base
    {
        typedef typename numeric::functional::average<Sample, std::size_t>::result_type float_type;
        typedef std::vector<std::pair<float_type, float_type> > histogram_type;
        typedef iterator_range<typename histogram_type::iterator> range_type;
        // for boost::result_of
        typedef float_type result_type;

        template<typename Args>
        with_density_weighted_median_impl(Args const &args)
          : sum(numeric::average(args[sample | Sample()], (std::size_t)1))
          , is_dirty(true)
        {
        }

        void operator ()(dont_care)
        {
            this->is_dirty = true;
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            if (this->is_dirty)
            {
                this->is_dirty = false;

                std::size_t cnt = count(args);
                range_type histogram = weighted_density(args);
                typename range_type::iterator it = histogram.begin();
                while (this->sum < 0.5 * cnt)
                {
                    this->sum += it->second * cnt;
                    ++it;
                }
                --it;
                float_type over = numeric::average(this->sum - 0.5 * cnt, it->second * cnt);
                this->median = it->first * over + (it + 1)->first * ( 1. - over );
            }

            return this->median;
        }

    private:
        mutable float_type sum;
        mutable bool is_dirty;
        mutable float_type median;
    };

    ///////////////////////////////////////////////////////////////////////////////
    // with_p_square_cumulative_distribution_weighted_median_impl
    //
    /**
        @brief Median estimation for weighted samples based on the \f$P^2\f$ cumulative distribution estimator

        The algorithm determines the first (leftmost) bin with a height exceeding 0.5. It
        returns the approximate horizontal position of where the cumulative distribution
        equals 0.5, based on a linear interpolation inside the bin.
    */
    template<typename Sample, typename Weight>
    struct with_p_square_cumulative_distribution_weighted_median_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Sample, Weight>::result_type weighted_sample;
        typedef typename numeric::functional::average<weighted_sample, std::size_t>::result_type float_type;
        typedef std::vector<std::pair<float_type, float_type> > histogram_type;
        typedef iterator_range<typename histogram_type::iterator> range_type;
        // for boost::result_of
        typedef float_type result_type;

        with_p_square_cumulative_distribution_weighted_median_impl(dont_care)
          : is_dirty(true)
        {
        }

        void operator ()(dont_care)
        {
            this->is_dirty = true;
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            if (this->is_dirty)
            {
                this->is_dirty = false;

                range_type histogram = weighted_p_square_cumulative_distribution(args);
                typename range_type::iterator it = histogram.begin();
                while (it->second < 0.5)
                {
                    ++it;
                }
                float_type over = numeric::average(it->second - 0.5, it->second - (it - 1)->second);
                this->median = it->first * over + (it + 1)->first * ( 1. - over );
            }

            return this->median;
        }
    private:
        mutable bool is_dirty;
        mutable float_type median;
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::weighted_median
// tag::with_density_weighted_median
// tag::with_p_square_cumulative_distribution_weighted_median
//
namespace tag
{
    struct weighted_median
      : depends_on<weighted_p_square_quantile_for_median>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::weighted_median_impl<mpl::_1> impl;
    };
    struct with_density_weighted_median
      : depends_on<count, weighted_density>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::with_density_weighted_median_impl<mpl::_1> impl;
    };
    struct with_p_square_cumulative_distribution_weighted_median
      : depends_on<weighted_p_square_cumulative_distribution>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::with_p_square_cumulative_distribution_weighted_median_impl<mpl::_1, mpl::_2> impl;
    };

}

///////////////////////////////////////////////////////////////////////////////
// extract::weighted_median
//
namespace extract
{
    extractor<tag::median> const weighted_median = {};
}

using extract::weighted_median;
// weighted_median(with_p_square_quantile) -> weighted_median
template<>
struct as_feature<tag::weighted_median(with_p_square_quantile)>
{
    typedef tag::weighted_median type;
};

// weighted_median(with_density) -> with_density_weighted_median
template<>
struct as_feature<tag::weighted_median(with_density)>
{
    typedef tag::with_density_weighted_median type;
};

// weighted_median(with_p_square_cumulative_distribution) -> with_p_square_cumulative_distribution_weighted_median
template<>
struct as_feature<tag::weighted_median(with_p_square_cumulative_distribution)>
{
    typedef tag::with_p_square_cumulative_distribution_weighted_median type;
};

}} // namespace boost::accumulators

#endif