#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


#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


/*
 * 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


//
//  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;
}


#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;

}


/*
 This program is free software; you can redistribute it and/or modify it under
 the terms of the European Union Public Licence - EUPL v.1.1 as published by
 the European Commission.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE. See the European Union Public Licence - EUPL v.1.1
 for more details.

 You should have received a copy of the European Union Public Licence - EUPL v.1.1
 along with this program.

 Further information about the European Union Public Licence - EUPL v.1.1 can
 also be found on the world wide web at http://ec.europa.eu/idabc/eupl

*/

/*
 ------ Copyright (C) 2010 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
*/

/*
 ------------------ Author: Tiziana Sabatini  ------------------------------------------------
 ------------------ E-mail: (tiziana.sabatini@yahoo.it) --------------------------------------
 Patched by Guillermo to correct the behaviour of the + button on the 3rd body selection panel. Nov 09
 */

#include "perturbations.h"
#include "Atmosphere/AtmosphereModel.h"
#include "cartesianTOorbital.h"
#include "cartesianTOspherical.h"
#include "statevector.h"
#include "stabody.h"
#include "stamath.h"
#include "date.h"
#include "math.h"
#include "inertialTOfixed.h"
#include "getGreenwichHourAngle.h"
#include <QFile>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "Entry/capsule.h"
#include <QErrorMessage>
#include <QDebug>

const double PI = 3.141592;

Perturbations::Perturbations()
{
}

Perturbations::~Perturbations()
{
}

// TODO: This method should be abstract
Vector3d
Perturbations::calculateAcceleration(sta::StateVector /* state */, double /* time */, double /* dt */)
{
    return Vector3d::Zero();
}

/////////////////////////////// Gravity Field Perturbation ///////////////////////////////
GravityPerturbations::GravityPerturbations(const StaBody* centralBody,
                                           const ScenarioGravityModel* gravityModel)
{
    m_body = centralBody;
    m_modelName = gravityModel->modelName();
    m_zonalCount = gravityModel->numberOfZonals();
    m_tesseralCount = gravityModel->numberOfTesserals();

    //Assigning dimension to matrices of harmonical coefficients
    J.resize(m_zonalCount + 1);
    JJ.resize(m_zonalCount + 1 , m_tesseralCount + 1);
    gamma.resize(m_zonalCount + 1, m_tesseralCount + 1);

    loadGravityConstants();
}

GravityPerturbations::~GravityPerturbations()
{
}

Vector3d GravityPerturbations::calculateAcceleration(sta::StateVector state, double time, double dt)
{
    double R = body()->equatorialRadius();
    double mu = body()->mu();
    double greenwich = getGreenwichHourAngle (time);

    int n_zonals = zonalCount();
    int m_tesserals = tesseralCount();

    //Operating coordinate conversion from inertial cartesian to earth-fixed spherical
    double longitude, latitude, r, V, g, chi;
    VectorXd state_f(6);

    //TO DO : change the number 0 and put directly the stabody
    inertialTOfixed(0, greenwich,
                state.position.x(), state.position.y(), state.position.z(),
                state.position.x(), state.position.y(), state.position.z(),
                state_f(0), state_f(1), state_f(2), state_f(3), state_f(4), state_f(5));

    cartesianTOspherical(state_f(0), state_f(1), state_f(2), state_f(3), state_f(4), state_f(5),
                         longitude, latitude, r, V, g, chi);

    //Calculating the accelerations
    double derivative_r_zonals(0);
    double derivative_r_tesserals(0);
    double derivative_lat_zonals(0);
    double derivative_lat_tesserals(0);
    double derivative_long_tesserals(0);

    double x = sin(latitude);

    //P Legendre Polynomials, PP associated Legendre functions of the first kind
    VectorXd P(n_zonals + 1);
    MatrixXd PP(n_zonals + 1, m_tesserals + 1);

    //Assign values of P and PP with index below 2
    P(0) = 1;
    PP(0,0) = P(0);
    if (n_zonals != 0)
    {
        P(1) = x;
        PP(1,0) = P(1);
    }
    if (m_tesserals != 0)
        PP(1,1) = - pow((1 - pow(x,2.0)),0.5);

    for (int n = 2; n <= n_zonals ; n++)
    {
        //Legendre polynomials calculation:
        P(n) = ((2*n - 1) * P(n-1) * x - (n-1) * P(n-2)) / n;  //three term recurrence relation
        PP(n,0) = P(n);

        //Spherical coordinates derivative calculation (zonals terms):
        derivative_r_zonals += ((n+1) * J(n) * pow(R,n) * P(n) * pow(r,-(n+2)));
        derivative_lat_zonals += - cos(latitude) * J(n) * pow(r,-(n+2)) * pow(R,n) * (1.0  / (pow(x,2) - 1) * n * (x*P(n) - P(n-1)));

        for (int m = 1; (m <= m_tesserals && m <= n) ; m++)
        {
            //Associated Legendre functions calculation:
            if(n == m)
            {
                PP(n,m) = pow(-1.0,n) * doublefactorial(2*n - 1) * pow((1 - pow(x,2.0)),n/2);
            }
            else if(n == m+1)
            {
                PP(n,m) = x * (2*m + 1) * PP(n-1,m);
            }
            else
            {
                PP(n,m) = 1/(n-m) * ((2*n-1) * x * PP(n-1,m) - (n+m-1) * PP(n-2,m));
            }

            //Spherical coordinates derivative calculation (tesserals terms):
            derivative_r_tesserals += (n+1) * JJ(n,m) * pow(R,n) * PP(n,m) * pow(r,-(n+2)) * cos(m * (longitude - gamma(n,m)));
            derivative_lat_tesserals += - sin(latitude)/pow(r,n+2) * JJ(n,m) * pow(R,n) * (-(n+m) * (n-m+1) * sqrt(1 - (pow(x,2))) / ((pow(x,2)) -1) * PP(n,m-1) - m * x * PP(n,m) / (pow(x,2) - 1)) * cos(m * (longitude - gamma(n,m)));
            derivative_long_tesserals +=  ((1/pow(r,n+2) * JJ(n,m) * pow(R,n) * PP(n,m)) * m * sin(m*(longitude - gamma(n,m))))/cos(latitude) ;

        }
    }

    Vector3d acc_spherical;
    acc_spherical.x() = mu * (derivative_r_zonals + derivative_r_tesserals);
    acc_spherical.y() = -mu * (derivative_lat_zonals + derivative_lat_tesserals);
    acc_spherical.z() = mu * derivative_long_tesserals;

    //TO DO change the following line and use astro-core functions
    //Coordinates transformation from spherical/fixed to cartesian/inertial
    Vector3d acceleration;
    Matrix3d rotation1, rotation2;

    rotation1 << cos(latitude) * cos(longitude), -sin(longitude)*cos(latitude), -sin(latitude),
                sin(longitude), cos(longitude), 0,
                sin(latitude) * cos(longitude), -sin(latitude) * sin(longitude), cos(latitude);
    rotation2 << cos(greenwich), -sin(greenwich), 0,
                sin(greenwich), cos(greenwich), 0,
                0, 0, 1;

    acceleration = rotation1 * rotation2 * acc_spherical;

    return acceleration;
}

void GravityPerturbations::loadGravityConstants()
{
    //Assign a null value to constant harmonics below index 2
    J(0) = 0;
    J(1) = 0;
    JJ(0,0) = 0;    gamma(0,0) = 0;
    JJ(1,0) = 0;    gamma(1,0) = 0;
    if (m_tesseralCount != 0)
    {
        JJ(1,1) = 0;
        gamma(1,1) = 0;
    }

    //Open the file .stad containing the normalized gravity constants.
    //The number of loaded constants is consistent with the accuracy order the user selected.
    QString path = QString("data/bodies/");
    path.append(m_modelName);

    //QTextStream out (stdout); out << "===> bodies path: " << path << endl;

    QFile gravity(path);

    if (!gravity.open(QIODevice::ReadOnly))
    {
        // TODO: It's an error if the gravity model doesn't exist! Need a mechanism
        // for reporting this to the user.
        return;
    }

    QTextStream gravitystream(&gravity);

    MatrixXd C(m_zonalCount + 1, m_zonalCount + 1), S(m_zonalCount + 1, m_zonalCount + 1);
    int n = 0, m = 0;

    while (gravitystream.status() == QTextStream::Ok)
    {
        gravitystream >> n;
        if (n > m_zonalCount) break;
        gravitystream >> m;
        gravitystream >> C(n,m);
        gravitystream >> S(n,m);
    }
    gravity.close();

    //Converting the gravity constants.
    for (int i = 2 ; i <= m_zonalCount ; i++)
    {
        J(i) = - sqrt(2*i + 1.0) * C(i,0);
        if (m_tesseralCount != 0)
        {
            for (int j = 1 ; (j <= m_tesseralCount && j <= i) ; j++)
            {
                gamma(i,j) = atan(S(i,j)/C(i,j)) / j;
                JJ(i,j) = - fabs(sqrt(factorial(i-j) * (2*i + 1) * 2 / factorial(i+j)) * C(i,j) / cos(j * gamma(i,j)));
            }
        }
    }
}

double factorial(int num)
{
    if (num==0 || num==1)
    return 1.0;
    return factorial(num-1)*num;
}

double doublefactorial(int num)
{
    if (num==0 || num==1)
    return 1.0;
    return doublefactorial(num-2)*num;
}

/////////////////////////////// Atmospheric Drag Perturbation ///////////////////////////////
AtmosphericDragPerturbations::AtmosphericDragPerturbations(const QString& atmosphereModel)
{
#if OLDSCENARIO
    m_atmosphericModel = perturbation->atmosphericModel();
    m_body = perturbation->centralBody();
    m_surface = properties->physicalProperties()->physicalCharacteristics()->surfaceArea();
    m_cdCoefficients = properties->aerodynamicProperties()->CDCoefficients();
    m_mass = properties->physicalProperties()->physicalCharacteristics()->mass();
#endif
}

AtmosphericDragPerturbations::~AtmosphericDragPerturbations()
{
}

Vector3d
AtmosphericDragPerturbations::calculateAcceleration(sta::StateVector state, double time, double dt)
{
    //Conversion from kilometers to meters has been made.
    //Calculating the density from the altitude:
    AtmosphereModel atmosphere;
    atmosphere.selectModel(atmosphericModel());
    double h = altitude(body(), state, time);
    double rho = atmosphere.density(h * pow(10,3.0)) * pow(10,9.0);

    //Calculating the CD coefficient from the altitude (calling re-entry module functions)
    capsule_class vehicle;
    vehicle.selectCdCprofile(cdCoefficients());
    double cd = vehicle.cdc(h * pow(10,3.0));

    //Considering the atmosphere rotating with the Earth
    //TO DO put omega as a property of StaBody; only earth rotation has been considered
    double omega = 7.29211585494e-5;
    state.velocity.x() += omega * state.position.y();
    state.velocity.y() -= omega * state.position.x();

    //Calculating the accelerations vector
    Vector3d acceleration = - 0.5 * rho * state.velocity.norm() * state.velocity * (cd * surface()*pow(10,-6.0))/mass();

    return acceleration;
}

/////////////////////////////// Solar Pressure Perturbation ///////////////////////////////
SolarPressurePerturbations::SolarPressurePerturbations(StaBody* centralBody,
                                                       double reflectivity,
                                                       double albedo,
                                                       double ir,
                                                       double mass,
                                                       double surfaceArea) :
    m_body(centralBody),
    m_reflectivity(reflectivity),
    m_albedo(albedo),
    m_ir(ir),
    m_mass(mass),
    m_surface(surfaceArea)
{
}

SolarPressurePerturbations::~SolarPressurePerturbations()
{
}

Vector3d
SolarPressurePerturbations::calculateAcceleration(sta::StateVector state, double time, double dt)
{
    //calculation of the Sun position via the ephemeris
    sta::StateVector sunVector = STA_SOLAR_SYSTEM->sun()->stateVector(time, body(), sta::COORDSYS_EME_J2000);

    double sunAscension = atan(sunVector.position.y()/sunVector.position.x());
    double sunDeclination = atan(sunVector.position.z() / sqrt(pow(sunVector.position.y(),2.0) + pow(sunVector.position.x(),2.0)));

    double distancePlanetSun = sunVector.position.norm();
    double distancePlanetSat = state.position.norm();
    double distanceSunSat = (sunVector.position - state.position).norm();

    //Modeling the solar radiation (using Km)
    double sigma = 5.6704 * pow(10,-8.0); //Boltzmann constant
    double Tsun = 5778; //Sun temperature
    double Rsun = 6.955 * pow(10,5.0); // Sun radius

    double W = sigma * pow(Tsun, 4.0) * pow((Rsun / distanceSunSat), 2.0);

    //Calculation of the acceleration module
    double c = 300000;
    double fp = (1 + reflectivity()) * W * (surface() * pow(10,-6.0)) / c / mass();

    Vector3d accelerationAlbedo(0,0,0);
    Vector3d accelerationIR(0,0,0);
    //Calculation of the albedo and infra-red radiations
    if (albedo())
    {
        double Walbedo = albedoReflectivityCoefficient(body()) * W * pow((body()->meanRadius() / distancePlanetSat), 2.0);
        double fpalbedo = (1 + reflectivity()) * Walbedo * (surface() * pow(10,-6.0)) / c / mass();
        accelerationAlbedo = fpalbedo * state.position/distancePlanetSat;
    }

    if (ir())
    {
        double Wir = irRadiationFlux(body()) * pow((body()->meanRadius() / distancePlanetSat), 2.0);
        double fpir = (1 + reflectivity()) * Wir * (surface() * pow(10,-6.0)) / c / mass();
        accelerationIR = fpir * state.position/distancePlanetSat;
    }

    Vector3d acceleration(-fp * cos(sunAscension) * cos(sunDeclination),
                          -fp * sin(sunAscension) * cos(sunDeclination),
                          -fp * sin(sunDeclination));

    //calculation of the Earth shadow; if the spacecraft is overshadowed by the Earth the solar pressure will be ignored
    double gamma = acos((pow(distanceSunSat,2.0) + pow(distancePlanetSun,2.0) - pow(distancePlanetSat,2.0)) / (2 * distancePlanetSun * distanceSunSat));
    double gamma_c = atan(body()->meanRadius() / distancePlanetSun);
//
//    if (gamma < gamma_c)
//    {
//        if (distanceSunSat > distancePlanetSun)
//            acceleration << 0, 0, 0;
//    }

    acceleration += accelerationAlbedo + accelerationIR;

    return acceleration;
}

double albedoReflectivityCoefficient(const StaBody* body)
{
    switch(body->id())
    {
    case STA_MERCURY:
        return 0.53;
    case STA_VENUS:
        return 0.76;
    case STA_EARTH:
        return 0.35;
    case STA_MARS:
        return 0.16;
    case STA_JUPITER:
        return 0.73;
    case STA_SATURN:
        return 0.76;
    case STA_URANUS:
        return 0.93;
    case STA_NEPTUNE:
        return 0.84;
    case STA_PLUTO:
        return 0.14;
    case STA_SUN:
        return 0;
    case STA_MOON:
        return 0.067;
    default:
        return 0;
    }
}

double irRadiationFlux(const StaBody* body)
{
    switch(body->id())
    {
    case STA_MERCURY:
        return 2139;
    case STA_VENUS:
        return 155;
    case STA_EARTH:
        return 240;
    case STA_MARS:
        return 123;
    case STA_JUPITER:
        return 3.4;
    case STA_SATURN:
        return 0.9;
    case STA_URANUS:
        return 0.063;
    case STA_NEPTUNE:
        return 0.06;
    case STA_PLUTO:
        return 0.191;
    case STA_SUN:
        return 0;
    case STA_MOON:
        return 316;
    default:
        return 0;
    }
}

/////////////////////////////// Third Body Perturbation ///////////////////////////////

ExternalBodyPerturbations::ExternalBodyPerturbations(const StaBody* centralBody,
                                                     const QList<const StaBody*>& bodies) :
    m_body(centralBody),
    m_perturbingBodyList(bodies)
{
}

ExternalBodyPerturbations::~ExternalBodyPerturbations()
{
}

Vector3d
ExternalBodyPerturbations::calculateAcceleration(sta::StateVector state, double time, double dt)
{
    Vector3d acceleration(0.0, 0.0, 0.0);

    foreach(const StaBody* thirdbody,  perturbingBodyList())
    {
        double mu = thirdbody->mu();
        sta::StateVector thirdbodystate = thirdbody->stateVector(time, body(), sta::COORDSYS_EME_J2000);

        acceleration += mu * ((thirdbodystate.position - state.position) / pow((thirdbodystate.position - state.position).norm(),3.0) - thirdbodystate.position / pow(thirdbodystate.position.norm(), 3.0));
    }

    return acceleration;
}

/////////////////////////////// Space Debris Perturbation ///////////////////////////////

DebrisPerturbations::DebrisPerturbations(const StaBody* centralBody,
                                         double mass,
                                         double surfaceArea) :
    m_body(centralBody),
    m_mass(mass),
    m_surface(surfaceArea),
    m_time(0)
{
    m_counterDebris.resize(25);
    m_counterDebris.setZero();
    m_counterMeteoroids.resize(25);
    m_counterMeteoroids.setZero();
}

DebrisPerturbations::~DebrisPerturbations()
{
}

Vector3d
DebrisPerturbations::calculateAcceleration(sta::StateVector state, double time, double dt)
{
    double mu = body()->mu();
    m_time += dt;
    sta::KeplerianElements keplerian = cartesianTOorbital(mu, state);

    //Trajectory analysis:
    double h = keplerian.SemimajorAxis * (1 - pow(keplerian.Eccentricity, 2)) / (1 + keplerian.Eccentricity * cos(keplerian.TrueAnomaly)) - body()->meanRadius();

    int Ndatapoints = 0;
    VectorXd diameter, Ndebris, Nmet, Ntot;

    // TODO: Shouldn't we use the absolute value of inclination here???
    double inclinationDeg = sta::radToDeg(keplerian.Inclination);

    if ( 300 <= h && h <= 500 && 46.6 <= inclinationDeg && inclinationDeg <= 57.6 )
        loadStatistics("debris_impact_low_altitude.stad", diameter, Ndebris, Nmet, Ntot, Ndatapoints);

    else if ( 700 <= h && h <= 900 && 70 <= inclinationDeg && inclinationDeg <= 90 )
        loadStatistics("debris_impact_medium_altitude.stad", diameter, Ndebris, Nmet, Ntot, Ndatapoints);

    else if ( 35600 <= h && h <= 35900 && 0 <= inclinationDeg && inclinationDeg <= 5.0 )
        loadStatistics("debris_impact_geo_altitude.stad", diameter, Ndebris, Nmet, Ntot, Ndatapoints);

    else return Vector3d(0.0, 0.0, 0.0);

    double density_deb = 0.0028; //kg/cm^3
    double density_met = 0.0025; //kg/cm^3
    double deltaT = 0.0001; //10^-4 s
    double accelerationDebris = 0;
    /* double accelerationMeteoroids = 0; */
    VectorXd nDebris(Ndatapoints), diffDebris(Ndatapoints);
    VectorXd nMeteoroids(Ndatapoints), diffMeteoroids(Ndatapoints);
    Vector3d acceleration(0.0, 0.0, 0.0);

    nDebris = Ndebris/365/24/60/60 * m_time * surface();
    nMeteoroids = Nmet/365/24/60/60 * m_time * surface();

    for (int i = 0; i < Ndatapoints; i++)
    {
        if (i != Ndatapoints-1)
        {
            diffDebris(i) = nDebris(i) - nDebris(i + 1);
            diffMeteoroids(i) = nMeteoroids(i) - nMeteoroids(i + 1);
        }
        else
        {
            diffDebris(i) = nDebris(i);
            diffMeteoroids(i) = nMeteoroids(i);
        }

        // Debris collisions calculation
        if (floor(diffDebris(i)) > counterDebris()(i))
        {
            setCounterDebris(i, nDebris(i));

            double accDebrisModule;
            if (i == 0)
                accDebrisModule = 3 * (density_deb * 4/3 * PI * pow(0.5 * randomNumber(0, diameter(i)),3)) / mass() * randomNumber(4.5,5.5) / deltaT;
            else
                accDebrisModule = 3 * (density_deb * 4/3 * PI * pow(0.5 * randomNumber(diameter(i-1),diameter(i)),3)) / mass() * randomNumber(4.5,5.5) / deltaT;

            //generate a random direction
            double alpha = randomNumber(0, 2*PI);
            double beta = randomNumber(-PI, PI);
            acceleration.x() += accDebrisModule * cos(alpha) * cos(beta);
            acceleration.y() += accDebrisModule * sin(alpha) * cos(beta);
            acceleration.z() += accDebrisModule * sin(beta);
        }

        // Meteoroids collisions calculation
        if (floor(diffMeteoroids(i)) > counterMeteoroids()(i))
        {
            setCounterMeteoroids(i, nMeteoroids(i));

            double accMeteoroidsModule;
            if (i == 0)
                accMeteoroidsModule = 3 * (density_met * 4/3 * PI * pow(0.5 * randomNumber(0, diameter(i)),3)) / mass() * randomNumber(4.5,5.5) / deltaT;
            else
                accMeteoroidsModule = 3 * (density_met * 4/3 * PI * pow(0.5 * randomNumber(diameter(i-1),diameter(i)),3)) / mass() * randomNumber(4.5,5.5) / deltaT;

            //generate a random direction
            double alpha = randomNumber(0, 2*PI);
            double beta = randomNumber(-PI, PI);
            acceleration.x() += accMeteoroidsModule * cos(alpha) * cos(beta);
            acceleration.y() += accMeteoroidsModule * sin(alpha) * cos(beta);
            acceleration.z() += accMeteoroidsModule * sin(beta);
        }
    }
    return acceleration;
}

void
DebrisPerturbations::loadStatistics(QString filename, VectorXd& diameter, VectorXd& Ndebris, VectorXd& Nmet, VectorXd& Ntot, int& Ndatapoints)
{
    QString path("data/atmospheres/");
    path.append(filename);
    QFile model("data/atmospheres/debris_impact_low_altitude.stad");

    model.open(QIODevice::ReadOnly);
    QTextStream modelstream(&model);

    while (!modelstream.atEnd())
    {
        modelstream.readLine();
        Ndatapoints ++;
    }
    model.close();
    diameter.resize(Ndatapoints); Ndebris.resize(Ndatapoints); Nmet.resize(Ndatapoints); Ntot.resize(Ndatapoints);

    model.open(QIODevice::ReadOnly);
    for (int i = 0; i < Ndatapoints; i++)
    {
        modelstream >> diameter(i);
        modelstream >> Ndebris(i);
        modelstream >> Nmet(i);
        modelstream >> Ntot(i);
    }
    model.close();
}

double randomNumber(double inf, double sup)
{
    const float scale = rand()/float(RAND_MAX);
    return inf + scale * (sup - inf);
}

/**
 * Function: evaluation of Legendre's associated function using recursive form.
 * Source: Vallado, Fundamentals of Astrodynamics and Applications
 * @param x The argument
 * @param l The degree of polynomial
 * @param m The order of derivative
 * @return The evaluation of Legendre associated function
 * Author: Michele Scotti
 * E-mail: michele.scotti@gmail.com
 */
double legendre (double x, int l, int m)
{
        if (m > l)						// m > l;
                return 0;
        else if ((l == 0) && (m == 0))	// l = 0; m = 0;
                return 1;
        else if ((l == 1) && (m == 0))	// l = 1; m = 0;
                return x;
        else if ((l == 1) && (m == 1))	// l = 1; m = 1;
                return sqrt(1 - x*x);
        else if (l >= 2 && m == 0)
            return ((2*l-1)*x*legendre(x, l-1, 0) - (l-1)*legendre(x, l-2, 0)) / (l);
        else if (m != 0 && m < l)
            return (legendre(x, l-2, m) + (2*l-1)*(sqrt(1- x*x))*legendre(x, l-1, m-1));
        else if(l == m)
            return ((2*l-1)*sqrt(1-x*x)*legendre(x, l-1, l-1));
        else
            return -1;
}


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

#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/erf.hpp>
#include <boost/math/constants/constants.hpp>
#include <map>
#include <iostream>
#include <iomanip>
#include "mp_t.hpp"

using namespace std;
using namespace boost::math;

//
// This program calculates the coefficients of the polynomials
// used for the regularized incomplete gamma functions gamma_p
// and gamma_q when parameter a is large, and sigma is small
// (where sigma = fabs(1 - x/a) ).
//
// See "The Asymptotic Expansion of the Incomplete Gamma Functions"
// N. M. Temme.
// Siam J. Math Anal. Vol 10 No 4, July 1979, p757.
// Coeffient calculation is described from Eq 3.8 (p762) onwards.
//

//
// Alpha:
//
mp_t alpha(unsigned k)
{
   static map<unsigned, mp_t> data;
   if(data.empty())
   {
      data[1] = 1;
   }

   map<unsigned, mp_t>::const_iterator pos = data.find(k);
   if(pos != data.end())
      return (*pos).second;
   //
   // OK try and calculate the value:
   //
   mp_t result = alpha(k-1);
   for(unsigned j = 2; j <= k-1; ++j)
   {
      result -= j * alpha(j) * alpha(k-j+1);
   }
   result /= (k+1);
   data[k] = result;
   return result;
}

mp_t gamma(unsigned k)
{
   static map<unsigned, mp_t> data;

   map<unsigned, mp_t>::const_iterator pos = data.find(k);
   if(pos != data.end())
      return (*pos).second;

   mp_t result = (k&1) ? -1 : 1;

   for(unsigned i = 1; i <= (2 * k + 1); i += 2)
      result *= i;
   result *= alpha(2 * k + 1);
   data[k] = result;
   return result;
}

mp_t Coeff(unsigned n, unsigned k)
{
   map<unsigned, map<unsigned, mp_t> > data;
   if(data.empty())
      data[0][0] = mp_t(-1) / 3;

   map<unsigned, map<unsigned, mp_t> >::const_iterator p1 = data.find(n);
   if(p1 != data.end())
   {
      map<unsigned, mp_t>::const_iterator p2 = p1->second.find(k);
      if(p2 != p1->second.end())
      {
         return p2->second;
      }
   }

   //
   // If we don't have the value, calculate it:
   //
   if(k == 0)
   {
      // special case:
      mp_t result = (n+2) * alpha(n+2);
      data[n][k] = result;
      return result;
   }
   // general case:
   mp_t result = gamma(k) * Coeff(n, 0) + (n+2) * Coeff(n+2, k-1);
   data[n][k] = result;
   return result;
}

void calculate_terms(double sigma, double a, unsigned bits)
{
   cout << endl << endl;
   cout << "Sigma:        " << sigma << endl;
   cout << "A:            " << a << endl;
   double lambda = 1 - sigma;
   cout << "Lambda:       " << lambda << endl;
   double y = a * (-sigma - log1p(-sigma));
   cout << "Y:            " << y << endl;
   double z = -sqrt(2 * (-sigma - log1p(-sigma)));
   cout << "Z:            " << z << endl;
   double dom = erfc(sqrt(y)) / 2;
   cout << "Erfc term:    " << dom << endl;
   double lead = exp(-y) / sqrt(2 * constants::pi<double>() * a);
   cout << "Remainder factor: " << lead << endl;
   double eps = ldexp(1.0, 1 - static_cast<int>(bits));
   double target = dom * eps / lead;
   cout << "Target smallest term: " << target << endl;

   unsigned max_n = 0;

   for(unsigned n = 0; n < 10000; ++n)
   {
      double term = tools::real_cast<double>(Coeff(n, 0) * pow(z, (double)n));
      if(fabs(term) < target)
      {
         max_n = n-1;
         break;
      }
   }
   cout << "Max n required:  " << max_n << endl;

   unsigned max_k;
   for(unsigned k = 1; k < 10000; ++k)
   {
      double term = tools::real_cast<double>(Coeff(0, k) * pow(a, -((double)k)));
      if(fabs(term) < target)
      {
         max_k = k-1;
         break;
      }
   }
   cout << "Max k required:  " << max_k << endl << endl;

   bool code = false;
   cout << "Print code [0|1]? ";
   cin >> code;

   int prec = 2 + (static_cast<double>(bits) * 3010LL)/10000;
   std::cout << std::scientific << std::setprecision(40);

   if(code)
   {
      cout << "   T workspace[" << max_k+1 << "];\n\n";
      for(unsigned k = 0; k <= max_k; ++k)
      {
         cout <<
            "   static const T C" << k << "[] = {\n";
         for(unsigned n = 0; n < 10000; ++n)
         {
            double term = tools::real_cast<double>(Coeff(n, k) * pow(a, -((double)k)) * pow(z, (double)n));
            if(fabs(term) < target)
            {
               break;
            }
            cout << "      " << Coeff(n, k) << "L,\n";
         }
         cout <<
            "   };\n"
            "   workspace[" << k << "] = tools::evaluate_polynomial(C" << k << ", z);\n\n";
      }
      cout << "   T result = tools::evaluate_polynomial(workspace, 1/a);\n\n";
   }
}

int main()
{
   bool cont;
   do{
      cont  = false;
      double sigma;
      cout << "Enter max value for sigma (sigma = |1 - x/a|): ";
      cin >> sigma;
      double a;
      cout << "Enter min value for a: ";
      cin >> a;
      unsigned precision;
      cout << "Enter number of bits precision required: ";
      cin >> precision;

      calculate_terms(sigma, a, precision);

      cout << "Try again[0|1]: ";
      cin >> cont;

   }while(cont);

   return 0;
}


/*******************************************************************************
Copyright (c) 2011, Dr. D. Studios
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or other
materials provided with the distribution.
Neither the name of the Dr. D. Studios nor the names of its contributors may be
used to endorse or promote products derived from this software without specific
prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/

#ifndef _PIMATH_ROOTS__H_
#define _PIMATH_ROOTS__H_

#include <boost/python.hpp>
#include <ImathMath.h>
#include <ImathRoots.h>
#include "util.h"

/**
 * These functions have been changed to return tuples rather than the root count.
 * If there are no roots, an empty tuple will be returned (rather than None).
 * Return arguments have been removed.
 */

namespace pimath
{
	namespace bp = boost::python;

	template<typename T>
	struct RootsBind
	{
		RootsBind()
		{
			bp::def("solveLinear", solveLinear);
			bp::def("solveQuadratic", solveQuadratic);
			bp::def("solveNormalizedCubic", solveNormalizedCubic );
			bp::def("solveCubic", solveCubic );
		}

		static bp::object
		solveLinear( T a, T b )
		{
			T rv;
			return Imath::solveLinear( a, b, rv ) == 1 ?
					bp::make_tuple( rv ) : bp::make_tuple();
		}

		static bp::object
		solveQuadratic( T a, T b, T c )
		{
			T x[2];
			int count = Imath::solveQuadratic( a, b, c, x );
			switch( count )
			{
				case 1:
					return bp::make_tuple(x[0]);
				case 2:
					return bp::make_tuple(x[0], x[1]);
			}
			return bp::make_tuple();
		}

		static bp::object
		solveNormalizedCubic( T r, T s, T t )
		{
			T x[3];
			int count = Imath::solveNormalizedCubic( r, s, t, x );
			switch( count )
			{
				case 1:
					return bp::make_tuple(x[0]);
				case 2:
					return bp::make_tuple(x[0],x[1]);
				case 3:
					return bp::make_tuple(x[0],x[1],x[2]);
			}
			return bp::make_tuple();
		}

		static bp::object
		solveCubic( T a, T b, T c, T d )
		{
			T x[3];
			int count = Imath::solveCubic( a, b, c, d, x );
			switch( count )
			{
				case 1:
					return bp::make_tuple(x[0]);
				case 2:
					return bp::make_tuple(x[0],x[1]);
				case 3:
					return bp::make_tuple(x[0],x[1],x[2]);
			}
			return bp::make_tuple();
		}
	};
}

#endif