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

 * 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 testNoiseModel.cpp
 * @date Jan 13, 2010
 * @author Richard Roberts
 * @author Frank Dellaert
 */

#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/TestableAssertions.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/assign/std/vector.hpp>

#include <iostream>
#include <limits>

using namespace std;
using namespace gtsam;
using namespace noiseModel;
using namespace boost::assign;

static const double kSigma = 2, kInverseSigma = 1.0 / kSigma,
                    kVariance = kSigma * kSigma, prc = 1.0 / kVariance;
static const Matrix R = Matrix3::Identity() * kInverseSigma;
static const Matrix kCovariance = Matrix3::Identity() * kVariance;
static const Vector3 kSigmas(kSigma, kSigma, kSigma);

/* ************************************************************************* */
TEST(NoiseModel, constructors)
{
  Vector whitened = Vector3(5.0,10.0,15.0);
  Vector unwhitened = Vector3(10.0,20.0,30.0);

  // Construct noise models
  vector<Gaussian::shared_ptr> m;
  m.push_back(Gaussian::SqrtInformation(R,false));
  m.push_back(Gaussian::Covariance(kCovariance,false));
  m.push_back(Gaussian::Information(kCovariance.inverse(),false));
  m.push_back(Diagonal::Sigmas(kSigmas,false));
  m.push_back(Diagonal::Variances((Vector3(kVariance, kVariance, kVariance)),false));
  m.push_back(Diagonal::Precisions(Vector3(prc, prc, prc),false));
  m.push_back(Isotropic::Sigma(3, kSigma,false));
  m.push_back(Isotropic::Variance(3, kVariance,false));
  m.push_back(Isotropic::Precision(3, prc,false));

  // test kSigmas
  for(Gaussian::shared_ptr mi: m)
    EXPECT(assert_equal(kSigmas,mi->sigmas()));

  // test whiten
  for(Gaussian::shared_ptr mi: m)
    EXPECT(assert_equal(whitened,mi->whiten(unwhitened)));

  // test unwhiten
  for(Gaussian::shared_ptr mi: m)
    EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened)));

  // test Mahalanobis distance
  double distance = 5*5+10*10+15*15;
  for(Gaussian::shared_ptr mi: m)
    DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9);

  // test R matrix
  for(Gaussian::shared_ptr mi: m)
    EXPECT(assert_equal(R,mi->R()));

  // test covariance
  for(Gaussian::shared_ptr mi: m)
    EXPECT(assert_equal(kCovariance,mi->covariance()));

  // test covariance
  for(Gaussian::shared_ptr mi: m)
    EXPECT(assert_equal(kCovariance.inverse(),mi->information()));

  // test Whiten operator
  Matrix H((Matrix(3, 4) <<
      0.0, 0.0, 1.0, 1.0,
      0.0, 1.0, 0.0, 1.0,
      1.0, 0.0, 0.0, 1.0).finished());
  Matrix expected = kInverseSigma * H;
  for(Gaussian::shared_ptr mi: m)
    EXPECT(assert_equal(expected,mi->Whiten(H)));

  // can only test inplace version once :-)
  m[0]->WhitenInPlace(H);
  EXPECT(assert_equal(expected,H));
}

/* ************************************************************************* */
TEST(NoiseModel, Unit)
{
  Vector v = Vector3(5.0,10.0,15.0);
  Gaussian::shared_ptr u(Unit::Create(3));
  EXPECT(assert_equal(v,u->whiten(v)));
}

/* ************************************************************************* */
TEST(NoiseModel, equals)
{
  Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R),
                       g2 = Gaussian::SqrtInformation(I_3x3);
  Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector3(kSigma, kSigma, kSigma)),
                       d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3));
  Isotropic::shared_ptr i1 = Isotropic::Sigma(3, kSigma),
                        i2 = Isotropic::Sigma(3, 0.7);

  EXPECT(assert_equal(*g1,*g1));
  EXPECT(assert_inequal(*g1, *g2));

  EXPECT(assert_equal(*d1,*d1));
  EXPECT(assert_inequal(*d1,*d2));

  EXPECT(assert_equal(*i1,*i1));
  EXPECT(assert_inequal(*i1,*i2));
}

// TODO enable test once a mechanism for smart constraints exists
///* ************************************************************************* */
//TEST(NoiseModel, ConstrainedSmart )
//{
//  Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vector3(sigma, 0.0, sigma), true);
//  Diagonal::shared_ptr n1 = boost::dynamic_pointer_cast<Diagonal>(nonconstrained);
//  Constrained::shared_ptr n2 = boost::dynamic_pointer_cast<Constrained>(nonconstrained);
//  EXPECT(n1);
//  EXPECT(!n2);
//
//  Gaussian::shared_ptr constrained = Constrained::MixedSigmas(zero(3), true);
//  Diagonal::shared_ptr c1 = boost::dynamic_pointer_cast<Diagonal>(constrained);
//  Constrained::shared_ptr c2 = boost::dynamic_pointer_cast<Constrained>(constrained);
//  EXPECT(c1);
//  EXPECT(c2);
//}

/* ************************************************************************* */
TEST(NoiseModel, ConstrainedConstructors )
{
  Constrained::shared_ptr actual;
  size_t d = 3;
  double m = 100.0;
  Vector3 sigmas(kSigma, 0.0, 0.0);
  Vector3 mu(200.0, 300.0, 400.0);
  actual = Constrained::All(d);
  // TODO: why should this be a thousand ??? Dummy variable?
  EXPECT(assert_equal(Vector::Constant(d, 1000.0), actual->mu()));
  EXPECT(assert_equal(Vector::Constant(d, 0), actual->sigmas()));
  EXPECT(assert_equal(Vector::Constant(d, 0), actual->invsigmas())); // Actually zero as dummy value
  EXPECT(assert_equal(Vector::Constant(d, 0), actual->precisions())); // Actually zero as dummy value

  actual = Constrained::All(d, m);
  EXPECT(assert_equal(Vector::Constant(d, m), actual->mu()));

  actual = Constrained::All(d, mu);
  EXPECT(assert_equal(mu, actual->mu()));

  actual = Constrained::MixedSigmas(mu, sigmas);
  EXPECT(assert_equal(mu, actual->mu()));

  actual = Constrained::MixedSigmas(m, sigmas);
  EXPECT(assert_equal(Vector::Constant(d, m), actual->mu()));
}

/* ************************************************************************* */
TEST(NoiseModel, ConstrainedMixed )
{
  Vector feasible = Vector3(1.0, 0.0, 1.0),
      infeasible = Vector3(1.0, 1.0, 1.0);
  Diagonal::shared_ptr d = Constrained::MixedSigmas(Vector3(kSigma, 0.0, kSigma));
  // NOTE: we catch constrained variables elsewhere, so whitening does nothing
  EXPECT(assert_equal(Vector3(0.5, 1.0, 0.5),d->whiten(infeasible)));
  EXPECT(assert_equal(Vector3(0.5, 0.0, 0.5),d->whiten(feasible)));

  DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9);
  DOUBLES_EQUAL(0.5,d->distance(feasible),1e-9);
}

/* ************************************************************************* */
TEST(NoiseModel, ConstrainedAll )
{
  Vector feasible = Vector3(0.0, 0.0, 0.0),
       infeasible = Vector3(1.0, 1.0, 1.0);

  Constrained::shared_ptr i = Constrained::All(3);
  // NOTE: we catch constrained variables elsewhere, so whitening does nothing
  EXPECT(assert_equal(Vector3(1.0, 1.0, 1.0),i->whiten(infeasible)));
  EXPECT(assert_equal(Vector3(0.0, 0.0, 0.0),i->whiten(feasible)));

  DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9);
  DOUBLES_EQUAL(0.0,i->distance(feasible),1e-9);
}

/* ************************************************************************* */
namespace exampleQR {
  // create a matrix to eliminate
  Matrix Ab = (Matrix(4, 7) <<
      -1.,  0.,  1.,  0.,  0.,  0., -0.2,
      0., -1.,  0.,  1.,  0.,  0.,  0.3,
      1.,  0.,  0.,  0., -1.,  0.,  0.2,
      0.,  1.,  0.,  0.,  0., -1., -0.1).finished();
  Vector sigmas = (Vector(4) << 0.2, 0.2, 0.1, 0.1).finished();

  // the matrix AB yields the following factorized version:
  Matrix Rd = (Matrix(4, 7) <<
      11.1803,   0.0,   -2.23607, 0.0,    -8.94427, 0.0,     2.23607,
      0.0,   11.1803,    0.0,    -2.23607, 0.0,    -8.94427,-1.56525,
      0.0,       0.0,    4.47214, 0.0,    -4.47214, 0.0,     0.0,
      0.0,       0.0,   0.0,     4.47214, 0.0,    -4.47214, 0.894427).finished();

  SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
}

/* ************************************************************************* */
TEST( NoiseModel, QR )
{
  Matrix Ab1 = exampleQR::Ab;
  Matrix Ab2 = exampleQR::Ab; // otherwise overwritten !

  // Call Gaussian version
  SharedDiagonal actual1 = exampleQR::diagonal->QR(Ab1);
  EXPECT(actual1->isUnit());
  EXPECT(linear_dependent(exampleQR::Rd,Ab1,1e-4)); // Ab was modified in place !!!

  // Expected result for constrained version
  Vector expectedSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished();
  SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
  Matrix expectedRd2 = (Matrix(4, 7) <<
      1.,  0., -0.2,  0., -0.8, 0.,  0.2,
      0.,  1.,  0.,-0.2,   0., -0.8,-0.14,
      0.,  0.,  1.,   0., -1.,  0.,  0.0,
      0.,  0.,  0.,   1.,  0., -1.,  0.2).finished();

  // Call Constrained version
  SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(exampleQR::sigmas);
  SharedDiagonal actual2 = constrained->QR(Ab2);
  EXPECT(assert_equal(*expectedModel, *actual2, 1e-6));
  EXPECT(linear_dependent(expectedRd2, Ab2, 1e-6));  // Ab was modified in place !!!
}

/* ************************************************************************* */
TEST(NoiseModel, OverdeterminedQR) {
  Matrix Ab1(9, 4);
  Ab1 << 0, 1, 0, 0,  //
      0, 0, 1, 0,    //
      Matrix74::Ones();
  Matrix Ab2 = Ab1; // otherwise overwritten !

  // Call Gaussian version
  Vector9 sigmas = Vector9::Ones() ;
  SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
  SharedDiagonal actual1 = diagonal->QR(Ab1);
  EXPECT(actual1->isUnit());
  Matrix expectedRd(9,4);
  expectedRd << -2.64575131, -2.64575131, -2.64575131, -2.64575131,  //
      0.0, -1, 0, 0,                                                 //
      0.0, 0.0, -1, 0,                                               //
      Matrix64::Zero();
  EXPECT(assert_equal(expectedRd, Ab1, 1e-4));  // Ab was modified in place !!!

  // Expected result for constrained version
  Vector3 expectedSigmas(0.377964473, 1, 1);
  SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);

  // Call Constrained version
  SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas);
  SharedDiagonal actual2 = constrained->QR(Ab2);
  EXPECT(assert_equal(*expectedModel, *actual2, 1e-6));
  expectedRd.row(0) *= 0.377964473; // not divided by sigma!
  EXPECT(assert_equal(-expectedRd, Ab2, 1e-6));  // Ab was modified in place !!!
}

/* ************************************************************************* */
TEST( NoiseModel, MixedQR )
{
  // Call Constrained version, with first and third row treated as constraints
  // Naming the 6 variables u,v,w,x,y,z, we have
  // u = -z
  // w = -x
  // And let's have simple priors on variables
  Matrix Ab(5,6+1);
  Ab <<
      1,0,0,0,0,1,  0, // u+z = 0
      0,0,0,0,1,0,  0, // y^2
      0,0,1,1,0,0,  0, // w+x = 0
      0,1,0,0,0,0,  0, // v^2
      0,0,0,0,0,1,  0; // z^2
  Vector mixed_sigmas = (Vector(5) << 0, 1, 0, 1, 1).finished();
  SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(mixed_sigmas);

  // Expected result
  Vector expectedSigmas = (Vector(5) << 0, 1, 0, 1, 1).finished();
  SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
  Matrix expectedRd(5, 6+1);
  expectedRd << 1, 0, 0, 0, 0, 1, 0,  //
                0, 1, 0, 0, 0, 0, 0,  //
                0, 0, 1, 1, 0, 0, 0,  //
                0, 0, 0, 0, 1, 0, 0,  //
                0, 0, 0, 0, 0, 1, 0;  //

  SharedDiagonal actual = constrained->QR(Ab);
  EXPECT(assert_equal(*expectedModel,*actual,1e-6));
  EXPECT(linear_dependent(expectedRd,Ab,1e-6)); // Ab was modified in place !!!
}

/* ************************************************************************* */
TEST( NoiseModel, MixedQR2 )
{
  // Let's have three variables x,y,z, but x=z and y=z
  // Hence, all non-constraints are really measurements on z
  Matrix Ab(11,3+1);
  Ab <<
      1,0,0,  0, //
      0,1,0,  0, //
      0,0,1,  0, //
     -1,0,1,  0, // x=z
      1,0,0,  0, //
      0,1,0,  0, //
      0,0,1,  0, //
     0,-1,1,  0, // y=z
      1,0,0,  0, //
      0,1,0,  0, //
      0,0,1,  0; //

  Vector sigmas(11);
  sigmas.setOnes();
  sigmas[3] = 0;
  sigmas[7] = 0;
  SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas);

  // Expected result
  Vector3 expectedSigmas(0,0,1.0/3);
  SharedDiagonal expectedModel = noiseModel::Constrained::MixedSigmas(expectedSigmas);
  Matrix expectedRd(11, 3+1);
  expectedRd.setZero();
  expectedRd.row(0) << -1,  0, 1,  0;  // x=z
  expectedRd.row(1) <<  0, -1, 1,  0;  // y=z
  expectedRd.row(2) <<  0,  0, 1,  0;  // z=0 +/- 1/3

  SharedDiagonal actual = constrained->QR(Ab);
  EXPECT(assert_equal(*expectedModel,*actual,1e-6));
  EXPECT(assert_equal(expectedRd,Ab,1e-6)); // Ab was modified in place !!!
}

/* ************************************************************************* */
TEST( NoiseModel, FullyConstrained )
{
  Matrix Ab(3,7);
  Ab <<
      1,0,0,0,0,1,  2, // u+z = 2
      0,0,1,1,0,0,  4, // w+x = 4
      0,1,0,1,1,1,  8; // v+x+y+z=8
  SharedDiagonal constrained = noiseModel::Constrained::All(3);

  // Expected result
  SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(Vector3 (0,0,0));
  Matrix expectedRd(3, 7);
  expectedRd << 1, 0, 0, 0, 0, 1, 2,  //
                0, 1, 0, 1, 1, 1, 8,  //
                0, 0, 1, 1, 0, 0, 4;  //

  SharedDiagonal actual = constrained->QR(Ab);
  EXPECT(assert_equal(*expectedModel,*actual,1e-6));
  EXPECT(linear_dependent(expectedRd,Ab,1e-6)); // Ab was modified in place !!!
}

/* ************************************************************************* */
// This matches constraint_eliminate2 in testJacobianFactor
TEST(NoiseModel, QRNan )
{
  SharedDiagonal constrained = noiseModel::Constrained::All(2);
  Matrix Ab = (Matrix25() << 2, 4, 2, 4, 6,   2, 1, 2, 4, 4).finished();

  SharedDiagonal expected = noiseModel::Constrained::All(2);
  Matrix expectedAb = (Matrix25() << 1, 2, 1, 2, 3, 0, 1, 0, 0, 2.0/3).finished();

  SharedDiagonal actual = constrained->QR(Ab);
  EXPECT(assert_equal(*expected,*actual));
  EXPECT(linear_dependent(expectedAb,Ab));
}

/* ************************************************************************* */
TEST(NoiseModel, SmartSqrtInformation )
{
  bool smart = true;
  gtsam::SharedGaussian expected = Unit::Create(3);
  gtsam::SharedGaussian actual = Gaussian::SqrtInformation(I_3x3, smart);
  EXPECT(assert_equal(*expected,*actual));
}

/* ************************************************************************* */
TEST(NoiseModel, SmartSqrtInformation2 )
{
  bool smart = true;
  gtsam::SharedGaussian expected = Unit::Isotropic::Sigma(3,2);
  gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*I_3x3, smart);
  EXPECT(assert_equal(*expected,*actual));
}

/* ************************************************************************* */
TEST(NoiseModel, SmartInformation )
{
  bool smart = true;
  gtsam::SharedGaussian expected = Unit::Isotropic::Variance(3,2);
  Matrix M = 0.5*I_3x3;
  EXPECT(checkIfDiagonal(M));
  gtsam::SharedGaussian actual = Gaussian::Information(M, smart);
  EXPECT(assert_equal(*expected,*actual));
}

/* ************************************************************************* */
TEST(NoiseModel, SmartCovariance )
{
  bool smart = true;
  gtsam::SharedGaussian expected = Unit::Create(3);
  gtsam::SharedGaussian actual = Gaussian::Covariance(I_3x3, smart);
  EXPECT(assert_equal(*expected,*actual));
}

/* ************************************************************************* */
TEST(NoiseModel, ScalarOrVector )
{
  bool smart = true;
  SharedGaussian expected = Unit::Create(3);
  SharedGaussian actual = Gaussian::Covariance(I_3x3, smart);
  EXPECT(assert_equal(*expected,*actual));
}

/* ************************************************************************* */
TEST(NoiseModel, WhitenInPlace)
{
  Vector sigmas = Vector3(0.1, 0.1, 0.1);
  SharedDiagonal model = Diagonal::Sigmas(sigmas);
  Matrix A = I_3x3;
  model->WhitenInPlace(A);
  Matrix expected = I_3x3 * 10;
  EXPECT(assert_equal(expected, A));
}

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

/*
 * These tests are responsible for testing the weight functions for the m-estimators in GTSAM.
 * The weight function is related to the analytic derivative of the residual function. See
 *  https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
 * for details. This weight function is required when optimizing cost functions with robust
 * penalties using iteratively re-weighted least squares.
 */

TEST(NoiseModel, robustFunctionFair)
{
  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
  const mEstimator::Fair::shared_ptr fair = mEstimator::Fair::Create(k);
  DOUBLES_EQUAL(0.8333333333333333, fair->weight(error1), 1e-8);
  DOUBLES_EQUAL(0.3333333333333333, fair->weight(error2), 1e-8);
  // Test negative value to ensure we take absolute value of error.
  DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8);
  DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8);

  DOUBLES_EQUAL(0.441961080151135, fair->residual(error1), 1e-8);
  DOUBLES_EQUAL(22.534692783297260, fair->residual(error2), 1e-8);
  DOUBLES_EQUAL(22.534692783297260, fair->residual(error3), 1e-8);
  DOUBLES_EQUAL(0.441961080151135, fair->residual(error4), 1e-8);
}

TEST(NoiseModel, robustFunctionHuber)
{
  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
  const mEstimator::Huber::shared_ptr huber = mEstimator::Huber::Create(k);
  DOUBLES_EQUAL(1.0, huber->weight(error1), 1e-8);
  DOUBLES_EQUAL(0.5, huber->weight(error2), 1e-8);
  // Test negative value to ensure we take absolute value of error.
  DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8);
  DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8);

  DOUBLES_EQUAL(0.5000, huber->residual(error1), 1e-8);
  DOUBLES_EQUAL(37.5000, huber->residual(error2), 1e-8);
  DOUBLES_EQUAL(37.5000, huber->residual(error3), 1e-8);
  DOUBLES_EQUAL(0.5000, huber->residual(error4), 1e-8);
}

TEST(NoiseModel, robustFunctionCauchy)
{
  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
  const mEstimator::Cauchy::shared_ptr cauchy = mEstimator::Cauchy::Create(k);
  DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error1), 1e-8);
  DOUBLES_EQUAL(0.2000, cauchy->weight(error2), 1e-8);
  // Test negative value to ensure we take absolute value of error.
  DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8);
  DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8);

  DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error1), 1e-8);
  DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error2), 1e-8);
  DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error3), 1e-8);
  DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error4), 1e-8);
}

TEST(NoiseModel, robustFunctionGemanMcClure)
{
  const double k = 1.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
  const mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(k);
  DOUBLES_EQUAL(0.25      , gmc->weight(error1), 1e-8);
  DOUBLES_EQUAL(9.80296e-5, gmc->weight(error2), 1e-8);
  DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8);
  DOUBLES_EQUAL(0.25      , gmc->weight(error4), 1e-8);

  DOUBLES_EQUAL(0.2500, gmc->residual(error1), 1e-8);
  DOUBLES_EQUAL(0.495049504950495, gmc->residual(error2), 1e-8);
  DOUBLES_EQUAL(0.495049504950495, gmc->residual(error3), 1e-8);
  DOUBLES_EQUAL(0.2500, gmc->residual(error4), 1e-8);
}

TEST(NoiseModel, robustFunctionWelsch)
{
  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
  const mEstimator::Welsch::shared_ptr welsch = mEstimator::Welsch::Create(k);
  DOUBLES_EQUAL(0.960789439152323, welsch->weight(error1), 1e-8);
  DOUBLES_EQUAL(0.018315638888734, welsch->weight(error2), 1e-8);
  // Test negative value to ensure we take absolute value of error.
  DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8);
  DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8);

  DOUBLES_EQUAL(0.490132010595960, welsch->residual(error1), 1e-8);
  DOUBLES_EQUAL(12.271054513890823, welsch->residual(error2), 1e-8);
  DOUBLES_EQUAL(12.271054513890823, welsch->residual(error3), 1e-8);
  DOUBLES_EQUAL(0.490132010595960, welsch->residual(error4), 1e-8);
}

TEST(NoiseModel, robustFunctionTukey)
{
  const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0;
  const mEstimator::Tukey::shared_ptr tukey = mEstimator::Tukey::Create(k);
  DOUBLES_EQUAL(0.9216, tukey->weight(error1), 1e-8);
  DOUBLES_EQUAL(0.0, tukey->weight(error2), 1e-8);
  // Test negative value to ensure we take absolute value of error.
  DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8);
  DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8);

  DOUBLES_EQUAL(0.480266666666667, tukey->residual(error1), 1e-8);
  DOUBLES_EQUAL(4.166666666666667, tukey->residual(error2), 1e-8);
  DOUBLES_EQUAL(4.166666666666667, tukey->residual(error3), 1e-8);
  DOUBLES_EQUAL(0.480266666666667, tukey->residual(error4), 1e-8);
}

TEST(NoiseModel, robustFunctionDCS)
{
  const double k = 1.0, error1 = 1.0, error2 = 10.0;
  const mEstimator::DCS::shared_ptr dcs = mEstimator::DCS::Create(k);

  DOUBLES_EQUAL(1.0       , dcs->weight(error1), 1e-8);
  DOUBLES_EQUAL(0.00039211, dcs->weight(error2), 1e-8);

  DOUBLES_EQUAL(0.5         , dcs->residual(error1), 1e-8);
  DOUBLES_EQUAL(0.9900990099, dcs->residual(error2), 1e-8);
}

TEST(NoiseModel, robustFunctionL2WithDeadZone)
{
  const double k = 1.0, e0 = -10.0, e1 = -1.01, e2 = -0.99, e3 = 0.99, e4 = 1.01, e5 = 10.0;
  const mEstimator::L2WithDeadZone::shared_ptr lsdz = mEstimator::L2WithDeadZone::Create(k);

  DOUBLES_EQUAL(0.9,           lsdz->weight(e0), 1e-8);
  DOUBLES_EQUAL(0.00990099009, lsdz->weight(e1), 1e-8);
  DOUBLES_EQUAL(0.0,           lsdz->weight(e2), 1e-8);
  DOUBLES_EQUAL(0.0,           lsdz->weight(e3), 1e-8);
  DOUBLES_EQUAL(0.00990099009, lsdz->weight(e4), 1e-8);
  DOUBLES_EQUAL(0.9,           lsdz->weight(e5), 1e-8);

  DOUBLES_EQUAL(40.5,    lsdz->residual(e0), 1e-8);
  DOUBLES_EQUAL(0.00005, lsdz->residual(e1), 1e-8);
  DOUBLES_EQUAL(0.0,     lsdz->residual(e2), 1e-8);
  DOUBLES_EQUAL(0.0,     lsdz->residual(e3), 1e-8);
  DOUBLES_EQUAL(0.00005, lsdz->residual(e4), 1e-8);
  DOUBLES_EQUAL(40.5,    lsdz->residual(e5), 1e-8);
}

/* ************************************************************************* */
TEST(NoiseModel, robustNoiseHuber)
{
  const double k = 10.0, error1 = 1.0, error2 = 100.0;
  Matrix A = (Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0).finished();
  Vector b = Vector2(error1, error2);
  const Robust::shared_ptr robust = Robust::Create(
    mEstimator::Huber::Create(k, mEstimator::Huber::Scalar),
    Unit::Create(2));

  robust->WhitenSystem(A, b);

  DOUBLES_EQUAL(error1, b(0), 1e-8);
  DOUBLES_EQUAL(sqrt(k*error2), b(1), 1e-8);

  DOUBLES_EQUAL(1.0, A(0,0), 1e-8);
  DOUBLES_EQUAL(10.0, A(0,1), 1e-8);
  DOUBLES_EQUAL(sqrt(k*100.0), A(1,0), 1e-8);
  DOUBLES_EQUAL(sqrt(k/100.0)*1000.0, A(1,1), 1e-8);
}

TEST(NoiseModel, robustNoiseGemanMcClure)
{
  const double k = 1.0, error1 = 1.0, error2 = 100.0;
  const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0;
  Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished();
  Vector b = Vector2(error1, error2);
  const Robust::shared_ptr robust = Robust::Create(
    mEstimator::GemanMcClure::Create(k, mEstimator::GemanMcClure::Scalar),
    Unit::Create(2));

  robust->WhitenSystem(A, b);

  const double k2 = k*k;
  const double k4 = k2*k2;
  const double k2error = k2 + error2*error2;

  const double sqrt_weight_error1 = sqrt(0.25);
  const double sqrt_weight_error2 = sqrt(k4/(k2error*k2error));

  DOUBLES_EQUAL(sqrt_weight_error1*error1, b(0), 1e-8);
  DOUBLES_EQUAL(sqrt_weight_error2*error2, b(1), 1e-8);

  DOUBLES_EQUAL(sqrt_weight_error1*a00, A(0,0), 1e-8);
  DOUBLES_EQUAL(sqrt_weight_error1*a01, A(0,1), 1e-8);
  DOUBLES_EQUAL(sqrt_weight_error2*a10, A(1,0), 1e-8);
  DOUBLES_EQUAL(sqrt_weight_error2*a11, A(1,1), 1e-8);
}

TEST(NoiseModel, robustNoiseDCS)
{
  const double k = 1.0, error1 = 1.0, error2 = 100.0;
  const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0;
  Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished();
  Vector b = Vector2(error1, error2);
  const Robust::shared_ptr robust = Robust::Create(
    mEstimator::DCS::Create(k, mEstimator::DCS::Scalar),
    Unit::Create(2));

  robust->WhitenSystem(A, b);

  const double sqrt_weight = 2.0*k/(k + error2*error2);

  DOUBLES_EQUAL(error1, b(0), 1e-8);
  DOUBLES_EQUAL(sqrt_weight*error2, b(1), 1e-8);

  DOUBLES_EQUAL(a00, A(0,0), 1e-8);
  DOUBLES_EQUAL(a01, A(0,1), 1e-8);
  DOUBLES_EQUAL(sqrt_weight*a10, A(1,0), 1e-8);
  DOUBLES_EQUAL(sqrt_weight*a11, A(1,1), 1e-8);
}

TEST(NoiseModel, robustNoiseL2WithDeadZone)
{
  double dead_zone_size = 1.0;
  SharedNoiseModel robust = noiseModel::Robust::Create(
    noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size),
    Unit::Create(3));

/*
 * TODO(mike): There is currently a bug in GTSAM, where none of the mEstimator classes
 * implement a residual function, and GTSAM calls the weight function to evaluate the
 * total penalty, rather than calling the residual function. The weight function should be
 * used during iteratively reweighted least squares optimization, but should not be used to
 * evaluate the total penalty. The long-term solution is for all mEstimators to implement
 * both a weight and a residual function, and for GTSAM to call the residual function when
 * evaluating the total penalty. This bug causes the test below to fail, so I'm leaving it
 * commented out until the underlying bug in GTSAM is fixed.
 *
 * for (int i = 0; i < 5; i++) {
 *   Vector3 error = Vector3(i, 0, 0);
 *   DOUBLES_EQUAL(0.5*max(0,i-1)*max(0,i-1), robust->distance(error), 1e-8);
 * }
 */

}

/* ************************************************************************* */
#define TEST_GAUSSIAN(gaussian)\
  EQUALITY(info, gaussian->information());\
  EQUALITY(cov, gaussian->covariance());\
  EXPECT(assert_equal(white, gaussian->whiten(e)));\
  EXPECT(assert_equal(e, gaussian->unwhiten(white)));\
  EXPECT_DOUBLES_EQUAL(251, gaussian->distance(e), 1e-9);\
  Matrix A = R.inverse(); Vector b = e;\
  gaussian->WhitenSystem(A, b);\
  EXPECT(assert_equal(I, A));\
  EXPECT(assert_equal(white, b));

TEST(NoiseModel, NonDiagonalGaussian)
{
  Matrix3 R;
  R << 6, 5, 4, 0, 3, 2, 0, 0, 1;
  const Matrix3 info = R.transpose() * R;
  const Matrix3 cov = info.inverse();
  const Vector3 e(1, 1, 1), white = R * e;
  Matrix I = Matrix3::Identity();

  {
  SharedGaussian gaussian = Gaussian::SqrtInformation(R);
  TEST_GAUSSIAN(gaussian);
  }

  {
  SharedGaussian gaussian = Gaussian::Information(info);
  TEST_GAUSSIAN(gaussian);
  }

  {
  SharedGaussian gaussian = Gaussian::Covariance(cov);
  TEST_GAUSSIAN(gaussian);
  }
}

/* ************************************************************************* */
int main() {  TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */


////////////////////////////////////////////////////////////////////////////////
#include "boundary_loop.h"
#include <cellogram/tri2hex.h>
#include <cellogram/navigation.h>
// #include <igl/is_border_vertex.h>
#include <igl/edges.h>
#include <Eigen/Dense>
#include <vector>
#include <iostream>
////////////////////////////////////////////////////////////////////////////////

namespace cellogram {

// -----------------------------------------------------------------------------
	typedef std::vector<int> Path;

Path get_longest_path(const std::vector<Path> &paths) {
	if (paths.empty()) return Path();
	int maxi = 0;
	for (unsigned int i = 1; i < paths.size(); i++) {
		if (paths[maxi].size() < paths[i].size()) {
			maxi = i;
		}
	}
	return paths[maxi];
}

Eigen::VectorXi path_to_vecXi(const Path &p) {
	Eigen::VectorXi res(p.size());
	for (size_t i = 0; i < p.size(); i++) res(i)=p[i];
	return res;
}

void boundary_graph(const Eigen::MatrixXi &F, std::vector<std::vector<int>> &adj) {
	int num_vertices = F.maxCoeff() + 1;
	NavigationData data(F);
	adj.clear();
	adj.resize(num_vertices);
	for (int f = 0; f < F.rows(); ++f) {
		NavigationIndex idx = index_from_face(F, data, f, 0);
		for (int lv = 0; lv < F.cols(); ++lv) {
			if (switch_face(data, idx).face < 0) {
				int v1 = idx.vertex;
				int v2 = switch_vertex(data, idx).vertex;
				adj[v1].push_back(v2);
				adj[v2].push_back(v1);
			}
			idx = next_around_face(data, idx);
		}
	}
}

void boundary_loop(const Eigen::MatrixXi &F, Eigen::VectorXi &longest_path) {

	int n = F.maxCoeff() + 1;

	// Eigen::VectorXd Vdummy(F.maxCoeff() + 1, 1);
	// std::vector<bool> border_vertex = igl::is_border_vertex(Vdummy, F);

	std::vector<std::vector<int>> adj;
	//tri2hex(F, adj);
	boundary_graph(F, adj);

	std::vector< Path > paths; // paths that we detect

	int num_vertices = adj.size();
	std::vector<bool> possible_starting_point(num_vertices, true);
	for (int v0 = 0; v0 < num_vertices; ++v0) {
		if (adj[v0].size() == 2 && possible_starting_point[v0]) {
			std::vector<bool> in_the_path(num_vertices, false);
			std::vector<bool> has_been_removed(num_vertices, false);
			std::vector<int> prev(num_vertices, -1);
			int x = v0;
			Path path;
			path.push_back(v0);
			in_the_path[v0] = true;
			prev[v0] = v0;
			for (int i = 0; i < num_vertices; ++i) {
				// Check neighbors of x
				bool found_v0 = false;
				for (int y : adj[x]) {
					if (in_the_path[y]) {
						if (y == v0) {
							found_v0 = true;
						} else if (prev[x] != y) {
							int z = x;
							while (z != y) {
								path.pop_back();
								has_been_removed[z] = true;
								in_the_path[z] = false;
								int old_z = z;
								z = prev[z];
								prev[old_z] = -1;
							}
							has_been_removed[y] = false;
							x = y;
							found_v0 = false;
							break;
						}
					} else if (!has_been_removed[y]) {
						prev[y] = x;
						x = y;
						found_v0 = false;
						path.push_back(x);
						in_the_path[x] = true;
						break;
					}
				}

				if (x == v0 || found_v0) {
					break;
				}
			}

			// Cleanup
			for (int x : path) {
				possible_starting_point[x] = false;
			}
			if (!path.empty()) {
				if (path.size() == 3) {
					//std::cout << "boundary_loop.cpp, path size is only three " << v0 << std::endl << "Path not added\n";
				}
				else{
					//std::cout << path.size() << std::endl;
					paths.push_back(path);
				}
			}
		}
	}

	longest_path = path_to_vecXi( get_longest_path(paths) );

}

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

} // namespace cellogram


#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/PoseStamped.h"
#include "videoray/Throttle.h"
#include "nav_msgs/Odometry.h"

#include <iostream>
#include <sstream>

#include <boost/numeric/odeint.hpp>

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

using namespace boost::numeric::odeint;

typedef boost::array< double , 6 > state_type;

#define PI (3.14159265359)

videoray::Throttle throttle_;
void throttleCallback(const videoray::Throttle::ConstPtr& msg)
{
     throttle_ = *msg;
     //// Left Throttle Conversion:
     //left_vel_ = saturate(msg->LeftThrottle, -100, 100);
     //left_vel_ = normalize(left_vel_, -100, 100, -1, 1);
     //
     //// Right Throttle Conversion:
     //right_vel_ = saturate(msg->RightThrottle, -100, 100);
     //right_vel_ = normalize(right_vel_, -100, 100, -1, 1);
     //
     //// Vertical Throttle Conversion:
     //vert_vel_ = saturate(msg->VerticalThrottle, -100, 100);
     //vert_vel_ = normalize(vert_vel_, -100, 100, -1, 1);
}

nav_msgs::Odometry odom_;
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
     odom_ = *msg;
}

// Linear and angular velocity states
double u;
double v;
double w;
double p;
double q;
double r;

// Added Mass Terms
double X_udot = 1.94; // inertia matrix M (m11)
double Y_vdot = 6.05; // inertia matrix M (m22)
double Z_wdot = 3.95; // m33
double N_rdot = 1.18e-2; // vehicle's motion of inertia about z-axis
// (6,6) entry of the vehicle inertia Matrix M

// Linear Drag Coefficients
double Xu = -0.95;
double Yv = -5.87;
double Nr = -0.023;
double Zw = -3.70;

// Quadratic Drag Coefficients
double Xuu = -6.04;
double Yvv = -30.73;
double Nrr = -0.45;
double Zww = -26.36;

double Ct_forw = 0.026667;
double Ct_back = 0.026667;
double Ct_vert_forw = 0.026667;
double Ct_vert_back = 0.026667;

double u_sat_low = -150;
double u_sat_high = 150;

// Control inputs
double X = 0;
double N = 0;
double Z = 0;

double u_port = 0;
double u_star = 0;
double u_vert = 0;

void videoray_model( const state_type &x , state_type &dxdt , double t )
{
/// States:
/// 0:  u     : surge velocity
/// 1:  v     : sway velocity
/// 2:  w     : heave velocity
/// 3:  p     : roll rate
/// 4:  q     : pitch rate
/// 5:  r     : yaw rate
     u = x[0];
     v = x[1];
     w = x[2];
     p = x[3];
     q = x[4];
     r = x[5];

     // Calculate fixed frame velocity rates
     dxdt[0] = (-Y_vdot*v*r + Xu*u + Xuu*u*abs(u) + X) / X_udot;
     dxdt[1] = (X_udot*u*r + Yv*v + Yvv*v*abs(v)) / Y_vdot;
     dxdt[2] = (Zw*w + Zww*w*abs(w) + Z) / Z_wdot;

     // Calculate fixed frame orientation rates
     dxdt[3] = 0;
     dxdt[4] = 0;
     dxdt[5] = (Nr*r + Nrr*r*abs(r) + N) / N_rdot;
}

//
// Converts input throttle commands to simulated linear and angular velocities.
//
geometry_msgs::Twist velocity_cmd_;
geometry_msgs::Vector3 velocity_linear_;
geometry_msgs::Vector3 velocity_angular_;

double saturate(double input, const double &min, const double &max)
{
     if (min > max) {
          ROS_INFO("saturate(): Invalid Min / Max Combo");
          return 0;
     } else if (input < min) {
          input = min;
     } else if(input > max) {
          input = max;
     }
     return input;
}

// Assumes that input has already been saturated within the in_min and in_max
// boundaries. Use the saturate() function on input before calling normalize
double normalize(double input, const double &in_min, const double &in_max,
                 const double &out_min, const double &out_max)
{
     if (in_min >= in_max || out_min >= out_max) {
          ROS_INFO("normalize(): Invalid Min / Max Combo");
          return 0;
     }

     double ratio = input / (in_max - in_min);
     return ratio * (out_max - out_min);

     return input;
}

double thrust_port = 0, thrust_star = 0;
void processThrottleCmds()
{
     u_port = saturate(throttle_.PortInput, u_sat_low, u_sat_high);
     u_star = saturate(throttle_.StarInput, u_sat_low, u_sat_high);
     u_vert = saturate(throttle_.VertInput, u_sat_low, u_sat_high);

     // Ct is different for reverse and forward
     if ( u_port >= 0 ) {
          thrust_port = u_port * Ct_forw;
     } else {
          thrust_port = u_port * Ct_back;
     }

     if ( u_star >= 0 ) {
          thrust_star = u_star * Ct_forw;
     } else {
          thrust_star = u_star * Ct_back;
     }

     X = thrust_port + thrust_star;
     N = thrust_star - thrust_port;

     // Ct is different for reverse and forward
     if ( u_vert >= 0 ) {
          Z = u_vert * Ct_vert_forw;
     } else {
          Z = u_vert * Ct_vert_back;
     }

}

void quaternionToEuler(const double &q0, const double &q1,
                       const double &q2, const double &q3,
                       double &roll, double &pitch, double &yaw)
{
     roll = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2) );
     pitch = asin(2*(q0*q2-q3*q1));
     yaw = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3) );
}

runge_kutta4< state_type > stepper;

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

     ros::Publisher twist_pub = n.advertise<geometry_msgs::Twist>("motion", 1);
     ros::Subscriber throttle_sub = n.subscribe("throttle_cmds", 1,
                                                throttleCallback);
     ros::Subscriber odom_sub = n.subscribe("odometry", 1,
                                            odomCallback);

     double rate = 10;
     ros::Rate loop_rate(rate);

     ros::Time begin = ros::Time::now();
     ros::Time curr_time = begin;
     ros::Time prev_time = begin;
     ros::Duration dt = curr_time - prev_time;

     while (ros::ok())
     {
          //cout << "*" << std::flush;

          curr_time = ros::Time::now();
          dt = curr_time - prev_time;
          prev_time = curr_time;

          //cout << dt.toSec() << endl << std::flush;

          // Update state vector with odometry data from morse...
          state_type x = {0,0,0,0,0,0};
          x[0] = odom_.twist.twist.linear.x;
          x[1] = odom_.twist.twist.linear.y;
          x[2] = odom_.twist.twist.linear.z;
          x[3] = odom_.twist.twist.angular.x;
          x[4] = odom_.twist.twist.angular.y;
          x[5] = odom_.twist.twist.angular.z;

          processThrottleCmds();

          //geometry_msgs::Quaternion quat = odom_.pose.pose.orientation;
          //quaternionToEuler(quat.x, quat.y, quat.z, quat.w,
          //                  roll_, pitch_, yaw_);

          //boost::numeric::odeint::integrate(videoray_model,
          //                                  x,
          //                                  curr_time.toSec() ,
          //                                  (curr_time + dt).toSec(),
          //                                  dt.toSec());
          boost::numeric::odeint::integrate(videoray_model,
                                            x,
                                            curr_time.toSec() ,
                                            curr_time.toSec() + 1.0/rate,
                                            1.0/rate);

          //ROS_INFO("========================");
          //ROS_INFO("Current: %f, \tdt: %f", curr_time.toSec(), dt.toSec());
          //ROS_INFO("Surge: %f", x[0]);
          //ROS_INFO("Sway: %f", x[1]);
          //ROS_INFO("Heave: %f", x[2]);
          //
          //ROS_INFO("3: %f", x[3]);
          //ROS_INFO("4: %f", x[4]);
          //ROS_INFO("5: %f", x[5]);
          ////ROS_INFO("6: %f", x[6]);
          ////ROS_INFO("7: %f", x[7]);
          ////ROS_INFO("8: %f", x[8]);
          ////ROS_INFO("9: %f", x[9]);
          ////ROS_INFO("10: %f", x[10]);
          ////ROS_INFO("11: %f", x[11]);

          velocity_linear_.x = x[0];
          velocity_linear_.y = x[1];
          velocity_linear_.z = x[2];
          velocity_angular_.z = x[5];

          velocity_cmd_.linear = velocity_linear_;
          velocity_cmd_.angular = velocity_angular_;
          twist_pub.publish(velocity_cmd_);

          ros::spinOnce();

          loop_rate.sleep();
     }
     return 0;
}


//////////////////////////////////////////////////////////////////////////////
// random::poisson_ex::devroye::detail::step4::squeeze.hpp         			//
//                                                                          //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_POISSON_EXT_DEVROYE_DETAIL_STEP4_SQUEEZE_HPP_ER_2010
#define BOOST_RANDOM_POISSON_EXT_DEVROYE_DETAIL_STEP4_SQUEEZE_HPP_ER_2010
#include <boost/random/poisson_ext/devroye/detail/q.hpp>
#include <boost/random/poisson_ext/devroye/detail/math.hpp>
#include <boost/random/poisson_ext/devroye/detail/int_mean.hpp>

namespace boost{
namespace random{
namespace poisson{
namespace devroye{
namespace detail{
namespace step4{

    // Poisson sampler by the method of Devroye with a squeeze step4
	//
	// Complexity O(1) as mean -> infinity, p.204
    //
    // Bug : currently does not converge.
	template<typename Int ,typename T,typename P>
    class squeeze :
        detail::q<Int,T,P>,
        public detail::int_mean<step4::squeeze<Int,T,P>,Int,T,P>
    {

		typedef detail::q<Int,T,P> q_fun_;
        typedef devroye::detail::math<Int,T,P> ma_;

		typedef detail::int_mean<step4::squeeze<Int,T,P>,Int,T,P> super_;

		public:
		typedef typename super_::result_type result_type;
		typedef typename super_::input_type input_type;

		squeeze():super_(){}
		explicit squeeze(const result_type& mean):super_(mean){}

		bool accept(){
            bool a = ma_::is_strictly_negative(this->x());

			typedef result_type int_;
			typedef input_type float_;
			int_ y1 = this->y();
			int_ yp1 = y1 + 1;
            int_ y1yp1 = y1 * yp1;

			float_ t = ma_::to_float(y1yp1) / this->m2(); 	// 1.

			if(
            	(!a) && ( this->v() < (-t) )
            ){
				// exit
            }else{
                int_ m1 = this->int_mean_val();
                int_ m6 = 6 * m1;
                int_ y2 = 2 * y1;
                int_ y2p1 = y2 + 1;
                int_ m1p_ayp1 = m1 + a ? yp1 : 0;

            	float_ qr = t * (
                	ma_::to_float(y2p1) /ma_::to_float(m6) - ma_::to_float(1)
                );
                float_ tsq = t * t;
            	float_ qa =  qr - tsq  / ma_::to_float(3 * m1p_ayp1);

				if(this->v()<qa){
					// exit
                }else{
                	if(this->v()>qr){
                    	return false;
                    }else{

                    	float_ q_val = this->q_fun(m1,this->y());

                    	if(this->v() < q_val)
                        {
                        	// exit
                        }else{
                        	return false;
                        }
                    }
                }
				return false;
            }

            // exit :
            this->y_ += this->int_mean_val();
            return true;
		}

	};

}// detail
}// step4
}// devroye
}// poisson
}// random
}// boost

#endif


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file gaussian_filter_nonlinear_generic.hpp
 * \date October 2014
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <fl/util/meta.hpp>
#include <fl/util/traits.hpp>
#include <fl/filter/filter_interface.hpp>
#include <fl/distribution/gaussian.hpp>

namespace fl
{

/**
* \defgroup generic_nonlinear_gaussian_filter Generic Nonlinear Gaussian Filter
* \ingroup filters
*/

// Forward delcaration
template <typename...> class GaussianFilter;

/**
 * \internal
 * \ingroup nonlinear_gaussian_filter
 * \ingroup generic_nonlinear_gaussian_filter
 *
 * Traits for generic GaussianFilter based on quadrature (numeric integration)
 * with customizable policies, i.e implementations of the time and measurement
 * updates.
 */
template <
    typename Transition,
    typename Sensor,
    typename Quadrature,
    typename ... Policies
>
struct Traits<
           GaussianFilter<
               Transition, Sensor, Quadrature, Policies...>>
{
    typedef typename Transition::State State;
    typedef typename Transition::Input Input;
    typedef typename Sensor::Obsrv Obsrv;
    typedef Gaussian<State> Belief;
};

/**
 * \ingroup nonlinear_gaussian_filter
 * \ingroup generic_nonlinear_gaussian_filter
 *
 * GaussianFilter represents all filters based on Gaussian distributed systems.
 * This includes the Kalman Filter and filters using non-linear models such as
 * Sigma Point Kalman Filter family.
 *
 * \tparam TransitionFunction
 * \tparam SensorFunction
 * \tparam Quadrature
 * \tparam PredictionPolicy
 * \tparam UpdatePolicy
 */
template<
    typename TransitionFunction,
    typename SensorFunction,
    typename Quadrature,
    typename PredictionPolicy,
    typename UpdatePolicy
>
class GaussianFilter<
          TransitionFunction,
          SensorFunction,
          Quadrature,
          PredictionPolicy,
          UpdatePolicy>
    :
    /* Implement the filter interface */
    public FilterInterface<
               GaussianFilter<
                   TransitionFunction,
                   SensorFunction,
                   Quadrature,
                   PredictionPolicy,
                   UpdatePolicy>>
{
public:
    typedef typename TransitionFunction::State State;
    typedef typename TransitionFunction::Input Input;
    typedef typename SensorFunction::Obsrv Obsrv;
    typedef Gaussian<State> Belief;

public:
    /**
     * Creates a Gaussian filter
     *
     * \param transition         Process model instance
     * \param sensor           Obsrv model instance
     * \param transform   Point set tranfrom such as the unscented
     *                              transform
     */
    GaussianFilter(const TransitionFunction& transition,
                   const SensorFunction& sensor,
                   const Quadrature& quadrature)
        : transition_(transition),
          sensor_(sensor),
          quadrature_(quadrature)
    { }

    /**
     * \brief Overridable default destructor
     */
    virtual ~GaussianFilter() { }

    /**
     * \copydoc FilterInterface::predict
     */
    virtual void predict(const Belief& prior_belief,
                         const Input& input,
                         Belief& predicted_belief)
    {
        prediction_policy_(transition(),
                           quadrature(),
                           prior_belief,
                           input,
                           predicted_belief);
    }

    /**
     * \copydoc FilterInterface::update
     */
    virtual void update(const Belief& predicted_belief,
                        const Obsrv& obsrv,
                        Belief& posterior_belief)
    {
        update_policy_(sensor(),
                       quadrature(),
                       predicted_belief,
                       obsrv,
                       posterior_belief);
    }

public: /* factory functions */
    virtual Belief create_belief() const
    {
        auto belief = Belief(transition().state_dimension());
        return belief; // RVO
    }

public: /* accessors & mutators */
    TransitionFunction& transition()
    {
        return transition_;
    }

    SensorFunction& sensor()
    {
        return sensor_;
    }

    Quadrature& quadrature()
    {
        return quadrature_;
    }

    const TransitionFunction& transition() const
    {
        return transition_;
    }

    const SensorFunction& sensor() const
    {
        return sensor_;
    }

    const Quadrature& quadrature() const
    {
        return quadrature_;
    }

    virtual std::string name() const
    {
        return "GaussianFilter<"
                + this->list_arguments(
                            transition().name(),
                            sensor().name(),
                            quadrature().name(),
                            prediction_policy_.name(),
                            update_policy_.name())
                + ">";
    }

    virtual std::string description() const
    {
        return "Sigma point based GaussianFilter with"
                + this->list_descriptions(
                            transition().description(),
                            sensor().description(),
                            quadrature().description(),
                            prediction_policy_.description(),
                            update_policy_.description());
    }

protected:
    /** \cond internal */
    TransitionFunction transition_;
    SensorFunction sensor_;
    Quadrature quadrature_;
    PredictionPolicy prediction_policy_;
    UpdatePolicy update_policy_;
    /** \endcond */
};

}