#include <stan/math.hpp>
#include <gtest/gtest.h>
#include <Eigen/Core>
#include <vector>

using Eigen::Array;
using Eigen::Dynamic;
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::RowVectorXd;
using Eigen::VectorXd;
using stan::math::ordered_logistic_glm_lpmf;
using stan::math::ordered_logistic_lpmf;
using stan::math::var;
using std::vector;

template <bool propto, typename T_x, typename T_beta, typename T_cuts>
stan::return_type_t<T_x, T_beta, T_cuts> ordered_logistic_glm_simple_lpmf(
    const vector<int>& y, const Matrix<T_x, Dynamic, Dynamic>& x,
    const T_beta& beta, const T_cuts& cuts) {
  using T_x_beta = stan::return_type_t<T_x, T_beta>;
  using stan::math::as_column_vector_or_scalar;

  auto& beta_col = as_column_vector_or_scalar(beta);

  Eigen::Matrix<T_x_beta, Dynamic, 1> location
      = x.template cast<T_x_beta>() * beta_col.template cast<T_x_beta>();

  return ordered_logistic_lpmf<propto>(y, location, cuts);
}

TEST(ProbDistributionsOrderedLogisticGLM,
     glm_matches_ordered_logistic_doubles) {
  double eps = 1e-13;
  int N = 5;
  int M = 2;
  int C = 4;
  vector<int> y{1, 4, 3, 3, 2};
  VectorXd cuts(C - 1);
  cuts << 0.9, 1.1, 7;
  VectorXd beta(M);
  beta << 1.1, 0.4;
  MatrixXd x(N, M);
  x << 1, 2, 3, 4, 5, 6, 7, 8, 9, 0;
  EXPECT_FLOAT_EQ(ordered_logistic_glm_lpmf(y, x, beta, cuts),
                  ordered_logistic_glm_simple_lpmf<false>(y, x, beta, cuts));
}

TEST(ProbDistributionsOrderedLogisticGLM,
     glm_matches_ordered_logistic_doubles_broadcast_y) {
  double eps = 1e-13;
  int N = 5;
  int M = 2;
  int C = 4;
  VectorXd cuts(C - 1);
  cuts << 0.9, 1.1, 7;
  VectorXd beta(M);
  beta << 1.1, 0.4;
  MatrixXd x(N, M);
  x << 1, 2, 3, 4, 5, 6, 7, 8, 9, 0;
  for (int y_scal = 1; y_scal <= C; y_scal++) {
    vector<int> y(N, y_scal);
    EXPECT_FLOAT_EQ(ordered_logistic_glm_lpmf(y_scal, x, beta, cuts),
                    ordered_logistic_glm_lpmf(y, x, beta, cuts));
  }
}

TEST(ProbDistributionsOrderedLogisticGLM, glm_matches_ordered_logistic_vars) {
  double eps = 1e-13;
  int N = 5;
  int M = 2;
  int C = 3;
  vector<int> y{1, 1, 2, 4, 4};
  Matrix<var, Dynamic, 1> cuts1(C), cuts2(C);
  cuts1 << 0.9, 1.1, 7;
  cuts2 << 0.9, 1.1, 7;
  Matrix<var, Dynamic, 1> beta1(M), beta2(M);
  beta1 << 1.1, 0.4;
  beta2 << 1.1, 0.4;
  Matrix<var, Dynamic, Dynamic> x1(N, M), x2(N, M);
  x1 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 0;
  x2 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 0;
  var res1 = ordered_logistic_glm_lpmf(y, x1, beta1, cuts1);
  var res2 = ordered_logistic_glm_simple_lpmf<false>(y, x2, beta2, cuts2);
  (res1 + res2).grad();

  EXPECT_NEAR(res1.val(), res2.val(), eps);
  for (int i = 0; i < M; i++) {
    for (int j = 0; j < N; j++) {
      EXPECT_NEAR(x1(j, i).adj(), x2(j, i).adj(), eps);
    }
  }
  for (int i = 0; i < M; i++) {
    EXPECT_NEAR(beta1[i].adj(), beta2[i].adj(), eps);
  }
  for (int i = 0; i < C; i++) {
    EXPECT_NEAR(cuts1[i].adj(), cuts2[i].adj(), eps);
  }
}

TEST(ProbDistributionsOrderedLogisticGLM,
     glm_matches_ordered_logistic_vars_broadcast_y) {
  double eps = 1e-13;
  int N = 5;
  int M = 2;
  int C = 3;
  Matrix<var, Dynamic, 1> cuts1(C), cuts2(C);
  cuts1 << 0.9, 1.1, 7;
  cuts2 << 0.9, 1.1, 7;
  Matrix<var, Dynamic, 1> beta1(M), beta2(M);
  beta1 << 1.1, 0.4;
  beta2 << 1.1, 0.4;
  Matrix<var, Dynamic, Dynamic> x1(N, M), x2(N, M);
  x1 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 0;
  x2 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 0;

  for (int y_scal = 1; y_scal <= C; y_scal++) {
    vector<int> y(N, y_scal);
    var res1 = ordered_logistic_glm_lpmf(y, x1, beta1, cuts1);
    var res2 = ordered_logistic_glm_lpmf(y_scal, x2, beta2, cuts2);
    (res1 + res2).grad();

    Matrix<double, Dynamic, Dynamic> x_adj(N, M);

    EXPECT_NEAR(res1.val(), res2.val(), eps);
    for (int i = 0; i < M; i++) {
      for (int j = 0; j < N; j++) {
        x_adj(j, i) = x2(j, i).adj();
        EXPECT_NEAR(x1(j, i).adj(), x2(j, i).adj(), eps);
      }
    }
    for (int i = 0; i < M; i++) {
      EXPECT_NEAR(beta1[i].adj(), beta2[i].adj(), eps);
    }
    for (int i = 0; i < C; i++) {
      EXPECT_NEAR(cuts1[i].adj(), cuts2[i].adj(), eps);
    }
  }
}

TEST(ProbDistributionsOrderedLogisticGLM,
     glm_matches_ordered_logistic_vars_broadcast_x) {
  double eps = 1e-13;
  int N = 5;
  int M = 2;
  int C = 3;
  vector<int> y{1, 1, 2, 4, 4};
  Matrix<var, Dynamic, 1> cuts1(C), cuts2(C);
  cuts1 << 0.9, 1.1, 7;
  cuts2 << 0.9, 1.1, 7;
  Matrix<var, Dynamic, 1> beta1(M), beta2(M);
  beta1 << 1.1, 0.4;
  beta2 << 1.1, 0.4;
  RowVectorXd x_double(M);
  x_double << 1, 2;
  Matrix<var, 1, Dynamic> x_row = x_double;
  Matrix<var, Dynamic, Dynamic> x = x_double.replicate(N, 1);

  var res1 = ordered_logistic_glm_lpmf(y, x_row, beta1, cuts1);
  var res2 = ordered_logistic_glm_lpmf(y, x, beta2, cuts2);
  (res1 + res2).grad();

  EXPECT_NEAR(res1.val(), res2.val(), eps);
  for (int i = 0; i < M; i++) {
    double x_sum = 0;
    for (int j = 0; j < N; j++) {
      x_sum += x(j, i).adj();
    }
    EXPECT_NEAR(x_row[i].adj(), x_sum, eps);
  }
  for (int i = 0; i < M; i++) {
    EXPECT_NEAR(beta1[i].adj(), beta2[i].adj(), eps);
  }
  for (int i = 0; i < C; i++) {
    EXPECT_NEAR(cuts1[i].adj(), cuts2[i].adj(), eps);
  }
}

TEST(ProbDistributionsOrderedLogisticGLM,
     glm_matches_ordered_logistic_single_instance) {
  double eps = 1e-13;
  int N = 1;
  int M = 2;
  int C = 3;
  vector<int> y{1};
  Matrix<var, Dynamic, 1> cuts1(C), cuts2(C);
  cuts1 << 0.9, 1.1, 7;
  cuts2 << 0.9, 1.1, 7;
  Matrix<var, Dynamic, 1> beta1(M), beta2(M);
  beta1 << 1.1, 0.4;
  beta2 << 1.1, 0.4;
  Matrix<var, Dynamic, Dynamic> x1(N, M), x2(N, M);
  x1 << 1, 2;
  x2 << 1, 2;
  var res1 = ordered_logistic_glm_lpmf(y, x1, beta1, cuts1);
  var res2 = ordered_logistic_glm_simple_lpmf<false>(y, x2, beta2, cuts2);
  (res1 + res2).grad();

  EXPECT_NEAR(res1.val(), res2.val(), eps);
  for (int i = 0; i < M; i++) {
    EXPECT_NEAR(x1(0, i).adj(), x2(0, i).adj(), eps);
  }
  for (int i = 0; i < M; i++) {
    EXPECT_NEAR(beta1[i].adj(), beta2[i].adj(), eps);
  }
  for (int i = 0; i < C; i++) {
    EXPECT_NEAR(cuts1[i].adj(), cuts2[i].adj(), eps);
  }
}

TEST(ProbDistributionsOrderedLogisticGLM,
     glm_matches_ordered_logistic_zero_instances) {
  double eps = 1e-13;
  int N = 0;
  int M = 2;
  int C = 3;
  vector<int> y{};
  Matrix<var, Dynamic, 1> cuts1(C), cuts2(C);
  cuts1 << 0.9, 1.1, 7;
  cuts2 << 0.9, 1.1, 7;
  Matrix<var, Dynamic, 1> beta1(M), beta2(M);
  beta1 << 1.1, 0.4;
  beta2 << 1.1, 0.4;
  Matrix<var, Dynamic, Dynamic> x1(N, M), x2(N, M);
  var res1 = ordered_logistic_glm_lpmf(y, x1, beta1, cuts1);
  var res2 = ordered_logistic_glm_simple_lpmf<false>(y, x2, beta2, cuts2);
  (res1 + res2).grad();

  EXPECT_NEAR(res1.val(), res2.val(), eps);
  for (int i = 0; i < M; i++) {
    EXPECT_NEAR(beta1[i].adj(), beta2[i].adj(), eps);
  }
  for (int i = 0; i < C; i++) {
    EXPECT_NEAR(cuts1[i].adj(), cuts2[i].adj(), eps);
  }
}

TEST(ProbDistributionsOrderedLogisticGLM,
     glm_matches_ordered_logistic_zero_attributes) {
  double eps = 1e-13;
  int N = 5;
  int M = 0;
  int C = 3;
  vector<int> y{1, 1, 2, 4, 4};
  Matrix<var, Dynamic, 1> cuts1(C), cuts2(C);
  cuts1 << 0.9, 1.1, 7;
  cuts2 << 0.9, 1.1, 7;
  Matrix<var, Dynamic, 1> beta1(M), beta2(M);
  Matrix<var, Dynamic, Dynamic> x1(N, M), x2(N, M);
  var res1 = ordered_logistic_glm_lpmf(y, x1, beta1, cuts1);
  var res2 = ordered_logistic_glm_simple_lpmf<false>(y, x2, beta2, cuts2);
  (res1 + res2).grad();

  EXPECT_NEAR(res1.val(), res2.val(), eps);
  for (int i = 0; i < C; i++) {
    EXPECT_NEAR(cuts1[i].adj(), cuts2[i].adj(), eps);
  }
}

TEST(ProbDistributionsOrderedLogisticGLM,
     glm_matches_ordered_logistic_single_class) {
  double eps = 1e-13;
  int N = 5;
  int M = 2;
  int C = 1;
  vector<int> y{1, 1, 1, 1, 1};
  Matrix<var, Dynamic, 1> cuts1(C), cuts2(C);
  cuts1 << 0.9;
  cuts2 << 0.9;
  Matrix<var, Dynamic, 1> beta1(M), beta2(M);
  beta1 << 1.1, 0.4;
  beta2 << 1.1, 0.4;
  Matrix<var, Dynamic, Dynamic> x1(N, M), x2(N, M);
  x1 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 0;
  x2 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 0;
  var res1 = ordered_logistic_glm_lpmf(y, x1, beta1, cuts1);
  var res2 = ordered_logistic_glm_simple_lpmf<false>(y, x2, beta2, cuts2);
  (res1 + res2).grad();

  EXPECT_NEAR(res1.val(), res2.val(), eps);
  for (int i = 0; i < M; i++) {
    for (int j = 0; j < N; j++) {
      EXPECT_NEAR(x1(j, i).adj(), x2(j, i).adj(), eps);
    }
  }
  for (int i = 0; i < M; i++) {
    EXPECT_NEAR(beta1[i].adj(), beta2[i].adj(), eps);
  }
  EXPECT_NEAR(cuts1[0].adj(), cuts2[0].adj(), eps);
}

TEST(ProbDistributionsOrderedLogisticGLM,
     glm_matches_ordered_logistic_vars_big) {
  double eps = 1e-7;
  int N = 155;
  int M = 15;
  int C = 10;
  vector<int> y(N);
  for (int i = 0; i < N; i++) {
    y[i] = Matrix<unsigned int, Dynamic, 1>::Random(1)[0] % (C + 1) + 1;
  }
  VectorXd cuts_double = (VectorXd::Random(C).array() + 1) / 2 / C;
  for (int i = 1; i < C; i++) {
    cuts_double[i] += cuts_double[i - 1];
  }
  Matrix<var, Dynamic, 1> cuts1 = cuts_double, cuts2 = cuts_double;
  VectorXd beta_double = VectorXd::Random(M);
  Matrix<var, Dynamic, 1> beta1 = beta_double, beta2 = beta_double;
  MatrixXd x_double = MatrixXd::Random(N, M);
  Matrix<var, Dynamic, Dynamic> x1 = x_double, x2 = x_double;
  var res1 = ordered_logistic_glm_lpmf(y, x1, beta1, cuts1);
  var res2 = ordered_logistic_glm_simple_lpmf<false>(y, x2, beta2, cuts2);
  (res1 + res2).grad();

  Matrix<double, Dynamic, Dynamic> x_adj(N, M);

  EXPECT_NEAR(res1.val(), res2.val(), eps);
  for (int i = 0; i < M; i++) {
    for (int j = 0; j < N; j++) {
      x_adj(j, i) = x2(j, i).adj();
      EXPECT_NEAR(x1(j, i).adj(), x2(j, i).adj(), eps);
    }
  }
  for (int i = 0; i < M; i++) {
    EXPECT_NEAR(beta1[i].adj(), beta2[i].adj(), eps);
  }
  for (int i = 0; i < C; i++) {
    EXPECT_NEAR(cuts1[i].adj(), cuts2[i].adj(), eps);
  }
}

TEST(ProbDistributionsOrderedLogisticGLM, glm_interfaces) {
  int N = 5;
  int M = 2;
  int C = 3;
  vector<int> y = {1, 1, 2, 4, 4};
  int y_scal = 1;
  VectorXd cuts_double(C);
  cuts_double << 0.9, 1.1, 7;
  Matrix<var, Dynamic, 1> cuts_var = cuts_double;
  VectorXd beta_double(M);
  beta_double << 1.1, 0.4;
  Matrix<var, Dynamic, 1> beta_var = beta_double;
  Matrix<double, Dynamic, Dynamic> x_double(N, M);
  x_double << 1, 2, 3, 4, 5, 6, 7, 8, 9, 0;
  Matrix<var, Dynamic, Dynamic> x_var = x_double;
  Matrix<double, 1, Dynamic> x_row_double(M);
  x_row_double << 1, 2;
  Matrix<var, 1, Dynamic> x_row_var = x_row_double;

  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y, x_double, beta_double, cuts_double));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y, x_var, beta_double, cuts_double));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y, x_double, beta_var, cuts_double));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y, x_double, beta_double, cuts_var));
  EXPECT_NO_THROW(ordered_logistic_glm_lpmf(y, x_double, beta_var, cuts_var));
  EXPECT_NO_THROW(ordered_logistic_glm_lpmf(y, x_var, beta_double, cuts_var));
  EXPECT_NO_THROW(ordered_logistic_glm_lpmf(y, x_var, beta_var, cuts_double));

  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y_scal, x_double, beta_double, cuts_double));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y_scal, x_var, beta_double, cuts_double));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y_scal, x_double, beta_var, cuts_double));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y_scal, x_double, beta_double, cuts_var));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y_scal, x_double, beta_var, cuts_var));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y_scal, x_var, beta_double, cuts_var));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y_scal, x_var, beta_var, cuts_double));

  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y, x_row_double, beta_double, cuts_double));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y, x_row_var, beta_double, cuts_double));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y, x_row_double, beta_var, cuts_double));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y, x_row_double, beta_double, cuts_var));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y, x_row_double, beta_var, cuts_var));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y, x_row_var, beta_double, cuts_var));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y, x_row_var, beta_var, cuts_double));

  EXPECT_NO_THROW(ordered_logistic_glm_lpmf(y_scal, x_row_double, beta_double,
                                            cuts_double));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y_scal, x_row_var, beta_double, cuts_double));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y_scal, x_row_double, beta_var, cuts_double));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y_scal, x_row_double, beta_double, cuts_var));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y_scal, x_row_double, beta_var, cuts_var));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y_scal, x_row_var, beta_double, cuts_var));
  EXPECT_NO_THROW(
      ordered_logistic_glm_lpmf(y_scal, x_row_var, beta_var, cuts_double));
}

TEST(ProbDistributionsOrderedLogisticGLM, glm_errors) {
  int N = 5;
  int M = 2;
  int C = 3;
  vector<int> y{1, 1, 2, 4, 4};
  vector<int> y_size{1, 1, 2, 4, 4, 2};
  vector<int> y_val1{1, 1, 2, 4, 5};
  vector<int> y_val2{1, 1, 2, 4, 0};
  VectorXd cuts(C);
  cuts << 0.9, 1.1, 7;
  VectorXd cuts_val1(C);
  cuts_val1 << 0.9, 1.1, INFINITY;
  VectorXd cuts_val2(C);
  cuts_val2 << -INFINITY, 1.1, 4;
  VectorXd cuts_val3(C);
  cuts_val3 << 0, 1.1, 0.5;
  VectorXd cuts_val4(C);
  cuts_val4 << 0, NAN, 0.5;
  VectorXd beta(M);
  beta << 1.1, 0.4;
  VectorXd beta_size(M + 1);
  beta_size << 1.1, 0.4, 0;
  VectorXd beta_val(M);
  beta_val << 1.1, INFINITY;
  Matrix<double, Dynamic, Dynamic> x(N, M);
  x << 1, 2, 3, 4, 5, 6, 7, 8, 9, 0;
  Matrix<double, Dynamic, Dynamic> x_size1(N + 1, M);
  x_size1 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 0, 9, 8;
  Matrix<double, Dynamic, Dynamic> x_size2(N, M + 1);
  x_size2 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 0, 4, 5, 6, 7, 9;
  Matrix<double, 1, Dynamic> x_size3(M + 1);
  x_size3 << 1, 2, 3;
  Matrix<double, Dynamic, Dynamic> x_val(N, M);
  x_val << 1, 2, 3, 4, 5, 6, 7, INFINITY, 9, 0;

  EXPECT_NO_THROW(ordered_logistic_glm_lpmf(y, x, beta, cuts));

  EXPECT_THROW(ordered_logistic_glm_lpmf(y_size, x, beta, cuts),
               std::invalid_argument);
  EXPECT_THROW(ordered_logistic_glm_lpmf(y, x_size1, beta, cuts),
               std::invalid_argument);
  EXPECT_THROW(ordered_logistic_glm_lpmf(y, x_size2, beta, cuts),
               std::invalid_argument);
  EXPECT_THROW(ordered_logistic_glm_lpmf(y, x_size3, beta, cuts),
               std::invalid_argument);
  EXPECT_THROW(ordered_logistic_glm_lpmf(y, x, beta_size, cuts),
               std::invalid_argument);

  EXPECT_THROW(ordered_logistic_glm_lpmf(y_val1, x, beta, cuts),
               std::domain_error);
  EXPECT_THROW(ordered_logistic_glm_lpmf(y_val2, x, beta, cuts),
               std::domain_error);
  EXPECT_THROW(ordered_logistic_glm_lpmf(y, x_val, beta, cuts),
               std::domain_error);
  EXPECT_THROW(ordered_logistic_glm_lpmf(y, x, beta_val, cuts),
               std::domain_error);
  EXPECT_THROW(ordered_logistic_glm_lpmf(y, x, beta, cuts_val1),
               std::domain_error);
  EXPECT_THROW(ordered_logistic_glm_lpmf(y, x, beta, cuts_val2),
               std::domain_error);
  EXPECT_THROW(ordered_logistic_glm_lpmf(y, x, beta, cuts_val3),
               std::domain_error);
  EXPECT_THROW(ordered_logistic_glm_lpmf(y, x, beta, cuts_val4),
               std::domain_error);
}


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

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"
#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/oblateSpheroidBodyShapeModel.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/stateRepresentationConversions.h"
#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_position_representation_conversions )

BOOST_AUTO_TEST_CASE( testGeneralCoordinateConversions )
{
    using namespace coordinate_conversions;
    using namespace unit_conversions;

    // Expected Cartesian state, Montenbruck & Gill (2000) Exercise 5.3.
    const Eigen::Vector3d testCartesianPosition( 1917032.190, 6029782.349, -801376.113 );

    // Expected Cartesian state, Montenbruck & Gill (2000) Exercise 5.3.
    const Eigen::Vector3d testGeodeticPosition( -63.667,
                                                convertDegreesToRadians( -7.26654999 ),
                                                convertDegreesToRadians( 72.36312094 ) );

    // Manually compute associated spherical position
    Eigen::Vector3d testSphericalPosition = coordinate_conversions::convertCartesianToSpherical(
                testCartesianPosition );
    testSphericalPosition( 1 ) = mathematical_constants::PI / 2.0 - testSphericalPosition( 1 );

    // Central body characteristics (WGS84 Earth ellipsoid).
    const double flattening = 1.0 / 298.257223563;
    const double equatorialRadius = 6378137.0;

    // Create shape model
    std::shared_ptr< basic_astrodynamics::OblateSpheroidBodyShapeModel > oblateSpheroidModel =
            std::make_shared< basic_astrodynamics::OblateSpheroidBodyShapeModel >(
                equatorialRadius, flattening );

    // Declare variables for computation
    Eigen::Vector3d computedCartesianPosition, computedSphericalPosition, computedGeodeticPosition;

    // Test computation from spherical position
    {
        computedCartesianPosition = coordinate_conversions::convertPositionElements(
                    testGeodeticPosition, coordinate_conversions::geodetic_position, coordinate_conversions::cartesian_position,
                    oblateSpheroidModel );

        computedSphericalPosition = coordinate_conversions::convertPositionElements(
                    testGeodeticPosition, coordinate_conversions::geodetic_position, coordinate_conversions::spherical_position,
                    oblateSpheroidModel );

        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL( testCartesianPosition( i ) - computedCartesianPosition( i ), 1.0E-3 );
        }

        BOOST_CHECK_SMALL( testSphericalPosition( 0 ) - computedSphericalPosition( 0 ), 1.0E-3 );
        BOOST_CHECK_SMALL( testSphericalPosition( 1 ) - computedSphericalPosition( 1 ), 1.0E-3 / equatorialRadius );
        BOOST_CHECK_SMALL( testSphericalPosition( 2 ) - computedSphericalPosition( 2 ), 1.0E-3 / equatorialRadius );
    }

    // Test computation from Cartesian position
    {
        computedSphericalPosition = coordinate_conversions::convertPositionElements(
                    testCartesianPosition, coordinate_conversions::cartesian_position, coordinate_conversions::spherical_position,
                    oblateSpheroidModel );

        computedGeodeticPosition = coordinate_conversions::convertPositionElements(
                    testCartesianPosition, coordinate_conversions::cartesian_position, coordinate_conversions::geodetic_position,
                    oblateSpheroidModel );

        BOOST_CHECK_SMALL( testGeodeticPosition( 0 ) - computedGeodeticPosition( 0 ), 1.0E-3 );
        BOOST_CHECK_SMALL( testGeodeticPosition( 1 ) - computedGeodeticPosition( 1 ), 1.0E-3 / equatorialRadius );
        BOOST_CHECK_SMALL( testGeodeticPosition( 2 ) - computedGeodeticPosition( 2 ), 1.0E-3 / equatorialRadius );

        BOOST_CHECK_SMALL( testSphericalPosition( 0 ) - computedSphericalPosition( 0 ), 1.0E-3 );
        BOOST_CHECK_SMALL( testSphericalPosition( 1 ) - computedSphericalPosition( 1 ), 1.0E-3 / equatorialRadius );
        BOOST_CHECK_SMALL( testSphericalPosition( 2 ) - computedSphericalPosition( 2 ), 1.0E-3 / equatorialRadius );
    }

    // Test computation from geodetic position
    {
        computedCartesianPosition = coordinate_conversions::convertPositionElements(
                    testSphericalPosition, coordinate_conversions::spherical_position, coordinate_conversions::cartesian_position,
                    oblateSpheroidModel );

        computedGeodeticPosition = coordinate_conversions::convertPositionElements(
                    testSphericalPosition, coordinate_conversions::spherical_position, coordinate_conversions::geodetic_position,
                    oblateSpheroidModel );

        for( unsigned int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL( testCartesianPosition( i ) - computedCartesianPosition( i ), 1.0E-3 );
        }

        BOOST_CHECK_SMALL( testGeodeticPosition( 0 ) - computedGeodeticPosition( 0 ), 1.0E-3 );
        BOOST_CHECK_SMALL( testGeodeticPosition( 1 ) - computedGeodeticPosition( 1 ), 1.0E-3 / equatorialRadius );
        BOOST_CHECK_SMALL( testGeodeticPosition( 2 ) - computedGeodeticPosition( 2 ), 1.0E-3 / equatorialRadius );
    }

}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/**
 * A set of wrapper functions for random number generation.
 * These allow us to keep coupling to a minimum. For example
 * we may want to use Matlab generators when compiling mex functions,
 * but boost/random for other purposes.
 */

#include <ctime>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_01.hpp>
#include "dec_brl/random.h"

/**
 * Module namespace defines objects used for random
 * number generation.
 */
namespace
{
   boost::random::mt19937 gen_m;

} // module namespace

/**
 * Initialise random generator using time dependent seed.
 */
void dec_brl::random::initRandomEngineByTime()
{
   gen_m.seed(std::time(0));
}

/**
 * Generate integer from uniform distribution
 * over closed interval [min, max]
 * @param min minimum generated value
 * @param max maximum generated value
 * @return random integer x, s.t. min <= x <= max
 */
int dec_brl::random::unidrnd(int min, int max)
{
   boost::random::uniform_int_distribution<> dist(min, max);
   return dist(gen_m);
}

/**
 * Generate a uniform random number in the range [0,1).
 */
double dec_brl::random::unirnd()
{
   boost::random::uniform_01<> dist;
   return dist(gen_m);
}


#include <armadillo>
#include <carma/carma.h>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>

py::array_t<double> manual_example(py::array_t<double> & arr) {
    // convert to armadillo matrix without copying.
    arma::Mat<double> mat = carma::arr_to_mat<double>(arr);

    // normally you do something useful here ...
    arma::Mat<double> result = arma::Mat<double>(arr.shape(0), arr.shape(1), arma::fill::randu);

    // convert to Numpy array and return
    return carma::mat_to_arr(result);
}

PYBIND11_MODULE(example, m) {
    m.def(
        "manual_example",
        &manual_example,
        R"pbdoc(
            Example function for manual conversion.

            Parameters
            ----------
            mat : np.array
                input array
        )pbdoc",
        py::arg("arr")
    );
}


#include <cmath>
#include <iostream>
#include <iomanip>
#include <string>
#include <vector>
#include <algorithm>
#include <boost/range/algorithm.hpp>
#include <boost/integer/common_factor_rt.hpp>

using namespace std;

int r;
int main() {

    while (cin >> r) {
        cout << 3 * r * r << endl;
    }

    return 0;
}


//////////////////////////////////////////////////////////////////////////////////////
// distribution::survival::models::importance_sampling::log_ratio_pdf_generator.hpp //
//                                                                           		//
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                		//
//  Software License, Version 1.0. (See accompanying file                    		//
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         		//
//////////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_IMPORTANCE_SAMPLING_LOG_RATIO_PDF_GENERATOR_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_IMPORTANCE_SAMPLING_LOG_RATIO_PDF_GENERATOR_HPP_ER_2009
#include <boost/fusion/include/at_key.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace distribution{
namespace survival {

	// Usage:
    // Let [b,e) contain a sequence of maps, each element of which contains
    // a value x and the lpdf
    // generated it
    // std::transform(b,e,out,make_log_ratio_pdf<TagP,TagLPdf>(dist));
    //
    template<typename TagX,typename TagLpdf,typename D>
    class log_ratio_pdf_generator{
    	public:

		typedef typename distribution::meta::value<D>::type result_type;

		log_ratio_pdf_generator(const D& dist):dist_(dist){}
		log_ratio_pdf_generator(const log_ratio_pdf_generator& that)
        :dist_(that.dist_){}

        template<typename M>
        result_type operator()(const M& map)const
        {
            result_type p = boost::fusion::at_key<TagX>(map);
            result_type l = boost::fusion::at_key<TagLpdf>(map);
            result_type log_r = log_unnormalized_pdf(this->dist_,p);
            log_r -= l;
            return log_r;
        }

        private:
		const D& dist_;
    };

	template<typename TagX,typename TagLpdf,typename D>
    log_ratio_pdf_generator<TagX,TagLpdf,D>
	make_log_ratio_pdf_generator(const D& dist)
    {
		typedef log_ratio_pdf_generator<TagX,TagLpdf,D> result_;
        return result_(dist);
	}

}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


// Copyright Stephan T. Lavavej, http://nuwen.net .
// Distributed under the Boost Software License, Version 1.0.
// See accompanying file LICENSE_1_0.txt or copy at
// http://boost.org/LICENSE_1_0.txt .

#ifndef PHAM_ARITH_HH
#define PHAM_ARITH_HH

#include "compiler.hh"

#ifdef NUWEN_PLATFORM_MSVC
    #pragma once
#endif

// Based on the ANSI C Arithmetic Coding Library by Fred Wheeler,
// which was adapted from the program in "Arithmetic Coding For Data
// Compression", by Ian H. Witten, Radford M. Neal, and John G. Cleary,
// Communications Of The ACM, June 1987, Vol. 30, No. 6.

#include "typedef.hh"
#include "vector.hh"

#include "external_begin.hh"
    #include <boost/utility.hpp>
#include "external_end.hh"

namespace nuwen {
    inline vuc_t   arith(const vuc_t& v);
    inline vuc_t unarith(const vuc_t& v);
}

namespace pham {
    namespace arith {

// The triple of (CODE_VALUE_BITS, MAX_FREQUENCY, INCREMENT_VALUE) is deeply magic.
// It controls how much compression is achieved by BWT/MTF-2/ZLE/Arith.
// The CACM paper explains that when frequency counts are represented in F bits,
// code values are represented in C bits, and arithmetic is performed with
// P bits of precision, the following must hold: F <= C - 2 and F + C <= P.
// For sl_t arithmetic, P = 31, so F = 14 and C = 16 are possible.
// For ul_t arithmetic, P = 32, so F = 15 and C = 17 are possible.
// CODE_VALUE_BITS is C, while MAX_FREQUENCY may be as large as 2^F - 1.
// As MAX_FREQUENCY becomes larger, the arithmetic coder gains a more precise
// memory (its model of the frequencies becomes closer to the true, unscaled
// frequencies), but the arithmetic coder also becomes less adaptive, as
// rescalings are forced less frequently. Adaptiveness is good for compressing
// BWT/MTF-2/ZLE data, which consists of lengthy stretches of "boring" low bytes,
// interspersed with bursts of "exciting" high bytes. When the arithmetic coder
// adapts quickly, better compression is achieved. A generalization of the
// CACM model can achieve the best of both worlds: use the largest possible
// MAX_FREQUENCY to give the coder a good memory, but increment the frequency counts
// by more than 1. This forces more frequent rescaling.
// (16, 16383, 1): Used by the CACM code (as well as Fred Wheeler's). As they used
// sl_t arithmetic, this was "maximally" precise but not very adaptive.
// (16, 8192, 16): Suggested by Fenwick, this forces rescaling 32x more often,
// at the cost of precision.
// (16, 16383, 32): Used by the first libnuwen implementation, 1.0.22.0. Combines
// Fenwick's rescaling with CACM's precision.
// (17, 32767, 1): Used by libnuwen 1.0.23.0 through 1.0.28.0. I reread the CACM
// paper and realized that ul_t arithmetic allowed higher precision.
// (17, 32767, 64): Used by libnuwen 2.0.0.0 and beyond. Combines maximum precision
// with Fenwick's frequent rescaling.

// Here is how BWT/MTF-2/ZLE/Arith behaves on suall10.txt (9873495 bytes)
// with the various triples:
//     CACM (16, 16383,  1): 2073812 bytes, 1.680 bits/byte: The untuned baseline.
//  Fenwick (16,  8192, 16): 2050015 bytes, 1.661 bits/byte: A clear improvement.
// 1.0.22.0 (16, 16383, 32): 2033592 bytes, 1.648 bits/byte: Even better.
// 1.0.23.0 (17, 32767,  1): 2078591 bytes, 1.684 bits/byte: Crippling regression.
//  2.0.0.0 (17, 32767, 64): 2026462 bytes, 1.642 bits/byte: Best of all worlds.

        const nuwen::ul_t CODE_VALUE_BITS = 17;
        const nuwen::ul_t MAX_FREQUENCY   = 32767;
        const nuwen::ul_t INCREMENT_VALUE = 64;

        const nuwen::ul_t TOP_VALUE       = (1 << CODE_VALUE_BITS) - 1;
        const nuwen::ul_t FIRST_QTR       = TOP_VALUE / 4 + 1;
        const nuwen::ul_t HALF            = 2 * FIRST_QTR;
        const nuwen::ul_t THIRD_QTR       = 3 * FIRST_QTR;

        typedef nuwen::us_t symbol_t;

        const symbol_t    SENTINEL        = 256;
        const symbol_t    NUM_SYMBOLS     = 257;

        class model : public boost::noncopyable {
        public:
            model() {
                for (nuwen::ul_t i = 0; i < NUM_SYMBOLS; ++i) {
                    m_freq[i] = 1;
                }

                for (nuwen::ul_t i = 0; i < NUM_SYMBOLS + 1; ++i) {
                    m_cfreq[i] = NUM_SYMBOLS - i;
                }
            }

            nuwen::ul_t operator[](const symbol_t i) const {
                return m_cfreq[i];
            }

            void update(const symbol_t sym) {
                // The CACM code used a hardcoded increment value of 1 and wrote this test
                // (using current terminology) as m_cfreq[0] == MAX_FREQUENCY, which was correct.
                // libnuwen 1.0.28.0 and earlier used a generalized INCREMENT_VALUE and wrote this test
                // (using current terminology) as m_cfreq[0] >= MAX_FREQUENCY, which was WRONG.
                // The correct generalization is below; an invariant of the model is that
                // m_cfreq[0] is ALWAYS <= MAX_FREQUENCY. If we detect that adding INCREMENT_VALUE
                // will destroy this invariant, we must rescale so that the invariant will be
                // maintained.

                if (m_cfreq[0] + INCREMENT_VALUE > MAX_FREQUENCY) {
                    nuwen::ul_t cum = 0;
                    m_cfreq[NUM_SYMBOLS] = 0;

                    for (int i = NUM_SYMBOLS - 1; i >= 0; --i) {
                        m_freq[i] = (m_freq[i] + 1) / 2;
                        cum += m_freq[i];
                        m_cfreq[i] = cum;
                    }
                }

                m_freq[sym] += INCREMENT_VALUE;

                for (int i = 0; i < sym + 1; ++i) {
                    m_cfreq[i] += INCREMENT_VALUE;
                }
            }

        private:
            nuwen::ul_t m_freq[NUM_SYMBOLS];
            nuwen::ul_t m_cfreq[NUM_SYMBOLS + 1];
        };

        class encoder : public boost::noncopyable {
        public:
            encoder() : m_low(0), m_high(TOP_VALUE), m_fbits(0), m_out(), m_acm() { }

            void encode(const symbol_t sym) {
                const nuwen::ul_t range = m_high - m_low + 1;
                m_high = m_low + range * m_acm[sym] / m_acm[0] - 1;
                m_low  = m_low + range * m_acm[static_cast<symbol_t>(sym + 1)] / m_acm[0];

                while (true) {
                    if (m_high < HALF) {
                        bit_plus_follow(0);
                    } else if (m_low >= HALF) {
                        bit_plus_follow(1);
                        m_low  -= HALF;
                        m_high -= HALF;
                    } else if (m_low >= FIRST_QTR && m_high < THIRD_QTR) {
                        ++m_fbits;
                        m_low  -= FIRST_QTR;
                        m_high -= FIRST_QTR;
                    } else {
                        break;
                    }

                    m_low  = 2 * m_low;
                    m_high = 2 * m_high + 1;
                }

                m_acm.update(sym);
            }

            nuwen::vuc_t finalize() {
                ++m_fbits;

                bit_plus_follow(m_low >= FIRST_QTR);

                return m_out.vuc();
            }

        private:
            void bit_plus_follow(const bool bit) {
                m_out.push_back(bit);

                while (m_fbits > 0) {
                    m_out.push_back(!bit);
                    --m_fbits;
                }
            }

            nuwen::ul_t              m_low;
            nuwen::ul_t              m_high;
            nuwen::ul_t              m_fbits;
            nuwen::pack::packed_bits m_out;
            model                    m_acm;
        };

        class decoder : public boost::noncopyable {
        public:
            decoder(const nuwen::vuc_ci_t start, const nuwen::vuc_ci_t finish)
                : m_curr(start), m_shift(7), m_end(finish), m_value(0), m_low(0), m_high(TOP_VALUE), m_acm() {

                for (nuwen::ul_t i = 0; i < CODE_VALUE_BITS; ++i) {
                    m_value <<= 1;
                    m_value += input_bit();
                }
            }

            symbol_t decode() {
                const nuwen::ul_t range = m_high - m_low + 1;
                const nuwen::ul_t cum = ((m_value - m_low + 1) * m_acm[0] - 1) / range;

                symbol_t sym;

                for (sym = 0; m_acm[static_cast<symbol_t>(sym + 1)] > cum; ++sym) { }

                m_high = m_low + range * m_acm[sym] / m_acm[0] - 1;
                m_low  = m_low + range * m_acm[static_cast<symbol_t>(sym + 1)] / m_acm[0];

                while (true) {
                    if (m_high < HALF) {
                    } else if (m_low >= HALF) {
                        m_value -= HALF;
                        m_low   -= HALF;
                        m_high  -= HALF;
                    } else if (m_low >= FIRST_QTR && m_high < THIRD_QTR) {
                        m_value -= FIRST_QTR;
                        m_low   -= FIRST_QTR;
                        m_high  -= FIRST_QTR;
                    } else {
                        break;
                    }

                    m_low   = 2 * m_low;
                    m_high  = 2 * m_high + 1;
                    m_value = 2 * m_value + input_bit();
                }

                m_acm.update(sym);

                return sym;
            }

        private:
            bool input_bit() {
                if (m_curr != m_end) {
                    const bool ret = *m_curr >> m_shift & 1;

                    if (m_shift == 0) {
                        m_shift = 7;
                        ++m_curr;
                    } else {
                        --m_shift;
                    }

                    return ret;
                } else {
                    return 0;
                }
            }

            nuwen::vuc_ci_t m_curr;
            nuwen::uc_t     m_shift;
            nuwen::vuc_ci_t m_end;
            nuwen::ul_t     m_value;
            nuwen::ul_t     m_low;
            nuwen::ul_t     m_high;
            model           m_acm;
        };
    }
}

inline nuwen::vuc_t nuwen::arith(const vuc_t& v) {
    pham::arith::encoder ae;

    for (vuc_ci_t i = v.begin(); i != v.end(); ++i) {
        ae.encode(*i);
    }

    ae.encode(pham::arith::SENTINEL);

    return ae.finalize();
}

inline nuwen::vuc_t nuwen::unarith(const vuc_t& v) {
    pham::arith::decoder ad(v.begin(), v.end());

    vuc_t ret;

    while (true) {
        const pham::arith::symbol_t decoded = ad.decode();

        if (decoded != pham::arith::SENTINEL) {
            ret.push_back(static_cast<uc_t>(decoded));
        } else {
            return ret;
        }
    }
}

#endif // Idempotency


#pragma once
//===----------------------------------------------------------------------===//
#include "rank1_logic_surf.hpp" // popcount_linear()

#include <dtl/dtl.hpp>
#include <dtl/math.hpp>

#include <boost/dynamic_bitset.hpp>

#include <string>
#include <type_traits>
//===----------------------------------------------------------------------===//
namespace dtl {
//===----------------------------------------------------------------------===//
/// Rank1 implementation which does not require additional space but has linear
/// time complexity.
template<
    /// The word type used to store bitmaps.
    typename _word_type = $u64,
    /// Inclusive [b,e] vs exclusive [b,e)
    u1 _inclusive = false>
struct rank1_logic_linear {
  using word_type = typename std::remove_cv<_word_type>::type;
  using size_type = $u32;
  static constexpr u64 is_inclusive = _inclusive ? 1 : 0;
  static constexpr u64 block_bitlength = 0;

  // Pure static.
  rank1_logic_linear() = delete;
  ~rank1_logic_linear() = delete;

  /// Does nothing. Exists for compatibility reasons.
  static void
  init_inplace(
      const word_type* const bitmap_begin,
      const word_type* const bitmap_end,
      size_type* lut) noexcept {
  }

  /// Always returns 0. Exists for compatibility reasons.
  static constexpr u64
  lut_entry_cnt(u64 bitmap_size) noexcept {
    return 0;
  }

  /// Always returns 0. Exists for compatibility reasons.
  static constexpr u64
  estimate_size_in_bytes(u64 bitmap_size) noexcept {
    return lut_entry_cnt(bitmap_size) * sizeof(size_type);
  }

  /// Computes the rank1 of the bit at position 'idx'.
  static size_type __forceinline__
  get(const size_type* lut, u64 idx, const word_type* bitmap_ptr) noexcept {
    return rank1_logic_surf<_word_type, _inclusive>
        ::popcount_linear(bitmap_ptr, 0, idx + is_inclusive);
  }

  /// Returns the name of the implementation.
  static std::string
  name() noexcept {
    return std::string("Linear");
  }
};
//===----------------------------------------------------------------------===//
} // namespace dtl