// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG, www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also tools/license/license.mtl.txt in the distribution.

#ifndef MTL_VECTOR_MAKE_SPARSE_INCLUDE
#define MTL_VECTOR_MAKE_SPARSE_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/operation/size.hpp>
#include <boost/numeric/mtl/operation/update.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/inserter.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>

namespace mtl { namespace vector {

// Commands in Matlab
// S = sparse(i,j,s,m,n,nzmax)
// S = sparse(i,j,s,m,n)
// S = sparse(i,j,s)
// S = sparse(m,n)

template <typename SizeVector, typename ValueVector>
struct make_sparse_trait
{
    typedef typename Collection<SizeVector>::value_type  size_type;
    typedef typename Collection<ValueVector>::value_type value_type;
    typedef matrix::parameters<row_major, index::c_index, mtl::non_fixed::dimensions, false, size_type> paras;
    typedef matrix::compressed2D<value_type, paras>   type;
};

/// Generates an \p m by \p n matrix from the vectors \p rows, \p cols, and \p values.
/** A sparse matrix is created (compressed2D). The value type is the same as the element type
    of the value vector and the size type the same as the entries of the vectors with the row indices.
    Zero entries in \p values are ignored. Entries with same coordinates are added.
    Same as Matlab's sparse function besides that it is zero-indexed.
 **/
template <typename SizeVector1, typename SizeVector2, typename ValueVector>
inline typename make_sparse_trait<SizeVector1, ValueVector>::type
make_sparse(const SizeVector1& rows, const SizeVector2& cols, const ValueVector& values,
	    std::size_t m, std::size_t n)
{
    MTL_THROW_IF(size(rows) != size(cols), incompatible_size());
    MTL_THROW_IF(size(rows) != size(values), incompatible_size());

    typedef make_sparse_trait<SizeVector1, ValueVector> traits;
    typedef typename traits::type       matrix_type;
    typedef typename traits::value_type value_type;
    typedef typename traits::size_type  size_type;

    size_type               ms(m), ns(n);  // shouldn't be needed :-!
    matrix_type             A(ms, ns);
    matrix::inserter<matrix_type, update_plus<value_type> > ins(A, size_type(size(rows) / m + 1));

    for (std::size_t i= 0; i < size(rows); i++)
	if (values[i] != value_type(0))
	    ins[rows[i]][cols[i]] << values[i];
    return A;
}

/// Generates an \p m by \p n matrix from the vectors \p rows, \p cols, and \p values.
/** A sparse matrix is created (compressed2D). The value type is the same as the element type
    of the value vector and the size type the same as the entries of the vectors with the row indices.
    Zero entries in \p values are ignored. Entries with same coordinates are added. Last parameter
    is ignored and can be omitted.
    Same as Matlab's sparse function besides that it is zero-indexed.
 **/
template <typename SizeVector1, typename SizeVector2, typename ValueVector>
inline typename make_sparse_trait<SizeVector1, ValueVector>::type
make_sparse(const SizeVector1& rows, const SizeVector2& cols, const ValueVector& values,
	    std::size_t m, std::size_t n, std::size_t)
{
    return make_sparse(rows, cols, values, m, n);
}

/// Generates a matrix from the vectors \p rows, \p cols, and \p values.
/** A sparse matrix is created (compressed2D).
    The number of rows/columns is one plus the maximum of the entries of \p rows and \p cols.
    The value type is the same as the element type
    of the value vector and the size type the same as the entries of the vectors with the row indices.
    Zero entries in \p values are ignored. Entries with same coordinates are added.
    Same as Matlab's sparse function besides that it is zero-indexed.
 **/
template <typename SizeVector1, typename SizeVector2, typename ValueVector>
inline typename make_sparse_trait<SizeVector1, ValueVector>::type
make_sparse(const SizeVector1& rows, const SizeVector2& cols, const ValueVector& values)
{
    return make_sparse(rows, cols, values, max(rows)+1, max(cols)+1);
}

/// Generates an empty \p m by \p n matrix.
/** A sparse matrix is created (compressed2D<double>).
    Same as Matlab's sparse function besides that it is zero-indexed.
 **/
inline matrix::compressed2D<double> make_sparse(std::size_t m, std::size_t n)
{
    return matrix::compressed2D<double>(m, n);
}

} // namespace :vector

    using vector::make_sparse;

} // namespace mtl

#endif // MTL_VECTOR_MAKE_SPARSE_INCLUDE


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/* sample.cpp - implementing various sampling routines */
#include <vector>
#include <NTL/ZZX.h>
#include <NTL/ZZ_pX.h>
#include <NTL/BasicThreadPool.h>
#include "NumbTh.h"
#include "FHEContext.h"
#include "sample.h"
#include "norms.h"

#include "powerful.h"
// only used in experimental Hwt sampler

NTL_CLIENT

// Sample a degree-(n-1) poly, with only Hwt nonzero coefficients
void sampleHWt(zzX &poly, long n, long Hwt)
{
  if (n<=0) n=lsize(poly); if (n<=0) return;
  if (Hwt>=n) {
#ifdef DEBUG_PRINTOUT
    std::cerr << "Hwt="<<Hwt<<">=n="<<n<<", is this ok?\n";
#endif
    Hwt = n-1;
  }
  poly.SetLength(n); // allocate space
  for (long i=0; i<n; i++) poly[i] = 0;

  long i=0;
  while (i<Hwt) {  // continue until exactly Hwt nonzero coefficients
    long u = NTL::RandomBnd(n);  // The next coefficient to choose
    if (poly[u]==0) { // if we didn't choose it already
      long b = NTL::RandomBits_long(2)&2; // b random in {0,2}
      poly[u] = b-1;                      //   random in {-1,1}

      i++; // count another nonzero coefficient
    }
  }
}
// Sample a degree-(n-1) ZZX, with only Hwt nonzero coefficients
void sampleHWt(ZZX &poly, long n, long Hwt)
{
  zzX pp;
  sampleHWt(pp, n, Hwt);
  convert(poly, pp);
}

// Sample a degree-(n-1) poly, with -1/0/+1 coefficients.
// Each coefficients is +-1 with probability prob/2 each,
// and 0 with probability 1-prob. By default, pr[nonzero]=1/2.
void sampleSmall(zzX &poly, long n, double prob)
{
  if (n<=0) n=lsize(poly); if (n<=0) return;
  //OLD: assert(prob>3.05e-5 && prob<=1); // prob must be in [2^{-15},1/2]
  helib::assertTrue<helib::InvalidArgument>(prob > 3.05e-5, "prob must be greater than 2^{-15}");
  helib::assertTrue<helib::InvalidArgument>(prob <= 1, "prob must be less than or equal to 1");
  poly.SetLength(n);

  constexpr long bitSize=16;
  constexpr long hiMask = (1<<(bitSize-1)); // top bit = 2^15
  constexpr long loMask = hiMask-1;         // bottom 15 bits

  long threshold = round(hiMask*prob); // threshold/2^15 = Pr[nonzero]

  NTL_EXEC_RANGE(n, first, last)
  for (long i=first; i<last; i++) {
    long u = NTL::RandomBits_long(bitSize); // a random 16-bit number
    long uLo = u & loMask; // bottom 15 bits
    long uHi = u & hiMask; // top bit

    // with probability threshold/2^15, choose between +-1
    if (uLo<threshold) { // compare low 15 bits to threshold
      poly[i] = (uHi>>(bitSize-2))-1; // topBit*2 - 1 \in {+-1}
    }

    // with probability 1-prob, set to zero
    else poly[i] = 0;
  }
  NTL_EXEC_RANGE_END
}
void sampleSmall(ZZX &poly, long n, double prob)
{
  zzX pp;
  sampleSmall(pp, n, prob);
  convert(poly.rep, pp);
  poly.normalize();
}

// Choose a vector of continuous Gaussians
void sampleGaussian(std::vector<double> &dvec, long n, double stdev)
{
  static double const Pi=4.0*atan(1.0);  // Pi=3.1415..
  static double const bignum = LONG_MAX; // convert to double
  // THREADS: C++11 guarantees these are initialized only once

  if (n<=0) n=lsize(dvec); if (n<=0) return;
  dvec.resize(n, 0.0);        // allocate space for n variables

  // Uses the Box-Muller method to get two Normal(0,stdev^2) variables
  for (long i=0; i<n; i+=2) {
    // r1, r2 are "uniform in (0,1)"
    double r1 = (1+NTL::RandomBnd(LONG_MAX))/(bignum+1);
    double r2 = (1+NTL::RandomBnd(LONG_MAX))/(bignum+1);
    double theta=2*Pi*r1;
    double rr= sqrt(-2.0*log(r2))*stdev;
    if (rr > 8*stdev) // sanity-check, trancate at 8 standard deviations
      rr = 8*stdev;

    // Generate two Gaussians RV's
    dvec[i] = rr*cos(theta);
    if (i+1 < n)
      dvec[i+1] = rr*sin(theta);
  }
}

// Sample a degree-(n-1) ZZX, with rounded Gaussian coefficients
void sampleGaussian(zzX &poly, long n, double stdev)
{
  if (n<=0) n=lsize(poly); if (n<=0) return;
  std::vector<double> dvec;
  sampleGaussian(dvec, n, stdev); // sample continuous Gaussians

  // round and copy to coefficients of poly
  clear(poly);
  poly.SetLength(n); // allocate space for degree-(n-1) polynomial
  for (long i=0; i<n; i++)
    poly[i] = long(round(dvec[i])); // round to nearest integer
}
// Sample a degree-(n-1) ZZX, with rounded Gaussian coefficients
void sampleGaussian(ZZX &poly, long n, double stdev)
{
  zzX pp;
  sampleGaussian(pp, n, stdev);
  convert(poly.rep, pp);
  poly.normalize();
}
#if 0
void sampleGaussian(ZZX &poly, long n, double stdev)
{
  static double const Pi=4.0*atan(1.0); // Pi=3.1415..
  static long const bignum = 0xfffffff;
  // THREADS: C++11 guarantees these are initialized only once

  if (n<=0) n=deg(poly)+1; if (n<=0) return;
  poly.SetMaxLength(n); // allocate space for degree-(n-1) polynomial
  for (long i=0; i<n; i++) SetCoeff(poly, i, ZZ::zero());

  // Uses the Box-Muller method to get two Normal(0,stdev^2) variables
  for (long i=0; i<n; i+=2) {
    double r1 = (1+RandomBnd(bignum))/((double)bignum+1);
    double r2 = (1+RandomBnd(bignum))/((double)bignum+1);
    double theta=2*Pi*r1;
    double rr= sqrt(-2.0*log(r2))*stdev;

    //OLD: assert(rr < 8*stdev); // sanity-check, no more than 8 standard deviations
    helib::assertTrue(rr < 8*stdev, "no more than 8 standard deviations");

    // Generate two Gaussians RV's, rounded to integers
    long x = (long) floor(rr*cos(theta) +0.5);
    SetCoeff(poly, i, x);
    if (i+1 < n) {
      x = (long) floor(rr*sin(theta) +0.5);
      SetCoeff(poly, i+1, x);
    }
  }
  poly.normalize(); // need to call this after we work on the coeffs
}
#endif

// Sample a degree-(n-1) zzX, with coefficients uniform in [-B,B]
void sampleUniform(zzX& poly, long n, long B)
{
  //OLD: assert (B>0);
  helib::assertTrue<helib::InvalidArgument>(B>0l, "Invalid coefficient interval");
  if (n<=0) n=lsize(poly); if (n<=0) return;
  poly.SetLength(n); // allocate space for degree-(n-1) polynomial

  for (long i = 0; i < n; i++)
    poly[i] = NTL::RandomBnd(2*B +1) - B;
}

// Sample a degree-(n-1) ZZX, with coefficients uniform in [-B,B]
void sampleUniform(ZZX& poly, long n, const ZZ& B)
{
  //OLD: assert (B>0);
  helib::assertTrue<helib::InvalidArgument>(static_cast<bool>(B>0l), "Invalid coefficient interval");
  if (n<=0) n=deg(poly)+1; if (n<=0) return;
  clear(poly);
  poly.SetMaxLength(n); // allocate space for degree-(n-1) polynomial

  ZZ UB = 2*B +1;
  for (long i = n-1; i >= 0; i--) {
    ZZ tmp = RandomBnd(UB) - B;
    SetCoeff(poly, i, tmp);
  }
}

/********************************************************************
 * Below are versions of the sampling routines that sample modulo
 * X^m-1 and then reduce mod Phi_m(X). The exception is when m is
 * a power of two, where we still sample directly mod Phi_m(X).
 ********************************************************************/
double sampleHWt(zzX &poly, const FHEcontext& context, long Hwt)
{
  const PAlgebra& palg = context.zMStar;
  double retval;

  if (palg.getPow2() == 0) { // not power of two
    long m = palg.getM();
    sampleHWt(poly, m, Hwt);
    reduceModPhimX(poly, palg);
    retval = context.noiseBoundForHWt(Hwt, m);
  }
  else { // power of two
    long phim = palg.getPhiM();
    sampleHWt(poly, phim, Hwt);
    retval = context.noiseBoundForHWt(Hwt, phim);
  }

  return retval;
}

double sampleHWtBoundedEffectiveBound(const FHEcontext& context, long Hwt)
{
#if FFT_IMPL // is there any implementation of canonicalEmbedding?
  const PAlgebra& palg = context.zMStar;

  long deg_bnd = (palg.getPow2() == 0) ? palg.getM() : palg.getPhiM();

  long log_deg_bnd = long(log(double(deg_bnd))/log(2.0) + 0.5) + 1;
  //  sqrt(2) * deg_bnd  <= 2^{log_deg_bnd} <= 2*sqrt(2) * deg_bnd

  // we use log_deg_bnd as in index into the erfc_inverse table
  // so that we get a noise bound that should be satisfied
  // with probablity at least 1/sqrt(2).

  //OLD: assert(log_deg_bnd < ERFC_INVERSE_SIZE);
  helib::assertTrue(log_deg_bnd < ERFC_INVERSE_SIZE, "log_deg_bnd must be less than ERFC_INVERSE_SIZE");
  double scale = erfc_inverse[log_deg_bnd];
  double bound = scale * sqrt(double(Hwt));

  return bound;
#else
  const PAlgebra& palg = context.zMStar;
  double retval;

  if (palg.getPow2() == 0) { // not power of two
    long m = palg.getM();
    retval = context.noiseBoundForHWt(Hwt, m);
  }
  else { // power of two
    long phim = palg.getPhiM();
    retval = context.noiseBoundForHWt(Hwt, phim);
  }
  return retval;
#endif
}

#if 1
double sampleHWtBounded(zzX &poly, const FHEcontext& context, long Hwt)
{
#if FFT_IMPL // is there any implementation of canonicalEmbedding?
  double bound = sampleHWtBoundedEffectiveBound(context, Hwt);
  const PAlgebra& palg = context.zMStar;

  double val;
  long count = 0;
  do {
    sampleHWt(poly, context, Hwt);
    val = embeddingLargestCoeff(poly,palg);
    //cerr << "****** " << (val/bound) << "\n";
  }
  while (++count<1000 && val>bound); // repeat until <= bound

  if (val>bound) {
    std::stringstream ss;
    ss << "Error: sampleSmallBounded, after "
         << count<<" trials, still val="<<val
         << '>'<<"bound="<<bound;
    throw helib::RuntimeError(ss.str());
  }
  return bound;
#else
  return sampleHWt(poly, context, Hwt);
#endif

}

#elif 0

// Experimental version

static ZZ
calculate_matrix_norm(const zzX& try_poly, const FHEcontext& context)
{
  const RecryptData& rcData = context.rcData;
  const PowerfulDCRT* p2dConv = rcData.p2dConv;
  const PAlgebra& palg = context.zMStar;
  long phim = palg.getPhiM();
  long m = palg.getM();
  const ZZX& PhimX = palg.getPhimX();

  ZZX poly;
  convert(poly, try_poly);

  IndexSet iset = IndexSet( context.ctxtPrimes.first(),
                            min( context.ctxtPrimes.first()+3,
                                context.ctxtPrimes.last() ) );
  // use a smaller prime set for powerful conversions
  // right now, we just use the first 3  ctxt primes

  ZZ retval {0};

  for (long i: range(phim)) {
    if (i%300 == 0) cerr << ".";
    Vec<ZZ> basis_vec;
    basis_vec.SetLength(phim);
    basis_vec[i] = 1;

    ZZX basis_poly;
    p2dConv->powerfulToZZX(basis_poly, basis_vec, iset);

    basis_poly = basis_poly*poly;

    // reduce basis_poly mod X^m-1, which is enough for ZZXtoPowerful
    long d = basis_poly.rep.length();

    if (d > m) {
      for (long j: range(m, d)) {
        basis_poly.rep[j-m] += basis_poly.rep[j];
      }
      basis_poly.rep.SetLength(m);
      basis_poly.normalize();
    }

    p2dConv->ZZXtoPowerful(basis_vec, basis_poly, iset);

    for (long j: range(phim)) {
      retval += basis_vec[j]*basis_vec[j];
    }
  }

  return retval;
}

double sampleHWtBounded(zzX &poly, const FHEcontext& context, long Hwt)
{
#if FFT_IMPL // is there any implementation of canonicalEmbedding?
  double bound = sampleHWtBoundedEffectiveBound(context, Hwt);
  const PAlgebra& palg = context.zMStar;

  ZZ best_matrix_norm;
  zzX best_poly;

  cerr << "*** starting trials\n";

  for (long trials = 0; trials < 20; trials++) {

    zzX try_poly;

    double val;
    long count = 0;
    do {
      sampleHWt(try_poly, context, Hwt);
      val = embeddingLargestCoeff(try_poly,palg);
      //cerr << "****** " << (val/bound) << "\n";
    }
    while (++count<1000 && val>bound); // repeat until <= bound

    if (val>bound) {
      std::stringstream ss;
      ss << "Error: sampleSmallBounded, after "
	   << count<<" trials, still val="<<val
	   << '>'<<"bound="<<bound;
      throw helib::RuntimeError(ss.str());
    }

    ZZ try_matrix_norm = calculate_matrix_norm(try_poly, context);

    if (trials == 0 || try_matrix_norm < best_matrix_norm) {
      best_matrix_norm = try_matrix_norm;
      best_poly = try_poly;
    }

    cerr << try_matrix_norm << "\n";
  }

  cerr << "*** ending trials\n";

  return bound;
#else
  return sampleHWt(poly, context, Hwt);
#endif

}

#else

void sampleHWtAlt(zzX& poly, const FHEcontext& context, long Hwt)
{
  const RecryptData& rcData = context.rcData;
  const PowerfulDCRT* p2dConv = rcData.p2dConv;
  const PAlgebra& palg = context.zMStar;
  long phim = palg.getPhiM();
  long m = palg.getM();

  Vec<ZZ> pwrfl;
  pwrfl.SetLength(phim);

  for (long i=0; i<Hwt; ) {  // continue until exactly Hwt nonzero coefficients
    long u = RandomBnd(phim);  // The next coefficient to choose
    if (pwrfl[u]==0) { // if we didn't choose it already
      long b = RandomBits_long(2)&2; // b random in {0,2}
      pwrfl[u] = b-1;                      //   random in {-1,1}
      i++; // count another nonzero coefficient
    }
  }

  ZZX poly1;
  p2dConv->powerfulToZZX(poly1, pwrfl);

  convert(poly, poly1);
}

// Experimental version

double sampleHWtBounded(zzX &poly, const FHEcontext& context, long Hwt)
{
#if FFT_IMPL // is there any implementation of canonicalEmbedding?
  double bound = sampleHWtBoundedEffectiveBound(context, Hwt);
  const PAlgebra& palg = context.zMStar;

  double val;
  long count = 0;
  do {
    sampleHWtAlt(poly, context, Hwt);
    val = embeddingLargestCoeff(poly,palg);
    //cerr << "****** " << (val/bound) << "\n";
  }
  while (++count<1000 && val>bound); // repeat until <= bound

  if (val>bound) {
    std::stringstream ss;
    ss << "Error: sampleSmallBounded, after "
         << count<<" trials, still val="<<val
         << '>'<<"bound="<<bound;
    throw helib::RuntimeError(ss.str());
  }
  return bound;
#else
  return sampleHWt(poly, context, Hwt);
#endif

}

#endif

double sampleSmall(zzX &poly, const FHEcontext& context)
{
  const PAlgebra& palg = context.zMStar;
  double retval;

  if (palg.getPow2() == 0) { // not power of two
    long m = palg.getM();
    long phim = palg.getPhiM();
    sampleSmall(poly, m, phim/(2.0*m)); // nonzero with prob phi(m)/2m
    // FIXME: does this probability make sense?  What is the goal there??
    reduceModPhimX(poly, palg);
    retval = context.noiseBoundForSmall(phim/(2.0*m), m);
  }
  else { // power of two
    long phim = palg.getPhiM();
    sampleSmall(poly, phim);
    retval = context.noiseBoundForSmall(0.5, phim);
  }

  return retval;
}

// Same as above, but ensure the result is not too much larger than typical
double sampleSmallBounded(zzX &poly, const FHEcontext& context)
{
#if FFT_IMPL // is there any implementation of canonicalEmbedding?
  const PAlgebra& palg = context.zMStar;
  long m = palg.getM();
  long phim = palg.getPhiM();
  // experimental bound, Pr[l-infty(canonical-embedding)>bound]<5%
  double bound = (1+sqrt(phim*log(phim)))*0.85;
  double val;
  long count = 0;
  do {
    sampleSmall(poly, context);
    val = embeddingLargestCoeff(poly,palg);
  }
  while (++count<1000 && val>bound); // repeat until <= bound
  if (val>bound) {
    std::stringstream ss;
    ss << "Error: sampleSmallBounded, after "
         << count<<" trials, still val="<<val
         << '>'<<"bound="<<bound;
    throw helib::RuntimeError(ss.str());
  }
  return bound;
#else
#warning "No FFT, sampleSmallBounded degenerates to sampleSmall"
  return sampleSmall(poly, context);
#endif
}

double sampleGaussian(zzX &poly, const FHEcontext& context, double stdev)
{
  const PAlgebra& palg = context.zMStar;
  double retval;

  if (palg.getPow2() == 0) { // not power of two
    long m = palg.getM();
    sampleGaussian(poly, m, stdev);
    reduceModPhimX(poly, palg);
    retval = context.noiseBoundForGaussian(stdev, m);
  }
  else { // power of two
    long phim = palg.getPhiM();
    sampleGaussian(poly, phim, stdev);
    retval = context.noiseBoundForGaussian(stdev, phim);
  }

  return retval;
}
// Same as above, but ensure the result is not too much larger than typical
double sampleGaussianBounded(zzX &poly, const FHEcontext& context, double stdev)
{
#if FFT_IMPL // is there any implementation of canonicalEmbedding?
  const PAlgebra& palg = context.zMStar;
  long m = palg.getM();
  long phim = palg.getPhiM();
  // experimental bound, Pr[l-infty(canonical-embedding)>bound]<5%
  double bound
    = stdev*1.15*(2+((palg.getPow2()==0)?
                     sqrt(m*log(phim)) : sqrt(phim*log(phim))));
  double val;
  long count = 0;
  do {
    sampleGaussian(poly, context, stdev);
    val = embeddingLargestCoeff(poly,palg);
  }
  while (++count<1000 && val>bound); // repeat until <=bound
  if (val>bound) {
    std::stringstream ss;
    ss << "Error: sampleGaussianBounded, after "
         << count<<" trials, still val="<<val
         << '>'<<"bound="<<bound;
    throw helib::RuntimeError(ss.str());
  }
  return bound;
#else
#warning "No FFT, sampleGaussianBounded degenerates to sampleGaussian"
  return sampleGaussian(poly, context, stdev);
#endif
}

double sampleUniform(zzX &poly, const FHEcontext& context, long B)
{
  const PAlgebra& palg = context.zMStar;
  double retval;

  if (palg.getPow2() == 0) { // not power of two
    long m = palg.getM();
    sampleUniform(poly, m, B);
    reduceModPhimX(poly, palg);
    retval = context.noiseBoundForUniform(B, m);
  }
  else { // power of two
    long phim = palg.getPhiM();
    sampleUniform(poly, phim, B);
    retval = context.noiseBoundForUniform(B, phim);
  }

  return retval;
}

xdouble sampleUniform(ZZX &poly, const FHEcontext& context, const ZZ& B)
{
  const PAlgebra& palg = context.zMStar;
  xdouble retval;

  if (palg.getPow2() == 0) { // not power of two
    long m = palg.getM();
    sampleUniform(poly, m, B);
    NTL::rem(poly, poly, palg.getPhimX());
    retval = context.noiseBoundForUniform(conv<xdouble>(B), m);
  }
  else {// power of two
    long phim = palg.getPhiM();
    sampleUniform(poly, phim, B);
    retval = context.noiseBoundForUniform(conv<xdouble>(B), phim);
  }

  return retval;
}

// Helper functions, return a bound B such that for random noise
// terms we have Pr[|canonicalEmbed(noise)|_{\infty} > B] < epsilon.
// (The default is epsilon = 2^{-40}.)
double boundFreshNoise(long m, long phim, double sigma, double epsilon)
{
  // The various constants in this function were determined experimentally.

  /* We begin by computing the standard deviation of the magnitude of
   * f(zeta), where f = sampleSmallBounded * sampleGaussian(sigma)
   *                 +  sampleSmall * sampleGaussianBounded(sigma)
   *                 + sampleGaussian(sigma)
   * and zeta is an m'th root-of-unity, which we approximate as:
   */
  double stdev = (sigma+0.1)*0.54*(1+(is2power(m)? sqrt(phim*m): phim));

  /* Then we use the following rules:
   *      Pr[|f(zeta)| > stdev] = 0.644
   *      Pr[|f(zeta)| > 2*stdev] = 0.266
   *      Pr[|f(zeta)| > 3*stdev] = 9.04e-2
   *      Pr[|f(zeta)| > 4*stdev] = 2.76e-2
   *      Pr[|f(zeta)| > 5*stdev] = 7.89e-3
   *      Pr[|f(zeta)| > 6*stdev] = 2.16e-3
   *      Pr[|f(zeta)| > n*stdev] = 2.16e-3 * 4^{6-n} for n>6
   *
   * We return the smallest number of standard deviations n satifying
   *      Pr[|f(zeta)|>(n stdev)] = epsilon / phi(m)
   */
  epsilon /= phim;

  if (epsilon >= 1.87e-3) { // use the values from above
    if (epsilon >= 0.64)        { return stdev; }
    else if (epsilon >= 0.26)   { return 2*stdev; }
    else if (epsilon >= 8.52e-2) { return 3*stdev; }
    else if (epsilon >= 2.54e-2) { return 4*stdev; }
    else if (epsilon >= 7.06e-3) { return 5*stdev; }
    else                         { return 6*stdev; }
  }
  long num = 7;
  for (double prob=(1.87e-3)/4; prob>epsilon; prob /= 4)
    num++;

  return stdev * num;
}
double boundRoundingNoise(long m, long phim, long p2r, double epsilon)
{
  // The various constants in this function were determined experimentally.

  /* We begin by computing the standard deviation of the magnitude of
   * f(zeta), where
   *          f= sampleSmallBounded*sampleUniform(p) +sampleUniform(p),
   * and zeta is an m'th root-of-unity, which we approximate as:
   */
  double stdev = (2*p2r+1)*(phim-2)/8.0;

  /* Then we use the following rules:
   *      Pr[|f(zeta)| > stdev] = 0.514
   *      Pr[|f(zeta)| > 2*stdev] = 0.194
   *      Pr[|f(zeta)| > 3*stdev] = 6.7e-2
   *      Pr[|f(zeta)| > 4*stdev] = 2.23e-2
   *      Pr[|f(zeta)| > 5*stdev] = 7.21e-3
   *      Pr[|f(zeta)| > 6*stdev] = 2.31e-3
   *      Pr[|f(zeta)| > 7*stdev] = 7.25e-4
   *      Pr[|f(zeta)| > n*stdev] = 7.25e-4 * 3.3^{7-n} for n>5
   *
   * We return the smallest number of standard deviations n satifying
   *      Pr[|f(zeta)|>(n stdev)] = epsilon / phi(m)
   */
  epsilon /= phim;

  if (epsilon >= 7.25e-4) { // use the values from above
    if (epsilon >= 0.514)        { return stdev; }
    else if (epsilon >= 0.194)   { return 2*stdev; }
    else if (epsilon >= 6.7e-2)  { return 3*stdev; }
    else if (epsilon >= 2.23e-2) { return 4*stdev; }
    else if (epsilon >= 7.21e-3) { return 5*stdev; }
    else if (epsilon >= 2.31e-3) { return 6*stdev; }
    else                         { return 7*stdev; }
  }
  long num = 8;
  for (double prob=(7.25e-4)/3.2; prob>epsilon; prob /= 3.2)
    num++;

  return stdev * num;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file   GaussianConditional.cpp
 * @brief  Conditional Gaussian Base class
 * @author Christian Potthast, Frank Dellaert
 */

#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/Sampler.h>

#include <boost/format.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif

#include <functional>
#include <list>
#include <string>

// In Wrappers we have no access to this so have a default ready
static std::mt19937_64 kRandomNumberGenerator(42);

using namespace std;

namespace gtsam {

  /* ************************************************************************* */
  GaussianConditional::GaussianConditional(
    Key key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas) :
  BaseFactor(key, R, d, sigmas), BaseConditional(1) {}

  /* ************************************************************************ */
  GaussianConditional::GaussianConditional(Key key, const Vector& d,
                                           const Matrix& R, Key parent1,
                                           const Matrix& S,
                                           const SharedDiagonal& sigmas)
      : BaseFactor(key, R, parent1, S, d, sigmas), BaseConditional(1) {}

  /* ************************************************************************ */
  GaussianConditional::GaussianConditional(Key key, const Vector& d,
                                           const Matrix& R, Key parent1,
                                           const Matrix& S, Key parent2,
                                           const Matrix& T,
                                           const SharedDiagonal& sigmas)
      : BaseFactor(key, R, parent1, S, parent2, T, d, sigmas),
        BaseConditional(1) {}

  /* ************************************************************************ */
  GaussianConditional GaussianConditional::FromMeanAndStddev(
      Key key, const Matrix& A, Key parent, const Vector& b, double sigma) {
    // |Rx + Sy - d| = |x-(Ay + b)|/sigma
    const Matrix R = Matrix::Identity(b.size(), b.size());
    const Matrix S = -A;
    const Vector d = b;
    return GaussianConditional(key, d, R, parent, S,
                               noiseModel::Isotropic::Sigma(b.size(), sigma));
  }

  /* ************************************************************************ */
  GaussianConditional GaussianConditional::FromMeanAndStddev(
      Key key, const Matrix& A1, Key parent1, const Matrix& A2, Key parent2,
      const Vector& b, double sigma) {
    // |Rx + Sy + Tz - d| = |x-(A1 y + A2 z + b)|/sigma
    const Matrix R = Matrix::Identity(b.size(), b.size());
    const Matrix S = -A1;
    const Matrix T = -A2;
    const Vector d = b;
    return GaussianConditional(key, d, R, parent1, S, parent2, T,
                               noiseModel::Isotropic::Sigma(b.size(), sigma));
  }

  /* ************************************************************************ */
  void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const {
    cout << s << " p(";
    for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
      cout << (boost::format("%1%") % (formatter(*it))).str()
           << (nrFrontals() > 1 ? " " : "");
    }

    if (nrParents()) {
      cout << " |";
      for (const_iterator it = beginParents(); it != endParents(); ++it) {
        cout << " " << (boost::format("%1%") % (formatter(*it))).str();
      }
    }
    cout << ")" << endl;

    cout << formatMatrixIndented("  R = ", R()) << endl;
    for (const_iterator it = beginParents() ; it != endParents() ; ++it) {
      cout << formatMatrixIndented((boost::format("  S[%1%] = ")%(formatter(*it))).str(), getA(it))
        << endl;
    }
    cout << formatMatrixIndented("  d = ", getb(), true) << "\n";
    if (model_)
      model_->print("  Noise model: ");
    else
      cout << "  No noise model" << endl;
  }

  /* ************************************************************************* */
  bool GaussianConditional::equals(const GaussianFactor& f, double tol) const {
    if (const GaussianConditional* c = dynamic_cast<const GaussianConditional*>(&f)) {
      // check if the size of the parents_ map is the same
      if (parents().size() != c->parents().size())
        return false;

      // check if R_ and d_ are linear independent
      for (DenseIndex i = 0; i < Ab_.rows(); i++) {
        list<Vector> rows1, rows2;
        rows1.push_back(Vector(R().row(i)));
        rows2.push_back(Vector(c->R().row(i)));

        // check if the matrices are the same
        // iterate over the parents_ map
        for (const_iterator it = beginParents(); it != endParents(); ++it) {
          const_iterator it2 = c->beginParents() + (it - beginParents());
          if (*it != *(it2))
            return false;
          rows1.push_back(row(getA(it), i));
          rows2.push_back(row(c->getA(it2), i));
        }

        Vector row1 = concatVectors(rows1);
        Vector row2 = concatVectors(rows2);
        if (!linear_dependent(row1, row2, tol))
          return false;
      }

      // check if sigmas are equal
      if ((model_ && !c->model_) || (!model_ && c->model_)
        || (model_ && c->model_ && !model_->equals(*c->model_, tol)))
        return false;

      return true;
    } else {
      return false;
    }
  }

  /* ************************************************************************* */
  VectorValues GaussianConditional::solve(const VectorValues& x) const {
    // Concatenate all vector values that correspond to parent variables
    const Vector xS = x.vector(KeyVector(beginParents(), endParents()));

    // Update right-hand-side
    const Vector rhs = d() - S() * xS;

    // Solve matrix
    const Vector solution = R().triangularView<Eigen::Upper>().solve(rhs);

    // Check for indeterminant solution
    if (solution.hasNaN()) {
      throw IndeterminantLinearSystemException(keys().front());
    }

    // Insert solution into a VectorValues
    VectorValues result;
    DenseIndex vectorPosition = 0;
    for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
      result.emplace(*frontal, solution.segment(vectorPosition, getDim(frontal)));
      vectorPosition += getDim(frontal);
    }

    return result;
  }

  /* ************************************************************************* */
  VectorValues GaussianConditional::solveOtherRHS(
    const VectorValues& parents, const VectorValues& rhs) const {
    // Concatenate all vector values that correspond to parent variables
    Vector xS = parents.vector(KeyVector(beginParents(), endParents()));

    // Instead of updating getb(), update the right-hand-side from the given rhs
    const Vector rhsR = rhs.vector(KeyVector(beginFrontals(), endFrontals()));
    xS = rhsR - S() * xS;

    // Solve Matrix
    Vector soln = R().triangularView<Eigen::Upper>().solve(xS);

    // Scale by sigmas
    if (model_)
      soln.array() *= model_->sigmas().array();

    // Insert solution into a VectorValues
    VectorValues result;
    DenseIndex vectorPosition = 0;
    for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
      result.emplace(*frontal, soln.segment(vectorPosition, getDim(frontal)));
      vectorPosition += getDim(frontal);
    }

    return result;
  }

  /* ************************************************************************* */
  void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
    Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals()));
    frontalVec = R().transpose().triangularView<Eigen::Lower>().solve(frontalVec);

    // Check for indeterminant solution
    if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());

    for (const_iterator it = beginParents(); it!= endParents(); it++)
      gy[*it].noalias() += -1.0 * getA(it).transpose() * frontalVec;

    // Scale by sigmas
    if (model_)
      frontalVec.array() *= model_->sigmas().array();

    // Write frontal solution into a VectorValues
    DenseIndex vectorPosition = 0;
    for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
      gy[*frontal] = frontalVec.segment(vectorPosition, getDim(frontal));
      vectorPosition += getDim(frontal);
    }
  }

  /* ************************************************************************ */
  JacobianFactor::shared_ptr GaussianConditional::likelihood(
      const VectorValues& frontalValues) const {
    // Error is |Rx - (d - Sy - Tz - ...)|^2
    // so when we instantiate x (which has to be completely known) we beget:
    // |Sy + Tz + ... - (d - Rx)|^2
    // The noise model just transfers over!

    // Get frontalValues as vector
    const Vector x =
        frontalValues.vector(KeyVector(beginFrontals(), endFrontals()));

    // Copy the augmented Jacobian matrix:
    auto newAb = Ab_;

    // Restrict view to parent blocks
    newAb.firstBlock() += nrFrontals_;

    // Update right-hand-side (last column)
    auto last = newAb.matrix().cols() - 1;
    const auto RR = R().triangularView<Eigen::Upper>();
    newAb.matrix().col(last) -= RR * x;

    // The keys now do not include the frontal keys:
    KeyVector newKeys;
    newKeys.reserve(nrParents());
    for (auto&& key : parents()) newKeys.push_back(key);

    // Hopefully second newAb copy below is optimized out...
    return boost::make_shared<JacobianFactor>(newKeys, newAb, model_);
  }

  /* **************************************************************************/
  JacobianFactor::shared_ptr GaussianConditional::likelihood(
      const Vector& frontal) const {
    if (nrFrontals() != 1)
      throw std::invalid_argument(
          "GaussianConditional Single value likelihood can only be invoked on "
          "single-variable conditional");
    VectorValues values;
    values.insert(keys_[0], frontal);
    return likelihood(values);
  }

  /* ************************************************************************ */
  VectorValues GaussianConditional::sample(const VectorValues& parentsValues,
                                           std::mt19937_64* rng) const {
    if (nrFrontals() != 1) {
      throw std::invalid_argument(
          "GaussianConditional::sample can only be called on single variable "
          "conditionals");
    }
    if (!model_) {
      throw std::invalid_argument(
          "GaussianConditional::sample can only be called if a diagonal noise "
          "model was specified at construction.");
    }
    VectorValues solution = solve(parentsValues);
    Key key = firstFrontalKey();
    const Vector& sigmas = model_->sigmas();
    solution[key] += Sampler::sampleDiagonal(sigmas, rng);
    return solution;
  }

  VectorValues GaussianConditional::sample(std::mt19937_64* rng) const {
    if (nrParents() != 0)
      throw std::invalid_argument(
          "sample() can only be invoked on no-parent prior");
    VectorValues values;
    return sample(values);
  }

  /* ************************************************************************ */
  VectorValues GaussianConditional::sample() const {
    return sample(&kRandomNumberGenerator);
  }

  VectorValues GaussianConditional::sample(const VectorValues& given) const {
    return sample(given, &kRandomNumberGenerator);
  }

  /* ************************************************************************ */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
  void GTSAM_DEPRECATED
  GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {
    DenseIndex vectorPosition = 0;
    for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
      gy[*frontal].array() *= model_->sigmas().segment(vectorPosition, getDim(frontal)).array();
      vectorPosition += getDim(frontal);
    }
  }
#endif

}  // namespace gtsam


/*    Copyright (c) 2010-2016, 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 <boost/make_shared.hpp>
#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>

#include "Tudat/Astrodynamics/Aerodynamics/flightConditions.h"
#include "Tudat/Astrodynamics/Ephemerides/frameManager.h"
#include "Tudat/Astrodynamics/Gravitation/sphericalHarmonicsGravityField.h"
#include "Tudat/Astrodynamics/ReferenceFrames/aerodynamicAngleCalculator.h"
#include "Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h"
#include "Tudat/SimulationSetup/accelerationSettings.h"
#include "Tudat/SimulationSetup/createAccelerationModels.h"
#include "Tudat/SimulationSetup/createFlightConditions.h"

namespace tudat
{

namespace simulation_setup
{

using namespace aerodynamics;
using namespace gravitation;
using namespace basic_astrodynamics;
using namespace electro_magnetism;
using namespace ephemerides;

//! Function to add to double-returning functions.
double evaluateDoubleFunctions(
        const boost::function< double( ) >& function1,
        const boost::function< double( ) >& function2 )
{
    return function1( ) + function2( );
}

//! Function to create central gravity acceleration model.
boost::shared_ptr< CentralGravitationalAccelerationModel3d > createCentralGravityAcceleratioModel(
        const boost::shared_ptr< Body > bodyUndergoingAcceleration,
        const boost::shared_ptr< Body > bodyExertingAcceleration,
        const std::string& nameOfBodyUndergoingAcceleration,
        const std::string& nameOfBodyExertingAcceleration,
        const bool useCentralBodyFixedFrame )
{
    // Declare pointer to return object.
    boost::shared_ptr< CentralGravitationalAccelerationModel3d > accelerationModelPointer;

    // Check if body is endowed with a gravity field model (i.e. is capable of exerting
    // gravitation acceleration).
    if( bodyExertingAcceleration->getGravityFieldModel( ) == NULL )
    {
        throw std::runtime_error(
                    std::string( "Error, gravity field model not set when making central ") +
                    " gravitational acceleration of " + nameOfBodyExertingAcceleration + " on " +
                    nameOfBodyUndergoingAcceleration );
    }
    else
    {
        boost::function< double( ) > gravitationalParameterFunction;

        // Set correct value for gravitational parameter.
        if( useCentralBodyFixedFrame == 0  ||
                bodyUndergoingAcceleration->getGravityFieldModel( ) == NULL )
        {
            gravitationalParameterFunction =
                    boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter,
                                 bodyExertingAcceleration->getGravityFieldModel( ) );
        }
        else
        {
            boost::function< double( ) > gravitationalParameterOfBodyExertingAcceleration =
                    boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter,
                                 bodyExertingAcceleration->getGravityFieldModel( ) );
            boost::function< double( ) > gravitationalParameterOfBodyUndergoingAcceleration =
                    boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter,
                                 bodyUndergoingAcceleration->getGravityFieldModel( ) );
            gravitationalParameterFunction =
                    boost::bind( &evaluateDoubleFunctions,
                                 gravitationalParameterOfBodyExertingAcceleration,
                                 gravitationalParameterOfBodyUndergoingAcceleration );
        }

        // Create acceleration object.
        accelerationModelPointer =
                boost::make_shared< CentralGravitationalAccelerationModel3d >(
                    boost::bind( &Body::getPosition, bodyUndergoingAcceleration ),
                    gravitationalParameterFunction,
                    boost::bind( &Body::getPosition, bodyExertingAcceleration ),
                    useCentralBodyFixedFrame );
    }

    return accelerationModelPointer;
}

//! Function to create spherical harmonic gravity acceleration model.
boost::shared_ptr< gravitation::SphericalHarmonicsGravitationalAccelerationModelXd >
createSphericalHarmonicsGravityAcceleration(
        const boost::shared_ptr< Body > bodyUndergoingAcceleration,
        const boost::shared_ptr< Body > bodyExertingAcceleration,
        const std::string& nameOfBodyUndergoingAcceleration,
        const std::string& nameOfBodyExertingAcceleration,
        const boost::shared_ptr< AccelerationSettings > accelerationSettings,
        const bool useCentralBodyFixedFrame )
{
    // Declare pointer to return object
    boost::shared_ptr< SphericalHarmonicsGravitationalAccelerationModelXd > accelerationModel;

    // Dynamic cast acceleration settings to required type and check consistency.
    boost::shared_ptr< SphericalHarmonicAccelerationSettings > sphericalHarmonicsSettings =
            boost::dynamic_pointer_cast< SphericalHarmonicAccelerationSettings >(
                accelerationSettings );
    if( sphericalHarmonicsSettings == NULL )
    {
        throw std::runtime_error(
                    std::string( "Error, acceleration settings inconsistent ") +
                    " making sh gravitational acceleration of " + nameOfBodyExertingAcceleration +
                    " on " + nameOfBodyUndergoingAcceleration );
    }
    else
    {
        // Get pointer to gravity field of central body and cast to required type.
        boost::shared_ptr< SphericalHarmonicsGravityField > sphericalHarmonicsGravityField =
                boost::dynamic_pointer_cast< SphericalHarmonicsGravityField >(
                    bodyExertingAcceleration->getGravityFieldModel( ) );

        boost::shared_ptr< RotationalEphemeris> rotationalEphemeris =
                bodyExertingAcceleration->getRotationalEphemeris( );
        if( sphericalHarmonicsGravityField == NULL )
        {
            throw std::runtime_error(
                        std::string( "Error, spherical harmonic gravity field model not set when ")
                        + " making sh gravitational acceleration of " +
                        nameOfBodyExertingAcceleration +
                        " on " + nameOfBodyUndergoingAcceleration );
        }
        else
        {
            if( rotationalEphemeris == NULL )
            {
                throw std::runtime_error( "Warning when making spherical harmonic acceleration on body " +
                                          nameOfBodyUndergoingAcceleration + ", no rotation model found for " +
                                          nameOfBodyExertingAcceleration );
            }

            if( rotationalEphemeris->getTargetFrameOrientation( ) !=
                    sphericalHarmonicsGravityField->getFixedReferenceFrame( ) )
            {
                throw std::runtime_error( "Warning when making spherical harmonic acceleration on body " +
                                          nameOfBodyUndergoingAcceleration + ", rotation model found for " +
                                          nameOfBodyExertingAcceleration + " is incompatible, frames are: " +
                                          rotationalEphemeris->getTargetFrameOrientation( ) + " and " +
                                          sphericalHarmonicsGravityField->getFixedReferenceFrame( ) );
            }

            boost::function< double( ) > gravitationalParameterFunction;

            // Check if mutual acceleration is to be used.
            if( useCentralBodyFixedFrame == false ||
                    bodyUndergoingAcceleration->getGravityFieldModel( ) == NULL )
            {
                gravitationalParameterFunction =
                        boost::bind( &SphericalHarmonicsGravityField::getGravitationalParameter,
                                     sphericalHarmonicsGravityField );
            }
            else
            {
                // Create function returning summed gravitational parameter of the two bodies.
                boost::function< double( ) > gravitationalParameterOfBodyExertingAcceleration =
                        boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter,
                                     sphericalHarmonicsGravityField );
                boost::function< double( ) > gravitationalParameterOfBodyUndergoingAcceleration =
                        boost::bind( &gravitation::GravityFieldModel::getGravitationalParameter,
                                     bodyUndergoingAcceleration->getGravityFieldModel( ) );
                gravitationalParameterFunction =
                        boost::bind( &evaluateDoubleFunctions,
                                     gravitationalParameterOfBodyExertingAcceleration,
                                     gravitationalParameterOfBodyUndergoingAcceleration );
            }

            // Create acceleration object.
            accelerationModel =
                    boost::make_shared< SphericalHarmonicsGravitationalAccelerationModelXd >
                    ( boost::bind( &Body::getPosition, bodyUndergoingAcceleration ),
                      gravitationalParameterFunction,
                      sphericalHarmonicsGravityField->getReferenceRadius( ),
                      boost::bind( &SphericalHarmonicsGravityField::getCosineCoefficients,
                                   sphericalHarmonicsGravityField,
                                   sphericalHarmonicsSettings->maximumDegree_,
                                   sphericalHarmonicsSettings->maximumOrder_ ),
                      boost::bind( &SphericalHarmonicsGravityField::getSineCoefficients,
                                   sphericalHarmonicsGravityField,
                                   sphericalHarmonicsSettings->maximumDegree_,
                                   sphericalHarmonicsSettings->maximumOrder_ ),
                      boost::bind( &Body::getPosition, bodyExertingAcceleration ),
                      boost::bind( &Body::getCurrentRotationToGlobalFrame,
                                   bodyExertingAcceleration ), useCentralBodyFixedFrame );
        }
    }
    return accelerationModel;
}

//! Function to create a third body central gravity acceleration model.
boost::shared_ptr< gravitation::ThirdBodyCentralGravityAcceleration >
createThirdBodyCentralGravityAccelerationModel(
        const boost::shared_ptr< Body > bodyUndergoingAcceleration,
        const boost::shared_ptr< Body > bodyExertingAcceleration,
        const boost::shared_ptr< Body > centralBody,
        const std::string& nameOfBodyUndergoingAcceleration,
        const std::string& nameOfBodyExertingAcceleration,
        const std::string& nameOfCentralBody )
{
    // Declare pointer to return object.
    boost::shared_ptr< ThirdBodyCentralGravityAcceleration > accelerationModelPointer;

    // Create acceleration object.
    accelerationModelPointer =  boost::make_shared< ThirdBodyCentralGravityAcceleration >(
                boost::dynamic_pointer_cast< CentralGravitationalAccelerationModel3d >(
                    createCentralGravityAcceleratioModel( bodyUndergoingAcceleration,
                                                          bodyExertingAcceleration,
                                                          nameOfBodyUndergoingAcceleration,
                                                          nameOfBodyExertingAcceleration, 0 ) ),
                boost::dynamic_pointer_cast< CentralGravitationalAccelerationModel3d >(
                    createCentralGravityAcceleratioModel( centralBody, bodyExertingAcceleration,
                                                          nameOfCentralBody,
                                                          nameOfBodyExertingAcceleration, 0 ) ), nameOfCentralBody );

    return accelerationModelPointer;
}

//! Function to create an aerodynamic acceleration model.
boost::shared_ptr< aerodynamics::AerodynamicAcceleration > createAerodynamicAcceleratioModel(
        const boost::shared_ptr< Body > bodyUndergoingAcceleration,
        const boost::shared_ptr< Body > bodyExertingAcceleration,
        const std::string& nameOfBodyUndergoingAcceleration,
        const std::string& nameOfBodyExertingAcceleration )
{
    // Check existence of required environment models
    if( bodyUndergoingAcceleration->getAerodynamicCoefficientInterface( ) == NULL )
    {
        throw std::runtime_error( "Error when making aerodynamic acceleration, body " +
                                  nameOfBodyUndergoingAcceleration +
                                  "has no aerodynamic coefficients." );
    }

    if( bodyExertingAcceleration->getAtmosphereModel( ) == NULL )
    {
        throw std::runtime_error(  "Error when making aerodynamic acceleration, central body " +
                                   nameOfBodyExertingAcceleration + " has no atmosphere model.");
    }

    if( bodyExertingAcceleration->getShapeModel( ) == NULL )
    {
        throw std::runtime_error( "Error when making aerodynamic acceleration, central body " +
                                  nameOfBodyExertingAcceleration + " has no shape model." );
    }

    // Retrieve flight conditions; create object if not yet extant.
    boost::shared_ptr< FlightConditions > bodyFlightConditions =
            bodyUndergoingAcceleration->getFlightConditions( );
    if( bodyFlightConditions == NULL )
    {
        bodyUndergoingAcceleration->setFlightConditions(
                    createFlightConditions( bodyUndergoingAcceleration,
                                            bodyExertingAcceleration,
                                            nameOfBodyUndergoingAcceleration,
                                            nameOfBodyExertingAcceleration ) );
        bodyFlightConditions = bodyUndergoingAcceleration->getFlightConditions( );
    }

    // Retrieve frame in which aerodynamic coefficients are defined.
    boost::shared_ptr< aerodynamics::AerodynamicCoefficientInterface > aerodynamicCoefficients =
            bodyUndergoingAcceleration->getAerodynamicCoefficientInterface( );
    reference_frames::AerodynamicsReferenceFrames accelerationFrame;
    if( aerodynamicCoefficients->getAreCoefficientsInAerodynamicFrame( ) )
    {
        accelerationFrame = reference_frames::aerodynamic_frame;
    }
    else
    {
        accelerationFrame = reference_frames::body_frame;
    }

    // Create function to transform from frame of aerodynamic coefficienrs to that of propagation.
    boost::function< Eigen::Vector3d( const Eigen::Vector3d& ) > toPropagationFrameTransformation;
    toPropagationFrameTransformation =
            reference_frames::getAerodynamicForceTransformationFunction(
                bodyFlightConditions->getAerodynamicAngleCalculator( ),
                accelerationFrame,
                boost::bind( &Body::getCurrentRotationToGlobalFrame, bodyExertingAcceleration ),
                reference_frames::inertial_frame );

    boost::function< Eigen::Vector3d( ) > coefficientFunction =
            boost::bind( &AerodynamicCoefficientInterface::getCurrentForceCoefficients,
                         aerodynamicCoefficients );
    boost::function< Eigen::Vector3d( ) > coefficientInPropagationFrameFunction =
            boost::bind( static_cast< Eigen::Vector3d(&)(
                             const boost::function< Eigen::Vector3d( ) >,
                             const boost::function< Eigen::Vector3d( const Eigen::Vector3d& ) > ) >(
                             &reference_frames::transformVector ),
                         coefficientFunction, toPropagationFrameTransformation );

    // Create acceleration model.
    return boost::make_shared< AerodynamicAcceleration >(
                coefficientInPropagationFrameFunction,
                boost::bind( &FlightConditions::getCurrentDensity, bodyFlightConditions ),
                boost::bind( &FlightConditions::getCurrentAirspeed, bodyFlightConditions ),
                boost::bind( &Body::getBodyMass, bodyUndergoingAcceleration ),
                boost::bind( &AerodynamicCoefficientInterface::getReferenceArea,
                             aerodynamicCoefficients ),
                aerodynamicCoefficients->getAreCoefficientsInNegativeAxisDirection( ) );
}

//! Function to create a cannonball radiation pressure acceleration model.
boost::shared_ptr< CannonBallRadiationPressureAcceleration >
createCannonballRadiationPressureAcceleratioModel(
        const boost::shared_ptr< Body > bodyUndergoingAcceleration,
        const boost::shared_ptr< Body > bodyExertingAcceleration,
        const std::string& nameOfBodyUndergoingAcceleration,
        const std::string& nameOfBodyExertingAcceleration )
{
    // Retrieve radiation pressure interface
    if( bodyUndergoingAcceleration->getRadiationPressureInterfaces( ).count(
                nameOfBodyExertingAcceleration ) == 0 )
    {
        throw std::runtime_error(
                    "Error when making radiation pressure, no radiation pressure interface found  in " +
                    nameOfBodyUndergoingAcceleration +
                    " for body " + nameOfBodyExertingAcceleration );
    }
    boost::shared_ptr< RadiationPressureInterface > radiationPressureInterface =
            bodyUndergoingAcceleration->getRadiationPressureInterfaces( ).at(
                nameOfBodyExertingAcceleration );

    // Create acceleration model.
    return boost::make_shared< CannonBallRadiationPressureAcceleration >(
                boost::bind( &Body::getPosition, bodyExertingAcceleration ),
                boost::bind( &Body::getPosition, bodyUndergoingAcceleration ),
                boost::bind( &RadiationPressureInterface::getCurrentRadiationPressure, radiationPressureInterface ),
                boost::bind( &RadiationPressureInterface::getRadiationPressureCoefficient, radiationPressureInterface ),
                boost::bind( &RadiationPressureInterface::getArea, radiationPressureInterface ),
                boost::bind( &Body::getBodyMass, bodyUndergoingAcceleration ) );

}

//! Function to create acceleration model object.
boost::shared_ptr< AccelerationModel< Eigen::Vector3d > > createAccelerationModel(
        const boost::shared_ptr< Body > bodyUndergoingAcceleration,
        const boost::shared_ptr< Body > bodyExertingAcceleration,
        const boost::shared_ptr< AccelerationSettings > accelerationSettings,
        const std::string& nameOfBodyUndergoingAcceleration,
        const std::string& nameOfBodyExertingAcceleration,
        const boost::shared_ptr< Body > centralBody,
        const std::string& nameOfCentralBody )
{
    // Declare pointer to return object.
    boost::shared_ptr< AccelerationModel< Eigen::Vector3d > > accelerationModelPointer;

    // Switch to call correct acceleration model type factory function.
    switch( accelerationSettings->accelerationType_ )
    {
    case central_gravity:
        // Check if body is a single-body central gravity acceleration (use third-body if not)
        if( nameOfCentralBody == nameOfBodyExertingAcceleration ||
                isFrameInertial( nameOfCentralBody ) )
        {
            // Check if gravitational parameter to use is sum of gravitational paramater of the
            // two bodies.
            bool useCentralBodyFixedFrame = 0;
            if( nameOfCentralBody == nameOfBodyExertingAcceleration )
            {
                useCentralBodyFixedFrame = 1;
            }

            accelerationModelPointer = createCentralGravityAcceleratioModel(
                        bodyUndergoingAcceleration,
                        bodyExertingAcceleration,
                        nameOfBodyUndergoingAcceleration,
                        nameOfBodyExertingAcceleration, useCentralBodyFixedFrame );
        }
        // Create third body central gravity acceleration
        else
        {

            accelerationModelPointer = createThirdBodyCentralGravityAccelerationModel(
                        bodyUndergoingAcceleration,
                        bodyExertingAcceleration,
                        centralBody,
                        nameOfBodyUndergoingAcceleration,
                        nameOfBodyExertingAcceleration,
                        nameOfCentralBody );
        }
        break;
    case spherical_harmonic_gravity:
        if( nameOfCentralBody == nameOfBodyExertingAcceleration ||
                isFrameInertial( nameOfCentralBody ) )
        {
            // Check if gravitational parameter to use is sum of gravitational paramater of the
            // two bodies.
            bool useCentralBodyFixedFrame = 0;
            if( nameOfCentralBody == nameOfBodyExertingAcceleration )
            {
                useCentralBodyFixedFrame = 1;
            }

            accelerationModelPointer = createSphericalHarmonicsGravityAcceleration(
                        bodyUndergoingAcceleration,
                        bodyExertingAcceleration,
                        nameOfBodyUndergoingAcceleration,
                        nameOfBodyExertingAcceleration,
                        accelerationSettings, useCentralBodyFixedFrame );

        }
        else
        {
            throw std::runtime_error(
                        "Error, cannot yet make third body spherical harmonic acceleration." );

        }
        break;
    case aerodynamic:
        accelerationModelPointer = createAerodynamicAcceleratioModel(
                    bodyUndergoingAcceleration,
                    bodyExertingAcceleration,
                    nameOfBodyUndergoingAcceleration,
                    nameOfBodyExertingAcceleration );
        break;
    case cannon_ball_radiation_pressure:
        accelerationModelPointer = createCannonballRadiationPressureAcceleratioModel(
                    bodyUndergoingAcceleration,
                    bodyExertingAcceleration,
                    nameOfBodyUndergoingAcceleration,
                    nameOfBodyExertingAcceleration );
        break;
    default:
        throw std::runtime_error(
                    std::string( "Error, acceleration model ") +
                    boost::lexical_cast< std::string >( accelerationSettings->accelerationType_ ) +
                    " not recognized when making acceleration model of" +
                    nameOfBodyExertingAcceleration + " on " +
                    nameOfBodyUndergoingAcceleration );
        break;
    }
    return accelerationModelPointer;
}

//! Function to create a set of acceleration models from a map of bodies and acceleration model
//! types.
AccelerationMap createAccelerationModelsMap(
        const NamedBodyMap& bodyMap,
        const SelectedAccelerationMap& selectedAccelerationPerBody,
        const std::map< std::string, std::string >& centralBodies )
{
    // Declare return map.
    AccelerationMap accelerationModelMap;

    // Iterate over all bodies which are undergoing acceleration
    for( SelectedAccelerationMap::const_iterator bodyIterator =
         selectedAccelerationPerBody.begin( ); bodyIterator != selectedAccelerationPerBody.end( );
         bodyIterator++ )
    {
        boost::shared_ptr< Body > currentCentralBody;

        // Retrieve name of body undergoing acceleration.
        std::string bodyUndergoingAcceleration = bodyIterator->first;

        // Retrieve name of current central body.
        std::string currentCentralBodyName = centralBodies.at( bodyUndergoingAcceleration );

        if( !isFrameInertial( currentCentralBodyName ) )
        {
            if( bodyMap.count( currentCentralBodyName ) == 0 )
            {
                throw std::runtime_error(
                            std::string( "Error, could not find non-inertial central body ") +
                            currentCentralBodyName + " of " + bodyUndergoingAcceleration +
                            " when making acceleration model." );
            }
            else
            {
                currentCentralBody = bodyMap.at( currentCentralBodyName );
            }
        }

        // Check if body undergoing acceleration is included in bodyMap
        if( bodyMap.count( bodyUndergoingAcceleration ) ==  0 )
        {
            throw std::runtime_error(
                        std::string( "Error when making acceleration models, requested forces" ) +
                        "acting on body " + bodyUndergoingAcceleration  +
                        ", but no such body found in map of bodies" );
        }

        // Declare map of acceleration models acting on current body.
        SingleBodyAccelerationMap mapOfAccelerationsForBody;

        // Retrieve list of required acceleration model types and bodies exerting accelerationd on
        // current body.
        std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > >
                accelerationsForBody = bodyIterator->second;

        // Iterate over all bodies exerting an acceleration
        for( std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > >::
             iterator body2Iterator = accelerationsForBody.begin( );
             body2Iterator != accelerationsForBody.end( ); body2Iterator++ )
        {
            // Retrieve name of body exerting acceleration.
            std::string bodyExertingAcceleration = body2Iterator->first;

            // Check if body exerting acceleration is included in bodyMap
            if( bodyMap.count( bodyExertingAcceleration ) ==  0 )
            {
                throw std::runtime_error(
                            std::string( "Error when making acceleration models, requested forces ")
                            + "acting on body " + bodyUndergoingAcceleration  + " due to body " +
                            bodyExertingAcceleration +
                            ", but no such body found in map of bodies" );
            }

            // Retrieve list of accelerations due to current body.
            std::vector< boost::shared_ptr< AccelerationSettings > > accelerationList =
                    body2Iterator->second;

            for( unsigned int i = 0; i < accelerationList.size( ); i++ )
            {
                // Create acceleration model.
                mapOfAccelerationsForBody[ bodyExertingAcceleration ].push_back(
                            createAccelerationModel( bodyMap.at( bodyUndergoingAcceleration ),
                                                     bodyMap.at( bodyExertingAcceleration ),
                                                     accelerationList.at( i ),
                                                     bodyUndergoingAcceleration,
                                                     bodyExertingAcceleration,
                                                     currentCentralBody,
                                                     currentCentralBodyName ) );
            }
        }

        // Put acceleration models on current body in return map.
        accelerationModelMap[ bodyUndergoingAcceleration ] = mapOfAccelerationsForBody;
    }

    return accelerationModelMap;
}

//! Function to create acceleration models from a map of bodies and acceleration model types.
basic_astrodynamics::AccelerationMap createAccelerationModelsMap(
        const NamedBodyMap& bodyMap,
        const SelectedAccelerationMap& selectedAccelerationPerBody,
        const std::vector< std::string >& propagatedBodies,
        const std::vector< std::string >& centralBodies )
{
    if( centralBodies.size( ) != propagatedBodies.size( ) )
    {
        throw std::runtime_error( "Error, number of propagated bodies must equal number of central bodies" );
    }

    std::map< std::string, std::string > centralBodyMap;
    for( unsigned int i = 0; i < propagatedBodies.size( ); i++ )
    {
        centralBodyMap[ propagatedBodies.at( i ) ] = centralBodies.at( i );
    }

    return createAccelerationModelsMap( bodyMap, selectedAccelerationPerBody, centralBodyMap );
}

} // namespace simulation_setup

} // namespace tudat


#include <frovedis.hpp>
#include <frovedis/ml/graph/graph.hpp>
#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;
using namespace std;

template <class T>
void call_pagerank(const std::string& data_p,
                   const std::string& out_p,
                   bool if_prep,
                   double df,
                   double epsilon,
                   size_t niter,
                   double thr) {
  graph<T> gr;
  time_spent t(INFO);
  if(if_prep) gr = read_edgelist<T>(data_p);
  else {
    auto mat = make_crs_matrix_load<T>(data_p);
    gr = graph<T>(mat);
  }
  t.show("data loading time: ");
  //gr.debug_print();
  auto res = gr.pagerank(df, epsilon, niter, thr);
  t.show("pagerank computation time: ");
  make_dvector_scatter(res).saveline(out_p);
  std::cout << "ranks: "; debug_print_vector(res, 5);
}

int main(int argc, char* argv[]){
    frovedis::use_frovedis use(argc, argv);
    using namespace boost::program_options;

    options_description opt("option");
    opt.add_options()
        ("help,h", "produce help message")
        ("input,i" , value<std::string>(), "input data path containing either edgelist or adjacency matrix data")
        ("dtype,t" , value<std::string>(), "input data type (int, float or double) [default: int]")
        ("output,o" , value<std::string>(), "output data path to save ranking scores")
        ("dfactor,d", value<double>(), "damping factor (default: 0.15)")
        ("epsilon,e", value<double>(), "convergence threshold (default: 1E-4)")
        ("threshold,l", value<double>(), "threshold forn shrink version (default: 0.4)")
        ("max_iter,k", value<size_t>(), "maximum no. of iterations (default: 100)")
        ("verbose", "set loglevel to DEBUG")
        ("verbose2", "set loglevel to TRACE")
        ("prepare,p" , "whether to generate the CRS matrix from original edgelist file ");

    variables_map argmap;
    store(command_line_parser(argc,argv).options(opt).allow_unregistered().
          run(), argmap);
    notify(argmap);

    bool if_prep = 0; // true if prepare data from raw dataset
    std::string data_p, out_p, dtype = "int";
    size_t niter = 100;
    double epsilon = 1e-4;
    double df = 0.15;
    double thr = 0.4;

    if(argmap.count("help")){
      std::cerr << opt << std::endl;
      exit(1);
    }
    if(argmap.count("input")){
      data_p = argmap["input"].as<std::string>();
    } else {
      std::cerr << "input path is not specified" << std::endl;
      std::cerr << opt << std::endl;
      exit(1);
    }
    if(argmap.count("output")){
      out_p = argmap["output"].as<std::string>();
    } else {
      std::cerr << "output path is not specified" << std::endl;
      std::cerr << opt << std::endl;
      exit(1);
    }
    if(argmap.count("prepare")){
      if_prep = true;
    }
    if(argmap.count("dtype")){
      dtype = argmap["dtype"].as<std::string>();
    }
    if(argmap.count("dfactor")){
       df = argmap["dfactor"].as<double>();
    }
    if(argmap.count("epsilon")){
       epsilon = argmap["epsilon"].as<double>();
    }
    if(argmap.count("threshold")){
       thr = argmap["threshold"].as<double>();
    }
    if(argmap.count("max_iter")){
       niter = argmap["max_iter"].as<size_t>();
    }
    if(argmap.count("verbose")){
      set_loglevel(DEBUG);
    }
    if(argmap.count("verbose2")){
      set_loglevel(TRACE);
    }

    try {
      if (dtype == "int") {
        call_pagerank<int>(data_p, out_p, if_prep, df, epsilon, niter, thr);
      }
      else if (dtype == "float") {
        call_pagerank<float>(data_p, out_p, if_prep, df, epsilon, niter, thr);
      }
      else if (dtype == "double") {
        call_pagerank<double>(data_p, out_p, if_prep, df, epsilon, niter, thr);
      }
      else {
        std::cerr << "Supported dtypes are only int, float and double!\n";
        std::cerr << opt << std::endl;
        exit(1);
      }
    }
    catch (std::exception& e) {
      std::cout << "exception caught: " << e.what() << std::endl;
    }
    return 0;
}


#include "ukf.h"
#include <Eigen/Dense>
#include <cmath>
using Eigen::MatrixXd;
using Eigen::VectorXd;

/**
 * Initializes Unscented Kalman filter
 */
UKF::UKF() {
  // if this is false, laser measurements will be ignored (except during init)
  use_laser_ = true;

  // if this is false, radar measurements will be ignored (except during init)
  use_radar_ = true;

  // initial state vector
  x_ = VectorXd(5);

  // initial covariance matrix
  P_ = MatrixXd(5, 5);

  // Process noise standard deviation longitudinal acceleration in m/s^2
  std_a_ = 2.0;

  // Process noise standard deviation yaw acceleration in rad/s^2
  std_yawdd_ = 1.0;

  /**
   * DO NOT MODIFY measurement noise values below.
   * These are provided by the sensor manufacturer.
   */

  // Laser measurement noise standard deviation position1 in m
  std_laspx_ = 0.15;

  // Laser measurement noise standard deviation position2 in m
  std_laspy_ = 0.15;

  // Radar measurement noise standard deviation radius in m
  std_radr_ = 0.3;

  // Radar measurement noise standard deviation angle in rad
  std_radphi_ = 0.03;

  // Radar measurement noise standard deviation radius change in m/s
  std_radrd_ = 0.3;

  /**
   * End DO NOT MODIFY section for measurement noise values
   */

  /**
   * TODO: Complete the initialization. See ukf.h for other member properties.
   * Hint: one or more values initialized above might be wildly off...
   */
  is_initialized_ = false;
  n_x_ = 5;
  n_aug_ = 7;
  lambda_ = 3 - n_x_;

  weights_ = VectorXd(2 * n_aug_ + 1);
  weights_.fill(1 / (2 * (lambda_ + n_aug_)));
  weights_(0) = lambda_ / (lambda_ + n_aug_);
}

UKF::~UKF() {}

void UKF::ProcessMeasurement(MeasurementPackage meas_package) {
  /**
   * TODO: Complete this function! Make sure you switch between lidar and radar
   * measurements.
   */
  if (!is_initialized_) {
    // set the state with the initial location and zero velocity
    // Must determine what sensor this is first
    if (meas_package.sensor_type_ == MeasurementPackage::SensorType::LASER) {
      x_ << meas_package.raw_measurements_[0],
          meas_package.raw_measurements_[1], 0, 0, 0;
      P_ << std_laspx_ * std_laspx_, 0, 0, 0, 0,
            0, std_laspy_ * std_laspy_, 0, 0, 0,
            0, 0, 1, 0, 0,
            0, 0, 0, 1, 0,
            0, 0, 0, 0, 1;
    } else if (meas_package.sensor_type_ ==
               MeasurementPackage::SensorType::RADAR) {
      const double rho = meas_package.raw_measurements_(0);
      const double phi = meas_package.raw_measurements_(1);
      const double rhodot = meas_package.raw_measurements_(2);
      const double x = rho * cos(phi);
      const double y = rho * sin(phi);
      const double vx = rhodot * cos(phi);
      const double vy = rhodot * sin(phi);
      const double v = rhodot; // std::sqrt(vx * vx + vy * vy);
      x_ << x, y, v, rho, rhodot;
      P_ << std_radr_* std_radr_, 0, 0, 0, 0,
            0, std_radr_ * std_radr_, 0, 0, 0,
            0, 0, std_radrd_ * std_radrd_, 0, 0,
            0, 0, 0, std_radphi_ * std_radphi_, 0,
            0, 0, 0, 0, std_radphi_ * std_radphi_;
    }

    time_us_ = meas_package.timestamp_;
    is_initialized_ = true;
    return;
  }

  // compute the time elapsed between the current and previous measurements
  // dt - expressed in seconds
  const float dt = (meas_package.timestamp_ - time_us_) / 1000000.0;
  time_us_ = meas_package.timestamp_;

  // Predict the next states and covariance matrix
  Prediction(dt);

  // Update the next states and covariance matrix
  if (meas_package.sensor_type_ == MeasurementPackage::SensorType::LASER) {
    UpdateLidar(meas_package);
  } else if (meas_package.sensor_type_ ==
             MeasurementPackage::SensorType::RADAR) {
    UpdateRadar(meas_package);
  }
}

void UKF::Prediction(double delta_t) {
  /**
   * TODO: Complete this function! Estimate the object's location.
   * Modify the state vector, x_. Predict sigma points, the state,
   * and the state covariance matrix.
   */

  // Step #1 - Create augmented mean vector, augmented state covariance
  VectorXd x_aug = VectorXd(n_aug_);
  x_aug.setZero(n_aug_);
  x_aug.head(5) = x_;

  MatrixXd P_aug = MatrixXd(n_aug_, n_aug_);
  P_aug.setZero(n_aug_, n_aug_);
  P_aug.topLeftCorner(5, 5) = P_;
  P_aug.bottomRightCorner(2, 2) << std_a_ * std_a_, 0, 0,
      std_yawdd_ * std_yawdd_;

  // Step #2 - Create square root matrix
  MatrixXd A = P_aug.llt().matrixL();

  // Step #3 - Create augmented sigma points
  MatrixXd Xsig_aug = MatrixXd(n_aug_, 2 * n_aug_ + 1);
  Xsig_aug.col(0) = x_aug;
  Xsig_aug.middleCols(1, n_aug_) =
      (std::sqrt(lambda_ + n_aug_) * A.array()).colwise() + x_aug.array();
  Xsig_aug.middleCols(n_aug_ + 1, n_aug_) =
      (-std::sqrt(lambda_ + n_aug_) * A.array()).colwise() + x_aug.array();

  // Step #4 - Predict sigma points
  Xsig_pred_ = MatrixXd(n_x_, 2 * n_aug_ + 1);
  for (int i = 0; i < Xsig_aug.cols(); i++) {
    const float px = Xsig_aug(0, i);
    const float py = Xsig_aug(1, i);
    const float v = Xsig_aug(2, i);
    const float yaw = Xsig_aug(3, i);
    const float yaw_rate = Xsig_aug(4, i);
    const float nu_accel = Xsig_aug(5, i);
    const float nu_yaw_accel = Xsig_aug(6, i);
    VectorXd vec(n_x_);
    VectorXd noise_vec(n_x_);
    noise_vec << 0.5 * delta_t * delta_t * std::cos(yaw) * nu_accel,
        0.5 * delta_t * delta_t * std::sin(yaw) * nu_accel, delta_t * nu_accel,
        0.5 * delta_t * delta_t * nu_yaw_accel, delta_t * nu_yaw_accel;
    // avoid division by zero
    if (std::abs(yaw_rate) <= 1e-10) {
      vec << v * std::cos(yaw) * delta_t, v * std::sin(yaw) * delta_t, 0, 0, 0;
    } else {
      vec << (v / yaw_rate) *
                 (std::sin(yaw + yaw_rate * delta_t) - std::sin(yaw)),
          (v / yaw_rate) *
              (-std::cos(yaw + yaw_rate * delta_t) + std::cos(yaw)),
          0, yaw_rate * delta_t, 0;
    }
    // write predicted sigma points into right column
    Xsig_pred_.col(i) = Xsig_aug.col(i).head(n_x_) + vec + noise_vec;
  }

  // Step #5 - Now predict state mean and covariance
  x_ = Xsig_pred_ * weights_;

  P_ = (Xsig_pred_.array().colwise() - x_.array());
  P_ = P_ * weights_.asDiagonal() * P_.transpose();
}

void UKF::UpdateLidar(MeasurementPackage meas_package) {
  /**
   * TODO: Complete this function! Use lidar data to update the belief
   * about the object's position. Modify the state vector, x_, and
   * covariance, P_.
   * You can also calculate the lidar NIS, if desired.
   */
  // Note that the LiDAR noise profile is linear, so we can simply
  // use the standard linear Kalman filter here
  // Taken directly from the previous assignments

  // New - define measurement matrix
  MatrixXd H;
  H.setZero(2, n_x_);
  H(0, 0) = H(1, 1) = 1;  // Select out the position elements only
  VectorXd z_pred = H * x_;
  const VectorXd z = meas_package.raw_measurements_;

  // Calculate residual vector y
  const VectorXd y = z - z_pred;

  // New - define measurement noise matrix
  R_.setZero(2, 2);
  R_ << std_laspx_ * std_laspx_, 0, 0, std_laspy_ * std_laspy_;

  // Create innovation covariance matrix S
  MatrixXd S = H * P_ * H.transpose() + R_;

  // Create Kalman gain matrix K
  MatrixXd K = P_ * H.transpose() * S.inverse();

  // Create new estimate for states and covariance
  x_ = x_ + (K * y);
  MatrixXd I = MatrixXd::Identity(x_.size(), x_.size());
  P_ = (I - K * H) * P_;

  // Calculate NIS for LiDAR
  NIS_lidar_ = y.transpose() * S.inverse() * y;
}

void UKF::UpdateRadar(MeasurementPackage meas_package) {
  /**
   * TODO: Complete this function! Use radar data to update the belief
   * about the object's position. Modify the state vector, x_, and
   * covariance, P_.
   * You can also calculate the radar NIS, if desired.
   */
  // Step #1 - Create matrix for sigma points in measurement space
  MatrixXd Zsig = MatrixXd(3, 2 * n_aug_ + 1);

  // Step #2 - Transform sigma points into measurement space
  Zsig.row(0) = ((Xsig_pred_.row(0).array() * Xsig_pred_.row(0).array()) +
                 (Xsig_pred_.row(1).array() * Xsig_pred_.row(1).array()))
                    .sqrt();
  for (int i = 0; i < Zsig.cols(); i++) {
    Zsig(1, i) = std::atan2(Xsig_pred_(1, i), Xsig_pred_(0, i));
  }
  Zsig.row(2) = ((Xsig_pred_.row(0).array() * Xsig_pred_.row(2).array() *
                  Xsig_pred_.row(3).array().cos()) +
                 (Xsig_pred_.row(1).array() * Xsig_pred_.row(2).array() *
                  Xsig_pred_.row(3).array().sin())) /
                Zsig.row(0).array();

  // Step #3 - Create and run final update method
  // mean predicted measurement
  const int n_z = 3;
  VectorXd z_pred = VectorXd(n_z);

  // measurement covariance matrix S
  MatrixXd S = MatrixXd(n_z, n_z);

  // Step #4 - Calculate mean predicted measurement
  z_pred = Zsig * weights_;

  // Step #5 - Create measurement covariance matrix R and
  // calculate innovation covariance matrix S
  R_.setZero(n_z, n_z);
  R_ << std_radr_ * std_radr_, 0, 0, 0, std_radphi_ * std_radphi_, 0, 0, 0,
      std_radrd_ * std_radrd_;

  S = (Zsig.array().colwise() - z_pred.array());
  S = S * weights_.asDiagonal() * S.transpose() + R_;

  // Step #6 - Calculate Cross Correlation Matrix
  MatrixXd Tc = MatrixXd(n_x_, n_z);
  Tc = (Xsig_pred_.array().colwise() - x_.array()).matrix() *
       weights_.asDiagonal() *
       (Zsig.array().colwise() - z_pred.array()).matrix().transpose();

  // Step #7 - Calculate Kalman gain K
  const MatrixXd K = Tc * S.inverse();

  // Step #8 - Update state mean and covariance matrix
  const VectorXd z = meas_package.raw_measurements_;
  const VectorXd y = z - z_pred;
  x_ = x_ + (K * y);
  P_ = P_ - K * S * K.transpose();

  // Calculate NIS for Radar
  NIS_radar_ = y.transpose() * S.inverse() * y;
}


/**
 * @file
 * @author Denise Ratasich
 * @date 05.09.2013
 *
 * @brief Configuration for ROS node.
 */

#include <Eigen/Core>
#include <string>
#include <stdexcept>
#include "estimation/models.h"
#include "configuration/configuration.h"

using namespace estimation;
using namespace Eigen;

// private functions
std::string getMethod();

// models
#if METHOD == EXTENDED_KALMAN_FILTER  ||	\
  METHOD == UNSCENTED_KALMAN_FILTER  ||		\
  METHOD == PARTICLE_FILTER_SIR
void f(VectorXd& x, const VectorXd& u);
void h(VectorXd& z, const VectorXd& x);
#endif
#if METHOD == EXTENDED_KALMAN_FILTER
void df(MatrixXd& A, const VectorXd& x, const VectorXd& u);
void dh(MatrixXd& H, const VectorXd& x);
#endif

// public functions for the ROS node
int getEstimatePeriod(void)
{
  return ESTIMATION_PERIOD;
}

void initEstimatorFactory(EstimatorFactory& factory)
{
  factory.addParam("method", getMethod());

#ifdef WINDOW_SIZE
  factory.addParam("window-size", WINDOW_SIZE);
#endif
#ifdef WEIGHTING_COEFFICIENTS
  VectorXd wc(VECTOR_SIZE(WEIGHTING_COEFFICIENTS));
  CODE_ASSIGN_VALUES_TO_VECTOR(wc, WEIGHTING_COEFFICIENTS);
  factory.addParam("weighting-coefficients", wc);
#endif

#ifdef STATE_TRANSITION_MODEL
  #if METHOD == KALMAN_FILTER
  MatrixXd stm(MATRIX_ROWS(STATE_TRANSITION_MODEL),
	       MATRIX_COLS(STATE_TRANSITION_MODEL));
  CODE_ASSIGN_VALUES_TO_MATRIX(stm, STATE_TRANSITION_MODEL);
  #elif METHOD == EXTENDED_KALMAN_FILTER  ||	\
    METHOD == UNSCENTED_KALMAN_FILTER  ||	\
    METHOD == PARTICLE_FILTER_SIR
  func_f stm = f;
  #endif
  factory.addParam("state-transition-model", stm);
#endif
#ifdef PROCESS_NOISE_COVARIANCE
  MatrixXd pnc(MATRIX_ROWS(PROCESS_NOISE_COVARIANCE),
	       MATRIX_COLS(PROCESS_NOISE_COVARIANCE));
  CODE_ASSIGN_VALUES_TO_MATRIX(pnc, PROCESS_NOISE_COVARIANCE);
  factory.addParam("process-noise-covariance", pnc);
#endif
#ifdef OBSERVATION_MODEL
  #if METHOD == KALMAN_FILTER
  MatrixXd om(MATRIX_ROWS(OBSERVATION_MODEL),
	       MATRIX_COLS(OBSERVATION_MODEL));
  CODE_ASSIGN_VALUES_TO_MATRIX(om, OBSERVATION_MODEL);
  #elif METHOD == EXTENDED_KALMAN_FILTER  ||	\
    METHOD == UNSCENTED_KALMAN_FILTER  ||	\
    METHOD == PARTICLE_FILTER_SIR
  func_h om = h;
  #endif
  factory.addParam("observation-model", om);
#endif
#ifdef MEASUREMENT_NOISE_COVARIANCE
  MatrixXd mnc(MATRIX_ROWS(MEASUREMENT_NOISE_COVARIANCE),
	       MATRIX_COLS(MEASUREMENT_NOISE_COVARIANCE));
  CODE_ASSIGN_VALUES_TO_MATRIX(mnc, MEASUREMENT_NOISE_COVARIANCE);
  factory.addParam("measurement-noise-covariance", mnc);
#endif
#ifdef CONTROL_INPUT_MODEL
  MatrixXd cim(MATRIX_ROWS(CONTROL_INPUT_MODEL),
	       MATRIX_COLS(CONTROL_INPUT_MODEL));
  CODE_ASSIGN_VALUES_TO_MATRIX(cim, CONTROL_INPUT_MODEL);
  factory.addParam("control-input-model", cim);
#endif
#ifdef INITIAL_STATE
  VectorXd is(VECTOR_SIZE(INITIAL_STATE));
  CODE_ASSIGN_VALUES_TO_VECTOR(is, INITIAL_STATE);
  factory.addParam("initial-state", is);
#endif
#ifdef INITIAL_ERROR_COVARIANCE
  MatrixXd iec(MATRIX_ROWS(INITIAL_ERROR_COVARIANCE),
	       MATRIX_COLS(INITIAL_ERROR_COVARIANCE));
  CODE_ASSIGN_VALUES_TO_MATRIX(iec, INITIAL_ERROR_COVARIANCE);
  factory.addParam("initial-error-covariance", iec);
#endif

#ifdef STATE_SIZE
  factory.addParam("state-size", STATE_SIZE);
#endif
#ifdef MEASUREMENT_SIZE
  factory.addParam("measurement-size", MEASUREMENT_SIZE);
#endif
#ifdef CONTROL_SIZE
  factory.addParam("control-input-size", CONTROL_SIZE);
#endif

#ifdef STATE_TRANSITION_MODEL_JACOBIAN
  func_df stmj = df;
  factory.addParam("state-transition-model-jacobian", stmj);
#endif
#ifdef OBSERVATION_MODEL_JACOBIAN
  func_dh omj = dh;
  factory.addParam("observation-model-jacobian", omj);
#endif

// SIR
#ifdef STATE_BOUNDS
  MatrixXd sb(MATRIX_ROWS(STATE_BOUNDS),
	      MATRIX_COLS(STATE_BOUNDS));
  CODE_ASSIGN_VALUES_TO_MATRIX(sb, STATE_BOUNDS);
  factory.addParam("state-bounds", sb);
#endif
#ifdef NUMBER_OF_PARTICLES
  factory.addParam("number-of-particles", NUMBER_OF_PARTICLES);
#endif

// Confidence-Weighted Averaging
#ifdef IGNORE_ZERO_VARIANCE_VALUES
  factory.addParam("ignore-zero-variance-values", 0);
#endif
}

// implementation of private functions
std::string getMethod()
{
#if METHOD == MOVING_MEDIAN
  return "MovingMedian";
#elif METHOD == MOVING_AVERAGE
  return "MovingAverage";
#elif METHOD == KALMAN_FILTER
  return "KalmanFilter";
#elif METHOD == EXTENDED_KALMAN_FILTER
  return "ExtendedKalmanFilter";
#elif METHOD == UNSCENTED_KALMAN_FILTER
  return "UnscentedKalmanFilter";
#elif METHOD == PARTICLE_FILTER_SIR
  return "ParticleFilterSIR";
#elif METHOD == CONFIDENCE_WEIGHTED_AVERAGING
  return "ConfidenceWeightedAveraging";
#endif
}

// implementation of models
#if METHOD == EXTENDED_KALMAN_FILTER  ||	\
  METHOD == UNSCENTED_KALMAN_FILTER  ||		\
  METHOD == PARTICLE_FILTER_SIR
void f(VectorXd& x, const VectorXd& u)
{
  if (x.size() != STATE_SIZE)
    throw std::runtime_error("Applying state transition model failed, state vector has invalid size.");

  // create state vector for result, i.e. the a priori state estimate;
  // copying must be done, because x occurs on the right-hand side,
  // e.g.: x[0] = x[1]; x[1] = x[0]; is different to x_apriori[0] =
  // x[1]; x_apriori[1] = x[0];!!
  VectorXd x_apriori(x.size());

  try
  {
    // assign formulas of the state transition model (from the
    // configuration header) to vector x, i.e. calculate a priori state
    // estimate
    CODE_ASSIGN_FORMULAS_TO_VECTOR(x_apriori, STATE_TRANSITION_MODEL);
  }
  catch (std::exception& e)
  {
    std::string additionalInfo = "Applying state transition model failed. ";
    throw std::runtime_error(additionalInfo + e.what());
  }

  // copy back, x will then represent the a priori state estimate
  x = x_apriori;
}

void h(VectorXd& z, const VectorXd& x)
{
  if (z.size() != MEASUREMENT_SIZE)
    throw std::runtime_error("Applying observation model failed, measurement vector has invalid size.");

  try
  {
    // z doesn't occur on the right-hand side, so no copying necessary
    CODE_ASSIGN_FORMULAS_TO_VECTOR(z, OBSERVATION_MODEL);
  }
  catch (std::exception& e)
  {
    std::string additionalInfo = "Applying observation model failed. ";
    throw std::runtime_error(additionalInfo + e.what());
  }
}
#endif

#if METHOD == EXTENDED_KALMAN_FILTER
void df(MatrixXd& A, const VectorXd& x, const VectorXd& u)
{
  try
  {
    CODE_ASSIGN_FORMULAS_TO_MATRIX(A, STATE_TRANSITION_MODEL_JACOBIAN);
  }
  catch (std::exception& e)
  {
    std::string additionalInfo = "Applying Jacobian of state transition model failed. ";
    throw std::runtime_error(additionalInfo + e.what());
  }
}

void dh(MatrixXd& H, const VectorXd& x)
{
  try
  {
    CODE_ASSIGN_FORMULAS_TO_MATRIX(H, OBSERVATION_MODEL_JACOBIAN);
  }
  catch (std::exception& e)
  {
    std::string additionalInfo = "Applying Jacobian of observation model failed. ";
    throw std::runtime_error(additionalInfo + e.what());
  }
}
#endif


/*
 * Copyright 2011-2013 Mario Mulansky
 * Copyright 2012 Karsten Ahnert
 *
 * 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)
 *
 * Example for the lorenz system with a 3D point type
*/

#include <iostream>
#include <cmath>

#include <boost/operators.hpp>

#include <boost/numeric/odeint.hpp>

//[point3D
class point3D :
    boost::additive1< point3D ,
    boost::additive2< point3D , double ,
    boost::multiplicative2< point3D , double > > >
{
public:

    double x , y , z;

    point3D()
        : x( 0.0 ) , y( 0.0 ) , z( 0.0 )
    { }

    point3D( const double val )
        : x( val ) , y( val ) , z( val )
    { }

    point3D( const double _x , const double _y , const double _z  )
        : x( _x ) , y( _y ) , z( _z )
    { }

    point3D& operator+=( const point3D &p )
    {
        x += p.x; y += p.y; z += p.z;
        return *this;
    }

    point3D& operator*=( const double a )
    {
        x *= a; y *= a; z *= a;
        return *this;
    }

};
//]

//[point3D_abs_div
// only required for steppers with error control
point3D operator/( const point3D &p1 , const point3D &p2 )
{
    return point3D( p1.x/p2.x , p1.y/p2.y , p1.z/p1.z );
}

point3D abs( const point3D &p )
{
    return point3D( std::abs(p.x) , std::abs(p.y) , std::abs(p.z) );
}
//]

//[point3D_norm
// also only for steppers with error control
namespace boost { namespace numeric { namespace odeint {
template<>
struct vector_space_norm_inf< point3D >
{
    typedef double result_type;
    double operator()( const point3D &p ) const
    {
        using std::max;
        using std::abs;
        return max( max( abs( p.x ) , abs( p.y ) ) , abs( p.z ) );
    }
};
} } }
//]

std::ostream& operator<<( std::ostream &out , const point3D &p )
{
    out << p.x << " " << p.y << " " << p.z;
    return out;
}

//[point3D_main
const double sigma = 10.0;
const double R = 28.0;
const double b = 8.0 / 3.0;

void lorenz( const point3D &x , point3D &dxdt , const double t )
{
    dxdt.x = sigma * ( x.y - x.x );
    dxdt.y = R * x.x - x.y - x.x * x.z;
    dxdt.z = -b * x.z + x.x * x.y;
}

using namespace boost::numeric::odeint;

int main()
{

    point3D x( 10.0 , 5.0 , 5.0 );
    // point type defines it's own operators -> use vector_space_algebra !
    typedef runge_kutta_dopri5< point3D , double , point3D ,
                                double , vector_space_algebra > stepper;
    int steps = integrate_adaptive( make_controlled<stepper>( 1E-10 , 1E-10 ) , lorenz , x ,
                                    0.0 , 10.0 , 0.1 );
    std::cout << x << std::endl;
    std::cout << "steps: " << steps << std::endl;
}
//]


// Copyright 2019 Erik Teichmann <kontakt.teichmann@gmail.com>

#ifndef INCLUDE_SAM_SYSTEM_EULER_SYSTEM_HPP_
#define INCLUDE_SAM_SYSTEM_EULER_SYSTEM_HPP_

#include <vector>

#include <boost/numeric/odeint/integrate/null_observer.hpp>

#include "./generic_system.hpp"

namespace sam {

/*! \brief A system that is integrated with an Euler method of order O(dt).
 *
 * A system that is integrated with an Euler method of order O(dt). The Euler
 * method is defined as
 * \f[ x_{n+1} = x_{n} + f_n dt, \f]
 * where \f$ f_n \f$ is the derivative at timestep \f$ n \f$.
 */
template<typename ODE, typename state_type = std::vector<double>>
class EulerSystem: public GenericSystem<ODE, state_type> {
 public:
  template<typename... Ts>
  explicit EulerSystem(unsigned int system_size, unsigned int dimension,
                       Ts... parameters);

  template<typename observer_type = boost::numeric::odeint::null_observer>
  void Integrate(double dt, unsigned int number_steps,
                 observer_type observer
                     = boost::numeric::odeint::null_observer());

 private:
  template<typename system_type, typename observer_type>
  double EulerMethod(system_type system, state_type& x, double t, double dt,
                     unsigned int number_steps, observer_type observer);
};

// Implementation

template<typename ODE, typename state_type>
template<typename... Ts>
EulerSystem<ODE, state_type>::EulerSystem(unsigned int system_size,
                                          unsigned int dimension,
                                          Ts... parameters)
      : GenericSystem<ODE, state_type>(system_size, dimension, parameters...) {}

template<typename ODE, typename state_type>
template<typename observer_type>
void EulerSystem<ODE, state_type>::Integrate(double dt,
                                             unsigned int number_steps,
                                             observer_type observer) {
    this->t_ = EulerMethod(*(this->ode_), this->x_, this->t_, dt, number_steps,
                           observer);
}

template<typename ODE, typename state_type>
template<typename system_type, typename observer_type>
double EulerSystem<ODE, state_type>::EulerMethod(system_type system,
                                                 state_type& x, double t,
                                                 double dt,
                                                 unsigned int number_steps,
                                                 observer_type observer) {
  observer(x, t);
  for (unsigned int i = 0; i < number_steps; ++i) {
    // TODO(boundter): copy the value? how to best initialize?
    state_type dx = x;
    system(x, dx, t);
    // TODO(boundter): Rather use iterators
    for (size_t j = 0; j < x.size(); ++j) x[j] += dx[j]*dt;
    t += dt;
    observer(x, t);
  }
  return t;
}

}  // namespace sam

#endif  // INCLUDE_SAM_SYSTEM_EULER_SYSTEM_HPP_


#include "Mbr.h"
#include <iostream>
#include <fstream>
#include <boost/algorithm/string.hpp>
#include <vector>
#include <iterator>
#include <string>
#include <limits>
#include <algorithm>
#include <math.h>
using namespace std;
Mbr::Mbr()
{
}

Mbr::Mbr(float x1, float y1, float x2, float y2)
{
    this->x1 = x1;
    this->y1 = y1;
    this->x2 = x2;
    this->y2 = y2;
}

void Mbr::update(Point point)
{
    update(point.x, point.y);
}

void Mbr::update(float x, float y)
{
    if (x < x1)
    {
        x1 = x;
    }
    if (y < y1)
    {
        y1 = y;
    }
    if (x > x2)
    {
        x2 = x;
    }
    if (y > y2)
    {
        y2 = y;
    }
}

void Mbr::update(Mbr mbr)
{
    if (mbr.x1 < x1)
    {
        x1 = mbr.x1;
    }
    if (mbr.y1 < y1)
    {
        y1 = mbr.y1;
    }
    if (mbr.x2 > x2)
    {
        x2 = mbr.x2;
    }
    if (mbr.y2 > y2)
    {
        y2 = mbr.y2;
    }
}

bool Mbr::contains(Point point)
{
    if (x1 > point.x || point.x > x2 || y1 > point.y || point.y > y2)
    {
        return false;
    }
    else
    {
        return true;
    }
}

bool Mbr::strict_contains(Point point)
{
    if (x1 < point.x && point.x < x2 && y1 < point.y && point.y < y2)
    {
        return true;
    }
    else
    {
        return false;
    }
}

bool Mbr::interact(Mbr mbr)
{
    if ((x1 <= mbr.x1 && mbr.x1 <= x2 && y1 <= mbr.y1 && mbr.y1 <= y2) || (x1 <= mbr.x1 && mbr.x1 <= x2 && y1 <= mbr.y2 && mbr.y2 <= y2) || (x1 <= mbr.x2 && mbr.x2 <= x2 && y1 <= mbr.y1 && mbr.y1 <= y2) || (x1 <= mbr.x2 && mbr.x2 <= x2 && y1 <= mbr.y2 && mbr.y2 <= y2))
    {
        return true;
    }
    if ((mbr.x1 <= x1 && x1 <= mbr.x2 && mbr.y1 <= y1 && y1 <= mbr.y2) || (mbr.x1 <= x1 && x1 <= mbr.x2 && mbr.y1 <= y2 && y2 <= mbr.y2) || (mbr.x1 <= x2 && x2 <= mbr.x2 && mbr.y1 <= y1 && y1 <= mbr.y2) || (mbr.x1 <= x2 && x2 <= mbr.x2 && mbr.y1 <= y2 && y2 <= mbr.y2))
    {
        return true;
    }
    return false;
}

vector<Mbr> Mbr::get_mbrs(vector<Point> dataset, float area, int num, float ratio)
{

    vector<Mbr> mbrs;
    srand(time(0));
    int maxInt = numeric_limits<int>::max();
    float x = sqrt(area * ratio);
    float y = sqrt(area / ratio);
    int i = 0;
    int length = dataset.size();
    while (i < num)
    {
        int index = rand() % length;
        Point point = dataset[index];
        if (point.x + x <= 1 && point.y + y <= 1)
        {
            Mbr mbr(point.x, point.y, point.x + x, point.y + y);
            mbrs.push_back(mbr);
            i++;
        }
    }

    return mbrs;
}

float Mbr::cal_dist(Point point)
{
    if (this->contains(point))
    {
        return 0;
    }
    else
    {
        float dist;
        if (point.x < x1)
        {
            if (point.y < y1)
            {
                dist = sqrt(pow((point.x - x1), 2) + pow((point.y - y1), 2));
            }
            else if (point.y <= y2)
            {
                dist = x1 - point.x;
            }
            else
            {
                dist = sqrt(pow((point.x - x1), 2) + pow((point.y - y2), 2));
            }
        }
        else if (point.x <= x2)
        {
            if (point.y < y1)
            {
                dist = y1 - point.y;
            }
            else
            {
                dist = point.y - y2;
            }
        }
        else
        {
            if (point.y < y1)
            {
                dist = sqrt(pow((point.x - x2), 2) + pow((point.y - y1), 2));
            }
            else if (point.y <= y2)
            {
                dist = point.x - x2;
            }
            else
            {
                dist = sqrt(pow((point.x - x2), 2) + pow((point.y - y2), 2));
            }
        }
        return dist;
    }
}

void Mbr::print()
{
    cout << "(x1=" << x1 << " y1=" << y1 << " x2=" << x2 << " y2=" << y2 << ")" << endl;
}

vector<Point> Mbr::get_corner_points()
{
    vector<Point> result;
    Point point1(0, x1, y1);
    Point point2(0, x2, y1);
    Point point3(0, x1, y2);
    Point point4(0, x2, y2);
    result.push_back(point1);
    result.push_back(point2);
    result.push_back(point3);
    result.push_back(point4);
    return result;
}

Mbr Mbr::get_mbr(Point point, float knnquerySide)
{
    float x1 = point.x - knnquerySide;
    float x2 = point.x + knnquerySide;
    float y1 = point.y - knnquerySide;
    float y2 = point.y + knnquerySide;

    x1 = x1 < 0 ? 0 : x1;
    y1 = y1 < 0 ? 0 : y1;

    x2 = x2 > 1 ? 1 : x2;
    y2 = y2 > 1 ? 1 : y2;

    Mbr mbr(x1, y1, x2, y2);
    return mbr;
}

void Mbr::clean()
{
    x1 = 0;
    x2 = 0;
    y1 = 0;
    y2 = 0;
}

string Mbr::get_self()
{
    return to_string(x1) + "," + to_string(y1) + "," + to_string(x2) + "," + to_string(y2) + "\n";
}