/*
Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved.

NVIDIA CORPORATION and its licensors retain all intellectual property
and proprietary rights in and to this software, related documentation
and any modifications thereto. Any use, reproduction, disclosure or
distribution of this software and related documentation without an express
license agreement from NVIDIA CORPORATION is strictly prohibited.
*/

#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <cmath>

#include <iostream>
#include <sstream>
#include <random>

#include "packages/pnp/gems/epnp/epnp_ransac.hpp"
#include "packages/pnp/gems/tests/simu.hpp"
#include "engine/core/logger.hpp"

// Print a RANSAC pose hypothesis.
std::ostream& operator<<(std::ostream& os,
                         const isaac::pnp::epnp::EpnpRansacAdaptor::ResultType& hypothesis) {
  // Print indices of input elements used as sample to generate this hypothesis.
  os << "seed:";
  for (auto idx : hypothesis.seed) {
    os << " " << idx;
  }

  // Print score of this hypothesis and the number of inliers.
  os << ", score: " << hypothesis.score << ", inliers: " << hypothesis.inliers.size();
  return os;
}

// Print any std::array for debugging.
template <typename T, size_t num_elements>
std::ostream& operator<<(std::ostream& os, const std::array<T, num_elements>& v) {
  for (const T& x : v) {
    os << " " << x;
  }
  return os;
}

// Print any std::vector for debugging.
template <typename T>
std::ostream& operator<<(std::ostream& os, const std::vector<T>& v) {
  for (const T& x : v) {
    os << " " << x;
  }
  return os;
}

// Test whether indices within a RANSAC sample set generated by DefaultRansacSampler repeat.
// Elements in a sample set should never repeat.
TEST(DefaultRansacSamplerTest, UniquenessPerSeed) {
  // Sampling parameters.
  constexpr unsigned sample_size = 6;     // Number of items in the sampled subset
  constexpr unsigned input_size = 10;     // Number of input items to sample from
  constexpr unsigned num_samples = 1000;  // Number of independent sample sets to draw
  constexpr unsigned rand_seed = 1256;    // Random seed to initialize the random engine

  // Create a sampler.
  isaac::pnp::DefaultRansacSampler<6> sampler(input_size, rand_seed);

  // Draw many independent subsets (= samples).
  for (unsigned k = 0; k < num_samples; k++) {
    // Draw a subset of unique indices.
    std::array<unsigned, sample_size> subset = sampler();

    // Check each element of the subset.
    for (unsigned i = 0; i < sample_size; i++) {
      // Check the range of the index.
      ASSERT_GE(subset[i], 0);
      ASSERT_LT(subset[i], input_size);

      // Make sure indices do not repeat within the subset.
      for (unsigned j = i + 1; j < sample_size; j++) {
        ASSERT_NE(subset[i], subset[j]);
      }
    }
  }
}

// Test whether each input item has equal chance of being selected into a RANSAC sample set.
// This is a probabilistic test, the chance of failing is non-zero but neglectable.
TEST(DefaultRansacSamplerTest, IsEqualChance) {
  // Create a RANSAC sampler object with random sample.
  constexpr unsigned sample_size = 6;
  constexpr unsigned input_size = 17;
  std::random_device r{};
  isaac::pnp::DefaultRansacSampler<6> sampler(input_size, r());

  // Draw a large number of RANSAC samples (subsets) and count
  // for every possible input index how many times it is chosen.
  // We expect that the result is a uniform distribution meaning that each input sample
  // has equal chance to be selected into the sample set.
  std::vector<double> histogram(input_size, 0);
  unsigned num_samples = 100000;
  for (unsigned k = 0; k < num_samples; k++) {
    // Draw a subset of unique indices.
    std::array<unsigned, sample_size> subset = sampler();

    for (unsigned i = 0; i < sample_size; i++) {
      // Check the range of the index.
      ASSERT_GE(subset[i], 0);
      ASSERT_LT(subset[i], input_size);

      // Count the number of occurences of every possible index.
      histogram[subset[i]]++;
    }
  }

  // Calculate the expected number occurences of each index (uniform distribution expected).
  // There are num_samples * sample_size indices drawn in total out of input_size indices.
  // Each of the input size indices should be selected 'expectation' number of times on average.
  double expectation = double(num_samples * sample_size) / input_size;

  // Measure the largest deviation from the expected number.
  double max_deviation = 0;
  for (unsigned i = 0; i < input_size; i++) {
    double d = std::abs(expectation - histogram[i]);
    if (d > max_deviation) {
      max_deviation = d;
    }
  }

  // Use a hard threshold on the largest deviation.
  // The threshold should be set high enough to reduce the chance of test failure (no guarantees)
  // but low enough to keep the uniformity test meaningful.
  // Print the statistics to the test output.
  std::stringstream ss;
  const double threshold = 0.05;
  ss << "Max deviation from uniform chance: ";
  ss << 100.0 * max_deviation / expectation << "%";
  ss << " (threshold: " << 100.0 * threshold << "%";
  ss << ", samples: " << num_samples << ")";
  LOG_INFO("%s", ss.str().c_str());

  ASSERT_LT(max_deviation / expectation, threshold);
}

// Test pose comparisons implemented in EpnpRansacAdaptor::isSimilar().
TEST(EpnpRansacAdaptorTest, PoseComparisonTests) {
  // Create an EpnpRansacAdaptor object.
  // The parameters passed to the constructor are not important, they are not used by isSimilar().
  isaac::Matrix2d points2;
  isaac::Matrix3d points3;
  isaac::pnp::epnp::EpnpRansacAdaptor adaptor(points3, points2, 1, 1, 0, 0, 0.1);

  // Identical poses.
  double angle = isaac::DegToRad(235.75);
  isaac::Vector3d axis(8.3, -0.3, 1.1);
  isaac::Vector3d axis2(-1.3, -3.3, -0.8);
  isaac::Vector3d position(25.3, -82.1, 39.1234);
  isaac::Pose3d pose1{isaac::SO3d::FromAngleAxis(angle, axis), isaac::Vector3d(8.3, -5.2, 125.6)};
  isaac::Pose3d pose2 = pose1;
  EXPECT_TRUE(adaptor.isSimilar(pose1, pose2));

  // Test simultaneous flipping of angle and axis signs, the underlying rotation being identical.
  pose2.rotation = isaac::SO3d::FromAngleAxis(-angle, -axis);
  EXPECT_TRUE(adaptor.isSimilar(pose1, pose2));

  // Test handling of two-fold angle-axis ambiguity within [0,2*pi] range.
  pose2.rotation = isaac::SO3d::FromAngleAxis(2 * isaac::Pi<double> - angle, -axis);
  EXPECT_TRUE(adaptor.isSimilar(pose1, pose2));

  // Test handling of angle-axis ambiguity modulo 2*pi and flipping.
  pose2.rotation = isaac::SO3d::FromAngleAxis(8 * isaac::TwoPi<double> - angle, -axis);
  EXPECT_TRUE(adaptor.isSimilar(pose1, pose2));

  // Test handling of angle-axis ambiguity at Pi.
  pose1.rotation = isaac::SO3d::FromAngleAxis(isaac::Pi<double>, axis);
  pose2.rotation = isaac::SO3d::FromAngleAxis(-isaac::Pi<double>, axis);
  EXPECT_TRUE(adaptor.isSimilar(pose1, pose2));

  // Test zero angle with different rotation axes.
  pose1.rotation = isaac::SO3d::FromAngleAxis(0.0, axis);
  pose2.rotation = isaac::SO3d::FromAngleAxis(0.0, axis2);
  EXPECT_TRUE(adaptor.isSimilar(pose1, pose2));

  // Test angle Pi with different rotation axes.
  pose1.rotation = isaac::SO3d::FromAngleAxis(isaac::Pi<double>, axis);
  pose2.rotation = isaac::SO3d::FromAngleAxis(isaac::Pi<double>, axis2);
  EXPECT_FALSE(adaptor.isSimilar(pose1, pose2));

  // Test difference in translation only.
  pose1.rotation = isaac::SO3d::FromAngleAxis(angle, axis);
  pose2.rotation = pose1.rotation;
  pose1.translation = -pose1.rotation.matrix() * position;
  pose2.translation = -pose2.rotation.matrix() * (position + isaac::Vector3d(13.25, 0, 0));
  EXPECT_FALSE(adaptor.isSimilar(pose1, pose2, 13.24));
  EXPECT_TRUE(adaptor.isSimilar(pose1, pose2, 13.26));

  // Test difference in rotation angle only with a tight threshold.
  // Make sure the camera position is the same (which differs from the translation component).
  pose1.rotation = isaac::SO3d::FromAngleAxis(angle, axis);
  pose2.rotation = isaac::SO3d::FromAngleAxis(angle + isaac::DegToRad(0.1234), axis);
  pose1.translation = -pose1.rotation.matrix() * position;
  pose2.translation = -pose2.rotation.matrix() * position;
  EXPECT_TRUE(adaptor.isSimilar(pose1, pose2, 1e-6, 0.1235));
  EXPECT_FALSE(adaptor.isSimilar(pose1, pose2, 1e-6, 0.1233));

  // Test larger difference in rotation angle.
  pose1.rotation = isaac::SO3d::FromAngleAxis(isaac::DegToRad(130.0), axis);
  pose2.rotation = isaac::SO3d::FromAngleAxis(isaac::DegToRad(230.0), axis);
  pose1.translation = -pose1.rotation.matrix() * position;
  pose2.translation = -pose2.rotation.matrix() * position;
  EXPECT_FALSE(adaptor.isSimilar(pose1, pose2, 1e-6, 90.0));
  EXPECT_TRUE(adaptor.isSimilar(pose1, pose2, 1e-6, 110.0));

  // Test turnaround for difference in rotation angle only.
  pose1.rotation = isaac::SO3d::FromAngleAxis(isaac::DegToRad(355.0), axis);
  pose2.rotation = isaac::SO3d::FromAngleAxis(isaac::DegToRad(5.0), axis);
  pose1.translation = -pose1.rotation.matrix() * position;
  pose2.translation = -pose2.rotation.matrix() * position;
  EXPECT_FALSE(adaptor.isSimilar(pose1, pose2, 1e-6, 9.9));
  EXPECT_TRUE(adaptor.isSimilar(pose1, pose2, 1e-6, 10.1));

  // test difference in rotation axis only
  pose1.rotation = isaac::SO3d::FromAngleAxis(angle, axis);
  pose2.rotation = isaac::SO3d::FromAngleAxis(angle, axis + isaac::Vector3d(0, 0.1, 1));
  pose1.translation = -pose1.rotation.matrix() * position;
  pose2.translation = -pose2.rotation.matrix() * position;
  EXPECT_FALSE(adaptor.isSimilar(pose1, pose2));
}

// A deterministic dummy RansacSampler to replace DefaultRansacSampler for controlled testing.
// This dummy sampler simply replays a preset sequence of index subsets, each of size 6.
class RansacTestSampler {
 private:
  // Number of input items to sample from. Sampled indices are in the range [0, input_size_].
  unsigned input_size_;

  // Sequence of samples.
  std::vector<std::array<unsigned, 6>> sequence_;

  // Index of the current sample in the sequence during "replay".
  unsigned index_;

 public:
  RansacTestSampler(unsigned input_size) : input_size_(input_size), index_(0) {}

  // Accessors
  unsigned sampleSize() const { return 6; }
  unsigned inputSize() const { return input_size_; }
  unsigned sequenceSize() const { return sequence_.size(); }

  // Add a sample to the end of the sequence.
  void Add(std::array<unsigned, 6>&& sample) { sequence_.push_back(std::move(sample)); }

  // Access elements of the sequence.
  const std::array<unsigned, 6>& operator[](unsigned i) const { return sequence_[i]; }

  // Return the next sample of the sequence.
  std::array<unsigned, 6> operator()() {
    auto& sample = sequence_[index_ % sequence_.size()];
    index_ = (index_ + 1) % sequence_.size();
    return sample;
  }
};

// Print elements of any container in C++ compatible format.
// Used in PrintRansacTestData() to generate data for a test fixture.
template <typename Iter>
void Dump(Iter begin, Iter end) {
  Iter it = begin;
  while (it != end) {
    std::cout << *it;
    ++it;
    if (it != end) {
      std::cout << ",";
    }
  }
}

// Print elements of a matrix columnwise in a single line in C++ compatible format.
// Used in PrintRansacTestData() to generate data for a test fixture.
void DumpMatrix(const isaac::MatrixXd& mat) {
  for (int i = 0; i < mat.rows(); i++) {
    std::cout << "  ";
    for (int j = 0; j < mat.cols(); j++) {
      std::cout << mat(i, j);

      // Elements are separated by a comma and the last element is followed by a semicolon.
      if (i == mat.rows() - 1 && j == mat.cols() - 1) {
        std::cout << ";";
      } else {
        std::cout << ", ";
      }
    }
    std::cout << std::endl;
  }
}

// Use this function to generate and print random test data for RANSAC tests.
// The output can be copy-pasted into EpnpRansacTest::SetUp() to use in the test fixture.
// Generates points from two incompatible poses plus some extra global outliers for testing RANSAC.
// Points 0,1...6  are inliers to model1 (pose1).
// Points 7,8...12 are inliers to model2 (pose2).
// Points 13, 14 are outliers to both models.
void PrintRansacTestData() {
  // Generate two cameras with random pose but fixed and identical intrinsics.
  const int width = 1280;
  const int height = 720;
  const double focal = 700.0;
  isaac::pnp::Camera camera1 = isaac::pnp::GenerateRandomCamera(width, height, focal);
  isaac::pnp::Camera camera2 = isaac::pnp::GenerateRandomCamera(width, height, focal);
  isaac::Vector3d axis1 = isaac::pnp::AngleAxisFromMatrix(camera1.rotation_matrix);
  isaac::Vector3d axis2 = isaac::pnp::AngleAxisFromMatrix(camera2.rotation_matrix);

  // Print all parameters of the two cameras in C++ code format.
  std::cout.unsetf(std::ios::fixed);
  std::cout << "<---------------------------------- start test data" << std::endl;
  std::cout << "// width  = " << width << std::endl;
  std::cout << "// height = " << height << std::endl;
  std::cout << "focal_u = focal_v = " << focal << ";" << std::endl;
  std::cout << "principal_u = " << camera1.calib_matrix(0, 2) << ";" << std::endl;
  std::cout << "principal_v = " << camera1.calib_matrix(1, 2) << ";" << std::endl;
  std::cout << std::ios::fixed << std::setprecision(12);
  std::cout << "gt_axis1 = isaac::Vector3d(";
  std::cout << axis1(0) << "," << axis1(1) << "," << axis1(2) << ");" << std::endl;
  std::cout << "gt_axis2 = isaac::Vector3d(";
  std::cout << axis2(0) << "," << axis2(1) << "," << axis2(2) << ");" << std::endl;
  std::cout << "gt_position1  = isaac::Vector3d(";
  std::cout << camera1.position(0) << ",";
  std::cout << camera1.position(1) << ",";
  std::cout << camera1.position(2) << ");" << std::endl;
  std::cout << "gt_position2  = isaac::Vector3d(";
  std::cout << camera2.position(0) << ",";
  std::cout << camera2.position(1) << ",";
  std::cout << camera2.position(2) << ");" << std::endl;

  // Generate equal number of inlier 2D-3D matches for each of the two camera models.
  // For each camera, its 3D points lie within the camera FoV between the near and far planes.
  // Stack the two generated 2D/3D points sets into a single 2xN/3xN matrix.
  const unsigned num_inliers = 7;
  const double near = 2.0;
  const double far = 6.0;
  isaac::Matrix3Xd points3_model1, points3_model2;
  isaac::Matrix2Xd points2_model1, points2_model2;
  isaac::pnp::GenerateFovPoints(num_inliers, camera1, near, far, &points3_model1, &points2_model1);
  isaac::pnp::GenerateFovPoints(num_inliers, camera2, near, far, &points3_model2, &points2_model2);
  isaac::Matrix3Xd points3 = isaac::pnp::StackMatricesHorizontally(points3_model1, points3_model2);
  isaac::Matrix2Xd points2 = isaac::pnp::StackMatricesHorizontally(points2_model1, points2_model2);

  // Insert 2D-3D matches that are outliers to both models.
  const unsigned num_outliers = 2;
  const double radius = 30.0;
  InsertOutliers(num_outliers, camera1, radius, &points3, &points2, false);

  // Collect the indices of inliers for each of the two models separately.
  // A 2D/3D match is inlier if its reprojection error in pixels is neglectable.
  const double threshold = 0.01;
  isaac::VectorXd errs1 =
      isaac::pnp::ColwiseNorms(points2 - ProjectPoints(camera1, points3, true, true));
  isaac::VectorXd errs2 =
      isaac::pnp::ColwiseNorms(points2 - ProjectPoints(camera2, points3, true, true));
  std::vector<unsigned> inliers1;
  std::vector<unsigned> inliers2;
  for (unsigned i = 0; i < errs1.size(); i++) {
    if (errs1[i] < threshold) {
      inliers1.push_back(i);
    }
  }
  for (unsigned i = 0; i < errs2.size(); i++) {
    if (errs2[i] < threshold) {
      inliers2.push_back(i);
    }
  }

  // Print the indices of the inliers in C++ code format.
  std::cout << "inliers1 = vector<unsigned>{";
  Dump(inliers1.begin(), inliers1.end());
  std::cout << "};" << std::endl;
  std::cout << "inliers2 = vector<unsigned>{";
  Dump(inliers2.begin(), inliers2.end());
  std::cout << "};" << std::endl;

  // Print the 2D and 3D points in C++ code format.
  // The point matrices are printed transposed (Nx2 and Nx3)
  // to make the output coordinates more readable.
  std::cout << std::ios::fixed << std::setprecision(12);
  std::cout << "isaac::MatrixXd pts3(" << points3.cols() << ", " << points3.rows() << ");";
  std::cout << std::endl;
  std::cout << "pts3 << " << std::endl;
  DumpMatrix(points3.transpose());
  std::cout << "points3 = pts3.transpose();" << std::endl;
  std::cout << "isaac::MatrixXd pts2(" << points2.cols() << ", " << points2.rows() << ");";
  std::cout << std::endl;
  std::cout << "pts2 << " << std::endl;
  DumpMatrix(points2.transpose());
  std::cout << "points2 = pts2.transpose();" << std::endl;
  std::cout << "<---------------------------------- end test data" << std::endl;
}

// Test fixture for testing RANSAC-based pose estimation.
// Stores ground-truth data for two synthetic cameras with identical intrinsics but different poses
// and a set of 2D/3D point matches with 3 subsets:
// (1) inlier matches to the first camera model,
// (2) inlier matches to the second camera model,
// (3) outliers to both models.
class EpnpRansacTest : public ::testing::Test {
 protected:
  // Identical intrinsic camera parameters for the two cameras.
  double focal_u, focal_v, principal_u, principal_v;

  // Joint set of 2D/3D point matches with inliers and outliers mixed together.
  isaac::Matrix2Xd points2;
  isaac::Matrix3Xd points3;
  std::vector<unsigned> inliers1;
  std::vector<unsigned> inliers2;

  // Ground truth poses of the two camera models.
  isaac::Vector3d gt_axis1;
  isaac::Vector3d gt_axis2;
  isaac::Vector3d gt_position1;
  isaac::Vector3d gt_position2;

 public:
  void SetUp() override {
    // Generate and dump new random test data. Copy the output and paste it below.
    // PrintRansacTestData();

    // The rest of this function is code generated by PrintRansacTestData().
    focal_u = focal_v = 700.0;
    principal_u = 640.0;
    principal_v = 360.0;
    gt_axis1 = isaac::Vector3d(1.246431082136, -1.488785282762, -1.717268329543);
    gt_axis2 = isaac::Vector3d(-1.474963839733, -0.393161160840, -0.552226884507);
    gt_position1 = isaac::Vector3d(40.959062410363, -49.812992848765, 48.298686644820);
    gt_position2 = isaac::Vector3d(90.028174426100, -18.458540753177, -80.312594442311);
    inliers1 = std::vector<unsigned>{0, 1, 2, 3, 4, 5, 6};
    inliers2 = std::vector<unsigned>{7, 8, 9, 10, 11, 12, 13};

    isaac::MatrixXd pts3(16, 3);
    pts3 << 41.400241668095, -44.804710827040, 51.713684730464, 39.430519169917, -44.348882210449,
        51.737095051124, 38.976047959394, -47.853245718666, 48.035537371287, 40.121192878014,
        -46.566809657102, 48.723233147234, 42.146531276923, -43.742220747133, 49.151236907191,
        39.689011504827, -45.123227520864, 52.569910421456, 40.532981841159, -44.985869661988,
        45.874549415954, 91.634076934706, -22.765244053848, -80.184494647314, 95.364222887767,
        -20.116176008183, -78.723192969495, 90.732603393096, -20.884428333034, -80.228258881278,
        95.769983747879, -21.320357375152, -81.698183257387, 94.628990286377, -21.090631619923,
        -80.527450089501, 89.974729650745, -25.308855163863, -77.586708526601, 94.514488443643,
        -21.030260810473, -78.703926460615, 49.361946471414, -59.553509542194, 32.889233874131,
        32.523546008870, -61.005788561317, 53.299292341019;
    points3 = pts3.transpose();

    isaac::MatrixXd pts2(16, 2);
    pts2 << 12.395121723710, 354.059751504487, 220.188841927345, 556.325084028859, 853.730568087874,
        675.275631428689, 522.223310954897, 386.704006604826, 347.281595704401, 89.307240469900,
        36.122163391302, 643.209326222118, 862.781555544812, 84.769172394853, 461.035680577970,
        346.450723118805, 1191.366827535621, 513.753105427619, 405.180431127773, 355.671223505314,
        1013.206030748049, 102.744712993492, 968.988030487053, 251.735658969613, 188.308022960250,
        681.152710697888, 981.848664116682, 520.116868250137, 354.655750007339, 315.040405692242,
        159.349736752775, 486.784479513776;
    points2 = pts2.transpose();
  }
};

// Verify if a pose hypothesis returned by RANSAC is correct, assuming no noise in
// the 2D/3D matches leading to this pose hypothesis.
//  (1) Test if the sample is reported correctly inside the hypothesis.
//  (2) Test if all actual inliers are found and reported within the hypothesis.
//  (3) Test if the hypothesized pose matches the ground-truth pose.
// Parameters:
//  hypothesis     The RANSAC pose hypothesis to test.
//  seed           The ground-truth sample fed to RANSAC in the test, leading to this hypothesis.
//                 This sample is randomly generated inside RANSAC during normal operation.
//  inliers        The known set of inliers RANSAC is supposed to find a part of this hypothesis.
//  gt_angle_axis  Ground-truth camera orientation as an angle-axis.
//  gt_position    Ground-truth camera position.
void CheckHypothesis(const isaac::pnp::RansacHypothesis<isaac::Pose3d, 6>& hypothesis,
                     const std::array<unsigned, 6>& seed, const std::vector<unsigned int>& inliers,
                     const isaac::Vector3d& gt_angle_axis, const isaac::Vector3d& gt_position) {
  // Test if the sample that generated the hypothesis and the inliers compatible with the hypothesis
  // are the expected ones.
  ASSERT_EQ(hypothesis.seed.size(), 6);
  ASSERT_EQ(hypothesis.inliers.size(), inliers.size());
  ASSERT_NEAR(hypothesis.score, double(inliers.size()), 1e-3);
  ASSERT_TRUE((hypothesis.inliers == inliers));
  ASSERT_TRUE((hypothesis.seed == seed));

  // Wrap the norm of the angle-axis in the [0,Pi] range without changing the underlying rotation.
  double angle = std::fmod(hypothesis.model.rotation.angle(), 2 * isaac::Pi<double>);
  if (angle < 0.0) angle += 2 * isaac::Pi<double>;
  isaac::Vector3d angle_axis = hypothesis.model.rotation.axis();
  if (angle > isaac::Pi<double>) {
    angle = 2 * isaac::Pi<double> - angle;
    angle_axis = -angle_axis;
  }
  angle_axis *= angle;

  // Extract the camera position from the pose.
  isaac::Vector3d position =
      -isaac::pnp::MatrixFromAngleAxis(angle_axis).transpose() * hypothesis.model.translation;

  // Test if the hypothesized pose matches the ground-truth pose to numerical precision.
  // This assumes no noise in the measurement.
  const double tol = 1e-6;
  ASSERT_LT((position - gt_position).norm(), tol);
  // Handle the remaining flip ambiguity of angle-axes at rotation angle Pi.
  // Angle axes with norm Pi are equal if they are parallel.
  if (std::abs(angle - isaac::Pi<double>) < tol &&
      std::abs(gt_angle_axis.norm() - isaac::Pi<double>) < tol) {
    // Parallelity test.
    ASSERT_LT(angle_axis.cross(gt_angle_axis).norm(), tol);
  } else {
    // Equality test for the angle range [0,Pi), excluding Pi.
    // The angle-axis representation is unique in this range.
    ASSERT_LT((angle_axis - gt_angle_axis).norm(), tol);
  }
}

// Test RANSAC with the noise-free test data in EpnpRansacTest.
// Sampling sequence: one all-inlier sample.
// Expected result: there should be a single correct hypothesis returned for the right model.
TEST_F(EpnpRansacTest, SingleGoodHypothesis) {
  // RANSAC parameters.
  const unsigned max_top_hypotheses = 5;
  const double threshold = 1e-6;  // Reprojection error in pixels for an inlier.

  // Set up RANSAC sampling sequence that replaces random sampling in the tests.
  RansacTestSampler sampler(points3.cols());
  sampler.Add({0, 1, 2, 3, 4, 5});  // good hypothesis for model1

  // Run RANSAC with test fixture data.
  isaac::pnp::epnp::EpnpRansacAdaptor adaptor(points3, points2, focal_u, focal_v, principal_u,
                                              principal_v, threshold);
  auto top_hypotheses =
      isaac::pnp::Ransac(adaptor, sampler.sequenceSize(), max_top_hypotheses, &sampler);

  // Test if there is a singe accepted hypothesis and that it is perfect.
  ASSERT_EQ(top_hypotheses.size(), 1);
  SCOPED_TRACE("SingleGoodHypothesis");
  CheckHypothesis(top_hypotheses[0], sampler[0], inliers1, gt_axis1, gt_position1);
}

// Test RANSAC with the noise-free test data in EpnpRansacTest.
// Sampling sequence: only one contaminated sample.
// Expected result: RANSAC is not supposed to find any solution.
TEST_F(EpnpRansacTest, SingleBadHypothesis) {
  // RANSAC parameters.
  const unsigned max_top_hypotheses = 5;
  const double threshold = 0.1;

  // Set up RANSAC sampling sequence that replaces random sampling in the tests.
  RansacTestSampler sampler(points3.cols());
  sampler.Add({0, 1, 2, 3, 9, 14});  // bad hypothesis for both models

  // Run RANSAC with test fixture data.
  isaac::pnp::epnp::EpnpRansacAdaptor adaptor(points3, points2, focal_u, focal_v, principal_u,
                                              principal_v, threshold);
  auto top_hypotheses =
      isaac::pnp::Ransac(adaptor, sampler.sequenceSize(), max_top_hypotheses, &sampler);

  // Solution should not be found from a single bad hypothesis.
  ASSERT_EQ(top_hypotheses.size(), 0);
}

// Test RANSAC with the noise-free test data in EpnpRansacTest.
// Sampling sequence: many all-inlier samples.
// Expected result: RANSAC should recognize internally that the samples lead to the same result
// and return it only once. This result should be the perfect solution.
TEST_F(EpnpRansacTest, RepeatingGoodHypotheses) {
  // RANSAC parameters.
  const unsigned max_top_hypotheses = 5;
  const double threshold = 0.1;

  // Set up RANSAC sampling sequence that replaces random sampling in the tests.
  RansacTestSampler sampler(points3.cols());
  sampler.Add({1, 2, 3, 4, 5, 6});  // good hypothesis for model1
  sampler.Add({0, 2, 3, 4, 5, 6});  // good hypothesis for model1
  sampler.Add({0, 1, 3, 4, 5, 6});  // good hypothesis for model1
  sampler.Add({0, 1, 2, 4, 5, 6});  // good hypothesis for model1
  sampler.Add({0, 1, 2, 3, 5, 6});  // good hypothesis for model1
  sampler.Add({0, 1, 2, 3, 4, 6});  // good hypothesis for model1
  sampler.Add({0, 1, 2, 3, 4, 5});  // good hypothesis for model1

  // Run RANSAC with test fixture data.
  isaac::pnp::epnp::EpnpRansacAdaptor adaptor(points3, points2, focal_u, focal_v, principal_u,
                                              principal_v, threshold);
  auto top_hypotheses =
      isaac::pnp::Ransac(adaptor, sampler.sequenceSize(), max_top_hypotheses, &sampler);

  // There should be a single correct hypothesis returned because the different sample sets
  // will lead to the same correct inlier set, hence, the same pose. Check this hypothesis.
  ASSERT_EQ(top_hypotheses.size(), 1);
  SCOPED_TRACE("RepeatingGoodHypotheses");
  CheckHypothesis(top_hypotheses[0], sampler[0], inliers1, gt_axis1, gt_position1);
}

// Test RANSAC with the noise-free test data in EpnpRansacTest.
// Sampling sequence: one perfect sample per model.
// Expected result: RANSAC is supposed to find both correct models.
TEST_F(EpnpRansacTest, OneGoodHypothesisPerModel) {
  // RANSAC parameters.
  const unsigned max_top_hypotheses = 5;
  const double threshold = 0.1;

  // Set up RANSAC sampling sequence that replaces random sampling in the tests.
  RansacTestSampler sampler(points3.cols());
  sampler.Add({0, 1, 2, 3, 4, 5});     // good hypothesis for model1
  sampler.Add({7, 8, 9, 10, 11, 12});  // good hypothesis for model2

  // Run RANSAC with test fixture data.
  isaac::pnp::epnp::EpnpRansacAdaptor adaptor(points3, points2, focal_u, focal_v, principal_u,
                                              principal_v, threshold);
  auto top_hypotheses =
      isaac::pnp::Ransac(adaptor, sampler.sequenceSize(), max_top_hypotheses, &sampler);

  // Check if two solutions are found: one accepted hypothesis for each of the two valid poses.
  ASSERT_EQ(top_hypotheses.size(), 2);
  SCOPED_TRACE("OneGoodHypothesisPerModel");
  CheckHypothesis(top_hypotheses[0], sampler[0], inliers1, gt_axis1, gt_position1);
  CheckHypothesis(top_hypotheses[1], sampler[1], inliers2, gt_axis2, gt_position2);
}

// Test RANSAC with the noise-free test data in EpnpRansacTest.
// Sampling sequence: only contaminated samples.
// Expected result: RANSAC is not supposed to find the correct solution.
TEST_F(EpnpRansacTest, OnlyBadHypotheses) {
  // RANSAC parameters.
  const unsigned max_top_hypotheses = 5;
  const double threshold = 0.1;

  // Set up RANSAC sampling sequence that replaces random sampling in the tests.
  RansacTestSampler sampler(points3.cols());
  sampler.Add({0, 1, 2, 3, 4, 10});    // bad hypothesis for both models
  sampler.Add({0, 1, 2, 3, 4, 10});    // bad hypothesis for both models
  sampler.Add({3, 8, 9, 10, 11, 12});  // bad hypothesis for both models
  sampler.Add({3, 5, 9, 11, 13, 15});  // bad hypothesis for both models

  // Run RANSAC with test fixture data.
  isaac::pnp::epnp::EpnpRansacAdaptor adaptor(points3, points2, focal_u, focal_v, principal_u,
                                              principal_v, threshold);
  auto top_hypotheses =
      isaac::pnp::Ransac(adaptor, sampler.sequenceSize(), max_top_hypotheses, &sampler);

  // No hypothesis should be accepted since all samples were contaminated by outliers.
  ASSERT_EQ(top_hypotheses.size(), 0);
}

// Test RANSAC with the noise-free test data in EpnpRansacTest.
// Sampling sequence: a mixture of contaminated samples and several clean samples for both models.
// Expected result: RANSAC should return a single accepted hypothesis per model.
TEST_F(EpnpRansacTest, ManyMixedHypotheses) {
  // RANSAC parameters.
  const unsigned max_top_hypotheses = 5;
  const double threshold = 0.1;

  // Set up RANSAC sampling sequence that replaces random sampling in the tests.
  RansacTestSampler sampler(points3.cols());
  sampler.Add({0, 1, 2, 3, 4, 10});     // bad hypothesis for both models
  sampler.Add({0, 1, 2, 3, 4, 10});     // bad hypothesis for both models
  sampler.Add({7, 8, 9, 10, 11, 12});   // good hypothesis for model2
  sampler.Add({3, 8, 9, 10, 11, 12});   // bad hypothesis for both models
  sampler.Add({3, 5, 9, 11, 13, 14});   // bad hypothesis for both models
  sampler.Add({0, 1, 2, 4, 5, 6});      // good hypothesis for model1
  sampler.Add({3, 5, 9, 11, 13, 14});   // bad hypothesis for both models
  sampler.Add({2, 4, 8, 10, 11, 13});   // bad hypothesis for both models
  sampler.Add({8, 9, 10, 11, 12, 13});  // good hypothesis for model2
  sampler.Add({1, 2, 3, 4, 5, 6});      // good hypothesis for model1

  // Run RANSAC with test fixture data.
  isaac::pnp::epnp::EpnpRansacAdaptor adaptor(points3, points2, focal_u, focal_v, principal_u,
                                              principal_v, threshold);
  auto top_hypotheses =
      isaac::pnp::Ransac(adaptor, sampler.sequenceSize(), max_top_hypotheses, &sampler);

  // Both models were sampled in the sequence several times, so both models should be returned
  // as one hypothesis each. No hypothesis should repeat.
  ASSERT_EQ(top_hypotheses.size(), 2);
  SCOPED_TRACE("ManyMixedHypotheses");
  CheckHypothesis(top_hypotheses[0], sampler[2], inliers2, gt_axis2, gt_position2);
  CheckHypothesis(top_hypotheses[1], sampler[5], inliers1, gt_axis1, gt_position1);
}

int main(int argc, char** argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


// Copyright 2004-present Facebook. All Rights Reserved.

#include <iomanip>
#include <sstream>

#if defined(__linux__)
#include <net/if.h>
#include <netinet/in.h>
#include <sys/ioctl.h>
#include <unistd.h>
#define AF_LINK AF_PACKET
#else
#include <net/if_dl.h>
#endif

#include <boost/algorithm/string/trim.hpp>

#include "osquery/tables/networking/utils.h"

std::string canonical_ip_address(const struct sockaddr *in) {
  char dst[INET6_ADDRSTRLEN];
  memset(dst, 0, sizeof(dst));
  void *in_addr;

  if (in->sa_family == AF_INET) {
    in_addr = (void *)&(((struct sockaddr_in *)in)->sin_addr);
  } else if (in->sa_family == AF_INET6) {
    in_addr = (void *)&(((struct sockaddr_in6 *)in)->sin6_addr);
  } else {
    return "";
  }

  inet_ntop(in->sa_family, in_addr, dst, sizeof(dst));
  std::string address(dst);
  boost::trim(address);

  return address;
}

inline short addBits(unsigned char byte) {
  short bits = 0;
  for (int i = 7; i >= 0; --i) {
    if ((byte & (1 << i)) == 0) {
      break;
    }
    bits++;
  }
  return bits;
}

int netmaskFromIP(const struct sockaddr *in) {
  int mask = 0;

  if (in->sa_family == AF_INET6) {
    struct sockaddr_in6 *in6 = (struct sockaddr_in6 *)in;
    for (size_t i = 0; i < 16; i++) {
      mask += addBits(in6->sin6_addr.s6_addr[i]);
    }
  } else {
    struct sockaddr_in *in4 = (struct sockaddr_in *)in;
    char *address = reinterpret_cast<char *>(&in4->sin_addr.s_addr);
    for (size_t i = 0; i < 4; i++) {
      mask += addBits(address[i]);
    }
  }

  return mask;
}

std::string canonical_mac_address(const struct ifaddrs *addr) {
  std::stringstream mac;

  if (addr->ifa_addr == NULL) {
    // No link or MAC exists.
    return "";
  }

#if defined(__linux__)
  struct ifreq ifr;

  int socket_fd = socket(AF_INET, SOCK_DGRAM, 0);

  ifr.ifr_addr.sa_family = AF_INET;
  strcpy(ifr.ifr_name, addr->ifa_name);
  ioctl(socket_fd, SIOCGIFHWADDR, &ifr);
  close(socket_fd);

  for (size_t i = 0; i < 6; i++) {
    mac << std::hex << std::setfill('0') << std::setw(2);
    mac << (int)((uint8_t)ifr.ifr_hwaddr.sa_data[i]) << ":";
  }
#else
  struct sockaddr_dl *sdl;

  sdl = (struct sockaddr_dl *)addr->ifa_addr;
  if (sdl->sdl_alen != 6) {
    // Do not support MAC address that are not 6 bytes...
    return "";
  }

  for (size_t i = 0; i < sdl->sdl_alen; i++) {
    mac << std::hex << std::setfill('0') << std::setw(2);
    // Prevent char sign extension.
    mac << (int)((uint8_t)sdl->sdl_data[i + sdl->sdl_nlen]) << ":";
  }
#endif

  return mac.str();
}


/*
 * Copyright (c) 2015, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: Erik Nelson            ( eanelson@eecs.berkeley.edu )
 *          David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

#include <matching/distance_metric.h>
#include <math/random_generator.h>
#include <util/types.h>

#include <Eigen/Core>
#include <gtest/gtest.h>

namespace bsfm {

// The L2 distance between two (descriptor) vectors induces an inner product
// space. Test 2 of the properties of inner product spaces (the other 2,
// scalar/vector linearity, won't hold unless we square our feature vectors, so
// we don't need to care too much about that property). Check:
// 1. (Symmetry)              <x, y> = <y, x>
// 2. (Positive definiteness) <x, x> = 0 ==> x = 0
//
// Also test for the correct value.

TEST(DistanceMetric, TestSymmetryScaledL2) {
  // Set the distance metric type.
  DistanceMetric& distance = DistanceMetric::Instance();
  distance.SetMetric(DistanceMetric::Metric::SCALED_L2);

  Descriptor descriptor1(64);
  Descriptor descriptor2(64);
  for (int ii = 0; ii < 1000; ++ii) {
    descriptor1.setRandom().normalize();
    descriptor2.setRandom().normalize();

    // Check symmetry.
    EXPECT_NEAR(distance(descriptor1, descriptor2),
                distance(descriptor2, descriptor1), 1e-4);
  }
}

TEST(DistanceMetric, TestPositiveDefinitenessScaledL2) {
  // Set the distance metric type.
  DistanceMetric& distance = DistanceMetric::Instance();
  distance.SetMetric(DistanceMetric::Metric::SCALED_L2);

  Descriptor descriptor1(64);
  for (int ii = 0; ii < 1000; ++ii) {
    descriptor1.setRandom().normalize();

    Descriptor descriptor2 = descriptor1;
    EXPECT_NEAR(0.0, distance(descriptor1, descriptor2), 1e-4);
  }
}

TEST(DistanceMetric, TestValueScaledL2) {
  // Set the distance metric type.
  DistanceMetric& distance = DistanceMetric::Instance();
  distance.SetMetric(DistanceMetric::Metric::SCALED_L2);

  Descriptor descriptor1(64);
  Descriptor descriptor2(64);
  for (int ii = 0; ii < 1000; ++ii) {
    descriptor1.setRandom().normalize();
    descriptor2.setRandom().normalize();

    const double expected_dist = 1.0 - descriptor1.dot(descriptor2);
    EXPECT_NEAR(expected_dist, distance(descriptor1, descriptor2), 1e-4);
  }
}

// For Hamming distance, test for symmetry, and also check that two binary
// descriptors have a large distance when they have 0 matches, and have a 0
// distance when they match exactly.
TEST(DistanceMetric, TestSymmetryHamming) {
  // Set the distance metric type.
  DistanceMetric& distance = DistanceMetric::Instance();
  distance.SetMetric(DistanceMetric::Metric::HAMMING);

  Descriptor descriptor1(32);
  Descriptor descriptor2(32);
  for (int ii = 0; ii < 1000; ++ii) {
    descriptor1.setRandom();
    descriptor2.setRandom();

    // Check symmetry.
    EXPECT_EQ(distance(descriptor1, descriptor2),
              distance(descriptor2, descriptor1));
  }
}

TEST(DistanceMetric, TestValueHamming) {
  // Set the distance metric type.
  DistanceMetric& distance = DistanceMetric::Instance();
  distance.SetMetric(DistanceMetric::Metric::HAMMING);

  Descriptor descriptor1(32);
  Descriptor descriptor2(32);
  for (int ii = 0; ii < 1000; ++ii) {
    descriptor1.setRandom();
    descriptor2.setRandom();

    int expected_dist = 0;
    for (int jj = 0; jj < 32; ++jj) {
      unsigned char d1 = static_cast<unsigned char>(descriptor1(jj));
      unsigned char d2 = static_cast<unsigned char>(descriptor2(jj));
      expected_dist += d1 ^ d2;
    }
    EXPECT_EQ(expected_dist, distance(descriptor1, descriptor2));
  }

  // Set the descriptors to exact opposites of one another.
  for (int ii = 0; ii < 32; ++ii) {
    descriptor1(ii) = static_cast<unsigned char>(ii % 2 == 0);
    descriptor2(ii) = static_cast<unsigned char>(ii % 2 != 0);
  }
  // Check that we get a maximum value (32).
  EXPECT_EQ(32, distance(descriptor1, descriptor2));

  // Now set the descriptors to be equal, and check that we get a distance of 0.
  descriptor2 = descriptor1;
  EXPECT_EQ(0, distance(descriptor1, descriptor2));
}

}  //\namespace bsfm


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

#ifndef MTL_CONJ_INCLUDE
#define MTL_CONJ_INCLUDE

#include <complex>

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/utility/enable_if.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/algebraic_category.hpp>
#include <boost/numeric/mtl/utility/is_what.hpp>
#include <boost/numeric/mtl/utility/view_code.hpp>
#include <boost/numeric/mtl/utility/viewed_collection.hpp>
#include <boost/numeric/mtl/utility/compose_view.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/matrix/view_ref.hpp>
#include <boost/numeric/mtl/matrix/map_view.hpp>
#include <boost/numeric/mtl/vector/map_view.hpp>

namespace mtl {

namespace sfunctor {

    template <typename Value, typename AlgebraicCategory>
    struct conj_aux
    {
	typedef Value result_type;

	static inline result_type apply(const Value& v)
	{
	    return v;
	}

	result_type operator() (const Value& v) const
	{
	    return v;
	}
    };

    template <typename Value, typename AlgebraicCategory>
    struct conj_aux<std::complex<Value>, AlgebraicCategory>
    {
	typedef std::complex<Value> result_type;

	static inline result_type apply(const std::complex<Value>& v)
	{
	    return std::conj(v);
	}

	result_type operator() (const std::complex<Value>& v) const
	{
	    return std::conj(v);
	}
    };

    // Only declarations here, definitions in mat::map_view (map_view)
    template <typename Matrix>
    struct conj_aux<Matrix, tag::matrix>;

    template <typename Vector>
    struct conj_aux<Vector, tag::vector>;

    // Short cut for result type
    template <typename Value>
    struct conj
	: public conj_aux<Value, typename mtl::traits::algebraic_category<Value>::type>
    {};

} // namespace sfunctor

    namespace vec {

	/// Conjugate of an vector
	template <typename Vector>
	typename mtl::traits::enable_if_vector<Vector, conj_view<Vector> >::type
	inline conj(const Vector& v)
	{
	    return conj_view<Vector>(v);
	}
    }

    namespace mat {

	namespace detail {

	    template <typename Matrix>
	    struct conj_trait
	    {
		static const unsigned code= mtl::traits::view_toggle_conj<mtl::traits::view_code<Matrix> >::value;
		typedef typename mtl::traits::compose_view<code, typename mtl::traits::viewed_collection<Matrix>::type>::type type;

		static inline type apply(const Matrix& A)
		{
		    return type(view_ref(A));
		}
	    };

	}

	/// Conjugate of a matrix
	template <typename Matrix>
	typename mtl::traits::lazy_enable_if_matrix<Matrix, detail::conj_trait<Matrix> >::type
	inline conj(const Matrix& A)
	{
	    return detail::conj_trait<Matrix>::apply(A);
	}
    }

    namespace scalar {

	// Only scalar values remain here
	template <typename Value>
	typename mtl::traits::enable_if_scalar<
	    Value
	  , typename sfunctor::conj<Value>::result_type
	>::type
	inline conj(const Value& v)
	{
	    return mtl::sfunctor::conj<Value>::apply(v);
	}

	float inline conj(float v) { return v; }
	double inline conj(double v) { return v; }
	long double inline conj(long double v) { return v; }
    }

    /// Conjugate of vector, matrix, or scalar
    using vec::conj;
    using mat::conj;
    using scalar::conj;
    // using std::conj;

} // namespace mtl

#endif // MTL_CONJ_INCLUDE


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

#include <boost/test/unit_test.hpp>
#include "algorithm/hyper_log_log.h"
#include <cstdlib>
#include <vector>

using namespace std;

BOOST_AUTO_TEST_SUITE(hyper_log_log)

BOOST_AUTO_TEST_CASE(hyper_simple) {
	{
		algorithm::hyper_log_log hl;

		BOOST_CHECK(hl.leading_zeros_plus_one(0x0ull) == 65);
		BOOST_CHECK(hl.leading_zeros_plus_one(0x1ull) == 64);
		BOOST_CHECK(hl.leading_zeros_plus_one(0xFFFFFFFFull) == 33);
		BOOST_CHECK(hl.leading_zeros_plus_one(0xFFFFFFFFull) == 33);
	}
}

BOOST_AUTO_TEST_CASE(hyper_inserts) {

	{
		algorithm::hyper_log_log hl;
		hl.insert(0);
		hl.insert(1);
		hl.insert(2);
		hl.insert(3);
		hl.insert(4);
		hl.insert(5);
		hl.insert(6);

		algorithm::hyper_log_log hl2;
		hl2.insert(0);
		hl2.insert(1);
		hl2.insert(2);
		hl2.insert(3);
		hl2.insert(4);
		hl2.insert(5);
		hl2.insert(7);

		algorithm::hyper_log_log hl3 = hl + hl2;
	}

	vector<size_t> intervals = {400000, 500000, 1000000, 10000000};

	for (size_t interval : intervals) {
		algorithm::hyper_log_log hl;
		for (size_t i = 0; i < interval; i++) {
			hl.insert(i);
		}
		BOOST_CHECK(std::abs((int)hl.count() - (int)interval) < interval * hl.error_bound());
	}

}

BOOST_AUTO_TEST_CASE(hyper_union) {
	algorithm::hyper_log_log hl1;
	algorithm::hyper_log_log hl2;

	for (size_t i = 0; i < 250000; i++) {
		hl1.insert(i);
	}
	for (size_t i = 250000; i < 500000; i++) {
		hl2.insert(i);
	}

	algorithm::hyper_log_log hl3 = hl1 + hl2;
	BOOST_CHECK(std::abs((int)hl3.count() - 500000) < 500000 * hl3.error_bound());
}

BOOST_AUTO_TEST_CASE(hyper_log_log_data_copy) {
	algorithm::hyper_log_log hl1;

	for (size_t i = 0; i < 250000; i++) {
		hl1.insert(i);
	}

	algorithm::hyper_log_log hl2(hl1.data());

	BOOST_CHECK(std::abs((int)hl2.count() - 250000) < 250000 * hl1.error_bound());

	std::vector<size_t> sizes = {25000, 50000, 75000, 100000, 200000, 300000, 400000};

	srand(100);
	for (size_t size : sizes) {
		algorithm::hyper_log_log hll;
		for (size_t i = 0; i < size; i++) {
			size_t rnd = (((size_t)rand()) << 32) | ((size_t)rand());
			hll.insert(rnd);
		}
		BOOST_CHECK(std::abs((int)hll.count() - (int)size) < size * hl1.error_bound());
	}
}

BOOST_AUTO_TEST_CASE(hyper_log_log_test2) {
	algorithm::hyper_log_log hl1(8);

	for (size_t i = 0; i < 1500000; i++) {
		hl1.insert(rand());
	}

	cout << "size: " << hl1.count() << endl;
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE lue core time duration
#include <boost/test/unit_test.hpp>
#include "lue/core/time.hpp"

namespace ldm = lue::data_model;

BOOST_AUTO_TEST_CASE(seconds)
{
    // Period of 60 seconds modelled as 6 * (10 seconds)
    std::size_t const nr_ticks = 6;

    using TickPeriod = ldm::time::TickPeriod<ldm::time::Second>;
    using Duration = ldm::time::Duration<TickPeriod>;

    Duration duration(nr_ticks);

    BOOST_CHECK_EQUAL(duration.nr_ticks(), nr_ticks);
}


/*******************************************************************************
 * Copyright (c) 2014, 2015  IBM Corporation and others
 *
 * 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.
 *******************************************************************************/

#ifndef EigenSerializeUtils_h
#define EigenSerializeUtils_h

#ifdef ANDROID_STL_EXT
#include "string_ext.hpp"
#endif /* ANDROID_STL_EXT */
#include <Eigen/Core>
#include <Eigen/SparseCore>

#include <cereal/archives/xml.hpp>
#include <cereal/types/vector.hpp>
#include <cereal/types/map.hpp>
#include <cereal/types/set.hpp>
#include <cereal/types/memory.hpp>
#include <cereal/types/polymorphic.hpp>
#include <cereal/types/string.hpp>
#include <cereal/archives/json.hpp>
#include <cereal/archives/portable_binary.hpp>

namespace cereal{
    /**
     Funtions to save/load Eigen::Matrix
     **/
    template<class Archive, class _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
    void save(Archive& archive, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> const & X){
        int rows = static_cast<int>(X.rows());
        int cols = static_cast<int>(X.cols());
        archive(CEREAL_NVP(rows));
        archive(CEREAL_NVP(cols));
        std::vector<double> elements(rows*cols);
        Eigen::Map<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>>(&elements[0], rows, cols ) = X;
        archive( CEREAL_NVP(elements));
    }

    template<class Archive, class _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
    void load(Archive & archive, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> & X)
    {
        int rows;
        int cols;
        archive(CEREAL_NVP(rows));
        archive(CEREAL_NVP(cols));
        std::vector<double> elements(rows*cols);
        archive(CEREAL_NVP(elements));
        X = Eigen::Map<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>>(&elements[0], rows, cols);
    }

    /**
     Funtions to save/load Eigen::SparseMatrix
     **/

    template<class Archive, class _Scalar, int _Options, class _StorageIndex>
    void save(Archive& ar, Eigen::SparseMatrix<_Scalar, _Options, _StorageIndex> const & X, const std::uint32_t version){
        // column-major
        typename Eigen::SparseMatrix<_Scalar, _Options, _StorageIndex>::Index rows = X.rows(), cols = X.cols(), nonZeros = X.nonZeros(), outerSize = X.outerSize(), innerSize = X.innerSize();
        ar(CEREAL_NVP(rows));
        ar(CEREAL_NVP(cols));
        ar(CEREAL_NVP(nonZeros));
        ar(CEREAL_NVP(outerSize));
        ar(CEREAL_NVP(innerSize));

        std::vector<_StorageIndex> outerStarts(X.outerIndexPtr(), X.outerIndexPtr() + outerSize + 1);
        std::vector<_StorageIndex> innerIndices(X.innerIndexPtr(), X.innerIndexPtr() + nonZeros);
        std::vector<_Scalar> values(X.valuePtr(), X.valuePtr() + nonZeros);

        ar(CEREAL_NVP(outerStarts));
        ar(CEREAL_NVP(innerIndices));
        ar(CEREAL_NVP(values));
    }

    template<class Archive, class _Scalar, int _Options, class _StorageIndex>
    void load(Archive& ar, Eigen::SparseMatrix<_Scalar, _Options, _StorageIndex> & X, const std::uint32_t version){
        // column-major
        typename Eigen::SparseMatrix<_Scalar, _Options, _StorageIndex>::Index rows = X.rows(), cols = X.cols(), nonZeros = X.nonZeros(), outerSize = X.outerSize(), innerSize = X.innerSize();
        ar(CEREAL_NVP(rows));
        ar(CEREAL_NVP(cols));
        ar(CEREAL_NVP(nonZeros));
        ar(CEREAL_NVP(outerSize));
        ar(CEREAL_NVP(innerSize));

        std::vector<_StorageIndex> outerStarts;
        std::vector<_StorageIndex> innerIndices;
        std::vector<_Scalar> values;

        ar(CEREAL_NVP(outerStarts));
        ar(CEREAL_NVP(innerIndices));
        ar(CEREAL_NVP(values));

        X.resize(rows, cols);
        X.makeCompressed();
        X.resizeNonZeros(nonZeros);

        std::copy(outerStarts.begin(), outerStarts.end(), X.outerIndexPtr());
        std::copy(innerIndices.begin(), innerIndices.end(), X.innerIndexPtr());
        std::copy(values.begin(), values.end(), X.valuePtr());

        X.finalize();
    }

    template <class Archive, class T>
    void make_optional_nvp(Archive &ar, const std::string &name, T &value) {
        try {
            ar(cereal::make_nvp(name, value));
        } catch (cereal::Exception& e) {
            std::cerr << "NOTICE: provided NVP not found (name=" << name << ")" << std::endl;
        }
    }
    #define     OPTIONAL_NVP(A, T)   make_optional_nvp(A, #T, T)
}

CEREAL_CLASS_VERSION(Eigen::SparseMatrix<uint8_t>, 0);
CEREAL_CLASS_VERSION(Eigen::SparseMatrix<double>, 0);

#endif /* EigenSerializeUtils_h */


#include <iostream>
#include <string>
#include <fstream>
#include <vector>
#include <cmath>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include "lexer.hpp"

using namespace boost::spirit;
using namespace boost;

void lexer::lex(std::string filename){
    std::string line;
    readfile.open(filename);
    if(!readfile.is_open()){
        std::cerr << "Can not open file : " << filename << std::endl;
    }
    while(getline(readfile, line)){
        parse_line(line);
    }

}

void lexer::parse_line(std::string line)
{
    std::vector<std::string> resultStr;
    std::string::iterator iter = line.begin(), end = line.end();
    std::vector<int> lineCNF;
    //qi::rule<std::string::iterator, *qi::char_type, qi::space_type> rule = *qi::char_;
//    bool success = qi::phrase_parse(
//        iter,
//        end,
//        *qi::char_,
//        qi::space,
//        resultStr
//    );
    bool success_num = qi::phrase_parse(
        iter,
        end,
        *qi::int_,
        qi::space,
        lineCNF
    );

    bool success = qi::parse(
        iter,
        end,
        *(*(qi::char_ - qi::lit(" ")) >> qi::lit(" ")) >> (*qi::char_ | qi::space),
        resultStr
    );

    if(success){
        //Parse Comments line
        if(resultStr.at(0) == "c"){
            std::cout << "comment line" << std::endl;
        }
        // Parse Problem line
        else if(resultStr.at(0) == "p"){
            std::cout << "definition" << std::endl;
            bool definitions = qi::phrase_parse(
                line.begin(),
                end,
                qi::lit("p") >> qi::lit("cnf") >> ((qi::int_ >> qi::int_)),
                qi::space,
                cnf.numVar,
                cnf.numLiteral
            );
            std::cout << "Number of Variable " << cnf.numVar << " : Number of Literal " << cnf.numLiteral << std::endl;
        }
        //Parse Clauses Line
        else if(lineCNF.size()){
            std::vector<int> lineLit(cnf.numVar+1, 0);
            for(int i = 0; i < lineCNF.size() && lineCNF.at(i) != 0; i++){
                if(lineCNF.at(i) > 0){
                    lineLit.at(lineCNF.at(i)) = 1;
                }
                else{
                    lineLit.at(std::abs(lineCNF.at(i))) = -1;
                }
            }
            cnf.CNF.push_back(lineLit);
        }
    }

}

clauses lexer::getClauses()
{
    return cnf;
}


#include <gtest/gtest.h>
#include <stdexcept>
#include <Eigen/Dense>
#include <ctime>

#include <iostream>

// testing following API
#include "estimation/OutputValue.h"

using namespace std;
using namespace estimation;

#define CLOCK_RESOLUTION	10

namespace OutputValueTest
{
  // -----------------------------------------
  // tests
  // -----------------------------------------
  TEST(OutputValueTest, initialization)
  {
    bool incorrect = false;
    clock_t last;

    // check default value
    for (int i = 0; i < 1000; i++)
    {
      OutputValue empty;
      last = clock();

      EXPECT_DOUBLE_EQ(empty.getValue(), -1);
      EXPECT_DOUBLE_EQ(empty.getVariance(), 0);
      EXPECT_NEAR(empty.getJitter(), 0, CLOCK_RESOLUTION + 0.01);
    }

    OutputValue val(1,2,3);
    clock_t start_time = clock();

    EXPECT_DOUBLE_EQ(val.getValue(), 1);
    EXPECT_DOUBLE_EQ(val.getVariance(), 2);
    EXPECT_NEAR(val.getJitter(), 3, CLOCK_RESOLUTION + 0.01);

    // wait a little bit
    for (int i = 0; i < 10000; i++)
      for (int j = 0; j < 10000; j++);

    clock_t end_time = clock();
    double elapsed_time_ms = (double)(end_time - start_time)*1000/CLOCKS_PER_SEC;

    EXPECT_NEAR(val.getJitter(), 3+elapsed_time_ms, CLOCK_RESOLUTION + 0.01);

    // test setters
    OutputValue fill;
    double value = 0;
    for (int i = 0; i < 1000; i++)
    {
      value += 0.1;

      fill.setValue(value);
      fill.setVariance(0.1);

      EXPECT_DOUBLE_EQ(fill.getValue(), value);
      EXPECT_DOUBLE_EQ(fill.getVariance(), 0.1);
      EXPECT_NEAR(fill.getJitter(), 0, CLOCK_RESOLUTION + 0.01);

      fill.setJitter(20);

      EXPECT_NEAR(fill.getJitter(), 20, CLOCK_RESOLUTION + 0.01);
    }
  }

  TEST(OutputValueTest, operationsOn)
  {
    OutputValue x(-9,1,2);
    OutputValue y(-1,0,1);
    OutputValue temp;

    // equality
    temp = x;
    EXPECT_DOUBLE_EQ(temp.getValue(), x.getValue());
    EXPECT_DOUBLE_EQ(temp.getVariance(), x.getVariance());
    EXPECT_NEAR(temp.getJitter(), x.getJitter(), CLOCK_RESOLUTION + 0.01);

    EXPECT_TRUE(temp == x);
    EXPECT_FALSE(temp == y);
    EXPECT_TRUE(temp != y);

    // <,>
    EXPECT_TRUE(x < y);
    EXPECT_TRUE(y > x);
    EXPECT_FALSE(x < temp);
    EXPECT_FALSE(y < x);

    // <=,>=
    EXPECT_TRUE(temp <= x);
    EXPECT_TRUE(x <= temp);
    EXPECT_TRUE(temp >= x);
    EXPECT_TRUE(x >= temp);
    EXPECT_TRUE(x <= y);
    EXPECT_FALSE(x >= y);
  }
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/mindenormal.hpp>
#include <boost/simd/as.hpp>
#include <scalar_test.hpp>

STF_CASE_TPL( "Check mindenormal behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::mindenormal;
  using boost::simd::Mindenormal;

  STF_TYPE_IS(decltype(Mindenormal<T>()), T);
  STF_EQUAL(Mindenormal<T>(), T(1));
  STF_EQUAL(mindenormal( as(T{}) ),T(1));
}

STF_CASE_TPL( "Check mindenormal behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::mindenormal;
  using boost::simd::Mindenormal;

  STF_TYPE_IS(decltype(Mindenormal<T>()), T);
  STF_IEEE_EQUAL(Mindenormal<T>(), 4.940656458412465e-324);
  STF_IEEE_EQUAL(mindenormal( as(T{}) ), 4.940656458412465e-324);
}
STF_CASE_TPL( "Check mindenormal behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::mindenormal;
  using boost::simd::Mindenormal;

  STF_TYPE_IS(decltype(Mindenormal<T>()), T);
  STF_IEEE_EQUAL(Mindenormal<T>(), 1.4012985e-45f);
  STF_IEEE_EQUAL(mindenormal( as(T{}) ), 1.4012985e-45f);
}


// Copyright John Maddock 2006, 2007.
// Copyright Paul A. Bristow 2008.

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_DISTRIBUTIONS_CHI_SQUARED_HPP
#define BOOST_MATH_DISTRIBUTIONS_CHI_SQUARED_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/gamma.hpp> // for incomplete beta.
#include <boost/math/distributions/complement.hpp> // complements
#include <boost/math/distributions/detail/common_error_handling.hpp> // error checks
#include <boost/math/special_functions/fpclassify.hpp>

#include <utility>

namespace boost{ namespace math{

template <class RealType = double, class Policy = policies::policy<> >
class chi_squared_distribution
{
public:
   typedef RealType value_type;
   typedef Policy policy_type;

   chi_squared_distribution(RealType i) : m_df(i)
   {
      RealType result;
      detail::check_df(
         "boost::math::chi_squared_distribution<%1%>::chi_squared_distribution", m_df, &result, Policy());
   } // chi_squared_distribution

   RealType degrees_of_freedom()const
   {
      return m_df;
   }

   // Parameter estimation:
   static RealType find_degrees_of_freedom(
      RealType difference_from_variance,
      RealType alpha,
      RealType beta,
      RealType variance,
      RealType hint = 100);

private:
   //
   // Data member:
   //
   RealType m_df;  // degrees of freedom are a real number.
}; // class chi_squared_distribution

typedef chi_squared_distribution<double> chi_squared;

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const chi_squared_distribution<RealType, Policy>& /*dist*/)
{ // Range of permissible values for random variable x.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(0, max_value<RealType>()); // 0 to + infinity.
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const chi_squared_distribution<RealType, Policy>& /*dist*/)
{ // Range of supported values for random variable x.
   // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
   return std::pair<RealType, RealType>(0, tools::max_value<RealType>()); // 0 to + infinity.
}

template <class RealType, class Policy>
RealType pdf(const chi_squared_distribution<RealType, Policy>& dist, const RealType& chi_square)
{
   BOOST_MATH_STD_USING  // for ADL of std functions
   RealType degrees_of_freedom = dist.degrees_of_freedom();
   // Error check:
   RealType error_result;

   static const char* function = "boost::math::pdf(const chi_squared_distribution<%1%>&, %1%)";

   if(false == detail::check_df(
         function, degrees_of_freedom, &error_result, Policy()))
      return error_result;

   if((chi_square < 0) || !(boost::math::isfinite)(chi_square))
   {
      return policies::raise_domain_error<RealType>(
         function, "Chi Square parameter was %1%, but must be > 0 !", chi_square, Policy());
   }

   if(chi_square == 0)
   {
      // Handle special cases:
      if(degrees_of_freedom < 2)
      {
         return policies::raise_overflow_error<RealType>(
            function, 0, Policy());
      }
      else if(degrees_of_freedom == 2)
      {
         return 0.5f;
      }
      else
      {
         return 0;
      }
   }

   return gamma_p_derivative(degrees_of_freedom / 2, chi_square / 2, Policy()) / 2;
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const chi_squared_distribution<RealType, Policy>& dist, const RealType& chi_square)
{
   RealType degrees_of_freedom = dist.degrees_of_freedom();
   // Error check:
   RealType error_result;
   static const char* function = "boost::math::cdf(const chi_squared_distribution<%1%>&, %1%)";

   if(false == detail::check_df(
         function, degrees_of_freedom, &error_result, Policy()))
      return error_result;

   if((chi_square < 0) || !(boost::math::isfinite)(chi_square))
   {
      return policies::raise_domain_error<RealType>(
         function, "Chi Square parameter was %1%, but must be > 0 !", chi_square, Policy());
   }

   return boost::math::gamma_p(degrees_of_freedom / 2, chi_square / 2, Policy());
} // cdf

template <class RealType, class Policy>
inline RealType quantile(const chi_squared_distribution<RealType, Policy>& dist, const RealType& p)
{
   RealType degrees_of_freedom = dist.degrees_of_freedom();
   static const char* function = "boost::math::quantile(const chi_squared_distribution<%1%>&, %1%)";
   // Error check:
   RealType error_result;
   if(false == detail::check_df(
         function, degrees_of_freedom, &error_result, Policy())
         && detail::check_probability(
            function, p, &error_result, Policy()))
      return error_result;

   return 2 * boost::math::gamma_p_inv(degrees_of_freedom / 2, p, Policy());
} // quantile

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<chi_squared_distribution<RealType, Policy>, RealType>& c)
{
   RealType const& degrees_of_freedom = c.dist.degrees_of_freedom();
   RealType const& chi_square = c.param;
   static const char* function = "boost::math::cdf(const chi_squared_distribution<%1%>&, %1%)";
   // Error check:
   RealType error_result;
   if(false == detail::check_df(
         function, degrees_of_freedom, &error_result, Policy()))
      return error_result;

   if((chi_square < 0) || !(boost::math::isfinite)(chi_square))
   {
      return policies::raise_domain_error<RealType>(
         function, "Chi Square parameter was %1%, but must be > 0 !", chi_square, Policy());
   }

   return boost::math::gamma_q(degrees_of_freedom / 2, chi_square / 2, Policy());
}

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<chi_squared_distribution<RealType, Policy>, RealType>& c)
{
   RealType const& degrees_of_freedom = c.dist.degrees_of_freedom();
   RealType const& q = c.param;
   static const char* function = "boost::math::quantile(const chi_squared_distribution<%1%>&, %1%)";
   // Error check:
   RealType error_result;
   if(false == detail::check_df(
         function, degrees_of_freedom, &error_result, Policy())
         && detail::check_probability(
            function, q, &error_result, Policy()))
      return error_result;

   return 2 * boost::math::gamma_q_inv(degrees_of_freedom / 2, q, Policy());
}

template <class RealType, class Policy>
inline RealType mean(const chi_squared_distribution<RealType, Policy>& dist)
{ // Mean of Chi-Squared distribution = v.
  return dist.degrees_of_freedom();
} // mean

template <class RealType, class Policy>
inline RealType variance(const chi_squared_distribution<RealType, Policy>& dist)
{ // Variance of Chi-Squared distribution = 2v.
  return 2 * dist.degrees_of_freedom();
} // variance

template <class RealType, class Policy>
inline RealType mode(const chi_squared_distribution<RealType, Policy>& dist)
{
   RealType df = dist.degrees_of_freedom();
   static const char* function = "boost::math::mode(const chi_squared_distribution<%1%>&)";
   // Most sources only define mode for df >= 2,
   // but for 0 <= df <= 2, the pdf maximum actually occurs at random variate = 0;
   // So one could extend the definition of mode thus:
   //if(df < 0)
   //{
   //   return policies::raise_domain_error<RealType>(
   //      function,
   //      "Chi-Squared distribution only has a mode for degrees of freedom >= 0, but got degrees of freedom = %1%.",
   //      df, Policy());
   //}
   //return (df <= 2) ? 0 : df - 2;

   if(df < 2)
      return policies::raise_domain_error<RealType>(
         function,
         "Chi-Squared distribution only has a mode for degrees of freedom >= 2, but got degrees of freedom = %1%.",
         df, Policy());
   return df - 2;
}

//template <class RealType, class Policy>
//inline RealType median(const chi_squared_distribution<RealType, Policy>& dist)
//{ // Median is given by Quantile[dist, 1/2]
//   RealType df = dist.degrees_of_freedom();
//   if(df <= 1)
//      return tools::domain_error<RealType>(
//         BOOST_CURRENT_FUNCTION,
//         "The Chi-Squared distribution only has a mode for degrees of freedom >= 2, but got degrees of freedom = %1%.",
//         df);
//   return df - RealType(2)/3;
//}
// Now implemented via quantile(half) in derived accessors.

template <class RealType, class Policy>
inline RealType skewness(const chi_squared_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING // For ADL
   RealType df = dist.degrees_of_freedom();
   return sqrt (8 / df);  // == 2 * sqrt(2 / df);
}

template <class RealType, class Policy>
inline RealType kurtosis(const chi_squared_distribution<RealType, Policy>& dist)
{
   RealType df = dist.degrees_of_freedom();
   return 3 + 12 / df;
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const chi_squared_distribution<RealType, Policy>& dist)
{
   RealType df = dist.degrees_of_freedom();
   return 12 / df;
}

//
// Parameter estimation comes last:
//
namespace detail
{

template <class RealType, class Policy>
struct df_estimator
{
   df_estimator(RealType a, RealType b, RealType variance, RealType delta)
      : alpha(a), beta(b), ratio(delta/variance) {}

   RealType operator()(const RealType& df)
   {
      if(df <= tools::min_value<RealType>())
         return 1;
      chi_squared_distribution<RealType, Policy> cs(df);

      RealType result;
      if(ratio > 0)
      {
         RealType r = 1 + ratio;
         result = cdf(cs, quantile(complement(cs, alpha)) / r) - beta;
      }
      else
      {
         RealType r = 1 + ratio;
         result = cdf(complement(cs, quantile(cs, alpha) / r)) - beta;
      }
      return result;
   }
private:
   RealType alpha, beta, ratio;
};

} // namespace detail

template <class RealType, class Policy>
RealType chi_squared_distribution<RealType, Policy>::find_degrees_of_freedom(
   RealType difference_from_variance,
   RealType alpha,
   RealType beta,
   RealType variance,
   RealType hint)
{
   static const char* function = "boost::math::chi_squared_distribution<%1%>::find_degrees_of_freedom(%1%,%1%,%1%,%1%,%1%)";
   // Check for domain errors:
   RealType error_result;
   if(false == detail::check_probability(
         function, alpha, &error_result, Policy())
         && detail::check_probability(function, beta, &error_result, Policy()))
      return error_result;

   if(hint <= 0)
      hint = 1;

   detail::df_estimator<RealType, Policy> f(alpha, beta, variance, difference_from_variance);
   tools::eps_tolerance<RealType> tol(policies::digits<RealType, Policy>());
   boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
   std::pair<RealType, RealType> r = tools::bracket_and_solve_root(f, hint, RealType(2), false, tol, max_iter, Policy());
   RealType result = r.first + (r.second - r.first) / 2;
   if(max_iter >= policies::get_max_root_iterations<Policy>())
   {
      policies::raise_evaluation_error<RealType>(function, "Unable to locate solution in a reasonable time:"
         " either there is no answer to how many degrees of freedom are required"
         " or the answer is infinite.  Current best guess is %1%", result, Policy());
   }
   return result;
}

} // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_MATH_DISTRIBUTIONS_CHI_SQUARED_HPP


#include "main.hpp"

#include <cassert>
#include <deque>
#include <functional>
#include <sstream>
#include <unordered_map>

#include <readline/history.h>
#include <readline/readline.h>
// GNU readline defines NEWLINE macro, but this conflicts with TOK::NEWLINE
#undef NEWLINE

#include <boost/algorithm/string/trim.hpp>
#include <boost/format.hpp>
#include <boost/iostreams/concepts.hpp>
#include <boost/iostreams/stream.hpp>

#define AQ1_RANGE(cont) std::begin(cont), std::end(cont)

#define AQ1_ASSERT(cond, msg)    \
    do {                         \
        assert((cond) && (msg)); \
    } while (0);

template <class T>
[[noreturn]] void error(T msg)
{
    std::cerr << "[ERROR]\t" << msg << std::endl;
    std::terminate();
}

[[noreturn]] void unreachable(const char *msg)
{
    AQ1_ASSERT(false, msg);
}

// Convert fractions to floating points
// TODO: Much smarter way?
MPFloat r2f(const MPRational &r)
{
    return MPFloat{r.numerator().str()} / MPFloat{r.denominator().str()};
}

MPInt f2i(MPFloat f)
{
    return MPInt{MPFloat{mp::floor(f)}.str()};
}

MPFloat i2f(MPInt i)
{
    return MPFloat{i.str()};
}

MPRational f2r(const MPFloat &f)
{
    // TODO: Is that correct?
    MPInt base{10};
    base = mp::pow(base, 100);  // 10^100
    return MPRational{f2i(f * i2f(base)), base};
}

int Lex::getch()
{
    int ch = is_.get();
    if (ch != EOF) history_.push_back(ch);
    return ch;
}

void Lex::putback(int ch)
{
    if (!history_.empty()) history_.pop_back();
    is_.putback(ch);
}

std::string Lex::clear_history()
{
    std::string ret{AQ1_RANGE(history_)};
    std::vector<char>{}.swap(history_);
    return ret;
}

Token Lex::next_token()
{
    if (pending_) {
        Token ret = *pending_;
        pending_.reset();
        return ret;
    }

    int ch;
    do {
        AQ1_ASSERT(is_.good(), "Invalid input stream.");
        ch = getch();
        if (ch == EOF) return Token::owari();  // TOKEN NO OWARI
    } while (isspace(ch) && ch != '\r' && ch != '\n');

    // When numeric literal
    if (isdigit(ch)) {
        MPRational n = ch - '0';
        while (isdigit(ch = getch())) n = n * 10 + ch - '0';
        if (ch == '.') {
            MPRational digit = 1;
            while (isdigit(ch = getch())) {
                digit /= 10;
                n += digit * (ch - '0');
            }
        }
        putback(ch);
        return {TOK::NUMLIT, n};
    }

    // When identifier
    if (isalpha(ch)) {
        std::stringstream ss;
        ss << (char)ch;
        while (isalnum(ch = getch())) ss << (char)ch;
        putback(ch);
        return {TOK::IDENT, ss.str()};
    }

    // When format literal
    if (ch == '$') {
        int field_width = 0, precision = 0;
        while (isdigit(ch = getch()))
            field_width = field_width * 10 + (ch - '0');
        if (ch != '.')
            error("Format literal should be formated as '$\\d*\\.\\d*'");
        while (isdigit(ch = getch())) precision = precision * 10 + (ch - '0');
        putback(ch);
        return {TOK::FMTLIT,
                FormatLiteral{false, false, field_width, precision}};
    }

    switch (ch) {
    case '+':
        return {TOK::PLUS};
    case '-':
        return {TOK::MINUS};
    case '*':
        return {TOK::STAR};
    case '/':
        return {TOK::SLASH};
    case '(':
        return {TOK::LPAREN};
    case ')':
        return {TOK::RPAREN};
    case ',':
        return {TOK::COMMA};
    case '%':
        return {TOK::PERCENT};
    case '\n':
        return {TOK::NEWLINE};
    case '\r': {
        ch = getch();
        if (ch != '\n') putback(ch);
        return {TOK::NEWLINE};
    }
    }

    // No valid token
    putback(ch);
    return Token::owari();
}

Token Lex::get()
{
    while (true) {
        Token tok = next_token();
        if (tok.kind != TOK::NEWLINE) return tok;
    }
}

Token Lex::expect(TOK kind)
{
    Token tok = get();
    if (tok.kind != kind)
        error("Unexpected token: got \"%1%\", but expect \"%2%\"");
    return tok;
}

Token Lex::peek_next()
{
    if (!pending_) pending_ = next_token();
    return *pending_;
}

bool Lex::match(TOK kind)
{
    if (!pending_ || pending_->kind == TOK::NEWLINE) pending_ = get();
    return pending_->kind == kind;
}

MPRational UnaryOp::eval() const
{
    MPRational src = src_->eval();

    switch (kind_) {
    case UNARYOP::PLUS:
        return src;
    case UNARYOP::MINUS:
        return -src;
    }

    unreachable("Invalid unaryop's kind");
}

MPRational BinOp::eval() const
{
    MPRational lhs = lhs_->eval(), rhs = rhs_->eval();

    switch (kind_) {
    case BINOP::ADD:
        return lhs + rhs;
    case BINOP::SUB:
        return lhs - rhs;
    case BINOP::MUL:
        return lhs * rhs;
    case BINOP::DIV:
        return lhs / rhs;
    }

    unreachable("Invalid binop's kind");
}

MPRational FuncCall::eval() const
{
    std::unordered_map<std::string,
                       std::pair<size_t, std::function<MPRational(
                                             const std::vector<MPRational> &)>>>
        functbl = {
            {"sqrt",
             {1,
              [](auto &&args) {
                  return f2r(mp::sqrt(i2f(args[0].numerator()))) /
                         f2r(mp::sqrt(i2f(args[0].denominator())));
              }}},
            {"floor",
             {1, [](auto &&args) { return f2r(mp::floor(r2f(args[0]))); }}},
            {"scale", {2, [](auto &&args) {
                           MPFloat base = mp::pow(10, r2f(args[0]));
                           return f2r(mp::floor(r2f(args[1]) * base) / base);
                       }}}};

    auto it = functbl.find(name_);
    if (it != functbl.end()) {
        if (args_.size() != it->second.first)
            error("Invalid number of arguments for \"%1%\"");

        std::vector<MPRational> args;
        for (auto &&arg : args_) args.push_back(arg->eval());

        return it->second.second(args);
    }

    error(boost::format("Invalid function name \"%1%\"") % name_);
}

MPRational FormatPrint::eval() const
{
    MPRational src{src_->eval()};
    std::string strsrc{r2f(src).str()};

    if (precision_ == -1) {  // No format
        std::cout << strsrc << std::endl;
        return src;
    }

    unsigned int i = 0;
    // Before period
    for (; i < strsrc.size(); i++) {
        if (strsrc[i] == '.') break;
        std::cout << strsrc[i];
    }
    // After period
    if (precision_ != 0) {
        for (int j = -1; i < strsrc.size() && j < precision_; i++, j++)
            std::cout << strsrc[i];
    }
    std::cout << std::endl;

    return src;
}

ASTNodePtr Parser::parse_primary()
{
    Token tok = lex_.get();

    switch (tok.kind) {
    case TOK::NUMLIT:
        return std::make_shared<NumImm>(std::get<MPRational>(tok.data));
    case TOK::LPAREN: {
        ASTNodePtr ast = parse_expr();
        lex_.expect(TOK::RPAREN);
        return ast;
    }
    case TOK::IDENT: {  // function call
        std::string funcname = std::get<std::string>(tok.data);
        std::vector<ASTNodePtr> args;

        lex_.expect(TOK::LPAREN);
        if (!lex_.match(TOK::RPAREN)) {
            args.push_back(parse_expr());
            while (lex_.match(TOK::COMMA)) {
                lex_.get();  // Eat TOK::COMMA
                args.push_back(parse_expr());
            }
        }
        lex_.expect(TOK::RPAREN);

        ASTNodePtr ast = std::make_shared<FuncCall>(funcname, args);
        return ast;
    }
    default:
        error("Expect primary token");
    }
}

ASTNodePtr Parser::parse_unary()
{
    if (lex_.match(TOK::PLUS)) {
        lex_.get();  // Eat TOK::PLUS
        return std::make_shared<UnaryOp>(UNARYOP::PLUS, parse_primary());
    }
    if (lex_.match(TOK::MINUS)) {
        lex_.get();  // Eat TOK::MINUS
        return std::make_shared<UnaryOp>(UNARYOP::MINUS, parse_primary());
    }
    return parse_primary();
}

ASTNodePtr Parser::parse_multiplicative()
{
    ASTNodePtr lhs = parse_unary();

    while (true) {
        switch (lex_.peek_next().kind) {
        case TOK::STAR: {
            lex_.get();  // Eat TOK::STAR
            ASTNodePtr rhs = parse_unary();
            lhs = std::make_shared<BinOp>(BINOP::MUL, lhs, rhs);
        }
            continue;

        case TOK::SLASH: {
            lex_.get();  // Eat TOK::SLASH
            ASTNodePtr rhs = parse_unary();
            lhs = std::make_shared<BinOp>(BINOP::DIV, lhs, rhs);
        }
            continue;

        default:
            break;
        }
        break;
    }

    return lhs;
}

ASTNodePtr Parser::parse_additive()
{
    ASTNodePtr lhs = parse_multiplicative();

    while (true) {
        switch (lex_.peek_next().kind) {
        case TOK::PLUS: {
            lex_.get();  // Eat TOK::PLUS
            ASTNodePtr rhs = parse_multiplicative();
            lhs = std::make_shared<BinOp>(BINOP::ADD, lhs, rhs);
        }
            continue;

        case TOK::MINUS: {
            lex_.get();  // Eat TOK::MINUS
            ASTNodePtr rhs = parse_multiplicative();
            lhs = std::make_shared<BinOp>(BINOP::SUB, lhs, rhs);
        }
            continue;

        default:
            break;
        }
        break;
    }

    return lhs;
}

ASTNodePtr Parser::parse_expr()
{
    return parse_additive();
}

ASTNodePtr Parser::parse()
{
    if (lex_.match(TOK::FMTLIT)) {
        FormatLiteral fmtlit = std::get<FormatLiteral>(lex_.get().data);
        return std::make_shared<FormatPrint>(parse_expr(), fmtlit.field_width,
                                             fmtlit.precision);
    }

    return std::make_shared<FormatPrint>(parse_expr(), -1, -1);
}

class StreamLine : public boost::iostreams::source {
private:
    std::deque<char> buffer_;

public:
    StreamLine()
    {
    }

    std::streamsize read(char *s, std::streamsize n)
    {
        assert(n >= 1);

        if (!isatty()) {  // When non-interactive (batch mode)
            int ch = std::cin.get();
            if (ch == EOF) return -1;
            s[0] = ch;
            return 1;
        }

        if (buffer_.empty()) {
            std::shared_ptr<char> buf =
                std::shared_ptr<char>{::readline(">> ")};
            if (!buf || *buf == '\0') return -1;  // EOF
            std::string line{buf.get()};
            std::copy(AQ1_RANGE(line), std::back_inserter(buffer_));
            buffer_.push_back('\n');
        }

        assert(!buffer_.empty());

        s[0] = buffer_.front();
        buffer_.pop_front();
        return 1;
    }

    static void add_history(const std::string &src)
    {
        ::add_history(src.c_str());
    }

    static bool isatty()
    {
        return ::isatty(0);
    }
};

int main(int argc, char **argv)
{
    boost::iostreams::stream<StreamLine> in{StreamLine{}};
    Lex lex{in};

    if (!StreamLine::isatty()) {  // When non-interactive (batch mode)
        Parser{lex}.parse()->eval();
        return 0;
    }

    while (true) {
        ASTNodePtr node = Parser{lex}.parse();
        StreamLine::add_history(
            boost::algorithm::trim_copy(lex.clear_history()));
        node->eval();
    }
}


//=======================================================================
// Copyright 2015 by Ireneusz Szcześniak
// Authors: Ireneusz Szcześniak <www.irkos.org>
//
// 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)
//=======================================================================

//=======================================================================
// This is the implementation of the Yen algorithm:
//
// Jin Y. Yen, Finding the k shortest loopless paths in a network,
// Management Science, vol. 17, no. 11, July 1971, pages 712-716
//
// But actually, I found the following explanation better:
//
// https://en.wikipedia.org/wiki/Yen%27s_algorithm
//=======================================================================

#ifndef BOOST_GRAPH_YEN_KSP
#define BOOST_GRAPH_YEN_KSP

#include <list>
#include <set>
#include <map>

#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/optional.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/utility/value_init.hpp>

#include "custom_dijkstra_call.hpp"
#include "exclude_filter.hpp"

namespace boost {

  template <typename Graph, typename WeightMap, typename IndexMap>
  std::list<std::pair<typename WeightMap::value_type,
                      std::list<typename Graph::edge_descriptor>>>
  yen_ksp(const Graph& g,
          typename Graph::vertex_descriptor s,
          typename Graph::vertex_descriptor t,
          WeightMap wm, IndexMap im, optional<unsigned> K)
  {
    typedef typename Graph::vertex_descriptor vertex_descriptor;
    typedef typename Graph::edge_descriptor edge_descriptor;
    typedef typename WeightMap::value_type weight_type;
    typedef exclude_filter<vertex_descriptor> evf_type;
    typedef exclude_filter<edge_descriptor> eef_type;
    typedef std::list<edge_descriptor> path_type;
    typedef std::pair<weight_type, path_type> kr_type;

    // The results.
    std::list<kr_type> A;
    // The tentative results.
    std::multimap<weight_type, path_type> C;

    // The set of excluded edges.
    std::set<edge_descriptor> exe;
    // The set of excluded vertexes.
    std::set<vertex_descriptor> exv;

    // The filter which excludes edges.
    eef_type ef(&exe);
    // The filter which excludes vertexes.
    evf_type vf(&exv);

    // The filtered graph type.
    typedef boost::filtered_graph<Graph, eef_type, evf_type> fg_type;

    // The filtered graph.
    fg_type fg(g, ef, vf);

    optional<kr_type> okr = custom_dijkstra_call(g, s, t, wm, im);

    if (okr)
      {
        A.push_back(okr.get());

        for (int k = 1; !K || k < K.get(); ++k)
          {
            // The previous shortest result.
            const kr_type &psr = A.back();
            const path_type &psp = psr.second;

            // Iterate over the edges of the previous shortest path.
            for(auto i = psp.begin(); i != psp.end(); ++i)
              {
                // The spur vertex.
                vertex_descriptor sv = source(*i, g);
                // The root path.
                path_type rp = path_type(psp.begin(), i);

                // Iterate over the previous shortest paths.
                for(const auto &kr: A)
                  {
                    const path_type &kp = kr.second;
                    typename path_type::const_iterator i1, i2;

                    // Iterate as long as possible, and as long as
                    // paths are equal.
                    for(tie(i1, i2) = std::make_pair(kp.begin(), rp.begin());
                        i1 != kp.end() && i2 != rp.end() && *i1 == *i2;
                        ++i1, ++i2);

                    // Make sure we didn't reach the end of kp.  If we
                    // did, there is no next edge in kp, which we
                    // could exclude.  Also, make sure we reached the
                    // end of rp, i.e., the kp begins with rp.
                    if (i1 != kp.end() && i2 == rp.end())
                      exe.insert(*i1);
                  }

                // Remove the vertexes that belong to the root path,
                // except the last vertex, i.e., the spur node.
                for (const auto &e: rp)
                  exv.insert(source(e, g));

                // Optional spur result.
                optional<kr_type> osr = custom_dijkstra_call(fg, sv, t, wm, im);

                if (osr)
                  {
                    // The tentative result.
                    kr_type tr = osr.get();
                    for(const auto &e: boost::adaptors::reverse(rp))
                      {
                        tr.second.push_front(e);
                        tr.first += get(wm, e);
                      }
                    C.insert(tr);
                  }

                // Clear the excluded edges and vertexes.
                exe.clear();
                exv.clear();
              }

            // Stop searching when there are no tentative paths.
            if (C.empty())
              break;

            // Take the shortest tentative path and make it the next
            // shortest path.
            A.push_back(*C.begin());
            C.erase(C.begin());
          }
      }

    return A;
  }

  template <typename Graph>
  std::list<std::pair<typename property_map<Graph, edge_weight_t>::value_type,
                      std::list<typename Graph::edge_descriptor>>>
  yen_ksp(Graph& g,
          typename Graph::vertex_descriptor s,
          typename Graph::vertex_descriptor t,
          optional<unsigned> K = optional<unsigned>())
  {
    return yen_ksp(g, s, t, get(edge_weight_t(), g),
                   get(vertex_index_t(), g), K);
  }

} // boost

#endif /* BOOST_GRAPH_YEN_KSP */


// Copyright (C) by Josh Blum. See LICENSE.txt for licensing information.

#include <gras/time_tag.hpp>
#include <PMC/Containers.hpp>
#include <boost/cstdint.hpp> //uint64
#include <boost/math/special_functions/round.hpp>

#define TIME_TAG_TPS time_ticks_t(1000000000UL)

using namespace gras;

static TimeTag &normalize(TimeTag &t)
{
    int num = int(t._ticks/TIME_TAG_TPS);
    if (num < 0) num--; //stops negative ticks
    t._fsecs += num;
    t._ticks -= num*TIME_TAG_TPS;
    return t;
}

TimeTag::TimeTag(void):
    _fsecs(0), _ticks(0)
{/*NOP*/}

TimeTag TimeTag::from_ticks(const time_ticks_t ticks)
{
    TimeTag t;
    t._ticks = ticks;
    return normalize(t);
}

TimeTag TimeTag::from_ticks(const time_ticks_t ticks, const double rate)
{
    TimeTag t;
    t._fsecs = time_ticks_t(ticks/rate);
    const double error = ticks - (t._fsecs*rate);
    t._ticks = boost::math::llround((error*TIME_TAG_TPS)/rate);
    return normalize(t);
}

TimeTag TimeTag::from_pmc(const PMCC &p)
{
    TimeTag t;
    const PMCTuple<2> &tuple = p.as<PMCTuple<2> >();
    t._fsecs = tuple[0].as<boost::uint64_t>();
    t._ticks = boost::math::llround(tuple[1].as<double>()*TIME_TAG_TPS);
    return normalize(t);
}

time_ticks_t TimeTag::to_ticks(void) const
{
    return _fsecs*TIME_TAG_TPS + _ticks;
}

time_ticks_t TimeTag::to_ticks(const double rate) const
{
    const time_ticks_t full = time_ticks_t(_fsecs*rate);
    const double error = _fsecs - (full/rate);
    return full + boost::math::llround(_ticks*rate/TIME_TAG_TPS + error*rate);
}

PMCC TimeTag::to_pmc(void) const
{
    PMCTuple<2> tuple;
    tuple[0] = PMC_M<boost::uint64_t>(_fsecs);
    tuple[1] = PMC_M<double>(_ticks/double(TIME_TAG_TPS));
    return PMC_M(tuple);
}

TimeTag &TimeTag::operator+=(const TimeTag &rhs)
{
    _fsecs += rhs._fsecs;
    _ticks += rhs._ticks;
    return normalize(*this);
}

TimeTag &TimeTag::operator-=(const TimeTag &rhs)
{
    _fsecs -= rhs._fsecs;
    _ticks -= rhs._ticks;
    return normalize(*this);
}

bool gras::operator<(const TimeTag &lhs, const TimeTag &rhs)
{
    if (lhs._fsecs == rhs._fsecs) return lhs._ticks < rhs._ticks;
    return lhs._fsecs < rhs._fsecs;
}

bool gras::operator==(const TimeTag &lhs, const TimeTag &rhs)
{
    return (lhs._fsecs == rhs._fsecs) and (lhs._ticks == rhs._ticks);
}


#ifndef VIENNACL_LINALG_QR_METHOD_COMMON_HPP
#define VIENNACL_LINALG_QR_METHOD_COMMON_HPP

/* =========================================================================
   Copyright (c) 2010-2012, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

#include <cmath>

#include "viennacl/linalg/kernels/svd_kernels.h"
#include "viennacl/meta/result_of.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"

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

/** @file viennacl/linalg/qr-method-common.hpp
    @brief Common routines used for the QR method and SVD. Experimental.
*/

namespace viennacl
{
  namespace linalg
  {
    const std::string SVD_BIDIAG_PACK_KERNEL = "bidiag_pack";
    const std::string SVD_HOUSEHOLDER_UPDATE_A_LEFT_KERNEL = "house_update_A_left";
    const std::string SVD_HOUSEHOLDER_UPDATE_A_RIGHT_KERNEL = "house_update_A_right";
    const std::string SVD_HOUSEHOLDER_UPDATE_QL_KERNEL = "house_update_QL";
    const std::string SVD_HOUSEHOLDER_UPDATE_QR_KERNEL = "house_update_QR";
    const std::string SVD_COPY_COL_KERNEL = "copy_col";
    const std::string SVD_COPY_ROW_KERNEL = "copy_row";
    const std::string SVD_MATRIX_TRANSPOSE_KERNEL = "transpose_inplace";
    const std::string SVD_INVERSE_SIGNS_KERNEL = "inverse_signs";
    const std::string SVD_GIVENS_PREV_KERNEL = "givens_prev";
    const std::string SVD_GIVENS_NEXT_KERNEL = "givens_next";
    const std::string SVD_FINAL_ITER_UPDATE_KERNEL = "final_iter_update";
    const std::string SVD_UPDATE_QR_COLUMN_KERNEL = "update_qr_column";

    namespace detail
    {
      //static const float EPS = 0.00001f;
      //static const std::size_t ITER_MAX = 50;

      static const double EPS = 1e-10;
      static const std::size_t ITER_MAX = 50;

      template <typename SCALARTYPE>
      SCALARTYPE pythag(SCALARTYPE a, SCALARTYPE b)
      {
        return std::sqrt(a*a + b*b);
      }

      template <typename SCALARTYPE>
      SCALARTYPE sign(SCALARTYPE val)
      {
          return (val >= 0) ? SCALARTYPE(1) : SCALARTYPE(-1);
      }

      // DEPRECATED: Replace with viennacl::linalg::norm_2
      template <typename VectorType>
      typename VectorType::value_type norm_lcl(VectorType const & x, unsigned int size)
      {
        typename VectorType::value_type x_norm = 0.0;
        for(std::size_t i = 0; i < size; i++)
          x_norm += std::pow(x[i], 2);
        return std::sqrt(x_norm);
      }

      template <typename VectorType>
      void normalize(VectorType & x, unsigned int size)
      {
        typename VectorType::value_type x_norm = norm_lcl(x, size);
        for(std::size_t i = 0; i < size; i++)
            x[i] /= x_norm;
      }

      template <typename VectorType>
      void householder_vector(VectorType & v, unsigned int start)
      {
        typedef typename VectorType::value_type    ScalarType;
        ScalarType x_norm = norm_lcl(v, v.size());
        ScalarType alpha = -sign(v[start]) * x_norm;
        v[start] += alpha;
        normalize(v, v.size());
      }

      template <typename MatrixType>
      void transpose(MatrixType & A)
      {
        typedef typename MatrixType::value_type                                   ScalarType;
        typedef typename viennacl::result_of::cpu_value_type<ScalarType>::type    CPU_ScalarType;

        viennacl::ocl::kernel & kernel = viennacl::ocl::get_kernel(viennacl::linalg::kernels::svd<CPU_ScalarType, 1>::program_name(), SVD_MATRIX_TRANSPOSE_KERNEL);

        viennacl::ocl::enqueue(kernel(A,
                                      static_cast<cl_uint>(A.internal_size1()),
                                      static_cast<cl_uint>(A.internal_size2())
                                     )
                              );
      }

      template <typename T>
      void cdiv(T xr, T xi, T yr, T yi, T& cdivr, T& cdivi)
      {
          // Complex scalar division.
          T r;
          T d;
          if (std::fabs(yr) > std::fabs(yi))
          {
              r = yi / yr;
              d = yr + r * yi;
              cdivr = (xr + r * xi) / d;
              cdivi = (xi - r * xr) / d;
          }
          else
          {
              r = yr / yi;
              d = yi + r * yr;
              cdivr = (r * xr + xi) / d;
              cdivi = (r * xi - xr) / d;
          }
      }

      template <typename SCALARTYPE, unsigned int ALIGNMENT>
      void copy_vec(viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT>& A,
                    viennacl::vector<SCALARTYPE, ALIGNMENT>& V,
                    std::size_t row_start,
                    std::size_t col_start,
                    bool copy_col
      )
      {

        std::string kernel_name = copy_col ? SVD_COPY_COL_KERNEL : SVD_COPY_ROW_KERNEL;
        viennacl::ocl::kernel& kernel = viennacl::ocl::get_kernel(viennacl::linalg::kernels::svd<SCALARTYPE, 1>::program_name(),
                                                                  kernel_name);

        viennacl::ocl::enqueue(kernel(
                                      A,
                                      V,
                                      static_cast<cl_uint>(row_start),
                                      static_cast<cl_uint>(col_start),
                                      copy_col ? static_cast<cl_uint>(A.size1())
                                               : static_cast<cl_uint>(A.size2()),
                                      static_cast<cl_uint>(A.internal_size2())
                              ));

      }

      template<typename SCALARTYPE, unsigned int ALIGNMENT>
      void prepare_householder_vector(
                                    viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT>& A,
                                    viennacl::vector<SCALARTYPE, ALIGNMENT>& D,
                                    std::size_t size,
                                    std::size_t row_start,
                                    std::size_t col_start,
                                    std::size_t start,
                                    bool is_column
                                    )
      {
        boost::numeric::ublas::vector<SCALARTYPE> tmp(size, 0);

        copy_vec(A, D, row_start, col_start, is_column);
        fast_copy(D.begin(), D.begin() + (size - start), tmp.begin() + start);

        //std::cout << "1: " << tmp << "\n";

        detail::householder_vector(tmp, start);
        fast_copy(tmp, D);

        //std::cout << "2: "  << D << "\n";
      }

      template <typename SCALARTYPE, unsigned int ALIGNMENT>
      void eye(viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT>& A)
      {
        std::vector<SCALARTYPE> foo(A.size1() * A.size1(), 0);

        for(std::size_t i = 0; i < A.size1(); i++)
        {
          foo[i*A.size1() + i] = 1;
        }

        viennacl::fast_copy(&foo[0], &foo[0] + foo.size(), A);
      }

      template <typename SCALARTYPE, unsigned int ALIGNMENT, typename VectorType>
      void bidiag_pack(viennacl::matrix<SCALARTYPE, row_major, ALIGNMENT>& A,
                       VectorType & dh,
                       VectorType & sh
                      )
      {
        viennacl::vector<SCALARTYPE, ALIGNMENT> D(dh.size());
        viennacl::vector<SCALARTYPE, ALIGNMENT> S(sh.size());

        viennacl::ocl::kernel& kernel = viennacl::ocl::get_kernel(viennacl::linalg::kernels::svd<SCALARTYPE, 1>::program_name(), SVD_BIDIAG_PACK_KERNEL);

        viennacl::ocl::enqueue(kernel(
                                      A,
                                      D,
                                      S,
                                      static_cast<cl_uint>(A.size1()),
                                      static_cast<cl_uint>(A.size2()),
                                      static_cast<cl_uint>(A.internal_size2())
                                    ));

        fast_copy(D, dh);
        fast_copy(S, sh);
      }

    }
  }
}

#endif