// Copyright 2002 Rensselaer Polytechnic Institute

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

//  Authors: Lauren Foutz
//           Scott Hill
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <map>
#include <algorithm>
#include <iostream>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include <boost/graph/random.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/core/lightweight_test.hpp>
#include <algorithm>
using namespace boost;

template < typename T > inline const T& my_min(const T& x, const T& y)
{
    return x < y ? x : y;
}

template < typename Graph > bool acceptance_test(Graph& g, int vec, int e)
{
    boost::minstd_rand ran(vec);

    {
        typename boost::property_map< Graph, boost::vertex_name_t >::type index
            = boost::get(boost::vertex_name, g);
        typename boost::graph_traits< Graph >::vertex_iterator firstv, lastv,
            firstv2, lastv2;
        int x = 0;
        for (boost::tie(firstv, lastv) = boost::vertices(g); firstv != lastv;
             firstv++)
        {
            boost::put(index, *firstv, x);
            x++;
        }

        for (int i = 0; i < e; i++)
        {
            boost::add_edge(index[ran() % vec], index[ran() % vec], g);
        }

        typename boost::graph_traits< Graph >::edge_iterator first, last;
        typename boost::property_map< Graph, boost::edge_weight_t >::type
            local_edge_map
            = boost::get(boost::edge_weight, g);
        for (boost::tie(first, last) = boost::edges(g); first != last; first++)
        {
            if (ran() % vec != 0)
            {
                boost::put(local_edge_map, *first, ran() % 100);
            }
            else
            {
                boost::put(local_edge_map, *first, 0 - (ran() % 100));
            }
        }

        int int_inf = std::numeric_limits< int >::max
        BOOST_PREVENT_MACRO_SUBSTITUTION();
        typedef
            typename boost::graph_traits< Graph >::vertex_descriptor vertex_des;
        std::map< vertex_des, int > matrixRow;
        std::map< vertex_des, std::map< vertex_des, int > > matrix;
        typedef typename boost::property_map< Graph,
            boost::vertex_distance_t >::type distance_type;
        distance_type distance_row = boost::get(boost::vertex_distance, g);
        for (boost::tie(firstv, lastv) = boost::vertices(g); firstv != lastv;
             firstv++)
        {
            boost::put(distance_row, *firstv, int_inf);
            matrixRow[*firstv] = int_inf;
        }
        for (boost::tie(firstv, lastv) = boost::vertices(g); firstv != lastv;
             firstv++)
        {
            matrix[*firstv] = matrixRow;
        }
        for (boost::tie(firstv, lastv) = boost::vertices(g); firstv != lastv;
             firstv++)
        {
            matrix[*firstv][*firstv] = 0;
        }
        std::map< vertex_des, std::map< vertex_des, int > > matrix3(matrix);
        std::map< vertex_des, std::map< vertex_des, int > > matrix4(matrix);
        for (boost::tie(first, last) = boost::edges(g); first != last; first++)
        {
            if (matrix[boost::source(*first, g)][boost::target(*first, g)]
                != int_inf)
            {
                matrix[boost::source(*first, g)][boost::target(*first, g)]
                    = my_min(boost::get(local_edge_map, *first),
                        matrix[boost::source(*first, g)]
                              [boost::target(*first, g)]);
            }
            else
            {
                matrix[boost::source(*first, g)][boost::target(*first, g)]
                    = boost::get(local_edge_map, *first);
            }
        }
        bool is_undirected = boost::is_same<
            typename boost::graph_traits< Graph >::directed_category,
            boost::undirected_tag >::value;
        if (is_undirected)
        {
            for (boost::tie(first, last) = boost::edges(g); first != last;
                 first++)
            {
                if (matrix[boost::target(*first, g)][boost::source(*first, g)]
                    != int_inf)
                {
                    matrix[boost::target(*first, g)][boost::source(*first, g)]
                        = my_min(boost::get(local_edge_map, *first),
                            matrix[boost::target(*first, g)]
                                  [boost::source(*first, g)]);
                }
                else
                {
                    matrix[boost::target(*first, g)][boost::source(*first, g)]
                        = boost::get(local_edge_map, *first);
                }
            }
        }

        bool bellman, floyd1, floyd2, floyd3;
        floyd1 = boost::floyd_warshall_initialized_all_pairs_shortest_paths(g,
            matrix,
            weight_map(boost::get(boost::edge_weight, g))
                .distance_inf(int_inf)
                .distance_zero(0));

        floyd2 = boost::floyd_warshall_all_pairs_shortest_paths(g, matrix3,
            weight_map(local_edge_map).distance_inf(int_inf).distance_zero(0));

        floyd3 = boost::floyd_warshall_all_pairs_shortest_paths(g, matrix4);

        boost::dummy_property_map dummy_map;
        std::map< vertex_des, std::map< vertex_des, int > > matrix2;
        for (boost::tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
        {
            boost::put(distance_row, *firstv, 0);
            bellman = boost::bellman_ford_shortest_paths(g, vec,
                weight_map(boost::get(boost::edge_weight, g))
                    .distance_map(boost::get(boost::vertex_distance, g))
                    .predecessor_map(dummy_map));
            distance_row = boost::get(boost::vertex_distance, g);
            for (boost::tie(firstv2, lastv2) = vertices(g); firstv2 != lastv2;
                 firstv2++)
            {
                matrix2[*firstv][*firstv2] = boost::get(distance_row, *firstv2);
                boost::put(distance_row, *firstv2, int_inf);
            }
            if (bellman == false)
            {
                break;
            }
        }

        if (bellman != floyd1 || bellman != floyd2 || bellman != floyd3)
        {
            std::cout << "A negative cycle was detected in one algorithm but "
                         "not the others. "
                      << std::endl;
            return false;
        }
        else if (bellman == false && floyd1 == false && floyd2 == false
            && floyd3 == false)
        {
            return true;
        }
        else
        {
            typename boost::graph_traits< Graph >::vertex_iterator first1,
                first2, last1, last2;
            for (boost::tie(first1, last1) = boost::vertices(g);
                 first1 != last1; first1++)
            {
                for (boost::tie(first2, last2) = boost::vertices(g);
                     first2 != last2; first2++)
                {
                    if (matrix2[*first1][*first2] != matrix[*first1][*first2])
                    {
                        std::cout
                            << "Algorithms do not match at matrix point "
                            << index[*first1] << " " << index[*first2]
                            << " Bellman results: " << matrix2[*first1][*first2]
                            << " floyd 1 results " << matrix[*first1][*first2]
                            << std::endl;
                        return false;
                    }
                    if (matrix2[*first1][*first2] != matrix3[*first1][*first2])
                    {
                        std::cout
                            << "Algorithms do not match at matrix point "
                            << index[*first1] << " " << index[*first2]
                            << " Bellman results: " << matrix2[*first1][*first2]
                            << " floyd 2 results " << matrix3[*first1][*first2]
                            << std::endl;
                        return false;
                    }
                    if (matrix2[*first1][*first2] != matrix4[*first1][*first2])
                    {
                        std::cout
                            << "Algorithms do not match at matrix point "
                            << index[*first1] << " " << index[*first2]
                            << " Bellman results: " << matrix2[*first1][*first2]
                            << " floyd 3 results " << matrix4[*first1][*first2]
                            << std::endl;
                        return false;
                    }
                }
            }
        }
    }
    return true;
}

template < typename Graph > bool acceptance_test2(Graph& g, int vec, int e)
{
    boost::minstd_rand ran(vec);

    {

        typename boost::property_map< Graph, boost::vertex_name_t >::type index
            = boost::get(boost::vertex_name, g);
        typename boost::graph_traits< Graph >::vertex_iterator firstv, lastv,
            firstv2, lastv2;
        int x = 0;
        for (boost::tie(firstv, lastv) = boost::vertices(g); firstv != lastv;
             firstv++)
        {
            boost::put(index, *firstv, x);
            x++;
        }

        boost::generate_random_graph(g, vec, e, ran, true);

        typename boost::graph_traits< Graph >::edge_iterator first, last;
        typename boost::property_map< Graph, boost::edge_weight_t >::type
            local_edge_map
            = boost::get(boost::edge_weight, g);
        for (boost::tie(first, last) = boost::edges(g); first != last; first++)
        {
            if (ran() % vec != 0)
            {
                boost::put(local_edge_map, *first, ran() % 100);
            }
            else
            {
                boost::put(local_edge_map, *first, 0 - (ran() % 100));
            }
        }

        int int_inf = std::numeric_limits< int >::max
        BOOST_PREVENT_MACRO_SUBSTITUTION();
        typedef
            typename boost::graph_traits< Graph >::vertex_descriptor vertex_des;
        std::map< vertex_des, int > matrixRow;
        std::map< vertex_des, std::map< vertex_des, int > > matrix;
        typedef typename boost::property_map< Graph,
            boost::vertex_distance_t >::type distance_type;
        distance_type distance_row = boost::get(boost::vertex_distance, g);
        for (boost::tie(firstv, lastv) = boost::vertices(g); firstv != lastv;
             firstv++)
        {
            boost::put(distance_row, *firstv, int_inf);
            matrixRow[*firstv] = int_inf;
        }
        for (boost::tie(firstv, lastv) = boost::vertices(g); firstv != lastv;
             firstv++)
        {
            matrix[*firstv] = matrixRow;
        }
        for (boost::tie(firstv, lastv) = boost::vertices(g); firstv != lastv;
             firstv++)
        {
            matrix[*firstv][*firstv] = 0;
        }
        std::map< vertex_des, std::map< vertex_des, int > > matrix3(matrix);
        std::map< vertex_des, std::map< vertex_des, int > > matrix4(matrix);
        for (boost::tie(first, last) = boost::edges(g); first != last; first++)
        {
            if (matrix[boost::source(*first, g)][boost::target(*first, g)]
                != int_inf)
            {
                matrix[boost::source(*first, g)][boost::target(*first, g)]
                    = my_min(boost::get(local_edge_map, *first),
                        matrix[boost::source(*first, g)]
                              [boost::target(*first, g)]);
            }
            else
            {
                matrix[boost::source(*first, g)][boost::target(*first, g)]
                    = boost::get(local_edge_map, *first);
            }
        }
        bool is_undirected = boost::is_same<
            typename boost::graph_traits< Graph >::directed_category,
            boost::undirected_tag >::value;
        if (is_undirected)
        {
            for (boost::tie(first, last) = boost::edges(g); first != last;
                 first++)
            {
                if (matrix[boost::target(*first, g)][boost::source(*first, g)]
                    != int_inf)
                {
                    matrix[boost::target(*first, g)][boost::source(*first, g)]
                        = my_min(boost::get(local_edge_map, *first),
                            matrix[boost::target(*first, g)]
                                  [boost::source(*first, g)]);
                }
                else
                {
                    matrix[boost::target(*first, g)][boost::source(*first, g)]
                        = boost::get(local_edge_map, *first);
                }
            }
        }

        bool bellman, floyd1, floyd2, floyd3;
        floyd1 = boost::floyd_warshall_initialized_all_pairs_shortest_paths(g,
            matrix,
            weight_map(boost::get(boost::edge_weight, g))
                .distance_inf(int_inf)
                .distance_zero(0));

        floyd2 = boost::floyd_warshall_all_pairs_shortest_paths(g, matrix3,
            weight_map(local_edge_map).distance_inf(int_inf).distance_zero(0));

        floyd3 = boost::floyd_warshall_all_pairs_shortest_paths(g, matrix4);

        boost::dummy_property_map dummy_map;
        std::map< vertex_des, std::map< vertex_des, int > > matrix2;
        for (boost::tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
        {
            boost::put(distance_row, *firstv, 0);
            bellman = boost::bellman_ford_shortest_paths(g, vec,
                weight_map(boost::get(boost::edge_weight, g))
                    .distance_map(boost::get(boost::vertex_distance, g))
                    .predecessor_map(dummy_map));
            distance_row = boost::get(boost::vertex_distance, g);
            for (boost::tie(firstv2, lastv2) = vertices(g); firstv2 != lastv2;
                 firstv2++)
            {
                matrix2[*firstv][*firstv2] = boost::get(distance_row, *firstv2);
                boost::put(distance_row, *firstv2, int_inf);
            }
            if (bellman == false)
            {
                break;
            }
        }

        if (bellman != floyd1 || bellman != floyd2 || bellman != floyd3)
        {
            std::cout << "A negative cycle was detected in one algorithm but "
                         "not the others. "
                      << std::endl;
            return false;
        }
        else if (bellman == false && floyd1 == false && floyd2 == false
            && floyd3 == false)
        {
            return true;
        }
        else
        {
            typename boost::graph_traits< Graph >::vertex_iterator first1,
                first2, last1, last2;
            for (boost::tie(first1, last1) = boost::vertices(g);
                 first1 != last1; first1++)
            {
                for (boost::tie(first2, last2) = boost::vertices(g);
                     first2 != last2; first2++)
                {
                    if (matrix2[*first1][*first2] != matrix[*first1][*first2])
                    {
                        std::cout
                            << "Algorithms do not match at matrix point "
                            << index[*first1] << " " << index[*first2]
                            << " Bellman results: " << matrix2[*first1][*first2]
                            << " floyd 1 results " << matrix[*first1][*first2]
                            << std::endl;
                        return false;
                    }
                    if (matrix2[*first1][*first2] != matrix3[*first1][*first2])
                    {
                        std::cout
                            << "Algorithms do not match at matrix point "
                            << index[*first1] << " " << index[*first2]
                            << " Bellman results: " << matrix2[*first1][*first2]
                            << " floyd 2 results " << matrix3[*first1][*first2]
                            << std::endl;
                        return false;
                    }
                    if (matrix2[*first1][*first2] != matrix4[*first1][*first2])
                    {
                        std::cout
                            << "Algorithms do not match at matrix point "
                            << index[*first1] << " " << index[*first2]
                            << " Bellman results: " << matrix2[*first1][*first2]
                            << " floyd 3 results " << matrix4[*first1][*first2]
                            << std::endl;
                        return false;
                    }
                }
            }
        }
    }
    return true;
}

int main(int, char*[])
{
    typedef boost::adjacency_list< boost::listS, boost::listS, boost::directedS,
        boost::property< boost::vertex_distance_t, int,
            boost::property< boost::vertex_name_t, int > >,
        boost::property< boost::edge_weight_t, int > >
        Digraph;
    Digraph adjlist_digraph;
    BOOST_TEST(acceptance_test2(adjlist_digraph, 100, 2000));

    typedef boost::adjacency_matrix< boost::undirectedS,
        boost::property< boost::vertex_distance_t, int,
            boost::property< boost::vertex_name_t, int > >,
        boost::property< boost::edge_weight_t, int > >
        Graph;
    Graph matrix_graph(100);
    BOOST_TEST(acceptance_test(matrix_graph, 100, 2000));

    return boost::report_errors();
}


/**
 * @file Mechanization_bRn.cpp
 * @date Jan 25, 2012
 * @author Chris Beall
 * @author Frank Dellaert
 */

#include "Mechanization_bRn2.h"
#include <boost/foreach.hpp>

namespace gtsam {

/* ************************************************************************* */
Mechanization_bRn2 Mechanization_bRn2::initializeVector(const std::list<Vector>& U,
    const std::list<Vector>& F, const double g_e, bool flat) {
  Matrix Umat = (Matrix(3, U.size()) << concatVectors(U));
  Matrix Fmat = (Matrix(3, F.size()) << concatVectors(F));

  return initialize(Umat, Fmat, g_e, flat);
}

/* ************************************************************************* */
Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U,
    const Matrix& F, const double g_e, bool flat) {

  // estimate gyro bias by averaging gyroscope readings (10.62)
  Vector x_g = U.rowwise().mean();
  Vector meanF = F.rowwise().mean();

  // estimate gravity
  Vector b_g;
  if(g_e == 0) {
    if (flat)
      // acceleration measured is  along the z-axis.
      b_g = (Vector(3) << 0.0, 0.0, norm_2(meanF));
    else
      // acceleration measured is the opposite of gravity (10.13)
      b_g = -meanF;
  } else {
    if (flat)
      // gravity is downward along the z-axis since we are flat on the ground
      b_g = (Vector(3) << 0.0,0.0,g_e);
    else
      // normalize b_g and attribute remainder to biases
      b_g = - g_e * meanF/meanF.norm();
  }

  // estimate accelerometer bias
  Vector x_a = meanF + b_g;

  // initialize roll, pitch (eqns. 10.14, 10.15)
  double g1=b_g(0);
  double g2=b_g(1);
  double g3=b_g(2);

  // Farrell08book eqn. 10.14
  double roll = atan2(g2, g3);
  // Farrell08book eqn. 10.15
  double pitch = atan2(-g1, sqrt(g2 * g2 + g3 * g3));
  double yaw = 0;
  // This returns body-to-nav nRb
  Rot3 bRn = Rot3::ypr(yaw, pitch, roll).inverse();

  return Mechanization_bRn2(bRn, x_g, x_a);
}

/* ************************************************************************* */
Mechanization_bRn2 Mechanization_bRn2::correct(const Vector& dx) const {
  Vector rho = sub(dx, 0, 3);

  Rot3 delta_nRn = Rot3::rodriguez(rho);
  Rot3 bRn = bRn_ * delta_nRn;

  Vector x_g = x_g_ + sub(dx, 3, 6);
  Vector x_a = x_a_ + sub(dx, 6, 9);

  return Mechanization_bRn2(bRn, x_g, x_a);
}

/* ************************************************************************* */
Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector& u,
    const double dt) const {
  // integrate rotation nRb based on gyro measurement u and bias x_g

#ifndef MODEL_NAV_FRAME_ROTATION
  // get body to inertial (measured in b) from gyro, subtract bias
  Vector b_omega_ib = u - x_g_;

  // nav to inertial due to Earth's rotation and our movement on Earth surface
  // TODO: figure out how to do this if we need it
  Vector b_omega_in = zero(3);

  // calculate angular velocity on bRn measured in body frame
  Vector b_omega_bn = b_omega_in - b_omega_ib;
#else
  // Assume a non-rotating nav frame (probably an approximation)
  // calculate angular velocity on bRn measured in body frame
  Vector b_omega_bn = x_g_ - u;
#endif

  // convert to navigation frame
  Rot3 nRb = bRn_.inverse();
  Vector n_omega_bn = (nRb*b_omega_bn).vector();

  // integrate bRn using exponential map, assuming constant over dt
  Rot3 delta_nRn = Rot3::rodriguez(n_omega_bn*dt);
  Rot3 bRn = bRn_.compose(delta_nRn);

  // implicit updating of biases (variables just do not change)
  // x_g = x_g;
  // x_a = x_a;
  return Mechanization_bRn2(bRn, x_g_, x_a_);
}

} /* namespace gtsam */


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Burden, R.L., Faires, J.D. Numerical Analysis, 7th Edition, Books/Cole, 2001.
 *      Montenbruck, O., Gill, E. Satellite Orbits: Models, Methods, Applications, Springer, 2005.
 *      The MathWorks, Inc. RKF54b, Symbolic Math Toolbox, 2012.
 *
 *    Notes
 *      For the tests using data from the Symbolic Math Toolbox (MathWorks, 2012), the single step
 *      and full integration error tolerances were picked to be as small as possible, without
 *      causing the tests to fail. These values are not deemed to indicate any bugs in the code;
 *      however, it is important to take these discrepancies into account when using this numerical
 *      integrator.
 *
 */

#define BOOST_TEST_MAIN

#include <limits>
#include <cmath>

#include <Eigen/Core>

#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/NumericalIntegrators/adamsBashforthMoultonIntegrator.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaVariableStepSizeIntegrator.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h"
#include "Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTestFunctions.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_adams_bashforth_moulton_integrator )

using numerical_integrator_test_functions::computeNonAutonomousModelStateDerivative;
using numerical_integrator_test_functions::computeVanDerPolStateDerivative;
using numerical_integrator_test_functions::computeFehlbergLogirithmicTestODEStateDerivative;
using numerical_integrator_test_functions::computeAnalyticalStateFehlbergODE;

using namespace numerical_integrators;

//! Test Compare with Runge Kutta 78
BOOST_AUTO_TEST_CASE( test_AdamsBashforthMoulton_Integrator_Compare78 )
{
    // Setup integrator
    RungeKuttaCoefficients coeff_rk78 =
            RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78);

    // Integrator settings
    double minimumStepSize = std::numeric_limits< double >::epsilon( );
    double maximumStepSize = std::numeric_limits< double >::infinity( );
    double initialStepSize = 1E-4; // Don't make this too small
    Eigen::VectorXd relativeTolerance(1);
    relativeTolerance << 1E-12;
    Eigen::VectorXd absoluteTolerance = relativeTolerance;

    // Initial conditions
    double initialTime = 0.5;
    Eigen::VectorXd initialState( 1 );
    initialState << 0.5; // 1 large error

    // Setup integrator
    AdamsBashforthMoultonIntegratorXd integrator_abam(
                computeNonAutonomousModelStateDerivative, initialTime, initialState,
                minimumStepSize, maximumStepSize, relativeTolerance, absoluteTolerance );

    RungeKuttaVariableStepSizeIntegratorXd integrator_rk78(
                coeff_rk78, computeNonAutonomousModelStateDerivative, initialTime, initialState,
                minimumStepSize, maximumStepSize, relativeTolerance, absoluteTolerance );

    double endTime = 1.5;
    Eigen::VectorXd solution_abam = integrator_abam.integrateTo( endTime, initialStepSize );
    Eigen::VectorXd solution_rk78 = integrator_rk78.integrateTo( endTime, initialStepSize );

    Eigen::VectorXd difference = solution_abam - solution_rk78;

    BOOST_CHECK_SMALL( std::fabs( difference( 0 ) ), 1E-11 );
}

//! Test Compare with Runge Kutta 78
BOOST_AUTO_TEST_CASE( test_AdamsBashforthMoulton_Integrator_Compare78_v2 )
{
    // Setup integrator
    RungeKuttaCoefficients coeff_rk78 =
            RungeKuttaCoefficients::get(
                RungeKuttaCoefficients::rungeKuttaFehlberg78 );

    // Integrator settings
    double minimumStepSize = std::numeric_limits< double >::epsilon( );
    double maximumStepSize = std::numeric_limits< double >::infinity( );
    double initialStepSize = 1.0; // Don't make this too small
    double relativeTolerance = 1E-10;
    double absoluteTolerance = 1E-10;

    // Initial conditions
    double initialTime = 0.2;
    Eigen::VectorXd initialState( 1 );
    initialState << -1.0;

    // Setup integrator
    AdamsBashforthMoultonIntegratorXd integrator_abam(
                computeNonAutonomousModelStateDerivative, initialTime, initialState,
                minimumStepSize, maximumStepSize, relativeTolerance, absoluteTolerance );

    RungeKuttaVariableStepSizeIntegratorXd integrator_rk78(
                coeff_rk78, computeNonAutonomousModelStateDerivative, initialTime, initialState,
                minimumStepSize, maximumStepSize, relativeTolerance, absoluteTolerance );

    double endTime = 2.0;
    Eigen::VectorXd solution_abam = integrator_abam.integrateTo( endTime, initialStepSize );
    Eigen::VectorXd solution_rk78 = integrator_rk78.integrateTo( endTime, initialStepSize );

    Eigen::VectorXd difference = solution_abam - solution_rk78;

    BOOST_CHECK_SMALL( std::fabs( difference( 0 ) ), 1E-8 );
}

//! Test Compare with Runge Kutta 78
BOOST_AUTO_TEST_CASE( test_AdamsBashforthMoulton_Integrator_Compare78_VanDerPol )
{
    // Setup integrator
    RungeKuttaCoefficients coeff_rk78 =
            RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78 );

    // Integrator settings
    double minimumStepSize = 0.001;
    double maximumStepSize = std::numeric_limits< double >::infinity( );
    double initialStepSize = 1; // Don't make this too small
    double relativeTolerance = 1E-15;
    double absoluteTolerance = 1E-15;

    // Initial conditions
    double initialTime = 0.2;
    Eigen::VectorXd initialState( 2 );
    initialState << -1.0, 1.0;

    // Setup integrator
    AdamsBashforthMoultonIntegratorXd integrator_abam(
                computeVanDerPolStateDerivative, initialTime, initialState,
                minimumStepSize, maximumStepSize, relativeTolerance, absoluteTolerance );

    RungeKuttaVariableStepSizeIntegratorXd integrator_rk78(
                coeff_rk78, computeVanDerPolStateDerivative, initialTime, initialState,
                minimumStepSize, maximumStepSize, relativeTolerance, absoluteTolerance );

    double endTime = 1.4;
    Eigen::VectorXd solution_abam = integrator_abam.integrateTo(endTime,initialStepSize);
    Eigen::VectorXd solution_rk78 = integrator_rk78.integrateTo(endTime,initialStepSize);

    Eigen::VectorXd difference = solution_rk78 - solution_abam;

    BOOST_CHECK_SMALL( std::fabs( difference( 0 ) ), 5E-12 );
    BOOST_CHECK_SMALL( std::fabs( difference( 1 ) ), 5E-12 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


//          Copyright Rein Halbersma 2010-2020.
// 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 <dctl/core/board/angle.hpp>    // angle, _deg, is_orthogonal, is_diagonal, is_up, is_down, is_left, is_right, is_positive, is_negative
#include <boost/test/unit_test.hpp>     // BOOST_AUTO_TEST_SUITE, BOOST_AUTO_TEST_SUITE_END, BOOST_AUTO_TEST_CASE, BOOST_CHECK
#include <algorithm>                    // all_of
#include <array>                        // array

using namespace dctl::core;
using namespace literals;

BOOST_AUTO_TEST_SUITE(AngleTraits)

BOOST_AUTO_TEST_CASE(IsUp)
{
        auto const angles = std::array{ 45_deg, 90_deg, 135_deg };
        BOOST_CHECK(
                std::all_of(angles.begin(), angles.end(), [](auto a) {
                        return is_up(a);
                })
        );
}

BOOST_AUTO_TEST_SUITE_END()


#include <Eigen/Core>
#include <ct/types/eigen.hpp>

namespace aq
{
    template <uint8_t D>
    struct Shape : Eigen::Array<uint32_t, D, 1>
    {
        using Super = Eigen::Array<uint32_t, D, 1>;

        Shape()
            : Super()
        {
        }

        template <class... ARGS>
        Shape(ARGS&&... args)
            : Super(std::forward<ARGS>(args)...)
        {
        }

        template <class OtherDerived>
        Shape(const Eigen::ArrayBase<OtherDerived>& other)
            : Super(other)
        {
        }

        template <class OtherDerived>
        Shape<D>& operator=(const Eigen::ArrayBase<OtherDerived>& other)
        {
            this->Super::operator=(other);
            return *this;
        }

        size_t numel() const
        {
            size_t out = 1;
            for (uint8_t i = 0; i < D; ++i)
            {
                out *= (*this)[i];
            }
            return out;
        }
    };
}

namespace ct
{
    // clang-format off
    template <uint8_t D>
    struct ReflectImpl<aq::Shape<D>>
    {
        static constexpr int SPECIALIZED = true;
        using DataType = aq::Shape<D>;
        using BaseTypes = ct::VariadicTypedef<Eigen::Array<uint32_t, D, 1>>;
        static constexpr StringView getName()
        {
            return GetName<DataType>::getName();
        }

        REFLECT_STUB
            // PROPERTY(data, &Reflect<DataType>::getData, &Reflect<DataType>::getDataMutable)
            // PROPERTY_WITH_FLAG(COMPILE_TIME_CONSTANT, shape, &Reflect<DataType>::shape)
            PROPERTY_WITH_FLAG(Flags::COMPILE_TIME_CONSTANT, size)
            PROPERTY_WITH_FLAG(Flags::COMPILE_TIME_CONSTANT, cols)
            PROPERTY_WITH_FLAG(Flags::COMPILE_TIME_CONSTANT, rows)
        REFLECT_INTERNAL_END;

        static constexpr auto end()
        {
            return ct::Indexer<NUM_FIELDS - 1>();
        }
    };
// clang-format on
}


/*    Copyright (c) 2010-2016, Delft University of Technology
 *    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 <string>

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"

#include "Tudat/External/SpiceInterface/spiceInterface.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h"
#include "Tudat/Mathematics/Interpolators/lagrangeInterpolator.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/keplerPropagator.h"
#include "Tudat/InputOutput/basicInputOutput.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/SimulationSetup/body.h"
#include "Tudat/Astrodynamics/Propagators/nBodyCowellStateDerivative.h"
#include "Tudat/Astrodynamics/Propagators/dynamicsSimulator.h"
#include "Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h"
#include "Tudat/SimulationSetup/createBodies.h"
#include "Tudat/SimulationSetup/createAccelerationModels.h"
#include "Tudat/SimulationSetup/defaultBodies.h"

namespace tudat
{

namespace unit_tests
{

//Using declarations.
using namespace tudat::ephemerides;
using namespace tudat::interpolators;
using namespace tudat::numerical_integrators;
using namespace tudat::spice_interface;
using namespace tudat::simulation_setup;
using namespace tudat::basic_astrodynamics;
using namespace tudat::orbital_element_conversions;
using namespace tudat::propagators;

BOOST_AUTO_TEST_SUITE( test_cowell_propagator )

//! Test to check whether the frame origin transformations are handled properly.
BOOST_AUTO_TEST_CASE( testCowellPopagatorCentralBodies )
{
    //Load spice kernels.
    std::string kernelsPath = input_output::getSpiceKernelPath( );
    spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc");
    spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp");
    spice_interface::loadSpiceKernelInTudat( kernelsPath + "naif0009.tls");
    spice_interface::loadSpiceKernelInTudat( kernelsPath + "pck00009.tpc");

    // Define bodies in simulation.
    unsigned int totalNumberOfBodies = 4;
    std::vector< std::string > bodyNames;
    bodyNames.resize( totalNumberOfBodies );
    bodyNames[ 0 ] = "Earth";
    bodyNames[ 1 ] = "Sun";
    bodyNames[ 2 ] = "Moon";
    bodyNames[ 3 ] = "Mars";

    // Specify initial time
    double initialEphemerisTime = 1.0E7;
    double finalEphemerisTime = 2.0E7;
    double maximumTimeStep = 3600.0;
    double buffer = 5.0 * maximumTimeStep;

    // Create bodies needed in simulation
    std::map< std::string, boost::shared_ptr< BodySettings > > bodySettings =
            getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, finalEphemerisTime + buffer );
    bodySettings[ "Mars" ]->ephemerisSettings->resetFrameOrigin( "Earth" );
    bodySettings[ "Earth" ]->ephemerisSettings->resetFrameOrigin( "Sun" );
    bodySettings[ "Moon" ]->ephemerisSettings->resetFrameOrigin( "Earth" );

    NamedBodyMap bodyMap = createBodies( bodySettings );

    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    // Set accelerations between bodies that are to be taken into account (mutual point mass gravity between all bodies).
    SelectedAccelerationMap accelerationMap;
    std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfEarth;
    accelerationsOfEarth[ "Sun" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationsOfEarth[ "Moon" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationsOfEarth[ "Mars" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationMap[ "Earth" ] = accelerationsOfEarth;

    std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfSun;
    accelerationsOfSun[ "Moon" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationsOfSun[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationsOfSun[ "Mars" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationMap[ "Sun" ] = accelerationsOfSun;

    std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfMoon;
    accelerationsOfMoon[ "Sun" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationsOfMoon[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationsOfMoon[ "Mars" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationMap[ "Moon" ] = accelerationsOfMoon;

    std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfMars;
    accelerationsOfMars[ "Sun" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationsOfMars[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationsOfMars[ "Moon" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationMap[ "Mars" ] = accelerationsOfMars;

    // Define list of bodies to propagate
    std::vector< std::string > bodiesToIntegrate;
    bodiesToIntegrate.push_back( "Earth" );
    bodiesToIntegrate.push_back( "Sun" );
    bodiesToIntegrate.push_back( "Moon" );
    bodiesToIntegrate.push_back( "Mars" );
    unsigned int numberOfNumericalBodies = bodiesToIntegrate.size( );

    // Define numerical integrator settings.
    boost::shared_ptr< IntegratorSettings< > > integratorSettings =
            boost::make_shared< IntegratorSettings< > >
            ( rungeKutta4, initialEphemerisTime, 200.0 );

    // Define central bodies to use in propagation (all w.r.t SSB).
    std::vector< std::string > centralBodies;
    std::map< std::string, std::string > centralBodyMap;
    centralBodies.resize( numberOfNumericalBodies );
    for( unsigned int i = 0; i < numberOfNumericalBodies; i++ )
    {
        centralBodies[ i ] = "SSB";
        centralBodyMap[ bodiesToIntegrate[ i ] ] = centralBodies[ i ];
    }

    // Get initial state vector as input to integration.
    Eigen::VectorXd systemInitialState = getInitialStatesOfBodies(
                bodiesToIntegrate, centralBodies, bodyMap, initialEphemerisTime );

    // Create acceleration models and propagation settings.
    AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                bodyMap, accelerationMap, centralBodyMap );
    boost::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
            boost::make_shared< TranslationalStatePropagatorSettings< double > >
            ( centralBodies, accelerationModelMap, bodiesToIntegrate, systemInitialState, finalEphemerisTime );

    // Create simulation object and propagate dynamics.
    SingleArcDynamicsSimulator< > dynamicsSimulator(
                bodyMap, integratorSettings, propagatorSettings, true, false, false );
    std::map< double, Eigen::VectorXd > solutionSet1 = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );

    // Define new central bodies (hierarchical system)
    centralBodies[ 0 ] = "Sun";
    centralBodies[ 1 ] = "SSB";
    centralBodies[ 2 ] = "Earth";
    centralBodies[ 3 ] = "Sun";
    for( unsigned int i = 0; i < numberOfNumericalBodies; i++ )
    {
        centralBodyMap[ bodiesToIntegrate[ i ] ] = centralBodies[ i ];
    }

    systemInitialState = getInitialStatesOfBodies(
                bodiesToIntegrate, centralBodies, bodyMap, initialEphemerisTime );

    // Create new acceleration models and propagation settings.
    AccelerationMap accelerationModelMap2 = createAccelerationModelsMap(
                bodyMap, accelerationMap, centralBodyMap );
    boost::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings2 =
            boost::make_shared< TranslationalStatePropagatorSettings< double > >
            ( centralBodies, accelerationModelMap2, bodiesToIntegrate, systemInitialState, finalEphemerisTime );

    // Create new simulation object and propagate dynamics.
    SingleArcDynamicsSimulator< > dynamicsSimulator2(
                bodyMap, integratorSettings, propagatorSettings2, true, false );
    std::map< double, Eigen::VectorXd > solutionSet2 = dynamicsSimulator2.getEquationsOfMotionNumericalSolution( );

    // Create integration and propagation settings for reverse in time propagation
    std::map< double, Eigen::VectorXd >::iterator solutionSetIterator = (--solutionSet2.end( ) );
    Eigen::VectorXd systemFinalState = solutionSetIterator->second;
    boost::shared_ptr< IntegratorSettings< > > integratorSettings2 =
            boost::make_shared< IntegratorSettings< > >
            ( rungeKutta4, solutionSetIterator->first, -200.0 );
    boost::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings3 =
            boost::make_shared< TranslationalStatePropagatorSettings< double > >
            ( centralBodies, accelerationModelMap2, bodiesToIntegrate, systemFinalState, initialEphemerisTime );

    // Create new simulation object and propagate dynamics backwards in time.
    SingleArcDynamicsSimulator< > dynamicsSimulator3(
               bodyMap, integratorSettings2, propagatorSettings3, true, false, false );
    std::map< double, Eigen::VectorXd > solutionSet3 = dynamicsSimulator3.getEquationsOfMotionNumericalSolution( );

    // Create interpolators from three numerical solutions (first one is inertial; second and third are non-inertial)
    boost::shared_ptr< LagrangeInterpolator< double, Eigen::VectorXd > > interpolator1 =
            boost::make_shared< LagrangeInterpolator< double, Eigen::VectorXd > >( solutionSet1, 8 );
    boost::shared_ptr< LagrangeInterpolator< double, Eigen::VectorXd > > interpolator2 =
            boost::make_shared< LagrangeInterpolator< double, Eigen::VectorXd > >( solutionSet2, 8 );
    boost::shared_ptr< LagrangeInterpolator< double, Eigen::VectorXd > > interpolator3 =
            boost::make_shared< LagrangeInterpolator< double, Eigen::VectorXd > >( solutionSet3, 8 );

    // Define step size to be out of sync with integration step size.
    double stepSize = 2001.1 + mathematical_constants::PI;
    double currentTime = initialEphemerisTime + stepSize;

    std::map< double, Eigen::VectorXd > analyticalSolutions;

    // Define maps to retrieve propagated orbits from interpolator.
    Eigen::VectorXd currentInertialSolution = Eigen::VectorXd::Zero( 6 * numberOfNumericalBodies );
    Eigen::VectorXd currentNonInertialSolution = Eigen::VectorXd::Zero( 6 * numberOfNumericalBodies );

    // Define map to put inertial orbit reconstructed from non-inertial orbits.
    Eigen::VectorXd reconstructedInertialSolution = Eigen::VectorXd::Zero( 6 * numberOfNumericalBodies );

    // Define error maps.
    Eigen::VectorXd stateDifference = Eigen::VectorXd::Zero( 6 * numberOfNumericalBodies );

    // Test numerical output against results with SSB as origin for ech body,
    boost::shared_ptr< ephemerides::Ephemeris > sunEphemeris = bodyMap[ "Sun" ]->getEphemeris( );
    boost::shared_ptr< ephemerides::Ephemeris > earthEphemeris = bodyMap[ "Earth" ]->getEphemeris( );
    boost::shared_ptr< ephemerides::Ephemeris > marsEphemeris = bodyMap[ "Mars" ]->getEphemeris( );
    boost::shared_ptr< ephemerides::Ephemeris > moonEphemeris = bodyMap[ "Moon" ]->getEphemeris( );

    boost::shared_ptr< LagrangeInterpolator< double, Eigen::VectorXd > > currentInterpolator;

    while( currentTime < finalEphemerisTime - stepSize )
    {
        // Retrieve data from interpolators; transform to inertial frames and compare.
        currentInertialSolution = interpolator1->interpolate( currentTime );

        for( unsigned int k = 0; k < 2; k++ )
        {
            if( k == 0 )
            {
                currentInterpolator = interpolator2;
            }
            else
            {
                currentInterpolator = interpolator3;
            }

            currentNonInertialSolution = currentInterpolator->interpolate( currentTime );

            reconstructedInertialSolution.segment( 0, 6 ) = currentNonInertialSolution.segment( 0, 6 ) +
                    currentNonInertialSolution.segment( 6, 6 );
            reconstructedInertialSolution.segment( 6, 6 ) = currentNonInertialSolution.segment( 6, 6 );
            reconstructedInertialSolution.segment( 12, 6 )= currentNonInertialSolution.segment( 12, 6 ) +
                    reconstructedInertialSolution.segment( 0, 6 );
            reconstructedInertialSolution.segment( 18, 6 ) = currentNonInertialSolution.segment( 18, 6 ) +
                    reconstructedInertialSolution.segment( 6, 6 );

            // Compare states.
            stateDifference = reconstructedInertialSolution - currentInertialSolution;

            for( unsigned j = 0; j < 4; j++ )
            {
                if( j != 2 )
                {
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( 0 + 6 * j ) ), 1.0E-2 );
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( 1 + 6 * j ) ), 1.0E-2 );
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( 2 + 6 * j ) ), 1.0E-2 );

                    BOOST_CHECK_SMALL( std::fabs( stateDifference( 3 + 6 * j ) ), 5.0E-9 );
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( 4 + 6 * j ) ), 5.0E-9 );
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( 5 + 6 * j ) ), 5.0E-9 );
                }
                else
                {
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( 0 + 6 * j ) ), 1.0E-1 );
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( 1 + 6 * j ) ), 1.0E-1 );
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( 2 + 6 * j ) ), 1.0E-1 );

                    BOOST_CHECK_SMALL( std::fabs( stateDifference( 3 + 6 * j ) ), 2.0E-7 );
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( 4 + 6 * j ) ), 2.0E-7 );
                    BOOST_CHECK_SMALL( std::fabs( stateDifference( 5 + 6 * j ) ), 2.0E-7 );
                }
            }
        }

        // Test whether ephemeris objects have been properly reset, i.e. whether all states have been properly transformed to the
        // ephemeris frame.

        // Retrieve data from interpolators; transform to inertial frames and compare.
        currentInertialSolution = interpolator1->interpolate( currentTime );

        reconstructedInertialSolution.segment( 0, 6 ) = earthEphemeris->getCartesianStateFromEphemeris( currentTime ) +
                sunEphemeris->getCartesianStateFromEphemeris( currentTime );
        reconstructedInertialSolution.segment( 6, 6 ) = sunEphemeris->getCartesianStateFromEphemeris( currentTime );
        reconstructedInertialSolution.segment( 12, 6 ) =
                moonEphemeris->getCartesianStateFromEphemeris( currentTime ) +
                earthEphemeris->getCartesianStateFromEphemeris( currentTime ) +
                sunEphemeris->getCartesianStateFromEphemeris( currentTime );
        reconstructedInertialSolution.segment( 18, 6 ) =
                marsEphemeris->getCartesianStateFromEphemeris( currentTime ) +
                earthEphemeris->getCartesianStateFromEphemeris( currentTime ) +
                sunEphemeris->getCartesianStateFromEphemeris( currentTime );

        // Compare states.
        stateDifference = reconstructedInertialSolution - currentInertialSolution;
        for( unsigned j = 0; j < 4; j++ )
        {
            if( j != 2 )
            {
                BOOST_CHECK_SMALL( std::fabs( stateDifference( 0 + 6 * j ) ), 1.0E-2 );
                BOOST_CHECK_SMALL( std::fabs( stateDifference( 1 + 6 * j ) ), 1.0E-2 );
                BOOST_CHECK_SMALL( std::fabs( stateDifference( 2 + 6 * j ) ), 1.0E-2 );

                BOOST_CHECK_SMALL( std::fabs( stateDifference( 3 + 6 * j ) ), 5.0E-9 );
                BOOST_CHECK_SMALL( std::fabs( stateDifference( 4 + 6 * j ) ), 5.0E-9 );
                BOOST_CHECK_SMALL( std::fabs( stateDifference( 5 + 6 * j ) ), 5.0E-9 );
            }
            else
            {
                BOOST_CHECK_SMALL( std::fabs( stateDifference( 0 + 6 * j ) ), 1.0E-1 );
                BOOST_CHECK_SMALL( std::fabs( stateDifference( 1 + 6 * j ) ), 1.0E-1 );
                BOOST_CHECK_SMALL( std::fabs( stateDifference( 2 + 6 * j ) ), 1.0E-1 );

                BOOST_CHECK_SMALL( std::fabs( stateDifference( 3 + 6 * j ) ), 2.0E-7 );
                BOOST_CHECK_SMALL( std::fabs( stateDifference( 4 + 6 * j ) ), 2.0E-7 );
                BOOST_CHECK_SMALL( std::fabs( stateDifference( 5 + 6 * j ) ), 2.0E-7 );
            }
        }

        currentTime += stepSize;
    }

}

//! Test to ensure that a point-mass acceleration on a body produces a Kepler orbit (to within numerical error bounds).
BOOST_AUTO_TEST_CASE( testCowellPopagatorKeplerCompare )
{
    //Load spice kernels.
    std::string kernelsPath = input_output::getSpiceKernelPath( );

    spice_interface::loadSpiceKernelInTudat( kernelsPath + "de-403-masses.tpc");
    spice_interface::loadSpiceKernelInTudat( kernelsPath + "de421.bsp");
    spice_interface::loadSpiceKernelInTudat( kernelsPath + "naif0009.tls");
    spice_interface::loadSpiceKernelInTudat( kernelsPath + "pck00009.tpc");

    // Define bodies in simulation.
    std::vector< std::string > bodyNames;
    bodyNames.push_back( "Earth" );
    bodyNames.push_back( "Moon" );

    // Specify initial time
    double initialEphemerisTime = 1.0E7;
    double finalEphemerisTime = 2.0E7;
    double maximumTimeStep = 3600.0;
    double buffer = 5.0 * maximumTimeStep;

    // Create bodies needed in simulation
    std::map< std::string, boost::shared_ptr< BodySettings > > bodySettings =
            getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, finalEphemerisTime + buffer );

    // Change ephemeris settings of Moon and Earth to make test results analysis more transparent.
    boost::dynamic_pointer_cast< InterpolatedSpiceEphemerisSettings >( bodySettings[ "Moon" ]->ephemerisSettings )->
            resetFrameOrigin( "Earth" );
    bodySettings[ "Earth" ]->ephemerisSettings = boost::make_shared< ConstantEphemerisSettings >(
                basic_mathematics::Vector6d::Zero( ), "SSB", "ECLIPJ2000" );

    NamedBodyMap bodyMap = createBodies( bodySettings );
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    // Set accelerations between bodies that are to be taken into account.
    SelectedAccelerationMap accelerationMap;
    std::map< std::string, std::vector< boost::shared_ptr< AccelerationSettings > > > accelerationsOfMoon;
    accelerationsOfMoon[ "Earth" ].push_back( boost::make_shared< AccelerationSettings >( central_gravity ) );
    accelerationMap[ "Moon" ] = accelerationsOfMoon;

    // Propagate the moon only
    std::vector< std::string > bodiesToIntegrate;
    bodiesToIntegrate.push_back( "Moon" );
    unsigned int numberOfNumericalBodies = bodiesToIntegrate.size( );

    // Define settings for numerical integrator.
    boost::shared_ptr< IntegratorSettings< > > integratorSettings =
            boost::make_shared< IntegratorSettings< > >
            ( rungeKutta4, initialEphemerisTime, 120.0 );

    // Run test where Moon gravity is/is not taken into account.
    for( unsigned testCase = 0; testCase < 2; testCase++ )
    {
        // Get initial kepler elements
        double effectiveGravitationalParameter;
        if( testCase == 0 )
        {
            effectiveGravitationalParameter =
                    bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ) +
                    bodyMap.at( "Moon" )->getGravityFieldModel( )->getGravitationalParameter( );
        }
        else
        {
            effectiveGravitationalParameter =
                    bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
        }

        // Define central bodies for integration.
        std::vector< std::string > centralBodies;
        std::map< std::string, std::string > centralBodyMap;

        if( testCase == 0 )
        {
            effectiveGravitationalParameter =
                    bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ) +
                    bodyMap.at( "Moon" )->getGravityFieldModel( )->getGravitationalParameter( );
            centralBodies.push_back( "Earth" );

        }
        else
        {
            effectiveGravitationalParameter =
                    bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
            centralBodies.push_back( "SSB" );
        }
        centralBodyMap[ bodiesToIntegrate[ 0 ] ] = centralBodies[ 0 ];

        // Create system initial state.
        Eigen::VectorXd systemInitialState = Eigen::VectorXd( bodiesToIntegrate.size( ) * 6 );
        for( unsigned int i = 0; i < numberOfNumericalBodies ; i++ )
        {
            systemInitialState.segment( i * 6 , 6 ) =
                    spice_interface::getBodyCartesianStateAtEpoch(
                        bodiesToIntegrate[ i ], "Earth", "ECLIPJ2000", "NONE", initialEphemerisTime );
        }

        // Create acceleration models and propagation settings.
        AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                    bodyMap, accelerationMap, centralBodyMap );
        boost::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
                boost::make_shared< TranslationalStatePropagatorSettings< double > >
                ( centralBodies, accelerationModelMap, bodiesToIntegrate, systemInitialState, finalEphemerisTime );

        // Create dynamics simulation object.
        SingleArcDynamicsSimulator< > dynamicsSimulator(
                    bodyMap, integratorSettings, propagatorSettings, true, false );

        basic_mathematics::Vector6d initialKeplerElements = orbital_element_conversions::convertCartesianToKeplerianElements(
                    basic_mathematics::Vector6d( systemInitialState ), effectiveGravitationalParameter );

        // Compare numerical state and kepler orbit at each time step.
        boost::shared_ptr< Ephemeris > moonEphemeris = bodyMap.at( "Moon" )->getEphemeris( );
        double currentTime = initialEphemerisTime + buffer;
        while( currentTime < finalEphemerisTime - buffer )
        {
            basic_mathematics::Vector6d stateDifference = orbital_element_conversions::convertKeplerianToCartesianElements(
                        propagateKeplerOrbit( initialKeplerElements, currentTime - initialEphemerisTime,
                                              effectiveGravitationalParameter ),
                        effectiveGravitationalParameter ) - moonEphemeris->getCartesianStateFromEphemeris( currentTime );
            for( int i = 0; i < 3; i++ )
            {
                BOOST_CHECK_SMALL( stateDifference( i ), 1E-3 );
                BOOST_CHECK_SMALL( stateDifference( i  + 3 ), 1.0E-9 );

            }
            currentTime += 10000.0;
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


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

#include <boost/numeric/mtl/mtl.hpp>
# include "boost/rational.hpp"
# include "boost/range.hpp"

typedef boost::rational<long>  t_Q;
typedef mtl::dense2D<t_Q> t_dMatQ;

int main(int, char**)
{
    //! Test Matrix-Matrix operators
    t_dMatQ mQ1(3,3); mtl::mat::diagonal_setup(mQ1,2);
    t_dMatQ mQ2(3,3);
    t_dMatQ mQ3(3,2);
    t_dMatQ mQ4(2,2);
    t_dMatQ mQ5;

    std::cout << "mQ1:\n" << mQ1 << "\n";
    std::cout << "size(mQ1):\n" << mtl::mat::size(mQ1) << "\n";

    mQ2 *= mQ1;
    mQ2 += mQ1;
    mQ4 = trans(mQ3)*mQ3;

    mQ5 = t_dMatQ( trans(mQ3)*mQ3 );
    mQ5 = t_dMatQ( mQ4+mQ4 );

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_DIVS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_DIVS_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/divs.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/mone.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divs_, tag::cpu_ , (A0)
                            , (scalar_< int_<A0> >)
                              (scalar_< int_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      if ((a0 == Valmin<result_type>()) && (a1 == Mone<result_type>()))
        return Valmax<result_type>();
      else if (a1) return a0/a1;
      else if (a0 > 0) return Valmax<result_type>();
      else if (a0 < 0) return Valmin<result_type>();
      else return Zero<result_type>();
    }
  };
} } }

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divs_, tag::cpu_ , (A0)
                            , (scalar_< uint_<A0> >)
                              (scalar_< uint_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      if (a1) return a0/a1;
      else if (a0 > 0) return Valmax<result_type>();
      else return Zero<result_type>();
    }
  };
} } }

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4723) // potential divide by 0
#endif

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divs_, tag::cpu_, (A0)
                            , (scalar_< floating_<A0> >)
                        (scalar_< floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return a0/a1; }
  };
} } }

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Olga Diamanti <olga.diam@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include <igl/conjugate_frame_fields.h>
#include <igl/speye.h>
#include <igl/slice.h>
#include <igl/polyroots.h>
#include <Eigen/Sparse>

#include <iostream>

namespace igl {
  template <typename DerivedV, typename DerivedF, typename DerivedO>
  class ConjugateFFSolver
  {
  public:
    IGL_INLINE ConjugateFFSolver(const ConjugateFFSolverData<DerivedV, DerivedF> &_data,
                                 int _maxIter = 50,
                                 const typename DerivedV::Scalar &_lambdaOrtho = .1,
                                 const typename DerivedV::Scalar &_lambdaInit = 100,
                                 const typename DerivedV::Scalar &_lambdaMultFactor = 1.01,
                                 bool _doHardConstraints = true);
    IGL_INLINE bool solve(const Eigen::VectorXi &isConstrained,
                          const Eigen::PlainObjectBase<DerivedO> &initialSolution,
                          Eigen::PlainObjectBase<DerivedO> &output,
                          typename DerivedV::Scalar *lambdaOut = NULL);

  private:

    const ConjugateFFSolverData<DerivedV, DerivedF> &data;

    //polyVF data
    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> Acoeff, Bcoeff;
    Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 2> pvU, pvV;
    typename DerivedV::Scalar lambda;

    //parameters
    typename DerivedV::Scalar lambdaOrtho;
    typename DerivedV::Scalar lambdaInit,lambdaMultFactor;
    int maxIter;
    bool doHardConstraints;

    IGL_INLINE void localStep();
    IGL_INLINE void getPolyCoeffsForLocalSolve(const Eigen::Matrix<typename DerivedV::Scalar, 4, 1> &s,
                                               const Eigen::Matrix<typename DerivedV::Scalar, 4, 1> &z,
                                               Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> &polyCoeff);

    IGL_INLINE void globalStep(const Eigen::Matrix<int, Eigen::Dynamic, 1>  &isConstrained,
                               const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1>  &Ak,
                               const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1>  &Bk);
    IGL_INLINE void minQuadWithKnownMini(const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &Q,
                         const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &f,
                         const Eigen::VectorXi isConstrained,
                         const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &xknown,
                                         Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &x);
    IGL_INLINE void setFieldFromCoefficients();
    IGL_INLINE void setCoefficientsFromField();

  };
}

//Implementation
/***************************** Solver ***********************************/
template <typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE igl::ConjugateFFSolver<DerivedV, DerivedF, DerivedO>::
ConjugateFFSolver(const ConjugateFFSolverData<DerivedV, DerivedF> &_data,
                  int _maxIter,
                  const typename DerivedV::Scalar &_lambdaOrtho,
                  const typename DerivedV::Scalar &_lambdaInit,
                  const typename DerivedV::Scalar &_lambdaMultFactor,
                  bool _doHardConstraints):
data(_data),
lambdaOrtho(_lambdaOrtho),
lambdaInit(_lambdaInit),
maxIter(_maxIter),
lambdaMultFactor(_lambdaMultFactor),
doHardConstraints(_doHardConstraints)
{
  Acoeff.resize(data.numF,1);
  Bcoeff.resize(data.numF,1);
  pvU.setZero(data.numF, 2);
  pvV.setZero(data.numF, 2);
};

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::ConjugateFFSolver<DerivedV, DerivedF, DerivedO>::
getPolyCoeffsForLocalSolve(const Eigen::Matrix<typename DerivedV::Scalar, 4, 1> &s,
                           const Eigen::Matrix<typename DerivedV::Scalar, 4, 1> &z,
                           Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> &polyCoeff)
{
  typename DerivedV::Scalar s0 = s(0);
  typename DerivedV::Scalar s1 = s(1);
  typename DerivedV::Scalar s2 = s(2);
  typename DerivedV::Scalar s3 = s(3);
  typename DerivedV::Scalar z0 = z(0);
  typename DerivedV::Scalar z1 = z(1);
  typename DerivedV::Scalar z2 = z(2);
  typename DerivedV::Scalar z3 = z(3);

  polyCoeff.resize(7,1);
  polyCoeff(0) =  s0*s0* s1*s1* s2*s2* s3* z3*z3 +  s0*s0* s1*s1* s2* s3*s3* z2*z2 +  s0*s0* s1* s2*s2* s3*s3* z1*z1 +  s0* s1*s1* s2*s2* s3*s3* z0*z0 ;
  polyCoeff(1) = 2* s0*s0* s1*s1* s2* s3* z2*z2 + 2* s0*s0* s1*s1* s2* s3* z3*z3 + 2* s0*s0* s1* s2*s2* s3* z1*z1 + 2* s0*s0* s1* s2*s2* s3* z3*z3 + 2* s0*s0* s1* s2* s3*s3* z1*z1 + 2* s0*s0* s1* s2* s3*s3* z2*z2 + 2* s0* s1*s1* s2*s2* s3* z0*z0 + 2* s0* s1*s1* s2*s2* s3* z3*z3 + 2* s0* s1*s1* s2* s3*s3* z0*z0 + 2* s0* s1*s1* s2* s3*s3* z2*z2 + 2* s0* s1* s2*s2* s3*s3* z0*z0 + 2* s0* s1* s2*s2* s3*s3* z1*z1 ;
  polyCoeff(2) =  s0*s0* s1*s1* s2* z2*z2 +  s0*s0* s1*s1* s3* z3*z3 +  s0*s0* s1* s2*s2* z1*z1 + 4* s0*s0* s1* s2* s3* z1*z1 + 4* s0*s0* s1* s2* s3* z2*z2 + 4* s0*s0* s1* s2* s3* z3*z3 +  s0*s0* s1* s3*s3* z1*z1 +  s0*s0* s2*s2* s3* z3*z3 +  s0*s0* s2* s3*s3* z2*z2 +  s0* s1*s1* s2*s2* z0*z0 + 4* s0* s1*s1* s2* s3* z0*z0 + 4* s0* s1*s1* s2* s3* z2*z2 + 4* s0* s1*s1* s2* s3* z3*z3 +  s0* s1*s1* s3*s3* z0*z0 + 4* s0* s1* s2*s2* s3* z0*z0 + 4* s0* s1* s2*s2* s3* z1*z1 + 4* s0* s1* s2*s2* s3* z3*z3 + 4* s0* s1* s2* s3*s3* z0*z0 + 4* s0* s1* s2* s3*s3* z1*z1 + 4* s0* s1* s2* s3*s3* z2*z2 +  s0* s2*s2* s3*s3* z0*z0 +  s1*s1* s2*s2* s3* z3*z3 +  s1*s1* s2* s3*s3* z2*z2 +  s1* s2*s2* s3*s3* z1*z1;
  polyCoeff(3) = 2* s0*s0* s1* s2* z1*z1 + 2* s0*s0* s1* s2* z2*z2 + 2* s0*s0* s1* s3* z1*z1 + 2* s0*s0* s1* s3* z3*z3 + 2* s0*s0* s2* s3* z2*z2 + 2* s0*s0* s2* s3* z3*z3 + 2* s0* s1*s1* s2* z0*z0 + 2* s0* s1*s1* s2* z2*z2 + 2* s0* s1*s1* s3* z0*z0 + 2* s0* s1*s1* s3* z3*z3 + 2* s0* s1* s2*s2* z0*z0 + 2* s0* s1* s2*s2* z1*z1 + 8* s0* s1* s2* s3* z0*z0 + 8* s0* s1* s2* s3* z1*z1 + 8* s0* s1* s2* s3* z2*z2 + 8* s0* s1* s2* s3* z3*z3 + 2* s0* s1* s3*s3* z0*z0 + 2* s0* s1* s3*s3* z1*z1 + 2* s0* s2*s2* s3* z0*z0 + 2* s0* s2*s2* s3* z3*z3 + 2* s0* s2* s3*s3* z0*z0 + 2* s0* s2* s3*s3* z2*z2 + 2* s1*s1* s2* s3* z2*z2 + 2* s1*s1* s2* s3* z3*z3 + 2* s1* s2*s2* s3* z1*z1 + 2* s1* s2*s2* s3* z3*z3 + 2* s1* s2* s3*s3* z1*z1 + 2* s1* s2* s3*s3* z2*z2 ;
  polyCoeff(4) =  s0*s0* s1* z1*z1 +  s0*s0* s2* z2*z2 +  s0*s0* s3* z3*z3 +  s0* s1*s1* z0*z0 + 4* s0* s1* s2* z0*z0 + 4* s0* s1* s2* z1*z1 + 4* s0* s1* s2* z2*z2 + 4* s0* s1* s3* z0*z0 + 4* s0* s1* s3* z1*z1 + 4* s0* s1* s3* z3*z3 +  s0* s2*s2* z0*z0 + 4* s0* s2* s3* z0*z0 + 4* s0* s2* s3* z2*z2 + 4* s0* s2* s3* z3*z3 +  s0* s3*s3* z0*z0 +  s1*s1* s2* z2*z2 +  s1*s1* s3* z3*z3 +  s1* s2*s2* z1*z1 + 4* s1* s2* s3* z1*z1 + 4* s1* s2* s3* z2*z2 + 4* s1* s2* s3* z3*z3 +  s1* s3*s3* z1*z1 +  s2*s2* s3* z3*z3 +  s2* s3*s3* z2*z2;
  polyCoeff(5) = 2* s0* s1* z0*z0 + 2* s0* s1* z1*z1 + 2* s0* s2* z0*z0 + 2* s0* s2* z2*z2 + 2* s0* s3* z0*z0 + 2* s0* s3* z3*z3 + 2* s1* s2* z1*z1 + 2* s1* s2* z2*z2 + 2* s1* s3* z1*z1 + 2* s1* s3* z3*z3 + 2* s2* s3* z2*z2 + 2* s2* s3* z3*z3 ;
  polyCoeff(6) =  s0* z0*z0 +  s1* z1*z1 +  s2* z2*z2 +  s3* z3*z3;

}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::ConjugateFFSolver<DerivedV, DerivedF, DerivedO>::
localStep()
{
  for (int j =0; j<data.numF; ++j)
  {
    Eigen::Matrix<typename DerivedV::Scalar, 4, 1> xproj; xproj << pvU.row(j).transpose(),pvV.row(j).transpose();
    Eigen::Matrix<typename DerivedV::Scalar, 4, 1> z = data.UH[j].transpose()*xproj;
    Eigen::Matrix<typename DerivedV::Scalar, 4, 1> x;

    Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> polyCoeff;
    getPolyCoeffsForLocalSolve(data.s[j], z, polyCoeff);
    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> roots;
    igl::polyRoots<typename DerivedV::Scalar, typename DerivedV::Scalar> (polyCoeff, roots );

    //  find closest real root to xproj
    typename DerivedV::Scalar minDist = 1e10;
    for (int i =0; i< 6; ++i)
    {
      if (fabs(imag(roots[i]))>1e-10)
        continue;
      Eigen::Matrix<typename DerivedV::Scalar, 4, 4> D = ((Eigen::Matrix<typename DerivedV::Scalar, 4, 1>::Ones()+real(roots(i))*data.s[j]).array().inverse()).matrix().asDiagonal();
      Eigen::Matrix<typename DerivedV::Scalar, 4, 1> candidate = data.UH[j]*D*z;
      typename DerivedV::Scalar dist = (candidate-xproj).norm();
      if (dist<minDist)
      {
        minDist = dist;
        x = candidate;
      }

    }

    pvU.row(j) << x(0),x(1);
    pvV.row(j) << x(2),x(3);
  }
}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::ConjugateFFSolver<DerivedV, DerivedF, DerivedO>::
setCoefficientsFromField()
{
  for (int i = 0; i <data.numF; ++i)
  {
    std::complex<typename DerivedV::Scalar> u(pvU(i,0),pvU(i,1));
    std::complex<typename DerivedV::Scalar> v(pvV(i,0),pvV(i,1));
    Acoeff(i) = u*u+v*v;
    Bcoeff(i) = u*u*v*v;
  }
}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::ConjugateFFSolver<DerivedV, DerivedF, DerivedO>::
globalStep(const Eigen::Matrix<int, Eigen::Dynamic, 1>  &isConstrained,
           const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1>  &Ak,
           const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1>  &Bk)
{
  setCoefficientsFromField();

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > I;
  igl::speye(data.numF, data.numF, I);
  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > QA = data.DDA+lambda*data.planarityWeight+lambdaOrtho*I;
  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > fA = (-2*lambda*data.planarityWeight*Acoeff).sparseView();

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > QB = data.DDB+lambda*data.planarityWeight;
  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > fB = (-2*lambda*data.planarityWeight*Bcoeff).sparseView();

  if(doHardConstraints)
  {
    minQuadWithKnownMini(QA, fA, isConstrained, Ak, Acoeff);
    minQuadWithKnownMini(QB, fB, isConstrained, Bk, Bcoeff);
  }
  else
  {
    Eigen::Matrix<int, Eigen::Dynamic, 1>isknown_; isknown_.setZero(data.numF,1);
    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> xknown_; xknown_.setZero(0,1);
    minQuadWithKnownMini(QA, fA, isknown_, xknown_, Acoeff);
    minQuadWithKnownMini(QB, fB, isknown_, xknown_, Bcoeff);
  }
  setFieldFromCoefficients();

}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::ConjugateFFSolver<DerivedV, DerivedF, DerivedO>::
setFieldFromCoefficients()
{
  for (int i = 0; i <data.numF; ++i)
  {
    //    poly coefficients: 1, 0, -Acoeff, 0, Bcoeff
    //    matlab code from roots (given there are no trailing zeros in the polynomial coefficients)
    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> polyCoeff(5,1);
    polyCoeff<<1., 0., -Acoeff(i), 0., Bcoeff(i);

    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> roots;
    polyRoots<std::complex<typename DerivedV::Scalar>>(polyCoeff,roots);

    std::complex<typename DerivedV::Scalar> u = roots[0];
    int maxi = -1;
    float maxd = -1;
    for (int k =1; k<4; ++k)
    {
      float dist = abs(roots[k]+u);
      if (dist>maxd)
      {
        maxd = dist;
        maxi = k;
      }
    }
    std::complex<typename DerivedV::Scalar> v = roots[maxi];
    pvU(i,0) = real(u); pvU(i,1) = imag(u);
    pvV(i,0) = real(v); pvV(i,1) = imag(v);
  }

}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::ConjugateFFSolver<DerivedV, DerivedF, DerivedO>::
minQuadWithKnownMini(const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &Q,
                     const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &f,
                     const Eigen::VectorXi isConstrained,
                     const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &xknown,
                     Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &x)
{
  int N = Q.rows();

  int nc = xknown.rows();
  Eigen::VectorXi known; known.setZero(nc,1);
  Eigen::VectorXi unknown; unknown.setZero(N-nc,1);

  int indk = 0, indu = 0;
  for (int i = 0; i<N; ++i)
    if (isConstrained[i])
    {
      known[indk] = i;
      indk++;
    }
    else
    {
      unknown[indu] = i;
      indu++;
    }

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>> Quu, Quk;

  igl::slice(Q,unknown, unknown, Quu);
  igl::slice(Q,unknown, known, Quk);

  std::vector<typename Eigen::Triplet<std::complex<typename DerivedV::Scalar> > > tripletList;

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > fu(N-nc,1);

  igl::slice(f,unknown, Eigen::VectorXi::Zero(1,1), fu);

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > rhs = (Quk*xknown).sparseView()+.5*fu;

  Eigen::SparseLU< Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>>> solver;
  solver.compute(-Quu);
  if(solver.info()!=Eigen::Success)
  {
    std::cerr<<"Decomposition failed!"<<std::endl;
    return;
  }
  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>>  b  = solver.solve(rhs);
  if(solver.info()!=Eigen::Success)
  {
    std::cerr<<"Solving failed!"<<std::endl;
    return;
  }

  indk = 0, indu = 0;
  x.setZero(N,1);
  for (int i = 0; i<N; ++i)
    if (isConstrained[i])
      x[i] = xknown[indk++];
    else
      x[i] = b.coeff(indu++,0);

}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE bool igl::ConjugateFFSolver<DerivedV, DerivedF, DerivedO>::
solve(const Eigen::VectorXi &isConstrained,
      const Eigen::PlainObjectBase<DerivedO> &initialSolution,
      Eigen::PlainObjectBase<DerivedO> &output,
      typename DerivedV::Scalar *lambdaOut)
{
  int numConstrained = isConstrained.sum();
  // coefficient values
  Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> Ak, Bk;

  pvU.resize(data.numF,2);
  pvV.resize(data.numF,2);
  for (int fi = 0; fi <data.numF; ++fi)
  {
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b1 = data.B1.row(fi);
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b2 = data.B2.row(fi);
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &u3 = initialSolution.block(fi,0,1,3);
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &v3 = initialSolution.block(fi,3,1,3);
    pvU.row(fi)<< u3.dot(b1), u3.dot(b2);
    pvV.row(fi)<< v3.dot(b1), v3.dot(b2);
  }
  setCoefficientsFromField();
  Ak.resize(numConstrained,1);
  Bk.resize(numConstrained,1);
  int ind = 0;
  for (int i = 0; i <data.numF; ++i)
  {
    if(isConstrained[i])
    {
      Ak(ind) = Acoeff[i];
      Bk(ind) = Bcoeff[i];
      ind ++;
    }
  }

  typename DerivedV::Scalar smoothnessValue;
  Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> conjValues;
  typename DerivedV::Scalar meanConj;
  typename DerivedV::Scalar maxConj;

  data.evaluateConjugacy(pvU, pvV, conjValues);
  meanConj = conjValues.cwiseAbs().mean();
  maxConj = conjValues.cwiseAbs().maxCoeff();
  printf("Initial max non-conjugacy: %.5g\n",maxConj);

  smoothnessValue = (Acoeff.adjoint()*data.DDA*Acoeff + Bcoeff.adjoint()*data.DDB*Bcoeff).real()[0];
  printf("\n\nInitial smoothness: %.5g\n",smoothnessValue);

  lambda = lambdaInit;

  bool doit = false;
  for (int iter = 0; iter<maxIter; ++iter)
  {
    printf("\n\n--- Iteration %d ---\n",iter);

    typename DerivedV::Scalar oldMeanConj = meanConj;

    localStep();
    globalStep(isConstrained, Ak, Bk);

    smoothnessValue = (Acoeff.adjoint()*data.DDA*Acoeff + Bcoeff.adjoint()*data.DDB*Bcoeff).real()[0];

    printf("Smoothness: %.5g\n",smoothnessValue);

    data.evaluateConjugacy(pvU, pvV, conjValues);
    meanConj = conjValues.cwiseAbs().mean();
    maxConj = conjValues.cwiseAbs().maxCoeff();
    printf("Mean/Max non-conjugacy: %.5g, %.5g\n",meanConj,maxConj);
    typename DerivedV::Scalar diffMeanConj = fabs(oldMeanConj-meanConj);

    if (diffMeanConj<1e-4)
      doit = true;

    if (doit)
      lambda = lambda*lambdaMultFactor;
    printf(" %d %.5g %.5g\n",iter, smoothnessValue,maxConj);

  }

  output.setZero(data.numF,6);
  for (int fi=0; fi<data.numF; ++fi)
  {
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b1 = data.B1.row(fi);
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b2 = data.B2.row(fi);
    output.block(fi,0, 1, 3) = pvU(fi,0)*b1 + pvU(fi,1)*b2;
    output.block(fi,3, 1, 3) = pvV(fi,0)*b1 + pvV(fi,1)*b2;
  }

  if (lambdaOut)
    *lambdaOut = lambda;

  return true;
}

template <typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::conjugate_frame_fields(const Eigen::PlainObjectBase<DerivedV> &V,
                                            const Eigen::PlainObjectBase<DerivedF> &F,
                                            const Eigen::VectorXi &isConstrained,
                                            const Eigen::PlainObjectBase<DerivedO> &initialSolution,
                                            Eigen::PlainObjectBase<DerivedO> &output,
                                            int maxIter,
                                            const typename DerivedV::Scalar &lambdaOrtho,
                                            const typename DerivedV::Scalar &lambdaInit,
                                            const typename DerivedV::Scalar &lambdaMultFactor,
                                            bool doHardConstraints)
{
  igl::ConjugateFFSolverData<DerivedV, DerivedF> csdata(V, F);
  igl::ConjugateFFSolver<DerivedV, DerivedF, DerivedO> cs(csdata, maxIter, lambdaOrtho, lambdaInit, lambdaMultFactor, doHardConstraints);
  cs.solve(isConstrained, initialSolution, output);
}

template <typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::conjugate_frame_fields(const igl::ConjugateFFSolverData<DerivedV, DerivedF> &csdata,
                                            const Eigen::VectorXi &isConstrained,
                                            const Eigen::PlainObjectBase<DerivedO> &initialSolution,
                                            Eigen::PlainObjectBase<DerivedO> &output,
                                            int maxIter,
                                            const typename DerivedV::Scalar &lambdaOrtho,
                                            const typename DerivedV::Scalar &lambdaInit,
                                            const typename DerivedV::Scalar &lambdaMultFactor,
                                            bool doHardConstraints,
                                            typename DerivedV::Scalar *lambdaOut)
{
  igl::ConjugateFFSolver<DerivedV, DerivedF, DerivedO> cs(csdata, maxIter, lambdaOrtho, lambdaInit, lambdaMultFactor, doHardConstraints);
  cs.solve(isConstrained, initialSolution, output, lambdaOut);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
template void igl::conjugate_frame_fields<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(igl::ConjugateFFSolverData<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, 1, 0, -1, 1> const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, int, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar const&, bool, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar*);
#endif


// Boost.Geometry

// Copyright (c) 2018 Yaghyavardhan Singh Khangarot, Hyderabad, India.
// Contributed and/or modified by Yaghyavardhan Singh Khangarot,
//   as part of Google Summer of Code 2018 program.

// This file was modified by Oracle on 2018-2021.
// Modifications copyright (c) 2018-2021, 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)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DISCRETE_HAUSDORFF_DISTANCE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DISCRETE_HAUSDORFF_DISTANCE_HPP

#include <algorithm>

#ifdef BOOST_GEOMETRY_DEBUG_HAUSDORFF_DISTANCE
#include <iostream>
#endif

#include <iterator>
#include <utility>
#include <vector>
#include <limits>

#include <boost/geometry/algorithms/detail/dummy_geometries.hpp>
#include <boost/geometry/algorithms/detail/throw_on_empty_input.hpp>
#include <boost/geometry/algorithms/not_implemented.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategies/discrete_distance/cartesian.hpp>
#include <boost/geometry/strategies/discrete_distance/geographic.hpp>
#include <boost/geometry/strategies/discrete_distance/spherical.hpp>
#include <boost/geometry/strategies/distance_result.hpp>
#include <boost/geometry/util/range.hpp>

// Note that in order for this to work umbrella strategy has to contain
// index strategies.
#ifdef BOOST_GEOMETRY_ENABLE_SIMILARITY_RTREE
#include <boost/geometry/index/rtree.hpp>
#endif // BOOST_GEOMETRY_ENABLE_SIMILARITY_RTREE

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace discrete_hausdorff_distance
{

// TODO: The implementation should calculate comparable distances

struct point_range
{
    template <typename Point, typename Range, typename Strategies>
    static inline auto apply(Point const& pnt, Range const& rng,
                             Strategies const& strategies)
    {
        typedef typename distance_result
            <
                Point,
                typename point_type<Range>::type,
                Strategies
            >::type result_type;

        typedef typename boost::range_size<Range>::type size_type;

        boost::geometry::detail::throw_on_empty_input(rng);

        auto const strategy = strategies.distance(dummy_point(), dummy_point());

        size_type const n = boost::size(rng);
        result_type dis_min = 0;
        bool is_dis_min_set = false;

        for (size_type i = 0 ; i < n ; i++)
        {
            result_type dis_temp = strategy.apply(pnt, range::at(rng, i));
            if (! is_dis_min_set || dis_temp < dis_min)
            {
                dis_min = dis_temp;
                is_dis_min_set = true;
            }
        }
        return dis_min;
    }
};

struct range_range
{
    template <typename Range1, typename Range2, typename Strategies>
    static inline auto apply(Range1 const& r1, Range2 const& r2,
                             Strategies const& strategies)
    {
        typedef typename distance_result
            <
                typename point_type<Range1>::type,
                typename point_type<Range2>::type,
                Strategies
            >::type result_type;

        typedef typename boost::range_size<Range1>::type size_type;

        boost::geometry::detail::throw_on_empty_input(r1);
        boost::geometry::detail::throw_on_empty_input(r2);

        size_type const n = boost::size(r1);
        result_type dis_max = 0;

#ifdef BOOST_GEOMETRY_ENABLE_SIMILARITY_RTREE
        namespace bgi = boost::geometry::index;
        typedef typename point_type<Range1>::type point_t;
        typedef bgi::rtree<point_t, bgi::linear<4> > rtree_type;
        rtree_type rtree(boost::begin(r2), boost::end(r2));
        point_t res;
#endif

        for (size_type i = 0 ; i < n ; i++)
        {
#ifdef BOOST_GEOMETRY_ENABLE_SIMILARITY_RTREE
            size_type found = rtree.query(bgi::nearest(range::at(r1, i), 1), &res);
            result_type dis_min = strategy.apply(range::at(r1,i), res);
#else
            result_type dis_min = point_range::apply(range::at(r1, i), r2, strategies);
#endif
            if (dis_min > dis_max )
            {
                dis_max = dis_min;
            }
        }
        return dis_max;
    }
};

struct range_multi_range
{
    template <typename Range, typename Multi_range, typename Strategies>
    static inline auto apply(Range const& rng, Multi_range const& mrng,
                             Strategies const& strategies)
    {
        typedef typename distance_result
            <
                typename point_type<Range>::type,
                typename point_type<Multi_range>::type,
                Strategies
            >::type result_type;
        typedef typename boost::range_size<Multi_range>::type size_type;

        boost::geometry::detail::throw_on_empty_input(rng);
        boost::geometry::detail::throw_on_empty_input(mrng);

        size_type b = boost::size(mrng);
        result_type haus_dis = 0;

        for (size_type j = 0 ; j < b ; j++)
        {
            result_type dis_max = range_range::apply(rng, range::at(mrng, j), strategies);
            if (dis_max > haus_dis)
            {
                haus_dis = dis_max;
            }
        }

        return haus_dis;
    }
};

struct multi_range_multi_range
{
    template <typename Multi_Range1, typename Multi_range2, typename Strategies>
    static inline auto apply(Multi_Range1 const& mrng1, Multi_range2 const& mrng2,
                             Strategies const& strategies)
    {
        typedef typename distance_result
            <
                typename point_type<Multi_Range1>::type,
                typename point_type<Multi_range2>::type,
                Strategies
            >::type result_type;
        typedef typename boost::range_size<Multi_Range1>::type size_type;

        boost::geometry::detail::throw_on_empty_input(mrng1);
        boost::geometry::detail::throw_on_empty_input(mrng2);

        size_type n = boost::size(mrng1);
        result_type haus_dis = 0;

        for (size_type i = 0 ; i < n ; i++)
        {
            result_type dis_max = range_multi_range::apply(range::at(mrng1, i), mrng2, strategies);
            if (dis_max > haus_dis)
            {
                haus_dis = dis_max;
            }
        }
        return haus_dis;
    }
};

}} // namespace detail::hausdorff_distance
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{
template
<
    typename Geometry1,
    typename Geometry2,
    typename Tag1 = typename tag<Geometry1>::type,
    typename Tag2 = typename tag<Geometry2>::type
>
struct discrete_hausdorff_distance : not_implemented<Tag1, Tag2>
{};

// Specialization for point and multi_point
template <typename Point, typename MultiPoint>
struct discrete_hausdorff_distance<Point, MultiPoint, point_tag, multi_point_tag>
    : detail::discrete_hausdorff_distance::point_range
{};

// Specialization for linestrings
template <typename Linestring1, typename Linestring2>
struct discrete_hausdorff_distance<Linestring1, Linestring2, linestring_tag, linestring_tag>
    : detail::discrete_hausdorff_distance::range_range
{};

// Specialization for multi_point-multi_point
template <typename MultiPoint1, typename MultiPoint2>
struct discrete_hausdorff_distance<MultiPoint1, MultiPoint2, multi_point_tag, multi_point_tag>
    : detail::discrete_hausdorff_distance::range_range
{};

// Specialization for Linestring and MultiLinestring
template <typename Linestring, typename MultiLinestring>
struct discrete_hausdorff_distance<Linestring, MultiLinestring, linestring_tag, multi_linestring_tag>
    : detail::discrete_hausdorff_distance::range_multi_range
{};

// Specialization for MultiLinestring and MultiLinestring
template <typename MultiLinestring1, typename MultiLinestring2>
struct discrete_hausdorff_distance<MultiLinestring1, MultiLinestring2, multi_linestring_tag, multi_linestring_tag>
    : detail::discrete_hausdorff_distance::multi_range_multi_range
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

namespace resolve_strategy {

template
<
    typename Strategies,
    bool IsUmbrella = strategies::detail::is_umbrella_strategy<Strategies>::value
>
struct discrete_hausdorff_distance
{
    template <typename Geometry1, typename Geometry2>
    static inline auto apply(Geometry1 const& geometry1, Geometry2 const& geometry2,
                             Strategies const& strategies)
    {
        return dispatch::discrete_hausdorff_distance
            <
                Geometry1, Geometry2
            >::apply(geometry1, geometry2, strategies);
    }
};

template <typename Strategy>
struct discrete_hausdorff_distance<Strategy, false>
{
    template <typename Geometry1, typename Geometry2>
    static inline auto apply(Geometry1 const& geometry1, Geometry2 const& geometry2,
                             Strategy const& strategy)
    {
        using strategies::discrete_distance::services::strategy_converter;
        return dispatch::discrete_hausdorff_distance
            <
                Geometry1, Geometry2
            >::apply(geometry1, geometry2,
                     strategy_converter<Strategy>::get(strategy));
    }
};

template <>
struct discrete_hausdorff_distance<default_strategy, false>
{
    template <typename Geometry1, typename Geometry2>
    static inline auto apply(Geometry1 const& geometry1, Geometry2 const& geometry2,
                             default_strategy const&)
    {
        typedef typename strategies::discrete_distance::services::default_strategy
            <
                Geometry1, Geometry2
            >::type strategies_type;

        return dispatch::discrete_hausdorff_distance
            <
                Geometry1, Geometry2
            >::apply(geometry1, geometry2, strategies_type());
    }
};

} // namespace resolve_strategy

/*!
\brief Calculate discrete Hausdorff distance between two geometries (currently
    works for LineString-LineString, MultiPoint-MultiPoint, Point-MultiPoint,
    MultiLineString-MultiLineString) using specified strategy.
\ingroup discrete_hausdorff_distance
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam Strategy A type fulfilling a DistanceStrategy concept
\param geometry1 Input geometry
\param geometry2 Input geometry
\param strategy Distance strategy to be used to calculate Pt-Pt distance

\qbk{distinguish,with strategy}
\qbk{[include reference/algorithms/discrete_hausdorff_distance.qbk]}

\qbk{
[heading Available Strategies]
\* [link geometry.reference.strategies.strategy_distance_pythagoras Pythagoras (cartesian)]
\* [link geometry.reference.strategies.strategy_distance_haversine Haversine (spherical)]
[/ \* more (currently extensions): Vincenty\, Andoyer (geographic) ]

[heading Example]
[discrete_hausdorff_distance_strategy]
[discrete_hausdorff_distance_strategy_output]
}
*/
template <typename Geometry1, typename Geometry2, typename Strategy>
inline auto discrete_hausdorff_distance(Geometry1 const& geometry1,
                                        Geometry2 const& geometry2,
                                        Strategy const& strategy)
{
    return resolve_strategy::discrete_hausdorff_distance
        <
            Strategy
        >::apply(geometry1, geometry2, strategy);
}

/*!
\brief Calculate discrete Hausdorff distance between two geometries (currently
    works for LineString-LineString, MultiPoint-MultiPoint, Point-MultiPoint,
    MultiLineString-MultiLineString).
\ingroup discrete_hausdorff_distance
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\param geometry1 Input geometry
\param geometry2 Input geometry

\qbk{[include reference/algorithms/discrete_hausdorff_distance.qbk]}

\qbk{
[heading Example]
[discrete_hausdorff_distance]
[discrete_hausdorff_distance_output]
}
*/
template <typename Geometry1, typename Geometry2>
inline auto discrete_hausdorff_distance(Geometry1 const& geometry1,
                                        Geometry2 const& geometry2)
{
    return resolve_strategy::discrete_hausdorff_distance
        <
            default_strategy
        >::apply(geometry1, geometry2, default_strategy());
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DISCRETE_HAUSDORFF_DISTANCE_HPP


#include <boost/python.hpp>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/vec2.h>

namespace prime { namespace index_ambiguity {
  scitbx::af::shared<double>
  calc_BD_alg_2(scitbx::af::versa<double, scitbx::af::flex_grid<> > r_grid, scitbx::af::shared<scitbx::vec2<double> > x_vec_set) {
    //calculate residuals of the Brehm & Diederichs Algorithm 2
    scitbx::af::shared<double> r_out;
    for (int i = 0; i < x_vec_set.size() - 1; i++) {
      double sum_r = 0;
      for (int j = i+1; j < x_vec_set.size(); j++) {
        sum_r += std::abs(r_grid(i,j) - (x_vec_set[i] * x_vec_set[j]));
      }
      r_out.push_back(sum_r);
    }
    return r_out;
  }
  double
  calc_BD_alg_2_sum_sqr(scitbx::af::versa<double, scitbx::af::flex_grid<> > r_grid, scitbx::af::shared<scitbx::vec2<double> > x_vec_set) {
    //calculate squared residuals of the Brehm & Diederichs Algorithm 2
    double sum_r_sqr = 0;
    for (int i = 0; i < x_vec_set.size() - 1; i++) {
      for (int j = i+1; j < x_vec_set.size(); j++) {
        double tmp = r_grid(i,j) - (x_vec_set[i] * x_vec_set[j]);
        sum_r_sqr += (tmp * tmp);
      }
    }
    return sum_r_sqr;
  }
  scitbx::af::shared<double>
  calc_gradient_BD_alg_2(scitbx::af::versa<double, scitbx::af::flex_grid<> > r_grid, scitbx::af::shared<scitbx::vec2<double> > x_vec_set) {
    //calculate finite differences of each pair of parameters
    scitbx::af::shared<double> g_out;
    double f = calc_BD_alg_2_sum_sqr(r_grid, x_vec_set);
    double delta = 1.0e-7;
    for (int i = 0; i < x_vec_set.size(); i++) {
      //each component of the vector has its own gradient
      x_vec_set[i][0] += delta;
      double df_x = calc_BD_alg_2_sum_sqr(r_grid, x_vec_set);
      g_out.push_back((df_x - f)/delta);
      x_vec_set[i][0] -= delta;
      x_vec_set[i][1] += delta;
      double df_y = calc_BD_alg_2_sum_sqr(r_grid, x_vec_set);
      g_out.push_back((df_y - f)/delta);
      x_vec_set[i][1] -= delta;
    }
    return g_out;
  }
}}

namespace prime { namespace index_ambiguity {
namespace boost_python {
  void
  wrap_all() {
    boost::python::def("calc_BD_alg_2", calc_BD_alg_2);
    boost::python::def("calc_BD_alg_2_sum_sqr", calc_BD_alg_2_sum_sqr);
    boost::python::def("calc_gradient_BD_alg_2", calc_gradient_BD_alg_2);
  }
}}}

BOOST_PYTHON_MODULE(prime_index_ambiguity_ext)
{
  prime::index_ambiguity::boost_python::wrap_all();
}


/*
 * protonReconstruct.cpp
 *
 *  Created on: Dec 22, 2011
 *      Author: u051747
 */
#include <iostream>
#include "parameterparser.h"
#include "cuNDArray.h"
#include "hoCuNDArray.h"

#include "hoCuProtonBufferedSubsetOperator.h"
#include "hoNDArray_fileio.h"
#include "check_CUDA.h"

#include "hoCuGPBBSolver.h"
#include "hoCuPartialDerivativeOperator.h"
#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_operators.h"

#include "osSARTSolver.h"

#include "hoOSGPBBSolver.h"
#include "hdf5_utils.h"

#include "encodingOperatorContainer.h"
#include "hoCuOperator.h"
#include "hoImageOperator.h"
#include "identityOperator.h"
#include <boost/program_options.hpp>
#include "vector_td_io.h"
#include "projectionSpaceOperator.h"
using namespace std;
using namespace Gadgetron;
typedef float _real;

typedef solver<hoCuNDArray<_real>, hoCuNDArray<_real> > baseSolver;
namespace po = boost::program_options;
int main( int argc, char** argv)
{

  //
  // Parse command line
  //

  _real background =  0.00106;
  std::string dataName;
	std::string outputFile;
	vector_td<int,3> dimensions;
	vector_td<float,3> physical_dims;
	vector_td<float,3> origin;
	int iterations;
	int device;
	int subsets;
	po::options_description desc("Allowed options");
	desc.add_options()
			("help", "produce help message")
			("data,D", po::value<std::string>(&dataName)->default_value("data.hdf5"), "HDF5 file containing projections and splines")
			("dimensions,d", po::value<vector_td<int,3> >(&dimensions)->default_value(vector_td<int,3>(512,512,1)), "Pixel dimensions of the image")
			("size,S", po::value<vector_td<float,3> >(&physical_dims)->default_value(vector_td<float,3>(20,20,5)), "Dimensions of the image")
			("center,c", po::value<vector_td<float,3> >(&origin)->default_value(vector_td<float,3>(0,0,0)), "Center of the reconstruction")
			("iterations,i", po::value<int>(&iterations)->default_value(10), "Dimensions of the image")
			("output,f", po::value<std::string>(&outputFile)->default_value("image.hdf5"), "Output filename")
			("prior,P", po::value<std::string>(),"Prior image filename")
			("prior-weight,k",po::value<float>(),"Weight of the prior image")
			("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
			("subsets,n", po::value<int>(&subsets)->default_value(10), "Number of subsets to use")
	;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}
	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		std::cout << it->first << ": ";
		if (a.type() == typeid(std::string)) std::cout << it->second.as<std::string>();
		else if (a.type() == typeid(int)) std::cout << it->second.as<int>();
		else if (a.type() == typeid(float)) std::cout << it->second.as<float>();
		else if (a.type() == typeid(vector_td<float,3>)) std::cout << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) std::cout << it->second.as<vector_td<int,3> >();
		else std::cout << "Unknown type" << std::endl;
		std::cout << std::endl;
	}
	cudaSetDevice(device);
	//cudaDeviceReset();

/*
  hoCuGPBBSolver< _real> solver;

  solver.set_max_iterations( iterations);

  solver.set_output_mode( hoCuGPBBSolver< _real>::OUTPUT_VERBOSE );*/
	//osSARTSolver<hoCuNDArray<_real> > solver;
	hoOSGPBBSolver<hoCuNDArray<_real> > solver;
  solver.set_non_negativity_constraint(true);
  solver.set_max_iterations(iterations);
  boost::shared_ptr< hoCuProtonBufferedSubsetOperator<_real> > E (new hoCuProtonBufferedSubsetOperator<_real>(subsets) );
  boost::shared_ptr<hoCuNDArray<_real> >  projections = E->load_data(dataName,physical_dims,origin,background);

  std::vector<size_t> rhs_dims(&dimensions[0],&dimensions[3]); //Quick and dirty vector_td to vector
  E->set_domain_dimensions(&rhs_dims);
  E->set_codomain_dimensions(projections->get_dimensions().get());
  boost::shared_ptr<hoCuNDArray<_real > > prior;
    if (vm.count("prior")){
   	  std::cout << "Prior image regularization in use" << std::endl;
  		prior = boost::static_pointer_cast<hoCuNDArray<_real > >(read_nd_array<_real >(vm["prior"].as<std::string>().c_str()));

  		prior->reshape(&rhs_dims);
  		_real offset = _real(0.01);
  		//cuNDA_add(offset,prior.get());

  		if (vm.count("prior-weight")){

  		//boost::shared_ptr<hoImageOperator<_real> > I (new hoImageOperator<_real > ());
  		//I->compute(prior.get());
  			boost::shared_ptr<identityOperator<hoCuNDArray<_real> > > Itmp ( new identityOperator<hoCuNDArray<_real> >);

  			boost::shared_ptr<projectionSpaceOperator<hoCuNDArray<_real> > > I (new projectionSpaceOperator<hoCuNDArray<_real> >(Itmp));

  		I->set_weight(vm["prior-weight"].as<float>());

  		I->set_codomain_dimensions(&rhs_dims);
  		I->set_domain_dimensions(&rhs_dims);
  		I->set_projections(prior);
  		//hoCuNDArray<_real> tmp = *prior;

  		//I->mult_M(prior.get(),&tmp);
  		solver.add_regularization_operator(I);

  		}
  		solver.set_x0(prior);
    }
  solver.set_encoding_operator(E);
  solver.set_output_mode(baseSolver::OUTPUT_VERBOSE);
	//hoCuNDA_clear(projections.get());
	//CHECK_FOR_CUDA_ERROR();

	//float res = dot(projections.get(),projections.get());

	boost::shared_ptr< hoCuNDArray<_real> > result = solver.solve(projections.get());

	hoCuNDArray<_real> tmp_proj(projections->get_dimensions());
	E->mult_M(result.get(),&tmp_proj,false);
	tmp_proj -= *projections;

	std::cout << "L2 norm of residual: " << nrm2(&tmp_proj) << std::endl;
	//write_nd_array<_real>(result.get(), (char*)parms.get_parameter('f')->get_string_value());
	std::stringstream ss;
	for (int i = 0; i < argc; i++){
		ss << argv[i] << " ";
	}
	saveNDArray2HDF5<3>(result.get(),outputFile,physical_dims,origin,ss.str(), solver.get_max_iterations());
}