#include "drake/solvers/dreal_solver.h"

#include <vector>

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

#include "drake/solvers/mathematical_program.h"
#include "drake/solvers/test/generic_trivial_constraints.h"
#include "drake/solvers/test/generic_trivial_costs.h"

namespace drake {
namespace solvers {
namespace {

using symbolic::Expression;
using symbolic::Formula;
using symbolic::Variable;
using symbolic::Variables;

using std::logic_error;
using std::make_shared;
using std::shared_ptr;
using std::vector;

class DrealSolverTest : public ::testing::Test {
 protected:
  void SetUp() override { xvec_ = prog_.NewContinuousVariables(4, "x"); }

  // Continuous variables.
  const Variable x_{"x", Variable::Type::CONTINUOUS};
  const Variable y_{"y", Variable::Type::CONTINUOUS};
  const Variable z_{"z", Variable::Type::CONTINUOUS};

  // Integer variables.
  const Variable i_{"i", Variable::Type::INTEGER};
  const Variable j_{"j", Variable::Type::INTEGER};

  // Binary variables.
  const Variable binary1_{"binary1", Variable::Type::BINARY};
  const Variable binary2_{"binary2", Variable::Type::BINARY};

  // Boolean variables.
  const Variable b1_{"b1", Variable::Type::BOOLEAN};
  const Variable b2_{"b2", Variable::Type::BOOLEAN};
  const Variable b3_{"b3", Variable::Type::BOOLEAN};

  const double delta_{0.001};
  MathematicalProgram prog_;
  VectorXDecisionVariable xvec_;
  DrealSolver solver_;
};

TEST_F(DrealSolverTest, Interval) {
  const double low{-10.0};
  const double high{10.0};
  DrealSolver::Interval i{low, high};

  EXPECT_EQ(i.low(), low);
  EXPECT_EQ(i.high(), high);
  EXPECT_EQ(i.mid(), 0.0);
  EXPECT_EQ(i.diam(), 20.0);
}

TEST_F(DrealSolverTest, Available) {
  if (solver_.available()) {
    const auto result = DrealSolver::CheckSatisfiability(
        Expression{0.0} > Expression{1.0}, delta_);
    ASSERT_FALSE(result);
  }
}

// 0.0 > 1.0 is trivially UNSAT.
TEST_F(DrealSolverTest, CheckSatisfiabilityTrivialUnsat) {
  if (solver_.available()) {
    const auto result = DrealSolver::CheckSatisfiability(
        Expression{0.0} > Expression{1.0}, delta_);
    ASSERT_FALSE(result);
  }
}

// 1.0 > 0.0 is trivially SAT.
TEST_F(DrealSolverTest, CheckSatisfiabilityTrivialSat) {
  if (solver_.available()) {
    const auto result = DrealSolver::CheckSatisfiability(
        Expression{1.0} > Expression{0.0}, delta_);
    ASSERT_TRUE(result);
    // The result is an empty box.
    EXPECT_EQ(result->size(), 0.0);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityConjunction) {
  if (solver_.available()) {
    const auto result =
        DrealSolver::CheckSatisfiability(b1_ && !b2_ && !b3_, delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    // We should have a point solution (i.e. lower-bound == upper-bound).
    EXPECT_EQ(solution.at(b1_).diam(), 0);
    EXPECT_EQ(solution.at(b2_).diam(), 0);
    EXPECT_EQ(solution.at(b3_).diam(), 0);
    const double v1{solution.at(b1_).mid()};
    const double v2{solution.at(b2_).mid()};
    const double v3{solution.at(b3_).mid()};
    // Should be either 1.0 (representing True) or 0.0 (False).
    EXPECT_TRUE(v1 == 1.0 || v1 == 0.0);
    EXPECT_TRUE(v2 == 1.0 || v2 == 0.0);
    EXPECT_TRUE(v3 == 1.0 || v3 == 0.0);
    EXPECT_TRUE(v1 && !v2 && !v3);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityDisjunction) {
  if (solver_.available()) {
    const auto result =
        DrealSolver::CheckSatisfiability(b1_ || !b2_ || b3_, delta_);
    const DrealSolver::IntervalBox& solution{*result};
    // We should have a point solution (i.e. lower-bound == upper-bound).
    EXPECT_EQ(solution.at(b1_).diam(), 0);
    EXPECT_EQ(solution.at(b2_).diam(), 0);
    EXPECT_EQ(solution.at(b3_).diam(), 0);
    const double v1{solution.at(b1_).mid()};
    const double v2{solution.at(b2_).mid()};
    const double v3{solution.at(b3_).mid()};
    // Should be either 1.0 (representing True) or 0.0 (False).
    EXPECT_TRUE(v1 == 1.0 || v1 == 0.0);
    EXPECT_TRUE(v2 == 1.0 || v2 == 0.0);
    EXPECT_TRUE(v3 == 1.0 || v3 == 0.0);
    EXPECT_TRUE(v1 || !v2 || v3);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityLinearReal) {
  const Formula f1{0 <= x_ && x_ <= 5};
  const Formula f2{0 <= y_ && y_ <= 5};
  const Formula f3{2 * x_ + 3 * y_ == 5 && -3 * x_ + 4 * y_ == 6};

  if (solver_.available()) {
    const auto result = DrealSolver::CheckSatisfiability(f1 && f2 && f3,
                                                         delta_);
    ASSERT_TRUE(result);

    const DrealSolver::IntervalBox& solution{*result};
    const double expected_x{2.0 / 17.0};
    const double expected_y{27.0 / 17.0};
    EXPECT_NEAR(solution.at(x_).mid(), expected_x, delta_);
    EXPECT_NEAR(solution.at(y_).mid(), expected_y, delta_);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityLinearInteger) {
  const Formula f1{0 <= i_ && i_ <= 5};
  const Formula f2{0 <= j_ && j_ <= 5};
  const Formula f3{2 * i_ + 3 * j_ == 5 && -3 * i_ + 4 * j_ == 6};
  if (solver_.available()) {
    const auto result = DrealSolver::CheckSatisfiability(f1 && f2 && f3,
                                                         delta_);
    // Note that this has the same constraint as the previous,
    // CheckSatisfiabilityLinearReal test. However, the domain constraint, i,j ∈
    // Z, makes the problem unsatisfiable.
    EXPECT_FALSE(result);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityBinaryVariables) {
  const Formula f{2 * binary1_ + 3 * binary2_ == 0};
  if (solver_.available()) {
    const auto result = DrealSolver::CheckSatisfiability(f, delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    // Because of the domain constraints, the only solution is {binary1 ↦ 0,
    // binary2 ↦ 0}.
    EXPECT_EQ(solution.at(binary1_).mid(), 0.0);
    EXPECT_EQ(solution.at(binary2_).mid(), 0.0);
    // They are points.
    EXPECT_EQ(solution.at(binary1_).diam(), 0.0);
    EXPECT_EQ(solution.at(binary2_).diam(), 0.0);
  }
}

// Tests CheckSatisfiability (δ-SAT case).
TEST_F(DrealSolverTest, CheckSatisfiabilityDeltaSat) {
  // Find a model satisfying the following constraints:
  //     0 ≤ x ≤ 5
  //     0 ≤ y ≤ 5
  //     0 ≤ z ≤ 5
  //     2x² + y = z
  const Formula f1{0 <= x_ && x_ <= 5};
  const Formula f2{0 <= y_ && y_ <= 5};
  const Formula f3{0 <= z_ && z_ <= 5};
  const Formula f4{2 * x_ * x_ + y_ == z_};

  if (solver_.available()) {
    const auto result =
        DrealSolver::CheckSatisfiability(f1 && f2 && f3 && f4, delta_);
    ASSERT_TRUE(result);

    const double x{result->at(x_).mid()};
    const double y{result->at(y_).mid()};
    const double z{result->at(z_).mid()};
    EXPECT_TRUE(0 <= x && x <= 5);
    EXPECT_TRUE(0 <= y && y <= 5);
    EXPECT_TRUE(0 <= z && z <= 5);
    EXPECT_NEAR(2 * x * x + y, z, delta_);
  }
}

// Tests CheckSatisfiability (UNSAT case).
TEST_F(DrealSolverTest, CheckSatisfiabilityUnsat) {
  // Find a model satisfying the following constraints:
  //     2x² + 6x + 5 < 0
  //     -10 ≤ x ≤ 10
  const Formula f1{2 * x_ * x_ + 6 * x_ + 5 < 0};
  const Formula f2{-10 <= x_ && x_ <= 10};

  if (solver_.available()) {
    const auto result = DrealSolver::CheckSatisfiability(f1 && f2, delta_);
    EXPECT_FALSE(result);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityNonlinear) {
  // Find a model satisfying the following constraints:
  //     x = 4
  //     y >= abs(-x²)
  //     sqrt(y) / 4 ≠ 1
  //     25 > y
  const Formula f1{x_ == 4};
  const Formula f2{y_ >= abs(-x_ * x_)};
  const Formula f3{sqrt(y_) / 4 != 1};
  const Formula f4{y_ > 25};

  if (solver_.available()) {
    const auto result =
        DrealSolver::CheckSatisfiability(f1 && f2 && f3 && f4, delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    const double y{solution.at(y_).mid()};
    EXPECT_NEAR(x, 4.0, delta_);  // f1
    EXPECT_TRUE(y >= std::abs(-x * x) - delta_);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityLog) {
  if (solver_.available()) {
    const auto result =
        DrealSolver::CheckSatisfiability(x_ == 4 && y_ == log(x_), delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    const double y{solution.at(y_).mid()};
    EXPECT_EQ(x, 4.0);
    EXPECT_NEAR(y, std::log(4.0), delta_);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityExp) {
  if (solver_.available()) {
    const auto result =
        DrealSolver::CheckSatisfiability(x_ == 4 && y_ == exp(x_), delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    const double y{solution.at(y_).mid()};
    EXPECT_EQ(x, 4.0);
    EXPECT_NEAR(y, std::exp(4.0), delta_);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilitySin) {
  if (solver_.available()) {
    const auto result =
        DrealSolver::CheckSatisfiability(x_ == 4 && y_ == sin(x_), delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    const double y{solution.at(y_).mid()};
    EXPECT_EQ(x, 4.0);
    EXPECT_NEAR(y, std::sin(4.0), delta_);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityCos) {
  if (solver_.available()) {
    const auto result =
        DrealSolver::CheckSatisfiability(x_ == 4 && y_ == cos(x_), delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    const double y{solution.at(y_).mid()};
    EXPECT_EQ(x, 4.0);
    EXPECT_NEAR(y, std::cos(4.0), delta_);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityTan) {
  if (solver_.available()) {
    const auto result =
        DrealSolver::CheckSatisfiability(x_ == 4 && y_ == tan(x_), delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    const double y{solution.at(y_).mid()};
    EXPECT_EQ(x, 4.0);
    EXPECT_NEAR(y, std::tan(4.0), delta_);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityAsin) {
  if (solver_.available()) {
    const auto result =
        DrealSolver::CheckSatisfiability(x_ == 0.5 && y_ == asin(x_), delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    const double y{solution.at(y_).mid()};
    EXPECT_EQ(x, 0.5);
    EXPECT_NEAR(y, std::asin(0.5), delta_);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityAcos) {
  if (solver_.available()) {
    const auto result =
        DrealSolver::CheckSatisfiability(x_ == 0.5 && y_ == acos(x_), delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    const double y{solution.at(y_).mid()};
    EXPECT_EQ(x, 0.5);
    EXPECT_NEAR(y, std::acos(0.5), delta_);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityAtan) {
  if (solver_.available()) {
    const auto result =
        DrealSolver::CheckSatisfiability(x_ == 4 && y_ == atan(x_), delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    const double y{solution.at(y_).mid()};
    EXPECT_EQ(x, 4.0);
    EXPECT_NEAR(y, std::atan(4.0), delta_);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityAtan2) {
  if (solver_.available()) {
    const auto result = DrealSolver::CheckSatisfiability(
        x_ == 4 && y_ == 3 && z_ == atan2(x_, y_), delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    const double y{solution.at(y_).mid()};
    const double z{solution.at(z_).mid()};
    EXPECT_EQ(x, 4.0);
    EXPECT_EQ(y, 3.0);
    EXPECT_NEAR(z, std::atan2(4.0, 3.0), delta_);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilitySinh) {
  if (solver_.available()) {
    const auto result =
        DrealSolver::CheckSatisfiability(x_ == 0.5 && y_ == sinh(x_), delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    const double y{solution.at(y_).mid()};
    EXPECT_EQ(x, 0.5);
    EXPECT_NEAR(y, std::sinh(0.5), delta_);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityCosh) {
  if (solver_.available()) {
    const auto result =
        DrealSolver::CheckSatisfiability(x_ == 0.5 && y_ == cosh(x_), delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    const double y{solution.at(y_).mid()};
    EXPECT_EQ(x, 0.5);
    EXPECT_NEAR(y, std::cosh(0.5), delta_);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityTanh) {
  if (solver_.available()) {
    const auto result =
        DrealSolver::CheckSatisfiability(x_ == 4 && y_ == tanh(x_), delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    const double y{solution.at(y_).mid()};
    EXPECT_EQ(x, 4.0);
    EXPECT_NEAR(y, std::tanh(4.0), delta_);
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityMin) {
  if (solver_.available()) {
    const auto result = DrealSolver::CheckSatisfiability(
        x_ == 4 && y_ == 3 && z_ == min(x_, y_), delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    const double y{solution.at(y_).mid()};
    const double z{solution.at(z_).mid()};
    EXPECT_EQ(x, 4.0);
    EXPECT_EQ(y, 3.0);
    EXPECT_EQ(z, std::min(4.0, 3.0));
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityMax) {
  if (solver_.available()) {
    const auto result = DrealSolver::CheckSatisfiability(
        x_ == 4 && y_ == 3 && z_ == max(x_, y_), delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    const double y{solution.at(y_).mid()};
    const double z{solution.at(z_).mid()};
    EXPECT_EQ(x, 4.0);
    EXPECT_EQ(y, 3.0);
    EXPECT_EQ(z, std::max(4.0, 3.0));
  }
}

TEST_F(DrealSolverTest, CheckSatisfiabilityForall) {
  // To test `forall` formulas, encode the problem of minimizing x² in
  // exist-forall formula.
  //
  //     min x² s.t. x ∈ [-3, 3].
  // ->  ∃x. (-3 ≤ x) ∧ (x ≤ 3) ∧ [∀y. ((-3 ≤ y) ∧ (y ≤ 3)) → (x² ≤ y²)]
  // ->  ∃x. (-3 ≤ x) ∧ (x ≤ 3) ∧ [∀y. ¬((-3 ≤ y) ∧ (y ≤ 3)) ∨ (x² ≤ y²)]
  if (solver_.available()) {
    const auto result = DrealSolver::CheckSatisfiability(
        (-3 <= x_) && (x_ <= 3) &&
            forall({y_}, !((-3 <= y_) && (y_ <= 3)) || (x_ * x_ <= y_ * y_)),
        delta_);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    EXPECT_NEAR(x, 0.0, delta_);
  }
}

TEST_F(DrealSolverTest, Minimize1) {
  // Minimize 2x² + 6x + 5 s.t. -4 ≤ x ≤ 0
  // The known minimum cost is -0.5.
  const Expression objective{2 * x_ * x_ + 6 * x_ + 5};
  const Formula constraint{-10 <= x_ && x_ <= 10};
  const double delta{0.01};
  const double known_minimum{0.5};

  if (solver_.available()) {
    const auto result = DrealSolver::Minimize(
        objective, constraint, delta, DrealSolver::LocalOptimization::kUse);
    ASSERT_TRUE(result);
    const DrealSolver::IntervalBox& solution{*result};
    const double x{solution.at(x_).mid()};
    EXPECT_TRUE(-10 <= x && x <= 10);
    EXPECT_LT(2 * x * x + 6 * x + 5, known_minimum + delta);
  }
}

TEST_F(DrealSolverTest, Minimize2) {
  // Minimize sin(3x) - 2cos(x) s.t. -3 ≤ x ≤ 3
  // The known minimum cost is -2.77877.
  const Expression objective{sin(3 * x_) - 2 * cos(x_)};
  const Formula constraint{-3 <= x_ && x_ <= 3};
  const double delta{0.001};
  const double known_minimum{-2.77877};
  if (solver_.available()) {
    const auto result = DrealSolver::Minimize(
        objective, constraint, delta, DrealSolver::LocalOptimization::kUse);
    ASSERT_TRUE(result);
    const double x{result->at(x_).mid()};
    EXPECT_TRUE(-3 <= x && x <= 3);
    EXPECT_LT(sin(3 * x) - 2 * cos(x), known_minimum + delta);
  }
}

TEST_F(DrealSolverTest, Minimize3) {
  // Minimize sin(3x) s.t. (-3 ≤ x ≤ 3) ∧ (x² - 16 = 0)
  // Note that the side constraints have no model.
  const Expression objective{sin(3 * x_)};
  const Formula constraint{-3 <= x_ && x_ <= 3 && (x_ * x_ - 16 == 0)};
  if (solver_.available()) {
    const auto result = DrealSolver::Minimize(
        objective, constraint, delta_, DrealSolver::LocalOptimization::kUse);
    EXPECT_FALSE(result);
  }
}

TEST_F(DrealSolverTest, UnsupportedFormulaIsnan) {
  if (solver_.available()) {
    EXPECT_THROW(DrealSolver::CheckSatisfiability(isnan(x_), delta_),
                 std::runtime_error);
  }
}

TEST_F(DrealSolverTest, UnsupportedFormulaCeil) {
  if (solver_.available()) {
    EXPECT_THROW(DrealSolver::CheckSatisfiability(ceil(x_) == 0, delta_),
                 std::runtime_error);
  }
}

TEST_F(DrealSolverTest, UnsupportedFormulaFloor) {
  if (solver_.available()) {
    EXPECT_THROW(DrealSolver::CheckSatisfiability(floor(x_) == 0, delta_),
                 std::runtime_error);
  }
}

TEST_F(DrealSolverTest, IfThenElse1) {
  const Formula f{x_ == 3 && y_ == 2 && z_ == if_then_else(x_ > y_, x_, y_)};
  if (solver_.available()) {
    const auto result = DrealSolver::CheckSatisfiability(f, delta_);
    ASSERT_TRUE(result);
    const double z{result->at(z_).mid()};
    EXPECT_EQ(z, 3);
  }
}

TEST_F(DrealSolverTest, IfThenElse2) {
  const Formula f{x_ == 2 && y_ == 3 && z_ == if_then_else(x_ > y_, x_, y_)};
  if (solver_.available()) {
    const auto result = DrealSolver::CheckSatisfiability(f, delta_);
    ASSERT_TRUE(result);
    const double z{result->at(z_).mid()};
    EXPECT_EQ(z, 3);
  }
}

TEST_F(DrealSolverTest, UnsupportedFormulaUninterpretedFunction) {
  if (solver_.available()) {
    EXPECT_THROW(
        DrealSolver::CheckSatisfiability(
            symbolic::uninterpreted_function("uf", {x_, y_}) == 0, delta_),
        std::runtime_error);
  }
}

TEST_F(DrealSolverTest, UnsupportedFormulaPositiveSemidefinite) {
  Eigen::Matrix<Expression, 2, 2> m;
  m << (x_ + y_), -1.0, -1.0, y_;
  if (solver_.available()) {
    EXPECT_THROW(
        DrealSolver::CheckSatisfiability(positive_semidefinite(m), delta_),
        std::runtime_error);
  }
}

TEST_F(DrealSolverTest, SolveLinearProgramming) {
  // Linear Cost + BoundingBox constraints + Linear constraint
  const Variable& x0{xvec_(0)};
  const Variable& x1{xvec_(1)};
  prog_.AddConstraint(x0, 100, 200);
  prog_.AddConstraint(x1, 80, 170);
  prog_.AddConstraint(x1 >= -x0 + 200);
  prog_.AddCost(2 * x0 - 5 * x1);
  if (solver_.available()) {
    auto result = solver_.Solve(prog_, {}, {});
    ASSERT_TRUE(result.is_success());
    const double delta{0.001};
    const double v0{result.GetSolution(x0)};
    const double v1{result.GetSolution(x1)};
    EXPECT_TRUE(100 - delta <= v0 && v0 <= 200 + delta);
    EXPECT_TRUE(80 - delta <= v1 && v1 <= 170 + delta);
    EXPECT_TRUE(v1 >= -v0 + 200 - delta);
    EXPECT_NEAR(2 * v0 - 5 * v1, 2 * 100 - 5 * 170 /* known minimum */,
                delta * 5.0);
  }
}

TEST_F(DrealSolverTest, SolveQuadraticProgramming) {
  // Linear Cost + BoundingBox constraints + Linear constraint
  const Variable& x0{xvec_(0)};
  const Variable& x1{xvec_(1)};
  prog_.AddConstraint(x0, 0, 20);
  prog_.AddConstraint(x1 >= 0);
  prog_.AddConstraint(2 * x0 + x1 >= 2);
  prog_.AddConstraint(-x0 + 2 * x1 <= 6);
  prog_.AddCost(4 + 1.5 * x0 - 2 * x1 + 4 * x0 * x0 + 2 * x0 + x1 +
                5 * x1 * x1);
  const double delta{1e-5};
  prog_.SetSolverOption(DrealSolver::id(), "precision", delta);
  prog_.SetSolverOption(DrealSolver::id(), "use_local_optimization", 0);
  if (solver_.available()) {
    auto result = solver_.Solve(prog_, {}, {});
    ASSERT_TRUE(result.is_success());
    const double v0{result.GetSolution(x0)};
    const double v1{result.GetSolution(x1)};
    EXPECT_TRUE(0 - delta <= v0 && v0 <= 20 + delta);
    EXPECT_TRUE(0 - delta <= v1);
    EXPECT_TRUE(2 * v0 + v1 >= 2 - delta);
    EXPECT_NEAR(4 + 1.5 * v0 - 2 * v1 + 4 * v0 * v0 + 2 * v0 + v1 + 5 * v1 * v1,
                4 + 1.5 * 0.71875 - 2 * 0.5625 + 4 * 0.71875 * 0.71875 +
                    2 * 0.71875 + 0.5625 + 5 * 0.5625 * 0.5625,
                delta * 5.0);
  }
}

TEST_F(DrealSolverTest, SolveLinearEqualityConstraint) {
  const Variable& x0{xvec_(0)};
  const Variable& x1{xvec_(1)};
  prog_.AddConstraint(x0, -5, 5);
  prog_.AddConstraint(x1, -5, 5);
  prog_.AddConstraint(2 * x0 + 3 * x1 == 2);
  prog_.AddConstraint(-3 * x0 + 4 * x1 <= 0);
  const double delta{1e-3};
  if (solver_.available()) {
    auto result = solver_.Solve(prog_, {}, {});
    ASSERT_TRUE(result.is_success());
    const double v0{result.GetSolution(x0)};
    const double v1{result.GetSolution(x1)};
    EXPECT_NEAR(2 * v0 + 3 * v1, 2.0, delta);
    EXPECT_TRUE(-3 * v0 + 4 * v1 <= delta);
  }
}

TEST_F(DrealSolverTest, SolveQuadraticConstraint) {
  const Variable& x0{xvec_(0)};
  const Variable& x1{xvec_(1)};
  prog_.AddConstraint(x0, 0, 5);
  prog_.AddConstraint(x1, 0, 5);
  prog_.AddConstraint(x0 <= x1 * x1);
  prog_.AddConstraint(x1 * x1 - 0.0001 <= x0);
  prog_.AddConstraint(x0 == 3.0);
  const double delta{1e-3};
  if (solver_.available()) {
    auto result = solver_.Solve(prog_, {}, {});
    ASSERT_TRUE(result.is_success());
    const double v1{result.GetSolution(x1)};
    EXPECT_NEAR(v1, 1.7320 /* sqrt(3.0) */, delta);
  }
}

TEST_F(DrealSolverTest, SolveLorentzConeConstraint) {
  const Variable& x0{xvec_(0)};
  const Variable& x1{xvec_(1)};
  const Variable& x2{xvec_(2)};
  prog_.AddConstraint(x0, -5, 5);
  prog_.AddConstraint(x1, -5, 5);
  prog_.AddConstraint(x2, 0, 5);
  prog_.AddLorentzConeConstraint(
      Vector3<symbolic::Expression>(0 * x0 + 1, x0 - 1, x1 - 1));
  prog_.AddLorentzConeConstraint(Vector3<symbolic::Expression>(x2, x0, x1));
  prog_.AddCost(x2);
  const double delta{1e-5};
  prog_.SetSolverOption(DrealSolver::id(), "precision", delta);
  if (solver_.available()) {
    auto result = solver_.Solve(prog_, {}, {});
    ASSERT_TRUE(result.is_success());
    const double v2{result.GetSolution(x2)};
    // We check if the found minimum (solution for x2) is close to the one from
    // SCS solver.
    EXPECT_NEAR(v2, /* Solution from SCS Solver */ 0.414212, delta * 5);
  }
}

TEST_F(DrealSolverTest, SolveRotatedLorentzConeConstraint) {
  const Variable& x0{xvec_(0)};
  const Variable& x1{xvec_(1)};
  const Variable& x2{xvec_(2)};
  prog_.AddLinearCost(2 * x0 + 3 * x1 - 2 * x2);
  prog_.AddConstraint(x0, -1, 1);
  prog_.AddConstraint(x1, -1, 1);
  prog_.AddConstraint(x2, -1, 1);
  prog_.AddRotatedLorentzConeConstraint(
      Vector4<symbolic::Expression>(x0 + x1, x1 + x2, +x0, +x1));
  const double delta{1e-10};
  prog_.SetSolverOption(DrealSolver::id(), "precision", delta);
  if (solver_.available()) {
    auto result = solver_.Solve(prog_, {}, {});
    ASSERT_TRUE(result.is_success());
    const double v0{result.GetSolution(x0)};
    const double v1{result.GetSolution(x1)};
    const double v2{result.GetSolution(x2)};
    // We check if the found minimum (solution for 2x0 + 3x1 - 2x2) is close to
    // the one from Gurobi solver.
    EXPECT_NEAR(2 * v0 + 3 * v1 - 2 * v2,
                /* Solution from Gurobi */
                2 * 0.0953487 + 3 * -0.0787482 - 2 * 1,
                1e-5);
  }
}

TEST_F(DrealSolverTest, SolveLinearComplementarityConstraint) {
  // The problem and the expected solution are copied from "bard1" test in
  // solvers/test/complementary_problem_test.cc.
  //
  // A problem from J.F. Bard, Convex two-level optimization,
  // Mathematical Programming 40(1), 15-27, 1988.
  // min (x-5)² + (2*y + 1)²
  // s.t 2*(y-1) - 1.5*x + l(0) - 0.5*l(1) + l(2) = 0
  //     0 <= l(0) ⊥ 3 * x - y - 3 >= 0
  //     0 <= l(1) ⊥ -x + 0.5*y + 4 >= 0
  //     0 <= l(2) ⊥ -x - y + 7 >= 0
  //     x >= 0, y >= 0
  const auto x = prog_.NewContinuousVariables<1>();
  const auto y = prog_.NewContinuousVariables<1>();
  const auto l = prog_.NewContinuousVariables<3>();
  prog_.AddCost(pow(x(0) - 5, 2) + pow(2 * y(0) + 1, 2));
  prog_.AddConstraint(x(0), -10, 10);
  prog_.AddConstraint(y(0), -10, 10);
  prog_.AddConstraint(l(0), -10, 10);
  prog_.AddConstraint(l(1), -10, 10);
  prog_.AddConstraint(l(2), -10, 10);
  prog_.AddLinearConstraint(
      2 * (y(0) - 1) - 1.5 * x(0) + l(0) - 0.5 * l(1) + l(2) == 0);
  Eigen::Matrix<double, 5, 5> M;
  // clang-format off
  M <<  3,  -1, 0, 0, 0,
       -1, 0.5, 0, 0, 0,
       -1,  -1, 0, 0, 0,
        0,   0, 0, 0, 0,
        0,   0, 0, 0, 0;
  // clang-format on
  Eigen::Matrix<double, 5, 1> q;
  q << -3, 4, 7, 0, 0;
  prog_.AddLinearComplementarityConstraint(M, q, {x, y, l});
  const double delta{1e-5};
  prog_.SetSolverOption(DrealSolver::id(), "precision", delta);
  if (solver_.available()) {
    auto result = solver_.Solve(prog_, {}, {});
    ASSERT_TRUE(result.is_success());
    const auto x_val = result.GetSolution(x);
    const auto y_val = result.GetSolution(y);
    EXPECT_NEAR(x_val(0), 1, 1E-6);
    EXPECT_NEAR(y_val(0), 0, 1E-6);
  }
}

TEST_F(DrealSolverTest, SolveNonLinearConstraint) {
  const Variable& x0{xvec_(0)};
  prog_.AddConstraint(x0, -3.141592, 3.141592);
  prog_.AddConstraint(sin(x0) + cos(x0), 0.4, 0.41);
  const double delta{1e-5};
  prog_.SetSolverOption(DrealSolver::id(), "precision", delta);
  if (solver_.available()) {
    auto result = solver_.Solve(prog_, {}, {});
    ASSERT_TRUE(result.is_success());
    const double v0{result.GetSolution(x0)};
    EXPECT_TRUE(-3.141592 - delta <= v0 && v0 <= 3.141592 + delta);
    EXPECT_TRUE(0.4 - delta <= sin(v0) + cos(v0));
    EXPECT_TRUE(sin(v0) + cos(v0) <= 0.41 + delta);
    // Add more constraints to make the problem infeasible.
    prog_.AddConstraint(cos(x0) * sin(x0), 0.9, 0.91);
    solver_.Solve(prog_, {}, {}, &result);
    ASSERT_FALSE(result.is_success());
    EXPECT_EQ(result.get_solution_result(),
              SolutionResult::kInfeasibleConstraints);
  }
}

TEST_F(DrealSolverTest, SolvePositiveSemidefiniteConstraint) {
  // No support yet. Checks DrealSolver throws std::logic_error.
  const auto X = prog_.NewSymmetricContinuousVariables<4>("X");
  prog_.AddPositiveSemidefiniteConstraint(X);
  EXPECT_THROW(solver_.Solve(prog_, {}, {}), logic_error);
}

TEST_F(DrealSolverTest, SolveLinearMatrixInequalityConstraint) {
  // No support yet. Checks DrealSolver throws std::logic_error.
  prog_.AddLinearMatrixInequalityConstraint(
      {Eigen::Matrix2d::Identity(), Eigen::Matrix2d::Ones(),
       2 * Eigen::Matrix2d::Ones()},
      xvec_.head<2>());
  if (solver_.available()) {
    EXPECT_THROW(solver_.Solve(prog_, {}, {}), logic_error);
  }
}

TEST_F(DrealSolverTest, SolveMultipleCostFunctions) {
  const Variable& x{xvec_(0)};
  prog_.AddConstraint(x, -10, 10);
  prog_.AddCost(-2 * x + 1);  // -2x + 1
  prog_.AddCost(x * x);       // x²
  if (solver_.available()) {
    // Cost function = x² - 2x + 1 = (x-1)²
    auto result = solver_.Solve(prog_, {}, {});
    ASSERT_TRUE(result.is_success());
    const double v{result.GetSolution(x)};
    EXPECT_NEAR(v, 1, 0.005);
  }
}

TEST_F(DrealSolverTest, SolveGenericConstraint) {
  const Variable& x0{xvec_(0)};
  const Variable& x1{xvec_(1)};
  const Variable& x2{xvec_(2)};
  prog_.AddConstraint(x0, -10, 10);
  prog_.AddConstraint(x1, -10, 10);
  prog_.AddConstraint(x2, -10, 10);
  // -1 <= x0 * x1 + x2 / x0 * 2 <= 2
  // -2 <= x1 * x2 - x0 <= 1
  const shared_ptr<Constraint> generic_trivial_constraint1 =
      make_shared<test::GenericTrivialConstraint1>();
  prog_.AddConstraint(Binding<Constraint>(
      generic_trivial_constraint1, VectorDecisionVariable<3>(x0, x1, x2)));
  if (solver_.available()) {
    auto result = solver_.Solve(prog_, {}, {});
    ASSERT_TRUE(result.is_success());
    const double v0{result.GetSolution(x0)};
    const double v1{result.GetSolution(x1)};
    const double v2{result.GetSolution(x2)};
    EXPECT_LE(-10, v0);
    EXPECT_LE(v0, 10);
    EXPECT_LE(-10, v1);
    EXPECT_LE(v1, 10);
    EXPECT_LE(-10, v2);
    EXPECT_LE(v2, 10);
    EXPECT_LE(-1, v0 * v1 + v2 / v0 * 2);
    EXPECT_LE(v0 * v1 + v2 / v0 * 2, 2);
    EXPECT_LE(-2, v1 * v2 - v0);
    EXPECT_LE(v1 * v2 - v0, 1);
  }
}

TEST_F(DrealSolverTest, SolveGenericCost) {
  // No support yet. Checks DrealSolver throws std::logic_error.
  const shared_ptr<Cost> generic_trivial_cost1 =
      make_shared<test::GenericTrivialCost1>();
  prog_.AddCost(
      Binding<Cost>(generic_trivial_cost1,
                    VectorDecisionVariable<3>(xvec_(0), xvec_(1), xvec_(2))));
  EXPECT_THROW(solver_.Solve(prog_, {}, {}), logic_error);
}

}  // namespace

}  // namespace solvers
}  // namespace drake


//
// Copyright (c) 2015-2018 CNRS
//

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/utils/timer.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

template<typename Derived>
inline bool isFinite(const Eigen::MatrixBase<Derived> & x)
{
  return ((x - x).array() == (x - x).array()).all();
}

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_jacobian )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model);

  VectorXd q = VectorXd::Zero(model.nq);
  computeJointJacobians(model,data,q);

  Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
  Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
  getJointJacobian(model,data,idx,WORLD,Jrh);

   /* Test J*q == v */
  VectorXd qdot = VectorXd::Random(model.nv);
  VectorXd qddot = VectorXd::Zero(model.nv);
  rnea( model,data,q,qdot,qddot );
  Motion v = data.oMi[idx].act( data.v[idx] );
  BOOST_CHECK(v.toVector().isApprox(Jrh*qdot,1e-12));

  /* Test local jacobian: rhJrh == rhXo oJrh */
  Data::Matrix6x rhJrh(6,model.nv); rhJrh.fill(0);
  getJointJacobian(model,data,idx,LOCAL,rhJrh);
  Data::Matrix6x XJrh(6,model.nv);
  motionSet::se3Action( data.oMi[idx].inverse(), Jrh,XJrh );
  BOOST_CHECK(XJrh.isApprox(rhJrh,1e-12));

  XJrh.setZero();
  Data data_jointJacobian(model);
  jointJacobian(model,data_jointJacobian,q,idx,XJrh);
  BOOST_CHECK(XJrh.isApprox(rhJrh,1e-12));

  /* Test computeJointJacobians with pre-computation of the forward kinematics */
  Data data_fk(model);
  forwardKinematics(model, data_fk, q);
  computeJointJacobians(model, data_fk);

  BOOST_CHECK(data_fk.J.isApprox(data.J));

}

BOOST_AUTO_TEST_CASE ( test_jacobian_time_variation )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  VectorXd q = randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) );
  VectorXd v = VectorXd::Random(model.nv);
  VectorXd a = VectorXd::Random(model.nv);

  computeJointJacobiansTimeVariation(model,data,q,v);

  BOOST_CHECK(isFinite(data.dJ));

  forwardKinematics(model,data_ref,q,v,a);
  Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);

  Data::Matrix6x J(6,model.nv); J.fill(0.);
  Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);

  // Regarding to the world origin
  getJointJacobian(model,data,idx,WORLD,J);
  getJointJacobianTimeVariation(model,data,idx,WORLD,dJ);

  Motion v_idx(J*v);
  BOOST_CHECK(v_idx.isApprox(data_ref.oMi[idx].act(data_ref.v[idx])));

  Motion a_idx(J*a + dJ*v);
  const Motion & a_ref = data_ref.oMi[idx].act(data_ref.a[idx]);
  BOOST_CHECK(a_idx.isApprox(a_ref));

  // Regarding to the local frame
  getJointJacobian(model,data,idx,LOCAL,J);
  getJointJacobianTimeVariation(model,data,idx,LOCAL,dJ);

  v_idx = (Motion::Vector6)(J*v);
  BOOST_CHECK(v_idx.isApprox(data_ref.v[idx]));

  a_idx = (Motion::Vector6)(J*a + dJ*v);
  BOOST_CHECK(a_idx.isApprox(data_ref.a[idx]));

  // compare to finite differencies
  {
    Data data_ref(model), data_ref_plus(model);

    const double alpha = 1e-8;
    Eigen::VectorXd q_plus(model.nq);
    q_plus = integrate(model,q,alpha*v);

    Data::Matrix6x J_ref(6,model.nv); J_ref.fill(0.);
    computeJointJacobians(model,data_ref,q);
    getJointJacobian(model,data_ref,idx,WORLD,J_ref);

    Data::Matrix6x J_ref_plus(6,model.nv); J_ref_plus.fill(0.);
    computeJointJacobians(model,data_ref_plus,q_plus);
    getJointJacobian(model,data_ref_plus,idx,WORLD,J_ref_plus);

    Data::Matrix6x dJ_ref(6,model.nv); dJ_ref.fill(0.);
    dJ_ref = (J_ref_plus - J_ref)/alpha;

    computeJointJacobiansTimeVariation(model,data,q,v);
    Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
    getJointJacobianTimeVariation(model,data,idx,WORLD,dJ);

    BOOST_CHECK(dJ.isApprox(dJ_ref,sqrt(alpha)));
  }
}

BOOST_AUTO_TEST_CASE ( test_timings )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model);

  long flag = BOOST_BINARY(1111);
  PinocchioTicToc timer(PinocchioTicToc::US);
  #ifdef NDEBUG
    #ifdef _INTENSE_TESTING_
      const size_t NBT = 1000*1000;
    #else
      const size_t NBT = 10;
    #endif
  #else
    const size_t NBT = 1;
    std::cout << "(the time score in debug mode is not relevant)  " ;
  #endif

  bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
  if(verbose) std::cout <<"--" << std::endl;
  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);

  if( flag >> 0 & 1 )
  {
    timer.tic();
    SMOOTH(NBT)
    {
      computeJointJacobians(model,data,q);
    }
    if(verbose) std::cout << "Compute =\t";
    timer.toc(std::cout,NBT);
  }

  if( flag >> 1 & 1 )
  {
    computeJointJacobians(model,data,q);
    Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoints-1);
    Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);

    timer.tic();
    SMOOTH(NBT)
    {
      getJointJacobian(model,data,idx,WORLD,Jrh);
    }
    if(verbose) std::cout << "Copy =\t";
    timer.toc(std::cout,NBT);
  }

  if( flag >> 2 & 1 )
  {
    computeJointJacobians(model,data,q);
    Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoints-1);
    Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);

    timer.tic();
    SMOOTH(NBT)
    {
      getJointJacobian(model,data,idx,LOCAL,Jrh);
    }
    if(verbose) std::cout << "Change frame =\t";
    timer.toc(std::cout,NBT);
  }

  if( flag >> 3 & 1 )
  {
    computeJointJacobians(model,data,q);
    Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoints-1);
    Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);

    timer.tic();
    SMOOTH(NBT)
    {
      jointJacobian(model,data,q,idx,Jrh);
    }
    if(verbose) std::cout << "Single jacobian =\t";
    timer.toc(std::cout,NBT);
  }
}

BOOST_AUTO_TEST_SUITE_END ()


/*=============================================================================
    Copyright (c) 2001-2003 Joel de Guzman

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
==============================================================================*/
#include <vector>
#include <algorithm>
#include <iostream>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_function.hpp>

using namespace boost::phoenix;
using namespace boost::phoenix::arg_names;
using namespace std;

struct factorial_impl
{
    template <typename Arg>
    struct result
    {
        typedef Arg type;
    };

    template <typename Arg>
    Arg operator()(Arg n) const
    {
        return (n <= 0) ? 1 : n * this->operator()(n-1);
    }
};

function<factorial_impl> factorial;

int
main()
{
    int i = 4;
    cout << factorial(arg1)(i) << endl;
    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2016-2017 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_AREA_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_AREA_HPP

#include <boost/geometry/formulas/area_formulas.hpp>
#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>
#include <boost/geometry/strategies/area.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace area
{

/*!
\brief Spherical area calculation
\ingroup strategies
\details Calculates area on the surface of a sphere using the trapezoidal rule
\tparam PointOfSegment \tparam_segment_point
\tparam CalculationType \tparam_calculation

\qbk{
[heading See also]
[link geometry.reference.algorithms.area.area_2_with_strategy area (with strategy)]
}
*/
template
<
    typename PointOfSegment,
    typename CalculationType = void
>
class spherical
{
    // Enables special handling of long segments
    static const bool LongSegment = false;

typedef typename boost::mpl::if_c
    <
        boost::is_void<CalculationType>::type::value,
        typename select_most_precise
            <
                typename coordinate_type<PointOfSegment>::type,
                double
            >::type,
        CalculationType
    >::type CT;

protected :
    struct excess_sum
    {
        CT m_sum;

        // Keep track if encircles some pole
        size_t m_crosses_prime_meridian;

        inline excess_sum()
            : m_sum(0)
            , m_crosses_prime_meridian(0)
        {}
        template <typename SphereType>
        inline CT area(SphereType sphere) const
        {
            CT result;
            CT radius = geometry::get_radius<0>(sphere);

            // Encircles pole
            if(m_crosses_prime_meridian % 2 == 1)
            {
                size_t times_crosses_prime_meridian
                        = 1 + (m_crosses_prime_meridian / 2);

                result = CT(2)
                         * geometry::math::pi<CT>()
                         * times_crosses_prime_meridian
                         - geometry::math::abs(m_sum);

                if(geometry::math::sign<CT>(m_sum) == 1)
                {
                    result = - result;
                }

            } else {
                result =  m_sum;
            }

            result *= radius * radius;

            return result;
        }
    };

public :
    typedef CT return_type;
    typedef PointOfSegment segment_point_type;
    typedef excess_sum state_type;
    typedef geometry::srs::sphere<CT> sphere_type;

    // For backward compatibility reasons the radius is set to 1
    inline spherical()
        : m_sphere(1.0)
    {}

    template <typename T>
    explicit inline spherical(geometry::srs::sphere<T> const& sphere)
        : m_sphere(geometry::get_radius<0>(sphere))
    {}

    explicit inline spherical(CT const& radius)
        : m_sphere(radius)
    {}

    inline void apply(PointOfSegment const& p1,
                      PointOfSegment const& p2,
                      excess_sum& state) const
    {
        if (! geometry::math::equals(get<0>(p1), get<0>(p2)))
        {
            typedef geometry::formula::area_formulas<CT> area_formulas;

            state.m_sum += area_formulas::template spherical<LongSegment>(p1, p2);

            // Keep track whenever a segment crosses the prime meridian
            if (area_formulas::crosses_prime_meridian(p1, p2))
            {
                state.m_crosses_prime_meridian++;
            }
        }
    }

    inline return_type result(excess_sum const& state) const
    {
        return state.area(m_sphere);
    }

private :
    /// srs Sphere
    sphere_type m_sphere;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <typename Point>
struct default_strategy<spherical_equatorial_tag, Point>
{
    typedef strategy::area::spherical<Point> type;
};

// Note: spherical polar coordinate system requires "get_as_radian_equatorial"
template <typename Point>
struct default_strategy<spherical_polar_tag, Point>
{
    typedef strategy::area::spherical<Point> type;
};

} // namespace services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::area

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_AREA_HPP


/*
 * @file
 * @author University of Warwick
 * @version 1.0
 *
 * @section LICENSE
 *
 * @section DESCRIPTION
 *
 * Unit Tests for the concrete methods of the Triangle2D class
 */

#define BOOST_TEST_MODULE Triangle2D
#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>
#include <stdexcept>

#include "Triangle2D.h"
#include "EuclideanPoint.h"

using namespace cupcfd::geometry::shapes;
namespace euc = cupcfd::geometry::euclidean;
namespace utf = boost::unit_test;

// === Constructor ===
// Test 1: Constructor: 3 Defined Points - 2D
BOOST_AUTO_TEST_CASE(constructor_test1, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	Triangle2D<double> shape(p1, p2, p3);

	BOOST_CHECK_EQUAL(shape.numVertices, 3);
	BOOST_CHECK_EQUAL(shape.vertices[0].cmp[0], p1.cmp[0]);
	BOOST_CHECK_EQUAL(shape.vertices[0].cmp[1], p1.cmp[1]);
	BOOST_CHECK_EQUAL(shape.vertices[1].cmp[0], p2.cmp[0]);
	BOOST_CHECK_EQUAL(shape.vertices[1].cmp[1], p2.cmp[1]);
	BOOST_CHECK_EQUAL(shape.vertices[2].cmp[0], p3.cmp[0]);
	BOOST_CHECK_EQUAL(shape.vertices[2].cmp[1], p3.cmp[1]);
}

// Test 2: Copy Constructor
BOOST_AUTO_TEST_CASE(constructor_test2, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	Triangle2D<double> shape(p1, p2, p3);
	Triangle2D<double> shape2(shape);

	BOOST_CHECK_EQUAL(shape2.numVertices, 3);
	BOOST_CHECK_EQUAL(shape2.vertices[0].cmp[0], p1.cmp[0]);
	BOOST_CHECK_EQUAL(shape2.vertices[0].cmp[1], p1.cmp[1]);
	BOOST_CHECK_EQUAL(shape2.vertices[1].cmp[0], p2.cmp[0]);
	BOOST_CHECK_EQUAL(shape2.vertices[1].cmp[1], p2.cmp[1]);
	BOOST_CHECK_EQUAL(shape2.vertices[2].cmp[0], p3.cmp[0]);
	BOOST_CHECK_EQUAL(shape2.vertices[2].cmp[1], p3.cmp[1]);
}

// === isPointInsideBarycentric (static, 2D) ===
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test1, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(3.1, 6.0);

	bool inside = Triangle2D<double>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, true);
}

// Test 2: Test a point on top of vertex a - 2D
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test2, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	euc::EuclideanPoint<double,2> p4 = p1;

	// Test and Check
	bool inside = Triangle2D<double>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, true);
}

// Test 3: Test a point on top of vertex b - 2D
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test3, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	euc::EuclideanPoint<double,2> p4 = p2;

	// Test and Check
	bool inside = Triangle2D<double>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, true);
}

// Test 4: Test a point on top of vertex c - 2D
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test4, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	euc::EuclideanPoint<double,2> p4 = p3;

	// Test and Check
	bool inside = Triangle2D<double>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, true);
}

// Test 5: Test a point on edge ab - 2D
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test5, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(3.5, 8.0);

	bool inside = Triangle2D<double>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, true);
}

// Test 6: Test a point on edge ac - 2D
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test6, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(3.15, 9.5);

	bool inside = Triangle2D<double>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, true);
}

// Test 7: Test a point on edge bc - 2D
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test7, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(3.65, 13.5);

	bool inside = Triangle2D<double>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, true);
}

// Test 8: Test a point outside the Triangle - 2D
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test8, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(1.0, 1.34);

	bool inside = Triangle2D<double>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, false);
}

// Test 9: Test a point inside when a.cmp[1] == c.cmp[1] - 2D
BOOST_AUTO_TEST_CASE(isPointInsideBarycentric_static_test9, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(3.15, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 4.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(3.15, 4.2);

	bool inside = Triangle2D<double>::isPointInsideBarycentric(p1,p2,p3,p4);
	BOOST_CHECK_EQUAL(inside, true);
}

// === getCentroid ===
// Test 1: Correctly compute the center for the triangles points
BOOST_AUTO_TEST_CASE(computeCenter_test1, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	Triangle2D<double> shape(p1,p2,p3);
	cupcfd::geometry::euclidean::EuclideanPoint<double,2> center = shape.getCentroid();
	BOOST_TEST(center.cmp[0] == 3.433333);
	BOOST_TEST(center.cmp[1] == 10.333333);
}

// === isPointInside ===
BOOST_AUTO_TEST_CASE(isPointInside_test1, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(3.1, 6.0);

	Triangle2D<double> shape(p1,p2,p3);
	bool inside = shape.isPointInside(p4);

	BOOST_CHECK_EQUAL(inside, true);
}

// Test 2: Test a point on top of vertex a - 2D
BOOST_AUTO_TEST_CASE(isPointInside_test2, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	euc::EuclideanPoint<double,2> p4 = p1;

	// Test and Check
	Triangle2D<double> shape(p1,p2,p3);
	bool inside = shape.isPointInside(p4);

	BOOST_CHECK_EQUAL(inside, true);
}

// Test 3: Test a point on top of vertex b - 2D
BOOST_AUTO_TEST_CASE(isPointInside_test3, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	euc::EuclideanPoint<double,2> p4 = p2;

	// Test and Check
	Triangle2D<double> shape(p1,p2,p3);
	bool inside = shape.isPointInside(p4);

	BOOST_CHECK_EQUAL(inside, true);
}

// Test 4: Test a point on top of vertex c - 2D
BOOST_AUTO_TEST_CASE(isPointInside_test4, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	euc::EuclideanPoint<double,2> p4 = p3;

	// Test and Check
	Triangle2D<double> shape(p1,p2,p3);
	bool inside = shape.isPointInside(p4);

	BOOST_CHECK_EQUAL(inside, true);
}

// Test 5: Test a point on edge ab - 2D
BOOST_AUTO_TEST_CASE(isPointInside_test5, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(3.5, 8.0);

	Triangle2D<double> shape(p1,p2,p3);
	bool inside = shape.isPointInside(p4);

	BOOST_CHECK_EQUAL(inside, true);
}

// Test 6: Test a point on edge ac - 2D
BOOST_AUTO_TEST_CASE(isPointInside_test6, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(3.15, 9.5);

	Triangle2D<double> shape(p1,p2,p3);
	bool inside = shape.isPointInside(p4);

	BOOST_CHECK_EQUAL(inside, true);
}

// Test 7: Test a point on edge bc - 2D
BOOST_AUTO_TEST_CASE(isPointInside_test7, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(3.65, 13.5);

	Triangle2D<double> shape(p1,p2,p3);
	bool inside = shape.isPointInside(p4);

	BOOST_CHECK_EQUAL(inside, true);
}

// Test 8: Test a point outside the Triangle - 2D
BOOST_AUTO_TEST_CASE(isPointInside_test8, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(4.0, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 15.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(1.0, 1.34);

	Triangle2D<double> shape(p1,p2,p3);
	bool inside = shape.isPointInside(p4);

	BOOST_CHECK_EQUAL(inside, false);
}

// Test 9: Test a point inside when a.cmp[1] == c.cmp[1] - 2D
BOOST_AUTO_TEST_CASE(isPointInside_test9, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(3.15, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 4.0);

	// Test and Check
	euc::EuclideanPoint<double,2> p4(3.15, 4.2);

	Triangle2D<double> shape(p1,p2,p3);
	bool inside = shape.isPointInside(p4);

	BOOST_CHECK_EQUAL(inside, true);
}

// === getArea ===
// Test 1: Test the area is computed correctly - 2D
BOOST_AUTO_TEST_CASE(getArea_test1, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(3.15, 12.0);
	euc::EuclideanPoint<double,2> p3(3.3, 4.0);

	// Test and Check
	Triangle2D<double> shape(p1, p2, p3);
	double area = shape.getArea();
	BOOST_TEST(area == 1.2);
}

// Test 2: Test the area is computed correctly - 2D
BOOST_AUTO_TEST_CASE(getArea_test2, * utf::tolerance(0.00001))
{
	// Setup
	euc::EuclideanPoint<double,2> p1(3.0, 4.0);
	euc::EuclideanPoint<double,2> p2(6.0, 4.0);
	euc::EuclideanPoint<double,2> p3(3.0, 12.0);

	// Test and Check
	Triangle2D<double> shape(p1, p2, p3);
	double area = shape.getArea();
	BOOST_TEST(area == 12.0);
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 Delft University of Technology 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      100903    K. Kumar          File created.
 *      100916    L. Abdulkadir     File checked.
 *      100929    K. Kumar          Checked code by D. Dirkx added.
 *      101110    K. Kumar          Added raiseToExponentPower( ) function.
 *      102410    D. Dirkx          Minor comment changes as code check.
 *      101213    K. Kumar          Bugfix raiseToIntegerExponent( ); renamed raiseToIntegerPower( ).
 *                                  Added computeAbsoluteValue( ) functions.
 *      110202    K. Kumar          Added overload for State* for computeLinearInterpolation( ).
 *      110111    J. Melman         Added computeModulo( ) function.
 *      110411    K. Kumar          Added convertCartesianToSpherical( ) function.
 *      110606    J. Melman         Removed possible singularity from
 *                                  convertCartesianToSpherical.
 *      110707    K. Kumar          Added computeSampleMean( ), computeSampleVariance( ) functions.
 *      110905    S. Billemont      Reorganized includes.
 *                                  Moved (con/de)structors and getter/setters to header.
 *      120202    K. Kumar          Moved linear interpolation functions into new Interpolators
 *                                  sub-directory.
 *      120716    D. Dirkx          Updated with interpolator architecture.
 *
 *    References
 *      Press W.H., et al. Numerical Recipes in C++: The Art of Scientific Computing. Cambridge
 *          University Press, February 2002.
 *
 *    Notes
 *
 */

#include <boost/multi_array.hpp>

#include "Tudat/Mathematics/Interpolators/linearInterpolator.h"

namespace tudat
{
namespace interpolators
{

//! Compute linear interpolation.
double computeLinearInterpolation( const Eigen::VectorXd& sortedIndependentVariables,
                                   const Eigen::VectorXd& associatedDependentVariables,
                                   const double targetIndependentVariableValue )
{
    // Declare local variables.
    // Declare nearest neighbor.
    int nearestNeighbor;
    double locationTargetIndependentVariableValueInInterval;

    // Compute nearest neighbor in sorted vector of independent variables.
    // Result is always to the left of the target independent variable value.
    nearestNeighbor = basic_mathematics::computeNearestLeftNeighborUsingBinarySearch(
            sortedIndependentVariables, targetIndependentVariableValue );

    // Compute location of target independent variable value in interval
    // between nearest neighbors.
    locationTargetIndependentVariableValueInInterval
            = ( targetIndependentVariableValue
              - sortedIndependentVariables[ nearestNeighbor ] )
             / ( sortedIndependentVariables[ nearestNeighbor + 1 ]
                 - sortedIndependentVariables[ nearestNeighbor ] );

    // Return the computed value of the dependent variable.
    return ( associatedDependentVariables[ nearestNeighbor ]
             * ( 1 - locationTargetIndependentVariableValueInInterval )
             + associatedDependentVariables[ nearestNeighbor + 1 ]
             * locationTargetIndependentVariableValueInInterval );
}

//! Compute linear interpolation.
Eigen::VectorXd computeLinearInterpolation(
        const std::map < double, Eigen::VectorXd >& sortedIndepedentAndDependentVariables,
        const double targetIndependentVariableValue )
{
    // Declare local variables.
    // Declare nearest neighbor.
    int nearestLeftNeighbor;

    // Declare location of target independent variable value in interval.
    double locationTargetIndependentVariableValueInInterval;

    // Declare map iterators
    std::map< double, Eigen::VectorXd >::const_iterator mapIteratorIntervalLeft;
    std::map< double, Eigen::VectorXd >::const_iterator mapIteratorIntervalRight;

    // Compute nearest neighbor in map of data.
    // Result is always to the left of the target independent variable value.
    nearestLeftNeighbor = basic_mathematics::computeNearestLeftNeighborUsingBinarySearch(
                sortedIndepedentAndDependentVariables, targetIndependentVariableValue );

    // Compute location of target independent variable value in interval
    // between nearest neighbors.
    mapIteratorIntervalLeft = sortedIndepedentAndDependentVariables.begin( );
    advance( mapIteratorIntervalLeft, nearestLeftNeighbor );
    mapIteratorIntervalRight = sortedIndepedentAndDependentVariables.begin( );
    advance( mapIteratorIntervalRight, nearestLeftNeighbor + 1 );
    locationTargetIndependentVariableValueInInterval
            = ( targetIndependentVariableValue
              - mapIteratorIntervalLeft->first )
             / ( mapIteratorIntervalRight->first
                 - mapIteratorIntervalLeft->first );

    // Return the computed value of the dependent variable.
    return ( mapIteratorIntervalLeft->second
             * ( 1 - locationTargetIndependentVariableValueInInterval )
             + mapIteratorIntervalRight->second
             * locationTargetIndependentVariableValueInInterval );
}

} // namespace interpolators
} // mamespace tudat


// Copyright (C) 2013  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/python.h>
#include <boost/shared_ptr.hpp>
#include <dlib/matrix.h>
#include <boost/python/slice.hpp>
#include <dlib/geometry/vector.h>
#include "indexing.h"

using namespace dlib;
using namespace std;
using namespace boost::python;

typedef matrix<double,0,1> cv;

void cv_set_size(cv& m, long s)
{
    m.set_size(s);
    m = 0;
}

double dotprod ( const cv& a, const cv& b)
{
    return dot(a,b);
}

string cv__str__(const cv& v)
{
    ostringstream sout;
    for (long i = 0; i < v.size(); ++i)
    {
        sout << v(i);
        if (i+1 < v.size())
            sout << "\n";
    }
    return sout.str();
}

string cv__repr__ (const cv& v)
{
    std::ostringstream sout;
    sout << "dlib.vector([";
    for (long i = 0; i < v.size(); ++i)
    {
        sout << v(i);
        if (i+1 < v.size())
            sout << ", ";
    }
    sout << "])";
    return sout.str();
}

boost::shared_ptr<cv> cv_from_object(object obj)
{
    extract<long> thesize(obj);
    if (thesize.check())
    {
        long nr = thesize;
        boost::shared_ptr<cv> temp(new cv(nr));
        *temp = 0;
        return temp;
    }
    else
    {
        const long nr = len(obj);
        boost::shared_ptr<cv> temp(new cv(nr));
        for ( long r = 0; r < nr; ++r)
        {
            (*temp)(r) = extract<double>(obj[r]);
        }
        return temp;
    }
}

long cv__len__(cv& c)
{
    return c.size();
}

void cv__setitem__(cv& c, long p, double val)
{
    if (p < 0) {
        p = c.size() + p; // negative index
    }
    if (p > c.size()-1) {
        PyErr_SetString( PyExc_IndexError, "index out of range"
        );
        boost::python::throw_error_already_set();
    }
    c(p) = val;
}

double cv__getitem__(cv& m, long r)
{
    if (r < 0) {
        r = m.size() + r; // negative index
    }
    if (r > m.size()-1 || r < 0) {
        PyErr_SetString( PyExc_IndexError, "index out of range"
        );
        boost::python::throw_error_already_set();
    }
    return m(r);
}

cv cv__getitem2__(cv& m, slice r)
{
    slice::range<cv::iterator> bounds;
    bounds = r.get_indicies<>(m.begin(), m.end());
    long num = (bounds.stop-bounds.start+1);
    // round num up to the next multiple of bounds.step.
    if ((num%bounds.step) != 0)
        num += bounds.step - num%bounds.step;

    cv temp(num/bounds.step);

    if (temp.size() == 0)
        return temp;
    long ii = 0;
    while(bounds.start != bounds.stop)
    {
        temp(ii++) = *bounds.start;
        std::advance(bounds.start, bounds.step);
    }
    temp(ii) = *bounds.start;
    return temp;
}

boost::python::tuple cv_get_matrix_size(cv& m)
{
    return boost::python::make_tuple(m.nr(), m.nc());
}

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

string point__repr__ (const point& p)
{
    std::ostringstream sout;
    sout << "point(" << p.x() << ", " << p.y() << ")";
    return sout.str();
}

string point__str__(const point& p)
{
    std::ostringstream sout;
    sout << "(" << p.x() << ", " << p.y() << ")";
    return sout.str();
}

long point_x(const point& p) { return p.x(); }
long point_y(const point& p) { return p.y(); }

// ----------------------------------------------------------------------------------------
void bind_vector()
{
    using boost::python::arg;
    {
    class_<cv>("vector", "This object represents the mathematical idea of a column vector.", init<>())
        .def("set_size", &cv_set_size)
        .def("resize", &cv_set_size)
        .def("__init__", make_constructor(&cv_from_object))
        .def("__repr__", &cv__repr__)
        .def("__str__", &cv__str__)
        .def("__len__", &cv__len__)
        .def("__getitem__", &cv__getitem__)
        .def("__getitem__", &cv__getitem2__)
        .def("__setitem__", &cv__setitem__)
        .add_property("shape", &cv_get_matrix_size)
        .def_pickle(serialize_pickle<cv>());

    def("dot", dotprod, "Compute the dot product between two dense column vectors.");
    }
    {
    typedef point type;
    class_<type>("point", "This object represents a single point of integer coordinates that maps directly to a dlib::point.")
            .def(init<long,long>((arg("x"), arg("y"))))
            .def("__repr__", &point__repr__)
            .def("__str__", &point__str__)
            .add_property("x", &point_x, "The x-coordinate of the point.")
            .add_property("y", &point_y, "The y-coordinate of the point.")
            .def_pickle(serialize_pickle<type>());
    }
    {
    typedef std::vector<point> type;
    class_<type>("points", "An array of point objects.")
        .def(vector_indexing_suite<type>())
        .def("clear", &type::clear)
        .def("resize", resize<type>)
        .def_pickle(serialize_pickle<type>());
    }
}


//  Copyright (c) 2006 Xiaogang Zhang
//  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)
//
//  History:
//  XZ wrote the original of this file as part of the Google
//  Summer of Code 2006.  JM modified it slightly to fit into the
//  Boost.Math conceptual framework better.

#ifndef BOOST_MATH_ELLINT_RD_HPP
#define BOOST_MATH_ELLINT_RD_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/policies/error_handling.hpp>

// Carlson's elliptic integral of the second kind
// R_D(x, y, z) = R_J(x, y, z, z) = 1.5 * \int_{0}^{\infty} [(t+x)(t+y)]^{-1/2} (t+z)^{-3/2} dt
// Carlson, Numerische Mathematik, vol 33, 1 (1979)

namespace boost { namespace math { namespace detail{

template <typename T, typename Policy>
T ellint_rd_imp(T x, T y, T z, const Policy& pol)
{
    T value, u, lambda, sigma, factor, tolerance;
    T X, Y, Z, EA, EB, EC, ED, EE, S1, S2;
    unsigned long k;

    BOOST_MATH_STD_USING
    using namespace boost::math::tools;

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

    if (x < 0)
    {
       return policies::raise_domain_error<T>(function,
            "Argument x must be >= 0, but got %1%", x, pol);
    }
    if (y < 0)
    {
       return policies::raise_domain_error<T>(function,
            "Argument y must be >= 0, but got %1%", y, pol);
    }
    if (z <= 0)
    {
       return policies::raise_domain_error<T>(function,
            "Argument z must be > 0, but got %1%", z, pol);
    }
    if (x + y == 0)
    {
       return policies::raise_domain_error<T>(function,
            "At most one argument can be zero, but got, x + y = %1%", x+y, pol);
    }

    // error scales as the 6th power of tolerance
    tolerance = pow(tools::epsilon<T>() / 3, T(1)/6);

    // duplication
    sigma = 0;
    factor = 1;
    k = 1;
    do
    {
        u = (x + y + z + z + z) / 5;
        X = (u - x) / u;
        Y = (u - y) / u;
        Z = (u - z) / u;
        if ((tools::max)(abs(X), abs(Y), abs(Z)) < tolerance)
           break;
        T sx = sqrt(x);
        T sy = sqrt(y);
        T sz = sqrt(z);
        lambda = sy * (sx + sz) + sz * sx; //sqrt(x * y) + sqrt(y * z) + sqrt(z * x);
        sigma += factor / (sz * (z + lambda));
        factor /= 4;
        x = (x + lambda) / 4;
        y = (y + lambda) / 4;
        z = (z + lambda) / 4;
        ++k;
    }
    while(k < policies::get_max_series_iterations<Policy>());

    // Check to see if we gave up too soon:
    policies::check_series_iterations<T>(function, k, pol);

    // Taylor series expansion to the 5th order
    EA = X * Y;
    EB = Z * Z;
    EC = EA - EB;
    ED = EA - 6 * EB;
    EE = ED + EC + EC;
    S1 = ED * (ED * T(9) / 88 - Z * EE * T(9) / 52 - T(3) / 14);
    S2 = Z * (EE / 6 + Z * (-EC * T(9) / 22 + Z * EA * T(3) / 26));
    value = 3 * sigma + factor * (1 + S1 + S2) / (u * sqrt(u));

    return value;
}

} // namespace detail

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type
   ellint_rd(T1 x, T2 y, T3 z, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::ellint_rd_imp(
         static_cast<value_type>(x),
         static_cast<value_type>(y),
         static_cast<value_type>(z), pol), "boost::math::ellint_rd<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type
   ellint_rd(T1 x, T2 y, T3 z)
{
   return ellint_rd(x, y, z, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_ELLINT_RD_HPP


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

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/sincpi.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/constant/mindenormal.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/function/sinpi.hpp>

STF_CASE_TPL(" sinc",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::sincpi;

  STF_EXPR_IS(sincpi(T()),T);

#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(sincpi(bs::Inf<T>()), bs::Zero<T>(), 1.0);
  STF_ULP_EQUAL(sincpi(bs::Minf<T>()), bs::Zero<T>(), 1.0);
  STF_ULP_EQUAL(sincpi(bs::Nan<T>()), bs::Nan<T>(), 1.0);
#endif
  STF_ULP_EQUAL(sincpi(-T(1)/T(2)), T(2)/(bs::Pi<T>()), 1.0);
  STF_ULP_EQUAL(sincpi(-T(1)/T(4)), bs::sinpi(T(1)/T(4))*T(4)/(bs::Pi<T>()), 1.0);
  STF_ULP_EQUAL(sincpi(T(1)/T(2)),  T(2)/(bs::Pi<T>()), 1.0);
  STF_ULP_EQUAL(sincpi(T(1)/T(4)), bs::sinpi(T(1)/T(4))*T(4)/(bs::Pi<T>()), 1.0);
  STF_ULP_EQUAL(sincpi(bs::Eps<T>()), bs::One<T>(), 1.0);
  STF_ULP_EQUAL(sincpi(bs::Mindenormal<T>()), bs::One<T>(), 1.0);
  STF_ULP_EQUAL(sincpi(bs::Zero<T>()), bs::One<T>(), 1.0);
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2018.
// Modifications copyright (c) 2018 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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)

//[area_with_strategy
//` Calculate the area of a polygon

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

namespace bg = boost::geometry; /*< Convenient namespace alias >*/

int main()
{
    // Create spherical polygon
    bg::model::polygon<bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree> > > sph_poly;
    bg::read_wkt("POLYGON((0 0,0 1,1 0,0 0))", sph_poly);

    // Create spherical strategy with mean Earth radius in meters
    bg::strategy::area::spherical<> sph_strategy(6371008.8);

    // Calculate the area of a spherical polygon
    double area = bg::area(sph_poly, sph_strategy);
    std::cout << "Area: " << area << std::endl;

    // Create geographic polygon
    bg::model::polygon<bg::model::point<double, 2, bg::cs::geographic<bg::degree> > > geo_poly;
    bg::read_wkt("POLYGON((0 0,0 1,1 0,0 0))", geo_poly);

    // Create geographic strategy with WGS84 spheroid
    bg::srs::spheroid<double> spheroid(6378137.0, 6356752.3142451793);
    bg::strategy::area::geographic<> geo_strategy(spheroid);

    // Calculate the area of a geographic polygon
    area = bg::area(geo_poly, geo_strategy);
    std::cout << "Area: " << area << std::endl;

    return 0;
}

//]

//[area_with_strategy_output
/*`
Output:
[pre
Area: 6.18249e+09
Area: 6.15479e+09
]
*/
//]


/// @file  embed_cmp_with_tau.cpp
/// @brief Implements kernel methods based on Kendall's tau.

#include <ogt/embed/cmp.hpp>
#include <ogt/linalg/linalg.hpp>
#include <Eigen/Sparse>

using std::min;
using std::max;
using std::vector;
using Eigen::Index;
using Eigen::MatrixXd;
using Eigen::SparseMatrix;
using Eigen::VectorXd;
using OGT_NAMESPACE::linalg::kernelForFeatures;

typedef Eigen::Triplet<double> sptriple;
typedef Eigen::SparseMatrix<double, Eigen::RowMajor> SpMat;

namespace OGT_NAMESPACE {
namespace embed {

/// Finds the index of the specified object pair.
Index pair(size_t a, size_t b, size_t nObj) {
	Index i = min(a, b), j = max(a, b);
	return (nObj * (nObj-1) / 2)
		- ((nObj - i) * (nObj - i - 1) / 2)
		+ (j - i - 1);
}

/// Computes a kernel matrix from a sparse feature matrix.
/// Computes the k1 kernel matrix from the paper cited for embedCmpWithTauForK()
MatrixXd embedCmpWithTauK1(const vector<CmpConstraint>& cons, size_t nObj) {
	vector<sptriple> triplets;
	triplets.reserve(cons.size());
	VectorXd norm = VectorXd::Zero(nObj);
	Index nPairs = nObj * (nObj - 1) / 2;
	for (const auto& con : cons) {
		assert(con.a < nObj && con.b < nObj && con.c < nObj);
		Index pr = pair(con.b, con.c, nObj);
		triplets.emplace_back(con.a, pr, (con.b < con.c) ? +1 : -1);
		norm(con.a)++;
	}
	SpMat phi(nObj, nPairs);
	phi.setFromTriplets(triplets.begin(), triplets.end());
	for (size_t obj = 0; obj < nObj; obj++) {
		if (norm(obj) > 0) {
			phi.row(obj) /= sqrt(norm(obj));
		}
	}
	return kernelForFeatures(phi);
}

/// Computes the k2 kernel matrix from the paper cited for embedCmpWithTauForK()
MatrixXd embedCmpWithTauK2(const vector<CmpConstraint>& cons, size_t nObj) {
	vector<sptriple> triplets;
	triplets.reserve(2 * cons.size());
	VectorXd norm = VectorXd::Zero(nObj);
	Index nPairs = nObj * (nObj - 1) / 2;
	for (const auto& con : cons) {
		assert(con.a < nObj && con.b < nObj && con.c < nObj);
		Index pr = pair(con.a, con.c, nObj);
		triplets.emplace_back(con.b, pr, +1);
		norm(con.b)++;

		pr = pair(con.a, con.b, nObj);
		if (pr >= nPairs) {
			std::cerr << "pair " << pr << " > " << nPairs << std::endl;
		}
		triplets.emplace_back(con.c, pr, -1);
		norm(con.c)++;
	}
	SpMat phi(nObj, nPairs);
	phi.setFromTriplets(triplets.begin(), triplets.end());
	for (size_t obj = 0; obj < nObj; obj++) {
		if (norm(obj) > 0) {
			phi.row(obj) /= sqrt(norm(obj));
		}
	}
	return kernelForFeatures(phi);
}

/// Compute a kernel matrix where K_ij is derived by how similarly points i and
/// j are ranked by the provided comparisons.
EmbedResult embedCmpWithTauForK(vector<CmpConstraint> cons, double k1,
	double k2) {
	size_t nObj = 0;
	for (const auto& con : cons) {
		nObj = max({nObj, con.a, con.b, con.c});
	}
	nObj++;
	EmbedResult result;
	result.K = MatrixXd::Zero(nObj, nObj);
	if (k1 > 0) {
		result.K += k1 * embedCmpWithTauK1(cons, nObj);
	}
	if (k2 > 0) {
		result.K += k2 * embedCmpWithTauK2(cons, nObj);
	}
	return result;
}

} // end namespace embed
} // end namespace OGT_NAMESPACE