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

 * 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    testImuFactor.cpp
 * @brief   Unit test for ImuFactor
 * @author  Luca Carlone
 * @author  Frank Dellaert
 * @author  Richard Roberts
 * @author  Stephen Williams
 */

#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/ScenarioRunner.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>

#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <list>

#include "imuFactorTesting.h"

namespace testing {
// Create default parameters with Z-down and above noise parameters
static boost::shared_ptr<PreintegrationParams> Params() {
  auto p = PreintegrationParams::MakeSharedD(kGravity);
  p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
  p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
  p->integrationCovariance = 0.0001 * I_3x3;
  return p;
}
}

/* ************************************************************************* */
namespace {
// Auxiliary functions to test evaluate error in ImuFactor
/* ************************************************************************* */
Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
    const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
    const Bias& bias) {
  return Rot3::Expmap(
      factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).head(3));
}

} // namespace

/* ************************************************************************* */
TEST(ImuFactor, PreintegratedMeasurementsConstruction) {
  // Actual pre-integrated values
  PreintegratedImuMeasurements actual(testing::Params());
  EXPECT(assert_equal(Rot3(), actual.deltaRij()));
  EXPECT(assert_equal(kZero, actual.deltaPij()));
  EXPECT(assert_equal(kZero, actual.deltaVij()));
  DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9);
}

/* ************************************************************************* */
TEST(ImuFactor, PreintegratedMeasurementsReset) {

	auto p = testing::Params();
	// Create a preintegrated measurement struct and integrate
	PreintegratedImuMeasurements pimActual(p);
	Vector3 measuredAcc(0.5, 1.0, 0.5);
	Vector3 measuredOmega(0.1, 0.3, 0.1);
	double deltaT = 1.0;
	pimActual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

	// reset and make sure that it is the same as a fresh one
	pimActual.resetIntegration();
	CHECK(assert_equal(pimActual, PreintegratedImuMeasurements(p)));

	// Now create one with a different bias ..
	Bias nonZeroBias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3));
	PreintegratedImuMeasurements pimExpected(p, nonZeroBias);

	// integrate again, then reset to a new bias
	pimActual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
	pimActual.resetIntegrationAndSetBias(nonZeroBias);
	CHECK(assert_equal(pimActual, pimExpected));
}

/* ************************************************************************* */
TEST(ImuFactor, Accelerating) {
  const double a = 0.2, v = 50;

  // Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X
  // The body itself has Z axis pointing down
  const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
  const Point3 initial_position(10, 20, 0);
  const Vector3 initial_velocity(v, 0, 0);

  const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
      Vector3(a, 0, 0));

  const double T = 3.0; // seconds
  ScenarioRunner runner(&scenario, testing::Params(), T / 10);

  PreintegratedImuMeasurements pim = runner.integrate(T);
  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));

  Matrix9 estimatedCov = runner.estimateCovariance(T);
  EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
}

/* ************************************************************************* */
TEST(ImuFactor, PreintegratedMeasurements) {
  // Measurements
  const double a = 0.1, w = M_PI / 100.0;
  Vector3 measuredAcc(a, 0.0, 0.0);
  Vector3 measuredOmega(w, 0.0, 0.0);
  double deltaT = 0.5;

  // Expected pre-integrated values
  Vector3 expectedDeltaR1(w * deltaT, 0.0, 0.0);
  Vector3 expectedDeltaP1(0.5 * a * deltaT*deltaT, 0, 0);
  Vector3 expectedDeltaV1(0.05, 0.0, 0.0);

  // Actual pre-integrated values
  PreintegratedImuMeasurements actual(testing::Params());
  actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
  EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR1), actual.deltaRij()));
  EXPECT(assert_equal(expectedDeltaP1, actual.deltaPij()));
  EXPECT(assert_equal(expectedDeltaV1, actual.deltaVij()));
  DOUBLES_EQUAL(0.5, actual.deltaTij(), 1e-9);

  // Check derivatives of computeError
  Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
  NavState x1, x2 = actual.predict(x1, bias);

  {
  Matrix9 aH1, aH2;
  Matrix96 aH3;
  actual.computeError(x1, x2, bias, aH1, aH2, aH3);
  boost::function<Vector9(const NavState&, const NavState&, const Bias&)> f =
      boost::bind(&PreintegrationBase::computeError, actual, _1, _2, _3,
                  boost::none, boost::none, boost::none);
  EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
  EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
  EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9));
  }

  // Integrate again
  Vector3 expectedDeltaR2(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0);
  Vector3 expectedDeltaP2(0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0);
  Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) +
                            Rot3::Expmap(expectedDeltaR1) * measuredAcc * 0.5;

  // Actual pre-integrated values
  actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
  EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR2), actual.deltaRij()));
  EXPECT(assert_equal(expectedDeltaP2, actual.deltaPij()));
  EXPECT(assert_equal(expectedDeltaV2, actual.deltaVij()));
  DOUBLES_EQUAL(1.0, actual.deltaTij(), 1e-9);
}
/* ************************************************************************* */
// Common linearization point and measurements for tests
namespace common {
static const Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
    Point3(5.0, 1.0, 0));
static const Vector3 v1(Vector3(0.5, 0.0, 0.0));
static const NavState state1(x1, v1);

// Measurements
static const double w = M_PI / 100;
static const Vector3 measuredOmega(w, 0, 0);
static const Vector3 measuredAcc = x1.rotation().unrotate(
    -kGravityAlongNavZDown);
static const double deltaT = 1.0;

static const Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + w, M_PI / 6.0, M_PI / 4.0),
    Point3(5.5, 1.0, 0));
static const Vector3 v2(Vector3(0.5, 0.0, 0.0));
static const NavState state2(x2, v2);
} // namespace common

/* ************************************************************************* */
TEST(ImuFactor, PreintegrationBaseMethods) {
  using namespace common;
  auto p = testing::Params();
  p->omegaCoriolis = Vector3(0.02, 0.03, 0.04);
  p->use2ndOrderCoriolis = true;

  PreintegratedImuMeasurements pim(p, kZeroBiasHat);
  pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
  pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  // biasCorrectedDelta
  Matrix96 actualH;
  pim.biasCorrectedDelta(kZeroBias, actualH);
  Matrix expectedH = numericalDerivative11<Vector9, Bias>(
      boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1,
          boost::none), kZeroBias);
  EXPECT(assert_equal(expectedH, actualH));

  Matrix9 aH1;
  Matrix96 aH2;
  NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2);
  Matrix eH1 = numericalDerivative11<NavState, NavState>(
      boost::bind(&PreintegrationBase::predict, pim, _1, kZeroBias, boost::none,
          boost::none), state1);
  EXPECT(assert_equal(eH1, aH1));
  Matrix eH2 = numericalDerivative11<NavState, Bias>(
      boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none,
          boost::none), kZeroBias);
  EXPECT(assert_equal(eH2, aH2));
}

/* ************************************************************************* */
TEST(ImuFactor, MultipleMeasurements) {
  using namespace common;

  PreintegratedImuMeasurements expected(testing::Params(), kZeroBiasHat);
  expected.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
  expected.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  Matrix32 acc,gyro;
  Matrix12 dts;
  acc << measuredAcc, measuredAcc;
  gyro << measuredOmega, measuredOmega;
  dts << deltaT, deltaT;
  PreintegratedImuMeasurements actual(testing::Params(), kZeroBiasHat);
  actual.integrateMeasurements(acc,gyro,dts);

  EXPECT(assert_equal(expected,actual));
}

/* ************************************************************************* */
TEST(ImuFactor, ErrorAndJacobians) {
  using namespace common;
  PreintegratedImuMeasurements pim(testing::Params());

  pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
  EXPECT(assert_equal(state2, pim.predict(state1, kZeroBias)));

  // Create factor
  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);

  // Expected error
  Vector expectedError(9);
  expectedError << 0, 0, 0, 0, 0, 0, 0, 0, 0;
  EXPECT(
      assert_equal(expectedError,
          factor.evaluateError(x1, v1, x2, v2, kZeroBias)));

  Values values;
  values.insert(X(1), x1);
  values.insert(V(1), v1);
  values.insert(X(2), x2);
  values.insert(V(2), v2);
  values.insert(B(1), kZeroBias);
  EXPECT(assert_equal(expectedError, factor.unwhitenedError(values)));

  // Make sure linearization is correct
  double diffDelta = 1e-7;
  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);

  // Actual Jacobians
  Matrix H1a, H2a, H3a, H4a, H5a;
  (void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, H1a, H2a, H3a, H4a,
      H5a);

  // Make sure rotation part is correct when error is interpreted as axis-angle
  // Jacobians are around zero, so the rotation part is the same as:
  Matrix H1Rot3 = numericalDerivative11<Rot3, Pose3>(
      boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias),
      x1);
  EXPECT(assert_equal(H1Rot3, H1a.topRows(3)));

  Matrix H3Rot3 = numericalDerivative11<Rot3, Pose3>(
      boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias),
      x2);
  EXPECT(assert_equal(H3Rot3, H3a.topRows(3)));

  // Evaluate error with wrong values
  Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1);
  values.update(V(2), v2_wrong);
  expectedError << 0, 0, 0, 0, 0, 0, -0.0724744871, -0.040715657, -0.151952901;
  EXPECT(
      assert_equal(expectedError,
          factor.evaluateError(x1, v1, x2, v2_wrong, kZeroBias), 1e-2));
  EXPECT(assert_equal(expectedError, factor.unwhitenedError(values), 1e-2));

  // Make sure the whitening is done correctly
  Matrix cov = pim.preintMeasCov();
  Matrix R = RtR(cov.inverse());
  Vector whitened = R * expectedError;
  EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-4));

  // Make sure linearization is correct
  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);
}

/* ************************************************************************* */
TEST(ImuFactor, ErrorAndJacobianWithBiases) {
  using common::x1;
  using common::v1;
  using common::v2;
  Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)),
      Point3(5.5, 1.0, -50.0));

  // Measurements
  Vector3 measuredOmega;
  measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
  Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown)
      + Vector3(0.2, 0.0, 0.0);
  double deltaT = 1.0;

  auto p = testing::Params();
  p->omegaCoriolis = kNonZeroOmegaCoriolis;

  Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1));
  PreintegratedImuMeasurements pim(p, biasHat);
  pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  // Make sure of biasCorrectedDelta
  Matrix96 actualH;
  pim.biasCorrectedDelta(bias, actualH);
  Matrix expectedH = numericalDerivative11<Vector9, Bias>(
      boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1,
          boost::none), bias);
  EXPECT(assert_equal(expectedH, actualH));

  // Create factor
  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);

  Values values;
  values.insert(X(1), x1);
  values.insert(V(1), v1);
  values.insert(X(2), x2);
  values.insert(V(2), v2);
  values.insert(B(1), bias);

  // Make sure linearization is correct
  double diffDelta = 1e-7;
  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);
}

/* ************************************************************************* */
TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
  using common::x1;
  using common::v1;
  using common::v2;
  Bias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)),
      Point3(5.5, 1.0, -50.0));

  // Measurements
  Vector3 measuredOmega;
  measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
  Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown)
      + Vector3(0.2, 0.0, 0.0);
  double deltaT = 1.0;

  auto p = testing::Params();
  p->omegaCoriolis = kNonZeroOmegaCoriolis;
  p->use2ndOrderCoriolis = true;

  PreintegratedImuMeasurements pim(p,
      Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)));
  pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  // Create factor
  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);

  Values values;
  values.insert(X(1), x1);
  values.insert(V(1), v1);
  values.insert(X(2), x2);
  values.insert(V(2), v2);
  values.insert(B(1), bias);

  // Make sure linearization is correct
  double diffDelta = 1e-7;
  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);
}

/* ************************************************************************* */
TEST(ImuFactor, PartialDerivative_wrt_Bias) {
  // Linearization point
  Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias

  // Measurements
  Vector3 measuredOmega(0.1, 0, 0);
  double deltaT = 0.5;

  auto evaluateRotation = [=](const Vector3 biasOmega) {
    return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
  };

  // Compute numerical derivatives
  Matrix expectedDelRdelBiasOmega =
      numericalDerivative11<Rot3, Vector3>(evaluateRotation, biasOmega);

  const Matrix3 Jr =
      Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);

  Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign

  // Compare Jacobians
  EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-9));
}

/* ************************************************************************* */
TEST(ImuFactor, PartialDerivativeLogmap) {
  // Linearization point
  Vector3 thetahat(0.1, 0.1, 0); // Current estimate of rotation rate bias

  // Measurements
  Vector3 deltatheta(0, 0, 0);

  auto evaluateLogRotation = [=](const Vector3 deltatheta) {
    return Rot3::Logmap(
        Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
  };

  // Compute numerical derivatives
  Matrix expectedDelFdeltheta =
      numericalDerivative11<Vector, Vector3>(evaluateLogRotation, deltatheta);

  Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat);

  // Compare Jacobians
  EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
}

/* ************************************************************************* */
TEST(ImuFactor, fistOrderExponential) {
  // Linearization point
  Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias

  // Measurements
  Vector3 measuredOmega(0.1, 0, 0);
  double deltaT = 1.0;

  // change w.r.t. linearization point
  double alpha = 0.0;
  Vector3 deltabiasOmega;
  deltabiasOmega << alpha, alpha, alpha;

  const Matrix3 Jr = Rot3::ExpmapDerivative(
      (measuredOmega - biasOmega) * deltaT);

  Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign

  const Matrix expectedRot = Rot3::Expmap(
      (measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();

  const Matrix3 hatRot =
      Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
  const Matrix3 actualRot = hatRot
      * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
  // hatRot * (I_3x3 + skewSymmetric(delRdelBiasOmega * deltabiasOmega));

  // This is a first order expansion so the equality is only an approximation
  EXPECT(assert_equal(expectedRot, actualRot));
}

/* ************************************************************************* */
Vector3 correctedAcc(const PreintegratedImuMeasurements& pim,
    const Vector3& measuredAcc, const Vector3& measuredOmega) {
  Vector3 correctedAcc = pim.biasHat().correctAccelerometer(measuredAcc);
  Vector3 correctedOmega = pim.biasHat().correctGyroscope(measuredOmega);
  return pim.correctMeasurementsBySensorPose(correctedAcc, correctedOmega).first;
}

TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
  const Rot3 nRb = Rot3::Expmap(Vector3(0, 0, M_PI / 4.0));
  const Point3 p1(5.0, 1.0, -50.0);
  const Vector3 v1(0.5, 0.0, 0.0);

  const Vector3 a = nRb * Vector3(0.2, 0.0, 0.0);
  const AcceleratingScenario scenario(nRb, p1, v1, a,
      Vector3(0, 0, M_PI / 10.0 + 0.3));

  auto p = testing::Params();
  p->body_P_sensor = Pose3(Rot3::Expmap(Vector3(0, M_PI / 2, 0)),
      Point3(0.1, 0.05, 0.01));
  p->omegaCoriolis = kNonZeroOmegaCoriolis;

  Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));

  const double T = 3.0; // seconds
  ScenarioRunner runner(&scenario, p, T / 10);

  //  PreintegratedImuMeasurements pim = runner.integrate(T);
  //  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
  //
  //  Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
  //  EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
  //
  ///////////////////////////////////////////////////////////////////////////////////////////
  Pose3 x1(nRb, p1);

  // Measurements
  Vector3 measuredOmega = runner.actualAngularVelocity(0);
  Vector3 measuredAcc = runner.actualSpecificForce(0);

  // Get mean prediction from "ground truth" measurements
  const Vector3 accNoiseVar2(0.01, 0.02, 0.03);
  const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02);
  PreintegratedImuMeasurements pim(p, biasHat);

  // Check updatedDeltaXij derivatives
  Matrix3 D_correctedAcc_measuredOmega = Z_3x3;
  pim.correctMeasurementsBySensorPose(measuredAcc, measuredOmega,
      boost::none, D_correctedAcc_measuredOmega, boost::none);
  Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(
      boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6);
  EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5));

  double dt = 0.1;

// TODO(frank): revive derivative tests
//  Matrix93 G1, G2;
//  Vector9 preint =
//      pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2);
//
//  Matrix93 expectedG1 = numericalDerivative21<NavState, Vector3, Vector3>(
//      boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2,
//          dt, boost::none, boost::none, boost::none), measuredAcc,
//      measuredOmega, 1e-6);
//  EXPECT(assert_equal(expectedG1, G1, 1e-5));
//
//  Matrix93 expectedG2 = numericalDerivative22<NavState, Vector3, Vector3>(
//      boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2,
//          dt, boost::none, boost::none, boost::none), measuredAcc,
//      measuredOmega, 1e-6);
//  EXPECT(assert_equal(expectedG2, G2, 1e-5));

  Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)

  // integrate at least twice to get position information
  // otherwise factor cov noise from preint_cov is not positive definite
  pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
  pim.integrateMeasurement(measuredAcc, measuredOmega, dt);

  // Create factor
  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);

  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
      Point3(5.5, 1.0, -50.0));
  Vector3 v2(Vector3(0.5, 0.0, 0.0));

  Values values;
  values.insert(X(1), x1);
  values.insert(V(1), v1);
  values.insert(X(2), x2);
  values.insert(V(2), v2);
  values.insert(B(1), bias);

  // Make sure linearization is correct
  double diffDelta = 1e-8;
  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);
}

/* ************************************************************************* */
TEST(ImuFactor, PredictPositionAndVelocity) {
  Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)

  // Measurements
  Vector3 measuredOmega;
  measuredOmega << 0, 0, 0; // M_PI/10.0+0.3;
  Vector3 measuredAcc;
  measuredAcc << 0, 1, -kGravity;
  double deltaT = 0.001;

  PreintegratedImuMeasurements pim(testing::Params(),
      Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));

  for (int i = 0; i < 1000; ++i)
    pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  // Create factor
  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);

  // Predict
  Pose3 x1;
  Vector3 v1(0, 0.0, 0.0);
  NavState actual = pim.predict(NavState(x1, v1), bias);
  NavState expected(Rot3(), Point3(0, 0.5, 0), Vector3(0, 1, 0));
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(ImuFactor, PredictRotation) {
  Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)

  // Measurements
  Vector3 measuredOmega;
  measuredOmega << 0, 0, M_PI / 10; // M_PI/10.0+0.3;
  Vector3 measuredAcc;
  measuredAcc << 0, 0, -kGravity;
  double deltaT = 0.001;

  PreintegratedImuMeasurements pim(testing::Params(),
      Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));

  for (int i = 0; i < 1000; ++i)
    pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  // Create factor
  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);

  // Predict
  NavState actual = pim.predict(NavState(), bias);
  NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0,0,0), Z_3x1);
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(ImuFactor, PredictArbitrary) {
  Pose3 x1;
  const Vector3 v1(0, 0, 0);

  const AcceleratingScenario scenario(x1.rotation(), x1.translation(), v1,
      Vector3(0.1, 0.2, 0), Vector3(M_PI / 10, M_PI / 10, M_PI / 10));

  const double T = 3.0; // seconds
  ScenarioRunner runner(&scenario, testing::Params(), T / 10);
  //
  //  PreintegratedImuMeasurements pim = runner.integrate(T);
  //  EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose, 1e-9));
  //
  //  Matrix6 estimatedCov = runner.estimatePoseCovariance(T);
  //  EXPECT(assert_equal(estimatedCov, runner.poseCovariance(pim), 0.1));
  //////////////////////////////////////////////////////////////////////////////////

  Bias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));

  // Measurements
  Vector3 measuredOmega = runner.actualAngularVelocity(0);
  Vector3 measuredAcc = runner.actualSpecificForce(0);

  auto p = testing::Params();
  p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise
  PreintegratedImuMeasurements pim(p, biasHat);
  Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
//  EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, measuredAcc, measuredOmega,
//                    Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000));

  double dt = 0.001;
  for (int i = 0; i < 1000; ++i)
    pim.integrateMeasurement(measuredAcc, measuredOmega, dt);

  // Create factor
  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);

  // Predict
  NavState actual = pim.predict(NavState(x1, v1), bias);

  // Regression test for Imu Refactor
  Rot3 expectedR( //
      +0.903715275, -0.250741668, 0.347026393, //
      +0.347026393, 0.903715275, -0.250741668, //
      -0.250741668, 0.347026393, 0.903715275);
  Point3 expectedT(-0.516077031, 0.57842919, 0.0876478403);
  Vector3 expectedV(-1.62337767, 1.57954409, 0.343833571);
  NavState expected(expectedR, expectedT, expectedV);
  EXPECT(assert_equal(expected, actual, 1e-7));
}

/* ************************************************************************* */
TEST(ImuFactor, bodyPSensorNoBias) {
  Bias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)

  // Rotate sensor (z-down) to body (same as navigation) i.e. z-up
  auto p = testing::Params();
  p->n_gravity = Vector3(0, 0, -kGravity); // z-up nav frame
  p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0, 0, 0));

  // Measurements
  // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame
  Vector3 s_omegaMeas_ns(0, 0.1, M_PI / 10);
  // Acc measurement is acceleration of sensor in the sensor frame, when stationary,
  // table exerts an equal and opposite force w.r.t gravity
  Vector3 s_accMeas(0, 0, -kGravity);
  double dt = 0.001;

  PreintegratedImuMeasurements pim(p, bias);

  for (int i = 0; i < 1000; ++i)
    pim.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt);

  // Create factor
  ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);

  // Predict
  NavState actual = pim.predict(NavState(), bias);

  Pose3 expectedPose(Rot3::Ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0));
  EXPECT(assert_equal(expectedPose, actual.pose()));

  Vector3 expectedVelocity(0, 0, 0);
  EXPECT(assert_equal(Vector(expectedVelocity), Vector(actual.velocity())));
}

/* ************************************************************************* */
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>

TEST(ImuFactor, bodyPSensorWithBias) {
  using noiseModel::Diagonal;
  typedef Bias Bias;

  int numFactors = 80;
  Vector6 noiseBetweenBiasSigma;
  noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6,
      3.0e-6, 3.0e-6);
  SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma);

  // Measurements
  // Sensor frame is z-down
  // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame
  Vector3 measuredOmega(0, 0.01, 0);
  // Acc measurement is acceleration of sensor in the sensor frame, when stationary,
  // table exerts an equal and opposite force w.r.t gravity
  Vector3 measuredAcc(0, 0, -kGravity);

  auto p = testing::Params();
  p->n_gravity = Vector3(0, 0, -kGravity);
  p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0,0,0));
  p->accelerometerCovariance = 1e-7 * I_3x3;
  p->gyroscopeCovariance = 1e-8 * I_3x3;
  p->integrationCovariance = 1e-9 * I_3x3;
  double deltaT = 0.005;

  //   Specify noise values on priors
  Vector6 priorNoisePoseSigmas(
      (Vector(6) << 0.001, 0.001, 0.001, 0.01, 0.01, 0.01).finished());
  Vector3 priorNoiseVelSigmas((Vector(3) << 0.1, 0.1, 0.1).finished());
  Vector6 priorNoiseBiasSigmas(
      (Vector(6) << 0.1, 0.1, 0.1, 0.5e-1, 0.5e-1, 0.5e-1).finished());
  SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas);
  SharedDiagonal priorNoiseVel = Diagonal::Sigmas(priorNoiseVelSigmas);
  SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas);
  Vector3 zeroVel(0, 0, 0);

  // Create a factor graph with priors on initial pose, vlocity and bias
  NonlinearFactorGraph graph;
  Values values;

  PriorFactor<Pose3> priorPose(X(0), Pose3(), priorNoisePose);
  graph.add(priorPose);
  values.insert(X(0), Pose3());

  PriorFactor<Vector3> priorVel(V(0), zeroVel, priorNoiseVel);
  graph.add(priorVel);
  values.insert(V(0), zeroVel);

  // The key to this test is that we specify the bias, in the sensor frame, as known a priori
  // We also create factors below that encode our assumption that this bias is constant over time
  // In theory, after optimization, we should recover that same bias estimate
  Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
  PriorFactor<Bias> priorBiasFactor(B(0), priorBias, priorNoiseBias);
  graph.add(priorBiasFactor);
  values.insert(B(0), priorBias);

  // Now add IMU factors and bias noise models
  Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0));
  for (int i = 1; i < numFactors; i++) {
    PreintegratedImuMeasurements pim = PreintegratedImuMeasurements(p,
        priorBias);
    for (int j = 0; j < 200; ++j)
      pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

    // Create factors
    graph.add(ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim));
    graph.add(BetweenFactor<Bias>(B(i - 1), B(i), zeroBias, biasNoiseModel));

    values.insert(X(i), Pose3());
    values.insert(V(i), zeroVel);
    values.insert(B(i), priorBias);
  }

  // Finally, optimize, and get bias at last time step
  Values results = LevenbergMarquardtOptimizer(graph, values).optimize();
  Bias biasActual = results.at<Bias>(B(numFactors - 1));

  // And compare it with expected value (our prior)
  Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0));
  EXPECT(assert_equal(biasExpected, biasActual, 1e-3));
}

/* ************************************************************************* */
#ifdef GTSAM_TANGENT_PREINTEGRATION
static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6;

struct ImuFactorMergeTest {
  boost::shared_ptr<PreintegratedImuMeasurements::Params> p_;
  const ConstantTwistScenario forward_, loop_;

  ImuFactorMergeTest()
      : p_(PreintegratedImuMeasurements::Params::MakeSharedU(kGravity)),
        forward_(kZero, Vector3(kVelocity, 0, 0)),
        loop_(Vector3(0, -kAngularVelocity, 0), Vector3(kVelocity, 0, 0)) {
    // arbitrary noise values
    p_->gyroscopeCovariance = I_3x3 * 0.01;
    p_->accelerometerCovariance = I_3x3 * 0.02;
    p_->accelerometerCovariance = I_3x3 * 0.03;
  }

  int TestScenario(TestResult& result_, const std::string& name_,
                           const Scenario& scenario,
                           const Bias& bias01,
                           const Bias& bias12, double tol) {
    // Test merge by creating a 01, 12, and 02 PreintegratedRotation,
    // then checking the merge of 01-12 matches 02.
    PreintegratedImuMeasurements pim01(p_, bias01);
    PreintegratedImuMeasurements pim12(p_, bias12);
    PreintegratedImuMeasurements pim02_expected(p_, bias01);

    double deltaT = 0.01;
    ScenarioRunner runner(&scenario, p_, deltaT);
    // TODO(frank) can this loop just go into runner ?
    for (int i = 0; i < 100; i++) {
      double t = i * deltaT;
      // integrate the measurements appropriately
      Vector3 accel_meas = runner.actualSpecificForce(t);
      Vector3 omega_meas = runner.actualAngularVelocity(t);
      pim02_expected.integrateMeasurement(accel_meas, omega_meas, deltaT);
      if (i < 50) {
        pim01.integrateMeasurement(accel_meas, omega_meas, deltaT);
      } else {
        pim12.integrateMeasurement(accel_meas, omega_meas, deltaT);
      }
    }
    auto actual_pim02 = ImuFactor::Merge(pim01, pim12);
    EXPECT(assert_equal(pim02_expected.preintegrated(),
                        actual_pim02.preintegrated(), tol));
    EXPECT(assert_equal(pim02_expected, actual_pim02, tol));

    ImuFactor::shared_ptr factor01 =
        boost::make_shared<ImuFactor>(X(0), V(0), X(1), V(1), B(0), pim01);
    ImuFactor::shared_ptr factor12 =
        boost::make_shared<ImuFactor>(X(1), V(1), X(2), V(2), B(0), pim12);
    ImuFactor::shared_ptr factor02_expected = boost::make_shared<ImuFactor>(
        X(0), V(0), X(2), V(2), B(0), pim02_expected);

    ImuFactor::shared_ptr factor02_merged = ImuFactor::Merge(factor01, factor12);
    EXPECT(assert_equal(*factor02_expected, *factor02_merged, tol));
    return result_.getFailureCount();
  }

  void TestScenarios(TestResult& result_, const std::string& name_,
                     const Bias& bias01,
                     const Bias& bias12, double tol) {
    for (auto scenario : {forward_, loop_})
      TestScenario(result_, name_, scenario, bias01, bias12, tol);
  }
};

/* ************************************************************************* */
// Test case with zero biases
TEST(ImuFactor, MergeZeroBias) {
  ImuFactorMergeTest mergeTest;
  mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4);
}

// Test case with identical biases: we expect an exact answer.
TEST(ImuFactor, MergeConstantBias) {
  ImuFactorMergeTest mergeTest;
  Bias bias(Vector3(0.03, -0.02, 0.01), Vector3(-0.01, 0.02, -0.03));
  mergeTest.TestScenarios(result_, name_, bias, bias, 1e-4);
}

// Test case with different biases where we expect there to be some variation.
TEST(ImuFactor, MergeChangingBias) {
  ImuFactorMergeTest mergeTest;
  mergeTest.TestScenarios(result_, name_,
      Bias(Vector3(0.03, -0.02, 0.01), Vector3(-0.01, 0.02, -0.03)),
      Bias(Vector3(0.01, 0.02, 0.03), Vector3(0.03, -0.02, 0.01)), 1e-1);
}

// Test case with non-zero coriolis
TEST(ImuFactor, MergeWithCoriolis) {
  ImuFactorMergeTest mergeTest;
  mergeTest.p_->omegaCoriolis = Vector3(0.1, 0.2, -0.1);
  mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4);
}
#endif

/* ************************************************************************* */
// Same values as pre-integration test but now testing covariance
TEST(ImuFactor, CheckCovariance) {
  // Measurements
  Vector3 measuredAcc(0.1, 0.0, 0.0);
  Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
  double deltaT = 0.5;

  PreintegratedImuMeasurements actual(testing::Params());
  actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
  Matrix9 expected;
  expected << 1.0577e-08, 0, 0, 0, 0, 0, 0, 0, 0,     //
      0, 1.0577e-08, 0, 0, 0, 0, 0, 0, 0,             //
      0, 0, 1.0577e-08, 0, 0, 0, 0, 0, 0,             //
      0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, 0, 0,  //
      0, 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, 0,  //
      0, 0, 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07,  //
      0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06, 0, 0,  //
      0, 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06, 0,  //
      0, 0, 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06;
  EXPECT(assert_equal(expected, actual.preintMeasCov()));
}

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


/*  This file is part of libDAI - http://www.libdai.org/
 *
 *  Copyright (c) 2006-2011, The libDAI authors. All rights reserved.
 *
 *  Use of this source code is governed by a BSD-style license that can be found in the LICENSE file.
 */

#include <iostream>
#include <fstream>
#include <map>
#include <numeric>
#include <cmath>
#include <cstdlib>
#include <cstring>
#include <boost/program_options.hpp>
#include <dai/util.h>
#include <dai/alldai.h>

using namespace std;
using namespace dai;
namespace po = boost::program_options;

std::vector<Real> calcDists( const vector<Factor> &x, const vector<Factor> &y ) {
    vector<Real> errs;
    errs.reserve( x.size() );
    DAI_ASSERT( x.size() == y.size() );
    for( size_t i = 0; i < x.size(); i++ )
        errs.push_back( dist( x[i], y[i], DISTTV ) );
    return errs;
}

/// Wrapper class for DAI approximate inference algorithms
class TestDAI {
    protected:
        /// Stores a pointer to an InfAlg object, managed by this class
        InfAlg          *obj;
        /// Stores the name of the InfAlg algorithm
        string          name;
        /// Stores the total variation distances of the variable marginals
        vector<Real>    varErr;
        /// Stores the total variation distances of the factor marginals
        vector<Real>    facErr;

    public:
        /// Stores the variable marginals
        vector<Factor>  varMarginals;
        /// Stores the factor marginals
        vector<Factor>  facMarginals;
        /// Stores all marginals
        vector<Factor>  allMarginals;
        /// Stores the logarithm of the partition sum
        Real            logZ;
        /// Stores the maximum difference in the last iteration
        Real            maxdiff;
        /// Stores the computation time (in seconds)
        double          time;
        /// Stores the number of iterations needed
        size_t          iters;
        /// Does the InfAlg support logZ()?
        bool            has_logZ;
        /// Does the InfAlg support maxDiff()?
        bool            has_maxdiff;
        /// Does the InfAlg support Iterations()?
        bool            has_iters;

        /// Construct from factor graph \a fg, name \a _name, and set of properties \a opts
        TestDAI( const FactorGraph &fg, const string &_name, const PropertySet &opts ) : obj(NULL), name(_name), varErr(), facErr(), varMarginals(), facMarginals(), allMarginals(), logZ(0.0), maxdiff(0.0), time(0), iters(0U), has_logZ(false), has_maxdiff(false), has_iters(false) {
            double tic = toc();

            if( name == "LDPC" ) {
                // special case: simulating a Low Density Parity Check code
                Real zero[2] = {1.0, 0.0};
                for( size_t i = 0; i < fg.nrVars(); i++ )
                    varMarginals.push_back( Factor(fg.var(i), zero) );
                allMarginals = varMarginals;
                logZ = 0.0;
                maxdiff = 0.0;
                iters = 1;
                has_logZ = false;
                has_maxdiff = false;
                has_iters = false;
            } else
                // create a corresponding InfAlg object
                obj = newInfAlg( name, fg, opts );

            // Add the time needed to create the object
            time += toc() - tic;
        }

        /// Destructor
        ~TestDAI() {
            if( obj != NULL )
                delete obj;
        }

        /// Identify
        string identify() const {
            if( obj != NULL )
                return obj->identify();
            else
                return "NULL";
        }

        /// Run the algorithm and store its results
        void doDAI() {
            double tic = toc();
            if( obj != NULL ) {
                // Initialize
                obj->init();
                // Run
                obj->run();
                // Record the time
                time += toc() - tic;

                // Store logarithm of the partition sum (if supported)
                try {
                    logZ = obj->logZ();
                    has_logZ = true;
                } catch( Exception &e ) {
                    if( e.getCode() == Exception::NOT_IMPLEMENTED )
                        has_logZ = false;
                    else
                        throw;
                }

                // Store maximum difference encountered in last iteration (if supported)
                try {
                    maxdiff = obj->maxDiff();
                    has_maxdiff = true;
                } catch( Exception &e ) {
                    if( e.getCode() == Exception::NOT_IMPLEMENTED )
                        has_maxdiff = false;
                    else
                        throw;
                }

                // Store number of iterations needed (if supported)
                try {
                    iters = obj->Iterations();
                    has_iters = true;
                } catch( Exception &e ) {
                    if( e.getCode() == Exception::NOT_IMPLEMENTED )
                        has_iters = false;
                    else
                        throw;
                }

                // Store variable marginals
                varMarginals.clear();
                for( size_t i = 0; i < obj->fg().nrVars(); i++ )
                    varMarginals.push_back( obj->beliefV( i ) );

                // Store factor marginals
                facMarginals.clear();
                for( size_t I = 0; I < obj->fg().nrFactors(); I++ )
                    try {
                        facMarginals.push_back( obj->beliefF( I ) );
                    } catch( Exception &e ) {
                        if( e.getCode() == Exception::BELIEF_NOT_AVAILABLE )
                            facMarginals.push_back( Factor( obj->fg().factor(I).vars(), INFINITY ) );
                        else
                            throw;
                    }

                // Store all marginals calculated by the method
                allMarginals = obj->beliefs();
            };
        }

        /// Calculate total variation distance of variable and factor marginals with respect to those in \a varMargs and \a facMargs
        void calcErrors( const vector<Factor>& varMargs, const vector<Factor>& facMargs ) {
            varErr = calcDists( varMarginals, varMargs );
            facErr = calcDists( facMarginals, facMargs );
        }

        /// Return maximum variable error
        Real maxVarErr() {
            return( *max_element( varErr.begin(), varErr.end() ) );
        }

        /// Return average variable error
        Real avgVarErr() {
            return( accumulate( varErr.begin(), varErr.end(), 0.0 ) / varErr.size() );
        }

        /// Return maximum factor error
        Real maxFacErr() {
            return( *max_element( facErr.begin(), facErr.end() ) );
        }

        /// Return average factor error
        Real avgFacErr() {
            return( accumulate( facErr.begin(), facErr.end(), 0.0 ) / facErr.size() );
        }
};

/// Clips a real number: if the absolute value of \a x is less than \a minabs, return \a minabs, else return \a x
Real clipReal( Real x, Real minabs ) {
    if( abs(x) < minabs )
        return minabs;
    else
        return x;
}

/// Which marginals to outpu (none, only variable, only factor, variable and factor, all)
DAI_ENUM(MarginalsOutputType,NONE,VAR,FAC,VARFAC,ALL);

/// Main function
int main( int argc, char *argv[] ) {
    // Variables to store command line options
    // Filename of factor graph
    string filename;
    // Filename for aliases
    string aliases;
    // Approximate Inference methods to use
    vector<string> methods;
    // Which marginals to output
    MarginalsOutputType marginals;
    // Output number of iterations?
    bool report_iters = true;
    // Output calculation time?
    bool report_time = true;

    // Define required command line options
    po::options_description opts_required("Required options");
    opts_required.add_options()
        ("filename", po::value< string >(&filename), "Filename of factor graph")
        ("methods", po::value< vector<string> >(&methods)->multitoken(), "DAI methods to perform")
    ;

    // Define allowed command line options
    po::options_description opts_optional("Allowed options");
    opts_optional.add_options()
        ("help", "Produce help message")
        ("aliases", po::value< string >(&aliases), "Filename for aliases")
        ("marginals", po::value< MarginalsOutputType >(&marginals), "Output marginals? (NONE/VAR/FAC/VARFAC/ALL, default=NONE)")
        ("report-time", po::value< bool >(&report_time), "Output calculation time (default==1)?")
        ("report-iters", po::value< bool >(&report_iters), "Output iterations needed (default==1)?")
    ;

    // Define all command line options
    po::options_description cmdline_options;
    cmdline_options.add(opts_required).add(opts_optional);

    // Parse command line
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
    po::notify(vm);

    // Display help message if necessary
    if( vm.count("help") || !(vm.count("filename") && vm.count("methods")) ) {
        cout << "This program is part of libDAI - http://www.libdai.org/" << endl << endl;
        cout << "Usage: ./testdai --filename <filename.fg> --methods <method1> [<method2> <method3> ...]" << endl << endl;
        cout << "Reads factor graph <filename.fg> and performs the approximate inference algorithms" << endl;
        cout << "<method*>, reporting for each method:" << endl;
        cout << "  o the calculation time needed, in seconds (if report-time == 1);" << endl;
        cout << "  o the number of iterations needed (if report-iters == 1);" << endl;
        cout << "  o the maximum (over all variables) total variation error in the variable marginals;" << endl;
        cout << "  o the average (over all variables) total variation error in the variable marginals;" << endl;
        cout << "  o the maximum (over all factors) total variation error in the factor marginals;" << endl;
        cout << "  o the average (over all factors) total variation error in the factor marginals;" << endl;
        cout << "  o the error (difference) of the logarithm of the partition sums;" << endl << endl;
        cout << "All errors are calculated by comparing the results of the current method with" << endl;
        cout << "the results of the first method (the base method). If marginals==VAR, additional" << endl;
        cout << "output consists of the variable marginals, if marginals==FAC, the factor marginals" << endl;
        cout << "if marginals==VARFAC, both variable and factor marginals, and if marginals==ALL, all" << endl;
        cout << "marginals calculated by the method are reported." << endl << endl;
        cout << "<method*> should be a list of one or more methods, seperated by spaces, in the format:" << endl << endl;
        cout << "    name[key1=val1,key2=val2,key3=val3,...,keyn=valn]" << endl << endl;
        cout << "where name should be the name of an algorithm in libDAI (or an alias, if an alias" << endl;
        cout << "filename is provided), followed by a list of properties (surrounded by rectangular" << endl;
        cout << "brackets), where each property consists of a key=value pair and the properties are" << endl;
        cout << "seperated by commas. If an alias file is specified, alias substitution is performed." << endl;
        cout << "This is done by looking up the name in the alias file and substituting the alias" << endl;
        cout << "by its corresponding method as defined in the alias file. Properties are parsed from" << endl;
        cout << "left to right, so if a property occurs repeatedly, the right-most value is used." << endl << endl;
        cout << opts_required << opts_optional << endl;
#ifdef DAI_DEBUG
        cout << "Note: this is a debugging build of libDAI." << endl << endl;
#endif
        cout << "Example:  ./testdai --filename testfast.fg --aliases aliases.conf --methods JTREE_HUGIN BP_SEQFIX BP_PARALL[maxiter=5]" << endl;
        return 1;
    }

    try {
        // Read aliases
        map<string,string> Aliases;
        if( !aliases.empty() )
            Aliases = readAliasesFile( aliases );

        // Read factor graph
        FactorGraph fg;
        fg.ReadFromFile( filename.c_str() );

        // Declare variables used for storing variable factor marginals and log partition sum of base method
        vector<Factor> varMarginals0;
        vector<Factor> facMarginals0;
        Real logZ0 = 0.0;

        // Output header
        cout.setf( ios_base::scientific );
        cout.precision( 3 );
        cout << "# " << filename << endl;
        cout.width( 39 );
        cout << left << "# METHOD" << "\t";
        if( report_time )
            cout << right << "SECONDS  " << "\t";
        if( report_iters )
            cout << "ITERS" << "\t";
        cout << "MAX VAR ERR" << "\t";
        cout << "AVG VAR ERR" << "\t";
        cout << "MAX FAC ERR" << "\t";
        cout << "AVG FAC ERR" << "\t";
        cout << "LOGZ ERROR" << "\t";
        cout << "MAXDIFF" << "\t";
        cout << endl;

        // For each method...
        for( size_t m = 0; m < methods.size(); m++ ) {
            // Parse method
            pair<string, PropertySet> meth = parseNameProperties( methods[m], Aliases );

            // Construct object for running the method
            TestDAI testdai(fg, meth.first, meth.second );

            // Run the method
            testdai.doDAI();

            // For the base method, store its variable marginals and logarithm of the partition sum
            if( m == 0 ) {
                varMarginals0 = testdai.varMarginals;
                facMarginals0 = testdai.facMarginals;
                logZ0 = testdai.logZ;
            }

            // Calculate errors relative to base method
            testdai.calcErrors( varMarginals0, facMarginals0 );

            // Output method name
            cout.width( 39 );
            cout << left << methods[m] << "\t";
            // Output calculation time, if requested
            if( report_time )
                cout << right << testdai.time << "\t";
            // Output number of iterations, if requested
            if( report_iters ) {
                if( testdai.has_iters ) {
                    cout << testdai.iters << "\t";
                } else {
                    cout << "N/A  \t";
                }
            }

            // If this is not the base method
            if( m > 0 ) {
                cout.setf( ios_base::scientific );
                cout.precision( 3 );

                // Output maximum error in variable marginals
                Real mev = clipReal( testdai.maxVarErr(), 1e-9 );
                cout << mev << "\t";

                // Output average error in variable marginals
                Real aev = clipReal( testdai.avgVarErr(), 1e-9 );
                cout << aev << "\t";

                // Output maximum error in factor marginals
                Real mef = clipReal( testdai.maxFacErr(), 1e-9 );
                if( mef == INFINITY )
                    cout << "N/A       \t";
                else
                    cout << mef << "\t";

                // Output average error in factor marginals
                Real aef = clipReal( testdai.avgFacErr(), 1e-9 );
                if( aef == INFINITY )
                    cout << "N/A       \t";
                else
                    cout << aef << "\t";

                // Output error in log partition sum
                if( testdai.has_logZ ) {
                    cout.setf( ios::showpos );
                    Real le = clipReal( testdai.logZ - logZ0, 1e-9 );
                    cout << le << "\t";
                    cout.unsetf( ios::showpos );
                } else
                    cout << "N/A       \t";

                // Output maximum difference in last iteration
                if( testdai.has_maxdiff ) {
                    Real md = clipReal( testdai.maxdiff, 1e-9 );
                    if( dai::isnan( mev ) )
                        md = mev;
                    if( dai::isnan( aev ) )
                        md = aev;
                    if( md == INFINITY )
                        md = 1.0;
                    cout << md << "\t";
                } else
                    cout << "N/A    \t";
            }
            cout << endl;

            // Output marginals, if requested
            if( marginals == MarginalsOutputType::VAR || marginals == MarginalsOutputType::VARFAC )
                for( size_t i = 0; i < testdai.varMarginals.size(); i++ )
                    cout << "# " << testdai.varMarginals[i] << endl;
            if( marginals == MarginalsOutputType::FAC || marginals == MarginalsOutputType::VARFAC )
                for( size_t I = 0; I < testdai.facMarginals.size(); I++ )
                    cout << "# " << testdai.facMarginals[I] << endl;
            if( marginals == MarginalsOutputType::ALL )
                for( size_t I = 0; I < testdai.allMarginals.size(); I++ )
                    cout << "# " << testdai.allMarginals[I] << endl;
        }

        return 0;
    } catch( string &s ) {
        // Abort with error message
        cerr << "Exception: " << s << endl;
        return 2;
    }
}


/*
 * File:   arma.hpp
 * Copyright (C) 2014  K M Masum Habib <masum.habib@gmail.com>
 *
 * Created on February 8, 2014, 8:12 PM
 */

#ifndef ARMA_HPP
#define	ARMA_HPP

#include <complex>
#include <armadillo>

namespace maths{
namespace armadillo{

/* Apparently Armadillo renamed uint to uword */
typedef unsigned int uint;  //! To cope with armadillo's change.

typedef std::complex<double>      dcmplx;   //!< Double precision complex
typedef std::complex<float>       fcmplx;   //!< Single precision complex

using arma::Mat;
using arma::Col;
using arma::Row;
using arma::span;

using arma::uword; // @TODO: Armadillo >5.000.1 Fix

using arma::field;
using arma::endr;
using arma::inv;
using arma::zeros;
using arma::ones;
using arma::linspace;
using arma::eye;
using arma::is_finite;
using arma::wall_clock;
namespace fill = arma::fill;

using arma::sum;
using arma::sqrt;
using arma::square;
using arma::abs;
using arma::cross;
using arma::conv_to;
using arma::normalise;

typedef arma::Col<double>         vec;       //!< Double precision column vector.
typedef arma::Col<dcmplx>         cxvec;     //!< Double precision complex column vector.
typedef arma::Col<float>          fvec;      //!< Double precision column vector.
typedef arma::Col<fcmplx>         cxfvec;    //!< Double precision complex column vector.
typedef arma::Col<int>            ivec;      //!< Signed integer column vector.
typedef arma::Col<uint>           uvec;      //!< Unsigned integer column vector.

typedef arma::Col<double>         col;       //!< Double precision column vector.
typedef arma::Col<dcmplx>         cxcol;     //!< Double precision complex column vector.
typedef arma::Col<float>          fcol;      //!< Double precision column vector.
typedef arma::Col<fcmplx>         cxfcol;    //!< Double precision complex column vector.
typedef arma::Col<int>            icol;      //!< Signed integer column vector.
typedef arma::Col<uint>           ucol;      //!< Unsigned integer column vector.

typedef arma::Col<uword>          uwcol;     //!< Unsigned long long int column vector. // @TODO: Armadillo >5.000.1 Fix

typedef arma::Row<double>         row;       //!< Double precision row vector.
typedef arma::Row<dcmplx>         cxrow;     //!< Double precision complex row vector.
typedef arma::Row<float>          frow;      //!< Double precision row vector.
typedef arma::Row<fcmplx>         cxfrow;    //!< Double precision complex row vector.
typedef arma::Row<int>            irow;      //!< Signed integer column vector.
typedef arma::Row<uint>           urow;      //!< Unsigned integer column vector.

typedef arma::Mat<double>         mat;       //!< Double precision matrix.
typedef arma::Mat<dcmplx>         cxmat;     //!< Double precision complex matrix.
typedef arma::Mat<float>          fmat;      //!< Double precision matrix.
typedef arma::Mat<fcmplx>         cxfmat;    //!< Double precision complex matrix.
typedef arma::Mat<int>            imat;      //!< Signed integer matrix.
typedef arma::Mat<uint>           umat;      //!< Unsigned integer matrix.

typedef arma::Cube<double>         cube;       //!< Double precision matrix.
typedef arma::Cube<dcmplx>         cxcube;     //!< Double precision complex matrix.
typedef arma::Cube<float>          fcube;      //!< Double precision matrix.
typedef arma::Cube<fcmplx>         cxfcube;    //!< Double precision complex matrix.
typedef arma::Cube<int>            icube;      //!< Signed integer matrix.
typedef arma::Cube<uint>           ucube;      //!< Unsigned integer matrix.

typedef arma::Mat<double>::fixed<2,2>mat22;     //!< 2x2 Double precision matrix.
typedef cxmat::fixed<2,2>          cxmat22;   //!< 2x2 Double precision complex matrix.
typedef fmat::fixed<2,2>           fmat22;    //!< 2x2 Double precision matrix.
typedef cxfmat::fixed<2,2>         cxfmat22;  //!< 2x2 Double precision complex matrix.
typedef imat::fixed<2,2>           imat22;    //!< 2x2 Signed integer matrix.
typedef umat::fixed<2,2>           umat22;    //!< 2x2 Unsigned integer matrix.

typedef row::fixed<2>              row2;      //!< 2 element double precision row vector.
typedef row::fixed<3>              row3;      //!< 3 element double precision row vector.

}
}

#endif	/* ARMA_HPP */


#include <shift/math/quaternion.hpp>
#include <shift/math/utility.hpp>
#include <shift/core/boost_disable_warnings.hpp>
#include <boost/test/unit_test.hpp>
#include <shift/core/boost_restore_warnings.hpp>

using namespace shift::math;

BOOST_AUTO_TEST_CASE(quaternion_is_trivial)
{
  static_assert(std::is_trivial_v<quaternion<float>>);
  BOOST_CHECK(std::is_trivial_v<quaternion<float>>);
}

BOOST_AUTO_TEST_CASE(quaternion_multiplication)
{
  const quaternion<float> q1{0.0f, 0.0f, 0.0f, 1.0f};
  const quaternion<float> qi{1.0f, 0.0f, 0.0f, 0.0f};
  const quaternion<float> qj{0.0f, 1.0f, 0.0f, 0.0f};
  const quaternion<float> qk{0.0f, 0.0f, 1.0f, 0.0f};
  BOOST_CHECK_EQUAL(q1 * q1, q1);
  BOOST_CHECK_EQUAL(q1 * qi, qi);
  BOOST_CHECK_EQUAL(q1 * qj, qj);
  BOOST_CHECK_EQUAL(q1 * qk, qk);
  BOOST_CHECK_EQUAL(qi * q1, qi);
  BOOST_CHECK_EQUAL(qi * qi, -q1);
  BOOST_CHECK_EQUAL(qi * qj, qk);
  BOOST_CHECK_EQUAL(qi * qk, -qj);
  BOOST_CHECK_EQUAL(qj * q1, qj);
  BOOST_CHECK_EQUAL(qj * qi, -qk);
  BOOST_CHECK_EQUAL(qj * qj, -q1);
  BOOST_CHECK_EQUAL(qj * qk, qi);
  BOOST_CHECK_EQUAL(qk * q1, qk);
  BOOST_CHECK_EQUAL(qk * qi, qj);
  BOOST_CHECK_EQUAL(qk * qj, -qi);
  BOOST_CHECK_EQUAL(qk * qk, -q1);
}

BOOST_AUTO_TEST_CASE(quaternion_conjugate)
{
  const quaternion<float> q1{1.0f, 2.0f, 3.0f, 4.0f};
  const quaternion<float> q2{2.0f, 3.0f, 2.0f, 1.0f};
  BOOST_CHECK_EQUAL(conjugate(q1 * q2), conjugate(q2) * conjugate(q1));
  // Check whether conjugate is an involutory function.
  BOOST_CHECK_EQUAL(conjugate(conjugate(q1)), q1);
}

BOOST_AUTO_TEST_CASE(quaternion_rotation_r3)
{
  using namespace shift::math::literals;

  auto x = make_vector_from(1.0f, 0.0f, 0.0f);
  auto y = make_vector_from(0.0f, 1.0f, 0.0f);
  auto z = make_vector_from(0.0f, 0.0f, 1.0f);
  auto r_x = make_quaternion_from_axis_angle(x, 90_fdeg);
  auto r_y = make_quaternion_from_axis_angle(y, 90_fdeg);
  auto r_z = make_quaternion_from_axis_angle(z, 90_fdeg);

  BOOST_CHECK(almost_equal(rotate(x, r_x), x));
  BOOST_CHECK(almost_equal(rotate(y, r_x), z));
  BOOST_CHECK(almost_equal(rotate(z, r_x), -y));
  BOOST_CHECK(almost_equal(rotate(x, r_y), -z));
  BOOST_CHECK(almost_equal(rotate(y, r_y), y));
  BOOST_CHECK(almost_equal(rotate(z, r_y), x));
  BOOST_CHECK(almost_equal(rotate(x, r_z), y));
  BOOST_CHECK(almost_equal(rotate(y, r_z), -x));
  BOOST_CHECK(almost_equal(rotate(z, r_z), z));
}

BOOST_AUTO_TEST_CASE(quaternion_rotation_r4)
{
  using namespace shift::math::literals;

  auto x = make_vector_from(1.0f, 0.0f, 0.0f);
  auto y = make_vector_from(0.0f, 1.0f, 0.0f);
  auto z = make_vector_from(0.0f, 0.0f, 1.0f);
  auto p_x = make_vector_from(+1.0f, 0.0f, 0.0f, 1.0f);
  auto p_y = make_vector_from(0.0f, +1.0f, 0.0f, 1.0f);
  auto p_z = make_vector_from(0.0f, 0.0f, +1.0f, 1.0f);
  auto n_x = make_vector_from(-1.0f, 0.0f, 0.0f, 1.0f);
  auto n_y = make_vector_from(0.0f, -1.0f, 0.0f, 1.0f);
  auto n_z = make_vector_from(0.0f, 0.0f, -1.0f, 1.0f);
  auto r_x = make_quaternion_from_axis_angle(x, 90_fdeg);
  auto r_y = make_quaternion_from_axis_angle(y, 90_fdeg);
  auto r_z = make_quaternion_from_axis_angle(z, 90_fdeg);

  BOOST_CHECK(almost_equal(rotate(p_x, r_x), p_x));
  BOOST_CHECK(almost_equal(rotate(p_y, r_x), p_z));
  BOOST_CHECK(almost_equal(rotate(p_z, r_x), n_y));
  BOOST_CHECK(almost_equal(rotate(p_x, r_y), n_z));
  BOOST_CHECK(almost_equal(rotate(p_y, r_y), p_y));
  BOOST_CHECK(almost_equal(rotate(p_z, r_y), p_x));
  BOOST_CHECK(almost_equal(rotate(p_x, r_z), p_y));
  BOOST_CHECK(almost_equal(rotate(p_y, r_z), n_x));
  BOOST_CHECK(almost_equal(rotate(p_z, r_z), p_z));
}


#ifndef FSTCRITERIONMULTINOMBHATTACHARYYA_H
#define FSTCRITERIONMULTINOMBHATTACHARYYA_H

/*!======================================================================
   Feature Selection Toolbox 3 source code
   ---------------------------------------

   \file    criterion_multinom_bhattacharyya.hpp
   \brief   Implements Bhattacharyya distance based on multinomial model to serve as feature selection criterion
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see Contacts at http://fst.utia.cz
   \date    March 2011
   \version 3.1.0.beta
   \note    FST3 was developed using gcc 4.3 and requires
   \note    \li Boost library (http://www.boost.org/, tested with versions 1.33.1 and 1.44),
   \note    \li (\e optionally) LibSVM (http://www.csie.ntu.edu.tw/~cjlin/libsvm/,
                tested with version 3.00)
   \note    Note that LibSVM is required for SVM related tools only,
            as demonstrated in demo12t.cpp, demo23.cpp, demo25t.cpp, demo32t.cpp, etc.

*/ /*
=========================================================================
Copyright:
  * FST3 software (with exception of any externally linked libraries)
    is copyrighted by Institute of Information Theory and Automation (UTIA),
    Academy of Sciences of the Czech Republic.
  * FST3 source codes as presented here do not contain code of third parties.
    FST3 may need linkage to external libraries to exploit its functionality
    in full. For details on obtaining and possible usage restrictions
    of external libraries follow their original sources (referenced from
    FST3 documentation wherever applicable).
  * FST3 software is available free of charge for non-commercial use.
    Please address all inquires concerning possible commercial use
    of FST3, or if in doubt, to FST3 maintainer (see http://fst.utia.cz)
  * Derivative works based on FST3 are permitted as long as they remain
    non-commercial only.
  * Re-distribution of FST3 software is not allowed without explicit
    consent of the copyright holder.
Disclaimer of Warranty:
  * FST3 software is presented "as is", without warranty of any kind,
    either expressed or implied, including, but not limited to, the implied
    warranties of merchantability and fitness for a particular purpose.
    The entire risk as to the quality and performance of the program
    is with you. Should the program prove defective, you assume the cost
    of all necessary servicing, repair or correction.
Limitation of Liability:
  * The copyright holder will in no event be liable to you for damages,
    including any general, special, incidental or consequential damages
    arising out of the use or inability to use the code (including but not
    limited to loss of data or data being rendered inaccurate or losses
    sustained by you or third parties or a failure of the program to operate
    with any other programs).
========================================================================== */

#include <boost/smart_ptr.hpp>
#include <iostream>
#include <sstream>
#include <cmath>
#include "error.hpp"
#include "global.hpp"
#include "criterion_multinom.hpp"
#include "model_multinom.hpp"

/*============== Template parameter type naming conventions ==============
--------- Numeric types: -------------------------------------------------
DATATYPE - data sample values - usually real numbers (but may be integers
          in text processing etc.)
REALTYPE - must be real numbers - for representing intermediate results of
          calculations like mean, covariance etc.
IDXTYPE - index values for enumeration of data samples - (nonnegative) integers,
          extent depends on numbers of samples in data
DIMTYPE - index values for enumeration of features (dimensions), or classes (not
          class sizes) - (nonnegative) integers, usually lower extent than IDXTYPE,
          but be aware of expressions like _classes*_features*_features !
          in linearized representations of feature matrices for all classes
BINTYPE - feature selection marker type - represents ca. <10 different feature
          states (selected, deselected, sel./desel. temporarily 1st nested loop, 2nd...)
RETURNTYPE - criterion value: real value, but may be extended in future to support
          multiple values
--------- Class types: ---------------------------------------------------
SUBSET       - class of class type Subset
CLASSIFIER   - class implementing interface defined in abstract class Classifier
EVALUATOR    - class implementing interface defined in abstract class Sequential_Step
DISTANCE     - class implementing interface defined in abstract class Distance
DATAACCESSOR - class implementing interface defined in abstract class Data_Accessor
INTERVALCONTAINER - class of class type TIntervaller
CONTAINER    - STL container of class type TInterval
========================================================================== */

namespace FST {

//! Implements Bhattacharyya distance based on multinomial model to serve as feature selection criterion
template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
class Criterion_Multinomial_Bhattacharyya : public Criterion_Multinomial<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> {
public:
	typedef Criterion_Multinomial<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> parent;
	typedef boost::shared_ptr<DATAACCESSOR> PDataAccessor;
	typedef boost::shared_ptr<SUBSET> PSubset;
	Criterion_Multinomial_Bhattacharyya() {_IB_computed=false; notify("Criterion_Multinomial_Bhattacharyya constructor.");}
	virtual ~Criterion_Multinomial_Bhattacharyya() {notify("Criterion_Multinomial_Bhattacharyya destructor.");}

	virtual bool evaluate(RETURNTYPE &result, const PSubset sub);
	virtual bool initialize(PDataAccessor da);

	Criterion_Multinomial_Bhattacharyya* clone() const;
	Criterion_Multinomial_Bhattacharyya* sharing_clone() const {throw fst_error("Criterion_Multinomial_Bhattacharyya::sharing_clone() not supported, use Criterion_Multinomial_Bhattacharyya::clone() instead.");}
	Criterion_Multinomial_Bhattacharyya* stateless_clone() const {throw fst_error("Criterion_Multinomial_Bhattacharyya::stateless_clone() not supported, use Criterion_Multinomial_Bhattacharyya::clone() instead.");}

	virtual std::ostream& print(std::ostream& os) const {os << "Criterion_Multinomial_Bhattacharyya()"; return os;}
private:
	Criterion_Multinomial_Bhattacharyya(const Criterion_Multinomial_Bhattacharyya& cmb); // copy-constructor
private:
	bool _IB_computed;
};

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Criterion_Multinomial_Bhattacharyya<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::Criterion_Multinomial_Bhattacharyya(const Criterion_Multinomial_Bhattacharyya& cmb) :
	Criterion_Multinomial<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>(cmb),
	_IB_computed(cmb._IB_computed)
{
	notify("Criterion_Multinomial_Bhattacharyya copy-constructor.");
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Criterion_Multinomial_Bhattacharyya<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>* Criterion_Multinomial_Bhattacharyya<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::clone() const
{
	Criterion_Multinomial_Bhattacharyya<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> *clone=new Criterion_Multinomial_Bhattacharyya<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>(*this);
	clone->set_cloned();
	return clone;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Criterion_Multinomial_Bhattacharyya<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::evaluate(RETURNTYPE &result, PSubset sub)
{
	notify("Criterion_Multinomial_Bhattacharyya::evaluate().");
	assert(parent::_model);
	assert(parent::get_n()>0);
	assert(sub);
	if(sub->get_d_raw()==0) return false;
	DIMTYPE _d = sub->get_d_raw();
	DIMTYPE f;

	if(_d==1) {// general form of Bhattacharyya unusable, return Indivdual Bhattacharyya
		if(!_IB_computed) {
			parent::_model->denarrow();
			parent::_model->compute_theta();
			parent::_model->compute_IB();
			_IB_computed=true;
		}
		if(!sub->getFirstFeature(f)) return false; assert(f>=0 && f<parent::_model->get_n());
		result = parent::_model->get_IB()[f]; // individual Bhatt - higher values denote better terms
		bool b=sub->getNextFeature(f); // just to finish the loop inside sub (to prevent future asserts in sub->set_forward_mode etc.)
		assert(b==false);
	} else {
		parent::_model->narrow_to(sub);
		parent::_model->compute_theta();
		REALTYPE doc_avg_length = parent::_model->get_doc_avg_length(); // valid after compute_theta() call
		DIMTYPE _classes=parent::_model->get_classes();

		REALTYPE *tmpoint1, *tmpoint2;
		RETURNTYPE thetasum;
		RETURNTYPE value=0.0;
		DIMTYPE c1,c2;
#ifdef DEBUG
		{
			ostringstream sos; sos << "d="<<_d<<", doc_avg_length="<<doc_avg_length<<std::endl;
			syncout::print(std::cout,sos);
		}
#endif
		for(c1=0;c1<_classes;c1++) for(c2=c1+1;c2<_classes;c2++) // for class pair c1,c2
		{
			tmpoint1=&(parent::_model->get_theta()[c1*_d]);
			tmpoint2=&(parent::_model->get_theta()[c2*_d]);
			thetasum=0.0;
			for(f=0;f<_d;f++)
			{
				thetasum+=sqrt(tmpoint1[f]*tmpoint2[f]);
#ifdef DEBUG
				{
					ostringstream sos; sos << "classes<"<<c1<<","<<c2<<">: feat "<<f<<".thetasum="<<thetasum<<", tmpoint1[i="<<f<<"]="<< tmpoint1[f]<< ", tmpoint2[i="<<f<<"]="<< tmpoint2[f]<<std::endl;
					syncout::print(std::cout,sos);
				}
#endif
			}
			value+= (double)log(thetasum) * parent::_model->get_Pc(c1) * parent::_model->get_Pc(c2); // weighted
		}
		result = (-doc_avg_length)*value;
#ifdef DEBUG
		{
			ostringstream sos; sos << "result="<<result<<std::endl<<std::endl;
			syncout::print(std::cout,sos);
		}
#endif
	}
	return true;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Criterion_Multinomial_Bhattacharyya<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::initialize(PDataAccessor da)
{
	notify("Criterion_Multinomial_Bhattacharyya::initialize().");
	parent::initialize(da);
	_IB_computed=false;
	return true;
}

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

/*! \brief Implements individual Mutual Information based on multinomial model to serve as feature selection criterion in Best Individual Feature setting (feature ranking) only
    \note Can be used to evaluate single features only ! */
template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
class Criterion_Multinomial_MI : public Criterion_Multinomial<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> {
public:
	typedef Criterion_Multinomial<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> parent;
	typedef boost::shared_ptr<DATAACCESSOR> PDataAccessor;
	typedef boost::shared_ptr<SUBSET> PSubset;
	Criterion_Multinomial_MI() {_MI_computed=false; notify("Criterion_Multinomial_MI constructor.");}
	virtual ~Criterion_Multinomial_MI() {notify("Criterion_Multinomial_MI destructor.");}

	virtual bool evaluate(RETURNTYPE &result, const PSubset sub);
	virtual bool initialize(PDataAccessor da);
private:
	bool _MI_computed;
};

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Criterion_Multinomial_MI<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::evaluate(RETURNTYPE &result, PSubset sub)
{
	notify("Criterion_Multinomial_MI::evaluate().");
	assert(parent::_model);
	assert(parent::get_n()>0);
	assert(sub);
	if(sub->get_d_raw()==0) return false;
	DIMTYPE _d = sub->get_d_raw();
	DIMTYPE f;

	assert(_d==1);

	if(!_MI_computed) {
		parent::_model->denarrow();
		parent::_model->compute_theta();
		parent::_model->compute_MI();
		_MI_computed=true;
	}
	if(!sub->getFirstFeature(f)) return false; assert(f>=0 && f<parent::_model->get_n());
	result = parent::_model->get_MI()[f]; // individual Bhatt - higher values denote better terms
	bool b=sub->getNextFeature(f); // just to finish the loop inside sub (to prevent future asserts in sub->set_forward_mode etc.)
	assert(b==false);
	return true;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Criterion_Multinomial_MI<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::initialize(PDataAccessor da)
{
	notify("Criterion_Multinomial_MI::initialize().");
	parent::initialize(da);
	_MI_computed=false;
	return true;
}

} // namespace
#endif // FSTCRITERIONMULTINOMBHATTACHARYYA_H ///:~


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/list/mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/minimal/comparable.hpp>
#include <boost/hana/detail/minimal/list.hpp>
using namespace boost::hana;

template <int i>
constexpr auto x = detail::minimal::comparable<>(i);

template <typename mcd>
void test() {
    BOOST_HANA_CONSTEXPR_LAMBDA auto list = detail::minimal::list<mcd>;

    BOOST_HANA_CONSTEXPR_LAMBDA auto permute = [=](auto xs) {
        return [=](auto ...expected_) {
            auto actual = permutations(xs);
            auto expected = list(expected_...);

            BOOST_HANA_CONSTEXPR_ASSERT(
                length(expected) == length(actual) &&
                all(actual, [=](auto x) { return elem(expected, x); })
            );
        };
    };

    BOOST_HANA_CONSTANT_ASSERT(permutations(list()) == list(list()));

    permute(list(x<0>))(list(x<0>));
    permute(list(x<0>, x<1>))(
        list(x<0>, x<1>),
        list(x<1>, x<0>)
    );
    permute(list(x<0>, x<1>, x<2>))(
        list(x<0>, x<1>, x<2>),
        list(x<0>, x<2>, x<1>),
        list(x<1>, x<0>, x<2>),
        list(x<1>, x<2>, x<0>),
        list(x<2>, x<0>, x<1>),
        list(x<2>, x<1>, x<0>)
    );
}

int main() {
    test<List::mcd<void>>();
}


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

#define BOOST_TEST_MODULE test module name
#include <boost/test/included/unit_test.hpp>

#include <cstdint>

#include "range.hpp"
#include "qformat.hpp"
#include "constants.hpp"
#include "activationFunctions.hpp"

typedef tinymind::QValue<21, 11, true, tinymind::RoundUpPolicy> QValueType;

BOOST_AUTO_TEST_SUITE(test_suite_activation_value_conversion)

BOOST_AUTO_TEST_CASE(test_case_activation_value_input_conversion_0_100)
{
    typedef tinymind::RangeConverter<QValueType> RangeConverterType;
    static const QValueType minInput(0);
    static const QValueType maxInput(100, 0);
    static const QValueType minOutput(0);
    static const QValueType maxOutput(10, 0);
    static const QValueType oneHalf(0,(1 << (QValueType::NumberOfFractionalBits - 1)));
    QValueType input(minInput);

    RangeConverterType::convertToFractionOfRange(input, minInput, maxInput);
    BOOST_TEST(tinymind::Constants<QValueType>::zero() == input);

    input = maxInput;
    RangeConverterType::convertToFractionOfRange(input, minInput, maxInput);
    BOOST_TEST(tinymind::Constants<QValueType>::one() == input);

    input = QValueType(50,0);
    RangeConverterType::convertToFractionOfRange(input, minInput, maxInput);
    BOOST_TEST(oneHalf == input);
}

BOOST_AUTO_TEST_CASE(test_case_activation_value_input_conversion_1_100)
{
    typedef tinymind::RangeConverter<QValueType> RangeConverterType;
    static const QValueType minInput(1,0);
    static const QValueType maxInput(100, 0);
    QValueType input(minInput);

    RangeConverterType::convertToFractionOfRange(input, minInput, maxInput);
    BOOST_TEST(tinymind::Constants<QValueType>::zero() == input);

    input = maxInput;
    RangeConverterType::convertToFractionOfRange(input, minInput, maxInput);
    BOOST_TEST(tinymind::Constants<QValueType>::one() == input);

    input = QValueType(50,0);
    RangeConverterType::convertToFractionOfRange(input, minInput, maxInput);
    BOOST_TEST(1013 == input.getValue()); // ~ 0.494949495 * 2^11
}

BOOST_AUTO_TEST_CASE(test_case_activation_value_output_sigmoid_conversion_0_10)
{
    typedef tinymind::ActivationOutputRangeConverter<QValueType, QValueType, tinymind::SigmoidActivationPolicy<QValueType> > ActivationOutputRangeConverterSigmoidType;
    static const QValueType minOutput(0);
    static const QValueType maxOutput(10, 0);
    static const QValueType oneHalf(0,(1 << (QValueType::NumberOfFractionalBits - 1)));
    static const QValueType midPoint = (maxOutput / 2);
    QValueType output(minOutput);

    ActivationOutputRangeConverterSigmoidType::convertToOutputValue(output, minOutput, maxOutput);
    BOOST_TEST(tinymind::Constants<QValueType>::zero() == output);

    output = oneHalf;
    ActivationOutputRangeConverterSigmoidType::convertToOutputValue(output, minOutput, maxOutput);
    BOOST_TEST(midPoint == output);

    output = tinymind::Constants<QValueType>::one();
    ActivationOutputRangeConverterSigmoidType::convertToOutputValue(output, minOutput, maxOutput);
    BOOST_TEST(maxOutput == output);
}

BOOST_AUTO_TEST_CASE(test_case_activation_value_output_tanh_conversion_0_10)
{
    typedef tinymind::ActivationOutputRangeConverter<QValueType, QValueType, tinymind::TanhActivationPolicy<QValueType> > ActivationOutputRangeConverterTanhType;
    static const QValueType minOutput(0);
    static const QValueType maxOutput(10, 0);
    static const QValueType oneHalf(0,(1 << (QValueType::NumberOfFractionalBits - 1)));
    static const QValueType midPoint = (maxOutput / 2);
    QValueType output(tinymind::Constants<QValueType>::negativeOne());

    ActivationOutputRangeConverterTanhType::convertToOutputValue(output, minOutput, maxOutput);
    BOOST_TEST(tinymind::Constants<QValueType>::zero() == output);

    output = tinymind::Constants<QValueType>::zero();
    ActivationOutputRangeConverterTanhType::convertToOutputValue(output, minOutput, maxOutput);
    BOOST_TEST(midPoint == output);

    output = tinymind::Constants<QValueType>::one();
    ActivationOutputRangeConverterTanhType::convertToOutputValue(output, minOutput, maxOutput);
    BOOST_TEST(maxOutput == output);
}

BOOST_AUTO_TEST_CASE(test_case_activation_value_output_sigmoid_conversion_2_10)
{
    typedef tinymind::ActivationOutputRangeConverter<QValueType, QValueType, tinymind::SigmoidActivationPolicy<QValueType> > ActivationOutputRangeConverterSigmoidType;
    static const QValueType minOutput(2,0);
    static const QValueType maxOutput(10, 0);
    static const QValueType oneFourth = (tinymind::Constants<QValueType>::one() / QValueType(4, 0));
    QValueType output(0);

    ActivationOutputRangeConverterSigmoidType::convertToOutputValue(output, minOutput, maxOutput);
    BOOST_TEST(minOutput == output);

    output = oneFourth;
    ActivationOutputRangeConverterSigmoidType::convertToOutputValue(output, minOutput, maxOutput);
    BOOST_TEST(QValueType(4, 0) == output);

    output = tinymind::Constants<QValueType>::one();
    ActivationOutputRangeConverterSigmoidType::convertToOutputValue(output, minOutput, maxOutput);
    BOOST_TEST(maxOutput == output);
}

BOOST_AUTO_TEST_CASE(test_case_activation_value_output_tanh_conversion_2_10)
{
    typedef tinymind::ActivationOutputRangeConverter<QValueType, QValueType, tinymind::TanhActivationPolicy<QValueType> > ActivationOutputRangeConverterTanhType;
    static const QValueType minOutput(2,0);
    static const QValueType maxOutput(10, 0);
    static const QValueType negativeOneHalf = (tinymind::Constants<QValueType>::negativeOne() * (tinymind::Constants<QValueType>::one() / QValueType(2, 0)));
    static const QValueType six(6, 0);
    QValueType output(tinymind::Constants<QValueType>::negativeOne());

    ActivationOutputRangeConverterTanhType::convertToOutputValue(output, minOutput, maxOutput);
    BOOST_TEST(minOutput == output);

    output = negativeOneHalf;
    ActivationOutputRangeConverterTanhType::convertToOutputValue(output, minOutput, maxOutput);
    BOOST_TEST(QValueType(4, 0) == output);

    output = tinymind::Constants<QValueType>::zero();
    ActivationOutputRangeConverterTanhType::convertToOutputValue(output, minOutput, maxOutput);
    BOOST_TEST(six == output);

    output = tinymind::Constants<QValueType>::one();
    ActivationOutputRangeConverterTanhType::convertToOutputValue(output, minOutput, maxOutput);
    BOOST_TEST(maxOutput == output);
}

BOOST_AUTO_TEST_CASE(test_case_activation_value_input_conversion_range_equals_0)
{
    typedef tinymind::RangeConverter<QValueType> RangeConverterType;
    static const QValueType minInput(100, 0);
    static const QValueType maxInput(100, 0);
    QValueType input(minInput);

    RangeConverterType::convertToFractionOfRange(input, minInput, maxInput);
    BOOST_TEST(minInput == input);
}

BOOST_AUTO_TEST_CASE(test_case_activation_value_input_conversion_range_invalid_min_max)
{
    typedef tinymind::RangeConverter<QValueType> RangeConverterType;
    static const QValueType minInput(100, 0);
    static const QValueType maxInput(101, 0);
    QValueType input(minInput);

    RangeConverterType::convertToFractionOfRange(input, maxInput, minInput);
    BOOST_TEST(minInput == input);
}

BOOST_AUTO_TEST_SUITE_END()


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Ion Gaztanaga 2015-2016.
// 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)
//
// See http://www.boost.org/libs/move for documentation.
//
//////////////////////////////////////////////////////////////////////////////

//! \file

#ifndef BOOST_MOVE_ALGO_BUFFERLESS_MERGE_SORT_HPP
#define BOOST_MOVE_ALGO_BUFFERLESS_MERGE_SORT_HPP

#ifndef BOOST_CONFIG_HPP
#  include <boost/config.hpp>
#endif
#
#if defined(BOOST_HAS_PRAGMA_ONCE)
#  pragma once
#endif

#include <boost/move/detail/config_begin.hpp>
#include <boost/move/detail/workaround.hpp>

#include <boost/move/utility_core.hpp>
#include <boost/move/adl_move_swap.hpp>

#include <boost/move/algo/move.hpp>
#include <boost/move/algo/detail/merge.hpp>

#include <boost/move/detail/iterator_traits.hpp>
#include <boost/move/algo/detail/insertion_sort.hpp>
#include <cassert>

namespace boost {
namespace movelib {
// @cond
namespace detail_bufferless_mergesort {

static const std::size_t UnbufferedMergeSortInsertionSortThreshold = 16;

//A in-placed version based on:
//Jyrki Katajainen, Tomi Pasanen, Jukka Teuhola.
//``Practical in-place mergesort''. Nordic Journal of Computing, 1996.

template<class RandIt, class Compare>
void bufferless_merge_sort(RandIt first, RandIt last, Compare comp);

template<class RandIt, class Compare>
void swap_sort(RandIt const first, RandIt const last, RandIt const buffer_first, RandIt const buffer_last, Compare comp, bool buffer_at_right)
{
   typedef typename iterator_traits<RandIt>::size_type size_type;
   if (size_type(last - first) > UnbufferedMergeSortInsertionSortThreshold) {
      RandIt m = first + (last - first) / 2;
      bufferless_merge_sort(first, m, comp);
      bufferless_merge_sort(m, last, comp);
      if(buffer_at_right){
         //Use antistable to minimize movements (if equal, move first half elements
         //to maximize the chance last half elements are already in place.
         boost::movelib::swap_merge_right(first, m, last, buffer_last, boost::movelib::antistable<Compare>(comp));
      }
      else{
         boost::movelib::swap_merge_left(buffer_first, first, m, last, comp);
      }
   }
   else
      boost::movelib::insertion_sort_swap(first, last, buffer_first, comp);
}

template<class RandIt, class Compare>
void bufferless_merge_sort(RandIt const first, RandIt const last, Compare comp)
{
   typedef typename iterator_traits<RandIt>::size_type size_type;
   size_type len = size_type(last - first);
   if (len > size_type(UnbufferedMergeSortInsertionSortThreshold)) {
      len /= 2;
      RandIt h = last - len;  //ceil(half)
      RandIt f = h - len;     //ceil(first)
      swap_sort(f, h, h, last, comp, true);     //[h, last) contains sorted elements

      //Divide unsorted first half in two
      len = size_type(h - first);
      while (len > size_type(UnbufferedMergeSortInsertionSortThreshold)) {
         len /= 2;
         RandIt n = h;  //new end
         h = n - len;   //ceil(half')
         f = h - len;   //ceil(first')
         swap_sort(h, n, f, h, comp, false); // the first half of the previous working area [f, h)
                                             //contains sorted elements: working area in the middle [h, n)
         //Now merge small (left) sorted with big (right) sorted (buffer is between them)
         swap_merge_with_right_placed(f, h, h, n, last, comp);
      }

      boost::movelib::insertion_sort(first, h, comp);
      boost::movelib::merge_bufferless(first, h, last, comp);
   }
   else{
      boost::movelib::insertion_sort(first, last, comp);
   }
}

}  //namespace detail_bufferless_mergesort {

// @endcond

//Unstable bufferless merge sort
template<class RandIt, class Compare>
void bufferless_merge_sort(RandIt first, RandIt last, Compare comp)
{
   detail_bufferless_mergesort::bufferless_merge_sort(first, last, comp);
}

}}   //namespace boost::movelib

#include <boost/move/detail/config_end.hpp>

#endif //#ifndef BOOST_MOVE_ALGO_BUFFERLESS_MERGE_SORT_HPP


/* Copyright (C) 2017 IBM Corp.
 *  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.
 */

/* TDMatrixParams.cpp - Parameters for trapdoor sampling
 */
#include <stdexcept>
#include <NTL/vector.h>
#include "utils/tools.h"
#include "utils/timing.h"
#include "TDMatrixParams.h"
#include <stdexcept>
#include <sys/stat.h>
#include <unistd.h>

//#define DEBUGPRINT
//#define DEBUG

NTL_CLIENT

// A static table of small co-prime factors (6-7.5 bits)
long TDMatrixParams::smallFactors[TDMATRIX_NUM_SMALL_FACTORS]
    = { TDMATRIX_SMALL_FACTORS }; // TDMATRIX_SMALL_FACTORS list in TDMatrix.h

// Calculate the sigmaX value. This value must be large enough so
// that sigmaX*I - r*maxFactor * (R/I)*(R^t|I) is positive definite.
// We compute an upper bound s on the singular values of R, then set
// sigmaX = r*maxFactor * s^2.
long getsigmaXVal(long wLen, long mBar, long maxFactor, long r)
{
  double s = r*(sqrt(wLen)+sqrt(mBar)+6);
  double sigmaZ = r*maxFactor;
  long val = ceil(sigmaZ*sigmaZ * s*s);

#ifdef DEBUG
  cout << "sigmaX val=" << val << endl;
#endif // DEBUG

  return val;
}

// lower bound the dimension to get sec bits of security
long mBound(long qBits, long errBits, long sec)
{
  return ceil((qBits-errBits)*(sec+110)/7.2);
}

long mbarBound(long qBits, long n, long sec)
{
  return ceil((2+sqrt(sec))*sqrt(n*qBits));
}

long get_qBits(long kFactors, long e)
{
  assert(kFactors>0 && e>0);
  if (kFactors > TDMATRIX_NUM_SMALL_FACTORS)
    kFactors = TDMATRIX_NUM_SMALL_FACTORS;

  double logProd = log(TDMatrixParams::smallFactors[0]);
  for (long i=1; i<kFactors; i++)
    logProd += log(TDMatrixParams::smallFactors[i]);

  return ceil(logProd*e/log(2.0));
}

// For each factor f_i, initialize zz_p context for F_i=f_i^e, and also
// compute f_i^{-1} mod F_j for all i<j. Returns max the largest factor
static long setFactors(mat_zz_p& fInv, Vec<zz_pContext>& zzp_context,
                       const vec_l factors, long e)
{
    FHE_TIMER_START;
    long kFactors = factors.length();

    // For each factor f_i, initialize zz_p context for F_i=f_i^e,
    // and also compute f_i^{-1} mod F_j for all i<j
    zzp_context.SetLength(kFactors);
    fInv.SetDims(kFactors,kFactors);

    NTL::zz_pPush ppush; // backup NTL's current modulus
    long maxFactor = 0;
    for (long j=kFactors-1; j>=0; j--)
    {
        if (factors[j]>maxFactor) maxFactor = factors[j];

        long f2e = NTL::power_long(factors[j],e);
        // FIXME: check that this fits in a single-precision integer
        NTL::zz_p::init(f2e);
        zzp_context[j].save(); // save the current zz_p::modulus()
        for (long i=j-1; i>=0; i--)
        {
            fInv[i][j] = NTL::inv(conv<zz_p>(factors[i])); // f_i^{-1} mod F_j
        }
    }
    return maxFactor;
}

// Initialize the parameters for a modulus q with kk factors,
// each factor is an e-th power of an even smaller number.
void TDMatrixParams::init(long nn, long kk, long ee, long mm, long rr)
{
    FHE_TIMER_START;

    assert(kk <= TDMATRIX_NUM_SMALL_FACTORS);
    factors.SetLength(kk);
    for (long i=0; i<kk; i++)
      factors[i] = smallFactors[i];
    kFactors = kk;

    e = ee;
    r = rr;
    n = nn;
    wLen = nn*kk*ee;

    if (mm > 0) { // caller supplied a value for m
      m = mm;
      mBar = m - wLen;
      if (mBar < nn) { // check that mBar is not too tiny
        mBar = nn;
	m = mBar + wLen;
      }
    }
    else { // compute m, mBar using security formulas
      long qBits = get_qBits(kk,ee);
      m = mBound(qBits, /*errBits=*/7, /*sec=*/80);
      mBar = mbarBound(qBits, n, /*sec=*/80);
      if (m < mBar+wLen) m = mBar+wLen;
      else               mBar = m - wLen;
    }

    // For each factor f_i, initialize zz_p context for F_i=f_i^e,
    // and also compute f_i^{-1} mod F_j for all i<j
    maxFactor = setFactors(fInv, zzp_context, factors, e);

    sigmaX = getsigmaXVal(wLen, mBar, maxFactor, r);//ceil(2*r*r*r*m*maxFactor);
    // The Gaussian parameter from which we can sample with a trapdoor

    //initialize stash
    stash.SetLength(kFactors);
    for (long i = 0; i < kFactors; i++)
      stash[i].init(factors[i]);

    cout << "TDMatrixParams::init: sigmaX="<<sigmaX<<", mBar="<<mBar;
    cout << ", |q|="<<NTL::NumBits(this->getQ())<<endl;
}

//returns the produce of all the factors to the power of e, output = product(factors)^e
ZZ TDMatrixParams::getQ() const
{
    FHE_TIMER_START;
    ZZ q = to_ZZ(1L);
    for (long i=0; i<factors.length(); i++) // product of factors
        q *= factors[i];

    return NTL::power(q,e);  // return product^e
}

//outputs the different parameters in the class p into the stream s
ostream& operator<<(ostream &s, const TDMatrixParams& p)
{
    s << "[" << p.n        << " "
      << p.m        << " "
      << p.mBar     << " "
      << p.e        << " "
      << p.r        << "\n "
      << p.factors  << "]";
    return s;
}

//This function gets an input streams and sets the different parameters in p from this stream
istream& operator>>(istream &s, TDMatrixParams& p)
{
    seekPastChar(s, '[');  // this function is defined in tools.cpp
    s >> p.n;
    s >> p.m;
    s >> p.mBar;
    s >> p.e;
    s >> p.r;
    s >> p.factors;

    p.wLen = p.n * p.kFactors * p.e;

    p.maxFactor = setFactors(p.fInv, p.zzp_context, p.factors, p.e);
    p.sigmaX = getsigmaXVal(p.wLen, p.mBar, p.maxFactor, p.r);

    seekPastChar(s, ']');  // this function is defined in tools.cpp
    return s;
}

// binary I/O - write all the variables to the file. The handle of the open file is an input to the function The function returns the number of items written
long TDMatrixParams::writeToFile(FILE* handle) const
{
    FHE_TIMER_START;
    #ifdef DEBUGPRINT
    char cwd[1024];
    getcwd(cwd, sizeof(cwd));
    cout << "write TDMatrixParams, current directory = " << cwd << endl;
    #endif

    long count = fwrite(&n, sizeof(n), 1, handle);
    count += fwrite(&m, sizeof(m), 1, handle);
    count += fwrite(&mBar, sizeof(mBar), 1, handle);
    count += fwrite(&e, sizeof(e), 1, handle);
    count += fwrite(&r, sizeof(r), 1, handle);
    count += fwrite(&kFactors, sizeof(kFactors), 1, handle);
    count += fwrite(factors.elts(), sizeof(long), factors.length(), handle);

    return count;
}

//Binary IO - reads the class parameters from the file. The open file handle
//is provided as input to the function, and the number of items read is
//returned by the function.
long TDMatrixParams::readFromFile(FILE* handle)
{
    FHE_TIMER_START;
    long count = 0;

#ifdef DEBUGPRINT
    char cwd[1024];
    getcwd(cwd, sizeof(cwd));
    cout << "read TDMatrixParams from current directory = " << cwd << endl;
#endif

    count += fread(&n, sizeof(n), 1, handle);
    count += fread(&m, sizeof(m), 1, handle);
    count += fread(&mBar, sizeof(mBar), 1, handle);
    count += fread(&e, sizeof(e), 1, handle);
    count += fread(&r, sizeof(r), 1, handle);
    count += fread(&kFactors, sizeof(kFactors), 1, handle);

    factors.SetLength(kFactors);
    count += fread(factors.elts(), sizeof(long), factors.length(), handle);

    //initialize stash
    stash.SetLength(kFactors);
    for (long i = 0; i < kFactors; i++)
      stash[i].init(factors[i]);

    wLen = n * kFactors * e;

    maxFactor = setFactors(fInv, zzp_context, factors, e);
    sigmaX = getsigmaXVal(wLen, mBar, maxFactor, r);

    return count;
}

//check if all the variables in the two classes are equal if Yes, return true. Else, false.

bool operator==(const TDMatrixParams& p, const TDMatrixParams& q)
{
    return (p.n == q.n && p.m == q.m && p.mBar == q.mBar && p.e == q.e
            && p.r == q.r && p.wLen == q.wLen && p.kFactors == q.kFactors
            && p.factors == q.factors);
}

#if 0
// Deprecated, kept for debuging purposes
TDMatrixParams::TDMatrixParams(vec_l &vFactors, long lm,long rSigma,long llogQ,long lq, long nIn, long eL)
{
    FHE_TIMER_START;
    factors = vFactors;
    r = rSigma;
    kFactors = llogQ;
    n = nIn;
    m = lm;//SMS.NumCols();
    mBar = m - n*(kFactors*eL);
    wLen = n*kFactors*eL;
    e = eL;

    // For each factor f_i, initialize zz_p context for F_i=f_i^e,
    // and also compute f_i^{-1} mod F_j for all i<j
    maxFactor = setFactors(fInv, zzp_context, factors, e);

    sigmaX = getsigmaXVal(wLen, mBar, maxFactor, r);
    // The Gaussian parameter from which we can sample with a trapdoor

    stash.SetLength(kFactors);
    for (long i = 0; i < kFactors; i++)
      stash[i].init(factors[i]);
}
#endif