#pragma once

#if USE_STAN
#include <stan/math.hpp>
#include <stan/math/fwd.hpp>
#endif

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

#include "spatial_vector.hpp"

namespace tds {

template <typename ScalarT = double>
struct EigenAlgebraT {
  using Index = Eigen::Index;
  using Scalar = ScalarT;
  using EigenAlgebra = EigenAlgebraT<Scalar>;
  using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
  using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
  using Matrix3 = Eigen::Matrix<Scalar, 3, 3>;
  using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
  using Matrix3X = Eigen::Matrix<Scalar, 3, Eigen::Dynamic>;
  using MatrixX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
  using Quaternion = Eigen::Quaternion<Scalar>;
  using SpatialVector = tds::SpatialVector<EigenAlgebra>;
  using MotionVector = tds::MotionVector<EigenAlgebra>;
  using ForceVector = tds::ForceVector<EigenAlgebra>;

  template <typename T>
  EIGEN_ALWAYS_INLINE static auto transpose(const T &matrix) {
    return matrix.transpose();
  }

  template <typename T>
  EIGEN_ALWAYS_INLINE static auto inverse(const T &matrix) {
    return matrix.inverse();
  }

  template <typename T>
  EIGEN_ALWAYS_INLINE static auto inverse_transpose(const T &matrix) {
    return matrix.inverse().transpose();
  }

  template <typename T1, typename T2>
  EIGEN_ALWAYS_INLINE static auto cross(const T1 &vector_a,
                                        const T2 &vector_b) {
    return vector_a.cross(vector_b);
  }

  /**
   * V1 = mv(w1, v1)
   * V2 = mv(w2, v2)
   * V1 x V2 = mv(w1 x w2, w1 x v2 + v1 x w2)
   */
  static inline MotionVector cross(const MotionVector &a,
                                   const MotionVector &b) {
    return MotionVector(a.top.cross(b.top),
                        a.top.cross(b.bottom) + a.bottom.cross(b.top));
  }

  /**
   * V = mv(w, v)
   * F = fv(n, f)
   * V x* F = fv(w x n + v x f, w x f)
   */
  static inline ForceVector cross(const MotionVector &a, const ForceVector &b) {
    return ForceVector(a.top.cross(b.top) + a.bottom.cross(b.bottom),
                       a.top.cross(b.bottom));
  }

  EIGEN_ALWAYS_INLINE static Index size(const VectorX &v) { return v.size(); }

  EIGEN_ALWAYS_INLINE static Matrix3X create_matrix_3x(int num_cols) {
    return Matrix3X(3, num_cols);
  }
  EIGEN_ALWAYS_INLINE static MatrixX create_matrix_x(int num_rows,
                                                     int num_cols) {
    return MatrixX(num_rows, num_cols);
  }

  template <typename T>
  EIGEN_ALWAYS_INLINE static int num_rows(const T &matrix) {
    return matrix.rows();
  }

  template <typename T>
  EIGEN_ALWAYS_INLINE static int num_cols(const T &matrix) {
    return matrix.cols();
  }

  EIGEN_ALWAYS_INLINE static Scalar determinant(const Matrix3 &m) {
    return m.determinant();
  }

  EIGEN_ALWAYS_INLINE static Scalar determinant(const MatrixX &m) {
    return m.determinant();
  }

  /**
   * Returns true if the matrix `mat` is positive-definite, and assigns
   * `mat_inv` to the inverse of mat.
   * `mat` must be a symmetric matrix.
   */
  static bool symmetric_inverse(const MatrixX &mat, MatrixX &mat_inv) {
    Eigen::LLT<MatrixX> llt(mat);
    if (llt.info() == Eigen::NumericalIssue) {
      return false;
    }
    mat_inv = mat.inverse();
    return true;
  }

  /**
   * V = mv(w, v)
   * F = mv(n, f)
   * V.F = w.n + v.f
   */
  EIGEN_ALWAYS_INLINE static Scalar dot(const MotionVector &a,
                                        const ForceVector &b) {
    return a.top.dot(b.top) + a.bottom.dot(b.bottom);
  }
  EIGEN_ALWAYS_INLINE static Scalar dot(const ForceVector &a,
                                        const MotionVector &b) {
    return dot(b, a);
  }

  template <typename T1, typename T2>
  EIGEN_ALWAYS_INLINE static auto dot(const T1 &vector_a, const T2 &vector_b) {
    return vector_a.dot(vector_b);
  }

  TINY_INLINE static Scalar norm(const MotionVector &v) {
    using std::sqrt;
    return sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3] +
                v[4] * v[4] + v[5] * v[5]);
  }
  TINY_INLINE static Scalar norm(const ForceVector &v) {
    using std::sqrt;
    return sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3] +
                v[4] * v[4] + v[5] * v[5]);
  }

  template <typename T>
  EIGEN_ALWAYS_INLINE static Scalar norm(const T &v) {
    return v.norm();
  }
  template <typename T>
  EIGEN_ALWAYS_INLINE static Scalar sqnorm(const T &v) {
    return v.squaredNorm();
  }

  template <typename T>
  EIGEN_ALWAYS_INLINE static auto normalize(T &v) {
    v.normalize();
    return v;
  }

  EIGEN_ALWAYS_INLINE static Matrix3 cross_matrix(const Vector3 &v) {
    Matrix3 tmp;
    tmp << 0., -v[2], v[1], v[2], 0., -v[0], -v[1], v[0], 0.;
    return tmp;
  }

  EIGEN_ALWAYS_INLINE static Matrix3 zero33() { return Matrix3::Zero(); }

  EIGEN_ALWAYS_INLINE static VectorX zerox(Index size) {
    return VectorX::Zero(size);
  }

  EIGEN_ALWAYS_INLINE static Matrix3 diagonal3(const Vector3 &v) {
    Matrix3 tmp;
    tmp << v[0], 0, 0, 0, v[1], 0, 0, 0, v[2];
    return tmp;
  }

  EIGEN_ALWAYS_INLINE static Matrix3 diagonal3(const Scalar &v) {
    Matrix3 tmp;
    tmp << v, 0, 0, 0, v, 0, 0, 0, v;
    return tmp;
  }

  EIGEN_ALWAYS_INLINE static Matrix3 eye3() { return Matrix3::Identity(); }
  EIGEN_ALWAYS_INLINE static void set_identity(Quaternion &quat) {
    quat = Quaternion(1., 0., 0., 0.);
  }

  EIGEN_ALWAYS_INLINE static Scalar zero() { return 0; }
  EIGEN_ALWAYS_INLINE static Scalar one() { return 1; }
  EIGEN_ALWAYS_INLINE static Scalar two() { return 2; }
  EIGEN_ALWAYS_INLINE static Scalar half() { return 0.5; }
  EIGEN_ALWAYS_INLINE static Scalar pi() { return M_PI; }
  EIGEN_ALWAYS_INLINE static Scalar fraction(int a, int b) {
    return ((double)a) / b;
  }

  static Scalar scalar_from_string(const std::string &s) {
    return std::stod(s);
  }

  EIGEN_ALWAYS_INLINE static Vector3 zero3() { return Vector3::Zero(); }
  EIGEN_ALWAYS_INLINE static Vector3 unit3_x() { return Vector3(1, 0, 0); }
  EIGEN_ALWAYS_INLINE static Vector3 unit3_y() { return Vector3(0, 1, 0); }
  EIGEN_ALWAYS_INLINE static Vector3 unit3_z() { return Vector3(0, 0, 1); }

  EIGEN_ALWAYS_INLINE static VectorX segment(const VectorX &vec,
                                             int start_index, int length) {
    return vec.segment(start_index, length);
  }

  EIGEN_ALWAYS_INLINE static MatrixX block(const MatrixX &mat,
                                           int start_row_index,
                                           int start_col_index, int rows,
                                           int cols) {
    return mat.block(start_row_index, start_col_index, rows, cols);
  }

  EIGEN_ALWAYS_INLINE static void assign_block(Matrix3X &output,
                                               const Matrix3 &input, int i,
                                               int j, int m = -1, int n = -1,
                                               int input_i = 0,
                                               int input_j = 0) {
    if (m < 0) m = input.rows();
    if (n < 0) n = input.cols();
    assert(i + m <= output.rows() && j + n <= output.cols());
    assert(input_i + m <= input.rows() && input_j + n <= input.cols());
    for (int ii = 0; ii < m; ++ii) {
      for (int jj = 0; jj < n; ++jj) {
        output(ii + i, jj + j) = input(ii + input_i, jj + input_j);
      }
    }
  }

  EIGEN_ALWAYS_INLINE static void assign_block(Matrix6 &output,
                                               const Matrix3 &input, int i,
                                               int j, int m = -1, int n = -1,
                                               int input_i = 0,
                                               int input_j = 0) {
    if (m < 0) m = input.rows();
    if (n < 0) n = input.cols();
    assert(i + m <= output.rows() && j + n <= output.cols());
    assert(input_i + m <= input.rows() && input_j + n <= input.cols());
    for (int ii = 0; ii < m; ++ii) {
      for (int jj = 0; jj < n; ++jj) {
        output(ii + i, jj + j) = input(ii + input_i, jj + input_j);
      }
    }
  }

  EIGEN_ALWAYS_INLINE static void assign_block(Matrix3 &output,
                                               const Matrix6 &input, int i,
                                               int j, int m = -1, int n = -1,
                                               int input_i = 0,
                                               int input_j = 0) {
    if (m < 0) m = input.rows();
    if (n < 0) n = input.cols();
    assert(i + m <= output.rows() && j + n <= output.cols());
    assert(input_i + m <= input.rows() && input_j + n <= input.cols());
    for (int ii = 0; ii < m; ++ii) {
      for (int jj = 0; jj < n; ++jj) {
        output(ii + i, jj + j) = input(ii + input_i, jj + input_j);
      }
    }
  }

  EIGEN_ALWAYS_INLINE static void assign_block(MatrixX &output,
                                               const MatrixX &input, int i,
                                               int j, int m = -1, int n = -1,
                                               int input_i = 0,
                                               int input_j = 0) {
    if (m < 0) m = input.rows();
    if (n < 0) n = input.cols();
    assert(i + m <= output.rows() && j + n <= output.cols());
    assert(input_i + m <= input.rows() && input_j + n <= input.cols());
    for (int ii = 0; ii < m; ++ii) {
      for (int jj = 0; jj < n; ++jj) {
        output(ii + i, jj + j) = input(ii + input_i, jj + input_j);
      }
    }
  }

  template <int Rows1, int Cols1, int Rows2, int Cols2>
  EIGEN_ALWAYS_INLINE static void assign_block(
      Eigen::Matrix<Scalar, Rows1, Cols1> &output,
      const Eigen::Matrix<Scalar, Rows2, Cols2> &input, int i, int j,
      int m = -1, int n = -1, int input_i = 0, int input_j = 0) {
    if (m < 0) m = input.rows();
    if (n < 0) n = input.cols();
    assert(i + m <= output.rows() && j + n <= output.cols());
    assert(input_i + m <= input.rows() && input_j + n <= input.cols());
    for (int ii = 0; ii < m; ++ii) {
      for (int jj = 0; jj < n; ++jj) {
        output(ii + i, jj + j) = input(ii + input_i, jj + input_j);
      }
    }
  }

  EIGEN_ALWAYS_INLINE static void assign_column(Matrix3 &m, Index i,
                                                const Vector3 &v) {
    m.col(i) = v;
  }

  EIGEN_ALWAYS_INLINE static void assign_column(Matrix3 &m, Index i,
                                                const Matrix6 &v) {
    m.col(i) = v;
  }
  EIGEN_ALWAYS_INLINE static void assign_column(Matrix3X &m, Index i,
                                                const Vector3 &v) {
    m.col(i) = v;
  }

  EIGEN_ALWAYS_INLINE static void assign_column(MatrixX &m, Index i,
                                                const MatrixX &v) {
    m.col(i) = v;
  }

  EIGEN_ALWAYS_INLINE static void assign_column(MatrixX &m, Index i,
                                                const SpatialVector &v) {
    m.block(0, i, 3, 1) = v.top;
    m.block(3, i, 3, 1) = v.bottom;
  }
  template <int Rows, int Cols, typename Derived>
  EIGEN_ALWAYS_INLINE static void assign_column(
      Eigen::Matrix<Scalar, Rows, Cols> &m, Index i,
      const Eigen::DenseBase<Derived> &v) {
    assign_column(m, i, v.eval());
  }

  EIGEN_ALWAYS_INLINE static void assign_row(MatrixX &m, Index i,
                                             const MatrixX &v) {
    m.row(i) = v;
  }

  EIGEN_ALWAYS_INLINE static void assign_row(MatrixX &m, Index i,
                                             const SpatialVector &v) {
    m.block(i, 0, 1, 3) = v.top;
    m.block(i, 3, 1, 3) = v.bottom;
  }

  EIGEN_ALWAYS_INLINE static void assign_horizontal(MatrixX &mat,
                                                    const VectorX &vec,
                                                    int start_row_index,
                                                    int start_col_index) {
    mat.block(start_row_index, start_col_index, 1, vec.cols()) = vec;
  }

  template <int Rows>
  EIGEN_ALWAYS_INLINE static void assign_vertical(
      MatrixX &mat, const Eigen::Matrix<Scalar, Rows, 1> &vec,
      int start_row_index, int start_col_index) {
    mat.block(start_row_index, start_col_index, vec.cols(), 1) = vec;
  }

  template <int Rows, int Cols>
  TINY_INLINE static VectorX mul_transpose(
      const Eigen::Matrix<Scalar, Rows, Cols> &mat,
      const Eigen::Matrix<Scalar, Cols, 1> &vec) {
    return mat.transpose() * vec;
  }
  TINY_INLINE static VectorX mul_transpose(const MatrixX &mat,
                                           const VectorX &vec) {
    return mat.transpose() * vec;
  }

  EIGEN_ALWAYS_INLINE static Matrix3 quat_to_matrix(const Quaternion &quat) {
    // NOTE: Eigen requires quat to be normalized
    return quat.toRotationMatrix();
  }
  EIGEN_ALWAYS_INLINE static Matrix3 quat_to_matrix(const Scalar &x,
                                                    const Scalar &y,
                                                    const Scalar &z,
                                                    const Scalar &w) {
    return Quaternion(w, x, y, z).toRotationMatrix();
  }
  EIGEN_ALWAYS_INLINE static Quaternion matrix_to_quat(const Matrix3 &m) {
    return Quaternion(m);
  }
  EIGEN_ALWAYS_INLINE static Quaternion axis_angle_quaternion(
      const Vector3 &axis, const Scalar &angle) {
    return Quaternion(Eigen::AngleAxis(angle, axis));
  }

  EIGEN_ALWAYS_INLINE static Matrix3 rotation_x_matrix(const Scalar &angle) {
    using std::cos, std::sin;
    Scalar c = cos(angle);
    Scalar s = sin(angle);
    Matrix3 temp;
    temp << 1, 0, 0, 0, c, s, 0, -s, c;
    return temp;
  }

  EIGEN_ALWAYS_INLINE static Matrix3 rotation_y_matrix(const Scalar &angle) {
    using std::cos, std::sin;
    Scalar c = cos(angle);
    Scalar s = sin(angle);
    Matrix3 temp;
    temp << c, 0, -s, 0, 1, 0, s, 0, c;
    return temp;
  }

  EIGEN_ALWAYS_INLINE static Matrix3 rotation_z_matrix(const Scalar &angle) {
    using std::cos, std::sin;
    Scalar c = cos(angle);
    Scalar s = sin(angle);
    Matrix3 temp;
    temp << c, s, 0, -s, c, 0, 0, 0, 1;
    return temp;
  }

  static Matrix3 rotation_zyx_matrix(const Scalar &r, const Scalar &p,
                                     const Scalar &y) {
    using std::cos, std::sin;
    Scalar ci(cos(r));
    Scalar cj(cos(p));
    Scalar ch(cos(y));
    Scalar si(sin(r));
    Scalar sj(sin(p));
    Scalar sh(sin(y));
    Scalar cc = ci * ch;
    Scalar cs = ci * sh;
    Scalar sc = si * ch;
    Scalar ss = si * sh;
    Matrix3 temp;
    temp << cj * ch, sj * sc - cs, sj * cc + ss, cj * sh, sj * ss + cc,
        sj * cs - sc, -sj, cj * si, cj * ci;
    return temp;
  }

  EIGEN_ALWAYS_INLINE static Vector3 rotate(const Quaternion &q,
                                            const Vector3 &v) {
    return q * v;
  }

  /**
   * Computes the quaternion delta given current rotation q, angular velocity w,
   * time step dt.
   */
  EIGEN_ALWAYS_INLINE static Quaternion quat_velocity(const Quaternion &q,
                                                      const Vector3 &w,
                                                      const Scalar &dt) {
    Quaternion delta((-q.x() * w[0] - q.y() * w[1] - q.z() * w[2]) * (0.5 * dt),
                     (q.w() * w[0] + q.y() * w[2] - q.z() * w[1]) * (0.5 * dt),
                     (q.w() * w[1] + q.z() * w[0] - q.x() * w[2]) * (0.5 * dt),
                     (q.w() * w[2] + q.x() * w[1] - q.y() * w[0]) * (0.5 * dt));
    return delta;
  }

  EIGEN_ALWAYS_INLINE static void quat_increment(Quaternion &a,
                                                 const Quaternion &b) {
    a.x() += b.x();
    a.y() += b.y();
    a.z() += b.z();
    a.w() += b.w();
  }

  EIGEN_ALWAYS_INLINE static const Scalar &quat_x(const Quaternion &q) {
    return q.x();
  }
  EIGEN_ALWAYS_INLINE static const Scalar &quat_y(const Quaternion &q) {
    return q.y();
  }
  EIGEN_ALWAYS_INLINE static const Scalar &quat_z(const Quaternion &q) {
    return q.z();
  }
  EIGEN_ALWAYS_INLINE static const Scalar &quat_w(const Quaternion &q) {
    return q.w();
  }
  EIGEN_ALWAYS_INLINE static const Quaternion quat_from_xyzw(const Scalar &x,
                                                             const Scalar &y,
                                                             const Scalar &z,
                                                             const Scalar &w) {
    // Eigen specific constructor coefficient order
    return Quaternion(w, x, y, z);
  }

  EIGEN_ALWAYS_INLINE static void set_zero(Matrix3X &m) { m.setZero(); }
  EIGEN_ALWAYS_INLINE static void set_zero(Vector3 &m) { m.setZero(); }
  EIGEN_ALWAYS_INLINE static void set_zero(VectorX &m) { m.setZero(); }

  EIGEN_ALWAYS_INLINE static void set_zero(MatrixX &m) { m.setZero(); }
  template <int Size1, int Size2 = 1>
  EIGEN_ALWAYS_INLINE static void set_zero(
      Eigen::Array<Scalar, Size1, Size2> &v) {
    v.setZero();
  }
  EIGEN_ALWAYS_INLINE static void set_zero(MotionVector &v) {
    v.top.setZero();
    v.bottom.setZero();
  }
  EIGEN_ALWAYS_INLINE static void set_zero(ForceVector &v) {
    v.top.setZero();
    v.bottom.setZero();
  }

  /**
   * Non-differentiable comparison operator.
   */
  TINY_INLINE static bool is_zero(const Scalar &a) { return a == zero(); }

  /**
   * Non-differentiable comparison operator.
   */
  EIGEN_ALWAYS_INLINE static bool less_than(const Scalar &a, const Scalar &b) {
    return a < b;
  }

  /**
   * Non-differentiable comparison operator.
   */
  EIGEN_ALWAYS_INLINE static bool less_than_zero(const Scalar &a) {
    return a < 0.;
  }

  /**
   * Non-differentiable comparison operator.
   */
  EIGEN_ALWAYS_INLINE static bool greater_than_zero(const Scalar &a) {
    return a > 0.;
  }

  /**
   * Non-differentiable comparison operator.
   */
  EIGEN_ALWAYS_INLINE static bool greater_than(const Scalar &a,
                                               const Scalar &b) {
    return a > b;
  }

  /**
   * Non-differentiable comparison operator.
   */
  EIGEN_ALWAYS_INLINE static bool equals(const Scalar &a, const Scalar &b) {
    return a == b;
  }

#ifdef USE_STAN
  template <typename InnerScalar>
  TINY_INLINE static std::enable_if_t<
      !std::is_same_v<Scalar, stan::math::fvar<InnerScalar>>, double>
  to_double(const stan::math::fvar<InnerScalar> &s) {
    return stan::math::value_of(s);
  }
#endif

  TINY_INLINE static double to_double(const Scalar &s) {
#ifdef USE_STAN
    if constexpr (std::is_same_v<Scalar, stan::math::var> ||
                  std::is_same_v<Scalar, stan::math::fvar<double>>) {
      return stan::math::value_of(s);
    } else {
      return static_cast<double>(s);
    }
#else
    return static_cast<double>(s);
#endif
  }

  TINY_INLINE static Scalar from_double(double s) {
    return static_cast<Scalar>(s);
  }

  template <int Size1, int Size2>
  static void print(const std::string &title,
                    Eigen::Matrix<Scalar, Size1, Size2> &m) {
    std::cout << title << "\n" << m << std::endl;
  }
  template <int Size1, int Size2 = 1>
  static void print(const std::string &title,
                    Eigen::Array<Scalar, Size1, Size2> &v) {
    std::cout << title << "\n" << v << std::endl;
  }
  static void print(const std::string &title, const Scalar &v) {
    std::cout << title << "\n" << to_double(v) << std::endl;
  }
  template <typename T>
  static void print(const std::string &title, const T &abi) {
    abi.print(title.c_str());
  }

  template <typename T>
  TINY_INLINE static auto sin(const T &s) {
    using std::sin;
    return sin(s);
  }

  template <typename T>
  TINY_INLINE static auto cos(const T &s) {
    using std::cos;
    return cos(s);
  }

  template <typename T>
  TINY_INLINE static auto tan(const T &s) {
    using std::tan;
    return tan(s);
  }

  template <typename T>
  TINY_INLINE static auto atan2(const T &dy, const T &dx) {
    using std::atan2;
    return atan2(dy, dx);
  }

  template <typename T>
  TINY_INLINE static auto abs(const T &s) {
    using std::abs;
    return abs(s);
  }

  template <typename T>
  TINY_INLINE static auto sqrt(const T &s) {
    using std::sqrt;
    return sqrt(s);
  }

  template <typename T>
  TINY_INLINE static auto tanh(const T &s) {
    using std::tanh;
    return tanh(s);
  }

  template <typename T>
  TINY_INLINE static auto exp(const T &s) {
    using std::exp;
    return exp(s);
  }

  template <typename T>
  TINY_INLINE static auto log(const T &s) {
    using std::log;
    return log(s);
  }

  template <typename T>
  TINY_INLINE static auto max(const T &x, const T &y) {
    using std::max;
    return max(x, y);
  }

  template <typename T>
  TINY_INLINE static auto min(const T &x, const T &y) {
    using std::min;
    return min(x, y);
  }

  EigenAlgebraT<Scalar>() = delete;
};

typedef EigenAlgebraT<double> EigenAlgebra;

}  // end namespace tds


#include "test_jain_commitments.h"
#include <hamming/jain_commitment/jain_commitment.h>

#include <NTL/vec_GF2.h>
#include <NTL/mat_GF2.h>
#include <utils/utils.h>
#include "test_params.h"
#include "test_functions.h"

int test::hamming_metric::commitment::test_verify() {

    NTL::vec_GF2 c, r, m;
    ::hamming_metric::commitment::public_key_t public_key;

    {
        ::utils::generate_random_binary_vector(
                m,
                JAIN_V);
        ::utils::generate_random_binary_vector(
                r,
                JAIN_L);
        ::hamming_metric::commitment::generate_public_key(
                &public_key);
    }

    ::hamming_metric::commitment::generate_commitment(
            c,
            &public_key,
            r,
            m);

    return ::hamming_metric::commitment::verify(
            c,
            &public_key,
            r,
            m);
}

static void test_perf_generate_vector_of_weight_w() {

    NTL::vec_GF2 e;

    clock_t start, lap, total_time;
    total_time = 0;
    int i;

    uint iterations = 0x100;
    int total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            ::utils::generate_vector_of_weight_w(
                    e,
                    JAIN_K,
                    W);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    double total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    double operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Hamming",
            "Generate vector of weight w",
            total_time_in_seconds,
            operation_per_second);
}

static void test_perf_generate_random_binary_matrix_A() {

    NTL::mat_GF2 A;

    clock_t start, lap, total_time;
    total_time = 0;
    int i;

    uint iterations = 0x100;
    int total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            utils::generate_random_binary_matrix(
                    A,
                    JAIN_K,
                    JAIN_L + JAIN_V);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    double total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    double operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Hamming",
            "Generate random binary matrix A",
            total_time_in_seconds,
            operation_per_second);
}

static void test_perf_generate_commitment() {

    NTL::mat_GF2 A;
    NTL::vec_GF2 c, r, m, e;

    ::utils::generate_random_binary_vector(
            m,
            JAIN_V);

    ::utils::generate_random_binary_vector(
            r,
            JAIN_L);

    utils::generate_random_binary_matrix(
            A,
            JAIN_K,
            JAIN_L + JAIN_V);

    ::utils::generate_vector_of_weight_w(
            e,
            JAIN_K,
            W);

    clock_t start, lap, total_time;
    total_time = 0;
    int i;

    uint iterations = 0x100;
    int total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            ::hamming_metric::commitment::generate_commitment(
                    c,
                    A,
                    r,
                    m,
                    e);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    double total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    double operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Hamming",
            "Generate commitment",
            total_time_in_seconds,
            operation_per_second);
}

static void test_perf_recover_e_from_c() {

    NTL::mat_GF2 A;
    NTL::vec_GF2 c, r, m;

    ::utils::generate_random_binary_vector(
            m,
            JAIN_V);

    ::utils::generate_random_binary_vector(
            r,
            JAIN_L);

    utils::generate_random_binary_matrix(
            A,
            JAIN_K,
            JAIN_L + JAIN_V);

    ::hamming_metric::commitment::generate_commitment(
            c,
            A,
            r,
            m);

    clock_t start, lap, total_time;
    total_time = 0;
    int i;

    uint iterations = 0x100;
    int total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            NTL::vec_GF2 _r(r);
            _r.append(m);
            auto e = (A * _r) + c;
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    double total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    double operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Hamming",
            "Recover e from c",
            total_time_in_seconds,
            operation_per_second);
}

static void test_perf_calculate_weight_of_vector() {

    NTL::vec_GF2 e;

    ::utils::generate_vector_of_weight_w(
            e,
            JAIN_K,
            W);

    clock_t start, lap, total_time;
    total_time = 0;
    int i;

    uint iterations = 0x100;
    int total_iterations = iterations;

    do {
        start = clock();

        for (i = 0; i < iterations; i++) {
            NTL::weight(e);
        }

        lap = clock() - start;
        total_time += lap;
        total_iterations += iterations;

        iterations <<= 1;

    } while (lap < TESTRUN_LEN * CLOCKS_PER_SEC);

    double total_time_in_seconds = (double) total_time / (double) CLOCKS_PER_SEC;
    double operation_per_second = total_time_in_seconds / total_iterations;

    test::test_functions::print_ops(
            "Hamming",
            "Calculate weight of vector",
            total_time_in_seconds,
            operation_per_second);
}

void test::hamming_metric::commitment::test_perf() {

    test_perf_generate_vector_of_weight_w();
    test_perf_generate_random_binary_matrix_A();
    test_perf_generate_commitment();
    test_perf_recover_e_from_c();
    test_perf_calculate_weight_of_vector();
}


#ifndef MULTINOMIAL_MODEL_HPP
#define MULTINOMIAL_MODEL_HPP

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <cmath>
#include <stdexcept>
#include <vector>

/**
 * Return the softmax of the specified vector.  Softmax is defined by
 *
 * ```
 * softmax(alpha) = exp(alpha) / sum(exp(alpha))
 * ```
 *
 * using offsets to prevent underflow of the exponentiation.
 *
 * @param alpha unconstrained input vector
 * @return softmax of input
 */
Eigen::VectorXf softmax(const Eigen::VectorXf& alpha) {
  using std::exp;
  auto delta = Eigen::VectorXf::Constant(alpha.size(), alpha.maxCoeff());
  Eigen::VectorXf phi = (alpha - delta).array().exp();
  return delta + phi / phi.sum();
}

/**
 * This class defines a Bayesian model implementing the joint density
 *
 * ```
 * log p(y, alpha | xt)
 *   = log multinomial(y | xt * softmax(alpha)) + log normal(alpha | 0, 3)
 *   = y' * log(xt * softmax(alpha)) - 1 / (2 * 3^2) * alpha' * alpha
 * ```
 *
 * K: kmer size
 * M: number of k-mers (4^K)
 * N: number of reads
 * T: number of isoforms
 * y: (M x 1) matrix of shredded reads
 * x: (T x M) sparse matrix of kmers per isoform with simplex rows
 * xt: (M x T) sparse matrix of kmers per isoform with simplex columns
 * alpha: (T x 1) vector of log odds
 */
struct multinomial_model {
  const Eigen::Map<Eigen::SparseMatrix<float, Eigen::RowMajor>>& xt_;
  const Eigen::VectorXf& y_;

  multinomial_model(
      const Eigen::Map<Eigen::SparseMatrix<float, Eigen::RowMajor>>& xt,
      const Eigen::VectorXf& y)
      : xt_(xt), y_(y) {
    if (xt.cols() != y.rows()) {
       throw std::runtime_error("xt rows must equal y cols");
    }
  }

  float log_density(const Eigen::VectorXf& beta) {
    Eigen::VectorXf theta = softmax(beta);
    std::cout << "beta.size() = " << beta.size()
              << "; theta.size() = " << theta.size()
              << std::endl;
    std::cout << "model:  xt_.rows() = " << xt_.rows() << std::endl;
    std::cout << "model:  xt_.cols() = " << xt_.cols() << std::endl;
    // Eigen::VectorXf xt_sm_a =  (xt_ * theta).array().log();
    float log_likelihood = 0;
    // float log_likelihood = y_.transpose() * xt_sm_a;
    float log_prior = -0.125 * beta.transpose() * beta;
    return log_likelihood + log_prior;
  }

  void grad_log_density(const Eigen::VectorXf& beta, Eigen::VectorXf& grad) {
    Eigen::VectorXf t1 = softmax(beta);
    Eigen::VectorXf t2 = xt_ * t1;
    Eigen::VectorXf t3 = (y_.cwiseProduct(t2.cwiseInverse()).transpose() * xt_).transpose();
    Eigen::VectorXf grad_likelihood = t1.cwiseProduct(t3) - t1.dot(t3) * t1;
    Eigen::VectorXf grad_prior = -0.25 * beta;
    grad = grad_likelihood + grad_prior;
  }

  //   std::vector<int> sample(uint64_t N, const Eigen::VectorXf& beta) {
  // return std::multinomial_rng(N, xt_ * softmax(beta));
  // }

  // std::vector<int> sample(uint64_t N, double mu, double sigma) {
  // return std::vector<int>();
  // }
};

#endif


#include "pitts_parallel.hpp"
#include "pitts_common.hpp"
#include "pitts_multivector.hpp"
#include "pitts_multivector_random.hpp"
#include "pitts_multivector_tsqr.hpp"
#include "pitts_tensor2.hpp"
#include "pitts_tensor2_eigen_adaptor.hpp"
#include <exception>
#include <charconv>
#include <iostream>
#pragma GCC push_options
#pragma GCC optimize("no-unsafe-math-optimizations")
#include <Eigen/Dense>
#pragma GCC pop_options

int main(int argc, char* argv[])
{
  PITTS::initialize(&argc, &argv);

  using mat = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
  using Chunk = PITTS::Chunk<double>;

  if( argc != 5 && argc != 6 )
    throw std::invalid_argument("Requires 4 arguments (n m reductionFactor nIter [colBlockingSize] )!");

  long long n = 0, m = 0;
  int reductionFactor = 20, nIter = 0;
  int colBlockingSize = 0;
  std::from_chars(argv[1], argv[2], n);
  std::from_chars(argv[2], argv[3], m);
  std::from_chars(argv[3], argv[4], reductionFactor);
  std::from_chars(argv[4], argv[5], nIter);
  if( argc == 6 )
    std::from_chars(argv[5], argv[6], colBlockingSize);

  const auto& [iProc,nProcs] = PITTS::internal::parallel::mpiProcInfo();
  {
    const auto& [nFirst,nLast] = PITTS::internal::parallel::distribute(n, {iProc,nProcs});
    n = nLast - nFirst + 1;
  }

  PITTS::MultiVector<double> M(n, m);
  randomize(M);

  PITTS::Tensor2<double> R(m,m);

double wtime = omp_get_wtime();
  for(int iter = 0; iter < nIter; iter++)
  {
    block_TSQR(M, R, reductionFactor, true, colBlockingSize);
  }
wtime = omp_get_wtime() - wtime;
  if( iProc == 0 )
    std::cout << "wtime: " << wtime << "\n";

  if( iProc == 0 )
  {
    Eigen::BDCSVD<mat> svd(ConstEigenMap(R));
    //std::cout << "Result:\n" << M << "\n";
    std::cout << "singular values (new):\n" << svd.singularValues().transpose() << "\n";
  }

  PITTS::finalize();

  return 0;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE CISHW1Test

// system includes
#include <boost/test/unit_test.hpp>
#include <exception>
#include <unistd.h>
#include <boost/math/constants/constants.hpp>
#include <iostream>
#include <vector>
#include <boost/bind.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

// local includes
#include "IterativeClosestPoint.hpp"

static const bool debug = false;
static const double tolerance = 0.01l;

template<typename T>
bool isWithinTolerance(const T& result, const T& groundTruth, double toleranceVal = tolerance){
    return (result.isApprox(groundTruth,toleranceVal) || (result - groundTruth).norm() < toleranceVal);
}

BOOST_AUTO_TEST_SUITE(cisPA3test)

void testClosestPoint(const Eigen::Vector3d&& point,std::vector<Eigen::Vector3d>& vertices, Eigen::VectorXd& triangle, const Eigen::Vector3d&& expectedResult){

    Eigen::Vector3d testresult = FindClosestPoint(point,vertices,triangle);

    if (!isWithinTolerance(testresult, expectedResult)) {
        std::cout << "error: input point:" << point.transpose() << " expectedResult: " << expectedResult.transpose() << " not equal to testresult: " << testresult.transpose() << "\n";
    }
    BOOST_CHECK(isWithinTolerance(testresult, expectedResult));
}

BOOST_AUTO_TEST_CASE(FindClosestPointPointTest)
{
	// Unit Tests to Check if the function FindClosestPoint() works correctly
    Eigen::VectorXd triangle(3);
    triangle(0) = 0;
    triangle(1) = 1;
    triangle(2) = 2;

	std::vector<Eigen::Vector3d> vertices;
	vertices.push_back(Eigen::Vector3d(0,1,0));
    vertices.push_back(Eigen::Vector3d(1,0,0));
    vertices.push_back(Eigen::Vector3d(0,0,0));

    testClosestPoint(Eigen::Vector3d(1,1,0)    , vertices, triangle, Eigen::Vector3d(  0.5   , 0.5    , 0.0 ));
    testClosestPoint(Eigen::Vector3d(0.5,-1,0) , vertices, triangle, Eigen::Vector3d(  0.5   , 0.0    , 0.0 ));
    testClosestPoint(Eigen::Vector3d(0.5,0.5,1), vertices, triangle, Eigen::Vector3d(  0.5   , 0.5    , 0.0 ));
    testClosestPoint(Eigen::Vector3d(-1,0.5,0) , vertices, triangle, Eigen::Vector3d(  0.0   , 0.5    , 0.0 ));
    testClosestPoint(Eigen::Vector3d(0,0,0)    , vertices, triangle, Eigen::Vector3d(  0.0   , 0.0    , 0.0 ));
    testClosestPoint(Eigen::Vector3d(0.2,0.2,1), vertices, triangle, Eigen::Vector3d(  0.2   , 0.2    , 0.0 ));
    testClosestPoint(Eigen::Vector3d(-1,-1,-1) , vertices, triangle, Eigen::Vector3d(  0.0   , 0.0    , 0.0 ));
    testClosestPoint(Eigen::Vector3d(2,0,0)    , vertices, triangle, Eigen::Vector3d(  1.0   , 0.0    , 0.0 ));
}

BOOST_AUTO_TEST_SUITE_END()


#include "thin_lq.hpp"

#include <gtest/gtest.h>

#include <armadillo>
#include <cmath>
#include <limits>
#include <random>
#include <stdexcept>
#include <vector>

template <typename Real>
void ThinLqTest(long m, long n) {
    arma::Mat<Real> A(m, n, arma::fill::randn);

    arma::Mat<Real> L;
    arma::Mat<Real> Q;
    ThinLq<Real>(A, L, Q);

    const long k = std::min(m, n);

    ASSERT_EQ(L.size(), m * k);
    ASSERT_EQ(Q.size(), k * n);

    for (long j = 0; j < k; ++j) {
        for (long i = 0; i < j; ++i) {
            ASSERT_NEAR(L(i, j), static_cast<Real>(0),
                        10 * std::numeric_limits<Real>::epsilon());
        }
    }

    for (long i1 = 0; i1 < k; ++i1) {
        for (long i2 = 0; i2 < i1; ++i2) {
            Real dot_product = static_cast<Real>(0);
            for (long j = 0; j < n; ++j) {
                dot_product += Q(i1, j) * Q(i2, j);
            }

            ASSERT_NEAR(dot_product, static_cast<Real>(0),
                        10 * std::numeric_limits<Real>::epsilon());
        }

        Real norm_squared = static_cast<Real>(0);
        for (long j = 0; j < n; ++j) {
            norm_squared += std::pow(Q(i1, j), 2);
        }

        ASSERT_NEAR(norm_squared, static_cast<Real>(1),
                    10 * std::numeric_limits<Real>::epsilon());
    }

    arma::Mat<Real> B = L * Q;

    Real frobenius_error = static_cast<Real>(0);
    for (long j = 0; j < n; ++j) {
        for (long i = 0; i < m; ++i) {
            frobenius_error += std::pow(B(i, j) - A(i, j), 2);
        }
    }
    frobenius_error = std::sqrt(frobenius_error);
    ASSERT_NEAR(frobenius_error, static_cast<Real>(0),
                100 * std::numeric_limits<Real>::epsilon());
}

TEST(ThinLq, Short) {
    ThinLqTest<float>(4, 64);
    ThinLqTest<double>(4, 64);
}

TEST(ThinLq, Square) {
    ThinLqTest<float>(16, 16);
    ThinLqTest<double>(16, 16);
}

TEST(ThinLq, Tall) {
    ThinLqTest<float>(64, 4);
    ThinLqTest<double>(64, 4);
}

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

    return RUN_ALL_TESTS();
}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/pending/integer_range.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/graph/graphviz.hpp>

#include <iostream>
using namespace boost;
template < typename TimeMap > class dfs_time_visitor:public default_dfs_visitor {
  typedef typename property_traits < TimeMap >::value_type T;
public:
  dfs_time_visitor(TimeMap dmap, TimeMap fmap, T & t)
:  m_dtimemap(dmap), m_ftimemap(fmap), m_time(t) {
  }
  template < typename Vertex, typename Graph >
    void discover_vertex(Vertex u, const Graph & g) const
  {
    put(m_dtimemap, u, m_time++);
  }
  template < typename Vertex, typename Graph >
    void finish_vertex(Vertex u, const Graph & g) const
  {
    put(m_ftimemap, u, m_time++);
  }
  TimeMap m_dtimemap;
  TimeMap m_ftimemap;
  T & m_time;
};

int
main()
{
  // Select the graph type we wish to use
  typedef adjacency_list < vecS, vecS, directedS > graph_t;
  typedef graph_traits < graph_t >::vertices_size_type size_type;
  // Set up the vertex names
  enum
  { u, v, w, x, y, z, N };
  char name[] = { 'u', 'v', 'w', 'x', 'y', 'z' };
  // Specify the edges in the graph
  typedef std::pair < int, int >E;
  E edge_array[] = { E(u, v), E(u, x), E(x, v), E(y, x),
    E(v, y), E(w, y), E(w, z), E(z, z)
  };
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  graph_t g(N);
  for (std::size_t j = 0; j < sizeof(edge_array) / sizeof(E); ++j)
    add_edge(edge_array[j].first, edge_array[j].second, g);
#else
  graph_t g(edge_array, edge_array + sizeof(edge_array) / sizeof(E), N);
#endif

  // Typedefs
  typedef boost::graph_traits < graph_t >::vertex_descriptor Vertex;
  typedef size_type* Iiter;

write_graphviz(std::cout, g);

  // discover time and finish time properties
  std::vector < size_type > dtime(num_vertices(g));
  std::vector < size_type > ftime(num_vertices(g));
  size_type t = 0;
  dfs_time_visitor < size_type * >vis(&dtime[0], &ftime[0], t);

  depth_first_search(g, visitor(vis));

  // use std::sort to order the vertices by their discover time
  std::vector < size_type > discover_order(N);
  integer_range < size_type > r(0, N);
  std::copy(r.begin(), r.end(), discover_order.begin());
  std::sort(discover_order.begin(), discover_order.end(),
            indirect_cmp < Iiter, std::less < size_type > >(&dtime[0]));
  std::cout << "order of discovery: ";
  int i;
  for (i = 0; i < N; ++i)
    std::cout << name[discover_order[i]] << " ";

  std::vector < size_type > finish_order(N);
  std::copy(r.begin(), r.end(), finish_order.begin());
  std::sort(finish_order.begin(), finish_order.end(),
            indirect_cmp < Iiter, std::less < size_type > >(&ftime[0]));
  std::cout << std::endl << "order of finish: ";
  for (i = 0; i < N; ++i)
    std::cout << name[finish_order[i]] << " ";
  std::cout << std::endl;

  return EXIT_SUCCESS;
}


/*
 * Copyright (c) Meta Platforms, Inc. and affiliates.
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */

#define _USE_MATH_DEFINES
#include <Eigen/Core>
#include <cmath>
#include <random>

#include "beanmachine/graph/distribution/log_normal.h"

namespace beanmachine {
namespace distribution {

using namespace graph;

LogNormal::LogNormal(AtomicType sample_type, const std::vector<Node*>& in_nodes)
    : Distribution(DistributionType::LOG_NORMAL, sample_type) {
  // a Log Normal distribution has two parents
  // mean of logarithm distribution -> real,
  // standard deviation of logarithm distribution -> positive real
  if (in_nodes.size() != 2) {
    throw std::invalid_argument(
        "LogNormal distribution must have exactly two parents");
  }
  if (in_nodes[0]->value.type != graph::AtomicType::REAL or
      in_nodes[1]->value.type != graph::AtomicType::POS_REAL) {
    throw std::invalid_argument(
        "LogNormal parents must be a real number and a positive real number");
  }
  if (sample_type != AtomicType::POS_REAL) {
    throw std::invalid_argument(
        "LogNormal distribution produces positive real number samples");
  }
}

double LogNormal::_double_sampler(std::mt19937& gen) const {
  std::lognormal_distribution<double> dist(
      in_nodes[0]->value._double, in_nodes[1]->value._double);
  return dist(gen);
}

// log_prob of a log normal:
//    - log(s) - 0.5 log(2*pi) - 0.5 (log(x) - m)^2 / s^2 - log(x)
// grad  w.r.t. value x: (m - log(x) - s^2) / (x * s^2)
// grad2 w.r.t. value x: (s^2 + log(x) - m - 1) / (s^2 * x^2)
// grad  w.r.t. s : -1/s + (log(x)-m)^2 / s^3
// grad2 w.r.t. s : 1/s^2 - 3 (log(x)-m)^2 / s^4
// grad  w.r.t. m : (log(x) - m) / s^2
// grad2 w.r.t. m : -1 / s^2
// First order chain rule: f(g(x))' = f'(g(x)) g'(x),
// - In backward propagation, f'(g(x)) is given by adjunct, the above equation
// computes g'(x). [g is the current function f is the final target]
// - In forward propagation, g'(x) is given by in_nodes[x]->grad1,
// the above equation computes f'(g) [f is the current function g is the input]
double LogNormal::log_prob(const NodeValue& value) const {
  double m = in_nodes[0]->value._double;
  double s = in_nodes[1]->value._double;
  double result, sum_logx, sum_logx_sq;
  int size;

  if (value.type.variable_type == graph::VariableType::SCALAR) {
    size = 1;
    sum_logx = std::log(value._double);
    sum_logx_sq = sum_logx * sum_logx;
  } else if (
      value.type.variable_type == graph::VariableType::BROADCAST_MATRIX) {
    size = static_cast<int>(value._matrix.size());
    sum_logx = value._matrix.array().log().matrix().sum();
    sum_logx_sq = value._matrix.array().log().matrix().squaredNorm();
  } else {
    throw std::runtime_error(
        "LogNormal::log_prob applied to invalid variable type");
  }
  result = (-std::log(s) - 0.5 * std::log(2 * M_PI)) * size -
      0.5 * (sum_logx_sq - 2 * m * sum_logx + m * m * size) / (s * s) -
      sum_logx;
  return result;
}

void LogNormal::log_prob_iid(
    const graph::NodeValue& value,
    Eigen::MatrixXd& log_probs) const {
  assert(value.type.variable_type == graph::VariableType::BROADCAST_MATRIX);
  double m = in_nodes[0]->value._double;
  double s = in_nodes[1]->value._double;
  Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic> logs =
      value._matrix.array().log();
  log_probs = (-std::log(s) - 0.5 * std::log(2 * M_PI)) -
      0.5 * (logs - m).pow(2) / (s * s) - logs;
}

void LogNormal::_grad1_log_prob_value(
    double& grad1,
    double val,
    double m,
    double s_sq) {
  grad1 += (m - std::log(val) - s_sq) / (val * s_sq);
};

void LogNormal::_grad2_log_prob_value(
    double& grad2,
    double val,
    double m,
    double s_sq) {
  grad2 += (s_sq + std::log(val) - m - 1) / (val * val * s_sq);
};

void LogNormal::gradient_log_prob_value(
    const NodeValue& value,
    double& grad1,
    double& grad2) const {
  assert(value.type.variable_type == graph::VariableType::SCALAR);
  double m = in_nodes[0]->value._double;
  double s = in_nodes[1]->value._double;
  double s_sq = s * s;
  _grad1_log_prob_value(grad1, value._double, m, s_sq);
  _grad2_log_prob_value(grad2, value._double, m, s_sq);
}

void LogNormal::gradient_log_prob_param(
    const NodeValue& value,
    double& grad1,
    double& grad2) const {
  assert(value.type.variable_type == graph::VariableType::SCALAR);
  double log_x = std::log(value._double);
  double m = in_nodes[0]->value._double;
  double s = in_nodes[1]->value._double;
  double s_sq = s * s;
  // gradients of m should be non-zero before computing gradients w.r.t. m
  double m_grad = in_nodes[0]->grad1;
  double m_grad2 = in_nodes[0]->grad2;
  if (m_grad != 0 or m_grad2 != 0) {
    double grad_m = (log_x - m) / s_sq;
    double grad2_m2 = -1 / s_sq;
    grad1 += grad_m * m_grad;
    grad2 += grad2_m2 * m_grad * m_grad + grad_m * m_grad2;
  }
  double s_grad = in_nodes[1]->grad1;
  double s_grad2 = in_nodes[1]->grad2;
  if (s_grad != 0 or s_grad2 != 0) {
    double grad_s = -1 / s + (log_x - m) * (log_x - m) / (s * s * s);
    double grad2_s2 = 1 / s_sq - 3 * (log_x - m) * (log_x - m) / (s_sq * s_sq);
    grad1 += grad_s * s_grad;
    grad2 += grad2_s2 * s_grad * s_grad + grad_s * s_grad2;
  }
}

void LogNormal::backward_value(
    const graph::NodeValue& value,
    graph::DoubleMatrix& back_grad,
    double adjunct) const {
  assert(value.type.variable_type == graph::VariableType::SCALAR);
  double m = in_nodes[0]->value._double;
  double s = in_nodes[1]->value._double;
  double s_sq = s * s;
  double increment = 0.0;
  _grad1_log_prob_value(increment, value._double, m, s_sq);
  back_grad += adjunct * increment;
}

void LogNormal::backward_value_iid(
    const graph::NodeValue& value,
    graph::DoubleMatrix& back_grad) const {
  assert(value.type.variable_type == graph::VariableType::BROADCAST_MATRIX);
  double m = in_nodes[0]->value._double;
  double s = in_nodes[1]->value._double;
  double s_sq = s * s;
  back_grad +=
      (m - value._matrix.array().log() - s_sq) / (value._matrix.array() * s_sq);
}

void LogNormal::backward_value_iid(
    const graph::NodeValue& value,
    graph::DoubleMatrix& back_grad,
    Eigen::MatrixXd& adjunct) const {
  assert(value.type.variable_type == graph::VariableType::BROADCAST_MATRIX);
  double m = in_nodes[0]->value._double;
  double s = in_nodes[1]->value._double;
  double s_sq = s * s;
  back_grad += (adjunct.array()) * (m - value._matrix.array().log() - s_sq) /
      (value._matrix.array() * s_sq);
}

void LogNormal::backward_param(const graph::NodeValue& value, double adjunct)
    const {
  assert(value.type.variable_type == graph::VariableType::SCALAR);
  double m = in_nodes[0]->value._double;
  double s = in_nodes[1]->value._double;
  double log_x = std::log(value._double);
  double s_sq = s * s;
  double jacob_0 = (log_x - m) / s_sq;

  if (in_nodes[0]->needs_gradient()) {
    in_nodes[0]->back_grad1 += adjunct * jacob_0;
  }
  if (in_nodes[1]->needs_gradient()) {
    in_nodes[1]->back_grad1 += adjunct * (-1 / s + jacob_0 * jacob_0 * s);
  }
}

void LogNormal::backward_param_iid(const graph::NodeValue& value) const {
  assert(value.type.variable_type == graph::VariableType::BROADCAST_MATRIX);
  double m = in_nodes[0]->value._double;
  double s = in_nodes[1]->value._double;
  double s_sq = s * s;

  int size = static_cast<int>(value._matrix.size());
  double sum_logx = value._matrix.array().log().sum();
  if (in_nodes[0]->needs_gradient()) {
    in_nodes[0]->back_grad1 += sum_logx / s_sq - size * m / s_sq;
  }
  if (in_nodes[1]->needs_gradient()) {
    double sum_logx_sq = value._matrix.array().log().matrix().squaredNorm();
    in_nodes[1]->back_grad1 +=
        (-size / s +
         (sum_logx_sq - 2 * m * sum_logx + m * m * size) / (s * s_sq));
  }
}

void LogNormal::backward_param_iid(
    const graph::NodeValue& value,
    Eigen::MatrixXd& adjunct) const {
  assert(value.type.variable_type == graph::VariableType::BROADCAST_MATRIX);
  double m = in_nodes[0]->value._double;
  double s = in_nodes[1]->value._double;
  double s_sq = s * s;

  double sum_logx = (value._matrix.array().log() * adjunct.array()).sum();
  double sum_adjunct = adjunct.sum();
  if (in_nodes[0]->needs_gradient()) {
    in_nodes[0]->back_grad1 += sum_logx / s_sq - sum_adjunct * m / s_sq;
  }
  if (in_nodes[1]->needs_gradient()) {
    double sum_logx_sq =
        (value._matrix.array().log().pow(2) * adjunct.array()).sum();
    in_nodes[1]->back_grad1 +=
        (-sum_adjunct / s +
         (sum_logx_sq - 2 * m * sum_logx + m * m * sum_adjunct) / (s * s_sq));
  }
}

} // namespace distribution
} // namespace beanmachine


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Anderson Jr., J.D. , Fundamentals of Aerodynamics, 3rd edition, McGraw Hill, 2001.
 *      Gentry, A., Smyth, D., and Oliver, W. . The Mark IV Supersonic-Hypersonic Arbitrary Body
 *          Program, Volume II - Program Formulation, Douglas Aircraft Company, 1973.
 *      Anderson Jr., J.D, Hypersonic and High-Temperature Gas Dynamics, 2nd edition, AIAA
 *          Education Series, 2006.
 *
 */

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/Aerodynamics/aerodynamics.h"
#include "Tudat/Astrodynamics/Aerodynamics/equilibriumWallTemperature.h"

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

namespace tudat
{
namespace aerodynamics
{

using mathematical_constants::PI;
using std::atan;
using std::exp;
using std::log;
using std::pow;
using std::sqrt;
using std::tan;

//! Compute local-to-static pressure ratio.
double computeLocalToStaticPressureRatio( double machNumber,
                                          double ratioOfSpecificHeats )
{
    // Return local-to-static pressure ratio.
    return pow( 2.0 / ( 2.0 + ( ratioOfSpecificHeats - 1.0 ) * pow( machNumber, 2.0 ) ),
                ratioOfSpecificHeats / ( ratioOfSpecificHeats - 1.0 ) );
}

//! Compute Prandtl-Meyer function.
double computePrandtlMeyerFunction( double machNumber, double ratioOfSpecificHeats )
{
    // Declare local variables.
    // Declare Mach number squared.
    double machNumberSquared_ = pow( machNumber, 2.0 );

    // Return value of Prandtl-Meyer function.
    return sqrt ( ( ratioOfSpecificHeats + 1.0 ) / ( ratioOfSpecificHeats - 1.0 ) )
            * atan ( sqrt ( ( ratioOfSpecificHeats - 1.0 ) / ( ratioOfSpecificHeats + 1.0 )
                            * ( machNumberSquared_ - 1.0 ) ) )
            - atan( sqrt ( machNumberSquared_ - 1.0 ) );
}

//! Compute stagnation pressure coefficient in supersonic flow.
double computeStagnationPressure( double machNumber,
                                  double ratioOfSpecificHeats )
{
    // Declare local variables.
    // Declare Mach number squared.
    double machNumberSquared_ = pow( machNumber, 2.0 );

    // Return stagnation pressure coefficient.
    return 2.0 / ( ratioOfSpecificHeats * machNumberSquared_ )
            * ( pow ( pow( ( ratioOfSpecificHeats + 1.0 ) * machNumber, 2.0 )
                      / ( 4.0 * ratioOfSpecificHeats * machNumberSquared_
                          - 2.0 * ( ratioOfSpecificHeats - 1.0 ) ),
                      ratioOfSpecificHeats / ( ratioOfSpecificHeats - 1.0 ) )
                * ( ( 1.0 - ratioOfSpecificHeats
                      + 2.0 * ratioOfSpecificHeats * machNumberSquared_ )
                    / ( ratioOfSpecificHeats + 1.0 ) ) - 1.0 );
}

//! Compute pressure coefficient based on Newtonian theory.
double computeNewtonianPressureCoefficient( double inclinationAngle )
{
    // Return pressure coefficient.
    return 2.0 * pow( sin( inclinationAngle ), 2.0 );
}

//! Compute pressure coefficient based on modified Newtonian theory.
double computeModifiedNewtonianPressureCoefficient(
    double inclinationAngle, double stagnationPressureCoefficient )
{
    // Return pressure coefficient.
    return stagnationPressureCoefficient * pow( sin( inclinationAngle ), 2.0 );
}

//! Compute pressure coefficient using empirical tangent wedge method.
double computeEmpiricalTangentWedgePressureCoefficient(
    double inclinationAngle, double machNumber )
{
    // Declare local variable.
    double machNumberSine_;

    // Set local variable.
    machNumberSine_ = machNumber * sin( inclinationAngle );

    // Return pressure coefficient approximation.
    return ( pow( 1.2 * machNumberSine_ + exp( -0.6 * machNumberSine_ ), 2.0 )
             - 1.0 ) / ( 0.6 * pow( machNumber, 2.0 ) );
}

//! Compute pressure coefficient using empirical tangent cone method.
double computeEmpiricalTangentConePressureCoefficient(
    double inclinationAngle, double machNumber )
{
    // Declare local variables.
    double machNumberSine_;
    double temporaryValue_;

    // Set local variables.
    machNumberSine_ = machNumber * sin ( inclinationAngle );
    temporaryValue_ = pow( ( 1.090909 * machNumberSine_
                             +  exp( -0.5454545 * machNumberSine_ ) ), 2.0 );

    // Return pressure coefficient approximation.
    return ( 48.0 * temporaryValue_ * pow( sin( inclinationAngle), 2.0 ) )
            / ( 23.0 * temporaryValue_ - 5.0 );
}

//! Compute pressure coefficient using modified Dahlem-Buck method.
double computeModifiedDahlemBuckPressureCoefficient(
    double inclinationAngle, double machNumber )
{
    // Declare local variables.
    double checkAngle_ = 22.5 * PI / 180.0;
    double factor1_;
    double factor2_;
    double exponent_;
    double pressureCoefficient_;

    // Check if inclination angle is greater than check angle. If so, use
    // Newtonian approximation.
    if ( inclinationAngle > checkAngle_ )
    {
        pressureCoefficient_
                = computeNewtonianPressureCoefficient( inclinationAngle );
    }

    // Else use Dahlem-Buck method.
    else
    {
        pressureCoefficient_
                = ( 1.0 + sin( 4.0 * pow( inclinationAngle , 0.75 ) ) )
                / ( pow( 4.0 * cos( inclinationAngle )
                         * cos( 2.0 * inclinationAngle ), 0.75 ) )
                * pow( sin( inclinationAngle ), 1.25 );
    }

    // For mach < 20, a correction term should be applied.
    if ( machNumber > 20.0 )
    {
        factor2_ = 1.0;
    }
    else
    {
        // Determine correction term.
        factor1_ = ( 6.0 - 0.3 * machNumber ) + sin( PI * ( log( machNumber ) - 0.588 ) / 1.20 );

        exponent_ = 1.15 + 0.5 * sin( PI * ( log( machNumber ) - 0.916 ) / 3.29 );

        factor2_ = 1.0 + factor1_ * pow( inclinationAngle * 180.0 / PI, -1.0 * exponent_ );
    }

    // Return pressure coefficient.
    return pressureCoefficient_ * factor2_;
}

//! Compute pressure coefficient using the Hankey flat surface method.
double computeHankeyFlatSurfacePressureCoefficient(
    double inclinationAngle, double machNumber )
{
    // Declare local variables.
    double stagnationPressureCoefficient_;

    // Calculate 'effective' stagnation pressure coefficient for low
    // inclination angle.
    if( inclinationAngle < PI / 18.0 )
    {
        stagnationPressureCoefficient_ = ( 0.195 + 0.222594 / pow( machNumber, 0.3 ) - 0.4 )
                * inclinationAngle * 180.0 / PI + 4.0;
    }
    // Calculate 'effective' stagnation pressure coefficient for other
    // inclination angle.
    else
    {
        stagnationPressureCoefficient_ = 1.95 + 0.3925 / ( pow( machNumber, 0.3 )
                                                           * tan( inclinationAngle ) );
    }

    // Return pressure coefficient using 'effective' stagnation pressure
    // coefficient.
    return computeModifiedNewtonianPressureCoefficient(
                inclinationAngle, stagnationPressureCoefficient_ );
}

//! Compute pressure coefficient using the Smyth delta wing method.
double computeSmythDeltaWingPressureCoefficient(
    double inclinationAngle, double machNumber )
{
    // Declare local variables.
    double machNumberSine_;
    double correctedInclinationAngle_;

    // Calculate inclination angle for use in calculations ( angles lower than
    // 1 degree not allowed ).
    if ( inclinationAngle < PI / 180.0 )
    {
        correctedInclinationAngle_ = PI / 180.0;
    }

    else
    {
        correctedInclinationAngle_ = inclinationAngle;
    }

    // Pre-compute for efficiency.
    machNumberSine_ = machNumber * sin( correctedInclinationAngle_ );

    // Employ empirical correlation to calculate pressure coefficient.
    // Return pressure coefficient.
    return 1.66667 * ( pow( 1.09 * machNumberSine_ + exp( -0.49 * machNumberSine_ ), 2.0 ) - 1.0 )
            / pow( machNumber, 2.0 );
}

//! Compute pressure coefficient using the van Dyke unified method.
double computeVanDykeUnifiedPressureCoefficient(
    double inclinationAngle, double machNumber,
    double ratioOfSpecificHeats, int type )
{
    // Declare and initialize local variables and pre-compute for efficiency.
    double ratioOfSpecificHeatsTerm_ = ( ratioOfSpecificHeats + 1.0 ) / 2.0;
    double machNumberTerm_ = sqrt( pow( machNumber , 2.0 ) - 1.0 );
    double exponent_ = 2.0 * ratioOfSpecificHeats / ( ratioOfSpecificHeats - 1.0 );

    // Declare and initialize value.
    double pressureCoefficient_ = 0.0;

    // Calculate compression pressure coefficient.
    if ( inclinationAngle >= 0.0 && type == 1 )
    {
        pressureCoefficient_ = pow( inclinationAngle, 2.0 )
                * ( ratioOfSpecificHeatsTerm_
                    + sqrt( pow( ratioOfSpecificHeatsTerm_, 2.0 )
                            + 4.0 / ( pow(  inclinationAngle * machNumberTerm_, 2.0 ) ) ) );
    }

    // Calculate expansion pressure coefficient.
    else if ( inclinationAngle < 0.0 && type == -1 )
    {
        // Calculate vacuum pressure coefficient.
        double vacuumPressureCoefficient_ = computeVacuumPressureCoefficient(
                    machNumber, ratioOfSpecificHeats );

        // Check to see if pressure coefficient will be lower than vacuum case,
        // set to vacuum if so.
        if ( -1.0 * inclinationAngle * machNumberTerm_
             > 2.0 / ( ratioOfSpecificHeats - 1.0 ) )
        {
            pressureCoefficient_ = vacuumPressureCoefficient_;
        }
        else
        {
            pressureCoefficient_
                    = 2.0 / ( ratioOfSpecificHeats * pow( machNumberTerm_, 2.0 ) )
                    * ( pow( 1.0 - ( ratioOfSpecificHeats - 1.0 ) / 2.0
                             * - 1.0 * inclinationAngle * machNumberTerm_, exponent_ ) - 1.0 );

            if ( pressureCoefficient_ < vacuumPressureCoefficient_ )
            {
                pressureCoefficient_ = vacuumPressureCoefficient_;
            }
        }
    }

    // Return pressure coefficient.
    return pressureCoefficient_;
}

//! Compute pressure coefficient using Prandtl-Meyer expansion.
double computePrandtlMeyerFreestreamPressureCoefficient(
    double inclinationAngle, double machNumber,
    double ratioOfSpecificHeats, double freestreamPrandtlMeyerFunction )
{
    // Declare local variables.
    double prandtlMeyerFunction_;
    double pressureCoefficient_;

    // Determine Prandtl-Meyer function value.
    prandtlMeyerFunction_ = freestreamPrandtlMeyerFunction - inclinationAngle;

    // If Prandtl-Meyer function is greater than the vacuum value, set vacuum
    // pressure coefficient.
    if ( prandtlMeyerFunction_ > maximumPrandtlMeyerFunctionValue )
    {
        pressureCoefficient_ = computeVacuumPressureCoefficient(
                    machNumber, ratioOfSpecificHeats );
    }

    else
    {
        // Determine local mach number.
        double localMachNumber_
                = computeInversePrandtlMeyerFunction( prandtlMeyerFunction_ );

        // Determine local to freestream pressure ratio.
        double pressureRatio_
                = computeLocalToStaticPressureRatio( localMachNumber_,
                                                     ratioOfSpecificHeats )
                / computeLocalToStaticPressureRatio( machNumber,
                                                     ratioOfSpecificHeats );

        // Form pressure coefficient.
        pressureCoefficient_ = 2.0 / ( ratioOfSpecificHeats * pow( machNumber, 2.0 ) )
                * ( pressureRatio_ - 1.0 );
    }

    // Return pressure coefficient.
    return pressureCoefficient_;
}

//! Compute pressure coefficient at vacuum.
double computeVacuumPressureCoefficient(
    double machNumber, double ratioOfSpecificHeats )
{
    // Return pressure coefficient.
    return -2.0 / ( ratioOfSpecificHeats * pow( machNumber, 2.0 ) );
}

//! Compute high Mach base pressure coefficient.
double computeHighMachBasePressure( double machNumber )
{
    // Calculate pressure coefficient.
    return -1.0 / pow( machNumber, 2.0 );
}

//! Compute pressure coefficient using the ACM empirical method.
double computeAcmEmpiricalPressureCoefficient(
    double inclinationAngle, double machNumber )
{
    // Declare local variables.
    double pressureCoefficient_;
    double minimumPressureCoefficient_;
    double preliminaryPressureCoefficient_;

    // Set minimum pressure coefficient.
    minimumPressureCoefficient_ = -1.0 / pow( machNumber, 2.0 );

    // Calculate preliminary pressure coefficient.
    preliminaryPressureCoefficient_ = 180.0 / PI * inclinationAngle
            / ( 16.0 * pow( machNumber, 2.0 ) );

    // If necessary, correct preliminary pressure coefficient.
    if ( minimumPressureCoefficient_ > preliminaryPressureCoefficient_ )
    {
        pressureCoefficient_ = minimumPressureCoefficient_;
    }

    else
    {
        pressureCoefficient_ = preliminaryPressureCoefficient_;
    }

    // Return pressure coefficient.
    return pressureCoefficient_;
}

//! Compute Mach number from Prandtl-Meyer function.
double computeInversePrandtlMeyerFunction( double prandtlMeyerFunctionValue )
{
    // Declare local variables.
    double inputVariableForCorrelation_;
    double machNumber_;

    // Determine input variable for correlation.
    inputVariableForCorrelation_ = pow( prandtlMeyerFunctionValue
                                        / maximumPrandtlMeyerFunctionValue, 2.0 / 3.0 );

    // Calculate Mach number.
    machNumber_ = ( 1.0 + inputVariableForCorrelation_
                    * ( PrandtlMeyerParameter1 + inputVariableForCorrelation_
                        * ( PrandtlMeyerParameter2 + inputVariableForCorrelation_
                            * PrandtlMeyerParameter3 ) ) )
            / ( 1.0 + inputVariableForCorrelation_ * ( PrandtlMeyerParameter4
                                                       + inputVariableForCorrelation_
                                                       * PrandtlMeyerParameter5 ) );

    // Return Mach number.
    return machNumber_;
}

//! Compute ratio of post- to pre-shock pressure.
double computeShockPressureRatio( double normalMachNumber,
                                  double ratioOfSpecificHeats )
{
    // Return pressure ratio.
    return 1.0 + 2.0 * ratioOfSpecificHeats / ( ratioOfSpecificHeats + 1.0 )
            * ( normalMachNumber * normalMachNumber - 1.0 );
}

//! Compute ratio of post- to pre-shock density.
double computeShockDensityRatio( double normalMachNumber,
                                 double ratioOfSpecificHeats )
{
    // Declare local variables.
    double machNumberSquared_;

    // Calculate mach number squared for efficiency.
    machNumberSquared_ = pow( normalMachNumber, 2.0 );

    // Return density ratio.
    return ( ratioOfSpecificHeats + 1.0 ) * machNumberSquared_
            / ( 2.0 + ( ratioOfSpecificHeats - 1.0 ) * machNumberSquared_ );
}

//! Compute ratio of post- to pre-shock temperature.
double computeShockTemperatureRatio( double normalMachNumber,
                                     double ratioOfSpecificHeats )
{
    // Return temperature ratio from perfect gas law.
    return 1.0 / computeShockDensityRatio( normalMachNumber, ratioOfSpecificHeats )
            * computeShockPressureRatio( normalMachNumber, ratioOfSpecificHeats );
}

//! Compute jump in entropy across a shock wave.
double computeShockEntropyJump( double normalMachNumber,
                                double ratioOfSpecificHeats,
                                double specificGasConstant )
{
    // Declare local variables.
    double specificHeatConstantPressure_;

    // Calculate specific heat at constant pressure.
    specificHeatConstantPressure_ = ratioOfSpecificHeats * specificGasConstant
            / ( ratioOfSpecificHeats - 1.0 );

    // Return entropy jump from temperature and pressure ratio.
    return specificHeatConstantPressure_
            * log( computeShockTemperatureRatio( normalMachNumber, ratioOfSpecificHeats ) )
            - specificGasConstant
            * log( computeShockPressureRatio( normalMachNumber, ratioOfSpecificHeats ) );
}

//! Compute post- to pre-shock total pressure ratio.
double computeShockTotalPressureRatio( double normalMachNumber,
                                       double ratioOfSpecificHeats,
                                       double specificGasConstant )
{
    // Return total pressure ratio from entropy jump.
    return exp( -1.0 * computeShockEntropyJump( normalMachNumber, ratioOfSpecificHeats,
                                                specificGasConstant ) / specificGasConstant );
}

//! Compute shock deflection angle.
double computeShockDeflectionAngle( double shockAngle, double machNumber,
                                    double ratioOfSpecificHeats )
{
    // Declare local variables.
    double tangentOfDeflectionAngle_;

    // Calculate tangent of deflection angle.
    tangentOfDeflectionAngle_ = 2.0 * ( pow( machNumber * sin( shockAngle ), 2.0 ) - 1.0 )
            / ( tan( shockAngle ) * ( pow( machNumber, 2.0 )
                                      * ( ratioOfSpecificHeats
                                          + cos( 2.0 * shockAngle ) ) + 2.0 ) );

    // Return deflection angle.
    return atan( tangentOfDeflectionAngle_ );
}

//! Function to compute the speed of sound in a gas
double computeSpeedOfSound( const double temperature, const double ratioOfSpecificHeats,
                            const double specificGasConstant )
{
    return std::sqrt( temperature * ratioOfSpecificHeats * specificGasConstant );
}

//! Compute Mach number
double computeMachNumber( const double speed, const double speedOfSound )
{
    return speed / speedOfSound;
}

//! Function to compute the mean free path of a particle.
double computeMeanFreePath( const double weightedAverageCollisionDiameter, const double averageNumberDensity )
{
    return 1.0 / ( std::sqrt( 2.0 ) * mathematical_constants::PI * weightedAverageCollisionDiameter *
                   weightedAverageCollisionDiameter * averageNumberDensity );
}

//! Compute the aerodynamic load experienced by a vehicle.
double computeAerodynamicLoad( const double airDensity,
                               const double airSpeed,
                               const double referenceArea,
                               const double vehicleMass,
                               const Eigen::Vector3d& aerodynamicForceCoefficients )
{
    return computeAerodynamicLoadFromAcceleration(
                0.5 * airDensity * airSpeed * airSpeed * referenceArea * aerodynamicForceCoefficients / vehicleMass );
}

//! Function to compute the aerodynamic load experienced by a vehicle.
double computeAerodynamicLoadFromAcceleration( const Eigen::Vector3d& aerodynamicAccelerationVector )
{
    return aerodynamicAccelerationVector.norm( ) / physical_constants::SEA_LEVEL_GRAVITATIONAL_ACCELERATION;
}

//! Funtion to compute the equilibrium heat flux experienced by a vehicle
double computeEquilibriumHeatflux( const std::function< double( const double ) > heatTransferFunction,
                                   const double wallEmmisivity,
                                   const double adiabaticWallTemperature )
{
    return heatTransferFunction( computeEquilibiumWallTemperature(
                                     heatTransferFunction, wallEmmisivity, adiabaticWallTemperature ) );
}

//! Function to compute the heat flux experienced by a vehicle, assuming an equlibrium wall temperature.
double computeEquilibriumFayRiddellHeatFlux( const double airDensity,
                                             const double airSpeed,
                                             const double airTemperature,
                                             const double machNumber,
                                             const double noseRadius,
                                             const double wallEmissivity )
{
    // Compute adiabatic wall temperature.
    double adiabaticWallTemperature
            = computeAdiabaticWallTemperature( airTemperature , machNumber );

    std::function< double( const double ) > heatTransferFunction = std::bind(
                &computeFayRiddellHeatFlux, airDensity, airSpeed, airTemperature, noseRadius, std::placeholders::_1 );

    return computeEquilibriumHeatflux( heatTransferFunction, wallEmissivity, adiabaticWallTemperature );
}

//! Function to compute the heat flux experienced by a vehicle.
double computeFayRiddellHeatFlux( const double airDensity,
                                  const double airSpeed,
                                  const double airTemperature,
                                  const double noseRadius,
                                  const double wallTemperature )
{
    // Compute the current heat flux.
    return FAY_RIDDEL_HEAT_FLUX_CONSTANT * sqrt( airDensity * std::pow( airSpeed , 2.0 ) / noseRadius )
            * ( 0.5 * std::pow( airSpeed , 2.0 ) + 1004.0 * ( airTemperature - wallTemperature ) );
}

//! Compute the adiabatic wall temperature experienced by a vehicle.
double computeAdiabaticWallTemperature(
        const double airTemperature, const double machNumber, const double ratioSpecificHeats,
        const double recoveryFactor )
{
    double totalTemperature
            = airTemperature * ( 1 + 0.5 * ( ratioSpecificHeats - 1 ) * machNumber * machNumber );

    return airTemperature + recoveryFactor * ( totalTemperature - airTemperature );
}

} // namespace aerodynamics
} // namespace tudat


#include <stan/math/mix/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/digamma.hpp>
#include <test/unit/math/rev/scal/fun/util.hpp>
#include <test/unit/math/mix/scal/fun/nan_util.hpp>

double eps = 1e-6;
double first_deriv_a =
  (stan::math::falling_factorial(5.0 + eps, 3.0)
  - stan::math::falling_factorial(5.0 - eps, 3.0))
  / (2 * eps);
double first_deriv_b =
  (stan::math::falling_factorial(5.0, 3.0 + eps)
  - stan::math::falling_factorial(5.0, 3.0 - eps))
  / (2 * eps);

double eps2 = 1e-4;
double second_deriv_aa =
  (stan::math::falling_factorial(5.0 + 2 * eps2, 3.0)
  - 2 * stan::math::falling_factorial(5.0 + eps2, 3.0)
  + stan::math::falling_factorial(5.0, 3.0))
  / std::pow(eps2, 2);
double second_deriv_bb =
  (stan::math::falling_factorial(5.0, 3.0 + 2 * eps2)
  - 2 * stan::math::falling_factorial(5.0, 3.0 + eps2)
  + stan::math::falling_factorial(5.0, 3.0))
  / std::pow(eps2, 2);
double second_deriv_ab =
  (stan::math::falling_factorial(5.0 + eps2, 3.0 + eps2)
  - stan::math::falling_factorial(5.0 - eps2, 3.0 + eps2)
  - stan::math::falling_factorial(5.0 + eps2, 3.0 - eps2)
  + stan::math::falling_factorial(5.0 - eps2, 3.0 - eps2))
  / 4 / std::pow(eps2, 2);

double third_deriv_aab =
  (stan::math::falling_factorial(5.0 + 2 * eps2, 3.0 + eps2)
  - 2 * stan::math::falling_factorial(5.0 + eps2, 3.0 + eps2)
  + stan::math::falling_factorial(5.0, 3.0 + eps2)
  - stan::math::falling_factorial(5.0 + 2 * eps2, 3.0 - eps2)
  + 2 * stan::math::falling_factorial(5.0 + eps2, 3.0 - eps2)
  - stan::math::falling_factorial(5.0, 3.0 - eps2))
  / 2 / std::pow(eps2, 3);

double third_deriv_abb =
  (stan::math::falling_factorial(5.0 + eps2, 3.0 + 2 * eps2)
  - 2 * stan::math::falling_factorial(5.0 + eps2, 3.0 + eps2)
  + stan::math::falling_factorial(5.0 + eps2, 3.0)
  - stan::math::falling_factorial(5.0 - eps2, 3.0 + 2 * eps2)
  + 2 * stan::math::falling_factorial(5.0 - eps2, 3.0 + eps2)
  - stan::math::falling_factorial(5.0 - eps2, 3.0))
  / 2 / std::pow(eps2, 3);

TEST(AgradFwdFallingFactorial,FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::falling_factorial;

  fvar<var> a(5.0, 1.0);
  fvar<var> b(3.0, 1.0);
  fvar<var> c = falling_factorial(a,b);

  EXPECT_FLOAT_EQ(60, c.val_.val());
  EXPECT_FLOAT_EQ(first_deriv_a + first_deriv_b, c.d_.val());

  AVEC y = createAVEC(a.val_,b.val_);
  VEC g;
  c.val_.grad(y,g);
  EXPECT_FLOAT_EQ(first_deriv_a, g[0]);
  EXPECT_FLOAT_EQ(first_deriv_b, g[1]);
}
TEST(AgradFwdFallingFactorial,FvarVar_2ndDeriv_x) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::falling_factorial;

  fvar<var> a(5.0,1.0);
  fvar<var> b(3.0,0.0);
  fvar<var> c = falling_factorial(a,b);

  AVEC y = createAVEC(a.val_,b.val_);
  VEC g;
  c.d_.grad(y,g);
  ASSERT_NEAR(second_deriv_aa, g[0], 0.1);
}
TEST(AgradFwdFallingFactorial,FvarVar_2ndDeriv_y) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::falling_factorial;

  fvar<var> a(5.0,0.0);
  fvar<var> b(3.0,1.0);
  fvar<var> c = falling_factorial(a,b);

  AVEC y = createAVEC(a.val_,b.val_);
  VEC g;
  c.d_.grad(y,g);
  ASSERT_NEAR(second_deriv_bb, g[1], 0.1);
}
TEST(AgradFwdFallingFactorial,FvarFvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::falling_factorial;

  fvar<fvar<var> > x;
  x.val_.val_ = 5.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 3.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = falling_factorial(x,y);

  EXPECT_FLOAT_EQ(falling_factorial(5, 3.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(first_deriv_a, a.val_.d_.val());
  EXPECT_FLOAT_EQ(first_deriv_b, a.d_.val_.val());
  ASSERT_NEAR(second_deriv_ab, a.d_.d_.val(), .01);

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.val_.val_.grad(p,g);
  EXPECT_FLOAT_EQ(first_deriv_a, g[0]);
  EXPECT_FLOAT_EQ(first_deriv_b, g[1]);
}

TEST(AgradFwdFallingFactorial,FvarFvarVar_2ndDeriv_x) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::falling_factorial;

  fvar<fvar<var> > x;
  x.val_.val_ = 5.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 3.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = falling_factorial(x,y);

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.val_.d_.grad(p,g);
  ASSERT_NEAR(second_deriv_aa, g[0], 0.01);
  ASSERT_NEAR(second_deriv_ab, g[1], 0.01);
}
TEST(AgradFwdFallingFactorial,FvarFvarVar_2ndDeriv_y) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::falling_factorial;

  fvar<fvar<var> > x;
  x.val_.val_ = 5.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 3.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = falling_factorial(x,y);

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.d_.val_.grad(p,g);
  ASSERT_NEAR(second_deriv_ab, g[0], 0.01);
  ASSERT_NEAR(second_deriv_bb, g[1], 0.01);
}
TEST(AgradFwdFallingFactorial,FvarFvarVar_3rdDeriv) {
  using stan::math::fvar;
  using stan::math::var;
  using stan::math::falling_factorial;

  fvar<fvar<var> > x;
  x.val_.val_ = 5.0;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 3.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = falling_factorial(x,y);

  AVEC p = createAVEC(x.val_.val_,y.val_.val_);
  VEC g;
  a.d_.d_.grad(p,g);
  ASSERT_NEAR(third_deriv_aab, g[0], 0.03);
  ASSERT_NEAR(third_deriv_abb, g[1], 0.03);
}

struct falling_factorial_fun {
  template <typename T0, typename T1>
  inline
  typename boost::math::tools::promote_args<T0,T1>::type
  operator()(const T0 arg1,
             const T1 arg2) const {
    return falling_factorial(arg1,arg2);
  }
};

TEST(AgradFwdFallingFactorial, nan) {
  falling_factorial_fun falling_factorial_;
  test_nan_mix(falling_factorial_,3.0,5.0,false);
}


//
//  util.cpp
//  calib
//
//  Created by jimmy on 2017-07-27.
//  Copyright (c) 2017 Nowhere Planet. All rights reserved.
//

#include "util.h"
#include <iostream>

#include <dirent.h>
#include <string.h>

// Eigen
#include <Eigen/Geometry>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

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

static Eigen::Vector3d rotation_to_rodrigues(const Eigen::Matrix3d &r)
{
    Eigen::AngleAxisd aa(r);

   // Eigen::Vector3d axis = aa.axis()*aa.angle();

    double ang = aa.angle();
    if (ang == 0.0) {
        return Eigen::Vector3d::Zero();
    }
    return aa.axis()*(ang);
}

bool writeCamera(const char *fileName, const char *imageName, const perspective_camera & camera)
{
    assert(fileName);
    assert(imageName);

    FILE *pf = fopen(fileName, "w");
    if (!pf) {
        printf("can not create file %s\n", fileName);
        return false;
    }

    fprintf(pf, "%s\n", imageName);
    fprintf(pf, "ppx\t ppy\t focal length\t Rx\t Ry\t Rz\t Cx\t Cy\t Cz\n");
    double ppx = camera.principal_point().x();
    double ppy = camera.principal_point().y();
    double fl = camera.get_calibration()(0, 0);
    Eigen::Matrix3d r = camera.get_rotation();
    Eigen::Vector3d rod = rotation_to_rodrigues(r);

    double Rx = rod.x();
    double Ry = rod.y();
    double Rz = rod.z();
    double Cx = camera.get_camera_center().x();
    double Cy = camera.get_camera_center().y();
    double Cz = camera.get_camera_center().z();
    fprintf(pf, "%f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\n", ppx, ppy, fl, Rx, Ry, Rz, Cx, Cy, Cz);
    fclose(pf);
    return true;
}

bool readCamera(const char *fileName, string & imageName, perspective_camera & camera)
{
    assert(fileName);
    FILE *pf = fopen(fileName, "r");
    if (!pf) {
        printf("can not open file %s\n", fileName);
        return false;
    }
    char buf[1024] = {NULL};
    int num = fscanf(pf, "%s\n", buf);
    assert(num == 1);
    imageName = string(buf);
    for (int i = 0; i<1; i++) {
        char lineBuf[BUFSIZ] = {NULL};
        fgets(lineBuf, sizeof(lineBuf), pf);
        cout<<lineBuf;
    }
    double ppx, ppy, fl, rx, ry, rz, cx, cy, cz;
    int ret = fscanf(pf, "%lf %lf %lf %lf %lf %lf %lf %lf %lf", &ppx, &ppy, &fl, &rx, &ry, &rz, &cx, &cy, &cz);
    if (ret != 9) {
        printf("Error: read camera parameters!\n");
        return false;
    }

    Eigen::Matrix3d K;
    K.setIdentity();
    K(0, 0) = fl;
    K(1, 1) = fl;
    K(0, 2) = ppx;
    K(1, 2) = ppy;

    Eigen::Vector3d rod(rx, ry, rz);
    Eigen::Vector3d cc(cx, cy, cz);

    camera.set_calibration(K);
    camera.set_rotation(rod);
    camera.set_camera_center(cc);
    fclose(pf);

    return true;
}

void readFilenames(const char *folder, vector<string> & file_names)
{
    const char *post_fix = strrchr(folder, '.');
    string pre_str(folder);
    pre_str = pre_str.substr(0, pre_str.rfind('/') + 1);
    //printf("pre_str is %s\n", pre_str.c_str());

    assert(post_fix);
    // vcl_vector<vcl_string> file_names;
    DIR *dir = NULL;
    struct dirent *ent = NULL;
    if ((dir = opendir (pre_str.c_str())) != NULL) {
        /* print all the files and directories within directory */
        while ((ent = readdir (dir)) != NULL) {
            const char *cur_post_fix = strrchr( ent->d_name, '.');
            if (!cur_post_fix ) {
                continue;
            }
            //printf("cur post_fix is %s %s\n", post_fix, cur_post_fix);

            if (!strcmp(post_fix, cur_post_fix)) {
                file_names.push_back(pre_str + string(ent->d_name));
                //  cout<<file_names.back()<<endl;
            }

            //printf ("%s\n", ent->d_name);
        }
        closedir (dir);
    }
    printf("read %lu files\n", file_names.size());
}

namespace {
    struct PureRotateFunctor
    {
        typedef double Scalar;

        typedef Eigen::VectorXd InputType;
        typedef Eigen::VectorXd ValueType;
        typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> JacobianType;

        enum {
            InputsAtCompileTime = Eigen::Dynamic,
            ValuesAtCompileTime = Eigen::Dynamic
        };

        vector<Eigen::Vector2d > pts1_;
        vector<Eigen::Vector2d > pts2_;
        Eigen::Matrix3d invR1_;
        Eigen::Matrix3d invK1_;
        Eigen::Vector2d pp_;   // principle point

        int m_inputs;
        int m_values;

        PureRotateFunctor()
        {
            m_inputs = 5;
            m_values = 10;
        }

        void setValue(const vector<Eigen::Vector2d >& pts1,
                      const vector<Eigen::Vector2d >& pts2,
                      const Eigen::Matrix3d& invR1,
                      const Eigen::Matrix3d& invK1,
                      const Eigen::Vector2d& pp)
        {
            pts1_ = pts1;
            pts2_ = pts2;
            invR1_ = invR1;
            invK1_ = invK1;
            pp_ = pp;
            m_inputs = 5;
            m_values = 2*(int)pts1.size();
        }

        int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fx) const
        {
            double fl = x[0];
            double qx = x[1];
            double qy = x[2];
            double qz = x[3];
            double qw = x[4];

            Eigen::Quaternion<double> q(qw, qx, qy, qz);
            Eigen::Matrix3d R2 = q.normalized().toRotationMatrix();

            Eigen::Matrix3d K2;
            K2.fill(0);
            K2(0, 0) = K2(1, 1) = fl;
            K2(0, 2) = pp_.x();
            K2(1, 2) = pp_.y();
            K2(2, 2) = 1.0;

            // x2 = K_2 * R_2 * R_1^{-1} * K_1^{-1} x1
            int idx = 0;
            for (int i = 0; i<pts1_.size(); i++) {
                Eigen::Vector3d p(pts1_[i].x(), pts1_[i].y(), 1.0);
                Eigen::Vector3d q = K2 * R2 * invR1_ * invK1_ * p;
                double x = q[0]/q[2];
                double y = q[1]/q[2];

                fx[idx] = pts2_[i].x() - x;
                idx++;
                fx[idx] = pts2_[i].y() - y;
                idx++;
            }

            return 0;
        }

        int inputs() const { return m_inputs; }// inputs is the dimension of x.
        int values() const { return m_values; } // "values" is the number of f_i and

        void setCameraMatrixRotation(const Eigen::VectorXd& x,
                                     cvx::perspective_camera& camera)
        {
            double fl = x[0];
            double qx = x[1];
            double qy = x[2];
            double qz = x[3];
            double qw = x[4];

            Eigen::Quaternion<double> q(qw, qx, qy, qz);
            Eigen::Matrix3d R2 = q.normalized().toRotationMatrix();
            camera.set_rotation(R2);

            camera.set_calibration(fl, pp_.x(), pp_.y());
        }
    };
}

bool calibratePureRotateCamera(const vector<Eigen::Vector2d > & pts1,
                               const vector<Eigen::Vector2d > & pts2,
                               const cvx::perspective_camera & camera1,
                               cvx::perspective_camera & camera2)
{
    assert(pts1.size() == pts2.size());
    assert(pts1.size() >= 4);

   // vnl_matrix_fixed<double, 3, 3> invR1 = vnl_inverse(camera1.get_rotation().as_matrix());
   // vnl_matrix_fixed<double, 3, 3> invK1 = vnl_inverse(camera1.get_calibration().get_matrix());
   // vgl_point_2d<double> pp = camera1.get_calibration().principal_point();
    Eigen::Matrix3d invR1 = camera1.get_rotation().inverse();
    Eigen::Matrix3d invK1 = camera1.get_calibration().inverse();
    Eigen::Vector2d pp = camera1.principal_point();

    Eigen::Quaternion<double> q(camera1.get_rotation());

    Eigen::VectorXd x(5);
    x[0] = camera1.focal_length();
    x[1] = q.x();
    x[2] = q.y();
    x[3] = q.z();
    x[4] = q.w();

    PureRotateFunctor myFunctor;
    myFunctor.setValue(pts1, pts2, invR1, invK1, pp);
    Eigen::NumericalDiff<PureRotateFunctor> numericalDiffMyFunctor(myFunctor);
    Eigen::LevenbergMarquardt<Eigen::NumericalDiff<PureRotateFunctor>, double> levenbergMarquardt(numericalDiffMyFunctor);

    levenbergMarquardt.parameters.ftol = 1e-6;
    levenbergMarquardt.parameters.xtol = 1e-6;
    levenbergMarquardt.parameters.maxfev = 100; // Max iterations

    Eigen::VectorXd xmin = x; // initialize
    levenbergMarquardt.minimize(xmin);

    myFunctor.setCameraMatrixRotation(xmin, camera2);
    camera2.set_camera_center(camera1.get_camera_center());

   // std::cout << "x that minimizes the function: " << xmin << std::endl;

   // x[1] = camera1.get_rotation().as_rodrigues()[0];
   // x[2] = camera1.get_rotation().as_rodrigues()[1];
   // x[3] = camera1.get_rotation().as_rodrigues()[2];

    /*

    calibrate_pure_rotate_camera_residual residual(pts1, pts2, invR1, invK1, pp);

    vnl_levenberg_marquardt lmq(residual);
    lmq.set_f_tolerance(0.0001);

    bool isMinized = lmq.minimize(x);
    if (!isMinized) {
        vcl_cerr<<"Error: minimization failed.\n";
        lmq.diagnose_outcome();
        return false;
    }
    lmq.diagnose_outcome();

    residual.setCameraMatrixRotation(x, camera2);
    camera2.set_camera_center(camera1.get_camera_center());
    return true;
     */

    return true;
}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/range/irange.hpp>

#include <iostream>

using namespace boost;
template < typename TimeMap > class bfs_time_visitor:public default_bfs_visitor {
  typedef typename property_traits < TimeMap >::value_type T;
public:
  bfs_time_visitor(TimeMap tmap, T & t):m_timemap(tmap), m_time(t) { }
  template < typename Vertex, typename Graph >
    void discover_vertex(Vertex u, const Graph & g) const
  {
    put(m_timemap, u, m_time++);
  }
  TimeMap m_timemap;
  T & m_time;
};

int
main()
{
  using namespace boost;
  // Select the graph type we wish to use
  typedef adjacency_list < vecS, vecS, undirectedS > graph_t;
  // Set up the vertex IDs and names
  enum { r, s, t, u, v, w, x, y, N };
  const char *name = "rstuvwxy";
  // Specify the edges in the graph
  typedef std::pair < int, int >E;
  E edge_array[] = { E(r, s), E(r, v), E(s, w), E(w, r), E(w, t),
    E(w, x), E(x, t), E(t, u), E(x, y), E(u, y)
  };
  // Create the graph object
  const int n_edges = sizeof(edge_array) / sizeof(E);
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  // VC++ has trouble with the edge iterator constructor
  graph_t g(N);
  for (std::size_t j = 0; j < n_edges; ++j)
    add_edge(edge_array[j].first, edge_array[j].second, g);
#else
  typedef graph_traits<graph_t>::vertices_size_type v_size_t;
  graph_t g(edge_array, edge_array + n_edges, v_size_t(N));
#endif

  // Typedefs
  typedef graph_traits < graph_t >::vertices_size_type Size;

  // a vector to hold the discover time property for each vertex
  std::vector < Size > dtime(num_vertices(g));
  typedef
    iterator_property_map<std::vector<Size>::iterator,
                          property_map<graph_t, vertex_index_t>::const_type>
    dtime_pm_type;
  dtime_pm_type dtime_pm(dtime.begin(), get(vertex_index, g));

  Size time = 0;
  bfs_time_visitor < dtime_pm_type >vis(dtime_pm, time);
  breadth_first_search(g, vertex(s, g), visitor(vis));

  // Use std::sort to order the vertices by their discover time
  std::vector<graph_traits<graph_t>::vertices_size_type > discover_order(N);
  integer_range < int >range(0, N);
  std::copy(range.begin(), range.end(), discover_order.begin());
  std::sort(discover_order.begin(), discover_order.end(),
            indirect_cmp < dtime_pm_type, std::less < Size > >(dtime_pm));

  std::cout << "order of discovery: ";
  for (int i = 0; i < N; ++i)
    std::cout << name[discover_order[i]] << " ";
  std::cout << std::endl;

  return EXIT_SUCCESS;
}


/*
   (c) Copyright 2012, Hewlett-Packard Development Company, LP

   See the file named COPYING for license details
*/

/** @file
    Interpolation search template
*/

#include <vector>
#include <iostream>

#include <boost/format.hpp>

#include <Lintel/DebugFlag.hpp>
#include <Lintel/unstable/InterpolationSearch.hpp>
#include <Lintel/MersenneTwisterRandom.hpp>

using namespace std;
using lintel::interpolationLowerBound;
using boost::format;

// static global so that the fact that std::lower_bound copies the comparator doesn't cause
// any problems.

size_t eo_count, compare_count;

template<typename ValueT> struct EstimateOffsetCount {
    size_t
    operator()(const ValueT &v, const ValueT &first, const ValueT &last, size_t last_pos) const {
        ++eo_count;
        ValueT value_range = last - first;
        double relative_pos = static_cast<double>(v - first) / value_range; // [0,1]
        return relative_pos * last_pos;
    }

};

template<typename ValueT> struct CompareCount {
    bool operator()(const ValueT &a, const ValueT &b) const {
        ++compare_count;
        return a < b;
    }
};

template<typename ValueT, typename Iterator> Iterator
countingILB(Iterator begin, Iterator end, const ValueT &v) {
    EstimateOffsetCount<ValueT> eo;
    return lintel::detail::interpolationLowerBound<2,4,256>(begin, end, v, eo,
                                                            CompareCount<ValueT>());
}

void testSimple() {
    vector<int> array;

    for(int i=0; i < 1000; ++i) {
        array.push_back(i);
    }

    SINVARIANT(interpolationLowerBound<int>(array.begin(), array.end(), -1) == array.begin());
    SINVARIANT(interpolationLowerBound(array.begin(), array.end(), 1000) == array.end());
    for(int i=0; i < 1000; ++i) {
        vector<int>::iterator iter = interpolationLowerBound(array.begin(), array.end(), i);
        SINVARIANT(iter >= array.begin() && iter < array.end());
        SINVARIANT(*iter == i);
    }

    // Now with counting
    SINVARIANT(countingILB<int>(array.begin(), array.end(), -1) == array.begin()); // 1 comparison
    SINVARIANT(countingILB(array.begin(), array.end(), 1000) == array.end()); // 2 comparisons
    for(int i=0; i < 1000; ++i) {
        // 1 comparison for 0 (same as -1), 1 eo, 4 comparisons for rest
        vector<int>::iterator iter = countingILB(array.begin(), array.end(), i);
        SINVARIANT(iter >= array.begin() && iter < array.end());
        SINVARIANT(*iter == i);
    }
    cout << format("%d offset estimations, %d comparisions\n") % eo_count % compare_count;

    size_t expect_compare = 1 + 2 + 1 + 4 * 999;
    IF_LINTEL_DEBUG(expect_compare += 2 * 999); // debug checks cost another 2 compares/cycle
    SINVARIANT(eo_count == 999 && compare_count == expect_compare);

    array.clear();
    array.resize(1000);
    for (ssize_t i = array.size() - 1; i >= 0; --i) {
        array[i] = 2;
        vector<int>::iterator at = interpolationLowerBound(array.begin(), array.end(), 1);
        INVARIANT(at == (array.begin() + i), format("%d != %d") % i % (at - array.begin()));
    }
    cout << "test simple passed.\n";

}

void testRandom() {
    MersenneTwisterRandom rng;
    cout << format("test random seed=%d...") % rng.seedUsed();

    static const size_t nvals = 256 * 1024;

    vector<int> array;

    for(size_t i=0; i < nvals; ++i) {
        // * 16 == some duplicates, mostly unique, efficient calculation
        array.push_back(rng.randInt(nvals*16));
    }
    sort(array.begin(), array.end());

    for(size_t i=0;i < nvals; ++i) {
        int v = rng.randInt(nvals*10);
        vector<int>::iterator ilb = interpolationLowerBound(array.begin(), array.end(), v);
        vector<int>::iterator lb = lower_bound(array.begin(), array.end(), v);
        INVARIANT(ilb == lb, format("for %d: %d != %d") % v % (ilb == array.end() ? -1 : *ilb)
                  % (lb == array.end() ? -1 : *lb));
    }

    cout << "passed.\n";
}

int main() {
    testSimple();
    testRandom();
    return 0;
}


#include <iostream>
#include <vector>
#include <boost/algorithm/string.hpp>
using namespace std;

double measureAUC(vector <float> &score, vector <int> &labels){

    int numInst = score.size();
    vector<pair<float, float> > scorelabels;
    for(int j=0; j<numInst; j++){
        scorelabels.push_back(make_pair(score[j], labels[j]));
    }

    //Sort the scores in descending order.
    sort(scorelabels.rbegin(), scorelabels.rend());

    //Update the score and labels vectors to make them appear in the sorted order of scores.
    for(int i=0; i<numInst; i++){
        score[i]=scorelabels[i].first;
        labels[i]=scorelabels[i].second;
    }

    //posLabel = 1: Data Anomalies
    //negLabel = 0: Normal Data
    int posLabel = 1;
    int negLabel = 0;
    int countPos = 0;
    int countNeg = 0;

    for(int j=0; j<numInst; j++){
        if(labels[j]==1){
            countPos++;
        }
        else{
            countNeg++;
        }
    }

    double accumPos = 0;
    double accumNeg = 0;
    double accumAuc = 0;

    double unitPos = (double)1/(double)countPos;
    double unitNeg = (double)1/(double)countNeg;

    int i=0;
    while(i<numInst){

        double temp = accumPos;
        if (i<numInst-2 && score[i] == score[i + 1]){
            while (i<numInst-2 && score[i] == score[i + 1]){
                if(labels[i] == negLabel){
                    accumNeg = accumNeg + 1;
                }
                else{
                    accumPos = accumPos + 1;
                }
                i++;
            }

            if(labels[i] == negLabel){
                accumNeg = accumNeg + 1;
            }
            else{
                accumPos = accumPos + 1;
            }

            accumAuc = accumAuc + (accumPos + temp) * unitPos * accumNeg * unitNeg / 2;
            accumNeg = 0;
        }
        else{
            if(labels[i] == negLabel){
                accumNeg = accumNeg + 1;
                accumAuc = accumAuc + accumPos * unitPos * accumNeg * unitNeg;
                accumNeg = 0;
            }
            else{
                accumPos = accumPos + 1;
            }
        }
        i++;
    }
    return accumAuc;

}