//=======================================================================
// Copyright 2014-2015 David Simmons-Duffin.
// Distributed under the MIT License.
// (See accompanying file LICENSE or copy at
//  http://opensource.org/licenses/MIT)
//=======================================================================

#pragma once

#include "Block_Info.hxx"
#include "Block_Matrix.hxx"
#include "Block_Vector.hxx"
#include "Index_Tuple.hxx"

#include <boost/filesystem.hpp>

// The class SDP encodes a semidefinite program of the following form
//
// Dual: maximize f + b.y over y,Y such that
//                Tr(A_p Y) + (B y)_p = c_p  (0 <= p < P)
//                Y >= 0
// Primal: minimize f + c.x over x,X such that
//                X = \sum_p A_p x_p - C
//                B^T x = b
//                X >= 0
//
// where the data of the SDP has the following structure
//
// - b,y are vectors of length N
//
// - c,x are vectors of length P
//
// - f is a constant that we add to the objective functions for
//   convenience. It has no effect on the running of our algorithm.
//
// - B is a P x N matrix, called the free variable matrix
//
// - X and Y are block diagonal:
//
//   X = BlockDiagonal(X^(0), ..., X^(bMax-1))
//   Y = BlockDiagonal(Y^(0), ..., Y^(bMax-1))
//
//   Let us define Block_b(M) as BlockDiagonal(0,...,0,M,0,...,0),
//   where M is in the b-th block.  Then X and Y can be written
//
//   X = \sum_{0<=b<bMax} Block_b(X^(b))
//   Y = \sum_{0<=b<bMax} Block_b(Y^(b))
//
// - The constraints labeled by 0 <= p < P are in 1-to-1
//   correspondence with tuples
//
//   p <-> (j,r,s,k) where 0 <= j < J,
//                         0 <= s < m_j,
//                         0 <= r <= s,
//                         0 <= k <= d_j,
//
//   We often interchange the index p with the tuple (j,r,s,k).  The
//   constraint matrices A_p are given by
//
//   A_(j,r,s,k) = \sum_{b \in blocks[j]}
//                     Block_b(v_{b,k} v_{b,k}^T \otimes E^{rs}),
//
//   where
//   - E^{rs} is the symmetrization of the m_j x m_j matrix with a
//     1 in entry (r,s) and zeros elsewhere.
//   - v_{b,k} is a vector of length (delta_b+1)
//   - \otimes denotes a tensor product
//   - each block above thus has dimension (delta_b+1)*m_j.
//   - blocks[j_1] and blocks[j_2] are disjoint if j_1 != j_2.  Thus,
//     the block index b determines a unique j, but not vice-versa.
//

struct SDP
{
  // bilinear_bases is a vector of Matrices encoding the v_{b,k}
  // that enter the constraint matrices. Specifically, v_{b,k} are
  // the columns of bilinear_bases[b],
  //
  // bilinear_bases[b].elt(m,k) = (v_{b,k})_m  (0 <= b < bMax,
  //                                           0 <= k <= d_j,
  //                                           0 <= m <= delta_b)
  //
  std::vector<El::Matrix<El::BigFloat>> bilinear_bases_local;
  std::vector<El::DistMatrix<El::BigFloat>> bilinear_bases_dist;

  // free_var_matrix = B, a PxN matrix
  Block_Matrix free_var_matrix;

  // c, a vector of length P used with primal_objective
  Block_Vector primal_objective_c;

  // b, a vector of length N used with dual_objective
  // It is duplicated amongst all the blocks
  El::DistMatrix<El::BigFloat> dual_objective_b;
  // Transformation from internal yp to original y
  El::DistMatrix<El::BigFloat> yp_to_y;

  // objectiveConst = f
  El::BigFloat objective_const;

  SDP(const boost::filesystem::path &sdp_directory,
      const Block_Info &block_info, const El::Grid &grid);
  SDP(const El::BigFloat &objective_const,
      const std::vector<El::BigFloat> &dual_objective_b_input,
      const std::vector<std::vector<El::BigFloat>> &primal_objective_c_input,
      const std::vector<El::Matrix<El::BigFloat>> &free_var_input,
      const Block_Info &block_info, const El::Grid &grid);
};


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

#include "libmv/multiview/five_point.h"

#include <Eigen/QR>

#include "libmv/multiview/fundamental.h"
#include "libmv/multiview/fundamental_kernel.h"
#include "libmv/multiview/five_point_internal.h"

namespace libmv {

Mat FivePointsNullspaceBasis(const Mat2X &x1, const Mat2X &x2) {
  Matrix<double, 9, 9> A;
  A.setZero();  // Make A square until Eigen supports rectangular SVD.
  fundamental::kernel::EncodeEpipolarEquation(x1, x2, &A);
  Eigen::JacobiSVD<Matrix<double, 9, 9> > svd;
  return svd.compute(A, Eigen::ComputeFullV).matrixV().topRightCorner<9, 4>();
}

Vec o1(const Vec &a, const Vec &b) {
  Vec res = Vec::Zero(20);

  res(coef_xx) = a(coef_x) * b(coef_x);
  res(coef_xy) = a(coef_x) * b(coef_y)
               + a(coef_y) * b(coef_x);
  res(coef_xz) = a(coef_x) * b(coef_z)
               + a(coef_z) * b(coef_x);
  res(coef_yy) = a(coef_y) * b(coef_y);
  res(coef_yz) = a(coef_y) * b(coef_z)
               + a(coef_z) * b(coef_y);
  res(coef_zz) = a(coef_z) * b(coef_z);
  res(coef_x)  = a(coef_x) * b(coef_1)
               + a(coef_1) * b(coef_x);
  res(coef_y)  = a(coef_y) * b(coef_1)
               + a(coef_1) * b(coef_y);
  res(coef_z)  = a(coef_z) * b(coef_1)
               + a(coef_1) * b(coef_z);
  res(coef_1)  = a(coef_1) * b(coef_1);

  return res;
}

Vec o2(const Vec &a, const Vec &b) {
  Vec res(20);

  res(coef_xxx) = a(coef_xx) * b(coef_x);
  res(coef_xxy) = a(coef_xx) * b(coef_y)
                + a(coef_xy) * b(coef_x);
  res(coef_xxz) = a(coef_xx) * b(coef_z)
                + a(coef_xz) * b(coef_x);
  res(coef_xyy) = a(coef_xy) * b(coef_y)
                + a(coef_yy) * b(coef_x);
  res(coef_xyz) = a(coef_xy) * b(coef_z)
                + a(coef_yz) * b(coef_x)
                + a(coef_xz) * b(coef_y);
  res(coef_xzz) = a(coef_xz) * b(coef_z)
                + a(coef_zz) * b(coef_x);
  res(coef_yyy) = a(coef_yy) * b(coef_y);
  res(coef_yyz) = a(coef_yy) * b(coef_z)
                + a(coef_yz) * b(coef_y);
  res(coef_yzz) = a(coef_yz) * b(coef_z)
                + a(coef_zz) * b(coef_y);
  res(coef_zzz) = a(coef_zz) * b(coef_z);
  res(coef_xx)  = a(coef_xx) * b(coef_1)
                + a(coef_x)  * b(coef_x);
  res(coef_xy)  = a(coef_xy) * b(coef_1)
                + a(coef_x)  * b(coef_y)
                + a(coef_y)  * b(coef_x);
  res(coef_xz)  = a(coef_xz) * b(coef_1)
                + a(coef_x)  * b(coef_z)
                + a(coef_z)  * b(coef_x);
  res(coef_yy)  = a(coef_yy) * b(coef_1)
                + a(coef_y)  * b(coef_y);
  res(coef_yz)  = a(coef_yz) * b(coef_1)
                + a(coef_y)  * b(coef_z)
                + a(coef_z)  * b(coef_y);
  res(coef_zz)  = a(coef_zz) * b(coef_1)
                + a(coef_z)  * b(coef_z);
  res(coef_x)   = a(coef_x)  * b(coef_1)
                + a(coef_1)  * b(coef_x);
  res(coef_y)   = a(coef_y)  * b(coef_1)
                + a(coef_1)  * b(coef_y);
  res(coef_z)   = a(coef_z)  * b(coef_1)
                + a(coef_1)  * b(coef_z);
  res(coef_1)   = a(coef_1)  * b(coef_1);

  return res;
}

// Builds the polynomial constraint matrix M.
Mat FivePointsPolynomialConstraints(const Mat &E_basis) {
  // Build the polynomial form of E (equation (8) in Stewenius et al. [1])
  Vec E[3][3];
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      E[i][j] = Vec::Zero(20);
      E[i][j](coef_x) = E_basis(3 * i + j, 0);
      E[i][j](coef_y) = E_basis(3 * i + j, 1);
      E[i][j](coef_z) = E_basis(3 * i + j, 2);
      E[i][j](coef_1) = E_basis(3 * i + j, 3);
    }
  }

  // The constraint matrix.
  Mat M(10, 20);
  int mrow = 0;

  // Determinant constraint det(E) = 0; equation (19) of Nister [2].
  M.row(mrow++) = o2(o1(E[0][1], E[1][2]) - o1(E[0][2], E[1][1]), E[2][0]) +
                  o2(o1(E[0][2], E[1][0]) - o1(E[0][0], E[1][2]), E[2][1]) +
                  o2(o1(E[0][0], E[1][1]) - o1(E[0][1], E[1][0]), E[2][2]);

  // Cubic singular values constraint.
  // Equation (20).
  Vec EET[3][3];
  for (int i = 0; i < 3; ++i) {    // Since EET is symmetric, we only compute
    for (int j = 0; j < 3; ++j) {  // its upper triangular part.
      if (i <= j) {
        EET[i][j] = o1(E[i][0], E[j][0])
                  + o1(E[i][1], E[j][1])
                  + o1(E[i][2], E[j][2]);
      } else {
        EET[i][j] = EET[j][i];
      }
    }
  }

  // Equation (21).
  Vec (&L)[3][3] = EET;
  Vec trace  = 0.5 * (EET[0][0] + EET[1][1] + EET[2][2]);
  for (int i = 0; i < 3; ++i) {
    L[i][i] -= trace;
  }

  // Equation (23).
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      Vec LEij = o2(L[i][0], E[0][j])
               + o2(L[i][1], E[1][j])
               + o2(L[i][2], E[2][j]);
      M.row(mrow++) = LEij;
    }
  }

  return M;
}

// Gauss--Jordan elimination for the constraint matrix.
void FivePointsGaussJordan(Mat *Mp) {
  Mat &M = *Mp;

  // Gauss Elimination.
  for (int i = 0; i < 10; ++i) {
    M.row(i) /= M(i, i);
    for (int j = i + 1; j < 10; ++j) {
      M.row(j) = M.row(j) / M(j, i) - M.row(i);
    }
  }

  // Backsubstitution.
  for (int i = 9; i >= 0; --i) {
    for (int j = 0; j < i; ++j) {
      M.row(j) = M.row(j) - M(j, i) * M.row(i);
    }
  }
}

void FivePointsRelativePose(const Mat2X &x1,
                            const Mat2X &x2,
                            vector<Mat3> *Es) {
  // Step 1: Nullspace exrtraction.
  Mat E_basis = FivePointsNullspaceBasis(x1, x2);

  // Step 2: Constraint expansion.
  Mat M = FivePointsPolynomialConstraints(E_basis);

  // Step 3: Gauss-Jordan elimination.
  FivePointsGaussJordan(&M);

  // For the next steps, follow the matlab code given in Stewenius et al [1].

  // Build the action matrix.
  Mat B = M.topRightCorner<10, 10>();
  Mat At = Mat::Zero(10, 10);
  At.row(0) = -B.row(0);
  At.row(1) = -B.row(1);
  At.row(2) = -B.row(2);
  At.row(3) = -B.row(4);
  At.row(4) = -B.row(5);
  At.row(5) = -B.row(7);
  At(6, 0) = 1;
  At(7, 1) = 1;
  At(8, 3) = 1;
  At(9, 6) = 1;

  // Compute the solutions from action matrix's eigenvectors.
  Eigen::EigenSolver<Mat> es(At);
  typedef Eigen::EigenSolver<Mat>::EigenvectorsType Matc;
  Matc V = es.eigenvectors();
  Matc solutions(4, 10);
  solutions.row(0) = V.row(6).array() / V.row(9).array();
  solutions.row(1) = V.row(7).array() / V.row(9).array();
  solutions.row(2) = V.row(8).array() / V.row(9).array();
  solutions.row(3).setOnes();

  // Get the ten candidate E matrices in vector form.
  Matc Evec = E_basis * solutions;

  // Build the essential matrices for the real solutions.
  Es->reserve(10);
  for (int s = 0; s < 10; ++s) {
    Evec.col(s) /= Evec.col(s).norm();
    bool is_real = true;
    for (int i = 0; i < 9; ++i) {
      if (Evec(i, s).imag() != 0) {
        is_real = false;
        break;
      }
    }
    if (is_real) {
      Mat3 E;
      for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
          E(i, j) = Evec(3 * i + j, s).real();
        }
      }
      Es->push_back(E);
    }
  }
}

}  // namespace libmv


#ifndef sphere_hpp
#define sphere_hpp

#include <three-dim-util/opengl2/primitives/abstract-primitive.hpp>
#include <Eigen/Core>

namespace threedimutil
{
    class Sphere : public AbstractPrimitive
    {
    public:
        static void Initialize(int latitude_resolution = 20, int longitude_resolution = 30)
        {
            Sphere::GetInstance().latitude_resolution()  = latitude_resolution;
            Sphere::GetInstance().longitude_resolution() = longitude_resolution;
            Sphere::GetInstance().InitializeInternal();
        }

        static void Draw()
        {
            Sphere::GetInstance().DrawInternal();
        }

        static Sphere& GetInstance()
        {
            static Sphere sphere;
            return sphere;
        }

        int latitude_resolution() const { return latitude_resolution_; }
        int& latitude_resolution() { return latitude_resolution_; }
        int longitude_resolution() const { return longitude_resolution_; }
        int& longitude_resolution() { return longitude_resolution_; }

    private:
        void CreateVertexData()
        {
            constexpr double pi = M_PI;

            vertices_.resize(3, latitude_resolution_ * longitude_resolution_ * 6);
            for (int i = 0; i < longitude_resolution_; ++ i)
            {
                const double theta_xy_1 = 2.0 * static_cast<double>(i + 0) * pi / static_cast<double>(longitude_resolution_);
                const double theta_xy_2 = 2.0 * static_cast<double>(i + 1) * pi / static_cast<double>(longitude_resolution_);
                const double x_1        = std::cos(theta_xy_1);
                const double x_2        = std::cos(theta_xy_2);
                const double y_1        = std::sin(theta_xy_1);
                const double y_2        = std::sin(theta_xy_2);

                for (int j = 0; j < latitude_resolution_; ++ j)
                {
                    const double theta_z_1 = static_cast<double>(j + 0) * pi / static_cast<double>(latitude_resolution_);
                    const double theta_z_2 = static_cast<double>(j + 1) * pi / static_cast<double>(latitude_resolution_);
                    const double cos_1 = std::cos(theta_z_1);
                    const double cos_2 = std::cos(theta_z_2);
                    const double sin_1 = std::sin(theta_z_1);
                    const double sin_2 = std::sin(theta_z_2);

                    const int offset = i * latitude_resolution_ * 6 + j * 6;

                    vertices_.col(offset + 0) = Eigen::Vector3d(sin_2 * x_1, sin_2 * y_1, cos_2);
                    vertices_.col(offset + 1) = Eigen::Vector3d(sin_2 * x_2, sin_2 * y_2, cos_2);
                    vertices_.col(offset + 2) = Eigen::Vector3d(sin_1 * x_2, sin_1 * y_2, cos_1);
                    vertices_.col(offset + 3) = Eigen::Vector3d(sin_2 * x_1, sin_2 * y_1, cos_2);
                    vertices_.col(offset + 4) = Eigen::Vector3d(sin_1 * x_2, sin_1 * y_2, cos_1);
                    vertices_.col(offset + 5) = Eigen::Vector3d(sin_1 * x_1, sin_1 * y_1, cos_1);
                }
            }
            normals_ = vertices_;
        }

        int latitude_resolution_  = 20;
        int longitude_resolution_ = 30;
    };
}

#endif /* sphere_hpp */


#include <aikido/common/PseudoInverse.hpp>

#include <iostream>
#include <limits>
#include <memory>
#include <Eigen/Dense>

namespace aikido {
namespace common {

//==============================================================================
Eigen::MatrixXd pseudoinverse(const Eigen::MatrixXd& mat, double eps)
{
  if (mat.rows() == mat.cols() && mat.determinant() > eps)
    return mat.inverse();

  else
  {
    if (mat.cols() == 1)
    {
      if (mat.isApproxToConstant(0))
      {
        return Eigen::VectorXd::Zero(mat.rows());
      }

      return mat.transpose() / (pow(mat.norm(), 2));
    }

    /// Use SVD decomposition.
    Eigen::JacobiSVD<Eigen::MatrixXd> jacSVD(
        mat, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::MatrixXd U = jacSVD.matrixU();
    Eigen::MatrixXd V = jacSVD.matrixV();
    Eigen::VectorXd S = jacSVD.singularValues();

    Eigen::MatrixXd S_inv(Eigen::MatrixXd::Zero(mat.cols(), mat.rows()));

    for (int i = 0; i < S.rows(); i++)
    {
      if (S(i) > eps)
      {
        S_inv(i, i) = 1.0 / S(i);
      }
      else
      {
        S_inv(i, i) = 0;
      }
    }

    return V * S_inv * U.transpose();
  }
}

} // namespace common
} // namespace aikido


/*=============================================================================
Copyright 2020 Syed Ali Hasan <alihasan9922@gmail.com>

Distributed under the Boost Software License, Version 1.0. (See accompanying
file License.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#ifndef BOOST_ASTRONOMY_UTILITY_HPP
#define BOOST_ASTRONOMY_UTILITY_HPP

#include <iostream>
#include <utility>
#include <cmath>

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

//Angle
#include <boost/units/quantity.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/dimensionless.hpp>
#include <boost/units/physical_dimensions/plane_angle.hpp>

//Time
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include <boost/astronomy/coordinate/coord_sys/coord_sys.hpp>

constexpr long double PI = 3.141592653589793238462643383279502884L;

using namespace std;
namespace bu = boost::units;
namespace bud = boost::units::degree;
using namespace boost::numeric::ublas;
namespace bnu = boost::numeric::ublas;

using namespace boost::gregorian;

namespace boost { namespace astronomy { namespace coordinate {

// Phi φ is the geographical latitude
// ST is the Local Sidereal Time
// ε is the obliquity of the ecliptic.

template
    <
        typename CoordinateType = double,
        typename Angle = bu::quantity<bu::si::plane_angle, CoordinateType>,
        typename ElementType = double
    >
struct column_vector
{
 public:
  matrix<ElementType> vec = matrix<ElementType>(3, 1);

  column_vector() {}

  column_vector(Angle u, Angle v){
    double _u = static_cast<bu::quantity<bu::si::plane_angle>>(u).value();
    double _v = static_cast<bu::quantity<bu::si::plane_angle>>(v).value();

    vec(0,0) = std::cos(_u) * std::cos(_v);
    vec(1,0) = std::sin(_u) * std::cos(_v);
    vec(2,0) = std::sin(_v);
  }

  matrix<ElementType> get(){
    return vec;
  }

  std::string to_string(){
    return "Column Vector";
  }
};

template
    <
        typename CoordinateType = double,
        typename Angle = bu::quantity<bu::si::plane_angle, CoordinateType>,
        typename ElementType = double
    >
struct ha_dec_horizon
{
 public:
  matrix<ElementType> conv = matrix<ElementType>(3, 3);

  ha_dec_horizon() {}

  ha_dec_horizon(Angle phi){
    double _phi = static_cast<bu::quantity<bu::si::plane_angle>>(phi).value();

    conv(0,0) = -std::sin(_phi);
    conv(0,1) = 0;
    conv(0,2) = std::cos(_phi);
    conv(1,0) = 0;
    conv(1,1) = -1;
    conv(1,2) = 0;
    conv(2,0) = std::cos(_phi);
    conv(2,1) = 0;
    conv(2,2) = std::sin(_phi);
  }

  matrix<ElementType> get(){
    return conv;
  }

  std::string to_string(){
    return "Equatorial Coordinate Hour Angle to and from Horizon";
  }
};

template
    <
        typename CoordinateType = double,
        typename Angle = bu::quantity<bu::si::plane_angle, CoordinateType>,
        typename ElementType = double
    >
struct ha_dec_ra_dec
{
 public:
  matrix<ElementType> conv = matrix<ElementType>(3, 3);

  ha_dec_ra_dec() {}

  ha_dec_ra_dec(Angle ST){
    double _ST = static_cast<bu::quantity<bu::si::plane_angle>>(ST).value();

    conv(0,0) = std::cos(_ST);
    conv(0,1) = std::sin(_ST);
    conv(0,2) = 0;
    conv(1,0) = std::sin(_ST);
    conv(1,1) = -std::cos(_ST);
    conv(1,2) = 0;
    conv(2,0) = 0;
    conv(2,1) = 0;
    conv(2,2) = 1;
  }

  matrix<ElementType> get(){
    return conv;
  }

  std::string to_string(){
    return "Equatorial Coordinate Hour Angle to and from Equatorial Coordinate Right Ascension";
  }
};

template
    <
        typename CoordinateType = double,
        typename Angle = bu::quantity<bu::si::plane_angle, CoordinateType>,
        typename ElementType = double
    >
struct ecliptic_to_ra_dec
{
 public:
  matrix<ElementType> conv = matrix<ElementType>(3, 3);

  ecliptic_to_ra_dec() {}

  ecliptic_to_ra_dec(Angle obliquity){
    double _obliquity = static_cast<bu::quantity<bu::si::plane_angle>>(obliquity).value();

    conv(0,0) = 1;
    conv(0,1) = 0;
    conv(0,2) = 0;
    conv(1,0) = 0;
    conv(1,1) = std::cos(_obliquity);
    conv(1,2) = -std::sin(_obliquity);
    conv(2,0) = 0;
    conv(2,1) = std::sin(_obliquity);
    conv(2,2) = std::cos(_obliquity);
  }

  matrix<ElementType> get(){
    return conv;
  }

  std::string to_string(){
    return "Ecliptic to Equatorial Coordinate Right Ascension";
  }
};

template
    <
        typename CoordinateType = double,
        typename Angle = bu::quantity<bu::si::plane_angle, CoordinateType>,
        typename ElementType = double
    >
struct ra_dec_to_ecliptic
{
 public:
  matrix<ElementType> conv = matrix<ElementType>(3, 3);

  ra_dec_to_ecliptic() {}

  ra_dec_to_ecliptic(Angle obliquity){
    double _obliquity = static_cast<bu::quantity<bu::si::plane_angle>>(obliquity).value();

    conv(0,0) = 1;
    conv(0,1) = 0;
    conv(0,2) = 0;
    conv(1,0) = 0;
    conv(1,1) = std::cos(_obliquity);
    conv(1,2) = std::sin(_obliquity);
    conv(2,0) = 0;
    conv(2,1) = -std::sin(_obliquity);
    conv(2,2) = std::cos(_obliquity);
  }

  matrix<ElementType> get(){
    return conv;
  }

  std::string to_string(){
    return "Equatorial Coordinate Right Ascension to Ecliptic";
  }
};

template
    <typename ElementType = double>
struct galactic_to_ra_dec
{
 public:
  matrix<ElementType> conv = matrix<ElementType>(3, 3);

  galactic_to_ra_dec(){
    conv(0,0) = -0.0669887;
    conv(0,1) = 0.8727558;
    conv(0,2) = -0.4835389;
    conv(1,0) = -0.4927285;
    conv(1,1) = -0.4503470;
    conv(1,2) = -0.7445846;
    conv(2,0) = -0.8676008;
    conv(2,1) = 0.1883746;
    conv(2,2) = 0.4601998;
  }

  matrix<ElementType> get(){
    return conv;
  }

  std::string to_string(){
    return "Galactic to Equatorial Coordinate Right Ascension";
  }
};

template
    <typename ElementType = double>
struct ra_dec_to_galactic
{
 public:
  matrix<ElementType> conv = matrix<ElementType>(3, 3);

  ra_dec_to_galactic()
  {
    conv(0,0) = -0.0669887;
    conv(0,1) = -0.8727558;
    conv(0,2) = -0.4835389;
    conv(1,0) = 0.4927285;
    conv(1,1) = -0.4503470;
    conv(1,2) = 0.7445846;
    conv(2,0) = -0.8676008;
    conv(2,1) = -0.1883746;
    conv(2,2) = 0.4601998;
  }

  matrix<ElementType> get()
  {
    return conv;
  }

  std::string to_string()
  {
    return "Equatorial Coordinate Right Ascension to Galactic";
  }

};

typedef bu::quantity<bu::si::plane_angle, double> angle_radian;

struct extract_coordinates{
 private:
  double theta = 0;
  double phi = 0;

 public:
  extract_coordinates(){}

  explicit extract_coordinates(matrix<double> column_vector)
  {
    double m = column_vector(0,0);
    double n = column_vector(1,0);
    double p = column_vector(2,0);

    theta = atan2(n,m);
    phi = asin(p);
  }

  pair<angle_radian,angle_radian> get_coordinates() const{
    return make_pair(theta * bu::si::radian,phi * bu::si::radian);
  }
};

struct obliquity_of_ecliptic{
 private:
  angle_radian e = 0.0 * bu::si::radian;

 public:
  obliquity_of_ecliptic(date d)
  {
    double julian_date = d.julian_day();

    double modified_julian_date = julian_date - 2451545.0;

    double julian_centuries = modified_julian_date / 36525.0;

    double e_degrees =  23.439292 - (46.815 * julian_centuries + (0.0006 - 0.00181 * julian_centuries) * julian_centuries * julian_centuries) / 3600.0;

    e = (e_degrees * PI / 180.0) * bu::si::radian;
  }

  angle_radian get(){
    return e;
  }
};

}}}

#endif  // BOOST_ASTRONOMY_UTILITY_HPP


#include <cstdlib>
#include <iostream>
#include <type_traits>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/hermitian.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/banded.hpp>
#include <boost/numeric/bindings/ublas/hermitian.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include <boost/numeric/bindings/lapack/driver.hpp>
#include "random.hpp"

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

int main(int argc, char *argv[]) {
  typedef ublas::vector<double> vector;
  typedef ublas::matrix<double, ublas::column_major> matrix;
  typedef ublas::banded_matrix<double, ublas::column_major> banded_matrix;
  typedef typename std::make_signed<vector::size_type>::type size_type;

  rand_normal<double>::reset();
  size_type n=128, k=8;
  banded_matrix A(n, n, 0, k);
  for (size_type j=0; j<n; ++j) {
    for (size_type i=std::max(j-k, size_type(0)); i<=j; ++i)
      A(i, j)=rand_normal<double>::get();
  }
  {
    vector lambda(n);
    banded_matrix A_bak(A);
    matrix vr(n ,n);
    ublas::hermitian_adaptor<banded_matrix, ublas::upper> B(A);
    int info=lapack::sbev('V', B, lambda, vr);
    ublas::hermitian_adaptor<banded_matrix, ublas::upper> B_bak(A_bak);
    if (info==0) {
      for (int i=0; i<n; ++i) {
  	// res <- A*vr(i) - lambda(i)*vr(i)
  	ublas::matrix_column<matrix> v(vr, i);
  	vector res(ublas::prod(B_bak, v)-lambda(i)*v);
  	std::cout << "norm of residual (right eigen vector " << i
  		  << " ): " << blas::nrm2(res) << '\n';
      }
    } else
      if (info>0)
  	std::cout << "unable to compute all eigen values\n";
      else
  	std::cout << "illegal arguments\n";
  }
  return EXIT_SUCCESS;
}


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <vector>

#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>

using namespace boost;

//a class to hold the coordinates of the straight line embedding
struct coord_t
{
  std::size_t x;
  std::size_t y;
};

int main(int argc, char** argv)
{
  typedef adjacency_list
    < vecS,
      vecS,
      undirectedS,
      property<vertex_index_t, int>
    > graph;

  //Define the storage type for the planar embedding
  typedef std::vector< std::vector< graph_traits<graph>::edge_descriptor > >
    embedding_storage_t;
  typedef boost::iterator_property_map
    < embedding_storage_t::iterator,
      property_map<graph, vertex_index_t>::type
    >
    embedding_t;

  // Create the graph - a maximal planar graph on 7 vertices. The functions
  // planar_canonical_ordering and chrobak_payne_straight_line_drawing both
  // require a maximal planar graph. If you start with a graph that isn't
  // maximal planar (or you're not sure), you can use the functions
  // make_connected, make_biconnected_planar, and make_maximal planar in
  // sequence to add a set of edges to any undirected planar graph to make
  // it maximal planar.

  graph g(7);
  add_edge(0,1,g);
  add_edge(1,2,g);
  add_edge(2,3,g);
  add_edge(3,0,g);
  add_edge(3,4,g);
  add_edge(4,5,g);
  add_edge(5,6,g);
  add_edge(6,3,g);
  add_edge(0,4,g);
  add_edge(1,3,g);
  add_edge(3,5,g);
  add_edge(2,6,g);
  add_edge(1,4,g);
  add_edge(1,5,g);
  add_edge(1,6,g);

  // Create the planar embedding
  embedding_storage_t embedding_storage(num_vertices(g));
  embedding_t embedding(embedding_storage.begin(), get(vertex_index,g));

  boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                               boyer_myrvold_params::embedding = embedding
                               );

  // Find a canonical ordering
  std::vector<graph_traits<graph>::vertex_descriptor> ordering;
  planar_canonical_ordering(g, embedding, std::back_inserter(ordering));

  //Set up a property map to hold the mapping from vertices to coord_t's
  typedef std::vector< coord_t > straight_line_drawing_storage_t;
  typedef boost::iterator_property_map
    < straight_line_drawing_storage_t::iterator,
      property_map<graph, vertex_index_t>::type
    >
    straight_line_drawing_t;

  straight_line_drawing_storage_t straight_line_drawing_storage
    (num_vertices(g));
  straight_line_drawing_t straight_line_drawing
    (straight_line_drawing_storage.begin(),
     get(vertex_index,g)
     );

  // Compute the straight line drawing
  chrobak_payne_straight_line_drawing(g,
                                      embedding,
                                      ordering.begin(),
                                      ordering.end(),
                                      straight_line_drawing
                                      );

  std::cout << "The straight line drawing is: " << std::endl;
  graph_traits<graph>::vertex_iterator vi, vi_end;
  for(boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
    {
      coord_t coord(get(straight_line_drawing,*vi));
      std::cout << *vi << " -> (" << coord.x << ", " << coord.y << ")"
                << std::endl;
    }

  // Verify that the drawing is actually a plane drawing
  if (is_straight_line_drawing(g, straight_line_drawing))
    std::cout << "Is a plane drawing." << std::endl;
  else
    std::cout << "Is not a plane drawing." << std::endl;

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// random::poisson::devroye::detail::crtp::crtp.hpp                        	//
//                                                                          //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_POISSON_EXT_DEVROYE_CRTP_CRTP_HPP_ER_2010
#define BOOST_RANDOM_POISSON_EXT_DEVROYE_CRTP_CRTP_HPP_ER_2010
#include <boost/range.hpp>
#include <boost/math/special_functions/modf.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/exponential_distribution.hpp>

#include <boost/random/poisson_ext/devroye/crtp/parameters.hpp>

namespace boost{
namespace random{
namespace poisson{
namespace devroye{

    // Samples from the poisson distribution with an integer mean
    //
    // Source : The computer generation of poisson random variables. L. Devroye,
    // Computing 26, Springer-Verlag, 1981
    //
    // Parameter     Description
    // Step4         A class that models the concept by the same name
    // Int           An integer type
    // T             A float type
    // P             An error handling policy
    // IntT          A converter from Int to T
    // TInt          A converter from T to Int
	//
    // Requiremens
    // Expression                       Description
    // crtp<Step4,Int,T,P,IntT,TInt>    Public base of Step4
	//
    // Concept Step4:
    // crtp<Step4,Int,T,P> is a public base, and
    // Expression    Result type   Side effect
    //  accept()     bool          if true, y is set to the value to be returned
    //
    // The numbers on the right are those of the lines of the Fortran listing
    // given in the reference above.
    template<typename Step4,typename Int,typename T,
        typename P = boost::math::policies::policy<>,
        typename IntT = boost::numeric::converter<T,Int>,
        typename TInt = boost::numeric::converter<Int,T>
    >
    class crtp : public devroye::parameters<Int,T,P,IntT>
    {
        typedef devroye::parameters<Int,T,P,IntT> parameters_;
        typedef IntT converter_;
        typedef std::string str_;

		public:

        typedef T input_type;
        typedef Int result_type;

        T mean() const { return converter_::convert(this->i_mean_); }
        void reset() { }

		crtp(){}
        explicit crtp(const Int& i_mean)
        	:parameters_(i_mean),max_step1_cnt_(10)
            {
            	BOOST_ASSERT(this->i_mean()>=0);
            }

        explicit crtp(const Int& i_mean,const Int& max_step1_cnt)
        	:parameters_(i_mean),max_step1_cnt_(max_step1_cnt)
            {
            	BOOST_ASSERT(this->i_mean()>=0);
            }

  		template<class U>
  		Int operator()(U& urng)const
  		{
            this->step1_cnt_ = 0;
			Int result =  this->step1(urng);
            BOOST_ASSERT(result >= 0);
            return result;
        }

        const Int& step1_cnt()const{ return this->step1_cnt_; }

        // --- I/O --- //

        template<class CharT, class Traits>
        friend std::basic_ostream<CharT,Traits>&
        operator<<(
            std::basic_ostream<CharT,Traits>& os,
            const crtp& pd
        )
        {
            os 	<< "devroye("
                << pd.i_mean()
                << ','
                << pd.u()
                << ','
                << pd.x()
                << ','
                << pd.i_y()
                << ','
                << pd.v()
                << ','
                << pd.step1_cnt()
                << ')';
            return os;
        }

        template<class CharT, class Traits>
        friend std::basic_istream<CharT,Traits>&
        operator>>(std::basic_istream<CharT,Traits>& is, crtp& pd)
        {
            Int new_mean;
            is	>> std::ws
                >> new_mean
                >> std::ws
                >> std::ws 	// u
                >> std::ws
                >> std::ws	// x
                >> std::ws
                >> std::ws	// y
                >> std::ws
                >> std::ws	// v
                >> std::ws
                >> std::ws	// step1_cnt
                >> std::ws;
            static_cast<Step4&>(pd) = Step4(new_mean);
            return is;
        }

        // ----------- //

        protected:

		template<typename U>
		Int step1(U& urng)const
        {
            static const str_ name = "devroy::crtp<%1%>::step1()";

            using namespace boost::math::policies;
            if( ( this->step1_cnt_++ ) > this->max_step1_cnt() ){
                 raise_evaluation_error( //<boost::uintmax_t>
                    name.c_str(),
                    "step1_cnt_ exceeded limit = %1%",
                    this->max_step1_cnt(), P());
            }
            // line 10 : case mean = 0;

            this->u_ = urng();												//13
            if( this->u() > this->p1() ){
            	// Reconciliation with Fortran listing :
                // ptail = p2 + p3 = 1-p1.
                // u' ~ 1-u. (u'<ptail) <=> (1-u<p2+p3) <=> (u > p1)
            	return this->step3( urng );
            }else{
                return this->step2( urng );
            }
		}

		template<typename U>
		Int step2(U& urng)const
        {
        	T z = this->random_z( urng ); 							        //15
        	this->x_ = ( this->sd1() * z ) + parameters_::loc1();
            if(
                ( this->x() > this->delta() ) || ( this->x() < ( -this->m1() ) )
            )
            {
            	return this->step1( urng );
            }else{
                using namespace std;
            	// y is rounded away from zero
                if( this->x() <= -parameters_::eps /*strictly negative*/)
                {
                    this->i_y_ = floor( this->x() );
                }else{
                    this->i_y_ = ceil( this->x() );
                }
                T e  = this->random_exp1(urng);
                this->v_ = -( e + pow( z, 2 ) / parameters_::two ) + this->c1();
                return this->step4( urng );
            }
        }

		template<typename U>
		Int step3(U& urng)const
        {

            // Reconciliation with Fortran listing :
            // pbody = p3 = 1-(p1+p2)
            // u' ~ 1-u. (u'<pbody) <=> (1-u<p3) <=> (u > p1+p2)
        	if( this->u() > this->p1() + this->p2() ){
            	this->i_y_ = this->i_mean();
                return this->i_y();
            }else{
                using namespace std;
            	T e1 = this->random_exp1( urng );
            	T e2 = this->random_exp1( urng );
            	this->x_ = this->delta() + e1 / this->shape2();				//51
            	this->i_y_ = TInt::convert( ceil( this->x() ) );
            	this->v_ = - (
                	e2 + this->shape2() * ( parameters_::one + this->x() )
                );
            	return this->step4( urng );
            }
        }

		template<typename U>
		Int step4(U& urng)const
        {
            const Step4& derived = static_cast<const Step4&>(*this);
            if( derived.accept() ){
            	return this->i_y();
            }else{
				return this->step1( urng );
            }
		}

		template<typename U>
		T random_z(U& urng)const
        {
        	typedef boost::normal_distribution<T> d_;
            static d_ d = d_( parameters_::zero, parameters_::one );
            return d( urng );
        }

		template<typename U>
		T random_exp1(U& urng)const
        {
        	typedef boost::exponential_distribution<T> d_;
            static d_ d = d_( parameters_::one );
            return d( urng );
        }

		// Parameter
		Int i_mean_;

        // These are quantities that vary throughout sampling
		mutable T	u_;
        mutable T 	x_;
        mutable Int i_y_;
        mutable T 	v_;

        // Implementation detail
        mutable Int step1_cnt_;
        mutable Int max_step1_cnt_;

        const Int& 	i_mean()const{ return this->i_mean_; }
        const T& 	u()const{ return this->u_; }
        const T& 	x()const{ return this->x_; }
        const Int& 	i_y()const{ return this->i_y_; }
        const T& 	v()const{ return this->v_; }
        const Int& 	max_step1_cnt()const{ return this->max_step1_cnt_; }

	};

}// devroye
}// poisson
}// random
}// boost

#endif


#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <fmt/format.h>
#include <string>
#include <iomanip>
#include <boost/variant.hpp>
#include <unordered_map>

struct Not
{
};
struct And
{
};
struct Or
{
};
struct Lshift
{
};
struct Rshift
{
};

template<typename OpTag>
struct binary_op;
//template<typename OpTag>
//struct unary_op;
typedef boost::variant<unsigned short,
  boost::recursive_wrapper<binary_op<And>>,
  boost::recursive_wrapper<binary_op<Or>>,
  boost::recursive_wrapper<binary_op<Lshift>>,
  boost::recursive_wrapper<binary_op<Rshift>>,
  boost::recursive_wrapper<binary_op<Not>>>
  Expr;
typedef std::unordered_map<std::string, Expr> ExprMap;
template<typename OpTag>
struct binary_op
{
  Expr left;
  Expr right;

  binary_op(const Expr &lhs, const Expr &rhs)
    : left(lhs), right(rhs)
  {
  }
  binary_op() {}
};
template<class IntType>
class Calculator : public boost::static_visitor<IntType>
{
public:
  using value_type = IntType;
  IntType operator()(IntType value) const
  {
    //std::cout << "calculator value:" << value << std::endl;
    return value;
  }

  IntType operator()(const binary_op<And> &binary) const
  {
    return boost::apply_visitor(Calculator(), binary.left)
           & boost::apply_visitor(Calculator(), binary.right);
  }

  IntType operator()(const binary_op<Or> &binary) const
  {
    return boost::apply_visitor(Calculator(), binary.left)
           | boost::apply_visitor(Calculator(), binary.right);
  }
  IntType operator()(const binary_op<Lshift> &binary) const
  {
    return boost::apply_visitor(Calculator(), binary.left)
           << boost::apply_visitor(Calculator(), binary.right);
  }
  IntType operator()(const binary_op<Rshift> &binary) const
  {
    return boost::apply_visitor(Calculator(), binary.left)
           >> boost::apply_visitor(Calculator(), binary.right);
  }
  IntType operator()(const binary_op<Not> &binary) const
  {
    return ~(boost::apply_visitor(Calculator(), binary.left));
  }
};
using AocCal = Calculator<unsigned short>;
struct IS
{
  AocCal c_;
  ExprMap exprmap_;
  AocCal::value_type operator()(const std::string &s) const
  {
    if (auto i = exprmap_.find(s); i != exprmap_.end()) {
      AocCal::value_type r = boost::apply_visitor(c_, i->second);
      return r;
    } else {
      return 0;
    }
  }
};

int main(int argc, char **argv)
{
  Expr e;
  e = 32;

  if (argc > 1) {
    std::ifstream ifs(argv[1]);
    std::string line;
    IS asms;
    while (std::getline(ifs, line)) {
      std::istringstream iss(line);
      std::vector<std::string> v;
      std::istream_iterator<std::string> b(iss);
      std::istream_iterator<std::string> e;
      std::copy(b, e, std::back_inserter(v));
      if (v.size() == 3) {
        //std::cout << v[0] << "\n";
        if (std::isdigit(v[0][0])) {
          auto n = static_cast<unsigned short>(std::stoi(v[0]));
          asms.exprmap_[v[2]] = n;
        } else {
          asms.exprmap_[v[2]] = asms.exprmap_[v[0]];
        }
      } else if (v.size() == 4) {
        asms.exprmap_[v[3]] = binary_op<Not>(asms.exprmap_[v[1]], asms.exprmap_[v[1]]);
      } else if (v.size() == 5) {
        if (v[1] == "OR") {
          asms.exprmap_[v[4]] = binary_op<Or>(asms.exprmap_[v[0]], asms.exprmap_[v[2]]);
        } else if (v[1] == "AND") {
          asms.exprmap_[v[4]] = binary_op<And>(asms.exprmap_[v[0]], asms.exprmap_[v[2]]);
        } else if (v[1] == "LSHIFT") {
          //std::cout << v[2] << "\n";
          asms.exprmap_[v[4]] = binary_op<Lshift>(asms.exprmap_[v[0]], std::stoi(v[2]));
        } else if (v[1] == "RSHIFT") {
          //std::cout << v[2] << "\n";
          asms.exprmap_[v[4]] = binary_op<Rshift>(asms.exprmap_[v[0]], std::stoi(v[2]));
        }
      }
    }
    std::cout << asms("d") << std::endl;
    std::cout << asms("e") << std::endl;
    std::cout << asms("f") << std::endl;
    std::cout << asms("g") << std::endl;
    std::cout << asms("h") << std::endl;
    std::cout << asms("i") << std::endl;
    std::cout << asms("x") << std::endl;
    std::cout << asms("y") << std::endl;
  }
}


#include "distributions.h"

#include <Eigen/Dense>
#include <random>
#include <stan/math/prim.hpp>

#include "src/utils/proto_utils.h"

int bayesmix::categorical_rng(const Eigen::VectorXd &probas,
                              std::mt19937_64 &rng, int start /*= 0*/) {
  return stan::math::categorical_rng(probas, rng) + (start - 1);
}

double bayesmix::multi_normal_prec_lpdf(const Eigen::VectorXd &datum,
                                        const Eigen::VectorXd &mean,
                                        const Eigen::MatrixXd &prec_chol,
                                        double prec_logdet) {
  using stan::math::NEG_LOG_SQRT_TWO_PI;
  double base = prec_logdet + NEG_LOG_SQRT_TWO_PI * datum.size();
  double exp = (prec_chol * (datum - mean)).squaredNorm();
  return 0.5 * (base - exp);
}

Eigen::VectorXd bayesmix::multi_normal_prec_lpdf_grid(
    const Eigen::MatrixXd &data, const Eigen::VectorXd &mean,
    const Eigen::MatrixXd &prec_chol, double prec_logdet) {
  using stan::math::NEG_LOG_SQRT_TWO_PI;
  Eigen::VectorXd exp =
      ((data.rowwise() - mean.transpose()) * prec_chol.transpose())
          .rowwise()
          .squaredNorm();
  Eigen::VectorXd base = Eigen::ArrayXd::Ones(data.rows()) * prec_logdet +
                         NEG_LOG_SQRT_TWO_PI * data.cols();
  return (base - exp) * 0.5;
}

double bayesmix::multi_student_t_invscale_lpdf(
    const Eigen::VectorXd &datum, double df, const Eigen::VectorXd &mean,
    const Eigen::MatrixXd &invscale_chol, double scale_logdet) {
  int dim = datum.size();
  double exp =
      0.5 * (df + dim) *
      std::log(1 + (invscale_chol * (datum - mean)).squaredNorm() / df);
  double base = stan::math::lgamma((df + dim) * 0.5) -
                stan::math::lgamma(df * 0.5) - (0.5 * dim) * std::log(df) -
                (0.5 * dim) * stan::math::LOG_PI + 0.5 * scale_logdet;
  return base - exp;
}

Eigen::VectorXd bayesmix::multi_student_t_invscale_lpdf_grid(
    const Eigen::MatrixXd &data, double df, const Eigen::VectorXd &mean,
    const Eigen::MatrixXd &invscale_chol, double scale_logdet) {
  int dim = data.cols();
  int n = data.rows();
  double base_coeff = stan::math::lgamma((df + dim) * 0.5) -
                      stan::math::lgamma(df * 0.5) -
                      (0.5 * dim) * std::log(df) -
                      (0.5 * dim) * stan::math::LOG_PI + 0.5 * scale_logdet;
  Eigen::VectorXd base = Eigen::VectorXd::Ones(n) * base_coeff;
  Eigen::VectorXd quadforms =
      ((data.rowwise() - mean.transpose()) * invscale_chol.transpose())
          .rowwise()
          .squaredNorm();
  Eigen::VectorXd exp =
      (quadforms.array() / df + 1.0).log() * 0.5 * (df + dim);
  return base - exp;
}

double bayesmix::gaussian_mixture_dist(
    Eigen::VectorXd means1, Eigen::VectorXd vars1, Eigen::VectorXd weights1,
    Eigen::VectorXd means2, Eigen::VectorXd vars2, Eigen::VectorXd weights2) {
  double mix1 = 0.0;
#pragma omp parallel for collapse(2) reduction(+ : mix1)
  for (int i = 0; i < means1.size(); i++) {
    for (int j = 0; j < means1.size(); j++) {
      mix1 += weights1(i) * weights1(j) *
              std::exp(stan::math::normal_lpdf(means1(i), means1(j),
                                               vars1(i) + vars1(j)));
    }
  }

  double mix2 = 0.0;
#pragma omp parallel for collapse(2) reduction(+ : mix2)
  for (int i = 0; i < means2.size(); i++) {
    for (int j = 0; j < means2.size(); j++) {
      mix2 += weights2(i) * weights2(j) *
              std::exp(stan::math::normal_lpdf(means2(i), means2(j),
                                               vars2(i) + vars2(j)));
    }
  }

  double inter = 0.0;
#pragma omp parallel for collapse(2) reduction(+ : inter)
  for (int i = 0; i < means1.size(); i++) {
    for (int j = 0; j < means2.size(); j++) {
      inter += weights1(i) * weights2(j) *
               std::exp(stan::math::normal_lpdf(means1(i), means2(j),
                                                vars1(i) + vars2(j)));
    }
  }

  return mix1 + mix2 - 2 * inter;
}

double bayesmix::gaussian_mixture_dist(std::vector<Eigen::VectorXd> means1,
                                       std::vector<Eigen::MatrixXd> precs1,
                                       Eigen::VectorXd weights1,
                                       std::vector<Eigen::VectorXd> means2,
                                       std::vector<Eigen::MatrixXd> precs2,
                                       Eigen::VectorXd weights2) {
  std::vector<Eigen::MatrixXd> vars1;
  std::vector<Eigen::MatrixXd> vars2;

  for (const auto &p : precs1) vars1.push_back(stan::math::inverse_spd(p));

  for (const auto &p : precs2) vars2.push_back(stan::math::inverse_spd(p));

  double mix1 = 0.0;
  for (int i = 0; i < means1.size(); i++) {
    for (int j = 0; j < means1.size(); j++) {
      Eigen::MatrixXd var_ij = vars1[i] + vars1[j];
      mix1 += weights1(i) * weights1(j) *
              std::exp(
                  stan::math::multi_normal_lpdf(means1[i], means1[j], var_ij));
    }
  }

  double mix2 = 0.0;
  for (int i = 0; i < means2.size(); i++) {
    for (int j = 0; j < means2.size(); j++) {
      Eigen::MatrixXd var_ij = vars2[i] + vars2[j];
      mix2 += weights2(i) * weights2(j) *
              std::exp(
                  stan::math::multi_normal_lpdf(means2[i], means2[j], var_ij));
    }
  }

  double inter = 0.0;
  for (int i = 0; i < means1.size(); i++) {
    for (int j = 0; j < means2.size(); j++) {
      Eigen::MatrixXd var_ij = vars1[i] + vars2[j];
      inter += weights1(i) * weights2(j) *
               std::exp(stan::math::multi_normal_lpdf(means1[i], means2[j],
                                                      var_ij));
    }
  }

  return mix1 + mix2 - 2 * inter;
}

double bayesmix::gaussian_mixture_dist(
    std::vector<bayesmix::AlgorithmState::ClusterState> clus1,
    Eigen::VectorXd weights1,
    std::vector<bayesmix::AlgorithmState::ClusterState> clus2,
    Eigen::VectorXd weights2) {
  double out;

  if (clus1[0].has_uni_ls_state()) {
    Eigen::VectorXd means1(clus1.size());
    Eigen::VectorXd vars1(clus1.size());
    Eigen::VectorXd means2(clus2.size());
    Eigen::VectorXd vars2(clus2.size());
    for (int i = 0; i < clus1.size(); i++) {
      means1(i) = clus1[i].uni_ls_state().mean();
      vars1(i) = clus1[i].uni_ls_state().var();
    }

    for (int i = 0; i < clus2.size(); i++) {
      means2(i) = clus2[i].uni_ls_state().mean();
      vars2(i) = clus2[i].uni_ls_state().var();
    }

    out = gaussian_mixture_dist(means1, vars1, weights1, means2, vars2,
                                weights2);
  } else if (clus1[0].has_multi_ls_state()) {
    std::vector<Eigen::VectorXd> means1, means2;
    std::vector<Eigen::MatrixXd> precs1, precs2;

    for (const auto &c : clus1) {
      means1.push_back(bayesmix::to_eigen(c.multi_ls_state().mean()));
      precs1.push_back(bayesmix::to_eigen(c.multi_ls_state().prec()));
    }

    for (const auto &c : clus2) {
      means2.push_back(bayesmix::to_eigen(c.multi_ls_state().mean()));
      precs2.push_back(bayesmix::to_eigen(c.multi_ls_state().prec()));
    }

    out = gaussian_mixture_dist(means1, precs1, weights1, means2, precs2,
                                weights2);
  } else {
    throw std::invalid_argument("Parameter type not recognized");
  }

  return out;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock. 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_MP_CPP_COMPLEX_HPP
#define BOOST_MP_CPP_COMPLEX_HPP

#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/complex_adaptor.hpp>

namespace boost {
   namespace multiprecision {

#ifndef BOOST_NO_CXX11_TEMPLATE_ALIASES

      template <unsigned Digits, backends::digit_base_type DigitBase = backends::digit_base_10, class Allocator = void, class Exponent = int, Exponent MinExponent = 0, Exponent MaxExponent = 0>
      using cpp_complex_backend = complex_adaptor<cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinExponent, MaxExponent> >;

      template <unsigned Digits, backends::digit_base_type DigitBase = digit_base_10, class Allocator = void, class Exponent = int, Exponent MinExponent = 0, Exponent MaxExponent = 0, expression_template_option ExpressionTemplates = et_off>
      using cpp_complex = number<complex_adaptor<cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinExponent, MaxExponent> >, ExpressionTemplates>;

      typedef cpp_complex<50> cpp_complex_50;
      typedef cpp_complex<100> cpp_complex_100;

      typedef cpp_complex<24, backends::digit_base_2, void, boost::int16_t, -126, 127> cpp_complex_single;
      typedef cpp_complex<53, backends::digit_base_2, void, boost::int16_t, -1022, 1023> cpp_complex_double;
      typedef cpp_complex<64, backends::digit_base_2, void, boost::int16_t, -16382, 16383> cpp_complex_extended;
      typedef cpp_complex<113, backends::digit_base_2, void, boost::int16_t, -16382, 16383> cpp_complex_quad;
      typedef cpp_complex<237, backends::digit_base_2, void, boost::int32_t, -262142, 262143> cpp_complex_oct;

#else

      typedef number<complex_adaptor<cpp_bin_float<50> >, et_off> cpp_complex_50;
      typedef number<complex_adaptor<cpp_bin_float<100> >, et_off> cpp_complex_100;

      typedef number<complex_adaptor<cpp_bin_float<24, backends::digit_base_2, void, boost::int16_t, -126, 127> >, et_off> cpp_complex_single;
      typedef number<complex_adaptor<cpp_bin_float<53, backends::digit_base_2, void, boost::int16_t, -1022, 1023> >, et_off> cpp_complex_double;
      typedef number<complex_adaptor<cpp_bin_float<64, backends::digit_base_2, void, boost::int16_t, -16382, 16383> >, et_off> cpp_complex_extended;
      typedef number<complex_adaptor<cpp_bin_float<113, backends::digit_base_2, void, boost::int16_t, -16382, 16383> >, et_off> cpp_complex_quad;
      typedef number<complex_adaptor<cpp_bin_float<237, backends::digit_base_2, void, boost::int32_t, -262142, 262143> >, et_off> cpp_complex_oct;

#endif

   }
}

#endif


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Generates constant Log_10 : \f$\log(10)\f$

    @par Semantic:

    @code
    T r = Log_10<T>();
    @endcode

    is similar to:

    @code
      r =  T(2.302585092994045684017991454684364207601101488628773);
    @endcode

**/
  template<typename T> T Log_10();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Generates constant Log_10. (\f$\log(10)\f$)

      Generate the  constant log_10.

      @return The Log_10 constant for the proper type
    **/
    const boost::dispatch::functor<tag::log_10_> log_10 = {};
  }
} }
#endif

#include <boost/simd/constant/definition/log_10.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


///////////////////////////////////////////////////////////////////////////////
// percentage_effective_sample_size.hpp                                      //
//                                                                           //
//  Copyright 2008 Erwann Rogard. 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_PERCENTAGE_EFFECTIVE_SAMPLE_SIZE_HPP_ER_2008_04
#define BOOST_ACCUMULATORS_STATISTICS_PERCENTAGE_EFFECTIVE_SAMPLE_SIZE_HPP_ER_2008_04
#include <cmath>

#include <boost/mpl/size_t.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/placeholders.hpp>

#include <boost/call_traits.hpp>
#include <boost/array.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/type_traits/add_const.hpp>
#include <boost/numeric/conversion/converter.hpp>

#include <boost/accumulators/framework/extractor.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/integrated_acvf.hpp>
#include <boost/accumulators/statistics/acv0.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ////////////////////////////////////////////////////////////////////////////
    // percentage_effective_sample_size
    template<typename T,typename I>
    class percentage_effective_sample_size_impl
      : public accumulator_base
    {
    public:
        typedef std::size_t result_type;

        percentage_effective_sample_size_impl(dont_care):val(0){}

        template<typename Args>
        void operator()(const Args& args)
        {
            T iacvf = integrated_acvf<I>(args[accumulator]);
            if(iacvf>static_cast<T>(0)){

                typedef boost::numeric::converter<result_type,T> T2res;
                T acv0_val = acv0<I>(args[accumulator]);
                T tmp = static_cast<T>(100)*acv0_val/iacvf;
                val = T2res::convert(tmp);
            }else{
                val = 0;
            }
        }

        result_type result(dont_care) const
        {
            return val;
        }
    private:
        result_type val;
    };

} // namespace impl
///////////////////////////////////////////////////////////////////////////////
// tag::integrated_acvf
//

namespace tag
{
    template <typename I = default_delay_discriminator>
    struct percentage_effective_sample_size
      : depends_on<acv0<I>, integrated_acvf<I> >
    {
        /// INTERNAL ONLY
      typedef
        accumulators::impl::percentage_effective_sample_size_impl<
            mpl::_1,I> impl;

    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::percentage_effective_sample_size
//

namespace extract
{

//  extractor<tag::percentage_effective_sample_size<> >
//    const percentage_effective_sample_size = {};

  // see acvf about default_delay_discriminator
  template<typename I,typename AccumulatorSet>
  typename mpl::apply<
    AccumulatorSet,tag::percentage_effective_sample_size<I>
    >::type::result_type
  percentage_effective_sample_size(AccumulatorSet const& acc){
    typedef tag::percentage_effective_sample_size<I> the_tag;
    return extract_result<the_tag>(acc);
  }

//  TODO
//  overload (default) see acvf

}

using extract::percentage_effective_sample_size;
}}
#endif


#include <cstdio>
#include <boost/math/distributions/beta.hpp>

#include <iostream>
#include <string>
#include <map>
#include <random>

using namespace boost::math;
using namespace std;

int main() {
	srand(time(0));
	double alpha = 0.5, beta = 0.5;
	beta_distribution<> dist(alpha, beta);

	for (int i = 0 ; i < 1000 ; i++) {
		double rngfloat = (double)(rand() % 101) / 100;
		cout<<" "<<quantile(dist, rngfloat)<<endl;
	}
}


#pragma once

#include <Eigen/Sparse>
#include <vector>

#include "mtao/eigen/shape_checks.hpp"
#include "mtao/geometry/volume.hpp"

namespace mtao::simulation::simplicial {
template <typename VDerived, typename SDerived>
Eigen::SparseMatrix<typename VDerived::Scalar> laplacian(
    const Eigen::MatrixBase<VDerived>& V, const Eigen::MatrixBase<SDerived>& S);

namespace internal {
    // NOTE: yes we do have a second impl in mtao/geometry/mesh/laplacian.hpp
template <typename VDerived, typename SDerived>
Eigen::SparseMatrix<typename VDerived::Scalar> laplacian2(
    const Eigen::MatrixBase<VDerived>& V,
    const Eigen::MatrixBase<SDerived>& S) {
    using Scalar = typename VDerived::Scalar;
    if (S.size() == 0) {
        return {};
    }
    int size = S.maxCoeff() + 1;
    Eigen::SparseMatrix<Scalar> L(size, size);
    std::vector<Eigen::Triplet<Scalar>> trips;
    trips.reserve(4 * 3 * S.cols());
    eigen::row_check<3>(S);
    assert(V.cols() >= size);

    for (int sidx = 0; sidx < S.cols(); ++sidx) {
        auto s = S.col(sidx);
        for (int j = 0; j < S.rows(); ++j) {
            const auto ai = s(j);
            const auto bi = s((j + 1) % 3);
            const auto ci = s((j + 2) % 3);
            auto a = V.col(ai);
            auto b = V.col(bi);
            auto c = V.col(ci);
            auto u = b - a;
            auto v = c - a;

            Scalar dot = u.dot(v);
            Scalar cross;
            if constexpr (VDerived::RowsAtCompileTime == 2) {
                cross = u.x() * v.y() - u.y() * v.x();
            } else if constexpr (VDerived::RowsAtCompileTime == 3) {
                cross = u.cross(v);
            }
            Scalar val = std::abs(.5 * dot / cross);
            trips.emplace_back(bi, ci, -val);
            trips.emplace_back(ci, bi, -val);
            trips.emplace_back(bi, bi, val);
            trips.emplace_back(ci, ci, val);
        }
    }
    L.setFromTriplets(trips.begin(), trips.end());
    return L;
}

template <typename VDerived, typename SDerived>
Eigen::SparseMatrix<typename VDerived::Scalar> laplacian3(
    const Eigen::MatrixBase<VDerived>& V,
    const Eigen::MatrixBase<SDerived>& S) {
    using Scalar = typename VDerived::Scalar;
    if (S.size() == 0) {
        return {};
    }
    int size = S.maxCoeff() + 1;
    Eigen::SparseMatrix<Scalar> L(size, size);
    std::vector<Eigen::Triplet<Scalar>> trips;
    trips.reserve(4 * 4 * S.cols());

    eigen::row_check<4>(S);
    eigen::row_check<3>(V);

    auto vols = geometry::volumes(V, S);

    assert(V.cols() >= size);
    for (int sidx = 0; sidx < S.cols(); ++sidx) {
        auto s = S.col(sidx);
        auto vol = vols(sidx);
        mtao::Matrix<Scalar, 3, 4> N;

        for (int j = 0; j < S.rows(); ++j) {
            const auto ai = s(j);
            const auto bi = s((j + 1) % 4);
            const auto ci = s((j + 2) % 4);
            const auto di = s((j + 3) % 4);
            auto a = V.col(ai);
            auto b = V.col(bi);
            auto c = V.col(ci);
            auto d = V.col(di);
            auto u = c - b;
            auto v = d - b;
            auto n = N.col(j);
            n = u.cross(v);
            n.normalize();
            Scalar len = (a - b).dot(n);
            if (len < 0) {
                n *= -len * len;
            } else {
                n *= len * len;
            }
        }
        for (int j = 0; j < S.rows(); ++j) {
            const auto ai = s(j);
            auto na = N.col(j);
            for (int k = j+1; k < S.rows(); ++k) {
                const auto bi = s(k);
                auto nb = N.col(k);
                Scalar val = vol * na.dot(nb);
                trips.emplace_back(ai, bi, -val);
                trips.emplace_back(bi, ai, -val);
                trips.emplace_back(ai, ai, val);
                trips.emplace_back(bi, bi, val);
            }
        }
    }
    L.setFromTriplets(trips.begin(), trips.end());
    return L;
}
}  // namespace internal

template <typename VDerived, typename SDerived>
Eigen::SparseMatrix<typename VDerived::Scalar> laplacian(
    const Eigen::MatrixBase<VDerived>& V,
    const Eigen::MatrixBase<SDerived>& S) {
    // TODO: make this use V with constant col sizes
    if constexpr(SDerived::RowsAtCompileTime == 3) {
        return internal::laplacian2(V, S);
    } else if constexpr(SDerived::RowsAtCompileTime == 4) {
        return internal::laplacian3(V, S);
    }
    return {};
}

}  // namespace mtao::simulation::simplicial


/**
 * August 8th, 2018
 * source: https://zhuanlan.zhihu.com/p/38745950
 * Constant velocity prediction Kalman filter for shield local position
 */

#include <iostream>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/TwistStamped.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include "rm_cv/ArmorRecord.h"

using namespace std;
using namespace Eigen;

string visual_topic, publisher_topic, debug_topic, real_visual_topic, transform_topic;
ros::Publisher filter_pub, debug_pub, transform_pub;
MatrixXd imu_R_camera = MatrixXd::Identity(3, 3); // rotation matrix from camera to imu
Vector3d imu_T_camera = MatrixXd::Zero(3, 1);

VectorXd x(6);      // state
MatrixXd P = MatrixXd::Identity(6, 6); // covariance
MatrixXd Q = MatrixXd::Identity(6, 6); // prediction noise covariance
MatrixXd R = MatrixXd::Identity(3, 3); // observation noise covariance
MatrixXd A = MatrixXd::Identity(6, 6); // state transfer function

bool visual_initialized = false, visual_valid = false;

ros::Time t_prev;
double R_pos, Q_pos, Q_vel, P_weight;
const double CV_UPDATE_TIME_MAX = 0.1; // maxium allowed update time
const double DELAY_MAX = 0.05;
const int ROS_FREQ = 100;
double OUTLIER_THRESHOLD = 10000.0;

Vector3d camera_T_shield_prev = MatrixXd::Zero(3, 1);
Vector3d imu_T_shield_prev = MatrixXd::Zero(3, 1);
bool vel_is_outlier = false;
double chi_square = 0;
double outlier_l2_norm_ratio = 1.5;
double yaw_delay = 0.0;
double pitch_delay = 0.0;
Vector3d OUTPUT_BOUND = MatrixXd::Zero(3, 1);;

static void pub_result(const ros::Time &stamp, double delay_dt)
{
//    geometry_msgs::TwistStamped odom;
    rm_cv::ArmorRecord odom;
    odom.header.stamp = stamp;
    double predict_x = x(0) + (delay_dt + yaw_delay)   * x(3);
    double predict_y = x(1) + (delay_dt + pitch_delay) * x(4);
    double predict_z = x(2) + delay_dt * x(5);
    odom.armorPose.linear.x  = (predict_x < OUTPUT_BOUND[0]) ? predict_x * 1000 : OUTPUT_BOUND[0] * 1000;
    odom.armorPose.linear.y  = (predict_y < OUTPUT_BOUND[1]) ? predict_y * 1000 : OUTPUT_BOUND[1] * 1000;
    odom.armorPose.linear.z  = (predict_z < OUTPUT_BOUND[2]) ? predict_z * 1000 : OUTPUT_BOUND[2] * 1000;
    odom.armorPose.angular.x = x(3) * 1000;
    odom.armorPose.angular.y = x(4) * 1000;
    odom.armorPose.angular.z = x(5) * 1000;
    filter_pub.publish(odom);
}

static void pub_preprocessed(const std_msgs::Header &header,
                      const Ref<const Vector3d> p,
                      const Ref<const Vector3d> v,
                      ros::Publisher &publisher)
{
    geometry_msgs::TwistStamped debug;
    debug.header = header;
    debug.twist.linear.x  = p[0] * 1000;
    debug.twist.linear.y  = p[1] * 1000;
    debug.twist.linear.z  = p[2] * 1000;
    debug.twist.angular.x = v[0] * 1000;
    debug.twist.angular.y = v[1] * 1000;
    debug.twist.angular.z = v[2] * 1000;
    publisher.publish(debug);
}

// Chi-square test for outlier rejection
static bool velocity_is_outlier(const Vector3d &pos)
{
    VectorXd r = MatrixXd::Zero(3, 1);
    VectorXd z = MatrixXd::Zero(3, 1);
    MatrixXd S = MatrixXd::Zero(3, 3);
    z << pos[0], pos[1], pos[2];

    double pos_norm   = z.segment<3>(0).norm();
    double state_norm = x.segment<3>(0).norm();
    ROS_INFO("pos_norm %f, state_norm %f", pos_norm, state_norm);
    if (pos_norm > state_norm * outlier_l2_norm_ratio) {
        return true;
    } else if (pos_norm < state_norm / outlier_l2_norm_ratio) {
        return true;
    }

    // r = z - H * x, residual
    // S = H P H' + R, residual covariance
    MatrixXd H = MatrixXd::Identity(3, 6); // observation matrix

//    MatrixXd K_next = (H * P * H.transpose() + R).ldlt().solve(P * H.transpose());
//    MatrixXd x_next = x + K_next * (z - H * x);
//    MatrixXd P_next = P - K_next * H * P;

    r = z - H * x;
    S = H * P * H.transpose() + R;
    chi_square = r.transpose() * S * r;
    // cout << "chi_square " << endl << chi_square << endl;

    return (chi_square > OUTLIER_THRESHOLD);
}

static void preprocess_visual(const std_msgs::Header &header,
                              const geometry_msgs::Twist &twist,
                              Vector3d &pos, double dt_update)
{

    Vector3d camera_T_shield, camera_vel_shield, imu_T_shield, imu_vel_shield;

    camera_T_shield[0] = twist.linear.x;
    camera_T_shield[1] = twist.linear.y;
    camera_T_shield[2] = twist.linear.z;

    camera_T_shield *= 0.001; // Convert millimeter to meter

    // calculate and check the velocity
    camera_vel_shield = (camera_T_shield - camera_T_shield_prev) / dt_update;
    camera_T_shield_prev = camera_T_shield;

    vel_is_outlier = velocity_is_outlier(camera_T_shield);

    if (vel_is_outlier) {
        pos = x.segment<3>(0);
        ROS_INFO("outlier rejected");
    }
    else {
        pos = camera_T_shield;
    }

    // store the state
    pub_preprocessed(header, camera_T_shield, camera_vel_shield, debug_pub);

    imu_T_shield = imu_R_camera * camera_T_shield + imu_T_camera * 0.001;
    imu_vel_shield = (imu_T_shield - imu_T_shield_prev) / dt_update;
    imu_T_shield_prev = imu_T_shield;
    pub_preprocessed(header, imu_T_shield, imu_vel_shield, transform_pub);
}

static void propagate(const double &dt) {
    A.topRightCorner(3, 3) = dt * MatrixXd::Identity(3, 3);
    // cout << "A " << endl << A << endl;
    x = A * x;
    P = A * P * A.transpose() + Q;
    // cout << "P " << endl << P << endl;
}

static void update(const Vector3d &pos)
{
    MatrixXd H, K, z;

    H = MatrixXd::Identity(3, 6); // observation matrix
    K = P * H.transpose() * (H * P * H.transpose() + R).inverse();
    z = MatrixXd::Zero(3, 1);
    z << pos[0], pos[1], pos[2];

    x = x + K * (z - H * x);
    P = P - K * H * P;
}

/**
 * initialization of the state and convariance from visual
 * @param pnp
 */
static void initialize_visual(const std_msgs::Header &header,
                              const geometry_msgs::Twist &twist)
{
    ros::Time t_update = header.stamp;
    ROS_INFO("visual init at %f", t_update.toSec());

    Vector3d camera_T_shield, imu_T_shield;
    camera_T_shield[0] = twist.linear.x;
    camera_T_shield[1] = twist.linear.y;
    camera_T_shield[2] = twist.linear.z;
    // imu_T_shield = imu_R_camera * camera_T_shield + imu_T_camera;
    camera_T_shield *= 0.001; // Convert millimeter to meter

    x.segment<3>(0) = camera_T_shield; // init velocity with zero
    x.segment<3>(3).setZero(); // init velocity with zero

    cout << "DEBUG: x initialized with " << endl << x.transpose() << endl;
    camera_T_shield_prev = camera_T_shield;
    t_prev = t_update;
    visual_initialized = true;
}

/**
 * handle, save, and process visual messages
 * @param pnp
 */
void visual_callback(const geometry_msgs::TwistStamped::ConstPtr &pnp)
{
    visual_valid = !(pnp->twist.linear.x == 0 &&
                     pnp->twist.linear.y == 0 &&
                     pnp->twist.linear.z == 0 );

    if (visual_valid) {
        if (!visual_initialized) {
            initialize_visual(pnp->header, pnp->twist);
        }
        else {
            double running_time = ros::Time::now().toSec();
            double delay_dt = running_time - pnp->header.stamp.toSec();
            delay_dt = (delay_dt < DELAY_MAX) ? delay_dt : DELAY_MAX;

            ros::Time t_update = pnp->header.stamp;
            double dt_update = (t_update - t_prev).toSec();
            dt_update = (dt_update < CV_UPDATE_TIME_MAX) ? dt_update : CV_UPDATE_TIME_MAX;

            Vector3d pos;

            preprocess_visual(pnp->header, pnp->twist, pos, dt_update);

            update(pos);

            pub_result(pnp->header.stamp, delay_dt);
            t_prev = t_update;
        }
    }
}

/**
 * handle, save, and process visual messages
 * @param armor
 */
void real_visual_cb(const rm_cv::ArmorRecord::ConstPtr &armor)
{
    visual_valid = !(armor->armorPose.linear.x == 0 &&
                     armor->armorPose.linear.y == 0 &&
                     armor->armorPose.linear.z == 0 );

    if (visual_valid) {
        if (!visual_initialized) {
            initialize_visual(armor->header, armor->armorPose);
        }
        else {

            double running_time = ros::Time::now().toSec();
            double delay_dt = running_time - armor->header.stamp.toSec();
            delay_dt = (delay_dt < DELAY_MAX) ? delay_dt : DELAY_MAX;

            ros::Time t_update = armor->header.stamp;
            double dt_update = (t_update - t_prev).toSec();
            dt_update = (dt_update < CV_UPDATE_TIME_MAX) ? dt_update : CV_UPDATE_TIME_MAX;

            Vector3d pos;
            preprocess_visual(armor->header, armor->armorPose, pos, dt_update);

            update(pos);

            propagate(dt_update);

            pub_result(armor->header.stamp, delay_dt);
            t_prev = t_update;
        }
    }

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "prediction_kalman_filter");
    ros::NodeHandle n("~");

    n.param("visual_topic", visual_topic, string("/pnp_twist"));
    n.param("real_visual_topic", real_visual_topic, string("/detected_armor"));
    n.param("publisher_topic", publisher_topic, string("/prediction_kf/predict"));
    n.param("debug_topic", debug_topic, string("/prediction_kf/preprocessed"));
    n.param("transform_topic", transform_topic, string("/prediction_kf/transformed"));
    n.param("chi_square_threshold", OUTLIER_THRESHOLD, 10000.0);
    n.param("outlier_l2_norm_ratio", outlier_l2_norm_ratio, 1.5);
    n.param("R_pos", R_pos, 16.0);
    n.param("Q_pos", Q_pos, 0.2);
    n.param("Q_vel", Q_vel, 1.0);
    n.param("P_matrix_weight", P_weight, 1.0);
    n.param("yaw_delay", yaw_delay, 0.0);
    n.param("pitch_delay", pitch_delay, 0.0);

    // For chassis reading only
    imu_R_camera <<  0, 0, 1,
                    -1, 0, 0,
                     0,-1, 0;
    imu_T_camera <<  110, -70, -40; // in millimeter

    OUTPUT_BOUND << 10.0, 10.0, 10.0;

    x.setZero();
    R = R_pos * MatrixXd::Identity(3, 3);
    // R(0, 1) = R_pos * 0.1;
    // R(0, 2) = R_pos * 0.1;
    // R(1, 2) = R_pos * 0.1;
    // R(1, 0) = R_pos * 0.1;
    // R(2, 0) = R_pos * 0.1;
    // R(2, 1) = R_pos * 0.1;
    Q.topLeftCorner(3, 3)     = Q_pos * MatrixXd::Identity(3, 3);
    Q.bottomRightCorner(3, 3) = Q_vel * MatrixXd::Identity(3, 3);
    P.topLeftCorner(3, 3)     = P_weight * MatrixXd::Identity(3, 3);
    // P.topRightCorner(3, 3)    = 0.5 * P_weight * MatrixXd::Identity(3, 3);
    // P.bottomLeftCorner(3, 3)  = 0.5 * P_weight * MatrixXd::Identity(3, 3);
    P.bottomRightCorner(3, 3) = 2 * P_weight * MatrixXd::Identity(3, 3);
    cout << "R " << endl << R << endl;
    cout << "Q " << endl << Q << endl;
    cout << "P " << endl << P << endl;

    ros::Subscriber s1 = n.subscribe(visual_topic, 40, visual_callback);
    ros::Subscriber s2 = n.subscribe(real_visual_topic, 40, real_visual_cb);
    filter_pub = n.advertise<rm_cv::ArmorRecord>(publisher_topic, 40);
    debug_pub  = n.advertise<geometry_msgs::TwistStamped>(debug_topic, 40);
    transform_pub = n.advertise<geometry_msgs::TwistStamped>(transform_topic, 40);
    ros::Rate r(ROS_FREQ);
    ros::spin();

}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    cpp_int x; cin >> x;
    for (int i = -118; i <= 119; i++) {
        for (int j = -119; j <= 118; j++) {
            if (i * i * i * i * i - j * j * j * j * j == x) {
                cout << i << " " << j << endl;
                return 0;
            }
        }
    }
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <cmath>

#include <boost/random.hpp>
#include <boost/make_shared.hpp>

#if USE_GSL
#include <gsl/gsl_qrng.h>
#endif

#include "tudat/math/statistics/randomSampling.h"
namespace tudat
{

namespace statistics
{

//! Generate sample of random vectors, with entries of each vector independently, but not identically, distributed.
std::vector< Eigen::VectorXd > generateRandomSampleFromGenerator(
        const int numberOfSamples,
        const std::vector< std::shared_ptr< RandomVariableGenerator< double > > > randomVariableGenerators )
{
    std::vector< Eigen::VectorXd > randomSamples;
    Eigen::VectorXd randomSample( randomVariableGenerators.size( ) );

    // Generate samples
    for( int i = 0; i < numberOfSamples; i++ )
    {
        for( unsigned int j = 0; j < randomVariableGenerators.size( ); j++ )
        {
            randomSample( j ) = randomVariableGenerators.at( j )->getRandomVariableValue( );
        }
        randomSamples.push_back( randomSample );
    }

    return randomSamples;
}

//! Generate sample of random vectors, with entries of each vector independently and identically distributed.
std::vector< Eigen::VectorXd > generateRandomSampleFromGenerator(
        const int numberOfSamples, const int numberOfDimensions,
        const std::shared_ptr< RandomVariableGenerator< double > > randomVariableGenerator )
{
    std::vector< std::shared_ptr< RandomVariableGenerator< double > > > randomVariableGenerators;
    for( int i = 0; i < numberOfDimensions; i++ )
    {
        randomVariableGenerators.push_back( randomVariableGenerator );
    }

    return generateRandomSampleFromGenerator( numberOfSamples, randomVariableGenerators );
}

//! Generator random vector using pseudo random generator
std::vector< Eigen::VectorXd > generateUniformRandomSample(
        const int seed, const int numberOfSamples,
        const Eigen::VectorXd& lowerBound, const Eigen::VectorXd& upperBound )
{
    if( lowerBound.rows( ) != upperBound.rows( ) )
    {
        throw std::runtime_error( "Error when making uniformly distributed samples, input is inconsistent" );
    }

    // Create distributions
    std::vector< std::shared_ptr< RandomVariableGenerator< double > > > randomVariableGenerators;
    std::vector< double > currentParameters;
    for( int i = 0; i < lowerBound.rows( ); i++ )
    {
        currentParameters = { lowerBound( i ), upperBound( i ) };
        randomVariableGenerators.push_back(
                    createBoostContinuousRandomVariableGenerator(
                        uniform_boost_distribution, currentParameters, seed + i ) );
    }

    // Generate samples
    return generateRandomSampleFromGenerator( numberOfSamples, randomVariableGenerators );
}

//! Generator random vector using pseudo random generator
std::vector< Eigen::VectorXd > generateUniformRandomSample(
        const int seed, const int numberOfSamples, const int numberOfDimensions,
         const double lowerBound, const double upperBound )
{
    return generateUniformRandomSample(
                seed, numberOfSamples,
                Eigen::VectorXd::Constant( numberOfDimensions, lowerBound ),
                Eigen::VectorXd::Constant( numberOfDimensions, upperBound ) );
}

//! Generator random vector using pseudo random generator with gaussian distribution (without correlation)
std::vector< Eigen::VectorXd > generateGaussianRandomSample(
        const int seed, const int numberOfSamples,
        const Eigen::VectorXd& mean, const Eigen::VectorXd& standardDeviation )
{
    if( mean.rows( ) != standardDeviation.rows( ) )
    {
        throw std::runtime_error( "Error when making Gaussian distributed samples, input is inconsistent" );
    }

    // Create distributions
    std::vector< std::shared_ptr< RandomVariableGenerator< double > > > randomVariableGenerators;
    std::vector< double > currentParameters;
    for( int i = 0; i < mean.rows( ); i++ )
    {
        currentParameters = { mean( i ), standardDeviation( i ) };
        randomVariableGenerators.push_back(
                    createBoostContinuousRandomVariableGenerator(
                        normal_boost_distribution, currentParameters, seed + i ) );
    }

    // Generate samples
    return generateRandomSampleFromGenerator( numberOfSamples, randomVariableGenerators );
}

//! Generator random vector using pseudo random generator with gaussian distribution (without correlation)
std::vector< Eigen::VectorXd > generateGaussianRandomSample(
        const int seed, const int numberOfSamples, const int numberOfDimensions,
        const double mean, const double standardDeviation )
{
    return generateGaussianRandomSample(
                seed, numberOfSamples,
                Eigen::VectorXd::Constant( numberOfDimensions, mean ),
                Eigen::VectorXd::Constant( numberOfDimensions, standardDeviation ) );
}

#if USE_GSL

//! Generator random vector using Sobol sampler
std::vector< Eigen::VectorXd > generateVectorSobolSample(
        int numberOfSamples,
        const Eigen::VectorXd& lowerBound, const Eigen::VectorXd& upperBound )
{
    int numberOfDimensions = upperBound.rows( );

    // Compute propertie
    Eigen::VectorXd width = upperBound - lowerBound;
    Eigen::VectorXd average = (upperBound + lowerBound)/2.0 ;

    std::vector< Eigen::VectorXd > sobolSamples( numberOfSamples );

    Eigen::VectorXd randomSample( numberOfDimensions ) ;
    double randomSampleArray[ numberOfDimensions ];

    gsl_qrng * q = gsl_qrng_alloc (gsl_qrng_sobol, numberOfDimensions );

    // Loop over samples
    for( int j = 0 ; j < numberOfSamples ; j++ )
    {
        gsl_qrng_get( q, randomSampleArray ); // Generate sobol [0,1]

        // Fill vector
        for( int i = 0 ; i < numberOfDimensions ; i++ )
        {
            randomSample( i ) = randomSampleArray[ i ] - 0.5 ;
        }
        sobolSamples[ j ] = randomSample.cwiseProduct( width ) + average ; // Save vector
    }

    gsl_qrng_free (q); // Deallocate GSL variables

    return sobolSamples;
}

//! Generator random vector using Sobol sampler
std::vector< Eigen::VectorXd > generateVectorSobolSample(
        const int numberOfDimensions, int numberOfSamples,
        const double lowerBound, const double upperBound )
{
    return generateVectorSobolSample( numberOfSamples,
                                      Eigen::VectorXd::Constant( numberOfDimensions, lowerBound ),
                                      Eigen::VectorXd::Constant( numberOfDimensions, upperBound ) );
}
#endif

} // Close Namespace statistics

} // Close Namespace tudat